Merge branch 'master' of /home/davem/src/GIT/linux-2.6/

909 files changed, 97776 insertions, 9507 deletions

diff --git a/drivers/Makefile b/drivers/Makefile
index f42a03029b7c..91874e048552 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -10,6 +10,7 @@ obj-$(CONFIG_PCI)		+= pci/
 obj-$(CONFIG_PARISC)            += parisc/
 obj-$(CONFIG_RAPIDIO)           += rapidio/
 obj-y                           += video/
+obj-y                           += idle/
 obj-$(CONFIG_ACPI)              += acpi/
 obj-$(CONFIG_SFI)               += sfi/
 # PnP must come after ACPI since it will eventually need to check if acpi
@@ -91,7 +92,6 @@ obj-$(CONFIG_EISA)		+= eisa/
 obj-y                           += lguest/
 obj-$(CONFIG_CPU_FREQ)          += cpufreq/
 obj-$(CONFIG_CPU_IDLE)          += cpuidle/
-obj-y                           += idle/
 obj-$(CONFIG_MMC)               += mmc/
 obj-$(CONFIG_MEMSTICK)          += memstick/
 obj-$(CONFIG_NEW_LEDS)          += leds/
diff --git a/drivers/acpi/Kconfig b/drivers/acpi/Kconfig
index 93d2c7971df6..746411518802 100644
--- a/drivers/acpi/Kconfig
+++ b/drivers/acpi/Kconfig
@@ -360,4 +360,13 @@ config ACPI_SBS
           To compile this driver as a module, choose M here:
           the modules will be called sbs and sbshc.
 
+config ACPI_HED
+        tristate "Hardware Error Device"
+        help
+          This driver supports the Hardware Error Device (PNP0C33),
+          which is used to report some hardware errors notified via
+          SCI, mainly the corrected errors.
+
+source "drivers/acpi/apei/Kconfig"
+
 endif   # ACPI
diff --git a/drivers/acpi/Makefile b/drivers/acpi/Makefile
index a8d8998dd5c5..6ee33169e1dc 100644
--- a/drivers/acpi/Makefile
+++ b/drivers/acpi/Makefile
@@ -19,7 +19,7 @@ obj-y				+= acpi.o \
 
 # All the builtin files are in the "acpi." module_param namespace.
 acpi-y                          += osl.o utils.o reboot.o
-acpi-y                          += hest.o
+acpi-y                          += atomicio.o
 
 # sleep related files
 acpi-y                          += wakeup.o
@@ -59,6 +59,7 @@ obj-$(CONFIG_ACPI_BATTERY)	+= battery.o
 obj-$(CONFIG_ACPI_SBS)          += sbshc.o
 obj-$(CONFIG_ACPI_SBS)          += sbs.o
 obj-$(CONFIG_ACPI_POWER_METER)  += power_meter.o
+obj-$(CONFIG_ACPI_HED)          += hed.o
 
 # processor has its own "processor." module_param namespace
 processor-y                     := processor_driver.o processor_throttling.o
@@ -66,3 +67,5 @@ processor-y			+= processor_idle.o processor_thermal.o
 processor-$(CONFIG_CPU_FREQ)    += processor_perflib.o
 
 obj-$(CONFIG_ACPI_PROCESSOR_AGGREGATOR) += acpi_pad.o
+
+obj-$(CONFIG_ACPI_APEI)         += apei/
diff --git a/drivers/acpi/acpi_pad.c b/drivers/acpi/acpi_pad.c
index 62122134693b..d269a8f3329c 100644
--- a/drivers/acpi/acpi_pad.c
+++ b/drivers/acpi/acpi_pad.c
@@ -43,6 +43,10 @@ static DEFINE_MUTEX(isolated_cpus_lock);
 #define CPUID5_ECX_EXTENSIONS_SUPPORTED (0x1)
 #define CPUID5_ECX_INTERRUPT_BREAK      (0x2)
 static unsigned long power_saving_mwait_eax;
+
+static unsigned char tsc_detected_unstable;
+static unsigned char tsc_marked_unstable;
+
 static void power_saving_mwait_init(void)
 {
         unsigned int eax, ebx, ecx, edx;
@@ -87,8 +91,8 @@ static void power_saving_mwait_init(void)
 
                 /*FALL THROUGH*/
         default:
-                /* TSC could halt in idle, so notify users */
-                mark_tsc_unstable("TSC halts in idle");
+                /* TSC could halt in idle */
+                tsc_detected_unstable = 1;
         }
 #endif
 }
@@ -168,16 +172,14 @@ static int power_saving_thread(void *data)
 
                 do_sleep = 0;
 
-                current_thread_info()->status &= ~TS_POLLING;
-                /*
-                 * TS_POLLING-cleared state must be visible before we test
-                 * NEED_RESCHED:
-                 */
-                smp_mb();
-
                 expire_time = jiffies + HZ * (100 - idle_pct) / 100;
 
                 while (!need_resched()) {
+                        if (tsc_detected_unstable && !tsc_marked_unstable) {
+                                /* TSC could halt in idle, so notify users */
+                                mark_tsc_unstable("TSC halts in idle");
+                                tsc_marked_unstable = 1;
+                        }
                         local_irq_disable();
                         cpu = smp_processor_id();
                         clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER,
@@ -200,8 +202,6 @@ static int power_saving_thread(void *data)
                         }
                 }
 
-                current_thread_info()->status |= TS_POLLING;
-
                 /*
                  * current sched_rt has threshold for rt task running time.
                  * When a rt task uses 95% CPU time, the rt thread will be
diff --git a/drivers/acpi/acpica/evxfevnt.c b/drivers/acpi/acpica/evxfevnt.c
index 7c7bbb4d402c..d5a5efc043bf 100644
--- a/drivers/acpi/acpica/evxfevnt.c
+++ b/drivers/acpi/acpica/evxfevnt.c
@@ -69,7 +69,7 @@ acpi_ev_get_gpe_device(struct acpi_gpe_xrupt_info *gpe_xrupt_info,
 
 acpi_status acpi_enable(void)
 {
-        acpi_status status = AE_OK;
+        acpi_status status;
 
         ACPI_FUNCTION_TRACE(acpi_enable);
 
@@ -84,21 +84,30 @@ acpi_status acpi_enable(void)
         if (acpi_hw_get_mode() == ACPI_SYS_MODE_ACPI) {
                 ACPI_DEBUG_PRINT((ACPI_DB_INIT,
                                   "System is already in ACPI mode\n"));
-        } else {
-                /* Transition to ACPI mode */
+                return_ACPI_STATUS(AE_OK);
+        }
 
-                status = acpi_hw_set_mode(ACPI_SYS_MODE_ACPI);
-                if (ACPI_FAILURE(status)) {
-                        ACPI_ERROR((AE_INFO,
-                                    "Could not transition to ACPI mode"));
-                        return_ACPI_STATUS(status);
-                }
+        /* Transition to ACPI mode */
 
-                ACPI_DEBUG_PRINT((ACPI_DB_INIT,
-                                  "Transition to ACPI mode successful\n"));
+        status = acpi_hw_set_mode(ACPI_SYS_MODE_ACPI);
+        if (ACPI_FAILURE(status)) {
+                ACPI_ERROR((AE_INFO,
+                            "Could not transition to ACPI mode"));
+                return_ACPI_STATUS(status);
         }
 
-        return_ACPI_STATUS(status);
+        /* Sanity check that transition succeeded */
+
+        if (acpi_hw_get_mode() != ACPI_SYS_MODE_ACPI) {
+                ACPI_ERROR((AE_INFO,
+                            "Hardware did not enter ACPI mode"));
+                return_ACPI_STATUS(AE_NO_HARDWARE_RESPONSE);
+        }
+
+        ACPI_DEBUG_PRINT((ACPI_DB_INIT,
+                          "Transition to ACPI mode successful\n"));
+
+        return_ACPI_STATUS(AE_OK);
 }
 
 ACPI_EXPORT_SYMBOL(acpi_enable)
diff --git a/drivers/acpi/acpica/hwacpi.c b/drivers/acpi/acpica/hwacpi.c
index 679a112a7d26..b44274a0b62c 100644
--- a/drivers/acpi/acpica/hwacpi.c
+++ b/drivers/acpi/acpica/hwacpi.c
@@ -63,7 +63,6 @@ acpi_status acpi_hw_set_mode(u32 mode)
 {
 
         acpi_status status;
-        u32 retry;
 
         ACPI_FUNCTION_TRACE(hw_set_mode);
 
@@ -125,24 +124,7 @@ acpi_status acpi_hw_set_mode(u32 mode)
                 return_ACPI_STATUS(status);
         }
 
-        /*
-         * Some hardware takes a LONG time to switch modes. Give them 3 sec to
-         * do so, but allow faster systems to proceed more quickly.
-         */
-        retry = 3000;
-        while (retry) {
-                if (acpi_hw_get_mode() == mode) {
-                        ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-                                          "Mode %X successfully enabled\n",
-                                          mode));
-                        return_ACPI_STATUS(AE_OK);
-                }
-                acpi_os_stall(1000);
-                retry--;
-        }
-
-        ACPI_ERROR((AE_INFO, "Hardware did not change modes"));
-        return_ACPI_STATUS(AE_NO_HARDWARE_RESPONSE);
+        return_ACPI_STATUS(AE_OK);
 }
 
 /*******************************************************************************
diff --git a/drivers/acpi/apei/Kconfig b/drivers/acpi/apei/Kconfig
new file mode 100644
index 000000000000..f8c668f27b5a
--- /dev/null
+++ b/drivers/acpi/apei/Kconfig
@@ -0,0 +1,30 @@
+config ACPI_APEI
+        bool "ACPI Platform Error Interface (APEI)"
+        depends on X86
+        help
+          APEI allows to report errors (for example from the chipset)
+          to the operating system. This improves NMI handling
+          especially. In addition it supports error serialization and
+          error injection.
+
+config ACPI_APEI_GHES
+        tristate "APEI Generic Hardware Error Source"
+        depends on ACPI_APEI && X86
+        select ACPI_HED
+        help
+          Generic Hardware Error Source provides a way to report
+          platform hardware errors (such as that from chipset). It
+          works in so called "Firmware First" mode, that is, hardware
+          errors are reported to firmware firstly, then reported to
+          Linux by firmware. This way, some non-standard hardware
+          error registers or non-standard hardware link can be checked
+          by firmware to produce more valuable hardware error
+          information for Linux.
+
+config ACPI_APEI_EINJ
+        tristate "APEI Error INJection (EINJ)"
+        depends on ACPI_APEI && DEBUG_FS
+        help
+          EINJ provides a hardware error injection mechanism, it is
+          mainly used for debugging and testing the other parts of
+          APEI and some other RAS features.
diff --git a/drivers/acpi/apei/Makefile b/drivers/acpi/apei/Makefile
new file mode 100644
index 000000000000..b13b03a17789
--- /dev/null
+++ b/drivers/acpi/apei/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_ACPI_APEI)         += apei.o
+obj-$(CONFIG_ACPI_APEI_GHES)    += ghes.o
+obj-$(CONFIG_ACPI_APEI_EINJ)    += einj.o
+
+apei-y := apei-base.o hest.o cper.o erst.o
diff --git a/drivers/acpi/apei/apei-base.c b/drivers/acpi/apei/apei-base.c
new file mode 100644
index 000000000000..db3946e9c66b
--- /dev/null
+++ b/drivers/acpi/apei/apei-base.c
@@ -0,0 +1,593 @@
+/*
+ * apei-base.c - ACPI Platform Error Interface (APEI) supporting
+ * infrastructure
+ *
+ * APEI allows to report errors (for example from the chipset) to the
+ * the operating system. This improves NMI handling especially. In
+ * addition it supports error serialization and error injection.
+ *
+ * For more information about APEI, please refer to ACPI Specification
+ * version 4.0, chapter 17.
+ *
+ * This file has Common functions used by more than one APEI table,
+ * including framework of interpreter for ERST and EINJ; resource
+ * management for APEI registers.
+ *
+ * Copyright (C) 2009, Intel Corp.
+ *      Author: 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 version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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/init.h>
+#include <linux/acpi.h>
+#include <linux/io.h>
+#include <linux/kref.h>
+#include <linux/rculist.h>
+#include <linux/interrupt.h>
+#include <linux/debugfs.h>
+#include <acpi/atomicio.h>
+
+#include "apei-internal.h"
+
+#define APEI_PFX "APEI: "
+
+/*
+ * APEI ERST (Error Record Serialization Table) and EINJ (Error
+ * INJection) interpreter framework.
+ */
+
+#define APEI_EXEC_PRESERVE_REGISTER     0x1
+
+void apei_exec_ctx_init(struct apei_exec_context *ctx,
+                        struct apei_exec_ins_type *ins_table,
+                        u32 instructions,
+                        struct acpi_whea_header *action_table,
+                        u32 entries)
+{
+        ctx->ins_table = ins_table;
+        ctx->instructions = instructions;
+        ctx->action_table = action_table;
+        ctx->entries = entries;
+}
+EXPORT_SYMBOL_GPL(apei_exec_ctx_init);
+
+int __apei_exec_read_register(struct acpi_whea_header *entry, u64 *val)
+{
+        int rc;
+
+        rc = acpi_atomic_read(val, &entry->register_region);
+        if (rc)
+                return rc;
+        *val >>= entry->register_region.bit_offset;
+        *val &= entry->mask;
+
+        return 0;
+}
+
+int apei_exec_read_register(struct apei_exec_context *ctx,
+                            struct acpi_whea_header *entry)
+{
+        int rc;
+        u64 val = 0;
+
+        rc = __apei_exec_read_register(entry, &val);
+        if (rc)
+                return rc;
+        ctx->value = val;
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(apei_exec_read_register);
+
+int apei_exec_read_register_value(struct apei_exec_context *ctx,
+                                  struct acpi_whea_header *entry)
+{
+        int rc;
+
+        rc = apei_exec_read_register(ctx, entry);
+        if (rc)
+                return rc;
+        ctx->value = (ctx->value == entry->value);
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(apei_exec_read_register_value);
+
+int __apei_exec_write_register(struct acpi_whea_header *entry, u64 val)
+{
+        int rc;
+
+        val &= entry->mask;
+        val <<= entry->register_region.bit_offset;
+        if (entry->flags & APEI_EXEC_PRESERVE_REGISTER) {
+                u64 valr = 0;
+                rc = acpi_atomic_read(&valr, &entry->register_region);
+                if (rc)
+                        return rc;
+                valr &= ~(entry->mask << entry->register_region.bit_offset);
+                val |= valr;
+        }
+        rc = acpi_atomic_write(val, &entry->register_region);
+
+        return rc;
+}
+
+int apei_exec_write_register(struct apei_exec_context *ctx,
+                             struct acpi_whea_header *entry)
+{
+        return __apei_exec_write_register(entry, ctx->value);
+}
+EXPORT_SYMBOL_GPL(apei_exec_write_register);
+
+int apei_exec_write_register_value(struct apei_exec_context *ctx,
+                                   struct acpi_whea_header *entry)
+{
+        int rc;
+
+        ctx->value = entry->value;
+        rc = apei_exec_write_register(ctx, entry);
+
+        return rc;
+}
+EXPORT_SYMBOL_GPL(apei_exec_write_register_value);
+
+int apei_exec_noop(struct apei_exec_context *ctx,
+                   struct acpi_whea_header *entry)
+{
+        return 0;
+}
+EXPORT_SYMBOL_GPL(apei_exec_noop);
+
+/*
+ * Interpret the specified action. Go through whole action table,
+ * execute all instructions belong to the action.
+ */
+int apei_exec_run(struct apei_exec_context *ctx, u8 action)
+{
+        int rc;
+        u32 i, ip;
+        struct acpi_whea_header *entry;
+        apei_exec_ins_func_t run;
+
+        ctx->ip = 0;
+
+        /*
+         * "ip" is the instruction pointer of current instruction,
+         * "ctx->ip" specifies the next instruction to executed,
+         * instruction "run" function may change the "ctx->ip" to
+         * implement "goto" semantics.
+         */
+rewind:
+        ip = 0;
+        for (i = 0; i < ctx->entries; i++) {
+                entry = &ctx->action_table[i];
+                if (entry->action != action)
+                        continue;
+                if (ip == ctx->ip) {
+                        if (entry->instruction >= ctx->instructions ||
+                            !ctx->ins_table[entry->instruction].run) {
+                                pr_warning(FW_WARN APEI_PFX
+                        "Invalid action table, unknown instruction type: %d\n",
+                                           entry->instruction);
+                                return -EINVAL;
+                        }
+                        run = ctx->ins_table[entry->instruction].run;
+                        rc = run(ctx, entry);
+                        if (rc < 0)
+                                return rc;
+                        else if (rc != APEI_EXEC_SET_IP)
+                                ctx->ip++;
+                }
+                ip++;
+                if (ctx->ip < ip)
+                        goto rewind;
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(apei_exec_run);
+
+typedef int (*apei_exec_entry_func_t)(struct apei_exec_context *ctx,
+                                      struct acpi_whea_header *entry,
+                                      void *data);
+
+static int apei_exec_for_each_entry(struct apei_exec_context *ctx,
+                                    apei_exec_entry_func_t func,
+                                    void *data,
+                                    int *end)
+{
+        u8 ins;
+        int i, rc;
+        struct acpi_whea_header *entry;
+        struct apei_exec_ins_type *ins_table = ctx->ins_table;
+
+        for (i = 0; i < ctx->entries; i++) {
+                entry = ctx->action_table + i;
+                ins = entry->instruction;
+                if (end)
+                        *end = i;
+                if (ins >= ctx->instructions || !ins_table[ins].run) {
+                        pr_warning(FW_WARN APEI_PFX
+                        "Invalid action table, unknown instruction type: %d\n",
+                                   ins);
+                        return -EINVAL;
+                }
+                rc = func(ctx, entry, data);
+                if (rc)
+                        return rc;
+        }
+
+        return 0;
+}
+
+static int pre_map_gar_callback(struct apei_exec_context *ctx,
+                                struct acpi_whea_header *entry,
+                                void *data)
+{
+        u8 ins = entry->instruction;
+
+        if (ctx->ins_table[ins].flags & APEI_EXEC_INS_ACCESS_REGISTER)
+                return acpi_pre_map_gar(&entry->register_region);
+
+        return 0;
+}
+
+/*
+ * Pre-map all GARs in action table to make it possible to access them
+ * in NMI handler.
+ */
+int apei_exec_pre_map_gars(struct apei_exec_context *ctx)
+{
+        int rc, end;
+
+        rc = apei_exec_for_each_entry(ctx, pre_map_gar_callback,
+                                      NULL, &end);
+        if (rc) {
+                struct apei_exec_context ctx_unmap;
+                memcpy(&ctx_unmap, ctx, sizeof(*ctx));
+                ctx_unmap.entries = end;
+                apei_exec_post_unmap_gars(&ctx_unmap);
+        }
+
+        return rc;
+}
+EXPORT_SYMBOL_GPL(apei_exec_pre_map_gars);
+
+static int post_unmap_gar_callback(struct apei_exec_context *ctx,
+                                   struct acpi_whea_header *entry,
+                                   void *data)
+{
+        u8 ins = entry->instruction;
+
+        if (ctx->ins_table[ins].flags & APEI_EXEC_INS_ACCESS_REGISTER)
+                acpi_post_unmap_gar(&entry->register_region);
+
+        return 0;
+}
+
+/* Post-unmap all GAR in action table. */
+int apei_exec_post_unmap_gars(struct apei_exec_context *ctx)
+{
+        return apei_exec_for_each_entry(ctx, post_unmap_gar_callback,
+                                        NULL, NULL);
+}
+EXPORT_SYMBOL_GPL(apei_exec_post_unmap_gars);
+
+/*
+ * Resource management for GARs in APEI
+ */
+struct apei_res {
+        struct list_head list;
+        unsigned long start;
+        unsigned long end;
+};
+
+/* Collect all resources requested, to avoid conflict */
+struct apei_resources apei_resources_all = {
+        .iomem = LIST_HEAD_INIT(apei_resources_all.iomem),
+        .ioport = LIST_HEAD_INIT(apei_resources_all.ioport),
+};
+
+static int apei_res_add(struct list_head *res_list,
+                        unsigned long start, unsigned long size)
+{
+        struct apei_res *res, *resn, *res_ins = NULL;
+        unsigned long end = start + size;
+
+        if (end <= start)
+                return 0;
+repeat:
+        list_for_each_entry_safe(res, resn, res_list, list) {
+                if (res->start > end || res->end < start)
+                        continue;
+                else if (end <= res->end && start >= res->start) {
+                        kfree(res_ins);
+                        return 0;
+                }
+                list_del(&res->list);
+                res->start = start = min(res->start, start);
+                res->end = end = max(res->end, end);
+                kfree(res_ins);
+                res_ins = res;
+                goto repeat;
+        }
+
+        if (res_ins)
+                list_add(&res_ins->list, res_list);
+        else {
+                res_ins = kmalloc(sizeof(*res), GFP_KERNEL);
+                if (!res_ins)
+                        return -ENOMEM;
+                res_ins->start = start;
+                res_ins->end = end;
+                list_add(&res_ins->list, res_list);
+        }
+
+        return 0;
+}
+
+static int apei_res_sub(struct list_head *res_list1,
+                        struct list_head *res_list2)
+{
+        struct apei_res *res1, *resn1, *res2, *res;
+        res1 = list_entry(res_list1->next, struct apei_res, list);
+        resn1 = list_entry(res1->list.next, struct apei_res, list);
+        while (&res1->list != res_list1) {
+                list_for_each_entry(res2, res_list2, list) {
+                        if (res1->start >= res2->end ||
+                            res1->end <= res2->start)
+                                continue;
+                        else if (res1->end <= res2->end &&
+                                 res1->start >= res2->start) {
+                                list_del(&res1->list);
+                                kfree(res1);
+                                break;
+                        } else if (res1->end > res2->end &&
+                                   res1->start < res2->start) {
+                                res = kmalloc(sizeof(*res), GFP_KERNEL);
+                                if (!res)
+                                        return -ENOMEM;
+                                res->start = res2->end;
+                                res->end = res1->end;
+                                res1->end = res2->start;
+                                list_add(&res->list, &res1->list);
+                                resn1 = res;
+                        } else {
+                                if (res1->start < res2->start)
+                                        res1->end = res2->start;
+                                else
+                                        res1->start = res2->end;
+                        }
+                }
+                res1 = resn1;
+                resn1 = list_entry(resn1->list.next, struct apei_res, list);
+        }
+
+        return 0;
+}
+
+static void apei_res_clean(struct list_head *res_list)
+{
+        struct apei_res *res, *resn;
+
+        list_for_each_entry_safe(res, resn, res_list, list) {
+                list_del(&res->list);
+                kfree(res);
+        }
+}
+
+void apei_resources_fini(struct apei_resources *resources)
+{
+        apei_res_clean(&resources->iomem);
+        apei_res_clean(&resources->ioport);
+}
+EXPORT_SYMBOL_GPL(apei_resources_fini);
+
+static int apei_resources_merge(struct apei_resources *resources1,
+                                struct apei_resources *resources2)
+{
+        int rc;
+        struct apei_res *res;
+
+        list_for_each_entry(res, &resources2->iomem, list) {
+                rc = apei_res_add(&resources1->iomem, res->start,
+                                  res->end - res->start);
+                if (rc)
+                        return rc;
+        }
+        list_for_each_entry(res, &resources2->ioport, list) {
+                rc = apei_res_add(&resources1->ioport, res->start,
+                                  res->end - res->start);
+                if (rc)
+                        return rc;
+        }
+
+        return 0;
+}
+
+/*
+ * EINJ has two groups of GARs (EINJ table entry and trigger table
+ * entry), so common resources are subtracted from the trigger table
+ * resources before the second requesting.
+ */
+int apei_resources_sub(struct apei_resources *resources1,
+                       struct apei_resources *resources2)
+{
+        int rc;
+
+        rc = apei_res_sub(&resources1->iomem, &resources2->iomem);
+        if (rc)
+                return rc;
+        return apei_res_sub(&resources1->ioport, &resources2->ioport);
+}
+EXPORT_SYMBOL_GPL(apei_resources_sub);
+
+/*
+ * IO memory/port rersource management mechanism is used to check
+ * whether memory/port area used by GARs conflicts with normal memory
+ * or IO memory/port of devices.
+ */
+int apei_resources_request(struct apei_resources *resources,
+                           const char *desc)
+{
+        struct apei_res *res, *res_bak;
+        struct resource *r;
+
+        apei_resources_sub(resources, &apei_resources_all);
+
+        list_for_each_entry(res, &resources->iomem, list) {
+                r = request_mem_region(res->start, res->end - res->start,
+                                       desc);
+                if (!r) {
+                        pr_err(APEI_PFX
+                "Can not request iomem region <%016llx-%016llx> for GARs.\n",
+                               (unsigned long long)res->start,
+                               (unsigned long long)res->end);
+                        res_bak = res;
+                        goto err_unmap_iomem;
+                }
+        }
+
+        list_for_each_entry(res, &resources->ioport, list) {
+                r = request_region(res->start, res->end - res->start, desc);
+                if (!r) {
+                        pr_err(APEI_PFX
+                "Can not request ioport region <%016llx-%016llx> for GARs.\n",
+                               (unsigned long long)res->start,
+                               (unsigned long long)res->end);
+                        res_bak = res;
+                        goto err_unmap_ioport;
+                }
+        }
+
+        apei_resources_merge(&apei_resources_all, resources);
+
+        return 0;
+err_unmap_ioport:
+        list_for_each_entry(res, &resources->ioport, list) {
+                if (res == res_bak)
+                        break;
+                release_mem_region(res->start, res->end - res->start);
+        }
+        res_bak = NULL;
+err_unmap_iomem:
+        list_for_each_entry(res, &resources->iomem, list) {
+                if (res == res_bak)
+                        break;
+                release_region(res->start, res->end - res->start);
+        }
+        return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(apei_resources_request);
+
+void apei_resources_release(struct apei_resources *resources)
+{
+        struct apei_res *res;
+
+        list_for_each_entry(res, &resources->iomem, list)
+                release_mem_region(res->start, res->end - res->start);
+        list_for_each_entry(res, &resources->ioport, list)
+                release_region(res->start, res->end - res->start);
+
+        apei_resources_sub(&apei_resources_all, resources);
+}
+EXPORT_SYMBOL_GPL(apei_resources_release);
+
+static int apei_check_gar(struct acpi_generic_address *reg, u64 *paddr)
+{
+        u32 width, space_id;
+
+        width = reg->bit_width;
+        space_id = reg->space_id;
+        /* Handle possible alignment issues */
+        memcpy(paddr, &reg->address, sizeof(*paddr));
+        if (!*paddr) {
+                pr_warning(FW_BUG APEI_PFX
+                           "Invalid physical address in GAR [0x%llx/%u/%u]\n",
+                           *paddr, width, space_id);
+                return -EINVAL;
+        }
+
+        if ((width != 8) && (width != 16) && (width != 32) && (width != 64)) {
+                pr_warning(FW_BUG APEI_PFX
+                           "Invalid bit width in GAR [0x%llx/%u/%u]\n",
+                           *paddr, width, space_id);
+                return -EINVAL;
+        }
+
+        if (space_id != ACPI_ADR_SPACE_SYSTEM_MEMORY &&
+            space_id != ACPI_ADR_SPACE_SYSTEM_IO) {
+                pr_warning(FW_BUG APEI_PFX
+                           "Invalid address space type in GAR [0x%llx/%u/%u]\n",
+                           *paddr, width, space_id);
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+static int collect_res_callback(struct apei_exec_context *ctx,
+                                struct acpi_whea_header *entry,
+                                void *data)
+{
+        struct apei_resources *resources = data;
+        struct acpi_generic_address *reg = &entry->register_region;
+        u8 ins = entry->instruction;
+        u64 paddr;
+        int rc;
+
+        if (!(ctx->ins_table[ins].flags & APEI_EXEC_INS_ACCESS_REGISTER))
+                return 0;
+
+        rc = apei_check_gar(reg, &paddr);
+        if (rc)
+                return rc;
+
+        switch (reg->space_id) {
+        case ACPI_ADR_SPACE_SYSTEM_MEMORY:
+                return apei_res_add(&resources->iomem, paddr,
+                                    reg->bit_width / 8);
+        case ACPI_ADR_SPACE_SYSTEM_IO:
+                return apei_res_add(&resources->ioport, paddr,
+                                    reg->bit_width / 8);
+        default:
+                return -EINVAL;
+        }
+}
+
+/*
+ * Same register may be used by multiple instructions in GARs, so
+ * resources are collected before requesting.
+ */
+int apei_exec_collect_resources(struct apei_exec_context *ctx,
+                                struct apei_resources *resources)
+{
+        return apei_exec_for_each_entry(ctx, collect_res_callback,
+                                        resources, NULL);
+}
+EXPORT_SYMBOL_GPL(apei_exec_collect_resources);
+
+struct dentry *apei_get_debugfs_dir(void)
+{
+        static struct dentry *dapei;
+
+        if (!dapei)
+                dapei = debugfs_create_dir("apei", NULL);
+
+        return dapei;
+}
+EXPORT_SYMBOL_GPL(apei_get_debugfs_dir);
diff --git a/drivers/acpi/apei/apei-internal.h b/drivers/acpi/apei/apei-internal.h
new file mode 100644
index 000000000000..18df1e940276
--- /dev/null
+++ b/drivers/acpi/apei/apei-internal.h
@@ -0,0 +1,114 @@
+/*
+ * apei-internal.h - ACPI Platform Error Interface internal
+ * definations.
+ */
+
+#ifndef APEI_INTERNAL_H
+#define APEI_INTERNAL_H
+
+#include <linux/cper.h>
+
+struct apei_exec_context;
+
+typedef int (*apei_exec_ins_func_t)(struct apei_exec_context *ctx,
+                                    struct acpi_whea_header *entry);
+
+#define APEI_EXEC_INS_ACCESS_REGISTER   0x0001
+
+struct apei_exec_ins_type {
+        u32 flags;
+        apei_exec_ins_func_t run;
+};
+
+struct apei_exec_context {
+        u32 ip;
+        u64 value;
+        u64 var1;
+        u64 var2;
+        u64 src_base;
+        u64 dst_base;
+        struct apei_exec_ins_type *ins_table;
+        u32 instructions;
+        struct acpi_whea_header *action_table;
+        u32 entries;
+};
+
+void apei_exec_ctx_init(struct apei_exec_context *ctx,
+                        struct apei_exec_ins_type *ins_table,
+                        u32 instructions,
+                        struct acpi_whea_header *action_table,
+                        u32 entries);
+
+static inline void apei_exec_ctx_set_input(struct apei_exec_context *ctx,
+                                           u64 input)
+{
+        ctx->value = input;
+}
+
+static inline u64 apei_exec_ctx_get_output(struct apei_exec_context *ctx)
+{
+        return ctx->value;
+}
+
+int apei_exec_run(struct apei_exec_context *ctx, u8 action);
+
+/* Common instruction implementation */
+
+/* IP has been set in instruction function */
+#define APEI_EXEC_SET_IP        1
+
+int __apei_exec_read_register(struct acpi_whea_header *entry, u64 *val);
+int __apei_exec_write_register(struct acpi_whea_header *entry, u64 val);
+int apei_exec_read_register(struct apei_exec_context *ctx,
+                            struct acpi_whea_header *entry);
+int apei_exec_read_register_value(struct apei_exec_context *ctx,
+                                  struct acpi_whea_header *entry);
+int apei_exec_write_register(struct apei_exec_context *ctx,
+                             struct acpi_whea_header *entry);
+int apei_exec_write_register_value(struct apei_exec_context *ctx,
+                                   struct acpi_whea_header *entry);
+int apei_exec_noop(struct apei_exec_context *ctx,
+                   struct acpi_whea_header *entry);
+int apei_exec_pre_map_gars(struct apei_exec_context *ctx);
+int apei_exec_post_unmap_gars(struct apei_exec_context *ctx);
+
+struct apei_resources {
+        struct list_head iomem;
+        struct list_head ioport;
+};
+
+static inline void apei_resources_init(struct apei_resources *resources)
+{
+        INIT_LIST_HEAD(&resources->iomem);
+        INIT_LIST_HEAD(&resources->ioport);
+}
+
+void apei_resources_fini(struct apei_resources *resources);
+int apei_resources_sub(struct apei_resources *resources1,
+                       struct apei_resources *resources2);
+int apei_resources_request(struct apei_resources *resources,
+                           const char *desc);
+void apei_resources_release(struct apei_resources *resources);
+int apei_exec_collect_resources(struct apei_exec_context *ctx,
+                                struct apei_resources *resources);
+
+struct dentry;
+struct dentry *apei_get_debugfs_dir(void);
+
+#define apei_estatus_for_each_section(estatus, section)                 \
+        for (section = (struct acpi_hest_generic_data *)(estatus + 1);  \
+             (void *)section - (void *)estatus < estatus->data_length;  \
+             section = (void *)(section+1) + section->error_data_length)
+
+static inline u32 apei_estatus_len(struct acpi_hest_generic_status *estatus)
+{
+        if (estatus->raw_data_length)
+                return estatus->raw_data_offset + \
+                        estatus->raw_data_length;
+        else
+                return sizeof(*estatus) + estatus->data_length;
+}
+
+int apei_estatus_check_header(const struct acpi_hest_generic_status *estatus);
+int apei_estatus_check(const struct acpi_hest_generic_status *estatus);
+#endif
diff --git a/drivers/acpi/apei/cper.c b/drivers/acpi/apei/cper.c
new file mode 100644
index 000000000000..f4cf2fc4c8c1
--- /dev/null
+++ b/drivers/acpi/apei/cper.c
@@ -0,0 +1,84 @@
+/*
+ * UEFI Common Platform Error Record (CPER) support
+ *
+ * Copyright (C) 2010, Intel Corp.
+ *      Author: Huang Ying <ying.huang@intel.com>
+ *
+ * CPER is the format used to describe platform hardware error by
+ * various APEI tables, such as ERST, BERT and HEST etc.
+ *
+ * For more information about CPER, please refer to Appendix N of UEFI
+ * Specification version 2.3.
+ *
+ * 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.
+ *
+ * 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/time.h>
+#include <linux/cper.h>
+#include <linux/acpi.h>
+
+/*
+ * CPER record ID need to be unique even after reboot, because record
+ * ID is used as index for ERST storage, while CPER records from
+ * multiple boot may co-exist in ERST.
+ */
+u64 cper_next_record_id(void)
+{
+        static atomic64_t seq;
+
+        if (!atomic64_read(&seq))
+                atomic64_set(&seq, ((u64)get_seconds()) << 32);
+
+        return atomic64_inc_return(&seq);
+}
+EXPORT_SYMBOL_GPL(cper_next_record_id);
+
+int apei_estatus_check_header(const struct acpi_hest_generic_status *estatus)
+{
+        if (estatus->data_length &&
+            estatus->data_length < sizeof(struct acpi_hest_generic_data))
+                return -EINVAL;
+        if (estatus->raw_data_length &&
+            estatus->raw_data_offset < sizeof(*estatus) + estatus->data_length)
+                return -EINVAL;
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(apei_estatus_check_header);
+
+int apei_estatus_check(const struct acpi_hest_generic_status *estatus)
+{
+        struct acpi_hest_generic_data *gdata;
+        unsigned int data_len, gedata_len;
+        int rc;
+
+        rc = apei_estatus_check_header(estatus);
+        if (rc)
+                return rc;
+        data_len = estatus->data_length;
+        gdata = (struct acpi_hest_generic_data *)(estatus + 1);
+        while (data_len > sizeof(*gdata)) {
+                gedata_len = gdata->error_data_length;
+                if (gedata_len > data_len - sizeof(*gdata))
+                        return -EINVAL;
+                data_len -= gedata_len + sizeof(*gdata);
+        }
+        if (data_len)
+                return -EINVAL;
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(apei_estatus_check);
diff --git a/drivers/acpi/apei/einj.c b/drivers/acpi/apei/einj.c
new file mode 100644
index 000000000000..465c885938ee
--- /dev/null
+++ b/drivers/acpi/apei/einj.c
@@ -0,0 +1,548 @@
+/*
+ * APEI Error INJection support
+ *
+ * EINJ provides a hardware error injection mechanism, this is useful
+ * for debugging and testing of other APEI and RAS features.
+ *
+ * For more information about EINJ, please refer to ACPI Specification
+ * version 4.0, section 17.5.
+ *
+ * Copyright 2009-2010 Intel Corp.
+ *   Author: 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 version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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/init.h>
+#include <linux/io.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/nmi.h>
+#include <linux/delay.h>
+#include <acpi/acpi.h>
+
+#include "apei-internal.h"
+
+#define EINJ_PFX "EINJ: "
+
+#define SPIN_UNIT               100                     /* 100ns */
+/* Firmware should respond within 1 miliseconds */
+#define FIRMWARE_TIMEOUT        (1 * NSEC_PER_MSEC)
+
+/*
+ * Some BIOSes allow parameters to the SET_ERROR_TYPE entries in the
+ * EINJ table through an unpublished extension. Use with caution as
+ * most will ignore the parameter and make their own choice of address
+ * for error injection.
+ */
+struct einj_parameter {
+        u64 type;
+        u64 reserved1;
+        u64 reserved2;
+        u64 param1;
+        u64 param2;
+};
+
+#define EINJ_OP_BUSY                    0x1
+#define EINJ_STATUS_SUCCESS             0x0
+#define EINJ_STATUS_FAIL                0x1
+#define EINJ_STATUS_INVAL               0x2
+
+#define EINJ_TAB_ENTRY(tab)                                             \
+        ((struct acpi_whea_header *)((char *)(tab) +                    \
+                                    sizeof(struct acpi_table_einj)))
+
+static struct acpi_table_einj *einj_tab;
+
+static struct apei_resources einj_resources;
+
+static struct apei_exec_ins_type einj_ins_type[] = {
+        [ACPI_EINJ_READ_REGISTER] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run   = apei_exec_read_register,
+        },
+        [ACPI_EINJ_READ_REGISTER_VALUE] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run   = apei_exec_read_register_value,
+        },
+        [ACPI_EINJ_WRITE_REGISTER] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run   = apei_exec_write_register,
+        },
+        [ACPI_EINJ_WRITE_REGISTER_VALUE] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run   = apei_exec_write_register_value,
+        },
+        [ACPI_EINJ_NOOP] = {
+                .flags = 0,
+                .run   = apei_exec_noop,
+        },
+};
+
+/*
+ * Prevent EINJ interpreter to run simultaneously, because the
+ * corresponding firmware implementation may not work properly when
+ * invoked simultaneously.
+ */
+static DEFINE_MUTEX(einj_mutex);
+
+static struct einj_parameter *einj_param;
+
+static void einj_exec_ctx_init(struct apei_exec_context *ctx)
+{
+        apei_exec_ctx_init(ctx, einj_ins_type, ARRAY_SIZE(einj_ins_type),
+                           EINJ_TAB_ENTRY(einj_tab), einj_tab->entries);
+}
+
+static int __einj_get_available_error_type(u32 *type)
+{
+        struct apei_exec_context ctx;
+        int rc;
+
+        einj_exec_ctx_init(&ctx);
+        rc = apei_exec_run(&ctx, ACPI_EINJ_GET_ERROR_TYPE);
+        if (rc)
+                return rc;
+        *type = apei_exec_ctx_get_output(&ctx);
+
+        return 0;
+}
+
+/* Get error injection capabilities of the platform */
+static int einj_get_available_error_type(u32 *type)
+{
+        int rc;
+
+        mutex_lock(&einj_mutex);
+        rc = __einj_get_available_error_type(type);
+        mutex_unlock(&einj_mutex);
+
+        return rc;
+}
+
+static int einj_timedout(u64 *t)
+{
+        if ((s64)*t < SPIN_UNIT) {
+                pr_warning(FW_WARN EINJ_PFX
+                           "Firmware does not respond in time\n");
+                return 1;
+        }
+        *t -= SPIN_UNIT;
+        ndelay(SPIN_UNIT);
+        touch_nmi_watchdog();
+        return 0;
+}
+
+static u64 einj_get_parameter_address(void)
+{
+        int i;
+        u64 paddr = 0;
+        struct acpi_whea_header *entry;
+
+        entry = EINJ_TAB_ENTRY(einj_tab);
+        for (i = 0; i < einj_tab->entries; i++) {
+                if (entry->action == ACPI_EINJ_SET_ERROR_TYPE &&
+                    entry->instruction == ACPI_EINJ_WRITE_REGISTER &&
+                    entry->register_region.space_id ==
+                    ACPI_ADR_SPACE_SYSTEM_MEMORY)
+                        memcpy(&paddr, &entry->register_region.address,
+                               sizeof(paddr));
+                entry++;
+        }
+
+        return paddr;
+}
+
+/* do sanity check to trigger table */
+static int einj_check_trigger_header(struct acpi_einj_trigger *trigger_tab)
+{
+        if (trigger_tab->header_size != sizeof(struct acpi_einj_trigger))
+                return -EINVAL;
+        if (trigger_tab->table_size > PAGE_SIZE ||
+            trigger_tab->table_size <= trigger_tab->header_size)
+                return -EINVAL;
+        if (trigger_tab->entry_count !=
+            (trigger_tab->table_size - trigger_tab->header_size) /
+            sizeof(struct acpi_einj_entry))
+                return -EINVAL;
+
+        return 0;
+}
+
+/* Execute instructions in trigger error action table */
+static int __einj_error_trigger(u64 trigger_paddr)
+{
+        struct acpi_einj_trigger *trigger_tab = NULL;
+        struct apei_exec_context trigger_ctx;
+        struct apei_resources trigger_resources;
+        struct acpi_whea_header *trigger_entry;
+        struct resource *r;
+        u32 table_size;
+        int rc = -EIO;
+
+        r = request_mem_region(trigger_paddr, sizeof(*trigger_tab),
+                               "APEI EINJ Trigger Table");
+        if (!r) {
+                pr_err(EINJ_PFX
+        "Can not request iomem region <%016llx-%016llx> for Trigger table.\n",
+                       (unsigned long long)trigger_paddr,
+                       (unsigned long long)trigger_paddr+sizeof(*trigger_tab));
+                goto out;
+        }
+        trigger_tab = ioremap_cache(trigger_paddr, sizeof(*trigger_tab));
+        if (!trigger_tab) {
+                pr_err(EINJ_PFX "Failed to map trigger table!\n");
+                goto out_rel_header;
+        }
+        rc = einj_check_trigger_header(trigger_tab);
+        if (rc) {
+                pr_warning(FW_BUG EINJ_PFX
+                           "The trigger error action table is invalid\n");
+                goto out_rel_header;
+        }
+        rc = -EIO;
+        table_size = trigger_tab->table_size;
+        r = request_mem_region(trigger_paddr + sizeof(*trigger_tab),
+                               table_size - sizeof(*trigger_tab),
+                               "APEI EINJ Trigger Table");
+        if (!r) {
+                pr_err(EINJ_PFX
+"Can not request iomem region <%016llx-%016llx> for Trigger Table Entry.\n",
+                       (unsigned long long)trigger_paddr+sizeof(*trigger_tab),
+                       (unsigned long long)trigger_paddr + table_size);
+                goto out_rel_header;
+        }
+        iounmap(trigger_tab);
+        trigger_tab = ioremap_cache(trigger_paddr, table_size);
+        if (!trigger_tab) {
+                pr_err(EINJ_PFX "Failed to map trigger table!\n");
+                goto out_rel_entry;
+        }
+        trigger_entry = (struct acpi_whea_header *)
+                ((char *)trigger_tab + sizeof(struct acpi_einj_trigger));
+        apei_resources_init(&trigger_resources);
+        apei_exec_ctx_init(&trigger_ctx, einj_ins_type,
+                           ARRAY_SIZE(einj_ins_type),
+                           trigger_entry, trigger_tab->entry_count);
+        rc = apei_exec_collect_resources(&trigger_ctx, &trigger_resources);
+        if (rc)
+                goto out_fini;
+        rc = apei_resources_sub(&trigger_resources, &einj_resources);
+        if (rc)
+                goto out_fini;
+        rc = apei_resources_request(&trigger_resources, "APEI EINJ Trigger");
+        if (rc)
+                goto out_fini;
+        rc = apei_exec_pre_map_gars(&trigger_ctx);
+        if (rc)
+                goto out_release;
+
+        rc = apei_exec_run(&trigger_ctx, ACPI_EINJ_TRIGGER_ERROR);
+
+        apei_exec_post_unmap_gars(&trigger_ctx);
+out_release:
+        apei_resources_release(&trigger_resources);
+out_fini:
+        apei_resources_fini(&trigger_resources);
+out_rel_entry:
+        release_mem_region(trigger_paddr + sizeof(*trigger_tab),
+                           table_size - sizeof(*trigger_tab));
+out_rel_header:
+        release_mem_region(trigger_paddr, sizeof(*trigger_tab));
+out:
+        if (trigger_tab)
+                iounmap(trigger_tab);
+
+        return rc;
+}
+
+static int __einj_error_inject(u32 type, u64 param1, u64 param2)
+{
+        struct apei_exec_context ctx;
+        u64 val, trigger_paddr, timeout = FIRMWARE_TIMEOUT;
+        int rc;
+
+        einj_exec_ctx_init(&ctx);
+
+        rc = apei_exec_run(&ctx, ACPI_EINJ_BEGIN_OPERATION);
+        if (rc)
+                return rc;
+        apei_exec_ctx_set_input(&ctx, type);
+        rc = apei_exec_run(&ctx, ACPI_EINJ_SET_ERROR_TYPE);
+        if (rc)
+                return rc;
+        if (einj_param) {
+                writeq(param1, &einj_param->param1);
+                writeq(param2, &einj_param->param2);
+        }
+        rc = apei_exec_run(&ctx, ACPI_EINJ_EXECUTE_OPERATION);
+        if (rc)
+                return rc;
+        for (;;) {
+                rc = apei_exec_run(&ctx, ACPI_EINJ_CHECK_BUSY_STATUS);
+                if (rc)
+                        return rc;
+                val = apei_exec_ctx_get_output(&ctx);
+                if (!(val & EINJ_OP_BUSY))
+                        break;
+                if (einj_timedout(&timeout))
+                        return -EIO;
+        }
+        rc = apei_exec_run(&ctx, ACPI_EINJ_GET_COMMAND_STATUS);
+        if (rc)
+                return rc;
+        val = apei_exec_ctx_get_output(&ctx);
+        if (val != EINJ_STATUS_SUCCESS)
+                return -EBUSY;
+
+        rc = apei_exec_run(&ctx, ACPI_EINJ_GET_TRIGGER_TABLE);
+        if (rc)
+                return rc;
+        trigger_paddr = apei_exec_ctx_get_output(&ctx);
+        rc = __einj_error_trigger(trigger_paddr);
+        if (rc)
+                return rc;
+        rc = apei_exec_run(&ctx, ACPI_EINJ_END_OPERATION);
+
+        return rc;
+}
+
+/* Inject the specified hardware error */
+static int einj_error_inject(u32 type, u64 param1, u64 param2)
+{
+        int rc;
+
+        mutex_lock(&einj_mutex);
+        rc = __einj_error_inject(type, param1, param2);
+        mutex_unlock(&einj_mutex);
+
+        return rc;
+}
+
+static u32 error_type;
+static u64 error_param1;
+static u64 error_param2;
+static struct dentry *einj_debug_dir;
+
+static int available_error_type_show(struct seq_file *m, void *v)
+{
+        int rc;
+        u32 available_error_type = 0;
+
+        rc = einj_get_available_error_type(&available_error_type);
+        if (rc)
+                return rc;
+        if (available_error_type & 0x0001)
+                seq_printf(m, "0x00000001\tProcessor Correctable\n");
+        if (available_error_type & 0x0002)
+                seq_printf(m, "0x00000002\tProcessor Uncorrectable non-fatal\n");
+        if (available_error_type & 0x0004)
+                seq_printf(m, "0x00000004\tProcessor Uncorrectable fatal\n");
+        if (available_error_type & 0x0008)
+                seq_printf(m, "0x00000008\tMemory Correctable\n");
+        if (available_error_type & 0x0010)
+                seq_printf(m, "0x00000010\tMemory Uncorrectable non-fatal\n");
+        if (available_error_type & 0x0020)
+                seq_printf(m, "0x00000020\tMemory Uncorrectable fatal\n");
+        if (available_error_type & 0x0040)
+                seq_printf(m, "0x00000040\tPCI Express Correctable\n");
+        if (available_error_type & 0x0080)
+                seq_printf(m, "0x00000080\tPCI Express Uncorrectable non-fatal\n");
+        if (available_error_type & 0x0100)
+                seq_printf(m, "0x00000100\tPCI Express Uncorrectable fatal\n");
+        if (available_error_type & 0x0200)
+                seq_printf(m, "0x00000200\tPlatform Correctable\n");
+        if (available_error_type & 0x0400)
+                seq_printf(m, "0x00000400\tPlatform Uncorrectable non-fatal\n");
+        if (available_error_type & 0x0800)
+                seq_printf(m, "0x00000800\tPlatform Uncorrectable fatal\n");
+
+        return 0;
+}
+
+static int available_error_type_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, available_error_type_show, NULL);
+}
+
+static const struct file_operations available_error_type_fops = {
+        .open           = available_error_type_open,
+        .read           = seq_read,
+        .llseek         = seq_lseek,
+        .release        = single_release,
+};
+
+static int error_type_get(void *data, u64 *val)
+{
+        *val = error_type;
+
+        return 0;
+}
+
+static int error_type_set(void *data, u64 val)
+{
+        int rc;
+        u32 available_error_type = 0;
+
+        /* Only one error type can be specified */
+        if (val & (val - 1))
+                return -EINVAL;
+        rc = einj_get_available_error_type(&available_error_type);
+        if (rc)
+                return rc;
+        if (!(val & available_error_type))
+                return -EINVAL;
+        error_type = val;
+
+        return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(error_type_fops, error_type_get,
+                        error_type_set, "0x%llx\n");
+
+static int error_inject_set(void *data, u64 val)
+{
+        if (!error_type)
+                return -EINVAL;
+
+        return einj_error_inject(error_type, error_param1, error_param2);
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(error_inject_fops, NULL,
+                        error_inject_set, "%llu\n");
+
+static int einj_check_table(struct acpi_table_einj *einj_tab)
+{
+        if (einj_tab->header_length != sizeof(struct acpi_table_einj))
+                return -EINVAL;
+        if (einj_tab->header.length < sizeof(struct acpi_table_einj))
+                return -EINVAL;
+        if (einj_tab->entries !=
+            (einj_tab->header.length - sizeof(struct acpi_table_einj)) /
+            sizeof(struct acpi_einj_entry))
+                return -EINVAL;
+
+        return 0;
+}
+
+static int __init einj_init(void)
+{
+        int rc;
+        u64 param_paddr;
+        acpi_status status;
+        struct dentry *fentry;
+        struct apei_exec_context ctx;
+
+        if (acpi_disabled)
+                return -ENODEV;
+
+        status = acpi_get_table(ACPI_SIG_EINJ, 0,
+                                (struct acpi_table_header **)&einj_tab);
+        if (status == AE_NOT_FOUND) {
+                pr_info(EINJ_PFX "Table is not found!\n");
+                return -ENODEV;
+        } else if (ACPI_FAILURE(status)) {
+                const char *msg = acpi_format_exception(status);
+                pr_err(EINJ_PFX "Failed to get table, %s\n", msg);
+                return -EINVAL;
+        }
+
+        rc = einj_check_table(einj_tab);
+        if (rc) {
+                pr_warning(FW_BUG EINJ_PFX "EINJ table is invalid\n");
+                return -EINVAL;
+        }
+
+        rc = -ENOMEM;
+        einj_debug_dir = debugfs_create_dir("einj", apei_get_debugfs_dir());
+        if (!einj_debug_dir)
+                goto err_cleanup;
+        fentry = debugfs_create_file("available_error_type", S_IRUSR,
+                                     einj_debug_dir, NULL,
+                                     &available_error_type_fops);
+        if (!fentry)
+                goto err_cleanup;
+        fentry = debugfs_create_file("error_type", S_IRUSR | S_IWUSR,
+                                     einj_debug_dir, NULL, &error_type_fops);
+        if (!fentry)
+                goto err_cleanup;
+        fentry = debugfs_create_x64("param1", S_IRUSR | S_IWUSR,
+                                    einj_debug_dir, &error_param1);
+        if (!fentry)
+                goto err_cleanup;
+        fentry = debugfs_create_x64("param2", S_IRUSR | S_IWUSR,
+                                    einj_debug_dir, &error_param2);
+        if (!fentry)
+                goto err_cleanup;
+        fentry = debugfs_create_file("error_inject", S_IWUSR,
+                                     einj_debug_dir, NULL, &error_inject_fops);
+        if (!fentry)
+                goto err_cleanup;
+
+        apei_resources_init(&einj_resources);
+        einj_exec_ctx_init(&ctx);
+        rc = apei_exec_collect_resources(&ctx, &einj_resources);
+        if (rc)
+                goto err_fini;
+        rc = apei_resources_request(&einj_resources, "APEI EINJ");
+        if (rc)
+                goto err_fini;
+        rc = apei_exec_pre_map_gars(&ctx);
+        if (rc)
+                goto err_release;
+        param_paddr = einj_get_parameter_address();
+        if (param_paddr) {
+                einj_param = ioremap(param_paddr, sizeof(*einj_param));
+                rc = -ENOMEM;
+                if (!einj_param)
+                        goto err_unmap;
+        }
+
+        pr_info(EINJ_PFX "Error INJection is initialized.\n");
+
+        return 0;
+
+err_unmap:
+        apei_exec_post_unmap_gars(&ctx);
+err_release:
+        apei_resources_release(&einj_resources);
+err_fini:
+        apei_resources_fini(&einj_resources);
+err_cleanup:
+        debugfs_remove_recursive(einj_debug_dir);
+
+        return rc;
+}
+
+static void __exit einj_exit(void)
+{
+        struct apei_exec_context ctx;
+
+        if (einj_param)
+                iounmap(einj_param);
+        einj_exec_ctx_init(&ctx);
+        apei_exec_post_unmap_gars(&ctx);
+        apei_resources_release(&einj_resources);
+        apei_resources_fini(&einj_resources);
+        debugfs_remove_recursive(einj_debug_dir);
+}
+
+module_init(einj_init);
+module_exit(einj_exit);
+
+MODULE_AUTHOR("Huang Ying");
+MODULE_DESCRIPTION("APEI Error INJection support");
+MODULE_LICENSE("GPL");
diff --git a/drivers/acpi/apei/erst.c b/drivers/acpi/apei/erst.c
new file mode 100644
index 000000000000..2ebc39115507
--- /dev/null
+++ b/drivers/acpi/apei/erst.c
@@ -0,0 +1,855 @@
+/*
+ * APEI Error Record Serialization Table support
+ *
+ * ERST is a way provided by APEI to save and retrieve hardware error
+ * infomation to and from a persistent store.
+ *
+ * For more information about ERST, please refer to ACPI Specification
+ * version 4.0, section 17.4.
+ *
+ * Copyright 2010 Intel Corp.
+ *   Author: 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 version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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/init.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/acpi.h>
+#include <linux/uaccess.h>
+#include <linux/cper.h>
+#include <linux/nmi.h>
+#include <acpi/apei.h>
+
+#include "apei-internal.h"
+
+#define ERST_PFX "ERST: "
+
+/* ERST command status */
+#define ERST_STATUS_SUCCESS                     0x0
+#define ERST_STATUS_NOT_ENOUGH_SPACE            0x1
+#define ERST_STATUS_HARDWARE_NOT_AVAILABLE      0x2
+#define ERST_STATUS_FAILED                      0x3
+#define ERST_STATUS_RECORD_STORE_EMPTY          0x4
+#define ERST_STATUS_RECORD_NOT_FOUND            0x5
+
+#define ERST_TAB_ENTRY(tab)                                             \
+        ((struct acpi_whea_header *)((char *)(tab) +                    \
+                                     sizeof(struct acpi_table_erst)))
+
+#define SPIN_UNIT               100                     /* 100ns */
+/* Firmware should respond within 1 miliseconds */
+#define FIRMWARE_TIMEOUT        (1 * NSEC_PER_MSEC)
+#define FIRMWARE_MAX_STALL      50                      /* 50us */
+
+int erst_disable;
+EXPORT_SYMBOL_GPL(erst_disable);
+
+static struct acpi_table_erst *erst_tab;
+
+/* ERST Error Log Address Range atrributes */
+#define ERST_RANGE_RESERVED     0x0001
+#define ERST_RANGE_NVRAM        0x0002
+#define ERST_RANGE_SLOW         0x0004
+
+/*
+ * ERST Error Log Address Range, used as buffer for reading/writing
+ * error records.
+ */
+static struct erst_erange {
+        u64 base;
+        u64 size;
+        void __iomem *vaddr;
+        u32 attr;
+} erst_erange;
+
+/*
+ * Prevent ERST interpreter to run simultaneously, because the
+ * corresponding firmware implementation may not work properly when
+ * invoked simultaneously.
+ *
+ * It is used to provide exclusive accessing for ERST Error Log
+ * Address Range too.
+ */
+static DEFINE_SPINLOCK(erst_lock);
+
+static inline int erst_errno(int command_status)
+{
+        switch (command_status) {
+        case ERST_STATUS_SUCCESS:
+                return 0;
+        case ERST_STATUS_HARDWARE_NOT_AVAILABLE:
+                return -ENODEV;
+        case ERST_STATUS_NOT_ENOUGH_SPACE:
+                return -ENOSPC;
+        case ERST_STATUS_RECORD_STORE_EMPTY:
+        case ERST_STATUS_RECORD_NOT_FOUND:
+                return -ENOENT;
+        default:
+                return -EINVAL;
+        }
+}
+
+static int erst_timedout(u64 *t, u64 spin_unit)
+{
+        if ((s64)*t < spin_unit) {
+                pr_warning(FW_WARN ERST_PFX
+                           "Firmware does not respond in time\n");
+                return 1;
+        }
+        *t -= spin_unit;
+        ndelay(spin_unit);
+        touch_nmi_watchdog();
+        return 0;
+}
+
+static int erst_exec_load_var1(struct apei_exec_context *ctx,
+                               struct acpi_whea_header *entry)
+{
+        return __apei_exec_read_register(entry, &ctx->var1);
+}
+
+static int erst_exec_load_var2(struct apei_exec_context *ctx,
+                               struct acpi_whea_header *entry)
+{
+        return __apei_exec_read_register(entry, &ctx->var2);
+}
+
+static int erst_exec_store_var1(struct apei_exec_context *ctx,
+                                struct acpi_whea_header *entry)
+{
+        return __apei_exec_write_register(entry, ctx->var1);
+}
+
+static int erst_exec_add(struct apei_exec_context *ctx,
+                         struct acpi_whea_header *entry)
+{
+        ctx->var1 += ctx->var2;
+        return 0;
+}
+
+static int erst_exec_subtract(struct apei_exec_context *ctx,
+                              struct acpi_whea_header *entry)
+{
+        ctx->var1 -= ctx->var2;
+        return 0;
+}
+
+static int erst_exec_add_value(struct apei_exec_context *ctx,
+                               struct acpi_whea_header *entry)
+{
+        int rc;
+        u64 val;
+
+        rc = __apei_exec_read_register(entry, &val);
+        if (rc)
+                return rc;
+        val += ctx->value;
+        rc = __apei_exec_write_register(entry, val);
+        return rc;
+}
+
+static int erst_exec_subtract_value(struct apei_exec_context *ctx,
+                                    struct acpi_whea_header *entry)
+{
+        int rc;
+        u64 val;
+
+        rc = __apei_exec_read_register(entry, &val);
+        if (rc)
+                return rc;
+        val -= ctx->value;
+        rc = __apei_exec_write_register(entry, val);
+        return rc;
+}
+
+static int erst_exec_stall(struct apei_exec_context *ctx,
+                           struct acpi_whea_header *entry)
+{
+        u64 stall_time;
+
+        if (ctx->value > FIRMWARE_MAX_STALL) {
+                if (!in_nmi())
+                        pr_warning(FW_WARN ERST_PFX
+                        "Too long stall time for stall instruction: %llx.\n",
+                                   ctx->value);
+                stall_time = FIRMWARE_MAX_STALL;
+        } else
+                stall_time = ctx->value;
+        udelay(stall_time);
+        return 0;
+}
+
+static int erst_exec_stall_while_true(struct apei_exec_context *ctx,
+                                      struct acpi_whea_header *entry)
+{
+        int rc;
+        u64 val;
+        u64 timeout = FIRMWARE_TIMEOUT;
+        u64 stall_time;
+
+        if (ctx->var1 > FIRMWARE_MAX_STALL) {
+                if (!in_nmi())
+                        pr_warning(FW_WARN ERST_PFX
+                "Too long stall time for stall while true instruction: %llx.\n",
+                                   ctx->var1);
+                stall_time = FIRMWARE_MAX_STALL;
+        } else
+                stall_time = ctx->var1;
+
+        for (;;) {
+                rc = __apei_exec_read_register(entry, &val);
+                if (rc)
+                        return rc;
+                if (val != ctx->value)
+                        break;
+                if (erst_timedout(&timeout, stall_time * NSEC_PER_USEC))
+                        return -EIO;
+        }
+        return 0;
+}
+
+static int erst_exec_skip_next_instruction_if_true(
+        struct apei_exec_context *ctx,
+        struct acpi_whea_header *entry)
+{
+        int rc;
+        u64 val;
+
+        rc = __apei_exec_read_register(entry, &val);
+        if (rc)
+                return rc;
+        if (val == ctx->value) {
+                ctx->ip += 2;
+                return APEI_EXEC_SET_IP;
+        }
+
+        return 0;
+}
+
+static int erst_exec_goto(struct apei_exec_context *ctx,
+                          struct acpi_whea_header *entry)
+{
+        ctx->ip = ctx->value;
+        return APEI_EXEC_SET_IP;
+}
+
+static int erst_exec_set_src_address_base(struct apei_exec_context *ctx,
+                                          struct acpi_whea_header *entry)
+{
+        return __apei_exec_read_register(entry, &ctx->src_base);
+}
+
+static int erst_exec_set_dst_address_base(struct apei_exec_context *ctx,
+                                          struct acpi_whea_header *entry)
+{
+        return __apei_exec_read_register(entry, &ctx->dst_base);
+}
+
+static int erst_exec_move_data(struct apei_exec_context *ctx,
+                               struct acpi_whea_header *entry)
+{
+        int rc;
+        u64 offset;
+
+        rc = __apei_exec_read_register(entry, &offset);
+        if (rc)
+                return rc;
+        memmove((void *)ctx->dst_base + offset,
+                (void *)ctx->src_base + offset,
+                ctx->var2);
+
+        return 0;
+}
+
+static struct apei_exec_ins_type erst_ins_type[] = {
+        [ACPI_ERST_READ_REGISTER] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = apei_exec_read_register,
+        },
+        [ACPI_ERST_READ_REGISTER_VALUE] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = apei_exec_read_register_value,
+        },
+        [ACPI_ERST_WRITE_REGISTER] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = apei_exec_write_register,
+        },
+        [ACPI_ERST_WRITE_REGISTER_VALUE] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = apei_exec_write_register_value,
+        },
+        [ACPI_ERST_NOOP] = {
+                .flags = 0,
+                .run = apei_exec_noop,
+        },
+        [ACPI_ERST_LOAD_VAR1] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = erst_exec_load_var1,
+        },
+        [ACPI_ERST_LOAD_VAR2] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = erst_exec_load_var2,
+        },
+        [ACPI_ERST_STORE_VAR1] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = erst_exec_store_var1,
+        },
+        [ACPI_ERST_ADD] = {
+                .flags = 0,
+                .run = erst_exec_add,
+        },
+        [ACPI_ERST_SUBTRACT] = {
+                .flags = 0,
+                .run = erst_exec_subtract,
+        },
+        [ACPI_ERST_ADD_VALUE] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = erst_exec_add_value,
+        },
+        [ACPI_ERST_SUBTRACT_VALUE] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = erst_exec_subtract_value,
+        },
+        [ACPI_ERST_STALL] = {
+                .flags = 0,
+                .run = erst_exec_stall,
+        },
+        [ACPI_ERST_STALL_WHILE_TRUE] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = erst_exec_stall_while_true,
+        },
+        [ACPI_ERST_SKIP_NEXT_IF_TRUE] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = erst_exec_skip_next_instruction_if_true,
+        },
+        [ACPI_ERST_GOTO] = {
+                .flags = 0,
+                .run = erst_exec_goto,
+        },
+        [ACPI_ERST_SET_SRC_ADDRESS_BASE] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = erst_exec_set_src_address_base,
+        },
+        [ACPI_ERST_SET_DST_ADDRESS_BASE] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = erst_exec_set_dst_address_base,
+        },
+        [ACPI_ERST_MOVE_DATA] = {
+                .flags = APEI_EXEC_INS_ACCESS_REGISTER,
+                .run = erst_exec_move_data,
+        },
+};
+
+static inline void erst_exec_ctx_init(struct apei_exec_context *ctx)
+{
+        apei_exec_ctx_init(ctx, erst_ins_type, ARRAY_SIZE(erst_ins_type),
+                           ERST_TAB_ENTRY(erst_tab), erst_tab->entries);
+}
+
+static int erst_get_erange(struct erst_erange *range)
+{
+        struct apei_exec_context ctx;
+        int rc;
+
+        erst_exec_ctx_init(&ctx);
+        rc = apei_exec_run(&ctx, ACPI_ERST_GET_ERROR_RANGE);
+        if (rc)
+                return rc;
+        range->base = apei_exec_ctx_get_output(&ctx);
+        rc = apei_exec_run(&ctx, ACPI_ERST_GET_ERROR_LENGTH);
+        if (rc)
+                return rc;
+        range->size = apei_exec_ctx_get_output(&ctx);
+        rc = apei_exec_run(&ctx, ACPI_ERST_GET_ERROR_ATTRIBUTES);
+        if (rc)
+                return rc;
+        range->attr = apei_exec_ctx_get_output(&ctx);
+
+        return 0;
+}
+
+static ssize_t __erst_get_record_count(void)
+{
+        struct apei_exec_context ctx;
+        int rc;
+
+        erst_exec_ctx_init(&ctx);
+        rc = apei_exec_run(&ctx, ACPI_ERST_GET_RECORD_COUNT);
+        if (rc)
+                return rc;
+        return apei_exec_ctx_get_output(&ctx);
+}
+
+ssize_t erst_get_record_count(void)
+{
+        ssize_t count;
+        unsigned long flags;
+
+        if (erst_disable)
+                return -ENODEV;
+
+        spin_lock_irqsave(&erst_lock, flags);
+        count = __erst_get_record_count();
+        spin_unlock_irqrestore(&erst_lock, flags);
+
+        return count;
+}
+EXPORT_SYMBOL_GPL(erst_get_record_count);
+
+static int __erst_get_next_record_id(u64 *record_id)
+{
+        struct apei_exec_context ctx;
+        int rc;
+
+        erst_exec_ctx_init(&ctx);
+        rc = apei_exec_run(&ctx, ACPI_ERST_GET_RECORD_ID);
+        if (rc)
+                return rc;
+        *record_id = apei_exec_ctx_get_output(&ctx);
+
+        return 0;
+}
+
+/*
+ * Get the record ID of an existing error record on the persistent
+ * storage. If there is no error record on the persistent storage, the
+ * returned record_id is APEI_ERST_INVALID_RECORD_ID.
+ */
+int erst_get_next_record_id(u64 *record_id)
+{
+        int rc;
+        unsigned long flags;
+
+        if (erst_disable)
+                return -ENODEV;
+
+        spin_lock_irqsave(&erst_lock, flags);
+        rc = __erst_get_next_record_id(record_id);
+        spin_unlock_irqrestore(&erst_lock, flags);
+
+        return rc;
+}
+EXPORT_SYMBOL_GPL(erst_get_next_record_id);
+
+static int __erst_write_to_storage(u64 offset)
+{
+        struct apei_exec_context ctx;
+        u64 timeout = FIRMWARE_TIMEOUT;
+        u64 val;
+        int rc;
+
+        erst_exec_ctx_init(&ctx);
+        rc = apei_exec_run(&ctx, ACPI_ERST_BEGIN_WRITE);
+        if (rc)
+                return rc;
+        apei_exec_ctx_set_input(&ctx, offset);
+        rc = apei_exec_run(&ctx, ACPI_ERST_SET_RECORD_OFFSET);
+        if (rc)
+                return rc;
+        rc = apei_exec_run(&ctx, ACPI_ERST_EXECUTE_OPERATION);
+        if (rc)
+                return rc;
+        for (;;) {
+                rc = apei_exec_run(&ctx, ACPI_ERST_CHECK_BUSY_STATUS);
+                if (rc)
+                        return rc;
+                val = apei_exec_ctx_get_output(&ctx);
+                if (!val)
+                        break;
+                if (erst_timedout(&timeout, SPIN_UNIT))
+                        return -EIO;
+        }
+        rc = apei_exec_run(&ctx, ACPI_ERST_GET_COMMAND_STATUS);
+        if (rc)
+                return rc;
+        val = apei_exec_ctx_get_output(&ctx);
+        rc = apei_exec_run(&ctx, ACPI_ERST_END);
+        if (rc)
+                return rc;
+
+        return erst_errno(val);
+}
+
+static int __erst_read_from_storage(u64 record_id, u64 offset)
+{
+        struct apei_exec_context ctx;
+        u64 timeout = FIRMWARE_TIMEOUT;
+        u64 val;
+        int rc;
+
+        erst_exec_ctx_init(&ctx);
+        rc = apei_exec_run(&ctx, ACPI_ERST_BEGIN_READ);
+        if (rc)
+                return rc;
+        apei_exec_ctx_set_input(&ctx, offset);
+        rc = apei_exec_run(&ctx, ACPI_ERST_SET_RECORD_OFFSET);
+        if (rc)
+                return rc;
+        apei_exec_ctx_set_input(&ctx, record_id);
+        rc = apei_exec_run(&ctx, ACPI_ERST_SET_RECORD_ID);
+        if (rc)
+                return rc;
+        rc = apei_exec_run(&ctx, ACPI_ERST_EXECUTE_OPERATION);
+        if (rc)
+                return rc;
+        for (;;) {
+                rc = apei_exec_run(&ctx, ACPI_ERST_CHECK_BUSY_STATUS);
+                if (rc)
+                        return rc;
+                val = apei_exec_ctx_get_output(&ctx);
+                if (!val)
+                        break;
+                if (erst_timedout(&timeout, SPIN_UNIT))
+                        return -EIO;
+        };
+        rc = apei_exec_run(&ctx, ACPI_ERST_GET_COMMAND_STATUS);
+        if (rc)
+                return rc;
+        val = apei_exec_ctx_get_output(&ctx);
+        rc = apei_exec_run(&ctx, ACPI_ERST_END);
+        if (rc)
+                return rc;
+
+        return erst_errno(val);
+}
+
+static int __erst_clear_from_storage(u64 record_id)
+{
+        struct apei_exec_context ctx;
+        u64 timeout = FIRMWARE_TIMEOUT;
+        u64 val;
+        int rc;
+
+        erst_exec_ctx_init(&ctx);
+        rc = apei_exec_run(&ctx, ACPI_ERST_BEGIN_CLEAR);
+        if (rc)
+                return rc;
+        apei_exec_ctx_set_input(&ctx, record_id);
+        rc = apei_exec_run(&ctx, ACPI_ERST_SET_RECORD_ID);
+        if (rc)
+                return rc;
+        rc = apei_exec_run(&ctx, ACPI_ERST_EXECUTE_OPERATION);
+        if (rc)
+                return rc;
+        for (;;) {
+                rc = apei_exec_run(&ctx, ACPI_ERST_CHECK_BUSY_STATUS);
+                if (rc)
+                        return rc;
+                val = apei_exec_ctx_get_output(&ctx);
+                if (!val)
+                        break;
+                if (erst_timedout(&timeout, SPIN_UNIT))
+                        return -EIO;
+        }
+        rc = apei_exec_run(&ctx, ACPI_ERST_GET_COMMAND_STATUS);
+        if (rc)
+                return rc;
+        val = apei_exec_ctx_get_output(&ctx);
+        rc = apei_exec_run(&ctx, ACPI_ERST_END);
+        if (rc)
+                return rc;
+
+        return erst_errno(val);
+}
+
+/* NVRAM ERST Error Log Address Range is not supported yet */
+static void pr_unimpl_nvram(void)
+{
+        if (printk_ratelimit())
+                pr_warning(ERST_PFX
+                "NVRAM ERST Log Address Range is not implemented yet\n");
+}
+
+static int __erst_write_to_nvram(const struct cper_record_header *record)
+{
+        /* do not print message, because printk is not safe for NMI */
+        return -ENOSYS;
+}
+
+static int __erst_read_to_erange_from_nvram(u64 record_id, u64 *offset)
+{
+        pr_unimpl_nvram();
+        return -ENOSYS;
+}
+
+static int __erst_clear_from_nvram(u64 record_id)
+{
+        pr_unimpl_nvram();
+        return -ENOSYS;
+}
+
+int erst_write(const struct cper_record_header *record)
+{
+        int rc;
+        unsigned long flags;
+        struct cper_record_header *rcd_erange;
+
+        if (erst_disable)
+                return -ENODEV;
+
+        if (memcmp(record->signature, CPER_SIG_RECORD, CPER_SIG_SIZE))
+                return -EINVAL;
+
+        if (erst_erange.attr & ERST_RANGE_NVRAM) {
+                if (!spin_trylock_irqsave(&erst_lock, flags))
+                        return -EBUSY;
+                rc = __erst_write_to_nvram(record);
+                spin_unlock_irqrestore(&erst_lock, flags);
+                return rc;
+        }
+
+        if (record->record_length > erst_erange.size)
+                return -EINVAL;
+
+        if (!spin_trylock_irqsave(&erst_lock, flags))
+                return -EBUSY;
+        memcpy(erst_erange.vaddr, record, record->record_length);
+        rcd_erange = erst_erange.vaddr;
+        /* signature for serialization system */
+        memcpy(&rcd_erange->persistence_information, "ER", 2);
+
+        rc = __erst_write_to_storage(0);
+        spin_unlock_irqrestore(&erst_lock, flags);
+
+        return rc;
+}
+EXPORT_SYMBOL_GPL(erst_write);
+
+static int __erst_read_to_erange(u64 record_id, u64 *offset)
+{
+        int rc;
+
+        if (erst_erange.attr & ERST_RANGE_NVRAM)
+                return __erst_read_to_erange_from_nvram(
+                        record_id, offset);
+
+        rc = __erst_read_from_storage(record_id, 0);
+        if (rc)
+                return rc;
+        *offset = 0;
+
+        return 0;
+}
+
+static ssize_t __erst_read(u64 record_id, struct cper_record_header *record,
+                           size_t buflen)
+{
+        int rc;
+        u64 offset, len = 0;
+        struct cper_record_header *rcd_tmp;
+
+        rc = __erst_read_to_erange(record_id, &offset);
+        if (rc)
+                return rc;
+        rcd_tmp = erst_erange.vaddr + offset;
+        len = rcd_tmp->record_length;
+        if (len <= buflen)
+                memcpy(record, rcd_tmp, len);
+
+        return len;
+}
+
+/*
+ * If return value > buflen, the buffer size is not big enough,
+ * else if return value < 0, something goes wrong,
+ * else everything is OK, and return value is record length
+ */
+ssize_t erst_read(u64 record_id, struct cper_record_header *record,
+                  size_t buflen)
+{
+        ssize_t len;
+        unsigned long flags;
+
+        if (erst_disable)
+                return -ENODEV;
+
+        spin_lock_irqsave(&erst_lock, flags);
+        len = __erst_read(record_id, record, buflen);
+        spin_unlock_irqrestore(&erst_lock, flags);
+        return len;
+}
+EXPORT_SYMBOL_GPL(erst_read);
+
+/*
+ * If return value > buflen, the buffer size is not big enough,
+ * else if return value = 0, there is no more record to read,
+ * else if return value < 0, something goes wrong,
+ * else everything is OK, and return value is record length
+ */
+ssize_t erst_read_next(struct cper_record_header *record, size_t buflen)
+{
+        int rc;
+        ssize_t len;
+        unsigned long flags;
+        u64 record_id;
+
+        if (erst_disable)
+                return -ENODEV;
+
+        spin_lock_irqsave(&erst_lock, flags);
+        rc = __erst_get_next_record_id(&record_id);
+        if (rc) {
+                spin_unlock_irqrestore(&erst_lock, flags);
+                return rc;
+        }
+        /* no more record */
+        if (record_id == APEI_ERST_INVALID_RECORD_ID) {
+                spin_unlock_irqrestore(&erst_lock, flags);
+                return 0;
+        }
+
+        len = __erst_read(record_id, record, buflen);
+        spin_unlock_irqrestore(&erst_lock, flags);
+
+        return len;
+}
+EXPORT_SYMBOL_GPL(erst_read_next);
+
+int erst_clear(u64 record_id)
+{
+        int rc;
+        unsigned long flags;
+
+        if (erst_disable)
+                return -ENODEV;
+
+        spin_lock_irqsave(&erst_lock, flags);
+        if (erst_erange.attr & ERST_RANGE_NVRAM)
+                rc = __erst_clear_from_nvram(record_id);
+        else
+                rc = __erst_clear_from_storage(record_id);
+        spin_unlock_irqrestore(&erst_lock, flags);
+
+        return rc;
+}
+EXPORT_SYMBOL_GPL(erst_clear);
+
+static int __init setup_erst_disable(char *str)
+{
+        erst_disable = 1;
+        return 0;
+}
+
+__setup("erst_disable", setup_erst_disable);
+
+static int erst_check_table(struct acpi_table_erst *erst_tab)
+{
+        if (erst_tab->header_length != sizeof(struct acpi_table_erst))
+                return -EINVAL;
+        if (erst_tab->header.length < sizeof(struct acpi_table_erst))
+                return -EINVAL;
+        if (erst_tab->entries !=
+            (erst_tab->header.length - sizeof(struct acpi_table_erst)) /
+            sizeof(struct acpi_erst_entry))
+                return -EINVAL;
+
+        return 0;
+}
+
+static int __init erst_init(void)
+{
+        int rc = 0;
+        acpi_status status;
+        struct apei_exec_context ctx;
+        struct apei_resources erst_resources;
+        struct resource *r;
+
+        if (acpi_disabled)
+                goto err;
+
+        if (erst_disable) {
+                pr_info(ERST_PFX
+        "Error Record Serialization Table (ERST) support is disabled.\n");
+                goto err;
+        }
+
+        status = acpi_get_table(ACPI_SIG_ERST, 0,
+                                (struct acpi_table_header **)&erst_tab);
+        if (status == AE_NOT_FOUND) {
+                pr_err(ERST_PFX "Table is not found!\n");
+                goto err;
+        } else if (ACPI_FAILURE(status)) {
+                const char *msg = acpi_format_exception(status);
+                pr_err(ERST_PFX "Failed to get table, %s\n", msg);
+                rc = -EINVAL;
+                goto err;
+        }
+
+        rc = erst_check_table(erst_tab);
+        if (rc) {
+                pr_err(FW_BUG ERST_PFX "ERST table is invalid\n");
+                goto err;
+        }
+
+        apei_resources_init(&erst_resources);
+        erst_exec_ctx_init(&ctx);
+        rc = apei_exec_collect_resources(&ctx, &erst_resources);
+        if (rc)
+                goto err_fini;
+        rc = apei_resources_request(&erst_resources, "APEI ERST");
+        if (rc)
+                goto err_fini;
+        rc = apei_exec_pre_map_gars(&ctx);
+        if (rc)
+                goto err_release;
+        rc = erst_get_erange(&erst_erange);
+        if (rc) {
+                if (rc == -ENODEV)
+                        pr_info(ERST_PFX
+        "The corresponding hardware device or firmware implementation "
+        "is not available.\n");
+                else
+                        pr_err(ERST_PFX
+                               "Failed to get Error Log Address Range.\n");
+                goto err_unmap_reg;
+        }
+
+        r = request_mem_region(erst_erange.base, erst_erange.size, "APEI ERST");
+        if (!r) {
+                pr_err(ERST_PFX
+                "Can not request iomem region <0x%16llx-0x%16llx> for ERST.\n",
+                (unsigned long long)erst_erange.base,
+                (unsigned long long)erst_erange.base + erst_erange.size);
+                rc = -EIO;
+                goto err_unmap_reg;
+        }
+        rc = -ENOMEM;
+        erst_erange.vaddr = ioremap_cache(erst_erange.base,
+                                          erst_erange.size);
+        if (!erst_erange.vaddr)
+                goto err_release_erange;
+
+        pr_info(ERST_PFX
+        "Error Record Serialization Table (ERST) support is initialized.\n");
+
+        return 0;
+
+err_release_erange:
+        release_mem_region(erst_erange.base, erst_erange.size);
+err_unmap_reg:
+        apei_exec_post_unmap_gars(&ctx);
+err_release:
+        apei_resources_release(&erst_resources);
+err_fini:
+        apei_resources_fini(&erst_resources);
+err:
+        erst_disable = 1;
+        return rc;
+}
+
+device_initcall(erst_init);
diff --git a/drivers/acpi/apei/ghes.c b/drivers/acpi/apei/ghes.c
new file mode 100644
index 000000000000..fd0cc016a099
--- /dev/null
+++ b/drivers/acpi/apei/ghes.c
@@ -0,0 +1,427 @@
+/*
+ * APEI Generic Hardware Error Source support
+ *
+ * Generic Hardware Error Source provides a way to report platform
+ * hardware errors (such as that from chipset). It works in so called
+ * "Firmware First" mode, that is, hardware errors are reported to
+ * firmware firstly, then reported to Linux by firmware. This way,
+ * some non-standard hardware error registers or non-standard hardware
+ * link can be checked by firmware to produce more hardware error
+ * information for Linux.
+ *
+ * For more information about Generic Hardware Error Source, please
+ * refer to ACPI Specification version 4.0, section 17.3.2.6
+ *
+ * Now, only SCI notification type and memory errors are
+ * supported. More notification type and hardware error type will be
+ * added later.
+ *
+ * Copyright 2010 Intel Corp.
+ *   Author: 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 version
+ * 2 as published by the Free Software Foundation;
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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/init.h>
+#include <linux/acpi.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/cper.h>
+#include <linux/kdebug.h>
+#include <acpi/apei.h>
+#include <acpi/atomicio.h>
+#include <acpi/hed.h>
+#include <asm/mce.h>
+
+#include "apei-internal.h"
+
+#define GHES_PFX        "GHES: "
+
+#define GHES_ESTATUS_MAX_SIZE           65536
+
+/*
+ * One struct ghes is created for each generic hardware error
+ * source.
+ *
+ * It provides the context for APEI hardware error timer/IRQ/SCI/NMI
+ * handler. Handler for one generic hardware error source is only
+ * triggered after the previous one is done. So handler can uses
+ * struct ghes without locking.
+ *
+ * estatus: memory buffer for error status block, allocated during
+ * HEST parsing.
+ */
+#define GHES_TO_CLEAR           0x0001
+
+struct ghes {
+        struct acpi_hest_generic *generic;
+        struct acpi_hest_generic_status *estatus;
+        struct list_head list;
+        u64 buffer_paddr;
+        unsigned long flags;
+};
+
+/*
+ * Error source lists, one list for each notification method. The
+ * members in lists are struct ghes.
+ *
+ * The list members are only added in HEST parsing and deleted during
+ * module_exit, that is, single-threaded. So no lock is needed for
+ * that.
+ *
+ * But the mutual exclusion is needed between members adding/deleting
+ * and timer/IRQ/SCI/NMI handler, which may traverse the list. RCU is
+ * used for that.
+ */
+static LIST_HEAD(ghes_sci);
+
+static struct ghes *ghes_new(struct acpi_hest_generic *generic)
+{
+        struct ghes *ghes;
+        unsigned int error_block_length;
+        int rc;
+
+        ghes = kzalloc(sizeof(*ghes), GFP_KERNEL);
+        if (!ghes)
+                return ERR_PTR(-ENOMEM);
+        ghes->generic = generic;
+        INIT_LIST_HEAD(&ghes->list);
+        rc = acpi_pre_map_gar(&generic->error_status_address);
+        if (rc)
+                goto err_free;
+        error_block_length = generic->error_block_length;
+        if (error_block_length > GHES_ESTATUS_MAX_SIZE) {
+                pr_warning(FW_WARN GHES_PFX
+                           "Error status block length is too long: %u for "
+                           "generic hardware error source: %d.\n",
+                           error_block_length, generic->header.source_id);
+                error_block_length = GHES_ESTATUS_MAX_SIZE;
+        }
+        ghes->estatus = kmalloc(error_block_length, GFP_KERNEL);
+        if (!ghes->estatus) {
+                rc = -ENOMEM;
+                goto err_unmap;
+        }
+
+        return ghes;
+
+err_unmap:
+        acpi_post_unmap_gar(&generic->error_status_address);
+err_free:
+        kfree(ghes);
+        return ERR_PTR(rc);
+}
+
+static void ghes_fini(struct ghes *ghes)
+{
+        kfree(ghes->estatus);
+        acpi_post_unmap_gar(&ghes->generic->error_status_address);
+}
+
+enum {
+        GHES_SER_NO = 0x0,
+        GHES_SER_CORRECTED = 0x1,
+        GHES_SER_RECOVERABLE = 0x2,
+        GHES_SER_PANIC = 0x3,
+};
+
+static inline int ghes_severity(int severity)
+{
+        switch (severity) {
+        case CPER_SER_INFORMATIONAL:
+                return GHES_SER_NO;
+        case CPER_SER_CORRECTED:
+                return GHES_SER_CORRECTED;
+        case CPER_SER_RECOVERABLE:
+                return GHES_SER_RECOVERABLE;
+        case CPER_SER_FATAL:
+                return GHES_SER_PANIC;
+        default:
+                /* Unkown, go panic */
+                return GHES_SER_PANIC;
+        }
+}
+
+/* SCI handler run in work queue, so ioremap can be used here */
+static int ghes_copy_tofrom_phys(void *buffer, u64 paddr, u32 len,
+                                 int from_phys)
+{
+        void *vaddr;
+
+        vaddr = ioremap_cache(paddr, len);
+        if (!vaddr)
+                return -ENOMEM;
+        if (from_phys)
+                memcpy(buffer, vaddr, len);
+        else
+                memcpy(vaddr, buffer, len);
+        iounmap(vaddr);
+
+        return 0;
+}
+
+static int ghes_read_estatus(struct ghes *ghes, int silent)
+{
+        struct acpi_hest_generic *g = ghes->generic;
+        u64 buf_paddr;
+        u32 len;
+        int rc;
+
+        rc = acpi_atomic_read(&buf_paddr, &g->error_status_address);
+        if (rc) {
+                if (!silent && printk_ratelimit())
+                        pr_warning(FW_WARN GHES_PFX
+"Failed to read error status block address for hardware error source: %d.\n",
+                                   g->header.source_id);
+                return -EIO;
+        }
+        if (!buf_paddr)
+                return -ENOENT;
+
+        rc = ghes_copy_tofrom_phys(ghes->estatus, buf_paddr,
+                                   sizeof(*ghes->estatus), 1);
+        if (rc)
+                return rc;
+        if (!ghes->estatus->block_status)
+                return -ENOENT;
+
+        ghes->buffer_paddr = buf_paddr;
+        ghes->flags |= GHES_TO_CLEAR;
+
+        rc = -EIO;
+        len = apei_estatus_len(ghes->estatus);
+        if (len < sizeof(*ghes->estatus))
+                goto err_read_block;
+        if (len > ghes->generic->error_block_length)
+                goto err_read_block;
+        if (apei_estatus_check_header(ghes->estatus))
+                goto err_read_block;
+        rc = ghes_copy_tofrom_phys(ghes->estatus + 1,
+                                   buf_paddr + sizeof(*ghes->estatus),
+                                   len - sizeof(*ghes->estatus), 1);
+        if (rc)
+                return rc;
+        if (apei_estatus_check(ghes->estatus))
+                goto err_read_block;
+        rc = 0;
+
+err_read_block:
+        if (rc && !silent)
+                pr_warning(FW_WARN GHES_PFX
+                           "Failed to read error status block!\n");
+        return rc;
+}
+
+static void ghes_clear_estatus(struct ghes *ghes)
+{
+        ghes->estatus->block_status = 0;
+        if (!(ghes->flags & GHES_TO_CLEAR))
+                return;
+        ghes_copy_tofrom_phys(ghes->estatus, ghes->buffer_paddr,
+                              sizeof(ghes->estatus->block_status), 0);
+        ghes->flags &= ~GHES_TO_CLEAR;
+}
+
+static void ghes_do_proc(struct ghes *ghes)
+{
+        int ser, processed = 0;
+        struct acpi_hest_generic_data *gdata;
+
+        ser = ghes_severity(ghes->estatus->error_severity);
+        apei_estatus_for_each_section(ghes->estatus, gdata) {
+#ifdef CONFIG_X86_MCE
+                if (!uuid_le_cmp(*(uuid_le *)gdata->section_type,
+                                 CPER_SEC_PLATFORM_MEM)) {
+                        apei_mce_report_mem_error(
+                                ser == GHES_SER_CORRECTED,
+                                (struct cper_sec_mem_err *)(gdata+1));
+                        processed = 1;
+                }
+#endif
+        }
+
+        if (!processed && printk_ratelimit())
+                pr_warning(GHES_PFX
+                "Unknown error record from generic hardware error source: %d\n",
+                           ghes->generic->header.source_id);
+}
+
+static int ghes_proc(struct ghes *ghes)
+{
+        int rc;
+
+        rc = ghes_read_estatus(ghes, 0);
+        if (rc)
+                goto out;
+        ghes_do_proc(ghes);
+
+out:
+        ghes_clear_estatus(ghes);
+        return 0;
+}
+
+static int ghes_notify_sci(struct notifier_block *this,
+                                  unsigned long event, void *data)
+{
+        struct ghes *ghes;
+        int ret = NOTIFY_DONE;
+
+        rcu_read_lock();
+        list_for_each_entry_rcu(ghes, &ghes_sci, list) {
+                if (!ghes_proc(ghes))
+                        ret = NOTIFY_OK;
+        }
+        rcu_read_unlock();
+
+        return ret;
+}
+
+static struct notifier_block ghes_notifier_sci = {
+        .notifier_call = ghes_notify_sci,
+};
+
+static int hest_ghes_parse(struct acpi_hest_header *hest_hdr, void *data)
+{
+        struct acpi_hest_generic *generic;
+        struct ghes *ghes = NULL;
+        int rc = 0;
+
+        if (hest_hdr->type != ACPI_HEST_TYPE_GENERIC_ERROR)
+                return 0;
+
+        generic = (struct acpi_hest_generic *)hest_hdr;
+        if (!generic->enabled)
+                return 0;
+
+        if (generic->error_block_length <
+            sizeof(struct acpi_hest_generic_status)) {
+                pr_warning(FW_BUG GHES_PFX
+"Invalid error block length: %u for generic hardware error source: %d\n",
+                           generic->error_block_length,
+                           generic->header.source_id);
+                goto err;
+        }
+        if (generic->records_to_preallocate == 0) {
+                pr_warning(FW_BUG GHES_PFX
+"Invalid records to preallocate: %u for generic hardware error source: %d\n",
+                           generic->records_to_preallocate,
+                           generic->header.source_id);
+                goto err;
+        }
+        ghes = ghes_new(generic);
+        if (IS_ERR(ghes)) {
+                rc = PTR_ERR(ghes);
+                ghes = NULL;
+                goto err;
+        }
+        switch (generic->notify.type) {
+        case ACPI_HEST_NOTIFY_POLLED:
+                pr_warning(GHES_PFX
+"Generic hardware error source: %d notified via POLL is not supported!\n",
+                           generic->header.source_id);
+                break;
+        case ACPI_HEST_NOTIFY_EXTERNAL:
+        case ACPI_HEST_NOTIFY_LOCAL:
+                pr_warning(GHES_PFX
+"Generic hardware error source: %d notified via IRQ is not supported!\n",
+                           generic->header.source_id);
+                break;
+        case ACPI_HEST_NOTIFY_SCI:
+                if (list_empty(&ghes_sci))
+                        register_acpi_hed_notifier(&ghes_notifier_sci);
+                list_add_rcu(&ghes->list, &ghes_sci);
+                break;
+        case ACPI_HEST_NOTIFY_NMI:
+                pr_warning(GHES_PFX
+"Generic hardware error source: %d notified via NMI is not supported!\n",
+                           generic->header.source_id);
+                break;
+        default:
+                pr_warning(FW_WARN GHES_PFX
+        "Unknown notification type: %u for generic hardware error source: %d\n",
+                           generic->notify.type, generic->header.source_id);
+                break;
+        }
+
+        return 0;
+err:
+        if (ghes)
+                ghes_fini(ghes);
+        return rc;
+}
+
+static void ghes_cleanup(void)
+{
+        struct ghes *ghes, *nghes;
+
+        if (!list_empty(&ghes_sci))
+                unregister_acpi_hed_notifier(&ghes_notifier_sci);
+
+        synchronize_rcu();
+
+        list_for_each_entry_safe(ghes, nghes, &ghes_sci, list) {
+                list_del(&ghes->list);
+                ghes_fini(ghes);
+                kfree(ghes);
+        }
+}
+
+static int __init ghes_init(void)
+{
+        int rc;
+
+        if (acpi_disabled)
+                return -ENODEV;
+
+        if (hest_disable) {
+                pr_info(GHES_PFX "HEST is not enabled!\n");
+                return -EINVAL;
+        }
+
+        rc = apei_hest_parse(hest_ghes_parse, NULL);
+        if (rc) {
+                pr_err(GHES_PFX
+                "Error during parsing HEST generic hardware error sources.\n");
+                goto err_cleanup;
+        }
+
+        if (list_empty(&ghes_sci)) {
+                pr_info(GHES_PFX
+                        "No functional generic hardware error sources.\n");
+                rc = -ENODEV;
+                goto err_cleanup;
+        }
+
+        pr_info(GHES_PFX
+                "Generic Hardware Error Source support is initialized.\n");
+
+        return 0;
+err_cleanup:
+        ghes_cleanup();
+        return rc;
+}
+
+static void __exit ghes_exit(void)
+{
+        ghes_cleanup();
+}
+
+module_init(ghes_init);
+module_exit(ghes_exit);
+
+MODULE_AUTHOR("Huang Ying");
+MODULE_DESCRIPTION("APEI Generic Hardware Error Source support");
+MODULE_LICENSE("GPL");
diff --git a/drivers/acpi/apei/hest.c b/drivers/acpi/apei/hest.c
new file mode 100644
index 000000000000..e7f40d362cb3
--- /dev/null
+++ b/drivers/acpi/apei/hest.c
@@ -0,0 +1,173 @@
+/*
+ * APEI Hardware Error Souce Table support
+ *
+ * HEST describes error sources in detail; communicates operational
+ * parameters (i.e. severity levels, masking bits, and threshold
+ * values) to Linux as necessary. It also allows the BIOS to report
+ * non-standard error sources to Linux (for example, chipset-specific
+ * error registers).
+ *
+ * For more information about HEST, please refer to ACPI Specification
+ * version 4.0, section 17.3.2.
+ *
+ * Copyright 2009 Intel Corp.
+ *   Author: 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 version
+ * 2 as published by the Free Software Foundation;
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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/init.h>
+#include <linux/acpi.h>
+#include <linux/kdebug.h>
+#include <linux/highmem.h>
+#include <linux/io.h>
+#include <acpi/apei.h>
+
+#include "apei-internal.h"
+
+#define HEST_PFX "HEST: "
+
+int hest_disable;
+EXPORT_SYMBOL_GPL(hest_disable);
+
+/* HEST table parsing */
+
+static struct acpi_table_hest *hest_tab;
+
+static int hest_void_parse(struct acpi_hest_header *hest_hdr, void *data)
+{
+        return 0;
+}
+
+static int hest_esrc_len_tab[ACPI_HEST_TYPE_RESERVED] = {
+        [ACPI_HEST_TYPE_IA32_CHECK] = -1,       /* need further calculation */
+        [ACPI_HEST_TYPE_IA32_CORRECTED_CHECK] = -1,
+        [ACPI_HEST_TYPE_IA32_NMI] = sizeof(struct acpi_hest_ia_nmi),
+        [ACPI_HEST_TYPE_AER_ROOT_PORT] = sizeof(struct acpi_hest_aer_root),
+        [ACPI_HEST_TYPE_AER_ENDPOINT] = sizeof(struct acpi_hest_aer),
+        [ACPI_HEST_TYPE_AER_BRIDGE] = sizeof(struct acpi_hest_aer_bridge),
+        [ACPI_HEST_TYPE_GENERIC_ERROR] = sizeof(struct acpi_hest_generic),
+};
+
+static int hest_esrc_len(struct acpi_hest_header *hest_hdr)
+{
+        u16 hest_type = hest_hdr->type;
+        int len;
+
+        if (hest_type >= ACPI_HEST_TYPE_RESERVED)
+                return 0;
+
+        len = hest_esrc_len_tab[hest_type];
+
+        if (hest_type == ACPI_HEST_TYPE_IA32_CORRECTED_CHECK) {
+                struct acpi_hest_ia_corrected *cmc;
+                cmc = (struct acpi_hest_ia_corrected *)hest_hdr;
+                len = sizeof(*cmc) + cmc->num_hardware_banks *
+                        sizeof(struct acpi_hest_ia_error_bank);
+        } else if (hest_type == ACPI_HEST_TYPE_IA32_CHECK) {
+                struct acpi_hest_ia_machine_check *mc;
+                mc = (struct acpi_hest_ia_machine_check *)hest_hdr;
+                len = sizeof(*mc) + mc->num_hardware_banks *
+                        sizeof(struct acpi_hest_ia_error_bank);
+        }
+        BUG_ON(len == -1);
+
+        return len;
+};
+
+int apei_hest_parse(apei_hest_func_t func, void *data)
+{
+        struct acpi_hest_header *hest_hdr;
+        int i, rc, len;
+
+        if (hest_disable)
+                return -EINVAL;
+
+        hest_hdr = (struct acpi_hest_header *)(hest_tab + 1);
+        for (i = 0; i < hest_tab->error_source_count; i++) {
+                len = hest_esrc_len(hest_hdr);
+                if (!len) {
+                        pr_warning(FW_WARN HEST_PFX
+                                   "Unknown or unused hardware error source "
+                                   "type: %d for hardware error source: %d.\n",
+                                   hest_hdr->type, hest_hdr->source_id);
+                        return -EINVAL;
+                }
+                if ((void *)hest_hdr + len >
+                    (void *)hest_tab + hest_tab->header.length) {
+                        pr_warning(FW_BUG HEST_PFX
+                "Table contents overflow for hardware error source: %d.\n",
+                                hest_hdr->source_id);
+                        return -EINVAL;
+                }
+
+                rc = func(hest_hdr, data);
+                if (rc)
+                        return rc;
+
+                hest_hdr = (void *)hest_hdr + len;
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(apei_hest_parse);
+
+static int __init setup_hest_disable(char *str)
+{
+        hest_disable = 1;
+        return 0;
+}
+
+__setup("hest_disable", setup_hest_disable);
+
+static int __init hest_init(void)
+{
+        acpi_status status;
+        int rc = -ENODEV;
+
+        if (acpi_disabled)
+                goto err;
+
+        if (hest_disable) {
+                pr_info(HEST_PFX "HEST tabling parsing is disabled.\n");
+                goto err;
+        }
+
+        status = acpi_get_table(ACPI_SIG_HEST, 0,
+                                (struct acpi_table_header **)&hest_tab);
+        if (status == AE_NOT_FOUND) {
+                pr_info(HEST_PFX "Table is not found!\n");
+                goto err;
+        } else if (ACPI_FAILURE(status)) {
+                const char *msg = acpi_format_exception(status);
+                pr_err(HEST_PFX "Failed to get table, %s\n", msg);
+                rc = -EINVAL;
+                goto err;
+        }
+
+        rc = apei_hest_parse(hest_void_parse, NULL);
+        if (rc)
+                goto err;
+
+        pr_info(HEST_PFX "HEST table parsing is initialized.\n");
+
+        return 0;
+err:
+        hest_disable = 1;
+        return rc;
+}
+
+subsys_initcall(hest_init);
diff --git a/drivers/acpi/atomicio.c b/drivers/acpi/atomicio.c
new file mode 100644
index 000000000000..814b19249616
--- /dev/null
+++ b/drivers/acpi/atomicio.c
@@ -0,0 +1,360 @@
+/*
+ * atomicio.c - ACPI IO memory pre-mapping/post-unmapping, then
+ * accessing in atomic context.
+ *
+ * This is used for NMI handler to access IO memory area, because
+ * ioremap/iounmap can not be used in NMI handler. The IO memory area
+ * is pre-mapped in process context and accessed in NMI handler.
+ *
+ * Copyright (C) 2009-2010, Intel Corp.
+ *      Author: 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 version
+ * 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * 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/init.h>
+#include <linux/acpi.h>
+#include <linux/io.h>
+#include <linux/kref.h>
+#include <linux/rculist.h>
+#include <linux/interrupt.h>
+#include <acpi/atomicio.h>
+
+#define ACPI_PFX "ACPI: "
+
+static LIST_HEAD(acpi_iomaps);
+/*
+ * Used for mutual exclusion between writers of acpi_iomaps list, for
+ * synchronization between readers and writer, RCU is used.
+ */
+static DEFINE_SPINLOCK(acpi_iomaps_lock);
+
+struct acpi_iomap {
+        struct list_head list;
+        void __iomem *vaddr;
+        unsigned long size;
+        phys_addr_t paddr;
+        struct kref ref;
+};
+
+/* acpi_iomaps_lock or RCU read lock must be held before calling */
+static struct acpi_iomap *__acpi_find_iomap(phys_addr_t paddr,
+                                            unsigned long size)
+{
+        struct acpi_iomap *map;
+
+        list_for_each_entry_rcu(map, &acpi_iomaps, list) {
+                if (map->paddr + map->size >= paddr + size &&
+                    map->paddr <= paddr)
+                        return map;
+        }
+        return NULL;
+}
+
+/*
+ * Atomic "ioremap" used by NMI handler, if the specified IO memory
+ * area is not pre-mapped, NULL will be returned.
+ *
+ * acpi_iomaps_lock or RCU read lock must be held before calling
+ */
+static void __iomem *__acpi_ioremap_fast(phys_addr_t paddr,
+                                         unsigned long size)
+{
+        struct acpi_iomap *map;
+
+        map = __acpi_find_iomap(paddr, size);
+        if (map)
+                return map->vaddr + (paddr - map->paddr);
+        else
+                return NULL;
+}
+
+/* acpi_iomaps_lock must be held before calling */
+static void __iomem *__acpi_try_ioremap(phys_addr_t paddr,
+                                        unsigned long size)
+{
+        struct acpi_iomap *map;
+
+        map = __acpi_find_iomap(paddr, size);
+        if (map) {
+                kref_get(&map->ref);
+                return map->vaddr + (paddr - map->paddr);
+        } else
+                return NULL;
+}
+
+/*
+ * Used to pre-map the specified IO memory area. First try to find
+ * whether the area is already pre-mapped, if it is, increase the
+ * reference count (in __acpi_try_ioremap) and return; otherwise, do
+ * the real ioremap, and add the mapping into acpi_iomaps list.
+ */
+static void __iomem *acpi_pre_map(phys_addr_t paddr,
+                                  unsigned long size)
+{
+        void __iomem *vaddr;
+        struct acpi_iomap *map;
+        unsigned long pg_sz, flags;
+        phys_addr_t pg_off;
+
+        spin_lock_irqsave(&acpi_iomaps_lock, flags);
+        vaddr = __acpi_try_ioremap(paddr, size);
+        spin_unlock_irqrestore(&acpi_iomaps_lock, flags);
+        if (vaddr)
+                return vaddr;
+
+        pg_off = paddr & PAGE_MASK;
+        pg_sz = ((paddr + size + PAGE_SIZE - 1) & PAGE_MASK) - pg_off;
+        vaddr = ioremap(pg_off, pg_sz);
+        if (!vaddr)
+                return NULL;
+        map = kmalloc(sizeof(*map), GFP_KERNEL);
+        if (!map)
+                goto err_unmap;
+        INIT_LIST_HEAD(&map->list);
+        map->paddr = pg_off;
+        map->size = pg_sz;
+        map->vaddr = vaddr;
+        kref_init(&map->ref);
+
+        spin_lock_irqsave(&acpi_iomaps_lock, flags);
+        vaddr = __acpi_try_ioremap(paddr, size);
+        if (vaddr) {
+                spin_unlock_irqrestore(&acpi_iomaps_lock, flags);
+                iounmap(map->vaddr);
+                kfree(map);
+                return vaddr;
+        }
+        list_add_tail_rcu(&map->list, &acpi_iomaps);
+        spin_unlock_irqrestore(&acpi_iomaps_lock, flags);
+
+        return vaddr + (paddr - pg_off);
+err_unmap:
+        iounmap(vaddr);
+        return NULL;
+}
+
+/* acpi_iomaps_lock must be held before calling */
+static void __acpi_kref_del_iomap(struct kref *ref)
+{
+        struct acpi_iomap *map;
+
+        map = container_of(ref, struct acpi_iomap, ref);
+        list_del_rcu(&map->list);
+}
+
+/*
+ * Used to post-unmap the specified IO memory area. The iounmap is
+ * done only if the reference count goes zero.
+ */
+static void acpi_post_unmap(phys_addr_t paddr, unsigned long size)
+{
+        struct acpi_iomap *map;
+        unsigned long flags;
+        int del;
+
+        spin_lock_irqsave(&acpi_iomaps_lock, flags);
+        map = __acpi_find_iomap(paddr, size);
+        BUG_ON(!map);
+        del = kref_put(&map->ref, __acpi_kref_del_iomap);
+        spin_unlock_irqrestore(&acpi_iomaps_lock, flags);
+
+        if (!del)
+                return;
+
+        synchronize_rcu();
+        iounmap(map->vaddr);
+        kfree(map);
+}
+
+/* In NMI handler, should set silent = 1 */
+static int acpi_check_gar(struct acpi_generic_address *reg,
+                          u64 *paddr, int silent)
+{
+        u32 width, space_id;
+
+        width = reg->bit_width;
+        space_id = reg->space_id;
+        /* Handle possible alignment issues */
+        memcpy(paddr, &reg->address, sizeof(*paddr));
+        if (!*paddr) {
+                if (!silent)
+                        pr_warning(FW_BUG ACPI_PFX
+                        "Invalid physical address in GAR [0x%llx/%u/%u]\n",
+                                   *paddr, width, space_id);
+                return -EINVAL;
+        }
+
+        if ((width != 8) && (width != 16) && (width != 32) && (width != 64)) {
+                if (!silent)
+                        pr_warning(FW_BUG ACPI_PFX
+                                   "Invalid bit width in GAR [0x%llx/%u/%u]\n",
+                                   *paddr, width, space_id);
+                return -EINVAL;
+        }
+
+        if (space_id != ACPI_ADR_SPACE_SYSTEM_MEMORY &&
+            space_id != ACPI_ADR_SPACE_SYSTEM_IO) {
+                if (!silent)
+                        pr_warning(FW_BUG ACPI_PFX
+                        "Invalid address space type in GAR [0x%llx/%u/%u]\n",
+                                   *paddr, width, space_id);
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+/* Pre-map, working on GAR */
+int acpi_pre_map_gar(struct acpi_generic_address *reg)
+{
+        u64 paddr;
+        void __iomem *vaddr;
+        int rc;
+
+        if (reg->space_id != ACPI_ADR_SPACE_SYSTEM_MEMORY)
+                return 0;
+
+        rc = acpi_check_gar(reg, &paddr, 0);
+        if (rc)
+                return rc;
+
+        vaddr = acpi_pre_map(paddr, reg->bit_width / 8);
+        if (!vaddr)
+                return -EIO;
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(acpi_pre_map_gar);
+
+/* Post-unmap, working on GAR */
+int acpi_post_unmap_gar(struct acpi_generic_address *reg)
+{
+        u64 paddr;
+        int rc;
+
+        if (reg->space_id != ACPI_ADR_SPACE_SYSTEM_MEMORY)
+                return 0;
+
+        rc = acpi_check_gar(reg, &paddr, 0);
+        if (rc)
+                return rc;
+
+        acpi_post_unmap(paddr, reg->bit_width / 8);
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(acpi_post_unmap_gar);
+
+/*
+ * Can be used in atomic (including NMI) or process context. RCU read
+ * lock can only be released after the IO memory area accessing.
+ */
+static int acpi_atomic_read_mem(u64 paddr, u64 *val, u32 width)
+{
+        void __iomem *addr;
+
+        rcu_read_lock();
+        addr = __acpi_ioremap_fast(paddr, width);
+        switch (width) {
+        case 8:
+                *val = readb(addr);
+                break;
+        case 16:
+                *val = readw(addr);
+                break;
+        case 32:
+                *val = readl(addr);
+                break;
+        case 64:
+                *val = readq(addr);
+                break;
+        default:
+                return -EINVAL;
+        }
+        rcu_read_unlock();
+
+        return 0;
+}
+
+static int acpi_atomic_write_mem(u64 paddr, u64 val, u32 width)
+{
+        void __iomem *addr;
+
+        rcu_read_lock();
+        addr = __acpi_ioremap_fast(paddr, width);
+        switch (width) {
+        case 8:
+                writeb(val, addr);
+                break;
+        case 16:
+                writew(val, addr);
+                break;
+        case 32:
+                writel(val, addr);
+                break;
+        case 64:
+                writeq(val, addr);
+                break;
+        default:
+                return -EINVAL;
+        }
+        rcu_read_unlock();
+
+        return 0;
+}
+
+/* GAR accessing in atomic (including NMI) or process context */
+int acpi_atomic_read(u64 *val, struct acpi_generic_address *reg)
+{
+        u64 paddr;
+        int rc;
+
+        rc = acpi_check_gar(reg, &paddr, 1);
+        if (rc)
+                return rc;
+
+        *val = 0;
+        switch (reg->space_id) {
+        case ACPI_ADR_SPACE_SYSTEM_MEMORY:
+                return acpi_atomic_read_mem(paddr, val, reg->bit_width);
+        case ACPI_ADR_SPACE_SYSTEM_IO:
+                return acpi_os_read_port(paddr, (u32 *)val, reg->bit_width);
+        default:
+                return -EINVAL;
+        }
+}
+EXPORT_SYMBOL_GPL(acpi_atomic_read);
+
+int acpi_atomic_write(u64 val, struct acpi_generic_address *reg)
+{
+        u64 paddr;
+        int rc;
+
+        rc = acpi_check_gar(reg, &paddr, 1);
+        if (rc)
+                return rc;
+
+        switch (reg->space_id) {
+        case ACPI_ADR_SPACE_SYSTEM_MEMORY:
+                return acpi_atomic_write_mem(paddr, val, reg->bit_width);
+        case ACPI_ADR_SPACE_SYSTEM_IO:
+                return acpi_os_write_port(paddr, val, reg->bit_width);
+        default:
+                return -EINVAL;
+        }
+}
+EXPORT_SYMBOL_GPL(acpi_atomic_write);
diff --git a/drivers/acpi/ec.c b/drivers/acpi/ec.c
index f2234db85da0..5f2027d782e8 100644
--- a/drivers/acpi/ec.c
+++ b/drivers/acpi/ec.c
@@ -79,7 +79,7 @@ enum {
         EC_FLAGS_GPE_STORM,             /* GPE storm detected */
         EC_FLAGS_HANDLERS_INSTALLED,    /* Handlers for GPE and
                                          * OpReg are installed */
-        EC_FLAGS_FROZEN,                /* Transactions are suspended */
+        EC_FLAGS_BLOCKED,               /* Transactions are blocked */
 };
 
 /* If we find an EC via the ECDT, we need to keep a ptr to its context */
@@ -293,7 +293,7 @@ static int acpi_ec_transaction(struct acpi_ec *ec, struct transaction *t)
         if (t->rdata)
                 memset(t->rdata, 0, t->rlen);
         mutex_lock(&ec->lock);
-        if (test_bit(EC_FLAGS_FROZEN, &ec->flags)) {
+        if (test_bit(EC_FLAGS_BLOCKED, &ec->flags)) {
                 status = -EINVAL;
                 goto unlock;
         }
@@ -459,7 +459,7 @@ int ec_transaction(u8 command,
 
 EXPORT_SYMBOL(ec_transaction);
 
-void acpi_ec_suspend_transactions(void)
+void acpi_ec_block_transactions(void)
 {
         struct acpi_ec *ec = first_ec;
 
@@ -468,11 +468,11 @@ void acpi_ec_suspend_transactions(void)
 
         mutex_lock(&ec->lock);
         /* Prevent transactions from being carried out */
-        set_bit(EC_FLAGS_FROZEN, &ec->flags);
+        set_bit(EC_FLAGS_BLOCKED, &ec->flags);
         mutex_unlock(&ec->lock);
 }
 
-void acpi_ec_resume_transactions(void)
+void acpi_ec_unblock_transactions(void)
 {
         struct acpi_ec *ec = first_ec;
 
@@ -481,10 +481,20 @@ void acpi_ec_resume_transactions(void)
 
         mutex_lock(&ec->lock);
         /* Allow transactions to be carried out again */
-        clear_bit(EC_FLAGS_FROZEN, &ec->flags);
+        clear_bit(EC_FLAGS_BLOCKED, &ec->flags);
         mutex_unlock(&ec->lock);
 }
 
+void acpi_ec_unblock_transactions_early(void)
+{
+        /*
+         * Allow transactions to happen again (this function is called from
+         * atomic context during wakeup, so we don't need to acquire the mutex).
+         */
+        if (first_ec)
+                clear_bit(EC_FLAGS_BLOCKED, &first_ec->flags);
+}
+
 static int acpi_ec_query_unlocked(struct acpi_ec *ec, u8 * data)
 {
         int result;
@@ -1027,10 +1037,9 @@ int __init acpi_ec_ecdt_probe(void)
                 /* Don't trust ECDT, which comes from ASUSTek */
                 if (!EC_FLAGS_VALIDATE_ECDT)
                         goto install;
-                saved_ec = kmalloc(sizeof(struct acpi_ec), GFP_KERNEL);
+                saved_ec = kmemdup(boot_ec, sizeof(struct acpi_ec), GFP_KERNEL);
                 if (!saved_ec)
                         return -ENOMEM;
-                memcpy(saved_ec, boot_ec, sizeof(struct acpi_ec));
         /* fall through */
         }
 
diff --git a/drivers/acpi/hed.c b/drivers/acpi/hed.c
new file mode 100644
index 000000000000..d0c1967f7597
--- /dev/null
+++ b/drivers/acpi/hed.c
@@ -0,0 +1,112 @@
+/*
+ * ACPI Hardware Error Device (PNP0C33) Driver
+ *
+ * Copyright (C) 2010, Intel Corp.
+ *      Author: Huang Ying <ying.huang@intel.com>
+ *
+ * ACPI Hardware Error Device is used to report some hardware errors
+ * notified via SCI, mainly the corrected errors.
+ *
+ * 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;
+ *
+ * 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/init.h>
+#include <linux/acpi.h>
+#include <acpi/acpi_bus.h>
+#include <acpi/acpi_drivers.h>
+#include <acpi/hed.h>
+
+static struct acpi_device_id acpi_hed_ids[] = {
+        {"PNP0C33", 0},
+        {"", 0},
+};
+MODULE_DEVICE_TABLE(acpi, acpi_hed_ids);
+
+static acpi_handle hed_handle;
+
+static BLOCKING_NOTIFIER_HEAD(acpi_hed_notify_list);
+
+int register_acpi_hed_notifier(struct notifier_block *nb)
+{
+        return blocking_notifier_chain_register(&acpi_hed_notify_list, nb);
+}
+EXPORT_SYMBOL_GPL(register_acpi_hed_notifier);
+
+void unregister_acpi_hed_notifier(struct notifier_block *nb)
+{
+        blocking_notifier_chain_unregister(&acpi_hed_notify_list, nb);
+}
+EXPORT_SYMBOL_GPL(unregister_acpi_hed_notifier);
+
+/*
+ * SCI to report hardware error is forwarded to the listeners of HED,
+ * it is used by HEST Generic Hardware Error Source with notify type
+ * SCI.
+ */
+static void acpi_hed_notify(struct acpi_device *device, u32 event)
+{
+        blocking_notifier_call_chain(&acpi_hed_notify_list, 0, NULL);
+}
+
+static int __devinit acpi_hed_add(struct acpi_device *device)
+{
+        /* Only one hardware error device */
+        if (hed_handle)
+                return -EINVAL;
+        hed_handle = device->handle;
+        return 0;
+}
+
+static int __devexit acpi_hed_remove(struct acpi_device *device, int type)
+{
+        hed_handle = NULL;
+        return 0;
+}
+
+static struct acpi_driver acpi_hed_driver = {
+        .name = "hardware_error_device",
+        .class = "hardware_error",
+        .ids = acpi_hed_ids,
+        .ops = {
+                .add = acpi_hed_add,
+                .remove = acpi_hed_remove,
+                .notify = acpi_hed_notify,
+        },
+};
+
+static int __init acpi_hed_init(void)
+{
+        if (acpi_disabled)
+                return -ENODEV;
+
+        if (acpi_bus_register_driver(&acpi_hed_driver) < 0)
+                return -ENODEV;
+
+        return 0;
+}
+
+static void __exit acpi_hed_exit(void)
+{
+        acpi_bus_unregister_driver(&acpi_hed_driver);
+}
+
+module_init(acpi_hed_init);
+module_exit(acpi_hed_exit);
+
+ACPI_MODULE_NAME("hed");
+MODULE_AUTHOR("Huang Ying");
+MODULE_DESCRIPTION("ACPI Hardware Error Device Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/acpi/hest.c b/drivers/acpi/hest.c
deleted file mode 100644
index 1c527a192872..000000000000
--- a/drivers/acpi/hest.c
+++ /dev/null
@@ -1,139 +0,0 @@
-#include <linux/acpi.h>
-#include <linux/pci.h>
-
-#define PREFIX "ACPI: "
-
-static inline unsigned long parse_acpi_hest_ia_machine_check(struct acpi_hest_ia_machine_check *p)
-{
-        return sizeof(*p) +
-                (sizeof(struct acpi_hest_ia_error_bank) * p->num_hardware_banks);
-}
-
-static inline unsigned long parse_acpi_hest_ia_corrected(struct acpi_hest_ia_corrected *p)
-{
-        return sizeof(*p) +
-                (sizeof(struct acpi_hest_ia_error_bank) * p->num_hardware_banks);
-}
-
-static inline unsigned long parse_acpi_hest_ia_nmi(struct acpi_hest_ia_nmi *p)
-{
-        return sizeof(*p);
-}
-
-static inline unsigned long parse_acpi_hest_generic(struct acpi_hest_generic *p)
-{
-        return sizeof(*p);
-}
-
-static inline unsigned int hest_match_pci(struct acpi_hest_aer_common *p, struct pci_dev *pci)
-{
-        return  (0           == pci_domain_nr(pci->bus) &&
-                 p->bus      == pci->bus->number &&
-                 p->device   == PCI_SLOT(pci->devfn) &&
-                 p->function == PCI_FUNC(pci->devfn));
-}
-
-static unsigned long parse_acpi_hest_aer(void *hdr, int type, struct pci_dev *pci, int *firmware_first)
-{
-        struct acpi_hest_aer_common *p = hdr + sizeof(struct acpi_hest_header);
-        unsigned long rc=0;
-        u8 pcie_type = 0;
-        u8 bridge = 0;
-        switch (type) {
-        case ACPI_HEST_TYPE_AER_ROOT_PORT:
-                rc = sizeof(struct acpi_hest_aer_root);
-                pcie_type = PCI_EXP_TYPE_ROOT_PORT;
-                break;
-        case ACPI_HEST_TYPE_AER_ENDPOINT:
-                rc = sizeof(struct acpi_hest_aer);
-                pcie_type = PCI_EXP_TYPE_ENDPOINT;
-                break;
-        case ACPI_HEST_TYPE_AER_BRIDGE:
-                rc = sizeof(struct acpi_hest_aer_bridge);
-                if ((pci->class >> 16) == PCI_BASE_CLASS_BRIDGE)
-                        bridge = 1;
-                break;
-        }
-
-        if (p->flags & ACPI_HEST_GLOBAL) {
-                if ((pci->is_pcie && (pci->pcie_type == pcie_type)) || bridge)
-                        *firmware_first = !!(p->flags & ACPI_HEST_FIRMWARE_FIRST);
-        }
-        else
-                if (hest_match_pci(p, pci))
-                        *firmware_first = !!(p->flags & ACPI_HEST_FIRMWARE_FIRST);
-        return rc;
-}
-
-static int acpi_hest_firmware_first(struct acpi_table_header *stdheader, struct pci_dev *pci)
-{
-        struct acpi_table_hest *hest = (struct acpi_table_hest *)stdheader;
-        void *p = (void *)hest + sizeof(*hest); /* defined by the ACPI 4.0 spec */
-        struct acpi_hest_header *hdr = p;
-
-        int i;
-        int firmware_first = 0;
-        static unsigned char printed_unused = 0;
-        static unsigned char printed_reserved = 0;
-
-        for (i=0, hdr=p; p < (((void *)hest) + hest->header.length) && i < hest->error_source_count; i++) {
-                switch (hdr->type) {
-                case ACPI_HEST_TYPE_IA32_CHECK:
-                        p += parse_acpi_hest_ia_machine_check(p);
-                        break;
-                case ACPI_HEST_TYPE_IA32_CORRECTED_CHECK:
-                        p += parse_acpi_hest_ia_corrected(p);
-                        break;
-                case ACPI_HEST_TYPE_IA32_NMI:
-                        p += parse_acpi_hest_ia_nmi(p);
-                        break;
-                /* These three should never appear */
-                case ACPI_HEST_TYPE_NOT_USED3:
-                case ACPI_HEST_TYPE_NOT_USED4:
-                case ACPI_HEST_TYPE_NOT_USED5:
-                        if (!printed_unused) {
-                                printk(KERN_DEBUG PREFIX
-                                       "HEST Error Source list contains an obsolete type (%d).\n", hdr->type);
-                                printed_unused = 1;
-                        }
-                        break;
-                case ACPI_HEST_TYPE_AER_ROOT_PORT:
-                case ACPI_HEST_TYPE_AER_ENDPOINT:
-                case ACPI_HEST_TYPE_AER_BRIDGE:
-                        p += parse_acpi_hest_aer(p, hdr->type, pci, &firmware_first);
-                        break;
-                case ACPI_HEST_TYPE_GENERIC_ERROR:
-                        p += parse_acpi_hest_generic(p);
-                        break;
-                /* These should never appear either */
-                case ACPI_HEST_TYPE_RESERVED:
-                default:
-                        if (!printed_reserved) {
-                                printk(KERN_DEBUG PREFIX
-                                       "HEST Error Source list contains a reserved type (%d).\n", hdr->type);
-                                printed_reserved = 1;
-                        }
-                        break;
-                }
-        }
-        return firmware_first;
-}
-
-int acpi_hest_firmware_first_pci(struct pci_dev *pci)
-{
-        acpi_status status = AE_NOT_FOUND;
-        struct acpi_table_header *hest = NULL;
-
-        if (acpi_disabled)
-                return 0;
-
-        status = acpi_get_table(ACPI_SIG_HEST, 1, &hest);
-
-        if (ACPI_SUCCESS(status)) {
-                if (acpi_hest_firmware_first(hest, pci)) {
-                        return 1;
-                }
-        }
-        return 0;
-}
-EXPORT_SYMBOL_GPL(acpi_hest_firmware_first_pci);
diff --git a/drivers/acpi/internal.h b/drivers/acpi/internal.h
index e28411367239..f8f190ec066e 100644
--- a/drivers/acpi/internal.h
+++ b/drivers/acpi/internal.h
@@ -49,8 +49,9 @@ void acpi_early_processor_set_pdc(void);
 int acpi_ec_init(void);
 int acpi_ec_ecdt_probe(void);
 int acpi_boot_ec_enable(void);
-void acpi_ec_suspend_transactions(void);
-void acpi_ec_resume_transactions(void);
+void acpi_ec_block_transactions(void);
+void acpi_ec_unblock_transactions(void);
+void acpi_ec_unblock_transactions_early(void);
 
 /*--------------------------------------------------------------------------
                                   Suspend/Resume
diff --git a/drivers/acpi/osl.c b/drivers/acpi/osl.c
index 4bc1c4178f50..78418ce4fc78 100644
--- a/drivers/acpi/osl.c
+++ b/drivers/acpi/osl.c
@@ -1207,6 +1207,15 @@ int acpi_check_mem_region(resource_size_t start, resource_size_t n,
 EXPORT_SYMBOL(acpi_check_mem_region);
 
 /*
+ * Let drivers know whether the resource checks are effective
+ */
+int acpi_resources_are_enforced(void)
+{
+        return acpi_enforce_resources == ENFORCE_RESOURCES_STRICT;
+}
+EXPORT_SYMBOL(acpi_resources_are_enforced);
+
+/*
  * Acquire a spinlock.
  *
  * handle is a pointer to the spinlock_t.
diff --git a/drivers/acpi/pci_root.c b/drivers/acpi/pci_root.c
index aefce33f2a09..4eac59393edc 100644
--- a/drivers/acpi/pci_root.c
+++ b/drivers/acpi/pci_root.c
@@ -120,7 +120,8 @@ acpi_handle acpi_get_pci_rootbridge_handle(unsigned int seg, unsigned int bus)
         struct acpi_pci_root *root;
         
         list_for_each_entry(root, &acpi_pci_roots, node)
-                if ((root->segment == (u16) seg) && (root->bus_nr == (u16) bus))
+                if ((root->segment == (u16) seg) &&
+                    (root->secondary.start == (u16) bus))
                         return root->device->handle;
         return NULL;            
 }
@@ -154,7 +155,7 @@ EXPORT_SYMBOL_GPL(acpi_is_root_bridge);
 static acpi_status
 get_root_bridge_busnr_callback(struct acpi_resource *resource, void *data)
 {
-        int *busnr = data;
+        struct resource *res = data;
         struct acpi_resource_address64 address;
 
         if (resource->type != ACPI_RESOURCE_TYPE_ADDRESS16 &&
@@ -164,28 +165,27 @@ get_root_bridge_busnr_callback(struct acpi_resource *resource, void *data)
 
         acpi_resource_to_address64(resource, &address);
         if ((address.address_length > 0) &&
-            (address.resource_type == ACPI_BUS_NUMBER_RANGE))
-                *busnr = address.minimum;
+            (address.resource_type == ACPI_BUS_NUMBER_RANGE)) {
+                res->start = address.minimum;
+                res->end = address.minimum + address.address_length - 1;
+        }
 
         return AE_OK;
 }
 
 static acpi_status try_get_root_bridge_busnr(acpi_handle handle,
-                                             unsigned long long *bus)
+                                             struct resource *res)
 {
         acpi_status status;
-        int busnum;
 
-        busnum = -1;
+        res->start = -1;
         status =
             acpi_walk_resources(handle, METHOD_NAME__CRS,
-                                get_root_bridge_busnr_callback, &busnum);
+                                get_root_bridge_busnr_callback, res);
         if (ACPI_FAILURE(status))
                 return status;
-        /* Check if we really get a bus number from _CRS */
-        if (busnum == -1)
+        if (res->start == -1)
                 return AE_ERROR;
-        *bus = busnum;
         return AE_OK;
 }
 
@@ -429,34 +429,47 @@ static int __devinit acpi_pci_root_add(struct acpi_device *device)
         struct acpi_device *child;
         u32 flags, base_flags;
 
+        root = kzalloc(sizeof(struct acpi_pci_root), GFP_KERNEL);
+        if (!root)
+                return -ENOMEM;
+
         segment = 0;
         status = acpi_evaluate_integer(device->handle, METHOD_NAME__SEG, NULL,
                                        &segment);
         if (ACPI_FAILURE(status) && status != AE_NOT_FOUND) {
                 printk(KERN_ERR PREFIX "can't evaluate _SEG\n");
-                return -ENODEV;
+                result = -ENODEV;
+                goto end;
         }
 
         /* Check _CRS first, then _BBN.  If no _BBN, default to zero. */
-        bus = 0;
-        status = try_get_root_bridge_busnr(device->handle, &bus);
+        root->secondary.flags = IORESOURCE_BUS;
+        status = try_get_root_bridge_busnr(device->handle, &root->secondary);
         if (ACPI_FAILURE(status)) {
+                /*
+                 * We need both the start and end of the downstream bus range
+                 * to interpret _CBA (MMCONFIG base address), so it really is
+                 * supposed to be in _CRS.  If we don't find it there, all we
+                 * can do is assume [_BBN-0xFF] or [0-0xFF].
+                 */
+                root->secondary.end = 0xFF;
+                printk(KERN_WARNING FW_BUG PREFIX
+                       "no secondary bus range in _CRS\n");
                 status = acpi_evaluate_integer(device->handle, METHOD_NAME__BBN,                                               NULL, &bus);
-                if (ACPI_FAILURE(status) && status != AE_NOT_FOUND) {
-                        printk(KERN_ERR PREFIX
-                             "no bus number in _CRS and can't evaluate _BBN\n");
-                        return -ENODEV;
+                if (ACPI_SUCCESS(status))
+                        root->secondary.start = bus;
+                else if (status == AE_NOT_FOUND)
+                        root->secondary.start = 0;
+                else {
+                        printk(KERN_ERR PREFIX "can't evaluate _BBN\n");
+                        result = -ENODEV;
+                        goto end;
                 }
         }
 
-        root = kzalloc(sizeof(struct acpi_pci_root), GFP_KERNEL);
-        if (!root)
-                return -ENOMEM;
-
         INIT_LIST_HEAD(&root->node);
         root->device = device;
         root->segment = segment & 0xFFFF;
-        root->bus_nr = bus & 0xFF;
         strcpy(acpi_device_name(device), ACPI_PCI_ROOT_DEVICE_NAME);
         strcpy(acpi_device_class(device), ACPI_PCI_ROOT_CLASS);
         device->driver_data = root;
@@ -475,9 +488,9 @@ static int __devinit acpi_pci_root_add(struct acpi_device *device)
         /* TBD: Locking */
         list_add_tail(&root->node, &acpi_pci_roots);
 
-        printk(KERN_INFO PREFIX "%s [%s] (%04x:%02x)\n",
+        printk(KERN_INFO PREFIX "%s [%s] (domain %04x %pR)\n",
                acpi_device_name(device), acpi_device_bid(device),
-               root->segment, root->bus_nr);
+               root->segment, &root->secondary);
 
         /*
          * Scan the Root Bridge
@@ -486,11 +499,11 @@ static int __devinit acpi_pci_root_add(struct acpi_device *device)
          * PCI namespace does not get created until this call is made (and 
          * thus the root bridge's pci_dev does not exist).
          */
-        root->bus = pci_acpi_scan_root(device, segment, bus);
+        root->bus = pci_acpi_scan_root(root);
         if (!root->bus) {
                 printk(KERN_ERR PREFIX
                             "Bus %04x:%02x not present in PCI namespace\n",
-                            root->segment, root->bus_nr);
+                            root->segment, (unsigned int)root->secondary.start);
                 result = -ENODEV;
                 goto end;
         }
diff --git a/drivers/acpi/processor_driver.c b/drivers/acpi/processor_driver.c
index 5675d9747e87..b1034a9ada4e 100644
--- a/drivers/acpi/processor_driver.c
+++ b/drivers/acpi/processor_driver.c
@@ -616,7 +616,8 @@ static int __cpuinit acpi_processor_add(struct acpi_device *device)
         acpi_processor_get_limit_info(pr);
 
 
-        acpi_processor_power_init(pr, device);
+        if (cpuidle_get_driver() == &acpi_idle_driver)
+                acpi_processor_power_init(pr, device);
 
         pr->cdev = thermal_cooling_device_register("Processor", device,
                                                 &processor_cooling_ops);
@@ -920,9 +921,14 @@ static int __init acpi_processor_init(void)
         if (!acpi_processor_dir)
                 return -ENOMEM;
 #endif
-        result = cpuidle_register_driver(&acpi_idle_driver);
-        if (result < 0)
-                goto out_proc;
+
+        if (!cpuidle_register_driver(&acpi_idle_driver)) {
+                printk(KERN_DEBUG "ACPI: %s registered with cpuidle\n",
+                        acpi_idle_driver.name);
+        } else {
+                printk(KERN_DEBUG "ACPI: acpi_idle yielding to %s",
+                        cpuidle_get_driver()->name);
+        }
 
         result = acpi_bus_register_driver(&acpi_processor_driver);
         if (result < 0)
@@ -941,7 +947,6 @@ static int __init acpi_processor_init(void)
 out_cpuidle:
         cpuidle_unregister_driver(&acpi_idle_driver);
 
-out_proc:
 #ifdef CONFIG_ACPI_PROCFS
         remove_proc_entry(ACPI_PROCESSOR_CLASS, acpi_root_dir);
 #endif
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index c3817e1f32c7..b1b385692f46 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -80,7 +80,7 @@ module_param(nocst, uint, 0000);
 static unsigned int latency_factor __read_mostly = 2;
 module_param(latency_factor, uint, 0644);
 
-static s64 us_to_pm_timer_ticks(s64 t)
+static u64 us_to_pm_timer_ticks(s64 t)
 {
         return div64_u64(t * PM_TIMER_FREQUENCY, 1000000);
 }
@@ -727,24 +727,14 @@ static int acpi_processor_power_seq_show(struct seq_file *seq, void *offset)
                         break;
                 }
 
-                if (pr->power.states[i].promotion.state)
-                        seq_printf(seq, "promotion[C%zd] ",
-                                   (pr->power.states[i].promotion.state -
-                                    pr->power.states));
-                else
-                        seq_puts(seq, "promotion[--] ");
-
-                if (pr->power.states[i].demotion.state)
-                        seq_printf(seq, "demotion[C%zd] ",
-                                   (pr->power.states[i].demotion.state -
-                                    pr->power.states));
-                else
-                        seq_puts(seq, "demotion[--] ");
-
-                seq_printf(seq, "latency[%03d] usage[%08d] duration[%020llu]\n",
+                seq_puts(seq, "promotion[--] ");
+
+                seq_puts(seq, "demotion[--] ");
+
+                seq_printf(seq, "latency[%03d] usage[%08d] duration[%020Lu]\n",
                            pr->power.states[i].latency,
                            pr->power.states[i].usage,
-                           (unsigned long long)pr->power.states[i].time);
+                           us_to_pm_timer_ticks(pr->power.states[i].time));
         }
 
       end:
@@ -869,8 +859,8 @@ static int acpi_idle_enter_simple(struct cpuidle_device *dev,
         struct acpi_processor *pr;
         struct acpi_processor_cx *cx = cpuidle_get_statedata(state);
         ktime_t  kt1, kt2;
+        s64 idle_time_ns;
         s64 idle_time;
-        s64 sleep_ticks = 0;
 
         pr = __get_cpu_var(processors);
 
@@ -881,6 +871,7 @@ static int acpi_idle_enter_simple(struct cpuidle_device *dev,
                 return(acpi_idle_enter_c1(dev, state));
 
         local_irq_disable();
+
         if (cx->entry_method != ACPI_CSTATE_FFH) {
                 current_thread_info()->status &= ~TS_POLLING;
                 /*
@@ -888,12 +879,12 @@ static int acpi_idle_enter_simple(struct cpuidle_device *dev,
                  * NEED_RESCHED:
                  */
                 smp_mb();
-        }
 
-        if (unlikely(need_resched())) {
-                current_thread_info()->status |= TS_POLLING;
-                local_irq_enable();
-                return 0;
+                if (unlikely(need_resched())) {
+                        current_thread_info()->status |= TS_POLLING;
+                        local_irq_enable();
+                        return 0;
+                }
         }
 
         /*
@@ -910,20 +901,21 @@ static int acpi_idle_enter_simple(struct cpuidle_device *dev,
         sched_clock_idle_sleep_event();
         acpi_idle_do_entry(cx);
         kt2 = ktime_get_real();
-        idle_time =  ktime_to_us(ktime_sub(kt2, kt1));
-
-        sleep_ticks = us_to_pm_timer_ticks(idle_time);
+        idle_time_ns = ktime_to_ns(ktime_sub(kt2, kt1));
+        idle_time = idle_time_ns;
+        do_div(idle_time, NSEC_PER_USEC);
 
         /* Tell the scheduler how much we idled: */
-        sched_clock_idle_wakeup_event(sleep_ticks*PM_TIMER_TICK_NS);
+        sched_clock_idle_wakeup_event(idle_time_ns);
 
         local_irq_enable();
-        current_thread_info()->status |= TS_POLLING;
+        if (cx->entry_method != ACPI_CSTATE_FFH)
+                current_thread_info()->status |= TS_POLLING;
 
         cx->usage++;
 
         lapic_timer_state_broadcast(pr, cx, 0);
-        cx->time += sleep_ticks;
+        cx->time += idle_time;
         return idle_time;
 }
 
@@ -943,8 +935,8 @@ static int acpi_idle_enter_bm(struct cpuidle_device *dev,
         struct acpi_processor *pr;
         struct acpi_processor_cx *cx = cpuidle_get_statedata(state);
         ktime_t  kt1, kt2;
+        s64 idle_time_ns;
         s64 idle_time;
-        s64 sleep_ticks = 0;
 
 
         pr = __get_cpu_var(processors);
@@ -968,6 +960,7 @@ static int acpi_idle_enter_bm(struct cpuidle_device *dev,
         }
 
         local_irq_disable();
+
         if (cx->entry_method != ACPI_CSTATE_FFH) {
                 current_thread_info()->status &= ~TS_POLLING;
                 /*
@@ -975,12 +968,12 @@ static int acpi_idle_enter_bm(struct cpuidle_device *dev,
                  * NEED_RESCHED:
                  */
                 smp_mb();
-        }
 
-        if (unlikely(need_resched())) {
-                current_thread_info()->status |= TS_POLLING;
-                local_irq_enable();
-                return 0;
+                if (unlikely(need_resched())) {
+                        current_thread_info()->status |= TS_POLLING;
+                        local_irq_enable();
+                        return 0;
+                }
         }
 
         acpi_unlazy_tlb(smp_processor_id());
@@ -1025,19 +1018,21 @@ static int acpi_idle_enter_bm(struct cpuidle_device *dev,
                 spin_unlock(&c3_lock);
         }
         kt2 = ktime_get_real();
-        idle_time =  ktime_to_us(ktime_sub(kt2, kt1));
+        idle_time_ns = ktime_to_ns(ktime_sub(kt2, kt1));
+        idle_time = idle_time_ns;
+        do_div(idle_time, NSEC_PER_USEC);
 
-        sleep_ticks = us_to_pm_timer_ticks(idle_time);
         /* Tell the scheduler how much we idled: */
-        sched_clock_idle_wakeup_event(sleep_ticks*PM_TIMER_TICK_NS);
+        sched_clock_idle_wakeup_event(idle_time_ns);
 
         local_irq_enable();
-        current_thread_info()->status |= TS_POLLING;
+        if (cx->entry_method != ACPI_CSTATE_FFH)
+                current_thread_info()->status |= TS_POLLING;
 
         cx->usage++;
 
         lapic_timer_state_broadcast(pr, cx, 0);
-        cx->time += sleep_ticks;
+        cx->time += idle_time;
         return idle_time;
 }
 
diff --git a/drivers/acpi/sleep.c b/drivers/acpi/sleep.c
index baa76bbf244a..3fb4bdea7e06 100644
--- a/drivers/acpi/sleep.c
+++ b/drivers/acpi/sleep.c
@@ -80,22 +80,6 @@ static int acpi_sleep_prepare(u32 acpi_state)
 
 #ifdef CONFIG_ACPI_SLEEP
 static u32 acpi_target_sleep_state = ACPI_STATE_S0;
-/*
- * According to the ACPI specification the BIOS should make sure that ACPI is
- * enabled and SCI_EN bit is set on wake-up from S1 - S3 sleep states.  Still,
- * some BIOSes don't do that and therefore we use acpi_enable() to enable ACPI
- * on such systems during resume.  Unfortunately that doesn't help in
- * particularly pathological cases in which SCI_EN has to be set directly on
- * resume, although the specification states very clearly that this flag is
- * owned by the hardware.  The set_sci_en_on_resume variable will be set in such
- * cases.
- */
-static bool set_sci_en_on_resume;
-
-void __init acpi_set_sci_en_on_resume(void)
-{
-        set_sci_en_on_resume = true;
-}
 
 /*
  * ACPI 1.0 wants us to execute _PTS before suspending devices, so we allow the
@@ -110,11 +94,13 @@ void __init acpi_old_suspend_ordering(void)
 }
 
 /**
- *      acpi_pm_disable_gpes - Disable the GPEs.
+ * acpi_pm_freeze - Disable the GPEs and suspend EC transactions.
  */
-static int acpi_pm_disable_gpes(void)
+static int acpi_pm_freeze(void)
 {
         acpi_disable_all_gpes();
+        acpi_os_wait_events_complete(NULL);
+        acpi_ec_block_transactions();
         return 0;
 }
 
@@ -142,7 +128,8 @@ static int acpi_pm_prepare(void)
         int error = __acpi_pm_prepare();
 
         if (!error)
-                acpi_disable_all_gpes();
+                acpi_pm_freeze();
+
         return error;
 }
 
@@ -253,11 +240,8 @@ static int acpi_suspend_enter(suspend_state_t pm_state)
                 break;
         }
 
-        /* If ACPI is not enabled by the BIOS, we need to enable it here. */
-        if (set_sci_en_on_resume)
-                acpi_write_bit_register(ACPI_BITREG_SCI_ENABLE, 1);
-        else
-                acpi_enable();
+        /* This violates the spec but is required for bug compatibility. */
+        acpi_write_bit_register(ACPI_BITREG_SCI_ENABLE, 1);
 
         /* Reprogram control registers and execute _BFS */
         acpi_leave_sleep_state_prep(acpi_state);
@@ -275,6 +259,8 @@ static int acpi_suspend_enter(suspend_state_t pm_state)
          * acpi_leave_sleep_state will reenable specific GPEs later
          */
         acpi_disable_all_gpes();
+        /* Allow EC transactions to happen. */
+        acpi_ec_unblock_transactions_early();
 
         local_irq_restore(flags);
         printk(KERN_DEBUG "Back to C!\n");
@@ -286,6 +272,12 @@ static int acpi_suspend_enter(suspend_state_t pm_state)
         return ACPI_SUCCESS(status) ? 0 : -EFAULT;
 }
 
+static void acpi_suspend_finish(void)
+{
+        acpi_ec_unblock_transactions();
+        acpi_pm_finish();
+}
+
 static int acpi_suspend_state_valid(suspend_state_t pm_state)
 {
         u32 acpi_state;
@@ -307,7 +299,7 @@ static struct platform_suspend_ops acpi_suspend_ops = {
         .begin = acpi_suspend_begin,
         .prepare_late = acpi_pm_prepare,
         .enter = acpi_suspend_enter,
-        .wake = acpi_pm_finish,
+        .wake = acpi_suspend_finish,
         .end = acpi_pm_end,
 };
 
@@ -333,9 +325,9 @@ static int acpi_suspend_begin_old(suspend_state_t pm_state)
 static struct platform_suspend_ops acpi_suspend_ops_old = {
         .valid = acpi_suspend_state_valid,
         .begin = acpi_suspend_begin_old,
-        .prepare_late = acpi_pm_disable_gpes,
+        .prepare_late = acpi_pm_freeze,
         .enter = acpi_suspend_enter,
-        .wake = acpi_pm_finish,
+        .wake = acpi_suspend_finish,
         .end = acpi_pm_end,
         .recover = acpi_pm_finish,
 };
@@ -346,12 +338,6 @@ static int __init init_old_suspend_ordering(const struct dmi_system_id *d)
         return 0;
 }
 
-static int __init init_set_sci_en_on_resume(const struct dmi_system_id *d)
-{
-        set_sci_en_on_resume = true;
-        return 0;
-}
-
 static struct dmi_system_id __initdata acpisleep_dmi_table[] = {
         {
         .callback = init_old_suspend_ordering,
@@ -370,22 +356,6 @@ static struct dmi_system_id __initdata acpisleep_dmi_table[] = {
                 },
         },
         {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Apple MacBook 1,1",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "Apple Computer, Inc."),
-                DMI_MATCH(DMI_PRODUCT_NAME, "MacBook1,1"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Apple MacMini 1,1",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "Apple Computer, Inc."),
-                DMI_MATCH(DMI_PRODUCT_NAME, "Macmini1,1"),
-                },
-        },
-        {
         .callback = init_old_suspend_ordering,
         .ident = "Asus Pundit P1-AH2 (M2N8L motherboard)",
         .matches = {
@@ -394,94 +364,6 @@ static struct dmi_system_id __initdata acpisleep_dmi_table[] = {
                 },
         },
         {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Toshiba Satellite L300",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "TOSHIBA"),
-                DMI_MATCH(DMI_PRODUCT_NAME, "Satellite L300"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Hewlett-Packard HP G7000 Notebook PC",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-                DMI_MATCH(DMI_PRODUCT_NAME, "HP G7000 Notebook PC"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Hewlett-Packard HP Pavilion dv3 Notebook PC",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-                DMI_MATCH(DMI_PRODUCT_NAME, "HP Pavilion dv3 Notebook PC"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Hewlett-Packard Pavilion dv4",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-                DMI_MATCH(DMI_PRODUCT_NAME, "HP Pavilion dv4"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Hewlett-Packard Pavilion dv7",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-                DMI_MATCH(DMI_PRODUCT_NAME, "HP Pavilion dv7"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Hewlett-Packard Compaq Presario C700 Notebook PC",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-                DMI_MATCH(DMI_PRODUCT_NAME, "Compaq Presario C700 Notebook PC"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Hewlett-Packard Compaq Presario CQ40 Notebook PC",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-                DMI_MATCH(DMI_PRODUCT_NAME, "Compaq Presario CQ40 Notebook PC"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Lenovo ThinkPad T410",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-                DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T410"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Lenovo ThinkPad T510",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-                DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T510"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Lenovo ThinkPad W510",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-                DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad W510"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Lenovo ThinkPad X201[s]",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-                DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad X201"),
-                },
-        },
-        {
         .callback = init_old_suspend_ordering,
         .ident = "Panasonic CF51-2L",
         .matches = {
@@ -490,30 +372,6 @@ static struct dmi_system_id __initdata acpisleep_dmi_table[] = {
                 DMI_MATCH(DMI_BOARD_NAME, "CF51-2L"),
                 },
         },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Dell Studio 1558",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
-                DMI_MATCH(DMI_PRODUCT_NAME, "Studio 1558"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Dell Studio 1557",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
-                DMI_MATCH(DMI_PRODUCT_NAME, "Studio 1557"),
-                },
-        },
-        {
-        .callback = init_set_sci_en_on_resume,
-        .ident = "Dell Studio 1555",
-        .matches = {
-                DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
-                DMI_MATCH(DMI_PRODUCT_NAME, "Studio 1555"),
-                },
-        },
         {},
 };
 #endif /* CONFIG_SUSPEND */
@@ -586,6 +444,7 @@ static int acpi_hibernation_enter(void)
 static void acpi_hibernation_finish(void)
 {
         hibernate_nvs_free();
+        acpi_ec_unblock_transactions();
         acpi_pm_finish();
 }
 
@@ -606,19 +465,13 @@ static void acpi_hibernation_leave(void)
         }
         /* Restore the NVS memory area */
         hibernate_nvs_restore();
+        /* Allow EC transactions to happen. */
+        acpi_ec_unblock_transactions_early();
 }
 
-static int acpi_pm_pre_restore(void)
+static void acpi_pm_thaw(void)
 {
-        acpi_disable_all_gpes();
-        acpi_os_wait_events_complete(NULL);
-        acpi_ec_suspend_transactions();
-        return 0;
-}
-
-static void acpi_pm_restore_cleanup(void)
-{
-        acpi_ec_resume_transactions();
+        acpi_ec_unblock_transactions();
         acpi_enable_all_runtime_gpes();
 }
 
@@ -630,8 +483,8 @@ static struct platform_hibernation_ops acpi_hibernation_ops = {
         .prepare = acpi_pm_prepare,
         .enter = acpi_hibernation_enter,
         .leave = acpi_hibernation_leave,
-        .pre_restore = acpi_pm_pre_restore,
-        .restore_cleanup = acpi_pm_restore_cleanup,
+        .pre_restore = acpi_pm_freeze,
+        .restore_cleanup = acpi_pm_thaw,
 };
 
 /**
@@ -663,12 +516,9 @@ static int acpi_hibernation_begin_old(void)
 
 static int acpi_hibernation_pre_snapshot_old(void)
 {
-        int error = acpi_pm_disable_gpes();
-
-        if (!error)
-                hibernate_nvs_save();
-
-        return error;
+        acpi_pm_freeze();
+        hibernate_nvs_save();
+        return 0;
 }
 
 /*
@@ -680,11 +530,11 @@ static struct platform_hibernation_ops acpi_hibernation_ops_old = {
         .end = acpi_pm_end,
         .pre_snapshot = acpi_hibernation_pre_snapshot_old,
         .finish = acpi_hibernation_finish,
-        .prepare = acpi_pm_disable_gpes,
+        .prepare = acpi_pm_freeze,
         .enter = acpi_hibernation_enter,
         .leave = acpi_hibernation_leave,
-        .pre_restore = acpi_pm_pre_restore,
-        .restore_cleanup = acpi_pm_restore_cleanup,
+        .pre_restore = acpi_pm_freeze,
+        .restore_cleanup = acpi_pm_thaw,
         .recover = acpi_pm_finish,
 };
 #endif /* CONFIG_HIBERNATION */
diff --git a/drivers/acpi/sleep.h b/drivers/acpi/sleep.h
index 8a8f3b3382a6..25b8bd149284 100644
--- a/drivers/acpi/sleep.h
+++ b/drivers/acpi/sleep.h
@@ -1,6 +1,6 @@
 
 extern u8 sleep_states[];
-extern int acpi_suspend (u32 state);
+extern int acpi_suspend(u32 state);
 
 extern void acpi_enable_wakeup_device_prep(u8 sleep_state);
 extern void acpi_enable_wakeup_device(u8 sleep_state);
diff --git a/drivers/acpi/tables.c b/drivers/acpi/tables.c
index 8a0ed2800e63..f336bca7c450 100644
--- a/drivers/acpi/tables.c
+++ b/drivers/acpi/tables.c
@@ -213,7 +213,7 @@ acpi_table_parse_entries(char *id,
         unsigned long table_end;
         acpi_size tbl_size;
 
-        if (acpi_disabled && !acpi_ht)
+        if (acpi_disabled)
                 return -ENODEV;
 
         if (!handler)
@@ -280,7 +280,7 @@ int __init acpi_table_parse(char *id, acpi_table_handler handler)
         struct acpi_table_header *table = NULL;
         acpi_size tbl_size;
 
-        if (acpi_disabled && !acpi_ht)
+        if (acpi_disabled)
                 return -ENODEV;
 
         if (!handler)
diff --git a/drivers/acpi/video.c b/drivers/acpi/video.c
index a0c93b321482..9865d46f49a8 100644
--- a/drivers/acpi/video.c
+++ b/drivers/acpi/video.c
@@ -45,6 +45,7 @@
 #include <acpi/acpi_bus.h>
 #include <acpi/acpi_drivers.h>
 #include <linux/suspend.h>
+#include <acpi/video.h>
 
 #define PREFIX "ACPI: "
 
@@ -65,11 +66,6 @@
 
 #define MAX_NAME_LEN    20
 
-#define ACPI_VIDEO_DISPLAY_CRT  1
-#define ACPI_VIDEO_DISPLAY_TV   2
-#define ACPI_VIDEO_DISPLAY_DVI  3
-#define ACPI_VIDEO_DISPLAY_LCD  4
-
 #define _COMPONENT              ACPI_VIDEO_COMPONENT
 ACPI_MODULE_NAME("video");
 
@@ -1007,11 +1003,11 @@ static void acpi_video_device_find_cap(struct acpi_video_device *device)
                 result = acpi_video_init_brightness(device);
                 if (result)
                         return;
-                name = kzalloc(MAX_NAME_LEN, GFP_KERNEL);
+                name = kasprintf(GFP_KERNEL, "acpi_video%d", count);
                 if (!name)
                         return;
+                count++;
 
-                sprintf(name, "acpi_video%d", count++);
                 memset(&props, 0, sizeof(struct backlight_properties));
                 props.max_brightness = device->brightness->count - 3;
                 device->backlight = backlight_device_register(name, NULL, device,
@@ -1067,10 +1063,10 @@ static void acpi_video_device_find_cap(struct acpi_video_device *device)
                 if (device->cap._DCS && device->cap._DSS) {
                         static int count;
                         char *name;
-                        name = kzalloc(MAX_NAME_LEN, GFP_KERNEL);
+                        name = kasprintf(GFP_KERNEL, "acpi_video%d", count);
                         if (!name)
                                 return;
-                        sprintf(name, "acpi_video%d", count++);
+                        count++;
                         device->output_dev = video_output_register(name,
                                         NULL, device, &acpi_output_properties);
                         kfree(name);
@@ -1748,11 +1744,27 @@ acpi_video_get_device_attr(struct acpi_video_bus *video, unsigned long device_id
 }
 
 static int
+acpi_video_get_device_type(struct acpi_video_bus *video,
+                           unsigned long device_id)
+{
+        struct acpi_video_enumerated_device *ids;
+        int i;
+
+        for (i = 0; i < video->attached_count; i++) {
+                ids = &video->attached_array[i];
+                if ((ids->value.int_val & 0xffff) == device_id)
+                        return ids->value.int_val;
+        }
+
+        return 0;
+}
+
+static int
 acpi_video_bus_get_one_device(struct acpi_device *device,
                               struct acpi_video_bus *video)
 {
         unsigned long long device_id;
-        int status;
+        int status, device_type;
         struct acpi_video_device *data;
         struct acpi_video_device_attrib* attribute;
 
@@ -1797,8 +1809,25 @@ acpi_video_bus_get_one_device(struct acpi_device *device,
                         }
                         if(attribute->bios_can_detect)
                                 data->flags.bios = 1;
-                } else
-                        data->flags.unknown = 1;
+                } else {
+                        /* Check for legacy IDs */
+                        device_type = acpi_video_get_device_type(video,
+                                                                 device_id);
+                        /* Ignore bits 16 and 18-20 */
+                        switch (device_type & 0xffe2ffff) {
+                        case ACPI_VIDEO_DISPLAY_LEGACY_MONITOR:
+                                data->flags.crt = 1;
+                                break;
+                        case ACPI_VIDEO_DISPLAY_LEGACY_PANEL:
+                                data->flags.lcd = 1;
+                                break;
+                        case ACPI_VIDEO_DISPLAY_LEGACY_TV:
+                                data->flags.tvout = 1;
+                                break;
+                        default:
+                                data->flags.unknown = 1;
+                        }
+                }
 
                 acpi_video_device_bind(video, data);
                 acpi_video_device_find_cap(data);
@@ -2032,6 +2061,71 @@ out:
         return result;
 }
 
+int acpi_video_get_edid(struct acpi_device *device, int type, int device_id,
+                        void **edid)
+{
+        struct acpi_video_bus *video;
+        struct acpi_video_device *video_device;
+        union acpi_object *buffer = NULL;
+        acpi_status status;
+        int i, length;
+
+        if (!device || !acpi_driver_data(device))
+                return -EINVAL;
+
+        video = acpi_driver_data(device);
+
+        for (i = 0; i < video->attached_count; i++) {
+                video_device = video->attached_array[i].bind_info;
+                length = 256;
+
+                if (!video_device)
+                        continue;
+
+                if (type) {
+                        switch (type) {
+                        case ACPI_VIDEO_DISPLAY_CRT:
+                                if (!video_device->flags.crt)
+                                        continue;
+                                break;
+                        case ACPI_VIDEO_DISPLAY_TV:
+                                if (!video_device->flags.tvout)
+                                        continue;
+                                break;
+                        case ACPI_VIDEO_DISPLAY_DVI:
+                                if (!video_device->flags.dvi)
+                                        continue;
+                                break;
+                        case ACPI_VIDEO_DISPLAY_LCD:
+                                if (!video_device->flags.lcd)
+                                        continue;
+                                break;
+                        }
+                } else if (video_device->device_id != device_id) {
+                        continue;
+                }
+
+                status = acpi_video_device_EDID(video_device, &buffer, length);
+
+                if (ACPI_FAILURE(status) || !buffer ||
+                    buffer->type != ACPI_TYPE_BUFFER) {
+                        length = 128;
+                        status = acpi_video_device_EDID(video_device, &buffer,
+                                                        length);
+                        if (ACPI_FAILURE(status) || !buffer ||
+                            buffer->type != ACPI_TYPE_BUFFER) {
+                                continue;
+                        }
+                }
+
+                *edid = buffer->buffer.pointer;
+                return length;
+        }
+
+        return -ENODEV;
+}
+EXPORT_SYMBOL(acpi_video_get_edid);
+
 static int
 acpi_video_bus_get_devices(struct acpi_video_bus *video,
                            struct acpi_device *device)
diff --git a/drivers/acpi/video_detect.c b/drivers/acpi/video_detect.c
index fc2f26b9b407..c5fef01b3c95 100644
--- a/drivers/acpi/video_detect.c
+++ b/drivers/acpi/video_detect.c
@@ -250,7 +250,7 @@ static int __init acpi_backlight(char *str)
                                 ACPI_VIDEO_BACKLIGHT_FORCE_VENDOR;
                 if (!strcmp("video", str))
                         acpi_video_support |=
-                                ACPI_VIDEO_OUTPUT_SWITCHING_FORCE_VIDEO;
+                                ACPI_VIDEO_BACKLIGHT_FORCE_VIDEO;
         }
         return 1;
 }
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index e68541f662b9..aa85a98d3a4f 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -57,6 +57,8 @@ config SATA_PMP
           This option adds support for SATA Port Multipliers
           (the SATA version of an ethernet hub, or SAS expander).
 
+comment "Controllers with non-SFF native interface"
+
 config SATA_AHCI
         tristate "AHCI SATA support"
         depends on PCI
@@ -73,11 +75,12 @@ config SATA_AHCI_PLATFORM
 
           If unsure, say N.
 
-config SATA_SIL24
-        tristate "Silicon Image 3124/3132 SATA support"
-        depends on PCI
+config SATA_FSL
+        tristate "Freescale 3.0Gbps SATA support"
+        depends on FSL_SOC
         help
-          This option enables support for Silicon Image 3124/3132 Serial ATA.
+          This option enables support for Freescale 3.0Gbps SATA controller.
+          It can be found on MPC837x and MPC8315.
 
           If unsure, say N.
 
@@ -87,12 +90,11 @@ config SATA_INIC162X
         help
           This option enables support for Initio 162x Serial ATA.
 
-config SATA_FSL
-        tristate "Freescale 3.0Gbps SATA support"
-        depends on FSL_SOC
+config SATA_SIL24
+        tristate "Silicon Image 3124/3132 SATA support"
+        depends on PCI
         help
-          This option enables support for Freescale 3.0Gbps SATA controller.
-          It can be found on MPC837x and MPC8315.
+          This option enables support for Silicon Image 3124/3132 Serial ATA.
 
           If unsure, say N.
 
@@ -116,15 +118,65 @@ config ATA_SFF
 
 if ATA_SFF
 
-config SATA_SVW
-        tristate "ServerWorks Frodo / Apple K2 SATA support"
+comment "SFF controllers with custom DMA interface"
+
+config PDC_ADMA
+        tristate "Pacific Digital ADMA support"
         depends on PCI
         help
-          This option enables support for Broadcom/Serverworks/Apple K2
-          SATA support.
+          This option enables support for Pacific Digital ADMA controllers
+
+          If unsure, say N.
+
+config PATA_MPC52xx
+        tristate "Freescale MPC52xx SoC internal IDE"
+        depends on PPC_MPC52xx && PPC_BESTCOMM
+        select PPC_BESTCOMM_ATA
+        help
+          This option enables support for integrated IDE controller
+          of the Freescale MPC52xx SoC.
+
+          If unsure, say N.
+
+config PATA_OCTEON_CF
+        tristate "OCTEON Boot Bus Compact Flash support"
+        depends on CPU_CAVIUM_OCTEON
+        help
+          This option enables a polled compact flash driver for use with
+          compact flash cards attached to the OCTEON boot bus.
+
+          If unsure, say N.
+
+config SATA_QSTOR
+        tristate "Pacific Digital SATA QStor support"
+        depends on PCI
+        help
+          This option enables support for Pacific Digital Serial ATA QStor.
+
+          If unsure, say N.
+
+config SATA_SX4
+        tristate "Promise SATA SX4 support (Experimental)"
+        depends on PCI && EXPERIMENTAL
+        help
+          This option enables support for Promise Serial ATA SX4.
 
           If unsure, say N.
 
+config ATA_BMDMA
+        bool "ATA BMDMA support"
+        default y
+        help
+          This option adds support for SFF ATA controllers with BMDMA
+          capability.  BMDMA stands for bus-master DMA and is the
+          de facto DMA interface for SFF controllers.
+
+          If unsure, say Y.
+
+if ATA_BMDMA
+
+comment "SATA SFF controllers with BMDMA"
+
 config ATA_PIIX
         tristate "Intel ESB, ICH, PIIX3, PIIX4 PATA/SATA support"
         depends on PCI
@@ -152,22 +204,6 @@ config SATA_NV
 
           If unsure, say N.
 
-config PDC_ADMA
-        tristate "Pacific Digital ADMA support"
-        depends on PCI
-        help
-          This option enables support for Pacific Digital ADMA controllers
-
-          If unsure, say N.
-
-config SATA_QSTOR
-        tristate "Pacific Digital SATA QStor support"
-        depends on PCI
-        help
-          This option enables support for Pacific Digital Serial ATA QStor.
-
-          If unsure, say N.
-
 config SATA_PROMISE
         tristate "Promise SATA TX2/TX4 support"
         depends on PCI
@@ -176,14 +212,6 @@ config SATA_PROMISE
 
           If unsure, say N.
 
-config SATA_SX4
-        tristate "Promise SATA SX4 support (Experimental)"
-        depends on PCI && EXPERIMENTAL
-        help
-          This option enables support for Promise Serial ATA SX4.
-
-          If unsure, say N.
-
 config SATA_SIL
         tristate "Silicon Image SATA support"
         depends on PCI
@@ -203,6 +231,15 @@ config SATA_SIS
           enable the PATA_SIS driver in the config.
           If unsure, say N.
 
+config SATA_SVW
+        tristate "ServerWorks Frodo / Apple K2 SATA support"
+        depends on PCI
+        help
+          This option enables support for Broadcom/Serverworks/Apple K2
+          SATA support.
+
+          If unsure, say N.
+
 config SATA_ULI
         tristate "ULi Electronics SATA support"
         depends on PCI
@@ -227,14 +264,7 @@ config SATA_VITESSE
 
           If unsure, say N.
 
-config PATA_ACPI
-        tristate "ACPI firmware driver for PATA"
-        depends on ATA_ACPI
-        help
-          This option enables an ACPI method driver which drives
-          motherboard PATA controller interfaces through the ACPI
-          firmware in the BIOS. This driver can sometimes handle
-          otherwise unsupported hardware.
+comment "PATA SFF controllers with BMDMA"
 
 config PATA_ALI
         tristate "ALi PATA support"
@@ -262,40 +292,30 @@ config PATA_ARTOP
 
           If unsure, say N.
 
-config PATA_ATP867X
-        tristate "ARTOP/Acard ATP867X PATA support"
+config PATA_ATIIXP
+        tristate "ATI PATA support"
         depends on PCI
         help
-          This option enables support for ARTOP/Acard ATP867X PATA
-          controllers.
-
-          If unsure, say N.
-
-config PATA_AT32
-        tristate "Atmel AVR32 PATA support (Experimental)"
-        depends on AVR32 && PLATFORM_AT32AP && EXPERIMENTAL
-        help
-          This option enables support for the IDE devices on the
-          Atmel AT32AP platform.
+          This option enables support for the ATI ATA interfaces
+          found on the many ATI chipsets.
 
           If unsure, say N.
 
-config PATA_ATIIXP
-        tristate "ATI PATA support"
+config PATA_ATP867X
+        tristate "ARTOP/Acard ATP867X PATA support"
         depends on PCI
         help
-          This option enables support for the ATI ATA interfaces
-          found on the many ATI chipsets.
+          This option enables support for ARTOP/Acard ATP867X PATA
+          controllers.
 
           If unsure, say N.
 
-config PATA_CMD640_PCI
-        tristate "CMD640 PCI PATA support (Experimental)"
-        depends on PCI && EXPERIMENTAL
+config PATA_BF54X
+        tristate "Blackfin 54x ATAPI support"
+        depends on BF542 || BF548 || BF549
         help
-          This option enables support for the CMD640 PCI IDE
-          interface chip. Only the primary channel is currently
-          supported.
+          This option enables support for the built-in ATAPI controller on
+          Blackfin 54x family chips.
 
           If unsure, say N.
 
@@ -362,15 +382,6 @@ config PATA_EFAR
 
           If unsure, say N.
 
-config ATA_GENERIC
-        tristate "Generic ATA support"
-        depends on PCI
-        help
-          This option enables support for generic BIOS configured
-          ATA controllers via the new ATA layer
-
-          If unsure, say N.
-
 config PATA_HPT366
         tristate "HPT 366/368 PATA support"
         depends on PCI
@@ -415,12 +426,20 @@ config PATA_HPT3X3_DMA
           controllers. Enable with care as there are still some
           problems with DMA on this chipset.
 
-config PATA_ISAPNP
-        tristate "ISA Plug and Play PATA support"
-        depends on ISAPNP
+config PATA_ICSIDE
+        tristate "Acorn ICS PATA support"
+        depends on ARM && ARCH_ACORN
         help
-          This option enables support for ISA plug & play ATA
-          controllers such as those found on old soundcards.
+          On Acorn systems, say Y here if you wish to use the ICS PATA
+          interface card.  This is not required for ICS partition support.
+          If you are unsure, say N to this.
+
+config PATA_IT8213
+        tristate "IT8213 PATA support (Experimental)"
+        depends on PCI && EXPERIMENTAL
+        help
+          This option enables support for the ITE 821 PATA
+          controllers via the new ATA layer.
 
           If unsure, say N.
 
@@ -434,15 +453,6 @@ config PATA_IT821X
 
           If unsure, say N.
 
-config PATA_IT8213
-        tristate "IT8213 PATA support (Experimental)"
-        depends on PCI && EXPERIMENTAL
-        help
-          This option enables support for the ITE 821 PATA
-          controllers via the new ATA layer.
-
-          If unsure, say N.
-
 config PATA_JMICRON
         tristate "JMicron PATA support"
         depends on PCI
@@ -452,23 +462,14 @@ config PATA_JMICRON
 
           If unsure, say N.
 
-config PATA_LEGACY
-        tristate "Legacy ISA PATA support (Experimental)"
-        depends on (ISA || PCI)  && EXPERIMENTAL
-        help
-          This option enables support for ISA/VLB/PCI bus legacy PATA
-          ports and allows them to be accessed via the new ATA layer.
-
-          If unsure, say N.
-
-config PATA_TRIFLEX
-        tristate "Compaq Triflex PATA support"
-        depends on PCI
+config PATA_MACIO
+        tristate "Apple PowerMac/PowerBook internal 'MacIO' IDE"
+        depends on PPC_PMAC
         help
-          Enable support for the Compaq 'Triflex' IDE controller as found
-          on many Compaq Pentium-Pro systems, via the new ATA layer.
-
-          If unsure, say N.
+          Most IDE capable PowerMacs have IDE busses driven by a variant
+          of this controller which is part of the Apple chipset used on
+          most PowerMac models. Some models have multiple busses using
+          different chipsets, though generally, MacIO is one of them.
 
 config PATA_MARVELL
         tristate "Marvell PATA support via legacy mode"
@@ -481,32 +482,6 @@ config PATA_MARVELL
 
           If unsure, say N.
 
-config PATA_MPC52xx
-        tristate "Freescale MPC52xx SoC internal IDE"
-        depends on PPC_MPC52xx && PPC_BESTCOMM
-        select PPC_BESTCOMM_ATA
-        help
-          This option enables support for integrated IDE controller
-          of the Freescale MPC52xx SoC.
-
-          If unsure, say N.
-
-config PATA_MPIIX
-        tristate "Intel PATA MPIIX support"
-        depends on PCI
-        help
-          This option enables support for MPIIX PATA support.
-
-          If unsure, say N.
-
-config PATA_OLDPIIX
-        tristate "Intel PATA old PIIX support"
-        depends on PCI
-        help
-          This option enables support for early PIIX PATA support.
-
-          If unsure, say N.
-
 config PATA_NETCELL
         tristate "NETCELL Revolution RAID support"
         depends on PCI
@@ -525,15 +500,6 @@ config PATA_NINJA32
 
           If unsure, say N.
 
-config PATA_NS87410
-        tristate "Nat Semi NS87410 PATA support"
-        depends on PCI
-        help
-          This option enables support for the National Semiconductor
-          NS87410 PCI-IDE controller.
-
-          If unsure, say N.
-
 config PATA_NS87415
         tristate "Nat Semi NS87415 PATA support"
         depends on PCI
@@ -543,12 +509,11 @@ config PATA_NS87415
 
           If unsure, say N.
 
-config PATA_OPTI
-        tristate "OPTI621/6215 PATA support (Very Experimental)"
-        depends on PCI && EXPERIMENTAL
+config PATA_OLDPIIX
+        tristate "Intel PATA old PIIX support"
+        depends on PCI
         help
-          This option enables full PIO support for the early Opti ATA
-          controllers found on some old motherboards.
+          This option enables support for early PIIX PATA support.
 
           If unsure, say N.
 
@@ -562,24 +527,6 @@ config PATA_OPTIDMA
 
           If unsure, say N.
 
-config PATA_PALMLD
-        tristate "Palm LifeDrive PATA support"
-        depends on MACH_PALMLD
-        help
-          This option enables support for Palm LifeDrive's internal ATA
-          port via the new ATA layer.
-
-          If unsure, say N.
-
-config PATA_PCMCIA
-        tristate "PCMCIA PATA support"
-        depends on PCMCIA
-        help
-          This option enables support for PCMCIA ATA interfaces, including
-          compact flash card adapters via the new ATA layer.
-
-          If unsure, say N.
-
 config PATA_PDC2027X
         tristate "Promise PATA 2027x support"
         depends on PCI
@@ -597,12 +544,6 @@ config PATA_PDC_OLD
 
           If unsure, say N.
 
-config PATA_QDI
-        tristate "QDI VLB PATA support"
-        depends on ISA
-        help
-          Support for QDI 6500 and 6580 PATA controllers on VESA local bus.
-
 config PATA_RADISYS
         tristate "RADISYS 82600 PATA support (Experimental)"
         depends on PCI && EXPERIMENTAL
@@ -612,15 +553,6 @@ config PATA_RADISYS
 
           If unsure, say N.
 
-config PATA_RB532
-        tristate "RouterBoard 532 PATA CompactFlash support"
-        depends on MIKROTIK_RB532
-        help
-          This option enables support for the RouterBoard 532
-          PATA CompactFlash controller.
-
-          If unsure, say N.
-
 config PATA_RDC
         tristate "RDC PATA support"
         depends on PCI
@@ -631,21 +563,30 @@ config PATA_RDC
 
           If unsure, say N.
 
-config PATA_RZ1000
-        tristate "PC Tech RZ1000 PATA support"
+config PATA_SC1200
+        tristate "SC1200 PATA support"
         depends on PCI
         help
-          This option enables basic support for the PC Tech RZ1000/1
-          PATA controllers via the new ATA layer
+          This option enables support for the NatSemi/AMD SC1200 SoC
+          companion chip used with the Geode processor family.
 
           If unsure, say N.
 
-config PATA_SC1200
-        tristate "SC1200 PATA support"
+config PATA_SCC
+        tristate "Toshiba's Cell Reference Set IDE support"
+        depends on PCI && PPC_CELLEB
+        help
+          This option enables support for the built-in IDE controller on
+          Toshiba Cell Reference Board.
+
+          If unsure, say N.
+
+config PATA_SCH
+        tristate "Intel SCH PATA support"
         depends on PCI
         help
-          This option enables support for the NatSemi/AMD SC1200 SoC
-          companion chip used with the Geode processor family.
+          This option enables support for Intel SCH PATA on the Intel
+          SCH (US15W, US15L, UL11L) series host controllers.
 
           If unsure, say N.
 
@@ -683,6 +624,15 @@ config PATA_TOSHIBA
 
           If unsure, say N.
 
+config PATA_TRIFLEX
+        tristate "Compaq Triflex PATA support"
+        depends on PCI
+        help
+          Enable support for the Compaq 'Triflex' IDE controller as found
+          on many Compaq Pentium-Pro systems, via the new ATA layer.
+
+          If unsure, say N.
+
 config PATA_VIA
         tristate "VIA PATA support"
         depends on PCI
@@ -701,12 +651,99 @@ config PATA_WINBOND
 
           If unsure, say N.
 
-config PATA_WINBOND_VLB
-        tristate "Winbond W83759A VLB PATA support (Experimental)"
-        depends on ISA && EXPERIMENTAL
+endif # ATA_BMDMA
+
+comment "PIO-only SFF controllers"
+
+config PATA_AT32
+        tristate "Atmel AVR32 PATA support (Experimental)"
+        depends on AVR32 && PLATFORM_AT32AP && EXPERIMENTAL
         help
-          Support for the Winbond W83759A controller on Vesa Local Bus
-          systems.
+          This option enables support for the IDE devices on the
+          Atmel AT32AP 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_CMD640_PCI
+        tristate "CMD640 PCI PATA support (Experimental)"
+        depends on PCI && EXPERIMENTAL
+        help
+          This option enables support for the CMD640 PCI IDE
+          interface chip. Only the primary channel is currently
+          supported.
+
+          If unsure, say N.
+
+config PATA_ISAPNP
+        tristate "ISA Plug and Play PATA support"
+        depends on ISAPNP
+        help
+          This option enables support for ISA plug & play ATA
+          controllers such as those found on old soundcards.
+
+          If unsure, say N.
+
+config PATA_IXP4XX_CF
+        tristate "IXP4XX Compact Flash support"
+        depends on ARCH_IXP4XX
+        help
+          This option enables support for a Compact Flash connected on
+          the ixp4xx expansion bus. This driver had been written for
+          Loft/Avila boards in mind but can work with others.
+
+          If unsure, say N.
+
+config PATA_MPIIX
+        tristate "Intel PATA MPIIX support"
+        depends on PCI
+        help
+          This option enables support for MPIIX PATA support.
+
+          If unsure, say N.
+
+config PATA_NS87410
+        tristate "Nat Semi NS87410 PATA support"
+        depends on PCI
+        help
+          This option enables support for the National Semiconductor
+          NS87410 PCI-IDE controller.
+
+          If unsure, say N.
+
+config PATA_OPTI
+        tristate "OPTI621/6215 PATA support (Very Experimental)"
+        depends on PCI && EXPERIMENTAL
+        help
+          This option enables full PIO support for the early Opti ATA
+          controllers found on some old motherboards.
+
+          If unsure, say N.
+
+config PATA_PALMLD
+        tristate "Palm LifeDrive PATA support"
+        depends on MACH_PALMLD
+        help
+          This option enables support for Palm LifeDrive's internal ATA
+          port via the new ATA layer.
+
+          If unsure, say N.
+
+config PATA_PCMCIA
+        tristate "PCMCIA PATA support"
+        depends on PCMCIA
+        help
+          This option enables support for PCMCIA ATA interfaces, including
+          compact flash card adapters via the new ATA layer.
+
+          If unsure, say N.
 
 config HAVE_PATA_PLATFORM
         bool
@@ -725,14 +762,6 @@ 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
@@ -743,69 +772,65 @@ config PATA_OF_PLATFORM
 
           If unsure, say N.
 
-config PATA_ICSIDE
-        tristate "Acorn ICS PATA support"
-        depends on ARM && ARCH_ACORN
+config PATA_QDI
+        tristate "QDI VLB PATA support"
+        depends on ISA
         help
-          On Acorn systems, say Y here if you wish to use the ICS PATA
-          interface card.  This is not required for ICS partition support.
-          If you are unsure, say N to this.
+          Support for QDI 6500 and 6580 PATA controllers on VESA local bus.
 
-config PATA_IXP4XX_CF
-        tristate "IXP4XX Compact Flash support"
-        depends on ARCH_IXP4XX
+config PATA_RB532
+        tristate "RouterBoard 532 PATA CompactFlash support"
+        depends on MIKROTIK_RB532
         help
-          This option enables support for a Compact Flash connected on
-          the ixp4xx expansion bus. This driver had been written for
-          Loft/Avila boards in mind but can work with others.
+          This option enables support for the RouterBoard 532
+          PATA CompactFlash controller.
 
           If unsure, say N.
 
-config PATA_OCTEON_CF
-        tristate "OCTEON Boot Bus Compact Flash support"
-        depends on CPU_CAVIUM_OCTEON
+config PATA_RZ1000
+        tristate "PC Tech RZ1000 PATA support"
+        depends on PCI
         help
-          This option enables a polled compact flash driver for use with
-          compact flash cards attached to the OCTEON boot bus.
+          This option enables basic support for the PC Tech RZ1000/1
+          PATA controllers via the new ATA layer
 
           If unsure, say N.
 
-config PATA_SCC
-        tristate "Toshiba's Cell Reference Set IDE support"
-        depends on PCI && PPC_CELLEB
+config PATA_WINBOND_VLB
+        tristate "Winbond W83759A VLB PATA support (Experimental)"
+        depends on ISA && EXPERIMENTAL
         help
-          This option enables support for the built-in IDE controller on
-          Toshiba Cell Reference Board.
+          Support for the Winbond W83759A controller on Vesa Local Bus
+          systems.
 
-          If unsure, say N.
+comment "Generic fallback / legacy drivers"
 
-config PATA_SCH
-        tristate "Intel SCH PATA support"
-        depends on PCI
+config PATA_ACPI
+        tristate "ACPI firmware driver for PATA"
+        depends on ATA_ACPI && ATA_BMDMA
         help
-          This option enables support for Intel SCH PATA on the Intel
-          SCH (US15W, US15L, UL11L) series host controllers.
-
-          If unsure, say N.
+          This option enables an ACPI method driver which drives
+          motherboard PATA controller interfaces through the ACPI
+          firmware in the BIOS. This driver can sometimes handle
+          otherwise unsupported hardware.
 
-config PATA_BF54X
-        tristate "Blackfin 54x ATAPI support"
-        depends on BF542 || BF548 || BF549
+config ATA_GENERIC
+        tristate "Generic ATA support"
+        depends on PCI && ATA_BMDMA
         help
-          This option enables support for the built-in ATAPI controller on
-          Blackfin 54x family chips.
+          This option enables support for generic BIOS configured
+          ATA controllers via the new ATA layer
 
           If unsure, say N.
 
-config PATA_MACIO
-        tristate "Apple PowerMac/PowerBook internal 'MacIO' IDE"
-        depends on PPC_PMAC
+config PATA_LEGACY
+        tristate "Legacy ISA PATA support (Experimental)"
+        depends on (ISA || PCI) && EXPERIMENTAL
         help
-          Most IDE capable PowerMacs have IDE busses driven by a variant
-          of this controller which is part of the Apple chipset used on
-          most PowerMac models. Some models have multiple busses using
-          different chipsets, though generally, MacIO is one of them.
+          This option enables support for ISA/VLB/PCI bus legacy PATA
+          ports and allows them to be accessed via the new ATA layer.
 
+          If unsure, say N.
 
 endif # ATA_SFF
 endif # ATA
diff --git a/drivers/ata/Makefile b/drivers/ata/Makefile
index d0a93c4ad3ec..7ef89d73df63 100644
--- a/drivers/ata/Makefile
+++ b/drivers/ata/Makefile
@@ -1,33 +1,39 @@
 
 obj-$(CONFIG_ATA)               += libata.o
 
+# non-SFF interface
 obj-$(CONFIG_SATA_AHCI)         += ahci.o libahci.o
 obj-$(CONFIG_SATA_AHCI_PLATFORM) += ahci_platform.o libahci.o
-obj-$(CONFIG_SATA_SVW)          += sata_svw.o
+obj-$(CONFIG_SATA_FSL)          += sata_fsl.o
+obj-$(CONFIG_SATA_INIC162X)     += sata_inic162x.o
+obj-$(CONFIG_SATA_SIL24)        += sata_sil24.o
+
+# SFF w/ custom DMA
+obj-$(CONFIG_PDC_ADMA)          += pdc_adma.o
+obj-$(CONFIG_PATA_MPC52xx)      += pata_mpc52xx.o
+obj-$(CONFIG_PATA_OCTEON_CF)    += pata_octeon_cf.o
+obj-$(CONFIG_SATA_QSTOR)        += sata_qstor.o
+obj-$(CONFIG_SATA_SX4)          += sata_sx4.o
+
+# SFF SATA w/ BMDMA
 obj-$(CONFIG_ATA_PIIX)          += ata_piix.o
+obj-$(CONFIG_SATA_MV)           += sata_mv.o
+obj-$(CONFIG_SATA_NV)           += sata_nv.o
 obj-$(CONFIG_SATA_PROMISE)      += sata_promise.o
-obj-$(CONFIG_SATA_QSTOR)        += sata_qstor.o
 obj-$(CONFIG_SATA_SIL)          += sata_sil.o
-obj-$(CONFIG_SATA_SIL24)        += sata_sil24.o
-obj-$(CONFIG_SATA_VIA)          += sata_via.o
-obj-$(CONFIG_SATA_VITESSE)      += sata_vsc.o
 obj-$(CONFIG_SATA_SIS)          += sata_sis.o
-obj-$(CONFIG_SATA_SX4)          += sata_sx4.o
-obj-$(CONFIG_SATA_NV)           += sata_nv.o
+obj-$(CONFIG_SATA_SVW)          += sata_svw.o
 obj-$(CONFIG_SATA_ULI)          += sata_uli.o
-obj-$(CONFIG_SATA_MV)           += sata_mv.o
-obj-$(CONFIG_SATA_INIC162X)     += sata_inic162x.o
-obj-$(CONFIG_PDC_ADMA)          += pdc_adma.o
-obj-$(CONFIG_SATA_FSL)          += sata_fsl.o
-obj-$(CONFIG_PATA_MACIO)        += pata_macio.o
+obj-$(CONFIG_SATA_VIA)          += sata_via.o
+obj-$(CONFIG_SATA_VITESSE)      += sata_vsc.o
 
+# SFF PATA w/ BMDMA
 obj-$(CONFIG_PATA_ALI)          += pata_ali.o
 obj-$(CONFIG_PATA_AMD)          += pata_amd.o
 obj-$(CONFIG_PATA_ARTOP)        += pata_artop.o
-obj-$(CONFIG_PATA_ATP867X)      += pata_atp867x.o
-obj-$(CONFIG_PATA_AT32)         += pata_at32.o
 obj-$(CONFIG_PATA_ATIIXP)       += pata_atiixp.o
-obj-$(CONFIG_PATA_CMD640_PCI)   += pata_cmd640.o
+obj-$(CONFIG_PATA_ATP867X)      += pata_atp867x.o
+obj-$(CONFIG_PATA_BF54X)        += pata_bf54x.o
 obj-$(CONFIG_PATA_CMD64X)       += pata_cmd64x.o
 obj-$(CONFIG_PATA_CS5520)       += pata_cs5520.o
 obj-$(CONFIG_PATA_CS5530)       += pata_cs5530.o
@@ -39,47 +45,50 @@ obj-$(CONFIG_PATA_HPT366)	+= pata_hpt366.o
 obj-$(CONFIG_PATA_HPT37X)       += pata_hpt37x.o
 obj-$(CONFIG_PATA_HPT3X2N)      += pata_hpt3x2n.o
 obj-$(CONFIG_PATA_HPT3X3)       += pata_hpt3x3.o
-obj-$(CONFIG_PATA_ISAPNP)       += pata_isapnp.o
-obj-$(CONFIG_PATA_IT821X)       += pata_it821x.o
+obj-$(CONFIG_PATA_ICSIDE)       += pata_icside.o
 obj-$(CONFIG_PATA_IT8213)       += pata_it8213.o
+obj-$(CONFIG_PATA_IT821X)       += pata_it821x.o
 obj-$(CONFIG_PATA_JMICRON)      += pata_jmicron.o
+obj-$(CONFIG_PATA_MACIO)        += pata_macio.o
+obj-$(CONFIG_PATA_MARVELL)      += pata_marvell.o
 obj-$(CONFIG_PATA_NETCELL)      += pata_netcell.o
 obj-$(CONFIG_PATA_NINJA32)      += pata_ninja32.o
-obj-$(CONFIG_PATA_NS87410)      += pata_ns87410.o
 obj-$(CONFIG_PATA_NS87415)      += pata_ns87415.o
-obj-$(CONFIG_PATA_OPTI)         += pata_opti.o
-obj-$(CONFIG_PATA_OPTIDMA)      += pata_optidma.o
-obj-$(CONFIG_PATA_MPC52xx)      += pata_mpc52xx.o
-obj-$(CONFIG_PATA_MARVELL)      += pata_marvell.o
-obj-$(CONFIG_PATA_MPIIX)        += pata_mpiix.o
 obj-$(CONFIG_PATA_OLDPIIX)      += pata_oldpiix.o
-obj-$(CONFIG_PATA_PALMLD)       += pata_palmld.o
-obj-$(CONFIG_PATA_PCMCIA)       += pata_pcmcia.o
+obj-$(CONFIG_PATA_OPTIDMA)      += pata_optidma.o
 obj-$(CONFIG_PATA_PDC2027X)     += pata_pdc2027x.o
 obj-$(CONFIG_PATA_PDC_OLD)      += pata_pdc202xx_old.o
-obj-$(CONFIG_PATA_QDI)          += pata_qdi.o
 obj-$(CONFIG_PATA_RADISYS)      += pata_radisys.o
-obj-$(CONFIG_PATA_RB532)        += pata_rb532_cf.o
 obj-$(CONFIG_PATA_RDC)          += pata_rdc.o
-obj-$(CONFIG_PATA_RZ1000)       += pata_rz1000.o
 obj-$(CONFIG_PATA_SC1200)       += pata_sc1200.o
+obj-$(CONFIG_PATA_SCC)          += pata_scc.o
+obj-$(CONFIG_PATA_SCH)          += pata_sch.o
 obj-$(CONFIG_PATA_SERVERWORKS)  += pata_serverworks.o
 obj-$(CONFIG_PATA_SIL680)       += pata_sil680.o
+obj-$(CONFIG_PATA_SIS)          += pata_sis.o
 obj-$(CONFIG_PATA_TOSHIBA)      += pata_piccolo.o
+obj-$(CONFIG_PATA_TRIFLEX)      += pata_triflex.o
 obj-$(CONFIG_PATA_VIA)          += pata_via.o
 obj-$(CONFIG_PATA_WINBOND)      += pata_sl82c105.o
-obj-$(CONFIG_PATA_WINBOND_VLB)  += pata_winbond.o
-obj-$(CONFIG_PATA_SIS)          += pata_sis.o
-obj-$(CONFIG_PATA_TRIFLEX)      += pata_triflex.o
+
+# SFF PIO only
+obj-$(CONFIG_PATA_AT32)         += pata_at32.o
+obj-$(CONFIG_PATA_AT91)         += pata_at91.o
+obj-$(CONFIG_PATA_CMD640_PCI)   += pata_cmd640.o
+obj-$(CONFIG_PATA_ISAPNP)       += pata_isapnp.o
 obj-$(CONFIG_PATA_IXP4XX_CF)    += pata_ixp4xx_cf.o
-obj-$(CONFIG_PATA_SCC)          += pata_scc.o
-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_MPIIX)        += pata_mpiix.o
+obj-$(CONFIG_PATA_NS87410)      += pata_ns87410.o
+obj-$(CONFIG_PATA_OPTI)         += pata_opti.o
+obj-$(CONFIG_PATA_PCMCIA)       += pata_pcmcia.o
+obj-$(CONFIG_PATA_PALMLD)       += pata_palmld.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
+obj-$(CONFIG_PATA_QDI)          += pata_qdi.o
+obj-$(CONFIG_PATA_RB532)        += pata_rb532_cf.o
+obj-$(CONFIG_PATA_RZ1000)       += pata_rz1000.o
+obj-$(CONFIG_PATA_WINBOND_VLB)  += pata_winbond.o
+
 # Should be last but two libata driver
 obj-$(CONFIG_PATA_ACPI)         += pata_acpi.o
 # Should be last but one libata driver
diff --git a/drivers/ata/ata_generic.c b/drivers/ata/ata_generic.c
index 33fb614f9784..573158a9668d 100644
--- a/drivers/ata/ata_generic.c
+++ b/drivers/ata/ata_generic.c
@@ -155,7 +155,7 @@ static int ata_generic_init_one(struct pci_dev *dev, const struct pci_device_id
                         return rc;
                 pcim_pin_device(dev);
         }
-        return ata_pci_sff_init_one(dev, ppi, &generic_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(dev, ppi, &generic_sht, NULL, 0);
 }
 
 static struct pci_device_id ata_generic[] = {
diff --git a/drivers/ata/ata_piix.c b/drivers/ata/ata_piix.c
index ec52fc618763..7409f98d2ae6 100644
--- a/drivers/ata/ata_piix.c
+++ b/drivers/ata/ata_piix.c
@@ -1589,7 +1589,7 @@ static int __devinit piix_init_one(struct pci_dev *pdev,
                 hpriv->map = piix_init_sata_map(pdev, port_info,
                                         piix_map_db_table[ent->driver_data]);
 
-        rc = ata_pci_sff_prepare_host(pdev, ppi, &host);
+        rc = ata_pci_bmdma_prepare_host(pdev, ppi, &host);
         if (rc)
                 return rc;
         host->private_data = hpriv;
@@ -1626,7 +1626,7 @@ static int __devinit piix_init_one(struct pci_dev *pdev,
         host->flags |= ATA_HOST_PARALLEL_SCAN;
 
         pci_set_master(pdev);
-        return ata_pci_sff_activate_host(host, ata_sff_interrupt, &piix_sht);
+        return ata_pci_sff_activate_host(host, ata_bmdma_interrupt, &piix_sht);
 }
 
 static void piix_remove_one(struct pci_dev *pdev)
diff --git a/drivers/ata/libahci.c b/drivers/ata/libahci.c
index 1984a6e89e84..261f86d102e8 100644
--- a/drivers/ata/libahci.c
+++ b/drivers/ata/libahci.c
@@ -541,29 +541,11 @@ static int ahci_scr_write(struct ata_link *link, unsigned int sc_reg, u32 val)
         return -EINVAL;
 }
 
-static int ahci_is_device_present(void __iomem *port_mmio)
-{
-        u8 status = readl(port_mmio + PORT_TFDATA) & 0xff;
-
-        /* Make sure PxTFD.STS.BSY and PxTFD.STS.DRQ are 0 */
-        if (status & (ATA_BUSY | ATA_DRQ))
-                return 0;
-
-        /* Make sure PxSSTS.DET is 3h */
-        status = readl(port_mmio + PORT_SCR_STAT) & 0xf;
-        if (status != 3)
-                return 0;
-        return 1;
-}
-
 void ahci_start_engine(struct ata_port *ap)
 {
         void __iomem *port_mmio = ahci_port_base(ap);
         u32 tmp;
 
-        if (!ahci_is_device_present(port_mmio))
-                return;
-
         /* start DMA */
         tmp = readl(port_mmio + PORT_CMD);
         tmp |= PORT_CMD_START;
@@ -1892,6 +1874,9 @@ static void ahci_error_handler(struct ata_port *ap)
         }
 
         sata_pmp_error_handler(ap);
+
+        if (!ata_dev_enabled(ap->link.device))
+                ahci_stop_engine(ap);
 }
 
 static void ahci_post_internal_cmd(struct ata_queued_cmd *qc)
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index c47373f01f89..ddf8e4862787 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -160,6 +160,10 @@ 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 (0=off [default], 1=on)");
 
+static int atapi_an;
+module_param(atapi_an, int, 0444);
+MODULE_PARM_DESC(atapi_an, "Enable ATAPI AN media presence notification (0=0ff [default], 1=on)");
+
 MODULE_AUTHOR("Jeff Garzik");
 MODULE_DESCRIPTION("Library module for ATA devices");
 MODULE_LICENSE("GPL");
@@ -2122,6 +2126,14 @@ retry:
                 goto err_out;
         }
 
+        if (dev->horkage & ATA_HORKAGE_DUMP_ID) {
+                ata_dev_printk(dev, KERN_DEBUG, "dumping IDENTIFY data, "
+                               "class=%d may_fallback=%d tried_spinup=%d\n",
+                               class, may_fallback, tried_spinup);
+                print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET,
+                               16, 2, id, ATA_ID_WORDS * sizeof(*id), true);
+        }
+
         /* Falling back doesn't make sense if ID data was read
          * successfully at least once.
          */
@@ -2510,7 +2522,8 @@ int ata_dev_configure(struct ata_device *dev)
                  * to enable ATAPI AN to discern between PHY status
                  * changed notifications and ATAPI ANs.
                  */
-                if ((ap->flags & ATA_FLAG_AN) && ata_id_has_atapi_AN(id) &&
+                if (atapi_an &&
+                    (ap->flags & ATA_FLAG_AN) && ata_id_has_atapi_AN(id) &&
                     (!sata_pmp_attached(ap) ||
                      sata_scr_read(&ap->link, SCR_NOTIFICATION, &sntf) == 0)) {
                         unsigned int err_mask;
@@ -4106,9 +4119,8 @@ int ata_dev_revalidate(struct ata_device *dev, unsigned int new_class,
             dev->n_sectors > n_sectors && dev->n_sectors == n_native_sectors) {
                 ata_dev_printk(dev, KERN_WARNING,
                                "new n_sectors matches native, probably "
-                               "late HPA unlock, continuing\n");
-                /* keep using the old n_sectors */
-                dev->n_sectors = n_sectors;
+                               "late HPA unlock, n_sectors updated\n");
+                /* use the larger n_sectors */
                 return 0;
         }
 
@@ -6372,6 +6384,7 @@ static int __init ata_parse_force_one(char **cur,
                 { "3.0Gbps",    .spd_limit      = 2 },
                 { "noncq",      .horkage_on     = ATA_HORKAGE_NONCQ },
                 { "ncq",        .horkage_off    = ATA_HORKAGE_NONCQ },
+                { "dump_id",    .horkage_on     = ATA_HORKAGE_DUMP_ID },
                 { "pio0",       .xfer_mask      = 1 << (ATA_SHIFT_PIO + 0) },
                 { "pio1",       .xfer_mask      = 1 << (ATA_SHIFT_PIO + 1) },
                 { "pio2",       .xfer_mask      = 1 << (ATA_SHIFT_PIO + 2) },
@@ -6655,6 +6668,7 @@ EXPORT_SYMBOL_GPL(ata_dummy_port_info);
 EXPORT_SYMBOL_GPL(ata_link_next);
 EXPORT_SYMBOL_GPL(ata_dev_next);
 EXPORT_SYMBOL_GPL(ata_std_bios_param);
+EXPORT_SYMBOL_GPL(ata_scsi_unlock_native_capacity);
 EXPORT_SYMBOL_GPL(ata_host_init);
 EXPORT_SYMBOL_GPL(ata_host_alloc);
 EXPORT_SYMBOL_GPL(ata_host_alloc_pinfo);
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index cfa9dd3d7253..a54273d2c3c6 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -415,6 +415,35 @@ int ata_std_bios_param(struct scsi_device *sdev, struct block_device *bdev,
 }
 
 /**
+ *      ata_scsi_unlock_native_capacity - unlock native capacity
+ *      @sdev: SCSI device to adjust device capacity for
+ *
+ *      This function is called if a partition on @sdev extends beyond
+ *      the end of the device.  It requests EH to unlock HPA.
+ *
+ *      LOCKING:
+ *      Defined by the SCSI layer.  Might sleep.
+ */
+void ata_scsi_unlock_native_capacity(struct scsi_device *sdev)
+{
+        struct ata_port *ap = ata_shost_to_port(sdev->host);
+        struct ata_device *dev;
+        unsigned long flags;
+
+        spin_lock_irqsave(ap->lock, flags);
+
+        dev = ata_scsi_find_dev(ap, sdev);
+        if (dev && dev->n_sectors < dev->n_native_sectors) {
+                dev->flags |= ATA_DFLAG_UNLOCK_HPA;
+                dev->link->eh_info.action |= ATA_EH_RESET;
+                ata_port_schedule_eh(ap);
+        }
+
+        spin_unlock_irqrestore(ap->lock, flags);
+        ata_port_wait_eh(ap);
+}
+
+/**
  *      ata_get_identity - Handler for HDIO_GET_IDENTITY ioctl
  *      @ap: target port
  *      @sdev: SCSI device to get identify data for
diff --git a/drivers/ata/libata-sff.c b/drivers/ata/libata-sff.c
index 19ddf924944f..efa4a18cfb9d 100644
--- a/drivers/ata/libata-sff.c
+++ b/drivers/ata/libata-sff.c
@@ -63,7 +63,6 @@ const struct ata_port_operations ata_sff_port_ops = {
         .sff_tf_read            = ata_sff_tf_read,
         .sff_exec_command       = ata_sff_exec_command,
         .sff_data_xfer          = ata_sff_data_xfer,
-        .sff_irq_clear          = ata_sff_irq_clear,
         .sff_drain_fifo         = ata_sff_drain_fifo,
 
         .lost_interrupt         = ata_sff_lost_interrupt,
@@ -395,33 +394,12 @@ void ata_sff_irq_on(struct ata_port *ap)
                 ata_sff_set_devctl(ap, ap->ctl);
         ata_wait_idle(ap);
 
-        ap->ops->sff_irq_clear(ap);
+        if (ap->ops->sff_irq_clear)
+                ap->ops->sff_irq_clear(ap);
 }
 EXPORT_SYMBOL_GPL(ata_sff_irq_on);
 
 /**
- *      ata_sff_irq_clear - Clear PCI IDE BMDMA interrupt.
- *      @ap: Port associated with this ATA transaction.
- *
- *      Clear interrupt and error flags in DMA status register.
- *
- *      May be used as the irq_clear() entry in ata_port_operations.
- *
- *      LOCKING:
- *      spin_lock_irqsave(host lock)
- */
-void ata_sff_irq_clear(struct ata_port *ap)
-{
-        void __iomem *mmio = ap->ioaddr.bmdma_addr;
-
-        if (!mmio)
-                return;
-
-        iowrite8(ioread8(mmio + ATA_DMA_STATUS), mmio + ATA_DMA_STATUS);
-}
-EXPORT_SYMBOL_GPL(ata_sff_irq_clear);
-
-/**
  *      ata_sff_tf_load - send taskfile registers to host controller
  *      @ap: Port to which output is sent
  *      @tf: ATA taskfile register set
@@ -820,11 +798,15 @@ static void atapi_send_cdb(struct ata_port *ap, struct ata_queued_cmd *qc)
         case ATAPI_PROT_NODATA:
                 ap->hsm_task_state = HSM_ST_LAST;
                 break;
+#ifdef CONFIG_ATA_BMDMA
         case ATAPI_PROT_DMA:
                 ap->hsm_task_state = HSM_ST_LAST;
                 /* initiate bmdma */
                 ap->ops->bmdma_start(qc);
                 break;
+#endif /* CONFIG_ATA_BMDMA */
+        default:
+                BUG();
         }
 }
 
@@ -1491,27 +1473,27 @@ bool ata_sff_qc_fill_rtf(struct ata_queued_cmd *qc)
 }
 EXPORT_SYMBOL_GPL(ata_sff_qc_fill_rtf);
 
-/**
- *      ata_sff_host_intr - Handle host interrupt for given (port, task)
- *      @ap: Port on which interrupt arrived (possibly...)
- *      @qc: Taskfile currently active in engine
- *
- *      Handle host interrupt for given queued command.  Currently,
- *      only DMA interrupts are handled.  All other commands are
- *      handled via polling with interrupts disabled (nIEN bit).
- *
- *      LOCKING:
- *      spin_lock_irqsave(host lock)
- *
- *      RETURNS:
- *      One if interrupt was handled, zero if not (shared irq).
- */
-unsigned int ata_sff_host_intr(struct ata_port *ap,
-                                      struct ata_queued_cmd *qc)
+static unsigned int ata_sff_idle_irq(struct ata_port *ap)
 {
-        struct ata_eh_info *ehi = &ap->link.eh_info;
-        u8 status, host_stat = 0;
-        bool bmdma_stopped = false;
+        ap->stats.idle_irq++;
+
+#ifdef ATA_IRQ_TRAP
+        if ((ap->stats.idle_irq % 1000) == 0) {
+                ap->ops->sff_check_status(ap);
+                if (ap->ops->sff_irq_clear)
+                        ap->ops->sff_irq_clear(ap);
+                ata_port_printk(ap, KERN_WARNING, "irq trap\n");
+                return 1;
+        }
+#endif
+        return 0;       /* irq not handled */
+}
+
+static unsigned int __ata_sff_port_intr(struct ata_port *ap,
+                                        struct ata_queued_cmd *qc,
+                                        bool hsmv_on_idle)
+{
+        u8 status;
 
         VPRINTK("ata%u: protocol %d task_state %d\n",
                 ap->print_id, qc->tf.protocol, ap->hsm_task_state);
@@ -1528,90 +1510,56 @@ unsigned int ata_sff_host_intr(struct ata_port *ap,
                  * need to check ata_is_atapi(qc->tf.protocol) again.
                  */
                 if (!(qc->dev->flags & ATA_DFLAG_CDB_INTR))
-                        goto idle_irq;
-                break;
-        case HSM_ST_LAST:
-                if (qc->tf.protocol == ATA_PROT_DMA ||
-                    qc->tf.protocol == ATAPI_PROT_DMA) {
-                        /* check status of DMA engine */
-                        host_stat = ap->ops->bmdma_status(ap);
-                        VPRINTK("ata%u: host_stat 0x%X\n",
-                                ap->print_id, host_stat);
-
-                        /* if it's not our irq... */
-                        if (!(host_stat & ATA_DMA_INTR))
-                                goto idle_irq;
-
-                        /* before we do anything else, clear DMA-Start bit */
-                        ap->ops->bmdma_stop(qc);
-                        bmdma_stopped = true;
-
-                        if (unlikely(host_stat & ATA_DMA_ERR)) {
-                                /* error when transfering data to/from memory */
-                                qc->err_mask |= AC_ERR_HOST_BUS;
-                                ap->hsm_task_state = HSM_ST_ERR;
-                        }
-                }
+                        return ata_sff_idle_irq(ap);
                 break;
         case HSM_ST:
+        case HSM_ST_LAST:
                 break;
         default:
-                goto idle_irq;
+                return ata_sff_idle_irq(ap);
         }
 
-
         /* check main status, clearing INTRQ if needed */
         status = ata_sff_irq_status(ap);
         if (status & ATA_BUSY) {
-                if (bmdma_stopped) {
+                if (hsmv_on_idle) {
                         /* BMDMA engine is already stopped, we're screwed */
                         qc->err_mask |= AC_ERR_HSM;
                         ap->hsm_task_state = HSM_ST_ERR;
                 } else
-                        goto idle_irq;
+                        return ata_sff_idle_irq(ap);
         }
 
         /* clear irq events */
-        ap->ops->sff_irq_clear(ap);
+        if (ap->ops->sff_irq_clear)
+                ap->ops->sff_irq_clear(ap);
 
         ata_sff_hsm_move(ap, qc, status, 0);
 
-        if (unlikely(qc->err_mask) && (qc->tf.protocol == ATA_PROT_DMA ||
-                                       qc->tf.protocol == ATAPI_PROT_DMA))
-                ata_ehi_push_desc(ehi, "BMDMA stat 0x%x", host_stat);
-
         return 1;       /* irq handled */
-
-idle_irq:
-        ap->stats.idle_irq++;
-
-#ifdef ATA_IRQ_TRAP
-        if ((ap->stats.idle_irq % 1000) == 0) {
-                ap->ops->sff_check_status(ap);
-                ap->ops->sff_irq_clear(ap);
-                ata_port_printk(ap, KERN_WARNING, "irq trap\n");
-                return 1;
-        }
-#endif
-        return 0;       /* irq not handled */
 }
-EXPORT_SYMBOL_GPL(ata_sff_host_intr);
 
 /**
- *      ata_sff_interrupt - Default ATA host interrupt handler
- *      @irq: irq line (unused)
- *      @dev_instance: pointer to our ata_host information structure
+ *      ata_sff_port_intr - Handle SFF port interrupt
+ *      @ap: Port on which interrupt arrived (possibly...)
+ *      @qc: Taskfile currently active in engine
  *
- *      Default interrupt handler for PCI IDE devices.  Calls
- *      ata_sff_host_intr() for each port that is not disabled.
+ *      Handle port interrupt for given queued command.
  *
  *      LOCKING:
- *      Obtains host lock during operation.
+ *      spin_lock_irqsave(host lock)
  *
  *      RETURNS:
- *      IRQ_NONE or IRQ_HANDLED.
+ *      One if interrupt was handled, zero if not (shared irq).
  */
-irqreturn_t ata_sff_interrupt(int irq, void *dev_instance)
+unsigned int ata_sff_port_intr(struct ata_port *ap, struct ata_queued_cmd *qc)
+{
+        return __ata_sff_port_intr(ap, qc, false);
+}
+EXPORT_SYMBOL_GPL(ata_sff_port_intr);
+
+static inline irqreturn_t __ata_sff_interrupt(int irq, void *dev_instance,
+        unsigned int (*port_intr)(struct ata_port *, struct ata_queued_cmd *))
 {
         struct ata_host *host = dev_instance;
         bool retried = false;
@@ -1631,7 +1579,7 @@ retry:
                 qc = ata_qc_from_tag(ap, ap->link.active_tag);
                 if (qc) {
                         if (!(qc->tf.flags & ATA_TFLAG_POLLING))
-                                handled |= ata_sff_host_intr(ap, qc);
+                                handled |= port_intr(ap, qc);
                         else
                                 polling |= 1 << i;
                 } else
@@ -1658,7 +1606,8 @@ retry:
 
                         if (idle & (1 << i)) {
                                 ap->ops->sff_check_status(ap);
-                                ap->ops->sff_irq_clear(ap);
+                                if (ap->ops->sff_irq_clear)
+                                        ap->ops->sff_irq_clear(ap);
                         } else {
                                 /* clear INTRQ and check if BUSY cleared */
                                 if (!(ap->ops->sff_check_status(ap) & ATA_BUSY))
@@ -1680,6 +1629,25 @@ retry:
 
         return IRQ_RETVAL(handled);
 }
+
+/**
+ *      ata_sff_interrupt - Default SFF ATA host interrupt handler
+ *      @irq: irq line (unused)
+ *      @dev_instance: pointer to our ata_host information structure
+ *
+ *      Default interrupt handler for PCI IDE devices.  Calls
+ *      ata_sff_port_intr() for each port that is not disabled.
+ *
+ *      LOCKING:
+ *      Obtains host lock during operation.
+ *
+ *      RETURNS:
+ *      IRQ_NONE or IRQ_HANDLED.
+ */
+irqreturn_t ata_sff_interrupt(int irq, void *dev_instance)
+{
+        return __ata_sff_interrupt(irq, dev_instance, ata_sff_port_intr);
+}
 EXPORT_SYMBOL_GPL(ata_sff_interrupt);
 
 /**
@@ -1717,7 +1685,7 @@ void ata_sff_lost_interrupt(struct ata_port *ap)
                                                                 status);
         /* Run the host interrupt logic as if the interrupt had not been
            lost */
-        ata_sff_host_intr(ap, qc);
+        ata_sff_port_intr(ap, qc);
 }
 EXPORT_SYMBOL_GPL(ata_sff_lost_interrupt);
 
@@ -1744,7 +1712,8 @@ void ata_sff_freeze(struct ata_port *ap)
          */
         ap->ops->sff_check_status(ap);
 
-        ap->ops->sff_irq_clear(ap);
+        if (ap->ops->sff_irq_clear)
+                ap->ops->sff_irq_clear(ap);
 }
 EXPORT_SYMBOL_GPL(ata_sff_freeze);
 
@@ -1761,7 +1730,8 @@ void ata_sff_thaw(struct ata_port *ap)
 {
         /* clear & re-enable interrupts */
         ap->ops->sff_check_status(ap);
-        ap->ops->sff_irq_clear(ap);
+        if (ap->ops->sff_irq_clear)
+                ap->ops->sff_irq_clear(ap);
         ata_sff_irq_on(ap);
 }
 EXPORT_SYMBOL_GPL(ata_sff_thaw);
@@ -2349,13 +2319,13 @@ int ata_pci_sff_init_host(struct ata_host *host)
 EXPORT_SYMBOL_GPL(ata_pci_sff_init_host);
 
 /**
- *      ata_pci_sff_prepare_host - helper to prepare native PCI ATA host
+ *      ata_pci_sff_prepare_host - helper to prepare PCI PIO-only SFF ATA host
  *      @pdev: target PCI device
  *      @ppi: array of port_info, must be enough for two ports
  *      @r_host: out argument for the initialized ATA host
  *
- *      Helper to allocate ATA host for @pdev, acquire all native PCI
- *      resources and initialize it accordingly in one go.
+ *      Helper to allocate PIO-only SFF ATA host for @pdev, acquire
+ *      all PCI resources and initialize it accordingly in one go.
  *
  *      LOCKING:
  *      Inherited from calling layer (may sleep).
@@ -2385,9 +2355,6 @@ int ata_pci_sff_prepare_host(struct pci_dev *pdev,
         if (rc)
                 goto err_out;
 
-        /* init DMA related stuff */
-        ata_pci_bmdma_init(host);
-
         devres_remove_group(&pdev->dev, NULL);
         *r_host = host;
         return 0;
@@ -2492,8 +2459,21 @@ out:
 }
 EXPORT_SYMBOL_GPL(ata_pci_sff_activate_host);
 
+static const struct ata_port_info *ata_sff_find_valid_pi(
+                                        const struct ata_port_info * const *ppi)
+{
+        int i;
+
+        /* look up the first valid port_info */
+        for (i = 0; i < 2 && ppi[i]; i++)
+                if (ppi[i]->port_ops != &ata_dummy_port_ops)
+                        return ppi[i];
+
+        return NULL;
+}
+
 /**
- *      ata_pci_sff_init_one - Initialize/register PCI IDE host controller
+ *      ata_pci_sff_init_one - Initialize/register PIO-only PCI IDE controller
  *      @pdev: Controller to be initialized
  *      @ppi: array of port_info, must be enough for two ports
  *      @sht: scsi_host_template to use when registering the host
@@ -2502,11 +2482,7 @@ EXPORT_SYMBOL_GPL(ata_pci_sff_activate_host);
  *
  *      This is a helper function which can be called from a driver's
  *      xxx_init_one() probe function if the hardware uses traditional
- *      IDE taskfile registers.
- *
- *      This function calls pci_enable_device(), reserves its register
- *      regions, sets the dma mask, enables bus master mode, and calls
- *      ata_device_add()
+ *      IDE taskfile registers and is PIO only.
  *
  *      ASSUMPTION:
  *      Nobody makes a single channel controller that appears solely as
@@ -2523,20 +2499,13 @@ int ata_pci_sff_init_one(struct pci_dev *pdev,
                  struct scsi_host_template *sht, void *host_priv, int hflag)
 {
         struct device *dev = &pdev->dev;
-        const struct ata_port_info *pi = NULL;
+        const struct ata_port_info *pi;
         struct ata_host *host = NULL;
-        int i, rc;
+        int rc;
 
         DPRINTK("ENTER\n");
 
-        /* look up the first valid port_info */
-        for (i = 0; i < 2 && ppi[i]; i++) {
-                if (ppi[i]->port_ops != &ata_dummy_port_ops) {
-                        pi = ppi[i];
-                        break;
-                }
-        }
-
+        pi = ata_sff_find_valid_pi(ppi);
         if (!pi) {
                 dev_printk(KERN_ERR, &pdev->dev,
                            "no valid port_info specified\n");
@@ -2557,7 +2526,6 @@ int ata_pci_sff_init_one(struct pci_dev *pdev,
         host->private_data = host_priv;
         host->flags |= hflag;
 
-        pci_set_master(pdev);
         rc = ata_pci_sff_activate_host(host, ata_sff_interrupt, sht);
 out:
         if (rc == 0)
@@ -2571,6 +2539,12 @@ EXPORT_SYMBOL_GPL(ata_pci_sff_init_one);
 
 #endif /* CONFIG_PCI */
 
+/*
+ *      BMDMA support
+ */
+
+#ifdef CONFIG_ATA_BMDMA
+
 const struct ata_port_operations ata_bmdma_port_ops = {
         .inherits               = &ata_sff_port_ops,
 
@@ -2580,6 +2554,7 @@ const struct ata_port_operations ata_bmdma_port_ops = {
         .qc_prep                = ata_bmdma_qc_prep,
         .qc_issue               = ata_bmdma_qc_issue,
 
+        .sff_irq_clear          = ata_bmdma_irq_clear,
         .bmdma_setup            = ata_bmdma_setup,
         .bmdma_start            = ata_bmdma_start,
         .bmdma_stop             = ata_bmdma_stop,
@@ -2804,6 +2779,75 @@ unsigned int ata_bmdma_qc_issue(struct ata_queued_cmd *qc)
 EXPORT_SYMBOL_GPL(ata_bmdma_qc_issue);
 
 /**
+ *      ata_bmdma_port_intr - Handle BMDMA port interrupt
+ *      @ap: Port on which interrupt arrived (possibly...)
+ *      @qc: Taskfile currently active in engine
+ *
+ *      Handle port interrupt for given queued command.
+ *
+ *      LOCKING:
+ *      spin_lock_irqsave(host lock)
+ *
+ *      RETURNS:
+ *      One if interrupt was handled, zero if not (shared irq).
+ */
+unsigned int ata_bmdma_port_intr(struct ata_port *ap, struct ata_queued_cmd *qc)
+{
+        struct ata_eh_info *ehi = &ap->link.eh_info;
+        u8 host_stat = 0;
+        bool bmdma_stopped = false;
+        unsigned int handled;
+
+        if (ap->hsm_task_state == HSM_ST_LAST && ata_is_dma(qc->tf.protocol)) {
+                /* check status of DMA engine */
+                host_stat = ap->ops->bmdma_status(ap);
+                VPRINTK("ata%u: host_stat 0x%X\n", ap->print_id, host_stat);
+
+                /* if it's not our irq... */
+                if (!(host_stat & ATA_DMA_INTR))
+                        return ata_sff_idle_irq(ap);
+
+                /* before we do anything else, clear DMA-Start bit */
+                ap->ops->bmdma_stop(qc);
+                bmdma_stopped = true;
+
+                if (unlikely(host_stat & ATA_DMA_ERR)) {
+                        /* error when transfering data to/from memory */
+                        qc->err_mask |= AC_ERR_HOST_BUS;
+                        ap->hsm_task_state = HSM_ST_ERR;
+                }
+        }
+
+        handled = __ata_sff_port_intr(ap, qc, bmdma_stopped);
+
+        if (unlikely(qc->err_mask) && ata_is_dma(qc->tf.protocol))
+                ata_ehi_push_desc(ehi, "BMDMA stat 0x%x", host_stat);
+
+        return handled;
+}
+EXPORT_SYMBOL_GPL(ata_bmdma_port_intr);
+
+/**
+ *      ata_bmdma_interrupt - Default BMDMA ATA host interrupt handler
+ *      @irq: irq line (unused)
+ *      @dev_instance: pointer to our ata_host information structure
+ *
+ *      Default interrupt handler for PCI IDE devices.  Calls
+ *      ata_bmdma_port_intr() for each port that is not disabled.
+ *
+ *      LOCKING:
+ *      Obtains host lock during operation.
+ *
+ *      RETURNS:
+ *      IRQ_NONE or IRQ_HANDLED.
+ */
+irqreturn_t ata_bmdma_interrupt(int irq, void *dev_instance)
+{
+        return __ata_sff_interrupt(irq, dev_instance, ata_bmdma_port_intr);
+}
+EXPORT_SYMBOL_GPL(ata_bmdma_interrupt);
+
+/**
  *      ata_bmdma_error_handler - Stock error handler for BMDMA controller
  *      @ap: port to handle error for
  *
@@ -2848,7 +2892,8 @@ void ata_bmdma_error_handler(struct ata_port *ap)
                 /* if we're gonna thaw, make sure IRQ is clear */
                 if (thaw) {
                         ap->ops->sff_check_status(ap);
-                        ap->ops->sff_irq_clear(ap);
+                        if (ap->ops->sff_irq_clear)
+                                ap->ops->sff_irq_clear(ap);
                 }
         }
 
@@ -2882,6 +2927,28 @@ void ata_bmdma_post_internal_cmd(struct ata_queued_cmd *qc)
 EXPORT_SYMBOL_GPL(ata_bmdma_post_internal_cmd);
 
 /**
+ *      ata_bmdma_irq_clear - Clear PCI IDE BMDMA interrupt.
+ *      @ap: Port associated with this ATA transaction.
+ *
+ *      Clear interrupt and error flags in DMA status register.
+ *
+ *      May be used as the irq_clear() entry in ata_port_operations.
+ *
+ *      LOCKING:
+ *      spin_lock_irqsave(host lock)
+ */
+void ata_bmdma_irq_clear(struct ata_port *ap)
+{
+        void __iomem *mmio = ap->ioaddr.bmdma_addr;
+
+        if (!mmio)
+                return;
+
+        iowrite8(ioread8(mmio + ATA_DMA_STATUS), mmio + ATA_DMA_STATUS);
+}
+EXPORT_SYMBOL_GPL(ata_bmdma_irq_clear);
+
+/**
  *      ata_bmdma_setup - Set up PCI IDE BMDMA transaction
  *      @qc: Info associated with this ATA transaction.
  *
@@ -3137,7 +3204,100 @@ void ata_pci_bmdma_init(struct ata_host *host)
 }
 EXPORT_SYMBOL_GPL(ata_pci_bmdma_init);
 
+/**
+ *      ata_pci_bmdma_prepare_host - helper to prepare PCI BMDMA ATA host
+ *      @pdev: target PCI device
+ *      @ppi: array of port_info, must be enough for two ports
+ *      @r_host: out argument for the initialized ATA host
+ *
+ *      Helper to allocate BMDMA ATA host for @pdev, acquire all PCI
+ *      resources and initialize it accordingly in one go.
+ *
+ *      LOCKING:
+ *      Inherited from calling layer (may sleep).
+ *
+ *      RETURNS:
+ *      0 on success, -errno otherwise.
+ */
+int ata_pci_bmdma_prepare_host(struct pci_dev *pdev,
+                               const struct ata_port_info * const * ppi,
+                               struct ata_host **r_host)
+{
+        int rc;
+
+        rc = ata_pci_sff_prepare_host(pdev, ppi, r_host);
+        if (rc)
+                return rc;
+
+        ata_pci_bmdma_init(*r_host);
+        return 0;
+}
+EXPORT_SYMBOL_GPL(ata_pci_bmdma_prepare_host);
+
+/**
+ *      ata_pci_bmdma_init_one - Initialize/register BMDMA PCI IDE controller
+ *      @pdev: Controller to be initialized
+ *      @ppi: array of port_info, must be enough for two ports
+ *      @sht: scsi_host_template to use when registering the host
+ *      @host_priv: host private_data
+ *      @hflags: host flags
+ *
+ *      This function is similar to ata_pci_sff_init_one() but also
+ *      takes care of BMDMA initialization.
+ *
+ *      LOCKING:
+ *      Inherited from PCI layer (may sleep).
+ *
+ *      RETURNS:
+ *      Zero on success, negative on errno-based value on error.
+ */
+int ata_pci_bmdma_init_one(struct pci_dev *pdev,
+                           const struct ata_port_info * const * ppi,
+                           struct scsi_host_template *sht, void *host_priv,
+                           int hflags)
+{
+        struct device *dev = &pdev->dev;
+        const struct ata_port_info *pi;
+        struct ata_host *host = NULL;
+        int rc;
+
+        DPRINTK("ENTER\n");
+
+        pi = ata_sff_find_valid_pi(ppi);
+        if (!pi) {
+                dev_printk(KERN_ERR, &pdev->dev,
+                           "no valid port_info specified\n");
+                return -EINVAL;
+        }
+
+        if (!devres_open_group(dev, NULL, GFP_KERNEL))
+                return -ENOMEM;
+
+        rc = pcim_enable_device(pdev);
+        if (rc)
+                goto out;
+
+        /* prepare and activate BMDMA host */
+        rc = ata_pci_bmdma_prepare_host(pdev, ppi, &host);
+        if (rc)
+                goto out;
+        host->private_data = host_priv;
+        host->flags |= hflags;
+
+        pci_set_master(pdev);
+        rc = ata_pci_sff_activate_host(host, ata_bmdma_interrupt, sht);
+ out:
+        if (rc == 0)
+                devres_remove_group(&pdev->dev, NULL);
+        else
+                devres_release_group(&pdev->dev, NULL);
+
+        return rc;
+}
+EXPORT_SYMBOL_GPL(ata_pci_bmdma_init_one);
+
 #endif /* CONFIG_PCI */
+#endif /* CONFIG_ATA_BMDMA */
 
 /**
  *      ata_sff_port_init - Initialize SFF/BMDMA ATA port
diff --git a/drivers/ata/pata_acpi.c b/drivers/ata/pata_acpi.c
index 066b9f301ed5..c8d47034d5e9 100644
--- a/drivers/ata/pata_acpi.c
+++ b/drivers/ata/pata_acpi.c
@@ -260,7 +260,7 @@ static int pacpi_init_one (struct pci_dev *pdev, const struct pci_device_id *id)
                         return rc;
                 pcim_pin_device(pdev);
         }
-        return ata_pci_sff_init_one(pdev, ppi, &pacpi_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &pacpi_sht, NULL, 0);
 }
 
 static const struct pci_device_id pacpi_pci_tbl[] = {
diff --git a/drivers/ata/pata_ali.c b/drivers/ata/pata_ali.c
index f306e10c748d..794ec6e3275d 100644
--- a/drivers/ata/pata_ali.c
+++ b/drivers/ata/pata_ali.c
@@ -583,7 +583,10 @@ static int ali_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
                         ppi[0] = &info_20_udma;
         }
 
-        return ata_pci_sff_init_one(pdev, ppi, &ali_sht, NULL, 0);
+        if (!ppi[0]->mwdma_mask && !ppi[0]->udma_mask)
+                return ata_pci_sff_init_one(pdev, ppi, &ali_sht, NULL, 0);
+        else
+                return ata_pci_bmdma_init_one(pdev, ppi, &ali_sht, NULL, 0);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/ata/pata_amd.c b/drivers/ata/pata_amd.c
index d95eca9c547e..620a07cabe31 100644
--- a/drivers/ata/pata_amd.c
+++ b/drivers/ata/pata_amd.c
@@ -574,7 +574,7 @@ static int amd_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
         }
 
         /* And fire it up */
-        return ata_pci_sff_init_one(pdev, ppi, &amd_sht, hpriv, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &amd_sht, hpriv, 0);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/ata/pata_artop.c b/drivers/ata/pata_artop.c
index 4d066d6c30fa..ba43f0f8c880 100644
--- a/drivers/ata/pata_artop.c
+++ b/drivers/ata/pata_artop.c
@@ -421,7 +421,7 @@ static int artop_init_one (struct pci_dev *pdev, const struct pci_device_id *id)
 
         BUG_ON(ppi[0] == NULL);
 
-        return ata_pci_sff_init_one(pdev, ppi, &artop_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &artop_sht, NULL, 0);
 }
 
 static const struct pci_device_id artop_pci_tbl[] = {
diff --git a/drivers/ata/pata_atiixp.c b/drivers/ata/pata_atiixp.c
index 44d88b380ddd..43755616dc5a 100644
--- a/drivers/ata/pata_atiixp.c
+++ b/drivers/ata/pata_atiixp.c
@@ -246,8 +246,8 @@ static int atiixp_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
                 if (!pci_test_config_bits(pdev, &atiixp_enable_bits[i]))
                         ppi[i] = &ata_dummy_port_info;
 
-        return ata_pci_sff_init_one(pdev, ppi, &atiixp_sht, NULL,
-                                                ATA_HOST_PARALLEL_SCAN);
+        return ata_pci_bmdma_init_one(pdev, ppi, &atiixp_sht, NULL,
+                                      ATA_HOST_PARALLEL_SCAN);
 }
 
 static const struct pci_device_id atiixp[] = {
diff --git a/drivers/ata/pata_atp867x.c b/drivers/ata/pata_atp867x.c
index bb6e0746e07d..95295935dd95 100644
--- a/drivers/ata/pata_atp867x.c
+++ b/drivers/ata/pata_atp867x.c
@@ -525,7 +525,7 @@ static int atp867x_init_one(struct pci_dev *pdev,
 
         pci_set_master(pdev);
 
-        rc = ata_host_activate(host, pdev->irq, ata_sff_interrupt,
+        rc = ata_host_activate(host, pdev->irq, ata_bmdma_interrupt,
                                 IRQF_SHARED, &atp867x_sht);
         if (rc)
                 dev_printk(KERN_ERR, &pdev->dev, "failed to activate host\n");
diff --git a/drivers/ata/pata_bf54x.c b/drivers/ata/pata_bf54x.c
index 6422cfd13d0d..9cae65de750e 100644
--- a/drivers/ata/pata_bf54x.c
+++ b/drivers/ata/pata_bf54x.c
@@ -1214,7 +1214,7 @@ static unsigned int bfin_data_xfer(struct ata_device *dev, unsigned char *buf,
  *      bfin_irq_clear - Clear ATAPI interrupt.
  *      @ap: Port associated with this ATA transaction.
  *
- *      Note: Original code is ata_sff_irq_clear().
+ *      Note: Original code is ata_bmdma_irq_clear().
  */
 
 static void bfin_irq_clear(struct ata_port *ap)
diff --git a/drivers/ata/pata_cmd64x.c b/drivers/ata/pata_cmd64x.c
index 4c81a71b8877..9f5da1c7454b 100644
--- a/drivers/ata/pata_cmd64x.c
+++ b/drivers/ata/pata_cmd64x.c
@@ -367,7 +367,7 @@ static int cmd64x_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
         pci_write_config_byte(pdev, UDIDETCR0, 0xF0);
 #endif
 
-        return ata_pci_sff_init_one(pdev, ppi, &cmd64x_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &cmd64x_sht, NULL, 0);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/ata/pata_cs5520.c b/drivers/ata/pata_cs5520.c
index 17c5f346ff01..030952f1f97c 100644
--- a/drivers/ata/pata_cs5520.c
+++ b/drivers/ata/pata_cs5520.c
@@ -221,7 +221,7 @@ static int __devinit cs5520_init_one(struct pci_dev *pdev, const struct pci_devi
                         continue;
 
                 rc = devm_request_irq(&pdev->dev, irq[ap->port_no],
-                                      ata_sff_interrupt, 0, DRV_NAME, host);
+                                      ata_bmdma_interrupt, 0, DRV_NAME, host);
                 if (rc)
                         return rc;
 
diff --git a/drivers/ata/pata_cs5530.c b/drivers/ata/pata_cs5530.c
index e809a4233a81..f792330f0d8e 100644
--- a/drivers/ata/pata_cs5530.c
+++ b/drivers/ata/pata_cs5530.c
@@ -324,7 +324,7 @@ static int cs5530_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
                 ppi[1] = &info_palmax_secondary;
 
         /* Now kick off ATA set up */
-        return ata_pci_sff_init_one(pdev, ppi, &cs5530_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &cs5530_sht, NULL, 0);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/ata/pata_cs5535.c b/drivers/ata/pata_cs5535.c
index a02e6459fdcc..03a93186aa19 100644
--- a/drivers/ata/pata_cs5535.c
+++ b/drivers/ata/pata_cs5535.c
@@ -198,7 +198,7 @@ static int cs5535_init_one(struct pci_dev *dev, const struct pci_device_id *id)
         rdmsr(ATAC_CH0D1_PIO, timings, dummy);
         if (CS5535_BAD_PIO(timings))
                 wrmsr(ATAC_CH0D1_PIO, 0xF7F4F7F4UL, 0);
-        return ata_pci_sff_init_one(dev, ppi, &cs5535_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(dev, ppi, &cs5535_sht, NULL, 0);
 }
 
 static const struct pci_device_id cs5535[] = {
diff --git a/drivers/ata/pata_cs5536.c b/drivers/ata/pata_cs5536.c
index 914ae3506ff5..21ee23f89e88 100644
--- a/drivers/ata/pata_cs5536.c
+++ b/drivers/ata/pata_cs5536.c
@@ -260,7 +260,7 @@ static int cs5536_init_one(struct pci_dev *dev, const struct pci_device_id *id)
                 return -ENODEV;
         }
 
-        return ata_pci_sff_init_one(dev, ppi, &cs5536_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(dev, ppi, &cs5536_sht, NULL, 0);
 }
 
 static const struct pci_device_id cs5536[] = {
diff --git a/drivers/ata/pata_cypress.c b/drivers/ata/pata_cypress.c
index 0fcc096b8dac..6d915b063d93 100644
--- a/drivers/ata/pata_cypress.c
+++ b/drivers/ata/pata_cypress.c
@@ -138,7 +138,7 @@ static int cy82c693_init_one(struct pci_dev *pdev, const struct pci_device_id *i
         if (PCI_FUNC(pdev->devfn) != 1)
                 return -ENODEV;
 
-        return ata_pci_sff_init_one(pdev, ppi, &cy82c693_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &cy82c693_sht, NULL, 0);
 }
 
 static const struct pci_device_id cy82c693[] = {
diff --git a/drivers/ata/pata_efar.c b/drivers/ata/pata_efar.c
index 3bac0e079691..a08834758ea2 100644
--- a/drivers/ata/pata_efar.c
+++ b/drivers/ata/pata_efar.c
@@ -277,8 +277,8 @@ static int efar_init_one (struct pci_dev *pdev, const struct pci_device_id *ent)
                 dev_printk(KERN_DEBUG, &pdev->dev,
                            "version " DRV_VERSION "\n");
 
-        return ata_pci_sff_init_one(pdev, ppi, &efar_sht, NULL,
-                                        ATA_HOST_PARALLEL_SCAN);
+        return ata_pci_bmdma_init_one(pdev, ppi, &efar_sht, NULL,
+                                      ATA_HOST_PARALLEL_SCAN);
 }
 
 static const struct pci_device_id efar_pci_tbl[] = {
diff --git a/drivers/ata/pata_hpt366.c b/drivers/ata/pata_hpt366.c
index 8580eb3cd54d..7688868557b9 100644
--- a/drivers/ata/pata_hpt366.c
+++ b/drivers/ata/pata_hpt366.c
@@ -361,7 +361,7 @@ static int hpt36x_init_one(struct pci_dev *dev, const struct pci_device_id *id)
                         break;
         }
         /* Now kick off ATA set up */
-        return ata_pci_sff_init_one(dev, ppi, &hpt36x_sht, hpriv, 0);
+        return ata_pci_bmdma_init_one(dev, ppi, &hpt36x_sht, hpriv, 0);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/ata/pata_hpt37x.c b/drivers/ata/pata_hpt37x.c
index 98b498b6907c..9ae4c0830577 100644
--- a/drivers/ata/pata_hpt37x.c
+++ b/drivers/ata/pata_hpt37x.c
@@ -987,7 +987,7 @@ static int hpt37x_init_one(struct pci_dev *dev, const struct pci_device_id *id)
         }
 
         /* Now kick off ATA set up */
-        return ata_pci_sff_init_one(dev, ppi, &hpt37x_sht, private_data, 0);
+        return ata_pci_bmdma_init_one(dev, ppi, &hpt37x_sht, private_data, 0);
 }
 
 static const struct pci_device_id hpt37x[] = {
diff --git a/drivers/ata/pata_hpt3x2n.c b/drivers/ata/pata_hpt3x2n.c
index 8b95aeba0e74..32f3463216b8 100644
--- a/drivers/ata/pata_hpt3x2n.c
+++ b/drivers/ata/pata_hpt3x2n.c
@@ -548,7 +548,7 @@ static int hpt3x2n_init_one(struct pci_dev *dev, const struct pci_device_id *id)
                 outb(inb(iobase + 0x9c) | 0x04, iobase + 0x9c);
 
         /* Now kick off ATA set up */
-        return ata_pci_sff_init_one(dev, ppi, &hpt3x2n_sht, hpriv, 0);
+        return ata_pci_bmdma_init_one(dev, ppi, &hpt3x2n_sht, hpriv, 0);
 }
 
 static const struct pci_device_id hpt3x2n[] = {
diff --git a/drivers/ata/pata_hpt3x3.c b/drivers/ata/pata_hpt3x3.c
index 727a81ce4c9f..b63d5e2d4628 100644
--- a/drivers/ata/pata_hpt3x3.c
+++ b/drivers/ata/pata_hpt3x3.c
@@ -248,7 +248,7 @@ static int hpt3x3_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
                 ata_port_pbar_desc(ap, 4, offset_cmd[i], "cmd");
         }
         pci_set_master(pdev);
-        return ata_host_activate(host, pdev->irq, ata_sff_interrupt,
+        return ata_host_activate(host, pdev->irq, ata_bmdma_interrupt,
                                  IRQF_SHARED, &hpt3x3_sht);
 }
 
diff --git a/drivers/ata/pata_icside.c b/drivers/ata/pata_icside.c
index b56e8f722d20..9f2889fe43b2 100644
--- a/drivers/ata/pata_icside.c
+++ b/drivers/ata/pata_icside.c
@@ -470,7 +470,7 @@ static int __devinit pata_icside_add_ports(struct pata_icside_info *info)
                 pata_icside_setup_ioaddr(ap, info->base, info, info->port[i]);
         }
 
-        return ata_host_activate(host, ec->irq, ata_sff_interrupt, 0,
+        return ata_host_activate(host, ec->irq, ata_bmdma_interrupt, 0,
                                  &pata_icside_sht);
 }
 
diff --git a/drivers/ata/pata_it8213.c b/drivers/ata/pata_it8213.c
index f971f0de88e6..4d142a2ab8fd 100644
--- a/drivers/ata/pata_it8213.c
+++ b/drivers/ata/pata_it8213.c
@@ -273,7 +273,7 @@ static int it8213_init_one (struct pci_dev *pdev, const struct pci_device_id *en
                 dev_printk(KERN_DEBUG, &pdev->dev,
                            "version " DRV_VERSION "\n");
 
-        return ata_pci_sff_init_one(pdev, ppi, &it8213_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &it8213_sht, NULL, 0);
 }
 
 static const struct pci_device_id it8213_pci_tbl[] = {
diff --git a/drivers/ata/pata_it821x.c b/drivers/ata/pata_it821x.c
index 2bd2b002d14a..bf88f71a21f4 100644
--- a/drivers/ata/pata_it821x.c
+++ b/drivers/ata/pata_it821x.c
@@ -933,7 +933,7 @@ static int it821x_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
                 else
                         ppi[0] = &info_smart;
         }
-        return ata_pci_sff_init_one(pdev, ppi, &it821x_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &it821x_sht, NULL, 0);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/ata/pata_jmicron.c b/drivers/ata/pata_jmicron.c
index 565e01e6ac7c..cb3babbb7035 100644
--- a/drivers/ata/pata_jmicron.c
+++ b/drivers/ata/pata_jmicron.c
@@ -144,7 +144,7 @@ static int jmicron_init_one (struct pci_dev *pdev, const struct pci_device_id *i
         };
         const struct ata_port_info *ppi[] = { &info, NULL };
 
-        return ata_pci_sff_init_one(pdev, ppi, &jmicron_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &jmicron_sht, NULL, 0);
 }
 
 static const struct pci_device_id jmicron_pci_tbl[] = {
diff --git a/drivers/ata/pata_macio.c b/drivers/ata/pata_macio.c
index b5b48e703cb7..75b49d01780b 100644
--- a/drivers/ata/pata_macio.c
+++ b/drivers/ata/pata_macio.c
@@ -1110,7 +1110,7 @@ static int __devinit pata_macio_common_init(struct pata_macio_priv	*priv,
 
         /* Start it up */
         priv->irq = irq;
-        return ata_host_activate(priv->host, irq, ata_sff_interrupt, 0,
+        return ata_host_activate(priv->host, irq, ata_bmdma_interrupt, 0,
                                  &pata_macio_sht);
 }
 
@@ -1355,8 +1355,11 @@ static struct of_device_id pata_macio_match[] =
 
 static struct macio_driver pata_macio_driver =
 {
-        .name           = "pata-macio",
-        .match_table    = pata_macio_match,
+        .driver = {
+                .name           = "pata-macio",
+                .owner          = THIS_MODULE,
+                .of_match_table = pata_macio_match,
+        },
         .probe          = pata_macio_attach,
         .remove         = pata_macio_detach,
 #ifdef CONFIG_PM
@@ -1366,9 +1369,6 @@ static struct macio_driver pata_macio_driver =
 #ifdef CONFIG_PMAC_MEDIABAY
         .mediabay_event = pata_macio_mb_event,
 #endif
-        .driver = {
-                .owner          = THIS_MODULE,
-        },
 };
 
 static const struct pci_device_id pata_macio_pci_match[] = {
diff --git a/drivers/ata/pata_marvell.c b/drivers/ata/pata_marvell.c
index e8ca02e5a71d..dd38083dcbeb 100644
--- a/drivers/ata/pata_marvell.c
+++ b/drivers/ata/pata_marvell.c
@@ -153,7 +153,7 @@ static int marvell_init_one (struct pci_dev *pdev, const struct pci_device_id *i
                 return -ENODEV;
         }
 #endif
-        return ata_pci_sff_init_one(pdev, ppi, &marvell_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &marvell_sht, NULL, 0);
 }
 
 static const struct pci_device_id marvell_pci_tbl[] = {
diff --git a/drivers/ata/pata_mpc52xx.c b/drivers/ata/pata_mpc52xx.c
index 36afe2c1c747..f087ab55b1df 100644
--- a/drivers/ata/pata_mpc52xx.c
+++ b/drivers/ata/pata_mpc52xx.c
@@ -659,7 +659,7 @@ mpc52xx_ata_init_one(struct device *dev, struct mpc52xx_ata_priv *priv,
         ata_port_desc(ap, "ata_regs 0x%lx", raw_ata_regs);
 
         /* activate host */
-        return ata_host_activate(host, priv->ata_irq, ata_sff_interrupt, 0,
+        return ata_host_activate(host, priv->ata_irq, ata_bmdma_interrupt, 0,
                                  &mpc52xx_ata_sht);
 }
 
diff --git a/drivers/ata/pata_netcell.c b/drivers/ata/pata_netcell.c
index 94f979a7f4f7..3eb921c746a1 100644
--- a/drivers/ata/pata_netcell.c
+++ b/drivers/ata/pata_netcell.c
@@ -82,7 +82,7 @@ static int netcell_init_one (struct pci_dev *pdev, const struct pci_device_id *e
         ata_pci_bmdma_clear_simplex(pdev);
 
         /* And let the library code do the work */
-        return ata_pci_sff_init_one(pdev, port_info, &netcell_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, port_info, &netcell_sht, NULL, 0);
 }
 
 static const struct pci_device_id netcell_pci_tbl[] = {
diff --git a/drivers/ata/pata_ninja32.c b/drivers/ata/pata_ninja32.c
index dd53a66b19e3..cc50bd09aa26 100644
--- a/drivers/ata/pata_ninja32.c
+++ b/drivers/ata/pata_ninja32.c
@@ -149,7 +149,7 @@ static int ninja32_init_one(struct pci_dev *dev, const struct pci_device_id *id)
 
         ninja32_program(base);
         /* FIXME: Should we disable them at remove ? */
-        return ata_host_activate(host, dev->irq, ata_sff_interrupt,
+        return ata_host_activate(host, dev->irq, ata_bmdma_interrupt,
                                  IRQF_SHARED, &ninja32_sht);
 }
 
diff --git a/drivers/ata/pata_ns87415.c b/drivers/ata/pata_ns87415.c
index fdbba2d76d3e..605f198f958c 100644
--- a/drivers/ata/pata_ns87415.c
+++ b/drivers/ata/pata_ns87415.c
@@ -380,7 +380,7 @@ static int ns87415_init_one (struct pci_dev *pdev, const struct pci_device_id *e
 
         ns87415_fixup(pdev);
 
-        return ata_pci_sff_init_one(pdev, ppi, &ns87415_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &ns87415_sht, NULL, 0);
 }
 
 static const struct pci_device_id ns87415_pci_tbl[] = {
diff --git a/drivers/ata/pata_octeon_cf.c b/drivers/ata/pata_octeon_cf.c
index 3001109352ea..06ddd91ffeda 100644
--- a/drivers/ata/pata_octeon_cf.c
+++ b/drivers/ata/pata_octeon_cf.c
@@ -750,20 +750,6 @@ static void octeon_cf_dev_config(struct ata_device *dev)
 }
 
 /*
- * Trap if driver tries to do standard bmdma commands.  They are not
- * supported.
- */
-static void unreachable_qc(struct ata_queued_cmd *qc)
-{
-        BUG();
-}
-
-static u8 unreachable_port(struct ata_port *ap)
-{
-        BUG();
-}
-
-/*
  * We don't do ATAPI DMA so return 0.
  */
 static int octeon_cf_check_atapi_dma(struct ata_queued_cmd *qc)
@@ -804,10 +790,6 @@ static struct ata_port_operations octeon_cf_ops = {
         .sff_dev_select         = octeon_cf_dev_select,
         .sff_irq_on             = octeon_cf_irq_on,
         .sff_irq_clear          = octeon_cf_irq_clear,
-        .bmdma_setup            = unreachable_qc,
-        .bmdma_start            = unreachable_qc,
-        .bmdma_stop             = unreachable_qc,
-        .bmdma_status           = unreachable_port,
         .cable_detect           = ata_cable_40wire,
         .set_piomode            = octeon_cf_set_piomode,
         .set_dmamode            = octeon_cf_set_dmamode,
diff --git a/drivers/ata/pata_oldpiix.c b/drivers/ata/pata_oldpiix.c
index 988ef2627be3..b811c1636204 100644
--- a/drivers/ata/pata_oldpiix.c
+++ b/drivers/ata/pata_oldpiix.c
@@ -248,7 +248,7 @@ static int oldpiix_init_one (struct pci_dev *pdev, const struct pci_device_id *e
                 dev_printk(KERN_DEBUG, &pdev->dev,
                            "version " DRV_VERSION "\n");
 
-        return ata_pci_sff_init_one(pdev, ppi, &oldpiix_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &oldpiix_sht, NULL, 0);
 }
 
 static const struct pci_device_id oldpiix_pci_tbl[] = {
diff --git a/drivers/ata/pata_optidma.c b/drivers/ata/pata_optidma.c
index 76b7d12b1e8d..0852cd07de08 100644
--- a/drivers/ata/pata_optidma.c
+++ b/drivers/ata/pata_optidma.c
@@ -429,7 +429,7 @@ static int optidma_init_one(struct pci_dev *dev, const struct pci_device_id *id)
         if (optiplus_with_udma(dev))
                 ppi[0] = &info_82c700_udma;
 
-        return ata_pci_sff_init_one(dev, ppi, &optidma_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(dev, ppi, &optidma_sht, NULL, 0);
 }
 
 static const struct pci_device_id optidma[] = {
diff --git a/drivers/ata/pata_pdc2027x.c b/drivers/ata/pata_pdc2027x.c
index 09f1f22c0307..b18351122525 100644
--- a/drivers/ata/pata_pdc2027x.c
+++ b/drivers/ata/pata_pdc2027x.c
@@ -754,7 +754,7 @@ static int __devinit pdc2027x_init_one(struct pci_dev *pdev, const struct pci_de
                 return -EIO;
 
         pci_set_master(pdev);
-        return ata_host_activate(host, pdev->irq, ata_sff_interrupt,
+        return ata_host_activate(host, pdev->irq, ata_bmdma_interrupt,
                                  IRQF_SHARED, &pdc2027x_sht);
 }
 
diff --git a/drivers/ata/pata_pdc202xx_old.c b/drivers/ata/pata_pdc202xx_old.c
index fa1e2f3bc0fd..c39f213e1bbc 100644
--- a/drivers/ata/pata_pdc202xx_old.c
+++ b/drivers/ata/pata_pdc202xx_old.c
@@ -337,7 +337,7 @@ static int pdc202xx_init_one(struct pci_dev *dev, const struct pci_device_id *id
                                 return -ENODEV;
                 }
         }
-        return ata_pci_sff_init_one(dev, ppi, &pdc202xx_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(dev, ppi, &pdc202xx_sht, NULL, 0);
 }
 
 static const struct pci_device_id pdc202xx[] = {
diff --git a/drivers/ata/pata_piccolo.c b/drivers/ata/pata_piccolo.c
index 981615414849..cb01bf9496fe 100644
--- a/drivers/ata/pata_piccolo.c
+++ b/drivers/ata/pata_piccolo.c
@@ -95,7 +95,7 @@ static int ata_tosh_init_one(struct pci_dev *dev, const struct pci_device_id *id
         };
         const struct ata_port_info *ppi[] = { &info, &ata_dummy_port_info };
         /* Just one port for the moment */
-        return ata_pci_sff_init_one(dev, ppi, &tosh_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(dev, ppi, &tosh_sht, NULL, 0);
 }
 
 static struct pci_device_id ata_tosh[] = {
diff --git a/drivers/ata/pata_radisys.c b/drivers/ata/pata_radisys.c
index a5fa388e5398..8574b31f1773 100644
--- a/drivers/ata/pata_radisys.c
+++ b/drivers/ata/pata_radisys.c
@@ -227,7 +227,7 @@ static int radisys_init_one (struct pci_dev *pdev, const struct pci_device_id *e
                 dev_printk(KERN_DEBUG, &pdev->dev,
                            "version " DRV_VERSION "\n");
 
-        return ata_pci_sff_init_one(pdev, ppi, &radisys_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &radisys_sht, NULL, 0);
 }
 
 static const struct pci_device_id radisys_pci_tbl[] = {
diff --git a/drivers/ata/pata_rdc.c b/drivers/ata/pata_rdc.c
index 37092cfd7bc6..5fbe9b166c69 100644
--- a/drivers/ata/pata_rdc.c
+++ b/drivers/ata/pata_rdc.c
@@ -344,7 +344,7 @@ static int __devinit rdc_init_one(struct pci_dev *pdev,
          */
         pci_read_config_dword(pdev, 0x54, &hpriv->saved_iocfg);
 
-        rc = ata_pci_sff_prepare_host(pdev, ppi, &host);
+        rc = ata_pci_bmdma_prepare_host(pdev, ppi, &host);
         if (rc)
                 return rc;
         host->private_data = hpriv;
@@ -354,7 +354,7 @@ static int __devinit rdc_init_one(struct pci_dev *pdev,
         host->flags |= ATA_HOST_PARALLEL_SCAN;
 
         pci_set_master(pdev);
-        return ata_pci_sff_activate_host(host, ata_sff_interrupt, &rdc_sht);
+        return ata_pci_sff_activate_host(host, ata_bmdma_interrupt, &rdc_sht);
 }
 
 static void rdc_remove_one(struct pci_dev *pdev)
diff --git a/drivers/ata/pata_sc1200.c b/drivers/ata/pata_sc1200.c
index 6b5b63a2fd8e..e2c18257adff 100644
--- a/drivers/ata/pata_sc1200.c
+++ b/drivers/ata/pata_sc1200.c
@@ -237,7 +237,7 @@ static int sc1200_init_one(struct pci_dev *dev, const struct pci_device_id *id)
         };
         const struct ata_port_info *ppi[] = { &info, NULL };
 
-        return ata_pci_sff_init_one(dev, ppi, &sc1200_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(dev, ppi, &sc1200_sht, NULL, 0);
 }
 
 static const struct pci_device_id sc1200[] = {
diff --git a/drivers/ata/pata_scc.c b/drivers/ata/pata_scc.c
index 6f6193b707cb..d9db3f8d60ef 100644
--- a/drivers/ata/pata_scc.c
+++ b/drivers/ata/pata_scc.c
@@ -875,7 +875,7 @@ static void scc_postreset(struct ata_link *link, unsigned int *classes)
  *      scc_irq_clear - Clear PCI IDE BMDMA interrupt.
  *      @ap: Port associated with this ATA transaction.
  *
- *      Note: Original code is ata_sff_irq_clear().
+ *      Note: Original code is ata_bmdma_irq_clear().
  */
 
 static void scc_irq_clear (struct ata_port *ap)
@@ -1105,7 +1105,7 @@ static int scc_init_one (struct pci_dev *pdev, const struct pci_device_id *ent)
         if (rc)
                 return rc;
 
-        return ata_host_activate(host, pdev->irq, ata_sff_interrupt,
+        return ata_host_activate(host, pdev->irq, ata_bmdma_interrupt,
                                  IRQF_SHARED, &scc_sht);
 }
 
diff --git a/drivers/ata/pata_sch.c b/drivers/ata/pata_sch.c
index 86b3d0133c7c..e97b32f03a6e 100644
--- a/drivers/ata/pata_sch.c
+++ b/drivers/ata/pata_sch.c
@@ -179,7 +179,7 @@ static int __devinit sch_init_one(struct pci_dev *pdev,
                 dev_printk(KERN_DEBUG, &pdev->dev,
                            "version " DRV_VERSION "\n");
 
-        return ata_pci_sff_init_one(pdev, ppi, &sch_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &sch_sht, NULL, 0);
 }
 
 static int __init sch_init(void)
diff --git a/drivers/ata/pata_serverworks.c b/drivers/ata/pata_serverworks.c
index 43ea389df2b3..86dd714e3e1d 100644
--- a/drivers/ata/pata_serverworks.c
+++ b/drivers/ata/pata_serverworks.c
@@ -460,7 +460,7 @@ static int serverworks_init_one(struct pci_dev *pdev, const struct pci_device_id
         if (pdev->device == PCI_DEVICE_ID_SERVERWORKS_CSB5IDE)
                 ata_pci_bmdma_clear_simplex(pdev);
 
-        return ata_pci_sff_init_one(pdev, ppi, &serverworks_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &serverworks_sht, NULL, 0);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/ata/pata_sil680.c b/drivers/ata/pata_sil680.c
index 43faf106f647..d3190d7ec304 100644
--- a/drivers/ata/pata_sil680.c
+++ b/drivers/ata/pata_sil680.c
@@ -374,11 +374,11 @@ static int __devinit sil680_init_one(struct pci_dev *pdev,
         ata_sff_std_ports(&host->ports[1]->ioaddr);
 
         /* Register & activate */
-        return ata_host_activate(host, pdev->irq, ata_sff_interrupt,
+        return ata_host_activate(host, pdev->irq, ata_bmdma_interrupt,
                                  IRQF_SHARED, &sil680_sht);
 
 use_ioports:
-        return ata_pci_sff_init_one(pdev, ppi, &sil680_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &sil680_sht, NULL, 0);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/ata/pata_sis.c b/drivers/ata/pata_sis.c
index b6708032f321..60cea13cccce 100644
--- a/drivers/ata/pata_sis.c
+++ b/drivers/ata/pata_sis.c
@@ -826,7 +826,7 @@ static int sis_init_one (struct pci_dev *pdev, const struct pci_device_id *ent)
 
         sis_fixup(pdev, chipset);
 
-        return ata_pci_sff_init_one(pdev, ppi, &sis_sht, chipset, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &sis_sht, chipset, 0);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/ata/pata_sl82c105.c b/drivers/ata/pata_sl82c105.c
index 733b042a7469..98548f640c8e 100644
--- a/drivers/ata/pata_sl82c105.c
+++ b/drivers/ata/pata_sl82c105.c
@@ -316,7 +316,7 @@ static int sl82c105_init_one(struct pci_dev *dev, const struct pci_device_id *id
         val |= CTRL_P0EN | CTRL_P0F16 | CTRL_P1F16;
         pci_write_config_dword(dev, 0x40, val);
 
-        return ata_pci_sff_init_one(dev, ppi, &sl82c105_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(dev, ppi, &sl82c105_sht, NULL, 0);
 }
 
 static const struct pci_device_id sl82c105[] = {
diff --git a/drivers/ata/pata_triflex.c b/drivers/ata/pata_triflex.c
index 48f50600ed2a..0d1f89e571dd 100644
--- a/drivers/ata/pata_triflex.c
+++ b/drivers/ata/pata_triflex.c
@@ -201,7 +201,7 @@ static int triflex_init_one(struct pci_dev *dev, const struct pci_device_id *id)
         if (!printed_version++)
                 dev_printk(KERN_DEBUG, &dev->dev, "version " DRV_VERSION "\n");
 
-        return ata_pci_sff_init_one(dev, ppi, &triflex_sht, NULL, 0);
+        return ata_pci_bmdma_init_one(dev, ppi, &triflex_sht, NULL, 0);
 }
 
 static const struct pci_device_id triflex[] = {
diff --git a/drivers/ata/pata_via.c b/drivers/ata/pata_via.c
index 7e3e0a5598b7..5e659885de16 100644
--- a/drivers/ata/pata_via.c
+++ b/drivers/ata/pata_via.c
@@ -627,7 +627,7 @@ static int via_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
         }
 
         /* We have established the device type, now fire it up */
-        return ata_pci_sff_init_one(pdev, ppi, &via_sht, (void *)config, 0);
+        return ata_pci_bmdma_init_one(pdev, ppi, &via_sht, (void *)config, 0);
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/ata/sata_mv.c b/drivers/ata/sata_mv.c
index f3471bc949d3..a476cd99b95d 100644
--- a/drivers/ata/sata_mv.c
+++ b/drivers/ata/sata_mv.c
@@ -675,8 +675,6 @@ static struct ata_port_operations mv5_ops = {
         .freeze                 = mv_eh_freeze,
         .thaw                   = mv_eh_thaw,
         .hardreset              = mv_hardreset,
-        .error_handler          = ata_std_error_handler, /* avoid SFF EH */
-        .post_internal_cmd      = ATA_OP_NULL,
 
         .scr_read               = mv5_scr_read,
         .scr_write              = mv5_scr_write,
@@ -2813,7 +2811,7 @@ static void mv_port_intr(struct ata_port *ap, u32 port_cause)
         } else if (!edma_was_enabled) {
                 struct ata_queued_cmd *qc = mv_get_active_qc(ap);
                 if (qc)
-                        ata_sff_host_intr(ap, qc);
+                        ata_bmdma_port_intr(ap, qc);
                 else
                         mv_unexpected_intr(ap, edma_was_enabled);
         }
diff --git a/drivers/ata/sata_nv.c b/drivers/ata/sata_nv.c
index baa8f0d2c86f..21161136cad0 100644
--- a/drivers/ata/sata_nv.c
+++ b/drivers/ata/sata_nv.c
@@ -920,7 +920,7 @@ static int nv_host_intr(struct ata_port *ap, u8 irq_stat)
         }
 
         /* handle interrupt */
-        return ata_sff_host_intr(ap, qc);
+        return ata_bmdma_port_intr(ap, qc);
 }
 
 static irqreturn_t nv_adma_interrupt(int irq, void *dev_instance)
@@ -1100,7 +1100,7 @@ static void nv_adma_irq_clear(struct ata_port *ap)
         u32 notifier_clears[2];
 
         if (pp->flags & NV_ADMA_ATAPI_SETUP_COMPLETE) {
-                ata_sff_irq_clear(ap);
+                ata_bmdma_irq_clear(ap);
                 return;
         }
 
@@ -1505,7 +1505,7 @@ static irqreturn_t nv_generic_interrupt(int irq, void *dev_instance)
 
                 qc = ata_qc_from_tag(ap, ap->link.active_tag);
                 if (qc && (!(qc->tf.flags & ATA_TFLAG_POLLING))) {
-                        handled += ata_sff_host_intr(ap, qc);
+                        handled += ata_bmdma_port_intr(ap, qc);
                 } else {
                         /*
                          * No request pending?  Clear interrupt status
@@ -1669,7 +1669,6 @@ static void nv_mcp55_freeze(struct ata_port *ap)
         mask = readl(mmio_base + NV_INT_ENABLE_MCP55);
         mask &= ~(NV_INT_ALL_MCP55 << shift);
         writel(mask, mmio_base + NV_INT_ENABLE_MCP55);
-        ata_sff_freeze(ap);
 }
 
 static void nv_mcp55_thaw(struct ata_port *ap)
@@ -1683,7 +1682,6 @@ static void nv_mcp55_thaw(struct ata_port *ap)
         mask = readl(mmio_base + NV_INT_ENABLE_MCP55);
         mask |= (NV_INT_MASK_MCP55 << shift);
         writel(mask, mmio_base + NV_INT_ENABLE_MCP55);
-        ata_sff_thaw(ap);
 }
 
 static void nv_adma_error_handler(struct ata_port *ap)
@@ -2430,7 +2428,7 @@ static int nv_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
         ppi[0] = &nv_port_info[type];
         ipriv = ppi[0]->private_data;
-        rc = ata_pci_sff_prepare_host(pdev, ppi, &host);
+        rc = ata_pci_bmdma_prepare_host(pdev, ppi, &host);
         if (rc)
                 return rc;
 
diff --git a/drivers/ata/sata_qstor.c b/drivers/ata/sata_qstor.c
index d533b3d20ca1..daeebf19a6a9 100644
--- a/drivers/ata/sata_qstor.c
+++ b/drivers/ata/sata_qstor.c
@@ -120,8 +120,6 @@ static void qs_host_stop(struct ata_host *host);
 static void qs_qc_prep(struct ata_queued_cmd *qc);
 static unsigned int qs_qc_issue(struct ata_queued_cmd *qc);
 static int qs_check_atapi_dma(struct ata_queued_cmd *qc);
-static void qs_bmdma_stop(struct ata_queued_cmd *qc);
-static u8 qs_bmdma_status(struct ata_port *ap);
 static void qs_freeze(struct ata_port *ap);
 static void qs_thaw(struct ata_port *ap);
 static int qs_prereset(struct ata_link *link, unsigned long deadline);
@@ -137,8 +135,6 @@ static struct ata_port_operations qs_ata_ops = {
         .inherits               = &ata_sff_port_ops,
 
         .check_atapi_dma        = qs_check_atapi_dma,
-        .bmdma_stop             = qs_bmdma_stop,
-        .bmdma_status           = qs_bmdma_status,
         .qc_prep                = qs_qc_prep,
         .qc_issue               = qs_qc_issue,
 
@@ -190,16 +186,6 @@ static int qs_check_atapi_dma(struct ata_queued_cmd *qc)
         return 1;       /* ATAPI DMA not supported */
 }
 
-static void qs_bmdma_stop(struct ata_queued_cmd *qc)
-{
-        /* nothing */
-}
-
-static u8 qs_bmdma_status(struct ata_port *ap)
-{
-        return 0;
-}
-
 static inline void qs_enter_reg_mode(struct ata_port *ap)
 {
         u8 __iomem *chan = qs_mmio_base(ap->host) + (ap->port_no * 0x4000);
@@ -454,7 +440,7 @@ static inline unsigned int qs_intr_mmio(struct ata_host *host)
                 if (!pp || pp->state != qs_state_mmio)
                         continue;
                 if (!(qc->tf.flags & ATA_TFLAG_POLLING))
-                        handled |= ata_sff_host_intr(ap, qc);
+                        handled |= ata_sff_port_intr(ap, qc);
         }
         return handled;
 }
diff --git a/drivers/ata/sata_sil.c b/drivers/ata/sata_sil.c
index 2dda312b6b9a..3a4f84219719 100644
--- a/drivers/ata/sata_sil.c
+++ b/drivers/ata/sata_sil.c
@@ -503,7 +503,7 @@ static void sil_host_intr(struct ata_port *ap, u32 bmdma2)
                 goto err_hsm;
 
         /* ack bmdma irq events */
-        ata_sff_irq_clear(ap);
+        ata_bmdma_irq_clear(ap);
 
         /* kick HSM in the ass */
         ata_sff_hsm_move(ap, qc, status, 0);
@@ -584,7 +584,7 @@ static void sil_thaw(struct ata_port *ap)
 
         /* clear IRQ */
         ap->ops->sff_check_status(ap);
-        ata_sff_irq_clear(ap);
+        ata_bmdma_irq_clear(ap);
 
         /* turn on SATA IRQ if supported */
         if (!(ap->flags & SIL_FLAG_NO_SATA_IRQ))
diff --git a/drivers/ata/sata_sil24.c b/drivers/ata/sata_sil24.c
index e9250514734b..be7726d7686d 100644
--- a/drivers/ata/sata_sil24.c
+++ b/drivers/ata/sata_sil24.c
@@ -539,12 +539,12 @@ static void sil24_config_port(struct ata_port *ap)
                 writel(PORT_CS_IRQ_WOC, port + PORT_CTRL_CLR);
 
         /* zero error counters. */
-        writel(0x8000, port + PORT_DECODE_ERR_THRESH);
-        writel(0x8000, port + PORT_CRC_ERR_THRESH);
-        writel(0x8000, port + PORT_HSHK_ERR_THRESH);
-        writel(0x0000, port + PORT_DECODE_ERR_CNT);
-        writel(0x0000, port + PORT_CRC_ERR_CNT);
-        writel(0x0000, port + PORT_HSHK_ERR_CNT);
+        writew(0x8000, port + PORT_DECODE_ERR_THRESH);
+        writew(0x8000, port + PORT_CRC_ERR_THRESH);
+        writew(0x8000, port + PORT_HSHK_ERR_THRESH);
+        writew(0x0000, port + PORT_DECODE_ERR_CNT);
+        writew(0x0000, port + PORT_CRC_ERR_CNT);
+        writew(0x0000, port + PORT_HSHK_ERR_CNT);
 
         /* always use 64bit activation */
         writel(PORT_CS_32BIT_ACTV, port + PORT_CTRL_CLR);
@@ -622,6 +622,11 @@ static int sil24_exec_polled_cmd(struct ata_port *ap, int pmp,
         irq_enabled = readl(port + PORT_IRQ_ENABLE_SET);
         writel(PORT_IRQ_COMPLETE | PORT_IRQ_ERROR, port + PORT_IRQ_ENABLE_CLR);
 
+        /*
+         * The barrier is required to ensure that writes to cmd_block reach
+         * the memory before the write to PORT_CMD_ACTIVATE.
+         */
+        wmb();
         writel((u32)paddr, port + PORT_CMD_ACTIVATE);
         writel((u64)paddr >> 32, port + PORT_CMD_ACTIVATE + 4);
 
@@ -865,7 +870,7 @@ static void sil24_qc_prep(struct ata_queued_cmd *qc)
         } else {
                 prb = &cb->atapi.prb;
                 sge = cb->atapi.sge;
-                memset(cb->atapi.cdb, 0, 32);
+                memset(cb->atapi.cdb, 0, sizeof(cb->atapi.cdb));
                 memcpy(cb->atapi.cdb, qc->cdb, qc->dev->cdb_len);
 
                 if (ata_is_data(qc->tf.protocol)) {
@@ -895,6 +900,11 @@ static unsigned int sil24_qc_issue(struct ata_queued_cmd *qc)
         paddr = pp->cmd_block_dma + tag * sizeof(*pp->cmd_block);
         activate = port + PORT_CMD_ACTIVATE + tag * 8;
 
+        /*
+         * The barrier is required to ensure that writes to cmd_block reach
+         * the memory before the write to PORT_CMD_ACTIVATE.
+         */
+        wmb();
         writel((u32)paddr, activate);
         writel((u64)paddr >> 32, activate + 4);
 
diff --git a/drivers/ata/sata_sis.c b/drivers/ata/sata_sis.c
index f8a91bfd66a8..2bfe3ae03976 100644
--- a/drivers/ata/sata_sis.c
+++ b/drivers/ata/sata_sis.c
@@ -279,7 +279,7 @@ static int sis_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
                 break;
         }
 
-        rc = ata_pci_sff_prepare_host(pdev, ppi, &host);
+        rc = ata_pci_bmdma_prepare_host(pdev, ppi, &host);
         if (rc)
                 return rc;
 
@@ -308,7 +308,7 @@ static int sis_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
         pci_set_master(pdev);
         pci_intx(pdev, 1);
-        return ata_host_activate(host, pdev->irq, ata_sff_interrupt,
+        return ata_host_activate(host, pdev->irq, ata_bmdma_interrupt,
                                  IRQF_SHARED, &sis_sht);
 }
 
diff --git a/drivers/ata/sata_svw.c b/drivers/ata/sata_svw.c
index 101fd6a19829..7d9db4aaf07e 100644
--- a/drivers/ata/sata_svw.c
+++ b/drivers/ata/sata_svw.c
@@ -502,7 +502,7 @@ static int k2_sata_init_one(struct pci_dev *pdev, const struct pci_device_id *en
         writel(0x0, mmio_base + K2_SATA_SIM_OFFSET);
 
         pci_set_master(pdev);
-        return ata_host_activate(host, pdev->irq, ata_sff_interrupt,
+        return ata_host_activate(host, pdev->irq, ata_bmdma_interrupt,
                                  IRQF_SHARED, &k2_sata_sht);
 }
 
diff --git a/drivers/ata/sata_uli.c b/drivers/ata/sata_uli.c
index d8dac17dc2c8..b8578c32d344 100644
--- a/drivers/ata/sata_uli.c
+++ b/drivers/ata/sata_uli.c
@@ -242,7 +242,7 @@ static int uli_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
         pci_set_master(pdev);
         pci_intx(pdev, 1);
-        return ata_host_activate(host, pdev->irq, ata_sff_interrupt,
+        return ata_host_activate(host, pdev->irq, ata_bmdma_interrupt,
                                  IRQF_SHARED, &uli_sht);
 }
 
diff --git a/drivers/ata/sata_via.c b/drivers/ata/sata_via.c
index 08f65492cc81..4730c42a5ee5 100644
--- a/drivers/ata/sata_via.c
+++ b/drivers/ata/sata_via.c
@@ -308,7 +308,7 @@ static void svia_noop_freeze(struct ata_port *ap)
          * certain way.  Leave it alone and just clear pending IRQ.
          */
         ap->ops->sff_check_status(ap);
-        ata_sff_irq_clear(ap);
+        ata_bmdma_irq_clear(ap);
 }
 
 /**
@@ -463,7 +463,7 @@ static int vt6420_prepare_host(struct pci_dev *pdev, struct ata_host **r_host)
         struct ata_host *host;
         int rc;
 
-        rc = ata_pci_sff_prepare_host(pdev, ppi, &host);
+        rc = ata_pci_bmdma_prepare_host(pdev, ppi, &host);
         if (rc)
                 return rc;
         *r_host = host;
@@ -520,7 +520,7 @@ static int vt8251_prepare_host(struct pci_dev *pdev, struct ata_host **r_host)
         struct ata_host *host;
         int i, rc;
 
-        rc = ata_pci_sff_prepare_host(pdev, ppi, &host);
+        rc = ata_pci_bmdma_prepare_host(pdev, ppi, &host);
         if (rc)
                 return rc;
         *r_host = host;
@@ -575,6 +575,33 @@ static void svia_configure(struct pci_dev *pdev)
                 tmp8 |= NATIVE_MODE_ALL;
                 pci_write_config_byte(pdev, SATA_NATIVE_MODE, tmp8);
         }
+
+        /*
+         * vt6421 has problems talking to some drives.  The following
+         * is the fix from Joseph Chan <JosephChan@via.com.tw>.
+         *
+         * When host issues HOLD, device may send up to 20DW of data
+         * before acknowledging it with HOLDA and the host should be
+         * able to buffer them in FIFO.  Unfortunately, some WD drives
+         * send upto 40DW before acknowledging HOLD and, in the
+         * default configuration, this ends up overflowing vt6421's
+         * FIFO, making the controller abort the transaction with
+         * R_ERR.
+         *
+         * Rx52[2] is the internal 128DW FIFO Flow control watermark
+         * adjusting mechanism enable bit and the default value 0
+         * means host will issue HOLD to device when the left FIFO
+         * size goes below 32DW.  Setting it to 1 makes the watermark
+         * 64DW.
+         *
+         * https://bugzilla.kernel.org/show_bug.cgi?id=15173
+         * http://article.gmane.org/gmane.linux.ide/46352
+         */
+        if (pdev->device == 0x3249) {
+                pci_read_config_byte(pdev, 0x52, &tmp8);
+                tmp8 |= 1 << 2;
+                pci_write_config_byte(pdev, 0x52, tmp8);
+        }
 }
 
 static int svia_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
@@ -628,7 +655,7 @@ static int svia_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
         svia_configure(pdev);
 
         pci_set_master(pdev);
-        return ata_host_activate(host, pdev->irq, ata_sff_interrupt,
+        return ata_host_activate(host, pdev->irq, ata_bmdma_interrupt,
                                  IRQF_SHARED, &svia_sht);
 }
 
diff --git a/drivers/ata/sata_vsc.c b/drivers/ata/sata_vsc.c
index 2107952ebff1..b777176ff494 100644
--- a/drivers/ata/sata_vsc.c
+++ b/drivers/ata/sata_vsc.c
@@ -245,7 +245,7 @@ static void vsc_port_intr(u8 port_status, struct ata_port *ap)
 
         qc = ata_qc_from_tag(ap, ap->link.active_tag);
         if (qc && likely(!(qc->tf.flags & ATA_TFLAG_POLLING)))
-                handled = ata_sff_host_intr(ap, qc);
+                handled = ata_bmdma_port_intr(ap, qc);
 
         /* We received an interrupt during a polled command,
          * or some other spurious condition.  Interrupt reporting
diff --git a/drivers/base/topology.c b/drivers/base/topology.c
index bf6b13206d00..9fc630ce1ddb 100644
--- a/drivers/base/topology.c
+++ b/drivers/base/topology.c
@@ -162,7 +162,7 @@ static int __cpuinit topology_cpu_callback(struct notifier_block *nfb,
                 topology_remove_dev(cpu);
                 break;
         }
-        return rc ? NOTIFY_BAD : NOTIFY_OK;
+        return notifier_from_errno(rc);
 }
 
 static int __cpuinit topology_sysfs_init(void)
diff --git a/drivers/block/brd.c b/drivers/block/brd.c
index 6081e81d5738..f1bf79d9bc0a 100644
--- a/drivers/block/brd.c
+++ b/drivers/block/brd.c
@@ -133,6 +133,28 @@ static struct page *brd_insert_page(struct brd_device *brd, sector_t sector)
         return page;
 }
 
+static void brd_free_page(struct brd_device *brd, sector_t sector)
+{
+        struct page *page;
+        pgoff_t idx;
+
+        spin_lock(&brd->brd_lock);
+        idx = sector >> PAGE_SECTORS_SHIFT;
+        page = radix_tree_delete(&brd->brd_pages, idx);
+        spin_unlock(&brd->brd_lock);
+        if (page)
+                __free_page(page);
+}
+
+static void brd_zero_page(struct brd_device *brd, sector_t sector)
+{
+        struct page *page;
+
+        page = brd_lookup_page(brd, sector);
+        if (page)
+                clear_highpage(page);
+}
+
 /*
  * Free all backing store pages and radix tree. This must only be called when
  * there are no other users of the device.
@@ -189,6 +211,24 @@ static int copy_to_brd_setup(struct brd_device *brd, sector_t sector, size_t n)
         return 0;
 }
 
+static void discard_from_brd(struct brd_device *brd,
+                        sector_t sector, size_t n)
+{
+        while (n >= PAGE_SIZE) {
+                /*
+                 * Don't want to actually discard pages here because
+                 * re-allocating the pages can result in writeback
+                 * deadlocks under heavy load.
+                 */
+                if (0)
+                        brd_free_page(brd, sector);
+                else
+                        brd_zero_page(brd, sector);
+                sector += PAGE_SIZE >> SECTOR_SHIFT;
+                n -= PAGE_SIZE;
+        }
+}
+
 /*
  * Copy n bytes from src to the brd starting at sector. Does not sleep.
  */
@@ -300,6 +340,12 @@ static int brd_make_request(struct request_queue *q, struct bio *bio)
                                                 get_capacity(bdev->bd_disk))
                 goto out;
 
+        if (unlikely(bio_rw_flagged(bio, BIO_RW_DISCARD))) {
+                err = 0;
+                discard_from_brd(brd, sector, bio->bi_size);
+                goto out;
+        }
+
         rw = bio_rw(bio);
         if (rw == READA)
                 rw = READ;
@@ -320,7 +366,7 @@ out:
 }
 
 #ifdef CONFIG_BLK_DEV_XIP
-static int brd_direct_access (struct block_device *bdev, sector_t sector,
+static int brd_direct_access(struct block_device *bdev, sector_t sector,
                         void **kaddr, unsigned long *pfn)
 {
         struct brd_device *brd = bdev->bd_disk->private_data;
@@ -437,6 +483,11 @@ static struct brd_device *brd_alloc(int i)
         blk_queue_max_hw_sectors(brd->brd_queue, 1024);
         blk_queue_bounce_limit(brd->brd_queue, BLK_BOUNCE_ANY);
 
+        brd->brd_queue->limits.discard_granularity = PAGE_SIZE;
+        brd->brd_queue->limits.max_discard_sectors = UINT_MAX;
+        brd->brd_queue->limits.discard_zeroes_data = 1;
+        queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, brd->brd_queue);
+
         disk = brd->brd_disk = alloc_disk(1 << part_shift);
         if (!disk)
                 goto out_free_queue;
diff --git a/drivers/block/cciss_scsi.c b/drivers/block/cciss_scsi.c
index e1d0e2cfec72..3381505c8a6c 100644
--- a/drivers/block/cciss_scsi.c
+++ b/drivers/block/cciss_scsi.c
@@ -188,11 +188,11 @@ scsi_cmd_free(ctlr_info_t *h, CommandList_struct *cmd)
 
         sa = h->scsi_ctlr;
         stk = &sa->cmd_stack; 
+        stk->top++;
         if (stk->top >= CMD_STACK_SIZE) {
                 printk("cciss: scsi_cmd_free called too many times.\n");
                 BUG();
         }
-        stk->top++;
         stk->elem[stk->top] = (struct cciss_scsi_cmd_stack_elem_t *) cmd;
 }
 
diff --git a/drivers/block/drbd/drbd_int.h b/drivers/block/drbd/drbd_int.h
index e9654c8d5b62..485ed8c7d623 100644
--- a/drivers/block/drbd/drbd_int.h
+++ b/drivers/block/drbd/drbd_int.h
@@ -943,8 +943,7 @@ struct drbd_conf {
         struct drbd_work  resync_work,
                           unplug_work,
                           md_sync_work,
-                          delay_probe_work,
-                          uuid_work;
+                          delay_probe_work;
         struct timer_list resync_timer;
         struct timer_list md_sync_timer;
         struct timer_list delay_probe_timer;
@@ -1069,7 +1068,6 @@ struct drbd_conf {
         struct timeval dps_time; /* delay-probes-start-time */
         unsigned int dp_volume_last;  /* send_cnt of last delay probe */
         int c_sync_rate; /* current resync rate after delay_probe magic */
-        atomic_t new_c_uuid;
 };
 
 static inline struct drbd_conf *minor_to_mdev(unsigned int minor)
@@ -1476,7 +1474,6 @@ extern int w_e_end_ov_req(struct drbd_conf *, struct drbd_work *, int);
 extern int w_ov_finished(struct drbd_conf *, struct drbd_work *, int);
 extern int w_resync_inactive(struct drbd_conf *, struct drbd_work *, int);
 extern int w_resume_next_sg(struct drbd_conf *, struct drbd_work *, int);
-extern int w_io_error(struct drbd_conf *, struct drbd_work *, int);
 extern int w_send_write_hint(struct drbd_conf *, struct drbd_work *, int);
 extern int w_make_resync_request(struct drbd_conf *, struct drbd_work *, int);
 extern int w_send_dblock(struct drbd_conf *, struct drbd_work *, int);
@@ -1542,7 +1539,7 @@ static inline void drbd_tcp_nodelay(struct socket *sock)
 
 static inline void drbd_tcp_quickack(struct socket *sock)
 {
-        int __user val = 1;
+        int __user val = 2;
         (void) drbd_setsockopt(sock, SOL_TCP, TCP_QUICKACK,
                         (char __user *)&val, sizeof(val));
 }
@@ -1728,7 +1725,7 @@ static inline void __drbd_chk_io_error_(struct drbd_conf *mdev, int forcedetach,
         switch (mdev->ldev->dc.on_io_error) {
         case EP_PASS_ON:
                 if (!forcedetach) {
-                        if (printk_ratelimit())
+                        if (__ratelimit(&drbd_ratelimit_state))
                                 dev_err(DEV, "Local IO failed in %s."
                                              "Passing error on...\n", where);
                         break;
@@ -2219,8 +2216,6 @@ static inline int __inc_ap_bio_cond(struct drbd_conf *mdev)
                 return 0;
         if (test_bit(BITMAP_IO, &mdev->flags))
                 return 0;
-        if (atomic_read(&mdev->new_c_uuid))
-                return 0;
         return 1;
 }
 
@@ -2241,9 +2236,6 @@ static inline void inc_ap_bio(struct drbd_conf *mdev, int count)
          * to avoid races with the reconnect code,
          * we need to atomic_inc within the spinlock. */
 
-        if (atomic_read(&mdev->new_c_uuid) && atomic_add_unless(&mdev->new_c_uuid, -1, 1))
-                drbd_queue_work_front(&mdev->data.work, &mdev->uuid_work);
-
         spin_lock_irq(&mdev->req_lock);
         while (!__inc_ap_bio_cond(mdev)) {
                 prepare_to_wait(&mdev->misc_wait, &wait, TASK_UNINTERRUPTIBLE);
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index be2d2da9cdba..6b077f93acc6 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -1215,18 +1215,17 @@ static void after_state_ch(struct drbd_conf *mdev, union drbd_state os,
              ns.pdsk == D_OUTDATED)) {
                 if (get_ldev(mdev)) {
                         if ((ns.role == R_PRIMARY || ns.peer == R_PRIMARY) &&
-                            mdev->ldev->md.uuid[UI_BITMAP] == 0 && ns.disk >= D_UP_TO_DATE &&
-                            !atomic_read(&mdev->new_c_uuid))
-                                atomic_set(&mdev->new_c_uuid, 2);
+                            mdev->ldev->md.uuid[UI_BITMAP] == 0 && ns.disk >= D_UP_TO_DATE) {
+                                drbd_uuid_new_current(mdev);
+                                drbd_send_uuids(mdev);
+                        }
                         put_ldev(mdev);
                 }
         }
 
         if (ns.pdsk < D_INCONSISTENT && get_ldev(mdev)) {
-                /* Diskless peer becomes primary or got connected do diskless, primary peer. */
-                if (ns.peer == R_PRIMARY && mdev->ldev->md.uuid[UI_BITMAP] == 0 &&
-                    !atomic_read(&mdev->new_c_uuid))
-                        atomic_set(&mdev->new_c_uuid, 2);
+                if (ns.peer == R_PRIMARY && mdev->ldev->md.uuid[UI_BITMAP] == 0)
+                        drbd_uuid_new_current(mdev);
 
                 /* D_DISKLESS Peer becomes secondary */
                 if (os.peer == R_PRIMARY && ns.peer == R_SECONDARY)
@@ -1350,24 +1349,6 @@ static void after_state_ch(struct drbd_conf *mdev, union drbd_state os,
         drbd_md_sync(mdev);
 }
 
-static int w_new_current_uuid(struct drbd_conf *mdev, struct drbd_work *w, int cancel)
-{
-        if (get_ldev(mdev)) {
-                if (mdev->ldev->md.uuid[UI_BITMAP] == 0) {
-                        drbd_uuid_new_current(mdev);
-                        if (get_net_conf(mdev)) {
-                                drbd_send_uuids(mdev);
-                                put_net_conf(mdev);
-                        }
-                        drbd_md_sync(mdev);
-                }
-                put_ldev(mdev);
-        }
-        atomic_dec(&mdev->new_c_uuid);
-        wake_up(&mdev->misc_wait);
-
-        return 1;
-}
 
 static int drbd_thread_setup(void *arg)
 {
@@ -2291,9 +2272,9 @@ static int we_should_drop_the_connection(struct drbd_conf *mdev, struct socket *
  * with page_count == 0 or PageSlab.
  */
 static int _drbd_no_send_page(struct drbd_conf *mdev, struct page *page,
-                   int offset, size_t size)
+                   int offset, size_t size, unsigned msg_flags)
 {
-        int sent = drbd_send(mdev, mdev->data.socket, kmap(page) + offset, size, 0);
+        int sent = drbd_send(mdev, mdev->data.socket, kmap(page) + offset, size, msg_flags);
         kunmap(page);
         if (sent == size)
                 mdev->send_cnt += size>>9;
@@ -2301,7 +2282,7 @@ static int _drbd_no_send_page(struct drbd_conf *mdev, struct page *page,
 }
 
 static int _drbd_send_page(struct drbd_conf *mdev, struct page *page,
-                    int offset, size_t size)
+                    int offset, size_t size, unsigned msg_flags)
 {
         mm_segment_t oldfs = get_fs();
         int sent, ok;
@@ -2314,14 +2295,15 @@ static int _drbd_send_page(struct drbd_conf *mdev, struct page *page,
          * __page_cache_release a page that would actually still be referenced
          * by someone, leading to some obscure delayed Oops somewhere else. */
         if (disable_sendpage || (page_count(page) < 1) || PageSlab(page))
-                return _drbd_no_send_page(mdev, page, offset, size);
+                return _drbd_no_send_page(mdev, page, offset, size, msg_flags);
 
+        msg_flags |= MSG_NOSIGNAL;
         drbd_update_congested(mdev);
         set_fs(KERNEL_DS);
         do {
                 sent = mdev->data.socket->ops->sendpage(mdev->data.socket, page,
                                                         offset, len,
-                                                        MSG_NOSIGNAL);
+                                                        msg_flags);
                 if (sent == -EAGAIN) {
                         if (we_should_drop_the_connection(mdev,
                                                           mdev->data.socket))
@@ -2350,9 +2332,11 @@ static int _drbd_send_bio(struct drbd_conf *mdev, struct bio *bio)
 {
         struct bio_vec *bvec;
         int i;
+        /* hint all but last page with MSG_MORE */
         __bio_for_each_segment(bvec, bio, i, 0) {
                 if (!_drbd_no_send_page(mdev, bvec->bv_page,
-                                     bvec->bv_offset, bvec->bv_len))
+                                     bvec->bv_offset, bvec->bv_len,
+                                     i == bio->bi_vcnt -1 ? 0 : MSG_MORE))
                         return 0;
         }
         return 1;
@@ -2362,12 +2346,13 @@ static int _drbd_send_zc_bio(struct drbd_conf *mdev, struct bio *bio)
 {
         struct bio_vec *bvec;
         int i;
+        /* hint all but last page with MSG_MORE */
         __bio_for_each_segment(bvec, bio, i, 0) {
                 if (!_drbd_send_page(mdev, bvec->bv_page,
-                                     bvec->bv_offset, bvec->bv_len))
+                                     bvec->bv_offset, bvec->bv_len,
+                                     i == bio->bi_vcnt -1 ? 0 : MSG_MORE))
                         return 0;
         }
-
         return 1;
 }
 
@@ -2375,9 +2360,11 @@ static int _drbd_send_zc_ee(struct drbd_conf *mdev, struct drbd_epoch_entry *e)
 {
         struct page *page = e->pages;
         unsigned len = e->size;
+        /* hint all but last page with MSG_MORE */
         page_chain_for_each(page) {
                 unsigned l = min_t(unsigned, len, PAGE_SIZE);
-                if (!_drbd_send_page(mdev, page, 0, l))
+                if (!_drbd_send_page(mdev, page, 0, l,
+                                page_chain_next(page) ? MSG_MORE : 0))
                         return 0;
                 len -= l;
         }
@@ -2457,11 +2444,11 @@ int drbd_send_dblock(struct drbd_conf *mdev, struct drbd_request *req)
         p.dp_flags = cpu_to_be32(dp_flags);
         set_bit(UNPLUG_REMOTE, &mdev->flags);
         ok = (sizeof(p) ==
-                drbd_send(mdev, mdev->data.socket, &p, sizeof(p), MSG_MORE));
+                drbd_send(mdev, mdev->data.socket, &p, sizeof(p), dgs ? MSG_MORE : 0));
         if (ok && dgs) {
                 dgb = mdev->int_dig_out;
                 drbd_csum_bio(mdev, mdev->integrity_w_tfm, req->master_bio, dgb);
-                ok = drbd_send(mdev, mdev->data.socket, dgb, dgs, MSG_MORE);
+                ok = drbd_send(mdev, mdev->data.socket, dgb, dgs, 0);
         }
         if (ok) {
                 if (mdev->net_conf->wire_protocol == DRBD_PROT_A)
@@ -2510,11 +2497,11 @@ int drbd_send_block(struct drbd_conf *mdev, enum drbd_packets cmd,
                 return 0;
 
         ok = sizeof(p) == drbd_send(mdev, mdev->data.socket, &p,
-                                        sizeof(p), MSG_MORE);
+                                        sizeof(p), dgs ? MSG_MORE : 0);
         if (ok && dgs) {
                 dgb = mdev->int_dig_out;
                 drbd_csum_ee(mdev, mdev->integrity_w_tfm, e, dgb);
-                ok = drbd_send(mdev, mdev->data.socket, dgb, dgs, MSG_MORE);
+                ok = drbd_send(mdev, mdev->data.socket, dgb, dgs, 0);
         }
         if (ok)
                 ok = _drbd_send_zc_ee(mdev, e);
@@ -2708,7 +2695,6 @@ void drbd_init_set_defaults(struct drbd_conf *mdev)
         atomic_set(&mdev->net_cnt, 0);
         atomic_set(&mdev->packet_seq, 0);
         atomic_set(&mdev->pp_in_use, 0);
-        atomic_set(&mdev->new_c_uuid, 0);
 
         mutex_init(&mdev->md_io_mutex);
         mutex_init(&mdev->data.mutex);
@@ -2739,14 +2725,12 @@ void drbd_init_set_defaults(struct drbd_conf *mdev)
         INIT_LIST_HEAD(&mdev->bm_io_work.w.list);
         INIT_LIST_HEAD(&mdev->delay_probes);
         INIT_LIST_HEAD(&mdev->delay_probe_work.list);
-        INIT_LIST_HEAD(&mdev->uuid_work.list);
 
         mdev->resync_work.cb  = w_resync_inactive;
         mdev->unplug_work.cb  = w_send_write_hint;
         mdev->md_sync_work.cb = w_md_sync;
         mdev->bm_io_work.w.cb = w_bitmap_io;
         mdev->delay_probe_work.cb = w_delay_probes;
-        mdev->uuid_work.cb = w_new_current_uuid;
         init_timer(&mdev->resync_timer);
         init_timer(&mdev->md_sync_timer);
         init_timer(&mdev->delay_probe_timer);
@@ -3799,7 +3783,7 @@ _drbd_insert_fault(struct drbd_conf *mdev, unsigned int type)
         if (ret) {
                 fault_count++;
 
-                if (printk_ratelimit())
+                if (__ratelimit(&drbd_ratelimit_state))
                         dev_warn(DEV, "***Simulating %s failure\n",
                                 _drbd_fault_str(type));
         }
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index bc9ab7fb2cc7..dff48701b84d 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -42,7 +42,6 @@
 #include <linux/unistd.h>
 #include <linux/vmalloc.h>
 #include <linux/random.h>
-#include <linux/mm.h>
 #include <linux/string.h>
 #include <linux/scatterlist.h>
 #include "drbd_int.h"
@@ -571,6 +570,25 @@ static int drbd_recv(struct drbd_conf *mdev, void *buf, size_t size)
         return rv;
 }
 
+/* quoting tcp(7):
+ *   On individual connections, the socket buffer size must be set prior to the
+ *   listen(2) or connect(2) calls in order to have it take effect.
+ * This is our wrapper to do so.
+ */
+static void drbd_setbufsize(struct socket *sock, unsigned int snd,
+                unsigned int rcv)
+{
+        /* open coded SO_SNDBUF, SO_RCVBUF */
+        if (snd) {
+                sock->sk->sk_sndbuf = snd;
+                sock->sk->sk_userlocks |= SOCK_SNDBUF_LOCK;
+        }
+        if (rcv) {
+                sock->sk->sk_rcvbuf = rcv;
+                sock->sk->sk_userlocks |= SOCK_RCVBUF_LOCK;
+        }
+}
+
 static struct socket *drbd_try_connect(struct drbd_conf *mdev)
 {
         const char *what;
@@ -592,6 +610,8 @@ static struct socket *drbd_try_connect(struct drbd_conf *mdev)
 
         sock->sk->sk_rcvtimeo =
         sock->sk->sk_sndtimeo =  mdev->net_conf->try_connect_int*HZ;
+        drbd_setbufsize(sock, mdev->net_conf->sndbuf_size,
+                        mdev->net_conf->rcvbuf_size);
 
        /* explicitly bind to the configured IP as source IP
         *  for the outgoing connections.
@@ -670,6 +690,8 @@ static struct socket *drbd_wait_for_connect(struct drbd_conf *mdev)
         s_listen->sk->sk_reuse    = 1; /* SO_REUSEADDR */
         s_listen->sk->sk_rcvtimeo = timeo;
         s_listen->sk->sk_sndtimeo = timeo;
+        drbd_setbufsize(s_listen, mdev->net_conf->sndbuf_size,
+                        mdev->net_conf->rcvbuf_size);
 
         what = "bind before listen";
         err = s_listen->ops->bind(s_listen,
@@ -856,16 +878,6 @@ retry:
         sock->sk->sk_priority = TC_PRIO_INTERACTIVE_BULK;
         msock->sk->sk_priority = TC_PRIO_INTERACTIVE;
 
-        if (mdev->net_conf->sndbuf_size) {
-                sock->sk->sk_sndbuf = mdev->net_conf->sndbuf_size;
-                sock->sk->sk_userlocks |= SOCK_SNDBUF_LOCK;
-        }
-
-        if (mdev->net_conf->rcvbuf_size) {
-                sock->sk->sk_rcvbuf = mdev->net_conf->rcvbuf_size;
-                sock->sk->sk_userlocks |= SOCK_RCVBUF_LOCK;
-        }
-
         /* NOT YET ...
          * sock->sk->sk_sndtimeo = mdev->net_conf->timeout*HZ/10;
          * sock->sk->sk_rcvtimeo = MAX_SCHEDULE_TIMEOUT;
@@ -1154,17 +1166,6 @@ int drbd_submit_ee(struct drbd_conf *mdev, struct drbd_epoch_entry *e,
         unsigned n_bios = 0;
         unsigned nr_pages = (ds + PAGE_SIZE -1) >> PAGE_SHIFT;
 
-        if (atomic_read(&mdev->new_c_uuid)) {
-                if (atomic_add_unless(&mdev->new_c_uuid, -1, 1)) {
-                        drbd_uuid_new_current(mdev);
-                        drbd_md_sync(mdev);
-
-                        atomic_dec(&mdev->new_c_uuid);
-                        wake_up(&mdev->misc_wait);
-                }
-                wait_event(mdev->misc_wait, !atomic_read(&mdev->new_c_uuid));
-        }
-
         /* In most cases, we will only need one bio.  But in case the lower
          * level restrictions happen to be different at this offset on this
          * side than those of the sending peer, we may need to submit the
diff --git a/drivers/block/drbd/drbd_req.c b/drivers/block/drbd/drbd_req.c
index 3397f11d0ba9..654f1ef5cbb0 100644
--- a/drivers/block/drbd/drbd_req.c
+++ b/drivers/block/drbd/drbd_req.c
@@ -102,32 +102,7 @@ static void _req_is_done(struct drbd_conf *mdev, struct drbd_request *req, const
                 }
         }
 
-        /* if it was a local io error, we want to notify our
-         * peer about that, and see if we need to
-         * detach the disk and stuff.
-         * to avoid allocating some special work
-         * struct, reuse the request. */
-
-        /* THINK
-         * why do we do this not when we detect the error,
-         * but delay it until it is "done", i.e. possibly
-         * until the next barrier ack? */
-
-        if (rw == WRITE &&
-            ((s & RQ_LOCAL_MASK) && !(s & RQ_LOCAL_OK))) {
-                if (!(req->w.list.next == LIST_POISON1 ||
-                      list_empty(&req->w.list))) {
-                        /* DEBUG ASSERT only; if this triggers, we
-                         * probably corrupt the worker list here */
-                        dev_err(DEV, "req->w.list.next = %p\n", req->w.list.next);
-                        dev_err(DEV, "req->w.list.prev = %p\n", req->w.list.prev);
-                }
-                req->w.cb = w_io_error;
-                drbd_queue_work(&mdev->data.work, &req->w);
-                /* drbd_req_free() is done in w_io_error */
-        } else {
-                drbd_req_free(req);
-        }
+        drbd_req_free(req);
 }
 
 static void queue_barrier(struct drbd_conf *mdev)
@@ -453,9 +428,6 @@ void __req_mod(struct drbd_request *req, enum drbd_req_event what,
                 req->rq_state |= RQ_LOCAL_COMPLETED;
                 req->rq_state &= ~RQ_LOCAL_PENDING;
 
-                dev_alert(DEV, "Local WRITE failed sec=%llus size=%u\n",
-                      (unsigned long long)req->sector, req->size);
-                /* and now: check how to handle local io error. */
                 __drbd_chk_io_error(mdev, FALSE);
                 _req_may_be_done(req, m);
                 put_ldev(mdev);
@@ -475,22 +447,21 @@ void __req_mod(struct drbd_request *req, enum drbd_req_event what,
                 req->rq_state |= RQ_LOCAL_COMPLETED;
                 req->rq_state &= ~RQ_LOCAL_PENDING;
 
-                dev_alert(DEV, "Local READ failed sec=%llus size=%u\n",
-                      (unsigned long long)req->sector, req->size);
-                /* _req_mod(req,to_be_send); oops, recursion... */
                 D_ASSERT(!(req->rq_state & RQ_NET_MASK));
-                req->rq_state |= RQ_NET_PENDING;
-                inc_ap_pending(mdev);
 
                 __drbd_chk_io_error(mdev, FALSE);
                 put_ldev(mdev);
-                /* NOTE: if we have no connection,
-                 * or know the peer has no good data either,
-                 * then we don't actually need to "queue_for_net_read",
-                 * but we do so anyways, since the drbd_io_error()
-                 * and the potential state change to "Diskless"
-                 * needs to be done from process context */
 
+                /* no point in retrying if there is no good remote data,
+                 * or we have no connection. */
+                if (mdev->state.pdsk != D_UP_TO_DATE) {
+                        _req_may_be_done(req, m);
+                        break;
+                }
+
+                /* _req_mod(req,to_be_send); oops, recursion... */
+                req->rq_state |= RQ_NET_PENDING;
+                inc_ap_pending(mdev);
                 /* fall through: _req_mod(req,queue_for_net_read); */
 
         case queue_for_net_read:
@@ -600,6 +571,9 @@ void __req_mod(struct drbd_request *req, enum drbd_req_event what,
                 _req_may_be_done(req, m);
                 break;
 
+        case read_retry_remote_canceled:
+                req->rq_state &= ~RQ_NET_QUEUED;
+                /* fall through, in case we raced with drbd_disconnect */
         case connection_lost_while_pending:
                 /* transfer log cleanup after connection loss */
                 /* assert something? */
diff --git a/drivers/block/drbd/drbd_req.h b/drivers/block/drbd/drbd_req.h
index 16119d7056cc..02d575d24518 100644
--- a/drivers/block/drbd/drbd_req.h
+++ b/drivers/block/drbd/drbd_req.h
@@ -91,6 +91,7 @@ enum drbd_req_event {
         send_failed,
         handed_over_to_network,
         connection_lost_while_pending,
+        read_retry_remote_canceled,
         recv_acked_by_peer,
         write_acked_by_peer,
         write_acked_by_peer_and_sis, /* and set_in_sync */
diff --git a/drivers/block/drbd/drbd_worker.c b/drivers/block/drbd/drbd_worker.c
index 727ff6339754..b623ceee2a4a 100644
--- a/drivers/block/drbd/drbd_worker.c
+++ b/drivers/block/drbd/drbd_worker.c
@@ -224,9 +224,6 @@ void drbd_endio_pri(struct bio *bio, int error)
         enum drbd_req_event what;
         int uptodate = bio_flagged(bio, BIO_UPTODATE);
 
-        if (error)
-                dev_warn(DEV, "p %s: error=%d\n",
-                         bio_data_dir(bio) == WRITE ? "write" : "read", error);
         if (!error && !uptodate) {
                 dev_warn(DEV, "p %s: setting error to -EIO\n",
                          bio_data_dir(bio) == WRITE ? "write" : "read");
@@ -257,20 +254,6 @@ void drbd_endio_pri(struct bio *bio, int error)
                 complete_master_bio(mdev, &m);
 }
 
-int w_io_error(struct drbd_conf *mdev, struct drbd_work *w, int cancel)
-{
-        struct drbd_request *req = container_of(w, struct drbd_request, w);
-
-        /* NOTE: mdev->ldev can be NULL by the time we get here! */
-        /* D_ASSERT(mdev->ldev->dc.on_io_error != EP_PASS_ON); */
-
-        /* the only way this callback is scheduled is from _req_may_be_done,
-         * when it is done and had a local write error, see comments there */
-        drbd_req_free(req);
-
-        return TRUE;
-}
-
 int w_read_retry_remote(struct drbd_conf *mdev, struct drbd_work *w, int cancel)
 {
         struct drbd_request *req = container_of(w, struct drbd_request, w);
@@ -280,12 +263,9 @@ int w_read_retry_remote(struct drbd_conf *mdev, struct drbd_work *w, int cancel)
          * to give the disk the chance to relocate that block */
 
         spin_lock_irq(&mdev->req_lock);
-        if (cancel ||
-            mdev->state.conn < C_CONNECTED ||
-            mdev->state.pdsk <= D_INCONSISTENT) {
-                _req_mod(req, send_canceled);
+        if (cancel || mdev->state.pdsk != D_UP_TO_DATE) {
+                _req_mod(req, read_retry_remote_canceled);
                 spin_unlock_irq(&mdev->req_lock);
-                dev_alert(DEV, "WE ARE LOST. Local IO failure, no peer.\n");
                 return 1;
         }
         spin_unlock_irq(&mdev->req_lock);
diff --git a/drivers/block/swim3.c b/drivers/block/swim3.c
index 52f2d11bc7b9..ed6fb91123ab 100644
--- a/drivers/block/swim3.c
+++ b/drivers/block/swim3.c
@@ -1159,8 +1159,10 @@ static struct of_device_id swim3_match[] =
 
 static struct macio_driver swim3_driver =
 {
-        .name           = "swim3",
-        .match_table    = swim3_match,
+        .driver = {
+                .name           = "swim3",
+                .of_match_table = swim3_match,
+        },
         .probe          = swim3_attach,
 #if 0
         .suspend        = swim3_suspend,
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index 83fa09a836ca..258bc2ae2885 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -298,7 +298,9 @@ static int __devinit virtblk_probe(struct virtio_device *vdev)
         err = virtio_config_val(vdev, VIRTIO_BLK_F_SEG_MAX,
                                 offsetof(struct virtio_blk_config, seg_max),
                                 &sg_elems);
-        if (err)
+
+        /* We need at least one SG element, whatever they say. */
+        if (err || !sg_elems)
                 sg_elems = 1;
 
         /* We need an extra sg elements at head and tail. */
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index e21175be25d0..7cfcc629a7fd 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -1121,5 +1121,13 @@ config DEVPORT
 
 source "drivers/s390/char/Kconfig"
 
+config RAMOOPS
+        tristate "Log panic/oops to a RAM buffer"
+        depends on HAS_IOMEM
+        default n
+        help
+          This enables panic and oops messages to be logged to a circular
+          buffer in RAM where it can be read back at some later point.
+
 endmenu
 
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index d39be4cf1f5d..88d6eac69754 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -108,6 +108,7 @@ obj-$(CONFIG_HANGCHECK_TIMER)	+= hangcheck-timer.o
 obj-$(CONFIG_TCG_TPM)           += tpm/
 
 obj-$(CONFIG_PS3_FLASH)         += ps3flash.o
+obj-$(CONFIG_RAMOOPS)           += ramoops.o
 
 obj-$(CONFIG_JS_RTC)            += js-rtc.o
 js-rtc-y = rtc.o
diff --git a/drivers/char/agp/amd64-agp.c b/drivers/char/agp/amd64-agp.c
index 67ea3a60de74..70312da4c968 100644
--- a/drivers/char/agp/amd64-agp.c
+++ b/drivers/char/agp/amd64-agp.c
@@ -384,7 +384,7 @@ static int __devinit uli_agp_init(struct pci_dev *pdev)
 {
         u32 httfea,baseaddr,enuscr;
         struct pci_dev *dev1;
-        int i;
+        int i, ret;
         unsigned size = amd64_fetch_size();
 
         dev_info(&pdev->dev, "setting up ULi AGP\n");
@@ -400,15 +400,18 @@ static int __devinit uli_agp_init(struct pci_dev *pdev)
 
         if (i == ARRAY_SIZE(uli_sizes)) {
                 dev_info(&pdev->dev, "no ULi size found for %d\n", size);
-                return -ENODEV;
+                ret = -ENODEV;
+                goto put;
         }
 
         /* shadow x86-64 registers into ULi registers */
         pci_read_config_dword (k8_northbridges[0], AMD64_GARTAPERTUREBASE, &httfea);
 
         /* if x86-64 aperture base is beyond 4G, exit here */
-        if ((httfea & 0x7fff) >> (32 - 25))
-                return -ENODEV;
+        if ((httfea & 0x7fff) >> (32 - 25)) {
+                ret = -ENODEV;
+                goto put;
+        }
 
         httfea = (httfea& 0x7fff) << 25;
 
@@ -420,9 +423,10 @@ static int __devinit uli_agp_init(struct pci_dev *pdev)
         enuscr= httfea+ (size * 1024 * 1024) - 1;
         pci_write_config_dword(dev1, ULI_X86_64_HTT_FEA_REG, httfea);
         pci_write_config_dword(dev1, ULI_X86_64_ENU_SCR_REG, enuscr);
-
+        ret = 0;
+put:
         pci_dev_put(dev1);
-        return 0;
+        return ret;
 }
 
 
@@ -441,7 +445,7 @@ static int nforce3_agp_init(struct pci_dev *pdev)
 {
         u32 tmp, apbase, apbar, aplimit;
         struct pci_dev *dev1;
-        int i;
+        int i, ret;
         unsigned size = amd64_fetch_size();
 
         dev_info(&pdev->dev, "setting up Nforce3 AGP\n");
@@ -458,7 +462,8 @@ static int nforce3_agp_init(struct pci_dev *pdev)
 
         if (i == ARRAY_SIZE(nforce3_sizes)) {
                 dev_info(&pdev->dev, "no NForce3 size found for %d\n", size);
-                return -ENODEV;
+                ret = -ENODEV;
+                goto put;
         }
 
         pci_read_config_dword(dev1, NVIDIA_X86_64_1_APSIZE, &tmp);
@@ -472,7 +477,8 @@ static int nforce3_agp_init(struct pci_dev *pdev)
         /* if x86-64 aperture base is beyond 4G, exit here */
         if ( (apbase & 0x7fff) >> (32 - 25) ) {
                 dev_info(&pdev->dev, "aperture base > 4G\n");
-                return -ENODEV;
+                ret = -ENODEV;
+                goto put;
         }
 
         apbase = (apbase & 0x7fff) << 25;
@@ -488,9 +494,11 @@ static int nforce3_agp_init(struct pci_dev *pdev)
         pci_write_config_dword(dev1, NVIDIA_X86_64_1_APBASE2, apbase);
         pci_write_config_dword(dev1, NVIDIA_X86_64_1_APLIMIT2, aplimit);
 
+        ret = 0;
+put:
         pci_dev_put(dev1);
 
-        return 0;
+        return ret;
 }
 
 static int __devinit agp_amd64_probe(struct pci_dev *pdev,
diff --git a/drivers/char/agp/intel-gtt.c b/drivers/char/agp/intel-gtt.c
index e8ea6825822c..9344216183a4 100644
--- a/drivers/char/agp/intel-gtt.c
+++ b/drivers/char/agp/intel-gtt.c
@@ -1059,7 +1059,7 @@ static void intel_i9xx_setup_flush(void)
         }
 }
 
-static int intel_i915_configure(void)
+static int intel_i9xx_configure(void)
 {
         struct aper_size_info_fixed *current_size;
         u32 temp;
@@ -1207,6 +1207,38 @@ static int intel_i9xx_fetch_size(void)
         return 0;
 }
 
+static int intel_i915_get_gtt_size(void)
+{
+        int size;
+
+        if (IS_G33) {
+                u16 gmch_ctrl;
+
+                /* G33's GTT size defined in gmch_ctrl */
+                pci_read_config_word(agp_bridge->dev, I830_GMCH_CTRL, &gmch_ctrl);
+                switch (gmch_ctrl & G33_PGETBL_SIZE_MASK) {
+                case G33_PGETBL_SIZE_1M:
+                        size = 1024;
+                        break;
+                case G33_PGETBL_SIZE_2M:
+                        size = 2048;
+                        break;
+                default:
+                        dev_info(&agp_bridge->dev->dev,
+                                 "unknown page table size 0x%x, assuming 512KB\n",
+                                (gmch_ctrl & G33_PGETBL_SIZE_MASK));
+                        size = 512;
+                }
+        } else {
+                /* On previous hardware, the GTT size was just what was
+                 * required to map the aperture.
+                 */
+                size = agp_bridge->driver->fetch_size();
+        }
+
+        return KB(size);
+}
+
 /* The intel i915 automatically initializes the agp aperture during POST.
  * Use the memory already set aside for in the GTT.
  */
@@ -1216,7 +1248,7 @@ static int intel_i915_create_gatt_table(struct agp_bridge_data *bridge)
         struct aper_size_info_fixed *size;
         int num_entries;
         u32 temp, temp2;
-        int gtt_map_size = 256 * 1024;
+        int gtt_map_size;
 
         size = agp_bridge->current_size;
         page_order = size->page_order;
@@ -1226,8 +1258,8 @@ static int intel_i915_create_gatt_table(struct agp_bridge_data *bridge)
         pci_read_config_dword(intel_private.pcidev, I915_MMADDR, &temp);
         pci_read_config_dword(intel_private.pcidev, I915_PTEADDR, &temp2);
 
-        if (IS_G33)
-            gtt_map_size = 1024 * 1024; /* 1M on G33 */
+        gtt_map_size = intel_i915_get_gtt_size();
+
         intel_private.gtt = ioremap(temp2, gtt_map_size);
         if (!intel_private.gtt)
                 return -ENOMEM;
@@ -1422,7 +1454,7 @@ static const struct agp_bridge_driver intel_915_driver = {
         .size_type              = FIXED_APER_SIZE,
         .num_aperture_sizes     = 4,
         .needs_scratch_page     = true,
-        .configure              = intel_i915_configure,
+        .configure              = intel_i9xx_configure,
         .fetch_size             = intel_i9xx_fetch_size,
         .cleanup                = intel_i915_cleanup,
         .mask_memory            = intel_i810_mask_memory,
@@ -1455,7 +1487,7 @@ static const struct agp_bridge_driver intel_i965_driver = {
         .size_type              = FIXED_APER_SIZE,
         .num_aperture_sizes     = 4,
         .needs_scratch_page     = true,
-        .configure              = intel_i915_configure,
+        .configure              = intel_i9xx_configure,
         .fetch_size             = intel_i9xx_fetch_size,
         .cleanup                = intel_i915_cleanup,
         .mask_memory            = intel_i965_mask_memory,
@@ -1488,7 +1520,7 @@ static const struct agp_bridge_driver intel_g33_driver = {
         .size_type              = FIXED_APER_SIZE,
         .num_aperture_sizes     = 4,
         .needs_scratch_page     = true,
-        .configure              = intel_i915_configure,
+        .configure              = intel_i9xx_configure,
         .fetch_size             = intel_i9xx_fetch_size,
         .cleanup                = intel_i915_cleanup,
         .mask_memory            = intel_i965_mask_memory,
diff --git a/drivers/char/agp/uninorth-agp.c b/drivers/char/agp/uninorth-agp.c
index 95db71360d24..f845a8f718b3 100644
--- a/drivers/char/agp/uninorth-agp.c
+++ b/drivers/char/agp/uninorth-agp.c
@@ -415,7 +415,7 @@ static int uninorth_create_gatt_table(struct agp_bridge_data *bridge)
         bridge->gatt_table_real = (u32 *) table;
         /* Need to clear out any dirty data still sitting in caches */
         flush_dcache_range((unsigned long)table,
-                           (unsigned long)(table_end + PAGE_SIZE));
+                           (unsigned long)table_end + 1);
         bridge->gatt_table = vmap(pages, (1 << page_order), 0, PAGE_KERNEL_NCG);
 
         if (bridge->gatt_table == NULL)
diff --git a/drivers/char/amiserial.c b/drivers/char/amiserial.c
index 56b27671adc4..4f8d60c25a98 100644
--- a/drivers/char/amiserial.c
+++ b/drivers/char/amiserial.c
@@ -84,6 +84,7 @@ static char *serial_version = "4.30";
 #include <linux/smp_lock.h>
 #include <linux/init.h>
 #include <linux/bitops.h>
+#include <linux/platform_device.h>
 
 #include <asm/setup.h>
 
@@ -1954,29 +1955,16 @@ static const struct tty_operations serial_ops = {
 /*
  * The serial driver boot-time initialization code!
  */
-static int __init rs_init(void)
+static int __init amiga_serial_probe(struct platform_device *pdev)
 {
         unsigned long flags;
         struct serial_state * state;
         int error;
 
-        if (!MACH_IS_AMIGA || !AMIGAHW_PRESENT(AMI_SERIAL))
-                return -ENODEV;
-
         serial_driver = alloc_tty_driver(1);
         if (!serial_driver)
                 return -ENOMEM;
 
-        /*
-         *  We request SERDAT and SERPER only, because the serial registers are
-         *  too spreaded over the custom register space
-         */
-        if (!request_mem_region(CUSTOM_PHYSADDR+0x30, 4,
-                                "amiserial [Paula]")) {
-                error = -EBUSY;
-                goto fail_put_tty_driver;
-        }
-
         IRQ_ports = NULL;
 
         show_serial_version();
@@ -1998,7 +1986,7 @@ static int __init rs_init(void)
 
         error = tty_register_driver(serial_driver);
         if (error)
-                goto fail_release_mem_region;
+                goto fail_put_tty_driver;
 
         state = rs_table;
         state->magic = SSTATE_MAGIC;
@@ -2050,23 +2038,24 @@ static int __init rs_init(void)
         ciab.ddra |= (SER_DTR | SER_RTS);   /* outputs */
         ciab.ddra &= ~(SER_DCD | SER_CTS | SER_DSR);  /* inputs */
 
+        platform_set_drvdata(pdev, state);
+
         return 0;
 
 fail_free_irq:
         free_irq(IRQ_AMIGA_TBE, state);
 fail_unregister:
         tty_unregister_driver(serial_driver);
-fail_release_mem_region:
-        release_mem_region(CUSTOM_PHYSADDR+0x30, 4);
 fail_put_tty_driver:
         put_tty_driver(serial_driver);
         return error;
 }
 
-static __exit void rs_exit(void) 
+static int __exit amiga_serial_remove(struct platform_device *pdev)
 {
         int error;
-        struct async_struct *info = rs_table[0].info;
+        struct serial_state *state = platform_get_drvdata(pdev);
+        struct async_struct *info = state->info;
 
         /* printk("Unloading %s: version %s\n", serial_name, serial_version); */
         tasklet_kill(&info->tlet);
@@ -2075,19 +2064,38 @@ static __exit void rs_exit(void)
                        error);
         put_tty_driver(serial_driver);
 
-        if (info) {
-          rs_table[0].info = NULL;
-          kfree(info);
-        }
+        rs_table[0].info = NULL;
+        kfree(info);
 
         free_irq(IRQ_AMIGA_TBE, rs_table);
         free_irq(IRQ_AMIGA_RBF, rs_table);
 
-        release_mem_region(CUSTOM_PHYSADDR+0x30, 4);
+        platform_set_drvdata(pdev, NULL);
+
+        return error;
+}
+
+static struct platform_driver amiga_serial_driver = {
+        .remove = __exit_p(amiga_serial_remove),
+        .driver   = {
+                .name   = "amiga-serial",
+                .owner  = THIS_MODULE,
+        },
+};
+
+static int __init amiga_serial_init(void)
+{
+        return platform_driver_probe(&amiga_serial_driver, amiga_serial_probe);
+}
+
+module_init(amiga_serial_init);
+
+static void __exit amiga_serial_exit(void)
+{
+        platform_driver_unregister(&amiga_serial_driver);
 }
 
-module_init(rs_init)
-module_exit(rs_exit)
+module_exit(amiga_serial_exit);
 
 
 #if defined(CONFIG_SERIAL_CONSOLE) && !defined(MODULE)
@@ -2154,3 +2162,4 @@ console_initcall(amiserial_console_init);
 #endif /* CONFIG_SERIAL_CONSOLE && !MODULE */
 
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:amiga-serial");
diff --git a/drivers/char/applicom.c b/drivers/char/applicom.c
index 63313a33ba5f..f4ae0e0fb631 100644
--- a/drivers/char/applicom.c
+++ b/drivers/char/applicom.c
@@ -703,14 +703,9 @@ static long ac_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
         /* In general, the device is only openable by root anyway, so we're not
            particularly concerned that bogus ioctls can flood the console. */
 
-        adgl = kmalloc(sizeof(struct st_ram_io), GFP_KERNEL);
-        if (!adgl)
-                return -ENOMEM;
-
-        if (copy_from_user(adgl, argp, sizeof(struct st_ram_io))) {
-                kfree(adgl);
-                return -EFAULT;
-        }
+        adgl = memdup_user(argp, sizeof(struct st_ram_io));
+        if (IS_ERR(adgl))
+                return PTR_ERR(adgl);
 
         lock_kernel();  
         IndexCard = adgl->num_card-1;
diff --git a/drivers/char/ipmi/ipmi_msghandler.c b/drivers/char/ipmi/ipmi_msghandler.c
index c6ad4234378d..4f3f8c9ec262 100644
--- a/drivers/char/ipmi/ipmi_msghandler.c
+++ b/drivers/char/ipmi/ipmi_msghandler.c
@@ -2505,12 +2505,11 @@ static int ipmi_bmc_register(ipmi_smi_t intf, int ifnum,
                         return rv;
                 }
 
-                printk(KERN_INFO
-                       "ipmi: Found new BMC (man_id: 0x%6.6x, "
-                       " prod_id: 0x%4.4x, dev_id: 0x%2.2x)\n",
-                       bmc->id.manufacturer_id,
-                       bmc->id.product_id,
-                       bmc->id.device_id);
+                dev_info(intf->si_dev, "Found new BMC (man_id: 0x%6.6x, "
+                         "prod_id: 0x%4.4x, dev_id: 0x%2.2x)\n",
+                         bmc->id.manufacturer_id,
+                         bmc->id.product_id,
+                         bmc->id.device_id);
         }
 
         /*
@@ -4037,8 +4036,8 @@ static void ipmi_request_event(void)
 
 static struct timer_list ipmi_timer;
 
-/* Call every ~100 ms. */
-#define IPMI_TIMEOUT_TIME       100
+/* Call every ~1000 ms. */
+#define IPMI_TIMEOUT_TIME       1000
 
 /* How many jiffies does it take to get to the timeout time. */
 #define IPMI_TIMEOUT_JIFFIES    ((IPMI_TIMEOUT_TIME * HZ) / 1000)
diff --git a/drivers/char/ipmi/ipmi_si_intf.c b/drivers/char/ipmi/ipmi_si_intf.c
index 47ffe4a90a95..35603dd4e6c5 100644
--- a/drivers/char/ipmi/ipmi_si_intf.c
+++ b/drivers/char/ipmi/ipmi_si_intf.c
@@ -107,6 +107,14 @@ enum si_type {
 };
 static char *si_to_str[] = { "kcs", "smic", "bt" };
 
+enum ipmi_addr_src {
+        SI_INVALID = 0, SI_HOTMOD, SI_HARDCODED, SI_SPMI, SI_ACPI, SI_SMBIOS,
+        SI_PCI, SI_DEVICETREE, SI_DEFAULT
+};
+static char *ipmi_addr_src_to_str[] = { NULL, "hotmod", "hardcoded", "SPMI",
+                                        "ACPI", "SMBIOS", "PCI",
+                                        "device-tree", "default" };
+
 #define DEVICE_NAME "ipmi_si"
 
 static struct platform_driver ipmi_driver = {
@@ -188,7 +196,7 @@ struct smi_info {
         int (*irq_setup)(struct smi_info *info);
         void (*irq_cleanup)(struct smi_info *info);
         unsigned int io_size;
-        char *addr_source; /* ACPI, PCI, SMBIOS, hardcode, default. */
+        enum ipmi_addr_src addr_source; /* ACPI, PCI, SMBIOS, hardcode, etc. */
         void (*addr_source_cleanup)(struct smi_info *info);
         void *addr_source_data;
 
@@ -300,6 +308,7 @@ static int num_max_busy_us;
 
 static int unload_when_empty = 1;
 
+static int add_smi(struct smi_info *smi);
 static int try_smi_init(struct smi_info *smi);
 static void cleanup_one_si(struct smi_info *to_clean);
 
@@ -314,9 +323,14 @@ static void deliver_recv_msg(struct smi_info *smi_info,
 {
         /* Deliver the message to the upper layer with the lock
            released. */
-        spin_unlock(&(smi_info->si_lock));
-        ipmi_smi_msg_received(smi_info->intf, msg);
-        spin_lock(&(smi_info->si_lock));
+
+        if (smi_info->run_to_completion) {
+                ipmi_smi_msg_received(smi_info->intf, msg);
+        } else {
+                spin_unlock(&(smi_info->si_lock));
+                ipmi_smi_msg_received(smi_info->intf, msg);
+                spin_lock(&(smi_info->si_lock));
+        }
 }
 
 static void return_hosed_msg(struct smi_info *smi_info, int cCode)
@@ -445,6 +459,9 @@ static inline void disable_si_irq(struct smi_info *smi_info)
         if ((smi_info->irq) && (!smi_info->interrupt_disabled)) {
                 start_disable_irq(smi_info);
                 smi_info->interrupt_disabled = 1;
+                if (!atomic_read(&smi_info->stop_operation))
+                        mod_timer(&smi_info->si_timer,
+                                  jiffies + SI_TIMEOUT_JIFFIES);
         }
 }
 
@@ -576,9 +593,8 @@ static void handle_transaction_done(struct smi_info *smi_info)
                 smi_info->handlers->get_result(smi_info->si_sm, msg, 3);
                 if (msg[2] != 0) {
                         /* Error clearing flags */
-                        printk(KERN_WARNING
-                               "ipmi_si: Error clearing flags: %2.2x\n",
-                               msg[2]);
+                        dev_warn(smi_info->dev,
+                                 "Error clearing flags: %2.2x\n", msg[2]);
                 }
                 if (smi_info->si_state == SI_CLEARING_FLAGS_THEN_SET_IRQ)
                         start_enable_irq(smi_info);
@@ -670,9 +686,8 @@ static void handle_transaction_done(struct smi_info *smi_info)
                 /* We got the flags from the SMI, now handle them. */
                 smi_info->handlers->get_result(smi_info->si_sm, msg, 4);
                 if (msg[2] != 0) {
-                        printk(KERN_WARNING
-                               "ipmi_si: Could not enable interrupts"
-                               ", failed get, using polled mode.\n");
+                        dev_warn(smi_info->dev, "Could not enable interrupts"
+                                 ", failed get, using polled mode.\n");
                         smi_info->si_state = SI_NORMAL;
                 } else {
                         msg[0] = (IPMI_NETFN_APP_REQUEST << 2);
@@ -693,11 +708,11 @@ static void handle_transaction_done(struct smi_info *smi_info)
 
                 /* We got the flags from the SMI, now handle them. */
                 smi_info->handlers->get_result(smi_info->si_sm, msg, 4);
-                if (msg[2] != 0) {
-                        printk(KERN_WARNING
-                               "ipmi_si: Could not enable interrupts"
-                               ", failed set, using polled mode.\n");
-                }
+                if (msg[2] != 0)
+                        dev_warn(smi_info->dev, "Could not enable interrupts"
+                                 ", failed set, using polled mode.\n");
+                else
+                        smi_info->interrupt_disabled = 0;
                 smi_info->si_state = SI_NORMAL;
                 break;
         }
@@ -709,9 +724,8 @@ static void handle_transaction_done(struct smi_info *smi_info)
                 /* We got the flags from the SMI, now handle them. */
                 smi_info->handlers->get_result(smi_info->si_sm, msg, 4);
                 if (msg[2] != 0) {
-                        printk(KERN_WARNING
-                               "ipmi_si: Could not disable interrupts"
-                               ", failed get.\n");
+                        dev_warn(smi_info->dev, "Could not disable interrupts"
+                                 ", failed get.\n");
                         smi_info->si_state = SI_NORMAL;
                 } else {
                         msg[0] = (IPMI_NETFN_APP_REQUEST << 2);
@@ -733,9 +747,8 @@ static void handle_transaction_done(struct smi_info *smi_info)
                 /* We got the flags from the SMI, now handle them. */
                 smi_info->handlers->get_result(smi_info->si_sm, msg, 4);
                 if (msg[2] != 0) {
-                        printk(KERN_WARNING
-                               "ipmi_si: Could not disable interrupts"
-                               ", failed set.\n");
+                        dev_warn(smi_info->dev, "Could not disable interrupts"
+                                 ", failed set.\n");
                 }
                 smi_info->si_state = SI_NORMAL;
                 break;
@@ -877,6 +890,11 @@ static void sender(void                *send_info,
         printk("**Enqueue: %d.%9.9d\n", t.tv_sec, t.tv_usec);
 #endif
 
+        mod_timer(&smi_info->si_timer, jiffies + SI_TIMEOUT_JIFFIES);
+
+        if (smi_info->thread)
+                wake_up_process(smi_info->thread);
+
         if (smi_info->run_to_completion) {
                 /*
                  * If we are running to completion, then throw it in
@@ -997,6 +1015,8 @@ static int ipmi_thread(void *data)
                         ; /* do nothing */
                 else if (smi_result == SI_SM_CALL_WITH_DELAY && busy_wait)
                         schedule();
+                else if (smi_result == SI_SM_IDLE)
+                        schedule_timeout_interruptible(100);
                 else
                         schedule_timeout_interruptible(0);
         }
@@ -1039,6 +1059,7 @@ static void smi_timeout(unsigned long data)
         unsigned long     flags;
         unsigned long     jiffies_now;
         long              time_diff;
+        long              timeout;
 #ifdef DEBUG_TIMING
         struct timeval    t;
 #endif
@@ -1059,9 +1080,9 @@ static void smi_timeout(unsigned long data)
 
         if ((smi_info->irq) && (!smi_info->interrupt_disabled)) {
                 /* Running with interrupts, only do long timeouts. */
-                smi_info->si_timer.expires = jiffies + SI_TIMEOUT_JIFFIES;
+                timeout = jiffies + SI_TIMEOUT_JIFFIES;
                 smi_inc_stat(smi_info, long_timeouts);
-                goto do_add_timer;
+                goto do_mod_timer;
         }
 
         /*
@@ -1070,14 +1091,15 @@ static void smi_timeout(unsigned long data)
          */
         if (smi_result == SI_SM_CALL_WITH_DELAY) {
                 smi_inc_stat(smi_info, short_timeouts);
-                smi_info->si_timer.expires = jiffies + 1;
+                timeout = jiffies + 1;
         } else {
                 smi_inc_stat(smi_info, long_timeouts);
-                smi_info->si_timer.expires = jiffies + SI_TIMEOUT_JIFFIES;
+                timeout = jiffies + SI_TIMEOUT_JIFFIES;
         }
 
- do_add_timer:
-        add_timer(&(smi_info->si_timer));
+ do_mod_timer:
+        if (smi_result != SI_SM_IDLE)
+                mod_timer(&(smi_info->si_timer), timeout);
 }
 
 static irqreturn_t si_irq_handler(int irq, void *data)
@@ -1144,10 +1166,10 @@ static int smi_start_processing(void       *send_info,
                 new_smi->thread = kthread_run(ipmi_thread, new_smi,
                                               "kipmi%d", new_smi->intf_num);
                 if (IS_ERR(new_smi->thread)) {
-                        printk(KERN_NOTICE "ipmi_si_intf: Could not start"
-                               " kernel thread due to error %ld, only using"
-                               " timers to drive the interface\n",
-                               PTR_ERR(new_smi->thread));
+                        dev_notice(new_smi->dev, "Could not start"
+                                   " kernel thread due to error %ld, only using"
+                                   " timers to drive the interface\n",
+                                   PTR_ERR(new_smi->thread));
                         new_smi->thread = NULL;
                 }
         }
@@ -1308,14 +1330,13 @@ static int std_irq_setup(struct smi_info *info)
                                  DEVICE_NAME,
                                  info);
         if (rv) {
-                printk(KERN_WARNING
-                       "ipmi_si: %s unable to claim interrupt %d,"
-                       " running polled\n",
-                       DEVICE_NAME, info->irq);
+                dev_warn(info->dev, "%s unable to claim interrupt %d,"
+                         " running polled\n",
+                         DEVICE_NAME, info->irq);
                 info->irq = 0;
         } else {
                 info->irq_cleanup = std_irq_cleanup;
-                printk("  Using irq %d\n", info->irq);
+                dev_info(info->dev, "Using irq %d\n", info->irq);
         }
 
         return rv;
@@ -1406,8 +1427,8 @@ static int port_setup(struct smi_info *info)
                 info->io.outputb = port_outl;
                 break;
         default:
-                printk(KERN_WARNING "ipmi_si: Invalid register size: %d\n",
-                       info->io.regsize);
+                dev_warn(info->dev, "Invalid register size: %d\n",
+                         info->io.regsize);
                 return -EINVAL;
         }
 
@@ -1529,8 +1550,8 @@ static int mem_setup(struct smi_info *info)
                 break;
 #endif
         default:
-                printk(KERN_WARNING "ipmi_si: Invalid register size: %d\n",
-                       info->io.regsize);
+                dev_warn(info->dev, "Invalid register size: %d\n",
+                         info->io.regsize);
                 return -EINVAL;
         }
 
@@ -1755,7 +1776,7 @@ static int hotmod_handler(const char *val, struct kernel_param *kp)
                                 goto out;
                         }
 
-                        info->addr_source = "hotmod";
+                        info->addr_source = SI_HOTMOD;
                         info->si_type = si_type;
                         info->io.addr_data = addr;
                         info->io.addr_type = addr_space;
@@ -1777,7 +1798,9 @@ static int hotmod_handler(const char *val, struct kernel_param *kp)
                                 info->irq_setup = std_irq_setup;
                         info->slave_addr = ipmb;
 
-                        try_smi_init(info);
+                        if (!add_smi(info))
+                                if (try_smi_init(info))
+                                        cleanup_one_si(info);
                 } else {
                         /* remove */
                         struct smi_info *e, *tmp_e;
@@ -1813,7 +1836,8 @@ static __devinit void hardcode_find_bmc(void)
                 if (!info)
                         return;
 
-                info->addr_source = "hardcoded";
+                info->addr_source = SI_HARDCODED;
+                printk(KERN_INFO PFX "probing via hardcoded address\n");
 
                 if (!si_type[i] || strcmp(si_type[i], "kcs") == 0) {
                         info->si_type = SI_KCS;
@@ -1822,8 +1846,7 @@ static __devinit void hardcode_find_bmc(void)
                 } else if (strcmp(si_type[i], "bt") == 0) {
                         info->si_type = SI_BT;
                 } else {
-                        printk(KERN_WARNING
-                               "ipmi_si: Interface type specified "
+                        printk(KERN_WARNING PFX "Interface type specified "
                                "for interface %d, was invalid: %s\n",
                                i, si_type[i]);
                         kfree(info);
@@ -1841,11 +1864,9 @@ static __devinit void hardcode_find_bmc(void)
                         info->io.addr_data = addrs[i];
                         info->io.addr_type = IPMI_MEM_ADDR_SPACE;
                 } else {
-                        printk(KERN_WARNING
-                               "ipmi_si: Interface type specified "
-                               "for interface %d, "
-                               "but port and address were not set or "
-                               "set to zero.\n", i);
+                        printk(KERN_WARNING PFX "Interface type specified "
+                               "for interface %d, but port and address were "
+                               "not set or set to zero.\n", i);
                         kfree(info);
                         continue;
                 }
@@ -1863,7 +1884,9 @@ static __devinit void hardcode_find_bmc(void)
                         info->irq_setup = std_irq_setup;
                 info->slave_addr = slave_addrs[i];
 
-                try_smi_init(info);
+                if (!add_smi(info))
+                        if (try_smi_init(info))
+                                cleanup_one_si(info);
         }
 }
 
@@ -1923,15 +1946,13 @@ static int acpi_gpe_irq_setup(struct smi_info *info)
                                           &ipmi_acpi_gpe,
                                           info);
         if (status != AE_OK) {
-                printk(KERN_WARNING
-                       "ipmi_si: %s unable to claim ACPI GPE %d,"
-                       " running polled\n",
-                       DEVICE_NAME, info->irq);
+                dev_warn(info->dev, "%s unable to claim ACPI GPE %d,"
+                         " running polled\n", DEVICE_NAME, info->irq);
                 info->irq = 0;
                 return -EINVAL;
         } else {
                 info->irq_cleanup = acpi_gpe_irq_cleanup;
-                printk("  Using ACPI GPE %d\n", info->irq);
+                dev_info(info->dev, "Using ACPI GPE %d\n", info->irq);
                 return 0;
         }
 }
@@ -1989,8 +2010,8 @@ static __devinit int try_init_spmi(struct SPMITable *spmi)
         u8               addr_space;
 
         if (spmi->IPMIlegacy != 1) {
-            printk(KERN_INFO "IPMI: Bad SPMI legacy %d\n", spmi->IPMIlegacy);
-            return -ENODEV;
+                printk(KERN_INFO PFX "Bad SPMI legacy %d\n", spmi->IPMIlegacy);
+                return -ENODEV;
         }
 
         if (spmi->addr.space_id == ACPI_ADR_SPACE_SYSTEM_MEMORY)
@@ -2000,11 +2021,12 @@ static __devinit int try_init_spmi(struct SPMITable *spmi)
 
         info = kzalloc(sizeof(*info), GFP_KERNEL);
         if (!info) {
-                printk(KERN_ERR "ipmi_si: Could not allocate SI data (3)\n");
+                printk(KERN_ERR PFX "Could not allocate SI data (3)\n");
                 return -ENOMEM;
         }
 
-        info->addr_source = "SPMI";
+        info->addr_source = SI_SPMI;
+        printk(KERN_INFO PFX "probing via SPMI\n");
 
         /* Figure out the interface type. */
         switch (spmi->InterfaceType) {
@@ -2018,8 +2040,8 @@ static __devinit int try_init_spmi(struct SPMITable *spmi)
                 info->si_type = SI_BT;
                 break;
         default:
-                printk(KERN_INFO "ipmi_si: Unknown ACPI/SPMI SI type %d\n",
-                        spmi->InterfaceType);
+                printk(KERN_INFO PFX "Unknown ACPI/SPMI SI type %d\n",
+                       spmi->InterfaceType);
                 kfree(info);
                 return -EIO;
         }
@@ -2055,13 +2077,12 @@ static __devinit int try_init_spmi(struct SPMITable *spmi)
                 info->io.addr_type = IPMI_IO_ADDR_SPACE;
         } else {
                 kfree(info);
-                printk(KERN_WARNING
-                       "ipmi_si: Unknown ACPI I/O Address type\n");
+                printk(KERN_WARNING PFX "Unknown ACPI I/O Address type\n");
                 return -EIO;
         }
         info->io.addr_data = spmi->addr.address;
 
-        try_smi_init(info);
+        add_smi(info);
 
         return 0;
 }
@@ -2093,6 +2114,7 @@ static int __devinit ipmi_pnp_probe(struct pnp_dev *dev,
 {
         struct acpi_device *acpi_dev;
         struct smi_info *info;
+        struct resource *res;
         acpi_handle handle;
         acpi_status status;
         unsigned long long tmp;
@@ -2105,7 +2127,8 @@ static int __devinit ipmi_pnp_probe(struct pnp_dev *dev,
         if (!info)
                 return -ENOMEM;
 
-        info->addr_source = "ACPI";
+        info->addr_source = SI_ACPI;
+        printk(KERN_INFO PFX "probing via ACPI\n");
 
         handle = acpi_dev->handle;
 
@@ -2125,22 +2148,26 @@ static int __devinit ipmi_pnp_probe(struct pnp_dev *dev,
                 info->si_type = SI_BT;
                 break;
         default:
-                dev_info(&dev->dev, "unknown interface type %lld\n", tmp);
+                dev_info(&dev->dev, "unknown IPMI type %lld\n", tmp);
                 goto err_free;
         }
 
-        if (pnp_port_valid(dev, 0)) {
+        res = pnp_get_resource(dev, IORESOURCE_IO, 0);
+        if (res) {
                 info->io_setup = port_setup;
                 info->io.addr_type = IPMI_IO_ADDR_SPACE;
-                info->io.addr_data = pnp_port_start(dev, 0);
-        } else if (pnp_mem_valid(dev, 0)) {
-                info->io_setup = mem_setup;
-                info->io.addr_type = IPMI_MEM_ADDR_SPACE;
-                info->io.addr_data = pnp_mem_start(dev, 0);
         } else {
+                res = pnp_get_resource(dev, IORESOURCE_MEM, 0);
+                if (res) {
+                        info->io_setup = mem_setup;
+                        info->io.addr_type = IPMI_MEM_ADDR_SPACE;
+                }
+        }
+        if (!res) {
                 dev_err(&dev->dev, "no I/O or memory address\n");
                 goto err_free;
         }
+        info->io.addr_data = res->start;
 
         info->io.regspacing = DEFAULT_REGSPACING;
         info->io.regsize = DEFAULT_REGSPACING;
@@ -2156,10 +2183,14 @@ static int __devinit ipmi_pnp_probe(struct pnp_dev *dev,
                 info->irq_setup = std_irq_setup;
         }
 
-        info->dev = &acpi_dev->dev;
+        info->dev = &dev->dev;
         pnp_set_drvdata(dev, info);
 
-        return try_smi_init(info);
+        dev_info(info->dev, "%pR regsize %d spacing %d irq %d\n",
+                 res, info->io.regsize, info->io.regspacing,
+                 info->irq);
+
+        return add_smi(info);
 
 err_free:
         kfree(info);
@@ -2264,12 +2295,12 @@ static __devinit void try_init_dmi(struct dmi_ipmi_data *ipmi_data)
 
         info = kzalloc(sizeof(*info), GFP_KERNEL);
         if (!info) {
-                printk(KERN_ERR
-                       "ipmi_si: Could not allocate SI data\n");
+                printk(KERN_ERR PFX "Could not allocate SI data\n");
                 return;
         }
 
-        info->addr_source = "SMBIOS";
+        info->addr_source = SI_SMBIOS;
+        printk(KERN_INFO PFX "probing via SMBIOS\n");
 
         switch (ipmi_data->type) {
         case 0x01: /* KCS */
@@ -2299,8 +2330,7 @@ static __devinit void try_init_dmi(struct dmi_ipmi_data *ipmi_data)
 
         default:
                 kfree(info);
-                printk(KERN_WARNING
-                       "ipmi_si: Unknown SMBIOS I/O Address type: %d.\n",
+                printk(KERN_WARNING PFX "Unknown SMBIOS I/O Address type: %d\n",
                        ipmi_data->addr_space);
                 return;
         }
@@ -2318,7 +2348,7 @@ static __devinit void try_init_dmi(struct dmi_ipmi_data *ipmi_data)
         if (info->irq)
                 info->irq_setup = std_irq_setup;
 
-        try_smi_init(info);
+        add_smi(info);
 }
 
 static void __devinit dmi_find_bmc(void)
@@ -2368,7 +2398,8 @@ static int __devinit ipmi_pci_probe(struct pci_dev *pdev,
         if (!info)
                 return -ENOMEM;
 
-        info->addr_source = "PCI";
+        info->addr_source = SI_PCI;
+        dev_info(&pdev->dev, "probing via PCI");
 
         switch (class_type) {
         case PCI_ERMC_CLASSCODE_TYPE_SMIC:
@@ -2385,15 +2416,13 @@ static int __devinit ipmi_pci_probe(struct pci_dev *pdev,
 
         default:
                 kfree(info);
-                printk(KERN_INFO "ipmi_si: %s: Unknown IPMI type: %d\n",
-                       pci_name(pdev), class_type);
+                dev_info(&pdev->dev, "Unknown IPMI type: %d\n", class_type);
                 return -ENOMEM;
         }
 
         rv = pci_enable_device(pdev);
         if (rv) {
-                printk(KERN_ERR "ipmi_si: %s: couldn't enable PCI device\n",
-                       pci_name(pdev));
+                dev_err(&pdev->dev, "couldn't enable PCI device\n");
                 kfree(info);
                 return rv;
         }
@@ -2421,7 +2450,11 @@ static int __devinit ipmi_pci_probe(struct pci_dev *pdev,
         info->dev = &pdev->dev;
         pci_set_drvdata(pdev, info);
 
-        return try_smi_init(info);
+        dev_info(&pdev->dev, "%pR regsize %d spacing %d irq %d\n",
+                &pdev->resource[0], info->io.regsize, info->io.regspacing,
+                info->irq);
+
+        return add_smi(info);
 }
 
 static void __devexit ipmi_pci_remove(struct pci_dev *pdev)
@@ -2473,7 +2506,7 @@ static int __devinit ipmi_of_probe(struct of_device *dev,
         int ret;
         int proplen;
 
-        dev_info(&dev->dev, PFX "probing via device tree\n");
+        dev_info(&dev->dev, "probing via device tree\n");
 
         ret = of_address_to_resource(np, 0, &resource);
         if (ret) {
@@ -2503,12 +2536,12 @@ static int __devinit ipmi_of_probe(struct of_device *dev,
 
         if (!info) {
                 dev_err(&dev->dev,
-                        PFX "could not allocate memory for OF probe\n");
+                        "could not allocate memory for OF probe\n");
                 return -ENOMEM;
         }
 
         info->si_type           = (enum si_type) match->data;
-        info->addr_source       = "device-tree";
+        info->addr_source       = SI_DEVICETREE;
         info->irq_setup         = std_irq_setup;
 
         if (resource.flags & IORESOURCE_IO) {
@@ -2528,13 +2561,13 @@ static int __devinit ipmi_of_probe(struct of_device *dev,
         info->irq               = irq_of_parse_and_map(dev->dev.of_node, 0);
         info->dev               = &dev->dev;
 
-        dev_dbg(&dev->dev, "addr 0x%lx regsize %d spacing %d irq %x\n",
+        dev_dbg(&dev->dev, "addr 0x%lx regsize %d spacing %d irq %d\n",
                 info->io.addr_data, info->io.regsize, info->io.regspacing,
                 info->irq);
 
         dev_set_drvdata(&dev->dev, info);
 
-        return try_smi_init(info);
+        return add_smi(info);
 }
 
 static int __devexit ipmi_of_remove(struct of_device *dev)
@@ -2643,9 +2676,8 @@ static int try_enable_event_buffer(struct smi_info *smi_info)
 
         rv = wait_for_msg_done(smi_info);
         if (rv) {
-                printk(KERN_WARNING
-                       "ipmi_si: Error getting response from get global,"
-                       " enables command, the event buffer is not"
+                printk(KERN_WARNING PFX "Error getting response from get"
+                       " global enables command, the event buffer is not"
                        " enabled.\n");
                 goto out;
         }
@@ -2657,10 +2689,8 @@ static int try_enable_event_buffer(struct smi_info *smi_info)
                         resp[0] != (IPMI_NETFN_APP_REQUEST | 1) << 2 ||
                         resp[1] != IPMI_GET_BMC_GLOBAL_ENABLES_CMD   ||
                         resp[2] != 0) {
-                printk(KERN_WARNING
-                       "ipmi_si: Invalid return from get global"
-                       " enables command, cannot enable the event"
-                       " buffer.\n");
+                printk(KERN_WARNING PFX "Invalid return from get global"
+                       " enables command, cannot enable the event buffer.\n");
                 rv = -EINVAL;
                 goto out;
         }
@@ -2676,9 +2706,8 @@ static int try_enable_event_buffer(struct smi_info *smi_info)
 
         rv = wait_for_msg_done(smi_info);
         if (rv) {
-                printk(KERN_WARNING
-                       "ipmi_si: Error getting response from set global,"
-                       " enables command, the event buffer is not"
+                printk(KERN_WARNING PFX "Error getting response from set"
+                       " global, enables command, the event buffer is not"
                        " enabled.\n");
                 goto out;
         }
@@ -2689,10 +2718,8 @@ static int try_enable_event_buffer(struct smi_info *smi_info)
         if (resp_len < 3 ||
                         resp[0] != (IPMI_NETFN_APP_REQUEST | 1) << 2 ||
                         resp[1] != IPMI_SET_BMC_GLOBAL_ENABLES_CMD) {
-                printk(KERN_WARNING
-                       "ipmi_si: Invalid return from get global,"
-                       "enables command, not enable the event"
-                       " buffer.\n");
+                printk(KERN_WARNING PFX "Invalid return from get global,"
+                       "enables command, not enable the event buffer.\n");
                 rv = -EINVAL;
                 goto out;
         }
@@ -2951,7 +2978,7 @@ static __devinit void default_find_bmc(void)
                 if (!info)
                         return;
 
-                info->addr_source = NULL;
+                info->addr_source = SI_DEFAULT;
 
                 info->si_type = ipmi_defaults[i].type;
                 info->io_setup = port_setup;
@@ -2963,14 +2990,16 @@ static __devinit void default_find_bmc(void)
                 info->io.regsize = DEFAULT_REGSPACING;
                 info->io.regshift = 0;
 
-                if (try_smi_init(info) == 0) {
-                        /* Found one... */
-                        printk(KERN_INFO "ipmi_si: Found default %s state"
-                               " machine at %s address 0x%lx\n",
-                               si_to_str[info->si_type],
-                               addr_space_to_str[info->io.addr_type],
-                               info->io.addr_data);
-                        return;
+                if (add_smi(info) == 0) {
+                        if ((try_smi_init(info)) == 0) {
+                                /* Found one... */
+                                printk(KERN_INFO PFX "Found default %s"
+                                " state machine at %s address 0x%lx\n",
+                                si_to_str[info->si_type],
+                                addr_space_to_str[info->io.addr_type],
+                                info->io.addr_data);
+                        } else
+                                cleanup_one_si(info);
                 }
         }
 }
@@ -2989,34 +3018,48 @@ static int is_new_interface(struct smi_info *info)
         return 1;
 }
 
-static int try_smi_init(struct smi_info *new_smi)
+static int add_smi(struct smi_info *new_smi)
 {
-        int rv;
-        int i;
-
-        if (new_smi->addr_source) {
-                printk(KERN_INFO "ipmi_si: Trying %s-specified %s state"
-                       " machine at %s address 0x%lx, slave address 0x%x,"
-                       " irq %d\n",
-                       new_smi->addr_source,
-                       si_to_str[new_smi->si_type],
-                       addr_space_to_str[new_smi->io.addr_type],
-                       new_smi->io.addr_data,
-                       new_smi->slave_addr, new_smi->irq);
-        }
+        int rv = 0;
 
+        printk(KERN_INFO PFX "Adding %s-specified %s state machine",
+                        ipmi_addr_src_to_str[new_smi->addr_source],
+                        si_to_str[new_smi->si_type]);
         mutex_lock(&smi_infos_lock);
         if (!is_new_interface(new_smi)) {
-                printk(KERN_WARNING "ipmi_si: duplicate interface\n");
+                printk(KERN_CONT PFX "duplicate interface\n");
                 rv = -EBUSY;
                 goto out_err;
         }
 
+        printk(KERN_CONT "\n");
+
         /* So we know not to free it unless we have allocated one. */
         new_smi->intf = NULL;
         new_smi->si_sm = NULL;
         new_smi->handlers = NULL;
 
+        list_add_tail(&new_smi->link, &smi_infos);
+
+out_err:
+        mutex_unlock(&smi_infos_lock);
+        return rv;
+}
+
+static int try_smi_init(struct smi_info *new_smi)
+{
+        int rv = 0;
+        int i;
+
+        printk(KERN_INFO PFX "Trying %s-specified %s state"
+               " machine at %s address 0x%lx, slave address 0x%x,"
+               " irq %d\n",
+               ipmi_addr_src_to_str[new_smi->addr_source],
+               si_to_str[new_smi->si_type],
+               addr_space_to_str[new_smi->io.addr_type],
+               new_smi->io.addr_data,
+               new_smi->slave_addr, new_smi->irq);
+
         switch (new_smi->si_type) {
         case SI_KCS:
                 new_smi->handlers = &kcs_smi_handlers;
@@ -3039,7 +3082,8 @@ static int try_smi_init(struct smi_info *new_smi)
         /* Allocate the state machine's data and initialize it. */
         new_smi->si_sm = kmalloc(new_smi->handlers->size(), GFP_KERNEL);
         if (!new_smi->si_sm) {
-                printk(KERN_ERR "Could not allocate state machine memory\n");
+                printk(KERN_ERR PFX
+                       "Could not allocate state machine memory\n");
                 rv = -ENOMEM;
                 goto out_err;
         }
@@ -3049,7 +3093,7 @@ static int try_smi_init(struct smi_info *new_smi)
         /* Now that we know the I/O size, we can set up the I/O. */
         rv = new_smi->io_setup(new_smi);
         if (rv) {
-                printk(KERN_ERR "Could not set up I/O space\n");
+                printk(KERN_ERR PFX "Could not set up I/O space\n");
                 goto out_err;
         }
 
@@ -3059,8 +3103,7 @@ static int try_smi_init(struct smi_info *new_smi)
         /* Do low-level detection first. */
         if (new_smi->handlers->detect(new_smi->si_sm)) {
                 if (new_smi->addr_source)
-                        printk(KERN_INFO "ipmi_si: Interface detection"
-                               " failed\n");
+                        printk(KERN_INFO PFX "Interface detection failed\n");
                 rv = -ENODEV;
                 goto out_err;
         }
@@ -3072,7 +3115,7 @@ static int try_smi_init(struct smi_info *new_smi)
         rv = try_get_dev_id(new_smi);
         if (rv) {
                 if (new_smi->addr_source)
-                        printk(KERN_INFO "ipmi_si: There appears to be no BMC"
+                        printk(KERN_INFO PFX "There appears to be no BMC"
                                " at this location\n");
                 goto out_err;
         }
@@ -3088,7 +3131,7 @@ static int try_smi_init(struct smi_info *new_smi)
         for (i = 0; i < SI_NUM_STATS; i++)
                 atomic_set(&new_smi->stats[i], 0);
 
-        new_smi->interrupt_disabled = 0;
+        new_smi->interrupt_disabled = 1;
         atomic_set(&new_smi->stop_operation, 0);
         new_smi->intf_num = smi_num;
         smi_num++;
@@ -3114,9 +3157,8 @@ static int try_smi_init(struct smi_info *new_smi)
                 new_smi->pdev = platform_device_alloc("ipmi_si",
                                                       new_smi->intf_num);
                 if (!new_smi->pdev) {
-                        printk(KERN_ERR
-                               "ipmi_si_intf:"
-                               " Unable to allocate platform device\n");
+                        printk(KERN_ERR PFX
+                               "Unable to allocate platform device\n");
                         goto out_err;
                 }
                 new_smi->dev = &new_smi->pdev->dev;
@@ -3124,9 +3166,8 @@ static int try_smi_init(struct smi_info *new_smi)
 
                 rv = platform_device_add(new_smi->pdev);
                 if (rv) {
-                        printk(KERN_ERR
-                               "ipmi_si_intf:"
-                               " Unable to register system interface device:"
+                        printk(KERN_ERR PFX
+                               "Unable to register system interface device:"
                                " %d\n",
                                rv);
                         goto out_err;
@@ -3141,9 +3182,8 @@ static int try_smi_init(struct smi_info *new_smi)
                                "bmc",
                                new_smi->slave_addr);
         if (rv) {
-                printk(KERN_ERR
-                       "ipmi_si: Unable to register device: error %d\n",
-                       rv);
+                dev_err(new_smi->dev, "Unable to register device: error %d\n",
+                        rv);
                 goto out_err_stop_timer;
         }
 
@@ -3151,9 +3191,7 @@ static int try_smi_init(struct smi_info *new_smi)
                                      type_file_read_proc,
                                      new_smi);
         if (rv) {
-                printk(KERN_ERR
-                       "ipmi_si: Unable to create proc entry: %d\n",
-                       rv);
+                dev_err(new_smi->dev, "Unable to create proc entry: %d\n", rv);
                 goto out_err_stop_timer;
         }
 
@@ -3161,9 +3199,7 @@ static int try_smi_init(struct smi_info *new_smi)
                                      stat_file_read_proc,
                                      new_smi);
         if (rv) {
-                printk(KERN_ERR
-                       "ipmi_si: Unable to create proc entry: %d\n",
-                       rv);
+                dev_err(new_smi->dev, "Unable to create proc entry: %d\n", rv);
                 goto out_err_stop_timer;
         }
 
@@ -3171,18 +3207,12 @@ static int try_smi_init(struct smi_info *new_smi)
                                      param_read_proc,
                                      new_smi);
         if (rv) {
-                printk(KERN_ERR
-                       "ipmi_si: Unable to create proc entry: %d\n",
-                       rv);
+                dev_err(new_smi->dev, "Unable to create proc entry: %d\n", rv);
                 goto out_err_stop_timer;
         }
 
-        list_add_tail(&new_smi->link, &smi_infos);
-
-        mutex_unlock(&smi_infos_lock);
-
-        printk(KERN_INFO "IPMI %s interface initialized\n",
-               si_to_str[new_smi->si_type]);
+        dev_info(new_smi->dev, "IPMI %s interface initialized\n",
+                 si_to_str[new_smi->si_type]);
 
         return 0;
 
@@ -3191,11 +3221,17 @@ static int try_smi_init(struct smi_info *new_smi)
         wait_for_timer_and_thread(new_smi);
 
  out_err:
-        if (new_smi->intf)
+        new_smi->interrupt_disabled = 1;
+
+        if (new_smi->intf) {
                 ipmi_unregister_smi(new_smi->intf);
+                new_smi->intf = NULL;
+        }
 
-        if (new_smi->irq_cleanup)
+        if (new_smi->irq_cleanup) {
                 new_smi->irq_cleanup(new_smi);
+                new_smi->irq_cleanup = NULL;
+        }
 
         /*
          * Wait until we know that we are out of any interrupt
@@ -3208,18 +3244,21 @@ static int try_smi_init(struct smi_info *new_smi)
                 if (new_smi->handlers)
                         new_smi->handlers->cleanup(new_smi->si_sm);
                 kfree(new_smi->si_sm);
+                new_smi->si_sm = NULL;
         }
-        if (new_smi->addr_source_cleanup)
+        if (new_smi->addr_source_cleanup) {
                 new_smi->addr_source_cleanup(new_smi);
-        if (new_smi->io_cleanup)
+                new_smi->addr_source_cleanup = NULL;
+        }
+        if (new_smi->io_cleanup) {
                 new_smi->io_cleanup(new_smi);
+                new_smi->io_cleanup = NULL;
+        }
 
-        if (new_smi->dev_registered)
+        if (new_smi->dev_registered) {
                 platform_device_unregister(new_smi->pdev);
-
-        kfree(new_smi);
-
-        mutex_unlock(&smi_infos_lock);
+                new_smi->dev_registered = 0;
+        }
 
         return rv;
 }
@@ -3229,6 +3268,8 @@ static __devinit int init_ipmi_si(void)
         int  i;
         char *str;
         int  rv;
+        struct smi_info *e;
+        enum ipmi_addr_src type = SI_INVALID;
 
         if (initialized)
                 return 0;
@@ -3237,9 +3278,7 @@ static __devinit int init_ipmi_si(void)
         /* Register the device drivers. */
         rv = driver_register(&ipmi_driver.driver);
         if (rv) {
-                printk(KERN_ERR
-                       "init_ipmi_si: Unable to register driver: %d\n",
-                       rv);
+                printk(KERN_ERR PFX "Unable to register driver: %d\n", rv);
                 return rv;
         }
 
@@ -3263,38 +3302,81 @@ static __devinit int init_ipmi_si(void)
 
         hardcode_find_bmc();
 
-#ifdef CONFIG_DMI
-        dmi_find_bmc();
-#endif
+        /* If the user gave us a device, they presumably want us to use it */
+        mutex_lock(&smi_infos_lock);
+        if (!list_empty(&smi_infos)) {
+                mutex_unlock(&smi_infos_lock);
+                return 0;
+        }
+        mutex_unlock(&smi_infos_lock);
 
-#ifdef CONFIG_ACPI
-        spmi_find_bmc();
+#ifdef CONFIG_PCI
+        rv = pci_register_driver(&ipmi_pci_driver);
+        if (rv)
+                printk(KERN_ERR PFX "Unable to register PCI driver: %d\n", rv);
 #endif
+
 #ifdef CONFIG_ACPI
         pnp_register_driver(&ipmi_pnp_driver);
 #endif
 
-#ifdef CONFIG_PCI
-        rv = pci_register_driver(&ipmi_pci_driver);
-        if (rv)
-                printk(KERN_ERR
-                       "init_ipmi_si: Unable to register PCI driver: %d\n",
-                       rv);
+#ifdef CONFIG_DMI
+        dmi_find_bmc();
+#endif
+
+#ifdef CONFIG_ACPI
+        spmi_find_bmc();
 #endif
 
 #ifdef CONFIG_PPC_OF
         of_register_platform_driver(&ipmi_of_platform_driver);
 #endif
 
+        /* We prefer devices with interrupts, but in the case of a machine
+           with multiple BMCs we assume that there will be several instances
+           of a given type so if we succeed in registering a type then also
+           try to register everything else of the same type */
+
+        mutex_lock(&smi_infos_lock);
+        list_for_each_entry(e, &smi_infos, link) {
+                /* Try to register a device if it has an IRQ and we either
+                   haven't successfully registered a device yet or this
+                   device has the same type as one we successfully registered */
+                if (e->irq && (!type || e->addr_source == type)) {
+                        if (!try_smi_init(e)) {
+                                type = e->addr_source;
+                        }
+                }
+        }
+
+        /* type will only have been set if we successfully registered an si */
+        if (type) {
+                mutex_unlock(&smi_infos_lock);
+                return 0;
+        }
+
+        /* Fall back to the preferred device */
+
+        list_for_each_entry(e, &smi_infos, link) {
+                if (!e->irq && (!type || e->addr_source == type)) {
+                        if (!try_smi_init(e)) {
+                                type = e->addr_source;
+                        }
+                }
+        }
+        mutex_unlock(&smi_infos_lock);
+
+        if (type)
+                return 0;
+
         if (si_trydefaults) {
                 mutex_lock(&smi_infos_lock);
                 if (list_empty(&smi_infos)) {
                         /* No BMC was found, try defaults. */
                         mutex_unlock(&smi_infos_lock);
                         default_find_bmc();
-                } else {
+                } else
                         mutex_unlock(&smi_infos_lock);
-                }
         }
 
         mutex_lock(&smi_infos_lock);
@@ -3308,8 +3390,8 @@ static __devinit int init_ipmi_si(void)
                 of_unregister_platform_driver(&ipmi_of_platform_driver);
 #endif
                 driver_unregister(&ipmi_driver.driver);
-                printk(KERN_WARNING
-                       "ipmi_si: Unable to find any System Interface(s)\n");
+                printk(KERN_WARNING PFX
+                       "Unable to find any System Interface(s)\n");
                 return -ENODEV;
         } else {
                 mutex_unlock(&smi_infos_lock);
@@ -3320,7 +3402,7 @@ module_init(init_ipmi_si);
 
 static void cleanup_one_si(struct smi_info *to_clean)
 {
-        int           rv;
+        int           rv = 0;
         unsigned long flags;
 
         if (!to_clean)
@@ -3364,14 +3446,16 @@ static void cleanup_one_si(struct smi_info *to_clean)
                 schedule_timeout_uninterruptible(1);
         }
 
-        rv = ipmi_unregister_smi(to_clean->intf);
+        if (to_clean->intf)
+                rv = ipmi_unregister_smi(to_clean->intf);
+
         if (rv) {
-                printk(KERN_ERR
-                       "ipmi_si: Unable to unregister device: errno=%d\n",
+                printk(KERN_ERR PFX "Unable to unregister device: errno=%d\n",
                        rv);
         }
 
-        to_clean->handlers->cleanup(to_clean->si_sm);
+        if (to_clean->handlers)
+                to_clean->handlers->cleanup(to_clean->si_sm);
 
         kfree(to_clean->si_sm);
 
diff --git a/drivers/char/n_gsm.c b/drivers/char/n_gsm.c
index c4161d5e053d..e4089c432f15 100644
--- a/drivers/char/n_gsm.c
+++ b/drivers/char/n_gsm.c
@@ -904,9 +904,7 @@ static void gsm_dlci_data_sweep(struct gsm_mux *gsm)
         int len;
         /* Priority ordering: We should do priority with RR of the groups */
         int i = 1;
-        unsigned long flags;
 
-        spin_lock_irqsave(&gsm->tx_lock, flags);
         while (i < NUM_DLCI) {
                 struct gsm_dlci *dlci;
 
@@ -927,7 +925,6 @@ static void gsm_dlci_data_sweep(struct gsm_mux *gsm)
                 if (len == 0)
                         i++;
         }
-        spin_unlock_irqrestore(&gsm->tx_lock, flags);
 }
 
 /**
@@ -2230,12 +2227,16 @@ static int gsmld_open(struct tty_struct *tty)
 static void gsmld_write_wakeup(struct tty_struct *tty)
 {
         struct gsm_mux *gsm = tty->disc_data;
+        unsigned long flags;
 
         /* Queue poll */
         clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
         gsm_data_kick(gsm);
-        if (gsm->tx_bytes < TX_THRESH_LO)
+        if (gsm->tx_bytes < TX_THRESH_LO) {
+                spin_lock_irqsave(&gsm->tx_lock, flags);
                 gsm_dlci_data_sweep(gsm);
+                spin_unlock_irqrestore(&gsm->tx_lock, flags);
+        }
 }
 
 /**
diff --git a/drivers/char/ppdev.c b/drivers/char/ppdev.c
index fdd37543aa79..02abfddce45a 100644
--- a/drivers/char/ppdev.c
+++ b/drivers/char/ppdev.c
@@ -287,12 +287,10 @@ static int register_device (int minor, struct pp_struct *pp)
         char *name;
         int fl;
 
-        name = kmalloc (strlen (CHRDEV) + 3, GFP_KERNEL);
+        name = kasprintf(GFP_KERNEL, CHRDEV "%x", minor);
         if (name == NULL)
                 return -ENOMEM;
 
-        sprintf (name, CHRDEV "%x", minor);
-
         port = parport_find_number (minor);
         if (!port) {
                 printk (KERN_WARNING "%s: no associated port!\n", name);
diff --git a/drivers/char/ps3flash.c b/drivers/char/ps3flash.c
index 606048b72bcf..85c004a518ee 100644
--- a/drivers/char/ps3flash.c
+++ b/drivers/char/ps3flash.c
@@ -305,8 +305,7 @@ static int ps3flash_flush(struct file *file, fl_owner_t id)
         return ps3flash_writeback(ps3flash_dev);
 }
 
-static int ps3flash_fsync(struct file *file, struct dentry *dentry,
-                          int datasync)
+static int ps3flash_fsync(struct file *file, int datasync)
 {
         return ps3flash_writeback(ps3flash_dev);
 }
diff --git a/drivers/char/ramoops.c b/drivers/char/ramoops.c
new file mode 100644
index 000000000000..74f00b5ffa36
--- /dev/null
+++ b/drivers/char/ramoops.c
@@ -0,0 +1,162 @@
+/*
+ * RAM Oops/Panic logger
+ *
+ * Copyright (C) 2010 Marco Stornelli <marco.stornelli@gmail.com>
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kmsg_dump.h>
+#include <linux/time.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+
+#define RAMOOPS_KERNMSG_HDR "===="
+#define RAMOOPS_HEADER_SIZE   (5 + sizeof(struct timeval))
+
+#define RECORD_SIZE 4096
+
+static ulong mem_address;
+module_param(mem_address, ulong, 0400);
+MODULE_PARM_DESC(mem_address,
+                "start of reserved RAM used to store oops/panic logs");
+
+static ulong mem_size;
+module_param(mem_size, ulong, 0400);
+MODULE_PARM_DESC(mem_size,
+                "size of reserved RAM used to store oops/panic logs");
+
+static int dump_oops = 1;
+module_param(dump_oops, int, 0600);
+MODULE_PARM_DESC(dump_oops,
+                "set to 1 to dump oopses, 0 to only dump panics (default 1)");
+
+static struct ramoops_context {
+        struct kmsg_dumper dump;
+        void *virt_addr;
+        phys_addr_t phys_addr;
+        unsigned long size;
+        int count;
+        int max_count;
+} oops_cxt;
+
+static void ramoops_do_dump(struct kmsg_dumper *dumper,
+                enum kmsg_dump_reason reason, const char *s1, unsigned long l1,
+                const char *s2, unsigned long l2)
+{
+        struct ramoops_context *cxt = container_of(dumper,
+                        struct ramoops_context, dump);
+        unsigned long s1_start, s2_start;
+        unsigned long l1_cpy, l2_cpy;
+        int res;
+        char *buf;
+        struct timeval timestamp;
+
+        /* Only dump oopses if dump_oops is set */
+        if (reason == KMSG_DUMP_OOPS && !dump_oops)
+                return;
+
+        buf = (char *)(cxt->virt_addr + (cxt->count * RECORD_SIZE));
+        memset(buf, '\0', RECORD_SIZE);
+        res = sprintf(buf, "%s", RAMOOPS_KERNMSG_HDR);
+        buf += res;
+        do_gettimeofday(&timestamp);
+        res = sprintf(buf, "%lu.%lu\n", (long)timestamp.tv_sec, (long)timestamp.tv_usec);
+        buf += res;
+
+        l2_cpy = min(l2, (unsigned long)(RECORD_SIZE - RAMOOPS_HEADER_SIZE));
+        l1_cpy = min(l1, (unsigned long)(RECORD_SIZE - RAMOOPS_HEADER_SIZE) - l2_cpy);
+
+        s2_start = l2 - l2_cpy;
+        s1_start = l1 - l1_cpy;
+
+        memcpy(buf, s1 + s1_start, l1_cpy);
+        memcpy(buf + l1_cpy, s2 + s2_start, l2_cpy);
+
+        cxt->count = (cxt->count + 1) % cxt->max_count;
+}
+
+static int __init ramoops_init(void)
+{
+        struct ramoops_context *cxt = &oops_cxt;
+        int err = -EINVAL;
+
+        if (!mem_size) {
+                printk(KERN_ERR "ramoops: invalid size specification");
+                goto fail3;
+        }
+
+        rounddown_pow_of_two(mem_size);
+
+        if (mem_size < RECORD_SIZE) {
+                printk(KERN_ERR "ramoops: size too small");
+                goto fail3;
+        }
+
+        cxt->max_count = mem_size / RECORD_SIZE;
+        cxt->count = 0;
+        cxt->size = mem_size;
+        cxt->phys_addr = mem_address;
+
+        if (!request_mem_region(cxt->phys_addr, cxt->size, "ramoops")) {
+                printk(KERN_ERR "ramoops: request mem region failed");
+                err = -EINVAL;
+                goto fail3;
+        }
+
+        cxt->virt_addr = ioremap(cxt->phys_addr,  cxt->size);
+        if (!cxt->virt_addr) {
+                printk(KERN_ERR "ramoops: ioremap failed");
+                goto fail2;
+        }
+
+        cxt->dump.dump = ramoops_do_dump;
+        err = kmsg_dump_register(&cxt->dump);
+        if (err) {
+                printk(KERN_ERR "ramoops: registering kmsg dumper failed");
+                goto fail1;
+        }
+
+        return 0;
+
+fail1:
+        iounmap(cxt->virt_addr);
+fail2:
+        release_mem_region(cxt->phys_addr, cxt->size);
+fail3:
+        return err;
+}
+
+static void __exit ramoops_exit(void)
+{
+        struct ramoops_context *cxt = &oops_cxt;
+
+        if (kmsg_dump_unregister(&cxt->dump) < 0)
+                printk(KERN_WARNING "ramoops: could not unregister kmsg_dumper");
+
+        iounmap(cxt->virt_addr);
+        release_mem_region(cxt->phys_addr, cxt->size);
+}
+
+
+module_init(ramoops_init);
+module_exit(ramoops_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Marco Stornelli <marco.stornelli@gmail.com>");
+MODULE_DESCRIPTION("RAM Oops/Panic logger/driver");
diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index 8c99bf1b5e9f..942a9826bd23 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -529,6 +529,10 @@ static bool will_write_block(struct port *port)
 {
         bool ret;
 
+        if (!port->guest_connected) {
+                /* Port got hot-unplugged. Let's exit. */
+                return false;
+        }
         if (!port->host_connected)
                 return true;
 
@@ -1099,6 +1103,13 @@ static int remove_port(struct port *port)
 {
         struct port_buffer *buf;
 
+        if (port->guest_connected) {
+                port->guest_connected = false;
+                port->host_connected = false;
+                wake_up_interruptible(&port->waitqueue);
+                send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 0);
+        }
+
         spin_lock_irq(&port->portdev->ports_lock);
         list_del(&port->list);
         spin_unlock_irq(&port->portdev->ports_lock);
@@ -1120,9 +1131,6 @@ static int remove_port(struct port *port)
                 hvc_remove(port->cons.hvc);
 #endif
         }
-        if (port->guest_connected)
-                send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 0);
-
         sysfs_remove_group(&port->dev->kobj, &port_attribute_group);
         device_destroy(pdrvdata.class, port->dev->devt);
         cdev_del(&port->cdev);
diff --git a/drivers/char/vt.c b/drivers/char/vt.c
index bd1d1164fec5..7cdb6ee569cd 100644
--- a/drivers/char/vt.c
+++ b/drivers/char/vt.c
@@ -3967,13 +3967,9 @@ static int con_font_set(struct vc_data *vc, struct console_font_op *op)
         font.charcount = op->charcount;
         font.height = op->height;
         font.width = op->width;
-        font.data = kmalloc(size, GFP_KERNEL);
-        if (!font.data)
-                return -ENOMEM;
-        if (copy_from_user(font.data, op->data, size)) {
-                kfree(font.data);
-                return -EFAULT;
-        }
+        font.data = memdup_user(op->data, size);
+        if (IS_ERR(font.data))
+                return PTR_ERR(font.data);
         acquire_console_sem();
         if (vc->vc_sw->con_font_set)
                 rc = vc->vc_sw->con_font_set(vc, &font, op->flags);
diff --git a/drivers/char/vt_ioctl.c b/drivers/char/vt_ioctl.c
index 6aa10284104a..cb19dbc52136 100644
--- a/drivers/char/vt_ioctl.c
+++ b/drivers/char/vt_ioctl.c
@@ -1303,7 +1303,9 @@ int vt_ioctl(struct tty_struct *tty, struct file * file,
                 if (!perm)
                         goto eperm;
                 ret = copy_from_user(&ui, up, sizeof(struct unimapinit));
-                if (!ret)
+                if (ret)
+                        ret = -EFAULT;
+                else
                         con_clear_unimap(vc, &ui);
                 break;
               }
diff --git a/drivers/clocksource/sh_cmt.c b/drivers/clocksource/sh_cmt.c
index f6677cb19789..f3d3898898ed 100644
--- a/drivers/clocksource/sh_cmt.c
+++ b/drivers/clocksource/sh_cmt.c
@@ -412,18 +412,10 @@ static cycle_t sh_cmt_clocksource_read(struct clocksource *cs)
 static int sh_cmt_clocksource_enable(struct clocksource *cs)
 {
         struct sh_cmt_priv *p = cs_to_sh_cmt(cs);
-        int ret;
 
         p->total_cycles = 0;
 
-        ret = sh_cmt_start(p, FLAG_CLOCKSOURCE);
-        if (ret)
-                return ret;
-
-        /* TODO: calculate good shift from rate and counter bit width */
-        cs->shift = 0;
-        cs->mult = clocksource_hz2mult(p->rate, cs->shift);
-        return 0;
+        return sh_cmt_start(p, FLAG_CLOCKSOURCE);
 }
 
 static void sh_cmt_clocksource_disable(struct clocksource *cs)
@@ -450,8 +442,20 @@ static int sh_cmt_register_clocksource(struct sh_cmt_priv *p,
         cs->resume = sh_cmt_clocksource_resume;
         cs->mask = CLOCKSOURCE_MASK(sizeof(unsigned long) * 8);
         cs->flags = CLOCK_SOURCE_IS_CONTINUOUS;
+
+        /* clk_get_rate() needs an enabled clock */
+        clk_enable(p->clk);
+        p->rate = clk_get_rate(p->clk) / (p->width == 16) ? 512 : 8;
+        clk_disable(p->clk);
+
+        /* TODO: calculate good shift from rate and counter bit width */
+        cs->shift = 10;
+        cs->mult = clocksource_hz2mult(p->rate, cs->shift);
+
         dev_info(&p->pdev->dev, "used as clock source\n");
+
         clocksource_register(cs);
+
         return 0;
 }
 
diff --git a/drivers/clocksource/sh_tmu.c b/drivers/clocksource/sh_tmu.c
index 8e44e14ec4c2..de715901b82a 100644
--- a/drivers/clocksource/sh_tmu.c
+++ b/drivers/clocksource/sh_tmu.c
@@ -199,16 +199,8 @@ static cycle_t sh_tmu_clocksource_read(struct clocksource *cs)
 static int sh_tmu_clocksource_enable(struct clocksource *cs)
 {
         struct sh_tmu_priv *p = cs_to_sh_tmu(cs);
-        int ret;
-
-        ret = sh_tmu_enable(p);
-        if (ret)
-                return ret;
 
-        /* TODO: calculate good shift from rate and counter bit width */
-        cs->shift = 10;
-        cs->mult = clocksource_hz2mult(p->rate, cs->shift);
-        return 0;
+        return sh_tmu_enable(p);
 }
 
 static void sh_tmu_clocksource_disable(struct clocksource *cs)
@@ -228,6 +220,16 @@ static int sh_tmu_register_clocksource(struct sh_tmu_priv *p,
         cs->disable = sh_tmu_clocksource_disable;
         cs->mask = CLOCKSOURCE_MASK(32);
         cs->flags = CLOCK_SOURCE_IS_CONTINUOUS;
+
+        /* clk_get_rate() needs an enabled clock */
+        clk_enable(p->clk);
+        /* channel will be configured at parent clock / 4 */
+        p->rate = clk_get_rate(p->clk) / 4;
+        clk_disable(p->clk);
+        /* TODO: calculate good shift from rate and counter bit width */
+        cs->shift = 10;
+        cs->mult = clocksource_hz2mult(p->rate, cs->shift);
+
         dev_info(&p->pdev->dev, "used as clock source\n");
         clocksource_register(cs);
         return 0;
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index 12fdd3987a36..199488576a05 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -156,7 +156,7 @@ int cpuidle_enable_device(struct cpuidle_device *dev)
 
         if (dev->enabled)
                 return 0;
-        if (!cpuidle_curr_driver || !cpuidle_curr_governor)
+        if (!cpuidle_get_driver() || !cpuidle_curr_governor)
                 return -EIO;
         if (!dev->state_count)
                 return -EINVAL;
@@ -207,7 +207,7 @@ void cpuidle_disable_device(struct cpuidle_device *dev)
 {
         if (!dev->enabled)
                 return;
-        if (!cpuidle_curr_driver || !cpuidle_curr_governor)
+        if (!cpuidle_get_driver() || !cpuidle_curr_governor)
                 return;
 
         dev->enabled = 0;
@@ -271,10 +271,11 @@ static int __cpuidle_register_device(struct cpuidle_device *dev)
 {
         int ret;
         struct sys_device *sys_dev = get_cpu_sysdev((unsigned long)dev->cpu);
+        struct cpuidle_driver *cpuidle_driver = cpuidle_get_driver();
 
         if (!sys_dev)
                 return -EINVAL;
-        if (!try_module_get(cpuidle_curr_driver->owner))
+        if (!try_module_get(cpuidle_driver->owner))
                 return -EINVAL;
 
         init_completion(&dev->kobj_unregister);
@@ -284,7 +285,7 @@ static int __cpuidle_register_device(struct cpuidle_device *dev)
         per_cpu(cpuidle_devices, dev->cpu) = dev;
         list_add(&dev->device_list, &cpuidle_detected_devices);
         if ((ret = cpuidle_add_sysfs(sys_dev))) {
-                module_put(cpuidle_curr_driver->owner);
+                module_put(cpuidle_driver->owner);
                 return ret;
         }
 
@@ -325,6 +326,7 @@ EXPORT_SYMBOL_GPL(cpuidle_register_device);
 void cpuidle_unregister_device(struct cpuidle_device *dev)
 {
         struct sys_device *sys_dev = get_cpu_sysdev((unsigned long)dev->cpu);
+        struct cpuidle_driver *cpuidle_driver = cpuidle_get_driver();
 
         if (dev->registered == 0)
                 return;
@@ -340,7 +342,7 @@ void cpuidle_unregister_device(struct cpuidle_device *dev)
 
         cpuidle_resume_and_unlock();
 
-        module_put(cpuidle_curr_driver->owner);
+        module_put(cpuidle_driver->owner);
 }
 
 EXPORT_SYMBOL_GPL(cpuidle_unregister_device);
diff --git a/drivers/cpuidle/cpuidle.h b/drivers/cpuidle/cpuidle.h
index 9476ba33ee2c..33e50d556f17 100644
--- a/drivers/cpuidle/cpuidle.h
+++ b/drivers/cpuidle/cpuidle.h
@@ -9,7 +9,6 @@
 
 /* For internal use only */
 extern struct cpuidle_governor *cpuidle_curr_governor;
-extern struct cpuidle_driver *cpuidle_curr_driver;
 extern struct list_head cpuidle_governors;
 extern struct list_head cpuidle_detected_devices;
 extern struct mutex cpuidle_lock;
diff --git a/drivers/cpuidle/driver.c b/drivers/cpuidle/driver.c
index 2257004fe33d..fd1601e3d125 100644
--- a/drivers/cpuidle/driver.c
+++ b/drivers/cpuidle/driver.c
@@ -14,7 +14,7 @@
 
 #include "cpuidle.h"
 
-struct cpuidle_driver *cpuidle_curr_driver;
+static struct cpuidle_driver *cpuidle_curr_driver;
 DEFINE_SPINLOCK(cpuidle_driver_lock);
 
 /**
@@ -40,13 +40,25 @@ int cpuidle_register_driver(struct cpuidle_driver *drv)
 EXPORT_SYMBOL_GPL(cpuidle_register_driver);
 
 /**
+ * cpuidle_get_driver - return the current driver
+ */
+struct cpuidle_driver *cpuidle_get_driver(void)
+{
+        return cpuidle_curr_driver;
+}
+EXPORT_SYMBOL_GPL(cpuidle_get_driver);
+
+/**
  * cpuidle_unregister_driver - unregisters a driver
  * @drv: the driver
  */
 void cpuidle_unregister_driver(struct cpuidle_driver *drv)
 {
-        if (!drv)
+        if (drv != cpuidle_curr_driver) {
+                WARN(1, "invalid cpuidle_unregister_driver(%s)\n",
+                        drv->name);
                 return;
+        }
 
         spin_lock(&cpuidle_driver_lock);
         cpuidle_curr_driver = NULL;
diff --git a/drivers/cpuidle/sysfs.c b/drivers/cpuidle/sysfs.c
index 0ba9c8b8ee74..0310ffaec9df 100644
--- a/drivers/cpuidle/sysfs.c
+++ b/drivers/cpuidle/sysfs.c
@@ -47,10 +47,11 @@ static ssize_t show_current_driver(struct sysdev_class *class,
                                    char *buf)
 {
         ssize_t ret;
+        struct cpuidle_driver *cpuidle_driver = cpuidle_get_driver();
 
         spin_lock(&cpuidle_driver_lock);
-        if (cpuidle_curr_driver)
-                ret = sprintf(buf, "%s\n", cpuidle_curr_driver->name);
+        if (cpuidle_driver)
+                ret = sprintf(buf, "%s\n", cpuidle_driver->name);
         else
                 ret = sprintf(buf, "none\n");
         spin_unlock(&cpuidle_driver_lock);
diff --git a/drivers/crypto/amcc/crypto4xx_core.c b/drivers/crypto/amcc/crypto4xx_core.c
index 9d65b371de64..983530ba04a7 100644
--- a/drivers/crypto/amcc/crypto4xx_core.c
+++ b/drivers/crypto/amcc/crypto4xx_core.c
@@ -1158,7 +1158,7 @@ static int __init crypto4xx_probe(struct of_device *ofdev,
         struct device *dev = &ofdev->dev;
         struct crypto4xx_core_device *core_dev;
 
-        rc = of_address_to_resource(ofdev->node, 0, &res);
+        rc = of_address_to_resource(ofdev->dev.of_node, 0, &res);
         if (rc)
                 return -ENODEV;
 
@@ -1215,13 +1215,13 @@ static int __init crypto4xx_probe(struct of_device *ofdev,
                      (unsigned long) dev);
 
         /* Register for Crypto isr, Crypto Engine IRQ */
-        core_dev->irq = irq_of_parse_and_map(ofdev->node, 0);
+        core_dev->irq = irq_of_parse_and_map(ofdev->dev.of_node, 0);
         rc = request_irq(core_dev->irq, crypto4xx_ce_interrupt_handler, 0,
                          core_dev->dev->name, dev);
         if (rc)
                 goto err_request_irq;
 
-        core_dev->dev->ce_base = of_iomap(ofdev->node, 0);
+        core_dev->dev->ce_base = of_iomap(ofdev->dev.of_node, 0);
         if (!core_dev->dev->ce_base) {
                 dev_err(dev, "failed to of_iomap\n");
                 goto err_iomap;
diff --git a/drivers/crypto/n2_core.c b/drivers/crypto/n2_core.c
index 8566be832f51..23163fda5035 100644
--- a/drivers/crypto/n2_core.c
+++ b/drivers/crypto/n2_core.c
@@ -251,16 +251,10 @@ static void n2_base_ctx_init(struct n2_base_ctx *ctx)
 struct n2_hash_ctx {
         struct n2_base_ctx              base;
 
-        struct crypto_ahash             *fallback;
+        struct crypto_ahash             *fallback_tfm;
+};
 
-        /* These next three members must match the layout created by
-         * crypto_init_shash_ops_async.  This allows us to properly
-         * plumb requests we can't do in hardware down to the fallback
-         * operation, providing all of the data structures and layouts
-         * expected by those paths.
-         */
-        struct ahash_request            fallback_req;
-        struct shash_desc               fallback_desc;
+struct n2_hash_req_ctx {
         union {
                 struct md5_state        md5;
                 struct sha1_state       sha1;
@@ -269,56 +263,62 @@ struct n2_hash_ctx {
 
         unsigned char                   hash_key[64];
         unsigned char                   keyed_zero_hash[32];
+
+        struct ahash_request            fallback_req;
 };
 
 static int n2_hash_async_init(struct ahash_request *req)
 {
+        struct n2_hash_req_ctx *rctx = ahash_request_ctx(req);
         struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
         struct n2_hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
-        ctx->fallback_req.base.tfm = crypto_ahash_tfm(ctx->fallback);
-        ctx->fallback_req.base.flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP;
+        ahash_request_set_tfm(&rctx->fallback_req, ctx->fallback_tfm);
+        rctx->fallback_req.base.flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP;
 
-        return crypto_ahash_init(&ctx->fallback_req);
+        return crypto_ahash_init(&rctx->fallback_req);
 }
 
 static int n2_hash_async_update(struct ahash_request *req)
 {
+        struct n2_hash_req_ctx *rctx = ahash_request_ctx(req);
         struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
         struct n2_hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
-        ctx->fallback_req.base.tfm = crypto_ahash_tfm(ctx->fallback);
-        ctx->fallback_req.base.flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP;
-        ctx->fallback_req.nbytes = req->nbytes;
-        ctx->fallback_req.src = req->src;
+        ahash_request_set_tfm(&rctx->fallback_req, ctx->fallback_tfm);
+        rctx->fallback_req.base.flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP;
+        rctx->fallback_req.nbytes = req->nbytes;
+        rctx->fallback_req.src = req->src;
 
-        return crypto_ahash_update(&ctx->fallback_req);
+        return crypto_ahash_update(&rctx->fallback_req);
 }
 
 static int n2_hash_async_final(struct ahash_request *req)
 {
+        struct n2_hash_req_ctx *rctx = ahash_request_ctx(req);
         struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
         struct n2_hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
-        ctx->fallback_req.base.tfm = crypto_ahash_tfm(ctx->fallback);
-        ctx->fallback_req.base.flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP;
-        ctx->fallback_req.result = req->result;
+        ahash_request_set_tfm(&rctx->fallback_req, ctx->fallback_tfm);
+        rctx->fallback_req.base.flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP;
+        rctx->fallback_req.result = req->result;
 
-        return crypto_ahash_final(&ctx->fallback_req);
+        return crypto_ahash_final(&rctx->fallback_req);
 }
 
 static int n2_hash_async_finup(struct ahash_request *req)
 {
+        struct n2_hash_req_ctx *rctx = ahash_request_ctx(req);
         struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
         struct n2_hash_ctx *ctx = crypto_ahash_ctx(tfm);
 
-        ctx->fallback_req.base.tfm = crypto_ahash_tfm(ctx->fallback);
-        ctx->fallback_req.base.flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP;
-        ctx->fallback_req.nbytes = req->nbytes;
-        ctx->fallback_req.src = req->src;
-        ctx->fallback_req.result = req->result;
+        ahash_request_set_tfm(&rctx->fallback_req, ctx->fallback_tfm);
+        rctx->fallback_req.base.flags = req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP;
+        rctx->fallback_req.nbytes = req->nbytes;
+        rctx->fallback_req.src = req->src;
+        rctx->fallback_req.result = req->result;
 
-        return crypto_ahash_finup(&ctx->fallback_req);
+        return crypto_ahash_finup(&rctx->fallback_req);
 }
 
 static int n2_hash_cra_init(struct crypto_tfm *tfm)
@@ -338,7 +338,10 @@ static int n2_hash_cra_init(struct crypto_tfm *tfm)
                 goto out;
         }
 
-        ctx->fallback = fallback_tfm;
+        crypto_ahash_set_reqsize(ahash, (sizeof(struct n2_hash_req_ctx) +
+                                         crypto_ahash_reqsize(fallback_tfm)));
+
+        ctx->fallback_tfm = fallback_tfm;
         return 0;
 
 out:
@@ -350,7 +353,7 @@ static void n2_hash_cra_exit(struct crypto_tfm *tfm)
         struct crypto_ahash *ahash = __crypto_ahash_cast(tfm);
         struct n2_hash_ctx *ctx = crypto_ahash_ctx(ahash);
 
-        crypto_free_ahash(ctx->fallback);
+        crypto_free_ahash(ctx->fallback_tfm);
 }
 
 static unsigned long wait_for_tail(struct spu_queue *qp)
@@ -399,14 +402,16 @@ static int n2_hash_async_digest(struct ahash_request *req,
          * exceed 2^16.
          */
         if (unlikely(req->nbytes > (1 << 16))) {
-                ctx->fallback_req.base.tfm = crypto_ahash_tfm(ctx->fallback);
-                ctx->fallback_req.base.flags =
+                struct n2_hash_req_ctx *rctx = ahash_request_ctx(req);
+
+                ahash_request_set_tfm(&rctx->fallback_req, ctx->fallback_tfm);
+                rctx->fallback_req.base.flags =
                         req->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP;
-                ctx->fallback_req.nbytes = req->nbytes;
-                ctx->fallback_req.src = req->src;
-                ctx->fallback_req.result = req->result;
+                rctx->fallback_req.nbytes = req->nbytes;
+                rctx->fallback_req.src = req->src;
+                rctx->fallback_req.result = req->result;
 
-                return crypto_ahash_digest(&ctx->fallback_req);
+                return crypto_ahash_digest(&rctx->fallback_req);
         }
 
         n2_base_ctx_init(&ctx->base);
@@ -472,9 +477,8 @@ out:
 
 static int n2_md5_async_digest(struct ahash_request *req)
 {
-        struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
-        struct n2_hash_ctx *ctx = crypto_ahash_ctx(tfm);
-        struct md5_state *m = &ctx->u.md5;
+        struct n2_hash_req_ctx *rctx = ahash_request_ctx(req);
+        struct md5_state *m = &rctx->u.md5;
 
         if (unlikely(req->nbytes == 0)) {
                 static const char md5_zero[MD5_DIGEST_SIZE] = {
@@ -497,9 +501,8 @@ static int n2_md5_async_digest(struct ahash_request *req)
 
 static int n2_sha1_async_digest(struct ahash_request *req)
 {
-        struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
-        struct n2_hash_ctx *ctx = crypto_ahash_ctx(tfm);
-        struct sha1_state *s = &ctx->u.sha1;
+        struct n2_hash_req_ctx *rctx = ahash_request_ctx(req);
+        struct sha1_state *s = &rctx->u.sha1;
 
         if (unlikely(req->nbytes == 0)) {
                 static const char sha1_zero[SHA1_DIGEST_SIZE] = {
@@ -524,9 +527,8 @@ static int n2_sha1_async_digest(struct ahash_request *req)
 
 static int n2_sha256_async_digest(struct ahash_request *req)
 {
-        struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
-        struct n2_hash_ctx *ctx = crypto_ahash_ctx(tfm);
-        struct sha256_state *s = &ctx->u.sha256;
+        struct n2_hash_req_ctx *rctx = ahash_request_ctx(req);
+        struct sha256_state *s = &rctx->u.sha256;
 
         if (req->nbytes == 0) {
                 static const char sha256_zero[SHA256_DIGEST_SIZE] = {
@@ -555,9 +557,8 @@ static int n2_sha256_async_digest(struct ahash_request *req)
 
 static int n2_sha224_async_digest(struct ahash_request *req)
 {
-        struct crypto_ahash *tfm = crypto_ahash_reqtfm(req);
-        struct n2_hash_ctx *ctx = crypto_ahash_ctx(tfm);
-        struct sha256_state *s = &ctx->u.sha256;
+        struct n2_hash_req_ctx *rctx = ahash_request_ctx(req);
+        struct sha256_state *s = &rctx->u.sha256;
 
         if (req->nbytes == 0) {
                 static const char sha224_zero[SHA224_DIGEST_SIZE] = {
@@ -1398,7 +1399,7 @@ static int find_devino_index(struct of_device *dev, struct spu_mdesc_info *ip,
 
         intr = ip->ino_table[i].intr;
 
-        dev_intrs = of_get_property(dev->node, "interrupts", NULL);
+        dev_intrs = of_get_property(dev->dev.of_node, "interrupts", NULL);
         if (!dev_intrs)
                 return -ENODEV;
 
@@ -1449,7 +1450,7 @@ static int queue_cache_init(void)
 {
         if (!queue_cache[HV_NCS_QTYPE_MAU - 1])
                 queue_cache[HV_NCS_QTYPE_MAU - 1] =
-                        kmem_cache_create("cwq_queue",
+                        kmem_cache_create("mau_queue",
                                           (MAU_NUM_ENTRIES *
                                            MAU_ENTRY_SIZE),
                                           MAU_ENTRY_SIZE, 0, NULL);
@@ -1574,7 +1575,7 @@ static int spu_mdesc_walk_arcs(struct mdesc_handle *mdesc,
                 id = mdesc_get_property(mdesc, tgt, "id", NULL);
                 if (table[*id] != NULL) {
                         dev_err(&dev->dev, "%s: SPU cpu slot already set.\n",
-                                dev->node->full_name);
+                                dev->dev.of_node->full_name);
                         return -EINVAL;
                 }
                 cpu_set(*id, p->sharing);
@@ -1595,7 +1596,7 @@ static int handle_exec_unit(struct spu_mdesc_info *ip, struct list_head *list,
         p = kzalloc(sizeof(struct spu_queue), GFP_KERNEL);
         if (!p) {
                 dev_err(&dev->dev, "%s: Could not allocate SPU queue.\n",
-                        dev->node->full_name);
+                        dev->dev.of_node->full_name);
                 return -ENOMEM;
         }
 
@@ -1684,7 +1685,7 @@ static int __devinit grab_mdesc_irq_props(struct mdesc_handle *mdesc,
         const unsigned int *reg;
         u64 node;
 
-        reg = of_get_property(dev->node, "reg", NULL);
+        reg = of_get_property(dev->dev.of_node, "reg", NULL);
         if (!reg)
                 return -ENODEV;
 
@@ -1836,7 +1837,7 @@ static int __devinit n2_crypto_probe(struct of_device *dev,
 
         n2_spu_driver_version();
 
-        full_name = dev->node->full_name;
+        full_name = dev->dev.of_node->full_name;
         pr_info("Found N2CP at %s\n", full_name);
 
         np = alloc_n2cp();
@@ -1948,7 +1949,7 @@ static int __devinit n2_mau_probe(struct of_device *dev,
 
         n2_spu_driver_version();
 
-        full_name = dev->node->full_name;
+        full_name = dev->dev.of_node->full_name;
         pr_info("Found NCP at %s\n", full_name);
 
         mp = alloc_ncp();
@@ -2034,8 +2035,11 @@ static struct of_device_id n2_crypto_match[] = {
 MODULE_DEVICE_TABLE(of, n2_crypto_match);
 
 static struct of_platform_driver n2_crypto_driver = {
-        .name           =       "n2cp",
-        .match_table    =       n2_crypto_match,
+        .driver = {
+                .name           =       "n2cp",
+                .owner          =       THIS_MODULE,
+                .of_match_table =       n2_crypto_match,
+        },
         .probe          =       n2_crypto_probe,
         .remove         =       __devexit_p(n2_crypto_remove),
 };
@@ -2055,8 +2059,11 @@ static struct of_device_id n2_mau_match[] = {
 MODULE_DEVICE_TABLE(of, n2_mau_match);
 
 static struct of_platform_driver n2_mau_driver = {
-        .name           =       "ncp",
-        .match_table    =       n2_mau_match,
+        .driver = {
+                .name           =       "ncp",
+                .owner          =       THIS_MODULE,
+                .of_match_table =       n2_mau_match,
+        },
         .probe          =       n2_mau_probe,
         .remove         =       __devexit_p(n2_mau_remove),
 };
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index 1b8877922fb0..9e01e96fee94 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -166,6 +166,15 @@ config TIMB_DMA
 config ARCH_HAS_ASYNC_TX_FIND_CHANNEL
         bool
 
+config PL330_DMA
+        tristate "DMA API Driver for PL330"
+        select DMA_ENGINE
+        depends on PL330
+        help
+          Select if your platform has one or more PL330 DMACs.
+          You need to provide platform specific settings via
+          platform_data for a dma-pl330 device.
+
 config DMA_ENGINE
         bool
 
diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
index 20881426c1ac..0fe5ebbfda5d 100644
--- a/drivers/dma/Makefile
+++ b/drivers/dma/Makefile
@@ -22,3 +22,4 @@ obj-$(CONFIG_COH901318) += coh901318.o coh901318_lli.o
 obj-$(CONFIG_AMCC_PPC440SPE_ADMA) += ppc4xx/
 obj-$(CONFIG_TIMB_DMA) += timb_dma.o
 obj-$(CONFIG_STE_DMA40) += ste_dma40.o ste_dma40_ll.o
+obj-$(CONFIG_PL330_DMA) += pl330.o
diff --git a/drivers/dma/mpc512x_dma.c b/drivers/dma/mpc512x_dma.c
index 201e6e19c344..14a8c0f1698e 100644
--- a/drivers/dma/mpc512x_dma.c
+++ b/drivers/dma/mpc512x_dma.c
@@ -630,7 +630,7 @@ mpc_dma_prep_memcpy(struct dma_chan *chan, dma_addr_t dst, dma_addr_t src,
 static int __devinit mpc_dma_probe(struct of_device *op,
                                         const struct of_device_id *match)
 {
-        struct device_node *dn = op->node;
+        struct device_node *dn = op->dev.of_node;
         struct device *dev = &op->dev;
         struct dma_device *dma;
         struct mpc_dma *mdma;
@@ -771,12 +771,12 @@ static struct of_device_id mpc_dma_match[] = {
 };
 
 static struct of_platform_driver mpc_dma_driver = {
-        .match_table    = mpc_dma_match,
         .probe          = mpc_dma_probe,
         .remove         = __devexit_p(mpc_dma_remove),
-        .driver         = {
-                .name   = DRV_NAME,
-                .owner  = THIS_MODULE,
+        .driver = {
+                .name = DRV_NAME,
+                .owner = THIS_MODULE,
+                .of_match_table = mpc_dma_match,
         },
 };
 
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
new file mode 100644
index 000000000000..7c50f6dfd3f4
--- /dev/null
+++ b/drivers/dma/pl330.c
@@ -0,0 +1,866 @@
+/* linux/drivers/dma/pl330.c
+ *
+ * Copyright (C) 2010 Samsung Electronics Co. Ltd.
+ *      Jaswinder Singh <jassi.brar@samsung.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/io.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/dmaengine.h>
+#include <linux/interrupt.h>
+#include <linux/amba/bus.h>
+#include <linux/amba/pl330.h>
+
+#define NR_DEFAULT_DESC 16
+
+enum desc_status {
+        /* In the DMAC pool */
+        FREE,
+        /*
+         * Allocted to some channel during prep_xxx
+         * Also may be sitting on the work_list.
+         */
+        PREP,
+        /*
+         * Sitting on the work_list and already submitted
+         * to the PL330 core. Not more than two descriptors
+         * of a channel can be BUSY at any time.
+         */
+        BUSY,
+        /*
+         * Sitting on the channel work_list but xfer done
+         * by PL330 core
+         */
+        DONE,
+};
+
+struct dma_pl330_chan {
+        /* Schedule desc completion */
+        struct tasklet_struct task;
+
+        /* DMA-Engine Channel */
+        struct dma_chan chan;
+
+        /* Last completed cookie */
+        dma_cookie_t completed;
+
+        /* List of to be xfered descriptors */
+        struct list_head work_list;
+
+        /* Pointer to the DMAC that manages this channel,
+         * NULL if the channel is available to be acquired.
+         * As the parent, this DMAC also provides descriptors
+         * to the channel.
+         */
+        struct dma_pl330_dmac *dmac;
+
+        /* To protect channel manipulation */
+        spinlock_t lock;
+
+        /* Token of a hardware channel thread of PL330 DMAC
+         * NULL if the channel is available to be acquired.
+         */
+        void *pl330_chid;
+};
+
+struct dma_pl330_dmac {
+        struct pl330_info pif;
+
+        /* DMA-Engine Device */
+        struct dma_device ddma;
+
+        /* Pool of descriptors available for the DMAC's channels */
+        struct list_head desc_pool;
+        /* To protect desc_pool manipulation */
+        spinlock_t pool_lock;
+
+        /* Peripheral channels connected to this DMAC */
+        struct dma_pl330_chan peripherals[0]; /* keep at end */
+};
+
+struct dma_pl330_desc {
+        /* To attach to a queue as child */
+        struct list_head node;
+
+        /* Descriptor for the DMA Engine API */
+        struct dma_async_tx_descriptor txd;
+
+        /* Xfer for PL330 core */
+        struct pl330_xfer px;
+
+        struct pl330_reqcfg rqcfg;
+        struct pl330_req req;
+
+        enum desc_status status;
+
+        /* The channel which currently holds this desc */
+        struct dma_pl330_chan *pchan;
+};
+
+static inline struct dma_pl330_chan *
+to_pchan(struct dma_chan *ch)
+{
+        if (!ch)
+                return NULL;
+
+        return container_of(ch, struct dma_pl330_chan, chan);
+}
+
+static inline struct dma_pl330_desc *
+to_desc(struct dma_async_tx_descriptor *tx)
+{
+        return container_of(tx, struct dma_pl330_desc, txd);
+}
+
+static inline void free_desc_list(struct list_head *list)
+{
+        struct dma_pl330_dmac *pdmac;
+        struct dma_pl330_desc *desc;
+        struct dma_pl330_chan *pch;
+        unsigned long flags;
+
+        if (list_empty(list))
+                return;
+
+        /* Finish off the work list */
+        list_for_each_entry(desc, list, node) {
+                dma_async_tx_callback callback;
+                void *param;
+
+                /* All desc in a list belong to same channel */
+                pch = desc->pchan;
+                callback = desc->txd.callback;
+                param = desc->txd.callback_param;
+
+                if (callback)
+                        callback(param);
+
+                desc->pchan = NULL;
+        }
+
+        pdmac = pch->dmac;
+
+        spin_lock_irqsave(&pdmac->pool_lock, flags);
+        list_splice_tail_init(list, &pdmac->desc_pool);
+        spin_unlock_irqrestore(&pdmac->pool_lock, flags);
+}
+
+static inline void fill_queue(struct dma_pl330_chan *pch)
+{
+        struct dma_pl330_desc *desc;
+        int ret;
+
+        list_for_each_entry(desc, &pch->work_list, node) {
+
+                /* If already submitted */
+                if (desc->status == BUSY)
+                        break;
+
+                ret = pl330_submit_req(pch->pl330_chid,
+                                                &desc->req);
+                if (!ret) {
+                        desc->status = BUSY;
+                        break;
+                } else if (ret == -EAGAIN) {
+                        /* QFull or DMAC Dying */
+                        break;
+                } else {
+                        /* Unacceptable request */
+                        desc->status = DONE;
+                        dev_err(pch->dmac->pif.dev, "%s:%d Bad Desc(%d)\n",
+                                        __func__, __LINE__, desc->txd.cookie);
+                        tasklet_schedule(&pch->task);
+                }
+        }
+}
+
+static void pl330_tasklet(unsigned long data)
+{
+        struct dma_pl330_chan *pch = (struct dma_pl330_chan *)data;
+        struct dma_pl330_desc *desc, *_dt;
+        unsigned long flags;
+        LIST_HEAD(list);
+
+        spin_lock_irqsave(&pch->lock, flags);
+
+        /* Pick up ripe tomatoes */
+        list_for_each_entry_safe(desc, _dt, &pch->work_list, node)
+                if (desc->status == DONE) {
+                        pch->completed = desc->txd.cookie;
+                        list_move_tail(&desc->node, &list);
+                }
+
+        /* Try to submit a req imm. next to the last completed cookie */
+        fill_queue(pch);
+
+        /* Make sure the PL330 Channel thread is active */
+        pl330_chan_ctrl(pch->pl330_chid, PL330_OP_START);
+
+        spin_unlock_irqrestore(&pch->lock, flags);
+
+        free_desc_list(&list);
+}
+
+static void dma_pl330_rqcb(void *token, enum pl330_op_err err)
+{
+        struct dma_pl330_desc *desc = token;
+        struct dma_pl330_chan *pch = desc->pchan;
+        unsigned long flags;
+
+        /* If desc aborted */
+        if (!pch)
+                return;
+
+        spin_lock_irqsave(&pch->lock, flags);
+
+        desc->status = DONE;
+
+        spin_unlock_irqrestore(&pch->lock, flags);
+
+        tasklet_schedule(&pch->task);
+}
+
+static int pl330_alloc_chan_resources(struct dma_chan *chan)
+{
+        struct dma_pl330_chan *pch = to_pchan(chan);
+        struct dma_pl330_dmac *pdmac = pch->dmac;
+        unsigned long flags;
+
+        spin_lock_irqsave(&pch->lock, flags);
+
+        pch->completed = chan->cookie = 1;
+
+        pch->pl330_chid = pl330_request_channel(&pdmac->pif);
+        if (!pch->pl330_chid) {
+                spin_unlock_irqrestore(&pch->lock, flags);
+                return 0;
+        }
+
+        tasklet_init(&pch->task, pl330_tasklet, (unsigned long) pch);
+
+        spin_unlock_irqrestore(&pch->lock, flags);
+
+        return 1;
+}
+
+static int pl330_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd, unsigned long arg)
+{
+        struct dma_pl330_chan *pch = to_pchan(chan);
+        struct dma_pl330_desc *desc;
+        unsigned long flags;
+
+        /* Only supports DMA_TERMINATE_ALL */
+        if (cmd != DMA_TERMINATE_ALL)
+                return -ENXIO;
+
+        spin_lock_irqsave(&pch->lock, flags);
+
+        /* FLUSH the PL330 Channel thread */
+        pl330_chan_ctrl(pch->pl330_chid, PL330_OP_FLUSH);
+
+        /* Mark all desc done */
+        list_for_each_entry(desc, &pch->work_list, node)
+                desc->status = DONE;
+
+        spin_unlock_irqrestore(&pch->lock, flags);
+
+        pl330_tasklet((unsigned long) pch);
+
+        return 0;
+}
+
+static void pl330_free_chan_resources(struct dma_chan *chan)
+{
+        struct dma_pl330_chan *pch = to_pchan(chan);
+        unsigned long flags;
+
+        spin_lock_irqsave(&pch->lock, flags);
+
+        tasklet_kill(&pch->task);
+
+        pl330_release_channel(pch->pl330_chid);
+        pch->pl330_chid = NULL;
+
+        spin_unlock_irqrestore(&pch->lock, flags);
+}
+
+static enum dma_status
+pl330_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
+                 struct dma_tx_state *txstate)
+{
+        struct dma_pl330_chan *pch = to_pchan(chan);
+        dma_cookie_t last_done, last_used;
+        int ret;
+
+        last_done = pch->completed;
+        last_used = chan->cookie;
+
+        ret = dma_async_is_complete(cookie, last_done, last_used);
+
+        dma_set_tx_state(txstate, last_done, last_used, 0);
+
+        return ret;
+}
+
+static void pl330_issue_pending(struct dma_chan *chan)
+{
+        pl330_tasklet((unsigned long) to_pchan(chan));
+}
+
+/*
+ * We returned the last one of the circular list of descriptor(s)
+ * from prep_xxx, so the argument to submit corresponds to the last
+ * descriptor of the list.
+ */
+static dma_cookie_t pl330_tx_submit(struct dma_async_tx_descriptor *tx)
+{
+        struct dma_pl330_desc *desc, *last = to_desc(tx);
+        struct dma_pl330_chan *pch = to_pchan(tx->chan);
+        dma_cookie_t cookie;
+        unsigned long flags;
+
+        spin_lock_irqsave(&pch->lock, flags);
+
+        /* Assign cookies to all nodes */
+        cookie = tx->chan->cookie;
+
+        while (!list_empty(&last->node)) {
+                desc = list_entry(last->node.next, struct dma_pl330_desc, node);
+
+                if (++cookie < 0)
+                        cookie = 1;
+                desc->txd.cookie = cookie;
+
+                list_move_tail(&desc->node, &pch->work_list);
+        }
+
+        if (++cookie < 0)
+                cookie = 1;
+        last->txd.cookie = cookie;
+
+        list_add_tail(&last->node, &pch->work_list);
+
+        tx->chan->cookie = cookie;
+
+        spin_unlock_irqrestore(&pch->lock, flags);
+
+        return cookie;
+}
+
+static inline void _init_desc(struct dma_pl330_desc *desc)
+{
+        desc->pchan = NULL;
+        desc->req.x = &desc->px;
+        desc->req.token = desc;
+        desc->rqcfg.swap = SWAP_NO;
+        desc->rqcfg.privileged = 0;
+        desc->rqcfg.insnaccess = 0;
+        desc->rqcfg.scctl = SCCTRL0;
+        desc->rqcfg.dcctl = DCCTRL0;
+        desc->req.cfg = &desc->rqcfg;
+        desc->req.xfer_cb = dma_pl330_rqcb;
+        desc->txd.tx_submit = pl330_tx_submit;
+
+        INIT_LIST_HEAD(&desc->node);
+}
+
+/* Returns the number of descriptors added to the DMAC pool */
+int add_desc(struct dma_pl330_dmac *pdmac, gfp_t flg, int count)
+{
+        struct dma_pl330_desc *desc;
+        unsigned long flags;
+        int i;
+
+        if (!pdmac)
+                return 0;
+
+        desc = kmalloc(count * sizeof(*desc), flg);
+        if (!desc)
+                return 0;
+
+        spin_lock_irqsave(&pdmac->pool_lock, flags);
+
+        for (i = 0; i < count; i++) {
+                _init_desc(&desc[i]);
+                list_add_tail(&desc[i].node, &pdmac->desc_pool);
+        }
+
+        spin_unlock_irqrestore(&pdmac->pool_lock, flags);
+
+        return count;
+}
+
+static struct dma_pl330_desc *
+pluck_desc(struct dma_pl330_dmac *pdmac)
+{
+        struct dma_pl330_desc *desc = NULL;
+        unsigned long flags;
+
+        if (!pdmac)
+                return NULL;
+
+        spin_lock_irqsave(&pdmac->pool_lock, flags);
+
+        if (!list_empty(&pdmac->desc_pool)) {
+                desc = list_entry(pdmac->desc_pool.next,
+                                struct dma_pl330_desc, node);
+
+                list_del_init(&desc->node);
+
+                desc->status = PREP;
+                desc->txd.callback = NULL;
+        }
+
+        spin_unlock_irqrestore(&pdmac->pool_lock, flags);
+
+        return desc;
+}
+
+static struct dma_pl330_desc *pl330_get_desc(struct dma_pl330_chan *pch)
+{
+        struct dma_pl330_dmac *pdmac = pch->dmac;
+        struct dma_pl330_peri *peri = pch->chan.private;
+        struct dma_pl330_desc *desc;
+
+        /* Pluck one desc from the pool of DMAC */
+        desc = pluck_desc(pdmac);
+
+        /* If the DMAC pool is empty, alloc new */
+        if (!desc) {
+                if (!add_desc(pdmac, GFP_ATOMIC, 1))
+                        return NULL;
+
+                /* Try again */
+                desc = pluck_desc(pdmac);
+                if (!desc) {
+                        dev_err(pch->dmac->pif.dev,
+                                "%s:%d ALERT!\n", __func__, __LINE__);
+                        return NULL;
+                }
+        }
+
+        /* Initialize the descriptor */
+        desc->pchan = pch;
+        desc->txd.cookie = 0;
+        async_tx_ack(&desc->txd);
+
+        desc->req.rqtype = peri->rqtype;
+        desc->req.peri = peri->peri_id;
+
+        dma_async_tx_descriptor_init(&desc->txd, &pch->chan);
+
+        return desc;
+}
+
+static inline void fill_px(struct pl330_xfer *px,
+                dma_addr_t dst, dma_addr_t src, size_t len)
+{
+        px->next = NULL;
+        px->bytes = len;
+        px->dst_addr = dst;
+        px->src_addr = src;
+}
+
+static struct dma_pl330_desc *
+__pl330_prep_dma_memcpy(struct dma_pl330_chan *pch, dma_addr_t dst,
+                dma_addr_t src, size_t len)
+{
+        struct dma_pl330_desc *desc = pl330_get_desc(pch);
+
+        if (!desc) {
+                dev_err(pch->dmac->pif.dev, "%s:%d Unable to fetch desc\n",
+                        __func__, __LINE__);
+                return NULL;
+        }
+
+        /*
+         * Ideally we should lookout for reqs bigger than
+         * those that can be programmed with 256 bytes of
+         * MC buffer, but considering a req size is seldom
+         * going to be word-unaligned and more than 200MB,
+         * we take it easy.
+         * Also, should the limit is reached we'd rather
+         * have the platform increase MC buffer size than
+         * complicating this API driver.
+         */
+        fill_px(&desc->px, dst, src, len);
+
+        return desc;
+}
+
+/* Call after fixing burst size */
+static inline int get_burst_len(struct dma_pl330_desc *desc, size_t len)
+{
+        struct dma_pl330_chan *pch = desc->pchan;
+        struct pl330_info *pi = &pch->dmac->pif;
+        int burst_len;
+
+        burst_len = pi->pcfg.data_bus_width / 8;
+        burst_len *= pi->pcfg.data_buf_dep;
+        burst_len >>= desc->rqcfg.brst_size;
+
+        /* src/dst_burst_len can't be more than 16 */
+        if (burst_len > 16)
+                burst_len = 16;
+
+        while (burst_len > 1) {
+                if (!(len % (burst_len << desc->rqcfg.brst_size)))
+                        break;
+                burst_len--;
+        }
+
+        return burst_len;
+}
+
+static struct dma_async_tx_descriptor *
+pl330_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dst,
+                dma_addr_t src, size_t len, unsigned long flags)
+{
+        struct dma_pl330_desc *desc;
+        struct dma_pl330_chan *pch = to_pchan(chan);
+        struct dma_pl330_peri *peri = chan->private;
+        struct pl330_info *pi;
+        int burst;
+
+        if (unlikely(!pch || !len || !peri))
+                return NULL;
+
+        if (peri->rqtype != MEMTOMEM)
+                return NULL;
+
+        pi = &pch->dmac->pif;
+
+        desc = __pl330_prep_dma_memcpy(pch, dst, src, len);
+        if (!desc)
+                return NULL;
+
+        desc->rqcfg.src_inc = 1;
+        desc->rqcfg.dst_inc = 1;
+
+        /* Select max possible burst size */
+        burst = pi->pcfg.data_bus_width / 8;
+
+        while (burst > 1) {
+                if (!(len % burst))
+                        break;
+                burst /= 2;
+        }
+
+        desc->rqcfg.brst_size = 0;
+        while (burst != (1 << desc->rqcfg.brst_size))
+                desc->rqcfg.brst_size++;
+
+        desc->rqcfg.brst_len = get_burst_len(desc, len);
+
+        desc->txd.flags = flags;
+
+        return &desc->txd;
+}
+
+static struct dma_async_tx_descriptor *
+pl330_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
+                unsigned int sg_len, enum dma_data_direction direction,
+                unsigned long flg)
+{
+        struct dma_pl330_desc *first, *desc = NULL;
+        struct dma_pl330_chan *pch = to_pchan(chan);
+        struct dma_pl330_peri *peri = chan->private;
+        struct scatterlist *sg;
+        unsigned long flags;
+        int i, burst_size;
+        dma_addr_t addr;
+
+        if (unlikely(!pch || !sgl || !sg_len))
+                return NULL;
+
+        /* Make sure the direction is consistent */
+        if ((direction == DMA_TO_DEVICE &&
+                                peri->rqtype != MEMTODEV) ||
+                        (direction == DMA_FROM_DEVICE &&
+                                peri->rqtype != DEVTOMEM)) {
+                dev_err(pch->dmac->pif.dev, "%s:%d Invalid Direction\n",
+                                __func__, __LINE__);
+                return NULL;
+        }
+
+        addr = peri->fifo_addr;
+        burst_size = peri->burst_sz;
+
+        first = NULL;
+
+        for_each_sg(sgl, sg, sg_len, i) {
+
+                desc = pl330_get_desc(pch);
+                if (!desc) {
+                        struct dma_pl330_dmac *pdmac = pch->dmac;
+
+                        dev_err(pch->dmac->pif.dev,
+                                "%s:%d Unable to fetch desc\n",
+                                __func__, __LINE__);
+                        if (!first)
+                                return NULL;
+
+                        spin_lock_irqsave(&pdmac->pool_lock, flags);
+
+                        while (!list_empty(&first->node)) {
+                                desc = list_entry(first->node.next,
+                                                struct dma_pl330_desc, node);
+                                list_move_tail(&desc->node, &pdmac->desc_pool);
+                        }
+
+                        list_move_tail(&first->node, &pdmac->desc_pool);
+
+                        spin_unlock_irqrestore(&pdmac->pool_lock, flags);
+
+                        return NULL;
+                }
+
+                if (!first)
+                        first = desc;
+                else
+                        list_add_tail(&desc->node, &first->node);
+
+                if (direction == DMA_TO_DEVICE) {
+                        desc->rqcfg.src_inc = 1;
+                        desc->rqcfg.dst_inc = 0;
+                        fill_px(&desc->px,
+                                addr, sg_dma_address(sg), sg_dma_len(sg));
+                } else {
+                        desc->rqcfg.src_inc = 0;
+                        desc->rqcfg.dst_inc = 1;
+                        fill_px(&desc->px,
+                                sg_dma_address(sg), addr, sg_dma_len(sg));
+                }
+
+                desc->rqcfg.brst_size = burst_size;
+                desc->rqcfg.brst_len = 1;
+        }
+
+        /* Return the last desc in the chain */
+        desc->txd.flags = flg;
+        return &desc->txd;
+}
+
+static irqreturn_t pl330_irq_handler(int irq, void *data)
+{
+        if (pl330_update(data))
+                return IRQ_HANDLED;
+        else
+                return IRQ_NONE;
+}
+
+static int __devinit
+pl330_probe(struct amba_device *adev, struct amba_id *id)
+{
+        struct dma_pl330_platdata *pdat;
+        struct dma_pl330_dmac *pdmac;
+        struct dma_pl330_chan *pch;
+        struct pl330_info *pi;
+        struct dma_device *pd;
+        struct resource *res;
+        int i, ret, irq;
+
+        pdat = adev->dev.platform_data;
+
+        if (!pdat || !pdat->nr_valid_peri) {
+                dev_err(&adev->dev, "platform data missing\n");
+                return -ENODEV;
+        }
+
+        /* Allocate a new DMAC and its Channels */
+        pdmac = kzalloc(pdat->nr_valid_peri * sizeof(*pch)
+                                + sizeof(*pdmac), GFP_KERNEL);
+        if (!pdmac) {
+                dev_err(&adev->dev, "unable to allocate mem\n");
+                return -ENOMEM;
+        }
+
+        pi = &pdmac->pif;
+        pi->dev = &adev->dev;
+        pi->pl330_data = NULL;
+        pi->mcbufsz = pdat->mcbuf_sz;
+
+        res = &adev->res;
+        request_mem_region(res->start, resource_size(res), "dma-pl330");
+
+        pi->base = ioremap(res->start, resource_size(res));
+        if (!pi->base) {
+                ret = -ENXIO;
+                goto probe_err1;
+        }
+
+        irq = adev->irq[0];
+        ret = request_irq(irq, pl330_irq_handler, 0,
+                        dev_name(&adev->dev), pi);
+        if (ret)
+                goto probe_err2;
+
+        ret = pl330_add(pi);
+        if (ret)
+                goto probe_err3;
+
+        INIT_LIST_HEAD(&pdmac->desc_pool);
+        spin_lock_init(&pdmac->pool_lock);
+
+        /* Create a descriptor pool of default size */
+        if (!add_desc(pdmac, GFP_KERNEL, NR_DEFAULT_DESC))
+                dev_warn(&adev->dev, "unable to allocate desc\n");
+
+        pd = &pdmac->ddma;
+        INIT_LIST_HEAD(&pd->channels);
+
+        /* Initialize channel parameters */
+        for (i = 0; i < pdat->nr_valid_peri; i++) {
+                struct dma_pl330_peri *peri = &pdat->peri[i];
+                pch = &pdmac->peripherals[i];
+
+                switch (peri->rqtype) {
+                case MEMTOMEM:
+                        dma_cap_set(DMA_MEMCPY, pd->cap_mask);
+                        break;
+                case MEMTODEV:
+                case DEVTOMEM:
+                        dma_cap_set(DMA_SLAVE, pd->cap_mask);
+                        break;
+                default:
+                        dev_err(&adev->dev, "DEVTODEV Not Supported\n");
+                        continue;
+                }
+
+                INIT_LIST_HEAD(&pch->work_list);
+                spin_lock_init(&pch->lock);
+                pch->pl330_chid = NULL;
+                pch->chan.private = peri;
+                pch->chan.device = pd;
+                pch->chan.chan_id = i;
+                pch->dmac = pdmac;
+
+                /* Add the channel to the DMAC list */
+                pd->chancnt++;
+                list_add_tail(&pch->chan.device_node, &pd->channels);
+        }
+
+        pd->dev = &adev->dev;
+
+        pd->device_alloc_chan_resources = pl330_alloc_chan_resources;
+        pd->device_free_chan_resources = pl330_free_chan_resources;
+        pd->device_prep_dma_memcpy = pl330_prep_dma_memcpy;
+        pd->device_tx_status = pl330_tx_status;
+        pd->device_prep_slave_sg = pl330_prep_slave_sg;
+        pd->device_control = pl330_control;
+        pd->device_issue_pending = pl330_issue_pending;
+
+        ret = dma_async_device_register(pd);
+        if (ret) {
+                dev_err(&adev->dev, "unable to register DMAC\n");
+                goto probe_err4;
+        }
+
+        amba_set_drvdata(adev, pdmac);
+
+        dev_info(&adev->dev,
+                "Loaded driver for PL330 DMAC-%d\n", adev->periphid);
+        dev_info(&adev->dev,
+                "\tDBUFF-%ux%ubytes Num_Chans-%u Num_Peri-%u Num_Events-%u\n",
+                pi->pcfg.data_buf_dep,
+                pi->pcfg.data_bus_width / 8, pi->pcfg.num_chan,
+                pi->pcfg.num_peri, pi->pcfg.num_events);
+
+        return 0;
+
+probe_err4:
+        pl330_del(pi);
+probe_err3:
+        free_irq(irq, pi);
+probe_err2:
+        iounmap(pi->base);
+probe_err1:
+        release_mem_region(res->start, resource_size(res));
+        kfree(pdmac);
+
+        return ret;
+}
+
+static int __devexit pl330_remove(struct amba_device *adev)
+{
+        struct dma_pl330_dmac *pdmac = amba_get_drvdata(adev);
+        struct dma_pl330_chan *pch, *_p;
+        struct pl330_info *pi;
+        struct resource *res;
+        int irq;
+
+        if (!pdmac)
+                return 0;
+
+        amba_set_drvdata(adev, NULL);
+
+        /* Idle the DMAC */
+        list_for_each_entry_safe(pch, _p, &pdmac->ddma.channels,
+                        chan.device_node) {
+
+                /* Remove the channel */
+                list_del(&pch->chan.device_node);
+
+                /* Flush the channel */
+                pl330_control(&pch->chan, DMA_TERMINATE_ALL, 0);
+                pl330_free_chan_resources(&pch->chan);
+        }
+
+        pi = &pdmac->pif;
+
+        pl330_del(pi);
+
+        irq = adev->irq[0];
+        free_irq(irq, pi);
+
+        iounmap(pi->base);
+
+        res = &adev->res;
+        release_mem_region(res->start, resource_size(res));
+
+        kfree(pdmac);
+
+        return 0;
+}
+
+static struct amba_id pl330_ids[] = {
+        {
+                .id     = 0x00041330,
+                .mask   = 0x000fffff,
+        },
+        { 0, 0 },
+};
+
+static struct amba_driver pl330_driver = {
+        .drv = {
+                .owner = THIS_MODULE,
+                .name = "dma-pl330",
+        },
+        .id_table = pl330_ids,
+        .probe = pl330_probe,
+        .remove = pl330_remove,
+};
+
+static int __init pl330_init(void)
+{
+        return amba_driver_register(&pl330_driver);
+}
+module_init(pl330_init);
+
+static void __exit pl330_exit(void)
+{
+        amba_driver_unregister(&pl330_driver);
+        return;
+}
+module_exit(pl330_exit);
+
+MODULE_AUTHOR("Jaswinder Singh <jassi.brar@samsung.com>");
+MODULE_DESCRIPTION("API Driver for PL330 DMAC");
+MODULE_LICENSE("GPL");
diff --git a/drivers/dma/ppc4xx/adma.c b/drivers/dma/ppc4xx/adma.c
index fa98abe4686f..5a22ca6927e5 100644
--- a/drivers/dma/ppc4xx/adma.c
+++ b/drivers/dma/ppc4xx/adma.c
@@ -4394,7 +4394,7 @@ static void ppc440spe_adma_release_irqs(struct ppc440spe_adma_device *adev,
 static int __devinit ppc440spe_adma_probe(struct of_device *ofdev,
                                           const struct of_device_id *match)
 {
-        struct device_node *np = ofdev->node;
+        struct device_node *np = ofdev->dev.of_node;
         struct resource res;
         struct ppc440spe_adma_device *adev;
         struct ppc440spe_adma_chan *chan;
@@ -4626,7 +4626,7 @@ out:
 static int __devexit ppc440spe_adma_remove(struct of_device *ofdev)
 {
         struct ppc440spe_adma_device *adev = dev_get_drvdata(&ofdev->dev);
-        struct device_node *np = ofdev->node;
+        struct device_node *np = ofdev->dev.of_node;
         struct resource res;
         struct dma_chan *chan, *_chan;
         struct ppc_dma_chan_ref *ref, *_ref;
diff --git a/drivers/edac/Kconfig b/drivers/edac/Kconfig
index 55c9c59b3f71..aedef7941b22 100644
--- a/drivers/edac/Kconfig
+++ b/drivers/edac/Kconfig
@@ -69,6 +69,9 @@ config EDAC_MM_EDAC
           occurred so that a particular failing memory module can be
           replaced.  If unsure, select 'Y'.
 
+config EDAC_MCE
+        bool
+
 config EDAC_AMD64
         tristate "AMD64 (Opteron, Athlon64) K8, F10h, F11h"
         depends on EDAC_MM_EDAC && K8_NB && X86_64 && PCI && EDAC_DECODE_MCE
@@ -166,6 +169,16 @@ config EDAC_I5400
           Support for error detection and correction the Intel
           i5400 MCH chipset (Seaburg).
 
+config EDAC_I7CORE
+        tristate "Intel i7 Core (Nehalem) processors"
+        depends on EDAC_MM_EDAC && PCI && X86
+        select EDAC_MCE
+        help
+          Support for error detection and correction the Intel
+          i7 Core (Nehalem) Integrated Memory Controller that exists on
+          newer processors like i7 Core, i7 Core Extreme, Xeon 35xx
+          and Xeon 55xx processors.
+
 config EDAC_I82860
         tristate "Intel 82860"
         depends on EDAC_MM_EDAC && PCI && X86_32
diff --git a/drivers/edac/Makefile b/drivers/edac/Makefile
index bc5dc232a0fb..ca6b1bb24ccc 100644
--- a/drivers/edac/Makefile
+++ b/drivers/edac/Makefile
@@ -8,6 +8,7 @@
 
 obj-$(CONFIG_EDAC)                      := edac_stub.o
 obj-$(CONFIG_EDAC_MM_EDAC)              += edac_core.o
+obj-$(CONFIG_EDAC_MCE)                  += edac_mce.o
 
 edac_core-objs  := edac_mc.o edac_device.o edac_mc_sysfs.o edac_pci_sysfs.o
 edac_core-objs  += edac_module.o edac_device_sysfs.o
@@ -23,6 +24,7 @@ obj-$(CONFIG_EDAC_CPC925)		+= cpc925_edac.o
 obj-$(CONFIG_EDAC_I5000)                += i5000_edac.o
 obj-$(CONFIG_EDAC_I5100)                += i5100_edac.o
 obj-$(CONFIG_EDAC_I5400)                += i5400_edac.o
+obj-$(CONFIG_EDAC_I7CORE)               += i7core_edac.o
 obj-$(CONFIG_EDAC_E7XXX)                += e7xxx_edac.o
 obj-$(CONFIG_EDAC_E752X)                += e752x_edac.o
 obj-$(CONFIG_EDAC_I82443BXGX)           += i82443bxgx_edac.o
diff --git a/drivers/edac/edac_core.h b/drivers/edac/edac_core.h
index 001b2e797fb3..efca9343d26a 100644
--- a/drivers/edac/edac_core.h
+++ b/drivers/edac/edac_core.h
@@ -341,12 +341,30 @@ struct csrow_info {
         struct channel_info *channels;
 };
 
+struct mcidev_sysfs_group {
+        const char *name;                               /* group name */
+        struct mcidev_sysfs_attribute *mcidev_attr;     /* group attributes */
+};
+
+struct mcidev_sysfs_group_kobj {
+        struct list_head list;          /* list for all instances within a mc */
+
+        struct kobject kobj;            /* kobj for the group */
+
+        struct mcidev_sysfs_group *grp; /* group description table */
+        struct mem_ctl_info *mci;       /* the parent */
+};
+
 /* mcidev_sysfs_attribute structure
  *      used for driver sysfs attributes and in mem_ctl_info
  *      sysfs top level entries
  */
 struct mcidev_sysfs_attribute {
-        struct attribute attr;
+        /* It should use either attr or grp */
+        struct attribute attr;
+        struct mcidev_sysfs_group *grp; /* Points to a group of attributes */
+
+        /* Ops for show/store values at the attribute - not used on group */
         ssize_t (*show)(struct mem_ctl_info *,char *);
         ssize_t (*store)(struct mem_ctl_info *, const char *,size_t);
 };
@@ -424,6 +442,9 @@ struct mem_ctl_info {
         /* edac sysfs device control */
         struct kobject edac_mci_kobj;
 
+        /* list for all grp instances within a mc */
+        struct list_head grp_kobj_list;
+
         /* Additional top controller level attributes, but specified
          * by the low level driver.
          *
diff --git a/drivers/edac/edac_mc_sysfs.c b/drivers/edac/edac_mc_sysfs.c
index 418b65f1a1da..c200c2fd43ea 100644
--- a/drivers/edac/edac_mc_sysfs.c
+++ b/drivers/edac/edac_mc_sysfs.c
@@ -557,6 +557,8 @@ static ssize_t mcidev_show(struct kobject *kobj, struct attribute *attr,
         struct mem_ctl_info *mem_ctl_info = to_mci(kobj);
         struct mcidev_sysfs_attribute *mcidev_attr = to_mcidev_attr(attr);
 
+        debugf1("%s() mem_ctl_info %p\n", __func__, mem_ctl_info);
+
         if (mcidev_attr->show)
                 return mcidev_attr->show(mem_ctl_info, buffer);
 
@@ -569,6 +571,8 @@ static ssize_t mcidev_store(struct kobject *kobj, struct attribute *attr,
         struct mem_ctl_info *mem_ctl_info = to_mci(kobj);
         struct mcidev_sysfs_attribute *mcidev_attr = to_mcidev_attr(attr);
 
+        debugf1("%s() mem_ctl_info %p\n", __func__, mem_ctl_info);
+
         if (mcidev_attr->store)
                 return mcidev_attr->store(mem_ctl_info, buffer, count);
 
@@ -726,28 +730,118 @@ void edac_mc_unregister_sysfs_main_kobj(struct mem_ctl_info *mci)
 
 #define EDAC_DEVICE_SYMLINK     "device"
 
+#define grp_to_mci(k) (container_of(k, struct mcidev_sysfs_group_kobj, kobj)->mci)
+
+/* MCI show/store functions for top most object */
+static ssize_t inst_grp_show(struct kobject *kobj, struct attribute *attr,
+                        char *buffer)
+{
+        struct mem_ctl_info *mem_ctl_info = grp_to_mci(kobj);
+        struct mcidev_sysfs_attribute *mcidev_attr = to_mcidev_attr(attr);
+
+        debugf1("%s() mem_ctl_info %p\n", __func__, mem_ctl_info);
+
+        if (mcidev_attr->show)
+                return mcidev_attr->show(mem_ctl_info, buffer);
+
+        return -EIO;
+}
+
+static ssize_t inst_grp_store(struct kobject *kobj, struct attribute *attr,
+                        const char *buffer, size_t count)
+{
+        struct mem_ctl_info *mem_ctl_info = grp_to_mci(kobj);
+        struct mcidev_sysfs_attribute *mcidev_attr = to_mcidev_attr(attr);
+
+        debugf1("%s() mem_ctl_info %p\n", __func__, mem_ctl_info);
+
+        if (mcidev_attr->store)
+                return mcidev_attr->store(mem_ctl_info, buffer, count);
+
+        return -EIO;
+}
+
+/* No memory to release for this kobj */
+static void edac_inst_grp_release(struct kobject *kobj)
+{
+        struct mcidev_sysfs_group_kobj *grp;
+        struct mem_ctl_info *mci;
+
+        debugf1("%s()\n", __func__);
+
+        grp = container_of(kobj, struct mcidev_sysfs_group_kobj, kobj);
+        mci = grp->mci;
+
+        kobject_put(&mci->edac_mci_kobj);
+}
+
+/* Intermediate show/store table */
+static struct sysfs_ops inst_grp_ops = {
+        .show = inst_grp_show,
+        .store = inst_grp_store
+};
+
+/* the kobj_type instance for a instance group */
+static struct kobj_type ktype_inst_grp = {
+        .release = edac_inst_grp_release,
+        .sysfs_ops = &inst_grp_ops,
+};
+
+
 /*
  * edac_create_mci_instance_attributes
- *      create MC driver specific attributes at the topmost level
- *      directory of this mci instance.
+ *      create MC driver specific attributes bellow an specified kobj
+ * This routine calls itself recursively, in order to create an entire
+ * object tree.
  */
-static int edac_create_mci_instance_attributes(struct mem_ctl_info *mci)
+static int edac_create_mci_instance_attributes(struct mem_ctl_info *mci,
+                                struct mcidev_sysfs_attribute *sysfs_attrib,
+                                struct kobject *kobj)
 {
         int err;
-        struct mcidev_sysfs_attribute *sysfs_attrib;
 
-        /* point to the start of the array and iterate over it
-         * adding each attribute listed to this mci instance's kobject
-         */
-        sysfs_attrib = mci->mc_driver_sysfs_attributes;
+        debugf1("%s()\n", __func__);
+
+        while (sysfs_attrib) {
+                if (sysfs_attrib->grp) {
+                        struct mcidev_sysfs_group_kobj *grp_kobj;
+
+                        grp_kobj = kzalloc(sizeof(*grp_kobj), GFP_KERNEL);
+                        if (!grp_kobj)
+                                return -ENOMEM;
+
+                        list_add_tail(&grp_kobj->list, &mci->grp_kobj_list);
+
+                        grp_kobj->grp = sysfs_attrib->grp;
+                        grp_kobj->mci = mci;
+
+                        debugf0("%s() grp %s, mci %p\n", __func__,
+                                sysfs_attrib->grp->name, mci);
+
+                        err = kobject_init_and_add(&grp_kobj->kobj,
+                                                &ktype_inst_grp,
+                                                &mci->edac_mci_kobj,
+                                                sysfs_attrib->grp->name);
+                        if (err)
+                                return err;
+
+                        err = edac_create_mci_instance_attributes(mci,
+                                        grp_kobj->grp->mcidev_attr,
+                                        &grp_kobj->kobj);
+
+                        if (err)
+                                return err;
+                } else if (sysfs_attrib->attr.name) {
+                        debugf0("%s() file %s\n", __func__,
+                                sysfs_attrib->attr.name);
+
+                        err = sysfs_create_file(kobj, &sysfs_attrib->attr);
+                } else
+                        break;
 
-        while (sysfs_attrib && sysfs_attrib->attr.name) {
-                err = sysfs_create_file(&mci->edac_mci_kobj,
-                                        (struct attribute*) sysfs_attrib);
                 if (err) {
                         return err;
                 }
-
                 sysfs_attrib++;
         }
 
@@ -759,21 +853,44 @@ static int edac_create_mci_instance_attributes(struct mem_ctl_info *mci)
  *      remove MC driver specific attributes at the topmost level
  *      directory of this mci instance.
  */
-static void edac_remove_mci_instance_attributes(struct mem_ctl_info *mci)
+static void edac_remove_mci_instance_attributes(struct mem_ctl_info *mci,
+                                struct mcidev_sysfs_attribute *sysfs_attrib,
+                                struct kobject *kobj, int count)
 {
-        struct mcidev_sysfs_attribute *sysfs_attrib;
+        struct mcidev_sysfs_group_kobj *grp_kobj, *tmp;
 
-        /* point to the start of the array and iterate over it
-         * adding each attribute listed to this mci instance's kobject
-         */
-        sysfs_attrib = mci->mc_driver_sysfs_attributes;
+        debugf1("%s()\n", __func__);
 
-        /* loop if there are attributes and until we hit a NULL entry */
-        while (sysfs_attrib && sysfs_attrib->attr.name) {
-                sysfs_remove_file(&mci->edac_mci_kobj,
-                                        (struct attribute *) sysfs_attrib);
+        /*
+         * loop if there are attributes and until we hit a NULL entry
+         * Remove first all the atributes
+         */
+        while (sysfs_attrib) {
+                if (sysfs_attrib->grp) {
+                        list_for_each_entry(grp_kobj, &mci->grp_kobj_list,
+                                            list)
+                                if (grp_kobj->grp == sysfs_attrib->grp)
+                                        edac_remove_mci_instance_attributes(mci,
+                                                    grp_kobj->grp->mcidev_attr,
+                                                    &grp_kobj->kobj, count + 1);
+                } else if (sysfs_attrib->attr.name) {
+                        debugf0("%s() file %s\n", __func__,
+                                sysfs_attrib->attr.name);
+                        sysfs_remove_file(kobj, &sysfs_attrib->attr);
+                } else
+                        break;
                 sysfs_attrib++;
         }
+
+        /*
+         * Now that all attributes got removed, it is save to remove all groups
+         */
+        if (!count)
+                list_for_each_entry_safe(grp_kobj, tmp, &mci->grp_kobj_list,
+                                         list) {
+                        debugf0("%s() grp %s\n", __func__, grp_kobj->grp->name);
+                        kobject_put(&grp_kobj->kobj);
+                }
 }
 
 
@@ -794,6 +911,8 @@ int edac_create_sysfs_mci_device(struct mem_ctl_info *mci)
 
         debugf0("%s() idx=%d\n", __func__, mci->mc_idx);
 
+        INIT_LIST_HEAD(&mci->grp_kobj_list);
+
         /* create a symlink for the device */
         err = sysfs_create_link(kobj_mci, &mci->dev->kobj,
                                 EDAC_DEVICE_SYMLINK);
@@ -806,7 +925,9 @@ int edac_create_sysfs_mci_device(struct mem_ctl_info *mci)
          * then create them now for the driver.
          */
         if (mci->mc_driver_sysfs_attributes) {
-                err = edac_create_mci_instance_attributes(mci);
+                err = edac_create_mci_instance_attributes(mci,
+                                        mci->mc_driver_sysfs_attributes,
+                                        &mci->edac_mci_kobj);
                 if (err) {
                         debugf1("%s() failure to create mci attributes\n",
                                 __func__);
@@ -841,7 +962,8 @@ fail1:
         }
 
         /* remove the mci instance's attributes, if any */
-        edac_remove_mci_instance_attributes(mci);
+        edac_remove_mci_instance_attributes(mci,
+                mci->mc_driver_sysfs_attributes, &mci->edac_mci_kobj, 0);
 
         /* remove the symlink */
         sysfs_remove_link(kobj_mci, EDAC_DEVICE_SYMLINK);
@@ -875,8 +997,9 @@ void edac_remove_sysfs_mci_device(struct mem_ctl_info *mci)
         debugf0("%s()  remove_mci_instance\n", __func__);
 
         /* remove this mci instance's attribtes */
-        edac_remove_mci_instance_attributes(mci);
-
+        edac_remove_mci_instance_attributes(mci,
+                                            mci->mc_driver_sysfs_attributes,
+                                            &mci->edac_mci_kobj, 0);
         debugf0("%s()  unregister this mci kobj\n", __func__);
 
         /* unregister this instance's kobject */
diff --git a/drivers/edac/edac_mce.c b/drivers/edac/edac_mce.c
new file mode 100644
index 000000000000..9ccdc5b140e7
--- /dev/null
+++ b/drivers/edac/edac_mce.c
@@ -0,0 +1,61 @@
+/* Provides edac interface to mcelog events
+ *
+ * This file may be distributed under the terms of the
+ * GNU General Public License version 2.
+ *
+ * Copyright (c) 2009 by:
+ *       Mauro Carvalho Chehab <mchehab@redhat.com>
+ *
+ * Red Hat Inc. http://www.redhat.com
+ */
+
+#include <linux/module.h>
+#include <linux/edac_mce.h>
+#include <asm/mce.h>
+
+int edac_mce_enabled;
+EXPORT_SYMBOL_GPL(edac_mce_enabled);
+
+
+/*
+ * Extension interface
+ */
+
+static LIST_HEAD(edac_mce_list);
+static DEFINE_MUTEX(edac_mce_lock);
+
+int edac_mce_register(struct edac_mce *edac_mce)
+{
+        mutex_lock(&edac_mce_lock);
+        list_add_tail(&edac_mce->list, &edac_mce_list);
+        mutex_unlock(&edac_mce_lock);
+        return 0;
+}
+EXPORT_SYMBOL(edac_mce_register);
+
+void edac_mce_unregister(struct edac_mce *edac_mce)
+{
+        mutex_lock(&edac_mce_lock);
+        list_del(&edac_mce->list);
+        mutex_unlock(&edac_mce_lock);
+}
+EXPORT_SYMBOL(edac_mce_unregister);
+
+int edac_mce_parse(struct mce *mce)
+{
+        struct edac_mce *edac_mce;
+
+        list_for_each_entry(edac_mce, &edac_mce_list, list) {
+                if (edac_mce->check_error(edac_mce->priv, mce))
+                        return 1;
+        }
+
+        /* Nobody queued the error */
+        return 0;
+}
+EXPORT_SYMBOL_GPL(edac_mce_parse);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
+MODULE_AUTHOR("Red Hat Inc. (http://www.redhat.com)");
+MODULE_DESCRIPTION("EDAC Driver for mcelog captured errors");
diff --git a/drivers/edac/i5000_edac.c b/drivers/edac/i5000_edac.c
index adc10a2ac5f6..996c1bdb5a34 100644
--- a/drivers/edac/i5000_edac.c
+++ b/drivers/edac/i5000_edac.c
@@ -774,7 +774,7 @@ static void i5000_clear_error(struct mem_ctl_info *mci)
 static void i5000_check_error(struct mem_ctl_info *mci)
 {
         struct i5000_error_info info;
-        debugf4("MC%d: " __FILE__ ": %s()\n", mci->mc_idx, __func__);
+        debugf4("MC%d: %s: %s()\n", mci->mc_idx, __FILE__, __func__);
         i5000_get_error_info(mci, &info);
         i5000_process_error_info(mci, &info, 1);
 }
@@ -1353,8 +1353,8 @@ static int i5000_probe1(struct pci_dev *pdev, int dev_idx)
         int num_dimms_per_channel;
         int num_csrows;
 
-        debugf0("MC: " __FILE__ ": %s(), pdev bus %u dev=0x%x fn=0x%x\n",
-                __func__,
+        debugf0("MC: %s: %s(), pdev bus %u dev=0x%x fn=0x%x\n",
+                __FILE__, __func__,
                 pdev->bus->number,
                 PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
 
@@ -1389,7 +1389,7 @@ static int i5000_probe1(struct pci_dev *pdev, int dev_idx)
                 return -ENOMEM;
 
         kobject_get(&mci->edac_mci_kobj);
-        debugf0("MC: " __FILE__ ": %s(): mci = %p\n", __func__, mci);
+        debugf0("MC: %s: %s(): mci = %p\n", __FILE__, __func__, mci);
 
         mci->dev = &pdev->dev;  /* record ptr  to the generic device */
 
@@ -1432,8 +1432,8 @@ static int i5000_probe1(struct pci_dev *pdev, int dev_idx)
 
         /* add this new MC control structure to EDAC's list of MCs */
         if (edac_mc_add_mc(mci)) {
-                debugf0("MC: " __FILE__
-                        ": %s(): failed edac_mc_add_mc()\n", __func__);
+                debugf0("MC: %s: %s(): failed edac_mc_add_mc()\n",
+                        __FILE__, __func__);
                 /* FIXME: perhaps some code should go here that disables error
                  * reporting if we just enabled it
                  */
@@ -1478,7 +1478,7 @@ static int __devinit i5000_init_one(struct pci_dev *pdev,
 {
         int rc;
 
-        debugf0("MC: " __FILE__ ": %s()\n", __func__);
+        debugf0("MC: %s: %s()\n", __FILE__, __func__);
 
         /* wake up device */
         rc = pci_enable_device(pdev);
@@ -1497,7 +1497,7 @@ static void __devexit i5000_remove_one(struct pci_dev *pdev)
 {
         struct mem_ctl_info *mci;
 
-        debugf0(__FILE__ ": %s()\n", __func__);
+        debugf0("%s: %s()\n", __FILE__, __func__);
 
         if (i5000_pci)
                 edac_pci_release_generic_ctl(i5000_pci);
@@ -1544,7 +1544,7 @@ static int __init i5000_init(void)
 {
         int pci_rc;
 
-        debugf2("MC: " __FILE__ ": %s()\n", __func__);
+        debugf2("MC: %s: %s()\n", __FILE__, __func__);
 
        /* Ensure that the OPSTATE is set correctly for POLL or NMI */
        opstate_init();
@@ -1560,7 +1560,7 @@ static int __init i5000_init(void)
  */
 static void __exit i5000_exit(void)
 {
-        debugf2("MC: " __FILE__ ": %s()\n", __func__);
+        debugf2("MC: %s: %s()\n", __FILE__, __func__);
         pci_unregister_driver(&i5000_driver);
 }
 
diff --git a/drivers/edac/i5400_edac.c b/drivers/edac/i5400_edac.c
index f99d10655ed4..010c1d6526f5 100644
--- a/drivers/edac/i5400_edac.c
+++ b/drivers/edac/i5400_edac.c
@@ -694,7 +694,7 @@ static void i5400_clear_error(struct mem_ctl_info *mci)
 static void i5400_check_error(struct mem_ctl_info *mci)
 {
         struct i5400_error_info info;
-        debugf4("MC%d: " __FILE__ ": %s()\n", mci->mc_idx, __func__);
+        debugf4("MC%d: %s: %s()\n", mci->mc_idx, __FILE__, __func__);
         i5400_get_error_info(mci, &info);
         i5400_process_error_info(mci, &info);
 }
@@ -1227,8 +1227,8 @@ static int i5400_probe1(struct pci_dev *pdev, int dev_idx)
         if (dev_idx >= ARRAY_SIZE(i5400_devs))
                 return -EINVAL;
 
-        debugf0("MC: " __FILE__ ": %s(), pdev bus %u dev=0x%x fn=0x%x\n",
-                __func__,
+        debugf0("MC: %s: %s(), pdev bus %u dev=0x%x fn=0x%x\n",
+                __FILE__, __func__,
                 pdev->bus->number,
                 PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
 
@@ -1256,7 +1256,7 @@ static int i5400_probe1(struct pci_dev *pdev, int dev_idx)
         if (mci == NULL)
                 return -ENOMEM;
 
-        debugf0("MC: " __FILE__ ": %s(): mci = %p\n", __func__, mci);
+        debugf0("MC: %s: %s(): mci = %p\n", __FILE__, __func__, mci);
 
         mci->dev = &pdev->dev;  /* record ptr  to the generic device */
 
@@ -1299,8 +1299,8 @@ static int i5400_probe1(struct pci_dev *pdev, int dev_idx)
 
         /* add this new MC control structure to EDAC's list of MCs */
         if (edac_mc_add_mc(mci)) {
-                debugf0("MC: " __FILE__
-                        ": %s(): failed edac_mc_add_mc()\n", __func__);
+                debugf0("MC: %s: %s(): failed edac_mc_add_mc()\n",
+                        __FILE__, __func__);
                 /* FIXME: perhaps some code should go here that disables error
                  * reporting if we just enabled it
                  */
@@ -1344,7 +1344,7 @@ static int __devinit i5400_init_one(struct pci_dev *pdev,
 {
         int rc;
 
-        debugf0("MC: " __FILE__ ": %s()\n", __func__);
+        debugf0("MC: %s: %s()\n", __FILE__, __func__);
 
         /* wake up device */
         rc = pci_enable_device(pdev);
@@ -1363,7 +1363,7 @@ static void __devexit i5400_remove_one(struct pci_dev *pdev)
 {
         struct mem_ctl_info *mci;
 
-        debugf0(__FILE__ ": %s()\n", __func__);
+        debugf0("%s: %s()\n", __FILE__, __func__);
 
         if (i5400_pci)
                 edac_pci_release_generic_ctl(i5400_pci);
@@ -1409,7 +1409,7 @@ static int __init i5400_init(void)
 {
         int pci_rc;
 
-        debugf2("MC: " __FILE__ ": %s()\n", __func__);
+        debugf2("MC: %s: %s()\n", __FILE__, __func__);
 
         /* Ensure that the OPSTATE is set correctly for POLL or NMI */
         opstate_init();
@@ -1425,7 +1425,7 @@ static int __init i5400_init(void)
  */
 static void __exit i5400_exit(void)
 {
-        debugf2("MC: " __FILE__ ": %s()\n", __func__);
+        debugf2("MC: %s: %s()\n", __FILE__, __func__);
         pci_unregister_driver(&i5400_driver);
 }
 
diff --git a/drivers/edac/i7core_edac.c b/drivers/edac/i7core_edac.c
new file mode 100644
index 000000000000..6b8b7b41ec5f
--- /dev/null
+++ b/drivers/edac/i7core_edac.c
@@ -0,0 +1,2078 @@
+/* Intel i7 core/Nehalem Memory Controller kernel module
+ *
+ * This driver supports yhe memory controllers found on the Intel
+ * processor families i7core, i7core 7xx/8xx, i5core, Xeon 35xx,
+ * Xeon 55xx and Xeon 56xx also known as Nehalem, Nehalem-EP, Lynnfield
+ * and Westmere-EP.
+ *
+ * This file may be distributed under the terms of the
+ * GNU General Public License version 2 only.
+ *
+ * Copyright (c) 2009-2010 by:
+ *       Mauro Carvalho Chehab <mchehab@redhat.com>
+ *
+ * Red Hat Inc. http://www.redhat.com
+ *
+ * Forked and adapted from the i5400_edac driver
+ *
+ * Based on the following public Intel datasheets:
+ * Intel Core i7 Processor Extreme Edition and Intel Core i7 Processor
+ * Datasheet, Volume 2:
+ *      http://download.intel.com/design/processor/datashts/320835.pdf
+ * Intel Xeon Processor 5500 Series Datasheet Volume 2
+ *      http://www.intel.com/Assets/PDF/datasheet/321322.pdf
+ * also available at:
+ *      http://www.arrownac.com/manufacturers/intel/s/nehalem/5500-datasheet-v2.pdf
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/pci_ids.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/edac.h>
+#include <linux/mmzone.h>
+#include <linux/edac_mce.h>
+#include <linux/smp.h>
+#include <asm/processor.h>
+
+#include "edac_core.h"
+
+/*
+ * This is used for Nehalem-EP and Nehalem-EX devices, where the non-core
+ * registers start at bus 255, and are not reported by BIOS.
+ * We currently find devices with only 2 sockets. In order to support more QPI
+ * Quick Path Interconnect, just increment this number.
+ */
+#define MAX_SOCKET_BUSES        2
+
+
+/*
+ * Alter this version for the module when modifications are made
+ */
+#define I7CORE_REVISION    " Ver: 1.0.0 " __DATE__
+#define EDAC_MOD_STR      "i7core_edac"
+
+/*
+ * Debug macros
+ */
+#define i7core_printk(level, fmt, arg...)                       \
+        edac_printk(level, "i7core", fmt, ##arg)
+
+#define i7core_mc_printk(mci, level, fmt, arg...)               \
+        edac_mc_chipset_printk(mci, level, "i7core", fmt, ##arg)
+
+/*
+ * i7core Memory Controller Registers
+ */
+
+        /* OFFSETS for Device 0 Function 0 */
+
+#define MC_CFG_CONTROL  0x90
+
+        /* OFFSETS for Device 3 Function 0 */
+
+#define MC_CONTROL      0x48
+#define MC_STATUS       0x4c
+#define MC_MAX_DOD      0x64
+
+/*
+ * OFFSETS for Device 3 Function 4, as inicated on Xeon 5500 datasheet:
+ * http://www.arrownac.com/manufacturers/intel/s/nehalem/5500-datasheet-v2.pdf
+ */
+
+#define MC_TEST_ERR_RCV1        0x60
+  #define DIMM2_COR_ERR(r)                      ((r) & 0x7fff)
+
+#define MC_TEST_ERR_RCV0        0x64
+  #define DIMM1_COR_ERR(r)                      (((r) >> 16) & 0x7fff)
+  #define DIMM0_COR_ERR(r)                      ((r) & 0x7fff)
+
+/* OFFSETS for Device 3 Function 2, as inicated on Xeon 5500 datasheet */
+#define MC_COR_ECC_CNT_0        0x80
+#define MC_COR_ECC_CNT_1        0x84
+#define MC_COR_ECC_CNT_2        0x88
+#define MC_COR_ECC_CNT_3        0x8c
+#define MC_COR_ECC_CNT_4        0x90
+#define MC_COR_ECC_CNT_5        0x94
+
+#define DIMM_TOP_COR_ERR(r)                     (((r) >> 16) & 0x7fff)
+#define DIMM_BOT_COR_ERR(r)                     ((r) & 0x7fff)
+
+
+        /* OFFSETS for Devices 4,5 and 6 Function 0 */
+
+#define MC_CHANNEL_DIMM_INIT_PARAMS 0x58
+  #define THREE_DIMMS_PRESENT           (1 << 24)
+  #define SINGLE_QUAD_RANK_PRESENT      (1 << 23)
+  #define QUAD_RANK_PRESENT             (1 << 22)
+  #define REGISTERED_DIMM               (1 << 15)
+
+#define MC_CHANNEL_MAPPER       0x60
+  #define RDLCH(r, ch)          ((((r) >> (3 + (ch * 6))) & 0x07) - 1)
+  #define WRLCH(r, ch)          ((((r) >> (ch * 6)) & 0x07) - 1)
+
+#define MC_CHANNEL_RANK_PRESENT 0x7c
+  #define RANK_PRESENT_MASK             0xffff
+
+#define MC_CHANNEL_ADDR_MATCH   0xf0
+#define MC_CHANNEL_ERROR_MASK   0xf8
+#define MC_CHANNEL_ERROR_INJECT 0xfc
+  #define INJECT_ADDR_PARITY    0x10
+  #define INJECT_ECC            0x08
+  #define MASK_CACHELINE        0x06
+  #define MASK_FULL_CACHELINE   0x06
+  #define MASK_MSB32_CACHELINE  0x04
+  #define MASK_LSB32_CACHELINE  0x02
+  #define NO_MASK_CACHELINE     0x00
+  #define REPEAT_EN             0x01
+
+        /* OFFSETS for Devices 4,5 and 6 Function 1 */
+
+#define MC_DOD_CH_DIMM0         0x48
+#define MC_DOD_CH_DIMM1         0x4c
+#define MC_DOD_CH_DIMM2         0x50
+  #define RANKOFFSET_MASK       ((1 << 12) | (1 << 11) | (1 << 10))
+  #define RANKOFFSET(x)         ((x & RANKOFFSET_MASK) >> 10)
+  #define DIMM_PRESENT_MASK     (1 << 9)
+  #define DIMM_PRESENT(x)       (((x) & DIMM_PRESENT_MASK) >> 9)
+  #define MC_DOD_NUMBANK_MASK           ((1 << 8) | (1 << 7))
+  #define MC_DOD_NUMBANK(x)             (((x) & MC_DOD_NUMBANK_MASK) >> 7)
+  #define MC_DOD_NUMRANK_MASK           ((1 << 6) | (1 << 5))
+  #define MC_DOD_NUMRANK(x)             (((x) & MC_DOD_NUMRANK_MASK) >> 5)
+  #define MC_DOD_NUMROW_MASK            ((1 << 4) | (1 << 3) | (1 << 2))
+  #define MC_DOD_NUMROW(x)              (((x) & MC_DOD_NUMROW_MASK) >> 2)
+  #define MC_DOD_NUMCOL_MASK            3
+  #define MC_DOD_NUMCOL(x)              ((x) & MC_DOD_NUMCOL_MASK)
+
+#define MC_RANK_PRESENT         0x7c
+
+#define MC_SAG_CH_0     0x80
+#define MC_SAG_CH_1     0x84
+#define MC_SAG_CH_2     0x88
+#define MC_SAG_CH_3     0x8c
+#define MC_SAG_CH_4     0x90
+#define MC_SAG_CH_5     0x94
+#define MC_SAG_CH_6     0x98
+#define MC_SAG_CH_7     0x9c
+
+#define MC_RIR_LIMIT_CH_0       0x40
+#define MC_RIR_LIMIT_CH_1       0x44
+#define MC_RIR_LIMIT_CH_2       0x48
+#define MC_RIR_LIMIT_CH_3       0x4C
+#define MC_RIR_LIMIT_CH_4       0x50
+#define MC_RIR_LIMIT_CH_5       0x54
+#define MC_RIR_LIMIT_CH_6       0x58
+#define MC_RIR_LIMIT_CH_7       0x5C
+#define MC_RIR_LIMIT_MASK       ((1 << 10) - 1)
+
+#define MC_RIR_WAY_CH           0x80
+  #define MC_RIR_WAY_OFFSET_MASK        (((1 << 14) - 1) & ~0x7)
+  #define MC_RIR_WAY_RANK_MASK          0x7
+
+/*
+ * i7core structs
+ */
+
+#define NUM_CHANS 3
+#define MAX_DIMMS 3             /* Max DIMMS per channel */
+#define MAX_MCR_FUNC  4
+#define MAX_CHAN_FUNC 3
+
+struct i7core_info {
+        u32     mc_control;
+        u32     mc_status;
+        u32     max_dod;
+        u32     ch_map;
+};
+
+
+struct i7core_inject {
+        int     enable;
+
+        u32     section;
+        u32     type;
+        u32     eccmask;
+
+        /* Error address mask */
+        int channel, dimm, rank, bank, page, col;
+};
+
+struct i7core_channel {
+        u32             ranks;
+        u32             dimms;
+};
+
+struct pci_id_descr {
+        int                     dev;
+        int                     func;
+        int                     dev_id;
+        int                     optional;
+};
+
+struct pci_id_table {
+        struct pci_id_descr     *descr;
+        int                     n_devs;
+};
+
+struct i7core_dev {
+        struct list_head        list;
+        u8                      socket;
+        struct pci_dev          **pdev;
+        int                     n_devs;
+        struct mem_ctl_info     *mci;
+};
+
+struct i7core_pvt {
+        struct pci_dev  *pci_noncore;
+        struct pci_dev  *pci_mcr[MAX_MCR_FUNC + 1];
+        struct pci_dev  *pci_ch[NUM_CHANS][MAX_CHAN_FUNC + 1];
+
+        struct i7core_dev *i7core_dev;
+
+        struct i7core_info      info;
+        struct i7core_inject    inject;
+        struct i7core_channel   channel[NUM_CHANS];
+
+        int             channels; /* Number of active channels */
+
+        int             ce_count_available;
+        int             csrow_map[NUM_CHANS][MAX_DIMMS];
+
+                        /* ECC corrected errors counts per udimm */
+        unsigned long   udimm_ce_count[MAX_DIMMS];
+        int             udimm_last_ce_count[MAX_DIMMS];
+                        /* ECC corrected errors counts per rdimm */
+        unsigned long   rdimm_ce_count[NUM_CHANS][MAX_DIMMS];
+        int             rdimm_last_ce_count[NUM_CHANS][MAX_DIMMS];
+
+        unsigned int    is_registered;
+
+        /* mcelog glue */
+        struct edac_mce         edac_mce;
+
+        /* Fifo double buffers */
+        struct mce              mce_entry[MCE_LOG_LEN];
+        struct mce              mce_outentry[MCE_LOG_LEN];
+
+        /* Fifo in/out counters */
+        unsigned                mce_in, mce_out;
+
+        /* Count indicator to show errors not got */
+        unsigned                mce_overrun;
+};
+
+/* Static vars */
+static LIST_HEAD(i7core_edac_list);
+static DEFINE_MUTEX(i7core_edac_lock);
+
+#define PCI_DESCR(device, function, device_id)  \
+        .dev = (device),                        \
+        .func = (function),                     \
+        .dev_id = (device_id)
+
+struct pci_id_descr pci_dev_descr_i7core_nehalem[] = {
+                /* Memory controller */
+        { PCI_DESCR(3, 0, PCI_DEVICE_ID_INTEL_I7_MCR)     },
+        { PCI_DESCR(3, 1, PCI_DEVICE_ID_INTEL_I7_MC_TAD)  },
+                        /* Exists only for RDIMM */
+        { PCI_DESCR(3, 2, PCI_DEVICE_ID_INTEL_I7_MC_RAS), .optional = 1  },
+        { PCI_DESCR(3, 4, PCI_DEVICE_ID_INTEL_I7_MC_TEST) },
+
+                /* Channel 0 */
+        { PCI_DESCR(4, 0, PCI_DEVICE_ID_INTEL_I7_MC_CH0_CTRL) },
+        { PCI_DESCR(4, 1, PCI_DEVICE_ID_INTEL_I7_MC_CH0_ADDR) },
+        { PCI_DESCR(4, 2, PCI_DEVICE_ID_INTEL_I7_MC_CH0_RANK) },
+        { PCI_DESCR(4, 3, PCI_DEVICE_ID_INTEL_I7_MC_CH0_TC)   },
+
+                /* Channel 1 */
+        { PCI_DESCR(5, 0, PCI_DEVICE_ID_INTEL_I7_MC_CH1_CTRL) },
+        { PCI_DESCR(5, 1, PCI_DEVICE_ID_INTEL_I7_MC_CH1_ADDR) },
+        { PCI_DESCR(5, 2, PCI_DEVICE_ID_INTEL_I7_MC_CH1_RANK) },
+        { PCI_DESCR(5, 3, PCI_DEVICE_ID_INTEL_I7_MC_CH1_TC)   },
+
+                /* Channel 2 */
+        { PCI_DESCR(6, 0, PCI_DEVICE_ID_INTEL_I7_MC_CH2_CTRL) },
+        { PCI_DESCR(6, 1, PCI_DEVICE_ID_INTEL_I7_MC_CH2_ADDR) },
+        { PCI_DESCR(6, 2, PCI_DEVICE_ID_INTEL_I7_MC_CH2_RANK) },
+        { PCI_DESCR(6, 3, PCI_DEVICE_ID_INTEL_I7_MC_CH2_TC)   },
+
+                /* Generic Non-core registers */
+        /*
+         * This is the PCI device on i7core and on Xeon 35xx (8086:2c41)
+         * On Xeon 55xx, however, it has a different id (8086:2c40). So,
+         * the probing code needs to test for the other address in case of
+         * failure of this one
+         */
+        { PCI_DESCR(0, 0, PCI_DEVICE_ID_INTEL_I7_NONCORE)  },
+
+};
+
+struct pci_id_descr pci_dev_descr_lynnfield[] = {
+        { PCI_DESCR( 3, 0, PCI_DEVICE_ID_INTEL_LYNNFIELD_MCR)         },
+        { PCI_DESCR( 3, 1, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_TAD)      },
+        { PCI_DESCR( 3, 4, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_TEST)     },
+
+        { PCI_DESCR( 4, 0, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH0_CTRL) },
+        { PCI_DESCR( 4, 1, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH0_ADDR) },
+        { PCI_DESCR( 4, 2, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH0_RANK) },
+        { PCI_DESCR( 4, 3, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH0_TC)   },
+
+        { PCI_DESCR( 5, 0, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH1_CTRL) },
+        { PCI_DESCR( 5, 1, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH1_ADDR) },
+        { PCI_DESCR( 5, 2, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH1_RANK) },
+        { PCI_DESCR( 5, 3, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH1_TC)   },
+
+        /*
+         * This is the PCI device has an alternate address on some
+         * processors like Core i7 860
+         */
+        { PCI_DESCR( 0, 0, PCI_DEVICE_ID_INTEL_LYNNFIELD_NONCORE)     },
+};
+
+struct pci_id_descr pci_dev_descr_i7core_westmere[] = {
+                /* Memory controller */
+        { PCI_DESCR(3, 0, PCI_DEVICE_ID_INTEL_LYNNFIELD_MCR_REV2)     },
+        { PCI_DESCR(3, 1, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_TAD_REV2)  },
+                        /* Exists only for RDIMM */
+        { PCI_DESCR(3, 2, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_RAS_REV2), .optional = 1  },
+        { PCI_DESCR(3, 4, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_TEST_REV2) },
+
+                /* Channel 0 */
+        { PCI_DESCR(4, 0, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH0_CTRL_REV2) },
+        { PCI_DESCR(4, 1, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH0_ADDR_REV2) },
+        { PCI_DESCR(4, 2, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH0_RANK_REV2) },
+        { PCI_DESCR(4, 3, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH0_TC_REV2)   },
+
+                /* Channel 1 */
+        { PCI_DESCR(5, 0, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH1_CTRL_REV2) },
+        { PCI_DESCR(5, 1, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH1_ADDR_REV2) },
+        { PCI_DESCR(5, 2, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH1_RANK_REV2) },
+        { PCI_DESCR(5, 3, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH1_TC_REV2)   },
+
+                /* Channel 2 */
+        { PCI_DESCR(6, 0, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH2_CTRL_REV2) },
+        { PCI_DESCR(6, 1, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH2_ADDR_REV2) },
+        { PCI_DESCR(6, 2, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH2_RANK_REV2) },
+        { PCI_DESCR(6, 3, PCI_DEVICE_ID_INTEL_LYNNFIELD_MC_CH2_TC_REV2)   },
+
+                /* Generic Non-core registers */
+        { PCI_DESCR(0, 0, PCI_DEVICE_ID_INTEL_LYNNFIELD_NONCORE_REV2)  },
+
+};
+
+#define PCI_ID_TABLE_ENTRY(A) { A, ARRAY_SIZE(A) }
+struct pci_id_table pci_dev_table[] = {
+        PCI_ID_TABLE_ENTRY(pci_dev_descr_i7core_nehalem),
+        PCI_ID_TABLE_ENTRY(pci_dev_descr_lynnfield),
+        PCI_ID_TABLE_ENTRY(pci_dev_descr_i7core_westmere),
+};
+
+/*
+ *      pci_device_id   table for which devices we are looking for
+ */
+static const struct pci_device_id i7core_pci_tbl[] __devinitdata = {
+        {PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_X58_HUB_MGMT)},
+        {PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_LYNNFIELD_QPI_LINK0)},
+        {0,}                    /* 0 terminated list. */
+};
+
+static struct edac_pci_ctl_info *i7core_pci;
+
+/****************************************************************************
+                        Anciliary status routines
+ ****************************************************************************/
+
+        /* MC_CONTROL bits */
+#define CH_ACTIVE(pvt, ch)      ((pvt)->info.mc_control & (1 << (8 + ch)))
+#define ECCx8(pvt)              ((pvt)->info.mc_control & (1 << 1))
+
+        /* MC_STATUS bits */
+#define ECC_ENABLED(pvt)        ((pvt)->info.mc_status & (1 << 4))
+#define CH_DISABLED(pvt, ch)    ((pvt)->info.mc_status & (1 << ch))
+
+        /* MC_MAX_DOD read functions */
+static inline int numdimms(u32 dimms)
+{
+        return (dimms & 0x3) + 1;
+}
+
+static inline int numrank(u32 rank)
+{
+        static int ranks[4] = { 1, 2, 4, -EINVAL };
+
+        return ranks[rank & 0x3];
+}
+
+static inline int numbank(u32 bank)
+{
+        static int banks[4] = { 4, 8, 16, -EINVAL };
+
+        return banks[bank & 0x3];
+}
+
+static inline int numrow(u32 row)
+{
+        static int rows[8] = {
+                1 << 12, 1 << 13, 1 << 14, 1 << 15,
+                1 << 16, -EINVAL, -EINVAL, -EINVAL,
+        };
+
+        return rows[row & 0x7];
+}
+
+static inline int numcol(u32 col)
+{
+        static int cols[8] = {
+                1 << 10, 1 << 11, 1 << 12, -EINVAL,
+        };
+        return cols[col & 0x3];
+}
+
+static struct i7core_dev *get_i7core_dev(u8 socket)
+{
+        struct i7core_dev *i7core_dev;
+
+        list_for_each_entry(i7core_dev, &i7core_edac_list, list) {
+                if (i7core_dev->socket == socket)
+                        return i7core_dev;
+        }
+
+        return NULL;
+}
+
+/****************************************************************************
+                        Memory check routines
+ ****************************************************************************/
+static struct pci_dev *get_pdev_slot_func(u8 socket, unsigned slot,
+                                          unsigned func)
+{
+        struct i7core_dev *i7core_dev = get_i7core_dev(socket);
+        int i;
+
+        if (!i7core_dev)
+                return NULL;
+
+        for (i = 0; i < i7core_dev->n_devs; i++) {
+                if (!i7core_dev->pdev[i])
+                        continue;
+
+                if (PCI_SLOT(i7core_dev->pdev[i]->devfn) == slot &&
+                    PCI_FUNC(i7core_dev->pdev[i]->devfn) == func) {
+                        return i7core_dev->pdev[i];
+                }
+        }
+
+        return NULL;
+}
+
+/**
+ * i7core_get_active_channels() - gets the number of channels and csrows
+ * @socket:     Quick Path Interconnect socket
+ * @channels:   Number of channels that will be returned
+ * @csrows:     Number of csrows found
+ *
+ * Since EDAC core needs to know in advance the number of available channels
+ * and csrows, in order to allocate memory for csrows/channels, it is needed
+ * to run two similar steps. At the first step, implemented on this function,
+ * it checks the number of csrows/channels present at one socket.
+ * this is used in order to properly allocate the size of mci components.
+ *
+ * It should be noticed that none of the current available datasheets explain
+ * or even mention how csrows are seen by the memory controller. So, we need
+ * to add a fake description for csrows.
+ * So, this driver is attributing one DIMM memory for one csrow.
+ */
+static int i7core_get_active_channels(u8 socket, unsigned *channels,
+                                      unsigned *csrows)
+{
+        struct pci_dev *pdev = NULL;
+        int i, j;
+        u32 status, control;
+
+        *channels = 0;
+        *csrows = 0;
+
+        pdev = get_pdev_slot_func(socket, 3, 0);
+        if (!pdev) {
+                i7core_printk(KERN_ERR, "Couldn't find socket %d fn 3.0!!!\n",
+                              socket);
+                return -ENODEV;
+        }
+
+        /* Device 3 function 0 reads */
+        pci_read_config_dword(pdev, MC_STATUS, &status);
+        pci_read_config_dword(pdev, MC_CONTROL, &control);
+
+        for (i = 0; i < NUM_CHANS; i++) {
+                u32 dimm_dod[3];
+                /* Check if the channel is active */
+                if (!(control & (1 << (8 + i))))
+                        continue;
+
+                /* Check if the channel is disabled */
+                if (status & (1 << i))
+                        continue;
+
+                pdev = get_pdev_slot_func(socket, i + 4, 1);
+                if (!pdev) {
+                        i7core_printk(KERN_ERR, "Couldn't find socket %d "
+                                                "fn %d.%d!!!\n",
+                                                socket, i + 4, 1);
+                        return -ENODEV;
+                }
+                /* Devices 4-6 function 1 */
+                pci_read_config_dword(pdev,
+                                MC_DOD_CH_DIMM0, &dimm_dod[0]);
+                pci_read_config_dword(pdev,
+                                MC_DOD_CH_DIMM1, &dimm_dod[1]);
+                pci_read_config_dword(pdev,
+                                MC_DOD_CH_DIMM2, &dimm_dod[2]);
+
+                (*channels)++;
+
+                for (j = 0; j < 3; j++) {
+                        if (!DIMM_PRESENT(dimm_dod[j]))
+                                continue;
+                        (*csrows)++;
+                }
+        }
+
+        debugf0("Number of active channels on socket %d: %d\n",
+                socket, *channels);
+
+        return 0;
+}
+
+static int get_dimm_config(struct mem_ctl_info *mci, int *csrow)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        struct csrow_info *csr;
+        struct pci_dev *pdev;
+        int i, j;
+        unsigned long last_page = 0;
+        enum edac_type mode;
+        enum mem_type mtype;
+
+        /* Get data from the MC register, function 0 */
+        pdev = pvt->pci_mcr[0];
+        if (!pdev)
+                return -ENODEV;
+
+        /* Device 3 function 0 reads */
+        pci_read_config_dword(pdev, MC_CONTROL, &pvt->info.mc_control);
+        pci_read_config_dword(pdev, MC_STATUS, &pvt->info.mc_status);
+        pci_read_config_dword(pdev, MC_MAX_DOD, &pvt->info.max_dod);
+        pci_read_config_dword(pdev, MC_CHANNEL_MAPPER, &pvt->info.ch_map);
+
+        debugf0("QPI %d control=0x%08x status=0x%08x dod=0x%08x map=0x%08x\n",
+                pvt->i7core_dev->socket, pvt->info.mc_control, pvt->info.mc_status,
+                pvt->info.max_dod, pvt->info.ch_map);
+
+        if (ECC_ENABLED(pvt)) {
+                debugf0("ECC enabled with x%d SDCC\n", ECCx8(pvt) ? 8 : 4);
+                if (ECCx8(pvt))
+                        mode = EDAC_S8ECD8ED;
+                else
+                        mode = EDAC_S4ECD4ED;
+        } else {
+                debugf0("ECC disabled\n");
+                mode = EDAC_NONE;
+        }
+
+        /* FIXME: need to handle the error codes */
+        debugf0("DOD Max limits: DIMMS: %d, %d-ranked, %d-banked "
+                "x%x x 0x%x\n",
+                numdimms(pvt->info.max_dod),
+                numrank(pvt->info.max_dod >> 2),
+                numbank(pvt->info.max_dod >> 4),
+                numrow(pvt->info.max_dod >> 6),
+                numcol(pvt->info.max_dod >> 9));
+
+        for (i = 0; i < NUM_CHANS; i++) {
+                u32 data, dimm_dod[3], value[8];
+
+                if (!pvt->pci_ch[i][0])
+                        continue;
+
+                if (!CH_ACTIVE(pvt, i)) {
+                        debugf0("Channel %i is not active\n", i);
+                        continue;
+                }
+                if (CH_DISABLED(pvt, i)) {
+                        debugf0("Channel %i is disabled\n", i);
+                        continue;
+                }
+
+                /* Devices 4-6 function 0 */
+                pci_read_config_dword(pvt->pci_ch[i][0],
+                                MC_CHANNEL_DIMM_INIT_PARAMS, &data);
+
+                pvt->channel[i].ranks = (data & QUAD_RANK_PRESENT) ?
+                                                4 : 2;
+
+                if (data & REGISTERED_DIMM)
+                        mtype = MEM_RDDR3;
+                else
+                        mtype = MEM_DDR3;
+#if 0
+                if (data & THREE_DIMMS_PRESENT)
+                        pvt->channel[i].dimms = 3;
+                else if (data & SINGLE_QUAD_RANK_PRESENT)
+                        pvt->channel[i].dimms = 1;
+                else
+                        pvt->channel[i].dimms = 2;
+#endif
+
+                /* Devices 4-6 function 1 */
+                pci_read_config_dword(pvt->pci_ch[i][1],
+                                MC_DOD_CH_DIMM0, &dimm_dod[0]);
+                pci_read_config_dword(pvt->pci_ch[i][1],
+                                MC_DOD_CH_DIMM1, &dimm_dod[1]);
+                pci_read_config_dword(pvt->pci_ch[i][1],
+                                MC_DOD_CH_DIMM2, &dimm_dod[2]);
+
+                debugf0("Ch%d phy rd%d, wr%d (0x%08x): "
+                        "%d ranks, %cDIMMs\n",
+                        i,
+                        RDLCH(pvt->info.ch_map, i), WRLCH(pvt->info.ch_map, i),
+                        data,
+                        pvt->channel[i].ranks,
+                        (data & REGISTERED_DIMM) ? 'R' : 'U');
+
+                for (j = 0; j < 3; j++) {
+                        u32 banks, ranks, rows, cols;
+                        u32 size, npages;
+
+                        if (!DIMM_PRESENT(dimm_dod[j]))
+                                continue;
+
+                        banks = numbank(MC_DOD_NUMBANK(dimm_dod[j]));
+                        ranks = numrank(MC_DOD_NUMRANK(dimm_dod[j]));
+                        rows = numrow(MC_DOD_NUMROW(dimm_dod[j]));
+                        cols = numcol(MC_DOD_NUMCOL(dimm_dod[j]));
+
+                        /* DDR3 has 8 I/O banks */
+                        size = (rows * cols * banks * ranks) >> (20 - 3);
+
+                        pvt->channel[i].dimms++;
+
+                        debugf0("\tdimm %d %d Mb offset: %x, "
+                                "bank: %d, rank: %d, row: %#x, col: %#x\n",
+                                j, size,
+                                RANKOFFSET(dimm_dod[j]),
+                                banks, ranks, rows, cols);
+
+#if PAGE_SHIFT > 20
+                        npages = size >> (PAGE_SHIFT - 20);
+#else
+                        npages = size << (20 - PAGE_SHIFT);
+#endif
+
+                        csr = &mci->csrows[*csrow];
+                        csr->first_page = last_page + 1;
+                        last_page += npages;
+                        csr->last_page = last_page;
+                        csr->nr_pages = npages;
+
+                        csr->page_mask = 0;
+                        csr->grain = 8;
+                        csr->csrow_idx = *csrow;
+                        csr->nr_channels = 1;
+
+                        csr->channels[0].chan_idx = i;
+                        csr->channels[0].ce_count = 0;
+
+                        pvt->csrow_map[i][j] = *csrow;
+
+                        switch (banks) {
+                        case 4:
+                                csr->dtype = DEV_X4;
+                                break;
+                        case 8:
+                                csr->dtype = DEV_X8;
+                                break;
+                        case 16:
+                                csr->dtype = DEV_X16;
+                                break;
+                        default:
+                                csr->dtype = DEV_UNKNOWN;
+                        }
+
+                        csr->edac_mode = mode;
+                        csr->mtype = mtype;
+
+                        (*csrow)++;
+                }
+
+                pci_read_config_dword(pdev, MC_SAG_CH_0, &value[0]);
+                pci_read_config_dword(pdev, MC_SAG_CH_1, &value[1]);
+                pci_read_config_dword(pdev, MC_SAG_CH_2, &value[2]);
+                pci_read_config_dword(pdev, MC_SAG_CH_3, &value[3]);
+                pci_read_config_dword(pdev, MC_SAG_CH_4, &value[4]);
+                pci_read_config_dword(pdev, MC_SAG_CH_5, &value[5]);
+                pci_read_config_dword(pdev, MC_SAG_CH_6, &value[6]);
+                pci_read_config_dword(pdev, MC_SAG_CH_7, &value[7]);
+                debugf1("\t[%i] DIVBY3\tREMOVED\tOFFSET\n", i);
+                for (j = 0; j < 8; j++)
+                        debugf1("\t\t%#x\t%#x\t%#x\n",
+                                (value[j] >> 27) & 0x1,
+                                (value[j] >> 24) & 0x7,
+                                (value[j] && ((1 << 24) - 1)));
+        }
+
+        return 0;
+}
+
+/****************************************************************************
+                        Error insertion routines
+ ****************************************************************************/
+
+/* The i7core has independent error injection features per channel.
+   However, to have a simpler code, we don't allow enabling error injection
+   on more than one channel.
+   Also, since a change at an inject parameter will be applied only at enable,
+   we're disabling error injection on all write calls to the sysfs nodes that
+   controls the error code injection.
+ */
+static int disable_inject(struct mem_ctl_info *mci)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+
+        pvt->inject.enable = 0;
+
+        if (!pvt->pci_ch[pvt->inject.channel][0])
+                return -ENODEV;
+
+        pci_write_config_dword(pvt->pci_ch[pvt->inject.channel][0],
+                                MC_CHANNEL_ERROR_INJECT, 0);
+
+        return 0;
+}
+
+/*
+ * i7core inject inject.section
+ *
+ *      accept and store error injection inject.section value
+ *      bit 0 - refers to the lower 32-byte half cacheline
+ *      bit 1 - refers to the upper 32-byte half cacheline
+ */
+static ssize_t i7core_inject_section_store(struct mem_ctl_info *mci,
+                                           const char *data, size_t count)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        unsigned long value;
+        int rc;
+
+        if (pvt->inject.enable)
+                disable_inject(mci);
+
+        rc = strict_strtoul(data, 10, &value);
+        if ((rc < 0) || (value > 3))
+                return -EIO;
+
+        pvt->inject.section = (u32) value;
+        return count;
+}
+
+static ssize_t i7core_inject_section_show(struct mem_ctl_info *mci,
+                                              char *data)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        return sprintf(data, "0x%08x\n", pvt->inject.section);
+}
+
+/*
+ * i7core inject.type
+ *
+ *      accept and store error injection inject.section value
+ *      bit 0 - repeat enable - Enable error repetition
+ *      bit 1 - inject ECC error
+ *      bit 2 - inject parity error
+ */
+static ssize_t i7core_inject_type_store(struct mem_ctl_info *mci,
+                                        const char *data, size_t count)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        unsigned long value;
+        int rc;
+
+        if (pvt->inject.enable)
+                disable_inject(mci);
+
+        rc = strict_strtoul(data, 10, &value);
+        if ((rc < 0) || (value > 7))
+                return -EIO;
+
+        pvt->inject.type = (u32) value;
+        return count;
+}
+
+static ssize_t i7core_inject_type_show(struct mem_ctl_info *mci,
+                                              char *data)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        return sprintf(data, "0x%08x\n", pvt->inject.type);
+}
+
+/*
+ * i7core_inject_inject.eccmask_store
+ *
+ * The type of error (UE/CE) will depend on the inject.eccmask value:
+ *   Any bits set to a 1 will flip the corresponding ECC bit
+ *   Correctable errors can be injected by flipping 1 bit or the bits within
+ *   a symbol pair (2 consecutive aligned 8-bit pairs - i.e. 7:0 and 15:8 or
+ *   23:16 and 31:24). Flipping bits in two symbol pairs will cause an
+ *   uncorrectable error to be injected.
+ */
+static ssize_t i7core_inject_eccmask_store(struct mem_ctl_info *mci,
+                                        const char *data, size_t count)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        unsigned long value;
+        int rc;
+
+        if (pvt->inject.enable)
+                disable_inject(mci);
+
+        rc = strict_strtoul(data, 10, &value);
+        if (rc < 0)
+                return -EIO;
+
+        pvt->inject.eccmask = (u32) value;
+        return count;
+}
+
+static ssize_t i7core_inject_eccmask_show(struct mem_ctl_info *mci,
+                                              char *data)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        return sprintf(data, "0x%08x\n", pvt->inject.eccmask);
+}
+
+/*
+ * i7core_addrmatch
+ *
+ * The type of error (UE/CE) will depend on the inject.eccmask value:
+ *   Any bits set to a 1 will flip the corresponding ECC bit
+ *   Correctable errors can be injected by flipping 1 bit or the bits within
+ *   a symbol pair (2 consecutive aligned 8-bit pairs - i.e. 7:0 and 15:8 or
+ *   23:16 and 31:24). Flipping bits in two symbol pairs will cause an
+ *   uncorrectable error to be injected.
+ */
+
+#define DECLARE_ADDR_MATCH(param, limit)                        \
+static ssize_t i7core_inject_store_##param(                     \
+                struct mem_ctl_info *mci,                       \
+                const char *data, size_t count)                 \
+{                                                               \
+        struct i7core_pvt *pvt;                                 \
+        long value;                                             \
+        int rc;                                                 \
+                                                                \
+        debugf1("%s()\n", __func__);                            \
+        pvt = mci->pvt_info;                                    \
+                                                                \
+        if (pvt->inject.enable)                                 \
+                disable_inject(mci);                            \
+                                                                \
+        if (!strcasecmp(data, "any") || !strcasecmp(data, "any\n"))\
+                value = -1;                                     \
+        else {                                                  \
+                rc = strict_strtoul(data, 10, &value);          \
+                if ((rc < 0) || (value >= limit))               \
+                        return -EIO;                            \
+        }                                                       \
+                                                                \
+        pvt->inject.param = value;                              \
+                                                                \
+        return count;                                           \
+}                                                               \
+                                                                \
+static ssize_t i7core_inject_show_##param(                      \
+                struct mem_ctl_info *mci,                       \
+                char *data)                                     \
+{                                                               \
+        struct i7core_pvt *pvt;                                 \
+                                                                \
+        pvt = mci->pvt_info;                                    \
+        debugf1("%s() pvt=%p\n", __func__, pvt);                \
+        if (pvt->inject.param < 0)                              \
+                return sprintf(data, "any\n");                  \
+        else                                                    \
+                return sprintf(data, "%d\n", pvt->inject.param);\
+}
+
+#define ATTR_ADDR_MATCH(param)                                  \
+        {                                                       \
+                .attr = {                                       \
+                        .name = #param,                         \
+                        .mode = (S_IRUGO | S_IWUSR)             \
+                },                                              \
+                .show  = i7core_inject_show_##param,            \
+                .store = i7core_inject_store_##param,           \
+        }
+
+DECLARE_ADDR_MATCH(channel, 3);
+DECLARE_ADDR_MATCH(dimm, 3);
+DECLARE_ADDR_MATCH(rank, 4);
+DECLARE_ADDR_MATCH(bank, 32);
+DECLARE_ADDR_MATCH(page, 0x10000);
+DECLARE_ADDR_MATCH(col, 0x4000);
+
+static int write_and_test(struct pci_dev *dev, int where, u32 val)
+{
+        u32 read;
+        int count;
+
+        debugf0("setting pci %02x:%02x.%x reg=%02x value=%08x\n",
+                dev->bus->number, PCI_SLOT(dev->devfn), PCI_FUNC(dev->devfn),
+                where, val);
+
+        for (count = 0; count < 10; count++) {
+                if (count)
+                        msleep(100);
+                pci_write_config_dword(dev, where, val);
+                pci_read_config_dword(dev, where, &read);
+
+                if (read == val)
+                        return 0;
+        }
+
+        i7core_printk(KERN_ERR, "Error during set pci %02x:%02x.%x reg=%02x "
+                "write=%08x. Read=%08x\n",
+                dev->bus->number, PCI_SLOT(dev->devfn), PCI_FUNC(dev->devfn),
+                where, val, read);
+
+        return -EINVAL;
+}
+
+/*
+ * This routine prepares the Memory Controller for error injection.
+ * The error will be injected when some process tries to write to the
+ * memory that matches the given criteria.
+ * The criteria can be set in terms of a mask where dimm, rank, bank, page
+ * and col can be specified.
+ * A -1 value for any of the mask items will make the MCU to ignore
+ * that matching criteria for error injection.
+ *
+ * It should be noticed that the error will only happen after a write operation
+ * on a memory that matches the condition. if REPEAT_EN is not enabled at
+ * inject mask, then it will produce just one error. Otherwise, it will repeat
+ * until the injectmask would be cleaned.
+ *
+ * FIXME: This routine assumes that MAXNUMDIMMS value of MC_MAX_DOD
+ *    is reliable enough to check if the MC is using the
+ *    three channels. However, this is not clear at the datasheet.
+ */
+static ssize_t i7core_inject_enable_store(struct mem_ctl_info *mci,
+                                       const char *data, size_t count)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        u32 injectmask;
+        u64 mask = 0;
+        int  rc;
+        long enable;
+
+        if (!pvt->pci_ch[pvt->inject.channel][0])
+                return 0;
+
+        rc = strict_strtoul(data, 10, &enable);
+        if ((rc < 0))
+                return 0;
+
+        if (enable) {
+                pvt->inject.enable = 1;
+        } else {
+                disable_inject(mci);
+                return count;
+        }
+
+        /* Sets pvt->inject.dimm mask */
+        if (pvt->inject.dimm < 0)
+                mask |= 1LL << 41;
+        else {
+                if (pvt->channel[pvt->inject.channel].dimms > 2)
+                        mask |= (pvt->inject.dimm & 0x3LL) << 35;
+                else
+                        mask |= (pvt->inject.dimm & 0x1LL) << 36;
+        }
+
+        /* Sets pvt->inject.rank mask */
+        if (pvt->inject.rank < 0)
+                mask |= 1LL << 40;
+        else {
+                if (pvt->channel[pvt->inject.channel].dimms > 2)
+                        mask |= (pvt->inject.rank & 0x1LL) << 34;
+                else
+                        mask |= (pvt->inject.rank & 0x3LL) << 34;
+        }
+
+        /* Sets pvt->inject.bank mask */
+        if (pvt->inject.bank < 0)
+                mask |= 1LL << 39;
+        else
+                mask |= (pvt->inject.bank & 0x15LL) << 30;
+
+        /* Sets pvt->inject.page mask */
+        if (pvt->inject.page < 0)
+                mask |= 1LL << 38;
+        else
+                mask |= (pvt->inject.page & 0xffff) << 14;
+
+        /* Sets pvt->inject.column mask */
+        if (pvt->inject.col < 0)
+                mask |= 1LL << 37;
+        else
+                mask |= (pvt->inject.col & 0x3fff);
+
+        /*
+         * bit    0: REPEAT_EN
+         * bits 1-2: MASK_HALF_CACHELINE
+         * bit    3: INJECT_ECC
+         * bit    4: INJECT_ADDR_PARITY
+         */
+
+        injectmask = (pvt->inject.type & 1) |
+                     (pvt->inject.section & 0x3) << 1 |
+                     (pvt->inject.type & 0x6) << (3 - 1);
+
+        /* Unlock writes to registers - this register is write only */
+        pci_write_config_dword(pvt->pci_noncore,
+                               MC_CFG_CONTROL, 0x2);
+
+        write_and_test(pvt->pci_ch[pvt->inject.channel][0],
+                               MC_CHANNEL_ADDR_MATCH, mask);
+        write_and_test(pvt->pci_ch[pvt->inject.channel][0],
+                               MC_CHANNEL_ADDR_MATCH + 4, mask >> 32L);
+
+        write_and_test(pvt->pci_ch[pvt->inject.channel][0],
+                               MC_CHANNEL_ERROR_MASK, pvt->inject.eccmask);
+
+        write_and_test(pvt->pci_ch[pvt->inject.channel][0],
+                               MC_CHANNEL_ERROR_INJECT, injectmask);
+
+        /*
+         * This is something undocumented, based on my tests
+         * Without writing 8 to this register, errors aren't injected. Not sure
+         * why.
+         */
+        pci_write_config_dword(pvt->pci_noncore,
+                               MC_CFG_CONTROL, 8);
+
+        debugf0("Error inject addr match 0x%016llx, ecc 0x%08x,"
+                " inject 0x%08x\n",
+                mask, pvt->inject.eccmask, injectmask);
+
+
+        return count;
+}
+
+static ssize_t i7core_inject_enable_show(struct mem_ctl_info *mci,
+                                        char *data)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        u32 injectmask;
+
+        if (!pvt->pci_ch[pvt->inject.channel][0])
+                return 0;
+
+        pci_read_config_dword(pvt->pci_ch[pvt->inject.channel][0],
+                               MC_CHANNEL_ERROR_INJECT, &injectmask);
+
+        debugf0("Inject error read: 0x%018x\n", injectmask);
+
+        if (injectmask & 0x0c)
+                pvt->inject.enable = 1;
+
+        return sprintf(data, "%d\n", pvt->inject.enable);
+}
+
+#define DECLARE_COUNTER(param)                                  \
+static ssize_t i7core_show_counter_##param(                     \
+                struct mem_ctl_info *mci,                       \
+                char *data)                                     \
+{                                                               \
+        struct i7core_pvt *pvt = mci->pvt_info;                 \
+                                                                \
+        debugf1("%s() \n", __func__);                           \
+        if (!pvt->ce_count_available || (pvt->is_registered))   \
+                return sprintf(data, "data unavailable\n");     \
+        return sprintf(data, "%lu\n",                           \
+                        pvt->udimm_ce_count[param]);            \
+}
+
+#define ATTR_COUNTER(param)                                     \
+        {                                                       \
+                .attr = {                                       \
+                        .name = __stringify(udimm##param),      \
+                        .mode = (S_IRUGO | S_IWUSR)             \
+                },                                              \
+                .show  = i7core_show_counter_##param            \
+        }
+
+DECLARE_COUNTER(0);
+DECLARE_COUNTER(1);
+DECLARE_COUNTER(2);
+
+/*
+ * Sysfs struct
+ */
+
+
+static struct mcidev_sysfs_attribute i7core_addrmatch_attrs[] = {
+        ATTR_ADDR_MATCH(channel),
+        ATTR_ADDR_MATCH(dimm),
+        ATTR_ADDR_MATCH(rank),
+        ATTR_ADDR_MATCH(bank),
+        ATTR_ADDR_MATCH(page),
+        ATTR_ADDR_MATCH(col),
+        { .attr = { .name = NULL } }
+};
+
+static struct mcidev_sysfs_group i7core_inject_addrmatch = {
+        .name  = "inject_addrmatch",
+        .mcidev_attr = i7core_addrmatch_attrs,
+};
+
+static struct mcidev_sysfs_attribute i7core_udimm_counters_attrs[] = {
+        ATTR_COUNTER(0),
+        ATTR_COUNTER(1),
+        ATTR_COUNTER(2),
+};
+
+static struct mcidev_sysfs_group i7core_udimm_counters = {
+        .name  = "all_channel_counts",
+        .mcidev_attr = i7core_udimm_counters_attrs,
+};
+
+static struct mcidev_sysfs_attribute i7core_sysfs_attrs[] = {
+        {
+                .attr = {
+                        .name = "inject_section",
+                        .mode = (S_IRUGO | S_IWUSR)
+                },
+                .show  = i7core_inject_section_show,
+                .store = i7core_inject_section_store,
+        }, {
+                .attr = {
+                        .name = "inject_type",
+                        .mode = (S_IRUGO | S_IWUSR)
+                },
+                .show  = i7core_inject_type_show,
+                .store = i7core_inject_type_store,
+        }, {
+                .attr = {
+                        .name = "inject_eccmask",
+                        .mode = (S_IRUGO | S_IWUSR)
+                },
+                .show  = i7core_inject_eccmask_show,
+                .store = i7core_inject_eccmask_store,
+        }, {
+                .grp = &i7core_inject_addrmatch,
+        }, {
+                .attr = {
+                        .name = "inject_enable",
+                        .mode = (S_IRUGO | S_IWUSR)
+                },
+                .show  = i7core_inject_enable_show,
+                .store = i7core_inject_enable_store,
+        },
+        { .attr = { .name = NULL } },   /* Reserved for udimm counters */
+        { .attr = { .name = NULL } }
+};
+
+/****************************************************************************
+        Device initialization routines: put/get, init/exit
+ ****************************************************************************/
+
+/*
+ *      i7core_put_devices      'put' all the devices that we have
+ *                              reserved via 'get'
+ */
+static void i7core_put_devices(struct i7core_dev *i7core_dev)
+{
+        int i;
+
+        debugf0(__FILE__ ": %s()\n", __func__);
+        for (i = 0; i < i7core_dev->n_devs; i++) {
+                struct pci_dev *pdev = i7core_dev->pdev[i];
+                if (!pdev)
+                        continue;
+                debugf0("Removing dev %02x:%02x.%d\n",
+                        pdev->bus->number,
+                        PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
+                pci_dev_put(pdev);
+        }
+        kfree(i7core_dev->pdev);
+        list_del(&i7core_dev->list);
+        kfree(i7core_dev);
+}
+
+static void i7core_put_all_devices(void)
+{
+        struct i7core_dev *i7core_dev, *tmp;
+
+        list_for_each_entry_safe(i7core_dev, tmp, &i7core_edac_list, list)
+                i7core_put_devices(i7core_dev);
+}
+
+static void __init i7core_xeon_pci_fixup(struct pci_id_table *table)
+{
+        struct pci_dev *pdev = NULL;
+        int i;
+        /*
+         * On Xeon 55xx, the Intel Quckpath Arch Generic Non-core pci buses
+         * aren't announced by acpi. So, we need to use a legacy scan probing
+         * to detect them
+         */
+        while (table && table->descr) {
+                pdev = pci_get_device(PCI_VENDOR_ID_INTEL, table->descr[0].dev_id, NULL);
+                if (unlikely(!pdev)) {
+                        for (i = 0; i < MAX_SOCKET_BUSES; i++)
+                                pcibios_scan_specific_bus(255-i);
+                }
+                table++;
+        }
+}
+
+/*
+ *      i7core_get_devices      Find and perform 'get' operation on the MCH's
+ *                      device/functions we want to reference for this driver
+ *
+ *                      Need to 'get' device 16 func 1 and func 2
+ */
+int i7core_get_onedevice(struct pci_dev **prev, int devno,
+                         struct pci_id_descr *dev_descr, unsigned n_devs)
+{
+        struct i7core_dev *i7core_dev;
+
+        struct pci_dev *pdev = NULL;
+        u8 bus = 0;
+        u8 socket = 0;
+
+        pdev = pci_get_device(PCI_VENDOR_ID_INTEL,
+                              dev_descr->dev_id, *prev);
+
+        /*
+         * On Xeon 55xx, the Intel Quckpath Arch Generic Non-core regs
+         * is at addr 8086:2c40, instead of 8086:2c41. So, we need
+         * to probe for the alternate address in case of failure
+         */
+        if (dev_descr->dev_id == PCI_DEVICE_ID_INTEL_I7_NONCORE && !pdev)
+                pdev = pci_get_device(PCI_VENDOR_ID_INTEL,
+                                      PCI_DEVICE_ID_INTEL_I7_NONCORE_ALT, *prev);
+
+        if (dev_descr->dev_id == PCI_DEVICE_ID_INTEL_LYNNFIELD_NONCORE && !pdev)
+                pdev = pci_get_device(PCI_VENDOR_ID_INTEL,
+                                      PCI_DEVICE_ID_INTEL_LYNNFIELD_NONCORE_ALT,
+                                      *prev);
+
+        if (!pdev) {
+                if (*prev) {
+                        *prev = pdev;
+                        return 0;
+                }
+
+                if (dev_descr->optional)
+                        return 0;
+
+                if (devno == 0)
+                        return -ENODEV;
+
+                i7core_printk(KERN_ERR,
+                        "Device not found: dev %02x.%d PCI ID %04x:%04x\n",
+                        dev_descr->dev, dev_descr->func,
+                        PCI_VENDOR_ID_INTEL, dev_descr->dev_id);
+
+                /* End of list, leave */
+                return -ENODEV;
+        }
+        bus = pdev->bus->number;
+
+        if (bus == 0x3f)
+                socket = 0;
+        else
+                socket = 255 - bus;
+
+        i7core_dev = get_i7core_dev(socket);
+        if (!i7core_dev) {
+                i7core_dev = kzalloc(sizeof(*i7core_dev), GFP_KERNEL);
+                if (!i7core_dev)
+                        return -ENOMEM;
+                i7core_dev->pdev = kzalloc(sizeof(*i7core_dev->pdev) * n_devs,
+                                           GFP_KERNEL);
+                if (!i7core_dev->pdev) {
+                        kfree(i7core_dev);
+                        return -ENOMEM;
+                }
+                i7core_dev->socket = socket;
+                i7core_dev->n_devs = n_devs;
+                list_add_tail(&i7core_dev->list, &i7core_edac_list);
+        }
+
+        if (i7core_dev->pdev[devno]) {
+                i7core_printk(KERN_ERR,
+                        "Duplicated device for "
+                        "dev %02x:%02x.%d PCI ID %04x:%04x\n",
+                        bus, dev_descr->dev, dev_descr->func,
+                        PCI_VENDOR_ID_INTEL, dev_descr->dev_id);
+                pci_dev_put(pdev);
+                return -ENODEV;
+        }
+
+        i7core_dev->pdev[devno] = pdev;
+
+        /* Sanity check */
+        if (unlikely(PCI_SLOT(pdev->devfn) != dev_descr->dev ||
+                        PCI_FUNC(pdev->devfn) != dev_descr->func)) {
+                i7core_printk(KERN_ERR,
+                        "Device PCI ID %04x:%04x "
+                        "has dev %02x:%02x.%d instead of dev %02x:%02x.%d\n",
+                        PCI_VENDOR_ID_INTEL, dev_descr->dev_id,
+                        bus, PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn),
+                        bus, dev_descr->dev, dev_descr->func);
+                return -ENODEV;
+        }
+
+        /* Be sure that the device is enabled */
+        if (unlikely(pci_enable_device(pdev) < 0)) {
+                i7core_printk(KERN_ERR,
+                        "Couldn't enable "
+                        "dev %02x:%02x.%d PCI ID %04x:%04x\n",
+                        bus, dev_descr->dev, dev_descr->func,
+                        PCI_VENDOR_ID_INTEL, dev_descr->dev_id);
+                return -ENODEV;
+        }
+
+        debugf0("Detected socket %d dev %02x:%02x.%d PCI ID %04x:%04x\n",
+                socket, bus, dev_descr->dev,
+                dev_descr->func,
+                PCI_VENDOR_ID_INTEL, dev_descr->dev_id);
+
+        *prev = pdev;
+
+        return 0;
+}
+
+static int i7core_get_devices(struct pci_id_table *table)
+{
+        int i, rc;
+        struct pci_dev *pdev = NULL;
+        struct pci_id_descr *dev_descr;
+
+        while (table && table->descr) {
+                dev_descr = table->descr;
+                for (i = 0; i < table->n_devs; i++) {
+                        pdev = NULL;
+                        do {
+                                rc = i7core_get_onedevice(&pdev, i, &dev_descr[i],
+                                                          table->n_devs);
+                                if (rc < 0) {
+                                        if (i == 0) {
+                                                i = table->n_devs;
+                                                break;
+                                        }
+                                        i7core_put_all_devices();
+                                        return -ENODEV;
+                                }
+                        } while (pdev);
+                }
+                table++;
+        }
+
+        return 0;
+        return 0;
+}
+
+static int mci_bind_devs(struct mem_ctl_info *mci,
+                         struct i7core_dev *i7core_dev)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        struct pci_dev *pdev;
+        int i, func, slot;
+
+        /* Associates i7core_dev and mci for future usage */
+        pvt->i7core_dev = i7core_dev;
+        i7core_dev->mci = mci;
+
+        pvt->is_registered = 0;
+        for (i = 0; i < i7core_dev->n_devs; i++) {
+                pdev = i7core_dev->pdev[i];
+                if (!pdev)
+                        continue;
+
+                func = PCI_FUNC(pdev->devfn);
+                slot = PCI_SLOT(pdev->devfn);
+                if (slot == 3) {
+                        if (unlikely(func > MAX_MCR_FUNC))
+                                goto error;
+                        pvt->pci_mcr[func] = pdev;
+                } else if (likely(slot >= 4 && slot < 4 + NUM_CHANS)) {
+                        if (unlikely(func > MAX_CHAN_FUNC))
+                                goto error;
+                        pvt->pci_ch[slot - 4][func] = pdev;
+                } else if (!slot && !func)
+                        pvt->pci_noncore = pdev;
+                else
+                        goto error;
+
+                debugf0("Associated fn %d.%d, dev = %p, socket %d\n",
+                        PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn),
+                        pdev, i7core_dev->socket);
+
+                if (PCI_SLOT(pdev->devfn) == 3 &&
+                        PCI_FUNC(pdev->devfn) == 2)
+                        pvt->is_registered = 1;
+        }
+
+        /*
+         * Add extra nodes to count errors on udimm
+         * For registered memory, this is not needed, since the counters
+         * are already displayed at the standard locations
+         */
+        if (!pvt->is_registered)
+                i7core_sysfs_attrs[ARRAY_SIZE(i7core_sysfs_attrs)-2].grp =
+                        &i7core_udimm_counters;
+
+        return 0;
+
+error:
+        i7core_printk(KERN_ERR, "Device %d, function %d "
+                      "is out of the expected range\n",
+                      slot, func);
+        return -EINVAL;
+}
+
+/****************************************************************************
+                        Error check routines
+ ****************************************************************************/
+static void i7core_rdimm_update_csrow(struct mem_ctl_info *mci,
+                                         int chan, int dimm, int add)
+{
+        char *msg;
+        struct i7core_pvt *pvt = mci->pvt_info;
+        int row = pvt->csrow_map[chan][dimm], i;
+
+        for (i = 0; i < add; i++) {
+                msg = kasprintf(GFP_KERNEL, "Corrected error "
+                                "(Socket=%d channel=%d dimm=%d)",
+                                pvt->i7core_dev->socket, chan, dimm);
+
+                edac_mc_handle_fbd_ce(mci, row, 0, msg);
+                kfree (msg);
+        }
+}
+
+static void i7core_rdimm_update_ce_count(struct mem_ctl_info *mci,
+                        int chan, int new0, int new1, int new2)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        int add0 = 0, add1 = 0, add2 = 0;
+        /* Updates CE counters if it is not the first time here */
+        if (pvt->ce_count_available) {
+                /* Updates CE counters */
+
+                add2 = new2 - pvt->rdimm_last_ce_count[chan][2];
+                add1 = new1 - pvt->rdimm_last_ce_count[chan][1];
+                add0 = new0 - pvt->rdimm_last_ce_count[chan][0];
+
+                if (add2 < 0)
+                        add2 += 0x7fff;
+                pvt->rdimm_ce_count[chan][2] += add2;
+
+                if (add1 < 0)
+                        add1 += 0x7fff;
+                pvt->rdimm_ce_count[chan][1] += add1;
+
+                if (add0 < 0)
+                        add0 += 0x7fff;
+                pvt->rdimm_ce_count[chan][0] += add0;
+        } else
+                pvt->ce_count_available = 1;
+
+        /* Store the new values */
+        pvt->rdimm_last_ce_count[chan][2] = new2;
+        pvt->rdimm_last_ce_count[chan][1] = new1;
+        pvt->rdimm_last_ce_count[chan][0] = new0;
+
+        /*updated the edac core */
+        if (add0 != 0)
+                i7core_rdimm_update_csrow(mci, chan, 0, add0);
+        if (add1 != 0)
+                i7core_rdimm_update_csrow(mci, chan, 1, add1);
+        if (add2 != 0)
+                i7core_rdimm_update_csrow(mci, chan, 2, add2);
+
+}
+
+static void i7core_rdimm_check_mc_ecc_err(struct mem_ctl_info *mci)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        u32 rcv[3][2];
+        int i, new0, new1, new2;
+
+        /*Read DEV 3: FUN 2:  MC_COR_ECC_CNT regs directly*/
+        pci_read_config_dword(pvt->pci_mcr[2], MC_COR_ECC_CNT_0,
+                                                                &rcv[0][0]);
+        pci_read_config_dword(pvt->pci_mcr[2], MC_COR_ECC_CNT_1,
+                                                                &rcv[0][1]);
+        pci_read_config_dword(pvt->pci_mcr[2], MC_COR_ECC_CNT_2,
+                                                                &rcv[1][0]);
+        pci_read_config_dword(pvt->pci_mcr[2], MC_COR_ECC_CNT_3,
+                                                                &rcv[1][1]);
+        pci_read_config_dword(pvt->pci_mcr[2], MC_COR_ECC_CNT_4,
+                                                                &rcv[2][0]);
+        pci_read_config_dword(pvt->pci_mcr[2], MC_COR_ECC_CNT_5,
+                                                                &rcv[2][1]);
+        for (i = 0 ; i < 3; i++) {
+                debugf3("MC_COR_ECC_CNT%d = 0x%x; MC_COR_ECC_CNT%d = 0x%x\n",
+                        (i * 2), rcv[i][0], (i * 2) + 1, rcv[i][1]);
+                /*if the channel has 3 dimms*/
+                if (pvt->channel[i].dimms > 2) {
+                        new0 = DIMM_BOT_COR_ERR(rcv[i][0]);
+                        new1 = DIMM_TOP_COR_ERR(rcv[i][0]);
+                        new2 = DIMM_BOT_COR_ERR(rcv[i][1]);
+                } else {
+                        new0 = DIMM_TOP_COR_ERR(rcv[i][0]) +
+                                        DIMM_BOT_COR_ERR(rcv[i][0]);
+                        new1 = DIMM_TOP_COR_ERR(rcv[i][1]) +
+                                        DIMM_BOT_COR_ERR(rcv[i][1]);
+                        new2 = 0;
+                }
+
+                i7core_rdimm_update_ce_count(mci, i, new0, new1, new2);
+        }
+}
+
+/* This function is based on the device 3 function 4 registers as described on:
+ * Intel Xeon Processor 5500 Series Datasheet Volume 2
+ *      http://www.intel.com/Assets/PDF/datasheet/321322.pdf
+ * also available at:
+ *      http://www.arrownac.com/manufacturers/intel/s/nehalem/5500-datasheet-v2.pdf
+ */
+static void i7core_udimm_check_mc_ecc_err(struct mem_ctl_info *mci)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        u32 rcv1, rcv0;
+        int new0, new1, new2;
+
+        if (!pvt->pci_mcr[4]) {
+                debugf0("%s MCR registers not found\n", __func__);
+                return;
+        }
+
+        /* Corrected test errors */
+        pci_read_config_dword(pvt->pci_mcr[4], MC_TEST_ERR_RCV1, &rcv1);
+        pci_read_config_dword(pvt->pci_mcr[4], MC_TEST_ERR_RCV0, &rcv0);
+
+        /* Store the new values */
+        new2 = DIMM2_COR_ERR(rcv1);
+        new1 = DIMM1_COR_ERR(rcv0);
+        new0 = DIMM0_COR_ERR(rcv0);
+
+        /* Updates CE counters if it is not the first time here */
+        if (pvt->ce_count_available) {
+                /* Updates CE counters */
+                int add0, add1, add2;
+
+                add2 = new2 - pvt->udimm_last_ce_count[2];
+                add1 = new1 - pvt->udimm_last_ce_count[1];
+                add0 = new0 - pvt->udimm_last_ce_count[0];
+
+                if (add2 < 0)
+                        add2 += 0x7fff;
+                pvt->udimm_ce_count[2] += add2;
+
+                if (add1 < 0)
+                        add1 += 0x7fff;
+                pvt->udimm_ce_count[1] += add1;
+
+                if (add0 < 0)
+                        add0 += 0x7fff;
+                pvt->udimm_ce_count[0] += add0;
+
+                if (add0 | add1 | add2)
+                        i7core_printk(KERN_ERR, "New Corrected error(s): "
+                                      "dimm0: +%d, dimm1: +%d, dimm2 +%d\n",
+                                      add0, add1, add2);
+        } else
+                pvt->ce_count_available = 1;
+
+        /* Store the new values */
+        pvt->udimm_last_ce_count[2] = new2;
+        pvt->udimm_last_ce_count[1] = new1;
+        pvt->udimm_last_ce_count[0] = new0;
+}
+
+/*
+ * According with tables E-11 and E-12 of chapter E.3.3 of Intel 64 and IA-32
+ * Architectures Software Developer’s Manual Volume 3B.
+ * Nehalem are defined as family 0x06, model 0x1a
+ *
+ * The MCA registers used here are the following ones:
+ *     struct mce field MCA Register
+ *     m->status        MSR_IA32_MC8_STATUS
+ *     m->addr          MSR_IA32_MC8_ADDR
+ *     m->misc          MSR_IA32_MC8_MISC
+ * In the case of Nehalem, the error information is masked at .status and .misc
+ * fields
+ */
+static void i7core_mce_output_error(struct mem_ctl_info *mci,
+                                    struct mce *m)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        char *type, *optype, *err, *msg;
+        unsigned long error = m->status & 0x1ff0000l;
+        u32 optypenum = (m->status >> 4) & 0x07;
+        u32 core_err_cnt = (m->status >> 38) && 0x7fff;
+        u32 dimm = (m->misc >> 16) & 0x3;
+        u32 channel = (m->misc >> 18) & 0x3;
+        u32 syndrome = m->misc >> 32;
+        u32 errnum = find_first_bit(&error, 32);
+        int csrow;
+
+        if (m->mcgstatus & 1)
+                type = "FATAL";
+        else
+                type = "NON_FATAL";
+
+        switch (optypenum) {
+        case 0:
+                optype = "generic undef request";
+                break;
+        case 1:
+                optype = "read error";
+                break;
+        case 2:
+                optype = "write error";
+                break;
+        case 3:
+                optype = "addr/cmd error";
+                break;
+        case 4:
+                optype = "scrubbing error";
+                break;
+        default:
+                optype = "reserved";
+                break;
+        }
+
+        switch (errnum) {
+        case 16:
+                err = "read ECC error";
+                break;
+        case 17:
+                err = "RAS ECC error";
+                break;
+        case 18:
+                err = "write parity error";
+                break;
+        case 19:
+                err = "redundacy loss";
+                break;
+        case 20:
+                err = "reserved";
+                break;
+        case 21:
+                err = "memory range error";
+                break;
+        case 22:
+                err = "RTID out of range";
+                break;
+        case 23:
+                err = "address parity error";
+                break;
+        case 24:
+                err = "byte enable parity error";
+                break;
+        default:
+                err = "unknown";
+        }
+
+        /* FIXME: should convert addr into bank and rank information */
+        msg = kasprintf(GFP_ATOMIC,
+                "%s (addr = 0x%08llx, cpu=%d, Dimm=%d, Channel=%d, "
+                "syndrome=0x%08x, count=%d, Err=%08llx:%08llx (%s: %s))\n",
+                type, (long long) m->addr, m->cpu, dimm, channel,
+                syndrome, core_err_cnt, (long long)m->status,
+                (long long)m->misc, optype, err);
+
+        debugf0("%s", msg);
+
+        csrow = pvt->csrow_map[channel][dimm];
+
+        /* Call the helper to output message */
+        if (m->mcgstatus & 1)
+                edac_mc_handle_fbd_ue(mci, csrow, 0,
+                                0 /* FIXME: should be channel here */, msg);
+        else if (!pvt->is_registered)
+                edac_mc_handle_fbd_ce(mci, csrow,
+                                0 /* FIXME: should be channel here */, msg);
+
+        kfree(msg);
+}
+
+/*
+ *      i7core_check_error      Retrieve and process errors reported by the
+ *                              hardware. Called by the Core module.
+ */
+static void i7core_check_error(struct mem_ctl_info *mci)
+{
+        struct i7core_pvt *pvt = mci->pvt_info;
+        int i;
+        unsigned count = 0;
+        struct mce *m;
+
+        /*
+         * MCE first step: Copy all mce errors into a temporary buffer
+         * We use a double buffering here, to reduce the risk of
+         * loosing an error.
+         */
+        smp_rmb();
+        count = (pvt->mce_out + MCE_LOG_LEN - pvt->mce_in)
+                % MCE_LOG_LEN;
+        if (!count)
+                goto check_ce_error;
+
+        m = pvt->mce_outentry;
+        if (pvt->mce_in + count > MCE_LOG_LEN) {
+                unsigned l = MCE_LOG_LEN - pvt->mce_in;
+
+                memcpy(m, &pvt->mce_entry[pvt->mce_in], sizeof(*m) * l);
+                smp_wmb();
+                pvt->mce_in = 0;
+                count -= l;
+                m += l;
+        }
+        memcpy(m, &pvt->mce_entry[pvt->mce_in], sizeof(*m) * count);
+        smp_wmb();
+        pvt->mce_in += count;
+
+        smp_rmb();
+        if (pvt->mce_overrun) {
+                i7core_printk(KERN_ERR, "Lost %d memory errors\n",
+                              pvt->mce_overrun);
+                smp_wmb();
+                pvt->mce_overrun = 0;
+        }
+
+        /*
+         * MCE second step: parse errors and display
+         */
+        for (i = 0; i < count; i++)
+                i7core_mce_output_error(mci, &pvt->mce_outentry[i]);
+
+        /*
+         * Now, let's increment CE error counts
+         */
+check_ce_error:
+        if (!pvt->is_registered)
+                i7core_udimm_check_mc_ecc_err(mci);
+        else
+                i7core_rdimm_check_mc_ecc_err(mci);
+}
+
+/*
+ * i7core_mce_check_error       Replicates mcelog routine to get errors
+ *                              This routine simply queues mcelog errors, and
+ *                              return. The error itself should be handled later
+ *                              by i7core_check_error.
+ * WARNING: As this routine should be called at NMI time, extra care should
+ * be taken to avoid deadlocks, and to be as fast as possible.
+ */
+static int i7core_mce_check_error(void *priv, struct mce *mce)
+{
+        struct mem_ctl_info *mci = priv;
+        struct i7core_pvt *pvt = mci->pvt_info;
+
+        /*
+         * Just let mcelog handle it if the error is
+         * outside the memory controller
+         */
+        if (((mce->status & 0xffff) >> 7) != 1)
+                return 0;
+
+        /* Bank 8 registers are the only ones that we know how to handle */
+        if (mce->bank != 8)
+                return 0;
+
+#ifdef CONFIG_SMP
+        /* Only handle if it is the right mc controller */
+        if (cpu_data(mce->cpu).phys_proc_id != pvt->i7core_dev->socket)
+                return 0;
+#endif
+
+        smp_rmb();
+        if ((pvt->mce_out + 1) % MCE_LOG_LEN == pvt->mce_in) {
+                smp_wmb();
+                pvt->mce_overrun++;
+                return 0;
+        }
+
+        /* Copy memory error at the ringbuffer */
+        memcpy(&pvt->mce_entry[pvt->mce_out], mce, sizeof(*mce));
+        smp_wmb();
+        pvt->mce_out = (pvt->mce_out + 1) % MCE_LOG_LEN;
+
+        /* Handle fatal errors immediately */
+        if (mce->mcgstatus & 1)
+                i7core_check_error(mci);
+
+        /* Advice mcelog that the error were handled */
+        return 1;
+}
+
+static int i7core_register_mci(struct i7core_dev *i7core_dev,
+                               int num_channels, int num_csrows)
+{
+        struct mem_ctl_info *mci;
+        struct i7core_pvt *pvt;
+        int csrow = 0;
+        int rc;
+
+        /* allocate a new MC control structure */
+        mci = edac_mc_alloc(sizeof(*pvt), num_csrows, num_channels,
+                            i7core_dev->socket);
+        if (unlikely(!mci))
+                return -ENOMEM;
+
+        debugf0("MC: " __FILE__ ": %s(): mci = %p\n", __func__, mci);
+
+        /* record ptr to the generic device */
+        mci->dev = &i7core_dev->pdev[0]->dev;
+
+        pvt = mci->pvt_info;
+        memset(pvt, 0, sizeof(*pvt));
+
+        /*
+         * FIXME: how to handle RDDR3 at MCI level? It is possible to have
+         * Mixed RDDR3/UDDR3 with Nehalem, provided that they are on different
+         * memory channels
+         */
+        mci->mtype_cap = MEM_FLAG_DDR3;
+        mci->edac_ctl_cap = EDAC_FLAG_NONE;
+        mci->edac_cap = EDAC_FLAG_NONE;
+        mci->mod_name = "i7core_edac.c";
+        mci->mod_ver = I7CORE_REVISION;
+        mci->ctl_name = kasprintf(GFP_KERNEL, "i7 core #%d",
+                                  i7core_dev->socket);
+        mci->dev_name = pci_name(i7core_dev->pdev[0]);
+        mci->ctl_page_to_phys = NULL;
+        mci->mc_driver_sysfs_attributes = i7core_sysfs_attrs;
+        /* Set the function pointer to an actual operation function */
+        mci->edac_check = i7core_check_error;
+
+        /* Store pci devices at mci for faster access */
+        rc = mci_bind_devs(mci, i7core_dev);
+        if (unlikely(rc < 0))
+                goto fail;
+
+        /* Get dimm basic config */
+        get_dimm_config(mci, &csrow);
+
+        /* add this new MC control structure to EDAC's list of MCs */
+        if (unlikely(edac_mc_add_mc(mci))) {
+                debugf0("MC: " __FILE__
+                        ": %s(): failed edac_mc_add_mc()\n", __func__);
+                /* FIXME: perhaps some code should go here that disables error
+                 * reporting if we just enabled it
+                 */
+
+                rc = -EINVAL;
+                goto fail;
+        }
+
+        /* allocating generic PCI control info */
+        i7core_pci = edac_pci_create_generic_ctl(&i7core_dev->pdev[0]->dev,
+                                                 EDAC_MOD_STR);
+        if (unlikely(!i7core_pci)) {
+                printk(KERN_WARNING
+                        "%s(): Unable to create PCI control\n",
+                        __func__);
+                printk(KERN_WARNING
+                        "%s(): PCI error report via EDAC not setup\n",
+                        __func__);
+        }
+
+        /* Default error mask is any memory */
+        pvt->inject.channel = 0;
+        pvt->inject.dimm = -1;
+        pvt->inject.rank = -1;
+        pvt->inject.bank = -1;
+        pvt->inject.page = -1;
+        pvt->inject.col = -1;
+
+        /* Registers on edac_mce in order to receive memory errors */
+        pvt->edac_mce.priv = mci;
+        pvt->edac_mce.check_error = i7core_mce_check_error;
+
+        rc = edac_mce_register(&pvt->edac_mce);
+        if (unlikely(rc < 0)) {
+                debugf0("MC: " __FILE__
+                        ": %s(): failed edac_mce_register()\n", __func__);
+        }
+
+fail:
+        if (rc < 0)
+                edac_mc_free(mci);
+        return rc;
+}
+
+/*
+ *      i7core_probe    Probe for ONE instance of device to see if it is
+ *                      present.
+ *      return:
+ *              0 for FOUND a device
+ *              < 0 for error code
+ */
+static int __devinit i7core_probe(struct pci_dev *pdev,
+                                  const struct pci_device_id *id)
+{
+        int dev_idx = id->driver_data;
+        int rc;
+        struct i7core_dev *i7core_dev;
+
+        /*
+         * All memory controllers are allocated at the first pass.
+         */
+        if (unlikely(dev_idx >= 1))
+                return -EINVAL;
+
+        /* get the pci devices we want to reserve for our use */
+        mutex_lock(&i7core_edac_lock);
+
+        rc = i7core_get_devices(pci_dev_table);
+        if (unlikely(rc < 0))
+                goto fail0;
+
+        list_for_each_entry(i7core_dev, &i7core_edac_list, list) {
+                int channels;
+                int csrows;
+
+                /* Check the number of active and not disabled channels */
+                rc = i7core_get_active_channels(i7core_dev->socket,
+                                                &channels, &csrows);
+                if (unlikely(rc < 0))
+                        goto fail1;
+
+                rc = i7core_register_mci(i7core_dev, channels, csrows);
+                if (unlikely(rc < 0))
+                        goto fail1;
+        }
+
+        i7core_printk(KERN_INFO, "Driver loaded.\n");
+
+        mutex_unlock(&i7core_edac_lock);
+        return 0;
+
+fail1:
+        i7core_put_all_devices();
+fail0:
+        mutex_unlock(&i7core_edac_lock);
+        return rc;
+}
+
+/*
+ *      i7core_remove   destructor for one instance of device
+ *
+ */
+static void __devexit i7core_remove(struct pci_dev *pdev)
+{
+        struct mem_ctl_info *mci;
+        struct i7core_dev *i7core_dev, *tmp;
+
+        debugf0(__FILE__ ": %s()\n", __func__);
+
+        if (i7core_pci)
+                edac_pci_release_generic_ctl(i7core_pci);
+
+        /*
+         * we have a trouble here: pdev value for removal will be wrong, since
+         * it will point to the X58 register used to detect that the machine
+         * is a Nehalem or upper design. However, due to the way several PCI
+         * devices are grouped together to provide MC functionality, we need
+         * to use a different method for releasing the devices
+         */
+
+        mutex_lock(&i7core_edac_lock);
+        list_for_each_entry_safe(i7core_dev, tmp, &i7core_edac_list, list) {
+                mci = edac_mc_del_mc(&i7core_dev->pdev[0]->dev);
+                if (mci) {
+                        struct i7core_pvt *pvt = mci->pvt_info;
+
+                        i7core_dev = pvt->i7core_dev;
+                        edac_mce_unregister(&pvt->edac_mce);
+                        kfree(mci->ctl_name);
+                        edac_mc_free(mci);
+                        i7core_put_devices(i7core_dev);
+                } else {
+                        i7core_printk(KERN_ERR,
+                                      "Couldn't find mci for socket %d\n",
+                                      i7core_dev->socket);
+                }
+        }
+        mutex_unlock(&i7core_edac_lock);
+}
+
+MODULE_DEVICE_TABLE(pci, i7core_pci_tbl);
+
+/*
+ *      i7core_driver   pci_driver structure for this module
+ *
+ */
+static struct pci_driver i7core_driver = {
+        .name     = "i7core_edac",
+        .probe    = i7core_probe,
+        .remove   = __devexit_p(i7core_remove),
+        .id_table = i7core_pci_tbl,
+};
+
+/*
+ *      i7core_init             Module entry function
+ *                      Try to initialize this module for its devices
+ */
+static int __init i7core_init(void)
+{
+        int pci_rc;
+
+        debugf2("MC: " __FILE__ ": %s()\n", __func__);
+
+        /* Ensure that the OPSTATE is set correctly for POLL or NMI */
+        opstate_init();
+
+        i7core_xeon_pci_fixup(pci_dev_table);
+
+        pci_rc = pci_register_driver(&i7core_driver);
+
+        if (pci_rc >= 0)
+                return 0;
+
+        i7core_printk(KERN_ERR, "Failed to register device with error %d.\n",
+                      pci_rc);
+
+        return pci_rc;
+}
+
+/*
+ *      i7core_exit()   Module exit function
+ *                      Unregister the driver
+ */
+static void __exit i7core_exit(void)
+{
+        debugf2("MC: " __FILE__ ": %s()\n", __func__);
+        pci_unregister_driver(&i7core_driver);
+}
+
+module_init(i7core_init);
+module_exit(i7core_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
+MODULE_AUTHOR("Red Hat Inc. (http://www.redhat.com)");
+MODULE_DESCRIPTION("MC Driver for Intel i7 Core memory controllers - "
+                   I7CORE_REVISION);
+
+module_param(edac_op_state, int, 0444);
+MODULE_PARM_DESC(edac_op_state, "EDAC Error Reporting state: 0=Poll,1=NMI");
diff --git a/drivers/edac/i82443bxgx_edac.c b/drivers/edac/i82443bxgx_edac.c
index 2bf2c5051bfe..a2fa1feed724 100644
--- a/drivers/edac/i82443bxgx_edac.c
+++ b/drivers/edac/i82443bxgx_edac.c
@@ -178,7 +178,7 @@ static void i82443bxgx_edacmc_check(struct mem_ctl_info *mci)
 {
         struct i82443bxgx_edacmc_error_info info;
 
-        debugf1("MC%d: " __FILE__ ": %s()\n", mci->mc_idx, __func__);
+        debugf1("MC%d: %s: %s()\n", mci->mc_idx, __FILE__, __func__);
         i82443bxgx_edacmc_get_error_info(mci, &info);
         i82443bxgx_edacmc_process_error_info(mci, &info, 1);
 }
@@ -198,13 +198,13 @@ static void i82443bxgx_init_csrows(struct mem_ctl_info *mci,
         for (index = 0; index < mci->nr_csrows; index++) {
                 csrow = &mci->csrows[index];
                 pci_read_config_byte(pdev, I82443BXGX_DRB + index, &drbar);
-                debugf1("MC%d: " __FILE__ ": %s() Row=%d DRB = %#0x\n",
-                        mci->mc_idx, __func__, index, drbar);
+                debugf1("MC%d: %s: %s() Row=%d DRB = %#0x\n",
+                        mci->mc_idx, __FILE__, __func__, index, drbar);
                 row_high_limit = ((u32) drbar << 23);
                 /* find the DRAM Chip Select Base address and mask */
-                debugf1("MC%d: " __FILE__ ": %s() Row=%d, "
-                        "Boundry Address=%#0x, Last = %#0x \n",
-                        mci->mc_idx, __func__, index, row_high_limit,
+                debugf1("MC%d: %s: %s() Row=%d, "
+                        "Boundry Address=%#0x, Last = %#0x\n",
+                        mci->mc_idx, __FILE__, __func__, index, row_high_limit,
                         row_high_limit_last);
 
                 /* 440GX goes to 2GB, represented with a DRB of 0. */
@@ -237,7 +237,7 @@ static int i82443bxgx_edacmc_probe1(struct pci_dev *pdev, int dev_idx)
         enum mem_type mtype;
         enum edac_type edac_mode;
 
-        debugf0("MC: " __FILE__ ": %s()\n", __func__);
+        debugf0("MC: %s: %s()\n", __FILE__, __func__);
 
         /* Something is really hosed if PCI config space reads from
          * the MC aren't working.
@@ -250,7 +250,7 @@ static int i82443bxgx_edacmc_probe1(struct pci_dev *pdev, int dev_idx)
         if (mci == NULL)
                 return -ENOMEM;
 
-        debugf0("MC: " __FILE__ ": %s(): mci = %p\n", __func__, mci);
+        debugf0("MC: %s: %s(): mci = %p\n", __FILE__, __func__, mci);
         mci->dev = &pdev->dev;
         mci->mtype_cap = MEM_FLAG_EDO | MEM_FLAG_SDR | MEM_FLAG_RDR;
         mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_EC | EDAC_FLAG_SECDED;
@@ -336,7 +336,7 @@ static int i82443bxgx_edacmc_probe1(struct pci_dev *pdev, int dev_idx)
                         __func__);
         }
 
-        debugf3("MC: " __FILE__ ": %s(): success\n", __func__);
+        debugf3("MC: %s: %s(): success\n", __FILE__, __func__);
         return 0;
 
 fail:
@@ -352,7 +352,7 @@ static int __devinit i82443bxgx_edacmc_init_one(struct pci_dev *pdev,
 {
         int rc;
 
-        debugf0("MC: " __FILE__ ": %s()\n", __func__);
+        debugf0("MC: %s: %s()\n", __FILE__, __func__);
 
         /* don't need to call pci_enable_device() */
         rc = i82443bxgx_edacmc_probe1(pdev, ent->driver_data);
@@ -367,7 +367,7 @@ static void __devexit i82443bxgx_edacmc_remove_one(struct pci_dev *pdev)
 {
         struct mem_ctl_info *mci;
 
-        debugf0(__FILE__ ": %s()\n", __func__);
+        debugf0("%s: %s()\n", __FILE__, __func__);
 
         if (i82443bxgx_pci)
                 edac_pci_release_generic_ctl(i82443bxgx_pci);
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index 6c1886b497ff..52ca09bf4726 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -229,7 +229,7 @@ static int __devinit mpc85xx_pci_err_probe(struct of_device *op,
 
         pdata->edac_idx = edac_pci_idx++;
 
-        res = of_address_to_resource(op->node, 0, &r);
+        res = of_address_to_resource(op->dev.of_node, 0, &r);
         if (res) {
                 printk(KERN_ERR "%s: Unable to get resource for "
                        "PCI err regs\n", __func__);
@@ -274,7 +274,7 @@ static int __devinit mpc85xx_pci_err_probe(struct of_device *op,
         }
 
         if (edac_op_state == EDAC_OPSTATE_INT) {
-                pdata->irq = irq_of_parse_and_map(op->node, 0);
+                pdata->irq = irq_of_parse_and_map(op->dev.of_node, 0);
                 res = devm_request_irq(&op->dev, pdata->irq,
                                        mpc85xx_pci_isr, IRQF_DISABLED,
                                        "[EDAC] PCI err", pci);
@@ -529,7 +529,7 @@ static int __devinit mpc85xx_l2_err_probe(struct of_device *op,
         edac_dev->ctl_name = pdata->name;
         edac_dev->dev_name = pdata->name;
 
-        res = of_address_to_resource(op->node, 0, &r);
+        res = of_address_to_resource(op->dev.of_node, 0, &r);
         if (res) {
                 printk(KERN_ERR "%s: Unable to get resource for "
                        "L2 err regs\n", __func__);
@@ -576,7 +576,7 @@ static int __devinit mpc85xx_l2_err_probe(struct of_device *op,
         }
 
         if (edac_op_state == EDAC_OPSTATE_INT) {
-                pdata->irq = irq_of_parse_and_map(op->node, 0);
+                pdata->irq = irq_of_parse_and_map(op->dev.of_node, 0);
                 res = devm_request_irq(&op->dev, pdata->irq,
                                        mpc85xx_l2_isr, IRQF_DISABLED,
                                        "[EDAC] L2 err", edac_dev);
@@ -978,7 +978,7 @@ static int __devinit mpc85xx_mc_err_probe(struct of_device *op,
         mci->ctl_name = pdata->name;
         mci->dev_name = pdata->name;
 
-        res = of_address_to_resource(op->node, 0, &r);
+        res = of_address_to_resource(op->dev.of_node, 0, &r);
         if (res) {
                 printk(KERN_ERR "%s: Unable to get resource for MC err regs\n",
                        __func__);
@@ -1052,7 +1052,7 @@ static int __devinit mpc85xx_mc_err_probe(struct of_device *op,
                 out_be32(pdata->mc_vbase + MPC85XX_MC_ERR_SBE, 0x10000);
 
                 /* register interrupts */
-                pdata->irq = irq_of_parse_and_map(op->node, 0);
+                pdata->irq = irq_of_parse_and_map(op->dev.of_node, 0);
                 res = devm_request_irq(&op->dev, pdata->irq,
                                        mpc85xx_mc_isr,
                                         IRQF_DISABLED | IRQF_SHARED,
diff --git a/drivers/edac/ppc4xx_edac.c b/drivers/edac/ppc4xx_edac.c
index 9d6f6783328c..e78839e89a06 100644
--- a/drivers/edac/ppc4xx_edac.c
+++ b/drivers/edac/ppc4xx_edac.c
@@ -1022,7 +1022,7 @@ ppc4xx_edac_mc_init(struct mem_ctl_info *mci,
         int status = 0;
         const u32 memcheck = (mcopt1 & SDRAM_MCOPT1_MCHK_MASK);
         struct ppc4xx_edac_pdata *pdata = NULL;
-        const struct device_node *np = op->node;
+        const struct device_node *np = op->dev.of_node;
 
         if (match == NULL)
                 return -EINVAL;
@@ -1113,7 +1113,7 @@ ppc4xx_edac_register_irq(struct of_device *op, struct mem_ctl_info *mci)
         int status = 0;
         int ded_irq, sec_irq;
         struct ppc4xx_edac_pdata *pdata = mci->pvt_info;
-        struct device_node *np = op->node;
+        struct device_node *np = op->dev.of_node;
 
         ded_irq = irq_of_parse_and_map(np, INTMAP_ECCDED_INDEX);
         sec_irq = irq_of_parse_and_map(np, INTMAP_ECCSEC_INDEX);
@@ -1243,7 +1243,7 @@ ppc4xx_edac_probe(struct of_device *op, const struct of_device_id *match)
         int status = 0;
         u32 mcopt1, memcheck;
         dcr_host_t dcr_host;
-        const struct device_node *np = op->node;
+        const struct device_node *np = op->dev.of_node;
         struct mem_ctl_info *mci = NULL;
         static int ppc4xx_edac_instance;
 
diff --git a/drivers/firewire/core-card.c b/drivers/firewire/core-card.c
index 5045156c5313..371713ff0266 100644
--- a/drivers/firewire/core-card.c
+++ b/drivers/firewire/core-card.c
@@ -30,7 +30,6 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/spinlock.h>
-#include <linux/timer.h>
 #include <linux/workqueue.h>
 
 #include <asm/atomic.h>
@@ -63,7 +62,7 @@ static size_t config_rom_length = 1 + 4 + 1 + 1;
 #define BIB_CRC(v)              ((v) <<  0)
 #define BIB_CRC_LENGTH(v)       ((v) << 16)
 #define BIB_INFO_LENGTH(v)      ((v) << 24)
-
+#define BIB_BUS_NAME            0x31333934 /* "1394" */
 #define BIB_LINK_SPEED(v)       ((v) <<  0)
 #define BIB_GENERATION(v)       ((v) <<  4)
 #define BIB_MAX_ROM(v)          ((v) <<  8)
@@ -73,7 +72,8 @@ static size_t config_rom_length = 1 + 4 + 1 + 1;
 #define BIB_BMC                 ((1) << 28)
 #define BIB_ISC                 ((1) << 29)
 #define BIB_CMC                 ((1) << 30)
-#define BIB_IMC                 ((1) << 31)
+#define BIB_IRMC                ((1) << 31)
+#define NODE_CAPABILITIES       0x0c0083c0 /* per IEEE 1394 clause 8.3.2.6.5.2 */
 
 static void generate_config_rom(struct fw_card *card, __be32 *config_rom)
 {
@@ -91,18 +91,18 @@ static void generate_config_rom(struct fw_card *card, __be32 *config_rom)
 
         config_rom[0] = cpu_to_be32(
                 BIB_CRC_LENGTH(4) | BIB_INFO_LENGTH(4) | BIB_CRC(0));
-        config_rom[1] = cpu_to_be32(0x31333934);
+        config_rom[1] = cpu_to_be32(BIB_BUS_NAME);
         config_rom[2] = cpu_to_be32(
                 BIB_LINK_SPEED(card->link_speed) |
                 BIB_GENERATION(card->config_rom_generation++ % 14 + 2) |
                 BIB_MAX_ROM(2) |
                 BIB_MAX_RECEIVE(card->max_receive) |
-                BIB_BMC | BIB_ISC | BIB_CMC | BIB_IMC);
+                BIB_BMC | BIB_ISC | BIB_CMC | BIB_IRMC);
         config_rom[3] = cpu_to_be32(card->guid >> 32);
         config_rom[4] = cpu_to_be32(card->guid);
 
         /* Generate root directory. */
-        config_rom[6] = cpu_to_be32(0x0c0083c0); /* node capabilities */
+        config_rom[6] = cpu_to_be32(NODE_CAPABILITIES);
         i = 7;
         j = 7 + descriptor_count;
 
@@ -231,7 +231,7 @@ void fw_schedule_bm_work(struct fw_card *card, unsigned long delay)
 static void fw_card_bm_work(struct work_struct *work)
 {
         struct fw_card *card = container_of(work, struct fw_card, work.work);
-        struct fw_device *root_device;
+        struct fw_device *root_device, *irm_device;
         struct fw_node *root_node;
         unsigned long flags;
         int root_id, new_root_id, irm_id, local_id;
@@ -239,6 +239,7 @@ static void fw_card_bm_work(struct work_struct *work)
         bool do_reset = false;
         bool root_device_is_running;
         bool root_device_is_cmc;
+        bool irm_is_1394_1995_only;
 
         spin_lock_irqsave(&card->lock, flags);
 
@@ -248,12 +249,18 @@ static void fw_card_bm_work(struct work_struct *work)
         }
 
         generation = card->generation;
+
         root_node = card->root_node;
         fw_node_get(root_node);
         root_device = root_node->data;
         root_device_is_running = root_device &&
                         atomic_read(&root_device->state) == FW_DEVICE_RUNNING;
         root_device_is_cmc = root_device && root_device->cmc;
+
+        irm_device = card->irm_node->data;
+        irm_is_1394_1995_only = irm_device && irm_device->config_rom &&
+                        (irm_device->config_rom[2] & 0x000000f0) == 0;
+
         root_id  = root_node->node_id;
         irm_id   = card->irm_node->node_id;
         local_id = card->local_node->node_id;
@@ -276,8 +283,15 @@ static void fw_card_bm_work(struct work_struct *work)
 
                 if (!card->irm_node->link_on) {
                         new_root_id = local_id;
-                        fw_notify("IRM has link off, making local node (%02x) root.\n",
-                                  new_root_id);
+                        fw_notify("%s, making local node (%02x) root.\n",
+                                  "IRM has link off", new_root_id);
+                        goto pick_me;
+                }
+
+                if (irm_is_1394_1995_only) {
+                        new_root_id = local_id;
+                        fw_notify("%s, making local node (%02x) root.\n",
+                                  "IRM is not 1394a compliant", new_root_id);
                         goto pick_me;
                 }
 
@@ -316,8 +330,8 @@ static void fw_card_bm_work(struct work_struct *work)
                          * root, and thus, IRM.
                          */
                         new_root_id = local_id;
-                        fw_notify("BM lock failed, making local node (%02x) root.\n",
-                                  new_root_id);
+                        fw_notify("%s, making local node (%02x) root.\n",
+                                  "BM lock failed", new_root_id);
                         goto pick_me;
                 }
         } else if (card->bm_generation != generation) {
@@ -407,13 +421,6 @@ static void fw_card_bm_work(struct work_struct *work)
         fw_card_put(card);
 }
 
-static void flush_timer_callback(unsigned long data)
-{
-        struct fw_card *card = (struct fw_card *)data;
-
-        fw_flush_transactions(card);
-}
-
 void fw_card_initialize(struct fw_card *card,
                         const struct fw_card_driver *driver,
                         struct device *device)
@@ -432,8 +439,6 @@ void fw_card_initialize(struct fw_card *card,
         init_completion(&card->done);
         INIT_LIST_HEAD(&card->transaction_list);
         spin_lock_init(&card->lock);
-        setup_timer(&card->flush_timer,
-                    flush_timer_callback, (unsigned long)card);
 
         card->local_node = NULL;
 
@@ -558,7 +563,6 @@ void fw_core_remove_card(struct fw_card *card)
         wait_for_completion(&card->done);
 
         WARN_ON(!list_empty(&card->transaction_list));
-        del_timer_sync(&card->flush_timer);
 }
 EXPORT_SYMBOL(fw_core_remove_card);
 
diff --git a/drivers/firewire/core-cdev.c b/drivers/firewire/core-cdev.c
index 14a34d99eea2..5bf106b9d791 100644
--- a/drivers/firewire/core-cdev.c
+++ b/drivers/firewire/core-cdev.c
@@ -227,7 +227,7 @@ static int fw_device_op_open(struct inode *inode, struct file *file)
         list_add_tail(&client->link, &device->client_list);
         mutex_unlock(&device->client_list_mutex);
 
-        return 0;
+        return nonseekable_open(inode, file);
 }
 
 static void queue_event(struct client *client, struct event *event,
@@ -1496,13 +1496,13 @@ static unsigned int fw_device_op_poll(struct file *file, poll_table * pt)
 
 const struct file_operations fw_device_ops = {
         .owner          = THIS_MODULE,
+        .llseek         = no_llseek,
         .open           = fw_device_op_open,
         .read           = fw_device_op_read,
         .unlocked_ioctl = fw_device_op_ioctl,
-        .poll           = fw_device_op_poll,
-        .release        = fw_device_op_release,
         .mmap           = fw_device_op_mmap,
-
+        .release        = fw_device_op_release,
+        .poll           = fw_device_op_poll,
 #ifdef CONFIG_COMPAT
         .compat_ioctl   = fw_device_op_compat_ioctl,
 #endif
diff --git a/drivers/firewire/core-transaction.c b/drivers/firewire/core-transaction.c
index 673b03f8b4ec..fdc33ff06dc1 100644
--- a/drivers/firewire/core-transaction.c
+++ b/drivers/firewire/core-transaction.c
@@ -81,7 +81,7 @@ static int close_transaction(struct fw_transaction *transaction,
         spin_lock_irqsave(&card->lock, flags);
         list_for_each_entry(t, &card->transaction_list, link) {
                 if (t == transaction) {
-                        list_del(&t->link);
+                        list_del_init(&t->link);
                         card->tlabel_mask &= ~(1ULL << t->tlabel);
                         break;
                 }
@@ -89,6 +89,7 @@ static int close_transaction(struct fw_transaction *transaction,
         spin_unlock_irqrestore(&card->lock, flags);
 
         if (&t->link != &card->transaction_list) {
+                del_timer_sync(&t->split_timeout_timer);
                 t->callback(card, rcode, NULL, 0, t->callback_data);
                 return 0;
         }
@@ -121,6 +122,31 @@ int fw_cancel_transaction(struct fw_card *card,
 }
 EXPORT_SYMBOL(fw_cancel_transaction);
 
+static void split_transaction_timeout_callback(unsigned long data)
+{
+        struct fw_transaction *t = (struct fw_transaction *)data;
+        struct fw_card *card = t->card;
+        unsigned long flags;
+
+        spin_lock_irqsave(&card->lock, flags);
+        if (list_empty(&t->link)) {
+                spin_unlock_irqrestore(&card->lock, flags);
+                return;
+        }
+        list_del(&t->link);
+        card->tlabel_mask &= ~(1ULL << t->tlabel);
+        spin_unlock_irqrestore(&card->lock, flags);
+
+        card->driver->cancel_packet(card, &t->packet);
+
+        /*
+         * At this point cancel_packet will never call the transaction
+         * callback, since we just took the transaction out of the list.
+         * So do it here.
+         */
+        t->callback(card, RCODE_CANCELLED, NULL, 0, t->callback_data);
+}
+
 static void transmit_complete_callback(struct fw_packet *packet,
                                        struct fw_card *card, int status)
 {
@@ -229,6 +255,23 @@ static void fw_fill_request(struct fw_packet *packet, int tcode, int tlabel,
         packet->payload_mapped = false;
 }
 
+static int allocate_tlabel(struct fw_card *card)
+{
+        int tlabel;
+
+        tlabel = card->current_tlabel;
+        while (card->tlabel_mask & (1ULL << tlabel)) {
+                tlabel = (tlabel + 1) & 0x3f;
+                if (tlabel == card->current_tlabel)
+                        return -EBUSY;
+        }
+
+        card->current_tlabel = (tlabel + 1) & 0x3f;
+        card->tlabel_mask |= 1ULL << tlabel;
+
+        return tlabel;
+}
+
 /**
  * This function provides low-level access to the IEEE1394 transaction
  * logic.  Most C programs would use either fw_read(), fw_write() or
@@ -277,31 +320,26 @@ void fw_send_request(struct fw_card *card, struct fw_transaction *t, int tcode,
         int tlabel;
 
         /*
-         * Bump the flush timer up 100ms first of all so we
-         * don't race with a flush timer callback.
-         */
-
-        mod_timer(&card->flush_timer, jiffies + DIV_ROUND_UP(HZ, 10));
-
-        /*
          * Allocate tlabel from the bitmap and put the transaction on
          * the list while holding the card spinlock.
          */
 
         spin_lock_irqsave(&card->lock, flags);
 
-        tlabel = card->current_tlabel;
-        if (card->tlabel_mask & (1ULL << tlabel)) {
+        tlabel = allocate_tlabel(card);
+        if (tlabel < 0) {
                 spin_unlock_irqrestore(&card->lock, flags);
                 callback(card, RCODE_SEND_ERROR, NULL, 0, callback_data);
                 return;
         }
 
-        card->current_tlabel = (card->current_tlabel + 1) & 0x3f;
-        card->tlabel_mask |= (1ULL << tlabel);
-
         t->node_id = destination_id;
         t->tlabel = tlabel;
+        t->card = card;
+        setup_timer(&t->split_timeout_timer,
+                    split_transaction_timeout_callback, (unsigned long)t);
+        /* FIXME: start this timer later, relative to t->timestamp */
+        mod_timer(&t->split_timeout_timer, jiffies + DIV_ROUND_UP(HZ, 10));
         t->callback = callback;
         t->callback_data = callback_data;
 
@@ -347,11 +385,13 @@ int fw_run_transaction(struct fw_card *card, int tcode, int destination_id,
         struct transaction_callback_data d;
         struct fw_transaction t;
 
+        init_timer_on_stack(&t.split_timeout_timer);
         init_completion(&d.done);
         d.payload = payload;
         fw_send_request(card, &t, tcode, destination_id, generation, speed,
                         offset, payload, length, transaction_callback, &d);
         wait_for_completion(&d.done);
+        destroy_timer_on_stack(&t.split_timeout_timer);
 
         return d.rcode;
 }
@@ -394,30 +434,6 @@ void fw_send_phy_config(struct fw_card *card,
         mutex_unlock(&phy_config_mutex);
 }
 
-void fw_flush_transactions(struct fw_card *card)
-{
-        struct fw_transaction *t, *next;
-        struct list_head list;
-        unsigned long flags;
-
-        INIT_LIST_HEAD(&list);
-        spin_lock_irqsave(&card->lock, flags);
-        list_splice_init(&card->transaction_list, &list);
-        card->tlabel_mask = 0;
-        spin_unlock_irqrestore(&card->lock, flags);
-
-        list_for_each_entry_safe(t, next, &list, link) {
-                card->driver->cancel_packet(card, &t->packet);
-
-                /*
-                 * At this point cancel_packet will never call the
-                 * transaction callback, since we just took all the
-                 * transactions out of the list.  So do it here.
-                 */
-                t->callback(card, RCODE_CANCELLED, NULL, 0, t->callback_data);
-        }
-}
-
 static struct fw_address_handler *lookup_overlapping_address_handler(
         struct list_head *list, unsigned long long offset, size_t length)
 {
@@ -827,8 +843,8 @@ void fw_core_handle_response(struct fw_card *card, struct fw_packet *p)
         spin_lock_irqsave(&card->lock, flags);
         list_for_each_entry(t, &card->transaction_list, link) {
                 if (t->node_id == source && t->tlabel == tlabel) {
-                        list_del(&t->link);
-                        card->tlabel_mask &= ~(1 << t->tlabel);
+                        list_del_init(&t->link);
+                        card->tlabel_mask &= ~(1ULL << t->tlabel);
                         break;
                 }
         }
@@ -869,6 +885,8 @@ void fw_core_handle_response(struct fw_card *card, struct fw_packet *p)
                 break;
         }
 
+        del_timer_sync(&t->split_timeout_timer);
+
         /*
          * The response handler may be executed while the request handler
          * is still pending.  Cancel the request handler.
diff --git a/drivers/firewire/core.h b/drivers/firewire/core.h
index fb0321300cce..0ecfcd95f4c5 100644
--- a/drivers/firewire/core.h
+++ b/drivers/firewire/core.h
@@ -27,7 +27,12 @@ struct fw_packet;
 #define PHY_LINK_ACTIVE         0x80
 #define PHY_CONTENDER           0x40
 #define PHY_BUS_RESET           0x40
+#define PHY_EXTENDED_REGISTERS  0xe0
 #define PHY_BUS_SHORT_RESET     0x40
+#define PHY_INT_STATUS_BITS     0x3c
+#define PHY_ENABLE_ACCEL        0x02
+#define PHY_ENABLE_MULTI        0x01
+#define PHY_PAGE_SELECT         0xe0
 
 #define BANDWIDTH_AVAILABLE_INITIAL     4915
 #define BROADCAST_CHANNEL_INITIAL       (1 << 31 | 31)
@@ -215,7 +220,6 @@ void fw_core_handle_request(struct fw_card *card, struct fw_packet *request);
 void fw_core_handle_response(struct fw_card *card, struct fw_packet *packet);
 void fw_fill_response(struct fw_packet *response, u32 *request_header,
                       int rcode, void *payload, size_t length);
-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);
 
diff --git a/drivers/firewire/ohci.c b/drivers/firewire/ohci.c
index a3b083a7403a..9f627e758cfc 100644
--- a/drivers/firewire/ohci.c
+++ b/drivers/firewire/ohci.c
@@ -236,13 +236,15 @@ static char ohci_driver_name[] = KBUILD_MODNAME;
 #define QUIRK_CYCLE_TIMER               1
 #define QUIRK_RESET_PACKET              2
 #define QUIRK_BE_HEADERS                4
+#define QUIRK_NO_1394A                  8
 
 /* In case of multiple matches in ohci_quirks[], only the first one is used. */
 static const struct {
         unsigned short vendor, device, flags;
 } ohci_quirks[] = {
         {PCI_VENDOR_ID_TI,      PCI_DEVICE_ID_TI_TSB12LV22, QUIRK_CYCLE_TIMER |
-                                                            QUIRK_RESET_PACKET},
+                                                            QUIRK_RESET_PACKET |
+                                                            QUIRK_NO_1394A},
         {PCI_VENDOR_ID_TI,      PCI_ANY_ID,     QUIRK_RESET_PACKET},
         {PCI_VENDOR_ID_AL,      PCI_ANY_ID,     QUIRK_CYCLE_TIMER},
         {PCI_VENDOR_ID_NEC,     PCI_ANY_ID,     QUIRK_CYCLE_TIMER},
@@ -257,15 +259,16 @@ MODULE_PARM_DESC(quirks, "Chip quirks (default = 0"
         ", nonatomic cycle timer = "    __stringify(QUIRK_CYCLE_TIMER)
         ", reset packet generation = "  __stringify(QUIRK_RESET_PACKET)
         ", AR/selfID endianess = "      __stringify(QUIRK_BE_HEADERS)
+        ", no 1394a enhancements = "    __stringify(QUIRK_NO_1394A)
         ")");
 
-#ifdef CONFIG_FIREWIRE_OHCI_DEBUG
-
 #define OHCI_PARAM_DEBUG_AT_AR          1
 #define OHCI_PARAM_DEBUG_SELFIDS        2
 #define OHCI_PARAM_DEBUG_IRQS           4
 #define OHCI_PARAM_DEBUG_BUSRESETS      8 /* only effective before chip init */
 
+#ifdef CONFIG_FIREWIRE_OHCI_DEBUG
+
 static int param_debug;
 module_param_named(debug, param_debug, int, 0644);
 MODULE_PARM_DESC(debug, "Verbose logging (default = 0"
@@ -438,9 +441,10 @@ static void log_ar_at_event(char dir, int speed, u32 *header, int evt)
 
 #else
 
-#define log_irqs(evt)
-#define log_selfids(node_id, generation, self_id_count, sid)
-#define log_ar_at_event(dir, speed, header, evt)
+#define param_debug 0
+static inline void log_irqs(u32 evt) {}
+static inline void log_selfids(int node_id, int generation, int self_id_count, u32 *s) {}
+static inline void log_ar_at_event(char dir, int speed, u32 *header, int evt) {}
 
 #endif /* CONFIG_FIREWIRE_OHCI_DEBUG */
 
@@ -460,27 +464,71 @@ static inline void flush_writes(const struct fw_ohci *ohci)
         reg_read(ohci, OHCI1394_Version);
 }
 
-static int ohci_update_phy_reg(struct fw_card *card, int addr,
-                               int clear_bits, int set_bits)
+static int read_phy_reg(struct fw_ohci *ohci, int addr)
 {
-        struct fw_ohci *ohci = fw_ohci(card);
-        u32 val, old;
+        u32 val;
+        int i;
 
         reg_write(ohci, OHCI1394_PhyControl, OHCI1394_PhyControl_Read(addr));
-        flush_writes(ohci);
-        msleep(2);
-        val = reg_read(ohci, OHCI1394_PhyControl);
-        if ((val & OHCI1394_PhyControl_ReadDone) == 0) {
-                fw_error("failed to set phy reg bits.\n");
-                return -EBUSY;
+        for (i = 0; i < 10; i++) {
+                val = reg_read(ohci, OHCI1394_PhyControl);
+                if (val & OHCI1394_PhyControl_ReadDone)
+                        return OHCI1394_PhyControl_ReadData(val);
+
+                msleep(1);
         }
+        fw_error("failed to read phy reg\n");
+
+        return -EBUSY;
+}
+
+static int write_phy_reg(const struct fw_ohci *ohci, int addr, u32 val)
+{
+        int i;
 
-        old = OHCI1394_PhyControl_ReadData(val);
-        old = (old & ~clear_bits) | set_bits;
         reg_write(ohci, OHCI1394_PhyControl,
-                  OHCI1394_PhyControl_Write(addr, old));
+                  OHCI1394_PhyControl_Write(addr, val));
+        for (i = 0; i < 100; i++) {
+                val = reg_read(ohci, OHCI1394_PhyControl);
+                if (!(val & OHCI1394_PhyControl_WritePending))
+                        return 0;
 
-        return 0;
+                msleep(1);
+        }
+        fw_error("failed to write phy reg\n");
+
+        return -EBUSY;
+}
+
+static int ohci_update_phy_reg(struct fw_card *card, int addr,
+                               int clear_bits, int set_bits)
+{
+        struct fw_ohci *ohci = fw_ohci(card);
+        int ret;
+
+        ret = read_phy_reg(ohci, addr);
+        if (ret < 0)
+                return ret;
+
+        /*
+         * The interrupt status bits are cleared by writing a one bit.
+         * Avoid clearing them unless explicitly requested in set_bits.
+         */
+        if (addr == 5)
+                clear_bits |= PHY_INT_STATUS_BITS;
+
+        return write_phy_reg(ohci, addr, (ret & ~clear_bits) | set_bits);
+}
+
+static int read_paged_phy_reg(struct fw_ohci *ohci, int page, int addr)
+{
+        int ret;
+
+        ret = ohci_update_phy_reg(&ohci->card, 7, PHY_PAGE_SELECT, page << 5);
+        if (ret < 0)
+                return ret;
+
+        return read_phy_reg(ohci, addr);
 }
 
 static int ar_context_add_page(struct ar_context *ctx)
@@ -1495,13 +1543,64 @@ static void copy_config_rom(__be32 *dest, const __be32 *src, size_t length)
                 memset(&dest[length], 0, CONFIG_ROM_SIZE - size);
 }
 
+static int configure_1394a_enhancements(struct fw_ohci *ohci)
+{
+        bool enable_1394a;
+        int ret, clear, set, offset;
+
+        /* Check if the driver should configure link and PHY. */
+        if (!(reg_read(ohci, OHCI1394_HCControlSet) &
+              OHCI1394_HCControl_programPhyEnable))
+                return 0;
+
+        /* Paranoia: check whether the PHY supports 1394a, too. */
+        enable_1394a = false;
+        ret = read_phy_reg(ohci, 2);
+        if (ret < 0)
+                return ret;
+        if ((ret & PHY_EXTENDED_REGISTERS) == PHY_EXTENDED_REGISTERS) {
+                ret = read_paged_phy_reg(ohci, 1, 8);
+                if (ret < 0)
+                        return ret;
+                if (ret >= 1)
+                        enable_1394a = true;
+        }
+
+        if (ohci->quirks & QUIRK_NO_1394A)
+                enable_1394a = false;
+
+        /* Configure PHY and link consistently. */
+        if (enable_1394a) {
+                clear = 0;
+                set = PHY_ENABLE_ACCEL | PHY_ENABLE_MULTI;
+        } else {
+                clear = PHY_ENABLE_ACCEL | PHY_ENABLE_MULTI;
+                set = 0;
+        }
+        ret = ohci_update_phy_reg(&ohci->card, 5, clear, set);
+        if (ret < 0)
+                return ret;
+
+        if (enable_1394a)
+                offset = OHCI1394_HCControlSet;
+        else
+                offset = OHCI1394_HCControlClear;
+        reg_write(ohci, offset, OHCI1394_HCControl_aPhyEnhanceEnable);
+
+        /* Clean up: configuration has been taken care of. */
+        reg_write(ohci, OHCI1394_HCControlClear,
+                  OHCI1394_HCControl_programPhyEnable);
+
+        return 0;
+}
+
 static int ohci_enable(struct fw_card *card,
                        const __be32 *config_rom, size_t length)
 {
         struct fw_ohci *ohci = fw_ohci(card);
         struct pci_dev *dev = to_pci_dev(card->device);
         u32 lps;
-        int i;
+        int i, ret;
 
         if (software_reset(ohci)) {
                 fw_error("Failed to reset ohci card.\n");
@@ -1565,10 +1664,14 @@ static int ohci_enable(struct fw_card *card,
         if (param_debug & OHCI_PARAM_DEBUG_BUSRESETS)
                 reg_write(ohci, OHCI1394_IntMaskSet, OHCI1394_busReset);
 
+        ret = configure_1394a_enhancements(ohci);
+        if (ret < 0)
+                return ret;
+
         /* Activate link_on bit and contender bit in our self ID packets.*/
-        if (ohci_update_phy_reg(card, 4, 0,
-                                PHY_LINK_ACTIVE | PHY_CONTENDER) < 0)
-                return -EIO;
+        ret = ohci_update_phy_reg(card, 4, 0, PHY_LINK_ACTIVE | PHY_CONTENDER);
+        if (ret < 0)
+                return ret;
 
         /*
          * When the link is not yet enabled, the atomic config rom
@@ -2304,7 +2407,7 @@ static const struct fw_card_driver ohci_driver = {
 };
 
 #ifdef CONFIG_PPC_PMAC
-static void ohci_pmac_on(struct pci_dev *dev)
+static void pmac_ohci_on(struct pci_dev *dev)
 {
         if (machine_is(powermac)) {
                 struct device_node *ofn = pci_device_to_OF_node(dev);
@@ -2316,7 +2419,7 @@ static void ohci_pmac_on(struct pci_dev *dev)
         }
 }
 
-static void ohci_pmac_off(struct pci_dev *dev)
+static void pmac_ohci_off(struct pci_dev *dev)
 {
         if (machine_is(powermac)) {
                 struct device_node *ofn = pci_device_to_OF_node(dev);
@@ -2328,15 +2431,15 @@ static void ohci_pmac_off(struct pci_dev *dev)
         }
 }
 #else
-#define ohci_pmac_on(dev)
-#define ohci_pmac_off(dev)
+static inline void pmac_ohci_on(struct pci_dev *dev) {}
+static inline void pmac_ohci_off(struct pci_dev *dev) {}
 #endif /* CONFIG_PPC_PMAC */
 
 static int __devinit pci_probe(struct pci_dev *dev,
                                const struct pci_device_id *ent)
 {
         struct fw_ohci *ohci;
-        u32 bus_options, max_receive, link_speed, version;
+        u32 bus_options, max_receive, link_speed, version, link_enh;
         u64 guid;
         int i, err, n_ir, n_it;
         size_t size;
@@ -2349,7 +2452,7 @@ static int __devinit pci_probe(struct pci_dev *dev,
 
         fw_card_initialize(&ohci->card, &ohci_driver, &dev->dev);
 
-        ohci_pmac_on(dev);
+        pmac_ohci_on(dev);
 
         err = pci_enable_device(dev);
         if (err) {
@@ -2389,6 +2492,23 @@ static int __devinit pci_probe(struct pci_dev *dev,
         if (param_quirks)
                 ohci->quirks = param_quirks;
 
+        /* TI OHCI-Lynx and compatible: set recommended configuration bits. */
+        if (dev->vendor == PCI_VENDOR_ID_TI) {
+                pci_read_config_dword(dev, PCI_CFG_TI_LinkEnh, &link_enh);
+
+                /* adjust latency of ATx FIFO: use 1.7 KB threshold */
+                link_enh &= ~TI_LinkEnh_atx_thresh_mask;
+                link_enh |= TI_LinkEnh_atx_thresh_1_7K;
+
+                /* use priority arbitration for asynchronous responses */
+                link_enh |= TI_LinkEnh_enab_unfair;
+
+                /* required for aPhyEnhanceEnable to work */
+                link_enh |= TI_LinkEnh_enab_accel;
+
+                pci_write_config_dword(dev, PCI_CFG_TI_LinkEnh, link_enh);
+        }
+
         ar_context_init(&ohci->ar_request_ctx, ohci,
                         OHCI1394_AsReqRcvContextControlSet);
 
@@ -2466,7 +2586,7 @@ static int __devinit pci_probe(struct pci_dev *dev,
         pci_disable_device(dev);
  fail_free:
         kfree(&ohci->card);
-        ohci_pmac_off(dev);
+        pmac_ohci_off(dev);
  fail:
         if (err == -ENOMEM)
                 fw_error("Out of memory\n");
@@ -2509,7 +2629,7 @@ static void pci_remove(struct pci_dev *dev)
         pci_release_region(dev, 0);
         pci_disable_device(dev);
         kfree(&ohci->card);
-        ohci_pmac_off(dev);
+        pmac_ohci_off(dev);
 
         fw_notify("Removed fw-ohci device.\n");
 }
@@ -2530,7 +2650,7 @@ static int pci_suspend(struct pci_dev *dev, pm_message_t state)
         err = pci_set_power_state(dev, pci_choose_state(dev, state));
         if (err)
                 fw_error("pci_set_power_state failed with %d\n", err);
-        ohci_pmac_off(dev);
+        pmac_ohci_off(dev);
 
         return 0;
 }
@@ -2540,7 +2660,7 @@ static int pci_resume(struct pci_dev *dev)
         struct fw_ohci *ohci = pci_get_drvdata(dev);
         int err;
 
-        ohci_pmac_on(dev);
+        pmac_ohci_on(dev);
         pci_set_power_state(dev, PCI_D0);
         pci_restore_state(dev);
         err = pci_enable_device(dev);
diff --git a/drivers/firewire/ohci.h b/drivers/firewire/ohci.h
index ba492d85c516..3bc9a5d744eb 100644
--- a/drivers/firewire/ohci.h
+++ b/drivers/firewire/ohci.h
@@ -67,7 +67,7 @@
 #define   OHCI1394_PhyControl_ReadDone          0x80000000
 #define   OHCI1394_PhyControl_ReadData(r)       (((r) & 0x00ff0000) >> 16)
 #define   OHCI1394_PhyControl_Write(addr, data) (((addr) << 8) | (data) | 0x00004000)
-#define   OHCI1394_PhyControl_WriteDone         0x00004000
+#define   OHCI1394_PhyControl_WritePending      0x00004000
 #define OHCI1394_IsochronousCycleTimer        0x0F0
 #define OHCI1394_AsReqFilterHiSet             0x100
 #define OHCI1394_AsReqFilterHiClear           0x104
@@ -154,4 +154,12 @@
 
 #define OHCI1394_phy_tcode              0xe
 
+/* TI extensions */
+
+#define PCI_CFG_TI_LinkEnh              0xf4
+#define  TI_LinkEnh_enab_accel          0x00000002
+#define  TI_LinkEnh_enab_unfair         0x00000080
+#define  TI_LinkEnh_atx_thresh_mask     0x00003000
+#define  TI_LinkEnh_atx_thresh_1_7K     0x00001000
+
 #endif /* _FIREWIRE_OHCI_H */
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index fee678f74a19..724038dab4ca 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -139,6 +139,13 @@ config GPIO_MAX732X
           Board setup code must specify the model to use, and the start
           number for these GPIOs.
 
+config GPIO_MAX732X_IRQ
+        bool "Interrupt controller support for MAX732x"
+        depends on GPIO_MAX732X=y && GENERIC_HARDIRQS
+        help
+          Say yes here to enable the max732x to be used as an interrupt
+          controller. It requires the driver to be built in the kernel.
+
 config GPIO_PCA953X
         tristate "PCA953x, PCA955x, TCA64xx, and MAX7310 I/O ports"
         depends on I2C
@@ -188,6 +195,13 @@ config GPIO_PCF857X
           This driver provides an in-kernel interface to those GPIOs using
           platform-neutral GPIO calls.
 
+config GPIO_TC35892
+        bool "TC35892 GPIOs"
+        depends on MFD_TC35892
+        help
+          This enables support for the GPIOs found on the TC35892
+          I/O Expander.
+
 config GPIO_TWL4030
         tristate "TWL4030, TWL5030, and TPS659x0 GPIOs"
         depends on TWL4030_CORE
@@ -264,10 +278,10 @@ config GPIO_BT8XX
           If unsure, say N.
 
 config GPIO_LANGWELL
-        bool "Intel Moorestown Platform Langwell GPIO support"
+        bool "Intel Langwell/Penwell GPIO support"
         depends on PCI
         help
-          Say Y here to support Intel Moorestown platform GPIO.
+          Say Y here to support Intel Langwell/Penwell GPIO.
 
 config GPIO_TIMBERDALE
         bool "Support for timberdale GPIO IP"
@@ -275,6 +289,15 @@ config GPIO_TIMBERDALE
         ---help---
         Add support for the GPIO IP in the timberdale FPGA.
 
+config GPIO_RDC321X
+        tristate "RDC R-321x GPIO support"
+        depends on PCI && GPIOLIB
+        select MFD_CORE
+        select MFD_RDC321X
+        help
+          Support for the RDC R321x SoC GPIOs over southbridge
+          PCI configuration space.
+
 comment "SPI GPIO expanders:"
 
 config GPIO_MAX7301
@@ -310,4 +333,14 @@ config GPIO_UCB1400
           To compile this driver as a module, choose M here: the
           module will be called ucb1400_gpio.
 
+comment "MODULbus GPIO expanders:"
+
+config GPIO_JANZ_TTL
+        tristate "Janz VMOD-TTL Digital IO Module"
+        depends on MFD_JANZ_CMODIO
+        help
+          This enables support for the Janz VMOD-TTL Digital IO module.
+          This driver provides support for driving the pins in output
+          mode only. Input mode is not supported.
+
 endif
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 10f3f8d958b1..51c3cdd41b5a 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -16,6 +16,7 @@ obj-$(CONFIG_GPIO_MCP23S08)	+= mcp23s08.o
 obj-$(CONFIG_GPIO_PCA953X)      += pca953x.o
 obj-$(CONFIG_GPIO_PCF857X)      += pcf857x.o
 obj-$(CONFIG_GPIO_PL061)        += pl061.o
+obj-$(CONFIG_GPIO_TC35892)      += tc35892-gpio.o
 obj-$(CONFIG_GPIO_TIMBERDALE)   += timbgpio.o
 obj-$(CONFIG_GPIO_TWL4030)      += twl4030-gpio.o
 obj-$(CONFIG_GPIO_UCB1400)      += ucb1400_gpio.o
@@ -27,4 +28,6 @@ obj-$(CONFIG_GPIO_VR41XX)	+= vr41xx_giu.o
 obj-$(CONFIG_GPIO_WM831X)       += wm831x-gpio.o
 obj-$(CONFIG_GPIO_WM8350)       += wm8350-gpiolib.o
 obj-$(CONFIG_GPIO_WM8994)       += wm8994-gpio.o
-obj-$(CONFIG_GPIO_SCH)          += sch_gpio.o
\ No newline at end of file
+obj-$(CONFIG_GPIO_SCH)          += sch_gpio.o
+obj-$(CONFIG_GPIO_RDC321X)      += rdc321x-gpio.o
+obj-$(CONFIG_GPIO_JANZ_TTL)     += janz-ttl.o
diff --git a/drivers/gpio/cs5535-gpio.c b/drivers/gpio/cs5535-gpio.c
index 0c3c498f2260..f73a1555e49d 100644
--- a/drivers/gpio/cs5535-gpio.c
+++ b/drivers/gpio/cs5535-gpio.c
@@ -197,7 +197,7 @@ static int chip_direction_output(struct gpio_chip *c, unsigned offset, int val)
         return 0;
 }
 
-static char *cs5535_gpio_names[] = {
+static const char * const cs5535_gpio_names[] = {
         "GPIO0", "GPIO1", "GPIO2", "GPIO3",
         "GPIO4", "GPIO5", "GPIO6", "GPIO7",
         "GPIO8", "GPIO9", "GPIO10", "GPIO11",
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index cae1b8c5b08c..3ca36542e338 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -722,7 +722,7 @@ int gpio_export(unsigned gpio, bool direction_may_change)
         unsigned long           flags;
         struct gpio_desc        *desc;
         int                     status = -EINVAL;
-        char                    *ioname = NULL;
+        const char              *ioname = NULL;
 
         /* can't export until sysfs is available ... */
         if (!gpio_class.p) {
@@ -753,7 +753,7 @@ int gpio_export(unsigned gpio, bool direction_may_change)
                 struct device   *dev;
 
                 dev = device_create(&gpio_class, desc->chip->dev, MKDEV(0, 0),
-                                desc, ioname ? ioname : "gpio%d", gpio);
+                                desc, ioname ? ioname : "gpio%u", gpio);
                 if (!IS_ERR(dev)) {
                         status = sysfs_create_group(&dev->kobj,
                                                 &gpio_attr_group);
@@ -1106,7 +1106,7 @@ unlock:
 fail:
         /* failures here can mean systems won't boot... */
         if (status)
-                pr_err("gpiochip_add: gpios %d..%d (%s) not registered\n",
+                pr_err("gpiochip_add: gpios %d..%d (%s) failed to register\n",
                         chip->base, chip->base + chip->ngpio - 1,
                         chip->label ? : "generic");
         return status;
@@ -1447,6 +1447,49 @@ fail:
 }
 EXPORT_SYMBOL_GPL(gpio_direction_output);
 
+/**
+ * gpio_set_debounce - sets @debounce time for a @gpio
+ * @gpio: the gpio to set debounce time
+ * @debounce: debounce time is microseconds
+ */
+int gpio_set_debounce(unsigned gpio, unsigned debounce)
+{
+        unsigned long           flags;
+        struct gpio_chip        *chip;
+        struct gpio_desc        *desc = &gpio_desc[gpio];
+        int                     status = -EINVAL;
+
+        spin_lock_irqsave(&gpio_lock, flags);
+
+        if (!gpio_is_valid(gpio))
+                goto fail;
+        chip = desc->chip;
+        if (!chip || !chip->set || !chip->set_debounce)
+                goto fail;
+        gpio -= chip->base;
+        if (gpio >= chip->ngpio)
+                goto fail;
+        status = gpio_ensure_requested(desc, gpio);
+        if (status < 0)
+                goto fail;
+
+        /* now we know the gpio is valid and chip won't vanish */
+
+        spin_unlock_irqrestore(&gpio_lock, flags);
+
+        might_sleep_if(extra_checks && chip->can_sleep);
+
+        return chip->set_debounce(chip, gpio, debounce);
+
+fail:
+        spin_unlock_irqrestore(&gpio_lock, flags);
+        if (status)
+                pr_debug("%s: gpio-%d status %d\n",
+                        __func__, gpio, status);
+
+        return status;
+}
+EXPORT_SYMBOL_GPL(gpio_set_debounce);
 
 /* I/O calls are only valid after configuration completed; the relevant
  * "is this a valid GPIO" error checks should already have been done.
diff --git a/drivers/gpio/it8761e_gpio.c b/drivers/gpio/it8761e_gpio.c
index 41a9388f2fde..48fc43c4bdd1 100644
--- a/drivers/gpio/it8761e_gpio.c
+++ b/drivers/gpio/it8761e_gpio.c
@@ -217,7 +217,10 @@ gpiochip_add_err:
 static void __exit it8761e_gpio_exit(void)
 {
         if (gpio_ba) {
-                gpiochip_remove(&it8761e_gpio_chip);
+                int ret = gpiochip_remove(&it8761e_gpio_chip);
+
+                WARN(ret, "%s(): gpiochip_remove() failed, ret=%d\n",
+                                __func__, ret);
 
                 release_region(gpio_ba, GPIO_IOSIZE);
                 gpio_ba = 0;
diff --git a/drivers/gpio/janz-ttl.c b/drivers/gpio/janz-ttl.c
new file mode 100644
index 000000000000..813ac077e5d7
--- /dev/null
+++ b/drivers/gpio/janz-ttl.c
@@ -0,0 +1,258 @@
+/*
+ * Janz MODULbus VMOD-TTL GPIO Driver
+ *
+ * Copyright (c) 2010 Ira W. Snyder <iws@ovro.caltech.edu>
+ *
+ * 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/init.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+
+#include <linux/mfd/janz.h>
+
+#define DRV_NAME "janz-ttl"
+
+#define PORTA_DIRECTION         0x23
+#define PORTB_DIRECTION         0x2B
+#define PORTC_DIRECTION         0x06
+#define PORTA_IOCTL             0x24
+#define PORTB_IOCTL             0x2C
+#define PORTC_IOCTL             0x07
+
+#define MASTER_INT_CTL          0x00
+#define MASTER_CONF_CTL         0x01
+
+#define CONF_PAE                (1 << 2)
+#define CONF_PBE                (1 << 7)
+#define CONF_PCE                (1 << 4)
+
+struct ttl_control_regs {
+        __be16 portc;
+        __be16 portb;
+        __be16 porta;
+        __be16 control;
+};
+
+struct ttl_module {
+        struct gpio_chip gpio;
+
+        /* base address of registers */
+        struct ttl_control_regs __iomem *regs;
+
+        u8 portc_shadow;
+        u8 portb_shadow;
+        u8 porta_shadow;
+
+        spinlock_t lock;
+};
+
+static int ttl_get_value(struct gpio_chip *gpio, unsigned offset)
+{
+        struct ttl_module *mod = dev_get_drvdata(gpio->dev);
+        u8 *shadow;
+        int ret;
+
+        if (offset < 8) {
+                shadow = &mod->porta_shadow;
+        } else if (offset < 16) {
+                shadow = &mod->portb_shadow;
+                offset -= 8;
+        } else {
+                shadow = &mod->portc_shadow;
+                offset -= 16;
+        }
+
+        spin_lock(&mod->lock);
+        ret = *shadow & (1 << offset);
+        spin_unlock(&mod->lock);
+        return ret;
+}
+
+static void ttl_set_value(struct gpio_chip *gpio, unsigned offset, int value)
+{
+        struct ttl_module *mod = dev_get_drvdata(gpio->dev);
+        void __iomem *port;
+        u8 *shadow;
+
+        if (offset < 8) {
+                port = &mod->regs->porta;
+                shadow = &mod->porta_shadow;
+        } else if (offset < 16) {
+                port = &mod->regs->portb;
+                shadow = &mod->portb_shadow;
+                offset -= 8;
+        } else {
+                port = &mod->regs->portc;
+                shadow = &mod->portc_shadow;
+                offset -= 16;
+        }
+
+        spin_lock(&mod->lock);
+        if (value)
+                *shadow |= (1 << offset);
+        else
+                *shadow &= ~(1 << offset);
+
+        iowrite16be(*shadow, port);
+        spin_unlock(&mod->lock);
+}
+
+static void __devinit ttl_write_reg(struct ttl_module *mod, u8 reg, u16 val)
+{
+        iowrite16be(reg, &mod->regs->control);
+        iowrite16be(val, &mod->regs->control);
+}
+
+static void __devinit ttl_setup_device(struct ttl_module *mod)
+{
+        /* reset the device to a known state */
+        iowrite16be(0x0000, &mod->regs->control);
+        iowrite16be(0x0001, &mod->regs->control);
+        iowrite16be(0x0000, &mod->regs->control);
+
+        /* put all ports in open-drain mode */
+        ttl_write_reg(mod, PORTA_IOCTL, 0x00ff);
+        ttl_write_reg(mod, PORTB_IOCTL, 0x00ff);
+        ttl_write_reg(mod, PORTC_IOCTL, 0x000f);
+
+        /* set all ports as outputs */
+        ttl_write_reg(mod, PORTA_DIRECTION, 0x0000);
+        ttl_write_reg(mod, PORTB_DIRECTION, 0x0000);
+        ttl_write_reg(mod, PORTC_DIRECTION, 0x0000);
+
+        /* set all ports to drive zeroes */
+        iowrite16be(0x0000, &mod->regs->porta);
+        iowrite16be(0x0000, &mod->regs->portb);
+        iowrite16be(0x0000, &mod->regs->portc);
+
+        /* enable all ports */
+        ttl_write_reg(mod, MASTER_CONF_CTL, CONF_PAE | CONF_PBE | CONF_PCE);
+}
+
+static int __devinit ttl_probe(struct platform_device *pdev)
+{
+        struct janz_platform_data *pdata;
+        struct device *dev = &pdev->dev;
+        struct ttl_module *mod;
+        struct gpio_chip *gpio;
+        struct resource *res;
+        int ret;
+
+        pdata = pdev->dev.platform_data;
+        if (!pdata) {
+                dev_err(dev, "no platform data\n");
+                ret = -ENXIO;
+                goto out_return;
+        }
+
+        mod = kzalloc(sizeof(*mod), GFP_KERNEL);
+        if (!mod) {
+                dev_err(dev, "unable to allocate private data\n");
+                ret = -ENOMEM;
+                goto out_return;
+        }
+
+        platform_set_drvdata(pdev, mod);
+        spin_lock_init(&mod->lock);
+
+        /* get access to the MODULbus registers for this module */
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res) {
+                dev_err(dev, "MODULbus registers not found\n");
+                ret = -ENODEV;
+                goto out_free_mod;
+        }
+
+        mod->regs = ioremap(res->start, resource_size(res));
+        if (!mod->regs) {
+                dev_err(dev, "MODULbus registers not ioremap\n");
+                ret = -ENOMEM;
+                goto out_free_mod;
+        }
+
+        ttl_setup_device(mod);
+
+        /* Initialize the GPIO data structures */
+        gpio = &mod->gpio;
+        gpio->dev = &pdev->dev;
+        gpio->label = pdev->name;
+        gpio->get = ttl_get_value;
+        gpio->set = ttl_set_value;
+        gpio->owner = THIS_MODULE;
+
+        /* request dynamic allocation */
+        gpio->base = -1;
+        gpio->ngpio = 20;
+
+        ret = gpiochip_add(gpio);
+        if (ret) {
+                dev_err(dev, "unable to add GPIO chip\n");
+                goto out_iounmap_regs;
+        }
+
+        dev_info(&pdev->dev, "module %d: registered GPIO device\n",
+                             pdata->modno);
+        return 0;
+
+out_iounmap_regs:
+        iounmap(mod->regs);
+out_free_mod:
+        kfree(mod);
+out_return:
+        return ret;
+}
+
+static int __devexit ttl_remove(struct platform_device *pdev)
+{
+        struct ttl_module *mod = platform_get_drvdata(pdev);
+        struct device *dev = &pdev->dev;
+        int ret;
+
+        ret = gpiochip_remove(&mod->gpio);
+        if (ret) {
+                dev_err(dev, "unable to remove GPIO chip\n");
+                return ret;
+        }
+
+        iounmap(mod->regs);
+        kfree(mod);
+        return 0;
+}
+
+static struct platform_driver ttl_driver = {
+        .driver         = {
+                .name   = DRV_NAME,
+                .owner  = THIS_MODULE,
+        },
+        .probe          = ttl_probe,
+        .remove         = __devexit_p(ttl_remove),
+};
+
+static int __init ttl_init(void)
+{
+        return platform_driver_register(&ttl_driver);
+}
+
+static void __exit ttl_exit(void)
+{
+        platform_driver_unregister(&ttl_driver);
+}
+
+MODULE_AUTHOR("Ira W. Snyder <iws@ovro.caltech.edu>");
+MODULE_DESCRIPTION("Janz MODULbus VMOD-TTL Driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:janz-ttl");
+
+module_init(ttl_init);
+module_exit(ttl_exit);
diff --git a/drivers/gpio/langwell_gpio.c b/drivers/gpio/langwell_gpio.c
index 00c3a14127af..8383a8d7f994 100644
--- a/drivers/gpio/langwell_gpio.c
+++ b/drivers/gpio/langwell_gpio.c
@@ -17,6 +17,7 @@
 
 /* Supports:
  * Moorestown platform Langwell chip.
+ * Medfield platform Penwell chip.
  */
 
 #include <linux/module.h>
@@ -31,44 +32,65 @@
 #include <linux/gpio.h>
 #include <linux/slab.h>
 
-struct lnw_gpio_register {
-        u32     GPLR[2];
-        u32     GPDR[2];
-        u32     GPSR[2];
-        u32     GPCR[2];
-        u32     GRER[2];
-        u32     GFER[2];
-        u32     GEDR[2];
+/*
+ * Langwell chip has 64 pins and thus there are 2 32bit registers to control
+ * each feature, while Penwell chip has 96 pins for each block, and need 3 32bit
+ * registers to control them, so we only define the order here instead of a
+ * structure, to get a bit offset for a pin (use GPDR as an example):
+ *
+ * nreg = ngpio / 32;
+ * reg = offset / 32;
+ * bit = offset % 32;
+ * reg_addr = reg_base + GPDR * nreg * 4 + reg * 4;
+ *
+ * so the bit of reg_addr is to control pin offset's GPDR feature
+*/
+
+enum GPIO_REG {
+        GPLR = 0,       /* pin level read-only */
+        GPDR,           /* pin direction */
+        GPSR,           /* pin set */
+        GPCR,           /* pin clear */
+        GRER,           /* rising edge detect */
+        GFER,           /* falling edge detect */
+        GEDR,           /* edge detect result */
 };
 
 struct lnw_gpio {
         struct gpio_chip                chip;
-        struct lnw_gpio_register        *reg_base;
+        void                            *reg_base;
         spinlock_t                      lock;
         unsigned                        irq_base;
 };
 
-static int lnw_gpio_get(struct gpio_chip *chip, unsigned offset)
+static void __iomem *gpio_reg(struct gpio_chip *chip, unsigned offset,
+                        enum GPIO_REG reg_type)
 {
         struct lnw_gpio *lnw = container_of(chip, struct lnw_gpio, chip);
+        unsigned nreg = chip->ngpio / 32;
         u8 reg = offset / 32;
-        void __iomem *gplr;
+        void __iomem *ptr;
+
+        ptr = (void __iomem *)(lnw->reg_base + reg_type * nreg * 4 + reg * 4);
+        return ptr;
+}
+
+static int lnw_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+        void __iomem *gplr = gpio_reg(chip, offset, GPLR);
 
-        gplr = (void __iomem *)(&lnw->reg_base->GPLR[reg]);
         return readl(gplr) & BIT(offset % 32);
 }
 
 static void lnw_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-        struct lnw_gpio *lnw = container_of(chip, struct lnw_gpio, chip);
-        u8 reg = offset / 32;
         void __iomem *gpsr, *gpcr;
 
         if (value) {
-                gpsr = (void __iomem *)(&lnw->reg_base->GPSR[reg]);
+                gpsr = gpio_reg(chip, offset, GPSR);
                 writel(BIT(offset % 32), gpsr);
         } else {
-                gpcr = (void __iomem *)(&lnw->reg_base->GPCR[reg]);
+                gpcr = gpio_reg(chip, offset, GPCR);
                 writel(BIT(offset % 32), gpcr);
         }
 }
@@ -76,12 +98,10 @@ static void lnw_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 static int lnw_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
         struct lnw_gpio *lnw = container_of(chip, struct lnw_gpio, chip);
-        u8 reg = offset / 32;
+        void __iomem *gpdr = gpio_reg(chip, offset, GPDR);
         u32 value;
         unsigned long flags;
-        void __iomem *gpdr;
 
-        gpdr = (void __iomem *)(&lnw->reg_base->GPDR[reg]);
         spin_lock_irqsave(&lnw->lock, flags);
         value = readl(gpdr);
         value &= ~BIT(offset % 32);
@@ -94,12 +114,10 @@ static int lnw_gpio_direction_output(struct gpio_chip *chip,
                         unsigned offset, int value)
 {
         struct lnw_gpio *lnw = container_of(chip, struct lnw_gpio, chip);
-        u8 reg = offset / 32;
+        void __iomem *gpdr = gpio_reg(chip, offset, GPDR);
         unsigned long flags;
-        void __iomem *gpdr;
 
         lnw_gpio_set(chip, offset, value);
-        gpdr = (void __iomem *)(&lnw->reg_base->GPDR[reg]);
         spin_lock_irqsave(&lnw->lock, flags);
         value = readl(gpdr);
         value |= BIT(offset % 32);;
@@ -118,11 +136,10 @@ static int lnw_irq_type(unsigned irq, unsigned type)
 {
         struct lnw_gpio *lnw = get_irq_chip_data(irq);
         u32 gpio = irq - lnw->irq_base;
-        u8 reg = gpio / 32;
         unsigned long flags;
         u32 value;
-        void __iomem *grer = (void __iomem *)(&lnw->reg_base->GRER[reg]);
-        void __iomem *gfer = (void __iomem *)(&lnw->reg_base->GFER[reg]);
+        void __iomem *grer = gpio_reg(&lnw->chip, gpio, GRER);
+        void __iomem *gfer = gpio_reg(&lnw->chip, gpio, GFER);
 
         if (gpio >= lnw->chip.ngpio)
                 return -EINVAL;
@@ -158,8 +175,10 @@ static struct irq_chip lnw_irqchip = {
         .set_type       = lnw_irq_type,
 };
 
-static struct pci_device_id lnw_gpio_ids[] = {
-        { PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x080f) },
+static DEFINE_PCI_DEVICE_TABLE(lnw_gpio_ids) = {   /* pin number */
+        { PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x080f), .driver_data = 64 },
+        { PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x081f), .driver_data = 96 },
+        { PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x081a), .driver_data = 96 },
         { 0, }
 };
 MODULE_DEVICE_TABLE(pci, lnw_gpio_ids);
@@ -167,17 +186,17 @@ MODULE_DEVICE_TABLE(pci, lnw_gpio_ids);
 static void lnw_irq_handler(unsigned irq, struct irq_desc *desc)
 {
         struct lnw_gpio *lnw = (struct lnw_gpio *)get_irq_data(irq);
-        u32 reg, gpio;
+        u32 base, gpio;
         void __iomem *gedr;
         u32 gedr_v;
 
         /* check GPIO controller to check which pin triggered the interrupt */
-        for (reg = 0; reg < lnw->chip.ngpio / 32; reg++) {
-                gedr = (void __iomem *)(&lnw->reg_base->GEDR[reg]);
+        for (base = 0; base < lnw->chip.ngpio; base += 32) {
+                gedr = gpio_reg(&lnw->chip, base, GEDR);
                 gedr_v = readl(gedr);
                 if (!gedr_v)
                         continue;
-                for (gpio = reg*32; gpio < reg*32+32; gpio++)
+                for (gpio = base; gpio < base + 32; gpio++)
                         if (gedr_v & BIT(gpio % 32)) {
                                 pr_debug("pin %d triggered\n", gpio);
                                 generic_handle_irq(lnw->irq_base + gpio);
@@ -245,7 +264,7 @@ static int __devinit lnw_gpio_probe(struct pci_dev *pdev,
         lnw->chip.set = lnw_gpio_set;
         lnw->chip.to_irq = lnw_gpio_to_irq;
         lnw->chip.base = gpio_base;
-        lnw->chip.ngpio = 64;
+        lnw->chip.ngpio = id->driver_data;
         lnw->chip.can_sleep = 0;
         pci_set_drvdata(pdev, lnw);
         retval = gpiochip_add(&lnw->chip);
diff --git a/drivers/gpio/max732x.c b/drivers/gpio/max732x.c
index f7868243af89..9cad60f9e962 100644
--- a/drivers/gpio/max732x.c
+++ b/drivers/gpio/max732x.c
@@ -17,7 +17,8 @@
 #include <linux/slab.h>
 #include <linux/string.h>
 #include <linux/gpio.h>
-
+#include <linux/interrupt.h>
+#include <linux/irq.h>
 #include <linux/i2c.h>
 #include <linux/i2c/max732x.h>
 
@@ -31,7 +32,8 @@
  *   - Open Drain I/O
  *
  * designated by 'O', 'I' and 'P' individually according to MAXIM's
- * datasheets.
+ * datasheets. 'I' and 'P' ports are interrupt capables, some with
+ * a dedicated interrupt mask.
  *
  * There are two groups of I/O ports, each group usually includes
  * up to 8 I/O ports, and is accessed by a specific I2C address:
@@ -44,7 +46,8 @@
  *
  * Within each group of ports, there are five known combinations of
  * I/O ports: 4I4O, 4P4O, 8I, 8P, 8O, see the definitions below for
- * the detailed organization of these ports.
+ * the detailed organization of these ports. Only Goup A is interrupt
+ * capable.
  *
  * GPIO numbers start from 'gpio_base + 0' to 'gpio_base + 8/16',
  * and GPIOs from GROUP_A are numbered before those from GROUP_B
@@ -68,16 +71,47 @@
 #define GROUP_A(x)      ((x) & 0xffff)  /* I2C Addr: 0b'110xxxx */
 #define GROUP_B(x)      ((x) << 16)     /* I2C Addr: 0b'101xxxx */
 
+#define INT_NONE        0x0     /* No interrupt capability */
+#define INT_NO_MASK     0x1     /* Has interrupts, no mask */
+#define INT_INDEP_MASK  0x2     /* Has interrupts, independent mask */
+#define INT_MERGED_MASK 0x3     /* Has interrupts, merged mask */
+
+#define INT_CAPS(x)     (((uint64_t)(x)) << 32)
+
+enum {
+        MAX7319,
+        MAX7320,
+        MAX7321,
+        MAX7322,
+        MAX7323,
+        MAX7324,
+        MAX7325,
+        MAX7326,
+        MAX7327,
+};
+
+static uint64_t max732x_features[] = {
+        [MAX7319] = GROUP_A(IO_8I) | INT_CAPS(INT_MERGED_MASK),
+        [MAX7320] = GROUP_B(IO_8O),
+        [MAX7321] = GROUP_A(IO_8P) | INT_CAPS(INT_NO_MASK),
+        [MAX7322] = GROUP_A(IO_4I4O) | INT_CAPS(INT_MERGED_MASK),
+        [MAX7323] = GROUP_A(IO_4P4O) | INT_CAPS(INT_INDEP_MASK),
+        [MAX7324] = GROUP_A(IO_8I) | GROUP_B(IO_8O) | INT_CAPS(INT_MERGED_MASK),
+        [MAX7325] = GROUP_A(IO_8P) | GROUP_B(IO_8O) | INT_CAPS(INT_NO_MASK),
+        [MAX7326] = GROUP_A(IO_4I4O) | GROUP_B(IO_8O) | INT_CAPS(INT_MERGED_MASK),
+        [MAX7327] = GROUP_A(IO_4P4O) | GROUP_B(IO_8O) | INT_CAPS(INT_NO_MASK),
+};
+
 static const struct i2c_device_id max732x_id[] = {
-        { "max7319", GROUP_A(IO_8I) },
-        { "max7320", GROUP_B(IO_8O) },
-        { "max7321", GROUP_A(IO_8P) },
-        { "max7322", GROUP_A(IO_4I4O) },
-        { "max7323", GROUP_A(IO_4P4O) },
-        { "max7324", GROUP_A(IO_8I) | GROUP_B(IO_8O) },
-        { "max7325", GROUP_A(IO_8P) | GROUP_B(IO_8O) },
-        { "max7326", GROUP_A(IO_4I4O) | GROUP_B(IO_8O) },
-        { "max7327", GROUP_A(IO_4P4O) | GROUP_B(IO_8O) },
+        { "max7319", MAX7319 },
+        { "max7320", MAX7320 },
+        { "max7321", MAX7321 },
+        { "max7322", MAX7322 },
+        { "max7323", MAX7323 },
+        { "max7324", MAX7324 },
+        { "max7325", MAX7325 },
+        { "max7326", MAX7326 },
+        { "max7327", MAX7327 },
         { },
 };
 MODULE_DEVICE_TABLE(i2c, max732x_id);
@@ -96,9 +130,19 @@ struct max732x_chip {
 
         struct mutex    lock;
         uint8_t         reg_out[2];
+
+#ifdef CONFIG_GPIO_MAX732X_IRQ
+        struct mutex    irq_lock;
+        int             irq_base;
+        uint8_t         irq_mask;
+        uint8_t         irq_mask_cur;
+        uint8_t         irq_trig_raise;
+        uint8_t         irq_trig_fall;
+        uint8_t         irq_features;
+#endif
 };
 
-static int max732x_write(struct max732x_chip *chip, int group_a, uint8_t val)
+static int max732x_writeb(struct max732x_chip *chip, int group_a, uint8_t val)
 {
         struct i2c_client *client;
         int ret;
@@ -113,7 +157,7 @@ static int max732x_write(struct max732x_chip *chip, int group_a, uint8_t val)
         return 0;
 }
 
-static int max732x_read(struct max732x_chip *chip, int group_a, uint8_t *val)
+static int max732x_readb(struct max732x_chip *chip, int group_a, uint8_t *val)
 {
         struct i2c_client *client;
         int ret;
@@ -142,7 +186,7 @@ static int max732x_gpio_get_value(struct gpio_chip *gc, unsigned off)
 
         chip = container_of(gc, struct max732x_chip, gpio_chip);
 
-        ret = max732x_read(chip, is_group_a(chip, off), &reg_val);
+        ret = max732x_readb(chip, is_group_a(chip, off), &reg_val);
         if (ret < 0)
                 return 0;
 
@@ -162,7 +206,7 @@ static void max732x_gpio_set_value(struct gpio_chip *gc, unsigned off, int val)
         reg_out = (off > 7) ? chip->reg_out[1] : chip->reg_out[0];
         reg_out = (val) ? reg_out | mask : reg_out & ~mask;
 
-        ret = max732x_write(chip, is_group_a(chip, off), reg_out);
+        ret = max732x_writeb(chip, is_group_a(chip, off), reg_out);
         if (ret < 0)
                 goto out;
 
@@ -188,6 +232,13 @@ static int max732x_gpio_direction_input(struct gpio_chip *gc, unsigned off)
                 return -EACCES;
         }
 
+        /*
+         * Open-drain pins must be set to high impedance (which is
+         * equivalent to output-high) to be turned into an input.
+         */
+        if ((mask & chip->dir_output))
+                max732x_gpio_set_value(gc, off, 1);
+
         return 0;
 }
 
@@ -209,12 +260,278 @@ static int max732x_gpio_direction_output(struct gpio_chip *gc,
         return 0;
 }
 
+#ifdef CONFIG_GPIO_MAX732X_IRQ
+static int max732x_writew(struct max732x_chip *chip, uint16_t val)
+{
+        int ret;
+
+        val = cpu_to_le16(val);
+
+        ret = i2c_master_send(chip->client_group_a, (char *)&val, 2);
+        if (ret < 0) {
+                dev_err(&chip->client_group_a->dev, "failed writing\n");
+                return ret;
+        }
+
+        return 0;
+}
+
+static int max732x_readw(struct max732x_chip *chip, uint16_t *val)
+{
+        int ret;
+
+        ret = i2c_master_recv(chip->client_group_a, (char *)val, 2);
+        if (ret < 0) {
+                dev_err(&chip->client_group_a->dev, "failed reading\n");
+                return ret;
+        }
+
+        *val = le16_to_cpu(*val);
+        return 0;
+}
+
+static void max732x_irq_update_mask(struct max732x_chip *chip)
+{
+        uint16_t msg;
+
+        if (chip->irq_mask == chip->irq_mask_cur)
+                return;
+
+        chip->irq_mask = chip->irq_mask_cur;
+
+        if (chip->irq_features == INT_NO_MASK)
+                return;
+
+        mutex_lock(&chip->lock);
+
+        switch (chip->irq_features) {
+        case INT_INDEP_MASK:
+                msg = (chip->irq_mask << 8) | chip->reg_out[0];
+                max732x_writew(chip, msg);
+                break;
+
+        case INT_MERGED_MASK:
+                msg = chip->irq_mask | chip->reg_out[0];
+                max732x_writeb(chip, 1, (uint8_t)msg);
+                break;
+        }
+
+        mutex_unlock(&chip->lock);
+}
+
+static int max732x_gpio_to_irq(struct gpio_chip *gc, unsigned off)
+{
+        struct max732x_chip *chip;
+
+        chip = container_of(gc, struct max732x_chip, gpio_chip);
+        return chip->irq_base + off;
+}
+
+static void max732x_irq_mask(unsigned int irq)
+{
+        struct max732x_chip *chip = get_irq_chip_data(irq);
+
+        chip->irq_mask_cur &= ~(1 << (irq - chip->irq_base));
+}
+
+static void max732x_irq_unmask(unsigned int irq)
+{
+        struct max732x_chip *chip = get_irq_chip_data(irq);
+
+        chip->irq_mask_cur |= 1 << (irq - chip->irq_base);
+}
+
+static void max732x_irq_bus_lock(unsigned int irq)
+{
+        struct max732x_chip *chip = get_irq_chip_data(irq);
+
+        mutex_lock(&chip->irq_lock);
+        chip->irq_mask_cur = chip->irq_mask;
+}
+
+static void max732x_irq_bus_sync_unlock(unsigned int irq)
+{
+        struct max732x_chip *chip = get_irq_chip_data(irq);
+
+        max732x_irq_update_mask(chip);
+        mutex_unlock(&chip->irq_lock);
+}
+
+static int max732x_irq_set_type(unsigned int irq, unsigned int type)
+{
+        struct max732x_chip *chip = get_irq_chip_data(irq);
+        uint16_t off = irq - chip->irq_base;
+        uint16_t mask = 1 << off;
+
+        if (!(mask & chip->dir_input)) {
+                dev_dbg(&chip->client->dev, "%s port %d is output only\n",
+                        chip->client->name, off);
+                return -EACCES;
+        }
+
+        if (!(type & IRQ_TYPE_EDGE_BOTH)) {
+                dev_err(&chip->client->dev, "irq %d: unsupported type %d\n",
+                        irq, type);
+                return -EINVAL;
+        }
+
+        if (type & IRQ_TYPE_EDGE_FALLING)
+                chip->irq_trig_fall |= mask;
+        else
+                chip->irq_trig_fall &= ~mask;
+
+        if (type & IRQ_TYPE_EDGE_RISING)
+                chip->irq_trig_raise |= mask;
+        else
+                chip->irq_trig_raise &= ~mask;
+
+        return max732x_gpio_direction_input(&chip->gpio_chip, off);
+}
+
+static struct irq_chip max732x_irq_chip = {
+        .name                   = "max732x",
+        .mask                   = max732x_irq_mask,
+        .unmask                 = max732x_irq_unmask,
+        .bus_lock               = max732x_irq_bus_lock,
+        .bus_sync_unlock        = max732x_irq_bus_sync_unlock,
+        .set_type               = max732x_irq_set_type,
+};
+
+static uint8_t max732x_irq_pending(struct max732x_chip *chip)
+{
+        uint8_t cur_stat;
+        uint8_t old_stat;
+        uint8_t trigger;
+        uint8_t pending;
+        uint16_t status;
+        int ret;
+
+        ret = max732x_readw(chip, &status);
+        if (ret)
+                return 0;
+
+        trigger = status >> 8;
+        trigger &= chip->irq_mask;
+
+        if (!trigger)
+                return 0;
+
+        cur_stat = status & 0xFF;
+        cur_stat &= chip->irq_mask;
+
+        old_stat = cur_stat ^ trigger;
+
+        pending = (old_stat & chip->irq_trig_fall) |
+                  (cur_stat & chip->irq_trig_raise);
+        pending &= trigger;
+
+        return pending;
+}
+
+static irqreturn_t max732x_irq_handler(int irq, void *devid)
+{
+        struct max732x_chip *chip = devid;
+        uint8_t pending;
+        uint8_t level;
+
+        pending = max732x_irq_pending(chip);
+
+        if (!pending)
+                return IRQ_HANDLED;
+
+        do {
+                level = __ffs(pending);
+                handle_nested_irq(level + chip->irq_base);
+
+                pending &= ~(1 << level);
+        } while (pending);
+
+        return IRQ_HANDLED;
+}
+
+static int max732x_irq_setup(struct max732x_chip *chip,
+                             const struct i2c_device_id *id)
+{
+        struct i2c_client *client = chip->client;
+        struct max732x_platform_data *pdata = client->dev.platform_data;
+        int has_irq = max732x_features[id->driver_data] >> 32;
+        int ret;
+
+        if (pdata->irq_base && has_irq != INT_NONE) {
+                int lvl;
+
+                chip->irq_base = pdata->irq_base;
+                chip->irq_features = has_irq;
+                mutex_init(&chip->irq_lock);
+
+                for (lvl = 0; lvl < chip->gpio_chip.ngpio; lvl++) {
+                        int irq = lvl + chip->irq_base;
+
+                        if (!(chip->dir_input & (1 << lvl)))
+                                continue;
+
+                        set_irq_chip_data(irq, chip);
+                        set_irq_chip_and_handler(irq, &max732x_irq_chip,
+                                                 handle_edge_irq);
+                        set_irq_nested_thread(irq, 1);
+#ifdef CONFIG_ARM
+                        set_irq_flags(irq, IRQF_VALID);
+#else
+                        set_irq_noprobe(irq);
+#endif
+                }
+
+                ret = request_threaded_irq(client->irq,
+                                           NULL,
+                                           max732x_irq_handler,
+                                           IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+                                           dev_name(&client->dev), chip);
+                if (ret) {
+                        dev_err(&client->dev, "failed to request irq %d\n",
+                                client->irq);
+                        goto out_failed;
+                }
+
+                chip->gpio_chip.to_irq = max732x_gpio_to_irq;
+        }
+
+        return 0;
+
+out_failed:
+        chip->irq_base = 0;
+        return ret;
+}
+
+static void max732x_irq_teardown(struct max732x_chip *chip)
+{
+        if (chip->irq_base)
+                free_irq(chip->client->irq, chip);
+}
+#else /* CONFIG_GPIO_MAX732X_IRQ */
+static int max732x_irq_setup(struct max732x_chip *chip,
+                             const struct i2c_device_id *id)
+{
+        struct i2c_client *client = chip->client;
+        struct max732x_platform_data *pdata = client->dev.platform_data;
+        int has_irq = max732x_features[id->driver_data] >> 32;
+
+        if (pdata->irq_base && has_irq != INT_NONE)
+                dev_warn(&client->dev, "interrupt support not compiled in\n");
+
+        return 0;
+}
+
+static void max732x_irq_teardown(struct max732x_chip *chip)
+{
+}
+#endif
+
 static int __devinit max732x_setup_gpio(struct max732x_chip *chip,
                                         const struct i2c_device_id *id,
                                         unsigned gpio_start)
 {
         struct gpio_chip *gc = &chip->gpio_chip;
-        uint32_t id_data = id->driver_data;
+        uint32_t id_data = (uint32_t)max732x_features[id->driver_data];
         int i, port = 0;
 
         for (i = 0; i < 16; i++, id_data >>= 2) {
@@ -285,14 +602,14 @@ static int __devinit max732x_probe(struct i2c_client *client,
         switch (client->addr & 0x70) {
         case 0x60:
                 chip->client_group_a = client;
-                if (nr_port > 7) {
+                if (nr_port > 8) {
                         c = i2c_new_dummy(client->adapter, addr_b);
                         chip->client_group_b = chip->client_dummy = c;
                 }
                 break;
         case 0x50:
                 chip->client_group_b = client;
-                if (nr_port > 7) {
+                if (nr_port > 8) {
                         c = i2c_new_dummy(client->adapter, addr_a);
                         chip->client_group_a = chip->client_dummy = c;
                 }
@@ -306,9 +623,13 @@ static int __devinit max732x_probe(struct i2c_client *client,
 
         mutex_init(&chip->lock);
 
-        max732x_read(chip, is_group_a(chip, 0), &chip->reg_out[0]);
-        if (nr_port > 7)
-                max732x_read(chip, is_group_a(chip, 8), &chip->reg_out[1]);
+        max732x_readb(chip, is_group_a(chip, 0), &chip->reg_out[0]);
+        if (nr_port > 8)
+                max732x_readb(chip, is_group_a(chip, 8), &chip->reg_out[1]);
+
+        ret = max732x_irq_setup(chip, id);
+        if (ret)
+                goto out_failed;
 
         ret = gpiochip_add(&chip->gpio_chip);
         if (ret)
@@ -325,6 +646,7 @@ static int __devinit max732x_probe(struct i2c_client *client,
         return 0;
 
 out_failed:
+        max732x_irq_teardown(chip);
         kfree(chip);
         return ret;
 }
@@ -352,6 +674,8 @@ static int __devexit max732x_remove(struct i2c_client *client)
                 return ret;
         }
 
+        max732x_irq_teardown(chip);
+
         /* unregister any dummy i2c_client */
         if (chip->client_dummy)
                 i2c_unregister_device(chip->client_dummy);
diff --git a/drivers/gpio/pca953x.c b/drivers/gpio/pca953x.c
index f156ab3bb6ed..a2b12aa1f2b9 100644
--- a/drivers/gpio/pca953x.c
+++ b/drivers/gpio/pca953x.c
@@ -73,7 +73,7 @@ struct pca953x_chip {
         struct i2c_client *client;
         struct pca953x_platform_data *dyn_pdata;
         struct gpio_chip gpio_chip;
-        char **names;
+        const char *const *names;
 };
 
 static int pca953x_write_reg(struct pca953x_chip *chip, int reg, uint16_t val)
diff --git a/drivers/gpio/pl061.c b/drivers/gpio/pl061.c
index 105701a1f05b..ee568c8fcbd0 100644
--- a/drivers/gpio/pl061.c
+++ b/drivers/gpio/pl061.c
@@ -164,7 +164,7 @@ static int pl061_irq_type(unsigned irq, unsigned trigger)
         unsigned long flags;
         u8 gpiois, gpioibe, gpioiev;
 
-        if (offset < 0 || offset > PL061_GPIO_NR)
+        if (offset < 0 || offset >= PL061_GPIO_NR)
                 return -EINVAL;
 
         spin_lock_irqsave(&chip->irq_lock, flags);
diff --git a/drivers/gpio/rdc321x-gpio.c b/drivers/gpio/rdc321x-gpio.c
new file mode 100644
index 000000000000..2762698e0204
--- /dev/null
+++ b/drivers/gpio/rdc321x-gpio.c
@@ -0,0 +1,246 @@
+/*
+ * RDC321x GPIO driver
+ *
+ * Copyright (C) 2008, Volker Weiss <dev@tintuc.de>
+ * Copyright (C) 2007-2010 Florian Fainelli <florian@openwrt.org>
+ *
+ * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/pci.h>
+#include <linux/gpio.h>
+#include <linux/mfd/rdc321x.h>
+#include <linux/slab.h>
+
+struct rdc321x_gpio {
+        spinlock_t              lock;
+        struct pci_dev          *sb_pdev;
+        u32                     data_reg[2];
+        int                     reg1_ctrl_base;
+        int                     reg1_data_base;
+        int                     reg2_ctrl_base;
+        int                     reg2_data_base;
+        struct gpio_chip        chip;
+};
+
+/* read GPIO pin */
+static int rdc_gpio_get_value(struct gpio_chip *chip, unsigned gpio)
+{
+        struct rdc321x_gpio *gpch;
+        u32 value = 0;
+        int reg;
+
+        gpch = container_of(chip, struct rdc321x_gpio, chip);
+        reg = gpio < 32 ? gpch->reg1_data_base : gpch->reg2_data_base;
+
+        spin_lock(&gpch->lock);
+        pci_write_config_dword(gpch->sb_pdev, reg,
+                                        gpch->data_reg[gpio < 32 ? 0 : 1]);
+        pci_read_config_dword(gpch->sb_pdev, reg, &value);
+        spin_unlock(&gpch->lock);
+
+        return (1 << (gpio & 0x1f)) & value ? 1 : 0;
+}
+
+static void rdc_gpio_set_value_impl(struct gpio_chip *chip,
+                                unsigned gpio, int value)
+{
+        struct rdc321x_gpio *gpch;
+        int reg = (gpio < 32) ? 0 : 1;
+
+        gpch = container_of(chip, struct rdc321x_gpio, chip);
+
+        if (value)
+                gpch->data_reg[reg] |= 1 << (gpio & 0x1f);
+        else
+                gpch->data_reg[reg] &= ~(1 << (gpio & 0x1f));
+
+        pci_write_config_dword(gpch->sb_pdev,
+                        reg ? gpch->reg2_data_base : gpch->reg1_data_base,
+                        gpch->data_reg[reg]);
+}
+
+/* set GPIO pin to value */
+static void rdc_gpio_set_value(struct gpio_chip *chip,
+                                unsigned gpio, int value)
+{
+        struct rdc321x_gpio *gpch;
+
+        gpch = container_of(chip, struct rdc321x_gpio, chip);
+        spin_lock(&gpch->lock);
+        rdc_gpio_set_value_impl(chip, gpio, value);
+        spin_unlock(&gpch->lock);
+}
+
+static int rdc_gpio_config(struct gpio_chip *chip,
+                                unsigned gpio, int value)
+{
+        struct rdc321x_gpio *gpch;
+        int err;
+        u32 reg;
+
+        gpch = container_of(chip, struct rdc321x_gpio, chip);
+
+        spin_lock(&gpch->lock);
+        err = pci_read_config_dword(gpch->sb_pdev, gpio < 32 ?
+                        gpch->reg1_ctrl_base : gpch->reg2_ctrl_base, &reg);
+        if (err)
+                goto unlock;
+
+        reg |= 1 << (gpio & 0x1f);
+
+        err = pci_write_config_dword(gpch->sb_pdev, gpio < 32 ?
+                        gpch->reg1_ctrl_base : gpch->reg2_ctrl_base, reg);
+        if (err)
+                goto unlock;
+
+        rdc_gpio_set_value_impl(chip, gpio, value);
+
+unlock:
+        spin_unlock(&gpch->lock);
+
+        return err;
+}
+
+/* configure GPIO pin as input */
+static int rdc_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
+{
+        return rdc_gpio_config(chip, gpio, 1);
+}
+
+/*
+ * Cache the initial value of both GPIO data registers
+ */
+static int __devinit rdc321x_gpio_probe(struct platform_device *pdev)
+{
+        int err;
+        struct resource *r;
+        struct rdc321x_gpio *rdc321x_gpio_dev;
+        struct rdc321x_gpio_pdata *pdata;
+
+        pdata = pdev->dev.platform_data;
+        if (!pdata) {
+                dev_err(&pdev->dev, "no platform data supplied\n");
+                return -ENODEV;
+        }
+
+        rdc321x_gpio_dev = kzalloc(sizeof(struct rdc321x_gpio), GFP_KERNEL);
+        if (!rdc321x_gpio_dev) {
+                dev_err(&pdev->dev, "failed to allocate private data\n");
+                return -ENOMEM;
+        }
+
+        r = platform_get_resource_byname(pdev, IORESOURCE_IO, "gpio-reg1");
+        if (!r) {
+                dev_err(&pdev->dev, "failed to get gpio-reg1 resource\n");
+                err = -ENODEV;
+                goto out_free;
+        }
+
+        spin_lock_init(&rdc321x_gpio_dev->lock);
+        rdc321x_gpio_dev->sb_pdev = pdata->sb_pdev;
+        rdc321x_gpio_dev->reg1_ctrl_base = r->start;
+        rdc321x_gpio_dev->reg1_data_base = r->start + 0x4;
+
+        r = platform_get_resource_byname(pdev, IORESOURCE_IO, "gpio-reg2");
+        if (!r) {
+                dev_err(&pdev->dev, "failed to get gpio-reg2 resource\n");
+                err = -ENODEV;
+                goto out_free;
+        }
+
+        rdc321x_gpio_dev->reg2_ctrl_base = r->start;
+        rdc321x_gpio_dev->reg2_data_base = r->start + 0x4;
+
+        rdc321x_gpio_dev->chip.label = "rdc321x-gpio";
+        rdc321x_gpio_dev->chip.direction_input = rdc_gpio_direction_input;
+        rdc321x_gpio_dev->chip.direction_output = rdc_gpio_config;
+        rdc321x_gpio_dev->chip.get = rdc_gpio_get_value;
+        rdc321x_gpio_dev->chip.set = rdc_gpio_set_value;
+        rdc321x_gpio_dev->chip.base = 0;
+        rdc321x_gpio_dev->chip.ngpio = pdata->max_gpios;
+
+        platform_set_drvdata(pdev, rdc321x_gpio_dev);
+
+        /* This might not be, what others (BIOS, bootloader, etc.)
+           wrote to these registers before, but it's a good guess. Still
+           better than just using 0xffffffff. */
+        err = pci_read_config_dword(rdc321x_gpio_dev->sb_pdev,
+                                        rdc321x_gpio_dev->reg1_data_base,
+                                        &rdc321x_gpio_dev->data_reg[0]);
+        if (err)
+                goto out_drvdata;
+
+        err = pci_read_config_dword(rdc321x_gpio_dev->sb_pdev,
+                                        rdc321x_gpio_dev->reg2_data_base,
+                                        &rdc321x_gpio_dev->data_reg[1]);
+        if (err)
+                goto out_drvdata;
+
+        dev_info(&pdev->dev, "registering %d GPIOs\n",
+                                        rdc321x_gpio_dev->chip.ngpio);
+        return gpiochip_add(&rdc321x_gpio_dev->chip);
+
+out_drvdata:
+        platform_set_drvdata(pdev, NULL);
+out_free:
+        kfree(rdc321x_gpio_dev);
+        return err;
+}
+
+static int __devexit rdc321x_gpio_remove(struct platform_device *pdev)
+{
+        int ret;
+        struct rdc321x_gpio *rdc321x_gpio_dev = platform_get_drvdata(pdev);
+
+        ret = gpiochip_remove(&rdc321x_gpio_dev->chip);
+        if (ret)
+                dev_err(&pdev->dev, "failed to unregister chip\n");
+
+        kfree(rdc321x_gpio_dev);
+        platform_set_drvdata(pdev, NULL);
+
+        return ret;
+}
+
+static struct platform_driver rdc321x_gpio_driver = {
+        .driver.name    = "rdc321x-gpio",
+        .driver.owner   = THIS_MODULE,
+        .probe          = rdc321x_gpio_probe,
+        .remove         = __devexit_p(rdc321x_gpio_remove),
+};
+
+static int __init rdc321x_gpio_init(void)
+{
+        return platform_driver_register(&rdc321x_gpio_driver);
+}
+
+static void __exit rdc321x_gpio_exit(void)
+{
+        platform_driver_unregister(&rdc321x_gpio_driver);
+}
+
+module_init(rdc321x_gpio_init);
+module_exit(rdc321x_gpio_exit);
+
+MODULE_AUTHOR("Florian Fainelli <florian@openwrt.org>");
+MODULE_DESCRIPTION("RDC321x GPIO driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rdc321x-gpio");
diff --git a/drivers/gpio/tc35892-gpio.c b/drivers/gpio/tc35892-gpio.c
new file mode 100644
index 000000000000..1be6288780de
--- /dev/null
+++ b/drivers/gpio/tc35892-gpio.c
@@ -0,0 +1,381 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License, version 2
+ * Author: Hanumath Prasad <hanumath.prasad@stericsson.com> for ST-Ericsson
+ * Author: Rabin Vincent <rabin.vincent@stericsson.com> for ST-Ericsson
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/tc35892.h>
+
+/*
+ * These registers are modified under the irq bus lock and cached to avoid
+ * unnecessary writes in bus_sync_unlock.
+ */
+enum { REG_IBE, REG_IEV, REG_IS, REG_IE };
+
+#define CACHE_NR_REGS   4
+#define CACHE_NR_BANKS  3
+
+struct tc35892_gpio {
+        struct gpio_chip chip;
+        struct tc35892 *tc35892;
+        struct device *dev;
+        struct mutex irq_lock;
+
+        int irq_base;
+
+        /* Caches of interrupt control registers for bus_lock */
+        u8 regs[CACHE_NR_REGS][CACHE_NR_BANKS];
+        u8 oldregs[CACHE_NR_REGS][CACHE_NR_BANKS];
+};
+
+static inline struct tc35892_gpio *to_tc35892_gpio(struct gpio_chip *chip)
+{
+        return container_of(chip, struct tc35892_gpio, chip);
+}
+
+static int tc35892_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+        struct tc35892_gpio *tc35892_gpio = to_tc35892_gpio(chip);
+        struct tc35892 *tc35892 = tc35892_gpio->tc35892;
+        u8 reg = TC35892_GPIODATA0 + (offset / 8) * 2;
+        u8 mask = 1 << (offset % 8);
+        int ret;
+
+        ret = tc35892_reg_read(tc35892, reg);
+        if (ret < 0)
+                return ret;
+
+        return ret & mask;
+}
+
+static void tc35892_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
+{
+        struct tc35892_gpio *tc35892_gpio = to_tc35892_gpio(chip);
+        struct tc35892 *tc35892 = tc35892_gpio->tc35892;
+        u8 reg = TC35892_GPIODATA0 + (offset / 8) * 2;
+        unsigned pos = offset % 8;
+        u8 data[] = {!!val << pos, 1 << pos};
+
+        tc35892_block_write(tc35892, reg, ARRAY_SIZE(data), data);
+}
+
+static int tc35892_gpio_direction_output(struct gpio_chip *chip,
+                                         unsigned offset, int val)
+{
+        struct tc35892_gpio *tc35892_gpio = to_tc35892_gpio(chip);
+        struct tc35892 *tc35892 = tc35892_gpio->tc35892;
+        u8 reg = TC35892_GPIODIR0 + offset / 8;
+        unsigned pos = offset % 8;
+
+        tc35892_gpio_set(chip, offset, val);
+
+        return tc35892_set_bits(tc35892, reg, 1 << pos, 1 << pos);
+}
+
+static int tc35892_gpio_direction_input(struct gpio_chip *chip,
+                                        unsigned offset)
+{
+        struct tc35892_gpio *tc35892_gpio = to_tc35892_gpio(chip);
+        struct tc35892 *tc35892 = tc35892_gpio->tc35892;
+        u8 reg = TC35892_GPIODIR0 + offset / 8;
+        unsigned pos = offset % 8;
+
+        return tc35892_set_bits(tc35892, reg, 1 << pos, 0);
+}
+
+static int tc35892_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
+{
+        struct tc35892_gpio *tc35892_gpio = to_tc35892_gpio(chip);
+
+        return tc35892_gpio->irq_base + offset;
+}
+
+static struct gpio_chip template_chip = {
+        .label                  = "tc35892",
+        .owner                  = THIS_MODULE,
+        .direction_input        = tc35892_gpio_direction_input,
+        .get                    = tc35892_gpio_get,
+        .direction_output       = tc35892_gpio_direction_output,
+        .set                    = tc35892_gpio_set,
+        .to_irq                 = tc35892_gpio_to_irq,
+        .can_sleep              = 1,
+};
+
+static int tc35892_gpio_irq_set_type(unsigned int irq, unsigned int type)
+{
+        struct tc35892_gpio *tc35892_gpio = get_irq_chip_data(irq);
+        int offset = irq - tc35892_gpio->irq_base;
+        int regoffset = offset / 8;
+        int mask = 1 << (offset % 8);
+
+        if (type == IRQ_TYPE_EDGE_BOTH) {
+                tc35892_gpio->regs[REG_IBE][regoffset] |= mask;
+                return 0;
+        }
+
+        tc35892_gpio->regs[REG_IBE][regoffset] &= ~mask;
+
+        if (type == IRQ_TYPE_LEVEL_LOW || type == IRQ_TYPE_LEVEL_HIGH)
+                tc35892_gpio->regs[REG_IS][regoffset] |= mask;
+        else
+                tc35892_gpio->regs[REG_IS][regoffset] &= ~mask;
+
+        if (type == IRQ_TYPE_EDGE_RISING || type == IRQ_TYPE_LEVEL_HIGH)
+                tc35892_gpio->regs[REG_IEV][regoffset] |= mask;
+        else
+                tc35892_gpio->regs[REG_IEV][regoffset] &= ~mask;
+
+        return 0;
+}
+
+static void tc35892_gpio_irq_lock(unsigned int irq)
+{
+        struct tc35892_gpio *tc35892_gpio = get_irq_chip_data(irq);
+
+        mutex_lock(&tc35892_gpio->irq_lock);
+}
+
+static void tc35892_gpio_irq_sync_unlock(unsigned int irq)
+{
+        struct tc35892_gpio *tc35892_gpio = get_irq_chip_data(irq);
+        struct tc35892 *tc35892 = tc35892_gpio->tc35892;
+        static const u8 regmap[] = {
+                [REG_IBE]       = TC35892_GPIOIBE0,
+                [REG_IEV]       = TC35892_GPIOIEV0,
+                [REG_IS]        = TC35892_GPIOIS0,
+                [REG_IE]        = TC35892_GPIOIE0,
+        };
+        int i, j;
+
+        for (i = 0; i < CACHE_NR_REGS; i++) {
+                for (j = 0; j < CACHE_NR_BANKS; j++) {
+                        u8 old = tc35892_gpio->oldregs[i][j];
+                        u8 new = tc35892_gpio->regs[i][j];
+
+                        if (new == old)
+                                continue;
+
+                        tc35892_gpio->oldregs[i][j] = new;
+                        tc35892_reg_write(tc35892, regmap[i] + j * 8, new);
+                }
+        }
+
+        mutex_unlock(&tc35892_gpio->irq_lock);
+}
+
+static void tc35892_gpio_irq_mask(unsigned int irq)
+{
+        struct tc35892_gpio *tc35892_gpio = get_irq_chip_data(irq);
+        int offset = irq - tc35892_gpio->irq_base;
+        int regoffset = offset / 8;
+        int mask = 1 << (offset % 8);
+
+        tc35892_gpio->regs[REG_IE][regoffset] &= ~mask;
+}
+
+static void tc35892_gpio_irq_unmask(unsigned int irq)
+{
+        struct tc35892_gpio *tc35892_gpio = get_irq_chip_data(irq);
+        int offset = irq - tc35892_gpio->irq_base;
+        int regoffset = offset / 8;
+        int mask = 1 << (offset % 8);
+
+        tc35892_gpio->regs[REG_IE][regoffset] |= mask;
+}
+
+static struct irq_chip tc35892_gpio_irq_chip = {
+        .name                   = "tc35892-gpio",
+        .bus_lock               = tc35892_gpio_irq_lock,
+        .bus_sync_unlock        = tc35892_gpio_irq_sync_unlock,
+        .mask                   = tc35892_gpio_irq_mask,
+        .unmask                 = tc35892_gpio_irq_unmask,
+        .set_type               = tc35892_gpio_irq_set_type,
+};
+
+static irqreturn_t tc35892_gpio_irq(int irq, void *dev)
+{
+        struct tc35892_gpio *tc35892_gpio = dev;
+        struct tc35892 *tc35892 = tc35892_gpio->tc35892;
+        u8 status[CACHE_NR_BANKS];
+        int ret;
+        int i;
+
+        ret = tc35892_block_read(tc35892, TC35892_GPIOMIS0,
+                                 ARRAY_SIZE(status), status);
+        if (ret < 0)
+                return IRQ_NONE;
+
+        for (i = 0; i < ARRAY_SIZE(status); i++) {
+                unsigned int stat = status[i];
+                if (!stat)
+                        continue;
+
+                while (stat) {
+                        int bit = __ffs(stat);
+                        int line = i * 8 + bit;
+
+                        handle_nested_irq(tc35892_gpio->irq_base + line);
+                        stat &= ~(1 << bit);
+                }
+
+                tc35892_reg_write(tc35892, TC35892_GPIOIC0 + i, status[i]);
+        }
+
+        return IRQ_HANDLED;
+}
+
+static int tc35892_gpio_irq_init(struct tc35892_gpio *tc35892_gpio)
+{
+        int base = tc35892_gpio->irq_base;
+        int irq;
+
+        for (irq = base; irq < base + tc35892_gpio->chip.ngpio; irq++) {
+                set_irq_chip_data(irq, tc35892_gpio);
+                set_irq_chip_and_handler(irq, &tc35892_gpio_irq_chip,
+                                         handle_simple_irq);
+                set_irq_nested_thread(irq, 1);
+#ifdef CONFIG_ARM
+                set_irq_flags(irq, IRQF_VALID);
+#else
+                set_irq_noprobe(irq);
+#endif
+        }
+
+        return 0;
+}
+
+static void tc35892_gpio_irq_remove(struct tc35892_gpio *tc35892_gpio)
+{
+        int base = tc35892_gpio->irq_base;
+        int irq;
+
+        for (irq = base; irq < base + tc35892_gpio->chip.ngpio; irq++) {
+#ifdef CONFIG_ARM
+                set_irq_flags(irq, 0);
+#endif
+                set_irq_chip_and_handler(irq, NULL, NULL);
+                set_irq_chip_data(irq, NULL);
+        }
+}
+
+static int __devinit tc35892_gpio_probe(struct platform_device *pdev)
+{
+        struct tc35892 *tc35892 = dev_get_drvdata(pdev->dev.parent);
+        struct tc35892_gpio_platform_data *pdata;
+        struct tc35892_gpio *tc35892_gpio;
+        int ret;
+        int irq;
+
+        pdata = tc35892->pdata->gpio;
+        if (!pdata)
+                return -ENODEV;
+
+        irq = platform_get_irq(pdev, 0);
+        if (irq < 0)
+                return irq;
+
+        tc35892_gpio = kzalloc(sizeof(struct tc35892_gpio), GFP_KERNEL);
+        if (!tc35892_gpio)
+                return -ENOMEM;
+
+        mutex_init(&tc35892_gpio->irq_lock);
+
+        tc35892_gpio->dev = &pdev->dev;
+        tc35892_gpio->tc35892 = tc35892;
+
+        tc35892_gpio->chip = template_chip;
+        tc35892_gpio->chip.ngpio = tc35892->num_gpio;
+        tc35892_gpio->chip.dev = &pdev->dev;
+        tc35892_gpio->chip.base = pdata->gpio_base;
+
+        tc35892_gpio->irq_base = tc35892->irq_base + TC35892_INT_GPIO(0);
+
+        /* Bring the GPIO module out of reset */
+        ret = tc35892_set_bits(tc35892, TC35892_RSTCTRL,
+                               TC35892_RSTCTRL_GPIRST, 0);
+        if (ret < 0)
+                goto out_free;
+
+        ret = tc35892_gpio_irq_init(tc35892_gpio);
+        if (ret)
+                goto out_free;
+
+        ret = request_threaded_irq(irq, NULL, tc35892_gpio_irq, IRQF_ONESHOT,
+                                   "tc35892-gpio", tc35892_gpio);
+        if (ret) {
+                dev_err(&pdev->dev, "unable to get irq: %d\n", ret);
+                goto out_removeirq;
+        }
+
+        ret = gpiochip_add(&tc35892_gpio->chip);
+        if (ret) {
+                dev_err(&pdev->dev, "unable to add gpiochip: %d\n", ret);
+                goto out_freeirq;
+        }
+
+        platform_set_drvdata(pdev, tc35892_gpio);
+
+        return 0;
+
+out_freeirq:
+        free_irq(irq, tc35892_gpio);
+out_removeirq:
+        tc35892_gpio_irq_remove(tc35892_gpio);
+out_free:
+        kfree(tc35892_gpio);
+        return ret;
+}
+
+static int __devexit tc35892_gpio_remove(struct platform_device *pdev)
+{
+        struct tc35892_gpio *tc35892_gpio = platform_get_drvdata(pdev);
+        int irq = platform_get_irq(pdev, 0);
+        int ret;
+
+        ret = gpiochip_remove(&tc35892_gpio->chip);
+        if (ret < 0) {
+                dev_err(tc35892_gpio->dev,
+                        "unable to remove gpiochip: %d\n", ret);
+                return ret;
+        }
+
+        free_irq(irq, tc35892_gpio);
+        tc35892_gpio_irq_remove(tc35892_gpio);
+
+        platform_set_drvdata(pdev, NULL);
+        kfree(tc35892_gpio);
+
+        return 0;
+}
+
+static struct platform_driver tc35892_gpio_driver = {
+        .driver.name    = "tc35892-gpio",
+        .driver.owner   = THIS_MODULE,
+        .probe          = tc35892_gpio_probe,
+        .remove         = __devexit_p(tc35892_gpio_remove),
+};
+
+static int __init tc35892_gpio_init(void)
+{
+        return platform_driver_register(&tc35892_gpio_driver);
+}
+subsys_initcall(tc35892_gpio_init);
+
+static void __exit tc35892_gpio_exit(void)
+{
+        platform_driver_unregister(&tc35892_gpio_driver);
+}
+module_exit(tc35892_gpio_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("TC35892 GPIO driver");
+MODULE_AUTHOR("Hanumath Prasad, Rabin Vincent");
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index 994d23beeb1d..57cea01c4ffb 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -1840,8 +1840,10 @@ int drm_mode_dirtyfb_ioctl(struct drm_device *dev,
 
                 ret = copy_from_user(clips, clips_ptr,
                                      num_clips * sizeof(*clips));
-                if (ret)
+                if (ret) {
+                        ret = -EFAULT;
                         goto out_err2;
+                }
         }
 
         if (fb->funcs->dirty) {
diff --git a/drivers/gpu/drm/drm_crtc_helper.c b/drivers/gpu/drm/drm_crtc_helper.c
index 764401951041..9b2a54117c91 100644
--- a/drivers/gpu/drm/drm_crtc_helper.c
+++ b/drivers/gpu/drm/drm_crtc_helper.c
@@ -860,19 +860,24 @@ static void output_poll_execute(struct slow_work *work)
         }
 }
 
-void drm_kms_helper_poll_init(struct drm_device *dev)
+void drm_kms_helper_poll_disable(struct drm_device *dev)
+{
+        if (!dev->mode_config.poll_enabled)
+                return;
+        delayed_slow_work_cancel(&dev->mode_config.output_poll_slow_work);
+}
+EXPORT_SYMBOL(drm_kms_helper_poll_disable);
+
+void drm_kms_helper_poll_enable(struct drm_device *dev)
 {
-        struct drm_connector *connector;
         bool poll = false;
+        struct drm_connector *connector;
         int ret;
 
         list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
                 if (connector->polled)
                         poll = true;
         }
-        slow_work_register_user(THIS_MODULE);
-        delayed_slow_work_init(&dev->mode_config.output_poll_slow_work,
-                               &output_poll_ops);
 
         if (poll) {
                 ret = delayed_slow_work_enqueue(&dev->mode_config.output_poll_slow_work, DRM_OUTPUT_POLL_PERIOD);
@@ -880,11 +885,22 @@ void drm_kms_helper_poll_init(struct drm_device *dev)
                         DRM_ERROR("delayed enqueue failed %d\n", ret);
         }
 }
+EXPORT_SYMBOL(drm_kms_helper_poll_enable);
+
+void drm_kms_helper_poll_init(struct drm_device *dev)
+{
+        slow_work_register_user(THIS_MODULE);
+        delayed_slow_work_init(&dev->mode_config.output_poll_slow_work,
+                               &output_poll_ops);
+        dev->mode_config.poll_enabled = true;
+
+        drm_kms_helper_poll_enable(dev);
+}
 EXPORT_SYMBOL(drm_kms_helper_poll_init);
 
 void drm_kms_helper_poll_fini(struct drm_device *dev)
 {
-        delayed_slow_work_cancel(&dev->mode_config.output_poll_slow_work);
+        drm_kms_helper_poll_disable(dev);
         slow_work_unregister_user(THIS_MODULE);
 }
 EXPORT_SYMBOL(drm_kms_helper_poll_fini);
diff --git a/drivers/gpu/drm/drm_edid.c b/drivers/gpu/drm/drm_edid.c
index f569ae88ab38..c1981861bbbd 100644
--- a/drivers/gpu/drm/drm_edid.c
+++ b/drivers/gpu/drm/drm_edid.c
@@ -147,7 +147,10 @@ drm_edid_block_valid(u8 *raw_edid)
                 csum += raw_edid[i];
         if (csum) {
                 DRM_ERROR("EDID checksum is invalid, remainder is %d\n", csum);
-                goto bad;
+
+                /* allow CEA to slide through, switches mangle this */
+                if (raw_edid[0] != 0x02)
+                        goto bad;
         }
 
         /* per-block-type checks */
diff --git a/drivers/gpu/drm/drm_fb_helper.c b/drivers/gpu/drm/drm_fb_helper.c
index b3779d243aef..08c4c926e65f 100644
--- a/drivers/gpu/drm/drm_fb_helper.c
+++ b/drivers/gpu/drm/drm_fb_helper.c
@@ -264,7 +264,7 @@ bool drm_fb_helper_force_kernel_mode(void)
 int drm_fb_helper_panic(struct notifier_block *n, unsigned long ununsed,
                         void *panic_str)
 {
-        DRM_ERROR("panic occurred, switching back to text console\n");
+        printk(KERN_ERR "panic occurred, switching back to text console\n");
         return drm_fb_helper_force_kernel_mode();
         return 0;
 }
diff --git a/drivers/gpu/drm/i915/Makefile b/drivers/gpu/drm/i915/Makefile
index 95639017bdbe..da78f2c0d909 100644
--- a/drivers/gpu/drm/i915/Makefile
+++ b/drivers/gpu/drm/i915/Makefile
@@ -22,6 +22,7 @@ i915-y := i915_drv.o i915_dma.o i915_irq.o i915_mem.o \
           intel_fb.o \
           intel_tv.o \
           intel_dvo.o \
+          intel_ringbuffer.o \
           intel_overlay.o \
           dvo_ch7xxx.o \
           dvo_ch7017.o \
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index 322070c0c631..52510ad8b25d 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -77,7 +77,7 @@ static int i915_gem_object_list_info(struct seq_file *m, void *data)
         case ACTIVE_LIST:
                 seq_printf(m, "Active:\n");
                 lock = &dev_priv->mm.active_list_lock;
-                head = &dev_priv->mm.active_list;
+                head = &dev_priv->render_ring.active_list;
                 break;
         case INACTIVE_LIST:
                 seq_printf(m, "Inactive:\n");
@@ -129,7 +129,8 @@ static int i915_gem_request_info(struct seq_file *m, void *data)
         struct drm_i915_gem_request *gem_request;
 
         seq_printf(m, "Request:\n");
-        list_for_each_entry(gem_request, &dev_priv->mm.request_list, list) {
+        list_for_each_entry(gem_request, &dev_priv->render_ring.request_list,
+                        list) {
                 seq_printf(m, "    %d @ %d\n",
                            gem_request->seqno,
                            (int) (jiffies - gem_request->emitted_jiffies));
@@ -143,9 +144,9 @@ static int i915_gem_seqno_info(struct seq_file *m, void *data)
         struct drm_device *dev = node->minor->dev;
         drm_i915_private_t *dev_priv = dev->dev_private;
 
-        if (dev_priv->hw_status_page != NULL) {
+        if (dev_priv->render_ring.status_page.page_addr != NULL) {
                 seq_printf(m, "Current sequence: %d\n",
-                           i915_get_gem_seqno(dev));
+                           i915_get_gem_seqno(dev,  &dev_priv->render_ring));
         } else {
                 seq_printf(m, "Current sequence: hws uninitialized\n");
         }
@@ -195,9 +196,9 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
         }
         seq_printf(m, "Interrupts received: %d\n",
                    atomic_read(&dev_priv->irq_received));
-        if (dev_priv->hw_status_page != NULL) {
+        if (dev_priv->render_ring.status_page.page_addr != NULL) {
                 seq_printf(m, "Current sequence:    %d\n",
-                           i915_get_gem_seqno(dev));
+                           i915_get_gem_seqno(dev,  &dev_priv->render_ring));
         } else {
                 seq_printf(m, "Current sequence:    hws uninitialized\n");
         }
@@ -251,7 +252,7 @@ static int i915_hws_info(struct seq_file *m, void *data)
         int i;
         volatile u32 *hws;
 
-        hws = (volatile u32 *)dev_priv->hw_status_page;
+        hws = (volatile u32 *)dev_priv->render_ring.status_page.page_addr;
         if (hws == NULL)
                 return 0;
 
@@ -287,7 +288,8 @@ static int i915_batchbuffer_info(struct seq_file *m, void *data)
 
         spin_lock(&dev_priv->mm.active_list_lock);
 
-        list_for_each_entry(obj_priv, &dev_priv->mm.active_list, list) {
+        list_for_each_entry(obj_priv, &dev_priv->render_ring.active_list,
+                        list) {
                 obj = &obj_priv->base;
                 if (obj->read_domains & I915_GEM_DOMAIN_COMMAND) {
                     ret = i915_gem_object_get_pages(obj, 0);
@@ -317,14 +319,14 @@ static int i915_ringbuffer_data(struct seq_file *m, void *data)
         u8 *virt;
         uint32_t *ptr, off;
 
-        if (!dev_priv->ring.ring_obj) {
+        if (!dev_priv->render_ring.gem_object) {
                 seq_printf(m, "No ringbuffer setup\n");
                 return 0;
         }
 
-        virt = dev_priv->ring.virtual_start;
+        virt = dev_priv->render_ring.virtual_start;
 
-        for (off = 0; off < dev_priv->ring.Size; off += 4) {
+        for (off = 0; off < dev_priv->render_ring.size; off += 4) {
                 ptr = (uint32_t *)(virt + off);
                 seq_printf(m, "%08x :  %08x\n", off, *ptr);
         }
@@ -344,7 +346,7 @@ static int i915_ringbuffer_info(struct seq_file *m, void *data)
 
         seq_printf(m, "RingHead :  %08x\n", head);
         seq_printf(m, "RingTail :  %08x\n", tail);
-        seq_printf(m, "RingSize :  %08lx\n", dev_priv->ring.Size);
+        seq_printf(m, "RingSize :  %08lx\n", dev_priv->render_ring.size);
         seq_printf(m, "Acthd :     %08x\n", I915_READ(IS_I965G(dev) ? ACTHD_I965 : ACTHD));
 
         return 0;
@@ -489,11 +491,14 @@ static int i915_cur_delayinfo(struct seq_file *m, void *unused)
         struct drm_device *dev = node->minor->dev;
         drm_i915_private_t *dev_priv = dev->dev_private;
         u16 rgvswctl = I915_READ16(MEMSWCTL);
+        u16 rgvstat = I915_READ16(MEMSTAT_ILK);
 
-        seq_printf(m, "Last command: 0x%01x\n", (rgvswctl >> 13) & 0x3);
-        seq_printf(m, "Command status: %d\n", (rgvswctl >> 12) & 1);
-        seq_printf(m, "P%d DELAY 0x%02x\n", (rgvswctl >> 8) & 0xf,
-                   rgvswctl & 0x3f);
+        seq_printf(m, "Requested P-state: %d\n", (rgvswctl >> 8) & 0xf);
+        seq_printf(m, "Requested VID: %d\n", rgvswctl & 0x3f);
+        seq_printf(m, "Current VID: %d\n", (rgvstat & MEMSTAT_VID_MASK) >>
+                   MEMSTAT_VID_SHIFT);
+        seq_printf(m, "Current P-state: %d\n",
+                   (rgvstat & MEMSTAT_PSTATE_MASK) >> MEMSTAT_PSTATE_SHIFT);
 
         return 0;
 }
@@ -508,7 +513,8 @@ static int i915_delayfreq_table(struct seq_file *m, void *unused)
 
         for (i = 0; i < 16; i++) {
                 delayfreq = I915_READ(PXVFREQ_BASE + i * 4);
-                seq_printf(m, "P%02dVIDFREQ: 0x%08x\n", i, delayfreq);
+                seq_printf(m, "P%02dVIDFREQ: 0x%08x (VID: %d)\n", i, delayfreq,
+                           (delayfreq & PXVFREQ_PX_MASK) >> PXVFREQ_PX_SHIFT);
         }
 
         return 0;
@@ -541,6 +547,8 @@ static int i915_drpc_info(struct seq_file *m, void *unused)
         struct drm_device *dev = node->minor->dev;
         drm_i915_private_t *dev_priv = dev->dev_private;
         u32 rgvmodectl = I915_READ(MEMMODECTL);
+        u32 rstdbyctl = I915_READ(MCHBAR_RENDER_STANDBY);
+        u16 crstandvid = I915_READ16(CRSTANDVID);
 
         seq_printf(m, "HD boost: %s\n", (rgvmodectl & MEMMODE_BOOST_EN) ?
                    "yes" : "no");
@@ -555,9 +563,13 @@ static int i915_drpc_info(struct seq_file *m, void *unused)
                    rgvmodectl & MEMMODE_RCLK_GATE ? "yes" : "no");
         seq_printf(m, "Starting frequency: P%d\n",
                    (rgvmodectl & MEMMODE_FSTART_MASK) >> MEMMODE_FSTART_SHIFT);
-        seq_printf(m, "Max frequency: P%d\n",
+        seq_printf(m, "Max P-state: P%d\n",
                    (rgvmodectl & MEMMODE_FMAX_MASK) >> MEMMODE_FMAX_SHIFT);
-        seq_printf(m, "Min frequency: P%d\n", (rgvmodectl & MEMMODE_FMIN_MASK));
+        seq_printf(m, "Min P-state: P%d\n", (rgvmodectl & MEMMODE_FMIN_MASK));
+        seq_printf(m, "RS1 VID: %d\n", (crstandvid & 0x3f));
+        seq_printf(m, "RS2 VID: %d\n", ((crstandvid >> 8) & 0x3f));
+        seq_printf(m, "Render standby enabled: %s\n",
+                   (rstdbyctl & RCX_SW_EXIT) ? "no" : "yes");
 
         return 0;
 }
@@ -621,6 +633,36 @@ static int i915_sr_status(struct seq_file *m, void *unused)
         return 0;
 }
 
+static int i915_emon_status(struct seq_file *m, void *unused)
+{
+        struct drm_info_node *node = (struct drm_info_node *) m->private;
+        struct drm_device *dev = node->minor->dev;
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        unsigned long temp, chipset, gfx;
+
+        temp = i915_mch_val(dev_priv);
+        chipset = i915_chipset_val(dev_priv);
+        gfx = i915_gfx_val(dev_priv);
+
+        seq_printf(m, "GMCH temp: %ld\n", temp);
+        seq_printf(m, "Chipset power: %ld\n", chipset);
+        seq_printf(m, "GFX power: %ld\n", gfx);
+        seq_printf(m, "Total power: %ld\n", chipset + gfx);
+
+        return 0;
+}
+
+static int i915_gfxec(struct seq_file *m, void *unused)
+{
+        struct drm_info_node *node = (struct drm_info_node *) m->private;
+        struct drm_device *dev = node->minor->dev;
+        drm_i915_private_t *dev_priv = dev->dev_private;
+
+        seq_printf(m, "GFXEC: %ld\n", (unsigned long)I915_READ(0x112f4));
+
+        return 0;
+}
+
 static int
 i915_wedged_open(struct inode *inode,
                  struct file *filp)
@@ -743,6 +785,8 @@ static struct drm_info_list i915_debugfs_list[] = {
         {"i915_delayfreq_table", i915_delayfreq_table, 0},
         {"i915_inttoext_table", i915_inttoext_table, 0},
         {"i915_drpc_info", i915_drpc_info, 0},
+        {"i915_emon_status", i915_emon_status, 0},
+        {"i915_gfxec", i915_gfxec, 0},
         {"i915_fbc_status", i915_fbc_status, 0},
         {"i915_sr_status", i915_sr_status, 0},
 };
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index 2a6b5de5ae5d..59a2bf8592ec 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -40,84 +40,6 @@
 #include <linux/vga_switcheroo.h>
 #include <linux/slab.h>
 
-/* Really want an OS-independent resettable timer.  Would like to have
- * this loop run for (eg) 3 sec, but have the timer reset every time
- * the head pointer changes, so that EBUSY only happens if the ring
- * actually stalls for (eg) 3 seconds.
- */
-int i915_wait_ring(struct drm_device * dev, int n, const char *caller)
-{
-        drm_i915_private_t *dev_priv = dev->dev_private;
-        drm_i915_ring_buffer_t *ring = &(dev_priv->ring);
-        u32 acthd_reg = IS_I965G(dev) ? ACTHD_I965 : ACTHD;
-        u32 last_acthd = I915_READ(acthd_reg);
-        u32 acthd;
-        u32 last_head = I915_READ(PRB0_HEAD) & HEAD_ADDR;
-        int i;
-
-        trace_i915_ring_wait_begin (dev);
-
-        for (i = 0; i < 100000; i++) {
-                ring->head = I915_READ(PRB0_HEAD) & HEAD_ADDR;
-                acthd = I915_READ(acthd_reg);
-                ring->space = ring->head - (ring->tail + 8);
-                if (ring->space < 0)
-                        ring->space += ring->Size;
-                if (ring->space >= n) {
-                        trace_i915_ring_wait_end (dev);
-                        return 0;
-                }
-
-                if (dev->primary->master) {
-                        struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
-                        if (master_priv->sarea_priv)
-                                master_priv->sarea_priv->perf_boxes |= I915_BOX_WAIT;
-                }
-
-
-                if (ring->head != last_head)
-                        i = 0;
-                if (acthd != last_acthd)
-                        i = 0;
-
-                last_head = ring->head;
-                last_acthd = acthd;
-                msleep_interruptible(10);
-
-        }
-
-        trace_i915_ring_wait_end (dev);
-        return -EBUSY;
-}
-
-/* As a ringbuffer is only allowed to wrap between instructions, fill
- * the tail with NOOPs.
- */
-int i915_wrap_ring(struct drm_device *dev)
-{
-        drm_i915_private_t *dev_priv = dev->dev_private;
-        volatile unsigned int *virt;
-        int rem;
-
-        rem = dev_priv->ring.Size - dev_priv->ring.tail;
-        if (dev_priv->ring.space < rem) {
-                int ret = i915_wait_ring(dev, rem, __func__);
-                if (ret)
-                        return ret;
-        }
-        dev_priv->ring.space -= rem;
-
-        virt = (unsigned int *)
-                (dev_priv->ring.virtual_start + dev_priv->ring.tail);
-        rem /= 4;
-        while (rem--)
-                *virt++ = MI_NOOP;
-
-        dev_priv->ring.tail = 0;
-
-        return 0;
-}
-
 /**
  * Sets up the hardware status page for devices that need a physical address
  * in the register.
@@ -133,10 +55,11 @@ static int i915_init_phys_hws(struct drm_device *dev)
                 DRM_ERROR("Can not allocate hardware status page\n");
                 return -ENOMEM;
         }
-        dev_priv->hw_status_page = dev_priv->status_page_dmah->vaddr;
+        dev_priv->render_ring.status_page.page_addr
+                = dev_priv->status_page_dmah->vaddr;
         dev_priv->dma_status_page = dev_priv->status_page_dmah->busaddr;
 
-        memset(dev_priv->hw_status_page, 0, PAGE_SIZE);
+        memset(dev_priv->render_ring.status_page.page_addr, 0, PAGE_SIZE);
 
         if (IS_I965G(dev))
                 dev_priv->dma_status_page |= (dev_priv->dma_status_page >> 28) &
@@ -159,8 +82,8 @@ static void i915_free_hws(struct drm_device *dev)
                 dev_priv->status_page_dmah = NULL;
         }
 
-        if (dev_priv->status_gfx_addr) {
-                dev_priv->status_gfx_addr = 0;
+        if (dev_priv->render_ring.status_page.gfx_addr) {
+                dev_priv->render_ring.status_page.gfx_addr = 0;
                 drm_core_ioremapfree(&dev_priv->hws_map, dev);
         }
 
@@ -172,7 +95,7 @@ void i915_kernel_lost_context(struct drm_device * dev)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
         struct drm_i915_master_private *master_priv;
-        drm_i915_ring_buffer_t *ring = &(dev_priv->ring);
+        struct intel_ring_buffer *ring = &dev_priv->render_ring;
 
         /*
          * We should never lose context on the ring with modesetting
@@ -185,7 +108,7 @@ void i915_kernel_lost_context(struct drm_device * dev)
         ring->tail = I915_READ(PRB0_TAIL) & TAIL_ADDR;
         ring->space = ring->head - (ring->tail + 8);
         if (ring->space < 0)
-                ring->space += ring->Size;
+                ring->space += ring->size;
 
         if (!dev->primary->master)
                 return;
@@ -205,12 +128,9 @@ static int i915_dma_cleanup(struct drm_device * dev)
         if (dev->irq_enabled)
                 drm_irq_uninstall(dev);
 
-        if (dev_priv->ring.virtual_start) {
-                drm_core_ioremapfree(&dev_priv->ring.map, dev);
-                dev_priv->ring.virtual_start = NULL;
-                dev_priv->ring.map.handle = NULL;
-                dev_priv->ring.map.size = 0;
-        }
+        intel_cleanup_ring_buffer(dev, &dev_priv->render_ring);
+        if (HAS_BSD(dev))
+                intel_cleanup_ring_buffer(dev, &dev_priv->bsd_ring);
 
         /* Clear the HWS virtual address at teardown */
         if (I915_NEED_GFX_HWS(dev))
@@ -233,24 +153,24 @@ static int i915_initialize(struct drm_device * dev, drm_i915_init_t * init)
         }
 
         if (init->ring_size != 0) {
-                if (dev_priv->ring.ring_obj != NULL) {
+                if (dev_priv->render_ring.gem_object != NULL) {
                         i915_dma_cleanup(dev);
                         DRM_ERROR("Client tried to initialize ringbuffer in "
                                   "GEM mode\n");
                         return -EINVAL;
                 }
 
-                dev_priv->ring.Size = init->ring_size;
+                dev_priv->render_ring.size = init->ring_size;
 
-                dev_priv->ring.map.offset = init->ring_start;
-                dev_priv->ring.map.size = init->ring_size;
-                dev_priv->ring.map.type = 0;
-                dev_priv->ring.map.flags = 0;
-                dev_priv->ring.map.mtrr = 0;
+                dev_priv->render_ring.map.offset = init->ring_start;
+                dev_priv->render_ring.map.size = init->ring_size;
+                dev_priv->render_ring.map.type = 0;
+                dev_priv->render_ring.map.flags = 0;
+                dev_priv->render_ring.map.mtrr = 0;
 
-                drm_core_ioremap_wc(&dev_priv->ring.map, dev);
+                drm_core_ioremap_wc(&dev_priv->render_ring.map, dev);
 
-                if (dev_priv->ring.map.handle == NULL) {
+                if (dev_priv->render_ring.map.handle == NULL) {
                         i915_dma_cleanup(dev);
                         DRM_ERROR("can not ioremap virtual address for"
                                   " ring buffer\n");
@@ -258,7 +178,7 @@ static int i915_initialize(struct drm_device * dev, drm_i915_init_t * init)
                 }
         }
 
-        dev_priv->ring.virtual_start = dev_priv->ring.map.handle;
+        dev_priv->render_ring.virtual_start = dev_priv->render_ring.map.handle;
 
         dev_priv->cpp = init->cpp;
         dev_priv->back_offset = init->back_offset;
@@ -278,26 +198,29 @@ static int i915_dma_resume(struct drm_device * dev)
 {
         drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
 
+        struct intel_ring_buffer *ring;
         DRM_DEBUG_DRIVER("%s\n", __func__);
 
-        if (dev_priv->ring.map.handle == NULL) {
+        ring = &dev_priv->render_ring;
+
+        if (ring->map.handle == NULL) {
                 DRM_ERROR("can not ioremap virtual address for"
                           " ring buffer\n");
                 return -ENOMEM;
         }
 
         /* Program Hardware Status Page */
-        if (!dev_priv->hw_status_page) {
+        if (!ring->status_page.page_addr) {
                 DRM_ERROR("Can not find hardware status page\n");
                 return -EINVAL;
         }
         DRM_DEBUG_DRIVER("hw status page @ %p\n",
-                                dev_priv->hw_status_page);
-
-        if (dev_priv->status_gfx_addr != 0)
-                I915_WRITE(HWS_PGA, dev_priv->status_gfx_addr);
+                                ring->status_page.page_addr);
+        if (ring->status_page.gfx_addr != 0)
+                ring->setup_status_page(dev, ring);
         else
                 I915_WRITE(HWS_PGA, dev_priv->dma_status_page);
+
         DRM_DEBUG_DRIVER("Enabled hardware status page\n");
 
         return 0;
@@ -407,9 +330,8 @@ static int i915_emit_cmds(struct drm_device * dev, int *buffer, int dwords)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
         int i;
-        RING_LOCALS;
 
-        if ((dwords+1) * sizeof(int) >= dev_priv->ring.Size - 8)
+        if ((dwords+1) * sizeof(int) >= dev_priv->render_ring.size - 8)
                 return -EINVAL;
 
         BEGIN_LP_RING((dwords+1)&~1);
@@ -442,9 +364,7 @@ i915_emit_box(struct drm_device *dev,
               struct drm_clip_rect *boxes,
               int i, int DR1, int DR4)
 {
-        drm_i915_private_t *dev_priv = dev->dev_private;
         struct drm_clip_rect box = boxes[i];
-        RING_LOCALS;
 
         if (box.y2 <= box.y1 || box.x2 <= box.x1 || box.y2 <= 0 || box.x2 <= 0) {
                 DRM_ERROR("Bad box %d,%d..%d,%d\n",
@@ -481,7 +401,6 @@ static void i915_emit_breadcrumb(struct drm_device *dev)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
         struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
-        RING_LOCALS;
 
         dev_priv->counter++;
         if (dev_priv->counter > 0x7FFFFFFFUL)
@@ -535,10 +454,8 @@ static int i915_dispatch_batchbuffer(struct drm_device * dev,
                                      drm_i915_batchbuffer_t * batch,
                                      struct drm_clip_rect *cliprects)
 {
-        drm_i915_private_t *dev_priv = dev->dev_private;
         int nbox = batch->num_cliprects;
         int i = 0, count;
-        RING_LOCALS;
 
         if ((batch->start | batch->used) & 0x7) {
                 DRM_ERROR("alignment");
@@ -587,7 +504,6 @@ static int i915_dispatch_flip(struct drm_device * dev)
         drm_i915_private_t *dev_priv = dev->dev_private;
         struct drm_i915_master_private *master_priv =
                 dev->primary->master->driver_priv;
-        RING_LOCALS;
 
         if (!master_priv->sarea_priv)
                 return -EINVAL;
@@ -640,7 +556,8 @@ static int i915_quiescent(struct drm_device * dev)
         drm_i915_private_t *dev_priv = dev->dev_private;
 
         i915_kernel_lost_context(dev);
-        return i915_wait_ring(dev, dev_priv->ring.Size - 8, __func__);
+        return intel_wait_ring_buffer(dev, &dev_priv->render_ring,
+                                      dev_priv->render_ring.size - 8);
 }
 
 static int i915_flush_ioctl(struct drm_device *dev, void *data,
@@ -827,6 +744,9 @@ static int i915_getparam(struct drm_device *dev, void *data,
                 /* depends on GEM */
                 value = dev_priv->has_gem;
                 break;
+        case I915_PARAM_HAS_BSD:
+                value = HAS_BSD(dev);
+                break;
         default:
                 DRM_DEBUG_DRIVER("Unknown parameter %d\n",
                                  param->param);
@@ -882,6 +802,7 @@ static int i915_set_status_page(struct drm_device *dev, void *data,
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
         drm_i915_hws_addr_t *hws = data;
+        struct intel_ring_buffer *ring = &dev_priv->render_ring;
 
         if (!I915_NEED_GFX_HWS(dev))
                 return -EINVAL;
@@ -898,7 +819,7 @@ static int i915_set_status_page(struct drm_device *dev, void *data,
 
         DRM_DEBUG_DRIVER("set status page addr 0x%08x\n", (u32)hws->addr);
 
-        dev_priv->status_gfx_addr = hws->addr & (0x1ffff<<12);
+        ring->status_page.gfx_addr = hws->addr & (0x1ffff<<12);
 
         dev_priv->hws_map.offset = dev->agp->base + hws->addr;
         dev_priv->hws_map.size = 4*1024;
@@ -909,19 +830,19 @@ static int i915_set_status_page(struct drm_device *dev, void *data,
         drm_core_ioremap_wc(&dev_priv->hws_map, dev);
         if (dev_priv->hws_map.handle == NULL) {
                 i915_dma_cleanup(dev);
-                dev_priv->status_gfx_addr = 0;
+                ring->status_page.gfx_addr = 0;
                 DRM_ERROR("can not ioremap virtual address for"
                                 " G33 hw status page\n");
                 return -ENOMEM;
         }
-        dev_priv->hw_status_page = dev_priv->hws_map.handle;
+        ring->status_page.page_addr = dev_priv->hws_map.handle;
+        memset(ring->status_page.page_addr, 0, PAGE_SIZE);
+        I915_WRITE(HWS_PGA, ring->status_page.gfx_addr);
 
-        memset(dev_priv->hw_status_page, 0, PAGE_SIZE);
-        I915_WRITE(HWS_PGA, dev_priv->status_gfx_addr);
         DRM_DEBUG_DRIVER("load hws HWS_PGA with gfx mem 0x%x\n",
-                                dev_priv->status_gfx_addr);
+                         ring->status_page.gfx_addr);
         DRM_DEBUG_DRIVER("load hws at %p\n",
-                                dev_priv->hw_status_page);
+                         ring->status_page.page_addr);
         return 0;
 }
 
@@ -1399,12 +1320,14 @@ static void i915_switcheroo_set_state(struct pci_dev *pdev, enum vga_switcheroo_
         struct drm_device *dev = pci_get_drvdata(pdev);
         pm_message_t pmm = { .event = PM_EVENT_SUSPEND };
         if (state == VGA_SWITCHEROO_ON) {
-                printk(KERN_INFO "i915: switched off\n");
+                printk(KERN_INFO "i915: switched on\n");
                 /* i915 resume handler doesn't set to D0 */
                 pci_set_power_state(dev->pdev, PCI_D0);
                 i915_resume(dev);
+                drm_kms_helper_poll_enable(dev);
         } else {
                 printk(KERN_ERR "i915: switched off\n");
+                drm_kms_helper_poll_disable(dev);
                 i915_suspend(dev, pmm);
         }
 }
@@ -1479,19 +1402,19 @@ static int i915_load_modeset_init(struct drm_device *dev,
         /* if we have > 1 VGA cards, then disable the radeon VGA resources */
         ret = vga_client_register(dev->pdev, dev, NULL, i915_vga_set_decode);
         if (ret)
-                goto destroy_ringbuffer;
+                goto cleanup_ringbuffer;
 
         ret = vga_switcheroo_register_client(dev->pdev,
                                              i915_switcheroo_set_state,
                                              i915_switcheroo_can_switch);
         if (ret)
-                goto destroy_ringbuffer;
+                goto cleanup_vga_client;
 
         intel_modeset_init(dev);
 
         ret = drm_irq_install(dev);
         if (ret)
-                goto destroy_ringbuffer;
+                goto cleanup_vga_switcheroo;
 
         /* Always safe in the mode setting case. */
         /* FIXME: do pre/post-mode set stuff in core KMS code */
@@ -1503,11 +1426,20 @@ static int i915_load_modeset_init(struct drm_device *dev,
 
         I915_WRITE(INSTPM, (1 << 5) | (1 << 21));
 
-        intel_fbdev_init(dev);
+        ret = intel_fbdev_init(dev);
+        if (ret)
+                goto cleanup_irq;
+
         drm_kms_helper_poll_init(dev);
         return 0;
 
-destroy_ringbuffer:
+cleanup_irq:
+        drm_irq_uninstall(dev);
+cleanup_vga_switcheroo:
+        vga_switcheroo_unregister_client(dev->pdev);
+cleanup_vga_client:
+        vga_client_register(dev->pdev, NULL, NULL, NULL);
+cleanup_ringbuffer:
         mutex_lock(&dev->struct_mutex);
         i915_gem_cleanup_ringbuffer(dev);
         mutex_unlock(&dev->struct_mutex);
@@ -1539,14 +1471,11 @@ void i915_master_destroy(struct drm_device *dev, struct drm_master *master)
         master->driver_priv = NULL;
 }
 
-static void i915_get_mem_freq(struct drm_device *dev)
+static void i915_pineview_get_mem_freq(struct drm_device *dev)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
         u32 tmp;
 
-        if (!IS_PINEVIEW(dev))
-                return;
-
         tmp = I915_READ(CLKCFG);
 
         switch (tmp & CLKCFG_FSB_MASK) {
@@ -1575,7 +1504,524 @@ static void i915_get_mem_freq(struct drm_device *dev)
                 dev_priv->mem_freq = 800;
                 break;
         }
+
+        /* detect pineview DDR3 setting */
+        tmp = I915_READ(CSHRDDR3CTL);
+        dev_priv->is_ddr3 = (tmp & CSHRDDR3CTL_DDR3) ? 1 : 0;
+}
+
+static void i915_ironlake_get_mem_freq(struct drm_device *dev)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        u16 ddrpll, csipll;
+
+        ddrpll = I915_READ16(DDRMPLL1);
+        csipll = I915_READ16(CSIPLL0);
+
+        switch (ddrpll & 0xff) {
+        case 0xc:
+                dev_priv->mem_freq = 800;
+                break;
+        case 0x10:
+                dev_priv->mem_freq = 1066;
+                break;
+        case 0x14:
+                dev_priv->mem_freq = 1333;
+                break;
+        case 0x18:
+                dev_priv->mem_freq = 1600;
+                break;
+        default:
+                DRM_DEBUG_DRIVER("unknown memory frequency 0x%02x\n",
+                                 ddrpll & 0xff);
+                dev_priv->mem_freq = 0;
+                break;
+        }
+
+        dev_priv->r_t = dev_priv->mem_freq;
+
+        switch (csipll & 0x3ff) {
+        case 0x00c:
+                dev_priv->fsb_freq = 3200;
+                break;
+        case 0x00e:
+                dev_priv->fsb_freq = 3733;
+                break;
+        case 0x010:
+                dev_priv->fsb_freq = 4266;
+                break;
+        case 0x012:
+                dev_priv->fsb_freq = 4800;
+                break;
+        case 0x014:
+                dev_priv->fsb_freq = 5333;
+                break;
+        case 0x016:
+                dev_priv->fsb_freq = 5866;
+                break;
+        case 0x018:
+                dev_priv->fsb_freq = 6400;
+                break;
+        default:
+                DRM_DEBUG_DRIVER("unknown fsb frequency 0x%04x\n",
+                                 csipll & 0x3ff);
+                dev_priv->fsb_freq = 0;
+                break;
+        }
+
+        if (dev_priv->fsb_freq == 3200) {
+                dev_priv->c_m = 0;
+        } else if (dev_priv->fsb_freq > 3200 && dev_priv->fsb_freq <= 4800) {
+                dev_priv->c_m = 1;
+        } else {
+                dev_priv->c_m = 2;
+        }
+}
+
+struct v_table {
+        u8 vid;
+        unsigned long vd; /* in .1 mil */
+        unsigned long vm; /* in .1 mil */
+        u8 pvid;
+};
+
+static struct v_table v_table[] = {
+        { 0, 16125, 15000, 0x7f, },
+        { 1, 16000, 14875, 0x7e, },
+        { 2, 15875, 14750, 0x7d, },
+        { 3, 15750, 14625, 0x7c, },
+        { 4, 15625, 14500, 0x7b, },
+        { 5, 15500, 14375, 0x7a, },
+        { 6, 15375, 14250, 0x79, },
+        { 7, 15250, 14125, 0x78, },
+        { 8, 15125, 14000, 0x77, },
+        { 9, 15000, 13875, 0x76, },
+        { 10, 14875, 13750, 0x75, },
+        { 11, 14750, 13625, 0x74, },
+        { 12, 14625, 13500, 0x73, },
+        { 13, 14500, 13375, 0x72, },
+        { 14, 14375, 13250, 0x71, },
+        { 15, 14250, 13125, 0x70, },
+        { 16, 14125, 13000, 0x6f, },
+        { 17, 14000, 12875, 0x6e, },
+        { 18, 13875, 12750, 0x6d, },
+        { 19, 13750, 12625, 0x6c, },
+        { 20, 13625, 12500, 0x6b, },
+        { 21, 13500, 12375, 0x6a, },
+        { 22, 13375, 12250, 0x69, },
+        { 23, 13250, 12125, 0x68, },
+        { 24, 13125, 12000, 0x67, },
+        { 25, 13000, 11875, 0x66, },
+        { 26, 12875, 11750, 0x65, },
+        { 27, 12750, 11625, 0x64, },
+        { 28, 12625, 11500, 0x63, },
+        { 29, 12500, 11375, 0x62, },
+        { 30, 12375, 11250, 0x61, },
+        { 31, 12250, 11125, 0x60, },
+        { 32, 12125, 11000, 0x5f, },
+        { 33, 12000, 10875, 0x5e, },
+        { 34, 11875, 10750, 0x5d, },
+        { 35, 11750, 10625, 0x5c, },
+        { 36, 11625, 10500, 0x5b, },
+        { 37, 11500, 10375, 0x5a, },
+        { 38, 11375, 10250, 0x59, },
+        { 39, 11250, 10125, 0x58, },
+        { 40, 11125, 10000, 0x57, },
+        { 41, 11000, 9875, 0x56, },
+        { 42, 10875, 9750, 0x55, },
+        { 43, 10750, 9625, 0x54, },
+        { 44, 10625, 9500, 0x53, },
+        { 45, 10500, 9375, 0x52, },
+        { 46, 10375, 9250, 0x51, },
+        { 47, 10250, 9125, 0x50, },
+        { 48, 10125, 9000, 0x4f, },
+        { 49, 10000, 8875, 0x4e, },
+        { 50, 9875, 8750, 0x4d, },
+        { 51, 9750, 8625, 0x4c, },
+        { 52, 9625, 8500, 0x4b, },
+        { 53, 9500, 8375, 0x4a, },
+        { 54, 9375, 8250, 0x49, },
+        { 55, 9250, 8125, 0x48, },
+        { 56, 9125, 8000, 0x47, },
+        { 57, 9000, 7875, 0x46, },
+        { 58, 8875, 7750, 0x45, },
+        { 59, 8750, 7625, 0x44, },
+        { 60, 8625, 7500, 0x43, },
+        { 61, 8500, 7375, 0x42, },
+        { 62, 8375, 7250, 0x41, },
+        { 63, 8250, 7125, 0x40, },
+        { 64, 8125, 7000, 0x3f, },
+        { 65, 8000, 6875, 0x3e, },
+        { 66, 7875, 6750, 0x3d, },
+        { 67, 7750, 6625, 0x3c, },
+        { 68, 7625, 6500, 0x3b, },
+        { 69, 7500, 6375, 0x3a, },
+        { 70, 7375, 6250, 0x39, },
+        { 71, 7250, 6125, 0x38, },
+        { 72, 7125, 6000, 0x37, },
+        { 73, 7000, 5875, 0x36, },
+        { 74, 6875, 5750, 0x35, },
+        { 75, 6750, 5625, 0x34, },
+        { 76, 6625, 5500, 0x33, },
+        { 77, 6500, 5375, 0x32, },
+        { 78, 6375, 5250, 0x31, },
+        { 79, 6250, 5125, 0x30, },
+        { 80, 6125, 5000, 0x2f, },
+        { 81, 6000, 4875, 0x2e, },
+        { 82, 5875, 4750, 0x2d, },
+        { 83, 5750, 4625, 0x2c, },
+        { 84, 5625, 4500, 0x2b, },
+        { 85, 5500, 4375, 0x2a, },
+        { 86, 5375, 4250, 0x29, },
+        { 87, 5250, 4125, 0x28, },
+        { 88, 5125, 4000, 0x27, },
+        { 89, 5000, 3875, 0x26, },
+        { 90, 4875, 3750, 0x25, },
+        { 91, 4750, 3625, 0x24, },
+        { 92, 4625, 3500, 0x23, },
+        { 93, 4500, 3375, 0x22, },
+        { 94, 4375, 3250, 0x21, },
+        { 95, 4250, 3125, 0x20, },
+        { 96, 4125, 3000, 0x1f, },
+        { 97, 4125, 3000, 0x1e, },
+        { 98, 4125, 3000, 0x1d, },
+        { 99, 4125, 3000, 0x1c, },
+        { 100, 4125, 3000, 0x1b, },
+        { 101, 4125, 3000, 0x1a, },
+        { 102, 4125, 3000, 0x19, },
+        { 103, 4125, 3000, 0x18, },
+        { 104, 4125, 3000, 0x17, },
+        { 105, 4125, 3000, 0x16, },
+        { 106, 4125, 3000, 0x15, },
+        { 107, 4125, 3000, 0x14, },
+        { 108, 4125, 3000, 0x13, },
+        { 109, 4125, 3000, 0x12, },
+        { 110, 4125, 3000, 0x11, },
+        { 111, 4125, 3000, 0x10, },
+        { 112, 4125, 3000, 0x0f, },
+        { 113, 4125, 3000, 0x0e, },
+        { 114, 4125, 3000, 0x0d, },
+        { 115, 4125, 3000, 0x0c, },
+        { 116, 4125, 3000, 0x0b, },
+        { 117, 4125, 3000, 0x0a, },
+        { 118, 4125, 3000, 0x09, },
+        { 119, 4125, 3000, 0x08, },
+        { 120, 1125, 0, 0x07, },
+        { 121, 1000, 0, 0x06, },
+        { 122, 875, 0, 0x05, },
+        { 123, 750, 0, 0x04, },
+        { 124, 625, 0, 0x03, },
+        { 125, 500, 0, 0x02, },
+        { 126, 375, 0, 0x01, },
+        { 127, 0, 0, 0x00, },
+};
+
+struct cparams {
+        int i;
+        int t;
+        int m;
+        int c;
+};
+
+static struct cparams cparams[] = {
+        { 1, 1333, 301, 28664 },
+        { 1, 1066, 294, 24460 },
+        { 1, 800, 294, 25192 },
+        { 0, 1333, 276, 27605 },
+        { 0, 1066, 276, 27605 },
+        { 0, 800, 231, 23784 },
+};
+
+unsigned long i915_chipset_val(struct drm_i915_private *dev_priv)
+{
+        u64 total_count, diff, ret;
+        u32 count1, count2, count3, m = 0, c = 0;
+        unsigned long now = jiffies_to_msecs(jiffies), diff1;
+        int i;
+
+        diff1 = now - dev_priv->last_time1;
+
+        count1 = I915_READ(DMIEC);
+        count2 = I915_READ(DDREC);
+        count3 = I915_READ(CSIEC);
+
+        total_count = count1 + count2 + count3;
+
+        /* FIXME: handle per-counter overflow */
+        if (total_count < dev_priv->last_count1) {
+                diff = ~0UL - dev_priv->last_count1;
+                diff += total_count;
+        } else {
+                diff = total_count - dev_priv->last_count1;
+        }
+
+        for (i = 0; i < ARRAY_SIZE(cparams); i++) {
+                if (cparams[i].i == dev_priv->c_m &&
+                    cparams[i].t == dev_priv->r_t) {
+                        m = cparams[i].m;
+                        c = cparams[i].c;
+                        break;
+                }
+        }
+
+        div_u64(diff, diff1);
+        ret = ((m * diff) + c);
+        div_u64(ret, 10);
+
+        dev_priv->last_count1 = total_count;
+        dev_priv->last_time1 = now;
+
+        return ret;
+}
+
+unsigned long i915_mch_val(struct drm_i915_private *dev_priv)
+{
+        unsigned long m, x, b;
+        u32 tsfs;
+
+        tsfs = I915_READ(TSFS);
+
+        m = ((tsfs & TSFS_SLOPE_MASK) >> TSFS_SLOPE_SHIFT);
+        x = I915_READ8(TR1);
+
+        b = tsfs & TSFS_INTR_MASK;
+
+        return ((m * x) / 127) - b;
+}
+
+static unsigned long pvid_to_extvid(struct drm_i915_private *dev_priv, u8 pxvid)
+{
+        unsigned long val = 0;
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(v_table); i++) {
+                if (v_table[i].pvid == pxvid) {
+                        if (IS_MOBILE(dev_priv->dev))
+                                val = v_table[i].vm;
+                        else
+                                val = v_table[i].vd;
+                }
+        }
+
+        return val;
+}
+
+void i915_update_gfx_val(struct drm_i915_private *dev_priv)
+{
+        struct timespec now, diff1;
+        u64 diff;
+        unsigned long diffms;
+        u32 count;
+
+        getrawmonotonic(&now);
+        diff1 = timespec_sub(now, dev_priv->last_time2);
+
+        /* Don't divide by 0 */
+        diffms = diff1.tv_sec * 1000 + diff1.tv_nsec / 1000000;
+        if (!diffms)
+                return;
+
+        count = I915_READ(GFXEC);
+
+        if (count < dev_priv->last_count2) {
+                diff = ~0UL - dev_priv->last_count2;
+                diff += count;
+        } else {
+                diff = count - dev_priv->last_count2;
+        }
+
+        dev_priv->last_count2 = count;
+        dev_priv->last_time2 = now;
+
+        /* More magic constants... */
+        diff = diff * 1181;
+        div_u64(diff, diffms * 10);
+        dev_priv->gfx_power = diff;
+}
+
+unsigned long i915_gfx_val(struct drm_i915_private *dev_priv)
+{
+        unsigned long t, corr, state1, corr2, state2;
+        u32 pxvid, ext_v;
+
+        pxvid = I915_READ(PXVFREQ_BASE + (dev_priv->cur_delay * 4));
+        pxvid = (pxvid >> 24) & 0x7f;
+        ext_v = pvid_to_extvid(dev_priv, pxvid);
+
+        state1 = ext_v;
+
+        t = i915_mch_val(dev_priv);
+
+        /* Revel in the empirically derived constants */
+
+        /* Correction factor in 1/100000 units */
+        if (t > 80)
+                corr = ((t * 2349) + 135940);
+        else if (t >= 50)
+                corr = ((t * 964) + 29317);
+        else /* < 50 */
+                corr = ((t * 301) + 1004);
+
+        corr = corr * ((150142 * state1) / 10000 - 78642);
+        corr /= 100000;
+        corr2 = (corr * dev_priv->corr);
+
+        state2 = (corr2 * state1) / 10000;
+        state2 /= 100; /* convert to mW */
+
+        i915_update_gfx_val(dev_priv);
+
+        return dev_priv->gfx_power + state2;
+}
+
+/* Global for IPS driver to get at the current i915 device */
+static struct drm_i915_private *i915_mch_dev;
+/*
+ * Lock protecting IPS related data structures
+ *   - i915_mch_dev
+ *   - dev_priv->max_delay
+ *   - dev_priv->min_delay
+ *   - dev_priv->fmax
+ *   - dev_priv->gpu_busy
+ */
+DEFINE_SPINLOCK(mchdev_lock);
+
+/**
+ * i915_read_mch_val - return value for IPS use
+ *
+ * Calculate and return a value for the IPS driver to use when deciding whether
+ * we have thermal and power headroom to increase CPU or GPU power budget.
+ */
+unsigned long i915_read_mch_val(void)
+{
+        struct drm_i915_private *dev_priv;
+        unsigned long chipset_val, graphics_val, ret = 0;
+
+        spin_lock(&mchdev_lock);
+        if (!i915_mch_dev)
+                goto out_unlock;
+        dev_priv = i915_mch_dev;
+
+        chipset_val = i915_chipset_val(dev_priv);
+        graphics_val = i915_gfx_val(dev_priv);
+
+        ret = chipset_val + graphics_val;
+
+out_unlock:
+        spin_unlock(&mchdev_lock);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(i915_read_mch_val);
+
+/**
+ * i915_gpu_raise - raise GPU frequency limit
+ *
+ * Raise the limit; IPS indicates we have thermal headroom.
+ */
+bool i915_gpu_raise(void)
+{
+        struct drm_i915_private *dev_priv;
+        bool ret = true;
+
+        spin_lock(&mchdev_lock);
+        if (!i915_mch_dev) {
+                ret = false;
+                goto out_unlock;
+        }
+        dev_priv = i915_mch_dev;
+
+        if (dev_priv->max_delay > dev_priv->fmax)
+                dev_priv->max_delay--;
+
+out_unlock:
+        spin_unlock(&mchdev_lock);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(i915_gpu_raise);
+
+/**
+ * i915_gpu_lower - lower GPU frequency limit
+ *
+ * IPS indicates we're close to a thermal limit, so throttle back the GPU
+ * frequency maximum.
+ */
+bool i915_gpu_lower(void)
+{
+        struct drm_i915_private *dev_priv;
+        bool ret = true;
+
+        spin_lock(&mchdev_lock);
+        if (!i915_mch_dev) {
+                ret = false;
+                goto out_unlock;
+        }
+        dev_priv = i915_mch_dev;
+
+        if (dev_priv->max_delay < dev_priv->min_delay)
+                dev_priv->max_delay++;
+
+out_unlock:
+        spin_unlock(&mchdev_lock);
+
+        return ret;
 }
+EXPORT_SYMBOL_GPL(i915_gpu_lower);
+
+/**
+ * i915_gpu_busy - indicate GPU business to IPS
+ *
+ * Tell the IPS driver whether or not the GPU is busy.
+ */
+bool i915_gpu_busy(void)
+{
+        struct drm_i915_private *dev_priv;
+        bool ret = false;
+
+        spin_lock(&mchdev_lock);
+        if (!i915_mch_dev)
+                goto out_unlock;
+        dev_priv = i915_mch_dev;
+
+        ret = dev_priv->busy;
+
+out_unlock:
+        spin_unlock(&mchdev_lock);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(i915_gpu_busy);
+
+/**
+ * i915_gpu_turbo_disable - disable graphics turbo
+ *
+ * Disable graphics turbo by resetting the max frequency and setting the
+ * current frequency to the default.
+ */
+bool i915_gpu_turbo_disable(void)
+{
+        struct drm_i915_private *dev_priv;
+        bool ret = true;
+
+        spin_lock(&mchdev_lock);
+        if (!i915_mch_dev) {
+                ret = false;
+                goto out_unlock;
+        }
+        dev_priv = i915_mch_dev;
+
+        dev_priv->max_delay = dev_priv->fstart;
+
+        if (!ironlake_set_drps(dev_priv->dev, dev_priv->fstart))
+                ret = false;
+
+out_unlock:
+        spin_unlock(&mchdev_lock);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(i915_gpu_turbo_disable);
 
 /**
  * i915_driver_load - setup chip and create an initial config
@@ -1594,7 +2040,6 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
         resource_size_t base, size;
         int ret = 0, mmio_bar;
         uint32_t agp_size, prealloc_size, prealloc_start;
-
         /* i915 has 4 more counters */
         dev->counters += 4;
         dev->types[6] = _DRM_STAT_IRQ;
@@ -1672,6 +2117,13 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
                 dev_priv->has_gem = 0;
         }
 
+        if (dev_priv->has_gem == 0 &&
+            drm_core_check_feature(dev, DRIVER_MODESET)) {
+                DRM_ERROR("kernel modesetting requires GEM, disabling driver.\n");
+                ret = -ENODEV;
+                goto out_iomapfree;
+        }
+
         dev->driver->get_vblank_counter = i915_get_vblank_counter;
         dev->max_vblank_count = 0xffffff; /* only 24 bits of frame count */
         if (IS_G4X(dev) || IS_IRONLAKE(dev) || IS_GEN6(dev)) {
@@ -1691,7 +2143,10 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
                         goto out_workqueue_free;
         }
 
-        i915_get_mem_freq(dev);
+        if (IS_PINEVIEW(dev))
+                i915_pineview_get_mem_freq(dev);
+        else if (IS_IRONLAKE(dev))
+                i915_ironlake_get_mem_freq(dev);
 
         /* On the 945G/GM, the chipset reports the MSI capability on the
          * integrated graphics even though the support isn't actually there
@@ -1709,7 +2164,6 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 
         spin_lock_init(&dev_priv->user_irq_lock);
         spin_lock_init(&dev_priv->error_lock);
-        dev_priv->user_irq_refcount = 0;
         dev_priv->trace_irq_seqno = 0;
 
         ret = drm_vblank_init(dev, I915_NUM_PIPE);
@@ -1738,6 +2192,12 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 
         setup_timer(&dev_priv->hangcheck_timer, i915_hangcheck_elapsed,
                     (unsigned long) dev);
+
+        spin_lock(&mchdev_lock);
+        i915_mch_dev = dev_priv;
+        dev_priv->mchdev_lock = &mchdev_lock;
+        spin_unlock(&mchdev_lock);
+
         return 0;
 
 out_workqueue_free:
@@ -1759,6 +2219,10 @@ int i915_driver_unload(struct drm_device *dev)
 
         i915_destroy_error_state(dev);
 
+        spin_lock(&mchdev_lock);
+        i915_mch_dev = NULL;
+        spin_unlock(&mchdev_lock);
+
         destroy_workqueue(dev_priv->wq);
         del_timer_sync(&dev_priv->hangcheck_timer);
 
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index 5c51e45ab68d..423dc90c1e20 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -60,95 +60,95 @@ extern int intel_agp_enabled;
         .subdevice = PCI_ANY_ID,                \
         .driver_data = (unsigned long) info }
 
-const static struct intel_device_info intel_i830_info = {
+static const struct intel_device_info intel_i830_info = {
         .is_i8xx = 1, .is_mobile = 1, .cursor_needs_physical = 1,
 };
 
-const static struct intel_device_info intel_845g_info = {
+static const struct intel_device_info intel_845g_info = {
         .is_i8xx = 1,
 };
 
-const static struct intel_device_info intel_i85x_info = {
+static const struct intel_device_info intel_i85x_info = {
         .is_i8xx = 1, .is_i85x = 1, .is_mobile = 1,
         .cursor_needs_physical = 1,
 };
 
-const static struct intel_device_info intel_i865g_info = {
+static const struct intel_device_info intel_i865g_info = {
         .is_i8xx = 1,
 };
 
-const static struct intel_device_info intel_i915g_info = {
+static const struct intel_device_info intel_i915g_info = {
         .is_i915g = 1, .is_i9xx = 1, .cursor_needs_physical = 1,
 };
-const static struct intel_device_info intel_i915gm_info = {
+static const struct intel_device_info intel_i915gm_info = {
         .is_i9xx = 1,  .is_mobile = 1,
         .cursor_needs_physical = 1,
 };
-const static struct intel_device_info intel_i945g_info = {
+static const struct intel_device_info intel_i945g_info = {
         .is_i9xx = 1, .has_hotplug = 1, .cursor_needs_physical = 1,
 };
-const static struct intel_device_info intel_i945gm_info = {
+static const struct intel_device_info intel_i945gm_info = {
         .is_i945gm = 1, .is_i9xx = 1, .is_mobile = 1,
         .has_hotplug = 1, .cursor_needs_physical = 1,
 };
 
-const static struct intel_device_info intel_i965g_info = {
+static const struct intel_device_info intel_i965g_info = {
         .is_i965g = 1, .is_i9xx = 1, .has_hotplug = 1,
 };
 
-const static struct intel_device_info intel_i965gm_info = {
+static const struct intel_device_info intel_i965gm_info = {
         .is_i965g = 1, .is_mobile = 1, .is_i965gm = 1, .is_i9xx = 1,
         .is_mobile = 1, .has_fbc = 1, .has_rc6 = 1,
         .has_hotplug = 1,
 };
 
-const static struct intel_device_info intel_g33_info = {
+static const struct intel_device_info intel_g33_info = {
         .is_g33 = 1, .is_i9xx = 1, .need_gfx_hws = 1,
         .has_hotplug = 1,
 };
 
-const static struct intel_device_info intel_g45_info = {
+static const struct intel_device_info intel_g45_info = {
         .is_i965g = 1, .is_g4x = 1, .is_i9xx = 1, .need_gfx_hws = 1,
         .has_pipe_cxsr = 1,
         .has_hotplug = 1,
 };
 
-const static struct intel_device_info intel_gm45_info = {
+static const struct intel_device_info intel_gm45_info = {
         .is_i965g = 1, .is_mobile = 1, .is_g4x = 1, .is_i9xx = 1,
         .is_mobile = 1, .need_gfx_hws = 1, .has_fbc = 1, .has_rc6 = 1,
         .has_pipe_cxsr = 1,
         .has_hotplug = 1,
 };
 
-const static struct intel_device_info intel_pineview_info = {
+static const struct intel_device_info intel_pineview_info = {
         .is_g33 = 1, .is_pineview = 1, .is_mobile = 1, .is_i9xx = 1,
         .need_gfx_hws = 1,
         .has_hotplug = 1,
 };
 
-const static struct intel_device_info intel_ironlake_d_info = {
+static const struct intel_device_info intel_ironlake_d_info = {
         .is_ironlake = 1, .is_i965g = 1, .is_i9xx = 1, .need_gfx_hws = 1,
         .has_pipe_cxsr = 1,
         .has_hotplug = 1,
 };
 
-const static struct intel_device_info intel_ironlake_m_info = {
+static const struct intel_device_info intel_ironlake_m_info = {
         .is_ironlake = 1, .is_mobile = 1, .is_i965g = 1, .is_i9xx = 1,
         .need_gfx_hws = 1, .has_rc6 = 1,
         .has_hotplug = 1,
 };
 
-const static struct intel_device_info intel_sandybridge_d_info = {
+static const struct intel_device_info intel_sandybridge_d_info = {
         .is_i965g = 1, .is_i9xx = 1, .need_gfx_hws = 1,
         .has_hotplug = 1, .is_gen6 = 1,
 };
 
-const static struct intel_device_info intel_sandybridge_m_info = {
+static const struct intel_device_info intel_sandybridge_m_info = {
         .is_i965g = 1, .is_mobile = 1, .is_i9xx = 1, .need_gfx_hws = 1,
         .has_hotplug = 1, .is_gen6 = 1,
 };
 
-const static struct pci_device_id pciidlist[] = {
+static const struct pci_device_id pciidlist[] = {
         INTEL_VGA_DEVICE(0x3577, &intel_i830_info),
         INTEL_VGA_DEVICE(0x2562, &intel_845g_info),
         INTEL_VGA_DEVICE(0x3582, &intel_i85x_info),
@@ -340,7 +340,7 @@ int i965_reset(struct drm_device *dev, u8 flags)
         /*
          * Clear request list
          */
-        i915_gem_retire_requests(dev);
+        i915_gem_retire_requests(dev, &dev_priv->render_ring);
 
         if (need_display)
                 i915_save_display(dev);
@@ -370,6 +370,7 @@ int i965_reset(struct drm_device *dev, u8 flags)
                 }
         } else {
                 DRM_ERROR("Error occurred. Don't know how to reset this chip.\n");
+                mutex_unlock(&dev->struct_mutex);
                 return -ENODEV;
         }
 
@@ -388,33 +389,10 @@ int i965_reset(struct drm_device *dev, u8 flags)
          * switched away).
          */
         if (drm_core_check_feature(dev, DRIVER_MODESET) ||
-            !dev_priv->mm.suspended) {
-                drm_i915_ring_buffer_t *ring = &dev_priv->ring;
-                struct drm_gem_object *obj = ring->ring_obj;
-                struct drm_i915_gem_object *obj_priv = to_intel_bo(obj);
+                        !dev_priv->mm.suspended) {
+                struct intel_ring_buffer *ring = &dev_priv->render_ring;
                 dev_priv->mm.suspended = 0;
-
-                /* Stop the ring if it's running. */
-                I915_WRITE(PRB0_CTL, 0);
-                I915_WRITE(PRB0_TAIL, 0);
-                I915_WRITE(PRB0_HEAD, 0);
-
-                /* Initialize the ring. */
-                I915_WRITE(PRB0_START, obj_priv->gtt_offset);
-                I915_WRITE(PRB0_CTL,
-                           ((obj->size - 4096) & RING_NR_PAGES) |
-                           RING_NO_REPORT |
-                           RING_VALID);
-                if (!drm_core_check_feature(dev, DRIVER_MODESET))
-                        i915_kernel_lost_context(dev);
-                else {
-                        ring->head = I915_READ(PRB0_HEAD) & HEAD_ADDR;
-                        ring->tail = I915_READ(PRB0_TAIL) & TAIL_ADDR;
-                        ring->space = ring->head - (ring->tail + 8);
-                        if (ring->space < 0)
-                                ring->space += ring->Size;
-                }
-
+                ring->init(dev, ring);
                 mutex_unlock(&dev->struct_mutex);
                 drm_irq_uninstall(dev);
                 drm_irq_install(dev);
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 7f797ef1ab39..276583159847 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -32,6 +32,7 @@
 
 #include "i915_reg.h"
 #include "intel_bios.h"
+#include "intel_ringbuffer.h"
 #include <linux/io-mapping.h>
 
 /* General customization:
@@ -55,6 +56,8 @@ enum plane {
 
 #define I915_NUM_PIPE   2
 
+#define I915_GEM_GPU_DOMAINS    (~(I915_GEM_DOMAIN_CPU | I915_GEM_DOMAIN_GTT))
+
 /* Interface history:
  *
  * 1.1: Original.
@@ -89,16 +92,6 @@ struct drm_i915_gem_phys_object {
         struct drm_gem_object *cur_obj;
 };
 
-typedef struct _drm_i915_ring_buffer {
-        unsigned long Size;
-        u8 *virtual_start;
-        int head;
-        int tail;
-        int space;
-        drm_local_map_t map;
-        struct drm_gem_object *ring_obj;
-} drm_i915_ring_buffer_t;
-
 struct mem_block {
         struct mem_block *next;
         struct mem_block *prev;
@@ -241,17 +234,15 @@ typedef struct drm_i915_private {
         void __iomem *regs;
 
         struct pci_dev *bridge_dev;
-        drm_i915_ring_buffer_t ring;
+        struct intel_ring_buffer render_ring;
+        struct intel_ring_buffer bsd_ring;
 
         drm_dma_handle_t *status_page_dmah;
-        void *hw_status_page;
         void *seqno_page;
         dma_addr_t dma_status_page;
         uint32_t counter;
-        unsigned int status_gfx_addr;
         unsigned int seqno_gfx_addr;
         drm_local_map_t hws_map;
-        struct drm_gem_object *hws_obj;
         struct drm_gem_object *seqno_obj;
         struct drm_gem_object *pwrctx;
 
@@ -267,8 +258,6 @@ typedef struct drm_i915_private {
         atomic_t irq_received;
         /** Protects user_irq_refcount and irq_mask_reg */
         spinlock_t user_irq_lock;
-        /** Refcount for i915_user_irq_get() versus i915_user_irq_put(). */
-        int user_irq_refcount;
         u32 trace_irq_seqno;
         /** Cached value of IMR to avoid reads in updating the bitfield */
         u32 irq_mask_reg;
@@ -289,6 +278,7 @@ typedef struct drm_i915_private {
         struct mem_block *agp_heap;
         unsigned int sr01, adpa, ppcr, dvob, dvoc, lvds;
         int vblank_pipe;
+        int num_pipe;
 
         /* For hangcheck timer */
 #define DRM_I915_HANGCHECK_PERIOD 75 /* in jiffies */
@@ -334,7 +324,7 @@ typedef struct drm_i915_private {
         int fence_reg_start; /* 4 if userland hasn't ioctl'd us yet */
         int num_fence_regs; /* 8 on pre-965, 16 otherwise */
 
-        unsigned int fsb_freq, mem_freq;
+        unsigned int fsb_freq, mem_freq, is_ddr3;
 
         spinlock_t error_lock;
         struct drm_i915_error_state *first_error;
@@ -514,18 +504,7 @@ typedef struct drm_i915_private {
                  */
                 struct list_head shrink_list;
 
-                /**
-                 * List of objects currently involved in rendering from the
-                 * ringbuffer.
-                 *
-                 * Includes buffers having the contents of their GPU caches
-                 * flushed, not necessarily primitives.  last_rendering_seqno
-                 * represents when the rendering involved will be completed.
-                 *
-                 * A reference is held on the buffer while on this list.
-                 */
                 spinlock_t active_list_lock;
-                struct list_head active_list;
 
                 /**
                  * List of objects which are not in the ringbuffer but which
@@ -563,12 +542,6 @@ typedef struct drm_i915_private {
                 struct list_head fence_list;
 
                 /**
-                 * List of breadcrumbs associated with GPU requests currently
-                 * outstanding.
-                 */
-                struct list_head request_list;
-
-                /**
                  * We leave the user IRQ off as much as possible,
                  * but this means that requests will finish and never
                  * be retired once the system goes idle. Set a timer to
@@ -644,6 +617,18 @@ typedef struct drm_i915_private {
         u8 cur_delay;
         u8 min_delay;
         u8 max_delay;
+        u8 fmax;
+        u8 fstart;
+
+        u64 last_count1;
+        unsigned long last_time1;
+        u64 last_count2;
+        struct timespec last_time2;
+        unsigned long gfx_power;
+        int c_m;
+        int r_t;
+        u8 corr;
+        spinlock_t *mchdev_lock;
 
         enum no_fbc_reason no_fbc_reason;
 
@@ -671,19 +656,64 @@ struct drm_i915_gem_object {
          * (has pending rendering), and is not set if it's on inactive (ready
          * to be unbound).
          */
-        int active;
+        unsigned int active : 1;
 
         /**
          * This is set if the object has been written to since last bound
          * to the GTT
          */
-        int dirty;
+        unsigned int dirty : 1;
+
+        /**
+         * Fence register bits (if any) for this object.  Will be set
+         * as needed when mapped into the GTT.
+         * Protected by dev->struct_mutex.
+         *
+         * Size: 4 bits for 16 fences + sign (for FENCE_REG_NONE)
+         */
+        int fence_reg : 5;
+
+        /**
+         * Used for checking the object doesn't appear more than once
+         * in an execbuffer object list.
+         */
+        unsigned int in_execbuffer : 1;
+
+        /**
+         * Advice: are the backing pages purgeable?
+         */
+        unsigned int madv : 2;
+
+        /**
+         * Refcount for the pages array. With the current locking scheme, there
+         * are at most two concurrent users: Binding a bo to the gtt and
+         * pwrite/pread using physical addresses. So two bits for a maximum
+         * of two users are enough.
+         */
+        unsigned int pages_refcount : 2;
+#define DRM_I915_GEM_OBJECT_MAX_PAGES_REFCOUNT 0x3
+
+        /**
+         * Current tiling mode for the object.
+         */
+        unsigned int tiling_mode : 2;
+
+        /** How many users have pinned this object in GTT space. The following
+         * users can each hold at most one reference: pwrite/pread, pin_ioctl
+         * (via user_pin_count), execbuffer (objects are not allowed multiple
+         * times for the same batchbuffer), and the framebuffer code. When
+         * switching/pageflipping, the framebuffer code has at most two buffers
+         * pinned per crtc.
+         *
+         * In the worst case this is 1 + 1 + 1 + 2*2 = 7. That would fit into 3
+         * bits with absolutely no headroom. So use 4 bits. */
+        int pin_count : 4;
+#define DRM_I915_GEM_OBJECT_MAX_PIN_COUNT 0xf
 
         /** AGP memory structure for our GTT binding. */
         DRM_AGP_MEM *agp_mem;
 
         struct page **pages;
-        int pages_refcount;
 
         /**
          * Current offset of the object in GTT space.
@@ -692,26 +722,18 @@ struct drm_i915_gem_object {
          */
         uint32_t gtt_offset;
 
+        /* Which ring is refering to is this object */
+        struct intel_ring_buffer *ring;
+
         /**
          * Fake offset for use by mmap(2)
          */
         uint64_t mmap_offset;
 
-        /**
-         * Fence register bits (if any) for this object.  Will be set
-         * as needed when mapped into the GTT.
-         * Protected by dev->struct_mutex.
-         */
-        int fence_reg;
-
-        /** How many users have pinned this object in GTT space */
-        int pin_count;
-
         /** Breadcrumb of last rendering to the buffer. */
         uint32_t last_rendering_seqno;
 
-        /** Current tiling mode for the object. */
-        uint32_t tiling_mode;
+        /** Current tiling stride for the object, if it's tiled. */
         uint32_t stride;
 
         /** Record of address bit 17 of each page at last unbind. */
@@ -734,17 +756,6 @@ struct drm_i915_gem_object {
         struct drm_i915_gem_phys_object *phys_obj;
 
         /**
-         * Used for checking the object doesn't appear more than once
-         * in an execbuffer object list.
-         */
-        int in_execbuffer;
-
-        /**
-         * Advice: are the backing pages purgeable?
-         */
-        int madv;
-
-        /**
          * Number of crtcs where this object is currently the fb, but
          * will be page flipped away on the next vblank.  When it
          * reaches 0, dev_priv->pending_flip_queue will be woken up.
@@ -765,6 +776,9 @@ struct drm_i915_gem_object {
  * an emission time with seqnos for tracking how far ahead of the GPU we are.
  */
 struct drm_i915_gem_request {
+        /** On Which ring this request was generated */
+        struct intel_ring_buffer *ring;
+
         /** GEM sequence number associated with this request. */
         uint32_t seqno;
 
@@ -821,6 +835,11 @@ extern int i915_emit_box(struct drm_device *dev,
                          struct drm_clip_rect *boxes,
                          int i, int DR1, int DR4);
 extern int i965_reset(struct drm_device *dev, u8 flags);
+extern unsigned long i915_chipset_val(struct drm_i915_private *dev_priv);
+extern unsigned long i915_mch_val(struct drm_i915_private *dev_priv);
+extern unsigned long i915_gfx_val(struct drm_i915_private *dev_priv);
+extern void i915_update_gfx_val(struct drm_i915_private *dev_priv);
+
 
 /* i915_irq.c */
 void i915_hangcheck_elapsed(unsigned long data);
@@ -829,9 +848,7 @@ extern int i915_irq_emit(struct drm_device *dev, void *data,
                          struct drm_file *file_priv);
 extern int i915_irq_wait(struct drm_device *dev, void *data,
                          struct drm_file *file_priv);
-void i915_user_irq_get(struct drm_device *dev);
 void i915_trace_irq_get(struct drm_device *dev, u32 seqno);
-void i915_user_irq_put(struct drm_device *dev);
 extern void i915_enable_interrupt (struct drm_device *dev);
 
 extern irqreturn_t i915_driver_irq_handler(DRM_IRQ_ARGS);
@@ -849,6 +866,11 @@ extern u32 gm45_get_vblank_counter(struct drm_device *dev, int crtc);
 extern int i915_vblank_swap(struct drm_device *dev, void *data,
                             struct drm_file *file_priv);
 extern void i915_enable_irq(drm_i915_private_t *dev_priv, u32 mask);
+extern void i915_disable_irq(drm_i915_private_t *dev_priv, u32 mask);
+extern void ironlake_enable_graphics_irq(drm_i915_private_t *dev_priv,
+                u32 mask);
+extern void ironlake_disable_graphics_irq(drm_i915_private_t *dev_priv,
+                u32 mask);
 
 void
 i915_enable_pipestat(drm_i915_private_t *dev_priv, int pipe, u32 mask);
@@ -922,11 +944,13 @@ void i915_gem_object_unpin(struct drm_gem_object *obj);
 int i915_gem_object_unbind(struct drm_gem_object *obj);
 void i915_gem_release_mmap(struct drm_gem_object *obj);
 void i915_gem_lastclose(struct drm_device *dev);
-uint32_t i915_get_gem_seqno(struct drm_device *dev);
+uint32_t i915_get_gem_seqno(struct drm_device *dev,
+                struct intel_ring_buffer *ring);
 bool i915_seqno_passed(uint32_t seq1, uint32_t seq2);
 int i915_gem_object_get_fence_reg(struct drm_gem_object *obj);
 int i915_gem_object_put_fence_reg(struct drm_gem_object *obj);
-void i915_gem_retire_requests(struct drm_device *dev);
+void i915_gem_retire_requests(struct drm_device *dev,
+                 struct intel_ring_buffer *ring);
 void i915_gem_retire_work_handler(struct work_struct *work);
 void i915_gem_clflush_object(struct drm_gem_object *obj);
 int i915_gem_object_set_domain(struct drm_gem_object *obj,
@@ -937,9 +961,13 @@ void i915_gem_cleanup_ringbuffer(struct drm_device *dev);
 int i915_gem_do_init(struct drm_device *dev, unsigned long start,
                      unsigned long end);
 int i915_gem_idle(struct drm_device *dev);
-uint32_t i915_add_request(struct drm_device *dev, struct drm_file *file_priv,
-                          uint32_t flush_domains);
-int i915_do_wait_request(struct drm_device *dev, uint32_t seqno, int interruptible);
+uint32_t i915_add_request(struct drm_device *dev,
+                struct drm_file *file_priv,
+                uint32_t flush_domains,
+                struct intel_ring_buffer *ring);
+int i915_do_wait_request(struct drm_device *dev,
+                uint32_t seqno, int interruptible,
+                struct intel_ring_buffer *ring);
 int i915_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
 int i915_gem_object_set_to_gtt_domain(struct drm_gem_object *obj,
                                       int write);
@@ -1015,7 +1043,7 @@ extern void g4x_disable_fbc(struct drm_device *dev);
 extern void intel_disable_fbc(struct drm_device *dev);
 extern void intel_enable_fbc(struct drm_crtc *crtc, unsigned long interval);
 extern bool intel_fbc_enabled(struct drm_device *dev);
-
+extern bool ironlake_set_drps(struct drm_device *dev, u8 val);
 extern void intel_detect_pch (struct drm_device *dev);
 extern int intel_trans_dp_port_sel (struct drm_crtc *crtc);
 
@@ -1026,7 +1054,8 @@ extern int intel_trans_dp_port_sel (struct drm_crtc *crtc);
  * has access to the ring.
  */
 #define RING_LOCK_TEST_WITH_RETURN(dev, file_priv) do {                 \
-        if (((drm_i915_private_t *)dev->dev_private)->ring.ring_obj == NULL) \
+        if (((drm_i915_private_t *)dev->dev_private)->render_ring.gem_object \
+                        == NULL)                                        \
                 LOCK_TEST_WITH_RETURN(dev, file_priv);                  \
 } while (0)
 
@@ -1039,35 +1068,31 @@ extern int intel_trans_dp_port_sel (struct drm_crtc *crtc);
 #define I915_WRITE64(reg, val)  writeq(val, dev_priv->regs + (reg))
 #define I915_READ64(reg)        readq(dev_priv->regs + (reg))
 #define POSTING_READ(reg)       (void)I915_READ(reg)
+#define POSTING_READ16(reg)     (void)I915_READ16(reg)
 
 #define I915_VERBOSE 0
 
-#define RING_LOCALS     volatile unsigned int *ring_virt__;
-
-#define BEGIN_LP_RING(n) do {                                           \
-        int bytes__ = 4*(n);                                            \
-        if (I915_VERBOSE) DRM_DEBUG("BEGIN_LP_RING(%d)\n", (n));        \
-        /* a wrap must occur between instructions so pad beforehand */  \
-        if (unlikely (dev_priv->ring.tail + bytes__ > dev_priv->ring.Size)) \
-                i915_wrap_ring(dev);                                    \
-        if (unlikely (dev_priv->ring.space < bytes__))                  \
-                i915_wait_ring(dev, bytes__, __func__);                 \
-        ring_virt__ = (unsigned int *)                                  \
-                (dev_priv->ring.virtual_start + dev_priv->ring.tail);   \
-        dev_priv->ring.tail += bytes__;                                 \
-        dev_priv->ring.tail &= dev_priv->ring.Size - 1;                 \
-        dev_priv->ring.space -= bytes__;                                \
+#define BEGIN_LP_RING(n)  do { \
+        drm_i915_private_t *dev_priv = dev->dev_private;                \
+        if (I915_VERBOSE)                                               \
+                DRM_DEBUG("   BEGIN_LP_RING %x\n", (int)(n));           \
+        intel_ring_begin(dev, &dev_priv->render_ring, 4*(n));           \
 } while (0)
 
-#define OUT_RING(n) do {                                                \
-        if (I915_VERBOSE) DRM_DEBUG("   OUT_RING %x\n", (int)(n));      \
-        *ring_virt__++ = (n);                                           \
+
+#define OUT_RING(x) do {                                                \
+        drm_i915_private_t *dev_priv = dev->dev_private;                \
+        if (I915_VERBOSE)                                               \
+                DRM_DEBUG("   OUT_RING %x\n", (int)(x));                \
+        intel_ring_emit(dev, &dev_priv->render_ring, x);                \
 } while (0)
 
 #define ADVANCE_LP_RING() do {                                          \
+        drm_i915_private_t *dev_priv = dev->dev_private;                \
         if (I915_VERBOSE)                                               \
-                DRM_DEBUG("ADVANCE_LP_RING %x\n", dev_priv->ring.tail); \
-        I915_WRITE(PRB0_TAIL, dev_priv->ring.tail);                     \
+                DRM_DEBUG("ADVANCE_LP_RING %x\n",                       \
+                                dev_priv->render_ring.tail);            \
+        intel_ring_advance(dev, &dev_priv->render_ring);                \
 } while(0)
 
 /**
@@ -1085,14 +1110,12 @@ extern int intel_trans_dp_port_sel (struct drm_crtc *crtc);
  *
  * The area from dword 0x20 to 0x3ff is available for driver usage.
  */
-#define READ_HWSP(dev_priv, reg)  (((volatile u32*)(dev_priv->hw_status_page))[reg])
+#define READ_HWSP(dev_priv, reg)  (((volatile u32 *)\
+                        (dev_priv->render_ring.status_page.page_addr))[reg])
 #define READ_BREADCRUMB(dev_priv) READ_HWSP(dev_priv, I915_BREADCRUMB_INDEX)
 #define I915_GEM_HWS_INDEX              0x20
 #define I915_BREADCRUMB_INDEX           0x21
 
-extern int i915_wrap_ring(struct drm_device * dev);
-extern int i915_wait_ring(struct drm_device * dev, int n, const char *caller);
-
 #define INTEL_INFO(dev) (((struct drm_i915_private *) (dev)->dev_private)->info)
 
 #define IS_I830(dev)            ((dev)->pci_device == 0x3577)
@@ -1138,6 +1161,7 @@ extern int i915_wait_ring(struct drm_device * dev, int n, const char *caller);
                          (dev)->pci_device == 0x2A42 ||         \
                          (dev)->pci_device == 0x2E42)
 
+#define HAS_BSD(dev)            (IS_IRONLAKE(dev) || IS_G4X(dev))
 #define I915_NEED_GFX_HWS(dev)  (INTEL_INFO(dev)->need_gfx_hws)
 
 /* With the 945 and later, Y tiling got adjusted so that it was 32 128-byte
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index 112699f71fa4..9ded3dae6c87 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -35,8 +35,6 @@
 #include <linux/swap.h>
 #include <linux/pci.h>
 
-#define I915_GEM_GPU_DOMAINS    (~(I915_GEM_DOMAIN_CPU | I915_GEM_DOMAIN_GTT))
-
 static void i915_gem_object_flush_gpu_write_domain(struct drm_gem_object *obj);
 static void i915_gem_object_flush_gtt_write_domain(struct drm_gem_object *obj);
 static void i915_gem_object_flush_cpu_write_domain(struct drm_gem_object *obj);
@@ -169,7 +167,7 @@ static int i915_gem_object_needs_bit17_swizzle(struct drm_gem_object *obj)
                 obj_priv->tiling_mode != I915_TILING_NONE;
 }
 
-static inline int
+static inline void
 slow_shmem_copy(struct page *dst_page,
                 int dst_offset,
                 struct page *src_page,
@@ -178,25 +176,16 @@ slow_shmem_copy(struct page *dst_page,
 {
         char *dst_vaddr, *src_vaddr;
 
-        dst_vaddr = kmap_atomic(dst_page, KM_USER0);
-        if (dst_vaddr == NULL)
-                return -ENOMEM;
-
-        src_vaddr = kmap_atomic(src_page, KM_USER1);
-        if (src_vaddr == NULL) {
-                kunmap_atomic(dst_vaddr, KM_USER0);
-                return -ENOMEM;
-        }
+        dst_vaddr = kmap(dst_page);
+        src_vaddr = kmap(src_page);
 
         memcpy(dst_vaddr + dst_offset, src_vaddr + src_offset, length);
 
-        kunmap_atomic(src_vaddr, KM_USER1);
-        kunmap_atomic(dst_vaddr, KM_USER0);
-
-        return 0;
+        kunmap(src_page);
+        kunmap(dst_page);
 }
 
-static inline int
+static inline void
 slow_shmem_bit17_copy(struct page *gpu_page,
                       int gpu_offset,
                       struct page *cpu_page,
@@ -216,15 +205,8 @@ slow_shmem_bit17_copy(struct page *gpu_page,
                                                cpu_page, cpu_offset, length);
         }
 
-        gpu_vaddr = kmap_atomic(gpu_page, KM_USER0);
-        if (gpu_vaddr == NULL)
-                return -ENOMEM;
-
-        cpu_vaddr = kmap_atomic(cpu_page, KM_USER1);
-        if (cpu_vaddr == NULL) {
-                kunmap_atomic(gpu_vaddr, KM_USER0);
-                return -ENOMEM;
-        }
+        gpu_vaddr = kmap(gpu_page);
+        cpu_vaddr = kmap(cpu_page);
 
         /* Copy the data, XORing A6 with A17 (1). The user already knows he's
          * XORing with the other bits (A9 for Y, A9 and A10 for X)
@@ -248,10 +230,8 @@ slow_shmem_bit17_copy(struct page *gpu_page,
                 length -= this_length;
         }
 
-        kunmap_atomic(cpu_vaddr, KM_USER1);
-        kunmap_atomic(gpu_vaddr, KM_USER0);
-
-        return 0;
+        kunmap(cpu_page);
+        kunmap(gpu_page);
 }
 
 /**
@@ -427,21 +407,19 @@ i915_gem_shmem_pread_slow(struct drm_device *dev, struct drm_gem_object *obj,
                         page_length = PAGE_SIZE - data_page_offset;
 
                 if (do_bit17_swizzling) {
-                        ret = slow_shmem_bit17_copy(obj_priv->pages[shmem_page_index],
-                                                    shmem_page_offset,
-                                                    user_pages[data_page_index],
-                                                    data_page_offset,
-                                                    page_length,
-                                                    1);
-                } else {
-                        ret = slow_shmem_copy(user_pages[data_page_index],
-                                              data_page_offset,
-                                              obj_priv->pages[shmem_page_index],
+                        slow_shmem_bit17_copy(obj_priv->pages[shmem_page_index],
                                               shmem_page_offset,
-                                              page_length);
+                                              user_pages[data_page_index],
+                                              data_page_offset,
+                                              page_length,
+                                              1);
+                } else {
+                        slow_shmem_copy(user_pages[data_page_index],
+                                        data_page_offset,
+                                        obj_priv->pages[shmem_page_index],
+                                        shmem_page_offset,
+                                        page_length);
                 }
-                if (ret)
-                        goto fail_put_pages;
 
                 remain -= page_length;
                 data_ptr += page_length;
@@ -531,25 +509,24 @@ fast_user_write(struct io_mapping *mapping,
  * page faults
  */
 
-static inline int
+static inline void
 slow_kernel_write(struct io_mapping *mapping,
                   loff_t gtt_base, int gtt_offset,
                   struct page *user_page, int user_offset,
                   int length)
 {
-        char *src_vaddr, *dst_vaddr;
-        unsigned long unwritten;
+        char __iomem *dst_vaddr;
+        char *src_vaddr;
 
-        dst_vaddr = io_mapping_map_atomic_wc(mapping, gtt_base);
-        src_vaddr = kmap_atomic(user_page, KM_USER1);
-        unwritten = __copy_from_user_inatomic_nocache(dst_vaddr + gtt_offset,
-                                                      src_vaddr + user_offset,
-                                                      length);
-        kunmap_atomic(src_vaddr, KM_USER1);
-        io_mapping_unmap_atomic(dst_vaddr);
-        if (unwritten)
-                return -EFAULT;
-        return 0;
+        dst_vaddr = io_mapping_map_wc(mapping, gtt_base);
+        src_vaddr = kmap(user_page);
+
+        memcpy_toio(dst_vaddr + gtt_offset,
+                    src_vaddr + user_offset,
+                    length);
+
+        kunmap(user_page);
+        io_mapping_unmap(dst_vaddr);
 }
 
 static inline int
@@ -722,18 +699,11 @@ i915_gem_gtt_pwrite_slow(struct drm_device *dev, struct drm_gem_object *obj,
                 if ((data_page_offset + page_length) > PAGE_SIZE)
                         page_length = PAGE_SIZE - data_page_offset;
 
-                ret = slow_kernel_write(dev_priv->mm.gtt_mapping,
-                                        gtt_page_base, gtt_page_offset,
-                                        user_pages[data_page_index],
-                                        data_page_offset,
-                                        page_length);
-
-                /* If we get a fault while copying data, then (presumably) our
-                 * source page isn't available.  Return the error and we'll
-                 * retry in the slow path.
-                 */
-                if (ret)
-                        goto out_unpin_object;
+                slow_kernel_write(dev_priv->mm.gtt_mapping,
+                                  gtt_page_base, gtt_page_offset,
+                                  user_pages[data_page_index],
+                                  data_page_offset,
+                                  page_length);
 
                 remain -= page_length;
                 offset += page_length;
@@ -902,21 +872,19 @@ i915_gem_shmem_pwrite_slow(struct drm_device *dev, struct drm_gem_object *obj,
                         page_length = PAGE_SIZE - data_page_offset;
 
                 if (do_bit17_swizzling) {
-                        ret = slow_shmem_bit17_copy(obj_priv->pages[shmem_page_index],
-                                                    shmem_page_offset,
-                                                    user_pages[data_page_index],
-                                                    data_page_offset,
-                                                    page_length,
-                                                    0);
-                } else {
-                        ret = slow_shmem_copy(obj_priv->pages[shmem_page_index],
+                        slow_shmem_bit17_copy(obj_priv->pages[shmem_page_index],
                                               shmem_page_offset,
                                               user_pages[data_page_index],
                                               data_page_offset,
-                                              page_length);
+                                              page_length,
+                                              0);
+                } else {
+                        slow_shmem_copy(obj_priv->pages[shmem_page_index],
+                                        shmem_page_offset,
+                                        user_pages[data_page_index],
+                                        data_page_offset,
+                                        page_length);
                 }
-                if (ret)
-                        goto fail_put_pages;
 
                 remain -= page_length;
                 data_ptr += page_length;
@@ -973,7 +941,8 @@ i915_gem_pwrite_ioctl(struct drm_device *dev, void *data,
         if (obj_priv->phys_obj)
                 ret = i915_gem_phys_pwrite(dev, obj, args, file_priv);
         else if (obj_priv->tiling_mode == I915_TILING_NONE &&
-                 dev->gtt_total != 0) {
+                 dev->gtt_total != 0 &&
+                 obj->write_domain != I915_GEM_DOMAIN_CPU) {
                 ret = i915_gem_gtt_pwrite_fast(dev, obj, args, file_priv);
                 if (ret == -EFAULT) {
                         ret = i915_gem_gtt_pwrite_slow(dev, obj, args,
@@ -1484,11 +1453,14 @@ i915_gem_object_put_pages(struct drm_gem_object *obj)
 }
 
 static void
-i915_gem_object_move_to_active(struct drm_gem_object *obj, uint32_t seqno)
+i915_gem_object_move_to_active(struct drm_gem_object *obj, uint32_t seqno,
+                               struct intel_ring_buffer *ring)
 {
         struct drm_device *dev = obj->dev;
         drm_i915_private_t *dev_priv = dev->dev_private;
         struct drm_i915_gem_object *obj_priv = to_intel_bo(obj);
+        BUG_ON(ring == NULL);
+        obj_priv->ring = ring;
 
         /* Add a reference if we're newly entering the active list. */
         if (!obj_priv->active) {
@@ -1497,8 +1469,7 @@ i915_gem_object_move_to_active(struct drm_gem_object *obj, uint32_t seqno)
         }
         /* Move from whatever list we were on to the tail of execution. */
         spin_lock(&dev_priv->mm.active_list_lock);
-        list_move_tail(&obj_priv->list,
-                       &dev_priv->mm.active_list);
+        list_move_tail(&obj_priv->list, &ring->active_list);
         spin_unlock(&dev_priv->mm.active_list_lock);
         obj_priv->last_rendering_seqno = seqno;
 }
@@ -1551,6 +1522,7 @@ i915_gem_object_move_to_inactive(struct drm_gem_object *obj)
         BUG_ON(!list_empty(&obj_priv->gpu_write_list));
 
         obj_priv->last_rendering_seqno = 0;
+        obj_priv->ring = NULL;
         if (obj_priv->active) {
                 obj_priv->active = 0;
                 drm_gem_object_unreference(obj);
@@ -1560,7 +1532,8 @@ i915_gem_object_move_to_inactive(struct drm_gem_object *obj)
 
 static void
 i915_gem_process_flushing_list(struct drm_device *dev,
-                               uint32_t flush_domains, uint32_t seqno)
+                               uint32_t flush_domains, uint32_t seqno,
+                               struct intel_ring_buffer *ring)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
         struct drm_i915_gem_object *obj_priv, *next;
@@ -1571,12 +1544,13 @@ i915_gem_process_flushing_list(struct drm_device *dev,
                 struct drm_gem_object *obj = &obj_priv->base;
 
                 if ((obj->write_domain & flush_domains) ==
-                    obj->write_domain) {
+                    obj->write_domain &&
+                    obj_priv->ring->ring_flag == ring->ring_flag) {
                         uint32_t old_write_domain = obj->write_domain;
 
                         obj->write_domain = 0;
                         list_del_init(&obj_priv->gpu_write_list);
-                        i915_gem_object_move_to_active(obj, seqno);
+                        i915_gem_object_move_to_active(obj, seqno, ring);
 
                         /* update the fence lru list */
                         if (obj_priv->fence_reg != I915_FENCE_REG_NONE) {
@@ -1593,31 +1567,15 @@ i915_gem_process_flushing_list(struct drm_device *dev,
         }
 }
 
-#define PIPE_CONTROL_FLUSH(addr)                                        \
-        OUT_RING(GFX_OP_PIPE_CONTROL | PIPE_CONTROL_QW_WRITE |          \
-                 PIPE_CONTROL_DEPTH_STALL);                             \
-        OUT_RING(addr | PIPE_CONTROL_GLOBAL_GTT);                       \
-        OUT_RING(0);                                                    \
-        OUT_RING(0);                                                    \
-
-/**
- * Creates a new sequence number, emitting a write of it to the status page
- * plus an interrupt, which will trigger i915_user_interrupt_handler.
- *
- * Must be called with struct_lock held.
- *
- * Returned sequence numbers are nonzero on success.
- */
 uint32_t
 i915_add_request(struct drm_device *dev, struct drm_file *file_priv,
-                 uint32_t flush_domains)
+                 uint32_t flush_domains, struct intel_ring_buffer *ring)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
         struct drm_i915_file_private *i915_file_priv = NULL;
         struct drm_i915_gem_request *request;
         uint32_t seqno;
         int was_empty;
-        RING_LOCALS;
 
         if (file_priv != NULL)
                 i915_file_priv = file_priv->driver_priv;
@@ -1626,62 +1584,14 @@ i915_add_request(struct drm_device *dev, struct drm_file *file_priv,
         if (request == NULL)
                 return 0;
 
-        /* Grab the seqno we're going to make this request be, and bump the
-         * next (skipping 0 so it can be the reserved no-seqno value).
-         */
-        seqno = dev_priv->mm.next_gem_seqno;
-        dev_priv->mm.next_gem_seqno++;
-        if (dev_priv->mm.next_gem_seqno == 0)
-                dev_priv->mm.next_gem_seqno++;
-
-        if (HAS_PIPE_CONTROL(dev)) {
-                u32 scratch_addr = dev_priv->seqno_gfx_addr + 128;
-
-                /*
-                 * Workaround qword write incoherence by flushing the
-                 * PIPE_NOTIFY buffers out to memory before requesting
-                 * an interrupt.
-                 */
-                BEGIN_LP_RING(32);
-                OUT_RING(GFX_OP_PIPE_CONTROL | PIPE_CONTROL_QW_WRITE |
-                         PIPE_CONTROL_WC_FLUSH | PIPE_CONTROL_TC_FLUSH);
-                OUT_RING(dev_priv->seqno_gfx_addr | PIPE_CONTROL_GLOBAL_GTT);
-                OUT_RING(seqno);
-                OUT_RING(0);
-                PIPE_CONTROL_FLUSH(scratch_addr);
-                scratch_addr += 128; /* write to separate cachelines */
-                PIPE_CONTROL_FLUSH(scratch_addr);
-                scratch_addr += 128;
-                PIPE_CONTROL_FLUSH(scratch_addr);
-                scratch_addr += 128;
-                PIPE_CONTROL_FLUSH(scratch_addr);
-                scratch_addr += 128;
-                PIPE_CONTROL_FLUSH(scratch_addr);
-                scratch_addr += 128;
-                PIPE_CONTROL_FLUSH(scratch_addr);
-                OUT_RING(GFX_OP_PIPE_CONTROL | PIPE_CONTROL_QW_WRITE |
-                         PIPE_CONTROL_WC_FLUSH | PIPE_CONTROL_TC_FLUSH |
-                         PIPE_CONTROL_NOTIFY);
-                OUT_RING(dev_priv->seqno_gfx_addr | PIPE_CONTROL_GLOBAL_GTT);
-                OUT_RING(seqno);
-                OUT_RING(0);
-                ADVANCE_LP_RING();
-        } else {
-                BEGIN_LP_RING(4);
-                OUT_RING(MI_STORE_DWORD_INDEX);
-                OUT_RING(I915_GEM_HWS_INDEX << MI_STORE_DWORD_INDEX_SHIFT);
-                OUT_RING(seqno);
-
-                OUT_RING(MI_USER_INTERRUPT);
-                ADVANCE_LP_RING();
-        }
-
-        DRM_DEBUG_DRIVER("%d\n", seqno);
+        seqno = ring->add_request(dev, ring, file_priv, flush_domains);
 
         request->seqno = seqno;
+        request->ring = ring;
         request->emitted_jiffies = jiffies;
-        was_empty = list_empty(&dev_priv->mm.request_list);
-        list_add_tail(&request->list, &dev_priv->mm.request_list);
+        was_empty = list_empty(&ring->request_list);
+        list_add_tail(&request->list, &ring->request_list);
+
         if (i915_file_priv) {
                 list_add_tail(&request->client_list,
                               &i915_file_priv->mm.request_list);
@@ -1693,7 +1603,7 @@ i915_add_request(struct drm_device *dev, struct drm_file *file_priv,
          * domain we're flushing with our flush.
          */
         if (flush_domains != 0) 
-                i915_gem_process_flushing_list(dev, flush_domains, seqno);
+                i915_gem_process_flushing_list(dev, flush_domains, seqno, ring);
 
         if (!dev_priv->mm.suspended) {
                 mod_timer(&dev_priv->hangcheck_timer, jiffies + DRM_I915_HANGCHECK_PERIOD);
@@ -1710,20 +1620,16 @@ i915_add_request(struct drm_device *dev, struct drm_file *file_priv,
  * before signalling the CPU
  */
 static uint32_t
-i915_retire_commands(struct drm_device *dev)
+i915_retire_commands(struct drm_device *dev, struct intel_ring_buffer *ring)
 {
-        drm_i915_private_t *dev_priv = dev->dev_private;
-        uint32_t cmd = MI_FLUSH | MI_NO_WRITE_FLUSH;
         uint32_t flush_domains = 0;
-        RING_LOCALS;
 
         /* The sampler always gets flushed on i965 (sigh) */
         if (IS_I965G(dev))
                 flush_domains |= I915_GEM_DOMAIN_SAMPLER;
-        BEGIN_LP_RING(2);
-        OUT_RING(cmd);
-        OUT_RING(0); /* noop */
-        ADVANCE_LP_RING();
+
+        ring->flush(dev, ring,
+                        I915_GEM_DOMAIN_COMMAND, flush_domains);
         return flush_domains;
 }
 
@@ -1743,11 +1649,11 @@ i915_gem_retire_request(struct drm_device *dev,
          * by the ringbuffer to the flushing/inactive lists as appropriate.
          */
         spin_lock(&dev_priv->mm.active_list_lock);
-        while (!list_empty(&dev_priv->mm.active_list)) {
+        while (!list_empty(&request->ring->active_list)) {
                 struct drm_gem_object *obj;
                 struct drm_i915_gem_object *obj_priv;
 
-                obj_priv = list_first_entry(&dev_priv->mm.active_list,
+                obj_priv = list_first_entry(&request->ring->active_list,
                                             struct drm_i915_gem_object,
                                             list);
                 obj = &obj_priv->base;
@@ -1794,35 +1700,33 @@ i915_seqno_passed(uint32_t seq1, uint32_t seq2)
 }
 
 uint32_t
-i915_get_gem_seqno(struct drm_device *dev)
+i915_get_gem_seqno(struct drm_device *dev,
+                   struct intel_ring_buffer *ring)
 {
-        drm_i915_private_t *dev_priv = dev->dev_private;
-
-        if (HAS_PIPE_CONTROL(dev))
-                return ((volatile u32 *)(dev_priv->seqno_page))[0];
-        else
-                return READ_HWSP(dev_priv, I915_GEM_HWS_INDEX);
+        return ring->get_gem_seqno(dev, ring);
 }
 
 /**
  * This function clears the request list as sequence numbers are passed.
  */
 void
-i915_gem_retire_requests(struct drm_device *dev)
+i915_gem_retire_requests(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
         uint32_t seqno;
 
-        if (!dev_priv->hw_status_page || list_empty(&dev_priv->mm.request_list))
+        if (!ring->status_page.page_addr
+                        || list_empty(&ring->request_list))
                 return;
 
-        seqno = i915_get_gem_seqno(dev);
+        seqno = i915_get_gem_seqno(dev, ring);
 
-        while (!list_empty(&dev_priv->mm.request_list)) {
+        while (!list_empty(&ring->request_list)) {
                 struct drm_i915_gem_request *request;
                 uint32_t retiring_seqno;
 
-                request = list_first_entry(&dev_priv->mm.request_list,
+                request = list_first_entry(&ring->request_list,
                                            struct drm_i915_gem_request,
                                            list);
                 retiring_seqno = request->seqno;
@@ -1840,7 +1744,8 @@ i915_gem_retire_requests(struct drm_device *dev)
 
         if (unlikely (dev_priv->trace_irq_seqno &&
                       i915_seqno_passed(dev_priv->trace_irq_seqno, seqno))) {
-                i915_user_irq_put(dev);
+
+                ring->user_irq_put(dev, ring);
                 dev_priv->trace_irq_seqno = 0;
         }
 }
@@ -1856,15 +1761,22 @@ i915_gem_retire_work_handler(struct work_struct *work)
         dev = dev_priv->dev;
 
         mutex_lock(&dev->struct_mutex);
-        i915_gem_retire_requests(dev);
+        i915_gem_retire_requests(dev, &dev_priv->render_ring);
+
+        if (HAS_BSD(dev))
+                i915_gem_retire_requests(dev, &dev_priv->bsd_ring);
+
         if (!dev_priv->mm.suspended &&
-            !list_empty(&dev_priv->mm.request_list))
+                (!list_empty(&dev_priv->render_ring.request_list) ||
+                        (HAS_BSD(dev) &&
+                         !list_empty(&dev_priv->bsd_ring.request_list))))
                 queue_delayed_work(dev_priv->wq, &dev_priv->mm.retire_work, HZ);
         mutex_unlock(&dev->struct_mutex);
 }
 
 int
-i915_do_wait_request(struct drm_device *dev, uint32_t seqno, int interruptible)
+i915_do_wait_request(struct drm_device *dev, uint32_t seqno,
+                int interruptible, struct intel_ring_buffer *ring)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
         u32 ier;
@@ -1875,7 +1787,7 @@ i915_do_wait_request(struct drm_device *dev, uint32_t seqno, int interruptible)
         if (atomic_read(&dev_priv->mm.wedged))
                 return -EIO;
 
-        if (!i915_seqno_passed(i915_get_gem_seqno(dev), seqno)) {
+        if (!i915_seqno_passed(ring->get_gem_seqno(dev, ring), seqno)) {
                 if (HAS_PCH_SPLIT(dev))
                         ier = I915_READ(DEIER) | I915_READ(GTIER);
                 else
@@ -1889,19 +1801,21 @@ i915_do_wait_request(struct drm_device *dev, uint32_t seqno, int interruptible)
 
                 trace_i915_gem_request_wait_begin(dev, seqno);
 
-                dev_priv->mm.waiting_gem_seqno = seqno;
-                i915_user_irq_get(dev);
+                ring->waiting_gem_seqno = seqno;
+                ring->user_irq_get(dev, ring);
                 if (interruptible)
-                        ret = wait_event_interruptible(dev_priv->irq_queue,
-                                i915_seqno_passed(i915_get_gem_seqno(dev), seqno) ||
-                                atomic_read(&dev_priv->mm.wedged));
+                        ret = wait_event_interruptible(ring->irq_queue,
+                                i915_seqno_passed(
+                                        ring->get_gem_seqno(dev, ring), seqno)
+                                || atomic_read(&dev_priv->mm.wedged));
                 else
-                        wait_event(dev_priv->irq_queue,
-                                i915_seqno_passed(i915_get_gem_seqno(dev), seqno) ||
-                                atomic_read(&dev_priv->mm.wedged));
+                        wait_event(ring->irq_queue,
+                                i915_seqno_passed(
+                                        ring->get_gem_seqno(dev, ring), seqno)
+                                || atomic_read(&dev_priv->mm.wedged));
 
-                i915_user_irq_put(dev);
-                dev_priv->mm.waiting_gem_seqno = 0;
+                ring->user_irq_put(dev, ring);
+                ring->waiting_gem_seqno = 0;
 
                 trace_i915_gem_request_wait_end(dev, seqno);
         }
@@ -1910,7 +1824,7 @@ i915_do_wait_request(struct drm_device *dev, uint32_t seqno, int interruptible)
 
         if (ret && ret != -ERESTARTSYS)
                 DRM_ERROR("%s returns %d (awaiting %d at %d)\n",
-                          __func__, ret, seqno, i915_get_gem_seqno(dev));
+                          __func__, ret, seqno, ring->get_gem_seqno(dev, ring));
 
         /* Directly dispatch request retiring.  While we have the work queue
          * to handle this, the waiter on a request often wants an associated
@@ -1918,7 +1832,7 @@ i915_do_wait_request(struct drm_device *dev, uint32_t seqno, int interruptible)
          * a separate wait queue to handle that.
          */
         if (ret == 0)
-                i915_gem_retire_requests(dev);
+                i915_gem_retire_requests(dev, ring);
 
         return ret;
 }
@@ -1928,9 +1842,10 @@ i915_do_wait_request(struct drm_device *dev, uint32_t seqno, int interruptible)
  * request and object lists appropriately for that event.
  */
 static int
-i915_wait_request(struct drm_device *dev, uint32_t seqno)
+i915_wait_request(struct drm_device *dev, uint32_t seqno,
+                struct intel_ring_buffer *ring)
 {
-        return i915_do_wait_request(dev, seqno, 1);
+        return i915_do_wait_request(dev, seqno, 1, ring);
 }
 
 static void
@@ -1939,71 +1854,29 @@ i915_gem_flush(struct drm_device *dev,
                uint32_t flush_domains)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
-        uint32_t cmd;
-        RING_LOCALS;
-
-#if WATCH_EXEC
-        DRM_INFO("%s: invalidate %08x flush %08x\n", __func__,
-                  invalidate_domains, flush_domains);
-#endif
-        trace_i915_gem_request_flush(dev, dev_priv->mm.next_gem_seqno,
-                                     invalidate_domains, flush_domains);
-
         if (flush_domains & I915_GEM_DOMAIN_CPU)
                 drm_agp_chipset_flush(dev);
+        dev_priv->render_ring.flush(dev, &dev_priv->render_ring,
+                        invalidate_domains,
+                        flush_domains);
+
+        if (HAS_BSD(dev))
+                dev_priv->bsd_ring.flush(dev, &dev_priv->bsd_ring,
+                                invalidate_domains,
+                                flush_domains);
+}
 
-        if ((invalidate_domains | flush_domains) & I915_GEM_GPU_DOMAINS) {
-                /*
-                 * read/write caches:
-                 *
-                 * I915_GEM_DOMAIN_RENDER is always invalidated, but is
-                 * only flushed if MI_NO_WRITE_FLUSH is unset.  On 965, it is
-                 * also flushed at 2d versus 3d pipeline switches.
-                 *
-                 * read-only caches:
-                 *
-                 * I915_GEM_DOMAIN_SAMPLER is flushed on pre-965 if
-                 * MI_READ_FLUSH is set, and is always flushed on 965.
-                 *
-                 * I915_GEM_DOMAIN_COMMAND may not exist?
-                 *
-                 * I915_GEM_DOMAIN_INSTRUCTION, which exists on 965, is
-                 * invalidated when MI_EXE_FLUSH is set.
-                 *
-                 * I915_GEM_DOMAIN_VERTEX, which exists on 965, is
-                 * invalidated with every MI_FLUSH.
-                 *
-                 * TLBs:
-                 *
-                 * On 965, TLBs associated with I915_GEM_DOMAIN_COMMAND
-                 * and I915_GEM_DOMAIN_CPU in are invalidated at PTE write and
-                 * I915_GEM_DOMAIN_RENDER and I915_GEM_DOMAIN_SAMPLER
-                 * are flushed at any MI_FLUSH.
-                 */
-
-                cmd = MI_FLUSH | MI_NO_WRITE_FLUSH;
-                if ((invalidate_domains|flush_domains) &
-                    I915_GEM_DOMAIN_RENDER)
-                        cmd &= ~MI_NO_WRITE_FLUSH;
-                if (!IS_I965G(dev)) {
-                        /*
-                         * On the 965, the sampler cache always gets flushed
-                         * and this bit is reserved.
-                         */
-                        if (invalidate_domains & I915_GEM_DOMAIN_SAMPLER)
-                                cmd |= MI_READ_FLUSH;
-                }
-                if (invalidate_domains & I915_GEM_DOMAIN_INSTRUCTION)
-                        cmd |= MI_EXE_FLUSH;
-
-#if WATCH_EXEC
-                DRM_INFO("%s: queue flush %08x to ring\n", __func__, cmd);
-#endif
-                BEGIN_LP_RING(2);
-                OUT_RING(cmd);
-                OUT_RING(MI_NOOP);
-                ADVANCE_LP_RING();
-        }
+static void
+i915_gem_flush_ring(struct drm_device *dev,
+               uint32_t invalidate_domains,
+               uint32_t flush_domains,
+               struct intel_ring_buffer *ring)
+{
+        if (flush_domains & I915_GEM_DOMAIN_CPU)
+                drm_agp_chipset_flush(dev);
+        ring->flush(dev, ring,
+                        invalidate_domains,
+                        flush_domains);
 }
 
 /**
@@ -2030,7 +1903,8 @@ i915_gem_object_wait_rendering(struct drm_gem_object *obj)
                 DRM_INFO("%s: object %p wait for seqno %08x\n",
                           __func__, obj, obj_priv->last_rendering_seqno);
 #endif
-                ret = i915_wait_request(dev, obj_priv->last_rendering_seqno);
+                ret = i915_wait_request(dev,
+                                obj_priv->last_rendering_seqno, obj_priv->ring);
                 if (ret != 0)
                         return ret;
         }
@@ -2146,11 +2020,14 @@ i915_gpu_idle(struct drm_device *dev)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
         bool lists_empty;
-        uint32_t seqno;
+        uint32_t seqno1, seqno2;
+        int ret;
 
         spin_lock(&dev_priv->mm.active_list_lock);
-        lists_empty = list_empty(&dev_priv->mm.flushing_list) &&
-                      list_empty(&dev_priv->mm.active_list);
+        lists_empty = (list_empty(&dev_priv->mm.flushing_list) &&
+                       list_empty(&dev_priv->render_ring.active_list) &&
+                       (!HAS_BSD(dev) ||
+                        list_empty(&dev_priv->bsd_ring.active_list)));
         spin_unlock(&dev_priv->mm.active_list_lock);
 
         if (lists_empty)
@@ -2158,11 +2035,25 @@ i915_gpu_idle(struct drm_device *dev)
 
         /* Flush everything onto the inactive list. */
         i915_gem_flush(dev, I915_GEM_GPU_DOMAINS, I915_GEM_GPU_DOMAINS);
-        seqno = i915_add_request(dev, NULL, I915_GEM_GPU_DOMAINS);
-        if (seqno == 0)
+        seqno1 = i915_add_request(dev, NULL, I915_GEM_GPU_DOMAINS,
+                        &dev_priv->render_ring);
+        if (seqno1 == 0)
                 return -ENOMEM;
+        ret = i915_wait_request(dev, seqno1, &dev_priv->render_ring);
+
+        if (HAS_BSD(dev)) {
+                seqno2 = i915_add_request(dev, NULL, I915_GEM_GPU_DOMAINS,
+                                &dev_priv->bsd_ring);
+                if (seqno2 == 0)
+                        return -ENOMEM;
+
+                ret = i915_wait_request(dev, seqno2, &dev_priv->bsd_ring);
+                if (ret)
+                        return ret;
+        }
+
 
-        return i915_wait_request(dev, seqno);
+        return ret;
 }
 
 static int
@@ -2175,7 +2066,9 @@ i915_gem_evict_everything(struct drm_device *dev)
         spin_lock(&dev_priv->mm.active_list_lock);
         lists_empty = (list_empty(&dev_priv->mm.inactive_list) &&
                        list_empty(&dev_priv->mm.flushing_list) &&
-                       list_empty(&dev_priv->mm.active_list));
+                       list_empty(&dev_priv->render_ring.active_list) &&
+                       (!HAS_BSD(dev)
+                        || list_empty(&dev_priv->bsd_ring.active_list)));
         spin_unlock(&dev_priv->mm.active_list_lock);
 
         if (lists_empty)
@@ -2195,7 +2088,9 @@ i915_gem_evict_everything(struct drm_device *dev)
         spin_lock(&dev_priv->mm.active_list_lock);
         lists_empty = (list_empty(&dev_priv->mm.inactive_list) &&
                        list_empty(&dev_priv->mm.flushing_list) &&
-                       list_empty(&dev_priv->mm.active_list));
+                       list_empty(&dev_priv->render_ring.active_list) &&
+                       (!HAS_BSD(dev)
+                        || list_empty(&dev_priv->bsd_ring.active_list)));
         spin_unlock(&dev_priv->mm.active_list_lock);
         BUG_ON(!lists_empty);
 
@@ -2209,8 +2104,13 @@ i915_gem_evict_something(struct drm_device *dev, int min_size)
         struct drm_gem_object *obj;
         int ret;
 
+        struct intel_ring_buffer *render_ring = &dev_priv->render_ring;
+        struct intel_ring_buffer *bsd_ring = &dev_priv->bsd_ring;
         for (;;) {
-                i915_gem_retire_requests(dev);
+                i915_gem_retire_requests(dev, render_ring);
+
+                if (HAS_BSD(dev))
+                        i915_gem_retire_requests(dev, bsd_ring);
 
                 /* If there's an inactive buffer available now, grab it
                  * and be done.
@@ -2234,14 +2134,30 @@ i915_gem_evict_something(struct drm_device *dev, int min_size)
                  * things, wait for the next to finish and hopefully leave us
                  * a buffer to evict.
                  */
-                if (!list_empty(&dev_priv->mm.request_list)) {
+                if (!list_empty(&render_ring->request_list)) {
+                        struct drm_i915_gem_request *request;
+
+                        request = list_first_entry(&render_ring->request_list,
+                                                   struct drm_i915_gem_request,
+                                                   list);
+
+                        ret = i915_wait_request(dev,
+                                        request->seqno, request->ring);
+                        if (ret)
+                                return ret;
+
+                        continue;
+                }
+
+                if (HAS_BSD(dev) && !list_empty(&bsd_ring->request_list)) {
                         struct drm_i915_gem_request *request;
 
-                        request = list_first_entry(&dev_priv->mm.request_list,
+                        request = list_first_entry(&bsd_ring->request_list,
                                                    struct drm_i915_gem_request,
                                                    list);
 
-                        ret = i915_wait_request(dev, request->seqno);
+                        ret = i915_wait_request(dev,
+                                        request->seqno, request->ring);
                         if (ret)
                                 return ret;
 
@@ -2268,10 +2184,13 @@ i915_gem_evict_something(struct drm_device *dev, int min_size)
                         if (obj != NULL) {
                                 uint32_t seqno;
 
-                                i915_gem_flush(dev,
+                                i915_gem_flush_ring(dev,
+                                               obj->write_domain,
                                                obj->write_domain,
-                                               obj->write_domain);
-                                seqno = i915_add_request(dev, NULL, obj->write_domain);
+                                               obj_priv->ring);
+                                seqno = i915_add_request(dev, NULL,
+                                                obj->write_domain,
+                                                obj_priv->ring);
                                 if (seqno == 0)
                                         return -ENOMEM;
                                 continue;
@@ -2299,6 +2218,9 @@ i915_gem_object_get_pages(struct drm_gem_object *obj,
         struct inode *inode;
         struct page *page;
 
+        BUG_ON(obj_priv->pages_refcount
+                        == DRM_I915_GEM_OBJECT_MAX_PAGES_REFCOUNT);
+
         if (obj_priv->pages_refcount++ != 0)
                 return 0;
 
@@ -2697,6 +2619,14 @@ i915_gem_object_bind_to_gtt(struct drm_gem_object *obj, unsigned alignment)
                 return -EINVAL;
         }
 
+        /* If the object is bigger than the entire aperture, reject it early
+         * before evicting everything in a vain attempt to find space.
+         */
+        if (obj->size > dev->gtt_total) {
+                DRM_ERROR("Attempting to bind an object larger than the aperture\n");
+                return -E2BIG;
+        }
+
  search_free:
         free_space = drm_mm_search_free(&dev_priv->mm.gtt_space,
                                         obj->size, alignment, 0);
@@ -2807,6 +2737,7 @@ i915_gem_object_flush_gpu_write_domain(struct drm_gem_object *obj)
 {
         struct drm_device *dev = obj->dev;
         uint32_t old_write_domain;
+        struct drm_i915_gem_object *obj_priv = to_intel_bo(obj);
 
         if ((obj->write_domain & I915_GEM_GPU_DOMAINS) == 0)
                 return;
@@ -2814,7 +2745,7 @@ i915_gem_object_flush_gpu_write_domain(struct drm_gem_object *obj)
         /* Queue the GPU write cache flushing we need. */
         old_write_domain = obj->write_domain;
         i915_gem_flush(dev, 0, obj->write_domain);
-        (void) i915_add_request(dev, NULL, obj->write_domain);
+        (void) i915_add_request(dev, NULL, obj->write_domain, obj_priv->ring);
         BUG_ON(obj->write_domain);
 
         trace_i915_gem_object_change_domain(obj,
@@ -2954,23 +2885,24 @@ i915_gem_object_set_to_display_plane(struct drm_gem_object *obj)
                 DRM_INFO("%s: object %p wait for seqno %08x\n",
                           __func__, obj, obj_priv->last_rendering_seqno);
 #endif
-                ret = i915_do_wait_request(dev, obj_priv->last_rendering_seqno, 0);
+                ret = i915_do_wait_request(dev,
+                                obj_priv->last_rendering_seqno,
+                                0,
+                                obj_priv->ring);
                 if (ret != 0)
                         return ret;
         }
 
+        i915_gem_object_flush_cpu_write_domain(obj);
+
         old_write_domain = obj->write_domain;
         old_read_domains = obj->read_domains;
 
-        obj->read_domains &= I915_GEM_DOMAIN_GTT;
-
-        i915_gem_object_flush_cpu_write_domain(obj);
-
         /* It should now be out of any other write domains, and we can update
          * the domain values for our changes.
          */
         BUG_ON((obj->write_domain & ~I915_GEM_DOMAIN_GTT) != 0);
-        obj->read_domains |= I915_GEM_DOMAIN_GTT;
+        obj->read_domains = I915_GEM_DOMAIN_GTT;
         obj->write_domain = I915_GEM_DOMAIN_GTT;
         obj_priv->dirty = 1;
 
@@ -3354,9 +3286,13 @@ i915_gem_object_pin_and_relocate(struct drm_gem_object *obj,
                      obj_priv->tiling_mode != I915_TILING_NONE;
 
         /* Check fence reg constraints and rebind if necessary */
-        if (need_fence && !i915_gem_object_fence_offset_ok(obj,
-            obj_priv->tiling_mode))
-                i915_gem_object_unbind(obj);
+        if (need_fence &&
+            !i915_gem_object_fence_offset_ok(obj,
+                                             obj_priv->tiling_mode)) {
+                ret = i915_gem_object_unbind(obj);
+                if (ret)
+                        return ret;
+        }
 
         /* Choose the GTT offset for our buffer and put it there. */
         ret = i915_gem_object_pin(obj, (uint32_t) entry->alignment);
@@ -3370,9 +3306,6 @@ i915_gem_object_pin_and_relocate(struct drm_gem_object *obj,
         if (need_fence) {
                 ret = i915_gem_object_get_fence_reg(obj);
                 if (ret != 0) {
-                        if (ret != -EBUSY && ret != -ERESTARTSYS)
-                                DRM_ERROR("Failure to install fence: %d\n",
-                                          ret);
                         i915_gem_object_unpin(obj);
                         return ret;
                 }
@@ -3545,62 +3478,6 @@ i915_gem_object_pin_and_relocate(struct drm_gem_object *obj,
         return 0;
 }
 
-/** Dispatch a batchbuffer to the ring
- */
-static int
-i915_dispatch_gem_execbuffer(struct drm_device *dev,
-                              struct drm_i915_gem_execbuffer2 *exec,
-                              struct drm_clip_rect *cliprects,
-                              uint64_t exec_offset)
-{
-        drm_i915_private_t *dev_priv = dev->dev_private;
-        int nbox = exec->num_cliprects;
-        int i = 0, count;
-        uint32_t exec_start, exec_len;
-        RING_LOCALS;
-
-        exec_start = (uint32_t) exec_offset + exec->batch_start_offset;
-        exec_len = (uint32_t) exec->batch_len;
-
-        trace_i915_gem_request_submit(dev, dev_priv->mm.next_gem_seqno + 1);
-
-        count = nbox ? nbox : 1;
-
-        for (i = 0; i < count; i++) {
-                if (i < nbox) {
-                        int ret = i915_emit_box(dev, cliprects, i,
-                                                exec->DR1, exec->DR4);
-                        if (ret)
-                                return ret;
-                }
-
-                if (IS_I830(dev) || IS_845G(dev)) {
-                        BEGIN_LP_RING(4);
-                        OUT_RING(MI_BATCH_BUFFER);
-                        OUT_RING(exec_start | MI_BATCH_NON_SECURE);
-                        OUT_RING(exec_start + exec_len - 4);
-                        OUT_RING(0);
-                        ADVANCE_LP_RING();
-                } else {
-                        BEGIN_LP_RING(2);
-                        if (IS_I965G(dev)) {
-                                OUT_RING(MI_BATCH_BUFFER_START |
-                                         (2 << 6) |
-                                         MI_BATCH_NON_SECURE_I965);
-                                OUT_RING(exec_start);
-                        } else {
-                                OUT_RING(MI_BATCH_BUFFER_START |
-                                         (2 << 6));
-                                OUT_RING(exec_start | MI_BATCH_NON_SECURE);
-                        }
-                        ADVANCE_LP_RING();
-                }
-        }
-
-        /* XXX breadcrumb */
-        return 0;
-}
-
 /* Throttle our rendering by waiting until the ring has completed our requests
  * emitted over 20 msec ago.
  *
@@ -3629,7 +3506,7 @@ i915_gem_ring_throttle(struct drm_device *dev, struct drm_file *file_priv)
                 if (time_after_eq(request->emitted_jiffies, recent_enough))
                         break;
 
-                ret = i915_wait_request(dev, request->seqno);
+                ret = i915_wait_request(dev, request->seqno, request->ring);
                 if (ret != 0)
                         break;
         }
@@ -3786,10 +3663,22 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
         uint32_t seqno, flush_domains, reloc_index;
         int pin_tries, flips;
 
+        struct intel_ring_buffer *ring = NULL;
+
 #if WATCH_EXEC
         DRM_INFO("buffers_ptr %d buffer_count %d len %08x\n",
                   (int) args->buffers_ptr, args->buffer_count, args->batch_len);
 #endif
+        if (args->flags & I915_EXEC_BSD) {
+                if (!HAS_BSD(dev)) {
+                        DRM_ERROR("execbuf with wrong flag\n");
+                        return -EINVAL;
+                }
+                ring = &dev_priv->bsd_ring;
+        } else {
+                ring = &dev_priv->render_ring;
+        }
+
 
         if (args->buffer_count < 1) {
                 DRM_ERROR("execbuf with %d buffers\n", args->buffer_count);
@@ -3902,11 +3791,19 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
                 if (ret != -ENOSPC || pin_tries >= 1) {
                         if (ret != -ERESTARTSYS) {
                                 unsigned long long total_size = 0;
-                                for (i = 0; i < args->buffer_count; i++)
+                                int num_fences = 0;
+                                for (i = 0; i < args->buffer_count; i++) {
+                                        obj_priv = object_list[i]->driver_private;
+
                                         total_size += object_list[i]->size;
-                                DRM_ERROR("Failed to pin buffer %d of %d, total %llu bytes: %d\n",
+                                        num_fences +=
+                                                exec_list[i].flags & EXEC_OBJECT_NEEDS_FENCE &&
+                                                obj_priv->tiling_mode != I915_TILING_NONE;
+                                }
+                                DRM_ERROR("Failed to pin buffer %d of %d, total %llu bytes, %d fences: %d\n",
                                           pinned+1, args->buffer_count,
-                                          total_size, ret);
+                                          total_size, num_fences,
+                                          ret);
                                 DRM_ERROR("%d objects [%d pinned], "
                                           "%d object bytes [%d pinned], "
                                           "%d/%d gtt bytes\n",
@@ -3976,9 +3873,16 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
                 i915_gem_flush(dev,
                                dev->invalidate_domains,
                                dev->flush_domains);
-                if (dev->flush_domains & I915_GEM_GPU_DOMAINS)
+                if (dev->flush_domains & I915_GEM_GPU_DOMAINS) {
                         (void)i915_add_request(dev, file_priv,
-                                               dev->flush_domains);
+                                        dev->flush_domains,
+                                        &dev_priv->render_ring);
+
+                        if (HAS_BSD(dev))
+                                (void)i915_add_request(dev, file_priv,
+                                                dev->flush_domains,
+                                                &dev_priv->bsd_ring);
+                }
         }
 
         for (i = 0; i < args->buffer_count; i++) {
@@ -4015,7 +3919,8 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 #endif
 
         /* Exec the batchbuffer */
-        ret = i915_dispatch_gem_execbuffer(dev, args, cliprects, exec_offset);
+        ret = ring->dispatch_gem_execbuffer(dev, ring, args,
+                        cliprects, exec_offset);
         if (ret) {
                 DRM_ERROR("dispatch failed %d\n", ret);
                 goto err;
@@ -4025,7 +3930,7 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
          * Ensure that the commands in the batch buffer are
          * finished before the interrupt fires
          */
-        flush_domains = i915_retire_commands(dev);
+        flush_domains = i915_retire_commands(dev, ring);
 
         i915_verify_inactive(dev, __FILE__, __LINE__);
 
@@ -4036,12 +3941,13 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
          * *some* interrupts representing completion of buffers that we can
          * wait on when trying to clear up gtt space).
          */
-        seqno = i915_add_request(dev, file_priv, flush_domains);
+        seqno = i915_add_request(dev, file_priv, flush_domains, ring);
         BUG_ON(seqno == 0);
         for (i = 0; i < args->buffer_count; i++) {
                 struct drm_gem_object *obj = object_list[i];
+                obj_priv = to_intel_bo(obj);
 
-                i915_gem_object_move_to_active(obj, seqno);
+                i915_gem_object_move_to_active(obj, seqno, ring);
 #if WATCH_LRU
                 DRM_INFO("%s: move to exec list %p\n", __func__, obj);
 #endif
@@ -4153,7 +4059,7 @@ i915_gem_execbuffer(struct drm_device *dev, void *data,
         exec2.DR4 = args->DR4;
         exec2.num_cliprects = args->num_cliprects;
         exec2.cliprects_ptr = args->cliprects_ptr;
-        exec2.flags = 0;
+        exec2.flags = I915_EXEC_RENDER;
 
         ret = i915_gem_do_execbuffer(dev, data, file_priv, &exec2, exec2_list);
         if (!ret) {
@@ -4239,7 +4145,20 @@ i915_gem_object_pin(struct drm_gem_object *obj, uint32_t alignment)
         struct drm_i915_gem_object *obj_priv = to_intel_bo(obj);
         int ret;
 
+        BUG_ON(obj_priv->pin_count == DRM_I915_GEM_OBJECT_MAX_PIN_COUNT);
+
         i915_verify_inactive(dev, __FILE__, __LINE__);
+
+        if (obj_priv->gtt_space != NULL) {
+                if (alignment == 0)
+                        alignment = i915_gem_get_gtt_alignment(obj);
+                if (obj_priv->gtt_offset & (alignment - 1)) {
+                        ret = i915_gem_object_unbind(obj);
+                        if (ret)
+                                return ret;
+                }
+        }
+
         if (obj_priv->gtt_space == NULL) {
                 ret = i915_gem_object_bind_to_gtt(obj, alignment);
                 if (ret)
@@ -4392,6 +4311,7 @@ i915_gem_busy_ioctl(struct drm_device *dev, void *data,
         struct drm_i915_gem_busy *args = data;
         struct drm_gem_object *obj;
         struct drm_i915_gem_object *obj_priv;
+        drm_i915_private_t *dev_priv = dev->dev_private;
 
         obj = drm_gem_object_lookup(dev, file_priv, args->handle);
         if (obj == NULL) {
@@ -4406,7 +4326,10 @@ i915_gem_busy_ioctl(struct drm_device *dev, void *data,
          * actually unmasked, and our working set ends up being larger than
          * required.
          */
-        i915_gem_retire_requests(dev);
+        i915_gem_retire_requests(dev, &dev_priv->render_ring);
+
+        if (HAS_BSD(dev))
+                i915_gem_retire_requests(dev, &dev_priv->bsd_ring);
 
         obj_priv = to_intel_bo(obj);
         /* Don't count being on the flushing list against the object being
@@ -4573,7 +4496,10 @@ i915_gem_idle(struct drm_device *dev)
 
         mutex_lock(&dev->struct_mutex);
 
-        if (dev_priv->mm.suspended || dev_priv->ring.ring_obj == NULL) {
+        if (dev_priv->mm.suspended ||
+                        (dev_priv->render_ring.gem_object == NULL) ||
+                        (HAS_BSD(dev) &&
+                         dev_priv->bsd_ring.gem_object == NULL)) {
                 mutex_unlock(&dev->struct_mutex);
                 return 0;
         }
@@ -4654,71 +4580,6 @@ err:
         return ret;
 }
 
-static int
-i915_gem_init_hws(struct drm_device *dev)
-{
-        drm_i915_private_t *dev_priv = dev->dev_private;
-        struct drm_gem_object *obj;
-        struct drm_i915_gem_object *obj_priv;
-        int ret;
-
-        /* If we need a physical address for the status page, it's already
-         * initialized at driver load time.
-         */
-        if (!I915_NEED_GFX_HWS(dev))
-                return 0;
-
-        obj = i915_gem_alloc_object(dev, 4096);
-        if (obj == NULL) {
-                DRM_ERROR("Failed to allocate status page\n");
-                ret = -ENOMEM;
-                goto err;
-        }
-        obj_priv = to_intel_bo(obj);
-        obj_priv->agp_type = AGP_USER_CACHED_MEMORY;
-
-        ret = i915_gem_object_pin(obj, 4096);
-        if (ret != 0) {
-                drm_gem_object_unreference(obj);
-                goto err_unref;
-        }
-
-        dev_priv->status_gfx_addr = obj_priv->gtt_offset;
-
-        dev_priv->hw_status_page = kmap(obj_priv->pages[0]);
-        if (dev_priv->hw_status_page == NULL) {
-                DRM_ERROR("Failed to map status page.\n");
-                memset(&dev_priv->hws_map, 0, sizeof(dev_priv->hws_map));
-                ret = -EINVAL;
-                goto err_unpin;
-        }
-
-        if (HAS_PIPE_CONTROL(dev)) {
-                ret = i915_gem_init_pipe_control(dev);
-                if (ret)
-                        goto err_unpin;
-        }
-
-        dev_priv->hws_obj = obj;
-        memset(dev_priv->hw_status_page, 0, PAGE_SIZE);
-        if (IS_GEN6(dev)) {
-                I915_WRITE(HWS_PGA_GEN6, dev_priv->status_gfx_addr);
-                I915_READ(HWS_PGA_GEN6); /* posting read */
-        } else {
-                I915_WRITE(HWS_PGA, dev_priv->status_gfx_addr);
-                I915_READ(HWS_PGA); /* posting read */
-        }
-        DRM_DEBUG_DRIVER("hws offset: 0x%08x\n", dev_priv->status_gfx_addr);
-
-        return 0;
-
-err_unpin:
-        i915_gem_object_unpin(obj);
-err_unref:
-        drm_gem_object_unreference(obj);
-err:
-        return 0;
-}
 
 static void
 i915_gem_cleanup_pipe_control(struct drm_device *dev)
@@ -4737,146 +4598,46 @@ i915_gem_cleanup_pipe_control(struct drm_device *dev)
         dev_priv->seqno_page = NULL;
 }
 
-static void
-i915_gem_cleanup_hws(struct drm_device *dev)
-{
-        drm_i915_private_t *dev_priv = dev->dev_private;
-        struct drm_gem_object *obj;
-        struct drm_i915_gem_object *obj_priv;
-
-        if (dev_priv->hws_obj == NULL)
-                return;
-
-        obj = dev_priv->hws_obj;
-        obj_priv = to_intel_bo(obj);
-
-        kunmap(obj_priv->pages[0]);
-        i915_gem_object_unpin(obj);
-        drm_gem_object_unreference(obj);
-        dev_priv->hws_obj = NULL;
-
-        memset(&dev_priv->hws_map, 0, sizeof(dev_priv->hws_map));
-        dev_priv->hw_status_page = NULL;
-
-        if (HAS_PIPE_CONTROL(dev))
-                i915_gem_cleanup_pipe_control(dev);
-
-        /* Write high address into HWS_PGA when disabling. */
-        I915_WRITE(HWS_PGA, 0x1ffff000);
-}
-
 int
 i915_gem_init_ringbuffer(struct drm_device *dev)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
-        struct drm_gem_object *obj;
-        struct drm_i915_gem_object *obj_priv;
-        drm_i915_ring_buffer_t *ring = &dev_priv->ring;
         int ret;
-        u32 head;
-
-        ret = i915_gem_init_hws(dev);
-        if (ret != 0)
-                return ret;
 
-        obj = i915_gem_alloc_object(dev, 128 * 1024);
-        if (obj == NULL) {
-                DRM_ERROR("Failed to allocate ringbuffer\n");
-                i915_gem_cleanup_hws(dev);
-                return -ENOMEM;
-        }
-        obj_priv = to_intel_bo(obj);
+        dev_priv->render_ring = render_ring;
 
-        ret = i915_gem_object_pin(obj, 4096);
-        if (ret != 0) {
-                drm_gem_object_unreference(obj);
-                i915_gem_cleanup_hws(dev);
-                return ret;
+        if (!I915_NEED_GFX_HWS(dev)) {
+                dev_priv->render_ring.status_page.page_addr
+                        = dev_priv->status_page_dmah->vaddr;
+                memset(dev_priv->render_ring.status_page.page_addr,
+                                0, PAGE_SIZE);
         }
 
-        /* Set up the kernel mapping for the ring. */
-        ring->Size = obj->size;
-
-        ring->map.offset = dev->agp->base + obj_priv->gtt_offset;
-        ring->map.size = obj->size;
-        ring->map.type = 0;
-        ring->map.flags = 0;
-        ring->map.mtrr = 0;
-
-        drm_core_ioremap_wc(&ring->map, dev);
-        if (ring->map.handle == NULL) {
-                DRM_ERROR("Failed to map ringbuffer.\n");
-                memset(&dev_priv->ring, 0, sizeof(dev_priv->ring));
-                i915_gem_object_unpin(obj);
-                drm_gem_object_unreference(obj);
-                i915_gem_cleanup_hws(dev);
-                return -EINVAL;
-        }
-        ring->ring_obj = obj;
-        ring->virtual_start = ring->map.handle;
-
-        /* Stop the ring if it's running. */
-        I915_WRITE(PRB0_CTL, 0);
-        I915_WRITE(PRB0_TAIL, 0);
-        I915_WRITE(PRB0_HEAD, 0);
-
-        /* Initialize the ring. */
-        I915_WRITE(PRB0_START, obj_priv->gtt_offset);
-        head = I915_READ(PRB0_HEAD) & HEAD_ADDR;
-
-        /* G45 ring initialization fails to reset head to zero */
-        if (head != 0) {
-                DRM_ERROR("Ring head not reset to zero "
-                          "ctl %08x head %08x tail %08x start %08x\n",
-                          I915_READ(PRB0_CTL),
-                          I915_READ(PRB0_HEAD),
-                          I915_READ(PRB0_TAIL),
-                          I915_READ(PRB0_START));
-                I915_WRITE(PRB0_HEAD, 0);
-
-                DRM_ERROR("Ring head forced to zero "
-                          "ctl %08x head %08x tail %08x start %08x\n",
-                          I915_READ(PRB0_CTL),
-                          I915_READ(PRB0_HEAD),
-                          I915_READ(PRB0_TAIL),
-                          I915_READ(PRB0_START));
-        }
-
-        I915_WRITE(PRB0_CTL,
-                   ((obj->size - 4096) & RING_NR_PAGES) |
-                   RING_NO_REPORT |
-                   RING_VALID);
-
-        head = I915_READ(PRB0_HEAD) & HEAD_ADDR;
-
-        /* If the head is still not zero, the ring is dead */
-        if (head != 0) {
-                DRM_ERROR("Ring initialization failed "
-                          "ctl %08x head %08x tail %08x start %08x\n",
-                          I915_READ(PRB0_CTL),
-                          I915_READ(PRB0_HEAD),
-                          I915_READ(PRB0_TAIL),
-                          I915_READ(PRB0_START));
-                return -EIO;
+        if (HAS_PIPE_CONTROL(dev)) {
+                ret = i915_gem_init_pipe_control(dev);
+                if (ret)
+                        return ret;
         }
 
-        /* Update our cache of the ring state */
-        if (!drm_core_check_feature(dev, DRIVER_MODESET))
-                i915_kernel_lost_context(dev);
-        else {
-                ring->head = I915_READ(PRB0_HEAD) & HEAD_ADDR;
-                ring->tail = I915_READ(PRB0_TAIL) & TAIL_ADDR;
-                ring->space = ring->head - (ring->tail + 8);
-                if (ring->space < 0)
-                        ring->space += ring->Size;
-        }
+        ret = intel_init_ring_buffer(dev, &dev_priv->render_ring);
+        if (ret)
+                goto cleanup_pipe_control;
 
-        if (IS_I9XX(dev) && !IS_GEN3(dev)) {
-                I915_WRITE(MI_MODE,
-                           (VS_TIMER_DISPATCH) << 16 | VS_TIMER_DISPATCH);
+        if (HAS_BSD(dev)) {
+                dev_priv->bsd_ring = bsd_ring;
+                ret = intel_init_ring_buffer(dev, &dev_priv->bsd_ring);
+                if (ret)
+                        goto cleanup_render_ring;
         }
 
         return 0;
+
+cleanup_render_ring:
+        intel_cleanup_ring_buffer(dev, &dev_priv->render_ring);
+cleanup_pipe_control:
+        if (HAS_PIPE_CONTROL(dev))
+                i915_gem_cleanup_pipe_control(dev);
+        return ret;
 }
 
 void
@@ -4884,17 +4645,11 @@ i915_gem_cleanup_ringbuffer(struct drm_device *dev)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
 
-        if (dev_priv->ring.ring_obj == NULL)
-                return;
-
-        drm_core_ioremapfree(&dev_priv->ring.map, dev);
-
-        i915_gem_object_unpin(dev_priv->ring.ring_obj);
-        drm_gem_object_unreference(dev_priv->ring.ring_obj);
-        dev_priv->ring.ring_obj = NULL;
-        memset(&dev_priv->ring, 0, sizeof(dev_priv->ring));
-
-        i915_gem_cleanup_hws(dev);
+        intel_cleanup_ring_buffer(dev, &dev_priv->render_ring);
+        if (HAS_BSD(dev))
+                intel_cleanup_ring_buffer(dev, &dev_priv->bsd_ring);
+        if (HAS_PIPE_CONTROL(dev))
+                i915_gem_cleanup_pipe_control(dev);
 }
 
 int
@@ -4922,12 +4677,14 @@ i915_gem_entervt_ioctl(struct drm_device *dev, void *data,
         }
 
         spin_lock(&dev_priv->mm.active_list_lock);
-        BUG_ON(!list_empty(&dev_priv->mm.active_list));
+        BUG_ON(!list_empty(&dev_priv->render_ring.active_list));
+        BUG_ON(HAS_BSD(dev) && !list_empty(&dev_priv->bsd_ring.active_list));
         spin_unlock(&dev_priv->mm.active_list_lock);
 
         BUG_ON(!list_empty(&dev_priv->mm.flushing_list));
         BUG_ON(!list_empty(&dev_priv->mm.inactive_list));
-        BUG_ON(!list_empty(&dev_priv->mm.request_list));
+        BUG_ON(!list_empty(&dev_priv->render_ring.request_list));
+        BUG_ON(HAS_BSD(dev) && !list_empty(&dev_priv->bsd_ring.request_list));
         mutex_unlock(&dev->struct_mutex);
 
         drm_irq_install(dev);
@@ -4966,18 +4723,20 @@ i915_gem_load(struct drm_device *dev)
         drm_i915_private_t *dev_priv = dev->dev_private;
 
         spin_lock_init(&dev_priv->mm.active_list_lock);
-        INIT_LIST_HEAD(&dev_priv->mm.active_list);
         INIT_LIST_HEAD(&dev_priv->mm.flushing_list);
         INIT_LIST_HEAD(&dev_priv->mm.gpu_write_list);
         INIT_LIST_HEAD(&dev_priv->mm.inactive_list);
-        INIT_LIST_HEAD(&dev_priv->mm.request_list);
         INIT_LIST_HEAD(&dev_priv->mm.fence_list);
+        INIT_LIST_HEAD(&dev_priv->render_ring.active_list);
+        INIT_LIST_HEAD(&dev_priv->render_ring.request_list);
+        if (HAS_BSD(dev)) {
+                INIT_LIST_HEAD(&dev_priv->bsd_ring.active_list);
+                INIT_LIST_HEAD(&dev_priv->bsd_ring.request_list);
+        }
         for (i = 0; i < 16; i++)
                 INIT_LIST_HEAD(&dev_priv->fence_regs[i].lru_list);
         INIT_DELAYED_WORK(&dev_priv->mm.retire_work,
                           i915_gem_retire_work_handler);
-        dev_priv->mm.next_gem_seqno = 1;
-
         spin_lock(&shrink_list_lock);
         list_add(&dev_priv->mm.shrink_list, &shrink_list);
         spin_unlock(&shrink_list_lock);
@@ -5209,7 +4968,9 @@ i915_gpu_is_active(struct drm_device *dev)
 
         spin_lock(&dev_priv->mm.active_list_lock);
         lists_empty = list_empty(&dev_priv->mm.flushing_list) &&
-                      list_empty(&dev_priv->mm.active_list);
+                      list_empty(&dev_priv->render_ring.active_list);
+        if (HAS_BSD(dev))
+                lists_empty &= list_empty(&dev_priv->bsd_ring.active_list);
         spin_unlock(&dev_priv->mm.active_list_lock);
 
         return !lists_empty;
@@ -5254,8 +5015,10 @@ rescan:
                         continue;
 
                 spin_unlock(&shrink_list_lock);
+                i915_gem_retire_requests(dev, &dev_priv->render_ring);
 
-                i915_gem_retire_requests(dev);
+                if (HAS_BSD(dev))
+                        i915_gem_retire_requests(dev, &dev_priv->bsd_ring);
 
                 list_for_each_entry_safe(obj_priv, next_obj,
                                          &dev_priv->mm.inactive_list,
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index 8c3f0802686d..2479be001e40 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -53,7 +53,7 @@
          I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT)
 
 /** Interrupts that we mask and unmask at runtime. */
-#define I915_INTERRUPT_ENABLE_VAR (I915_USER_INTERRUPT)
+#define I915_INTERRUPT_ENABLE_VAR (I915_USER_INTERRUPT | I915_BSD_USER_INTERRUPT)
 
 #define I915_PIPE_VBLANK_STATUS (PIPE_START_VBLANK_INTERRUPT_STATUS |\
                                  PIPE_VBLANK_INTERRUPT_STATUS)
@@ -74,7 +74,7 @@ ironlake_enable_graphics_irq(drm_i915_private_t *dev_priv, u32 mask)
         }
 }
 
-static inline void
+void
 ironlake_disable_graphics_irq(drm_i915_private_t *dev_priv, u32 mask)
 {
         if ((dev_priv->gt_irq_mask_reg & mask) != mask) {
@@ -115,7 +115,7 @@ i915_enable_irq(drm_i915_private_t *dev_priv, u32 mask)
         }
 }
 
-static inline void
+void
 i915_disable_irq(drm_i915_private_t *dev_priv, u32 mask)
 {
         if ((dev_priv->irq_mask_reg & mask) != mask) {
@@ -278,10 +278,9 @@ static void i915_handle_rps_change(struct drm_device *dev)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
         u32 busy_up, busy_down, max_avg, min_avg;
-        u16 rgvswctl;
         u8 new_delay = dev_priv->cur_delay;
 
-        I915_WRITE(MEMINTRSTS, I915_READ(MEMINTRSTS) & ~MEMINT_EVAL_CHG);
+        I915_WRITE16(MEMINTRSTS, MEMINT_EVAL_CHG);
         busy_up = I915_READ(RCPREVBSYTUPAVG);
         busy_down = I915_READ(RCPREVBSYTDNAVG);
         max_avg = I915_READ(RCBMAXAVG);
@@ -300,27 +299,8 @@ static void i915_handle_rps_change(struct drm_device *dev)
                         new_delay = dev_priv->min_delay;
         }
 
-        DRM_DEBUG("rps change requested: %d -> %d\n",
-                  dev_priv->cur_delay, new_delay);
-
-        rgvswctl = I915_READ(MEMSWCTL);
-        if (rgvswctl & MEMCTL_CMD_STS) {
-                DRM_ERROR("gpu busy, RCS change rejected\n");
-                return; /* still busy with another command */
-        }
-
-        /* Program the new state */
-        rgvswctl = (MEMCTL_CMD_CHFREQ << MEMCTL_CMD_SHIFT) |
-                (new_delay << MEMCTL_FREQ_SHIFT) | MEMCTL_SFCAVM;
-        I915_WRITE(MEMSWCTL, rgvswctl);
-        POSTING_READ(MEMSWCTL);
-
-        rgvswctl |= MEMCTL_CMD_STS;
-        I915_WRITE(MEMSWCTL, rgvswctl);
-
-        dev_priv->cur_delay = new_delay;
-
-        DRM_DEBUG("rps changed\n");
+        if (ironlake_set_drps(dev, new_delay))
+                dev_priv->cur_delay = new_delay;
 
         return;
 }
@@ -331,6 +311,7 @@ irqreturn_t ironlake_irq_handler(struct drm_device *dev)
         int ret = IRQ_NONE;
         u32 de_iir, gt_iir, de_ier, pch_iir;
         struct drm_i915_master_private *master_priv;
+        struct intel_ring_buffer *render_ring = &dev_priv->render_ring;
 
         /* disable master interrupt before clearing iir  */
         de_ier = I915_READ(DEIER);
@@ -354,13 +335,16 @@ irqreturn_t ironlake_irq_handler(struct drm_device *dev)
         }
 
         if (gt_iir & GT_PIPE_NOTIFY) {
-                u32 seqno = i915_get_gem_seqno(dev);
-                dev_priv->mm.irq_gem_seqno = seqno;
+                u32 seqno = render_ring->get_gem_seqno(dev, render_ring);
+                render_ring->irq_gem_seqno = seqno;
                 trace_i915_gem_request_complete(dev, seqno);
-                DRM_WAKEUP(&dev_priv->irq_queue);
+                DRM_WAKEUP(&dev_priv->render_ring.irq_queue);
                 dev_priv->hangcheck_count = 0;
                 mod_timer(&dev_priv->hangcheck_timer, jiffies + DRM_I915_HANGCHECK_PERIOD);
         }
+        if (gt_iir & GT_BSD_USER_INTERRUPT)
+                DRM_WAKEUP(&dev_priv->bsd_ring.irq_queue);
+
 
         if (de_iir & DE_GSE)
                 ironlake_opregion_gse_intr(dev);
@@ -388,7 +372,7 @@ irqreturn_t ironlake_irq_handler(struct drm_device *dev)
         }
 
         if (de_iir & DE_PCU_EVENT) {
-                I915_WRITE(MEMINTRSTS, I915_READ(MEMINTRSTS));
+                I915_WRITE16(MEMINTRSTS, I915_READ(MEMINTRSTS));
                 i915_handle_rps_change(dev);
         }
 
@@ -536,17 +520,18 @@ i915_ringbuffer_last_batch(struct drm_device *dev)
          */
         bbaddr = 0;
         head = I915_READ(PRB0_HEAD) & HEAD_ADDR;
-        ring = (u32 *)(dev_priv->ring.virtual_start + head);
+        ring = (u32 *)(dev_priv->render_ring.virtual_start + head);
 
-        while (--ring >= (u32 *)dev_priv->ring.virtual_start) {
+        while (--ring >= (u32 *)dev_priv->render_ring.virtual_start) {
                 bbaddr = i915_get_bbaddr(dev, ring);
                 if (bbaddr)
                         break;
         }
 
         if (bbaddr == 0) {
-                ring = (u32 *)(dev_priv->ring.virtual_start + dev_priv->ring.Size);
-                while (--ring >= (u32 *)dev_priv->ring.virtual_start) {
+                ring = (u32 *)(dev_priv->render_ring.virtual_start
+                                + dev_priv->render_ring.size);
+                while (--ring >= (u32 *)dev_priv->render_ring.virtual_start) {
                         bbaddr = i915_get_bbaddr(dev, ring);
                         if (bbaddr)
                                 break;
@@ -587,7 +572,7 @@ static void i915_capture_error_state(struct drm_device *dev)
                 return;
         }
 
-        error->seqno = i915_get_gem_seqno(dev);
+        error->seqno = i915_get_gem_seqno(dev, &dev_priv->render_ring);
         error->eir = I915_READ(EIR);
         error->pgtbl_er = I915_READ(PGTBL_ER);
         error->pipeastat = I915_READ(PIPEASTAT);
@@ -615,7 +600,9 @@ static void i915_capture_error_state(struct drm_device *dev)
         batchbuffer[0] = NULL;
         batchbuffer[1] = NULL;
         count = 0;
-        list_for_each_entry(obj_priv, &dev_priv->mm.active_list, list) {
+        list_for_each_entry(obj_priv,
+                        &dev_priv->render_ring.active_list, list) {
+
                 struct drm_gem_object *obj = &obj_priv->base;
 
                 if (batchbuffer[0] == NULL &&
@@ -639,7 +626,8 @@ static void i915_capture_error_state(struct drm_device *dev)
         error->batchbuffer[1] = i915_error_object_create(dev, batchbuffer[1]);
 
         /* Record the ringbuffer */
-        error->ringbuffer = i915_error_object_create(dev, dev_priv->ring.ring_obj);
+        error->ringbuffer = i915_error_object_create(dev,
+                        dev_priv->render_ring.gem_object);
 
         /* Record buffers on the active list. */
         error->active_bo = NULL;
@@ -651,7 +639,8 @@ static void i915_capture_error_state(struct drm_device *dev)
 
         if (error->active_bo) {
                 int i = 0;
-                list_for_each_entry(obj_priv, &dev_priv->mm.active_list, list) {
+                list_for_each_entry(obj_priv,
+                                &dev_priv->render_ring.active_list, list) {
                         struct drm_gem_object *obj = &obj_priv->base;
 
                         error->active_bo[i].size = obj->size;
@@ -703,24 +692,13 @@ void i915_destroy_error_state(struct drm_device *dev)
                 i915_error_state_free(dev, error);
 }
 
-/**
- * i915_handle_error - handle an error interrupt
- * @dev: drm device
- *
- * Do some basic checking of regsiter state at error interrupt time and
- * dump it to the syslog.  Also call i915_capture_error_state() to make
- * sure we get a record and make it available in debugfs.  Fire a uevent
- * so userspace knows something bad happened (should trigger collection
- * of a ring dump etc.).
- */
-static void i915_handle_error(struct drm_device *dev, bool wedged)
+static void i915_report_and_clear_eir(struct drm_device *dev)
 {
         struct drm_i915_private *dev_priv = dev->dev_private;
         u32 eir = I915_READ(EIR);
-        u32 pipea_stats = I915_READ(PIPEASTAT);
-        u32 pipeb_stats = I915_READ(PIPEBSTAT);
 
-        i915_capture_error_state(dev);
+        if (!eir)
+                return;
 
         printk(KERN_ERR "render error detected, EIR: 0x%08x\n",
                eir);
@@ -766,6 +744,9 @@ static void i915_handle_error(struct drm_device *dev, bool wedged)
         }
 
         if (eir & I915_ERROR_MEMORY_REFRESH) {
+                u32 pipea_stats = I915_READ(PIPEASTAT);
+                u32 pipeb_stats = I915_READ(PIPEBSTAT);
+
                 printk(KERN_ERR "memory refresh error\n");
                 printk(KERN_ERR "PIPEASTAT: 0x%08x\n",
                        pipea_stats);
@@ -822,6 +803,24 @@ static void i915_handle_error(struct drm_device *dev, bool wedged)
                 I915_WRITE(EMR, I915_READ(EMR) | eir);
                 I915_WRITE(IIR, I915_RENDER_COMMAND_PARSER_ERROR_INTERRUPT);
         }
+}
+
+/**
+ * i915_handle_error - handle an error interrupt
+ * @dev: drm device
+ *
+ * Do some basic checking of regsiter state at error interrupt time and
+ * dump it to the syslog.  Also call i915_capture_error_state() to make
+ * sure we get a record and make it available in debugfs.  Fire a uevent
+ * so userspace knows something bad happened (should trigger collection
+ * of a ring dump etc.).
+ */
+static void i915_handle_error(struct drm_device *dev, bool wedged)
+{
+        struct drm_i915_private *dev_priv = dev->dev_private;
+
+        i915_capture_error_state(dev);
+        i915_report_and_clear_eir(dev);
 
         if (wedged) {
                 atomic_set(&dev_priv->mm.wedged, 1);
@@ -829,7 +828,7 @@ static void i915_handle_error(struct drm_device *dev, bool wedged)
                 /*
                  * Wakeup waiting processes so they don't hang
                  */
-                DRM_WAKEUP(&dev_priv->irq_queue);
+                DRM_WAKEUP(&dev_priv->render_ring.irq_queue);
         }
 
         queue_work(dev_priv->wq, &dev_priv->error_work);
@@ -848,6 +847,7 @@ irqreturn_t i915_driver_irq_handler(DRM_IRQ_ARGS)
         unsigned long irqflags;
         int irq_received;
         int ret = IRQ_NONE;
+        struct intel_ring_buffer *render_ring = &dev_priv->render_ring;
 
         atomic_inc(&dev_priv->irq_received);
 
@@ -928,14 +928,18 @@ irqreturn_t i915_driver_irq_handler(DRM_IRQ_ARGS)
                 }
 
                 if (iir & I915_USER_INTERRUPT) {
-                        u32 seqno = i915_get_gem_seqno(dev);
-                        dev_priv->mm.irq_gem_seqno = seqno;
+                        u32 seqno =
+                                render_ring->get_gem_seqno(dev, render_ring);
+                        render_ring->irq_gem_seqno = seqno;
                         trace_i915_gem_request_complete(dev, seqno);
-                        DRM_WAKEUP(&dev_priv->irq_queue);
+                        DRM_WAKEUP(&dev_priv->render_ring.irq_queue);
                         dev_priv->hangcheck_count = 0;
                         mod_timer(&dev_priv->hangcheck_timer, jiffies + DRM_I915_HANGCHECK_PERIOD);
                 }
 
+                if (HAS_BSD(dev) && (iir & I915_BSD_USER_INTERRUPT))
+                        DRM_WAKEUP(&dev_priv->bsd_ring.irq_queue);
+
                 if (iir & I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT)
                         intel_prepare_page_flip(dev, 0);
 
@@ -984,7 +988,6 @@ static int i915_emit_irq(struct drm_device * dev)
 {
         drm_i915_private_t *dev_priv = dev->dev_private;
         struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
-        RING_LOCALS;
 
         i915_kernel_lost_context(dev);
 
@@ -1006,43 +1009,13 @@ static int i915_emit_irq(struct drm_device * dev)
         return dev_priv->counter;
 }
 
-void i915_user_irq_get(struct drm_device *dev)
-{
-        drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-        unsigned long irqflags;
-
-        spin_lock_irqsave(&dev_priv->user_irq_lock, irqflags);
-        if (dev->irq_enabled && (++dev_priv->user_irq_refcount == 1)) {
-                if (HAS_PCH_SPLIT(dev))
-                        ironlake_enable_graphics_irq(dev_priv, GT_PIPE_NOTIFY);
-                else
-                        i915_enable_irq(dev_priv, I915_USER_INTERRUPT);
-        }
-        spin_unlock_irqrestore(&dev_priv->user_irq_lock, irqflags);
-}
-
-void i915_user_irq_put(struct drm_device *dev)
-{
-        drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
-        unsigned long irqflags;
-
-        spin_lock_irqsave(&dev_priv->user_irq_lock, irqflags);
-        BUG_ON(dev->irq_enabled && dev_priv->user_irq_refcount <= 0);
-        if (dev->irq_enabled && (--dev_priv->user_irq_refcount == 0)) {
-                if (HAS_PCH_SPLIT(dev))
-                        ironlake_disable_graphics_irq(dev_priv, GT_PIPE_NOTIFY);
-                else
-                        i915_disable_irq(dev_priv, I915_USER_INTERRUPT);
-        }
-        spin_unlock_irqrestore(&dev_priv->user_irq_lock, irqflags);
-}
-
 void i915_trace_irq_get(struct drm_device *dev, u32 seqno)
 {
         drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+        struct intel_ring_buffer *render_ring = &dev_priv->render_ring;
 
         if (dev_priv->trace_irq_seqno == 0)
-                i915_user_irq_get(dev);
+                render_ring->user_irq_get(dev, render_ring);
 
         dev_priv->trace_irq_seqno = seqno;
 }
@@ -1052,6 +1025,7 @@ static int i915_wait_irq(struct drm_device * dev, int irq_nr)
         drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
         struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
         int ret = 0;
+        struct intel_ring_buffer *render_ring = &dev_priv->render_ring;
 
         DRM_DEBUG_DRIVER("irq_nr=%d breadcrumb=%d\n", irq_nr,
                   READ_BREADCRUMB(dev_priv));
@@ -1065,10 +1039,10 @@ static int i915_wait_irq(struct drm_device * dev, int irq_nr)
         if (master_priv->sarea_priv)
                 master_priv->sarea_priv->perf_boxes |= I915_BOX_WAIT;
 
-        i915_user_irq_get(dev);
-        DRM_WAIT_ON(ret, dev_priv->irq_queue, 3 * DRM_HZ,
+        render_ring->user_irq_get(dev, render_ring);
+        DRM_WAIT_ON(ret, dev_priv->render_ring.irq_queue, 3 * DRM_HZ,
                     READ_BREADCRUMB(dev_priv) >= irq_nr);
-        i915_user_irq_put(dev);
+        render_ring->user_irq_put(dev, render_ring);
 
         if (ret == -EBUSY) {
                 DRM_ERROR("EBUSY -- rec: %d emitted: %d\n",
@@ -1087,7 +1061,7 @@ int i915_irq_emit(struct drm_device *dev, void *data,
         drm_i915_irq_emit_t *emit = data;
         int result;
 
-        if (!dev_priv || !dev_priv->ring.virtual_start) {
+        if (!dev_priv || !dev_priv->render_ring.virtual_start) {
                 DRM_ERROR("called with no initialization\n");
                 return -EINVAL;
         }
@@ -1233,9 +1207,12 @@ int i915_vblank_swap(struct drm_device *dev, void *data,
         return -EINVAL;
 }
 
-struct drm_i915_gem_request *i915_get_tail_request(struct drm_device *dev) {
+struct drm_i915_gem_request *
+i915_get_tail_request(struct drm_device *dev)
+{
         drm_i915_private_t *dev_priv = dev->dev_private;
-        return list_entry(dev_priv->mm.request_list.prev, struct drm_i915_gem_request, list);
+        return list_entry(dev_priv->render_ring.request_list.prev,
+                        struct drm_i915_gem_request, list);
 }
 
 /**
@@ -1260,8 +1237,10 @@ void i915_hangcheck_elapsed(unsigned long data)
                 acthd = I915_READ(ACTHD_I965);
 
         /* If all work is done then ACTHD clearly hasn't advanced. */
-        if (list_empty(&dev_priv->mm.request_list) ||
-                       i915_seqno_passed(i915_get_gem_seqno(dev), i915_get_tail_request(dev)->seqno)) {
+        if (list_empty(&dev_priv->render_ring.request_list) ||
+                i915_seqno_passed(i915_get_gem_seqno(dev,
+                                &dev_priv->render_ring),
+                        i915_get_tail_request(dev)->seqno)) {
                 dev_priv->hangcheck_count = 0;
                 return;
         }
@@ -1314,7 +1293,7 @@ static int ironlake_irq_postinstall(struct drm_device *dev)
         /* enable kind of interrupts always enabled */
         u32 display_mask = DE_MASTER_IRQ_CONTROL | DE_GSE | DE_PCH_EVENT |
                            DE_PLANEA_FLIP_DONE | DE_PLANEB_FLIP_DONE;
-        u32 render_mask = GT_PIPE_NOTIFY;
+        u32 render_mask = GT_PIPE_NOTIFY | GT_BSD_USER_INTERRUPT;
         u32 hotplug_mask = SDE_CRT_HOTPLUG | SDE_PORTB_HOTPLUG |
                            SDE_PORTC_HOTPLUG | SDE_PORTD_HOTPLUG;
 
@@ -1328,7 +1307,7 @@ static int ironlake_irq_postinstall(struct drm_device *dev)
         (void) I915_READ(DEIER);
 
         /* user interrupt should be enabled, but masked initial */
-        dev_priv->gt_irq_mask_reg = 0xffffffff;
+        dev_priv->gt_irq_mask_reg = ~render_mask;
         dev_priv->gt_irq_enable_reg = render_mask;
 
         I915_WRITE(GTIIR, I915_READ(GTIIR));
@@ -1391,7 +1370,10 @@ int i915_driver_irq_postinstall(struct drm_device *dev)
         u32 enable_mask = I915_INTERRUPT_ENABLE_FIX | I915_INTERRUPT_ENABLE_VAR;
         u32 error_mask;
 
-        DRM_INIT_WAITQUEUE(&dev_priv->irq_queue);
+        DRM_INIT_WAITQUEUE(&dev_priv->render_ring.irq_queue);
+
+        if (HAS_BSD(dev))
+                DRM_INIT_WAITQUEUE(&dev_priv->bsd_ring.irq_queue);
 
         dev_priv->vblank_pipe = DRM_I915_VBLANK_PIPE_A | DRM_I915_VBLANK_PIPE_B;
 
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index f3e39cc46f0d..64b0a3afd92b 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -334,6 +334,7 @@
 #define   I915_DEBUG_INTERRUPT                          (1<<2)
 #define   I915_USER_INTERRUPT                           (1<<1)
 #define   I915_ASLE_INTERRUPT                           (1<<0)
+#define   I915_BSD_USER_INTERRUPT                      (1<<25)
 #define EIR             0x020b0
 #define EMR             0x020b4
 #define ESR             0x020b8
@@ -368,6 +369,36 @@
 #define BB_ADDR         0x02140 /* 8 bytes */
 #define GFX_FLSH_CNTL   0x02170 /* 915+ only */
 
+/* GEN6 interrupt control */
+#define GEN6_RENDER_HWSTAM      0x2098
+#define GEN6_RENDER_IMR         0x20a8
+#define   GEN6_RENDER_CONTEXT_SWITCH_INTERRUPT          (1 << 8)
+#define   GEN6_RENDER_PPGTT_PAGE_FAULT                  (1 << 7)
+#define   GEN6_RENDER TIMEOUT_COUNTER_EXPIRED           (1 << 6)
+#define   GEN6_RENDER_L3_PARITY_ERROR                   (1 << 5)
+#define   GEN6_RENDER_PIPE_CONTROL_NOTIFY_INTERRUPT     (1 << 4)
+#define   GEN6_RENDER_COMMAND_PARSER_MASTER_ERROR       (1 << 3)
+#define   GEN6_RENDER_SYNC_STATUS                       (1 << 2)
+#define   GEN6_RENDER_DEBUG_INTERRUPT                   (1 << 1)
+#define   GEN6_RENDER_USER_INTERRUPT                    (1 << 0)
+
+#define GEN6_BLITTER_HWSTAM     0x22098
+#define GEN6_BLITTER_IMR        0x220a8
+#define   GEN6_BLITTER_MI_FLUSH_DW_NOTIFY_INTERRUPT     (1 << 26)
+#define   GEN6_BLITTER_COMMAND_PARSER_MASTER_ERROR      (1 << 25)
+#define   GEN6_BLITTER_SYNC_STATUS                      (1 << 24)
+#define   GEN6_BLITTER_USER_INTERRUPT                   (1 << 22)
+/*
+ * BSD (bit stream decoder instruction and interrupt control register defines
+ * (G4X and Ironlake only)
+ */
+
+#define BSD_RING_TAIL          0x04030
+#define BSD_RING_HEAD          0x04034
+#define BSD_RING_START         0x04038
+#define BSD_RING_CTL           0x0403c
+#define BSD_RING_ACTHD         0x04074
+#define BSD_HWS_PGA            0x04080
 
 /*
  * Framebuffer compression (915+ only)
@@ -805,6 +836,10 @@
 #define DCC_CHANNEL_XOR_DISABLE                         (1 << 10)
 #define DCC_CHANNEL_XOR_BIT_17                          (1 << 9)
 
+/** Pineview MCH register contains DDR3 setting */
+#define CSHRDDR3CTL            0x101a8
+#define CSHRDDR3CTL_DDR3       (1 << 2)
+
 /** 965 MCH register controlling DRAM channel configuration */
 #define C0DRB3                  0x10206
 #define C1DRB3                  0x10606
@@ -826,6 +861,12 @@
 #define CLKCFG_MEM_800                                  (3 << 4)
 #define CLKCFG_MEM_MASK                                 (7 << 4)
 
+#define TR1                     0x11006
+#define TSFS                    0x11020
+#define   TSFS_SLOPE_MASK       0x0000ff00
+#define   TSFS_SLOPE_SHIFT      8
+#define   TSFS_INTR_MASK        0x000000ff
+
 #define CRSTANDVID              0x11100
 #define PXVFREQ_BASE            0x11110 /* P[0-15]VIDFREQ (0x1114c) (Ironlake) */
 #define   PXVFREQ_PX_MASK       0x7f000000
@@ -964,6 +1005,41 @@
 #define   MEMSTAT_SRC_CTL_STDBY 3
 #define RCPREVBSYTUPAVG         0x113b8
 #define RCPREVBSYTDNAVG         0x113bc
+#define SDEW                    0x1124c
+#define CSIEW0                  0x11250
+#define CSIEW1                  0x11254
+#define CSIEW2                  0x11258
+#define PEW                     0x1125c
+#define DEW                     0x11270
+#define MCHAFE                  0x112c0
+#define CSIEC                   0x112e0
+#define DMIEC                   0x112e4
+#define DDREC                   0x112e8
+#define PEG0EC                  0x112ec
+#define PEG1EC                  0x112f0
+#define GFXEC                   0x112f4
+#define RPPREVBSYTUPAVG         0x113b8
+#define RPPREVBSYTDNAVG         0x113bc
+#define ECR                     0x11600
+#define   ECR_GPFE              (1<<31)
+#define   ECR_IMONE             (1<<30)
+#define   ECR_CAP_MASK          0x0000001f /* Event range, 0-31 */
+#define OGW0                    0x11608
+#define OGW1                    0x1160c
+#define EG0                     0x11610
+#define EG1                     0x11614
+#define EG2                     0x11618
+#define EG3                     0x1161c
+#define EG4                     0x11620
+#define EG5                     0x11624
+#define EG6                     0x11628
+#define EG7                     0x1162c
+#define PXW                     0x11664
+#define PXWL                    0x11680
+#define LCFUSE02                0x116c0
+#define   LCFUSE_HIV_MASK       0x000000ff
+#define CSIPLL0                 0x12c10
+#define DDRMPLL1                0X12c20
 #define PEG_BAND_GAP_DATA       0x14d68
 
 /*
@@ -1055,7 +1131,6 @@
 #define CRT_HOTPLUG_DETECT_VOLTAGE_325MV        (0 << 2)
 #define CRT_HOTPLUG_DETECT_VOLTAGE_475MV        (1 << 2)
 #define CRT_HOTPLUG_MASK                        (0x3fc) /* Bits 9-2 */
-#define CRT_FORCE_HOTPLUG_MASK                  0xfffffe1f
 
 #define PORT_HOTPLUG_STAT       0x61114
 #define   HDMIB_HOTPLUG_INT_STATUS              (1 << 29)
@@ -2355,6 +2430,8 @@
 #define GT_PIPE_NOTIFY          (1 << 4)
 #define GT_SYNC_STATUS          (1 << 2)
 #define GT_USER_INTERRUPT       (1 << 0)
+#define GT_BSD_USER_INTERRUPT   (1 << 5)
+
 
 #define GTISR   0x44010
 #define GTIMR   0x44014
@@ -2690,6 +2767,9 @@
 #define  SDVO_ENCODING          (0)
 #define  TMDS_ENCODING          (2 << 10)
 #define  NULL_PACKET_VSYNC_ENABLE       (1 << 9)
+/* CPT */
+#define  HDMI_MODE_SELECT       (1 << 9)
+#define  DVI_MODE_SELECT        (0)
 #define  SDVOB_BORDER_ENABLE    (1 << 7)
 #define  AUDIO_ENABLE           (1 << 6)
 #define  VSYNC_ACTIVE_HIGH      (1 << 4)
diff --git a/drivers/gpu/drm/i915/i915_trace.h b/drivers/gpu/drm/i915/i915_trace.h
index 9e4c45f68d6e..fab21760dd57 100644
--- a/drivers/gpu/drm/i915/i915_trace.h
+++ b/drivers/gpu/drm/i915/i915_trace.h
@@ -53,23 +53,6 @@ TRACE_EVENT(i915_gem_object_bind,
                       __entry->obj, __entry->gtt_offset)
 );
 
-TRACE_EVENT(i915_gem_object_clflush,
-
-            TP_PROTO(struct drm_gem_object *obj),
-
-            TP_ARGS(obj),
-
-            TP_STRUCT__entry(
-                             __field(struct drm_gem_object *, obj)
-                             ),
-
-            TP_fast_assign(
-                           __entry->obj = obj;
-                           ),
-
-            TP_printk("obj=%p", __entry->obj)
-);
-
 TRACE_EVENT(i915_gem_object_change_domain,
 
             TP_PROTO(struct drm_gem_object *obj, uint32_t old_read_domains, uint32_t old_write_domain),
@@ -132,6 +115,13 @@ DECLARE_EVENT_CLASS(i915_gem_object,
             TP_printk("obj=%p", __entry->obj)
 );
 
+DEFINE_EVENT(i915_gem_object, i915_gem_object_clflush,
+
+            TP_PROTO(struct drm_gem_object *obj),
+
+            TP_ARGS(obj)
+);
+
 DEFINE_EVENT(i915_gem_object, i915_gem_object_unbind,
 
             TP_PROTO(struct drm_gem_object *obj),
diff --git a/drivers/gpu/drm/i915/intel_bios.c b/drivers/gpu/drm/i915/intel_bios.c
index 4c748d8f73d6..96f75d7f6633 100644
--- a/drivers/gpu/drm/i915/intel_bios.c
+++ b/drivers/gpu/drm/i915/intel_bios.c
@@ -95,6 +95,16 @@ fill_detail_timing_data(struct drm_display_mode *panel_fixed_mode,
         panel_fixed_mode->clock = dvo_timing->clock * 10;
         panel_fixed_mode->type = DRM_MODE_TYPE_PREFERRED;
 
+        if (dvo_timing->hsync_positive)
+                panel_fixed_mode->flags |= DRM_MODE_FLAG_PHSYNC;
+        else
+                panel_fixed_mode->flags |= DRM_MODE_FLAG_NHSYNC;
+
+        if (dvo_timing->vsync_positive)
+                panel_fixed_mode->flags |= DRM_MODE_FLAG_PVSYNC;
+        else
+                panel_fixed_mode->flags |= DRM_MODE_FLAG_NVSYNC;
+
         /* Some VBTs have bogus h/vtotal values */
         if (panel_fixed_mode->hsync_end > panel_fixed_mode->htotal)
                 panel_fixed_mode->htotal = panel_fixed_mode->hsync_end + 1;
diff --git a/drivers/gpu/drm/i915/intel_crt.c b/drivers/gpu/drm/i915/intel_crt.c
index e16ac5a28c3c..22ff38455731 100644
--- a/drivers/gpu/drm/i915/intel_crt.c
+++ b/drivers/gpu/drm/i915/intel_crt.c
@@ -217,7 +217,8 @@ static bool intel_crt_detect_hotplug(struct drm_connector *connector)
 {
         struct drm_device *dev = connector->dev;
         struct drm_i915_private *dev_priv = dev->dev_private;
-        u32 hotplug_en;
+        u32 hotplug_en, orig, stat;
+        bool ret = false;
         int i, tries = 0;
 
         if (HAS_PCH_SPLIT(dev))
@@ -232,8 +233,8 @@ static bool intel_crt_detect_hotplug(struct drm_connector *connector)
                 tries = 2;
         else
                 tries = 1;
-        hotplug_en = I915_READ(PORT_HOTPLUG_EN);
-        hotplug_en &= CRT_FORCE_HOTPLUG_MASK;
+        hotplug_en = orig = I915_READ(PORT_HOTPLUG_EN);
+        hotplug_en &= CRT_HOTPLUG_MASK;
         hotplug_en |= CRT_HOTPLUG_FORCE_DETECT;
 
         if (IS_G4X(dev))
@@ -255,11 +256,17 @@ static bool intel_crt_detect_hotplug(struct drm_connector *connector)
                 } while (time_after(timeout, jiffies));
         }
 
-        if ((I915_READ(PORT_HOTPLUG_STAT) & CRT_HOTPLUG_MONITOR_MASK) !=
-            CRT_HOTPLUG_MONITOR_NONE)
-                return true;
+        stat = I915_READ(PORT_HOTPLUG_STAT);
+        if ((stat & CRT_HOTPLUG_MONITOR_MASK) != CRT_HOTPLUG_MONITOR_NONE)
+                ret = true;
+
+        /* clear the interrupt we just generated, if any */
+        I915_WRITE(PORT_HOTPLUG_STAT, CRT_HOTPLUG_INT_STATUS);
 
-        return false;
+        /* and put the bits back */
+        I915_WRITE(PORT_HOTPLUG_EN, orig);
+
+        return ret;
 }
 
 static bool intel_crt_detect_ddc(struct drm_encoder *encoder)
@@ -569,7 +576,7 @@ void intel_crt_init(struct drm_device *dev)
                                    (1 << INTEL_ANALOG_CLONE_BIT) |
                                    (1 << INTEL_SDVO_LVDS_CLONE_BIT);
         intel_encoder->crtc_mask = (1 << 0) | (1 << 1);
-        connector->interlace_allowed = 0;
+        connector->interlace_allowed = 1;
         connector->doublescan_allowed = 0;
 
         drm_encoder_helper_add(&intel_encoder->enc, &intel_crt_helper_funcs);
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index f469a84cacfd..cc8131ff319f 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -1029,19 +1029,28 @@ static void i8xx_enable_fbc(struct drm_crtc *crtc, unsigned long interval)
 void i8xx_disable_fbc(struct drm_device *dev)
 {
         struct drm_i915_private *dev_priv = dev->dev_private;
+        unsigned long timeout = jiffies + msecs_to_jiffies(1);
         u32 fbc_ctl;
 
         if (!I915_HAS_FBC(dev))
                 return;
 
+        if (!(I915_READ(FBC_CONTROL) & FBC_CTL_EN))
+                return; /* Already off, just return */
+
         /* Disable compression */
         fbc_ctl = I915_READ(FBC_CONTROL);
         fbc_ctl &= ~FBC_CTL_EN;
         I915_WRITE(FBC_CONTROL, fbc_ctl);
 
         /* Wait for compressing bit to clear */
-        while (I915_READ(FBC_STATUS) & FBC_STAT_COMPRESSING)
-                ; /* nothing */
+        while (I915_READ(FBC_STATUS) & FBC_STAT_COMPRESSING) {
+                if (time_after(jiffies, timeout)) {
+                        DRM_DEBUG_DRIVER("FBC idle timed out\n");
+                        break;
+                }
+                ; /* do nothing */
+        }
 
         intel_wait_for_vblank(dev);
 
@@ -1239,10 +1248,11 @@ static void intel_update_fbc(struct drm_crtc *crtc,
         return;
 
 out_disable:
-        DRM_DEBUG_KMS("unsupported config, disabling FBC\n");
         /* Multiple disables should be harmless */
-        if (intel_fbc_enabled(dev))
+        if (intel_fbc_enabled(dev)) {
+                DRM_DEBUG_KMS("unsupported config, disabling FBC\n");
                 intel_disable_fbc(dev);
+        }
 }
 
 static int
@@ -1386,7 +1396,8 @@ intel_pipe_set_base(struct drm_crtc *crtc, int x, int y,
         Start = obj_priv->gtt_offset;
         Offset = y * crtc->fb->pitch + x * (crtc->fb->bits_per_pixel / 8);
 
-        DRM_DEBUG_KMS("Writing base %08lX %08lX %d %d\n", Start, Offset, x, y);
+        DRM_DEBUG_KMS("Writing base %08lX %08lX %d %d %d\n",
+                      Start, Offset, x, y, crtc->fb->pitch);
         I915_WRITE(dspstride, crtc->fb->pitch);
         if (IS_I965G(dev)) {
                 I915_WRITE(dspbase, Offset);
@@ -2345,6 +2356,8 @@ static bool intel_crtc_mode_fixup(struct drm_crtc *crtc,
                 if (mode->clock * 3 > 27000 * 4)
                         return MODE_CLOCK_HIGH;
         }
+
+        drm_mode_set_crtcinfo(adjusted_mode, 0);
         return true;
 }
 
@@ -2629,6 +2642,7 @@ static unsigned long intel_calculate_wm(unsigned long clock_in_khz,
 
 struct cxsr_latency {
         int is_desktop;
+        int is_ddr3;
         unsigned long fsb_freq;
         unsigned long mem_freq;
         unsigned long display_sr;
@@ -2638,33 +2652,45 @@ struct cxsr_latency {
 };
 
 static struct cxsr_latency cxsr_latency_table[] = {
-        {1, 800, 400, 3382, 33382, 3983, 33983},    /* DDR2-400 SC */
-        {1, 800, 667, 3354, 33354, 3807, 33807},    /* DDR2-667 SC */
-        {1, 800, 800, 3347, 33347, 3763, 33763},    /* DDR2-800 SC */
-
-        {1, 667, 400, 3400, 33400, 4021, 34021},    /* DDR2-400 SC */
-        {1, 667, 667, 3372, 33372, 3845, 33845},    /* DDR2-667 SC */
-        {1, 667, 800, 3386, 33386, 3822, 33822},    /* DDR2-800 SC */
-
-        {1, 400, 400, 3472, 33472, 4173, 34173},    /* DDR2-400 SC */
-        {1, 400, 667, 3443, 33443, 3996, 33996},    /* DDR2-667 SC */
-        {1, 400, 800, 3430, 33430, 3946, 33946},    /* DDR2-800 SC */
-
-        {0, 800, 400, 3438, 33438, 4065, 34065},    /* DDR2-400 SC */
-        {0, 800, 667, 3410, 33410, 3889, 33889},    /* DDR2-667 SC */
-        {0, 800, 800, 3403, 33403, 3845, 33845},    /* DDR2-800 SC */
-
-        {0, 667, 400, 3456, 33456, 4103, 34106},    /* DDR2-400 SC */
-        {0, 667, 667, 3428, 33428, 3927, 33927},    /* DDR2-667 SC */
-        {0, 667, 800, 3443, 33443, 3905, 33905},    /* DDR2-800 SC */
-
-        {0, 400, 400, 3528, 33528, 4255, 34255},    /* DDR2-400 SC */
-        {0, 400, 667, 3500, 33500, 4079, 34079},    /* DDR2-667 SC */
-        {0, 400, 800, 3487, 33487, 4029, 34029},    /* DDR2-800 SC */
+        {1, 0, 800, 400, 3382, 33382, 3983, 33983},    /* DDR2-400 SC */
+        {1, 0, 800, 667, 3354, 33354, 3807, 33807},    /* DDR2-667 SC */
+        {1, 0, 800, 800, 3347, 33347, 3763, 33763},    /* DDR2-800 SC */
+        {1, 1, 800, 667, 6420, 36420, 6873, 36873},    /* DDR3-667 SC */
+        {1, 1, 800, 800, 5902, 35902, 6318, 36318},    /* DDR3-800 SC */
+
+        {1, 0, 667, 400, 3400, 33400, 4021, 34021},    /* DDR2-400 SC */
+        {1, 0, 667, 667, 3372, 33372, 3845, 33845},    /* DDR2-667 SC */
+        {1, 0, 667, 800, 3386, 33386, 3822, 33822},    /* DDR2-800 SC */
+        {1, 1, 667, 667, 6438, 36438, 6911, 36911},    /* DDR3-667 SC */
+        {1, 1, 667, 800, 5941, 35941, 6377, 36377},    /* DDR3-800 SC */
+
+        {1, 0, 400, 400, 3472, 33472, 4173, 34173},    /* DDR2-400 SC */
+        {1, 0, 400, 667, 3443, 33443, 3996, 33996},    /* DDR2-667 SC */
+        {1, 0, 400, 800, 3430, 33430, 3946, 33946},    /* DDR2-800 SC */
+        {1, 1, 400, 667, 6509, 36509, 7062, 37062},    /* DDR3-667 SC */
+        {1, 1, 400, 800, 5985, 35985, 6501, 36501},    /* DDR3-800 SC */
+
+        {0, 0, 800, 400, 3438, 33438, 4065, 34065},    /* DDR2-400 SC */
+        {0, 0, 800, 667, 3410, 33410, 3889, 33889},    /* DDR2-667 SC */
+        {0, 0, 800, 800, 3403, 33403, 3845, 33845},    /* DDR2-800 SC */
+        {0, 1, 800, 667, 6476, 36476, 6955, 36955},    /* DDR3-667 SC */
+        {0, 1, 800, 800, 5958, 35958, 6400, 36400},    /* DDR3-800 SC */
+
+        {0, 0, 667, 400, 3456, 33456, 4103, 34106},    /* DDR2-400 SC */
+        {0, 0, 667, 667, 3428, 33428, 3927, 33927},    /* DDR2-667 SC */
+        {0, 0, 667, 800, 3443, 33443, 3905, 33905},    /* DDR2-800 SC */
+        {0, 1, 667, 667, 6494, 36494, 6993, 36993},    /* DDR3-667 SC */
+        {0, 1, 667, 800, 5998, 35998, 6460, 36460},    /* DDR3-800 SC */
+
+        {0, 0, 400, 400, 3528, 33528, 4255, 34255},    /* DDR2-400 SC */
+        {0, 0, 400, 667, 3500, 33500, 4079, 34079},    /* DDR2-667 SC */
+        {0, 0, 400, 800, 3487, 33487, 4029, 34029},    /* DDR2-800 SC */
+        {0, 1, 400, 667, 6566, 36566, 7145, 37145},    /* DDR3-667 SC */
+        {0, 1, 400, 800, 6042, 36042, 6584, 36584},    /* DDR3-800 SC */
 };
 
-static struct cxsr_latency *intel_get_cxsr_latency(int is_desktop, int fsb,
-                                                   int mem)
+static struct cxsr_latency *intel_get_cxsr_latency(int is_desktop, int is_ddr3, 
+                                                   int fsb, int mem)
 {
         int i;
         struct cxsr_latency *latency;
@@ -2675,6 +2701,7 @@ static struct cxsr_latency *intel_get_cxsr_latency(int is_desktop, int fsb,
         for (i = 0; i < ARRAY_SIZE(cxsr_latency_table); i++) {
                 latency = &cxsr_latency_table[i];
                 if (is_desktop == latency->is_desktop &&
+                    is_ddr3 == latency->is_ddr3 &&
                     fsb == latency->fsb_freq && mem == latency->mem_freq)
                         return latency;
         }
@@ -2789,8 +2816,8 @@ static void pineview_update_wm(struct drm_device *dev,  int planea_clock,
         struct cxsr_latency *latency;
         int sr_clock;
 
-        latency = intel_get_cxsr_latency(IS_PINEVIEW_G(dev), dev_priv->fsb_freq,
-                                         dev_priv->mem_freq);
+        latency = intel_get_cxsr_latency(IS_PINEVIEW_G(dev), dev_priv->is_ddr3, 
+                                         dev_priv->fsb_freq, dev_priv->mem_freq);
         if (!latency) {
                 DRM_DEBUG_KMS("Unknown FSB/MEM found, disable CxSR\n");
                 pineview_disable_cxsr(dev);
@@ -3626,6 +3653,11 @@ static int intel_crtc_mode_set(struct drm_crtc *crtc,
                         pipeconf &= ~PIPEACONF_DOUBLE_WIDE;
         }
 
+        dspcntr |= DISPLAY_PLANE_ENABLE;
+        pipeconf |= PIPEACONF_ENABLE;
+        dpll |= DPLL_VCO_ENABLE;
+
+
         /* Disable the panel fitter if it was on our pipe */
         if (!HAS_PCH_SPLIT(dev) && intel_panel_fitter_pipe(dev) == pipe)
                 I915_WRITE(PFIT_CONTROL, 0);
@@ -3772,6 +3804,18 @@ static int intel_crtc_mode_set(struct drm_crtc *crtc,
                 }
         }
 
+        if (adjusted_mode->flags & DRM_MODE_FLAG_INTERLACE) {
+                pipeconf |= PIPECONF_INTERLACE_W_FIELD_INDICATION;
+                /* the chip adds 2 halflines automatically */
+                adjusted_mode->crtc_vdisplay -= 1;
+                adjusted_mode->crtc_vtotal -= 1;
+                adjusted_mode->crtc_vblank_start -= 1;
+                adjusted_mode->crtc_vblank_end -= 1;
+                adjusted_mode->crtc_vsync_end -= 1;
+                adjusted_mode->crtc_vsync_start -= 1;
+        } else
+                pipeconf &= ~PIPECONF_INTERLACE_W_FIELD_INDICATION; /* progressive */
+
         I915_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
                    ((adjusted_mode->crtc_htotal - 1) << 16));
         I915_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
@@ -3934,6 +3978,13 @@ static int intel_crtc_cursor_set(struct drm_crtc *crtc,
                         DRM_ERROR("failed to pin cursor bo\n");
                         goto fail_locked;
                 }
+
+                ret = i915_gem_object_set_to_gtt_domain(bo, 0);
+                if (ret) {
+                        DRM_ERROR("failed to move cursor bo into the GTT\n");
+                        goto fail_unpin;
+                }
+
                 addr = obj_priv->gtt_offset;
         } else {
                 ret = i915_gem_attach_phys_object(dev, bo, (pipe == 0) ? I915_GEM_PHYS_CURSOR_0 : I915_GEM_PHYS_CURSOR_1);
@@ -3977,6 +4028,8 @@ static int intel_crtc_cursor_set(struct drm_crtc *crtc,
         intel_crtc->cursor_bo = bo;
 
         return 0;
+fail_unpin:
+        i915_gem_object_unpin(bo);
 fail_locked:
         mutex_unlock(&dev->struct_mutex);
 fail:
@@ -4436,6 +4489,8 @@ static void intel_idle_update(struct work_struct *work)
 
         mutex_lock(&dev->struct_mutex);
 
+        i915_update_gfx_val(dev_priv);
+
         if (IS_I945G(dev) || IS_I945GM(dev)) {
                 DRM_DEBUG_DRIVER("enable memory self refresh on 945\n");
                 I915_WRITE(FW_BLC_SELF, FW_BLC_SELF_EN_MASK | FW_BLC_SELF_EN);
@@ -4564,12 +4619,6 @@ void intel_finish_page_flip(struct drm_device *dev, int pipe)
         spin_lock_irqsave(&dev->event_lock, flags);
         work = intel_crtc->unpin_work;
         if (work == NULL || !work->pending) {
-                if (work && !work->pending) {
-                        obj_priv = to_intel_bo(work->pending_flip_obj);
-                        DRM_DEBUG_DRIVER("flip finish: %p (%d) not pending?\n",
-                                         obj_priv,
-                                         atomic_read(&obj_priv->pending_flip));
-                }
                 spin_unlock_irqrestore(&dev->event_lock, flags);
                 return;
         }
@@ -4629,14 +4678,11 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
         unsigned long flags;
         int pipesrc_reg = (intel_crtc->pipe == 0) ? PIPEASRC : PIPEBSRC;
         int ret, pipesrc;
-        RING_LOCALS;
 
         work = kzalloc(sizeof *work, GFP_KERNEL);
         if (work == NULL)
                 return -ENOMEM;
 
-        mutex_lock(&dev->struct_mutex);
-
         work->event = event;
         work->dev = crtc->dev;
         intel_fb = to_intel_framebuffer(crtc->fb);
@@ -4646,10 +4692,10 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
         /* We borrow the event spin lock for protecting unpin_work */
         spin_lock_irqsave(&dev->event_lock, flags);
         if (intel_crtc->unpin_work) {
-                DRM_DEBUG_DRIVER("flip queue: crtc already busy\n");
                 spin_unlock_irqrestore(&dev->event_lock, flags);
                 kfree(work);
-                mutex_unlock(&dev->struct_mutex);
+
+                DRM_DEBUG_DRIVER("flip queue: crtc already busy\n");
                 return -EBUSY;
         }
         intel_crtc->unpin_work = work;
@@ -4658,13 +4704,19 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
         intel_fb = to_intel_framebuffer(fb);
         obj = intel_fb->obj;
 
+        mutex_lock(&dev->struct_mutex);
         ret = intel_pin_and_fence_fb_obj(dev, obj);
         if (ret != 0) {
-                DRM_DEBUG_DRIVER("flip queue: %p pin & fence failed\n",
-                          to_intel_bo(obj));
-                kfree(work);
-                intel_crtc->unpin_work = NULL;
                 mutex_unlock(&dev->struct_mutex);
+
+                spin_lock_irqsave(&dev->event_lock, flags);
+                intel_crtc->unpin_work = NULL;
+                spin_unlock_irqrestore(&dev->event_lock, flags);
+
+                kfree(work);
+
+                DRM_DEBUG_DRIVER("flip queue: %p pin & fence failed\n",
+                                 to_intel_bo(obj));
                 return ret;
         }
 
@@ -5023,10 +5075,32 @@ err_unref:
         return NULL;
 }
 
+bool ironlake_set_drps(struct drm_device *dev, u8 val)
+{
+        struct drm_i915_private *dev_priv = dev->dev_private;
+        u16 rgvswctl;
+
+        rgvswctl = I915_READ16(MEMSWCTL);
+        if (rgvswctl & MEMCTL_CMD_STS) {
+                DRM_DEBUG("gpu busy, RCS change rejected\n");
+                return false; /* still busy with another command */
+        }
+
+        rgvswctl = (MEMCTL_CMD_CHFREQ << MEMCTL_CMD_SHIFT) |
+                (val << MEMCTL_FREQ_SHIFT) | MEMCTL_SFCAVM;
+        I915_WRITE16(MEMSWCTL, rgvswctl);
+        POSTING_READ16(MEMSWCTL);
+
+        rgvswctl |= MEMCTL_CMD_STS;
+        I915_WRITE16(MEMSWCTL, rgvswctl);
+
+        return true;
+}
+
 void ironlake_enable_drps(struct drm_device *dev)
 {
         struct drm_i915_private *dev_priv = dev->dev_private;
-        u32 rgvmodectl = I915_READ(MEMMODECTL), rgvswctl;
+        u32 rgvmodectl = I915_READ(MEMMODECTL);
         u8 fmax, fmin, fstart, vstart;
         int i = 0;
 
@@ -5045,13 +5119,21 @@ void ironlake_enable_drps(struct drm_device *dev)
         fmin = (rgvmodectl & MEMMODE_FMIN_MASK);
         fstart = (rgvmodectl & MEMMODE_FSTART_MASK) >>
                 MEMMODE_FSTART_SHIFT;
+        fstart = fmax;
+
         vstart = (I915_READ(PXVFREQ_BASE + (fstart * 4)) & PXVFREQ_PX_MASK) >>
                 PXVFREQ_PX_SHIFT;
 
-        dev_priv->max_delay = fstart; /* can't go to fmax w/o IPS */
+        dev_priv->fmax = fstart; /* IPS callback will increase this */
+        dev_priv->fstart = fstart;
+
+        dev_priv->max_delay = fmax;
         dev_priv->min_delay = fmin;
         dev_priv->cur_delay = fstart;
 
+        DRM_DEBUG_DRIVER("fmax: %d, fmin: %d, fstart: %d\n", fmax, fmin,
+                         fstart);
+
         I915_WRITE(MEMINTREN, MEMINT_CX_SUPR_EN | MEMINT_EVAL_CHG_EN);
 
         /*
@@ -5073,20 +5155,19 @@ void ironlake_enable_drps(struct drm_device *dev)
         }
         msleep(1);
 
-        rgvswctl = (MEMCTL_CMD_CHFREQ << MEMCTL_CMD_SHIFT) |
-                (fstart << MEMCTL_FREQ_SHIFT) | MEMCTL_SFCAVM;
-        I915_WRITE(MEMSWCTL, rgvswctl);
-        POSTING_READ(MEMSWCTL);
+        ironlake_set_drps(dev, fstart);
 
-        rgvswctl |= MEMCTL_CMD_STS;
-        I915_WRITE(MEMSWCTL, rgvswctl);
+        dev_priv->last_count1 = I915_READ(0x112e4) + I915_READ(0x112e8) +
+                I915_READ(0x112e0);
+        dev_priv->last_time1 = jiffies_to_msecs(jiffies);
+        dev_priv->last_count2 = I915_READ(0x112f4);
+        getrawmonotonic(&dev_priv->last_time2);
 }
 
 void ironlake_disable_drps(struct drm_device *dev)
 {
         struct drm_i915_private *dev_priv = dev->dev_private;
-        u32 rgvswctl;
-        u8 fstart;
+        u16 rgvswctl = I915_READ16(MEMSWCTL);
 
         /* Ack interrupts, disable EFC interrupt */
         I915_WRITE(MEMINTREN, I915_READ(MEMINTREN) & ~MEMINT_EVAL_CHG_EN);
@@ -5096,11 +5177,7 @@ void ironlake_disable_drps(struct drm_device *dev)
         I915_WRITE(DEIMR, I915_READ(DEIMR) | DE_PCU_EVENT);
 
         /* Go back to the starting frequency */
-        fstart = (I915_READ(MEMMODECTL) & MEMMODE_FSTART_MASK) >>
-                MEMMODE_FSTART_SHIFT;
-        rgvswctl = (MEMCTL_CMD_CHFREQ << MEMCTL_CMD_SHIFT) |
-                (fstart << MEMCTL_FREQ_SHIFT) | MEMCTL_SFCAVM;
-        I915_WRITE(MEMSWCTL, rgvswctl);
+        ironlake_set_drps(dev, dev_priv->fstart);
         msleep(1);
         rgvswctl |= MEMCTL_CMD_STS;
         I915_WRITE(MEMSWCTL, rgvswctl);
@@ -5108,6 +5185,92 @@ void ironlake_disable_drps(struct drm_device *dev)
 
 }
 
+static unsigned long intel_pxfreq(u32 vidfreq)
+{
+        unsigned long freq;
+        int div = (vidfreq & 0x3f0000) >> 16;
+        int post = (vidfreq & 0x3000) >> 12;
+        int pre = (vidfreq & 0x7);
+
+        if (!pre)
+                return 0;
+
+        freq = ((div * 133333) / ((1<<post) * pre));
+
+        return freq;
+}
+
+void intel_init_emon(struct drm_device *dev)
+{
+        struct drm_i915_private *dev_priv = dev->dev_private;
+        u32 lcfuse;
+        u8 pxw[16];
+        int i;
+
+        /* Disable to program */
+        I915_WRITE(ECR, 0);
+        POSTING_READ(ECR);
+
+        /* Program energy weights for various events */
+        I915_WRITE(SDEW, 0x15040d00);
+        I915_WRITE(CSIEW0, 0x007f0000);
+        I915_WRITE(CSIEW1, 0x1e220004);
+        I915_WRITE(CSIEW2, 0x04000004);
+
+        for (i = 0; i < 5; i++)
+                I915_WRITE(PEW + (i * 4), 0);
+        for (i = 0; i < 3; i++)
+                I915_WRITE(DEW + (i * 4), 0);
+
+        /* Program P-state weights to account for frequency power adjustment */
+        for (i = 0; i < 16; i++) {
+                u32 pxvidfreq = I915_READ(PXVFREQ_BASE + (i * 4));
+                unsigned long freq = intel_pxfreq(pxvidfreq);
+                unsigned long vid = (pxvidfreq & PXVFREQ_PX_MASK) >>
+                        PXVFREQ_PX_SHIFT;
+                unsigned long val;
+
+                val = vid * vid;
+                val *= (freq / 1000);
+                val *= 255;
+                val /= (127*127*900);
+                if (val > 0xff)
+                        DRM_ERROR("bad pxval: %ld\n", val);
+                pxw[i] = val;
+        }
+        /* Render standby states get 0 weight */
+        pxw[14] = 0;
+        pxw[15] = 0;
+
+        for (i = 0; i < 4; i++) {
+                u32 val = (pxw[i*4] << 24) | (pxw[(i*4)+1] << 16) |
+                        (pxw[(i*4)+2] << 8) | (pxw[(i*4)+3]);
+                I915_WRITE(PXW + (i * 4), val);
+        }
+
+        /* Adjust magic regs to magic values (more experimental results) */
+        I915_WRITE(OGW0, 0);
+        I915_WRITE(OGW1, 0);
+        I915_WRITE(EG0, 0x00007f00);
+        I915_WRITE(EG1, 0x0000000e);
+        I915_WRITE(EG2, 0x000e0000);
+        I915_WRITE(EG3, 0x68000300);
+        I915_WRITE(EG4, 0x42000000);
+        I915_WRITE(EG5, 0x00140031);
+        I915_WRITE(EG6, 0);
+        I915_WRITE(EG7, 0);
+
+        for (i = 0; i < 8; i++)
+                I915_WRITE(PXWL + (i * 4), 0);
+
+        /* Enable PMON + select events */
+        I915_WRITE(ECR, 0x80000019);
+
+        lcfuse = I915_READ(LCFUSE02);
+
+        dev_priv->corr = (lcfuse & LCFUSE_HIV_MASK);
+}
+
 void intel_init_clock_gating(struct drm_device *dev)
 {
         struct drm_i915_private *dev_priv = dev->dev_private;
@@ -5277,11 +5440,13 @@ static void intel_init_display(struct drm_device *dev)
                         dev_priv->display.update_wm = NULL;
         } else if (IS_PINEVIEW(dev)) {
                 if (!intel_get_cxsr_latency(IS_PINEVIEW_G(dev),
+                                            dev_priv->is_ddr3,
                                             dev_priv->fsb_freq,
                                             dev_priv->mem_freq)) {
                         DRM_INFO("failed to find known CxSR latency "
-                                 "(found fsb freq %d, mem freq %d), "
+                                 "(found ddr%s fsb freq %d, mem freq %d), "
                                  "disabling CxSR\n",
+                                 (dev_priv->is_ddr3 == 1) ? "3": "2",
                                  dev_priv->fsb_freq, dev_priv->mem_freq);
                         /* Disable CxSR and never update its watermark again */
                         pineview_disable_cxsr(dev);
@@ -5310,7 +5475,6 @@ static void intel_init_display(struct drm_device *dev)
 void intel_modeset_init(struct drm_device *dev)
 {
         struct drm_i915_private *dev_priv = dev->dev_private;
-        int num_pipe;
         int i;
 
         drm_mode_config_init(dev);
@@ -5340,13 +5504,13 @@ void intel_modeset_init(struct drm_device *dev)
                 dev->mode_config.fb_base = pci_resource_start(dev->pdev, 0);
 
         if (IS_MOBILE(dev) || IS_I9XX(dev))
-                num_pipe = 2;
+                dev_priv->num_pipe = 2;
         else
-                num_pipe = 1;
+                dev_priv->num_pipe = 1;
         DRM_DEBUG_KMS("%d display pipe%s available.\n",
-                  num_pipe, num_pipe > 1 ? "s" : "");
+                      dev_priv->num_pipe, dev_priv->num_pipe > 1 ? "s" : "");
 
-        for (i = 0; i < num_pipe; i++) {
+        for (i = 0; i < dev_priv->num_pipe; i++) {
                 intel_crtc_init(dev, i);
         }
 
@@ -5354,8 +5518,10 @@ void intel_modeset_init(struct drm_device *dev)
 
         intel_init_clock_gating(dev);
 
-        if (IS_IRONLAKE_M(dev))
+        if (IS_IRONLAKE_M(dev)) {
                 ironlake_enable_drps(dev);
+                intel_init_emon(dev);
+        }
 
         INIT_WORK(&dev_priv->idle_work, intel_idle_update);
         setup_timer(&dev_priv->idle_timer, intel_gpu_idle_timer,
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index 6b1c9a27c27a..49b54f05d3cf 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -576,7 +576,7 @@ intel_dp_set_m_n(struct drm_crtc *crtc, struct drm_display_mode *mode,
                 struct intel_encoder *intel_encoder;
                 struct intel_dp_priv *dp_priv;
 
-                if (!encoder || encoder->crtc != crtc)
+                if (encoder->crtc != crtc)
                         continue;
 
                 intel_encoder = enc_to_intel_encoder(encoder);
@@ -675,10 +675,9 @@ intel_dp_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode,
         dp_priv->link_configuration[1] = dp_priv->lane_count;
 
         /*
-         * Check for DPCD version > 1.1,
-         * enable enahanced frame stuff in that case
+         * Check for DPCD version > 1.1 and enhanced framing support
          */
-        if (dp_priv->dpcd[0] >= 0x11) {
+        if (dp_priv->dpcd[0] >= 0x11 && (dp_priv->dpcd[2] & DP_ENHANCED_FRAME_CAP)) {
                 dp_priv->link_configuration[1] |= DP_LANE_COUNT_ENHANCED_FRAME_EN;
                 dp_priv->DP |= DP_ENHANCED_FRAMING;
         }
@@ -1208,6 +1207,8 @@ ironlake_dp_detect(struct drm_connector *connector)
                 if (dp_priv->dpcd[0] != 0)
                         status = connector_status_connected;
         }
+        DRM_DEBUG_KMS("DPCD: %hx%hx%hx%hx\n", dp_priv->dpcd[0],
+                      dp_priv->dpcd[1], dp_priv->dpcd[2], dp_priv->dpcd[3]);
         return status;
 }
 
@@ -1352,7 +1353,7 @@ intel_trans_dp_port_sel (struct drm_crtc *crtc)
         struct intel_encoder *intel_encoder = NULL;
 
         list_for_each_entry(encoder, &mode_config->encoder_list, head) {
-                if (!encoder || encoder->crtc != crtc)
+                if (encoder->crtc != crtc)
                         continue;
 
                 intel_encoder = enc_to_intel_encoder(encoder);
diff --git a/drivers/gpu/drm/i915/intel_fb.c b/drivers/gpu/drm/i915/intel_fb.c
index 6f53cf7fbc50..c3c505244e07 100644
--- a/drivers/gpu/drm/i915/intel_fb.c
+++ b/drivers/gpu/drm/i915/intel_fb.c
@@ -105,7 +105,11 @@ static int intelfb_create(struct intel_fbdev *ifbdev,
         }
 
         /* Flush everything out, we'll be doing GTT only from now on */
-        i915_gem_object_set_to_gtt_domain(fbo, 1);
+        ret = i915_gem_object_set_to_gtt_domain(fbo, 1);
+        if (ret) {
+                DRM_ERROR("failed to bind fb: %d.\n", ret);
+                goto out_unpin;
+        }
 
         info = framebuffer_alloc(0, device);
         if (!info) {
@@ -241,6 +245,7 @@ int intel_fbdev_init(struct drm_device *dev)
 {
         struct intel_fbdev *ifbdev;
         drm_i915_private_t *dev_priv = dev->dev_private;
+        int ret;
 
         ifbdev = kzalloc(sizeof(struct intel_fbdev), GFP_KERNEL);
         if (!ifbdev)
@@ -249,8 +254,13 @@ int intel_fbdev_init(struct drm_device *dev)
         dev_priv->fbdev = ifbdev;
         ifbdev->helper.funcs = &intel_fb_helper_funcs;
 
-        drm_fb_helper_init(dev, &ifbdev->helper, 2,
-                           INTELFB_CONN_LIMIT);
+        ret = drm_fb_helper_init(dev, &ifbdev->helper,
+                                 dev_priv->num_pipe,
+                                 INTELFB_CONN_LIMIT);
+        if (ret) {
+                kfree(ifbdev);
+                return ret;
+        }
 
         drm_fb_helper_single_add_all_connectors(&ifbdev->helper);
         drm_fb_helper_initial_config(&ifbdev->helper, 32);
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index 65727f0a79a3..83bd764b000e 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -59,8 +59,11 @@ static void intel_hdmi_mode_set(struct drm_encoder *encoder,
                 SDVO_VSYNC_ACTIVE_HIGH |
                 SDVO_HSYNC_ACTIVE_HIGH;
 
-        if (hdmi_priv->has_hdmi_sink)
+        if (hdmi_priv->has_hdmi_sink) {
                 sdvox |= SDVO_AUDIO_ENABLE;
+                if (HAS_PCH_CPT(dev))
+                        sdvox |= HDMI_MODE_SELECT;
+        }
 
         if (intel_crtc->pipe == 1) {
                 if (HAS_PCH_CPT(dev))
diff --git a/drivers/gpu/drm/i915/intel_overlay.c b/drivers/gpu/drm/i915/intel_overlay.c
index b0e17b06eb6e..d7ad5139d17c 100644
--- a/drivers/gpu/drm/i915/intel_overlay.c
+++ b/drivers/gpu/drm/i915/intel_overlay.c
@@ -211,9 +211,8 @@ static void intel_overlay_unmap_regs_atomic(struct intel_overlay *overlay)
 static int intel_overlay_on(struct intel_overlay *overlay)
 {
         struct drm_device *dev = overlay->dev;
-        drm_i915_private_t *dev_priv = dev->dev_private;
         int ret;
-        RING_LOCALS;
+        drm_i915_private_t *dev_priv = dev->dev_private;
 
         BUG_ON(overlay->active);
 
@@ -227,11 +226,13 @@ static int intel_overlay_on(struct intel_overlay *overlay)
         OUT_RING(MI_NOOP);
         ADVANCE_LP_RING();
 
-        overlay->last_flip_req = i915_add_request(dev, NULL, 0);
+        overlay->last_flip_req =
+                i915_add_request(dev, NULL, 0, &dev_priv->render_ring);
         if (overlay->last_flip_req == 0)
                 return -ENOMEM;
 
-        ret = i915_do_wait_request(dev, overlay->last_flip_req, 1);
+        ret = i915_do_wait_request(dev,
+                        overlay->last_flip_req, 1, &dev_priv->render_ring);
         if (ret != 0)
                 return ret;
 
@@ -248,7 +249,6 @@ static void intel_overlay_continue(struct intel_overlay *overlay,
         drm_i915_private_t *dev_priv = dev->dev_private;
         u32 flip_addr = overlay->flip_addr;
         u32 tmp;
-        RING_LOCALS;
 
         BUG_ON(!overlay->active);
 
@@ -265,7 +265,8 @@ static void intel_overlay_continue(struct intel_overlay *overlay,
         OUT_RING(flip_addr);
         ADVANCE_LP_RING();
 
-        overlay->last_flip_req = i915_add_request(dev, NULL, 0);
+        overlay->last_flip_req =
+                i915_add_request(dev, NULL, 0, &dev_priv->render_ring);
 }
 
 static int intel_overlay_wait_flip(struct intel_overlay *overlay)
@@ -274,10 +275,10 @@ static int intel_overlay_wait_flip(struct intel_overlay *overlay)
         drm_i915_private_t *dev_priv = dev->dev_private;
         int ret;
         u32 tmp;
-        RING_LOCALS;
 
         if (overlay->last_flip_req != 0) {
-                ret = i915_do_wait_request(dev, overlay->last_flip_req, 1);
+                ret = i915_do_wait_request(dev, overlay->last_flip_req,
+                                1, &dev_priv->render_ring);
                 if (ret == 0) {
                         overlay->last_flip_req = 0;
 
@@ -296,11 +297,13 @@ static int intel_overlay_wait_flip(struct intel_overlay *overlay)
         OUT_RING(MI_NOOP);
         ADVANCE_LP_RING();
 
-        overlay->last_flip_req = i915_add_request(dev, NULL, 0);
+        overlay->last_flip_req =
+                i915_add_request(dev, NULL, 0, &dev_priv->render_ring);
         if (overlay->last_flip_req == 0)
                 return -ENOMEM;
 
-        ret = i915_do_wait_request(dev, overlay->last_flip_req, 1);
+        ret = i915_do_wait_request(dev, overlay->last_flip_req,
+                        1, &dev_priv->render_ring);
         if (ret != 0)
                 return ret;
 
@@ -314,9 +317,8 @@ static int intel_overlay_off(struct intel_overlay *overlay)
 {
         u32 flip_addr = overlay->flip_addr;
         struct drm_device *dev = overlay->dev;
-        drm_i915_private_t *dev_priv = dev->dev_private;
+        drm_i915_private_t *dev_priv = dev->dev_private;
         int ret;
-        RING_LOCALS;
 
         BUG_ON(!overlay->active);
 
@@ -336,11 +338,13 @@ static int intel_overlay_off(struct intel_overlay *overlay)
         OUT_RING(MI_NOOP);
         ADVANCE_LP_RING();
 
-        overlay->last_flip_req = i915_add_request(dev, NULL, 0);
+        overlay->last_flip_req =
+                i915_add_request(dev, NULL, 0, &dev_priv->render_ring);
         if (overlay->last_flip_req == 0)
                 return -ENOMEM;
 
-        ret = i915_do_wait_request(dev, overlay->last_flip_req, 1);
+        ret = i915_do_wait_request(dev, overlay->last_flip_req,
+                        1, &dev_priv->render_ring);
         if (ret != 0)
                 return ret;
 
@@ -354,11 +358,13 @@ static int intel_overlay_off(struct intel_overlay *overlay)
         OUT_RING(MI_NOOP);
         ADVANCE_LP_RING();
 
-        overlay->last_flip_req = i915_add_request(dev, NULL, 0);
+        overlay->last_flip_req =
+                i915_add_request(dev, NULL, 0, &dev_priv->render_ring);
         if (overlay->last_flip_req == 0)
                 return -ENOMEM;
 
-        ret = i915_do_wait_request(dev, overlay->last_flip_req, 1);
+        ret = i915_do_wait_request(dev, overlay->last_flip_req,
+                        1, &dev_priv->render_ring);
         if (ret != 0)
                 return ret;
 
@@ -390,22 +396,23 @@ int intel_overlay_recover_from_interrupt(struct intel_overlay *overlay,
                                          int interruptible)
 {
         struct drm_device *dev = overlay->dev;
-        drm_i915_private_t *dev_priv = dev->dev_private;
         struct drm_gem_object *obj;
+        drm_i915_private_t *dev_priv = dev->dev_private;
         u32 flip_addr;
         int ret;
-        RING_LOCALS;
 
         if (overlay->hw_wedged == HW_WEDGED)
                 return -EIO;
 
         if (overlay->last_flip_req == 0) {
-                overlay->last_flip_req = i915_add_request(dev, NULL, 0);
+                overlay->last_flip_req =
+                        i915_add_request(dev, NULL, 0, &dev_priv->render_ring);
                 if (overlay->last_flip_req == 0)
                         return -ENOMEM;
         }
 
-        ret = i915_do_wait_request(dev, overlay->last_flip_req, interruptible);
+        ret = i915_do_wait_request(dev, overlay->last_flip_req,
+                        interruptible, &dev_priv->render_ring);
         if (ret != 0)
                 return ret;
 
@@ -429,12 +436,13 @@ int intel_overlay_recover_from_interrupt(struct intel_overlay *overlay,
                         OUT_RING(MI_NOOP);
                         ADVANCE_LP_RING();
 
-                        overlay->last_flip_req = i915_add_request(dev, NULL, 0);
+                        overlay->last_flip_req = i915_add_request(dev, NULL,
+                                        0, &dev_priv->render_ring);
                         if (overlay->last_flip_req == 0)
                                 return -ENOMEM;
 
                         ret = i915_do_wait_request(dev, overlay->last_flip_req,
-                                        interruptible);
+                                        interruptible, &dev_priv->render_ring);
                         if (ret != 0)
                                 return ret;
 
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
new file mode 100644
index 000000000000..cea4f1a8709e
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -0,0 +1,849 @@
+/*
+ * Copyright © 2008-2010 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ * Authors:
+ *    Eric Anholt <eric@anholt.net>
+ *    Zou Nan hai <nanhai.zou@intel.com>
+ *    Xiang Hai hao<haihao.xiang@intel.com>
+ *
+ */
+
+#include "drmP.h"
+#include "drm.h"
+#include "i915_drv.h"
+#include "i915_drm.h"
+#include "i915_trace.h"
+
+static void
+render_ring_flush(struct drm_device *dev,
+                struct intel_ring_buffer *ring,
+                u32     invalidate_domains,
+                u32     flush_domains)
+{
+#if WATCH_EXEC
+        DRM_INFO("%s: invalidate %08x flush %08x\n", __func__,
+                  invalidate_domains, flush_domains);
+#endif
+        u32 cmd;
+        trace_i915_gem_request_flush(dev, ring->next_seqno,
+                                     invalidate_domains, flush_domains);
+
+        if ((invalidate_domains | flush_domains) & I915_GEM_GPU_DOMAINS) {
+                /*
+                 * read/write caches:
+                 *
+                 * I915_GEM_DOMAIN_RENDER is always invalidated, but is
+                 * only flushed if MI_NO_WRITE_FLUSH is unset.  On 965, it is
+                 * also flushed at 2d versus 3d pipeline switches.
+                 *
+                 * read-only caches:
+                 *
+                 * I915_GEM_DOMAIN_SAMPLER is flushed on pre-965 if
+                 * MI_READ_FLUSH is set, and is always flushed on 965.
+                 *
+                 * I915_GEM_DOMAIN_COMMAND may not exist?
+                 *
+                 * I915_GEM_DOMAIN_INSTRUCTION, which exists on 965, is
+                 * invalidated when MI_EXE_FLUSH is set.
+                 *
+                 * I915_GEM_DOMAIN_VERTEX, which exists on 965, is
+                 * invalidated with every MI_FLUSH.
+                 *
+                 * TLBs:
+                 *
+                 * On 965, TLBs associated with I915_GEM_DOMAIN_COMMAND
+                 * and I915_GEM_DOMAIN_CPU in are invalidated at PTE write and
+                 * I915_GEM_DOMAIN_RENDER and I915_GEM_DOMAIN_SAMPLER
+                 * are flushed at any MI_FLUSH.
+                 */
+
+                cmd = MI_FLUSH | MI_NO_WRITE_FLUSH;
+                if ((invalidate_domains|flush_domains) &
+                    I915_GEM_DOMAIN_RENDER)
+                        cmd &= ~MI_NO_WRITE_FLUSH;
+                if (!IS_I965G(dev)) {
+                        /*
+                         * On the 965, the sampler cache always gets flushed
+                         * and this bit is reserved.
+                         */
+                        if (invalidate_domains & I915_GEM_DOMAIN_SAMPLER)
+                                cmd |= MI_READ_FLUSH;
+                }
+                if (invalidate_domains & I915_GEM_DOMAIN_INSTRUCTION)
+                        cmd |= MI_EXE_FLUSH;
+
+#if WATCH_EXEC
+                DRM_INFO("%s: queue flush %08x to ring\n", __func__, cmd);
+#endif
+                intel_ring_begin(dev, ring, 8);
+                intel_ring_emit(dev, ring, cmd);
+                intel_ring_emit(dev, ring, MI_NOOP);
+                intel_ring_advance(dev, ring);
+        }
+}
+
+static unsigned int render_ring_get_head(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        return I915_READ(PRB0_HEAD) & HEAD_ADDR;
+}
+
+static unsigned int render_ring_get_tail(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        return I915_READ(PRB0_TAIL) & TAIL_ADDR;
+}
+
+static unsigned int render_ring_get_active_head(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        u32 acthd_reg = IS_I965G(dev) ? ACTHD_I965 : ACTHD;
+
+        return I915_READ(acthd_reg);
+}
+
+static void render_ring_advance_ring(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        I915_WRITE(PRB0_TAIL, ring->tail);
+}
+
+static int init_ring_common(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        u32 head;
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        struct drm_i915_gem_object *obj_priv;
+        obj_priv = to_intel_bo(ring->gem_object);
+
+        /* Stop the ring if it's running. */
+        I915_WRITE(ring->regs.ctl, 0);
+        I915_WRITE(ring->regs.head, 0);
+        I915_WRITE(ring->regs.tail, 0);
+
+        /* Initialize the ring. */
+        I915_WRITE(ring->regs.start, obj_priv->gtt_offset);
+        head = ring->get_head(dev, ring);
+
+        /* G45 ring initialization fails to reset head to zero */
+        if (head != 0) {
+                DRM_ERROR("%s head not reset to zero "
+                                "ctl %08x head %08x tail %08x start %08x\n",
+                                ring->name,
+                                I915_READ(ring->regs.ctl),
+                                I915_READ(ring->regs.head),
+                                I915_READ(ring->regs.tail),
+                                I915_READ(ring->regs.start));
+
+                I915_WRITE(ring->regs.head, 0);
+
+                DRM_ERROR("%s head forced to zero "
+                                "ctl %08x head %08x tail %08x start %08x\n",
+                                ring->name,
+                                I915_READ(ring->regs.ctl),
+                                I915_READ(ring->regs.head),
+                                I915_READ(ring->regs.tail),
+                                I915_READ(ring->regs.start));
+        }
+
+        I915_WRITE(ring->regs.ctl,
+                        ((ring->gem_object->size - PAGE_SIZE) & RING_NR_PAGES)
+                        | RING_NO_REPORT | RING_VALID);
+
+        head = I915_READ(ring->regs.head) & HEAD_ADDR;
+        /* If the head is still not zero, the ring is dead */
+        if (head != 0) {
+                DRM_ERROR("%s initialization failed "
+                                "ctl %08x head %08x tail %08x start %08x\n",
+                                ring->name,
+                                I915_READ(ring->regs.ctl),
+                                I915_READ(ring->regs.head),
+                                I915_READ(ring->regs.tail),
+                                I915_READ(ring->regs.start));
+                return -EIO;
+        }
+
+        if (!drm_core_check_feature(dev, DRIVER_MODESET))
+                i915_kernel_lost_context(dev);
+        else {
+                ring->head = ring->get_head(dev, ring);
+                ring->tail = ring->get_tail(dev, ring);
+                ring->space = ring->head - (ring->tail + 8);
+                if (ring->space < 0)
+                        ring->space += ring->size;
+        }
+        return 0;
+}
+
+static int init_render_ring(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        int ret = init_ring_common(dev, ring);
+        if (IS_I9XX(dev) && !IS_GEN3(dev)) {
+                I915_WRITE(MI_MODE,
+                                (VS_TIMER_DISPATCH) << 16 | VS_TIMER_DISPATCH);
+        }
+        return ret;
+}
+
+#define PIPE_CONTROL_FLUSH(addr)                                        \
+do {                                                                    \
+        OUT_RING(GFX_OP_PIPE_CONTROL | PIPE_CONTROL_QW_WRITE |          \
+                 PIPE_CONTROL_DEPTH_STALL | 2);                         \
+        OUT_RING(addr | PIPE_CONTROL_GLOBAL_GTT);                       \
+        OUT_RING(0);                                                    \
+        OUT_RING(0);                                                    \
+} while (0)
+
+/**
+ * Creates a new sequence number, emitting a write of it to the status page
+ * plus an interrupt, which will trigger i915_user_interrupt_handler.
+ *
+ * Must be called with struct_lock held.
+ *
+ * Returned sequence numbers are nonzero on success.
+ */
+static u32
+render_ring_add_request(struct drm_device *dev,
+                struct intel_ring_buffer *ring,
+                struct drm_file *file_priv,
+                u32 flush_domains)
+{
+        u32 seqno;
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        seqno = intel_ring_get_seqno(dev, ring);
+
+        if (IS_GEN6(dev)) {
+                BEGIN_LP_RING(6);
+                OUT_RING(GFX_OP_PIPE_CONTROL | 3);
+                OUT_RING(PIPE_CONTROL_QW_WRITE |
+                         PIPE_CONTROL_WC_FLUSH | PIPE_CONTROL_IS_FLUSH |
+                         PIPE_CONTROL_NOTIFY);
+                OUT_RING(dev_priv->seqno_gfx_addr | PIPE_CONTROL_GLOBAL_GTT);
+                OUT_RING(seqno);
+                OUT_RING(0);
+                OUT_RING(0);
+                ADVANCE_LP_RING();
+        } else if (HAS_PIPE_CONTROL(dev)) {
+                u32 scratch_addr = dev_priv->seqno_gfx_addr + 128;
+
+                /*
+                 * Workaround qword write incoherence by flushing the
+                 * PIPE_NOTIFY buffers out to memory before requesting
+                 * an interrupt.
+                 */
+                BEGIN_LP_RING(32);
+                OUT_RING(GFX_OP_PIPE_CONTROL | PIPE_CONTROL_QW_WRITE |
+                         PIPE_CONTROL_WC_FLUSH | PIPE_CONTROL_TC_FLUSH);
+                OUT_RING(dev_priv->seqno_gfx_addr | PIPE_CONTROL_GLOBAL_GTT);
+                OUT_RING(seqno);
+                OUT_RING(0);
+                PIPE_CONTROL_FLUSH(scratch_addr);
+                scratch_addr += 128; /* write to separate cachelines */
+                PIPE_CONTROL_FLUSH(scratch_addr);
+                scratch_addr += 128;
+                PIPE_CONTROL_FLUSH(scratch_addr);
+                scratch_addr += 128;
+                PIPE_CONTROL_FLUSH(scratch_addr);
+                scratch_addr += 128;
+                PIPE_CONTROL_FLUSH(scratch_addr);
+                scratch_addr += 128;
+                PIPE_CONTROL_FLUSH(scratch_addr);
+                OUT_RING(GFX_OP_PIPE_CONTROL | PIPE_CONTROL_QW_WRITE |
+                         PIPE_CONTROL_WC_FLUSH | PIPE_CONTROL_TC_FLUSH |
+                         PIPE_CONTROL_NOTIFY);
+                OUT_RING(dev_priv->seqno_gfx_addr | PIPE_CONTROL_GLOBAL_GTT);
+                OUT_RING(seqno);
+                OUT_RING(0);
+                ADVANCE_LP_RING();
+        } else {
+                BEGIN_LP_RING(4);
+                OUT_RING(MI_STORE_DWORD_INDEX);
+                OUT_RING(I915_GEM_HWS_INDEX << MI_STORE_DWORD_INDEX_SHIFT);
+                OUT_RING(seqno);
+
+                OUT_RING(MI_USER_INTERRUPT);
+                ADVANCE_LP_RING();
+        }
+        return seqno;
+}
+
+static u32
+render_ring_get_gem_seqno(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+        if (HAS_PIPE_CONTROL(dev))
+                return ((volatile u32 *)(dev_priv->seqno_page))[0];
+        else
+                return intel_read_status_page(ring, I915_GEM_HWS_INDEX);
+}
+
+static void
+render_ring_get_user_irq(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+        unsigned long irqflags;
+
+        spin_lock_irqsave(&dev_priv->user_irq_lock, irqflags);
+        if (dev->irq_enabled && (++ring->user_irq_refcount == 1)) {
+                if (HAS_PCH_SPLIT(dev))
+                        ironlake_enable_graphics_irq(dev_priv, GT_PIPE_NOTIFY);
+                else
+                        i915_enable_irq(dev_priv, I915_USER_INTERRUPT);
+        }
+        spin_unlock_irqrestore(&dev_priv->user_irq_lock, irqflags);
+}
+
+static void
+render_ring_put_user_irq(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = (drm_i915_private_t *) dev->dev_private;
+        unsigned long irqflags;
+
+        spin_lock_irqsave(&dev_priv->user_irq_lock, irqflags);
+        BUG_ON(dev->irq_enabled && ring->user_irq_refcount <= 0);
+        if (dev->irq_enabled && (--ring->user_irq_refcount == 0)) {
+                if (HAS_PCH_SPLIT(dev))
+                        ironlake_disable_graphics_irq(dev_priv, GT_PIPE_NOTIFY);
+                else
+                        i915_disable_irq(dev_priv, I915_USER_INTERRUPT);
+        }
+        spin_unlock_irqrestore(&dev_priv->user_irq_lock, irqflags);
+}
+
+static void render_setup_status_page(struct drm_device *dev,
+        struct  intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        if (IS_GEN6(dev)) {
+                I915_WRITE(HWS_PGA_GEN6, ring->status_page.gfx_addr);
+                I915_READ(HWS_PGA_GEN6); /* posting read */
+        } else {
+                I915_WRITE(HWS_PGA, ring->status_page.gfx_addr);
+                I915_READ(HWS_PGA); /* posting read */
+        }
+
+}
+
+void
+bsd_ring_flush(struct drm_device *dev,
+                struct intel_ring_buffer *ring,
+                u32     invalidate_domains,
+                u32     flush_domains)
+{
+        intel_ring_begin(dev, ring, 8);
+        intel_ring_emit(dev, ring, MI_FLUSH);
+        intel_ring_emit(dev, ring, MI_NOOP);
+        intel_ring_advance(dev, ring);
+}
+
+static inline unsigned int bsd_ring_get_head(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        return I915_READ(BSD_RING_HEAD) & HEAD_ADDR;
+}
+
+static inline unsigned int bsd_ring_get_tail(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        return I915_READ(BSD_RING_TAIL) & TAIL_ADDR;
+}
+
+static inline unsigned int bsd_ring_get_active_head(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        return I915_READ(BSD_RING_ACTHD);
+}
+
+static inline void bsd_ring_advance_ring(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        I915_WRITE(BSD_RING_TAIL, ring->tail);
+}
+
+static int init_bsd_ring(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        return init_ring_common(dev, ring);
+}
+
+static u32
+bsd_ring_add_request(struct drm_device *dev,
+                struct intel_ring_buffer *ring,
+                struct drm_file *file_priv,
+                u32 flush_domains)
+{
+        u32 seqno;
+        seqno = intel_ring_get_seqno(dev, ring);
+        intel_ring_begin(dev, ring, 4);
+        intel_ring_emit(dev, ring, MI_STORE_DWORD_INDEX);
+        intel_ring_emit(dev, ring,
+                        I915_GEM_HWS_INDEX << MI_STORE_DWORD_INDEX_SHIFT);
+        intel_ring_emit(dev, ring, seqno);
+        intel_ring_emit(dev, ring, MI_USER_INTERRUPT);
+        intel_ring_advance(dev, ring);
+
+        DRM_DEBUG_DRIVER("%s %d\n", ring->name, seqno);
+
+        return seqno;
+}
+
+static void bsd_setup_status_page(struct drm_device *dev,
+                struct  intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        I915_WRITE(BSD_HWS_PGA, ring->status_page.gfx_addr);
+        I915_READ(BSD_HWS_PGA);
+}
+
+static void
+bsd_ring_get_user_irq(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        /* do nothing */
+}
+static void
+bsd_ring_put_user_irq(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        /* do nothing */
+}
+
+static u32
+bsd_ring_get_gem_seqno(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        return intel_read_status_page(ring, I915_GEM_HWS_INDEX);
+}
+
+static int
+bsd_ring_dispatch_gem_execbuffer(struct drm_device *dev,
+                struct intel_ring_buffer *ring,
+                struct drm_i915_gem_execbuffer2 *exec,
+                struct drm_clip_rect *cliprects,
+                uint64_t exec_offset)
+{
+        uint32_t exec_start;
+        exec_start = (uint32_t) exec_offset + exec->batch_start_offset;
+        intel_ring_begin(dev, ring, 2);
+        intel_ring_emit(dev, ring, MI_BATCH_BUFFER_START |
+                        (2 << 6) | MI_BATCH_NON_SECURE_I965);
+        intel_ring_emit(dev, ring, exec_start);
+        intel_ring_advance(dev, ring);
+        return 0;
+}
+
+
+static int
+render_ring_dispatch_gem_execbuffer(struct drm_device *dev,
+                struct intel_ring_buffer *ring,
+                struct drm_i915_gem_execbuffer2 *exec,
+                struct drm_clip_rect *cliprects,
+                uint64_t exec_offset)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        int nbox = exec->num_cliprects;
+        int i = 0, count;
+        uint32_t exec_start, exec_len;
+        exec_start = (uint32_t) exec_offset + exec->batch_start_offset;
+        exec_len = (uint32_t) exec->batch_len;
+
+        trace_i915_gem_request_submit(dev, dev_priv->mm.next_gem_seqno + 1);
+
+        count = nbox ? nbox : 1;
+
+        for (i = 0; i < count; i++) {
+                if (i < nbox) {
+                        int ret = i915_emit_box(dev, cliprects, i,
+                                                exec->DR1, exec->DR4);
+                        if (ret)
+                                return ret;
+                }
+
+                if (IS_I830(dev) || IS_845G(dev)) {
+                        intel_ring_begin(dev, ring, 4);
+                        intel_ring_emit(dev, ring, MI_BATCH_BUFFER);
+                        intel_ring_emit(dev, ring,
+                                        exec_start | MI_BATCH_NON_SECURE);
+                        intel_ring_emit(dev, ring, exec_start + exec_len - 4);
+                        intel_ring_emit(dev, ring, 0);
+                } else {
+                        intel_ring_begin(dev, ring, 4);
+                        if (IS_I965G(dev)) {
+                                intel_ring_emit(dev, ring,
+                                                MI_BATCH_BUFFER_START | (2 << 6)
+                                                | MI_BATCH_NON_SECURE_I965);
+                                intel_ring_emit(dev, ring, exec_start);
+                        } else {
+                                intel_ring_emit(dev, ring, MI_BATCH_BUFFER_START
+                                                | (2 << 6));
+                                intel_ring_emit(dev, ring, exec_start |
+                                                MI_BATCH_NON_SECURE);
+                        }
+                }
+                intel_ring_advance(dev, ring);
+        }
+
+        /* XXX breadcrumb */
+        return 0;
+}
+
+static void cleanup_status_page(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        struct drm_gem_object *obj;
+        struct drm_i915_gem_object *obj_priv;
+
+        obj = ring->status_page.obj;
+        if (obj == NULL)
+                return;
+        obj_priv = to_intel_bo(obj);
+
+        kunmap(obj_priv->pages[0]);
+        i915_gem_object_unpin(obj);
+        drm_gem_object_unreference(obj);
+        ring->status_page.obj = NULL;
+
+        memset(&dev_priv->hws_map, 0, sizeof(dev_priv->hws_map));
+}
+
+static int init_status_page(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        drm_i915_private_t *dev_priv = dev->dev_private;
+        struct drm_gem_object *obj;
+        struct drm_i915_gem_object *obj_priv;
+        int ret;
+
+        obj = i915_gem_alloc_object(dev, 4096);
+        if (obj == NULL) {
+                DRM_ERROR("Failed to allocate status page\n");
+                ret = -ENOMEM;
+                goto err;
+        }
+        obj_priv = to_intel_bo(obj);
+        obj_priv->agp_type = AGP_USER_CACHED_MEMORY;
+
+        ret = i915_gem_object_pin(obj, 4096);
+        if (ret != 0) {
+                goto err_unref;
+        }
+
+        ring->status_page.gfx_addr = obj_priv->gtt_offset;
+        ring->status_page.page_addr = kmap(obj_priv->pages[0]);
+        if (ring->status_page.page_addr == NULL) {
+                memset(&dev_priv->hws_map, 0, sizeof(dev_priv->hws_map));
+                goto err_unpin;
+        }
+        ring->status_page.obj = obj;
+        memset(ring->status_page.page_addr, 0, PAGE_SIZE);
+
+        ring->setup_status_page(dev, ring);
+        DRM_DEBUG_DRIVER("%s hws offset: 0x%08x\n",
+                        ring->name, ring->status_page.gfx_addr);
+
+        return 0;
+
+err_unpin:
+        i915_gem_object_unpin(obj);
+err_unref:
+        drm_gem_object_unreference(obj);
+err:
+        return ret;
+}
+
+
+int intel_init_ring_buffer(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        int ret;
+        struct drm_i915_gem_object *obj_priv;
+        struct drm_gem_object *obj;
+        ring->dev = dev;
+
+        if (I915_NEED_GFX_HWS(dev)) {
+                ret = init_status_page(dev, ring);
+                if (ret)
+                        return ret;
+        }
+
+        obj = i915_gem_alloc_object(dev, ring->size);
+        if (obj == NULL) {
+                DRM_ERROR("Failed to allocate ringbuffer\n");
+                ret = -ENOMEM;
+                goto cleanup;
+        }
+
+        ring->gem_object = obj;
+
+        ret = i915_gem_object_pin(obj, ring->alignment);
+        if (ret != 0) {
+                drm_gem_object_unreference(obj);
+                goto cleanup;
+        }
+
+        obj_priv = to_intel_bo(obj);
+        ring->map.size = ring->size;
+        ring->map.offset = dev->agp->base + obj_priv->gtt_offset;
+        ring->map.type = 0;
+        ring->map.flags = 0;
+        ring->map.mtrr = 0;
+
+        drm_core_ioremap_wc(&ring->map, dev);
+        if (ring->map.handle == NULL) {
+                DRM_ERROR("Failed to map ringbuffer.\n");
+                i915_gem_object_unpin(obj);
+                drm_gem_object_unreference(obj);
+                ret = -EINVAL;
+                goto cleanup;
+        }
+
+        ring->virtual_start = ring->map.handle;
+        ret = ring->init(dev, ring);
+        if (ret != 0) {
+                intel_cleanup_ring_buffer(dev, ring);
+                return ret;
+        }
+
+        if (!drm_core_check_feature(dev, DRIVER_MODESET))
+                i915_kernel_lost_context(dev);
+        else {
+                ring->head = ring->get_head(dev, ring);
+                ring->tail = ring->get_tail(dev, ring);
+                ring->space = ring->head - (ring->tail + 8);
+                if (ring->space < 0)
+                        ring->space += ring->size;
+        }
+        INIT_LIST_HEAD(&ring->active_list);
+        INIT_LIST_HEAD(&ring->request_list);
+        return ret;
+cleanup:
+        cleanup_status_page(dev, ring);
+        return ret;
+}
+
+void intel_cleanup_ring_buffer(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        if (ring->gem_object == NULL)
+                return;
+
+        drm_core_ioremapfree(&ring->map, dev);
+
+        i915_gem_object_unpin(ring->gem_object);
+        drm_gem_object_unreference(ring->gem_object);
+        ring->gem_object = NULL;
+        cleanup_status_page(dev, ring);
+}
+
+int intel_wrap_ring_buffer(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        unsigned int *virt;
+        int rem;
+        rem = ring->size - ring->tail;
+
+        if (ring->space < rem) {
+                int ret = intel_wait_ring_buffer(dev, ring, rem);
+                if (ret)
+                        return ret;
+        }
+
+        virt = (unsigned int *)(ring->virtual_start + ring->tail);
+        rem /= 4;
+        while (rem--)
+                *virt++ = MI_NOOP;
+
+        ring->tail = 0;
+
+        return 0;
+}
+
+int intel_wait_ring_buffer(struct drm_device *dev,
+                struct intel_ring_buffer *ring, int n)
+{
+        unsigned long end;
+
+        trace_i915_ring_wait_begin (dev);
+        end = jiffies + 3 * HZ;
+        do {
+                ring->head = ring->get_head(dev, ring);
+                ring->space = ring->head - (ring->tail + 8);
+                if (ring->space < 0)
+                        ring->space += ring->size;
+                if (ring->space >= n) {
+                        trace_i915_ring_wait_end (dev);
+                        return 0;
+                }
+
+                if (dev->primary->master) {
+                        struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
+                        if (master_priv->sarea_priv)
+                                master_priv->sarea_priv->perf_boxes |= I915_BOX_WAIT;
+                }
+
+                yield();
+        } while (!time_after(jiffies, end));
+        trace_i915_ring_wait_end (dev);
+        return -EBUSY;
+}
+
+void intel_ring_begin(struct drm_device *dev,
+                struct intel_ring_buffer *ring, int n)
+{
+        if (unlikely(ring->tail + n > ring->size))
+                intel_wrap_ring_buffer(dev, ring);
+        if (unlikely(ring->space < n))
+                intel_wait_ring_buffer(dev, ring, n);
+}
+
+void intel_ring_emit(struct drm_device *dev,
+                struct intel_ring_buffer *ring, unsigned int data)
+{
+        unsigned int *virt = ring->virtual_start + ring->tail;
+        *virt = data;
+        ring->tail += 4;
+        ring->tail &= ring->size - 1;
+        ring->space -= 4;
+}
+
+void intel_ring_advance(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        ring->advance_ring(dev, ring);
+}
+
+void intel_fill_struct(struct drm_device *dev,
+                struct intel_ring_buffer *ring,
+                void *data,
+                unsigned int len)
+{
+        unsigned int *virt = ring->virtual_start + ring->tail;
+        BUG_ON((len&~(4-1)) != 0);
+        intel_ring_begin(dev, ring, len);
+        memcpy(virt, data, len);
+        ring->tail += len;
+        ring->tail &= ring->size - 1;
+        ring->space -= len;
+        intel_ring_advance(dev, ring);
+}
+
+u32 intel_ring_get_seqno(struct drm_device *dev,
+                struct intel_ring_buffer *ring)
+{
+        u32 seqno;
+        seqno = ring->next_seqno;
+
+        /* reserve 0 for non-seqno */
+        if (++ring->next_seqno == 0)
+                ring->next_seqno = 1;
+        return seqno;
+}
+
+struct intel_ring_buffer render_ring = {
+        .name                   = "render ring",
+        .regs                   = {
+                .ctl = PRB0_CTL,
+                .head = PRB0_HEAD,
+                .tail = PRB0_TAIL,
+                .start = PRB0_START
+        },
+        .ring_flag              = I915_EXEC_RENDER,
+        .size                   = 32 * PAGE_SIZE,
+        .alignment              = PAGE_SIZE,
+        .virtual_start          = NULL,
+        .dev                    = NULL,
+        .gem_object             = NULL,
+        .head                   = 0,
+        .tail                   = 0,
+        .space                  = 0,
+        .next_seqno             = 1,
+        .user_irq_refcount      = 0,
+        .irq_gem_seqno          = 0,
+        .waiting_gem_seqno      = 0,
+        .setup_status_page      = render_setup_status_page,
+        .init                   = init_render_ring,
+        .get_head               = render_ring_get_head,
+        .get_tail               = render_ring_get_tail,
+        .get_active_head        = render_ring_get_active_head,
+        .advance_ring           = render_ring_advance_ring,
+        .flush                  = render_ring_flush,
+        .add_request            = render_ring_add_request,
+        .get_gem_seqno          = render_ring_get_gem_seqno,
+        .user_irq_get           = render_ring_get_user_irq,
+        .user_irq_put           = render_ring_put_user_irq,
+        .dispatch_gem_execbuffer = render_ring_dispatch_gem_execbuffer,
+        .status_page            = {NULL, 0, NULL},
+        .map                    = {0,}
+};
+
+/* ring buffer for bit-stream decoder */
+
+struct intel_ring_buffer bsd_ring = {
+        .name                   = "bsd ring",
+        .regs                   = {
+                .ctl = BSD_RING_CTL,
+                .head = BSD_RING_HEAD,
+                .tail = BSD_RING_TAIL,
+                .start = BSD_RING_START
+        },
+        .ring_flag              = I915_EXEC_BSD,
+        .size                   = 32 * PAGE_SIZE,
+        .alignment              = PAGE_SIZE,
+        .virtual_start          = NULL,
+        .dev                    = NULL,
+        .gem_object             = NULL,
+        .head                   = 0,
+        .tail                   = 0,
+        .space                  = 0,
+        .next_seqno             = 1,
+        .user_irq_refcount      = 0,
+        .irq_gem_seqno          = 0,
+        .waiting_gem_seqno      = 0,
+        .setup_status_page      = bsd_setup_status_page,
+        .init                   = init_bsd_ring,
+        .get_head               = bsd_ring_get_head,
+        .get_tail               = bsd_ring_get_tail,
+        .get_active_head        = bsd_ring_get_active_head,
+        .advance_ring           = bsd_ring_advance_ring,
+        .flush                  = bsd_ring_flush,
+        .add_request            = bsd_ring_add_request,
+        .get_gem_seqno          = bsd_ring_get_gem_seqno,
+        .user_irq_get           = bsd_ring_get_user_irq,
+        .user_irq_put           = bsd_ring_put_user_irq,
+        .dispatch_gem_execbuffer = bsd_ring_dispatch_gem_execbuffer,
+        .status_page            = {NULL, 0, NULL},
+        .map                    = {0,}
+};
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
new file mode 100644
index 000000000000..d5568d3766de
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -0,0 +1,124 @@
+#ifndef _INTEL_RINGBUFFER_H_
+#define _INTEL_RINGBUFFER_H_
+
+struct  intel_hw_status_page {
+        void            *page_addr;
+        unsigned int    gfx_addr;
+        struct          drm_gem_object *obj;
+};
+
+struct drm_i915_gem_execbuffer2;
+struct  intel_ring_buffer {
+        const char      *name;
+        struct          ring_regs {
+                        u32 ctl;
+                        u32 head;
+                        u32 tail;
+                        u32 start;
+        } regs;
+        unsigned int    ring_flag;
+        unsigned long   size;
+        unsigned int    alignment;
+        void            *virtual_start;
+        struct          drm_device *dev;
+        struct          drm_gem_object *gem_object;
+
+        unsigned int    head;
+        unsigned int    tail;
+        unsigned int    space;
+        u32             next_seqno;
+        struct intel_hw_status_page status_page;
+
+        u32             irq_gem_seqno;          /* last seq seem at irq time */
+        u32             waiting_gem_seqno;
+        int             user_irq_refcount;
+        void            (*user_irq_get)(struct drm_device *dev,
+                        struct intel_ring_buffer *ring);
+        void            (*user_irq_put)(struct drm_device *dev,
+                        struct intel_ring_buffer *ring);
+        void            (*setup_status_page)(struct drm_device *dev,
+                        struct  intel_ring_buffer *ring);
+
+        int             (*init)(struct drm_device *dev,
+                        struct intel_ring_buffer *ring);
+
+        unsigned int    (*get_head)(struct drm_device *dev,
+                        struct intel_ring_buffer *ring);
+        unsigned int    (*get_tail)(struct drm_device *dev,
+                        struct intel_ring_buffer *ring);
+        unsigned int    (*get_active_head)(struct drm_device *dev,
+                        struct intel_ring_buffer *ring);
+        void            (*advance_ring)(struct drm_device *dev,
+                        struct intel_ring_buffer *ring);
+        void            (*flush)(struct drm_device *dev,
+                        struct intel_ring_buffer *ring,
+                        u32     invalidate_domains,
+                        u32     flush_domains);
+        u32             (*add_request)(struct drm_device *dev,
+                        struct intel_ring_buffer *ring,
+                        struct drm_file *file_priv,
+                        u32 flush_domains);
+        u32             (*get_gem_seqno)(struct drm_device *dev,
+                        struct intel_ring_buffer *ring);
+        int             (*dispatch_gem_execbuffer)(struct drm_device *dev,
+                        struct intel_ring_buffer *ring,
+                        struct drm_i915_gem_execbuffer2 *exec,
+                        struct drm_clip_rect *cliprects,
+                        uint64_t exec_offset);
+
+        /**
+         * List of objects currently involved in rendering from the
+         * ringbuffer.
+         *
+         * Includes buffers having the contents of their GPU caches
+         * flushed, not necessarily primitives.  last_rendering_seqno
+         * represents when the rendering involved will be completed.
+         *
+         * A reference is held on the buffer while on this list.
+         */
+        struct list_head active_list;
+
+        /**
+         * List of breadcrumbs associated with GPU requests currently
+         * outstanding.
+         */
+        struct list_head request_list;
+
+        wait_queue_head_t irq_queue;
+        drm_local_map_t map;
+};
+
+static inline u32
+intel_read_status_page(struct intel_ring_buffer *ring,
+                int reg)
+{
+        u32 *regs = ring->status_page.page_addr;
+        return regs[reg];
+}
+
+int intel_init_ring_buffer(struct drm_device *dev,
+                struct intel_ring_buffer *ring);
+void intel_cleanup_ring_buffer(struct drm_device *dev,
+                struct intel_ring_buffer *ring);
+int intel_wait_ring_buffer(struct drm_device *dev,
+                struct intel_ring_buffer *ring, int n);
+int intel_wrap_ring_buffer(struct drm_device *dev,
+                struct intel_ring_buffer *ring);
+void intel_ring_begin(struct drm_device *dev,
+                struct intel_ring_buffer *ring, int n);
+void intel_ring_emit(struct drm_device *dev,
+                struct intel_ring_buffer *ring, u32 data);
+void intel_fill_struct(struct drm_device *dev,
+                struct intel_ring_buffer *ring,
+                void *data,
+                unsigned int len);
+void intel_ring_advance(struct drm_device *dev,
+                struct intel_ring_buffer *ring);
+
+u32 intel_ring_get_seqno(struct drm_device *dev,
+                struct intel_ring_buffer *ring);
+
+extern struct intel_ring_buffer render_ring;
+extern struct intel_ring_buffer bsd_ring;
+
+#endif /* _INTEL_RINGBUFFER_H_ */
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index aba72c489a2f..76993ac16cc1 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -1479,7 +1479,7 @@ intel_find_analog_connector(struct drm_device *dev)
                 intel_encoder = enc_to_intel_encoder(encoder);
                 if (intel_encoder->type == INTEL_OUTPUT_ANALOG) {
                         list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-                                if (connector && encoder == intel_attached_encoder(connector))
+                                if (encoder == intel_attached_encoder(connector))
                                         return connector;
                         }
                 }
diff --git a/drivers/gpu/drm/nouveau/nouveau_acpi.c b/drivers/gpu/drm/nouveau/nouveau_acpi.c
index e13f6af0037a..d4bcca8a5133 100644
--- a/drivers/gpu/drm/nouveau/nouveau_acpi.c
+++ b/drivers/gpu/drm/nouveau/nouveau_acpi.c
@@ -34,7 +34,7 @@
 static struct nouveau_dsm_priv {
         bool dsm_detected;
         acpi_handle dhandle;
-        acpi_handle dsm_handle;
+        acpi_handle rom_handle;
 } nouveau_dsm_priv;
 
 static const char nouveau_dsm_muid[] = {
@@ -107,9 +107,9 @@ static int nouveau_dsm_set_discrete_state(acpi_handle handle, enum vga_switchero
 static int nouveau_dsm_switchto(enum vga_switcheroo_client_id id)
 {
         if (id == VGA_SWITCHEROO_IGD)
-                return nouveau_dsm_switch_mux(nouveau_dsm_priv.dsm_handle, NOUVEAU_DSM_LED_STAMINA);
+                return nouveau_dsm_switch_mux(nouveau_dsm_priv.dhandle, NOUVEAU_DSM_LED_STAMINA);
         else
-                return nouveau_dsm_switch_mux(nouveau_dsm_priv.dsm_handle, NOUVEAU_DSM_LED_SPEED);
+                return nouveau_dsm_switch_mux(nouveau_dsm_priv.dhandle, NOUVEAU_DSM_LED_SPEED);
 }
 
 static int nouveau_dsm_power_state(enum vga_switcheroo_client_id id,
@@ -118,7 +118,7 @@ static int nouveau_dsm_power_state(enum vga_switcheroo_client_id id,
         if (id == VGA_SWITCHEROO_IGD)
                 return 0;
 
-        return nouveau_dsm_set_discrete_state(nouveau_dsm_priv.dsm_handle, state);
+        return nouveau_dsm_set_discrete_state(nouveau_dsm_priv.dhandle, state);
 }
 
 static int nouveau_dsm_init(void)
@@ -151,18 +151,18 @@ static bool nouveau_dsm_pci_probe(struct pci_dev *pdev)
         dhandle = DEVICE_ACPI_HANDLE(&pdev->dev);
         if (!dhandle)
                 return false;
+
         status = acpi_get_handle(dhandle, "_DSM", &nvidia_handle);
         if (ACPI_FAILURE(status)) {
                 return false;
         }
 
-        ret= nouveau_dsm(nvidia_handle, NOUVEAU_DSM_SUPPORTED,
-                         NOUVEAU_DSM_SUPPORTED_FUNCTIONS, &result);
+        ret = nouveau_dsm(dhandle, NOUVEAU_DSM_SUPPORTED,
+                          NOUVEAU_DSM_SUPPORTED_FUNCTIONS, &result);
         if (ret < 0)
                 return false;
 
         nouveau_dsm_priv.dhandle = dhandle;
-        nouveau_dsm_priv.dsm_handle = nvidia_handle;
         return true;
 }
 
@@ -173,6 +173,7 @@ static bool nouveau_dsm_detect(void)
         struct pci_dev *pdev = NULL;
         int has_dsm = 0;
         int vga_count = 0;
+
         while ((pdev = pci_get_class(PCI_CLASS_DISPLAY_VGA << 8, pdev)) != NULL) {
                 vga_count++;
 
@@ -180,7 +181,7 @@ static bool nouveau_dsm_detect(void)
         }
 
         if (vga_count == 2 && has_dsm) {
-                acpi_get_name(nouveau_dsm_priv.dsm_handle, ACPI_FULL_PATHNAME, &buffer);
+                acpi_get_name(nouveau_dsm_priv.dhandle, ACPI_FULL_PATHNAME, &buffer);
                 printk(KERN_INFO "VGA switcheroo: detected DSM switching method %s handle\n",
                        acpi_method_name);
                 nouveau_dsm_priv.dsm_detected = true;
@@ -204,3 +205,57 @@ void nouveau_unregister_dsm_handler(void)
 {
         vga_switcheroo_unregister_handler();
 }
+
+/* retrieve the ROM in 4k blocks */
+static int nouveau_rom_call(acpi_handle rom_handle, uint8_t *bios,
+                            int offset, int len)
+{
+        acpi_status status;
+        union acpi_object rom_arg_elements[2], *obj;
+        struct acpi_object_list rom_arg;
+        struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL};
+
+        rom_arg.count = 2;
+        rom_arg.pointer = &rom_arg_elements[0];
+
+        rom_arg_elements[0].type = ACPI_TYPE_INTEGER;
+        rom_arg_elements[0].integer.value = offset;
+
+        rom_arg_elements[1].type = ACPI_TYPE_INTEGER;
+        rom_arg_elements[1].integer.value = len;
+
+        status = acpi_evaluate_object(rom_handle, NULL, &rom_arg, &buffer);
+        if (ACPI_FAILURE(status)) {
+                printk(KERN_INFO "failed to evaluate ROM got %s\n", acpi_format_exception(status));
+                return -ENODEV;
+        }
+        obj = (union acpi_object *)buffer.pointer;
+        memcpy(bios+offset, obj->buffer.pointer, len);
+        kfree(buffer.pointer);
+        return len;
+}
+
+bool nouveau_acpi_rom_supported(struct pci_dev *pdev)
+{
+        acpi_status status;
+        acpi_handle dhandle, rom_handle;
+
+        if (!nouveau_dsm_priv.dsm_detected)
+                return false;
+
+        dhandle = DEVICE_ACPI_HANDLE(&pdev->dev);
+        if (!dhandle)
+                return false;
+
+        status = acpi_get_handle(dhandle, "_ROM", &rom_handle);
+        if (ACPI_FAILURE(status))
+                return false;
+
+        nouveau_dsm_priv.rom_handle = rom_handle;
+        return true;
+}
+
+int nouveau_acpi_get_bios_chunk(uint8_t *bios, int offset, int len)
+{
+        return nouveau_rom_call(nouveau_dsm_priv.rom_handle, bios, offset, len);
+}
diff --git a/drivers/gpu/drm/nouveau/nouveau_bios.c b/drivers/gpu/drm/nouveau/nouveau_bios.c
index e7e69ccce5c9..fc924b649195 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bios.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bios.c
@@ -178,6 +178,25 @@ out:
         pci_disable_rom(dev->pdev);
 }
 
+static void load_vbios_acpi(struct drm_device *dev, uint8_t *data)
+{
+        int i;
+        int ret;
+        int size = 64 * 1024;
+
+        if (!nouveau_acpi_rom_supported(dev->pdev))
+                return;
+
+        for (i = 0; i < (size / ROM_BIOS_PAGE); i++) {
+                ret = nouveau_acpi_get_bios_chunk(data,
+                                                  (i * ROM_BIOS_PAGE),
+                                                  ROM_BIOS_PAGE);
+                if (ret <= 0)
+                        break;
+        }
+        return;
+}
+
 struct methods {
         const char desc[8];
         void (*loadbios)(struct drm_device *, uint8_t *);
@@ -191,6 +210,7 @@ static struct methods nv04_methods[] = {
 };
 
 static struct methods nv50_methods[] = {
+        { "ACPI", load_vbios_acpi, true },
         { "PRAMIN", load_vbios_pramin, true },
         { "PROM", load_vbios_prom, false },
         { "PCIROM", load_vbios_pci, true },
@@ -814,7 +834,7 @@ init_i2c_device_find(struct drm_device *dev, int i2c_index)
         if (i2c_index == 0x81)
                 i2c_index = (dcb->i2c_default_indices & 0xf0) >> 4;
 
-        if (i2c_index > DCB_MAX_NUM_I2C_ENTRIES) {
+        if (i2c_index >= DCB_MAX_NUM_I2C_ENTRIES) {
                 NV_ERROR(dev, "invalid i2c_index 0x%x\n", i2c_index);
                 return NULL;
         }
@@ -2807,7 +2827,10 @@ init_gpio(struct nvbios *bios, uint16_t offset, struct init_exec *iexec)
 
                 BIOSLOG(bios, "0x%04X: Entry: 0x%08X\n", offset, gpio->entry);
 
-                nv50_gpio_set(bios->dev, gpio->tag, gpio->state_default);
+                BIOSLOG(bios, "0x%04X: set gpio 0x%02x, state %d\n",
+                        offset, gpio->tag, gpio->state_default);
+                if (bios->execute)
+                        nv50_gpio_set(bios->dev, gpio->tag, gpio->state_default);
 
                 /* The NVIDIA binary driver doesn't appear to actually do
                  * any of this, my VBIOS does however.
@@ -3897,7 +3920,8 @@ int nouveau_bios_parse_lvds_table(struct drm_device *dev, int pxclk, bool *dl, b
 
 static uint8_t *
 bios_output_config_match(struct drm_device *dev, struct dcb_entry *dcbent,
-                         uint16_t record, int record_len, int record_nr)
+                         uint16_t record, int record_len, int record_nr,
+                         bool match_link)
 {
         struct drm_nouveau_private *dev_priv = dev->dev_private;
         struct nvbios *bios = &dev_priv->vbios;
@@ -3905,12 +3929,28 @@ bios_output_config_match(struct drm_device *dev, struct dcb_entry *dcbent,
         uint16_t table;
         int i, v;
 
+        switch (dcbent->type) {
+        case OUTPUT_TMDS:
+        case OUTPUT_LVDS:
+        case OUTPUT_DP:
+                break;
+        default:
+                match_link = false;
+                break;
+        }
+
         for (i = 0; i < record_nr; i++, record += record_len) {
                 table = ROM16(bios->data[record]);
                 if (!table)
                         continue;
                 entry = ROM32(bios->data[table]);
 
+                if (match_link) {
+                        v = (entry & 0x00c00000) >> 22;
+                        if (!(v & dcbent->sorconf.link))
+                                continue;
+                }
+
                 v = (entry & 0x000f0000) >> 16;
                 if (!(v & dcbent->or))
                         continue;
@@ -3952,7 +3992,7 @@ nouveau_bios_dp_table(struct drm_device *dev, struct dcb_entry *dcbent,
         *length = table[4];
         return bios_output_config_match(dev, dcbent,
                                         bios->display.dp_table_ptr + table[1],
-                                        table[2], table[3]);
+                                        table[2], table[3], table[0] >= 0x21);
 }
 
 int
@@ -4041,7 +4081,7 @@ nouveau_bios_run_display_table(struct drm_device *dev, struct dcb_entry *dcbent,
                         dcbent->type, dcbent->location, dcbent->or);
         otable = bios_output_config_match(dev, dcbent, table[1] +
                                           bios->display.script_table_ptr,
-                                          table[2], table[3]);
+                                          table[2], table[3], table[0] >= 0x21);
         if (!otable) {
                 NV_ERROR(dev, "Couldn't find matching output script table\n");
                 return 1;
@@ -5533,12 +5573,6 @@ parse_dcb20_entry(struct drm_device *dev, struct dcb_table *dcb,
         entry->bus = (conn >> 16) & 0xf;
         entry->location = (conn >> 20) & 0x3;
         entry->or = (conn >> 24) & 0xf;
-        /*
-         * Normal entries consist of a single bit, but dual link has the
-         * next most significant bit set too
-         */
-        entry->duallink_possible =
-                        ((1 << (ffs(entry->or) - 1)) * 3 == entry->or);
 
         switch (entry->type) {
         case OUTPUT_ANALOG:
@@ -5622,6 +5656,16 @@ parse_dcb20_entry(struct drm_device *dev, struct dcb_table *dcb,
                 break;
         }
 
+        if (dcb->version < 0x40) {
+                /* Normal entries consist of a single bit, but dual link has
+                 * the next most significant bit set too
+                 */
+                entry->duallink_possible =
+                        ((1 << (ffs(entry->or) - 1)) * 3 == entry->or);
+        } else {
+                entry->duallink_possible = (entry->sorconf.link == 3);
+        }
+
         /* unsure what DCB version introduces this, 3.0? */
         if (conf & 0x100000)
                 entry->i2c_upper_default = true;
@@ -6205,6 +6249,30 @@ nouveau_bios_i2c_devices_takedown(struct drm_device *dev)
                 nouveau_i2c_fini(dev, entry);
 }
 
+static bool
+nouveau_bios_posted(struct drm_device *dev)
+{
+        struct drm_nouveau_private *dev_priv = dev->dev_private;
+        bool was_locked;
+        unsigned htotal;
+
+        if (dev_priv->chipset >= NV_50) {
+                if (NVReadVgaCrtc(dev, 0, 0x00) == 0 &&
+                    NVReadVgaCrtc(dev, 0, 0x1a) == 0)
+                        return false;
+                return true;
+        }
+
+        was_locked = NVLockVgaCrtcs(dev, false);
+        htotal  = NVReadVgaCrtc(dev, 0, 0x06);
+        htotal |= (NVReadVgaCrtc(dev, 0, 0x07) & 0x01) << 8;
+        htotal |= (NVReadVgaCrtc(dev, 0, 0x07) & 0x20) << 4;
+        htotal |= (NVReadVgaCrtc(dev, 0, 0x25) & 0x01) << 10;
+        htotal |= (NVReadVgaCrtc(dev, 0, 0x41) & 0x01) << 11;
+        NVLockVgaCrtcs(dev, was_locked);
+        return (htotal != 0);
+}
+
 int
 nouveau_bios_init(struct drm_device *dev)
 {
@@ -6239,11 +6307,9 @@ nouveau_bios_init(struct drm_device *dev)
         bios->execute = false;
 
         /* ... unless card isn't POSTed already */
-        if (dev_priv->card_type >= NV_10 &&
-            NVReadVgaCrtc(dev, 0, 0x00) == 0 &&
-            NVReadVgaCrtc(dev, 0, 0x1a) == 0) {
+        if (!nouveau_bios_posted(dev)) {
                 NV_INFO(dev, "Adaptor not initialised\n");
-                if (dev_priv->card_type < NV_50) {
+                if (dev_priv->card_type < NV_40) {
                         NV_ERROR(dev, "Unable to POST this chipset\n");
                         return -ENODEV;
                 }
diff --git a/drivers/gpu/drm/nouveau/nouveau_connector.c b/drivers/gpu/drm/nouveau/nouveau_connector.c
index 7e663a79829f..149ed224c3cb 100644
--- a/drivers/gpu/drm/nouveau/nouveau_connector.c
+++ b/drivers/gpu/drm/nouveau/nouveau_connector.c
@@ -241,7 +241,8 @@ nouveau_connector_detect(struct drm_connector *connector)
         if (nv_encoder && nv_connector->native_mode) {
                 unsigned status = connector_status_connected;
 
-#ifdef CONFIG_ACPI
+#if defined(CONFIG_ACPI_BUTTON) || \
+        (defined(CONFIG_ACPI_BUTTON_MODULE) && defined(MODULE))
                 if (!nouveau_ignorelid && !acpi_lid_open())
                         status = connector_status_unknown;
 #endif
@@ -431,24 +432,27 @@ nouveau_connector_set_property(struct drm_connector *connector,
 }
 
 static struct drm_display_mode *
-nouveau_connector_native_mode(struct nouveau_connector *connector)
+nouveau_connector_native_mode(struct drm_connector *connector)
 {
-        struct drm_device *dev = connector->base.dev;
+        struct drm_connector_helper_funcs *helper = connector->helper_private;
+        struct nouveau_connector *nv_connector = nouveau_connector(connector);
+        struct drm_device *dev = connector->dev;
         struct drm_display_mode *mode, *largest = NULL;
         int high_w = 0, high_h = 0, high_v = 0;
 
-        /* Use preferred mode if there is one.. */
-        list_for_each_entry(mode, &connector->base.probed_modes, head) {
+        list_for_each_entry(mode, &nv_connector->base.probed_modes, head) {
+                if (helper->mode_valid(connector, mode) != MODE_OK)
+                        continue;
+
+                /* Use preferred mode if there is one.. */
                 if (mode->type & DRM_MODE_TYPE_PREFERRED) {
                         NV_DEBUG_KMS(dev, "native mode from preferred\n");
                         return drm_mode_duplicate(dev, mode);
                 }
-        }
 
-        /* Otherwise, take the resolution with the largest width, then height,
-         * then vertical refresh
-         */
-        list_for_each_entry(mode, &connector->base.probed_modes, head) {
+                /* Otherwise, take the resolution with the largest width, then
+                 * height, then vertical refresh
+                 */
                 if (mode->hdisplay < high_w)
                         continue;
 
@@ -552,7 +556,7 @@ nouveau_connector_get_modes(struct drm_connector *connector)
          */
         if (!nv_connector->native_mode)
                 nv_connector->native_mode =
-                        nouveau_connector_native_mode(nv_connector);
+                        nouveau_connector_native_mode(connector);
         if (ret == 0 && nv_connector->native_mode) {
                 struct drm_display_mode *mode;
 
@@ -583,9 +587,9 @@ nouveau_connector_mode_valid(struct drm_connector *connector,
 
         switch (nv_encoder->dcb->type) {
         case OUTPUT_LVDS:
-                BUG_ON(!nv_connector->native_mode);
-                if (mode->hdisplay > nv_connector->native_mode->hdisplay ||
-                    mode->vdisplay > nv_connector->native_mode->vdisplay)
+                if (nv_connector->native_mode &&
+                    (mode->hdisplay > nv_connector->native_mode->hdisplay ||
+                     mode->vdisplay > nv_connector->native_mode->vdisplay))
                         return MODE_PANEL;
 
                 min_clock = 0;
@@ -593,8 +597,7 @@ nouveau_connector_mode_valid(struct drm_connector *connector,
                 break;
         case OUTPUT_TMDS:
                 if ((dev_priv->card_type >= NV_50 && !nouveau_duallink) ||
-                    (dev_priv->card_type < NV_50 &&
-                     !nv_encoder->dcb->duallink_possible))
+                    !nv_encoder->dcb->duallink_possible)
                         max_clock = 165000;
                 else
                         max_clock = 330000;
@@ -728,7 +731,7 @@ nouveau_connector_create_lvds(struct drm_device *dev,
         if (ret == 0)
                 goto out;
         nv_connector->detected_encoder = nv_encoder;
-        nv_connector->native_mode = nouveau_connector_native_mode(nv_connector);
+        nv_connector->native_mode = nouveau_connector_native_mode(connector);
         list_for_each_entry_safe(mode, temp, &connector->probed_modes, head)
                 drm_mode_remove(connector, mode);
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_crtc.h b/drivers/gpu/drm/nouveau/nouveau_crtc.h
index 49fa7b2d257e..cb1ce2a09162 100644
--- a/drivers/gpu/drm/nouveau/nouveau_crtc.h
+++ b/drivers/gpu/drm/nouveau/nouveau_crtc.h
@@ -40,6 +40,8 @@ struct nouveau_crtc {
         int sharpness;
         int last_dpms;
 
+        int cursor_saved_x, cursor_saved_y;
+
         struct {
                 int cpp;
                 bool blanked;
diff --git a/drivers/gpu/drm/nouveau/nouveau_drv.c b/drivers/gpu/drm/nouveau/nouveau_drv.c
index c6079e36669d..273770432298 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drv.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drv.c
@@ -175,6 +175,13 @@ nouveau_pci_suspend(struct pci_dev *pdev, pm_message_t pm_state)
                 nouveau_bo_unpin(nouveau_fb->nvbo);
         }
 
+        list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+                struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
+
+                nouveau_bo_unmap(nv_crtc->cursor.nvbo);
+                nouveau_bo_unpin(nv_crtc->cursor.nvbo);
+        }
+
         NV_INFO(dev, "Evicting buffers...\n");
         ttm_bo_evict_mm(&dev_priv->ttm.bdev, TTM_PL_VRAM);
 
@@ -314,12 +321,34 @@ nouveau_pci_resume(struct pci_dev *pdev)
                 nouveau_bo_pin(nouveau_fb->nvbo, TTM_PL_FLAG_VRAM);
         }
 
+        list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+                struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
+                int ret;
+
+                ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM);
+                if (!ret)
+                        ret = nouveau_bo_map(nv_crtc->cursor.nvbo);
+                if (ret)
+                        NV_ERROR(dev, "Could not pin/map cursor.\n");
+        }
+
         if (dev_priv->card_type < NV_50) {
                 nv04_display_restore(dev);
                 NVLockVgaCrtcs(dev, false);
         } else
                 nv50_display_init(dev);
 
+        list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+                struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
+
+                nv_crtc->cursor.set_offset(nv_crtc,
+                                        nv_crtc->cursor.nvbo->bo.offset -
+                                        dev_priv->vm_vram_base);
+
+                nv_crtc->cursor.set_pos(nv_crtc, nv_crtc->cursor_saved_x,
+                        nv_crtc->cursor_saved_y);
+        }
+
         /* Force CLUT to get re-loaded during modeset */
         list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
                 struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
diff --git a/drivers/gpu/drm/nouveau/nouveau_drv.h b/drivers/gpu/drm/nouveau/nouveau_drv.h
index 5b134438effe..c69719106489 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drv.h
+++ b/drivers/gpu/drm/nouveau/nouveau_drv.h
@@ -851,12 +851,17 @@ extern int  nouveau_dma_init(struct nouveau_channel *);
 extern int  nouveau_dma_wait(struct nouveau_channel *, int slots, int size);
 
 /* nouveau_acpi.c */
+#define ROM_BIOS_PAGE 4096
 #if defined(CONFIG_ACPI)
 void nouveau_register_dsm_handler(void);
 void nouveau_unregister_dsm_handler(void);
+int nouveau_acpi_get_bios_chunk(uint8_t *bios, int offset, int len);
+bool nouveau_acpi_rom_supported(struct pci_dev *pdev);
 #else
 static inline void nouveau_register_dsm_handler(void) {}
 static inline void nouveau_unregister_dsm_handler(void) {}
+static inline bool nouveau_acpi_rom_supported(struct pci_dev *pdev) { return false; }
+static inline int nouveau_acpi_get_bios_chunk(uint8_t *bios, int offset, int len) { return -EINVAL; }
 #endif
 
 /* nouveau_backlight.c */
diff --git a/drivers/gpu/drm/nouveau/nouveau_fbcon.c b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
index fd4a2df715e9..c9a4a0d2a115 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
@@ -377,6 +377,7 @@ int nouveau_fbcon_init(struct drm_device *dev)
 {
         struct drm_nouveau_private *dev_priv = dev->dev_private;
         struct nouveau_fbdev *nfbdev;
+        int ret;
 
         nfbdev = kzalloc(sizeof(struct nouveau_fbdev), GFP_KERNEL);
         if (!nfbdev)
@@ -386,7 +387,12 @@ int nouveau_fbcon_init(struct drm_device *dev)
         dev_priv->nfbdev = nfbdev;
         nfbdev->helper.funcs = &nouveau_fbcon_helper_funcs;
 
-        drm_fb_helper_init(dev, &nfbdev->helper, 2, 4);
+        ret = drm_fb_helper_init(dev, &nfbdev->helper, 2, 4);
+        if (ret) {
+                kfree(nfbdev);
+                return ret;
+        }
+
         drm_fb_helper_single_add_all_connectors(&nfbdev->helper);
         drm_fb_helper_initial_config(&nfbdev->helper, 32);
         return 0;
diff --git a/drivers/gpu/drm/nouveau/nouveau_mem.c b/drivers/gpu/drm/nouveau/nouveau_mem.c
index 775a7017af64..c1fd42b0dad1 100644
--- a/drivers/gpu/drm/nouveau/nouveau_mem.c
+++ b/drivers/gpu/drm/nouveau/nouveau_mem.c
@@ -540,7 +540,8 @@ nouveau_mem_detect(struct drm_device *dev)
                 dev_priv->vram_size  = nv_rd32(dev, NV04_FIFO_DATA);
                 dev_priv->vram_size &= NV10_FIFO_DATA_RAM_AMOUNT_MB_MASK;
                 if (dev_priv->chipset == 0xaa || dev_priv->chipset == 0xac)
-                        dev_priv->vram_sys_base = nv_rd32(dev, 0x100e10) << 12;
+                        dev_priv->vram_sys_base = nv_rd32(dev, 0x100e10);
+                        dev_priv->vram_sys_base <<= 12;
         }
 
         NV_INFO(dev, "Detected %dMiB VRAM\n", (int)(dev_priv->vram_size >> 20));
diff --git a/drivers/gpu/drm/nouveau/nouveau_state.c b/drivers/gpu/drm/nouveau/nouveau_state.c
index e632339c323e..b02a231d6937 100644
--- a/drivers/gpu/drm/nouveau/nouveau_state.c
+++ b/drivers/gpu/drm/nouveau/nouveau_state.c
@@ -376,12 +376,15 @@ out_err:
 static void nouveau_switcheroo_set_state(struct pci_dev *pdev,
                                          enum vga_switcheroo_state state)
 {
+        struct drm_device *dev = pci_get_drvdata(pdev);
         pm_message_t pmm = { .event = PM_EVENT_SUSPEND };
         if (state == VGA_SWITCHEROO_ON) {
                 printk(KERN_ERR "VGA switcheroo: switched nouveau on\n");
                 nouveau_pci_resume(pdev);
+                drm_kms_helper_poll_enable(dev);
         } else {
                 printk(KERN_ERR "VGA switcheroo: switched nouveau off\n");
+                drm_kms_helper_poll_disable(dev);
                 nouveau_pci_suspend(pdev, pmm);
         }
 }
@@ -776,29 +779,24 @@ int nouveau_load(struct drm_device *dev, unsigned long flags)
                         return ret;
         }
 
-        /* map larger RAMIN aperture on NV40 cards */
-        dev_priv->ramin  = NULL;
+        /* Map PRAMIN BAR, or on older cards, the aperture withing BAR0 */
         if (dev_priv->card_type >= NV_40) {
                 int ramin_bar = 2;
                 if (pci_resource_len(dev->pdev, ramin_bar) == 0)
                         ramin_bar = 3;
 
                 dev_priv->ramin_size = pci_resource_len(dev->pdev, ramin_bar);
-                dev_priv->ramin = ioremap(
-                                pci_resource_start(dev->pdev, ramin_bar),
+                dev_priv->ramin =
+                        ioremap(pci_resource_start(dev->pdev, ramin_bar),
                                 dev_priv->ramin_size);
                 if (!dev_priv->ramin) {
-                        NV_ERROR(dev, "Failed to init RAMIN mapping, "
-                                      "limited instance memory available\n");
+                        NV_ERROR(dev, "Failed to PRAMIN BAR");
+                        return -ENOMEM;
                 }
-        }
-
-        /* On older cards (or if the above failed), create a map covering
-         * the BAR0 PRAMIN aperture */
-        if (!dev_priv->ramin) {
+        } else {
                 dev_priv->ramin_size = 1 * 1024 * 1024;
                 dev_priv->ramin = ioremap(mmio_start_offs + NV_RAMIN,
-                                                        dev_priv->ramin_size);
+                                          dev_priv->ramin_size);
                 if (!dev_priv->ramin) {
                         NV_ERROR(dev, "Failed to map BAR0 PRAMIN.\n");
                         return -ENOMEM;
@@ -913,6 +911,9 @@ int nouveau_ioctl_getparam(struct drm_device *dev, void *data,
         case NOUVEAU_GETPARAM_VM_VRAM_BASE:
                 getparam->value = dev_priv->vm_vram_base;
                 break;
+        case NOUVEAU_GETPARAM_PTIMER_TIME:
+                getparam->value = dev_priv->engine.timer.read(dev);
+                break;
         case NOUVEAU_GETPARAM_GRAPH_UNITS:
                 /* NV40 and NV50 versions are quite different, but register
                  * address is the same. User is supposed to know the card
diff --git a/drivers/gpu/drm/nouveau/nv04_cursor.c b/drivers/gpu/drm/nouveau/nv04_cursor.c
index 89a91b9d8b25..aaf3de3bc816 100644
--- a/drivers/gpu/drm/nouveau/nv04_cursor.c
+++ b/drivers/gpu/drm/nouveau/nv04_cursor.c
@@ -20,6 +20,7 @@ nv04_cursor_hide(struct nouveau_crtc *nv_crtc, bool update)
 static void
 nv04_cursor_set_pos(struct nouveau_crtc *nv_crtc, int x, int y)
 {
+        nv_crtc->cursor_saved_x = x; nv_crtc->cursor_saved_y = y;
         NVWriteRAMDAC(nv_crtc->base.dev, nv_crtc->index,
                       NV_PRAMDAC_CU_START_POS,
                       XLATE(y, 0, NV_PRAMDAC_CU_START_POS_Y) |
diff --git a/drivers/gpu/drm/nouveau/nv40_graph.c b/drivers/gpu/drm/nouveau/nv40_graph.c
index 0616c96e4b67..704a25d04ac9 100644
--- a/drivers/gpu/drm/nouveau/nv40_graph.c
+++ b/drivers/gpu/drm/nouveau/nv40_graph.c
@@ -253,7 +253,11 @@ nv40_graph_init(struct drm_device *dev)
 
         if (!dev_priv->engine.graph.ctxprog) {
                 struct nouveau_grctx ctx = {};
-                uint32_t cp[256];
+                uint32_t *cp;
+
+                cp = kmalloc(sizeof(*cp) * 256, GFP_KERNEL);
+                if (!cp)
+                        return -ENOMEM;
 
                 ctx.dev = dev;
                 ctx.mode = NOUVEAU_GRCTX_PROG;
@@ -265,6 +269,8 @@ nv40_graph_init(struct drm_device *dev)
                 nv_wr32(dev, NV40_PGRAPH_CTXCTL_UCODE_INDEX, 0);
                 for (i = 0; i < ctx.ctxprog_len; i++)
                         nv_wr32(dev, NV40_PGRAPH_CTXCTL_UCODE_DATA, cp[i]);
+
+                kfree(cp);
         }
 
         /* No context present currently */
diff --git a/drivers/gpu/drm/nouveau/nv50_cursor.c b/drivers/gpu/drm/nouveau/nv50_cursor.c
index 753e723adb3a..03ad7ab14f09 100644
--- a/drivers/gpu/drm/nouveau/nv50_cursor.c
+++ b/drivers/gpu/drm/nouveau/nv50_cursor.c
@@ -107,6 +107,7 @@ nv50_cursor_set_pos(struct nouveau_crtc *nv_crtc, int x, int y)
 {
         struct drm_device *dev = nv_crtc->base.dev;
 
+        nv_crtc->cursor_saved_x = x; nv_crtc->cursor_saved_y = y;
         nv_wr32(dev, NV50_PDISPLAY_CURSOR_USER_POS(nv_crtc->index),
                 ((y & 0xFFFF) << 16) | (x & 0xFFFF));
         /* Needed to make the cursor move. */
diff --git a/drivers/gpu/drm/nouveau/nv50_fb.c b/drivers/gpu/drm/nouveau/nv50_fb.c
index a95e6941ba88..32611bd30e6d 100644
--- a/drivers/gpu/drm/nouveau/nv50_fb.c
+++ b/drivers/gpu/drm/nouveau/nv50_fb.c
@@ -6,10 +6,16 @@
 int
 nv50_fb_init(struct drm_device *dev)
 {
-        /* This is needed to get meaningful information from 100c90
-         * on traps. No idea what these values mean exactly. */
         struct drm_nouveau_private *dev_priv = dev->dev_private;
 
+        /* Not a clue what this is exactly.  Without pointing it at a
+         * scratch page, VRAM->GART blits with M2MF (as in DDX DFS)
+         * cause IOMMU "read from address 0" errors (rh#561267)
+         */
+        nv_wr32(dev, 0x100c08, dev_priv->gart_info.sg_dummy_bus >> 8);
+
+        /* This is needed to get meaningful information from 100c90
+         * on traps. No idea what these values mean exactly. */
         switch (dev_priv->chipset) {
         case 0x50:
                 nv_wr32(dev, 0x100c90, 0x0707ff);
diff --git a/drivers/gpu/drm/nouveau/nv50_gpio.c b/drivers/gpu/drm/nouveau/nv50_gpio.c
index c61782b314e7..bb47ad737267 100644
--- a/drivers/gpu/drm/nouveau/nv50_gpio.c
+++ b/drivers/gpu/drm/nouveau/nv50_gpio.c
@@ -31,7 +31,7 @@ nv50_gpio_location(struct dcb_gpio_entry *gpio, uint32_t *reg, uint32_t *shift)
 {
         const uint32_t nv50_gpio_reg[4] = { 0xe104, 0xe108, 0xe280, 0xe284 };
 
-        if (gpio->line > 32)
+        if (gpio->line >= 32)
                 return -EINVAL;
 
         *reg = nv50_gpio_reg[gpio->line >> 3];
diff --git a/drivers/gpu/drm/nouveau/nv50_sor.c b/drivers/gpu/drm/nouveau/nv50_sor.c
index b11eaf9c5c7c..812778db76ac 100644
--- a/drivers/gpu/drm/nouveau/nv50_sor.c
+++ b/drivers/gpu/drm/nouveau/nv50_sor.c
@@ -274,7 +274,6 @@ static const struct drm_encoder_funcs nv50_sor_encoder_funcs = {
 int
 nv50_sor_create(struct drm_device *dev, struct dcb_entry *entry)
 {
-        struct drm_nouveau_private *dev_priv = dev->dev_private;
         struct nouveau_encoder *nv_encoder = NULL;
         struct drm_encoder *encoder;
         bool dum;
@@ -324,11 +323,7 @@ nv50_sor_create(struct drm_device *dev, struct dcb_entry *entry)
                 int or = nv_encoder->or, link = !(entry->dpconf.sor.link & 1);
                 uint32_t tmp;
 
-                if (dev_priv->chipset < 0x90 ||
-                    dev_priv->chipset == 0x92 || dev_priv->chipset == 0xa0)
-                        tmp = nv_rd32(dev, NV50_PDISPLAY_SOR_MODE_CTRL_C(or));
-                else
-                        tmp = nv_rd32(dev, NV90_PDISPLAY_SOR_MODE_CTRL_C(or));
+                tmp = nv_rd32(dev, 0x61c700 + (or * 0x800));
 
                 switch ((tmp & 0x00000f00) >> 8) {
                 case 8:
diff --git a/drivers/gpu/drm/radeon/Makefile b/drivers/gpu/drm/radeon/Makefile
index 3c91312dea9a..84b1f2729d43 100644
--- a/drivers/gpu/drm/radeon/Makefile
+++ b/drivers/gpu/drm/radeon/Makefile
@@ -33,6 +33,9 @@ $(obj)/rs600_reg_safe.h: $(src)/reg_srcs/rs600 $(obj)/mkregtable
 $(obj)/r600_reg_safe.h: $(src)/reg_srcs/r600 $(obj)/mkregtable
         $(call if_changed,mkregtable)
 
+$(obj)/evergreen_reg_safe.h: $(src)/reg_srcs/evergreen $(obj)/mkregtable
+        $(call if_changed,mkregtable)
+
 $(obj)/r100.o: $(obj)/r100_reg_safe.h $(obj)/rn50_reg_safe.h
 
 $(obj)/r200.o: $(obj)/r200_reg_safe.h
@@ -47,6 +50,8 @@ $(obj)/rs600.o: $(obj)/rs600_reg_safe.h
 
 $(obj)/r600_cs.o: $(obj)/r600_reg_safe.h
 
+$(obj)/evergreen_cs.o: $(obj)/evergreen_reg_safe.h
+
 radeon-y := radeon_drv.o radeon_cp.o radeon_state.o radeon_mem.o \
         radeon_irq.o r300_cmdbuf.o r600_cp.o
 # add KMS driver
@@ -60,7 +65,7 @@ radeon-y += radeon_device.o radeon_asic.o radeon_kms.o \
         rs400.o rs600.o rs690.o rv515.o r520.o r600.o rv770.o radeon_test.o \
         r200.o radeon_legacy_tv.o r600_cs.o r600_blit.o r600_blit_shaders.o \
         r600_blit_kms.o radeon_pm.o atombios_dp.o r600_audio.o r600_hdmi.o \
-        evergreen.o
+        evergreen.o evergreen_cs.o
 
 radeon-$(CONFIG_COMPAT) += radeon_ioc32.o
 radeon-$(CONFIG_VGA_SWITCHEROO) += radeon_atpx_handler.o
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index 03dd6c41dc19..f3f2827017ef 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -707,6 +707,7 @@ static void atombios_crtc_set_pll(struct drm_crtc *crtc, struct drm_display_mode
                 break;
         case ATOM_DCPLL:
         case ATOM_PPLL_INVALID:
+        default:
                 pll = &rdev->clock.dcpll;
                 break;
         }
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index 8c8e4d3cbaa3..4b6623df3b96 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -41,7 +41,18 @@ void evergreen_fini(struct radeon_device *rdev);
 
 void evergreen_pm_misc(struct radeon_device *rdev)
 {
-
+        int req_ps_idx = rdev->pm.requested_power_state_index;
+        int req_cm_idx = rdev->pm.requested_clock_mode_index;
+        struct radeon_power_state *ps = &rdev->pm.power_state[req_ps_idx];
+        struct radeon_voltage *voltage = &ps->clock_info[req_cm_idx].voltage;
+
+        if ((voltage->type == VOLTAGE_SW) && voltage->voltage) {
+                if (voltage->voltage != rdev->pm.current_vddc) {
+                        radeon_atom_set_voltage(rdev, voltage->voltage);
+                        rdev->pm.current_vddc = voltage->voltage;
+                        DRM_DEBUG("Setting: v: %d\n", voltage->voltage);
+                }
+        }
 }
 
 void evergreen_pm_prepare(struct radeon_device *rdev)
@@ -2148,7 +2159,7 @@ int evergreen_init(struct radeon_device *rdev)
         if (r)
                 return r;
 
-        rdev->accel_working = false;
+        rdev->accel_working = true;
         r = evergreen_startup(rdev);
         if (r) {
                 dev_err(rdev->dev, "disabling GPU acceleration\n");
diff --git a/drivers/gpu/drm/radeon/evergreen_cs.c b/drivers/gpu/drm/radeon/evergreen_cs.c
new file mode 100644
index 000000000000..64516b950891
--- /dev/null
+++ b/drivers/gpu/drm/radeon/evergreen_cs.c
@@ -0,0 +1,1356 @@
+/*
+ * Copyright 2010 Advanced Micro Devices, Inc.
+ * Copyright 2008 Red Hat Inc.
+ * Copyright 2009 Jerome Glisse.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Dave Airlie
+ *          Alex Deucher
+ *          Jerome Glisse
+ */
+#include "drmP.h"
+#include "radeon.h"
+#include "evergreend.h"
+#include "evergreen_reg_safe.h"
+
+static int evergreen_cs_packet_next_reloc(struct radeon_cs_parser *p,
+                                          struct radeon_cs_reloc **cs_reloc);
+
+struct evergreen_cs_track {
+        u32                     group_size;
+        u32                     nbanks;
+        u32                     npipes;
+        /* value we track */
+        u32                     nsamples;
+        u32                     cb_color_base_last[12];
+        struct radeon_bo        *cb_color_bo[12];
+        u32                     cb_color_bo_offset[12];
+        struct radeon_bo        *cb_color_fmask_bo[8];
+        struct radeon_bo        *cb_color_cmask_bo[8];
+        u32                     cb_color_info[12];
+        u32                     cb_color_view[12];
+        u32                     cb_color_pitch_idx[12];
+        u32                     cb_color_slice_idx[12];
+        u32                     cb_color_dim_idx[12];
+        u32                     cb_color_dim[12];
+        u32                     cb_color_pitch[12];
+        u32                     cb_color_slice[12];
+        u32                     cb_color_cmask_slice[8];
+        u32                     cb_color_fmask_slice[8];
+        u32                     cb_target_mask;
+        u32                     cb_shader_mask;
+        u32                     vgt_strmout_config;
+        u32                     vgt_strmout_buffer_config;
+        u32                     db_depth_control;
+        u32                     db_depth_view;
+        u32                     db_depth_size;
+        u32                     db_depth_size_idx;
+        u32                     db_z_info;
+        u32                     db_z_idx;
+        u32                     db_z_read_offset;
+        u32                     db_z_write_offset;
+        struct radeon_bo        *db_z_read_bo;
+        struct radeon_bo        *db_z_write_bo;
+        u32                     db_s_info;
+        u32                     db_s_idx;
+        u32                     db_s_read_offset;
+        u32                     db_s_write_offset;
+        struct radeon_bo        *db_s_read_bo;
+        struct radeon_bo        *db_s_write_bo;
+};
+
+static void evergreen_cs_track_init(struct evergreen_cs_track *track)
+{
+        int i;
+
+        for (i = 0; i < 8; i++) {
+                track->cb_color_fmask_bo[i] = NULL;
+                track->cb_color_cmask_bo[i] = NULL;
+                track->cb_color_cmask_slice[i] = 0;
+                track->cb_color_fmask_slice[i] = 0;
+        }
+
+        for (i = 0; i < 12; i++) {
+                track->cb_color_base_last[i] = 0;
+                track->cb_color_bo[i] = NULL;
+                track->cb_color_bo_offset[i] = 0xFFFFFFFF;
+                track->cb_color_info[i] = 0;
+                track->cb_color_view[i] = 0;
+                track->cb_color_pitch_idx[i] = 0;
+                track->cb_color_slice_idx[i] = 0;
+                track->cb_color_dim[i] = 0;
+                track->cb_color_pitch[i] = 0;
+                track->cb_color_slice[i] = 0;
+                track->cb_color_dim[i] = 0;
+        }
+        track->cb_target_mask = 0xFFFFFFFF;
+        track->cb_shader_mask = 0xFFFFFFFF;
+
+        track->db_depth_view = 0xFFFFC000;
+        track->db_depth_size = 0xFFFFFFFF;
+        track->db_depth_size_idx = 0;
+        track->db_depth_control = 0xFFFFFFFF;
+        track->db_z_info = 0xFFFFFFFF;
+        track->db_z_idx = 0xFFFFFFFF;
+        track->db_z_read_offset = 0xFFFFFFFF;
+        track->db_z_write_offset = 0xFFFFFFFF;
+        track->db_z_read_bo = NULL;
+        track->db_z_write_bo = NULL;
+        track->db_s_info = 0xFFFFFFFF;
+        track->db_s_idx = 0xFFFFFFFF;
+        track->db_s_read_offset = 0xFFFFFFFF;
+        track->db_s_write_offset = 0xFFFFFFFF;
+        track->db_s_read_bo = NULL;
+        track->db_s_write_bo = NULL;
+}
+
+static inline int evergreen_cs_track_validate_cb(struct radeon_cs_parser *p, int i)
+{
+        /* XXX fill in */
+        return 0;
+}
+
+static int evergreen_cs_track_check(struct radeon_cs_parser *p)
+{
+        struct evergreen_cs_track *track = p->track;
+
+        /* we don't support stream out buffer yet */
+        if (track->vgt_strmout_config || track->vgt_strmout_buffer_config) {
+                dev_warn(p->dev, "this kernel doesn't support SMX output buffer\n");
+                return -EINVAL;
+        }
+
+        /* XXX fill in */
+        return 0;
+}
+
+/**
+ * evergreen_cs_packet_parse() - parse cp packet and point ib index to next packet
+ * @parser:     parser structure holding parsing context.
+ * @pkt:        where to store packet informations
+ *
+ * Assume that chunk_ib_index is properly set. Will return -EINVAL
+ * if packet is bigger than remaining ib size. or if packets is unknown.
+ **/
+int evergreen_cs_packet_parse(struct radeon_cs_parser *p,
+                              struct radeon_cs_packet *pkt,
+                              unsigned idx)
+{
+        struct radeon_cs_chunk *ib_chunk = &p->chunks[p->chunk_ib_idx];
+        uint32_t header;
+
+        if (idx >= ib_chunk->length_dw) {
+                DRM_ERROR("Can not parse packet at %d after CS end %d !\n",
+                          idx, ib_chunk->length_dw);
+                return -EINVAL;
+        }
+        header = radeon_get_ib_value(p, idx);
+        pkt->idx = idx;
+        pkt->type = CP_PACKET_GET_TYPE(header);
+        pkt->count = CP_PACKET_GET_COUNT(header);
+        pkt->one_reg_wr = 0;
+        switch (pkt->type) {
+        case PACKET_TYPE0:
+                pkt->reg = CP_PACKET0_GET_REG(header);
+                break;
+        case PACKET_TYPE3:
+                pkt->opcode = CP_PACKET3_GET_OPCODE(header);
+                break;
+        case PACKET_TYPE2:
+                pkt->count = -1;
+                break;
+        default:
+                DRM_ERROR("Unknown packet type %d at %d !\n", pkt->type, idx);
+                return -EINVAL;
+        }
+        if ((pkt->count + 1 + pkt->idx) >= ib_chunk->length_dw) {
+                DRM_ERROR("Packet (%d:%d:%d) end after CS buffer (%d) !\n",
+                          pkt->idx, pkt->type, pkt->count, ib_chunk->length_dw);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+/**
+ * evergreen_cs_packet_next_reloc() - parse next packet which should be reloc packet3
+ * @parser:             parser structure holding parsing context.
+ * @data:               pointer to relocation data
+ * @offset_start:       starting offset
+ * @offset_mask:        offset mask (to align start offset on)
+ * @reloc:              reloc informations
+ *
+ * Check next packet is relocation packet3, do bo validation and compute
+ * GPU offset using the provided start.
+ **/
+static int evergreen_cs_packet_next_reloc(struct radeon_cs_parser *p,
+                                          struct radeon_cs_reloc **cs_reloc)
+{
+        struct radeon_cs_chunk *relocs_chunk;
+        struct radeon_cs_packet p3reloc;
+        unsigned idx;
+        int r;
+
+        if (p->chunk_relocs_idx == -1) {
+                DRM_ERROR("No relocation chunk !\n");
+                return -EINVAL;
+        }
+        *cs_reloc = NULL;
+        relocs_chunk = &p->chunks[p->chunk_relocs_idx];
+        r = evergreen_cs_packet_parse(p, &p3reloc, p->idx);
+        if (r) {
+                return r;
+        }
+        p->idx += p3reloc.count + 2;
+        if (p3reloc.type != PACKET_TYPE3 || p3reloc.opcode != PACKET3_NOP) {
+                DRM_ERROR("No packet3 for relocation for packet at %d.\n",
+                          p3reloc.idx);
+                return -EINVAL;
+        }
+        idx = radeon_get_ib_value(p, p3reloc.idx + 1);
+        if (idx >= relocs_chunk->length_dw) {
+                DRM_ERROR("Relocs at %d after relocations chunk end %d !\n",
+                          idx, relocs_chunk->length_dw);
+                return -EINVAL;
+        }
+        /* FIXME: we assume reloc size is 4 dwords */
+        *cs_reloc = p->relocs_ptr[(idx / 4)];
+        return 0;
+}
+
+/**
+ * evergreen_cs_packet_next_is_pkt3_nop() - test if next packet is packet3 nop for reloc
+ * @parser:             parser structure holding parsing context.
+ *
+ * Check next packet is relocation packet3, do bo validation and compute
+ * GPU offset using the provided start.
+ **/
+static inline int evergreen_cs_packet_next_is_pkt3_nop(struct radeon_cs_parser *p)
+{
+        struct radeon_cs_packet p3reloc;
+        int r;
+
+        r = evergreen_cs_packet_parse(p, &p3reloc, p->idx);
+        if (r) {
+                return 0;
+        }
+        if (p3reloc.type != PACKET_TYPE3 || p3reloc.opcode != PACKET3_NOP) {
+                return 0;
+        }
+        return 1;
+}
+
+/**
+ * evergreen_cs_packet_next_vline() - parse userspace VLINE packet
+ * @parser:             parser structure holding parsing context.
+ *
+ * Userspace sends a special sequence for VLINE waits.
+ * PACKET0 - VLINE_START_END + value
+ * PACKET3 - WAIT_REG_MEM poll vline status reg
+ * RELOC (P3) - crtc_id in reloc.
+ *
+ * This function parses this and relocates the VLINE START END
+ * and WAIT_REG_MEM packets to the correct crtc.
+ * It also detects a switched off crtc and nulls out the
+ * wait in that case.
+ */
+static int evergreen_cs_packet_parse_vline(struct radeon_cs_parser *p)
+{
+        struct drm_mode_object *obj;
+        struct drm_crtc *crtc;
+        struct radeon_crtc *radeon_crtc;
+        struct radeon_cs_packet p3reloc, wait_reg_mem;
+        int crtc_id;
+        int r;
+        uint32_t header, h_idx, reg, wait_reg_mem_info;
+        volatile uint32_t *ib;
+
+        ib = p->ib->ptr;
+
+        /* parse the WAIT_REG_MEM */
+        r = evergreen_cs_packet_parse(p, &wait_reg_mem, p->idx);
+        if (r)
+                return r;
+
+        /* check its a WAIT_REG_MEM */
+        if (wait_reg_mem.type != PACKET_TYPE3 ||
+            wait_reg_mem.opcode != PACKET3_WAIT_REG_MEM) {
+                DRM_ERROR("vline wait missing WAIT_REG_MEM segment\n");
+                r = -EINVAL;
+                return r;
+        }
+
+        wait_reg_mem_info = radeon_get_ib_value(p, wait_reg_mem.idx + 1);
+        /* bit 4 is reg (0) or mem (1) */
+        if (wait_reg_mem_info & 0x10) {
+                DRM_ERROR("vline WAIT_REG_MEM waiting on MEM rather than REG\n");
+                r = -EINVAL;
+                return r;
+        }
+        /* waiting for value to be equal */
+        if ((wait_reg_mem_info & 0x7) != 0x3) {
+                DRM_ERROR("vline WAIT_REG_MEM function not equal\n");
+                r = -EINVAL;
+                return r;
+        }
+        if ((radeon_get_ib_value(p, wait_reg_mem.idx + 2) << 2) != EVERGREEN_VLINE_STATUS) {
+                DRM_ERROR("vline WAIT_REG_MEM bad reg\n");
+                r = -EINVAL;
+                return r;
+        }
+
+        if (radeon_get_ib_value(p, wait_reg_mem.idx + 5) != EVERGREEN_VLINE_STAT) {
+                DRM_ERROR("vline WAIT_REG_MEM bad bit mask\n");
+                r = -EINVAL;
+                return r;
+        }
+
+        /* jump over the NOP */
+        r = evergreen_cs_packet_parse(p, &p3reloc, p->idx + wait_reg_mem.count + 2);
+        if (r)
+                return r;
+
+        h_idx = p->idx - 2;
+        p->idx += wait_reg_mem.count + 2;
+        p->idx += p3reloc.count + 2;
+
+        header = radeon_get_ib_value(p, h_idx);
+        crtc_id = radeon_get_ib_value(p, h_idx + 2 + 7 + 1);
+        reg = CP_PACKET0_GET_REG(header);
+        mutex_lock(&p->rdev->ddev->mode_config.mutex);
+        obj = drm_mode_object_find(p->rdev->ddev, crtc_id, DRM_MODE_OBJECT_CRTC);
+        if (!obj) {
+                DRM_ERROR("cannot find crtc %d\n", crtc_id);
+                r = -EINVAL;
+                goto out;
+        }
+        crtc = obj_to_crtc(obj);
+        radeon_crtc = to_radeon_crtc(crtc);
+        crtc_id = radeon_crtc->crtc_id;
+
+        if (!crtc->enabled) {
+                /* if the CRTC isn't enabled - we need to nop out the WAIT_REG_MEM */
+                ib[h_idx + 2] = PACKET2(0);
+                ib[h_idx + 3] = PACKET2(0);
+                ib[h_idx + 4] = PACKET2(0);
+                ib[h_idx + 5] = PACKET2(0);
+                ib[h_idx + 6] = PACKET2(0);
+                ib[h_idx + 7] = PACKET2(0);
+                ib[h_idx + 8] = PACKET2(0);
+        } else {
+                switch (reg) {
+                case EVERGREEN_VLINE_START_END:
+                        header &= ~R600_CP_PACKET0_REG_MASK;
+                        header |= (EVERGREEN_VLINE_START_END + radeon_crtc->crtc_offset) >> 2;
+                        ib[h_idx] = header;
+                        ib[h_idx + 4] = (EVERGREEN_VLINE_STATUS + radeon_crtc->crtc_offset) >> 2;
+                        break;
+                default:
+                        DRM_ERROR("unknown crtc reloc\n");
+                        r = -EINVAL;
+                        goto out;
+                }
+        }
+out:
+        mutex_unlock(&p->rdev->ddev->mode_config.mutex);
+        return r;
+}
+
+static int evergreen_packet0_check(struct radeon_cs_parser *p,
+                                   struct radeon_cs_packet *pkt,
+                                   unsigned idx, unsigned reg)
+{
+        int r;
+
+        switch (reg) {
+        case EVERGREEN_VLINE_START_END:
+                r = evergreen_cs_packet_parse_vline(p);
+                if (r) {
+                        DRM_ERROR("No reloc for ib[%d]=0x%04X\n",
+                                        idx, reg);
+                        return r;
+                }
+                break;
+        default:
+                printk(KERN_ERR "Forbidden register 0x%04X in cs at %d\n",
+                       reg, idx);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+static int evergreen_cs_parse_packet0(struct radeon_cs_parser *p,
+                                      struct radeon_cs_packet *pkt)
+{
+        unsigned reg, i;
+        unsigned idx;
+        int r;
+
+        idx = pkt->idx + 1;
+        reg = pkt->reg;
+        for (i = 0; i <= pkt->count; i++, idx++, reg += 4) {
+                r = evergreen_packet0_check(p, pkt, idx, reg);
+                if (r) {
+                        return r;
+                }
+        }
+        return 0;
+}
+
+/**
+ * evergreen_cs_check_reg() - check if register is authorized or not
+ * @parser: parser structure holding parsing context
+ * @reg: register we are testing
+ * @idx: index into the cs buffer
+ *
+ * This function will test against evergreen_reg_safe_bm and return 0
+ * if register is safe. If register is not flag as safe this function
+ * will test it against a list of register needind special handling.
+ */
+static inline int evergreen_cs_check_reg(struct radeon_cs_parser *p, u32 reg, u32 idx)
+{
+        struct evergreen_cs_track *track = (struct evergreen_cs_track *)p->track;
+        struct radeon_cs_reloc *reloc;
+        u32 last_reg = ARRAY_SIZE(evergreen_reg_safe_bm);
+        u32 m, i, tmp, *ib;
+        int r;
+
+        i = (reg >> 7);
+        if (i > last_reg) {
+                dev_warn(p->dev, "forbidden register 0x%08x at %d\n", reg, idx);
+                return -EINVAL;
+        }
+        m = 1 << ((reg >> 2) & 31);
+        if (!(evergreen_reg_safe_bm[i] & m))
+                return 0;
+        ib = p->ib->ptr;
+        switch (reg) {
+        /* force following reg to 0 in an attemp to disable out buffer
+         * which will need us to better understand how it works to perform
+         * security check on it (Jerome)
+         */
+        case SQ_ESGS_RING_SIZE:
+        case SQ_GSVS_RING_SIZE:
+        case SQ_ESTMP_RING_SIZE:
+        case SQ_GSTMP_RING_SIZE:
+        case SQ_HSTMP_RING_SIZE:
+        case SQ_LSTMP_RING_SIZE:
+        case SQ_PSTMP_RING_SIZE:
+        case SQ_VSTMP_RING_SIZE:
+        case SQ_ESGS_RING_ITEMSIZE:
+        case SQ_ESTMP_RING_ITEMSIZE:
+        case SQ_GSTMP_RING_ITEMSIZE:
+        case SQ_GSVS_RING_ITEMSIZE:
+        case SQ_GS_VERT_ITEMSIZE:
+        case SQ_GS_VERT_ITEMSIZE_1:
+        case SQ_GS_VERT_ITEMSIZE_2:
+        case SQ_GS_VERT_ITEMSIZE_3:
+        case SQ_GSVS_RING_OFFSET_1:
+        case SQ_GSVS_RING_OFFSET_2:
+        case SQ_GSVS_RING_OFFSET_3:
+        case SQ_HSTMP_RING_ITEMSIZE:
+        case SQ_LSTMP_RING_ITEMSIZE:
+        case SQ_PSTMP_RING_ITEMSIZE:
+        case SQ_VSTMP_RING_ITEMSIZE:
+        case VGT_TF_RING_SIZE:
+                /* get value to populate the IB don't remove */
+                tmp =radeon_get_ib_value(p, idx);
+                ib[idx] = 0;
+                break;
+        case DB_DEPTH_CONTROL:
+                track->db_depth_control = radeon_get_ib_value(p, idx);
+                break;
+        case DB_Z_INFO:
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        dev_warn(p->dev, "bad SET_CONTEXT_REG "
+                                        "0x%04X\n", reg);
+                        return -EINVAL;
+                }
+                track->db_z_info = radeon_get_ib_value(p, idx);
+                ib[idx] &= ~Z_ARRAY_MODE(0xf);
+                track->db_z_info &= ~Z_ARRAY_MODE(0xf);
+                if (reloc->lobj.tiling_flags & RADEON_TILING_MACRO) {
+                        ib[idx] |= Z_ARRAY_MODE(ARRAY_2D_TILED_THIN1);
+                        track->db_z_info |= Z_ARRAY_MODE(ARRAY_2D_TILED_THIN1);
+                } else {
+                        ib[idx] |= Z_ARRAY_MODE(ARRAY_1D_TILED_THIN1);
+                        track->db_z_info |= Z_ARRAY_MODE(ARRAY_1D_TILED_THIN1);
+                }
+                break;
+        case DB_STENCIL_INFO:
+                track->db_s_info = radeon_get_ib_value(p, idx);
+                break;
+        case DB_DEPTH_VIEW:
+                track->db_depth_view = radeon_get_ib_value(p, idx);
+                break;
+        case DB_DEPTH_SIZE:
+                track->db_depth_size = radeon_get_ib_value(p, idx);
+                track->db_depth_size_idx = idx;
+                break;
+        case DB_Z_READ_BASE:
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        dev_warn(p->dev, "bad SET_CONTEXT_REG "
+                                        "0x%04X\n", reg);
+                        return -EINVAL;
+                }
+                track->db_z_read_offset = radeon_get_ib_value(p, idx);
+                ib[idx] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
+                track->db_z_read_bo = reloc->robj;
+                break;
+        case DB_Z_WRITE_BASE:
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        dev_warn(p->dev, "bad SET_CONTEXT_REG "
+                                        "0x%04X\n", reg);
+                        return -EINVAL;
+                }
+                track->db_z_write_offset = radeon_get_ib_value(p, idx);
+                ib[idx] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
+                track->db_z_write_bo = reloc->robj;
+                break;
+        case DB_STENCIL_READ_BASE:
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        dev_warn(p->dev, "bad SET_CONTEXT_REG "
+                                        "0x%04X\n", reg);
+                        return -EINVAL;
+                }
+                track->db_s_read_offset = radeon_get_ib_value(p, idx);
+                ib[idx] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
+                track->db_s_read_bo = reloc->robj;
+                break;
+        case DB_STENCIL_WRITE_BASE:
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        dev_warn(p->dev, "bad SET_CONTEXT_REG "
+                                        "0x%04X\n", reg);
+                        return -EINVAL;
+                }
+                track->db_s_write_offset = radeon_get_ib_value(p, idx);
+                ib[idx] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
+                track->db_s_write_bo = reloc->robj;
+                break;
+        case VGT_STRMOUT_CONFIG:
+                track->vgt_strmout_config = radeon_get_ib_value(p, idx);
+                break;
+        case VGT_STRMOUT_BUFFER_CONFIG:
+                track->vgt_strmout_buffer_config = radeon_get_ib_value(p, idx);
+                break;
+        case CB_TARGET_MASK:
+                track->cb_target_mask = radeon_get_ib_value(p, idx);
+                break;
+        case CB_SHADER_MASK:
+                track->cb_shader_mask = radeon_get_ib_value(p, idx);
+                break;
+        case PA_SC_AA_CONFIG:
+                tmp = radeon_get_ib_value(p, idx) & MSAA_NUM_SAMPLES_MASK;
+                track->nsamples = 1 << tmp;
+                break;
+        case CB_COLOR0_VIEW:
+        case CB_COLOR1_VIEW:
+        case CB_COLOR2_VIEW:
+        case CB_COLOR3_VIEW:
+        case CB_COLOR4_VIEW:
+        case CB_COLOR5_VIEW:
+        case CB_COLOR6_VIEW:
+        case CB_COLOR7_VIEW:
+                tmp = (reg - CB_COLOR0_VIEW) / 0x3c;
+                track->cb_color_view[tmp] = radeon_get_ib_value(p, idx);
+                break;
+        case CB_COLOR8_VIEW:
+        case CB_COLOR9_VIEW:
+        case CB_COLOR10_VIEW:
+        case CB_COLOR11_VIEW:
+                tmp = ((reg - CB_COLOR8_VIEW) / 0x1c) + 8;
+                track->cb_color_view[tmp] = radeon_get_ib_value(p, idx);
+                break;
+        case CB_COLOR0_INFO:
+        case CB_COLOR1_INFO:
+        case CB_COLOR2_INFO:
+        case CB_COLOR3_INFO:
+        case CB_COLOR4_INFO:
+        case CB_COLOR5_INFO:
+        case CB_COLOR6_INFO:
+        case CB_COLOR7_INFO:
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        dev_warn(p->dev, "bad SET_CONTEXT_REG "
+                                        "0x%04X\n", reg);
+                        return -EINVAL;
+                }
+                tmp = (reg - CB_COLOR0_INFO) / 0x3c;
+                track->cb_color_info[tmp] = radeon_get_ib_value(p, idx);
+                if (reloc->lobj.tiling_flags & RADEON_TILING_MACRO) {
+                        ib[idx] |= CB_ARRAY_MODE(ARRAY_2D_TILED_THIN1);
+                        track->cb_color_info[tmp] |= CB_ARRAY_MODE(ARRAY_2D_TILED_THIN1);
+                } else if (reloc->lobj.tiling_flags & RADEON_TILING_MICRO) {
+                        ib[idx] |= CB_ARRAY_MODE(ARRAY_1D_TILED_THIN1);
+                        track->cb_color_info[tmp] |= CB_ARRAY_MODE(ARRAY_1D_TILED_THIN1);
+                }
+                break;
+        case CB_COLOR8_INFO:
+        case CB_COLOR9_INFO:
+        case CB_COLOR10_INFO:
+        case CB_COLOR11_INFO:
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        dev_warn(p->dev, "bad SET_CONTEXT_REG "
+                                        "0x%04X\n", reg);
+                        return -EINVAL;
+                }
+                tmp = ((reg - CB_COLOR8_INFO) / 0x1c) + 8;
+                track->cb_color_info[tmp] = radeon_get_ib_value(p, idx);
+                if (reloc->lobj.tiling_flags & RADEON_TILING_MACRO) {
+                        ib[idx] |= CB_ARRAY_MODE(ARRAY_2D_TILED_THIN1);
+                        track->cb_color_info[tmp] |= CB_ARRAY_MODE(ARRAY_2D_TILED_THIN1);
+                } else if (reloc->lobj.tiling_flags & RADEON_TILING_MICRO) {
+                        ib[idx] |= CB_ARRAY_MODE(ARRAY_1D_TILED_THIN1);
+                        track->cb_color_info[tmp] |= CB_ARRAY_MODE(ARRAY_1D_TILED_THIN1);
+                }
+                break;
+        case CB_COLOR0_PITCH:
+        case CB_COLOR1_PITCH:
+        case CB_COLOR2_PITCH:
+        case CB_COLOR3_PITCH:
+        case CB_COLOR4_PITCH:
+        case CB_COLOR5_PITCH:
+        case CB_COLOR6_PITCH:
+        case CB_COLOR7_PITCH:
+                tmp = (reg - CB_COLOR0_PITCH) / 0x3c;
+                track->cb_color_pitch[tmp] = radeon_get_ib_value(p, idx);
+                track->cb_color_pitch_idx[tmp] = idx;
+                break;
+        case CB_COLOR8_PITCH:
+        case CB_COLOR9_PITCH:
+        case CB_COLOR10_PITCH:
+        case CB_COLOR11_PITCH:
+                tmp = ((reg - CB_COLOR8_PITCH) / 0x1c) + 8;
+                track->cb_color_pitch[tmp] = radeon_get_ib_value(p, idx);
+                track->cb_color_pitch_idx[tmp] = idx;
+                break;
+        case CB_COLOR0_SLICE:
+        case CB_COLOR1_SLICE:
+        case CB_COLOR2_SLICE:
+        case CB_COLOR3_SLICE:
+        case CB_COLOR4_SLICE:
+        case CB_COLOR5_SLICE:
+        case CB_COLOR6_SLICE:
+        case CB_COLOR7_SLICE:
+                tmp = (reg - CB_COLOR0_SLICE) / 0x3c;
+                track->cb_color_slice[tmp] = radeon_get_ib_value(p, idx);
+                track->cb_color_slice_idx[tmp] = idx;
+                break;
+        case CB_COLOR8_SLICE:
+        case CB_COLOR9_SLICE:
+        case CB_COLOR10_SLICE:
+        case CB_COLOR11_SLICE:
+                tmp = ((reg - CB_COLOR8_SLICE) / 0x1c) + 8;
+                track->cb_color_slice[tmp] = radeon_get_ib_value(p, idx);
+                track->cb_color_slice_idx[tmp] = idx;
+                break;
+        case CB_COLOR0_ATTRIB:
+        case CB_COLOR1_ATTRIB:
+        case CB_COLOR2_ATTRIB:
+        case CB_COLOR3_ATTRIB:
+        case CB_COLOR4_ATTRIB:
+        case CB_COLOR5_ATTRIB:
+        case CB_COLOR6_ATTRIB:
+        case CB_COLOR7_ATTRIB:
+        case CB_COLOR8_ATTRIB:
+        case CB_COLOR9_ATTRIB:
+        case CB_COLOR10_ATTRIB:
+        case CB_COLOR11_ATTRIB:
+                break;
+        case CB_COLOR0_DIM:
+        case CB_COLOR1_DIM:
+        case CB_COLOR2_DIM:
+        case CB_COLOR3_DIM:
+        case CB_COLOR4_DIM:
+        case CB_COLOR5_DIM:
+        case CB_COLOR6_DIM:
+        case CB_COLOR7_DIM:
+                tmp = (reg - CB_COLOR0_DIM) / 0x3c;
+                track->cb_color_dim[tmp] = radeon_get_ib_value(p, idx);
+                track->cb_color_dim_idx[tmp] = idx;
+                break;
+        case CB_COLOR8_DIM:
+        case CB_COLOR9_DIM:
+        case CB_COLOR10_DIM:
+        case CB_COLOR11_DIM:
+                tmp = ((reg - CB_COLOR8_DIM) / 0x1c) + 8;
+                track->cb_color_dim[tmp] = radeon_get_ib_value(p, idx);
+                track->cb_color_dim_idx[tmp] = idx;
+                break;
+        case CB_COLOR0_FMASK:
+        case CB_COLOR1_FMASK:
+        case CB_COLOR2_FMASK:
+        case CB_COLOR3_FMASK:
+        case CB_COLOR4_FMASK:
+        case CB_COLOR5_FMASK:
+        case CB_COLOR6_FMASK:
+        case CB_COLOR7_FMASK:
+                tmp = (reg - CB_COLOR0_FMASK) / 0x3c;
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        dev_err(p->dev, "bad SET_CONTEXT_REG 0x%04X\n", reg);
+                        return -EINVAL;
+                }
+                ib[idx] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
+                track->cb_color_fmask_bo[tmp] = reloc->robj;
+                break;
+        case CB_COLOR0_CMASK:
+        case CB_COLOR1_CMASK:
+        case CB_COLOR2_CMASK:
+        case CB_COLOR3_CMASK:
+        case CB_COLOR4_CMASK:
+        case CB_COLOR5_CMASK:
+        case CB_COLOR6_CMASK:
+        case CB_COLOR7_CMASK:
+                tmp = (reg - CB_COLOR0_CMASK) / 0x3c;
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        dev_err(p->dev, "bad SET_CONTEXT_REG 0x%04X\n", reg);
+                        return -EINVAL;
+                }
+                ib[idx] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
+                track->cb_color_cmask_bo[tmp] = reloc->robj;
+                break;
+        case CB_COLOR0_FMASK_SLICE:
+        case CB_COLOR1_FMASK_SLICE:
+        case CB_COLOR2_FMASK_SLICE:
+        case CB_COLOR3_FMASK_SLICE:
+        case CB_COLOR4_FMASK_SLICE:
+        case CB_COLOR5_FMASK_SLICE:
+        case CB_COLOR6_FMASK_SLICE:
+        case CB_COLOR7_FMASK_SLICE:
+                tmp = (reg - CB_COLOR0_FMASK_SLICE) / 0x3c;
+                track->cb_color_fmask_slice[tmp] = radeon_get_ib_value(p, idx);
+                break;
+        case CB_COLOR0_CMASK_SLICE:
+        case CB_COLOR1_CMASK_SLICE:
+        case CB_COLOR2_CMASK_SLICE:
+        case CB_COLOR3_CMASK_SLICE:
+        case CB_COLOR4_CMASK_SLICE:
+        case CB_COLOR5_CMASK_SLICE:
+        case CB_COLOR6_CMASK_SLICE:
+        case CB_COLOR7_CMASK_SLICE:
+                tmp = (reg - CB_COLOR0_CMASK_SLICE) / 0x3c;
+                track->cb_color_cmask_slice[tmp] = radeon_get_ib_value(p, idx);
+                break;
+        case CB_COLOR0_BASE:
+        case CB_COLOR1_BASE:
+        case CB_COLOR2_BASE:
+        case CB_COLOR3_BASE:
+        case CB_COLOR4_BASE:
+        case CB_COLOR5_BASE:
+        case CB_COLOR6_BASE:
+        case CB_COLOR7_BASE:
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        dev_warn(p->dev, "bad SET_CONTEXT_REG "
+                                        "0x%04X\n", reg);
+                        return -EINVAL;
+                }
+                tmp = (reg - CB_COLOR0_BASE) / 0x3c;
+                track->cb_color_bo_offset[tmp] = radeon_get_ib_value(p, idx);
+                ib[idx] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
+                track->cb_color_base_last[tmp] = ib[idx];
+                track->cb_color_bo[tmp] = reloc->robj;
+                break;
+        case CB_COLOR8_BASE:
+        case CB_COLOR9_BASE:
+        case CB_COLOR10_BASE:
+        case CB_COLOR11_BASE:
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        dev_warn(p->dev, "bad SET_CONTEXT_REG "
+                                        "0x%04X\n", reg);
+                        return -EINVAL;
+                }
+                tmp = ((reg - CB_COLOR8_BASE) / 0x1c) + 8;
+                track->cb_color_bo_offset[tmp] = radeon_get_ib_value(p, idx);
+                ib[idx] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
+                track->cb_color_base_last[tmp] = ib[idx];
+                track->cb_color_bo[tmp] = reloc->robj;
+                break;
+        case CB_IMMED0_BASE:
+        case CB_IMMED1_BASE:
+        case CB_IMMED2_BASE:
+        case CB_IMMED3_BASE:
+        case CB_IMMED4_BASE:
+        case CB_IMMED5_BASE:
+        case CB_IMMED6_BASE:
+        case CB_IMMED7_BASE:
+        case CB_IMMED8_BASE:
+        case CB_IMMED9_BASE:
+        case CB_IMMED10_BASE:
+        case CB_IMMED11_BASE:
+        case DB_HTILE_DATA_BASE:
+        case SQ_PGM_START_FS:
+        case SQ_PGM_START_ES:
+        case SQ_PGM_START_VS:
+        case SQ_PGM_START_GS:
+        case SQ_PGM_START_PS:
+        case SQ_PGM_START_HS:
+        case SQ_PGM_START_LS:
+        case GDS_ADDR_BASE:
+        case SQ_CONST_MEM_BASE:
+        case SQ_ALU_CONST_CACHE_GS_0:
+        case SQ_ALU_CONST_CACHE_GS_1:
+        case SQ_ALU_CONST_CACHE_GS_2:
+        case SQ_ALU_CONST_CACHE_GS_3:
+        case SQ_ALU_CONST_CACHE_GS_4:
+        case SQ_ALU_CONST_CACHE_GS_5:
+        case SQ_ALU_CONST_CACHE_GS_6:
+        case SQ_ALU_CONST_CACHE_GS_7:
+        case SQ_ALU_CONST_CACHE_GS_8:
+        case SQ_ALU_CONST_CACHE_GS_9:
+        case SQ_ALU_CONST_CACHE_GS_10:
+        case SQ_ALU_CONST_CACHE_GS_11:
+        case SQ_ALU_CONST_CACHE_GS_12:
+        case SQ_ALU_CONST_CACHE_GS_13:
+        case SQ_ALU_CONST_CACHE_GS_14:
+        case SQ_ALU_CONST_CACHE_GS_15:
+        case SQ_ALU_CONST_CACHE_PS_0:
+        case SQ_ALU_CONST_CACHE_PS_1:
+        case SQ_ALU_CONST_CACHE_PS_2:
+        case SQ_ALU_CONST_CACHE_PS_3:
+        case SQ_ALU_CONST_CACHE_PS_4:
+        case SQ_ALU_CONST_CACHE_PS_5:
+        case SQ_ALU_CONST_CACHE_PS_6:
+        case SQ_ALU_CONST_CACHE_PS_7:
+        case SQ_ALU_CONST_CACHE_PS_8:
+        case SQ_ALU_CONST_CACHE_PS_9:
+        case SQ_ALU_CONST_CACHE_PS_10:
+        case SQ_ALU_CONST_CACHE_PS_11:
+        case SQ_ALU_CONST_CACHE_PS_12:
+        case SQ_ALU_CONST_CACHE_PS_13:
+        case SQ_ALU_CONST_CACHE_PS_14:
+        case SQ_ALU_CONST_CACHE_PS_15:
+        case SQ_ALU_CONST_CACHE_VS_0:
+        case SQ_ALU_CONST_CACHE_VS_1:
+        case SQ_ALU_CONST_CACHE_VS_2:
+        case SQ_ALU_CONST_CACHE_VS_3:
+        case SQ_ALU_CONST_CACHE_VS_4:
+        case SQ_ALU_CONST_CACHE_VS_5:
+        case SQ_ALU_CONST_CACHE_VS_6:
+        case SQ_ALU_CONST_CACHE_VS_7:
+        case SQ_ALU_CONST_CACHE_VS_8:
+        case SQ_ALU_CONST_CACHE_VS_9:
+        case SQ_ALU_CONST_CACHE_VS_10:
+        case SQ_ALU_CONST_CACHE_VS_11:
+        case SQ_ALU_CONST_CACHE_VS_12:
+        case SQ_ALU_CONST_CACHE_VS_13:
+        case SQ_ALU_CONST_CACHE_VS_14:
+        case SQ_ALU_CONST_CACHE_VS_15:
+        case SQ_ALU_CONST_CACHE_HS_0:
+        case SQ_ALU_CONST_CACHE_HS_1:
+        case SQ_ALU_CONST_CACHE_HS_2:
+        case SQ_ALU_CONST_CACHE_HS_3:
+        case SQ_ALU_CONST_CACHE_HS_4:
+        case SQ_ALU_CONST_CACHE_HS_5:
+        case SQ_ALU_CONST_CACHE_HS_6:
+        case SQ_ALU_CONST_CACHE_HS_7:
+        case SQ_ALU_CONST_CACHE_HS_8:
+        case SQ_ALU_CONST_CACHE_HS_9:
+        case SQ_ALU_CONST_CACHE_HS_10:
+        case SQ_ALU_CONST_CACHE_HS_11:
+        case SQ_ALU_CONST_CACHE_HS_12:
+        case SQ_ALU_CONST_CACHE_HS_13:
+        case SQ_ALU_CONST_CACHE_HS_14:
+        case SQ_ALU_CONST_CACHE_HS_15:
+        case SQ_ALU_CONST_CACHE_LS_0:
+        case SQ_ALU_CONST_CACHE_LS_1:
+        case SQ_ALU_CONST_CACHE_LS_2:
+        case SQ_ALU_CONST_CACHE_LS_3:
+        case SQ_ALU_CONST_CACHE_LS_4:
+        case SQ_ALU_CONST_CACHE_LS_5:
+        case SQ_ALU_CONST_CACHE_LS_6:
+        case SQ_ALU_CONST_CACHE_LS_7:
+        case SQ_ALU_CONST_CACHE_LS_8:
+        case SQ_ALU_CONST_CACHE_LS_9:
+        case SQ_ALU_CONST_CACHE_LS_10:
+        case SQ_ALU_CONST_CACHE_LS_11:
+        case SQ_ALU_CONST_CACHE_LS_12:
+        case SQ_ALU_CONST_CACHE_LS_13:
+        case SQ_ALU_CONST_CACHE_LS_14:
+        case SQ_ALU_CONST_CACHE_LS_15:
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        dev_warn(p->dev, "bad SET_CONTEXT_REG "
+                                        "0x%04X\n", reg);
+                        return -EINVAL;
+                }
+                ib[idx] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
+                break;
+        default:
+                dev_warn(p->dev, "forbidden register 0x%08x at %d\n", reg, idx);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+/**
+ * evergreen_check_texture_resource() - check if register is authorized or not
+ * @p: parser structure holding parsing context
+ * @idx: index into the cs buffer
+ * @texture: texture's bo structure
+ * @mipmap: mipmap's bo structure
+ *
+ * This function will check that the resource has valid field and that
+ * the texture and mipmap bo object are big enough to cover this resource.
+ */
+static inline int evergreen_check_texture_resource(struct radeon_cs_parser *p,  u32 idx,
+                                                   struct radeon_bo *texture,
+                                                   struct radeon_bo *mipmap)
+{
+        /* XXX fill in */
+        return 0;
+}
+
+static int evergreen_packet3_check(struct radeon_cs_parser *p,
+                                   struct radeon_cs_packet *pkt)
+{
+        struct radeon_cs_reloc *reloc;
+        struct evergreen_cs_track *track;
+        volatile u32 *ib;
+        unsigned idx;
+        unsigned i;
+        unsigned start_reg, end_reg, reg;
+        int r;
+        u32 idx_value;
+
+        track = (struct evergreen_cs_track *)p->track;
+        ib = p->ib->ptr;
+        idx = pkt->idx + 1;
+        idx_value = radeon_get_ib_value(p, idx);
+
+        switch (pkt->opcode) {
+        case PACKET3_CONTEXT_CONTROL:
+                if (pkt->count != 1) {
+                        DRM_ERROR("bad CONTEXT_CONTROL\n");
+                        return -EINVAL;
+                }
+                break;
+        case PACKET3_INDEX_TYPE:
+        case PACKET3_NUM_INSTANCES:
+        case PACKET3_CLEAR_STATE:
+                if (pkt->count) {
+                        DRM_ERROR("bad INDEX_TYPE/NUM_INSTANCES/CLEAR_STATE\n");
+                        return -EINVAL;
+                }
+                break;
+        case PACKET3_INDEX_BASE:
+                if (pkt->count != 1) {
+                        DRM_ERROR("bad INDEX_BASE\n");
+                        return -EINVAL;
+                }
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        DRM_ERROR("bad INDEX_BASE\n");
+                        return -EINVAL;
+                }
+                ib[idx+0] = idx_value + (u32)(reloc->lobj.gpu_offset & 0xffffffff);
+                ib[idx+1] += upper_32_bits(reloc->lobj.gpu_offset) & 0xff;
+                r = evergreen_cs_track_check(p);
+                if (r) {
+                        dev_warn(p->dev, "%s:%d invalid cmd stream\n", __func__, __LINE__);
+                        return r;
+                }
+                break;
+        case PACKET3_DRAW_INDEX:
+                if (pkt->count != 3) {
+                        DRM_ERROR("bad DRAW_INDEX\n");
+                        return -EINVAL;
+                }
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        DRM_ERROR("bad DRAW_INDEX\n");
+                        return -EINVAL;
+                }
+                ib[idx+0] = idx_value + (u32)(reloc->lobj.gpu_offset & 0xffffffff);
+                ib[idx+1] += upper_32_bits(reloc->lobj.gpu_offset) & 0xff;
+                r = evergreen_cs_track_check(p);
+                if (r) {
+                        dev_warn(p->dev, "%s:%d invalid cmd stream\n", __func__, __LINE__);
+                        return r;
+                }
+                break;
+        case PACKET3_DRAW_INDEX_2:
+                if (pkt->count != 4) {
+                        DRM_ERROR("bad DRAW_INDEX_2\n");
+                        return -EINVAL;
+                }
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        DRM_ERROR("bad DRAW_INDEX_2\n");
+                        return -EINVAL;
+                }
+                ib[idx+1] = idx_value + (u32)(reloc->lobj.gpu_offset & 0xffffffff);
+                ib[idx+2] += upper_32_bits(reloc->lobj.gpu_offset) & 0xff;
+                r = evergreen_cs_track_check(p);
+                if (r) {
+                        dev_warn(p->dev, "%s:%d invalid cmd stream\n", __func__, __LINE__);
+                        return r;
+                }
+                break;
+        case PACKET3_DRAW_INDEX_AUTO:
+                if (pkt->count != 1) {
+                        DRM_ERROR("bad DRAW_INDEX_AUTO\n");
+                        return -EINVAL;
+                }
+                r = evergreen_cs_track_check(p);
+                if (r) {
+                        dev_warn(p->dev, "%s:%d invalid cmd stream %d\n", __func__, __LINE__, idx);
+                        return r;
+                }
+                break;
+        case PACKET3_DRAW_INDEX_MULTI_AUTO:
+                if (pkt->count != 2) {
+                        DRM_ERROR("bad DRAW_INDEX_MULTI_AUTO\n");
+                        return -EINVAL;
+                }
+                r = evergreen_cs_track_check(p);
+                if (r) {
+                        dev_warn(p->dev, "%s:%d invalid cmd stream %d\n", __func__, __LINE__, idx);
+                        return r;
+                }
+                break;
+        case PACKET3_DRAW_INDEX_IMMD:
+                if (pkt->count < 2) {
+                        DRM_ERROR("bad DRAW_INDEX_IMMD\n");
+                        return -EINVAL;
+                }
+                r = evergreen_cs_track_check(p);
+                if (r) {
+                        dev_warn(p->dev, "%s:%d invalid cmd stream\n", __func__, __LINE__);
+                        return r;
+                }
+                break;
+        case PACKET3_DRAW_INDEX_OFFSET:
+                if (pkt->count != 2) {
+                        DRM_ERROR("bad DRAW_INDEX_OFFSET\n");
+                        return -EINVAL;
+                }
+                r = evergreen_cs_track_check(p);
+                if (r) {
+                        dev_warn(p->dev, "%s:%d invalid cmd stream\n", __func__, __LINE__);
+                        return r;
+                }
+                break;
+        case PACKET3_DRAW_INDEX_OFFSET_2:
+                if (pkt->count != 3) {
+                        DRM_ERROR("bad DRAW_INDEX_OFFSET_2\n");
+                        return -EINVAL;
+                }
+                r = evergreen_cs_track_check(p);
+                if (r) {
+                        dev_warn(p->dev, "%s:%d invalid cmd stream\n", __func__, __LINE__);
+                        return r;
+                }
+                break;
+        case PACKET3_WAIT_REG_MEM:
+                if (pkt->count != 5) {
+                        DRM_ERROR("bad WAIT_REG_MEM\n");
+                        return -EINVAL;
+                }
+                /* bit 4 is reg (0) or mem (1) */
+                if (idx_value & 0x10) {
+                        r = evergreen_cs_packet_next_reloc(p, &reloc);
+                        if (r) {
+                                DRM_ERROR("bad WAIT_REG_MEM\n");
+                                return -EINVAL;
+                        }
+                        ib[idx+1] += (u32)(reloc->lobj.gpu_offset & 0xffffffff);
+                        ib[idx+2] += upper_32_bits(reloc->lobj.gpu_offset) & 0xff;
+                }
+                break;
+        case PACKET3_SURFACE_SYNC:
+                if (pkt->count != 3) {
+                        DRM_ERROR("bad SURFACE_SYNC\n");
+                        return -EINVAL;
+                }
+                /* 0xffffffff/0x0 is flush all cache flag */
+                if (radeon_get_ib_value(p, idx + 1) != 0xffffffff ||
+                    radeon_get_ib_value(p, idx + 2) != 0) {
+                        r = evergreen_cs_packet_next_reloc(p, &reloc);
+                        if (r) {
+                                DRM_ERROR("bad SURFACE_SYNC\n");
+                                return -EINVAL;
+                        }
+                        ib[idx+2] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
+                }
+                break;
+        case PACKET3_EVENT_WRITE:
+                if (pkt->count != 2 && pkt->count != 0) {
+                        DRM_ERROR("bad EVENT_WRITE\n");
+                        return -EINVAL;
+                }
+                if (pkt->count) {
+                        r = evergreen_cs_packet_next_reloc(p, &reloc);
+                        if (r) {
+                                DRM_ERROR("bad EVENT_WRITE\n");
+                                return -EINVAL;
+                        }
+                        ib[idx+1] += (u32)(reloc->lobj.gpu_offset & 0xffffffff);
+                        ib[idx+2] += upper_32_bits(reloc->lobj.gpu_offset) & 0xff;
+                }
+                break;
+        case PACKET3_EVENT_WRITE_EOP:
+                if (pkt->count != 4) {
+                        DRM_ERROR("bad EVENT_WRITE_EOP\n");
+                        return -EINVAL;
+                }
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        DRM_ERROR("bad EVENT_WRITE_EOP\n");
+                        return -EINVAL;
+                }
+                ib[idx+1] += (u32)(reloc->lobj.gpu_offset & 0xffffffff);
+                ib[idx+2] += upper_32_bits(reloc->lobj.gpu_offset) & 0xff;
+                break;
+        case PACKET3_EVENT_WRITE_EOS:
+                if (pkt->count != 3) {
+                        DRM_ERROR("bad EVENT_WRITE_EOS\n");
+                        return -EINVAL;
+                }
+                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                if (r) {
+                        DRM_ERROR("bad EVENT_WRITE_EOS\n");
+                        return -EINVAL;
+                }
+                ib[idx+1] += (u32)(reloc->lobj.gpu_offset & 0xffffffff);
+                ib[idx+2] += upper_32_bits(reloc->lobj.gpu_offset) & 0xff;
+                break;
+        case PACKET3_SET_CONFIG_REG:
+                start_reg = (idx_value << 2) + PACKET3_SET_CONFIG_REG_START;
+                end_reg = 4 * pkt->count + start_reg - 4;
+                if ((start_reg < PACKET3_SET_CONFIG_REG_START) ||
+                    (start_reg >= PACKET3_SET_CONFIG_REG_END) ||
+                    (end_reg >= PACKET3_SET_CONFIG_REG_END)) {
+                        DRM_ERROR("bad PACKET3_SET_CONFIG_REG\n");
+                        return -EINVAL;
+                }
+                for (i = 0; i < pkt->count; i++) {
+                        reg = start_reg + (4 * i);
+                        r = evergreen_cs_check_reg(p, reg, idx+1+i);
+                        if (r)
+                                return r;
+                }
+                break;
+        case PACKET3_SET_CONTEXT_REG:
+                start_reg = (idx_value << 2) + PACKET3_SET_CONTEXT_REG_START;
+                end_reg = 4 * pkt->count + start_reg - 4;
+                if ((start_reg < PACKET3_SET_CONTEXT_REG_START) ||
+                    (start_reg >= PACKET3_SET_CONTEXT_REG_END) ||
+                    (end_reg >= PACKET3_SET_CONTEXT_REG_END)) {
+                        DRM_ERROR("bad PACKET3_SET_CONTEXT_REG\n");
+                        return -EINVAL;
+                }
+                for (i = 0; i < pkt->count; i++) {
+                        reg = start_reg + (4 * i);
+                        r = evergreen_cs_check_reg(p, reg, idx+1+i);
+                        if (r)
+                                return r;
+                }
+                break;
+        case PACKET3_SET_RESOURCE:
+                if (pkt->count % 8) {
+                        DRM_ERROR("bad SET_RESOURCE\n");
+                        return -EINVAL;
+                }
+                start_reg = (idx_value << 2) + PACKET3_SET_RESOURCE_START;
+                end_reg = 4 * pkt->count + start_reg - 4;
+                if ((start_reg < PACKET3_SET_RESOURCE_START) ||
+                    (start_reg >= PACKET3_SET_RESOURCE_END) ||
+                    (end_reg >= PACKET3_SET_RESOURCE_END)) {
+                        DRM_ERROR("bad SET_RESOURCE\n");
+                        return -EINVAL;
+                }
+                for (i = 0; i < (pkt->count / 8); i++) {
+                        struct radeon_bo *texture, *mipmap;
+                        u32 size, offset;
+
+                        switch (G__SQ_CONSTANT_TYPE(radeon_get_ib_value(p, idx+1+(i*8)+7))) {
+                        case SQ_TEX_VTX_VALID_TEXTURE:
+                                /* tex base */
+                                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                                if (r) {
+                                        DRM_ERROR("bad SET_RESOURCE (tex)\n");
+                                        return -EINVAL;
+                                }
+                                ib[idx+1+(i*8)+3] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
+                                if (reloc->lobj.tiling_flags & RADEON_TILING_MACRO)
+                                        ib[idx+1+(i*8)+1] |= TEX_ARRAY_MODE(ARRAY_2D_TILED_THIN1);
+                                else if (reloc->lobj.tiling_flags & RADEON_TILING_MICRO)
+                                        ib[idx+1+(i*8)+1] |= TEX_ARRAY_MODE(ARRAY_1D_TILED_THIN1);
+                                texture = reloc->robj;
+                                /* tex mip base */
+                                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                                if (r) {
+                                        DRM_ERROR("bad SET_RESOURCE (tex)\n");
+                                        return -EINVAL;
+                                }
+                                ib[idx+1+(i*8)+4] += (u32)((reloc->lobj.gpu_offset >> 8) & 0xffffffff);
+                                mipmap = reloc->robj;
+                                r = evergreen_check_texture_resource(p,  idx+1+(i*8),
+                                                texture, mipmap);
+                                if (r)
+                                        return r;
+                                break;
+                        case SQ_TEX_VTX_VALID_BUFFER:
+                                /* vtx base */
+                                r = evergreen_cs_packet_next_reloc(p, &reloc);
+                                if (r) {
+                                        DRM_ERROR("bad SET_RESOURCE (vtx)\n");
+                                        return -EINVAL;
+                                }
+                                offset = radeon_get_ib_value(p, idx+1+(i*8)+0);
+                                size = radeon_get_ib_value(p, idx+1+(i*8)+1);
+                                if (p->rdev && (size + offset) > radeon_bo_size(reloc->robj)) {
+                                        /* force size to size of the buffer */
+                                        dev_warn(p->dev, "vbo resource seems too big for the bo\n");
+                                        ib[idx+1+(i*8)+1] = radeon_bo_size(reloc->robj);
+                                }
+                                ib[idx+1+(i*8)+0] += (u32)((reloc->lobj.gpu_offset) & 0xffffffff);
+                                ib[idx+1+(i*8)+2] += upper_32_bits(reloc->lobj.gpu_offset) & 0xff;
+                                break;
+                        case SQ_TEX_VTX_INVALID_TEXTURE:
+                        case SQ_TEX_VTX_INVALID_BUFFER:
+                        default:
+                                DRM_ERROR("bad SET_RESOURCE\n");
+                                return -EINVAL;
+                        }
+                }
+                break;
+        case PACKET3_SET_ALU_CONST:
+                /* XXX fix me ALU const buffers only */
+                break;
+        case PACKET3_SET_BOOL_CONST:
+                start_reg = (idx_value << 2) + PACKET3_SET_BOOL_CONST_START;
+                end_reg = 4 * pkt->count + start_reg - 4;
+                if ((start_reg < PACKET3_SET_BOOL_CONST_START) ||
+                    (start_reg >= PACKET3_SET_BOOL_CONST_END) ||
+                    (end_reg >= PACKET3_SET_BOOL_CONST_END)) {
+                        DRM_ERROR("bad SET_BOOL_CONST\n");
+                        return -EINVAL;
+                }
+                break;
+        case PACKET3_SET_LOOP_CONST:
+                start_reg = (idx_value << 2) + PACKET3_SET_LOOP_CONST_START;
+                end_reg = 4 * pkt->count + start_reg - 4;
+                if ((start_reg < PACKET3_SET_LOOP_CONST_START) ||
+                    (start_reg >= PACKET3_SET_LOOP_CONST_END) ||
+                    (end_reg >= PACKET3_SET_LOOP_CONST_END)) {
+                        DRM_ERROR("bad SET_LOOP_CONST\n");
+                        return -EINVAL;
+                }
+                break;
+        case PACKET3_SET_CTL_CONST:
+                start_reg = (idx_value << 2) + PACKET3_SET_CTL_CONST_START;
+                end_reg = 4 * pkt->count + start_reg - 4;
+                if ((start_reg < PACKET3_SET_CTL_CONST_START) ||
+                    (start_reg >= PACKET3_SET_CTL_CONST_END) ||
+                    (end_reg >= PACKET3_SET_CTL_CONST_END)) {
+                        DRM_ERROR("bad SET_CTL_CONST\n");
+                        return -EINVAL;
+                }
+                break;
+        case PACKET3_SET_SAMPLER:
+                if (pkt->count % 3) {
+                        DRM_ERROR("bad SET_SAMPLER\n");
+                        return -EINVAL;
+                }
+                start_reg = (idx_value << 2) + PACKET3_SET_SAMPLER_START;
+                end_reg = 4 * pkt->count + start_reg - 4;
+                if ((start_reg < PACKET3_SET_SAMPLER_START) ||
+                    (start_reg >= PACKET3_SET_SAMPLER_END) ||
+                    (end_reg >= PACKET3_SET_SAMPLER_END)) {
+                        DRM_ERROR("bad SET_SAMPLER\n");
+                        return -EINVAL;
+                }
+                break;
+        case PACKET3_NOP:
+                break;
+        default:
+                DRM_ERROR("Packet3 opcode %x not supported\n", pkt->opcode);
+                return -EINVAL;
+        }
+        return 0;
+}
+
+int evergreen_cs_parse(struct radeon_cs_parser *p)
+{
+        struct radeon_cs_packet pkt;
+        struct evergreen_cs_track *track;
+        int r;
+
+        if (p->track == NULL) {
+                /* initialize tracker, we are in kms */
+                track = kzalloc(sizeof(*track), GFP_KERNEL);
+                if (track == NULL)
+                        return -ENOMEM;
+                evergreen_cs_track_init(track);
+                track->npipes = p->rdev->config.evergreen.tiling_npipes;
+                track->nbanks = p->rdev->config.evergreen.tiling_nbanks;
+                track->group_size = p->rdev->config.evergreen.tiling_group_size;
+                p->track = track;
+        }
+        do {
+                r = evergreen_cs_packet_parse(p, &pkt, p->idx);
+                if (r) {
+                        kfree(p->track);
+                        p->track = NULL;
+                        return r;
+                }
+                p->idx += pkt.count + 2;
+                switch (pkt.type) {
+                case PACKET_TYPE0:
+                        r = evergreen_cs_parse_packet0(p, &pkt);
+                        break;
+                case PACKET_TYPE2:
+                        break;
+                case PACKET_TYPE3:
+                        r = evergreen_packet3_check(p, &pkt);
+                        break;
+                default:
+                        DRM_ERROR("Unknown packet type %d !\n", pkt.type);
+                        kfree(p->track);
+                        p->track = NULL;
+                        return -EINVAL;
+                }
+                if (r) {
+                        kfree(p->track);
+                        p->track = NULL;
+                        return r;
+                }
+        } while (p->idx < p->chunks[p->chunk_ib_idx].length_dw);
+#if 0
+        for (r = 0; r < p->ib->length_dw; r++) {
+                printk(KERN_INFO "%05d  0x%08X\n", r, p->ib->ptr[r]);
+                mdelay(1);
+        }
+#endif
+        kfree(p->track);
+        p->track = NULL;
+        return 0;
+}
+
diff --git a/drivers/gpu/drm/radeon/evergreen_reg.h b/drivers/gpu/drm/radeon/evergreen_reg.h
index af86af836f13..e028c1cd9d9b 100644
--- a/drivers/gpu/drm/radeon/evergreen_reg.h
+++ b/drivers/gpu/drm/radeon/evergreen_reg.h
@@ -151,6 +151,9 @@
 #define EVERGREEN_DATA_FORMAT                           0x6b00
 #       define EVERGREEN_INTERLEAVE_EN                  (1 << 0)
 #define EVERGREEN_DESKTOP_HEIGHT                        0x6b04
+#define EVERGREEN_VLINE_START_END                       0x6b08
+#define EVERGREEN_VLINE_STATUS                          0x6bb8
+#       define EVERGREEN_VLINE_STAT                     (1 << 12)
 
 #define EVERGREEN_VIEWPORT_START                        0x6d70
 #define EVERGREEN_VIEWPORT_SIZE                         0x6d74
diff --git a/drivers/gpu/drm/radeon/evergreend.h b/drivers/gpu/drm/radeon/evergreend.h
index 93e9e17ad54a..79683f6b4452 100644
--- a/drivers/gpu/drm/radeon/evergreend.h
+++ b/drivers/gpu/drm/radeon/evergreend.h
@@ -218,6 +218,8 @@
 #define         CLIP_VTX_REORDER_ENA                            (1 << 0)
 #define         NUM_CLIP_SEQ(x)                                 ((x) << 1)
 #define PA_SC_AA_CONFIG                                 0x28C04
+#define         MSAA_NUM_SAMPLES_SHIFT                  0
+#define         MSAA_NUM_SAMPLES_MASK                   0x3
 #define PA_SC_CLIPRECT_RULE                             0x2820C
 #define PA_SC_EDGERULE                                  0x28230
 #define PA_SC_FIFO_SIZE                                 0x8BCC
@@ -553,4 +555,466 @@
 #       define DC_HPDx_RX_INT_TIMER(x)                    ((x) << 16)
 #       define DC_HPDx_EN                                 (1 << 28)
 
+/*
+ * PM4
+ */
+#define PACKET_TYPE0    0
+#define PACKET_TYPE1    1
+#define PACKET_TYPE2    2
+#define PACKET_TYPE3    3
+
+#define CP_PACKET_GET_TYPE(h) (((h) >> 30) & 3)
+#define CP_PACKET_GET_COUNT(h) (((h) >> 16) & 0x3FFF)
+#define CP_PACKET0_GET_REG(h) (((h) & 0xFFFF) << 2)
+#define CP_PACKET3_GET_OPCODE(h) (((h) >> 8) & 0xFF)
+#define PACKET0(reg, n) ((PACKET_TYPE0 << 30) |                         \
+                         (((reg) >> 2) & 0xFFFF) |                      \
+                         ((n) & 0x3FFF) << 16)
+#define CP_PACKET2                      0x80000000
+#define         PACKET2_PAD_SHIFT               0
+#define         PACKET2_PAD_MASK                (0x3fffffff << 0)
+
+#define PACKET2(v)      (CP_PACKET2 | REG_SET(PACKET2_PAD, (v)))
+
+#define PACKET3(op, n)  ((PACKET_TYPE3 << 30) |                         \
+                         (((op) & 0xFF) << 8) |                         \
+                         ((n) & 0x3FFF) << 16)
+
+/* Packet 3 types */
+#define PACKET3_NOP                                     0x10
+#define PACKET3_SET_BASE                                0x11
+#define PACKET3_CLEAR_STATE                             0x12
+#define PACKET3_INDIRECT_BUFFER_SIZE                    0x13
+#define PACKET3_DISPATCH_DIRECT                         0x15
+#define PACKET3_DISPATCH_INDIRECT                       0x16
+#define PACKET3_INDIRECT_BUFFER_END                     0x17
+#define PACKET3_SET_PREDICATION                         0x20
+#define PACKET3_REG_RMW                                 0x21
+#define PACKET3_COND_EXEC                               0x22
+#define PACKET3_PRED_EXEC                               0x23
+#define PACKET3_DRAW_INDIRECT                           0x24
+#define PACKET3_DRAW_INDEX_INDIRECT                     0x25
+#define PACKET3_INDEX_BASE                              0x26
+#define PACKET3_DRAW_INDEX_2                            0x27
+#define PACKET3_CONTEXT_CONTROL                         0x28
+#define PACKET3_DRAW_INDEX_OFFSET                       0x29
+#define PACKET3_INDEX_TYPE                              0x2A
+#define PACKET3_DRAW_INDEX                              0x2B
+#define PACKET3_DRAW_INDEX_AUTO                         0x2D
+#define PACKET3_DRAW_INDEX_IMMD                         0x2E
+#define PACKET3_NUM_INSTANCES                           0x2F
+#define PACKET3_DRAW_INDEX_MULTI_AUTO                   0x30
+#define PACKET3_STRMOUT_BUFFER_UPDATE                   0x34
+#define PACKET3_DRAW_INDEX_OFFSET_2                     0x35
+#define PACKET3_DRAW_INDEX_MULTI_ELEMENT                0x36
+#define PACKET3_MEM_SEMAPHORE                           0x39
+#define PACKET3_MPEG_INDEX                              0x3A
+#define PACKET3_WAIT_REG_MEM                            0x3C
+#define PACKET3_MEM_WRITE                               0x3D
+#define PACKET3_INDIRECT_BUFFER                         0x32
+#define PACKET3_SURFACE_SYNC                            0x43
+#              define PACKET3_CB0_DEST_BASE_ENA    (1 << 6)
+#              define PACKET3_CB1_DEST_BASE_ENA    (1 << 7)
+#              define PACKET3_CB2_DEST_BASE_ENA    (1 << 8)
+#              define PACKET3_CB3_DEST_BASE_ENA    (1 << 9)
+#              define PACKET3_CB4_DEST_BASE_ENA    (1 << 10)
+#              define PACKET3_CB5_DEST_BASE_ENA    (1 << 11)
+#              define PACKET3_CB6_DEST_BASE_ENA    (1 << 12)
+#              define PACKET3_CB7_DEST_BASE_ENA    (1 << 13)
+#              define PACKET3_DB_DEST_BASE_ENA     (1 << 14)
+#              define PACKET3_CB8_DEST_BASE_ENA    (1 << 15)
+#              define PACKET3_CB9_DEST_BASE_ENA    (1 << 16)
+#              define PACKET3_CB10_DEST_BASE_ENA   (1 << 17)
+#              define PACKET3_CB11_DEST_BASE_ENA   (1 << 17)
+#              define PACKET3_FULL_CACHE_ENA       (1 << 20)
+#              define PACKET3_TC_ACTION_ENA        (1 << 23)
+#              define PACKET3_VC_ACTION_ENA        (1 << 24)
+#              define PACKET3_CB_ACTION_ENA        (1 << 25)
+#              define PACKET3_DB_ACTION_ENA        (1 << 26)
+#              define PACKET3_SH_ACTION_ENA        (1 << 27)
+#              define PACKET3_SMX_ACTION_ENA       (1 << 28)
+#define PACKET3_ME_INITIALIZE                           0x44
+#define         PACKET3_ME_INITIALIZE_DEVICE_ID(x) ((x) << 16)
+#define PACKET3_COND_WRITE                              0x45
+#define PACKET3_EVENT_WRITE                             0x46
+#define PACKET3_EVENT_WRITE_EOP                         0x47
+#define PACKET3_EVENT_WRITE_EOS                         0x48
+#define PACKET3_PREAMBLE_CNTL                           0x4A
+#define PACKET3_RB_OFFSET                               0x4B
+#define PACKET3_ALU_PS_CONST_BUFFER_COPY                0x4C
+#define PACKET3_ALU_VS_CONST_BUFFER_COPY                0x4D
+#define PACKET3_ALU_PS_CONST_UPDATE                     0x4E
+#define PACKET3_ALU_VS_CONST_UPDATE                     0x4F
+#define PACKET3_ONE_REG_WRITE                           0x57
+#define PACKET3_SET_CONFIG_REG                          0x68
+#define         PACKET3_SET_CONFIG_REG_START                    0x00008000
+#define         PACKET3_SET_CONFIG_REG_END                      0x0000ac00
+#define PACKET3_SET_CONTEXT_REG                         0x69
+#define         PACKET3_SET_CONTEXT_REG_START                   0x00028000
+#define         PACKET3_SET_CONTEXT_REG_END                     0x00029000
+#define PACKET3_SET_ALU_CONST                           0x6A
+/* alu const buffers only; no reg file */
+#define PACKET3_SET_BOOL_CONST                          0x6B
+#define         PACKET3_SET_BOOL_CONST_START                    0x0003a500
+#define         PACKET3_SET_BOOL_CONST_END                      0x0003a518
+#define PACKET3_SET_LOOP_CONST                          0x6C
+#define         PACKET3_SET_LOOP_CONST_START                    0x0003a200
+#define         PACKET3_SET_LOOP_CONST_END                      0x0003a500
+#define PACKET3_SET_RESOURCE                            0x6D
+#define         PACKET3_SET_RESOURCE_START                      0x00030000
+#define         PACKET3_SET_RESOURCE_END                        0x00038000
+#define PACKET3_SET_SAMPLER                             0x6E
+#define         PACKET3_SET_SAMPLER_START                       0x0003c000
+#define         PACKET3_SET_SAMPLER_END                         0x0003c600
+#define PACKET3_SET_CTL_CONST                           0x6F
+#define         PACKET3_SET_CTL_CONST_START                     0x0003cff0
+#define         PACKET3_SET_CTL_CONST_END                       0x0003ff0c
+#define PACKET3_SET_RESOURCE_OFFSET                     0x70
+#define PACKET3_SET_ALU_CONST_VS                        0x71
+#define PACKET3_SET_ALU_CONST_DI                        0x72
+#define PACKET3_SET_CONTEXT_REG_INDIRECT                0x73
+#define PACKET3_SET_RESOURCE_INDIRECT                   0x74
+#define PACKET3_SET_APPEND_CNT                          0x75
+
+#define SQ_RESOURCE_CONSTANT_WORD7_0                            0x3001c
+#define         S__SQ_CONSTANT_TYPE(x)                  (((x) & 3) << 30)
+#define         G__SQ_CONSTANT_TYPE(x)                  (((x) >> 30) & 3)
+#define                 SQ_TEX_VTX_INVALID_TEXTURE                      0x0
+#define                 SQ_TEX_VTX_INVALID_BUFFER                       0x1
+#define                 SQ_TEX_VTX_VALID_TEXTURE                        0x2
+#define                 SQ_TEX_VTX_VALID_BUFFER                         0x3
+
+#define SQ_CONST_MEM_BASE                               0x8df8
+
+#define SQ_ESGS_RING_SIZE                               0x8c44
+#define SQ_GSVS_RING_SIZE                               0x8c4c
+#define SQ_ESTMP_RING_SIZE                              0x8c54
+#define SQ_GSTMP_RING_SIZE                              0x8c5c
+#define SQ_VSTMP_RING_SIZE                              0x8c64
+#define SQ_PSTMP_RING_SIZE                              0x8c6c
+#define SQ_LSTMP_RING_SIZE                              0x8e14
+#define SQ_HSTMP_RING_SIZE                              0x8e1c
+#define VGT_TF_RING_SIZE                                0x8988
+
+#define SQ_ESGS_RING_ITEMSIZE                           0x28900
+#define SQ_GSVS_RING_ITEMSIZE                           0x28904
+#define SQ_ESTMP_RING_ITEMSIZE                          0x28908
+#define SQ_GSTMP_RING_ITEMSIZE                          0x2890c
+#define SQ_VSTMP_RING_ITEMSIZE                          0x28910
+#define SQ_PSTMP_RING_ITEMSIZE                          0x28914
+#define SQ_LSTMP_RING_ITEMSIZE                          0x28830
+#define SQ_HSTMP_RING_ITEMSIZE                          0x28834
+
+#define SQ_GS_VERT_ITEMSIZE                             0x2891c
+#define SQ_GS_VERT_ITEMSIZE_1                           0x28920
+#define SQ_GS_VERT_ITEMSIZE_2                           0x28924
+#define SQ_GS_VERT_ITEMSIZE_3                           0x28928
+#define SQ_GSVS_RING_OFFSET_1                           0x2892c
+#define SQ_GSVS_RING_OFFSET_2                           0x28930
+#define SQ_GSVS_RING_OFFSET_3                           0x28934
+
+#define SQ_ALU_CONST_CACHE_PS_0                         0x28940
+#define SQ_ALU_CONST_CACHE_PS_1                         0x28944
+#define SQ_ALU_CONST_CACHE_PS_2                         0x28948
+#define SQ_ALU_CONST_CACHE_PS_3                         0x2894c
+#define SQ_ALU_CONST_CACHE_PS_4                         0x28950
+#define SQ_ALU_CONST_CACHE_PS_5                         0x28954
+#define SQ_ALU_CONST_CACHE_PS_6                         0x28958
+#define SQ_ALU_CONST_CACHE_PS_7                         0x2895c
+#define SQ_ALU_CONST_CACHE_PS_8                         0x28960
+#define SQ_ALU_CONST_CACHE_PS_9                         0x28964
+#define SQ_ALU_CONST_CACHE_PS_10                        0x28968
+#define SQ_ALU_CONST_CACHE_PS_11                        0x2896c
+#define SQ_ALU_CONST_CACHE_PS_12                        0x28970
+#define SQ_ALU_CONST_CACHE_PS_13                        0x28974
+#define SQ_ALU_CONST_CACHE_PS_14                        0x28978
+#define SQ_ALU_CONST_CACHE_PS_15                        0x2897c
+#define SQ_ALU_CONST_CACHE_VS_0                         0x28980
+#define SQ_ALU_CONST_CACHE_VS_1                         0x28984
+#define SQ_ALU_CONST_CACHE_VS_2                         0x28988
+#define SQ_ALU_CONST_CACHE_VS_3                         0x2898c
+#define SQ_ALU_CONST_CACHE_VS_4                         0x28990
+#define SQ_ALU_CONST_CACHE_VS_5                         0x28994
+#define SQ_ALU_CONST_CACHE_VS_6                         0x28998
+#define SQ_ALU_CONST_CACHE_VS_7                         0x2899c
+#define SQ_ALU_CONST_CACHE_VS_8                         0x289a0
+#define SQ_ALU_CONST_CACHE_VS_9                         0x289a4
+#define SQ_ALU_CONST_CACHE_VS_10                        0x289a8
+#define SQ_ALU_CONST_CACHE_VS_11                        0x289ac
+#define SQ_ALU_CONST_CACHE_VS_12                        0x289b0
+#define SQ_ALU_CONST_CACHE_VS_13                        0x289b4
+#define SQ_ALU_CONST_CACHE_VS_14                        0x289b8
+#define SQ_ALU_CONST_CACHE_VS_15                        0x289bc
+#define SQ_ALU_CONST_CACHE_GS_0                         0x289c0
+#define SQ_ALU_CONST_CACHE_GS_1                         0x289c4
+#define SQ_ALU_CONST_CACHE_GS_2                         0x289c8
+#define SQ_ALU_CONST_CACHE_GS_3                         0x289cc
+#define SQ_ALU_CONST_CACHE_GS_4                         0x289d0
+#define SQ_ALU_CONST_CACHE_GS_5                         0x289d4
+#define SQ_ALU_CONST_CACHE_GS_6                         0x289d8
+#define SQ_ALU_CONST_CACHE_GS_7                         0x289dc
+#define SQ_ALU_CONST_CACHE_GS_8                         0x289e0
+#define SQ_ALU_CONST_CACHE_GS_9                         0x289e4
+#define SQ_ALU_CONST_CACHE_GS_10                        0x289e8
+#define SQ_ALU_CONST_CACHE_GS_11                        0x289ec
+#define SQ_ALU_CONST_CACHE_GS_12                        0x289f0
+#define SQ_ALU_CONST_CACHE_GS_13                        0x289f4
+#define SQ_ALU_CONST_CACHE_GS_14                        0x289f8
+#define SQ_ALU_CONST_CACHE_GS_15                        0x289fc
+#define SQ_ALU_CONST_CACHE_HS_0                         0x28f00
+#define SQ_ALU_CONST_CACHE_HS_1                         0x28f04
+#define SQ_ALU_CONST_CACHE_HS_2                         0x28f08
+#define SQ_ALU_CONST_CACHE_HS_3                         0x28f0c
+#define SQ_ALU_CONST_CACHE_HS_4                         0x28f10
+#define SQ_ALU_CONST_CACHE_HS_5                         0x28f14
+#define SQ_ALU_CONST_CACHE_HS_6                         0x28f18
+#define SQ_ALU_CONST_CACHE_HS_7                         0x28f1c
+#define SQ_ALU_CONST_CACHE_HS_8                         0x28f20
+#define SQ_ALU_CONST_CACHE_HS_9                         0x28f24
+#define SQ_ALU_CONST_CACHE_HS_10                        0x28f28
+#define SQ_ALU_CONST_CACHE_HS_11                        0x28f2c
+#define SQ_ALU_CONST_CACHE_HS_12                        0x28f30
+#define SQ_ALU_CONST_CACHE_HS_13                        0x28f34
+#define SQ_ALU_CONST_CACHE_HS_14                        0x28f38
+#define SQ_ALU_CONST_CACHE_HS_15                        0x28f3c
+#define SQ_ALU_CONST_CACHE_LS_0                         0x28f40
+#define SQ_ALU_CONST_CACHE_LS_1                         0x28f44
+#define SQ_ALU_CONST_CACHE_LS_2                         0x28f48
+#define SQ_ALU_CONST_CACHE_LS_3                         0x28f4c
+#define SQ_ALU_CONST_CACHE_LS_4                         0x28f50
+#define SQ_ALU_CONST_CACHE_LS_5                         0x28f54
+#define SQ_ALU_CONST_CACHE_LS_6                         0x28f58
+#define SQ_ALU_CONST_CACHE_LS_7                         0x28f5c
+#define SQ_ALU_CONST_CACHE_LS_8                         0x28f60
+#define SQ_ALU_CONST_CACHE_LS_9                         0x28f64
+#define SQ_ALU_CONST_CACHE_LS_10                        0x28f68
+#define SQ_ALU_CONST_CACHE_LS_11                        0x28f6c
+#define SQ_ALU_CONST_CACHE_LS_12                        0x28f70
+#define SQ_ALU_CONST_CACHE_LS_13                        0x28f74
+#define SQ_ALU_CONST_CACHE_LS_14                        0x28f78
+#define SQ_ALU_CONST_CACHE_LS_15                        0x28f7c
+
+#define DB_DEPTH_CONTROL                                0x28800
+#define DB_DEPTH_VIEW                                   0x28008
+#define DB_HTILE_DATA_BASE                              0x28014
+#define DB_Z_INFO                                       0x28040
+#       define Z_ARRAY_MODE(x)                          ((x) << 4)
+#define DB_STENCIL_INFO                                 0x28044
+#define DB_Z_READ_BASE                                  0x28048
+#define DB_STENCIL_READ_BASE                            0x2804c
+#define DB_Z_WRITE_BASE                                 0x28050
+#define DB_STENCIL_WRITE_BASE                           0x28054
+#define DB_DEPTH_SIZE                                   0x28058
+
+#define SQ_PGM_START_PS                                 0x28840
+#define SQ_PGM_START_VS                                 0x2885c
+#define SQ_PGM_START_GS                                 0x28874
+#define SQ_PGM_START_ES                                 0x2888c
+#define SQ_PGM_START_FS                                 0x288a4
+#define SQ_PGM_START_HS                                 0x288b8
+#define SQ_PGM_START_LS                                 0x288d0
+
+#define VGT_STRMOUT_CONFIG                              0x28b94
+#define VGT_STRMOUT_BUFFER_CONFIG                       0x28b98
+
+#define CB_TARGET_MASK                                  0x28238
+#define CB_SHADER_MASK                                  0x2823c
+
+#define GDS_ADDR_BASE                                   0x28720
+
+#define CB_IMMED0_BASE                                  0x28b9c
+#define CB_IMMED1_BASE                                  0x28ba0
+#define CB_IMMED2_BASE                                  0x28ba4
+#define CB_IMMED3_BASE                                  0x28ba8
+#define CB_IMMED4_BASE                                  0x28bac
+#define CB_IMMED5_BASE                                  0x28bb0
+#define CB_IMMED6_BASE                                  0x28bb4
+#define CB_IMMED7_BASE                                  0x28bb8
+#define CB_IMMED8_BASE                                  0x28bbc
+#define CB_IMMED9_BASE                                  0x28bc0
+#define CB_IMMED10_BASE                                 0x28bc4
+#define CB_IMMED11_BASE                                 0x28bc8
+
+/* all 12 CB blocks have these regs */
+#define CB_COLOR0_BASE                                  0x28c60
+#define CB_COLOR0_PITCH                                 0x28c64
+#define CB_COLOR0_SLICE                                 0x28c68
+#define CB_COLOR0_VIEW                                  0x28c6c
+#define CB_COLOR0_INFO                                  0x28c70
+#       define CB_ARRAY_MODE(x)                         ((x) << 8)
+#       define ARRAY_LINEAR_GENERAL                     0
+#       define ARRAY_LINEAR_ALIGNED                     1
+#       define ARRAY_1D_TILED_THIN1                     2
+#       define ARRAY_2D_TILED_THIN1                     4
+#define CB_COLOR0_ATTRIB                                0x28c74
+#define CB_COLOR0_DIM                                   0x28c78
+/* only CB0-7 blocks have these regs */
+#define CB_COLOR0_CMASK                                 0x28c7c
+#define CB_COLOR0_CMASK_SLICE                           0x28c80
+#define CB_COLOR0_FMASK                                 0x28c84
+#define CB_COLOR0_FMASK_SLICE                           0x28c88
+#define CB_COLOR0_CLEAR_WORD0                           0x28c8c
+#define CB_COLOR0_CLEAR_WORD1                           0x28c90
+#define CB_COLOR0_CLEAR_WORD2                           0x28c94
+#define CB_COLOR0_CLEAR_WORD3                           0x28c98
+
+#define CB_COLOR1_BASE                                  0x28c9c
+#define CB_COLOR2_BASE                                  0x28cd8
+#define CB_COLOR3_BASE                                  0x28d14
+#define CB_COLOR4_BASE                                  0x28d50
+#define CB_COLOR5_BASE                                  0x28d8c
+#define CB_COLOR6_BASE                                  0x28dc8
+#define CB_COLOR7_BASE                                  0x28e04
+#define CB_COLOR8_BASE                                  0x28e40
+#define CB_COLOR9_BASE                                  0x28e5c
+#define CB_COLOR10_BASE                                 0x28e78
+#define CB_COLOR11_BASE                                 0x28e94
+
+#define CB_COLOR1_PITCH                                 0x28ca0
+#define CB_COLOR2_PITCH                                 0x28cdc
+#define CB_COLOR3_PITCH                                 0x28d18
+#define CB_COLOR4_PITCH                                 0x28d54
+#define CB_COLOR5_PITCH                                 0x28d90
+#define CB_COLOR6_PITCH                                 0x28dcc
+#define CB_COLOR7_PITCH                                 0x28e08
+#define CB_COLOR8_PITCH                                 0x28e44
+#define CB_COLOR9_PITCH                                 0x28e60
+#define CB_COLOR10_PITCH                                0x28e7c
+#define CB_COLOR11_PITCH                                0x28e98
+
+#define CB_COLOR1_SLICE                                 0x28ca4
+#define CB_COLOR2_SLICE                                 0x28ce0
+#define CB_COLOR3_SLICE                                 0x28d1c
+#define CB_COLOR4_SLICE                                 0x28d58
+#define CB_COLOR5_SLICE                                 0x28d94
+#define CB_COLOR6_SLICE                                 0x28dd0
+#define CB_COLOR7_SLICE                                 0x28e0c
+#define CB_COLOR8_SLICE                                 0x28e48
+#define CB_COLOR9_SLICE                                 0x28e64
+#define CB_COLOR10_SLICE                                0x28e80
+#define CB_COLOR11_SLICE                                0x28e9c
+
+#define CB_COLOR1_VIEW                                  0x28ca8
+#define CB_COLOR2_VIEW                                  0x28ce4
+#define CB_COLOR3_VIEW                                  0x28d20
+#define CB_COLOR4_VIEW                                  0x28d5c
+#define CB_COLOR5_VIEW                                  0x28d98
+#define CB_COLOR6_VIEW                                  0x28dd4
+#define CB_COLOR7_VIEW                                  0x28e10
+#define CB_COLOR8_VIEW                                  0x28e4c
+#define CB_COLOR9_VIEW                                  0x28e68
+#define CB_COLOR10_VIEW                                 0x28e84
+#define CB_COLOR11_VIEW                                 0x28ea0
+
+#define CB_COLOR1_INFO                                  0x28cac
+#define CB_COLOR2_INFO                                  0x28ce8
+#define CB_COLOR3_INFO                                  0x28d24
+#define CB_COLOR4_INFO                                  0x28d60
+#define CB_COLOR5_INFO                                  0x28d9c
+#define CB_COLOR6_INFO                                  0x28dd8
+#define CB_COLOR7_INFO                                  0x28e14
+#define CB_COLOR8_INFO                                  0x28e50
+#define CB_COLOR9_INFO                                  0x28e6c
+#define CB_COLOR10_INFO                                 0x28e88
+#define CB_COLOR11_INFO                                 0x28ea4
+
+#define CB_COLOR1_ATTRIB                                0x28cb0
+#define CB_COLOR2_ATTRIB                                0x28cec
+#define CB_COLOR3_ATTRIB                                0x28d28
+#define CB_COLOR4_ATTRIB                                0x28d64
+#define CB_COLOR5_ATTRIB                                0x28da0
+#define CB_COLOR6_ATTRIB                                0x28ddc
+#define CB_COLOR7_ATTRIB                                0x28e18
+#define CB_COLOR8_ATTRIB                                0x28e54
+#define CB_COLOR9_ATTRIB                                0x28e70
+#define CB_COLOR10_ATTRIB                               0x28e8c
+#define CB_COLOR11_ATTRIB                               0x28ea8
+
+#define CB_COLOR1_DIM                                   0x28cb4
+#define CB_COLOR2_DIM                                   0x28cf0
+#define CB_COLOR3_DIM                                   0x28d2c
+#define CB_COLOR4_DIM                                   0x28d68
+#define CB_COLOR5_DIM                                   0x28da4
+#define CB_COLOR6_DIM                                   0x28de0
+#define CB_COLOR7_DIM                                   0x28e1c
+#define CB_COLOR8_DIM                                   0x28e58
+#define CB_COLOR9_DIM                                   0x28e74
+#define CB_COLOR10_DIM                                  0x28e90
+#define CB_COLOR11_DIM                                  0x28eac
+
+#define CB_COLOR1_CMASK                                 0x28cb8
+#define CB_COLOR2_CMASK                                 0x28cf4
+#define CB_COLOR3_CMASK                                 0x28d30
+#define CB_COLOR4_CMASK                                 0x28d6c
+#define CB_COLOR5_CMASK                                 0x28da8
+#define CB_COLOR6_CMASK                                 0x28de4
+#define CB_COLOR7_CMASK                                 0x28e20
+
+#define CB_COLOR1_CMASK_SLICE                           0x28cbc
+#define CB_COLOR2_CMASK_SLICE                           0x28cf8
+#define CB_COLOR3_CMASK_SLICE                           0x28d34
+#define CB_COLOR4_CMASK_SLICE                           0x28d70
+#define CB_COLOR5_CMASK_SLICE                           0x28dac
+#define CB_COLOR6_CMASK_SLICE                           0x28de8
+#define CB_COLOR7_CMASK_SLICE                           0x28e24
+
+#define CB_COLOR1_FMASK                                 0x28cc0
+#define CB_COLOR2_FMASK                                 0x28cfc
+#define CB_COLOR3_FMASK                                 0x28d38
+#define CB_COLOR4_FMASK                                 0x28d74
+#define CB_COLOR5_FMASK                                 0x28db0
+#define CB_COLOR6_FMASK                                 0x28dec
+#define CB_COLOR7_FMASK                                 0x28e28
+
+#define CB_COLOR1_FMASK_SLICE                           0x28cc4
+#define CB_COLOR2_FMASK_SLICE                           0x28d00
+#define CB_COLOR3_FMASK_SLICE                           0x28d3c
+#define CB_COLOR4_FMASK_SLICE                           0x28d78
+#define CB_COLOR5_FMASK_SLICE                           0x28db4
+#define CB_COLOR6_FMASK_SLICE                           0x28df0
+#define CB_COLOR7_FMASK_SLICE                           0x28e2c
+
+#define CB_COLOR1_CLEAR_WORD0                           0x28cc8
+#define CB_COLOR2_CLEAR_WORD0                           0x28d04
+#define CB_COLOR3_CLEAR_WORD0                           0x28d40
+#define CB_COLOR4_CLEAR_WORD0                           0x28d7c
+#define CB_COLOR5_CLEAR_WORD0                           0x28db8
+#define CB_COLOR6_CLEAR_WORD0                           0x28df4
+#define CB_COLOR7_CLEAR_WORD0                           0x28e30
+
+#define CB_COLOR1_CLEAR_WORD1                           0x28ccc
+#define CB_COLOR2_CLEAR_WORD1                           0x28d08
+#define CB_COLOR3_CLEAR_WORD1                           0x28d44
+#define CB_COLOR4_CLEAR_WORD1                           0x28d80
+#define CB_COLOR5_CLEAR_WORD1                           0x28dbc
+#define CB_COLOR6_CLEAR_WORD1                           0x28df8
+#define CB_COLOR7_CLEAR_WORD1                           0x28e34
+
+#define CB_COLOR1_CLEAR_WORD2                           0x28cd0
+#define CB_COLOR2_CLEAR_WORD2                           0x28d0c
+#define CB_COLOR3_CLEAR_WORD2                           0x28d48
+#define CB_COLOR4_CLEAR_WORD2                           0x28d84
+#define CB_COLOR5_CLEAR_WORD2                           0x28dc0
+#define CB_COLOR6_CLEAR_WORD2                           0x28dfc
+#define CB_COLOR7_CLEAR_WORD2                           0x28e38
+
+#define CB_COLOR1_CLEAR_WORD3                           0x28cd4
+#define CB_COLOR2_CLEAR_WORD3                           0x28d10
+#define CB_COLOR3_CLEAR_WORD3                           0x28d4c
+#define CB_COLOR4_CLEAR_WORD3                           0x28d88
+#define CB_COLOR5_CLEAR_WORD3                           0x28dc4
+#define CB_COLOR6_CLEAR_WORD3                           0x28e00
+#define CB_COLOR7_CLEAR_WORD3                           0x28e3c
+
+#define SQ_TEX_RESOURCE_WORD0_0                         0x30000
+#define SQ_TEX_RESOURCE_WORD1_0                         0x30004
+#       define TEX_ARRAY_MODE(x)                        ((x) << 28)
+#define SQ_TEX_RESOURCE_WORD2_0                         0x30008
+#define SQ_TEX_RESOURCE_WORD3_0                         0x3000C
+#define SQ_TEX_RESOURCE_WORD4_0                         0x30010
+#define SQ_TEX_RESOURCE_WORD5_0                         0x30014
+#define SQ_TEX_RESOURCE_WORD6_0                         0x30018
+#define SQ_TEX_RESOURCE_WORD7_0                         0x3001c
+
+
 #endif
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index cc004b05d63e..cf89aa2eb28c 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -162,6 +162,11 @@ void r100_pm_init_profile(struct radeon_device *rdev)
         rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_ps_idx = 0;
         rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_off_cm_idx = 0;
         rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_cm_idx = 0;
+        /* mid sh */
+        rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_ps_idx = 0;
+        rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_ps_idx = 0;
+        rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_cm_idx = 0;
+        rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_cm_idx = 0;
         /* high sh */
         rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_off_ps_idx = 0;
         rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
@@ -172,6 +177,11 @@ void r100_pm_init_profile(struct radeon_device *rdev)
         rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
         rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_off_cm_idx = 0;
         rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_cm_idx = 0;
+        /* mid mh */
+        rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_ps_idx = 0;
+        rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
+        rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_cm_idx = 0;
+        rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_cm_idx = 0;
         /* high mh */
         rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_off_ps_idx = 0;
         rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
diff --git a/drivers/gpu/drm/radeon/r420.c b/drivers/gpu/drm/radeon/r420.c
index 4415a5ee5871..e6c89142bb4d 100644
--- a/drivers/gpu/drm/radeon/r420.c
+++ b/drivers/gpu/drm/radeon/r420.c
@@ -45,9 +45,14 @@ void r420_pm_init_profile(struct radeon_device *rdev)
         rdev->pm.profiles[PM_PROFILE_DEFAULT_IDX].dpms_on_cm_idx = 0;
         /* low sh */
         rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_off_ps_idx = 0;
-        rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_ps_idx = 1;
+        rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_ps_idx = 0;
         rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_off_cm_idx = 0;
         rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_cm_idx = 0;
+        /* mid sh */
+        rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_ps_idx = 0;
+        rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_ps_idx = 1;
+        rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_cm_idx = 0;
+        rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_cm_idx = 0;
         /* high sh */
         rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_off_ps_idx = 0;
         rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
@@ -58,6 +63,11 @@ void r420_pm_init_profile(struct radeon_device *rdev)
         rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
         rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_off_cm_idx = 0;
         rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_cm_idx = 0;
+        /* mid mh */
+        rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_ps_idx = 0;
+        rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
+        rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_cm_idx = 0;
+        rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_cm_idx = 0;
         /* high mh */
         rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_off_ps_idx = 0;
         rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index 44e96a2ae25a..0e91871f45be 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -291,6 +291,11 @@ void rs780_pm_init_profile(struct radeon_device *rdev)
                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_ps_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_off_cm_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_cm_idx = 0;
+                /* mid sh */
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_ps_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_ps_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_cm_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_cm_idx = 0;
                 /* high sh */
                 rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_off_ps_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_on_ps_idx = 1;
@@ -301,6 +306,11 @@ void rs780_pm_init_profile(struct radeon_device *rdev)
                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_ps_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_off_cm_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_cm_idx = 0;
+                /* mid mh */
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_ps_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_ps_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_cm_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_cm_idx = 0;
                 /* high mh */
                 rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_off_ps_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_on_ps_idx = 1;
@@ -317,6 +327,11 @@ void rs780_pm_init_profile(struct radeon_device *rdev)
                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_ps_idx = 1;
                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_off_cm_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_cm_idx = 0;
+                /* mid sh */
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_ps_idx = 1;
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_ps_idx = 1;
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_cm_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_cm_idx = 0;
                 /* high sh */
                 rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_off_ps_idx = 1;
                 rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_on_ps_idx = 2;
@@ -327,6 +342,11 @@ void rs780_pm_init_profile(struct radeon_device *rdev)
                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_ps_idx = 1;
                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_off_cm_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_cm_idx = 0;
+                /* mid mh */
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_ps_idx = 1;
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_ps_idx = 1;
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_cm_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_cm_idx = 0;
                 /* high mh */
                 rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_off_ps_idx = 1;
                 rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_on_ps_idx = 2;
@@ -343,6 +363,11 @@ void rs780_pm_init_profile(struct radeon_device *rdev)
                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_ps_idx = 2;
                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_off_cm_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_cm_idx = 0;
+                /* mid sh */
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_ps_idx = 2;
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_ps_idx = 2;
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_cm_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_cm_idx = 0;
                 /* high sh */
                 rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_off_ps_idx = 2;
                 rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_on_ps_idx = 3;
@@ -353,6 +378,11 @@ void rs780_pm_init_profile(struct radeon_device *rdev)
                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_ps_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_off_cm_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_cm_idx = 0;
+                /* mid mh */
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_ps_idx = 2;
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_ps_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_cm_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_cm_idx = 0;
                 /* high mh */
                 rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_off_ps_idx = 2;
                 rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_on_ps_idx = 3;
@@ -375,6 +405,11 @@ void r600_pm_init_profile(struct radeon_device *rdev)
                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_off_cm_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_cm_idx = 0;
+                /* mid sh */
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_ps_idx = rdev->pm.default_power_state_index;
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_cm_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_cm_idx = 0;
                 /* high sh */
                 rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_off_ps_idx = rdev->pm.default_power_state_index;
                 rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
@@ -385,6 +420,11 @@ void r600_pm_init_profile(struct radeon_device *rdev)
                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_off_cm_idx = 0;
                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_cm_idx = 0;
+                /* mid mh */
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_ps_idx = rdev->pm.default_power_state_index;
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_cm_idx = 0;
+                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_cm_idx = 0;
                 /* high mh */
                 rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_off_ps_idx = rdev->pm.default_power_state_index;
                 rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_on_ps_idx = rdev->pm.default_power_state_index;
@@ -401,7 +441,12 @@ void r600_pm_init_profile(struct radeon_device *rdev)
                         rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_off_ps_idx = 1;
                         rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_ps_idx = 1;
                         rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_off_cm_idx = 0;
-                        rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_cm_idx = 1;
+                        rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_cm_idx = 0;
+                        /* mid sh */
+                        rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_ps_idx = 1;
+                        rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_ps_idx = 1;
+                        rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_cm_idx = 0;
+                        rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_cm_idx = 1;
                         /* high sh */
                         rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_off_ps_idx = 1;
                         rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_on_ps_idx = 1;
@@ -411,7 +456,12 @@ void r600_pm_init_profile(struct radeon_device *rdev)
                         rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_off_ps_idx = 2;
                         rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_ps_idx = 2;
                         rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_off_cm_idx = 0;
-                        rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_cm_idx = 1;
+                        rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_cm_idx = 0;
+                        /* low mh */
+                        rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_ps_idx = 2;
+                        rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_ps_idx = 2;
+                        rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_cm_idx = 0;
+                        rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_cm_idx = 1;
                         /* high mh */
                         rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_off_ps_idx = 2;
                         rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_on_ps_idx = 2;
@@ -430,14 +480,30 @@ void r600_pm_init_profile(struct radeon_device *rdev)
                                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_ps_idx =
                                         r600_pm_get_type_index(rdev, POWER_STATE_TYPE_BATTERY, 0);
                                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_off_cm_idx = 0;
-                                rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_cm_idx = 1;
+                                rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_cm_idx = 0;
                         } else {
                                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_off_ps_idx =
                                         r600_pm_get_type_index(rdev, POWER_STATE_TYPE_PERFORMANCE, 0);
                                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_ps_idx =
                                         r600_pm_get_type_index(rdev, POWER_STATE_TYPE_PERFORMANCE, 0);
                                 rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_off_cm_idx = 0;
-                                rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_cm_idx = 1;
+                                rdev->pm.profiles[PM_PROFILE_LOW_SH_IDX].dpms_on_cm_idx = 0;
+                        }
+                        /* mid sh */
+                        if (rdev->flags & RADEON_IS_MOBILITY) {
+                                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_ps_idx =
+                                        r600_pm_get_type_index(rdev, POWER_STATE_TYPE_BATTERY, 0);
+                                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_ps_idx =
+                                        r600_pm_get_type_index(rdev, POWER_STATE_TYPE_BATTERY, 0);
+                                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_cm_idx = 0;
+                                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_cm_idx = 1;
+                        } else {
+                                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_ps_idx =
+                                        r600_pm_get_type_index(rdev, POWER_STATE_TYPE_PERFORMANCE, 0);
+                                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_ps_idx =
+                                        r600_pm_get_type_index(rdev, POWER_STATE_TYPE_PERFORMANCE, 0);
+                                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_off_cm_idx = 0;
+                                rdev->pm.profiles[PM_PROFILE_MID_SH_IDX].dpms_on_cm_idx = 1;
                         }
                         /* high sh */
                         rdev->pm.profiles[PM_PROFILE_HIGH_SH_IDX].dpms_off_ps_idx =
@@ -453,14 +519,30 @@ void r600_pm_init_profile(struct radeon_device *rdev)
                                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_ps_idx =
                                         r600_pm_get_type_index(rdev, POWER_STATE_TYPE_BATTERY, 1);
                                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_off_cm_idx = 0;
-                                rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_cm_idx = 2;
+                                rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_cm_idx = 0;
                         } else {
                                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_off_ps_idx =
                                         r600_pm_get_type_index(rdev, POWER_STATE_TYPE_PERFORMANCE, 1);
                                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_ps_idx =
                                         r600_pm_get_type_index(rdev, POWER_STATE_TYPE_PERFORMANCE, 1);
                                 rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_off_cm_idx = 0;
-                                rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_cm_idx = 1;
+                                rdev->pm.profiles[PM_PROFILE_LOW_MH_IDX].dpms_on_cm_idx = 0;
+                        }
+                        /* mid mh */
+                        if (rdev->flags & RADEON_IS_MOBILITY) {
+                                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_ps_idx =
+                                        r600_pm_get_type_index(rdev, POWER_STATE_TYPE_BATTERY, 1);
+                                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_ps_idx =
+                                        r600_pm_get_type_index(rdev, POWER_STATE_TYPE_BATTERY, 1);
+                                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_cm_idx = 0;
+                                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_cm_idx = 1;
+                        } else {
+                                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_ps_idx =
+                                        r600_pm_get_type_index(rdev, POWER_STATE_TYPE_PERFORMANCE, 1);
+                                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_ps_idx =
+                                        r600_pm_get_type_index(rdev, POWER_STATE_TYPE_PERFORMANCE, 1);
+                                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_off_cm_idx = 0;
+                                rdev->pm.profiles[PM_PROFILE_MID_MH_IDX].dpms_on_cm_idx = 1;
                         }
                         /* high mh */
                         rdev->pm.profiles[PM_PROFILE_HIGH_MH_IDX].dpms_off_ps_idx =
@@ -475,7 +557,18 @@ void r600_pm_init_profile(struct radeon_device *rdev)
 
 void r600_pm_misc(struct radeon_device *rdev)
 {
+        int req_ps_idx = rdev->pm.requested_power_state_index;
+        int req_cm_idx = rdev->pm.requested_clock_mode_index;
+        struct radeon_power_state *ps = &rdev->pm.power_state[req_ps_idx];
+        struct radeon_voltage *voltage = &ps->clock_info[req_cm_idx].voltage;
 
+        if ((voltage->type == VOLTAGE_SW) && voltage->voltage) {
+                if (voltage->voltage != rdev->pm.current_vddc) {
+                        radeon_atom_set_voltage(rdev, voltage->voltage);
+                        rdev->pm.current_vddc = voltage->voltage;
+                        DRM_DEBUG("Setting: v: %d\n", voltage->voltage);
+                }
+        }
 }
 
 bool r600_gui_idle(struct radeon_device *rdev)
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index 66a37fb75839..8e1d44ca26ec 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -176,6 +176,7 @@ void radeon_pm_suspend(struct radeon_device *rdev);
 void radeon_pm_resume(struct radeon_device *rdev);
 void radeon_combios_get_power_modes(struct radeon_device *rdev);
 void radeon_atombios_get_power_modes(struct radeon_device *rdev);
+void radeon_atom_set_voltage(struct radeon_device *rdev, u16 level);
 
 /*
  * Fences.
@@ -576,6 +577,7 @@ typedef int (*radeon_packet3_check_t)(struct radeon_cs_parser *p,
  */
 int radeon_agp_init(struct radeon_device *rdev);
 void radeon_agp_resume(struct radeon_device *rdev);
+void radeon_agp_suspend(struct radeon_device *rdev);
 void radeon_agp_fini(struct radeon_device *rdev);
 
 
@@ -646,15 +648,18 @@ enum radeon_pm_profile_type {
         PM_PROFILE_DEFAULT,
         PM_PROFILE_AUTO,
         PM_PROFILE_LOW,
+        PM_PROFILE_MID,
         PM_PROFILE_HIGH,
 };
 
 #define PM_PROFILE_DEFAULT_IDX 0
 #define PM_PROFILE_LOW_SH_IDX  1
-#define PM_PROFILE_HIGH_SH_IDX 2
-#define PM_PROFILE_LOW_MH_IDX  3
-#define PM_PROFILE_HIGH_MH_IDX 4
-#define PM_PROFILE_MAX         5
+#define PM_PROFILE_MID_SH_IDX  2
+#define PM_PROFILE_HIGH_SH_IDX 3
+#define PM_PROFILE_LOW_MH_IDX  4
+#define PM_PROFILE_MID_MH_IDX  5
+#define PM_PROFILE_HIGH_MH_IDX 6
+#define PM_PROFILE_MAX         7
 
 struct radeon_pm_profile {
         int dpms_off_ps_idx;
@@ -743,6 +748,7 @@ struct radeon_pm {
         int                     default_power_state_index;
         u32                     current_sclk;
         u32                     current_mclk;
+        u32                     current_vddc;
         struct radeon_i2c_chan *i2c_bus;
         /* selected pm method */
         enum radeon_pm_method     pm_method;
diff --git a/drivers/gpu/drm/radeon/radeon_agp.c b/drivers/gpu/drm/radeon/radeon_agp.c
index 28e473f1f56f..f40dfb77f9b1 100644
--- a/drivers/gpu/drm/radeon/radeon_agp.c
+++ b/drivers/gpu/drm/radeon/radeon_agp.c
@@ -270,3 +270,8 @@ void radeon_agp_fini(struct radeon_device *rdev)
         }
 #endif
 }
+
+void radeon_agp_suspend(struct radeon_device *rdev)
+{
+        radeon_agp_fini(rdev);
+}
diff --git a/drivers/gpu/drm/radeon/radeon_asic.c b/drivers/gpu/drm/radeon/radeon_asic.c
index e57df08d4aeb..87f7e2cc52d4 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.c
+++ b/drivers/gpu/drm/radeon/radeon_asic.c
@@ -724,8 +724,8 @@ static struct radeon_asic evergreen_asic = {
         .irq_set = &evergreen_irq_set,
         .irq_process = &evergreen_irq_process,
         .get_vblank_counter = &evergreen_get_vblank_counter,
-        .fence_ring_emit = NULL,
-        .cs_parse = NULL,
+        .fence_ring_emit = &r600_fence_ring_emit,
+        .cs_parse = &evergreen_cs_parse,
         .copy_blit = NULL,
         .copy_dma = NULL,
         .copy = NULL,
diff --git a/drivers/gpu/drm/radeon/radeon_asic.h b/drivers/gpu/drm/radeon/radeon_asic.h
index 5c40a3dfaca2..c0bbaa64157a 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.h
+++ b/drivers/gpu/drm/radeon/radeon_asic.h
@@ -314,6 +314,7 @@ void evergreen_hpd_set_polarity(struct radeon_device *rdev,
 u32 evergreen_get_vblank_counter(struct radeon_device *rdev, int crtc);
 int evergreen_irq_set(struct radeon_device *rdev);
 int evergreen_irq_process(struct radeon_device *rdev);
+extern int evergreen_cs_parse(struct radeon_cs_parser *p);
 extern void evergreen_pm_misc(struct radeon_device *rdev);
 extern void evergreen_pm_prepare(struct radeon_device *rdev);
 extern void evergreen_pm_finish(struct radeon_device *rdev);
diff --git a/drivers/gpu/drm/radeon/radeon_atombios.c b/drivers/gpu/drm/radeon/radeon_atombios.c
index 6e733fdc3349..99bd8a9c56b3 100644
--- a/drivers/gpu/drm/radeon/radeon_atombios.c
+++ b/drivers/gpu/drm/radeon/radeon_atombios.c
@@ -680,11 +680,19 @@ bool radeon_get_atom_connector_info_from_supported_devices_table(struct
         uint8_t dac;
         union atom_supported_devices *supported_devices;
         int i, j, max_device;
-        struct bios_connector bios_connectors[ATOM_MAX_SUPPORTED_DEVICE];
+        struct bios_connector *bios_connectors;
+        size_t bc_size = sizeof(*bios_connectors) * ATOM_MAX_SUPPORTED_DEVICE;
 
-        if (!atom_parse_data_header(ctx, index, &size, &frev, &crev, &data_offset))
+        bios_connectors = kzalloc(bc_size, GFP_KERNEL);
+        if (!bios_connectors)
                 return false;
 
+        if (!atom_parse_data_header(ctx, index, &size, &frev, &crev,
+                                    &data_offset)) {
+                kfree(bios_connectors);
+                return false;
+        }
+
         supported_devices =
             (union atom_supported_devices *)(ctx->bios + data_offset);
 
@@ -851,6 +859,7 @@ bool radeon_get_atom_connector_info_from_supported_devices_table(struct
 
         radeon_link_encoder_connector(dev);
 
+        kfree(bios_connectors);
         return true;
 }
 
@@ -1529,7 +1538,8 @@ void radeon_atombios_get_power_modes(struct radeon_device *rdev)
                                         rdev->pm.power_state[state_index].pcie_lanes =
                                                 power_info->info.asPowerPlayInfo[i].ucNumPciELanes;
                                         misc = le32_to_cpu(power_info->info.asPowerPlayInfo[i].ulMiscInfo);
-                                        if (misc & ATOM_PM_MISCINFO_VOLTAGE_DROP_SUPPORT) {
+                                        if ((misc & ATOM_PM_MISCINFO_VOLTAGE_DROP_SUPPORT) ||
+                                            (misc & ATOM_PM_MISCINFO_VOLTAGE_DROP_ACTIVE_HIGH)) {
                                                 rdev->pm.power_state[state_index].clock_info[0].voltage.type =
                                                         VOLTAGE_GPIO;
                                                 rdev->pm.power_state[state_index].clock_info[0].voltage.gpio =
@@ -1596,7 +1606,8 @@ void radeon_atombios_get_power_modes(struct radeon_device *rdev)
                                                 power_info->info_2.asPowerPlayInfo[i].ucNumPciELanes;
                                         misc = le32_to_cpu(power_info->info_2.asPowerPlayInfo[i].ulMiscInfo);
                                         misc2 = le32_to_cpu(power_info->info_2.asPowerPlayInfo[i].ulMiscInfo2);
-                                        if (misc & ATOM_PM_MISCINFO_VOLTAGE_DROP_SUPPORT) {
+                                        if ((misc & ATOM_PM_MISCINFO_VOLTAGE_DROP_SUPPORT) ||
+                                            (misc & ATOM_PM_MISCINFO_VOLTAGE_DROP_ACTIVE_HIGH)) {
                                                 rdev->pm.power_state[state_index].clock_info[0].voltage.type =
                                                         VOLTAGE_GPIO;
                                                 rdev->pm.power_state[state_index].clock_info[0].voltage.gpio =
@@ -1670,7 +1681,8 @@ void radeon_atombios_get_power_modes(struct radeon_device *rdev)
                                                 power_info->info_3.asPowerPlayInfo[i].ucNumPciELanes;
                                         misc = le32_to_cpu(power_info->info_3.asPowerPlayInfo[i].ulMiscInfo);
                                         misc2 = le32_to_cpu(power_info->info_3.asPowerPlayInfo[i].ulMiscInfo2);
-                                        if (misc & ATOM_PM_MISCINFO_VOLTAGE_DROP_SUPPORT) {
+                                        if ((misc & ATOM_PM_MISCINFO_VOLTAGE_DROP_SUPPORT) ||
+                                            (misc & ATOM_PM_MISCINFO_VOLTAGE_DROP_ACTIVE_HIGH)) {
                                                 rdev->pm.power_state[state_index].clock_info[0].voltage.type =
                                                         VOLTAGE_GPIO;
                                                 rdev->pm.power_state[state_index].clock_info[0].voltage.gpio =
@@ -1746,9 +1758,22 @@ void radeon_atombios_get_power_modes(struct radeon_device *rdev)
                                 rdev->pm.power_state[state_index].misc2 = 0;
                         }
                 } else {
+                        int fw_index = GetIndexIntoMasterTable(DATA, FirmwareInfo);
+                        uint8_t fw_frev, fw_crev;
+                        uint16_t fw_data_offset, vddc = 0;
+                        union firmware_info *firmware_info;
+                        ATOM_PPLIB_THERMALCONTROLLER *controller = &power_info->info_4.sThermalController;
+
+                        if (atom_parse_data_header(mode_info->atom_context, fw_index, NULL,
+                                                   &fw_frev, &fw_crev, &fw_data_offset)) {
+                                firmware_info =
+                                        (union firmware_info *)(mode_info->atom_context->bios +
+                                                                fw_data_offset);
+                                vddc = firmware_info->info_14.usBootUpVDDCVoltage;
+                        }
+
                         /* add the i2c bus for thermal/fan chip */
                         /* no support for internal controller yet */
-                        ATOM_PPLIB_THERMALCONTROLLER *controller = &power_info->info_4.sThermalController;
                         if (controller->ucType > 0) {
                                 if ((controller->ucType == ATOM_PP_THERMALCONTROLLER_RV6xx) ||
                                     (controller->ucType == ATOM_PP_THERMALCONTROLLER_RV770) ||
@@ -1808,10 +1833,7 @@ void radeon_atombios_get_power_modes(struct radeon_device *rdev)
                                                 /* skip invalid modes */
                                                 if (rdev->pm.power_state[state_index].clock_info[mode_index].sclk == 0)
                                                         continue;
-                                                rdev->pm.power_state[state_index].clock_info[mode_index].voltage.type =
-                                                        VOLTAGE_SW;
-                                                rdev->pm.power_state[state_index].clock_info[mode_index].voltage.voltage =
-                                                        clock_info->usVDDC;
+                                                /* voltage works differently on IGPs */
                                                 mode_index++;
                                         } else if (ASIC_IS_DCE4(rdev)) {
                                                 struct _ATOM_PPLIB_EVERGREEN_CLOCK_INFO *clock_info =
@@ -1895,6 +1917,16 @@ void radeon_atombios_get_power_modes(struct radeon_device *rdev)
                                                 rdev->pm.default_power_state_index = state_index;
                                                 rdev->pm.power_state[state_index].default_clock_mode =
                                                         &rdev->pm.power_state[state_index].clock_info[mode_index - 1];
+                                                /* patch the table values with the default slck/mclk from firmware info */
+                                                for (j = 0; j < mode_index; j++) {
+                                                        rdev->pm.power_state[state_index].clock_info[j].mclk =
+                                                                rdev->clock.default_mclk;
+                                                        rdev->pm.power_state[state_index].clock_info[j].sclk =
+                                                                rdev->clock.default_sclk;
+                                                        if (vddc)
+                                                                rdev->pm.power_state[state_index].clock_info[j].voltage.voltage =
+                                                                        vddc;
+                                                }
                                         }
                                         state_index++;
                                 }
@@ -1934,6 +1966,7 @@ void radeon_atombios_get_power_modes(struct radeon_device *rdev)
 
         rdev->pm.current_power_state_index = rdev->pm.default_power_state_index;
         rdev->pm.current_clock_mode_index = 0;
+        rdev->pm.current_vddc = rdev->pm.power_state[rdev->pm.default_power_state_index].clock_info[0].voltage.voltage;
 }
 
 void radeon_atom_set_clock_gating(struct radeon_device *rdev, int enable)
@@ -1989,6 +2022,42 @@ void radeon_atom_set_memory_clock(struct radeon_device *rdev,
         atom_execute_table(rdev->mode_info.atom_context, index, (uint32_t *)&args);
 }
 
+union set_voltage {
+        struct _SET_VOLTAGE_PS_ALLOCATION alloc;
+        struct _SET_VOLTAGE_PARAMETERS v1;
+        struct _SET_VOLTAGE_PARAMETERS_V2 v2;
+};
+
+void radeon_atom_set_voltage(struct radeon_device *rdev, u16 level)
+{
+        union set_voltage args;
+        int index = GetIndexIntoMasterTable(COMMAND, SetVoltage);
+        u8 frev, crev, volt_index = level;
+
+        if (!atom_parse_cmd_header(rdev->mode_info.atom_context, index, &frev, &crev))
+                return;
+
+        switch (crev) {
+        case 1:
+                args.v1.ucVoltageType = SET_VOLTAGE_TYPE_ASIC_VDDC;
+                args.v1.ucVoltageMode = SET_ASIC_VOLTAGE_MODE_ALL_SOURCE;
+                args.v1.ucVoltageIndex = volt_index;
+                break;
+        case 2:
+                args.v2.ucVoltageType = SET_VOLTAGE_TYPE_ASIC_VDDC;
+                args.v2.ucVoltageMode = SET_ASIC_VOLTAGE_MODE_SET_VOLTAGE;
+                args.v2.usVoltageLevel = cpu_to_le16(level);
+                break;
+        default:
+                DRM_ERROR("Unknown table version %d, %d\n", frev, crev);
+                return;
+        }
+
+        atom_execute_table(rdev->mode_info.atom_context, index, (uint32_t *)&args);
+}
+
+
+
 void radeon_atom_initialize_bios_scratch_regs(struct drm_device *dev)
 {
         struct radeon_device *rdev = dev->dev_private;
diff --git a/drivers/gpu/drm/radeon/radeon_combios.c b/drivers/gpu/drm/radeon/radeon_combios.c
index 7b5e10d3e9c9..1bee2f9e24a5 100644
--- a/drivers/gpu/drm/radeon/radeon_combios.c
+++ b/drivers/gpu/drm/radeon/radeon_combios.c
@@ -2026,6 +2026,7 @@ bool radeon_get_legacy_connector_info_from_bios(struct drm_device *dev)
                                         combios_setup_i2c_bus(rdev, RADEON_GPIO_CRT2_DDC);
                                 break;
                         default:
+                                ddc_i2c.valid = false;
                                 break;
                         }
 
@@ -2339,6 +2340,7 @@ bool radeon_get_legacy_connector_info_from_bios(struct drm_device *dev)
                         if (RBIOS8(tv_info + 6) == 'T') {
                                 if (radeon_apply_legacy_tv_quirks(dev)) {
                                         hpd.hpd = RADEON_HPD_NONE;
+                                        ddc_i2c.valid = false;
                                         radeon_add_legacy_encoder(dev,
                                                                   radeon_get_encoder_id
                                                                   (dev,
@@ -2454,7 +2456,12 @@ default_mode:
         rdev->pm.power_state[state_index].clock_info[0].mclk = rdev->clock.default_mclk;
         rdev->pm.power_state[state_index].clock_info[0].sclk = rdev->clock.default_sclk;
         rdev->pm.power_state[state_index].default_clock_mode = &rdev->pm.power_state[state_index].clock_info[0];
-        rdev->pm.power_state[state_index].clock_info[0].voltage.type = VOLTAGE_NONE;
+        if ((state_index > 0) &&
+            (rdev->pm.power_state[0].clock_info[0].voltage.type == VOLTAGE_GPIO))
+                rdev->pm.power_state[state_index].clock_info[0].voltage =
+                        rdev->pm.power_state[0].clock_info[0].voltage;
+        else
+                rdev->pm.power_state[state_index].clock_info[0].voltage.type = VOLTAGE_NONE;
         rdev->pm.power_state[state_index].pcie_lanes = 16;
         rdev->pm.power_state[state_index].flags = 0;
         rdev->pm.default_power_state_index = state_index;
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index a20b612ffe75..f10faed21567 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -546,8 +546,10 @@ static void radeon_switcheroo_set_state(struct pci_dev *pdev, enum vga_switchero
                 /* don't suspend or resume card normally */
                 rdev->powered_down = false;
                 radeon_resume_kms(dev);
+                drm_kms_helper_poll_enable(dev);
         } else {
                 printk(KERN_INFO "radeon: switched off\n");
+                drm_kms_helper_poll_disable(dev);
                 radeon_suspend_kms(dev, pmm);
                 /* don't suspend or resume card normally */
                 rdev->powered_down = true;
@@ -711,6 +713,7 @@ int radeon_suspend_kms(struct drm_device *dev, pm_message_t state)
 {
         struct radeon_device *rdev;
         struct drm_crtc *crtc;
+        struct drm_connector *connector;
         int r;
 
         if (dev == NULL || dev->dev_private == NULL) {
@@ -723,6 +726,12 @@ int radeon_suspend_kms(struct drm_device *dev, pm_message_t state)
 
         if (rdev->powered_down)
                 return 0;
+
+        /* turn off display hw */
+        list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
+                drm_helper_connector_dpms(connector, DRM_MODE_DPMS_OFF);
+        }
+
         /* unpin the front buffers */
         list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
                 struct radeon_framebuffer *rfb = to_radeon_framebuffer(crtc->fb);
@@ -754,6 +763,8 @@ int radeon_suspend_kms(struct drm_device *dev, pm_message_t state)
         /* evict remaining vram memory */
         radeon_bo_evict_vram(rdev);
 
+        radeon_agp_suspend(rdev);
+
         pci_save_state(dev->pdev);
         if (state.event == PM_EVENT_SUSPEND) {
                 /* Shut down the device */
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index 1006549d1570..8154cdf796e4 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -284,8 +284,7 @@ static const char *connector_names[15] = {
         "eDP",
 };
 
-static const char *hpd_names[7] = {
-        "NONE",
+static const char *hpd_names[6] = {
         "HPD1",
         "HPD2",
         "HPD3",
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index 902d1731a652..e166fe4d7c30 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -45,9 +45,10 @@
  * - 2.2.0 - add r6xx/r7xx const buffer support
  * - 2.3.0 - add MSPOS + 3D texture + r500 VAP regs
  * - 2.4.0 - add crtc id query
+ * - 2.5.0 - add get accel 2 to work around ddx breakage for evergreen
  */
 #define KMS_DRIVER_MAJOR        2
-#define KMS_DRIVER_MINOR        4
+#define KMS_DRIVER_MINOR        5
 #define KMS_DRIVER_PATCHLEVEL   0
 int radeon_driver_load_kms(struct drm_device *dev, unsigned long flags);
 int radeon_driver_unload_kms(struct drm_device *dev);
diff --git a/drivers/gpu/drm/radeon/radeon_fb.c b/drivers/gpu/drm/radeon/radeon_fb.c
index e192acfbf0cd..dc1634bb0c11 100644
--- a/drivers/gpu/drm/radeon/radeon_fb.c
+++ b/drivers/gpu/drm/radeon/radeon_fb.c
@@ -363,6 +363,7 @@ int radeon_fbdev_init(struct radeon_device *rdev)
 {
         struct radeon_fbdev *rfbdev;
         int bpp_sel = 32;
+        int ret;
 
         /* select 8 bpp console on RN50 or 16MB cards */
         if (ASIC_IS_RN50(rdev) || rdev->mc.real_vram_size <= (32*1024*1024))
@@ -376,9 +377,14 @@ int radeon_fbdev_init(struct radeon_device *rdev)
         rdev->mode_info.rfbdev = rfbdev;
         rfbdev->helper.funcs = &radeon_fb_helper_funcs;
 
-        drm_fb_helper_init(rdev->ddev, &rfbdev->helper,
-                           rdev->num_crtc,
-                           RADEONFB_CONN_LIMIT);
+        ret = drm_fb_helper_init(rdev->ddev, &rfbdev->helper,
+                                 rdev->num_crtc,
+                                 RADEONFB_CONN_LIMIT);
+        if (ret) {
+                kfree(rfbdev);
+                return ret;
+        }
+
         drm_fb_helper_single_add_all_connectors(&rfbdev->helper);
         drm_fb_helper_initial_config(&rfbdev->helper, bpp_sel);
         return 0;
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index 04068352ccd2..6a70c0dc7f92 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -118,7 +118,11 @@ int radeon_info_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
                 value = rdev->num_z_pipes;
                 break;
         case RADEON_INFO_ACCEL_WORKING:
-                value = rdev->accel_working;
+                /* xf86-video-ati 6.13.0 relies on this being false for evergreen */
+                if ((rdev->family >= CHIP_CEDAR) && (rdev->family <= CHIP_HEMLOCK))
+                        value = false;
+                else
+                        value = rdev->accel_working;
                 break;
         case RADEON_INFO_CRTC_FROM_ID:
                 for (i = 0, found = 0; i < rdev->num_crtc; i++) {
@@ -134,6 +138,9 @@ int radeon_info_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
                         return -EINVAL;
                 }
                 break;
+        case RADEON_INFO_ACCEL_WORKING2:
+                value = rdev->accel_working;
+                break;
         default:
                 DRM_DEBUG("Invalid request %d\n", info->request);
                 return -EINVAL;
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_encoders.c b/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
index 5a13b3eeef19..5b07b8848e09 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
@@ -1168,6 +1168,17 @@ static enum drm_connector_status radeon_legacy_tv_dac_detect(struct drm_encoder
         struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
         struct radeon_encoder_tv_dac *tv_dac = radeon_encoder->enc_priv;
         bool color = true;
+        struct drm_crtc *crtc;
+
+        /* find out if crtc2 is in use or if this encoder is using it */
+        list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+                struct radeon_crtc *radeon_crtc = to_radeon_crtc(crtc);
+                if ((radeon_crtc->crtc_id == 1) && crtc->enabled) {
+                        if (encoder->crtc != crtc) {
+                                return connector_status_disconnected;
+                        }
+                }
+        }
 
         if (connector->connector_type == DRM_MODE_CONNECTOR_SVIDEO ||
             connector->connector_type == DRM_MODE_CONNECTOR_Composite ||
diff --git a/drivers/gpu/drm/radeon/radeon_pm.c b/drivers/gpu/drm/radeon/radeon_pm.c
index a8d162c6f829..63f679a04b25 100644
--- a/drivers/gpu/drm/radeon/radeon_pm.c
+++ b/drivers/gpu/drm/radeon/radeon_pm.c
@@ -33,6 +33,14 @@
 #define RADEON_WAIT_VBLANK_TIMEOUT 200
 #define RADEON_WAIT_IDLE_TIMEOUT 200
 
+static const char *radeon_pm_state_type_name[5] = {
+        "Default",
+        "Powersave",
+        "Battery",
+        "Balanced",
+        "Performance",
+};
+
 static void radeon_dynpm_idle_work_handler(struct work_struct *work);
 static int radeon_debugfs_pm_init(struct radeon_device *rdev);
 static bool radeon_pm_in_vbl(struct radeon_device *rdev);
@@ -84,9 +92,9 @@ static void radeon_pm_update_profile(struct radeon_device *rdev)
                                 rdev->pm.profile_index = PM_PROFILE_HIGH_SH_IDX;
                 } else {
                         if (rdev->pm.active_crtc_count > 1)
-                                rdev->pm.profile_index = PM_PROFILE_LOW_MH_IDX;
+                                rdev->pm.profile_index = PM_PROFILE_MID_MH_IDX;
                         else
-                                rdev->pm.profile_index = PM_PROFILE_LOW_SH_IDX;
+                                rdev->pm.profile_index = PM_PROFILE_MID_SH_IDX;
                 }
                 break;
         case PM_PROFILE_LOW:
@@ -95,6 +103,12 @@ static void radeon_pm_update_profile(struct radeon_device *rdev)
                 else
                         rdev->pm.profile_index = PM_PROFILE_LOW_SH_IDX;
                 break;
+        case PM_PROFILE_MID:
+                if (rdev->pm.active_crtc_count > 1)
+                        rdev->pm.profile_index = PM_PROFILE_MID_MH_IDX;
+                else
+                        rdev->pm.profile_index = PM_PROFILE_MID_SH_IDX;
+                break;
         case PM_PROFILE_HIGH:
                 if (rdev->pm.active_crtc_count > 1)
                         rdev->pm.profile_index = PM_PROFILE_HIGH_MH_IDX;
@@ -127,15 +141,6 @@ static void radeon_unmap_vram_bos(struct radeon_device *rdev)
                 if (bo->tbo.mem.mem_type == TTM_PL_VRAM)
                         ttm_bo_unmap_virtual(&bo->tbo);
         }
-
-        if (rdev->gart.table.vram.robj)
-                ttm_bo_unmap_virtual(&rdev->gart.table.vram.robj->tbo);
-
-        if (rdev->stollen_vga_memory)
-                ttm_bo_unmap_virtual(&rdev->stollen_vga_memory->tbo);
-
-        if (rdev->r600_blit.shader_obj)
-                ttm_bo_unmap_virtual(&rdev->r600_blit.shader_obj->tbo);
 }
 
 static void radeon_sync_with_vblank(struct radeon_device *rdev)
@@ -151,6 +156,7 @@ static void radeon_sync_with_vblank(struct radeon_device *rdev)
 static void radeon_set_power_state(struct radeon_device *rdev)
 {
         u32 sclk, mclk;
+        bool misc_after = false;
 
         if ((rdev->pm.requested_clock_mode_index == rdev->pm.current_clock_mode_index) &&
             (rdev->pm.requested_power_state_index == rdev->pm.current_power_state_index))
@@ -167,55 +173,47 @@ static void radeon_set_power_state(struct radeon_device *rdev)
                 if (mclk > rdev->clock.default_mclk)
                         mclk = rdev->clock.default_mclk;
 
-                /* voltage, pcie lanes, etc.*/
-                radeon_pm_misc(rdev);
+                /* upvolt before raising clocks, downvolt after lowering clocks */
+                if (sclk < rdev->pm.current_sclk)
+                        misc_after = true;
 
-                if (rdev->pm.pm_method == PM_METHOD_DYNPM) {
-                        radeon_sync_with_vblank(rdev);
+                radeon_sync_with_vblank(rdev);
 
+                if (rdev->pm.pm_method == PM_METHOD_DYNPM) {
                         if (!radeon_pm_in_vbl(rdev))
                                 return;
+                }
 
-                        radeon_pm_prepare(rdev);
-                        /* set engine clock */
-                        if (sclk != rdev->pm.current_sclk) {
-                                radeon_pm_debug_check_in_vbl(rdev, false);
-                                radeon_set_engine_clock(rdev, sclk);
-                                radeon_pm_debug_check_in_vbl(rdev, true);
-                                rdev->pm.current_sclk = sclk;
-                                DRM_DEBUG("Setting: e: %d\n", sclk);
-                        }
+                radeon_pm_prepare(rdev);
 
-                        /* set memory clock */
-                        if (rdev->asic->set_memory_clock && (mclk != rdev->pm.current_mclk)) {
-                                radeon_pm_debug_check_in_vbl(rdev, false);
-                                radeon_set_memory_clock(rdev, mclk);
-                                radeon_pm_debug_check_in_vbl(rdev, true);
-                                rdev->pm.current_mclk = mclk;
-                                DRM_DEBUG("Setting: m: %d\n", mclk);
-                        }
-                        radeon_pm_finish(rdev);
-                } else {
-                        /* set engine clock */
-                        if (sclk != rdev->pm.current_sclk) {
-                                radeon_sync_with_vblank(rdev);
-                                radeon_pm_prepare(rdev);
-                                radeon_set_engine_clock(rdev, sclk);
-                                radeon_pm_finish(rdev);
-                                rdev->pm.current_sclk = sclk;
-                                DRM_DEBUG("Setting: e: %d\n", sclk);
-                        }
-                        /* set memory clock */
-                        if (rdev->asic->set_memory_clock && (mclk != rdev->pm.current_mclk)) {
-                                radeon_sync_with_vblank(rdev);
-                                radeon_pm_prepare(rdev);
-                                radeon_set_memory_clock(rdev, mclk);
-                                radeon_pm_finish(rdev);
-                                rdev->pm.current_mclk = mclk;
-                                DRM_DEBUG("Setting: m: %d\n", mclk);
-                        }
+                if (!misc_after)
+                        /* voltage, pcie lanes, etc.*/
+                        radeon_pm_misc(rdev);
+
+                /* set engine clock */
+                if (sclk != rdev->pm.current_sclk) {
+                        radeon_pm_debug_check_in_vbl(rdev, false);
+                        radeon_set_engine_clock(rdev, sclk);
+                        radeon_pm_debug_check_in_vbl(rdev, true);
+                        rdev->pm.current_sclk = sclk;
+                        DRM_DEBUG("Setting: e: %d\n", sclk);
                 }
 
+                /* set memory clock */
+                if (rdev->asic->set_memory_clock && (mclk != rdev->pm.current_mclk)) {
+                        radeon_pm_debug_check_in_vbl(rdev, false);
+                        radeon_set_memory_clock(rdev, mclk);
+                        radeon_pm_debug_check_in_vbl(rdev, true);
+                        rdev->pm.current_mclk = mclk;
+                        DRM_DEBUG("Setting: m: %d\n", mclk);
+                }
+
+                if (misc_after)
+                        /* voltage, pcie lanes, etc.*/
+                        radeon_pm_misc(rdev);
+
+                radeon_pm_finish(rdev);
+
                 rdev->pm.current_power_state_index = rdev->pm.requested_power_state_index;
                 rdev->pm.current_clock_mode_index = rdev->pm.requested_clock_mode_index;
         } else
@@ -288,6 +286,42 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
         mutex_unlock(&rdev->ddev->struct_mutex);
 }
 
+static void radeon_pm_print_states(struct radeon_device *rdev)
+{
+        int i, j;
+        struct radeon_power_state *power_state;
+        struct radeon_pm_clock_info *clock_info;
+
+        DRM_DEBUG("%d Power State(s)\n", rdev->pm.num_power_states);
+        for (i = 0; i < rdev->pm.num_power_states; i++) {
+                power_state = &rdev->pm.power_state[i];
+                DRM_DEBUG("State %d: %s\n", i,
+                        radeon_pm_state_type_name[power_state->type]);
+                if (i == rdev->pm.default_power_state_index)
+                        DRM_DEBUG("\tDefault");
+                if ((rdev->flags & RADEON_IS_PCIE) && !(rdev->flags & RADEON_IS_IGP))
+                        DRM_DEBUG("\t%d PCIE Lanes\n", power_state->pcie_lanes);
+                if (power_state->flags & RADEON_PM_STATE_SINGLE_DISPLAY_ONLY)
+                        DRM_DEBUG("\tSingle display only\n");
+                DRM_DEBUG("\t%d Clock Mode(s)\n", power_state->num_clock_modes);
+                for (j = 0; j < power_state->num_clock_modes; j++) {
+                        clock_info = &(power_state->clock_info[j]);
+                        if (rdev->flags & RADEON_IS_IGP)
+                                DRM_DEBUG("\t\t%d e: %d%s\n",
+                                        j,
+                                        clock_info->sclk * 10,
+                                        clock_info->flags & RADEON_PM_MODE_NO_DISPLAY ? "\tNo display only" : "");
+                        else
+                                DRM_DEBUG("\t\t%d e: %d\tm: %d\tv: %d%s\n",
+                                        j,
+                                        clock_info->sclk * 10,
+                                        clock_info->mclk * 10,
+                                        clock_info->voltage.voltage,
+                                        clock_info->flags & RADEON_PM_MODE_NO_DISPLAY ? "\tNo display only" : "");
+                }
+        }
+}
+
 static ssize_t radeon_get_pm_profile(struct device *dev,
                                      struct device_attribute *attr,
                                      char *buf)
@@ -318,6 +352,8 @@ static ssize_t radeon_set_pm_profile(struct device *dev,
                         rdev->pm.profile = PM_PROFILE_AUTO;
                 else if (strncmp("low", buf, strlen("low")) == 0)
                         rdev->pm.profile = PM_PROFILE_LOW;
+                else if (strncmp("mid", buf, strlen("mid")) == 0)
+                        rdev->pm.profile = PM_PROFILE_MID;
                 else if (strncmp("high", buf, strlen("high")) == 0)
                         rdev->pm.profile = PM_PROFILE_HIGH;
                 else {
@@ -384,15 +420,19 @@ void radeon_pm_suspend(struct radeon_device *rdev)
 {
         mutex_lock(&rdev->pm.mutex);
         cancel_delayed_work(&rdev->pm.dynpm_idle_work);
-        rdev->pm.current_power_state_index = -1;
-        rdev->pm.current_clock_mode_index = -1;
-        rdev->pm.current_sclk = 0;
-        rdev->pm.current_mclk = 0;
         mutex_unlock(&rdev->pm.mutex);
 }
 
 void radeon_pm_resume(struct radeon_device *rdev)
 {
+        /* asic init will reset the default power state */
+        mutex_lock(&rdev->pm.mutex);
+        rdev->pm.current_power_state_index = rdev->pm.default_power_state_index;
+        rdev->pm.current_clock_mode_index = 0;
+        rdev->pm.current_sclk = rdev->clock.default_sclk;
+        rdev->pm.current_mclk = rdev->clock.default_mclk;
+        rdev->pm.current_vddc = rdev->pm.power_state[rdev->pm.default_power_state_index].clock_info[0].voltage.voltage;
+        mutex_unlock(&rdev->pm.mutex);
         radeon_pm_compute_clocks(rdev);
 }
 
@@ -401,32 +441,24 @@ int radeon_pm_init(struct radeon_device *rdev)
         int ret;
         /* default to profile method */
         rdev->pm.pm_method = PM_METHOD_PROFILE;
+        rdev->pm.profile = PM_PROFILE_DEFAULT;
         rdev->pm.dynpm_state = DYNPM_STATE_DISABLED;
         rdev->pm.dynpm_planned_action = DYNPM_ACTION_NONE;
         rdev->pm.dynpm_can_upclock = true;
         rdev->pm.dynpm_can_downclock = true;
-        rdev->pm.current_sclk = 0;
-        rdev->pm.current_mclk = 0;
+        rdev->pm.current_sclk = rdev->clock.default_sclk;
+        rdev->pm.current_mclk = rdev->clock.default_mclk;
 
         if (rdev->bios) {
                 if (rdev->is_atom_bios)
                         radeon_atombios_get_power_modes(rdev);
                 else
                         radeon_combios_get_power_modes(rdev);
+                radeon_pm_print_states(rdev);
                 radeon_pm_init_profile(rdev);
-                rdev->pm.current_power_state_index = -1;
-                rdev->pm.current_clock_mode_index = -1;
         }
 
         if (rdev->pm.num_power_states > 1) {
-                if (rdev->pm.pm_method == PM_METHOD_PROFILE) {
-                        mutex_lock(&rdev->pm.mutex);
-                        rdev->pm.profile = PM_PROFILE_DEFAULT;
-                        radeon_pm_update_profile(rdev);
-                        radeon_pm_set_clocks(rdev);
-                        mutex_unlock(&rdev->pm.mutex);
-                }
-
                 /* where's the best place to put these? */
                 ret = device_create_file(rdev->dev, &dev_attr_power_profile);
                 if (ret)
@@ -712,6 +744,8 @@ static int radeon_debugfs_pm_info(struct seq_file *m, void *data)
         seq_printf(m, "default memory clock: %u0 kHz\n", rdev->clock.default_mclk);
         if (rdev->asic->get_memory_clock)
                 seq_printf(m, "current memory clock: %u0 kHz\n", radeon_get_memory_clock(rdev));
+        if (rdev->pm.current_vddc)
+                seq_printf(m, "voltage: %u mV\n", rdev->pm.current_vddc);
         if (rdev->asic->get_pcie_lanes)
                 seq_printf(m, "PCIE lanes: %d\n", radeon_get_pcie_lanes(rdev));
 
diff --git a/drivers/gpu/drm/radeon/radeon_state.c b/drivers/gpu/drm/radeon/radeon_state.c
index cc5316dcf580..b3ba44c0a818 100644
--- a/drivers/gpu/drm/radeon/radeon_state.c
+++ b/drivers/gpu/drm/radeon/radeon_state.c
@@ -900,9 +900,10 @@ static void radeon_cp_dispatch_clear(struct drm_device * dev,
                         flags |= RADEON_FRONT;
         }
         if (flags & (RADEON_DEPTH|RADEON_STENCIL)) {
-                if (!dev_priv->have_z_offset)
+                if (!dev_priv->have_z_offset) {
                         printk_once(KERN_ERR "radeon: illegal depth clear request. Buggy mesa detected - please update.\n");
-                flags &= ~(RADEON_DEPTH | RADEON_STENCIL);
+                        flags &= ~(RADEON_DEPTH | RADEON_STENCIL);
+                }
         }
 
         if (flags & (RADEON_FRONT | RADEON_BACK)) {
diff --git a/drivers/gpu/drm/radeon/reg_srcs/evergreen b/drivers/gpu/drm/radeon/reg_srcs/evergreen
new file mode 100644
index 000000000000..b5c757f68d3c
--- /dev/null
+++ b/drivers/gpu/drm/radeon/reg_srcs/evergreen
@@ -0,0 +1,611 @@
+evergreen 0x9400
+0x00008040 WAIT_UNTIL
+0x00008044 WAIT_UNTIL_POLL_CNTL
+0x00008048 WAIT_UNTIL_POLL_MASK
+0x0000804c WAIT_UNTIL_POLL_REFDATA
+0x000088B0 VGT_VTX_VECT_EJECT_REG
+0x000088C4 VGT_CACHE_INVALIDATION
+0x000088D4 VGT_GS_VERTEX_REUSE
+0x00008958 VGT_PRIMITIVE_TYPE
+0x0000895C VGT_INDEX_TYPE
+0x00008970 VGT_NUM_INDICES
+0x00008974 VGT_NUM_INSTANCES
+0x00008990 VGT_COMPUTE_DIM_X
+0x00008994 VGT_COMPUTE_DIM_Y
+0x00008998 VGT_COMPUTE_DIM_Z
+0x0000899C VGT_COMPUTE_START_X
+0x000089A0 VGT_COMPUTE_START_Y
+0x000089A4 VGT_COMPUTE_START_Z
+0x000089AC VGT_COMPUTE_THREAD_GOURP_SIZE
+0x00008A14 PA_CL_ENHANCE
+0x00008A60 PA_SC_LINE_STIPPLE_VALUE
+0x00008B10 PA_SC_LINE_STIPPLE_STATE
+0x00008BF0 PA_SC_ENHANCE
+0x00008D8C SQ_DYN_GPR_CNTL_PS_FLUSH_REQ
+0x00008C00 SQ_CONFIG
+0x00008C04 SQ_GPR_RESOURCE_MGMT_1
+0x00008C08 SQ_GPR_RESOURCE_MGMT_2
+0x00008C0C SQ_GPR_RESOURCE_MGMT_3
+0x00008C10 SQ_GLOBAL_GPR_RESOURCE_MGMT_1
+0x00008C14 SQ_GLOBAL_GPR_RESOURCE_MGMT_2
+0x00008C18 SQ_THREAD_RESOURCE_MGMT
+0x00008C1C SQ_THREAD_RESOURCE_MGMT_2
+0x00008C20 SQ_STACK_RESOURCE_MGMT_1
+0x00008C24 SQ_STACK_RESOURCE_MGMT_2
+0x00008C28 SQ_STACK_RESOURCE_MGMT_3
+0x00008DF8 SQ_CONST_MEM_BASE
+0x00008E48 SQ_EX_ALLOC_TABLE_SLOTS
+0x00009100 SPI_CONFIG_CNTL
+0x0000913C SPI_CONFIG_CNTL_1
+0x00009700 VC_CNTL
+0x00009714 VC_ENHANCE
+0x00009830 DB_DEBUG
+0x00009834 DB_DEBUG2
+0x00009838 DB_DEBUG3
+0x0000983C DB_DEBUG4
+0x00009854 DB_WATERMARKS
+0x0000A400 TD_PS_BORDER_COLOR_INDEX
+0x0000A404 TD_PS_BORDER_COLOR_RED
+0x0000A408 TD_PS_BORDER_COLOR_GREEN
+0x0000A40C TD_PS_BORDER_COLOR_BLUE
+0x0000A410 TD_PS_BORDER_COLOR_ALPHA
+0x0000A414 TD_VS_BORDER_COLOR_INDEX
+0x0000A418 TD_VS_BORDER_COLOR_RED
+0x0000A41C TD_VS_BORDER_COLOR_GREEN
+0x0000A420 TD_VS_BORDER_COLOR_BLUE
+0x0000A424 TD_VS_BORDER_COLOR_ALPHA
+0x0000A428 TD_GS_BORDER_COLOR_INDEX
+0x0000A42C TD_GS_BORDER_COLOR_RED
+0x0000A430 TD_GS_BORDER_COLOR_GREEN
+0x0000A434 TD_GS_BORDER_COLOR_BLUE
+0x0000A438 TD_GS_BORDER_COLOR_ALPHA
+0x0000A43C TD_HS_BORDER_COLOR_INDEX
+0x0000A440 TD_HS_BORDER_COLOR_RED
+0x0000A444 TD_HS_BORDER_COLOR_GREEN
+0x0000A448 TD_HS_BORDER_COLOR_BLUE
+0x0000A44C TD_HS_BORDER_COLOR_ALPHA
+0x0000A450 TD_LS_BORDER_COLOR_INDEX
+0x0000A454 TD_LS_BORDER_COLOR_RED
+0x0000A458 TD_LS_BORDER_COLOR_GREEN
+0x0000A45C TD_LS_BORDER_COLOR_BLUE
+0x0000A460 TD_LS_BORDER_COLOR_ALPHA
+0x0000A464 TD_CS_BORDER_COLOR_INDEX
+0x0000A468 TD_CS_BORDER_COLOR_RED
+0x0000A46C TD_CS_BORDER_COLOR_GREEN
+0x0000A470 TD_CS_BORDER_COLOR_BLUE
+0x0000A474 TD_CS_BORDER_COLOR_ALPHA
+0x00028000 DB_RENDER_CONTROL
+0x00028004 DB_COUNT_CONTROL
+0x0002800C DB_RENDER_OVERRIDE
+0x00028010 DB_RENDER_OVERRIDE2
+0x00028028 DB_STENCIL_CLEAR
+0x0002802C DB_DEPTH_CLEAR
+0x00028034 PA_SC_SCREEN_SCISSOR_BR
+0x00028030 PA_SC_SCREEN_SCISSOR_TL
+0x0002805C DB_DEPTH_SLICE
+0x00028140 SQ_ALU_CONST_BUFFER_SIZE_PS_0
+0x00028144 SQ_ALU_CONST_BUFFER_SIZE_PS_1
+0x00028148 SQ_ALU_CONST_BUFFER_SIZE_PS_2
+0x0002814C SQ_ALU_CONST_BUFFER_SIZE_PS_3
+0x00028150 SQ_ALU_CONST_BUFFER_SIZE_PS_4
+0x00028154 SQ_ALU_CONST_BUFFER_SIZE_PS_5
+0x00028158 SQ_ALU_CONST_BUFFER_SIZE_PS_6
+0x0002815C SQ_ALU_CONST_BUFFER_SIZE_PS_7
+0x00028160 SQ_ALU_CONST_BUFFER_SIZE_PS_8
+0x00028164 SQ_ALU_CONST_BUFFER_SIZE_PS_9
+0x00028168 SQ_ALU_CONST_BUFFER_SIZE_PS_10
+0x0002816C SQ_ALU_CONST_BUFFER_SIZE_PS_11
+0x00028170 SQ_ALU_CONST_BUFFER_SIZE_PS_12
+0x00028174 SQ_ALU_CONST_BUFFER_SIZE_PS_13
+0x00028178 SQ_ALU_CONST_BUFFER_SIZE_PS_14
+0x0002817C SQ_ALU_CONST_BUFFER_SIZE_PS_15
+0x00028180 SQ_ALU_CONST_BUFFER_SIZE_VS_0
+0x00028184 SQ_ALU_CONST_BUFFER_SIZE_VS_1
+0x00028188 SQ_ALU_CONST_BUFFER_SIZE_VS_2
+0x0002818C SQ_ALU_CONST_BUFFER_SIZE_VS_3
+0x00028190 SQ_ALU_CONST_BUFFER_SIZE_VS_4
+0x00028194 SQ_ALU_CONST_BUFFER_SIZE_VS_5
+0x00028198 SQ_ALU_CONST_BUFFER_SIZE_VS_6
+0x0002819C SQ_ALU_CONST_BUFFER_SIZE_VS_7
+0x000281A0 SQ_ALU_CONST_BUFFER_SIZE_VS_8
+0x000281A4 SQ_ALU_CONST_BUFFER_SIZE_VS_9
+0x000281A8 SQ_ALU_CONST_BUFFER_SIZE_VS_10
+0x000281AC SQ_ALU_CONST_BUFFER_SIZE_VS_11
+0x000281B0 SQ_ALU_CONST_BUFFER_SIZE_VS_12
+0x000281B4 SQ_ALU_CONST_BUFFER_SIZE_VS_13
+0x000281B8 SQ_ALU_CONST_BUFFER_SIZE_VS_14
+0x000281BC SQ_ALU_CONST_BUFFER_SIZE_VS_15
+0x000281C0 SQ_ALU_CONST_BUFFER_SIZE_GS_0
+0x000281C4 SQ_ALU_CONST_BUFFER_SIZE_GS_1
+0x000281C8 SQ_ALU_CONST_BUFFER_SIZE_GS_2
+0x000281CC SQ_ALU_CONST_BUFFER_SIZE_GS_3
+0x000281D0 SQ_ALU_CONST_BUFFER_SIZE_GS_4
+0x000281D4 SQ_ALU_CONST_BUFFER_SIZE_GS_5
+0x000281D8 SQ_ALU_CONST_BUFFER_SIZE_GS_6
+0x000281DC SQ_ALU_CONST_BUFFER_SIZE_GS_7
+0x000281E0 SQ_ALU_CONST_BUFFER_SIZE_GS_8
+0x000281E4 SQ_ALU_CONST_BUFFER_SIZE_GS_9
+0x000281E8 SQ_ALU_CONST_BUFFER_SIZE_GS_10
+0x000281EC SQ_ALU_CONST_BUFFER_SIZE_GS_11
+0x000281F0 SQ_ALU_CONST_BUFFER_SIZE_GS_12
+0x000281F4 SQ_ALU_CONST_BUFFER_SIZE_GS_13
+0x000281F8 SQ_ALU_CONST_BUFFER_SIZE_GS_14
+0x000281FC SQ_ALU_CONST_BUFFER_SIZE_GS_15
+0x00028200 PA_SC_WINDOW_OFFSET
+0x00028204 PA_SC_WINDOW_SCISSOR_TL
+0x00028208 PA_SC_WINDOW_SCISSOR_BR
+0x0002820C PA_SC_CLIPRECT_RULE
+0x00028210 PA_SC_CLIPRECT_0_TL
+0x00028214 PA_SC_CLIPRECT_0_BR
+0x00028218 PA_SC_CLIPRECT_1_TL
+0x0002821C PA_SC_CLIPRECT_1_BR
+0x00028220 PA_SC_CLIPRECT_2_TL
+0x00028224 PA_SC_CLIPRECT_2_BR
+0x00028228 PA_SC_CLIPRECT_3_TL
+0x0002822C PA_SC_CLIPRECT_3_BR
+0x00028230 PA_SC_EDGERULE
+0x00028234 PA_SU_HARDWARE_SCREEN_OFFSET
+0x00028240 PA_SC_GENERIC_SCISSOR_TL
+0x00028244 PA_SC_GENERIC_SCISSOR_BR
+0x00028250 PA_SC_VPORT_SCISSOR_0_TL
+0x00028254 PA_SC_VPORT_SCISSOR_0_BR
+0x00028258 PA_SC_VPORT_SCISSOR_1_TL
+0x0002825C PA_SC_VPORT_SCISSOR_1_BR
+0x00028260 PA_SC_VPORT_SCISSOR_2_TL
+0x00028264 PA_SC_VPORT_SCISSOR_2_BR
+0x00028268 PA_SC_VPORT_SCISSOR_3_TL
+0x0002826C PA_SC_VPORT_SCISSOR_3_BR
+0x00028270 PA_SC_VPORT_SCISSOR_4_TL
+0x00028274 PA_SC_VPORT_SCISSOR_4_BR
+0x00028278 PA_SC_VPORT_SCISSOR_5_TL
+0x0002827C PA_SC_VPORT_SCISSOR_5_BR
+0x00028280 PA_SC_VPORT_SCISSOR_6_TL
+0x00028284 PA_SC_VPORT_SCISSOR_6_BR
+0x00028288 PA_SC_VPORT_SCISSOR_7_TL
+0x0002828C PA_SC_VPORT_SCISSOR_7_BR
+0x00028290 PA_SC_VPORT_SCISSOR_8_TL
+0x00028294 PA_SC_VPORT_SCISSOR_8_BR
+0x00028298 PA_SC_VPORT_SCISSOR_9_TL
+0x0002829C PA_SC_VPORT_SCISSOR_9_BR
+0x000282A0 PA_SC_VPORT_SCISSOR_10_TL
+0x000282A4 PA_SC_VPORT_SCISSOR_10_BR
+0x000282A8 PA_SC_VPORT_SCISSOR_11_TL
+0x000282AC PA_SC_VPORT_SCISSOR_11_BR
+0x000282B0 PA_SC_VPORT_SCISSOR_12_TL
+0x000282B4 PA_SC_VPORT_SCISSOR_12_BR
+0x000282B8 PA_SC_VPORT_SCISSOR_13_TL
+0x000282BC PA_SC_VPORT_SCISSOR_13_BR
+0x000282C0 PA_SC_VPORT_SCISSOR_14_TL
+0x000282C4 PA_SC_VPORT_SCISSOR_14_BR
+0x000282C8 PA_SC_VPORT_SCISSOR_15_TL
+0x000282CC PA_SC_VPORT_SCISSOR_15_BR
+0x000282D0 PA_SC_VPORT_ZMIN_0
+0x000282D4 PA_SC_VPORT_ZMAX_0
+0x000282D8 PA_SC_VPORT_ZMIN_1
+0x000282DC PA_SC_VPORT_ZMAX_1
+0x000282E0 PA_SC_VPORT_ZMIN_2
+0x000282E4 PA_SC_VPORT_ZMAX_2
+0x000282E8 PA_SC_VPORT_ZMIN_3
+0x000282EC PA_SC_VPORT_ZMAX_3
+0x000282F0 PA_SC_VPORT_ZMIN_4
+0x000282F4 PA_SC_VPORT_ZMAX_4
+0x000282F8 PA_SC_VPORT_ZMIN_5
+0x000282FC PA_SC_VPORT_ZMAX_5
+0x00028300 PA_SC_VPORT_ZMIN_6
+0x00028304 PA_SC_VPORT_ZMAX_6
+0x00028308 PA_SC_VPORT_ZMIN_7
+0x0002830C PA_SC_VPORT_ZMAX_7
+0x00028310 PA_SC_VPORT_ZMIN_8
+0x00028314 PA_SC_VPORT_ZMAX_8
+0x00028318 PA_SC_VPORT_ZMIN_9
+0x0002831C PA_SC_VPORT_ZMAX_9
+0x00028320 PA_SC_VPORT_ZMIN_10
+0x00028324 PA_SC_VPORT_ZMAX_10
+0x00028328 PA_SC_VPORT_ZMIN_11
+0x0002832C PA_SC_VPORT_ZMAX_11
+0x00028330 PA_SC_VPORT_ZMIN_12
+0x00028334 PA_SC_VPORT_ZMAX_12
+0x00028338 PA_SC_VPORT_ZMIN_13
+0x0002833C PA_SC_VPORT_ZMAX_13
+0x00028340 PA_SC_VPORT_ZMIN_14
+0x00028344 PA_SC_VPORT_ZMAX_14
+0x00028348 PA_SC_VPORT_ZMIN_15
+0x0002834C PA_SC_VPORT_ZMAX_15
+0x00028350 SX_MISC
+0x00028380 SQ_VTX_SEMANTIC_0
+0x00028384 SQ_VTX_SEMANTIC_1
+0x00028388 SQ_VTX_SEMANTIC_2
+0x0002838C SQ_VTX_SEMANTIC_3
+0x00028390 SQ_VTX_SEMANTIC_4
+0x00028394 SQ_VTX_SEMANTIC_5
+0x00028398 SQ_VTX_SEMANTIC_6
+0x0002839C SQ_VTX_SEMANTIC_7
+0x000283A0 SQ_VTX_SEMANTIC_8
+0x000283A4 SQ_VTX_SEMANTIC_9
+0x000283A8 SQ_VTX_SEMANTIC_10
+0x000283AC SQ_VTX_SEMANTIC_11
+0x000283B0 SQ_VTX_SEMANTIC_12
+0x000283B4 SQ_VTX_SEMANTIC_13
+0x000283B8 SQ_VTX_SEMANTIC_14
+0x000283BC SQ_VTX_SEMANTIC_15
+0x000283C0 SQ_VTX_SEMANTIC_16
+0x000283C4 SQ_VTX_SEMANTIC_17
+0x000283C8 SQ_VTX_SEMANTIC_18
+0x000283CC SQ_VTX_SEMANTIC_19
+0x000283D0 SQ_VTX_SEMANTIC_20
+0x000283D4 SQ_VTX_SEMANTIC_21
+0x000283D8 SQ_VTX_SEMANTIC_22
+0x000283DC SQ_VTX_SEMANTIC_23
+0x000283E0 SQ_VTX_SEMANTIC_24
+0x000283E4 SQ_VTX_SEMANTIC_25
+0x000283E8 SQ_VTX_SEMANTIC_26
+0x000283EC SQ_VTX_SEMANTIC_27
+0x000283F0 SQ_VTX_SEMANTIC_28
+0x000283F4 SQ_VTX_SEMANTIC_29
+0x000283F8 SQ_VTX_SEMANTIC_30
+0x000283FC SQ_VTX_SEMANTIC_31
+0x00028400 VGT_MAX_VTX_INDX
+0x00028404 VGT_MIN_VTX_INDX
+0x00028408 VGT_INDX_OFFSET
+0x0002840C VGT_MULTI_PRIM_IB_RESET_INDX
+0x00028410 SX_ALPHA_TEST_CONTROL
+0x00028414 CB_BLEND_RED
+0x00028418 CB_BLEND_GREEN
+0x0002841C CB_BLEND_BLUE
+0x00028420 CB_BLEND_ALPHA
+0x00028430 DB_STENCILREFMASK
+0x00028434 DB_STENCILREFMASK_BF
+0x00028438 SX_ALPHA_REF
+0x0002843C PA_CL_VPORT_XSCALE_0
+0x00028440 PA_CL_VPORT_XOFFSET_0
+0x00028444 PA_CL_VPORT_YSCALE_0
+0x00028448 PA_CL_VPORT_YOFFSET_0
+0x0002844C PA_CL_VPORT_ZSCALE_0
+0x00028450 PA_CL_VPORT_ZOFFSET_0
+0x00028454 PA_CL_VPORT_XSCALE_1
+0x00028458 PA_CL_VPORT_XOFFSET_1
+0x0002845C PA_CL_VPORT_YSCALE_1
+0x00028460 PA_CL_VPORT_YOFFSET_1
+0x00028464 PA_CL_VPORT_ZSCALE_1
+0x00028468 PA_CL_VPORT_ZOFFSET_1
+0x0002846C PA_CL_VPORT_XSCALE_2
+0x00028470 PA_CL_VPORT_XOFFSET_2
+0x00028474 PA_CL_VPORT_YSCALE_2
+0x00028478 PA_CL_VPORT_YOFFSET_2
+0x0002847C PA_CL_VPORT_ZSCALE_2
+0x00028480 PA_CL_VPORT_ZOFFSET_2
+0x00028484 PA_CL_VPORT_XSCALE_3
+0x00028488 PA_CL_VPORT_XOFFSET_3
+0x0002848C PA_CL_VPORT_YSCALE_3
+0x00028490 PA_CL_VPORT_YOFFSET_3
+0x00028494 PA_CL_VPORT_ZSCALE_3
+0x00028498 PA_CL_VPORT_ZOFFSET_3
+0x0002849C PA_CL_VPORT_XSCALE_4
+0x000284A0 PA_CL_VPORT_XOFFSET_4
+0x000284A4 PA_CL_VPORT_YSCALE_4
+0x000284A8 PA_CL_VPORT_YOFFSET_4
+0x000284AC PA_CL_VPORT_ZSCALE_4
+0x000284B0 PA_CL_VPORT_ZOFFSET_4
+0x000284B4 PA_CL_VPORT_XSCALE_5
+0x000284B8 PA_CL_VPORT_XOFFSET_5
+0x000284BC PA_CL_VPORT_YSCALE_5
+0x000284C0 PA_CL_VPORT_YOFFSET_5
+0x000284C4 PA_CL_VPORT_ZSCALE_5
+0x000284C8 PA_CL_VPORT_ZOFFSET_5
+0x000284CC PA_CL_VPORT_XSCALE_6
+0x000284D0 PA_CL_VPORT_XOFFSET_6
+0x000284D4 PA_CL_VPORT_YSCALE_6
+0x000284D8 PA_CL_VPORT_YOFFSET_6
+0x000284DC PA_CL_VPORT_ZSCALE_6
+0x000284E0 PA_CL_VPORT_ZOFFSET_6
+0x000284E4 PA_CL_VPORT_XSCALE_7
+0x000284E8 PA_CL_VPORT_XOFFSET_7
+0x000284EC PA_CL_VPORT_YSCALE_7
+0x000284F0 PA_CL_VPORT_YOFFSET_7
+0x000284F4 PA_CL_VPORT_ZSCALE_7
+0x000284F8 PA_CL_VPORT_ZOFFSET_7
+0x000284FC PA_CL_VPORT_XSCALE_8
+0x00028500 PA_CL_VPORT_XOFFSET_8
+0x00028504 PA_CL_VPORT_YSCALE_8
+0x00028508 PA_CL_VPORT_YOFFSET_8
+0x0002850C PA_CL_VPORT_ZSCALE_8
+0x00028510 PA_CL_VPORT_ZOFFSET_8
+0x00028514 PA_CL_VPORT_XSCALE_9
+0x00028518 PA_CL_VPORT_XOFFSET_9
+0x0002851C PA_CL_VPORT_YSCALE_9
+0x00028520 PA_CL_VPORT_YOFFSET_9
+0x00028524 PA_CL_VPORT_ZSCALE_9
+0x00028528 PA_CL_VPORT_ZOFFSET_9
+0x0002852C PA_CL_VPORT_XSCALE_10
+0x00028530 PA_CL_VPORT_XOFFSET_10
+0x00028534 PA_CL_VPORT_YSCALE_10
+0x00028538 PA_CL_VPORT_YOFFSET_10
+0x0002853C PA_CL_VPORT_ZSCALE_10
+0x00028540 PA_CL_VPORT_ZOFFSET_10
+0x00028544 PA_CL_VPORT_XSCALE_11
+0x00028548 PA_CL_VPORT_XOFFSET_11
+0x0002854C PA_CL_VPORT_YSCALE_11
+0x00028550 PA_CL_VPORT_YOFFSET_11
+0x00028554 PA_CL_VPORT_ZSCALE_11
+0x00028558 PA_CL_VPORT_ZOFFSET_11
+0x0002855C PA_CL_VPORT_XSCALE_12
+0x00028560 PA_CL_VPORT_XOFFSET_12
+0x00028564 PA_CL_VPORT_YSCALE_12
+0x00028568 PA_CL_VPORT_YOFFSET_12
+0x0002856C PA_CL_VPORT_ZSCALE_12
+0x00028570 PA_CL_VPORT_ZOFFSET_12
+0x00028574 PA_CL_VPORT_XSCALE_13
+0x00028578 PA_CL_VPORT_XOFFSET_13
+0x0002857C PA_CL_VPORT_YSCALE_13
+0x00028580 PA_CL_VPORT_YOFFSET_13
+0x00028584 PA_CL_VPORT_ZSCALE_13
+0x00028588 PA_CL_VPORT_ZOFFSET_13
+0x0002858C PA_CL_VPORT_XSCALE_14
+0x00028590 PA_CL_VPORT_XOFFSET_14
+0x00028594 PA_CL_VPORT_YSCALE_14
+0x00028598 PA_CL_VPORT_YOFFSET_14
+0x0002859C PA_CL_VPORT_ZSCALE_14
+0x000285A0 PA_CL_VPORT_ZOFFSET_14
+0x000285A4 PA_CL_VPORT_XSCALE_15
+0x000285A8 PA_CL_VPORT_XOFFSET_15
+0x000285AC PA_CL_VPORT_YSCALE_15
+0x000285B0 PA_CL_VPORT_YOFFSET_15
+0x000285B4 PA_CL_VPORT_ZSCALE_15
+0x000285B8 PA_CL_VPORT_ZOFFSET_15
+0x000285BC PA_CL_UCP_0_X
+0x000285C0 PA_CL_UCP_0_Y
+0x000285C4 PA_CL_UCP_0_Z
+0x000285C8 PA_CL_UCP_0_W
+0x000285CC PA_CL_UCP_1_X
+0x000285D0 PA_CL_UCP_1_Y
+0x000285D4 PA_CL_UCP_1_Z
+0x000285D8 PA_CL_UCP_1_W
+0x000285DC PA_CL_UCP_2_X
+0x000285E0 PA_CL_UCP_2_Y
+0x000285E4 PA_CL_UCP_2_Z
+0x000285E8 PA_CL_UCP_2_W
+0x000285EC PA_CL_UCP_3_X
+0x000285F0 PA_CL_UCP_3_Y
+0x000285F4 PA_CL_UCP_3_Z
+0x000285F8 PA_CL_UCP_3_W
+0x000285FC PA_CL_UCP_4_X
+0x00028600 PA_CL_UCP_4_Y
+0x00028604 PA_CL_UCP_4_Z
+0x00028608 PA_CL_UCP_4_W
+0x0002860C PA_CL_UCP_5_X
+0x00028610 PA_CL_UCP_5_Y
+0x00028614 PA_CL_UCP_5_Z
+0x00028618 PA_CL_UCP_5_W
+0x0002861C SPI_VS_OUT_ID_0
+0x00028620 SPI_VS_OUT_ID_1
+0x00028624 SPI_VS_OUT_ID_2
+0x00028628 SPI_VS_OUT_ID_3
+0x0002862C SPI_VS_OUT_ID_4
+0x00028630 SPI_VS_OUT_ID_5
+0x00028634 SPI_VS_OUT_ID_6
+0x00028638 SPI_VS_OUT_ID_7
+0x0002863C SPI_VS_OUT_ID_8
+0x00028640 SPI_VS_OUT_ID_9
+0x00028644 SPI_PS_INPUT_CNTL_0
+0x00028648 SPI_PS_INPUT_CNTL_1
+0x0002864C SPI_PS_INPUT_CNTL_2
+0x00028650 SPI_PS_INPUT_CNTL_3
+0x00028654 SPI_PS_INPUT_CNTL_4
+0x00028658 SPI_PS_INPUT_CNTL_5
+0x0002865C SPI_PS_INPUT_CNTL_6
+0x00028660 SPI_PS_INPUT_CNTL_7
+0x00028664 SPI_PS_INPUT_CNTL_8
+0x00028668 SPI_PS_INPUT_CNTL_9
+0x0002866C SPI_PS_INPUT_CNTL_10
+0x00028670 SPI_PS_INPUT_CNTL_11
+0x00028674 SPI_PS_INPUT_CNTL_12
+0x00028678 SPI_PS_INPUT_CNTL_13
+0x0002867C SPI_PS_INPUT_CNTL_14
+0x00028680 SPI_PS_INPUT_CNTL_15
+0x00028684 SPI_PS_INPUT_CNTL_16
+0x00028688 SPI_PS_INPUT_CNTL_17
+0x0002868C SPI_PS_INPUT_CNTL_18
+0x00028690 SPI_PS_INPUT_CNTL_19
+0x00028694 SPI_PS_INPUT_CNTL_20
+0x00028698 SPI_PS_INPUT_CNTL_21
+0x0002869C SPI_PS_INPUT_CNTL_22
+0x000286A0 SPI_PS_INPUT_CNTL_23
+0x000286A4 SPI_PS_INPUT_CNTL_24
+0x000286A8 SPI_PS_INPUT_CNTL_25
+0x000286AC SPI_PS_INPUT_CNTL_26
+0x000286B0 SPI_PS_INPUT_CNTL_27
+0x000286B4 SPI_PS_INPUT_CNTL_28
+0x000286B8 SPI_PS_INPUT_CNTL_29
+0x000286BC SPI_PS_INPUT_CNTL_30
+0x000286C0 SPI_PS_INPUT_CNTL_31
+0x000286C4 SPI_VS_OUT_CONFIG
+0x000286C8 SPI_THREAD_GROUPING
+0x000286CC SPI_PS_IN_CONTROL_0
+0x000286D0 SPI_PS_IN_CONTROL_1
+0x000286D4 SPI_INTERP_CONTROL_0
+0x000286D8 SPI_INPUT_Z
+0x000286DC SPI_FOG_CNTL
+0x000286E0 SPI_BARYC_CNTL
+0x000286E4 SPI_PS_IN_CONTROL_2
+0x000286E8 SPI_COMPUTE_INPUT_CNTL
+0x000286EC SPI_COMPUTE_NUM_THREAD_X
+0x000286F0 SPI_COMPUTE_NUM_THREAD_Y
+0x000286F4 SPI_COMPUTE_NUM_THREAD_Z
+0x000286F8 GDS_ADDR_SIZE
+0x00028780 CB_BLEND0_CONTROL
+0x00028784 CB_BLEND1_CONTROL
+0x00028788 CB_BLEND2_CONTROL
+0x0002878C CB_BLEND3_CONTROL
+0x00028790 CB_BLEND4_CONTROL
+0x00028794 CB_BLEND5_CONTROL
+0x00028798 CB_BLEND6_CONTROL
+0x0002879C CB_BLEND7_CONTROL
+0x000287CC CS_COPY_STATE
+0x000287D0 GFX_COPY_STATE
+0x000287D4 PA_CL_POINT_X_RAD
+0x000287D8 PA_CL_POINT_Y_RAD
+0x000287DC PA_CL_POINT_SIZE
+0x000287E0 PA_CL_POINT_CULL_RAD
+0x00028808 CB_COLOR_CONTROL
+0x0002880C DB_SHADER_CONTROL
+0x00028810 PA_CL_CLIP_CNTL
+0x00028814 PA_SU_SC_MODE_CNTL
+0x00028818 PA_CL_VTE_CNTL
+0x0002881C PA_CL_VS_OUT_CNTL
+0x00028820 PA_CL_NANINF_CNTL
+0x00028824 PA_SU_LINE_STIPPLE_CNTL
+0x00028828 PA_SU_LINE_STIPPLE_SCALE
+0x0002882C PA_SU_PRIM_FILTER_CNTL
+0x00028838 SQ_DYN_GPR_RESOURCE_LIMIT_1
+0x00028844 SQ_PGM_RESOURCES_PS
+0x00028848 SQ_PGM_RESOURCES_2_PS
+0x0002884C SQ_PGM_EXPORTS_PS
+0x0002885C SQ_PGM_RESOURCES_VS
+0x00028860 SQ_PGM_RESOURCES_2_VS
+0x00028878 SQ_PGM_RESOURCES_GS
+0x0002887C SQ_PGM_RESOURCES_2_GS
+0x00028890 SQ_PGM_RESOURCES_ES
+0x00028894 SQ_PGM_RESOURCES_2_ES
+0x000288A8 SQ_PGM_RESOURCES_FS
+0x000288BC SQ_PGM_RESOURCES_HS
+0x000288C0 SQ_PGM_RESOURCES_2_HS
+0x000288D0 SQ_PGM_RESOURCES_LS
+0x000288D4 SQ_PGM_RESOURCES_2_LS
+0x000288E8 SQ_LDS_ALLOC
+0x000288EC SQ_LDS_ALLOC_PS
+0x000288F0 SQ_VTX_SEMANTIC_CLEAR
+0x00028A00 PA_SU_POINT_SIZE
+0x00028A04 PA_SU_POINT_MINMAX
+0x00028A08 PA_SU_LINE_CNTL
+0x00028A0C PA_SC_LINE_STIPPLE
+0x00028A10 VGT_OUTPUT_PATH_CNTL
+0x00028A14 VGT_HOS_CNTL
+0x00028A18 VGT_HOS_MAX_TESS_LEVEL
+0x00028A1C VGT_HOS_MIN_TESS_LEVEL
+0x00028A20 VGT_HOS_REUSE_DEPTH
+0x00028A24 VGT_GROUP_PRIM_TYPE
+0x00028A28 VGT_GROUP_FIRST_DECR
+0x00028A2C VGT_GROUP_DECR
+0x00028A30 VGT_GROUP_VECT_0_CNTL
+0x00028A34 VGT_GROUP_VECT_1_CNTL
+0x00028A38 VGT_GROUP_VECT_0_FMT_CNTL
+0x00028A3C VGT_GROUP_VECT_1_FMT_CNTL
+0x00028A40 VGT_GS_MODE
+0x00028A48 PA_SC_MODE_CNTL_0
+0x00028A4C PA_SC_MODE_CNTL_1
+0x00028A50 VGT_ENHANCE
+0x00028A54 VGT_GS_PER_ES
+0x00028A58 VGT_ES_PER_GS
+0x00028A5C VGT_GS_PER_VS
+0x00028A6C VGT_GS_OUT_PRIM_TYPE
+0x00028A84 VGT_PRIMITIVEID_EN
+0x00028A94 VGT_MULTI_PRIM_IB_RESET_EN
+0x00028AA0 VGT_INSTANCE_STEP_RATE_0
+0x00028AA4 VGT_INSTANCE_STEP_RATE_1
+0x00028AB4 VGT_REUSE_OFF
+0x00028AB8 VGT_VTX_CNT_EN
+0x00028ABC DB_HTILE_SURFACE
+0x00028AC0 DB_SRESULTS_COMPARE_STATE0
+0x00028AC4 DB_SRESULTS_COMPARE_STATE1
+0x00028AC8 DB_PRELOAD_CONTROL
+0x00028B38 VGT_GS_MAX_VERT_OUT
+0x00028B54 VGT_SHADER_STAGES_EN
+0x00028B58 VGT_LS_HS_CONFIG
+0x00028B5C VGT_LS_SIZE
+0x00028B60 VGT_HS_SIZE
+0x00028B64 VGT_LS_HS_ALLOC
+0x00028B68 VGT_HS_PATCH_CONST
+0x00028B6C VGT_TF_PARAM
+0x00028B70 DB_ALPHA_TO_MASK
+0x00028B74 VGT_DISPATCH_INITIATOR
+0x00028B78 PA_SU_POLY_OFFSET_DB_FMT_CNTL
+0x00028B7C PA_SU_POLY_OFFSET_CLAMP
+0x00028B80 PA_SU_POLY_OFFSET_FRONT_SCALE
+0x00028B84 PA_SU_POLY_OFFSET_FRONT_OFFSET
+0x00028B88 PA_SU_POLY_OFFSET_BACK_SCALE
+0x00028B8C PA_SU_POLY_OFFSET_BACK_OFFSET
+0x00028B74 VGT_GS_INSTANCE_CNT
+0x00028C00 PA_SC_LINE_CNTL
+0x00028C08 PA_SU_VTX_CNTL
+0x00028C0C PA_CL_GB_VERT_CLIP_ADJ
+0x00028C10 PA_CL_GB_VERT_DISC_ADJ
+0x00028C14 PA_CL_GB_HORZ_CLIP_ADJ
+0x00028C18 PA_CL_GB_HORZ_DISC_ADJ
+0x00028C1C PA_SC_AA_SAMPLE_LOCS_0
+0x00028C20 PA_SC_AA_SAMPLE_LOCS_1
+0x00028C24 PA_SC_AA_SAMPLE_LOCS_2
+0x00028C28 PA_SC_AA_SAMPLE_LOCS_3
+0x00028C2C PA_SC_AA_SAMPLE_LOCS_4
+0x00028C30 PA_SC_AA_SAMPLE_LOCS_5
+0x00028C34 PA_SC_AA_SAMPLE_LOCS_6
+0x00028C38 PA_SC_AA_SAMPLE_LOCS_7
+0x00028C3C PA_SC_AA_MASK
+0x00028C8C CB_COLOR0_CLEAR_WORD0
+0x00028C90 CB_COLOR0_CLEAR_WORD1
+0x00028C94 CB_COLOR0_CLEAR_WORD2
+0x00028C98 CB_COLOR0_CLEAR_WORD3
+0x00028CC8 CB_COLOR1_CLEAR_WORD0
+0x00028CCC CB_COLOR1_CLEAR_WORD1
+0x00028CD0 CB_COLOR1_CLEAR_WORD2
+0x00028CD4 CB_COLOR1_CLEAR_WORD3
+0x00028D04 CB_COLOR2_CLEAR_WORD0
+0x00028D08 CB_COLOR2_CLEAR_WORD1
+0x00028D0C CB_COLOR2_CLEAR_WORD2
+0x00028D10 CB_COLOR2_CLEAR_WORD3
+0x00028D40 CB_COLOR3_CLEAR_WORD0
+0x00028D44 CB_COLOR3_CLEAR_WORD1
+0x00028D48 CB_COLOR3_CLEAR_WORD2
+0x00028D4C CB_COLOR3_CLEAR_WORD3
+0x00028D7C CB_COLOR4_CLEAR_WORD0
+0x00028D80 CB_COLOR4_CLEAR_WORD1
+0x00028D84 CB_COLOR4_CLEAR_WORD2
+0x00028D88 CB_COLOR4_CLEAR_WORD3
+0x00028DB8 CB_COLOR5_CLEAR_WORD0
+0x00028DBC CB_COLOR5_CLEAR_WORD1
+0x00028DC0 CB_COLOR5_CLEAR_WORD2
+0x00028DC4 CB_COLOR5_CLEAR_WORD3
+0x00028DF4 CB_COLOR6_CLEAR_WORD0
+0x00028DF8 CB_COLOR6_CLEAR_WORD1
+0x00028DFC CB_COLOR6_CLEAR_WORD2
+0x00028E00 CB_COLOR6_CLEAR_WORD3
+0x00028E30 CB_COLOR7_CLEAR_WORD0
+0x00028E34 CB_COLOR7_CLEAR_WORD1
+0x00028E38 CB_COLOR7_CLEAR_WORD2
+0x00028E3C CB_COLOR7_CLEAR_WORD3
+0x00028F80 SQ_ALU_CONST_BUFFER_SIZE_HS_0
+0x00028F84 SQ_ALU_CONST_BUFFER_SIZE_HS_1
+0x00028F88 SQ_ALU_CONST_BUFFER_SIZE_HS_2
+0x00028F8C SQ_ALU_CONST_BUFFER_SIZE_HS_3
+0x00028F90 SQ_ALU_CONST_BUFFER_SIZE_HS_4
+0x00028F94 SQ_ALU_CONST_BUFFER_SIZE_HS_5
+0x00028F98 SQ_ALU_CONST_BUFFER_SIZE_HS_6
+0x00028F9C SQ_ALU_CONST_BUFFER_SIZE_HS_7
+0x00028FA0 SQ_ALU_CONST_BUFFER_SIZE_HS_8
+0x00028FA4 SQ_ALU_CONST_BUFFER_SIZE_HS_9
+0x00028FA8 SQ_ALU_CONST_BUFFER_SIZE_HS_10
+0x00028FAC SQ_ALU_CONST_BUFFER_SIZE_HS_11
+0x00028FB0 SQ_ALU_CONST_BUFFER_SIZE_HS_12
+0x00028FB4 SQ_ALU_CONST_BUFFER_SIZE_HS_13
+0x00028FB8 SQ_ALU_CONST_BUFFER_SIZE_HS_14
+0x00028FBC SQ_ALU_CONST_BUFFER_SIZE_HS_15
+0x00028FC0 SQ_ALU_CONST_BUFFER_SIZE_LS_0
+0x00028FC4 SQ_ALU_CONST_BUFFER_SIZE_LS_1
+0x00028FC8 SQ_ALU_CONST_BUFFER_SIZE_LS_2
+0x00028FCC SQ_ALU_CONST_BUFFER_SIZE_LS_3
+0x00028FD0 SQ_ALU_CONST_BUFFER_SIZE_LS_4
+0x00028FD4 SQ_ALU_CONST_BUFFER_SIZE_LS_5
+0x00028FD8 SQ_ALU_CONST_BUFFER_SIZE_LS_6
+0x00028FDC SQ_ALU_CONST_BUFFER_SIZE_LS_7
+0x00028FE0 SQ_ALU_CONST_BUFFER_SIZE_LS_8
+0x00028FE4 SQ_ALU_CONST_BUFFER_SIZE_LS_9
+0x00028FE8 SQ_ALU_CONST_BUFFER_SIZE_LS_10
+0x00028FEC SQ_ALU_CONST_BUFFER_SIZE_LS_11
+0x00028FF0 SQ_ALU_CONST_BUFFER_SIZE_LS_12
+0x00028FF4 SQ_ALU_CONST_BUFFER_SIZE_LS_13
+0x00028FF8 SQ_ALU_CONST_BUFFER_SIZE_LS_14
+0x00028FFC SQ_ALU_CONST_BUFFER_SIZE_LS_15
+0x0003CFF0 SQ_VTX_BASE_VTX_LOC
+0x0003CFF4 SQ_VTX_START_INST_LOC
+0x0003FF00 SQ_TEX_SAMPLER_CLEAR
+0x0003FF04 SQ_TEX_RESOURCE_CLEAR
+0x0003FF08 SQ_LOOP_BOOL_CLEAR
diff --git a/drivers/gpu/drm/radeon/rs600.c b/drivers/gpu/drm/radeon/rs600.c
index 79887cac5b54..7bb4c3e52f3b 100644
--- a/drivers/gpu/drm/radeon/rs600.c
+++ b/drivers/gpu/drm/radeon/rs600.c
@@ -74,7 +74,8 @@ void rs600_pm_misc(struct radeon_device *rdev)
                         if (voltage->delay)
                                 udelay(voltage->delay);
                 }
-        }
+        } else if (voltage->type == VOLTAGE_VDDC)
+                radeon_atom_set_voltage(rdev, voltage->vddc_id);
 
         dyn_pwrmgt_sclk_length = RREG32_PLL(DYN_PWRMGT_SCLK_LENGTH);
         dyn_pwrmgt_sclk_length &= ~REDUCED_POWER_SCLK_HILEN(0xf);
diff --git a/drivers/gpu/drm/radeon/rv770.c b/drivers/gpu/drm/radeon/rv770.c
index 253f24aec031..cec536c222c5 100644
--- a/drivers/gpu/drm/radeon/rv770.c
+++ b/drivers/gpu/drm/radeon/rv770.c
@@ -44,7 +44,18 @@ void rv770_fini(struct radeon_device *rdev);
 
 void rv770_pm_misc(struct radeon_device *rdev)
 {
-
+        int req_ps_idx = rdev->pm.requested_power_state_index;
+        int req_cm_idx = rdev->pm.requested_clock_mode_index;
+        struct radeon_power_state *ps = &rdev->pm.power_state[req_ps_idx];
+        struct radeon_voltage *voltage = &ps->clock_info[req_cm_idx].voltage;
+
+        if ((voltage->type == VOLTAGE_SW) && voltage->voltage) {
+                if (voltage->voltage != rdev->pm.current_vddc) {
+                        radeon_atom_set_voltage(rdev, voltage->voltage);
+                        rdev->pm.current_vddc = voltage->voltage;
+                        DRM_DEBUG("Setting: v: %d\n", voltage->voltage);
+                }
+        }
 }
 
 /*
diff --git a/drivers/gpu/drm/ttm/ttm_page_alloc.c b/drivers/gpu/drm/ttm/ttm_page_alloc.c
index 0d9a42c2394f..ef910694bd63 100644
--- a/drivers/gpu/drm/ttm/ttm_page_alloc.c
+++ b/drivers/gpu/drm/ttm/ttm_page_alloc.c
@@ -77,7 +77,7 @@ struct ttm_page_pool {
 /**
  * Limits for the pool. They are handled without locks because only place where
  * they may change is in sysfs store. They won't have immediate effect anyway
- * so forcing serialiazation to access them is pointless.
+ * so forcing serialization to access them is pointless.
  */
 
 struct ttm_pool_opts {
@@ -165,16 +165,18 @@ static ssize_t ttm_pool_store(struct kobject *kobj,
                 m->options.small = val;
         else if (attr == &ttm_page_pool_alloc_size) {
                 if (val > NUM_PAGES_TO_ALLOC*8) {
-                        printk(KERN_ERR "[ttm] Setting allocation size to %lu "
-                                        "is not allowed. Recomended size is "
-                                        "%lu\n",
-                                        NUM_PAGES_TO_ALLOC*(PAGE_SIZE >> 7),
-                                        NUM_PAGES_TO_ALLOC*(PAGE_SIZE >> 10));
+                        printk(KERN_ERR TTM_PFX
+                               "Setting allocation size to %lu "
+                               "is not allowed. Recommended size is "
+                               "%lu\n",
+                               NUM_PAGES_TO_ALLOC*(PAGE_SIZE >> 7),
+                               NUM_PAGES_TO_ALLOC*(PAGE_SIZE >> 10));
                         return size;
                 } else if (val > NUM_PAGES_TO_ALLOC) {
-                        printk(KERN_WARNING "[ttm] Setting allocation size to "
-                                        "larger than %lu is not recomended.\n",
-                                        NUM_PAGES_TO_ALLOC*(PAGE_SIZE >> 10));
+                        printk(KERN_WARNING TTM_PFX
+                               "Setting allocation size to "
+                               "larger than %lu is not recommended.\n",
+                               NUM_PAGES_TO_ALLOC*(PAGE_SIZE >> 10));
                 }
                 m->options.alloc_size = val;
         }
@@ -277,7 +279,7 @@ static void ttm_pages_put(struct page *pages[], unsigned npages)
 {
         unsigned i;
         if (set_pages_array_wb(pages, npages))
-                printk(KERN_ERR "[ttm] Failed to set %d pages to wb!\n",
+                printk(KERN_ERR TTM_PFX "Failed to set %d pages to wb!\n",
                                 npages);
         for (i = 0; i < npages; ++i)
                 __free_page(pages[i]);
@@ -313,7 +315,8 @@ static int ttm_page_pool_free(struct ttm_page_pool *pool, unsigned nr_free)
         pages_to_free = kmalloc(npages_to_free * sizeof(struct page *),
                         GFP_KERNEL);
         if (!pages_to_free) {
-                printk(KERN_ERR "Failed to allocate memory for pool free operation.\n");
+                printk(KERN_ERR TTM_PFX
+                       "Failed to allocate memory for pool free operation.\n");
                 return 0;
         }
 
@@ -390,7 +393,7 @@ static int ttm_pool_get_num_unused_pages(void)
 }
 
 /**
- * Calback for mm to request pool to reduce number of page held.
+ * Callback for mm to request pool to reduce number of page held.
  */
 static int ttm_pool_mm_shrink(int shrink_pages, gfp_t gfp_mask)
 {
@@ -433,14 +436,16 @@ static int ttm_set_pages_caching(struct page **pages,
         case tt_uncached:
                 r = set_pages_array_uc(pages, cpages);
                 if (r)
-                        printk(KERN_ERR "[ttm] Failed to set %d pages to uc!\n",
-                                        cpages);
+                        printk(KERN_ERR TTM_PFX
+                               "Failed to set %d pages to uc!\n",
+                               cpages);
                 break;
         case tt_wc:
                 r = set_pages_array_wc(pages, cpages);
                 if (r)
-                        printk(KERN_ERR "[ttm] Failed to set %d pages to wc!\n",
-                                        cpages);
+                        printk(KERN_ERR TTM_PFX
+                               "Failed to set %d pages to wc!\n",
+                               cpages);
                 break;
         default:
                 break;
@@ -458,7 +463,7 @@ static void ttm_handle_caching_state_failure(struct list_head *pages,
                 struct page **failed_pages, unsigned cpages)
 {
         unsigned i;
-        /* Failed pages has to be reed */
+        /* Failed pages have to be freed */
         for (i = 0; i < cpages; ++i) {
                 list_del(&failed_pages[i]->lru);
                 __free_page(failed_pages[i]);
@@ -485,7 +490,8 @@ static int ttm_alloc_new_pages(struct list_head *pages, int gfp_flags,
         caching_array = kmalloc(max_cpages*sizeof(struct page *), GFP_KERNEL);
 
         if (!caching_array) {
-                printk(KERN_ERR "[ttm] unable to allocate table for new pages.");
+                printk(KERN_ERR TTM_PFX
+                       "Unable to allocate table for new pages.");
                 return -ENOMEM;
         }
 
@@ -493,12 +499,13 @@ static int ttm_alloc_new_pages(struct list_head *pages, int gfp_flags,
                 p = alloc_page(gfp_flags);
 
                 if (!p) {
-                        printk(KERN_ERR "[ttm] unable to get page %u\n", i);
+                        printk(KERN_ERR TTM_PFX "Unable to get page %u.\n", i);
 
                         /* store already allocated pages in the pool after
                          * setting the caching state */
                         if (cpages) {
-                                r = ttm_set_pages_caching(caching_array, cstate, cpages);
+                                r = ttm_set_pages_caching(caching_array,
+                                                          cstate, cpages);
                                 if (r)
                                         ttm_handle_caching_state_failure(pages,
                                                 ttm_flags, cstate,
@@ -590,7 +597,8 @@ static void ttm_page_pool_fill_locked(struct ttm_page_pool *pool,
                         ++pool->nrefills;
                         pool->npages += alloc_size;
                 } else {
-                        printk(KERN_ERR "[ttm] Failed to fill pool (%p).", pool);
+                        printk(KERN_ERR TTM_PFX
+                               "Failed to fill pool (%p).", pool);
                         /* If we have any pages left put them to the pool. */
                         list_for_each_entry(p, &pool->list, lru) {
                                 ++cpages;
@@ -671,13 +679,14 @@ int ttm_get_pages(struct list_head *pages, int flags,
                 if (flags & TTM_PAGE_FLAG_DMA32)
                         gfp_flags |= GFP_DMA32;
                 else
-                        gfp_flags |= __GFP_HIGHMEM;
+                        gfp_flags |= GFP_HIGHUSER;
 
                 for (r = 0; r < count; ++r) {
                         p = alloc_page(gfp_flags);
                         if (!p) {
 
-                                printk(KERN_ERR "[ttm] unable to allocate page.");
+                                printk(KERN_ERR TTM_PFX
+                                       "Unable to allocate page.");
                                 return -ENOMEM;
                         }
 
@@ -709,8 +718,9 @@ int ttm_get_pages(struct list_head *pages, int flags,
                 if (r) {
                         /* If there is any pages in the list put them back to
                          * the pool. */
-                        printk(KERN_ERR "[ttm] Failed to allocate extra pages "
-                                        "for large request.");
+                        printk(KERN_ERR TTM_PFX
+                               "Failed to allocate extra pages "
+                               "for large request.");
                         ttm_put_pages(pages, 0, flags, cstate);
                         return r;
                 }
@@ -778,7 +788,7 @@ int ttm_page_alloc_init(struct ttm_mem_global *glob, unsigned max_pages)
         if (atomic_add_return(1, &_manager.page_alloc_inited) > 1)
                 return 0;
 
-        printk(KERN_INFO "[ttm] Initializing pool allocator.\n");
+        printk(KERN_INFO TTM_PFX "Initializing pool allocator.\n");
 
         ttm_page_pool_init_locked(&_manager.wc_pool, GFP_HIGHUSER, "wc");
 
@@ -813,7 +823,7 @@ void ttm_page_alloc_fini()
         if (atomic_sub_return(1, &_manager.page_alloc_inited) > 0)
                 return;
 
-        printk(KERN_INFO "[ttm] Finilizing pool allocator.\n");
+        printk(KERN_INFO TTM_PFX "Finalizing pool allocator.\n");
         ttm_pool_mm_shrink_fini(&_manager);
 
         for (i = 0; i < NUM_POOLS; ++i)
diff --git a/drivers/gpu/drm/vmwgfx/Makefile b/drivers/gpu/drm/vmwgfx/Makefile
index 1a3cb6816d1c..4505e17df3f5 100644
--- a/drivers/gpu/drm/vmwgfx/Makefile
+++ b/drivers/gpu/drm/vmwgfx/Makefile
@@ -4,6 +4,6 @@ ccflags-y := -Iinclude/drm
 vmwgfx-y := vmwgfx_execbuf.o vmwgfx_gmr.o vmwgfx_kms.o vmwgfx_drv.o \
             vmwgfx_fb.o vmwgfx_ioctl.o vmwgfx_resource.o vmwgfx_buffer.o \
             vmwgfx_fifo.o vmwgfx_irq.o vmwgfx_ldu.o vmwgfx_ttm_glue.o \
-            vmwgfx_overlay.o
+            vmwgfx_overlay.o vmwgfx_fence.o
 
 obj-$(CONFIG_DRM_VMWGFX) := vmwgfx.o
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
index 0c9c0811f42d..b793c8c9acb3 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
@@ -88,6 +88,9 @@
 #define DRM_IOCTL_VMW_FENCE_WAIT                                \
         DRM_IOWR(DRM_COMMAND_BASE + DRM_VMW_FENCE_WAIT,         \
                  struct drm_vmw_fence_wait_arg)
+#define DRM_IOCTL_VMW_UPDATE_LAYOUT                             \
+        DRM_IOWR(DRM_COMMAND_BASE + DRM_VMW_UPDATE_LAYOUT,      \
+                 struct drm_vmw_update_layout_arg)
 
 
 /**
@@ -135,7 +138,9 @@ static struct drm_ioctl_desc vmw_ioctls[] = {
         VMW_IOCTL_DEF(DRM_IOCTL_VMW_FIFO_DEBUG, vmw_fifo_debug_ioctl,
                       DRM_AUTH | DRM_ROOT_ONLY | DRM_MASTER | DRM_UNLOCKED),
         VMW_IOCTL_DEF(DRM_IOCTL_VMW_FENCE_WAIT, vmw_fence_wait_ioctl,
-                      DRM_AUTH | DRM_UNLOCKED)
+                      DRM_AUTH | DRM_UNLOCKED),
+        VMW_IOCTL_DEF(DRM_IOCTL_VMW_UPDATE_LAYOUT, vmw_kms_update_layout_ioctl,
+                      DRM_MASTER | DRM_CONTROL_ALLOW | DRM_UNLOCKED)
 };
 
 static struct pci_device_id vmw_pci_id_list[] = {
@@ -318,6 +323,15 @@ static int vmw_driver_load(struct drm_device *dev, unsigned long chipset)
                 goto out_err3;
         }
 
+        /* Need mmio memory to check for fifo pitchlock cap. */
+        if (!(dev_priv->capabilities & SVGA_CAP_DISPLAY_TOPOLOGY) &&
+            !(dev_priv->capabilities & SVGA_CAP_PITCHLOCK) &&
+            !vmw_fifo_have_pitchlock(dev_priv)) {
+                ret = -ENOSYS;
+                DRM_ERROR("Hardware has no pitchlock\n");
+                goto out_err4;
+        }
+
         dev_priv->tdev = ttm_object_device_init
             (dev_priv->mem_global_ref.object, 12);
 
@@ -399,8 +413,6 @@ static int vmw_driver_unload(struct drm_device *dev)
 {
         struct vmw_private *dev_priv = vmw_priv(dev);
 
-        DRM_INFO(VMWGFX_DRIVER_NAME " unload.\n");
-
         unregister_pm_notifier(&dev_priv->pm_nb);
 
         vmw_fb_close(dev_priv);
@@ -546,7 +558,6 @@ static int vmw_master_create(struct drm_device *dev,
 {
         struct vmw_master *vmaster;
 
-        DRM_INFO("Master create.\n");
         vmaster = kzalloc(sizeof(*vmaster), GFP_KERNEL);
         if (unlikely(vmaster == NULL))
                 return -ENOMEM;
@@ -563,7 +574,6 @@ static void vmw_master_destroy(struct drm_device *dev,
 {
         struct vmw_master *vmaster = vmw_master(master);
 
-        DRM_INFO("Master destroy.\n");
         master->driver_priv = NULL;
         kfree(vmaster);
 }
@@ -579,8 +589,6 @@ static int vmw_master_set(struct drm_device *dev,
         struct vmw_master *vmaster = vmw_master(file_priv->master);
         int ret = 0;
 
-        DRM_INFO("Master set.\n");
-
         if (active) {
                 BUG_ON(active != &dev_priv->fbdev_master);
                 ret = ttm_vt_lock(&active->lock, false, vmw_fp->tfile);
@@ -622,8 +630,6 @@ static void vmw_master_drop(struct drm_device *dev,
         struct vmw_master *vmaster = vmw_master(file_priv->master);
         int ret;
 
-        DRM_INFO("Master drop.\n");
-
         /**
          * Make sure the master doesn't disappear while we have
          * it locked.
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h
index 356dc935ec13..eaad52095339 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h
@@ -41,12 +41,13 @@
 
 #define VMWGFX_DRIVER_DATE "20100209"
 #define VMWGFX_DRIVER_MAJOR 1
-#define VMWGFX_DRIVER_MINOR 0
+#define VMWGFX_DRIVER_MINOR 2
 #define VMWGFX_DRIVER_PATCHLEVEL 0
 #define VMWGFX_FILE_PAGE_OFFSET 0x00100000
 #define VMWGFX_FIFO_STATIC_SIZE (1024*1024)
 #define VMWGFX_MAX_RELOCATIONS 2048
 #define VMWGFX_MAX_GMRS 2048
+#define VMWGFX_MAX_DISPLAYS 16
 
 struct vmw_fpriv {
         struct drm_master *locked_master;
@@ -102,6 +103,13 @@ struct vmw_surface {
         struct vmw_cursor_snooper snooper;
 };
 
+struct vmw_fence_queue {
+        struct list_head head;
+        struct timespec lag;
+        struct timespec lag_time;
+        spinlock_t lock;
+};
+
 struct vmw_fifo_state {
         unsigned long reserved_size;
         __le32 *dynamic_buffer;
@@ -115,6 +123,7 @@ struct vmw_fifo_state {
         uint32_t capabilities;
         struct mutex fifo_mutex;
         struct rw_semaphore rwsem;
+        struct vmw_fence_queue fence_queue;
 };
 
 struct vmw_relocation {
@@ -144,6 +153,14 @@ struct vmw_master {
         struct ttm_lock lock;
 };
 
+struct vmw_vga_topology_state {
+        uint32_t width;
+        uint32_t height;
+        uint32_t primary;
+        uint32_t pos_x;
+        uint32_t pos_y;
+};
+
 struct vmw_private {
         struct ttm_bo_device bdev;
         struct ttm_bo_global_ref bo_global_ref;
@@ -171,14 +188,19 @@ struct vmw_private {
          * VGA registers.
          */
 
+        struct vmw_vga_topology_state vga_save[VMWGFX_MAX_DISPLAYS];
         uint32_t vga_width;
         uint32_t vga_height;
         uint32_t vga_depth;
         uint32_t vga_bpp;
         uint32_t vga_pseudo;
         uint32_t vga_red_mask;
-        uint32_t vga_blue_mask;
         uint32_t vga_green_mask;
+        uint32_t vga_blue_mask;
+        uint32_t vga_bpl;
+        uint32_t vga_pitchlock;
+
+        uint32_t num_displays;
 
         /*
          * Framebuffer info.
@@ -393,6 +415,7 @@ extern int vmw_fifo_send_fence(struct vmw_private *dev_priv,
 extern void vmw_fifo_ping_host(struct vmw_private *dev_priv, uint32_t reason);
 extern int vmw_fifo_mmap(struct file *filp, struct vm_area_struct *vma);
 extern bool vmw_fifo_have_3d(struct vmw_private *dev_priv);
+extern bool vmw_fifo_have_pitchlock(struct vmw_private *dev_priv);
 
 /**
  * TTM glue - vmwgfx_ttm_glue.c
@@ -441,6 +464,23 @@ extern int vmw_fallback_wait(struct vmw_private *dev_priv,
                              uint32_t sequence,
                              bool interruptible,
                              unsigned long timeout);
+extern void vmw_update_sequence(struct vmw_private *dev_priv,
+                                struct vmw_fifo_state *fifo_state);
+
+
+/**
+ * Rudimentary fence objects currently used only for throttling -
+ * vmwgfx_fence.c
+ */
+
+extern void vmw_fence_queue_init(struct vmw_fence_queue *queue);
+extern void vmw_fence_queue_takedown(struct vmw_fence_queue *queue);
+extern int vmw_fence_push(struct vmw_fence_queue *queue,
+                          uint32_t sequence);
+extern int vmw_fence_pull(struct vmw_fence_queue *queue,
+                          uint32_t signaled_sequence);
+extern int vmw_wait_lag(struct vmw_private *dev_priv,
+                        struct vmw_fence_queue *queue, uint32_t us);
 
 /**
  * Kernel framebuffer - vmwgfx_fb.c
@@ -466,6 +506,11 @@ void vmw_kms_cursor_snoop(struct vmw_surface *srf,
                           struct ttm_object_file *tfile,
                           struct ttm_buffer_object *bo,
                           SVGA3dCmdHeader *header);
+void vmw_kms_write_svga(struct vmw_private *vmw_priv,
+                        unsigned width, unsigned height, unsigned pitch,
+                        unsigned bbp, unsigned depth);
+int vmw_kms_update_layout_ioctl(struct drm_device *dev, void *data,
+                                struct drm_file *file_priv);
 
 /**
  * Overlay control - vmwgfx_overlay.c
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
index dbd36b8910cf..8e396850513c 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
@@ -644,6 +644,7 @@ int vmw_execbuf_ioctl(struct drm_device *dev, void *data,
         ret = copy_from_user(cmd, user_cmd, arg->command_size);
 
         if (unlikely(ret != 0)) {
+                ret = -EFAULT;
                 DRM_ERROR("Failed copying commands.\n");
                 goto out_commit;
         }
@@ -669,6 +670,15 @@ int vmw_execbuf_ioctl(struct drm_device *dev, void *data,
                 goto out_err;
 
         vmw_apply_relocations(sw_context);
+
+        if (arg->throttle_us) {
+                ret = vmw_wait_lag(dev_priv, &dev_priv->fifo.fence_queue,
+                                   arg->throttle_us);
+
+                if (unlikely(ret != 0))
+                        goto out_err;
+        }
+
         vmw_fifo_commit(dev_priv, arg->command_size);
 
         ret = vmw_fifo_send_fence(dev_priv, &sequence);
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_fb.c b/drivers/gpu/drm/vmwgfx/vmwgfx_fb.c
index 7421aaad8d09..b0866f04ec76 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_fb.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_fb.c
@@ -132,16 +132,14 @@ static int vmw_fb_check_var(struct fb_var_screeninfo *var,
                 return -EINVAL;
         }
 
-        /* without multimon its hard to resize */
-        if (!(vmw_priv->capabilities & SVGA_CAP_MULTIMON) &&
-            (var->xres != par->max_width ||
-             var->yres != par->max_height)) {
-                DRM_ERROR("Tried to resize, but we don't have multimon\n");
+        if (!(vmw_priv->capabilities & SVGA_CAP_DISPLAY_TOPOLOGY) &&
+            (var->xoffset != 0 || var->yoffset != 0)) {
+                DRM_ERROR("Can not handle panning without display topology\n");
                 return -EINVAL;
         }
 
-        if (var->xres > par->max_width ||
-            var->yres > par->max_height) {
+        if ((var->xoffset + var->xres) > par->max_width ||
+            (var->yoffset + var->yres) > par->max_height) {
                 DRM_ERROR("Requested geom can not fit in framebuffer\n");
                 return -EINVAL;
         }
@@ -154,27 +152,11 @@ static int vmw_fb_set_par(struct fb_info *info)
         struct vmw_fb_par *par = info->par;
         struct vmw_private *vmw_priv = par->vmw_priv;
 
-        if (vmw_priv->capabilities & SVGA_CAP_MULTIMON) {
-                vmw_write(vmw_priv, SVGA_REG_NUM_GUEST_DISPLAYS, 1);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_ID, 0);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_IS_PRIMARY, true);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_POSITION_X, 0);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_POSITION_Y, 0);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_WIDTH, 0);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_HEIGHT, 0);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_ID, SVGA_ID_INVALID);
-
-                vmw_write(vmw_priv, SVGA_REG_ENABLE, 1);
-                vmw_write(vmw_priv, SVGA_REG_WIDTH, par->max_width);
-                vmw_write(vmw_priv, SVGA_REG_HEIGHT, par->max_height);
-                vmw_write(vmw_priv, SVGA_REG_BITS_PER_PIXEL, par->bpp);
-                vmw_write(vmw_priv, SVGA_REG_DEPTH, par->depth);
-                vmw_write(vmw_priv, SVGA_REG_RED_MASK, 0x00ff0000);
-                vmw_write(vmw_priv, SVGA_REG_GREEN_MASK, 0x0000ff00);
-                vmw_write(vmw_priv, SVGA_REG_BLUE_MASK, 0x000000ff);
-
+        vmw_kms_write_svga(vmw_priv, info->var.xres, info->var.yres,
+                           info->fix.line_length,
+                           par->bpp, par->depth);
+        if (vmw_priv->capabilities & SVGA_CAP_DISPLAY_TOPOLOGY) {
                 /* TODO check if pitch and offset changes */
-
                 vmw_write(vmw_priv, SVGA_REG_NUM_GUEST_DISPLAYS, 1);
                 vmw_write(vmw_priv, SVGA_REG_DISPLAY_ID, 0);
                 vmw_write(vmw_priv, SVGA_REG_DISPLAY_IS_PRIMARY, true);
@@ -183,13 +165,13 @@ static int vmw_fb_set_par(struct fb_info *info)
                 vmw_write(vmw_priv, SVGA_REG_DISPLAY_WIDTH, info->var.xres);
                 vmw_write(vmw_priv, SVGA_REG_DISPLAY_HEIGHT, info->var.yres);
                 vmw_write(vmw_priv, SVGA_REG_DISPLAY_ID, SVGA_ID_INVALID);
-        } else {
-                vmw_write(vmw_priv, SVGA_REG_WIDTH, info->var.xres);
-                vmw_write(vmw_priv, SVGA_REG_HEIGHT, info->var.yres);
-
-                /* TODO check if pitch and offset changes */
         }
 
+        /* This is really helpful since if this fails the user
+         * can probably not see anything on the screen.
+         */
+        WARN_ON(vmw_read(vmw_priv, SVGA_REG_FB_OFFSET) != 0);
+
         return 0;
 }
 
@@ -416,48 +398,23 @@ int vmw_fb_init(struct vmw_private *vmw_priv)
         unsigned fb_bbp, fb_depth, fb_offset, fb_pitch, fb_size;
         int ret;
 
+        /* XXX These shouldn't be hardcoded. */
         initial_width = 800;
         initial_height = 600;
 
         fb_bbp = 32;
         fb_depth = 24;
 
-        if (vmw_priv->capabilities & SVGA_CAP_MULTIMON) {
-                fb_width = min(vmw_priv->fb_max_width, (unsigned)2048);
-                fb_height = min(vmw_priv->fb_max_height, (unsigned)2048);
-        } else {
-                fb_width = min(vmw_priv->fb_max_width, initial_width);
-                fb_height = min(vmw_priv->fb_max_height, initial_height);
-        }
+        /* XXX As shouldn't these be as well. */
+        fb_width = min(vmw_priv->fb_max_width, (unsigned)2048);
+        fb_height = min(vmw_priv->fb_max_height, (unsigned)2048);
 
         initial_width = min(fb_width, initial_width);
         initial_height = min(fb_height, initial_height);
 
-        vmw_write(vmw_priv, SVGA_REG_WIDTH, fb_width);
-        vmw_write(vmw_priv, SVGA_REG_HEIGHT, fb_height);
-        vmw_write(vmw_priv, SVGA_REG_BITS_PER_PIXEL, fb_bbp);
-        vmw_write(vmw_priv, SVGA_REG_DEPTH, fb_depth);
-        vmw_write(vmw_priv, SVGA_REG_RED_MASK, 0x00ff0000);
-        vmw_write(vmw_priv, SVGA_REG_GREEN_MASK, 0x0000ff00);
-        vmw_write(vmw_priv, SVGA_REG_BLUE_MASK, 0x000000ff);
-
-        fb_size = vmw_read(vmw_priv, SVGA_REG_FB_SIZE);
+        fb_pitch = fb_width * fb_bbp / 8;
+        fb_size = fb_pitch * fb_height;
         fb_offset = vmw_read(vmw_priv, SVGA_REG_FB_OFFSET);
-        fb_pitch = vmw_read(vmw_priv, SVGA_REG_BYTES_PER_LINE);
-
-        DRM_DEBUG("width  %u\n", vmw_read(vmw_priv, SVGA_REG_MAX_WIDTH));
-        DRM_DEBUG("height %u\n", vmw_read(vmw_priv, SVGA_REG_MAX_HEIGHT));
-        DRM_DEBUG("width  %u\n", vmw_read(vmw_priv, SVGA_REG_WIDTH));
-        DRM_DEBUG("height %u\n", vmw_read(vmw_priv, SVGA_REG_HEIGHT));
-        DRM_DEBUG("bpp    %u\n", vmw_read(vmw_priv, SVGA_REG_BITS_PER_PIXEL));
-        DRM_DEBUG("depth  %u\n", vmw_read(vmw_priv, SVGA_REG_DEPTH));
-        DRM_DEBUG("bpl    %u\n", vmw_read(vmw_priv, SVGA_REG_BYTES_PER_LINE));
-        DRM_DEBUG("r mask %08x\n", vmw_read(vmw_priv, SVGA_REG_RED_MASK));
-        DRM_DEBUG("g mask %08x\n", vmw_read(vmw_priv, SVGA_REG_GREEN_MASK));
-        DRM_DEBUG("b mask %08x\n", vmw_read(vmw_priv, SVGA_REG_BLUE_MASK));
-        DRM_DEBUG("fb_offset 0x%08x\n", fb_offset);
-        DRM_DEBUG("fb_pitch  %u\n", fb_pitch);
-        DRM_DEBUG("fb_size   %u kiB\n", fb_size / 1024);
 
         info = framebuffer_alloc(sizeof(*par), device);
         if (!info)
@@ -659,6 +616,10 @@ int vmw_dmabuf_to_start_of_vram(struct vmw_private *vmw_priv,
                 goto err_unlock;
 
         ret = ttm_bo_validate(bo, &ne_placement, false, false, false);
+
+        /* Could probably bug on */
+        WARN_ON(bo->offset != 0);
+
         ttm_bo_unreserve(bo);
 err_unlock:
         ttm_write_unlock(&vmw_priv->active_master->lock);
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c b/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c
new file mode 100644
index 000000000000..61eacc1b5ca3
--- /dev/null
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c
@@ -0,0 +1,173 @@
+/**************************************************************************
+ *
+ * Copyright (C) 2010 VMware, Inc., Palo Alto, CA., USA
+ * All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ **************************************************************************/
+
+
+#include "vmwgfx_drv.h"
+
+struct vmw_fence {
+        struct list_head head;
+        uint32_t sequence;
+        struct timespec submitted;
+};
+
+void vmw_fence_queue_init(struct vmw_fence_queue *queue)
+{
+        INIT_LIST_HEAD(&queue->head);
+        queue->lag = ns_to_timespec(0);
+        getrawmonotonic(&queue->lag_time);
+        spin_lock_init(&queue->lock);
+}
+
+void vmw_fence_queue_takedown(struct vmw_fence_queue *queue)
+{
+        struct vmw_fence *fence, *next;
+
+        spin_lock(&queue->lock);
+        list_for_each_entry_safe(fence, next, &queue->head, head) {
+                kfree(fence);
+        }
+        spin_unlock(&queue->lock);
+}
+
+int vmw_fence_push(struct vmw_fence_queue *queue,
+                   uint32_t sequence)
+{
+        struct vmw_fence *fence = kmalloc(sizeof(*fence), GFP_KERNEL);
+
+        if (unlikely(!fence))
+                return -ENOMEM;
+
+        fence->sequence = sequence;
+        getrawmonotonic(&fence->submitted);
+        spin_lock(&queue->lock);
+        list_add_tail(&fence->head, &queue->head);
+        spin_unlock(&queue->lock);
+
+        return 0;
+}
+
+int vmw_fence_pull(struct vmw_fence_queue *queue,
+                   uint32_t signaled_sequence)
+{
+        struct vmw_fence *fence, *next;
+        struct timespec now;
+        bool updated = false;
+
+        spin_lock(&queue->lock);
+        getrawmonotonic(&now);
+
+        if (list_empty(&queue->head)) {
+                queue->lag = ns_to_timespec(0);
+                queue->lag_time = now;
+                updated = true;
+                goto out_unlock;
+        }
+
+        list_for_each_entry_safe(fence, next, &queue->head, head) {
+                if (signaled_sequence - fence->sequence > (1 << 30))
+                        continue;
+
+                queue->lag = timespec_sub(now, fence->submitted);
+                queue->lag_time = now;
+                updated = true;
+                list_del(&fence->head);
+                kfree(fence);
+        }
+
+out_unlock:
+        spin_unlock(&queue->lock);
+
+        return (updated) ? 0 : -EBUSY;
+}
+
+static struct timespec vmw_timespec_add(struct timespec t1,
+                                        struct timespec t2)
+{
+        t1.tv_sec += t2.tv_sec;
+        t1.tv_nsec += t2.tv_nsec;
+        if (t1.tv_nsec >= 1000000000L) {
+                t1.tv_sec += 1;
+                t1.tv_nsec -= 1000000000L;
+        }
+
+        return t1;
+}
+
+static struct timespec vmw_fifo_lag(struct vmw_fence_queue *queue)
+{
+        struct timespec now;
+
+        spin_lock(&queue->lock);
+        getrawmonotonic(&now);
+        queue->lag = vmw_timespec_add(queue->lag,
+                                      timespec_sub(now, queue->lag_time));
+        queue->lag_time = now;
+        spin_unlock(&queue->lock);
+        return queue->lag;
+}
+
+
+static bool vmw_lag_lt(struct vmw_fence_queue *queue,
+                       uint32_t us)
+{
+        struct timespec lag, cond;
+
+        cond = ns_to_timespec((s64) us * 1000);
+        lag = vmw_fifo_lag(queue);
+        return (timespec_compare(&lag, &cond) < 1);
+}
+
+int vmw_wait_lag(struct vmw_private *dev_priv,
+                 struct vmw_fence_queue *queue, uint32_t us)
+{
+        struct vmw_fence *fence;
+        uint32_t sequence;
+        int ret;
+
+        while (!vmw_lag_lt(queue, us)) {
+                spin_lock(&queue->lock);
+                if (list_empty(&queue->head))
+                        sequence = atomic_read(&dev_priv->fence_seq);
+                else {
+                        fence = list_first_entry(&queue->head,
+                                                 struct vmw_fence, head);
+                        sequence = fence->sequence;
+                }
+                spin_unlock(&queue->lock);
+
+                ret = vmw_wait_fence(dev_priv, false, sequence, true,
+                                     3*HZ);
+
+                if (unlikely(ret != 0))
+                        return ret;
+
+                (void) vmw_fence_pull(queue, sequence);
+        }
+        return 0;
+}
+
+
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_fifo.c b/drivers/gpu/drm/vmwgfx/vmwgfx_fifo.c
index 39d43a01d846..e6a1eb7ea954 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_fifo.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_fifo.c
@@ -34,6 +34,9 @@ bool vmw_fifo_have_3d(struct vmw_private *dev_priv)
         __le32 __iomem *fifo_mem = dev_priv->mmio_virt;
         uint32_t fifo_min, hwversion;
 
+        if (!(dev_priv->capabilities & SVGA_CAP_EXTENDED_FIFO))
+                return false;
+
         fifo_min = ioread32(fifo_mem  + SVGA_FIFO_MIN);
         if (fifo_min <= SVGA_FIFO_3D_HWVERSION * sizeof(unsigned int))
                 return false;
@@ -48,6 +51,21 @@ bool vmw_fifo_have_3d(struct vmw_private *dev_priv)
         return true;
 }
 
+bool vmw_fifo_have_pitchlock(struct vmw_private *dev_priv)
+{
+        __le32 __iomem *fifo_mem = dev_priv->mmio_virt;
+        uint32_t caps;
+
+        if (!(dev_priv->capabilities & SVGA_CAP_EXTENDED_FIFO))
+                return false;
+
+        caps = ioread32(fifo_mem + SVGA_FIFO_CAPABILITIES);
+        if (caps & SVGA_FIFO_CAP_PITCHLOCK)
+                return true;
+
+        return false;
+}
+
 int vmw_fifo_init(struct vmw_private *dev_priv, struct vmw_fifo_state *fifo)
 {
         __le32 __iomem *fifo_mem = dev_priv->mmio_virt;
@@ -120,7 +138,7 @@ int vmw_fifo_init(struct vmw_private *dev_priv, struct vmw_fifo_state *fifo)
 
         atomic_set(&dev_priv->fence_seq, dev_priv->last_read_sequence);
         iowrite32(dev_priv->last_read_sequence, fifo_mem + SVGA_FIFO_FENCE);
-
+        vmw_fence_queue_init(&fifo->fence_queue);
         return vmw_fifo_send_fence(dev_priv, &dummy);
 out_err:
         vfree(fifo->static_buffer);
@@ -159,6 +177,7 @@ void vmw_fifo_release(struct vmw_private *dev_priv, struct vmw_fifo_state *fifo)
                   dev_priv->enable_state);
 
         mutex_unlock(&dev_priv->hw_mutex);
+        vmw_fence_queue_takedown(&fifo->fence_queue);
 
         if (likely(fifo->last_buffer != NULL)) {
                 vfree(fifo->last_buffer);
@@ -484,6 +503,8 @@ int vmw_fifo_send_fence(struct vmw_private *dev_priv, uint32_t *sequence)
         fifo_state->last_buffer_add = true;
         vmw_fifo_commit(dev_priv, bytes);
         fifo_state->last_buffer_add = false;
+        (void) vmw_fence_push(&fifo_state->fence_queue, *sequence);
+        vmw_update_sequence(dev_priv, fifo_state);
 
 out_err:
         return ret;
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_irq.c b/drivers/gpu/drm/vmwgfx/vmwgfx_irq.c
index 4d7cb5393860..e92298a6a383 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_irq.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_irq.c
@@ -64,22 +64,33 @@ static bool vmw_fifo_idle(struct vmw_private *dev_priv, uint32_t sequence)
         return (busy == 0);
 }
 
+void vmw_update_sequence(struct vmw_private *dev_priv,
+                         struct vmw_fifo_state *fifo_state)
+{
+        __le32 __iomem *fifo_mem = dev_priv->mmio_virt;
+
+        uint32_t sequence = ioread32(fifo_mem + SVGA_FIFO_FENCE);
+
+        if (dev_priv->last_read_sequence != sequence) {
+                dev_priv->last_read_sequence = sequence;
+                vmw_fence_pull(&fifo_state->fence_queue, sequence);
+        }
+}
 
 bool vmw_fence_signaled(struct vmw_private *dev_priv,
                         uint32_t sequence)
 {
-        __le32 __iomem *fifo_mem = dev_priv->mmio_virt;
         struct vmw_fifo_state *fifo_state;
         bool ret;
 
         if (likely(dev_priv->last_read_sequence - sequence < VMW_FENCE_WRAP))
                 return true;
 
-        dev_priv->last_read_sequence = ioread32(fifo_mem + SVGA_FIFO_FENCE);
+        fifo_state = &dev_priv->fifo;
+        vmw_update_sequence(dev_priv, fifo_state);
         if (likely(dev_priv->last_read_sequence - sequence < VMW_FENCE_WRAP))
                 return true;
 
-        fifo_state = &dev_priv->fifo;
         if (!(fifo_state->capabilities & SVGA_FIFO_CAP_FENCE) &&
             vmw_fifo_idle(dev_priv, sequence))
                 return true;
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
index bbc7c4c30bc7..f1d626112415 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
@@ -30,6 +30,8 @@
 /* Might need a hrtimer here? */
 #define VMWGFX_PRESENT_RATE ((HZ / 60 > 0) ? HZ / 60 : 1)
 
+static int vmw_surface_dmabuf_pin(struct vmw_framebuffer *vfb);
+static int vmw_surface_dmabuf_unpin(struct vmw_framebuffer *vfb);
 
 void vmw_display_unit_cleanup(struct vmw_display_unit *du)
 {
@@ -326,6 +328,7 @@ int vmw_framebuffer_create_handle(struct drm_framebuffer *fb,
 struct vmw_framebuffer_surface {
         struct vmw_framebuffer base;
         struct vmw_surface *surface;
+        struct vmw_dma_buffer *buffer;
         struct delayed_work d_work;
         struct mutex work_lock;
         bool present_fs;
@@ -500,8 +503,8 @@ int vmw_kms_new_framebuffer_surface(struct vmw_private *dev_priv,
         vfbs->base.base.depth = 24;
         vfbs->base.base.width = width;
         vfbs->base.base.height = height;
-        vfbs->base.pin = NULL;
-        vfbs->base.unpin = NULL;
+        vfbs->base.pin = &vmw_surface_dmabuf_pin;
+        vfbs->base.unpin = &vmw_surface_dmabuf_unpin;
         vfbs->surface = surface;
         mutex_init(&vfbs->work_lock);
         INIT_DELAYED_WORK(&vfbs->d_work, &vmw_framebuffer_present_fs_callback);
@@ -589,6 +592,40 @@ static struct drm_framebuffer_funcs vmw_framebuffer_dmabuf_funcs = {
         .create_handle = vmw_framebuffer_create_handle,
 };
 
+static int vmw_surface_dmabuf_pin(struct vmw_framebuffer *vfb)
+{
+        struct vmw_private *dev_priv = vmw_priv(vfb->base.dev);
+        struct vmw_framebuffer_surface *vfbs =
+                vmw_framebuffer_to_vfbs(&vfb->base);
+        unsigned long size = vfbs->base.base.pitch * vfbs->base.base.height;
+        int ret;
+
+        vfbs->buffer = kzalloc(sizeof(*vfbs->buffer), GFP_KERNEL);
+        if (unlikely(vfbs->buffer == NULL))
+                return -ENOMEM;
+
+        vmw_overlay_pause_all(dev_priv);
+        ret = vmw_dmabuf_init(dev_priv, vfbs->buffer, size,
+                               &vmw_vram_ne_placement,
+                               false, &vmw_dmabuf_bo_free);
+        vmw_overlay_resume_all(dev_priv);
+
+        return ret;
+}
+
+static int vmw_surface_dmabuf_unpin(struct vmw_framebuffer *vfb)
+{
+        struct ttm_buffer_object *bo;
+        struct vmw_framebuffer_surface *vfbs =
+                vmw_framebuffer_to_vfbs(&vfb->base);
+
+        bo = &vfbs->buffer->base;
+        ttm_bo_unref(&bo);
+        vfbs->buffer = NULL;
+
+        return 0;
+}
+
 static int vmw_framebuffer_dmabuf_pin(struct vmw_framebuffer *vfb)
 {
         struct vmw_private *dev_priv = vmw_priv(vfb->base.dev);
@@ -596,33 +633,15 @@ static int vmw_framebuffer_dmabuf_pin(struct vmw_framebuffer *vfb)
                 vmw_framebuffer_to_vfbd(&vfb->base);
         int ret;
 
+
         vmw_overlay_pause_all(dev_priv);
 
         ret = vmw_dmabuf_to_start_of_vram(dev_priv, vfbd->buffer);
 
-        if (dev_priv->capabilities & SVGA_CAP_MULTIMON) {
-                vmw_write(dev_priv, SVGA_REG_NUM_GUEST_DISPLAYS, 1);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_ID, 0);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_IS_PRIMARY, true);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_POSITION_X, 0);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_POSITION_Y, 0);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_WIDTH, 0);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_HEIGHT, 0);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_ID, SVGA_ID_INVALID);
-
-                vmw_write(dev_priv, SVGA_REG_ENABLE, 1);
-                vmw_write(dev_priv, SVGA_REG_WIDTH, vfb->base.width);
-                vmw_write(dev_priv, SVGA_REG_HEIGHT, vfb->base.height);
-                vmw_write(dev_priv, SVGA_REG_BITS_PER_PIXEL, vfb->base.bits_per_pixel);
-                vmw_write(dev_priv, SVGA_REG_DEPTH, vfb->base.depth);
-                vmw_write(dev_priv, SVGA_REG_RED_MASK, 0x00ff0000);
-                vmw_write(dev_priv, SVGA_REG_GREEN_MASK, 0x0000ff00);
-                vmw_write(dev_priv, SVGA_REG_BLUE_MASK, 0x000000ff);
-        } else
-                WARN_ON(true);
-
         vmw_overlay_resume_all(dev_priv);
 
+        WARN_ON(ret != 0);
+
         return 0;
 }
 
@@ -668,7 +687,7 @@ int vmw_kms_new_framebuffer_dmabuf(struct vmw_private *dev_priv,
 
         /* XXX get the first 3 from the surface info */
         vfbd->base.base.bits_per_pixel = 32;
-        vfbd->base.base.pitch = width * 32 / 4;
+        vfbd->base.base.pitch = width * vfbd->base.base.bits_per_pixel / 8;
         vfbd->base.base.depth = 24;
         vfbd->base.base.width = width;
         vfbd->base.base.height = height;
@@ -765,8 +784,9 @@ int vmw_kms_init(struct vmw_private *dev_priv)
         dev->mode_config.funcs = &vmw_kms_funcs;
         dev->mode_config.min_width = 1;
         dev->mode_config.min_height = 1;
-        dev->mode_config.max_width = dev_priv->fb_max_width;
-        dev->mode_config.max_height = dev_priv->fb_max_height;
+        /* assumed largest fb size */
+        dev->mode_config.max_width = 8192;
+        dev->mode_config.max_height = 8192;
 
         ret = vmw_kms_init_legacy_display_system(dev_priv);
 
@@ -826,49 +846,140 @@ out:
         return ret;
 }
 
+void vmw_kms_write_svga(struct vmw_private *vmw_priv,
+                        unsigned width, unsigned height, unsigned pitch,
+                        unsigned bbp, unsigned depth)
+{
+        if (vmw_priv->capabilities & SVGA_CAP_PITCHLOCK)
+                vmw_write(vmw_priv, SVGA_REG_PITCHLOCK, pitch);
+        else if (vmw_fifo_have_pitchlock(vmw_priv))
+                iowrite32(pitch, vmw_priv->mmio_virt + SVGA_FIFO_PITCHLOCK);
+        vmw_write(vmw_priv, SVGA_REG_WIDTH, width);
+        vmw_write(vmw_priv, SVGA_REG_HEIGHT, height);
+        vmw_write(vmw_priv, SVGA_REG_BITS_PER_PIXEL, bbp);
+        vmw_write(vmw_priv, SVGA_REG_DEPTH, depth);
+        vmw_write(vmw_priv, SVGA_REG_RED_MASK, 0x00ff0000);
+        vmw_write(vmw_priv, SVGA_REG_GREEN_MASK, 0x0000ff00);
+        vmw_write(vmw_priv, SVGA_REG_BLUE_MASK, 0x000000ff);
+}
+
 int vmw_kms_save_vga(struct vmw_private *vmw_priv)
 {
-        /*
-         * setup a single multimon monitor with the size
-         * of 0x0, this stops the UI from resizing when we
-         * change the framebuffer size
-         */
-        if (vmw_priv->capabilities & SVGA_CAP_MULTIMON) {
-                vmw_write(vmw_priv, SVGA_REG_NUM_GUEST_DISPLAYS, 1);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_ID, 0);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_IS_PRIMARY, true);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_POSITION_X, 0);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_POSITION_Y, 0);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_WIDTH, 0);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_HEIGHT, 0);
-                vmw_write(vmw_priv, SVGA_REG_DISPLAY_ID, SVGA_ID_INVALID);
-        }
+        struct vmw_vga_topology_state *save;
+        uint32_t i;
 
         vmw_priv->vga_width = vmw_read(vmw_priv, SVGA_REG_WIDTH);
         vmw_priv->vga_height = vmw_read(vmw_priv, SVGA_REG_HEIGHT);
-        vmw_priv->vga_bpp = vmw_read(vmw_priv, SVGA_REG_BITS_PER_PIXEL);
         vmw_priv->vga_depth = vmw_read(vmw_priv, SVGA_REG_DEPTH);
+        vmw_priv->vga_bpp = vmw_read(vmw_priv, SVGA_REG_BITS_PER_PIXEL);
         vmw_priv->vga_pseudo = vmw_read(vmw_priv, SVGA_REG_PSEUDOCOLOR);
         vmw_priv->vga_red_mask = vmw_read(vmw_priv, SVGA_REG_RED_MASK);
-        vmw_priv->vga_green_mask = vmw_read(vmw_priv, SVGA_REG_GREEN_MASK);
         vmw_priv->vga_blue_mask = vmw_read(vmw_priv, SVGA_REG_BLUE_MASK);
+        vmw_priv->vga_green_mask = vmw_read(vmw_priv, SVGA_REG_GREEN_MASK);
+        if (vmw_priv->capabilities & SVGA_CAP_PITCHLOCK)
+                vmw_priv->vga_pitchlock =
+                  vmw_read(vmw_priv, SVGA_REG_PITCHLOCK);
+        else if (vmw_fifo_have_pitchlock(vmw_priv))
+                vmw_priv->vga_pitchlock = ioread32(vmw_priv->mmio_virt +
+                                                       SVGA_FIFO_PITCHLOCK);
+
+        if (!(vmw_priv->capabilities & SVGA_CAP_DISPLAY_TOPOLOGY))
+                return 0;
 
+        vmw_priv->num_displays = vmw_read(vmw_priv,
+                                          SVGA_REG_NUM_GUEST_DISPLAYS);
+
+        for (i = 0; i < vmw_priv->num_displays; ++i) {
+                save = &vmw_priv->vga_save[i];
+                vmw_write(vmw_priv, SVGA_REG_DISPLAY_ID, i);
+                save->primary = vmw_read(vmw_priv, SVGA_REG_DISPLAY_IS_PRIMARY);
+                save->pos_x = vmw_read(vmw_priv, SVGA_REG_DISPLAY_POSITION_X);
+                save->pos_y = vmw_read(vmw_priv, SVGA_REG_DISPLAY_POSITION_Y);
+                save->width = vmw_read(vmw_priv, SVGA_REG_DISPLAY_WIDTH);
+                save->height = vmw_read(vmw_priv, SVGA_REG_DISPLAY_HEIGHT);
+                vmw_write(vmw_priv, SVGA_REG_DISPLAY_ID, SVGA_ID_INVALID);
+        }
         return 0;
 }
 
 int vmw_kms_restore_vga(struct vmw_private *vmw_priv)
 {
+        struct vmw_vga_topology_state *save;
+        uint32_t i;
+
         vmw_write(vmw_priv, SVGA_REG_WIDTH, vmw_priv->vga_width);
         vmw_write(vmw_priv, SVGA_REG_HEIGHT, vmw_priv->vga_height);
-        vmw_write(vmw_priv, SVGA_REG_BITS_PER_PIXEL, vmw_priv->vga_bpp);
         vmw_write(vmw_priv, SVGA_REG_DEPTH, vmw_priv->vga_depth);
+        vmw_write(vmw_priv, SVGA_REG_BITS_PER_PIXEL, vmw_priv->vga_bpp);
         vmw_write(vmw_priv, SVGA_REG_PSEUDOCOLOR, vmw_priv->vga_pseudo);
         vmw_write(vmw_priv, SVGA_REG_RED_MASK, vmw_priv->vga_red_mask);
         vmw_write(vmw_priv, SVGA_REG_GREEN_MASK, vmw_priv->vga_green_mask);
         vmw_write(vmw_priv, SVGA_REG_BLUE_MASK, vmw_priv->vga_blue_mask);
+        if (vmw_priv->capabilities & SVGA_CAP_PITCHLOCK)
+                vmw_write(vmw_priv, SVGA_REG_PITCHLOCK,
+                          vmw_priv->vga_pitchlock);
+        else if (vmw_fifo_have_pitchlock(vmw_priv))
+                iowrite32(vmw_priv->vga_pitchlock,
+                          vmw_priv->mmio_virt + SVGA_FIFO_PITCHLOCK);
+
+        if (!(vmw_priv->capabilities & SVGA_CAP_DISPLAY_TOPOLOGY))
+                return 0;
 
-        /* TODO check for multimon */
-        vmw_write(vmw_priv, SVGA_REG_NUM_GUEST_DISPLAYS, 0);
+        for (i = 0; i < vmw_priv->num_displays; ++i) {
+                save = &vmw_priv->vga_save[i];
+                vmw_write(vmw_priv, SVGA_REG_DISPLAY_ID, i);
+                vmw_write(vmw_priv, SVGA_REG_DISPLAY_IS_PRIMARY, save->primary);
+                vmw_write(vmw_priv, SVGA_REG_DISPLAY_POSITION_X, save->pos_x);
+                vmw_write(vmw_priv, SVGA_REG_DISPLAY_POSITION_Y, save->pos_y);
+                vmw_write(vmw_priv, SVGA_REG_DISPLAY_WIDTH, save->width);
+                vmw_write(vmw_priv, SVGA_REG_DISPLAY_HEIGHT, save->height);
+                vmw_write(vmw_priv, SVGA_REG_DISPLAY_ID, SVGA_ID_INVALID);
+        }
 
         return 0;
 }
+
+int vmw_kms_update_layout_ioctl(struct drm_device *dev, void *data,
+                                struct drm_file *file_priv)
+{
+        struct vmw_private *dev_priv = vmw_priv(dev);
+        struct drm_vmw_update_layout_arg *arg =
+                (struct drm_vmw_update_layout_arg *)data;
+        struct vmw_master *vmaster = vmw_master(file_priv->master);
+        void __user *user_rects;
+        struct drm_vmw_rect *rects;
+        unsigned rects_size;
+        int ret;
+
+        ret = ttm_read_lock(&vmaster->lock, true);
+        if (unlikely(ret != 0))
+                return ret;
+
+        if (!arg->num_outputs) {
+                struct drm_vmw_rect def_rect = {0, 0, 800, 600};
+                vmw_kms_ldu_update_layout(dev_priv, 1, &def_rect);
+                goto out_unlock;
+        }
+
+        rects_size = arg->num_outputs * sizeof(struct drm_vmw_rect);
+        rects = kzalloc(rects_size, GFP_KERNEL);
+        if (unlikely(!rects)) {
+                ret = -ENOMEM;
+                goto out_unlock;
+        }
+
+        user_rects = (void __user *)(unsigned long)arg->rects;
+        ret = copy_from_user(rects, user_rects, rects_size);
+        if (unlikely(ret != 0)) {
+                DRM_ERROR("Failed to get rects.\n");
+                goto out_free;
+        }
+
+        vmw_kms_ldu_update_layout(dev_priv, arg->num_outputs, rects);
+
+out_free:
+        kfree(rects);
+out_unlock:
+        ttm_read_unlock(&vmaster->lock);
+        return ret;
+}
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.h b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.h
index 8b95249f0531..8a398a0339b6 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.h
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.h
@@ -94,9 +94,11 @@ int vmw_du_crtc_cursor_set(struct drm_crtc *crtc, struct drm_file *file_priv,
 int vmw_du_crtc_cursor_move(struct drm_crtc *crtc, int x, int y);
 
 /*
- * Legacy display unit functions - vmwgfx_ldu.h
+ * Legacy display unit functions - vmwgfx_ldu.c
  */
 int vmw_kms_init_legacy_display_system(struct vmw_private *dev_priv);
 int vmw_kms_close_legacy_display_system(struct vmw_private *dev_priv);
+int vmw_kms_ldu_update_layout(struct vmw_private *dev_priv, unsigned num,
+                              struct drm_vmw_rect *rects);
 
 #endif
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c b/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
index 90891593bf6c..cfaf690a5b2f 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
@@ -38,6 +38,7 @@ struct vmw_legacy_display {
         struct list_head active;
 
         unsigned num_active;
+        unsigned last_num_active;
 
         struct vmw_framebuffer *fb;
 };
@@ -48,9 +49,12 @@ struct vmw_legacy_display {
 struct vmw_legacy_display_unit {
         struct vmw_display_unit base;
 
-        struct list_head active;
+        unsigned pref_width;
+        unsigned pref_height;
+        bool pref_active;
+        struct drm_display_mode *pref_mode;
 
-        unsigned unit;
+        struct list_head active;
 };
 
 static void vmw_ldu_destroy(struct vmw_legacy_display_unit *ldu)
@@ -88,23 +92,44 @@ static int vmw_ldu_commit_list(struct vmw_private *dev_priv)
 {
         struct vmw_legacy_display *lds = dev_priv->ldu_priv;
         struct vmw_legacy_display_unit *entry;
-        struct drm_crtc *crtc;
+        struct drm_framebuffer *fb = NULL;
+        struct drm_crtc *crtc = NULL;
         int i = 0;
 
-        /* to stop the screen from changing size on resize */
-        vmw_write(dev_priv, SVGA_REG_NUM_GUEST_DISPLAYS, 0);
-        for (i = 0; i < lds->num_active; i++) {
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_ID, i);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_IS_PRIMARY, !i);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_POSITION_X, 0);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_POSITION_Y, 0);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_WIDTH, 0);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_HEIGHT, 0);
-                vmw_write(dev_priv, SVGA_REG_DISPLAY_ID, SVGA_ID_INVALID);
+        /* If there is no display topology the host just assumes
+         * that the guest will set the same layout as the host.
+         */
+        if (!(dev_priv->capabilities & SVGA_CAP_DISPLAY_TOPOLOGY)) {
+                int w = 0, h = 0;
+                list_for_each_entry(entry, &lds->active, active) {
+                        crtc = &entry->base.crtc;
+                        w = max(w, crtc->x + crtc->mode.hdisplay);
+                        h = max(h, crtc->y + crtc->mode.vdisplay);
+                        i++;
+                }
+
+                if (crtc == NULL)
+                        return 0;
+                fb = entry->base.crtc.fb;
+
+                vmw_kms_write_svga(dev_priv, w, h, fb->pitch,
+                                   fb->bits_per_pixel, fb->depth);
+
+                return 0;
         }
 
-        /* Now set the mode */
-        vmw_write(dev_priv, SVGA_REG_NUM_GUEST_DISPLAYS, lds->num_active);
+        if (!list_empty(&lds->active)) {
+                entry = list_entry(lds->active.next, typeof(*entry), active);
+                fb = entry->base.crtc.fb;
+
+                vmw_kms_write_svga(dev_priv, fb->width, fb->height, fb->pitch,
+                                   fb->bits_per_pixel, fb->depth);
+        }
+
+        /* Make sure we always show something. */
+        vmw_write(dev_priv, SVGA_REG_NUM_GUEST_DISPLAYS,
+                  lds->num_active ? lds->num_active : 1);
+
         i = 0;
         list_for_each_entry(entry, &lds->active, active) {
                 crtc = &entry->base.crtc;
@@ -120,6 +145,10 @@ static int vmw_ldu_commit_list(struct vmw_private *dev_priv)
                 i++;
         }
 
+        BUG_ON(i != lds->num_active);
+
+        lds->last_num_active = lds->num_active;
+
         return 0;
 }
 
@@ -130,6 +159,7 @@ static int vmw_ldu_del_active(struct vmw_private *vmw_priv,
         if (list_empty(&ldu->active))
                 return 0;
 
+        /* Must init otherwise list_empty(&ldu->active) will not work. */
         list_del_init(&ldu->active);
         if (--(ld->num_active) == 0) {
                 BUG_ON(!ld->fb);
@@ -149,24 +179,29 @@ static int vmw_ldu_add_active(struct vmw_private *vmw_priv,
         struct vmw_legacy_display_unit *entry;
         struct list_head *at;
 
+        BUG_ON(!ld->num_active && ld->fb);
+        if (vfb != ld->fb) {
+                if (ld->fb && ld->fb->unpin)
+                        ld->fb->unpin(ld->fb);
+                if (vfb->pin)
+                        vfb->pin(vfb);
+                ld->fb = vfb;
+        }
+
         if (!list_empty(&ldu->active))
                 return 0;
 
         at = &ld->active;
         list_for_each_entry(entry, &ld->active, active) {
-                if (entry->unit > ldu->unit)
+                if (entry->base.unit > ldu->base.unit)
                         break;
 
                 at = &entry->active;
         }
 
         list_add(&ldu->active, at);
-        if (ld->num_active++ == 0) {
-                BUG_ON(ld->fb);
-                if (vfb->pin)
-                        vfb->pin(vfb);
-                ld->fb = vfb;
-        }
+
+        ld->num_active++;
 
         return 0;
 }
@@ -208,6 +243,8 @@ static int vmw_ldu_crtc_set_config(struct drm_mode_set *set)
 
         /* ldu only supports one fb active at the time */
         if (dev_priv->ldu_priv->fb && vfb &&
+            !(dev_priv->ldu_priv->num_active == 1 &&
+              !list_empty(&ldu->active)) &&
             dev_priv->ldu_priv->fb != vfb) {
                 DRM_ERROR("Multiple framebuffers not supported\n");
                 return -EINVAL;
@@ -300,8 +337,7 @@ static void vmw_ldu_connector_restore(struct drm_connector *connector)
 static enum drm_connector_status
         vmw_ldu_connector_detect(struct drm_connector *connector)
 {
-        /* XXX vmwctrl should control connection status */
-        if (vmw_connector_to_ldu(connector)->base.unit == 0)
+        if (vmw_connector_to_ldu(connector)->pref_active)
                 return connector_status_connected;
         return connector_status_disconnected;
 }
@@ -312,10 +348,9 @@ static struct drm_display_mode vmw_ldu_connector_builtin[] = {
                    752, 800, 0, 480, 489, 492, 525, 0,
                    DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC) },
         /* 800x600@60Hz */
-        { DRM_MODE("800x600",
-                   DRM_MODE_TYPE_DRIVER | DRM_MODE_TYPE_PREFERRED,
-                   40000, 800, 840, 968, 1056, 0, 600, 601, 605, 628,
-                   0, DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
+        { DRM_MODE("800x600", DRM_MODE_TYPE_DRIVER, 40000, 800, 840,
+                   968, 1056, 0, 600, 601, 605, 628, 0,
+                   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC) },
         /* 1024x768@60Hz */
         { DRM_MODE("1024x768", DRM_MODE_TYPE_DRIVER, 65000, 1024, 1048,
                    1184, 1344, 0, 768, 771, 777, 806, 0,
@@ -387,10 +422,34 @@ static struct drm_display_mode vmw_ldu_connector_builtin[] = {
 static int vmw_ldu_connector_fill_modes(struct drm_connector *connector,
                                         uint32_t max_width, uint32_t max_height)
 {
+        struct vmw_legacy_display_unit *ldu = vmw_connector_to_ldu(connector);
         struct drm_device *dev = connector->dev;
         struct drm_display_mode *mode = NULL;
+        struct drm_display_mode prefmode = { DRM_MODE("preferred",
+                DRM_MODE_TYPE_DRIVER | DRM_MODE_TYPE_PREFERRED,
+                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_PVSYNC)
+        };
         int i;
 
+        /* Add preferred mode */
+        {
+                mode = drm_mode_duplicate(dev, &prefmode);
+                if (!mode)
+                        return 0;
+                mode->hdisplay = ldu->pref_width;
+                mode->vdisplay = ldu->pref_height;
+                mode->vrefresh = drm_mode_vrefresh(mode);
+                drm_mode_probed_add(connector, mode);
+
+                if (ldu->pref_mode) {
+                        list_del_init(&ldu->pref_mode->head);
+                        drm_mode_destroy(dev, ldu->pref_mode);
+                }
+
+                ldu->pref_mode = mode;
+        }
+
         for (i = 0; vmw_ldu_connector_builtin[i].type != 0; i++) {
                 if (vmw_ldu_connector_builtin[i].hdisplay > max_width ||
                     vmw_ldu_connector_builtin[i].vdisplay > max_height)
@@ -443,18 +502,21 @@ static int vmw_ldu_init(struct vmw_private *dev_priv, unsigned unit)
         if (!ldu)
                 return -ENOMEM;
 
-        ldu->unit = unit;
+        ldu->base.unit = unit;
         crtc = &ldu->base.crtc;
         encoder = &ldu->base.encoder;
         connector = &ldu->base.connector;
 
+        INIT_LIST_HEAD(&ldu->active);
+
+        ldu->pref_active = (unit == 0);
+        ldu->pref_width = 800;
+        ldu->pref_height = 600;
+        ldu->pref_mode = NULL;
+
         drm_connector_init(dev, connector, &vmw_legacy_connector_funcs,
                            DRM_MODE_CONNECTOR_LVDS);
-        /* Initial status */
-        if (unit == 0)
-                connector->status = connector_status_connected;
-        else
-                connector->status = connector_status_disconnected;
+        connector->status = vmw_ldu_connector_detect(connector);
 
         drm_encoder_init(dev, encoder, &vmw_legacy_encoder_funcs,
                          DRM_MODE_ENCODER_LVDS);
@@ -462,8 +524,6 @@ static int vmw_ldu_init(struct vmw_private *dev_priv, unsigned unit)
         encoder->possible_crtcs = (1 << unit);
         encoder->possible_clones = 0;
 
-        INIT_LIST_HEAD(&ldu->active);
-
         drm_crtc_init(dev, crtc, &vmw_legacy_crtc_funcs);
 
         drm_connector_attach_property(connector,
@@ -487,18 +547,22 @@ int vmw_kms_init_legacy_display_system(struct vmw_private *dev_priv)
 
         INIT_LIST_HEAD(&dev_priv->ldu_priv->active);
         dev_priv->ldu_priv->num_active = 0;
+        dev_priv->ldu_priv->last_num_active = 0;
         dev_priv->ldu_priv->fb = NULL;
 
         drm_mode_create_dirty_info_property(dev_priv->dev);
 
         vmw_ldu_init(dev_priv, 0);
-        vmw_ldu_init(dev_priv, 1);
-        vmw_ldu_init(dev_priv, 2);
-        vmw_ldu_init(dev_priv, 3);
-        vmw_ldu_init(dev_priv, 4);
-        vmw_ldu_init(dev_priv, 5);
-        vmw_ldu_init(dev_priv, 6);
-        vmw_ldu_init(dev_priv, 7);
+        /* for old hardware without multimon only enable one display */
+        if (dev_priv->capabilities & SVGA_CAP_MULTIMON) {
+                vmw_ldu_init(dev_priv, 1);
+                vmw_ldu_init(dev_priv, 2);
+                vmw_ldu_init(dev_priv, 3);
+                vmw_ldu_init(dev_priv, 4);
+                vmw_ldu_init(dev_priv, 5);
+                vmw_ldu_init(dev_priv, 6);
+                vmw_ldu_init(dev_priv, 7);
+        }
 
         return 0;
 }
@@ -514,3 +578,42 @@ int vmw_kms_close_legacy_display_system(struct vmw_private *dev_priv)
 
         return 0;
 }
+
+int vmw_kms_ldu_update_layout(struct vmw_private *dev_priv, unsigned num,
+                              struct drm_vmw_rect *rects)
+{
+        struct drm_device *dev = dev_priv->dev;
+        struct vmw_legacy_display_unit *ldu;
+        struct drm_connector *con;
+        int i;
+
+        mutex_lock(&dev->mode_config.mutex);
+
+#if 0
+        DRM_INFO("%s: new layout ", __func__);
+        for (i = 0; i < (int)num; i++)
+                DRM_INFO("(%i, %i %ux%u) ", rects[i].x, rects[i].y,
+                         rects[i].w, rects[i].h);
+        DRM_INFO("\n");
+#else
+        (void)i;
+#endif
+
+        list_for_each_entry(con, &dev->mode_config.connector_list, head) {
+                ldu = vmw_connector_to_ldu(con);
+                if (num > ldu->base.unit) {
+                        ldu->pref_width = rects[ldu->base.unit].w;
+                        ldu->pref_height = rects[ldu->base.unit].h;
+                        ldu->pref_active = true;
+                } else {
+                        ldu->pref_width = 800;
+                        ldu->pref_height = 600;
+                        ldu->pref_active = false;
+                }
+                con->status = vmw_ldu_connector_detect(con);
+        }
+
+        mutex_unlock(&dev->mode_config.mutex);
+
+        return 0;
+}
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_overlay.c b/drivers/gpu/drm/vmwgfx/vmwgfx_overlay.c
index ad566c85b075..df2036ed18d5 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_overlay.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_overlay.c
@@ -358,6 +358,8 @@ static int vmw_overlay_update_stream(struct vmw_private *dev_priv,
         if (stream->buf != buf)
                 stream->buf = vmw_dmabuf_reference(buf);
         stream->saved = *arg;
+        /* stream is no longer stopped/paused */
+        stream->paused = false;
 
         return 0;
 }
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
index f8fbbc67a406..8612378b131e 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
@@ -597,8 +597,10 @@ int vmw_surface_define_ioctl(struct drm_device *dev, void *data,
 
         ret = copy_from_user(srf->sizes, user_sizes,
                              srf->num_sizes * sizeof(*srf->sizes));
-        if (unlikely(ret != 0))
+        if (unlikely(ret != 0)) {
+                ret = -EFAULT;
                 goto out_err1;
+        }
 
         if (srf->scanout &&
             srf->num_sizes == 1 &&
@@ -697,9 +699,11 @@ int vmw_surface_reference_ioctl(struct drm_device *dev, void *data,
         if (user_sizes)
                 ret = copy_to_user(user_sizes, srf->sizes,
                                    srf->num_sizes * sizeof(*srf->sizes));
-        if (unlikely(ret != 0))
+        if (unlikely(ret != 0)) {
                 DRM_ERROR("copy_to_user failed %p %u\n",
                           user_sizes, srf->num_sizes);
+                ret = -EFAULT;
+        }
 out_bad_resource:
 out_no_reference:
         ttm_base_object_unref(&base);
diff --git a/drivers/gpu/vga/vgaarb.c b/drivers/gpu/vga/vgaarb.c
index 441e38c95a85..b87569e96b16 100644
--- a/drivers/gpu/vga/vgaarb.c
+++ b/drivers/gpu/vga/vgaarb.c
@@ -1,12 +1,32 @@
 /*
- * vgaarb.c
+ * vgaarb.c: Implements the VGA arbitration. For details refer to
+ * Documentation/vgaarbiter.txt
+ *
  *
  * (C) Copyright 2005 Benjamin Herrenschmidt <benh@kernel.crashing.org>
  * (C) Copyright 2007 Paulo R. Zanoni <przanoni@gmail.com>
  * (C) Copyright 2007, 2009 Tiago Vignatti <vignatti@freedesktop.org>
  *
- * Implements the VGA arbitration. For details refer to
- * Documentation/vgaarbiter.txt
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS
+ * IN THE SOFTWARE.
+ *
  */
 
 #include <linux/module.h>
@@ -155,8 +175,8 @@ static struct vga_device *__vga_tryget(struct vga_device *vgadev,
             (vgadev->decodes & VGA_RSRC_LEGACY_MEM))
                 rsrc |= VGA_RSRC_LEGACY_MEM;
 
-        pr_devel("%s: %d\n", __func__, rsrc);
-        pr_devel("%s: owns: %d\n", __func__, vgadev->owns);
+        pr_debug("%s: %d\n", __func__, rsrc);
+        pr_debug("%s: owns: %d\n", __func__, vgadev->owns);
 
         /* Check what resources we need to acquire */
         wants = rsrc & ~vgadev->owns;
@@ -268,7 +288,7 @@ static void __vga_put(struct vga_device *vgadev, unsigned int rsrc)
 {
         unsigned int old_locks = vgadev->locks;
 
-        pr_devel("%s\n", __func__);
+        pr_debug("%s\n", __func__);
 
         /* Update our counters, and account for equivalent legacy resources
          * if we decode them
@@ -575,6 +595,7 @@ static inline void vga_update_device_decodes(struct vga_device *vgadev,
                 else
                         vga_decode_count--;
         }
+        pr_debug("vgaarb: decoding count now is: %d\n", vga_decode_count);
 }
 
 void __vga_set_legacy_decoding(struct pci_dev *pdev, unsigned int decodes, bool userspace)
@@ -831,7 +852,7 @@ static ssize_t vga_arb_write(struct file *file, const char __user * buf,
                 curr_pos += 5;
                 remaining -= 5;
 
-                pr_devel("client 0x%p called 'lock'\n", priv);
+                pr_debug("client 0x%p called 'lock'\n", priv);
 
                 if (!vga_str_to_iostate(curr_pos, remaining, &io_state)) {
                         ret_val = -EPROTO;
@@ -867,7 +888,7 @@ static ssize_t vga_arb_write(struct file *file, const char __user * buf,
                 curr_pos += 7;
                 remaining -= 7;
 
-                pr_devel("client 0x%p called 'unlock'\n", priv);
+                pr_debug("client 0x%p called 'unlock'\n", priv);
 
                 if (strncmp(curr_pos, "all", 3) == 0)
                         io_state = VGA_RSRC_LEGACY_IO | VGA_RSRC_LEGACY_MEM;
@@ -917,7 +938,7 @@ static ssize_t vga_arb_write(struct file *file, const char __user * buf,
                 curr_pos += 8;
                 remaining -= 8;
 
-                pr_devel("client 0x%p called 'trylock'\n", priv);
+                pr_debug("client 0x%p called 'trylock'\n", priv);
 
                 if (!vga_str_to_iostate(curr_pos, remaining, &io_state)) {
                         ret_val = -EPROTO;
@@ -961,7 +982,7 @@ static ssize_t vga_arb_write(struct file *file, const char __user * buf,
 
                 curr_pos += 7;
                 remaining -= 7;
-                pr_devel("client 0x%p called 'target'\n", priv);
+                pr_debug("client 0x%p called 'target'\n", priv);
                 /* if target is default */
                 if (!strncmp(curr_pos, "default", 7))
                         pdev = pci_dev_get(vga_default_device());
@@ -971,11 +992,11 @@ static ssize_t vga_arb_write(struct file *file, const char __user * buf,
                                 ret_val = -EPROTO;
                                 goto done;
                         }
-                        pr_devel("vgaarb: %s ==> %x:%x:%x.%x\n", curr_pos,
+                        pr_debug("vgaarb: %s ==> %x:%x:%x.%x\n", curr_pos,
                                 domain, bus, PCI_SLOT(devfn), PCI_FUNC(devfn));
 
                         pbus = pci_find_bus(domain, bus);
-                        pr_devel("vgaarb: pbus %p\n", pbus);
+                        pr_debug("vgaarb: pbus %p\n", pbus);
                         if (pbus == NULL) {
                                 pr_err("vgaarb: invalid PCI domain and/or bus address %x:%x\n",
                                         domain, bus);
@@ -983,7 +1004,7 @@ static ssize_t vga_arb_write(struct file *file, const char __user * buf,
                                 goto done;
                         }
                         pdev = pci_get_slot(pbus, devfn);
-                        pr_devel("vgaarb: pdev %p\n", pdev);
+                        pr_debug("vgaarb: pdev %p\n", pdev);
                         if (!pdev) {
                                 pr_err("vgaarb: invalid PCI address %x:%x\n",
                                         bus, devfn);
@@ -993,7 +1014,7 @@ static ssize_t vga_arb_write(struct file *file, const char __user * buf,
                 }
 
                 vgadev = vgadev_find(pdev);
-                pr_devel("vgaarb: vgadev %p\n", vgadev);
+                pr_debug("vgaarb: vgadev %p\n", vgadev);
                 if (vgadev == NULL) {
                         pr_err("vgaarb: this pci device is not a vga device\n");
                         pci_dev_put(pdev);
@@ -1029,7 +1050,7 @@ static ssize_t vga_arb_write(struct file *file, const char __user * buf,
         } else if (strncmp(curr_pos, "decodes ", 8) == 0) {
                 curr_pos += 8;
                 remaining -= 8;
-                pr_devel("vgaarb: client 0x%p called 'decodes'\n", priv);
+                pr_debug("vgaarb: client 0x%p called 'decodes'\n", priv);
 
                 if (!vga_str_to_iostate(curr_pos, remaining, &io_state)) {
                         ret_val = -EPROTO;
@@ -1058,7 +1079,7 @@ static unsigned int vga_arb_fpoll(struct file *file, poll_table * wait)
 {
         struct vga_arb_private *priv = file->private_data;
 
-        pr_devel("%s\n", __func__);
+        pr_debug("%s\n", __func__);
 
         if (priv == NULL)
                 return -ENODEV;
@@ -1071,7 +1092,7 @@ static int vga_arb_open(struct inode *inode, struct file *file)
         struct vga_arb_private *priv;
         unsigned long flags;
 
-        pr_devel("%s\n", __func__);
+        pr_debug("%s\n", __func__);
 
         priv = kmalloc(sizeof(struct vga_arb_private), GFP_KERNEL);
         if (priv == NULL)
@@ -1101,7 +1122,7 @@ static int vga_arb_release(struct inode *inode, struct file *file)
         unsigned long flags;
         int i;
 
-        pr_devel("%s\n", __func__);
+        pr_debug("%s\n", __func__);
 
         if (priv == NULL)
                 return -ENODEV;
@@ -1112,7 +1133,7 @@ static int vga_arb_release(struct inode *inode, struct file *file)
                 uc = &priv->cards[i];
                 if (uc->pdev == NULL)
                         continue;
-                pr_devel("uc->io_cnt == %d, uc->mem_cnt == %d\n",
+                pr_debug("uc->io_cnt == %d, uc->mem_cnt == %d\n",
                          uc->io_cnt, uc->mem_cnt);
                 while (uc->io_cnt--)
                         vga_put(uc->pdev, VGA_RSRC_LEGACY_IO);
@@ -1165,7 +1186,7 @@ static int pci_notify(struct notifier_block *nb, unsigned long action,
         struct pci_dev *pdev = to_pci_dev(dev);
         bool notify = false;
 
-        pr_devel("%s\n", __func__);
+        pr_debug("%s\n", __func__);
 
         /* For now we're only intereted in devices added and removed. I didn't
          * test this thing here, so someone needs to double check for the
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index 76ba59b9fea1..132278fa6240 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -347,6 +347,14 @@ config HID_QUANTA
         ---help---
         Support for Quanta Optical Touch dual-touch panels.
 
+config HID_ROCCAT
+        tristate "Roccat special event support"
+        depends on USB_HID
+        ---help---
+        Support for Roccat special events.
+        Say Y here if you have a Roccat mouse or keyboard and want OSD or
+        macro execution support.
+
 config HID_ROCCAT_KONE
         tristate "Roccat Kone Mouse support"
         depends on USB_HID
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index 22e47eaeea32..987fa0627367 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -48,6 +48,7 @@ obj-$(CONFIG_HID_QUANTA)	+= hid-quanta.o
 obj-$(CONFIG_HID_PANTHERLORD)   += hid-pl.o
 obj-$(CONFIG_HID_PETALYNX)      += hid-petalynx.o
 obj-$(CONFIG_HID_PICOLCD)       += hid-picolcd.o
+obj-$(CONFIG_HID_ROCCAT)        += hid-roccat.o
 obj-$(CONFIG_HID_ROCCAT_KONE)   += hid-roccat-kone.o
 obj-$(CONFIG_HID_SAMSUNG)       += hid-samsung.o
 obj-$(CONFIG_HID_SMARTJOYPLUS)  += hid-sjoy.o
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index e10e314d38cc..aa0f7dcabcd7 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -1301,6 +1301,7 @@ static const struct hid_device_id hid_blacklist[] = {
         { HID_USB_DEVICE(USB_VENDOR_ID_GREENASIA, 0x0012) },
         { HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE) },
         { HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE_2) },
+        { HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE_3) },
         { HID_USB_DEVICE(USB_VENDOR_ID_KENSINGTON, USB_DEVICE_ID_KS_SLIMBLADE) },
         { HID_USB_DEVICE(USB_VENDOR_ID_KYE, USB_DEVICE_ID_KYE_ERGO_525V) },
         { HID_USB_DEVICE(USB_VENDOR_ID_LABTEC, USB_DEVICE_ID_LABTEC_WIRELESS_KEYBOARD) },
diff --git a/drivers/hid/hid-debug.c b/drivers/hid/hid-debug.c
index 56f314fbd4f9..c94026768570 100644
--- a/drivers/hid/hid-debug.c
+++ b/drivers/hid/hid-debug.c
@@ -811,7 +811,7 @@ static const char *relatives[REL_MAX + 1] = {
         [REL_WHEEL] = "Wheel",          [REL_MISC] = "Misc",
 };
 
-static const char *absolutes[ABS_MAX + 1] = {
+static const char *absolutes[ABS_CNT] = {
         [ABS_X] = "X",                  [ABS_Y] = "Y",
         [ABS_Z] = "Z",                  [ABS_RX] = "Rx",
         [ABS_RY] = "Ry",                [ABS_RZ] = "Rz",
diff --git a/drivers/hid/hid-gyration.c b/drivers/hid/hid-gyration.c
index 62416e6baeca..3975e039c3dd 100644
--- a/drivers/hid/hid-gyration.c
+++ b/drivers/hid/hid-gyration.c
@@ -73,6 +73,7 @@ static int gyration_event(struct hid_device *hdev, struct hid_field *field,
 static const struct hid_device_id gyration_devices[] = {
         { HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE) },
         { HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE_2) },
+        { HID_USB_DEVICE(USB_VENDOR_ID_GYRATION, USB_DEVICE_ID_GYRATION_REMOTE_3) },
         { }
 };
 MODULE_DEVICE_TABLE(hid, gyration_devices);
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 9776896cc4fc..6af77ed0b555 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -282,6 +282,7 @@
 #define USB_VENDOR_ID_GYRATION          0x0c16
 #define USB_DEVICE_ID_GYRATION_REMOTE   0x0002
 #define USB_DEVICE_ID_GYRATION_REMOTE_2 0x0003
+#define USB_DEVICE_ID_GYRATION_REMOTE_3 0x0008
 
 #define USB_VENDOR_ID_HAPP              0x078b
 #define USB_DEVICE_ID_UGCI_DRIVING      0x0010
diff --git a/drivers/hid/hid-roccat-kone.c b/drivers/hid/hid-roccat-kone.c
index 66e694054ba2..17f2dc04f883 100644
--- a/drivers/hid/hid-roccat-kone.c
+++ b/drivers/hid/hid-roccat-kone.c
@@ -37,6 +37,7 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include "hid-ids.h"
+#include "hid-roccat.h"
 #include "hid-roccat-kone.h"
 
 static void kone_set_settings_checksum(struct kone_settings *settings)
@@ -263,7 +264,7 @@ static int kone_get_firmware_version(struct usb_device *usb_dev, int *result)
         return 0;
 }
 
-static ssize_t kone_sysfs_read_settings(struct kobject *kobj,
+static ssize_t kone_sysfs_read_settings(struct file *fp, struct kobject *kobj,
                 struct bin_attribute *attr, char *buf,
                 loff_t off, size_t count) {
         struct device *dev = container_of(kobj, struct device, kobj);
@@ -287,7 +288,7 @@ static ssize_t kone_sysfs_read_settings(struct kobject *kobj,
  * This function keeps values in kone_device up to date and assumes that in
  * case of error the old data is still valid
  */
-static ssize_t kone_sysfs_write_settings(struct kobject *kobj,
+static ssize_t kone_sysfs_write_settings(struct file *fp, struct kobject *kobj,
                 struct bin_attribute *attr, char *buf,
                 loff_t off, size_t count) {
         struct device *dev = container_of(kobj, struct device, kobj);
@@ -342,31 +343,31 @@ static ssize_t kone_sysfs_read_profilex(struct kobject *kobj,
         return count;
 }
 
-static ssize_t kone_sysfs_read_profile1(struct kobject *kobj,
+static ssize_t kone_sysfs_read_profile1(struct file *fp, struct kobject *kobj,
                 struct bin_attribute *attr, char *buf,
                 loff_t off, size_t count) {
         return kone_sysfs_read_profilex(kobj, attr, buf, off, count, 1);
 }
 
-static ssize_t kone_sysfs_read_profile2(struct kobject *kobj,
+static ssize_t kone_sysfs_read_profile2(struct file *fp, struct kobject *kobj,
                 struct bin_attribute *attr, char *buf,
                 loff_t off, size_t count) {
         return kone_sysfs_read_profilex(kobj, attr, buf, off, count, 2);
 }
 
-static ssize_t kone_sysfs_read_profile3(struct kobject *kobj,
+static ssize_t kone_sysfs_read_profile3(struct file *fp, struct kobject *kobj,
                 struct bin_attribute *attr, char *buf,
                 loff_t off, size_t count) {
         return kone_sysfs_read_profilex(kobj, attr, buf, off, count, 3);
 }
 
-static ssize_t kone_sysfs_read_profile4(struct kobject *kobj,
+static ssize_t kone_sysfs_read_profile4(struct file *fp, struct kobject *kobj,
                 struct bin_attribute *attr, char *buf,
                 loff_t off, size_t count) {
         return kone_sysfs_read_profilex(kobj, attr, buf, off, count, 4);
 }
 
-static ssize_t kone_sysfs_read_profile5(struct kobject *kobj,
+static ssize_t kone_sysfs_read_profile5(struct file *fp, struct kobject *kobj,
                 struct bin_attribute *attr, char *buf,
                 loff_t off, size_t count) {
         return kone_sysfs_read_profilex(kobj, attr, buf, off, count, 5);
@@ -404,31 +405,31 @@ static ssize_t kone_sysfs_write_profilex(struct kobject *kobj,
         return sizeof(struct kone_profile);
 }
 
-static ssize_t kone_sysfs_write_profile1(struct kobject *kobj,
+static ssize_t kone_sysfs_write_profile1(struct file *fp, struct kobject *kobj,
                 struct bin_attribute *attr, char *buf,
                 loff_t off, size_t count) {
         return kone_sysfs_write_profilex(kobj, attr, buf, off, count, 1);
 }
 
-static ssize_t kone_sysfs_write_profile2(struct kobject *kobj,
+static ssize_t kone_sysfs_write_profile2(struct file *fp, struct kobject *kobj,
                 struct bin_attribute *attr, char *buf,
                 loff_t off, size_t count) {
         return kone_sysfs_write_profilex(kobj, attr, buf, off, count, 2);
 }
 
-static ssize_t kone_sysfs_write_profile3(struct kobject *kobj,
+static ssize_t kone_sysfs_write_profile3(struct file *fp, struct kobject *kobj,
                 struct bin_attribute *attr, char *buf,
                 loff_t off, size_t count) {
         return kone_sysfs_write_profilex(kobj, attr, buf, off, count, 3);
 }
 
-static ssize_t kone_sysfs_write_profile4(struct kobject *kobj,
+static ssize_t kone_sysfs_write_profile4(struct file *fp, struct kobject *kobj,
                 struct bin_attribute *attr, char *buf,
                 loff_t off, size_t count) {
         return kone_sysfs_write_profilex(kobj, attr, buf, off, count, 4);
 }
 
-static ssize_t kone_sysfs_write_profile5(struct kobject *kobj,
+static ssize_t kone_sysfs_write_profile5(struct file *fp, struct kobject *kobj,
                 struct bin_attribute *attr, char *buf,
                 loff_t off, size_t count) {
         return kone_sysfs_write_profilex(kobj, attr, buf, off, count, 5);
@@ -849,6 +850,16 @@ static int kone_init_specials(struct hid_device *hdev)
                                         "couldn't init struct kone_device\n");
                         goto exit_free;
                 }
+
+                retval = roccat_connect(hdev);
+                if (retval < 0) {
+                        dev_err(&hdev->dev, "couldn't init char dev\n");
+                        /* be tolerant about not getting chrdev */
+                } else {
+                        kone->roccat_claimed = 1;
+                        kone->chrdev_minor = retval;
+                }
+
                 retval = kone_create_sysfs_attributes(intf);
                 if (retval) {
                         dev_err(&hdev->dev, "cannot create sysfs files\n");
@@ -868,10 +879,14 @@ exit_free:
 static void kone_remove_specials(struct hid_device *hdev)
 {
         struct usb_interface *intf = to_usb_interface(hdev->dev.parent);
+        struct kone_device *kone;
 
         if (intf->cur_altsetting->desc.bInterfaceProtocol
                         == USB_INTERFACE_PROTOCOL_MOUSE) {
                 kone_remove_sysfs_attributes(intf);
+                kone = hid_get_drvdata(hdev);
+                if (kone->roccat_claimed)
+                        roccat_disconnect(kone->chrdev_minor);
                 kfree(hid_get_drvdata(hdev));
         }
 }
@@ -930,6 +945,37 @@ static void kone_keep_values_up_to_date(struct kone_device *kone,
         }
 }
 
+static void kone_report_to_chrdev(struct kone_device const *kone,
+                struct kone_mouse_event const *event)
+{
+        struct kone_roccat_report roccat_report;
+
+        switch (event->event) {
+        case kone_mouse_event_switch_profile:
+        case kone_mouse_event_switch_dpi:
+        case kone_mouse_event_osd_profile:
+        case kone_mouse_event_osd_dpi:
+                roccat_report.event = event->event;
+                roccat_report.value = event->value;
+                roccat_report.key = 0;
+                roccat_report_event(kone->chrdev_minor,
+                                (uint8_t *)&roccat_report,
+                                sizeof(struct kone_roccat_report));
+                break;
+        case kone_mouse_event_call_overlong_macro:
+                if (event->value == kone_keystroke_action_press) {
+                        roccat_report.event = kone_mouse_event_call_overlong_macro;
+                        roccat_report.value = kone->actual_profile;
+                        roccat_report.key = event->macro_key;
+                        roccat_report_event(kone->chrdev_minor,
+                                        (uint8_t *)&roccat_report,
+                                        sizeof(struct kone_roccat_report));
+                }
+                break;
+        }
+
+}
+
 /*
  * Is called for keyboard- and mousepart.
  * Only mousepart gets informations about special events in its extended event
@@ -958,6 +1004,9 @@ static int kone_raw_event(struct hid_device *hdev, struct hid_report *report,
 
         kone_keep_values_up_to_date(kone, event);
 
+        if (kone->roccat_claimed)
+                kone_report_to_chrdev(kone, event);
+
         return 0; /* always do further processing */
 }
 
diff --git a/drivers/hid/hid-roccat-kone.h b/drivers/hid/hid-roccat-kone.h
index b413b10a7f8a..003e6f81c195 100644
--- a/drivers/hid/hid-roccat-kone.h
+++ b/drivers/hid/hid-roccat-kone.h
@@ -189,6 +189,12 @@ enum kone_commands {
         kone_command_firmware = 0xe5a
 };
 
+struct kone_roccat_report {
+        uint8_t event;
+        uint8_t value; /* holds dpi or profile value */
+        uint8_t key; /* macro key on overlong macro execution */
+};
+
 #pragma pack(pop)
 
 struct kone_device {
@@ -219,6 +225,9 @@ struct kone_device {
          * so it's read only once
          */
         int firmware_version;
+
+        int roccat_claimed;
+        int chrdev_minor;
 };
 
 #endif
diff --git a/drivers/hid/hid-roccat.c b/drivers/hid/hid-roccat.c
new file mode 100644
index 000000000000..e05d48edb66f
--- /dev/null
+++ b/drivers/hid/hid-roccat.c
@@ -0,0 +1,428 @@
+/*
+ * Roccat driver for Linux
+ *
+ * Copyright (c) 2010 Stefan Achatz <erazor_de@users.sourceforge.net>
+ */
+
+/*
+ * 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.
+ */
+
+/*
+ * Module roccat is a char device used to report special events of roccat
+ * hardware to userland. These events include requests for on-screen-display of
+ * profile or dpi settings or requests for execution of macro sequences that are
+ * not stored in device. The information in these events depends on hid device
+ * implementation and contains data that is not available in a single hid event
+ * or else hidraw could have been used.
+ * It is inspired by hidraw, but uses only one circular buffer for all readers.
+ */
+
+#include <linux/cdev.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+
+#include "hid-roccat.h"
+
+#define ROCCAT_FIRST_MINOR 0
+#define ROCCAT_MAX_DEVICES 8
+
+/* should be a power of 2 for performance reason */
+#define ROCCAT_CBUF_SIZE 16
+
+struct roccat_report {
+        uint8_t *value;
+        int len;
+};
+
+struct roccat_device {
+        unsigned int minor;
+        int open;
+        int exist;
+        wait_queue_head_t wait;
+        struct device *dev;
+        struct hid_device *hid;
+        struct list_head readers;
+        /* protects modifications of readers list */
+        struct mutex readers_lock;
+
+        /*
+         * circular_buffer has one writer and multiple readers with their own
+         * read pointers
+         */
+        struct roccat_report cbuf[ROCCAT_CBUF_SIZE];
+        int cbuf_end;
+        struct mutex cbuf_lock;
+};
+
+struct roccat_reader {
+        struct list_head node;
+        struct roccat_device *device;
+        int cbuf_start;
+};
+
+static int roccat_major;
+static struct class *roccat_class;
+static struct cdev roccat_cdev;
+
+static struct roccat_device *devices[ROCCAT_MAX_DEVICES];
+/* protects modifications of devices array */
+static DEFINE_MUTEX(devices_lock);
+
+static ssize_t roccat_read(struct file *file, char __user *buffer,
+                size_t count, loff_t *ppos)
+{
+        struct roccat_reader *reader = file->private_data;
+        struct roccat_device *device = reader->device;
+        struct roccat_report *report;
+        ssize_t retval = 0, len;
+        DECLARE_WAITQUEUE(wait, current);
+
+        mutex_lock(&device->cbuf_lock);
+
+        /* no data? */
+        if (reader->cbuf_start == device->cbuf_end) {
+                add_wait_queue(&device->wait, &wait);
+                set_current_state(TASK_INTERRUPTIBLE);
+
+                /* wait for data */
+                while (reader->cbuf_start == device->cbuf_end) {
+                        if (file->f_flags & O_NONBLOCK) {
+                                retval = -EAGAIN;
+                                break;
+                        }
+                        if (signal_pending(current)) {
+                                retval = -ERESTARTSYS;
+                                break;
+                        }
+                        if (!device->exist) {
+                                retval = -EIO;
+                                break;
+                        }
+
+                        mutex_unlock(&device->cbuf_lock);
+                        schedule();
+                        mutex_lock(&device->cbuf_lock);
+                        set_current_state(TASK_INTERRUPTIBLE);
+                }
+
+                set_current_state(TASK_RUNNING);
+                remove_wait_queue(&device->wait, &wait);
+        }
+
+        /* here we either have data or a reason to return if retval is set */
+        if (retval)
+                goto exit_unlock;
+
+        report = &device->cbuf[reader->cbuf_start];
+        /*
+         * If report is larger than requested amount of data, rest of report
+         * is lost!
+         */
+        len = report->len > count ? count : report->len;
+
+        if (copy_to_user(buffer, report->value, len)) {
+                retval = -EFAULT;
+                goto exit_unlock;
+        }
+        retval += len;
+        reader->cbuf_start = (reader->cbuf_start + 1) % ROCCAT_CBUF_SIZE;
+
+exit_unlock:
+        mutex_unlock(&device->cbuf_lock);
+        return retval;
+}
+
+static unsigned int roccat_poll(struct file *file, poll_table *wait)
+{
+        struct roccat_reader *reader = file->private_data;
+        poll_wait(file, &reader->device->wait, wait);
+        if (reader->cbuf_start != reader->device->cbuf_end)
+                return POLLIN | POLLRDNORM;
+        if (!reader->device->exist)
+                return POLLERR | POLLHUP;
+        return 0;
+}
+
+static int roccat_open(struct inode *inode, struct file *file)
+{
+        unsigned int minor = iminor(inode);
+        struct roccat_reader *reader;
+        struct roccat_device *device;
+        int error = 0;
+
+        reader = kzalloc(sizeof(struct roccat_reader), GFP_KERNEL);
+        if (!reader)
+                return -ENOMEM;
+
+        mutex_lock(&devices_lock);
+
+        device = devices[minor];
+
+        mutex_lock(&device->readers_lock);
+
+        if (!device) {
+                printk(KERN_EMERG "roccat device with minor %d doesn't exist\n",
+                                minor);
+                error = -ENODEV;
+                goto exit_unlock;
+        }
+
+        if (!device->open++) {
+                /* power on device on adding first reader */
+                if (device->hid->ll_driver->power) {
+                        error = device->hid->ll_driver->power(device->hid,
+                                        PM_HINT_FULLON);
+                        if (error < 0) {
+                                --device->open;
+                                goto exit_unlock;
+                        }
+                }
+                error = device->hid->ll_driver->open(device->hid);
+                if (error < 0) {
+                        if (device->hid->ll_driver->power)
+                                device->hid->ll_driver->power(device->hid,
+                                                PM_HINT_NORMAL);
+                        --device->open;
+                        goto exit_unlock;
+                }
+        }
+
+        reader->device = device;
+        /* new reader doesn't get old events */
+        reader->cbuf_start = device->cbuf_end;
+
+        list_add_tail(&reader->node, &device->readers);
+        file->private_data = reader;
+
+exit_unlock:
+        mutex_unlock(&device->readers_lock);
+        mutex_unlock(&devices_lock);
+        return error;
+}
+
+static int roccat_release(struct inode *inode, struct file *file)
+{
+        unsigned int minor = iminor(inode);
+        struct roccat_reader *reader = file->private_data;
+        struct roccat_device *device;
+
+        mutex_lock(&devices_lock);
+
+        device = devices[minor];
+        if (!device) {
+                mutex_unlock(&devices_lock);
+                printk(KERN_EMERG "roccat device with minor %d doesn't exist\n",
+                                minor);
+                return -ENODEV;
+        }
+
+        mutex_lock(&device->readers_lock);
+        list_del(&reader->node);
+        mutex_unlock(&device->readers_lock);
+        kfree(reader);
+
+        if (!--device->open) {
+                /* removing last reader */
+                if (device->exist) {
+                        if (device->hid->ll_driver->power)
+                                device->hid->ll_driver->power(device->hid,
+                                                PM_HINT_NORMAL);
+                        device->hid->ll_driver->close(device->hid);
+                } else {
+                        kfree(device);
+                }
+        }
+
+        mutex_unlock(&devices_lock);
+
+        return 0;
+}
+
+/*
+ * roccat_report_event() - output data to readers
+ * @minor: minor device number returned by roccat_connect()
+ * @data: pointer to data
+ * @len: size of data
+ *
+ * Return value is zero on success, a negative error code on failure.
+ *
+ * This is called from interrupt handler.
+ */
+int roccat_report_event(int minor, u8 const *data, int len)
+{
+        struct roccat_device *device;
+        struct roccat_reader *reader;
+        struct roccat_report *report;
+        uint8_t *new_value;
+
+        new_value = kmemdup(data, len, GFP_ATOMIC);
+        if (!new_value)
+                return -ENOMEM;
+
+        device = devices[minor];
+
+        report = &device->cbuf[device->cbuf_end];
+
+        /* passing NULL is safe */
+        kfree(report->value);
+
+        report->value = new_value;
+        report->len = len;
+        device->cbuf_end = (device->cbuf_end + 1) % ROCCAT_CBUF_SIZE;
+
+        list_for_each_entry(reader, &device->readers, node) {
+                /*
+                 * As we already inserted one element, the buffer can't be
+                 * empty. If start and end are equal, buffer is full and we
+                 * increase start, so that slow reader misses one event, but
+                 * gets the newer ones in the right order.
+                 */
+                if (reader->cbuf_start == device->cbuf_end)
+                        reader->cbuf_start = (reader->cbuf_start + 1) % ROCCAT_CBUF_SIZE;
+        }
+
+        wake_up_interruptible(&device->wait);
+        return 0;
+}
+EXPORT_SYMBOL_GPL(roccat_report_event);
+
+/*
+ * roccat_connect() - create a char device for special event output
+ * @hid: the hid device the char device should be connected to.
+ *
+ * Return value is minor device number in Range [0, ROCCAT_MAX_DEVICES] on
+ * success, a negative error code on failure.
+ */
+int roccat_connect(struct hid_device *hid)
+{
+        unsigned int minor;
+        struct roccat_device *device;
+        int temp;
+
+        device = kzalloc(sizeof(struct roccat_device), GFP_KERNEL);
+        if (!device)
+                return -ENOMEM;
+
+        mutex_lock(&devices_lock);
+
+        for (minor = 0; minor < ROCCAT_MAX_DEVICES; ++minor) {
+                if (devices[minor])
+                        continue;
+                break;
+        }
+
+        if (minor < ROCCAT_MAX_DEVICES) {
+                devices[minor] = device;
+        } else {
+                mutex_unlock(&devices_lock);
+                kfree(device);
+                return -EINVAL;
+        }
+
+        device->dev = device_create(roccat_class, &hid->dev,
+                        MKDEV(roccat_major, minor), NULL,
+                        "%s%s%d", "roccat", hid->driver->name, minor);
+
+        if (IS_ERR(device->dev)) {
+                devices[minor] = NULL;
+                mutex_unlock(&devices_lock);
+                temp = PTR_ERR(device->dev);
+                kfree(device);
+                return temp;
+        }
+
+        mutex_unlock(&devices_lock);
+
+        init_waitqueue_head(&device->wait);
+        INIT_LIST_HEAD(&device->readers);
+        mutex_init(&device->readers_lock);
+        mutex_init(&device->cbuf_lock);
+        device->minor = minor;
+        device->hid = hid;
+        device->exist = 1;
+        device->cbuf_end = 0;
+
+        return minor;
+}
+EXPORT_SYMBOL_GPL(roccat_connect);
+
+/* roccat_disconnect() - remove char device from hid device
+ * @minor: the minor device number returned by roccat_connect()
+ */
+void roccat_disconnect(int minor)
+{
+        struct roccat_device *device;
+
+        mutex_lock(&devices_lock);
+        device = devices[minor];
+        devices[minor] = NULL;
+        mutex_unlock(&devices_lock);
+
+        device->exist = 0; /* TODO exist maybe not needed */
+
+        device_destroy(roccat_class, MKDEV(roccat_major, minor));
+
+        if (device->open) {
+                device->hid->ll_driver->close(device->hid);
+                wake_up_interruptible(&device->wait);
+        } else {
+                kfree(device);
+        }
+}
+EXPORT_SYMBOL_GPL(roccat_disconnect);
+
+static const struct file_operations roccat_ops = {
+        .owner = THIS_MODULE,
+        .read = roccat_read,
+        .poll = roccat_poll,
+        .open = roccat_open,
+        .release = roccat_release,
+};
+
+static int __init roccat_init(void)
+{
+        int retval;
+        dev_t dev_id;
+
+        retval = alloc_chrdev_region(&dev_id, ROCCAT_FIRST_MINOR,
+                        ROCCAT_MAX_DEVICES, "roccat");
+
+        roccat_major = MAJOR(dev_id);
+
+        if (retval < 0) {
+                printk(KERN_WARNING "roccat: can't get major number\n");
+                return retval;
+        }
+
+        roccat_class = class_create(THIS_MODULE, "roccat");
+        if (IS_ERR(roccat_class)) {
+                retval = PTR_ERR(roccat_class);
+                unregister_chrdev_region(dev_id, ROCCAT_MAX_DEVICES);
+                return retval;
+        }
+
+        cdev_init(&roccat_cdev, &roccat_ops);
+        cdev_add(&roccat_cdev, dev_id, ROCCAT_MAX_DEVICES);
+
+        return 0;
+}
+
+static void __exit roccat_exit(void)
+{
+        dev_t dev_id = MKDEV(roccat_major, 0);
+
+        cdev_del(&roccat_cdev);
+        class_destroy(roccat_class);
+        unregister_chrdev_region(dev_id, ROCCAT_MAX_DEVICES);
+}
+
+module_init(roccat_init);
+module_exit(roccat_exit);
+
+MODULE_AUTHOR("Stefan Achatz");
+MODULE_DESCRIPTION("USB Roccat char device");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hid/hid-roccat.h b/drivers/hid/hid-roccat.h
new file mode 100644
index 000000000000..d8aae0c1fa7e
--- /dev/null
+++ b/drivers/hid/hid-roccat.h
@@ -0,0 +1,31 @@
+#ifndef __HID_ROCCAT_H
+#define __HID_ROCCAT_H
+
+/*
+ * Copyright (c) 2010 Stefan Achatz <erazor_de@users.sourceforge.net>
+ */
+
+/*
+ * 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/hid.h>
+#include <linux/types.h>
+
+#if defined(CONFIG_HID_ROCCAT) || defined (CONFIG_HID_ROCCAT_MODULE)
+int roccat_connect(struct hid_device *hid);
+void roccat_disconnect(int minor);
+int roccat_report_event(int minor, u8 const *data, int len);
+#else
+static inline int roccat_connect(struct hid_device *hid) { return -1; }
+static inline void roccat_disconnect(int minor) {}
+static inline int roccat_report_event(int minor, u8 const *data, int len)
+{
+        return 0;
+}
+#endif
+
+#endif
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index 6a9ac754ca5d..e19cf8eb6ccf 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -447,13 +447,14 @@ config SENSORS_IT87
           will be called it87.
 
 config SENSORS_LM63
-        tristate "National Semiconductor LM63"
+        tristate "National Semiconductor LM63 and LM64"
         depends on I2C
         help
-          If you say yes here you get support for the National Semiconductor
-          LM63 remote diode digital temperature sensor with integrated fan
-          control.  Such chips are found on the Tyan S4882 (Thunder K8QS Pro)
-          motherboard, among others.
+          If you say yes here you get support for the National
+          Semiconductor LM63 and LM64 remote diode digital temperature
+          sensors with integrated fan control.  Such chips are found
+          on the Tyan S4882 (Thunder K8QS Pro) motherboard, among
+          others.
 
           This driver can also be built as a module.  If so, the module
           will be called lm63.
@@ -492,7 +493,8 @@ config SENSORS_LM75
                 - NXP's LM75A
                 - ST Microelectronics STDS75
                 - TelCom (now Microchip) TCN75
-                - Texas Instruments TMP100, TMP101, TMP75, TMP175, TMP275
+                - Texas Instruments TMP100, TMP101, TMP105, TMP75, TMP175,
+                  TMP275
 
           This driver supports driver model based binding through board
           specific I2C device tables.
@@ -749,6 +751,16 @@ config SENSORS_DME1737
           This driver can also be built as a module.  If so, the module
           will be called dme1737.
 
+config SENSORS_EMC1403
+        tristate "SMSC EMC1403 thermal sensor"
+        depends on I2C
+        help
+          If you say yes here you get support for the SMSC EMC1403
+          temperature monitoring chip.
+
+          Threshold values can be configured using sysfs.
+          Data from the different diodes are accessible via sysfs.
+
 config SENSORS_SMSC47M1
         tristate "SMSC LPC47M10x and compatibles"
         help
@@ -831,6 +843,16 @@ config SENSORS_THMC50
           This driver can also be built as a module.  If so, the module
           will be called thmc50.
 
+config SENSORS_TMP102
+        tristate "Texas Instruments TMP102"
+        depends on I2C && EXPERIMENTAL
+        help
+          If you say yes here you get support for Texas Instruments TMP102
+          sensor chips.
+
+          This driver can also be built as a module.  If so, the module
+          will be called tmp102.
+
 config SENSORS_TMP401
         tristate "Texas Instruments TMP401 and compatibles"
         depends on I2C && EXPERIMENTAL
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index 86920fb34118..2138ceb1a713 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -41,6 +41,7 @@ obj-$(CONFIG_SENSORS_ATXP1)	+= atxp1.o
 obj-$(CONFIG_SENSORS_CORETEMP)  += coretemp.o
 obj-$(CONFIG_SENSORS_DME1737)   += dme1737.o
 obj-$(CONFIG_SENSORS_DS1621)    += ds1621.o
+obj-$(CONFIG_SENSORS_EMC1403)   += emc1403.o
 obj-$(CONFIG_SENSORS_F71805F)   += f71805f.o
 obj-$(CONFIG_SENSORS_F71882FG)  += f71882fg.o
 obj-$(CONFIG_SENSORS_F75375S)   += f75375s.o
@@ -90,6 +91,7 @@ obj-$(CONFIG_SENSORS_SMSC47M1)	+= smsc47m1.o
 obj-$(CONFIG_SENSORS_SMSC47M192)+= smsc47m192.o
 obj-$(CONFIG_SENSORS_AMC6821)   += amc6821.o
 obj-$(CONFIG_SENSORS_THMC50)    += thmc50.o
+obj-$(CONFIG_SENSORS_TMP102)    += tmp102.o
 obj-$(CONFIG_SENSORS_TMP401)    += tmp401.o
 obj-$(CONFIG_SENSORS_TMP421)    += tmp421.o
 obj-$(CONFIG_SENSORS_VIA_CPUTEMP)+= via-cputemp.o
diff --git a/drivers/hwmon/adm1031.c b/drivers/hwmon/adm1031.c
index 1644b92e7cc4..15c1a9616af3 100644
--- a/drivers/hwmon/adm1031.c
+++ b/drivers/hwmon/adm1031.c
@@ -36,6 +36,7 @@
 #define ADM1031_REG_FAN_DIV(nr)         (0x20 + (nr))
 #define ADM1031_REG_PWM                 (0x22)
 #define ADM1031_REG_FAN_MIN(nr)         (0x10 + (nr))
+#define ADM1031_REG_FAN_FILTER          (0x23)
 
 #define ADM1031_REG_TEMP_OFFSET(nr)     (0x0d + (nr))
 #define ADM1031_REG_TEMP_MAX(nr)        (0x14 + 4 * (nr))
@@ -61,6 +62,9 @@
 #define ADM1031_CONF2_TACH2_ENABLE      0x08
 #define ADM1031_CONF2_TEMP_ENABLE(chan) (0x10 << (chan))
 
+#define ADM1031_UPDATE_RATE_MASK        0x1c
+#define ADM1031_UPDATE_RATE_SHIFT       2
+
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = { 0x2c, 0x2d, 0x2e, I2C_CLIENT_END };
 
@@ -75,6 +79,7 @@ struct adm1031_data {
         int chip_type;
         char valid;             /* !=0 if following fields are valid */
         unsigned long last_updated;     /* In jiffies */
+        unsigned int update_rate;       /* In milliseconds */
         /* The chan_select_table contains the possible configurations for
          * auto fan control.
          */
@@ -738,6 +743,57 @@ static SENSOR_DEVICE_ATTR(temp3_crit_alarm, S_IRUGO, show_alarm, NULL, 12);
 static SENSOR_DEVICE_ATTR(temp3_fault, S_IRUGO, show_alarm, NULL, 13);
 static SENSOR_DEVICE_ATTR(temp1_crit_alarm, S_IRUGO, show_alarm, NULL, 14);
 
+/* Update Rate */
+static const unsigned int update_rates[] = {
+        16000, 8000, 4000, 2000, 1000, 500, 250, 125,
+};
+
+static ssize_t show_update_rate(struct device *dev,
+                                struct device_attribute *attr, char *buf)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct adm1031_data *data = i2c_get_clientdata(client);
+
+        return sprintf(buf, "%u\n", data->update_rate);
+}
+
+static ssize_t set_update_rate(struct device *dev,
+                               struct device_attribute *attr,
+                               const char *buf, size_t count)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct adm1031_data *data = i2c_get_clientdata(client);
+        unsigned long val;
+        int i, err;
+        u8 reg;
+
+        err = strict_strtoul(buf, 10, &val);
+        if (err)
+                return err;
+
+        /* find the nearest update rate from the table */
+        for (i = 0; i < ARRAY_SIZE(update_rates) - 1; i++) {
+                if (val >= update_rates[i])
+                        break;
+        }
+        /* if not found, we point to the last entry (lowest update rate) */
+
+        /* set the new update rate while preserving other settings */
+        reg = adm1031_read_value(client, ADM1031_REG_FAN_FILTER);
+        reg &= ~ADM1031_UPDATE_RATE_MASK;
+        reg |= i << ADM1031_UPDATE_RATE_SHIFT;
+        adm1031_write_value(client, ADM1031_REG_FAN_FILTER, reg);
+
+        mutex_lock(&data->update_lock);
+        data->update_rate = update_rates[i];
+        mutex_unlock(&data->update_lock);
+
+        return count;
+}
+
+static DEVICE_ATTR(update_rate, S_IRUGO | S_IWUSR, show_update_rate,
+                   set_update_rate);
+
 static struct attribute *adm1031_attributes[] = {
         &sensor_dev_attr_fan1_input.dev_attr.attr,
         &sensor_dev_attr_fan1_div.dev_attr.attr,
@@ -774,6 +830,7 @@ static struct attribute *adm1031_attributes[] = {
 
         &sensor_dev_attr_auto_fan1_min_pwm.dev_attr.attr,
 
+        &dev_attr_update_rate.attr,
         &dev_attr_alarms.attr,
 
         NULL
@@ -900,6 +957,7 @@ static void adm1031_init_client(struct i2c_client *client)
 {
         unsigned int read_val;
         unsigned int mask;
+        int i;
         struct adm1031_data *data = i2c_get_clientdata(client);
 
         mask = (ADM1031_CONF2_PWM1_ENABLE | ADM1031_CONF2_TACH1_ENABLE);
@@ -919,18 +977,24 @@ static void adm1031_init_client(struct i2c_client *client)
                                 ADM1031_CONF1_MONITOR_ENABLE);
         }
 
+        /* Read the chip's update rate */
+        mask = ADM1031_UPDATE_RATE_MASK;
+        read_val = adm1031_read_value(client, ADM1031_REG_FAN_FILTER);
+        i = (read_val & mask) >> ADM1031_UPDATE_RATE_SHIFT;
+        data->update_rate = update_rates[i];
 }
 
 static struct adm1031_data *adm1031_update_device(struct device *dev)
 {
         struct i2c_client *client = to_i2c_client(dev);
         struct adm1031_data *data = i2c_get_clientdata(client);
+        unsigned long next_update;
         int chan;
 
         mutex_lock(&data->update_lock);
 
-        if (time_after(jiffies, data->last_updated + HZ + HZ / 2)
-            || !data->valid) {
+        next_update = data->last_updated + msecs_to_jiffies(data->update_rate);
+        if (time_after(jiffies, next_update) || !data->valid) {
 
                 dev_dbg(&client->dev, "Starting adm1031 update\n");
                 for (chan = 0;
diff --git a/drivers/hwmon/adt7411.c b/drivers/hwmon/adt7411.c
index 4086c7257f91..f13c843a2964 100644
--- a/drivers/hwmon/adt7411.c
+++ b/drivers/hwmon/adt7411.c
@@ -316,7 +316,6 @@ static int __devinit adt7411_probe(struct i2c_client *client,
  exit_remove:
         sysfs_remove_group(&client->dev.kobj, &adt7411_attr_grp);
  exit_free:
-        i2c_set_clientdata(client, NULL);
         kfree(data);
         return ret;
 }
@@ -327,7 +326,6 @@ static int __devexit adt7411_remove(struct i2c_client *client)
 
         hwmon_device_unregister(data->hwmon_dev);
         sysfs_remove_group(&client->dev.kobj, &adt7411_attr_grp);
-        i2c_set_clientdata(client, NULL);
         kfree(data);
         return 0;
 }
diff --git a/drivers/hwmon/applesmc.c b/drivers/hwmon/applesmc.c
index f085c18d2905..b6598aa557a0 100644
--- a/drivers/hwmon/applesmc.c
+++ b/drivers/hwmon/applesmc.c
@@ -148,6 +148,20 @@ static const char *temperature_sensors_sets[][41] = {
 /* Set 18: MacBook Pro 2,2 */
         { "TB0T", "TC0D", "TC0P", "TG0H", "TG0P", "TG0T", "TM0P", "TTF0",
           "Th0H", "Th1H", "Tm0P", "Ts0P", NULL },
+/* Set 19: Macbook Pro 5,3 */
+        { "TB0T", "TB1T", "TB2T", "TB3T", "TC0D", "TC0F", "TC0P", "TG0D",
+          "TG0F", "TG0H", "TG0P", "TG0T", "TN0D", "TN0P", "TTF0", "Th2H",
+          "Tm0P", "Ts0P", "Ts0S", NULL },
+/* Set 20: MacBook Pro 5,4 */
+        { "TB0T", "TB1T", "TB2T", "TB3T", "TC0D", "TC0F", "TC0P", "TN0D",
+          "TN0P", "TTF0", "Th2H", "Ts0P", "Ts0S", NULL },
+/* Set 21: MacBook Pro 6,2 */
+        { "TB0T", "TB1T", "TB2T", "TC0C", "TC0D", "TC0P", "TC1C", "TG0D",
+          "TG0P", "TG0T", "TMCD", "TP0P", "TPCD", "Th1H", "Th2H", "Tm0P",
+          "Ts0P", "Ts0S", NULL },
+/* Set 22: MacBook Pro 7,1 */
+        { "TB0T", "TB1T", "TB2T", "TC0D", "TC0P", "TN0D", "TN0P", "TN0S",
+          "TN1D", "TN1F", "TN1G", "TN1S", "Th1H", "Ts0P", "Ts0S", NULL },
 };
 
 /* List of keys used to read/write fan speeds */
@@ -646,6 +660,17 @@ out:
                 return snprintf(sysfsbuf, PAGE_SIZE, "(%d,%d)\n", left, right);
 }
 
+/* Displays sensor key as label */
+static ssize_t applesmc_show_sensor_label(struct device *dev,
+                        struct device_attribute *devattr, char *sysfsbuf)
+{
+        struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+        const char *key =
+                temperature_sensors_sets[applesmc_temperature_set][attr->index];
+
+        return snprintf(sysfsbuf, PAGE_SIZE, "%s\n", key);
+}
+
 /* Displays degree Celsius * 1000 */
 static ssize_t applesmc_show_temperature(struct device *dev,
                         struct device_attribute *devattr, char *sysfsbuf)
@@ -1113,6 +1138,86 @@ static const struct attribute_group fan_attribute_groups[] = {
 /*
  * Temperature sensors sysfs entries.
  */
+static SENSOR_DEVICE_ATTR(temp1_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp2_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 1);
+static SENSOR_DEVICE_ATTR(temp3_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 2);
+static SENSOR_DEVICE_ATTR(temp4_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 3);
+static SENSOR_DEVICE_ATTR(temp5_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 4);
+static SENSOR_DEVICE_ATTR(temp6_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 5);
+static SENSOR_DEVICE_ATTR(temp7_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 6);
+static SENSOR_DEVICE_ATTR(temp8_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 7);
+static SENSOR_DEVICE_ATTR(temp9_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 8);
+static SENSOR_DEVICE_ATTR(temp10_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 9);
+static SENSOR_DEVICE_ATTR(temp11_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 10);
+static SENSOR_DEVICE_ATTR(temp12_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 11);
+static SENSOR_DEVICE_ATTR(temp13_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 12);
+static SENSOR_DEVICE_ATTR(temp14_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 13);
+static SENSOR_DEVICE_ATTR(temp15_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 14);
+static SENSOR_DEVICE_ATTR(temp16_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 15);
+static SENSOR_DEVICE_ATTR(temp17_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 16);
+static SENSOR_DEVICE_ATTR(temp18_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 17);
+static SENSOR_DEVICE_ATTR(temp19_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 18);
+static SENSOR_DEVICE_ATTR(temp20_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 19);
+static SENSOR_DEVICE_ATTR(temp21_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 20);
+static SENSOR_DEVICE_ATTR(temp22_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 21);
+static SENSOR_DEVICE_ATTR(temp23_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 22);
+static SENSOR_DEVICE_ATTR(temp24_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 23);
+static SENSOR_DEVICE_ATTR(temp25_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 24);
+static SENSOR_DEVICE_ATTR(temp26_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 25);
+static SENSOR_DEVICE_ATTR(temp27_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 26);
+static SENSOR_DEVICE_ATTR(temp28_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 27);
+static SENSOR_DEVICE_ATTR(temp29_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 28);
+static SENSOR_DEVICE_ATTR(temp30_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 29);
+static SENSOR_DEVICE_ATTR(temp31_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 30);
+static SENSOR_DEVICE_ATTR(temp32_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 31);
+static SENSOR_DEVICE_ATTR(temp33_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 32);
+static SENSOR_DEVICE_ATTR(temp34_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 33);
+static SENSOR_DEVICE_ATTR(temp35_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 34);
+static SENSOR_DEVICE_ATTR(temp36_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 35);
+static SENSOR_DEVICE_ATTR(temp37_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 36);
+static SENSOR_DEVICE_ATTR(temp38_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 37);
+static SENSOR_DEVICE_ATTR(temp39_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 38);
+static SENSOR_DEVICE_ATTR(temp40_label, S_IRUGO,
+                                        applesmc_show_sensor_label, NULL, 39);
 static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO,
                                         applesmc_show_temperature, NULL, 0);
 static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO,
@@ -1194,6 +1299,50 @@ static SENSOR_DEVICE_ATTR(temp39_input, S_IRUGO,
 static SENSOR_DEVICE_ATTR(temp40_input, S_IRUGO,
                                         applesmc_show_temperature, NULL, 39);
 
+static struct attribute *label_attributes[] = {
+        &sensor_dev_attr_temp1_label.dev_attr.attr,
+        &sensor_dev_attr_temp2_label.dev_attr.attr,
+        &sensor_dev_attr_temp3_label.dev_attr.attr,
+        &sensor_dev_attr_temp4_label.dev_attr.attr,
+        &sensor_dev_attr_temp5_label.dev_attr.attr,
+        &sensor_dev_attr_temp6_label.dev_attr.attr,
+        &sensor_dev_attr_temp7_label.dev_attr.attr,
+        &sensor_dev_attr_temp8_label.dev_attr.attr,
+        &sensor_dev_attr_temp9_label.dev_attr.attr,
+        &sensor_dev_attr_temp10_label.dev_attr.attr,
+        &sensor_dev_attr_temp11_label.dev_attr.attr,
+        &sensor_dev_attr_temp12_label.dev_attr.attr,
+        &sensor_dev_attr_temp13_label.dev_attr.attr,
+        &sensor_dev_attr_temp14_label.dev_attr.attr,
+        &sensor_dev_attr_temp15_label.dev_attr.attr,
+        &sensor_dev_attr_temp16_label.dev_attr.attr,
+        &sensor_dev_attr_temp17_label.dev_attr.attr,
+        &sensor_dev_attr_temp18_label.dev_attr.attr,
+        &sensor_dev_attr_temp19_label.dev_attr.attr,
+        &sensor_dev_attr_temp20_label.dev_attr.attr,
+        &sensor_dev_attr_temp21_label.dev_attr.attr,
+        &sensor_dev_attr_temp22_label.dev_attr.attr,
+        &sensor_dev_attr_temp23_label.dev_attr.attr,
+        &sensor_dev_attr_temp24_label.dev_attr.attr,
+        &sensor_dev_attr_temp25_label.dev_attr.attr,
+        &sensor_dev_attr_temp26_label.dev_attr.attr,
+        &sensor_dev_attr_temp27_label.dev_attr.attr,
+        &sensor_dev_attr_temp28_label.dev_attr.attr,
+        &sensor_dev_attr_temp29_label.dev_attr.attr,
+        &sensor_dev_attr_temp30_label.dev_attr.attr,
+        &sensor_dev_attr_temp31_label.dev_attr.attr,
+        &sensor_dev_attr_temp32_label.dev_attr.attr,
+        &sensor_dev_attr_temp33_label.dev_attr.attr,
+        &sensor_dev_attr_temp34_label.dev_attr.attr,
+        &sensor_dev_attr_temp35_label.dev_attr.attr,
+        &sensor_dev_attr_temp36_label.dev_attr.attr,
+        &sensor_dev_attr_temp37_label.dev_attr.attr,
+        &sensor_dev_attr_temp38_label.dev_attr.attr,
+        &sensor_dev_attr_temp39_label.dev_attr.attr,
+        &sensor_dev_attr_temp40_label.dev_attr.attr,
+        NULL
+};
+
 static struct attribute *temperature_attributes[] = {
         &sensor_dev_attr_temp1_input.dev_attr.attr,
         &sensor_dev_attr_temp2_input.dev_attr.attr,
@@ -1241,6 +1390,10 @@ static struct attribute *temperature_attributes[] = {
 static const struct attribute_group temperature_attributes_group =
         { .attrs = temperature_attributes };
 
+static const struct attribute_group label_attributes_group = {
+        .attrs = label_attributes
+};
+
 /* Module stuff */
 
 /*
@@ -1363,6 +1516,14 @@ static __initdata struct dmi_match_data applesmc_dmi_data[] = {
         { .accelerometer = 0, .light = 0, .temperature_set = 17 },
 /* MacBook Pro 2,2: accelerometer, backlight and temperature set 18 */
         { .accelerometer = 1, .light = 1, .temperature_set = 18 },
+/* MacBook Pro 5,3: accelerometer, backlight and temperature set 19 */
+        { .accelerometer = 1, .light = 1, .temperature_set = 19 },
+/* MacBook Pro 5,4: accelerometer, backlight and temperature set 20 */
+        { .accelerometer = 1, .light = 1, .temperature_set = 20 },
+/* MacBook Pro 6,2: accelerometer, backlight and temperature set 21 */
+        { .accelerometer = 1, .light = 1, .temperature_set = 21 },
+/* MacBook Pro 7,1: accelerometer, backlight and temperature set 22 */
+        { .accelerometer = 1, .light = 1, .temperature_set = 22 },
 };
 
 /* Note that DMI_MATCH(...,"MacBook") will match "MacBookPro1,1".
@@ -1376,6 +1537,22 @@ static __initdata struct dmi_system_id applesmc_whitelist[] = {
           DMI_MATCH(DMI_BOARD_VENDOR, "Apple"),
           DMI_MATCH(DMI_PRODUCT_NAME, "MacBookAir") },
                 &applesmc_dmi_data[7]},
+        { applesmc_dmi_match, "Apple MacBook Pro 7", {
+          DMI_MATCH(DMI_BOARD_VENDOR, "Apple"),
+          DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro7") },
+                &applesmc_dmi_data[22]},
+        { applesmc_dmi_match, "Apple MacBook Pro 5,4", {
+          DMI_MATCH(DMI_BOARD_VENDOR, "Apple"),
+          DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5,4") },
+                &applesmc_dmi_data[20]},
+        { applesmc_dmi_match, "Apple MacBook Pro 5,3", {
+          DMI_MATCH(DMI_BOARD_VENDOR, "Apple"),
+          DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5,3") },
+                &applesmc_dmi_data[19]},
+        { applesmc_dmi_match, "Apple MacBook Pro 6", {
+          DMI_MATCH(DMI_BOARD_VENDOR, "Apple"),
+          DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro6") },
+                &applesmc_dmi_data[21]},
         { applesmc_dmi_match, "Apple MacBook Pro 5", {
           DMI_MATCH(DMI_BOARD_VENDOR, "Apple"),
           DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro5") },
@@ -1518,7 +1695,8 @@ static int __init applesmc_init(void)
         for (i = 0;
              temperature_sensors_sets[applesmc_temperature_set][i] != NULL;
              i++) {
-                if (temperature_attributes[i] == NULL) {
+                if (temperature_attributes[i] == NULL ||
+                    label_attributes[i] == NULL) {
                         printk(KERN_ERR "applesmc: More temperature sensors "
                                 "in temperature_sensors_sets (at least %i)"
                                 "than available sysfs files in "
@@ -1530,6 +1708,10 @@ static int __init applesmc_init(void)
                                                 temperature_attributes[i]);
                 if (ret)
                         goto out_temperature;
+                ret = sysfs_create_file(&pdev->dev.kobj,
+                                                label_attributes[i]);
+                if (ret)
+                        goto out_temperature;
         }
 
         if (applesmc_accelerometer) {
@@ -1580,6 +1762,7 @@ out_accelerometer:
         if (applesmc_accelerometer)
                 applesmc_release_accelerometer();
 out_temperature:
+        sysfs_remove_group(&pdev->dev.kobj, &label_attributes_group);
         sysfs_remove_group(&pdev->dev.kobj, &temperature_attributes_group);
 out_fans:
         while (fans_handled)
@@ -1609,6 +1792,7 @@ static void __exit applesmc_exit(void)
         }
         if (applesmc_accelerometer)
                 applesmc_release_accelerometer();
+        sysfs_remove_group(&pdev->dev.kobj, &label_attributes_group);
         sysfs_remove_group(&pdev->dev.kobj, &temperature_attributes_group);
         while (fans_handled)
                 sysfs_remove_group(&pdev->dev.kobj,
diff --git a/drivers/hwmon/asc7621.c b/drivers/hwmon/asc7621.c
index 0f388adc6187..3b973f30b1f6 100644
--- a/drivers/hwmon/asc7621.c
+++ b/drivers/hwmon/asc7621.c
@@ -1141,7 +1141,6 @@ exit_remove:
                                    &(asc7621_params[i].sda.dev_attr));
         }
 
-        i2c_set_clientdata(client, NULL);
         kfree(data);
         return err;
 }
@@ -1196,7 +1195,6 @@ static int asc7621_remove(struct i2c_client *client)
                                    &(asc7621_params[i].sda.dev_attr));
         }
 
-        i2c_set_clientdata(client, NULL);
         kfree(data);
         return 0;
 }
diff --git a/drivers/hwmon/asus_atk0110.c b/drivers/hwmon/asus_atk0110.c
index 16c420240724..653db1bda934 100644
--- a/drivers/hwmon/asus_atk0110.c
+++ b/drivers/hwmon/asus_atk0110.c
@@ -1411,6 +1411,13 @@ static int __init atk0110_init(void)
 {
         int ret;
 
+        /* Make sure it's safe to access the device through ACPI */
+        if (!acpi_resources_are_enforced()) {
+                pr_err("atk: Resources not safely usable due to "
+                       "acpi_enforce_resources kernel parameter\n");
+                return -EBUSY;
+        }
+
         ret = acpi_bus_register_driver(&atk_driver);
         if (ret)
                 pr_info("atk: acpi_bus_register_driver failed: %d\n", ret);
diff --git a/drivers/hwmon/dme1737.c b/drivers/hwmon/dme1737.c
index 823dd28a902c..980c17d5eeae 100644
--- a/drivers/hwmon/dme1737.c
+++ b/drivers/hwmon/dme1737.c
@@ -1,12 +1,14 @@
 /*
- * dme1737.c - Driver for the SMSC DME1737, Asus A8000, SMSC SCH311x and
- *             SCH5027 Super-I/O chips integrated hardware monitoring features.
- * Copyright (c) 2007, 2008 Juerg Haefliger <juergh@gmail.com>
+ * dme1737.c - Driver for the SMSC DME1737, Asus A8000, SMSC SCH311x, SCH5027,
+ *             and SCH5127 Super-I/O chips integrated hardware monitoring
+ *             features.
+ * Copyright (c) 2007, 2008, 2009, 2010 Juerg Haefliger <juergh@gmail.com>
  *
  * This driver is an I2C/ISA hybrid, meaning that it uses the I2C bus to access
  * the chip registers if a DME1737, A8000, or SCH5027 is found and the ISA bus
- * if a SCH311x chip is found. Both types of chips have very similar hardware
- * monitoring capabilities but differ in the way they can be accessed.
+ * if a SCH311x or SCH5127 chip is found. Both types of chips have very
+ * similar hardware monitoring capabilities but differ in the way they can be
+ * accessed.
  *
  * 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
@@ -57,7 +59,7 @@ MODULE_PARM_DESC(probe_all_addr, "Include probing of non-standard LPC "
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = {0x2c, 0x2d, 0x2e, I2C_CLIENT_END};
 
-enum chips { dme1737, sch5027, sch311x };
+enum chips { dme1737, sch5027, sch311x, sch5127 };
 
 /* ---------------------------------------------------------------------
  * Registers
@@ -164,10 +166,29 @@ static const u8 DME1737_BIT_ALARM_FAN[] = {10, 11, 12, 13, 22, 23};
 #define DME1737_VERSTEP_MASK    0xf8
 #define SCH311X_DEVICE          0x8c
 #define SCH5027_VERSTEP         0x69
+#define SCH5127_DEVICE          0x8e
+
+/* Device ID values (global configuration register index 0x20) */
+#define DME1737_ID_1    0x77
+#define DME1737_ID_2    0x78
+#define SCH3112_ID      0x7c
+#define SCH3114_ID      0x7d
+#define SCH3116_ID      0x7f
+#define SCH5027_ID      0x89
+#define SCH5127_ID      0x86
 
 /* Length of ISA address segment */
 #define DME1737_EXTENT  2
 
+/* chip-dependent features */
+#define HAS_TEMP_OFFSET         (1 << 0)                /* bit 0 */
+#define HAS_VID                 (1 << 1)                /* bit 1 */
+#define HAS_ZONE3               (1 << 2)                /* bit 2 */
+#define HAS_ZONE_HYST           (1 << 3)                /* bit 3 */
+#define HAS_PWM_MIN             (1 << 4)                /* bit 4 */
+#define HAS_FAN(ix)             (1 << ((ix) + 5))       /* bits 5-10 */
+#define HAS_PWM(ix)             (1 << ((ix) + 11))      /* bits 11-16 */
+
 /* ---------------------------------------------------------------------
  * Data structures and manipulation thereof
  * --------------------------------------------------------------------- */
@@ -187,8 +208,7 @@ struct dme1737_data {
 
         u8 vid;
         u8 pwm_rr_en;
-        u8 has_pwm;
-        u8 has_fan;
+        u32 has_features;
 
         /* Register values */
         u16 in[7];
@@ -224,8 +244,11 @@ static const int IN_NOMINAL_SCH311x[] = {2500, 1500, 3300, 5000, 12000, 3300,
                                          3300};
 static const int IN_NOMINAL_SCH5027[] = {5000, 2250, 3300, 1125, 1125, 3300,
                                          3300};
+static const int IN_NOMINAL_SCH5127[] = {2500, 2250, 3300, 1125, 1125, 3300,
+                                         3300};
 #define IN_NOMINAL(type)        ((type) == sch311x ? IN_NOMINAL_SCH311x : \
                                  (type) == sch5027 ? IN_NOMINAL_SCH5027 : \
+                                 (type) == sch5127 ? IN_NOMINAL_SCH5127 : \
                                  IN_NOMINAL_DME1737)
 
 /* Voltage input
@@ -568,7 +591,7 @@ static struct dme1737_data *dme1737_update_device(struct device *dev)
 
         /* Sample register contents every 1 sec */
         if (time_after(jiffies, data->last_update + HZ) || !data->valid) {
-                if (data->type == dme1737) {
+                if (data->has_features & HAS_VID) {
                         data->vid = dme1737_read(data, DME1737_REG_VID) &
                                 0x3f;
                 }
@@ -599,7 +622,7 @@ static struct dme1737_data *dme1737_update_device(struct device *dev)
                                         DME1737_REG_TEMP_MIN(ix));
                         data->temp_max[ix] = dme1737_read(data,
                                         DME1737_REG_TEMP_MAX(ix));
-                        if (data->type != sch5027) {
+                        if (data->has_features & HAS_TEMP_OFFSET) {
                                 data->temp_offset[ix] = dme1737_read(data,
                                                 DME1737_REG_TEMP_OFFSET(ix));
                         }
@@ -626,7 +649,7 @@ static struct dme1737_data *dme1737_update_device(struct device *dev)
                 for (ix = 0; ix < ARRAY_SIZE(data->fan); ix++) {
                         /* Skip reading registers if optional fans are not
                          * present */
-                        if (!(data->has_fan & (1 << ix))) {
+                        if (!(data->has_features & HAS_FAN(ix))) {
                                 continue;
                         }
                         data->fan[ix] = dme1737_read(data,
@@ -650,7 +673,7 @@ static struct dme1737_data *dme1737_update_device(struct device *dev)
                 for (ix = 0; ix < ARRAY_SIZE(data->pwm); ix++) {
                         /* Skip reading registers if optional PWMs are not
                          * present */
-                        if (!(data->has_pwm & (1 << ix))) {
+                        if (!(data->has_features & HAS_PWM(ix))) {
                                 continue;
                         }
                         data->pwm[ix] = dme1737_read(data,
@@ -672,12 +695,24 @@ static struct dme1737_data *dme1737_update_device(struct device *dev)
 
                 /* Thermal zone registers */
                 for (ix = 0; ix < ARRAY_SIZE(data->zone_low); ix++) {
-                        data->zone_low[ix] = dme1737_read(data,
-                                        DME1737_REG_ZONE_LOW(ix));
-                        data->zone_abs[ix] = dme1737_read(data,
-                                        DME1737_REG_ZONE_ABS(ix));
+                        /* Skip reading registers if zone3 is not present */
+                        if ((ix == 2) && !(data->has_features & HAS_ZONE3)) {
+                                continue;
+                        }
+                        /* sch5127 zone2 registers are special */
+                        if ((ix == 1) && (data->type == sch5127)) {
+                                data->zone_low[1] = dme1737_read(data,
+                                                DME1737_REG_ZONE_LOW(2));
+                                data->zone_abs[1] = dme1737_read(data,
+                                                DME1737_REG_ZONE_ABS(2));
+                        } else {
+                                data->zone_low[ix] = dme1737_read(data,
+                                                DME1737_REG_ZONE_LOW(ix));
+                                data->zone_abs[ix] = dme1737_read(data,
+                                                DME1737_REG_ZONE_ABS(ix));
+                        }
                 }
-                if (data->type != sch5027) {
+                if (data->has_features & HAS_ZONE_HYST) {
                         for (ix = 0; ix < ARRAY_SIZE(data->zone_hyst); ix++) {
                                 data->zone_hyst[ix] = dme1737_read(data,
                                                 DME1737_REG_ZONE_HYST(ix));
@@ -1594,10 +1629,6 @@ static struct attribute *dme1737_attr[] ={
         &sensor_dev_attr_zone2_auto_point2_temp.dev_attr.attr,
         &sensor_dev_attr_zone2_auto_point3_temp.dev_attr.attr,
         &sensor_dev_attr_zone2_auto_channels_temp.dev_attr.attr,
-        &sensor_dev_attr_zone3_auto_point1_temp.dev_attr.attr,
-        &sensor_dev_attr_zone3_auto_point2_temp.dev_attr.attr,
-        &sensor_dev_attr_zone3_auto_point3_temp.dev_attr.attr,
-        &sensor_dev_attr_zone3_auto_channels_temp.dev_attr.attr,
         NULL
 };
 
@@ -1605,27 +1636,23 @@ static const struct attribute_group dme1737_group = {
         .attrs = dme1737_attr,
 };
 
-/* The following struct holds misc attributes, which are not available in all
- * chips. Their creation depends on the chip type which is determined during
- * module load. */
-static struct attribute *dme1737_misc_attr[] = {
-        /* Temperatures */
+/* The following struct holds temp offset attributes, which are not available
+ * in all chips. The following chips support them:
+ * DME1737, SCH311x */
+static struct attribute *dme1737_temp_offset_attr[] = {
         &sensor_dev_attr_temp1_offset.dev_attr.attr,
         &sensor_dev_attr_temp2_offset.dev_attr.attr,
         &sensor_dev_attr_temp3_offset.dev_attr.attr,
-        /* Zones */
-        &sensor_dev_attr_zone1_auto_point1_temp_hyst.dev_attr.attr,
-        &sensor_dev_attr_zone2_auto_point1_temp_hyst.dev_attr.attr,
-        &sensor_dev_attr_zone3_auto_point1_temp_hyst.dev_attr.attr,
         NULL
 };
 
-static const struct attribute_group dme1737_misc_group = {
-        .attrs = dme1737_misc_attr,
+static const struct attribute_group dme1737_temp_offset_group = {
+        .attrs = dme1737_temp_offset_attr,
 };
 
-/* The following struct holds VID-related attributes. Their creation
-   depends on the chip type which is determined during module load. */
+/* The following struct holds VID related attributes, which are not available
+ * in all chips. The following chips support them:
+ * DME1737 */
 static struct attribute *dme1737_vid_attr[] = {
         &dev_attr_vrm.attr,
         &dev_attr_cpu0_vid.attr,
@@ -1636,6 +1663,36 @@ static const struct attribute_group dme1737_vid_group = {
         .attrs = dme1737_vid_attr,
 };
 
+/* The following struct holds temp zone 3 related attributes, which are not
+ * available in all chips. The following chips support them:
+ * DME1737, SCH311x, SCH5027 */
+static struct attribute *dme1737_zone3_attr[] = {
+        &sensor_dev_attr_zone3_auto_point1_temp.dev_attr.attr,
+        &sensor_dev_attr_zone3_auto_point2_temp.dev_attr.attr,
+        &sensor_dev_attr_zone3_auto_point3_temp.dev_attr.attr,
+        &sensor_dev_attr_zone3_auto_channels_temp.dev_attr.attr,
+        NULL
+};
+
+static const struct attribute_group dme1737_zone3_group = {
+        .attrs = dme1737_zone3_attr,
+};
+
+
+/* The following struct holds temp zone hysteresis  related attributes, which
+ * are not available in all chips. The following chips support them:
+ * DME1737, SCH311x */
+static struct attribute *dme1737_zone_hyst_attr[] = {
+        &sensor_dev_attr_zone1_auto_point1_temp_hyst.dev_attr.attr,
+        &sensor_dev_attr_zone2_auto_point1_temp_hyst.dev_attr.attr,
+        &sensor_dev_attr_zone3_auto_point1_temp_hyst.dev_attr.attr,
+        NULL
+};
+
+static const struct attribute_group dme1737_zone_hyst_group = {
+        .attrs = dme1737_zone_hyst_attr,
+};
+
 /* The following structs hold the PWM attributes, some of which are optional.
  * Their creation depends on the chip configuration which is determined during
  * module load. */
@@ -1691,10 +1748,10 @@ static const struct attribute_group dme1737_pwm_group[] = {
         { .attrs = dme1737_pwm6_attr },
 };
 
-/* The following struct holds misc PWM attributes, which are not available in
- * all chips. Their creation depends on the chip type which is determined
+/* The following struct holds auto PWM min attributes, which are not available
+ * in all chips. Their creation depends on the chip type which is determined
  * during module load. */
-static struct attribute *dme1737_pwm_misc_attr[] = {
+static struct attribute *dme1737_auto_pwm_min_attr[] = {
         &sensor_dev_attr_pwm1_auto_pwm_min.dev_attr.attr,
         &sensor_dev_attr_pwm2_auto_pwm_min.dev_attr.attr,
         &sensor_dev_attr_pwm3_auto_pwm_min.dev_attr.attr,
@@ -1764,14 +1821,25 @@ static struct attribute *dme1737_zone_chmod_attr[] = {
         &sensor_dev_attr_zone2_auto_point1_temp.dev_attr.attr,
         &sensor_dev_attr_zone2_auto_point2_temp.dev_attr.attr,
         &sensor_dev_attr_zone2_auto_point3_temp.dev_attr.attr,
+        NULL
+};
+
+static const struct attribute_group dme1737_zone_chmod_group = {
+        .attrs = dme1737_zone_chmod_attr,
+};
+
+
+/* The permissions of the following zone 3 attributes are changed to read-
+ * writeable if the chip is *not* locked. Otherwise they stay read-only. */
+static struct attribute *dme1737_zone3_chmod_attr[] = {
         &sensor_dev_attr_zone3_auto_point1_temp.dev_attr.attr,
         &sensor_dev_attr_zone3_auto_point2_temp.dev_attr.attr,
         &sensor_dev_attr_zone3_auto_point3_temp.dev_attr.attr,
         NULL
 };
 
-static const struct attribute_group dme1737_zone_chmod_group = {
-        .attrs = dme1737_zone_chmod_attr,
+static const struct attribute_group dme1737_zone3_chmod_group = {
+        .attrs = dme1737_zone3_chmod_attr,
 };
 
 /* The permissions of the following PWM attributes are changed to read-
@@ -1887,30 +1955,35 @@ static void dme1737_remove_files(struct device *dev)
         int ix;
 
         for (ix = 0; ix < ARRAY_SIZE(dme1737_fan_group); ix++) {
-                if (data->has_fan & (1 << ix)) {
+                if (data->has_features & HAS_FAN(ix)) {
                         sysfs_remove_group(&dev->kobj,
                                            &dme1737_fan_group[ix]);
                 }
         }
 
         for (ix = 0; ix < ARRAY_SIZE(dme1737_pwm_group); ix++) {
-                if (data->has_pwm & (1 << ix)) {
+                if (data->has_features & HAS_PWM(ix)) {
                         sysfs_remove_group(&dev->kobj,
                                            &dme1737_pwm_group[ix]);
-                        if (data->type != sch5027 && ix < 3) {
+                        if ((data->has_features & HAS_PWM_MIN) && ix < 3) {
                                 sysfs_remove_file(&dev->kobj,
-                                                  dme1737_pwm_misc_attr[ix]);
+                                                dme1737_auto_pwm_min_attr[ix]);
                         }
                 }
         }
 
-        if (data->type != sch5027) {
-                sysfs_remove_group(&dev->kobj, &dme1737_misc_group);
+        if (data->has_features & HAS_TEMP_OFFSET) {
+                sysfs_remove_group(&dev->kobj, &dme1737_temp_offset_group);
         }
-        if (data->type == dme1737) {
+        if (data->has_features & HAS_VID) {
                 sysfs_remove_group(&dev->kobj, &dme1737_vid_group);
         }
-
+        if (data->has_features & HAS_ZONE3) {
+                sysfs_remove_group(&dev->kobj, &dme1737_zone3_group);
+        }
+        if (data->has_features & HAS_ZONE_HYST) {
+                sysfs_remove_group(&dev->kobj, &dme1737_zone_hyst_group);
+        }
         sysfs_remove_group(&dev->kobj, &dme1737_group);
 
         if (!data->client) {
@@ -1934,23 +2007,31 @@ static int dme1737_create_files(struct device *dev)
                 goto exit_remove;
         }
 
-        /* Create misc sysfs attributes */
-        if ((data->type != sch5027) &&
+        /* Create chip-dependent sysfs attributes */
+        if ((data->has_features & HAS_TEMP_OFFSET) &&
             (err = sysfs_create_group(&dev->kobj,
-                                      &dme1737_misc_group))) {
+                                      &dme1737_temp_offset_group))) {
                 goto exit_remove;
         }
-
-        /* Create VID-related sysfs attributes */
-        if ((data->type == dme1737) &&
+        if ((data->has_features & HAS_VID) &&
             (err = sysfs_create_group(&dev->kobj,
                                       &dme1737_vid_group))) {
                 goto exit_remove;
         }
+        if ((data->has_features & HAS_ZONE3) &&
+            (err = sysfs_create_group(&dev->kobj,
+                                      &dme1737_zone3_group))) {
+                goto exit_remove;
+        }
+        if ((data->has_features & HAS_ZONE_HYST) &&
+            (err = sysfs_create_group(&dev->kobj,
+                                      &dme1737_zone_hyst_group))) {
+                goto exit_remove;
+        }
 
         /* Create fan sysfs attributes */
         for (ix = 0; ix < ARRAY_SIZE(dme1737_fan_group); ix++) {
-                if (data->has_fan & (1 << ix)) {
+                if (data->has_features & HAS_FAN(ix)) {
                         if ((err = sysfs_create_group(&dev->kobj,
                                                 &dme1737_fan_group[ix]))) {
                                 goto exit_remove;
@@ -1960,14 +2041,14 @@ static int dme1737_create_files(struct device *dev)
 
         /* Create PWM sysfs attributes */
         for (ix = 0; ix < ARRAY_SIZE(dme1737_pwm_group); ix++) {
-                if (data->has_pwm & (1 << ix)) {
+                if (data->has_features & HAS_PWM(ix)) {
                         if ((err = sysfs_create_group(&dev->kobj,
                                                 &dme1737_pwm_group[ix]))) {
                                 goto exit_remove;
                         }
-                        if (data->type != sch5027 && ix < 3 &&
+                        if ((data->has_features & HAS_PWM_MIN) && ix < 3 &&
                             (err = sysfs_create_file(&dev->kobj,
-                                                dme1737_pwm_misc_attr[ix]))) {
+                                        dme1737_auto_pwm_min_attr[ix]))) {
                                 goto exit_remove;
                         }
                 }
@@ -1983,21 +2064,30 @@ static int dme1737_create_files(struct device *dev)
                 dme1737_chmod_group(dev, &dme1737_zone_chmod_group,
                                     S_IRUGO | S_IWUSR);
 
-                /* Change permissions of misc sysfs attributes */
-                if (data->type != sch5027) {
-                        dme1737_chmod_group(dev, &dme1737_misc_group,
+                /* Change permissions of chip-dependent sysfs attributes */
+                if (data->has_features & HAS_TEMP_OFFSET) {
+                        dme1737_chmod_group(dev, &dme1737_temp_offset_group,
+                                            S_IRUGO | S_IWUSR);
+                }
+                if (data->has_features & HAS_ZONE3) {
+                        dme1737_chmod_group(dev, &dme1737_zone3_chmod_group,
+                                            S_IRUGO | S_IWUSR);
+                }
+                if (data->has_features & HAS_ZONE_HYST) {
+                        dme1737_chmod_group(dev, &dme1737_zone_hyst_group,
                                             S_IRUGO | S_IWUSR);
                 }
 
                 /* Change permissions of PWM sysfs attributes */
                 for (ix = 0; ix < ARRAY_SIZE(dme1737_pwm_chmod_group); ix++) {
-                        if (data->has_pwm & (1 << ix)) {
+                        if (data->has_features & HAS_PWM(ix)) {
                                 dme1737_chmod_group(dev,
                                                 &dme1737_pwm_chmod_group[ix],
                                                 S_IRUGO | S_IWUSR);
-                                if (data->type != sch5027 && ix < 3) {
+                                if ((data->has_features & HAS_PWM_MIN) &&
+                                    ix < 3) {
                                         dme1737_chmod_file(dev,
-                                                dme1737_pwm_misc_attr[ix],
+                                                dme1737_auto_pwm_min_attr[ix],
                                                 S_IRUGO | S_IWUSR);
                                 }
                         }
@@ -2005,7 +2095,7 @@ static int dme1737_create_files(struct device *dev)
 
                 /* Change permissions of pwm[1-3] if in manual mode */
                 for (ix = 0; ix < 3; ix++) {
-                        if ((data->has_pwm & (1 << ix)) &&
+                        if ((data->has_features & HAS_PWM(ix)) &&
                             (PWM_EN_FROM_REG(data->pwm_config[ix]) == 1)) {
                                 dme1737_chmod_file(dev,
                                                 dme1737_pwm_chmod_attr[ix],
@@ -2052,20 +2142,20 @@ static int dme1737_init_device(struct device *dev)
                 return -EFAULT;
         }
 
-        /* Determine which optional fan and pwm features are enabled/present */
+        /* Determine which optional fan and pwm features are enabled (only
+         * valid for I2C devices) */
         if (client) {   /* I2C chip */
                 data->config2 = dme1737_read(data, DME1737_REG_CONFIG2);
                 /* Check if optional fan3 input is enabled */
                 if (data->config2 & 0x04) {
-                        data->has_fan |= (1 << 2);
+                        data->has_features |= HAS_FAN(2);
                 }
 
                 /* Fan4 and pwm3 are only available if the client's I2C address
                  * is the default 0x2e. Otherwise the I/Os associated with
                  * these functions are used for addr enable/select. */
                 if (client->addr == 0x2e) {
-                        data->has_fan |= (1 << 3);
-                        data->has_pwm |= (1 << 2);
+                        data->has_features |= HAS_FAN(3) | HAS_PWM(2);
                 }
 
                 /* Determine which of the optional fan[5-6] and pwm[5-6]
@@ -2077,26 +2167,40 @@ static int dme1737_init_device(struct device *dev)
                         dev_warn(dev, "Failed to query Super-IO for optional "
                                  "features.\n");
                 }
-        } else {   /* ISA chip */
-                /* Fan3 and pwm3 are always available. Fan[4-5] and pwm[5-6]
-                 * don't exist in the ISA chip. */
-                data->has_fan |= (1 << 2);
-                data->has_pwm |= (1 << 2);
         }
 
-        /* Fan1, fan2, pwm1, and pwm2 are always present */
-        data->has_fan |= 0x03;
-        data->has_pwm |= 0x03;
+        /* Fan[1-2] and pwm[1-2] are present in all chips */
+        data->has_features |= HAS_FAN(0) | HAS_FAN(1) | HAS_PWM(0) | HAS_PWM(1);
+
+        /* Chip-dependent features */
+        switch (data->type) {
+        case dme1737:
+                data->has_features |= HAS_TEMP_OFFSET | HAS_VID | HAS_ZONE3 |
+                        HAS_ZONE_HYST | HAS_PWM_MIN;
+                break;
+        case sch311x:
+                data->has_features |= HAS_TEMP_OFFSET | HAS_ZONE3 |
+                        HAS_ZONE_HYST | HAS_PWM_MIN | HAS_FAN(2) | HAS_PWM(2);
+                break;
+        case sch5027:
+                data->has_features |= HAS_ZONE3;
+                break;
+        case sch5127:
+                data->has_features |= HAS_FAN(2) | HAS_PWM(2);
+                break;
+        default:
+                break;
+        }
 
         dev_info(dev, "Optional features: pwm3=%s, pwm5=%s, pwm6=%s, "
                  "fan3=%s, fan4=%s, fan5=%s, fan6=%s.\n",
-                 (data->has_pwm & (1 << 2)) ? "yes" : "no",
-                 (data->has_pwm & (1 << 4)) ? "yes" : "no",
-                 (data->has_pwm & (1 << 5)) ? "yes" : "no",
-                 (data->has_fan & (1 << 2)) ? "yes" : "no",
-                 (data->has_fan & (1 << 3)) ? "yes" : "no",
-                 (data->has_fan & (1 << 4)) ? "yes" : "no",
-                 (data->has_fan & (1 << 5)) ? "yes" : "no");
+                 (data->has_features & HAS_PWM(2)) ? "yes" : "no",
+                 (data->has_features & HAS_PWM(4)) ? "yes" : "no",
+                 (data->has_features & HAS_PWM(5)) ? "yes" : "no",
+                 (data->has_features & HAS_FAN(2)) ? "yes" : "no",
+                 (data->has_features & HAS_FAN(3)) ? "yes" : "no",
+                 (data->has_features & HAS_FAN(4)) ? "yes" : "no",
+                 (data->has_features & HAS_FAN(5)) ? "yes" : "no");
 
         reg = dme1737_read(data, DME1737_REG_TACH_PWM);
         /* Inform if fan-to-pwm mapping differs from the default */
@@ -2122,7 +2226,7 @@ static int dme1737_init_device(struct device *dev)
                 for (ix = 0; ix < 3; ix++) {
                         data->pwm_config[ix] = dme1737_read(data,
                                                 DME1737_REG_PWM_CONFIG(ix));
-                        if ((data->has_pwm & (1 << ix)) &&
+                        if ((data->has_features & HAS_PWM(ix)) &&
                             (PWM_EN_FROM_REG(data->pwm_config[ix]) == -1)) {
                                 dev_info(dev, "Switching pwm%d to "
                                          "manual mode.\n", ix + 1);
@@ -2142,7 +2246,7 @@ static int dme1737_init_device(struct device *dev)
         data->pwm_acz[2] = 4;   /* pwm3 -> zone3 */
 
         /* Set VRM */
-        if (data->type == dme1737) {
+        if (data->has_features & HAS_VID) {
                 data->vrm = vid_which_vrm();
         }
 
@@ -2163,10 +2267,10 @@ static int dme1737_i2c_get_features(int sio_cip, struct dme1737_data *data)
         dme1737_sio_enter(sio_cip);
 
         /* Check device ID
-         * The DME1737 can return either 0x78 or 0x77 as its device ID.
-         * The SCH5027 returns 0x89 as its device ID. */
+         * We currently know about two kinds of DME1737 and SCH5027. */
         reg = force_id ? force_id : dme1737_sio_inb(sio_cip, 0x20);
-        if (!(reg == 0x77 || reg == 0x78 || reg == 0x89)) {
+        if (!(reg == DME1737_ID_1 || reg == DME1737_ID_2 ||
+              reg == SCH5027_ID)) {
                 err = -ENODEV;
                 goto exit;
         }
@@ -2185,16 +2289,16 @@ static int dme1737_i2c_get_features(int sio_cip, struct dme1737_data *data)
          * are enabled and available. Bits [3:2] of registers 0x43-0x46 are set
          * to '10' if the respective feature is enabled. */
         if ((inb(addr + 0x43) & 0x0c) == 0x08) { /* fan6 */
-                data->has_fan |= (1 << 5);
+                data->has_features |= HAS_FAN(5);
         }
         if ((inb(addr + 0x44) & 0x0c) == 0x08) { /* pwm6 */
-                data->has_pwm |= (1 << 5);
+                data->has_features |= HAS_PWM(5);
         }
         if ((inb(addr + 0x45) & 0x0c) == 0x08) { /* fan5 */
-                data->has_fan |= (1 << 4);
+                data->has_features |= HAS_FAN(4);
         }
         if ((inb(addr + 0x46) & 0x0c) == 0x08) { /* pwm5 */
-                data->has_pwm |= (1 << 4);
+                data->has_features |= HAS_PWM(4);
         }
 
 exit:
@@ -2222,7 +2326,6 @@ static int dme1737_i2c_detect(struct i2c_client *client,
         if (company == DME1737_COMPANY_SMSC &&
             verstep == SCH5027_VERSTEP) {
                 name = "sch5027";
-
         } else if (company == DME1737_COMPANY_SMSC &&
                    (verstep & DME1737_VERSTEP_MASK) == DME1737_VERSTEP) {
                 name = "dme1737";
@@ -2329,10 +2432,10 @@ static int __init dme1737_isa_detect(int sio_cip, unsigned short *addr)
         dme1737_sio_enter(sio_cip);
 
         /* Check device ID
-         * We currently know about SCH3112 (0x7c), SCH3114 (0x7d), and
-         * SCH3116 (0x7f). */
+         * We currently know about SCH3112, SCH3114, SCH3116, and SCH5127 */
         reg = force_id ? force_id : dme1737_sio_inb(sio_cip, 0x20);
-        if (!(reg == 0x7c || reg == 0x7d || reg == 0x7f)) {
+        if (!(reg == SCH3112_ID || reg == SCH3114_ID || reg == SCH3116_ID ||
+              reg == SCH5127_ID)) {
                 err = -ENODEV;
                 goto exit;
         }
@@ -2424,23 +2527,42 @@ static int __devinit dme1737_isa_probe(struct platform_device *pdev)
         platform_set_drvdata(pdev, data);
 
         /* Skip chip detection if module is loaded with force_id parameter */
-        if (!force_id) {
+        switch (force_id) {
+        case SCH3112_ID:
+        case SCH3114_ID:
+        case SCH3116_ID:
+                data->type = sch311x;
+                break;
+        case SCH5127_ID:
+                data->type = sch5127;
+                break;
+        default:
                 company = dme1737_read(data, DME1737_REG_COMPANY);
                 device = dme1737_read(data, DME1737_REG_DEVICE);
 
-                if (!((company == DME1737_COMPANY_SMSC) &&
-                      (device == SCH311X_DEVICE))) {
+                if ((company == DME1737_COMPANY_SMSC) &&
+                    (device == SCH311X_DEVICE)) {
+                        data->type = sch311x;
+                } else if ((company == DME1737_COMPANY_SMSC) &&
+                           (device == SCH5127_DEVICE)) {
+                        data->type = sch5127;
+                } else {
                         err = -ENODEV;
                         goto exit_kfree;
                 }
         }
-        data->type = sch311x;
 
-        /* Fill in the remaining client fields and initialize the mutex */
-        data->name = "sch311x";
+        if (data->type == sch5127) {
+                data->name = "sch5127";
+        } else {
+                data->name = "sch311x";
+        }
+
+        /* Initialize the mutex */
         mutex_init(&data->update_lock);
 
-        dev_info(dev, "Found a SCH311x chip at 0x%04x\n", data->addr);
+        dev_info(dev, "Found a %s chip at 0x%04x\n",
+                 data->type == sch5127 ? "SCH5127" : "SCH311x", data->addr);
 
         /* Initialize the chip */
         if ((err = dme1737_init_device(dev))) {
diff --git a/drivers/hwmon/emc1403.c b/drivers/hwmon/emc1403.c
new file mode 100644
index 000000000000..0e4b5642638d
--- /dev/null
+++ b/drivers/hwmon/emc1403.c
@@ -0,0 +1,344 @@
+/*
+ * emc1403.c - SMSC Thermal Driver
+ *
+ * Copyright (C) 2008 Intel Corp
+ *
+ *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *
+ * 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.
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *
+ * TODO
+ *      -       cache alarm and critical limit registers
+ *      -       add emc1404 support
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
+#include <linux/sysfs.h>
+#include <linux/mutex.h>
+
+#define THERMAL_PID_REG         0xfd
+#define THERMAL_SMSC_ID_REG     0xfe
+#define THERMAL_REVISION_REG    0xff
+
+struct thermal_data {
+        struct device *hwmon_dev;
+        struct mutex mutex;
+        /* Cache the hyst value so we don't keep re-reading it. In theory
+           we could cache it forever as nobody else should be writing it. */
+        u8 cached_hyst;
+        unsigned long hyst_valid;
+};
+
+static ssize_t show_temp(struct device *dev,
+                        struct device_attribute *attr, char *buf)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
+        int retval = i2c_smbus_read_byte_data(client, sda->index);
+
+        if (retval < 0)
+                return retval;
+        return sprintf(buf, "%d000\n", retval);
+}
+
+static ssize_t show_bit(struct device *dev,
+                        struct device_attribute *attr, char *buf)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct sensor_device_attribute_2 *sda = to_sensor_dev_attr_2(attr);
+        int retval = i2c_smbus_read_byte_data(client, sda->nr);
+
+        if (retval < 0)
+                return retval;
+        retval &= sda->index;
+        return sprintf(buf, "%d\n", retval ? 1 : 0);
+}
+
+static ssize_t store_temp(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t count)
+{
+        struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
+        struct i2c_client *client = to_i2c_client(dev);
+        unsigned long val;
+        int retval;
+
+        if (strict_strtoul(buf, 10, &val))
+                return -EINVAL;
+        retval = i2c_smbus_write_byte_data(client, sda->index,
+                                        DIV_ROUND_CLOSEST(val, 1000));
+        if (retval < 0)
+                return retval;
+        return count;
+}
+
+static ssize_t show_hyst(struct device *dev,
+                        struct device_attribute *attr, char *buf)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct thermal_data *data = i2c_get_clientdata(client);
+        struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
+        int retval;
+        int hyst;
+
+        retval = i2c_smbus_read_byte_data(client, sda->index);
+        if (retval < 0)
+                return retval;
+
+        if (time_after(jiffies, data->hyst_valid)) {
+                hyst = i2c_smbus_read_byte_data(client, 0x21);
+                if (hyst < 0)
+                        return retval;
+                data->cached_hyst = hyst;
+                data->hyst_valid = jiffies + HZ;
+        }
+        return sprintf(buf, "%d000\n", retval - data->cached_hyst);
+}
+
+static ssize_t store_hyst(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t count)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct thermal_data *data = i2c_get_clientdata(client);
+        struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
+        int retval;
+        int hyst;
+        unsigned long val;
+
+        if (strict_strtoul(buf, 10, &val))
+                return -EINVAL;
+
+        mutex_lock(&data->mutex);
+        retval = i2c_smbus_read_byte_data(client, sda->index);
+        if (retval < 0)
+                goto fail;
+
+        hyst = val - retval * 1000;
+        hyst = DIV_ROUND_CLOSEST(hyst, 1000);
+        if (hyst < 0 || hyst > 255) {
+                retval = -ERANGE;
+                goto fail;
+        }
+
+        retval = i2c_smbus_write_byte_data(client, 0x21, hyst);
+        if (retval == 0) {
+                retval = count;
+                data->cached_hyst = hyst;
+                data->hyst_valid = jiffies + HZ;
+        }
+fail:
+        mutex_unlock(&data->mutex);
+        return retval;
+}
+
+/*
+ *      Sensors. We pass the actual i2c register to the methods.
+ */
+
+static SENSOR_DEVICE_ATTR(temp1_min, S_IRUGO | S_IWUSR,
+        show_temp, store_temp, 0x06);
+static SENSOR_DEVICE_ATTR(temp1_max, S_IRUGO | S_IWUSR,
+        show_temp, store_temp, 0x05);
+static SENSOR_DEVICE_ATTR(temp1_crit, S_IRUGO | S_IWUSR,
+        show_temp, store_temp, 0x20);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL, 0x00);
+static SENSOR_DEVICE_ATTR_2(temp1_min_alarm, S_IRUGO,
+        show_bit, NULL, 0x36, 0x01);
+static SENSOR_DEVICE_ATTR_2(temp1_max_alarm, S_IRUGO,
+        show_bit, NULL, 0x35, 0x01);
+static SENSOR_DEVICE_ATTR_2(temp1_crit_alarm, S_IRUGO,
+        show_bit, NULL, 0x37, 0x01);
+static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IRUGO | S_IWUSR,
+        show_hyst, store_hyst, 0x20);
+
+static SENSOR_DEVICE_ATTR(temp2_min, S_IRUGO | S_IWUSR,
+        show_temp, store_temp, 0x08);
+static SENSOR_DEVICE_ATTR(temp2_max, S_IRUGO | S_IWUSR,
+        show_temp, store_temp, 0x07);
+static SENSOR_DEVICE_ATTR(temp2_crit, S_IRUGO | S_IWUSR,
+        show_temp, store_temp, 0x19);
+static SENSOR_DEVICE_ATTR(temp2_input, S_IRUGO, show_temp, NULL, 0x01);
+static SENSOR_DEVICE_ATTR_2(temp2_min_alarm, S_IRUGO,
+        show_bit, NULL, 0x36, 0x02);
+static SENSOR_DEVICE_ATTR_2(temp2_max_alarm, S_IRUGO,
+        show_bit, NULL, 0x35, 0x02);
+static SENSOR_DEVICE_ATTR_2(temp2_crit_alarm, S_IRUGO,
+        show_bit, NULL, 0x37, 0x02);
+static SENSOR_DEVICE_ATTR(temp2_crit_hyst, S_IRUGO | S_IWUSR,
+        show_hyst, store_hyst, 0x19);
+
+static SENSOR_DEVICE_ATTR(temp3_min, S_IRUGO | S_IWUSR,
+        show_temp, store_temp, 0x16);
+static SENSOR_DEVICE_ATTR(temp3_max, S_IRUGO | S_IWUSR,
+        show_temp, store_temp, 0x15);
+static SENSOR_DEVICE_ATTR(temp3_crit, S_IRUGO | S_IWUSR,
+        show_temp, store_temp, 0x1A);
+static SENSOR_DEVICE_ATTR(temp3_input, S_IRUGO, show_temp, NULL, 0x23);
+static SENSOR_DEVICE_ATTR_2(temp3_min_alarm, S_IRUGO,
+        show_bit, NULL, 0x36, 0x04);
+static SENSOR_DEVICE_ATTR_2(temp3_max_alarm, S_IRUGO,
+        show_bit, NULL, 0x35, 0x04);
+static SENSOR_DEVICE_ATTR_2(temp3_crit_alarm, S_IRUGO,
+        show_bit, NULL, 0x37, 0x04);
+static SENSOR_DEVICE_ATTR(temp3_crit_hyst, S_IRUGO | S_IWUSR,
+        show_hyst, store_hyst, 0x1A);
+
+static struct attribute *mid_att_thermal[] = {
+        &sensor_dev_attr_temp1_min.dev_attr.attr,
+        &sensor_dev_attr_temp1_max.dev_attr.attr,
+        &sensor_dev_attr_temp1_crit.dev_attr.attr,
+        &sensor_dev_attr_temp1_input.dev_attr.attr,
+        &sensor_dev_attr_temp1_min_alarm.dev_attr.attr,
+        &sensor_dev_attr_temp1_max_alarm.dev_attr.attr,
+        &sensor_dev_attr_temp1_crit_alarm.dev_attr.attr,
+        &sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+        &sensor_dev_attr_temp2_min.dev_attr.attr,
+        &sensor_dev_attr_temp2_max.dev_attr.attr,
+        &sensor_dev_attr_temp2_crit.dev_attr.attr,
+        &sensor_dev_attr_temp2_input.dev_attr.attr,
+        &sensor_dev_attr_temp2_min_alarm.dev_attr.attr,
+        &sensor_dev_attr_temp2_max_alarm.dev_attr.attr,
+        &sensor_dev_attr_temp2_crit_alarm.dev_attr.attr,
+        &sensor_dev_attr_temp2_crit_hyst.dev_attr.attr,
+        &sensor_dev_attr_temp3_min.dev_attr.attr,
+        &sensor_dev_attr_temp3_max.dev_attr.attr,
+        &sensor_dev_attr_temp3_crit.dev_attr.attr,
+        &sensor_dev_attr_temp3_input.dev_attr.attr,
+        &sensor_dev_attr_temp3_min_alarm.dev_attr.attr,
+        &sensor_dev_attr_temp3_max_alarm.dev_attr.attr,
+        &sensor_dev_attr_temp3_crit_alarm.dev_attr.attr,
+        &sensor_dev_attr_temp3_crit_hyst.dev_attr.attr,
+        NULL
+};
+
+static const struct attribute_group m_thermal_gr = {
+        .attrs = mid_att_thermal
+};
+
+static int emc1403_detect(struct i2c_client *client,
+                        struct i2c_board_info *info)
+{
+        int id;
+        /* Check if thermal chip is SMSC and EMC1403 */
+
+        id = i2c_smbus_read_byte_data(client, THERMAL_SMSC_ID_REG);
+        if (id != 0x5d)
+                return -ENODEV;
+
+        /* Note: 0x25 is the 1404 which is very similar and this
+           driver could be extended */
+        id = i2c_smbus_read_byte_data(client, THERMAL_PID_REG);
+        if (id != 0x21)
+                return -ENODEV;
+
+        id = i2c_smbus_read_byte_data(client, THERMAL_REVISION_REG);
+        if (id != 0x01)
+                return -ENODEV;
+
+        strlcpy(info->type, "emc1403", I2C_NAME_SIZE);
+        return 0;
+}
+
+static int emc1403_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        int res;
+        struct thermal_data *data;
+
+        data = kzalloc(sizeof(struct thermal_data), GFP_KERNEL);
+        if (data == NULL) {
+                dev_warn(&client->dev, "out of memory");
+                return -ENOMEM;
+        }
+
+        i2c_set_clientdata(client, data);
+        mutex_init(&data->mutex);
+        data->hyst_valid = jiffies - 1;         /* Expired */
+
+        res = sysfs_create_group(&client->dev.kobj, &m_thermal_gr);
+        if (res) {
+                dev_warn(&client->dev, "create group failed\n");
+                hwmon_device_unregister(data->hwmon_dev);
+                goto thermal_error1;
+        }
+        data->hwmon_dev = hwmon_device_register(&client->dev);
+        if (IS_ERR(data->hwmon_dev)) {
+                res = PTR_ERR(data->hwmon_dev);
+                dev_warn(&client->dev, "register hwmon dev failed\n");
+                goto thermal_error2;
+        }
+        dev_info(&client->dev, "EMC1403 Thermal chip found\n");
+        return res;
+
+thermal_error2:
+        sysfs_remove_group(&client->dev.kobj, &m_thermal_gr);
+thermal_error1:
+        kfree(data);
+        return res;
+}
+
+static int emc1403_remove(struct i2c_client *client)
+{
+        struct thermal_data *data = i2c_get_clientdata(client);
+
+        hwmon_device_unregister(data->hwmon_dev);
+        sysfs_remove_group(&client->dev.kobj, &m_thermal_gr);
+        kfree(data);
+        return 0;
+}
+
+static const unsigned short emc1403_address_list[] = {
+        0x18, 0x2a, 0x4c, 0x4d, I2C_CLIENT_END
+};
+
+static const struct i2c_device_id emc1403_idtable[] = {
+        { "emc1403", 0 },
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, emc1403_idtable);
+
+static struct i2c_driver sensor_emc1403 = {
+        .class = I2C_CLASS_HWMON,
+        .driver = {
+                .name = "emc1403",
+        },
+        .detect = emc1403_detect,
+        .probe = emc1403_probe,
+        .remove = emc1403_remove,
+        .id_table = emc1403_idtable,
+        .address_list = emc1403_address_list,
+};
+
+static int __init sensor_emc1403_init(void)
+{
+        return i2c_add_driver(&sensor_emc1403);
+}
+
+static void  __exit sensor_emc1403_exit(void)
+{
+        i2c_del_driver(&sensor_emc1403);
+}
+
+module_init(sensor_emc1403_init);
+module_exit(sensor_emc1403_exit);
+
+MODULE_AUTHOR("Kalhan Trisal <kalhan.trisal@intel.com");
+MODULE_DESCRIPTION("emc1403 Thermal Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/hwmon/f71882fg.c b/drivers/hwmon/f71882fg.c
index a95fa4256caa..537841ef44b9 100644
--- a/drivers/hwmon/f71882fg.c
+++ b/drivers/hwmon/f71882fg.c
@@ -856,21 +856,19 @@ static inline int superio_inb(int base, int reg)
 static int superio_inw(int base, int reg)
 {
         int val;
-        outb(reg++, base);
-        val = inb(base + 1) << 8;
-        outb(reg, base);
-        val |= inb(base + 1);
+        val  = superio_inb(base, reg) << 8;
+        val |= superio_inb(base, reg + 1);
         return val;
 }
 
 static inline void superio_enter(int base)
 {
         /* according to the datasheet the key must be send twice! */
-        outb( SIO_UNLOCK_KEY, base);
-        outb( SIO_UNLOCK_KEY, base);
+        outb(SIO_UNLOCK_KEY, base);
+        outb(SIO_UNLOCK_KEY, base);
 }
 
-static inline void superio_select( int base, int ld)
+static inline void superio_select(int base, int ld)
 {
         outb(SIO_REG_LDSEL, base);
         outb(ld, base + 1);
@@ -905,10 +903,8 @@ static u16 f71882fg_read16(struct f71882fg_data *data, u8 reg)
 {
         u16 val;
 
-        outb(reg++, data->addr + ADDR_REG_OFFSET);
-        val = inb(data->addr + DATA_REG_OFFSET) << 8;
-        outb(reg, data->addr + ADDR_REG_OFFSET);
-        val |= inb(data->addr + DATA_REG_OFFSET);
+        val  = f71882fg_read8(data, reg) << 8;
+        val |= f71882fg_read8(data, reg + 1);
 
         return val;
 }
@@ -921,10 +917,8 @@ static void f71882fg_write8(struct f71882fg_data *data, u8 reg, u8 val)
 
 static void f71882fg_write16(struct f71882fg_data *data, u8 reg, u16 val)
 {
-        outb(reg++, data->addr + ADDR_REG_OFFSET);
-        outb(val >> 8, data->addr + DATA_REG_OFFSET);
-        outb(reg, data->addr + ADDR_REG_OFFSET);
-        outb(val & 255, data->addr + DATA_REG_OFFSET);
+        f71882fg_write8(data, reg,     val >> 8);
+        f71882fg_write8(data, reg + 1, val & 0xff);
 }
 
 static u16 f71882fg_read_temp(struct f71882fg_data *data, int nr)
@@ -945,7 +939,7 @@ static struct f71882fg_data *f71882fg_update_device(struct device *dev)
         mutex_lock(&data->update_lock);
 
         /* Update once every 60 seconds */
-        if ( time_after(jiffies, data->last_limits + 60 * HZ ) ||
+        if (time_after(jiffies, data->last_limits + 60 * HZ) ||
                         !data->valid) {
                 if (data->type == f71882fg || data->type == f71889fg) {
                         data->in1_max =
@@ -1127,8 +1121,12 @@ static ssize_t store_fan_full_speed(struct device *dev,
                                     const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int nr = to_sensor_dev_attr_2(devattr)->index;
-        long val = simple_strtol(buf, NULL, 10);
+        int err, nr = to_sensor_dev_attr_2(devattr)->index;
+        long val;
+
+        err = strict_strtol(buf, 10, &val);
+        if (err)
+                return err;
 
         val = SENSORS_LIMIT(val, 23, 1500000);
         val = fan_to_reg(val);
@@ -1157,8 +1155,12 @@ static ssize_t store_fan_beep(struct device *dev, struct device_attribute
         *devattr, const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int nr = to_sensor_dev_attr_2(devattr)->index;
-        unsigned long val = simple_strtoul(buf, NULL, 10);
+        int err, nr = to_sensor_dev_attr_2(devattr)->index;
+        unsigned long val;
+
+        err = strict_strtoul(buf, 10, &val);
+        if (err)
+                return err;
 
         mutex_lock(&data->update_lock);
         data->fan_beep = f71882fg_read8(data, F71882FG_REG_FAN_BEEP);
@@ -1206,7 +1208,14 @@ static ssize_t store_in_max(struct device *dev, struct device_attribute
         *devattr, const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        long val = simple_strtol(buf, NULL, 10) / 8;
+        int err;
+        long val;
+
+        err = strict_strtol(buf, 10, &val);
+        if (err)
+                return err;
+
+        val /= 8;
         val = SENSORS_LIMIT(val, 0, 255);
 
         mutex_lock(&data->update_lock);
@@ -1233,8 +1242,12 @@ static ssize_t store_in_beep(struct device *dev, struct device_attribute
         *devattr, const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int nr = to_sensor_dev_attr_2(devattr)->index;
-        unsigned long val = simple_strtoul(buf, NULL, 10);
+        int err, nr = to_sensor_dev_attr_2(devattr)->index;
+        unsigned long val;
+
+        err = strict_strtoul(buf, 10, &val);
+        if (err)
+                return err;
 
         mutex_lock(&data->update_lock);
         data->in_beep = f71882fg_read8(data, F71882FG_REG_IN_BEEP);
@@ -1299,8 +1312,14 @@ static ssize_t store_temp_max(struct device *dev, struct device_attribute
         *devattr, const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int nr = to_sensor_dev_attr_2(devattr)->index;
-        long val = simple_strtol(buf, NULL, 10) / 1000;
+        int err, nr = to_sensor_dev_attr_2(devattr)->index;
+        long val;
+
+        err = strict_strtol(buf, 10, &val);
+        if (err)
+                return err;
+
+        val /= 1000;
         val = SENSORS_LIMIT(val, 0, 255);
 
         mutex_lock(&data->update_lock);
@@ -1333,10 +1352,16 @@ static ssize_t store_temp_max_hyst(struct device *dev, struct device_attribute
         *devattr, const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int nr = to_sensor_dev_attr_2(devattr)->index;
-        long val = simple_strtol(buf, NULL, 10) / 1000;
+        int err, nr = to_sensor_dev_attr_2(devattr)->index;
         ssize_t ret = count;
         u8 reg;
+        long val;
+
+        err = strict_strtol(buf, 10, &val);
+        if (err)
+                return err;
+
+        val /= 1000;
 
         mutex_lock(&data->update_lock);
 
@@ -1372,8 +1397,14 @@ static ssize_t store_temp_crit(struct device *dev, struct device_attribute
         *devattr, const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int nr = to_sensor_dev_attr_2(devattr)->index;
-        long val = simple_strtol(buf, NULL, 10) / 1000;
+        int err, nr = to_sensor_dev_attr_2(devattr)->index;
+        long val;
+
+        err = strict_strtol(buf, 10, &val);
+        if (err)
+                return err;
+
+        val /= 1000;
         val = SENSORS_LIMIT(val, 0, 255);
 
         mutex_lock(&data->update_lock);
@@ -1427,8 +1458,12 @@ static ssize_t store_temp_beep(struct device *dev, struct device_attribute
         *devattr, const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int nr = to_sensor_dev_attr_2(devattr)->index;
-        unsigned long val = simple_strtoul(buf, NULL, 10);
+        int err, nr = to_sensor_dev_attr_2(devattr)->index;
+        unsigned long val;
+
+        err = strict_strtoul(buf, 10, &val);
+        if (err)
+                return err;
 
         mutex_lock(&data->update_lock);
         data->temp_beep = f71882fg_read8(data, F71882FG_REG_TEMP_BEEP);
@@ -1490,8 +1525,13 @@ static ssize_t store_pwm(struct device *dev,
                          size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int nr = to_sensor_dev_attr_2(devattr)->index;
-        long val = simple_strtol(buf, NULL, 10);
+        int err, nr = to_sensor_dev_attr_2(devattr)->index;
+        long val;
+
+        err = strict_strtol(buf, 10, &val);
+        if (err)
+                return err;
+
         val = SENSORS_LIMIT(val, 0, 255);
 
         mutex_lock(&data->update_lock);
@@ -1551,8 +1591,12 @@ static ssize_t store_pwm_enable(struct device *dev, struct device_attribute
                                 *devattr, const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int nr = to_sensor_dev_attr_2(devattr)->index;
-        long val = simple_strtol(buf, NULL, 10);
+        int err, nr = to_sensor_dev_attr_2(devattr)->index;
+        long val;
+
+        err = strict_strtol(buf, 10, &val);
+        if (err)
+                return err;
 
         /* Special case for F8000 pwm channel 3 which only does auto mode */
         if (data->type == f8000 && nr == 2 && val != 2)
@@ -1626,9 +1670,14 @@ static ssize_t store_pwm_auto_point_pwm(struct device *dev,
                                         const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int pwm = to_sensor_dev_attr_2(devattr)->index;
+        int err, pwm = to_sensor_dev_attr_2(devattr)->index;
         int point = to_sensor_dev_attr_2(devattr)->nr;
-        long val = simple_strtol(buf, NULL, 10);
+        long val;
+
+        err = strict_strtol(buf, 10, &val);
+        if (err)
+                return err;
+
         val = SENSORS_LIMIT(val, 0, 255);
 
         mutex_lock(&data->update_lock);
@@ -1674,10 +1723,16 @@ static ssize_t store_pwm_auto_point_temp_hyst(struct device *dev,
                                               const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int nr = to_sensor_dev_attr_2(devattr)->index;
+        int err, nr = to_sensor_dev_attr_2(devattr)->index;
         int point = to_sensor_dev_attr_2(devattr)->nr;
-        long val = simple_strtol(buf, NULL, 10) / 1000;
         u8 reg;
+        long val;
+
+        err = strict_strtol(buf, 10, &val);
+        if (err)
+                return err;
+
+        val /= 1000;
 
         mutex_lock(&data->update_lock);
         data->pwm_auto_point_temp[nr][point] =
@@ -1716,8 +1771,12 @@ static ssize_t store_pwm_interpolate(struct device *dev,
                                      const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int nr = to_sensor_dev_attr_2(devattr)->index;
-        unsigned long val = simple_strtoul(buf, NULL, 10);
+        int err, nr = to_sensor_dev_attr_2(devattr)->index;
+        unsigned long val;
+
+        err = strict_strtoul(buf, 10, &val);
+        if (err)
+                return err;
 
         mutex_lock(&data->update_lock);
         data->pwm_auto_point_mapping[nr] =
@@ -1752,8 +1811,12 @@ static ssize_t store_pwm_auto_point_channel(struct device *dev,
                                             const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int nr = to_sensor_dev_attr_2(devattr)->index;
-        long val = simple_strtol(buf, NULL, 10);
+        int err, nr = to_sensor_dev_attr_2(devattr)->index;
+        long val;
+
+        err = strict_strtol(buf, 10, &val);
+        if (err)
+                return err;
 
         switch (val) {
         case 1:
@@ -1798,9 +1861,15 @@ static ssize_t store_pwm_auto_point_temp(struct device *dev,
                                          const char *buf, size_t count)
 {
         struct f71882fg_data *data = dev_get_drvdata(dev);
-        int pwm = to_sensor_dev_attr_2(devattr)->index;
+        int err, pwm = to_sensor_dev_attr_2(devattr)->index;
         int point = to_sensor_dev_attr_2(devattr)->nr;
-        long val = simple_strtol(buf, NULL, 10) / 1000;
+        long val;
+
+        err = strict_strtol(buf, 10, &val);
+        if (err)
+                return err;
+
+        val /= 1000;
 
         if (data->type == f71889fg)
                 val = SENSORS_LIMIT(val, -128, 127);
@@ -2109,6 +2178,13 @@ static int __init f71882fg_find(int sioaddr, unsigned short *address,
         int err = -ENODEV;
         u16 devid;
 
+        /* Don't step on other drivers' I/O space by accident */
+        if (!request_region(sioaddr, 2, DRVNAME)) {
+                printk(KERN_ERR DRVNAME ": I/O address 0x%04x already in use\n",
+                                (int)sioaddr);
+                return -EBUSY;
+        }
+
         superio_enter(sioaddr);
 
         devid = superio_inw(sioaddr, SIO_REG_MANID);
@@ -2151,8 +2227,7 @@ static int __init f71882fg_find(int sioaddr, unsigned short *address,
         }
 
         *address = superio_inw(sioaddr, SIO_REG_ADDR);
-        if (*address == 0)
-        {
+        if (*address == 0) {
                 printk(KERN_WARNING DRVNAME ": Base address not set\n");
                 goto exit;
         }
@@ -2164,6 +2239,7 @@ static int __init f71882fg_find(int sioaddr, unsigned short *address,
                 (int)superio_inb(sioaddr, SIO_REG_DEVREV));
 exit:
         superio_exit(sioaddr);
+        release_region(sioaddr, 2);
         return err;
 }
 
diff --git a/drivers/hwmon/f75375s.c b/drivers/hwmon/f75375s.c
index bad2cf3ef4a4..0f58ecc5334d 100644
--- a/drivers/hwmon/f75375s.c
+++ b/drivers/hwmon/f75375s.c
@@ -662,7 +662,6 @@ exit_remove:
         sysfs_remove_group(&client->dev.kobj, &f75375_group);
 exit_free:
         kfree(data);
-        i2c_set_clientdata(client, NULL);
         return err;
 }
 
@@ -672,7 +671,6 @@ static int f75375_remove(struct i2c_client *client)
         hwmon_device_unregister(data->hwmon_dev);
         sysfs_remove_group(&client->dev.kobj, &f75375_group);
         kfree(data);
-        i2c_set_clientdata(client, NULL);
         return 0;
 }
 
diff --git a/drivers/hwmon/g760a.c b/drivers/hwmon/g760a.c
index 09ea12e0a551..1f63d1a3af5e 100644
--- a/drivers/hwmon/g760a.c
+++ b/drivers/hwmon/g760a.c
@@ -236,7 +236,6 @@ error_hwmon_device_register:
         sysfs_remove_group(&client->dev.kobj, &g760a_group);
 error_sysfs_create_group:
         kfree(data);
-        i2c_set_clientdata(client, NULL);
 
         return err;
 }
@@ -247,7 +246,6 @@ static int g760a_remove(struct i2c_client *client)
         hwmon_device_unregister(data->hwmon_dev);
         sysfs_remove_group(&client->dev.kobj, &g760a_group);
         kfree(data);
-        i2c_set_clientdata(client, NULL);
 
         return 0;
 }
diff --git a/drivers/hwmon/lm63.c b/drivers/hwmon/lm63.c
index bf81aff7051d..776aeb3019d2 100644
--- a/drivers/hwmon/lm63.c
+++ b/drivers/hwmon/lm63.c
@@ -53,7 +53,7 @@
  * Address is fully defined internally and cannot be changed.
  */
 
-static const unsigned short normal_i2c[] = { 0x4c, I2C_CLIENT_END };
+static const unsigned short normal_i2c[] = { 0x18, 0x4c, 0x4e, I2C_CLIENT_END };
 
 /*
  * The LM63 registers
@@ -131,12 +131,15 @@ static struct lm63_data *lm63_update_device(struct device *dev);
 static int lm63_detect(struct i2c_client *client, struct i2c_board_info *info);
 static void lm63_init_client(struct i2c_client *client);
 
+enum chips { lm63, lm64 };
+
 /*
  * Driver data (common to all clients)
  */
 
 static const struct i2c_device_id lm63_id[] = {
-        { "lm63", 0 },
+        { "lm63", lm63 },
+        { "lm64", lm64 },
         { }
 };
 MODULE_DEVICE_TABLE(i2c, lm63_id);
@@ -422,6 +425,7 @@ static int lm63_detect(struct i2c_client *new_client,
         struct i2c_adapter *adapter = new_client->adapter;
         u8 man_id, chip_id, reg_config1, reg_config2;
         u8 reg_alert_status, reg_alert_mask;
+        int address = new_client->addr;
 
         if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
                 return -ENODEV;
@@ -439,7 +443,6 @@ static int lm63_detect(struct i2c_client *new_client,
                          LM63_REG_ALERT_MASK);
 
         if (man_id != 0x01 /* National Semiconductor */
-         || chip_id != 0x41 /* LM63 */
          || (reg_config1 & 0x18) != 0x00
          || (reg_config2 & 0xF8) != 0x00
          || (reg_alert_status & 0x20) != 0x00
@@ -450,7 +453,12 @@ static int lm63_detect(struct i2c_client *new_client,
                 return -ENODEV;
         }
 
-        strlcpy(info->type, "lm63", I2C_NAME_SIZE);
+        if (chip_id == 0x41 && address == 0x4c)
+                strlcpy(info->type, "lm63", I2C_NAME_SIZE);
+        else if (chip_id == 0x51 && (address == 0x18 || address == 0x4e))
+                strlcpy(info->type, "lm64", I2C_NAME_SIZE);
+        else
+                return -ENODEV;
 
         return 0;
 }
diff --git a/drivers/hwmon/lm73.c b/drivers/hwmon/lm73.c
index 4d1b76bc8148..29b9030d42c3 100644
--- a/drivers/hwmon/lm73.c
+++ b/drivers/hwmon/lm73.c
@@ -136,7 +136,6 @@ static int lm73_remove(struct i2c_client *client)
 
         hwmon_device_unregister(hwmon_dev);
         sysfs_remove_group(&client->dev.kobj, &lm73_group);
-        i2c_set_clientdata(client, NULL);
         return 0;
 }
 
diff --git a/drivers/hwmon/lm75.c b/drivers/hwmon/lm75.c
index 8ae2cfe2d827..393f354f92a4 100644
--- a/drivers/hwmon/lm75.c
+++ b/drivers/hwmon/lm75.c
@@ -46,6 +46,7 @@ enum lm75_type {		/* keep sorted in alphabetical order */
         tcn75,
         tmp100,
         tmp101,
+        tmp105,
         tmp175,
         tmp275,
         tmp75,
@@ -191,7 +192,6 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 exit_remove:
         sysfs_remove_group(&client->dev.kobj, &lm75_group);
 exit_free:
-        i2c_set_clientdata(client, NULL);
         kfree(data);
         return status;
 }
@@ -203,7 +203,6 @@ static int lm75_remove(struct i2c_client *client)
         hwmon_device_unregister(data->hwmon_dev);
         sysfs_remove_group(&client->dev.kobj, &lm75_group);
         lm75_write_value(client, LM75_REG_CONF, data->orig_conf);
-        i2c_set_clientdata(client, NULL);
         kfree(data);
         return 0;
 }
@@ -220,6 +219,7 @@ static const struct i2c_device_id lm75_ids[] = {
         { "tcn75", tcn75, },
         { "tmp100", tmp100, },
         { "tmp101", tmp101, },
+        { "tmp105", tmp105, },
         { "tmp175", tmp175, },
         { "tmp275", tmp275, },
         { "tmp75", tmp75, },
diff --git a/drivers/hwmon/lm90.c b/drivers/hwmon/lm90.c
index 7cc2708871ab..760ef72eea56 100644
--- a/drivers/hwmon/lm90.c
+++ b/drivers/hwmon/lm90.c
@@ -982,7 +982,8 @@ static struct lm90_data *lm90_update_device(struct device *dev)
 
         mutex_lock(&data->update_lock);
 
-        if (time_after(jiffies, data->last_updated + HZ * 2) || !data->valid) {
+        if (time_after(jiffies, data->last_updated + HZ / 2 + HZ / 10)
+         || !data->valid) {
                 u8 h, l;
 
                 dev_dbg(&client->dev, "Updating lm90 data.\n");
diff --git a/drivers/hwmon/lm95241.c b/drivers/hwmon/lm95241.c
index 8fc8eb8cba47..94741d42112d 100644
--- a/drivers/hwmon/lm95241.c
+++ b/drivers/hwmon/lm95241.c
@@ -399,7 +399,6 @@ static int lm95241_remove(struct i2c_client *client)
         hwmon_device_unregister(data->hwmon_dev);
         sysfs_remove_group(&client->dev.kobj, &lm95241_group);
 
-        i2c_set_clientdata(client, NULL);
         kfree(data);
         return 0;
 }
diff --git a/drivers/hwmon/ltc4245.c b/drivers/hwmon/ltc4245.c
index 65c232a9d0c5..21d201befc2c 100644
--- a/drivers/hwmon/ltc4245.c
+++ b/drivers/hwmon/ltc4245.c
@@ -45,9 +45,7 @@ enum ltc4245_cmd {
         LTC4245_VEEIN                   = 0x19,
         LTC4245_VEESENSE                = 0x1a,
         LTC4245_VEEOUT                  = 0x1b,
-        LTC4245_GPIOADC1                = 0x1c,
-        LTC4245_GPIOADC2                = 0x1d,
-        LTC4245_GPIOADC3                = 0x1e,
+        LTC4245_GPIOADC                 = 0x1c,
 };
 
 struct ltc4245_data {
@@ -61,7 +59,7 @@ struct ltc4245_data {
         u8 cregs[0x08];
 
         /* Voltage registers */
-        u8 vregs[0x0f];
+        u8 vregs[0x0d];
 };
 
 static struct ltc4245_data *ltc4245_update_device(struct device *dev)
@@ -86,7 +84,7 @@ static struct ltc4245_data *ltc4245_update_device(struct device *dev)
                                 data->cregs[i] = val;
                 }
 
-                /* Read voltage registers -- 0x10 to 0x1f */
+                /* Read voltage registers -- 0x10 to 0x1c */
                 for (i = 0; i < ARRAY_SIZE(data->vregs); i++) {
                         val = i2c_smbus_read_byte_data(client, i+0x10);
                         if (unlikely(val < 0))
@@ -128,9 +126,7 @@ static int ltc4245_get_voltage(struct device *dev, u8 reg)
         case LTC4245_VEEOUT:
                 voltage = regval * -55;
                 break;
-        case LTC4245_GPIOADC1:
-        case LTC4245_GPIOADC2:
-        case LTC4245_GPIOADC3:
+        case LTC4245_GPIOADC:
                 voltage = regval * 10;
                 break;
         default:
@@ -297,9 +293,7 @@ LTC4245_ALARM(in7_min_alarm,	(1 << 2),	LTC4245_FAULT2);
 LTC4245_ALARM(in8_min_alarm,    (1 << 3),       LTC4245_FAULT2);
 
 /* GPIO voltages */
-LTC4245_VOLTAGE(in9_input,                      LTC4245_GPIOADC1);
-LTC4245_VOLTAGE(in10_input,                     LTC4245_GPIOADC2);
-LTC4245_VOLTAGE(in11_input,                     LTC4245_GPIOADC3);
+LTC4245_VOLTAGE(in9_input,                      LTC4245_GPIOADC);
 
 /* Power Consumption (virtual) */
 LTC4245_POWER(power1_input,                     LTC4245_12VSENSE);
@@ -342,8 +336,6 @@ static struct attribute *ltc4245_attributes[] = {
         &sensor_dev_attr_in8_min_alarm.dev_attr.attr,
 
         &sensor_dev_attr_in9_input.dev_attr.attr,
-        &sensor_dev_attr_in10_input.dev_attr.attr,
-        &sensor_dev_attr_in11_input.dev_attr.attr,
 
         &sensor_dev_attr_power1_input.dev_attr.attr,
         &sensor_dev_attr_power2_input.dev_attr.attr,
diff --git a/drivers/hwmon/tmp102.c b/drivers/hwmon/tmp102.c
new file mode 100644
index 000000000000..93187c3cb5e7
--- /dev/null
+++ b/drivers/hwmon/tmp102.c
@@ -0,0 +1,319 @@
+/* Texas Instruments TMP102 SMBus temperature sensor driver
+ *
+ * Copyright (C) 2010 Steven King <sfking@fdwdc.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/err.h>
+#include <linux/mutex.h>
+#include <linux/device.h>
+
+#define DRIVER_NAME "tmp102"
+
+#define TMP102_TEMP_REG                 0x00
+#define TMP102_CONF_REG                 0x01
+/* note: these bit definitions are byte swapped */
+#define         TMP102_CONF_SD          0x0100
+#define         TMP102_CONF_TM          0x0200
+#define         TMP102_CONF_POL         0x0400
+#define         TMP102_CONF_F0          0x0800
+#define         TMP102_CONF_F1          0x1000
+#define         TMP102_CONF_R0          0x2000
+#define         TMP102_CONF_R1          0x4000
+#define         TMP102_CONF_OS          0x8000
+#define         TMP102_CONF_EM          0x0010
+#define         TMP102_CONF_AL          0x0020
+#define         TMP102_CONF_CR0         0x0040
+#define         TMP102_CONF_CR1         0x0080
+#define TMP102_TLOW_REG                 0x02
+#define TMP102_THIGH_REG                0x03
+
+struct tmp102 {
+        struct device *hwmon_dev;
+        struct mutex lock;
+        u16 config_orig;
+        unsigned long last_update;
+        int temp[3];
+};
+
+/* SMBus specifies low byte first, but the TMP102 returns high byte first,
+ * so we have to swab16 the values */
+static inline int tmp102_read_reg(struct i2c_client *client, u8 reg)
+{
+        int result = i2c_smbus_read_word_data(client, reg);
+        return result < 0 ? result : swab16(result);
+}
+
+static inline int tmp102_write_reg(struct i2c_client *client, u8 reg, u16 val)
+{
+        return i2c_smbus_write_word_data(client, reg, swab16(val));
+}
+
+/* convert left adjusted 13-bit TMP102 register value to milliCelsius */
+static inline int tmp102_reg_to_mC(s16 val)
+{
+        return ((val & ~0x01) * 1000) / 128;
+}
+
+/* convert milliCelsius to left adjusted 13-bit TMP102 register value */
+static inline u16 tmp102_mC_to_reg(int val)
+{
+        return (val * 128) / 1000;
+}
+
+static const u8 tmp102_reg[] = {
+        TMP102_TEMP_REG,
+        TMP102_TLOW_REG,
+        TMP102_THIGH_REG,
+};
+
+static struct tmp102 *tmp102_update_device(struct i2c_client *client)
+{
+        struct tmp102 *tmp102 = i2c_get_clientdata(client);
+
+        mutex_lock(&tmp102->lock);
+        if (time_after(jiffies, tmp102->last_update + HZ / 3)) {
+                int i;
+                for (i = 0; i < ARRAY_SIZE(tmp102->temp); ++i) {
+                        int status = tmp102_read_reg(client, tmp102_reg[i]);
+                        if (status > -1)
+                                tmp102->temp[i] = tmp102_reg_to_mC(status);
+                }
+                tmp102->last_update = jiffies;
+        }
+        mutex_unlock(&tmp102->lock);
+        return tmp102;
+}
+
+static ssize_t tmp102_show_temp(struct device *dev,
+                                struct device_attribute *attr,
+                                char *buf)
+{
+        struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
+        struct tmp102 *tmp102 = tmp102_update_device(to_i2c_client(dev));
+
+        return sprintf(buf, "%d\n", tmp102->temp[sda->index]);
+}
+
+static ssize_t tmp102_set_temp(struct device *dev,
+                               struct device_attribute *attr,
+                               const char *buf, size_t count)
+{
+        struct sensor_device_attribute *sda = to_sensor_dev_attr(attr);
+        struct i2c_client *client = to_i2c_client(dev);
+        struct tmp102 *tmp102 = i2c_get_clientdata(client);
+        long val;
+        int status;
+
+        if (strict_strtol(buf, 10, &val) < 0)
+                return -EINVAL;
+        val = SENSORS_LIMIT(val, -256000, 255000);
+
+        mutex_lock(&tmp102->lock);
+        tmp102->temp[sda->index] = val;
+        status = tmp102_write_reg(client, tmp102_reg[sda->index],
+                                  tmp102_mC_to_reg(val));
+        mutex_unlock(&tmp102->lock);
+        return status ? : count;
+}
+
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, tmp102_show_temp, NULL , 0);
+
+static SENSOR_DEVICE_ATTR(temp1_max_hyst, S_IWUSR | S_IRUGO, tmp102_show_temp,
+                          tmp102_set_temp, 1);
+
+static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, tmp102_show_temp,
+                          tmp102_set_temp, 2);
+
+static struct attribute *tmp102_attributes[] = {
+        &sensor_dev_attr_temp1_input.dev_attr.attr,
+        &sensor_dev_attr_temp1_max_hyst.dev_attr.attr,
+        &sensor_dev_attr_temp1_max.dev_attr.attr,
+        NULL
+};
+
+static const struct attribute_group tmp102_attr_group = {
+        .attrs = tmp102_attributes,
+};
+
+#define TMP102_CONFIG  (TMP102_CONF_TM | TMP102_CONF_EM | TMP102_CONF_CR1)
+#define TMP102_CONFIG_RD_ONLY (TMP102_CONF_R0 | TMP102_CONF_R1 | TMP102_CONF_AL)
+
+static int __devinit tmp102_probe(struct i2c_client *client,
+                                  const struct i2c_device_id *id)
+{
+        struct tmp102 *tmp102;
+        int status;
+
+        if (!i2c_check_functionality(client->adapter,
+                                     I2C_FUNC_SMBUS_WORD_DATA)) {
+                dev_err(&client->dev, "adapter doesnt support SMBus word "
+                        "transactions\n");
+                return -ENODEV;
+        }
+
+        tmp102 = kzalloc(sizeof(*tmp102), GFP_KERNEL);
+        if (!tmp102) {
+                dev_dbg(&client->dev, "kzalloc failed\n");
+                return -ENOMEM;
+        }
+        i2c_set_clientdata(client, tmp102);
+
+        status = tmp102_read_reg(client, TMP102_CONF_REG);
+        if (status < 0) {
+                dev_err(&client->dev, "error reading config register\n");
+                goto fail_free;
+        }
+        tmp102->config_orig = status;
+        status = tmp102_write_reg(client, TMP102_CONF_REG, TMP102_CONFIG);
+        if (status < 0) {
+                dev_err(&client->dev, "error writing config register\n");
+                goto fail_restore_config;
+        }
+        status = tmp102_read_reg(client, TMP102_CONF_REG);
+        if (status < 0) {
+                dev_err(&client->dev, "error reading config register\n");
+                goto fail_restore_config;
+        }
+        status &= ~TMP102_CONFIG_RD_ONLY;
+        if (status != TMP102_CONFIG) {
+                dev_err(&client->dev, "config settings did not stick\n");
+                status = -ENODEV;
+                goto fail_restore_config;
+        }
+        tmp102->last_update = jiffies - HZ;
+        mutex_init(&tmp102->lock);
+
+        status = sysfs_create_group(&client->dev.kobj, &tmp102_attr_group);
+        if (status) {
+                dev_dbg(&client->dev, "could not create sysfs files\n");
+                goto fail_restore_config;
+        }
+        tmp102->hwmon_dev = hwmon_device_register(&client->dev);
+        if (IS_ERR(tmp102->hwmon_dev)) {
+                dev_dbg(&client->dev, "unable to register hwmon device\n");
+                status = PTR_ERR(tmp102->hwmon_dev);
+                goto fail_remove_sysfs;
+        }
+
+        dev_info(&client->dev, "initialized\n");
+
+        return 0;
+
+fail_remove_sysfs:
+        sysfs_remove_group(&client->dev.kobj, &tmp102_attr_group);
+fail_restore_config:
+        tmp102_write_reg(client, TMP102_CONF_REG, tmp102->config_orig);
+fail_free:
+        kfree(tmp102);
+
+        return status;
+}
+
+static int __devexit tmp102_remove(struct i2c_client *client)
+{
+        struct tmp102 *tmp102 = i2c_get_clientdata(client);
+
+        hwmon_device_unregister(tmp102->hwmon_dev);
+        sysfs_remove_group(&client->dev.kobj, &tmp102_attr_group);
+
+        /* Stop monitoring if device was stopped originally */
+        if (tmp102->config_orig & TMP102_CONF_SD) {
+                int config;
+
+                config = tmp102_read_reg(client, TMP102_CONF_REG);
+                if (config >= 0)
+                        tmp102_write_reg(client, TMP102_CONF_REG,
+                                         config | TMP102_CONF_SD);
+        }
+
+        kfree(tmp102);
+
+        return 0;
+}
+
+#ifdef CONFIG_PM
+static int tmp102_suspend(struct device *dev)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        int config;
+
+        config = tmp102_read_reg(client, TMP102_CONF_REG);
+        if (config < 0)
+                return config;
+
+        config |= TMP102_CONF_SD;
+        return tmp102_write_reg(client, TMP102_CONF_REG, config);
+}
+
+static int tmp102_resume(struct device *dev)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        int config;
+
+        config = tmp102_read_reg(client, TMP102_CONF_REG);
+        if (config < 0)
+                return config;
+
+        config &= ~TMP102_CONF_SD;
+        return tmp102_write_reg(client, TMP102_CONF_REG, config);
+}
+
+static const struct dev_pm_ops tmp102_dev_pm_ops = {
+        .suspend        = tmp102_suspend,
+        .resume         = tmp102_resume,
+};
+
+#define TMP102_DEV_PM_OPS (&tmp102_dev_pm_ops)
+#else
+#define TMP102_DEV_PM_OPS NULL
+#endif /* CONFIG_PM */
+
+static const struct i2c_device_id tmp102_id[] = {
+        { "tmp102", 0 },
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, tmp102_id);
+
+static struct i2c_driver tmp102_driver = {
+        .driver.name    = DRIVER_NAME,
+        .driver.pm      = TMP102_DEV_PM_OPS,
+        .probe          = tmp102_probe,
+        .remove         = __devexit_p(tmp102_remove),
+        .id_table       = tmp102_id,
+};
+
+static int __init tmp102_init(void)
+{
+        return i2c_add_driver(&tmp102_driver);
+}
+module_init(tmp102_init);
+
+static void __exit tmp102_exit(void)
+{
+        i2c_del_driver(&tmp102_driver);
+}
+module_exit(tmp102_exit);
+
+MODULE_AUTHOR("Steven King <sfking@fdwdc.com>");
+MODULE_DESCRIPTION("Texas Instruments TMP102 temperature sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/tmp401.c b/drivers/hwmon/tmp401.c
index d14a1af9f550..ad8d535235c5 100644
--- a/drivers/hwmon/tmp401.c
+++ b/drivers/hwmon/tmp401.c
@@ -92,17 +92,6 @@ static const u8 TMP411_TEMP_HIGHEST_LSB[2]		= { 0x33, 0x37 };
 #define TMP411_DEVICE_ID                        0x12
 
 /*
- * Functions declarations
- */
-
-static int tmp401_probe(struct i2c_client *client,
-                        const struct i2c_device_id *id);
-static int tmp401_detect(struct i2c_client *client,
-                         struct i2c_board_info *info);
-static int tmp401_remove(struct i2c_client *client);
-static struct tmp401_data *tmp401_update_device(struct device *dev);
-
-/*
  * Driver data (common to all clients)
  */
 
@@ -113,18 +102,6 @@ static const struct i2c_device_id tmp401_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, tmp401_id);
 
-static struct i2c_driver tmp401_driver = {
-        .class          = I2C_CLASS_HWMON,
-        .driver = {
-                .name   = "tmp401",
-        },
-        .probe          = tmp401_probe,
-        .remove         = tmp401_remove,
-        .id_table       = tmp401_id,
-        .detect         = tmp401_detect,
-        .address_list   = normal_i2c,
-};
-
 /*
  * Client data (each client gets its own)
  */
@@ -194,6 +171,71 @@ static u8 tmp401_crit_temp_to_register(long temp, u8 config)
         return (temp + 500) / 1000;
 }
 
+static struct tmp401_data *tmp401_update_device_reg16(
+        struct i2c_client *client, struct tmp401_data *data)
+{
+        int i;
+
+        for (i = 0; i < 2; i++) {
+                /*
+                 * High byte must be read first immediately followed
+                 * by the low byte
+                 */
+                data->temp[i] = i2c_smbus_read_byte_data(client,
+                        TMP401_TEMP_MSB[i]) << 8;
+                data->temp[i] |= i2c_smbus_read_byte_data(client,
+                        TMP401_TEMP_LSB[i]);
+                data->temp_low[i] = i2c_smbus_read_byte_data(client,
+                        TMP401_TEMP_LOW_LIMIT_MSB_READ[i]) << 8;
+                data->temp_low[i] |= i2c_smbus_read_byte_data(client,
+                        TMP401_TEMP_LOW_LIMIT_LSB[i]);
+                data->temp_high[i] = i2c_smbus_read_byte_data(client,
+                        TMP401_TEMP_HIGH_LIMIT_MSB_READ[i]) << 8;
+                data->temp_high[i] |= i2c_smbus_read_byte_data(client,
+                        TMP401_TEMP_HIGH_LIMIT_LSB[i]);
+                data->temp_crit[i] = i2c_smbus_read_byte_data(client,
+                        TMP401_TEMP_CRIT_LIMIT[i]);
+
+                if (data->kind == tmp411) {
+                        data->temp_lowest[i] = i2c_smbus_read_byte_data(client,
+                                TMP411_TEMP_LOWEST_MSB[i]) << 8;
+                        data->temp_lowest[i] |= i2c_smbus_read_byte_data(
+                                client, TMP411_TEMP_LOWEST_LSB[i]);
+
+                        data->temp_highest[i] = i2c_smbus_read_byte_data(
+                                client, TMP411_TEMP_HIGHEST_MSB[i]) << 8;
+                        data->temp_highest[i] |= i2c_smbus_read_byte_data(
+                                client, TMP411_TEMP_HIGHEST_LSB[i]);
+                }
+        }
+        return data;
+}
+
+static struct tmp401_data *tmp401_update_device(struct device *dev)
+{
+        struct i2c_client *client = to_i2c_client(dev);
+        struct tmp401_data *data = i2c_get_clientdata(client);
+
+        mutex_lock(&data->update_lock);
+
+        if (time_after(jiffies, data->last_updated + HZ) || !data->valid) {
+                data->status = i2c_smbus_read_byte_data(client, TMP401_STATUS);
+                data->config = i2c_smbus_read_byte_data(client,
+                                                TMP401_CONFIG_READ);
+                tmp401_update_device_reg16(client, data);
+
+                data->temp_crit_hyst = i2c_smbus_read_byte_data(client,
+                                                TMP401_TEMP_CRIT_HYST);
+
+                data->last_updated = jiffies;
+                data->valid = 1;
+        }
+
+        mutex_unlock(&data->update_lock);
+
+        return data;
+}
+
 static ssize_t show_temp_value(struct device *dev,
         struct device_attribute *devattr, char *buf)
 {
@@ -420,30 +462,36 @@ static ssize_t reset_temp_history(struct device *dev,
 }
 
 static struct sensor_device_attribute tmp401_attr[] = {
-        SENSOR_ATTR(temp1_input, 0444, show_temp_value, NULL, 0),
-        SENSOR_ATTR(temp1_min, 0644, show_temp_min, store_temp_min, 0),
-        SENSOR_ATTR(temp1_max, 0644, show_temp_max, store_temp_max, 0),
-        SENSOR_ATTR(temp1_crit, 0644, show_temp_crit, store_temp_crit, 0),
-        SENSOR_ATTR(temp1_crit_hyst, 0644, show_temp_crit_hyst,
+        SENSOR_ATTR(temp1_input, S_IRUGO, show_temp_value, NULL, 0),
+        SENSOR_ATTR(temp1_min, S_IWUSR | S_IRUGO, show_temp_min,
+                    store_temp_min, 0),
+        SENSOR_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp_max,
+                    store_temp_max, 0),
+        SENSOR_ATTR(temp1_crit, S_IWUSR | S_IRUGO, show_temp_crit,
+                    store_temp_crit, 0),
+        SENSOR_ATTR(temp1_crit_hyst, S_IWUSR | S_IRUGO, show_temp_crit_hyst,
                     store_temp_crit_hyst, 0),
-        SENSOR_ATTR(temp1_min_alarm, 0444, show_status, NULL,
+        SENSOR_ATTR(temp1_min_alarm, S_IRUGO, show_status, NULL,
                     TMP401_STATUS_LOCAL_LOW),
-        SENSOR_ATTR(temp1_max_alarm, 0444, show_status, NULL,
+        SENSOR_ATTR(temp1_max_alarm, S_IRUGO, show_status, NULL,
                     TMP401_STATUS_LOCAL_HIGH),
-        SENSOR_ATTR(temp1_crit_alarm, 0444, show_status, NULL,
+        SENSOR_ATTR(temp1_crit_alarm, S_IRUGO, show_status, NULL,
                     TMP401_STATUS_LOCAL_CRIT),
-        SENSOR_ATTR(temp2_input, 0444, show_temp_value, NULL, 1),
-        SENSOR_ATTR(temp2_min, 0644, show_temp_min, store_temp_min, 1),
-        SENSOR_ATTR(temp2_max, 0644, show_temp_max, store_temp_max, 1),
-        SENSOR_ATTR(temp2_crit, 0644, show_temp_crit, store_temp_crit, 1),
-        SENSOR_ATTR(temp2_crit_hyst, 0444, show_temp_crit_hyst, NULL, 1),
-        SENSOR_ATTR(temp2_fault, 0444, show_status, NULL,
+        SENSOR_ATTR(temp2_input, S_IRUGO, show_temp_value, NULL, 1),
+        SENSOR_ATTR(temp2_min, S_IWUSR | S_IRUGO, show_temp_min,
+                    store_temp_min, 1),
+        SENSOR_ATTR(temp2_max, S_IWUSR | S_IRUGO, show_temp_max,
+                    store_temp_max, 1),
+        SENSOR_ATTR(temp2_crit, S_IWUSR | S_IRUGO, show_temp_crit,
+                    store_temp_crit, 1),
+        SENSOR_ATTR(temp2_crit_hyst, S_IRUGO, show_temp_crit_hyst, NULL, 1),
+        SENSOR_ATTR(temp2_fault, S_IRUGO, show_status, NULL,
                     TMP401_STATUS_REMOTE_OPEN),
-        SENSOR_ATTR(temp2_min_alarm, 0444, show_status, NULL,
+        SENSOR_ATTR(temp2_min_alarm, S_IRUGO, show_status, NULL,
                     TMP401_STATUS_REMOTE_LOW),
-        SENSOR_ATTR(temp2_max_alarm, 0444, show_status, NULL,
+        SENSOR_ATTR(temp2_max_alarm, S_IRUGO, show_status, NULL,
                     TMP401_STATUS_REMOTE_HIGH),
-        SENSOR_ATTR(temp2_crit_alarm, 0444, show_status, NULL,
+        SENSOR_ATTR(temp2_crit_alarm, S_IRUGO, show_status, NULL,
                     TMP401_STATUS_REMOTE_CRIT),
 };
 
@@ -455,11 +503,11 @@ static struct sensor_device_attribute tmp401_attr[] = {
  * and remote channels.
  */
 static struct sensor_device_attribute tmp411_attr[] = {
-        SENSOR_ATTR(temp1_highest, 0444, show_temp_highest, NULL, 0),
-        SENSOR_ATTR(temp1_lowest, 0444, show_temp_lowest, NULL, 0),
-        SENSOR_ATTR(temp2_highest, 0444, show_temp_highest, NULL, 1),
-        SENSOR_ATTR(temp2_lowest, 0444, show_temp_lowest, NULL, 1),
-        SENSOR_ATTR(temp_reset_history, 0200, NULL, reset_temp_history, 0),
+        SENSOR_ATTR(temp1_highest, S_IRUGO, show_temp_highest, NULL, 0),
+        SENSOR_ATTR(temp1_lowest, S_IRUGO, show_temp_lowest, NULL, 0),
+        SENSOR_ATTR(temp2_highest, S_IRUGO, show_temp_highest, NULL, 1),
+        SENSOR_ATTR(temp2_lowest, S_IRUGO, show_temp_lowest, NULL, 1),
+        SENSOR_ATTR(temp_reset_history, S_IWUSR, NULL, reset_temp_history, 0),
 };
 
 /*
@@ -529,6 +577,27 @@ static int tmp401_detect(struct i2c_client *client,
         return 0;
 }
 
+static int tmp401_remove(struct i2c_client *client)
+{
+        struct tmp401_data *data = i2c_get_clientdata(client);
+        int i;
+
+        if (data->hwmon_dev)
+                hwmon_device_unregister(data->hwmon_dev);
+
+        for (i = 0; i < ARRAY_SIZE(tmp401_attr); i++)
+                device_remove_file(&client->dev, &tmp401_attr[i].dev_attr);
+
+        if (data->kind == tmp411) {
+                for (i = 0; i < ARRAY_SIZE(tmp411_attr); i++)
+                        device_remove_file(&client->dev,
+                                           &tmp411_attr[i].dev_attr);
+        }
+
+        kfree(data);
+        return 0;
+}
+
 static int tmp401_probe(struct i2c_client *client,
                         const struct i2c_device_id *id)
 {
@@ -581,91 +650,17 @@ exit_remove:
         return err;
 }
 
-static int tmp401_remove(struct i2c_client *client)
-{
-        struct tmp401_data *data = i2c_get_clientdata(client);
-        int i;
-
-        if (data->hwmon_dev)
-                hwmon_device_unregister(data->hwmon_dev);
-
-        for (i = 0; i < ARRAY_SIZE(tmp401_attr); i++)
-                device_remove_file(&client->dev, &tmp401_attr[i].dev_attr);
-
-        if (data->kind == tmp411) {
-                for (i = 0; i < ARRAY_SIZE(tmp411_attr); i++)
-                        device_remove_file(&client->dev,
-                                           &tmp411_attr[i].dev_attr);
-        }
-
-        kfree(data);
-        return 0;
-}
-
-static struct tmp401_data *tmp401_update_device_reg16(
-        struct i2c_client *client, struct tmp401_data *data)
-{
-        int i;
-
-        for (i = 0; i < 2; i++) {
-                /*
-                 * High byte must be read first immediately followed
-                 * by the low byte
-                 */
-                data->temp[i] = i2c_smbus_read_byte_data(client,
-                        TMP401_TEMP_MSB[i]) << 8;
-                data->temp[i] |= i2c_smbus_read_byte_data(client,
-                        TMP401_TEMP_LSB[i]);
-                data->temp_low[i] = i2c_smbus_read_byte_data(client,
-                        TMP401_TEMP_LOW_LIMIT_MSB_READ[i]) << 8;
-                data->temp_low[i] |= i2c_smbus_read_byte_data(client,
-                        TMP401_TEMP_LOW_LIMIT_LSB[i]);
-                data->temp_high[i] = i2c_smbus_read_byte_data(client,
-                        TMP401_TEMP_HIGH_LIMIT_MSB_READ[i]) << 8;
-                data->temp_high[i] |= i2c_smbus_read_byte_data(client,
-                        TMP401_TEMP_HIGH_LIMIT_LSB[i]);
-                data->temp_crit[i] = i2c_smbus_read_byte_data(client,
-                        TMP401_TEMP_CRIT_LIMIT[i]);
-
-                if (data->kind == tmp411) {
-                        data->temp_lowest[i] = i2c_smbus_read_byte_data(client,
-                                TMP411_TEMP_LOWEST_MSB[i]) << 8;
-                        data->temp_lowest[i] |= i2c_smbus_read_byte_data(
-                                client, TMP411_TEMP_LOWEST_LSB[i]);
-
-                        data->temp_highest[i] = i2c_smbus_read_byte_data(
-                                client, TMP411_TEMP_HIGHEST_MSB[i]) << 8;
-                        data->temp_highest[i] |= i2c_smbus_read_byte_data(
-                                client, TMP411_TEMP_HIGHEST_LSB[i]);
-                }
-        }
-        return data;
-}
-
-static struct tmp401_data *tmp401_update_device(struct device *dev)
-{
-        struct i2c_client *client = to_i2c_client(dev);
-        struct tmp401_data *data = i2c_get_clientdata(client);
-
-        mutex_lock(&data->update_lock);
-
-        if (time_after(jiffies, data->last_updated + HZ) || !data->valid) {
-                data->status = i2c_smbus_read_byte_data(client, TMP401_STATUS);
-                data->config = i2c_smbus_read_byte_data(client,
-                                                TMP401_CONFIG_READ);
-                tmp401_update_device_reg16(client, data);
-
-                data->temp_crit_hyst = i2c_smbus_read_byte_data(client,
-                                                TMP401_TEMP_CRIT_HYST);
-
-                data->last_updated = jiffies;
-                data->valid = 1;
-        }
-
-        mutex_unlock(&data->update_lock);
-
-        return data;
-}
+static struct i2c_driver tmp401_driver = {
+        .class          = I2C_CLASS_HWMON,
+        .driver = {
+                .name   = "tmp401",
+        },
+        .probe          = tmp401_probe,
+        .remove         = tmp401_remove,
+        .id_table       = tmp401_id,
+        .detect         = tmp401_detect,
+        .address_list   = normal_i2c,
+};
 
 static int __init tmp401_init(void)
 {
diff --git a/drivers/hwmon/tmp421.c b/drivers/hwmon/tmp421.c
index 738c472ece27..6b4165c12092 100644
--- a/drivers/hwmon/tmp421.c
+++ b/drivers/hwmon/tmp421.c
@@ -295,7 +295,6 @@ exit_remove:
         sysfs_remove_group(&client->dev.kobj, &tmp421_group);
 
 exit_free:
-        i2c_set_clientdata(client, NULL);
         kfree(data);
 
         return err;
@@ -308,7 +307,6 @@ static int tmp421_remove(struct i2c_client *client)
         hwmon_device_unregister(data->hwmon_dev);
         sysfs_remove_group(&client->dev.kobj, &tmp421_group);
 
-        i2c_set_clientdata(client, NULL);
         kfree(data);
 
         return 0;
diff --git a/drivers/hwmon/w83781d.c b/drivers/hwmon/w83781d.c
index 32d4adee73db..c84b9b4e6960 100644
--- a/drivers/hwmon/w83781d.c
+++ b/drivers/hwmon/w83781d.c
@@ -1197,7 +1197,6 @@ ERROR4:
         if (data->lm75[1])
                 i2c_unregister_device(data->lm75[1]);
 ERROR3:
-        i2c_set_clientdata(client, NULL);
         kfree(data);
 ERROR1:
         return err;
@@ -1219,7 +1218,6 @@ w83781d_remove(struct i2c_client *client)
         if (data->lm75[1])
                 i2c_unregister_device(data->lm75[1]);
 
-        i2c_set_clientdata(client, NULL);
         kfree(data);
 
         return 0;
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index 87ab0568bb0e..bceafbfa7268 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -475,6 +475,26 @@ config I2C_PASEMI
         help
           Supports the PA Semi PWRficient on-chip SMBus interfaces.
 
+config I2C_PCA_PLATFORM
+        tristate "PCA9564/PCA9665 as platform device"
+        select I2C_ALGOPCA
+        default n
+        help
+          This driver supports a memory mapped Philips PCA9564/PCA9665
+          parallel bus to I2C bus controller.
+
+          This driver can also be built as a module.  If so, the module
+          will be called i2c-pca-platform.
+
+config I2C_PMCMSP
+        tristate "PMC MSP I2C TWI Controller"
+        depends on PMC_MSP
+        help
+          This driver supports the PMC TWI controller on MSP devices.
+
+          This driver can also be built as module. If so, the module
+          will be called i2c-pmcmsp.
+
 config I2C_PNX
         tristate "I2C bus support for Philips PNX targets"
         depends on ARCH_PNX4008
@@ -711,26 +731,6 @@ config I2C_PCA_ISA
           delays when I2C/SMBus chip drivers are loaded (e.g. at boot
           time).  If unsure, say N.
 
-config I2C_PCA_PLATFORM
-        tristate "PCA9564/PCA9665 as platform device"
-        select I2C_ALGOPCA
-        default n
-        help
-          This driver supports a memory mapped Philips PCA9564/PCA9665
-          parallel bus to I2C bus controller.
-
-          This driver can also be built as a module.  If so, the module
-          will be called i2c-pca-platform.
-
-config I2C_PMCMSP
-        tristate "PMC MSP I2C TWI Controller"
-        depends on PMC_MSP
-        help
-          This driver supports the PMC TWI controller on MSP devices.
-
-          This driver can also be built as module. If so, the module
-          will be called i2c-pmcmsp.
-
 config I2C_SIBYTE
         tristate "SiByte SMBus interface"
         depends on SIBYTE_SB1xxx_SOC
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index 097236f631e8..936880bd1dc5 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -27,7 +27,7 @@ obj-$(CONFIG_I2C_VIAPRO)	+= i2c-viapro.o
 obj-$(CONFIG_I2C_HYDRA)         += i2c-hydra.o
 obj-$(CONFIG_I2C_POWERMAC)      += i2c-powermac.o
 
-# Embebbed system I2C/SMBus host controller drivers
+# Embedded system I2C/SMBus host controller drivers
 obj-$(CONFIG_I2C_AT91)          += i2c-at91.o
 obj-$(CONFIG_I2C_AU1550)        += i2c-au1550.o
 obj-$(CONFIG_I2C_BLACKFIN_TWI)  += i2c-bfin-twi.o
@@ -46,6 +46,8 @@ obj-$(CONFIG_I2C_NOMADIK)	+= i2c-nomadik.o
 obj-$(CONFIG_I2C_OCORES)        += i2c-ocores.o
 obj-$(CONFIG_I2C_OMAP)          += i2c-omap.o
 obj-$(CONFIG_I2C_PASEMI)        += i2c-pasemi.o
+obj-$(CONFIG_I2C_PCA_PLATFORM)  += i2c-pca-platform.o
+obj-$(CONFIG_I2C_PMCMSP)        += i2c-pmcmsp.o
 obj-$(CONFIG_I2C_PNX)           += i2c-pnx.o
 obj-$(CONFIG_I2C_PXA)           += i2c-pxa.o
 obj-$(CONFIG_I2C_S3C2410)       += i2c-s3c2410.o
@@ -68,8 +70,6 @@ obj-$(CONFIG_I2C_TINY_USB)	+= i2c-tiny-usb.o
 obj-$(CONFIG_I2C_ACORN)         += i2c-acorn.o
 obj-$(CONFIG_I2C_ELEKTOR)       += i2c-elektor.o
 obj-$(CONFIG_I2C_PCA_ISA)       += i2c-pca-isa.o
-obj-$(CONFIG_I2C_PCA_PLATFORM)  += i2c-pca-platform.o
-obj-$(CONFIG_I2C_PMCMSP)        += i2c-pmcmsp.o
 obj-$(CONFIG_I2C_SIBYTE)        += i2c-sibyte.o
 obj-$(CONFIG_I2C_STUB)          += i2c-stub.o
 obj-$(CONFIG_SCx200_ACB)        += scx200_acb.o
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index e0f833cca3f1..1cca2631e5b3 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -47,7 +47,6 @@ static DEFINE_MUTEX(core_lock);
 static DEFINE_IDR(i2c_adapter_idr);
 
 static struct device_type i2c_client_type;
-static int i2c_check_addr(struct i2c_adapter *adapter, int addr);
 static int i2c_detect(struct i2c_adapter *adapter, struct i2c_driver *driver);
 
 /* ------------------------------------------------------------------------- */
@@ -371,6 +370,59 @@ struct i2c_client *i2c_verify_client(struct device *dev)
 EXPORT_SYMBOL(i2c_verify_client);
 
 
+/* This is a permissive address validity check, I2C address map constraints
+ * are purposedly not enforced, except for the general call address. */
+static int i2c_check_client_addr_validity(const struct i2c_client *client)
+{
+        if (client->flags & I2C_CLIENT_TEN) {
+                /* 10-bit address, all values are valid */
+                if (client->addr > 0x3ff)
+                        return -EINVAL;
+        } else {
+                /* 7-bit address, reject the general call address */
+                if (client->addr == 0x00 || client->addr > 0x7f)
+                        return -EINVAL;
+        }
+        return 0;
+}
+
+/* And this is a strict address validity check, used when probing. If a
+ * device uses a reserved address, then it shouldn't be probed. 7-bit
+ * addressing is assumed, 10-bit address devices are rare and should be
+ * explicitly enumerated. */
+static int i2c_check_addr_validity(unsigned short addr)
+{
+        /*
+         * Reserved addresses per I2C specification:
+         *  0x00       General call address / START byte
+         *  0x01       CBUS address
+         *  0x02       Reserved for different bus format
+         *  0x03       Reserved for future purposes
+         *  0x04-0x07  Hs-mode master code
+         *  0x78-0x7b  10-bit slave addressing
+         *  0x7c-0x7f  Reserved for future purposes
+         */
+        if (addr < 0x08 || addr > 0x77)
+                return -EINVAL;
+        return 0;
+}
+
+static int __i2c_check_addr_busy(struct device *dev, void *addrp)
+{
+        struct i2c_client       *client = i2c_verify_client(dev);
+        int                     addr = *(int *)addrp;
+
+        if (client && client->addr == addr)
+                return -EBUSY;
+        return 0;
+}
+
+static int i2c_check_addr_busy(struct i2c_adapter *adapter, int addr)
+{
+        return device_for_each_child(&adapter->dev, &addr,
+                                     __i2c_check_addr_busy);
+}
+
 /**
  * i2c_new_device - instantiate an i2c device
  * @adap: the adapter managing the device
@@ -410,8 +462,16 @@ i2c_new_device(struct i2c_adapter *adap, struct i2c_board_info const *info)
 
         strlcpy(client->name, info->type, sizeof(client->name));
 
+        /* Check for address validity */
+        status = i2c_check_client_addr_validity(client);
+        if (status) {
+                dev_err(&adap->dev, "Invalid %d-bit I2C address 0x%02hx\n",
+                        client->flags & I2C_CLIENT_TEN ? 10 : 7, client->addr);
+                goto out_err_silent;
+        }
+
         /* Check for address business */
-        status = i2c_check_addr(adap, client->addr);
+        status = i2c_check_addr_busy(adap, client->addr);
         if (status)
                 goto out_err;
 
@@ -436,6 +496,7 @@ i2c_new_device(struct i2c_adapter *adap, struct i2c_board_info const *info)
 out_err:
         dev_err(&adap->dev, "Failed to register i2c client %s at 0x%02x "
                 "(%d)\n", client->name, client->addr, status);
+out_err_silent:
         kfree(client);
         return NULL;
 }
@@ -561,15 +622,9 @@ i2c_sysfs_new_device(struct device *dev, struct device_attribute *attr,
                 return -EINVAL;
         }
 
-        if (info.addr < 0x03 || info.addr > 0x77) {
-                dev_err(dev, "%s: Invalid I2C address 0x%hx\n", "new_device",
-                        info.addr);
-                return -EINVAL;
-        }
-
         client = i2c_new_device(adap, &info);
         if (!client)
-                return -EEXIST;
+                return -EINVAL;
 
         /* Keep track of the added device */
         i2c_lock_adapter(adap);
@@ -1024,21 +1079,6 @@ EXPORT_SYMBOL(i2c_del_driver);
 
 /* ------------------------------------------------------------------------- */
 
-static int __i2c_check_addr(struct device *dev, void *addrp)
-{
-        struct i2c_client       *client = i2c_verify_client(dev);
-        int                     addr = *(int *)addrp;
-
-        if (client && client->addr == addr)
-                return -EBUSY;
-        return 0;
-}
-
-static int i2c_check_addr(struct i2c_adapter *adapter, int addr)
-{
-        return device_for_each_child(&adapter->dev, &addr, __i2c_check_addr);
-}
-
 /**
  * i2c_use_client - increments the reference count of the i2c client structure
  * @client: the client being referenced
@@ -1277,6 +1317,41 @@ EXPORT_SYMBOL(i2c_master_recv);
  * ----------------------------------------------------
  */
 
+/*
+ * Legacy default probe function, mostly relevant for SMBus. The default
+ * probe method is a quick write, but it is known to corrupt the 24RF08
+ * EEPROMs due to a state machine bug, and could also irreversibly
+ * write-protect some EEPROMs, so for address ranges 0x30-0x37 and 0x50-0x5f,
+ * we use a short byte read instead. Also, some bus drivers don't implement
+ * quick write, so we fallback to a byte read in that case too.
+ * On x86, there is another special case for FSC hardware monitoring chips,
+ * which want regular byte reads (address 0x73.) Fortunately, these are the
+ * only known chips using this I2C address on PC hardware.
+ * Returns 1 if probe succeeded, 0 if not.
+ */
+static int i2c_default_probe(struct i2c_adapter *adap, unsigned short addr)
+{
+        int err;
+        union i2c_smbus_data dummy;
+
+#ifdef CONFIG_X86
+        if (addr == 0x73 && (adap->class & I2C_CLASS_HWMON)
+         && i2c_check_functionality(adap, I2C_FUNC_SMBUS_READ_BYTE_DATA))
+                err = i2c_smbus_xfer(adap, addr, 0, I2C_SMBUS_READ, 0,
+                                     I2C_SMBUS_BYTE_DATA, &dummy);
+        else
+#endif
+        if ((addr & ~0x07) == 0x30 || (addr & ~0x0f) == 0x50
+         || !i2c_check_functionality(adap, I2C_FUNC_SMBUS_QUICK))
+                err = i2c_smbus_xfer(adap, addr, 0, I2C_SMBUS_READ, 0,
+                                     I2C_SMBUS_BYTE, &dummy);
+        else
+                err = i2c_smbus_xfer(adap, addr, 0, I2C_SMBUS_WRITE, 0,
+                                     I2C_SMBUS_QUICK, NULL);
+
+        return err >= 0;
+}
+
 static int i2c_detect_address(struct i2c_client *temp_client,
                               struct i2c_driver *driver)
 {
@@ -1286,34 +1361,20 @@ static int i2c_detect_address(struct i2c_client *temp_client,
         int err;
 
         /* Make sure the address is valid */
-        if (addr < 0x03 || addr > 0x77) {
+        err = i2c_check_addr_validity(addr);
+        if (err) {
                 dev_warn(&adapter->dev, "Invalid probe address 0x%02x\n",
                          addr);
-                return -EINVAL;
+                return err;
         }
 
         /* Skip if already in use */
-        if (i2c_check_addr(adapter, addr))
+        if (i2c_check_addr_busy(adapter, addr))
                 return 0;
 
         /* Make sure there is something at this address */
-        if (addr == 0x73 && (adapter->class & I2C_CLASS_HWMON)) {
-                /* Special probe for FSC hwmon chips */
-                union i2c_smbus_data dummy;
-
-                if (i2c_smbus_xfer(adapter, addr, 0, I2C_SMBUS_READ, 0,
-                                   I2C_SMBUS_BYTE_DATA, &dummy) < 0)
-                        return 0;
-        } else {
-                if (i2c_smbus_xfer(adapter, addr, 0, I2C_SMBUS_WRITE, 0,
-                                   I2C_SMBUS_QUICK, NULL) < 0)
-                        return 0;
-
-                /* Prevent 24RF08 corruption */
-                if ((addr & ~0x0f) == 0x50)
-                        i2c_smbus_xfer(adapter, addr, 0, I2C_SMBUS_WRITE, 0,
-                                       I2C_SMBUS_QUICK, NULL);
-        }
+        if (!i2c_default_probe(adapter, addr))
+                return 0;
 
         /* Finally call the custom detection function */
         memset(&info, 0, sizeof(struct i2c_board_info));
@@ -1407,42 +1468,22 @@ i2c_new_probed_device(struct i2c_adapter *adap,
 
         for (i = 0; addr_list[i] != I2C_CLIENT_END; i++) {
                 /* Check address validity */
-                if (addr_list[i] < 0x03 || addr_list[i] > 0x77) {
+                if (i2c_check_addr_validity(addr_list[i]) < 0) {
                         dev_warn(&adap->dev, "Invalid 7-bit address "
                                  "0x%02x\n", addr_list[i]);
                         continue;
                 }
 
                 /* Check address availability */
-                if (i2c_check_addr(adap, addr_list[i])) {
+                if (i2c_check_addr_busy(adap, addr_list[i])) {
                         dev_dbg(&adap->dev, "Address 0x%02x already in "
                                 "use, not probing\n", addr_list[i]);
                         continue;
                 }
 
-                /* Test address responsiveness
-                   The default probe method is a quick write, but it is known
-                   to corrupt the 24RF08 EEPROMs due to a state machine bug,
-                   and could also irreversibly write-protect some EEPROMs, so
-                   for address ranges 0x30-0x37 and 0x50-0x5f, we use a byte
-                   read instead. Also, some bus drivers don't implement
-                   quick write, so we fallback to a byte read it that case
-                   too. */
-                if ((addr_list[i] & ~0x07) == 0x30
-                 || (addr_list[i] & ~0x0f) == 0x50
-                 || !i2c_check_functionality(adap, I2C_FUNC_SMBUS_QUICK)) {
-                        union i2c_smbus_data data;
-
-                        if (i2c_smbus_xfer(adap, addr_list[i], 0,
-                                           I2C_SMBUS_READ, 0,
-                                           I2C_SMBUS_BYTE, &data) >= 0)
-                                break;
-                } else {
-                        if (i2c_smbus_xfer(adap, addr_list[i], 0,
-                                           I2C_SMBUS_WRITE, 0,
-                                           I2C_SMBUS_QUICK, NULL) >= 0)
-                                break;
-                }
+                /* Test address responsiveness */
+                if (i2c_default_probe(adap, addr_list[i]))
+                        break;
         }
 
         if (addr_list[i] == I2C_CLIENT_END) {
diff --git a/drivers/i2c/i2c-smbus.c b/drivers/i2c/i2c-smbus.c
index a24e0bfe9201..f61ccc1e5ea3 100644
--- a/drivers/i2c/i2c-smbus.c
+++ b/drivers/i2c/i2c-smbus.c
@@ -173,7 +173,6 @@ static int smbalert_remove(struct i2c_client *ara)
 
         cancel_work_sync(&alert->alert);
 
-        i2c_set_clientdata(ara, NULL);
         kfree(alert);
         return 0;
 }
diff --git a/drivers/ide/gayle.c b/drivers/ide/gayle.c
index b9e517de6a82..3feaa26410be 100644
--- a/drivers/ide/gayle.c
+++ b/drivers/ide/gayle.c
@@ -16,6 +16,7 @@
 #include <linux/init.h>
 #include <linux/zorro.h>
 #include <linux/module.h>
+#include <linux/platform_device.h>
 
 #include <asm/setup.h>
 #include <asm/amigahw.h>
@@ -24,15 +25,6 @@
 
 
     /*
-     *  Bases of the IDE interfaces
-     */
-
-#define GAYLE_BASE_4000 0xdd2020        /* A4000/A4000T */
-#define GAYLE_BASE_1200 0xda0000        /* A1200/A600 and E-Matrix 530 */
-
-#define GAYLE_IDEREG_SIZE       0x2000
-
-    /*
      *  Offsets from one of the above bases
      */
 
@@ -68,20 +60,20 @@ MODULE_PARM_DESC(doubler, "enable support for IDE doublers");
 
 static int gayle_test_irq(ide_hwif_t *hwif)
 {
-    unsigned char ch;
+        unsigned char ch;
 
-    ch = z_readb(hwif->io_ports.irq_addr);
-    if (!(ch & GAYLE_IRQ_IDE))
-        return 0;
-    return 1;
+        ch = z_readb(hwif->io_ports.irq_addr);
+        if (!(ch & GAYLE_IRQ_IDE))
+                return 0;
+        return 1;
 }
 
 static void gayle_a1200_clear_irq(ide_drive_t *drive)
 {
-    ide_hwif_t *hwif = drive->hwif;
+        ide_hwif_t *hwif = drive->hwif;
 
-    (void)z_readb(hwif->io_ports.status_addr);
-    z_writeb(0x7c, hwif->io_ports.irq_addr);
+        (void)z_readb(hwif->io_ports.status_addr);
+        z_writeb(0x7c, hwif->io_ports.irq_addr);
 }
 
 static void __init gayle_setup_ports(struct ide_hw *hw, unsigned long base,
@@ -122,64 +114,89 @@ static const struct ide_port_info gayle_port_info = {
      *  Probe for a Gayle IDE interface (and optionally for an IDE doubler)
      */
 
-static int __init gayle_init(void)
+static int __init amiga_gayle_ide_probe(struct platform_device *pdev)
 {
-    unsigned long phys_base, res_start, res_n;
-    unsigned long base, ctrlport, irqport;
-    int a4000, i, rc;
-    struct ide_hw hw[GAYLE_NUM_HWIFS], *hws[GAYLE_NUM_HWIFS];
-    struct ide_port_info d = gayle_port_info;
-
-    if (!MACH_IS_AMIGA)
-        return -ENODEV;
-
-    if ((a4000 = AMIGAHW_PRESENT(A4000_IDE)) || AMIGAHW_PRESENT(A1200_IDE))
-        goto found;
-
-#ifdef CONFIG_ZORRO
-    if (zorro_find_device(ZORRO_PROD_MTEC_VIPER_MK_V_E_MATRIX_530_SCSI_IDE,
-                          NULL))
-        goto found;
-#endif
-    return -ENODEV;
-
-found:
-        printk(KERN_INFO "ide: Gayle IDE controller (A%d style%s)\n",
-                         a4000 ? 4000 : 1200,
-                         ide_doubler ? ", IDE doubler" : "");
-
-        if (a4000) {
-            phys_base = GAYLE_BASE_4000;
-            irqport = (unsigned long)ZTWO_VADDR(GAYLE_IRQ_4000);
-            d.port_ops = &gayle_a4000_port_ops;
-        } else {
-            phys_base = GAYLE_BASE_1200;
-            irqport = (unsigned long)ZTWO_VADDR(GAYLE_IRQ_1200);
-            d.port_ops = &gayle_a1200_port_ops;
+        struct resource *res;
+        struct gayle_ide_platform_data *pdata;
+        unsigned long base, ctrlport, irqport;
+        unsigned int i;
+        int error;
+        struct ide_hw hw[GAYLE_NUM_HWIFS], *hws[GAYLE_NUM_HWIFS];
+        struct ide_port_info d = gayle_port_info;
+        struct ide_host *host;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res)
+                return -ENODEV;
+
+        if (!request_mem_region(res->start, resource_size(res), "IDE"))
+                return -EBUSY;
+
+        pdata = pdev->dev.platform_data;
+        pr_info("ide: Gayle IDE controller (A%u style%s)\n",
+                pdata->explicit_ack ? 1200 : 4000,
+                ide_doubler ? ", IDE doubler" : "");
+
+        base = (unsigned long)ZTWO_VADDR(pdata->base);
+        ctrlport = 0;
+        irqport = (unsigned long)ZTWO_VADDR(pdata->irqport);
+        if (pdata->explicit_ack)
+                d.port_ops = &gayle_a1200_port_ops;
+        else
+                d.port_ops = &gayle_a4000_port_ops;
+
+        for (i = 0; i < GAYLE_NUM_PROBE_HWIFS; i++, base += GAYLE_NEXT_PORT) {
+                if (GAYLE_HAS_CONTROL_REG)
+                        ctrlport = base + GAYLE_CONTROL;
+
+                gayle_setup_ports(&hw[i], base, ctrlport, irqport);
+                hws[i] = &hw[i];
         }
 
-        res_start = ((unsigned long)phys_base) & ~(GAYLE_NEXT_PORT-1);
-        res_n = GAYLE_IDEREG_SIZE;
+        error = ide_host_add(&d, hws, i, &host);
+        if (error)
+                goto out;
 
-        if (!request_mem_region(res_start, res_n, "IDE"))
-                return -EBUSY;
+        platform_set_drvdata(pdev, host);
+        return 0;
 
-    for (i = 0; i < GAYLE_NUM_PROBE_HWIFS; i++) {
-        base = (unsigned long)ZTWO_VADDR(phys_base + i * GAYLE_NEXT_PORT);
-        ctrlport = GAYLE_HAS_CONTROL_REG ? (base + GAYLE_CONTROL) : 0;
+out:
+        release_mem_region(res->start, resource_size(res));
+        return error;
+}
+
+static int __exit amiga_gayle_ide_remove(struct platform_device *pdev)
+{
+        struct ide_host *host = platform_get_drvdata(pdev);
+        struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+        ide_host_remove(host);
+        release_mem_region(res->start, resource_size(res));
+        return 0;
+}
 
-        gayle_setup_ports(&hw[i], base, ctrlport, irqport);
+static struct platform_driver amiga_gayle_ide_driver = {
+        .remove = __exit_p(amiga_gayle_ide_remove),
+        .driver   = {
+                .name   = "amiga-gayle-ide",
+                .owner  = THIS_MODULE,
+        },
+};
 
-        hws[i] = &hw[i];
-    }
+static int __init amiga_gayle_ide_init(void)
+{
+        return platform_driver_probe(&amiga_gayle_ide_driver,
+                                     amiga_gayle_ide_probe);
+}
 
-    rc = ide_host_add(&d, hws, i, NULL);
-    if (rc)
-        release_mem_region(res_start, res_n);
+module_init(amiga_gayle_ide_init);
 
-    return rc;
+static void __exit amiga_gayle_ide_exit(void)
+{
+        platform_driver_unregister(&amiga_gayle_ide_driver);
 }
 
-module_init(gayle_init);
+module_exit(amiga_gayle_ide_exit);
 
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:amiga-gayle-ide");
diff --git a/drivers/ide/pmac.c b/drivers/ide/pmac.c
index 183fa38760d8..ebcf8e470a97 100644
--- a/drivers/ide/pmac.c
+++ b/drivers/ide/pmac.c
@@ -1400,8 +1400,11 @@ static struct of_device_id pmac_ide_macio_match[] =
 
 static struct macio_driver pmac_ide_macio_driver = 
 {
-        .name           = "ide-pmac",
-        .match_table    = pmac_ide_macio_match,
+        .driver = {
+                .name           = "ide-pmac",
+                .owner          = THIS_MODULE,
+                .of_match_table = pmac_ide_macio_match,
+        },
         .probe          = pmac_ide_macio_attach,
         .suspend        = pmac_ide_macio_suspend,
         .resume         = pmac_ide_macio_resume,
diff --git a/drivers/idle/Kconfig b/drivers/idle/Kconfig
index f15e90a453d1..fb5c5186d4aa 100644
--- a/drivers/idle/Kconfig
+++ b/drivers/idle/Kconfig
@@ -1,3 +1,14 @@
+config INTEL_IDLE
+        tristate "Cpuidle Driver for Intel Processors"
+        depends on CPU_IDLE
+        depends on X86
+        depends on CPU_SUP_INTEL
+        depends on EXPERIMENTAL
+        help
+          Enable intel_idle, a cpuidle driver that includes knowledge of
+          native Intel hardware idle features.  The acpi_idle driver
+          can be configured at the same time, in order to handle
+          processors intel_idle does not support.
 
 menu "Memory power savings"
 depends on X86_64
diff --git a/drivers/idle/Makefile b/drivers/idle/Makefile
index 5f68fc377e21..23d295cf10f2 100644
--- a/drivers/idle/Makefile
+++ b/drivers/idle/Makefile
@@ -1,2 +1,3 @@
 obj-$(CONFIG_I7300_IDLE)                        += i7300_idle.o
+obj-$(CONFIG_INTEL_IDLE)                        += intel_idle.o
 
diff --git a/drivers/idle/intel_idle.c b/drivers/idle/intel_idle.c
new file mode 100755
index 000000000000..54f0fb4cd5d2
--- /dev/null
+++ b/drivers/idle/intel_idle.c
@@ -0,0 +1,461 @@
+/*
+ * intel_idle.c - native hardware idle loop for modern Intel processors
+ *
+ * Copyright (c) 2010, Intel Corporation.
+ * Len Brown <len.brown@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+/*
+ * intel_idle is a cpuidle driver that loads on specific Intel processors
+ * in lieu of the legacy ACPI processor_idle driver.  The intent is to
+ * make Linux more efficient on these processors, as intel_idle knows
+ * more than ACPI, as well as make Linux more immune to ACPI BIOS bugs.
+ */
+
+/*
+ * Design Assumptions
+ *
+ * All CPUs have same idle states as boot CPU
+ *
+ * Chipset BM_STS (bus master status) bit is a NOP
+ *      for preventing entry into deep C-stats
+ */
+
+/*
+ * Known limitations
+ *
+ * The driver currently initializes for_each_online_cpu() upon modprobe.
+ * It it unaware of subsequent processors hot-added to the system.
+ * This means that if you boot with maxcpus=n and later online
+ * processors above n, those processors will use C1 only.
+ *
+ * ACPI has a .suspend hack to turn off deep c-statees during suspend
+ * to avoid complications with the lapic timer workaround.
+ * Have not seen issues with suspend, but may need same workaround here.
+ *
+ * There is currently no kernel-based automatic probing/loading mechanism
+ * if the driver is built as a module.
+ */
+
+/* un-comment DEBUG to enable pr_debug() statements */
+#define DEBUG
+
+#include <linux/kernel.h>
+#include <linux/cpuidle.h>
+#include <linux/clockchips.h>
+#include <linux/hrtimer.h>      /* ktime_get_real() */
+#include <trace/events/power.h>
+#include <linux/sched.h>
+
+#define INTEL_IDLE_VERSION "0.4"
+#define PREFIX "intel_idle: "
+
+#define MWAIT_SUBSTATE_MASK     (0xf)
+#define MWAIT_CSTATE_MASK       (0xf)
+#define MWAIT_SUBSTATE_SIZE     (4)
+#define MWAIT_MAX_NUM_CSTATES   8
+#define CPUID_MWAIT_LEAF (5)
+#define CPUID5_ECX_EXTENSIONS_SUPPORTED (0x1)
+#define CPUID5_ECX_INTERRUPT_BREAK      (0x2)
+
+static struct cpuidle_driver intel_idle_driver = {
+        .name = "intel_idle",
+        .owner = THIS_MODULE,
+};
+/* intel_idle.max_cstate=0 disables driver */
+static int max_cstate = MWAIT_MAX_NUM_CSTATES - 1;
+static int power_policy = 7; /* 0 = max perf; 15 = max powersave */
+
+static unsigned int substates;
+static int (*choose_substate)(int);
+
+/* Reliable LAPIC Timer States, bit 1 for C1 etc.  */
+static unsigned int lapic_timer_reliable_states;
+
+static struct cpuidle_device *intel_idle_cpuidle_devices;
+static int intel_idle(struct cpuidle_device *dev, struct cpuidle_state *state);
+
+static struct cpuidle_state *cpuidle_state_table;
+
+/*
+ * States are indexed by the cstate number,
+ * which is also the index into the MWAIT hint array.
+ * Thus C0 is a dummy.
+ */
+static struct cpuidle_state nehalem_cstates[MWAIT_MAX_NUM_CSTATES] = {
+        { /* MWAIT C0 */ },
+        { /* MWAIT C1 */
+                .name = "NHM-C1",
+                .desc = "MWAIT 0x00",
+                .driver_data = (void *) 0x00,
+                .flags = CPUIDLE_FLAG_TIME_VALID,
+                .exit_latency = 3,
+                .power_usage = 1000,
+                .target_residency = 6,
+                .enter = &intel_idle },
+        { /* MWAIT C2 */
+                .name = "NHM-C3",
+                .desc = "MWAIT 0x10",
+                .driver_data = (void *) 0x10,
+                .flags = CPUIDLE_FLAG_TIME_VALID,
+                .exit_latency = 20,
+                .power_usage = 500,
+                .target_residency = 80,
+                .enter = &intel_idle },
+        { /* MWAIT C3 */
+                .name = "NHM-C6",
+                .desc = "MWAIT 0x20",
+                .driver_data = (void *) 0x20,
+                .flags = CPUIDLE_FLAG_TIME_VALID,
+                .exit_latency = 200,
+                .power_usage = 350,
+                .target_residency = 800,
+                .enter = &intel_idle },
+};
+
+static struct cpuidle_state atom_cstates[MWAIT_MAX_NUM_CSTATES] = {
+        { /* MWAIT C0 */ },
+        { /* MWAIT C1 */
+                .name = "ATM-C1",
+                .desc = "MWAIT 0x00",
+                .driver_data = (void *) 0x00,
+                .flags = CPUIDLE_FLAG_TIME_VALID,
+                .exit_latency = 1,
+                .power_usage = 1000,
+                .target_residency = 4,
+                .enter = &intel_idle },
+        { /* MWAIT C2 */
+                .name = "ATM-C2",
+                .desc = "MWAIT 0x10",
+                .driver_data = (void *) 0x10,
+                .flags = CPUIDLE_FLAG_TIME_VALID,
+                .exit_latency = 20,
+                .power_usage = 500,
+                .target_residency = 80,
+                .enter = &intel_idle },
+        { /* MWAIT C3 */ },
+        { /* MWAIT C4 */
+                .name = "ATM-C4",
+                .desc = "MWAIT 0x30",
+                .driver_data = (void *) 0x30,
+                .flags = CPUIDLE_FLAG_TIME_VALID,
+                .exit_latency = 100,
+                .power_usage = 250,
+                .target_residency = 400,
+                .enter = &intel_idle },
+        { /* MWAIT C5 */ },
+        { /* MWAIT C6 */
+                .name = "ATM-C6",
+                .desc = "MWAIT 0x40",
+                .driver_data = (void *) 0x40,
+                .flags = CPUIDLE_FLAG_TIME_VALID,
+                .exit_latency = 200,
+                .power_usage = 150,
+                .target_residency = 800,
+                .enter = NULL },        /* disabled */
+};
+
+/*
+ * choose_tunable_substate()
+ *
+ * Run-time decision on which C-state substate to invoke
+ * If power_policy = 0, choose shallowest substate (0)
+ * If power_policy = 15, choose deepest substate
+ * If power_policy = middle, choose middle substate etc.
+ */
+static int choose_tunable_substate(int cstate)
+{
+        unsigned int num_substates;
+        unsigned int substate_choice;
+
+        power_policy &= 0xF;    /* valid range: 0-15 */
+        cstate &= 7;    /* valid range: 0-7 */
+
+        num_substates = (substates >> ((cstate) * 4)) & MWAIT_SUBSTATE_MASK;
+
+        if (num_substates <= 1)
+                return 0;
+
+        substate_choice = ((power_policy + (power_policy + 1) *
+                                (num_substates - 1)) / 16);
+
+        return substate_choice;
+}
+
+/*
+ * choose_zero_substate()
+ */
+static int choose_zero_substate(int cstate)
+{
+        return 0;
+}
+
+/**
+ * intel_idle
+ * @dev: cpuidle_device
+ * @state: cpuidle state
+ *
+ */
+static int intel_idle(struct cpuidle_device *dev, struct cpuidle_state *state)
+{
+        unsigned long ecx = 1; /* break on interrupt flag */
+        unsigned long eax = (unsigned long)cpuidle_get_statedata(state);
+        unsigned int cstate;
+        ktime_t kt_before, kt_after;
+        s64 usec_delta;
+        int cpu = smp_processor_id();
+
+        cstate = (((eax) >> MWAIT_SUBSTATE_SIZE) & MWAIT_CSTATE_MASK) + 1;
+
+        eax = eax + (choose_substate)(cstate);
+
+        local_irq_disable();
+
+        if (!(lapic_timer_reliable_states & (1 << (cstate))))
+                clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &cpu);
+
+        kt_before = ktime_get_real();
+
+        stop_critical_timings();
+#ifndef MODULE
+        trace_power_start(POWER_CSTATE, (eax >> 4) + 1);
+#endif
+        if (!need_resched()) {
+
+                __monitor((void *)&current_thread_info()->flags, 0, 0);
+                smp_mb();
+                if (!need_resched())
+                        __mwait(eax, ecx);
+        }
+
+        start_critical_timings();
+
+        kt_after = ktime_get_real();
+        usec_delta = ktime_to_us(ktime_sub(kt_after, kt_before));
+
+        local_irq_enable();
+
+        if (!(lapic_timer_reliable_states & (1 << (cstate))))
+                clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &cpu);
+
+        return usec_delta;
+}
+
+/*
+ * intel_idle_probe()
+ */
+static int intel_idle_probe(void)
+{
+        unsigned int eax, ebx, ecx, edx;
+
+        if (max_cstate == 0) {
+                pr_debug(PREFIX "disabled\n");
+                return -EPERM;
+        }
+
+        if (boot_cpu_data.x86_vendor != X86_VENDOR_INTEL)
+                return -ENODEV;
+
+        if (!boot_cpu_has(X86_FEATURE_MWAIT))
+                return -ENODEV;
+
+        if (boot_cpu_data.cpuid_level < CPUID_MWAIT_LEAF)
+                return -ENODEV;
+
+        cpuid(CPUID_MWAIT_LEAF, &eax, &ebx, &ecx, &edx);
+
+        if (!(ecx & CPUID5_ECX_EXTENSIONS_SUPPORTED) ||
+                !(ecx & CPUID5_ECX_INTERRUPT_BREAK))
+                        return -ENODEV;
+#ifdef DEBUG
+        if (substates == 0)     /* can over-ride via modparam */
+#endif
+                substates = edx;
+
+        pr_debug(PREFIX "MWAIT substates: 0x%x\n", substates);
+
+        if (boot_cpu_has(X86_FEATURE_ARAT))     /* Always Reliable APIC Timer */
+                lapic_timer_reliable_states = 0xFFFFFFFF;
+
+        if (boot_cpu_data.x86 != 6)     /* family 6 */
+                return -ENODEV;
+
+        switch (boot_cpu_data.x86_model) {
+
+        case 0x1A:      /* Core i7, Xeon 5500 series */
+        case 0x1E:      /* Core i7 and i5 Processor - Lynnfield Jasper Forest */
+        case 0x1F:      /* Core i7 and i5 Processor - Nehalem */
+        case 0x2E:      /* Nehalem-EX Xeon */
+                lapic_timer_reliable_states = (1 << 1);  /* C1 */
+
+        case 0x25:      /* Westmere */
+        case 0x2C:      /* Westmere */
+                cpuidle_state_table = nehalem_cstates;
+                choose_substate = choose_tunable_substate;
+                break;
+
+        case 0x1C:      /* 28 - Atom Processor */
+                lapic_timer_reliable_states = (1 << 2) | (1 << 1); /* C2, C1 */
+                cpuidle_state_table = atom_cstates;
+                choose_substate = choose_zero_substate;
+                break;
+#ifdef FUTURE_USE
+        case 0x17:      /* 23 - Core 2 Duo */
+                lapic_timer_reliable_states = (1 << 2) | (1 << 1); /* C2, C1 */
+#endif
+
+        default:
+                pr_debug(PREFIX "does not run on family %d model %d\n",
+                        boot_cpu_data.x86, boot_cpu_data.x86_model);
+                return -ENODEV;
+        }
+
+        pr_debug(PREFIX "v" INTEL_IDLE_VERSION
+                " model 0x%X\n", boot_cpu_data.x86_model);
+
+        pr_debug(PREFIX "lapic_timer_reliable_states 0x%x\n",
+                lapic_timer_reliable_states);
+        return 0;
+}
+
+/*
+ * intel_idle_cpuidle_devices_uninit()
+ * unregister, free cpuidle_devices
+ */
+static void intel_idle_cpuidle_devices_uninit(void)
+{
+        int i;
+        struct cpuidle_device *dev;
+
+        for_each_online_cpu(i) {
+                dev = per_cpu_ptr(intel_idle_cpuidle_devices, i);
+                cpuidle_unregister_device(dev);
+        }
+
+        free_percpu(intel_idle_cpuidle_devices);
+        return;
+}
+/*
+ * intel_idle_cpuidle_devices_init()
+ * allocate, initialize, register cpuidle_devices
+ */
+static int intel_idle_cpuidle_devices_init(void)
+{
+        int i, cstate;
+        struct cpuidle_device *dev;
+
+        intel_idle_cpuidle_devices = alloc_percpu(struct cpuidle_device);
+        if (intel_idle_cpuidle_devices == NULL)
+                return -ENOMEM;
+
+        for_each_online_cpu(i) {
+                dev = per_cpu_ptr(intel_idle_cpuidle_devices, i);
+
+                dev->state_count = 1;
+
+                for (cstate = 1; cstate < MWAIT_MAX_NUM_CSTATES; ++cstate) {
+                        int num_substates;
+
+                        if (cstate > max_cstate) {
+                                printk(PREFIX "max_cstate %d reached\n",
+                                        max_cstate);
+                                break;
+                        }
+
+                        /* does the state exist in CPUID.MWAIT? */
+                        num_substates = (substates >> ((cstate) * 4))
+                                                & MWAIT_SUBSTATE_MASK;
+                        if (num_substates == 0)
+                                continue;
+                        /* is the state not enabled? */
+                        if (cpuidle_state_table[cstate].enter == NULL) {
+                                /* does the driver not know about the state? */
+                                if (*cpuidle_state_table[cstate].name == '\0')
+                                        pr_debug(PREFIX "unaware of model 0x%x"
+                                                " MWAIT %d please"
+                                                " contact lenb@kernel.org",
+                                        boot_cpu_data.x86_model, cstate);
+                                continue;
+                        }
+
+                        if ((cstate > 2) &&
+                                !boot_cpu_has(X86_FEATURE_NONSTOP_TSC))
+                                mark_tsc_unstable("TSC halts in idle"
+                                        " states deeper than C2");
+
+                        dev->states[dev->state_count] = /* structure copy */
+                                cpuidle_state_table[cstate];
+
+                        dev->state_count += 1;
+                }
+
+                dev->cpu = i;
+                if (cpuidle_register_device(dev)) {
+                        pr_debug(PREFIX "cpuidle_register_device %d failed!\n",
+                                 i);
+                        intel_idle_cpuidle_devices_uninit();
+                        return -EIO;
+                }
+        }
+
+        return 0;
+}
+
+
+static int __init intel_idle_init(void)
+{
+        int retval;
+
+        retval = intel_idle_probe();
+        if (retval)
+                return retval;
+
+        retval = cpuidle_register_driver(&intel_idle_driver);
+        if (retval) {
+                printk(KERN_DEBUG PREFIX "intel_idle yielding to %s",
+                        cpuidle_get_driver()->name);
+                return retval;
+        }
+
+        retval = intel_idle_cpuidle_devices_init();
+        if (retval) {
+                cpuidle_unregister_driver(&intel_idle_driver);
+                return retval;
+        }
+
+        return 0;
+}
+
+static void __exit intel_idle_exit(void)
+{
+        intel_idle_cpuidle_devices_uninit();
+        cpuidle_unregister_driver(&intel_idle_driver);
+
+        return;
+}
+
+module_init(intel_idle_init);
+module_exit(intel_idle_exit);
+
+module_param(power_policy, int, 0644);
+module_param(max_cstate, int, 0444);
+#ifdef DEBUG
+module_param(substates, int, 0444);
+#endif
+
+MODULE_AUTHOR("Len Brown <len.brown@intel.com>");
+MODULE_DESCRIPTION("Cpuidle driver for Intel Hardware v" INTEL_IDLE_VERSION);
+MODULE_LICENSE("GPL");
diff --git a/drivers/ieee1394/dv1394.c b/drivers/ieee1394/dv1394.c
index 9fd4a0d3206e..adaefabc40e9 100644
--- a/drivers/ieee1394/dv1394.c
+++ b/drivers/ieee1394/dv1394.c
@@ -1824,7 +1824,7 @@ static int dv1394_open(struct inode *inode, struct file *file)
                "and will not be available in the new firewire driver stack. "
                "Try libraw1394 based programs instead.\n", current->comm);
 
-        return 0;
+        return nonseekable_open(inode, file);
 }
 
 
@@ -2153,17 +2153,18 @@ static struct cdev dv1394_cdev;
 static const struct file_operations dv1394_fops=
 {
         .owner =        THIS_MODULE,
-        .poll =         dv1394_poll,
+        .poll =         dv1394_poll,
         .unlocked_ioctl = dv1394_ioctl,
 #ifdef CONFIG_COMPAT
         .compat_ioctl = dv1394_compat_ioctl,
 #endif
         .mmap =         dv1394_mmap,
         .open =         dv1394_open,
-        .write =        dv1394_write,
-        .read =         dv1394_read,
+        .write =        dv1394_write,
+        .read =         dv1394_read,
         .release =      dv1394_release,
-        .fasync =       dv1394_fasync,
+        .fasync =       dv1394_fasync,
+        .llseek =       no_llseek,
 };
 
 
diff --git a/drivers/ieee1394/raw1394.c b/drivers/ieee1394/raw1394.c
index 8aa56ac07e29..b563d5e9fa2e 100644
--- a/drivers/ieee1394/raw1394.c
+++ b/drivers/ieee1394/raw1394.c
@@ -2834,7 +2834,7 @@ static int raw1394_open(struct inode *inode, struct file *file)
 
         file->private_data = fi;
 
-        return 0;
+        return nonseekable_open(inode, file);
 }
 
 static int raw1394_release(struct inode *inode, struct file *file)
@@ -3035,6 +3035,7 @@ static const struct file_operations raw1394_fops = {
         .poll = raw1394_poll,
         .open = raw1394_open,
         .release = raw1394_release,
+        .llseek = no_llseek,
 };
 
 static int __init init_raw1394(void)
diff --git a/drivers/ieee1394/video1394.c b/drivers/ieee1394/video1394.c
index 949064a05675..a42bd6893bcf 100644
--- a/drivers/ieee1394/video1394.c
+++ b/drivers/ieee1394/video1394.c
@@ -1239,7 +1239,7 @@ static int video1394_open(struct inode *inode, struct file *file)
         ctx->current_ctx = NULL;
         file->private_data = ctx;
 
-        return 0;
+        return nonseekable_open(inode, file);
 }
 
 static int video1394_release(struct inode *inode, struct file *file)
@@ -1287,7 +1287,8 @@ static const struct file_operations video1394_fops=
         .poll =         video1394_poll,
         .mmap =         video1394_mmap,
         .open =         video1394_open,
-        .release =      video1394_release
+        .release =      video1394_release,
+        .llseek =       no_llseek,
 };
 
 /*** HOTPLUG STUFF **********************************************************/
diff --git a/drivers/infiniband/core/ucm.c b/drivers/infiniband/core/ucm.c
index 46474842cfe9..08f948df8fa9 100644
--- a/drivers/infiniband/core/ucm.c
+++ b/drivers/infiniband/core/ucm.c
@@ -706,14 +706,9 @@ static int ib_ucm_alloc_data(const void **dest, u64 src, u32 len)
         if (!len)
                 return 0;
 
-        data = kmalloc(len, GFP_KERNEL);
-        if (!data)
-                return -ENOMEM;
-
-        if (copy_from_user(data, (void __user *)(unsigned long)src, len)) {
-                kfree(data);
-                return -EFAULT;
-        }
+        data = memdup_user((void __user *)(unsigned long)src, len);
+        if (IS_ERR(data))
+                return PTR_ERR(data);
 
         *dest = data;
         return 0;
diff --git a/drivers/infiniband/hw/ehca/ehca_irq.c b/drivers/infiniband/hw/ehca/ehca_irq.c
index 07cae552cafb..e571e60ecb88 100644
--- a/drivers/infiniband/hw/ehca/ehca_irq.c
+++ b/drivers/infiniband/hw/ehca/ehca_irq.c
@@ -847,7 +847,7 @@ static int __cpuinit comp_pool_callback(struct notifier_block *nfb,
                 ehca_gen_dbg("CPU: %x (CPU_PREPARE)", cpu);
                 if (!create_comp_task(pool, cpu)) {
                         ehca_gen_err("Can't create comp_task for cpu: %x", cpu);
-                        return NOTIFY_BAD;
+                        return notifier_from_errno(-ENOMEM);
                 }
                 break;
         case CPU_UP_CANCELED:
diff --git a/drivers/infiniband/hw/qib/qib_fs.c b/drivers/infiniband/hw/qib/qib_fs.c
index 755470440ef1..844954bf417b 100644
--- a/drivers/infiniband/hw/qib/qib_fs.c
+++ b/drivers/infiniband/hw/qib/qib_fs.c
@@ -144,10 +144,11 @@ static ssize_t dev_counters_read(struct file *file, char __user *buf,
                                  size_t count, loff_t *ppos)
 {
         u64 *counters;
+        size_t avail;
         struct qib_devdata *dd = private2dd(file);
 
-        return simple_read_from_buffer(buf, count, ppos, counters,
-                dd->f_read_cntrs(dd, *ppos, NULL, &counters));
+        avail = dd->f_read_cntrs(dd, *ppos, NULL, &counters);
+        return simple_read_from_buffer(buf, count, ppos, counters, avail);
 }
 
 /* read the per-device counters */
@@ -155,10 +156,11 @@ static ssize_t dev_names_read(struct file *file, char __user *buf,
                               size_t count, loff_t *ppos)
 {
         char *names;
+        size_t avail;
         struct qib_devdata *dd = private2dd(file);
 
-        return simple_read_from_buffer(buf, count, ppos, names,
-                dd->f_read_cntrs(dd, *ppos, &names, NULL));
+        avail = dd->f_read_cntrs(dd, *ppos, &names, NULL);
+        return simple_read_from_buffer(buf, count, ppos, names, avail);
 }
 
 static const struct file_operations cntr_ops[] = {
@@ -176,10 +178,11 @@ static ssize_t portnames_read(struct file *file, char __user *buf,
                               size_t count, loff_t *ppos)
 {
         char *names;
+        size_t avail;
         struct qib_devdata *dd = private2dd(file);
 
-        return simple_read_from_buffer(buf, count, ppos, names,
-                dd->f_read_portcntrs(dd, *ppos, 0, &names, NULL));
+        avail = dd->f_read_portcntrs(dd, *ppos, 0, &names, NULL);
+        return simple_read_from_buffer(buf, count, ppos, names, avail);
 }
 
 /* read the per-port counters for port 1 (pidx 0) */
@@ -187,10 +190,11 @@ static ssize_t portcntrs_1_read(struct file *file, char __user *buf,
                                 size_t count, loff_t *ppos)
 {
         u64 *counters;
+        size_t avail;
         struct qib_devdata *dd = private2dd(file);
 
-        return simple_read_from_buffer(buf, count, ppos, counters,
-                dd->f_read_portcntrs(dd, *ppos, 0, NULL, &counters));
+        avail = dd->f_read_portcntrs(dd, *ppos, 0, NULL, &counters);
+        return simple_read_from_buffer(buf, count, ppos, counters, avail);
 }
 
 /* read the per-port counters for port 2 (pidx 1) */
@@ -198,10 +202,11 @@ static ssize_t portcntrs_2_read(struct file *file, char __user *buf,
                                 size_t count, loff_t *ppos)
 {
         u64 *counters;
+        size_t avail;
         struct qib_devdata *dd = private2dd(file);
 
-        return simple_read_from_buffer(buf, count, ppos, counters,
-                dd->f_read_portcntrs(dd, *ppos, 1, NULL, &counters));
+        avail = dd->f_read_portcntrs(dd, *ppos, 1, NULL, &counters);
+        return simple_read_from_buffer(buf, count, ppos, counters, avail);
 }
 
 static const struct file_operations portcntr_ops[] = {
@@ -537,10 +542,8 @@ static int qibfs_fill_super(struct super_block *sb, void *data, int silent)
         list_for_each_entry_safe(dd, tmp, &qib_dev_list, list) {
                 spin_unlock_irqrestore(&qib_devs_lock, flags);
                 ret = add_cntr_files(sb, dd);
-                if (ret) {
-                        deactivate_super(sb);
+                if (ret)
                         goto bail;
-                }
                 spin_lock_irqsave(&qib_devs_lock, flags);
         }
 
diff --git a/drivers/infiniband/hw/qib/qib_iba6120.c b/drivers/infiniband/hw/qib/qib_iba6120.c
index 7b6549fd429b..1eadadc13da8 100644
--- a/drivers/infiniband/hw/qib/qib_iba6120.c
+++ b/drivers/infiniband/hw/qib/qib_iba6120.c
@@ -3475,14 +3475,6 @@ struct qib_devdata *qib_init_iba6120_funcs(struct pci_dev *pdev,
         struct qib_devdata *dd;
         int ret;
 
-#ifndef CONFIG_PCI_MSI
-        qib_early_err(&pdev->dev, "QLogic PCIE device 0x%x cannot "
-              "work if CONFIG_PCI_MSI is not enabled\n",
-              ent->device);
-        dd = ERR_PTR(-ENODEV);
-        goto bail;
-#endif
-
         dd = qib_alloc_devdata(pdev, sizeof(struct qib_pportdata) +
                                sizeof(struct qib_chip_specific));
         if (IS_ERR(dd))
@@ -3554,10 +3546,6 @@ struct qib_devdata *qib_init_iba6120_funcs(struct pci_dev *pdev,
         if (qib_mini_init)
                 goto bail;
 
-#ifndef CONFIG_PCI_MSI
-        qib_dev_err(dd, "PCI_MSI not configured, NO interrupts\n");
-#endif
-
         if (qib_pcie_params(dd, 8, NULL, NULL))
                 qib_dev_err(dd, "Failed to setup PCIe or interrupts; "
                             "continuing anyway\n");
diff --git a/drivers/infiniband/hw/qib/qib_iba7322.c b/drivers/infiniband/hw/qib/qib_iba7322.c
index 2c24eab35b54..503992d9c5ce 100644
--- a/drivers/infiniband/hw/qib/qib_iba7322.c
+++ b/drivers/infiniband/hw/qib/qib_iba7322.c
@@ -42,9 +42,6 @@
 #include <linux/jiffies.h>
 #include <rdma/ib_verbs.h>
 #include <rdma/ib_smi.h>
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-#include <linux/dca.h>
-#endif
 
 #include "qib.h"
 #include "qib_7322_regs.h"
@@ -114,40 +111,18 @@ static ushort qib_singleport;
 module_param_named(singleport, qib_singleport, ushort, S_IRUGO);
 MODULE_PARM_DESC(singleport, "Use only IB port 1; more per-port buffer space");
 
-
-/*
- * Setup QMH7342 receive and transmit parameters, necessary because
- * each bay, Mez connector, and IB port need different tuning, beyond
- * what the switch and HCA can do automatically.
- * It's expected to be done by cat'ing files to the modules file,
- * rather than setting up as a module parameter.
- * It's a "write-only" file, returns 0 when read back.
- * The unit, port, bay (if given), and values MUST be done as a single write.
- * The unit, port, and bay must precede the values to be effective.
- */
-static int setup_qmh_params(const char *, struct kernel_param *);
-static unsigned dummy_qmh_params;
-module_param_call(qmh_serdes_setup, setup_qmh_params, param_get_uint,
-                  &dummy_qmh_params, S_IWUSR | S_IRUGO);
-
-/* similarly for QME7342, but it's simpler */
-static int setup_qme_params(const char *, struct kernel_param *);
-static unsigned dummy_qme_params;
-module_param_call(qme_serdes_setup, setup_qme_params, param_get_uint,
-                  &dummy_qme_params, S_IWUSR | S_IRUGO);
-
 #define MAX_ATTEN_LEN 64 /* plenty for any real system */
 /* for read back, default index is ~5m copper cable */
-static char cable_atten_list[MAX_ATTEN_LEN] = "10";
-static struct kparam_string kp_cable_atten = {
-        .string = cable_atten_list,
+static char txselect_list[MAX_ATTEN_LEN] = "10";
+static struct kparam_string kp_txselect = {
+        .string = txselect_list,
         .maxlen = MAX_ATTEN_LEN
 };
-static int  setup_cable_atten(const char *, struct kernel_param *);
-module_param_call(cable_atten, setup_cable_atten, param_get_string,
-                  &kp_cable_atten, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(cable_atten, \
-                 "cable attenuation indices for cables with invalid EEPROM");
+static int  setup_txselect(const char *, struct kernel_param *);
+module_param_call(txselect, setup_txselect, param_get_string,
+                  &kp_txselect, S_IWUSR | S_IRUGO);
+MODULE_PARM_DESC(txselect, \
+                 "Tx serdes indices (for no QSFP or invalid QSFP data)");
 
 #define BOARD_QME7342 5
 #define BOARD_QMH7342 6
@@ -540,12 +515,6 @@ struct qib_chip_specific {
         u32 lastbuf_for_pio;
         u32 stay_in_freeze;
         u32 recovery_ports_initted;
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-        u32 dca_ctrl;
-        int rhdr_cpu[18];
-        int sdma_cpu[2];
-        u64 dca_rcvhdr_ctrl[5]; /* B, C, D, E, F */
-#endif
         struct msix_entry *msix_entries;
         void  **msix_arg;
         unsigned long *sendchkenable;
@@ -574,11 +543,12 @@ struct vendor_txdds_ent {
 static void write_tx_serdes_param(struct qib_pportdata *, struct txdds_ent *);
 
 #define TXDDS_TABLE_SZ 16 /* number of entries per speed in onchip table */
+#define TXDDS_EXTRA_SZ 11 /* number of extra tx settings entries */
 #define SERDES_CHANS 4 /* yes, it's obvious, but one less magic number */
 
 #define H1_FORCE_VAL 8
-#define H1_FORCE_QME 1 /*  may be overridden via setup_qme_params() */
-#define H1_FORCE_QMH 7 /*  may be overridden via setup_qmh_params() */
+#define H1_FORCE_QME 1 /*  may be overridden via setup_txselect() */
+#define H1_FORCE_QMH 7 /*  may be overridden via setup_txselect() */
 
 /* The static and dynamic registers are paired, and the pairs indexed by spd */
 #define krp_static_adapt_dis(spd) (KREG_IBPORT_IDX(ADAPT_DISABLE_STATIC_SDR) \
@@ -590,15 +560,6 @@ static void write_tx_serdes_param(struct qib_pportdata *, struct txdds_ent *);
 #define QDR_STATIC_ADAPT_INIT 0xffffffffffULL /* up, disable H0,H1-8, LE */
 #define QDR_STATIC_ADAPT_INIT_R1 0xf0ffffffffULL /* r1 up, disable H0,H1-8 */
 
-static const struct txdds_ent qmh_sdr_txdds =  { 11, 0,  5,  6 };
-static const struct txdds_ent qmh_ddr_txdds =  {  7, 0,  2,  8 };
-static const struct txdds_ent qmh_qdr_txdds =  {  0, 1,  3, 10 };
-
-/* this is used for unknown mez cards also */
-static const struct txdds_ent qme_sdr_txdds =  { 11, 0,  4,  4 };
-static const struct txdds_ent qme_ddr_txdds =  {  7, 0,  2,  7 };
-static const struct txdds_ent qme_qdr_txdds =  {  0, 1, 12, 11 };
-
 struct qib_chippport_specific {
         u64 __iomem *kpregbase;
         u64 __iomem *cpregbase;
@@ -637,12 +598,8 @@ struct qib_chippport_specific {
          * Per-bay per-channel rcv QMH H1 values and Tx values for QDR.
          * entry zero is unused, to simplify indexing
          */
-        u16 h1_val;
-        u8 amp[SERDES_CHANS];
-        u8 pre[SERDES_CHANS];
-        u8 mainv[SERDES_CHANS];
-        u8 post[SERDES_CHANS];
-        u8 no_eep;  /* attenuation index to use if no qsfp info */
+        u8 h1_val;
+        u8 no_eep;  /* txselect table index to use if no qsfp info */
         u8 ipg_tries;
         u8 ibmalfusesnap;
         struct qib_qsfp_data qsfp_data;
@@ -676,52 +633,6 @@ static struct {
                 SYM_LSB(IntStatus, SDmaCleanupDone_1), 2 },
 };
 
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-static const struct dca_reg_map {
-        int     shadow_inx;
-        int     lsb;
-        u64     mask;
-        u16     regno;
-} dca_rcvhdr_reg_map[] = {
-        { 0, SYM_LSB(DCACtrlB, RcvHdrq0DCAOPH),
-           ~SYM_MASK(DCACtrlB, RcvHdrq0DCAOPH) , KREG_IDX(DCACtrlB) },
-        { 0, SYM_LSB(DCACtrlB, RcvHdrq1DCAOPH),
-           ~SYM_MASK(DCACtrlB, RcvHdrq1DCAOPH) , KREG_IDX(DCACtrlB) },
-        { 0, SYM_LSB(DCACtrlB, RcvHdrq2DCAOPH),
-           ~SYM_MASK(DCACtrlB, RcvHdrq2DCAOPH) , KREG_IDX(DCACtrlB) },
-        { 0, SYM_LSB(DCACtrlB, RcvHdrq3DCAOPH),
-           ~SYM_MASK(DCACtrlB, RcvHdrq3DCAOPH) , KREG_IDX(DCACtrlB) },
-        { 1, SYM_LSB(DCACtrlC, RcvHdrq4DCAOPH),
-           ~SYM_MASK(DCACtrlC, RcvHdrq4DCAOPH) , KREG_IDX(DCACtrlC) },
-        { 1, SYM_LSB(DCACtrlC, RcvHdrq5DCAOPH),
-           ~SYM_MASK(DCACtrlC, RcvHdrq5DCAOPH) , KREG_IDX(DCACtrlC) },
-        { 1, SYM_LSB(DCACtrlC, RcvHdrq6DCAOPH),
-           ~SYM_MASK(DCACtrlC, RcvHdrq6DCAOPH) , KREG_IDX(DCACtrlC) },
-        { 1, SYM_LSB(DCACtrlC, RcvHdrq7DCAOPH),
-           ~SYM_MASK(DCACtrlC, RcvHdrq7DCAOPH) , KREG_IDX(DCACtrlC) },
-        { 2, SYM_LSB(DCACtrlD, RcvHdrq8DCAOPH),
-           ~SYM_MASK(DCACtrlD, RcvHdrq8DCAOPH) , KREG_IDX(DCACtrlD) },
-        { 2, SYM_LSB(DCACtrlD, RcvHdrq9DCAOPH),
-           ~SYM_MASK(DCACtrlD, RcvHdrq9DCAOPH) , KREG_IDX(DCACtrlD) },
-        { 2, SYM_LSB(DCACtrlD, RcvHdrq10DCAOPH),
-           ~SYM_MASK(DCACtrlD, RcvHdrq10DCAOPH) , KREG_IDX(DCACtrlD) },
-        { 2, SYM_LSB(DCACtrlD, RcvHdrq11DCAOPH),
-           ~SYM_MASK(DCACtrlD, RcvHdrq11DCAOPH) , KREG_IDX(DCACtrlD) },
-        { 3, SYM_LSB(DCACtrlE, RcvHdrq12DCAOPH),
-           ~SYM_MASK(DCACtrlE, RcvHdrq12DCAOPH) , KREG_IDX(DCACtrlE) },
-        { 3, SYM_LSB(DCACtrlE, RcvHdrq13DCAOPH),
-           ~SYM_MASK(DCACtrlE, RcvHdrq13DCAOPH) , KREG_IDX(DCACtrlE) },
-        { 3, SYM_LSB(DCACtrlE, RcvHdrq14DCAOPH),
-           ~SYM_MASK(DCACtrlE, RcvHdrq14DCAOPH) , KREG_IDX(DCACtrlE) },
-        { 3, SYM_LSB(DCACtrlE, RcvHdrq15DCAOPH),
-           ~SYM_MASK(DCACtrlE, RcvHdrq15DCAOPH) , KREG_IDX(DCACtrlE) },
-        { 4, SYM_LSB(DCACtrlF, RcvHdrq16DCAOPH),
-           ~SYM_MASK(DCACtrlF, RcvHdrq16DCAOPH) , KREG_IDX(DCACtrlF) },
-        { 4, SYM_LSB(DCACtrlF, RcvHdrq17DCAOPH),
-           ~SYM_MASK(DCACtrlF, RcvHdrq17DCAOPH) , KREG_IDX(DCACtrlF) },
-};
-#endif
-
 /* ibcctrl bits */
 #define QLOGIC_IB_IBCC_LINKINITCMD_DISABLE 1
 /* cycle through TS1/TS2 till OK */
@@ -2572,95 +2483,6 @@ static void qib_setup_7322_setextled(struct qib_pportdata *ppd, u32 on)
                 qib_write_kreg_port(ppd, krp_rcvpktledcnt, ledblink);
 }
 
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-static void qib_update_rhdrq_dca(struct qib_ctxtdata *rcd)
-{
-        struct qib_devdata *dd = rcd->dd;
-        struct qib_chip_specific *cspec = dd->cspec;
-        int cpu = get_cpu();
-
-        if (cspec->rhdr_cpu[rcd->ctxt] != cpu) {
-                const struct dca_reg_map *rmp;
-
-                cspec->rhdr_cpu[rcd->ctxt] = cpu;
-                rmp = &dca_rcvhdr_reg_map[rcd->ctxt];
-                cspec->dca_rcvhdr_ctrl[rmp->shadow_inx] &= rmp->mask;
-                cspec->dca_rcvhdr_ctrl[rmp->shadow_inx] |=
-                        (u64) dca3_get_tag(&dd->pcidev->dev, cpu) << rmp->lsb;
-                qib_write_kreg(dd, rmp->regno,
-                               cspec->dca_rcvhdr_ctrl[rmp->shadow_inx]);
-                cspec->dca_ctrl |= SYM_MASK(DCACtrlA, RcvHdrqDCAEnable);
-                qib_write_kreg(dd, KREG_IDX(DCACtrlA), cspec->dca_ctrl);
-        }
-        put_cpu();
-}
-
-static void qib_update_sdma_dca(struct qib_pportdata *ppd)
-{
-        struct qib_devdata *dd = ppd->dd;
-        struct qib_chip_specific *cspec = dd->cspec;
-        int cpu = get_cpu();
-        unsigned pidx = ppd->port - 1;
-
-        if (cspec->sdma_cpu[pidx] != cpu) {
-                cspec->sdma_cpu[pidx] = cpu;
-                cspec->dca_rcvhdr_ctrl[4] &= ~(ppd->hw_pidx ?
-                        SYM_MASK(DCACtrlF, SendDma1DCAOPH) :
-                        SYM_MASK(DCACtrlF, SendDma0DCAOPH));
-                cspec->dca_rcvhdr_ctrl[4] |=
-                        (u64) dca3_get_tag(&dd->pcidev->dev, cpu) <<
-                                (ppd->hw_pidx ?
-                                        SYM_LSB(DCACtrlF, SendDma1DCAOPH) :
-                                        SYM_LSB(DCACtrlF, SendDma0DCAOPH));
-                qib_write_kreg(dd, KREG_IDX(DCACtrlF),
-                               cspec->dca_rcvhdr_ctrl[4]);
-                cspec->dca_ctrl |= ppd->hw_pidx ?
-                        SYM_MASK(DCACtrlA, SendDMAHead1DCAEnable) :
-                        SYM_MASK(DCACtrlA, SendDMAHead0DCAEnable);
-                qib_write_kreg(dd, KREG_IDX(DCACtrlA), cspec->dca_ctrl);
-        }
-        put_cpu();
-}
-
-static void qib_setup_dca(struct qib_devdata *dd)
-{
-        struct qib_chip_specific *cspec = dd->cspec;
-        int i;
-
-        for (i = 0; i < ARRAY_SIZE(cspec->rhdr_cpu); i++)
-                cspec->rhdr_cpu[i] = -1;
-        for (i = 0; i < ARRAY_SIZE(cspec->sdma_cpu); i++)
-                cspec->sdma_cpu[i] = -1;
-        cspec->dca_rcvhdr_ctrl[0] =
-                (1ULL << SYM_LSB(DCACtrlB, RcvHdrq0DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlB, RcvHdrq1DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlB, RcvHdrq2DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlB, RcvHdrq3DCAXfrCnt));
-        cspec->dca_rcvhdr_ctrl[1] =
-                (1ULL << SYM_LSB(DCACtrlC, RcvHdrq4DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlC, RcvHdrq5DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlC, RcvHdrq6DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlC, RcvHdrq7DCAXfrCnt));
-        cspec->dca_rcvhdr_ctrl[2] =
-                (1ULL << SYM_LSB(DCACtrlD, RcvHdrq8DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlD, RcvHdrq9DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlD, RcvHdrq10DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlD, RcvHdrq11DCAXfrCnt));
-        cspec->dca_rcvhdr_ctrl[3] =
-                (1ULL << SYM_LSB(DCACtrlE, RcvHdrq12DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlE, RcvHdrq13DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlE, RcvHdrq14DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlE, RcvHdrq15DCAXfrCnt));
-        cspec->dca_rcvhdr_ctrl[4] =
-                (1ULL << SYM_LSB(DCACtrlF, RcvHdrq16DCAXfrCnt)) |
-                (1ULL << SYM_LSB(DCACtrlF, RcvHdrq17DCAXfrCnt));
-        for (i = 0; i < ARRAY_SIZE(cspec->sdma_cpu); i++)
-                qib_write_kreg(dd, KREG_IDX(DCACtrlB) + i,
-                               cspec->dca_rcvhdr_ctrl[i]);
-}
-
-#endif
-
 /*
  * Disable MSIx interrupt if enabled, call generic MSIx code
  * to cleanup, and clear pending MSIx interrupts.
@@ -2701,15 +2523,6 @@ static void qib_setup_7322_cleanup(struct qib_devdata *dd)
 {
         int i;
 
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-        if (dd->flags & QIB_DCA_ENABLED) {
-                dca_remove_requester(&dd->pcidev->dev);
-                dd->flags &= ~QIB_DCA_ENABLED;
-                dd->cspec->dca_ctrl = 0;
-                qib_write_kreg(dd, KREG_IDX(DCACtrlA), dd->cspec->dca_ctrl);
-        }
-#endif
-
         qib_7322_free_irq(dd);
         kfree(dd->cspec->cntrs);
         kfree(dd->cspec->sendchkenable);
@@ -3017,11 +2830,6 @@ static irqreturn_t qib_7322pintr(int irq, void *data)
         if (dd->int_counter != (u32) -1)
                 dd->int_counter++;
 
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-        if (dd->flags & QIB_DCA_ENABLED)
-                qib_update_rhdrq_dca(rcd);
-#endif
-
         /* Clear the interrupt bit we expect to be set. */
         qib_write_kreg(dd, kr_intclear, ((1ULL << QIB_I_RCVAVAIL_LSB) |
                        (1ULL << QIB_I_RCVURG_LSB)) << rcd->ctxt);
@@ -3085,11 +2893,6 @@ static irqreturn_t sdma_intr(int irq, void *data)
         if (dd->int_counter != (u32) -1)
                 dd->int_counter++;
 
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-        if (dd->flags & QIB_DCA_ENABLED)
-                qib_update_sdma_dca(ppd);
-#endif
-
         /* Clear the interrupt bit we expect to be set. */
         qib_write_kreg(dd, kr_intclear, ppd->hw_pidx ?
                        INT_MASK_P(SDma, 1) : INT_MASK_P(SDma, 0));
@@ -3119,11 +2922,6 @@ static irqreturn_t sdma_idle_intr(int irq, void *data)
         if (dd->int_counter != (u32) -1)
                 dd->int_counter++;
 
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-        if (dd->flags & QIB_DCA_ENABLED)
-                qib_update_sdma_dca(ppd);
-#endif
-
         /* Clear the interrupt bit we expect to be set. */
         qib_write_kreg(dd, kr_intclear, ppd->hw_pidx ?
                        INT_MASK_P(SDmaIdle, 1) : INT_MASK_P(SDmaIdle, 0));
@@ -3153,11 +2951,6 @@ static irqreturn_t sdma_progress_intr(int irq, void *data)
         if (dd->int_counter != (u32) -1)
                 dd->int_counter++;
 
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-        if (dd->flags & QIB_DCA_ENABLED)
-                qib_update_sdma_dca(ppd);
-#endif
-
         /* Clear the interrupt bit we expect to be set. */
         qib_write_kreg(dd, kr_intclear, ppd->hw_pidx ?
                        INT_MASK_P(SDmaProgress, 1) :
@@ -3188,11 +2981,6 @@ static irqreturn_t sdma_cleanup_intr(int irq, void *data)
         if (dd->int_counter != (u32) -1)
                 dd->int_counter++;
 
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-        if (dd->flags & QIB_DCA_ENABLED)
-                qib_update_sdma_dca(ppd);
-#endif
-
         /* Clear the interrupt bit we expect to be set. */
         qib_write_kreg(dd, kr_intclear, ppd->hw_pidx ?
                        INT_MASK_PM(SDmaCleanupDone, 1) :
@@ -4299,10 +4087,6 @@ static void rcvctrl_7322_mod(struct qib_pportdata *ppd, unsigned int op,
                 qib_write_kreg_ctxt(dd, krc_rcvhdraddr, ctxt,
                                     rcd->rcvhdrq_phys);
                 rcd->seq_cnt = 1;
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-                if (dd->flags & QIB_DCA_ENABLED)
-                        qib_update_rhdrq_dca(rcd);
-#endif
         }
         if (op & QIB_RCVCTRL_CTXT_DIS)
                 ppd->p_rcvctrl &=
@@ -5360,7 +5144,13 @@ static int qib_7322_ib_updown(struct qib_pportdata *ppd, int ibup, u64 ibcs)
                                      QIBL_IB_AUTONEG_INPROG)))
                         set_7322_ibspeed_fast(ppd, ppd->link_speed_enabled);
                 if (!(ppd->lflags & QIBL_IB_AUTONEG_INPROG)) {
+                        /* unlock the Tx settings, speed may change */
+                        qib_write_kreg_port(ppd, krp_tx_deemph_override,
+                                SYM_MASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
+                                reset_tx_deemphasis_override));
                         qib_cancel_sends(ppd);
+                        /* on link down, ensure sane pcs state */
+                        qib_7322_mini_pcs_reset(ppd);
                         spin_lock_irqsave(&ppd->sdma_lock, flags);
                         if (__qib_sdma_running(ppd))
                                 __qib_sdma_process_event(ppd,
@@ -5766,26 +5556,28 @@ static void qib_init_7322_qsfp(struct qib_pportdata *ppd)
 }
 
 /*
- * called at device initialization time, and also if the cable_atten
+ * called at device initialization time, and also if the txselect
  * module parameter is changed.  This is used for cables that don't
  * have valid QSFP EEPROMs (not present, or attenuation is zero).
  * We initialize to the default, then if there is a specific
- * unit,port match, we use that.
+ * unit,port match, we use that (and set it immediately, for the
+ * current speed, if the link is at INIT or better).
  * String format is "default# unit#,port#=# ... u,p=#", separators must
- * be a SPACE character.  A newline terminates.
+ * be a SPACE character.  A newline terminates.  The u,p=# tuples may
+ * optionally have "u,p=#,#", where the final # is the H1 value
  * The last specific match is used (actually, all are used, but last
  * one is the one that winds up set); if none at all, fall back on default.
  */
 static void set_no_qsfp_atten(struct qib_devdata *dd, int change)
 {
         char *nxt, *str;
-        int pidx, unit, port, deflt;
+        u32 pidx, unit, port, deflt, h1;
         unsigned long val;
-        int any = 0;
+        int any = 0, seth1;
 
-        str = cable_atten_list;
+        str = txselect_list;
 
-        /* default number is validated in setup_cable_atten() */
+        /* default number is validated in setup_txselect() */
         deflt = simple_strtoul(str, &nxt, 0);
         for (pidx = 0; pidx < dd->num_pports; ++pidx)
                 dd->pport[pidx].cpspec->no_eep = deflt;
@@ -5812,16 +5604,28 @@ static void set_no_qsfp_atten(struct qib_devdata *dd, int change)
                                 ;
                         continue;
                 }
-                if (val >= TXDDS_TABLE_SZ)
+                if (val >= TXDDS_TABLE_SZ + TXDDS_EXTRA_SZ)
                         continue;
+                seth1 = 0;
+                h1 = 0; /* gcc thinks it might be used uninitted */
+                if (*nxt == ',' && nxt[1]) {
+                        str = ++nxt;
+                        h1 = (u32)simple_strtoul(str, &nxt, 0);
+                        if (nxt == str)
+                                while (*nxt && *nxt++ != ' ') /* skip */
+                                        ;
+                        else
+                                seth1 = 1;
+                }
                 for (pidx = 0; dd->unit == unit && pidx < dd->num_pports;
                      ++pidx) {
-                        if (dd->pport[pidx].port != port ||
-                                !dd->pport[pidx].link_speed_supported)
+                        struct qib_pportdata *ppd = &dd->pport[pidx];
+
+                        if (ppd->port != port || !ppd->link_speed_supported)
                                 continue;
-                        dd->pport[pidx].cpspec->no_eep = val;
+                        ppd->cpspec->no_eep = val;
                         /* now change the IBC and serdes, overriding generic */
-                        init_txdds_table(&dd->pport[pidx], 1);
+                        init_txdds_table(ppd, 1);
                         any++;
                 }
                 if (*nxt == '\n')
@@ -5832,35 +5636,35 @@ static void set_no_qsfp_atten(struct qib_devdata *dd, int change)
                  * Change the IBC and serdes, but since it's
                  * general, don't override specific settings.
                  */
-                for (pidx = 0; pidx < dd->num_pports; ++pidx) {
-                        if (!dd->pport[pidx].link_speed_supported)
-                                continue;
-                        init_txdds_table(&dd->pport[pidx], 0);
-                }
+                for (pidx = 0; pidx < dd->num_pports; ++pidx)
+                        if (dd->pport[pidx].link_speed_supported)
+                                init_txdds_table(&dd->pport[pidx], 0);
         }
 }
 
-/* handle the cable_atten parameter changing */
-static int setup_cable_atten(const char *str, struct kernel_param *kp)
+/* handle the txselect parameter changing */
+static int setup_txselect(const char *str, struct kernel_param *kp)
 {
         struct qib_devdata *dd;
         unsigned long val;
         char *n;
         if (strlen(str) >= MAX_ATTEN_LEN) {
-                printk(KERN_INFO QIB_DRV_NAME " cable_atten_values string "
+                printk(KERN_INFO QIB_DRV_NAME " txselect_values string "
                        "too long\n");
                 return -ENOSPC;
         }
         val = simple_strtoul(str, &n, 0);
-        if (n == str || val >= TXDDS_TABLE_SZ) {
+        if (n == str || val >= (TXDDS_TABLE_SZ + TXDDS_EXTRA_SZ)) {
                 printk(KERN_INFO QIB_DRV_NAME
-                       "cable_atten_values must start with a number\n");
+                       "txselect_values must start with a number < %d\n",
+                        TXDDS_TABLE_SZ + TXDDS_EXTRA_SZ);
                 return -EINVAL;
         }
-        strcpy(cable_atten_list, str);
+        strcpy(txselect_list, str);
 
         list_for_each_entry(dd, &qib_dev_list, list)
-                set_no_qsfp_atten(dd, 1);
+                if (dd->deviceid == PCI_DEVICE_ID_QLOGIC_IB_7322)
+                        set_no_qsfp_atten(dd, 1);
         return 0;
 }
 
@@ -6261,28 +6065,17 @@ static int qib_init_7322_variables(struct qib_devdata *dd)
                  * in adapter-specific routines.
                  */
                 if (!(ppd->dd->flags & QIB_HAS_QSFP)) {
-                        int i;
-                        const struct txdds_ent *txdds;
-
                         if (!IS_QMH(ppd->dd) && !IS_QME(ppd->dd))
                                 qib_devinfo(ppd->dd->pcidev, "IB%u:%u: "
                                             "Unknown mezzanine card type\n",
-                                            ppd->dd->unit, ppd->port);
-                        txdds = IS_QMH(ppd->dd) ? &qmh_qdr_txdds :
-                                &qme_qdr_txdds;
-
+                                            dd->unit, ppd->port);
+                        cp->h1_val = IS_QMH(dd) ? H1_FORCE_QMH : H1_FORCE_QME;
                         /*
-                         * set values in case link comes up
-                         * before table is written to driver.
+                         * Choose center value as default tx serdes setting
+                         * until changed through module parameter.
                          */
-                        cp->h1_val = IS_QMH(ppd->dd) ? H1_FORCE_QMH :
-                                H1_FORCE_QME;
-                        for (i = 0; i < SERDES_CHANS; i++) {
-                                cp->amp[i] = txdds->amp;
-                                cp->pre[i] = txdds->pre;
-                                cp->mainv[i] = txdds->main;
-                                cp->post[i] = txdds->post;
-                        }
+                        ppd->cpspec->no_eep = IS_QMH(dd) ?
+                                TXDDS_TABLE_SZ + 2 : TXDDS_TABLE_SZ + 4;
                 } else
                         cp->h1_val = H1_FORCE_VAL;
 
@@ -6299,8 +6092,7 @@ static int qib_init_7322_variables(struct qib_devdata *dd)
 
         dd->rcvhdrentsize = QIB_RCVHDR_ENTSIZE;
         dd->rcvhdrsize = QIB_DFLT_RCVHDRSIZE;
-        dd->rhf_offset =
-                dd->rcvhdrentsize - sizeof(u64) / sizeof(u32);
+        dd->rhf_offset = dd->rcvhdrentsize - sizeof(u64) / sizeof(u32);
 
         /* we always allocate at least 2048 bytes for eager buffers */
         dd->rcvegrbufsize = max(mtu, 2048);
@@ -6919,13 +6711,6 @@ struct qib_devdata *qib_init_iba7322_funcs(struct pci_dev *pdev,
         /* clear diagctrl register, in case diags were running and crashed */
         qib_write_kreg(dd, kr_hwdiagctrl, 0);
 
-#if defined(CONFIG_DCA) || defined(CONFIG_DCA_MODULE)
-        ret = dca_add_requester(&pdev->dev);
-        if (!ret) {
-                dd->flags |= QIB_DCA_ENABLED;
-                qib_setup_dca(dd);
-        }
-#endif
         goto bail;
 
 bail_cleanup:
@@ -7111,8 +6896,8 @@ static const struct txdds_ent txdds_ddr[TXDDS_TABLE_SZ] = {
 static const struct txdds_ent txdds_qdr[TXDDS_TABLE_SZ] = {
         /* amp, pre, main, post */
         {  2, 2, 15,  6 },      /* Loopback */
-        {  0, 1,  0,  7 },      /*  2 dB */
-        {  0, 1,  0,  9 },      /*  3 dB */
+        {  0, 1,  0,  7 },      /*  2 dB (also QMH7342) */
+        {  0, 1,  0,  9 },      /*  3 dB (also QMH7342) */
         {  0, 1,  0, 11 },      /*  4 dB */
         {  0, 1,  0, 13 },      /*  5 dB */
         {  0, 1,  0, 15 },      /*  6 dB */
@@ -7128,6 +6913,57 @@ static const struct txdds_ent txdds_qdr[TXDDS_TABLE_SZ] = {
         {  0, 2,  9, 15 },      /* 16 dB */
 };
 
+/*
+ * extra entries for use with txselect, for indices >= TXDDS_TABLE_SZ.
+ * These are mostly used for mez cards going through connectors
+ * and backplane traces, but can be used to add other "unusual"
+ * table values as well.
+ */
+static const struct txdds_ent txdds_extra_sdr[TXDDS_EXTRA_SZ] = {
+        /* amp, pre, main, post */
+        {  0, 0, 0,  1 },       /* QMH7342 backplane settings */
+        {  0, 0, 0,  1 },       /* QMH7342 backplane settings */
+        {  0, 0, 0,  2 },       /* QMH7342 backplane settings */
+        {  0, 0, 0,  2 },       /* QMH7342 backplane settings */
+        {  0, 0, 0, 11 },       /* QME7342 backplane settings */
+        {  0, 0, 0, 11 },       /* QME7342 backplane settings */
+        {  0, 0, 0, 11 },       /* QME7342 backplane settings */
+        {  0, 0, 0, 11 },       /* QME7342 backplane settings */
+        {  0, 0, 0, 11 },       /* QME7342 backplane settings */
+        {  0, 0, 0, 11 },       /* QME7342 backplane settings */
+        {  0, 0, 0, 11 },       /* QME7342 backplane settings */
+};
+
+static const struct txdds_ent txdds_extra_ddr[TXDDS_EXTRA_SZ] = {
+        /* amp, pre, main, post */
+        {  0, 0, 0,  7 },       /* QMH7342 backplane settings */
+        {  0, 0, 0,  7 },       /* QMH7342 backplane settings */
+        {  0, 0, 0,  8 },       /* QMH7342 backplane settings */
+        {  0, 0, 0,  8 },       /* QMH7342 backplane settings */
+        {  0, 0, 0, 13 },       /* QME7342 backplane settings */
+        {  0, 0, 0, 13 },       /* QME7342 backplane settings */
+        {  0, 0, 0, 13 },       /* QME7342 backplane settings */
+        {  0, 0, 0, 13 },       /* QME7342 backplane settings */
+        {  0, 0, 0, 13 },       /* QME7342 backplane settings */
+        {  0, 0, 0, 13 },       /* QME7342 backplane settings */
+        {  0, 0, 0, 13 },       /* QME7342 backplane settings */
+};
+
+static const struct txdds_ent txdds_extra_qdr[TXDDS_EXTRA_SZ] = {
+        /* amp, pre, main, post */
+        {  0, 1,  0,  4 },      /* QMH7342 backplane settings */
+        {  0, 1,  0,  5 },      /* QMH7342 backplane settings */
+        {  0, 1,  0,  6 },      /* QMH7342 backplane settings */
+        {  0, 1,  0,  8 },      /* QMH7342 backplane settings */
+        {  0, 1, 12, 10 },      /* QME7342 backplane setting */
+        {  0, 1, 12, 11 },      /* QME7342 backplane setting */
+        {  0, 1, 12, 12 },      /* QME7342 backplane setting */
+        {  0, 1, 12, 14 },      /* QME7342 backplane setting */
+        {  0, 1, 12,  6 },      /* QME7342 backplane setting */
+        {  0, 1, 12,  7 },      /* QME7342 backplane setting */
+        {  0, 1, 12,  8 },      /* QME7342 backplane setting */
+};
+
 static const struct txdds_ent *get_atten_table(const struct txdds_ent *txdds,
                                                unsigned atten)
 {
@@ -7145,7 +6981,7 @@ static const struct txdds_ent *get_atten_table(const struct txdds_ent *txdds,
 }
 
 /*
- * if override is set, the module parameter cable_atten has a value
+ * if override is set, the module parameter txselect has a value
  * for this specific port, so use it, rather than our normal mechanism.
  */
 static void find_best_ent(struct qib_pportdata *ppd,
@@ -7184,15 +7020,28 @@ static void find_best_ent(struct qib_pportdata *ppd,
                 *ddr_dds = get_atten_table(txdds_ddr, qd->atten[0]);
                 *qdr_dds = get_atten_table(txdds_qdr, qd->atten[1]);
                 return;
-        } else {
+        } else if (ppd->cpspec->no_eep < TXDDS_TABLE_SZ) {
                 /*
                  * If we have no (or incomplete) data from the cable
-                 * EEPROM, or no QSFP, use the module parameter value
-                 * to index into the attentuation table.
+                 * EEPROM, or no QSFP, or override is set, use the
+                 * module parameter value to index into the attentuation
+                 * table.
                  */
-                *sdr_dds = &txdds_sdr[ppd->cpspec->no_eep];
-                *ddr_dds = &txdds_ddr[ppd->cpspec->no_eep];
-                *qdr_dds = &txdds_qdr[ppd->cpspec->no_eep];
+                idx = ppd->cpspec->no_eep;
+                *sdr_dds = &txdds_sdr[idx];
+                *ddr_dds = &txdds_ddr[idx];
+                *qdr_dds = &txdds_qdr[idx];
+        } else if (ppd->cpspec->no_eep < (TXDDS_TABLE_SZ + TXDDS_EXTRA_SZ)) {
+                /* similar to above, but index into the "extra" table. */
+                idx = ppd->cpspec->no_eep - TXDDS_TABLE_SZ;
+                *sdr_dds = &txdds_extra_sdr[idx];
+                *ddr_dds = &txdds_extra_ddr[idx];
+                *qdr_dds = &txdds_extra_qdr[idx];
+        } else {
+                /* this shouldn't happen, it's range checked */
+                *sdr_dds = txdds_sdr + qib_long_atten;
+                *ddr_dds = txdds_ddr + qib_long_atten;
+                *qdr_dds = txdds_qdr + qib_long_atten;
         }
 }
 
@@ -7203,33 +7052,24 @@ static void init_txdds_table(struct qib_pportdata *ppd, int override)
         int idx;
         int single_ent = 0;
 
-        if (IS_QMH(ppd->dd)) {
-                /* normally will be overridden, via setup_qmh() */
-                sdr_dds = &qmh_sdr_txdds;
-                ddr_dds = &qmh_ddr_txdds;
-                qdr_dds = &qmh_qdr_txdds;
-                single_ent = 1;
-        } else if (IS_QME(ppd->dd)) {
-                sdr_dds = &qme_sdr_txdds;
-                ddr_dds = &qme_ddr_txdds;
-                qdr_dds = &qme_qdr_txdds;
+        find_best_ent(ppd, &sdr_dds, &ddr_dds, &qdr_dds, override);
+
+        /* for mez cards or override, use the selected value for all entries */
+        if (!(ppd->dd->flags & QIB_HAS_QSFP) || override)
                 single_ent = 1;
-        } else
-                find_best_ent(ppd, &sdr_dds, &ddr_dds, &qdr_dds, override);
 
         /* Fill in the first entry with the best entry found. */
         set_txdds(ppd, 0, sdr_dds);
         set_txdds(ppd, TXDDS_TABLE_SZ, ddr_dds);
         set_txdds(ppd, 2 * TXDDS_TABLE_SZ, qdr_dds);
-
-        /*
-         * for our current speed, also write that value into the
-         * tx serdes registers.
-         */
-        dds = (struct txdds_ent *)(ppd->link_speed_active == QIB_IB_QDR ?
-                                   qdr_dds : (ppd->link_speed_active ==
-                                              QIB_IB_DDR ? ddr_dds : sdr_dds));
-        write_tx_serdes_param(ppd, dds);
+        if (ppd->lflags & (QIBL_LINKINIT | QIBL_LINKARMED |
+                QIBL_LINKACTIVE)) {
+                dds = (struct txdds_ent *)(ppd->link_speed_active ==
+                                           QIB_IB_QDR ?  qdr_dds :
+                                           (ppd->link_speed_active ==
+                                            QIB_IB_DDR ? ddr_dds : sdr_dds));
+                write_tx_serdes_param(ppd, dds);
+        }
 
         /* Fill in the remaining entries with the default table values. */
         for (idx = 1; idx < ARRAY_SIZE(txdds_sdr); ++idx) {
@@ -7352,6 +7192,11 @@ static int serdes_7322_init(struct qib_pportdata *ppd)
          */
         init_txdds_table(ppd, 0);
 
+        /* ensure no tx overrides from earlier driver loads */
+        qib_write_kreg_port(ppd, krp_tx_deemph_override,
+                SYM_MASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
+                reset_tx_deemphasis_override));
+
         /* Patch some SerDes defaults to "Better for IB" */
         /* Timing Loop Bandwidth: cdr_timing[11:9] = 0 */
         ibsd_wr_allchans(ppd, 2, 0, BMASK(11, 9));
@@ -7421,7 +7266,7 @@ static int serdes_7322_init(struct qib_pportdata *ppd)
                             QDR_STATIC_ADAPT_DOWN_R1 : QDR_STATIC_ADAPT_DOWN);
         ppd->cpspec->qdr_dfe_on = 1;
 
-        /* (FLoop LOS gate: PPM filter  enabled */
+        /* FLoop LOS gate: PPM filter  enabled */
         ibsd_wr_allchans(ppd, 38, 0 << 10, 1 << 10);
 
         /* rx offset center enabled */
@@ -7486,68 +7331,39 @@ static void write_tx_serdes_param(struct qib_pportdata *ppd,
                     SYM_MASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0, txc0_ena) |
                     SYM_MASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0, txcp1_ena) |
                     SYM_MASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0, txcn1_ena));
-        deemph |= 1ULL << SYM_LSB(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
-                                  tx_override_deemphasis_select);
-        deemph |= txdds->amp << SYM_LSB(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
-                                    txampcntl_d2a);
-        deemph |= txdds->main << SYM_LSB(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
-                                      txc0_ena);
-        deemph |= txdds->post << SYM_LSB(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
-                                     txcp1_ena);
-        deemph |= txdds->pre << SYM_LSB(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
+
+        deemph |= SYM_MASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
+                           tx_override_deemphasis_select);
+        deemph |= (txdds->amp & SYM_RMASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
+                    txampcntl_d2a)) << SYM_LSB(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
+                                       txampcntl_d2a);
+        deemph |= (txdds->main & SYM_RMASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
+                     txc0_ena)) << SYM_LSB(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
+                                   txc0_ena);
+        deemph |= (txdds->post & SYM_RMASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
+                     txcp1_ena)) << SYM_LSB(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
+                                    txcp1_ena);
+        deemph |= (txdds->pre & SYM_RMASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
+                     txcn1_ena)) << SYM_LSB(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
                                     txcn1_ena);
         qib_write_kreg_port(ppd, krp_tx_deemph_override, deemph);
 }
 
 /*
- * set per-bay, per channel parameters.  For now, we ignore
- * do_tx, and always set tx parameters, and set them with the same value
- * for all channels, using the channel 0 value.   We may switch to
- * per-channel settings in the future, and that method only needs
- * to be done once.
- * Because this also writes the IBC txdds table with a single set
- * of values, it should be called only for cases where we want to completely
- * force a specific setting, typically only for mez cards.
+ * Set the parameters for mez cards on link bounce, so they are
+ * always exactly what was requested.  Similar logic to init_txdds
+ * but does just the serdes.
  */
 static void adj_tx_serdes(struct qib_pportdata *ppd)
 {
-        struct txdds_ent txdds;
-        int i;
-        u8 *amp, *pre, *mainv, *post;
-
-        /*
-         * Because we use TX_DEEMPHASIS_OVERRIDE, we need to
-         * always do tx side, just like H1, since it is cleared
-         * by link down
-         */
-        amp = ppd->cpspec->amp;
-        pre = ppd->cpspec->pre;
-        mainv = ppd->cpspec->mainv;
-        post = ppd->cpspec->post;
-
-        amp[0] &= SYM_RMASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
-                            txampcntl_d2a);
-        mainv[0] &= SYM_RMASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
-                              txc0_ena);
-        post[0] &= SYM_RMASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
-                             txcp1_ena);
-        pre[0] &= SYM_RMASK(IBSD_TX_DEEMPHASIS_OVERRIDE_0,
-                            txcn1_ena);
-
-        /*
-         * Use the channel zero values, only, for now, for
-         * all channels
-        */
-        txdds.amp = amp[0];
-        txdds.pre = pre[0];
-        txdds.main = mainv[0];
-        txdds.post = post[0];
-
-        /* write the QDR table for IBC use, as backup for link down */
-        for (i = 0; i < ARRAY_SIZE(txdds_qdr); ++i)
-                set_txdds(ppd, i + 32, &txdds);
+        const struct txdds_ent *sdr_dds, *ddr_dds, *qdr_dds;
+        struct txdds_ent *dds;
 
-        write_tx_serdes_param(ppd, &txdds);
+        find_best_ent(ppd, &sdr_dds, &ddr_dds, &qdr_dds, 1);
+        dds = (struct txdds_ent *)(ppd->link_speed_active == QIB_IB_QDR ?
+                qdr_dds : (ppd->link_speed_active == QIB_IB_DDR ?
+                                ddr_dds : sdr_dds));
+        write_tx_serdes_param(ppd, dds);
 }
 
 /* set QDR forced value for H1, if needed */
@@ -7567,235 +7383,6 @@ static void force_h1(struct qib_pportdata *ppd)
         }
 }
 
-/*
- * Parse the parameters for the QMH7342, to get rx and tx serdes
- * settings for that Bay, for both possible mez connectors (PCIe bus)
- * and IB link (one link on mez1, two possible on mez2).
- *
- * Data is comma or white space separated.
- *
- * A set of data has 7 groups, rx and tx groups have SERDES_CHANS values,
- * one per IB lane (serdes channel).
- * The groups are Bay, bus# H1 rcv, and amp, pre, post, main Tx values (QDR).
- * The Bay # is used only for debugging currently.
- * H1 values are set whenever the link goes down, or is at cfg_test or
- * cfg_wait_enh.  Tx values are programmed once, when this routine is called
- * (and with default values at chip initialization).  Values are any base, in
- * strtoul style, and values are seperated by comma, or any white space
- * (space, tab, newline).
- *
- * An example set might look like this (white space vs
- * comma used for human ease of reading)
- * The ordering is a set of Bay# Bus# H1, amp, pre, post, and main for mez1 IB1,
- * repeat for mez2 IB1, then mez2 IB2.
- *
- * B B H1:0       amp:0       pre:0        post: 0        main:0
- * a u H1:  1     amp:  1     pre:  1      post:   1      main:  1
- * y s H1:    2   amp:    2   pre:    2    post:      2   main:    2
- *     H1:      4 amp:      3 pre:      3  post:        3 main:      3
- * 1 3    8,6,5,6     0,0,0,0     1,1,1,1       10,10,10,10    3,3,3,3
- * 1 6    7,6,6,7     0,0,0,0     1,1,1,1       10,10,10,10    3,3,3,3
- * 1 6    9,7,7,8     0,0,0,0     1,1,1,1       10,10,10,10    3,3,3,3
- */
-#define N_QMH_FIELDS 22
-static int setup_qmh_params(const char *str, struct kernel_param *kp)
-{
-        char *abuf, *v, *nv, *nvp;
-        struct qib_devdata *dd;
-        struct qib_pportdata *ppd;
-        u32 mez, vlen, nf, port, bay;
-        int ret = 0, found = 0;
-
-        vlen = strlen(str) + 1;
-        abuf = kmalloc(vlen, GFP_KERNEL);
-        if (!abuf) {
-                printk(KERN_INFO QIB_DRV_NAME
-                       " Unable to allocate QMH param buffer; ignoring\n");
-                return 0;
-        }
-        memcpy(abuf, str, vlen);
-        v = abuf;
-
-        /* these 3 are because gcc can't know they are set before used */
-        port = 1;
-        mez = 1; /* used only for debugging */
-        bay = 0; /* used only for debugging */
-        ppd = NULL;
-        for (nf = 0; (nv = strsep(&v, ", \t\n\r")) &&
-             nf < (N_QMH_FIELDS * 3);) {
-                u32 val;
-
-                if (!*nv)
-                        /* allow for multiple separators */
-                        continue;
-
-                val = simple_strtoul(nv, &nvp, 0);
-                if (nv == nvp) {
-                        printk(KERN_INFO QIB_DRV_NAME
-                               " Bay%u, mez%u IB%u non-numeric value (%s) "
-                               "field #%u, ignoring rest\n", bay, mez,
-                               port, nv, nf % (N_QMH_FIELDS * 3));
-                        ret = -EINVAL;
-                        goto bail;
-                }
-                if (!(nf % N_QMH_FIELDS)) {
-                        ppd = NULL;
-                        bay = val;
-                        if (!bay || bay > 16) {
-                                printk(KERN_INFO QIB_DRV_NAME
-                                       " Invalid bay # %u, field %u, "
-                                       "ignoring rest\n", bay, nf);
-                                ret = -EINVAL;
-                                goto bail;
-                        }
-                } else if ((nf % N_QMH_FIELDS) == 1) {
-                        u32 bus = val;
-                        if (nf == 1) {
-                                mez = 1;
-                                port = 1;
-                        } else if (nf == (N_QMH_FIELDS + 1)) {
-                                mez = 2;
-                                port = 1;
-                        } else {
-                                mez = 2;
-                                port = 2;
-                        }
-                        list_for_each_entry(dd, &qib_dev_list, list) {
-                                if (dd->deviceid != PCI_DEVICE_ID_QLOGIC_IB_7322
-                                    || !IS_QMH(dd))
-                                        continue; /* only for QMH cards */
-                                if (dd->pcidev->bus->number == bus) {
-                                        found++;
-                                        ppd = &dd->pport[port - 1];
-                                }
-                        }
-                } else if (ppd) {
-                        u32 parm = (nf % N_QMH_FIELDS) - 2;
-                        if (parm < SERDES_CHANS && !(parm % SERDES_CHANS))
-                                ppd->cpspec->h1_val = val;
-                        else if (parm < (2 * SERDES_CHANS))
-                                ppd->cpspec->amp[parm % SERDES_CHANS] = val;
-                        else if (parm < (3 * SERDES_CHANS))
-                                ppd->cpspec->pre[parm % SERDES_CHANS] = val;
-                        else if (parm < (4 * SERDES_CHANS))
-                                ppd->cpspec->post[parm % SERDES_CHANS] = val;
-                        else {
-                                ppd->cpspec->mainv[parm % SERDES_CHANS] = val;
-                                /* At the end of a port, set params */
-                                if (parm == ((5 * SERDES_CHANS) - 1))
-                                        adj_tx_serdes(ppd);
-                        }
-                }
-                nf++;
-        }
-        if (!found) {
-                printk(KERN_ERR QIB_DRV_NAME
-                       ": No match found for qmh_serdes_setup parameter\n");
-                ret = -EINVAL;
-        }
-bail:
-        kfree(abuf);
-        return ret;
-}
-
-/*
- * Similarly for QME7342, but the format is simpler, values are the
- * same for all mez card positions in a blade (2 or 4 per blade), but
- * are different for some blades vs others, and we don't need to
- * specify different parameters for different serdes channels or different
- * IB ports.
- * Format is: h1 amp,pre,post,main
- * Alternate format (so ports can be different): Pport# h1 amp,pre,post,main
- */
-#define N_QME_FIELDS 5
-static int setup_qme_params(const char *str, struct kernel_param *kp)
-{
-        char *abuf, *v, *nv, *nvp;
-        struct qib_devdata *dd;
-        u32 vlen, nf, port = 0;
-        u8 h1, tx[4]; /* amp, pre, post, main */
-        int ret =  -EINVAL;
-        char *seplist;
-
-        vlen = strlen(str) + 1;
-        abuf = kmalloc(vlen, GFP_KERNEL);
-        if (!abuf) {
-                printk(KERN_INFO QIB_DRV_NAME
-                       " Unable to allocate QME param buffer; ignoring\n");
-                return 0;
-        }
-        strncpy(abuf, str, vlen);
-
-        v = abuf;
-        seplist = " \t";
-        h1 = H1_FORCE_QME; /* gcc can't figure out always set before used */
-
-        for (nf = 0; (nv = strsep(&v, seplist)); ) {
-                u32 val;
-
-                if (!*nv)
-                        /* allow for multiple separators */
-                        continue;
-
-                if (!nf && *nv == 'P') {
-                        /* alternate format with port */
-                        val = simple_strtoul(++nv, &nvp, 0);
-                        if (nv == nvp || port >= NUM_IB_PORTS) {
-                                printk(KERN_INFO QIB_DRV_NAME
-                                       " %s: non-numeric port value (%s) "
-                                       "ignoring rest\n", __func__, nv);
-                                goto done;
-                        }
-                        port = val;
-                        continue; /* without incrementing nf */
-                }
-                val = simple_strtoul(nv, &nvp, 0);
-                if (nv == nvp) {
-                        printk(KERN_INFO QIB_DRV_NAME
-                               " %s: non-numeric value (%s) "
-                               "field #%u, ignoring rest\n", __func__,
-                               nv, nf);
-                        goto done;
-                }
-                if (!nf) {
-                        h1 = val;
-                        seplist = ",";
-                } else
-                        tx[nf - 1] = val;
-                if (++nf == N_QME_FIELDS) {
-                        list_for_each_entry(dd, &qib_dev_list, list) {
-                                int pidx, i;
-                                if (dd->deviceid != PCI_DEVICE_ID_QLOGIC_IB_7322
-                                    || !IS_QME(dd))
-                                        continue; /* only for QME cards */
-                                for (pidx = 0; pidx < dd->num_pports; ++pidx) {
-                                        struct qib_pportdata *ppd;
-                                        ppd = &dd->pport[pidx];
-                                        if ((port && ppd->port != port) ||
-                                                !ppd->link_speed_supported)
-                                                continue;
-                                        ppd->cpspec->h1_val = h1;
-                                        for (i = 0; i < SERDES_CHANS; i++) {
-                                                ppd->cpspec->amp[i] = tx[0];
-                                                ppd->cpspec->pre[i] = tx[1];
-                                                ppd->cpspec->post[i] = tx[2];
-                                                ppd->cpspec->mainv[i] = tx[3];
-                                        }
-                                        adj_tx_serdes(ppd);
-                                }
-                        }
-                        ret = 0;
-                        goto done;
-                }
-        }
-        printk(KERN_INFO QIB_DRV_NAME
-               " %s: Only %u of %u fields provided, skipping\n",
-               __func__, nf, N_QME_FIELDS);
-done:
-        kfree(abuf);
-        return ret;
-}
-
 #define SJA_EN SYM_MASK(SPC_JTAG_ACCESS_REG, SPC_JTAG_ACCESS_EN)
 #define BISTEN_LSB SYM_LSB(SPC_JTAG_ACCESS_REG, bist_en)
 
diff --git a/drivers/infiniband/hw/qib/qib_init.c b/drivers/infiniband/hw/qib/qib_init.c
index c0139c07e97e..9b40f345ac3f 100644
--- a/drivers/infiniband/hw/qib/qib_init.c
+++ b/drivers/infiniband/hw/qib/qib_init.c
@@ -1237,7 +1237,13 @@ static int __devinit qib_init_one(struct pci_dev *pdev,
          */
         switch (ent->device) {
         case PCI_DEVICE_ID_QLOGIC_IB_6120:
+#ifdef CONFIG_PCI_MSI
                 dd = qib_init_iba6120_funcs(pdev, ent);
+#else
+                qib_early_err(&pdev->dev, "QLogic PCIE device 0x%x cannot "
+                      "work if CONFIG_PCI_MSI is not enabled\n",
+                      ent->device);
+#endif
                 break;
 
         case PCI_DEVICE_ID_QLOGIC_IB_7220:
diff --git a/drivers/input/joydev.c b/drivers/input/joydev.c
index 423e0e6031ab..34157bb97ed6 100644
--- a/drivers/input/joydev.c
+++ b/drivers/input/joydev.c
@@ -47,15 +47,15 @@ struct joydev {
         struct mutex mutex;
         struct device dev;
 
-        struct js_corr corr[ABS_MAX + 1];
+        struct js_corr corr[ABS_CNT];
         struct JS_DATA_SAVE_TYPE glue;
         int nabs;
         int nkey;
         __u16 keymap[KEY_MAX - BTN_MISC + 1];
         __u16 keypam[KEY_MAX - BTN_MISC + 1];
-        __u8 absmap[ABS_MAX + 1];
-        __u8 abspam[ABS_MAX + 1];
-        __s16 abs[ABS_MAX + 1];
+        __u8 absmap[ABS_CNT];
+        __u8 abspam[ABS_CNT];
+        __s16 abs[ABS_CNT];
 };
 
 struct joydev_client {
@@ -826,7 +826,7 @@ static int joydev_connect(struct input_handler *handler, struct input_dev *dev,
         joydev->handle.handler = handler;
         joydev->handle.private = joydev;
 
-        for (i = 0; i < ABS_MAX + 1; i++)
+        for (i = 0; i < ABS_CNT; i++)
                 if (test_bit(i, dev->absbit)) {
                         joydev->absmap[i] = joydev->nabs;
                         joydev->abspam[joydev->nabs] = i;
diff --git a/drivers/input/keyboard/adp5588-keys.c b/drivers/input/keyboard/adp5588-keys.c
index 4771ab172b59..744600eff222 100644
--- a/drivers/input/keyboard/adp5588-keys.c
+++ b/drivers/input/keyboard/adp5588-keys.c
@@ -287,7 +287,6 @@ static int __devexit adp5588_remove(struct i2c_client *client)
         free_irq(client->irq, kpad);
         cancel_delayed_work_sync(&kpad->work);
         input_unregister_device(kpad->input);
-        i2c_set_clientdata(client, NULL);
         kfree(kpad);
 
         return 0;
diff --git a/drivers/input/keyboard/amikbd.c b/drivers/input/keyboard/amikbd.c
index 35149ec455a9..79172af164f2 100644
--- a/drivers/input/keyboard/amikbd.c
+++ b/drivers/input/keyboard/amikbd.c
@@ -35,6 +35,7 @@
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 #include <linux/keyboard.h>
+#include <linux/platform_device.h>
 
 #include <asm/amigaints.h>
 #include <asm/amigahw.h>
@@ -154,10 +155,9 @@ static const char *amikbd_messages[8] = {
         [7] = KERN_WARNING "amikbd: keyboard interrupt\n"
 };
 
-static struct input_dev *amikbd_dev;
-
-static irqreturn_t amikbd_interrupt(int irq, void *dummy)
+static irqreturn_t amikbd_interrupt(int irq, void *data)
 {
+        struct input_dev *dev = data;
         unsigned char scancode, down;
 
         scancode = ~ciaa.sdr;           /* get and invert scancode (keyboard is active low) */
@@ -170,47 +170,42 @@ static irqreturn_t amikbd_interrupt(int irq, void *dummy)
 
         if (scancode < 0x78) {          /* scancodes < 0x78 are keys */
                 if (scancode == 98) {   /* CapsLock is a toggle switch key on Amiga */
-                        input_report_key(amikbd_dev, scancode, 1);
-                        input_report_key(amikbd_dev, scancode, 0);
+                        input_report_key(dev, scancode, 1);
+                        input_report_key(dev, scancode, 0);
                 } else {
-                        input_report_key(amikbd_dev, scancode, down);
+                        input_report_key(dev, scancode, down);
                 }
 
-                input_sync(amikbd_dev);
+                input_sync(dev);
         } else                          /* scancodes >= 0x78 are error codes */
                 printk(amikbd_messages[scancode - 0x78]);
 
         return IRQ_HANDLED;
 }
 
-static int __init amikbd_init(void)
+static int __init amikbd_probe(struct platform_device *pdev)
 {
+        struct input_dev *dev;
         int i, j, err;
 
-        if (!AMIGAHW_PRESENT(AMI_KEYBOARD))
-                return -ENODEV;
-
-        if (!request_mem_region(CIAA_PHYSADDR-1+0xb00, 0x100, "amikeyb"))
-                return -EBUSY;
-
-        amikbd_dev = input_allocate_device();
-        if (!amikbd_dev) {
-                printk(KERN_ERR "amikbd: not enough memory for input device\n");
-                err = -ENOMEM;
-                goto fail1;
+        dev = input_allocate_device();
+        if (!dev) {
+                dev_err(&pdev->dev, "Not enough memory for input device\n");
+                return -ENOMEM;
         }
 
-        amikbd_dev->name = "Amiga Keyboard";
-        amikbd_dev->phys = "amikbd/input0";
-        amikbd_dev->id.bustype = BUS_AMIGA;
-        amikbd_dev->id.vendor = 0x0001;
-        amikbd_dev->id.product = 0x0001;
-        amikbd_dev->id.version = 0x0100;
+        dev->name = pdev->name;
+        dev->phys = "amikbd/input0";
+        dev->id.bustype = BUS_AMIGA;
+        dev->id.vendor = 0x0001;
+        dev->id.product = 0x0001;
+        dev->id.version = 0x0100;
+        dev->dev.parent = &pdev->dev;
 
-        amikbd_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
+        dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
 
         for (i = 0; i < 0x78; i++)
-                set_bit(i, amikbd_dev->keybit);
+                set_bit(i, dev->keybit);
 
         for (i = 0; i < MAX_NR_KEYMAPS; i++) {
                 static u_short temp_map[NR_KEYS] __initdata;
@@ -229,30 +224,54 @@ static int __init amikbd_init(void)
                 memcpy(key_maps[i], temp_map, sizeof(temp_map));
         }
         ciaa.cra &= ~0x41;       /* serial data in, turn off TA */
-        if (request_irq(IRQ_AMIGA_CIAA_SP, amikbd_interrupt, 0, "amikbd",
-                        amikbd_interrupt)) {
-                err = -EBUSY;
+        err = request_irq(IRQ_AMIGA_CIAA_SP, amikbd_interrupt, 0, "amikbd",
+                          dev);
+        if (err)
                 goto fail2;
-        }
 
-        err = input_register_device(amikbd_dev);
+        err = input_register_device(dev);
         if (err)
                 goto fail3;
 
+        platform_set_drvdata(pdev, dev);
+
         return 0;
 
- fail3: free_irq(IRQ_AMIGA_CIAA_SP, amikbd_interrupt);
- fail2: input_free_device(amikbd_dev);
- fail1: release_mem_region(CIAA_PHYSADDR - 1 + 0xb00, 0x100);
+ fail3: free_irq(IRQ_AMIGA_CIAA_SP, dev);
+ fail2: input_free_device(dev);
         return err;
 }
 
-static void __exit amikbd_exit(void)
+static int __exit amikbd_remove(struct platform_device *pdev)
+{
+        struct input_dev *dev = platform_get_drvdata(pdev);
+
+        platform_set_drvdata(pdev, NULL);
+        free_irq(IRQ_AMIGA_CIAA_SP, dev);
+        input_unregister_device(dev);
+        return 0;
+}
+
+static struct platform_driver amikbd_driver = {
+        .remove = __exit_p(amikbd_remove),
+        .driver   = {
+                .name   = "amiga-keyboard",
+                .owner  = THIS_MODULE,
+        },
+};
+
+static int __init amikbd_init(void)
 {
-        free_irq(IRQ_AMIGA_CIAA_SP, amikbd_interrupt);
-        input_unregister_device(amikbd_dev);
-        release_mem_region(CIAA_PHYSADDR - 1 + 0xb00, 0x100);
+        return platform_driver_probe(&amikbd_driver, amikbd_probe);
 }
 
 module_init(amikbd_init);
+
+static void __exit amikbd_exit(void)
+{
+        platform_driver_unregister(&amikbd_driver);
+}
+
 module_exit(amikbd_exit);
+
+MODULE_ALIAS("platform:amiga-keyboard");
diff --git a/drivers/input/keyboard/lm8323.c b/drivers/input/keyboard/lm8323.c
index bc696931fed7..40b032f0e32c 100644
--- a/drivers/input/keyboard/lm8323.c
+++ b/drivers/input/keyboard/lm8323.c
@@ -778,8 +778,6 @@ static int __devexit lm8323_remove(struct i2c_client *client)
         struct lm8323_chip *lm = i2c_get_clientdata(client);
         int i;
 
-        i2c_set_clientdata(client, NULL);
-
         disable_irq_wake(client->irq);
         free_irq(client->irq, lm);
         cancel_work_sync(&lm->work);
diff --git a/drivers/input/keyboard/max7359_keypad.c b/drivers/input/keyboard/max7359_keypad.c
index 7fc8185e5c1b..9091ff5ea808 100644
--- a/drivers/input/keyboard/max7359_keypad.c
+++ b/drivers/input/keyboard/max7359_keypad.c
@@ -265,7 +265,6 @@ static int __devexit max7359_remove(struct i2c_client *client)
 
         free_irq(client->irq, keypad);
         input_unregister_device(keypad->input_dev);
-        i2c_set_clientdata(client, NULL);
         kfree(keypad);
 
         return 0;
diff --git a/drivers/input/keyboard/qt2160.c b/drivers/input/keyboard/qt2160.c
index 31f30087b591..fac695157e8a 100644
--- a/drivers/input/keyboard/qt2160.c
+++ b/drivers/input/keyboard/qt2160.c
@@ -358,7 +358,6 @@ static int __devexit qt2160_remove(struct i2c_client *client)
         input_unregister_device(qt2160->input);
         kfree(qt2160);
 
-        i2c_set_clientdata(client, NULL);
         return 0;
 }
 
diff --git a/drivers/input/keyboard/tca6416-keypad.c b/drivers/input/keyboard/tca6416-keypad.c
index 493c93f25e2a..00137bebcf97 100644
--- a/drivers/input/keyboard/tca6416-keypad.c
+++ b/drivers/input/keyboard/tca6416-keypad.c
@@ -316,8 +316,6 @@ static int __devexit tca6416_keypad_remove(struct i2c_client *client)
         input_unregister_device(chip->input);
         kfree(chip);
 
-        i2c_set_clientdata(client, NULL);
-
         return 0;
 }
 
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index 48cdabec372a..c44b9eafc556 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -80,6 +80,16 @@ config INPUT_M68K_BEEP
         tristate "M68k Beeper support"
         depends on M68K
 
+config INPUT_MAX8925_ONKEY
+        tristate "MAX8925 ONKEY support"
+        depends on MFD_MAX8925
+        help
+          Support the ONKEY of MAX8925 PMICs as an input device
+          reporting power button status.
+
+          To compile this driver as a module, choose M here: the module
+          will be called max8925_onkey.
+
 config INPUT_APANEL
         tristate "Fujitsu Lifebook Application Panel buttons"
         depends on X86 && I2C && LEDS_CLASS
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index f9f577031e06..71fe57d8023f 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_HP_SDC_RTC)		+= hp_sdc_rtc.o
 obj-$(CONFIG_INPUT_IXP4XX_BEEPER)       += ixp4xx-beeper.o
 obj-$(CONFIG_INPUT_KEYSPAN_REMOTE)      += keyspan_remote.o
 obj-$(CONFIG_INPUT_M68K_BEEP)           += m68kspkr.o
+obj-$(CONFIG_INPUT_MAX8925_ONKEY)       += max8925_onkey.o
 obj-$(CONFIG_INPUT_PCAP)                += pcap_keys.o
 obj-$(CONFIG_INPUT_PCF50633_PMU)        += pcf50633-input.o
 obj-$(CONFIG_INPUT_PCF8574)             += pcf8574_keypad.o
diff --git a/drivers/input/misc/ad714x-i2c.c b/drivers/input/misc/ad714x-i2c.c
index e9adbe49f6a4..2bef8fa56c94 100644
--- a/drivers/input/misc/ad714x-i2c.c
+++ b/drivers/input/misc/ad714x-i2c.c
@@ -97,7 +97,6 @@ static int __devexit ad714x_i2c_remove(struct i2c_client *client)
         struct ad714x_chip *chip = i2c_get_clientdata(client);
 
         ad714x_remove(chip);
-        i2c_set_clientdata(client, NULL);
 
         return 0;
 }
diff --git a/drivers/input/misc/hp_sdc_rtc.c b/drivers/input/misc/hp_sdc_rtc.c
index e00a1cc79c0a..c19066479057 100644
--- a/drivers/input/misc/hp_sdc_rtc.c
+++ b/drivers/input/misc/hp_sdc_rtc.c
@@ -678,7 +678,7 @@ static const struct file_operations hp_sdc_rtc_fops = {
         .llseek =               no_llseek,
         .read =                 hp_sdc_rtc_read,
         .poll =                 hp_sdc_rtc_poll,
-        .unlocked_ioctl =       hp_sdc_rtc_ioctl,
+        .unlocked_ioctl =       hp_sdc_rtc_unlocked_ioctl,
         .open =                 hp_sdc_rtc_open,
         .fasync =               hp_sdc_rtc_fasync,
 };
diff --git a/drivers/input/misc/max8925_onkey.c b/drivers/input/misc/max8925_onkey.c
new file mode 100644
index 000000000000..80af44608018
--- /dev/null
+++ b/drivers/input/misc/max8925_onkey.c
@@ -0,0 +1,148 @@
+/**
+ * max8925_onkey.c - MAX8925 ONKEY driver
+ *
+ * Copyright (C) 2009 Marvell International Ltd.
+ *      Haojian Zhuang <haojian.zhuang@marvell.com>
+ *
+ * This file is subject to the terms and conditions of the GNU General
+ * Public License. See the file "COPYING" in the main directory of this
+ * archive for more details.
+ *
+ * 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/platform_device.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/max8925.h>
+#include <linux/slab.h>
+
+#define HARDRESET_EN            (1 << 7)
+#define PWREN_EN                (1 << 7)
+
+struct max8925_onkey_info {
+        struct input_dev        *idev;
+        struct i2c_client       *i2c;
+        int                     irq;
+};
+
+/*
+ * MAX8925 gives us an interrupt when ONKEY is held for 3 seconds.
+ * max8925_set_bits() operates I2C bus and may sleep. So implement
+ * it in thread IRQ handler.
+ */
+static irqreturn_t max8925_onkey_handler(int irq, void *data)
+{
+        struct max8925_onkey_info *info = data;
+
+        input_report_key(info->idev, KEY_POWER, 1);
+        input_sync(info->idev);
+
+        /* Enable hardreset to halt if system isn't shutdown on time */
+        max8925_set_bits(info->i2c, MAX8925_SYSENSEL,
+                         HARDRESET_EN, HARDRESET_EN);
+
+        return IRQ_HANDLED;
+}
+
+static int __devinit max8925_onkey_probe(struct platform_device *pdev)
+{
+        struct max8925_chip *chip = dev_get_drvdata(pdev->dev.parent);
+        struct max8925_onkey_info *info;
+        int error;
+
+        info = kzalloc(sizeof(struct max8925_onkey_info), GFP_KERNEL);
+        if (!info)
+                return -ENOMEM;
+
+        info->i2c = chip->i2c;
+        info->irq = chip->irq_base + MAX8925_IRQ_GPM_SW_3SEC;
+
+        info->idev = input_allocate_device();
+        if (!info->idev) {
+                dev_err(chip->dev, "Failed to allocate input dev\n");
+                error = -ENOMEM;
+                goto out_input;
+        }
+
+        info->idev->name = "max8925_on";
+        info->idev->phys = "max8925_on/input0";
+        info->idev->id.bustype = BUS_I2C;
+        info->idev->dev.parent = &pdev->dev;
+        info->idev->evbit[0] = BIT_MASK(EV_KEY);
+        info->idev->keybit[BIT_WORD(KEY_POWER)] = BIT_MASK(KEY_POWER);
+
+        error = request_threaded_irq(info->irq, NULL, max8925_onkey_handler,
+                                     IRQF_ONESHOT, "onkey", info);
+        if (error < 0) {
+                dev_err(chip->dev, "Failed to request IRQ: #%d: %d\n",
+                        info->irq, error);
+                goto out_irq;
+        }
+
+        error = input_register_device(info->idev);
+        if (error) {
+                dev_err(chip->dev, "Can't register input device: %d\n", error);
+                goto out;
+        }
+
+        platform_set_drvdata(pdev, info);
+
+        return 0;
+
+out:
+        free_irq(info->irq, info);
+out_irq:
+        input_free_device(info->idev);
+out_input:
+        kfree(info);
+        return error;
+}
+
+static int __devexit max8925_onkey_remove(struct platform_device *pdev)
+{
+        struct max8925_onkey_info *info = platform_get_drvdata(pdev);
+
+        free_irq(info->irq, info);
+        input_unregister_device(info->idev);
+        kfree(info);
+
+        platform_set_drvdata(pdev, NULL);
+
+        return 0;
+}
+
+static struct platform_driver max8925_onkey_driver = {
+        .driver         = {
+                .name   = "max8925-onkey",
+                .owner  = THIS_MODULE,
+        },
+        .probe          = max8925_onkey_probe,
+        .remove         = __devexit_p(max8925_onkey_remove),
+};
+
+static int __init max8925_onkey_init(void)
+{
+        return platform_driver_register(&max8925_onkey_driver);
+}
+module_init(max8925_onkey_init);
+
+static void __exit max8925_onkey_exit(void)
+{
+        platform_driver_unregister(&max8925_onkey_driver);
+}
+module_exit(max8925_onkey_exit);
+
+MODULE_DESCRIPTION("Maxim MAX8925 ONKEY driver");
+MODULE_AUTHOR("Haojian Zhuang <haojian.zhuang@marvell.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/misc/pcf8574_keypad.c b/drivers/input/misc/pcf8574_keypad.c
index 5c3ac4e0b055..0ac47d2898ec 100644
--- a/drivers/input/misc/pcf8574_keypad.c
+++ b/drivers/input/misc/pcf8574_keypad.c
@@ -168,8 +168,6 @@ static int __devexit pcf8574_kp_remove(struct i2c_client *client)
         input_unregister_device(lp->idev);
         kfree(lp);
 
-        i2c_set_clientdata(client, NULL);
-
         return 0;
 }
 
diff --git a/drivers/input/misc/twl4030-vibra.c b/drivers/input/misc/twl4030-vibra.c
index fee9eac8e04a..4f9b2afc24e8 100644
--- a/drivers/input/misc/twl4030-vibra.c
+++ b/drivers/input/misc/twl4030-vibra.c
@@ -90,8 +90,8 @@ static void vibra_disable(struct vibra_info *info)
         twl_i2c_write_u8(TWL4030_MODULE_AUDIO_VOICE,
                          (reg & ~TWL4030_VIBRA_EN), TWL4030_REG_VIBRA_CTL);
 
-        twl4030_codec_disable_resource(TWL4030_CODEC_RES_POWER);
         twl4030_codec_disable_resource(TWL4030_CODEC_RES_APLL);
+        twl4030_codec_disable_resource(TWL4030_CODEC_RES_POWER);
 
         info->enabled = false;
 }
diff --git a/drivers/input/misc/uinput.c b/drivers/input/misc/uinput.c
index 1477466076ad..b71eb55f2dbc 100644
--- a/drivers/input/misc/uinput.c
+++ b/drivers/input/misc/uinput.c
@@ -300,7 +300,7 @@ static int uinput_validate_absbits(struct input_dev *dev)
         unsigned int cnt;
         int retval = 0;
 
-        for (cnt = 0; cnt < ABS_MAX + 1; cnt++) {
+        for (cnt = 0; cnt < ABS_CNT; cnt++) {
                 if (!test_bit(cnt, dev->absbit))
                         continue;
 
@@ -387,7 +387,7 @@ static int uinput_setup_device(struct uinput_device *udev, const char __user *bu
         dev->id.product = user_dev->id.product;
         dev->id.version = user_dev->id.version;
 
-        size = sizeof(int) * (ABS_MAX + 1);
+        size = sizeof(int) * ABS_CNT;
         memcpy(dev->absmax, user_dev->absmax, size);
         memcpy(dev->absmin, user_dev->absmin, size);
         memcpy(dev->absfuzz, user_dev->absfuzz, size);
diff --git a/drivers/input/mouse/amimouse.c b/drivers/input/mouse/amimouse.c
index a185ac78a42c..ff5f61a0fd3a 100644
--- a/drivers/input/mouse/amimouse.c
+++ b/drivers/input/mouse/amimouse.c
@@ -21,6 +21,7 @@
 #include <linux/init.h>
 #include <linux/input.h>
 #include <linux/interrupt.h>
+#include <linux/platform_device.h>
 
 #include <asm/irq.h>
 #include <asm/setup.h>
@@ -34,10 +35,10 @@ MODULE_DESCRIPTION("Amiga mouse driver");
 MODULE_LICENSE("GPL");
 
 static int amimouse_lastx, amimouse_lasty;
-static struct input_dev *amimouse_dev;
 
-static irqreturn_t amimouse_interrupt(int irq, void *dummy)
+static irqreturn_t amimouse_interrupt(int irq, void *data)
 {
+        struct input_dev *dev = data;
         unsigned short joy0dat, potgor;
         int nx, ny, dx, dy;
 
@@ -59,14 +60,14 @@ static irqreturn_t amimouse_interrupt(int irq, void *dummy)
 
         potgor = amiga_custom.potgor;
 
-        input_report_rel(amimouse_dev, REL_X, dx);
-        input_report_rel(amimouse_dev, REL_Y, dy);
+        input_report_rel(dev, REL_X, dx);
+        input_report_rel(dev, REL_Y, dy);
 
-        input_report_key(amimouse_dev, BTN_LEFT,   ciaa.pra & 0x40);
-        input_report_key(amimouse_dev, BTN_MIDDLE, potgor & 0x0100);
-        input_report_key(amimouse_dev, BTN_RIGHT,  potgor & 0x0400);
+        input_report_key(dev, BTN_LEFT,   ciaa.pra & 0x40);
+        input_report_key(dev, BTN_MIDDLE, potgor & 0x0100);
+        input_report_key(dev, BTN_RIGHT,  potgor & 0x0400);
 
-        input_sync(amimouse_dev);
+        input_sync(dev);
 
         return IRQ_HANDLED;
 }
@@ -74,63 +75,90 @@ static irqreturn_t amimouse_interrupt(int irq, void *dummy)
 static int amimouse_open(struct input_dev *dev)
 {
         unsigned short joy0dat;
+        int error;
 
         joy0dat = amiga_custom.joy0dat;
 
         amimouse_lastx = joy0dat & 0xff;
         amimouse_lasty = joy0dat >> 8;
 
-        if (request_irq(IRQ_AMIGA_VERTB, amimouse_interrupt, 0, "amimouse", amimouse_interrupt)) {
-                printk(KERN_ERR "amimouse.c: Can't allocate irq %d\n", IRQ_AMIGA_VERTB);
-                return -EBUSY;
-        }
+        error = request_irq(IRQ_AMIGA_VERTB, amimouse_interrupt, 0, "amimouse",
+                            dev);
+        if (error)
+                dev_err(&dev->dev, "Can't allocate irq %d\n", IRQ_AMIGA_VERTB);
 
-        return 0;
+        return error;
 }
 
 static void amimouse_close(struct input_dev *dev)
 {
-        free_irq(IRQ_AMIGA_VERTB, amimouse_interrupt);
+        free_irq(IRQ_AMIGA_VERTB, dev);
 }
 
-static int __init amimouse_init(void)
+static int __init amimouse_probe(struct platform_device *pdev)
 {
         int err;
+        struct input_dev *dev;
 
-        if (!MACH_IS_AMIGA || !AMIGAHW_PRESENT(AMI_MOUSE))
-                return -ENODEV;
-
-        amimouse_dev = input_allocate_device();
-        if (!amimouse_dev)
+        dev = input_allocate_device();
+        if (!dev)
                 return -ENOMEM;
 
-        amimouse_dev->name = "Amiga mouse";
-        amimouse_dev->phys = "amimouse/input0";
-        amimouse_dev->id.bustype = BUS_AMIGA;
-        amimouse_dev->id.vendor = 0x0001;
-        amimouse_dev->id.product = 0x0002;
-        amimouse_dev->id.version = 0x0100;
+        dev->name = pdev->name;
+        dev->phys = "amimouse/input0";
+        dev->id.bustype = BUS_AMIGA;
+        dev->id.vendor = 0x0001;
+        dev->id.product = 0x0002;
+        dev->id.version = 0x0100;
 
-        amimouse_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REL);
-        amimouse_dev->relbit[0] = BIT_MASK(REL_X) | BIT_MASK(REL_Y);
-        amimouse_dev->keybit[BIT_WORD(BTN_LEFT)] = BIT_MASK(BTN_LEFT) |
+        dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REL);
+        dev->relbit[0] = BIT_MASK(REL_X) | BIT_MASK(REL_Y);
+        dev->keybit[BIT_WORD(BTN_LEFT)] = BIT_MASK(BTN_LEFT) |
                 BIT_MASK(BTN_MIDDLE) | BIT_MASK(BTN_RIGHT);
-        amimouse_dev->open = amimouse_open;
-        amimouse_dev->close = amimouse_close;
+        dev->open = amimouse_open;
+        dev->close = amimouse_close;
+        dev->dev.parent = &pdev->dev;
 
-        err = input_register_device(amimouse_dev);
+        err = input_register_device(dev);
         if (err) {
-                input_free_device(amimouse_dev);
+                input_free_device(dev);
                 return err;
         }
 
+        platform_set_drvdata(pdev, dev);
+
         return 0;
 }
 
-static void __exit amimouse_exit(void)
+static int __exit amimouse_remove(struct platform_device *pdev)
 {
-        input_unregister_device(amimouse_dev);
+        struct input_dev *dev = platform_get_drvdata(pdev);
+
+        platform_set_drvdata(pdev, NULL);
+        input_unregister_device(dev);
+        return 0;
+}
+
+static struct platform_driver amimouse_driver = {
+        .remove = __exit_p(amimouse_remove),
+        .driver   = {
+                .name   = "amiga-mouse",
+                .owner  = THIS_MODULE,
+        },
+};
+
+static int __init amimouse_init(void)
+{
+        return platform_driver_probe(&amimouse_driver, amimouse_probe);
 }
 
 module_init(amimouse_init);
+
+static void __exit amimouse_exit(void)
+{
+        platform_driver_unregister(&amimouse_driver);
+}
+
 module_exit(amimouse_exit);
+
+MODULE_ALIAS("platform:amiga-mouse");
diff --git a/drivers/input/mouse/synaptics_i2c.c b/drivers/input/mouse/synaptics_i2c.c
index 8291e7399ffa..0ae62f0bcb32 100644
--- a/drivers/input/mouse/synaptics_i2c.c
+++ b/drivers/input/mouse/synaptics_i2c.c
@@ -613,7 +613,6 @@ static int __devexit synaptics_i2c_remove(struct i2c_client *client)
                 free_irq(client->irq, touch);
 
         input_unregister_device(touch->input);
-        i2c_set_clientdata(client, NULL);
         kfree(touch);
 
         return 0;
diff --git a/drivers/input/serio/Kconfig b/drivers/input/serio/Kconfig
index f34f1dbeb577..3bfe8fafc6ad 100644
--- a/drivers/input/serio/Kconfig
+++ b/drivers/input/serio/Kconfig
@@ -21,7 +21,8 @@ if SERIO
 config SERIO_I8042
         tristate "i8042 PC Keyboard controller" if EMBEDDED || !X86
         default y
-        depends on !PARISC && (!ARM || ARCH_SHARK || FOOTBRIDGE_HOST) && !M68K && !BLACKFIN
+        depends on !PARISC && (!ARM || ARCH_SHARK || FOOTBRIDGE_HOST) && \
+                   (!SUPERH || SH_CAYMAN) && !M68K && !BLACKFIN
         help
           i8042 is the chip over which the standard AT keyboard and PS/2
           mouse are connected to the computer. If you use these devices,
diff --git a/drivers/input/tablet/wacom_sys.c b/drivers/input/tablet/wacom_sys.c
index 2dc0c07c0469..42ba3691d908 100644
--- a/drivers/input/tablet/wacom_sys.c
+++ b/drivers/input/tablet/wacom_sys.c
@@ -508,7 +508,6 @@ static int wacom_probe(struct usb_interface *intf, const struct usb_device_id *i
         }
 
         input_dev->name = wacom_wac->name;
-        input_dev->name = wacom_wac->name;
         input_dev->dev.parent = &intf->dev;
         input_dev->open = wacom_open;
         input_dev->close = wacom_close;
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index 847fd0135bcf..d564af58175c 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -300,7 +300,7 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
                 case 0x823: /* Intuos3 Grip Pen */
                 case 0x813: /* Intuos3 Classic Pen */
                 case 0x885: /* Intuos3 Marker Pen */
-                case 0x802: /* Intuos4 Grip Pen Eraser */
+                case 0x802: /* Intuos4 General Pen */
                 case 0x804: /* Intuos4 Marker Pen */
                 case 0x40802: /* Intuos4 Classic Pen */
                 case 0x022:
@@ -335,7 +335,7 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
                 case 0x81b: /* Intuos3 Classic Pen Eraser */
                 case 0x91b: /* Intuos3 Airbrush Eraser */
                 case 0x80c: /* Intuos4 Marker Pen Eraser */
-                case 0x80a: /* Intuos4 Grip Pen Eraser */
+                case 0x80a: /* Intuos4 General Pen Eraser */
                 case 0x4080a: /* Intuos4 Classic Pen Eraser */
                 case 0x90a: /* Intuos4 Airbrush Eraser */
                         wacom->tool[idx] = BTN_TOOL_RUBBER;
@@ -356,6 +356,11 @@ static int wacom_intuos_inout(struct wacom_wac *wacom)
                 return 1;
         }
 
+        /* older I4 styli don't work with new Cintiqs */
+        if (!((wacom->id[idx] >> 20) & 0x01) &&
+                        (features->type == WACOM_21UX2))
+                return 1;
+
         /* Exit report */
         if ((data[1] & 0xfe) == 0x80) {
                 /*
@@ -474,21 +479,43 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
                                 input_report_abs(input, ABS_MISC, 0);
                         }
                 } else {
-                        input_report_key(input, BTN_0, (data[5] & 0x01));
-                        input_report_key(input, BTN_1, (data[5] & 0x02));
-                        input_report_key(input, BTN_2, (data[5] & 0x04));
-                        input_report_key(input, BTN_3, (data[5] & 0x08));
-                        input_report_key(input, BTN_4, (data[6] & 0x01));
-                        input_report_key(input, BTN_5, (data[6] & 0x02));
-                        input_report_key(input, BTN_6, (data[6] & 0x04));
-                        input_report_key(input, BTN_7, (data[6] & 0x08));
-                        input_report_key(input, BTN_8, (data[5] & 0x10));
-                        input_report_key(input, BTN_9, (data[6] & 0x10));
+                        if (features->type == WACOM_21UX2) {
+                                input_report_key(input, BTN_0, (data[5] & 0x01));
+                                input_report_key(input, BTN_1, (data[6] & 0x01));
+                                input_report_key(input, BTN_2, (data[6] & 0x02));
+                                input_report_key(input, BTN_3, (data[6] & 0x04));
+                                input_report_key(input, BTN_4, (data[6] & 0x08));
+                                input_report_key(input, BTN_5, (data[6] & 0x10));
+                                input_report_key(input, BTN_6, (data[6] & 0x20));
+                                input_report_key(input, BTN_7, (data[6] & 0x40));
+                                input_report_key(input, BTN_8, (data[6] & 0x80));
+                                input_report_key(input, BTN_9, (data[7] & 0x01));
+                                input_report_key(input, BTN_A, (data[8] & 0x01));
+                                input_report_key(input, BTN_B, (data[8] & 0x02));
+                                input_report_key(input, BTN_C, (data[8] & 0x04));
+                                input_report_key(input, BTN_X, (data[8] & 0x08));
+                                input_report_key(input, BTN_Y, (data[8] & 0x10));
+                                input_report_key(input, BTN_Z, (data[8] & 0x20));
+                                input_report_key(input, BTN_BASE, (data[8] & 0x40));
+                                input_report_key(input, BTN_BASE2, (data[8] & 0x80));
+                        } else {
+                                input_report_key(input, BTN_0, (data[5] & 0x01));
+                                input_report_key(input, BTN_1, (data[5] & 0x02));
+                                input_report_key(input, BTN_2, (data[5] & 0x04));
+                                input_report_key(input, BTN_3, (data[5] & 0x08));
+                                input_report_key(input, BTN_4, (data[6] & 0x01));
+                                input_report_key(input, BTN_5, (data[6] & 0x02));
+                                input_report_key(input, BTN_6, (data[6] & 0x04));
+                                input_report_key(input, BTN_7, (data[6] & 0x08));
+                                input_report_key(input, BTN_8, (data[5] & 0x10));
+                                input_report_key(input, BTN_9, (data[6] & 0x10));
+                        }
                         input_report_abs(input, ABS_RX, ((data[1] & 0x1f) << 8) | data[2]);
                         input_report_abs(input, ABS_RY, ((data[3] & 0x1f) << 8) | data[4]);
 
                         if ((data[5] & 0x1f) | (data[6] & 0x1f) | (data[1] & 0x1f) |
-                                data[2] | (data[3] & 0x1f) | data[4]) {
+                                data[2] | (data[3] & 0x1f) | data[4] | data[8] |
+                                (data[7] & 0x01)) {
                                 input_report_key(input, wacom->tool[1], 1);
                                 input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
                         } else {
@@ -640,7 +667,7 @@ static void wacom_tpc_finger_in(struct wacom_wac *wacom, char *data, int idx)
         if (!idx)
                 input_report_key(input, BTN_TOUCH, 1);
         input_event(input, EV_MSC, MSC_SERIAL, finger);
-        input_sync(wacom->input);
+        input_sync(input);
 
         wacom->last_finger = finger;
 }
@@ -826,6 +853,7 @@ void wacom_wac_irq(struct wacom_wac *wacom_wac, size_t len)
         case INTUOS4L:
         case CINTIQ:
         case WACOM_BEE:
+        case WACOM_21UX2:
                 sync = wacom_intuos_irq(wacom_wac);
                 break;
 
@@ -921,6 +949,17 @@ void wacom_setup_input_capabilities(struct input_dev *input_dev,
                 __set_bit(BTN_STYLUS2, input_dev->keybit);
                 break;
 
+        case WACOM_21UX2:
+                __set_bit(BTN_A, input_dev->keybit);
+                __set_bit(BTN_B, input_dev->keybit);
+                __set_bit(BTN_C, input_dev->keybit);
+                __set_bit(BTN_X, input_dev->keybit);
+                __set_bit(BTN_Y, input_dev->keybit);
+                __set_bit(BTN_Z, input_dev->keybit);
+                __set_bit(BTN_BASE, input_dev->keybit);
+                __set_bit(BTN_BASE2, input_dev->keybit);
+                /* fall through */
+
         case WACOM_BEE:
                 __set_bit(BTN_8, input_dev->keybit);
                 __set_bit(BTN_9, input_dev->keybit);
@@ -1105,6 +1144,8 @@ static const struct wacom_features wacom_features_0xBA =
         { "Wacom Intuos4 8x13",   WACOM_PKGLEN_INTUOS,    65024, 40640, 2047, 63, INTUOS4L };
 static const struct wacom_features wacom_features_0xBB =
         { "Wacom Intuos4 12x19",  WACOM_PKGLEN_INTUOS,    97536, 60960, 2047, 63, INTUOS4L };
+static const struct wacom_features wacom_features_0xBC =
+        { "Wacom Intuos4 WL",     WACOM_PKGLEN_INTUOS,    40840, 25400, 2047, 63, INTUOS4 };
 static const struct wacom_features wacom_features_0x3F =
         { "Wacom Cintiq 21UX",    WACOM_PKGLEN_INTUOS,    87200, 65600, 1023, 63, CINTIQ };
 static const struct wacom_features wacom_features_0xC5 =
@@ -1113,6 +1154,8 @@ static const struct wacom_features wacom_features_0xC6 =
         { "Wacom Cintiq 12WX",    WACOM_PKGLEN_INTUOS,    53020, 33440, 1023, 63, WACOM_BEE };
 static const struct wacom_features wacom_features_0xC7 =
         { "Wacom DTU1931",        WACOM_PKGLEN_GRAPHIRE,  37832, 30305,  511,  0, PL };
+static const struct wacom_features wacom_features_0xCC =
+        { "Wacom Cintiq 21UX2",   WACOM_PKGLEN_INTUOS,    87200, 65600, 2047, 63, WACOM_21UX2 };
 static const struct wacom_features wacom_features_0x90 =
         { "Wacom ISDv4 90",       WACOM_PKGLEN_GRAPHIRE,  26202, 16325,  255,  0, TABLETPC };
 static const struct wacom_features wacom_features_0x93 =
@@ -1185,10 +1228,12 @@ const struct usb_device_id wacom_ids[] = {
         { USB_DEVICE_WACOM(0xB9) },
         { USB_DEVICE_WACOM(0xBA) },
         { USB_DEVICE_WACOM(0xBB) },
+        { USB_DEVICE_WACOM(0xBC) },
         { USB_DEVICE_WACOM(0x3F) },
         { USB_DEVICE_WACOM(0xC5) },
         { USB_DEVICE_WACOM(0xC6) },
         { USB_DEVICE_WACOM(0xC7) },
+        { USB_DEVICE_WACOM(0xCC) },
         { USB_DEVICE_WACOM(0x90) },
         { USB_DEVICE_WACOM(0x93) },
         { USB_DEVICE_WACOM(0x9A) },
diff --git a/drivers/input/tablet/wacom_wac.h b/drivers/input/tablet/wacom_wac.h
index 063f1af3204f..854b92092dfc 100644
--- a/drivers/input/tablet/wacom_wac.h
+++ b/drivers/input/tablet/wacom_wac.h
@@ -50,6 +50,7 @@ enum {
         INTUOS4S,
         INTUOS4,
         INTUOS4L,
+        WACOM_21UX2,
         CINTIQ,
         WACOM_BEE,
         WACOM_MO,
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index b9f58ca82fd1..3b9d5e2105d7 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -156,7 +156,7 @@ config TOUCHSCREEN_FUJITSU
 config TOUCHSCREEN_S3C2410
         tristate "Samsung S3C2410/generic touchscreen input driver"
         depends on ARCH_S3C2410 || SAMSUNG_DEV_TS
-        select S3C24XX_ADC
+        select S3C_ADC
         help
           Say Y here if you have the s3c2410 touchscreen.
 
@@ -590,4 +590,17 @@ config TOUCHSCREEN_PCAP
 
           To compile this driver as a module, choose M here: the
           module will be called pcap_ts.
+
+config TOUCHSCREEN_TPS6507X
+        tristate "TPS6507x based touchscreens"
+        depends on I2C
+        help
+          Say Y here if you have a TPS6507x based touchscreen
+          controller.
+
+          If unsure, say N.
+
+          To compile this driver as a module, choose M here: the
+          module will be called tps6507x_ts.
+
 endif
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 8ad36eef90a2..497964a7a214 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -46,3 +46,4 @@ obj-$(CONFIG_TOUCHSCREEN_WM97XX_ATMEL)	+= atmel-wm97xx.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX_MAINSTONE)      += mainstone-wm97xx.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX_ZYLONITE)       += zylonite-wm97xx.o
 obj-$(CONFIG_TOUCHSCREEN_W90X900)       += w90p910_ts.o
+obj-$(CONFIG_TOUCHSCREEN_TPS6507X)      += tps6507x-ts.o
diff --git a/drivers/input/touchscreen/ad7879.c b/drivers/input/touchscreen/ad7879.c
index 794d070c6900..4b32fb4704cd 100644
--- a/drivers/input/touchscreen/ad7879.c
+++ b/drivers/input/touchscreen/ad7879.c
@@ -812,10 +812,8 @@ static int __devinit ad7879_probe(struct i2c_client *client,
         ts->bus = client;
 
         error = ad7879_construct(client, ts);
-        if (error) {
-                i2c_set_clientdata(client, NULL);
+        if (error)
                 kfree(ts);
-        }
 
         return error;
 }
@@ -825,7 +823,6 @@ static int __devexit ad7879_remove(struct i2c_client *client)
         struct ad7879 *ts = dev_get_drvdata(&client->dev);
 
         ad7879_destroy(client, ts);
-        i2c_set_clientdata(client, NULL);
         kfree(ts);
 
         return 0;
diff --git a/drivers/input/touchscreen/ads7846.c b/drivers/input/touchscreen/ads7846.c
index 532279cda0e4..a9fdf55c0238 100644
--- a/drivers/input/touchscreen/ads7846.c
+++ b/drivers/input/touchscreen/ads7846.c
@@ -1163,8 +1163,8 @@ static int __devinit ads7846_probe(struct spi_device *spi)
 
         ts->reg = regulator_get(&spi->dev, "vcc");
         if (IS_ERR(ts->reg)) {
-                dev_err(&spi->dev, "unable to get regulator: %ld\n",
-                        PTR_ERR(ts->reg));
+                err = PTR_ERR(ts->reg);
+                dev_err(&spi->dev, "unable to get regulator: %d\n", err);
                 goto err_free_gpio;
         }
 
diff --git a/drivers/input/touchscreen/eeti_ts.c b/drivers/input/touchscreen/eeti_ts.c
index 75f8b73010fa..7a3a916f84a8 100644
--- a/drivers/input/touchscreen/eeti_ts.c
+++ b/drivers/input/touchscreen/eeti_ts.c
@@ -238,7 +238,6 @@ err2:
         input = NULL; /* so we dont try to free it below */
 err1:
         input_free_device(input);
-        i2c_set_clientdata(client, NULL);
         kfree(priv);
 err0:
         return err;
@@ -256,7 +255,6 @@ static int __devexit eeti_ts_remove(struct i2c_client *client)
         enable_irq(priv->irq);
 
         input_unregister_device(priv->input);
-        i2c_set_clientdata(client, NULL);
         kfree(priv);
 
         return 0;
diff --git a/drivers/input/touchscreen/mcs5000_ts.c b/drivers/input/touchscreen/mcs5000_ts.c
index ce8ab0269f6f..1fb0c2f06a44 100644
--- a/drivers/input/touchscreen/mcs5000_ts.c
+++ b/drivers/input/touchscreen/mcs5000_ts.c
@@ -256,7 +256,6 @@ static int __devexit mcs5000_ts_remove(struct i2c_client *client)
         free_irq(client->irq, data);
         input_unregister_device(data->input_dev);
         kfree(data);
-        i2c_set_clientdata(client, NULL);
 
         return 0;
 }
diff --git a/drivers/input/touchscreen/s3c2410_ts.c b/drivers/input/touchscreen/s3c2410_ts.c
index e0b7c834111d..6085d12fd561 100644
--- a/drivers/input/touchscreen/s3c2410_ts.c
+++ b/drivers/input/touchscreen/s3c2410_ts.c
@@ -173,7 +173,7 @@ static irqreturn_t stylus_irq(int irq, void *dev_id)
         if (down)
                 s3c_adc_start(ts.client, 0, 1 << ts.shift);
         else
-                dev_info(ts.dev, "%s: count=%d\n", __func__, ts.count);
+                dev_dbg(ts.dev, "%s: count=%d\n", __func__, ts.count);
 
         if (ts.features & FEAT_PEN_IRQ) {
                 /* Clear pen down/up interrupt */
@@ -413,6 +413,8 @@ static struct dev_pm_ops s3c_ts_pmops = {
 #endif
 
 static struct platform_device_id s3cts_driver_ids[] = {
+        { "s3c2410-ts", 0 },
+        { "s3c2440-ts", 0 },
         { "s3c64xx-ts", FEAT_PEN_IRQ },
         { }
 };
diff --git a/drivers/input/touchscreen/tps6507x-ts.c b/drivers/input/touchscreen/tps6507x-ts.c
new file mode 100644
index 000000000000..5b70a1419b4d
--- /dev/null
+++ b/drivers/input/touchscreen/tps6507x-ts.c
@@ -0,0 +1,396 @@
+/*
+ * drivers/input/touchscreen/tps6507x_ts.c
+ *
+ * Touchscreen driver for the tps6507x chip.
+ *
+ * Copyright (c) 2009 RidgeRun (todd.fischer@ridgerun.com)
+ *
+ * Credits:
+ *
+ *    Using code from tsc2007, MtekVision Co., Ltd.
+ *
+ * For licencing details see kernel-base/COPYING
+ *
+ * TPS65070, TPS65073, TPS650731, and TPS650732 support
+ * 10 bit touch screen interface.
+ */
+
+#include <linux/module.h>
+#include <linux/workqueue.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/tps6507x.h>
+#include <linux/input/tps6507x-ts.h>
+#include <linux/delay.h>
+
+#define TSC_DEFAULT_POLL_PERIOD 30 /* ms */
+#define TPS_DEFAULT_MIN_PRESSURE 0x30
+#define MAX_10BIT ((1 << 10) - 1)
+
+#define TPS6507X_ADCONFIG_CONVERT_TS (TPS6507X_ADCONFIG_AD_ENABLE | \
+                                         TPS6507X_ADCONFIG_START_CONVERSION | \
+                                         TPS6507X_ADCONFIG_INPUT_REAL_TSC)
+#define TPS6507X_ADCONFIG_POWER_DOWN_TS (TPS6507X_ADCONFIG_INPUT_REAL_TSC)
+
+struct ts_event {
+        u16     x;
+        u16     y;
+        u16     pressure;
+};
+
+struct tps6507x_ts {
+        struct input_dev        *input_dev;
+        struct device           *dev;
+        char                    phys[32];
+        struct workqueue_struct *wq;
+        struct delayed_work     work;
+        unsigned                polling;        /* polling is active */
+        struct ts_event         tc;
+        struct tps6507x_dev     *mfd;
+        u16                     model;
+        unsigned                pendown;
+        int                     irq;
+        void                    (*clear_penirq)(void);
+        unsigned long           poll_period;    /* ms */
+        u16                     min_pressure;
+        int                     vref;           /* non-zero to leave vref on */
+};
+
+static int tps6507x_read_u8(struct tps6507x_ts *tsc, u8 reg, u8 *data)
+{
+        int err;
+
+        err = tsc->mfd->read_dev(tsc->mfd, reg, 1, data);
+
+        if (err)
+                return err;
+
+        return 0;
+}
+
+static int tps6507x_write_u8(struct tps6507x_ts *tsc, u8 reg, u8 data)
+{
+        return tsc->mfd->write_dev(tsc->mfd, reg, 1, &data);
+}
+
+static s32 tps6507x_adc_conversion(struct tps6507x_ts *tsc,
+                                   u8 tsc_mode, u16 *value)
+{
+        s32 ret;
+        u8 adc_status;
+        u8 result;
+
+        /* Route input signal to A/D converter */
+
+        ret = tps6507x_write_u8(tsc, TPS6507X_REG_TSCMODE, tsc_mode);
+        if (ret) {
+                dev_err(tsc->dev, "TSC mode read failed\n");
+                goto err;
+        }
+
+        /* Start A/D conversion */
+
+        ret = tps6507x_write_u8(tsc, TPS6507X_REG_ADCONFIG,
+                                TPS6507X_ADCONFIG_CONVERT_TS);
+        if (ret) {
+                dev_err(tsc->dev, "ADC config write failed\n");
+                return ret;
+        }
+
+        do {
+                ret = tps6507x_read_u8(tsc, TPS6507X_REG_ADCONFIG,
+                                       &adc_status);
+                if (ret) {
+                        dev_err(tsc->dev, "ADC config read failed\n");
+                        goto err;
+                }
+        } while (adc_status & TPS6507X_ADCONFIG_START_CONVERSION);
+
+        ret = tps6507x_read_u8(tsc, TPS6507X_REG_ADRESULT_2, &result);
+        if (ret) {
+                dev_err(tsc->dev, "ADC result 2 read failed\n");
+                goto err;
+        }
+
+        *value = (result & TPS6507X_REG_ADRESULT_2_MASK) << 8;
+
+        ret = tps6507x_read_u8(tsc, TPS6507X_REG_ADRESULT_1, &result);
+        if (ret) {
+                dev_err(tsc->dev, "ADC result 1 read failed\n");
+                goto err;
+        }
+
+        *value |= result;
+
+        dev_dbg(tsc->dev, "TSC channel %d = 0x%X\n", tsc_mode, *value);
+
+err:
+        return ret;
+}
+
+/* Need to call tps6507x_adc_standby() after using A/D converter for the
+ * touch screen interrupt to work properly.
+ */
+
+static s32 tps6507x_adc_standby(struct tps6507x_ts *tsc)
+{
+        s32 ret;
+        s32 loops = 0;
+        u8 val;
+
+        ret = tps6507x_write_u8(tsc,  TPS6507X_REG_ADCONFIG,
+                                TPS6507X_ADCONFIG_INPUT_TSC);
+        if (ret)
+                return ret;
+
+        ret = tps6507x_write_u8(tsc, TPS6507X_REG_TSCMODE,
+                                TPS6507X_TSCMODE_STANDBY);
+        if (ret)
+                return ret;
+
+        ret = tps6507x_read_u8(tsc, TPS6507X_REG_INT, &val);
+        if (ret)
+                return ret;
+
+        while (val & TPS6507X_REG_TSC_INT) {
+                mdelay(10);
+                ret = tps6507x_read_u8(tsc, TPS6507X_REG_INT, &val);
+                if (ret)
+                        return ret;
+                loops++;
+        }
+
+        return ret;
+}
+
+static void tps6507x_ts_handler(struct work_struct *work)
+{
+        struct tps6507x_ts *tsc =  container_of(work,
+                                struct tps6507x_ts, work.work);
+        struct input_dev *input_dev = tsc->input_dev;
+        int pendown;
+        int schd;
+        int poll = 0;
+        s32 ret;
+
+        ret =  tps6507x_adc_conversion(tsc, TPS6507X_TSCMODE_PRESSURE,
+                                       &tsc->tc.pressure);
+        if (ret)
+                goto done;
+
+        pendown = tsc->tc.pressure > tsc->min_pressure;
+
+        if (unlikely(!pendown && tsc->pendown)) {
+                dev_dbg(tsc->dev, "UP\n");
+                input_report_key(input_dev, BTN_TOUCH, 0);
+                input_report_abs(input_dev, ABS_PRESSURE, 0);
+                input_sync(input_dev);
+                tsc->pendown = 0;
+        }
+
+        if (pendown) {
+
+                if (!tsc->pendown) {
+                        dev_dbg(tsc->dev, "DOWN\n");
+                        input_report_key(input_dev, BTN_TOUCH, 1);
+                } else
+                        dev_dbg(tsc->dev, "still down\n");
+
+                ret =  tps6507x_adc_conversion(tsc, TPS6507X_TSCMODE_X_POSITION,
+                                               &tsc->tc.x);
+                if (ret)
+                        goto done;
+
+                ret =  tps6507x_adc_conversion(tsc, TPS6507X_TSCMODE_Y_POSITION,
+                                               &tsc->tc.y);
+                if (ret)
+                        goto done;
+
+                input_report_abs(input_dev, ABS_X, tsc->tc.x);
+                input_report_abs(input_dev, ABS_Y, tsc->tc.y);
+                input_report_abs(input_dev, ABS_PRESSURE, tsc->tc.pressure);
+                input_sync(input_dev);
+                tsc->pendown = 1;
+                poll = 1;
+        }
+
+done:
+        /* always poll if not using interrupts */
+        poll = 1;
+
+        if (poll) {
+                schd = queue_delayed_work(tsc->wq, &tsc->work,
+                                          msecs_to_jiffies(tsc->poll_period));
+                if (schd)
+                        tsc->polling = 1;
+                else {
+                        tsc->polling = 0;
+                        dev_err(tsc->dev, "re-schedule failed");
+                }
+        } else
+                tsc->polling = 0;
+
+        ret = tps6507x_adc_standby(tsc);
+}
+
+static int tps6507x_ts_probe(struct platform_device *pdev)
+{
+        int error;
+        struct tps6507x_ts *tsc;
+        struct tps6507x_dev *tps6507x_dev = dev_get_drvdata(pdev->dev.parent);
+        struct touchscreen_init_data *init_data;
+        struct input_dev *input_dev;
+        struct tps6507x_board *tps_board;
+        int schd;
+
+        /**
+         * tps_board points to pmic related constants
+         * coming from the board-evm file.
+         */
+
+        tps_board = (struct tps6507x_board *)tps6507x_dev->dev->platform_data;
+
+        if (!tps_board) {
+                dev_err(tps6507x_dev->dev,
+                        "Could not find tps6507x platform data\n");
+                return -EIO;
+        }
+
+        /**
+         * init_data points to array of regulator_init structures
+         * coming from the board-evm file.
+         */
+
+        init_data = tps_board->tps6507x_ts_init_data;
+
+        tsc = kzalloc(sizeof(struct tps6507x_ts), GFP_KERNEL);
+        if (!tsc) {
+                dev_err(tps6507x_dev->dev, "failed to allocate driver data\n");
+                error = -ENOMEM;
+                goto err0;
+        }
+
+        tps6507x_dev->ts = tsc;
+        tsc->mfd = tps6507x_dev;
+        tsc->dev = tps6507x_dev->dev;
+        input_dev = input_allocate_device();
+        if (!input_dev) {
+                dev_err(tsc->dev, "Failed to allocate input device.\n");
+                error = -ENOMEM;
+                goto err1;
+        }
+
+        input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+        input_dev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
+
+        input_set_abs_params(input_dev, ABS_X, 0, MAX_10BIT, 0, 0);
+        input_set_abs_params(input_dev, ABS_Y, 0, MAX_10BIT, 0, 0);
+        input_set_abs_params(input_dev, ABS_PRESSURE, 0, MAX_10BIT, 0, 0);
+
+        input_dev->name = "TPS6507x Touchscreen";
+        input_dev->id.bustype = BUS_I2C;
+        input_dev->dev.parent = tsc->dev;
+
+        snprintf(tsc->phys, sizeof(tsc->phys),
+                 "%s/input0", dev_name(tsc->dev));
+        input_dev->phys = tsc->phys;
+
+        dev_dbg(tsc->dev, "device: %s\n", input_dev->phys);
+
+        input_set_drvdata(input_dev, tsc);
+
+        tsc->input_dev = input_dev;
+
+        INIT_DELAYED_WORK(&tsc->work, tps6507x_ts_handler);
+        tsc->wq = create_workqueue("TPS6507x Touchscreen");
+
+        if (init_data) {
+                tsc->poll_period = init_data->poll_period;
+                tsc->vref = init_data->vref;
+                tsc->min_pressure = init_data->min_pressure;
+                input_dev->id.vendor = init_data->vendor;
+                input_dev->id.product = init_data->product;
+                input_dev->id.version = init_data->version;
+        } else {
+                tsc->poll_period = TSC_DEFAULT_POLL_PERIOD;
+                tsc->min_pressure = TPS_DEFAULT_MIN_PRESSURE;
+        }
+
+        error = tps6507x_adc_standby(tsc);
+        if (error)
+                goto err2;
+
+        error = input_register_device(input_dev);
+        if (error)
+                goto err2;
+
+        schd = queue_delayed_work(tsc->wq, &tsc->work,
+                                  msecs_to_jiffies(tsc->poll_period));
+
+        if (schd)
+                tsc->polling = 1;
+        else {
+                tsc->polling = 0;
+                dev_err(tsc->dev, "schedule failed");
+                goto err2;
+         }
+
+        return 0;
+
+err2:
+        cancel_delayed_work_sync(&tsc->work);
+        destroy_workqueue(tsc->wq);
+        input_free_device(input_dev);
+err1:
+        kfree(tsc);
+        tps6507x_dev->ts = NULL;
+err0:
+        return error;
+}
+
+static int __devexit tps6507x_ts_remove(struct platform_device *pdev)
+{
+        struct tps6507x_dev *tps6507x_dev = platform_get_drvdata(pdev);
+        struct tps6507x_ts *tsc = tps6507x_dev->ts;
+        struct input_dev *input_dev = tsc->input_dev;
+
+        if (!tsc)
+                return 0;
+
+        cancel_delayed_work_sync(&tsc->work);
+        destroy_workqueue(tsc->wq);
+
+        input_free_device(input_dev);
+
+        tps6507x_dev->ts = NULL;
+        kfree(tsc);
+
+        return 0;
+}
+
+static struct platform_driver tps6507x_ts_driver = {
+        .driver = {
+                .name = "tps6507x-ts",
+                .owner = THIS_MODULE,
+        },
+        .probe = tps6507x_ts_probe,
+        .remove = __devexit_p(tps6507x_ts_remove),
+};
+
+static int __init tps6507x_ts_init(void)
+{
+        return platform_driver_register(&tps6507x_ts_driver);
+}
+module_init(tps6507x_ts_init);
+
+static void __exit tps6507x_ts_exit(void)
+{
+        platform_driver_unregister(&tps6507x_ts_driver);
+}
+module_exit(tps6507x_ts_exit);
+
+MODULE_AUTHOR("Todd Fischer <todd.fischer@ridgerun.com>");
+MODULE_DESCRIPTION("TPS6507x - TouchScreen driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:tps6507x-tsc");
diff --git a/drivers/input/touchscreen/tsc2007.c b/drivers/input/touchscreen/tsc2007.c
index 769b479fcaa6..be23780e8a3e 100644
--- a/drivers/input/touchscreen/tsc2007.c
+++ b/drivers/input/touchscreen/tsc2007.c
@@ -347,8 +347,6 @@ static int __devexit tsc2007_remove(struct i2c_client *client)
         struct tsc2007  *ts = i2c_get_clientdata(client);
         struct tsc2007_platform_data *pdata = client->dev.platform_data;
 
-        i2c_set_clientdata(client, NULL);
-
         tsc2007_free_irq(ts);
 
         if (pdata->exit_platform_hw)
diff --git a/drivers/input/touchscreen/usbtouchscreen.c b/drivers/input/touchscreen/usbtouchscreen.c
index 29a8bbf3f086..567d57215c28 100644
--- a/drivers/input/touchscreen/usbtouchscreen.c
+++ b/drivers/input/touchscreen/usbtouchscreen.c
@@ -857,6 +857,11 @@ static int nexio_read_data(struct usbtouch_usb *usbtouch, unsigned char *pkt)
         if ((pkt[0] & 0xe0) != 0xe0)
                 return 0;
 
+        if (be16_to_cpu(packet->data_len) > 0xff)
+                packet->data_len = cpu_to_be16(be16_to_cpu(packet->data_len) - 0x100);
+        if (be16_to_cpu(packet->x_len) > 0xff)
+                packet->x_len = cpu_to_be16(be16_to_cpu(packet->x_len) - 0x80);
+
         /* send ACK */
         ret = usb_submit_urb(priv->ack, GFP_ATOMIC);
 
@@ -1112,7 +1117,7 @@ static struct usbtouch_device_info usbtouch_dev_info[] = {
 
 #ifdef CONFIG_TOUCHSCREEN_USB_NEXIO
         [DEVTYPE_NEXIO] = {
-                .rept_size      = 128,
+                .rept_size      = 1024,
                 .irq_always     = true,
                 .read_data      = nexio_read_data,
                 .init           = nexio_init,
diff --git a/drivers/isdn/capi/kcapi.c b/drivers/isdn/capi/kcapi.c
index bde3c88b8b27..b054494df846 100644
--- a/drivers/isdn/capi/kcapi.c
+++ b/drivers/isdn/capi/kcapi.c
@@ -1020,12 +1020,12 @@ static int old_capi_manufacturer(unsigned int cmd, void __user *data)
                 if (cmd == AVMB1_ADDCARD) {
                    if ((retval = copy_from_user(&cdef, data,
                                             sizeof(avmb1_carddef))))
-                           return retval;
+                           return -EFAULT;
                    cdef.cardtype = AVM_CARDTYPE_B1;
                 } else {
                    if ((retval = copy_from_user(&cdef, data,
                                             sizeof(avmb1_extcarddef))))
-                           return retval;
+                           return -EFAULT;
                 }
                 cparams.port = cdef.port;
                 cparams.irq = cdef.irq;
@@ -1218,7 +1218,7 @@ int capi20_manufacturer(unsigned int cmd, void __user *data)
                 kcapi_carddef cdef;
 
                 if ((retval = copy_from_user(&cdef, data, sizeof(cdef))))
-                        return retval;
+                        return -EFAULT;
 
                 cparams.port = cdef.port;
                 cparams.irq = cdef.irq;
diff --git a/drivers/isdn/hardware/mISDN/hfcsusb.c b/drivers/isdn/hardware/mISDN/hfcsusb.c
index b3b7e2879bac..8700474747e8 100644
--- a/drivers/isdn/hardware/mISDN/hfcsusb.c
+++ b/drivers/isdn/hardware/mISDN/hfcsusb.c
@@ -97,8 +97,10 @@ static int write_reg(struct hfcsusb *hw, __u8 reg, __u8 val)
                         hw->name, __func__, reg, val);
 
         spin_lock(&hw->ctrl_lock);
-        if (hw->ctrl_cnt >= HFC_CTRL_BUFSIZE)
+        if (hw->ctrl_cnt >= HFC_CTRL_BUFSIZE) {
+                spin_unlock(&hw->ctrl_lock);
                 return 1;
+        }
         buf = &hw->ctrl_buff[hw->ctrl_in_idx];
         buf->hfcs_reg = reg;
         buf->reg_val = val;
diff --git a/drivers/isdn/hardware/mISDN/netjet.c b/drivers/isdn/hardware/mISDN/netjet.c
index 0a3553df065f..54ae71a907f9 100644
--- a/drivers/isdn/hardware/mISDN/netjet.c
+++ b/drivers/isdn/hardware/mISDN/netjet.c
@@ -320,12 +320,12 @@ inittiger(struct tiger_hw *card)
                 return -ENOMEM;
         }
         for (i = 0; i < 2; i++) {
-                card->bc[i].hsbuf = kmalloc(NJ_DMA_TXSIZE, GFP_KERNEL);
+                card->bc[i].hsbuf = kmalloc(NJ_DMA_TXSIZE, GFP_ATOMIC);
                 if (!card->bc[i].hsbuf) {
                         pr_info("%s: no B%d send buffer\n", card->name, i + 1);
                         return -ENOMEM;
                 }
-                card->bc[i].hrbuf = kmalloc(NJ_DMA_RXSIZE, GFP_KERNEL);
+                card->bc[i].hrbuf = kmalloc(NJ_DMA_RXSIZE, GFP_ATOMIC);
                 if (!card->bc[i].hrbuf) {
                         pr_info("%s: no B%d recv buffer\n", card->name, i + 1);
                         return -ENOMEM;
diff --git a/drivers/isdn/mISDN/timerdev.c b/drivers/isdn/mISDN/timerdev.c
index c3243c913ec0..81048b8ed8ad 100644
--- a/drivers/isdn/mISDN/timerdev.c
+++ b/drivers/isdn/mISDN/timerdev.c
@@ -98,8 +98,6 @@ mISDN_read(struct file *filep, char __user *buf, size_t count, loff_t *off)
         if (*debug & DEBUG_TIMER)
                 printk(KERN_DEBUG "%s(%p, %p, %d, %p)\n", __func__,
                         filep, buf, (int)count, off);
-        if (*off != filep->f_pos)
-                return -ESPIPE;
 
         if (list_empty(&dev->expired) && (dev->work == 0)) {
                 if (filep->f_flags & O_NONBLOCK)
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 505eb64c329c..81bf25e67ce1 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -21,7 +21,7 @@ comment "LED drivers"
 
 config LEDS_88PM860X
         tristate "LED Support for Marvell 88PM860x PMIC"
-        depends on LEDS_CLASS && MFD_88PM860X
+        depends on MFD_88PM860X
         help
           This option enables support for on-chip LED drivers found on Marvell
           Semiconductor 88PM8606 PMIC.
@@ -67,6 +67,16 @@ config LEDS_NET48XX
           This option enables support for the Soekris net4801 and net4826 error
           LED.
 
+config LEDS_NET5501
+        tristate "LED Support for Soekris net5501 series Error LED"
+        depends on LEDS_TRIGGERS
+        depends on X86 && LEDS_GPIO_PLATFORM && GPIO_CS5535
+        select LEDS_TRIGGER_DEFAULT_ON
+        default n
+        help
+          Add support for the Soekris net5501 board (detection, error led
+          and GPIO).
+
 config LEDS_FSG
         tristate "LED Support for the Freecom FSG-3"
         depends on MACH_FSG
@@ -285,6 +295,13 @@ config LEDS_DELL_NETBOOKS
           This adds support for the Latitude 2100 and similar
           notebooks that have an external LED.
 
+config LEDS_MC13783
+        tristate "LED Support for MC13783 PMIC"
+        depends on MFD_MC13783
+        help
+          This option enable support for on-chip LED drivers found
+          on Freescale Semiconductor MC13783 PMIC.
+
 config LEDS_TRIGGERS
         bool "LED Trigger support"
         help
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index 0cd8b9957380..2493de499374 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_LEDS_MIKROTIK_RB532)	+= leds-rb532.o
 obj-$(CONFIG_LEDS_S3C24XX)              += leds-s3c24xx.o
 obj-$(CONFIG_LEDS_AMS_DELTA)            += leds-ams-delta.o
 obj-$(CONFIG_LEDS_NET48XX)              += leds-net48xx.o
+obj-$(CONFIG_LEDS_NET5501)              += leds-net5501.o
 obj-$(CONFIG_LEDS_WRAP)                 += leds-wrap.o
 obj-$(CONFIG_LEDS_ALIX2)                += leds-alix2.o
 obj-$(CONFIG_LEDS_H1940)                += leds-h1940.o
@@ -35,6 +36,7 @@ obj-$(CONFIG_LEDS_INTEL_SS4200)		+= leds-ss4200.o
 obj-$(CONFIG_LEDS_LT3593)               += leds-lt3593.o
 obj-$(CONFIG_LEDS_ADP5520)              += leds-adp5520.o
 obj-$(CONFIG_LEDS_DELL_NETBOOKS)        += dell-led.o
+obj-$(CONFIG_LEDS_MC13783)              += leds-mc13783.o
 
 # LED SPI Drivers
 obj-$(CONFIG_LEDS_DAC124S085)           += leds-dac124s085.o
diff --git a/drivers/leds/led-class.c b/drivers/leds/led-class.c
index 69e7d86a5143..260660076507 100644
--- a/drivers/leds/led-class.c
+++ b/drivers/leds/led-class.c
@@ -74,7 +74,7 @@ static ssize_t led_max_brightness_show(struct device *dev,
 
 static struct device_attribute led_class_attrs[] = {
         __ATTR(brightness, 0644, led_brightness_show, led_brightness_store),
-        __ATTR(max_brightness, 0644, led_max_brightness_show, NULL),
+        __ATTR(max_brightness, 0444, led_max_brightness_show, NULL),
 #ifdef CONFIG_LEDS_TRIGGERS
         __ATTR(trigger, 0644, led_trigger_show, led_trigger_store),
 #endif
diff --git a/drivers/leds/leds-88pm860x.c b/drivers/leds/leds-88pm860x.c
index 16a60c06c96c..b7677106cff8 100644
--- a/drivers/leds/leds-88pm860x.c
+++ b/drivers/leds/leds-88pm860x.c
@@ -256,8 +256,10 @@ static int pm860x_led_probe(struct platform_device *pdev)
         if (pdev->dev.parent->platform_data) {
                 pm860x_pdata = pdev->dev.parent->platform_data;
                 pdata = pm860x_pdata->led;
-        } else
-                pdata = NULL;
+        } else {
+                dev_err(&pdev->dev, "missing platform data\n");
+                return -EINVAL;
+        }
 
         data = kzalloc(sizeof(struct pm860x_led), GFP_KERNEL);
         if (data == NULL)
@@ -268,8 +270,11 @@ static int pm860x_led_probe(struct platform_device *pdev)
         data->i2c = (chip->id == CHIP_PM8606) ? chip->client : chip->companion;
         data->iset = pdata->iset;
         data->port = __check_device(pdata, data->name);
-        if (data->port < 0)
+        if (data->port < 0) {
+                dev_err(&pdev->dev, "check device failed\n");
+                kfree(data);
                 return -EINVAL;
+        }
 
         data->current_brightness = 0;
         data->cdev.name = data->name;
diff --git a/drivers/leds/leds-bd2802.c b/drivers/leds/leds-bd2802.c
index 286b501a3573..5dcdf9d69b3a 100644
--- a/drivers/leds/leds-bd2802.c
+++ b/drivers/leds/leds-bd2802.c
@@ -742,7 +742,6 @@ failed_unregister_dev_file:
         for (i--; i >= 0; i--)
                 device_remove_file(&led->client->dev, bd2802_attributes[i]);
 failed_free:
-        i2c_set_clientdata(client, NULL);
         kfree(led);
 
         return ret;
@@ -759,7 +758,6 @@ static int __exit bd2802_remove(struct i2c_client *client)
                 bd2802_disable_adv_conf(led);
         for (i = 0; i < ARRAY_SIZE(bd2802_attributes); i++)
                 device_remove_file(&led->client->dev, bd2802_attributes[i]);
-        i2c_set_clientdata(client, NULL);
         kfree(led);
 
         return 0;
diff --git a/drivers/leds/leds-gpio.c b/drivers/leds/leds-gpio.c
index 6d94b0b9979c..cc22eeefa10b 100644
--- a/drivers/leds/leds-gpio.c
+++ b/drivers/leds/leds-gpio.c
@@ -26,7 +26,8 @@ struct gpio_led_data {
         u8 new_level;
         u8 can_sleep;
         u8 active_low;
-        int (*platform_gpio_blink_set)(unsigned gpio,
+        u8 blinking;
+        int (*platform_gpio_blink_set)(unsigned gpio, int state,
                         unsigned long *delay_on, unsigned long *delay_off);
 };
 
@@ -35,7 +36,13 @@ static void gpio_led_work(struct work_struct *work)
         struct gpio_led_data    *led_dat =
                 container_of(work, struct gpio_led_data, work);
 
-        gpio_set_value_cansleep(led_dat->gpio, led_dat->new_level);
+        if (led_dat->blinking) {
+                led_dat->platform_gpio_blink_set(led_dat->gpio,
+                                                 led_dat->new_level,
+                                                 NULL, NULL);
+                led_dat->blinking = 0;
+        } else
+                gpio_set_value_cansleep(led_dat->gpio, led_dat->new_level);
 }
 
 static void gpio_led_set(struct led_classdev *led_cdev,
@@ -60,8 +67,14 @@ static void gpio_led_set(struct led_classdev *led_cdev,
         if (led_dat->can_sleep) {
                 led_dat->new_level = level;
                 schedule_work(&led_dat->work);
-        } else
-                gpio_set_value(led_dat->gpio, level);
+        } else {
+                if (led_dat->blinking) {
+                        led_dat->platform_gpio_blink_set(led_dat->gpio, level,
+                                                         NULL, NULL);
+                        led_dat->blinking = 0;
+                } else
+                        gpio_set_value(led_dat->gpio, level);
+        }
 }
 
 static int gpio_blink_set(struct led_classdev *led_cdev,
@@ -70,12 +83,14 @@ static int gpio_blink_set(struct led_classdev *led_cdev,
         struct gpio_led_data *led_dat =
                 container_of(led_cdev, struct gpio_led_data, cdev);
 
-        return led_dat->platform_gpio_blink_set(led_dat->gpio, delay_on, delay_off);
+        led_dat->blinking = 1;
+        return led_dat->platform_gpio_blink_set(led_dat->gpio, GPIO_LED_BLINK,
+                                                delay_on, delay_off);
 }
 
 static int __devinit create_gpio_led(const struct gpio_led *template,
         struct gpio_led_data *led_dat, struct device *parent,
-        int (*blink_set)(unsigned, unsigned long *, unsigned long *))
+        int (*blink_set)(unsigned, int, unsigned long *, unsigned long *))
 {
         int ret, state;
 
@@ -97,6 +112,7 @@ static int __devinit create_gpio_led(const struct gpio_led *template,
         led_dat->gpio = template->gpio;
         led_dat->can_sleep = gpio_cansleep(template->gpio);
         led_dat->active_low = template->active_low;
+        led_dat->blinking = 0;
         if (blink_set) {
                 led_dat->platform_gpio_blink_set = blink_set;
                 led_dat->cdev.blink_set = gpio_blink_set;
@@ -113,7 +129,7 @@ static int __devinit create_gpio_led(const struct gpio_led *template,
         ret = gpio_direction_output(led_dat->gpio, led_dat->active_low ^ state);
         if (ret < 0)
                 goto err;
-
+                
         INIT_WORK(&led_dat->work, gpio_led_work);
 
         ret = led_classdev_register(parent, &led_dat->cdev);
diff --git a/drivers/leds/leds-lp3944.c b/drivers/leds/leds-lp3944.c
index 8d5ecceba181..9010c054615e 100644
--- a/drivers/leds/leds-lp3944.c
+++ b/drivers/leds/leds-lp3944.c
@@ -379,6 +379,7 @@ static int __devinit lp3944_probe(struct i2c_client *client,
 {
         struct lp3944_platform_data *lp3944_pdata = client->dev.platform_data;
         struct lp3944_data *data;
+        int err;
 
         if (lp3944_pdata == NULL) {
                 dev_err(&client->dev, "no platform data\n");
@@ -401,9 +402,13 @@ static int __devinit lp3944_probe(struct i2c_client *client,
 
         mutex_init(&data->lock);
 
-        dev_info(&client->dev, "lp3944 enabled\n");
+        err = lp3944_configure(client, data, lp3944_pdata);
+        if (err < 0) {
+                kfree(data);
+                return err;
+        }
 
-        lp3944_configure(client, data, lp3944_pdata);
+        dev_info(&client->dev, "lp3944 enabled\n");
         return 0;
 }
 
@@ -427,7 +432,6 @@ static int __devexit lp3944_remove(struct i2c_client *client)
                 }
 
         kfree(data);
-        i2c_set_clientdata(client, NULL);
 
         return 0;
 }
diff --git a/drivers/leds/leds-mc13783.c b/drivers/leds/leds-mc13783.c
new file mode 100644
index 000000000000..f05bb08d0f09
--- /dev/null
+++ b/drivers/leds/leds-mc13783.c
@@ -0,0 +1,403 @@
+/*
+ * LEDs driver for Freescale MC13783
+ *
+ * Copyright (C) 2010 Philippe Rétornaz
+ *
+ * Based on leds-da903x:
+ * Copyright (C) 2008 Compulab, Ltd.
+ *      Mike Rapoport <mike@compulab.co.il>
+ *
+ * Copyright (C) 2006-2008 Marvell International Ltd.
+ *      Eric Miao <eric.miao@marvell.com>
+ *
+ * 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/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/leds.h>
+#include <linux/workqueue.h>
+#include <linux/mfd/mc13783.h>
+#include <linux/slab.h>
+
+struct mc13783_led {
+        struct led_classdev     cdev;
+        struct work_struct      work;
+        struct mc13783          *master;
+        enum led_brightness     new_brightness;
+        int                     id;
+};
+
+#define MC13783_REG_LED_CONTROL_0       51
+#define MC13783_LED_C0_ENABLE_BIT       (1 << 0)
+#define MC13783_LED_C0_TRIODE_MD_BIT    (1 << 7)
+#define MC13783_LED_C0_TRIODE_AD_BIT    (1 << 8)
+#define MC13783_LED_C0_TRIODE_KP_BIT    (1 << 9)
+#define MC13783_LED_C0_BOOST_BIT        (1 << 10)
+#define MC13783_LED_C0_ABMODE_MASK      0x7
+#define MC13783_LED_C0_ABMODE           11
+#define MC13783_LED_C0_ABREF_MASK       0x3
+#define MC13783_LED_C0_ABREF            14
+
+#define MC13783_REG_LED_CONTROL_1       52
+#define MC13783_LED_C1_TC1HALF_BIT      (1 << 18)
+
+#define MC13783_REG_LED_CONTROL_2       53
+#define MC13783_LED_C2_BL_P_MASK        0xf
+#define MC13783_LED_C2_MD_P             9
+#define MC13783_LED_C2_AD_P             13
+#define MC13783_LED_C2_KP_P             17
+#define MC13783_LED_C2_BL_C_MASK        0x7
+#define MC13783_LED_C2_MD_C             0
+#define MC13783_LED_C2_AD_C             3
+#define MC13783_LED_C2_KP_C             6
+
+#define MC13783_REG_LED_CONTROL_3       54
+#define MC13783_LED_C3_TC_P             6
+#define MC13783_LED_C3_TC_P_MASK        0x1f
+
+#define MC13783_REG_LED_CONTROL_4       55
+#define MC13783_REG_LED_CONTROL_5       56
+
+#define MC13783_LED_Cx_PERIOD           21
+#define MC13783_LED_Cx_PERIOD_MASK      0x3
+#define MC13783_LED_Cx_SLEWLIM_BIT      (1 << 23)
+#define MC13783_LED_Cx_TRIODE_TC_BIT    (1 << 23)
+#define MC13783_LED_Cx_TC_C_MASK        0x3
+
+static void mc13783_led_work(struct work_struct *work)
+{
+        struct mc13783_led *led = container_of(work, struct mc13783_led, work);
+        int reg = 0;
+        int mask = 0;
+        int value = 0;
+        int bank, off, shift;
+
+        switch (led->id) {
+        case MC13783_LED_MD:
+                reg = MC13783_REG_LED_CONTROL_2;
+                mask = MC13783_LED_C2_BL_P_MASK << MC13783_LED_C2_MD_P;
+                value = (led->new_brightness >> 4) << MC13783_LED_C2_MD_P;
+                break;
+        case MC13783_LED_AD:
+                reg = MC13783_REG_LED_CONTROL_2;
+                mask = MC13783_LED_C2_BL_P_MASK << MC13783_LED_C2_AD_P;
+                value = (led->new_brightness >> 4) << MC13783_LED_C2_AD_P;
+                break;
+        case MC13783_LED_KP:
+                reg = MC13783_REG_LED_CONTROL_2;
+                mask = MC13783_LED_C2_BL_P_MASK << MC13783_LED_C2_KP_P;
+                value = (led->new_brightness >> 4) << MC13783_LED_C2_KP_P;
+                break;
+        case MC13783_LED_R1:
+        case MC13783_LED_G1:
+        case MC13783_LED_B1:
+        case MC13783_LED_R2:
+        case MC13783_LED_G2:
+        case MC13783_LED_B2:
+        case MC13783_LED_R3:
+        case MC13783_LED_G3:
+        case MC13783_LED_B3:
+                off = led->id - MC13783_LED_R1;
+                bank = off/3;
+                reg = MC13783_REG_LED_CONTROL_3 + off/3;
+                shift = (off - bank * 3) * 5 + MC13783_LED_C3_TC_P;
+                value = (led->new_brightness >> 3) << shift;
+                mask = MC13783_LED_C3_TC_P_MASK << shift;
+                break;
+        }
+
+        mc13783_lock(led->master);
+
+        mc13783_reg_rmw(led->master, reg, mask, value);
+
+        mc13783_unlock(led->master);
+}
+
+static void mc13783_led_set(struct led_classdev *led_cdev,
+                           enum led_brightness value)
+{
+        struct mc13783_led *led;
+
+        led = container_of(led_cdev, struct mc13783_led, cdev);
+        led->new_brightness = value;
+        schedule_work(&led->work);
+}
+
+static int __devinit mc13783_led_setup(struct mc13783_led *led, int max_current)
+{
+        int shift = 0;
+        int mask = 0;
+        int value = 0;
+        int reg = 0;
+        int ret, bank;
+
+        switch (led->id) {
+        case MC13783_LED_MD:
+                shift = MC13783_LED_C2_MD_C;
+                mask = MC13783_LED_C2_BL_C_MASK;
+                value = max_current & MC13783_LED_C2_BL_C_MASK;
+                reg = MC13783_REG_LED_CONTROL_2;
+                break;
+        case MC13783_LED_AD:
+                shift = MC13783_LED_C2_AD_C;
+                mask = MC13783_LED_C2_BL_C_MASK;
+                value = max_current & MC13783_LED_C2_BL_C_MASK;
+                reg = MC13783_REG_LED_CONTROL_2;
+                break;
+        case MC13783_LED_KP:
+                shift = MC13783_LED_C2_KP_C;
+                mask = MC13783_LED_C2_BL_C_MASK;
+                value = max_current & MC13783_LED_C2_BL_C_MASK;
+                reg = MC13783_REG_LED_CONTROL_2;
+                break;
+        case MC13783_LED_R1:
+        case MC13783_LED_G1:
+        case MC13783_LED_B1:
+        case MC13783_LED_R2:
+        case MC13783_LED_G2:
+        case MC13783_LED_B2:
+        case MC13783_LED_R3:
+        case MC13783_LED_G3:
+        case MC13783_LED_B3:
+                bank = (led->id - MC13783_LED_R1)/3;
+                reg = MC13783_REG_LED_CONTROL_3 + bank;
+                shift = ((led->id - MC13783_LED_R1) - bank * 3) * 2;
+                mask = MC13783_LED_Cx_TC_C_MASK;
+                value = max_current & MC13783_LED_Cx_TC_C_MASK;
+                break;
+        }
+
+        mc13783_lock(led->master);
+
+        ret = mc13783_reg_rmw(led->master, reg, mask << shift,
+                                                value << shift);
+
+        mc13783_unlock(led->master);
+        return ret;
+}
+
+static int __devinit mc13783_leds_prepare(struct platform_device *pdev)
+{
+        struct mc13783_leds_platform_data *pdata = dev_get_platdata(&pdev->dev);
+        struct mc13783 *dev = dev_get_drvdata(pdev->dev.parent);
+        int ret = 0;
+        int reg = 0;
+
+        mc13783_lock(dev);
+
+        if (pdata->flags & MC13783_LED_TC1HALF)
+                reg |= MC13783_LED_C1_TC1HALF_BIT;
+
+        if (pdata->flags & MC13783_LED_SLEWLIMTC)
+                reg |= MC13783_LED_Cx_SLEWLIM_BIT;
+
+        ret = mc13783_reg_write(dev, MC13783_REG_LED_CONTROL_1, reg);
+        if (ret)
+                goto out;
+
+        reg = (pdata->bl_period & MC13783_LED_Cx_PERIOD_MASK) <<
+                                                        MC13783_LED_Cx_PERIOD;
+
+        if (pdata->flags & MC13783_LED_SLEWLIMBL)
+                reg |= MC13783_LED_Cx_SLEWLIM_BIT;
+
+        ret = mc13783_reg_write(dev, MC13783_REG_LED_CONTROL_2, reg);
+        if (ret)
+                goto out;
+
+        reg = (pdata->tc1_period & MC13783_LED_Cx_PERIOD_MASK) <<
+                                                        MC13783_LED_Cx_PERIOD;
+
+        if (pdata->flags & MC13783_LED_TRIODE_TC1)
+                reg |= MC13783_LED_Cx_TRIODE_TC_BIT;
+
+        ret = mc13783_reg_write(dev, MC13783_REG_LED_CONTROL_3, reg);
+        if (ret)
+                goto out;
+
+        reg = (pdata->tc2_period & MC13783_LED_Cx_PERIOD_MASK) <<
+                                                        MC13783_LED_Cx_PERIOD;
+
+        if (pdata->flags & MC13783_LED_TRIODE_TC2)
+                reg |= MC13783_LED_Cx_TRIODE_TC_BIT;
+
+        ret = mc13783_reg_write(dev, MC13783_REG_LED_CONTROL_4, reg);
+        if (ret)
+                goto out;
+
+        reg = (pdata->tc3_period & MC13783_LED_Cx_PERIOD_MASK) <<
+                                                        MC13783_LED_Cx_PERIOD;
+
+        if (pdata->flags & MC13783_LED_TRIODE_TC3)
+                reg |= MC13783_LED_Cx_TRIODE_TC_BIT;;
+
+        ret = mc13783_reg_write(dev, MC13783_REG_LED_CONTROL_5, reg);
+        if (ret)
+                goto out;
+
+        reg = MC13783_LED_C0_ENABLE_BIT;
+        if (pdata->flags & MC13783_LED_TRIODE_MD)
+                reg |= MC13783_LED_C0_TRIODE_MD_BIT;
+        if (pdata->flags & MC13783_LED_TRIODE_AD)
+                reg |= MC13783_LED_C0_TRIODE_AD_BIT;
+        if (pdata->flags & MC13783_LED_TRIODE_KP)
+                reg |= MC13783_LED_C0_TRIODE_KP_BIT;
+        if (pdata->flags & MC13783_LED_BOOST_EN)
+                reg |= MC13783_LED_C0_BOOST_BIT;
+
+        reg |= (pdata->abmode & MC13783_LED_C0_ABMODE_MASK) <<
+                                                        MC13783_LED_C0_ABMODE;
+        reg |= (pdata->abref & MC13783_LED_C0_ABREF_MASK) <<
+                                                        MC13783_LED_C0_ABREF;
+
+        ret = mc13783_reg_write(dev, MC13783_REG_LED_CONTROL_0, reg);
+
+out:
+        mc13783_unlock(dev);
+        return ret;
+}
+
+static int __devinit mc13783_led_probe(struct platform_device *pdev)
+{
+        struct mc13783_leds_platform_data *pdata = dev_get_platdata(&pdev->dev);
+        struct mc13783_led_platform_data *led_cur;
+        struct mc13783_led *led, *led_dat;
+        int ret, i;
+        int init_led = 0;
+
+        if (pdata == NULL) {
+                dev_err(&pdev->dev, "missing platform data\n");
+                return -ENODEV;
+        }
+
+        if (pdata->num_leds < 1 || pdata->num_leds > MC13783_LED_MAX) {
+                dev_err(&pdev->dev, "Invalid led count %d\n", pdata->num_leds);
+                return -EINVAL;
+        }
+
+        led = kzalloc(sizeof(*led) * pdata->num_leds, GFP_KERNEL);
+        if (led == NULL) {
+                dev_err(&pdev->dev, "failed to alloc memory\n");
+                return -ENOMEM;
+        }
+
+        ret = mc13783_leds_prepare(pdev);
+        if (ret) {
+                dev_err(&pdev->dev, "unable to init led driver\n");
+                goto err_free;
+        }
+
+        for (i = 0; i < pdata->num_leds; i++) {
+                led_dat = &led[i];
+                led_cur = &pdata->led[i];
+
+                if (led_cur->id > MC13783_LED_MAX || led_cur->id < 0) {
+                        dev_err(&pdev->dev, "invalid id %d\n", led_cur->id);
+                        ret = -EINVAL;
+                        goto err_register;
+                }
+
+                if (init_led & (1 << led_cur->id)) {
+                        dev_err(&pdev->dev, "led %d already initialized\n",
+                                        led_cur->id);
+                        ret = -EINVAL;
+                        goto err_register;
+                }
+
+                init_led |= 1 << led_cur->id;
+                led_dat->cdev.name = led_cur->name;
+                led_dat->cdev.default_trigger = led_cur->default_trigger;
+                led_dat->cdev.brightness_set = mc13783_led_set;
+                led_dat->cdev.brightness = LED_OFF;
+                led_dat->id = led_cur->id;
+                led_dat->master = dev_get_drvdata(pdev->dev.parent);
+
+                INIT_WORK(&led_dat->work, mc13783_led_work);
+
+                ret = led_classdev_register(pdev->dev.parent, &led_dat->cdev);
+                if (ret) {
+                        dev_err(&pdev->dev, "failed to register led %d\n",
+                                        led_dat->id);
+                        goto err_register;
+                }
+
+                ret = mc13783_led_setup(led_dat, led_cur->max_current);
+                if (ret) {
+                        dev_err(&pdev->dev, "unable to init led %d\n",
+                                        led_dat->id);
+                        i++;
+                        goto err_register;
+                }
+        }
+
+        platform_set_drvdata(pdev, led);
+        return 0;
+
+err_register:
+        for (i = i - 1; i >= 0; i--) {
+                led_classdev_unregister(&led[i].cdev);
+                cancel_work_sync(&led[i].work);
+        }
+
+err_free:
+        kfree(led);
+        return ret;
+}
+
+static int __devexit mc13783_led_remove(struct platform_device *pdev)
+{
+        struct mc13783_leds_platform_data *pdata = dev_get_platdata(&pdev->dev);
+        struct mc13783_led *led = platform_get_drvdata(pdev);
+        struct mc13783 *dev = dev_get_drvdata(pdev->dev.parent);
+        int i;
+
+        for (i = 0; i < pdata->num_leds; i++) {
+                led_classdev_unregister(&led[i].cdev);
+                cancel_work_sync(&led[i].work);
+        }
+
+        mc13783_lock(dev);
+
+        mc13783_reg_write(dev, MC13783_REG_LED_CONTROL_0, 0);
+        mc13783_reg_write(dev, MC13783_REG_LED_CONTROL_1, 0);
+        mc13783_reg_write(dev, MC13783_REG_LED_CONTROL_2, 0);
+        mc13783_reg_write(dev, MC13783_REG_LED_CONTROL_3, 0);
+        mc13783_reg_write(dev, MC13783_REG_LED_CONTROL_4, 0);
+        mc13783_reg_write(dev, MC13783_REG_LED_CONTROL_5, 0);
+
+        mc13783_unlock(dev);
+
+        kfree(led);
+        return 0;
+}
+
+static struct platform_driver mc13783_led_driver = {
+        .driver = {
+                .name   = "mc13783-led",
+                .owner  = THIS_MODULE,
+        },
+        .probe          = mc13783_led_probe,
+        .remove         = __devexit_p(mc13783_led_remove),
+};
+
+static int __init mc13783_led_init(void)
+{
+        return platform_driver_register(&mc13783_led_driver);
+}
+module_init(mc13783_led_init);
+
+static void __exit mc13783_led_exit(void)
+{
+        platform_driver_unregister(&mc13783_led_driver);
+}
+module_exit(mc13783_led_exit);
+
+MODULE_DESCRIPTION("LEDs driver for Freescale MC13783 PMIC");
+MODULE_AUTHOR("Philippe Retornaz <philippe.retornaz@epfl.ch>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:mc13783-led");
diff --git a/drivers/leds/leds-net5501.c b/drivers/leds/leds-net5501.c
new file mode 100644
index 000000000000..3063f591f0dc
--- /dev/null
+++ b/drivers/leds/leds-net5501.c
@@ -0,0 +1,94 @@
+/*
+ * Soekris board support code
+ *
+ * Copyright (C) 2008-2009 Tower Technologies
+ * Written by Alessandro Zummo <a.zummo@towertech.it>
+ *
+ * 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/init.h>
+#include <linux/io.h>
+#include <linux/string.h>
+#include <linux/leds.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+
+#include <asm/geode.h>
+
+static struct gpio_led net5501_leds[] = {
+        {
+                .name = "error",
+                .gpio = 6,
+                .default_trigger = "default-on",
+        },
+};
+
+static struct gpio_led_platform_data net5501_leds_data = {
+        .num_leds = ARRAY_SIZE(net5501_leds),
+        .leds = net5501_leds,
+};
+
+static struct platform_device net5501_leds_dev = {
+        .name = "leds-gpio",
+        .id = -1,
+        .dev.platform_data = &net5501_leds_data,
+};
+
+static void __init init_net5501(void)
+{
+        platform_device_register(&net5501_leds_dev);
+}
+
+struct soekris_board {
+        u16     offset;
+        char    *sig;
+        u8      len;
+        void    (*init)(void);
+};
+
+static struct soekris_board __initdata boards[] = {
+        { 0xb7b, "net5501", 7, init_net5501 },  /* net5501 v1.33/1.33c */
+        { 0xb1f, "net5501", 7, init_net5501 },  /* net5501 v1.32i */
+};
+
+static int __init soekris_init(void)
+{
+        int i;
+        unsigned char *rombase, *bios;
+
+        if (!is_geode())
+                return 0;
+
+        rombase = ioremap(0xffff0000, 0xffff);
+        if (!rombase) {
+                printk(KERN_INFO "Soekris net5501 LED driver failed to get rombase");
+                return 0;
+        }
+
+        bios = rombase + 0x20;  /* null terminated */
+
+        if (strncmp(bios, "comBIOS", 7))
+                goto unmap;
+
+        for (i = 0; i < ARRAY_SIZE(boards); i++) {
+                unsigned char *model = rombase + boards[i].offset;
+
+                if (strncmp(model, boards[i].sig, boards[i].len) == 0) {
+                        printk(KERN_INFO "Soekris %s: %s\n", model, bios);
+
+                        if (boards[i].init)
+                                boards[i].init();
+                        break;
+                }
+        }
+
+unmap:
+        iounmap(rombase);
+        return 0;
+}
+
+arch_initcall(soekris_init);
diff --git a/drivers/leds/leds-pca9532.c b/drivers/leds/leds-pca9532.c
index 6682175fa9f7..43d08756d823 100644
--- a/drivers/leds/leds-pca9532.c
+++ b/drivers/leds/leds-pca9532.c
@@ -320,10 +320,8 @@ static int pca9532_probe(struct i2c_client *client,
         mutex_init(&data->update_lock);
 
         err = pca9532_configure(client, data, pca9532_pdata);
-        if (err) {
+        if (err)
                 kfree(data);
-                i2c_set_clientdata(client, NULL);
-        }
 
         return err;
 }
@@ -351,7 +349,6 @@ static int pca9532_remove(struct i2c_client *client)
                 }
 
         kfree(data);
-        i2c_set_clientdata(client, NULL);
         return 0;
 }
 
diff --git a/drivers/leds/leds-pca955x.c b/drivers/leds/leds-pca955x.c
index 8ff50f234190..66aa3e8e786f 100644
--- a/drivers/leds/leds-pca955x.c
+++ b/drivers/leds/leds-pca955x.c
@@ -342,7 +342,6 @@ exit:
         }
 
         kfree(pca955x);
-        i2c_set_clientdata(client, NULL);
 
         return err;
 }
@@ -358,7 +357,6 @@ static int __devexit pca955x_remove(struct i2c_client *client)
         }
 
         kfree(pca955x);
-        i2c_set_clientdata(client, NULL);
 
         return 0;
 }
diff --git a/drivers/leds/leds-ss4200.c b/drivers/leds/leds-ss4200.c
index 51477ec71391..a688293abd0b 100644
--- a/drivers/leds/leds-ss4200.c
+++ b/drivers/leds/leds-ss4200.c
@@ -534,7 +534,7 @@ static int __init nas_gpio_init(void)
         set_power_light_amber_noblink();
         return 0;
 out_err:
-        for (; i >= 0; i--)
+        for (i--; i >= 0; i--)
                 unregister_nasgpio_led(i);
         pci_unregister_driver(&nas_gpio_pci_driver);
         return ret;
diff --git a/drivers/macintosh/macio_asic.c b/drivers/macintosh/macio_asic.c
index 97147804a49c..b6e7ddc09d76 100644
--- a/drivers/macintosh/macio_asic.c
+++ b/drivers/macintosh/macio_asic.c
@@ -492,8 +492,8 @@ static void macio_pci_add_devices(struct macio_chip *chip)
         }
 
         /* Add media bay devices if any */
-        pnode = mbdev->ofdev.dev.of_node;
-        if (mbdev)
+        if (mbdev) {
+                pnode = mbdev->ofdev.dev.of_node;
                 for (np = NULL; (np = of_get_next_child(pnode, np)) != NULL;) {
                         if (macio_skip_device(np))
                                 continue;
@@ -502,10 +502,11 @@ static void macio_pci_add_devices(struct macio_chip *chip)
                                                  mbdev,  root_res) == NULL)
                                 of_node_put(np);
                 }
+        }
 
         /* Add serial ports if any */
-        pnode = sdev->ofdev.dev.of_node;
         if (sdev) {
+                pnode = sdev->ofdev.dev.of_node;
                 for (np = NULL; (np = of_get_next_child(pnode, np)) != NULL;) {
                         if (macio_skip_device(np))
                                 continue;
@@ -525,7 +526,6 @@ static void macio_pci_add_devices(struct macio_chip *chip)
 int macio_register_driver(struct macio_driver *drv)
 {
         /* initialize common driver fields */
-        drv->driver.name = drv->name;
         drv->driver.bus = &macio_bus_type;
 
         /* register with core */
diff --git a/drivers/macintosh/mediabay.c b/drivers/macintosh/mediabay.c
index 288acce76b74..2fd435bc542e 100644
--- a/drivers/macintosh/mediabay.c
+++ b/drivers/macintosh/mediabay.c
@@ -728,8 +728,10 @@ static struct of_device_id media_bay_match[] =
 
 static struct macio_driver media_bay_driver =
 {
-        .name           = "media-bay",
-        .match_table    = media_bay_match,
+        .driver = {
+                .name           = "media-bay",
+                .of_match_table = media_bay_match,
+        },
         .probe          = media_bay_attach,
         .suspend        = media_bay_suspend,
         .resume         = media_bay_resume
diff --git a/drivers/macintosh/rack-meter.c b/drivers/macintosh/rack-meter.c
index 12946c5f583f..53cce3a5da23 100644
--- a/drivers/macintosh/rack-meter.c
+++ b/drivers/macintosh/rack-meter.c
@@ -584,9 +584,11 @@ static struct of_device_id rackmeter_match[] = {
 };
 
 static struct macio_driver rackmeter_driver = {
-        .name = "rackmeter",
-        .owner = THIS_MODULE,
-        .match_table = rackmeter_match,
+        .driver = {
+                .name = "rackmeter",
+                .owner = THIS_MODULE,
+                .of_match_table = rackmeter_match,
+        },
         .probe = rackmeter_probe,
         .remove = __devexit_p(rackmeter_remove),
         .shutdown = rackmeter_shutdown,
diff --git a/drivers/macintosh/therm_adt746x.c b/drivers/macintosh/therm_adt746x.c
index 16d82f17ae82..c42eeb43042d 100644
--- a/drivers/macintosh/therm_adt746x.c
+++ b/drivers/macintosh/therm_adt746x.c
@@ -182,7 +182,6 @@ remove_thermostat(struct i2c_client *client)
 
         thermostat = NULL;
 
-        i2c_set_clientdata(client, NULL);
         kfree(th);
 
         return 0;
@@ -400,7 +399,6 @@ static int probe_thermostat(struct i2c_client *client,
         rc = read_reg(th, CONFIG_REG);
         if (rc < 0) {
                 dev_err(&client->dev, "Thermostat failed to read config!\n");
-                i2c_set_clientdata(client, NULL);
                 kfree(th);
                 return -ENODEV;
         }
diff --git a/drivers/macintosh/windfarm_lm75_sensor.c b/drivers/macintosh/windfarm_lm75_sensor.c
index d8257d35afde..647c6add2193 100644
--- a/drivers/macintosh/windfarm_lm75_sensor.c
+++ b/drivers/macintosh/windfarm_lm75_sensor.c
@@ -107,10 +107,8 @@ static int wf_lm75_probe(struct i2c_client *client,
         i2c_set_clientdata(client, lm);
 
         rc = wf_register_sensor(&lm->sens);
-        if (rc) {
-                i2c_set_clientdata(client, NULL);
+        if (rc)
                 kfree(lm);
-        }
 
         return rc;
 }
@@ -216,7 +214,6 @@ static int wf_lm75_remove(struct i2c_client *client)
         /* release sensor */
         wf_unregister_sensor(&lm->sens);
 
-        i2c_set_clientdata(client, NULL);
         return 0;
 }
 
diff --git a/drivers/macintosh/windfarm_max6690_sensor.c b/drivers/macintosh/windfarm_max6690_sensor.c
index b486eb929fde..8204113268f4 100644
--- a/drivers/macintosh/windfarm_max6690_sensor.c
+++ b/drivers/macintosh/windfarm_max6690_sensor.c
@@ -81,7 +81,6 @@ static int wf_max6690_probe(struct i2c_client *client,
 
         rc = wf_register_sensor(&max->sens);
         if (rc) {
-                i2c_set_clientdata(client, NULL);
                 kfree(max);
         }
 
diff --git a/drivers/macintosh/windfarm_smu_sat.c b/drivers/macintosh/windfarm_smu_sat.c
index e20330a28959..65a8ff3e1f8e 100644
--- a/drivers/macintosh/windfarm_smu_sat.c
+++ b/drivers/macintosh/windfarm_smu_sat.c
@@ -376,7 +376,6 @@ static int wf_sat_remove(struct i2c_client *client)
         /* XXX TODO */
 
         sat->i2c = NULL;
-        i2c_set_clientdata(client, NULL);
         return 0;
 }
 
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index 9ea17d6c799b..d2c0f94fa37d 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -4645,7 +4645,7 @@ static int raid456_cpu_notify(struct notifier_block *nfb, unsigned long action,
                         kfree(percpu->scribble);
                         pr_err("%s: failed memory allocation for cpu%ld\n",
                                __func__, cpu);
-                        return NOTIFY_BAD;
+                        return notifier_from_errno(-ENOMEM);
                 }
                 break;
         case CPU_DEAD:
diff --git a/drivers/media/IR/Kconfig b/drivers/media/IR/Kconfig
index 195c6cf359f6..d22a8ec523fc 100644
--- a/drivers/media/IR/Kconfig
+++ b/drivers/media/IR/Kconfig
@@ -13,6 +13,7 @@ source "drivers/media/IR/keymaps/Kconfig"
 config IR_NEC_DECODER
         tristate "Enable IR raw decoder for the NEC protocol"
         depends on IR_CORE
+        select BITREVERSE
         default y
 
         ---help---
@@ -22,6 +23,7 @@ config IR_NEC_DECODER
 config IR_RC5_DECODER
         tristate "Enable IR raw decoder for the RC-5 protocol"
         depends on IR_CORE
+        select BITREVERSE
         default y
 
         ---help---
diff --git a/drivers/media/IR/imon.c b/drivers/media/IR/imon.c
index 5e2045670004..4bbd45f4284c 100644
--- a/drivers/media/IR/imon.c
+++ b/drivers/media/IR/imon.c
@@ -94,6 +94,7 @@ struct imon_context {
 
         bool display_supported;         /* not all controllers do */
         bool display_isopen;            /* display port has been opened */
+        bool rf_device;                 /* true if iMON 2.4G LT/DT RF device */
         bool rf_isassociating;          /* RF remote associating */
         bool dev_present_intf0;         /* USB device presence, interface 0 */
         bool dev_present_intf1;         /* USB device presence, interface 1 */
@@ -385,7 +386,7 @@ static int display_open(struct inode *inode, struct file *file)
                 err("%s: display port is already open", __func__);
                 retval = -EBUSY;
         } else {
-                ictx->display_isopen = 1;
+                ictx->display_isopen = true;
                 file->private_data = ictx;
                 dev_dbg(ictx->dev, "display port opened\n");
         }
@@ -422,7 +423,7 @@ static int display_close(struct inode *inode, struct file *file)
                 err("%s: display is not open", __func__);
                 retval = -EIO;
         } else {
-                ictx->display_isopen = 0;
+                ictx->display_isopen = false;
                 dev_dbg(ictx->dev, "display port closed\n");
                 if (!ictx->dev_present_intf0) {
                         /*
@@ -491,12 +492,12 @@ static int send_packet(struct imon_context *ictx)
         }
 
         init_completion(&ictx->tx.finished);
-        ictx->tx.busy = 1;
+        ictx->tx.busy = true;
         smp_rmb(); /* ensure later readers know we're busy */
 
         retval = usb_submit_urb(ictx->tx_urb, GFP_KERNEL);
         if (retval) {
-                ictx->tx.busy = 0;
+                ictx->tx.busy = false;
                 smp_rmb(); /* ensure later readers know we're not busy */
                 err("%s: error submitting urb(%d)", __func__, retval);
         } else {
@@ -682,7 +683,7 @@ static ssize_t store_associate_remote(struct device *d,
                 return -ENODEV;
 
         mutex_lock(&ictx->lock);
-        ictx->rf_isassociating = 1;
+        ictx->rf_isassociating = true;
         send_associate_24g(ictx);
         mutex_unlock(&ictx->lock);
 
@@ -950,7 +951,7 @@ static void usb_tx_callback(struct urb *urb)
         ictx->tx.status = urb->status;
 
         /* notify waiters that write has finished */
-        ictx->tx.busy = 0;
+        ictx->tx.busy = false;
         smp_rmb(); /* ensure later readers know we're not busy */
         complete(&ictx->tx.finished);
 }
@@ -1215,7 +1216,7 @@ static bool imon_mouse_event(struct imon_context *ictx,
 {
         char rel_x = 0x00, rel_y = 0x00;
         u8 right_shift = 1;
-        bool mouse_input = 1;
+        bool mouse_input = true;
         int dir = 0;
 
         /* newer iMON device PAD or mouse button */
@@ -1246,7 +1247,7 @@ static bool imon_mouse_event(struct imon_context *ictx,
         } else if (ictx->kc == KEY_CHANNELDOWN && (buf[2] & 0x40) != 0x40) {
                 dir = -1;
         } else
-                mouse_input = 0;
+                mouse_input = false;
 
         if (mouse_input) {
                 dev_dbg(ictx->dev, "sending mouse data via input subsystem\n");
@@ -1450,7 +1451,7 @@ static void imon_incoming_packet(struct imon_context *ictx,
         unsigned char *buf = urb->transfer_buffer;
         struct device *dev = ictx->dev;
         u32 kc;
-        bool norelease = 0;
+        bool norelease = false;
         int i;
         u64 temp_key;
         u64 panel_key = 0;
@@ -1465,7 +1466,7 @@ static void imon_incoming_packet(struct imon_context *ictx,
         idev = ictx->idev;
 
         /* filter out junk data on the older 0xffdc imon devices */
-        if ((buf[0] == 0xff) && (buf[7] == 0xff))
+        if ((buf[0] == 0xff) && (buf[1] == 0xff) && (buf[2] == 0xff))
                 return;
 
         /* Figure out what key was pressed */
@@ -1517,7 +1518,7 @@ static void imon_incoming_packet(struct imon_context *ictx,
              !(buf[1] & 0x1 || buf[1] >> 2 & 0x1))) {
                 len = 8;
                 imon_pad_to_keys(ictx, buf);
-                norelease = 1;
+                norelease = true;
         }
 
         if (debug) {
@@ -1580,7 +1581,7 @@ not_input_data:
             (buf[6] == 0x5E && buf[7] == 0xDF))) {      /* DT */
                 dev_warn(dev, "%s: remote associated refid=%02X\n",
                          __func__, buf[1]);
-                ictx->rf_isassociating = 0;
+                ictx->rf_isassociating = false;
         }
 }
 
@@ -1790,9 +1791,9 @@ static bool imon_find_endpoints(struct imon_context *ictx,
         int ifnum = iface_desc->desc.bInterfaceNumber;
         int num_endpts = iface_desc->desc.bNumEndpoints;
         int i, ep_dir, ep_type;
-        bool ir_ep_found = 0;
-        bool display_ep_found = 0;
-        bool tx_control = 0;
+        bool ir_ep_found = false;
+        bool display_ep_found = false;
+        bool tx_control = false;
 
         /*
          * Scan the endpoint list and set:
@@ -1808,13 +1809,13 @@ static bool imon_find_endpoints(struct imon_context *ictx,
                     ep_type == USB_ENDPOINT_XFER_INT) {
 
                         rx_endpoint = ep;
-                        ir_ep_found = 1;
+                        ir_ep_found = true;
                         dev_dbg(ictx->dev, "%s: found IR endpoint\n", __func__);
 
                 } else if (!display_ep_found && ep_dir == USB_DIR_OUT &&
                            ep_type == USB_ENDPOINT_XFER_INT) {
                         tx_endpoint = ep;
-                        display_ep_found = 1;
+                        display_ep_found = true;
                         dev_dbg(ictx->dev, "%s: found display endpoint\n", __func__);
                 }
         }
@@ -1835,8 +1836,8 @@ static bool imon_find_endpoints(struct imon_context *ictx,
          * newer iMON devices that use control urb instead of interrupt
          */
         if (!display_ep_found) {
-                tx_control = 1;
-                display_ep_found = 1;
+                tx_control = true;
+                display_ep_found = true;
                 dev_dbg(ictx->dev, "%s: device uses control endpoint, not "
                         "interface OUT endpoint\n", __func__);
         }
@@ -1847,7 +1848,7 @@ static bool imon_find_endpoints(struct imon_context *ictx,
          * and without... :\
          */
         if (ictx->display_type == IMON_DISPLAY_TYPE_NONE) {
-                display_ep_found = 0;
+                display_ep_found = false;
                 dev_dbg(ictx->dev, "%s: device has no display\n", __func__);
         }
 
@@ -1856,7 +1857,7 @@ static bool imon_find_endpoints(struct imon_context *ictx,
          * that refers to e.g. /dev/lcd0 (a character device LCD or VFD).
          */
         if (ictx->display_type == IMON_DISPLAY_TYPE_VGA) {
-                display_ep_found = 0;
+                display_ep_found = false;
                 dev_dbg(ictx->dev, "%s: iMON Touch device found\n", __func__);
         }
 
@@ -1905,9 +1906,10 @@ static struct imon_context *imon_init_intf0(struct usb_interface *intf)
 
         ictx->dev = dev;
         ictx->usbdev_intf0 = usb_get_dev(interface_to_usbdev(intf));
-        ictx->dev_present_intf0 = 1;
+        ictx->dev_present_intf0 = true;
         ictx->rx_urb_intf0 = rx_urb;
         ictx->tx_urb = tx_urb;
+        ictx->rf_device = false;
 
         ictx->vendor  = le16_to_cpu(ictx->usbdev_intf0->descriptor.idVendor);
         ictx->product = le16_to_cpu(ictx->usbdev_intf0->descriptor.idProduct);
@@ -1979,7 +1981,7 @@ static struct imon_context *imon_init_intf1(struct usb_interface *intf,
         }
 
         ictx->usbdev_intf1 = usb_get_dev(interface_to_usbdev(intf));
-        ictx->dev_present_intf1 = 1;
+        ictx->dev_present_intf1 = true;
         ictx->rx_urb_intf1 = rx_urb;
 
         ret = -ENODEV;
@@ -2047,6 +2049,12 @@ static void imon_get_ffdc_type(struct imon_context *ictx)
                 dev_info(ictx->dev, "0xffdc iMON Knob, iMON IR");
                 ictx->display_supported = false;
                 break;
+        /* iMON 2.4G LT (usb stick), no display, iMON RF */
+        case 0x4e:
+                dev_info(ictx->dev, "0xffdc iMON 2.4G LT, iMON RF");
+                ictx->display_supported = false;
+                ictx->rf_device = true;
+                break;
         /* iMON VFD, no IR (does have vol knob tho) */
         case 0x35:
                 dev_info(ictx->dev, "0xffdc iMON VFD + knob, no IR");
@@ -2197,15 +2205,6 @@ static int __devinit imon_probe(struct usb_interface *interface,
                         goto fail;
                 }
 
-                if (product == 0xffdc) {
-                        /* RF products *also* use 0xffdc... sigh... */
-                        sysfs_err = sysfs_create_group(&interface->dev.kobj,
-                                                       &imon_rf_attribute_group);
-                        if (sysfs_err)
-                                err("%s: Could not create RF sysfs entries(%d)",
-                                    __func__, sysfs_err);
-                }
-
         } else {
         /* this is the secondary interface on the device */
                 ictx = imon_init_intf1(interface, first_if_ctx);
@@ -2233,6 +2232,14 @@ static int __devinit imon_probe(struct usb_interface *interface,
 
                 imon_set_display_type(ictx, interface);
 
+                if (product == 0xffdc && ictx->rf_device) {
+                        sysfs_err = sysfs_create_group(&interface->dev.kobj,
+                                                       &imon_rf_attribute_group);
+                        if (sysfs_err)
+                                err("%s: Could not create RF sysfs entries(%d)",
+                                    __func__, sysfs_err);
+                }
+
                 if (ictx->display_supported)
                         imon_init_display(ictx, interface);
         }
@@ -2297,7 +2304,7 @@ static void __devexit imon_disconnect(struct usb_interface *interface)
         }
 
         if (ifnum == 0) {
-                ictx->dev_present_intf0 = 0;
+                ictx->dev_present_intf0 = false;
                 usb_kill_urb(ictx->rx_urb_intf0);
                 input_unregister_device(ictx->idev);
                 if (ictx->display_supported) {
@@ -2307,7 +2314,7 @@ static void __devexit imon_disconnect(struct usb_interface *interface)
                                 usb_deregister_dev(interface, &imon_vfd_class);
                 }
         } else {
-                ictx->dev_present_intf1 = 0;
+                ictx->dev_present_intf1 = false;
                 usb_kill_urb(ictx->rx_urb_intf1);
                 if (ictx->display_type == IMON_DISPLAY_TYPE_VGA)
                         input_unregister_device(ictx->touch);
diff --git a/drivers/media/IR/ir-keytable.c b/drivers/media/IR/ir-keytable.c
index 9374a006f43d..94a8577e72eb 100644
--- a/drivers/media/IR/ir-keytable.c
+++ b/drivers/media/IR/ir-keytable.c
@@ -490,11 +490,12 @@ int __ir_input_register(struct input_dev *input_dev,
         if (rc < 0)
                 goto out_table;
 
-        if (ir_dev->props->driver_type == RC_DRIVER_IR_RAW) {
-                rc = ir_raw_event_register(input_dev);
-                if (rc < 0)
-                        goto out_event;
-        }
+        if (ir_dev->props)
+                if (ir_dev->props->driver_type == RC_DRIVER_IR_RAW) {
+                        rc = ir_raw_event_register(input_dev);
+                        if (rc < 0)
+                                goto out_event;
+                }
 
         IR_dprintk(1, "Registered input device on %s for %s remote.\n",
                    driver_name, rc_tab->name);
@@ -530,8 +531,10 @@ void ir_input_unregister(struct input_dev *input_dev)
         IR_dprintk(1, "Freed keycode table\n");
 
         del_timer_sync(&ir_dev->timer_keyup);
-        if (ir_dev->props->driver_type == RC_DRIVER_IR_RAW)
-                ir_raw_event_unregister(input_dev);
+        if (ir_dev->props)
+                if (ir_dev->props->driver_type == RC_DRIVER_IR_RAW)
+                        ir_raw_event_unregister(input_dev);
+
         rc_tab = &ir_dev->rc_tab;
         rc_tab->size = 0;
         kfree(rc_tab->scan);
diff --git a/drivers/media/IR/ir-sysfs.c b/drivers/media/IR/ir-sysfs.c
index d7da63e16c92..2098dd1488e0 100644
--- a/drivers/media/IR/ir-sysfs.c
+++ b/drivers/media/IR/ir-sysfs.c
@@ -221,9 +221,10 @@ int ir_register_class(struct input_dev *input_dev)
         if (unlikely(devno < 0))
                 return devno;
 
-        if (ir_dev->props->driver_type == RC_DRIVER_SCANCODE)
-                ir_dev->dev.type = &rc_dev_type;
-        else
+        if (ir_dev->props) {
+                if (ir_dev->props->driver_type == RC_DRIVER_SCANCODE)
+                        ir_dev->dev.type = &rc_dev_type;
+        } else
                 ir_dev->dev.type = &ir_raw_dev_type;
 
         ir_dev->dev.class = &ir_input_class;
diff --git a/drivers/media/IR/keymaps/Makefile b/drivers/media/IR/keymaps/Makefile
index ec25258a955f..aea649fbcf5a 100644
--- a/drivers/media/IR/keymaps/Makefile
+++ b/drivers/media/IR/keymaps/Makefile
@@ -6,7 +6,8 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
                         rc-avermedia.o \
                         rc-avermedia-cardbus.o \
                         rc-avermedia-dvbt.o \
-                        rc-avermedia-m135a-rm-jx.o \
+                        rc-avermedia-m135a.o \
+                        rc-avermedia-m733a-rm-k6.o \
                         rc-avertv-303.o \
                         rc-behold.o \
                         rc-behold-columbus.o \
diff --git a/drivers/media/IR/keymaps/rc-avermedia-m135a-rm-jx.c b/drivers/media/IR/keymaps/rc-avermedia-m135a-rm-jx.c
deleted file mode 100644
index 101e7ea85941..000000000000
--- a/drivers/media/IR/keymaps/rc-avermedia-m135a-rm-jx.c
+++ /dev/null
@@ -1,90 +0,0 @@
-/* avermedia-m135a-rm-jx.h - Keytable for avermedia_m135a_rm_jx Remote Controller
- *
- * keymap imported from ir-keymaps.c
- *
- * Copyright (c) 2010 by Mauro Carvalho Chehab <mchehab@redhat.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 <media/rc-map.h>
-
-/*
- * Avermedia M135A with IR model RM-JX
- * The same codes exist on both Positivo (BR) and original IR
- * Mauro Carvalho Chehab <mchehab@infradead.org>
- */
-
-static struct ir_scancode avermedia_m135a_rm_jx[] = {
-        { 0x0200, KEY_POWER2 },
-        { 0x022e, KEY_DOT },            /* '.' */
-        { 0x0201, KEY_MODE },           /* TV/FM or SOURCE */
-
-        { 0x0205, KEY_1 },
-        { 0x0206, KEY_2 },
-        { 0x0207, KEY_3 },
-        { 0x0209, KEY_4 },
-        { 0x020a, KEY_5 },
-        { 0x020b, KEY_6 },
-        { 0x020d, KEY_7 },
-        { 0x020e, KEY_8 },
-        { 0x020f, KEY_9 },
-        { 0x0211, KEY_0 },
-
-        { 0x0213, KEY_RIGHT },          /* -> or L */
-        { 0x0212, KEY_LEFT },           /* <- or R */
-
-        { 0x0217, KEY_SLEEP },          /* Capturar Imagem or Snapshot */
-        { 0x0210, KEY_SHUFFLE },        /* Amostra or 16 chan prev */
-
-        { 0x0303, KEY_CHANNELUP },
-        { 0x0302, KEY_CHANNELDOWN },
-        { 0x021f, KEY_VOLUMEUP },
-        { 0x021e, KEY_VOLUMEDOWN },
-        { 0x020c, KEY_ENTER },          /* Full Screen */
-
-        { 0x0214, KEY_MUTE },
-        { 0x0208, KEY_AUDIO },
-
-        { 0x0203, KEY_TEXT },           /* Teletext */
-        { 0x0204, KEY_EPG },
-        { 0x022b, KEY_TV2 },            /* TV2 or PIP */
-
-        { 0x021d, KEY_RED },
-        { 0x021c, KEY_YELLOW },
-        { 0x0301, KEY_GREEN },
-        { 0x0300, KEY_BLUE },
-
-        { 0x021a, KEY_PLAYPAUSE },
-        { 0x0219, KEY_RECORD },
-        { 0x0218, KEY_PLAY },
-        { 0x021b, KEY_STOP },
-};
-
-static struct rc_keymap avermedia_m135a_rm_jx_map = {
-        .map = {
-                .scan    = avermedia_m135a_rm_jx,
-                .size    = ARRAY_SIZE(avermedia_m135a_rm_jx),
-                .ir_type = IR_TYPE_NEC,
-                .name    = RC_MAP_AVERMEDIA_M135A_RM_JX,
-        }
-};
-
-static int __init init_rc_map_avermedia_m135a_rm_jx(void)
-{
-        return ir_register_map(&avermedia_m135a_rm_jx_map);
-}
-
-static void __exit exit_rc_map_avermedia_m135a_rm_jx(void)
-{
-        ir_unregister_map(&avermedia_m135a_rm_jx_map);
-}
-
-module_init(init_rc_map_avermedia_m135a_rm_jx)
-module_exit(exit_rc_map_avermedia_m135a_rm_jx)
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
diff --git a/drivers/media/IR/keymaps/rc-avermedia-m135a.c b/drivers/media/IR/keymaps/rc-avermedia-m135a.c
new file mode 100644
index 000000000000..e4471fb2ad1e
--- /dev/null
+++ b/drivers/media/IR/keymaps/rc-avermedia-m135a.c
@@ -0,0 +1,147 @@
+/* avermedia-m135a.c - Keytable for Avermedia M135A Remote Controllers
+ *
+ * Copyright (c) 2010 by Mauro Carvalho Chehab <mchehab@redhat.com>
+ * Copyright (c) 2010 by Herton Ronaldo Krzesinski <herton@mandriva.com.br>
+ *
+ * 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 <media/rc-map.h>
+
+/*
+ * Avermedia M135A with RM-JX and RM-K6 remote controls
+ *
+ * On Avermedia M135A with IR model RM-JX, the same codes exist on both
+ * Positivo (BR) and original IR, initial version and remote control codes
+ * added by Mauro Carvalho Chehab <mchehab@infradead.org>
+ *
+ * Positivo also ships Avermedia M135A with model RM-K6, extra control
+ * codes added by Herton Ronaldo Krzesinski <herton@mandriva.com.br>
+ */
+
+static struct ir_scancode avermedia_m135a[] = {
+        /* RM-JX */
+        { 0x0200, KEY_POWER2 },
+        { 0x022e, KEY_DOT },            /* '.' */
+        { 0x0201, KEY_MODE },           /* TV/FM or SOURCE */
+
+        { 0x0205, KEY_1 },
+        { 0x0206, KEY_2 },
+        { 0x0207, KEY_3 },
+        { 0x0209, KEY_4 },
+        { 0x020a, KEY_5 },
+        { 0x020b, KEY_6 },
+        { 0x020d, KEY_7 },
+        { 0x020e, KEY_8 },
+        { 0x020f, KEY_9 },
+        { 0x0211, KEY_0 },
+
+        { 0x0213, KEY_RIGHT },          /* -> or L */
+        { 0x0212, KEY_LEFT },           /* <- or R */
+
+        { 0x0217, KEY_SLEEP },          /* Capturar Imagem or Snapshot */
+        { 0x0210, KEY_SHUFFLE },        /* Amostra or 16 chan prev */
+
+        { 0x0303, KEY_CHANNELUP },
+        { 0x0302, KEY_CHANNELDOWN },
+        { 0x021f, KEY_VOLUMEUP },
+        { 0x021e, KEY_VOLUMEDOWN },
+        { 0x020c, KEY_ENTER },          /* Full Screen */
+
+        { 0x0214, KEY_MUTE },
+        { 0x0208, KEY_AUDIO },
+
+        { 0x0203, KEY_TEXT },           /* Teletext */
+        { 0x0204, KEY_EPG },
+        { 0x022b, KEY_TV2 },            /* TV2 or PIP */
+
+        { 0x021d, KEY_RED },
+        { 0x021c, KEY_YELLOW },
+        { 0x0301, KEY_GREEN },
+        { 0x0300, KEY_BLUE },
+
+        { 0x021a, KEY_PLAYPAUSE },
+        { 0x0219, KEY_RECORD },
+        { 0x0218, KEY_PLAY },
+        { 0x021b, KEY_STOP },
+
+        /* RM-K6 */
+        { 0x0401, KEY_POWER2 },
+        { 0x0406, KEY_MUTE },
+        { 0x0408, KEY_MODE },     /* TV/FM */
+
+        { 0x0409, KEY_1 },
+        { 0x040a, KEY_2 },
+        { 0x040b, KEY_3 },
+        { 0x040c, KEY_4 },
+        { 0x040d, KEY_5 },
+        { 0x040e, KEY_6 },
+        { 0x040f, KEY_7 },
+        { 0x0410, KEY_8 },
+        { 0x0411, KEY_9 },
+        { 0x044c, KEY_DOT },      /* '.' */
+        { 0x0412, KEY_0 },
+        { 0x0407, KEY_REFRESH },  /* Refresh/Reload */
+
+        { 0x0413, KEY_AUDIO },
+        { 0x0440, KEY_SCREEN },   /* Full Screen toggle */
+        { 0x0441, KEY_HOME },
+        { 0x0442, KEY_BACK },
+        { 0x0447, KEY_UP },
+        { 0x0448, KEY_DOWN },
+        { 0x0449, KEY_LEFT },
+        { 0x044a, KEY_RIGHT },
+        { 0x044b, KEY_OK },
+        { 0x0404, KEY_VOLUMEUP },
+        { 0x0405, KEY_VOLUMEDOWN },
+        { 0x0402, KEY_CHANNELUP },
+        { 0x0403, KEY_CHANNELDOWN },
+
+        { 0x0443, KEY_RED },
+        { 0x0444, KEY_GREEN },
+        { 0x0445, KEY_YELLOW },
+        { 0x0446, KEY_BLUE },
+
+        { 0x0414, KEY_TEXT },
+        { 0x0415, KEY_EPG },
+        { 0x041a, KEY_TV2 },      /* PIP */
+        { 0x041b, KEY_MHP },      /* Snapshot */
+
+        { 0x0417, KEY_RECORD },
+        { 0x0416, KEY_PLAYPAUSE },
+        { 0x0418, KEY_STOP },
+        { 0x0419, KEY_PAUSE },
+
+        { 0x041f, KEY_PREVIOUS },
+        { 0x041c, KEY_REWIND },
+        { 0x041d, KEY_FORWARD },
+        { 0x041e, KEY_NEXT },
+};
+
+static struct rc_keymap avermedia_m135a_map = {
+        .map = {
+                .scan    = avermedia_m135a,
+                .size    = ARRAY_SIZE(avermedia_m135a),
+                .ir_type = IR_TYPE_NEC,
+                .name    = RC_MAP_AVERMEDIA_M135A,
+        }
+};
+
+static int __init init_rc_map_avermedia_m135a(void)
+{
+        return ir_register_map(&avermedia_m135a_map);
+}
+
+static void __exit exit_rc_map_avermedia_m135a(void)
+{
+        ir_unregister_map(&avermedia_m135a_map);
+}
+
+module_init(init_rc_map_avermedia_m135a)
+module_exit(exit_rc_map_avermedia_m135a)
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
diff --git a/drivers/media/IR/keymaps/rc-avermedia-m733a-rm-k6.c b/drivers/media/IR/keymaps/rc-avermedia-m733a-rm-k6.c
new file mode 100644
index 000000000000..cf8d45717cb3
--- /dev/null
+++ b/drivers/media/IR/keymaps/rc-avermedia-m733a-rm-k6.c
@@ -0,0 +1,95 @@
+/* avermedia-m733a-rm-k6.h - Keytable for avermedia_m733a_rm_k6 Remote Controller
+ *
+ * Copyright (c) 2010 by Herton Ronaldo Krzesinski <herton@mandriva.com.br>
+ *
+ * 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 <media/rc-map.h>
+
+/*
+ * Avermedia M733A with IR model RM-K6
+ * This is the stock remote controller used with Positivo machines with M733A
+ * Herton Ronaldo Krzesinski <herton@mandriva.com.br>
+ */
+
+static struct ir_scancode avermedia_m733a_rm_k6[] = {
+        { 0x0401, KEY_POWER2 },
+        { 0x0406, KEY_MUTE },
+        { 0x0408, KEY_MODE },     /* TV/FM */
+
+        { 0x0409, KEY_1 },
+        { 0x040a, KEY_2 },
+        { 0x040b, KEY_3 },
+        { 0x040c, KEY_4 },
+        { 0x040d, KEY_5 },
+        { 0x040e, KEY_6 },
+        { 0x040f, KEY_7 },
+        { 0x0410, KEY_8 },
+        { 0x0411, KEY_9 },
+        { 0x044c, KEY_DOT },      /* '.' */
+        { 0x0412, KEY_0 },
+        { 0x0407, KEY_REFRESH },  /* Refresh/Reload */
+
+        { 0x0413, KEY_AUDIO },
+        { 0x0440, KEY_SCREEN },   /* Full Screen toggle */
+        { 0x0441, KEY_HOME },
+        { 0x0442, KEY_BACK },
+        { 0x0447, KEY_UP },
+        { 0x0448, KEY_DOWN },
+        { 0x0449, KEY_LEFT },
+        { 0x044a, KEY_RIGHT },
+        { 0x044b, KEY_OK },
+        { 0x0404, KEY_VOLUMEUP },
+        { 0x0405, KEY_VOLUMEDOWN },
+        { 0x0402, KEY_CHANNELUP },
+        { 0x0403, KEY_CHANNELDOWN },
+
+        { 0x0443, KEY_RED },
+        { 0x0444, KEY_GREEN },
+        { 0x0445, KEY_YELLOW },
+        { 0x0446, KEY_BLUE },
+
+        { 0x0414, KEY_TEXT },
+        { 0x0415, KEY_EPG },
+        { 0x041a, KEY_TV2 },      /* PIP */
+        { 0x041b, KEY_MHP },      /* Snapshot */
+
+        { 0x0417, KEY_RECORD },
+        { 0x0416, KEY_PLAYPAUSE },
+        { 0x0418, KEY_STOP },
+        { 0x0419, KEY_PAUSE },
+
+        { 0x041f, KEY_PREVIOUS },
+        { 0x041c, KEY_REWIND },
+        { 0x041d, KEY_FORWARD },
+        { 0x041e, KEY_NEXT },
+};
+
+static struct rc_keymap avermedia_m733a_rm_k6_map = {
+        .map = {
+                .scan    = avermedia_m733a_rm_k6,
+                .size    = ARRAY_SIZE(avermedia_m733a_rm_k6),
+                .ir_type = IR_TYPE_NEC,
+                .name    = RC_MAP_AVERMEDIA_M733A_RM_K6,
+        }
+};
+
+static int __init init_rc_map_avermedia_m733a_rm_k6(void)
+{
+        return ir_register_map(&avermedia_m733a_rm_k6_map);
+}
+
+static void __exit exit_rc_map_avermedia_m733a_rm_k6(void)
+{
+        ir_unregister_map(&avermedia_m733a_rm_k6_map);
+}
+
+module_init(init_rc_map_avermedia_m733a_rm_k6)
+module_exit(exit_rc_map_avermedia_m733a_rm_k6)
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
diff --git a/drivers/media/dvb/dm1105/dm1105.c b/drivers/media/dvb/dm1105/dm1105.c
index b762e561a6d5..bca07c0bcd01 100644
--- a/drivers/media/dvb/dm1105/dm1105.c
+++ b/drivers/media/dvb/dm1105/dm1105.c
@@ -594,7 +594,7 @@ static irqreturn_t dm1105_irq(int irq, void *dev_id)
 int __devinit dm1105_ir_init(struct dm1105_dev *dm1105)
 {
         struct input_dev *input_dev;
-        char *ir_codes = NULL;
+        char *ir_codes = RC_MAP_DM1105_NEC;
         int err = -ENOMEM;
 
         input_dev = input_allocate_device();
diff --git a/drivers/media/dvb/dvb-core/dvb_net.c b/drivers/media/dvb/dvb-core/dvb_net.c
index f6dac2bb0ac6..6c3a8a06ccab 100644
--- a/drivers/media/dvb/dvb-core/dvb_net.c
+++ b/drivers/media/dvb/dvb-core/dvb_net.c
@@ -351,6 +351,7 @@ static void dvb_net_ule( struct net_device *dev, const u8 *buf, size_t buf_len )
         const u8 *ts, *ts_end, *from_where = NULL;
         u8 ts_remain = 0, how_much = 0, new_ts = 1;
         struct ethhdr *ethh = NULL;
+        bool error = false;
 
 #ifdef ULE_DEBUG
         /* The code inside ULE_DEBUG keeps a history of the last 100 TS cells processed. */
@@ -460,10 +461,16 @@ static void dvb_net_ule( struct net_device *dev, const u8 *buf, size_t buf_len )
 
                                                 /* Drop partly decoded SNDU, reset state, resync on PUSI. */
                                                 if (priv->ule_skb) {
-                                                        dev_kfree_skb( priv->ule_skb );
+                                                        error = true;
+                                                        dev_kfree_skb(priv->ule_skb);
+                                                }
+
+                                                if (error || priv->ule_sndu_remain) {
                                                         dev->stats.rx_errors++;
                                                         dev->stats.rx_frame_errors++;
+                                                        error = false;
                                                 }
+
                                                 reset_ule(priv);
                                                 priv->need_pusi = 1;
                                                 continue;
@@ -535,6 +542,7 @@ static void dvb_net_ule( struct net_device *dev, const u8 *buf, size_t buf_len )
                                 from_where += 2;
                         }
 
+                        priv->ule_sndu_remain = priv->ule_sndu_len + 2;
                         /*
                          * State of current TS:
                          *   ts_remain (remaining bytes in the current TS cell)
@@ -544,6 +552,7 @@ static void dvb_net_ule( struct net_device *dev, const u8 *buf, size_t buf_len )
                          */
                         switch (ts_remain) {
                                 case 1:
+                                        priv->ule_sndu_remain--;
                                         priv->ule_sndu_type = from_where[0] << 8;
                                         priv->ule_sndu_type_1 = 1; /* first byte of ule_type is set. */
                                         ts_remain -= 1; from_where += 1;
@@ -557,6 +566,7 @@ static void dvb_net_ule( struct net_device *dev, const u8 *buf, size_t buf_len )
                                 default: /* complete ULE header is present in current TS. */
                                         /* Extract ULE type field. */
                                         if (priv->ule_sndu_type_1) {
+                                                priv->ule_sndu_type_1 = 0;
                                                 priv->ule_sndu_type |= from_where[0];
                                                 from_where += 1; /* points to payload start. */
                                                 ts_remain -= 1;
diff --git a/drivers/media/dvb/dvb-usb/Kconfig b/drivers/media/dvb/dvb-usb/Kconfig
index e5f91f16ffa4..553b48ac1919 100644
--- a/drivers/media/dvb/dvb-usb/Kconfig
+++ b/drivers/media/dvb/dvb-usb/Kconfig
@@ -76,6 +76,7 @@ config DVB_USB_DIB0700
         select DVB_S5H1411 if !DVB_FE_CUSTOMISE
         select DVB_LGDT3305 if !DVB_FE_CUSTOMISE
         select DVB_TUNER_DIB0070 if !DVB_FE_CUSTOMISE
+        select DVB_TUNER_DIB0090 if !DVB_FE_CUSTOMISE
         select MEDIA_TUNER_MT2060 if !MEDIA_TUNER_CUSTOMISE
         select MEDIA_TUNER_MT2266 if !MEDIA_TUNER_CUSTOMISE
         select MEDIA_TUNER_XC2028 if !MEDIA_TUNER_CUSTOMISE
@@ -134,6 +135,7 @@ config DVB_USB_M920X
         select DVB_TDA1004X if !DVB_FE_CUSTOMISE
         select MEDIA_TUNER_QT1010 if !MEDIA_TUNER_CUSTOMISE
         select MEDIA_TUNER_TDA827X if !MEDIA_TUNER_CUSTOMISE
+        select MEDIA_TUNER_SIMPLE if !MEDIA_TUNER_CUSTOMISE
         help
           Say Y here to support the MSI Mega Sky 580 USB2.0 DVB-T receiver.
           Currently, only devices with a product id of
@@ -264,7 +266,7 @@ config DVB_USB_DW2102
         select DVB_STB6000 if !DVB_FE_CUSTOMISE
         select DVB_CX24116 if !DVB_FE_CUSTOMISE
         select DVB_SI21XX if !DVB_FE_CUSTOMISE
-        select DVB_TDA10021 if !DVB_FE_CUSTOMISE
+        select DVB_TDA10023 if !DVB_FE_CUSTOMISE
         select DVB_MT312 if !DVB_FE_CUSTOMISE
         select DVB_ZL10039 if !DVB_FE_CUSTOMISE
         select DVB_DS3000 if !DVB_FE_CUSTOMISE
diff --git a/drivers/media/dvb/dvb-usb/cxusb.c b/drivers/media/dvb/dvb-usb/cxusb.c
index 0eb490889162..11e9e85dac86 100644
--- a/drivers/media/dvb/dvb-usb/cxusb.c
+++ b/drivers/media/dvb/dvb-usb/cxusb.c
@@ -1026,8 +1026,10 @@ static int cxusb_dualdig4_rev2_frontend_attach(struct dvb_usb_adapter *adap)
         cxusb_bluebird_gpio_pulse(adap->dev, 0x02, 1);
 
         if (dib7000p_i2c_enumeration(&adap->dev->i2c_adap, 1, 18,
-                                 &cxusb_dualdig4_rev2_config) < 0)
+                                     &cxusb_dualdig4_rev2_config) < 0) {
+                printk(KERN_WARNING "Unable to enumerate dib7000p\n");
                 return -ENODEV;
+        }
 
         adap->fe = dvb_attach(dib7000p_attach, &adap->dev->i2c_adap, 0x80,
                               &cxusb_dualdig4_rev2_config);
diff --git a/drivers/media/dvb/dvb-usb/dvb-usb-ids.h b/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
index 085c4e457e0e..b4afe6f8ed19 100644
--- a/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
+++ b/drivers/media/dvb/dvb-usb/dvb-usb-ids.h
@@ -198,6 +198,7 @@
 #define USB_PID_AVERMEDIA_A850                          0x850a
 #define USB_PID_AVERMEDIA_A805                          0xa805
 #define USB_PID_TECHNOTREND_CONNECT_S2400               0x3006
+#define USB_PID_TECHNOTREND_CONNECT_CT3650              0x300d
 #define USB_PID_TERRATEC_CINERGY_DT_XS_DIVERSITY        0x005a
 #define USB_PID_TERRATEC_CINERGY_DT_XS_DIVERSITY_2      0x0081
 #define USB_PID_TERRATEC_CINERGY_HT_USB_XE              0x0058
diff --git a/drivers/media/dvb/dvb-usb/ttusb2.c b/drivers/media/dvb/dvb-usb/ttusb2.c
index 20ca9d9ee99b..a6de489a6a39 100644
--- a/drivers/media/dvb/dvb-usb/ttusb2.c
+++ b/drivers/media/dvb/dvb-usb/ttusb2.c
@@ -29,6 +29,8 @@
 
 #include "tda826x.h"
 #include "tda10086.h"
+#include "tda1002x.h"
+#include "tda827x.h"
 #include "lnbp21.h"
 
 /* debug */
@@ -150,7 +152,17 @@ static struct tda10086_config tda10086_config = {
         .xtal_freq = TDA10086_XTAL_16M,
 };
 
-static int ttusb2_frontend_attach(struct dvb_usb_adapter *adap)
+static struct tda10023_config tda10023_config = {
+        .demod_address = 0x0c,
+        .invert = 0,
+        .xtal = 16000000,
+        .pll_m = 11,
+        .pll_p = 3,
+        .pll_n = 1,
+        .deltaf = 0xa511,
+};
+
+static int ttusb2_frontend_tda10086_attach(struct dvb_usb_adapter *adap)
 {
         if (usb_set_interface(adap->dev->udev,0,3) < 0)
                 err("set interface to alts=3 failed");
@@ -163,7 +175,27 @@ static int ttusb2_frontend_attach(struct dvb_usb_adapter *adap)
         return 0;
 }
 
-static int ttusb2_tuner_attach(struct dvb_usb_adapter *adap)
+static int ttusb2_frontend_tda10023_attach(struct dvb_usb_adapter *adap)
+{
+        if (usb_set_interface(adap->dev->udev, 0, 3) < 0)
+                err("set interface to alts=3 failed");
+        if ((adap->fe = dvb_attach(tda10023_attach, &tda10023_config, &adap->dev->i2c_adap, 0x48)) == NULL) {
+                deb_info("TDA10023 attach failed\n");
+                return -ENODEV;
+        }
+        return 0;
+}
+
+static int ttusb2_tuner_tda827x_attach(struct dvb_usb_adapter *adap)
+{
+        if (dvb_attach(tda827x_attach, adap->fe, 0x61, &adap->dev->i2c_adap, NULL) == NULL) {
+                printk(KERN_ERR "%s: No tda827x found!\n", __func__);
+                return -ENODEV;
+        }
+        return 0;
+}
+
+static int ttusb2_tuner_tda826x_attach(struct dvb_usb_adapter *adap)
 {
         if (dvb_attach(tda826x_attach, adap->fe, 0x60, &adap->dev->i2c_adap, 0) == NULL) {
                 deb_info("TDA8263 attach failed\n");
@@ -180,6 +212,7 @@ static int ttusb2_tuner_attach(struct dvb_usb_adapter *adap)
 /* DVB USB Driver stuff */
 static struct dvb_usb_device_properties ttusb2_properties;
 static struct dvb_usb_device_properties ttusb2_properties_s2400;
+static struct dvb_usb_device_properties ttusb2_properties_ct3650;
 
 static int ttusb2_probe(struct usb_interface *intf,
                 const struct usb_device_id *id)
@@ -187,6 +220,8 @@ static int ttusb2_probe(struct usb_interface *intf,
         if (0 == dvb_usb_device_init(intf, &ttusb2_properties,
                                      THIS_MODULE, NULL, adapter_nr) ||
             0 == dvb_usb_device_init(intf, &ttusb2_properties_s2400,
+                                     THIS_MODULE, NULL, adapter_nr) ||
+            0 == dvb_usb_device_init(intf, &ttusb2_properties_ct3650,
                                      THIS_MODULE, NULL, adapter_nr))
                 return 0;
         return -ENODEV;
@@ -197,6 +232,8 @@ static struct usb_device_id ttusb2_table [] = {
         { USB_DEVICE(USB_VID_PINNACLE, USB_PID_PCTV_450E) },
         { USB_DEVICE(USB_VID_TECHNOTREND,
                 USB_PID_TECHNOTREND_CONNECT_S2400) },
+        { USB_DEVICE(USB_VID_TECHNOTREND,
+                USB_PID_TECHNOTREND_CONNECT_CT3650) },
         {}              /* Terminating entry */
 };
 MODULE_DEVICE_TABLE (usb, ttusb2_table);
@@ -214,8 +251,8 @@ static struct dvb_usb_device_properties ttusb2_properties = {
                 {
                         .streaming_ctrl   = NULL, // ttusb2_streaming_ctrl,
 
-                        .frontend_attach  = ttusb2_frontend_attach,
-                        .tuner_attach     = ttusb2_tuner_attach,
+                        .frontend_attach  = ttusb2_frontend_tda10086_attach,
+                        .tuner_attach     = ttusb2_tuner_tda826x_attach,
 
                         /* parameter for the MPEG2-data transfer */
                         .stream = {
@@ -266,8 +303,8 @@ static struct dvb_usb_device_properties ttusb2_properties_s2400 = {
                 {
                         .streaming_ctrl   = NULL,
 
-                        .frontend_attach  = ttusb2_frontend_attach,
-                        .tuner_attach     = ttusb2_tuner_attach,
+                        .frontend_attach  = ttusb2_frontend_tda10086_attach,
+                        .tuner_attach     = ttusb2_tuner_tda826x_attach,
 
                         /* parameter for the MPEG2-data transfer */
                         .stream = {
@@ -301,6 +338,52 @@ static struct dvb_usb_device_properties ttusb2_properties_s2400 = {
         }
 };
 
+static struct dvb_usb_device_properties ttusb2_properties_ct3650 = {
+        .caps = DVB_USB_IS_AN_I2C_ADAPTER,
+
+        .usb_ctrl = CYPRESS_FX2,
+
+        .size_of_priv = sizeof(struct ttusb2_state),
+
+        .num_adapters = 1,
+        .adapter = {
+                {
+                        .streaming_ctrl   = NULL,
+
+                        .frontend_attach  = ttusb2_frontend_tda10023_attach,
+                        .tuner_attach = ttusb2_tuner_tda827x_attach,
+
+                        /* parameter for the MPEG2-data transfer */
+                        .stream = {
+                                .type = USB_ISOC,
+                                .count = 5,
+                                .endpoint = 0x02,
+                                .u = {
+                                        .isoc = {
+                                                .framesperurb = 4,
+                                                .framesize = 940,
+                                                .interval = 1,
+                                        }
+                                }
+                        }
+                },
+        },
+
+        .power_ctrl       = ttusb2_power_ctrl,
+        .identify_state   = ttusb2_identify_state,
+
+        .i2c_algo         = &ttusb2_i2c_algo,
+
+        .generic_bulk_ctrl_endpoint = 0x01,
+
+        .num_device_descs = 1,
+        .devices = {
+                {   "Technotrend TT-connect CT-3650",
+                        .warm_ids = { &ttusb2_table[3], NULL },
+                },
+        }
+};
+
 static struct usb_driver ttusb2_driver = {
         .name           = "dvb_usb_ttusb2",
         .probe          = ttusb2_probe,
diff --git a/drivers/media/dvb/firewire/firedtv-1394.c b/drivers/media/dvb/firewire/firedtv-1394.c
index 26333b4f4d3e..b34ca7afb0e6 100644
--- a/drivers/media/dvb/firewire/firedtv-1394.c
+++ b/drivers/media/dvb/firewire/firedtv-1394.c
@@ -58,7 +58,7 @@ static void rawiso_activity_cb(struct hpsb_iso *iso)
         num = hpsb_iso_n_ready(iso);
 
         if (!fdtv) {
-                dev_err(fdtv->device, "received at unknown iso channel\n");
+                pr_err("received at unknown iso channel\n");
                 goto out;
         }
 
diff --git a/drivers/media/dvb/frontends/au8522_decoder.c b/drivers/media/dvb/frontends/au8522_decoder.c
index 68dba3a4b4da..29cdbfe36852 100644
--- a/drivers/media/dvb/frontends/au8522_decoder.c
+++ b/drivers/media/dvb/frontends/au8522_decoder.c
@@ -567,30 +567,6 @@ static int au8522_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
 
 /* ----------------------------------------------------------------------- */
 
-static int au8522_g_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
-{
-        switch (fmt->type) {
-        default:
-                return -EINVAL;
-        }
-        return 0;
-}
-
-static int au8522_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
-{
-        switch (fmt->type) {
-        case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-                /* Not yet implemented */
-                break;
-        default:
-                return -EINVAL;
-        }
-
-        return 0;
-}
-
-/* ----------------------------------------------------------------------- */
-
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 static int au8522_g_register(struct v4l2_subdev *sd,
                              struct v4l2_dbg_register *reg)
@@ -772,8 +748,6 @@ static const struct v4l2_subdev_audio_ops au8522_audio_ops = {
 
 static const struct v4l2_subdev_video_ops au8522_video_ops = {
         .s_routing = au8522_s_video_routing,
-        .g_fmt = au8522_g_fmt,
-        .s_fmt = au8522_s_fmt,
         .s_stream = au8522_s_stream,
 };
 
diff --git a/drivers/media/dvb/frontends/ds3000.c b/drivers/media/dvb/frontends/ds3000.c
index 78001e8bcdb7..fc61d9230db8 100644
--- a/drivers/media/dvb/frontends/ds3000.c
+++ b/drivers/media/dvb/frontends/ds3000.c
@@ -969,15 +969,12 @@ struct dvb_frontend *ds3000_attach(const struct ds3000_config *config,
         dprintk("%s\n", __func__);
 
         /* allocate memory for the internal state */
-        state = kmalloc(sizeof(struct ds3000_state), GFP_KERNEL);
+        state = kzalloc(sizeof(struct ds3000_state), GFP_KERNEL);
         if (state == NULL) {
                 printk(KERN_ERR "Unable to kmalloc\n");
                 goto error2;
         }
 
-        /* setup the state */
-        memset(state, 0, sizeof(struct ds3000_state));
-
         state->config = config;
         state->i2c = i2c;
         state->prevUCBS2 = 0;
diff --git a/drivers/media/dvb/frontends/stv6110x.c b/drivers/media/dvb/frontends/stv6110x.c
index 42591ce1aaad..f36cab12bdc7 100644
--- a/drivers/media/dvb/frontends/stv6110x.c
+++ b/drivers/media/dvb/frontends/stv6110x.c
@@ -303,7 +303,10 @@ static int stv6110x_set_mode(struct dvb_frontend *fe, enum tuner_mode mode)
 
 static int stv6110x_sleep(struct dvb_frontend *fe)
 {
-        return stv6110x_set_mode(fe, TUNER_SLEEP);
+        if (fe->tuner_priv)
+                return stv6110x_set_mode(fe, TUNER_SLEEP);
+
+        return 0;
 }
 
 static int stv6110x_get_status(struct dvb_frontend *fe, u32 *status)
diff --git a/drivers/media/dvb/ngene/ngene-cards.c b/drivers/media/dvb/ngene/ngene-cards.c
index 692c3e226e83..4692a41ad95b 100644
--- a/drivers/media/dvb/ngene/ngene-cards.c
+++ b/drivers/media/dvb/ngene/ngene-cards.c
@@ -217,6 +217,19 @@ static struct ngene_info ngene_info_cineS2v5 = {
         .fw_version     = 15,
 };
 
+static struct ngene_info ngene_info_duoFlexS2 = {
+        .type           = NGENE_SIDEWINDER,
+        .name           = "Digital Devices DuoFlex S2 miniPCIe",
+        .io_type        = {NGENE_IO_TSIN, NGENE_IO_TSIN},
+        .demod_attach   = {demod_attach_stv0900, demod_attach_stv0900},
+        .tuner_attach   = {tuner_attach_stv6110, tuner_attach_stv6110},
+        .fe_config      = {&fe_cineS2, &fe_cineS2},
+        .tuner_config   = {&tuner_cineS2_0, &tuner_cineS2_1},
+        .lnb            = {0x0a, 0x08},
+        .tsf            = {3, 3},
+        .fw_version     = 15,
+};
+
 static struct ngene_info ngene_info_m780 = {
         .type           = NGENE_APP,
         .name           = "Aver M780 ATSC/QAM-B",
@@ -256,6 +269,8 @@ static const struct pci_device_id ngene_id_tbl[] __devinitdata = {
         NGENE_ID(0x18c3, 0xdb01, ngene_info_satixS2),
         NGENE_ID(0x18c3, 0xdb02, ngene_info_satixS2v2),
         NGENE_ID(0x18c3, 0xdd00, ngene_info_cineS2v5),
+        NGENE_ID(0x18c3, 0xdd10, ngene_info_duoFlexS2),
+        NGENE_ID(0x18c3, 0xdd20, ngene_info_duoFlexS2),
         NGENE_ID(0x1461, 0x062e, ngene_info_m780),
         {0}
 };
diff --git a/drivers/media/dvb/ngene/ngene-core.c b/drivers/media/dvb/ngene/ngene-core.c
index c8b4dfa0ab5f..4caeb163a666 100644
--- a/drivers/media/dvb/ngene/ngene-core.c
+++ b/drivers/media/dvb/ngene/ngene-core.c
@@ -53,8 +53,6 @@ MODULE_PARM_DESC(debug, "Print debugging information.");
 
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
-#define COMMAND_TIMEOUT_WORKAROUND
-
 #define dprintk if (debug) printk
 
 #define ngwriteb(dat, adr)         writeb((dat), (char *)(dev->iomem + (adr)))
@@ -147,24 +145,24 @@ static void demux_tasklet(unsigned long data)
                 } else {
                         if (chan->HWState == HWSTATE_RUN) {
                                 u32 Flags = 0;
+                                IBufferExchange *exch1 = chan->pBufferExchange;
+                                IBufferExchange *exch2 = chan->pBufferExchange2;
                                 if (Cur->ngeneBuffer.SR.Flags & 0x01)
                                         Flags |= BEF_EVEN_FIELD;
                                 if (Cur->ngeneBuffer.SR.Flags & 0x20)
                                         Flags |= BEF_OVERFLOW;
-                                if (chan->pBufferExchange)
-                                        chan->pBufferExchange(chan,
-                                                              Cur->Buffer1,
-                                                              chan->
-                                                              Capture1Length,
-                                                              Cur->ngeneBuffer.
-                                                              SR.Clock, Flags);
-                                if (chan->pBufferExchange2)
-                                        chan->pBufferExchange2(chan,
-                                                               Cur->Buffer2,
-                                                               chan->
-                                                               Capture2Length,
-                                                               Cur->ngeneBuffer.
-                                                               SR.Clock, Flags);
+                                spin_unlock_irq(&chan->state_lock);
+                                if (exch1)
+                                        exch1(chan, Cur->Buffer1,
+                                                chan->Capture1Length,
+                                                Cur->ngeneBuffer.SR.Clock,
+                                                Flags);
+                                if (exch2)
+                                        exch2(chan, Cur->Buffer2,
+                                                chan->Capture2Length,
+                                                Cur->ngeneBuffer.SR.Clock,
+                                                Flags);
+                                spin_lock_irq(&chan->state_lock);
                         } else if (chan->HWState != HWSTATE_STOP)
                                 chan->HWState = HWSTATE_RUN;
                 }
@@ -572,11 +570,7 @@ static int ngene_command_stream_control(struct ngene *dev, u8 stream,
         u16 BsSPI = ((stream & 1) ? 0x9800 : 0x9700);
         u16 BsSDO = 0x9B00;
 
-        /* down(&dev->stream_mutex); */
-        while (down_trylock(&dev->stream_mutex)) {
-                printk(KERN_INFO DEVICE_NAME ": SC locked\n");
-                msleep(1);
-        }
+        down(&dev->stream_mutex);
         memset(&com, 0, sizeof(com));
         com.cmd.hdr.Opcode = CMD_CONTROL;
         com.cmd.hdr.Length = sizeof(struct FW_STREAM_CONTROL) - 2;
@@ -1252,14 +1246,17 @@ static int ngene_load_firm(struct ngene *dev)
                 version = 15;
                 size = 23466;
                 fw_name = "ngene_15.fw";
+                dev->cmd_timeout_workaround = true;
                 break;
         case 16:
                 size = 23498;
                 fw_name = "ngene_16.fw";
+                dev->cmd_timeout_workaround = true;
                 break;
         case 17:
                 size = 24446;
                 fw_name = "ngene_17.fw";
+                dev->cmd_timeout_workaround = true;
                 break;
         }
 
@@ -1299,11 +1296,16 @@ static void ngene_stop(struct ngene *dev)
         ngwritel(0, NGENE_EVENT);
         ngwritel(0, NGENE_EVENT_HI);
         free_irq(dev->pci_dev->irq, dev);
+#ifdef CONFIG_PCI_MSI
+        if (dev->msi_enabled)
+                pci_disable_msi(dev->pci_dev);
+#endif
 }
 
 static int ngene_start(struct ngene *dev)
 {
         int stat;
+        unsigned long flags;
         int i;
 
         pci_set_master(dev->pci_dev);
@@ -1333,6 +1335,28 @@ static int ngene_start(struct ngene *dev)
         if (stat < 0)
                 goto fail;
 
+#ifdef CONFIG_PCI_MSI
+        /* enable MSI if kernel and card support it */
+        if (pci_msi_enabled() && dev->card_info->msi_supported) {
+                ngwritel(0, NGENE_INT_ENABLE);
+                free_irq(dev->pci_dev->irq, dev);
+                stat = pci_enable_msi(dev->pci_dev);
+                if (stat) {
+                        printk(KERN_INFO DEVICE_NAME
+                                ": MSI not available\n");
+                        flags = IRQF_SHARED;
+                } else {
+                        flags = 0;
+                        dev->msi_enabled = true;
+                }
+                stat = request_irq(dev->pci_dev->irq, irq_handler,
+                                        flags, "nGene", dev);
+                if (stat < 0)
+                        goto fail2;
+                ngwritel(1, NGENE_INT_ENABLE);
+        }
+#endif
+
         stat = ngene_i2c_init(dev, 0);
         if (stat < 0)
                 goto fail;
@@ -1358,10 +1382,18 @@ static int ngene_start(struct ngene *dev)
                         bconf = BUFFER_CONFIG_3333;
                 stat = ngene_command_config_buf(dev, bconf);
         }
-        return stat;
+        if (!stat)
+                return stat;
+
+        /* otherwise error: fall through */
 fail:
         ngwritel(0, NGENE_INT_ENABLE);
         free_irq(dev->pci_dev->irq, dev);
+#ifdef CONFIG_PCI_MSI
+fail2:
+        if (dev->msi_enabled)
+                pci_disable_msi(dev->pci_dev);
+#endif
         return stat;
 }
 
@@ -1379,10 +1411,8 @@ static void release_channel(struct ngene_channel *chan)
         struct ngene_info *ni = dev->card_info;
         int io = ni->io_type[chan->number];
 
-#ifdef COMMAND_TIMEOUT_WORKAROUND
-        if (chan->running)
+        if (chan->dev->cmd_timeout_workaround && chan->running)
                 set_transfer(chan, 0);
-#endif
 
         tasklet_kill(&chan->demux_tasklet);
 
diff --git a/drivers/media/dvb/ngene/ngene-dvb.c b/drivers/media/dvb/ngene/ngene-dvb.c
index 96013eb353cd..48f980b21d66 100644
--- a/drivers/media/dvb/ngene/ngene-dvb.c
+++ b/drivers/media/dvb/ngene/ngene-dvb.c
@@ -37,15 +37,12 @@
 #include <linux/pci.h>
 #include <linux/smp_lock.h>
 #include <linux/timer.h>
-#include <linux/version.h>
 #include <linux/byteorder/generic.h>
 #include <linux/firmware.h>
 #include <linux/vmalloc.h>
 
 #include "ngene.h"
 
-#define COMMAND_TIMEOUT_WORKAROUND
-
 
 /****************************************************************************/
 /* COMMAND API interface ****************************************************/
@@ -69,9 +66,7 @@ void *tsin_exchange(void *priv, void *buf, u32 len, u32 clock, u32 flags)
         struct ngene_channel *chan = priv;
 
 
-#ifdef COMMAND_TIMEOUT_WORKAROUND
         if (chan->users > 0)
-#endif
                 dvb_dmx_swfilter(&chan->demux, buf, len);
         return NULL;
 }
@@ -106,11 +101,8 @@ int ngene_start_feed(struct dvb_demux_feed *dvbdmxfeed)
         struct ngene_channel *chan = dvbdmx->priv;
 
         if (chan->users == 0) {
-#ifdef COMMAND_TIMEOUT_WORKAROUND
-                if (!chan->running)
-#endif
+                if (!chan->dev->cmd_timeout_workaround || !chan->running)
                         set_transfer(chan, 1);
-                /* msleep(10); */
         }
 
         return ++chan->users;
@@ -124,9 +116,8 @@ int ngene_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
         if (--chan->users)
                 return chan->users;
 
-#ifndef COMMAND_TIMEOUT_WORKAROUND
-        set_transfer(chan, 0);
-#endif
+        if (!chan->dev->cmd_timeout_workaround)
+                set_transfer(chan, 0);
 
         return 0;
 }
diff --git a/drivers/media/dvb/ngene/ngene-i2c.c b/drivers/media/dvb/ngene/ngene-i2c.c
index 2ef54ca6badd..477fe0aade86 100644
--- a/drivers/media/dvb/ngene/ngene-i2c.c
+++ b/drivers/media/dvb/ngene/ngene-i2c.c
@@ -39,7 +39,6 @@
 #include <linux/pci_ids.h>
 #include <linux/smp_lock.h>
 #include <linux/timer.h>
-#include <linux/version.h>
 #include <linux/byteorder/generic.h>
 #include <linux/firmware.h>
 #include <linux/vmalloc.h>
diff --git a/drivers/media/dvb/ngene/ngene.h b/drivers/media/dvb/ngene/ngene.h
index 676fcbb79026..8fb4200f83f8 100644
--- a/drivers/media/dvb/ngene/ngene.h
+++ b/drivers/media/dvb/ngene/ngene.h
@@ -725,6 +725,8 @@ struct ngene {
         u32                   device_version;
         u32                   fw_interface_version;
         u32                   icounts;
+        bool                  msi_enabled;
+        bool                  cmd_timeout_workaround;
 
         u8                   *CmdDoneByte;
         int                   BootFirmware;
@@ -797,6 +799,7 @@ struct ngene_info {
 #define NGENE_VBOX_V2    7
 
         int   fw_version;
+        bool  msi_supported;
         char *name;
 
         int   io_type[MAX_STREAM];
diff --git a/drivers/media/dvb/ttpci/Kconfig b/drivers/media/dvb/ttpci/Kconfig
index d8d4214fd65f..32a7ec65ec42 100644
--- a/drivers/media/dvb/ttpci/Kconfig
+++ b/drivers/media/dvb/ttpci/Kconfig
@@ -68,13 +68,14 @@ config DVB_BUDGET
         select DVB_VES1820 if !DVB_FE_CUSTOMISE
         select DVB_L64781 if !DVB_FE_CUSTOMISE
         select DVB_TDA8083 if !DVB_FE_CUSTOMISE
-        select DVB_TDA10021 if !DVB_FE_CUSTOMISE
-        select DVB_TDA10023 if !DVB_FE_CUSTOMISE
         select DVB_S5H1420 if !DVB_FE_CUSTOMISE
         select DVB_TDA10086 if !DVB_FE_CUSTOMISE
         select DVB_TDA826X if !DVB_FE_CUSTOMISE
         select DVB_LNBP21 if !DVB_FE_CUSTOMISE
         select DVB_TDA1004X if !DVB_FE_CUSTOMISE
+        select DVB_ISL6423 if !DVB_FE_CUSTOMISE
+        select DVB_STV090x if !DVB_FE_CUSTOMISE
+        select DVB_STV6110x if !DVB_FE_CUSTOMISE
         help
           Support for simple SAA7146 based DVB cards (so called Budget-
           or Nova-PCI cards) without onboard MPEG2 decoder, and without
diff --git a/drivers/media/dvb/ttpci/budget-ci.c b/drivers/media/dvb/ttpci/budget-ci.c
index 461714396331..13ac9e3ab121 100644
--- a/drivers/media/dvb/ttpci/budget-ci.c
+++ b/drivers/media/dvb/ttpci/budget-ci.c
@@ -215,6 +215,7 @@ static int msp430_ir_init(struct budget_ci *budget_ci)
                 break;
         case 0x1010:
         case 0x1017:
+        case 0x1019:
         case 0x101a:
                 /* for the Technotrend 1500 bundled remote */
                 ir_codes = RC_MAP_TT_1500;
diff --git a/drivers/media/radio/si470x/radio-si470x-i2c.c b/drivers/media/radio/si470x/radio-si470x-i2c.c
index a5844d08d8b7..67a4ec8768a6 100644
--- a/drivers/media/radio/si470x/radio-si470x-i2c.c
+++ b/drivers/media/radio/si470x/radio-si470x-i2c.c
@@ -482,7 +482,6 @@ static __devexit int si470x_i2c_remove(struct i2c_client *client)
         cancel_work_sync(&radio->radio_work);
         video_unregister_device(radio->videodev);
         kfree(radio);
-        i2c_set_clientdata(client, NULL);
 
         return 0;
 }
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index ad9e6f9c22e9..bdbc9d305419 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -646,7 +646,7 @@ config VIDEO_PMS
 
 config VIDEO_BWQCAM
         tristate "Quickcam BW Video For Linux"
-        depends on PARPORT && VIDEO_V4L1
+        depends on PARPORT && VIDEO_V4L2
         help
           Say Y have if you the black and white version of the QuickCam
           camera. See the next option for the color version.
@@ -656,7 +656,7 @@ config VIDEO_BWQCAM
 
 config VIDEO_CQCAM
         tristate "QuickCam Colour Video For Linux (EXPERIMENTAL)"
-        depends on EXPERIMENTAL && PARPORT && VIDEO_V4L1
+        depends on EXPERIMENTAL && PARPORT && VIDEO_V4L2
         help
           This is the video4linux driver for the colour version of the
           Connectix QuickCam.  If you have one of these cameras, say Y here,
diff --git a/drivers/media/video/ak881x.c b/drivers/media/video/ak881x.c
index 35390d4717b9..1573392f74bd 100644
--- a/drivers/media/video/ak881x.c
+++ b/drivers/media/video/ak881x.c
@@ -11,6 +11,7 @@
 #include <linux/i2c.h>
 #include <linux/init.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
 #include <linux/videodev2.h>
 
 #include <media/ak881x.h>
@@ -141,7 +142,7 @@ static int ak881x_s_mbus_fmt(struct v4l2_subdev *sd,
         return ak881x_try_g_mbus_fmt(sd, mf);
 }
 
-static int ak881x_enum_mbus_fmt(struct v4l2_subdev *sd, int index,
+static int ak881x_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned int index,
                                 enum v4l2_mbus_pixelcode *code)
 {
         if (index)
diff --git a/drivers/media/video/bw-qcam.c b/drivers/media/video/bw-qcam.c
index 3c9e754d73a0..935e0c9a9674 100644
--- a/drivers/media/video/bw-qcam.c
+++ b/drivers/media/video/bw-qcam.c
@@ -66,19 +66,58 @@ OTHER DEALINGS IN THE SOFTWARE.
 #include <linux/delay.h>
 #include <linux/errno.h>
 #include <linux/fs.h>
-#include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/mm.h>
 #include <linux/parport.h>
 #include <linux/sched.h>
-#include <linux/videodev.h>
-#include <media/v4l2-common.h>
-#include <media/v4l2-ioctl.h>
+#include <linux/version.h>
+#include <linux/videodev2.h>
 #include <linux/mutex.h>
 #include <asm/uaccess.h>
-
-#include "bw-qcam.h"
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-device.h>
+
+/* One from column A... */
+#define QC_NOTSET 0
+#define QC_UNIDIR 1
+#define QC_BIDIR  2
+#define QC_SERIAL 3
+
+/* ... and one from column B */
+#define QC_ANY          0x00
+#define QC_FORCE_UNIDIR 0x10
+#define QC_FORCE_BIDIR  0x20
+#define QC_FORCE_SERIAL 0x30
+/* in the port_mode member */
+
+#define QC_MODE_MASK    0x07
+#define QC_FORCE_MASK   0x70
+
+#define MAX_HEIGHT 243
+#define MAX_WIDTH 336
+
+/* Bit fields for status flags */
+#define QC_PARAM_CHANGE 0x01 /* Camera status change has occurred */
+
+struct qcam {
+        struct v4l2_device v4l2_dev;
+        struct video_device vdev;
+        struct pardevice *pdev;
+        struct parport *pport;
+        struct mutex lock;
+        int width, height;
+        int bpp;
+        int mode;
+        int contrast, brightness, whitebal;
+        int port_mode;
+        int transfer_scale;
+        int top, left;
+        int status;
+        unsigned int saved_bits;
+        unsigned long in_use;
+};
 
 static unsigned int maxpoll = 250;   /* Maximum busy-loop count for qcam I/O */
 static unsigned int yieldlines = 4;  /* Yield after this many during capture */
@@ -93,22 +132,26 @@ module_param(video_nr, int, 0);
  * immediately attempt to initialize qcam */
 module_param(force_init, int, 0);
 
-static inline int read_lpstatus(struct qcam_device *q)
+#define MAX_CAMS 4
+static struct qcam *qcams[MAX_CAMS];
+static unsigned int num_cams;
+
+static inline int read_lpstatus(struct qcam *q)
 {
         return parport_read_status(q->pport);
 }
 
-static inline int read_lpdata(struct qcam_device *q)
+static inline int read_lpdata(struct qcam *q)
 {
         return parport_read_data(q->pport);
 }
 
-static inline void write_lpdata(struct qcam_device *q, int d)
+static inline void write_lpdata(struct qcam *q, int d)
 {
         parport_write_data(q->pport, d);
 }
 
-static inline void write_lpcontrol(struct qcam_device *q, int d)
+static void write_lpcontrol(struct qcam *q, int d)
 {
         if (d & 0x20) {
                 /* Set bidirectional mode to reverse (data in) */
@@ -124,126 +167,11 @@ static inline void write_lpcontrol(struct qcam_device *q, int d)
         parport_write_control(q->pport, d);
 }
 
-static int qc_waithand(struct qcam_device *q, int val);
-static int qc_command(struct qcam_device *q, int command);
-static int qc_readparam(struct qcam_device *q);
-static int qc_setscanmode(struct qcam_device *q);
-static int qc_readbytes(struct qcam_device *q, char buffer[]);
-
-static struct video_device qcam_template;
-
-static int qc_calibrate(struct qcam_device *q)
-{
-        /*
-         *      Bugfix by Hanno Mueller hmueller@kabel.de, Mai 21 96
-         *      The white balance is an individiual value for each
-         *      quickcam.
-         */
-
-        int value;
-        int count = 0;
-
-        qc_command(q, 27);      /* AutoAdjustOffset */
-        qc_command(q, 0);       /* Dummy Parameter, ignored by the camera */
-
-        /* GetOffset (33) will read 255 until autocalibration */
-        /* is finished. After that, a value of 1-254 will be */
-        /* returned. */
-
-        do {
-                qc_command(q, 33);
-                value = qc_readparam(q);
-                mdelay(1);
-                schedule();
-                count++;
-        } while (value == 0xff && count < 2048);
-
-        q->whitebal = value;
-        return value;
-}
-
-/* Initialize the QuickCam driver control structure.  This is where
- * defaults are set for people who don't have a config file.*/
-
-static struct qcam_device *qcam_init(struct parport *port)
-{
-        struct qcam_device *q;
-
-        q = kmalloc(sizeof(struct qcam_device), GFP_KERNEL);
-        if (q == NULL)
-                return NULL;
-
-        q->pport = port;
-        q->pdev = parport_register_device(port, "bw-qcam", NULL, NULL,
-                        NULL, 0, NULL);
-        if (q->pdev == NULL) {
-                printk(KERN_ERR "bw-qcam: couldn't register for %s.\n",
-                                port->name);
-                kfree(q);
-                return NULL;
-        }
-
-        memcpy(&q->vdev, &qcam_template, sizeof(qcam_template));
-
-        mutex_init(&q->lock);
-
-        q->port_mode = (QC_ANY | QC_NOTSET);
-        q->width = 320;
-        q->height = 240;
-        q->bpp = 4;
-        q->transfer_scale = 2;
-        q->contrast = 192;
-        q->brightness = 180;
-        q->whitebal = 105;
-        q->top = 1;
-        q->left = 14;
-        q->mode = -1;
-        q->status = QC_PARAM_CHANGE;
-        return q;
-}
-
-
-/* qc_command is probably a bit of a misnomer -- it's used to send
- * bytes *to* the camera.  Generally, these bytes are either commands
- * or arguments to commands, so the name fits, but it still bugs me a
- * bit.  See the documentation for a list of commands. */
-
-static int qc_command(struct qcam_device *q, int command)
-{
-        int n1, n2;
-        int cmd;
-
-        write_lpdata(q, command);
-        write_lpcontrol(q, 6);
-
-        n1 = qc_waithand(q, 1);
-
-        write_lpcontrol(q, 0xe);
-        n2 = qc_waithand(q, 0);
-
-        cmd = (n1 & 0xf0) | ((n2 & 0xf0) >> 4);
-        return cmd;
-}
-
-static int qc_readparam(struct qcam_device *q)
-{
-        int n1, n2;
-        int cmd;
-
-        write_lpcontrol(q, 6);
-        n1 = qc_waithand(q, 1);
-
-        write_lpcontrol(q, 0xe);
-        n2 = qc_waithand(q, 0);
-
-        cmd = (n1 & 0xf0) | ((n2 & 0xf0) >> 4);
-        return cmd;
-}
 
 /* qc_waithand busy-waits for a handshake signal from the QuickCam.
  * Almost all communication with the camera requires handshaking. */
 
-static int qc_waithand(struct qcam_device *q, int val)
+static int qc_waithand(struct qcam *q, int val)
 {
         int status;
         int runs = 0;
@@ -286,7 +214,7 @@ static int qc_waithand(struct qcam_device *q, int val)
  * (bit 3 of status register).  It also returns the last value read,
  * since this data is useful. */
 
-static unsigned int qc_waithand2(struct qcam_device *q, int val)
+static unsigned int qc_waithand2(struct qcam *q, int val)
 {
         unsigned int status;
         int runs = 0;
@@ -309,6 +237,43 @@ static unsigned int qc_waithand2(struct qcam_device *q, int val)
         return status;
 }
 
+/* qc_command is probably a bit of a misnomer -- it's used to send
+ * bytes *to* the camera.  Generally, these bytes are either commands
+ * or arguments to commands, so the name fits, but it still bugs me a
+ * bit.  See the documentation for a list of commands. */
+
+static int qc_command(struct qcam *q, int command)
+{
+        int n1, n2;
+        int cmd;
+
+        write_lpdata(q, command);
+        write_lpcontrol(q, 6);
+
+        n1 = qc_waithand(q, 1);
+
+        write_lpcontrol(q, 0xe);
+        n2 = qc_waithand(q, 0);
+
+        cmd = (n1 & 0xf0) | ((n2 & 0xf0) >> 4);
+        return cmd;
+}
+
+static int qc_readparam(struct qcam *q)
+{
+        int n1, n2;
+        int cmd;
+
+        write_lpcontrol(q, 6);
+        n1 = qc_waithand(q, 1);
+
+        write_lpcontrol(q, 0xe);
+        n2 = qc_waithand(q, 0);
+
+        cmd = (n1 & 0xf0) | ((n2 & 0xf0) >> 4);
+        return cmd;
+}
+
 
 /* Try to detect a QuickCam.  It appears to flash the upper 4 bits of
    the status register at 5-10 Hz.  This is only used in the autoprobe
@@ -317,7 +282,7 @@ static unsigned int qc_waithand2(struct qcam_device *q, int val)
    almost completely safe, while their method screws up my printer if
    I plug it in before the camera. */
 
-static int qc_detect(struct qcam_device *q)
+static int qc_detect(struct qcam *q)
 {
         int reg, lastreg;
         int count = 0;
@@ -358,41 +323,6 @@ static int qc_detect(struct qcam_device *q)
         }
 }
 
-
-/* Reset the QuickCam.  This uses the same sequence the Windows
- * QuickPic program uses.  Someone with a bi-directional port should
- * check that bi-directional mode is detected right, and then
- * implement bi-directional mode in qc_readbyte(). */
-
-static void qc_reset(struct qcam_device *q)
-{
-        switch (q->port_mode & QC_FORCE_MASK) {
-        case QC_FORCE_UNIDIR:
-                q->port_mode = (q->port_mode & ~QC_MODE_MASK) | QC_UNIDIR;
-                break;
-
-        case QC_FORCE_BIDIR:
-                q->port_mode = (q->port_mode & ~QC_MODE_MASK) | QC_BIDIR;
-                break;
-
-        case QC_ANY:
-                write_lpcontrol(q, 0x20);
-                write_lpdata(q, 0x75);
-
-                if (read_lpdata(q) != 0x75)
-                        q->port_mode = (q->port_mode & ~QC_MODE_MASK) | QC_BIDIR;
-                else
-                        q->port_mode = (q->port_mode & ~QC_MODE_MASK) | QC_UNIDIR;
-                break;
-        }
-
-        write_lpcontrol(q, 0xb);
-        udelay(250);
-        write_lpcontrol(q, 0xe);
-        qc_setscanmode(q);              /* in case port_mode changed */
-}
-
-
 /* Decide which scan mode to use.  There's no real requirement that
  * the scanmode match the resolution in q->height and q-> width -- the
  * camera takes the picture at the resolution specified in the
@@ -402,7 +332,7 @@ static void qc_reset(struct qcam_device *q)
  * returned.  If the scan is smaller, then the rest of the image
  * returned contains garbage. */
 
-static int qc_setscanmode(struct qcam_device *q)
+static int qc_setscanmode(struct qcam *q)
 {
         int old_mode = q->mode;
 
@@ -442,10 +372,45 @@ static int qc_setscanmode(struct qcam_device *q)
 }
 
 
+/* Reset the QuickCam.  This uses the same sequence the Windows
+ * QuickPic program uses.  Someone with a bi-directional port should
+ * check that bi-directional mode is detected right, and then
+ * implement bi-directional mode in qc_readbyte(). */
+
+static void qc_reset(struct qcam *q)
+{
+        switch (q->port_mode & QC_FORCE_MASK) {
+        case QC_FORCE_UNIDIR:
+                q->port_mode = (q->port_mode & ~QC_MODE_MASK) | QC_UNIDIR;
+                break;
+
+        case QC_FORCE_BIDIR:
+                q->port_mode = (q->port_mode & ~QC_MODE_MASK) | QC_BIDIR;
+                break;
+
+        case QC_ANY:
+                write_lpcontrol(q, 0x20);
+                write_lpdata(q, 0x75);
+
+                if (read_lpdata(q) != 0x75)
+                        q->port_mode = (q->port_mode & ~QC_MODE_MASK) | QC_BIDIR;
+                else
+                        q->port_mode = (q->port_mode & ~QC_MODE_MASK) | QC_UNIDIR;
+                break;
+        }
+
+        write_lpcontrol(q, 0xb);
+        udelay(250);
+        write_lpcontrol(q, 0xe);
+        qc_setscanmode(q);              /* in case port_mode changed */
+}
+
+
+
 /* Reset the QuickCam and program for brightness, contrast,
  * white-balance, and resolution. */
 
-static void qc_set(struct qcam_device *q)
+static void qc_set(struct qcam *q)
 {
         int val;
         int val2;
@@ -499,7 +464,7 @@ static void qc_set(struct qcam_device *q)
    the supplied buffer.  It returns the number of bytes read,
    or -1 on error. */
 
-static inline int qc_readbytes(struct qcam_device *q, char buffer[])
+static inline int qc_readbytes(struct qcam *q, char buffer[])
 {
         int ret = 1;
         unsigned int hi, lo;
@@ -590,7 +555,7 @@ static inline int qc_readbytes(struct qcam_device *q, char buffer[])
  * n=2^(bit depth)-1.  Ask me for more details if you don't understand
  * this. */
 
-static long qc_capture(struct qcam_device *q, char __user *buf, unsigned long len)
+static long qc_capture(struct qcam *q, char __user *buf, unsigned long len)
 {
         int i, j, k, yield;
         int bytes;
@@ -674,171 +639,206 @@ static long qc_capture(struct qcam_device *q, char __user *buf, unsigned long le
  *      Video4linux interfacing
  */
 
-static long qcam_do_ioctl(struct file *file, unsigned int cmd, void *arg)
+static int qcam_querycap(struct file *file, void  *priv,
+                                        struct v4l2_capability *vcap)
 {
-        struct video_device *dev = video_devdata(file);
-        struct qcam_device *qcam = (struct qcam_device *)dev;
-
-        switch (cmd) {
-        case VIDIOCGCAP:
-                {
-                        struct video_capability *b = arg;
-                        strcpy(b->name, "Quickcam");
-                        b->type = VID_TYPE_CAPTURE|VID_TYPE_SCALES|VID_TYPE_MONOCHROME;
-                        b->channels = 1;
-                        b->audios = 0;
-                        b->maxwidth = 320;
-                        b->maxheight = 240;
-                        b->minwidth = 80;
-                        b->minheight = 60;
-                        return 0;
-                }
-        case VIDIOCGCHAN:
-                {
-                        struct video_channel *v = arg;
-                        if (v->channel != 0)
-                                return -EINVAL;
-                        v->flags = 0;
-                        v->tuners = 0;
-                        /* Good question.. its composite or SVHS so.. */
-                        v->type = VIDEO_TYPE_CAMERA;
-                        strcpy(v->name, "Camera");
-                        return 0;
-                }
-        case VIDIOCSCHAN:
-                {
-                        struct video_channel *v = arg;
-                        if (v->channel != 0)
-                                return -EINVAL;
-                        return 0;
-                }
-        case VIDIOCGTUNER:
-                {
-                        struct video_tuner *v = arg;
-                        if (v->tuner)
-                                return -EINVAL;
-                        strcpy(v->name, "Format");
-                        v->rangelow = 0;
-                        v->rangehigh = 0;
-                        v->flags = 0;
-                        v->mode = VIDEO_MODE_AUTO;
-                        return 0;
-                }
-        case VIDIOCSTUNER:
-                {
-                        struct video_tuner *v = arg;
-                        if (v->tuner)
-                                return -EINVAL;
-                        if (v->mode != VIDEO_MODE_AUTO)
-                                return -EINVAL;
-                        return 0;
-                }
-        case VIDIOCGPICT:
-                {
-                        struct video_picture *p = arg;
-                        p->colour = 0x8000;
-                        p->hue = 0x8000;
-                        p->brightness = qcam->brightness << 8;
-                        p->contrast = qcam->contrast << 8;
-                        p->whiteness = qcam->whitebal << 8;
-                        p->depth = qcam->bpp;
-                        p->palette = VIDEO_PALETTE_GREY;
-                        return 0;
-                }
-        case VIDIOCSPICT:
-                {
-                        struct video_picture *p = arg;
-                        if (p->palette != VIDEO_PALETTE_GREY)
-                                return -EINVAL;
-                        if (p->depth != 4 && p->depth != 6)
-                                return -EINVAL;
-
-                        /*
-                         *      Now load the camera.
-                         */
-
-                        qcam->brightness = p->brightness >> 8;
-                        qcam->contrast = p->contrast >> 8;
-                        qcam->whitebal = p->whiteness >> 8;
-                        qcam->bpp = p->depth;
-
-                        mutex_lock(&qcam->lock);
-                        qc_setscanmode(qcam);
-                        mutex_unlock(&qcam->lock);
-                        qcam->status |= QC_PARAM_CHANGE;
+        struct qcam *qcam = video_drvdata(file);
 
-                        return 0;
-                }
-        case VIDIOCSWIN:
-                {
-                        struct video_window *vw = arg;
-                        if (vw->flags)
-                                return -EINVAL;
-                        if (vw->clipcount)
-                                return -EINVAL;
-                        if (vw->height < 60 || vw->height > 240)
-                                return -EINVAL;
-                        if (vw->width < 80 || vw->width > 320)
-                                return -EINVAL;
-
-                        qcam->width = 320;
-                        qcam->height = 240;
-                        qcam->transfer_scale = 4;
-
-                        if (vw->width >= 160 && vw->height >= 120)
-                                qcam->transfer_scale = 2;
-                        if (vw->width >= 320 && vw->height >= 240) {
-                                qcam->width = 320;
-                                qcam->height = 240;
-                                qcam->transfer_scale = 1;
-                        }
-                        mutex_lock(&qcam->lock);
-                        qc_setscanmode(qcam);
-                        mutex_unlock(&qcam->lock);
+        strlcpy(vcap->driver, qcam->v4l2_dev.name, sizeof(vcap->driver));
+        strlcpy(vcap->card, "B&W Quickcam", sizeof(vcap->card));
+        strlcpy(vcap->bus_info, "parport", sizeof(vcap->bus_info));
+        vcap->version = KERNEL_VERSION(0, 0, 2);
+        vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+        return 0;
+}
 
-                        /* We must update the camera before we grab. We could
-                           just have changed the grab size */
-                        qcam->status |= QC_PARAM_CHANGE;
+static int qcam_enum_input(struct file *file, void *fh, struct v4l2_input *vin)
+{
+        if (vin->index > 0)
+                return -EINVAL;
+        strlcpy(vin->name, "Camera", sizeof(vin->name));
+        vin->type = V4L2_INPUT_TYPE_CAMERA;
+        vin->audioset = 0;
+        vin->tuner = 0;
+        vin->std = 0;
+        vin->status = 0;
+        return 0;
+}
 
-                        /* Ok we figured out what to use from our wide choice */
-                        return 0;
-                }
-        case VIDIOCGWIN:
-                {
-                        struct video_window *vw = arg;
+static int qcam_g_input(struct file *file, void *fh, unsigned int *inp)
+{
+        *inp = 0;
+        return 0;
+}
 
-                        memset(vw, 0, sizeof(*vw));
-                        vw->width = qcam->width / qcam->transfer_scale;
-                        vw->height = qcam->height / qcam->transfer_scale;
-                        return 0;
-                }
-        case VIDIOCKEY:
-                return 0;
-        case VIDIOCCAPTURE:
-        case VIDIOCGFBUF:
-        case VIDIOCSFBUF:
-        case VIDIOCGFREQ:
-        case VIDIOCSFREQ:
-        case VIDIOCGAUDIO:
-        case VIDIOCSAUDIO:
-                return -EINVAL;
+static int qcam_s_input(struct file *file, void *fh, unsigned int inp)
+{
+        return (inp > 0) ? -EINVAL : 0;
+}
+
+static int qcam_queryctrl(struct file *file, void *priv,
+                                        struct v4l2_queryctrl *qc)
+{
+        switch (qc->id) {
+        case V4L2_CID_BRIGHTNESS:
+                return v4l2_ctrl_query_fill(qc, 0, 255, 1, 180);
+        case V4L2_CID_CONTRAST:
+                return v4l2_ctrl_query_fill(qc, 0, 255, 1, 192);
+        case V4L2_CID_GAMMA:
+                return v4l2_ctrl_query_fill(qc, 0, 255, 1, 105);
+        }
+        return -EINVAL;
+}
+
+static int qcam_g_ctrl(struct file *file, void *priv,
+                                        struct v4l2_control *ctrl)
+{
+        struct qcam *qcam = video_drvdata(file);
+        int ret = 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_BRIGHTNESS:
+                ctrl->value = qcam->brightness;
+                break;
+        case V4L2_CID_CONTRAST:
+                ctrl->value = qcam->contrast;
+                break;
+        case V4L2_CID_GAMMA:
+                ctrl->value = qcam->whitebal;
+                break;
         default:
-                return -ENOIOCTLCMD;
+                ret = -EINVAL;
+                break;
         }
+        return ret;
+}
+
+static int qcam_s_ctrl(struct file *file, void *priv,
+                                        struct v4l2_control *ctrl)
+{
+        struct qcam *qcam = video_drvdata(file);
+        int ret = 0;
+
+        mutex_lock(&qcam->lock);
+        switch (ctrl->id) {
+        case V4L2_CID_BRIGHTNESS:
+                qcam->brightness = ctrl->value;
+                break;
+        case V4L2_CID_CONTRAST:
+                qcam->contrast = ctrl->value;
+                break;
+        case V4L2_CID_GAMMA:
+                qcam->whitebal = ctrl->value;
+                break;
+        default:
+                ret = -EINVAL;
+                break;
+        }
+        if (ret == 0) {
+                qc_setscanmode(qcam);
+                qcam->status |= QC_PARAM_CHANGE;
+        }
+        mutex_unlock(&qcam->lock);
+        return ret;
+}
+
+static int qcam_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+        struct qcam *qcam = video_drvdata(file);
+        struct v4l2_pix_format *pix = &fmt->fmt.pix;
+
+        pix->width = qcam->width / qcam->transfer_scale;
+        pix->height = qcam->height / qcam->transfer_scale;
+        pix->pixelformat = (qcam->bpp == 4) ? V4L2_PIX_FMT_Y4 : V4L2_PIX_FMT_Y6;
+        pix->field = V4L2_FIELD_NONE;
+        pix->bytesperline = qcam->width;
+        pix->sizeimage = qcam->width * qcam->height;
+        /* Just a guess */
+        pix->colorspace = V4L2_COLORSPACE_SRGB;
+        return 0;
+}
+
+static int qcam_try_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+        struct v4l2_pix_format *pix = &fmt->fmt.pix;
+
+        if (pix->height <= 60 || pix->width <= 80) {
+                pix->height = 60;
+                pix->width = 80;
+        } else if (pix->height <= 120 || pix->width <= 160) {
+                pix->height = 120;
+                pix->width = 160;
+        } else {
+                pix->height = 240;
+                pix->width = 320;
+        }
+        if (pix->pixelformat != V4L2_PIX_FMT_Y4 &&
+            pix->pixelformat != V4L2_PIX_FMT_Y6)
+                pix->pixelformat = V4L2_PIX_FMT_Y4;
+        pix->field = V4L2_FIELD_NONE;
+        pix->bytesperline = pix->width;
+        pix->sizeimage = pix->width * pix->height;
+        /* Just a guess */
+        pix->colorspace = V4L2_COLORSPACE_SRGB;
+        return 0;
+}
+
+static int qcam_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+        struct qcam *qcam = video_drvdata(file);
+        struct v4l2_pix_format *pix = &fmt->fmt.pix;
+        int ret = qcam_try_fmt_vid_cap(file, fh, fmt);
+
+        if (ret)
+                return ret;
+        qcam->width = 320;
+        qcam->height = 240;
+        if (pix->height == 60)
+                qcam->transfer_scale = 4;
+        else if (pix->height == 120)
+                qcam->transfer_scale = 2;
+        else
+                qcam->transfer_scale = 1;
+        if (pix->pixelformat == V4L2_PIX_FMT_Y6)
+                qcam->bpp = 6;
+        else
+                qcam->bpp = 4;
+
+        mutex_lock(&qcam->lock);
+        qc_setscanmode(qcam);
+        /* We must update the camera before we grab. We could
+           just have changed the grab size */
+        qcam->status |= QC_PARAM_CHANGE;
+        mutex_unlock(&qcam->lock);
         return 0;
 }
 
-static long qcam_ioctl(struct file *file,
-                unsigned int cmd, unsigned long arg)
+static int qcam_enum_fmt_vid_cap(struct file *file, void *fh, struct v4l2_fmtdesc *fmt)
 {
-        return video_usercopy(file, cmd, arg, qcam_do_ioctl);
+        static struct v4l2_fmtdesc formats[] = {
+                { 0, 0, 0,
+                  "4-Bit Monochrome", V4L2_PIX_FMT_Y4,
+                  { 0, 0, 0, 0 }
+                },
+                { 0, 0, 0,
+                  "6-Bit Monochrome", V4L2_PIX_FMT_Y6,
+                  { 0, 0, 0, 0 }
+                },
+        };
+        enum v4l2_buf_type type = fmt->type;
+
+        if (fmt->index > 1)
+                return -EINVAL;
+
+        *fmt = formats[fmt->index];
+        fmt->type = type;
+        return 0;
 }
 
 static ssize_t qcam_read(struct file *file, char __user *buf,
                 size_t count, loff_t *ppos)
 {
-        struct video_device *v = video_devdata(file);
-        struct qcam_device *qcam = (struct qcam_device *)v;
+        struct qcam *qcam = video_drvdata(file);
         int len;
         parport_claim_or_block(qcam->pdev);
 
@@ -858,43 +858,112 @@ static ssize_t qcam_read(struct file *file, char __user *buf,
         return len;
 }
 
-static int qcam_exclusive_open(struct file *file)
+static const struct v4l2_file_operations qcam_fops = {
+        .owner          = THIS_MODULE,
+        .ioctl          = video_ioctl2,
+        .read           = qcam_read,
+};
+
+static const struct v4l2_ioctl_ops qcam_ioctl_ops = {
+        .vidioc_querycap                    = qcam_querycap,
+        .vidioc_g_input                     = qcam_g_input,
+        .vidioc_s_input                     = qcam_s_input,
+        .vidioc_enum_input                  = qcam_enum_input,
+        .vidioc_queryctrl                   = qcam_queryctrl,
+        .vidioc_g_ctrl                      = qcam_g_ctrl,
+        .vidioc_s_ctrl                      = qcam_s_ctrl,
+        .vidioc_enum_fmt_vid_cap            = qcam_enum_fmt_vid_cap,
+        .vidioc_g_fmt_vid_cap               = qcam_g_fmt_vid_cap,
+        .vidioc_s_fmt_vid_cap               = qcam_s_fmt_vid_cap,
+        .vidioc_try_fmt_vid_cap             = qcam_try_fmt_vid_cap,
+};
+
+/* Initialize the QuickCam driver control structure.  This is where
+ * defaults are set for people who don't have a config file.*/
+
+static struct qcam *qcam_init(struct parport *port)
 {
-        struct video_device *dev = video_devdata(file);
-        struct qcam_device *qcam = (struct qcam_device *)dev;
+        struct qcam *qcam;
+        struct v4l2_device *v4l2_dev;
+
+        qcam = kzalloc(sizeof(struct qcam), GFP_KERNEL);
+        if (qcam == NULL)
+                return NULL;
+
+        v4l2_dev = &qcam->v4l2_dev;
+        strlcpy(v4l2_dev->name, "bw-qcam", sizeof(v4l2_dev->name));
 
-        return test_and_set_bit(0, &qcam->in_use) ? -EBUSY : 0;
+        if (v4l2_device_register(NULL, v4l2_dev) < 0) {
+                v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
+                return NULL;
+        }
+
+        qcam->pport = port;
+        qcam->pdev = parport_register_device(port, "bw-qcam", NULL, NULL,
+                        NULL, 0, NULL);
+        if (qcam->pdev == NULL) {
+                v4l2_err(v4l2_dev, "couldn't register for %s.\n", port->name);
+                kfree(qcam);
+                return NULL;
+        }
+
+        strlcpy(qcam->vdev.name, "Connectix QuickCam", sizeof(qcam->vdev.name));
+        qcam->vdev.v4l2_dev = v4l2_dev;
+        qcam->vdev.fops = &qcam_fops;
+        qcam->vdev.ioctl_ops = &qcam_ioctl_ops;
+        qcam->vdev.release = video_device_release_empty;
+        video_set_drvdata(&qcam->vdev, qcam);
+
+        mutex_init(&qcam->lock);
+
+        qcam->port_mode = (QC_ANY | QC_NOTSET);
+        qcam->width = 320;
+        qcam->height = 240;
+        qcam->bpp = 4;
+        qcam->transfer_scale = 2;
+        qcam->contrast = 192;
+        qcam->brightness = 180;
+        qcam->whitebal = 105;
+        qcam->top = 1;
+        qcam->left = 14;
+        qcam->mode = -1;
+        qcam->status = QC_PARAM_CHANGE;
+        return qcam;
 }
 
-static int qcam_exclusive_release(struct file *file)
+static int qc_calibrate(struct qcam *q)
 {
-        struct video_device *dev = video_devdata(file);
-        struct qcam_device *qcam = (struct qcam_device *)dev;
+        /*
+         *      Bugfix by Hanno Mueller hmueller@kabel.de, Mai 21 96
+         *      The white balance is an individual value for each
+         *      quickcam.
+         */
 
-        clear_bit(0, &qcam->in_use);
-        return 0;
-}
+        int value;
+        int count = 0;
 
-static const struct v4l2_file_operations qcam_fops = {
-        .owner          = THIS_MODULE,
-        .open           = qcam_exclusive_open,
-        .release        = qcam_exclusive_release,
-        .ioctl          = qcam_ioctl,
-        .read           = qcam_read,
-};
-static struct video_device qcam_template = {
-        .name           = "Connectix Quickcam",
-        .fops           = &qcam_fops,
-        .release        = video_device_release_empty,
-};
+        qc_command(q, 27);      /* AutoAdjustOffset */
+        qc_command(q, 0);       /* Dummy Parameter, ignored by the camera */
 
-#define MAX_CAMS 4
-static struct qcam_device *qcams[MAX_CAMS];
-static unsigned int num_cams;
+        /* GetOffset (33) will read 255 until autocalibration */
+        /* is finished. After that, a value of 1-254 will be */
+        /* returned. */
+
+        do {
+                qc_command(q, 33);
+                value = qc_readparam(q);
+                mdelay(1);
+                schedule();
+                count++;
+        } while (value == 0xff && count < 2048);
+
+        q->whitebal = value;
+        return value;
+}
 
 static int init_bwqcam(struct parport *port)
 {
-        struct qcam_device *qcam;
+        struct qcam *qcam;
 
         if (num_cams == MAX_CAMS) {
                 printk(KERN_ERR "Too many Quickcams (max %d)\n", MAX_CAMS);
@@ -919,7 +988,7 @@ static int init_bwqcam(struct parport *port)
 
         parport_release(qcam->pdev);
 
-        printk(KERN_INFO "Connectix Quickcam on %s\n", qcam->pport->name);
+        v4l2_info(&qcam->v4l2_dev, "Connectix Quickcam on %s\n", qcam->pport->name);
 
         if (video_register_device(&qcam->vdev, VFL_TYPE_GRABBER, video_nr) < 0) {
                 parport_unregister_device(qcam->pdev);
@@ -932,7 +1001,7 @@ static int init_bwqcam(struct parport *port)
         return 0;
 }
 
-static void close_bwqcam(struct qcam_device *qcam)
+static void close_bwqcam(struct qcam *qcam)
 {
         video_unregister_device(&qcam->vdev);
         parport_unregister_device(qcam->pdev);
@@ -983,7 +1052,7 @@ static void bwqcam_detach(struct parport *port)
 {
         int i;
         for (i = 0; i < num_cams; i++) {
-                struct qcam_device *qcam = qcams[i];
+                struct qcam *qcam = qcams[i];
                 if (qcam && qcam->pdev->port == port) {
                         qcams[i] = NULL;
                         close_bwqcam(qcam);
diff --git a/drivers/media/video/bw-qcam.h b/drivers/media/video/bw-qcam.h
deleted file mode 100644
index 8a60c5de0935..000000000000
--- a/drivers/media/video/bw-qcam.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- *      Video4Linux bw-qcam driver
- *
- *      Derived from code..
- */
-
-/******************************************************************
-
-Copyright (C) 1996 by Scott Laird
-
-Permission is hereby granted, free of charge, to any person obtaining
-a copy of this software and associated documentation files (the
-"Software"), to deal in the Software without restriction, including
-without limitation the rights to use, copy, modify, merge, publish,
-distribute, sublicense, and/or sell copies of the Software, and to
-permit persons to whom the Software is furnished to do so, subject to
-the following conditions:
-
-The above copyright notice and this permission notice shall be
-included in all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
-EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
-MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
-IN NO EVENT SHALL SCOTT LAIRD BE LIABLE FOR ANY CLAIM, DAMAGES OR
-OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
-ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
-OTHER DEALINGS IN THE SOFTWARE.
-
-******************************************************************/
-
-/* One from column A... */
-#define QC_NOTSET 0
-#define QC_UNIDIR 1
-#define QC_BIDIR  2
-#define QC_SERIAL 3
-
-/* ... and one from column B */
-#define QC_ANY          0x00
-#define QC_FORCE_UNIDIR 0x10
-#define QC_FORCE_BIDIR  0x20
-#define QC_FORCE_SERIAL 0x30
-/* in the port_mode member */
-
-#define QC_MODE_MASK    0x07
-#define QC_FORCE_MASK   0x70
-
-#define MAX_HEIGHT 243
-#define MAX_WIDTH 336
-
-/* Bit fields for status flags */
-#define QC_PARAM_CHANGE 0x01 /* Camera status change has occurred */
-
-struct qcam_device {
-        struct video_device vdev;
-        struct pardevice *pdev;
-        struct parport *pport;
-        struct mutex lock;
-        int width, height;
-        int bpp;
-        int mode;
-        int contrast, brightness, whitebal;
-        int port_mode;
-        int transfer_scale;
-        int top, left;
-        int status;
-        unsigned int saved_bits;
-        unsigned long in_use;
-};
diff --git a/drivers/media/video/c-qcam.c b/drivers/media/video/c-qcam.c
index 8f1dd88b32a6..6e4b19698c13 100644
--- a/drivers/media/video/c-qcam.c
+++ b/drivers/media/video/c-qcam.c
@@ -33,15 +33,17 @@
 #include <linux/mm.h>
 #include <linux/parport.h>
 #include <linux/sched.h>
-#include <linux/videodev.h>
-#include <media/v4l2-common.h>
-#include <media/v4l2-ioctl.h>
 #include <linux/mutex.h>
 #include <linux/jiffies.h>
-
+#include <linux/version.h>
+#include <linux/videodev2.h>
 #include <asm/uaccess.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
 
-struct qcam_device {
+struct qcam {
+        struct v4l2_device v4l2_dev;
         struct video_device vdev;
         struct pardevice *pdev;
         struct parport *pport;
@@ -51,7 +53,6 @@ struct qcam_device {
         int contrast, brightness, whitebal;
         int top, left;
         unsigned int bidirectional;
-        unsigned long in_use;
         struct mutex lock;
 };
 
@@ -68,33 +69,45 @@ struct qcam_device {
 #define QC_DECIMATION_2         2
 #define QC_DECIMATION_4         4
 
-#define BANNER "Colour QuickCam for Video4Linux v0.05"
+#define BANNER "Colour QuickCam for Video4Linux v0.06"
 
 static int parport[MAX_CAMS] = { [1 ... MAX_CAMS-1] = -1 };
 static int probe = 2;
 static int force_rgb;
 static int video_nr = -1;
 
-static inline void qcam_set_ack(struct qcam_device *qcam, unsigned int i)
+/* FIXME: parport=auto would never have worked, surely? --RR */
+MODULE_PARM_DESC(parport, "parport=<auto|n[,n]...> for port detection method\n"
+                          "probe=<0|1|2> for camera detection method\n"
+                          "force_rgb=<0|1> for RGB data format (default BGR)");
+module_param_array(parport, int, NULL, 0);
+module_param(probe, int, 0);
+module_param(force_rgb, bool, 0);
+module_param(video_nr, int, 0);
+
+static struct qcam *qcams[MAX_CAMS];
+static unsigned int num_cams;
+
+static inline void qcam_set_ack(struct qcam *qcam, unsigned int i)
 {
         /* note: the QC specs refer to the PCAck pin by voltage, not
            software level.  PC ports have builtin inverters. */
         parport_frob_control(qcam->pport, 8, i ? 8 : 0);
 }
 
-static inline unsigned int qcam_ready1(struct qcam_device *qcam)
+static inline unsigned int qcam_ready1(struct qcam *qcam)
 {
         return (parport_read_status(qcam->pport) & 0x8) ? 1 : 0;
 }
 
-static inline unsigned int qcam_ready2(struct qcam_device *qcam)
+static inline unsigned int qcam_ready2(struct qcam *qcam)
 {
         return (parport_read_data(qcam->pport) & 0x1) ? 1 : 0;
 }
 
-static unsigned int qcam_await_ready1(struct qcam_device *qcam,
-                                             int value)
+static unsigned int qcam_await_ready1(struct qcam *qcam, int value)
 {
+        struct v4l2_device *v4l2_dev = &qcam->v4l2_dev;
         unsigned long oldjiffies = jiffies;
         unsigned int i;
 
@@ -112,14 +125,15 @@ static unsigned int qcam_await_ready1(struct qcam_device *qcam,
         }
 
         /* Probably somebody pulled the plug out.  Not much we can do. */
-        printk(KERN_ERR "c-qcam: ready1 timeout (%d) %x %x\n", value,
+        v4l2_err(v4l2_dev, "ready1 timeout (%d) %x %x\n", value,
                parport_read_status(qcam->pport),
                parport_read_control(qcam->pport));
         return 1;
 }
 
-static unsigned int qcam_await_ready2(struct qcam_device *qcam, int value)
+static unsigned int qcam_await_ready2(struct qcam *qcam, int value)
 {
+        struct v4l2_device *v4l2_dev = &qcam->v4l2_dev;
         unsigned long oldjiffies = jiffies;
         unsigned int i;
 
@@ -137,14 +151,14 @@ static unsigned int qcam_await_ready2(struct qcam_device *qcam, int value)
         }
 
         /* Probably somebody pulled the plug out.  Not much we can do. */
-        printk(KERN_ERR "c-qcam: ready2 timeout (%d) %x %x %x\n", value,
+        v4l2_err(v4l2_dev, "ready2 timeout (%d) %x %x %x\n", value,
                parport_read_status(qcam->pport),
                parport_read_control(qcam->pport),
                parport_read_data(qcam->pport));
         return 1;
 }
 
-static int qcam_read_data(struct qcam_device *qcam)
+static int qcam_read_data(struct qcam *qcam)
 {
         unsigned int idata;
 
@@ -159,21 +173,22 @@ static int qcam_read_data(struct qcam_device *qcam)
         return idata;
 }
 
-static int qcam_write_data(struct qcam_device *qcam, unsigned int data)
+static int qcam_write_data(struct qcam *qcam, unsigned int data)
 {
+        struct v4l2_device *v4l2_dev = &qcam->v4l2_dev;
         unsigned int idata;
 
         parport_write_data(qcam->pport, data);
         idata = qcam_read_data(qcam);
         if (data != idata) {
-                printk(KERN_WARNING "cqcam: sent %x but received %x\n", data,
+                v4l2_warn(v4l2_dev, "sent %x but received %x\n", data,
                        idata);
                 return 1;
         }
         return 0;
 }
 
-static inline int qcam_set(struct qcam_device *qcam, unsigned int cmd, unsigned int data)
+static inline int qcam_set(struct qcam *qcam, unsigned int cmd, unsigned int data)
 {
         if (qcam_write_data(qcam, cmd))
                 return -1;
@@ -182,14 +197,14 @@ static inline int qcam_set(struct qcam_device *qcam, unsigned int cmd, unsigned
         return 0;
 }
 
-static inline int qcam_get(struct qcam_device *qcam, unsigned int cmd)
+static inline int qcam_get(struct qcam *qcam, unsigned int cmd)
 {
         if (qcam_write_data(qcam, cmd))
                 return -1;
         return qcam_read_data(qcam);
 }
 
-static int qc_detect(struct qcam_device *qcam)
+static int qc_detect(struct qcam *qcam)
 {
         unsigned int stat, ostat, i, count = 0;
 
@@ -246,7 +261,7 @@ static int qc_detect(struct qcam_device *qcam)
         return 0;
 }
 
-static void qc_reset(struct qcam_device *qcam)
+static void qc_reset(struct qcam *qcam)
 {
         parport_write_control(qcam->pport, 0xc);
         parport_write_control(qcam->pport, 0x8);
@@ -258,55 +273,55 @@ static void qc_reset(struct qcam_device *qcam)
 /* Reset the QuickCam and program for brightness, contrast,
  * white-balance, and resolution. */
 
-static void qc_setup(struct qcam_device *q)
+static void qc_setup(struct qcam *qcam)
 {
-        qc_reset(q);
+        qc_reset(qcam);
 
         /* Set the brightness. */
-        qcam_set(q, 11, q->brightness);
+        qcam_set(qcam, 11, qcam->brightness);
 
         /* Set the height and width.  These refer to the actual
            CCD area *before* applying the selected decimation.  */
-        qcam_set(q, 17, q->ccd_height);
-        qcam_set(q, 19, q->ccd_width / 2);
+        qcam_set(qcam, 17, qcam->ccd_height);
+        qcam_set(qcam, 19, qcam->ccd_width / 2);
 
         /* Set top and left.  */
-        qcam_set(q, 0xd, q->top);
-        qcam_set(q, 0xf, q->left);
+        qcam_set(qcam, 0xd, qcam->top);
+        qcam_set(qcam, 0xf, qcam->left);
 
         /* Set contrast and white balance.  */
-        qcam_set(q, 0x19, q->contrast);
-        qcam_set(q, 0x1f, q->whitebal);
+        qcam_set(qcam, 0x19, qcam->contrast);
+        qcam_set(qcam, 0x1f, qcam->whitebal);
 
         /* Set the speed.  */
-        qcam_set(q, 45, 2);
+        qcam_set(qcam, 45, 2);
 }
 
 /* Read some bytes from the camera and put them in the buffer.
    nbytes should be a multiple of 3, because bidirectional mode gives
    us three bytes at a time.  */
 
-static unsigned int qcam_read_bytes(struct qcam_device *q, unsigned char *buf, unsigned int nbytes)
+static unsigned int qcam_read_bytes(struct qcam *qcam, unsigned char *buf, unsigned int nbytes)
 {
         unsigned int bytes = 0;
 
-        qcam_set_ack(q, 0);
-        if (q->bidirectional) {
+        qcam_set_ack(qcam, 0);
+        if (qcam->bidirectional) {
                 /* It's a bidirectional port */
                 while (bytes < nbytes) {
                         unsigned int lo1, hi1, lo2, hi2;
                         unsigned char r, g, b;
 
-                        if (qcam_await_ready2(q, 1))
+                        if (qcam_await_ready2(qcam, 1))
                                 return bytes;
-                        lo1 = parport_read_data(q->pport) >> 1;
-                        hi1 = ((parport_read_status(q->pport) >> 3) & 0x1f) ^ 0x10;
-                        qcam_set_ack(q, 1);
-                        if (qcam_await_ready2(q, 0))
+                        lo1 = parport_read_data(qcam->pport) >> 1;
+                        hi1 = ((parport_read_status(qcam->pport) >> 3) & 0x1f) ^ 0x10;
+                        qcam_set_ack(qcam, 1);
+                        if (qcam_await_ready2(qcam, 0))
                                 return bytes;
-                        lo2 = parport_read_data(q->pport) >> 1;
-                        hi2 = ((parport_read_status(q->pport) >> 3) & 0x1f) ^ 0x10;
-                        qcam_set_ack(q, 0);
+                        lo2 = parport_read_data(qcam->pport) >> 1;
+                        hi2 = ((parport_read_status(qcam->pport) >> 3) & 0x1f) ^ 0x10;
+                        qcam_set_ack(qcam, 0);
                         r = lo1 | ((hi1 & 1) << 7);
                         g = ((hi1 & 0x1e) << 3) | ((hi2 & 0x1e) >> 1);
                         b = lo2 | ((hi2 & 1) << 7);
@@ -328,14 +343,14 @@ static unsigned int qcam_read_bytes(struct qcam_device *q, unsigned char *buf, u
                 while (bytes < nbytes) {
                         unsigned int hi, lo;
 
-                        if (qcam_await_ready1(q, 1))
+                        if (qcam_await_ready1(qcam, 1))
                                 return bytes;
-                        hi = (parport_read_status(q->pport) & 0xf0);
-                        qcam_set_ack(q, 1);
-                        if (qcam_await_ready1(q, 0))
+                        hi = (parport_read_status(qcam->pport) & 0xf0);
+                        qcam_set_ack(qcam, 1);
+                        if (qcam_await_ready1(qcam, 0))
                                 return bytes;
-                        lo = (parport_read_status(q->pport) & 0xf0);
-                        qcam_set_ack(q, 0);
+                        lo = (parport_read_status(qcam->pport) & 0xf0);
+                        qcam_set_ack(qcam, 0);
                         /* flip some bits */
                         rgb[(i = bytes++ % 3)] = (hi | (lo >> 4)) ^ 0x88;
                         if (i >= 2) {
@@ -361,10 +376,11 @@ get_fragment:
 
 #define BUFSZ   150
 
-static long qc_capture(struct qcam_device *q, char __user *buf, unsigned long len)
+static long qc_capture(struct qcam *qcam, char __user *buf, unsigned long len)
 {
+        struct v4l2_device *v4l2_dev = &qcam->v4l2_dev;
         unsigned lines, pixelsperline, bitsperxfer;
-        unsigned int is_bi_dir = q->bidirectional;
+        unsigned int is_bi_dir = qcam->bidirectional;
         size_t wantlen, outptr = 0;
         char tmpbuf[BUFSZ];
 
@@ -373,10 +389,10 @@ static long qc_capture(struct qcam_device *q, char __user *buf, unsigned long le
 
         /* Wait for camera to become ready */
         for (;;) {
-                int i = qcam_get(q, 41);
+                int i = qcam_get(qcam, 41);
 
                 if (i == -1) {
-                        qc_setup(q);
+                        qc_setup(qcam);
                         return -EIO;
                 }
                 if ((i & 0x80) == 0)
@@ -384,25 +400,25 @@ static long qc_capture(struct qcam_device *q, char __user *buf, unsigned long le
                 schedule();
         }
 
-        if (qcam_set(q, 7, (q->mode | (is_bi_dir ? 1 : 0)) + 1))
+        if (qcam_set(qcam, 7, (qcam->mode | (is_bi_dir ? 1 : 0)) + 1))
                 return -EIO;
 
-        lines = q->height;
-        pixelsperline = q->width;
+        lines = qcam->height;
+        pixelsperline = qcam->width;
         bitsperxfer = (is_bi_dir) ? 24 : 8;
 
         if (is_bi_dir) {
                 /* Turn the port around */
-                parport_data_reverse(q->pport);
+                parport_data_reverse(qcam->pport);
                 mdelay(3);
-                qcam_set_ack(q, 0);
-                if (qcam_await_ready1(q, 1)) {
-                        qc_setup(q);
+                qcam_set_ack(qcam, 0);
+                if (qcam_await_ready1(qcam, 1)) {
+                        qc_setup(qcam);
                         return -EIO;
                 }
-                qcam_set_ack(q, 1);
-                if (qcam_await_ready1(q, 0)) {
-                        qc_setup(q);
+                qcam_set_ack(qcam, 1);
+                if (qcam_await_ready1(qcam, 0)) {
+                        qc_setup(qcam);
                         return -EIO;
                 }
         }
@@ -413,7 +429,7 @@ static long qc_capture(struct qcam_device *q, char __user *buf, unsigned long le
                 size_t t, s;
 
                 s = (wantlen > BUFSZ) ? BUFSZ : wantlen;
-                t = qcam_read_bytes(q, tmpbuf, s);
+                t = qcam_read_bytes(qcam, tmpbuf, s);
                 if (outptr < len) {
                         size_t sz = len - outptr;
 
@@ -432,10 +448,10 @@ static long qc_capture(struct qcam_device *q, char __user *buf, unsigned long le
         len = outptr;
 
         if (wantlen) {
-                printk(KERN_ERR "qcam: short read.\n");
+                v4l2_err(v4l2_dev, "short read.\n");
                 if (is_bi_dir)
-                        parport_data_forward(q->pport);
-                qc_setup(q);
+                        parport_data_forward(qcam->pport);
+                qc_setup(qcam);
                 return len;
         }
 
@@ -443,49 +459,49 @@ static long qc_capture(struct qcam_device *q, char __user *buf, unsigned long le
                 int l;
 
                 do {
-                        l = qcam_read_bytes(q, tmpbuf, 3);
+                        l = qcam_read_bytes(qcam, tmpbuf, 3);
                         cond_resched();
                 } while (l && (tmpbuf[0] == 0x7e || tmpbuf[1] == 0x7e || tmpbuf[2] == 0x7e));
                 if (force_rgb) {
                         if (tmpbuf[0] != 0xe || tmpbuf[1] != 0x0 || tmpbuf[2] != 0xf)
-                                printk(KERN_ERR "qcam: bad EOF\n");
+                                v4l2_err(v4l2_dev, "bad EOF\n");
                 } else {
                         if (tmpbuf[0] != 0xf || tmpbuf[1] != 0x0 || tmpbuf[2] != 0xe)
-                                printk(KERN_ERR "qcam: bad EOF\n");
+                                v4l2_err(v4l2_dev, "bad EOF\n");
                 }
-                qcam_set_ack(q, 0);
-                if (qcam_await_ready1(q, 1)) {
-                        printk(KERN_ERR "qcam: no ack after EOF\n");
-                        parport_data_forward(q->pport);
-                        qc_setup(q);
+                qcam_set_ack(qcam, 0);
+                if (qcam_await_ready1(qcam, 1)) {
+                        v4l2_err(v4l2_dev, "no ack after EOF\n");
+                        parport_data_forward(qcam->pport);
+                        qc_setup(qcam);
                         return len;
                 }
-                parport_data_forward(q->pport);
+                parport_data_forward(qcam->pport);
                 mdelay(3);
-                qcam_set_ack(q, 1);
-                if (qcam_await_ready1(q, 0)) {
-                        printk(KERN_ERR "qcam: no ack to port turnaround\n");
-                        qc_setup(q);
+                qcam_set_ack(qcam, 1);
+                if (qcam_await_ready1(qcam, 0)) {
+                        v4l2_err(v4l2_dev, "no ack to port turnaround\n");
+                        qc_setup(qcam);
                         return len;
                 }
         } else {
                 int l;
 
                 do {
-                        l = qcam_read_bytes(q, tmpbuf, 1);
+                        l = qcam_read_bytes(qcam, tmpbuf, 1);
                         cond_resched();
                 } while (l && tmpbuf[0] == 0x7e);
-                l = qcam_read_bytes(q, tmpbuf + 1, 2);
+                l = qcam_read_bytes(qcam, tmpbuf + 1, 2);
                 if (force_rgb) {
                         if (tmpbuf[0] != 0xe || tmpbuf[1] != 0x0 || tmpbuf[2] != 0xf)
-                                printk(KERN_ERR "qcam: bad EOF\n");
+                                v4l2_err(v4l2_dev, "bad EOF\n");
                 } else {
                         if (tmpbuf[0] != 0xf || tmpbuf[1] != 0x0 || tmpbuf[2] != 0xe)
-                                printk(KERN_ERR "qcam: bad EOF\n");
+                                v4l2_err(v4l2_dev, "bad EOF\n");
                 }
         }
 
-        qcam_write_data(q, 0);
+        qcam_write_data(qcam, 0);
         return len;
 }
 
@@ -493,184 +509,202 @@ static long qc_capture(struct qcam_device *q, char __user *buf, unsigned long le
  *      Video4linux interfacing
  */
 
-static long qcam_do_ioctl(struct file *file, unsigned int cmd, void *arg)
+static int qcam_querycap(struct file *file, void  *priv,
+                                        struct v4l2_capability *vcap)
 {
-        struct video_device *dev = video_devdata(file);
-        struct qcam_device *qcam = (struct qcam_device *)dev;
+        struct qcam *qcam = video_drvdata(file);
 
-        switch (cmd) {
-        case VIDIOCGCAP:
-        {
-                struct video_capability *b = arg;
+        strlcpy(vcap->driver, qcam->v4l2_dev.name, sizeof(vcap->driver));
+        strlcpy(vcap->card, "Color Quickcam", sizeof(vcap->card));
+        strlcpy(vcap->bus_info, "parport", sizeof(vcap->bus_info));
+        vcap->version = KERNEL_VERSION(0, 0, 3);
+        vcap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE;
+        return 0;
+}
 
-                strcpy(b->name, "Quickcam");
-                b->type = VID_TYPE_CAPTURE | VID_TYPE_SCALES;
-                b->channels = 1;
-                b->audios = 0;
-                b->maxwidth = 320;
-                b->maxheight = 240;
-                b->minwidth = 80;
-                b->minheight = 60;
-                return 0;
-        }
-        case VIDIOCGCHAN:
-        {
-                struct video_channel *v = arg;
-
-                if (v->channel != 0)
-                        return -EINVAL;
-                v->flags = 0;
-                v->tuners = 0;
-                /* Good question.. its composite or SVHS so.. */
-                v->type = VIDEO_TYPE_CAMERA;
-                strcpy(v->name, "Camera");
-                return 0;
-        }
-        case VIDIOCSCHAN:
-        {
-                struct video_channel *v = arg;
+static int qcam_enum_input(struct file *file, void *fh, struct v4l2_input *vin)
+{
+        if (vin->index > 0)
+                return -EINVAL;
+        strlcpy(vin->name, "Camera", sizeof(vin->name));
+        vin->type = V4L2_INPUT_TYPE_CAMERA;
+        vin->audioset = 0;
+        vin->tuner = 0;
+        vin->std = 0;
+        vin->status = 0;
+        return 0;
+}
 
-                if (v->channel != 0)
-                        return -EINVAL;
-                return 0;
-        }
-        case VIDIOCGTUNER:
-        {
-                struct video_tuner *v = arg;
-
-                if (v->tuner)
-                        return -EINVAL;
-                memset(v, 0, sizeof(*v));
-                strcpy(v->name, "Format");
-                v->mode = VIDEO_MODE_AUTO;
-                return 0;
+static int qcam_g_input(struct file *file, void *fh, unsigned int *inp)
+{
+        *inp = 0;
+        return 0;
+}
+
+static int qcam_s_input(struct file *file, void *fh, unsigned int inp)
+{
+        return (inp > 0) ? -EINVAL : 0;
+}
+
+static int qcam_queryctrl(struct file *file, void *priv,
+                                        struct v4l2_queryctrl *qc)
+{
+        switch (qc->id) {
+        case V4L2_CID_BRIGHTNESS:
+                return v4l2_ctrl_query_fill(qc, 0, 255, 1, 240);
+        case V4L2_CID_CONTRAST:
+                return v4l2_ctrl_query_fill(qc, 0, 255, 1, 192);
+        case V4L2_CID_GAMMA:
+                return v4l2_ctrl_query_fill(qc, 0, 255, 1, 128);
         }
-        case VIDIOCSTUNER:
-        {
-                struct video_tuner *v = arg;
-
-                if (v->tuner)
-                        return -EINVAL;
-                if (v->mode != VIDEO_MODE_AUTO)
-                        return -EINVAL;
-                return 0;
+        return -EINVAL;
+}
+
+static int qcam_g_ctrl(struct file *file, void *priv,
+                                        struct v4l2_control *ctrl)
+{
+        struct qcam *qcam = video_drvdata(file);
+        int ret = 0;
+
+        switch (ctrl->id) {
+        case V4L2_CID_BRIGHTNESS:
+                ctrl->value = qcam->brightness;
+                break;
+        case V4L2_CID_CONTRAST:
+                ctrl->value = qcam->contrast;
+                break;
+        case V4L2_CID_GAMMA:
+                ctrl->value = qcam->whitebal;
+                break;
+        default:
+                ret = -EINVAL;
+                break;
         }
-        case VIDIOCGPICT:
-        {
-                struct video_picture *p = arg;
-
-                p->colour = 0x8000;
-                p->hue = 0x8000;
-                p->brightness = qcam->brightness << 8;
-                p->contrast = qcam->contrast << 8;
-                p->whiteness = qcam->whitebal << 8;
-                p->depth = 24;
-                p->palette = VIDEO_PALETTE_RGB24;
-                return 0;
+        return ret;
+}
+
+static int qcam_s_ctrl(struct file *file, void *priv,
+                                        struct v4l2_control *ctrl)
+{
+        struct qcam *qcam = video_drvdata(file);
+        int ret = 0;
+
+        mutex_lock(&qcam->lock);
+        switch (ctrl->id) {
+        case V4L2_CID_BRIGHTNESS:
+                qcam->brightness = ctrl->value;
+                break;
+        case V4L2_CID_CONTRAST:
+                qcam->contrast = ctrl->value;
+                break;
+        case V4L2_CID_GAMMA:
+                qcam->whitebal = ctrl->value;
+                break;
+        default:
+                ret = -EINVAL;
+                break;
         }
-        case VIDIOCSPICT:
-        {
-                struct video_picture *p = arg;
-
-                /*
-                 *      Sanity check args
-                 */
-                if (p->depth != 24 || p->palette != VIDEO_PALETTE_RGB24)
-                        return -EINVAL;
-
-                /*
-                 *      Now load the camera.
-                 */
-                qcam->brightness = p->brightness >> 8;
-                qcam->contrast = p->contrast >> 8;
-                qcam->whitebal = p->whiteness >> 8;
-
-                mutex_lock(&qcam->lock);
+        if (ret == 0) {
                 parport_claim_or_block(qcam->pdev);
                 qc_setup(qcam);
                 parport_release(qcam->pdev);
-                mutex_unlock(&qcam->lock);
-                return 0;
         }
-        case VIDIOCSWIN:
-        {
-                struct video_window *vw = arg;
-
-                if (vw->flags)
-                        return -EINVAL;
-                if (vw->clipcount)
-                        return -EINVAL;
-                if (vw->height < 60 || vw->height > 240)
-                        return -EINVAL;
-                if (vw->width < 80 || vw->width > 320)
-                        return -EINVAL;
-
-                qcam->width = 80;
-                qcam->height = 60;
-                qcam->mode = QC_DECIMATION_4;
+        mutex_unlock(&qcam->lock);
+        return ret;
+}
 
-                if (vw->width >= 160 && vw->height >= 120) {
-                        qcam->width = 160;
-                        qcam->height = 120;
-                        qcam->mode = QC_DECIMATION_2;
-                }
-                if (vw->width >= 320 && vw->height >= 240) {
-                        qcam->width = 320;
-                        qcam->height = 240;
-                        qcam->mode = QC_DECIMATION_1;
-                }
-                qcam->mode |= QC_MILLIONS;
-#if 0
-                if (vw->width >= 640 && vw->height >= 480) {
-                        qcam->width = 640;
-                        qcam->height = 480;
-                        qcam->mode = QC_BILLIONS | QC_DECIMATION_1;
-                }
-#endif
-                /* Ok we figured out what to use from our
-                   wide choice */
-                mutex_lock(&qcam->lock);
-                parport_claim_or_block(qcam->pdev);
-                qc_setup(qcam);
-                parport_release(qcam->pdev);
-                mutex_unlock(&qcam->lock);
-                return 0;
-        }
-        case VIDIOCGWIN:
-        {
-                struct video_window *vw = arg;
-                memset(vw, 0, sizeof(*vw));
-                vw->width = qcam->width;
-                vw->height = qcam->height;
-                return 0;
+static int qcam_g_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+        struct qcam *qcam = video_drvdata(file);
+        struct v4l2_pix_format *pix = &fmt->fmt.pix;
+
+        pix->width = qcam->width;
+        pix->height = qcam->height;
+        pix->pixelformat = V4L2_PIX_FMT_RGB24;
+        pix->field = V4L2_FIELD_NONE;
+        pix->bytesperline = 3 * qcam->width;
+        pix->sizeimage = 3 * qcam->width * qcam->height;
+        /* Just a guess */
+        pix->colorspace = V4L2_COLORSPACE_SRGB;
+        return 0;
+}
+
+static int qcam_try_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+        struct v4l2_pix_format *pix = &fmt->fmt.pix;
+
+        if (pix->height < 60 || pix->width < 80) {
+                pix->height = 60;
+                pix->width = 80;
+        } else if (pix->height < 120 || pix->width < 160) {
+                pix->height = 120;
+                pix->width = 160;
+        } else {
+                pix->height = 240;
+                pix->width = 320;
         }
-        case VIDIOCKEY:
-                return 0;
-        case VIDIOCCAPTURE:
-        case VIDIOCGFBUF:
-        case VIDIOCSFBUF:
-        case VIDIOCGFREQ:
-        case VIDIOCSFREQ:
-        case VIDIOCGAUDIO:
-        case VIDIOCSAUDIO:
-                return -EINVAL;
+        pix->pixelformat = V4L2_PIX_FMT_RGB24;
+        pix->field = V4L2_FIELD_NONE;
+        pix->bytesperline = 3 * pix->width;
+        pix->sizeimage = 3 * pix->width * pix->height;
+        /* Just a guess */
+        pix->colorspace = V4L2_COLORSPACE_SRGB;
+        return 0;
+}
+
+static int qcam_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+        struct qcam *qcam = video_drvdata(file);
+        struct v4l2_pix_format *pix = &fmt->fmt.pix;
+        int ret = qcam_try_fmt_vid_cap(file, fh, fmt);
+
+        if (ret)
+                return ret;
+        switch (pix->height) {
+        case 60:
+                qcam->mode = QC_DECIMATION_4;
+                break;
+        case 120:
+                qcam->mode = QC_DECIMATION_2;
+                break;
         default:
-                return -ENOIOCTLCMD;
+                qcam->mode = QC_DECIMATION_1;
+                break;
         }
+
+        mutex_lock(&qcam->lock);
+        qcam->mode |= QC_MILLIONS;
+        qcam->height = pix->height;
+        qcam->width = pix->width;
+        parport_claim_or_block(qcam->pdev);
+        qc_setup(qcam);
+        parport_release(qcam->pdev);
+        mutex_unlock(&qcam->lock);
         return 0;
 }
 
-static long qcam_ioctl(struct file *file,
-                      unsigned int cmd, unsigned long arg)
+static int qcam_enum_fmt_vid_cap(struct file *file, void *fh, struct v4l2_fmtdesc *fmt)
 {
-        return video_usercopy(file, cmd, arg, qcam_do_ioctl);
+        static struct v4l2_fmtdesc formats[] = {
+                { 0, 0, 0,
+                  "RGB 8:8:8", V4L2_PIX_FMT_RGB24,
+                  { 0, 0, 0, 0 }
+                },
+        };
+        enum v4l2_buf_type type = fmt->type;
+
+        if (fmt->index > 0)
+                return -EINVAL;
+
+        *fmt = formats[fmt->index];
+        fmt->type = type;
+        return 0;
 }
 
 static ssize_t qcam_read(struct file *file, char __user *buf,
                          size_t count, loff_t *ppos)
 {
-        struct video_device *v = video_devdata(file);
-        struct qcam_device *qcam = (struct qcam_device *)v;
+        struct qcam *qcam = video_drvdata(file);
         int len;
 
         mutex_lock(&qcam->lock);
@@ -682,81 +716,80 @@ static ssize_t qcam_read(struct file *file, char __user *buf,
         return len;
 }
 
-static int qcam_exclusive_open(struct file *file)
-{
-        struct video_device *dev = video_devdata(file);
-        struct qcam_device *qcam = (struct qcam_device *)dev;
-
-        return test_and_set_bit(0, &qcam->in_use) ? -EBUSY : 0;
-}
-
-static int qcam_exclusive_release(struct file *file)
-{
-        struct video_device *dev = video_devdata(file);
-        struct qcam_device *qcam = (struct qcam_device *)dev;
-
-        clear_bit(0, &qcam->in_use);
-        return 0;
-}
-
-/* video device template */
 static const struct v4l2_file_operations qcam_fops = {
         .owner          = THIS_MODULE,
-        .open           = qcam_exclusive_open,
-        .release        = qcam_exclusive_release,
-        .ioctl          = qcam_ioctl,
+        .ioctl          = video_ioctl2,
         .read           = qcam_read,
 };
 
-static struct video_device qcam_template = {
-        .name           = "Colour QuickCam",
-        .fops           = &qcam_fops,
-        .release        = video_device_release_empty,
+static const struct v4l2_ioctl_ops qcam_ioctl_ops = {
+        .vidioc_querycap                    = qcam_querycap,
+        .vidioc_g_input                     = qcam_g_input,
+        .vidioc_s_input                     = qcam_s_input,
+        .vidioc_enum_input                  = qcam_enum_input,
+        .vidioc_queryctrl                   = qcam_queryctrl,
+        .vidioc_g_ctrl                      = qcam_g_ctrl,
+        .vidioc_s_ctrl                      = qcam_s_ctrl,
+        .vidioc_enum_fmt_vid_cap            = qcam_enum_fmt_vid_cap,
+        .vidioc_g_fmt_vid_cap               = qcam_g_fmt_vid_cap,
+        .vidioc_s_fmt_vid_cap               = qcam_s_fmt_vid_cap,
+        .vidioc_try_fmt_vid_cap             = qcam_try_fmt_vid_cap,
 };
 
 /* Initialize the QuickCam driver control structure. */
 
-static struct qcam_device *qcam_init(struct parport *port)
+static struct qcam *qcam_init(struct parport *port)
 {
-        struct qcam_device *q;
+        struct qcam *qcam;
+        struct v4l2_device *v4l2_dev;
 
-        q = kmalloc(sizeof(struct qcam_device), GFP_KERNEL);
-        if (q == NULL)
+        qcam = kzalloc(sizeof(*qcam), GFP_KERNEL);
+        if (qcam == NULL)
                 return NULL;
 
-        q->pport = port;
-        q->pdev = parport_register_device(port, "c-qcam", NULL, NULL,
+        v4l2_dev = &qcam->v4l2_dev;
+        strlcpy(v4l2_dev->name, "c-qcam", sizeof(v4l2_dev->name));
+
+        if (v4l2_device_register(NULL, v4l2_dev) < 0) {
+                v4l2_err(v4l2_dev, "Could not register v4l2_device\n");
+                return NULL;
+        }
+
+        qcam->pport = port;
+        qcam->pdev = parport_register_device(port, "c-qcam", NULL, NULL,
                                           NULL, 0, NULL);
 
-        q->bidirectional = (q->pport->modes & PARPORT_MODE_TRISTATE) ? 1 : 0;
+        qcam->bidirectional = (qcam->pport->modes & PARPORT_MODE_TRISTATE) ? 1 : 0;
 
-        if (q->pdev == NULL) {
-                printk(KERN_ERR "c-qcam: couldn't register for %s.\n",
-                       port->name);
-                kfree(q);
+        if (qcam->pdev == NULL) {
+                v4l2_err(v4l2_dev, "couldn't register for %s.\n", port->name);
+                kfree(qcam);
                 return NULL;
         }
 
-        memcpy(&q->vdev, &qcam_template, sizeof(qcam_template));
-
-        mutex_init(&q->lock);
-        q->width = q->ccd_width = 320;
-        q->height = q->ccd_height = 240;
-        q->mode = QC_MILLIONS | QC_DECIMATION_1;
-        q->contrast = 192;
-        q->brightness = 240;
-        q->whitebal = 128;
-        q->top = 1;
-        q->left = 14;
-        return q;
+        strlcpy(qcam->vdev.name, "Colour QuickCam", sizeof(qcam->vdev.name));
+        qcam->vdev.v4l2_dev = v4l2_dev;
+        qcam->vdev.fops = &qcam_fops;
+        qcam->vdev.ioctl_ops = &qcam_ioctl_ops;
+        qcam->vdev.release = video_device_release_empty;
+        video_set_drvdata(&qcam->vdev, qcam);
+
+        mutex_init(&qcam->lock);
+        qcam->width = qcam->ccd_width = 320;
+        qcam->height = qcam->ccd_height = 240;
+        qcam->mode = QC_MILLIONS | QC_DECIMATION_1;
+        qcam->contrast = 192;
+        qcam->brightness = 240;
+        qcam->whitebal = 128;
+        qcam->top = 1;
+        qcam->left = 14;
+        return qcam;
 }
 
-static struct qcam_device *qcams[MAX_CAMS];
-static unsigned int num_cams;
-
 static int init_cqcam(struct parport *port)
 {
-        struct qcam_device *qcam;
+        struct qcam *qcam;
+        struct v4l2_device *v4l2_dev;
 
         if (parport[0] != -1) {
                 /* The user gave specific instructions */
@@ -777,6 +810,8 @@ static int init_cqcam(struct parport *port)
         if (qcam == NULL)
                 return -ENODEV;
 
+        v4l2_dev = &qcam->v4l2_dev;
+
         parport_claim_or_block(qcam->pdev);
 
         qc_reset(qcam);
@@ -793,14 +828,14 @@ static int init_cqcam(struct parport *port)
         parport_release(qcam->pdev);
 
         if (video_register_device(&qcam->vdev, VFL_TYPE_GRABBER, video_nr) < 0) {
-                printk(KERN_ERR "Unable to register Colour QuickCam on %s\n",
+                v4l2_err(v4l2_dev, "Unable to register Colour QuickCam on %s\n",
                        qcam->pport->name);
                 parport_unregister_device(qcam->pdev);
                 kfree(qcam);
                 return -ENODEV;
         }
 
-        printk(KERN_INFO "%s: Colour QuickCam found on %s\n",
+        v4l2_info(v4l2_dev, "%s: Colour QuickCam found on %s\n",
                video_device_node_name(&qcam->vdev), qcam->pport->name);
 
         qcams[num_cams++] = qcam;
@@ -808,7 +843,7 @@ static int init_cqcam(struct parport *port)
         return 0;
 }
 
-static void close_cqcam(struct qcam_device *qcam)
+static void close_cqcam(struct qcam *qcam)
 {
         video_unregister_device(&qcam->vdev);
         parport_unregister_device(qcam->pdev);
@@ -833,7 +868,7 @@ static struct parport_driver cqcam_driver = {
 
 static int __init cqcam_init(void)
 {
-        printk(BANNER "\n");
+        printk(KERN_INFO BANNER "\n");
 
         return parport_register_driver(&cqcam_driver);
 }
@@ -852,14 +887,5 @@ MODULE_AUTHOR("Philip Blundell <philb@gnu.org>");
 MODULE_DESCRIPTION(BANNER);
 MODULE_LICENSE("GPL");
 
-/* FIXME: parport=auto would never have worked, surely? --RR */
-MODULE_PARM_DESC(parport, "parport=<auto|n[,n]...> for port detection method\n"
-                          "probe=<0|1|2> for camera detection method\n"
-                          "force_rgb=<0|1> for RGB data format (default BGR)");
-module_param_array(parport, int, NULL, 0);
-module_param(probe, int, 0);
-module_param(force_rgb, bool, 0);
-module_param(video_nr, int, 0);
-
 module_init(cqcam_init);
 module_exit(cqcam_cleanup);
diff --git a/drivers/media/video/cx18/cx18-alsa-main.c b/drivers/media/video/cx18/cx18-alsa-main.c
index b5d7cbf4528a..d50d69da387b 100644
--- a/drivers/media/video/cx18/cx18-alsa-main.c
+++ b/drivers/media/video/cx18/cx18-alsa-main.c
@@ -1,7 +1,7 @@
 /*
  *  ALSA interface to cx18 PCM capture streams
  *
- *  Copyright (C) 2009  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2009  Andy Walls <awalls@md.metrocast.net>
  *  Copyright (C) 2009  Devin Heitmueller <dheitmueller@kernellabs.com>
  *
  *  Portions of this work were sponsored by ONELAN Limited.
diff --git a/drivers/media/video/cx18/cx18-alsa-mixer.c b/drivers/media/video/cx18/cx18-alsa-mixer.c
index ef21114309fe..341bddc00b77 100644
--- a/drivers/media/video/cx18/cx18-alsa-mixer.c
+++ b/drivers/media/video/cx18/cx18-alsa-mixer.c
@@ -2,7 +2,7 @@
  *  ALSA mixer controls for the
  *  ALSA interface to cx18 PCM capture streams
  *
- *  Copyright (C) 2009  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2009  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-alsa-mixer.h b/drivers/media/video/cx18/cx18-alsa-mixer.h
index 2d418db000fe..ec9238793f6f 100644
--- a/drivers/media/video/cx18/cx18-alsa-mixer.h
+++ b/drivers/media/video/cx18/cx18-alsa-mixer.h
@@ -2,7 +2,7 @@
  *  ALSA mixer controls for the
  *  ALSA interface to cx18 PCM capture streams
  *
- *  Copyright (C) 2009  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2009  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-alsa-pcm.c b/drivers/media/video/cx18/cx18-alsa-pcm.c
index 2bd312daeb1e..8f55692db36d 100644
--- a/drivers/media/video/cx18/cx18-alsa-pcm.c
+++ b/drivers/media/video/cx18/cx18-alsa-pcm.c
@@ -2,7 +2,7 @@
  *  ALSA PCM device for the
  *  ALSA interface to cx18 PCM capture streams
  *
- *  Copyright (C) 2009  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2009  Andy Walls <awalls@md.metrocast.net>
  *  Copyright (C) 2009  Devin Heitmueller <dheitmueller@kernellabs.com>
  *
  *  Portions of this work were sponsored by ONELAN Limited.
diff --git a/drivers/media/video/cx18/cx18-alsa-pcm.h b/drivers/media/video/cx18/cx18-alsa-pcm.h
index 325662c647a0..d26e51f94577 100644
--- a/drivers/media/video/cx18/cx18-alsa-pcm.h
+++ b/drivers/media/video/cx18/cx18-alsa-pcm.h
@@ -2,7 +2,7 @@
  *  ALSA PCM device for the
  *  ALSA interface to cx18 PCM capture streams
  *
- *  Copyright (C) 2009  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2009  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-alsa.h b/drivers/media/video/cx18/cx18-alsa.h
index 88a1cde7540b..447da374c9e8 100644
--- a/drivers/media/video/cx18/cx18-alsa.h
+++ b/drivers/media/video/cx18/cx18-alsa.h
@@ -1,7 +1,7 @@
 /*
  *  ALSA interface to cx18 PCM capture streams
  *
- *  Copyright (C) 2009  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2009  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-av-audio.c b/drivers/media/video/cx18/cx18-av-audio.c
index 9e30983f2ff6..43d09a24b262 100644
--- a/drivers/media/video/cx18/cx18-av-audio.c
+++ b/drivers/media/video/cx18/cx18-av-audio.c
@@ -4,7 +4,7 @@
  *  Derived from cx25840-audio.c
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
diff --git a/drivers/media/video/cx18/cx18-av-core.c b/drivers/media/video/cx18/cx18-av-core.c
index 0e5006b14279..a41951cab276 100644
--- a/drivers/media/video/cx18/cx18-av-core.c
+++ b/drivers/media/video/cx18/cx18-av-core.c
@@ -4,7 +4,7 @@
  *  Derived from cx25840-core.c
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
@@ -1021,86 +1021,74 @@ static int cx18_av_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc)
         return -EINVAL;
 }
 
-static int cx18_av_g_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
-{
-        if (fmt->type != V4L2_BUF_TYPE_SLICED_VBI_CAPTURE)
-                return -EINVAL;
-        return cx18_av_g_sliced_fmt(sd, &fmt->fmt.sliced);
-}
-
-static int cx18_av_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
+static int cx18_av_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
 {
         struct cx18_av_state *state = to_cx18_av_state(sd);
         struct cx18 *cx = v4l2_get_subdevdata(sd);
-
-        struct v4l2_pix_format *pix;
         int HSC, VSC, Vsrc, Hsrc, filter, Vlines;
         int is_50Hz = !(state->std & V4L2_STD_525_60);
 
-        switch (fmt->type) {
-        case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-                pix = &(fmt->fmt.pix);
+        if (fmt->code != V4L2_MBUS_FMT_FIXED)
+                return -EINVAL;
 
-                Vsrc = (cx18_av_read(cx, 0x476) & 0x3f) << 4;
-                Vsrc |= (cx18_av_read(cx, 0x475) & 0xf0) >> 4;
+        fmt->field = V4L2_FIELD_INTERLACED;
+        fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
 
-                Hsrc = (cx18_av_read(cx, 0x472) & 0x3f) << 4;
-                Hsrc |= (cx18_av_read(cx, 0x471) & 0xf0) >> 4;
+        Vsrc = (cx18_av_read(cx, 0x476) & 0x3f) << 4;
+        Vsrc |= (cx18_av_read(cx, 0x475) & 0xf0) >> 4;
 
-                /*
-                 * This adjustment reflects the excess of vactive, set in
-                 * cx18_av_std_setup(), above standard values:
-                 *
-                 * 480 + 1 for 60 Hz systems
-                 * 576 + 3 for 50 Hz systems
-                 */
-                Vlines = pix->height + (is_50Hz ? 3 : 1);
-
-                /*
-                 * Invalid height and width scaling requests are:
-                 * 1. width less than 1/16 of the source width
-                 * 2. width greater than the source width
-                 * 3. height less than 1/8 of the source height
-                 * 4. height greater than the source height
-                 */
-                if ((pix->width * 16 < Hsrc) || (Hsrc < pix->width) ||
-                    (Vlines * 8 < Vsrc) || (Vsrc < Vlines)) {
-                        CX18_ERR_DEV(sd, "%dx%d is not a valid size!\n",
-                                     pix->width, pix->height);
-                        return -ERANGE;
-                }
+        Hsrc = (cx18_av_read(cx, 0x472) & 0x3f) << 4;
+        Hsrc |= (cx18_av_read(cx, 0x471) & 0xf0) >> 4;
 
-                HSC = (Hsrc * (1 << 20)) / pix->width - (1 << 20);
-                VSC = (1 << 16) - (Vsrc * (1 << 9) / Vlines - (1 << 9));
-                VSC &= 0x1fff;
+        /*
+         * This adjustment reflects the excess of vactive, set in
+         * cx18_av_std_setup(), above standard values:
+         *
+         * 480 + 1 for 60 Hz systems
+         * 576 + 3 for 50 Hz systems
+         */
+        Vlines = fmt->height + (is_50Hz ? 3 : 1);
 
-                if (pix->width >= 385)
-                        filter = 0;
-                else if (pix->width > 192)
-                        filter = 1;
-                else if (pix->width > 96)
-                        filter = 2;
-                else
-                        filter = 3;
+        /*
+         * Invalid height and width scaling requests are:
+         * 1. width less than 1/16 of the source width
+         * 2. width greater than the source width
+         * 3. height less than 1/8 of the source height
+         * 4. height greater than the source height
+         */
+        if ((fmt->width * 16 < Hsrc) || (Hsrc < fmt->width) ||
+            (Vlines * 8 < Vsrc) || (Vsrc < Vlines)) {
+                CX18_ERR_DEV(sd, "%dx%d is not a valid size!\n",
+                             fmt->width, fmt->height);
+                return -ERANGE;
+        }
 
-                CX18_DEBUG_INFO_DEV(sd,
-                                    "decoder set size %dx%d -> scale  %ux%u\n",
-                                    pix->width, pix->height, HSC, VSC);
-
-                /* HSCALE=HSC */
-                cx18_av_write(cx, 0x418, HSC & 0xff);
-                cx18_av_write(cx, 0x419, (HSC >> 8) & 0xff);
-                cx18_av_write(cx, 0x41a, HSC >> 16);
-                /* VSCALE=VSC */
-                cx18_av_write(cx, 0x41c, VSC & 0xff);
-                cx18_av_write(cx, 0x41d, VSC >> 8);
-                /* VS_INTRLACE=1 VFILT=filter */
-                cx18_av_write(cx, 0x41e, 0x8 | filter);
-                break;
+        HSC = (Hsrc * (1 << 20)) / fmt->width - (1 << 20);
+        VSC = (1 << 16) - (Vsrc * (1 << 9) / Vlines - (1 << 9));
+        VSC &= 0x1fff;
 
-        default:
-                return -EINVAL;
-        }
+        if (fmt->width >= 385)
+                filter = 0;
+        else if (fmt->width > 192)
+                filter = 1;
+        else if (fmt->width > 96)
+                filter = 2;
+        else
+                filter = 3;
+
+        CX18_DEBUG_INFO_DEV(sd,
+                            "decoder set size %dx%d -> scale  %ux%u\n",
+                            fmt->width, fmt->height, HSC, VSC);
+
+        /* HSCALE=HSC */
+        cx18_av_write(cx, 0x418, HSC & 0xff);
+        cx18_av_write(cx, 0x419, (HSC >> 8) & 0xff);
+        cx18_av_write(cx, 0x41a, HSC >> 16);
+        /* VSCALE=VSC */
+        cx18_av_write(cx, 0x41c, VSC & 0xff);
+        cx18_av_write(cx, 0x41d, VSC >> 8);
+        /* VS_INTRLACE=1 VFILT=filter */
+        cx18_av_write(cx, 0x41e, 0x8 | filter);
         return 0;
 }
 
@@ -1398,8 +1386,7 @@ static const struct v4l2_subdev_audio_ops cx18_av_audio_ops = {
 static const struct v4l2_subdev_video_ops cx18_av_video_ops = {
         .s_routing = cx18_av_s_video_routing,
         .s_stream = cx18_av_s_stream,
-        .g_fmt = cx18_av_g_fmt,
-        .s_fmt = cx18_av_s_fmt,
+        .s_mbus_fmt = cx18_av_s_mbus_fmt,
 };
 
 static const struct v4l2_subdev_vbi_ops cx18_av_vbi_ops = {
diff --git a/drivers/media/video/cx18/cx18-av-core.h b/drivers/media/video/cx18/cx18-av-core.h
index c106967bdcc3..1956991795e3 100644
--- a/drivers/media/video/cx18/cx18-av-core.h
+++ b/drivers/media/video/cx18/cx18-av-core.h
@@ -4,7 +4,7 @@
  *  Derived from cx25840-core.h
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
diff --git a/drivers/media/video/cx18/cx18-av-firmware.c b/drivers/media/video/cx18/cx18-av-firmware.c
index b9e8cc5d264a..280aa4d22488 100644
--- a/drivers/media/video/cx18/cx18-av-firmware.c
+++ b/drivers/media/video/cx18/cx18-av-firmware.c
@@ -2,7 +2,7 @@
  *  cx18 ADEC firmware functions
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
diff --git a/drivers/media/video/cx18/cx18-cards.c b/drivers/media/video/cx18/cx18-cards.c
index 74e122b5fc49..6b805afe5d20 100644
--- a/drivers/media/video/cx18/cx18-cards.c
+++ b/drivers/media/video/cx18/cx18-cards.c
@@ -4,7 +4,7 @@
  *  Derived from ivtv-cards.c
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-cards.h b/drivers/media/video/cx18/cx18-cards.h
index 796e517300ac..3e750068f275 100644
--- a/drivers/media/video/cx18/cx18-cards.h
+++ b/drivers/media/video/cx18/cx18-cards.h
@@ -4,7 +4,7 @@
  *  Derived from ivtv-cards.c
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-controls.c b/drivers/media/video/cx18/cx18-controls.c
index 4b4b46544d5a..67043c7b452b 100644
--- a/drivers/media/video/cx18/cx18-controls.c
+++ b/drivers/media/video/cx18/cx18-controls.c
@@ -297,14 +297,13 @@ int cx18_s_ext_ctrls(struct file *file, void *fh, struct v4l2_ext_controls *c)
                 if (p.video_encoding != cx->params.video_encoding) {
                         int is_mpeg1 = p.video_encoding ==
                                                 V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
-                        struct v4l2_format fmt;
+                        struct v4l2_mbus_framefmt fmt;
 
                         /* fix videodecoder resolution */
-                        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-                        fmt.fmt.pix.width = cx->params.width
-                                                / (is_mpeg1 ? 2 : 1);
-                        fmt.fmt.pix.height = cx->params.height;
-                        v4l2_subdev_call(cx->sd_av, video, s_fmt, &fmt);
+                        fmt.width = cx->params.width / (is_mpeg1 ? 2 : 1);
+                        fmt.height = cx->params.height;
+                        fmt.code = V4L2_MBUS_FMT_FIXED;
+                        v4l2_subdev_call(cx->sd_av, video, s_mbus_fmt, &fmt);
                 }
                 priv.cx = cx;
                 priv.s = &cx->streams[id->type];
diff --git a/drivers/media/video/cx18/cx18-driver.c b/drivers/media/video/cx18/cx18-driver.c
index c95a86ba33b0..df60f27337cf 100644
--- a/drivers/media/video/cx18/cx18-driver.c
+++ b/drivers/media/video/cx18/cx18-driver.c
@@ -4,7 +4,7 @@
  *  Derived from ivtv-driver.c
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-driver.h b/drivers/media/video/cx18/cx18-driver.h
index b9728e8eee40..9bc51a99376b 100644
--- a/drivers/media/video/cx18/cx18-driver.h
+++ b/drivers/media/video/cx18/cx18-driver.h
@@ -4,7 +4,7 @@
  *  Derived from ivtv-driver.h
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-dvb.c b/drivers/media/video/cx18/cx18-dvb.c
index 0ae2c2e1eab5..6d19f040d70f 100644
--- a/drivers/media/video/cx18/cx18-dvb.c
+++ b/drivers/media/video/cx18/cx18-dvb.c
@@ -2,7 +2,7 @@
  *  cx18 functions for DVB support
  *
  *  Copyright (c) 2008 Steven Toth <stoth@linuxtv.org>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-fileops.c b/drivers/media/video/cx18/cx18-fileops.c
index e12a15020cda..9f23b90732f2 100644
--- a/drivers/media/video/cx18/cx18-fileops.c
+++ b/drivers/media/video/cx18/cx18-fileops.c
@@ -4,7 +4,7 @@
  *  Derived from ivtv-fileops.c
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-firmware.c b/drivers/media/video/cx18/cx18-firmware.c
index 83cd559cc609..1b3fb502e6be 100644
--- a/drivers/media/video/cx18/cx18-firmware.c
+++ b/drivers/media/video/cx18/cx18-firmware.c
@@ -2,7 +2,7 @@
  *  cx18 firmware functions
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-gpio.c b/drivers/media/video/cx18/cx18-gpio.c
index 86a204b5448e..5374aeb0cd22 100644
--- a/drivers/media/video/cx18/cx18-gpio.c
+++ b/drivers/media/video/cx18/cx18-gpio.c
@@ -4,7 +4,7 @@
  *  Derived from ivtv-gpio.c
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-gpio.h b/drivers/media/video/cx18/cx18-gpio.h
index f9a5ca3566af..4aea2ef88e8d 100644
--- a/drivers/media/video/cx18/cx18-gpio.h
+++ b/drivers/media/video/cx18/cx18-gpio.h
@@ -4,7 +4,7 @@
  *  Derived from ivtv-gpio.h
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-i2c.c b/drivers/media/video/cx18/cx18-i2c.c
index cfa1f289b0f5..809f7d37129c 100644
--- a/drivers/media/video/cx18/cx18-i2c.c
+++ b/drivers/media/video/cx18/cx18-i2c.c
@@ -4,7 +4,7 @@
  *  Derived from ivtv-i2c.c
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-io.c b/drivers/media/video/cx18/cx18-io.c
index ec5b3d7bcc6b..49b9dbd06248 100644
--- a/drivers/media/video/cx18/cx18-io.c
+++ b/drivers/media/video/cx18/cx18-io.c
@@ -2,7 +2,7 @@
  *  cx18 driver PCI memory mapped IO access routines
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-io.h b/drivers/media/video/cx18/cx18-io.h
index 2635b3a8cc96..18974d886cf7 100644
--- a/drivers/media/video/cx18/cx18-io.h
+++ b/drivers/media/video/cx18/cx18-io.h
@@ -2,7 +2,7 @@
  *  cx18 driver PCI memory mapped IO access routines
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
@@ -28,7 +28,7 @@
 /*
  * Readback and retry of MMIO access for reliability:
  * The concept was suggested by Steve Toth <stoth@linuxtv.org>.
- * The implmentation is the fault of Andy Walls <awalls@radix.net>.
+ * The implmentation is the fault of Andy Walls <awalls@md.metrocast.net>.
  *
  * *write* functions are implied to retry the mmio unless suffixed with _noretry
  * *read* functions never retry the mmio (it never helps to do so)
diff --git a/drivers/media/video/cx18/cx18-ioctl.c b/drivers/media/video/cx18/cx18-ioctl.c
index 2530fc54daaf..20eaf38ba959 100644
--- a/drivers/media/video/cx18/cx18-ioctl.c
+++ b/drivers/media/video/cx18/cx18-ioctl.c
@@ -4,7 +4,7 @@
  *  Derived from ivtv-ioctl.c
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
@@ -274,6 +274,7 @@ static int cx18_s_fmt_vid_cap(struct file *file, void *fh,
 {
         struct cx18_open_id *id = fh;
         struct cx18 *cx = id->cx;
+        struct v4l2_mbus_framefmt mbus_fmt;
         int ret;
         int w, h;
 
@@ -293,9 +294,10 @@ static int cx18_s_fmt_vid_cap(struct file *file, void *fh,
         if (atomic_read(&cx->ana_capturing) > 0)
                 return -EBUSY;
 
-        cx->params.width = w;
-        cx->params.height = h;
-        v4l2_subdev_call(cx->sd_av, video, s_fmt, fmt);
+        mbus_fmt.width = cx->params.width = w;
+        mbus_fmt.height = cx->params.height = h;
+        mbus_fmt.code = V4L2_MBUS_FMT_FIXED;
+        v4l2_subdev_call(cx->sd_av, video, s_mbus_fmt, &mbus_fmt);
         return cx18_g_fmt_vid_cap(file, fh, fmt);
 }
 
diff --git a/drivers/media/video/cx18/cx18-ioctl.h b/drivers/media/video/cx18/cx18-ioctl.h
index e2ca0d152116..dcb2559ad520 100644
--- a/drivers/media/video/cx18/cx18-ioctl.h
+++ b/drivers/media/video/cx18/cx18-ioctl.h
@@ -4,7 +4,7 @@
  *  Derived from ivtv-ioctl.h
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-irq.c b/drivers/media/video/cx18/cx18-irq.c
index af2f504eda2b..80edfe93a3d8 100644
--- a/drivers/media/video/cx18/cx18-irq.c
+++ b/drivers/media/video/cx18/cx18-irq.c
@@ -2,7 +2,7 @@
  *  cx18 interrupt handling
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-irq.h b/drivers/media/video/cx18/cx18-irq.h
index 91f0b5278ef9..30e7eaf8cb55 100644
--- a/drivers/media/video/cx18/cx18-irq.h
+++ b/drivers/media/video/cx18/cx18-irq.h
@@ -2,7 +2,7 @@
  *  cx18 interrupt handling
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-mailbox.c b/drivers/media/video/cx18/cx18-mailbox.c
index 6dcce297752f..956aa190ecca 100644
--- a/drivers/media/video/cx18/cx18-mailbox.c
+++ b/drivers/media/video/cx18/cx18-mailbox.c
@@ -2,7 +2,7 @@
  *  cx18 mailbox functions
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-mailbox.h b/drivers/media/video/cx18/cx18-mailbox.h
index 33a3491c4537..077952fcbcca 100644
--- a/drivers/media/video/cx18/cx18-mailbox.h
+++ b/drivers/media/video/cx18/cx18-mailbox.h
@@ -2,7 +2,7 @@
  *  cx18 mailbox functions
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-queue.c b/drivers/media/video/cx18/cx18-queue.c
index aefc8c8cf3c1..8884537bd62f 100644
--- a/drivers/media/video/cx18/cx18-queue.c
+++ b/drivers/media/video/cx18/cx18-queue.c
@@ -4,7 +4,7 @@
  *  Derived from ivtv-queue.c
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-queue.h b/drivers/media/video/cx18/cx18-queue.h
index 88a6d34ad3bb..4201ddc16091 100644
--- a/drivers/media/video/cx18/cx18-queue.h
+++ b/drivers/media/video/cx18/cx18-queue.h
@@ -4,7 +4,7 @@
  *  Derived from ivtv-queue.h
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-scb.c b/drivers/media/video/cx18/cx18-scb.c
index 34b4d03c55cd..85cc59637e54 100644
--- a/drivers/media/video/cx18/cx18-scb.c
+++ b/drivers/media/video/cx18/cx18-scb.c
@@ -2,7 +2,7 @@
  *  cx18 System Control Block initialization
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-scb.h b/drivers/media/video/cx18/cx18-scb.h
index 368f23d08709..08877652e321 100644
--- a/drivers/media/video/cx18/cx18-scb.h
+++ b/drivers/media/video/cx18/cx18-scb.h
@@ -2,7 +2,7 @@
  *  cx18 System Control Block initialization
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-streams.c b/drivers/media/video/cx18/cx18-streams.c
index f5c91261b2db..9045f1ece0eb 100644
--- a/drivers/media/video/cx18/cx18-streams.c
+++ b/drivers/media/video/cx18/cx18-streams.c
@@ -4,7 +4,7 @@
  *  Derived from ivtv-streams.c
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx18/cx18-streams.h b/drivers/media/video/cx18/cx18-streams.h
index 0bff0fa29763..77412bee5963 100644
--- a/drivers/media/video/cx18/cx18-streams.h
+++ b/drivers/media/video/cx18/cx18-streams.h
@@ -4,7 +4,7 @@
  *  Derived from ivtv-streams.h
  *
  *  Copyright (C) 2007  Hans Verkuil <hverkuil@xs4all.nl>
- *  Copyright (C) 2008  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2008  Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx231xx/cx231xx-video.c b/drivers/media/video/cx231xx/cx231xx-video.c
index 2782709b263f..e76014561aa7 100644
--- a/drivers/media/video/cx231xx/cx231xx-video.c
+++ b/drivers/media/video/cx231xx/cx231xx-video.c
@@ -993,6 +993,7 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
         struct cx231xx *dev = fh->dev;
         int rc;
         struct cx231xx_fmt *fmt;
+        struct v4l2_mbus_framefmt mbus_fmt;
 
         rc = check_dev(dev);
         if (rc < 0)
@@ -1026,7 +1027,9 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
         dev->format = fmt;
         get_scale(dev, dev->width, dev->height, &dev->hscale, &dev->vscale);
 
-        call_all(dev, video, s_fmt, f);
+        v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, V4L2_MBUS_FMT_FIXED);
+        call_all(dev, video, s_mbus_fmt, &mbus_fmt);
+        v4l2_fill_pix_format(&f->fmt.pix, &mbus_fmt);
 
         /* Set the correct alternate setting for this resolution */
         cx231xx_resolution_set(dev);
diff --git a/drivers/media/video/cx2341x.c b/drivers/media/video/cx2341x.c
index 022b480918cd..2bf44ef10fec 100644
--- a/drivers/media/video/cx2341x.c
+++ b/drivers/media/video/cx2341x.c
@@ -1113,7 +1113,6 @@ invalid:
 void cx2341x_log_status(const struct cx2341x_mpeg_params *p, const char *prefix)
 {
         int is_mpeg1 = p->video_encoding == V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
-        int temporal = p->video_temporal_filter;
 
         /* Stream */
         printk(KERN_INFO "%s: Stream: %s",
@@ -1179,14 +1178,11 @@ void cx2341x_log_status(const struct cx2341x_mpeg_params *p, const char *prefix)
                     V4L2_CID_MPEG_CX2341X_VIDEO_CHROMA_SPATIAL_FILTER_TYPE),
                 p->video_spatial_filter);
 
-        if (p->width != 720 || p->height != (p->is_50hz ? 576 : 480))
-                temporal = 0;
-
         printk(KERN_INFO "%s: Temporal Filter: %s, %d\n",
                 prefix,
                 cx2341x_menu_item(p,
                         V4L2_CID_MPEG_CX2341X_VIDEO_TEMPORAL_FILTER_MODE),
-                temporal);
+                p->video_temporal_filter);
         printk(KERN_INFO
                 "%s: Median Filter:   %s, Luma [%d, %d], Chroma [%d, %d]\n",
                 prefix,
diff --git a/drivers/media/video/cx23885/cx23885-dvb.c b/drivers/media/video/cx23885/cx23885-dvb.c
index 9e1460828b2f..0a199d774d9b 100644
--- a/drivers/media/video/cx23885/cx23885-dvb.c
+++ b/drivers/media/video/cx23885/cx23885-dvb.c
@@ -991,6 +991,8 @@ static int dvb_register(struct cx23885_tsport *port)
         ret = videobuf_dvb_register_bus(&port->frontends, THIS_MODULE, port,
                                         &dev->pci->dev, adapter_nr, 0,
                                         cx23885_dvb_fe_ioctl_override);
+        if (!ret)
+                return ret;
 
         /* init CI & MAC */
         switch (dev->board) {
diff --git a/drivers/media/video/cx23885/cx23885-i2c.c b/drivers/media/video/cx23885/cx23885-i2c.c
index 4172cb387420..d4746e064516 100644
--- a/drivers/media/video/cx23885/cx23885-i2c.c
+++ b/drivers/media/video/cx23885/cx23885-i2c.c
@@ -365,7 +365,17 @@ int cx23885_i2c_register(struct cx23885_i2c *bus)
 
                 memset(&info, 0, sizeof(struct i2c_board_info));
                 strlcpy(info.type, "ir_video", I2C_NAME_SIZE);
-                i2c_new_probed_device(&bus->i2c_adap, &info, addr_list);
+                /*
+                 * We can't call i2c_new_probed_device() because it uses
+                 * quick writes for probing and the IR receiver device only
+                 * replies to reads.
+                 */
+                if (i2c_smbus_xfer(&bus->i2c_adap, addr_list[0], 0,
+                                   I2C_SMBUS_READ, 0, I2C_SMBUS_QUICK,
+                                   NULL) >= 0) {
+                        info.addr = addr_list[0];
+                        i2c_new_device(&bus->i2c_adap, &info);
+                }
         }
 
         return bus->i2c_rc;
diff --git a/drivers/media/video/cx23885/cx23885-input.c b/drivers/media/video/cx23885/cx23885-input.c
index 8d306d8bb61c..5de6ba98f7a8 100644
--- a/drivers/media/video/cx23885/cx23885-input.c
+++ b/drivers/media/video/cx23885/cx23885-input.c
@@ -5,7 +5,7 @@
  *
  *  Most of this file is
  *
- *  Copyright (C) 2009  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2009  Andy Walls <awalls@md.metrocast.net>
  *
  *  However, the cx23885_input_{init,fini} functions contained herein are
  *  derived from Linux kernel files linux/media/video/.../...-input.c marked as:
diff --git a/drivers/media/video/cx23885/cx23885-input.h b/drivers/media/video/cx23885/cx23885-input.h
index 3572cb1ecfc2..75ef15d3f523 100644
--- a/drivers/media/video/cx23885/cx23885-input.h
+++ b/drivers/media/video/cx23885/cx23885-input.h
@@ -3,7 +3,7 @@
  *
  *  Infrared remote control input device
  *
- *  Copyright (C) 2009  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2009  Andy Walls <awalls@md.metrocast.net>
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
diff --git a/drivers/media/video/cx23885/cx23885-ioctl.c b/drivers/media/video/cx23885/cx23885-ioctl.c
index dfb4627fb340..44812ca78899 100644
--- a/drivers/media/video/cx23885/cx23885-ioctl.c
+++ b/drivers/media/video/cx23885/cx23885-ioctl.c
@@ -3,7 +3,7 @@
  *
  *  Various common ioctl() support functions
  *
- *  Copyright (c) 2009 Andy Walls <awalls@radix.net>
+ *  Copyright (c) 2009 Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx23885/cx23885-ioctl.h b/drivers/media/video/cx23885/cx23885-ioctl.h
index 80b0f4923c6a..315be0ca5a04 100644
--- a/drivers/media/video/cx23885/cx23885-ioctl.h
+++ b/drivers/media/video/cx23885/cx23885-ioctl.h
@@ -3,7 +3,7 @@
  *
  *  Various common ioctl() support functions
  *
- *  Copyright (c) 2009 Andy Walls <awalls@radix.net>
+ *  Copyright (c) 2009 Andy Walls <awalls@md.metrocast.net>
  *
  *  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
diff --git a/drivers/media/video/cx23885/cx23885-ir.c b/drivers/media/video/cx23885/cx23885-ir.c
index 6ae982cc9856..9a677eb080af 100644
--- a/drivers/media/video/cx23885/cx23885-ir.c
+++ b/drivers/media/video/cx23885/cx23885-ir.c
@@ -3,7 +3,7 @@
  *
  *  Infrared device support routines - non-input, non-vl42_subdev routines
  *
- *  Copyright (C) 2009  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2009  Andy Walls <awalls@md.metrocast.net>
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
diff --git a/drivers/media/video/cx23885/cx23885-ir.h b/drivers/media/video/cx23885/cx23885-ir.h
index 9b8a6d5d1ef6..0c9d8bda9e28 100644
--- a/drivers/media/video/cx23885/cx23885-ir.h
+++ b/drivers/media/video/cx23885/cx23885-ir.h
@@ -3,7 +3,7 @@
  *
  *  Infrared device support routines - non-input, non-vl42_subdev routines
  *
- *  Copyright (C) 2009  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2009  Andy Walls <awalls@md.metrocast.net>
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
diff --git a/drivers/media/video/cx23885/cx23885-video.c b/drivers/media/video/cx23885/cx23885-video.c
index 543b854f6a62..4e44dcda3875 100644
--- a/drivers/media/video/cx23885/cx23885-video.c
+++ b/drivers/media/video/cx23885/cx23885-video.c
@@ -976,6 +976,7 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 {
         struct cx23885_fh *fh = priv;
         struct cx23885_dev *dev  = ((struct cx23885_fh *)priv)->dev;
+        struct v4l2_mbus_framefmt mbus_fmt;
         int err;
 
         dprintk(2, "%s()\n", __func__);
@@ -989,7 +990,9 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
         fh->vidq.field = f->fmt.pix.field;
         dprintk(2, "%s() width=%d height=%d field=%d\n", __func__,
                 fh->width, fh->height, fh->vidq.field);
-        call_all(dev, video, s_fmt, f);
+        v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, V4L2_MBUS_FMT_FIXED);
+        call_all(dev, video, s_mbus_fmt, &mbus_fmt);
+        v4l2_fill_pix_format(&f->fmt.pix, &mbus_fmt);
         return 0;
 }
 
diff --git a/drivers/media/video/cx23885/cx23888-ir.c b/drivers/media/video/cx23885/cx23888-ir.c
index ad728d767d69..f63d378257a7 100644
--- a/drivers/media/video/cx23885/cx23888-ir.c
+++ b/drivers/media/video/cx23885/cx23888-ir.c
@@ -3,7 +3,7 @@
  *
  *  CX23888 Integrated Consumer Infrared Controller
  *
- *  Copyright (C) 2009  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2009  Andy Walls <awalls@md.metrocast.net>
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
diff --git a/drivers/media/video/cx23885/cx23888-ir.h b/drivers/media/video/cx23885/cx23888-ir.h
index 3d446f9eb94b..d2de41caaf1d 100644
--- a/drivers/media/video/cx23885/cx23888-ir.h
+++ b/drivers/media/video/cx23885/cx23888-ir.h
@@ -3,7 +3,7 @@
  *
  *  CX23888 Integrated Consumer Infrared Controller
  *
- *  Copyright (C) 2009  Andy Walls <awalls@radix.net>
+ *  Copyright (C) 2009  Andy Walls <awalls@md.metrocast.net>
  *
  *  This program is free software; you can redistribute it and/or
  *  modify it under the terms of the GNU General Public License
diff --git a/drivers/media/video/cx25840/cx25840-core.c b/drivers/media/video/cx25840/cx25840-core.c
index 8b6fb3544376..bb4872b2ceb0 100644
--- a/drivers/media/video/cx25840/cx25840-core.c
+++ b/drivers/media/video/cx25840/cx25840-core.c
@@ -1014,75 +1014,59 @@ static int cx25840_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
 
 /* ----------------------------------------------------------------------- */
 
-static int cx25840_g_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
-{
-        switch (fmt->type) {
-        case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
-                return cx25840_g_sliced_fmt(sd, &fmt->fmt.sliced);
-        default:
-                return -EINVAL;
-        }
-        return 0;
-}
-
-static int cx25840_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
+static int cx25840_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
 {
         struct cx25840_state *state = to_state(sd);
         struct i2c_client *client = v4l2_get_subdevdata(sd);
-        struct v4l2_pix_format *pix;
         int HSC, VSC, Vsrc, Hsrc, filter, Vlines;
         int is_50Hz = !(state->std & V4L2_STD_525_60);
 
-        switch (fmt->type) {
-        case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-                pix = &(fmt->fmt.pix);
-
-                Vsrc = (cx25840_read(client, 0x476) & 0x3f) << 4;
-                Vsrc |= (cx25840_read(client, 0x475) & 0xf0) >> 4;
-
-                Hsrc = (cx25840_read(client, 0x472) & 0x3f) << 4;
-                Hsrc |= (cx25840_read(client, 0x471) & 0xf0) >> 4;
+        if (fmt->code != V4L2_MBUS_FMT_FIXED)
+                return -EINVAL;
 
-                Vlines = pix->height + (is_50Hz ? 4 : 7);
+        fmt->field = V4L2_FIELD_INTERLACED;
+        fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
 
-                if ((pix->width * 16 < Hsrc) || (Hsrc < pix->width) ||
-                    (Vlines * 8 < Vsrc) || (Vsrc < Vlines)) {
-                        v4l_err(client, "%dx%d is not a valid size!\n",
-                                    pix->width, pix->height);
-                        return -ERANGE;
-                }
+        Vsrc = (cx25840_read(client, 0x476) & 0x3f) << 4;
+        Vsrc |= (cx25840_read(client, 0x475) & 0xf0) >> 4;
 
-                HSC = (Hsrc * (1 << 20)) / pix->width - (1 << 20);
-                VSC = (1 << 16) - (Vsrc * (1 << 9) / Vlines - (1 << 9));
-                VSC &= 0x1fff;
+        Hsrc = (cx25840_read(client, 0x472) & 0x3f) << 4;
+        Hsrc |= (cx25840_read(client, 0x471) & 0xf0) >> 4;
 
-                if (pix->width >= 385)
-                        filter = 0;
-                else if (pix->width > 192)
-                        filter = 1;
-                else if (pix->width > 96)
-                        filter = 2;
-                else
-                        filter = 3;
-
-                v4l_dbg(1, cx25840_debug, client, "decoder set size %dx%d -> scale  %ux%u\n",
-                            pix->width, pix->height, HSC, VSC);
-
-                /* HSCALE=HSC */
-                cx25840_write(client, 0x418, HSC & 0xff);
-                cx25840_write(client, 0x419, (HSC >> 8) & 0xff);
-                cx25840_write(client, 0x41a, HSC >> 16);
-                /* VSCALE=VSC */
-                cx25840_write(client, 0x41c, VSC & 0xff);
-                cx25840_write(client, 0x41d, VSC >> 8);
-                /* VS_INTRLACE=1 VFILT=filter */
-                cx25840_write(client, 0x41e, 0x8 | filter);
-                break;
+        Vlines = fmt->height + (is_50Hz ? 4 : 7);
 
-        default:
-                return -EINVAL;
+        if ((fmt->width * 16 < Hsrc) || (Hsrc < fmt->width) ||
+                        (Vlines * 8 < Vsrc) || (Vsrc < Vlines)) {
+                v4l_err(client, "%dx%d is not a valid size!\n",
+                                fmt->width, fmt->height);
+                return -ERANGE;
         }
 
+        HSC = (Hsrc * (1 << 20)) / fmt->width - (1 << 20);
+        VSC = (1 << 16) - (Vsrc * (1 << 9) / Vlines - (1 << 9));
+        VSC &= 0x1fff;
+
+        if (fmt->width >= 385)
+                filter = 0;
+        else if (fmt->width > 192)
+                filter = 1;
+        else if (fmt->width > 96)
+                filter = 2;
+        else
+                filter = 3;
+
+        v4l_dbg(1, cx25840_debug, client, "decoder set size %dx%d -> scale  %ux%u\n",
+                        fmt->width, fmt->height, HSC, VSC);
+
+        /* HSCALE=HSC */
+        cx25840_write(client, 0x418, HSC & 0xff);
+        cx25840_write(client, 0x419, (HSC >> 8) & 0xff);
+        cx25840_write(client, 0x41a, HSC >> 16);
+        /* VSCALE=VSC */
+        cx25840_write(client, 0x41c, VSC & 0xff);
+        cx25840_write(client, 0x41d, VSC >> 8);
+        /* VS_INTRLACE=1 VFILT=filter */
+        cx25840_write(client, 0x41e, 0x8 | filter);
         return 0;
 }
 
@@ -1627,8 +1611,7 @@ static const struct v4l2_subdev_audio_ops cx25840_audio_ops = {
 
 static const struct v4l2_subdev_video_ops cx25840_video_ops = {
         .s_routing = cx25840_s_video_routing,
-        .g_fmt = cx25840_g_fmt,
-        .s_fmt = cx25840_s_fmt,
+        .s_mbus_fmt = cx25840_s_mbus_fmt,
         .s_stream = cx25840_s_stream,
 };
 
diff --git a/drivers/media/video/cx88/cx88-i2c.c b/drivers/media/video/cx88/cx88-i2c.c
index ee1ca39db06a..fb39f1184558 100644
--- a/drivers/media/video/cx88/cx88-i2c.c
+++ b/drivers/media/video/cx88/cx88-i2c.c
@@ -188,10 +188,24 @@ int cx88_i2c_init(struct cx88_core *core, struct pci_dev *pci)
                         0x18, 0x6b, 0x71,
                         I2C_CLIENT_END
                 };
+                const unsigned short *addrp;
 
                 memset(&info, 0, sizeof(struct i2c_board_info));
                 strlcpy(info.type, "ir_video", I2C_NAME_SIZE);
-                i2c_new_probed_device(&core->i2c_adap, &info, addr_list);
+                /*
+                 * We can't call i2c_new_probed_device() because it uses
+                 * quick writes for probing and at least some R receiver
+                 * devices only reply to reads.
+                 */
+                for (addrp = addr_list; *addrp != I2C_CLIENT_END; addrp++) {
+                        if (i2c_smbus_xfer(&core->i2c_adap, *addrp, 0,
+                                           I2C_SMBUS_READ, 0,
+                                           I2C_SMBUS_QUICK, NULL) >= 0) {
+                                info.addr = *addrp;
+                                i2c_new_device(&core->i2c_adap, &info);
+                                break;
+                        }
+                }
         }
         return core->i2c_rc;
 }
diff --git a/drivers/media/video/em28xx/em28xx-core.c b/drivers/media/video/em28xx/em28xx-core.c
index 331e1cac4272..44c63cbd6dda 100644
--- a/drivers/media/video/em28xx/em28xx-core.c
+++ b/drivers/media/video/em28xx/em28xx-core.c
@@ -1186,8 +1186,7 @@ int em28xx_register_extension(struct em28xx_ops *ops)
         mutex_lock(&em28xx_devlist_mutex);
         list_add_tail(&ops->next, &em28xx_extension_devlist);
         list_for_each_entry(dev, &em28xx_devlist, devlist) {
-                if (dev)
-                        ops->init(dev);
+                ops->init(dev);
         }
         printk(KERN_INFO "Em28xx: Initialized (%s) extension\n", ops->name);
         mutex_unlock(&em28xx_devlist_mutex);
@@ -1201,10 +1200,8 @@ void em28xx_unregister_extension(struct em28xx_ops *ops)
 
         mutex_lock(&em28xx_devlist_mutex);
         list_for_each_entry(dev, &em28xx_devlist, devlist) {
-                if (dev)
-                        ops->fini(dev);
+                ops->fini(dev);
         }
-
         printk(KERN_INFO "Em28xx: Removed (%s) extension\n", ops->name);
         list_del(&ops->next);
         mutex_unlock(&em28xx_devlist_mutex);
diff --git a/drivers/media/video/gspca/sonixb.c b/drivers/media/video/gspca/sonixb.c
index 785eeb4c2014..95354a339e3d 100644
--- a/drivers/media/video/gspca/sonixb.c
+++ b/drivers/media/video/gspca/sonixb.c
@@ -1453,9 +1453,7 @@ static const struct usb_device_id device_table[] __devinitconst = {
         {USB_DEVICE(0x0c45, 0x6029), SB(PAS106, 102)},
         {USB_DEVICE(0x0c45, 0x602c), SB(OV7630, 102)},
         {USB_DEVICE(0x0c45, 0x602d), SB(HV7131R, 102)},
-#if !defined CONFIG_USB_SN9C102 && !defined CONFIG_USB_SN9C102_MODULE
         {USB_DEVICE(0x0c45, 0x602e), SB(OV7630, 102)},
-#endif
         {USB_DEVICE(0x0c45, 0x608f), SB(OV7630, 103)},
 #if !defined CONFIG_USB_SN9C102 && !defined CONFIG_USB_SN9C102_MODULE
         {USB_DEVICE(0x0c45, 0x60af), SB(PAS202, 103)},
diff --git a/drivers/media/video/gspca/sonixj.c b/drivers/media/video/gspca/sonixj.c
index bb923efb75bd..176c5b3d5e6f 100644
--- a/drivers/media/video/gspca/sonixj.c
+++ b/drivers/media/video/gspca/sonixj.c
@@ -3022,16 +3022,18 @@ static const __devinitdata struct usb_device_id device_table[] = {
 /*      {USB_DEVICE(0x0c45, 0x60c2), BS(SN9C105, P1030xC)}, */
 /*      {USB_DEVICE(0x0c45, 0x60c8), BS(SN9C105, OM6802)}, */
 /*      {USB_DEVICE(0x0c45, 0x60cc), BS(SN9C105, HV7131GP)}, */
+        {USB_DEVICE(0x0c45, 0x60ce), BS(SN9C105, SP80708)},
         {USB_DEVICE(0x0c45, 0x60ec), BS(SN9C105, MO4000)},
 /*      {USB_DEVICE(0x0c45, 0x60ef), BS(SN9C105, ICM105C)}, */
 /*      {USB_DEVICE(0x0c45, 0x60fa), BS(SN9C105, OV7648)}, */
+/*      {USB_DEVICE(0x0c45, 0x60f2), BS(SN9C105, OV7660)}, */
         {USB_DEVICE(0x0c45, 0x60fb), BS(SN9C105, OV7660)},
 #if !defined CONFIG_USB_SN9C102 && !defined CONFIG_USB_SN9C102_MODULE
         {USB_DEVICE(0x0c45, 0x60fc), BS(SN9C105, HV7131R)},
         {USB_DEVICE(0x0c45, 0x60fe), BS(SN9C105, OV7630)},
 #endif
         {USB_DEVICE(0x0c45, 0x6100), BS(SN9C120, MI0360)},      /*sn9c128*/
-/*      {USB_DEVICE(0x0c45, 0x6102), BS(SN9C120, P1030xC)}, */
+/*      {USB_DEVICE(0x0c45, 0x6102), BS(SN9C120, PO2030N)}, * / GC0305*/
 /*      {USB_DEVICE(0x0c45, 0x6108), BS(SN9C120, OM6802)}, */
         {USB_DEVICE(0x0c45, 0x610a), BS(SN9C120, OV7648)},      /*sn9c128*/
         {USB_DEVICE(0x0c45, 0x610b), BS(SN9C120, OV7660)},      /*sn9c128*/
@@ -3058,6 +3060,7 @@ static const __devinitdata struct usb_device_id device_table[] = {
         {USB_DEVICE(0x0c45, 0x613c), BS(SN9C120, HV7131R)},
         {USB_DEVICE(0x0c45, 0x613e), BS(SN9C120, OV7630)},
         {USB_DEVICE(0x0c45, 0x6142), BS(SN9C120, PO2030N)},     /*sn9c120b*/
+                                                /* or GC0305 / GC0307 */
         {USB_DEVICE(0x0c45, 0x6143), BS(SN9C120, SP80708)},     /*sn9c120b*/
         {USB_DEVICE(0x0c45, 0x6148), BS(SN9C120, OM6802)},      /*sn9c120b*/
         {USB_DEVICE(0x0c45, 0x614a), BS(SN9C120, ADCM1700)},    /*sn9c120b*/
diff --git a/drivers/media/video/hdpvr/hdpvr-video.c b/drivers/media/video/hdpvr/hdpvr-video.c
index 7cfccfd1b870..c338f3f62e77 100644
--- a/drivers/media/video/hdpvr/hdpvr-video.c
+++ b/drivers/media/video/hdpvr/hdpvr-video.c
@@ -366,7 +366,7 @@ static int hdpvr_open(struct file *file)
 
         dev = (struct hdpvr_device *)video_get_drvdata(video_devdata(file));
         if (!dev) {
-                v4l2_err(&dev->v4l2_dev, "open failing with with ENODEV\n");
+                pr_err("open failing with with ENODEV\n");
                 retval = -ENODEV;
                 goto err;
         }
diff --git a/drivers/media/video/ivtv/ivtv-controls.c b/drivers/media/video/ivtv/ivtv-controls.c
index b59475bfc243..b588e30cbcf0 100644
--- a/drivers/media/video/ivtv/ivtv-controls.c
+++ b/drivers/media/video/ivtv/ivtv-controls.c
@@ -267,13 +267,13 @@ int ivtv_s_ext_ctrls(struct file *file, void *fh, struct v4l2_ext_controls *c)
                 if (p.video_encoding != itv->params.video_encoding) {
                         int is_mpeg1 = p.video_encoding ==
                                 V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
-                        struct v4l2_format fmt;
+                        struct v4l2_mbus_framefmt fmt;
 
                         /* fix videodecoder resolution */
-                        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-                        fmt.fmt.pix.width = itv->params.width / (is_mpeg1 ? 2 : 1);
-                        fmt.fmt.pix.height = itv->params.height;
-                        v4l2_subdev_call(itv->sd_video, video, s_fmt, &fmt);
+                        fmt.width = itv->params.width / (is_mpeg1 ? 2 : 1);
+                        fmt.height = itv->params.height;
+                        fmt.code = V4L2_MBUS_FMT_FIXED;
+                        v4l2_subdev_call(itv->sd_video, video, s_mbus_fmt, &fmt);
                 }
                 err = cx2341x_update(itv, ivtv_api_func, &itv->params, &p);
                 if (!err && itv->params.stream_vbi_fmt != p.stream_vbi_fmt)
diff --git a/drivers/media/video/ivtv/ivtv-fileops.c b/drivers/media/video/ivtv/ivtv-fileops.c
index abf410943cc9..3c2cc270ccd5 100644
--- a/drivers/media/video/ivtv/ivtv-fileops.c
+++ b/drivers/media/video/ivtv/ivtv-fileops.c
@@ -823,6 +823,12 @@ static void ivtv_stop_decoding(struct ivtv_open_id *id, int flags, u64 pts)
 
         IVTV_DEBUG_FILE("close() of %s\n", s->name);
 
+        if (id->type == IVTV_DEC_STREAM_TYPE_YUV &&
+                test_bit(IVTV_F_I_DECODING_YUV, &itv->i_flags)) {
+                /* Restore registers we've changed & clean up any mess */
+                ivtv_yuv_close(itv);
+        }
+
         /* Stop decoding */
         if (test_bit(IVTV_F_S_STREAMING, &s->s_flags)) {
                 IVTV_DEBUG_INFO("close stopping decode\n");
@@ -832,10 +838,7 @@ static void ivtv_stop_decoding(struct ivtv_open_id *id, int flags, u64 pts)
         }
         clear_bit(IVTV_F_S_APPL_IO, &s->s_flags);
         clear_bit(IVTV_F_S_STREAMOFF, &s->s_flags);
-        if (id->type == IVTV_DEC_STREAM_TYPE_YUV && test_bit(IVTV_F_I_DECODING_YUV, &itv->i_flags)) {
-                /* Restore registers we've changed & clean up any mess we've made */
-                ivtv_yuv_close(itv);
-        }
+
         if (itv->output_mode == OUT_UDMA_YUV && id->yuv_frames)
                 itv->output_mode = OUT_NONE;
 
diff --git a/drivers/media/video/ivtv/ivtv-ioctl.c b/drivers/media/video/ivtv/ivtv-ioctl.c
index fa9f0d958f96..11ac2fa33ef7 100644
--- a/drivers/media/video/ivtv/ivtv-ioctl.c
+++ b/drivers/media/video/ivtv/ivtv-ioctl.c
@@ -569,6 +569,7 @@ static int ivtv_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f
         struct ivtv_open_id *id = fh;
         struct ivtv *itv = id->itv;
         struct cx2341x_mpeg_params *p = &itv->params;
+        struct v4l2_mbus_framefmt mbus_fmt;
         int ret = ivtv_try_fmt_vid_cap(file, fh, fmt);
         int w = fmt->fmt.pix.width;
         int h = fmt->fmt.pix.height;
@@ -586,7 +587,10 @@ static int ivtv_s_fmt_vid_cap(struct file *file, void *fh, struct v4l2_format *f
         p->height = h;
         if (p->video_encoding == V4L2_MPEG_VIDEO_ENCODING_MPEG_1)
                 fmt->fmt.pix.width /= 2;
-        v4l2_subdev_call(itv->sd_video, video, s_fmt, fmt);
+        mbus_fmt.width = fmt->fmt.pix.width;
+        mbus_fmt.height = h;
+        mbus_fmt.code = V4L2_MBUS_FMT_FIXED;
+        v4l2_subdev_call(itv->sd_video, video, s_mbus_fmt, &mbus_fmt);
         return ivtv_g_fmt_vid_cap(file, fh, fmt);
 }
 
diff --git a/drivers/media/video/ivtv/ivtv-streams.c b/drivers/media/video/ivtv/ivtv-streams.c
index de4288cc1889..9ecacab4b89b 100644
--- a/drivers/media/video/ivtv/ivtv-streams.c
+++ b/drivers/media/video/ivtv/ivtv-streams.c
@@ -618,12 +618,17 @@ static int ivtv_setup_v4l2_decode_stream(struct ivtv_stream *s)
         struct ivtv *itv = s->itv;
         struct cx2341x_mpeg_params *p = &itv->params;
         int datatype;
+        u16 width;
+        u16 height;
 
         if (s->vdev == NULL)
                 return -EINVAL;
 
         IVTV_DEBUG_INFO("Setting some initial decoder settings\n");
 
+        width = p->width;
+        height = p->height;
+
         /* set audio mode to left/stereo  for dual/stereo mode. */
         ivtv_vapi(itv, CX2341X_DEC_SET_AUDIO_MODE, 2, itv->audio_bilingual_mode, itv->audio_stereo_mode);
 
@@ -646,7 +651,14 @@ static int ivtv_setup_v4l2_decode_stream(struct ivtv_stream *s)
            2 = yuv_from_host */
         switch (s->type) {
         case IVTV_DEC_STREAM_TYPE_YUV:
-                datatype = itv->output_mode == OUT_PASSTHROUGH ? 1 : 2;
+                if (itv->output_mode == OUT_PASSTHROUGH) {
+                        datatype = 1;
+                } else {
+                        /* Fake size to avoid switching video standard */
+                        datatype = 2;
+                        width = 720;
+                        height = itv->is_out_50hz ? 576 : 480;
+                }
                 IVTV_DEBUG_INFO("Setup DEC YUV Stream data[0] = %d\n", datatype);
                 break;
         case IVTV_DEC_STREAM_TYPE_MPG:
@@ -655,9 +667,13 @@ static int ivtv_setup_v4l2_decode_stream(struct ivtv_stream *s)
                 break;
         }
         if (ivtv_vapi(itv, CX2341X_DEC_SET_DECODER_SOURCE, 4, datatype,
-                        p->width, p->height, p->audio_properties)) {
+                        width, height, p->audio_properties)) {
                 IVTV_DEBUG_WARN("Couldn't initialize decoder source\n");
         }
+
+        /* Decoder sometimes dies here, so wait a moment */
+        ivtv_msleep_timeout(10, 0);
+
         return 0;
 }
 
@@ -697,6 +713,9 @@ int ivtv_start_v4l2_decode_stream(struct ivtv_stream *s, int gop_offset)
         /* start playback */
         ivtv_vapi(itv, CX2341X_DEC_START_PLAYBACK, 2, gop_offset, 0);
 
+        /* Let things settle before we actually start */
+        ivtv_msleep_timeout(10, 0);
+
         /* Clear the following Interrupt mask bits for decoding */
         ivtv_clear_irq_mask(itv, IVTV_IRQ_MASK_DECODE);
         IVTV_DEBUG_IRQ("IRQ Mask is now: 0x%08x\n", itv->irqmask);
diff --git a/drivers/media/video/ivtv/ivtvfb.c b/drivers/media/video/ivtv/ivtvfb.c
index 49e1a283ed36..9ff3425891ed 100644
--- a/drivers/media/video/ivtv/ivtvfb.c
+++ b/drivers/media/video/ivtv/ivtvfb.c
@@ -1066,7 +1066,11 @@ static int ivtvfb_init_io(struct ivtv *itv)
         }
         mutex_unlock(&itv->serialize_lock);
 
-        ivtvfb_get_framebuffer(itv, &oi->video_rbase, &oi->video_buffer_size);
+        if (ivtvfb_get_framebuffer(itv, &oi->video_rbase,
+                                        &oi->video_buffer_size) < 0) {
+                IVTVFB_ERR("Firmware failed to respond\n");
+                return -EIO;
+        }
 
         /* The osd buffer size depends on the number of video buffers allocated
            on the PVR350 itself. For now we'll hardcode the smallest osd buffer
@@ -1158,8 +1162,11 @@ static int ivtvfb_init_card(struct ivtv *itv)
         }
 
         /* Find & setup the OSD buffer */
-        if ((rc = ivtvfb_init_io(itv)))
+        rc = ivtvfb_init_io(itv);
+        if (rc) {
+                ivtvfb_release_buffers(itv);
                 return rc;
+        }
 
         /* Set the startup video mode information */
         if ((rc = ivtvfb_init_vidmode(itv))) {
@@ -1210,6 +1217,7 @@ static int ivtvfb_callback_cleanup(struct device *dev, void *p)
 {
         struct v4l2_device *v4l2_dev = dev_get_drvdata(dev);
         struct ivtv *itv = container_of(v4l2_dev, struct ivtv, v4l2_dev);
+        struct osd_info *oi = itv->osd_info;
 
         if (itv && (itv->v4l2_cap & V4L2_CAP_VIDEO_OUTPUT)) {
                 if (unregister_framebuffer(&itv->osd_info->ivtvfb_info)) {
@@ -1218,7 +1226,7 @@ static int ivtvfb_callback_cleanup(struct device *dev, void *p)
                         return 0;
                 }
                 IVTVFB_INFO("Unregister framebuffer %d\n", itv->instance);
-                ivtvfb_blank(FB_BLANK_POWERDOWN, &itv->osd_info->ivtvfb_info);
+                ivtvfb_blank(FB_BLANK_VSYNC_SUSPEND, &oi->ivtvfb_info);
                 ivtvfb_release_buffers(itv);
                 itv->osd_video_pbase = 0;
         }
diff --git a/drivers/media/video/mt9m001.c b/drivers/media/video/mt9m001.c
index b62c0bd3f8ea..79f096ddcf5d 100644
--- a/drivers/media/video/mt9m001.c
+++ b/drivers/media/video/mt9m001.c
@@ -701,13 +701,13 @@ static struct v4l2_subdev_core_ops mt9m001_subdev_core_ops = {
 #endif
 };
 
-static int mt9m001_enum_fmt(struct v4l2_subdev *sd, int index,
+static int mt9m001_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
                             enum v4l2_mbus_pixelcode *code)
 {
         struct i2c_client *client = sd->priv;
         struct mt9m001 *mt9m001 = to_mt9m001(client);
 
-        if ((unsigned int)index >= mt9m001->num_fmts)
+        if (index >= mt9m001->num_fmts)
                 return -EINVAL;
 
         *code = mt9m001->fmts[index].code;
@@ -785,7 +785,6 @@ static int mt9m001_probe(struct i2c_client *client,
         ret = mt9m001_video_probe(icd, client);
         if (ret) {
                 icd->ops = NULL;
-                i2c_set_clientdata(client, NULL);
                 kfree(mt9m001);
         }
 
@@ -799,7 +798,6 @@ static int mt9m001_remove(struct i2c_client *client)
 
         icd->ops = NULL;
         mt9m001_video_remove(icd);
-        i2c_set_clientdata(client, NULL);
         client->driver = NULL;
         kfree(mt9m001);
 
diff --git a/drivers/media/video/mt9m111.c b/drivers/media/video/mt9m111.c
index d35f536f9fc3..fbd0fc794720 100644
--- a/drivers/media/video/mt9m111.c
+++ b/drivers/media/video/mt9m111.c
@@ -999,10 +999,10 @@ static struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
 #endif
 };
 
-static int mt9m111_enum_fmt(struct v4l2_subdev *sd, int index,
+static int mt9m111_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
                             enum v4l2_mbus_pixelcode *code)
 {
-        if ((unsigned int)index >= ARRAY_SIZE(mt9m111_colour_fmts))
+        if (index >= ARRAY_SIZE(mt9m111_colour_fmts))
                 return -EINVAL;
 
         *code = mt9m111_colour_fmts[index].code;
@@ -1068,7 +1068,6 @@ static int mt9m111_probe(struct i2c_client *client,
         ret = mt9m111_video_probe(icd, client);
         if (ret) {
                 icd->ops = NULL;
-                i2c_set_clientdata(client, NULL);
                 kfree(mt9m111);
         }
 
@@ -1081,7 +1080,6 @@ static int mt9m111_remove(struct i2c_client *client)
         struct soc_camera_device *icd = client->dev.platform_data;
 
         icd->ops = NULL;
-        i2c_set_clientdata(client, NULL);
         client->driver = NULL;
         kfree(mt9m111);
 
diff --git a/drivers/media/video/mt9t031.c b/drivers/media/video/mt9t031.c
index 78b4e091d2d5..a9a28b214235 100644
--- a/drivers/media/video/mt9t031.c
+++ b/drivers/media/video/mt9t031.c
@@ -798,7 +798,7 @@ static struct v4l2_subdev_core_ops mt9t031_subdev_core_ops = {
 #endif
 };
 
-static int mt9t031_enum_fmt(struct v4l2_subdev *sd, int index,
+static int mt9t031_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
                             enum v4l2_mbus_pixelcode *code)
 {
         if (index)
@@ -883,7 +883,6 @@ static int mt9t031_probe(struct i2c_client *client,
         if (ret) {
                 if (icd)
                         icd->ops = NULL;
-                i2c_set_clientdata(client, NULL);
                 kfree(mt9t031);
         }
 
@@ -897,7 +896,6 @@ static int mt9t031_remove(struct i2c_client *client)
 
         if (icd)
                 icd->ops = NULL;
-        i2c_set_clientdata(client, NULL);
         client->driver = NULL;
         kfree(mt9t031);
 
diff --git a/drivers/media/video/mt9t112.c b/drivers/media/video/mt9t112.c
index 7438f8d775ba..e4bf1db9a87b 100644
--- a/drivers/media/video/mt9t112.c
+++ b/drivers/media/video/mt9t112.c
@@ -1017,10 +1017,10 @@ static int mt9t112_try_fmt(struct v4l2_subdev *sd,
         return 0;
 }
 
-static int mt9t112_enum_fmt(struct v4l2_subdev *sd, int index,
+static int mt9t112_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
                            enum v4l2_mbus_pixelcode *code)
 {
-        if ((unsigned int)index >= ARRAY_SIZE(mt9t112_cfmts))
+        if (index >= ARRAY_SIZE(mt9t112_cfmts))
                 return -EINVAL;
 
         *code = mt9t112_cfmts[index].code;
@@ -1119,7 +1119,6 @@ static int mt9t112_probe(struct i2c_client *client,
         ret = mt9t112_camera_probe(icd, client);
         if (ret) {
                 icd->ops = NULL;
-                i2c_set_clientdata(client, NULL);
                 kfree(priv);
         }
 
@@ -1132,7 +1131,6 @@ static int mt9t112_remove(struct i2c_client *client)
         struct soc_camera_device *icd = client->dev.platform_data;
 
         icd->ops = NULL;
-        i2c_set_clientdata(client, NULL);
         kfree(priv);
         return 0;
 }
diff --git a/drivers/media/video/mt9v011.c b/drivers/media/video/mt9v011.c
index 72e55be0b4ab..f5e778d5ca9f 100644
--- a/drivers/media/video/mt9v011.c
+++ b/drivers/media/video/mt9v011.c
@@ -392,27 +392,25 @@ static int mt9v011_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
         return 0;
 }
 
-static int mt9v011_enum_fmt(struct v4l2_subdev *sd, struct v4l2_fmtdesc *fmt)
+static int mt9v011_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
+                                        enum v4l2_mbus_pixelcode *code)
 {
-        if (fmt->index > 0)
+        if (index > 0)
                 return -EINVAL;
 
-        fmt->flags = 0;
-        strcpy(fmt->description, "8 bpp Bayer GRGR..BGBG");
-        fmt->pixelformat = V4L2_PIX_FMT_SGRBG8;
-
+        *code = V4L2_MBUS_FMT_SGRBG8_1X8;
         return 0;
 }
 
-static int mt9v011_try_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
+static int mt9v011_try_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
 {
-        struct v4l2_pix_format *pix = &fmt->fmt.pix;
-
-        if (pix->pixelformat != V4L2_PIX_FMT_SGRBG8)
+        if (fmt->code != V4L2_MBUS_FMT_SGRBG8_1X8)
                 return -EINVAL;
 
-        v4l_bound_align_image(&pix->width, 48, 639, 1,
-                              &pix->height, 32, 480, 1, 0);
+        v4l_bound_align_image(&fmt->width, 48, 639, 1,
+                              &fmt->height, 32, 480, 1, 0);
+        fmt->field = V4L2_FIELD_NONE;
+        fmt->colorspace = V4L2_COLORSPACE_SRGB;
 
         return 0;
 }
@@ -455,18 +453,17 @@ static int mt9v011_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
         return 0;
 }
 
-static int mt9v011_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
+static int mt9v011_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
 {
-        struct v4l2_pix_format *pix = &fmt->fmt.pix;
         struct mt9v011 *core = to_mt9v011(sd);
         int rc;
 
-        rc = mt9v011_try_fmt(sd, fmt);
+        rc = mt9v011_try_mbus_fmt(sd, fmt);
         if (rc < 0)
                 return -EINVAL;
 
-        core->width = pix->width;
-        core->height = pix->height;
+        core->width = fmt->width;
+        core->height = fmt->height;
 
         set_res(sd);
 
@@ -549,9 +546,9 @@ static const struct v4l2_subdev_core_ops mt9v011_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops mt9v011_video_ops = {
-        .enum_fmt = mt9v011_enum_fmt,
-        .try_fmt = mt9v011_try_fmt,
-        .s_fmt = mt9v011_s_fmt,
+        .enum_mbus_fmt = mt9v011_enum_mbus_fmt,
+        .try_mbus_fmt = mt9v011_try_mbus_fmt,
+        .s_mbus_fmt = mt9v011_s_mbus_fmt,
         .g_parm = mt9v011_g_parm,
         .s_parm = mt9v011_s_parm,
 };
diff --git a/drivers/media/video/mt9v022.c b/drivers/media/video/mt9v022.c
index e5bae4c9393b..e7cd23cd6394 100644
--- a/drivers/media/video/mt9v022.c
+++ b/drivers/media/video/mt9v022.c
@@ -838,13 +838,13 @@ static struct v4l2_subdev_core_ops mt9v022_subdev_core_ops = {
 #endif
 };
 
-static int mt9v022_enum_fmt(struct v4l2_subdev *sd, int index,
+static int mt9v022_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
                             enum v4l2_mbus_pixelcode *code)
 {
         struct i2c_client *client = sd->priv;
         struct mt9v022 *mt9v022 = to_mt9v022(client);
 
-        if ((unsigned int)index >= mt9v022->num_fmts)
+        if (index >= mt9v022->num_fmts)
                 return -EINVAL;
 
         *code = mt9v022->fmts[index].code;
@@ -920,7 +920,6 @@ static int mt9v022_probe(struct i2c_client *client,
         ret = mt9v022_video_probe(icd, client);
         if (ret) {
                 icd->ops = NULL;
-                i2c_set_clientdata(client, NULL);
                 kfree(mt9v022);
         }
 
@@ -934,7 +933,6 @@ static int mt9v022_remove(struct i2c_client *client)
 
         icd->ops = NULL;
         mt9v022_video_remove(icd);
-        i2c_set_clientdata(client, NULL);
         client->driver = NULL;
         kfree(mt9v022);
 
diff --git a/drivers/media/video/mx3_camera.c b/drivers/media/video/mx3_camera.c
index d477e3058002..a9be14c23912 100644
--- a/drivers/media/video/mx3_camera.c
+++ b/drivers/media/video/mx3_camera.c
@@ -672,7 +672,7 @@ static bool mx3_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
                  fmt->packing == SOC_MBUS_PACKING_EXTEND16);
 }
 
-static int mx3_camera_get_formats(struct soc_camera_device *icd, int idx,
+static int mx3_camera_get_formats(struct soc_camera_device *icd, unsigned int idx,
                                   struct soc_camera_format_xlate *xlate)
 {
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
@@ -689,7 +689,7 @@ static int mx3_camera_get_formats(struct soc_camera_device *icd, int idx,
         fmt = soc_mbus_get_fmtdesc(code);
         if (!fmt) {
                 dev_err(icd->dev.parent,
-                        "Invalid format code #%d: %d\n", idx, code);
+                        "Invalid format code #%u: %d\n", idx, code);
                 return 0;
         }
 
diff --git a/drivers/media/video/omap/omap_vout.c b/drivers/media/video/omap/omap_vout.c
index 4c0ab499228b..e7db0554949a 100644
--- a/drivers/media/video/omap/omap_vout.c
+++ b/drivers/media/video/omap/omap_vout.c
@@ -2371,12 +2371,11 @@ static int __init omap_vout_create_video_devices(struct platform_device *pdev)
 
         for (k = 0; k < pdev->num_resources; k++) {
 
-                vout = kmalloc(sizeof(struct omap_vout_device), GFP_KERNEL);
+                vout = kzalloc(sizeof(struct omap_vout_device), GFP_KERNEL);
                 if (!vout) {
                         dev_err(&pdev->dev, ": could not allocate memory\n");
                         return -ENOMEM;
                 }
-                memset(vout, 0, sizeof(struct omap_vout_device));
 
                 vout->vid = k;
                 vid_dev->vouts[k] = vout;
diff --git a/drivers/media/video/ov772x.c b/drivers/media/video/ov772x.c
index 7f8ece30c77b..34034a710214 100644
--- a/drivers/media/video/ov772x.c
+++ b/drivers/media/video/ov772x.c
@@ -1092,10 +1092,10 @@ static struct v4l2_subdev_core_ops ov772x_subdev_core_ops = {
 #endif
 };
 
-static int ov772x_enum_fmt(struct v4l2_subdev *sd, int index,
+static int ov772x_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
                            enum v4l2_mbus_pixelcode *code)
 {
-        if ((unsigned int)index >= ARRAY_SIZE(ov772x_cfmts))
+        if (index >= ARRAY_SIZE(ov772x_cfmts))
                 return -EINVAL;
 
         *code = ov772x_cfmts[index].code;
@@ -1159,7 +1159,6 @@ static int ov772x_probe(struct i2c_client *client,
         ret = ov772x_video_probe(icd, client);
         if (ret) {
                 icd->ops = NULL;
-                i2c_set_clientdata(client, NULL);
                 kfree(priv);
         }
 
@@ -1172,7 +1171,6 @@ static int ov772x_remove(struct i2c_client *client)
         struct soc_camera_device *icd = client->dev.platform_data;
 
         icd->ops = NULL;
-        i2c_set_clientdata(client, NULL);
         kfree(priv);
         return 0;
 }
diff --git a/drivers/media/video/ov9640.c b/drivers/media/video/ov9640.c
index 36599a65f548..7ce9e05b4781 100644
--- a/drivers/media/video/ov9640.c
+++ b/drivers/media/video/ov9640.c
@@ -614,10 +614,10 @@ static int ov9640_try_fmt(struct v4l2_subdev *sd,
         return 0;
 }
 
-static int ov9640_enum_fmt(struct v4l2_subdev *sd, int index,
+static int ov9640_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
                            enum v4l2_mbus_pixelcode *code)
 {
-        if ((unsigned int)index >= ARRAY_SIZE(ov9640_codes))
+        if (index >= ARRAY_SIZE(ov9640_codes))
                 return -EINVAL;
 
         *code = ov9640_codes[index];
@@ -783,7 +783,6 @@ static int ov9640_probe(struct i2c_client *client,
 
         if (ret) {
                 icd->ops = NULL;
-                i2c_set_clientdata(client, NULL);
                 kfree(priv);
         }
 
@@ -794,7 +793,6 @@ static int ov9640_remove(struct i2c_client *client)
 {
         struct ov9640_priv *priv = i2c_get_clientdata(client);
 
-        i2c_set_clientdata(client, NULL);
         kfree(priv);
         return 0;
 }
diff --git a/drivers/media/video/pvrusb2/pvrusb2-devattr.c b/drivers/media/video/pvrusb2/pvrusb2-devattr.c
index 6bc16c13ccef..3092abfd66a2 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-devattr.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-devattr.c
@@ -117,6 +117,7 @@ static const struct pvr2_device_desc pvr2_device_24xxx = {
 static const struct pvr2_device_client_desc pvr2_cli_gotview_2[] = {
         { .module_id = PVR2_CLIENT_ID_CX25840 },
         { .module_id = PVR2_CLIENT_ID_TUNER },
+        { .module_id = PVR2_CLIENT_ID_DEMOD },
 };
 
 static const struct pvr2_device_desc pvr2_device_gotview_2 = {
diff --git a/drivers/media/video/pvrusb2/pvrusb2-devattr.h b/drivers/media/video/pvrusb2/pvrusb2-devattr.h
index e5b9594eb5f6..273c8d4b3853 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-devattr.h
+++ b/drivers/media/video/pvrusb2/pvrusb2-devattr.h
@@ -177,6 +177,11 @@ struct pvr2_device_desc {
         unsigned int flag_has_composite:1;     /* Has composite input */
         unsigned int flag_has_svideo:1;        /* Has s-video input */
         unsigned int flag_fx2_16kb:1;          /* 16KB FX2 firmware OK here */
+
+        /* If this driver is considered experimental, i.e. not all aspects
+           are working correctly and/or it is untested, mark that fact
+           with this flag. */
+        unsigned int flag_is_experimental:1;
 };
 
 extern struct usb_device_id pvr2_device_table[];
diff --git a/drivers/media/video/pvrusb2/pvrusb2-hdw.c b/drivers/media/video/pvrusb2/pvrusb2-hdw.c
index 301ef197d038..70ea578d6266 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-hdw.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-hdw.c
@@ -2459,6 +2459,19 @@ struct pvr2_hdw *pvr2_hdw_create(struct usb_interface *intf,
                    hdw,hdw_desc->description);
         pvr2_trace(PVR2_TRACE_INFO, "Hardware description: %s",
                 hdw_desc->description);
+        if (hdw_desc->flag_is_experimental) {
+                pvr2_trace(PVR2_TRACE_INFO, "**********");
+                pvr2_trace(PVR2_TRACE_INFO,
+                           "WARNING: Support for this device (%s) is"
+                           " experimental.", hdw_desc->description);
+                pvr2_trace(PVR2_TRACE_INFO,
+                           "Important functionality might not be"
+                           " entirely working.");
+                pvr2_trace(PVR2_TRACE_INFO,
+                           "Please consider contacting the driver author to"
+                           " help with further stabilization of the driver.");
+                pvr2_trace(PVR2_TRACE_INFO, "**********");
+        }
         if (!hdw) goto fail;
 
         init_timer(&hdw->quiescent_timer);
@@ -3056,14 +3069,14 @@ static void pvr2_subdev_update(struct pvr2_hdw *hdw)
         }
 
         if (hdw->res_hor_dirty || hdw->res_ver_dirty || hdw->force_dirty) {
-                struct v4l2_format fmt;
+                struct v4l2_mbus_framefmt fmt;
                 memset(&fmt, 0, sizeof(fmt));
-                fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-                fmt.fmt.pix.width = hdw->res_hor_val;
-                fmt.fmt.pix.height = hdw->res_ver_val;
+                fmt.width = hdw->res_hor_val;
+                fmt.height = hdw->res_ver_val;
+                fmt.code = V4L2_MBUS_FMT_FIXED;
                 pvr2_trace(PVR2_TRACE_CHIPS, "subdev v4l2 set_size(%dx%d)",
-                           fmt.fmt.pix.width, fmt.fmt.pix.height);
-                v4l2_device_call_all(&hdw->v4l2_dev, 0, video, s_fmt, &fmt);
+                           fmt.width, fmt.height);
+                v4l2_device_call_all(&hdw->v4l2_dev, 0, video, s_mbus_fmt, &fmt);
         }
 
         if (hdw->srate_dirty || hdw->force_dirty) {
@@ -4084,12 +4097,20 @@ void pvr2_hdw_device_reset(struct pvr2_hdw *hdw)
 
 void pvr2_hdw_cpureset_assert(struct pvr2_hdw *hdw,int val)
 {
-        char da[1];
+        char *da;
         unsigned int pipe;
         int ret;
 
         if (!hdw->usb_dev) return;
 
+        da = kmalloc(16, GFP_KERNEL);
+
+        if (da == NULL) {
+                pvr2_trace(PVR2_TRACE_ERROR_LEGS,
+                           "Unable to allocate memory to control CPU reset");
+                return;
+        }
+
         pvr2_trace(PVR2_TRACE_INIT,"cpureset_assert(%d)",val);
 
         da[0] = val ? 0x01 : 0x00;
@@ -4103,6 +4124,8 @@ void pvr2_hdw_cpureset_assert(struct pvr2_hdw *hdw,int val)
                            "cpureset_assert(%d) error=%d",val,ret);
                 pvr2_hdw_render_useless(hdw);
         }
+
+        kfree(da);
 }
 
 
diff --git a/drivers/media/video/pvrusb2/pvrusb2-main.c b/drivers/media/video/pvrusb2/pvrusb2-main.c
index eeacd0f67855..2254194aad57 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-main.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-main.c
@@ -153,12 +153,12 @@ static void __exit pvr_exit(void)
 
         usb_deregister(&pvr_driver);
 
+        pvr2_context_global_done();
+
 #ifdef CONFIG_VIDEO_PVRUSB2_SYSFS
         pvr2_sysfs_class_destroy(class_ptr);
 #endif /* CONFIG_VIDEO_PVRUSB2_SYSFS */
 
-        pvr2_context_global_done();
-
         pvr2_trace(PVR2_TRACE_INIT,"pvr_exit complete");
 }
 
diff --git a/drivers/media/video/pvrusb2/pvrusb2-sysfs.c b/drivers/media/video/pvrusb2/pvrusb2-sysfs.c
index 71f50565f637..3d7e5aab547f 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-sysfs.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-sysfs.c
@@ -74,7 +74,7 @@ struct pvr2_sysfs_ctl_item {
         int ctl_id;
         struct pvr2_sysfs *chptr;
         struct pvr2_sysfs_ctl_item *item_next;
-        struct attribute *attr_gen[7];
+        struct attribute *attr_gen[8];
         struct attribute_group grp;
         int created_ok;
         char name[80];
@@ -511,6 +511,7 @@ static void pvr2_sysfs_release(struct device *class_dev)
 
 static void class_dev_destroy(struct pvr2_sysfs *sfp)
 {
+        struct device *dev;
         if (!sfp->class_dev) return;
 #ifdef CONFIG_VIDEO_PVRUSB2_DEBUGIFC
         pvr2_sysfs_tear_down_debugifc(sfp);
@@ -542,6 +543,9 @@ static void class_dev_destroy(struct pvr2_sysfs *sfp)
         }
         pvr2_sysfs_trace("Destroying class_dev id=%p",sfp->class_dev);
         dev_set_drvdata(sfp->class_dev, NULL);
+        dev = sfp->class_dev->parent;
+        sfp->class_dev->parent = NULL;
+        put_device(dev);
         device_unregister(sfp->class_dev);
         sfp->class_dev = NULL;
 }
@@ -631,10 +635,11 @@ static void class_dev_create(struct pvr2_sysfs *sfp,
         pvr2_sysfs_trace("Creating class_dev id=%p",class_dev);
 
         class_dev->class = &class_ptr->class;
+
         dev_set_name(class_dev, "%s",
                      pvr2_hdw_get_device_identifier(sfp->channel.hdw));
 
-        class_dev->parent = &usb_dev->dev;
+        class_dev->parent = get_device(&usb_dev->dev);
 
         sfp->class_dev = class_dev;
         dev_set_drvdata(class_dev, sfp);
@@ -775,7 +780,8 @@ struct pvr2_sysfs_class *pvr2_sysfs_class_create(void)
         struct pvr2_sysfs_class *clp;
         clp = kzalloc(sizeof(*clp),GFP_KERNEL);
         if (!clp) return clp;
-        pvr2_sysfs_trace("Creating pvr2_sysfs_class id=%p",clp);
+        pvr2_sysfs_trace("Creating and registering pvr2_sysfs_class id=%p",
+                         clp);
         clp->class.name = "pvrusb2";
         clp->class.class_release = pvr2_sysfs_class_release;
         clp->class.dev_release = pvr2_sysfs_release;
@@ -791,6 +797,7 @@ struct pvr2_sysfs_class *pvr2_sysfs_class_create(void)
 
 void pvr2_sysfs_class_destroy(struct pvr2_sysfs_class *clp)
 {
+        pvr2_sysfs_trace("Unregistering pvr2_sysfs_class id=%p", clp);
         class_unregister(&clp->class);
 }
 
diff --git a/drivers/media/video/pvrusb2/pvrusb2-v4l2.c b/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
index 5ffa0d2b0b0d..aaafa0398fd5 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-v4l2.c
@@ -883,6 +883,17 @@ static void pvr2_v4l2_dev_destroy(struct pvr2_v4l2_dev *dip)
 {
         struct pvr2_hdw *hdw = dip->v4lp->channel.mc_head->hdw;
         enum pvr2_config cfg = dip->config;
+        char msg[80];
+        unsigned int mcnt;
+
+        /* Construct the unregistration message *before* we actually
+           perform the unregistration step.  By doing it this way we don't
+           have to worry about potentially touching deleted resources. */
+        mcnt = scnprintf(msg, sizeof(msg) - 1,
+                         "pvrusb2: unregistered device %s [%s]",
+                         video_device_node_name(&dip->devbase),
+                         pvr2_config_get_name(cfg));
+        msg[mcnt] = 0;
 
         pvr2_hdw_v4l_store_minor_number(hdw,dip->minor_type,-1);
 
@@ -894,9 +905,7 @@ static void pvr2_v4l2_dev_destroy(struct pvr2_v4l2_dev *dip)
            are gone. */
         video_unregister_device(&dip->devbase);
 
-        printk(KERN_INFO "pvrusb2: unregistered device %s [%s]\n",
-               video_device_node_name(&dip->devbase),
-               pvr2_config_get_name(cfg));
+        printk(KERN_INFO "%s\n", msg);
 
 }
 
diff --git a/drivers/media/video/pxa_camera.c b/drivers/media/video/pxa_camera.c
index 7fe70e718656..fb242f6cfb1f 100644
--- a/drivers/media/video/pxa_camera.c
+++ b/drivers/media/video/pxa_camera.c
@@ -1247,7 +1247,7 @@ static bool pxa_camera_packing_supported(const struct soc_mbus_pixelfmt *fmt)
                  fmt->packing == SOC_MBUS_PACKING_EXTEND16);
 }
 
-static int pxa_camera_get_formats(struct soc_camera_device *icd, int idx,
+static int pxa_camera_get_formats(struct soc_camera_device *icd, unsigned int idx,
                                   struct soc_camera_format_xlate *xlate)
 {
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
@@ -1264,7 +1264,7 @@ static int pxa_camera_get_formats(struct soc_camera_device *icd, int idx,
 
         fmt = soc_mbus_get_fmtdesc(code);
         if (!fmt) {
-                dev_err(dev, "Invalid format code #%d: %d\n", idx, code);
+                dev_err(dev, "Invalid format code #%u: %d\n", idx, code);
                 return 0;
         }
 
diff --git a/drivers/media/video/rj54n1cb0c.c b/drivers/media/video/rj54n1cb0c.c
index bbd9c11e2c5a..47fd207ba3b1 100644
--- a/drivers/media/video/rj54n1cb0c.c
+++ b/drivers/media/video/rj54n1cb0c.c
@@ -481,10 +481,10 @@ static int reg_write_multiple(struct i2c_client *client,
         return 0;
 }
 
-static int rj54n1_enum_fmt(struct v4l2_subdev *sd, int index,
+static int rj54n1_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
                            enum v4l2_mbus_pixelcode *code)
 {
-        if ((unsigned int)index >= ARRAY_SIZE(rj54n1_colour_fmts))
+        if (index >= ARRAY_SIZE(rj54n1_colour_fmts))
                 return -EINVAL;
 
         *code = rj54n1_colour_fmts[index].code;
@@ -1444,7 +1444,6 @@ static int rj54n1_probe(struct i2c_client *client,
         ret = rj54n1_video_probe(icd, client, rj54n1_priv);
         if (ret < 0) {
                 icd->ops = NULL;
-                i2c_set_clientdata(client, NULL);
                 kfree(rj54n1);
                 return ret;
         }
@@ -1461,7 +1460,6 @@ static int rj54n1_remove(struct i2c_client *client)
         icd->ops = NULL;
         if (icl->free_bus)
                 icl->free_bus(icl);
-        i2c_set_clientdata(client, NULL);
         client->driver = NULL;
         kfree(rj54n1);
 
diff --git a/drivers/media/video/saa7115.c b/drivers/media/video/saa7115.c
index 53b6fcde3800..76da74368680 100644
--- a/drivers/media/video/saa7115.c
+++ b/drivers/media/video/saa7115.c
@@ -1117,13 +1117,6 @@ static int saa711x_g_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_f
         return 0;
 }
 
-static int saa711x_g_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
-{
-        if (fmt->type != V4L2_BUF_TYPE_SLICED_VBI_CAPTURE)
-                return -EINVAL;
-        return saa711x_g_sliced_fmt(sd, &fmt->fmt.sliced);
-}
-
 static int saa711x_s_raw_fmt(struct v4l2_subdev *sd, struct v4l2_vbi_format *fmt)
 {
         saa711x_set_lcr(sd, NULL);
@@ -1136,12 +1129,13 @@ static int saa711x_s_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_f
         return 0;
 }
 
-static int saa711x_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
+static int saa711x_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
 {
-        if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+        if (fmt->code != V4L2_MBUS_FMT_FIXED)
                 return -EINVAL;
-
-        return saa711x_set_size(sd, fmt->fmt.pix.width, fmt->fmt.pix.height);
+        fmt->field = V4L2_FIELD_INTERLACED;
+        fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+        return saa711x_set_size(sd, fmt->width, fmt->height);
 }
 
 /* Decode the sliced VBI data stream as created by the saa7115.
@@ -1556,8 +1550,7 @@ static const struct v4l2_subdev_audio_ops saa711x_audio_ops = {
 static const struct v4l2_subdev_video_ops saa711x_video_ops = {
         .s_routing = saa711x_s_routing,
         .s_crystal_freq = saa711x_s_crystal_freq,
-        .g_fmt = saa711x_g_fmt,
-        .s_fmt = saa711x_s_fmt,
+        .s_mbus_fmt = saa711x_s_mbus_fmt,
         .s_stream = saa711x_s_stream,
         .querystd = saa711x_querystd,
         .g_input_status = saa711x_g_input_status,
diff --git a/drivers/media/video/saa7127.c b/drivers/media/video/saa7127.c
index 87986ad62f86..79fffcf39ba8 100644
--- a/drivers/media/video/saa7127.c
+++ b/drivers/media/video/saa7127.c
@@ -645,13 +645,6 @@ static int saa7127_g_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_f
         return 0;
 }
 
-static int saa7127_g_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
-{
-        if (fmt->type != V4L2_BUF_TYPE_SLICED_VBI_CAPTURE)
-                return -EINVAL;
-        return saa7127_g_sliced_fmt(sd, &fmt->fmt.sliced);
-}
-
 static int saa7127_s_vbi_data(struct v4l2_subdev *sd, const struct v4l2_sliced_vbi_data *data)
 {
         switch (data->id) {
@@ -731,7 +724,6 @@ static const struct v4l2_subdev_core_ops saa7127_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops saa7127_video_ops = {
-        .g_fmt = saa7127_g_fmt,
         .s_std_output = saa7127_s_std_output,
         .s_routing = saa7127_s_routing,
         .s_stream = saa7127_s_stream,
diff --git a/drivers/media/video/saa7134/saa6752hs.c b/drivers/media/video/saa7134/saa6752hs.c
index 1eabff6b2456..40fd31ca7716 100644
--- a/drivers/media/video/saa7134/saa6752hs.c
+++ b/drivers/media/video/saa7134/saa6752hs.c
@@ -846,24 +846,28 @@ static int saa6752hs_g_ext_ctrls(struct v4l2_subdev *sd, struct v4l2_ext_control
         return 0;
 }
 
-static int saa6752hs_g_fmt(struct v4l2_subdev *sd, struct v4l2_format *f)
+static int saa6752hs_g_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
 {
         struct saa6752hs_state *h = to_state(sd);
 
         if (h->video_format == SAA6752HS_VF_UNKNOWN)
                 h->video_format = SAA6752HS_VF_D1;
-        f->fmt.pix.width =
-                v4l2_format_table[h->video_format].fmt.pix.width;
-        f->fmt.pix.height =
-                v4l2_format_table[h->video_format].fmt.pix.height;
+        f->width = v4l2_format_table[h->video_format].fmt.pix.width;
+        f->height = v4l2_format_table[h->video_format].fmt.pix.height;
+        f->code = V4L2_MBUS_FMT_FIXED;
+        f->field = V4L2_FIELD_INTERLACED;
+        f->colorspace = V4L2_COLORSPACE_SMPTE170M;
         return 0;
 }
 
-static int saa6752hs_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *f)
+static int saa6752hs_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *f)
 {
         struct saa6752hs_state *h = to_state(sd);
         int dist_352, dist_480, dist_720;
 
+        if (f->code != V4L2_MBUS_FMT_FIXED)
+                return -EINVAL;
+
         /*
           FIXME: translate and round width/height into EMPRESS
           subsample type:
@@ -876,28 +880,30 @@ static int saa6752hs_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *f)
           D1     | 720x576 | 720x480
         */
 
-        dist_352 = abs(f->fmt.pix.width - 352);
-        dist_480 = abs(f->fmt.pix.width - 480);
-        dist_720 = abs(f->fmt.pix.width - 720);
+        dist_352 = abs(f->width - 352);
+        dist_480 = abs(f->width - 480);
+        dist_720 = abs(f->width - 720);
         if (dist_720 < dist_480) {
-                f->fmt.pix.width = 720;
-                f->fmt.pix.height = 576;
+                f->width = 720;
+                f->height = 576;
                 h->video_format = SAA6752HS_VF_D1;
         } else if (dist_480 < dist_352) {
-                f->fmt.pix.width = 480;
-                f->fmt.pix.height = 576;
+                f->width = 480;
+                f->height = 576;
                 h->video_format = SAA6752HS_VF_2_3_D1;
         } else {
-                f->fmt.pix.width = 352;
-                if (abs(f->fmt.pix.height - 576) <
-                    abs(f->fmt.pix.height - 288)) {
-                        f->fmt.pix.height = 576;
+                f->width = 352;
+                if (abs(f->height - 576) <
+                    abs(f->height - 288)) {
+                        f->height = 576;
                         h->video_format = SAA6752HS_VF_1_2_D1;
                 } else {
-                        f->fmt.pix.height = 288;
+                        f->height = 288;
                         h->video_format = SAA6752HS_VF_SIF;
                 }
         }
+        f->field = V4L2_FIELD_INTERLACED;
+        f->colorspace = V4L2_COLORSPACE_SMPTE170M;
         return 0;
 }
 
@@ -932,8 +938,8 @@ static const struct v4l2_subdev_core_ops saa6752hs_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops saa6752hs_video_ops = {
-        .s_fmt = saa6752hs_s_fmt,
-        .g_fmt = saa6752hs_g_fmt,
+        .s_mbus_fmt = saa6752hs_s_mbus_fmt,
+        .g_mbus_fmt = saa6752hs_g_mbus_fmt,
 };
 
 static const struct v4l2_subdev_ops saa6752hs_ops = {
diff --git a/drivers/media/video/saa7134/saa7134-cards.c b/drivers/media/video/saa7134/saa7134-cards.c
index 72700d4e3941..07f6bb8ef9d9 100644
--- a/drivers/media/video/saa7134/saa7134-cards.c
+++ b/drivers/media/video/saa7134/saa7134-cards.c
@@ -3897,6 +3897,40 @@ struct saa7134_board saa7134_boards[] = {
                         .gpio = 0x01,
                 },
         },
+        [SAA7134_BOARD_AVERMEDIA_M733A] = {
+                .name           = "Avermedia PCI M733A",
+                .audio_clock    = 0x00187de7,
+                .tuner_type     = TUNER_PHILIPS_TDA8290,
+                .radio_type     = UNSET,
+                .tuner_addr     = ADDR_UNSET,
+                .radio_addr     = ADDR_UNSET,
+                .tuner_config   = 0,
+                .gpiomask       = 0x020200000,
+                .inputs         = {{
+                        .name = name_tv,
+                        .vmux = 1,
+                        .amux = TV,
+                        .tv   = 1,
+                }, {
+                        .name = name_comp1,
+                        .vmux = 3,
+                        .amux = LINE1,
+                }, {
+                        .name = name_svideo,
+                        .vmux = 8,
+                        .amux = LINE1,
+                } },
+                .radio = {
+                        .name = name_radio,
+                        .amux = TV,
+                        .gpio = 0x00200000,
+                },
+                .mute = {
+                        .name = name_mute,
+                        .amux = TV,
+                        .gpio = 0x01,
+                },
+        },
         [SAA7134_BOARD_BEHOLD_401] = {
                 /*       Beholder Intl. Ltd. 2008      */
                 /*Dmitry Belimov <d.belimov@gmail.com> */
@@ -5822,6 +5856,18 @@ struct pci_device_id saa7134_pci_tbl[] = {
                 .driver_data  = SAA7134_BOARD_AVERMEDIA_M135A,
         }, {
                 .vendor       = PCI_VENDOR_ID_PHILIPS,
+                .device       = PCI_DEVICE_ID_PHILIPS_SAA7133,
+                .subvendor    = 0x1461, /* Avermedia Technologies Inc */
+                .subdevice    = 0x4155,
+                .driver_data  = SAA7134_BOARD_AVERMEDIA_M733A,
+        }, {
+                .vendor       = PCI_VENDOR_ID_PHILIPS,
+                .device       = PCI_DEVICE_ID_PHILIPS_SAA7133,
+                .subvendor    = 0x1461, /* Avermedia Technologies Inc */
+                .subdevice    = 0x4255,
+                .driver_data  = SAA7134_BOARD_AVERMEDIA_M733A,
+        }, {
+                .vendor       = PCI_VENDOR_ID_PHILIPS,
                 .device       = PCI_DEVICE_ID_PHILIPS_SAA7130,
                 .subvendor    = PCI_VENDOR_ID_PHILIPS,
                 .subdevice    = 0x2004,
@@ -6786,6 +6832,7 @@ static int saa7134_tda8290_callback(struct saa7134_dev *dev,
         switch (dev->board) {
         case SAA7134_BOARD_HAUPPAUGE_HVR1150:
         case SAA7134_BOARD_HAUPPAUGE_HVR1120:
+        case SAA7134_BOARD_AVERMEDIA_M733A:
                 /* tda8290 + tda18271 */
                 ret = saa7134_tda8290_18271_callback(dev, command, arg);
                 break;
@@ -7087,6 +7134,14 @@ int saa7134_board_init1(struct saa7134_dev *dev)
                 saa_andorl(SAA7134_GPIO_GPMODE0 >> 2,   0x0000C000, 0x0000C000);
                 saa_andorl(SAA7134_GPIO_GPSTATUS0 >> 2, 0x0000C000, 0x0000C000);
                 break;
+        case SAA7134_BOARD_AVERMEDIA_M733A:
+                saa7134_set_gpio(dev, 1, 1);
+                msleep(10);
+                saa7134_set_gpio(dev, 1, 0);
+                msleep(10);
+                saa7134_set_gpio(dev, 1, 1);
+                dev->has_remote = SAA7134_REMOTE_GPIO;
+                break;
         }
         return 0;
 }
diff --git a/drivers/media/video/saa7134/saa7134-empress.c b/drivers/media/video/saa7134/saa7134-empress.c
index ea877a50f52d..e763f9fd0133 100644
--- a/drivers/media/video/saa7134/saa7134-empress.c
+++ b/drivers/media/video/saa7134/saa7134-empress.c
@@ -223,9 +223,11 @@ static int empress_g_fmt_vid_cap(struct file *file, void *priv,
                                 struct v4l2_format *f)
 {
         struct saa7134_dev *dev = file->private_data;
+        struct v4l2_mbus_framefmt mbus_fmt;
 
-        saa_call_all(dev, video, g_fmt, f);
+        saa_call_all(dev, video, g_mbus_fmt, &mbus_fmt);
 
+        v4l2_fill_pix_format(&f->fmt.pix, &mbus_fmt);
         f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
         f->fmt.pix.sizeimage    = TS_PACKET_SIZE * dev->ts.nr_packets;
 
@@ -236,8 +238,11 @@ static int empress_s_fmt_vid_cap(struct file *file, void *priv,
                                 struct v4l2_format *f)
 {
         struct saa7134_dev *dev = file->private_data;
+        struct v4l2_mbus_framefmt mbus_fmt;
 
-        saa_call_all(dev, video, s_fmt, f);
+        v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, V4L2_MBUS_FMT_FIXED);
+        saa_call_all(dev, video, s_mbus_fmt, &mbus_fmt);
+        v4l2_fill_pix_format(&f->fmt.pix, &mbus_fmt);
 
         f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
         f->fmt.pix.sizeimage    = TS_PACKET_SIZE * dev->ts.nr_packets;
diff --git a/drivers/media/video/saa7134/saa7134-input.c b/drivers/media/video/saa7134/saa7134-input.c
index e5565e2fd426..0b336ca6d55b 100644
--- a/drivers/media/video/saa7134/saa7134-input.c
+++ b/drivers/media/video/saa7134/saa7134-input.c
@@ -141,8 +141,8 @@ static int get_key_flydvb_trio(struct IR_i2c *ir, u32 *ir_key, u32 *ir_raw)
         struct saa7134_dev *dev = ir->c->adapter->algo_data;
 
         if (dev == NULL) {
-                dprintk("get_key_flydvb_trio: "
-                         "gir->c->adapter->algo_data is NULL!\n");
+                i2cdprintk("get_key_flydvb_trio: "
+                           "ir->c->adapter->algo_data is NULL!\n");
                 return -EIO;
         }
 
@@ -195,8 +195,8 @@ static int get_key_msi_tvanywhere_plus(struct IR_i2c *ir, u32 *ir_key,
         /* <dev> is needed to access GPIO. Used by the saa_readl macro. */
         struct saa7134_dev *dev = ir->c->adapter->algo_data;
         if (dev == NULL) {
-                dprintk("get_key_msi_tvanywhere_plus: "
-                        "gir->c->adapter->algo_data is NULL!\n");
+                i2cdprintk("get_key_msi_tvanywhere_plus: "
+                           "ir->c->adapter->algo_data is NULL!\n");
                 return -EIO;
         }
 
@@ -657,12 +657,19 @@ int saa7134_input_init1(struct saa7134_dev *dev)
                 saa_setb(SAA7134_GPIO_GPSTATUS0, 0x4);
                 break;
         case SAA7134_BOARD_AVERMEDIA_M135A:
-                ir_codes     = RC_MAP_AVERMEDIA_M135A_RM_JX;
+                ir_codes     = RC_MAP_AVERMEDIA_M135A;
                 mask_keydown = 0x0040000;       /* Enable GPIO18 line on both edges */
                 mask_keyup   = 0x0040000;
                 mask_keycode = 0xffff;
                 raw_decode   = 1;
                 break;
+        case SAA7134_BOARD_AVERMEDIA_M733A:
+                ir_codes     = RC_MAP_AVERMEDIA_M733A_RM_K6;
+                mask_keydown = 0x0040000;
+                mask_keyup   = 0x0040000;
+                mask_keycode = 0xffff;
+                raw_decode   = 1;
+                break;
         case SAA7134_BOARD_AVERMEDIA_777:
         case SAA7134_BOARD_AVERMEDIA_A16AR:
                 ir_codes     = RC_MAP_AVERMEDIA;
@@ -815,7 +822,6 @@ int saa7134_input_init1(struct saa7134_dev *dev)
                 mask_keyup   = 0x020000;
                 polling      = 50; /* ms */
                 break;
-        break;
         }
         if (NULL == ir_codes) {
                 printk("%s: Oops: IR config error [card=%d]\n",
diff --git a/drivers/media/video/saa7134/saa7134.h b/drivers/media/video/saa7134/saa7134.h
index 3962534267be..756a1ca8833d 100644
--- a/drivers/media/video/saa7134/saa7134.h
+++ b/drivers/media/video/saa7134/saa7134.h
@@ -303,6 +303,7 @@ struct saa7134_format {
 #define SAA7134_BOARD_HAWELL_HW_404M7           177
 #define SAA7134_BOARD_BEHOLD_H7             178
 #define SAA7134_BOARD_BEHOLD_A7             179
+#define SAA7134_BOARD_AVERMEDIA_M733A       180
 
 #define SAA7134_MAXBOARDS 32
 #define SAA7134_INPUT_MAX 8
diff --git a/drivers/media/video/saa717x.c b/drivers/media/video/saa717x.c
index d521c648e157..78d69950c00a 100644
--- a/drivers/media/video/saa717x.c
+++ b/drivers/media/video/saa717x.c
@@ -1199,28 +1199,32 @@ static int saa717x_s_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *
 }
 #endif
 
-static int saa717x_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
+static int saa717x_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt)
 {
-        struct v4l2_pix_format *pix;
         int prescale, h_scale, v_scale;
 
-        pix = &fmt->fmt.pix;
         v4l2_dbg(1, debug, sd, "decoder set size\n");
 
+        if (fmt->code != V4L2_MBUS_FMT_FIXED)
+                return -EINVAL;
+
         /* FIXME need better bounds checking here */
-        if (pix->width < 1 || pix->width > 1440)
+        if (fmt->width < 1 || fmt->width > 1440)
                 return -EINVAL;
-        if (pix->height < 1 || pix->height > 960)
+        if (fmt->height < 1 || fmt->height > 960)
                 return -EINVAL;
 
+        fmt->field = V4L2_FIELD_INTERLACED;
+        fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
+
         /* scaling setting */
         /* NTSC and interlace only */
-        prescale = SAA717X_NTSC_WIDTH / pix->width;
+        prescale = SAA717X_NTSC_WIDTH / fmt->width;
         if (prescale == 0)
                 prescale = 1;
-        h_scale = 1024 * SAA717X_NTSC_WIDTH / prescale / pix->width;
+        h_scale = 1024 * SAA717X_NTSC_WIDTH / prescale / fmt->width;
         /* interlace */
-        v_scale = 512 * 2 * SAA717X_NTSC_HEIGHT / pix->height;
+        v_scale = 512 * 2 * SAA717X_NTSC_HEIGHT / fmt->height;
 
         /* Horizontal prescaling etc */
         set_h_prescale(sd, 0, prescale);
@@ -1241,19 +1245,19 @@ static int saa717x_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
         /* set video output size */
         /* video number of pixels at output */
         /* TASK A */
-        saa717x_write(sd, 0x5C, (u8)(pix->width & 0xFF));
-        saa717x_write(sd, 0x5D, (u8)((pix->width >> 8) & 0xFF));
+        saa717x_write(sd, 0x5C, (u8)(fmt->width & 0xFF));
+        saa717x_write(sd, 0x5D, (u8)((fmt->width >> 8) & 0xFF));
         /* TASK B */
-        saa717x_write(sd, 0x9C, (u8)(pix->width & 0xFF));
-        saa717x_write(sd, 0x9D, (u8)((pix->width >> 8) & 0xFF));
+        saa717x_write(sd, 0x9C, (u8)(fmt->width & 0xFF));
+        saa717x_write(sd, 0x9D, (u8)((fmt->width >> 8) & 0xFF));
 
         /* video number of lines at output */
         /* TASK A */
-        saa717x_write(sd, 0x5E, (u8)(pix->height & 0xFF));
-        saa717x_write(sd, 0x5F, (u8)((pix->height >> 8) & 0xFF));
+        saa717x_write(sd, 0x5E, (u8)(fmt->height & 0xFF));
+        saa717x_write(sd, 0x5F, (u8)((fmt->height >> 8) & 0xFF));
         /* TASK B */
-        saa717x_write(sd, 0x9E, (u8)(pix->height & 0xFF));
-        saa717x_write(sd, 0x9F, (u8)((pix->height >> 8) & 0xFF));
+        saa717x_write(sd, 0x9E, (u8)(fmt->height & 0xFF));
+        saa717x_write(sd, 0x9F, (u8)((fmt->height >> 8) & 0xFF));
         return 0;
 }
 
@@ -1403,7 +1407,7 @@ static const struct v4l2_subdev_tuner_ops saa717x_tuner_ops = {
 
 static const struct v4l2_subdev_video_ops saa717x_video_ops = {
         .s_routing = saa717x_s_video_routing,
-        .s_fmt = saa717x_s_fmt,
+        .s_mbus_fmt = saa717x_s_mbus_fmt,
         .s_stream = saa717x_s_stream,
 };
 
diff --git a/drivers/media/video/sh_mobile_ceu_camera.c b/drivers/media/video/sh_mobile_ceu_camera.c
index 4ac3b482fbb4..961bfa2fea97 100644
--- a/drivers/media/video/sh_mobile_ceu_camera.c
+++ b/drivers/media/video/sh_mobile_ceu_camera.c
@@ -878,7 +878,7 @@ static bool sh_mobile_ceu_packing_supported(const struct soc_mbus_pixelfmt *fmt)
 
 static int client_g_rect(struct v4l2_subdev *sd, struct v4l2_rect *rect);
 
-static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, int idx,
+static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, unsigned int idx,
                                      struct soc_camera_format_xlate *xlate)
 {
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
@@ -897,7 +897,7 @@ static int sh_mobile_ceu_get_formats(struct soc_camera_device *icd, int idx,
         fmt = soc_mbus_get_fmtdesc(code);
         if (!fmt) {
                 dev_err(icd->dev.parent,
-                        "Invalid format code #%d: %d\n", idx, code);
+                        "Invalid format code #%u: %d\n", idx, code);
                 return -EINVAL;
         }
 
diff --git a/drivers/media/video/sn9c102/sn9c102_devtable.h b/drivers/media/video/sn9c102/sn9c102_devtable.h
index 522ba3f4c285..b6643ca7656a 100644
--- a/drivers/media/video/sn9c102/sn9c102_devtable.h
+++ b/drivers/media/video/sn9c102/sn9c102_devtable.h
@@ -62,8 +62,8 @@ static const struct usb_device_id sn9c102_id_table[] = {
 #if !defined CONFIG_USB_GSPCA_SONIXB && !defined CONFIG_USB_GSPCA_SONIXB_MODULE
         { SN9C102_USB_DEVICE(0x0c45, 0x602c, BRIDGE_SN9C102), },
 /*      { SN9C102_USB_DEVICE(0x0c45, 0x602d, BRIDGE_SN9C102), }, HV7131R */
-#endif
         { SN9C102_USB_DEVICE(0x0c45, 0x602e, BRIDGE_SN9C102), },
+#endif
         { SN9C102_USB_DEVICE(0x0c45, 0x6030, BRIDGE_SN9C102), },
         /* SN9C103 */
         { SN9C102_USB_DEVICE(0x0c45, 0x6080, BRIDGE_SN9C103), },
diff --git a/drivers/media/video/soc_camera.c b/drivers/media/video/soc_camera.c
index db1ca0e90d76..475757bfd7ba 100644
--- a/drivers/media/video/soc_camera.c
+++ b/drivers/media/video/soc_camera.c
@@ -200,7 +200,8 @@ static int soc_camera_init_user_formats(struct soc_camera_device *icd)
 {
         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
         struct soc_camera_host *ici = to_soc_camera_host(icd->dev.parent);
-        int i, fmts = 0, raw_fmts = 0, ret;
+        unsigned int i, fmts = 0, raw_fmts = 0;
+        int ret;
         enum v4l2_mbus_pixelcode code;
 
         while (!v4l2_subdev_call(sd, video, enum_mbus_fmt, raw_fmts, &code))
diff --git a/drivers/media/video/soc_camera_platform.c b/drivers/media/video/soc_camera_platform.c
index 10b003a8be83..248c986f0989 100644
--- a/drivers/media/video/soc_camera_platform.c
+++ b/drivers/media/video/soc_camera_platform.c
@@ -71,7 +71,7 @@ static int soc_camera_platform_try_fmt(struct v4l2_subdev *sd,
 
 static struct v4l2_subdev_core_ops platform_subdev_core_ops;
 
-static int soc_camera_platform_enum_fmt(struct v4l2_subdev *sd, int index,
+static int soc_camera_platform_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
                                         enum v4l2_mbus_pixelcode *code)
 {
         struct soc_camera_platform_info *p = v4l2_get_subdevdata(sd);
diff --git a/drivers/media/video/tcm825x.c b/drivers/media/video/tcm825x.c
index b90e9da3167d..54681a535822 100644
--- a/drivers/media/video/tcm825x.c
+++ b/drivers/media/video/tcm825x.c
@@ -850,7 +850,6 @@ static int tcm825x_probe(struct i2c_client *client,
                          const struct i2c_device_id *did)
 {
         struct tcm825x_sensor *sensor = &tcm825x;
-        int rval;
 
         if (i2c_get_clientdata(client))
                 return -EBUSY;
@@ -871,11 +870,7 @@ static int tcm825x_probe(struct i2c_client *client,
         sensor->pix.height = tcm825x_sizes[QVGA].height;
         sensor->pix.pixelformat = V4L2_PIX_FMT_RGB565;
 
-        rval = v4l2_int_device_register(sensor->v4l2_int_device);
-        if (rval)
-                i2c_set_clientdata(client, NULL);
-
-        return rval;
+        return v4l2_int_device_register(sensor->v4l2_int_device);
 }
 
 static int tcm825x_remove(struct i2c_client *client)
@@ -886,7 +881,6 @@ static int tcm825x_remove(struct i2c_client *client)
                 return -ENODEV; /* our client isn't attached */
 
         v4l2_int_device_unregister(sensor->v4l2_int_device);
-        i2c_set_clientdata(client, NULL);
 
         return 0;
 }
diff --git a/drivers/media/video/tvp514x.c b/drivers/media/video/tvp514x.c
index e826114b7fb8..71c73fa0d68c 100644
--- a/drivers/media/video/tvp514x.c
+++ b/drivers/media/video/tvp514x.c
@@ -88,9 +88,6 @@ static int tvp514x_s_stream(struct v4l2_subdev *sd, int enable);
  * @pdata: Board specific
  * @ver: Chip version
  * @streaming: TVP5146/47 decoder streaming - enabled or disabled.
- * @pix: Current pixel format
- * @num_fmts: Number of formats
- * @fmt_list: Format list
  * @current_std: Current standard
  * @num_stds: Number of standards
  * @std_list: Standards list
@@ -105,13 +102,9 @@ struct tvp514x_decoder {
         int ver;
         int streaming;
 
-        struct v4l2_pix_format pix;
-        int num_fmts;
-        const struct v4l2_fmtdesc *fmt_list;
-
         enum tvp514x_std current_std;
         int num_stds;
-        struct tvp514x_std_info *std_list;
+        const struct tvp514x_std_info *std_list;
         /* Input and Output Routing parameters */
         u32 input;
         u32 output;
@@ -203,27 +196,12 @@ static struct tvp514x_reg tvp514x_reg_list_default[] = {
 };
 
 /**
- * List of image formats supported by TVP5146/47 decoder
- * Currently we are using 8 bit mode only, but can be
- * extended to 10/20 bit mode.
- */
-static const struct v4l2_fmtdesc tvp514x_fmt_list[] = {
-        {
-         .index = 0,
-         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
-         .flags = 0,
-         .description = "8-bit UYVY 4:2:2 Format",
-         .pixelformat = V4L2_PIX_FMT_UYVY,
-        },
-};
-
-/**
  * Supported standards -
  *
  * Currently supports two standards only, need to add support for rest of the
  * modes, like SECAM, etc...
  */
-static struct tvp514x_std_info tvp514x_std_list[] = {
+static const struct tvp514x_std_info tvp514x_std_list[] = {
         /* Standard: STD_NTSC_MJ */
         [STD_NTSC_MJ] = {
          .width = NTSC_NUM_ACTIVE_PIXELS,
@@ -366,13 +344,13 @@ static int tvp514x_write_regs(struct v4l2_subdev *sd,
 }
 
 /**
- * tvp514x_get_current_std() : Get the current standard detected by TVP5146/47
+ * tvp514x_query_current_std() : Query the current standard detected by TVP5146/47
  * @sd: ptr to v4l2_subdev struct
  *
- * Get current standard detected by TVP5146/47, STD_INVALID if there is no
+ * Returns the current standard detected by TVP5146/47, STD_INVALID if there is no
  * standard detected.
  */
-static enum tvp514x_std tvp514x_get_current_std(struct v4l2_subdev *sd)
+static enum tvp514x_std tvp514x_query_current_std(struct v4l2_subdev *sd)
 {
         u8 std, std_status;
 
@@ -518,7 +496,7 @@ static int tvp514x_detect(struct v4l2_subdev *sd,
  * @std_id: standard V4L2 std_id ioctl enum
  *
  * Returns the current standard detected by TVP5146/47. If no active input is
- * detected, returns -EINVAL
+ * detected then *std_id is set to 0 and the function returns 0.
  */
 static int tvp514x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std_id)
 {
@@ -530,10 +508,12 @@ static int tvp514x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std_id)
         if (std_id == NULL)
                 return -EINVAL;
 
-        /* get the current standard */
-        current_std = tvp514x_get_current_std(sd);
+        *std_id = V4L2_STD_UNKNOWN;
+
+        /* query the current standard */
+        current_std = tvp514x_query_current_std(sd);
         if (current_std == STD_INVALID)
-                return -EINVAL;
+                return 0;
 
         input_sel = decoder->input;
 
@@ -575,12 +555,11 @@ static int tvp514x_querystd(struct v4l2_subdev *sd, v4l2_std_id *std_id)
         /* check whether signal is locked */
         sync_lock_status = tvp514x_read_reg(sd, REG_STATUS1);
         if (lock_mask != (sync_lock_status & lock_mask))
-                return -EINVAL; /* No input detected */
+                return 0;       /* No input detected */
 
-        decoder->current_std = current_std;
         *std_id = decoder->std_list[current_std].standard.id;
 
-        v4l2_dbg(1, debug, sd, "Current STD: %s",
+        v4l2_dbg(1, debug, sd, "Current STD: %s\n",
                         decoder->std_list[current_std].standard.name);
         return 0;
 }
@@ -614,7 +593,7 @@ static int tvp514x_s_std(struct v4l2_subdev *sd, v4l2_std_id std_id)
         decoder->tvp514x_regs[REG_VIDEO_STD].val =
                 decoder->std_list[i].video_std;
 
-        v4l2_dbg(1, debug, sd, "Standard set to: %s",
+        v4l2_dbg(1, debug, sd, "Standard set to: %s\n",
                         decoder->std_list[i].standard.name);
         return 0;
 }
@@ -637,7 +616,6 @@ static int tvp514x_s_routing(struct v4l2_subdev *sd,
         int err;
         enum tvp514x_input input_sel;
         enum tvp514x_output output_sel;
-        enum tvp514x_std current_std = STD_INVALID;
         u8 sync_lock_status, lock_mask;
         int try_count = LOCK_RETRY_COUNT;
 
@@ -721,11 +699,6 @@ static int tvp514x_s_routing(struct v4l2_subdev *sd,
                 /* Allow decoder to sync up with new input */
                 msleep(LOCK_RETRY_DELAY);
 
-                /* get the current standard for future reference */
-                current_std = tvp514x_get_current_std(sd);
-                if (current_std == STD_INVALID)
-                        continue;
-
                 sync_lock_status = tvp514x_read_reg(sd,
                                 REG_STATUS1);
                 if (lock_mask == (sync_lock_status & lock_mask))
@@ -733,15 +706,13 @@ static int tvp514x_s_routing(struct v4l2_subdev *sd,
                         break;
         }
 
-        if ((current_std == STD_INVALID) || (try_count < 0))
+        if (try_count < 0)
                 return -EINVAL;
 
-        decoder->current_std = current_std;
         decoder->input = input;
         decoder->output = output;
 
-        v4l2_dbg(1, debug, sd, "Input set to: %d, std : %d",
-                        input_sel, current_std);
+        v4l2_dbg(1, debug, sd, "Input set to: %d\n", input_sel);
 
         return 0;
 }
@@ -794,7 +765,7 @@ tvp514x_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qctrl)
                 return err;
         }
 
-        v4l2_dbg(1, debug, sd, "Query Control:%s: Min - %d, Max - %d, Def - %d",
+        v4l2_dbg(1, debug, sd, "Query Control:%s: Min - %d, Max - %d, Def - %d\n",
                         qctrl->name, qctrl->minimum, qctrl->maximum,
                         qctrl->default_value);
 
@@ -851,7 +822,7 @@ tvp514x_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
                 return -EINVAL;
         }
 
-        v4l2_dbg(1, debug, sd, "Get Control: ID - %d - %d",
+        v4l2_dbg(1, debug, sd, "Get Control: ID - %d - %d\n",
                         ctrl->id, ctrl->value);
         return 0;
 }
@@ -951,7 +922,7 @@ tvp514x_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
                 return err;
         }
 
-        v4l2_dbg(1, debug, sd, "Set Control: ID - %d - %d",
+        v4l2_dbg(1, debug, sd, "Set Control: ID - %d - %d\n",
                         ctrl->id, ctrl->value);
 
         return err;
@@ -967,44 +938,33 @@ tvp514x_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
 static int
 tvp514x_enum_fmt_cap(struct v4l2_subdev *sd, struct v4l2_fmtdesc *fmt)
 {
-        struct tvp514x_decoder *decoder = to_decoder(sd);
-        int index;
-
-        if (fmt == NULL)
-                return -EINVAL;
-
-        index = fmt->index;
-        if ((index >= decoder->num_fmts) || (index < 0))
-                /* Index out of bound */
+        if (fmt == NULL || fmt->index)
                 return -EINVAL;
 
         if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
                 /* only capture is supported */
                 return -EINVAL;
 
-        memcpy(fmt, &decoder->fmt_list[index],
-                sizeof(struct v4l2_fmtdesc));
-
-        v4l2_dbg(1, debug, sd, "Current FMT: index - %d (%s)",
-                        decoder->fmt_list[index].index,
-                        decoder->fmt_list[index].description);
+        /* only one format */
+        fmt->flags = 0;
+        strlcpy(fmt->description, "8-bit UYVY 4:2:2 Format",
+                                        sizeof(fmt->description));
+        fmt->pixelformat = V4L2_PIX_FMT_UYVY;
         return 0;
 }
 
 /**
- * tvp514x_try_fmt_cap() - V4L2 decoder interface handler for try_fmt
+ * tvp514x_fmt_cap() - V4L2 decoder interface handler for try/s/g_fmt
  * @sd: pointer to standard V4L2 sub-device structure
  * @f: pointer to standard V4L2 VIDIOC_TRY_FMT ioctl structure
  *
- * Implement the VIDIOC_TRY_FMT ioctl for the CAPTURE buffer type. This
- * ioctl is used to negotiate the image capture size and pixel format
- * without actually making it take effect.
+ * Implement the VIDIOC_TRY/S/G_FMT ioctl for the CAPTURE buffer type. This
+ * ioctl is used to negotiate the image capture size and pixel format.
  */
 static int
-tvp514x_try_fmt_cap(struct v4l2_subdev *sd, struct v4l2_format *f)
+tvp514x_fmt_cap(struct v4l2_subdev *sd, struct v4l2_format *f)
 {
         struct tvp514x_decoder *decoder = to_decoder(sd);
-        int ifmt;
         struct v4l2_pix_format *pix;
         enum tvp514x_std current_std;
 
@@ -1012,106 +972,30 @@ tvp514x_try_fmt_cap(struct v4l2_subdev *sd, struct v4l2_format *f)
                 return -EINVAL;
 
         if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-                /* only capture is supported */
-                f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+                return -EINVAL;
 
         pix = &f->fmt.pix;
 
         /* Calculate height and width based on current standard */
-        current_std = tvp514x_get_current_std(sd);
-        if (current_std == STD_INVALID)
-                return -EINVAL;
+        current_std = decoder->current_std;
 
-        decoder->current_std = current_std;
+        pix->pixelformat = V4L2_PIX_FMT_UYVY;
         pix->width = decoder->std_list[current_std].width;
         pix->height = decoder->std_list[current_std].height;
-
-        for (ifmt = 0; ifmt < decoder->num_fmts; ifmt++) {
-                if (pix->pixelformat ==
-                        decoder->fmt_list[ifmt].pixelformat)
-                        break;
-        }
-        if (ifmt == decoder->num_fmts)
-                /* None of the format matched, select default */
-                ifmt = 0;
-        pix->pixelformat = decoder->fmt_list[ifmt].pixelformat;
-
         pix->field = V4L2_FIELD_INTERLACED;
         pix->bytesperline = pix->width * 2;
         pix->sizeimage = pix->bytesperline * pix->height;
         pix->colorspace = V4L2_COLORSPACE_SMPTE170M;
         pix->priv = 0;
 
-        v4l2_dbg(1, debug, sd, "Try FMT: pixelformat - %s, bytesperline - %d"
-                        "Width - %d, Height - %d",
-                        decoder->fmt_list[ifmt].description, pix->bytesperline,
+        v4l2_dbg(1, debug, sd, "FMT: bytesperline - %d"
+                        "Width - %d, Height - %d\n",
+                        pix->bytesperline,
                         pix->width, pix->height);
         return 0;
 }
 
 /**
- * tvp514x_s_fmt_cap() - V4L2 decoder interface handler for s_fmt
- * @sd: pointer to standard V4L2 sub-device structure
- * @f: pointer to standard V4L2 VIDIOC_S_FMT ioctl structure
- *
- * If the requested format is supported, configures the HW to use that
- * format, returns error code if format not supported or HW can't be
- * correctly configured.
- */
-static int
-tvp514x_s_fmt_cap(struct v4l2_subdev *sd, struct v4l2_format *f)
-{
-        struct tvp514x_decoder *decoder = to_decoder(sd);
-        struct v4l2_pix_format *pix;
-        int rval;
-
-        if (f == NULL)
-                return -EINVAL;
-
-        if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-                /* only capture is supported */
-                return -EINVAL;
-
-        pix = &f->fmt.pix;
-        rval = tvp514x_try_fmt_cap(sd, f);
-        if (rval)
-                return rval;
-
-                decoder->pix = *pix;
-
-        return rval;
-}
-
-/**
- * tvp514x_g_fmt_cap() - V4L2 decoder interface handler for tvp514x_g_fmt_cap
- * @sd: pointer to standard V4L2 sub-device structure
- * @f: pointer to standard V4L2 v4l2_format structure
- *
- * Returns the decoder's current pixel format in the v4l2_format
- * parameter.
- */
-static int
-tvp514x_g_fmt_cap(struct v4l2_subdev *sd, struct v4l2_format *f)
-{
-        struct tvp514x_decoder *decoder = to_decoder(sd);
-
-        if (f == NULL)
-                return -EINVAL;
-
-        if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-                /* only capture is supported */
-                return -EINVAL;
-
-        f->fmt.pix = decoder->pix;
-
-        v4l2_dbg(1, debug, sd, "Current FMT: bytesperline - %d"
-                        "Width - %d, Height - %d",
-                        decoder->pix.bytesperline,
-                        decoder->pix.width, decoder->pix.height);
-        return 0;
-}
-
-/**
  * tvp514x_g_parm() - V4L2 decoder interface handler for g_parm
  * @sd: pointer to standard V4L2 sub-device structure
  * @a: pointer to standard V4L2 VIDIOC_G_PARM ioctl structure
@@ -1132,15 +1016,8 @@ tvp514x_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)
                 /* only capture is supported */
                 return -EINVAL;
 
-        memset(a, 0, sizeof(*a));
-        a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
         /* get the current standard */
-        current_std = tvp514x_get_current_std(sd);
-        if (current_std == STD_INVALID)
-                return -EINVAL;
-
-        decoder->current_std = current_std;
+        current_std = decoder->current_std;
 
         cparm = &a->parm.capture;
         cparm->capability = V4L2_CAP_TIMEPERFRAME;
@@ -1175,11 +1052,7 @@ tvp514x_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *a)
         timeperframe = &a->parm.capture.timeperframe;
 
         /* get the current standard */
-        current_std = tvp514x_get_current_std(sd);
-        if (current_std == STD_INVALID)
-                return -EINVAL;
-
-        decoder->current_std = current_std;
+        current_std = decoder->current_std;
 
         *timeperframe =
             decoder->std_list[current_std].standard.frameperiod;
@@ -1259,9 +1132,9 @@ static const struct v4l2_subdev_video_ops tvp514x_video_ops = {
         .s_routing = tvp514x_s_routing,
         .querystd = tvp514x_querystd,
         .enum_fmt = tvp514x_enum_fmt_cap,
-        .g_fmt = tvp514x_g_fmt_cap,
-        .try_fmt = tvp514x_try_fmt_cap,
-        .s_fmt = tvp514x_s_fmt_cap,
+        .g_fmt = tvp514x_fmt_cap,
+        .try_fmt = tvp514x_fmt_cap,
+        .s_fmt = tvp514x_fmt_cap,
         .g_parm = tvp514x_g_parm,
         .s_parm = tvp514x_s_parm,
         .s_stream = tvp514x_s_stream,
@@ -1274,22 +1147,6 @@ static const struct v4l2_subdev_ops tvp514x_ops = {
 
 static struct tvp514x_decoder tvp514x_dev = {
         .streaming = 0,
-
-        .fmt_list = tvp514x_fmt_list,
-        .num_fmts = ARRAY_SIZE(tvp514x_fmt_list),
-
-        .pix = {
-                /* Default to NTSC 8-bit YUV 422 */
-                .width = NTSC_NUM_ACTIVE_PIXELS,
-                .height = NTSC_NUM_ACTIVE_LINES,
-                .pixelformat = V4L2_PIX_FMT_UYVY,
-                .field = V4L2_FIELD_INTERLACED,
-                .bytesperline = NTSC_NUM_ACTIVE_PIXELS * 2,
-                .sizeimage =
-                NTSC_NUM_ACTIVE_PIXELS * 2 * NTSC_NUM_ACTIVE_LINES,
-                .colorspace = V4L2_COLORSPACE_SMPTE170M,
-                },
-
         .current_std = STD_NTSC_MJ,
         .std_list = tvp514x_std_list,
         .num_stds = ARRAY_SIZE(tvp514x_std_list),
diff --git a/drivers/media/video/tvp5150.c b/drivers/media/video/tvp5150.c
index 47f0582d50a5..1654f65cca7c 100644
--- a/drivers/media/video/tvp5150.c
+++ b/drivers/media/video/tvp5150.c
@@ -934,17 +934,6 @@ static int tvp5150_s_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_f
         return 0;
 }
 
-static int tvp5150_s_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
-{
-        switch (fmt->type) {
-        case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
-                return tvp5150_s_sliced_fmt(sd, &fmt->fmt.sliced);
-
-        default:
-                return -EINVAL;
-        }
-}
-
 static int tvp5150_g_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *svbi)
 {
         int i, mask = 0;
@@ -960,13 +949,6 @@ static int tvp5150_g_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_f
         return 0;
 }
 
-static int tvp5150_g_fmt(struct v4l2_subdev *sd, struct v4l2_format *fmt)
-{
-        if (fmt->type != V4L2_BUF_TYPE_SLICED_VBI_CAPTURE)
-                return -EINVAL;
-        return tvp5150_g_sliced_fmt(sd, &fmt->fmt.sliced);
-}
-
 static int tvp5150_g_chip_ident(struct v4l2_subdev *sd,
                                 struct v4l2_dbg_chip_ident *chip)
 {
@@ -1054,8 +1036,6 @@ static const struct v4l2_subdev_tuner_ops tvp5150_tuner_ops = {
 
 static const struct v4l2_subdev_video_ops tvp5150_video_ops = {
         .s_routing = tvp5150_s_routing,
-        .g_fmt = tvp5150_g_fmt,
-        .s_fmt = tvp5150_s_fmt,
 };
 
 static const struct v4l2_subdev_vbi_ops tvp5150_vbi_ops = {
diff --git a/drivers/media/video/tw9910.c b/drivers/media/video/tw9910.c
index 76be733eabfd..445dc93413e3 100644
--- a/drivers/media/video/tw9910.c
+++ b/drivers/media/video/tw9910.c
@@ -903,7 +903,7 @@ static struct v4l2_subdev_core_ops tw9910_subdev_core_ops = {
 #endif
 };
 
-static int tw9910_enum_fmt(struct v4l2_subdev *sd, int index,
+static int tw9910_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
                            enum v4l2_mbus_pixelcode *code)
 {
         if (index)
@@ -977,7 +977,6 @@ static int tw9910_probe(struct i2c_client *client,
         ret = tw9910_video_probe(icd, client);
         if (ret) {
                 icd->ops = NULL;
-                i2c_set_clientdata(client, NULL);
                 kfree(priv);
         }
 
@@ -990,7 +989,6 @@ static int tw9910_remove(struct i2c_client *client)
         struct soc_camera_device *icd = client->dev.platform_data;
 
         icd->ops = NULL;
-        i2c_set_clientdata(client, NULL);
         kfree(priv);
         return 0;
 }
diff --git a/drivers/media/video/usbvision/usbvision-video.c b/drivers/media/video/usbvision/usbvision-video.c
index 6248a639ba2d..c2690df33438 100644
--- a/drivers/media/video/usbvision/usbvision-video.c
+++ b/drivers/media/video/usbvision/usbvision-video.c
@@ -1671,8 +1671,7 @@ static void __devexit usbvision_disconnect(struct usb_interface *intf)
         PDEBUG(DBG_PROBE, "");
 
         if (usbvision == NULL) {
-                dev_err(&usbvision->dev->dev,
-                        "%s: usb_get_intfdata() failed\n", __func__);
+                pr_err("%s: usb_get_intfdata() failed\n", __func__);
                 return;
         }
 
diff --git a/drivers/message/i2o/i2o_config.c b/drivers/message/i2o/i2o_config.c
index d33693c13368..c4b117f5fb70 100644
--- a/drivers/message/i2o/i2o_config.c
+++ b/drivers/message/i2o/i2o_config.c
@@ -186,14 +186,9 @@ static int i2o_cfg_parms(unsigned long arg, unsigned int type)
         if (!dev)
                 return -ENXIO;
 
-        ops = kmalloc(kcmd.oplen, GFP_KERNEL);
-        if (!ops)
-                return -ENOMEM;
-
-        if (copy_from_user(ops, kcmd.opbuf, kcmd.oplen)) {
-                kfree(ops);
-                return -EFAULT;
-        }
+        ops = memdup_user(kcmd.opbuf, kcmd.oplen);
+        if (IS_ERR(ops))
+                return PTR_ERR(ops);
 
         /*
          * It's possible to have a _very_ large table
diff --git a/drivers/mfd/88pm860x-core.c b/drivers/mfd/88pm860x-core.c
index 405d2d5183cf..2c65a2c57294 100644
--- a/drivers/mfd/88pm860x-core.c
+++ b/drivers/mfd/88pm860x-core.c
@@ -566,7 +566,7 @@ out:
         return ret;
 }
 
-static void __devexit device_irq_exit(struct pm860x_chip *chip)
+static void device_irq_exit(struct pm860x_chip *chip)
 {
         if (chip->core_irq)
                 free_irq(chip->core_irq, chip);
@@ -703,7 +703,7 @@ out:
         return;
 }
 
-int pm860x_device_init(struct pm860x_chip *chip,
+int __devinit pm860x_device_init(struct pm860x_chip *chip,
                        struct pm860x_platform_data *pdata)
 {
         chip->core_irq = 0;
@@ -731,7 +731,7 @@ int pm860x_device_init(struct pm860x_chip *chip,
         return 0;
 }
 
-void pm860x_device_exit(struct pm860x_chip *chip)
+void __devexit pm860x_device_exit(struct pm860x_chip *chip)
 {
         device_irq_exit(chip);
         mfd_remove_devices(chip->dev);
diff --git a/drivers/mfd/88pm860x-i2c.c b/drivers/mfd/88pm860x-i2c.c
index 4a6e7186334e..bc02e6b21608 100644
--- a/drivers/mfd/88pm860x-i2c.c
+++ b/drivers/mfd/88pm860x-i2c.c
@@ -200,8 +200,6 @@ static int __devexit pm860x_remove(struct i2c_client *client)
 
         pm860x_device_exit(chip);
         i2c_unregister_device(chip->companion);
-        i2c_set_clientdata(chip->companion, NULL);
-        i2c_set_clientdata(chip->client, NULL);
         kfree(chip);
         return 0;
 }
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 3c6a9860dd9c..9da0e504bbe9 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -2,8 +2,14 @@
 # Multifunction miscellaneous devices
 #
 
-menu "Multifunction device drivers"
+menuconfig MFD_SUPPORT
+        bool "Multifunction device drivers"
         depends on HAS_IOMEM
+        default y
+        help
+          Configure MFD device drivers.
+
+if MFD_SUPPORT
 
 config MFD_CORE
         tristate
@@ -116,6 +122,18 @@ config TPS65010
           This driver can also be built as a module.  If so, the module
           will be called tps65010.
 
+config TPS6507X
+        tristate "TPS6507x Power Management / Touch Screen chips"
+        select MFD_CORE
+        depends on I2C
+        help
+          If you say yes here you get support for the TPS6507x series of
+          Power Management / Touch Screen chips.  These include voltage
+          regulators, lithium ion/polymer battery charging, touch screen
+          and other features that are often used in portable devices.
+          This driver can also be built as a module.  If so, the module
+          will be called tps6507x.
+
 config MENELAUS
         bool "Texas Instruments TWL92330/Menelaus PM chip"
         depends on I2C=y && ARCH_OMAP2
@@ -159,6 +177,17 @@ config TWL4030_CODEC
         select MFD_CORE
         default n
 
+config MFD_TC35892
+        bool "Support Toshiba TC35892"
+        depends on I2C=y && GENERIC_HARDIRQS
+        select MFD_CORE
+        help
+          Support for the Toshiba TC35892 I/O Expander.
+
+          This driver provides common support for accessing the device,
+          additional drivers must be enabled in order to use the
+          functionality of the device.
+
 config MFD_TMIO
         bool
         default n
@@ -351,9 +380,19 @@ config PCF50633_GPIO
          Say yes here if you want to include support GPIO for pins on
          the PCF50633 chip.
 
+config ABX500_CORE
+        bool "ST-Ericsson ABX500 Mixed Signal Circuit register functions"
+        default y if ARCH_U300
+        help
+          Say yes here if you have the ABX500 Mixed Signal IC family
+          chips. This core driver expose register access functions.
+          Functionality specific drivers using these functions can
+          remain unchanged when IC changes. Binding of the functions to
+          actual register access is done by the IC core driver.
+
 config AB3100_CORE
         bool "ST-Ericsson AB3100 Mixed Signal Circuit core functions"
-        depends on I2C=y
+        depends on I2C=y && ABX500_CORE
         default y if ARCH_U300
         help
           Select this to enable the AB3100 Mixed Signal IC core
@@ -381,15 +420,30 @@ config EZX_PCAP
           This enables the PCAP ASIC present on EZX Phones. This is
           needed for MMC, TouchScreen, Sound, USB, etc..
 
-config AB4500_CORE
-        tristate "ST-Ericsson's AB4500 Mixed Signal Power management chip"
-        depends on SPI
+config AB8500_CORE
+        bool "ST-Ericsson AB8500 Mixed Signal Power Management chip"
+        depends on SPI=y && GENERIC_HARDIRQS
+        select MFD_CORE
         help
-          Select this option to enable access to AB4500 power management
+          Select this option to enable access to AB8500 power management
           chip. This connects to U8500 on the SSP/SPI bus and exports
           read/write functions for the devices to get access to this chip.
           This chip embeds various other multimedia funtionalities as well.
 
+config AB3550_CORE
+        bool "ST-Ericsson AB3550 Mixed Signal Circuit core functions"
+        select MFD_CORE
+        depends on I2C=y && GENERIC_HARDIRQS && ABX500_CORE
+        help
+          Select this to enable the AB3550 Mixed Signal IC core
+          functionality. This connects to a AB3550 on the I2C bus
+          and expose a number of symbols needed for dependent devices
+          to read and write registers and subscribe to events from
+          this multi-functional IC. This is needed to use other features
+          of the AB3550 such as battery-backed RTC, charging control,
+          LEDs, vibrator, system power and temperature, power management
+          and ALSA sound.
+
 config MFD_TIMBERDALE
         tristate "Support for the Timberdale FPGA"
         select MFD_CORE
@@ -409,7 +463,26 @@ config LPC_SCH
           LPC bridge function of the Intel SCH provides support for
           System Management Bus and General Purpose I/O.
 
-endmenu
+config MFD_RDC321X
+        tristate "Support for RDC-R321x southbridge"
+        select MFD_CORE
+        depends on PCI
+        help
+          Say yes here if you want to have support for the RDC R-321x SoC
+          southbridge which provides access to GPIOs and Watchdog using the
+          southbridge PCI device configuration space.
+
+config MFD_JANZ_CMODIO
+        tristate "Support for Janz CMOD-IO PCI MODULbus Carrier Board"
+        select MFD_CORE
+        depends on PCI
+        help
+          This is the core driver for the Janz CMOD-IO PCI MODULbus
+          carrier board. This device is a PCI to MODULbus bridge which may
+          host many different types of MODULbus daughterboards, including
+          CAN and GPIO controllers.
+
+endif # MFD_SUPPORT
 
 menu "Multimedia Capabilities Port drivers"
         depends on ARCH_SA1100
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index 87935f967aa0..fb503e77dc60 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -15,6 +15,7 @@ obj-$(CONFIG_HTC_I2CPLD)	+= htc-i2cpld.o
 obj-$(CONFIG_MFD_DAVINCI_VOICECODEC)    += davinci_voicecodec.o
 obj-$(CONFIG_MFD_DM355EVM_MSP)  += dm355evm_msp.o
 
+obj-$(CONFIG_MFD_TC35892)       += tc35892.o
 obj-$(CONFIG_MFD_T7L66XB)       += t7l66xb.o tmio_core.o
 obj-$(CONFIG_MFD_TC6387XB)      += tc6387xb.o tmio_core.o
 obj-$(CONFIG_MFD_TC6393XB)      += tc6393xb.o tmio_core.o
@@ -29,6 +30,7 @@ obj-$(CONFIG_MFD_WM8350_I2C)	+= wm8350-i2c.o
 obj-$(CONFIG_MFD_WM8994)        += wm8994-core.o wm8994-irq.o
 
 obj-$(CONFIG_TPS65010)          += tps65010.o
+obj-$(CONFIG_TPS6507X)          += tps6507x.o
 obj-$(CONFIG_MENELAUS)          += menelaus.o
 
 obj-$(CONFIG_TWL4030_CORE)      += twl-core.o twl4030-irq.o twl6030-irq.o
@@ -55,12 +57,17 @@ obj-$(CONFIG_PMIC_DA903X)	+= da903x.o
 max8925-objs                    := max8925-core.o max8925-i2c.o
 obj-$(CONFIG_MFD_MAX8925)       += max8925.o
 
-obj-$(CONFIG_MFD_PCF50633)      += pcf50633-core.o
+pcf50633-objs                   := pcf50633-core.o pcf50633-irq.o
+obj-$(CONFIG_MFD_PCF50633)      += pcf50633.o
 obj-$(CONFIG_PCF50633_ADC)      += pcf50633-adc.o
 obj-$(CONFIG_PCF50633_GPIO)     += pcf50633-gpio.o
+obj-$(CONFIG_ABX500_CORE)       += abx500-core.o
 obj-$(CONFIG_AB3100_CORE)       += ab3100-core.o
 obj-$(CONFIG_AB3100_OTP)        += ab3100-otp.o
-obj-$(CONFIG_AB4500_CORE)       += ab4500-core.o
+obj-$(CONFIG_AB3550_CORE)       += ab3550-core.o
+obj-$(CONFIG_AB8500_CORE)       += ab8500-core.o ab8500-spi.o
 obj-$(CONFIG_MFD_TIMBERDALE)    += timberdale.o
 obj-$(CONFIG_PMIC_ADP5520)      += adp5520.o
-obj-$(CONFIG_LPC_SCH)           += lpc_sch.o
\ No newline at end of file
+obj-$(CONFIG_LPC_SCH)           += lpc_sch.o
+obj-$(CONFIG_MFD_RDC321X)       += rdc321x-southbridge.o
+obj-$(CONFIG_MFD_JANZ_CMODIO)   += janz-cmodio.o
diff --git a/drivers/mfd/ab3100-core.c b/drivers/mfd/ab3100-core.c
index e4ca5909e424..66379b413906 100644
--- a/drivers/mfd/ab3100-core.c
+++ b/drivers/mfd/ab3100-core.c
@@ -19,7 +19,7 @@
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
 #include <linux/uaccess.h>
-#include <linux/mfd/ab3100.h>
+#include <linux/mfd/abx500.h>
 
 /* These are the only registers inside AB3100 used in this main file */
 
@@ -59,24 +59,15 @@
  * The AB3100 is usually assigned address 0x48 (7-bit)
  * The chip is defined in the platform i2c_board_data section.
  */
-
-u8 ab3100_get_chip_type(struct ab3100 *ab3100)
+static int ab3100_get_chip_id(struct device *dev)
 {
-        u8 chip = ABUNKNOWN;
-
-        switch (ab3100->chip_id & 0xf0) {
-        case  0xa0:
-                chip = AB3000;
-                break;
-        case  0xc0:
-                chip = AB3100;
-                break;
-        }
-        return chip;
+        struct ab3100 *ab3100 = dev_get_drvdata(dev->parent);
+
+        return (int)ab3100->chip_id;
 }
-EXPORT_SYMBOL(ab3100_get_chip_type);
 
-int ab3100_set_register_interruptible(struct ab3100 *ab3100, u8 reg, u8 regval)
+static int ab3100_set_register_interruptible(struct ab3100 *ab3100,
+        u8 reg, u8 regval)
 {
         u8 regandval[2] = {reg, regval};
         int err;
@@ -108,8 +99,14 @@ int ab3100_set_register_interruptible(struct ab3100 *ab3100, u8 reg, u8 regval)
         mutex_unlock(&ab3100->access_mutex);
         return err;
 }
-EXPORT_SYMBOL(ab3100_set_register_interruptible);
 
+static int set_register_interruptible(struct device *dev,
+        u8 bank, u8 reg, u8 value)
+{
+        struct ab3100 *ab3100 = dev_get_drvdata(dev->parent);
+
+        return ab3100_set_register_interruptible(ab3100, reg, value);
+}
 
 /*
  * The test registers exist at an I2C bus address up one
@@ -148,8 +145,8 @@ static int ab3100_set_test_register_interruptible(struct ab3100 *ab3100,
         return err;
 }
 
-
-int ab3100_get_register_interruptible(struct ab3100 *ab3100, u8 reg, u8 *regval)
+static int ab3100_get_register_interruptible(struct ab3100 *ab3100,
+        u8 reg, u8 *regval)
 {
         int err;
 
@@ -203,10 +200,16 @@ int ab3100_get_register_interruptible(struct ab3100 *ab3100, u8 reg, u8 *regval)
         mutex_unlock(&ab3100->access_mutex);
         return err;
 }
-EXPORT_SYMBOL(ab3100_get_register_interruptible);
 
+static int get_register_interruptible(struct device *dev, u8 bank, u8 reg,
+        u8 *value)
+{
+        struct ab3100 *ab3100 = dev_get_drvdata(dev->parent);
+
+        return ab3100_get_register_interruptible(ab3100, reg, value);
+}
 
-int ab3100_get_register_page_interruptible(struct ab3100 *ab3100,
+static int ab3100_get_register_page_interruptible(struct ab3100 *ab3100,
                              u8 first_reg, u8 *regvals, u8 numregs)
 {
         int err;
@@ -260,10 +263,17 @@ int ab3100_get_register_page_interruptible(struct ab3100 *ab3100,
         mutex_unlock(&ab3100->access_mutex);
         return err;
 }
-EXPORT_SYMBOL(ab3100_get_register_page_interruptible);
 
+static int get_register_page_interruptible(struct device *dev, u8 bank,
+        u8 first_reg, u8 *regvals, u8 numregs)
+{
+        struct ab3100 *ab3100 = dev_get_drvdata(dev->parent);
+
+        return ab3100_get_register_page_interruptible(ab3100,
+                        first_reg, regvals, numregs);
+}
 
-int ab3100_mask_and_set_register_interruptible(struct ab3100 *ab3100,
+static int ab3100_mask_and_set_register_interruptible(struct ab3100 *ab3100,
                                  u8 reg, u8 andmask, u8 ormask)
 {
         u8 regandval[2] = {reg, 0};
@@ -331,8 +341,15 @@ int ab3100_mask_and_set_register_interruptible(struct ab3100 *ab3100,
         mutex_unlock(&ab3100->access_mutex);
         return err;
 }
-EXPORT_SYMBOL(ab3100_mask_and_set_register_interruptible);
 
+static int mask_and_set_register_interruptible(struct device *dev, u8 bank,
+        u8 reg, u8 bitmask, u8 bitvalues)
+{
+        struct ab3100 *ab3100 = dev_get_drvdata(dev->parent);
+
+        return ab3100_mask_and_set_register_interruptible(ab3100,
+                        reg, bitmask, (bitmask & bitvalues));
+}
 
 /*
  * Register a simple callback for handling any AB3100 events.
@@ -357,15 +374,27 @@ int ab3100_event_unregister(struct ab3100 *ab3100,
 EXPORT_SYMBOL(ab3100_event_unregister);
 
 
-int ab3100_event_registers_startup_state_get(struct ab3100 *ab3100,
-                                             u32 *fatevent)
+static int ab3100_event_registers_startup_state_get(struct device *dev,
+                                             u8 *event)
 {
+        struct ab3100 *ab3100 = dev_get_drvdata(dev->parent);
         if (!ab3100->startup_events_read)
                 return -EAGAIN; /* Try again later */
-        *fatevent = ab3100->startup_events;
+        memcpy(event, ab3100->startup_events, 3);
         return 0;
 }
-EXPORT_SYMBOL(ab3100_event_registers_startup_state_get);
+
+static struct abx500_ops ab3100_ops = {
+        .get_chip_id = ab3100_get_chip_id,
+        .set_register = set_register_interruptible,
+        .get_register = get_register_interruptible,
+        .get_register_page = get_register_page_interruptible,
+        .set_register_page = NULL,
+        .mask_and_set_register = mask_and_set_register_interruptible,
+        .event_registers_startup_state_get =
+                ab3100_event_registers_startup_state_get,
+        .startup_irq_enabled = NULL,
+};
 
 /*
  * This is a threaded interrupt handler so we can make some
@@ -390,7 +419,9 @@ static irqreturn_t ab3100_irq_handler(int irq, void *data)
                 event_regs[2];
 
         if (!ab3100->startup_events_read) {
-                ab3100->startup_events = fatevent;
+                ab3100->startup_events[0] = event_regs[0];
+                ab3100->startup_events[1] = event_regs[1];
+                ab3100->startup_events[2] = event_regs[2];
                 ab3100->startup_events_read = true;
         }
         /*
@@ -703,7 +734,8 @@ static int __init ab3100_setup(struct ab3100 *ab3100)
                 dev_warn(ab3100->dev,
                          "AB3100 P1E variant detected, "
                          "forcing chip to 32KHz\n");
-                err = ab3100_set_test_register_interruptible(ab3100, 0x02, 0x08);
+                err = ab3100_set_test_register_interruptible(ab3100,
+                        0x02, 0x08);
         }
 
  exit_no_setup:
@@ -898,6 +930,10 @@ static int __init ab3100_probe(struct i2c_client *client,
         if (err)
                 goto exit_no_irq;
 
+        err = abx500_register_ops(&client->dev, &ab3100_ops);
+        if (err)
+                goto exit_no_ops;
+
         /* Set parent and a pointer back to the container in device data */
         for (i = 0; i < ARRAY_SIZE(ab3100_platform_devs); i++) {
                 ab3100_platform_devs[i]->dev.parent =
@@ -915,6 +951,7 @@ static int __init ab3100_probe(struct i2c_client *client,
 
         return 0;
 
+ exit_no_ops:
  exit_no_irq:
  exit_no_setup:
         i2c_unregister_device(ab3100->testreg_client);
diff --git a/drivers/mfd/ab3100-otp.c b/drivers/mfd/ab3100-otp.c
index 2d14655fdebd..63d2b727ddbb 100644
--- a/drivers/mfd/ab3100-otp.c
+++ b/drivers/mfd/ab3100-otp.c
@@ -12,7 +12,7 @@
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/platform_device.h>
-#include <linux/mfd/ab3100.h>
+#include <linux/mfd/abx500.h>
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
 
@@ -30,7 +30,6 @@
 /**
  * struct ab3100_otp
  * @dev containing device
- * @ab3100 a pointer to the parent ab3100 device struct
  * @locked whether the OTP is locked, after locking, no more bits
  *       can be changed but before locking it is still possible
  *       to change bits from 1->0.
@@ -49,7 +48,6 @@
  */
 struct ab3100_otp {
         struct device *dev;
-        struct ab3100 *ab3100;
         bool locked;
         u32 freq;
         bool paf;
@@ -63,19 +61,19 @@ struct ab3100_otp {
 
 static int __init ab3100_otp_read(struct ab3100_otp *otp)
 {
-        struct ab3100 *ab = otp->ab3100;
         u8 otpval[8];
         u8 otpp;
         int err;
 
-        err = ab3100_get_register_interruptible(ab, AB3100_OTPP, &otpp);
+        err = abx500_get_register_interruptible(otp->dev, 0,
+                AB3100_OTPP, &otpp);
         if (err) {
                 dev_err(otp->dev, "unable to read OTPP register\n");
                 return err;
         }
 
-        err = ab3100_get_register_page_interruptible(ab, AB3100_OTP0,
-                                                     otpval, 8);
+        err = abx500_get_register_page_interruptible(otp->dev, 0,
+                AB3100_OTP0, otpval, 8);
         if (err) {
                 dev_err(otp->dev, "unable to read OTP register page\n");
                 return err;
@@ -197,7 +195,6 @@ static int __init ab3100_otp_probe(struct platform_device *pdev)
         otp->dev = &pdev->dev;
 
         /* Replace platform data coming in with a local struct */
-        otp->ab3100 = platform_get_drvdata(pdev);
         platform_set_drvdata(pdev, otp);
 
         err = ab3100_otp_read(otp);
diff --git a/drivers/mfd/ab3550-core.c b/drivers/mfd/ab3550-core.c
new file mode 100644
index 000000000000..f54ab62e7bc6
--- /dev/null
+++ b/drivers/mfd/ab3550-core.c
@@ -0,0 +1,1400 @@
+/*
+ * Copyright (C) 2007-2010 ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ * Low-level core for exclusive access to the AB3550 IC on the I2C bus
+ * and some basic chip-configuration.
+ * Author: Bengt Jonsson <bengt.g.jonsson@stericsson.com>
+ * Author: Mattias Nilsson <mattias.i.nilsson@stericsson.com>
+ * Author: Mattias Wallin <mattias.wallin@stericsson.com>
+ * Author: Rickard Andersson <rickard.andersson@stericsson.com>
+ */
+
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/random.h>
+#include <linux/workqueue.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/uaccess.h>
+#include <linux/mfd/abx500.h>
+#include <linux/list.h>
+#include <linux/bitops.h>
+#include <linux/spinlock.h>
+#include <linux/mfd/core.h>
+
+#define AB3550_NAME_STRING "ab3550"
+#define AB3550_ID_FORMAT_STRING "AB3550 %s"
+#define AB3550_NUM_BANKS 2
+#define AB3550_NUM_EVENT_REG 5
+
+/* These are the only registers inside AB3550 used in this main file */
+
+/* Chip ID register */
+#define AB3550_CID_REG           0x20
+
+/* Interrupt event registers */
+#define AB3550_EVENT_BANK        0
+#define AB3550_EVENT_REG         0x22
+
+/* Read/write operation values. */
+#define AB3550_PERM_RD (0x01)
+#define AB3550_PERM_WR (0x02)
+
+/* Read/write permissions. */
+#define AB3550_PERM_RO (AB3550_PERM_RD)
+#define AB3550_PERM_RW (AB3550_PERM_RD | AB3550_PERM_WR)
+
+/**
+ * struct ab3550
+ * @access_mutex: lock out concurrent accesses to the AB registers
+ * @i2c_client: I2C client for this chip
+ * @chip_name: name of this chip variant
+ * @chip_id: 8 bit chip ID for this chip variant
+ * @mask_work: a worker for writing to mask registers
+ * @event_lock: a lock to protect the event_mask
+ * @event_mask: a local copy of the mask event registers
+ * @startup_events: a copy of the first reading of the event registers
+ * @startup_events_read: whether the first events have been read
+ */
+struct ab3550 {
+        struct mutex access_mutex;
+        struct i2c_client *i2c_client[AB3550_NUM_BANKS];
+        char chip_name[32];
+        u8 chip_id;
+        struct work_struct mask_work;
+        spinlock_t event_lock;
+        u8 event_mask[AB3550_NUM_EVENT_REG];
+        u8 startup_events[AB3550_NUM_EVENT_REG];
+        bool startup_events_read;
+#ifdef CONFIG_DEBUG_FS
+        unsigned int debug_bank;
+        unsigned int debug_address;
+#endif
+};
+
+/**
+ * struct ab3550_reg_range
+ * @first: the first address of the range
+ * @last: the last address of the range
+ * @perm: access permissions for the range
+ */
+struct ab3550_reg_range {
+        u8 first;
+        u8 last;
+        u8 perm;
+};
+
+/**
+ * struct ab3550_reg_ranges
+ * @count: the number of ranges in the list
+ * @range: the list of register ranges
+ */
+struct ab3550_reg_ranges {
+        u8 count;
+        const struct ab3550_reg_range *range;
+};
+
+/*
+ * Permissible register ranges for reading and writing per device and bank.
+ *
+ * The ranges must be listed in increasing address order, and no overlaps are
+ * allowed. It is assumed that write permission implies read permission
+ * (i.e. only RO and RW permissions should be used).  Ranges with write
+ * permission must not be split up.
+ */
+
+#define NO_RANGE {.count = 0, .range = NULL,}
+
+static struct
+ab3550_reg_ranges ab3550_reg_ranges[AB3550_NUM_DEVICES][AB3550_NUM_BANKS] = {
+        [AB3550_DEVID_DAC] = {
+                NO_RANGE,
+                {
+                        .count = 2,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0xb0,
+                                        .last = 0xba,
+                                        .perm = AB3550_PERM_RW,
+                                },
+                                {
+                                        .first = 0xbc,
+                                        .last = 0xc3,
+                                        .perm = AB3550_PERM_RW,
+                                },
+                        },
+                },
+        },
+        [AB3550_DEVID_LEDS] = {
+                NO_RANGE,
+                {
+                        .count = 2,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x5a,
+                                        .last = 0x88,
+                                        .perm = AB3550_PERM_RW,
+                                },
+                                {
+                                        .first = 0x8a,
+                                        .last = 0xad,
+                                        .perm = AB3550_PERM_RW,
+                                },
+                        }
+                },
+        },
+        [AB3550_DEVID_POWER] = {
+                {
+                        .count = 1,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x21,
+                                        .last = 0x21,
+                                        .perm = AB3550_PERM_RO,
+                                },
+                        }
+                },
+                NO_RANGE,
+        },
+        [AB3550_DEVID_REGULATORS] = {
+                {
+                        .count = 1,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x69,
+                                        .last = 0xa3,
+                                        .perm = AB3550_PERM_RW,
+                                },
+                        }
+                },
+                {
+                        .count = 1,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x14,
+                                        .last = 0x16,
+                                        .perm = AB3550_PERM_RW,
+                                },
+                        }
+                },
+        },
+        [AB3550_DEVID_SIM] = {
+                {
+                        .count = 1,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x21,
+                                        .last = 0x21,
+                                        .perm = AB3550_PERM_RO,
+                                },
+                        }
+                },
+                {
+                        .count = 1,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x14,
+                                        .last = 0x17,
+                                        .perm = AB3550_PERM_RW,
+                                },
+                        }
+
+                },
+        },
+        [AB3550_DEVID_UART] = {
+                NO_RANGE,
+                NO_RANGE,
+        },
+        [AB3550_DEVID_RTC] = {
+                {
+                        .count = 1,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x00,
+                                        .last = 0x0c,
+                                        .perm = AB3550_PERM_RW,
+                                },
+                        }
+                },
+                NO_RANGE,
+        },
+        [AB3550_DEVID_CHARGER] = {
+                {
+                        .count = 2,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x10,
+                                        .last = 0x1a,
+                                        .perm = AB3550_PERM_RW,
+                                },
+                                {
+                                        .first = 0x21,
+                                        .last = 0x21,
+                                        .perm = AB3550_PERM_RO,
+                                },
+                        }
+                },
+                NO_RANGE,
+        },
+        [AB3550_DEVID_ADC] = {
+                NO_RANGE,
+                {
+                        .count = 1,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x20,
+                                        .last = 0x56,
+                                        .perm = AB3550_PERM_RW,
+                                },
+
+                        }
+                },
+        },
+        [AB3550_DEVID_FUELGAUGE] = {
+                {
+                        .count = 1,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x21,
+                                        .last = 0x21,
+                                        .perm = AB3550_PERM_RO,
+                                },
+                        }
+                },
+                {
+                        .count = 1,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x00,
+                                        .last = 0x0e,
+                                        .perm = AB3550_PERM_RW,
+                                },
+                        }
+                },
+        },
+        [AB3550_DEVID_VIBRATOR] = {
+                NO_RANGE,
+                {
+                        .count = 1,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x10,
+                                        .last = 0x13,
+                                        .perm = AB3550_PERM_RW,
+                                },
+
+                        }
+                },
+        },
+        [AB3550_DEVID_CODEC] = {
+                {
+                        .count = 2,
+                        .range = (struct ab3550_reg_range[]) {
+                                {
+                                        .first = 0x31,
+                                        .last = 0x63,
+                                        .perm = AB3550_PERM_RW,
+                                },
+                                {
+                                        .first = 0x65,
+                                        .last = 0x68,
+                                        .perm = AB3550_PERM_RW,
+                                },
+                        }
+                },
+                NO_RANGE,
+        },
+};
+
+static struct mfd_cell ab3550_devs[AB3550_NUM_DEVICES] = {
+        [AB3550_DEVID_DAC] = {
+                .name = "ab3550-dac",
+                .id = AB3550_DEVID_DAC,
+                .num_resources = 0,
+        },
+        [AB3550_DEVID_LEDS] = {
+                .name = "ab3550-leds",
+                .id = AB3550_DEVID_LEDS,
+        },
+        [AB3550_DEVID_POWER] = {
+                .name = "ab3550-power",
+                .id = AB3550_DEVID_POWER,
+        },
+        [AB3550_DEVID_REGULATORS] = {
+                .name = "ab3550-regulators",
+                .id = AB3550_DEVID_REGULATORS,
+        },
+        [AB3550_DEVID_SIM] = {
+                .name = "ab3550-sim",
+                .id = AB3550_DEVID_SIM,
+        },
+        [AB3550_DEVID_UART] = {
+                .name = "ab3550-uart",
+                .id = AB3550_DEVID_UART,
+        },
+        [AB3550_DEVID_RTC] = {
+                .name = "ab3550-rtc",
+                .id = AB3550_DEVID_RTC,
+        },
+        [AB3550_DEVID_CHARGER] = {
+                .name = "ab3550-charger",
+                .id = AB3550_DEVID_CHARGER,
+        },
+        [AB3550_DEVID_ADC] = {
+                .name = "ab3550-adc",
+                .id = AB3550_DEVID_ADC,
+                .num_resources = 10,
+                .resources = (struct resource[]) {
+                        {
+                                .name = "TRIGGER-0",
+                                .flags = IORESOURCE_IRQ,
+                                .start = 16,
+                                .end = 16,
+                        },
+                        {
+                                .name = "TRIGGER-1",
+                                .flags = IORESOURCE_IRQ,
+                                .start = 17,
+                                .end = 17,
+                        },
+                        {
+                                .name = "TRIGGER-2",
+                                .flags = IORESOURCE_IRQ,
+                                .start = 18,
+                                .end = 18,
+                        },
+                        {
+                                .name = "TRIGGER-3",
+                                .flags = IORESOURCE_IRQ,
+                                .start = 19,
+                                .end = 19,
+                        },
+                        {
+                                .name = "TRIGGER-4",
+                                .flags = IORESOURCE_IRQ,
+                                .start = 20,
+                                .end = 20,
+                        },
+                        {
+                                .name = "TRIGGER-5",
+                                .flags = IORESOURCE_IRQ,
+                                .start = 21,
+                                .end = 21,
+                        },
+                        {
+                                .name = "TRIGGER-6",
+                                .flags = IORESOURCE_IRQ,
+                                .start = 22,
+                                .end = 22,
+                        },
+                        {
+                                .name = "TRIGGER-7",
+                                .flags = IORESOURCE_IRQ,
+                                .start = 23,
+                                .end = 23,
+                        },
+                        {
+                                .name = "TRIGGER-VBAT-TXON",
+                                .flags = IORESOURCE_IRQ,
+                                .start = 13,
+                                .end = 13,
+                        },
+                        {
+                                .name = "TRIGGER-VBAT",
+                                .flags = IORESOURCE_IRQ,
+                                .start = 12,
+                                .end = 12,
+                        },
+                },
+        },
+        [AB3550_DEVID_FUELGAUGE] = {
+                .name = "ab3550-fuelgauge",
+                .id = AB3550_DEVID_FUELGAUGE,
+        },
+        [AB3550_DEVID_VIBRATOR] = {
+                .name = "ab3550-vibrator",
+                .id = AB3550_DEVID_VIBRATOR,
+        },
+        [AB3550_DEVID_CODEC] = {
+                .name = "ab3550-codec",
+                .id = AB3550_DEVID_CODEC,
+        },
+};
+
+/*
+ * I2C transactions with error messages.
+ */
+static int ab3550_i2c_master_send(struct ab3550 *ab, u8 bank, u8 *data,
+        u8 count)
+{
+        int err;
+
+        err = i2c_master_send(ab->i2c_client[bank], data, count);
+        if (err < 0) {
+                dev_err(&ab->i2c_client[0]->dev, "send error: %d\n", err);
+                return err;
+        }
+        return 0;
+}
+
+static int ab3550_i2c_master_recv(struct ab3550 *ab, u8 bank, u8 *data,
+        u8 count)
+{
+        int err;
+
+        err = i2c_master_recv(ab->i2c_client[bank], data, count);
+        if (err < 0) {
+                dev_err(&ab->i2c_client[0]->dev, "receive error: %d\n", err);
+                return err;
+        }
+        return 0;
+}
+
+/*
+ * Functionality for getting/setting register values.
+ */
+static int get_register_interruptible(struct ab3550 *ab, u8 bank, u8 reg,
+        u8 *value)
+{
+        int err;
+
+        err = mutex_lock_interruptible(&ab->access_mutex);
+        if (err)
+                return err;
+
+        err = ab3550_i2c_master_send(ab, bank, &reg, 1);
+        if (!err)
+                err = ab3550_i2c_master_recv(ab, bank, value, 1);
+
+        mutex_unlock(&ab->access_mutex);
+        return err;
+}
+
+static int get_register_page_interruptible(struct ab3550 *ab, u8 bank,
+        u8 first_reg, u8 *regvals, u8 numregs)
+{
+        int err;
+
+        err = mutex_lock_interruptible(&ab->access_mutex);
+        if (err)
+                return err;
+
+        err = ab3550_i2c_master_send(ab, bank, &first_reg, 1);
+        if (!err)
+                err = ab3550_i2c_master_recv(ab, bank, regvals, numregs);
+
+        mutex_unlock(&ab->access_mutex);
+        return err;
+}
+
+static int mask_and_set_register_interruptible(struct ab3550 *ab, u8 bank,
+        u8 reg, u8 bitmask, u8 bitvalues)
+{
+        int err = 0;
+
+        if (likely(bitmask)) {
+                u8 reg_bits[2] = {reg, 0};
+
+                err = mutex_lock_interruptible(&ab->access_mutex);
+                if (err)
+                        return err;
+
+                if (bitmask == 0xFF) /* No need to read in this case. */
+                        reg_bits[1] = bitvalues;
+                else { /* Read and modify the register value. */
+                        u8 bits;
+
+                        err = ab3550_i2c_master_send(ab, bank, &reg, 1);
+                        if (err)
+                                goto unlock_and_return;
+                        err = ab3550_i2c_master_recv(ab, bank, &bits, 1);
+                        if (err)
+                                goto unlock_and_return;
+                        reg_bits[1] = ((~bitmask & bits) |
+                                (bitmask & bitvalues));
+                }
+                /* Write the new value. */
+                err = ab3550_i2c_master_send(ab, bank, reg_bits, 2);
+unlock_and_return:
+                mutex_unlock(&ab->access_mutex);
+        }
+        return err;
+}
+
+/*
+ * Read/write permission checking functions.
+ */
+static bool page_write_allowed(const struct ab3550_reg_ranges *ranges,
+        u8 first_reg, u8 last_reg)
+{
+        u8 i;
+
+        if (last_reg < first_reg)
+                return false;
+
+        for (i = 0; i < ranges->count; i++) {
+                if (first_reg < ranges->range[i].first)
+                        break;
+                if ((last_reg <= ranges->range[i].last) &&
+                        (ranges->range[i].perm & AB3550_PERM_WR))
+                        return true;
+        }
+        return false;
+}
+
+static bool reg_write_allowed(const struct ab3550_reg_ranges *ranges, u8 reg)
+{
+        return page_write_allowed(ranges, reg, reg);
+}
+
+static bool page_read_allowed(const struct ab3550_reg_ranges *ranges,
+        u8 first_reg, u8 last_reg)
+{
+        u8 i;
+
+        if (last_reg < first_reg)
+                return false;
+        /* Find the range (if it exists in the list) that includes first_reg. */
+        for (i = 0; i < ranges->count; i++) {
+                if (first_reg < ranges->range[i].first)
+                        return false;
+                if (first_reg <= ranges->range[i].last)
+                        break;
+        }
+        /* Make sure that the entire range up to and including last_reg is
+         * readable. This may span several of the ranges in the list.
+         */
+        while ((i < ranges->count) &&
+                (ranges->range[i].perm & AB3550_PERM_RD)) {
+                if (last_reg <= ranges->range[i].last)
+                        return true;
+                if ((++i >= ranges->count) ||
+                        (ranges->range[i].first !=
+                         (ranges->range[i - 1].last + 1))) {
+                        break;
+                }
+        }
+        return false;
+}
+
+static bool reg_read_allowed(const struct ab3550_reg_ranges *ranges, u8 reg)
+{
+        return page_read_allowed(ranges, reg, reg);
+}
+
+/*
+ * The exported register access functionality.
+ */
+int ab3550_get_chip_id(struct device *dev)
+{
+        struct ab3550 *ab = dev_get_drvdata(dev->parent);
+        return (int)ab->chip_id;
+}
+
+int ab3550_mask_and_set_register_interruptible(struct device *dev, u8 bank,
+        u8 reg, u8 bitmask, u8 bitvalues)
+{
+        struct ab3550 *ab;
+        struct platform_device *pdev = to_platform_device(dev);
+
+        if ((AB3550_NUM_BANKS <= bank) ||
+                !reg_write_allowed(&ab3550_reg_ranges[pdev->id][bank], reg))
+                return -EINVAL;
+
+        ab = dev_get_drvdata(dev->parent);
+        return mask_and_set_register_interruptible(ab, bank, reg,
+                bitmask, bitvalues);
+}
+
+int ab3550_set_register_interruptible(struct device *dev, u8 bank, u8 reg,
+        u8 value)
+{
+        return ab3550_mask_and_set_register_interruptible(dev, bank, reg, 0xFF,
+                value);
+}
+
+int ab3550_get_register_interruptible(struct device *dev, u8 bank, u8 reg,
+        u8 *value)
+{
+        struct ab3550 *ab;
+        struct platform_device *pdev = to_platform_device(dev);
+
+        if ((AB3550_NUM_BANKS <= bank) ||
+                !reg_read_allowed(&ab3550_reg_ranges[pdev->id][bank], reg))
+                return -EINVAL;
+
+        ab = dev_get_drvdata(dev->parent);
+        return get_register_interruptible(ab, bank, reg, value);
+}
+
+int ab3550_get_register_page_interruptible(struct device *dev, u8 bank,
+        u8 first_reg, u8 *regvals, u8 numregs)
+{
+        struct ab3550 *ab;
+        struct platform_device *pdev = to_platform_device(dev);
+
+        if ((AB3550_NUM_BANKS <= bank) ||
+                !page_read_allowed(&ab3550_reg_ranges[pdev->id][bank],
+                        first_reg, (first_reg + numregs - 1)))
+                return -EINVAL;
+
+        ab = dev_get_drvdata(dev->parent);
+        return get_register_page_interruptible(ab, bank, first_reg, regvals,
+                numregs);
+}
+
+int ab3550_event_registers_startup_state_get(struct device *dev, u8 *event)
+{
+        struct ab3550 *ab;
+
+        ab = dev_get_drvdata(dev->parent);
+        if (!ab->startup_events_read)
+                return -EAGAIN; /* Try again later */
+
+        memcpy(event, ab->startup_events, AB3550_NUM_EVENT_REG);
+        return 0;
+}
+
+int ab3550_startup_irq_enabled(struct device *dev, unsigned int irq)
+{
+        struct ab3550 *ab;
+        struct ab3550_platform_data *plf_data;
+        bool val;
+
+        ab = get_irq_chip_data(irq);
+        plf_data = ab->i2c_client[0]->dev.platform_data;
+        irq -= plf_data->irq.base;
+        val = ((ab->startup_events[irq / 8] & BIT(irq % 8)) != 0);
+
+        return val;
+}
+
+static struct abx500_ops ab3550_ops = {
+        .get_chip_id = ab3550_get_chip_id,
+        .get_register = ab3550_get_register_interruptible,
+        .set_register = ab3550_set_register_interruptible,
+        .get_register_page = ab3550_get_register_page_interruptible,
+        .set_register_page = NULL,
+        .mask_and_set_register = ab3550_mask_and_set_register_interruptible,
+        .event_registers_startup_state_get =
+                ab3550_event_registers_startup_state_get,
+        .startup_irq_enabled = ab3550_startup_irq_enabled,
+};
+
+static irqreturn_t ab3550_irq_handler(int irq, void *data)
+{
+        struct ab3550 *ab = data;
+        int err;
+        unsigned int i;
+        u8 e[AB3550_NUM_EVENT_REG];
+        u8 *events;
+        unsigned long flags;
+
+        events = (ab->startup_events_read ? e : ab->startup_events);
+
+        err = get_register_page_interruptible(ab, AB3550_EVENT_BANK,
+                AB3550_EVENT_REG, events, AB3550_NUM_EVENT_REG);
+        if (err)
+                goto err_event_rd;
+
+        if (!ab->startup_events_read) {
+                dev_info(&ab->i2c_client[0]->dev,
+                        "startup events 0x%x,0x%x,0x%x,0x%x,0x%x\n",
+                        ab->startup_events[0], ab->startup_events[1],
+                        ab->startup_events[2], ab->startup_events[3],
+                        ab->startup_events[4]);
+                ab->startup_events_read = true;
+                goto out;
+        }
+
+        /* The two highest bits in event[4] are not used. */
+        events[4] &= 0x3f;
+
+        spin_lock_irqsave(&ab->event_lock, flags);
+        for (i = 0; i < AB3550_NUM_EVENT_REG; i++)
+                events[i] &= ~ab->event_mask[i];
+        spin_unlock_irqrestore(&ab->event_lock, flags);
+
+        for (i = 0; i < AB3550_NUM_EVENT_REG; i++) {
+                u8 bit;
+                u8 event_reg;
+
+                dev_dbg(&ab->i2c_client[0]->dev, "IRQ Event[%d]: 0x%2x\n",
+                        i, events[i]);
+
+                event_reg = events[i];
+                for (bit = 0; event_reg; bit++, event_reg /= 2) {
+                        if (event_reg % 2) {
+                                unsigned int irq;
+                                struct ab3550_platform_data *plf_data;
+
+                                plf_data = ab->i2c_client[0]->dev.platform_data;
+                                irq = plf_data->irq.base + (i * 8) + bit;
+                                handle_nested_irq(irq);
+                        }
+                }
+        }
+out:
+        return IRQ_HANDLED;
+
+err_event_rd:
+        dev_dbg(&ab->i2c_client[0]->dev, "error reading event registers\n");
+        return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_DEBUG_FS
+static struct ab3550_reg_ranges debug_ranges[AB3550_NUM_BANKS] = {
+        {
+                .count = 6,
+                .range = (struct ab3550_reg_range[]) {
+                        {
+                                .first = 0x00,
+                                .last = 0x0e,
+                        },
+                        {
+                                .first = 0x10,
+                                .last = 0x1a,
+                        },
+                        {
+                                .first = 0x1e,
+                                .last = 0x4f,
+                        },
+                        {
+                                .first = 0x51,
+                                .last = 0x63,
+                        },
+                        {
+                                .first = 0x65,
+                                .last = 0xa3,
+                        },
+                        {
+                                .first = 0xa5,
+                                .last = 0xa8,
+                        },
+                }
+        },
+        {
+                .count = 8,
+                .range = (struct ab3550_reg_range[]) {
+                        {
+                                .first = 0x00,
+                                .last = 0x0e,
+                        },
+                        {
+                                .first = 0x10,
+                                .last = 0x17,
+                        },
+                        {
+                                .first = 0x1a,
+                                .last = 0x1c,
+                        },
+                        {
+                                .first = 0x20,
+                                .last = 0x56,
+                        },
+                        {
+                                .first = 0x5a,
+                                .last = 0x88,
+                        },
+                        {
+                                .first = 0x8a,
+                                .last = 0xad,
+                        },
+                        {
+                                .first = 0xb0,
+                                .last = 0xba,
+                        },
+                        {
+                                .first = 0xbc,
+                                .last = 0xc3,
+                        },
+                }
+        },
+};
+
+static int ab3550_registers_print(struct seq_file *s, void *p)
+{
+        struct ab3550 *ab = s->private;
+        int bank;
+
+        seq_printf(s, AB3550_NAME_STRING " register values:\n");
+
+        for (bank = 0; bank < AB3550_NUM_BANKS; bank++) {
+                unsigned int i;
+
+                seq_printf(s, " bank %d:\n", bank);
+                for (i = 0; i < debug_ranges[bank].count; i++) {
+                        u8 reg;
+
+                        for (reg = debug_ranges[bank].range[i].first;
+                                reg <= debug_ranges[bank].range[i].last;
+                                reg++) {
+                                u8 value;
+
+                                get_register_interruptible(ab, bank, reg,
+                                        &value);
+                                seq_printf(s, "  [%d/0x%02X]: 0x%02X\n", bank,
+                                        reg, value);
+                        }
+                }
+        }
+        return 0;
+}
+
+static int ab3550_registers_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, ab3550_registers_print, inode->i_private);
+}
+
+static const struct file_operations ab3550_registers_fops = {
+        .open = ab3550_registers_open,
+        .read = seq_read,
+        .llseek = seq_lseek,
+        .release = single_release,
+        .owner = THIS_MODULE,
+};
+
+static int ab3550_bank_print(struct seq_file *s, void *p)
+{
+        struct ab3550 *ab = s->private;
+
+        seq_printf(s, "%d\n", ab->debug_bank);
+        return 0;
+}
+
+static int ab3550_bank_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, ab3550_bank_print, inode->i_private);
+}
+
+static ssize_t ab3550_bank_write(struct file *file,
+        const char __user *user_buf,
+        size_t count, loff_t *ppos)
+{
+        struct ab3550 *ab = ((struct seq_file *)(file->private_data))->private;
+        char buf[32];
+        int buf_size;
+        unsigned long user_bank;
+        int err;
+
+        /* Get userspace string and assure termination */
+        buf_size = min(count, (sizeof(buf) - 1));
+        if (copy_from_user(buf, user_buf, buf_size))
+                return -EFAULT;
+        buf[buf_size] = 0;
+
+        err = strict_strtoul(buf, 0, &user_bank);
+        if (err)
+                return -EINVAL;
+
+        if (user_bank >= AB3550_NUM_BANKS) {
+                dev_err(&ab->i2c_client[0]->dev,
+                        "debugfs error input > number of banks\n");
+                return -EINVAL;
+        }
+
+        ab->debug_bank = user_bank;
+
+        return buf_size;
+}
+
+static int ab3550_address_print(struct seq_file *s, void *p)
+{
+        struct ab3550 *ab = s->private;
+
+        seq_printf(s, "0x%02X\n", ab->debug_address);
+        return 0;
+}
+
+static int ab3550_address_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, ab3550_address_print, inode->i_private);
+}
+
+static ssize_t ab3550_address_write(struct file *file,
+        const char __user *user_buf,
+        size_t count, loff_t *ppos)
+{
+        struct ab3550 *ab = ((struct seq_file *)(file->private_data))->private;
+        char buf[32];
+        int buf_size;
+        unsigned long user_address;
+        int err;
+
+        /* Get userspace string and assure termination */
+        buf_size = min(count, (sizeof(buf) - 1));
+        if (copy_from_user(buf, user_buf, buf_size))
+                return -EFAULT;
+        buf[buf_size] = 0;
+
+        err = strict_strtoul(buf, 0, &user_address);
+        if (err)
+                return -EINVAL;
+        if (user_address > 0xff) {
+                dev_err(&ab->i2c_client[0]->dev,
+                        "debugfs error input > 0xff\n");
+                return -EINVAL;
+        }
+        ab->debug_address = user_address;
+        return buf_size;
+}
+
+static int ab3550_val_print(struct seq_file *s, void *p)
+{
+        struct ab3550 *ab = s->private;
+        int err;
+        u8 regvalue;
+
+        err = get_register_interruptible(ab, (u8)ab->debug_bank,
+                (u8)ab->debug_address, &regvalue);
+        if (err)
+                return -EINVAL;
+        seq_printf(s, "0x%02X\n", regvalue);
+
+        return 0;
+}
+
+static int ab3550_val_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, ab3550_val_print, inode->i_private);
+}
+
+static ssize_t ab3550_val_write(struct file *file,
+        const char __user *user_buf,
+        size_t count, loff_t *ppos)
+{
+        struct ab3550 *ab = ((struct seq_file *)(file->private_data))->private;
+        char buf[32];
+        int buf_size;
+        unsigned long user_val;
+        int err;
+        u8 regvalue;
+
+        /* Get userspace string and assure termination */
+        buf_size = min(count, (sizeof(buf)-1));
+        if (copy_from_user(buf, user_buf, buf_size))
+                return -EFAULT;
+        buf[buf_size] = 0;
+
+        err = strict_strtoul(buf, 0, &user_val);
+        if (err)
+                return -EINVAL;
+        if (user_val > 0xff) {
+                dev_err(&ab->i2c_client[0]->dev,
+                        "debugfs error input > 0xff\n");
+                return -EINVAL;
+        }
+        err = mask_and_set_register_interruptible(
+                ab, (u8)ab->debug_bank,
+                (u8)ab->debug_address, 0xFF, (u8)user_val);
+        if (err)
+                return -EINVAL;
+
+        get_register_interruptible(ab, (u8)ab->debug_bank,
+                (u8)ab->debug_address, &regvalue);
+        if (err)
+                return -EINVAL;
+
+        return buf_size;
+}
+
+static const struct file_operations ab3550_bank_fops = {
+        .open = ab3550_bank_open,
+        .write = ab3550_bank_write,
+        .read = seq_read,
+        .llseek = seq_lseek,
+        .release = single_release,
+        .owner = THIS_MODULE,
+};
+
+static const struct file_operations ab3550_address_fops = {
+        .open = ab3550_address_open,
+        .write = ab3550_address_write,
+        .read = seq_read,
+        .llseek = seq_lseek,
+        .release = single_release,
+        .owner = THIS_MODULE,
+};
+
+static const struct file_operations ab3550_val_fops = {
+        .open = ab3550_val_open,
+        .write = ab3550_val_write,
+        .read = seq_read,
+        .llseek = seq_lseek,
+        .release = single_release,
+        .owner = THIS_MODULE,
+};
+
+static struct dentry *ab3550_dir;
+static struct dentry *ab3550_reg_file;
+static struct dentry *ab3550_bank_file;
+static struct dentry *ab3550_address_file;
+static struct dentry *ab3550_val_file;
+
+static inline void ab3550_setup_debugfs(struct ab3550 *ab)
+{
+        ab->debug_bank = 0;
+        ab->debug_address = 0x00;
+
+        ab3550_dir = debugfs_create_dir(AB3550_NAME_STRING, NULL);
+        if (!ab3550_dir)
+                goto exit_no_debugfs;
+
+        ab3550_reg_file = debugfs_create_file("all-registers",
+                S_IRUGO, ab3550_dir, ab, &ab3550_registers_fops);
+        if (!ab3550_reg_file)
+                goto exit_destroy_dir;
+
+        ab3550_bank_file = debugfs_create_file("register-bank",
+                (S_IRUGO | S_IWUGO), ab3550_dir, ab, &ab3550_bank_fops);
+        if (!ab3550_bank_file)
+                goto exit_destroy_reg;
+
+        ab3550_address_file = debugfs_create_file("register-address",
+                (S_IRUGO | S_IWUGO), ab3550_dir, ab, &ab3550_address_fops);
+        if (!ab3550_address_file)
+                goto exit_destroy_bank;
+
+        ab3550_val_file = debugfs_create_file("register-value",
+                (S_IRUGO | S_IWUGO), ab3550_dir, ab, &ab3550_val_fops);
+        if (!ab3550_val_file)
+                goto exit_destroy_address;
+
+        return;
+
+exit_destroy_address:
+        debugfs_remove(ab3550_address_file);
+exit_destroy_bank:
+        debugfs_remove(ab3550_bank_file);
+exit_destroy_reg:
+        debugfs_remove(ab3550_reg_file);
+exit_destroy_dir:
+        debugfs_remove(ab3550_dir);
+exit_no_debugfs:
+        dev_err(&ab->i2c_client[0]->dev, "failed to create debugfs entries.\n");
+        return;
+}
+
+static inline void ab3550_remove_debugfs(void)
+{
+        debugfs_remove(ab3550_val_file);
+        debugfs_remove(ab3550_address_file);
+        debugfs_remove(ab3550_bank_file);
+        debugfs_remove(ab3550_reg_file);
+        debugfs_remove(ab3550_dir);
+}
+
+#else /* !CONFIG_DEBUG_FS */
+static inline void ab3550_setup_debugfs(struct ab3550 *ab)
+{
+}
+static inline void ab3550_remove_debugfs(void)
+{
+}
+#endif
+
+/*
+ * Basic set-up, datastructure creation/destruction and I2C interface.
+ * This sets up a default config in the AB3550 chip so that it
+ * will work as expected.
+ */
+static int __init ab3550_setup(struct ab3550 *ab)
+{
+        int err = 0;
+        int i;
+        struct ab3550_platform_data *plf_data;
+        struct abx500_init_settings *settings;
+
+        plf_data = ab->i2c_client[0]->dev.platform_data;
+        settings = plf_data->init_settings;
+
+        for (i = 0; i < plf_data->init_settings_sz; i++) {
+                err = mask_and_set_register_interruptible(ab,
+                        settings[i].bank,
+                        settings[i].reg,
+                        0xFF, settings[i].setting);
+                if (err)
+                        goto exit_no_setup;
+
+                /* If event mask register update the event mask in ab3550 */
+                if ((settings[i].bank == 0) &&
+                        (AB3550_IMR1 <= settings[i].reg) &&
+                        (settings[i].reg <= AB3550_IMR5)) {
+                        ab->event_mask[settings[i].reg - AB3550_IMR1] =
+                                settings[i].setting;
+                }
+        }
+exit_no_setup:
+        return err;
+}
+
+static void ab3550_mask_work(struct work_struct *work)
+{
+        struct ab3550 *ab = container_of(work, struct ab3550, mask_work);
+        int i;
+        unsigned long flags;
+        u8 mask[AB3550_NUM_EVENT_REG];
+
+        spin_lock_irqsave(&ab->event_lock, flags);
+        for (i = 0; i < AB3550_NUM_EVENT_REG; i++)
+                mask[i] = ab->event_mask[i];
+        spin_unlock_irqrestore(&ab->event_lock, flags);
+
+        for (i = 0; i < AB3550_NUM_EVENT_REG; i++) {
+                int err;
+
+                err = mask_and_set_register_interruptible(ab, 0,
+                        (AB3550_IMR1 + i), ~0, mask[i]);
+                if (err)
+                        dev_err(&ab->i2c_client[0]->dev,
+                                "ab3550_mask_work failed 0x%x,0x%x\n",
+                                (AB3550_IMR1 + i), mask[i]);
+        }
+}
+
+static void ab3550_mask(unsigned int irq)
+{
+        unsigned long flags;
+        struct ab3550 *ab;
+        struct ab3550_platform_data *plf_data;
+
+        ab = get_irq_chip_data(irq);
+        plf_data = ab->i2c_client[0]->dev.platform_data;
+        irq -= plf_data->irq.base;
+
+        spin_lock_irqsave(&ab->event_lock, flags);
+        ab->event_mask[irq / 8] |= BIT(irq % 8);
+        spin_unlock_irqrestore(&ab->event_lock, flags);
+
+        schedule_work(&ab->mask_work);
+}
+
+static void ab3550_unmask(unsigned int irq)
+{
+        unsigned long flags;
+        struct ab3550 *ab;
+        struct ab3550_platform_data *plf_data;
+
+        ab = get_irq_chip_data(irq);
+        plf_data = ab->i2c_client[0]->dev.platform_data;
+        irq -= plf_data->irq.base;
+
+        spin_lock_irqsave(&ab->event_lock, flags);
+        ab->event_mask[irq / 8] &= ~BIT(irq % 8);
+        spin_unlock_irqrestore(&ab->event_lock, flags);
+
+        schedule_work(&ab->mask_work);
+}
+
+static void noop(unsigned int irq)
+{
+}
+
+static struct irq_chip ab3550_irq_chip = {
+        .name           = "ab3550-core", /* Keep the same name as the request */
+        .startup        = NULL, /* defaults to enable */
+        .shutdown       = NULL, /* defaults to disable */
+        .enable         = NULL, /* defaults to unmask */
+        .disable        = ab3550_mask, /* No default to mask in chip.c */
+        .ack            = noop,
+        .mask           = ab3550_mask,
+        .unmask         = ab3550_unmask,
+        .end            = NULL,
+};
+
+struct ab_family_id {
+        u8      id;
+        char    *name;
+};
+
+static const struct ab_family_id ids[] __initdata = {
+        /* AB3550 */
+        {
+                .id = AB3550_P1A,
+                .name = "P1A"
+        },
+        /* Terminator */
+        {
+                .id = 0x00,
+        }
+};
+
+static int __init ab3550_probe(struct i2c_client *client,
+        const struct i2c_device_id *id)
+{
+        struct ab3550 *ab;
+        struct ab3550_platform_data *ab3550_plf_data =
+                client->dev.platform_data;
+        int err;
+        int i;
+        int num_i2c_clients = 0;
+
+        ab = kzalloc(sizeof(struct ab3550), GFP_KERNEL);
+        if (!ab) {
+                dev_err(&client->dev,
+                        "could not allocate " AB3550_NAME_STRING " device\n");
+                return -ENOMEM;
+        }
+
+        /* Initialize data structure */
+        mutex_init(&ab->access_mutex);
+        spin_lock_init(&ab->event_lock);
+        ab->i2c_client[0] = client;
+
+        i2c_set_clientdata(client, ab);
+
+        /* Read chip ID register */
+        err = get_register_interruptible(ab, 0, AB3550_CID_REG, &ab->chip_id);
+        if (err) {
+                dev_err(&client->dev, "could not communicate with the analog "
+                        "baseband chip\n");
+                goto exit_no_detect;
+        }
+
+        for (i = 0; ids[i].id != 0x0; i++) {
+                if (ids[i].id == ab->chip_id) {
+                        snprintf(&ab->chip_name[0], sizeof(ab->chip_name) - 1,
+                                AB3550_ID_FORMAT_STRING, ids[i].name);
+                        break;
+                }
+        }
+
+        if (ids[i].id == 0x0) {
+                dev_err(&client->dev, "unknown analog baseband chip id: 0x%x\n",
+                        ab->chip_id);
+                dev_err(&client->dev, "driver not started!\n");
+                goto exit_no_detect;
+        }
+
+        dev_info(&client->dev, "detected AB chip: %s\n", &ab->chip_name[0]);
+
+        /* Attach other dummy I2C clients. */
+        while (++num_i2c_clients < AB3550_NUM_BANKS) {
+                ab->i2c_client[num_i2c_clients] =
+                        i2c_new_dummy(client->adapter,
+                                (client->addr + num_i2c_clients));
+                if (!ab->i2c_client[num_i2c_clients]) {
+                        err = -ENOMEM;
+                        goto exit_no_dummy_client;
+                }
+                strlcpy(ab->i2c_client[num_i2c_clients]->name, id->name,
+                        sizeof(ab->i2c_client[num_i2c_clients]->name));
+        }
+
+        err = ab3550_setup(ab);
+        if (err)
+                goto exit_no_setup;
+
+        INIT_WORK(&ab->mask_work, ab3550_mask_work);
+
+        for (i = 0; i < ab3550_plf_data->irq.count; i++) {
+                unsigned int irq;
+
+                irq = ab3550_plf_data->irq.base + i;
+                set_irq_chip_data(irq, ab);
+                set_irq_chip_and_handler(irq, &ab3550_irq_chip,
+                        handle_simple_irq);
+                set_irq_nested_thread(irq, 1);
+#ifdef CONFIG_ARM
+                set_irq_flags(irq, IRQF_VALID);
+#else
+                set_irq_noprobe(irq);
+#endif
+        }
+
+        err = request_threaded_irq(client->irq, NULL, ab3550_irq_handler,
+                IRQF_ONESHOT, "ab3550-core", ab);
+        /* This real unpredictable IRQ is of course sampled for entropy */
+        rand_initialize_irq(client->irq);
+
+        if (err)
+                goto exit_no_irq;
+
+        err = abx500_register_ops(&client->dev, &ab3550_ops);
+        if (err)
+                goto exit_no_ops;
+
+        /* Set up and register the platform devices. */
+        for (i = 0; i < AB3550_NUM_DEVICES; i++) {
+                ab3550_devs[i].platform_data = ab3550_plf_data->dev_data[i];
+                ab3550_devs[i].data_size = ab3550_plf_data->dev_data_sz[i];
+        }
+
+        err = mfd_add_devices(&client->dev, 0, ab3550_devs,
+                ARRAY_SIZE(ab3550_devs), NULL,
+                ab3550_plf_data->irq.base);
+
+        ab3550_setup_debugfs(ab);
+
+        return 0;
+
+exit_no_ops:
+exit_no_irq:
+exit_no_setup:
+exit_no_dummy_client:
+        /* Unregister the dummy i2c clients. */
+        while (--num_i2c_clients)
+                i2c_unregister_device(ab->i2c_client[num_i2c_clients]);
+exit_no_detect:
+        kfree(ab);
+        return err;
+}
+
+static int __exit ab3550_remove(struct i2c_client *client)
+{
+        struct ab3550 *ab = i2c_get_clientdata(client);
+        int num_i2c_clients = AB3550_NUM_BANKS;
+
+        mfd_remove_devices(&client->dev);
+        ab3550_remove_debugfs();
+
+        while (--num_i2c_clients)
+                i2c_unregister_device(ab->i2c_client[num_i2c_clients]);
+
+        /*
+         * At this point, all subscribers should have unregistered
+         * their notifiers so deactivate IRQ
+         */
+        free_irq(client->irq, ab);
+        kfree(ab);
+        return 0;
+}
+
+static const struct i2c_device_id ab3550_id[] = {
+        {AB3550_NAME_STRING, 0},
+        {}
+};
+MODULE_DEVICE_TABLE(i2c, ab3550_id);
+
+static struct i2c_driver ab3550_driver = {
+        .driver = {
+                .name   = AB3550_NAME_STRING,
+                .owner  = THIS_MODULE,
+        },
+        .id_table       = ab3550_id,
+        .probe          = ab3550_probe,
+        .remove         = __exit_p(ab3550_remove),
+};
+
+static int __init ab3550_i2c_init(void)
+{
+        return i2c_add_driver(&ab3550_driver);
+}
+
+static void __exit ab3550_i2c_exit(void)
+{
+        i2c_del_driver(&ab3550_driver);
+}
+
+subsys_initcall(ab3550_i2c_init);
+module_exit(ab3550_i2c_exit);
+
+MODULE_AUTHOR("Mattias Wallin <mattias.wallin@stericsson.com>");
+MODULE_DESCRIPTION("AB3550 core driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/ab4500-core.c b/drivers/mfd/ab4500-core.c
deleted file mode 100644
index c275daa3ab1a..000000000000
--- a/drivers/mfd/ab4500-core.c
+++ /dev/null
@@ -1,209 +0,0 @@
-/*
- * Copyright (C) 2009 ST-Ericsson
- *
- * Author: Srinidhi KASAGAR <srinidhi.kasagar@stericsson.com>
- *
- * 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.
- *
- * AB4500 is a companion power management chip used with U8500.
- * On this platform, this is interfaced with SSP0 controller
- * which is a ARM primecell pl022.
- *
- * At the moment the module just exports read/write features.
- * Interrupt management to be added - TODO.
- */
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/spi/spi.h>
-#include <linux/mfd/ab4500.h>
-
-/* just required if probe fails, we need to
- * unregister the device
- */
-static struct spi_driver ab4500_driver;
-
-/*
- * This funtion writes to any AB4500 registers using
- * SPI protocol &  before it writes it packs the data
- * in the below 24 bit frame format
- *
- *       *|------------------------------------|
- *       *| 23|22...18|17.......10|9|8|7......0|
- *       *| r/w  bank       adr          data  |
- *       * ------------------------------------
- *
- * This function shouldn't be called from interrupt
- * context
- */
-int ab4500_write(struct ab4500 *ab4500, unsigned char block,
-                unsigned long addr, unsigned char data)
-{
-        struct spi_transfer xfer;
-        struct spi_message      msg;
-        int err;
-        unsigned long spi_data =
-                block << 18 | addr << 10 | data;
-
-        mutex_lock(&ab4500->lock);
-        ab4500->tx_buf[0] = spi_data;
-        ab4500->rx_buf[0] = 0;
-
-        xfer.tx_buf     = ab4500->tx_buf;
-        xfer.rx_buf     = NULL;
-        xfer.len        = sizeof(unsigned long);
-
-        spi_message_init(&msg);
-        spi_message_add_tail(&xfer, &msg);
-
-        err = spi_sync(ab4500->spi, &msg);
-        mutex_unlock(&ab4500->lock);
-
-        return err;
-}
-EXPORT_SYMBOL(ab4500_write);
-
-int ab4500_read(struct ab4500 *ab4500, unsigned char block,
-                unsigned long addr)
-{
-        struct spi_transfer xfer;
-        struct spi_message      msg;
-        unsigned long spi_data =
-                1 << 23 | block << 18 | addr << 10;
-
-        mutex_lock(&ab4500->lock);
-        ab4500->tx_buf[0] = spi_data;
-        ab4500->rx_buf[0] = 0;
-
-        xfer.tx_buf     = ab4500->tx_buf;
-        xfer.rx_buf     = ab4500->rx_buf;
-        xfer.len        = sizeof(unsigned long);
-
-        spi_message_init(&msg);
-        spi_message_add_tail(&xfer, &msg);
-
-        spi_sync(ab4500->spi, &msg);
-        mutex_unlock(&ab4500->lock);
-
-        return  ab4500->rx_buf[0];
-}
-EXPORT_SYMBOL(ab4500_read);
-
-/* ref: ab3100 core */
-#define AB4500_DEVICE(devname, devid)                           \
-static struct platform_device ab4500_##devname##_device = {     \
-        .name   = devid,                                        \
-        .id     = -1,                                           \
-}
-
-/* list of childern devices of ab4500 - all are
- * not populated here - TODO
- */
-AB4500_DEVICE(charger, "ab4500-charger");
-AB4500_DEVICE(audio, "ab4500-audio");
-AB4500_DEVICE(usb, "ab4500-usb");
-AB4500_DEVICE(tvout, "ab4500-tvout");
-AB4500_DEVICE(sim, "ab4500-sim");
-AB4500_DEVICE(gpadc, "ab4500-gpadc");
-AB4500_DEVICE(clkmgt, "ab4500-clkmgt");
-AB4500_DEVICE(misc, "ab4500-misc");
-
-static struct platform_device *ab4500_platform_devs[] = {
-        &ab4500_charger_device,
-        &ab4500_audio_device,
-        &ab4500_usb_device,
-        &ab4500_tvout_device,
-        &ab4500_sim_device,
-        &ab4500_gpadc_device,
-        &ab4500_clkmgt_device,
-        &ab4500_misc_device,
-};
-
-static int __init ab4500_probe(struct spi_device *spi)
-{
-        struct ab4500   *ab4500;
-        unsigned char revision;
-        int err = 0;
-        int i;
-
-        ab4500 = kzalloc(sizeof *ab4500, GFP_KERNEL);
-        if (!ab4500) {
-                dev_err(&spi->dev, "could not allocate AB4500\n");
-                err = -ENOMEM;
-                goto not_detect;
-        }
-
-        ab4500->spi = spi;
-        spi_set_drvdata(spi, ab4500);
-
-        mutex_init(&ab4500->lock);
-
-        /* read the revision register */
-        revision = ab4500_read(ab4500, AB4500_MISC, AB4500_REV_REG);
-
-        /* revision id 0x0 is for early drop, 0x10 is for cut1.0 */
-        if (revision == 0x0 || revision == 0x10)
-                dev_info(&spi->dev, "Detected chip: %s, revision = %x\n",
-                        ab4500_driver.driver.name, revision);
-        else    {
-                dev_err(&spi->dev, "unknown chip: 0x%x\n", revision);
-                goto not_detect;
-        }
-
-        for (i = 0; i < ARRAY_SIZE(ab4500_platform_devs); i++)  {
-                ab4500_platform_devs[i]->dev.parent =
-                        &spi->dev;
-                platform_set_drvdata(ab4500_platform_devs[i], ab4500);
-        }
-
-        /* register the ab4500 platform devices */
-        platform_add_devices(ab4500_platform_devs,
-                        ARRAY_SIZE(ab4500_platform_devs));
-
-        return err;
-
- not_detect:
-        spi_unregister_driver(&ab4500_driver);
-        kfree(ab4500);
-        return err;
-}
-
-static int __devexit ab4500_remove(struct spi_device *spi)
-{
-        struct ab4500 *ab4500 =
-                spi_get_drvdata(spi);
-
-        kfree(ab4500);
-
-        return 0;
-}
-
-static struct spi_driver ab4500_driver = {
-        .driver = {
-                .name = "ab4500",
-                .owner = THIS_MODULE,
-        },
-        .probe = ab4500_probe,
-        .remove = __devexit_p(ab4500_remove)
-};
-
-static int __devinit ab4500_init(void)
-{
-        return spi_register_driver(&ab4500_driver);
-}
-
-static void __exit ab4500_exit(void)
-{
-        spi_unregister_driver(&ab4500_driver);
-}
-
-subsys_initcall(ab4500_init);
-module_exit(ab4500_exit);
-
-MODULE_AUTHOR("Srinidhi KASAGAR <srinidhi.kasagar@stericsson.com");
-MODULE_DESCRIPTION("AB4500 core driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
new file mode 100644
index 000000000000..f3d26fa9c34d
--- /dev/null
+++ b/drivers/mfd/ab8500-core.c
@@ -0,0 +1,444 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Srinidhi Kasagar <srinidhi.kasagar@stericsson.com>
+ * Author: Rabin Vincent <rabin.vincent@stericsson.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/irq.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/ab8500.h>
+
+/*
+ * Interrupt register offsets
+ * Bank : 0x0E
+ */
+#define AB8500_IT_SOURCE1_REG           0x0E00
+#define AB8500_IT_SOURCE2_REG           0x0E01
+#define AB8500_IT_SOURCE3_REG           0x0E02
+#define AB8500_IT_SOURCE4_REG           0x0E03
+#define AB8500_IT_SOURCE5_REG           0x0E04
+#define AB8500_IT_SOURCE6_REG           0x0E05
+#define AB8500_IT_SOURCE7_REG           0x0E06
+#define AB8500_IT_SOURCE8_REG           0x0E07
+#define AB8500_IT_SOURCE19_REG          0x0E12
+#define AB8500_IT_SOURCE20_REG          0x0E13
+#define AB8500_IT_SOURCE21_REG          0x0E14
+#define AB8500_IT_SOURCE22_REG          0x0E15
+#define AB8500_IT_SOURCE23_REG          0x0E16
+#define AB8500_IT_SOURCE24_REG          0x0E17
+
+/*
+ * latch registers
+ */
+#define AB8500_IT_LATCH1_REG            0x0E20
+#define AB8500_IT_LATCH2_REG            0x0E21
+#define AB8500_IT_LATCH3_REG            0x0E22
+#define AB8500_IT_LATCH4_REG            0x0E23
+#define AB8500_IT_LATCH5_REG            0x0E24
+#define AB8500_IT_LATCH6_REG            0x0E25
+#define AB8500_IT_LATCH7_REG            0x0E26
+#define AB8500_IT_LATCH8_REG            0x0E27
+#define AB8500_IT_LATCH9_REG            0x0E28
+#define AB8500_IT_LATCH10_REG           0x0E29
+#define AB8500_IT_LATCH19_REG           0x0E32
+#define AB8500_IT_LATCH20_REG           0x0E33
+#define AB8500_IT_LATCH21_REG           0x0E34
+#define AB8500_IT_LATCH22_REG           0x0E35
+#define AB8500_IT_LATCH23_REG           0x0E36
+#define AB8500_IT_LATCH24_REG           0x0E37
+
+/*
+ * mask registers
+ */
+
+#define AB8500_IT_MASK1_REG             0x0E40
+#define AB8500_IT_MASK2_REG             0x0E41
+#define AB8500_IT_MASK3_REG             0x0E42
+#define AB8500_IT_MASK4_REG             0x0E43
+#define AB8500_IT_MASK5_REG             0x0E44
+#define AB8500_IT_MASK6_REG             0x0E45
+#define AB8500_IT_MASK7_REG             0x0E46
+#define AB8500_IT_MASK8_REG             0x0E47
+#define AB8500_IT_MASK9_REG             0x0E48
+#define AB8500_IT_MASK10_REG            0x0E49
+#define AB8500_IT_MASK11_REG            0x0E4A
+#define AB8500_IT_MASK12_REG            0x0E4B
+#define AB8500_IT_MASK13_REG            0x0E4C
+#define AB8500_IT_MASK14_REG            0x0E4D
+#define AB8500_IT_MASK15_REG            0x0E4E
+#define AB8500_IT_MASK16_REG            0x0E4F
+#define AB8500_IT_MASK17_REG            0x0E50
+#define AB8500_IT_MASK18_REG            0x0E51
+#define AB8500_IT_MASK19_REG            0x0E52
+#define AB8500_IT_MASK20_REG            0x0E53
+#define AB8500_IT_MASK21_REG            0x0E54
+#define AB8500_IT_MASK22_REG            0x0E55
+#define AB8500_IT_MASK23_REG            0x0E56
+#define AB8500_IT_MASK24_REG            0x0E57
+
+#define AB8500_REV_REG                  0x1080
+
+/*
+ * Map interrupt numbers to the LATCH and MASK register offsets, Interrupt
+ * numbers are indexed into this array with (num / 8).
+ *
+ * This is one off from the register names, i.e. AB8500_IT_MASK1_REG is at
+ * offset 0.
+ */
+static const int ab8500_irq_regoffset[AB8500_NUM_IRQ_REGS] = {
+        0, 1, 2, 3, 4, 6, 7, 8, 9, 18, 19, 20, 21,
+};
+
+static int __ab8500_write(struct ab8500 *ab8500, u16 addr, u8 data)
+{
+        int ret;
+
+        dev_vdbg(ab8500->dev, "wr: addr %#x <= %#x\n", addr, data);
+
+        ret = ab8500->write(ab8500, addr, data);
+        if (ret < 0)
+                dev_err(ab8500->dev, "failed to write reg %#x: %d\n",
+                        addr, ret);
+
+        return ret;
+}
+
+/**
+ * ab8500_write() - write an AB8500 register
+ * @ab8500: device to write to
+ * @addr: address of the register
+ * @data: value to write
+ */
+int ab8500_write(struct ab8500 *ab8500, u16 addr, u8 data)
+{
+        int ret;
+
+        mutex_lock(&ab8500->lock);
+        ret = __ab8500_write(ab8500, addr, data);
+        mutex_unlock(&ab8500->lock);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(ab8500_write);
+
+static int __ab8500_read(struct ab8500 *ab8500, u16 addr)
+{
+        int ret;
+
+        ret = ab8500->read(ab8500, addr);
+        if (ret < 0)
+                dev_err(ab8500->dev, "failed to read reg %#x: %d\n",
+                        addr, ret);
+
+        dev_vdbg(ab8500->dev, "rd: addr %#x => data %#x\n", addr, ret);
+
+        return ret;
+}
+
+/**
+ * ab8500_read() - read an AB8500 register
+ * @ab8500: device to read from
+ * @addr: address of the register
+ */
+int ab8500_read(struct ab8500 *ab8500, u16 addr)
+{
+        int ret;
+
+        mutex_lock(&ab8500->lock);
+        ret = __ab8500_read(ab8500, addr);
+        mutex_unlock(&ab8500->lock);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(ab8500_read);
+
+/**
+ * ab8500_set_bits() - set a bitfield in an AB8500 register
+ * @ab8500: device to read from
+ * @addr: address of the register
+ * @mask: mask of the bitfield to modify
+ * @data: value to set to the bitfield
+ */
+int ab8500_set_bits(struct ab8500 *ab8500, u16 addr, u8 mask, u8 data)
+{
+        int ret;
+
+        mutex_lock(&ab8500->lock);
+
+        ret = __ab8500_read(ab8500, addr);
+        if (ret < 0)
+                goto out;
+
+        ret &= ~mask;
+        ret |= data;
+
+        ret = __ab8500_write(ab8500, addr, ret);
+
+out:
+        mutex_unlock(&ab8500->lock);
+        return ret;
+}
+EXPORT_SYMBOL_GPL(ab8500_set_bits);
+
+static void ab8500_irq_lock(unsigned int irq)
+{
+        struct ab8500 *ab8500 = get_irq_chip_data(irq);
+
+        mutex_lock(&ab8500->irq_lock);
+}
+
+static void ab8500_irq_sync_unlock(unsigned int irq)
+{
+        struct ab8500 *ab8500 = get_irq_chip_data(irq);
+        int i;
+
+        for (i = 0; i < AB8500_NUM_IRQ_REGS; i++) {
+                u8 old = ab8500->oldmask[i];
+                u8 new = ab8500->mask[i];
+                int reg;
+
+                if (new == old)
+                        continue;
+
+                ab8500->oldmask[i] = new;
+
+                reg = AB8500_IT_MASK1_REG + ab8500_irq_regoffset[i];
+                ab8500_write(ab8500, reg, new);
+        }
+
+        mutex_unlock(&ab8500->irq_lock);
+}
+
+static void ab8500_irq_mask(unsigned int irq)
+{
+        struct ab8500 *ab8500 = get_irq_chip_data(irq);
+        int offset = irq - ab8500->irq_base;
+        int index = offset / 8;
+        int mask = 1 << (offset % 8);
+
+        ab8500->mask[index] |= mask;
+}
+
+static void ab8500_irq_unmask(unsigned int irq)
+{
+        struct ab8500 *ab8500 = get_irq_chip_data(irq);
+        int offset = irq - ab8500->irq_base;
+        int index = offset / 8;
+        int mask = 1 << (offset % 8);
+
+        ab8500->mask[index] &= ~mask;
+}
+
+static struct irq_chip ab8500_irq_chip = {
+        .name                   = "ab8500",
+        .bus_lock               = ab8500_irq_lock,
+        .bus_sync_unlock        = ab8500_irq_sync_unlock,
+        .mask                   = ab8500_irq_mask,
+        .unmask                 = ab8500_irq_unmask,
+};
+
+static irqreturn_t ab8500_irq(int irq, void *dev)
+{
+        struct ab8500 *ab8500 = dev;
+        int i;
+
+        dev_vdbg(ab8500->dev, "interrupt\n");
+
+        for (i = 0; i < AB8500_NUM_IRQ_REGS; i++) {
+                int regoffset = ab8500_irq_regoffset[i];
+                int status;
+
+                status = ab8500_read(ab8500, AB8500_IT_LATCH1_REG + regoffset);
+                if (status <= 0)
+                        continue;
+
+                do {
+                        int bit = __ffs(status);
+                        int line = i * 8 + bit;
+
+                        handle_nested_irq(ab8500->irq_base + line);
+                        status &= ~(1 << bit);
+                } while (status);
+        }
+
+        return IRQ_HANDLED;
+}
+
+static int ab8500_irq_init(struct ab8500 *ab8500)
+{
+        int base = ab8500->irq_base;
+        int irq;
+
+        for (irq = base; irq < base + AB8500_NR_IRQS; irq++) {
+                set_irq_chip_data(irq, ab8500);
+                set_irq_chip_and_handler(irq, &ab8500_irq_chip,
+                                         handle_simple_irq);
+                set_irq_nested_thread(irq, 1);
+#ifdef CONFIG_ARM
+                set_irq_flags(irq, IRQF_VALID);
+#else
+                set_irq_noprobe(irq);
+#endif
+        }
+
+        return 0;
+}
+
+static void ab8500_irq_remove(struct ab8500 *ab8500)
+{
+        int base = ab8500->irq_base;
+        int irq;
+
+        for (irq = base; irq < base + AB8500_NR_IRQS; irq++) {
+#ifdef CONFIG_ARM
+                set_irq_flags(irq, 0);
+#endif
+                set_irq_chip_and_handler(irq, NULL, NULL);
+                set_irq_chip_data(irq, NULL);
+        }
+}
+
+static struct resource ab8500_gpadc_resources[] = {
+        {
+                .name   = "HW_CONV_END",
+                .start  = AB8500_INT_GP_HW_ADC_CONV_END,
+                .end    = AB8500_INT_GP_HW_ADC_CONV_END,
+                .flags  = IORESOURCE_IRQ,
+        },
+        {
+                .name   = "SW_CONV_END",
+                .start  = AB8500_INT_GP_SW_ADC_CONV_END,
+                .end    = AB8500_INT_GP_SW_ADC_CONV_END,
+                .flags  = IORESOURCE_IRQ,
+        },
+};
+
+static struct resource ab8500_rtc_resources[] = {
+        {
+                .name   = "60S",
+                .start  = AB8500_INT_RTC_60S,
+                .end    = AB8500_INT_RTC_60S,
+                .flags  = IORESOURCE_IRQ,
+        },
+        {
+                .name   = "ALARM",
+                .start  = AB8500_INT_RTC_ALARM,
+                .end    = AB8500_INT_RTC_ALARM,
+                .flags  = IORESOURCE_IRQ,
+        },
+};
+
+static struct mfd_cell ab8500_devs[] = {
+        {
+                .name = "ab8500-gpadc",
+                .num_resources = ARRAY_SIZE(ab8500_gpadc_resources),
+                .resources = ab8500_gpadc_resources,
+        },
+        {
+                .name = "ab8500-rtc",
+                .num_resources = ARRAY_SIZE(ab8500_rtc_resources),
+                .resources = ab8500_rtc_resources,
+        },
+        { .name = "ab8500-charger", },
+        { .name = "ab8500-audio", },
+        { .name = "ab8500-usb", },
+        { .name = "ab8500-pwm", },
+};
+
+int __devinit ab8500_init(struct ab8500 *ab8500)
+{
+        struct ab8500_platform_data *plat = dev_get_platdata(ab8500->dev);
+        int ret;
+        int i;
+
+        if (plat)
+                ab8500->irq_base = plat->irq_base;
+
+        mutex_init(&ab8500->lock);
+        mutex_init(&ab8500->irq_lock);
+
+        ret = ab8500_read(ab8500, AB8500_REV_REG);
+        if (ret < 0)
+                return ret;
+
+        /*
+         * 0x0 - Early Drop
+         * 0x10 - Cut 1.0
+         * 0x11 - Cut 1.1
+         */
+        if (ret == 0x0 || ret == 0x10 || ret == 0x11) {
+                ab8500->revision = ret;
+                dev_info(ab8500->dev, "detected chip, revision: %#x\n", ret);
+        } else {
+                dev_err(ab8500->dev, "unknown chip, revision: %#x\n", ret);
+                return -EINVAL;
+        }
+
+        if (plat && plat->init)
+                plat->init(ab8500);
+
+        /* Clear and mask all interrupts */
+        for (i = 0; i < 10; i++) {
+                ab8500_read(ab8500, AB8500_IT_LATCH1_REG + i);
+                ab8500_write(ab8500, AB8500_IT_MASK1_REG + i, 0xff);
+        }
+
+        for (i = 18; i < 24; i++) {
+                ab8500_read(ab8500, AB8500_IT_LATCH1_REG + i);
+                ab8500_write(ab8500, AB8500_IT_MASK1_REG + i, 0xff);
+        }
+
+        for (i = 0; i < AB8500_NUM_IRQ_REGS; i++)
+                ab8500->mask[i] = ab8500->oldmask[i] = 0xff;
+
+        if (ab8500->irq_base) {
+                ret = ab8500_irq_init(ab8500);
+                if (ret)
+                        return ret;
+
+                ret = request_threaded_irq(ab8500->irq, NULL, ab8500_irq,
+                                           IRQF_ONESHOT, "ab8500", ab8500);
+                if (ret)
+                        goto out_removeirq;
+        }
+
+        ret = mfd_add_devices(ab8500->dev, -1, ab8500_devs,
+                              ARRAY_SIZE(ab8500_devs), NULL,
+                              ab8500->irq_base);
+        if (ret)
+                goto out_freeirq;
+
+        return ret;
+
+out_freeirq:
+        if (ab8500->irq_base) {
+                free_irq(ab8500->irq, ab8500);
+out_removeirq:
+                ab8500_irq_remove(ab8500);
+        }
+        return ret;
+}
+
+int __devexit ab8500_exit(struct ab8500 *ab8500)
+{
+        mfd_remove_devices(ab8500->dev);
+        if (ab8500->irq_base) {
+                free_irq(ab8500->irq, ab8500);
+                ab8500_irq_remove(ab8500);
+        }
+
+        return 0;
+}
+
+MODULE_AUTHOR("Srinidhi Kasagar, Rabin Vincent");
+MODULE_DESCRIPTION("AB8500 MFD core");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/ab8500-spi.c b/drivers/mfd/ab8500-spi.c
new file mode 100644
index 000000000000..b81d4f768ef6
--- /dev/null
+++ b/drivers/mfd/ab8500-spi.c
@@ -0,0 +1,133 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Srinidhi Kasagar <srinidhi.kasagar@stericsson.com>
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <linux/mfd/ab8500.h>
+
+/*
+ * This funtion writes to any AB8500 registers using
+ * SPI protocol &  before it writes it packs the data
+ * in the below 24 bit frame format
+ *
+ *       *|------------------------------------|
+ *       *| 23|22...18|17.......10|9|8|7......0|
+ *       *| r/w  bank       adr          data  |
+ *       * ------------------------------------
+ *
+ * This function shouldn't be called from interrupt
+ * context
+ */
+static int ab8500_spi_write(struct ab8500 *ab8500, u16 addr, u8 data)
+{
+        struct spi_device *spi = container_of(ab8500->dev, struct spi_device,
+                                              dev);
+        unsigned long spi_data = addr << 10 | data;
+        struct spi_transfer xfer;
+        struct spi_message msg;
+
+        ab8500->tx_buf[0] = spi_data;
+        ab8500->rx_buf[0] = 0;
+
+        xfer.tx_buf     = ab8500->tx_buf;
+        xfer.rx_buf     = NULL;
+        xfer.len        = sizeof(unsigned long);
+
+        spi_message_init(&msg);
+        spi_message_add_tail(&xfer, &msg);
+
+        return spi_sync(spi, &msg);
+}
+
+static int ab8500_spi_read(struct ab8500 *ab8500, u16 addr)
+{
+        struct spi_device *spi = container_of(ab8500->dev, struct spi_device,
+                                              dev);
+        unsigned long spi_data = 1 << 23 | addr << 10;
+        struct spi_transfer xfer;
+        struct spi_message msg;
+        int ret;
+
+        ab8500->tx_buf[0] = spi_data;
+        ab8500->rx_buf[0] = 0;
+
+        xfer.tx_buf     = ab8500->tx_buf;
+        xfer.rx_buf     = ab8500->rx_buf;
+        xfer.len        = sizeof(unsigned long);
+
+        spi_message_init(&msg);
+        spi_message_add_tail(&xfer, &msg);
+
+        ret = spi_sync(spi, &msg);
+        if (!ret)
+                ret = ab8500->rx_buf[0];
+
+        return ret;
+}
+
+static int __devinit ab8500_spi_probe(struct spi_device *spi)
+{
+        struct ab8500 *ab8500;
+        int ret;
+
+        ab8500 = kzalloc(sizeof *ab8500, GFP_KERNEL);
+        if (!ab8500)
+                return -ENOMEM;
+
+        ab8500->dev = &spi->dev;
+        ab8500->irq = spi->irq;
+
+        ab8500->read = ab8500_spi_read;
+        ab8500->write = ab8500_spi_write;
+
+        spi_set_drvdata(spi, ab8500);
+
+        ret = ab8500_init(ab8500);
+        if (ret)
+                kfree(ab8500);
+
+        return ret;
+}
+
+static int __devexit ab8500_spi_remove(struct spi_device *spi)
+{
+        struct ab8500 *ab8500 = spi_get_drvdata(spi);
+
+        ab8500_exit(ab8500);
+        kfree(ab8500);
+
+        return 0;
+}
+
+static struct spi_driver ab8500_spi_driver = {
+        .driver = {
+                .name = "ab8500",
+                .owner = THIS_MODULE,
+        },
+        .probe  = ab8500_spi_probe,
+        .remove = __devexit_p(ab8500_spi_remove)
+};
+
+static int __init ab8500_spi_init(void)
+{
+        return spi_register_driver(&ab8500_spi_driver);
+}
+subsys_initcall(ab8500_spi_init);
+
+static void __exit ab8500_spi_exit(void)
+{
+        spi_unregister_driver(&ab8500_spi_driver);
+}
+module_exit(ab8500_spi_exit);
+
+MODULE_AUTHOR("Srinidhi KASAGAR <srinidhi.kasagar@stericsson.com");
+MODULE_DESCRIPTION("AB8500 SPI");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/abx500-core.c b/drivers/mfd/abx500-core.c
new file mode 100644
index 000000000000..3b3b97ec32a7
--- /dev/null
+++ b/drivers/mfd/abx500-core.c
@@ -0,0 +1,157 @@
+/*
+ * Copyright (C) 2007-2010 ST-Ericsson
+ * License terms: GNU General Public License (GPL) version 2
+ * Register access functions for the ABX500 Mixed Signal IC family.
+ * Author: Mattias Wallin <mattias.wallin@stericsson.com>
+ */
+
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/mfd/abx500.h>
+
+static LIST_HEAD(abx500_list);
+
+struct abx500_device_entry {
+        struct list_head list;
+        struct abx500_ops ops;
+        struct device *dev;
+};
+
+static void lookup_ops(struct device *dev, struct abx500_ops **ops)
+{
+        struct abx500_device_entry *dev_entry;
+
+        *ops = NULL;
+        list_for_each_entry(dev_entry, &abx500_list, list) {
+                if (dev_entry->dev == dev) {
+                        *ops = &dev_entry->ops;
+                        return;
+                }
+        }
+}
+
+int abx500_register_ops(struct device *dev, struct abx500_ops *ops)
+{
+        struct abx500_device_entry *dev_entry;
+
+        dev_entry = kzalloc(sizeof(struct abx500_device_entry), GFP_KERNEL);
+        if (IS_ERR(dev_entry)) {
+                dev_err(dev, "register_ops kzalloc failed");
+                return -ENOMEM;
+        }
+        dev_entry->dev = dev;
+        memcpy(&dev_entry->ops, ops, sizeof(struct abx500_ops));
+
+        list_add_tail(&dev_entry->list, &abx500_list);
+        return 0;
+}
+EXPORT_SYMBOL(abx500_register_ops);
+
+void abx500_remove_ops(struct device *dev)
+{
+        struct abx500_device_entry *dev_entry, *tmp;
+
+        list_for_each_entry_safe(dev_entry, tmp, &abx500_list, list)
+        {
+                if (dev_entry->dev == dev) {
+                        list_del(&dev_entry->list);
+                        kfree(dev_entry);
+                }
+        }
+}
+EXPORT_SYMBOL(abx500_remove_ops);
+
+int abx500_set_register_interruptible(struct device *dev, u8 bank, u8 reg,
+        u8 value)
+{
+        struct abx500_ops *ops;
+
+        lookup_ops(dev->parent, &ops);
+        if ((ops != NULL) && (ops->set_register != NULL))
+                return ops->set_register(dev, bank, reg, value);
+        else
+                return -ENOTSUPP;
+}
+EXPORT_SYMBOL(abx500_set_register_interruptible);
+
+int abx500_get_register_interruptible(struct device *dev, u8 bank, u8 reg,
+        u8 *value)
+{
+        struct abx500_ops *ops;
+
+        lookup_ops(dev->parent, &ops);
+        if ((ops != NULL) && (ops->get_register != NULL))
+                return ops->get_register(dev, bank, reg, value);
+        else
+                return -ENOTSUPP;
+}
+EXPORT_SYMBOL(abx500_get_register_interruptible);
+
+int abx500_get_register_page_interruptible(struct device *dev, u8 bank,
+        u8 first_reg, u8 *regvals, u8 numregs)
+{
+        struct abx500_ops *ops;
+
+        lookup_ops(dev->parent, &ops);
+        if ((ops != NULL) && (ops->get_register_page != NULL))
+                return ops->get_register_page(dev, bank,
+                        first_reg, regvals, numregs);
+        else
+                return -ENOTSUPP;
+}
+EXPORT_SYMBOL(abx500_get_register_page_interruptible);
+
+int abx500_mask_and_set_register_interruptible(struct device *dev, u8 bank,
+        u8 reg, u8 bitmask, u8 bitvalues)
+{
+        struct abx500_ops *ops;
+
+        lookup_ops(dev->parent, &ops);
+        if ((ops != NULL) && (ops->mask_and_set_register != NULL))
+                return ops->mask_and_set_register(dev, bank,
+                        reg, bitmask, bitvalues);
+        else
+                return -ENOTSUPP;
+}
+EXPORT_SYMBOL(abx500_mask_and_set_register_interruptible);
+
+int abx500_get_chip_id(struct device *dev)
+{
+        struct abx500_ops *ops;
+
+        lookup_ops(dev->parent, &ops);
+        if ((ops != NULL) && (ops->get_chip_id != NULL))
+                return ops->get_chip_id(dev);
+        else
+                return -ENOTSUPP;
+}
+EXPORT_SYMBOL(abx500_get_chip_id);
+
+int abx500_event_registers_startup_state_get(struct device *dev, u8 *event)
+{
+        struct abx500_ops *ops;
+
+        lookup_ops(dev->parent, &ops);
+        if ((ops != NULL) && (ops->event_registers_startup_state_get != NULL))
+                return ops->event_registers_startup_state_get(dev, event);
+        else
+                return -ENOTSUPP;
+}
+EXPORT_SYMBOL(abx500_event_registers_startup_state_get);
+
+int abx500_startup_irq_enabled(struct device *dev, unsigned int irq)
+{
+        struct abx500_ops *ops;
+
+        lookup_ops(dev->parent, &ops);
+        if ((ops != NULL) && (ops->startup_irq_enabled != NULL))
+                return ops->startup_irq_enabled(dev, irq);
+        else
+                return -ENOTSUPP;
+}
+EXPORT_SYMBOL(abx500_startup_irq_enabled);
+
+MODULE_AUTHOR("Mattias Wallin <mattias.wallin@stericsson.com>");
+MODULE_DESCRIPTION("ABX500 core driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/adp5520.c b/drivers/mfd/adp5520.c
index 005532865654..3122139b4300 100644
--- a/drivers/mfd/adp5520.c
+++ b/drivers/mfd/adp5520.c
@@ -302,7 +302,6 @@ out_free_irq:
                 free_irq(chip->irq, chip);
 
 out_free_chip:
-        i2c_set_clientdata(client, NULL);
         kfree(chip);
 
         return ret;
@@ -317,7 +316,6 @@ static int __devexit adp5520_remove(struct i2c_client *client)
 
         adp5520_remove_subdevs(chip);
         adp5520_write(chip->dev, ADP5520_MODE_STATUS, 0);
-        i2c_set_clientdata(client, NULL);
         kfree(chip);
         return 0;
 }
diff --git a/drivers/mfd/da903x.c b/drivers/mfd/da903x.c
index 67181b147ab3..c07aece900fb 100644
--- a/drivers/mfd/da903x.c
+++ b/drivers/mfd/da903x.c
@@ -534,7 +534,6 @@ static int __devinit da903x_probe(struct i2c_client *client,
 out_free_irq:
         free_irq(client->irq, chip);
 out_free_chip:
-        i2c_set_clientdata(client, NULL);
         kfree(chip);
         return ret;
 }
diff --git a/drivers/mfd/janz-cmodio.c b/drivers/mfd/janz-cmodio.c
new file mode 100644
index 000000000000..9ed630799acc
--- /dev/null
+++ b/drivers/mfd/janz-cmodio.c
@@ -0,0 +1,304 @@
+/*
+ * Janz CMOD-IO MODULbus Carrier Board PCI Driver
+ *
+ * Copyright (c) 2010 Ira W. Snyder <iws@ovro.caltech.edu>
+ *
+ * Lots of inspiration and code was copied from drivers/mfd/sm501.c
+ *
+ * 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/init.h>
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/core.h>
+
+#include <linux/mfd/janz.h>
+
+#define DRV_NAME "janz-cmodio"
+
+/* Size of each MODULbus module in PCI BAR4 */
+#define CMODIO_MODULBUS_SIZE    0x200
+
+/* Maximum number of MODULbus modules on a CMOD-IO carrier board */
+#define CMODIO_MAX_MODULES      4
+
+/* Module Parameters */
+static unsigned int num_modules = CMODIO_MAX_MODULES;
+static unsigned char *modules[CMODIO_MAX_MODULES] = {
+        "empty", "empty", "empty", "empty",
+};
+
+module_param_array(modules, charp, &num_modules, S_IRUGO);
+MODULE_PARM_DESC(modules, "MODULbus modules attached to the carrier board");
+
+/* Unique Device Id */
+static unsigned int cmodio_id;
+
+struct cmodio_device {
+        /* Parent PCI device */
+        struct pci_dev *pdev;
+
+        /* PLX control registers */
+        struct janz_cmodio_onboard_regs __iomem *ctrl;
+
+        /* hex switch position */
+        u8 hex;
+
+        /* mfd-core API */
+        struct mfd_cell cells[CMODIO_MAX_MODULES];
+        struct resource resources[3 * CMODIO_MAX_MODULES];
+        struct janz_platform_data pdata[CMODIO_MAX_MODULES];
+};
+
+/*
+ * Subdevices using the mfd-core API
+ */
+
+static int __devinit cmodio_setup_subdevice(struct cmodio_device *priv,
+                                            char *name, unsigned int devno,
+                                            unsigned int modno)
+{
+        struct janz_platform_data *pdata;
+        struct mfd_cell *cell;
+        struct resource *res;
+        struct pci_dev *pci;
+
+        pci = priv->pdev;
+        cell = &priv->cells[devno];
+        res = &priv->resources[devno * 3];
+        pdata = &priv->pdata[devno];
+
+        cell->name = name;
+        cell->resources = res;
+        cell->num_resources = 3;
+
+        /* Setup the subdevice ID -- must be unique */
+        cell->id = cmodio_id++;
+
+        /* Add platform data */
+        pdata->modno = modno;
+        cell->platform_data = pdata;
+        cell->data_size = sizeof(*pdata);
+
+        /* MODULbus registers -- PCI BAR3 is big-endian MODULbus access */
+        res->flags = IORESOURCE_MEM;
+        res->parent = &pci->resource[3];
+        res->start = pci->resource[3].start + (CMODIO_MODULBUS_SIZE * modno);
+        res->end = res->start + CMODIO_MODULBUS_SIZE - 1;
+        res++;
+
+        /* PLX Control Registers -- PCI BAR4 is interrupt and other registers */
+        res->flags = IORESOURCE_MEM;
+        res->parent = &pci->resource[4];
+        res->start = pci->resource[4].start;
+        res->end = pci->resource[4].end;
+        res++;
+
+        /*
+         * IRQ
+         *
+         * The start and end fields are used as an offset to the irq_base
+         * parameter passed into the mfd_add_devices() function call. All
+         * devices share the same IRQ.
+         */
+        res->flags = IORESOURCE_IRQ;
+        res->parent = NULL;
+        res->start = 0;
+        res->end = 0;
+        res++;
+
+        return 0;
+}
+
+/* Probe each submodule using kernel parameters */
+static int __devinit cmodio_probe_submodules(struct cmodio_device *priv)
+{
+        struct pci_dev *pdev = priv->pdev;
+        unsigned int num_probed = 0;
+        char *name;
+        int i;
+
+        for (i = 0; i < num_modules; i++) {
+                name = modules[i];
+                if (!strcmp(name, "") || !strcmp(name, "empty"))
+                        continue;
+
+                dev_dbg(&priv->pdev->dev, "MODULbus %d: name %s\n", i, name);
+                cmodio_setup_subdevice(priv, name, num_probed, i);
+                num_probed++;
+        }
+
+        /* print an error message if no modules were probed */
+        if (num_probed == 0) {
+                dev_err(&priv->pdev->dev, "no MODULbus modules specified, "
+                                          "please set the ``modules'' kernel "
+                                          "parameter according to your "
+                                          "hardware configuration\n");
+                return -ENODEV;
+        }
+
+        return mfd_add_devices(&pdev->dev, 0, priv->cells,
+                               num_probed, NULL, pdev->irq);
+}
+
+/*
+ * SYSFS Attributes
+ */
+
+static ssize_t mbus_show(struct device *dev, struct device_attribute *attr,
+                         char *buf)
+{
+        struct cmodio_device *priv = dev_get_drvdata(dev);
+
+        return snprintf(buf, PAGE_SIZE, "%x\n", priv->hex);
+}
+
+static DEVICE_ATTR(modulbus_number, S_IRUGO, mbus_show, NULL);
+
+static struct attribute *cmodio_sysfs_attrs[] = {
+        &dev_attr_modulbus_number.attr,
+        NULL,
+};
+
+static const struct attribute_group cmodio_sysfs_attr_group = {
+        .attrs = cmodio_sysfs_attrs,
+};
+
+/*
+ * PCI Driver
+ */
+
+static int __devinit cmodio_pci_probe(struct pci_dev *dev,
+                                      const struct pci_device_id *id)
+{
+        struct cmodio_device *priv;
+        int ret;
+
+        priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+        if (!priv) {
+                dev_err(&dev->dev, "unable to allocate private data\n");
+                ret = -ENOMEM;
+                goto out_return;
+        }
+
+        pci_set_drvdata(dev, priv);
+        priv->pdev = dev;
+
+        /* Hardware Initialization */
+        ret = pci_enable_device(dev);
+        if (ret) {
+                dev_err(&dev->dev, "unable to enable device\n");
+                goto out_free_priv;
+        }
+
+        pci_set_master(dev);
+        ret = pci_request_regions(dev, DRV_NAME);
+        if (ret) {
+                dev_err(&dev->dev, "unable to request regions\n");
+                goto out_pci_disable_device;
+        }
+
+        /* Onboard configuration registers */
+        priv->ctrl = pci_ioremap_bar(dev, 4);
+        if (!priv->ctrl) {
+                dev_err(&dev->dev, "unable to remap onboard regs\n");
+                ret = -ENOMEM;
+                goto out_pci_release_regions;
+        }
+
+        /* Read the hex switch on the carrier board */
+        priv->hex = ioread8(&priv->ctrl->int_enable);
+
+        /* Add the MODULbus number (hex switch value) to the device's sysfs */
+        ret = sysfs_create_group(&dev->dev.kobj, &cmodio_sysfs_attr_group);
+        if (ret) {
+                dev_err(&dev->dev, "unable to create sysfs attributes\n");
+                goto out_unmap_ctrl;
+        }
+
+        /*
+         * Disable all interrupt lines, each submodule will enable its
+         * own interrupt line if needed
+         */
+        iowrite8(0xf, &priv->ctrl->int_disable);
+
+        /* Register drivers for all submodules */
+        ret = cmodio_probe_submodules(priv);
+        if (ret) {
+                dev_err(&dev->dev, "unable to probe submodules\n");
+                goto out_sysfs_remove_group;
+        }
+
+        return 0;
+
+out_sysfs_remove_group:
+        sysfs_remove_group(&dev->dev.kobj, &cmodio_sysfs_attr_group);
+out_unmap_ctrl:
+        iounmap(priv->ctrl);
+out_pci_release_regions:
+        pci_release_regions(dev);
+out_pci_disable_device:
+        pci_disable_device(dev);
+out_free_priv:
+        kfree(priv);
+out_return:
+        return ret;
+}
+
+static void __devexit cmodio_pci_remove(struct pci_dev *dev)
+{
+        struct cmodio_device *priv = pci_get_drvdata(dev);
+
+        mfd_remove_devices(&dev->dev);
+        sysfs_remove_group(&dev->dev.kobj, &cmodio_sysfs_attr_group);
+        iounmap(priv->ctrl);
+        pci_release_regions(dev);
+        pci_disable_device(dev);
+        kfree(priv);
+}
+
+#define PCI_VENDOR_ID_JANZ              0x13c3
+
+/* The list of devices that this module will support */
+static DEFINE_PCI_DEVICE_TABLE(cmodio_pci_ids) = {
+        { PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9030, PCI_VENDOR_ID_JANZ, 0x0101 },
+        { PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_9050, PCI_VENDOR_ID_JANZ, 0x0100 },
+        { 0, }
+};
+MODULE_DEVICE_TABLE(pci, cmodio_pci_ids);
+
+static struct pci_driver cmodio_pci_driver = {
+        .name     = DRV_NAME,
+        .id_table = cmodio_pci_ids,
+        .probe    = cmodio_pci_probe,
+        .remove   = __devexit_p(cmodio_pci_remove),
+};
+
+/*
+ * Module Init / Exit
+ */
+
+static int __init cmodio_init(void)
+{
+        return pci_register_driver(&cmodio_pci_driver);
+}
+
+static void __exit cmodio_exit(void)
+{
+        pci_unregister_driver(&cmodio_pci_driver);
+}
+
+MODULE_AUTHOR("Ira W. Snyder <iws@ovro.caltech.edu>");
+MODULE_DESCRIPTION("Janz CMOD-IO PCI MODULbus Carrier Board Driver");
+MODULE_LICENSE("GPL");
+
+module_init(cmodio_init);
+module_exit(cmodio_exit);
diff --git a/drivers/mfd/max8925-core.c b/drivers/mfd/max8925-core.c
index 85d63c04749b..f621bcea3d02 100644
--- a/drivers/mfd/max8925-core.c
+++ b/drivers/mfd/max8925-core.c
@@ -508,7 +508,7 @@ static int max8925_irq_init(struct max8925_chip *chip, int irq,
         max8925_reg_read(chip->i2c, MAX8925_ON_OFF_IRQ2);
         max8925_reg_read(chip->rtc, MAX8925_RTC_IRQ);
         max8925_reg_read(chip->adc, MAX8925_TSC_IRQ);
-        /* mask all interrupts */
+        /* mask all interrupts except for TSC */
         max8925_reg_write(chip->rtc, MAX8925_ALARM0_CNTL, 0);
         max8925_reg_write(chip->rtc, MAX8925_ALARM1_CNTL, 0);
         max8925_reg_write(chip->i2c, MAX8925_CHG_IRQ1_MASK, 0xff);
@@ -516,7 +516,6 @@ static int max8925_irq_init(struct max8925_chip *chip, int irq,
         max8925_reg_write(chip->i2c, MAX8925_ON_OFF_IRQ1_MASK, 0xff);
         max8925_reg_write(chip->i2c, MAX8925_ON_OFF_IRQ2_MASK, 0xff);
         max8925_reg_write(chip->rtc, MAX8925_RTC_IRQ_MASK, 0xff);
-        max8925_reg_write(chip->adc, MAX8925_TSC_IRQ_MASK, 0xff);
 
         mutex_init(&chip->irq_lock);
         chip->core_irq = irq;
@@ -547,7 +546,11 @@ static int max8925_irq_init(struct max8925_chip *chip, int irq,
                 dev_err(chip->dev, "Failed to request core IRQ: %d\n", ret);
                 chip->core_irq = 0;
         }
+
 tsc_irq:
+        /* mask TSC interrupt */
+        max8925_reg_write(chip->adc, MAX8925_TSC_IRQ_MASK, 0x0f);
+
         if (!pdata->tsc_irq) {
                 dev_warn(chip->dev, "No interrupt support on TSC IRQ\n");
                 return 0;
diff --git a/drivers/mfd/max8925-i2c.c b/drivers/mfd/max8925-i2c.c
index d9fd8785da4d..0219115e00c7 100644
--- a/drivers/mfd/max8925-i2c.c
+++ b/drivers/mfd/max8925-i2c.c
@@ -173,9 +173,6 @@ static int __devexit max8925_remove(struct i2c_client *client)
         max8925_device_exit(chip);
         i2c_unregister_device(chip->adc);
         i2c_unregister_device(chip->rtc);
-        i2c_set_clientdata(chip->adc, NULL);
-        i2c_set_clientdata(chip->rtc, NULL);
-        i2c_set_clientdata(chip->i2c, NULL);
         kfree(chip);
         return 0;
 }
diff --git a/drivers/mfd/mc13783-core.c b/drivers/mfd/mc13783-core.c
index 1f68ecadddc2..fecf38a4f025 100644
--- a/drivers/mfd/mc13783-core.c
+++ b/drivers/mfd/mc13783-core.c
@@ -679,6 +679,10 @@ err_revision:
         if (pdata->flags & MC13783_USE_TOUCHSCREEN)
                 mc13783_add_subdevice(mc13783, "mc13783-ts");
 
+        if (pdata->flags & MC13783_USE_LED)
+                mc13783_add_subdevice_pdata(mc13783, "mc13783-led",
+                                        pdata->leds, sizeof(*pdata->leds));
+
         return 0;
 }
 
diff --git a/drivers/mfd/menelaus.c b/drivers/mfd/menelaus.c
index a94b131a18ef..a3fb4bcb9889 100644
--- a/drivers/mfd/menelaus.c
+++ b/drivers/mfd/menelaus.c
@@ -1238,7 +1238,6 @@ static int __exit menelaus_remove(struct i2c_client *client)
 
         free_irq(client->irq, menelaus);
         kfree(menelaus);
-        i2c_set_clientdata(client, NULL);
         the_menelaus = NULL;
         return 0;
 }
diff --git a/drivers/mfd/mfd-core.c b/drivers/mfd/mfd-core.c
index 8ffbb7a85a7e..7dd76bceaae8 100644
--- a/drivers/mfd/mfd-core.c
+++ b/drivers/mfd/mfd-core.c
@@ -48,7 +48,7 @@ static int mfd_add_device(struct device *parent, int id,
                 res[r].flags = cell->resources[r].flags;
 
                 /* Find out base to use */
-                if (cell->resources[r].flags & IORESOURCE_MEM) {
+                if ((cell->resources[r].flags & IORESOURCE_MEM) && mem_base) {
                         res[r].parent = mem_base;
                         res[r].start = mem_base->start +
                                 cell->resources[r].start;
diff --git a/drivers/mfd/pcf50633-adc.c b/drivers/mfd/pcf50633-adc.c
index fe8f922f6654..aed0d2a9b032 100644
--- a/drivers/mfd/pcf50633-adc.c
+++ b/drivers/mfd/pcf50633-adc.c
@@ -30,13 +30,13 @@
 struct pcf50633_adc_request {
         int mux;
         int avg;
-        int result;
         void (*callback)(struct pcf50633 *, void *, int);
         void *callback_param;
+};
 
-        /* Used in case of sync requests */
+struct pcf50633_adc_sync_request {
+        int result;
         struct completion completion;
-
 };
 
 #define PCF50633_MAX_ADC_FIFO_DEPTH 8
@@ -109,10 +109,10 @@ adc_enqueue_request(struct pcf50633 *pcf, struct pcf50633_adc_request *req)
         return 0;
 }
 
-static void
-pcf50633_adc_sync_read_callback(struct pcf50633 *pcf, void *param, int result)
+static void pcf50633_adc_sync_read_callback(struct pcf50633 *pcf, void *param,
+        int result)
 {
-        struct pcf50633_adc_request *req = param;
+        struct pcf50633_adc_sync_request *req = param;
 
         req->result = result;
         complete(&req->completion);
@@ -120,28 +120,19 @@ pcf50633_adc_sync_read_callback(struct pcf50633 *pcf, void *param, int result)
 
 int pcf50633_adc_sync_read(struct pcf50633 *pcf, int mux, int avg)
 {
-        struct pcf50633_adc_request *req;
-        int err;
+        struct pcf50633_adc_sync_request req;
+        int ret;
 
-        /* req is freed when the result is ready, in interrupt handler */
-        req = kzalloc(sizeof(*req), GFP_KERNEL);
-        if (!req)
-                return -ENOMEM;
-
-        req->mux = mux;
-        req->avg = avg;
-        req->callback =  pcf50633_adc_sync_read_callback;
-        req->callback_param = req;
+        init_completion(&req.completion);
 
-        init_completion(&req->completion);
-        err = adc_enqueue_request(pcf, req);
-        if (err)
-                return err;
+        ret = pcf50633_adc_async_read(pcf, mux, avg,
+                pcf50633_adc_sync_read_callback, &req);
+        if (ret)
+                return ret;
 
-        wait_for_completion(&req->completion);
+        wait_for_completion(&req.completion);
 
-        /* FIXME by this time req might be already freed */
-        return req->result;
+        return req.result;
 }
 EXPORT_SYMBOL_GPL(pcf50633_adc_sync_read);
 
diff --git a/drivers/mfd/pcf50633-core.c b/drivers/mfd/pcf50633-core.c
index 63a614d696c1..23e585527285 100644
--- a/drivers/mfd/pcf50633-core.c
+++ b/drivers/mfd/pcf50633-core.c
@@ -21,16 +21,16 @@
 #include <linux/workqueue.h>
 #include <linux/platform_device.h>
 #include <linux/i2c.h>
-#include <linux/irq.h>
 #include <linux/slab.h>
 
 #include <linux/mfd/pcf50633/core.h>
 
-/* Two MBCS registers used during cold start */
-#define PCF50633_REG_MBCS1              0x4b
-#define PCF50633_REG_MBCS2              0x4c
-#define PCF50633_MBCS1_USBPRES          0x01
-#define PCF50633_MBCS1_ADAPTPRES        0x01
+int pcf50633_irq_init(struct pcf50633 *pcf, int irq);
+void pcf50633_irq_free(struct pcf50633 *pcf);
+#ifdef CONFIG_PM
+int pcf50633_irq_suspend(struct pcf50633 *pcf);
+int pcf50633_irq_resume(struct pcf50633 *pcf);
+#endif
 
 static int __pcf50633_read(struct pcf50633 *pcf, u8 reg, int num, u8 *data)
 {
@@ -215,244 +215,6 @@ static struct attribute_group pcf_attr_group = {
         .attrs  = pcf_sysfs_entries,
 };
 
-int pcf50633_register_irq(struct pcf50633 *pcf, int irq,
-                        void (*handler) (int, void *), void *data)
-{
-        if (irq < 0 || irq > PCF50633_NUM_IRQ || !handler)
-                return -EINVAL;
-
-        if (WARN_ON(pcf->irq_handler[irq].handler))
-                return -EBUSY;
-
-        mutex_lock(&pcf->lock);
-        pcf->irq_handler[irq].handler = handler;
-        pcf->irq_handler[irq].data = data;
-        mutex_unlock(&pcf->lock);
-
-        return 0;
-}
-EXPORT_SYMBOL_GPL(pcf50633_register_irq);
-
-int pcf50633_free_irq(struct pcf50633 *pcf, int irq)
-{
-        if (irq < 0 || irq > PCF50633_NUM_IRQ)
-                return -EINVAL;
-
-        mutex_lock(&pcf->lock);
-        pcf->irq_handler[irq].handler = NULL;
-        mutex_unlock(&pcf->lock);
-
-        return 0;
-}
-EXPORT_SYMBOL_GPL(pcf50633_free_irq);
-
-static int __pcf50633_irq_mask_set(struct pcf50633 *pcf, int irq, u8 mask)
-{
-        u8 reg, bits, tmp;
-        int ret = 0, idx;
-
-        idx = irq >> 3;
-        reg =  PCF50633_REG_INT1M + idx;
-        bits = 1 << (irq & 0x07);
-
-        mutex_lock(&pcf->lock);
-
-        if (mask) {
-                ret = __pcf50633_read(pcf, reg, 1, &tmp);
-                if (ret < 0)
-                        goto out;
-
-                tmp |= bits;
-
-                ret = __pcf50633_write(pcf, reg, 1, &tmp);
-                if (ret < 0)
-                        goto out;
-
-                pcf->mask_regs[idx] &= ~bits;
-                pcf->mask_regs[idx] |= bits;
-        } else {
-                ret = __pcf50633_read(pcf, reg, 1, &tmp);
-                if (ret < 0)
-                        goto out;
-
-                tmp &= ~bits;
-
-                ret = __pcf50633_write(pcf, reg, 1, &tmp);
-                if (ret < 0)
-                        goto out;
-
-                pcf->mask_regs[idx] &= ~bits;
-        }
-out:
-        mutex_unlock(&pcf->lock);
-
-        return ret;
-}
-
-int pcf50633_irq_mask(struct pcf50633 *pcf, int irq)
-{
-        dev_dbg(pcf->dev, "Masking IRQ %d\n", irq);
-
-        return __pcf50633_irq_mask_set(pcf, irq, 1);
-}
-EXPORT_SYMBOL_GPL(pcf50633_irq_mask);
-
-int pcf50633_irq_unmask(struct pcf50633 *pcf, int irq)
-{
-        dev_dbg(pcf->dev, "Unmasking IRQ %d\n", irq);
-
-        return __pcf50633_irq_mask_set(pcf, irq, 0);
-}
-EXPORT_SYMBOL_GPL(pcf50633_irq_unmask);
-
-int pcf50633_irq_mask_get(struct pcf50633 *pcf, int irq)
-{
-        u8 reg, bits;
-
-        reg =  irq >> 3;
-        bits = 1 << (irq & 0x07);
-
-        return pcf->mask_regs[reg] & bits;
-}
-EXPORT_SYMBOL_GPL(pcf50633_irq_mask_get);
-
-static void pcf50633_irq_call_handler(struct pcf50633 *pcf, int irq)
-{
-        if (pcf->irq_handler[irq].handler)
-                pcf->irq_handler[irq].handler(irq, pcf->irq_handler[irq].data);
-}
-
-/* Maximum amount of time ONKEY is held before emergency action is taken */
-#define PCF50633_ONKEY1S_TIMEOUT 8
-
-static void pcf50633_irq_worker(struct work_struct *work)
-{
-        struct pcf50633 *pcf;
-        int ret, i, j;
-        u8 pcf_int[5], chgstat;
-
-        pcf = container_of(work, struct pcf50633, irq_work);
-
-        /* Read the 5 INT regs in one transaction */
-        ret = pcf50633_read_block(pcf, PCF50633_REG_INT1,
-                                                ARRAY_SIZE(pcf_int), pcf_int);
-        if (ret != ARRAY_SIZE(pcf_int)) {
-                dev_err(pcf->dev, "Error reading INT registers\n");
-
-                /*
-                 * If this doesn't ACK the interrupt to the chip, we'll be
-                 * called once again as we're level triggered.
-                 */
-                goto out;
-        }
-
-        /* defeat 8s death from lowsys on A5 */
-        pcf50633_reg_write(pcf, PCF50633_REG_OOCSHDWN,  0x04);
-
-        /* We immediately read the usb and adapter status. We thus make sure
-         * only of USBINS/USBREM IRQ handlers are called */
-        if (pcf_int[0] & (PCF50633_INT1_USBINS | PCF50633_INT1_USBREM)) {
-                chgstat = pcf50633_reg_read(pcf, PCF50633_REG_MBCS2);
-                if (chgstat & (0x3 << 4))
-                        pcf_int[0] &= ~(1 << PCF50633_INT1_USBREM);
-                else
-                        pcf_int[0] &= ~(1 << PCF50633_INT1_USBINS);
-        }
-
-        /* Make sure only one of ADPINS or ADPREM is set */
-        if (pcf_int[0] & (PCF50633_INT1_ADPINS | PCF50633_INT1_ADPREM)) {
-                chgstat = pcf50633_reg_read(pcf, PCF50633_REG_MBCS2);
-                if (chgstat & (0x3 << 4))
-                        pcf_int[0] &= ~(1 << PCF50633_INT1_ADPREM);
-                else
-                        pcf_int[0] &= ~(1 << PCF50633_INT1_ADPINS);
-        }
-
-        dev_dbg(pcf->dev, "INT1=0x%02x INT2=0x%02x INT3=0x%02x "
-                        "INT4=0x%02x INT5=0x%02x\n", pcf_int[0],
-                        pcf_int[1], pcf_int[2], pcf_int[3], pcf_int[4]);
-
-        /* Some revisions of the chip don't have a 8s standby mode on
-         * ONKEY1S press. We try to manually do it in such cases. */
-        if ((pcf_int[0] & PCF50633_INT1_SECOND) && pcf->onkey1s_held) {
-                dev_info(pcf->dev, "ONKEY1S held for %d secs\n",
-                                                        pcf->onkey1s_held);
-                if (pcf->onkey1s_held++ == PCF50633_ONKEY1S_TIMEOUT)
-                        if (pcf->pdata->force_shutdown)
-                                pcf->pdata->force_shutdown(pcf);
-        }
-
-        if (pcf_int[2] & PCF50633_INT3_ONKEY1S) {
-                dev_info(pcf->dev, "ONKEY1S held\n");
-                pcf->onkey1s_held = 1 ;
-
-                /* Unmask IRQ_SECOND */
-                pcf50633_reg_clear_bits(pcf, PCF50633_REG_INT1M,
-                                                PCF50633_INT1_SECOND);
-
-                /* Unmask IRQ_ONKEYR */
-                pcf50633_reg_clear_bits(pcf, PCF50633_REG_INT2M,
-                                                PCF50633_INT2_ONKEYR);
-        }
-
-        if ((pcf_int[1] & PCF50633_INT2_ONKEYR) && pcf->onkey1s_held) {
-                pcf->onkey1s_held = 0;
-
-                /* Mask SECOND and ONKEYR interrupts */
-                if (pcf->mask_regs[0] & PCF50633_INT1_SECOND)
-                        pcf50633_reg_set_bit_mask(pcf,
-                                        PCF50633_REG_INT1M,
-                                        PCF50633_INT1_SECOND,
-                                        PCF50633_INT1_SECOND);
-
-                if (pcf->mask_regs[1] & PCF50633_INT2_ONKEYR)
-                        pcf50633_reg_set_bit_mask(pcf,
-                                        PCF50633_REG_INT2M,
-                                        PCF50633_INT2_ONKEYR,
-                                        PCF50633_INT2_ONKEYR);
-        }
-
-        /* Have we just resumed ? */
-        if (pcf->is_suspended) {
-                pcf->is_suspended = 0;
-
-                /* Set the resume reason filtering out non resumers */
-                for (i = 0; i < ARRAY_SIZE(pcf_int); i++)
-                        pcf->resume_reason[i] = pcf_int[i] &
-                                                pcf->pdata->resumers[i];
-
-                /* Make sure we don't pass on any ONKEY events to
-                 * userspace now */
-                pcf_int[1] &= ~(PCF50633_INT2_ONKEYR | PCF50633_INT2_ONKEYF);
-        }
-
-        for (i = 0; i < ARRAY_SIZE(pcf_int); i++) {
-                /* Unset masked interrupts */
-                pcf_int[i] &= ~pcf->mask_regs[i];
-
-                for (j = 0; j < 8 ; j++)
-                        if (pcf_int[i] & (1 << j))
-                                pcf50633_irq_call_handler(pcf, (i * 8) + j);
-        }
-
-out:
-        put_device(pcf->dev);
-        enable_irq(pcf->irq);
-}
-
-static irqreturn_t pcf50633_irq(int irq, void *data)
-{
-        struct pcf50633 *pcf = data;
-
-        dev_dbg(pcf->dev, "pcf50633_irq\n");
-
-        get_device(pcf->dev);
-        disable_irq_nosync(pcf->irq);
-        queue_work(pcf->work_queue, &pcf->irq_work);
-
-        return IRQ_HANDLED;
-}
-
 static void
 pcf50633_client_dev_register(struct pcf50633 *pcf, const char *name,
                                                 struct platform_device **pdev)
@@ -479,70 +241,17 @@ pcf50633_client_dev_register(struct pcf50633 *pcf, const char *name,
 static int pcf50633_suspend(struct i2c_client *client, pm_message_t state)
 {
         struct pcf50633 *pcf;
-        int ret = 0, i;
-        u8 res[5];
-
         pcf = i2c_get_clientdata(client);
 
-        /* Make sure our interrupt handlers are not called
-         * henceforth */
-        disable_irq(pcf->irq);
-
-        /* Make sure that any running IRQ worker has quit */
-        cancel_work_sync(&pcf->irq_work);
-
-        /* Save the masks */
-        ret = pcf50633_read_block(pcf, PCF50633_REG_INT1M,
-                                ARRAY_SIZE(pcf->suspend_irq_masks),
-                                        pcf->suspend_irq_masks);
-        if (ret < 0) {
-                dev_err(pcf->dev, "error saving irq masks\n");
-                goto out;
-        }
-
-        /* Write wakeup irq masks */
-        for (i = 0; i < ARRAY_SIZE(res); i++)
-                res[i] = ~pcf->pdata->resumers[i];
-
-        ret = pcf50633_write_block(pcf, PCF50633_REG_INT1M,
-                                        ARRAY_SIZE(res), &res[0]);
-        if (ret < 0) {
-                dev_err(pcf->dev, "error writing wakeup irq masks\n");
-                goto out;
-        }
-
-        pcf->is_suspended = 1;
-
-out:
-        return ret;
+        return pcf50633_irq_suspend(pcf);
 }
 
 static int pcf50633_resume(struct i2c_client *client)
 {
         struct pcf50633 *pcf;
-        int ret;
-
         pcf = i2c_get_clientdata(client);
 
-        /* Write the saved mask registers */
-        ret = pcf50633_write_block(pcf, PCF50633_REG_INT1M,
-                                ARRAY_SIZE(pcf->suspend_irq_masks),
-                                        pcf->suspend_irq_masks);
-        if (ret < 0)
-                dev_err(pcf->dev, "Error restoring saved suspend masks\n");
-
-        /* Restore regulators' state */
-
-
-        get_device(pcf->dev);
-
-        /*
-         * Clear any pending interrupts and set resume reason if any.
-         * This will leave with enable_irq()
-         */
-        pcf50633_irq_worker(&pcf->irq_work);
-
-        return 0;
+        return pcf50633_irq_resume(pcf);
 }
 #else
 #define pcf50633_suspend NULL
@@ -573,43 +282,19 @@ static int __devinit pcf50633_probe(struct i2c_client *client,
         i2c_set_clientdata(client, pcf);
         pcf->dev = &client->dev;
         pcf->i2c_client = client;
-        pcf->irq = client->irq;
-        pcf->work_queue = create_singlethread_workqueue("pcf50633");
-
-        if (!pcf->work_queue) {
-                dev_err(&client->dev, "Failed to alloc workqueue\n");
-                ret = -ENOMEM;
-                goto err_free;
-        }
-
-        INIT_WORK(&pcf->irq_work, pcf50633_irq_worker);
 
         version = pcf50633_reg_read(pcf, 0);
         variant = pcf50633_reg_read(pcf, 1);
         if (version < 0 || variant < 0) {
                 dev_err(pcf->dev, "Unable to probe pcf50633\n");
                 ret = -ENODEV;
-                goto err_destroy_workqueue;
+                goto err_free;
         }
 
         dev_info(pcf->dev, "Probed device version %d variant %d\n",
                                                         version, variant);
 
-        /* Enable all interrupts except RTC SECOND */
-        pcf->mask_regs[0] = 0x80;
-        pcf50633_reg_write(pcf, PCF50633_REG_INT1M, pcf->mask_regs[0]);
-        pcf50633_reg_write(pcf, PCF50633_REG_INT2M, 0x00);
-        pcf50633_reg_write(pcf, PCF50633_REG_INT3M, 0x00);
-        pcf50633_reg_write(pcf, PCF50633_REG_INT4M, 0x00);
-        pcf50633_reg_write(pcf, PCF50633_REG_INT5M, 0x00);
-
-        ret = request_irq(client->irq, pcf50633_irq,
-                                        IRQF_TRIGGER_LOW, "pcf50633", pcf);
-
-        if (ret) {
-                dev_err(pcf->dev, "Failed to request IRQ %d\n", ret);
-                goto err_destroy_workqueue;
-        }
+        pcf50633_irq_init(pcf, client->irq);
 
         /* Create sub devices */
         pcf50633_client_dev_register(pcf, "pcf50633-input",
@@ -620,6 +305,9 @@ static int __devinit pcf50633_probe(struct i2c_client *client,
                                                 &pcf->mbc_pdev);
         pcf50633_client_dev_register(pcf, "pcf50633-adc",
                                                 &pcf->adc_pdev);
+        pcf50633_client_dev_register(pcf, "pcf50633-backlight",
+                                                &pcf->bl_pdev);
+
 
         for (i = 0; i < PCF50633_NUM_REGULATORS; i++) {
                 struct platform_device *pdev;
@@ -638,10 +326,6 @@ static int __devinit pcf50633_probe(struct i2c_client *client,
                 platform_device_add(pdev);
         }
 
-        if (enable_irq_wake(client->irq) < 0)
-                dev_err(pcf->dev, "IRQ %u cannot be enabled as wake-up source"
-                        "in this hardware revision", client->irq);
-
         ret = sysfs_create_group(&client->dev.kobj, &pcf_attr_group);
         if (ret)
                 dev_err(pcf->dev, "error creating sysfs entries\n");
@@ -651,10 +335,7 @@ static int __devinit pcf50633_probe(struct i2c_client *client,
 
         return 0;
 
-err_destroy_workqueue:
-        destroy_workqueue(pcf->work_queue);
 err_free:
-        i2c_set_clientdata(client, NULL);
         kfree(pcf);
 
         return ret;
@@ -665,8 +346,7 @@ static int __devexit pcf50633_remove(struct i2c_client *client)
         struct pcf50633 *pcf = i2c_get_clientdata(client);
         int i;
 
-        free_irq(pcf->irq, pcf);
-        destroy_workqueue(pcf->work_queue);
+        pcf50633_irq_free(pcf);
 
         platform_device_unregister(pcf->input_pdev);
         platform_device_unregister(pcf->rtc_pdev);
diff --git a/drivers/mfd/pcf50633-irq.c b/drivers/mfd/pcf50633-irq.c
new file mode 100644
index 000000000000..1b0192f1efff
--- /dev/null
+++ b/drivers/mfd/pcf50633-irq.c
@@ -0,0 +1,318 @@
+/* NXP PCF50633 Power Management Unit (PMU) driver
+ *
+ * (C) 2006-2008 by Openmoko, Inc.
+ * Author: Harald Welte <laforge@openmoko.org>
+ *         Balaji Rao <balajirrao@openmoko.org>
+ * 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/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mutex.h>
+#include <linux/slab.h>
+
+#include <linux/mfd/pcf50633/core.h>
+
+/* Two MBCS registers used during cold start */
+#define PCF50633_REG_MBCS1              0x4b
+#define PCF50633_REG_MBCS2              0x4c
+#define PCF50633_MBCS1_USBPRES          0x01
+#define PCF50633_MBCS1_ADAPTPRES        0x01
+
+int pcf50633_register_irq(struct pcf50633 *pcf, int irq,
+                        void (*handler) (int, void *), void *data)
+{
+        if (irq < 0 || irq >= PCF50633_NUM_IRQ || !handler)
+                return -EINVAL;
+
+        if (WARN_ON(pcf->irq_handler[irq].handler))
+                return -EBUSY;
+
+        mutex_lock(&pcf->lock);
+        pcf->irq_handler[irq].handler = handler;
+        pcf->irq_handler[irq].data = data;
+        mutex_unlock(&pcf->lock);
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(pcf50633_register_irq);
+
+int pcf50633_free_irq(struct pcf50633 *pcf, int irq)
+{
+        if (irq < 0 || irq >= PCF50633_NUM_IRQ)
+                return -EINVAL;
+
+        mutex_lock(&pcf->lock);
+        pcf->irq_handler[irq].handler = NULL;
+        mutex_unlock(&pcf->lock);
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(pcf50633_free_irq);
+
+static int __pcf50633_irq_mask_set(struct pcf50633 *pcf, int irq, u8 mask)
+{
+        u8 reg, bit;
+        int ret = 0, idx;
+
+        idx = irq >> 3;
+        reg = PCF50633_REG_INT1M + idx;
+        bit = 1 << (irq & 0x07);
+
+        pcf50633_reg_set_bit_mask(pcf, reg, bit, mask ? bit : 0);
+
+        mutex_lock(&pcf->lock);
+
+        if (mask)
+                pcf->mask_regs[idx] |= bit;
+        else
+                pcf->mask_regs[idx] &= ~bit;
+
+        mutex_unlock(&pcf->lock);
+
+        return ret;
+}
+
+int pcf50633_irq_mask(struct pcf50633 *pcf, int irq)
+{
+        dev_dbg(pcf->dev, "Masking IRQ %d\n", irq);
+
+        return __pcf50633_irq_mask_set(pcf, irq, 1);
+}
+EXPORT_SYMBOL_GPL(pcf50633_irq_mask);
+
+int pcf50633_irq_unmask(struct pcf50633 *pcf, int irq)
+{
+        dev_dbg(pcf->dev, "Unmasking IRQ %d\n", irq);
+
+        return __pcf50633_irq_mask_set(pcf, irq, 0);
+}
+EXPORT_SYMBOL_GPL(pcf50633_irq_unmask);
+
+int pcf50633_irq_mask_get(struct pcf50633 *pcf, int irq)
+{
+        u8 reg, bits;
+
+        reg =  irq >> 3;
+        bits = 1 << (irq & 0x07);
+
+        return pcf->mask_regs[reg] & bits;
+}
+EXPORT_SYMBOL_GPL(pcf50633_irq_mask_get);
+
+static void pcf50633_irq_call_handler(struct pcf50633 *pcf, int irq)
+{
+        if (pcf->irq_handler[irq].handler)
+                pcf->irq_handler[irq].handler(irq, pcf->irq_handler[irq].data);
+}
+
+/* Maximum amount of time ONKEY is held before emergency action is taken */
+#define PCF50633_ONKEY1S_TIMEOUT 8
+
+static irqreturn_t pcf50633_irq(int irq, void *data)
+{
+        struct pcf50633 *pcf = data;
+        int ret, i, j;
+        u8 pcf_int[5], chgstat;
+
+        /* Read the 5 INT regs in one transaction */
+        ret = pcf50633_read_block(pcf, PCF50633_REG_INT1,
+                                                ARRAY_SIZE(pcf_int), pcf_int);
+        if (ret != ARRAY_SIZE(pcf_int)) {
+                dev_err(pcf->dev, "Error reading INT registers\n");
+
+                /*
+                 * If this doesn't ACK the interrupt to the chip, we'll be
+                 * called once again as we're level triggered.
+                 */
+                goto out;
+        }
+
+        /* defeat 8s death from lowsys on A5 */
+        pcf50633_reg_write(pcf, PCF50633_REG_OOCSHDWN,  0x04);
+
+        /* We immediately read the usb and adapter status. We thus make sure
+         * only of USBINS/USBREM IRQ handlers are called */
+        if (pcf_int[0] & (PCF50633_INT1_USBINS | PCF50633_INT1_USBREM)) {
+                chgstat = pcf50633_reg_read(pcf, PCF50633_REG_MBCS2);
+                if (chgstat & (0x3 << 4))
+                        pcf_int[0] &= ~PCF50633_INT1_USBREM;
+                else
+                        pcf_int[0] &= ~PCF50633_INT1_USBINS;
+        }
+
+        /* Make sure only one of ADPINS or ADPREM is set */
+        if (pcf_int[0] & (PCF50633_INT1_ADPINS | PCF50633_INT1_ADPREM)) {
+                chgstat = pcf50633_reg_read(pcf, PCF50633_REG_MBCS2);
+                if (chgstat & (0x3 << 4))
+                        pcf_int[0] &= ~PCF50633_INT1_ADPREM;
+                else
+                        pcf_int[0] &= ~PCF50633_INT1_ADPINS;
+        }
+
+        dev_dbg(pcf->dev, "INT1=0x%02x INT2=0x%02x INT3=0x%02x "
+                        "INT4=0x%02x INT5=0x%02x\n", pcf_int[0],
+                        pcf_int[1], pcf_int[2], pcf_int[3], pcf_int[4]);
+
+        /* Some revisions of the chip don't have a 8s standby mode on
+         * ONKEY1S press. We try to manually do it in such cases. */
+        if ((pcf_int[0] & PCF50633_INT1_SECOND) && pcf->onkey1s_held) {
+                dev_info(pcf->dev, "ONKEY1S held for %d secs\n",
+                                                        pcf->onkey1s_held);
+                if (pcf->onkey1s_held++ == PCF50633_ONKEY1S_TIMEOUT)
+                        if (pcf->pdata->force_shutdown)
+                                pcf->pdata->force_shutdown(pcf);
+        }
+
+        if (pcf_int[2] & PCF50633_INT3_ONKEY1S) {
+                dev_info(pcf->dev, "ONKEY1S held\n");
+                pcf->onkey1s_held = 1 ;
+
+                /* Unmask IRQ_SECOND */
+                pcf50633_reg_clear_bits(pcf, PCF50633_REG_INT1M,
+                                                PCF50633_INT1_SECOND);
+
+                /* Unmask IRQ_ONKEYR */
+                pcf50633_reg_clear_bits(pcf, PCF50633_REG_INT2M,
+                                                PCF50633_INT2_ONKEYR);
+        }
+
+        if ((pcf_int[1] & PCF50633_INT2_ONKEYR) && pcf->onkey1s_held) {
+                pcf->onkey1s_held = 0;
+
+                /* Mask SECOND and ONKEYR interrupts */
+                if (pcf->mask_regs[0] & PCF50633_INT1_SECOND)
+                        pcf50633_reg_set_bit_mask(pcf,
+                                        PCF50633_REG_INT1M,
+                                        PCF50633_INT1_SECOND,
+                                        PCF50633_INT1_SECOND);
+
+                if (pcf->mask_regs[1] & PCF50633_INT2_ONKEYR)
+                        pcf50633_reg_set_bit_mask(pcf,
+                                        PCF50633_REG_INT2M,
+                                        PCF50633_INT2_ONKEYR,
+                                        PCF50633_INT2_ONKEYR);
+        }
+
+        /* Have we just resumed ? */
+        if (pcf->is_suspended) {
+                pcf->is_suspended = 0;
+
+                /* Set the resume reason filtering out non resumers */
+                for (i = 0; i < ARRAY_SIZE(pcf_int); i++)
+                        pcf->resume_reason[i] = pcf_int[i] &
+                                                pcf->pdata->resumers[i];
+
+                /* Make sure we don't pass on any ONKEY events to
+                 * userspace now */
+                pcf_int[1] &= ~(PCF50633_INT2_ONKEYR | PCF50633_INT2_ONKEYF);
+        }
+
+        for (i = 0; i < ARRAY_SIZE(pcf_int); i++) {
+                /* Unset masked interrupts */
+                pcf_int[i] &= ~pcf->mask_regs[i];
+
+                for (j = 0; j < 8 ; j++)
+                        if (pcf_int[i] & (1 << j))
+                                pcf50633_irq_call_handler(pcf, (i * 8) + j);
+        }
+
+out:
+        return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_PM
+
+int pcf50633_irq_suspend(struct pcf50633 *pcf)
+{
+        int ret;
+        int i;
+        u8 res[5];
+
+
+        /* Make sure our interrupt handlers are not called
+         * henceforth */
+        disable_irq(pcf->irq);
+
+        /* Save the masks */
+        ret = pcf50633_read_block(pcf, PCF50633_REG_INT1M,
+                                ARRAY_SIZE(pcf->suspend_irq_masks),
+                                        pcf->suspend_irq_masks);
+        if (ret < 0) {
+                dev_err(pcf->dev, "error saving irq masks\n");
+                goto out;
+        }
+
+        /* Write wakeup irq masks */
+        for (i = 0; i < ARRAY_SIZE(res); i++)
+                res[i] = ~pcf->pdata->resumers[i];
+
+        ret = pcf50633_write_block(pcf, PCF50633_REG_INT1M,
+                                        ARRAY_SIZE(res), &res[0]);
+        if (ret < 0) {
+                dev_err(pcf->dev, "error writing wakeup irq masks\n");
+                goto out;
+        }
+
+        pcf->is_suspended = 1;
+
+out:
+        return ret;
+}
+
+int pcf50633_irq_resume(struct pcf50633 *pcf)
+{
+        int ret;
+
+        /* Write the saved mask registers */
+        ret = pcf50633_write_block(pcf, PCF50633_REG_INT1M,
+                                ARRAY_SIZE(pcf->suspend_irq_masks),
+                                        pcf->suspend_irq_masks);
+        if (ret < 0)
+                dev_err(pcf->dev, "Error restoring saved suspend masks\n");
+
+        enable_irq(pcf->irq);
+
+        return ret;
+}
+
+#endif
+
+int pcf50633_irq_init(struct pcf50633 *pcf, int irq)
+{
+        int ret;
+
+        pcf->irq = irq;
+
+        /* Enable all interrupts except RTC SECOND */
+        pcf->mask_regs[0] = 0x80;
+        pcf50633_reg_write(pcf, PCF50633_REG_INT1M, pcf->mask_regs[0]);
+        pcf50633_reg_write(pcf, PCF50633_REG_INT2M, 0x00);
+        pcf50633_reg_write(pcf, PCF50633_REG_INT3M, 0x00);
+        pcf50633_reg_write(pcf, PCF50633_REG_INT4M, 0x00);
+        pcf50633_reg_write(pcf, PCF50633_REG_INT5M, 0x00);
+
+        ret = request_threaded_irq(irq, NULL, pcf50633_irq,
+                                        IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+                                        "pcf50633", pcf);
+
+        if (ret)
+                dev_err(pcf->dev, "Failed to request IRQ %d\n", ret);
+
+        if (enable_irq_wake(irq) < 0)
+                dev_err(pcf->dev, "IRQ %u cannot be enabled as wake-up source"
+                        "in this hardware revision", irq);
+
+        return ret;
+}
+
+void pcf50633_irq_free(struct pcf50633 *pcf)
+{
+        free_irq(pcf->irq, pcf);
+}
diff --git a/drivers/mfd/rdc321x-southbridge.c b/drivers/mfd/rdc321x-southbridge.c
new file mode 100644
index 000000000000..50922975bda3
--- /dev/null
+++ b/drivers/mfd/rdc321x-southbridge.c
@@ -0,0 +1,123 @@
+/*
+ * RDC321x MFD southbrige driver
+ *
+ * Copyright (C) 2007-2010 Florian Fainelli <florian@openwrt.org>
+ * Copyright (C) 2010 Bernhard Loos <bernhardloos@googlemail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/pci.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/rdc321x.h>
+
+static struct rdc321x_wdt_pdata rdc321x_wdt_pdata;
+
+static struct resource rdc321x_wdt_resource[] = {
+        {
+                .name   = "wdt-reg",
+                .start  = RDC321X_WDT_CTRL,
+                .end    = RDC321X_WDT_CTRL + 0x3,
+                .flags  = IORESOURCE_IO,
+        }
+};
+
+static struct rdc321x_gpio_pdata rdc321x_gpio_pdata = {
+        .max_gpios      = RDC321X_MAX_GPIO,
+};
+
+static struct resource rdc321x_gpio_resources[] = {
+        {
+                .name   = "gpio-reg1",
+                .start  = RDC321X_GPIO_CTRL_REG1,
+                .end    = RDC321X_GPIO_CTRL_REG1 + 0x7,
+                .flags  = IORESOURCE_IO,
+        }, {
+                .name   = "gpio-reg2",
+                .start  = RDC321X_GPIO_CTRL_REG2,
+                .end    = RDC321X_GPIO_CTRL_REG2 + 0x7,
+                .flags  = IORESOURCE_IO,
+        }
+};
+
+static struct mfd_cell rdc321x_sb_cells[] = {
+        {
+                .name           = "rdc321x-wdt",
+                .resources      = rdc321x_wdt_resource,
+                .num_resources  = ARRAY_SIZE(rdc321x_wdt_resource),
+                .driver_data    = &rdc321x_wdt_pdata,
+        }, {
+                .name           = "rdc321x-gpio",
+                .resources      = rdc321x_gpio_resources,
+                .num_resources  = ARRAY_SIZE(rdc321x_gpio_resources),
+                .driver_data    = &rdc321x_gpio_pdata,
+        },
+};
+
+static int __devinit rdc321x_sb_probe(struct pci_dev *pdev,
+                                        const struct pci_device_id *ent)
+{
+        int err;
+
+        err = pci_enable_device(pdev);
+        if (err) {
+                dev_err(&pdev->dev, "failed to enable device\n");
+                return err;
+        }
+
+        rdc321x_gpio_pdata.sb_pdev = pdev;
+        rdc321x_wdt_pdata.sb_pdev = pdev;
+
+        return mfd_add_devices(&pdev->dev, -1,
+                rdc321x_sb_cells, ARRAY_SIZE(rdc321x_sb_cells), NULL, 0);
+}
+
+static void __devexit rdc321x_sb_remove(struct pci_dev *pdev)
+{
+        mfd_remove_devices(&pdev->dev);
+}
+
+static DEFINE_PCI_DEVICE_TABLE(rdc321x_sb_table) = {
+        { PCI_DEVICE(PCI_VENDOR_ID_RDC, PCI_DEVICE_ID_RDC_R6030) },
+        {}
+};
+
+static struct pci_driver rdc321x_sb_driver = {
+        .name           = "RDC321x Southbridge",
+        .id_table       = rdc321x_sb_table,
+        .probe          = rdc321x_sb_probe,
+        .remove         = __devexit_p(rdc321x_sb_remove),
+};
+
+static int __init rdc321x_sb_init(void)
+{
+        return pci_register_driver(&rdc321x_sb_driver);
+}
+
+static void __exit rdc321x_sb_exit(void)
+{
+        pci_unregister_driver(&rdc321x_sb_driver);
+}
+
+module_init(rdc321x_sb_init);
+module_exit(rdc321x_sb_exit);
+
+MODULE_AUTHOR("Florian Fainelli <florian@openwrt.org>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("RDC R-321x MFD southbridge driver");
diff --git a/drivers/mfd/t7l66xb.c b/drivers/mfd/t7l66xb.c
index da6383a934ac..5041d33adf0b 100644
--- a/drivers/mfd/t7l66xb.c
+++ b/drivers/mfd/t7l66xb.c
@@ -318,6 +318,9 @@ static int t7l66xb_probe(struct platform_device *dev)
         struct resource *iomem, *rscr;
         int ret;
 
+        if (pdata == NULL)
+                return -EINVAL;
+
         iomem = platform_get_resource(dev, IORESOURCE_MEM, 0);
         if (!iomem)
                 return -EINVAL;
diff --git a/drivers/mfd/tc35892.c b/drivers/mfd/tc35892.c
new file mode 100644
index 000000000000..e619e2a55997
--- /dev/null
+++ b/drivers/mfd/tc35892.c
@@ -0,0 +1,345 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License, version 2
+ * Author: Hanumath Prasad <hanumath.prasad@stericsson.com> for ST-Ericsson
+ * Author: Rabin Vincent <rabin.vincent@stericsson.com> for ST-Ericsson
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/tc35892.h>
+
+/**
+ * tc35892_reg_read() - read a single TC35892 register
+ * @tc35892:    Device to read from
+ * @reg:        Register to read
+ */
+int tc35892_reg_read(struct tc35892 *tc35892, u8 reg)
+{
+        int ret;
+
+        ret = i2c_smbus_read_byte_data(tc35892->i2c, reg);
+        if (ret < 0)
+                dev_err(tc35892->dev, "failed to read reg %#x: %d\n",
+                        reg, ret);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(tc35892_reg_read);
+
+/**
+ * tc35892_reg_read() - write a single TC35892 register
+ * @tc35892:    Device to write to
+ * @reg:        Register to read
+ * @data:       Value to write
+ */
+int tc35892_reg_write(struct tc35892 *tc35892, u8 reg, u8 data)
+{
+        int ret;
+
+        ret = i2c_smbus_write_byte_data(tc35892->i2c, reg, data);
+        if (ret < 0)
+                dev_err(tc35892->dev, "failed to write reg %#x: %d\n",
+                        reg, ret);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(tc35892_reg_write);
+
+/**
+ * tc35892_block_read() - read multiple TC35892 registers
+ * @tc35892:    Device to read from
+ * @reg:        First register
+ * @length:     Number of registers
+ * @values:     Buffer to write to
+ */
+int tc35892_block_read(struct tc35892 *tc35892, u8 reg, u8 length, u8 *values)
+{
+        int ret;
+
+        ret = i2c_smbus_read_i2c_block_data(tc35892->i2c, reg, length, values);
+        if (ret < 0)
+                dev_err(tc35892->dev, "failed to read regs %#x: %d\n",
+                        reg, ret);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(tc35892_block_read);
+
+/**
+ * tc35892_block_write() - write multiple TC35892 registers
+ * @tc35892:    Device to write to
+ * @reg:        First register
+ * @length:     Number of registers
+ * @values:     Values to write
+ */
+int tc35892_block_write(struct tc35892 *tc35892, u8 reg, u8 length,
+                        const u8 *values)
+{
+        int ret;
+
+        ret = i2c_smbus_write_i2c_block_data(tc35892->i2c, reg, length,
+                                             values);
+        if (ret < 0)
+                dev_err(tc35892->dev, "failed to write regs %#x: %d\n",
+                        reg, ret);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(tc35892_block_write);
+
+/**
+ * tc35892_set_bits() - set the value of a bitfield in a TC35892 register
+ * @tc35892:    Device to write to
+ * @reg:        Register to write
+ * @mask:       Mask of bits to set
+ * @values:     Value to set
+ */
+int tc35892_set_bits(struct tc35892 *tc35892, u8 reg, u8 mask, u8 val)
+{
+        int ret;
+
+        mutex_lock(&tc35892->lock);
+
+        ret = tc35892_reg_read(tc35892, reg);
+        if (ret < 0)
+                goto out;
+
+        ret &= ~mask;
+        ret |= val;
+
+        ret = tc35892_reg_write(tc35892, reg, ret);
+
+out:
+        mutex_unlock(&tc35892->lock);
+        return ret;
+}
+EXPORT_SYMBOL_GPL(tc35892_set_bits);
+
+static struct resource gpio_resources[] = {
+        {
+                .start  = TC35892_INT_GPIIRQ,
+                .end    = TC35892_INT_GPIIRQ,
+                .flags  = IORESOURCE_IRQ,
+        },
+};
+
+static struct mfd_cell tc35892_devs[] = {
+        {
+                .name           = "tc35892-gpio",
+                .num_resources  = ARRAY_SIZE(gpio_resources),
+                .resources      = &gpio_resources[0],
+        },
+};
+
+static irqreturn_t tc35892_irq(int irq, void *data)
+{
+        struct tc35892 *tc35892 = data;
+        int status;
+
+        status = tc35892_reg_read(tc35892, TC35892_IRQST);
+        if (status < 0)
+                return IRQ_NONE;
+
+        while (status) {
+                int bit = __ffs(status);
+
+                handle_nested_irq(tc35892->irq_base + bit);
+                status &= ~(1 << bit);
+        }
+
+        /*
+         * A dummy read or write (to any register) appears to be necessary to
+         * have the last interrupt clear (for example, GPIO IC write) take
+         * effect.
+         */
+        tc35892_reg_read(tc35892, TC35892_IRQST);
+
+        return IRQ_HANDLED;
+}
+
+static void tc35892_irq_dummy(unsigned int irq)
+{
+        /* No mask/unmask at this level */
+}
+
+static struct irq_chip tc35892_irq_chip = {
+        .name   = "tc35892",
+        .mask   = tc35892_irq_dummy,
+        .unmask = tc35892_irq_dummy,
+};
+
+static int tc35892_irq_init(struct tc35892 *tc35892)
+{
+        int base = tc35892->irq_base;
+        int irq;
+
+        for (irq = base; irq < base + TC35892_NR_INTERNAL_IRQS; irq++) {
+                set_irq_chip_data(irq, tc35892);
+                set_irq_chip_and_handler(irq, &tc35892_irq_chip,
+                                         handle_edge_irq);
+                set_irq_nested_thread(irq, 1);
+#ifdef CONFIG_ARM
+                set_irq_flags(irq, IRQF_VALID);
+#else
+                set_irq_noprobe(irq);
+#endif
+        }
+
+        return 0;
+}
+
+static void tc35892_irq_remove(struct tc35892 *tc35892)
+{
+        int base = tc35892->irq_base;
+        int irq;
+
+        for (irq = base; irq < base + TC35892_NR_INTERNAL_IRQS; irq++) {
+#ifdef CONFIG_ARM
+                set_irq_flags(irq, 0);
+#endif
+                set_irq_chip_and_handler(irq, NULL, NULL);
+                set_irq_chip_data(irq, NULL);
+        }
+}
+
+static int tc35892_chip_init(struct tc35892 *tc35892)
+{
+        int manf, ver, ret;
+
+        manf = tc35892_reg_read(tc35892, TC35892_MANFCODE);
+        if (manf < 0)
+                return manf;
+
+        ver = tc35892_reg_read(tc35892, TC35892_VERSION);
+        if (ver < 0)
+                return ver;
+
+        if (manf != TC35892_MANFCODE_MAGIC) {
+                dev_err(tc35892->dev, "unknown manufacturer: %#x\n", manf);
+                return -EINVAL;
+        }
+
+        dev_info(tc35892->dev, "manufacturer: %#x, version: %#x\n", manf, ver);
+
+        /* Put everything except the IRQ module into reset */
+        ret = tc35892_reg_write(tc35892, TC35892_RSTCTRL,
+                                TC35892_RSTCTRL_TIMRST
+                                | TC35892_RSTCTRL_ROTRST
+                                | TC35892_RSTCTRL_KBDRST
+                                | TC35892_RSTCTRL_GPIRST);
+        if (ret < 0)
+                return ret;
+
+        /* Clear the reset interrupt. */
+        return tc35892_reg_write(tc35892, TC35892_RSTINTCLR, 0x1);
+}
+
+static int __devinit tc35892_probe(struct i2c_client *i2c,
+                                   const struct i2c_device_id *id)
+{
+        struct tc35892_platform_data *pdata = i2c->dev.platform_data;
+        struct tc35892 *tc35892;
+        int ret;
+
+        if (!i2c_check_functionality(i2c->adapter, I2C_FUNC_SMBUS_BYTE_DATA
+                                     | I2C_FUNC_SMBUS_I2C_BLOCK))
+                return -EIO;
+
+        tc35892 = kzalloc(sizeof(struct tc35892), GFP_KERNEL);
+        if (!tc35892)
+                return -ENOMEM;
+
+        mutex_init(&tc35892->lock);
+
+        tc35892->dev = &i2c->dev;
+        tc35892->i2c = i2c;
+        tc35892->pdata = pdata;
+        tc35892->irq_base = pdata->irq_base;
+        tc35892->num_gpio = id->driver_data;
+
+        i2c_set_clientdata(i2c, tc35892);
+
+        ret = tc35892_chip_init(tc35892);
+        if (ret)
+                goto out_free;
+
+        ret = tc35892_irq_init(tc35892);
+        if (ret)
+                goto out_free;
+
+        ret = request_threaded_irq(tc35892->i2c->irq, NULL, tc35892_irq,
+                                   IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+                                   "tc35892", tc35892);
+        if (ret) {
+                dev_err(tc35892->dev, "failed to request IRQ: %d\n", ret);
+                goto out_removeirq;
+        }
+
+        ret = mfd_add_devices(tc35892->dev, -1, tc35892_devs,
+                              ARRAY_SIZE(tc35892_devs), NULL,
+                              tc35892->irq_base);
+        if (ret) {
+                dev_err(tc35892->dev, "failed to add children\n");
+                goto out_freeirq;
+        }
+
+        return 0;
+
+out_freeirq:
+        free_irq(tc35892->i2c->irq, tc35892);
+out_removeirq:
+        tc35892_irq_remove(tc35892);
+out_free:
+        kfree(tc35892);
+        return ret;
+}
+
+static int __devexit tc35892_remove(struct i2c_client *client)
+{
+        struct tc35892 *tc35892 = i2c_get_clientdata(client);
+
+        mfd_remove_devices(tc35892->dev);
+
+        free_irq(tc35892->i2c->irq, tc35892);
+        tc35892_irq_remove(tc35892);
+
+        kfree(tc35892);
+
+        return 0;
+}
+
+static const struct i2c_device_id tc35892_id[] = {
+        { "tc35892", 24 },
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, tc35892_id);
+
+static struct i2c_driver tc35892_driver = {
+        .driver.name    = "tc35892",
+        .driver.owner   = THIS_MODULE,
+        .probe          = tc35892_probe,
+        .remove         = __devexit_p(tc35892_remove),
+        .id_table       = tc35892_id,
+};
+
+static int __init tc35892_init(void)
+{
+        return i2c_add_driver(&tc35892_driver);
+}
+subsys_initcall(tc35892_init);
+
+static void __exit tc35892_exit(void)
+{
+        i2c_del_driver(&tc35892_driver);
+}
+module_exit(tc35892_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("TC35892 MFD core driver");
+MODULE_AUTHOR("Hanumath Prasad, Rabin Vincent");
diff --git a/drivers/mfd/timberdale.c b/drivers/mfd/timberdale.c
index 7f478ec4184b..ac5995026c88 100644
--- a/drivers/mfd/timberdale.c
+++ b/drivers/mfd/timberdale.c
@@ -31,6 +31,7 @@
 
 #include <linux/i2c.h>
 #include <linux/i2c-ocores.h>
+#include <linux/i2c-xiic.h>
 #include <linux/i2c/tsc2007.h>
 
 #include <linux/spi/spi.h>
@@ -40,6 +41,8 @@
 
 #include <media/timb_radio.h>
 
+#include <linux/timb_dma.h>
+
 #include "timberdale.h"
 
 #define DRIVER_NAME "timberdale"
@@ -69,6 +72,12 @@ static struct i2c_board_info timberdale_i2c_board_info[] = {
         },
 };
 
+static __devinitdata struct xiic_i2c_platform_data
+timberdale_xiic_platform_data = {
+        .devices = timberdale_i2c_board_info,
+        .num_devices = ARRAY_SIZE(timberdale_i2c_board_info)
+};
+
 static __devinitdata struct ocores_i2c_platform_data
 timberdale_ocores_platform_data = {
         .regstep = 4,
@@ -77,7 +86,20 @@ timberdale_ocores_platform_data = {
         .num_devices = ARRAY_SIZE(timberdale_i2c_board_info)
 };
 
-const static __devinitconst struct resource timberdale_ocores_resources[] = {
+static const __devinitconst struct resource timberdale_xiic_resources[] = {
+        {
+                .start  = XIICOFFSET,
+                .end    = XIICEND,
+                .flags  = IORESOURCE_MEM,
+        },
+        {
+                .start  = IRQ_TIMBERDALE_I2C,
+                .end    = IRQ_TIMBERDALE_I2C,
+                .flags  = IORESOURCE_IRQ,
+        },
+};
+
+static const __devinitconst struct resource timberdale_ocores_resources[] = {
         {
                 .start  = OCORESOFFSET,
                 .end    = OCORESEND,
@@ -126,7 +148,7 @@ static __devinitdata struct xspi_platform_data timberdale_xspi_platform_data = {
          */
 };
 
-const static __devinitconst struct resource timberdale_spi_resources[] = {
+static const __devinitconst struct resource timberdale_spi_resources[] = {
         {
                 .start  = SPIOFFSET,
                 .end    = SPIEND,
@@ -139,7 +161,7 @@ const static __devinitconst struct resource timberdale_spi_resources[] = {
         },
 };
 
-const static __devinitconst struct resource timberdale_eth_resources[] = {
+static const __devinitconst struct resource timberdale_eth_resources[] = {
         {
                 .start  = ETHOFFSET,
                 .end    = ETHEND,
@@ -159,7 +181,7 @@ static __devinitdata struct timbgpio_platform_data
         .irq_base = 200,
 };
 
-const static __devinitconst struct resource timberdale_gpio_resources[] = {
+static const __devinitconst struct resource timberdale_gpio_resources[] = {
         {
                 .start  = GPIOOFFSET,
                 .end    = GPIOEND,
@@ -172,7 +194,7 @@ const static __devinitconst struct resource timberdale_gpio_resources[] = {
         },
 };
 
-const static __devinitconst struct resource timberdale_mlogicore_resources[] = {
+static const __devinitconst struct resource timberdale_mlogicore_resources[] = {
         {
                 .start  = MLCOREOFFSET,
                 .end    = MLCOREEND,
@@ -190,7 +212,7 @@ const static __devinitconst struct resource timberdale_mlogicore_resources[] = {
         },
 };
 
-const static __devinitconst struct resource timberdale_uart_resources[] = {
+static const __devinitconst struct resource timberdale_uart_resources[] = {
         {
                 .start  = UARTOFFSET,
                 .end    = UARTEND,
@@ -203,7 +225,7 @@ const static __devinitconst struct resource timberdale_uart_resources[] = {
         },
 };
 
-const static __devinitconst struct resource timberdale_uartlite_resources[] = {
+static const __devinitconst struct resource timberdale_uartlite_resources[] = {
         {
                 .start  = UARTLITEOFFSET,
                 .end    = UARTLITEEND,
@@ -216,7 +238,7 @@ const static __devinitconst struct resource timberdale_uartlite_resources[] = {
         },
 };
 
-const static __devinitconst struct resource timberdale_radio_resources[] = {
+static const __devinitconst struct resource timberdale_radio_resources[] = {
         {
                 .start  = RDSOFFSET,
                 .end    = RDSEND,
@@ -250,7 +272,66 @@ static __devinitdata struct timb_radio_platform_data
         }
 };
 
-const static __devinitconst struct resource timberdale_dma_resources[] = {
+static __devinitdata struct timb_dma_platform_data timb_dma_platform_data = {
+        .nr_channels = 10,
+        .channels = {
+                {
+                        /* UART RX */
+                        .rx = true,
+                        .descriptors = 2,
+                        .descriptor_elements = 1
+                },
+                {
+                        /* UART TX */
+                        .rx = false,
+                        .descriptors = 2,
+                        .descriptor_elements = 1
+                },
+                {
+                        /* MLB RX */
+                        .rx = true,
+                        .descriptors = 2,
+                        .descriptor_elements = 1
+                },
+                {
+                        /* MLB TX */
+                        .rx = false,
+                        .descriptors = 2,
+                        .descriptor_elements = 1
+                },
+                {
+                        /* Video RX */
+                        .rx = true,
+                        .bytes_per_line = 1440,
+                        .descriptors = 2,
+                        .descriptor_elements = 16
+                },
+                {
+                        /* Video framedrop */
+                },
+                {
+                        /* SDHCI RX */
+                        .rx = true,
+                },
+                {
+                        /* SDHCI TX */
+                },
+                {
+                        /* ETH RX */
+                        .rx = true,
+                        .descriptors = 2,
+                        .descriptor_elements = 1
+                },
+                {
+                        /* ETH TX */
+                        .rx = false,
+                        .descriptors = 2,
+                        .descriptor_elements = 1
+                },
+        }
+};
+
+static const __devinitconst struct resource timberdale_dma_resources[] = {
         {
                 .start  = DMAOFFSET,
                 .end    = DMAEND,
@@ -265,11 +346,25 @@ const static __devinitconst struct resource timberdale_dma_resources[] = {
 
 static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg0[] = {
         {
+                .name = "timb-dma",
+                .num_resources = ARRAY_SIZE(timberdale_dma_resources),
+                .resources = timberdale_dma_resources,
+                .platform_data = &timb_dma_platform_data,
+                .data_size = sizeof(timb_dma_platform_data),
+        },
+        {
                 .name = "timb-uart",
                 .num_resources = ARRAY_SIZE(timberdale_uart_resources),
                 .resources = timberdale_uart_resources,
         },
         {
+                .name = "xiic-i2c",
+                .num_resources = ARRAY_SIZE(timberdale_xiic_resources),
+                .resources = timberdale_xiic_resources,
+                .platform_data = &timberdale_xiic_platform_data,
+                .data_size = sizeof(timberdale_xiic_platform_data),
+        },
+        {
                 .name = "timb-gpio",
                 .num_resources = ARRAY_SIZE(timberdale_gpio_resources),
                 .resources = timberdale_gpio_resources,
@@ -295,14 +390,16 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg0[] = {
                 .num_resources = ARRAY_SIZE(timberdale_eth_resources),
                 .resources = timberdale_eth_resources,
         },
+};
+
+static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg1[] = {
         {
                 .name = "timb-dma",
                 .num_resources = ARRAY_SIZE(timberdale_dma_resources),
                 .resources = timberdale_dma_resources,
+                .platform_data = &timb_dma_platform_data,
+                .data_size = sizeof(timb_dma_platform_data),
         },
-};
-
-static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg1[] = {
         {
                 .name = "timb-uart",
                 .num_resources = ARRAY_SIZE(timberdale_uart_resources),
@@ -314,6 +411,13 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg1[] = {
                 .resources = timberdale_uartlite_resources,
         },
         {
+                .name = "xiic-i2c",
+                .num_resources = ARRAY_SIZE(timberdale_xiic_resources),
+                .resources = timberdale_xiic_resources,
+                .platform_data = &timberdale_xiic_platform_data,
+                .data_size = sizeof(timberdale_xiic_platform_data),
+        },
+        {
                 .name = "timb-gpio",
                 .num_resources = ARRAY_SIZE(timberdale_gpio_resources),
                 .resources = timberdale_gpio_resources,
@@ -344,20 +448,29 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg1[] = {
                 .num_resources = ARRAY_SIZE(timberdale_eth_resources),
                 .resources = timberdale_eth_resources,
         },
+};
+
+static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg2[] = {
         {
                 .name = "timb-dma",
                 .num_resources = ARRAY_SIZE(timberdale_dma_resources),
                 .resources = timberdale_dma_resources,
+                .platform_data = &timb_dma_platform_data,
+                .data_size = sizeof(timb_dma_platform_data),
         },
-};
-
-static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg2[] = {
         {
                 .name = "timb-uart",
                 .num_resources = ARRAY_SIZE(timberdale_uart_resources),
                 .resources = timberdale_uart_resources,
         },
         {
+                .name = "xiic-i2c",
+                .num_resources = ARRAY_SIZE(timberdale_xiic_resources),
+                .resources = timberdale_xiic_resources,
+                .platform_data = &timberdale_xiic_platform_data,
+                .data_size = sizeof(timberdale_xiic_platform_data),
+        },
+        {
                 .name = "timb-gpio",
                 .num_resources = ARRAY_SIZE(timberdale_gpio_resources),
                 .resources = timberdale_gpio_resources,
@@ -378,14 +491,16 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg2[] = {
                 .platform_data = &timberdale_xspi_platform_data,
                 .data_size = sizeof(timberdale_xspi_platform_data),
         },
+};
+
+static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg3[] = {
         {
                 .name = "timb-dma",
                 .num_resources = ARRAY_SIZE(timberdale_dma_resources),
                 .resources = timberdale_dma_resources,
+                .platform_data = &timb_dma_platform_data,
+                .data_size = sizeof(timb_dma_platform_data),
         },
-};
-
-static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg3[] = {
         {
                 .name = "timb-uart",
                 .num_resources = ARRAY_SIZE(timberdale_uart_resources),
@@ -424,11 +539,6 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg3[] = {
                 .num_resources = ARRAY_SIZE(timberdale_eth_resources),
                 .resources = timberdale_eth_resources,
         },
-        {
-                .name = "timb-dma",
-                .num_resources = ARRAY_SIZE(timberdale_dma_resources),
-                .resources = timberdale_dma_resources,
-        },
 };
 
 static const __devinitconst struct resource timberdale_sdhc_resources[] = {
diff --git a/drivers/mfd/timberdale.h b/drivers/mfd/timberdale.h
index 8d27ffabc25d..c11bf6ebfe00 100644
--- a/drivers/mfd/timberdale.h
+++ b/drivers/mfd/timberdale.h
@@ -23,7 +23,7 @@
 #ifndef MFD_TIMBERDALE_H
 #define MFD_TIMBERDALE_H
 
-#define DRV_VERSION             "0.1"
+#define DRV_VERSION             "0.2"
 
 /* This driver only support versions >= 3.8 and < 4.0  */
 #define TIMB_SUPPORTED_MAJOR    3
@@ -66,7 +66,7 @@
 
 #define CHIPCTLOFFSET   0x800
 #define CHIPCTLEND      0x8ff
-#define CHIPCTLSIZE     (CHIPCTLEND - CHIPCTLOFFSET)
+#define CHIPCTLSIZE     (CHIPCTLEND - CHIPCTLOFFSET + 1)
 
 #define INTCOFFSET      0xc00
 #define INTCEND         0xfff
@@ -127,4 +127,16 @@
 #define GPIO_PIN_BT_RST         15
 #define GPIO_NR_PINS            16
 
+/* DMA Channels */
+#define DMA_UART_RX         0
+#define DMA_UART_TX         1
+#define DMA_MLB_RX          2
+#define DMA_MLB_TX          3
+#define DMA_VIDEO_RX        4
+#define DMA_VIDEO_DROP      5
+#define DMA_SDHCI_RX        6
+#define DMA_SDHCI_TX        7
+#define DMA_ETH_RX          8
+#define DMA_ETH_TX          9
+
 #endif
diff --git a/drivers/mfd/tps65010.c b/drivers/mfd/tps65010.c
index e5955306c2fa..d0016b67d125 100644
--- a/drivers/mfd/tps65010.c
+++ b/drivers/mfd/tps65010.c
@@ -531,7 +531,6 @@ static int __exit tps65010_remove(struct i2c_client *client)
         flush_scheduled_work();
         debugfs_remove(tps->file);
         kfree(tps);
-        i2c_set_clientdata(client, NULL);
         the_tps = NULL;
         return 0;
 }
diff --git a/drivers/mfd/tps6507x.c b/drivers/mfd/tps6507x.c
new file mode 100644
index 000000000000..d859dffed39f
--- /dev/null
+++ b/drivers/mfd/tps6507x.c
@@ -0,0 +1,159 @@
+/*
+ * tps6507x.c  --  TPS6507x chip family multi-function driver
+ *
+ *  Copyright (c) 2010 RidgeRun (todd.fischer@ridgerun.com)
+ *
+ * Author: Todd Fischer
+ *         todd.fischer@ridgerun.com
+ *
+ * Credits:
+ *
+ *    Using code from wm831x-*.c, wm8400-core, Wolfson Microelectronics PLC.
+ *
+ * For licencing details see kernel-base/COPYING
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/tps6507x.h>
+
+static struct mfd_cell tps6507x_devs[] = {
+        {
+                .name = "tps6507x-pmic",
+        },
+        {
+                .name = "tps6507x-ts",
+        },
+};
+
+
+static int tps6507x_i2c_read_device(struct tps6507x_dev *tps6507x, char reg,
+                                  int bytes, void *dest)
+{
+        struct i2c_client *i2c = tps6507x->i2c_client;
+        struct i2c_msg xfer[2];
+        int ret;
+
+        /* Write register */
+        xfer[0].addr = i2c->addr;
+        xfer[0].flags = 0;
+        xfer[0].len = 1;
+        xfer[0].buf = &reg;
+
+        /* Read data */
+        xfer[1].addr = i2c->addr;
+        xfer[1].flags = I2C_M_RD;
+        xfer[1].len = bytes;
+        xfer[1].buf = dest;
+
+        ret = i2c_transfer(i2c->adapter, xfer, 2);
+        if (ret == 2)
+                ret = 0;
+        else if (ret >= 0)
+                ret = -EIO;
+
+        return ret;
+}
+
+static int tps6507x_i2c_write_device(struct tps6507x_dev *tps6507x, char reg,
+                                   int bytes, void *src)
+{
+        struct i2c_client *i2c = tps6507x->i2c_client;
+        /* we add 1 byte for device register */
+        u8 msg[TPS6507X_MAX_REGISTER + 1];
+        int ret;
+
+        if (bytes > (TPS6507X_MAX_REGISTER + 1))
+                return -EINVAL;
+
+        msg[0] = reg;
+        memcpy(&msg[1], src, bytes);
+
+        ret = i2c_master_send(i2c, msg, bytes + 1);
+        if (ret < 0)
+                return ret;
+        if (ret != bytes + 1)
+                return -EIO;
+        return 0;
+}
+
+static int tps6507x_i2c_probe(struct i2c_client *i2c,
+                            const struct i2c_device_id *id)
+{
+        struct tps6507x_dev *tps6507x;
+        int ret = 0;
+
+        tps6507x = kzalloc(sizeof(struct tps6507x_dev), GFP_KERNEL);
+        if (tps6507x == NULL) {
+                kfree(i2c);
+                return -ENOMEM;
+        }
+
+        i2c_set_clientdata(i2c, tps6507x);
+        tps6507x->dev = &i2c->dev;
+        tps6507x->i2c_client = i2c;
+        tps6507x->read_dev = tps6507x_i2c_read_device;
+        tps6507x->write_dev = tps6507x_i2c_write_device;
+
+        ret = mfd_add_devices(tps6507x->dev, -1,
+                              tps6507x_devs, ARRAY_SIZE(tps6507x_devs),
+                              NULL, 0);
+
+        if (ret < 0)
+                goto err;
+
+        return ret;
+
+err:
+        mfd_remove_devices(tps6507x->dev);
+        kfree(tps6507x);
+        return ret;
+}
+
+static int tps6507x_i2c_remove(struct i2c_client *i2c)
+{
+        struct tps6507x_dev *tps6507x = i2c_get_clientdata(i2c);
+
+        mfd_remove_devices(tps6507x->dev);
+        kfree(tps6507x);
+
+        return 0;
+}
+
+static const struct i2c_device_id tps6507x_i2c_id[] = {
+       { "tps6507x", 0 },
+       { }
+};
+MODULE_DEVICE_TABLE(i2c, tps6507x_i2c_id);
+
+
+static struct i2c_driver tps6507x_i2c_driver = {
+        .driver = {
+                   .name = "tps6507x",
+                   .owner = THIS_MODULE,
+        },
+        .probe = tps6507x_i2c_probe,
+        .remove = tps6507x_i2c_remove,
+        .id_table = tps6507x_i2c_id,
+};
+
+static int __init tps6507x_i2c_init(void)
+{
+        return i2c_add_driver(&tps6507x_i2c_driver);
+}
+/* init early so consumer devices can complete system boot */
+subsys_initcall(tps6507x_i2c_init);
+
+static void __exit tps6507x_i2c_exit(void)
+{
+        i2c_del_driver(&tps6507x_i2c_driver);
+}
+module_exit(tps6507x_i2c_exit);
+
+MODULE_DESCRIPTION("TPS6507x chip family multi-function driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/twl4030-irq.c b/drivers/mfd/twl4030-irq.c
index 202bdd59632d..097f24d8bceb 100644
--- a/drivers/mfd/twl4030-irq.c
+++ b/drivers/mfd/twl4030-irq.c
@@ -232,10 +232,11 @@ static const struct sih sih_modules_twl5031[8] = {
         },
         [6] = {
                 /*
-                 * ACI doesn't use the same SIH organization.
-                 * For example, it supports only one interrupt line
+                 * ECI/DBI doesn't use the same SIH organization.
+                 * For example, it supports only one interrupt output line.
+                 * That is, the interrupts are seen on both INT1 and INT2 lines.
                  */
-                .name           = "aci",
+                .name           = "eci_dbi",
                 .module         = TWL5031_MODULE_ACCESSORY,
                 .bits           = 9,
                 .bytes_ixr      = 2,
@@ -247,8 +248,8 @@ static const struct sih sih_modules_twl5031[8] = {
 
         },
         [7] = {
-                /* Accessory */
-                .name           = "acc",
+                /* Audio accessory */
+                .name           = "audio",
                 .module         = TWL5031_MODULE_ACCESSORY,
                 .control_offset = TWL5031_ACCSIHCTRL,
                 .bits           = 2,
diff --git a/drivers/mfd/wm831x-core.c b/drivers/mfd/wm831x-core.c
index f2ab025ad97a..1a968f34d679 100644
--- a/drivers/mfd/wm831x-core.c
+++ b/drivers/mfd/wm831x-core.c
@@ -322,7 +322,11 @@ EXPORT_SYMBOL_GPL(wm831x_set_bits);
  */
 int wm831x_auxadc_read(struct wm831x *wm831x, enum wm831x_auxadc input)
 {
-        int ret, src;
+        int ret, src, irq_masked, timeout;
+
+        /* Are we using the interrupt? */
+        irq_masked = wm831x_reg_read(wm831x, WM831X_INTERRUPT_STATUS_1_MASK);
+        irq_masked &= WM831X_AUXADC_DATA_EINT;
 
         mutex_lock(&wm831x->auxadc_lock);
 
@@ -342,6 +346,9 @@ int wm831x_auxadc_read(struct wm831x *wm831x, enum wm831x_auxadc input)
                 goto out;
         }
 
+        /* Clear any notification from a very late arriving interrupt */
+        try_wait_for_completion(&wm831x->auxadc_done);
+
         ret = wm831x_set_bits(wm831x, WM831X_AUXADC_CONTROL,
                               WM831X_AUX_CVT_ENA, WM831X_AUX_CVT_ENA);
         if (ret < 0) {
@@ -349,22 +356,46 @@ int wm831x_auxadc_read(struct wm831x *wm831x, enum wm831x_auxadc input)
                 goto disable;
         }
 
-        /* If an interrupt arrived late clean up after it */
-        try_wait_for_completion(&wm831x->auxadc_done);
-
-        /* Ignore the result to allow us to soldier on without IRQ hookup */
-        wait_for_completion_timeout(&wm831x->auxadc_done, msecs_to_jiffies(5));
-
-        ret = wm831x_reg_read(wm831x, WM831X_AUXADC_CONTROL);
-        if (ret < 0) {
-                dev_err(wm831x->dev, "AUXADC status read failed: %d\n", ret);
-                goto disable;
-        }
-
-        if (ret & WM831X_AUX_CVT_ENA) {
-                dev_err(wm831x->dev, "Timed out reading AUXADC\n");
-                ret = -EBUSY;
-                goto disable;
+        if (irq_masked) {
+                /* If we're not using interrupts then poll the
+                 * interrupt status register */
+                timeout = 5;
+                while (timeout) {
+                        msleep(1);
+
+                        ret = wm831x_reg_read(wm831x,
+                                              WM831X_INTERRUPT_STATUS_1);
+                        if (ret < 0) {
+                                dev_err(wm831x->dev,
+                                        "ISR 1 read failed: %d\n", ret);
+                                goto disable;
+                        }
+
+                        /* Did it complete? */
+                        if (ret & WM831X_AUXADC_DATA_EINT) {
+                                wm831x_reg_write(wm831x,
+                                                 WM831X_INTERRUPT_STATUS_1,
+                                                 WM831X_AUXADC_DATA_EINT);
+                                break;
+                        } else {
+                                dev_err(wm831x->dev,
+                                        "AUXADC conversion timeout\n");
+                                ret = -EBUSY;
+                                goto disable;
+                        }
+                }
+        } else {
+                /* If we are using interrupts then wait for the
+                 * interrupt to complete.  Use an extremely long
+                 * timeout to handle situations with heavy load where
+                 * the notification of the interrupt may be delayed by
+                 * threaded IRQ handling. */
+                if (!wait_for_completion_timeout(&wm831x->auxadc_done,
+                                                 msecs_to_jiffies(500))) {
+                        dev_err(wm831x->dev, "Timed out waiting for AUXADC\n");
+                        ret = -EBUSY;
+                        goto disable;
+                }
         }
 
         ret = wm831x_reg_read(wm831x, WM831X_AUXADC_DATA);
@@ -1463,6 +1494,7 @@ static int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
         case WM8310:
                 parent = WM8310;
                 wm831x->num_gpio = 16;
+                wm831x->charger_irq_wake = 1;
                 if (rev > 0) {
                         wm831x->has_gpio_ena = 1;
                         wm831x->has_cs_sts = 1;
@@ -1474,6 +1506,7 @@ static int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
         case WM8311:
                 parent = WM8311;
                 wm831x->num_gpio = 16;
+                wm831x->charger_irq_wake = 1;
                 if (rev > 0) {
                         wm831x->has_gpio_ena = 1;
                         wm831x->has_cs_sts = 1;
@@ -1485,6 +1518,7 @@ static int wm831x_device_init(struct wm831x *wm831x, unsigned long id, int irq)
         case WM8312:
                 parent = WM8312;
                 wm831x->num_gpio = 16;
+                wm831x->charger_irq_wake = 1;
                 if (rev > 0) {
                         wm831x->has_gpio_ena = 1;
                         wm831x->has_cs_sts = 1;
@@ -1623,6 +1657,42 @@ static void wm831x_device_exit(struct wm831x *wm831x)
         kfree(wm831x);
 }
 
+static int wm831x_device_suspend(struct wm831x *wm831x)
+{
+        int reg, mask;
+
+        /* If the charger IRQs are a wake source then make sure we ack
+         * them even if they're not actively being used (eg, no power
+         * driver or no IRQ line wired up) then acknowledge the
+         * interrupts otherwise suspend won't last very long.
+         */
+        if (wm831x->charger_irq_wake) {
+                reg = wm831x_reg_read(wm831x, WM831X_INTERRUPT_STATUS_2_MASK);
+
+                mask = WM831X_CHG_BATT_HOT_EINT |
+                        WM831X_CHG_BATT_COLD_EINT |
+                        WM831X_CHG_BATT_FAIL_EINT |
+                        WM831X_CHG_OV_EINT | WM831X_CHG_END_EINT |
+                        WM831X_CHG_TO_EINT | WM831X_CHG_MODE_EINT |
+                        WM831X_CHG_START_EINT;
+
+                /* If any of the interrupts are masked read the statuses */
+                if (reg & mask)
+                        reg = wm831x_reg_read(wm831x,
+                                              WM831X_INTERRUPT_STATUS_2);
+
+                if (reg & mask) {
+                        dev_info(wm831x->dev,
+                                 "Acknowledging masked charger IRQs: %x\n",
+                                 reg & mask);
+                        wm831x_reg_write(wm831x, WM831X_INTERRUPT_STATUS_2,
+                                         reg & mask);
+                }
+        }
+
+        return 0;
+}
+
 static int wm831x_i2c_read_device(struct wm831x *wm831x, unsigned short reg,
                                   int bytes, void *dest)
 {
@@ -1697,6 +1767,13 @@ static int wm831x_i2c_remove(struct i2c_client *i2c)
         return 0;
 }
 
+static int wm831x_i2c_suspend(struct i2c_client *i2c, pm_message_t mesg)
+{
+        struct wm831x *wm831x = i2c_get_clientdata(i2c);
+
+        return wm831x_device_suspend(wm831x);
+}
+
 static const struct i2c_device_id wm831x_i2c_id[] = {
         { "wm8310", WM8310 },
         { "wm8311", WM8311 },
@@ -1714,6 +1791,7 @@ static struct i2c_driver wm831x_i2c_driver = {
         },
         .probe = wm831x_i2c_probe,
         .remove = wm831x_i2c_remove,
+        .suspend = wm831x_i2c_suspend,
         .id_table = wm831x_i2c_id,
 };
 
diff --git a/drivers/mfd/wm831x-irq.c b/drivers/mfd/wm831x-irq.c
index 4c1122ceb443..7dabe4dbd373 100644
--- a/drivers/mfd/wm831x-irq.c
+++ b/drivers/mfd/wm831x-irq.c
@@ -39,8 +39,6 @@ struct wm831x_irq_data {
         int primary;
         int reg;
         int mask;
-        irq_handler_t handler;
-        void *handler_data;
 };
 
 static struct wm831x_irq_data wm831x_irqs[] = {
@@ -492,6 +490,14 @@ int wm831x_irq_init(struct wm831x *wm831x, int irq)
 
         mutex_init(&wm831x->irq_lock);
 
+        /* Mask the individual interrupt sources */
+        for (i = 0; i < ARRAY_SIZE(wm831x->irq_masks_cur); i++) {
+                wm831x->irq_masks_cur[i] = 0xffff;
+                wm831x->irq_masks_cache[i] = 0xffff;
+                wm831x_reg_write(wm831x, WM831X_INTERRUPT_STATUS_1_MASK + i,
+                                 0xffff);
+        }
+
         if (!irq) {
                 dev_warn(wm831x->dev,
                          "No interrupt specified - functionality limited\n");
@@ -507,14 +513,6 @@ int wm831x_irq_init(struct wm831x *wm831x, int irq)
         wm831x->irq = irq;
         wm831x->irq_base = pdata->irq_base;
 
-        /* Mask the individual interrupt sources */
-        for (i = 0; i < ARRAY_SIZE(wm831x->irq_masks_cur); i++) {
-                wm831x->irq_masks_cur[i] = 0xffff;
-                wm831x->irq_masks_cache[i] = 0xffff;
-                wm831x_reg_write(wm831x, WM831X_INTERRUPT_STATUS_1_MASK + i,
-                                 0xffff);
-        }
-
         /* Register them with genirq */
         for (cur_irq = wm831x->irq_base;
              cur_irq < ARRAY_SIZE(wm831x_irqs) + wm831x->irq_base;
diff --git a/drivers/mfd/wm8350-i2c.c b/drivers/mfd/wm8350-i2c.c
index 65830f57c093..5fe5de166adb 100644
--- a/drivers/mfd/wm8350-i2c.c
+++ b/drivers/mfd/wm8350-i2c.c
@@ -64,10 +64,8 @@ static int wm8350_i2c_probe(struct i2c_client *i2c,
         int ret = 0;
 
         wm8350 = kzalloc(sizeof(struct wm8350), GFP_KERNEL);
-        if (wm8350 == NULL) {
-                kfree(i2c);
+        if (wm8350 == NULL)
                 return -ENOMEM;
-        }
 
         i2c_set_clientdata(i2c, wm8350);
         wm8350->dev = &i2c->dev;
diff --git a/drivers/mfd/wm8400-core.c b/drivers/mfd/wm8400-core.c
index 865ce013a821..1bfef4846b07 100644
--- a/drivers/mfd/wm8400-core.c
+++ b/drivers/mfd/wm8400-core.c
@@ -118,7 +118,7 @@ static int wm8400_read(struct wm8400 *wm8400, u8 reg, int num_regs, u16 *dest)
 {
         int i, ret = 0;
 
-        BUG_ON(reg + num_regs - 1 > ARRAY_SIZE(wm8400->reg_cache));
+        BUG_ON(reg + num_regs > ARRAY_SIZE(wm8400->reg_cache));
 
         /* If there are any volatile reads then read back the entire block */
         for (i = reg; i < reg + num_regs; i++)
@@ -144,7 +144,7 @@ static int wm8400_write(struct wm8400 *wm8400, u8 reg, int num_regs,
 {
         int ret, i;
 
-        BUG_ON(reg + num_regs - 1 > ARRAY_SIZE(wm8400->reg_cache));
+        BUG_ON(reg + num_regs > ARRAY_SIZE(wm8400->reg_cache));
 
         for (i = 0; i < num_regs; i++) {
                 BUG_ON(!reg_data[reg + i].writable);
@@ -415,7 +415,6 @@ static int wm8400_i2c_probe(struct i2c_client *i2c,
         return 0;
 
 struct_err:
-        i2c_set_clientdata(i2c, NULL);
         kfree(wm8400);
 err:
         return ret;
@@ -426,7 +425,6 @@ static int wm8400_i2c_remove(struct i2c_client *i2c)
         struct wm8400 *wm8400 = i2c_get_clientdata(i2c);
 
         wm8400_release(wm8400);
-        i2c_set_clientdata(i2c, NULL);
         kfree(wm8400);
 
         return 0;
diff --git a/drivers/misc/eeprom/at24.c b/drivers/misc/eeprom/at24.c
index f7ca3a42b490..559b0b3c16c3 100644
--- a/drivers/misc/eeprom/at24.c
+++ b/drivers/misc/eeprom/at24.c
@@ -643,7 +643,6 @@ static int __devexit at24_remove(struct i2c_client *client)
 
         kfree(at24->writebuf);
         kfree(at24);
-        i2c_set_clientdata(client, NULL);
         return 0;
 }
 
diff --git a/drivers/misc/lkdtm.c b/drivers/misc/lkdtm.c
index 31a991161f0a..5bfb2a2041b8 100644
--- a/drivers/misc/lkdtm.c
+++ b/drivers/misc/lkdtm.c
@@ -75,6 +75,9 @@ enum ctype {
         UNALIGNED_LOAD_STORE_WRITE,
         OVERWRITE_ALLOCATION,
         WRITE_AFTER_FREE,
+        SOFTLOCKUP,
+        HARDLOCKUP,
+        HUNG_TASK,
 };
 
 static char* cp_name[] = {
@@ -99,6 +102,9 @@ static char* cp_type[] = {
         "UNALIGNED_LOAD_STORE_WRITE",
         "OVERWRITE_ALLOCATION",
         "WRITE_AFTER_FREE",
+        "SOFTLOCKUP",
+        "HARDLOCKUP",
+        "HUNG_TASK",
 };
 
 static struct jprobe lkdtm;
@@ -320,6 +326,20 @@ static void lkdtm_do_action(enum ctype which)
                 memset(data, 0x78, len);
                 break;
         }
+        case SOFTLOCKUP:
+                preempt_disable();
+                for (;;)
+                        cpu_relax();
+                break;
+        case HARDLOCKUP:
+                local_irq_disable();
+                for (;;)
+                        cpu_relax();
+                break;
+        case HUNG_TASK:
+                set_current_state(TASK_UNINTERRUPTIBLE);
+                schedule();
+                break;
         case NONE:
         default:
                 break;
diff --git a/drivers/misc/vmware_balloon.c b/drivers/misc/vmware_balloon.c
index db9cd0240c6f..2a1e804a71aa 100644
--- a/drivers/misc/vmware_balloon.c
+++ b/drivers/misc/vmware_balloon.c
@@ -45,7 +45,7 @@
 
 MODULE_AUTHOR("VMware, Inc.");
 MODULE_DESCRIPTION("VMware Memory Control (Balloon) Driver");
-MODULE_VERSION("1.2.1.0-K");
+MODULE_VERSION("1.2.1.1-k");
 MODULE_ALIAS("dmi:*:svnVMware*:*");
 MODULE_ALIAS("vmware_vmmemctl");
 MODULE_LICENSE("GPL");
@@ -101,6 +101,8 @@ MODULE_LICENSE("GPL");
 /* Maximum number of page allocations without yielding processor */
 #define VMW_BALLOON_YIELD_THRESHOLD     1024
 
+/* Maximum number of refused pages we accumulate during inflation cycle */
+#define VMW_BALLOON_MAX_REFUSED         16
 
 /*
  * Hypervisor communication port definitions.
@@ -183,6 +185,7 @@ struct vmballoon {
 
         /* transient list of non-balloonable pages */
         struct list_head refused_pages;
+        unsigned int n_refused_pages;
 
         /* balloon size in pages */
         unsigned int size;
@@ -428,14 +431,21 @@ static int vmballoon_reserve_page(struct vmballoon *b, bool can_sleep)
                 /* inform monitor */
                 locked = vmballoon_send_lock_page(b, page_to_pfn(page));
                 if (!locked) {
+                        STATS_INC(b->stats.refused_alloc);
+
                         if (b->reset_required) {
                                 __free_page(page);
                                 return -EIO;
                         }
 
-                        /* place on list of non-balloonable pages, retry allocation */
+                        /*
+                         * Place page on the list of non-balloonable pages
+                         * and retry allocation, unless we already accumulated
+                         * too many of them, in which case take a breather.
+                         */
                         list_add(&page->lru, &b->refused_pages);
-                        STATS_INC(b->stats.refused_alloc);
+                        if (++b->n_refused_pages >= VMW_BALLOON_MAX_REFUSED)
+                                return -EIO;
                 }
         } while (!locked);
 
@@ -483,6 +493,8 @@ static void vmballoon_release_refused_pages(struct vmballoon *b)
                 __free_page(page);
                 STATS_INC(b->stats.refused_free);
         }
+
+        b->n_refused_pages = 0;
 }
 
 /*
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 3168ebd616b2..569e94da844c 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -1252,9 +1252,8 @@ EXPORT_SYMBOL(mmc_card_can_sleep);
 /**
  *      mmc_suspend_host - suspend a host
  *      @host: mmc host
- *      @state: suspend mode (PM_SUSPEND_xxx)
  */
-int mmc_suspend_host(struct mmc_host *host, pm_message_t state)
+int mmc_suspend_host(struct mmc_host *host)
 {
         int err = 0;
 
diff --git a/drivers/mmc/core/sd_ops.c b/drivers/mmc/core/sd_ops.c
index 0d96080d44b0..63772e7e7608 100644
--- a/drivers/mmc/core/sd_ops.c
+++ b/drivers/mmc/core/sd_ops.c
@@ -79,8 +79,6 @@ int mmc_wait_for_app_cmd(struct mmc_host *host, struct mmc_card *card,
          * we cannot use the retries field in mmc_command.
          */
         for (i = 0;i <= retries;i++) {
-                memset(&mrq, 0, sizeof(struct mmc_request));
-
                 err = mmc_app_cmd(host, card);
                 if (err) {
                         /* no point in retrying; no APP commands allowed */
diff --git a/drivers/mmc/core/sdio_io.c b/drivers/mmc/core/sdio_io.c
index ff27c8c71355..0f687cdeb064 100644
--- a/drivers/mmc/core/sdio_io.c
+++ b/drivers/mmc/core/sdio_io.c
@@ -406,6 +406,36 @@ void sdio_writeb(struct sdio_func *func, u8 b, unsigned int addr, int *err_ret)
 EXPORT_SYMBOL_GPL(sdio_writeb);
 
 /**
+ *      sdio_writeb_readb - write and read a byte from SDIO function
+ *      @func: SDIO function to access
+ *      @write_byte: byte to write
+ *      @addr: address to write to
+ *      @err_ret: optional status value from transfer
+ *
+ *      Performs a RAW (Read after Write) operation as defined by SDIO spec -
+ *      single byte is written to address space of a given SDIO function and
+ *      response is read back from the same address, both using single request.
+ *      If there is a problem with the operation, 0xff is returned and
+ *      @err_ret will contain the error code.
+ */
+u8 sdio_writeb_readb(struct sdio_func *func, u8 write_byte,
+        unsigned int addr, int *err_ret)
+{
+        int ret;
+        u8 val;
+
+        ret = mmc_io_rw_direct(func->card, 1, func->num, addr,
+                        write_byte, &val);
+        if (err_ret)
+                *err_ret = ret;
+        if (ret)
+                val = 0xff;
+
+        return val;
+}
+EXPORT_SYMBOL_GPL(sdio_writeb_readb);
+
+/**
  *      sdio_memcpy_fromio - read a chunk of memory from a SDIO function
  *      @func: SDIO function to access
  *      @dst: buffer to store the data
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index 2e13b94769fd..f06d06e7fdfa 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -136,6 +136,18 @@ config MMC_SDHCI_S3C
 
           If unsure, say N.
 
+config MMC_SDHCI_SPEAR
+        tristate "SDHCI support on ST SPEAr platform"
+        depends on MMC_SDHCI && PLAT_SPEAR
+        help
+          This selects the Secure Digital Host Controller Interface (SDHCI)
+          often referrered to as the HSMMC block in some of the ST SPEAR range
+          of SoC
+
+          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
@@ -237,7 +249,7 @@ config MMC_IMX
 
 config MMC_MSM7X00A
         tristate "Qualcomm MSM 7X00A SDCC Controller Support"
-        depends on MMC && ARCH_MSM
+        depends on MMC && ARCH_MSM && !ARCH_MSM7X30
         help
           This provides support for the SD/MMC cell found in the
           MSM 7X00A controllers from Qualcomm.
@@ -412,3 +424,11 @@ config SDH_BFIN_MISSING_CMD_PULLUP_WORKAROUND
         depends on SDH_BFIN
         help
           If you say yes here SD-Cards may work on the EZkit.
+
+config MMC_SH_MMCIF
+        tristate "SuperH Internal MMCIF support"
+        depends on MMC_BLOCK && (SUPERH || ARCH_SHMOBILE)
+        help
+          This selects the MMC Host Interface controler (MMCIF).
+
+          This driver supports MMCIF in sh7724/sh7757/sh7372.
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index f4803977dfce..e30c2ee48894 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -14,6 +14,7 @@ obj-$(CONFIG_MMC_SDHCI)		+= sdhci.o
 obj-$(CONFIG_MMC_SDHCI_PCI)     += sdhci-pci.o
 obj-$(CONFIG_MMC_SDHCI_PLTFM)   += sdhci-pltfm.o
 obj-$(CONFIG_MMC_SDHCI_S3C)     += sdhci-s3c.o
+obj-$(CONFIG_MMC_SDHCI_SPEAR)   += sdhci-spear.o
 obj-$(CONFIG_MMC_WBSD)          += wbsd.o
 obj-$(CONFIG_MMC_AU1X)          += au1xmmc.o
 obj-$(CONFIG_MMC_OMAP)          += omap.o
@@ -34,6 +35,7 @@ obj-$(CONFIG_MMC_TMIO)		+= tmio_mmc.o
 obj-$(CONFIG_MMC_CB710) += cb710-mmc.o
 obj-$(CONFIG_MMC_VIA_SDMMC)     += via-sdmmc.o
 obj-$(CONFIG_SDH_BFIN)          += bfin_sdh.o
+obj-$(CONFIG_MMC_SH_MMCIF)      += sh_mmcif.o
 
 obj-$(CONFIG_MMC_SDHCI_OF)      += sdhci-of.o
 sdhci-of-y                              := sdhci-of-core.o
diff --git a/drivers/mmc/host/at91_mci.c b/drivers/mmc/host/at91_mci.c
index 336d9f553f3e..5f3a599ead07 100644
--- a/drivers/mmc/host/at91_mci.c
+++ b/drivers/mmc/host/at91_mci.c
@@ -1157,7 +1157,7 @@ static int at91_mci_suspend(struct platform_device *pdev, pm_message_t state)
                 enable_irq_wake(host->board->det_pin);
 
         if (mmc)
-                ret = mmc_suspend_host(mmc, state);
+                ret = mmc_suspend_host(mmc);
 
         return ret;
 }
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index df0e8a88d85f..95ef864ad8f9 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -173,6 +173,7 @@ struct atmel_mci {
  * @mmc: The mmc_host representing this slot.
  * @host: The MMC controller this slot is using.
  * @sdc_reg: Value of SDCR to be written before using this slot.
+ * @sdio_irq: SDIO irq mask for this slot.
  * @mrq: mmc_request currently being processed or waiting to be
  *      processed, or NULL when the slot is idle.
  * @queue_node: List node for placing this node in the @queue list of
@@ -191,6 +192,7 @@ struct atmel_mci_slot {
         struct atmel_mci        *host;
 
         u32                     sdc_reg;
+        u32                     sdio_irq;
 
         struct mmc_request      *mrq;
         struct list_head        queue_node;
@@ -792,7 +794,7 @@ static void atmci_start_request(struct atmel_mci *host,
         mci_writel(host, SDCR, slot->sdc_reg);
 
         iflags = mci_readl(host, IMR);
-        if (iflags)
+        if (iflags & ~(MCI_SDIOIRQA | MCI_SDIOIRQB))
                 dev_warn(&slot->mmc->class_dev, "WARNING: IMR=0x%08x\n",
                                 iflags);
 
@@ -952,10 +954,21 @@ static void atmci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
                 if (mci_has_rwproof())
                         host->mode_reg |= (MCI_MR_WRPROOF | MCI_MR_RDPROOF);
 
-                if (list_empty(&host->queue))
+                if (atmci_is_mci2()) {
+                        /* setup High Speed mode in relation with card capacity */
+                        if (ios->timing == MMC_TIMING_SD_HS)
+                                host->cfg_reg |= MCI_CFG_HSMODE;
+                        else
+                                host->cfg_reg &= ~MCI_CFG_HSMODE;
+                }
+
+                if (list_empty(&host->queue)) {
                         mci_writel(host, MR, host->mode_reg);
-                else
+                        if (atmci_is_mci2())
+                                mci_writel(host, CFG, host->cfg_reg);
+                } else {
                         host->need_clock_update = true;
+                }
 
                 spin_unlock_bh(&host->lock);
         } else {
@@ -1030,11 +1043,23 @@ static int atmci_get_cd(struct mmc_host *mmc)
         return present;
 }
 
+static void atmci_enable_sdio_irq(struct mmc_host *mmc, int enable)
+{
+        struct atmel_mci_slot   *slot = mmc_priv(mmc);
+        struct atmel_mci        *host = slot->host;
+
+        if (enable)
+                mci_writel(host, IER, slot->sdio_irq);
+        else
+                mci_writel(host, IDR, slot->sdio_irq);
+}
+
 static const struct mmc_host_ops atmci_ops = {
         .request        = atmci_request,
         .set_ios        = atmci_set_ios,
         .get_ro         = atmci_get_ro,
         .get_cd         = atmci_get_cd,
+        .enable_sdio_irq = atmci_enable_sdio_irq,
 };
 
 /* Called with host->lock held */
@@ -1052,8 +1077,11 @@ static void atmci_request_end(struct atmel_mci *host, struct mmc_request *mrq)
          * necessary if set_ios() is called when a different slot is
          * busy transfering data.
          */
-        if (host->need_clock_update)
+        if (host->need_clock_update) {
                 mci_writel(host, MR, host->mode_reg);
+                if (atmci_is_mci2())
+                        mci_writel(host, CFG, host->cfg_reg);
+        }
 
         host->cur_slot->mrq = NULL;
         host->mrq = NULL;
@@ -1483,6 +1511,19 @@ static void atmci_cmd_interrupt(struct atmel_mci *host, u32 status)
         tasklet_schedule(&host->tasklet);
 }
 
+static void atmci_sdio_interrupt(struct atmel_mci *host, u32 status)
+{
+        int     i;
+
+        for (i = 0; i < ATMEL_MCI_MAX_NR_SLOTS; i++) {
+                struct atmel_mci_slot *slot = host->slot[i];
+                if (slot && (status & slot->sdio_irq)) {
+                        mmc_signal_sdio_irq(slot->mmc);
+                }
+        }
+}
+
+
 static irqreturn_t atmci_interrupt(int irq, void *dev_id)
 {
         struct atmel_mci        *host = dev_id;
@@ -1522,6 +1563,10 @@ static irqreturn_t atmci_interrupt(int irq, void *dev_id)
 
                 if (pending & MCI_CMDRDY)
                         atmci_cmd_interrupt(host, status);
+
+                if (pending & (MCI_SDIOIRQA | MCI_SDIOIRQB))
+                        atmci_sdio_interrupt(host, status);
+
         } while (pass_count++ < 5);
 
         return pass_count ? IRQ_HANDLED : IRQ_NONE;
@@ -1544,7 +1589,7 @@ static irqreturn_t atmci_detect_interrupt(int irq, void *dev_id)
 
 static int __init atmci_init_slot(struct atmel_mci *host,
                 struct mci_slot_pdata *slot_data, unsigned int id,
-                u32 sdc_reg)
+                u32 sdc_reg, u32 sdio_irq)
 {
         struct mmc_host                 *mmc;
         struct atmel_mci_slot           *slot;
@@ -1560,11 +1605,16 @@ static int __init atmci_init_slot(struct atmel_mci *host,
         slot->wp_pin = slot_data->wp_pin;
         slot->detect_is_active_high = slot_data->detect_is_active_high;
         slot->sdc_reg = sdc_reg;
+        slot->sdio_irq = sdio_irq;
 
         mmc->ops = &atmci_ops;
         mmc->f_min = DIV_ROUND_UP(host->bus_hz, 512);
         mmc->f_max = host->bus_hz / 2;
         mmc->ocr_avail  = MMC_VDD_32_33 | MMC_VDD_33_34;
+        if (sdio_irq)
+                mmc->caps |= MMC_CAP_SDIO_IRQ;
+        if (atmci_is_mci2())
+                mmc->caps |= MMC_CAP_SD_HIGHSPEED;
         if (slot_data->bus_width >= 4)
                 mmc->caps |= MMC_CAP_4_BIT_DATA;
 
@@ -1753,13 +1803,13 @@ static int __init atmci_probe(struct platform_device *pdev)
         ret = -ENODEV;
         if (pdata->slot[0].bus_width) {
                 ret = atmci_init_slot(host, &pdata->slot[0],
-                                0, MCI_SDCSEL_SLOT_A);
+                                0, MCI_SDCSEL_SLOT_A, MCI_SDIOIRQA);
                 if (!ret)
                         nr_slots++;
         }
         if (pdata->slot[1].bus_width) {
                 ret = atmci_init_slot(host, &pdata->slot[1],
-                                1, MCI_SDCSEL_SLOT_B);
+                                1, MCI_SDCSEL_SLOT_B, MCI_SDIOIRQB);
                 if (!ret)
                         nr_slots++;
         }
diff --git a/drivers/mmc/host/au1xmmc.c b/drivers/mmc/host/au1xmmc.c
index f5834449400e..c8da5d30a861 100644
--- a/drivers/mmc/host/au1xmmc.c
+++ b/drivers/mmc/host/au1xmmc.c
@@ -1142,7 +1142,7 @@ static int au1xmmc_suspend(struct platform_device *pdev, pm_message_t state)
         struct au1xmmc_host *host = platform_get_drvdata(pdev);
         int ret;
 
-        ret = mmc_suspend_host(host->mmc, state);
+        ret = mmc_suspend_host(host->mmc);
         if (ret)
                 return ret;
 
diff --git a/drivers/mmc/host/bfin_sdh.c b/drivers/mmc/host/bfin_sdh.c
index 6919e844072c..4b0e677d7295 100644
--- a/drivers/mmc/host/bfin_sdh.c
+++ b/drivers/mmc/host/bfin_sdh.c
@@ -576,7 +576,7 @@ static int sdh_suspend(struct platform_device *dev, pm_message_t state)
         int ret = 0;
 
         if (mmc)
-                ret = mmc_suspend_host(mmc, state);
+                ret = mmc_suspend_host(mmc);
 
         bfin_write_SDH_PWR_CTL(bfin_read_SDH_PWR_CTL() & ~PWR_ON);
         peripheral_free_list(drv_data->pin_req);
diff --git a/drivers/mmc/host/cb710-mmc.c b/drivers/mmc/host/cb710-mmc.c
index 92a324f7417c..ca3bdc831900 100644
--- a/drivers/mmc/host/cb710-mmc.c
+++ b/drivers/mmc/host/cb710-mmc.c
@@ -675,7 +675,7 @@ static int cb710_mmc_suspend(struct platform_device *pdev, pm_message_t state)
         struct mmc_host *mmc = cb710_slot_to_mmc(slot);
         int err;
 
-        err = mmc_suspend_host(mmc, state);
+        err = mmc_suspend_host(mmc);
         if (err)
                 return err;
 
diff --git a/drivers/mmc/host/davinci_mmc.c b/drivers/mmc/host/davinci_mmc.c
index 3bd0ba294e9d..33d9f1b00862 100644
--- a/drivers/mmc/host/davinci_mmc.c
+++ b/drivers/mmc/host/davinci_mmc.c
@@ -137,15 +137,15 @@
 
 /*
  * One scatterlist dma "segment" is at most MAX_CCNT rw_threshold units,
- * and we handle up to NR_SG segments.  MMC_BLOCK_BOUNCE kicks in only
+ * and we handle up to MAX_NR_SG segments.  MMC_BLOCK_BOUNCE kicks in only
  * for drivers with max_hw_segs == 1, making the segments bigger (64KB)
- * than the page or two that's otherwise typical.  NR_SG == 16 gives at
- * least the same throughput boost, using EDMA transfer linkage instead
- * of spending CPU time copying pages.
+ * than the page or two that's otherwise typical. nr_sg (passed from
+ * platform data) == 16 gives at least the same throughput boost, using
+ * EDMA transfer linkage instead of spending CPU time copying pages.
  */
 #define MAX_CCNT        ((1 << 16) - 1)
 
-#define NR_SG           16
+#define MAX_NR_SG       16
 
 static unsigned rw_threshold = 32;
 module_param(rw_threshold, uint, S_IRUGO);
@@ -171,6 +171,7 @@ struct mmc_davinci_host {
 #define DAVINCI_MMC_DATADIR_READ        1
 #define DAVINCI_MMC_DATADIR_WRITE       2
         unsigned char data_dir;
+        unsigned char suspended;
 
         /* buffer is used during PIO of one scatterlist segment, and
          * is updated along with buffer_bytes_left.  bytes_left applies
@@ -192,7 +193,7 @@ struct mmc_davinci_host {
         struct edmacc_param     tx_template;
         struct edmacc_param     rx_template;
         unsigned                n_link;
-        u32                     links[NR_SG - 1];
+        u32                     links[MAX_NR_SG - 1];
 
         /* For PIO we walk scatterlists one segment at a time. */
         unsigned int            sg_len;
@@ -202,6 +203,8 @@ struct mmc_davinci_host {
         u8 version;
         /* for ns in one cycle calculation */
         unsigned ns_in_one_cycle;
+        /* Number of sg segments */
+        u8 nr_sg;
 #ifdef CONFIG_CPU_FREQ
         struct notifier_block   freq_transition;
 #endif
@@ -568,6 +571,7 @@ davinci_release_dma_channels(struct mmc_davinci_host *host)
 
 static int __init davinci_acquire_dma_channels(struct mmc_davinci_host *host)
 {
+        u32 link_size;
         int r, i;
 
         /* Acquire master DMA write channel */
@@ -593,7 +597,8 @@ static int __init davinci_acquire_dma_channels(struct mmc_davinci_host *host)
         /* Allocate parameter RAM slots, which will later be bound to a
          * channel as needed to handle a scatterlist.
          */
-        for (i = 0; i < ARRAY_SIZE(host->links); i++) {
+        link_size = min_t(unsigned, host->nr_sg, ARRAY_SIZE(host->links));
+        for (i = 0; i < link_size; i++) {
                 r = edma_alloc_slot(EDMA_CTLR(host->txdma), EDMA_SLOT_ANY);
                 if (r < 0) {
                         dev_dbg(mmc_dev(host->mmc), "dma PaRAM alloc --> %d\n",
@@ -905,19 +910,26 @@ static void mmc_davinci_cmd_done(struct mmc_davinci_host *host,
         }
 }
 
-static void
-davinci_abort_data(struct mmc_davinci_host *host, struct mmc_data *data)
+static inline void mmc_davinci_reset_ctrl(struct mmc_davinci_host *host,
+                                                                int val)
 {
         u32 temp;
 
-        /* reset command and data state machines */
         temp = readl(host->base + DAVINCI_MMCCTL);
-        writel(temp | MMCCTL_CMDRST | MMCCTL_DATRST,
-                host->base + DAVINCI_MMCCTL);
+        if (val)        /* reset */
+                temp |= MMCCTL_CMDRST | MMCCTL_DATRST;
+        else            /* enable */
+                temp &= ~(MMCCTL_CMDRST | MMCCTL_DATRST);
 
-        temp &= ~(MMCCTL_CMDRST | MMCCTL_DATRST);
-        udelay(10);
         writel(temp, host->base + DAVINCI_MMCCTL);
+        udelay(10);
+}
+
+static void
+davinci_abort_data(struct mmc_davinci_host *host, struct mmc_data *data)
+{
+        mmc_davinci_reset_ctrl(host, 1);
+        mmc_davinci_reset_ctrl(host, 0);
 }
 
 static irqreturn_t mmc_davinci_irq(int irq, void *dev_id)
@@ -1121,15 +1133,8 @@ static inline void mmc_davinci_cpufreq_deregister(struct mmc_davinci_host *host)
 #endif
 static void __init init_mmcsd_host(struct mmc_davinci_host *host)
 {
-        /* DAT line portion is diabled and in reset state */
-        writel(readl(host->base + DAVINCI_MMCCTL) | MMCCTL_DATRST,
-                host->base + DAVINCI_MMCCTL);
-
-        /* CMD line portion is diabled and in reset state */
-        writel(readl(host->base + DAVINCI_MMCCTL) | MMCCTL_CMDRST,
-                host->base + DAVINCI_MMCCTL);
 
-        udelay(10);
+        mmc_davinci_reset_ctrl(host, 1);
 
         writel(0, host->base + DAVINCI_MMCCLK);
         writel(MMCCLK_CLKEN, host->base + DAVINCI_MMCCLK);
@@ -1137,12 +1142,7 @@ static void __init init_mmcsd_host(struct mmc_davinci_host *host)
         writel(0x1FFF, host->base + DAVINCI_MMCTOR);
         writel(0xFFFF, host->base + DAVINCI_MMCTOD);
 
-        writel(readl(host->base + DAVINCI_MMCCTL) & ~MMCCTL_DATRST,
-                host->base + DAVINCI_MMCCTL);
-        writel(readl(host->base + DAVINCI_MMCCTL) & ~MMCCTL_CMDRST,
-                host->base + DAVINCI_MMCCTL);
-
-        udelay(10);
+        mmc_davinci_reset_ctrl(host, 0);
 }
 
 static int __init davinci_mmcsd_probe(struct platform_device *pdev)
@@ -1202,6 +1202,12 @@ static int __init davinci_mmcsd_probe(struct platform_device *pdev)
 
         init_mmcsd_host(host);
 
+        if (pdata->nr_sg)
+                host->nr_sg = pdata->nr_sg - 1;
+
+        if (host->nr_sg > MAX_NR_SG || !host->nr_sg)
+                host->nr_sg = MAX_NR_SG;
+
         host->use_dma = use_dma;
         host->irq = irq;
 
@@ -1327,32 +1333,65 @@ static int __exit davinci_mmcsd_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_PM
-static int davinci_mmcsd_suspend(struct platform_device *pdev, pm_message_t msg)
+static int davinci_mmcsd_suspend(struct device *dev)
 {
+        struct platform_device *pdev = to_platform_device(dev);
         struct mmc_davinci_host *host = platform_get_drvdata(pdev);
+        int ret;
 
-        return mmc_suspend_host(host->mmc, msg);
+        mmc_host_enable(host->mmc);
+        ret = mmc_suspend_host(host->mmc);
+        if (!ret) {
+                writel(0, host->base + DAVINCI_MMCIM);
+                mmc_davinci_reset_ctrl(host, 1);
+                mmc_host_disable(host->mmc);
+                clk_disable(host->clk);
+                host->suspended = 1;
+        } else {
+                host->suspended = 0;
+                mmc_host_disable(host->mmc);
+        }
+
+        return ret;
 }
 
-static int davinci_mmcsd_resume(struct platform_device *pdev)
+static int davinci_mmcsd_resume(struct device *dev)
 {
+        struct platform_device *pdev = to_platform_device(dev);
         struct mmc_davinci_host *host = platform_get_drvdata(pdev);
+        int ret;
+
+        if (!host->suspended)
+                return 0;
 
-        return mmc_resume_host(host->mmc);
+        clk_enable(host->clk);
+        mmc_host_enable(host->mmc);
+
+        mmc_davinci_reset_ctrl(host, 0);
+        ret = mmc_resume_host(host->mmc);
+        if (!ret)
+                host->suspended = 0;
+
+        return ret;
 }
+
+static const struct dev_pm_ops davinci_mmcsd_pm = {
+        .suspend        = davinci_mmcsd_suspend,
+        .resume         = davinci_mmcsd_resume,
+};
+
+#define davinci_mmcsd_pm_ops (&davinci_mmcsd_pm)
 #else
-#define davinci_mmcsd_suspend   NULL
-#define davinci_mmcsd_resume    NULL
+#define davinci_mmcsd_pm_ops NULL
 #endif
 
 static struct platform_driver davinci_mmcsd_driver = {
         .driver         = {
                 .name   = "davinci_mmc",
                 .owner  = THIS_MODULE,
+                .pm     = davinci_mmcsd_pm_ops,
         },
         .remove         = __exit_p(davinci_mmcsd_remove),
-        .suspend        = davinci_mmcsd_suspend,
-        .resume         = davinci_mmcsd_resume,
 };
 
 static int __init davinci_mmcsd_init(void)
diff --git a/drivers/mmc/host/imxmmc.c b/drivers/mmc/host/imxmmc.c
index bf98d7cc928a..9a68ff4353a2 100644
--- a/drivers/mmc/host/imxmmc.c
+++ b/drivers/mmc/host/imxmmc.c
@@ -1115,7 +1115,7 @@ static int imxmci_suspend(struct platform_device *dev, pm_message_t state)
         int ret = 0;
 
         if (mmc)
-                ret = mmc_suspend_host(mmc, state);
+                ret = mmc_suspend_host(mmc);
 
         return ret;
 }
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index ff115d920888..4917af96bae1 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -824,7 +824,7 @@ static int mmci_suspend(struct amba_device *dev, pm_message_t state)
         if (mmc) {
                 struct mmci_host *host = mmc_priv(mmc);
 
-                ret = mmc_suspend_host(mmc, state);
+                ret = mmc_suspend_host(mmc);
                 if (ret == 0)
                         writel(0, host->base + MMCIMASK0);
         }
diff --git a/drivers/mmc/host/msm_sdcc.c b/drivers/mmc/host/msm_sdcc.c
index 61f1d27fed3f..24e09454e522 100644
--- a/drivers/mmc/host/msm_sdcc.c
+++ b/drivers/mmc/host/msm_sdcc.c
@@ -1327,7 +1327,7 @@ msmsdcc_suspend(struct platform_device *dev, pm_message_t state)
                         disable_irq(host->stat_irq);
 
                 if (mmc->card && mmc->card->type != MMC_TYPE_SDIO)
-                        rc = mmc_suspend_host(mmc, state);
+                        rc = mmc_suspend_host(mmc);
                 if (!rc)
                         msmsdcc_writel(host, 0, MMCIMASK0);
                 if (host->clks_on)
diff --git a/drivers/mmc/host/mvsdio.c b/drivers/mmc/host/mvsdio.c
index 34e23489811a..366eefa77c5a 100644
--- a/drivers/mmc/host/mvsdio.c
+++ b/drivers/mmc/host/mvsdio.c
@@ -865,7 +865,7 @@ static int mvsd_suspend(struct platform_device *dev, pm_message_t state)
         int ret = 0;
 
         if (mmc)
-                ret = mmc_suspend_host(mmc, state);
+                ret = mmc_suspend_host(mmc);
 
         return ret;
 }
diff --git a/drivers/mmc/host/mxcmmc.c b/drivers/mmc/host/mxcmmc.c
index ec18e3b60342..d9d4a72e0ec7 100644
--- a/drivers/mmc/host/mxcmmc.c
+++ b/drivers/mmc/host/mxcmmc.c
@@ -932,7 +932,7 @@ static int mxcmci_suspend(struct platform_device *dev, pm_message_t state)
         int ret = 0;
 
         if (mmc)
-                ret = mmc_suspend_host(mmc, state);
+                ret = mmc_suspend_host(mmc);
 
         return ret;
 }
diff --git a/drivers/mmc/host/omap.c b/drivers/mmc/host/omap.c
index 84d280406341..d98ddcfac5e5 100644
--- a/drivers/mmc/host/omap.c
+++ b/drivers/mmc/host/omap.c
@@ -39,30 +39,30 @@
 #include <plat/fpga.h>
 
 #define OMAP_MMC_REG_CMD        0x00
-#define OMAP_MMC_REG_ARGL       0x04
-#define OMAP_MMC_REG_ARGH       0x08
-#define OMAP_MMC_REG_CON        0x0c
-#define OMAP_MMC_REG_STAT       0x10
-#define OMAP_MMC_REG_IE         0x14
-#define OMAP_MMC_REG_CTO        0x18
-#define OMAP_MMC_REG_DTO        0x1c
-#define OMAP_MMC_REG_DATA       0x20
-#define OMAP_MMC_REG_BLEN       0x24
-#define OMAP_MMC_REG_NBLK       0x28
-#define OMAP_MMC_REG_BUF        0x2c
-#define OMAP_MMC_REG_SDIO       0x34
-#define OMAP_MMC_REG_REV        0x3c
-#define OMAP_MMC_REG_RSP0       0x40
-#define OMAP_MMC_REG_RSP1       0x44
-#define OMAP_MMC_REG_RSP2       0x48
-#define OMAP_MMC_REG_RSP3       0x4c
-#define OMAP_MMC_REG_RSP4       0x50
-#define OMAP_MMC_REG_RSP5       0x54
-#define OMAP_MMC_REG_RSP6       0x58
-#define OMAP_MMC_REG_RSP7       0x5c
-#define OMAP_MMC_REG_IOSR       0x60
-#define OMAP_MMC_REG_SYSC       0x64
-#define OMAP_MMC_REG_SYSS       0x68
+#define OMAP_MMC_REG_ARGL       0x01
+#define OMAP_MMC_REG_ARGH       0x02
+#define OMAP_MMC_REG_CON        0x03
+#define OMAP_MMC_REG_STAT       0x04
+#define OMAP_MMC_REG_IE         0x05
+#define OMAP_MMC_REG_CTO        0x06
+#define OMAP_MMC_REG_DTO        0x07
+#define OMAP_MMC_REG_DATA       0x08
+#define OMAP_MMC_REG_BLEN       0x09
+#define OMAP_MMC_REG_NBLK       0x0a
+#define OMAP_MMC_REG_BUF        0x0b
+#define OMAP_MMC_REG_SDIO       0x0d
+#define OMAP_MMC_REG_REV        0x0f
+#define OMAP_MMC_REG_RSP0       0x10
+#define OMAP_MMC_REG_RSP1       0x11
+#define OMAP_MMC_REG_RSP2       0x12
+#define OMAP_MMC_REG_RSP3       0x13
+#define OMAP_MMC_REG_RSP4       0x14
+#define OMAP_MMC_REG_RSP5       0x15
+#define OMAP_MMC_REG_RSP6       0x16
+#define OMAP_MMC_REG_RSP7       0x17
+#define OMAP_MMC_REG_IOSR       0x18
+#define OMAP_MMC_REG_SYSC       0x19
+#define OMAP_MMC_REG_SYSS       0x1a
 
 #define OMAP_MMC_STAT_CARD_ERR          (1 << 14)
 #define OMAP_MMC_STAT_CARD_IRQ          (1 << 13)
@@ -78,8 +78,9 @@
 #define OMAP_MMC_STAT_CARD_BUSY         (1 <<  2)
 #define OMAP_MMC_STAT_END_OF_CMD        (1 <<  0)
 
-#define OMAP_MMC_READ(host, reg)        __raw_readw((host)->virt_base + OMAP_MMC_REG_##reg)
-#define OMAP_MMC_WRITE(host, reg, val)  __raw_writew((val), (host)->virt_base + OMAP_MMC_REG_##reg)
+#define OMAP_MMC_REG(host, reg)         (OMAP_MMC_REG_##reg << (host)->reg_shift)
+#define OMAP_MMC_READ(host, reg)        __raw_readw((host)->virt_base + OMAP_MMC_REG(host, reg))
+#define OMAP_MMC_WRITE(host, reg, val)  __raw_writew((val), (host)->virt_base + OMAP_MMC_REG(host, reg))
 
 /*
  * Command types
@@ -133,6 +134,7 @@ struct mmc_omap_host {
         int                     irq;
         unsigned char           bus_mode;
         unsigned char           hw_bus_mode;
+        unsigned int            reg_shift;
 
         struct work_struct      cmd_abort_work;
         unsigned                abort:1;
@@ -680,9 +682,9 @@ mmc_omap_xfer_data(struct mmc_omap_host *host, int write)
         host->data->bytes_xfered += n;
 
         if (write) {
-                __raw_writesw(host->virt_base + OMAP_MMC_REG_DATA, host->buffer, n);
+                __raw_writesw(host->virt_base + OMAP_MMC_REG(host, DATA), host->buffer, n);
         } else {
-                __raw_readsw(host->virt_base + OMAP_MMC_REG_DATA, host->buffer, n);
+                __raw_readsw(host->virt_base + OMAP_MMC_REG(host, DATA), host->buffer, n);
         }
 }
 
@@ -900,7 +902,7 @@ mmc_omap_prepare_dma(struct mmc_omap_host *host, struct mmc_data *data)
         int dst_port = 0;
         int sync_dev = 0;
 
-        data_addr = host->phys_base + OMAP_MMC_REG_DATA;
+        data_addr = host->phys_base + OMAP_MMC_REG(host, DATA);
         frame = data->blksz;
         count = sg_dma_len(sg);
 
@@ -1155,7 +1157,6 @@ static void mmc_omap_start_request(struct mmc_omap_host *host,
         mmc_omap_start_command(host, req->cmd);
         if (host->dma_in_use)
                 omap_start_dma(host->dma_ch);
-        BUG_ON(irqs_disabled());
 }
 
 static void mmc_omap_request(struct mmc_host *mmc, struct mmc_request *req)
@@ -1493,6 +1494,8 @@ static int __init mmc_omap_probe(struct platform_device *pdev)
                 }
         }
 
+        host->reg_shift = (cpu_is_omap7xx() ? 1 : 2);
+
         return 0;
 
 err_plat_cleanup:
@@ -1557,7 +1560,7 @@ static int mmc_omap_suspend(struct platform_device *pdev, pm_message_t mesg)
                 struct mmc_omap_slot *slot;
 
                 slot = host->slots[i];
-                ret = mmc_suspend_host(slot->mmc, mesg);
+                ret = mmc_suspend_host(slot->mmc);
                 if (ret < 0) {
                         while (--i >= 0) {
                                 slot = host->slots[i];
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index e9caf694c59e..b032828c6126 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -157,12 +157,10 @@ struct omap_hsmmc_host {
          */
         struct  regulator       *vcc;
         struct  regulator       *vcc_aux;
-        struct  semaphore       sem;
         struct  work_struct     mmc_carddetect_work;
         void    __iomem         *base;
         resource_size_t         mapbase;
         spinlock_t              irq_lock; /* Prevent races with irq handler */
-        unsigned long           flags;
         unsigned int            id;
         unsigned int            dma_len;
         unsigned int            dma_sg_idx;
@@ -183,6 +181,7 @@ struct omap_hsmmc_host {
         int                     protect_card;
         int                     reqs_blocked;
         int                     use_reg;
+        int                     req_in_progress;
 
         struct  omap_mmc_platform_data  *pdata;
 };
@@ -524,6 +523,27 @@ static void omap_hsmmc_stop_clock(struct omap_hsmmc_host *host)
                 dev_dbg(mmc_dev(host->mmc), "MMC Clock is not stoped\n");
 }
 
+static void omap_hsmmc_enable_irq(struct omap_hsmmc_host *host)
+{
+        unsigned int irq_mask;
+
+        if (host->use_dma)
+                irq_mask = INT_EN_MASK & ~(BRR_ENABLE | BWR_ENABLE);
+        else
+                irq_mask = INT_EN_MASK;
+
+        OMAP_HSMMC_WRITE(host->base, STAT, STAT_CLEAR);
+        OMAP_HSMMC_WRITE(host->base, ISE, irq_mask);
+        OMAP_HSMMC_WRITE(host->base, IE, irq_mask);
+}
+
+static void omap_hsmmc_disable_irq(struct omap_hsmmc_host *host)
+{
+        OMAP_HSMMC_WRITE(host->base, ISE, 0);
+        OMAP_HSMMC_WRITE(host->base, IE, 0);
+        OMAP_HSMMC_WRITE(host->base, STAT, STAT_CLEAR);
+}
+
 #ifdef CONFIG_PM
 
 /*
@@ -592,9 +612,7 @@ static int omap_hsmmc_context_restore(struct omap_hsmmc_host *host)
                 && time_before(jiffies, timeout))
                 ;
 
-        OMAP_HSMMC_WRITE(host->base, STAT, STAT_CLEAR);
-        OMAP_HSMMC_WRITE(host->base, ISE, INT_EN_MASK);
-        OMAP_HSMMC_WRITE(host->base, IE, INT_EN_MASK);
+        omap_hsmmc_disable_irq(host);
 
         /* Do not initialize card-specific things if the power is off */
         if (host->power_mode == MMC_POWER_OFF)
@@ -697,6 +715,8 @@ static void send_init_stream(struct omap_hsmmc_host *host)
                 return;
 
         disable_irq(host->irq);
+
+        OMAP_HSMMC_WRITE(host->base, IE, INT_EN_MASK);
         OMAP_HSMMC_WRITE(host->base, CON,
                 OMAP_HSMMC_READ(host->base, CON) | INIT_STREAM);
         OMAP_HSMMC_WRITE(host->base, CMD, INIT_STREAM_CMD);
@@ -762,17 +782,7 @@ omap_hsmmc_start_command(struct omap_hsmmc_host *host, struct mmc_command *cmd,
                 mmc_hostname(host->mmc), cmd->opcode, cmd->arg);
         host->cmd = cmd;
 
-        /*
-         * Clear status bits and enable interrupts
-         */
-        OMAP_HSMMC_WRITE(host->base, STAT, STAT_CLEAR);
-        OMAP_HSMMC_WRITE(host->base, ISE, INT_EN_MASK);
-
-        if (host->use_dma)
-                OMAP_HSMMC_WRITE(host->base, IE,
-                                 INT_EN_MASK & ~(BRR_ENABLE | BWR_ENABLE));
-        else
-                OMAP_HSMMC_WRITE(host->base, IE, INT_EN_MASK);
+        omap_hsmmc_enable_irq(host);
 
         host->response_busy = 0;
         if (cmd->flags & MMC_RSP_PRESENT) {
@@ -806,13 +816,7 @@ omap_hsmmc_start_command(struct omap_hsmmc_host *host, struct mmc_command *cmd,
         if (host->use_dma)
                 cmdreg |= DMA_EN;
 
-        /*
-         * In an interrupt context (i.e. STOP command), the spinlock is unlocked
-         * by the interrupt handler, otherwise (i.e. for a new request) it is
-         * unlocked here.
-         */
-        if (!in_interrupt())
-                spin_unlock_irqrestore(&host->irq_lock, host->flags);
+        host->req_in_progress = 1;
 
         OMAP_HSMMC_WRITE(host->base, ARG, cmd->arg);
         OMAP_HSMMC_WRITE(host->base, CMD, cmdreg);
@@ -827,6 +831,23 @@ omap_hsmmc_get_dma_dir(struct omap_hsmmc_host *host, struct mmc_data *data)
                 return DMA_FROM_DEVICE;
 }
 
+static void omap_hsmmc_request_done(struct omap_hsmmc_host *host, struct mmc_request *mrq)
+{
+        int dma_ch;
+
+        spin_lock(&host->irq_lock);
+        host->req_in_progress = 0;
+        dma_ch = host->dma_ch;
+        spin_unlock(&host->irq_lock);
+
+        omap_hsmmc_disable_irq(host);
+        /* Do not complete the request if DMA is still in progress */
+        if (mrq->data && host->use_dma && dma_ch != -1)
+                return;
+        host->mrq = NULL;
+        mmc_request_done(host->mmc, mrq);
+}
+
 /*
  * Notify the transfer complete to MMC core
  */
@@ -843,25 +864,19 @@ omap_hsmmc_xfer_done(struct omap_hsmmc_host *host, struct mmc_data *data)
                         return;
                 }
 
-                host->mrq = NULL;
-                mmc_request_done(host->mmc, mrq);
+                omap_hsmmc_request_done(host, mrq);
                 return;
         }
 
         host->data = NULL;
 
-        if (host->use_dma && host->dma_ch != -1)
-                dma_unmap_sg(mmc_dev(host->mmc), data->sg, host->dma_len,
-                        omap_hsmmc_get_dma_dir(host, data));
-
         if (!data->error)
                 data->bytes_xfered += data->blocks * (data->blksz);
         else
                 data->bytes_xfered = 0;
 
         if (!data->stop) {
-                host->mrq = NULL;
-                mmc_request_done(host->mmc, data->mrq);
+                omap_hsmmc_request_done(host, data->mrq);
                 return;
         }
         omap_hsmmc_start_command(host, data->stop, NULL);
@@ -887,10 +902,8 @@ omap_hsmmc_cmd_done(struct omap_hsmmc_host *host, struct mmc_command *cmd)
                         cmd->resp[0] = OMAP_HSMMC_READ(host->base, RSP10);
                 }
         }
-        if ((host->data == NULL && !host->response_busy) || cmd->error) {
-                host->mrq = NULL;
-                mmc_request_done(host->mmc, cmd->mrq);
-        }
+        if ((host->data == NULL && !host->response_busy) || cmd->error)
+                omap_hsmmc_request_done(host, cmd->mrq);
 }
 
 /*
@@ -898,14 +911,19 @@ omap_hsmmc_cmd_done(struct omap_hsmmc_host *host, struct mmc_command *cmd)
  */
 static void omap_hsmmc_dma_cleanup(struct omap_hsmmc_host *host, int errno)
 {
+        int dma_ch;
+
         host->data->error = errno;
 
-        if (host->use_dma && host->dma_ch != -1) {
+        spin_lock(&host->irq_lock);
+        dma_ch = host->dma_ch;
+        host->dma_ch = -1;
+        spin_unlock(&host->irq_lock);
+
+        if (host->use_dma && dma_ch != -1) {
                 dma_unmap_sg(mmc_dev(host->mmc), host->data->sg, host->dma_len,
                         omap_hsmmc_get_dma_dir(host, host->data));
-                omap_free_dma(host->dma_ch);
-                host->dma_ch = -1;
-                up(&host->sem);
+                omap_free_dma(dma_ch);
         }
         host->data = NULL;
 }
@@ -967,28 +985,21 @@ static inline void omap_hsmmc_reset_controller_fsm(struct omap_hsmmc_host *host,
                         __func__);
 }
 
-/*
- * MMC controller IRQ handler
- */
-static irqreturn_t omap_hsmmc_irq(int irq, void *dev_id)
+static void omap_hsmmc_do_irq(struct omap_hsmmc_host *host, int status)
 {
-        struct omap_hsmmc_host *host = dev_id;
         struct mmc_data *data;
-        int end_cmd = 0, end_trans = 0, status;
-
-        spin_lock(&host->irq_lock);
-
-        if (host->mrq == NULL) {
-                OMAP_HSMMC_WRITE(host->base, STAT,
-                        OMAP_HSMMC_READ(host->base, STAT));
-                /* Flush posted write */
-                OMAP_HSMMC_READ(host->base, STAT);
-                spin_unlock(&host->irq_lock);
-                return IRQ_HANDLED;
+        int end_cmd = 0, end_trans = 0;
+
+        if (!host->req_in_progress) {
+                do {
+                        OMAP_HSMMC_WRITE(host->base, STAT, status);
+                        /* Flush posted write */
+                        status = OMAP_HSMMC_READ(host->base, STAT);
+                } while (status & INT_EN_MASK);
+                return;
         }
 
         data = host->data;
-        status = OMAP_HSMMC_READ(host->base, STAT);
         dev_dbg(mmc_dev(host->mmc), "IRQ Status is %x\n", status);
 
         if (status & ERR) {
@@ -1041,15 +1052,27 @@ static irqreturn_t omap_hsmmc_irq(int irq, void *dev_id)
         }
 
         OMAP_HSMMC_WRITE(host->base, STAT, status);
-        /* Flush posted write */
-        OMAP_HSMMC_READ(host->base, STAT);
 
         if (end_cmd || ((status & CC) && host->cmd))
                 omap_hsmmc_cmd_done(host, host->cmd);
         if ((end_trans || (status & TC)) && host->mrq)
                 omap_hsmmc_xfer_done(host, data);
+}
 
-        spin_unlock(&host->irq_lock);
+/*
+ * MMC controller IRQ handler
+ */
+static irqreturn_t omap_hsmmc_irq(int irq, void *dev_id)
+{
+        struct omap_hsmmc_host *host = dev_id;
+        int status;
+
+        status = OMAP_HSMMC_READ(host->base, STAT);
+        do {
+                omap_hsmmc_do_irq(host, status);
+                /* Flush posted write */
+                status = OMAP_HSMMC_READ(host->base, STAT);
+        } while (status & INT_EN_MASK);
 
         return IRQ_HANDLED;
 }
@@ -1244,31 +1267,47 @@ static void omap_hsmmc_config_dma_params(struct omap_hsmmc_host *host,
 /*
  * DMA call back function
  */
-static void omap_hsmmc_dma_cb(int lch, u16 ch_status, void *data)
+static void omap_hsmmc_dma_cb(int lch, u16 ch_status, void *cb_data)
 {
-        struct omap_hsmmc_host *host = data;
+        struct omap_hsmmc_host *host = cb_data;
+        struct mmc_data *data = host->mrq->data;
+        int dma_ch, req_in_progress;
 
         if (ch_status & OMAP2_DMA_MISALIGNED_ERR_IRQ)
                 dev_dbg(mmc_dev(host->mmc), "MISALIGNED_ADRS_ERR\n");
 
-        if (host->dma_ch < 0)
+        spin_lock(&host->irq_lock);
+        if (host->dma_ch < 0) {
+                spin_unlock(&host->irq_lock);
                 return;
+        }
 
         host->dma_sg_idx++;
         if (host->dma_sg_idx < host->dma_len) {
                 /* Fire up the next transfer. */
-                omap_hsmmc_config_dma_params(host, host->data,
-                                           host->data->sg + host->dma_sg_idx);
+                omap_hsmmc_config_dma_params(host, data,
+                                           data->sg + host->dma_sg_idx);
+                spin_unlock(&host->irq_lock);
                 return;
         }
 
-        omap_free_dma(host->dma_ch);
+        dma_unmap_sg(mmc_dev(host->mmc), data->sg, host->dma_len,
+                omap_hsmmc_get_dma_dir(host, data));
+
+        req_in_progress = host->req_in_progress;
+        dma_ch = host->dma_ch;
         host->dma_ch = -1;
-        /*
-         * DMA Callback: run in interrupt context.
-         * mutex_unlock will throw a kernel warning if used.
-         */
-        up(&host->sem);
+        spin_unlock(&host->irq_lock);
+
+        omap_free_dma(dma_ch);
+
+        /* If DMA has finished after TC, complete the request */
+        if (!req_in_progress) {
+                struct mmc_request *mrq = host->mrq;
+
+                host->mrq = NULL;
+                mmc_request_done(host->mmc, mrq);
+        }
 }
 
 /*
@@ -1277,7 +1316,7 @@ static void omap_hsmmc_dma_cb(int lch, u16 ch_status, void *data)
 static int omap_hsmmc_start_dma_transfer(struct omap_hsmmc_host *host,
                                         struct mmc_request *req)
 {
-        int dma_ch = 0, ret = 0, err = 1, i;
+        int dma_ch = 0, ret = 0, i;
         struct mmc_data *data = req->data;
 
         /* Sanity check: all the SG entries must be aligned by block size. */
@@ -1294,23 +1333,7 @@ static int omap_hsmmc_start_dma_transfer(struct omap_hsmmc_host *host,
                  */
                 return -EINVAL;
 
-        /*
-         * If for some reason the DMA transfer is still active,
-         * we wait for timeout period and free the dma
-         */
-        if (host->dma_ch != -1) {
-                set_current_state(TASK_UNINTERRUPTIBLE);
-                schedule_timeout(100);
-                if (down_trylock(&host->sem)) {
-                        omap_free_dma(host->dma_ch);
-                        host->dma_ch = -1;
-                        up(&host->sem);
-                        return err;
-                }
-        } else {
-                if (down_trylock(&host->sem))
-                        return err;
-        }
+        BUG_ON(host->dma_ch != -1);
 
         ret = omap_request_dma(omap_hsmmc_get_dma_sync_dev(host, data),
                                "MMC/SD", omap_hsmmc_dma_cb, host, &dma_ch);
@@ -1410,37 +1433,27 @@ static void omap_hsmmc_request(struct mmc_host *mmc, struct mmc_request *req)
         struct omap_hsmmc_host *host = mmc_priv(mmc);
         int err;
 
-        /*
-         * Prevent races with the interrupt handler because of unexpected
-         * interrupts, but not if we are already in interrupt context i.e.
-         * retries.
-         */
-        if (!in_interrupt()) {
-                spin_lock_irqsave(&host->irq_lock, host->flags);
-                /*
-                 * Protect the card from I/O if there is a possibility
-                 * it can be removed.
-                 */
-                if (host->protect_card) {
-                        if (host->reqs_blocked < 3) {
-                                /*
-                                 * Ensure the controller is left in a consistent
-                                 * state by resetting the command and data state
-                                 * machines.
-                                 */
-                                omap_hsmmc_reset_controller_fsm(host, SRD);
-                                omap_hsmmc_reset_controller_fsm(host, SRC);
-                                host->reqs_blocked += 1;
-                        }
-                        req->cmd->error = -EBADF;
-                        if (req->data)
-                                req->data->error = -EBADF;
-                        spin_unlock_irqrestore(&host->irq_lock, host->flags);
-                        mmc_request_done(mmc, req);
-                        return;
-                } else if (host->reqs_blocked)
-                        host->reqs_blocked = 0;
-        }
+        BUG_ON(host->req_in_progress);
+        BUG_ON(host->dma_ch != -1);
+        if (host->protect_card) {
+                if (host->reqs_blocked < 3) {
+                        /*
+                         * Ensure the controller is left in a consistent
+                         * state by resetting the command and data state
+                         * machines.
+                         */
+                        omap_hsmmc_reset_controller_fsm(host, SRD);
+                        omap_hsmmc_reset_controller_fsm(host, SRC);
+                        host->reqs_blocked += 1;
+                }
+                req->cmd->error = -EBADF;
+                if (req->data)
+                        req->data->error = -EBADF;
+                req->cmd->retries = 0;
+                mmc_request_done(mmc, req);
+                return;
+        } else if (host->reqs_blocked)
+                host->reqs_blocked = 0;
         WARN_ON(host->mrq != NULL);
         host->mrq = req;
         err = omap_hsmmc_prepare_data(host, req);
@@ -1449,8 +1462,6 @@ static void omap_hsmmc_request(struct mmc_host *mmc, struct mmc_request *req)
                 if (req->data)
                         req->data->error = err;
                 host->mrq = NULL;
-                if (!in_interrupt())
-                        spin_unlock_irqrestore(&host->irq_lock, host->flags);
                 mmc_request_done(mmc, req);
                 return;
         }
@@ -2019,7 +2030,6 @@ static int __init omap_hsmmc_probe(struct platform_device *pdev)
         mmc->f_min      = 400000;
         mmc->f_max      = 52000000;
 
-        sema_init(&host->sem, 1);
         spin_lock_init(&host->irq_lock);
 
         host->iclk = clk_get(&pdev->dev, "ick");
@@ -2162,8 +2172,7 @@ static int __init omap_hsmmc_probe(struct platform_device *pdev)
                 }
         }
 
-        OMAP_HSMMC_WRITE(host->base, ISE, INT_EN_MASK);
-        OMAP_HSMMC_WRITE(host->base, IE, INT_EN_MASK);
+        omap_hsmmc_disable_irq(host);
 
         mmc_host_lazy_disable(host->mmc);
 
@@ -2258,10 +2267,12 @@ static int omap_hsmmc_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_PM
-static int omap_hsmmc_suspend(struct platform_device *pdev, pm_message_t state)
+static int omap_hsmmc_suspend(struct device *dev)
 {
         int ret = 0;
+        struct platform_device *pdev = to_platform_device(dev);
         struct omap_hsmmc_host *host = platform_get_drvdata(pdev);
+        pm_message_t state = PMSG_SUSPEND; /* unused by MMC core */
 
         if (host && host->suspended)
                 return 0;
@@ -2281,12 +2292,9 @@ static int omap_hsmmc_suspend(struct platform_device *pdev, pm_message_t state)
                 }
                 cancel_work_sync(&host->mmc_carddetect_work);
                 mmc_host_enable(host->mmc);
-                ret = mmc_suspend_host(host->mmc, state);
+                ret = mmc_suspend_host(host->mmc);
                 if (ret == 0) {
-                        OMAP_HSMMC_WRITE(host->base, ISE, 0);
-                        OMAP_HSMMC_WRITE(host->base, IE, 0);
-
-
+                        omap_hsmmc_disable_irq(host);
                         OMAP_HSMMC_WRITE(host->base, HCTL,
                                 OMAP_HSMMC_READ(host->base, HCTL) & ~SDBP);
                         mmc_host_disable(host->mmc);
@@ -2310,9 +2318,10 @@ static int omap_hsmmc_suspend(struct platform_device *pdev, pm_message_t state)
 }
 
 /* Routine to resume the MMC device */
-static int omap_hsmmc_resume(struct platform_device *pdev)
+static int omap_hsmmc_resume(struct device *dev)
 {
         int ret = 0;
+        struct platform_device *pdev = to_platform_device(dev);
         struct omap_hsmmc_host *host = platform_get_drvdata(pdev);
 
         if (host && !host->suspended)
@@ -2363,13 +2372,17 @@ clk_en_err:
 #define omap_hsmmc_resume               NULL
 #endif
 
-static struct platform_driver omap_hsmmc_driver = {
-        .remove         = omap_hsmmc_remove,
+static struct dev_pm_ops omap_hsmmc_dev_pm_ops = {
         .suspend        = omap_hsmmc_suspend,
         .resume         = omap_hsmmc_resume,
+};
+
+static struct platform_driver omap_hsmmc_driver = {
+        .remove         = omap_hsmmc_remove,
         .driver         = {
                 .name = DRIVER_NAME,
                 .owner = THIS_MODULE,
+                .pm = &omap_hsmmc_dev_pm_ops,
         },
 };
 
diff --git a/drivers/mmc/host/pxamci.c b/drivers/mmc/host/pxamci.c
index e4f00e70a749..0a4e43f37140 100644
--- a/drivers/mmc/host/pxamci.c
+++ b/drivers/mmc/host/pxamci.c
@@ -813,7 +813,7 @@ static int pxamci_suspend(struct device *dev)
         int ret = 0;
 
         if (mmc)
-                ret = mmc_suspend_host(mmc, PMSG_SUSPEND);
+                ret = mmc_suspend_host(mmc);
 
         return ret;
 }
diff --git a/drivers/mmc/host/s3cmci.c b/drivers/mmc/host/s3cmci.c
index 2fdf7689ae6c..2e16e0a90a5e 100644
--- a/drivers/mmc/host/s3cmci.c
+++ b/drivers/mmc/host/s3cmci.c
@@ -1881,9 +1881,8 @@ MODULE_DEVICE_TABLE(platform, s3cmci_driver_ids);
 static int s3cmci_suspend(struct device *dev)
 {
         struct mmc_host *mmc = platform_get_drvdata(to_platform_device(dev));
-        struct pm_message event = { PM_EVENT_SUSPEND };
 
-        return mmc_suspend_host(mmc, event);
+        return mmc_suspend_host(mmc);
 }
 
 static int s3cmci_resume(struct device *dev)
diff --git a/drivers/mmc/host/sdhci-of-core.c b/drivers/mmc/host/sdhci-of-core.c
index 7802a543d8fc..a2e9820cd42f 100644
--- a/drivers/mmc/host/sdhci-of-core.c
+++ b/drivers/mmc/host/sdhci-of-core.c
@@ -89,7 +89,7 @@ static int sdhci_of_suspend(struct of_device *ofdev, pm_message_t state)
 {
         struct sdhci_host *host = dev_get_drvdata(&ofdev->dev);
 
-        return mmc_suspend_host(host->mmc, state);
+        return mmc_suspend_host(host->mmc);
 }
 
 static int sdhci_of_resume(struct of_device *ofdev)
diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index d5b11a17e648..c8623de13af3 100644
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -129,12 +129,12 @@ struct sdhci_of_data sdhci_esdhc = {
                   SDHCI_QUIRK_RESTORE_IRQS_AFTER_RESET |
                   SDHCI_QUIRK_NO_CARD_NO_RESET,
         .ops = {
-                .readl = sdhci_be32bs_readl,
-                .readw = esdhc_readw,
-                .readb = sdhci_be32bs_readb,
-                .writel = sdhci_be32bs_writel,
-                .writew = esdhc_writew,
-                .writeb = esdhc_writeb,
+                .read_l = sdhci_be32bs_readl,
+                .read_w = esdhc_readw,
+                .read_b = sdhci_be32bs_readb,
+                .write_l = sdhci_be32bs_writel,
+                .write_w = esdhc_writew,
+                .write_b = esdhc_writeb,
                 .set_clock = esdhc_set_clock,
                 .enable_dma = esdhc_enable_dma,
                 .get_max_clock = esdhc_get_max_clock,
diff --git a/drivers/mmc/host/sdhci-of-hlwd.c b/drivers/mmc/host/sdhci-of-hlwd.c
index 35117f3ed757..68ddb7546ae2 100644
--- a/drivers/mmc/host/sdhci-of-hlwd.c
+++ b/drivers/mmc/host/sdhci-of-hlwd.c
@@ -55,11 +55,11 @@ struct sdhci_of_data sdhci_hlwd = {
         .quirks = SDHCI_QUIRK_32BIT_DMA_ADDR |
                   SDHCI_QUIRK_32BIT_DMA_SIZE,
         .ops = {
-                .readl = sdhci_be32bs_readl,
-                .readw = sdhci_be32bs_readw,
-                .readb = sdhci_be32bs_readb,
-                .writel = sdhci_hlwd_writel,
-                .writew = sdhci_hlwd_writew,
-                .writeb = sdhci_hlwd_writeb,
+                .read_l = sdhci_be32bs_readl,
+                .read_w = sdhci_be32bs_readw,
+                .read_b = sdhci_be32bs_readb,
+                .write_l = sdhci_hlwd_writel,
+                .write_w = sdhci_hlwd_writew,
+                .write_b = sdhci_hlwd_writeb,
         },
 };
diff --git a/drivers/mmc/host/sdhci-pci.c b/drivers/mmc/host/sdhci-pci.c
index 6701af629c30..65483fdea45b 100644
--- a/drivers/mmc/host/sdhci-pci.c
+++ b/drivers/mmc/host/sdhci-pci.c
@@ -628,7 +628,7 @@ static struct sdhci_pci_slot * __devinit sdhci_pci_probe_slot(
         host = sdhci_alloc_host(&pdev->dev, sizeof(struct sdhci_pci_slot));
         if (IS_ERR(host)) {
                 dev_err(&pdev->dev, "cannot allocate host\n");
-                return ERR_PTR(PTR_ERR(host));
+                return ERR_CAST(host);
         }
 
         slot = sdhci_priv(host);
diff --git a/drivers/mmc/host/sdhci-pltfm.c b/drivers/mmc/host/sdhci-pltfm.c
index 297f40ae6ad5..b6ee0d719698 100644
--- a/drivers/mmc/host/sdhci-pltfm.c
+++ b/drivers/mmc/host/sdhci-pltfm.c
@@ -29,6 +29,7 @@
 #include <linux/mmc/host.h>
 
 #include <linux/io.h>
+#include <linux/sdhci-pltfm.h>
 
 #include "sdhci.h"
 
@@ -49,19 +50,18 @@ static struct sdhci_ops sdhci_pltfm_ops = {
 
 static int __devinit sdhci_pltfm_probe(struct platform_device *pdev)
 {
+        struct sdhci_pltfm_data *pdata = pdev->dev.platform_data;
         struct sdhci_host *host;
         struct resource *iomem;
         int ret;
 
-        BUG_ON(pdev == NULL);
-
         iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         if (!iomem) {
                 ret = -ENOMEM;
                 goto err;
         }
 
-        if (resource_size(iomem) != 0x100)
+        if (resource_size(iomem) < 0x100)
                 dev_err(&pdev->dev, "Invalid iomem size. You may "
                         "experience problems.\n");
 
@@ -76,7 +76,12 @@ static int __devinit sdhci_pltfm_probe(struct platform_device *pdev)
         }
 
         host->hw_name = "platform";
-        host->ops = &sdhci_pltfm_ops;
+        if (pdata && pdata->ops)
+                host->ops = pdata->ops;
+        else
+                host->ops = &sdhci_pltfm_ops;
+        if (pdata)
+                host->quirks = pdata->quirks;
         host->irq = platform_get_irq(pdev, 0);
 
         if (!request_mem_region(iomem->start, resource_size(iomem),
@@ -93,6 +98,12 @@ static int __devinit sdhci_pltfm_probe(struct platform_device *pdev)
                 goto err_remap;
         }
 
+        if (pdata && pdata->init) {
+                ret = pdata->init(host);
+                if (ret)
+                        goto err_plat_init;
+        }
+
         ret = sdhci_add_host(host);
         if (ret)
                 goto err_add_host;
@@ -102,6 +113,9 @@ static int __devinit sdhci_pltfm_probe(struct platform_device *pdev)
         return 0;
 
 err_add_host:
+        if (pdata && pdata->exit)
+                pdata->exit(host);
+err_plat_init:
         iounmap(host->ioaddr);
 err_remap:
         release_mem_region(iomem->start, resource_size(iomem));
@@ -114,6 +128,7 @@ err:
 
 static int __devexit sdhci_pltfm_remove(struct platform_device *pdev)
 {
+        struct sdhci_pltfm_data *pdata = pdev->dev.platform_data;
         struct sdhci_host *host = platform_get_drvdata(pdev);
         struct resource *iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         int dead;
@@ -125,6 +140,8 @@ static int __devexit sdhci_pltfm_remove(struct platform_device *pdev)
                 dead = 1;
 
         sdhci_remove_host(host, dead);
+        if (pdata && pdata->exit)
+                pdata->exit(host);
         iounmap(host->ioaddr);
         release_mem_region(iomem->start, resource_size(iomem));
         sdhci_free_host(host);
@@ -165,4 +182,3 @@ MODULE_DESCRIPTION("Secure Digital Host Controller Interface platform driver");
 MODULE_AUTHOR("Mocean Laboratories <info@mocean-labs.com>");
 MODULE_LICENSE("GPL v2");
 MODULE_ALIAS("platform:sdhci");
-
diff --git a/drivers/mmc/host/sdhci-s3c.c b/drivers/mmc/host/sdhci-s3c.c
index 2136794c0cfa..af217924a76e 100644
--- a/drivers/mmc/host/sdhci-s3c.c
+++ b/drivers/mmc/host/sdhci-s3c.c
@@ -317,12 +317,7 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
         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;
+        host->quirks |= SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC;
 
 #ifndef CONFIG_MMC_SDHCI_S3C_DMA
 
@@ -330,9 +325,6 @@ static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
          * 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
diff --git a/drivers/mmc/host/sdhci-spear.c b/drivers/mmc/host/sdhci-spear.c
new file mode 100644
index 000000000000..d70c54c7b70a
--- /dev/null
+++ b/drivers/mmc/host/sdhci-spear.c
@@ -0,0 +1,298 @@
+/*
+ * drivers/mmc/host/sdhci-spear.c
+ *
+ * Support of SDHCI platform devices for spear soc family
+ *
+ * Copyright (C) 2010 ST Microelectronics
+ * Viresh Kumar<viresh.kumar@st.com>
+ *
+ * Inspired by sdhci-pltfm.c
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/highmem.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/sdhci-spear.h>
+#include <linux/io.h>
+#include "sdhci.h"
+
+struct spear_sdhci {
+        struct clk *clk;
+        struct sdhci_plat_data *data;
+};
+
+/* sdhci ops */
+static struct sdhci_ops sdhci_pltfm_ops = {
+        /* Nothing to do for now. */
+};
+
+/* gpio card detection interrupt handler */
+static irqreturn_t sdhci_gpio_irq(int irq, void *dev_id)
+{
+        struct platform_device *pdev = dev_id;
+        struct sdhci_host *host = platform_get_drvdata(pdev);
+        struct spear_sdhci *sdhci = dev_get_platdata(&pdev->dev);
+        unsigned long gpio_irq_type;
+        int val;
+
+        val = gpio_get_value(sdhci->data->card_int_gpio);
+
+        /* val == 1 -> card removed, val == 0 -> card inserted */
+        /* if card removed - set irq for low level, else vice versa */
+        gpio_irq_type = val ? IRQF_TRIGGER_LOW : IRQF_TRIGGER_HIGH;
+        set_irq_type(irq, gpio_irq_type);
+
+        if (sdhci->data->card_power_gpio >= 0) {
+                if (!sdhci->data->power_always_enb) {
+                        /* if card inserted, give power, otherwise remove it */
+                        val = sdhci->data->power_active_high ? !val : val ;
+                        gpio_set_value(sdhci->data->card_power_gpio, val);
+                }
+        }
+
+        /* inform sdhci driver about card insertion/removal */
+        tasklet_schedule(&host->card_tasklet);
+
+        return IRQ_HANDLED;
+}
+
+static int __devinit sdhci_probe(struct platform_device *pdev)
+{
+        struct sdhci_host *host;
+        struct resource *iomem;
+        struct spear_sdhci *sdhci;
+        int ret;
+
+        BUG_ON(pdev == NULL);
+
+        iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!iomem) {
+                ret = -ENOMEM;
+                dev_dbg(&pdev->dev, "memory resource not defined\n");
+                goto err;
+        }
+
+        if (!request_mem_region(iomem->start, resource_size(iomem),
+                                "spear-sdhci")) {
+                ret = -EBUSY;
+                dev_dbg(&pdev->dev, "cannot request region\n");
+                goto err;
+        }
+
+        sdhci = kzalloc(sizeof(*sdhci), GFP_KERNEL);
+        if (!sdhci) {
+                ret = -ENOMEM;
+                dev_dbg(&pdev->dev, "cannot allocate memory for sdhci\n");
+                goto err_kzalloc;
+        }
+
+        /* clk enable */
+        sdhci->clk = clk_get(&pdev->dev, NULL);
+        if (IS_ERR(sdhci->clk)) {
+                ret = PTR_ERR(sdhci->clk);
+                dev_dbg(&pdev->dev, "Error getting clock\n");
+                goto err_clk_get;
+        }
+
+        ret = clk_enable(sdhci->clk);
+        if (ret) {
+                dev_dbg(&pdev->dev, "Error enabling clock\n");
+                goto err_clk_enb;
+        }
+
+        /* overwrite platform_data */
+        sdhci->data = dev_get_platdata(&pdev->dev);
+        pdev->dev.platform_data = sdhci;
+
+        if (pdev->dev.parent)
+                host = sdhci_alloc_host(pdev->dev.parent, 0);
+        else
+                host = sdhci_alloc_host(&pdev->dev, 0);
+
+        if (IS_ERR(host)) {
+                ret = PTR_ERR(host);
+                dev_dbg(&pdev->dev, "error allocating host\n");
+                goto err_alloc_host;
+        }
+
+        host->hw_name = "sdhci";
+        host->ops = &sdhci_pltfm_ops;
+        host->irq = platform_get_irq(pdev, 0);
+        host->quirks = SDHCI_QUIRK_BROKEN_ADMA;
+
+        host->ioaddr = ioremap(iomem->start, resource_size(iomem));
+        if (!host->ioaddr) {
+                ret = -ENOMEM;
+                dev_dbg(&pdev->dev, "failed to remap registers\n");
+                goto err_ioremap;
+        }
+
+        ret = sdhci_add_host(host);
+        if (ret) {
+                dev_dbg(&pdev->dev, "error adding host\n");
+                goto err_add_host;
+        }
+
+        platform_set_drvdata(pdev, host);
+
+        /*
+         * It is optional to use GPIOs for sdhci Power control & sdhci card
+         * interrupt detection. If sdhci->data is NULL, then use original sdhci
+         * lines otherwise GPIO lines.
+         * If GPIO is selected for power control, then power should be disabled
+         * after card removal and should be enabled when card insertion
+         * interrupt occurs
+         */
+        if (!sdhci->data)
+                return 0;
+
+        if (sdhci->data->card_power_gpio >= 0) {
+                int val = 0;
+
+                ret = gpio_request(sdhci->data->card_power_gpio, "sdhci");
+                if (ret < 0) {
+                        dev_dbg(&pdev->dev, "gpio request fail: %d\n",
+                                        sdhci->data->card_power_gpio);
+                        goto err_pgpio_request;
+                }
+
+                if (sdhci->data->power_always_enb)
+                        val = sdhci->data->power_active_high;
+                else
+                        val = !sdhci->data->power_active_high;
+
+                ret = gpio_direction_output(sdhci->data->card_power_gpio, val);
+                if (ret) {
+                        dev_dbg(&pdev->dev, "gpio set direction fail: %d\n",
+                                        sdhci->data->card_power_gpio);
+                        goto err_pgpio_direction;
+                }
+
+                gpio_set_value(sdhci->data->card_power_gpio, 1);
+        }
+
+        if (sdhci->data->card_int_gpio >= 0) {
+                ret = gpio_request(sdhci->data->card_int_gpio, "sdhci");
+                if (ret < 0) {
+                        dev_dbg(&pdev->dev, "gpio request fail: %d\n",
+                                        sdhci->data->card_int_gpio);
+                        goto err_igpio_request;
+                }
+
+                ret = gpio_direction_input(sdhci->data->card_int_gpio);
+                if (ret) {
+                        dev_dbg(&pdev->dev, "gpio set direction fail: %d\n",
+                                        sdhci->data->card_int_gpio);
+                        goto err_igpio_direction;
+                }
+                ret = request_irq(gpio_to_irq(sdhci->data->card_int_gpio),
+                                sdhci_gpio_irq, IRQF_TRIGGER_LOW,
+                                mmc_hostname(host->mmc), pdev);
+                if (ret) {
+                        dev_dbg(&pdev->dev, "gpio request irq fail: %d\n",
+                                        sdhci->data->card_int_gpio);
+                        goto err_igpio_request_irq;
+                }
+
+        }
+
+        return 0;
+
+err_igpio_request_irq:
+err_igpio_direction:
+        if (sdhci->data->card_int_gpio >= 0)
+                gpio_free(sdhci->data->card_int_gpio);
+err_igpio_request:
+err_pgpio_direction:
+        if (sdhci->data->card_power_gpio >= 0)
+                gpio_free(sdhci->data->card_power_gpio);
+err_pgpio_request:
+        platform_set_drvdata(pdev, NULL);
+        sdhci_remove_host(host, 1);
+err_add_host:
+        iounmap(host->ioaddr);
+err_ioremap:
+        sdhci_free_host(host);
+err_alloc_host:
+        clk_disable(sdhci->clk);
+err_clk_enb:
+        clk_put(sdhci->clk);
+err_clk_get:
+        kfree(sdhci);
+err_kzalloc:
+        release_mem_region(iomem->start, resource_size(iomem));
+err:
+        dev_err(&pdev->dev, "spear-sdhci probe failed: %d\n", ret);
+        return ret;
+}
+
+static int __devexit sdhci_remove(struct platform_device *pdev)
+{
+        struct sdhci_host *host = platform_get_drvdata(pdev);
+        struct resource *iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        struct spear_sdhci *sdhci = dev_get_platdata(&pdev->dev);
+        int dead;
+        u32 scratch;
+
+        if (sdhci->data) {
+                if (sdhci->data->card_int_gpio >= 0) {
+                        free_irq(gpio_to_irq(sdhci->data->card_int_gpio), pdev);
+                        gpio_free(sdhci->data->card_int_gpio);
+                }
+
+                if (sdhci->data->card_power_gpio >= 0)
+                        gpio_free(sdhci->data->card_power_gpio);
+        }
+
+        platform_set_drvdata(pdev, NULL);
+        dead = 0;
+        scratch = readl(host->ioaddr + SDHCI_INT_STATUS);
+        if (scratch == (u32)-1)
+                dead = 1;
+
+        sdhci_remove_host(host, dead);
+        iounmap(host->ioaddr);
+        sdhci_free_host(host);
+        clk_disable(sdhci->clk);
+        clk_put(sdhci->clk);
+        kfree(sdhci);
+        if (iomem)
+                release_mem_region(iomem->start, resource_size(iomem));
+
+        return 0;
+}
+
+static struct platform_driver sdhci_driver = {
+        .driver = {
+                .name   = "sdhci",
+                .owner  = THIS_MODULE,
+        },
+        .probe          = sdhci_probe,
+        .remove         = __devexit_p(sdhci_remove),
+};
+
+static int __init sdhci_init(void)
+{
+        return platform_driver_register(&sdhci_driver);
+}
+module_init(sdhci_init);
+
+static void __exit sdhci_exit(void)
+{
+        platform_driver_unregister(&sdhci_driver);
+}
+module_exit(sdhci_exit);
+
+MODULE_DESCRIPTION("SPEAr Secure Digital Host Controller Interface driver");
+MODULE_AUTHOR("Viresh Kumar <viresh.kumar@st.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 9d4fdfa685e5..c6d1bd8d4ac4 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -496,12 +496,22 @@ static int sdhci_adma_table_pre(struct sdhci_host *host,
                 WARN_ON((desc - host->adma_desc) > (128 * 2 + 1) * 4);
         }
 
-        /*
-         * Add a terminating entry.
-         */
+        if (host->quirks & SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC) {
+                /*
+                * Mark the last descriptor as the terminating descriptor
+                */
+                if (desc != host->adma_desc) {
+                        desc -= 8;
+                        desc[0] |= 0x2; /* end */
+                }
+        } else {
+                /*
+                * Add a terminating entry.
+                */
 
-        /* nop, end, valid */
-        sdhci_set_adma_desc(desc, 0, 0, 0x3);
+                /* nop, end, valid */
+                sdhci_set_adma_desc(desc, 0, 0, 0x3);
+        }
 
         /*
          * Resync align buffer as we might have changed it.
@@ -1587,7 +1597,7 @@ int sdhci_suspend_host(struct sdhci_host *host, pm_message_t state)
 
         sdhci_disable_card_detection(host);
 
-        ret = mmc_suspend_host(host->mmc, state);
+        ret = mmc_suspend_host(host->mmc);
         if (ret)
                 return ret;
 
@@ -1744,7 +1754,8 @@ int sdhci_add_host(struct sdhci_host *host)
         host->max_clk =
                 (caps & SDHCI_CLOCK_BASE_MASK) >> SDHCI_CLOCK_BASE_SHIFT;
         host->max_clk *= 1000000;
-        if (host->max_clk == 0) {
+        if (host->max_clk == 0 || host->quirks &
+                        SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN) {
                 if (!host->ops->get_max_clock) {
                         printk(KERN_ERR
                                "%s: Hardware doesn't specify base clock "
diff --git a/drivers/mmc/host/sdhci.h b/drivers/mmc/host/sdhci.h
index 842f46f94284..c8468134adc9 100644
--- a/drivers/mmc/host/sdhci.h
+++ b/drivers/mmc/host/sdhci.h
@@ -127,7 +127,7 @@
 #define  SDHCI_INT_DATA_MASK    (SDHCI_INT_DATA_END | SDHCI_INT_DMA_END | \
                 SDHCI_INT_DATA_AVAIL | SDHCI_INT_SPACE_AVAIL | \
                 SDHCI_INT_DATA_TIMEOUT | SDHCI_INT_DATA_CRC | \
-                SDHCI_INT_DATA_END_BIT | SDHCI_ADMA_ERROR)
+                SDHCI_INT_DATA_END_BIT | SDHCI_INT_ADMA_ERROR)
 #define SDHCI_INT_ALL_MASK      ((unsigned int)-1)
 
 #define SDHCI_ACMD12_ERR        0x3C
@@ -236,6 +236,10 @@ struct sdhci_host {
 #define SDHCI_QUIRK_DELAY_AFTER_POWER                   (1<<23)
 /* Controller uses SDCLK instead of TMCLK for data timeouts */
 #define SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK             (1<<24)
+/* Controller reports wrong base clock capability */
+#define SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN               (1<<25)
+/* Controller cannot support End Attribute in NOP ADMA descriptor */
+#define SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC               (1<<26)
 
         int                     irq;            /* Device IRQ */
         void __iomem *          ioaddr;         /* Mapped address */
@@ -294,12 +298,12 @@ struct sdhci_host {
 
 struct sdhci_ops {
 #ifdef CONFIG_MMC_SDHCI_IO_ACCESSORS
-        u32             (*readl)(struct sdhci_host *host, int reg);
-        u16             (*readw)(struct sdhci_host *host, int reg);
-        u8              (*readb)(struct sdhci_host *host, int reg);
-        void            (*writel)(struct sdhci_host *host, u32 val, int reg);
-        void            (*writew)(struct sdhci_host *host, u16 val, int reg);
-        void            (*writeb)(struct sdhci_host *host, u8 val, int reg);
+        u32             (*read_l)(struct sdhci_host *host, int reg);
+        u16             (*read_w)(struct sdhci_host *host, int reg);
+        u8              (*read_b)(struct sdhci_host *host, int reg);
+        void            (*write_l)(struct sdhci_host *host, u32 val, int reg);
+        void            (*write_w)(struct sdhci_host *host, u16 val, int reg);
+        void            (*write_b)(struct sdhci_host *host, u8 val, int reg);
 #endif
 
         void    (*set_clock)(struct sdhci_host *host, unsigned int clock);
@@ -314,48 +318,48 @@ struct sdhci_ops {
 
 static inline void sdhci_writel(struct sdhci_host *host, u32 val, int reg)
 {
-        if (unlikely(host->ops->writel))
-                host->ops->writel(host, val, reg);
+        if (unlikely(host->ops->write_l))
+                host->ops->write_l(host, val, reg);
         else
                 writel(val, host->ioaddr + reg);
 }
 
 static inline void sdhci_writew(struct sdhci_host *host, u16 val, int reg)
 {
-        if (unlikely(host->ops->writew))
-                host->ops->writew(host, val, reg);
+        if (unlikely(host->ops->write_w))
+                host->ops->write_w(host, val, reg);
         else
                 writew(val, host->ioaddr + reg);
 }
 
 static inline void sdhci_writeb(struct sdhci_host *host, u8 val, int reg)
 {
-        if (unlikely(host->ops->writeb))
-                host->ops->writeb(host, val, reg);
+        if (unlikely(host->ops->write_b))
+                host->ops->write_b(host, val, reg);
         else
                 writeb(val, host->ioaddr + reg);
 }
 
 static inline u32 sdhci_readl(struct sdhci_host *host, int reg)
 {
-        if (unlikely(host->ops->readl))
-                return host->ops->readl(host, reg);
+        if (unlikely(host->ops->read_l))
+                return host->ops->read_l(host, reg);
         else
                 return readl(host->ioaddr + reg);
 }
 
 static inline u16 sdhci_readw(struct sdhci_host *host, int reg)
 {
-        if (unlikely(host->ops->readw))
-                return host->ops->readw(host, reg);
+        if (unlikely(host->ops->read_w))
+                return host->ops->read_w(host, reg);
         else
                 return readw(host->ioaddr + reg);
 }
 
 static inline u8 sdhci_readb(struct sdhci_host *host, int reg)
 {
-        if (unlikely(host->ops->readb))
-                return host->ops->readb(host, reg);
+        if (unlikely(host->ops->read_b))
+                return host->ops->read_b(host, reg);
         else
                 return readb(host->ioaddr + reg);
 }
diff --git a/drivers/mmc/host/sdricoh_cs.c b/drivers/mmc/host/sdricoh_cs.c
index cb41e9c3ac07..e7507af3856e 100644
--- a/drivers/mmc/host/sdricoh_cs.c
+++ b/drivers/mmc/host/sdricoh_cs.c
@@ -519,7 +519,7 @@ static int sdricoh_pcmcia_suspend(struct pcmcia_device *link)
 {
         struct mmc_host *mmc = link->priv;
         dev_dbg(&link->dev, "suspend\n");
-        mmc_suspend_host(mmc, PMSG_SUSPEND);
+        mmc_suspend_host(mmc);
         return 0;
 }
 
diff --git a/drivers/mmc/host/sh_mmcif.c b/drivers/mmc/host/sh_mmcif.c
new file mode 100644
index 000000000000..5d3f824bb5a3
--- /dev/null
+++ b/drivers/mmc/host/sh_mmcif.c
@@ -0,0 +1,940 @@
+/*
+ * MMCIF eMMC driver.
+ *
+ * Copyright (C) 2010 Renesas Solutions Corp.
+ * Yusuke Goda <yusuke.goda.sx@renesas.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.
+ *
+ *
+ * TODO
+ *  1. DMA
+ *  2. Power management
+ *  3. Handle MMC errors better
+ *
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/core.h>
+#include <linux/mmc/mmc.h>
+#include <linux/mmc/sdio.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/mmc/sh_mmcif.h>
+
+#define DRIVER_NAME     "sh_mmcif"
+#define DRIVER_VERSION  "2010-04-28"
+
+/* CE_CMD_SET */
+#define CMD_MASK                0x3f000000
+#define CMD_SET_RTYP_NO         ((0 << 23) | (0 << 22))
+#define CMD_SET_RTYP_6B         ((0 << 23) | (1 << 22)) /* R1/R1b/R3/R4/R5 */
+#define CMD_SET_RTYP_17B        ((1 << 23) | (0 << 22)) /* R2 */
+#define CMD_SET_RBSY            (1 << 21) /* R1b */
+#define CMD_SET_CCSEN           (1 << 20)
+#define CMD_SET_WDAT            (1 << 19) /* 1: on data, 0: no data */
+#define CMD_SET_DWEN            (1 << 18) /* 1: write, 0: read */
+#define CMD_SET_CMLTE           (1 << 17) /* 1: multi block trans, 0: single */
+#define CMD_SET_CMD12EN         (1 << 16) /* 1: CMD12 auto issue */
+#define CMD_SET_RIDXC_INDEX     ((0 << 15) | (0 << 14)) /* index check */
+#define CMD_SET_RIDXC_BITS      ((0 << 15) | (1 << 14)) /* check bits check */
+#define CMD_SET_RIDXC_NO        ((1 << 15) | (0 << 14)) /* no check */
+#define CMD_SET_CRC7C           ((0 << 13) | (0 << 12)) /* CRC7 check*/
+#define CMD_SET_CRC7C_BITS      ((0 << 13) | (1 << 12)) /* check bits check*/
+#define CMD_SET_CRC7C_INTERNAL  ((1 << 13) | (0 << 12)) /* internal CRC7 check*/
+#define CMD_SET_CRC16C          (1 << 10) /* 0: CRC16 check*/
+#define CMD_SET_CRCSTE          (1 << 8) /* 1: not receive CRC status */
+#define CMD_SET_TBIT            (1 << 7) /* 1: tran mission bit "Low" */
+#define CMD_SET_OPDM            (1 << 6) /* 1: open/drain */
+#define CMD_SET_CCSH            (1 << 5)
+#define CMD_SET_DATW_1          ((0 << 1) | (0 << 0)) /* 1bit */
+#define CMD_SET_DATW_4          ((0 << 1) | (1 << 0)) /* 4bit */
+#define CMD_SET_DATW_8          ((1 << 1) | (0 << 0)) /* 8bit */
+
+/* CE_CMD_CTRL */
+#define CMD_CTRL_BREAK          (1 << 0)
+
+/* CE_BLOCK_SET */
+#define BLOCK_SIZE_MASK         0x0000ffff
+
+/* CE_CLK_CTRL */
+#define CLK_ENABLE              (1 << 24) /* 1: output mmc clock */
+#define CLK_CLEAR               ((1 << 19) | (1 << 18) | (1 << 17) | (1 << 16))
+#define CLK_SUP_PCLK            ((1 << 19) | (1 << 18) | (1 << 17) | (1 << 16))
+#define SRSPTO_256              ((1 << 13) | (0 << 12)) /* resp timeout */
+#define SRBSYTO_29              ((1 << 11) | (1 << 10) |        \
+                                 (1 << 9) | (1 << 8)) /* resp busy timeout */
+#define SRWDTO_29               ((1 << 7) | (1 << 6) |          \
+                                 (1 << 5) | (1 << 4)) /* read/write timeout */
+#define SCCSTO_29               ((1 << 3) | (1 << 2) |          \
+                                 (1 << 1) | (1 << 0)) /* ccs timeout */
+
+/* CE_BUF_ACC */
+#define BUF_ACC_DMAWEN          (1 << 25)
+#define BUF_ACC_DMAREN          (1 << 24)
+#define BUF_ACC_BUSW_32         (0 << 17)
+#define BUF_ACC_BUSW_16         (1 << 17)
+#define BUF_ACC_ATYP            (1 << 16)
+
+/* CE_INT */
+#define INT_CCSDE               (1 << 29)
+#define INT_CMD12DRE            (1 << 26)
+#define INT_CMD12RBE            (1 << 25)
+#define INT_CMD12CRE            (1 << 24)
+#define INT_DTRANE              (1 << 23)
+#define INT_BUFRE               (1 << 22)
+#define INT_BUFWEN              (1 << 21)
+#define INT_BUFREN              (1 << 20)
+#define INT_CCSRCV              (1 << 19)
+#define INT_RBSYE               (1 << 17)
+#define INT_CRSPE               (1 << 16)
+#define INT_CMDVIO              (1 << 15)
+#define INT_BUFVIO              (1 << 14)
+#define INT_WDATERR             (1 << 11)
+#define INT_RDATERR             (1 << 10)
+#define INT_RIDXERR             (1 << 9)
+#define INT_RSPERR              (1 << 8)
+#define INT_CCSTO               (1 << 5)
+#define INT_CRCSTO              (1 << 4)
+#define INT_WDATTO              (1 << 3)
+#define INT_RDATTO              (1 << 2)
+#define INT_RBSYTO              (1 << 1)
+#define INT_RSPTO               (1 << 0)
+#define INT_ERR_STS             (INT_CMDVIO | INT_BUFVIO | INT_WDATERR |  \
+                                 INT_RDATERR | INT_RIDXERR | INT_RSPERR | \
+                                 INT_CCSTO | INT_CRCSTO | INT_WDATTO |    \
+                                 INT_RDATTO | INT_RBSYTO | INT_RSPTO)
+
+/* CE_INT_MASK */
+#define MASK_ALL                0x00000000
+#define MASK_MCCSDE             (1 << 29)
+#define MASK_MCMD12DRE          (1 << 26)
+#define MASK_MCMD12RBE          (1 << 25)
+#define MASK_MCMD12CRE          (1 << 24)
+#define MASK_MDTRANE            (1 << 23)
+#define MASK_MBUFRE             (1 << 22)
+#define MASK_MBUFWEN            (1 << 21)
+#define MASK_MBUFREN            (1 << 20)
+#define MASK_MCCSRCV            (1 << 19)
+#define MASK_MRBSYE             (1 << 17)
+#define MASK_MCRSPE             (1 << 16)
+#define MASK_MCMDVIO            (1 << 15)
+#define MASK_MBUFVIO            (1 << 14)
+#define MASK_MWDATERR           (1 << 11)
+#define MASK_MRDATERR           (1 << 10)
+#define MASK_MRIDXERR           (1 << 9)
+#define MASK_MRSPERR            (1 << 8)
+#define MASK_MCCSTO             (1 << 5)
+#define MASK_MCRCSTO            (1 << 4)
+#define MASK_MWDATTO            (1 << 3)
+#define MASK_MRDATTO            (1 << 2)
+#define MASK_MRBSYTO            (1 << 1)
+#define MASK_MRSPTO             (1 << 0)
+
+/* CE_HOST_STS1 */
+#define STS1_CMDSEQ             (1 << 31)
+
+/* CE_HOST_STS2 */
+#define STS2_CRCSTE             (1 << 31)
+#define STS2_CRC16E             (1 << 30)
+#define STS2_AC12CRCE           (1 << 29)
+#define STS2_RSPCRC7E           (1 << 28)
+#define STS2_CRCSTEBE           (1 << 27)
+#define STS2_RDATEBE            (1 << 26)
+#define STS2_AC12REBE           (1 << 25)
+#define STS2_RSPEBE             (1 << 24)
+#define STS2_AC12IDXE           (1 << 23)
+#define STS2_RSPIDXE            (1 << 22)
+#define STS2_CCSTO              (1 << 15)
+#define STS2_RDATTO             (1 << 14)
+#define STS2_DATBSYTO           (1 << 13)
+#define STS2_CRCSTTO            (1 << 12)
+#define STS2_AC12BSYTO          (1 << 11)
+#define STS2_RSPBSYTO           (1 << 10)
+#define STS2_AC12RSPTO          (1 << 9)
+#define STS2_RSPTO              (1 << 8)
+#define STS2_CRC_ERR            (STS2_CRCSTE | STS2_CRC16E |            \
+                                 STS2_AC12CRCE | STS2_RSPCRC7E | STS2_CRCSTEBE)
+#define STS2_TIMEOUT_ERR        (STS2_CCSTO | STS2_RDATTO |             \
+                                 STS2_DATBSYTO | STS2_CRCSTTO |         \
+                                 STS2_AC12BSYTO | STS2_RSPBSYTO |       \
+                                 STS2_AC12RSPTO | STS2_RSPTO)
+
+/* CE_VERSION */
+#define SOFT_RST_ON             (1 << 31)
+#define SOFT_RST_OFF            (0 << 31)
+
+#define CLKDEV_EMMC_DATA        52000000 /* 52MHz */
+#define CLKDEV_MMC_DATA         20000000 /* 20MHz */
+#define CLKDEV_INIT             400000   /* 400 KHz */
+
+struct sh_mmcif_host {
+        struct mmc_host *mmc;
+        struct mmc_data *data;
+        struct mmc_command *cmd;
+        struct platform_device *pd;
+        struct clk *hclk;
+        unsigned int clk;
+        int bus_width;
+        u16 wait_int;
+        u16 sd_error;
+        long timeout;
+        void __iomem *addr;
+        wait_queue_head_t intr_wait;
+};
+
+
+static inline void sh_mmcif_bitset(struct sh_mmcif_host *host,
+                                        unsigned int reg, u32 val)
+{
+        writel(val | readl(host->addr + reg), host->addr + reg);
+}
+
+static inline void sh_mmcif_bitclr(struct sh_mmcif_host *host,
+                                        unsigned int reg, u32 val)
+{
+        writel(~val & readl(host->addr + reg), host->addr + reg);
+}
+
+
+static void sh_mmcif_clock_control(struct sh_mmcif_host *host, unsigned int clk)
+{
+        struct sh_mmcif_plat_data *p = host->pd->dev.platform_data;
+
+        sh_mmcif_bitclr(host, MMCIF_CE_CLK_CTRL, CLK_ENABLE);
+        sh_mmcif_bitclr(host, MMCIF_CE_CLK_CTRL, CLK_CLEAR);
+
+        if (!clk)
+                return;
+        if (p->sup_pclk && clk == host->clk)
+                sh_mmcif_bitset(host, MMCIF_CE_CLK_CTRL, CLK_SUP_PCLK);
+        else
+                sh_mmcif_bitset(host, MMCIF_CE_CLK_CTRL, CLK_CLEAR &
+                        (ilog2(__rounddown_pow_of_two(host->clk / clk)) << 16));
+
+        sh_mmcif_bitset(host, MMCIF_CE_CLK_CTRL, CLK_ENABLE);
+}
+
+static void sh_mmcif_sync_reset(struct sh_mmcif_host *host)
+{
+        u32 tmp;
+
+        tmp = 0x010f0000 & sh_mmcif_readl(host->addr, MMCIF_CE_CLK_CTRL);
+
+        sh_mmcif_writel(host->addr, MMCIF_CE_VERSION, SOFT_RST_ON);
+        sh_mmcif_writel(host->addr, MMCIF_CE_VERSION, SOFT_RST_OFF);
+        sh_mmcif_bitset(host, MMCIF_CE_CLK_CTRL, tmp |
+                SRSPTO_256 | SRBSYTO_29 | SRWDTO_29 | SCCSTO_29);
+        /* byte swap on */
+        sh_mmcif_bitset(host, MMCIF_CE_BUF_ACC, BUF_ACC_ATYP);
+}
+
+static int sh_mmcif_error_manage(struct sh_mmcif_host *host)
+{
+        u32 state1, state2;
+        int ret, timeout = 10000000;
+
+        host->sd_error = 0;
+        host->wait_int = 0;
+
+        state1 = sh_mmcif_readl(host->addr, MMCIF_CE_HOST_STS1);
+        state2 = sh_mmcif_readl(host->addr, MMCIF_CE_HOST_STS2);
+        pr_debug("%s: ERR HOST_STS1 = %08x\n", DRIVER_NAME, state1);
+        pr_debug("%s: ERR HOST_STS2 = %08x\n", DRIVER_NAME, state2);
+
+        if (state1 & STS1_CMDSEQ) {
+                sh_mmcif_bitset(host, MMCIF_CE_CMD_CTRL, CMD_CTRL_BREAK);
+                sh_mmcif_bitset(host, MMCIF_CE_CMD_CTRL, ~CMD_CTRL_BREAK);
+                while (1) {
+                        timeout--;
+                        if (timeout < 0) {
+                                pr_err(DRIVER_NAME": Forceed end of " \
+                                        "command sequence timeout err\n");
+                                return -EIO;
+                        }
+                        if (!(sh_mmcif_readl(host->addr, MMCIF_CE_HOST_STS1)
+                                                                & STS1_CMDSEQ))
+                                break;
+                        mdelay(1);
+                }
+                sh_mmcif_sync_reset(host);
+                pr_debug(DRIVER_NAME": Forced end of command sequence\n");
+                return -EIO;
+        }
+
+        if (state2 & STS2_CRC_ERR) {
+                pr_debug(DRIVER_NAME": Happened CRC error\n");
+                ret = -EIO;
+        } else if (state2 & STS2_TIMEOUT_ERR) {
+                pr_debug(DRIVER_NAME": Happened Timeout error\n");
+                ret = -ETIMEDOUT;
+        } else {
+                pr_debug(DRIVER_NAME": Happened End/Index error\n");
+                ret = -EIO;
+        }
+        return ret;
+}
+
+static int sh_mmcif_single_read(struct sh_mmcif_host *host,
+                                        struct mmc_request *mrq)
+{
+        struct mmc_data *data = mrq->data;
+        long time;
+        u32 blocksize, i, *p = sg_virt(data->sg);
+
+        host->wait_int = 0;
+
+        /* buf read enable */
+        sh_mmcif_bitset(host, MMCIF_CE_INT_MASK, MASK_MBUFREN);
+        time = wait_event_interruptible_timeout(host->intr_wait,
+                        host->wait_int == 1 ||
+                        host->sd_error == 1, host->timeout);
+        if (host->wait_int != 1 && (time == 0 || host->sd_error != 0))
+                return sh_mmcif_error_manage(host);
+
+        host->wait_int = 0;
+        blocksize = (BLOCK_SIZE_MASK &
+                        sh_mmcif_readl(host->addr, MMCIF_CE_BLOCK_SET)) + 3;
+        for (i = 0; i < blocksize / 4; i++)
+                *p++ = sh_mmcif_readl(host->addr, MMCIF_CE_DATA);
+
+        /* buffer read end */
+        sh_mmcif_bitset(host, MMCIF_CE_INT_MASK, MASK_MBUFRE);
+        time = wait_event_interruptible_timeout(host->intr_wait,
+                        host->wait_int == 1 ||
+                        host->sd_error == 1, host->timeout);
+        if (host->wait_int != 1 && (time == 0 || host->sd_error != 0))
+                return sh_mmcif_error_manage(host);
+
+        host->wait_int = 0;
+        return 0;
+}
+
+static int sh_mmcif_multi_read(struct sh_mmcif_host *host,
+                                        struct mmc_request *mrq)
+{
+        struct mmc_data *data = mrq->data;
+        long time;
+        u32 blocksize, i, j, sec, *p;
+
+        blocksize = BLOCK_SIZE_MASK & sh_mmcif_readl(host->addr,
+                                                     MMCIF_CE_BLOCK_SET);
+        for (j = 0; j < data->sg_len; j++) {
+                p = sg_virt(data->sg);
+                host->wait_int = 0;
+                for (sec = 0; sec < data->sg->length / blocksize; sec++) {
+                        sh_mmcif_bitset(host, MMCIF_CE_INT_MASK, MASK_MBUFREN);
+                        /* buf read enable */
+                        time = wait_event_interruptible_timeout(host->intr_wait,
+                                host->wait_int == 1 ||
+                                host->sd_error == 1, host->timeout);
+
+                        if (host->wait_int != 1 &&
+                            (time == 0 || host->sd_error != 0))
+                                return sh_mmcif_error_manage(host);
+
+                        host->wait_int = 0;
+                        for (i = 0; i < blocksize / 4; i++)
+                                *p++ = sh_mmcif_readl(host->addr,
+                                                      MMCIF_CE_DATA);
+                }
+                if (j < data->sg_len - 1)
+                        data->sg++;
+        }
+        return 0;
+}
+
+static int sh_mmcif_single_write(struct sh_mmcif_host *host,
+                                        struct mmc_request *mrq)
+{
+        struct mmc_data *data = mrq->data;
+        long time;
+        u32 blocksize, i, *p = sg_virt(data->sg);
+
+        host->wait_int = 0;
+        sh_mmcif_bitset(host, MMCIF_CE_INT_MASK, MASK_MBUFWEN);
+
+        /* buf write enable */
+        time = wait_event_interruptible_timeout(host->intr_wait,
+                        host->wait_int == 1 ||
+                        host->sd_error == 1, host->timeout);
+        if (host->wait_int != 1 && (time == 0 || host->sd_error != 0))
+                return sh_mmcif_error_manage(host);
+
+        host->wait_int = 0;
+        blocksize = (BLOCK_SIZE_MASK &
+                        sh_mmcif_readl(host->addr, MMCIF_CE_BLOCK_SET)) + 3;
+        for (i = 0; i < blocksize / 4; i++)
+                sh_mmcif_writel(host->addr, MMCIF_CE_DATA, *p++);
+
+        /* buffer write end */
+        sh_mmcif_bitset(host, MMCIF_CE_INT_MASK, MASK_MDTRANE);
+
+        time = wait_event_interruptible_timeout(host->intr_wait,
+                        host->wait_int == 1 ||
+                        host->sd_error == 1, host->timeout);
+        if (host->wait_int != 1 && (time == 0 || host->sd_error != 0))
+                return sh_mmcif_error_manage(host);
+
+        host->wait_int = 0;
+        return 0;
+}
+
+static int sh_mmcif_multi_write(struct sh_mmcif_host *host,
+                                                struct mmc_request *mrq)
+{
+        struct mmc_data *data = mrq->data;
+        long time;
+        u32 i, sec, j, blocksize, *p;
+
+        blocksize = BLOCK_SIZE_MASK & sh_mmcif_readl(host->addr,
+                                                     MMCIF_CE_BLOCK_SET);
+
+        for (j = 0; j < data->sg_len; j++) {
+                p = sg_virt(data->sg);
+                host->wait_int = 0;
+                for (sec = 0; sec < data->sg->length / blocksize; sec++) {
+                        sh_mmcif_bitset(host, MMCIF_CE_INT_MASK, MASK_MBUFWEN);
+                        /* buf write enable*/
+                        time = wait_event_interruptible_timeout(host->intr_wait,
+                                host->wait_int == 1 ||
+                                host->sd_error == 1, host->timeout);
+
+                        if (host->wait_int != 1 &&
+                            (time == 0 || host->sd_error != 0))
+                                return sh_mmcif_error_manage(host);
+
+                        host->wait_int = 0;
+                        for (i = 0; i < blocksize / 4; i++)
+                                sh_mmcif_writel(host->addr,
+                                                MMCIF_CE_DATA, *p++);
+                }
+                if (j < data->sg_len - 1)
+                        data->sg++;
+        }
+        return 0;
+}
+
+static void sh_mmcif_get_response(struct sh_mmcif_host *host,
+                                                struct mmc_command *cmd)
+{
+        if (cmd->flags & MMC_RSP_136) {
+                cmd->resp[0] = sh_mmcif_readl(host->addr, MMCIF_CE_RESP3);
+                cmd->resp[1] = sh_mmcif_readl(host->addr, MMCIF_CE_RESP2);
+                cmd->resp[2] = sh_mmcif_readl(host->addr, MMCIF_CE_RESP1);
+                cmd->resp[3] = sh_mmcif_readl(host->addr, MMCIF_CE_RESP0);
+        } else
+                cmd->resp[0] = sh_mmcif_readl(host->addr, MMCIF_CE_RESP0);
+}
+
+static void sh_mmcif_get_cmd12response(struct sh_mmcif_host *host,
+                                                struct mmc_command *cmd)
+{
+        cmd->resp[0] = sh_mmcif_readl(host->addr, MMCIF_CE_RESP_CMD12);
+}
+
+static u32 sh_mmcif_set_cmd(struct sh_mmcif_host *host,
+                struct mmc_request *mrq, struct mmc_command *cmd, u32 opc)
+{
+        u32 tmp = 0;
+
+        /* Response Type check */
+        switch (mmc_resp_type(cmd)) {
+        case MMC_RSP_NONE:
+                tmp |= CMD_SET_RTYP_NO;
+                break;
+        case MMC_RSP_R1:
+        case MMC_RSP_R1B:
+        case MMC_RSP_R3:
+                tmp |= CMD_SET_RTYP_6B;
+                break;
+        case MMC_RSP_R2:
+                tmp |= CMD_SET_RTYP_17B;
+                break;
+        default:
+                pr_err(DRIVER_NAME": Not support type response.\n");
+                break;
+        }
+        switch (opc) {
+        /* RBSY */
+        case MMC_SWITCH:
+        case MMC_STOP_TRANSMISSION:
+        case MMC_SET_WRITE_PROT:
+        case MMC_CLR_WRITE_PROT:
+        case MMC_ERASE:
+        case MMC_GEN_CMD:
+                tmp |= CMD_SET_RBSY;
+                break;
+        }
+        /* WDAT / DATW */
+        if (host->data) {
+                tmp |= CMD_SET_WDAT;
+                switch (host->bus_width) {
+                case MMC_BUS_WIDTH_1:
+                        tmp |= CMD_SET_DATW_1;
+                        break;
+                case MMC_BUS_WIDTH_4:
+                        tmp |= CMD_SET_DATW_4;
+                        break;
+                case MMC_BUS_WIDTH_8:
+                        tmp |= CMD_SET_DATW_8;
+                        break;
+                default:
+                        pr_err(DRIVER_NAME": Not support bus width.\n");
+                        break;
+                }
+        }
+        /* DWEN */
+        if (opc == MMC_WRITE_BLOCK || opc == MMC_WRITE_MULTIPLE_BLOCK)
+                tmp |= CMD_SET_DWEN;
+        /* CMLTE/CMD12EN */
+        if (opc == MMC_READ_MULTIPLE_BLOCK || opc == MMC_WRITE_MULTIPLE_BLOCK) {
+                tmp |= CMD_SET_CMLTE | CMD_SET_CMD12EN;
+                sh_mmcif_bitset(host, MMCIF_CE_BLOCK_SET,
+                                        mrq->data->blocks << 16);
+        }
+        /* RIDXC[1:0] check bits */
+        if (opc == MMC_SEND_OP_COND || opc == MMC_ALL_SEND_CID ||
+            opc == MMC_SEND_CSD || opc == MMC_SEND_CID)
+                tmp |= CMD_SET_RIDXC_BITS;
+        /* RCRC7C[1:0] check bits */
+        if (opc == MMC_SEND_OP_COND)
+                tmp |= CMD_SET_CRC7C_BITS;
+        /* RCRC7C[1:0] internal CRC7 */
+        if (opc == MMC_ALL_SEND_CID ||
+                opc == MMC_SEND_CSD || opc == MMC_SEND_CID)
+                tmp |= CMD_SET_CRC7C_INTERNAL;
+
+        return opc = ((opc << 24) | tmp);
+}
+
+static u32 sh_mmcif_data_trans(struct sh_mmcif_host *host,
+                                struct mmc_request *mrq, u32 opc)
+{
+        u32 ret;
+
+        switch (opc) {
+        case MMC_READ_MULTIPLE_BLOCK:
+                ret = sh_mmcif_multi_read(host, mrq);
+                break;
+        case MMC_WRITE_MULTIPLE_BLOCK:
+                ret = sh_mmcif_multi_write(host, mrq);
+                break;
+        case MMC_WRITE_BLOCK:
+                ret = sh_mmcif_single_write(host, mrq);
+                break;
+        case MMC_READ_SINGLE_BLOCK:
+        case MMC_SEND_EXT_CSD:
+                ret = sh_mmcif_single_read(host, mrq);
+                break;
+        default:
+                pr_err(DRIVER_NAME": NOT SUPPORT CMD = d'%08d\n", opc);
+                ret = -EINVAL;
+                break;
+        }
+        return ret;
+}
+
+static void sh_mmcif_start_cmd(struct sh_mmcif_host *host,
+                        struct mmc_request *mrq, struct mmc_command *cmd)
+{
+        long time;
+        int ret = 0, mask = 0;
+        u32 opc = cmd->opcode;
+
+        host->cmd = cmd;
+
+        switch (opc) {
+        /* respons busy check */
+        case MMC_SWITCH:
+        case MMC_STOP_TRANSMISSION:
+        case MMC_SET_WRITE_PROT:
+        case MMC_CLR_WRITE_PROT:
+        case MMC_ERASE:
+        case MMC_GEN_CMD:
+                mask = MASK_MRBSYE;
+                break;
+        default:
+                mask = MASK_MCRSPE;
+                break;
+        }
+        mask |= MASK_MCMDVIO | MASK_MBUFVIO | MASK_MWDATERR |
+                MASK_MRDATERR | MASK_MRIDXERR | MASK_MRSPERR |
+                MASK_MCCSTO | MASK_MCRCSTO | MASK_MWDATTO |
+                MASK_MRDATTO | MASK_MRBSYTO | MASK_MRSPTO;
+
+        if (host->data) {
+                sh_mmcif_writel(host->addr, MMCIF_CE_BLOCK_SET, 0);
+                sh_mmcif_writel(host->addr, MMCIF_CE_BLOCK_SET,
+                                mrq->data->blksz);
+        }
+        opc = sh_mmcif_set_cmd(host, mrq, cmd, opc);
+
+        sh_mmcif_writel(host->addr, MMCIF_CE_INT, 0xD80430C0);
+        sh_mmcif_writel(host->addr, MMCIF_CE_INT_MASK, mask);
+        /* set arg */
+        sh_mmcif_writel(host->addr, MMCIF_CE_ARG, cmd->arg);
+        host->wait_int = 0;
+        /* set cmd */
+        sh_mmcif_writel(host->addr, MMCIF_CE_CMD_SET, opc);
+
+        time = wait_event_interruptible_timeout(host->intr_wait,
+                host->wait_int == 1 || host->sd_error == 1, host->timeout);
+        if (host->wait_int != 1 && time == 0) {
+                cmd->error = sh_mmcif_error_manage(host);
+                return;
+        }
+        if (host->sd_error) {
+                switch (cmd->opcode) {
+                case MMC_ALL_SEND_CID:
+                case MMC_SELECT_CARD:
+                case MMC_APP_CMD:
+                        cmd->error = -ETIMEDOUT;
+                        break;
+                default:
+                        pr_debug("%s: Cmd(d'%d) err\n",
+                                        DRIVER_NAME, cmd->opcode);
+                        cmd->error = sh_mmcif_error_manage(host);
+                        break;
+                }
+                host->sd_error = 0;
+                host->wait_int = 0;
+                return;
+        }
+        if (!(cmd->flags & MMC_RSP_PRESENT)) {
+                cmd->error = ret;
+                host->wait_int = 0;
+                return;
+        }
+        if (host->wait_int == 1) {
+                sh_mmcif_get_response(host, cmd);
+                host->wait_int = 0;
+        }
+        if (host->data) {
+                ret = sh_mmcif_data_trans(host, mrq, cmd->opcode);
+                if (ret < 0)
+                        mrq->data->bytes_xfered = 0;
+                else
+                        mrq->data->bytes_xfered =
+                                mrq->data->blocks * mrq->data->blksz;
+        }
+        cmd->error = ret;
+}
+
+static void sh_mmcif_stop_cmd(struct sh_mmcif_host *host,
+                struct mmc_request *mrq, struct mmc_command *cmd)
+{
+        long time;
+
+        if (mrq->cmd->opcode == MMC_READ_MULTIPLE_BLOCK)
+                sh_mmcif_bitset(host, MMCIF_CE_INT_MASK, MASK_MCMD12DRE);
+        else if (mrq->cmd->opcode == MMC_WRITE_MULTIPLE_BLOCK)
+                sh_mmcif_bitset(host, MMCIF_CE_INT_MASK, MASK_MCMD12RBE);
+        else {
+                pr_err(DRIVER_NAME": not support stop cmd\n");
+                cmd->error = sh_mmcif_error_manage(host);
+                return;
+        }
+
+        time = wait_event_interruptible_timeout(host->intr_wait,
+                        host->wait_int == 1 ||
+                        host->sd_error == 1, host->timeout);
+        if (host->wait_int != 1 && (time == 0 || host->sd_error != 0)) {
+                cmd->error = sh_mmcif_error_manage(host);
+                return;
+        }
+        sh_mmcif_get_cmd12response(host, cmd);
+        host->wait_int = 0;
+        cmd->error = 0;
+}
+
+static void sh_mmcif_request(struct mmc_host *mmc, struct mmc_request *mrq)
+{
+        struct sh_mmcif_host *host = mmc_priv(mmc);
+
+        switch (mrq->cmd->opcode) {
+        /* MMCIF does not support SD/SDIO command */
+        case SD_IO_SEND_OP_COND:
+        case MMC_APP_CMD:
+                mrq->cmd->error = -ETIMEDOUT;
+                mmc_request_done(mmc, mrq);
+                return;
+        case MMC_SEND_EXT_CSD: /* = SD_SEND_IF_COND (8) */
+                if (!mrq->data) {
+                        /* send_if_cond cmd (not support) */
+                        mrq->cmd->error = -ETIMEDOUT;
+                        mmc_request_done(mmc, mrq);
+                        return;
+                }
+                break;
+        default:
+                break;
+        }
+        host->data = mrq->data;
+        sh_mmcif_start_cmd(host, mrq, mrq->cmd);
+        host->data = NULL;
+
+        if (mrq->cmd->error != 0) {
+                mmc_request_done(mmc, mrq);
+                return;
+        }
+        if (mrq->stop)
+                sh_mmcif_stop_cmd(host, mrq, mrq->stop);
+        mmc_request_done(mmc, mrq);
+}
+
+static void sh_mmcif_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+        struct sh_mmcif_host *host = mmc_priv(mmc);
+        struct sh_mmcif_plat_data *p = host->pd->dev.platform_data;
+
+        if (ios->power_mode == MMC_POWER_OFF) {
+                /* clock stop */
+                sh_mmcif_clock_control(host, 0);
+                if (p->down_pwr)
+                        p->down_pwr(host->pd);
+                return;
+        } else if (ios->power_mode == MMC_POWER_UP) {
+                if (p->set_pwr)
+                        p->set_pwr(host->pd, ios->power_mode);
+        }
+
+        if (ios->clock)
+                sh_mmcif_clock_control(host, ios->clock);
+
+        host->bus_width = ios->bus_width;
+}
+
+static struct mmc_host_ops sh_mmcif_ops = {
+        .request        = sh_mmcif_request,
+        .set_ios        = sh_mmcif_set_ios,
+};
+
+static void sh_mmcif_detect(struct mmc_host *mmc)
+{
+        mmc_detect_change(mmc, 0);
+}
+
+static irqreturn_t sh_mmcif_intr(int irq, void *dev_id)
+{
+        struct sh_mmcif_host *host = dev_id;
+        u32 state = 0;
+        int err = 0;
+
+        state = sh_mmcif_readl(host->addr, MMCIF_CE_INT);
+
+        if (state & INT_RBSYE) {
+                sh_mmcif_writel(host->addr, MMCIF_CE_INT,
+                                ~(INT_RBSYE | INT_CRSPE));
+                sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, MASK_MRBSYE);
+        } else if (state & INT_CRSPE) {
+                sh_mmcif_writel(host->addr, MMCIF_CE_INT, ~INT_CRSPE);
+                sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, MASK_MCRSPE);
+        } else if (state & INT_BUFREN) {
+                sh_mmcif_writel(host->addr, MMCIF_CE_INT, ~INT_BUFREN);
+                sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, MASK_MBUFREN);
+        } else if (state & INT_BUFWEN) {
+                sh_mmcif_writel(host->addr, MMCIF_CE_INT, ~INT_BUFWEN);
+                sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, MASK_MBUFWEN);
+        } else if (state & INT_CMD12DRE) {
+                sh_mmcif_writel(host->addr, MMCIF_CE_INT,
+                        ~(INT_CMD12DRE | INT_CMD12RBE |
+                          INT_CMD12CRE | INT_BUFRE));
+                sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, MASK_MCMD12DRE);
+        } else if (state & INT_BUFRE) {
+                sh_mmcif_writel(host->addr, MMCIF_CE_INT, ~INT_BUFRE);
+                sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, MASK_MBUFRE);
+        } else if (state & INT_DTRANE) {
+                sh_mmcif_writel(host->addr, MMCIF_CE_INT, ~INT_DTRANE);
+                sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, MASK_MDTRANE);
+        } else if (state & INT_CMD12RBE) {
+                sh_mmcif_writel(host->addr, MMCIF_CE_INT,
+                                ~(INT_CMD12RBE | INT_CMD12CRE));
+                sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, MASK_MCMD12RBE);
+        } else if (state & INT_ERR_STS) {
+                /* err interrupts */
+                sh_mmcif_writel(host->addr, MMCIF_CE_INT, ~state);
+                sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, state);
+                err = 1;
+        } else {
+                pr_debug("%s: Not support int\n", DRIVER_NAME);
+                sh_mmcif_writel(host->addr, MMCIF_CE_INT, ~state);
+                sh_mmcif_bitclr(host, MMCIF_CE_INT_MASK, state);
+                err = 1;
+        }
+        if (err) {
+                host->sd_error = 1;
+                pr_debug("%s: int err state = %08x\n", DRIVER_NAME, state);
+        }
+        host->wait_int = 1;
+        wake_up(&host->intr_wait);
+
+        return IRQ_HANDLED;
+}
+
+static int __devinit sh_mmcif_probe(struct platform_device *pdev)
+{
+        int ret = 0, irq[2];
+        struct mmc_host *mmc;
+        struct sh_mmcif_host *host = NULL;
+        struct sh_mmcif_plat_data *pd = NULL;
+        struct resource *res;
+        void __iomem *reg;
+        char clk_name[8];
+
+        irq[0] = platform_get_irq(pdev, 0);
+        irq[1] = platform_get_irq(pdev, 1);
+        if (irq[0] < 0 || irq[1] < 0) {
+                pr_err(DRIVER_NAME": Get irq error\n");
+                return -ENXIO;
+        }
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res) {
+                dev_err(&pdev->dev, "platform_get_resource error.\n");
+                return -ENXIO;
+        }
+        reg = ioremap(res->start, resource_size(res));
+        if (!reg) {
+                dev_err(&pdev->dev, "ioremap error.\n");
+                return -ENOMEM;
+        }
+        pd = (struct sh_mmcif_plat_data *)(pdev->dev.platform_data);
+        if (!pd) {
+                dev_err(&pdev->dev, "sh_mmcif plat data error.\n");
+                ret = -ENXIO;
+                goto clean_up;
+        }
+        mmc = mmc_alloc_host(sizeof(struct sh_mmcif_host), &pdev->dev);
+        if (!mmc) {
+                ret = -ENOMEM;
+                goto clean_up;
+        }
+        host            = mmc_priv(mmc);
+        host->mmc       = mmc;
+        host->addr      = reg;
+        host->timeout   = 1000;
+
+        snprintf(clk_name, sizeof(clk_name), "mmc%d", pdev->id);
+        host->hclk = clk_get(&pdev->dev, clk_name);
+        if (IS_ERR(host->hclk)) {
+                dev_err(&pdev->dev, "cannot get clock \"%s\"\n", clk_name);
+                ret = PTR_ERR(host->hclk);
+                goto clean_up1;
+        }
+        clk_enable(host->hclk);
+        host->clk = clk_get_rate(host->hclk);
+        host->pd = pdev;
+
+        init_waitqueue_head(&host->intr_wait);
+
+        mmc->ops = &sh_mmcif_ops;
+        mmc->f_max = host->clk;
+        /* close to 400KHz */
+        if (mmc->f_max < 51200000)
+                mmc->f_min = mmc->f_max / 128;
+        else if (mmc->f_max < 102400000)
+                mmc->f_min = mmc->f_max / 256;
+        else
+                mmc->f_min = mmc->f_max / 512;
+        if (pd->ocr)
+                mmc->ocr_avail = pd->ocr;
+        mmc->caps = MMC_CAP_MMC_HIGHSPEED;
+        if (pd->caps)
+                mmc->caps |= pd->caps;
+        mmc->max_phys_segs = 128;
+        mmc->max_hw_segs = 128;
+        mmc->max_blk_size = 512;
+        mmc->max_blk_count = 65535;
+        mmc->max_req_size = mmc->max_blk_size * mmc->max_blk_count;
+        mmc->max_seg_size = mmc->max_req_size;
+
+        sh_mmcif_sync_reset(host);
+        platform_set_drvdata(pdev, host);
+        mmc_add_host(mmc);
+
+        ret = request_irq(irq[0], sh_mmcif_intr, 0, "sh_mmc:error", host);
+        if (ret) {
+                pr_err(DRIVER_NAME": request_irq error (sh_mmc:error)\n");
+                goto clean_up2;
+        }
+        ret = request_irq(irq[1], sh_mmcif_intr, 0, "sh_mmc:int", host);
+        if (ret) {
+                free_irq(irq[0], host);
+                pr_err(DRIVER_NAME": request_irq error (sh_mmc:int)\n");
+                goto clean_up2;
+        }
+
+        sh_mmcif_writel(host->addr, MMCIF_CE_INT_MASK, MASK_ALL);
+        sh_mmcif_detect(host->mmc);
+
+        pr_info("%s: driver version %s\n", DRIVER_NAME, DRIVER_VERSION);
+        pr_debug("%s: chip ver H'%04x\n", DRIVER_NAME,
+                sh_mmcif_readl(host->addr, MMCIF_CE_VERSION) & 0x0000ffff);
+        return ret;
+
+clean_up2:
+        clk_disable(host->hclk);
+clean_up1:
+        mmc_free_host(mmc);
+clean_up:
+        if (reg)
+                iounmap(reg);
+        return ret;
+}
+
+static int __devexit sh_mmcif_remove(struct platform_device *pdev)
+{
+        struct sh_mmcif_host *host = platform_get_drvdata(pdev);
+        int irq[2];
+
+        sh_mmcif_writel(host->addr, MMCIF_CE_INT_MASK, MASK_ALL);
+
+        irq[0] = platform_get_irq(pdev, 0);
+        irq[1] = platform_get_irq(pdev, 1);
+
+        if (host->addr)
+                iounmap(host->addr);
+
+        platform_set_drvdata(pdev, NULL);
+        mmc_remove_host(host->mmc);
+
+        free_irq(irq[0], host);
+        free_irq(irq[1], host);
+
+        clk_disable(host->hclk);
+        mmc_free_host(host->mmc);
+
+        return 0;
+}
+
+static struct platform_driver sh_mmcif_driver = {
+        .probe          = sh_mmcif_probe,
+        .remove         = sh_mmcif_remove,
+        .driver         = {
+                .name   = DRIVER_NAME,
+        },
+};
+
+static int __init sh_mmcif_init(void)
+{
+        return platform_driver_register(&sh_mmcif_driver);
+}
+
+static void __exit sh_mmcif_exit(void)
+{
+        platform_driver_unregister(&sh_mmcif_driver);
+}
+
+module_init(sh_mmcif_init);
+module_exit(sh_mmcif_exit);
+
+
+MODULE_DESCRIPTION("SuperH on-chip MMC/eMMC interface driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS(DRIVER_NAME);
+MODULE_AUTHOR("Yusuke Goda <yusuke.goda.sx@renesas.com>");
diff --git a/drivers/mmc/host/tifm_sd.c b/drivers/mmc/host/tifm_sd.c
index 82554ddec6b3..cec99958b652 100644
--- a/drivers/mmc/host/tifm_sd.c
+++ b/drivers/mmc/host/tifm_sd.c
@@ -1032,7 +1032,7 @@ static void tifm_sd_remove(struct tifm_dev *sock)
 
 static int tifm_sd_suspend(struct tifm_dev *sock, pm_message_t state)
 {
-        return mmc_suspend_host(tifm_get_drvdata(sock), state);
+        return mmc_suspend_host(tifm_get_drvdata(sock));
 }
 
 static int tifm_sd_resume(struct tifm_dev *sock)
diff --git a/drivers/mmc/host/tmio_mmc.c b/drivers/mmc/host/tmio_mmc.c
index 883fcac21004..ee7d0a5a51c4 100644
--- a/drivers/mmc/host/tmio_mmc.c
+++ b/drivers/mmc/host/tmio_mmc.c
@@ -768,7 +768,7 @@ static int tmio_mmc_suspend(struct platform_device *dev, pm_message_t state)
         struct mmc_host *mmc = platform_get_drvdata(dev);
         int ret;
 
-        ret = mmc_suspend_host(mmc, state);
+        ret = mmc_suspend_host(mmc);
 
         /* Tell MFD core it can disable us now.*/
         if (!ret && cell->disable)
diff --git a/drivers/mmc/host/via-sdmmc.c b/drivers/mmc/host/via-sdmmc.c
index 632858a94376..19f2d72dbca5 100644
--- a/drivers/mmc/host/via-sdmmc.c
+++ b/drivers/mmc/host/via-sdmmc.c
@@ -1280,7 +1280,7 @@ static int via_sd_suspend(struct pci_dev *pcidev, pm_message_t state)
         via_save_pcictrlreg(host);
         via_save_sdcreg(host);
 
-        ret = mmc_suspend_host(host->mmc, state);
+        ret = mmc_suspend_host(host->mmc);
 
         pci_save_state(pcidev);
         pci_enable_wake(pcidev, pci_choose_state(pcidev, state), 0);
diff --git a/drivers/mmc/host/wbsd.c b/drivers/mmc/host/wbsd.c
index 69efe01eece8..0012f5d13d28 100644
--- a/drivers/mmc/host/wbsd.c
+++ b/drivers/mmc/host/wbsd.c
@@ -1819,7 +1819,7 @@ static int wbsd_suspend(struct wbsd_host *host, pm_message_t state)
 {
         BUG_ON(host == NULL);
 
-        return mmc_suspend_host(host->mmc, state);
+        return mmc_suspend_host(host->mmc);
 }
 
 static int wbsd_resume(struct wbsd_host *host)
diff --git a/drivers/mtd/maps/pismo.c b/drivers/mtd/maps/pismo.c
index eb476b7f8d11..f4ce273e93fd 100644
--- a/drivers/mtd/maps/pismo.c
+++ b/drivers/mtd/maps/pismo.c
@@ -234,7 +234,6 @@ static int __devexit pismo_remove(struct i2c_client *client)
         /* FIXME: set_vpp needs saner arguments */
         pismo_setvpp_remove_fix(pismo);
 
-        i2c_set_clientdata(client, NULL);
         kfree(pismo);
 
         return 0;
@@ -286,7 +285,6 @@ static int __devinit pismo_probe(struct i2c_client *client,
         return 0;
 
  exit_free:
-        i2c_set_clientdata(client, NULL);
         kfree(pismo);
         return ret;
 }
diff --git a/drivers/mtd/mtdchar.c b/drivers/mtd/mtdchar.c
index 000d65ea55a4..91c8013cf0d9 100644
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -404,14 +404,9 @@ static int mtd_do_writeoob(struct file *file, struct mtd_info *mtd,
         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;
-        }
+        ops.oobbuf = memdup_user(ptr, length);
+        if (IS_ERR(ops.oobbuf))
+                return PTR_ERR(ops.oobbuf);
 
         start &= ~((uint64_t)mtd->oobsize - 1);
         ret = mtd->write_oob(mtd, start, &ops);
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 98a04b3c9526..ffc3720929f1 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -1,13 +1,3 @@
-menuconfig MTD_NAND
-        tristate "NAND Device Support"
-        depends on MTD
-        select MTD_NAND_IDS
-        select MTD_NAND_ECC
-        help
-          This enables support for accessing all type of NAND flash
-          devices. For further information see
-          <http://www.linux-mtd.infradead.org/doc/nand.html>.
-
 config MTD_NAND_ECC
         tristate
 
@@ -19,6 +9,17 @@ config MTD_NAND_ECC_SMC
           Software ECC according to the Smart Media Specification.
           The original Linux implementation had byte 0 and 1 swapped.
 
+
+menuconfig MTD_NAND
+        tristate "NAND Device Support"
+        depends on MTD
+        select MTD_NAND_IDS
+        select MTD_NAND_ECC
+        help
+          This enables support for accessing all type of NAND flash
+          devices. For further information see
+          <http://www.linux-mtd.infradead.org/doc/nand.html>.
+
 if MTD_NAND
 
 config MTD_NAND_VERIFY_WRITE
diff --git a/drivers/mtd/nand/fsl_upm.c b/drivers/mtd/nand/fsl_upm.c
index 00aea6f7d1f1..1312eda57ba6 100644
--- a/drivers/mtd/nand/fsl_upm.c
+++ b/drivers/mtd/nand/fsl_upm.c
@@ -232,7 +232,7 @@ static int __devinit fun_probe(struct of_device *ofdev,
         if (!fun)
                 return -ENOMEM;
 
-        ret = of_address_to_resource(ofdev->node, 0, &io_res);
+        ret = of_address_to_resource(ofdev->dev.of_node, 0, &io_res);
         if (ret) {
                 dev_err(&ofdev->dev, "can't get IO base\n");
                 goto err1;
@@ -244,7 +244,8 @@ static int __devinit fun_probe(struct of_device *ofdev,
                 goto err1;
         }
 
-        prop = of_get_property(ofdev->node, "fsl,upm-addr-offset", &size);
+        prop = of_get_property(ofdev->dev.of_node, "fsl,upm-addr-offset",
+                               &size);
         if (!prop || size != sizeof(uint32_t)) {
                 dev_err(&ofdev->dev, "can't get UPM address offset\n");
                 ret = -EINVAL;
@@ -252,7 +253,7 @@ static int __devinit fun_probe(struct of_device *ofdev,
         }
         fun->upm_addr_offset = *prop;
 
-        prop = of_get_property(ofdev->node, "fsl,upm-cmd-offset", &size);
+        prop = of_get_property(ofdev->dev.of_node, "fsl,upm-cmd-offset", &size);
         if (!prop || size != sizeof(uint32_t)) {
                 dev_err(&ofdev->dev, "can't get UPM command offset\n");
                 ret = -EINVAL;
@@ -260,7 +261,7 @@ static int __devinit fun_probe(struct of_device *ofdev,
         }
         fun->upm_cmd_offset = *prop;
 
-        prop = of_get_property(ofdev->node,
+        prop = of_get_property(ofdev->dev.of_node,
                                "fsl,upm-addr-line-cs-offsets", &size);
         if (prop && (size / sizeof(uint32_t)) > 0) {
                 fun->mchip_count = size / sizeof(uint32_t);
@@ -276,7 +277,7 @@ static int __devinit fun_probe(struct of_device *ofdev,
 
         for (i = 0; i < fun->mchip_count; i++) {
                 fun->rnb_gpio[i] = -1;
-                rnb_gpio = of_get_gpio(ofdev->node, i);
+                rnb_gpio = of_get_gpio(ofdev->dev.of_node, i);
                 if (rnb_gpio >= 0) {
                         ret = gpio_request(rnb_gpio, dev_name(&ofdev->dev));
                         if (ret) {
@@ -292,13 +293,13 @@ static int __devinit fun_probe(struct of_device *ofdev,
                 }
         }
 
-        prop = of_get_property(ofdev->node, "chip-delay", NULL);
+        prop = of_get_property(ofdev->dev.of_node, "chip-delay", NULL);
         if (prop)
                 fun->chip_delay = *prop;
         else
                 fun->chip_delay = 50;
 
-        prop = of_get_property(ofdev->node, "fsl,upm-wait-flags", &size);
+        prop = of_get_property(ofdev->dev.of_node, "fsl,upm-wait-flags", &size);
         if (prop && size == sizeof(uint32_t))
                 fun->wait_flags = *prop;
         else
@@ -315,7 +316,7 @@ static int __devinit fun_probe(struct of_device *ofdev,
         fun->dev = &ofdev->dev;
         fun->last_ctrl = NAND_CLE;
 
-        ret = fun_chip_init(fun, ofdev->node, &io_res);
+        ret = fun_chip_init(fun, ofdev->dev.of_node, &io_res);
         if (ret)
                 goto err2;
 
diff --git a/drivers/mtd/nand/mpc5121_nfc.c b/drivers/mtd/nand/mpc5121_nfc.c
index 3d0867d829cb..0a130dcaa129 100644
--- a/drivers/mtd/nand/mpc5121_nfc.c
+++ b/drivers/mtd/nand/mpc5121_nfc.c
@@ -650,7 +650,7 @@ static void mpc5121_nfc_free(struct device *dev, struct mtd_info *mtd)
 static int __devinit mpc5121_nfc_probe(struct of_device *op,
                                         const struct of_device_id *match)
 {
-        struct device_node *rootnode, *dn = op->node;
+        struct device_node *rootnode, *dn = op->dev.of_node;
         struct device *dev = &op->dev;
         struct mpc5121_nfc_prv *prv;
         struct resource res;
@@ -889,12 +889,12 @@ static struct of_device_id mpc5121_nfc_match[] __devinitdata = {
 };
 
 static struct of_platform_driver mpc5121_nfc_driver = {
-        .match_table    = mpc5121_nfc_match,
         .probe          = mpc5121_nfc_probe,
         .remove         = __devexit_p(mpc5121_nfc_remove),
         .driver         = {
-                .name   = DRV_NAME,
-                .owner  = THIS_MODULE,
+                .name = DRV_NAME,
+                .owner = THIS_MODULE,
+                .of_match_table = mpc5121_nfc_match,
         },
 };
 
diff --git a/drivers/mtd/nand/r852.c b/drivers/mtd/nand/r852.c
index 78a423295474..bcfc851fe550 100644
--- a/drivers/mtd/nand/r852.c
+++ b/drivers/mtd/nand/r852.c
@@ -150,7 +150,6 @@ static void r852_dma_done(struct r852_device *dev, int error)
         if (dev->phys_dma_addr && dev->phys_dma_addr != dev->phys_bounce_buffer)
                 pci_unmap_single(dev->pci_dev, dev->phys_dma_addr, R852_DMA_LEN,
                         dev->dma_dir ? PCI_DMA_FROMDEVICE : PCI_DMA_TODEVICE);
-        complete(&dev->dma_done);
 }
 
 /*
@@ -182,6 +181,7 @@ static void r852_do_dma(struct r852_device *dev, uint8_t *buf, int do_read)
         /* Set dma direction */
         dev->dma_dir = do_read;
         dev->dma_stage = 1;
+        INIT_COMPLETION(dev->dma_done);
 
         dbg_verbose("doing dma %s ", do_read ? "read" : "write");
 
@@ -494,6 +494,11 @@ int r852_ecc_correct(struct mtd_info *mtd, uint8_t *dat,
         if (dev->card_unstable)
                 return 0;
 
+        if (dev->dma_error) {
+                dev->dma_error = 0;
+                return -1;
+        }
+
         r852_write_reg(dev, R852_CTL, dev->ctlreg | R852_CTL_ECC_ACCESS);
         ecc_reg = r852_read_reg_dword(dev, R852_DATALINE);
         r852_write_reg(dev, R852_CTL, dev->ctlreg);
@@ -707,6 +712,7 @@ void r852_card_detect_work(struct work_struct *work)
                 container_of(work, struct r852_device, card_detect_work.work);
 
         r852_card_update_present(dev);
+        r852_update_card_detect(dev);
         dev->card_unstable = 0;
 
         /* False alarm */
@@ -722,7 +728,6 @@ void r852_card_detect_work(struct work_struct *work)
         else
                 r852_unregister_nand_device(dev);
 exit:
-        /* Update detection logic */
         r852_update_card_detect(dev);
 }
 
@@ -796,6 +801,7 @@ static irqreturn_t r852_irq(int irq, void *data)
                 if (dma_status & R852_DMA_IRQ_ERROR) {
                         dbg("recieved dma error IRQ");
                         r852_dma_done(dev, -EIO);
+                        complete(&dev->dma_done);
                         goto out;
                 }
 
@@ -825,8 +831,10 @@ static irqreturn_t r852_irq(int irq, void *data)
                         r852_dma_enable(dev);
 
                 /* Operation done */
-                if (dev->dma_stage == 3)
+                if (dev->dma_stage == 3) {
                         r852_dma_done(dev, 0);
+                        complete(&dev->dma_done);
+                }
                 goto out;
         }
 
@@ -940,18 +948,19 @@ int  r852_probe(struct pci_dev *pci_dev, const struct pci_device_id *id)
 
         r852_dma_test(dev);
 
+        dev->irq = pci_dev->irq;
+        spin_lock_init(&dev->irqlock);
+
+        dev->card_detected = 0;
+        r852_card_update_present(dev);
+
         /*register irq handler*/
         error = -ENODEV;
         if (request_irq(pci_dev->irq, &r852_irq, IRQF_SHARED,
                           DRV_NAME, dev))
                 goto error10;
 
-        dev->irq = pci_dev->irq;
-        spin_lock_init(&dev->irqlock);
-
         /* kick initial present test */
-        dev->card_detected = 0;
-        r852_card_update_present(dev);
         queue_delayed_work(dev->card_workqueue,
                 &dev->card_detect_work, 0);
 
@@ -1081,7 +1090,7 @@ int r852_resume(struct device *device)
                         dev->card_detected ? "added" : "removed");
 
                 queue_delayed_work(dev->card_workqueue,
-                &dev->card_detect_work, 1000);
+                &dev->card_detect_work, msecs_to_jiffies(1000));
                 return 0;
         }
 
diff --git a/drivers/mtd/nand/socrates_nand.c b/drivers/mtd/nand/socrates_nand.c
index 884852dc7eb4..cc728b12de82 100644
--- a/drivers/mtd/nand/socrates_nand.c
+++ b/drivers/mtd/nand/socrates_nand.c
@@ -183,7 +183,7 @@ static int __devinit socrates_nand_probe(struct of_device *ofdev,
                 return -ENOMEM;
         }
 
-        host->io_base = of_iomap(ofdev->node, 0);
+        host->io_base = of_iomap(ofdev->dev.of_node, 0);
         if (host->io_base == NULL) {
                 printk(KERN_ERR "socrates_nand: ioremap failed\n");
                 kfree(host);
@@ -244,7 +244,7 @@ static int __devinit socrates_nand_probe(struct of_device *ofdev,
 #ifdef CONFIG_MTD_OF_PARTS
         if (num_partitions == 0) {
                 num_partitions = of_mtd_parse_partitions(&ofdev->dev,
-                                                         ofdev->node,
+                                                         ofdev->dev.of_node,
                                                          &partitions);
                 if (num_partitions < 0) {
                         res = num_partitions;
diff --git a/drivers/mtd/ubi/cdev.c b/drivers/mtd/ubi/cdev.c
index 72ebb3f06b86..4dfa6b90c21c 100644
--- a/drivers/mtd/ubi/cdev.c
+++ b/drivers/mtd/ubi/cdev.c
@@ -189,8 +189,7 @@ static loff_t vol_cdev_llseek(struct file *file, loff_t offset, int origin)
         return new_offset;
 }
 
-static int vol_cdev_fsync(struct file *file, struct dentry *dentry,
-                          int datasync)
+static int vol_cdev_fsync(struct file *file, int datasync)
 {
         struct ubi_volume_desc *desc = file->private_data;
         struct ubi_device *ubi = desc->vol->ubi;
diff --git a/drivers/net/3c507.c b/drivers/net/3c507.c
index 82eaf65d2d85..ea9b7a098c9b 100644
--- a/drivers/net/3c507.c
+++ b/drivers/net/3c507.c
@@ -551,8 +551,7 @@ static irqreturn_t el16_interrupt(int irq, void *dev_id)
         void __iomem *shmem;
 
         if (dev == NULL) {
-                pr_err("%s: net_interrupt(): irq %d for unknown device.\n",
-                        dev->name, irq);
+                pr_err("net_interrupt(): irq %d for unknown device.\n", irq);
                 return IRQ_NONE;
         }
 
diff --git a/drivers/net/8139cp.c b/drivers/net/8139cp.c
index 9c149750e2bf..284a5f4a63ac 100644
--- a/drivers/net/8139cp.c
+++ b/drivers/net/8139cp.c
@@ -598,8 +598,8 @@ rx_next:
                         goto rx_status_loop;
 
                 spin_lock_irqsave(&cp->lock, flags);
-                cpw16_f(IntrMask, cp_intr_mask);
                 __napi_complete(napi);
+                cpw16_f(IntrMask, cp_intr_mask);
                 spin_unlock_irqrestore(&cp->lock, flags);
         }
 
diff --git a/drivers/net/8139too.c b/drivers/net/8139too.c
index 4ba72933f0da..97d8068b372b 100644
--- a/drivers/net/8139too.c
+++ b/drivers/net/8139too.c
@@ -860,6 +860,7 @@ retry:
                 }
 
         /* if unknown chip, assume array element #0, original RTL-8139 in this case */
+        i = 0;
         dev_dbg(&pdev->dev, "unknown chip version, assuming RTL-8139\n");
         dev_dbg(&pdev->dev, "TxConfig = 0x%lx\n", RTL_R32 (TxConfig));
         tp->chipset = 0;
@@ -2088,8 +2089,8 @@ static int rtl8139_poll(struct napi_struct *napi, int budget)
                  * again when we think we are done.
                  */
                 spin_lock_irqsave(&tp->lock, flags);
-                RTL_W16_F(IntrMask, rtl8139_intr_mask);
                 __napi_complete(napi);
+                RTL_W16_F(IntrMask, rtl8139_intr_mask);
                 spin_unlock_irqrestore(&tp->lock, flags);
         }
         spin_unlock(&tp->rx_lock);
diff --git a/drivers/net/benet/be_cmds.c b/drivers/net/benet/be_cmds.c
index c911bfb55b19..b9ad799c719f 100644
--- a/drivers/net/benet/be_cmds.c
+++ b/drivers/net/benet/be_cmds.c
@@ -294,7 +294,7 @@ int be_cmd_POST(struct be_adapter *adapter)
                 } else {
                         return 0;
                 }
-        } while (timeout < 20);
+        } while (timeout < 40);
 
         dev_err(&adapter->pdev->dev, "POST timeout; stage=0x%x\n", stage);
         return -1;
@@ -1429,7 +1429,7 @@ int be_cmd_write_flashrom(struct be_adapter *adapter, struct be_dma_mem *cmd,
         wrb = wrb_from_mccq(adapter);
         if (!wrb) {
                 status = -EBUSY;
-                goto err;
+                goto err_unlock;
         }
         req = cmd->va;
         sge = nonembedded_sgl(wrb);
@@ -1457,7 +1457,10 @@ int be_cmd_write_flashrom(struct be_adapter *adapter, struct be_dma_mem *cmd,
         else
                 status = adapter->flash_status;
 
-err:
+        return status;
+
+err_unlock:
+        spin_unlock_bh(&adapter->mcc_lock);
         return status;
 }
 
@@ -1497,7 +1500,7 @@ err:
         return status;
 }
 
-extern int be_cmd_enable_magic_wol(struct be_adapter *adapter, u8 *mac,
+int be_cmd_enable_magic_wol(struct be_adapter *adapter, u8 *mac,
                                 struct be_dma_mem *nonemb_cmd)
 {
         struct be_mcc_wrb *wrb;
@@ -1590,7 +1593,7 @@ int be_cmd_loopback_test(struct be_adapter *adapter, u32 port_num,
 
         be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_LOWLEVEL,
                         OPCODE_LOWLEVEL_LOOPBACK_TEST, sizeof(*req));
-        req->hdr.timeout = 4;
+        req->hdr.timeout = cpu_to_le32(4);
 
         req->pattern = cpu_to_le64(pattern);
         req->src_port = cpu_to_le32(port_num);
@@ -1662,7 +1665,7 @@ err:
         return status;
 }
 
-extern int be_cmd_get_seeprom_data(struct be_adapter *adapter,
+int be_cmd_get_seeprom_data(struct be_adapter *adapter,
                                 struct be_dma_mem *nonemb_cmd)
 {
         struct be_mcc_wrb *wrb;
diff --git a/drivers/net/benet/be_main.c b/drivers/net/benet/be_main.c
index aa065c71ddd8..54b14272f333 100644
--- a/drivers/net/benet/be_main.c
+++ b/drivers/net/benet/be_main.c
@@ -1861,7 +1861,7 @@ static int be_setup(struct be_adapter *adapter)
                                 goto if_destroy;
                         }
                         vf++;
-                } while (vf < num_vfs);
+                }
         } else if (!be_physfn(adapter)) {
                 status = be_cmd_mac_addr_query(adapter, mac,
                         MAC_ADDRESS_TYPE_NETWORK, false, adapter->if_handle);
diff --git a/drivers/net/bmac.c b/drivers/net/bmac.c
index 39250b2ca886..959add2410bf 100644
--- a/drivers/net/bmac.c
+++ b/drivers/net/bmac.c
@@ -1654,8 +1654,11 @@ MODULE_DEVICE_TABLE (of, bmac_match);
 
 static struct macio_driver bmac_driver =
 {
-        .name           = "bmac",
-        .match_table    = bmac_match,
+        .driver = {
+                .name           = "bmac",
+                .owner          = THIS_MODULE,
+                .of_match_table = bmac_match,
+        },
         .probe          = bmac_probe,
         .remove         = bmac_remove,
 #ifdef CONFIG_PM
diff --git a/drivers/net/bnx2.c b/drivers/net/bnx2.c
index 188e356c30a3..949d7a9dcf92 100644
--- a/drivers/net/bnx2.c
+++ b/drivers/net/bnx2.c
@@ -247,6 +247,7 @@ static const struct flash_spec flash_5709 = {
 MODULE_DEVICE_TABLE(pci, bnx2_pci_tbl);
 
 static void bnx2_init_napi(struct bnx2 *bp);
+static void bnx2_del_napi(struct bnx2 *bp);
 
 static inline u32 bnx2_tx_avail(struct bnx2 *bp, struct bnx2_tx_ring_info *txr)
 {
@@ -6270,6 +6271,7 @@ open_err:
         bnx2_free_skbs(bp);
         bnx2_free_irq(bp);
         bnx2_free_mem(bp);
+        bnx2_del_napi(bp);
         return rc;
 }
 
@@ -6537,6 +6539,7 @@ bnx2_close(struct net_device *dev)
         bnx2_free_irq(bp);
         bnx2_free_skbs(bp);
         bnx2_free_mem(bp);
+        bnx2_del_napi(bp);
         bp->link_up = 0;
         netif_carrier_off(bp->dev);
         bnx2_set_power_state(bp, PCI_D3hot);
@@ -8227,7 +8230,16 @@ bnx2_bus_string(struct bnx2 *bp, char *str)
         return str;
 }
 
-static void __devinit
+static void
+bnx2_del_napi(struct bnx2 *bp)
+{
+        int i;
+
+        for (i = 0; i < bp->irq_nvecs; i++)
+                netif_napi_del(&bp->bnx2_napi[i].napi);
+}
+
+static void
 bnx2_init_napi(struct bnx2 *bp)
 {
         int i;
diff --git a/drivers/net/can/Kconfig b/drivers/net/can/Kconfig
index 05b751719bd5..2c5227c02fa0 100644
--- a/drivers/net/can/Kconfig
+++ b/drivers/net/can/Kconfig
@@ -63,6 +63,16 @@ config CAN_BFIN
           To compile this driver as a module, choose M here: the
           module will be called bfin_can.
 
+config CAN_JANZ_ICAN3
+        tristate "Janz VMOD-ICAN3 Intelligent CAN controller"
+        depends on CAN_DEV && MFD_JANZ_CMODIO
+        ---help---
+          Driver for Janz VMOD-ICAN3 Intelligent CAN controller module, which
+          connects to a MODULbus carrier board.
+
+          This driver can also be built as a module. If so, the module will be
+          called janz-ican3.ko.
+
 source "drivers/net/can/mscan/Kconfig"
 
 source "drivers/net/can/sja1000/Kconfig"
diff --git a/drivers/net/can/Makefile b/drivers/net/can/Makefile
index 7a702f28d01c..9047cd066fea 100644
--- a/drivers/net/can/Makefile
+++ b/drivers/net/can/Makefile
@@ -15,5 +15,6 @@ obj-$(CONFIG_CAN_AT91)		+= at91_can.o
 obj-$(CONFIG_CAN_TI_HECC)       += ti_hecc.o
 obj-$(CONFIG_CAN_MCP251X)       += mcp251x.o
 obj-$(CONFIG_CAN_BFIN)          += bfin_can.o
+obj-$(CONFIG_CAN_JANZ_ICAN3)    += janz-ican3.o
 
 ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
diff --git a/drivers/net/can/janz-ican3.c b/drivers/net/can/janz-ican3.c
new file mode 100644
index 000000000000..6e533dcc36c0
--- /dev/null
+++ b/drivers/net/can/janz-ican3.c
@@ -0,0 +1,1830 @@
+/*
+ * Janz MODULbus VMOD-ICAN3 CAN Interface Driver
+ *
+ * Copyright (c) 2010 Ira W. Snyder <iws@ovro.caltech.edu>
+ *
+ * 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/init.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+
+#include <linux/netdevice.h>
+#include <linux/can.h>
+#include <linux/can/dev.h>
+#include <linux/can/error.h>
+
+#include <linux/mfd/janz.h>
+
+/* the DPM has 64k of memory, organized into 256x 256 byte pages */
+#define DPM_NUM_PAGES           256
+#define DPM_PAGE_SIZE           256
+#define DPM_PAGE_ADDR(p)        ((p) * DPM_PAGE_SIZE)
+
+/* JANZ ICAN3 "old-style" host interface queue page numbers */
+#define QUEUE_OLD_CONTROL       0
+#define QUEUE_OLD_RB0           1
+#define QUEUE_OLD_RB1           2
+#define QUEUE_OLD_WB0           3
+#define QUEUE_OLD_WB1           4
+
+/* Janz ICAN3 "old-style" host interface control registers */
+#define MSYNC_PEER              0x00            /* ICAN only */
+#define MSYNC_LOCL              0x01            /* host only */
+#define TARGET_RUNNING          0x02
+
+#define MSYNC_RB0               0x01
+#define MSYNC_RB1               0x02
+#define MSYNC_RBLW              0x04
+#define MSYNC_RB_MASK           (MSYNC_RB0 | MSYNC_RB1)
+
+#define MSYNC_WB0               0x10
+#define MSYNC_WB1               0x20
+#define MSYNC_WBLW              0x40
+#define MSYNC_WB_MASK           (MSYNC_WB0 | MSYNC_WB1)
+
+/* Janz ICAN3 "new-style" host interface queue page numbers */
+#define QUEUE_TOHOST            5
+#define QUEUE_FROMHOST_MID      6
+#define QUEUE_FROMHOST_HIGH     7
+#define QUEUE_FROMHOST_LOW      8
+
+/* The first free page in the DPM is #9 */
+#define DPM_FREE_START          9
+
+/* Janz ICAN3 "new-style" and "fast" host interface descriptor flags */
+#define DESC_VALID              0x80
+#define DESC_WRAP               0x40
+#define DESC_INTERRUPT          0x20
+#define DESC_IVALID             0x10
+#define DESC_LEN(len)           (len)
+
+/* Janz ICAN3 Firmware Messages */
+#define MSG_CONNECTI            0x02
+#define MSG_DISCONNECT          0x03
+#define MSG_IDVERS              0x04
+#define MSG_MSGLOST             0x05
+#define MSG_NEWHOSTIF           0x08
+#define MSG_INQUIRY             0x0a
+#define MSG_SETAFILMASK         0x10
+#define MSG_INITFDPMQUEUE       0x11
+#define MSG_HWCONF              0x12
+#define MSG_FMSGLOST            0x15
+#define MSG_CEVTIND             0x37
+#define MSG_CBTRREQ             0x41
+#define MSG_COFFREQ             0x42
+#define MSG_CONREQ              0x43
+#define MSG_CCONFREQ            0x47
+
+/*
+ * Janz ICAN3 CAN Inquiry Message Types
+ *
+ * NOTE: there appears to be a firmware bug here. You must send
+ * NOTE: INQUIRY_STATUS and expect to receive an INQUIRY_EXTENDED
+ * NOTE: response. The controller never responds to a message with
+ * NOTE: the INQUIRY_EXTENDED subspec :(
+ */
+#define INQUIRY_STATUS          0x00
+#define INQUIRY_TERMINATION     0x01
+#define INQUIRY_EXTENDED        0x04
+
+/* Janz ICAN3 CAN Set Acceptance Filter Mask Message Types */
+#define SETAFILMASK_REJECT      0x00
+#define SETAFILMASK_FASTIF      0x02
+
+/* Janz ICAN3 CAN Hardware Configuration Message Types */
+#define HWCONF_TERMINATE_ON     0x01
+#define HWCONF_TERMINATE_OFF    0x00
+
+/* Janz ICAN3 CAN Event Indication Message Types */
+#define CEVTIND_EI              0x01
+#define CEVTIND_DOI             0x02
+#define CEVTIND_LOST            0x04
+#define CEVTIND_FULL            0x08
+#define CEVTIND_BEI             0x10
+
+#define CEVTIND_CHIP_SJA1000    0x02
+
+#define ICAN3_BUSERR_QUOTA_MAX  255
+
+/* Janz ICAN3 CAN Frame Conversion */
+#define ICAN3_ECHO      0x10
+#define ICAN3_EFF_RTR   0x40
+#define ICAN3_SFF_RTR   0x10
+#define ICAN3_EFF       0x80
+
+#define ICAN3_CAN_TYPE_MASK     0x0f
+#define ICAN3_CAN_TYPE_SFF      0x00
+#define ICAN3_CAN_TYPE_EFF      0x01
+
+#define ICAN3_CAN_DLC_MASK      0x0f
+
+/*
+ * SJA1000 Status and Error Register Definitions
+ *
+ * Copied from drivers/net/can/sja1000/sja1000.h
+ */
+
+/* status register content */
+#define SR_BS           0x80
+#define SR_ES           0x40
+#define SR_TS           0x20
+#define SR_RS           0x10
+#define SR_TCS          0x08
+#define SR_TBS          0x04
+#define SR_DOS          0x02
+#define SR_RBS          0x01
+
+#define SR_CRIT (SR_BS|SR_ES)
+
+/* ECC register */
+#define ECC_SEG         0x1F
+#define ECC_DIR         0x20
+#define ECC_ERR         6
+#define ECC_BIT         0x00
+#define ECC_FORM        0x40
+#define ECC_STUFF       0x80
+#define ECC_MASK        0xc0
+
+/* Number of buffers for use in the "new-style" host interface */
+#define ICAN3_NEW_BUFFERS       16
+
+/* Number of buffers for use in the "fast" host interface */
+#define ICAN3_TX_BUFFERS        512
+#define ICAN3_RX_BUFFERS        1024
+
+/* SJA1000 Clock Input */
+#define ICAN3_CAN_CLOCK         8000000
+
+/* Driver Name */
+#define DRV_NAME "janz-ican3"
+
+/* DPM Control Registers -- starts at offset 0x100 in the MODULbus registers */
+struct ican3_dpm_control {
+        /* window address register */
+        u8 window_address;
+        u8 unused1;
+
+        /*
+         * Read access: clear interrupt from microcontroller
+         * Write access: send interrupt to microcontroller
+         */
+        u8 interrupt;
+        u8 unused2;
+
+        /* write-only: reset all hardware on the module */
+        u8 hwreset;
+        u8 unused3;
+
+        /* write-only: generate an interrupt to the TPU */
+        u8 tpuinterrupt;
+};
+
+struct ican3_dev {
+
+        /* must be the first member */
+        struct can_priv can;
+
+        /* CAN network device */
+        struct net_device *ndev;
+        struct napi_struct napi;
+
+        /* Device for printing */
+        struct device *dev;
+
+        /* module number */
+        unsigned int num;
+
+        /* base address of registers and IRQ */
+        struct janz_cmodio_onboard_regs __iomem *ctrl;
+        struct ican3_dpm_control __iomem *dpmctrl;
+        void __iomem *dpm;
+        int irq;
+
+        /* CAN bus termination status */
+        struct completion termination_comp;
+        bool termination_enabled;
+
+        /* CAN bus error status registers */
+        struct completion buserror_comp;
+        struct can_berr_counter bec;
+
+        /* old and new style host interface */
+        unsigned int iftype;
+
+        /*
+         * Any function which changes the current DPM page must hold this
+         * lock while it is performing data accesses. This ensures that the
+         * function will not be preempted and end up reading data from a
+         * different DPM page than it expects.
+         */
+        spinlock_t lock;
+
+        /* new host interface */
+        unsigned int rx_int;
+        unsigned int rx_num;
+        unsigned int tx_num;
+
+        /* fast host interface */
+        unsigned int fastrx_start;
+        unsigned int fastrx_int;
+        unsigned int fastrx_num;
+        unsigned int fasttx_start;
+        unsigned int fasttx_num;
+
+        /* first free DPM page */
+        unsigned int free_page;
+};
+
+struct ican3_msg {
+        u8 control;
+        u8 spec;
+        __le16 len;
+        u8 data[252];
+};
+
+struct ican3_new_desc {
+        u8 control;
+        u8 pointer;
+};
+
+struct ican3_fast_desc {
+        u8 control;
+        u8 command;
+        u8 data[14];
+};
+
+/* write to the window basic address register */
+static inline void ican3_set_page(struct ican3_dev *mod, unsigned int page)
+{
+        BUG_ON(page >= DPM_NUM_PAGES);
+        iowrite8(page, &mod->dpmctrl->window_address);
+}
+
+/*
+ * ICAN3 "old-style" host interface
+ */
+
+/*
+ * Recieve a message from the ICAN3 "old-style" firmware interface
+ *
+ * LOCKING: must hold mod->lock
+ *
+ * returns 0 on success, -ENOMEM when no message exists
+ */
+static int ican3_old_recv_msg(struct ican3_dev *mod, struct ican3_msg *msg)
+{
+        unsigned int mbox, mbox_page;
+        u8 locl, peer, xord;
+
+        /* get the MSYNC registers */
+        ican3_set_page(mod, QUEUE_OLD_CONTROL);
+        peer = ioread8(mod->dpm + MSYNC_PEER);
+        locl = ioread8(mod->dpm + MSYNC_LOCL);
+        xord = locl ^ peer;
+
+        if ((xord & MSYNC_RB_MASK) == 0x00) {
+                dev_dbg(mod->dev, "no mbox for reading\n");
+                return -ENOMEM;
+        }
+
+        /* find the first free mbox to read */
+        if ((xord & MSYNC_RB_MASK) == MSYNC_RB_MASK)
+                mbox = (xord & MSYNC_RBLW) ? MSYNC_RB0 : MSYNC_RB1;
+        else
+                mbox = (xord & MSYNC_RB0) ? MSYNC_RB0 : MSYNC_RB1;
+
+        /* copy the message */
+        mbox_page = (mbox == MSYNC_RB0) ? QUEUE_OLD_RB0 : QUEUE_OLD_RB1;
+        ican3_set_page(mod, mbox_page);
+        memcpy_fromio(msg, mod->dpm, sizeof(*msg));
+
+        /*
+         * notify the firmware that the read buffer is available
+         * for it to fill again
+         */
+        locl ^= mbox;
+
+        ican3_set_page(mod, QUEUE_OLD_CONTROL);
+        iowrite8(locl, mod->dpm + MSYNC_LOCL);
+        return 0;
+}
+
+/*
+ * Send a message through the "old-style" firmware interface
+ *
+ * LOCKING: must hold mod->lock
+ *
+ * returns 0 on success, -ENOMEM when no free space exists
+ */
+static int ican3_old_send_msg(struct ican3_dev *mod, struct ican3_msg *msg)
+{
+        unsigned int mbox, mbox_page;
+        u8 locl, peer, xord;
+
+        /* get the MSYNC registers */
+        ican3_set_page(mod, QUEUE_OLD_CONTROL);
+        peer = ioread8(mod->dpm + MSYNC_PEER);
+        locl = ioread8(mod->dpm + MSYNC_LOCL);
+        xord = locl ^ peer;
+
+        if ((xord & MSYNC_WB_MASK) == MSYNC_WB_MASK) {
+                dev_err(mod->dev, "no mbox for writing\n");
+                return -ENOMEM;
+        }
+
+        /* calculate a free mbox to use */
+        mbox = (xord & MSYNC_WB0) ? MSYNC_WB1 : MSYNC_WB0;
+
+        /* copy the message to the DPM */
+        mbox_page = (mbox == MSYNC_WB0) ? QUEUE_OLD_WB0 : QUEUE_OLD_WB1;
+        ican3_set_page(mod, mbox_page);
+        memcpy_toio(mod->dpm, msg, sizeof(*msg));
+
+        locl ^= mbox;
+        if (mbox == MSYNC_WB1)
+                locl |= MSYNC_WBLW;
+
+        ican3_set_page(mod, QUEUE_OLD_CONTROL);
+        iowrite8(locl, mod->dpm + MSYNC_LOCL);
+        return 0;
+}
+
+/*
+ * ICAN3 "new-style" Host Interface Setup
+ */
+
+static void __devinit ican3_init_new_host_interface(struct ican3_dev *mod)
+{
+        struct ican3_new_desc desc;
+        unsigned long flags;
+        void __iomem *dst;
+        int i;
+
+        spin_lock_irqsave(&mod->lock, flags);
+
+        /* setup the internal datastructures for RX */
+        mod->rx_num = 0;
+        mod->rx_int = 0;
+
+        /* tohost queue descriptors are in page 5 */
+        ican3_set_page(mod, QUEUE_TOHOST);
+        dst = mod->dpm;
+
+        /* initialize the tohost (rx) queue descriptors: pages 9-24 */
+        for (i = 0; i < ICAN3_NEW_BUFFERS; i++) {
+                desc.control = DESC_INTERRUPT | DESC_LEN(1); /* I L=1 */
+                desc.pointer = mod->free_page;
+
+                /* set wrap flag on last buffer */
+                if (i == ICAN3_NEW_BUFFERS - 1)
+                        desc.control |= DESC_WRAP;
+
+                memcpy_toio(dst, &desc, sizeof(desc));
+                dst += sizeof(desc);
+                mod->free_page++;
+        }
+
+        /* fromhost (tx) mid queue descriptors are in page 6 */
+        ican3_set_page(mod, QUEUE_FROMHOST_MID);
+        dst = mod->dpm;
+
+        /* setup the internal datastructures for TX */
+        mod->tx_num = 0;
+
+        /* initialize the fromhost mid queue descriptors: pages 25-40 */
+        for (i = 0; i < ICAN3_NEW_BUFFERS; i++) {
+                desc.control = DESC_VALID | DESC_LEN(1); /* V L=1 */
+                desc.pointer = mod->free_page;
+
+                /* set wrap flag on last buffer */
+                if (i == ICAN3_NEW_BUFFERS - 1)
+                        desc.control |= DESC_WRAP;
+
+                memcpy_toio(dst, &desc, sizeof(desc));
+                dst += sizeof(desc);
+                mod->free_page++;
+        }
+
+        /* fromhost hi queue descriptors are in page 7 */
+        ican3_set_page(mod, QUEUE_FROMHOST_HIGH);
+        dst = mod->dpm;
+
+        /* initialize only a single buffer in the fromhost hi queue (unused) */
+        desc.control = DESC_VALID | DESC_WRAP | DESC_LEN(1); /* VW L=1 */
+        desc.pointer = mod->free_page;
+        memcpy_toio(dst, &desc, sizeof(desc));
+        mod->free_page++;
+
+        /* fromhost low queue descriptors are in page 8 */
+        ican3_set_page(mod, QUEUE_FROMHOST_LOW);
+        dst = mod->dpm;
+
+        /* initialize only a single buffer in the fromhost low queue (unused) */
+        desc.control = DESC_VALID | DESC_WRAP | DESC_LEN(1); /* VW L=1 */
+        desc.pointer = mod->free_page;
+        memcpy_toio(dst, &desc, sizeof(desc));
+        mod->free_page++;
+
+        spin_unlock_irqrestore(&mod->lock, flags);
+}
+
+/*
+ * ICAN3 Fast Host Interface Setup
+ */
+
+static void __devinit ican3_init_fast_host_interface(struct ican3_dev *mod)
+{
+        struct ican3_fast_desc desc;
+        unsigned long flags;
+        unsigned int addr;
+        void __iomem *dst;
+        int i;
+
+        spin_lock_irqsave(&mod->lock, flags);
+
+        /* save the start recv page */
+        mod->fastrx_start = mod->free_page;
+        mod->fastrx_num = 0;
+        mod->fastrx_int = 0;
+
+        /* build a single fast tohost queue descriptor */
+        memset(&desc, 0, sizeof(desc));
+        desc.control = 0x00;
+        desc.command = 1;
+
+        /* build the tohost queue descriptor ring in memory */
+        addr = 0;
+        for (i = 0; i < ICAN3_RX_BUFFERS; i++) {
+
+                /* set the wrap bit on the last buffer */
+                if (i == ICAN3_RX_BUFFERS - 1)
+                        desc.control |= DESC_WRAP;
+
+                /* switch to the correct page */
+                ican3_set_page(mod, mod->free_page);
+
+                /* copy the descriptor to the DPM */
+                dst = mod->dpm + addr;
+                memcpy_toio(dst, &desc, sizeof(desc));
+                addr += sizeof(desc);
+
+                /* move to the next page if necessary */
+                if (addr >= DPM_PAGE_SIZE) {
+                        addr = 0;
+                        mod->free_page++;
+                }
+        }
+
+        /* make sure we page-align the next queue */
+        if (addr != 0)
+                mod->free_page++;
+
+        /* save the start xmit page */
+        mod->fasttx_start = mod->free_page;
+        mod->fasttx_num = 0;
+
+        /* build a single fast fromhost queue descriptor */
+        memset(&desc, 0, sizeof(desc));
+        desc.control = DESC_VALID;
+        desc.command = 1;
+
+        /* build the fromhost queue descriptor ring in memory */
+        addr = 0;
+        for (i = 0; i < ICAN3_TX_BUFFERS; i++) {
+
+                /* set the wrap bit on the last buffer */
+                if (i == ICAN3_TX_BUFFERS - 1)
+                        desc.control |= DESC_WRAP;
+
+                /* switch to the correct page */
+                ican3_set_page(mod, mod->free_page);
+
+                /* copy the descriptor to the DPM */
+                dst = mod->dpm + addr;
+                memcpy_toio(dst, &desc, sizeof(desc));
+                addr += sizeof(desc);
+
+                /* move to the next page if necessary */
+                if (addr >= DPM_PAGE_SIZE) {
+                        addr = 0;
+                        mod->free_page++;
+                }
+        }
+
+        spin_unlock_irqrestore(&mod->lock, flags);
+}
+
+/*
+ * ICAN3 "new-style" Host Interface Message Helpers
+ */
+
+/*
+ * LOCKING: must hold mod->lock
+ */
+static int ican3_new_send_msg(struct ican3_dev *mod, struct ican3_msg *msg)
+{
+        struct ican3_new_desc desc;
+        void __iomem *desc_addr = mod->dpm + (mod->tx_num * sizeof(desc));
+
+        /* switch to the fromhost mid queue, and read the buffer descriptor */
+        ican3_set_page(mod, QUEUE_FROMHOST_MID);
+        memcpy_fromio(&desc, desc_addr, sizeof(desc));
+
+        if (!(desc.control & DESC_VALID)) {
+                dev_dbg(mod->dev, "%s: no free buffers\n", __func__);
+                return -ENOMEM;
+        }
+
+        /* switch to the data page, copy the data */
+        ican3_set_page(mod, desc.pointer);
+        memcpy_toio(mod->dpm, msg, sizeof(*msg));
+
+        /* switch back to the descriptor, set the valid bit, write it back */
+        ican3_set_page(mod, QUEUE_FROMHOST_MID);
+        desc.control ^= DESC_VALID;
+        memcpy_toio(desc_addr, &desc, sizeof(desc));
+
+        /* update the tx number */
+        mod->tx_num = (desc.control & DESC_WRAP) ? 0 : (mod->tx_num + 1);
+        return 0;
+}
+
+/*
+ * LOCKING: must hold mod->lock
+ */
+static int ican3_new_recv_msg(struct ican3_dev *mod, struct ican3_msg *msg)
+{
+        struct ican3_new_desc desc;
+        void __iomem *desc_addr = mod->dpm + (mod->rx_num * sizeof(desc));
+
+        /* switch to the tohost queue, and read the buffer descriptor */
+        ican3_set_page(mod, QUEUE_TOHOST);
+        memcpy_fromio(&desc, desc_addr, sizeof(desc));
+
+        if (!(desc.control & DESC_VALID)) {
+                dev_dbg(mod->dev, "%s: no buffers to recv\n", __func__);
+                return -ENOMEM;
+        }
+
+        /* switch to the data page, copy the data */
+        ican3_set_page(mod, desc.pointer);
+        memcpy_fromio(msg, mod->dpm, sizeof(*msg));
+
+        /* switch back to the descriptor, toggle the valid bit, write it back */
+        ican3_set_page(mod, QUEUE_TOHOST);
+        desc.control ^= DESC_VALID;
+        memcpy_toio(desc_addr, &desc, sizeof(desc));
+
+        /* update the rx number */
+        mod->rx_num = (desc.control & DESC_WRAP) ? 0 : (mod->rx_num + 1);
+        return 0;
+}
+
+/*
+ * Message Send / Recv Helpers
+ */
+
+static int ican3_send_msg(struct ican3_dev *mod, struct ican3_msg *msg)
+{
+        unsigned long flags;
+        int ret;
+
+        spin_lock_irqsave(&mod->lock, flags);
+
+        if (mod->iftype == 0)
+                ret = ican3_old_send_msg(mod, msg);
+        else
+                ret = ican3_new_send_msg(mod, msg);
+
+        spin_unlock_irqrestore(&mod->lock, flags);
+        return ret;
+}
+
+static int ican3_recv_msg(struct ican3_dev *mod, struct ican3_msg *msg)
+{
+        unsigned long flags;
+        int ret;
+
+        spin_lock_irqsave(&mod->lock, flags);
+
+        if (mod->iftype == 0)
+                ret = ican3_old_recv_msg(mod, msg);
+        else
+                ret = ican3_new_recv_msg(mod, msg);
+
+        spin_unlock_irqrestore(&mod->lock, flags);
+        return ret;
+}
+
+/*
+ * Quick Pre-constructed Messages
+ */
+
+static int __devinit ican3_msg_connect(struct ican3_dev *mod)
+{
+        struct ican3_msg msg;
+
+        memset(&msg, 0, sizeof(msg));
+        msg.spec = MSG_CONNECTI;
+        msg.len = cpu_to_le16(0);
+
+        return ican3_send_msg(mod, &msg);
+}
+
+static int __devexit ican3_msg_disconnect(struct ican3_dev *mod)
+{
+        struct ican3_msg msg;
+
+        memset(&msg, 0, sizeof(msg));
+        msg.spec = MSG_DISCONNECT;
+        msg.len = cpu_to_le16(0);
+
+        return ican3_send_msg(mod, &msg);
+}
+
+static int __devinit ican3_msg_newhostif(struct ican3_dev *mod)
+{
+        struct ican3_msg msg;
+        int ret;
+
+        memset(&msg, 0, sizeof(msg));
+        msg.spec = MSG_NEWHOSTIF;
+        msg.len = cpu_to_le16(0);
+
+        /* If we're not using the old interface, switching seems bogus */
+        WARN_ON(mod->iftype != 0);
+
+        ret = ican3_send_msg(mod, &msg);
+        if (ret)
+                return ret;
+
+        /* mark the module as using the new host interface */
+        mod->iftype = 1;
+        return 0;
+}
+
+static int __devinit ican3_msg_fasthostif(struct ican3_dev *mod)
+{
+        struct ican3_msg msg;
+        unsigned int addr;
+
+        memset(&msg, 0, sizeof(msg));
+        msg.spec = MSG_INITFDPMQUEUE;
+        msg.len = cpu_to_le16(8);
+
+        /* write the tohost queue start address */
+        addr = DPM_PAGE_ADDR(mod->fastrx_start);
+        msg.data[0] = addr & 0xff;
+        msg.data[1] = (addr >> 8) & 0xff;
+        msg.data[2] = (addr >> 16) & 0xff;
+        msg.data[3] = (addr >> 24) & 0xff;
+
+        /* write the fromhost queue start address */
+        addr = DPM_PAGE_ADDR(mod->fasttx_start);
+        msg.data[4] = addr & 0xff;
+        msg.data[5] = (addr >> 8) & 0xff;
+        msg.data[6] = (addr >> 16) & 0xff;
+        msg.data[7] = (addr >> 24) & 0xff;
+
+        /* If we're not using the new interface yet, we cannot do this */
+        WARN_ON(mod->iftype != 1);
+
+        return ican3_send_msg(mod, &msg);
+}
+
+/*
+ * Setup the CAN filter to either accept or reject all
+ * messages from the CAN bus.
+ */
+static int __devinit ican3_set_id_filter(struct ican3_dev *mod, bool accept)
+{
+        struct ican3_msg msg;
+        int ret;
+
+        /* Standard Frame Format */
+        memset(&msg, 0, sizeof(msg));
+        msg.spec = MSG_SETAFILMASK;
+        msg.len = cpu_to_le16(5);
+        msg.data[0] = 0x00; /* IDLo LSB */
+        msg.data[1] = 0x00; /* IDLo MSB */
+        msg.data[2] = 0xff; /* IDHi LSB */
+        msg.data[3] = 0x07; /* IDHi MSB */
+
+        /* accept all frames for fast host if, or reject all frames */
+        msg.data[4] = accept ? SETAFILMASK_FASTIF : SETAFILMASK_REJECT;
+
+        ret = ican3_send_msg(mod, &msg);
+        if (ret)
+                return ret;
+
+        /* Extended Frame Format */
+        memset(&msg, 0, sizeof(msg));
+        msg.spec = MSG_SETAFILMASK;
+        msg.len = cpu_to_le16(13);
+        msg.data[0] = 0;    /* MUX = 0 */
+        msg.data[1] = 0x00; /* IDLo LSB */
+        msg.data[2] = 0x00;
+        msg.data[3] = 0x00;
+        msg.data[4] = 0x20; /* IDLo MSB */
+        msg.data[5] = 0xff; /* IDHi LSB */
+        msg.data[6] = 0xff;
+        msg.data[7] = 0xff;
+        msg.data[8] = 0x3f; /* IDHi MSB */
+
+        /* accept all frames for fast host if, or reject all frames */
+        msg.data[9] = accept ? SETAFILMASK_FASTIF : SETAFILMASK_REJECT;
+
+        return ican3_send_msg(mod, &msg);
+}
+
+/*
+ * Bring the CAN bus online or offline
+ */
+static int ican3_set_bus_state(struct ican3_dev *mod, bool on)
+{
+        struct ican3_msg msg;
+
+        memset(&msg, 0, sizeof(msg));
+        msg.spec = on ? MSG_CONREQ : MSG_COFFREQ;
+        msg.len = cpu_to_le16(0);
+
+        return ican3_send_msg(mod, &msg);
+}
+
+static int ican3_set_termination(struct ican3_dev *mod, bool on)
+{
+        struct ican3_msg msg;
+
+        memset(&msg, 0, sizeof(msg));
+        msg.spec = MSG_HWCONF;
+        msg.len = cpu_to_le16(2);
+        msg.data[0] = 0x00;
+        msg.data[1] = on ? HWCONF_TERMINATE_ON : HWCONF_TERMINATE_OFF;
+
+        return ican3_send_msg(mod, &msg);
+}
+
+static int ican3_send_inquiry(struct ican3_dev *mod, u8 subspec)
+{
+        struct ican3_msg msg;
+
+        memset(&msg, 0, sizeof(msg));
+        msg.spec = MSG_INQUIRY;
+        msg.len = cpu_to_le16(2);
+        msg.data[0] = subspec;
+        msg.data[1] = 0x00;
+
+        return ican3_send_msg(mod, &msg);
+}
+
+static int ican3_set_buserror(struct ican3_dev *mod, u8 quota)
+{
+        struct ican3_msg msg;
+
+        memset(&msg, 0, sizeof(msg));
+        msg.spec = MSG_CCONFREQ;
+        msg.len = cpu_to_le16(2);
+        msg.data[0] = 0x00;
+        msg.data[1] = quota;
+
+        return ican3_send_msg(mod, &msg);
+}
+
+/*
+ * ICAN3 to Linux CAN Frame Conversion
+ */
+
+static void ican3_to_can_frame(struct ican3_dev *mod,
+                               struct ican3_fast_desc *desc,
+                               struct can_frame *cf)
+{
+        if ((desc->command & ICAN3_CAN_TYPE_MASK) == ICAN3_CAN_TYPE_SFF) {
+                if (desc->data[1] & ICAN3_SFF_RTR)
+                        cf->can_id |= CAN_RTR_FLAG;
+
+                cf->can_id |= desc->data[0] << 3;
+                cf->can_id |= (desc->data[1] & 0xe0) >> 5;
+                cf->can_dlc = desc->data[1] & ICAN3_CAN_DLC_MASK;
+                memcpy(cf->data, &desc->data[2], sizeof(cf->data));
+        } else {
+                cf->can_dlc = desc->data[0] & ICAN3_CAN_DLC_MASK;
+                if (desc->data[0] & ICAN3_EFF_RTR)
+                        cf->can_id |= CAN_RTR_FLAG;
+
+                if (desc->data[0] & ICAN3_EFF) {
+                        cf->can_id |= CAN_EFF_FLAG;
+                        cf->can_id |= desc->data[2] << 21; /* 28-21 */
+                        cf->can_id |= desc->data[3] << 13; /* 20-13 */
+                        cf->can_id |= desc->data[4] << 5;  /* 12-5  */
+                        cf->can_id |= (desc->data[5] & 0xf8) >> 3;
+                } else {
+                        cf->can_id |= desc->data[2] << 3;  /* 10-3  */
+                        cf->can_id |= desc->data[3] >> 5;  /* 2-0   */
+                }
+
+                memcpy(cf->data, &desc->data[6], sizeof(cf->data));
+        }
+}
+
+static void can_frame_to_ican3(struct ican3_dev *mod,
+                               struct can_frame *cf,
+                               struct ican3_fast_desc *desc)
+{
+        /* clear out any stale data in the descriptor */
+        memset(desc->data, 0, sizeof(desc->data));
+
+        /* we always use the extended format, with the ECHO flag set */
+        desc->command = ICAN3_CAN_TYPE_EFF;
+        desc->data[0] |= cf->can_dlc;
+        desc->data[1] |= ICAN3_ECHO;
+
+        if (cf->can_id & CAN_RTR_FLAG)
+                desc->data[0] |= ICAN3_EFF_RTR;
+
+        /* pack the id into the correct places */
+        if (cf->can_id & CAN_EFF_FLAG) {
+                desc->data[0] |= ICAN3_EFF;
+                desc->data[2] = (cf->can_id & 0x1fe00000) >> 21; /* 28-21 */
+                desc->data[3] = (cf->can_id & 0x001fe000) >> 13; /* 20-13 */
+                desc->data[4] = (cf->can_id & 0x00001fe0) >> 5;  /* 12-5  */
+                desc->data[5] = (cf->can_id & 0x0000001f) << 3;  /* 4-0   */
+        } else {
+                desc->data[2] = (cf->can_id & 0x7F8) >> 3; /* bits 10-3 */
+                desc->data[3] = (cf->can_id & 0x007) << 5; /* bits 2-0  */
+        }
+
+        /* copy the data bits into the descriptor */
+        memcpy(&desc->data[6], cf->data, sizeof(cf->data));
+}
+
+/*
+ * Interrupt Handling
+ */
+
+/*
+ * Handle an ID + Version message response from the firmware. We never generate
+ * this message in production code, but it is very useful when debugging to be
+ * able to display this message.
+ */
+static void ican3_handle_idvers(struct ican3_dev *mod, struct ican3_msg *msg)
+{
+        dev_dbg(mod->dev, "IDVERS response: %s\n", msg->data);
+}
+
+static void ican3_handle_msglost(struct ican3_dev *mod, struct ican3_msg *msg)
+{
+        struct net_device *dev = mod->ndev;
+        struct net_device_stats *stats = &dev->stats;
+        struct can_frame *cf;
+        struct sk_buff *skb;
+
+        /*
+         * Report that communication messages with the microcontroller firmware
+         * are being lost. These are never CAN frames, so we do not generate an
+         * error frame for userspace
+         */
+        if (msg->spec == MSG_MSGLOST) {
+                dev_err(mod->dev, "lost %d control messages\n", msg->data[0]);
+                return;
+        }
+
+        /*
+         * Oops, this indicates that we have lost messages in the fast queue,
+         * which are exclusively CAN messages. Our driver isn't reading CAN
+         * frames fast enough.
+         *
+         * We'll pretend that the SJA1000 told us that it ran out of buffer
+         * space, because there is not a better message for this.
+         */
+        skb = alloc_can_err_skb(dev, &cf);
+        if (skb) {
+                cf->can_id |= CAN_ERR_CRTL;
+                cf->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
+                stats->rx_errors++;
+                stats->rx_bytes += cf->can_dlc;
+                netif_rx(skb);
+        }
+}
+
+/*
+ * Handle CAN Event Indication Messages from the firmware
+ *
+ * The ICAN3 firmware provides the values of some SJA1000 registers when it
+ * generates this message. The code below is largely copied from the
+ * drivers/net/can/sja1000/sja1000.c file, and adapted as necessary
+ */
+static int ican3_handle_cevtind(struct ican3_dev *mod, struct ican3_msg *msg)
+{
+        struct net_device *dev = mod->ndev;
+        struct net_device_stats *stats = &dev->stats;
+        enum can_state state = mod->can.state;
+        u8 status, isrc, rxerr, txerr;
+        struct can_frame *cf;
+        struct sk_buff *skb;
+
+        /* we can only handle the SJA1000 part */
+        if (msg->data[1] != CEVTIND_CHIP_SJA1000) {
+                dev_err(mod->dev, "unable to handle errors on non-SJA1000\n");
+                return -ENODEV;
+        }
+
+        /* check the message length for sanity */
+        if (le16_to_cpu(msg->len) < 6) {
+                dev_err(mod->dev, "error message too short\n");
+                return -EINVAL;
+        }
+
+        skb = alloc_can_err_skb(dev, &cf);
+        if (skb == NULL)
+                return -ENOMEM;
+
+        isrc = msg->data[0];
+        status = msg->data[3];
+        rxerr = msg->data[4];
+        txerr = msg->data[5];
+
+        /* data overrun interrupt */
+        if (isrc == CEVTIND_DOI || isrc == CEVTIND_LOST) {
+                dev_dbg(mod->dev, "data overrun interrupt\n");
+                cf->can_id |= CAN_ERR_CRTL;
+                cf->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
+                stats->rx_over_errors++;
+                stats->rx_errors++;
+        }
+
+        /* error warning + passive interrupt */
+        if (isrc == CEVTIND_EI) {
+                dev_dbg(mod->dev, "error warning + passive interrupt\n");
+                if (status & SR_BS) {
+                        state = CAN_STATE_BUS_OFF;
+                        cf->can_id |= CAN_ERR_BUSOFF;
+                        can_bus_off(dev);
+                } else if (status & SR_ES) {
+                        if (rxerr >= 128 || txerr >= 128)
+                                state = CAN_STATE_ERROR_PASSIVE;
+                        else
+                                state = CAN_STATE_ERROR_WARNING;
+                } else {
+                        state = CAN_STATE_ERROR_ACTIVE;
+                }
+        }
+
+        /* bus error interrupt */
+        if (isrc == CEVTIND_BEI) {
+                u8 ecc = msg->data[2];
+
+                dev_dbg(mod->dev, "bus error interrupt\n");
+                mod->can.can_stats.bus_error++;
+                stats->rx_errors++;
+                cf->can_id |= CAN_ERR_PROT | CAN_ERR_BUSERROR;
+
+                switch (ecc & ECC_MASK) {
+                case ECC_BIT:
+                        cf->data[2] |= CAN_ERR_PROT_BIT;
+                        break;
+                case ECC_FORM:
+                        cf->data[2] |= CAN_ERR_PROT_FORM;
+                        break;
+                case ECC_STUFF:
+                        cf->data[2] |= CAN_ERR_PROT_STUFF;
+                        break;
+                default:
+                        cf->data[2] |= CAN_ERR_PROT_UNSPEC;
+                        cf->data[3] = ecc & ECC_SEG;
+                        break;
+                }
+
+                if ((ecc & ECC_DIR) == 0)
+                        cf->data[2] |= CAN_ERR_PROT_TX;
+
+                cf->data[6] = txerr;
+                cf->data[7] = rxerr;
+        }
+
+        if (state != mod->can.state && (state == CAN_STATE_ERROR_WARNING ||
+                                        state == CAN_STATE_ERROR_PASSIVE)) {
+                cf->can_id |= CAN_ERR_CRTL;
+                if (state == CAN_STATE_ERROR_WARNING) {
+                        mod->can.can_stats.error_warning++;
+                        cf->data[1] = (txerr > rxerr) ?
+                                CAN_ERR_CRTL_TX_WARNING :
+                                CAN_ERR_CRTL_RX_WARNING;
+                } else {
+                        mod->can.can_stats.error_passive++;
+                        cf->data[1] = (txerr > rxerr) ?
+                                CAN_ERR_CRTL_TX_PASSIVE :
+                                CAN_ERR_CRTL_RX_PASSIVE;
+                }
+
+                cf->data[6] = txerr;
+                cf->data[7] = rxerr;
+        }
+
+        mod->can.state = state;
+        stats->rx_errors++;
+        stats->rx_bytes += cf->can_dlc;
+        netif_rx(skb);
+        return 0;
+}
+
+static void ican3_handle_inquiry(struct ican3_dev *mod, struct ican3_msg *msg)
+{
+        switch (msg->data[0]) {
+        case INQUIRY_STATUS:
+        case INQUIRY_EXTENDED:
+                mod->bec.rxerr = msg->data[5];
+                mod->bec.txerr = msg->data[6];
+                complete(&mod->buserror_comp);
+                break;
+        case INQUIRY_TERMINATION:
+                mod->termination_enabled = msg->data[6] & HWCONF_TERMINATE_ON;
+                complete(&mod->termination_comp);
+                break;
+        default:
+                dev_err(mod->dev, "recieved an unknown inquiry response\n");
+                break;
+        }
+}
+
+static void ican3_handle_unknown_message(struct ican3_dev *mod,
+                                        struct ican3_msg *msg)
+{
+        dev_warn(mod->dev, "recieved unknown message: spec 0x%.2x length %d\n",
+                           msg->spec, le16_to_cpu(msg->len));
+}
+
+/*
+ * Handle a control message from the firmware
+ */
+static void ican3_handle_message(struct ican3_dev *mod, struct ican3_msg *msg)
+{
+        dev_dbg(mod->dev, "%s: modno %d spec 0x%.2x len %d bytes\n", __func__,
+                           mod->num, msg->spec, le16_to_cpu(msg->len));
+
+        switch (msg->spec) {
+        case MSG_IDVERS:
+                ican3_handle_idvers(mod, msg);
+                break;
+        case MSG_MSGLOST:
+        case MSG_FMSGLOST:
+                ican3_handle_msglost(mod, msg);
+                break;
+        case MSG_CEVTIND:
+                ican3_handle_cevtind(mod, msg);
+                break;
+        case MSG_INQUIRY:
+                ican3_handle_inquiry(mod, msg);
+                break;
+        default:
+                ican3_handle_unknown_message(mod, msg);
+                break;
+        }
+}
+
+/*
+ * Check that there is room in the TX ring to transmit another skb
+ *
+ * LOCKING: must hold mod->lock
+ */
+static bool ican3_txok(struct ican3_dev *mod)
+{
+        struct ican3_fast_desc __iomem *desc;
+        u8 control;
+
+        /* copy the control bits of the descriptor */
+        ican3_set_page(mod, mod->fasttx_start + (mod->fasttx_num / 16));
+        desc = mod->dpm + ((mod->fasttx_num % 16) * sizeof(*desc));
+        control = ioread8(&desc->control);
+
+        /* if the control bits are not valid, then we have no more space */
+        if (!(control & DESC_VALID))
+                return false;
+
+        return true;
+}
+
+/*
+ * Recieve one CAN frame from the hardware
+ *
+ * This works like the core of a NAPI function, but is intended to be called
+ * from workqueue context instead. This driver already needs a workqueue to
+ * process control messages, so we use the workqueue instead of using NAPI.
+ * This was done to simplify locking.
+ *
+ * CONTEXT: must be called from user context
+ */
+static int ican3_recv_skb(struct ican3_dev *mod)
+{
+        struct net_device *ndev = mod->ndev;
+        struct net_device_stats *stats = &ndev->stats;
+        struct ican3_fast_desc desc;
+        void __iomem *desc_addr;
+        struct can_frame *cf;
+        struct sk_buff *skb;
+        unsigned long flags;
+
+        spin_lock_irqsave(&mod->lock, flags);
+
+        /* copy the whole descriptor */
+        ican3_set_page(mod, mod->fastrx_start + (mod->fastrx_num / 16));
+        desc_addr = mod->dpm + ((mod->fastrx_num % 16) * sizeof(desc));
+        memcpy_fromio(&desc, desc_addr, sizeof(desc));
+
+        spin_unlock_irqrestore(&mod->lock, flags);
+
+        /* check that we actually have a CAN frame */
+        if (!(desc.control & DESC_VALID))
+                return -ENOBUFS;
+
+        /* allocate an skb */
+        skb = alloc_can_skb(ndev, &cf);
+        if (unlikely(skb == NULL)) {
+                stats->rx_dropped++;
+                goto err_noalloc;
+        }
+
+        /* convert the ICAN3 frame into Linux CAN format */
+        ican3_to_can_frame(mod, &desc, cf);
+
+        /* receive the skb, update statistics */
+        netif_receive_skb(skb);
+        stats->rx_packets++;
+        stats->rx_bytes += cf->can_dlc;
+
+err_noalloc:
+        /* toggle the valid bit and return the descriptor to the ring */
+        desc.control ^= DESC_VALID;
+
+        spin_lock_irqsave(&mod->lock, flags);
+
+        ican3_set_page(mod, mod->fastrx_start + (mod->fastrx_num / 16));
+        memcpy_toio(desc_addr, &desc, 1);
+
+        /* update the next buffer pointer */
+        mod->fastrx_num = (desc.control & DESC_WRAP) ? 0
+                                                     : (mod->fastrx_num + 1);
+
+        /* there are still more buffers to process */
+        spin_unlock_irqrestore(&mod->lock, flags);
+        return 0;
+}
+
+static int ican3_napi(struct napi_struct *napi, int budget)
+{
+        struct ican3_dev *mod = container_of(napi, struct ican3_dev, napi);
+        struct ican3_msg msg;
+        unsigned long flags;
+        int received = 0;
+        int ret;
+
+        /* process all communication messages */
+        while (true) {
+                ret = ican3_recv_msg(mod, &msg);
+                if (ret)
+                        break;
+
+                ican3_handle_message(mod, &msg);
+        }
+
+        /* process all CAN frames from the fast interface */
+        while (received < budget) {
+                ret = ican3_recv_skb(mod);
+                if (ret)
+                        break;
+
+                received++;
+        }
+
+        /* We have processed all packets that the adapter had, but it
+         * was less than our budget, stop polling */
+        if (received < budget)
+                napi_complete(napi);
+
+        spin_lock_irqsave(&mod->lock, flags);
+
+        /* Wake up the transmit queue if necessary */
+        if (netif_queue_stopped(mod->ndev) && ican3_txok(mod))
+                netif_wake_queue(mod->ndev);
+
+        spin_unlock_irqrestore(&mod->lock, flags);
+
+        /* re-enable interrupt generation */
+        iowrite8(1 << mod->num, &mod->ctrl->int_enable);
+        return received;
+}
+
+static irqreturn_t ican3_irq(int irq, void *dev_id)
+{
+        struct ican3_dev *mod = dev_id;
+        u8 stat;
+
+        /*
+         * The interrupt status register on this device reports interrupts
+         * as zeroes instead of using ones like most other devices
+         */
+        stat = ioread8(&mod->ctrl->int_disable) & (1 << mod->num);
+        if (stat == (1 << mod->num))
+                return IRQ_NONE;
+
+        /* clear the MODULbus interrupt from the microcontroller */
+        ioread8(&mod->dpmctrl->interrupt);
+
+        /* disable interrupt generation, schedule the NAPI poller */
+        iowrite8(1 << mod->num, &mod->ctrl->int_disable);
+        napi_schedule(&mod->napi);
+        return IRQ_HANDLED;
+}
+
+/*
+ * Firmware reset, startup, and shutdown
+ */
+
+/*
+ * Reset an ICAN module to its power-on state
+ *
+ * CONTEXT: no network device registered
+ * LOCKING: work function disabled
+ */
+static int ican3_reset_module(struct ican3_dev *mod)
+{
+        u8 val = 1 << mod->num;
+        unsigned long start;
+        u8 runold, runnew;
+
+        /* disable interrupts so no more work is scheduled */
+        iowrite8(1 << mod->num, &mod->ctrl->int_disable);
+
+        /* flush any pending work */
+        flush_scheduled_work();
+
+        /* the first unallocated page in the DPM is #9 */
+        mod->free_page = DPM_FREE_START;
+
+        ican3_set_page(mod, QUEUE_OLD_CONTROL);
+        runold = ioread8(mod->dpm + TARGET_RUNNING);
+
+        /* reset the module */
+        iowrite8(val, &mod->ctrl->reset_assert);
+        iowrite8(val, &mod->ctrl->reset_deassert);
+
+        /* wait until the module has finished resetting and is running */
+        start = jiffies;
+        do {
+                ican3_set_page(mod, QUEUE_OLD_CONTROL);
+                runnew = ioread8(mod->dpm + TARGET_RUNNING);
+                if (runnew == (runold ^ 0xff))
+                        return 0;
+
+                msleep(10);
+        } while (time_before(jiffies, start + HZ / 4));
+
+        dev_err(mod->dev, "failed to reset CAN module\n");
+        return -ETIMEDOUT;
+}
+
+static void __devexit ican3_shutdown_module(struct ican3_dev *mod)
+{
+        ican3_msg_disconnect(mod);
+        ican3_reset_module(mod);
+}
+
+/*
+ * Startup an ICAN module, bringing it into fast mode
+ */
+static int __devinit ican3_startup_module(struct ican3_dev *mod)
+{
+        int ret;
+
+        ret = ican3_reset_module(mod);
+        if (ret) {
+                dev_err(mod->dev, "unable to reset module\n");
+                return ret;
+        }
+
+        /* re-enable interrupts so we can send messages */
+        iowrite8(1 << mod->num, &mod->ctrl->int_enable);
+
+        ret = ican3_msg_connect(mod);
+        if (ret) {
+                dev_err(mod->dev, "unable to connect to module\n");
+                return ret;
+        }
+
+        ican3_init_new_host_interface(mod);
+        ret = ican3_msg_newhostif(mod);
+        if (ret) {
+                dev_err(mod->dev, "unable to switch to new-style interface\n");
+                return ret;
+        }
+
+        /* default to "termination on" */
+        ret = ican3_set_termination(mod, true);
+        if (ret) {
+                dev_err(mod->dev, "unable to enable termination\n");
+                return ret;
+        }
+
+        /* default to "bus errors enabled" */
+        ret = ican3_set_buserror(mod, ICAN3_BUSERR_QUOTA_MAX);
+        if (ret) {
+                dev_err(mod->dev, "unable to set bus-error\n");
+                return ret;
+        }
+
+        ican3_init_fast_host_interface(mod);
+        ret = ican3_msg_fasthostif(mod);
+        if (ret) {
+                dev_err(mod->dev, "unable to switch to fast host interface\n");
+                return ret;
+        }
+
+        ret = ican3_set_id_filter(mod, true);
+        if (ret) {
+                dev_err(mod->dev, "unable to set acceptance filter\n");
+                return ret;
+        }
+
+        return 0;
+}
+
+/*
+ * CAN Network Device
+ */
+
+static int ican3_open(struct net_device *ndev)
+{
+        struct ican3_dev *mod = netdev_priv(ndev);
+        u8 quota;
+        int ret;
+
+        /* open the CAN layer */
+        ret = open_candev(ndev);
+        if (ret) {
+                dev_err(mod->dev, "unable to start CAN layer\n");
+                return ret;
+        }
+
+        /* set the bus error generation state appropriately */
+        if (mod->can.ctrlmode & CAN_CTRLMODE_BERR_REPORTING)
+                quota = ICAN3_BUSERR_QUOTA_MAX;
+        else
+                quota = 0;
+
+        ret = ican3_set_buserror(mod, quota);
+        if (ret) {
+                dev_err(mod->dev, "unable to set bus-error\n");
+                close_candev(ndev);
+                return ret;
+        }
+
+        /* bring the bus online */
+        ret = ican3_set_bus_state(mod, true);
+        if (ret) {
+                dev_err(mod->dev, "unable to set bus-on\n");
+                close_candev(ndev);
+                return ret;
+        }
+
+        /* start up the network device */
+        mod->can.state = CAN_STATE_ERROR_ACTIVE;
+        netif_start_queue(ndev);
+
+        return 0;
+}
+
+static int ican3_stop(struct net_device *ndev)
+{
+        struct ican3_dev *mod = netdev_priv(ndev);
+        int ret;
+
+        /* stop the network device xmit routine */
+        netif_stop_queue(ndev);
+        mod->can.state = CAN_STATE_STOPPED;
+
+        /* bring the bus offline, stop receiving packets */
+        ret = ican3_set_bus_state(mod, false);
+        if (ret) {
+                dev_err(mod->dev, "unable to set bus-off\n");
+                return ret;
+        }
+
+        /* close the CAN layer */
+        close_candev(ndev);
+        return 0;
+}
+
+static int ican3_xmit(struct sk_buff *skb, struct net_device *ndev)
+{
+        struct ican3_dev *mod = netdev_priv(ndev);
+        struct net_device_stats *stats = &ndev->stats;
+        struct can_frame *cf = (struct can_frame *)skb->data;
+        struct ican3_fast_desc desc;
+        void __iomem *desc_addr;
+        unsigned long flags;
+
+        spin_lock_irqsave(&mod->lock, flags);
+
+        /* check that we can actually transmit */
+        if (!ican3_txok(mod)) {
+                dev_err(mod->dev, "no free descriptors, stopping queue\n");
+                netif_stop_queue(ndev);
+                spin_unlock_irqrestore(&mod->lock, flags);
+                return NETDEV_TX_BUSY;
+        }
+
+        /* copy the control bits of the descriptor */
+        ican3_set_page(mod, mod->fasttx_start + (mod->fasttx_num / 16));
+        desc_addr = mod->dpm + ((mod->fasttx_num % 16) * sizeof(desc));
+        memset(&desc, 0, sizeof(desc));
+        memcpy_fromio(&desc, desc_addr, 1);
+
+        /* convert the Linux CAN frame into ICAN3 format */
+        can_frame_to_ican3(mod, cf, &desc);
+
+        /*
+         * the programming manual says that you must set the IVALID bit, then
+         * interrupt, then set the valid bit. Quite weird, but it seems to be
+         * required for this to work
+         */
+        desc.control |= DESC_IVALID;
+        memcpy_toio(desc_addr, &desc, sizeof(desc));
+
+        /* generate a MODULbus interrupt to the microcontroller */
+        iowrite8(0x01, &mod->dpmctrl->interrupt);
+
+        desc.control ^= DESC_VALID;
+        memcpy_toio(desc_addr, &desc, sizeof(desc));
+
+        /* update the next buffer pointer */
+        mod->fasttx_num = (desc.control & DESC_WRAP) ? 0
+                                                     : (mod->fasttx_num + 1);
+
+        /* update statistics */
+        stats->tx_packets++;
+        stats->tx_bytes += cf->can_dlc;
+        kfree_skb(skb);
+
+        /*
+         * This hardware doesn't have TX-done notifications, so we'll try and
+         * emulate it the best we can using ECHO skbs. Get the next TX
+         * descriptor, and see if we have room to send. If not, stop the queue.
+         * It will be woken when the ECHO skb for the current packet is recv'd.
+         */
+
+        /* copy the control bits of the descriptor */
+        if (!ican3_txok(mod))
+                netif_stop_queue(ndev);
+
+        spin_unlock_irqrestore(&mod->lock, flags);
+        return NETDEV_TX_OK;
+}
+
+static const struct net_device_ops ican3_netdev_ops = {
+        .ndo_open       = ican3_open,
+        .ndo_stop       = ican3_stop,
+        .ndo_start_xmit = ican3_xmit,
+};
+
+/*
+ * Low-level CAN Device
+ */
+
+/* This structure was stolen from drivers/net/can/sja1000/sja1000.c */
+static struct can_bittiming_const ican3_bittiming_const = {
+        .name = DRV_NAME,
+        .tseg1_min = 1,
+        .tseg1_max = 16,
+        .tseg2_min = 1,
+        .tseg2_max = 8,
+        .sjw_max = 4,
+        .brp_min = 1,
+        .brp_max = 64,
+        .brp_inc = 1,
+};
+
+/*
+ * This routine was stolen from drivers/net/can/sja1000/sja1000.c
+ *
+ * The bittiming register command for the ICAN3 just sets the bit timing
+ * registers on the SJA1000 chip directly
+ */
+static int ican3_set_bittiming(struct net_device *ndev)
+{
+        struct ican3_dev *mod = netdev_priv(ndev);
+        struct can_bittiming *bt = &mod->can.bittiming;
+        struct ican3_msg msg;
+        u8 btr0, btr1;
+
+        btr0 = ((bt->brp - 1) & 0x3f) | (((bt->sjw - 1) & 0x3) << 6);
+        btr1 = ((bt->prop_seg + bt->phase_seg1 - 1) & 0xf) |
+                (((bt->phase_seg2 - 1) & 0x7) << 4);
+        if (mod->can.ctrlmode & CAN_CTRLMODE_3_SAMPLES)
+                btr1 |= 0x80;
+
+        memset(&msg, 0, sizeof(msg));
+        msg.spec = MSG_CBTRREQ;
+        msg.len = cpu_to_le16(4);
+        msg.data[0] = 0x00;
+        msg.data[1] = 0x00;
+        msg.data[2] = btr0;
+        msg.data[3] = btr1;
+
+        return ican3_send_msg(mod, &msg);
+}
+
+static int ican3_set_mode(struct net_device *ndev, enum can_mode mode)
+{
+        struct ican3_dev *mod = netdev_priv(ndev);
+        int ret;
+
+        if (mode != CAN_MODE_START)
+                return -ENOTSUPP;
+
+        /* bring the bus online */
+        ret = ican3_set_bus_state(mod, true);
+        if (ret) {
+                dev_err(mod->dev, "unable to set bus-on\n");
+                return ret;
+        }
+
+        /* start up the network device */
+        mod->can.state = CAN_STATE_ERROR_ACTIVE;
+
+        if (netif_queue_stopped(ndev))
+                netif_wake_queue(ndev);
+
+        return 0;
+}
+
+static int ican3_get_berr_counter(const struct net_device *ndev,
+                                  struct can_berr_counter *bec)
+{
+        struct ican3_dev *mod = netdev_priv(ndev);
+        int ret;
+
+        ret = ican3_send_inquiry(mod, INQUIRY_STATUS);
+        if (ret)
+                return ret;
+
+        ret = wait_for_completion_timeout(&mod->buserror_comp, HZ);
+        if (ret <= 0) {
+                dev_info(mod->dev, "%s timed out\n", __func__);
+                return -ETIMEDOUT;
+        }
+
+        bec->rxerr = mod->bec.rxerr;
+        bec->txerr = mod->bec.txerr;
+        return 0;
+}
+
+/*
+ * Sysfs Attributes
+ */
+
+static ssize_t ican3_sysfs_show_term(struct device *dev,
+                                     struct device_attribute *attr,
+                                     char *buf)
+{
+        struct ican3_dev *mod = netdev_priv(to_net_dev(dev));
+        int ret;
+
+        ret = ican3_send_inquiry(mod, INQUIRY_TERMINATION);
+        if (ret)
+                return ret;
+
+        ret = wait_for_completion_timeout(&mod->termination_comp, HZ);
+        if (ret <= 0) {
+                dev_info(mod->dev, "%s timed out\n", __func__);
+                return -ETIMEDOUT;
+        }
+
+        return snprintf(buf, PAGE_SIZE, "%u\n", mod->termination_enabled);
+}
+
+static ssize_t ican3_sysfs_set_term(struct device *dev,
+                                    struct device_attribute *attr,
+                                    const char *buf, size_t count)
+{
+        struct ican3_dev *mod = netdev_priv(to_net_dev(dev));
+        unsigned long enable;
+        int ret;
+
+        if (strict_strtoul(buf, 0, &enable))
+                return -EINVAL;
+
+        ret = ican3_set_termination(mod, enable);
+        if (ret)
+                return ret;
+
+        return count;
+}
+
+static DEVICE_ATTR(termination, S_IWUGO | S_IRUGO, ican3_sysfs_show_term,
+                                                   ican3_sysfs_set_term);
+
+static struct attribute *ican3_sysfs_attrs[] = {
+        &dev_attr_termination.attr,
+        NULL,
+};
+
+static struct attribute_group ican3_sysfs_attr_group = {
+        .attrs = ican3_sysfs_attrs,
+};
+
+/*
+ * PCI Subsystem
+ */
+
+static int __devinit ican3_probe(struct platform_device *pdev)
+{
+        struct janz_platform_data *pdata;
+        struct net_device *ndev;
+        struct ican3_dev *mod;
+        struct resource *res;
+        struct device *dev;
+        int ret;
+
+        pdata = pdev->dev.platform_data;
+        if (!pdata)
+                return -ENXIO;
+
+        dev_dbg(&pdev->dev, "probe: module number %d\n", pdata->modno);
+
+        /* save the struct device for printing */
+        dev = &pdev->dev;
+
+        /* allocate the CAN device and private data */
+        ndev = alloc_candev(sizeof(*mod), 0);
+        if (!ndev) {
+                dev_err(dev, "unable to allocate CANdev\n");
+                ret = -ENOMEM;
+                goto out_return;
+        }
+
+        platform_set_drvdata(pdev, ndev);
+        mod = netdev_priv(ndev);
+        mod->ndev = ndev;
+        mod->dev = &pdev->dev;
+        mod->num = pdata->modno;
+        netif_napi_add(ndev, &mod->napi, ican3_napi, ICAN3_RX_BUFFERS);
+        spin_lock_init(&mod->lock);
+        init_completion(&mod->termination_comp);
+        init_completion(&mod->buserror_comp);
+
+        /* setup device-specific sysfs attributes */
+        ndev->sysfs_groups[0] = &ican3_sysfs_attr_group;
+
+        /* the first unallocated page in the DPM is 9 */
+        mod->free_page = DPM_FREE_START;
+
+        ndev->netdev_ops = &ican3_netdev_ops;
+        ndev->flags |= IFF_ECHO;
+        SET_NETDEV_DEV(ndev, &pdev->dev);
+
+        mod->can.clock.freq = ICAN3_CAN_CLOCK;
+        mod->can.bittiming_const = &ican3_bittiming_const;
+        mod->can.do_set_bittiming = ican3_set_bittiming;
+        mod->can.do_set_mode = ican3_set_mode;
+        mod->can.do_get_berr_counter = ican3_get_berr_counter;
+        mod->can.ctrlmode_supported = CAN_CTRLMODE_3_SAMPLES
+                                    | CAN_CTRLMODE_BERR_REPORTING;
+
+        /* find our IRQ number */
+        mod->irq = platform_get_irq(pdev, 0);
+        if (mod->irq < 0) {
+                dev_err(dev, "IRQ line not found\n");
+                ret = -ENODEV;
+                goto out_free_ndev;
+        }
+
+        ndev->irq = mod->irq;
+
+        /* get access to the MODULbus registers for this module */
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res) {
+                dev_err(dev, "MODULbus registers not found\n");
+                ret = -ENODEV;
+                goto out_free_ndev;
+        }
+
+        mod->dpm = ioremap(res->start, resource_size(res));
+        if (!mod->dpm) {
+                dev_err(dev, "MODULbus registers not ioremap\n");
+                ret = -ENOMEM;
+                goto out_free_ndev;
+        }
+
+        mod->dpmctrl = mod->dpm + DPM_PAGE_SIZE;
+
+        /* get access to the control registers for this module */
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+        if (!res) {
+                dev_err(dev, "CONTROL registers not found\n");
+                ret = -ENODEV;
+                goto out_iounmap_dpm;
+        }
+
+        mod->ctrl = ioremap(res->start, resource_size(res));
+        if (!mod->ctrl) {
+                dev_err(dev, "CONTROL registers not ioremap\n");
+                ret = -ENOMEM;
+                goto out_iounmap_dpm;
+        }
+
+        /* disable our IRQ, then hookup the IRQ handler */
+        iowrite8(1 << mod->num, &mod->ctrl->int_disable);
+        ret = request_irq(mod->irq, ican3_irq, IRQF_SHARED, DRV_NAME, mod);
+        if (ret) {
+                dev_err(dev, "unable to request IRQ\n");
+                goto out_iounmap_ctrl;
+        }
+
+        /* reset and initialize the CAN controller into fast mode */
+        napi_enable(&mod->napi);
+        ret = ican3_startup_module(mod);
+        if (ret) {
+                dev_err(dev, "%s: unable to start CANdev\n", __func__);
+                goto out_free_irq;
+        }
+
+        /* register with the Linux CAN layer */
+        ret = register_candev(ndev);
+        if (ret) {
+                dev_err(dev, "%s: unable to register CANdev\n", __func__);
+                goto out_free_irq;
+        }
+
+        dev_info(dev, "module %d: registered CAN device\n", pdata->modno);
+        return 0;
+
+out_free_irq:
+        napi_disable(&mod->napi);
+        iowrite8(1 << mod->num, &mod->ctrl->int_disable);
+        free_irq(mod->irq, mod);
+out_iounmap_ctrl:
+        iounmap(mod->ctrl);
+out_iounmap_dpm:
+        iounmap(mod->dpm);
+out_free_ndev:
+        free_candev(ndev);
+out_return:
+        return ret;
+}
+
+static int __devexit ican3_remove(struct platform_device *pdev)
+{
+        struct net_device *ndev = platform_get_drvdata(pdev);
+        struct ican3_dev *mod = netdev_priv(ndev);
+
+        /* unregister the netdevice, stop interrupts */
+        unregister_netdev(ndev);
+        napi_disable(&mod->napi);
+        iowrite8(1 << mod->num, &mod->ctrl->int_disable);
+        free_irq(mod->irq, mod);
+
+        /* put the module into reset */
+        ican3_shutdown_module(mod);
+
+        /* unmap all registers */
+        iounmap(mod->ctrl);
+        iounmap(mod->dpm);
+
+        free_candev(ndev);
+
+        return 0;
+}
+
+static struct platform_driver ican3_driver = {
+        .driver         = {
+                .name   = DRV_NAME,
+                .owner  = THIS_MODULE,
+        },
+        .probe          = ican3_probe,
+        .remove         = __devexit_p(ican3_remove),
+};
+
+static int __init ican3_init(void)
+{
+        return platform_driver_register(&ican3_driver);
+}
+
+static void __exit ican3_exit(void)
+{
+        platform_driver_unregister(&ican3_driver);
+}
+
+MODULE_AUTHOR("Ira W. Snyder <iws@ovro.caltech.edu>");
+MODULE_DESCRIPTION("Janz MODULbus VMOD-ICAN3 Driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:janz-ican3");
+
+module_init(ican3_init);
+module_exit(ican3_exit);
diff --git a/drivers/net/can/mscan/mpc5xxx_can.c b/drivers/net/can/mscan/mpc5xxx_can.c
index 8af8442c694a..af753936e835 100644
--- a/drivers/net/can/mscan/mpc5xxx_can.c
+++ b/drivers/net/can/mscan/mpc5xxx_can.c
@@ -73,7 +73,7 @@ static u32 __devinit mpc52xx_can_get_clock(struct of_device *ofdev,
         else
                 *mscan_clksrc = MSCAN_CLKSRC_XTAL;
 
-        freq = mpc5xxx_get_bus_frequency(ofdev->node);
+        freq = mpc5xxx_get_bus_frequency(ofdev->dev.of_node);
         if (!freq)
                 return 0;
 
@@ -152,7 +152,7 @@ static u32 __devinit mpc512x_can_get_clock(struct of_device *ofdev,
         }
 
         /* Determine the MSCAN device index from the physical address */
-        pval = of_get_property(ofdev->node, "reg", &plen);
+        pval = of_get_property(ofdev->dev.of_node, "reg", &plen);
         BUG_ON(!pval || plen < sizeof(*pval));
         clockidx = (*pval & 0x80) ? 1 : 0;
         if (*pval & 0x2000)
@@ -168,11 +168,11 @@ static u32 __devinit mpc512x_can_get_clock(struct of_device *ofdev,
          */
         if (clock_name && !strcmp(clock_name, "ip")) {
                 *mscan_clksrc = MSCAN_CLKSRC_IPS;
-                freq = mpc5xxx_get_bus_frequency(ofdev->node);
+                freq = mpc5xxx_get_bus_frequency(ofdev->dev.of_node);
         } else {
                 *mscan_clksrc = MSCAN_CLKSRC_BUS;
 
-                pval = of_get_property(ofdev->node,
+                pval = of_get_property(ofdev->dev.of_node,
                                        "fsl,mscan-clock-divider", &plen);
                 if (pval && plen == sizeof(*pval))
                         clockdiv = *pval;
@@ -251,7 +251,7 @@ static int __devinit mpc5xxx_can_probe(struct of_device *ofdev,
                                        const struct of_device_id *id)
 {
         struct mpc5xxx_can_data *data = (struct mpc5xxx_can_data *)id->data;
-        struct device_node *np = ofdev->node;
+        struct device_node *np = ofdev->dev.of_node;
         struct net_device *dev;
         struct mscan_priv *priv;
         void __iomem *base;
diff --git a/drivers/net/cnic.c b/drivers/net/cnic.c
index be90d3598bca..fe925663d39a 100644
--- a/drivers/net/cnic.c
+++ b/drivers/net/cnic.c
@@ -3367,13 +3367,9 @@ static int cnic_cm_shutdown(struct cnic_dev *dev)
 
 static void cnic_init_context(struct cnic_dev *dev, u32 cid)
 {
-        struct cnic_local *cp = dev->cnic_priv;
         u32 cid_addr;
         int i;
 
-        if (CHIP_NUM(cp) == CHIP_NUM_5709)
-                return;
-
         cid_addr = GET_CID_ADDR(cid);
 
         for (i = 0; i < CTX_SIZE; i += 4)
@@ -3530,14 +3526,11 @@ static void cnic_init_bnx2_tx_ring(struct cnic_dev *dev)
 
         sb_id = cp->status_blk_num;
         tx_cid = 20;
-        cnic_init_context(dev, tx_cid);
-        cnic_init_context(dev, tx_cid + 1);
         cp->tx_cons_ptr = &s_blk->status_tx_quick_consumer_index2;
         if (ethdev->drv_state & CNIC_DRV_STATE_USING_MSIX) {
                 struct status_block_msix *sblk = cp->status_blk.bnx2;
 
                 tx_cid = TX_TSS_CID + sb_id - 1;
-                cnic_init_context(dev, tx_cid);
                 CNIC_WR(dev, BNX2_TSCH_TSS_CFG, (sb_id << 24) |
                         (TX_TSS_CID << 7));
                 cp->tx_cons_ptr = &sblk->status_tx_quick_consumer_index;
@@ -3556,6 +3549,9 @@ static void cnic_init_bnx2_tx_ring(struct cnic_dev *dev)
                 offset2 = BNX2_L2CTX_TBDR_BHADDR_HI_XI;
                 offset3 = BNX2_L2CTX_TBDR_BHADDR_LO_XI;
         } else {
+                cnic_init_context(dev, tx_cid);
+                cnic_init_context(dev, tx_cid + 1);
+
                 offset0 = BNX2_L2CTX_TYPE;
                 offset1 = BNX2_L2CTX_CMD_TYPE;
                 offset2 = BNX2_L2CTX_TBDR_BHADDR_HI;
diff --git a/drivers/net/cnic_if.h b/drivers/net/cnic_if.h
index 110c62072e6f..0c55177db046 100644
--- a/drivers/net/cnic_if.h
+++ b/drivers/net/cnic_if.h
@@ -12,8 +12,8 @@
 #ifndef CNIC_IF_H
 #define CNIC_IF_H
 
-#define CNIC_MODULE_VERSION     "2.1.1"
-#define CNIC_MODULE_RELDATE     "Feb 22, 2010"
+#define CNIC_MODULE_VERSION     "2.1.2"
+#define CNIC_MODULE_RELDATE     "May 26, 2010"
 
 #define CNIC_ULP_RDMA           0
 #define CNIC_ULP_ISCSI          1
diff --git a/drivers/net/e1000e/netdev.c b/drivers/net/e1000e/netdev.c
index 24507f3b8b17..57a7e41da69e 100644
--- a/drivers/net/e1000e/netdev.c
+++ b/drivers/net/e1000e/netdev.c
@@ -2554,7 +2554,7 @@ static void e1000_init_manageability_pt(struct e1000_adapter *adapter)
                         mdef = er32(MDEF(i));
 
                         /* Ignore filters with anything other than IPMI ports */
-                        if (mdef & !(E1000_MDEF_PORT_623 | E1000_MDEF_PORT_664))
+                        if (mdef & ~(E1000_MDEF_PORT_623 | E1000_MDEF_PORT_664))
                                 continue;
 
                         /* Enable this decision filter in MANC2H */
diff --git a/drivers/net/enic/enic.h b/drivers/net/enic/enic.h
index 85f2a2e7030a..45e86d1e5b1b 100644
--- a/drivers/net/enic/enic.h
+++ b/drivers/net/enic/enic.h
@@ -74,7 +74,14 @@ struct enic_msix_entry {
         void *devid;
 };
 
+#define ENIC_SET_APPLIED                (1 << 0)
+#define ENIC_SET_REQUEST                (1 << 1)
+#define ENIC_SET_NAME                   (1 << 2)
+#define ENIC_SET_INSTANCE               (1 << 3)
+#define ENIC_SET_HOST                   (1 << 4)
+
 struct enic_port_profile {
+        u32 set;
         u8 request;
         char name[PORT_PROFILE_MAX];
         u8 instance_uuid[PORT_UUID_MAX];
diff --git a/drivers/net/enic/enic_main.c b/drivers/net/enic/enic_main.c
index 6586b5c7e4b6..bc7d6b96de3d 100644
--- a/drivers/net/enic/enic_main.c
+++ b/drivers/net/enic/enic_main.c
@@ -1029,8 +1029,7 @@ static int enic_dev_init_done(struct enic *enic, int *done, int *error)
         return err;
 }
 
-static int enic_set_port_profile(struct enic *enic, u8 request, u8 *mac,
-        char *name, u8 *instance_uuid, u8 *host_uuid)
+static int enic_set_port_profile(struct enic *enic, u8 *mac)
 {
         struct vic_provinfo *vp;
         u8 oui[3] = VIC_PROVINFO_CISCO_OUI;
@@ -1040,97 +1039,112 @@ static int enic_set_port_profile(struct enic *enic, u8 request, u8 *mac,
                 "%02X%02X-%02X%02X%02X%02X%0X%02X";
         int err;
 
-        if (!name)
-                return -EINVAL;
+        err = enic_vnic_dev_deinit(enic);
+        if (err)
+                return err;
 
-        if (!is_valid_ether_addr(mac))
-                return -EADDRNOTAVAIL;
+        switch (enic->pp.request) {
 
-        vp = vic_provinfo_alloc(GFP_KERNEL, oui, VIC_PROVINFO_LINUX_TYPE);
-        if (!vp)
-                return -ENOMEM;
+        case PORT_REQUEST_ASSOCIATE:
 
-        vic_provinfo_add_tlv(vp,
-                VIC_LINUX_PROV_TLV_PORT_PROFILE_NAME_STR,
-                strlen(name) + 1, name);
-
-        vic_provinfo_add_tlv(vp,
-                VIC_LINUX_PROV_TLV_CLIENT_MAC_ADDR,
-                ETH_ALEN, mac);
-
-        if (instance_uuid) {
-                uuid = instance_uuid;
-                sprintf(uuid_str, uuid_fmt,
-                        uuid[0],  uuid[1],  uuid[2],  uuid[3],
-                        uuid[4],  uuid[5],  uuid[6],  uuid[7],
-                        uuid[8],  uuid[9],  uuid[10], uuid[11],
-                        uuid[12], uuid[13], uuid[14], uuid[15]);
-                vic_provinfo_add_tlv(vp,
-                        VIC_LINUX_PROV_TLV_CLIENT_UUID_STR,
-                        sizeof(uuid_str), uuid_str);
-        }
+                if (!(enic->pp.set & ENIC_SET_NAME) || !strlen(enic->pp.name))
+                        return -EINVAL;
 
-        if (host_uuid) {
-                uuid = host_uuid;
-                sprintf(uuid_str, uuid_fmt,
-                        uuid[0],  uuid[1],  uuid[2],  uuid[3],
-                        uuid[4],  uuid[5],  uuid[6],  uuid[7],
-                        uuid[8],  uuid[9],  uuid[10], uuid[11],
-                        uuid[12], uuid[13], uuid[14], uuid[15]);
-                vic_provinfo_add_tlv(vp,
-                        VIC_LINUX_PROV_TLV_HOST_UUID_STR,
-                        sizeof(uuid_str), uuid_str);
-        }
+                if (!is_valid_ether_addr(mac))
+                        return -EADDRNOTAVAIL;
 
-        err = enic_vnic_dev_deinit(enic);
-        if (err)
-                goto err_out;
+                vp = vic_provinfo_alloc(GFP_KERNEL, oui,
+                        VIC_PROVINFO_LINUX_TYPE);
+                if (!vp)
+                        return -ENOMEM;
 
-        memset(&enic->pp, 0, sizeof(enic->pp));
+                vic_provinfo_add_tlv(vp,
+                        VIC_LINUX_PROV_TLV_PORT_PROFILE_NAME_STR,
+                        strlen(enic->pp.name) + 1, enic->pp.name);
 
-        err = enic_dev_init_prov(enic, vp);
-        if (err)
-                goto err_out;
+                vic_provinfo_add_tlv(vp,
+                        VIC_LINUX_PROV_TLV_CLIENT_MAC_ADDR,
+                        ETH_ALEN, mac);
+
+                if (enic->pp.set & ENIC_SET_INSTANCE) {
+                        uuid = enic->pp.instance_uuid;
+                        sprintf(uuid_str, uuid_fmt,
+                                uuid[0],  uuid[1],  uuid[2],  uuid[3],
+                                uuid[4],  uuid[5],  uuid[6],  uuid[7],
+                                uuid[8],  uuid[9],  uuid[10], uuid[11],
+                                uuid[12], uuid[13], uuid[14], uuid[15]);
+                        vic_provinfo_add_tlv(vp,
+                                VIC_LINUX_PROV_TLV_CLIENT_UUID_STR,
+                                sizeof(uuid_str), uuid_str);
+                }
 
-        enic->pp.request = request;
-        memcpy(enic->pp.name, name, PORT_PROFILE_MAX);
-        if (instance_uuid)
-                memcpy(enic->pp.instance_uuid,
-                        instance_uuid, PORT_UUID_MAX);
-        if (host_uuid)
-                memcpy(enic->pp.host_uuid,
-                        host_uuid, PORT_UUID_MAX);
+                if (enic->pp.set & ENIC_SET_HOST) {
+                        uuid = enic->pp.host_uuid;
+                        sprintf(uuid_str, uuid_fmt,
+                                uuid[0],  uuid[1],  uuid[2],  uuid[3],
+                                uuid[4],  uuid[5],  uuid[6],  uuid[7],
+                                uuid[8],  uuid[9],  uuid[10], uuid[11],
+                                uuid[12], uuid[13], uuid[14], uuid[15]);
+                        vic_provinfo_add_tlv(vp,
+                                VIC_LINUX_PROV_TLV_HOST_UUID_STR,
+                                sizeof(uuid_str), uuid_str);
+                }
 
-err_out:
-        vic_provinfo_free(vp);
+                err = enic_dev_init_prov(enic, vp);
+                vic_provinfo_free(vp);
+                if (err)
+                        return err;
+                break;
 
-        return err;
-}
+        case PORT_REQUEST_DISASSOCIATE:
+                break;
 
-static int enic_unset_port_profile(struct enic *enic)
-{
-        memset(&enic->pp, 0, sizeof(enic->pp));
-        return enic_vnic_dev_deinit(enic);
+        default:
+                return -EINVAL;
+        }
+
+        enic->pp.set |= ENIC_SET_APPLIED;
+        return 0;
 }
 
 static int enic_set_vf_port(struct net_device *netdev, int vf,
         struct nlattr *port[])
 {
         struct enic *enic = netdev_priv(netdev);
-        char *name = NULL;
-        u8 *instance_uuid = NULL;
-        u8 *host_uuid = NULL;
-        u8 request = PORT_REQUEST_DISASSOCIATE;
+
+        memset(&enic->pp, 0, sizeof(enic->pp));
+
+        if (port[IFLA_PORT_REQUEST]) {
+                enic->pp.set |= ENIC_SET_REQUEST;
+                enic->pp.request = nla_get_u8(port[IFLA_PORT_REQUEST]);
+        }
+
+        if (port[IFLA_PORT_PROFILE]) {
+                enic->pp.set |= ENIC_SET_NAME;
+                memcpy(enic->pp.name, nla_data(port[IFLA_PORT_PROFILE]),
+                        PORT_PROFILE_MAX);
+        }
+
+        if (port[IFLA_PORT_INSTANCE_UUID]) {
+                enic->pp.set |= ENIC_SET_INSTANCE;
+                memcpy(enic->pp.instance_uuid,
+                        nla_data(port[IFLA_PORT_INSTANCE_UUID]), PORT_UUID_MAX);
+        }
+
+        if (port[IFLA_PORT_HOST_UUID]) {
+                enic->pp.set |= ENIC_SET_HOST;
+                memcpy(enic->pp.host_uuid,
+                        nla_data(port[IFLA_PORT_HOST_UUID]), PORT_UUID_MAX);
+        }
 
         /* don't support VFs, yet */
         if (vf != PORT_SELF_VF)
                 return -EOPNOTSUPP;
 
-        if (port[IFLA_PORT_REQUEST])
-                request = nla_get_u8(port[IFLA_PORT_REQUEST]);
+        if (!(enic->pp.set & ENIC_SET_REQUEST))
+                return -EOPNOTSUPP;
 
-        switch (request) {
-        case PORT_REQUEST_ASSOCIATE:
+        if (enic->pp.request == PORT_REQUEST_ASSOCIATE) {
 
                 /* If the interface mac addr hasn't been assigned,
                  * assign a random mac addr before setting port-
@@ -1139,30 +1153,9 @@ static int enic_set_vf_port(struct net_device *netdev, int vf,
 
                 if (is_zero_ether_addr(netdev->dev_addr))
                         random_ether_addr(netdev->dev_addr);
-
-                if (port[IFLA_PORT_PROFILE])
-                        name = nla_data(port[IFLA_PORT_PROFILE]);
-
-                if (port[IFLA_PORT_INSTANCE_UUID])
-                        instance_uuid =
-                                nla_data(port[IFLA_PORT_INSTANCE_UUID]);
-
-                if (port[IFLA_PORT_HOST_UUID])
-                        host_uuid = nla_data(port[IFLA_PORT_HOST_UUID]);
-
-                return enic_set_port_profile(enic, request,
-                        netdev->dev_addr, name,
-                        instance_uuid, host_uuid);
-
-        case PORT_REQUEST_DISASSOCIATE:
-
-                return enic_unset_port_profile(enic);
-
-        default:
-                break;
         }
 
-        return -EOPNOTSUPP;
+        return enic_set_port_profile(enic, netdev->dev_addr);
 }
 
 static int enic_get_vf_port(struct net_device *netdev, int vf,
@@ -1172,14 +1165,12 @@ static int enic_get_vf_port(struct net_device *netdev, int vf,
         int err, error, done;
         u16 response = PORT_PROFILE_RESPONSE_SUCCESS;
 
-        /* don't support VFs, yet */
-        if (vf != PORT_SELF_VF)
-                return -EOPNOTSUPP;
+        if (!(enic->pp.set & ENIC_SET_APPLIED))
+                return -ENODATA;
 
         err = enic_dev_init_done(enic, &done, &error);
-
         if (err)
-                return err;
+                error = err;
 
         switch (error) {
         case ERR_SUCCESS:
@@ -1202,12 +1193,15 @@ static int enic_get_vf_port(struct net_device *netdev, int vf,
 
         NLA_PUT_U16(skb, IFLA_PORT_REQUEST, enic->pp.request);
         NLA_PUT_U16(skb, IFLA_PORT_RESPONSE, response);
-        NLA_PUT(skb, IFLA_PORT_PROFILE, PORT_PROFILE_MAX,
-                enic->pp.name);
-        NLA_PUT(skb, IFLA_PORT_INSTANCE_UUID, PORT_UUID_MAX,
-                enic->pp.instance_uuid);
-        NLA_PUT(skb, IFLA_PORT_HOST_UUID, PORT_UUID_MAX,
-                enic->pp.host_uuid);
+        if (enic->pp.set & ENIC_SET_NAME)
+                NLA_PUT(skb, IFLA_PORT_PROFILE, PORT_PROFILE_MAX,
+                        enic->pp.name);
+        if (enic->pp.set & ENIC_SET_INSTANCE)
+                NLA_PUT(skb, IFLA_PORT_INSTANCE_UUID, PORT_UUID_MAX,
+                        enic->pp.instance_uuid);
+        if (enic->pp.set & ENIC_SET_HOST)
+                NLA_PUT(skb, IFLA_PORT_HOST_UUID, PORT_UUID_MAX,
+                        enic->pp.host_uuid);
 
         return 0;
 
diff --git a/drivers/net/epic100.c b/drivers/net/epic100.c
index 6838dfc9ef23..4c274657283c 100644
--- a/drivers/net/epic100.c
+++ b/drivers/net/epic100.c
@@ -87,6 +87,7 @@ static int rx_copybreak;
 #include <linux/bitops.h>
 #include <asm/io.h>
 #include <asm/uaccess.h>
+#include <asm/byteorder.h>
 
 /* These identify the driver base version and may not be removed. */
 static char version[] __devinitdata =
@@ -230,7 +231,7 @@ static const u16 media2miictl[16] = {
  * The EPIC100 Rx and Tx buffer descriptors.  Note that these
  * really ARE host-endian; it's not a misannotation.  We tell
  * the card to byteswap them internally on big-endian hosts -
- * look for #ifdef CONFIG_BIG_ENDIAN in epic_open().
+ * look for #ifdef __BIG_ENDIAN in epic_open().
  */
 
 struct epic_tx_desc {
@@ -690,7 +691,7 @@ static int epic_open(struct net_device *dev)
                 outl((inl(ioaddr + NVCTL) & ~0x003C) | 0x4800, ioaddr + NVCTL);
 
         /* Tell the chip to byteswap descriptors on big-endian hosts */
-#ifdef CONFIG_BIG_ENDIAN
+#ifdef __BIG_ENDIAN
         outl(0x4432 | (RX_FIFO_THRESH<<8), ioaddr + GENCTL);
         inl(ioaddr + GENCTL);
         outl(0x0432 | (RX_FIFO_THRESH<<8), ioaddr + GENCTL);
@@ -806,7 +807,7 @@ static void epic_restart(struct net_device *dev)
         for (i = 16; i > 0; i--)
                 outl(0x0008, ioaddr + TEST1);
 
-#ifdef CONFIG_BIG_ENDIAN
+#ifdef __BIG_ENDIAN
         outl(0x0432 | (RX_FIFO_THRESH<<8), ioaddr + GENCTL);
 #else
         outl(0x0412 | (RX_FIFO_THRESH<<8), ioaddr + GENCTL);
diff --git a/drivers/net/fec.c b/drivers/net/fec.c
index 326465ffbb23..edfff92a6d8e 100644
--- a/drivers/net/fec.c
+++ b/drivers/net/fec.c
@@ -681,6 +681,8 @@ static int fec_enet_mii_probe(struct net_device *dev)
         struct phy_device *phy_dev = NULL;
         int phy_addr;
 
+        fep->phy_dev = NULL;
+
         /* find the first phy */
         for (phy_addr = 0; phy_addr < PHY_MAX_ADDR; phy_addr++) {
                 if (fep->mii_bus->phy_map[phy_addr]) {
@@ -711,6 +713,11 @@ static int fec_enet_mii_probe(struct net_device *dev)
         fep->link = 0;
         fep->full_duplex = 0;
 
+        printk(KERN_INFO "%s: Freescale FEC PHY driver [%s] "
+                "(mii_bus:phy_addr=%s, irq=%d)\n", dev->name,
+                fep->phy_dev->drv->name, dev_name(&fep->phy_dev->dev),
+                fep->phy_dev->irq);
+
         return 0;
 }
 
@@ -756,13 +763,8 @@ static int fec_enet_mii_init(struct platform_device *pdev)
         if (mdiobus_register(fep->mii_bus))
                 goto err_out_free_mdio_irq;
 
-        if (fec_enet_mii_probe(dev) != 0)
-                goto err_out_unregister_bus;
-
         return 0;
 
-err_out_unregister_bus:
-        mdiobus_unregister(fep->mii_bus);
 err_out_free_mdio_irq:
         kfree(fep->mii_bus->irq);
 err_out_free_mdiobus:
@@ -915,7 +917,12 @@ fec_enet_open(struct net_device *dev)
         if (ret)
                 return ret;
 
-        /* schedule a link state check */
+        /* Probe and connect to PHY when open the interface */
+        ret = fec_enet_mii_probe(dev);
+        if (ret) {
+                fec_enet_free_buffers(dev);
+                return ret;
+        }
         phy_start(fep->phy_dev);
         netif_start_queue(dev);
         fep->opened = 1;
@@ -929,10 +936,12 @@ fec_enet_close(struct net_device *dev)
 
         /* Don't know what to do yet. */
         fep->opened = 0;
-        phy_stop(fep->phy_dev);
         netif_stop_queue(dev);
         fec_stop(dev);
 
+        if (fep->phy_dev)
+                phy_disconnect(fep->phy_dev);
+
         fec_enet_free_buffers(dev);
 
         return 0;
@@ -1316,11 +1325,6 @@ fec_probe(struct platform_device *pdev)
         if (ret)
                 goto failed_register;
 
-        printk(KERN_INFO "%s: Freescale FEC PHY driver [%s] "
-                "(mii_bus:phy_addr=%s, irq=%d)\n", ndev->name,
-                fep->phy_dev->drv->name, dev_name(&fep->phy_dev->dev),
-                fep->phy_dev->irq);
-
         return 0;
 
 failed_register:
@@ -1369,10 +1373,9 @@ fec_suspend(struct platform_device *dev, pm_message_t state)
 
         if (ndev) {
                 fep = netdev_priv(ndev);
-                if (netif_running(ndev)) {
-                        netif_device_detach(ndev);
-                        fec_stop(ndev);
-                }
+                if (netif_running(ndev))
+                        fec_enet_close(ndev);
+                clk_disable(fep->clk);
         }
         return 0;
 }
@@ -1381,12 +1384,13 @@ static int
 fec_resume(struct platform_device *dev)
 {
         struct net_device *ndev = platform_get_drvdata(dev);
+        struct fec_enet_private *fep;
 
         if (ndev) {
-                if (netif_running(ndev)) {
-                        fec_enet_init(ndev, 0);
-                        netif_device_attach(ndev);
-                }
+                fep = netdev_priv(ndev);
+                clk_enable(fep->clk);
+                if (netif_running(ndev))
+                        fec_enet_open(ndev);
         }
         return 0;
 }
diff --git a/drivers/net/fs_enet/mac-fcc.c b/drivers/net/fs_enet/mac-fcc.c
index 5d45084b287d..48e91b6242ce 100644
--- a/drivers/net/fs_enet/mac-fcc.c
+++ b/drivers/net/fs_enet/mac-fcc.c
@@ -504,17 +504,54 @@ static int get_regs_len(struct net_device *dev)
 }
 
 /* Some transmit errors cause the transmitter to shut
- * down.  We now issue a restart transmit.  Since the
- * errors close the BD and update the pointers, the restart
- * _should_ pick up without having to reset any of our
- * pointers either.  Also, To workaround 8260 device erratum
- * CPM37, we must disable and then re-enable the transmitter
- * following a Late Collision, Underrun, or Retry Limit error.
+ * down.  We now issue a restart transmit.
+ * Also, to workaround 8260 device erratum CPM37, we must
+ * disable and then re-enable the transmitterfollowing a
+ * Late Collision, Underrun, or Retry Limit error.
+ * In addition, tbptr may point beyond BDs beyond still marked
+ * as ready due to internal pipelining, so we need to look back
+ * through the BDs and adjust tbptr to point to the last BD
+ * marked as ready.  This may result in some buffers being
+ * retransmitted.
  */
 static void tx_restart(struct net_device *dev)
 {
         struct fs_enet_private *fep = netdev_priv(dev);
         fcc_t __iomem *fccp = fep->fcc.fccp;
+        const struct fs_platform_info *fpi = fep->fpi;
+        fcc_enet_t __iomem *ep = fep->fcc.ep;
+        cbd_t __iomem *curr_tbptr;
+        cbd_t __iomem *recheck_bd;
+        cbd_t __iomem *prev_bd;
+        cbd_t __iomem *last_tx_bd;
+
+        last_tx_bd = fep->tx_bd_base + (fpi->tx_ring * sizeof(cbd_t));
+
+        /* get the current bd held in TBPTR  and scan back from this point */
+        recheck_bd = curr_tbptr = (cbd_t __iomem *)
+                ((R32(ep, fen_genfcc.fcc_tbptr) - fep->ring_mem_addr) +
+                fep->ring_base);
+
+        prev_bd = (recheck_bd == fep->tx_bd_base) ? last_tx_bd : recheck_bd - 1;
+
+        /* Move through the bds in reverse, look for the earliest buffer
+         * that is not ready.  Adjust TBPTR to the following buffer */
+        while ((CBDR_SC(prev_bd) & BD_ENET_TX_READY) != 0) {
+                /* Go back one buffer */
+                recheck_bd = prev_bd;
+
+                /* update the previous buffer */
+                prev_bd = (prev_bd == fep->tx_bd_base) ? last_tx_bd : prev_bd - 1;
+
+                /* We should never see all bds marked as ready, check anyway */
+                if (recheck_bd == curr_tbptr)
+                        break;
+        }
+        /* Now update the TBPTR and dirty flag to the current buffer */
+        W32(ep, fen_genfcc.fcc_tbptr,
+                (uint) (((void *)recheck_bd - fep->ring_base) +
+                fep->ring_mem_addr));
+        fep->dirty_tx = recheck_bd;
 
         C32(fccp, fcc_gfmr, FCC_GFMR_ENT);
         udelay(10);
diff --git a/drivers/net/fs_enet/mii-bitbang.c b/drivers/net/fs_enet/mii-bitbang.c
index 0f90685d3d19..3607340f3da7 100644
--- a/drivers/net/fs_enet/mii-bitbang.c
+++ b/drivers/net/fs_enet/mii-bitbang.c
@@ -169,7 +169,7 @@ static int __devinit fs_enet_mdio_probe(struct of_device *ofdev,
 
         new_bus->name = "CPM2 Bitbanged MII",
 
-        ret = fs_mii_bitbang_init(new_bus, ofdev->node);
+        ret = fs_mii_bitbang_init(new_bus, ofdev->dev.of_node);
         if (ret)
                 goto out_free_bus;
 
@@ -181,7 +181,7 @@ static int __devinit fs_enet_mdio_probe(struct of_device *ofdev,
         new_bus->parent = &ofdev->dev;
         dev_set_drvdata(&ofdev->dev, new_bus);
 
-        ret = of_mdiobus_register(new_bus, ofdev->node);
+        ret = of_mdiobus_register(new_bus, ofdev->dev.of_node);
         if (ret)
                 goto out_free_irqs;
 
diff --git a/drivers/net/gianfar.c b/drivers/net/gianfar.c
index 1830f3199cb5..46c69cd06553 100644
--- a/drivers/net/gianfar.c
+++ b/drivers/net/gianfar.c
@@ -747,8 +747,7 @@ static int gfar_of_init(struct of_device *ofdev, struct net_device **pdev)
                         FSL_GIANFAR_DEV_HAS_CSUM |
                         FSL_GIANFAR_DEV_HAS_VLAN |
                         FSL_GIANFAR_DEV_HAS_MAGIC_PACKET |
-                        FSL_GIANFAR_DEV_HAS_EXTENDED_HASH |
-                        FSL_GIANFAR_DEV_HAS_TIMER;
+                        FSL_GIANFAR_DEV_HAS_EXTENDED_HASH;
 
         ctype = of_get_property(np, "phy-connection-type", NULL);
 
diff --git a/drivers/net/greth.c b/drivers/net/greth.c
index f37a4c143ddd..3a029d02c2b4 100644
--- a/drivers/net/greth.c
+++ b/drivers/net/greth.c
@@ -1607,14 +1607,13 @@ static struct of_device_id greth_of_match[] = {
 MODULE_DEVICE_TABLE(of, greth_of_match);
 
 static struct of_platform_driver greth_of_driver = {
-        .name = "grlib-greth",
-        .match_table = greth_of_match,
+        .driver = {
+                .name = "grlib-greth",
+                .owner = THIS_MODULE,
+                .of_match_table = greth_of_match,
+        },
         .probe = greth_of_probe,
         .remove = __devexit_p(greth_of_remove),
-        .driver = {
-                   .owner = THIS_MODULE,
-                   .name = "grlib-greth",
-                   },
 };
 
 static int __init greth_init(void)
diff --git a/drivers/net/hamradio/yam.c b/drivers/net/hamradio/yam.c
index 694132e04af6..4e7d1d0a2340 100644
--- a/drivers/net/hamradio/yam.c
+++ b/drivers/net/hamradio/yam.c
@@ -1151,8 +1151,7 @@ static int __init yam_init_driver(void)
                 dev = alloc_netdev(sizeof(struct yam_port), name,
                                    yam_setup);
                 if (!dev) {
-                        printk(KERN_ERR "yam: cannot allocate net device %s\n",
-                               dev->name);
+                        pr_err("yam: cannot allocate net device\n");
                         err = -ENOMEM;
                         goto error;
                 }
diff --git a/drivers/net/ixgbe/ixgbe_common.c b/drivers/net/ixgbe/ixgbe_common.c
index 1159d9138f05..9595b1bfb8dd 100644
--- a/drivers/net/ixgbe/ixgbe_common.c
+++ b/drivers/net/ixgbe/ixgbe_common.c
@@ -1188,6 +1188,7 @@ s32 ixgbe_set_rar_generic(struct ixgbe_hw *hw, u32 index, u8 *addr, u32 vmdq,
                 IXGBE_WRITE_REG(hw, IXGBE_RAH(index), rar_high);
         } else {
                 hw_dbg(hw, "RAR index %d is out of range.\n", index);
+                return IXGBE_ERR_RAR_INDEX;
         }
 
         return 0;
@@ -1219,6 +1220,7 @@ s32 ixgbe_clear_rar_generic(struct ixgbe_hw *hw, u32 index)
                 IXGBE_WRITE_REG(hw, IXGBE_RAH(index), rar_high);
         } else {
                 hw_dbg(hw, "RAR index %d is out of range.\n", index);
+                return IXGBE_ERR_RAR_INDEX;
         }
 
         /* clear VMDq pool/queue selection for this RAR */
diff --git a/drivers/net/ixgbe/ixgbe_main.c b/drivers/net/ixgbe/ixgbe_main.c
index d571d101de08..b2af2f67f604 100644
--- a/drivers/net/ixgbe/ixgbe_main.c
+++ b/drivers/net/ixgbe/ixgbe_main.c
@@ -642,7 +642,7 @@ static inline bool ixgbe_tx_xon_state(struct ixgbe_adapter *adapter,
         u32 txoff = IXGBE_TFCS_TXOFF;
 
 #ifdef CONFIG_IXGBE_DCB
-        if (adapter->flags & IXGBE_FLAG_DCB_ENABLED) {
+        if (adapter->dcb_cfg.pfc_mode_enable) {
                 int tc;
                 int reg_idx = tx_ring->reg_idx;
                 int dcb_i = adapter->ring_feature[RING_F_DCB].indices;
diff --git a/drivers/net/ixgbe/ixgbe_type.h b/drivers/net/ixgbe/ixgbe_type.h
index 2eb6e151016c..cdd1998f18c7 100644
--- a/drivers/net/ixgbe/ixgbe_type.h
+++ b/drivers/net/ixgbe/ixgbe_type.h
@@ -2609,6 +2609,7 @@ struct ixgbe_info {
 #define IXGBE_ERR_EEPROM_VERSION                -24
 #define IXGBE_ERR_NO_SPACE                      -25
 #define IXGBE_ERR_OVERTEMP                      -26
+#define IXGBE_ERR_RAR_INDEX                     -27
 #define IXGBE_NOT_IMPLEMENTED                   0x7FFFFFFF
 
 #endif /* _IXGBE_TYPE_H_ */
diff --git a/drivers/net/korina.c b/drivers/net/korina.c
index 26bf1b76b997..c7a9bef4dfb0 100644
--- a/drivers/net/korina.c
+++ b/drivers/net/korina.c
@@ -135,6 +135,7 @@ struct korina_private {
         struct napi_struct napi;
         struct timer_list media_check_timer;
         struct mii_if_info mii_if;
+        struct work_struct restart_task;
         struct net_device *dev;
         int phy_addr;
 };
@@ -375,7 +376,7 @@ static int korina_rx(struct net_device *dev, int limit)
                 if (devcs & ETH_RX_LE)
                         dev->stats.rx_length_errors++;
                 if (devcs & ETH_RX_OVR)
-                        dev->stats.rx_over_errors++;
+                        dev->stats.rx_fifo_errors++;
                 if (devcs & ETH_RX_CV)
                         dev->stats.rx_frame_errors++;
                 if (devcs & ETH_RX_CES)
@@ -764,10 +765,9 @@ static int korina_alloc_ring(struct net_device *dev)
 
         /* Initialize the receive descriptors */
         for (i = 0; i < KORINA_NUM_RDS; i++) {
-                skb = dev_alloc_skb(KORINA_RBSIZE + 2);
+                skb = netdev_alloc_skb_ip_align(dev, KORINA_RBSIZE);
                 if (!skb)
                         return -ENOMEM;
-                skb_reserve(skb, 2);
                 lp->rx_skb[i] = skb;
                 lp->rd_ring[i].control = DMA_DESC_IOD |
                                 DMA_COUNT(KORINA_RBSIZE);
@@ -890,12 +890,12 @@ static int korina_init(struct net_device *dev)
 
 /*
  * Restart the RC32434 ethernet controller.
- * FIXME: check the return status where we call it
  */
-static int korina_restart(struct net_device *dev)
+static void korina_restart_task(struct work_struct *work)
 {
-        struct korina_private *lp = netdev_priv(dev);
-        int ret;
+        struct korina_private *lp = container_of(work,
+                        struct korina_private, restart_task);
+        struct net_device *dev = lp->dev;
 
         /*
          * Disable interrupts
@@ -916,10 +916,9 @@ static int korina_restart(struct net_device *dev)
 
         napi_disable(&lp->napi);
 
-        ret = korina_init(dev);
-        if (ret < 0) {
+        if (korina_init(dev) < 0) {
                 printk(KERN_ERR "%s: cannot restart device\n", dev->name);
-                return ret;
+                return;
         }
         korina_multicast_list(dev);
 
@@ -927,8 +926,6 @@ static int korina_restart(struct net_device *dev)
         enable_irq(lp->ovr_irq);
         enable_irq(lp->tx_irq);
         enable_irq(lp->rx_irq);
-
-        return ret;
 }
 
 static void korina_clear_and_restart(struct net_device *dev, u32 value)
@@ -937,7 +934,7 @@ static void korina_clear_and_restart(struct net_device *dev, u32 value)
 
         netif_stop_queue(dev);
         writel(value, &lp->eth_regs->ethintfc);
-        korina_restart(dev);
+        schedule_work(&lp->restart_task);
 }
 
 /* Ethernet Tx Underflow interrupt */
@@ -962,11 +959,8 @@ static irqreturn_t korina_und_interrupt(int irq, void *dev_id)
 static void korina_tx_timeout(struct net_device *dev)
 {
         struct korina_private *lp = netdev_priv(dev);
-        unsigned long flags;
 
-        spin_lock_irqsave(&lp->lock, flags);
-        korina_restart(dev);
-        spin_unlock_irqrestore(&lp->lock, flags);
+        schedule_work(&lp->restart_task);
 }
 
 /* Ethernet Rx Overflow interrupt */
@@ -1086,6 +1080,8 @@ static int korina_close(struct net_device *dev)
 
         napi_disable(&lp->napi);
 
+        cancel_work_sync(&lp->restart_task);
+
         free_irq(lp->rx_irq, dev);
         free_irq(lp->tx_irq, dev);
         free_irq(lp->ovr_irq, dev);
@@ -1198,6 +1194,8 @@ static int korina_probe(struct platform_device *pdev)
         }
         setup_timer(&lp->media_check_timer, korina_poll_media, (unsigned long) dev);
 
+        INIT_WORK(&lp->restart_task, korina_restart_task);
+
         printk(KERN_INFO "%s: " DRV_NAME "-" DRV_VERSION " " DRV_RELDATE "\n",
                         dev->name);
 out:
diff --git a/drivers/net/ksz884x.c b/drivers/net/ksz884x.c
index c80ca64277b2..7805bbf1d53a 100644
--- a/drivers/net/ksz884x.c
+++ b/drivers/net/ksz884x.c
@@ -4854,7 +4854,7 @@ static inline void copy_old_skb(struct sk_buff *old, struct sk_buff *skb)
  *
  * Return 0 if successful; otherwise an error code indicating failure.
  */
-static int netdev_tx(struct sk_buff *skb, struct net_device *dev)
+static netdev_tx_t netdev_tx(struct sk_buff *skb, struct net_device *dev)
 {
         struct dev_priv *priv = netdev_priv(dev);
         struct dev_info *hw_priv = priv->adapter;
@@ -6863,6 +6863,7 @@ static const struct net_device_ops netdev_ops = {
         .ndo_tx_timeout         = netdev_tx_timeout,
         .ndo_change_mtu         = netdev_change_mtu,
         .ndo_set_mac_address    = netdev_set_mac_address,
+        .ndo_validate_addr      = eth_validate_addr,
         .ndo_do_ioctl           = netdev_ioctl,
         .ndo_set_rx_mode        = netdev_set_rx_mode,
 #ifdef CONFIG_NET_POLL_CONTROLLER
diff --git a/drivers/net/ll_temac.h b/drivers/net/ll_temac.h
index c03358434acb..522abe2ff25a 100644
--- a/drivers/net/ll_temac.h
+++ b/drivers/net/ll_temac.h
@@ -295,6 +295,10 @@ This option defaults to enabled (set) */
 
 #define MULTICAST_CAM_TABLE_NUM 4
 
+/* TEMAC Synthesis features */
+#define TEMAC_FEATURE_RX_CSUM  (1 << 0)
+#define TEMAC_FEATURE_TX_CSUM  (1 << 1)
+
 /* TX/RX CURDESC_PTR points to first descriptor */
 /* TX/RX TAILDESC_PTR points to last descriptor in linked list */
 
@@ -353,6 +357,7 @@ struct temac_local {
         struct mutex indirect_mutex;
         u32 options;                    /* Current options word */
         int last_link;
+        unsigned int temac_features;
 
         /* Buffer descriptors */
         struct cdmac_bd *tx_bd_v;
diff --git a/drivers/net/ll_temac_main.c b/drivers/net/ll_temac_main.c
index fa7620e28404..52dcc8495647 100644
--- a/drivers/net/ll_temac_main.c
+++ b/drivers/net/ll_temac_main.c
@@ -245,7 +245,7 @@ static int temac_dma_bd_init(struct net_device *ndev)
                                           CHNL_CTRL_IRQ_COAL_EN);
         /* 0x10220483 */
         /* 0x00100483 */
-        lp->dma_out(lp, RX_CHNL_CTRL, 0xff010000 |
+        lp->dma_out(lp, RX_CHNL_CTRL, 0xff070000 |
                                           CHNL_CTRL_IRQ_EN |
                                           CHNL_CTRL_IRQ_DLY_EN |
                                           CHNL_CTRL_IRQ_COAL_EN |
@@ -574,6 +574,10 @@ static void temac_start_xmit_done(struct net_device *ndev)
                 if (cur_p->app4)
                         dev_kfree_skb_irq((struct sk_buff *)cur_p->app4);
                 cur_p->app0 = 0;
+                cur_p->app1 = 0;
+                cur_p->app2 = 0;
+                cur_p->app3 = 0;
+                cur_p->app4 = 0;
 
                 ndev->stats.tx_packets++;
                 ndev->stats.tx_bytes += cur_p->len;
@@ -589,6 +593,29 @@ static void temac_start_xmit_done(struct net_device *ndev)
         netif_wake_queue(ndev);
 }
 
+static inline int temac_check_tx_bd_space(struct temac_local *lp, int num_frag)
+{
+        struct cdmac_bd *cur_p;
+        int tail;
+
+        tail = lp->tx_bd_tail;
+        cur_p = &lp->tx_bd_v[tail];
+
+        do {
+                if (cur_p->app0)
+                        return NETDEV_TX_BUSY;
+
+                tail++;
+                if (tail >= TX_BD_NUM)
+                        tail = 0;
+
+                cur_p = &lp->tx_bd_v[tail];
+                num_frag--;
+        } while (num_frag >= 0);
+
+        return 0;
+}
+
 static int temac_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 {
         struct temac_local *lp = netdev_priv(ndev);
@@ -603,7 +630,7 @@ static int temac_start_xmit(struct sk_buff *skb, struct net_device *ndev)
         start_p = lp->tx_bd_p + sizeof(*lp->tx_bd_v) * lp->tx_bd_tail;
         cur_p = &lp->tx_bd_v[lp->tx_bd_tail];
 
-        if (cur_p->app0 & STS_CTRL_APP0_CMPLT) {
+        if (temac_check_tx_bd_space(lp, num_frag)) {
                 if (!netif_queue_stopped(ndev)) {
                         netif_stop_queue(ndev);
                         return NETDEV_TX_BUSY;
@@ -613,29 +640,14 @@ static int temac_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 
         cur_p->app0 = 0;
         if (skb->ip_summed == CHECKSUM_PARTIAL) {
-                const struct iphdr *ip = ip_hdr(skb);
-                int length = 0, start = 0, insert = 0;
-
-                switch (ip->protocol) {
-                case IPPROTO_TCP:
-                        start = sizeof(struct iphdr) + ETH_HLEN;
-                        insert = sizeof(struct iphdr) + ETH_HLEN + 16;
-                        length = ip->tot_len - sizeof(struct iphdr);
-                        break;
-                case IPPROTO_UDP:
-                        start = sizeof(struct iphdr) + ETH_HLEN;
-                        insert = sizeof(struct iphdr) + ETH_HLEN + 6;
-                        length = ip->tot_len - sizeof(struct iphdr);
-                        break;
-                default:
-                        break;
-                }
-                cur_p->app1 = ((start << 16) | insert);
-                cur_p->app2 = csum_tcpudp_magic(ip->saddr, ip->daddr,
-                                                length, ip->protocol, 0);
-                skb->data[insert] = 0;
-                skb->data[insert + 1] = 0;
+                unsigned int csum_start_off = skb_transport_offset(skb);
+                unsigned int csum_index_off = csum_start_off + skb->csum_offset;
+
+                cur_p->app0 |= 1; /* TX Checksum Enabled */
+                cur_p->app1 = (csum_start_off << 16) | csum_index_off;
+                cur_p->app2 = 0;  /* initial checksum seed */
         }
+
         cur_p->app0 |= STS_CTRL_APP0_SOP;
         cur_p->len = skb_headlen(skb);
         cur_p->phys = dma_map_single(ndev->dev.parent, skb->data, skb->len,
@@ -699,6 +711,15 @@ static void ll_temac_recv(struct net_device *ndev)
                 skb->protocol = eth_type_trans(skb, ndev);
                 skb->ip_summed = CHECKSUM_NONE;
 
+                /* if we're doing rx csum offload, set it up */
+                if (((lp->temac_features & TEMAC_FEATURE_RX_CSUM) != 0) &&
+                        (skb->protocol == __constant_htons(ETH_P_IP)) &&
+                        (skb->len > 64)) {
+
+                        skb->csum = cur_p->app3 & 0xFFFF;
+                        skb->ip_summed = CHECKSUM_COMPLETE;
+                }
+
                 netif_rx(skb);
 
                 ndev->stats.rx_packets++;
@@ -883,6 +904,7 @@ temac_of_probe(struct of_device *op, const struct of_device_id *match)
         struct temac_local *lp;
         struct net_device *ndev;
         const void *addr;
+        __be32 *p;
         int size, rc = 0;
 
         /* Init network device structure */
@@ -926,6 +948,18 @@ temac_of_probe(struct of_device *op, const struct of_device_id *match)
                 goto nodev;
         }
 
+        /* Setup checksum offload, but default to off if not specified */
+        lp->temac_features = 0;
+        p = (__be32 *)of_get_property(op->dev.of_node, "xlnx,txcsum", NULL);
+        if (p && be32_to_cpu(*p)) {
+                lp->temac_features |= TEMAC_FEATURE_TX_CSUM;
+                /* Can checksum TCP/UDP over IPv4. */
+                ndev->features |= NETIF_F_IP_CSUM;
+        }
+        p = (__be32 *)of_get_property(op->dev.of_node, "xlnx,rxcsum", NULL);
+        if (p && be32_to_cpu(*p))
+                lp->temac_features |= TEMAC_FEATURE_RX_CSUM;
+
         /* Find the DMA node, map the DMA registers, and decode the DMA IRQs */
         np = of_parse_phandle(op->dev.of_node, "llink-connected", 0);
         if (!np) {
@@ -950,7 +984,7 @@ temac_of_probe(struct of_device *op, const struct of_device_id *match)
 
         lp->rx_irq = irq_of_parse_and_map(np, 0);
         lp->tx_irq = irq_of_parse_and_map(np, 1);
-        if (!lp->rx_irq || !lp->tx_irq) {
+        if ((lp->rx_irq == NO_IRQ) || (lp->tx_irq == NO_IRQ)) {
                 dev_err(&op->dev, "could not determine irqs\n");
                 rc = -ENOMEM;
                 goto nodev;
diff --git a/drivers/net/mace.c b/drivers/net/mace.c
index b6855a6476f8..1c5221f79d6f 100644
--- a/drivers/net/mace.c
+++ b/drivers/net/mace.c
@@ -997,8 +997,11 @@ MODULE_DEVICE_TABLE (of, mace_match);
 
 static struct macio_driver mace_driver =
 {
-        .name           = "mace",
-        .match_table    = mace_match,
+        .driver = {
+                .name           = "mace",
+                .owner          = THIS_MODULE,
+                .of_match_table = mace_match,
+        },
         .probe          = mace_probe,
         .remove         = mace_remove,
 };
diff --git a/drivers/net/pcmcia/smc91c92_cs.c b/drivers/net/pcmcia/smc91c92_cs.c
index 7b6fe89f9db0..64e6a84bbbbe 100644
--- a/drivers/net/pcmcia/smc91c92_cs.c
+++ b/drivers/net/pcmcia/smc91c92_cs.c
@@ -322,6 +322,7 @@ static int smc91c92_probe(struct pcmcia_device *link)
         return -ENOMEM;
     smc = netdev_priv(dev);
     smc->p_dev = link;
+    link->priv = dev;
 
     spin_lock_init(&smc->lock);
     link->io.NumPorts1 = 16;
diff --git a/drivers/net/phy/lxt.c b/drivers/net/phy/lxt.c
index 8ee929b796d8..dbd003453737 100644
--- a/drivers/net/phy/lxt.c
+++ b/drivers/net/phy/lxt.c
@@ -53,6 +53,9 @@
 
 #define MII_LXT971_ISR          19  /* Interrupt Status Register */
 
+/* register definitions for the 973 */
+#define MII_LXT973_PCR 16 /* Port Configuration Register */
+#define PCR_FIBER_SELECT 1
 
 MODULE_DESCRIPTION("Intel LXT PHY driver");
 MODULE_AUTHOR("Andy Fleming");
@@ -119,6 +122,33 @@ static int lxt971_config_intr(struct phy_device *phydev)
         return err;
 }
 
+static int lxt973_probe(struct phy_device *phydev)
+{
+        int val = phy_read(phydev, MII_LXT973_PCR);
+
+        if (val & PCR_FIBER_SELECT) {
+                /*
+                 * If fiber is selected, then the only correct setting
+                 * is 100Mbps, full duplex, and auto negotiation off.
+                 */
+                val = phy_read(phydev, MII_BMCR);
+                val |= (BMCR_SPEED100 | BMCR_FULLDPLX);
+                val &= ~BMCR_ANENABLE;
+                phy_write(phydev, MII_BMCR, val);
+                /* Remember that the port is in fiber mode. */
+                phydev->priv = lxt973_probe;
+        } else {
+                phydev->priv = NULL;
+        }
+        return 0;
+}
+
+static int lxt973_config_aneg(struct phy_device *phydev)
+{
+        /* Do nothing if port is in fiber mode. */
+        return phydev->priv ? 0 : genphy_config_aneg(phydev);
+}
+
 static struct phy_driver lxt970_driver = {
         .phy_id         = 0x78100000,
         .name           = "LXT970",
@@ -146,6 +176,18 @@ static struct phy_driver lxt971_driver = {
         .driver         = { .owner = THIS_MODULE,},
 };
 
+static struct phy_driver lxt973_driver = {
+        .phy_id         = 0x00137a10,
+        .name           = "LXT973",
+        .phy_id_mask    = 0xfffffff0,
+        .features       = PHY_BASIC_FEATURES,
+        .flags          = 0,
+        .probe          = lxt973_probe,
+        .config_aneg    = lxt973_config_aneg,
+        .read_status    = genphy_read_status,
+        .driver         = { .owner = THIS_MODULE,},
+};
+
 static int __init lxt_init(void)
 {
         int ret;
@@ -157,9 +199,15 @@ static int __init lxt_init(void)
         ret = phy_driver_register(&lxt971_driver);
         if (ret)
                 goto err2;
+
+        ret = phy_driver_register(&lxt973_driver);
+        if (ret)
+                goto err3;
         return 0;
 
- err2:  
+ err3:
+        phy_driver_unregister(&lxt971_driver);
+ err2:
         phy_driver_unregister(&lxt970_driver);
  err1:
         return ret;
@@ -169,6 +217,7 @@ static void __exit lxt_exit(void)
 {
         phy_driver_unregister(&lxt970_driver);
         phy_driver_unregister(&lxt971_driver);
+        phy_driver_unregister(&lxt973_driver);
 }
 
 module_init(lxt_init);
diff --git a/drivers/net/ppp_generic.c b/drivers/net/ppp_generic.c
index c5f8eb102bf7..1b2c29150202 100644
--- a/drivers/net/ppp_generic.c
+++ b/drivers/net/ppp_generic.c
@@ -1422,7 +1422,7 @@ static int ppp_mp_explode(struct ppp *ppp, struct sk_buff *skb)
                 flen = len;
                 if (nfree > 0) {
                         if (pch->speed == 0) {
-                                flen = totlen/nfree;
+                                flen = len/nfree;
                                 if (nbigger > 0) {
                                         flen++;
                                         nbigger--;
diff --git a/drivers/net/r8169.c b/drivers/net/r8169.c
index 217e709bda3e..96b6cfbf0a3a 100644
--- a/drivers/net/r8169.c
+++ b/drivers/net/r8169.c
@@ -559,6 +559,11 @@ static void mdio_write(void __iomem *ioaddr, int reg_addr, int value)
                         break;
                 udelay(25);
         }
+        /*
+         * According to hardware specs a 20us delay is required after write
+         * complete indication, but before sending next command.
+         */
+        udelay(20);
 }
 
 static int mdio_read(void __iomem *ioaddr, int reg_addr)
@@ -578,6 +583,12 @@ static int mdio_read(void __iomem *ioaddr, int reg_addr)
                 }
                 udelay(25);
         }
+        /*
+         * According to hardware specs a 20us delay is required after read
+         * complete indication, but before sending next command.
+         */
+        udelay(20);
+
         return value;
 }
 
diff --git a/drivers/net/sfc/net_driver.h b/drivers/net/sfc/net_driver.h
index 2e6fd89f2a72..4762c91cb587 100644
--- a/drivers/net/sfc/net_driver.h
+++ b/drivers/net/sfc/net_driver.h
@@ -830,7 +830,7 @@ static inline const char *efx_dev_name(struct efx_nic *efx)
 
 static inline unsigned int efx_port_num(struct efx_nic *efx)
 {
-        return PCI_FUNC(efx->pci_dev->devfn);
+        return efx->net_dev->dev_id;
 }
 
 /**
diff --git a/drivers/net/sfc/siena.c b/drivers/net/sfc/siena.c
index 727b4228e081..f2b1e6180753 100644
--- a/drivers/net/sfc/siena.c
+++ b/drivers/net/sfc/siena.c
@@ -206,6 +206,7 @@ static int siena_probe_nic(struct efx_nic *efx)
 {
         struct siena_nic_data *nic_data;
         bool already_attached = 0;
+        efx_oword_t reg;
         int rc;
 
         /* Allocate storage for hardware specific data */
@@ -220,6 +221,9 @@ static int siena_probe_nic(struct efx_nic *efx)
                 goto fail1;
         }
 
+        efx_reado(efx, &reg, FR_AZ_CS_DEBUG);
+        efx->net_dev->dev_id = EFX_OWORD_FIELD(reg, FRF_CZ_CS_PORT_NUM) - 1;
+
         efx_mcdi_init(efx);
 
         /* Recover from a failed assertion before probing */
diff --git a/drivers/net/tehuti.c b/drivers/net/tehuti.c
index 20ab16192325..737df6032bbc 100644
--- a/drivers/net/tehuti.c
+++ b/drivers/net/tehuti.c
@@ -646,7 +646,7 @@ static int bdx_ioctl_priv(struct net_device *ndev, struct ifreq *ifr, int cmd)
                 error = copy_from_user(data, ifr->ifr_data, sizeof(data));
                 if (error) {
                         pr_err("cant copy from user\n");
-                        RET(error);
+                        RET(-EFAULT);
                 }
                 DBG("%d 0x%x 0x%x\n", data[0], data[1], data[2]);
         }
@@ -665,7 +665,7 @@ static int bdx_ioctl_priv(struct net_device *ndev, struct ifreq *ifr, int cmd)
                     data[2]);
                 error = copy_to_user(ifr->ifr_data, data, sizeof(data));
                 if (error)
-                        RET(error);
+                        RET(-EFAULT);
                 break;
 
         case BDX_OP_WRITE:
diff --git a/drivers/net/usb/asix.c b/drivers/net/usb/asix.c
index 1f802e90474c..9516f382a6ba 100644
--- a/drivers/net/usb/asix.c
+++ b/drivers/net/usb/asix.c
@@ -344,7 +344,7 @@ static int asix_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
                         return 2;
                 }
 
-                if (size > ETH_FRAME_LEN) {
+                if (size > dev->net->mtu + ETH_HLEN) {
                         netdev_err(dev->net, "asix_rx_fixup() Bad RX Length %d\n",
                                    size);
                         return 0;
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index 78eb3190b9b1..1edb7a61983c 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -340,7 +340,7 @@ static int add_recvbuf_small(struct virtnet_info *vi, gfp_t gfp)
 
         skb_to_sgvec(skb, vi->rx_sg + 1, 0, skb->len);
 
-        err = virtqueue_add_buf(vi->rvq, vi->rx_sg, 0, 2, skb);
+        err = virtqueue_add_buf_gfp(vi->rvq, vi->rx_sg, 0, 2, skb, gfp);
         if (err < 0)
                 dev_kfree_skb(skb);
 
@@ -385,8 +385,8 @@ static int add_recvbuf_big(struct virtnet_info *vi, gfp_t gfp)
 
         /* chain first in list head */
         first->private = (unsigned long)list;
-        err = virtqueue_add_buf(vi->rvq, vi->rx_sg, 0, MAX_SKB_FRAGS + 2,
-                                       first);
+        err = virtqueue_add_buf_gfp(vi->rvq, vi->rx_sg, 0, MAX_SKB_FRAGS + 2,
+                                    first, gfp);
         if (err < 0)
                 give_pages(vi, first);
 
@@ -404,7 +404,7 @@ static int add_recvbuf_mergeable(struct virtnet_info *vi, gfp_t gfp)
 
         sg_init_one(vi->rx_sg, page_address(page), PAGE_SIZE);
 
-        err = virtqueue_add_buf(vi->rvq, vi->rx_sg, 0, 1, page);
+        err = virtqueue_add_buf_gfp(vi->rvq, vi->rx_sg, 0, 1, page, gfp);
         if (err < 0)
                 give_pages(vi, page);
 
diff --git a/drivers/net/wan/x25_asy.c b/drivers/net/wan/x25_asy.c
index 166e77dfffda..e47f5a986b1c 100644
--- a/drivers/net/wan/x25_asy.c
+++ b/drivers/net/wan/x25_asy.c
@@ -37,8 +37,6 @@
 #include <net/x25device.h>
 #include "x25_asy.h"
 
-#include <net/x25device.h>
-
 static struct net_device **x25_asy_devs;
 static int x25_asy_maxdev = SL_NRUNIT;
 
diff --git a/drivers/net/wimax/i2400m/fw.c b/drivers/net/wimax/i2400m/fw.c
index 3f283bff0ff7..11491354e5b5 100644
--- a/drivers/net/wimax/i2400m/fw.c
+++ b/drivers/net/wimax/i2400m/fw.c
@@ -1192,7 +1192,7 @@ int i2400m_fw_hdr_check(struct i2400m *i2400m,
         unsigned module_type, header_len, major_version, minor_version,
                 module_id, module_vendor, date, size;
 
-        module_type = bcf_hdr->module_type;
+        module_type = le32_to_cpu(bcf_hdr->module_type);
         header_len = sizeof(u32) * le32_to_cpu(bcf_hdr->header_len);
         major_version = (le32_to_cpu(bcf_hdr->header_version) & 0xffff0000)
                 >> 16;
diff --git a/drivers/net/wireless/ath/ar9170/usb.c b/drivers/net/wireless/ath/ar9170/usb.c
index 82ab532a4923..a93dc18a45c3 100644
--- a/drivers/net/wireless/ath/ar9170/usb.c
+++ b/drivers/net/wireless/ath/ar9170/usb.c
@@ -739,17 +739,27 @@ err_out:
 static void ar9170_usb_firmware_failed(struct ar9170_usb *aru)
 {
         struct device *parent = aru->udev->dev.parent;
+        struct usb_device *udev;
+
+        /*
+         * Store a copy of the usb_device pointer locally.
+         * This is because device_release_driver initiates
+         * ar9170_usb_disconnect, which in turn frees our
+         * driver context (aru).
+         */
+        udev = aru->udev;
 
         complete(&aru->firmware_loading_complete);
 
         /* unbind anything failed */
         if (parent)
                 device_lock(parent);
-        device_release_driver(&aru->udev->dev);
+
+        device_release_driver(&udev->dev);
         if (parent)
                 device_unlock(parent);
 
-        usb_put_dev(aru->udev);
+        usb_put_dev(udev);
 }
 
 static void ar9170_usb_firmware_finish(const struct firmware *fw, void *context)
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index cc6d41dec332..648972df369d 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -195,7 +195,7 @@ static const struct ieee80211_rate ath5k_rates[] = {
 static int __devinit    ath5k_pci_probe(struct pci_dev *pdev,
                                 const struct pci_device_id *id);
 static void __devexit   ath5k_pci_remove(struct pci_dev *pdev);
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int              ath5k_pci_suspend(struct device *dev);
 static int              ath5k_pci_resume(struct device *dev);
 
@@ -203,7 +203,7 @@ static SIMPLE_DEV_PM_OPS(ath5k_pm_ops, ath5k_pci_suspend, ath5k_pci_resume);
 #define ATH5K_PM_OPS    (&ath5k_pm_ops)
 #else
 #define ATH5K_PM_OPS    NULL
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static struct pci_driver ath5k_pci_driver = {
         .name           = KBUILD_MODNAME,
@@ -222,7 +222,6 @@ static int ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
 static int ath5k_tx_queue(struct ieee80211_hw *hw, struct sk_buff *skb,
                 struct ath5k_txq *txq);
 static int ath5k_reset(struct ath5k_softc *sc, struct ieee80211_channel *chan);
-static int ath5k_reset_wake(struct ath5k_softc *sc);
 static int ath5k_start(struct ieee80211_hw *hw);
 static void ath5k_stop(struct ieee80211_hw *hw);
 static int ath5k_add_interface(struct ieee80211_hw *hw,
@@ -709,7 +708,7 @@ ath5k_pci_remove(struct pci_dev *pdev)
         ieee80211_free_hw(hw);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int ath5k_pci_suspend(struct device *dev)
 {
         struct ieee80211_hw *hw = pci_get_drvdata(to_pci_dev(dev));
@@ -735,7 +734,7 @@ static int ath5k_pci_resume(struct device *dev)
         ath5k_led_enable(sc);
         return 0;
 }
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 
 /***********************\
@@ -2770,7 +2769,7 @@ ath5k_tasklet_reset(unsigned long data)
 {
         struct ath5k_softc *sc = (void *)data;
 
-        ath5k_reset_wake(sc);
+        ath5k_reset(sc, sc->curchan);
 }
 
 /*
@@ -2941,23 +2940,13 @@ ath5k_reset(struct ath5k_softc *sc, struct ieee80211_channel *chan)
         ath5k_beacon_config(sc);
         /* intrs are enabled by ath5k_beacon_config */
 
+        ieee80211_wake_queues(sc->hw);
+
         return 0;
 err:
         return ret;
 }
 
-static int
-ath5k_reset_wake(struct ath5k_softc *sc)
-{
-        int ret;
-
-        ret = ath5k_reset(sc, sc->curchan);
-        if (!ret)
-                ieee80211_wake_queues(sc->hw);
-
-        return ret;
-}
-
 static int ath5k_start(struct ieee80211_hw *hw)
 {
         return ath5k_init(hw->priv);
@@ -3151,13 +3140,15 @@ static void ath5k_configure_filter(struct ieee80211_hw *hw,
 
         if (changed_flags & (FIF_PROMISC_IN_BSS | FIF_OTHER_BSS)) {
                 if (*new_flags & FIF_PROMISC_IN_BSS) {
-                        rfilt |= AR5K_RX_FILTER_PROM;
                         __set_bit(ATH_STAT_PROMISC, sc->status);
                 } else {
                         __clear_bit(ATH_STAT_PROMISC, sc->status);
                 }
         }
 
+        if (test_bit(ATH_STAT_PROMISC, sc->status))
+                rfilt |= AR5K_RX_FILTER_PROM;
+
         /* Note, AR5K_RX_FILTER_MCAST is already enabled */
         if (*new_flags & FIF_ALLMULTI) {
                 mfilt[0] =  ~0;
diff --git a/drivers/net/wireless/ath/ath5k/phy.c b/drivers/net/wireless/ath/ath5k/phy.c
index 1b81c4778800..492cbb15720d 100644
--- a/drivers/net/wireless/ath/ath5k/phy.c
+++ b/drivers/net/wireless/ath/ath5k/phy.c
@@ -1814,6 +1814,13 @@ ath5k_hw_set_antenna_mode(struct ath5k_hw *ah, u8 ant_mode)
         u8 def_ant, tx_ant, ee_mode;
         u32 sta_id1 = 0;
 
+        /* if channel is not initialized yet we can't set the antennas
+         * so just store the mode. it will be set on the next reset */
+        if (channel == NULL) {
+                ah->ah_ant_mode = ant_mode;
+                return;
+        }
+
         def_ant = ah->ah_def_ant;
 
         ATH5K_TRACE(ah->ah_sc);
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 3db19172b43b..859aa4ab0769 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -1198,7 +1198,7 @@ void ath_drain_all_txq(struct ath_softc *sc, bool retry_tx)
                 int r;
 
                 ath_print(common, ATH_DBG_FATAL,
-                          "Unable to stop TxDMA. Reset HAL!\n");
+                          "Failed to stop TX DMA. Resetting hardware!\n");
 
                 spin_lock_bh(&sc->sc_resetlock);
                 r = ath9k_hw_reset(ah, sc->sc_ah->curchan, false);
@@ -1728,6 +1728,8 @@ static int ath_tx_setup_buffer(struct ieee80211_hw *hw, struct ath_buf *bf,
         } else
                 bf->bf_isnullfunc = false;
 
+        bf->bf_tx_aborted = false;
+
         return 0;
 }
 
@@ -1989,7 +1991,7 @@ static int ath_tx_num_badfrms(struct ath_softc *sc, struct ath_buf *bf,
         int nbad = 0;
         int isaggr = 0;
 
-        if (bf->bf_tx_aborted)
+        if (bf->bf_lastbf->bf_tx_aborted)
                 return 0;
 
         isaggr = bf_isaggr(bf);
diff --git a/drivers/net/wireless/hostap/hostap_hw.c b/drivers/net/wireless/hostap/hostap_hw.c
index d70732819423..ff9b5c882184 100644
--- a/drivers/net/wireless/hostap/hostap_hw.c
+++ b/drivers/net/wireless/hostap/hostap_hw.c
@@ -2618,15 +2618,6 @@ static irqreturn_t prism2_interrupt(int irq, void *dev_id)
         int events = 0;
         u16 ev;
 
-        /* Detect early interrupt before driver is fully configued */
-        if (!dev->base_addr) {
-                if (net_ratelimit()) {
-                        printk(KERN_DEBUG "%s: Interrupt, but dev not configured\n",
-                               dev->name);
-                }
-                return IRQ_HANDLED;
-        }
-
         iface = netdev_priv(dev);
         local = iface->local;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945.c b/drivers/net/wireless/iwlwifi/iwl-3945.c
index 068f7f8435c5..c44a303e62ed 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945.c
+++ b/drivers/net/wireless/iwlwifi/iwl-3945.c
@@ -2852,6 +2852,7 @@ static struct iwl_lib_ops iwl3945_lib = {
         .isr = iwl_isr_legacy,
         .config_ap = iwl3945_config_ap,
         .manage_ibss_station = iwl3945_manage_ibss_station,
+        .recover_from_tx_stall = iwl_bg_monitor_recover,
         .check_plcp_health = iwl3945_good_plcp_health,
 
         .debugfs_ops = {
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c b/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
index 1004cfc403b1..0f292a210ed9 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
@@ -1119,10 +1119,9 @@ static int iwl_get_single_channel_for_scan(struct iwl_priv *priv,
                                            struct iwl_scan_channel *scan_ch)
 {
         const struct ieee80211_supported_band *sband;
-        const struct iwl_channel_info *ch_info;
         u16 passive_dwell = 0;
         u16 active_dwell = 0;
-        int i, added = 0;
+        int added = 0;
         u16 channel = 0;
 
         sband = iwl_get_hw_mode(priv, band);
@@ -1137,32 +1136,7 @@ static int iwl_get_single_channel_for_scan(struct iwl_priv *priv,
         if (passive_dwell <= active_dwell)
                 passive_dwell = active_dwell + 1;
 
-        /* only scan single channel, good enough to reset the RF */
-        /* pick the first valid not in-use channel */
-        if (band == IEEE80211_BAND_5GHZ) {
-                for (i = 14; i < priv->channel_count; i++) {
-                        if (priv->channel_info[i].channel !=
-                            le16_to_cpu(priv->staging_rxon.channel)) {
-                                channel = priv->channel_info[i].channel;
-                                ch_info = iwl_get_channel_info(priv,
-                                        band, channel);
-                                if (is_channel_valid(ch_info))
-                                        break;
-                        }
-                }
-        } else {
-                for (i = 0; i < 14; i++) {
-                        if (priv->channel_info[i].channel !=
-                            le16_to_cpu(priv->staging_rxon.channel)) {
-                                        channel =
-                                                priv->channel_info[i].channel;
-                                        ch_info = iwl_get_channel_info(priv,
-                                                band, channel);
-                                        if (is_channel_valid(ch_info))
-                                                break;
-                        }
-                }
-        }
+        channel = iwl_get_single_channel_number(priv, band);
         if (channel) {
                 scan_ch->channel = cpu_to_le16(channel);
                 scan_ch->type = SCAN_CHANNEL_TYPE_PASSIVE;
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
index c402bfc83f36..a732f1094e5d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
@@ -1125,6 +1125,7 @@ static void iwlagn_tx_status(struct iwl_priv *priv, struct sk_buff *skb)
         struct ieee80211_sta *sta;
         struct iwl_station_priv *sta_priv;
 
+        rcu_read_lock();
         sta = ieee80211_find_sta(priv->vif, hdr->addr1);
         if (sta) {
                 sta_priv = (void *)sta->drv_priv;
@@ -1133,6 +1134,7 @@ static void iwlagn_tx_status(struct iwl_priv *priv, struct sk_buff *skb)
                     atomic_dec_return(&sta_priv->pending_frames) == 0)
                         ieee80211_sta_block_awake(priv->hw, sta, false);
         }
+        rcu_read_unlock();
 
         ieee80211_tx_status_irqsafe(priv->hw, skb);
 }
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.c b/drivers/net/wireless/iwlwifi/iwl-agn.c
index aef4f71f1981..7726e67044c0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn.c
@@ -1484,6 +1484,156 @@ bool iwl_good_ack_health(struct iwl_priv *priv,
 }
 
 
+/*****************************************************************************
+ *
+ * sysfs attributes
+ *
+ *****************************************************************************/
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+
+/*
+ * The following adds a new attribute to the sysfs representation
+ * of this device driver (i.e. a new file in /sys/class/net/wlan0/device/)
+ * used for controlling the debug level.
+ *
+ * See the level definitions in iwl for details.
+ *
+ * The debug_level being managed using sysfs below is a per device debug
+ * level that is used instead of the global debug level if it (the per
+ * device debug level) is set.
+ */
+static ssize_t show_debug_level(struct device *d,
+                                struct device_attribute *attr, char *buf)
+{
+        struct iwl_priv *priv = dev_get_drvdata(d);
+        return sprintf(buf, "0x%08X\n", iwl_get_debug_level(priv));
+}
+static ssize_t store_debug_level(struct device *d,
+                                struct device_attribute *attr,
+                                 const char *buf, size_t count)
+{
+        struct iwl_priv *priv = dev_get_drvdata(d);
+        unsigned long val;
+        int ret;
+
+        ret = strict_strtoul(buf, 0, &val);
+        if (ret)
+                IWL_ERR(priv, "%s is not in hex or decimal form.\n", buf);
+        else {
+                priv->debug_level = val;
+                if (iwl_alloc_traffic_mem(priv))
+                        IWL_ERR(priv,
+                                "Not enough memory to generate traffic log\n");
+        }
+        return strnlen(buf, count);
+}
+
+static DEVICE_ATTR(debug_level, S_IWUSR | S_IRUGO,
+                        show_debug_level, store_debug_level);
+
+
+#endif /* CONFIG_IWLWIFI_DEBUG */
+
+
+static ssize_t show_temperature(struct device *d,
+                                struct device_attribute *attr, char *buf)
+{
+        struct iwl_priv *priv = dev_get_drvdata(d);
+
+        if (!iwl_is_alive(priv))
+                return -EAGAIN;
+
+        return sprintf(buf, "%d\n", priv->temperature);
+}
+
+static DEVICE_ATTR(temperature, S_IRUGO, show_temperature, NULL);
+
+static ssize_t show_tx_power(struct device *d,
+                             struct device_attribute *attr, char *buf)
+{
+        struct iwl_priv *priv = dev_get_drvdata(d);
+
+        if (!iwl_is_ready_rf(priv))
+                return sprintf(buf, "off\n");
+        else
+                return sprintf(buf, "%d\n", priv->tx_power_user_lmt);
+}
+
+static ssize_t store_tx_power(struct device *d,
+                              struct device_attribute *attr,
+                              const char *buf, size_t count)
+{
+        struct iwl_priv *priv = dev_get_drvdata(d);
+        unsigned long val;
+        int ret;
+
+        ret = strict_strtoul(buf, 10, &val);
+        if (ret)
+                IWL_INFO(priv, "%s is not in decimal form.\n", buf);
+        else {
+                ret = iwl_set_tx_power(priv, val, false);
+                if (ret)
+                        IWL_ERR(priv, "failed setting tx power (0x%d).\n",
+                                ret);
+                else
+                        ret = count;
+        }
+        return ret;
+}
+
+static DEVICE_ATTR(tx_power, S_IWUSR | S_IRUGO, show_tx_power, store_tx_power);
+
+static ssize_t show_rts_ht_protection(struct device *d,
+                             struct device_attribute *attr, char *buf)
+{
+        struct iwl_priv *priv = dev_get_drvdata(d);
+
+        return sprintf(buf, "%s\n",
+                priv->cfg->use_rts_for_ht ? "RTS/CTS" : "CTS-to-self");
+}
+
+static ssize_t store_rts_ht_protection(struct device *d,
+                              struct device_attribute *attr,
+                              const char *buf, size_t count)
+{
+        struct iwl_priv *priv = dev_get_drvdata(d);
+        unsigned long val;
+        int ret;
+
+        ret = strict_strtoul(buf, 10, &val);
+        if (ret)
+                IWL_INFO(priv, "Input is not in decimal form.\n");
+        else {
+                if (!iwl_is_associated(priv))
+                        priv->cfg->use_rts_for_ht = val ? true : false;
+                else
+                        IWL_ERR(priv, "Sta associated with AP - "
+                                "Change protection mechanism is not allowed\n");
+                ret = count;
+        }
+        return ret;
+}
+
+static DEVICE_ATTR(rts_ht_protection, S_IWUSR | S_IRUGO,
+                        show_rts_ht_protection, store_rts_ht_protection);
+
+
+static struct attribute *iwl_sysfs_entries[] = {
+        &dev_attr_temperature.attr,
+        &dev_attr_tx_power.attr,
+        &dev_attr_rts_ht_protection.attr,
+#ifdef CONFIG_IWLWIFI_DEBUG
+        &dev_attr_debug_level.attr,
+#endif
+        NULL
+};
+
+static struct attribute_group iwl_attribute_group = {
+        .name = NULL,           /* put in device directory */
+        .attrs = iwl_sysfs_entries,
+};
+
 /******************************************************************************
  *
  * uCode download functions
@@ -1965,6 +2115,13 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
         if (err)
                 IWL_ERR(priv, "failed to create debugfs files. Ignoring error: %d\n", err);
 
+        err = sysfs_create_group(&priv->pci_dev->dev.kobj,
+                                        &iwl_attribute_group);
+        if (err) {
+                IWL_ERR(priv, "failed to create sysfs device attributes\n");
+                goto out_unbind;
+        }
+
         /* We have our copies now, allow OS release its copies */
         release_firmware(ucode_raw);
         complete(&priv->_agn.firmware_loading_complete);
@@ -3264,141 +3421,6 @@ static int iwlagn_mac_sta_add(struct ieee80211_hw *hw,
 
 /*****************************************************************************
  *
- * sysfs attributes
- *
- *****************************************************************************/
-
-#ifdef CONFIG_IWLWIFI_DEBUG
-
-/*
- * The following adds a new attribute to the sysfs representation
- * of this device driver (i.e. a new file in /sys/class/net/wlan0/device/)
- * used for controlling the debug level.
- *
- * See the level definitions in iwl for details.
- *
- * The debug_level being managed using sysfs below is a per device debug
- * level that is used instead of the global debug level if it (the per
- * device debug level) is set.
- */
-static ssize_t show_debug_level(struct device *d,
-                                struct device_attribute *attr, char *buf)
-{
-        struct iwl_priv *priv = dev_get_drvdata(d);
-        return sprintf(buf, "0x%08X\n", iwl_get_debug_level(priv));
-}
-static ssize_t store_debug_level(struct device *d,
-                                struct device_attribute *attr,
-                                 const char *buf, size_t count)
-{
-        struct iwl_priv *priv = dev_get_drvdata(d);
-        unsigned long val;
-        int ret;
-
-        ret = strict_strtoul(buf, 0, &val);
-        if (ret)
-                IWL_ERR(priv, "%s is not in hex or decimal form.\n", buf);
-        else {
-                priv->debug_level = val;
-                if (iwl_alloc_traffic_mem(priv))
-                        IWL_ERR(priv,
-                                "Not enough memory to generate traffic log\n");
-        }
-        return strnlen(buf, count);
-}
-
-static DEVICE_ATTR(debug_level, S_IWUSR | S_IRUGO,
-                        show_debug_level, store_debug_level);
-
-
-#endif /* CONFIG_IWLWIFI_DEBUG */
-
-
-static ssize_t show_temperature(struct device *d,
-                                struct device_attribute *attr, char *buf)
-{
-        struct iwl_priv *priv = dev_get_drvdata(d);
-
-        if (!iwl_is_alive(priv))
-                return -EAGAIN;
-
-        return sprintf(buf, "%d\n", priv->temperature);
-}
-
-static DEVICE_ATTR(temperature, S_IRUGO, show_temperature, NULL);
-
-static ssize_t show_tx_power(struct device *d,
-                             struct device_attribute *attr, char *buf)
-{
-        struct iwl_priv *priv = dev_get_drvdata(d);
-
-        if (!iwl_is_ready_rf(priv))
-                return sprintf(buf, "off\n");
-        else
-                return sprintf(buf, "%d\n", priv->tx_power_user_lmt);
-}
-
-static ssize_t store_tx_power(struct device *d,
-                              struct device_attribute *attr,
-                              const char *buf, size_t count)
-{
-        struct iwl_priv *priv = dev_get_drvdata(d);
-        unsigned long val;
-        int ret;
-
-        ret = strict_strtoul(buf, 10, &val);
-        if (ret)
-                IWL_INFO(priv, "%s is not in decimal form.\n", buf);
-        else {
-                ret = iwl_set_tx_power(priv, val, false);
-                if (ret)
-                        IWL_ERR(priv, "failed setting tx power (0x%d).\n",
-                                ret);
-                else
-                        ret = count;
-        }
-        return ret;
-}
-
-static DEVICE_ATTR(tx_power, S_IWUSR | S_IRUGO, show_tx_power, store_tx_power);
-
-static ssize_t show_rts_ht_protection(struct device *d,
-                             struct device_attribute *attr, char *buf)
-{
-        struct iwl_priv *priv = dev_get_drvdata(d);
-
-        return sprintf(buf, "%s\n",
-                priv->cfg->use_rts_for_ht ? "RTS/CTS" : "CTS-to-self");
-}
-
-static ssize_t store_rts_ht_protection(struct device *d,
-                              struct device_attribute *attr,
-                              const char *buf, size_t count)
-{
-        struct iwl_priv *priv = dev_get_drvdata(d);
-        unsigned long val;
-        int ret;
-
-        ret = strict_strtoul(buf, 10, &val);
-        if (ret)
-                IWL_INFO(priv, "Input is not in decimal form.\n");
-        else {
-                if (!iwl_is_associated(priv))
-                        priv->cfg->use_rts_for_ht = val ? true : false;
-                else
-                        IWL_ERR(priv, "Sta associated with AP - "
-                                "Change protection mechanism is not allowed\n");
-                ret = count;
-        }
-        return ret;
-}
-
-static DEVICE_ATTR(rts_ht_protection, S_IWUSR | S_IRUGO,
-                        show_rts_ht_protection, store_rts_ht_protection);
-
-
-/*****************************************************************************
- *
  * driver setup and teardown
  *
  *****************************************************************************/
@@ -3550,21 +3572,6 @@ static void iwl_uninit_drv(struct iwl_priv *priv)
         kfree(priv->scan_cmd);
 }
 
-static struct attribute *iwl_sysfs_entries[] = {
-        &dev_attr_temperature.attr,
-        &dev_attr_tx_power.attr,
-        &dev_attr_rts_ht_protection.attr,
-#ifdef CONFIG_IWLWIFI_DEBUG
-        &dev_attr_debug_level.attr,
-#endif
-        NULL
-};
-
-static struct attribute_group iwl_attribute_group = {
-        .name = NULL,           /* put in device directory */
-        .attrs = iwl_sysfs_entries,
-};
-
 static struct ieee80211_ops iwl_hw_ops = {
         .tx = iwl_mac_tx,
         .start = iwl_mac_start,
@@ -3750,11 +3757,6 @@ static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
                 IWL_ERR(priv, "Error allocating IRQ %d\n", priv->pci_dev->irq);
                 goto out_disable_msi;
         }
-        err = sysfs_create_group(&pdev->dev.kobj, &iwl_attribute_group);
-        if (err) {
-                IWL_ERR(priv, "failed to create sysfs device attributes\n");
-                goto out_free_irq;
-        }
 
         iwl_setup_deferred_work(priv);
         iwl_setup_rx_handlers(priv);
@@ -3788,15 +3790,13 @@ static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
         err = iwl_request_firmware(priv, true);
         if (err)
-                goto out_remove_sysfs;
+                goto out_destroy_workqueue;
 
         return 0;
 
- out_remove_sysfs:
+ out_destroy_workqueue:
         destroy_workqueue(priv->workqueue);
         priv->workqueue = NULL;
-        sysfs_remove_group(&pdev->dev.kobj, &iwl_attribute_group);
- out_free_irq:
         free_irq(priv->pci_dev->irq, priv);
         iwl_free_isr_ict(priv);
  out_disable_msi:
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.c b/drivers/net/wireless/iwlwifi/iwl-core.c
index 5a7eca8fb789..426e95567de3 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.c
+++ b/drivers/net/wireless/iwlwifi/iwl-core.c
@@ -854,6 +854,45 @@ void iwl_set_rxon_chain(struct iwl_priv *priv)
 }
 EXPORT_SYMBOL(iwl_set_rxon_chain);
 
+/* Return valid channel */
+u8 iwl_get_single_channel_number(struct iwl_priv *priv,
+                                  enum ieee80211_band band)
+{
+        const struct iwl_channel_info *ch_info;
+        int i;
+        u8 channel = 0;
+
+        /* only scan single channel, good enough to reset the RF */
+        /* pick the first valid not in-use channel */
+        if (band == IEEE80211_BAND_5GHZ) {
+                for (i = 14; i < priv->channel_count; i++) {
+                        if (priv->channel_info[i].channel !=
+                            le16_to_cpu(priv->staging_rxon.channel)) {
+                                channel = priv->channel_info[i].channel;
+                                ch_info = iwl_get_channel_info(priv,
+                                        band, channel);
+                                if (is_channel_valid(ch_info))
+                                        break;
+                        }
+                }
+        } else {
+                for (i = 0; i < 14; i++) {
+                        if (priv->channel_info[i].channel !=
+                            le16_to_cpu(priv->staging_rxon.channel)) {
+                                        channel =
+                                                priv->channel_info[i].channel;
+                                        ch_info = iwl_get_channel_info(priv,
+                                                band, channel);
+                                        if (is_channel_valid(ch_info))
+                                                break;
+                        }
+                }
+        }
+
+        return channel;
+}
+EXPORT_SYMBOL(iwl_get_single_channel_number);
+
 /**
  * iwl_set_rxon_channel - Set the phymode and channel values in staging RXON
  * @phymode: MODE_IEEE80211A sets to 5.2GHz; all else set to 2.4GHz
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.h b/drivers/net/wireless/iwlwifi/iwl-core.h
index 7e5a5ba41fd2..31775bd9c361 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.h
+++ b/drivers/net/wireless/iwlwifi/iwl-core.h
@@ -343,6 +343,8 @@ int iwl_check_rxon_cmd(struct iwl_priv *priv);
 int iwl_full_rxon_required(struct iwl_priv *priv);
 void iwl_set_rxon_chain(struct iwl_priv *priv);
 int iwl_set_rxon_channel(struct iwl_priv *priv, struct ieee80211_channel *ch);
+u8 iwl_get_single_channel_number(struct iwl_priv *priv,
+                                  enum ieee80211_band band);
 void iwl_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_config *ht_conf);
 u8 iwl_is_ht40_tx_allowed(struct iwl_priv *priv,
                          struct ieee80211_sta_ht_cap *sta_ht_inf);
diff --git a/drivers/net/wireless/iwlwifi/iwl3945-base.c b/drivers/net/wireless/iwlwifi/iwl3945-base.c
index 3e5bffb6034f..6c353cacc8d6 100644
--- a/drivers/net/wireless/iwlwifi/iwl3945-base.c
+++ b/drivers/net/wireless/iwlwifi/iwl3945-base.c
@@ -1844,6 +1844,49 @@ static void iwl3945_irq_tasklet(struct iwl_priv *priv)
 #endif
 }
 
+static int iwl3945_get_single_channel_for_scan(struct iwl_priv *priv,
+                                               struct ieee80211_vif *vif,
+                                               enum ieee80211_band band,
+                                               struct iwl3945_scan_channel *scan_ch)
+{
+        const struct ieee80211_supported_band *sband;
+        u16 passive_dwell = 0;
+        u16 active_dwell = 0;
+        int added = 0;
+        u8 channel = 0;
+
+        sband = iwl_get_hw_mode(priv, band);
+        if (!sband) {
+                IWL_ERR(priv, "invalid band\n");
+                return added;
+        }
+
+        active_dwell = iwl_get_active_dwell_time(priv, band, 0);
+        passive_dwell = iwl_get_passive_dwell_time(priv, band, vif);
+
+        if (passive_dwell <= active_dwell)
+                passive_dwell = active_dwell + 1;
+
+
+        channel = iwl_get_single_channel_number(priv, band);
+
+        if (channel) {
+                scan_ch->channel = channel;
+                scan_ch->type = 0;      /* passive */
+                scan_ch->active_dwell = cpu_to_le16(active_dwell);
+                scan_ch->passive_dwell = cpu_to_le16(passive_dwell);
+                /* Set txpower levels to defaults */
+                scan_ch->tpc.dsp_atten = 110;
+                if (band == IEEE80211_BAND_5GHZ)
+                        scan_ch->tpc.tx_gain = ((1 << 5) | (3 << 3)) | 3;
+                else
+                        scan_ch->tpc.tx_gain = ((1 << 5) | (5 << 3));
+                added++;
+        } else
+                IWL_ERR(priv, "no valid channel found\n");
+        return added;
+}
+
 static int iwl3945_get_channels_for_scan(struct iwl_priv *priv,
                                          enum ieee80211_band band,
                                      u8 is_active, u8 n_probes,
@@ -2992,9 +3035,16 @@ void iwl3945_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
         /* select Rx antennas */
         scan->flags |= iwl3945_get_antenna_flags(priv);
 
-        scan->channel_count =
-                iwl3945_get_channels_for_scan(priv, band, is_active, n_probes,
-                        (void *)&scan->data[le16_to_cpu(scan->tx_cmd.len)], vif);
+        if (priv->is_internal_short_scan) {
+                scan->channel_count =
+                        iwl3945_get_single_channel_for_scan(priv, vif, band,
+                                (void *)&scan->data[le16_to_cpu(
+                                scan->tx_cmd.len)]);
+        } else {
+                scan->channel_count =
+                        iwl3945_get_channels_for_scan(priv, band, is_active, n_probes,
+                                (void *)&scan->data[le16_to_cpu(scan->tx_cmd.len)], vif);
+        }
 
         if (scan->channel_count == 0) {
                 IWL_DEBUG_SCAN(priv, "channel count %d\n", scan->channel_count);
diff --git a/drivers/net/wireless/libertas/rx.c b/drivers/net/wireless/libertas/rx.c
index a115bfa9513a..7a377f5b7662 100644
--- a/drivers/net/wireless/libertas/rx.c
+++ b/drivers/net/wireless/libertas/rx.c
@@ -329,9 +329,8 @@ static int process_rxed_802_11_packet(struct lbs_private *priv,
         /* create the exported radio header */
 
         /* radiotap header */
-        radiotap_hdr.hdr.it_version = 0;
-        /* XXX must check this value for pad */
-        radiotap_hdr.hdr.it_pad = 0;
+        memset(&radiotap_hdr, 0, sizeof(radiotap_hdr));
+        /* XXX must check radiotap_hdr.hdr.it_pad for pad */
         radiotap_hdr.hdr.it_len = cpu_to_le16 (sizeof(struct rx_radiotap_hdr));
         radiotap_hdr.hdr.it_present = cpu_to_le32 (RX_RADIOTAP_PRESENT);
         radiotap_hdr.rate = convert_mv_rate_to_radiotap(prxpd->rx_rate);
diff --git a/drivers/net/wireless/orinoco/airport.c b/drivers/net/wireless/orinoco/airport.c
index 9bcee10c9308..4a0a0e5265c9 100644
--- a/drivers/net/wireless/orinoco/airport.c
+++ b/drivers/net/wireless/orinoco/airport.c
@@ -239,8 +239,11 @@ static struct of_device_id airport_match[] =
 MODULE_DEVICE_TABLE(of, airport_match);
 
 static struct macio_driver airport_driver = {
-        .name           = DRIVER_NAME,
-        .match_table    = airport_match,
+        .driver = {
+                .name           = DRIVER_NAME,
+                .owner          = THIS_MODULE,
+                .of_match_table = airport_match,
+        },
         .probe          = airport_attach,
         .remove         = airport_detach,
         .suspend        = airport_suspend,
diff --git a/drivers/net/wireless/p54/p54usb.c b/drivers/net/wireless/p54/p54usb.c
index d5b197b4d5bb..73073259f508 100644
--- a/drivers/net/wireless/p54/p54usb.c
+++ b/drivers/net/wireless/p54/p54usb.c
@@ -80,6 +80,7 @@ static struct usb_device_id p54u_table[] __devinitdata = {
         {USB_DEVICE(0x1413, 0x5400)},   /* Telsey 802.11g USB2.0 Adapter */
         {USB_DEVICE(0x1435, 0x0427)},   /* Inventel UR054G */
         {USB_DEVICE(0x2001, 0x3704)},   /* DLink DWL-G122 rev A2 */
+        {USB_DEVICE(0x413c, 0x5513)},   /* Dell WLA3310 USB Wireless Adapter */
         {USB_DEVICE(0x413c, 0x8102)},   /* Spinnaker DUT */
         {USB_DEVICE(0x413c, 0x8104)},   /* Cohiba Proto board */
         {}
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index 699161327d65..0f8b84b7224c 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -413,7 +413,7 @@ static void rt2800usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
          */
         rt2x00_desc_read(txi, 0, &word);
         rt2x00_set_field32(&word, TXINFO_W0_USB_DMA_TX_PKT_LEN,
-                           skb->len - TXINFO_DESC_SIZE);
+                           skb->len + TXWI_DESC_SIZE);
         rt2x00_set_field32(&word, TXINFO_W0_WIV,
                            !test_bit(ENTRY_TXD_ENCRYPT_IV, &txdesc->flags));
         rt2x00_set_field32(&word, TXINFO_W0_QSEL, 2);
diff --git a/drivers/net/wireless/wl12xx/wl1251_sdio.c b/drivers/net/wireless/wl12xx/wl1251_sdio.c
index d234285c2c81..c561332e7009 100644
--- a/drivers/net/wireless/wl12xx/wl1251_sdio.c
+++ b/drivers/net/wireless/wl12xx/wl1251_sdio.c
@@ -259,6 +259,7 @@ disable:
         sdio_disable_func(func);
 release:
         sdio_release_host(func);
+        wl1251_free_hw(wl);
         return ret;
 }
 
diff --git a/drivers/parport/parport_amiga.c b/drivers/parport/parport_amiga.c
index 1586e1caa2f5..8bef6d60f88b 100644
--- a/drivers/parport/parport_amiga.c
+++ b/drivers/parport/parport_amiga.c
@@ -18,6 +18,8 @@
 #include <linux/parport.h>
 #include <linux/ioport.h>
 #include <linux/interrupt.h>
+#include <linux/platform_device.h>
+
 #include <asm/setup.h>
 #include <asm/amigahw.h>
 #include <asm/irq.h>
@@ -31,7 +33,6 @@
 #define DPRINTK(x...)   do { } while (0)
 #endif
 
-static struct parport *this_port = NULL;
 
 static void amiga_write_data(struct parport *p, unsigned char data)
 {
@@ -227,18 +228,11 @@ static struct parport_operations pp_amiga_ops = {
 
 /* ----------- Initialisation code --------------------------------- */
 
-static int __init parport_amiga_init(void)
+static int __init amiga_parallel_probe(struct platform_device *pdev)
 {
         struct parport *p;
         int err;
 
-        if (!MACH_IS_AMIGA || !AMIGAHW_PRESENT(AMI_PARALLEL))
-                return -ENODEV;
-
-        err = -EBUSY;
-        if (!request_mem_region(CIAA_PHYSADDR-1+0x100, 0x100, "parallel"))
-                goto out_mem;
-
         ciaa.ddrb = 0xff;
         ciab.ddra &= 0xf8;
         mb();
@@ -246,41 +240,63 @@ static int __init parport_amiga_init(void)
         p = parport_register_port((unsigned long)&ciaa.prb, IRQ_AMIGA_CIAA_FLG,
                                    PARPORT_DMA_NONE, &pp_amiga_ops);
         if (!p)
-                goto out_port;
+                return -EBUSY;
 
-        err = request_irq(IRQ_AMIGA_CIAA_FLG, parport_irq_handler, 0, p->name, p);
+        err = request_irq(IRQ_AMIGA_CIAA_FLG, parport_irq_handler, 0, p->name,
+                          p);
         if (err)
                 goto out_irq;
 
-        this_port = p;
         printk(KERN_INFO "%s: Amiga built-in port using irq\n", p->name);
         /* XXX: set operating mode */
         parport_announce_port(p);
 
+        platform_set_drvdata(pdev, p);
+
         return 0;
 
 out_irq:
         parport_put_port(p);
-out_port:
-        release_mem_region(CIAA_PHYSADDR-1+0x100, 0x100);
-out_mem:
         return err;
 }
 
-static void __exit parport_amiga_exit(void)
+static int __exit amiga_parallel_remove(struct platform_device *pdev)
+{
+        struct parport *port = platform_get_drvdata(pdev);
+
+        parport_remove_port(port);
+        if (port->irq != PARPORT_IRQ_NONE)
+                free_irq(IRQ_AMIGA_CIAA_FLG, port);
+        parport_put_port(port);
+        platform_set_drvdata(pdev, NULL);
+        return 0;
+}
+
+static struct platform_driver amiga_parallel_driver = {
+        .remove = __exit_p(amiga_parallel_remove),
+        .driver   = {
+                .name   = "amiga-parallel",
+                .owner  = THIS_MODULE,
+        },
+};
+
+static int __init amiga_parallel_init(void)
+{
+        return platform_driver_probe(&amiga_parallel_driver,
+                                     amiga_parallel_probe);
+}
+
+module_init(amiga_parallel_init);
+
+static void __exit amiga_parallel_exit(void)
 {
-        parport_remove_port(this_port);
-        if (this_port->irq != PARPORT_IRQ_NONE)
-                free_irq(IRQ_AMIGA_CIAA_FLG, this_port);
-        parport_put_port(this_port);
-        release_mem_region(CIAA_PHYSADDR-1+0x100, 0x100);
+        platform_driver_unregister(&amiga_parallel_driver);
 }
 
+module_exit(amiga_parallel_exit);
 
 MODULE_AUTHOR("Joerg Dorchain <joerg@dorchain.net>");
 MODULE_DESCRIPTION("Parport Driver for Amiga builtin Port");
 MODULE_SUPPORTED_DEVICE("Amiga builtin Parallel Port");
 MODULE_LICENSE("GPL");
-
-module_init(parport_amiga_init)
-module_exit(parport_amiga_exit)
+MODULE_ALIAS("platform:amiga-parallel");
diff --git a/drivers/pci/hotplug/cpqphp_core.c b/drivers/pci/hotplug/cpqphp_core.c
index b3e5580c837b..4952c3b9379d 100644
--- a/drivers/pci/hotplug/cpqphp_core.c
+++ b/drivers/pci/hotplug/cpqphp_core.c
@@ -828,7 +828,14 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
                         pci_name(pdev), err);
                 return err;
         }
+
         bus = pdev->subordinate;
+        if (!bus) {
+                dev_notice(&pdev->dev, "the device is not a bridge, "
+                                "skipping\n");
+                rc = -ENODEV;
+                goto err_disable_device;
+        }
 
         /* Need to read VID early b/c it's used to differentiate CPQ and INTC
          * discovery
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index afd2fbf7d797..c9957f68ac9b 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -1035,39 +1035,6 @@ error:
         return retval;
 }
 
-static void pci_remove_slot_links(struct pci_dev *dev)
-{
-        char func[10];
-        struct pci_slot *slot;
-
-        sysfs_remove_link(&dev->dev.kobj, "slot");
-        list_for_each_entry(slot, &dev->bus->slots, list) {
-                if (slot->number != PCI_SLOT(dev->devfn))
-                        continue;
-                snprintf(func, 10, "function%d", PCI_FUNC(dev->devfn));
-                sysfs_remove_link(&slot->kobj, func);
-        }
-}
-
-static int pci_create_slot_links(struct pci_dev *dev)
-{
-        int result = 0;
-        char func[10];
-        struct pci_slot *slot;
-
-        list_for_each_entry(slot, &dev->bus->slots, list) {
-                if (slot->number != PCI_SLOT(dev->devfn))
-                        continue;
-                result = sysfs_create_link(&dev->dev.kobj, &slot->kobj, "slot");
-                if (result)
-                        goto out;
-                snprintf(func, 10, "function%d", PCI_FUNC(dev->devfn));
-                result = sysfs_create_link(&slot->kobj, &dev->dev.kobj, func);
-        }
-out:
-        return result;
-}
-
 int __must_check pci_create_sysfs_dev_files (struct pci_dev *pdev)
 {
         int retval;
@@ -1130,8 +1097,6 @@ int __must_check pci_create_sysfs_dev_files (struct pci_dev *pdev)
         if (retval)
                 goto err_vga_file;
 
-        pci_create_slot_links(pdev);
-
         return 0;
 
 err_vga_file:
@@ -1181,8 +1146,6 @@ void pci_remove_sysfs_dev_files(struct pci_dev *pdev)
         if (!sysfs_initialized)
                 return;
 
-        pci_remove_slot_links(pdev);
-
         pci_remove_capabilities_sysfs(pdev);
 
         if (pdev->cfg_size < PCI_CFG_SPACE_EXP_SIZE)
diff --git a/drivers/pci/pcie/aer/aerdrv.h b/drivers/pci/pcie/aer/aerdrv.h
index 7aaae2d2bd67..80c11d131499 100644
--- a/drivers/pci/pcie/aer/aerdrv.h
+++ b/drivers/pci/pcie/aer/aerdrv.h
@@ -130,4 +130,21 @@ static inline int aer_osc_setup(struct pcie_device *pciedev)
 }
 #endif
 
+#ifdef CONFIG_ACPI_APEI
+extern int pcie_aer_get_firmware_first(struct pci_dev *pci_dev);
+#else
+static inline int pcie_aer_get_firmware_first(struct pci_dev *pci_dev)
+{
+        if (pci_dev->__aer_firmware_first_valid)
+                return pci_dev->__aer_firmware_first;
+        return 0;
+}
+#endif
+
+static inline void pcie_aer_force_firmware_first(struct pci_dev *pci_dev,
+                                                 int enable)
+{
+        pci_dev->__aer_firmware_first = !!enable;
+        pci_dev->__aer_firmware_first_valid = 1;
+}
 #endif /* _AERDRV_H_ */
diff --git a/drivers/pci/pcie/aer/aerdrv_acpi.c b/drivers/pci/pcie/aer/aerdrv_acpi.c
index 04814087658d..f278d7b0d95d 100644
--- a/drivers/pci/pcie/aer/aerdrv_acpi.c
+++ b/drivers/pci/pcie/aer/aerdrv_acpi.c
@@ -16,6 +16,7 @@
 #include <linux/acpi.h>
 #include <linux/pci-acpi.h>
 #include <linux/delay.h>
+#include <acpi/apei.h>
 #include "aerdrv.h"
 
 /**
@@ -53,3 +54,79 @@ int aer_osc_setup(struct pcie_device *pciedev)
 
         return 0;
 }
+
+#ifdef CONFIG_ACPI_APEI
+static inline int hest_match_pci(struct acpi_hest_aer_common *p,
+                                 struct pci_dev *pci)
+{
+        return  (0           == pci_domain_nr(pci->bus) &&
+                 p->bus      == pci->bus->number &&
+                 p->device   == PCI_SLOT(pci->devfn) &&
+                 p->function == PCI_FUNC(pci->devfn));
+}
+
+struct aer_hest_parse_info {
+        struct pci_dev *pci_dev;
+        int firmware_first;
+};
+
+static int aer_hest_parse(struct acpi_hest_header *hest_hdr, void *data)
+{
+        struct aer_hest_parse_info *info = data;
+        struct acpi_hest_aer_common *p;
+        u8 pcie_type = 0;
+        u8 bridge = 0;
+        int ff = 0;
+
+        switch (hest_hdr->type) {
+        case ACPI_HEST_TYPE_AER_ROOT_PORT:
+                pcie_type = PCI_EXP_TYPE_ROOT_PORT;
+                break;
+        case ACPI_HEST_TYPE_AER_ENDPOINT:
+                pcie_type = PCI_EXP_TYPE_ENDPOINT;
+                break;
+        case ACPI_HEST_TYPE_AER_BRIDGE:
+                if ((info->pci_dev->class >> 16) == PCI_BASE_CLASS_BRIDGE)
+                        bridge = 1;
+                break;
+        default:
+                return 0;
+        }
+
+        p = (struct acpi_hest_aer_common *)(hest_hdr + 1);
+        if (p->flags & ACPI_HEST_GLOBAL) {
+                if ((info->pci_dev->is_pcie &&
+                     info->pci_dev->pcie_type == pcie_type) || bridge)
+                        ff = !!(p->flags & ACPI_HEST_FIRMWARE_FIRST);
+        } else
+                if (hest_match_pci(p, info->pci_dev))
+                        ff = !!(p->flags & ACPI_HEST_FIRMWARE_FIRST);
+        info->firmware_first = ff;
+
+        return 0;
+}
+
+static void aer_set_firmware_first(struct pci_dev *pci_dev)
+{
+        int rc;
+        struct aer_hest_parse_info info = {
+                .pci_dev        = pci_dev,
+                .firmware_first = 0,
+        };
+
+        rc = apei_hest_parse(aer_hest_parse, &info);
+
+        if (rc)
+                pci_dev->__aer_firmware_first = 0;
+        else
+                pci_dev->__aer_firmware_first = info.firmware_first;
+        pci_dev->__aer_firmware_first_valid = 1;
+}
+
+int pcie_aer_get_firmware_first(struct pci_dev *dev)
+{
+        if (!dev->__aer_firmware_first_valid)
+                aer_set_firmware_first(dev);
+        return dev->__aer_firmware_first;
+}
+#endif
diff --git a/drivers/pci/pcie/aer/aerdrv_core.c b/drivers/pci/pcie/aer/aerdrv_core.c
index df2d686fe3dd..8af4f619bba2 100644
--- a/drivers/pci/pcie/aer/aerdrv_core.c
+++ b/drivers/pci/pcie/aer/aerdrv_core.c
@@ -36,7 +36,7 @@ int pci_enable_pcie_error_reporting(struct pci_dev *dev)
         u16 reg16 = 0;
         int pos;
 
-        if (dev->aer_firmware_first)
+        if (pcie_aer_get_firmware_first(dev))
                 return -EIO;
 
         pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
@@ -63,7 +63,7 @@ int pci_disable_pcie_error_reporting(struct pci_dev *dev)
         u16 reg16 = 0;
         int pos;
 
-        if (dev->aer_firmware_first)
+        if (pcie_aer_get_firmware_first(dev))
                 return -EIO;
 
         pos = pci_pcie_cap(dev);
@@ -771,7 +771,7 @@ void aer_isr(struct work_struct *work)
  */
 int aer_init(struct pcie_device *dev)
 {
-        if (dev->port->aer_firmware_first) {
+        if (pcie_aer_get_firmware_first(dev->port)) {
                 dev_printk(KERN_DEBUG, &dev->device,
                            "PCIe errors handled by platform firmware.\n");
                 goto out;
@@ -785,7 +785,7 @@ out:
         if (forceload) {
                 dev_printk(KERN_DEBUG, &dev->device,
                            "aerdrv forceload requested.\n");
-                dev->port->aer_firmware_first = 0;
+                pcie_aer_force_firmware_first(dev->port, 0);
                 return 0;
         }
         return -ENXIO;
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index c82548afcd5c..f4adba2d1dd3 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -10,7 +10,6 @@
 #include <linux/module.h>
 #include <linux/cpumask.h>
 #include <linux/pci-aspm.h>
-#include <acpi/acpi_hest.h>
 #include "pci.h"
 
 #define CARDBUS_LATENCY_TIMER   176     /* secondary latency timer */
@@ -904,12 +903,6 @@ void set_pcie_hotplug_bridge(struct pci_dev *pdev)
                 pdev->is_hotplug_bridge = 1;
 }
 
-static void set_pci_aer_firmware_first(struct pci_dev *pdev)
-{
-        if (acpi_hest_firmware_first_pci(pdev))
-                pdev->aer_firmware_first = 1;
-}
-
 #define LEGACY_IO_RESOURCE      (IORESOURCE_IO | IORESOURCE_PCI_FIXED)
 
 /**
@@ -939,7 +932,6 @@ int pci_setup_device(struct pci_dev *dev)
         dev->multifunction = !!(hdr_type & 0x80);
         dev->error_state = pci_channel_io_normal;
         set_pcie_port_type(dev);
-        set_pci_aer_firmware_first(dev);
 
         list_for_each_entry(slot, &dev->bus->slots, list)
                 if (PCI_SLOT(dev->devfn) == slot->number)
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index b7512cf08c58..477345d41641 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -1457,7 +1457,8 @@ static void quirk_jmicron_ata(struct pci_dev *pdev)
         conf5 &= ~(1 << 24);  /* Clear bit 24 */
 
         switch (pdev->device) {
-        case PCI_DEVICE_ID_JMICRON_JMB360:
+        case PCI_DEVICE_ID_JMICRON_JMB360: /* SATA single port */
+        case PCI_DEVICE_ID_JMICRON_JMB362: /* SATA dual ports */
                 /* The controller should be in single function ahci mode */
                 conf1 |= 0x0002A100; /* Set 8, 13, 15, 17 */
                 break;
@@ -1493,12 +1494,14 @@ static void quirk_jmicron_ata(struct pci_dev *pdev)
 }
 DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB360, quirk_jmicron_ata);
 DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB361, quirk_jmicron_ata);
+DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB362, quirk_jmicron_ata);
 DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB363, quirk_jmicron_ata);
 DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB365, quirk_jmicron_ata);
 DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB366, quirk_jmicron_ata);
 DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB368, quirk_jmicron_ata);
 DECLARE_PCI_FIXUP_RESUME_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB360, quirk_jmicron_ata);
 DECLARE_PCI_FIXUP_RESUME_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB361, quirk_jmicron_ata);
+DECLARE_PCI_FIXUP_RESUME_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB362, quirk_jmicron_ata);
 DECLARE_PCI_FIXUP_RESUME_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB363, quirk_jmicron_ata);
 DECLARE_PCI_FIXUP_RESUME_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB365, quirk_jmicron_ata);
 DECLARE_PCI_FIXUP_RESUME_EARLY(PCI_VENDOR_ID_JMICRON, PCI_DEVICE_ID_JMICRON_JMB366, quirk_jmicron_ata);
diff --git a/drivers/pci/setup-res.c b/drivers/pci/setup-res.c
index 17bed18d24ad..92379e2d37e7 100644
--- a/drivers/pci/setup-res.c
+++ b/drivers/pci/setup-res.c
@@ -97,16 +97,16 @@ int pci_claim_resource(struct pci_dev *dev, int resource)
 
         root = pci_find_parent_resource(dev, res);
         if (!root) {
-                dev_err(&dev->dev, "no compatible bridge window for %pR\n",
-                        res);
+                dev_info(&dev->dev, "no compatible bridge window for %pR\n",
+                         res);
                 return -EINVAL;
         }
 
         conflict = request_resource_conflict(root, res);
         if (conflict) {
-                dev_err(&dev->dev,
-                        "address space collision: %pR conflicts with %s %pR\n",
-                        res, conflict->name, conflict);
+                dev_info(&dev->dev,
+                         "address space collision: %pR conflicts with %s %pR\n",
+                         res, conflict->name, conflict);
                 return -EBUSY;
         }
 
diff --git a/drivers/pci/slot.c b/drivers/pci/slot.c
index e0189cf7c558..659eaa0fc48f 100644
--- a/drivers/pci/slot.c
+++ b/drivers/pci/slot.c
@@ -97,50 +97,6 @@ static ssize_t cur_speed_read_file(struct pci_slot *slot, char *buf)
         return bus_speed_read(slot->bus->cur_bus_speed, buf);
 }
 
-static void remove_sysfs_files(struct pci_slot *slot)
-{
-        char func[10];
-        struct list_head *tmp;
-
-        list_for_each(tmp, &slot->bus->devices) {
-                struct pci_dev *dev = pci_dev_b(tmp);
-                if (PCI_SLOT(dev->devfn) != slot->number)
-                        continue;
-                sysfs_remove_link(&dev->dev.kobj, "slot");
-
-                snprintf(func, 10, "function%d", PCI_FUNC(dev->devfn));
-                sysfs_remove_link(&slot->kobj, func);
-        }
-}
-
-static int create_sysfs_files(struct pci_slot *slot)
-{
-        int result;
-        char func[10];
-        struct list_head *tmp;
-
-        list_for_each(tmp, &slot->bus->devices) {
-                struct pci_dev *dev = pci_dev_b(tmp);
-                if (PCI_SLOT(dev->devfn) != slot->number)
-                        continue;
-
-                result = sysfs_create_link(&dev->dev.kobj, &slot->kobj, "slot");
-                if (result)
-                        goto fail;
-
-                snprintf(func, 10, "function%d", PCI_FUNC(dev->devfn));
-                result = sysfs_create_link(&slot->kobj, &dev->dev.kobj, func);
-                if (result)
-                        goto fail;
-        }
-
-        return 0;
-
-fail:
-        remove_sysfs_files(slot);
-        return result;
-}
-
 static void pci_slot_release(struct kobject *kobj)
 {
         struct pci_dev *dev;
@@ -153,8 +109,6 @@ static void pci_slot_release(struct kobject *kobj)
                 if (PCI_SLOT(dev->devfn) == slot->number)
                         dev->slot = NULL;
 
-        remove_sysfs_files(slot);
-
         list_del(&slot->list);
 
         kfree(slot);
@@ -346,8 +300,6 @@ placeholder:
         INIT_LIST_HEAD(&slot->list);
         list_add(&slot->list, &parent->slots);
 
-        create_sysfs_files(slot);
-
         list_for_each_entry(dev, &parent->devices, bus_list)
                 if (PCI_SLOT(dev->devfn) == slot_nr)
                         dev->slot = slot;
diff --git a/drivers/pcmcia/ds.c b/drivers/pcmcia/ds.c
index 7ef7adee5e4f..9fc339845538 100644
--- a/drivers/pcmcia/ds.c
+++ b/drivers/pcmcia/ds.c
@@ -671,6 +671,7 @@ static void pcmcia_requery(struct pcmcia_socket *s)
                 if (old_funcs != new_funcs) {
                         /* we need to re-start */
                         pcmcia_card_remove(s, NULL);
+                        s->functions = 0;
                         pcmcia_card_add(s);
                 }
         }
diff --git a/drivers/pcmcia/m8xx_pcmcia.c b/drivers/pcmcia/m8xx_pcmcia.c
index 1a648b90b634..25e5e30a18af 100644
--- a/drivers/pcmcia/m8xx_pcmcia.c
+++ b/drivers/pcmcia/m8xx_pcmcia.c
@@ -1157,7 +1157,7 @@ static int __init m8xx_probe(struct of_device *ofdev,
         unsigned int i, m, hwirq;
         pcmconf8xx_t *pcmcia;
         int status;
-        struct device_node *np = ofdev->node;
+        struct device_node *np = ofdev->dev.of_node;
 
         pcmcia_info("%s\n", version);
 
@@ -1301,7 +1301,7 @@ static struct of_platform_driver m8xx_pcmcia_driver = {
         .driver = {
                 .name = driver_name,
                 .owner = THIS_MODULE,
-                .match_table = m8xx_pcmcia_match,
+                .of_match_table = m8xx_pcmcia_match,
         },
         .probe = m8xx_probe,
         .remove = m8xx_remove,
diff --git a/drivers/pcmcia/yenta_socket.c b/drivers/pcmcia/yenta_socket.c
index 424e576f3acb..f1d41374eea7 100644
--- a/drivers/pcmcia/yenta_socket.c
+++ b/drivers/pcmcia/yenta_socket.c
@@ -880,6 +880,12 @@ static struct cardbus_type cardbus_type[] = {
                 .restore_state  = ti_restore_state,
                 .sock_init      = ti_init,
         },
+        [CARDBUS_TYPE_ENE]      = {
+                .override       = ene_override,
+                .save_state     = ti_save_state,
+                .restore_state  = ti_restore_state,
+                .sock_init      = ti_init,
+        },
 #endif
 #ifdef CONFIG_YENTA_RICOH
         [CARDBUS_TYPE_RICOH]    = {
@@ -902,14 +908,6 @@ static struct cardbus_type cardbus_type[] = {
                 .restore_state  = o2micro_restore_state,
         },
 #endif
-#ifdef CONFIG_YENTA_TI
-        [CARDBUS_TYPE_ENE]      = {
-                .override       = ene_override,
-                .save_state     = ti_save_state,
-                .restore_state  = ti_restore_state,
-                .sock_init      = ti_init,
-        },
-#endif
 };
 
 
@@ -975,7 +973,7 @@ static irqreturn_t yenta_probe_handler(int irq, void *dev_id)
 /* probes the PCI interrupt, use only on override functions */
 static int yenta_probe_cb_irq(struct yenta_socket *socket)
 {
-        u8 reg;
+        u8 reg = 0;
 
         if (!socket->cb_irq)
                 return -1;
@@ -989,7 +987,8 @@ static int yenta_probe_cb_irq(struct yenta_socket *socket)
         }
 
         /* generate interrupt, wait */
-        reg = exca_readb(socket, I365_CSCINT);
+        if (!socket->dev->irq)
+                reg = exca_readb(socket, I365_CSCINT);
         exca_writeb(socket, I365_CSCINT, reg | I365_CSC_STSCHG);
         cb_writel(socket, CB_SOCKET_EVENT, -1);
         cb_writel(socket, CB_SOCKET_MASK, CB_CSTSMASK);
diff --git a/drivers/platform/x86/intel_scu_ipc.c b/drivers/platform/x86/intel_scu_ipc.c
index 576c3ed92435..40658e3385b4 100644
--- a/drivers/platform/x86/intel_scu_ipc.c
+++ b/drivers/platform/x86/intel_scu_ipc.c
@@ -524,7 +524,7 @@ int intel_scu_ipc_command(int cmd, int sub, u32 *in, int inlen,
         for (i = 0; i < inlen; i++)
                 ipc_data_writel(*in++, 4 * i);
 
-        ipc_command(cmd << 12 | sub);
+        ipc_command((cmd << 12) | sub | (inlen << 18));
         err = busy_loop();
 
         for (i = 0; i < outlen; i++)
diff --git a/drivers/power/max17040_battery.c b/drivers/power/max17040_battery.c
index f3e22c9fe20a..2f2f9a6f54fa 100644
--- a/drivers/power/max17040_battery.c
+++ b/drivers/power/max17040_battery.c
@@ -225,7 +225,6 @@ static int __devinit max17040_probe(struct i2c_client *client,
         ret = power_supply_register(&client->dev, &chip->battery);
         if (ret) {
                 dev_err(&client->dev, "failed: power supply register\n");
-                i2c_set_clientdata(client, NULL);
                 kfree(chip);
                 return ret;
         }
@@ -245,7 +244,6 @@ static int __devexit max17040_remove(struct i2c_client *client)
 
         power_supply_unregister(&chip->battery);
         cancel_delayed_work(&chip->work);
-        i2c_set_clientdata(client, NULL);
         kfree(chip);
         return 0;
 }
diff --git a/drivers/rapidio/Kconfig b/drivers/rapidio/Kconfig
index c32822ad84a4..070211a5955c 100644
--- a/drivers/rapidio/Kconfig
+++ b/drivers/rapidio/Kconfig
@@ -8,3 +8,27 @@ config RAPIDIO_DISC_TIMEOUT
         ---help---
           Amount of time a discovery node waits for a host to complete
           enumeration before giving up.
+
+config RAPIDIO_ENABLE_RX_TX_PORTS
+        bool "Enable RapidIO Input/Output Ports"
+        depends on RAPIDIO
+        ---help---
+          The RapidIO specification describes a Output port transmit
+          enable and a Input port receive enable. The recommended state
+          for Input ports and Output ports should be disabled. When
+          this switch is set the RapidIO subsystem will enable all
+          ports for Input/Output direction to allow other traffic
+          than Maintenance transfers.
+
+source "drivers/rapidio/switches/Kconfig"
+
+config RAPIDIO_DEBUG
+        bool "RapidIO subsystem debug messages"
+        depends on RAPIDIO
+        help
+          Say Y here if you want the RapidIO subsystem to produce a bunch of
+          debug messages to the system log. Select this if you are having a
+          problem with the RapidIO subsystem and want to see more of what is
+          going on.
+
+          If you are unsure about this, say N here.
diff --git a/drivers/rapidio/Makefile b/drivers/rapidio/Makefile
index 7c0e1818de51..b6139fe187bf 100644
--- a/drivers/rapidio/Makefile
+++ b/drivers/rapidio/Makefile
@@ -4,3 +4,7 @@
 obj-y += rio.o rio-access.o rio-driver.o rio-scan.o rio-sysfs.o
 
 obj-$(CONFIG_RAPIDIO)           += switches/
+
+ifeq ($(CONFIG_RAPIDIO_DEBUG),y)
+EXTRA_CFLAGS += -DDEBUG
+endif
diff --git a/drivers/rapidio/rio-scan.c b/drivers/rapidio/rio-scan.c
index 45415096c294..8070e074c739 100644
--- a/drivers/rapidio/rio-scan.c
+++ b/drivers/rapidio/rio-scan.c
@@ -4,6 +4,14 @@
  * Copyright 2005 MontaVista Software, Inc.
  * Matt Porter <mporter@kernel.crashing.org>
  *
+ * Copyright 2009 Integrated Device Technology, Inc.
+ * Alex Bounine <alexandre.bounine@idt.com>
+ * - Added Port-Write/Error Management initialization and handling
+ *
+ * Copyright 2009 Sysgo AG
+ * Thomas Moll <thomas.moll@sysgo.com>
+ * - Added Input- Output- enable functionality, to allow full communication
+ *
  * 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
@@ -31,15 +39,16 @@
 LIST_HEAD(rio_devices);
 static LIST_HEAD(rio_switches);
 
-#define RIO_ENUM_CMPL_MAGIC     0xdeadbeef
-
 static void rio_enum_timeout(unsigned long);
 
+static void rio_init_em(struct rio_dev *rdev);
+
 DEFINE_SPINLOCK(rio_global_list_lock);
 
 static int next_destid = 0;
 static int next_switchid = 0;
 static int next_net = 0;
+static int next_comptag;
 
 static struct timer_list rio_enum_timer =
 TIMER_INITIALIZER(rio_enum_timeout, 0, 0);
@@ -52,12 +61,6 @@ static int rio_mport_phys_table[] = {
         -1,
 };
 
-static int rio_sport_phys_table[] = {
-        RIO_EFB_PAR_EP_FREE_ID,
-        RIO_EFB_SER_EP_FREE_ID,
-        -1,
-};
-
 /**
  * rio_get_device_id - Get the base/extended device id for a device
  * @port: RIO master port
@@ -118,12 +121,26 @@ static int rio_clear_locks(struct rio_mport *port)
         u32 result;
         int ret = 0;
 
-        /* Write component tag CSR magic complete value */
-        rio_local_write_config_32(port, RIO_COMPONENT_TAG_CSR,
-                                  RIO_ENUM_CMPL_MAGIC);
-        list_for_each_entry(rdev, &rio_devices, global_list)
-            rio_write_config_32(rdev, RIO_COMPONENT_TAG_CSR,
-                                RIO_ENUM_CMPL_MAGIC);
+        /* Assign component tag to all devices */
+        next_comptag = 1;
+        rio_local_write_config_32(port, RIO_COMPONENT_TAG_CSR, next_comptag++);
+
+        list_for_each_entry(rdev, &rio_devices, global_list) {
+                /* Mark device as discovered */
+                rio_read_config_32(rdev,
+                                   rdev->phys_efptr + RIO_PORT_GEN_CTL_CSR,
+                                   &result);
+                rio_write_config_32(rdev,
+                                    rdev->phys_efptr + RIO_PORT_GEN_CTL_CSR,
+                                    result | RIO_PORT_GEN_DISCOVERED);
+
+                rio_write_config_32(rdev, RIO_COMPONENT_TAG_CSR, next_comptag);
+                rdev->comp_tag = next_comptag++;
+                if (next_comptag >= 0x10000) {
+                        pr_err("RIO: Component Tag Counter Overflow\n");
+                        break;
+                }
+        }
 
         /* Release host device id locks */
         rio_local_write_config_32(port, RIO_HOST_DID_LOCK_CSR,
@@ -229,27 +246,37 @@ static int rio_is_switch(struct rio_dev *rdev)
 }
 
 /**
- * rio_route_set_ops- Sets routing operations for a particular vendor switch
+ * rio_switch_init - Sets switch operations for a particular vendor switch
  * @rdev: RIO device
+ * @do_enum: Enumeration/Discovery mode flag
  *
- * Searches the RIO route ops table for known switch types. If the vid
- * and did match a switch table entry, then set the add_entry() and
- * get_entry() ops to the table entry values.
+ * Searches the RIO switch ops table for known switch types. If the vid
+ * and did match a switch table entry, then call switch initialization
+ * routine to setup switch-specific routines.
  */
-static void rio_route_set_ops(struct rio_dev *rdev)
+static void rio_switch_init(struct rio_dev *rdev, int do_enum)
 {
-        struct rio_route_ops *cur = __start_rio_route_ops;
-        struct rio_route_ops *end = __end_rio_route_ops;
+        struct rio_switch_ops *cur = __start_rio_switch_ops;
+        struct rio_switch_ops *end = __end_rio_switch_ops;
 
         while (cur < end) {
                 if ((cur->vid == rdev->vid) && (cur->did == rdev->did)) {
-                        pr_debug("RIO: adding routing ops for %s\n", rio_name(rdev));
-                        rdev->rswitch->add_entry = cur->add_hook;
-                        rdev->rswitch->get_entry = cur->get_hook;
+                        pr_debug("RIO: calling init routine for %s\n",
+                                 rio_name(rdev));
+                        cur->init_hook(rdev, do_enum);
+                        break;
                 }
                 cur++;
         }
 
+        if ((cur >= end) && (rdev->pef & RIO_PEF_STD_RT)) {
+                pr_debug("RIO: adding STD routing ops for %s\n",
+                        rio_name(rdev));
+                rdev->rswitch->add_entry = rio_std_route_add_entry;
+                rdev->rswitch->get_entry = rio_std_route_get_entry;
+                rdev->rswitch->clr_table = rio_std_route_clr_table;
+        }
+
         if (!rdev->rswitch->add_entry || !rdev->rswitch->get_entry)
                 printk(KERN_ERR "RIO: missing routing ops for %s\n",
                        rio_name(rdev));
@@ -281,6 +308,65 @@ static int __devinit rio_add_device(struct rio_dev *rdev)
 }
 
 /**
+ * rio_enable_rx_tx_port - enable input reciever and output transmitter of
+ * given port
+ * @port: Master port associated with the RIO network
+ * @local: local=1 select local port otherwise a far device is reached
+ * @destid: Destination ID of the device to check host bit
+ * @hopcount: Number of hops to reach the target
+ * @port_num: Port (-number on switch) to enable on a far end device
+ *
+ * Returns 0 or 1 from on General Control Command and Status Register
+ * (EXT_PTR+0x3C)
+ */
+inline int rio_enable_rx_tx_port(struct rio_mport *port,
+                                 int local, u16 destid,
+                                 u8 hopcount, u8 port_num) {
+#ifdef CONFIG_RAPIDIO_ENABLE_RX_TX_PORTS
+        u32 regval;
+        u32 ext_ftr_ptr;
+
+        /*
+        * enable rx input tx output port
+        */
+        pr_debug("rio_enable_rx_tx_port(local = %d, destid = %d, hopcount = "
+                 "%d, port_num = %d)\n", local, destid, hopcount, port_num);
+
+        ext_ftr_ptr = rio_mport_get_physefb(port, local, destid, hopcount);
+
+        if (local) {
+                rio_local_read_config_32(port, ext_ftr_ptr +
+                                RIO_PORT_N_CTL_CSR(0),
+                                &regval);
+        } else {
+                if (rio_mport_read_config_32(port, destid, hopcount,
+                ext_ftr_ptr + RIO_PORT_N_CTL_CSR(port_num), &regval) < 0)
+                        return -EIO;
+        }
+
+        if (regval & RIO_PORT_N_CTL_P_TYP_SER) {
+                /* serial */
+                regval = regval | RIO_PORT_N_CTL_EN_RX_SER
+                                | RIO_PORT_N_CTL_EN_TX_SER;
+        } else {
+                /* parallel */
+                regval = regval | RIO_PORT_N_CTL_EN_RX_PAR
+                                | RIO_PORT_N_CTL_EN_TX_PAR;
+        }
+
+        if (local) {
+                rio_local_write_config_32(port, ext_ftr_ptr +
+                                          RIO_PORT_N_CTL_CSR(0), regval);
+        } else {
+                if (rio_mport_write_config_32(port, destid, hopcount,
+                    ext_ftr_ptr + RIO_PORT_N_CTL_CSR(port_num), regval) < 0)
+                        return -EIO;
+        }
+#endif
+        return 0;
+}
+
+/**
  * rio_setup_device- Allocates and sets up a RIO device
  * @net: RIO network
  * @port: Master port to send transactions
@@ -325,8 +411,14 @@ static struct rio_dev __devinit *rio_setup_device(struct rio_net *net,
         rdev->asm_rev = result >> 16;
         rio_mport_read_config_32(port, destid, hopcount, RIO_PEF_CAR,
                                  &rdev->pef);
-        if (rdev->pef & RIO_PEF_EXT_FEATURES)
+        if (rdev->pef & RIO_PEF_EXT_FEATURES) {
                 rdev->efptr = result & 0xffff;
+                rdev->phys_efptr = rio_mport_get_physefb(port, 0, destid,
+                                                         hopcount);
+
+                rdev->em_efptr = rio_mport_get_feature(port, 0, destid,
+                                                hopcount, RIO_EFB_ERR_MGMNT);
+        }
 
         rio_mport_read_config_32(port, destid, hopcount, RIO_SRC_OPS_CAR,
                                  &rdev->src_ops);
@@ -349,12 +441,13 @@ static struct rio_dev __devinit *rio_setup_device(struct rio_net *net,
         if (rio_is_switch(rdev)) {
                 rio_mport_read_config_32(port, destid, hopcount,
                                          RIO_SWP_INFO_CAR, &rdev->swpinfo);
-                rswitch = kmalloc(sizeof(struct rio_switch), GFP_KERNEL);
+                rswitch = kzalloc(sizeof(struct rio_switch), GFP_KERNEL);
                 if (!rswitch)
                         goto cleanup;
                 rswitch->switchid = next_switchid;
                 rswitch->hopcount = hopcount;
                 rswitch->destid = destid;
+                rswitch->port_ok = 0;
                 rswitch->route_table = kzalloc(sizeof(u8)*
                                         RIO_MAX_ROUTE_ENTRIES(port->sys_size),
                                         GFP_KERNEL);
@@ -367,13 +460,22 @@ static struct rio_dev __devinit *rio_setup_device(struct rio_net *net,
                 rdev->rswitch = rswitch;
                 dev_set_name(&rdev->dev, "%02x:s:%04x", rdev->net->id,
                              rdev->rswitch->switchid);
-                rio_route_set_ops(rdev);
+                rio_switch_init(rdev, do_enum);
+
+                if (do_enum && rdev->rswitch->clr_table)
+                        rdev->rswitch->clr_table(port, destid, hopcount,
+                                                 RIO_GLOBAL_TABLE);
 
                 list_add_tail(&rswitch->node, &rio_switches);
 
-        } else
+        } else {
+                if (do_enum)
+                        /*Enable Input Output Port (transmitter reviever)*/
+                        rio_enable_rx_tx_port(port, 0, destid, hopcount, 0);
+
                 dev_set_name(&rdev->dev, "%02x:e:%04x", rdev->net->id,
                              rdev->destid);
+        }
 
         rdev->dev.bus = &rio_bus_type;
 
@@ -414,23 +516,29 @@ cleanup:
  *
  * Reads the port error status CSR for a particular switch port to
  * determine if the port has an active link.  Returns
- * %PORT_N_ERR_STS_PORT_OK if the port is active or %0 if it is
+ * %RIO_PORT_N_ERR_STS_PORT_OK if the port is active or %0 if it is
  * inactive.
  */
 static int
 rio_sport_is_active(struct rio_mport *port, u16 destid, u8 hopcount, int sport)
 {
-        u32 result;
+        u32 result = 0;
         u32 ext_ftr_ptr;
 
-        int *entry = rio_sport_phys_table;
-
-        do {
-                if ((ext_ftr_ptr =
-                     rio_mport_get_feature(port, 0, destid, hopcount, *entry)))
+        ext_ftr_ptr = rio_mport_get_efb(port, 0, destid, hopcount, 0);
 
+        while (ext_ftr_ptr) {
+                rio_mport_read_config_32(port, destid, hopcount,
+                                         ext_ftr_ptr, &result);
+                result = RIO_GET_BLOCK_ID(result);
+                if ((result == RIO_EFB_SER_EP_FREE_ID) ||
+                    (result == RIO_EFB_SER_EP_FREE_ID_V13P) ||
+                    (result == RIO_EFB_SER_EP_FREC_ID))
                         break;
-        } while (*++entry >= 0);
+
+                ext_ftr_ptr = rio_mport_get_efb(port, 0, destid, hopcount,
+                                                ext_ftr_ptr);
+        }
 
         if (ext_ftr_ptr)
                 rio_mport_read_config_32(port, destid, hopcount,
@@ -438,7 +546,81 @@ rio_sport_is_active(struct rio_mport *port, u16 destid, u8 hopcount, int sport)
                                          RIO_PORT_N_ERR_STS_CSR(sport),
                                          &result);
 
-        return (result & PORT_N_ERR_STS_PORT_OK);
+        return result & RIO_PORT_N_ERR_STS_PORT_OK;
+}
+
+/**
+ * rio_lock_device - Acquires host device lock for specified device
+ * @port: Master port to send transaction
+ * @destid: Destination ID for device/switch
+ * @hopcount: Hopcount to reach switch
+ * @wait_ms: Max wait time in msec (0 = no timeout)
+ *
+ * Attepts to acquire host device lock for specified device
+ * Returns 0 if device lock acquired or EINVAL if timeout expires.
+ */
+static int
+rio_lock_device(struct rio_mport *port, u16 destid, u8 hopcount, int wait_ms)
+{
+        u32 result;
+        int tcnt = 0;
+
+        /* Attempt to acquire device lock */
+        rio_mport_write_config_32(port, destid, hopcount,
+                                  RIO_HOST_DID_LOCK_CSR, port->host_deviceid);
+        rio_mport_read_config_32(port, destid, hopcount,
+                                 RIO_HOST_DID_LOCK_CSR, &result);
+
+        while (result != port->host_deviceid) {
+                if (wait_ms != 0 && tcnt == wait_ms) {
+                        pr_debug("RIO: timeout when locking device %x:%x\n",
+                                destid, hopcount);
+                        return -EINVAL;
+                }
+
+                /* Delay a bit */
+                mdelay(1);
+                tcnt++;
+                /* Try to acquire device lock again */
+                rio_mport_write_config_32(port, destid,
+                        hopcount,
+                        RIO_HOST_DID_LOCK_CSR,
+                        port->host_deviceid);
+                rio_mport_read_config_32(port, destid,
+                        hopcount,
+                        RIO_HOST_DID_LOCK_CSR, &result);
+        }
+
+        return 0;
+}
+
+/**
+ * rio_unlock_device - Releases host device lock for specified device
+ * @port: Master port to send transaction
+ * @destid: Destination ID for device/switch
+ * @hopcount: Hopcount to reach switch
+ *
+ * Returns 0 if device lock released or EINVAL if fails.
+ */
+static int
+rio_unlock_device(struct rio_mport *port, u16 destid, u8 hopcount)
+{
+        u32 result;
+
+        /* Release device lock */
+        rio_mport_write_config_32(port, destid,
+                                  hopcount,
+                                  RIO_HOST_DID_LOCK_CSR,
+                                  port->host_deviceid);
+        rio_mport_read_config_32(port, destid, hopcount,
+                RIO_HOST_DID_LOCK_CSR, &result);
+        if ((result & 0xffff) != 0xffff) {
+                pr_debug("RIO: badness when releasing device lock %x:%x\n",
+                         destid, hopcount);
+                return -EINVAL;
+        }
+
+        return 0;
 }
 
 /**
@@ -448,6 +630,7 @@ rio_sport_is_active(struct rio_mport *port, u16 destid, u8 hopcount, int sport)
  * @table: Routing table ID
  * @route_destid: Destination ID to be routed
  * @route_port: Port number to be routed
+ * @lock: lock switch device flag
  *
  * Calls the switch specific add_entry() method to add a route entry
  * on a switch. The route table can be specified using the @table
@@ -456,12 +639,26 @@ rio_sport_is_active(struct rio_mport *port, u16 destid, u8 hopcount, int sport)
  * %RIO_GLOBAL_TABLE in @table. Returns %0 on success or %-EINVAL
  * on failure.
  */
-static int rio_route_add_entry(struct rio_mport *mport, struct rio_switch *rswitch,
-                               u16 table, u16 route_destid, u8 route_port)
+static int
+rio_route_add_entry(struct rio_mport *mport, struct rio_switch *rswitch,
+                    u16 table, u16 route_destid, u8 route_port, int lock)
 {
-        return rswitch->add_entry(mport, rswitch->destid,
+        int rc;
+
+        if (lock) {
+                rc = rio_lock_device(mport, rswitch->destid,
+                                     rswitch->hopcount, 1000);
+                if (rc)
+                        return rc;
+        }
+
+        rc = rswitch->add_entry(mport, rswitch->destid,
                                         rswitch->hopcount, table,
                                         route_destid, route_port);
+        if (lock)
+                rio_unlock_device(mport, rswitch->destid, rswitch->hopcount);
+
+        return rc;
 }
 
 /**
@@ -471,6 +668,7 @@ static int rio_route_add_entry(struct rio_mport *mport, struct rio_switch *rswit
  * @table: Routing table ID
  * @route_destid: Destination ID to be routed
  * @route_port: Pointer to read port number into
+ * @lock: lock switch device flag
  *
  * Calls the switch specific get_entry() method to read a route entry
  * in a switch. The route table can be specified using the @table
@@ -481,11 +679,24 @@ static int rio_route_add_entry(struct rio_mport *mport, struct rio_switch *rswit
  */
 static int
 rio_route_get_entry(struct rio_mport *mport, struct rio_switch *rswitch, u16 table,
-                    u16 route_destid, u8 * route_port)
+                    u16 route_destid, u8 *route_port, int lock)
 {
-        return rswitch->get_entry(mport, rswitch->destid,
+        int rc;
+
+        if (lock) {
+                rc = rio_lock_device(mport, rswitch->destid,
+                                     rswitch->hopcount, 1000);
+                if (rc)
+                        return rc;
+        }
+
+        rc = rswitch->get_entry(mport, rswitch->destid,
                                         rswitch->hopcount, table,
                                         route_destid, route_port);
+        if (lock)
+                rio_unlock_device(mport, rswitch->destid, rswitch->hopcount);
+
+        return rc;
 }
 
 /**
@@ -625,14 +836,14 @@ static int __devinit rio_enum_peer(struct rio_net *net, struct rio_mport *port,
                 sw_inport = rio_get_swpinfo_inport(port,
                                 RIO_ANY_DESTID(port->sys_size), hopcount);
                 rio_route_add_entry(port, rdev->rswitch, RIO_GLOBAL_TABLE,
-                                    port->host_deviceid, sw_inport);
+                                    port->host_deviceid, sw_inport, 0);
                 rdev->rswitch->route_table[port->host_deviceid] = sw_inport;
 
                 for (destid = 0; destid < next_destid; destid++) {
                         if (destid == port->host_deviceid)
                                 continue;
                         rio_route_add_entry(port, rdev->rswitch, RIO_GLOBAL_TABLE,
-                                            destid, sw_inport);
+                                            destid, sw_inport, 0);
                         rdev->rswitch->route_table[destid] = sw_inport;
                 }
 
@@ -644,8 +855,15 @@ static int __devinit rio_enum_peer(struct rio_net *net, struct rio_mport *port,
                     rio_name(rdev), rdev->vid, rdev->did, num_ports);
                 sw_destid = next_destid;
                 for (port_num = 0; port_num < num_ports; port_num++) {
-                        if (sw_inport == port_num)
+                        /*Enable Input Output Port (transmitter reviever)*/
+                        rio_enable_rx_tx_port(port, 0,
+                                              RIO_ANY_DESTID(port->sys_size),
+                                              hopcount, port_num);
+
+                        if (sw_inport == port_num) {
+                                rdev->rswitch->port_ok |= (1 << port_num);
                                 continue;
+                        }
 
                         cur_destid = next_destid;
 
@@ -655,10 +873,11 @@ static int __devinit rio_enum_peer(struct rio_net *net, struct rio_mport *port,
                                 pr_debug(
                                     "RIO: scanning device on port %d\n",
                                     port_num);
+                                rdev->rswitch->port_ok |= (1 << port_num);
                                 rio_route_add_entry(port, rdev->rswitch,
                                                 RIO_GLOBAL_TABLE,
                                                 RIO_ANY_DESTID(port->sys_size),
-                                                port_num);
+                                                port_num, 0);
 
                                 if (rio_enum_peer(net, port, hopcount + 1) < 0)
                                         return -1;
@@ -672,15 +891,35 @@ static int __devinit rio_enum_peer(struct rio_net *net, struct rio_mport *port,
                                                 rio_route_add_entry(port, rdev->rswitch,
                                                                     RIO_GLOBAL_TABLE,
                                                                     destid,
-                                                                    port_num);
+                                                                    port_num,
+                                                                    0);
                                                 rdev->rswitch->
                                                     route_table[destid] =
                                                     port_num;
                                         }
                                 }
+                        } else {
+                                /* If switch supports Error Management,
+                                 * set PORT_LOCKOUT bit for unused port
+                                 */
+                                if (rdev->em_efptr)
+                                        rio_set_port_lockout(rdev, port_num, 1);
+
+                                rdev->rswitch->port_ok &= ~(1 << port_num);
                         }
                 }
 
+                /* Direct Port-write messages to the enumeratiing host */
+                if ((rdev->src_ops & RIO_SRC_OPS_PORT_WRITE) &&
+                    (rdev->em_efptr)) {
+                        rio_write_config_32(rdev,
+                                        rdev->em_efptr + RIO_EM_PW_TGT_DEVID,
+                                        (port->host_deviceid << 16) |
+                                        (port->sys_size << 15));
+                }
+
+                rio_init_em(rdev);
+
                 /* Check for empty switch */
                 if (next_destid == sw_destid) {
                         next_destid++;
@@ -700,21 +939,16 @@ static int __devinit rio_enum_peer(struct rio_net *net, struct rio_mport *port,
  * rio_enum_complete- Tests if enumeration of a network is complete
  * @port: Master port to send transaction
  *
- * Tests the Component Tag CSR for presence of the magic enumeration
- * complete flag. Return %1 if enumeration is complete or %0 if
+ * Tests the Component Tag CSR for non-zero value (enumeration
+ * complete flag). Return %1 if enumeration is complete or %0 if
  * enumeration is incomplete.
  */
 static int rio_enum_complete(struct rio_mport *port)
 {
         u32 tag_csr;
-        int ret = 0;
 
         rio_local_read_config_32(port, RIO_COMPONENT_TAG_CSR, &tag_csr);
-
-        if (tag_csr == RIO_ENUM_CMPL_MAGIC)
-                ret = 1;
-
-        return ret;
+        return (tag_csr & 0xffff) ? 1 : 0;
 }
 
 /**
@@ -763,17 +997,21 @@ rio_disc_peer(struct rio_net *net, struct rio_mport *port, u16 destid,
                                 pr_debug(
                                     "RIO: scanning device on port %d\n",
                                     port_num);
+
+                                rio_lock_device(port, destid, hopcount, 1000);
+
                                 for (ndestid = 0;
                                      ndestid < RIO_ANY_DESTID(port->sys_size);
                                      ndestid++) {
                                         rio_route_get_entry(port, rdev->rswitch,
                                                             RIO_GLOBAL_TABLE,
                                                             ndestid,
-                                                            &route_port);
+                                                            &route_port, 0);
                                         if (route_port == port_num)
                                                 break;
                                 }
 
+                                rio_unlock_device(port, destid, hopcount);
                                 if (rio_disc_peer
                                     (net, port, ndestid, hopcount + 1) < 0)
                                         return -1;
@@ -792,7 +1030,7 @@ rio_disc_peer(struct rio_net *net, struct rio_mport *port, u16 destid,
  *
  * Reads the port error status CSR for the master port to
  * determine if the port has an active link.  Returns
- * %PORT_N_ERR_STS_PORT_OK if the  master port is active
+ * %RIO_PORT_N_ERR_STS_PORT_OK if the  master port is active
  * or %0 if it is inactive.
  */
 static int rio_mport_is_active(struct rio_mport *port)
@@ -813,7 +1051,7 @@ static int rio_mport_is_active(struct rio_mport *port)
                                          RIO_PORT_N_ERR_STS_CSR(port->index),
                                          &result);
 
-        return (result & PORT_N_ERR_STS_PORT_OK);
+        return result & RIO_PORT_N_ERR_STS_PORT_OK;
 }
 
 /**
@@ -866,12 +1104,17 @@ static void rio_update_route_tables(struct rio_mport *port)
                                 continue;
 
                         if (RIO_INVALID_ROUTE == rswitch->route_table[destid]) {
+                                /* Skip if destid ends in empty switch*/
+                                if (rswitch->destid == destid)
+                                        continue;
 
                                 sport = rio_get_swpinfo_inport(port,
                                                 rswitch->destid, rswitch->hopcount);
 
                                 if (rswitch->add_entry) {
-                                        rio_route_add_entry(port, rswitch, RIO_GLOBAL_TABLE, destid, sport);
+                                        rio_route_add_entry(port, rswitch,
+                                                RIO_GLOBAL_TABLE, destid,
+                                                sport, 0);
                                         rswitch->route_table[destid] = sport;
                                 }
                         }
@@ -880,6 +1123,32 @@ static void rio_update_route_tables(struct rio_mport *port)
 }
 
 /**
+ * rio_init_em - Initializes RIO Error Management (for switches)
+ * @rdev: RIO device
+ *
+ * For each enumerated switch, call device-specific error management
+ * initialization routine (if supplied by the switch driver).
+ */
+static void rio_init_em(struct rio_dev *rdev)
+{
+        if (rio_is_switch(rdev) && (rdev->em_efptr) &&
+            (rdev->rswitch->em_init)) {
+                rdev->rswitch->em_init(rdev);
+        }
+}
+
+/**
+ * rio_pw_enable - Enables/disables port-write handling by a master port
+ * @port: Master port associated with port-write handling
+ * @enable:  1=enable,  0=disable
+ */
+static void rio_pw_enable(struct rio_mport *port, int enable)
+{
+        if (port->ops->pwenable)
+                port->ops->pwenable(port, enable);
+}
+
+/**
  * rio_enum_mport- Start enumeration through a master port
  * @mport: Master port to send transactions
  *
@@ -911,6 +1180,10 @@ int __devinit rio_enum_mport(struct rio_mport *mport)
                         rc = -ENOMEM;
                         goto out;
                 }
+
+                /* Enable Input Output Port (transmitter reviever) */
+                rio_enable_rx_tx_port(mport, 1, 0, 0, 0);
+
                 if (rio_enum_peer(net, mport, 0) < 0) {
                         /* A higher priority host won enumeration, bail. */
                         printk(KERN_INFO
@@ -922,6 +1195,7 @@ int __devinit rio_enum_mport(struct rio_mport *mport)
                 }
                 rio_update_route_tables(mport);
                 rio_clear_locks(mport);
+                rio_pw_enable(mport, 1);
         } else {
                 printk(KERN_INFO "RIO: master port %d link inactive\n",
                        mport->id);
@@ -945,15 +1219,22 @@ static void rio_build_route_tables(void)
         u8 sport;
 
         list_for_each_entry(rdev, &rio_devices, global_list)
-            if (rio_is_switch(rdev))
-                for (i = 0;
-                     i < RIO_MAX_ROUTE_ENTRIES(rdev->net->hport->sys_size);
-                     i++) {
-                        if (rio_route_get_entry
-                            (rdev->net->hport, rdev->rswitch, RIO_GLOBAL_TABLE,
-                             i, &sport) < 0)
-                                continue;
-                        rdev->rswitch->route_table[i] = sport;
+                if (rio_is_switch(rdev)) {
+                        rio_lock_device(rdev->net->hport, rdev->rswitch->destid,
+                                        rdev->rswitch->hopcount, 1000);
+                        for (i = 0;
+                             i < RIO_MAX_ROUTE_ENTRIES(rdev->net->hport->sys_size);
+                             i++) {
+                                if (rio_route_get_entry
+                                    (rdev->net->hport, rdev->rswitch,
+                                     RIO_GLOBAL_TABLE, i, &sport, 0) < 0)
+                                        continue;
+                                rdev->rswitch->route_table[i] = sport;
+                        }
+
+                        rio_unlock_device(rdev->net->hport,
+                                          rdev->rswitch->destid,
+                                          rdev->rswitch->hopcount);
                 }
 }
 
@@ -1012,6 +1293,13 @@ int __devinit rio_disc_mport(struct rio_mport *mport)
                 del_timer_sync(&rio_enum_timer);
 
                 pr_debug("done\n");
+
+                /* Read DestID assigned by enumerator */
+                rio_local_read_config_32(mport, RIO_DID_CSR,
+                                         &mport->host_deviceid);
+                mport->host_deviceid = RIO_GET_DID(mport->sys_size,
+                                                   mport->host_deviceid);
+
                 if (rio_disc_peer(net, mport, RIO_ANY_DESTID(mport->sys_size),
                                         0) < 0) {
                         printk(KERN_INFO
diff --git a/drivers/rapidio/rio.c b/drivers/rapidio/rio.c
index 6395c780008b..08fa453af974 100644
--- a/drivers/rapidio/rio.c
+++ b/drivers/rapidio/rio.c
@@ -5,6 +5,10 @@
  * Copyright 2005 MontaVista Software, Inc.
  * Matt Porter <mporter@kernel.crashing.org>
  *
+ * Copyright 2009 Integrated Device Technology, Inc.
+ * Alex Bounine <alexandre.bounine@idt.com>
+ * - Added Port-Write/Error Management initialization and handling
+ *
  * 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
@@ -333,6 +337,331 @@ int rio_release_outb_dbell(struct rio_dev *rdev, struct resource *res)
 }
 
 /**
+ * rio_request_inb_pwrite - request inbound port-write message service
+ * @rdev: RIO device to which register inbound port-write callback routine
+ * @pwcback: Callback routine to execute when port-write is received
+ *
+ * Binds a port-write callback function to the RapidIO device.
+ * Returns 0 if the request has been satisfied.
+ */
+int rio_request_inb_pwrite(struct rio_dev *rdev,
+        int (*pwcback)(struct rio_dev *rdev, union rio_pw_msg *msg, int step))
+{
+        int rc = 0;
+
+        spin_lock(&rio_global_list_lock);
+        if (rdev->pwcback != NULL)
+                rc = -ENOMEM;
+        else
+                rdev->pwcback = pwcback;
+
+        spin_unlock(&rio_global_list_lock);
+        return rc;
+}
+EXPORT_SYMBOL_GPL(rio_request_inb_pwrite);
+
+/**
+ * rio_release_inb_pwrite - release inbound port-write message service
+ * @rdev: RIO device which registered for inbound port-write callback
+ *
+ * Removes callback from the rio_dev structure. Returns 0 if the request
+ * has been satisfied.
+ */
+int rio_release_inb_pwrite(struct rio_dev *rdev)
+{
+        int rc = -ENOMEM;
+
+        spin_lock(&rio_global_list_lock);
+        if (rdev->pwcback) {
+                rdev->pwcback = NULL;
+                rc = 0;
+        }
+
+        spin_unlock(&rio_global_list_lock);
+        return rc;
+}
+EXPORT_SYMBOL_GPL(rio_release_inb_pwrite);
+
+/**
+ * rio_mport_get_physefb - Helper function that returns register offset
+ *                      for Physical Layer Extended Features Block.
+ * @port: Master port to issue transaction
+ * @local: Indicate a local master port or remote device access
+ * @destid: Destination ID of the device
+ * @hopcount: Number of switch hops to the device
+ */
+u32
+rio_mport_get_physefb(struct rio_mport *port, int local,
+                      u16 destid, u8 hopcount)
+{
+        u32 ext_ftr_ptr;
+        u32 ftr_header;
+
+        ext_ftr_ptr = rio_mport_get_efb(port, local, destid, hopcount, 0);
+
+        while (ext_ftr_ptr)  {
+                if (local)
+                        rio_local_read_config_32(port, ext_ftr_ptr,
+                                                 &ftr_header);
+                else
+                        rio_mport_read_config_32(port, destid, hopcount,
+                                                 ext_ftr_ptr, &ftr_header);
+
+                ftr_header = RIO_GET_BLOCK_ID(ftr_header);
+                switch (ftr_header) {
+
+                case RIO_EFB_SER_EP_ID_V13P:
+                case RIO_EFB_SER_EP_REC_ID_V13P:
+                case RIO_EFB_SER_EP_FREE_ID_V13P:
+                case RIO_EFB_SER_EP_ID:
+                case RIO_EFB_SER_EP_REC_ID:
+                case RIO_EFB_SER_EP_FREE_ID:
+                case RIO_EFB_SER_EP_FREC_ID:
+
+                        return ext_ftr_ptr;
+
+                default:
+                        break;
+                }
+
+                ext_ftr_ptr = rio_mport_get_efb(port, local, destid,
+                                                hopcount, ext_ftr_ptr);
+        }
+
+        return ext_ftr_ptr;
+}
+
+/**
+ * rio_get_comptag - Begin or continue searching for a RIO device by component tag
+ * @comp_tag: RIO component tag to match
+ * @from: Previous RIO device found in search, or %NULL for new search
+ *
+ * Iterates through the list of known RIO devices. If a RIO device is
+ * found with a matching @comp_tag, a pointer to its device
+ * structure is returned. Otherwise, %NULL is returned. A new search
+ * is initiated by passing %NULL to the @from argument. Otherwise, if
+ * @from is not %NULL, searches continue from next device on the global
+ * list.
+ */
+static struct rio_dev *rio_get_comptag(u32 comp_tag, struct rio_dev *from)
+{
+        struct list_head *n;
+        struct rio_dev *rdev;
+
+        spin_lock(&rio_global_list_lock);
+        n = from ? from->global_list.next : rio_devices.next;
+
+        while (n && (n != &rio_devices)) {
+                rdev = rio_dev_g(n);
+                if (rdev->comp_tag == comp_tag)
+                        goto exit;
+                n = n->next;
+        }
+        rdev = NULL;
+exit:
+        spin_unlock(&rio_global_list_lock);
+        return rdev;
+}
+
+/**
+ * rio_set_port_lockout - Sets/clears LOCKOUT bit (RIO EM 1.3) for a switch port.
+ * @rdev: Pointer to RIO device control structure
+ * @pnum: Switch port number to set LOCKOUT bit
+ * @lock: Operation : set (=1) or clear (=0)
+ */
+int rio_set_port_lockout(struct rio_dev *rdev, u32 pnum, int lock)
+{
+        u8 hopcount = 0xff;
+        u16 destid = rdev->destid;
+        u32 regval;
+
+        if (rdev->rswitch) {
+                destid = rdev->rswitch->destid;
+                hopcount = rdev->rswitch->hopcount;
+        }
+
+        rio_mport_read_config_32(rdev->net->hport, destid, hopcount,
+                                 rdev->phys_efptr + RIO_PORT_N_CTL_CSR(pnum),
+                                 &regval);
+        if (lock)
+                regval |= RIO_PORT_N_CTL_LOCKOUT;
+        else
+                regval &= ~RIO_PORT_N_CTL_LOCKOUT;
+
+        rio_mport_write_config_32(rdev->net->hport, destid, hopcount,
+                                  rdev->phys_efptr + RIO_PORT_N_CTL_CSR(pnum),
+                                  regval);
+        return 0;
+}
+
+/**
+ * rio_inb_pwrite_handler - process inbound port-write message
+ * @pw_msg: pointer to inbound port-write message
+ *
+ * Processes an inbound port-write message. Returns 0 if the request
+ * has been satisfied.
+ */
+int rio_inb_pwrite_handler(union rio_pw_msg *pw_msg)
+{
+        struct rio_dev *rdev;
+        struct rio_mport *mport;
+        u8 hopcount;
+        u16 destid;
+        u32 err_status;
+        int rc, portnum;
+
+        rdev = rio_get_comptag(pw_msg->em.comptag, NULL);
+        if (rdev == NULL) {
+                /* Someting bad here (probably enumeration error) */
+                pr_err("RIO: %s No matching device for CTag 0x%08x\n",
+                        __func__, pw_msg->em.comptag);
+                return -EIO;
+        }
+
+        pr_debug("RIO: Port-Write message from %s\n", rio_name(rdev));
+
+#ifdef DEBUG_PW
+        {
+        u32 i;
+        for (i = 0; i < RIO_PW_MSG_SIZE/sizeof(u32);) {
+                        pr_debug("0x%02x: %08x %08x %08x %08x",
+                                 i*4, pw_msg->raw[i], pw_msg->raw[i + 1],
+                                 pw_msg->raw[i + 2], pw_msg->raw[i + 3]);
+                        i += 4;
+        }
+        pr_debug("\n");
+        }
+#endif
+
+        /* Call an external service function (if such is registered
+         * for this device). This may be the service for endpoints that send
+         * device-specific port-write messages. End-point messages expected
+         * to be handled completely by EP specific device driver.
+         * For switches rc==0 signals that no standard processing required.
+         */
+        if (rdev->pwcback != NULL) {
+                rc = rdev->pwcback(rdev, pw_msg, 0);
+                if (rc == 0)
+                        return 0;
+        }
+
+        /* For End-point devices processing stops here */
+        if (!(rdev->pef & RIO_PEF_SWITCH))
+                return 0;
+
+        if (rdev->phys_efptr == 0) {
+                pr_err("RIO_PW: Bad switch initialization for %s\n",
+                        rio_name(rdev));
+                return 0;
+        }
+
+        mport = rdev->net->hport;
+        destid = rdev->rswitch->destid;
+        hopcount = rdev->rswitch->hopcount;
+
+        /*
+         * Process the port-write notification from switch
+         */
+
+        portnum = pw_msg->em.is_port & 0xFF;
+
+        if (rdev->rswitch->em_handle)
+                rdev->rswitch->em_handle(rdev, portnum);
+
+        rio_mport_read_config_32(mport, destid, hopcount,
+                        rdev->phys_efptr + RIO_PORT_N_ERR_STS_CSR(portnum),
+                        &err_status);
+        pr_debug("RIO_PW: SP%d_ERR_STS_CSR=0x%08x\n", portnum, err_status);
+
+        if (pw_msg->em.errdetect) {
+                pr_debug("RIO_PW: RIO_EM_P%d_ERR_DETECT=0x%08x\n",
+                         portnum, pw_msg->em.errdetect);
+                /* Clear EM Port N Error Detect CSR */
+                rio_mport_write_config_32(mport, destid, hopcount,
+                        rdev->em_efptr + RIO_EM_PN_ERR_DETECT(portnum), 0);
+        }
+
+        if (pw_msg->em.ltlerrdet) {
+                pr_debug("RIO_PW: RIO_EM_LTL_ERR_DETECT=0x%08x\n",
+                         pw_msg->em.ltlerrdet);
+                /* Clear EM L/T Layer Error Detect CSR */
+                rio_mport_write_config_32(mport, destid, hopcount,
+                        rdev->em_efptr + RIO_EM_LTL_ERR_DETECT, 0);
+        }
+
+        /* Clear Port Errors */
+        rio_mport_write_config_32(mport, destid, hopcount,
+                        rdev->phys_efptr + RIO_PORT_N_ERR_STS_CSR(portnum),
+                        err_status & RIO_PORT_N_ERR_STS_CLR_MASK);
+
+        if (rdev->rswitch->port_ok & (1 << portnum)) {
+                if (err_status & RIO_PORT_N_ERR_STS_PORT_UNINIT) {
+                        rdev->rswitch->port_ok &= ~(1 << portnum);
+                        rio_set_port_lockout(rdev, portnum, 1);
+
+                        rio_mport_write_config_32(mport, destid, hopcount,
+                                rdev->phys_efptr +
+                                        RIO_PORT_N_ACK_STS_CSR(portnum),
+                                RIO_PORT_N_ACK_CLEAR);
+
+                        /* Schedule Extraction Service */
+                        pr_debug("RIO_PW: Device Extraction on [%s]-P%d\n",
+                               rio_name(rdev), portnum);
+                }
+        } else {
+                if (err_status & RIO_PORT_N_ERR_STS_PORT_OK) {
+                        rdev->rswitch->port_ok |= (1 << portnum);
+                        rio_set_port_lockout(rdev, portnum, 0);
+
+                        /* Schedule Insertion Service */
+                        pr_debug("RIO_PW: Device Insertion on [%s]-P%d\n",
+                               rio_name(rdev), portnum);
+                }
+        }
+
+        /* Clear Port-Write Pending bit */
+        rio_mport_write_config_32(mport, destid, hopcount,
+                        rdev->phys_efptr + RIO_PORT_N_ERR_STS_CSR(portnum),
+                        RIO_PORT_N_ERR_STS_PW_PEND);
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(rio_inb_pwrite_handler);
+
+/**
+ * rio_mport_get_efb - get pointer to next extended features block
+ * @port: Master port to issue transaction
+ * @local: Indicate a local master port or remote device access
+ * @destid: Destination ID of the device
+ * @hopcount: Number of switch hops to the device
+ * @from: Offset of  current Extended Feature block header (if 0 starts
+ * from ExtFeaturePtr)
+ */
+u32
+rio_mport_get_efb(struct rio_mport *port, int local, u16 destid,
+                      u8 hopcount, u32 from)
+{
+        u32 reg_val;
+
+        if (from == 0) {
+                if (local)
+                        rio_local_read_config_32(port, RIO_ASM_INFO_CAR,
+                                                 &reg_val);
+                else
+                        rio_mport_read_config_32(port, destid, hopcount,
+                                                 RIO_ASM_INFO_CAR, &reg_val);
+                return reg_val & RIO_EXT_FTR_PTR_MASK;
+        } else {
+                if (local)
+                        rio_local_read_config_32(port, from, &reg_val);
+                else
+                        rio_mport_read_config_32(port, destid, hopcount,
+                                                 from, &reg_val);
+                return RIO_GET_BLOCK_ID(reg_val);
+        }
+}
+
+/**
  * rio_mport_get_feature - query for devices' extended features
  * @port: Master port to issue transaction
  * @local: Indicate a local master port or remote device access
@@ -451,6 +780,110 @@ struct rio_dev *rio_get_device(u16 vid, u16 did, struct rio_dev *from)
         return rio_get_asm(vid, did, RIO_ANY_ID, RIO_ANY_ID, from);
 }
 
+/**
+ * rio_std_route_add_entry - Add switch route table entry using standard
+ *   registers defined in RIO specification rev.1.3
+ * @mport: Master port to issue transaction
+ * @destid: Destination ID of the device
+ * @hopcount: Number of switch hops to the device
+ * @table: routing table ID (global or port-specific)
+ * @route_destid: destID entry in the RT
+ * @route_port: destination port for specified destID
+ */
+int rio_std_route_add_entry(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u16 table, u16 route_destid, u8 route_port)
+{
+        if (table == RIO_GLOBAL_TABLE) {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                RIO_STD_RTE_CONF_DESTID_SEL_CSR,
+                                (u32)route_destid);
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                RIO_STD_RTE_CONF_PORT_SEL_CSR,
+                                (u32)route_port);
+        }
+
+        udelay(10);
+        return 0;
+}
+
+/**
+ * rio_std_route_get_entry - Read switch route table entry (port number)
+ *   assosiated with specified destID using standard registers defined in RIO
+ *   specification rev.1.3
+ * @mport: Master port to issue transaction
+ * @destid: Destination ID of the device
+ * @hopcount: Number of switch hops to the device
+ * @table: routing table ID (global or port-specific)
+ * @route_destid: destID entry in the RT
+ * @route_port: returned destination port for specified destID
+ */
+int rio_std_route_get_entry(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u16 table, u16 route_destid, u8 *route_port)
+{
+        u32 result;
+
+        if (table == RIO_GLOBAL_TABLE) {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                RIO_STD_RTE_CONF_DESTID_SEL_CSR, route_destid);
+                rio_mport_read_config_32(mport, destid, hopcount,
+                                RIO_STD_RTE_CONF_PORT_SEL_CSR, &result);
+
+                *route_port = (u8)result;
+        }
+
+        return 0;
+}
+
+/**
+ * rio_std_route_clr_table - Clear swotch route table using standard registers
+ *   defined in RIO specification rev.1.3.
+ * @mport: Master port to issue transaction
+ * @destid: Destination ID of the device
+ * @hopcount: Number of switch hops to the device
+ * @table: routing table ID (global or port-specific)
+ */
+int rio_std_route_clr_table(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u16 table)
+{
+        u32 max_destid = 0xff;
+        u32 i, pef, id_inc = 1, ext_cfg = 0;
+        u32 port_sel = RIO_INVALID_ROUTE;
+
+        if (table == RIO_GLOBAL_TABLE) {
+                rio_mport_read_config_32(mport, destid, hopcount,
+                                         RIO_PEF_CAR, &pef);
+
+                if (mport->sys_size) {
+                        rio_mport_read_config_32(mport, destid, hopcount,
+                                                 RIO_SWITCH_RT_LIMIT,
+                                                 &max_destid);
+                        max_destid &= RIO_RT_MAX_DESTID;
+                }
+
+                if (pef & RIO_PEF_EXT_RT) {
+                        ext_cfg = 0x80000000;
+                        id_inc = 4;
+                        port_sel = (RIO_INVALID_ROUTE << 24) |
+                                   (RIO_INVALID_ROUTE << 16) |
+                                   (RIO_INVALID_ROUTE << 8) |
+                                   RIO_INVALID_ROUTE;
+                }
+
+                for (i = 0; i <= max_destid;) {
+                        rio_mport_write_config_32(mport, destid, hopcount,
+                                        RIO_STD_RTE_CONF_DESTID_SEL_CSR,
+                                        ext_cfg | i);
+                        rio_mport_write_config_32(mport, destid, hopcount,
+                                        RIO_STD_RTE_CONF_PORT_SEL_CSR,
+                                        port_sel);
+                        i += id_inc;
+                }
+        }
+
+        udelay(10);
+        return 0;
+}
+
 static void rio_fixup_device(struct rio_dev *dev)
 {
 }
diff --git a/drivers/rapidio/rio.h b/drivers/rapidio/rio.h
index 7786d02581f2..f27b7a9c47d2 100644
--- a/drivers/rapidio/rio.h
+++ b/drivers/rapidio/rio.h
@@ -18,38 +18,50 @@
 
 extern u32 rio_mport_get_feature(struct rio_mport *mport, int local, u16 destid,
                                  u8 hopcount, int ftr);
+extern u32 rio_mport_get_physefb(struct rio_mport *port, int local,
+                                 u16 destid, u8 hopcount);
+extern u32 rio_mport_get_efb(struct rio_mport *port, int local, u16 destid,
+                             u8 hopcount, u32 from);
 extern int rio_create_sysfs_dev_files(struct rio_dev *rdev);
 extern int rio_enum_mport(struct rio_mport *mport);
 extern int rio_disc_mport(struct rio_mport *mport);
+extern int rio_std_route_add_entry(struct rio_mport *mport, u16 destid,
+                                   u8 hopcount, u16 table, u16 route_destid,
+                                   u8 route_port);
+extern int rio_std_route_get_entry(struct rio_mport *mport, u16 destid,
+                                   u8 hopcount, u16 table, u16 route_destid,
+                                   u8 *route_port);
+extern int rio_std_route_clr_table(struct rio_mport *mport, u16 destid,
+                                   u8 hopcount, u16 table);
+extern int rio_set_port_lockout(struct rio_dev *rdev, u32 pnum, int lock);
 
 /* Structures internal to the RIO core code */
 extern struct device_attribute rio_dev_attrs[];
 extern spinlock_t rio_global_list_lock;
 
-extern struct rio_route_ops __start_rio_route_ops[];
-extern struct rio_route_ops __end_rio_route_ops[];
+extern struct rio_switch_ops __start_rio_switch_ops[];
+extern struct rio_switch_ops __end_rio_switch_ops[];
 
 /* Helpers internal to the RIO core code */
-#define DECLARE_RIO_ROUTE_SECTION(section, vid, did, add_hook, get_hook)  \
-        static struct rio_route_ops __rio_route_ops __used   \
-        __section(section)= { vid, did, add_hook, get_hook };
+#define DECLARE_RIO_SWITCH_SECTION(section, name, vid, did, init_hook) \
+        static const struct rio_switch_ops __rio_switch_##name __used \
+        __section(section) = { vid, did, init_hook };
 
 /**
- * DECLARE_RIO_ROUTE_OPS - Registers switch routing operations
+ * DECLARE_RIO_SWITCH_INIT - Registers switch initialization routine
  * @vid: RIO vendor ID
  * @did: RIO device ID
- * @add_hook: Callback that adds a route entry
- * @get_hook: Callback that gets a route entry
+ * @init_hook: Callback that performs switch-specific initialization
  *
- * Manipulating switch route tables in RIO is switch specific. This
- * registers a switch by vendor and device ID with two callbacks for
- * modifying and retrieving route entries in a switch. A &struct
- * rio_route_ops is initialized with the ops and placed into a
- * RIO-specific kernel section.
+ * Manipulating switch route tables and error management in RIO
+ * is switch specific. This registers a switch by vendor and device ID with
+ * initialization callback for setting up switch operations and (if required)
+ * hardware initialization. A &struct rio_switch_ops is initialized with
+ * pointer to the init routine and placed into a RIO-specific kernel section.
  */
-#define DECLARE_RIO_ROUTE_OPS(vid, did, add_hook, get_hook)             \
-        DECLARE_RIO_ROUTE_SECTION(.rio_route_ops,                       \
-                        vid, did, add_hook, get_hook)
+#define DECLARE_RIO_SWITCH_INIT(vid, did, init_hook)            \
+        DECLARE_RIO_SWITCH_SECTION(.rio_switch_ops, vid##did, \
+                        vid, did, init_hook)
 
 #define RIO_GET_DID(size, x)    (size ? (x & 0xffff) : ((x & 0x00ff0000) >> 16))
 #define RIO_SET_DID(size, x)    (size ? (x & 0xffff) : ((x & 0x000000ff) << 16))
diff --git a/drivers/rapidio/switches/Kconfig b/drivers/rapidio/switches/Kconfig
new file mode 100644
index 000000000000..2b4e9b2b6631
--- /dev/null
+++ b/drivers/rapidio/switches/Kconfig
@@ -0,0 +1,28 @@
+#
+# RapidIO switches configuration
+#
+config RAPIDIO_TSI57X
+        bool "IDT Tsi57x SRIO switches support"
+        depends on RAPIDIO
+        ---help---
+          Includes support for IDT Tsi57x family of serial RapidIO switches.
+
+config RAPIDIO_CPS_XX
+        bool "IDT CPS-xx SRIO switches support"
+        depends on RAPIDIO
+        ---help---
+          Includes support for IDT CPS-16/12/10/8 serial RapidIO switches.
+
+config RAPIDIO_TSI568
+        bool "Tsi568 SRIO switch support"
+        depends on RAPIDIO
+        default n
+        ---help---
+          Includes support for IDT Tsi568 serial RapidIO switch.
+
+config RAPIDIO_TSI500
+        bool "Tsi500 Parallel RapidIO switch support"
+        depends on RAPIDIO
+        default n
+        ---help---
+          Includes support for IDT Tsi500 parallel RapidIO switch.
diff --git a/drivers/rapidio/switches/Makefile b/drivers/rapidio/switches/Makefile
index b924f8301761..fe4adc3e8d5f 100644
--- a/drivers/rapidio/switches/Makefile
+++ b/drivers/rapidio/switches/Makefile
@@ -2,4 +2,11 @@
 # Makefile for RIO switches
 #
 
-obj-$(CONFIG_RAPIDIO)   += tsi500.o
+obj-$(CONFIG_RAPIDIO_TSI57X)    += tsi57x.o
+obj-$(CONFIG_RAPIDIO_CPS_XX)    += idtcps.o
+obj-$(CONFIG_RAPIDIO_TSI568)    += tsi568.o
+obj-$(CONFIG_RAPIDIO_TSI500)    += tsi500.o
+
+ifeq ($(CONFIG_RAPIDIO_DEBUG),y)
+EXTRA_CFLAGS += -DDEBUG
+endif
diff --git a/drivers/rapidio/switches/idtcps.c b/drivers/rapidio/switches/idtcps.c
new file mode 100644
index 000000000000..2c790c144f89
--- /dev/null
+++ b/drivers/rapidio/switches/idtcps.c
@@ -0,0 +1,137 @@
+/*
+ * IDT CPS RapidIO switches support
+ *
+ * Copyright 2009-2010 Integrated Device Technology, Inc.
+ * Alexandre Bounine <alexandre.bounine@idt.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/rio.h>
+#include <linux/rio_drv.h>
+#include <linux/rio_ids.h>
+#include "../rio.h"
+
+#define CPS_DEFAULT_ROUTE       0xde
+#define CPS_NO_ROUTE            0xdf
+
+#define IDTCPS_RIO_DOMAIN 0xf20020
+
+static int
+idtcps_route_add_entry(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u16 table, u16 route_destid, u8 route_port)
+{
+        u32 result;
+
+        if (table == RIO_GLOBAL_TABLE) {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                RIO_STD_RTE_CONF_DESTID_SEL_CSR, route_destid);
+
+                rio_mport_read_config_32(mport, destid, hopcount,
+                                RIO_STD_RTE_CONF_PORT_SEL_CSR, &result);
+
+                result = (0xffffff00 & result) | (u32)route_port;
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                RIO_STD_RTE_CONF_PORT_SEL_CSR, result);
+        }
+
+        return 0;
+}
+
+static int
+idtcps_route_get_entry(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u16 table, u16 route_destid, u8 *route_port)
+{
+        u32 result;
+
+        if (table == RIO_GLOBAL_TABLE) {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                RIO_STD_RTE_CONF_DESTID_SEL_CSR, route_destid);
+
+                rio_mport_read_config_32(mport, destid, hopcount,
+                                RIO_STD_RTE_CONF_PORT_SEL_CSR, &result);
+
+                if (CPS_DEFAULT_ROUTE == (u8)result ||
+                    CPS_NO_ROUTE == (u8)result)
+                        *route_port = RIO_INVALID_ROUTE;
+                else
+                        *route_port = (u8)result;
+        }
+
+        return 0;
+}
+
+static int
+idtcps_route_clr_table(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u16 table)
+{
+        u32 i;
+
+        if (table == RIO_GLOBAL_TABLE) {
+                for (i = 0x80000000; i <= 0x800000ff;) {
+                        rio_mport_write_config_32(mport, destid, hopcount,
+                                RIO_STD_RTE_CONF_DESTID_SEL_CSR, i);
+                        rio_mport_write_config_32(mport, destid, hopcount,
+                                RIO_STD_RTE_CONF_PORT_SEL_CSR,
+                                (CPS_DEFAULT_ROUTE << 24) |
+                                (CPS_DEFAULT_ROUTE << 16) |
+                                (CPS_DEFAULT_ROUTE << 8) | CPS_DEFAULT_ROUTE);
+                        i += 4;
+                }
+        }
+
+        return 0;
+}
+
+static int
+idtcps_set_domain(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u8 sw_domain)
+{
+        /*
+         * Switch domain configuration operates only at global level
+         */
+        rio_mport_write_config_32(mport, destid, hopcount,
+                                  IDTCPS_RIO_DOMAIN, (u32)sw_domain);
+        return 0;
+}
+
+static int
+idtcps_get_domain(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u8 *sw_domain)
+{
+        u32 regval;
+
+        /*
+         * Switch domain configuration operates only at global level
+         */
+        rio_mport_read_config_32(mport, destid, hopcount,
+                                IDTCPS_RIO_DOMAIN, &regval);
+
+        *sw_domain = (u8)(regval & 0xff);
+
+        return 0;
+}
+
+static int idtcps_switch_init(struct rio_dev *rdev, int do_enum)
+{
+        pr_debug("RIO: %s for %s\n", __func__, rio_name(rdev));
+        rdev->rswitch->add_entry = idtcps_route_add_entry;
+        rdev->rswitch->get_entry = idtcps_route_get_entry;
+        rdev->rswitch->clr_table = idtcps_route_clr_table;
+        rdev->rswitch->set_domain = idtcps_set_domain;
+        rdev->rswitch->get_domain = idtcps_get_domain;
+        rdev->rswitch->em_init = NULL;
+        rdev->rswitch->em_handle = NULL;
+
+        return 0;
+}
+
+DECLARE_RIO_SWITCH_INIT(RIO_VID_IDT, RIO_DID_IDTCPS6Q, idtcps_switch_init);
+DECLARE_RIO_SWITCH_INIT(RIO_VID_IDT, RIO_DID_IDTCPS8, idtcps_switch_init);
+DECLARE_RIO_SWITCH_INIT(RIO_VID_IDT, RIO_DID_IDTCPS10Q, idtcps_switch_init);
+DECLARE_RIO_SWITCH_INIT(RIO_VID_IDT, RIO_DID_IDTCPS12, idtcps_switch_init);
+DECLARE_RIO_SWITCH_INIT(RIO_VID_IDT, RIO_DID_IDTCPS16, idtcps_switch_init);
+DECLARE_RIO_SWITCH_INIT(RIO_VID_IDT, RIO_DID_IDT70K200, idtcps_switch_init);
diff --git a/drivers/rapidio/switches/tsi500.c b/drivers/rapidio/switches/tsi500.c
index c77c23bd9840..914eddd5aa42 100644
--- a/drivers/rapidio/switches/tsi500.c
+++ b/drivers/rapidio/switches/tsi500.c
@@ -1,6 +1,10 @@
 /*
  * RapidIO Tsi500 switch support
  *
+ * Copyright 2009-2010 Integrated Device Technology, Inc.
+ * Alexandre Bounine <alexandre.bounine@idt.com>
+ *  - Modified switch operations initialization.
+ *
  * Copyright 2005 MontaVista Software, Inc.
  * Matt Porter <mporter@kernel.crashing.org>
  *
@@ -57,4 +61,18 @@ tsi500_route_get_entry(struct rio_mport *mport, u16 destid, u8 hopcount, u16 tab
         return ret;
 }
 
-DECLARE_RIO_ROUTE_OPS(RIO_VID_TUNDRA, RIO_DID_TSI500, tsi500_route_add_entry, tsi500_route_get_entry);
+static int tsi500_switch_init(struct rio_dev *rdev, int do_enum)
+{
+        pr_debug("RIO: %s for %s\n", __func__, rio_name(rdev));
+        rdev->rswitch->add_entry = tsi500_route_add_entry;
+        rdev->rswitch->get_entry = tsi500_route_get_entry;
+        rdev->rswitch->clr_table = NULL;
+        rdev->rswitch->set_domain = NULL;
+        rdev->rswitch->get_domain = NULL;
+        rdev->rswitch->em_init = NULL;
+        rdev->rswitch->em_handle = NULL;
+
+        return 0;
+}
+
+DECLARE_RIO_SWITCH_INIT(RIO_VID_TUNDRA, RIO_DID_TSI500, tsi500_switch_init);
diff --git a/drivers/rapidio/switches/tsi568.c b/drivers/rapidio/switches/tsi568.c
new file mode 100644
index 000000000000..f7fd7898606e
--- /dev/null
+++ b/drivers/rapidio/switches/tsi568.c
@@ -0,0 +1,146 @@
+/*
+ * RapidIO Tsi568 switch support
+ *
+ * Copyright 2009-2010 Integrated Device Technology, Inc.
+ * Alexandre Bounine <alexandre.bounine@idt.com>
+ *  - Added EM support
+ *  - Modified switch operations initialization.
+ *
+ * Copyright 2005 MontaVista Software, Inc.
+ * Matt Porter <mporter@kernel.crashing.org>
+ *
+ * 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/rio.h>
+#include <linux/rio_drv.h>
+#include <linux/rio_ids.h>
+#include <linux/delay.h>
+#include "../rio.h"
+
+/* Global (broadcast) route registers */
+#define SPBC_ROUTE_CFG_DESTID   0x10070
+#define SPBC_ROUTE_CFG_PORT     0x10074
+
+/* Per port route registers */
+#define SPP_ROUTE_CFG_DESTID(n) (0x11070 + 0x100*n)
+#define SPP_ROUTE_CFG_PORT(n)   (0x11074 + 0x100*n)
+
+#define TSI568_SP_MODE_BC       0x10004
+#define  TSI568_SP_MODE_PW_DIS  0x08000000
+
+static int
+tsi568_route_add_entry(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u16 table, u16 route_destid, u8 route_port)
+{
+        if (table == RIO_GLOBAL_TABLE) {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                        SPBC_ROUTE_CFG_DESTID, route_destid);
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                        SPBC_ROUTE_CFG_PORT, route_port);
+        } else {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                        SPP_ROUTE_CFG_DESTID(table),
+                                        route_destid);
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                        SPP_ROUTE_CFG_PORT(table), route_port);
+        }
+
+        udelay(10);
+
+        return 0;
+}
+
+static int
+tsi568_route_get_entry(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u16 table, u16 route_destid, u8 *route_port)
+{
+        int ret = 0;
+        u32 result;
+
+        if (table == RIO_GLOBAL_TABLE) {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                        SPBC_ROUTE_CFG_DESTID, route_destid);
+                rio_mport_read_config_32(mport, destid, hopcount,
+                                        SPBC_ROUTE_CFG_PORT, &result);
+        } else {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                        SPP_ROUTE_CFG_DESTID(table),
+                                        route_destid);
+                rio_mport_read_config_32(mport, destid, hopcount,
+                                        SPP_ROUTE_CFG_PORT(table), &result);
+        }
+
+        *route_port = result;
+        if (*route_port > 15)
+                ret = -1;
+
+        return ret;
+}
+
+static int
+tsi568_route_clr_table(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u16 table)
+{
+        u32 route_idx;
+        u32 lut_size;
+
+        lut_size = (mport->sys_size) ? 0x1ff : 0xff;
+
+        if (table == RIO_GLOBAL_TABLE) {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                        SPBC_ROUTE_CFG_DESTID, 0x80000000);
+                for (route_idx = 0; route_idx <= lut_size; route_idx++)
+                        rio_mport_write_config_32(mport, destid, hopcount,
+                                                SPBC_ROUTE_CFG_PORT,
+                                                RIO_INVALID_ROUTE);
+        } else {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                        SPP_ROUTE_CFG_DESTID(table),
+                                        0x80000000);
+                for (route_idx = 0; route_idx <= lut_size; route_idx++)
+                        rio_mport_write_config_32(mport, destid, hopcount,
+                                                SPP_ROUTE_CFG_PORT(table),
+                                                RIO_INVALID_ROUTE);
+        }
+
+        return 0;
+}
+
+static int
+tsi568_em_init(struct rio_dev *rdev)
+{
+        struct rio_mport *mport = rdev->net->hport;
+        u16 destid = rdev->rswitch->destid;
+        u8 hopcount = rdev->rswitch->hopcount;
+        u32 regval;
+
+        pr_debug("TSI568 %s [%d:%d]\n", __func__, destid, hopcount);
+
+        /* Make sure that Port-Writes are disabled (for all ports) */
+        rio_mport_read_config_32(mport, destid, hopcount,
+                        TSI568_SP_MODE_BC, &regval);
+        rio_mport_write_config_32(mport, destid, hopcount,
+                        TSI568_SP_MODE_BC, regval | TSI568_SP_MODE_PW_DIS);
+
+        return 0;
+}
+
+static int tsi568_switch_init(struct rio_dev *rdev, int do_enum)
+{
+        pr_debug("RIO: %s for %s\n", __func__, rio_name(rdev));
+        rdev->rswitch->add_entry = tsi568_route_add_entry;
+        rdev->rswitch->get_entry = tsi568_route_get_entry;
+        rdev->rswitch->clr_table = tsi568_route_clr_table;
+        rdev->rswitch->set_domain = NULL;
+        rdev->rswitch->get_domain = NULL;
+        rdev->rswitch->em_init = tsi568_em_init;
+        rdev->rswitch->em_handle = NULL;
+
+        return 0;
+}
+
+DECLARE_RIO_SWITCH_INIT(RIO_VID_TUNDRA, RIO_DID_TSI568, tsi568_switch_init);
diff --git a/drivers/rapidio/switches/tsi57x.c b/drivers/rapidio/switches/tsi57x.c
new file mode 100644
index 000000000000..d34df722d95f
--- /dev/null
+++ b/drivers/rapidio/switches/tsi57x.c
@@ -0,0 +1,315 @@
+/*
+ * RapidIO Tsi57x switch family support
+ *
+ * Copyright 2009-2010 Integrated Device Technology, Inc.
+ * Alexandre Bounine <alexandre.bounine@idt.com>
+ *  - Added EM support
+ *  - Modified switch operations initialization.
+ *
+ * Copyright 2005 MontaVista Software, Inc.
+ * Matt Porter <mporter@kernel.crashing.org>
+ *
+ * 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/rio.h>
+#include <linux/rio_drv.h>
+#include <linux/rio_ids.h>
+#include <linux/delay.h>
+#include "../rio.h"
+
+/* Global (broadcast) route registers */
+#define SPBC_ROUTE_CFG_DESTID   0x10070
+#define SPBC_ROUTE_CFG_PORT     0x10074
+
+/* Per port route registers */
+#define SPP_ROUTE_CFG_DESTID(n) (0x11070 + 0x100*n)
+#define SPP_ROUTE_CFG_PORT(n)   (0x11074 + 0x100*n)
+
+#define TSI578_SP_MODE(n)       (0x11004 + n*0x100)
+#define TSI578_SP_MODE_GLBL     0x10004
+#define  TSI578_SP_MODE_PW_DIS  0x08000000
+#define  TSI578_SP_MODE_LUT_512 0x01000000
+
+#define TSI578_SP_CTL_INDEP(n)  (0x13004 + n*0x100)
+#define TSI578_SP_LUT_PEINF(n)  (0x13010 + n*0x100)
+#define TSI578_SP_CS_TX(n)      (0x13014 + n*0x100)
+#define TSI578_SP_INT_STATUS(n) (0x13018 + n*0x100)
+
+#define TSI578_GLBL_ROUTE_BASE  0x10078
+
+static int
+tsi57x_route_add_entry(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u16 table, u16 route_destid, u8 route_port)
+{
+        if (table == RIO_GLOBAL_TABLE) {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                          SPBC_ROUTE_CFG_DESTID, route_destid);
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                          SPBC_ROUTE_CFG_PORT, route_port);
+        } else {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                SPP_ROUTE_CFG_DESTID(table), route_destid);
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                SPP_ROUTE_CFG_PORT(table), route_port);
+        }
+
+        udelay(10);
+
+        return 0;
+}
+
+static int
+tsi57x_route_get_entry(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u16 table, u16 route_destid, u8 *route_port)
+{
+        int ret = 0;
+        u32 result;
+
+        if (table == RIO_GLOBAL_TABLE) {
+                /* Use local RT of the ingress port to avoid possible
+                   race condition */
+                rio_mport_read_config_32(mport, destid, hopcount,
+                        RIO_SWP_INFO_CAR, &result);
+                table = (result & RIO_SWP_INFO_PORT_NUM_MASK);
+        }
+
+        rio_mport_write_config_32(mport, destid, hopcount,
+                                SPP_ROUTE_CFG_DESTID(table), route_destid);
+        rio_mport_read_config_32(mport, destid, hopcount,
+                                SPP_ROUTE_CFG_PORT(table), &result);
+
+        *route_port = (u8)result;
+        if (*route_port > 15)
+                ret = -1;
+
+        return ret;
+}
+
+static int
+tsi57x_route_clr_table(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u16 table)
+{
+        u32 route_idx;
+        u32 lut_size;
+
+        lut_size = (mport->sys_size) ? 0x1ff : 0xff;
+
+        if (table == RIO_GLOBAL_TABLE) {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                          SPBC_ROUTE_CFG_DESTID, 0x80000000);
+                for (route_idx = 0; route_idx <= lut_size; route_idx++)
+                        rio_mport_write_config_32(mport, destid, hopcount,
+                                                  SPBC_ROUTE_CFG_PORT,
+                                                  RIO_INVALID_ROUTE);
+        } else {
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                SPP_ROUTE_CFG_DESTID(table), 0x80000000);
+                for (route_idx = 0; route_idx <= lut_size; route_idx++)
+                        rio_mport_write_config_32(mport, destid, hopcount,
+                                SPP_ROUTE_CFG_PORT(table) , RIO_INVALID_ROUTE);
+        }
+
+        return 0;
+}
+
+static int
+tsi57x_set_domain(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u8 sw_domain)
+{
+        u32 regval;
+
+        /*
+         * Switch domain configuration operates only at global level
+         */
+
+        /* Turn off flat (LUT_512) mode */
+        rio_mport_read_config_32(mport, destid, hopcount,
+                                 TSI578_SP_MODE_GLBL, &regval);
+        rio_mport_write_config_32(mport, destid, hopcount, TSI578_SP_MODE_GLBL,
+                                  regval & ~TSI578_SP_MODE_LUT_512);
+        /* Set switch domain base */
+        rio_mport_write_config_32(mport, destid, hopcount,
+                                  TSI578_GLBL_ROUTE_BASE,
+                                  (u32)(sw_domain << 24));
+        return 0;
+}
+
+static int
+tsi57x_get_domain(struct rio_mport *mport, u16 destid, u8 hopcount,
+                       u8 *sw_domain)
+{
+        u32 regval;
+
+        /*
+         * Switch domain configuration operates only at global level
+         */
+        rio_mport_read_config_32(mport, destid, hopcount,
+                                TSI578_GLBL_ROUTE_BASE, &regval);
+
+        *sw_domain = (u8)(regval >> 24);
+
+        return 0;
+}
+
+static int
+tsi57x_em_init(struct rio_dev *rdev)
+{
+        struct rio_mport *mport = rdev->net->hport;
+        u16 destid = rdev->rswitch->destid;
+        u8 hopcount = rdev->rswitch->hopcount;
+        u32 regval;
+        int portnum;
+
+        pr_debug("TSI578 %s [%d:%d]\n", __func__, destid, hopcount);
+
+        for (portnum = 0; portnum < 16; portnum++) {
+                /* Make sure that Port-Writes are enabled (for all ports) */
+                rio_mport_read_config_32(mport, destid, hopcount,
+                                TSI578_SP_MODE(portnum), &regval);
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                TSI578_SP_MODE(portnum),
+                                regval & ~TSI578_SP_MODE_PW_DIS);
+
+                /* Clear all pending interrupts */
+                rio_mport_read_config_32(mport, destid, hopcount,
+                                rdev->phys_efptr +
+                                        RIO_PORT_N_ERR_STS_CSR(portnum),
+                                &regval);
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                rdev->phys_efptr +
+                                        RIO_PORT_N_ERR_STS_CSR(portnum),
+                                regval & 0x07120214);
+
+                rio_mport_read_config_32(mport, destid, hopcount,
+                                TSI578_SP_INT_STATUS(portnum), &regval);
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                TSI578_SP_INT_STATUS(portnum),
+                                regval & 0x000700bd);
+
+                /* Enable all interrupts to allow ports to send a port-write */
+                rio_mport_read_config_32(mport, destid, hopcount,
+                                TSI578_SP_CTL_INDEP(portnum), &regval);
+                rio_mport_write_config_32(mport, destid, hopcount,
+                                TSI578_SP_CTL_INDEP(portnum),
+                                regval | 0x000b0000);
+
+                /* Skip next (odd) port if the current port is in x4 mode */
+                rio_mport_read_config_32(mport, destid, hopcount,
+                                rdev->phys_efptr + RIO_PORT_N_CTL_CSR(portnum),
+                                &regval);
+                if ((regval & RIO_PORT_N_CTL_PWIDTH) == RIO_PORT_N_CTL_PWIDTH_4)
+                        portnum++;
+        }
+
+        return 0;
+}
+
+static int
+tsi57x_em_handler(struct rio_dev *rdev, u8 portnum)
+{
+        struct rio_mport *mport = rdev->net->hport;
+        u16 destid = rdev->rswitch->destid;
+        u8 hopcount = rdev->rswitch->hopcount;
+        u32 intstat, err_status;
+        int sendcount, checkcount;
+        u8 route_port;
+        u32 regval;
+
+        rio_mport_read_config_32(mport, destid, hopcount,
+                        rdev->phys_efptr + RIO_PORT_N_ERR_STS_CSR(portnum),
+                        &err_status);
+
+        if ((err_status & RIO_PORT_N_ERR_STS_PORT_OK) &&
+            (err_status & (RIO_PORT_N_ERR_STS_PW_OUT_ES |
+                          RIO_PORT_N_ERR_STS_PW_INP_ES))) {
+                /* Remove any queued packets by locking/unlocking port */
+                rio_mport_read_config_32(mport, destid, hopcount,
+                        rdev->phys_efptr + RIO_PORT_N_CTL_CSR(portnum),
+                        &regval);
+                if (!(regval & RIO_PORT_N_CTL_LOCKOUT)) {
+                        rio_mport_write_config_32(mport, destid, hopcount,
+                                rdev->phys_efptr + RIO_PORT_N_CTL_CSR(portnum),
+                                regval | RIO_PORT_N_CTL_LOCKOUT);
+                        udelay(50);
+                        rio_mport_write_config_32(mport, destid, hopcount,
+                                rdev->phys_efptr + RIO_PORT_N_CTL_CSR(portnum),
+                                regval);
+                }
+
+                /* Read from link maintenance response register to clear
+                 * valid bit
+                 */
+                rio_mport_read_config_32(mport, destid, hopcount,
+                        rdev->phys_efptr + RIO_PORT_N_MNT_RSP_CSR(portnum),
+                        &regval);
+
+                /* Send a Packet-Not-Accepted/Link-Request-Input-Status control
+                 * symbol to recover from IES/OES
+                 */
+                sendcount = 3;
+                while (sendcount) {
+                        rio_mport_write_config_32(mport, destid, hopcount,
+                                          TSI578_SP_CS_TX(portnum), 0x40fc8000);
+                        checkcount = 3;
+                        while (checkcount--) {
+                                udelay(50);
+                                rio_mport_read_config_32(
+                                        mport, destid, hopcount,
+                                        rdev->phys_efptr +
+                                                RIO_PORT_N_MNT_RSP_CSR(portnum),
+                                        &regval);
+                                if (regval & RIO_PORT_N_MNT_RSP_RVAL)
+                                        goto exit_es;
+                        }
+
+                        sendcount--;
+                }
+        }
+
+exit_es:
+        /* Clear implementation specific error status bits */
+        rio_mport_read_config_32(mport, destid, hopcount,
+                                 TSI578_SP_INT_STATUS(portnum), &intstat);
+        pr_debug("TSI578[%x:%x] SP%d_INT_STATUS=0x%08x\n",
+                 destid, hopcount, portnum, intstat);
+
+        if (intstat & 0x10000) {
+                rio_mport_read_config_32(mport, destid, hopcount,
+                                TSI578_SP_LUT_PEINF(portnum), &regval);
+                regval = (mport->sys_size) ? (regval >> 16) : (regval >> 24);
+                route_port = rdev->rswitch->route_table[regval];
+                pr_debug("RIO: TSI578[%s] P%d LUT Parity Error (destID=%d)\n",
+                        rio_name(rdev), portnum, regval);
+                tsi57x_route_add_entry(mport, destid, hopcount,
+                                RIO_GLOBAL_TABLE, regval, route_port);
+        }
+
+        rio_mport_write_config_32(mport, destid, hopcount,
+                                  TSI578_SP_INT_STATUS(portnum),
+                                  intstat & 0x000700bd);
+
+        return 0;
+}
+
+static int tsi57x_switch_init(struct rio_dev *rdev, int do_enum)
+{
+        pr_debug("RIO: %s for %s\n", __func__, rio_name(rdev));
+        rdev->rswitch->add_entry = tsi57x_route_add_entry;
+        rdev->rswitch->get_entry = tsi57x_route_get_entry;
+        rdev->rswitch->clr_table = tsi57x_route_clr_table;
+        rdev->rswitch->set_domain = tsi57x_set_domain;
+        rdev->rswitch->get_domain = tsi57x_get_domain;
+        rdev->rswitch->em_init = tsi57x_em_init;
+        rdev->rswitch->em_handle = tsi57x_em_handler;
+
+        return 0;
+}
+
+DECLARE_RIO_SWITCH_INIT(RIO_VID_TUNDRA, RIO_DID_TSI572, tsi57x_switch_init);
+DECLARE_RIO_SWITCH_INIT(RIO_VID_TUNDRA, RIO_DID_TSI574, tsi57x_switch_init);
+DECLARE_RIO_SWITCH_INIT(RIO_VID_TUNDRA, RIO_DID_TSI577, tsi57x_switch_init);
+DECLARE_RIO_SWITCH_INIT(RIO_VID_TUNDRA, RIO_DID_TSI578, tsi57x_switch_init);
diff --git a/drivers/regulator/ab3100.c b/drivers/regulator/ab3100.c
index 1afd008ca957..7b14a67bdca2 100644
--- a/drivers/regulator/ab3100.c
+++ b/drivers/regulator/ab3100.c
@@ -16,7 +16,7 @@
 #include <linux/delay.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
-#include <linux/mfd/ab3100.h>
+#include <linux/mfd/abx500.h>
 
 /* LDO registers and some handy masking definitions for AB3100 */
 #define AB3100_LDO_A            0x40
@@ -41,7 +41,7 @@
  * struct ab3100_regulator
  * A struct passed around the individual regulator functions
  * @platform_device: platform device holding this regulator
- * @ab3100: handle to the AB3100 parent chip
+ * @dev: handle to the device
  * @plfdata: AB3100 platform data passed in at probe time
  * @regreg: regulator register number in the AB3100
  * @fixed_voltage: a fixed voltage for this regulator, if this
@@ -52,7 +52,7 @@
  */
 struct ab3100_regulator {
         struct regulator_dev *rdev;
-        struct ab3100 *ab3100;
+        struct device *dev;
         struct ab3100_platform_data *plfdata;
         u8 regreg;
         int fixed_voltage;
@@ -183,7 +183,7 @@ static int ab3100_enable_regulator(struct regulator_dev *reg)
         int err;
         u8 regval;
 
-        err = ab3100_get_register_interruptible(abreg->ab3100, abreg->regreg,
+        err = abx500_get_register_interruptible(abreg->dev, 0, abreg->regreg,
                                                 &regval);
         if (err) {
                 dev_warn(&reg->dev, "failed to get regid %d value\n",
@@ -197,7 +197,7 @@ static int ab3100_enable_regulator(struct regulator_dev *reg)
 
         regval |= AB3100_REG_ON_MASK;
 
-        err = ab3100_set_register_interruptible(abreg->ab3100, abreg->regreg,
+        err = abx500_set_register_interruptible(abreg->dev, 0, abreg->regreg,
                                                 regval);
         if (err) {
                 dev_warn(&reg->dev, "failed to set regid %d value\n",
@@ -245,14 +245,14 @@ static int ab3100_disable_regulator(struct regulator_dev *reg)
         if (abreg->regreg == AB3100_LDO_D) {
                 dev_info(&reg->dev, "disabling LDO D - shut down system\n");
                 /* Setting LDO D to 0x00 cuts the power to the SoC */
-                return ab3100_set_register_interruptible(abreg->ab3100,
+                return abx500_set_register_interruptible(abreg->dev, 0,
                                                          AB3100_LDO_D, 0x00U);
         }
 
         /*
          * All other regulators are handled here
          */
-        err = ab3100_get_register_interruptible(abreg->ab3100, abreg->regreg,
+        err = abx500_get_register_interruptible(abreg->dev, 0, abreg->regreg,
                                                 &regval);
         if (err) {
                 dev_err(&reg->dev, "unable to get register 0x%x\n",
@@ -260,7 +260,7 @@ static int ab3100_disable_regulator(struct regulator_dev *reg)
                 return err;
         }
         regval &= ~AB3100_REG_ON_MASK;
-        return ab3100_set_register_interruptible(abreg->ab3100, abreg->regreg,
+        return abx500_set_register_interruptible(abreg->dev, 0, abreg->regreg,
                                                  regval);
 }
 
@@ -270,7 +270,7 @@ static int ab3100_is_enabled_regulator(struct regulator_dev *reg)
         u8 regval;
         int err;
 
-        err = ab3100_get_register_interruptible(abreg->ab3100, abreg->regreg,
+        err = abx500_get_register_interruptible(abreg->dev, 0, abreg->regreg,
                                                 &regval);
         if (err) {
                 dev_err(&reg->dev, "unable to get register 0x%x\n",
@@ -305,7 +305,7 @@ static int ab3100_get_voltage_regulator(struct regulator_dev *reg)
          * For variable types, read out setting and index into
          * supplied voltage list.
          */
-        err = ab3100_get_register_interruptible(abreg->ab3100,
+        err = abx500_get_register_interruptible(abreg->dev, 0,
                                                 abreg->regreg, &regval);
         if (err) {
                 dev_warn(&reg->dev,
@@ -373,7 +373,7 @@ static int ab3100_set_voltage_regulator(struct regulator_dev *reg,
         if (bestindex < 0)
                 return bestindex;
 
-        err = ab3100_get_register_interruptible(abreg->ab3100,
+        err = abx500_get_register_interruptible(abreg->dev, 0,
                                                 abreg->regreg, &regval);
         if (err) {
                 dev_warn(&reg->dev,
@@ -386,7 +386,7 @@ static int ab3100_set_voltage_regulator(struct regulator_dev *reg,
         regval &= ~0xE0;
         regval |= (bestindex << 5);
 
-        err = ab3100_set_register_interruptible(abreg->ab3100,
+        err = abx500_set_register_interruptible(abreg->dev, 0,
                                                 abreg->regreg, regval);
         if (err)
                 dev_warn(&reg->dev, "failed to set regulator register %02x\n",
@@ -414,7 +414,7 @@ static int ab3100_set_suspend_voltage_regulator(struct regulator_dev *reg,
         /* LDO E and BUCK have special suspend voltages you can set */
         bestindex = ab3100_get_best_voltage_index(reg, uV, uV);
 
-        err = ab3100_get_register_interruptible(abreg->ab3100,
+        err = abx500_get_register_interruptible(abreg->dev, 0,
                                                 targetreg, &regval);
         if (err) {
                 dev_warn(&reg->dev,
@@ -427,7 +427,7 @@ static int ab3100_set_suspend_voltage_regulator(struct regulator_dev *reg,
         regval &= ~0xE0;
         regval |= (bestindex << 5);
 
-        err = ab3100_set_register_interruptible(abreg->ab3100,
+        err = abx500_set_register_interruptible(abreg->dev, 0,
                                                 targetreg, regval);
         if (err)
                 dev_warn(&reg->dev, "failed to set regulator register %02x\n",
@@ -574,13 +574,12 @@ ab3100_regulator_desc[AB3100_NUM_REGULATORS] = {
 static int __devinit ab3100_regulators_probe(struct platform_device *pdev)
 {
         struct ab3100_platform_data *plfdata = pdev->dev.platform_data;
-        struct ab3100 *ab3100 = platform_get_drvdata(pdev);
         int err = 0;
         u8 data;
         int i;
 
         /* Check chip state */
-        err = ab3100_get_register_interruptible(ab3100,
+        err = abx500_get_register_interruptible(&pdev->dev, 0,
                                                 AB3100_LDO_D, &data);
         if (err) {
                 dev_err(&pdev->dev, "could not read initial status of LDO_D\n");
@@ -595,7 +594,7 @@ static int __devinit ab3100_regulators_probe(struct platform_device *pdev)
 
         /* Set up regulators */
         for (i = 0; i < ARRAY_SIZE(ab3100_reg_init_order); i++) {
-                err = ab3100_set_register_interruptible(ab3100,
+                err = abx500_set_register_interruptible(&pdev->dev, 0,
                                         ab3100_reg_init_order[i],
                                         plfdata->reg_initvals[i]);
                 if (err) {
@@ -617,7 +616,7 @@ static int __devinit ab3100_regulators_probe(struct platform_device *pdev)
                  * see what it looks like for a certain machine, go
                  * into the machine I2C setup.
                  */
-                reg->ab3100 = ab3100;
+                reg->dev = &pdev->dev;
                 reg->plfdata = plfdata;
 
                 /*
diff --git a/drivers/regulator/lp3971.c b/drivers/regulator/lp3971.c
index 671a7d1f1f0e..8ae3732eb24b 100644
--- a/drivers/regulator/lp3971.c
+++ b/drivers/regulator/lp3971.c
@@ -519,8 +519,6 @@ static int __devexit lp3971_i2c_remove(struct i2c_client *i2c)
         struct lp3971 *lp3971 = i2c_get_clientdata(i2c);
         int i;
 
-        i2c_set_clientdata(i2c, NULL);
-
         for (i = 0; i < lp3971->num_regulators; i++)
                 regulator_unregister(lp3971->rdev[i]);
 
diff --git a/drivers/regulator/max1586.c b/drivers/regulator/max1586.c
index b3c1afc16889..2b54d9d75f11 100644
--- a/drivers/regulator/max1586.c
+++ b/drivers/regulator/max1586.c
@@ -244,7 +244,6 @@ static int __devexit max1586_pmic_remove(struct i2c_client *client)
         for (i = 0; i <= MAX1586_V6; i++)
                 if (rdev[i])
                         regulator_unregister(rdev[i]);
-        i2c_set_clientdata(client, NULL);
         kfree(rdev);
 
         return 0;
diff --git a/drivers/regulator/max8649.c b/drivers/regulator/max8649.c
index bfc4c5ffdc96..4520ace3f7e7 100644
--- a/drivers/regulator/max8649.c
+++ b/drivers/regulator/max8649.c
@@ -357,7 +357,6 @@ static int __devinit max8649_regulator_probe(struct i2c_client *client,
         dev_info(info->dev, "Max8649 regulator device is detected.\n");
         return 0;
 out:
-        i2c_set_clientdata(client, NULL);
         kfree(info);
         return ret;
 }
@@ -369,7 +368,6 @@ static int __devexit max8649_regulator_remove(struct i2c_client *client)
         if (info) {
                 if (info->regulator)
                         regulator_unregister(info->regulator);
-                i2c_set_clientdata(client, NULL);
                 kfree(info);
         }
 
diff --git a/drivers/regulator/max8660.c b/drivers/regulator/max8660.c
index 3790b21879ff..d97220efae5a 100644
--- a/drivers/regulator/max8660.c
+++ b/drivers/regulator/max8660.c
@@ -471,7 +471,6 @@ static int __devexit max8660_remove(struct i2c_client *client)
         for (i = 0; i < MAX8660_V_END; i++)
                 if (rdev[i])
                         regulator_unregister(rdev[i]);
-        i2c_set_clientdata(client, NULL);
         kfree(rdev);
 
         return 0;
diff --git a/drivers/regulator/tps65023-regulator.c b/drivers/regulator/tps65023-regulator.c
index 8e2f2098b005..f50afc9f287a 100644
--- a/drivers/regulator/tps65023-regulator.c
+++ b/drivers/regulator/tps65023-regulator.c
@@ -538,9 +538,6 @@ static int __devexit tps_65023_remove(struct i2c_client *client)
         struct tps_pmic *tps = i2c_get_clientdata(client);
         int i;
 
-        /* clear the client data in i2c */
-        i2c_set_clientdata(client, NULL);
-
         for (i = 0; i < TPS65023_NUM_REGULATOR; i++)
                 regulator_unregister(tps->rdev[i]);
 
diff --git a/drivers/regulator/tps6507x-regulator.c b/drivers/regulator/tps6507x-regulator.c
index 74841abcc9cc..14b4576281c5 100644
--- a/drivers/regulator/tps6507x-regulator.c
+++ b/drivers/regulator/tps6507x-regulator.c
@@ -22,68 +22,9 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
-#include <linux/i2c.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
-
-/* Register definitions */
-#define TPS6507X_REG_PPATH1                             0X01
-#define TPS6507X_REG_INT                                0X02
-#define TPS6507X_REG_CHGCONFIG0                         0X03
-#define TPS6507X_REG_CHGCONFIG1                         0X04
-#define TPS6507X_REG_CHGCONFIG2                         0X05
-#define TPS6507X_REG_CHGCONFIG3                         0X06
-#define TPS6507X_REG_REG_ADCONFIG                       0X07
-#define TPS6507X_REG_TSCMODE                            0X08
-#define TPS6507X_REG_ADRESULT_1                         0X09
-#define TPS6507X_REG_ADRESULT_2                         0X0A
-#define TPS6507X_REG_PGOOD                              0X0B
-#define TPS6507X_REG_PGOODMASK                          0X0C
-#define TPS6507X_REG_CON_CTRL1                          0X0D
-#define TPS6507X_REG_CON_CTRL2                          0X0E
-#define TPS6507X_REG_CON_CTRL3                          0X0F
-#define TPS6507X_REG_DEFDCDC1                           0X10
-#define TPS6507X_REG_DEFDCDC2_LOW                       0X11
-#define TPS6507X_REG_DEFDCDC2_HIGH                      0X12
-#define TPS6507X_REG_DEFDCDC3_LOW                       0X13
-#define TPS6507X_REG_DEFDCDC3_HIGH                      0X14
-#define TPS6507X_REG_DEFSLEW                            0X15
-#define TPS6507X_REG_LDO_CTRL1                          0X16
-#define TPS6507X_REG_DEFLDO2                            0X17
-#define TPS6507X_REG_WLED_CTRL1                         0X18
-#define TPS6507X_REG_WLED_CTRL2                         0X19
-
-/* CON_CTRL1 bitfields */
-#define TPS6507X_CON_CTRL1_DCDC1_ENABLE         BIT(4)
-#define TPS6507X_CON_CTRL1_DCDC2_ENABLE         BIT(3)
-#define TPS6507X_CON_CTRL1_DCDC3_ENABLE         BIT(2)
-#define TPS6507X_CON_CTRL1_LDO1_ENABLE          BIT(1)
-#define TPS6507X_CON_CTRL1_LDO2_ENABLE          BIT(0)
-
-/* DEFDCDC1 bitfields */
-#define TPS6507X_DEFDCDC1_DCDC1_EXT_ADJ_EN      BIT(7)
-#define TPS6507X_DEFDCDC1_DCDC1_MASK            0X3F
-
-/* DEFDCDC2_LOW bitfields */
-#define TPS6507X_DEFDCDC2_LOW_DCDC2_MASK        0X3F
-
-/* DEFDCDC2_HIGH bitfields */
-#define TPS6507X_DEFDCDC2_HIGH_DCDC2_MASK       0X3F
-
-/* DEFDCDC3_LOW bitfields */
-#define TPS6507X_DEFDCDC3_LOW_DCDC3_MASK        0X3F
-
-/* DEFDCDC3_HIGH bitfields */
-#define TPS6507X_DEFDCDC3_HIGH_DCDC3_MASK       0X3F
-
-/* TPS6507X_REG_LDO_CTRL1 bitfields */
-#define TPS6507X_REG_LDO_CTRL1_LDO1_MASK        0X0F
-
-/* TPS6507X_REG_DEFLDO2 bitfields */
-#define TPS6507X_REG_DEFLDO2_LDO2_MASK          0X3F
-
-/* VDCDC MASK */
-#define TPS6507X_DEFDCDCX_DCDC_MASK             0X3F
+#include <linux/mfd/tps6507x.h>
 
 /* DCDC's */
 #define TPS6507X_DCDC_1                         0
@@ -162,101 +103,146 @@ struct tps_info {
         const u16 *table;
 };
 
-struct tps_pmic {
+static const struct tps_info tps6507x_pmic_regs[] = {
+        {
+                .name = "VDCDC1",
+                .min_uV = 725000,
+                .max_uV = 3300000,
+                .table_len = ARRAY_SIZE(VDCDCx_VSEL_table),
+                .table = VDCDCx_VSEL_table,
+        },
+        {
+                .name = "VDCDC2",
+                .min_uV = 725000,
+                .max_uV = 3300000,
+                .table_len = ARRAY_SIZE(VDCDCx_VSEL_table),
+                .table = VDCDCx_VSEL_table,
+        },
+        {
+                .name = "VDCDC3",
+                .min_uV = 725000,
+                .max_uV = 3300000,
+                .table_len = ARRAY_SIZE(VDCDCx_VSEL_table),
+                .table = VDCDCx_VSEL_table,
+        },
+        {
+                .name = "LDO1",
+                .min_uV = 1000000,
+                .max_uV = 3300000,
+                .table_len = ARRAY_SIZE(LDO1_VSEL_table),
+                .table = LDO1_VSEL_table,
+        },
+        {
+                .name = "LDO2",
+                .min_uV = 725000,
+                .max_uV = 3300000,
+                .table_len = ARRAY_SIZE(LDO2_VSEL_table),
+                .table = LDO2_VSEL_table,
+        },
+};
+
+struct tps6507x_pmic {
         struct regulator_desc desc[TPS6507X_NUM_REGULATOR];
-        struct i2c_client *client;
+        struct tps6507x_dev *mfd;
         struct regulator_dev *rdev[TPS6507X_NUM_REGULATOR];
         const struct tps_info *info[TPS6507X_NUM_REGULATOR];
         struct mutex io_lock;
 };
-
-static inline int tps_6507x_read(struct tps_pmic *tps, u8 reg)
+static inline int tps6507x_pmic_read(struct tps6507x_pmic *tps, u8 reg)
 {
-        return i2c_smbus_read_byte_data(tps->client, reg);
+        u8 val;
+        int err;
+
+        err = tps->mfd->read_dev(tps->mfd, reg, 1, &val);
+
+        if (err)
+                return err;
+
+        return val;
 }
 
-static inline int tps_6507x_write(struct tps_pmic *tps, u8 reg, u8 val)
+static inline int tps6507x_pmic_write(struct tps6507x_pmic *tps, u8 reg, u8 val)
 {
-        return i2c_smbus_write_byte_data(tps->client, reg, val);
+        return tps->mfd->write_dev(tps->mfd, reg, 1, &val);
 }
 
-static int tps_6507x_set_bits(struct tps_pmic *tps, u8 reg, u8 mask)
+static int tps6507x_pmic_set_bits(struct tps6507x_pmic *tps, u8 reg, u8 mask)
 {
         int err, data;
 
         mutex_lock(&tps->io_lock);
 
-        data = tps_6507x_read(tps, reg);
+        data = tps6507x_pmic_read(tps, reg);
         if (data < 0) {
-                dev_err(&tps->client->dev, "Read from reg 0x%x failed\n", reg);
+                dev_err(tps->mfd->dev, "Read from reg 0x%x failed\n", reg);
                 err = data;
                 goto out;
         }
 
         data |= mask;
-        err = tps_6507x_write(tps, reg, data);
+        err = tps6507x_pmic_write(tps, reg, data);
         if (err)
-                dev_err(&tps->client->dev, "Write for reg 0x%x failed\n", reg);
+                dev_err(tps->mfd->dev, "Write for reg 0x%x failed\n", reg);
 
 out:
         mutex_unlock(&tps->io_lock);
         return err;
 }
 
-static int tps_6507x_clear_bits(struct tps_pmic *tps, u8 reg, u8 mask)
+static int tps6507x_pmic_clear_bits(struct tps6507x_pmic *tps, u8 reg, u8 mask)
 {
         int err, data;
 
         mutex_lock(&tps->io_lock);
 
-        data = tps_6507x_read(tps, reg);
+        data = tps6507x_pmic_read(tps, reg);
         if (data < 0) {
-                dev_err(&tps->client->dev, "Read from reg 0x%x failed\n", reg);
+                dev_err(tps->mfd->dev, "Read from reg 0x%x failed\n", reg);
                 err = data;
                 goto out;
         }
 
         data &= ~mask;
-        err = tps_6507x_write(tps, reg, data);
+        err = tps6507x_pmic_write(tps, reg, data);
         if (err)
-                dev_err(&tps->client->dev, "Write for reg 0x%x failed\n", reg);
+                dev_err(tps->mfd->dev, "Write for reg 0x%x failed\n", reg);
 
 out:
         mutex_unlock(&tps->io_lock);
         return err;
 }
 
-static int tps_6507x_reg_read(struct tps_pmic *tps, u8 reg)
+static int tps6507x_pmic_reg_read(struct tps6507x_pmic *tps, u8 reg)
 {
         int data;
 
         mutex_lock(&tps->io_lock);
 
-        data = tps_6507x_read(tps, reg);
+        data = tps6507x_pmic_read(tps, reg);
         if (data < 0)
-                dev_err(&tps->client->dev, "Read from reg 0x%x failed\n", reg);
+                dev_err(tps->mfd->dev, "Read from reg 0x%x failed\n", reg);
 
         mutex_unlock(&tps->io_lock);
         return data;
 }
 
-static int tps_6507x_reg_write(struct tps_pmic *tps, u8 reg, u8 val)
+static int tps6507x_pmic_reg_write(struct tps6507x_pmic *tps, u8 reg, u8 val)
 {
         int err;
 
         mutex_lock(&tps->io_lock);
 
-        err = tps_6507x_write(tps, reg, val);
+        err = tps6507x_pmic_write(tps, reg, val);
         if (err < 0)
-                dev_err(&tps->client->dev, "Write for reg 0x%x failed\n", reg);
+                dev_err(tps->mfd->dev, "Write for reg 0x%x failed\n", reg);
 
         mutex_unlock(&tps->io_lock);
         return err;
 }
 
-static int tps6507x_dcdc_is_enabled(struct regulator_dev *dev)
+static int tps6507x_pmic_dcdc_is_enabled(struct regulator_dev *dev)
 {
-        struct tps_pmic *tps = rdev_get_drvdata(dev);
+        struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
         int data, dcdc = rdev_get_id(dev);
         u8 shift;
 
@@ -264,7 +250,7 @@ static int tps6507x_dcdc_is_enabled(struct regulator_dev *dev)
                 return -EINVAL;
 
         shift = TPS6507X_MAX_REG_ID - dcdc;
-        data = tps_6507x_reg_read(tps, TPS6507X_REG_CON_CTRL1);
+        data = tps6507x_pmic_reg_read(tps, TPS6507X_REG_CON_CTRL1);
 
         if (data < 0)
                 return data;
@@ -272,9 +258,9 @@ static int tps6507x_dcdc_is_enabled(struct regulator_dev *dev)
                 return (data & 1<<shift) ? 1 : 0;
 }
 
-static int tps6507x_ldo_is_enabled(struct regulator_dev *dev)
+static int tps6507x_pmic_ldo_is_enabled(struct regulator_dev *dev)
 {
-        struct tps_pmic *tps = rdev_get_drvdata(dev);
+        struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
         int data, ldo = rdev_get_id(dev);
         u8 shift;
 
@@ -282,7 +268,7 @@ static int tps6507x_ldo_is_enabled(struct regulator_dev *dev)
                 return -EINVAL;
 
         shift = TPS6507X_MAX_REG_ID - ldo;
-        data = tps_6507x_reg_read(tps, TPS6507X_REG_CON_CTRL1);
+        data = tps6507x_pmic_reg_read(tps, TPS6507X_REG_CON_CTRL1);
 
         if (data < 0)
                 return data;
@@ -290,9 +276,9 @@ static int tps6507x_ldo_is_enabled(struct regulator_dev *dev)
                 return (data & 1<<shift) ? 1 : 0;
 }
 
-static int tps6507x_dcdc_enable(struct regulator_dev *dev)
+static int tps6507x_pmic_dcdc_enable(struct regulator_dev *dev)
 {
-        struct tps_pmic *tps = rdev_get_drvdata(dev);
+        struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
         int dcdc = rdev_get_id(dev);
         u8 shift;
 
@@ -300,12 +286,12 @@ static int tps6507x_dcdc_enable(struct regulator_dev *dev)
                 return -EINVAL;
 
         shift = TPS6507X_MAX_REG_ID - dcdc;
-        return tps_6507x_set_bits(tps, TPS6507X_REG_CON_CTRL1, 1 << shift);
+        return tps6507x_pmic_set_bits(tps, TPS6507X_REG_CON_CTRL1, 1 << shift);
 }
 
-static int tps6507x_dcdc_disable(struct regulator_dev *dev)
+static int tps6507x_pmic_dcdc_disable(struct regulator_dev *dev)
 {
-        struct tps_pmic *tps = rdev_get_drvdata(dev);
+        struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
         int dcdc = rdev_get_id(dev);
         u8 shift;
 
@@ -313,12 +299,13 @@ static int tps6507x_dcdc_disable(struct regulator_dev *dev)
                 return -EINVAL;
 
         shift = TPS6507X_MAX_REG_ID - dcdc;
-        return tps_6507x_clear_bits(tps, TPS6507X_REG_CON_CTRL1, 1 << shift);
+        return tps6507x_pmic_clear_bits(tps, TPS6507X_REG_CON_CTRL1,
+                                        1 << shift);
 }
 
-static int tps6507x_ldo_enable(struct regulator_dev *dev)
+static int tps6507x_pmic_ldo_enable(struct regulator_dev *dev)
 {
-        struct tps_pmic *tps = rdev_get_drvdata(dev);
+        struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
         int ldo = rdev_get_id(dev);
         u8 shift;
 
@@ -326,12 +313,12 @@ static int tps6507x_ldo_enable(struct regulator_dev *dev)
                 return -EINVAL;
 
         shift = TPS6507X_MAX_REG_ID - ldo;
-        return tps_6507x_set_bits(tps, TPS6507X_REG_CON_CTRL1, 1 << shift);
+        return tps6507x_pmic_set_bits(tps, TPS6507X_REG_CON_CTRL1, 1 << shift);
 }
 
-static int tps6507x_ldo_disable(struct regulator_dev *dev)
+static int tps6507x_pmic_ldo_disable(struct regulator_dev *dev)
 {
-        struct tps_pmic *tps = rdev_get_drvdata(dev);
+        struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
         int ldo = rdev_get_id(dev);
         u8 shift;
 
@@ -339,12 +326,13 @@ static int tps6507x_ldo_disable(struct regulator_dev *dev)
                 return -EINVAL;
 
         shift = TPS6507X_MAX_REG_ID - ldo;
-        return tps_6507x_clear_bits(tps, TPS6507X_REG_CON_CTRL1, 1 << shift);
+        return tps6507x_pmic_clear_bits(tps, TPS6507X_REG_CON_CTRL1,
+                                        1 << shift);
 }
 
-static int tps6507x_dcdc_get_voltage(struct regulator_dev *dev)
+static int tps6507x_pmic_dcdc_get_voltage(struct regulator_dev *dev)
 {
-        struct tps_pmic *tps = rdev_get_drvdata(dev);
+        struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
         int data, dcdc = rdev_get_id(dev);
         u8 reg;
 
@@ -362,7 +350,7 @@ static int tps6507x_dcdc_get_voltage(struct regulator_dev *dev)
                 return -EINVAL;
         }
 
-        data = tps_6507x_reg_read(tps, reg);
+        data = tps6507x_pmic_reg_read(tps, reg);
         if (data < 0)
                 return data;
 
@@ -370,10 +358,10 @@ static int tps6507x_dcdc_get_voltage(struct regulator_dev *dev)
         return tps->info[dcdc]->table[data] * 1000;
 }
 
-static int tps6507x_dcdc_set_voltage(struct regulator_dev *dev,
+static int tps6507x_pmic_dcdc_set_voltage(struct regulator_dev *dev,
                                 int min_uV, int max_uV)
 {
-        struct tps_pmic *tps = rdev_get_drvdata(dev);
+        struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
         int data, vsel, dcdc = rdev_get_id(dev);
         u8 reg;
 
@@ -411,19 +399,19 @@ static int tps6507x_dcdc_set_voltage(struct regulator_dev *dev,
         if (vsel == tps->info[dcdc]->table_len)
                 return -EINVAL;
 
-        data = tps_6507x_reg_read(tps, reg);
+        data = tps6507x_pmic_reg_read(tps, reg);
         if (data < 0)
                 return data;
 
         data &= ~TPS6507X_DEFDCDCX_DCDC_MASK;
         data |= vsel;
 
-        return tps_6507x_reg_write(tps, reg, data);
+        return tps6507x_pmic_reg_write(tps, reg, data);
 }
 
-static int tps6507x_ldo_get_voltage(struct regulator_dev *dev)
+static int tps6507x_pmic_ldo_get_voltage(struct regulator_dev *dev)
 {
-        struct tps_pmic *tps = rdev_get_drvdata(dev);
+        struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
         int data, ldo = rdev_get_id(dev);
         u8 reg, mask;
 
@@ -437,7 +425,7 @@ static int tps6507x_ldo_get_voltage(struct regulator_dev *dev)
                                 TPS6507X_REG_DEFLDO2_LDO2_MASK);
         }
 
-        data = tps_6507x_reg_read(tps, reg);
+        data = tps6507x_pmic_reg_read(tps, reg);
         if (data < 0)
                 return data;
 
@@ -445,10 +433,10 @@ static int tps6507x_ldo_get_voltage(struct regulator_dev *dev)
         return tps->info[ldo]->table[data] * 1000;
 }
 
-static int tps6507x_ldo_set_voltage(struct regulator_dev *dev,
+static int tps6507x_pmic_ldo_set_voltage(struct regulator_dev *dev,
                                 int min_uV, int max_uV)
 {
-        struct tps_pmic *tps = rdev_get_drvdata(dev);
+        struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
         int data, vsel, ldo = rdev_get_id(dev);
         u8 reg, mask;
 
@@ -479,20 +467,20 @@ static int tps6507x_ldo_set_voltage(struct regulator_dev *dev,
         if (vsel == tps->info[ldo]->table_len)
                 return -EINVAL;
 
-        data = tps_6507x_reg_read(tps, reg);
+        data = tps6507x_pmic_reg_read(tps, reg);
         if (data < 0)
                 return data;
 
         data &= ~mask;
         data |= vsel;
 
-        return tps_6507x_reg_write(tps, reg, data);
+        return tps6507x_pmic_reg_write(tps, reg, data);
 }
 
-static int tps6507x_dcdc_list_voltage(struct regulator_dev *dev,
+static int tps6507x_pmic_dcdc_list_voltage(struct regulator_dev *dev,
                                         unsigned selector)
 {
-        struct tps_pmic *tps = rdev_get_drvdata(dev);
+        struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
         int dcdc = rdev_get_id(dev);
 
         if (dcdc < TPS6507X_DCDC_1 || dcdc > TPS6507X_DCDC_3)
@@ -504,10 +492,10 @@ static int tps6507x_dcdc_list_voltage(struct regulator_dev *dev,
                 return tps->info[dcdc]->table[selector] * 1000;
 }
 
-static int tps6507x_ldo_list_voltage(struct regulator_dev *dev,
+static int tps6507x_pmic_ldo_list_voltage(struct regulator_dev *dev,
                                         unsigned selector)
 {
-        struct tps_pmic *tps = rdev_get_drvdata(dev);
+        struct tps6507x_pmic *tps = rdev_get_drvdata(dev);
         int ldo = rdev_get_id(dev);
 
         if (ldo < TPS6507X_LDO_1 || ldo > TPS6507X_LDO_2)
@@ -520,47 +508,54 @@ static int tps6507x_ldo_list_voltage(struct regulator_dev *dev,
 }
 
 /* Operations permitted on VDCDCx */
-static struct regulator_ops tps6507x_dcdc_ops = {
-        .is_enabled = tps6507x_dcdc_is_enabled,
-        .enable = tps6507x_dcdc_enable,
-        .disable = tps6507x_dcdc_disable,
-        .get_voltage = tps6507x_dcdc_get_voltage,
-        .set_voltage = tps6507x_dcdc_set_voltage,
-        .list_voltage = tps6507x_dcdc_list_voltage,
+static struct regulator_ops tps6507x_pmic_dcdc_ops = {
+        .is_enabled = tps6507x_pmic_dcdc_is_enabled,
+        .enable = tps6507x_pmic_dcdc_enable,
+        .disable = tps6507x_pmic_dcdc_disable,
+        .get_voltage = tps6507x_pmic_dcdc_get_voltage,
+        .set_voltage = tps6507x_pmic_dcdc_set_voltage,
+        .list_voltage = tps6507x_pmic_dcdc_list_voltage,
 };
 
 /* Operations permitted on LDOx */
-static struct regulator_ops tps6507x_ldo_ops = {
-        .is_enabled = tps6507x_ldo_is_enabled,
-        .enable = tps6507x_ldo_enable,
-        .disable = tps6507x_ldo_disable,
-        .get_voltage = tps6507x_ldo_get_voltage,
-        .set_voltage = tps6507x_ldo_set_voltage,
-        .list_voltage = tps6507x_ldo_list_voltage,
+static struct regulator_ops tps6507x_pmic_ldo_ops = {
+        .is_enabled = tps6507x_pmic_ldo_is_enabled,
+        .enable = tps6507x_pmic_ldo_enable,
+        .disable = tps6507x_pmic_ldo_disable,
+        .get_voltage = tps6507x_pmic_ldo_get_voltage,
+        .set_voltage = tps6507x_pmic_ldo_set_voltage,
+        .list_voltage = tps6507x_pmic_ldo_list_voltage,
 };
 
-static int __devinit tps_6507x_probe(struct i2c_client *client,
-                                     const struct i2c_device_id *id)
+static __devinit
+int tps6507x_pmic_probe(struct platform_device *pdev)
 {
+        struct tps6507x_dev *tps6507x_dev = dev_get_drvdata(pdev->dev.parent);
         static int desc_id;
-        const struct tps_info *info = (void *)id->driver_data;
+        const struct tps_info *info = &tps6507x_pmic_regs[0];
         struct regulator_init_data *init_data;
         struct regulator_dev *rdev;
-        struct tps_pmic *tps;
+        struct tps6507x_pmic *tps;
+        struct tps6507x_board *tps_board;
         int i;
         int error;
 
-        if (!i2c_check_functionality(client->adapter,
-                                I2C_FUNC_SMBUS_BYTE_DATA))
-                return -EIO;
+        /**
+         * tps_board points to pmic related constants
+         * coming from the board-evm file.
+         */
+
+        tps_board = dev_get_platdata(tps6507x_dev->dev);
+        if (!tps_board)
+                return -EINVAL;
 
         /**
          * init_data points to array of regulator_init structures
          * coming from the board-evm file.
          */
-        init_data = client->dev.platform_data;
+        init_data = tps_board->tps6507x_pmic_init_data;
         if (!init_data)
-                return -EIO;
+                return -EINVAL;
 
         tps = kzalloc(sizeof(*tps), GFP_KERNEL);
         if (!tps)
@@ -569,7 +564,7 @@ static int __devinit tps_6507x_probe(struct i2c_client *client,
         mutex_init(&tps->io_lock);
 
         /* common for all regulators */
-        tps->client = client;
+        tps->mfd = tps6507x_dev;
 
         for (i = 0; i < TPS6507X_NUM_REGULATOR; i++, info++, init_data++) {
                 /* Register the regulators */
@@ -578,15 +573,16 @@ static int __devinit tps_6507x_probe(struct i2c_client *client,
                 tps->desc[i].id = desc_id++;
                 tps->desc[i].n_voltages = num_voltages[i];
                 tps->desc[i].ops = (i > TPS6507X_DCDC_3 ?
-                                &tps6507x_ldo_ops : &tps6507x_dcdc_ops);
+                &tps6507x_pmic_ldo_ops : &tps6507x_pmic_dcdc_ops);
                 tps->desc[i].type = REGULATOR_VOLTAGE;
                 tps->desc[i].owner = THIS_MODULE;
 
                 rdev = regulator_register(&tps->desc[i],
-                                        &client->dev, init_data, tps);
+                                        tps6507x_dev->dev, init_data, tps);
                 if (IS_ERR(rdev)) {
-                        dev_err(&client->dev, "failed to register %s\n",
-                                id->name);
+                        dev_err(tps6507x_dev->dev,
+                                "failed to register %s regulator\n",
+                                pdev->name);
                         error = PTR_ERR(rdev);
                         goto fail;
                 }
@@ -595,7 +591,7 @@ static int __devinit tps_6507x_probe(struct i2c_client *client,
                 tps->rdev[i] = rdev;
         }
 
-        i2c_set_clientdata(client, tps);
+        tps6507x_dev->pmic = tps;
 
         return 0;
 
@@ -608,19 +604,17 @@ fail:
 }
 
 /**
- * tps_6507x_remove - TPS6507x driver i2c remove handler
+ * tps6507x_remove - TPS6507x driver i2c remove handler
  * @client: i2c driver client device structure
  *
  * Unregister TPS driver as an i2c client device driver
  */
-static int __devexit tps_6507x_remove(struct i2c_client *client)
+static int __devexit tps6507x_pmic_remove(struct platform_device *pdev)
 {
-        struct tps_pmic *tps = i2c_get_clientdata(client);
+        struct tps6507x_dev *tps6507x_dev = platform_get_drvdata(pdev);
+        struct tps6507x_pmic *tps = tps6507x_dev->pmic;
         int i;
 
-        /* clear the client data in i2c */
-        i2c_set_clientdata(client, NULL);
-
         for (i = 0; i < TPS6507X_NUM_REGULATOR; i++)
                 regulator_unregister(tps->rdev[i]);
 
@@ -629,83 +623,38 @@ static int __devexit tps_6507x_remove(struct i2c_client *client)
         return 0;
 }
 
-static const struct tps_info tps6507x_regs[] = {
-        {
-                .name = "VDCDC1",
-                .min_uV = 725000,
-                .max_uV = 3300000,
-                .table_len = ARRAY_SIZE(VDCDCx_VSEL_table),
-                .table = VDCDCx_VSEL_table,
-        },
-        {
-                .name = "VDCDC2",
-                .min_uV = 725000,
-                .max_uV = 3300000,
-                .table_len = ARRAY_SIZE(VDCDCx_VSEL_table),
-                .table = VDCDCx_VSEL_table,
-        },
-        {
-                .name = "VDCDC3",
-                .min_uV = 725000,
-                .max_uV = 3300000,
-                .table_len = ARRAY_SIZE(VDCDCx_VSEL_table),
-                .table = VDCDCx_VSEL_table,
-        },
-        {
-                .name = "LDO1",
-                .min_uV = 1000000,
-                .max_uV = 3300000,
-                .table_len = ARRAY_SIZE(LDO1_VSEL_table),
-                .table = LDO1_VSEL_table,
-        },
-        {
-                .name = "LDO2",
-                .min_uV = 725000,
-                .max_uV = 3300000,
-                .table_len = ARRAY_SIZE(LDO2_VSEL_table),
-                .table = LDO2_VSEL_table,
-        },
-};
-
-static const struct i2c_device_id tps_6507x_id[] = {
-        {.name = "tps6507x",
-        .driver_data = (unsigned long) tps6507x_regs,},
-        { },
-};
-MODULE_DEVICE_TABLE(i2c, tps_6507x_id);
-
-static struct i2c_driver tps_6507x_i2c_driver = {
+static struct platform_driver tps6507x_pmic_driver = {
         .driver = {
-                .name = "tps6507x",
+                .name = "tps6507x-pmic",
                 .owner = THIS_MODULE,
         },
-        .probe = tps_6507x_probe,
-        .remove = __devexit_p(tps_6507x_remove),
-        .id_table = tps_6507x_id,
+        .probe = tps6507x_pmic_probe,
+        .remove = __devexit_p(tps6507x_pmic_remove),
 };
 
 /**
- * tps_6507x_init
+ * tps6507x_pmic_init
  *
  * Module init function
  */
-static int __init tps_6507x_init(void)
+static int __init tps6507x_pmic_init(void)
 {
-        return i2c_add_driver(&tps_6507x_i2c_driver);
+        return platform_driver_register(&tps6507x_pmic_driver);
 }
-subsys_initcall(tps_6507x_init);
+subsys_initcall(tps6507x_pmic_init);
 
 /**
- * tps_6507x_cleanup
+ * tps6507x_pmic_cleanup
  *
  * Module exit function
  */
-static void __exit tps_6507x_cleanup(void)
+static void __exit tps6507x_pmic_cleanup(void)
 {
-        i2c_del_driver(&tps_6507x_i2c_driver);
+        platform_driver_unregister(&tps6507x_pmic_driver);
 }
-module_exit(tps_6507x_cleanup);
+module_exit(tps6507x_pmic_cleanup);
 
 MODULE_AUTHOR("Texas Instruments");
 MODULE_DESCRIPTION("TPS6507x voltage regulator driver");
 MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:tps6507x-pmic");
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index f1598324344c..10ba12c8c5e0 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -611,6 +611,13 @@ config RTC_DRV_AB3100
           Select this to enable the ST-Ericsson AB3100 Mixed Signal IC RTC
           support. This chip contains a battery- and capacitor-backed RTC.
 
+config RTC_DRV_AB8500
+        tristate "ST-Ericsson AB8500 RTC"
+        depends on AB8500_CORE
+        help
+          Select this to enable the ST-Ericsson AB8500 power management IC RTC
+          support. This chip contains a battery- and capacitor-backed RTC.
+
 config RTC_DRV_NUC900
         tristate "NUC910/NUC920 RTC driver"
         depends on RTC_CLASS && ARCH_W90X900
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 245311a1348f..5adbba7cf89c 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -18,6 +18,7 @@ rtc-core-$(CONFIG_RTC_INTF_SYSFS) += rtc-sysfs.o
 # Keep the list ordered.
 
 obj-$(CONFIG_RTC_DRV_AB3100)    += rtc-ab3100.o
+obj-$(CONFIG_RTC_DRV_AB8500)    += rtc-ab8500.o
 obj-$(CONFIG_RTC_DRV_AT32AP700X)+= rtc-at32ap700x.o
 obj-$(CONFIG_RTC_DRV_AT91RM9200)+= rtc-at91rm9200.o
 obj-$(CONFIG_RTC_DRV_AT91SAM9)  += rtc-at91sam9.o
diff --git a/drivers/rtc/rtc-ab3100.c b/drivers/rtc/rtc-ab3100.c
index 4704aac2b5af..d26780ea254b 100644
--- a/drivers/rtc/rtc-ab3100.c
+++ b/drivers/rtc/rtc-ab3100.c
@@ -9,7 +9,7 @@
 #include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/rtc.h>
-#include <linux/mfd/ab3100.h>
+#include <linux/mfd/abx500.h>
 
 /* Clock rate in Hz */
 #define AB3100_RTC_CLOCK_RATE   32768
@@ -45,7 +45,6 @@
  */
 static int ab3100_rtc_set_mmss(struct device *dev, unsigned long secs)
 {
-        struct ab3100 *ab3100_data = dev_get_drvdata(dev);
         u8 regs[] = {AB3100_TI0, AB3100_TI1, AB3100_TI2,
                      AB3100_TI3, AB3100_TI4, AB3100_TI5};
         unsigned char buf[6];
@@ -61,27 +60,26 @@ static int ab3100_rtc_set_mmss(struct device *dev, unsigned long secs)
         buf[5] = (fat_time >> 40) & 0xFF;
 
         for (i = 0; i < 6; i++) {
-                err = ab3100_set_register_interruptible(ab3100_data,
+                err = abx500_set_register_interruptible(dev, 0,
                                                         regs[i], buf[i]);
                 if (err)
                         return err;
         }
 
         /* Set the flag to mark that the clock is now set */
-        return ab3100_mask_and_set_register_interruptible(ab3100_data,
+        return abx500_mask_and_set_register_interruptible(dev, 0,
                                                           AB3100_RTC,
-                                                          0xFE, 0x01);
+                                                          0x01, 0x01);
 
 }
 
 static int ab3100_rtc_read_time(struct device *dev, struct rtc_time *tm)
 {
-        struct ab3100 *ab3100_data = dev_get_drvdata(dev);
         unsigned long time;
         u8 rtcval;
         int err;
 
-        err = ab3100_get_register_interruptible(ab3100_data,
+        err = abx500_get_register_interruptible(dev, 0,
                                                 AB3100_RTC, &rtcval);
         if (err)
                 return err;
@@ -94,7 +92,7 @@ static int ab3100_rtc_read_time(struct device *dev, struct rtc_time *tm)
                 u8 buf[6];
 
                 /* Read out time registers */
-                err = ab3100_get_register_page_interruptible(ab3100_data,
+                err = abx500_get_register_page_interruptible(dev, 0,
                                                              AB3100_TI0,
                                                              buf, 6);
                 if (err != 0)
@@ -114,7 +112,6 @@ static int ab3100_rtc_read_time(struct device *dev, struct rtc_time *tm)
 
 static int ab3100_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 {
-        struct ab3100 *ab3100_data = dev_get_drvdata(dev);
         unsigned long time;
         u64 fat_time;
         u8 buf[6];
@@ -122,7 +119,7 @@ static int ab3100_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
         int err;
 
         /* Figure out if alarm is enabled or not */
-        err = ab3100_get_register_interruptible(ab3100_data,
+        err = abx500_get_register_interruptible(dev, 0,
                                                 AB3100_RTC, &rtcval);
         if (err)
                 return err;
@@ -133,7 +130,7 @@ static int ab3100_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
         /* No idea how this could be represented */
         alarm->pending = 0;
         /* Read out alarm registers, only 4 bytes */
-        err = ab3100_get_register_page_interruptible(ab3100_data,
+        err = abx500_get_register_page_interruptible(dev, 0,
                                                      AB3100_AL0, buf, 4);
         if (err)
                 return err;
@@ -148,7 +145,6 @@ static int ab3100_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 
 static int ab3100_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 {
-        struct ab3100 *ab3100_data = dev_get_drvdata(dev);
         u8 regs[] = {AB3100_AL0, AB3100_AL1, AB3100_AL2, AB3100_AL3};
         unsigned char buf[4];
         unsigned long secs;
@@ -165,21 +161,19 @@ static int ab3100_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
 
         /* Set the alarm */
         for (i = 0; i < 4; i++) {
-                err = ab3100_set_register_interruptible(ab3100_data,
+                err = abx500_set_register_interruptible(dev, 0,
                                                         regs[i], buf[i]);
                 if (err)
                         return err;
         }
         /* Then enable the alarm */
-        return ab3100_mask_and_set_register_interruptible(ab3100_data,
-                                                          AB3100_RTC, ~(1 << 2),
+        return abx500_mask_and_set_register_interruptible(dev, 0,
+                                                          AB3100_RTC, (1 << 2),
                                                           alarm->enabled << 2);
 }
 
 static int ab3100_rtc_irq_enable(struct device *dev, unsigned int enabled)
 {
-        struct ab3100 *ab3100_data = dev_get_drvdata(dev);
-
         /*
          * It's not possible to enable/disable the alarm IRQ for this RTC.
          * It does not actually trigger any IRQ: instead its only function is
@@ -188,12 +182,12 @@ static int ab3100_rtc_irq_enable(struct device *dev, unsigned int enabled)
          * and need to be handled there instead.
          */
         if (enabled)
-                return ab3100_mask_and_set_register_interruptible(ab3100_data,
-                                                    AB3100_RTC, ~(1 << 2),
+                return abx500_mask_and_set_register_interruptible(dev, 0,
+                                                    AB3100_RTC, (1 << 2),
                                                     1 << 2);
         else
-                return ab3100_mask_and_set_register_interruptible(ab3100_data,
-                                                    AB3100_RTC, ~(1 << 2),
+                return abx500_mask_and_set_register_interruptible(dev, 0,
+                                                    AB3100_RTC, (1 << 2),
                                                     0);
 }
 
@@ -210,10 +204,9 @@ static int __init ab3100_rtc_probe(struct platform_device *pdev)
         int err;
         u8 regval;
         struct rtc_device *rtc;
-        struct ab3100 *ab3100_data = platform_get_drvdata(pdev);
 
         /* The first RTC register needs special treatment */
-        err = ab3100_get_register_interruptible(ab3100_data,
+        err = abx500_get_register_interruptible(&pdev->dev, 0,
                                                 AB3100_RTC, &regval);
         if (err) {
                 dev_err(&pdev->dev, "unable to read RTC register\n");
@@ -231,7 +224,7 @@ static int __init ab3100_rtc_probe(struct platform_device *pdev)
                  * This bit remains until RTC power is lost.
                  */
                 regval = 1 | RTC_SETTING;
-                err = ab3100_set_register_interruptible(ab3100_data,
+                err = abx500_set_register_interruptible(&pdev->dev, 0,
                                                         AB3100_RTC, regval);
                 /* Ignore any error on this write */
         }
diff --git a/drivers/rtc/rtc-ab8500.c b/drivers/rtc/rtc-ab8500.c
new file mode 100644
index 000000000000..2fda03125e55
--- /dev/null
+++ b/drivers/rtc/rtc-ab8500.c
@@ -0,0 +1,363 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ * Author: Virupax Sadashivpetimath <virupax.sadashivpetimath@stericsson.com>
+ *
+ * RTC clock driver for the RTC part of the AB8500 Power management chip.
+ * Based on RTC clock driver for the AB3100 Analog Baseband Chip by
+ * Linus Walleij <linus.walleij@stericsson.com>
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/rtc.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/delay.h>
+
+#define AB8500_RTC_SOFF_STAT_REG        0x0F00
+#define AB8500_RTC_CC_CONF_REG          0x0F01
+#define AB8500_RTC_READ_REQ_REG         0x0F02
+#define AB8500_RTC_WATCH_TSECMID_REG    0x0F03
+#define AB8500_RTC_WATCH_TSECHI_REG     0x0F04
+#define AB8500_RTC_WATCH_TMIN_LOW_REG   0x0F05
+#define AB8500_RTC_WATCH_TMIN_MID_REG   0x0F06
+#define AB8500_RTC_WATCH_TMIN_HI_REG    0x0F07
+#define AB8500_RTC_ALRM_MIN_LOW_REG     0x0F08
+#define AB8500_RTC_ALRM_MIN_MID_REG     0x0F09
+#define AB8500_RTC_ALRM_MIN_HI_REG      0x0F0A
+#define AB8500_RTC_STAT_REG             0x0F0B
+#define AB8500_RTC_BKUP_CHG_REG         0x0F0C
+#define AB8500_RTC_FORCE_BKUP_REG       0x0F0D
+#define AB8500_RTC_CALIB_REG            0x0F0E
+#define AB8500_RTC_SWITCH_STAT_REG      0x0F0F
+#define AB8500_REV_REG                  0x1080
+
+/* RtcReadRequest bits */
+#define RTC_READ_REQUEST                0x01
+#define RTC_WRITE_REQUEST               0x02
+
+/* RtcCtrl bits */
+#define RTC_ALARM_ENA                   0x04
+#define RTC_STATUS_DATA                 0x01
+
+#define COUNTS_PER_SEC                  (0xF000 / 60)
+#define AB8500_RTC_EPOCH                2000
+
+static const unsigned long ab8500_rtc_time_regs[] = {
+        AB8500_RTC_WATCH_TMIN_HI_REG, AB8500_RTC_WATCH_TMIN_MID_REG,
+        AB8500_RTC_WATCH_TMIN_LOW_REG, AB8500_RTC_WATCH_TSECHI_REG,
+        AB8500_RTC_WATCH_TSECMID_REG
+};
+
+static const unsigned long ab8500_rtc_alarm_regs[] = {
+        AB8500_RTC_ALRM_MIN_HI_REG, AB8500_RTC_ALRM_MIN_MID_REG,
+        AB8500_RTC_ALRM_MIN_LOW_REG
+};
+
+/* Calculate the seconds from 1970 to 01-01-2000 00:00:00 */
+static unsigned long get_elapsed_seconds(int year)
+{
+        unsigned long secs;
+        struct rtc_time tm = {
+                .tm_year = year - 1900,
+                .tm_mday = 1,
+        };
+
+        /*
+         * This function calculates secs from 1970 and not from
+         * 1900, even if we supply the offset from year 1900.
+         */
+        rtc_tm_to_time(&tm, &secs);
+        return secs;
+}
+
+static int ab8500_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+        struct ab8500 *ab8500 = dev_get_drvdata(dev->parent);
+        unsigned long timeout = jiffies + HZ;
+        int retval, i;
+        unsigned long mins, secs;
+        unsigned char buf[ARRAY_SIZE(ab8500_rtc_time_regs)];
+
+        /* Request a data read */
+        retval = ab8500_write(ab8500, AB8500_RTC_READ_REQ_REG,
+                              RTC_READ_REQUEST);
+        if (retval < 0)
+                return retval;
+
+        /* Early AB8500 chips will not clear the rtc read request bit */
+        if (ab8500->revision == 0) {
+                msleep(1);
+        } else {
+                /* Wait for some cycles after enabling the rtc read in ab8500 */
+                while (time_before(jiffies, timeout)) {
+                        retval = ab8500_read(ab8500, AB8500_RTC_READ_REQ_REG);
+                        if (retval < 0)
+                                return retval;
+
+                        if (!(retval & RTC_READ_REQUEST))
+                                break;
+
+                        msleep(1);
+                }
+        }
+
+        /* Read the Watchtime registers */
+        for (i = 0; i < ARRAY_SIZE(ab8500_rtc_time_regs); i++) {
+                retval = ab8500_read(ab8500, ab8500_rtc_time_regs[i]);
+                if (retval < 0)
+                        return retval;
+                buf[i] = retval;
+        }
+
+        mins = (buf[0] << 16) | (buf[1] << 8) | buf[2];
+
+        secs =  (buf[3] << 8) | buf[4];
+        secs =  secs / COUNTS_PER_SEC;
+        secs =  secs + (mins * 60);
+
+        /* Add back the initially subtracted number of seconds */
+        secs += get_elapsed_seconds(AB8500_RTC_EPOCH);
+
+        rtc_time_to_tm(secs, tm);
+        return rtc_valid_tm(tm);
+}
+
+static int ab8500_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+        struct ab8500 *ab8500 = dev_get_drvdata(dev->parent);
+        int retval, i;
+        unsigned char buf[ARRAY_SIZE(ab8500_rtc_time_regs)];
+        unsigned long no_secs, no_mins, secs = 0;
+
+        if (tm->tm_year < (AB8500_RTC_EPOCH - 1900)) {
+                dev_dbg(dev, "year should be equal to or greater than %d\n",
+                                AB8500_RTC_EPOCH);
+                return -EINVAL;
+        }
+
+        /* Get the number of seconds since 1970 */
+        rtc_tm_to_time(tm, &secs);
+
+        /*
+         * Convert it to the number of seconds since 01-01-2000 00:00:00, since
+         * we only have a small counter in the RTC.
+         */
+        secs -= get_elapsed_seconds(AB8500_RTC_EPOCH);
+
+        no_mins = secs / 60;
+
+        no_secs = secs % 60;
+        /* Make the seconds count as per the RTC resolution */
+        no_secs = no_secs * COUNTS_PER_SEC;
+
+        buf[4] = no_secs & 0xFF;
+        buf[3] = (no_secs >> 8) & 0xFF;
+
+        buf[2] = no_mins & 0xFF;
+        buf[1] = (no_mins >> 8) & 0xFF;
+        buf[0] = (no_mins >> 16) & 0xFF;
+
+        for (i = 0; i < ARRAY_SIZE(ab8500_rtc_time_regs); i++) {
+                retval = ab8500_write(ab8500, ab8500_rtc_time_regs[i], buf[i]);
+                if (retval < 0)
+                        return retval;
+        }
+
+        /* Request a data write */
+        return ab8500_write(ab8500, AB8500_RTC_READ_REQ_REG, RTC_WRITE_REQUEST);
+}
+
+static int ab8500_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alarm)
+{
+        struct ab8500 *ab8500 = dev_get_drvdata(dev->parent);
+        int retval, i;
+        int rtc_ctrl;
+        unsigned char buf[ARRAY_SIZE(ab8500_rtc_alarm_regs)];
+        unsigned long secs, mins;
+
+        /* Check if the alarm is enabled or not */
+        rtc_ctrl = ab8500_read(ab8500, AB8500_RTC_STAT_REG);
+        if (rtc_ctrl < 0)
+                return rtc_ctrl;
+
+        if (rtc_ctrl & RTC_ALARM_ENA)
+                alarm->enabled = 1;
+        else
+                alarm->enabled = 0;
+
+        alarm->pending = 0;
+
+        for (i = 0; i < ARRAY_SIZE(ab8500_rtc_alarm_regs); i++) {
+                retval = ab8500_read(ab8500, ab8500_rtc_alarm_regs[i]);
+                if (retval < 0)
+                        return retval;
+                buf[i] = retval;
+        }
+
+        mins = (buf[0] << 16) | (buf[1] << 8) | (buf[2]);
+        secs = mins * 60;
+
+        /* Add back the initially subtracted number of seconds */
+        secs += get_elapsed_seconds(AB8500_RTC_EPOCH);
+
+        rtc_time_to_tm(secs, &alarm->time);
+
+        return rtc_valid_tm(&alarm->time);
+}
+
+static int ab8500_rtc_irq_enable(struct device *dev, unsigned int enabled)
+{
+        struct ab8500 *ab8500 = dev_get_drvdata(dev->parent);
+
+        return ab8500_set_bits(ab8500, AB8500_RTC_STAT_REG, RTC_ALARM_ENA,
+                               enabled ? RTC_ALARM_ENA : 0);
+}
+
+static int ab8500_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alarm)
+{
+        struct ab8500 *ab8500 = dev_get_drvdata(dev->parent);
+        int retval, i;
+        unsigned char buf[ARRAY_SIZE(ab8500_rtc_alarm_regs)];
+        unsigned long mins, secs = 0;
+
+        if (alarm->time.tm_year < (AB8500_RTC_EPOCH - 1900)) {
+                dev_dbg(dev, "year should be equal to or greater than %d\n",
+                                AB8500_RTC_EPOCH);
+                return -EINVAL;
+        }
+
+        /* Get the number of seconds since 1970 */
+        rtc_tm_to_time(&alarm->time, &secs);
+
+        /*
+         * Convert it to the number of seconds since 01-01-2000 00:00:00, since
+         * we only have a small counter in the RTC.
+         */
+        secs -= get_elapsed_seconds(AB8500_RTC_EPOCH);
+
+        mins = secs / 60;
+
+        buf[2] = mins & 0xFF;
+        buf[1] = (mins >> 8) & 0xFF;
+        buf[0] = (mins >> 16) & 0xFF;
+
+        /* Set the alarm time */
+        for (i = 0; i < ARRAY_SIZE(ab8500_rtc_alarm_regs); i++) {
+                retval = ab8500_write(ab8500, ab8500_rtc_alarm_regs[i], buf[i]);
+                if (retval < 0)
+                        return retval;
+        }
+
+        return ab8500_rtc_irq_enable(dev, alarm->enabled);
+}
+
+static irqreturn_t rtc_alarm_handler(int irq, void *data)
+{
+        struct rtc_device *rtc = data;
+        unsigned long events = RTC_IRQF | RTC_AF;
+
+        dev_dbg(&rtc->dev, "%s\n", __func__);
+        rtc_update_irq(rtc, 1, events);
+
+        return IRQ_HANDLED;
+}
+
+static const struct rtc_class_ops ab8500_rtc_ops = {
+        .read_time              = ab8500_rtc_read_time,
+        .set_time               = ab8500_rtc_set_time,
+        .read_alarm             = ab8500_rtc_read_alarm,
+        .set_alarm              = ab8500_rtc_set_alarm,
+        .alarm_irq_enable       = ab8500_rtc_irq_enable,
+};
+
+static int __devinit ab8500_rtc_probe(struct platform_device *pdev)
+{
+        struct ab8500 *ab8500 = dev_get_drvdata(pdev->dev.parent);
+        int err;
+        struct rtc_device *rtc;
+        int rtc_ctrl;
+        int irq;
+
+        irq = platform_get_irq_byname(pdev, "ALARM");
+        if (irq < 0)
+                return irq;
+
+        /* For RTC supply test */
+        err = ab8500_set_bits(ab8500, AB8500_RTC_STAT_REG, RTC_STATUS_DATA,
+                        RTC_STATUS_DATA);
+        if (err < 0)
+                return err;
+
+        /* Wait for reset by the PorRtc */
+        msleep(1);
+
+        rtc_ctrl = ab8500_read(ab8500, AB8500_RTC_STAT_REG);
+        if (rtc_ctrl < 0)
+                return rtc_ctrl;
+
+        /* Check if the RTC Supply fails */
+        if (!(rtc_ctrl & RTC_STATUS_DATA)) {
+                dev_err(&pdev->dev, "RTC supply failure\n");
+                return -ENODEV;
+        }
+
+        rtc = rtc_device_register("ab8500-rtc", &pdev->dev, &ab8500_rtc_ops,
+                        THIS_MODULE);
+        if (IS_ERR(rtc)) {
+                dev_err(&pdev->dev, "Registration failed\n");
+                err = PTR_ERR(rtc);
+                return err;
+        }
+
+        err = request_threaded_irq(irq, NULL, rtc_alarm_handler, 0,
+                                   "ab8500-rtc", rtc);
+        if (err < 0) {
+                rtc_device_unregister(rtc);
+                return err;
+        }
+
+        platform_set_drvdata(pdev, rtc);
+
+        return 0;
+}
+
+static int __devexit ab8500_rtc_remove(struct platform_device *pdev)
+{
+        struct rtc_device *rtc = platform_get_drvdata(pdev);
+        int irq = platform_get_irq_byname(pdev, "ALARM");
+
+        free_irq(irq, rtc);
+        rtc_device_unregister(rtc);
+        platform_set_drvdata(pdev, NULL);
+
+        return 0;
+}
+
+static struct platform_driver ab8500_rtc_driver = {
+        .driver = {
+                .name = "ab8500-rtc",
+                .owner = THIS_MODULE,
+        },
+        .probe  = ab8500_rtc_probe,
+        .remove = __devexit_p(ab8500_rtc_remove),
+};
+
+static int __init ab8500_rtc_init(void)
+{
+        return platform_driver_register(&ab8500_rtc_driver);
+}
+
+static void __exit ab8500_rtc_exit(void)
+{
+        platform_driver_unregister(&ab8500_rtc_driver);
+}
+
+module_init(ab8500_rtc_init);
+module_exit(ab8500_rtc_exit);
+MODULE_AUTHOR("Virupax Sadashivpetimath <virupax.sadashivpetimath@stericsson.com>");
+MODULE_DESCRIPTION("AB8500 RTC Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/rtc/rtc-ds1374.c b/drivers/rtc/rtc-ds1374.c
index 61945734ad00..1f0007fd4431 100644
--- a/drivers/rtc/rtc-ds1374.c
+++ b/drivers/rtc/rtc-ds1374.c
@@ -403,7 +403,6 @@ out_irq:
                 free_irq(client->irq, client);
 
 out_free:
-        i2c_set_clientdata(client, NULL);
         kfree(ds1374);
         return ret;
 }
@@ -422,7 +421,6 @@ static int __devexit ds1374_remove(struct i2c_client *client)
         }
 
         rtc_device_unregister(ds1374->rtc);
-        i2c_set_clientdata(client, NULL);
         kfree(ds1374);
         return 0;
 }
diff --git a/drivers/rtc/rtc-m41t80.c b/drivers/rtc/rtc-m41t80.c
index 038095d99976..6dc4e6241418 100644
--- a/drivers/rtc/rtc-m41t80.c
+++ b/drivers/rtc/rtc-m41t80.c
@@ -595,10 +595,6 @@ static void wdt_disable(void)
 static ssize_t wdt_write(struct file *file, const char __user *buf,
                          size_t count, loff_t *ppos)
 {
-        /*  Can't seek (pwrite) on this device
-        if (ppos != &file->f_pos)
-        return -ESPIPE;
-        */
         if (count) {
                 wdt_ping();
                 return 1;
@@ -707,7 +703,7 @@ static int wdt_open(struct inode *inode, struct file *file)
                  */
                 wdt_is_open = 1;
                 unlock_kernel();
-                return 0;
+                return nonseekable_open(inode, file);
         }
         return -ENODEV;
 }
diff --git a/drivers/rtc/rtc-mpc5121.c b/drivers/rtc/rtc-mpc5121.c
index f0dbf9cb8f9c..db5d8c416d26 100644
--- a/drivers/rtc/rtc-mpc5121.c
+++ b/drivers/rtc/rtc-mpc5121.c
@@ -279,7 +279,7 @@ static int __devinit mpc5121_rtc_probe(struct of_device *op,
         if (!rtc)
                 return -ENOMEM;
 
-        rtc->regs = of_iomap(op->node, 0);
+        rtc->regs = of_iomap(op->dev.of_node, 0);
         if (!rtc->regs) {
                 dev_err(&op->dev, "%s: couldn't map io space\n", __func__);
                 err = -ENOSYS;
@@ -290,7 +290,7 @@ static int __devinit mpc5121_rtc_probe(struct of_device *op,
 
         dev_set_drvdata(&op->dev, rtc);
 
-        rtc->irq = irq_of_parse_and_map(op->node, 1);
+        rtc->irq = irq_of_parse_and_map(op->dev.of_node, 1);
         err = request_irq(rtc->irq, mpc5121_rtc_handler, IRQF_DISABLED,
                                                 "mpc5121-rtc", &op->dev);
         if (err) {
@@ -299,7 +299,7 @@ static int __devinit mpc5121_rtc_probe(struct of_device *op,
                 goto out_dispose;
         }
 
-        rtc->irq_periodic = irq_of_parse_and_map(op->node, 0);
+        rtc->irq_periodic = irq_of_parse_and_map(op->dev.of_node, 0);
         err = request_irq(rtc->irq_periodic, mpc5121_rtc_handler_upd,
                                 IRQF_DISABLED, "mpc5121-rtc_upd", &op->dev);
         if (err) {
@@ -365,9 +365,11 @@ static struct of_device_id mpc5121_rtc_match[] __devinitdata = {
 };
 
 static struct of_platform_driver mpc5121_rtc_driver = {
-        .owner = THIS_MODULE,
-        .name = "mpc5121-rtc",
-        .match_table = mpc5121_rtc_match,
+        .driver = {
+                .name = "mpc5121-rtc",
+                .owner = THIS_MODULE,
+                .of_match_table = mpc5121_rtc_match,
+        },
         .probe = mpc5121_rtc_probe,
         .remove = __devexit_p(mpc5121_rtc_remove),
 };
diff --git a/drivers/rtc/rtc-rx8025.c b/drivers/rtc/rtc-rx8025.c
index b65c82f792d9..789f62f9b47d 100644
--- a/drivers/rtc/rtc-rx8025.c
+++ b/drivers/rtc/rtc-rx8025.c
@@ -632,7 +632,6 @@ errout_reg:
         rtc_device_unregister(rx8025->rtc);
 
 errout_free:
-        i2c_set_clientdata(client, NULL);
         kfree(rx8025);
 
 errout:
@@ -656,7 +655,6 @@ static int __devexit rx8025_remove(struct i2c_client *client)
 
         rx8025_sysfs_unregister(&client->dev);
         rtc_device_unregister(rx8025->rtc);
-        i2c_set_clientdata(client, NULL);
         kfree(rx8025);
         return 0;
 }
diff --git a/drivers/rtc/rtc-s35390a.c b/drivers/rtc/rtc-s35390a.c
index def4d396d0b0..f789e002c9b0 100644
--- a/drivers/rtc/rtc-s35390a.c
+++ b/drivers/rtc/rtc-s35390a.c
@@ -275,7 +275,6 @@ exit_dummy:
                 if (s35390a->client[i])
                         i2c_unregister_device(s35390a->client[i]);
         kfree(s35390a);
-        i2c_set_clientdata(client, NULL);
 
 exit:
         return err;
@@ -292,7 +291,6 @@ static int s35390a_remove(struct i2c_client *client)
 
         rtc_device_unregister(s35390a->rtc);
         kfree(s35390a);
-        i2c_set_clientdata(client, NULL);
 
         return 0;
 }
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index e5972b2c17b7..70b68d35f969 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -495,8 +495,6 @@ static int __devinit s3c_rtc_probe(struct platform_device *pdev)
         pr_debug("s3c2410_rtc: RTCCON=%02x\n",
                  readb(s3c_rtc_base + S3C2410_RTCCON));
 
-        s3c_rtc_setfreq(&pdev->dev, 1);
-
         device_init_wakeup(&pdev->dev, 1);
 
         /* register RTC and exit */
@@ -510,14 +508,17 @@ static int __devinit s3c_rtc_probe(struct platform_device *pdev)
                 goto err_nortc;
         }
 
+        s3c_rtc_cpu_type = platform_get_device_id(pdev)->driver_data;
+
         if (s3c_rtc_cpu_type == TYPE_S3C64XX)
                 rtc->max_user_freq = 32768;
         else
                 rtc->max_user_freq = 128;
 
-        s3c_rtc_cpu_type = platform_get_device_id(pdev)->driver_data;
-
         platform_set_drvdata(pdev, rtc);
+
+        s3c_rtc_setfreq(&pdev->dev, 1);
+
         return 0;
 
  err_nortc:
diff --git a/drivers/s390/block/dasd.c b/drivers/s390/block/dasd.c
index 0e86247d791e..33975e922d65 100644
--- a/drivers/s390/block/dasd.c
+++ b/drivers/s390/block/dasd.c
@@ -1186,6 +1186,29 @@ void dasd_int_handler(struct ccw_device *cdev, unsigned long intparm,
         dasd_schedule_device_bh(device);
 }
 
+enum uc_todo dasd_generic_uc_handler(struct ccw_device *cdev, struct irb *irb)
+{
+        struct dasd_device *device;
+
+        device = dasd_device_from_cdev_locked(cdev);
+
+        if (IS_ERR(device))
+                goto out;
+        if (test_bit(DASD_FLAG_OFFLINE, &device->flags) ||
+           device->state != device->target ||
+           !device->discipline->handle_unsolicited_interrupt){
+                dasd_put_device(device);
+                goto out;
+        }
+
+        dasd_device_clear_timer(device);
+        device->discipline->handle_unsolicited_interrupt(device, irb);
+        dasd_put_device(device);
+out:
+        return UC_TODO_RETRY;
+}
+EXPORT_SYMBOL_GPL(dasd_generic_uc_handler);
+
 /*
  * If we have an error on a dasd_block layer request then we cancel
  * and return all further requests from the same dasd_block as well.
diff --git a/drivers/s390/block/dasd_eckd.c b/drivers/s390/block/dasd_eckd.c
index 5b1cd8d6e971..ab84da5592e8 100644
--- a/drivers/s390/block/dasd_eckd.c
+++ b/drivers/s390/block/dasd_eckd.c
@@ -3436,6 +3436,7 @@ static struct ccw_driver dasd_eckd_driver = {
         .freeze      = dasd_generic_pm_freeze,
         .thaw        = dasd_generic_restore_device,
         .restore     = dasd_generic_restore_device,
+        .uc_handler  = dasd_generic_uc_handler,
 };
 
 /*
diff --git a/drivers/s390/block/dasd_int.h b/drivers/s390/block/dasd_int.h
index 32fac186ba3f..49b431d135e0 100644
--- a/drivers/s390/block/dasd_int.h
+++ b/drivers/s390/block/dasd_int.h
@@ -617,6 +617,7 @@ int dasd_generic_notify(struct ccw_device *, int);
 void dasd_generic_handle_state_change(struct dasd_device *);
 int dasd_generic_pm_freeze(struct ccw_device *);
 int dasd_generic_restore_device(struct ccw_device *);
+enum uc_todo dasd_generic_uc_handler(struct ccw_device *, struct irb *);
 
 int dasd_generic_read_dev_chars(struct dasd_device *, int, void *, int);
 char *dasd_get_sense(struct irb *);
diff --git a/drivers/s390/cio/ccwgroup.c b/drivers/s390/cio/ccwgroup.c
index 5f97ea2ee6b1..97b25d68e3e7 100644
--- a/drivers/s390/cio/ccwgroup.c
+++ b/drivers/s390/cio/ccwgroup.c
@@ -123,8 +123,10 @@ ccwgroup_release (struct device *dev)
 
         for (i = 0; i < gdev->count; i++) {
                 if (gdev->cdev[i]) {
+                        spin_lock_irq(gdev->cdev[i]->ccwlock);
                         if (dev_get_drvdata(&gdev->cdev[i]->dev) == gdev)
                                 dev_set_drvdata(&gdev->cdev[i]->dev, NULL);
+                        spin_unlock_irq(gdev->cdev[i]->ccwlock);
                         put_device(&gdev->cdev[i]->dev);
                 }
         }
@@ -262,11 +264,14 @@ int ccwgroup_create_from_string(struct device *root, unsigned int creator_id,
                         goto error;
                 }
                 /* Don't allow a device to belong to more than one group. */
+                spin_lock_irq(gdev->cdev[i]->ccwlock);
                 if (dev_get_drvdata(&gdev->cdev[i]->dev)) {
+                        spin_unlock_irq(gdev->cdev[i]->ccwlock);
                         rc = -EINVAL;
                         goto error;
                 }
                 dev_set_drvdata(&gdev->cdev[i]->dev, gdev);
+                spin_unlock_irq(gdev->cdev[i]->ccwlock);
         }
         /* Check for sufficient number of bus ids. */
         if (i < num_devices && !curr_buf) {
@@ -303,8 +308,10 @@ int ccwgroup_create_from_string(struct device *root, unsigned int creator_id,
 error:
         for (i = 0; i < num_devices; i++)
                 if (gdev->cdev[i]) {
+                        spin_lock_irq(gdev->cdev[i]->ccwlock);
                         if (dev_get_drvdata(&gdev->cdev[i]->dev) == gdev)
                                 dev_set_drvdata(&gdev->cdev[i]->dev, NULL);
+                        spin_unlock_irq(gdev->cdev[i]->ccwlock);
                         put_device(&gdev->cdev[i]->dev);
                         gdev->cdev[i] = NULL;
                 }
diff --git a/drivers/s390/cio/ccwreq.c b/drivers/s390/cio/ccwreq.c
index 37df42af05ec..7f206ed44fdf 100644
--- a/drivers/s390/cio/ccwreq.c
+++ b/drivers/s390/cio/ccwreq.c
@@ -159,6 +159,7 @@ static enum io_status ccwreq_status(struct ccw_device *cdev, struct irb *lcirb)
 {
         struct irb *irb = &cdev->private->irb;
         struct cmd_scsw *scsw = &irb->scsw.cmd;
+        enum uc_todo todo;
 
         /* Perform BASIC SENSE if needed. */
         if (ccw_device_accumulate_and_sense(cdev, lcirb))
@@ -178,6 +179,20 @@ static enum io_status ccwreq_status(struct ccw_device *cdev, struct irb *lcirb)
                 /* Check for command reject. */
                 if (irb->ecw[0] & SNS0_CMD_REJECT)
                         return IO_REJECTED;
+                /* Ask the driver what to do */
+                if (cdev->drv && cdev->drv->uc_handler) {
+                        todo = cdev->drv->uc_handler(cdev, lcirb);
+                        switch (todo) {
+                        case UC_TODO_RETRY:
+                                return IO_STATUS_ERROR;
+                        case UC_TODO_RETRY_ON_NEW_PATH:
+                                return IO_PATH_ERROR;
+                        case UC_TODO_STOP:
+                                return IO_REJECTED;
+                        default:
+                                return IO_STATUS_ERROR;
+                        }
+                }
                 /* Assume that unexpected SENSE data implies an error. */
                 return IO_STATUS_ERROR;
         }
diff --git a/drivers/s390/cio/ioasm.h b/drivers/s390/cio/ioasm.h
index 759262792633..fac06155773f 100644
--- a/drivers/s390/cio/ioasm.h
+++ b/drivers/s390/cio/ioasm.h
@@ -23,21 +23,6 @@ struct tpi_info {
  * Some S390 specific IO instructions as inline
  */
 
-static inline int stsch(struct subchannel_id schid, struct schib *addr)
-{
-        register struct subchannel_id reg1 asm ("1") = schid;
-        int ccode;
-
-        asm volatile(
-                "       stsch   0(%3)\n"
-                "       ipm     %0\n"
-                "       srl     %0,28"
-                : "=d" (ccode), "=m" (*addr)
-                : "d" (reg1), "a" (addr)
-                : "cc");
-        return ccode;
-}
-
 static inline int stsch_err(struct subchannel_id schid, struct schib *addr)
 {
         register struct subchannel_id reg1 asm ("1") = schid;
diff --git a/drivers/s390/cio/itcw.c b/drivers/s390/cio/itcw.c
index 17da9ab932ed..a0ae29564774 100644
--- a/drivers/s390/cio/itcw.c
+++ b/drivers/s390/cio/itcw.c
@@ -42,7 +42,7 @@
  * size_t size;
  *
  * size = itcw_calc_size(1, 2, 0);
- * buffer = kmalloc(size, GFP_DMA);
+ * buffer = kmalloc(size, GFP_KERNEL | GFP_DMA);
  * if (!buffer)
  *      return -ENOMEM;
  * itcw = itcw_init(buffer, size, ITCW_OP_READ, 1, 2, 0);
diff --git a/drivers/scsi/a2091.c b/drivers/scsi/a2091.c
index 308541ff85cf..1bb5d3f0e260 100644
--- a/drivers/scsi/a2091.c
+++ b/drivers/scsi/a2091.c
@@ -1,34 +1,31 @@
 #include <linux/types.h>
-#include <linux/mm.h>
-#include <linux/slab.h>
-#include <linux/blkdev.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/zorro.h>
 
-#include <asm/setup.h>
 #include <asm/page.h>
 #include <asm/pgtable.h>
 #include <asm/amigaints.h>
 #include <asm/amigahw.h>
-#include <linux/zorro.h>
-#include <asm/irq.h>
-#include <linux/spinlock.h>
 
 #include "scsi.h"
-#include <scsi/scsi_host.h>
 #include "wd33c93.h"
 #include "a2091.h"
 
-#include <linux/stat.h>
-
 
-static int a2091_release(struct Scsi_Host *instance);
+struct a2091_hostdata {
+        struct WD33C93_hostdata wh;
+        struct a2091_scsiregs *regs;
+};
 
 static irqreturn_t a2091_intr(int irq, void *data)
 {
         struct Scsi_Host *instance = data;
-        a2091_scsiregs *regs = (a2091_scsiregs *)(instance->base);
-        unsigned int status = regs->ISTR;
+        struct a2091_hostdata *hdata = shost_priv(instance);
+        unsigned int status = hdata->regs->ISTR;
         unsigned long flags;
 
         if (!(status & (ISTR_INT_F | ISTR_INT_P)) || !(status & ISTR_INTS))
@@ -43,38 +40,39 @@ static irqreturn_t a2091_intr(int irq, void *data)
 static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
 {
         struct Scsi_Host *instance = cmd->device->host;
-        struct WD33C93_hostdata *hdata = shost_priv(instance);
-        a2091_scsiregs *regs = (a2091_scsiregs *)(instance->base);
+        struct a2091_hostdata *hdata = shost_priv(instance);
+        struct WD33C93_hostdata *wh = &hdata->wh;
+        struct a2091_scsiregs *regs = hdata->regs;
         unsigned short cntr = CNTR_PDMD | CNTR_INTEN;
         unsigned long addr = virt_to_bus(cmd->SCp.ptr);
 
         /* don't allow DMA if the physical address is bad */
         if (addr & A2091_XFER_MASK) {
-                hdata->dma_bounce_len = (cmd->SCp.this_residual + 511) & ~0x1ff;
-                hdata->dma_bounce_buffer = kmalloc(hdata->dma_bounce_len,
-                                                   GFP_KERNEL);
+                wh->dma_bounce_len = (cmd->SCp.this_residual + 511) & ~0x1ff;
+                wh->dma_bounce_buffer = kmalloc(wh->dma_bounce_len,
+                                                GFP_KERNEL);
 
                 /* can't allocate memory; use PIO */
-                if (!hdata->dma_bounce_buffer) {
-                        hdata->dma_bounce_len = 0;
+                if (!wh->dma_bounce_buffer) {
+                        wh->dma_bounce_len = 0;
                         return 1;
                 }
 
                 /* get the physical address of the bounce buffer */
-                addr = virt_to_bus(hdata->dma_bounce_buffer);
+                addr = virt_to_bus(wh->dma_bounce_buffer);
 
                 /* the bounce buffer may not be in the first 16M of physmem */
                 if (addr & A2091_XFER_MASK) {
                         /* we could use chipmem... maybe later */
-                        kfree(hdata->dma_bounce_buffer);
-                        hdata->dma_bounce_buffer = NULL;
-                        hdata->dma_bounce_len = 0;
+                        kfree(wh->dma_bounce_buffer);
+                        wh->dma_bounce_buffer = NULL;
+                        wh->dma_bounce_len = 0;
                         return 1;
                 }
 
                 if (!dir_in) {
                         /* copy to bounce buffer for a write */
-                        memcpy(hdata->dma_bounce_buffer, cmd->SCp.ptr,
+                        memcpy(wh->dma_bounce_buffer, cmd->SCp.ptr,
                                cmd->SCp.this_residual);
                 }
         }
@@ -84,7 +82,7 @@ static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
                 cntr |= CNTR_DDIR;
 
         /* remember direction */
-        hdata->dma_dir = dir_in;
+        wh->dma_dir = dir_in;
 
         regs->CNTR = cntr;
 
@@ -108,20 +106,21 @@ static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
 static void dma_stop(struct Scsi_Host *instance, struct scsi_cmnd *SCpnt,
                      int status)
 {
-        struct WD33C93_hostdata *hdata = shost_priv(instance);
-        a2091_scsiregs *regs = (a2091_scsiregs *)(instance->base);
+        struct a2091_hostdata *hdata = shost_priv(instance);
+        struct WD33C93_hostdata *wh = &hdata->wh;
+        struct a2091_scsiregs *regs = hdata->regs;
 
         /* disable SCSI interrupts */
         unsigned short cntr = CNTR_PDMD;
 
-        if (!hdata->dma_dir)
+        if (!wh->dma_dir)
                 cntr |= CNTR_DDIR;
 
         /* disable SCSI interrupts */
         regs->CNTR = cntr;
 
         /* flush if we were reading */
-        if (hdata->dma_dir) {
+        if (wh->dma_dir) {
                 regs->FLUSH = 1;
                 while (!(regs->ISTR & ISTR_FE_FLG))
                         ;
@@ -137,95 +136,37 @@ static void dma_stop(struct Scsi_Host *instance, struct scsi_cmnd *SCpnt,
         regs->CNTR = CNTR_PDMD | CNTR_INTEN;
 
         /* copy from a bounce buffer, if necessary */
-        if (status && hdata->dma_bounce_buffer) {
-                if (hdata->dma_dir)
-                        memcpy(SCpnt->SCp.ptr, hdata->dma_bounce_buffer,
+        if (status && wh->dma_bounce_buffer) {
+                if (wh->dma_dir)
+                        memcpy(SCpnt->SCp.ptr, wh->dma_bounce_buffer,
                                SCpnt->SCp.this_residual);
-                kfree(hdata->dma_bounce_buffer);
-                hdata->dma_bounce_buffer = NULL;
-                hdata->dma_bounce_len = 0;
-        }
-}
-
-static int __init a2091_detect(struct scsi_host_template *tpnt)
-{
-        static unsigned char called = 0;
-        struct Scsi_Host *instance;
-        unsigned long address;
-        struct zorro_dev *z = NULL;
-        wd33c93_regs wdregs;
-        a2091_scsiregs *regs;
-        struct WD33C93_hostdata *hdata;
-        int num_a2091 = 0;
-
-        if (!MACH_IS_AMIGA || called)
-                return 0;
-        called = 1;
-
-        tpnt->proc_name = "A2091";
-        tpnt->proc_info = &wd33c93_proc_info;
-
-        while ((z = zorro_find_device(ZORRO_WILDCARD, z))) {
-                if (z->id != ZORRO_PROD_CBM_A590_A2091_1 &&
-                    z->id != ZORRO_PROD_CBM_A590_A2091_2)
-                        continue;
-                address = z->resource.start;
-                if (!request_mem_region(address, 256, "wd33c93"))
-                        continue;
-
-                instance = scsi_register(tpnt, sizeof(struct WD33C93_hostdata));
-                if (instance == NULL)
-                        goto release;
-                instance->base = ZTWO_VADDR(address);
-                instance->irq = IRQ_AMIGA_PORTS;
-                instance->unique_id = z->slotaddr;
-                regs = (a2091_scsiregs *)(instance->base);
-                regs->DAWR = DAWR_A2091;
-                wdregs.SASR = &regs->SASR;
-                wdregs.SCMD = &regs->SCMD;
-                hdata = shost_priv(instance);
-                hdata->no_sync = 0xff;
-                hdata->fast = 0;
-                hdata->dma_mode = CTRL_DMA;
-                wd33c93_init(instance, wdregs, dma_setup, dma_stop,
-                             WD33C93_FS_8_10);
-                if (request_irq(IRQ_AMIGA_PORTS, a2091_intr, IRQF_SHARED,
-                                "A2091 SCSI", instance))
-                        goto unregister;
-                regs->CNTR = CNTR_PDMD | CNTR_INTEN;
-                num_a2091++;
-                continue;
-
-unregister:
-                scsi_unregister(instance);
-release:
-                release_mem_region(address, 256);
+                kfree(wh->dma_bounce_buffer);
+                wh->dma_bounce_buffer = NULL;
+                wh->dma_bounce_len = 0;
         }
-
-        return num_a2091;
 }
 
 static int a2091_bus_reset(struct scsi_cmnd *cmd)
 {
+        struct Scsi_Host *instance = cmd->device->host;
+
         /* FIXME perform bus-specific reset */
 
         /* FIXME 2: kill this function, and let midlayer fall back
            to the same action, calling wd33c93_host_reset() */
 
-        spin_lock_irq(cmd->device->host->host_lock);
+        spin_lock_irq(instance->host_lock);
         wd33c93_host_reset(cmd);
-        spin_unlock_irq(cmd->device->host->host_lock);
+        spin_unlock_irq(instance->host_lock);
 
         return SUCCESS;
 }
 
-#define HOSTS_C
-
-static struct scsi_host_template driver_template = {
-        .proc_name              = "A2901",
+static struct scsi_host_template a2091_scsi_template = {
+        .module                 = THIS_MODULE,
         .name                   = "Commodore A2091/A590 SCSI",
-        .detect                 = a2091_detect,
-        .release                = a2091_release,
+        .proc_info              = wd33c93_proc_info,
+        .proc_name              = "A2901",
         .queuecommand           = wd33c93_queuecommand,
         .eh_abort_handler       = wd33c93_abort,
         .eh_bus_reset_handler   = a2091_bus_reset,
@@ -237,19 +178,103 @@ static struct scsi_host_template driver_template = {
         .use_clustering         = DISABLE_CLUSTERING
 };
 
+static int __devinit a2091_probe(struct zorro_dev *z,
+                                 const struct zorro_device_id *ent)
+{
+        struct Scsi_Host *instance;
+        int error;
+        struct a2091_scsiregs *regs;
+        wd33c93_regs wdregs;
+        struct a2091_hostdata *hdata;
 
-#include "scsi_module.c"
+        if (!request_mem_region(z->resource.start, 256, "wd33c93"))
+                return -EBUSY;
 
-static int a2091_release(struct Scsi_Host *instance)
+        instance = scsi_host_alloc(&a2091_scsi_template,
+                                   sizeof(struct a2091_hostdata));
+        if (!instance) {
+                error = -ENOMEM;
+                goto fail_alloc;
+        }
+
+        instance->irq = IRQ_AMIGA_PORTS;
+        instance->unique_id = z->slotaddr;
+
+        regs = (struct a2091_scsiregs *)ZTWO_VADDR(z->resource.start);
+        regs->DAWR = DAWR_A2091;
+
+        wdregs.SASR = &regs->SASR;
+        wdregs.SCMD = &regs->SCMD;
+
+        hdata = shost_priv(instance);
+        hdata->wh.no_sync = 0xff;
+        hdata->wh.fast = 0;
+        hdata->wh.dma_mode = CTRL_DMA;
+        hdata->regs = regs;
+
+        wd33c93_init(instance, wdregs, dma_setup, dma_stop, WD33C93_FS_8_10);
+        error = request_irq(IRQ_AMIGA_PORTS, a2091_intr, IRQF_SHARED,
+                            "A2091 SCSI", instance);
+        if (error)
+                goto fail_irq;
+
+        regs->CNTR = CNTR_PDMD | CNTR_INTEN;
+
+        error = scsi_add_host(instance, NULL);
+        if (error)
+                goto fail_host;
+
+        zorro_set_drvdata(z, instance);
+
+        scsi_scan_host(instance);
+        return 0;
+
+fail_host:
+        free_irq(IRQ_AMIGA_PORTS, instance);
+fail_irq:
+        scsi_host_put(instance);
+fail_alloc:
+        release_mem_region(z->resource.start, 256);
+        return error;
+}
+
+static void __devexit a2091_remove(struct zorro_dev *z)
 {
-#ifdef MODULE
-        a2091_scsiregs *regs = (a2091_scsiregs *)(instance->base);
+        struct Scsi_Host *instance = zorro_get_drvdata(z);
+        struct a2091_hostdata *hdata = shost_priv(instance);
 
-        regs->CNTR = 0;
-        release_mem_region(ZTWO_PADDR(instance->base), 256);
+        hdata->regs->CNTR = 0;
+        scsi_remove_host(instance);
         free_irq(IRQ_AMIGA_PORTS, instance);
-#endif
-        return 1;
+        scsi_host_put(instance);
+        release_mem_region(z->resource.start, 256);
+}
+
+static struct zorro_device_id a2091_zorro_tbl[] __devinitdata = {
+        { ZORRO_PROD_CBM_A590_A2091_1 },
+        { ZORRO_PROD_CBM_A590_A2091_2 },
+        { 0 }
+};
+MODULE_DEVICE_TABLE(zorro, a2091_zorro_tbl);
+
+static struct zorro_driver a2091_driver = {
+        .name           = "a2091",
+        .id_table       = a2091_zorro_tbl,
+        .probe          = a2091_probe,
+        .remove         = __devexit_p(a2091_remove),
+};
+
+static int __init a2091_init(void)
+{
+        return zorro_register_driver(&a2091_driver);
+}
+module_init(a2091_init);
+
+static void __exit a2091_exit(void)
+{
+        zorro_unregister_driver(&a2091_driver);
 }
+module_exit(a2091_exit);
 
+MODULE_DESCRIPTION("Commodore A2091/A590 SCSI");
 MODULE_LICENSE("GPL");
diff --git a/drivers/scsi/a2091.h b/drivers/scsi/a2091.h
index 1c3daa1fd754..794b8e65c711 100644
--- a/drivers/scsi/a2091.h
+++ b/drivers/scsi/a2091.h
@@ -25,7 +25,7 @@
  */
 #define A2091_XFER_MASK         (0xff000001)
 
-typedef struct {
+struct a2091_scsiregs {
                  unsigned char  pad1[64];
         volatile unsigned short ISTR;
         volatile unsigned short CNTR;
@@ -44,7 +44,7 @@ typedef struct {
         volatile unsigned short CINT;
                  unsigned char  pad7[2];
         volatile unsigned short FLUSH;
-} a2091_scsiregs;
+};
 
 #define DAWR_A2091              (3)
 
diff --git a/drivers/scsi/a3000.c b/drivers/scsi/a3000.c
index bc6eb69f5fd0..d9468027fb61 100644
--- a/drivers/scsi/a3000.c
+++ b/drivers/scsi/a3000.c
@@ -1,53 +1,52 @@
 #include <linux/types.h>
 #include <linux/mm.h>
-#include <linux/slab.h>
-#include <linux/blkdev.h>
 #include <linux/ioport.h>
 #include <linux/init.h>
+#include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/interrupt.h>
+#include <linux/platform_device.h>
 
-#include <asm/setup.h>
 #include <asm/page.h>
 #include <asm/pgtable.h>
 #include <asm/amigaints.h>
 #include <asm/amigahw.h>
-#include <asm/irq.h>
 
 #include "scsi.h"
-#include <scsi/scsi_host.h>
 #include "wd33c93.h"
 #include "a3000.h"
 
-#include <linux/stat.h>
-
 
-#define DMA(ptr)        ((a3000_scsiregs *)((ptr)->base))
-
-static struct Scsi_Host *a3000_host = NULL;
-
-static int a3000_release(struct Scsi_Host *instance);
+struct a3000_hostdata {
+        struct WD33C93_hostdata wh;
+        struct a3000_scsiregs *regs;
+};
 
-static irqreturn_t a3000_intr(int irq, void *dummy)
+static irqreturn_t a3000_intr(int irq, void *data)
 {
+        struct Scsi_Host *instance = data;
+        struct a3000_hostdata *hdata = shost_priv(instance);
+        unsigned int status = hdata->regs->ISTR;
         unsigned long flags;
-        unsigned int status = DMA(a3000_host)->ISTR;
 
         if (!(status & ISTR_INT_P))
                 return IRQ_NONE;
         if (status & ISTR_INTS) {
-                spin_lock_irqsave(a3000_host->host_lock, flags);
-                wd33c93_intr(a3000_host);
-                spin_unlock_irqrestore(a3000_host->host_lock, flags);
+                spin_lock_irqsave(instance->host_lock, flags);
+                wd33c93_intr(instance);
+                spin_unlock_irqrestore(instance->host_lock, flags);
                 return IRQ_HANDLED;
         }
-        printk("Non-serviced A3000 SCSI-interrupt? ISTR = %02x\n", status);
+        pr_warning("Non-serviced A3000 SCSI-interrupt? ISTR = %02x\n", status);
         return IRQ_NONE;
 }
 
 static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
 {
-        struct WD33C93_hostdata *hdata = shost_priv(a3000_host);
+        struct Scsi_Host *instance = cmd->device->host;
+        struct a3000_hostdata *hdata = shost_priv(instance);
+        struct WD33C93_hostdata *wh = &hdata->wh;
+        struct a3000_scsiregs *regs = hdata->regs;
         unsigned short cntr = CNTR_PDMD | CNTR_INTEN;
         unsigned long addr = virt_to_bus(cmd->SCp.ptr);
 
@@ -58,23 +57,23 @@ static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
          * buffer
          */
         if (addr & A3000_XFER_MASK) {
-                hdata->dma_bounce_len = (cmd->SCp.this_residual + 511) & ~0x1ff;
-                hdata->dma_bounce_buffer = kmalloc(hdata->dma_bounce_len,
-                                                   GFP_KERNEL);
+                wh->dma_bounce_len = (cmd->SCp.this_residual + 511) & ~0x1ff;
+                wh->dma_bounce_buffer = kmalloc(wh->dma_bounce_len,
+                                                GFP_KERNEL);
 
                 /* can't allocate memory; use PIO */
-                if (!hdata->dma_bounce_buffer) {
-                        hdata->dma_bounce_len = 0;
+                if (!wh->dma_bounce_buffer) {
+                        wh->dma_bounce_len = 0;
                         return 1;
                 }
 
                 if (!dir_in) {
                         /* copy to bounce buffer for a write */
-                        memcpy(hdata->dma_bounce_buffer, cmd->SCp.ptr,
+                        memcpy(wh->dma_bounce_buffer, cmd->SCp.ptr,
                                cmd->SCp.this_residual);
                 }
 
-                addr = virt_to_bus(hdata->dma_bounce_buffer);
+                addr = virt_to_bus(wh->dma_bounce_buffer);
         }
 
         /* setup dma direction */
@@ -82,12 +81,12 @@ static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
                 cntr |= CNTR_DDIR;
 
         /* remember direction */
-        hdata->dma_dir = dir_in;
+        wh->dma_dir = dir_in;
 
-        DMA(a3000_host)->CNTR = cntr;
+        regs->CNTR = cntr;
 
         /* setup DMA *physical* address */
-        DMA(a3000_host)->ACR = addr;
+        regs->ACR = addr;
 
         if (dir_in) {
                 /* invalidate any cache */
@@ -99,7 +98,7 @@ static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
 
         /* start DMA */
         mb();                   /* make sure setup is completed */
-        DMA(a3000_host)->ST_DMA = 1;
+        regs->ST_DMA = 1;
         mb();                   /* make sure DMA has started before next IO */
 
         /* return success */
@@ -109,22 +108,24 @@ static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
 static void dma_stop(struct Scsi_Host *instance, struct scsi_cmnd *SCpnt,
                      int status)
 {
-        struct WD33C93_hostdata *hdata = shost_priv(instance);
+        struct a3000_hostdata *hdata = shost_priv(instance);
+        struct WD33C93_hostdata *wh = &hdata->wh;
+        struct a3000_scsiregs *regs = hdata->regs;
 
         /* disable SCSI interrupts */
         unsigned short cntr = CNTR_PDMD;
 
-        if (!hdata->dma_dir)
+        if (!wh->dma_dir)
                 cntr |= CNTR_DDIR;
 
-        DMA(instance)->CNTR = cntr;
+        regs->CNTR = cntr;
         mb();                   /* make sure CNTR is updated before next IO */
 
         /* flush if we were reading */
-        if (hdata->dma_dir) {
-                DMA(instance)->FLUSH = 1;
+        if (wh->dma_dir) {
+                regs->FLUSH = 1;
                 mb();           /* don't allow prefetch */
-                while (!(DMA(instance)->ISTR & ISTR_FE_FLG))
+                while (!(regs->ISTR & ISTR_FE_FLG))
                         barrier();
                 mb();           /* no IO until FLUSH is done */
         }
@@ -133,96 +134,54 @@ static void dma_stop(struct Scsi_Host *instance, struct scsi_cmnd *SCpnt,
         /* I think that this CINT is only necessary if you are
          * using the terminal count features.   HM 7 Mar 1994
          */
-        DMA(instance)->CINT = 1;
+        regs->CINT = 1;
 
         /* stop DMA */
-        DMA(instance)->SP_DMA = 1;
+        regs->SP_DMA = 1;
         mb();                   /* make sure DMA is stopped before next IO */
 
         /* restore the CONTROL bits (minus the direction flag) */
-        DMA(instance)->CNTR = CNTR_PDMD | CNTR_INTEN;
+        regs->CNTR = CNTR_PDMD | CNTR_INTEN;
         mb();                   /* make sure CNTR is updated before next IO */
 
         /* copy from a bounce buffer, if necessary */
-        if (status && hdata->dma_bounce_buffer) {
+        if (status && wh->dma_bounce_buffer) {
                 if (SCpnt) {
-                        if (hdata->dma_dir && SCpnt)
-                                memcpy(SCpnt->SCp.ptr,
-                                       hdata->dma_bounce_buffer,
+                        if (wh->dma_dir && SCpnt)
+                                memcpy(SCpnt->SCp.ptr, wh->dma_bounce_buffer,
                                        SCpnt->SCp.this_residual);
-                        kfree(hdata->dma_bounce_buffer);
-                        hdata->dma_bounce_buffer = NULL;
-                        hdata->dma_bounce_len = 0;
+                        kfree(wh->dma_bounce_buffer);
+                        wh->dma_bounce_buffer = NULL;
+                        wh->dma_bounce_len = 0;
                 } else {
-                        kfree(hdata->dma_bounce_buffer);
-                        hdata->dma_bounce_buffer = NULL;
-                        hdata->dma_bounce_len = 0;
+                        kfree(wh->dma_bounce_buffer);
+                        wh->dma_bounce_buffer = NULL;
+                        wh->dma_bounce_len = 0;
                 }
         }
 }
 
-static int __init a3000_detect(struct scsi_host_template *tpnt)
-{
-        wd33c93_regs regs;
-        struct WD33C93_hostdata *hdata;
-
-        if (!MACH_IS_AMIGA || !AMIGAHW_PRESENT(A3000_SCSI))
-                return 0;
-        if (!request_mem_region(0xDD0000, 256, "wd33c93"))
-                return 0;
-
-        tpnt->proc_name = "A3000";
-        tpnt->proc_info = &wd33c93_proc_info;
-
-        a3000_host = scsi_register(tpnt, sizeof(struct WD33C93_hostdata));
-        if (a3000_host == NULL)
-                goto fail_register;
-
-        a3000_host->base = ZTWO_VADDR(0xDD0000);
-        a3000_host->irq = IRQ_AMIGA_PORTS;
-        DMA(a3000_host)->DAWR = DAWR_A3000;
-        regs.SASR = &(DMA(a3000_host)->SASR);
-        regs.SCMD = &(DMA(a3000_host)->SCMD);
-        hdata = shost_priv(a3000_host);
-        hdata->no_sync = 0xff;
-        hdata->fast = 0;
-        hdata->dma_mode = CTRL_DMA;
-        wd33c93_init(a3000_host, regs, dma_setup, dma_stop, WD33C93_FS_12_15);
-        if (request_irq(IRQ_AMIGA_PORTS, a3000_intr, IRQF_SHARED, "A3000 SCSI",
-                        a3000_intr))
-                goto fail_irq;
-        DMA(a3000_host)->CNTR = CNTR_PDMD | CNTR_INTEN;
-
-        return 1;
-
-fail_irq:
-        scsi_unregister(a3000_host);
-fail_register:
-        release_mem_region(0xDD0000, 256);
-        return 0;
-}
-
 static int a3000_bus_reset(struct scsi_cmnd *cmd)
 {
+        struct Scsi_Host *instance = cmd->device->host;
+
         /* FIXME perform bus-specific reset */
 
         /* FIXME 2: kill this entire function, which should
            cause mid-layer to call wd33c93_host_reset anyway? */
 
-        spin_lock_irq(cmd->device->host->host_lock);
+        spin_lock_irq(instance->host_lock);
         wd33c93_host_reset(cmd);
-        spin_unlock_irq(cmd->device->host->host_lock);
+        spin_unlock_irq(instance->host_lock);
 
         return SUCCESS;
 }
 
-#define HOSTS_C
-
-static struct scsi_host_template driver_template = {
-        .proc_name              = "A3000",
+static struct scsi_host_template amiga_a3000_scsi_template = {
+        .module                 = THIS_MODULE,
         .name                   = "Amiga 3000 built-in SCSI",
-        .detect                 = a3000_detect,
-        .release                = a3000_release,
+        .proc_info              = wd33c93_proc_info,
+        .proc_name              = "A3000",
         .queuecommand           = wd33c93_queuecommand,
         .eh_abort_handler       = wd33c93_abort,
         .eh_bus_reset_handler   = a3000_bus_reset,
@@ -234,15 +193,104 @@ static struct scsi_host_template driver_template = {
         .use_clustering         = ENABLE_CLUSTERING
 };
 
+static int __init amiga_a3000_scsi_probe(struct platform_device *pdev)
+{
+        struct resource *res;
+        struct Scsi_Host *instance;
+        int error;
+        struct a3000_scsiregs *regs;
+        wd33c93_regs wdregs;
+        struct a3000_hostdata *hdata;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res)
+                return -ENODEV;
+
+        if (!request_mem_region(res->start, resource_size(res), "wd33c93"))
+                return -EBUSY;
+
+        instance = scsi_host_alloc(&amiga_a3000_scsi_template,
+                                   sizeof(struct a3000_hostdata));
+        if (!instance) {
+                error = -ENOMEM;
+                goto fail_alloc;
+        }
+
+        instance->irq = IRQ_AMIGA_PORTS;
 
-#include "scsi_module.c"
+        regs = (struct a3000_scsiregs *)ZTWO_VADDR(res->start);
+        regs->DAWR = DAWR_A3000;
+
+        wdregs.SASR = &regs->SASR;
+        wdregs.SCMD = &regs->SCMD;
+
+        hdata = shost_priv(instance);
+        hdata->wh.no_sync = 0xff;
+        hdata->wh.fast = 0;
+        hdata->wh.dma_mode = CTRL_DMA;
+        hdata->regs = regs;
+
+        wd33c93_init(instance, wdregs, dma_setup, dma_stop, WD33C93_FS_12_15);
+        error = request_irq(IRQ_AMIGA_PORTS, a3000_intr, IRQF_SHARED,
+                            "A3000 SCSI", instance);
+        if (error)
+                goto fail_irq;
+
+        regs->CNTR = CNTR_PDMD | CNTR_INTEN;
+
+        error = scsi_add_host(instance, NULL);
+        if (error)
+                goto fail_host;
+
+        platform_set_drvdata(pdev, instance);
+
+        scsi_scan_host(instance);
+        return 0;
+
+fail_host:
+        free_irq(IRQ_AMIGA_PORTS, instance);
+fail_irq:
+        scsi_host_put(instance);
+fail_alloc:
+        release_mem_region(res->start, resource_size(res));
+        return error;
+}
+
+static int __exit amiga_a3000_scsi_remove(struct platform_device *pdev)
+{
+        struct Scsi_Host *instance = platform_get_drvdata(pdev);
+        struct a3000_hostdata *hdata = shost_priv(instance);
+        struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+        hdata->regs->CNTR = 0;
+        scsi_remove_host(instance);
+        free_irq(IRQ_AMIGA_PORTS, instance);
+        scsi_host_put(instance);
+        release_mem_region(res->start, resource_size(res));
+        return 0;
+}
+
+static struct platform_driver amiga_a3000_scsi_driver = {
+        .remove = __exit_p(amiga_a3000_scsi_remove),
+        .driver   = {
+                .name   = "amiga-a3000-scsi",
+                .owner  = THIS_MODULE,
+        },
+};
+
+static int __init amiga_a3000_scsi_init(void)
+{
+        return platform_driver_probe(&amiga_a3000_scsi_driver,
+                                     amiga_a3000_scsi_probe);
+}
+module_init(amiga_a3000_scsi_init);
 
-static int a3000_release(struct Scsi_Host *instance)
+static void __exit amiga_a3000_scsi_exit(void)
 {
-        DMA(instance)->CNTR = 0;
-        release_mem_region(0xDD0000, 256);
-        free_irq(IRQ_AMIGA_PORTS, a3000_intr);
-        return 1;
+        platform_driver_unregister(&amiga_a3000_scsi_driver);
 }
+module_exit(amiga_a3000_scsi_exit);
 
+MODULE_DESCRIPTION("Amiga 3000 built-in SCSI");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:amiga-a3000-scsi");
diff --git a/drivers/scsi/a3000.h b/drivers/scsi/a3000.h
index 684813ee378c..49db4a335aab 100644
--- a/drivers/scsi/a3000.h
+++ b/drivers/scsi/a3000.h
@@ -25,7 +25,7 @@
  */
 #define A3000_XFER_MASK         (0x00000003)
 
-typedef struct {
+struct a3000_scsiregs {
                  unsigned char  pad1[2];
         volatile unsigned short DAWR;
         volatile unsigned int   WTC;
@@ -46,7 +46,7 @@ typedef struct {
         volatile unsigned char  SASR;
                  unsigned char  pad9;
         volatile unsigned char  SCMD;
-} a3000_scsiregs;
+};
 
 #define DAWR_A3000              (3)
 
diff --git a/drivers/scsi/a4000t.c b/drivers/scsi/a4000t.c
index 11ae6be8aeaf..23c76f41883c 100644
--- a/drivers/scsi/a4000t.c
+++ b/drivers/scsi/a4000t.c
@@ -20,10 +20,6 @@
 
 #include "53c700.h"
 
-MODULE_AUTHOR("Alan Hourihane <alanh@fairlite.demon.co.uk> / Kars de Jong <jongk@linux-m68k.org>");
-MODULE_DESCRIPTION("Amiga A4000T NCR53C710 driver");
-MODULE_LICENSE("GPL");
-
 
 static struct scsi_host_template a4000t_scsi_driver_template = {
         .name           = "A4000T builtin SCSI",
@@ -32,30 +28,35 @@ static struct scsi_host_template a4000t_scsi_driver_template = {
         .module         = THIS_MODULE,
 };
 
-static struct platform_device *a4000t_scsi_device;
 
-#define A4000T_SCSI_ADDR 0xdd0040
+#define A4000T_SCSI_OFFSET      0x40
 
-static int __devinit a4000t_probe(struct platform_device *dev)
+static int __init amiga_a4000t_scsi_probe(struct platform_device *pdev)
 {
-        struct Scsi_Host *host;
+        struct resource *res;
+        phys_addr_t scsi_addr;
         struct NCR_700_Host_Parameters *hostdata;
+        struct Scsi_Host *host;
 
-        if (!(MACH_IS_AMIGA && AMIGAHW_PRESENT(A4000_SCSI)))
-                goto out;
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res)
+                return -ENODEV;
 
-        if (!request_mem_region(A4000T_SCSI_ADDR, 0x1000,
+        if (!request_mem_region(res->start, resource_size(res),
                                 "A4000T builtin SCSI"))
-                goto out;
+                return -EBUSY;
 
-        hostdata = kzalloc(sizeof(struct NCR_700_Host_Parameters), GFP_KERNEL);
+        hostdata = kzalloc(sizeof(struct NCR_700_Host_Parameters),
+                           GFP_KERNEL);
         if (!hostdata) {
-                printk(KERN_ERR "a4000t-scsi: Failed to allocate host data\n");
+                dev_err(&pdev->dev, "Failed to allocate host data\n");
                 goto out_release;
         }
 
+        scsi_addr = res->start + A4000T_SCSI_OFFSET;
+
         /* Fill in the required pieces of hostdata */
-        hostdata->base = (void __iomem *)ZTWO_VADDR(A4000T_SCSI_ADDR);
+        hostdata->base = (void __iomem *)ZTWO_VADDR(scsi_addr);
         hostdata->clock = 50;
         hostdata->chip710 = 1;
         hostdata->dmode_extra = DMODE_FC2;
@@ -63,26 +64,25 @@ static int __devinit a4000t_probe(struct platform_device *dev)
 
         /* and register the chip */
         host = NCR_700_detect(&a4000t_scsi_driver_template, hostdata,
-                              &dev->dev);
+                              &pdev->dev);
         if (!host) {
-                printk(KERN_ERR "a4000t-scsi: No host detected; "
-                                "board configuration problem?\n");
+                dev_err(&pdev->dev,
+                        "No host detected; board configuration problem?\n");
                 goto out_free;
         }
 
         host->this_id = 7;
-        host->base = A4000T_SCSI_ADDR;
+        host->base = scsi_addr;
         host->irq = IRQ_AMIGA_PORTS;
 
         if (request_irq(host->irq, NCR_700_intr, IRQF_SHARED, "a4000t-scsi",
                         host)) {
-                printk(KERN_ERR "a4000t-scsi: request_irq failed\n");
+                dev_err(&pdev->dev, "request_irq failed\n");
                 goto out_put_host;
         }
 
-        platform_set_drvdata(dev, host);
+        platform_set_drvdata(pdev, host);
         scsi_scan_host(host);
-
         return 0;
 
  out_put_host:
@@ -90,58 +90,49 @@ static int __devinit a4000t_probe(struct platform_device *dev)
  out_free:
         kfree(hostdata);
  out_release:
-        release_mem_region(A4000T_SCSI_ADDR, 0x1000);
- out:
+        release_mem_region(res->start, resource_size(res));
         return -ENODEV;
 }
 
-static __devexit int a4000t_device_remove(struct platform_device *dev)
+static int __exit amiga_a4000t_scsi_remove(struct platform_device *pdev)
 {
-        struct Scsi_Host *host = platform_get_drvdata(dev);
+        struct Scsi_Host *host = platform_get_drvdata(pdev);
         struct NCR_700_Host_Parameters *hostdata = shost_priv(host);
+        struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
         scsi_remove_host(host);
-
         NCR_700_release(host);
         kfree(hostdata);
         free_irq(host->irq, host);
-        release_mem_region(A4000T_SCSI_ADDR, 0x1000);
-
+        release_mem_region(res->start, resource_size(res));
         return 0;
 }
 
-static struct platform_driver a4000t_scsi_driver = {
-        .driver = {
-                .name           = "a4000t-scsi",
-                .owner          = THIS_MODULE,
+static struct platform_driver amiga_a4000t_scsi_driver = {
+        .remove = __exit_p(amiga_a4000t_scsi_remove),
+        .driver   = {
+                .name   = "amiga-a4000t-scsi",
+                .owner  = THIS_MODULE,
         },
-        .probe          = a4000t_probe,
-        .remove         = __devexit_p(a4000t_device_remove),
 };
 
-static int __init a4000t_scsi_init(void)
+static int __init amiga_a4000t_scsi_init(void)
 {
-        int err;
-
-        err = platform_driver_register(&a4000t_scsi_driver);
-        if (err)
-                return err;
-
-        a4000t_scsi_device = platform_device_register_simple("a4000t-scsi",
-                        -1, NULL, 0);
-        if (IS_ERR(a4000t_scsi_device)) {
-                platform_driver_unregister(&a4000t_scsi_driver);
-                return PTR_ERR(a4000t_scsi_device);
-        }
-
-        return err;
+        return platform_driver_probe(&amiga_a4000t_scsi_driver,
+                                     amiga_a4000t_scsi_probe);
 }
 
-static void __exit a4000t_scsi_exit(void)
+module_init(amiga_a4000t_scsi_init);
+
+static void __exit amiga_a4000t_scsi_exit(void)
 {
-        platform_device_unregister(a4000t_scsi_device);
-        platform_driver_unregister(&a4000t_scsi_driver);
+        platform_driver_unregister(&amiga_a4000t_scsi_driver);
 }
 
-module_init(a4000t_scsi_init);
-module_exit(a4000t_scsi_exit);
+module_exit(amiga_a4000t_scsi_exit);
+
+MODULE_AUTHOR("Alan Hourihane <alanh@fairlite.demon.co.uk> / "
+              "Kars de Jong <jongk@linux-m68k.org>");
+MODULE_DESCRIPTION("Amiga A4000T NCR53C710 driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:amiga-a4000t-scsi");
diff --git a/drivers/scsi/aacraid/commctrl.c b/drivers/scsi/aacraid/commctrl.c
index 9c0c91178538..1a5bf5724750 100644
--- a/drivers/scsi/aacraid/commctrl.c
+++ b/drivers/scsi/aacraid/commctrl.c
@@ -655,9 +655,9 @@ static int aac_send_raw_srb(struct aac_dev* dev, void __user * arg)
                                 /* Does this really need to be GFP_DMA? */
                                 p = kmalloc(usg->sg[i].count,GFP_KERNEL|__GFP_DMA);
                                 if(!p) {
-                                        kfree (usg);
-                                        dprintk((KERN_DEBUG"aacraid: Could not allocate SG buffer - size = %d buffer number %d of %d\n",
+                                        dprintk((KERN_DEBUG "aacraid: Could not allocate SG buffer - size = %d buffer number %d of %d\n",
                                           usg->sg[i].count,i,usg->count));
+                                        kfree(usg);
                                         rcode = -ENOMEM;
                                         goto cleanup;
                                 }
diff --git a/drivers/scsi/arcmsr/arcmsr.h b/drivers/scsi/arcmsr/arcmsr.h
index ab646e580d64..ce5371b3cdd5 100644
--- a/drivers/scsi/arcmsr/arcmsr.h
+++ b/drivers/scsi/arcmsr/arcmsr.h
@@ -48,7 +48,7 @@ struct device_attribute;
 /*The limit of outstanding scsi command that firmware can handle*/
 #define ARCMSR_MAX_OUTSTANDING_CMD                                              256
 #define ARCMSR_MAX_FREECCB_NUM                                                  320
-#define ARCMSR_DRIVER_VERSION                "Driver Version 1.20.00.15 2008/02/27"
+#define ARCMSR_DRIVER_VERSION                "Driver Version 1.20.00.15 2008/11/03"
 #define ARCMSR_SCSI_INITIATOR_ID                                                255
 #define ARCMSR_MAX_XFER_SECTORS                                                 512
 #define ARCMSR_MAX_XFER_SECTORS_B                                               4096
@@ -110,6 +110,8 @@ struct CMD_MESSAGE_FIELD
 #define FUNCTION_SAY_HELLO                      0x0807
 #define FUNCTION_SAY_GOODBYE                    0x0808
 #define FUNCTION_FLUSH_ADAPTER_CACHE            0x0809
+#define FUNCTION_GET_FIRMWARE_STATUS                    0x080A
+#define FUNCTION_HARDWARE_RESET                 0x080B
 /* ARECA IO CONTROL CODE*/
 #define ARCMSR_MESSAGE_READ_RQBUFFER       \
         ARECA_SATA_RAID | FUNCTION_READ_RQBUFFER
@@ -133,6 +135,7 @@ struct CMD_MESSAGE_FIELD
 #define ARCMSR_MESSAGE_RETURNCODE_OK              0x00000001
 #define ARCMSR_MESSAGE_RETURNCODE_ERROR           0x00000006
 #define ARCMSR_MESSAGE_RETURNCODE_3F              0x0000003F
+#define ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON   0x00000088
 /*
 *************************************************************
 **   structure for holding DMA address data
@@ -341,13 +344,13 @@ struct MessageUnit_B
         uint32_t        done_qbuffer[ARCMSR_MAX_HBB_POSTQUEUE];
         uint32_t        postq_index;
         uint32_t        doneq_index;
-        void            __iomem *drv2iop_doorbell_reg;
-        void            __iomem *drv2iop_doorbell_mask_reg;
-        void            __iomem *iop2drv_doorbell_reg;
-        void            __iomem *iop2drv_doorbell_mask_reg;
-        void            __iomem *msgcode_rwbuffer_reg;
-        void            __iomem *ioctl_wbuffer_reg;
-        void            __iomem *ioctl_rbuffer_reg;
+        uint32_t                __iomem *drv2iop_doorbell_reg;
+        uint32_t                __iomem *drv2iop_doorbell_mask_reg;
+        uint32_t                __iomem *iop2drv_doorbell_reg;
+        uint32_t                __iomem *iop2drv_doorbell_mask_reg;
+        uint32_t                __iomem *msgcode_rwbuffer_reg;
+        uint32_t                __iomem *ioctl_wbuffer_reg;
+        uint32_t                __iomem *ioctl_rbuffer_reg;
 };
 
 /*
@@ -375,6 +378,7 @@ struct AdapterControlBlock
         /* message unit ATU inbound base address0 */
 
         uint32_t                        acb_flags;
+        uint8_t                                 adapter_index;
         #define ACB_F_SCSISTOPADAPTER           0x0001
         #define ACB_F_MSG_STOP_BGRB             0x0002
         /* stop RAID background rebuild */
@@ -390,7 +394,7 @@ struct AdapterControlBlock
         #define ACB_F_BUS_RESET                 0x0080
         #define ACB_F_IOP_INITED                0x0100
         /* iop init */
-
+        #define ACB_F_FIRMWARE_TRAP                     0x0400
         struct CommandControlBlock *                    pccb_pool[ARCMSR_MAX_FREECCB_NUM];
         /* used for memory free */
         struct list_head                ccb_free_list;
@@ -423,12 +427,19 @@ struct AdapterControlBlock
 #define ARECA_RAID_GOOD               0xaa
         uint32_t                        num_resets;
         uint32_t                        num_aborts;
+        uint32_t                        signature;
         uint32_t                        firm_request_len;
         uint32_t                        firm_numbers_queue;
         uint32_t                        firm_sdram_size;
         uint32_t                        firm_hd_channels;
         char                            firm_model[12];
         char                            firm_version[20];
+        char                    device_map[20];                 /*21,84-99*/
+        struct work_struct              arcmsr_do_message_isr_bh;
+        struct timer_list               eternal_timer;
+        unsigned short          fw_state;
+        atomic_t                        rq_map_token;
+        int                     ante_token_value;
 };/* HW_DEVICE_EXTENSION */
 /*
 *******************************************************************************
diff --git a/drivers/scsi/arcmsr/arcmsr_attr.c b/drivers/scsi/arcmsr/arcmsr_attr.c
index a4e04c50c436..07fdfe57e38e 100644
--- a/drivers/scsi/arcmsr/arcmsr_attr.c
+++ b/drivers/scsi/arcmsr/arcmsr_attr.c
@@ -192,6 +192,7 @@ static struct bin_attribute arcmsr_sysfs_message_read_attr = {
         .attr = {
                 .name = "mu_read",
                 .mode = S_IRUSR ,
+                .owner = THIS_MODULE,
         },
         .size = 1032,
         .read = arcmsr_sysfs_iop_message_read,
@@ -201,6 +202,7 @@ static struct bin_attribute arcmsr_sysfs_message_write_attr = {
         .attr = {
                 .name = "mu_write",
                 .mode = S_IWUSR,
+                .owner = THIS_MODULE,
         },
         .size = 1032,
         .write = arcmsr_sysfs_iop_message_write,
@@ -210,6 +212,7 @@ static struct bin_attribute arcmsr_sysfs_message_clear_attr = {
         .attr = {
                 .name = "mu_clear",
                 .mode = S_IWUSR,
+                .owner = THIS_MODULE,
         },
         .size = 1,
         .write = arcmsr_sysfs_iop_message_clear,
diff --git a/drivers/scsi/arcmsr/arcmsr_hba.c b/drivers/scsi/arcmsr/arcmsr_hba.c
index ffbe2192da3c..ffa54792bb33 100644
--- a/drivers/scsi/arcmsr/arcmsr_hba.c
+++ b/drivers/scsi/arcmsr/arcmsr_hba.c
@@ -72,8 +72,16 @@
 #include <scsi/scsicam.h>
 #include "arcmsr.h"
 
+#ifdef CONFIG_SCSI_ARCMSR_RESET
+        static int sleeptime = 20;
+        static int retrycount = 12;
+        module_param(sleeptime, int, S_IRUGO|S_IWUSR);
+        MODULE_PARM_DESC(sleeptime, "The waiting period for FW ready while bus reset");
+        module_param(retrycount, int, S_IRUGO|S_IWUSR);
+        MODULE_PARM_DESC(retrycount, "The retry count for FW ready while bus reset");
+#endif
 MODULE_AUTHOR("Erich Chen <support@areca.com.tw>");
-MODULE_DESCRIPTION("ARECA (ARC11xx/12xx/13xx/16xx) SATA/SAS RAID HOST Adapter");
+MODULE_DESCRIPTION("ARECA (ARC11xx/12xx/13xx/16xx) SATA/SAS RAID Host Bus Adapter");
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_VERSION(ARCMSR_DRIVER_VERSION);
 
@@ -96,6 +104,13 @@ static u32 arcmsr_disable_outbound_ints(struct AdapterControlBlock *acb);
 static void arcmsr_stop_adapter_bgrb(struct AdapterControlBlock *acb);
 static void arcmsr_flush_hba_cache(struct AdapterControlBlock *acb);
 static void arcmsr_flush_hbb_cache(struct AdapterControlBlock *acb);
+static void arcmsr_request_device_map(unsigned long pacb);
+static void arcmsr_request_hba_device_map(struct AdapterControlBlock *acb);
+static void arcmsr_request_hbb_device_map(struct AdapterControlBlock *acb);
+static void arcmsr_message_isr_bh_fn(struct work_struct *work);
+static void *arcmsr_get_firmware_spec(struct AdapterControlBlock *acb, int mode);
+static void arcmsr_start_adapter_bgrb(struct AdapterControlBlock *acb);
+
 static const char *arcmsr_info(struct Scsi_Host *);
 static irqreturn_t arcmsr_interrupt(struct AdapterControlBlock *acb);
 static int arcmsr_adjust_disk_queue_depth(struct scsi_device *sdev,
@@ -112,7 +127,7 @@ static int arcmsr_adjust_disk_queue_depth(struct scsi_device *sdev,
 
 static struct scsi_host_template arcmsr_scsi_host_template = {
         .module                 = THIS_MODULE,
-        .name                   = "ARCMSR ARECA SATA/SAS RAID HOST Adapter"
+        .name                   = "ARCMSR ARECA SATA/SAS RAID Host Bus Adapter"
                                                         ARCMSR_DRIVER_VERSION,
         .info                   = arcmsr_info,
         .queuecommand           = arcmsr_queue_command,
@@ -128,16 +143,6 @@ static struct scsi_host_template arcmsr_scsi_host_template = {
         .use_clustering         = ENABLE_CLUSTERING,
         .shost_attrs            = arcmsr_host_attrs,
 };
-#ifdef CONFIG_SCSI_ARCMSR_AER
-static pci_ers_result_t arcmsr_pci_slot_reset(struct pci_dev *pdev);
-static pci_ers_result_t arcmsr_pci_error_detected(struct pci_dev *pdev,
-                                                pci_channel_state_t state);
-
-static struct pci_error_handlers arcmsr_pci_error_handlers = {
-        .error_detected         = arcmsr_pci_error_detected,
-        .slot_reset             = arcmsr_pci_slot_reset,
-};
-#endif
 static struct pci_device_id arcmsr_device_id_table[] = {
         {PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1110)},
         {PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1120)},
@@ -166,9 +171,6 @@ static struct pci_driver arcmsr_pci_driver = {
         .probe                  = arcmsr_probe,
         .remove                 = arcmsr_remove,
         .shutdown               = arcmsr_shutdown,
-        #ifdef CONFIG_SCSI_ARCMSR_AER
-        .err_handler            = &arcmsr_pci_error_handlers,
-        #endif
 };
 
 static irqreturn_t arcmsr_do_interrupt(int irq, void *dev_id)
@@ -236,10 +238,9 @@ static int arcmsr_alloc_ccb_pool(struct AdapterControlBlock *acb)
                 void *dma_coherent;
                 dma_addr_t dma_coherent_handle, dma_addr;
                 struct CommandControlBlock *ccb_tmp;
-                uint32_t intmask_org;
                 int i, j;
 
-                acb->pmuA = pci_ioremap_bar(pdev, 0);
+                acb->pmuA = ioremap(pci_resource_start(pdev, 0), pci_resource_len(pdev, 0));
                 if (!acb->pmuA) {
                         printk(KERN_NOTICE "arcmsr%d: memory mapping region fail \n",
                                                         acb->host->host_no);
@@ -281,12 +282,6 @@ static int arcmsr_alloc_ccb_pool(struct AdapterControlBlock *acb)
                 for (i = 0; i < ARCMSR_MAX_TARGETID; i++)
                         for (j = 0; j < ARCMSR_MAX_TARGETLUN; j++)
                                 acb->devstate[i][j] = ARECA_RAID_GONE;
-
-                /*
-                ** here we need to tell iop 331 our ccb_tmp.HighPart
-                ** if ccb_tmp.HighPart is not zero
-                */
-                intmask_org = arcmsr_disable_outbound_ints(acb);
                 }
                 break;
 
@@ -297,7 +292,6 @@ static int arcmsr_alloc_ccb_pool(struct AdapterControlBlock *acb)
                 void __iomem *mem_base0, *mem_base1;
                 void *dma_coherent;
                 dma_addr_t dma_coherent_handle, dma_addr;
-                uint32_t intmask_org;
                 struct CommandControlBlock *ccb_tmp;
                 int i, j;
 
@@ -333,11 +327,13 @@ static int arcmsr_alloc_ccb_pool(struct AdapterControlBlock *acb)
                 reg = (struct MessageUnit_B *)(dma_coherent +
                 ARCMSR_MAX_FREECCB_NUM * sizeof(struct CommandControlBlock));
                 acb->pmuB = reg;
-                mem_base0 = pci_ioremap_bar(pdev, 0);
+                mem_base0 = ioremap(pci_resource_start(pdev, 0),
+                                        pci_resource_len(pdev, 0));
                 if (!mem_base0)
                         goto out;
 
-                mem_base1 = pci_ioremap_bar(pdev, 2);
+                mem_base1 = ioremap(pci_resource_start(pdev, 2),
+                                        pci_resource_len(pdev, 2));
                 if (!mem_base1) {
                         iounmap(mem_base0);
                         goto out;
@@ -357,12 +353,6 @@ static int arcmsr_alloc_ccb_pool(struct AdapterControlBlock *acb)
                 for (i = 0; i < ARCMSR_MAX_TARGETID; i++)
                         for (j = 0; j < ARCMSR_MAX_TARGETLUN; j++)
                                 acb->devstate[i][j] = ARECA_RAID_GOOD;
-
-                /*
-                ** here we need to tell iop 331 our ccb_tmp.HighPart
-                ** if ccb_tmp.HighPart is not zero
-                */
-                intmask_org = arcmsr_disable_outbound_ints(acb);
                 }
                 break;
         }
@@ -374,6 +364,88 @@ out:
                 sizeof(struct MessageUnit_B)), acb->dma_coherent, acb->dma_coherent_handle);
         return -ENOMEM;
 }
+static void arcmsr_message_isr_bh_fn(struct work_struct *work)
+{
+        struct AdapterControlBlock *acb = container_of(work, struct AdapterControlBlock, arcmsr_do_message_isr_bh);
+
+        switch (acb->adapter_type) {
+                case ACB_ADAPTER_TYPE_A: {
+
+                        struct MessageUnit_A __iomem *reg  = acb->pmuA;
+                        char *acb_dev_map = (char *)acb->device_map;
+                        uint32_t __iomem *signature = (uint32_t __iomem *) (&reg->message_rwbuffer[0]);
+                        char __iomem *devicemap = (char __iomem *) (&reg->message_rwbuffer[21]);
+                        int target, lun;
+                        struct scsi_device *psdev;
+                        char diff;
+
+                        atomic_inc(&acb->rq_map_token);
+                        if (readl(signature) == ARCMSR_SIGNATURE_GET_CONFIG) {
+                                for (target = 0; target < ARCMSR_MAX_TARGETID - 1; target++) {
+                                        diff = (*acb_dev_map)^readb(devicemap);
+                                        if (diff != 0) {
+                                                char temp;
+                                                *acb_dev_map = readb(devicemap);
+                                                temp = *acb_dev_map;
+                                                for (lun = 0; lun < ARCMSR_MAX_TARGETLUN; lun++) {
+                                                        if ((temp & 0x01) == 1 && (diff & 0x01) == 1) {
+                                                                scsi_add_device(acb->host, 0, target, lun);
+                                                        } else if ((temp & 0x01) == 0 && (diff & 0x01) == 1) {
+                                                                psdev = scsi_device_lookup(acb->host, 0, target, lun);
+                                                                if (psdev != NULL) {
+                                                                        scsi_remove_device(psdev);
+                                                                        scsi_device_put(psdev);
+                                                                }
+                                                        }
+                                                        temp >>= 1;
+                                                        diff >>= 1;
+                                                }
+                                        }
+                                        devicemap++;
+                                        acb_dev_map++;
+                                }
+                        }
+                        break;
+                }
+
+                case ACB_ADAPTER_TYPE_B: {
+                        struct MessageUnit_B *reg  = acb->pmuB;
+                        char *acb_dev_map = (char *)acb->device_map;
+                        uint32_t __iomem *signature = (uint32_t __iomem *)(&reg->msgcode_rwbuffer_reg[0]);
+                        char __iomem *devicemap = (char __iomem *)(&reg->msgcode_rwbuffer_reg[21]);
+                        int target, lun;
+                        struct scsi_device *psdev;
+                        char diff;
+
+                        atomic_inc(&acb->rq_map_token);
+                        if (readl(signature) == ARCMSR_SIGNATURE_GET_CONFIG) {
+                                for (target = 0; target < ARCMSR_MAX_TARGETID - 1; target++) {
+                                        diff = (*acb_dev_map)^readb(devicemap);
+                                        if (diff != 0) {
+                                                char temp;
+                                                *acb_dev_map = readb(devicemap);
+                                                temp = *acb_dev_map;
+                                                for (lun = 0; lun < ARCMSR_MAX_TARGETLUN; lun++) {
+                                                        if ((temp & 0x01) == 1 && (diff & 0x01) == 1) {
+                                                                scsi_add_device(acb->host, 0, target, lun);
+                                                        } else if ((temp & 0x01) == 0 && (diff & 0x01) == 1) {
+                                                                psdev = scsi_device_lookup(acb->host, 0, target, lun);
+                                                                if (psdev != NULL) {
+                                                                        scsi_remove_device(psdev);
+                                                                        scsi_device_put(psdev);
+                                                                }
+                                                        }
+                                                        temp >>= 1;
+                                                        diff >>= 1;
+                                                }
+                                        }
+                                        devicemap++;
+                                        acb_dev_map++;
+                                }
+                        }
+                }
+        }
+}
 
 static int arcmsr_probe(struct pci_dev *pdev,
         const struct pci_device_id *id)
@@ -432,17 +504,17 @@ static int arcmsr_probe(struct pci_dev *pdev,
                            ACB_F_MESSAGE_WQBUFFER_READED);
         acb->acb_flags &= ~ACB_F_SCSISTOPADAPTER;
         INIT_LIST_HEAD(&acb->ccb_free_list);
-
+        INIT_WORK(&acb->arcmsr_do_message_isr_bh, arcmsr_message_isr_bh_fn);
         error = arcmsr_alloc_ccb_pool(acb);
         if (error)
                 goto out_release_regions;
 
+        arcmsr_iop_init(acb);
         error = request_irq(pdev->irq, arcmsr_do_interrupt,
                             IRQF_SHARED, "arcmsr", acb);
         if (error)
                 goto out_free_ccb_pool;
 
-        arcmsr_iop_init(acb);
         pci_set_drvdata(pdev, host);
         if (strncmp(acb->firm_version, "V1.42", 5) >= 0)
                 host->max_sectors= ARCMSR_MAX_XFER_SECTORS_B;
@@ -459,6 +531,14 @@ static int arcmsr_probe(struct pci_dev *pdev,
         #ifdef CONFIG_SCSI_ARCMSR_AER
         pci_enable_pcie_error_reporting(pdev);
         #endif
+        atomic_set(&acb->rq_map_token, 16);
+        acb->fw_state = true;
+        init_timer(&acb->eternal_timer);
+        acb->eternal_timer.expires = jiffies + msecs_to_jiffies(10*HZ);
+        acb->eternal_timer.data = (unsigned long) acb;
+        acb->eternal_timer.function = &arcmsr_request_device_map;
+        add_timer(&acb->eternal_timer);
+
         return 0;
  out_free_sysfs:
  out_free_irq:
@@ -518,40 +598,48 @@ static uint8_t arcmsr_hbb_wait_msgint_ready(struct AdapterControlBlock *acb)
         return 0xff;
 }
 
-static void arcmsr_abort_hba_allcmd(struct AdapterControlBlock *acb)
+static uint8_t arcmsr_abort_hba_allcmd(struct AdapterControlBlock *acb)
 {
         struct MessageUnit_A __iomem *reg = acb->pmuA;
 
         writel(ARCMSR_INBOUND_MESG0_ABORT_CMD, &reg->inbound_msgaddr0);
-        if (arcmsr_hba_wait_msgint_ready(acb))
+        if (arcmsr_hba_wait_msgint_ready(acb)) {
                 printk(KERN_NOTICE
                         "arcmsr%d: wait 'abort all outstanding command' timeout \n"
                         , acb->host->host_no);
+                return 0xff;
+        }
+        return 0x00;
 }
 
-static void arcmsr_abort_hbb_allcmd(struct AdapterControlBlock *acb)
+static uint8_t arcmsr_abort_hbb_allcmd(struct AdapterControlBlock *acb)
 {
         struct MessageUnit_B *reg = acb->pmuB;
 
         writel(ARCMSR_MESSAGE_ABORT_CMD, reg->drv2iop_doorbell_reg);
-        if (arcmsr_hbb_wait_msgint_ready(acb))
+        if (arcmsr_hbb_wait_msgint_ready(acb)) {
                 printk(KERN_NOTICE
                         "arcmsr%d: wait 'abort all outstanding command' timeout \n"
                         , acb->host->host_no);
+                return 0xff;
+        }
+        return 0x00;
 }
 
-static void arcmsr_abort_allcmd(struct AdapterControlBlock *acb)
+static uint8_t arcmsr_abort_allcmd(struct AdapterControlBlock *acb)
 {
+        uint8_t rtnval = 0;
         switch (acb->adapter_type) {
         case ACB_ADAPTER_TYPE_A: {
-                arcmsr_abort_hba_allcmd(acb);
+                rtnval = arcmsr_abort_hba_allcmd(acb);
                 }
                 break;
 
         case ACB_ADAPTER_TYPE_B: {
-                arcmsr_abort_hbb_allcmd(acb);
+                rtnval = arcmsr_abort_hbb_allcmd(acb);
                 }
         }
+        return rtnval;
 }
 
 static void arcmsr_pci_unmap_dma(struct CommandControlBlock *ccb)
@@ -649,8 +737,7 @@ static u32 arcmsr_disable_outbound_ints(struct AdapterControlBlock *acb)
 
         case ACB_ADAPTER_TYPE_A : {
                 struct MessageUnit_A __iomem *reg = acb->pmuA;
-                orig_mask = readl(&reg->outbound_intmask)|\
-                                ARCMSR_MU_OUTBOUND_MESSAGE0_INTMASKENABLE;
+                orig_mask = readl(&reg->outbound_intmask);
                 writel(orig_mask|ARCMSR_MU_OUTBOUND_ALL_INTMASKENABLE, \
                                                 &reg->outbound_intmask);
                 }
@@ -658,8 +745,7 @@ static u32 arcmsr_disable_outbound_ints(struct AdapterControlBlock *acb)
 
         case ACB_ADAPTER_TYPE_B : {
                 struct MessageUnit_B *reg = acb->pmuB;
-                orig_mask = readl(reg->iop2drv_doorbell_mask_reg) & \
-                                        (~ARCMSR_IOP2DRV_MESSAGE_CMD_DONE);
+                orig_mask = readl(reg->iop2drv_doorbell_mask_reg);
                 writel(0, reg->iop2drv_doorbell_mask_reg);
                 }
                 break;
@@ -795,12 +881,13 @@ static void arcmsr_remove(struct pci_dev *pdev)
         struct AdapterControlBlock *acb =
                 (struct AdapterControlBlock *) host->hostdata;
         int poll_count = 0;
-
         arcmsr_free_sysfs_attr(acb);
         scsi_remove_host(host);
+        flush_scheduled_work();
+        del_timer_sync(&acb->eternal_timer);
+        arcmsr_disable_outbound_ints(acb);
         arcmsr_stop_adapter_bgrb(acb);
         arcmsr_flush_adapter_cache(acb);
-        arcmsr_disable_outbound_ints(acb);
         acb->acb_flags |= ACB_F_SCSISTOPADAPTER;
         acb->acb_flags &= ~ACB_F_IOP_INITED;
 
@@ -841,7 +928,9 @@ static void arcmsr_shutdown(struct pci_dev *pdev)
         struct Scsi_Host *host = pci_get_drvdata(pdev);
         struct AdapterControlBlock *acb =
                 (struct AdapterControlBlock *)host->hostdata;
-
+        del_timer_sync(&acb->eternal_timer);
+        arcmsr_disable_outbound_ints(acb);
+        flush_scheduled_work();
         arcmsr_stop_adapter_bgrb(acb);
         arcmsr_flush_adapter_cache(acb);
 }
@@ -861,7 +950,7 @@ static void arcmsr_module_exit(void)
 module_init(arcmsr_module_init);
 module_exit(arcmsr_module_exit);
 
-static void arcmsr_enable_outbound_ints(struct AdapterControlBlock *acb, \
+static void arcmsr_enable_outbound_ints(struct AdapterControlBlock *acb,
                                                 u32 intmask_org)
 {
         u32 mask;
@@ -871,7 +960,8 @@ static void arcmsr_enable_outbound_ints(struct AdapterControlBlock *acb, \
         case ACB_ADAPTER_TYPE_A : {
                 struct MessageUnit_A __iomem *reg = acb->pmuA;
                 mask = intmask_org & ~(ARCMSR_MU_OUTBOUND_POSTQUEUE_INTMASKENABLE |
-                             ARCMSR_MU_OUTBOUND_DOORBELL_INTMASKENABLE);
+                             ARCMSR_MU_OUTBOUND_DOORBELL_INTMASKENABLE|
+                             ARCMSR_MU_OUTBOUND_MESSAGE0_INTMASKENABLE);
                 writel(mask, &reg->outbound_intmask);
                 acb->outbound_int_enable = ~(intmask_org & mask) & 0x000000ff;
                 }
@@ -879,8 +969,10 @@ static void arcmsr_enable_outbound_ints(struct AdapterControlBlock *acb, \
 
         case ACB_ADAPTER_TYPE_B : {
                 struct MessageUnit_B *reg = acb->pmuB;
-                mask = intmask_org | (ARCMSR_IOP2DRV_DATA_WRITE_OK | \
-                        ARCMSR_IOP2DRV_DATA_READ_OK | ARCMSR_IOP2DRV_CDB_DONE);
+                mask = intmask_org | (ARCMSR_IOP2DRV_DATA_WRITE_OK |
+                        ARCMSR_IOP2DRV_DATA_READ_OK |
+                        ARCMSR_IOP2DRV_CDB_DONE |
+                        ARCMSR_IOP2DRV_MESSAGE_CMD_DONE);
                 writel(mask, reg->iop2drv_doorbell_mask_reg);
                 acb->outbound_int_enable = (intmask_org | mask) & 0x0000000f;
                 }
@@ -1048,8 +1140,8 @@ static void arcmsr_free_ccb_pool(struct AdapterControlBlock *acb)
         }
         case ACB_ADAPTER_TYPE_B: {
                 struct MessageUnit_B *reg = acb->pmuB;
-                iounmap(reg->drv2iop_doorbell_reg - ARCMSR_DRV2IOP_DOORBELL);
-                iounmap(reg->ioctl_wbuffer_reg - ARCMSR_IOCTL_WBUFFER);
+                iounmap((u8 *)reg->drv2iop_doorbell_reg - ARCMSR_DRV2IOP_DOORBELL);
+                iounmap((u8 *)reg->ioctl_wbuffer_reg - ARCMSR_IOCTL_WBUFFER);
                 dma_free_coherent(&acb->pdev->dev,
                 (ARCMSR_MAX_FREECCB_NUM * sizeof(struct CommandControlBlock) + 0x20 +
                 sizeof(struct MessageUnit_B)), acb->dma_coherent, acb->dma_coherent_handle);
@@ -1249,13 +1341,36 @@ static void arcmsr_hbb_postqueue_isr(struct AdapterControlBlock *acb)
                 reg->doneq_index = index;
         }
 }
+/*
+**********************************************************************************
+** Handle a message interrupt
+**
+** The only message interrupt we expect is in response to a query for the current adapter config.
+** We want this in order to compare the drivemap so that we can detect newly-attached drives.
+**********************************************************************************
+*/
+static void arcmsr_hba_message_isr(struct AdapterControlBlock *acb)
+{
+        struct MessageUnit_A *reg  = acb->pmuA;
+
+        /*clear interrupt and message state*/
+        writel(ARCMSR_MU_OUTBOUND_MESSAGE0_INT, &reg->outbound_intstatus);
+        schedule_work(&acb->arcmsr_do_message_isr_bh);
+}
+static void arcmsr_hbb_message_isr(struct AdapterControlBlock *acb)
+{
+        struct MessageUnit_B *reg  = acb->pmuB;
 
+        /*clear interrupt and message state*/
+        writel(ARCMSR_MESSAGE_INT_CLEAR_PATTERN, reg->iop2drv_doorbell_reg);
+        schedule_work(&acb->arcmsr_do_message_isr_bh);
+}
 static int arcmsr_handle_hba_isr(struct AdapterControlBlock *acb)
 {
         uint32_t outbound_intstatus;
         struct MessageUnit_A __iomem *reg = acb->pmuA;
 
-        outbound_intstatus = readl(&reg->outbound_intstatus) & \
+        outbound_intstatus = readl(&reg->outbound_intstatus) &
                                                         acb->outbound_int_enable;
         if (!(outbound_intstatus & ARCMSR_MU_OUTBOUND_HANDLE_INT))      {
                 return 1;
@@ -1267,6 +1382,10 @@ static int arcmsr_handle_hba_isr(struct AdapterControlBlock *acb)
         if (outbound_intstatus & ARCMSR_MU_OUTBOUND_POSTQUEUE_INT) {
                 arcmsr_hba_postqueue_isr(acb);
         }
+        if (outbound_intstatus & ARCMSR_MU_OUTBOUND_MESSAGE0_INT)       {
+                /* messenger of "driver to iop commands" */
+                arcmsr_hba_message_isr(acb);
+        }
         return 0;
 }
 
@@ -1275,13 +1394,14 @@ static int arcmsr_handle_hbb_isr(struct AdapterControlBlock *acb)
         uint32_t outbound_doorbell;
         struct MessageUnit_B *reg = acb->pmuB;
 
-        outbound_doorbell = readl(reg->iop2drv_doorbell_reg) & \
+        outbound_doorbell = readl(reg->iop2drv_doorbell_reg) &
                                                         acb->outbound_int_enable;
         if (!outbound_doorbell)
                 return 1;
 
         writel(~outbound_doorbell, reg->iop2drv_doorbell_reg);
-        /*in case the last action of doorbell interrupt clearance is cached, this action can push HW to write down the clear bit*/
+        /*in case the last action of doorbell interrupt clearance is cached,
+        this action can push HW to write down the clear bit*/
         readl(reg->iop2drv_doorbell_reg);
         writel(ARCMSR_DRV2IOP_END_OF_INTERRUPT, reg->drv2iop_doorbell_reg);
         if (outbound_doorbell & ARCMSR_IOP2DRV_DATA_WRITE_OK)   {
@@ -1293,6 +1413,10 @@ static int arcmsr_handle_hbb_isr(struct AdapterControlBlock *acb)
         if (outbound_doorbell & ARCMSR_IOP2DRV_CDB_DONE) {
                 arcmsr_hbb_postqueue_isr(acb);
         }
+        if (outbound_doorbell & ARCMSR_IOP2DRV_MESSAGE_CMD_DONE) {
+                /* messenger of "driver to iop commands" */
+                arcmsr_hbb_message_isr(acb);
+        }
 
         return 0;
 }
@@ -1360,7 +1484,7 @@ void arcmsr_post_ioctldata2iop(struct AdapterControlBlock *acb)
         }
 }
 
-static int arcmsr_iop_message_xfer(struct AdapterControlBlock *acb, \
+static int arcmsr_iop_message_xfer(struct AdapterControlBlock *acb,
                                         struct scsi_cmnd *cmd)
 {
         struct CMD_MESSAGE_FIELD *pcmdmessagefld;
@@ -1398,6 +1522,13 @@ static int arcmsr_iop_message_xfer(struct AdapterControlBlock *acb, \
                         retvalue = ARCMSR_MESSAGE_FAIL;
                         goto message_out;
                 }
+
+                if (!acb->fw_state) {
+                        pcmdmessagefld->cmdmessage.ReturnCode =
+                        ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+                        goto message_out;
+                }
+
                 ptmpQbuffer = ver_addr;
                 while ((acb->rqbuf_firstindex != acb->rqbuf_lastindex)
                         && (allxfer_len < 1031)) {
@@ -1444,6 +1575,12 @@ static int arcmsr_iop_message_xfer(struct AdapterControlBlock *acb, \
                         retvalue = ARCMSR_MESSAGE_FAIL;
                         goto message_out;
                 }
+                if (!acb->fw_state) {
+                        pcmdmessagefld->cmdmessage.ReturnCode =
+                        ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+                        goto message_out;
+                }
+
                 ptmpuserbuffer = ver_addr;
                 user_len = pcmdmessagefld->cmdmessage.Length;
                 memcpy(ptmpuserbuffer, pcmdmessagefld->messagedatabuffer, user_len);
@@ -1496,6 +1633,11 @@ static int arcmsr_iop_message_xfer(struct AdapterControlBlock *acb, \
 
         case ARCMSR_MESSAGE_CLEAR_RQBUFFER: {
                 uint8_t *pQbuffer = acb->rqbuffer;
+                if (!acb->fw_state) {
+                        pcmdmessagefld->cmdmessage.ReturnCode =
+                        ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+                        goto message_out;
+                }
 
                 if (acb->acb_flags & ACB_F_IOPDATA_OVERFLOW) {
                         acb->acb_flags &= ~ACB_F_IOPDATA_OVERFLOW;
@@ -1511,6 +1653,11 @@ static int arcmsr_iop_message_xfer(struct AdapterControlBlock *acb, \
 
         case ARCMSR_MESSAGE_CLEAR_WQBUFFER: {
                 uint8_t *pQbuffer = acb->wqbuffer;
+                if (!acb->fw_state) {
+                        pcmdmessagefld->cmdmessage.ReturnCode =
+                        ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+                        goto message_out;
+                }
 
                 if (acb->acb_flags & ACB_F_IOPDATA_OVERFLOW) {
                         acb->acb_flags &= ~ACB_F_IOPDATA_OVERFLOW;
@@ -1529,6 +1676,11 @@ static int arcmsr_iop_message_xfer(struct AdapterControlBlock *acb, \
 
         case ARCMSR_MESSAGE_CLEAR_ALLQBUFFER: {
                 uint8_t *pQbuffer;
+                if (!acb->fw_state) {
+                        pcmdmessagefld->cmdmessage.ReturnCode =
+                        ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+                        goto message_out;
+                }
 
                 if (acb->acb_flags & ACB_F_IOPDATA_OVERFLOW) {
                         acb->acb_flags &= ~ACB_F_IOPDATA_OVERFLOW;
@@ -1551,13 +1703,22 @@ static int arcmsr_iop_message_xfer(struct AdapterControlBlock *acb, \
                 break;
 
         case ARCMSR_MESSAGE_RETURN_CODE_3F: {
+                if (!acb->fw_state) {
+                        pcmdmessagefld->cmdmessage.ReturnCode =
+                        ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+                        goto message_out;
+                }
                 pcmdmessagefld->cmdmessage.ReturnCode = ARCMSR_MESSAGE_RETURNCODE_3F;
                 }
                 break;
 
         case ARCMSR_MESSAGE_SAY_HELLO: {
                 int8_t *hello_string = "Hello! I am ARCMSR";
-
+                if (!acb->fw_state) {
+                        pcmdmessagefld->cmdmessage.ReturnCode =
+                        ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+                        goto message_out;
+                }
                 memcpy(pcmdmessagefld->messagedatabuffer, hello_string
                         , (int16_t)strlen(hello_string));
                 pcmdmessagefld->cmdmessage.ReturnCode = ARCMSR_MESSAGE_RETURNCODE_OK;
@@ -1565,10 +1726,20 @@ static int arcmsr_iop_message_xfer(struct AdapterControlBlock *acb, \
                 break;
 
         case ARCMSR_MESSAGE_SAY_GOODBYE:
+                if (!acb->fw_state) {
+                        pcmdmessagefld->cmdmessage.ReturnCode =
+                        ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+                        goto message_out;
+                }
                 arcmsr_iop_parking(acb);
                 break;
 
         case ARCMSR_MESSAGE_FLUSH_ADAPTER_CACHE:
+                if (!acb->fw_state) {
+                        pcmdmessagefld->cmdmessage.ReturnCode =
+                        ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+                        goto message_out;
+                }
                 arcmsr_flush_adapter_cache(acb);
                 break;
 
@@ -1651,16 +1822,57 @@ static int arcmsr_queue_command(struct scsi_cmnd *cmd,
         struct CommandControlBlock *ccb;
         int target = cmd->device->id;
         int lun = cmd->device->lun;
-
+        uint8_t scsicmd = cmd->cmnd[0];
         cmd->scsi_done = done;
         cmd->host_scribble = NULL;
         cmd->result = 0;
+
+        if ((scsicmd == SYNCHRONIZE_CACHE) || (scsicmd == SEND_DIAGNOSTIC)) {
+                if (acb->devstate[target][lun] == ARECA_RAID_GONE) {
+                        cmd->result = (DID_NO_CONNECT << 16);
+                }
+                cmd->scsi_done(cmd);
+                return 0;
+        }
+
         if (acb->acb_flags & ACB_F_BUS_RESET) {
-                printk(KERN_NOTICE "arcmsr%d: bus reset"
-                        " and return busy \n"
-                        , acb->host->host_no);
+                switch (acb->adapter_type) {
+                        case ACB_ADAPTER_TYPE_A: {
+                                struct MessageUnit_A __iomem *reg = acb->pmuA;
+                                uint32_t intmask_org, outbound_doorbell;
+
+                                if ((readl(&reg->outbound_msgaddr1) &
+                                        ARCMSR_OUTBOUND_MESG1_FIRMWARE_OK) == 0) {
+                                        printk(KERN_NOTICE "arcmsr%d: bus reset and return busy\n",
+                                                acb->host->host_no);
                 return SCSI_MLQUEUE_HOST_BUSY;
         }
+
+                                acb->acb_flags &= ~ACB_F_FIRMWARE_TRAP;
+                                printk(KERN_NOTICE "arcmsr%d: hardware bus reset and reset ok\n",
+                                        acb->host->host_no);
+                                /* disable all outbound interrupt */
+                                intmask_org = arcmsr_disable_outbound_ints(acb);
+                                arcmsr_get_firmware_spec(acb, 1);
+                                /*start background rebuild*/
+                                arcmsr_start_adapter_bgrb(acb);
+                                /* clear Qbuffer if door bell ringed */
+                                outbound_doorbell = readl(&reg->outbound_doorbell);
+                                /*clear interrupt */
+                                writel(outbound_doorbell, &reg->outbound_doorbell);
+                                writel(ARCMSR_INBOUND_DRIVER_DATA_READ_OK,
+                                        &reg->inbound_doorbell);
+                                /* enable outbound Post Queue,outbound doorbell Interrupt */
+                                arcmsr_enable_outbound_ints(acb, intmask_org);
+                                acb->acb_flags |= ACB_F_IOP_INITED;
+                                acb->acb_flags &= ~ACB_F_BUS_RESET;
+                        }
+                        break;
+                        case ACB_ADAPTER_TYPE_B: {
+                        }
+                }
+        }
+
         if (target == 16) {
                 /* virtual device for iop message transfer */
                 arcmsr_handle_virtual_command(acb, cmd);
@@ -1699,21 +1911,25 @@ static int arcmsr_queue_command(struct scsi_cmnd *cmd,
         return 0;
 }
 
-static void arcmsr_get_hba_config(struct AdapterControlBlock *acb)
+static void *arcmsr_get_hba_config(struct AdapterControlBlock *acb, int mode)
 {
         struct MessageUnit_A __iomem *reg = acb->pmuA;
         char *acb_firm_model = acb->firm_model;
         char *acb_firm_version = acb->firm_version;
+        char *acb_device_map = acb->device_map;
         char __iomem *iop_firm_model = (char __iomem *)(&reg->message_rwbuffer[15]);
         char __iomem *iop_firm_version = (char __iomem *)(&reg->message_rwbuffer[17]);
+        char __iomem *iop_device_map = (char __iomem *) (&reg->message_rwbuffer[21]);
         int count;
 
         writel(ARCMSR_INBOUND_MESG0_GET_CONFIG, &reg->inbound_msgaddr0);
         if (arcmsr_hba_wait_msgint_ready(acb)) {
                 printk(KERN_NOTICE "arcmsr%d: wait 'get adapter firmware \
                         miscellaneous data' timeout \n", acb->host->host_no);
+                return NULL;
         }
 
+        if (mode == 1) {
         count = 8;
         while (count) {
                 *acb_firm_model = readb(iop_firm_model);
@@ -1730,34 +1946,48 @@ static void arcmsr_get_hba_config(struct AdapterControlBlock *acb)
                 count--;
         }
 
+                count = 16;
+                while (count) {
+                        *acb_device_map = readb(iop_device_map);
+                        acb_device_map++;
+                        iop_device_map++;
+                        count--;
+                }
+
         printk(KERN_INFO        "ARECA RAID ADAPTER%d: FIRMWARE VERSION %s \n"
                 , acb->host->host_no
                 , acb->firm_version);
-
+                acb->signature = readl(&reg->message_rwbuffer[0]);
         acb->firm_request_len = readl(&reg->message_rwbuffer[1]);
         acb->firm_numbers_queue = readl(&reg->message_rwbuffer[2]);
         acb->firm_sdram_size = readl(&reg->message_rwbuffer[3]);
         acb->firm_hd_channels = readl(&reg->message_rwbuffer[4]);
 }
-
-static void arcmsr_get_hbb_config(struct AdapterControlBlock *acb)
+        return reg->message_rwbuffer;
+}
+static void __iomem *arcmsr_get_hbb_config(struct AdapterControlBlock *acb, int mode)
 {
         struct MessageUnit_B *reg = acb->pmuB;
         uint32_t __iomem *lrwbuffer = reg->msgcode_rwbuffer_reg;
         char *acb_firm_model = acb->firm_model;
         char *acb_firm_version = acb->firm_version;
+        char *acb_device_map = acb->device_map;
         char __iomem *iop_firm_model = (char __iomem *)(&lrwbuffer[15]);
         /*firm_model,15,60-67*/
         char __iomem *iop_firm_version = (char __iomem *)(&lrwbuffer[17]);
         /*firm_version,17,68-83*/
+        char __iomem *iop_device_map = (char __iomem *) (&lrwbuffer[21]);
+        /*firm_version,21,84-99*/
         int count;
 
         writel(ARCMSR_MESSAGE_GET_CONFIG, reg->drv2iop_doorbell_reg);
         if (arcmsr_hbb_wait_msgint_ready(acb)) {
                 printk(KERN_NOTICE "arcmsr%d: wait 'get adapter firmware \
                         miscellaneous data' timeout \n", acb->host->host_no);
+                return NULL;
         }
 
+        if (mode == 1) {
         count = 8;
         while (count)
         {
@@ -1776,11 +2006,20 @@ static void arcmsr_get_hbb_config(struct AdapterControlBlock *acb)
                 count--;
         }
 
+                count = 16;
+                while (count) {
+                        *acb_device_map = readb(iop_device_map);
+                        acb_device_map++;
+                        iop_device_map++;
+                        count--;
+                }
+
         printk(KERN_INFO "ARECA RAID ADAPTER%d: FIRMWARE VERSION %s \n",
                         acb->host->host_no,
                         acb->firm_version);
 
-        lrwbuffer++;
+                acb->signature = readl(lrwbuffer++);
+                /*firm_signature,1,00-03*/
         acb->firm_request_len = readl(lrwbuffer++);
         /*firm_request_len,1,04-07*/
         acb->firm_numbers_queue = readl(lrwbuffer++);
@@ -1790,20 +2029,23 @@ static void arcmsr_get_hbb_config(struct AdapterControlBlock *acb)
         acb->firm_hd_channels = readl(lrwbuffer);
         /*firm_ide_channels,4,16-19*/
 }
-
-static void arcmsr_get_firmware_spec(struct AdapterControlBlock *acb)
+        return reg->msgcode_rwbuffer_reg;
+}
+static void *arcmsr_get_firmware_spec(struct AdapterControlBlock *acb, int mode)
 {
+        void *rtnval = 0;
         switch (acb->adapter_type) {
         case ACB_ADAPTER_TYPE_A: {
-                arcmsr_get_hba_config(acb);
+                rtnval = arcmsr_get_hba_config(acb, mode);
                 }
                 break;
 
         case ACB_ADAPTER_TYPE_B: {
-                arcmsr_get_hbb_config(acb);
+                rtnval = arcmsr_get_hbb_config(acb, mode);
                 }
                 break;
         }
+        return rtnval;
 }
 
 static void arcmsr_polling_hba_ccbdone(struct AdapterControlBlock *acb,
@@ -2043,6 +2285,66 @@ static void arcmsr_wait_firmware_ready(struct AdapterControlBlock *acb)
         }
 }
 
+static void arcmsr_request_hba_device_map(struct AdapterControlBlock *acb)
+{
+        struct MessageUnit_A __iomem *reg = acb->pmuA;
+
+        if (unlikely(atomic_read(&acb->rq_map_token) == 0)) {
+                acb->fw_state = false;
+        } else {
+        /*to prevent rq_map_token from changing by other interrupt, then
+        avoid the dead-lock*/
+                acb->fw_state = true;
+                atomic_dec(&acb->rq_map_token);
+                if (!(acb->fw_state) ||
+                        (acb->ante_token_value == atomic_read(&acb->rq_map_token))) {
+                        atomic_set(&acb->rq_map_token, 16);
+                }
+                acb->ante_token_value = atomic_read(&acb->rq_map_token);
+                writel(ARCMSR_INBOUND_MESG0_GET_CONFIG, &reg->inbound_msgaddr0);
+        }
+        mod_timer(&acb->eternal_timer, jiffies + msecs_to_jiffies(6000));
+        return;
+}
+
+static void arcmsr_request_hbb_device_map(struct AdapterControlBlock *acb)
+{
+        struct MessageUnit_B __iomem *reg = acb->pmuB;
+
+        if (unlikely(atomic_read(&acb->rq_map_token) == 0)) {
+                acb->fw_state = false;
+        } else {
+        /*to prevent rq_map_token from changing by other interrupt, then
+        avoid the dead-lock*/
+                acb->fw_state = true;
+                atomic_dec(&acb->rq_map_token);
+                if (!(acb->fw_state) ||
+                        (acb->ante_token_value == atomic_read(&acb->rq_map_token))) {
+                        atomic_set(&acb->rq_map_token, 16);
+                }
+                acb->ante_token_value = atomic_read(&acb->rq_map_token);
+                writel(ARCMSR_MESSAGE_GET_CONFIG, reg->drv2iop_doorbell_reg);
+        }
+        mod_timer(&acb->eternal_timer, jiffies + msecs_to_jiffies(6000));
+        return;
+}
+
+static void arcmsr_request_device_map(unsigned long pacb)
+{
+        struct AdapterControlBlock *acb = (struct AdapterControlBlock *)pacb;
+
+        switch (acb->adapter_type) {
+                case ACB_ADAPTER_TYPE_A: {
+                        arcmsr_request_hba_device_map(acb);
+                }
+                break;
+                case ACB_ADAPTER_TYPE_B: {
+                        arcmsr_request_hbb_device_map(acb);
+                }
+                break;
+        }
+}
+
 static void arcmsr_start_hba_bgrb(struct AdapterControlBlock *acb)
 {
         struct MessageUnit_A __iomem *reg = acb->pmuA;
@@ -2121,6 +2423,60 @@ static void arcmsr_enable_eoi_mode(struct AdapterControlBlock *acb)
         return;
 }
 
+static void arcmsr_hardware_reset(struct AdapterControlBlock *acb)
+{
+        uint8_t value[64];
+        int i;
+
+        /* backup pci config data */
+        for (i = 0; i < 64; i++) {
+                pci_read_config_byte(acb->pdev, i, &value[i]);
+        }
+        /* hardware reset signal */
+        pci_write_config_byte(acb->pdev, 0x84, 0x20);
+        msleep(1000);
+        /* write back pci config data */
+        for (i = 0; i < 64; i++) {
+                pci_write_config_byte(acb->pdev, i, value[i]);
+        }
+        msleep(1000);
+        return;
+}
+/*
+****************************************************************************
+****************************************************************************
+*/
+#ifdef CONFIG_SCSI_ARCMSR_RESET
+        int arcmsr_sleep_for_bus_reset(struct scsi_cmnd *cmd)
+        {
+                        struct Scsi_Host *shost = NULL;
+                        spinlock_t *host_lock = NULL;
+                        int i, isleep;
+
+                        shost = cmd->device->host;
+                        host_lock = shost->host_lock;
+
+                        printk(KERN_NOTICE "Host %d bus reset over, sleep %d seconds (busy %d, can queue %d) ...........\n",
+                                        shost->host_no, sleeptime, shost->host_busy, shost->can_queue);
+                        isleep = sleeptime / 10;
+                        spin_unlock_irq(host_lock);
+                        if (isleep > 0) {
+                                for (i = 0; i < isleep; i++) {
+                                        msleep(10000);
+                                        printk(KERN_NOTICE "^%d^\n", i);
+                                }
+                        }
+
+                        isleep = sleeptime % 10;
+                        if (isleep > 0) {
+                                msleep(isleep * 1000);
+                                printk(KERN_NOTICE "^v^\n");
+                        }
+                        spin_lock_irq(host_lock);
+                        printk(KERN_NOTICE "***** wake up *****\n");
+                        return 0;
+        }
+#endif
 static void arcmsr_iop_init(struct AdapterControlBlock *acb)
 {
         uint32_t intmask_org;
@@ -2129,7 +2485,7 @@ static void arcmsr_iop_init(struct AdapterControlBlock *acb)
        intmask_org = arcmsr_disable_outbound_ints(acb);
         arcmsr_wait_firmware_ready(acb);
         arcmsr_iop_confirm(acb);
-        arcmsr_get_firmware_spec(acb);
+        arcmsr_get_firmware_spec(acb, 1);
         /*start background rebuild*/
         arcmsr_start_adapter_bgrb(acb);
         /* empty doorbell Qbuffer if door bell ringed */
@@ -2140,51 +2496,110 @@ static void arcmsr_iop_init(struct AdapterControlBlock *acb)
         acb->acb_flags |= ACB_F_IOP_INITED;
 }
 
-static void arcmsr_iop_reset(struct AdapterControlBlock *acb)
+static uint8_t arcmsr_iop_reset(struct AdapterControlBlock *acb)
 {
         struct CommandControlBlock *ccb;
         uint32_t intmask_org;
+        uint8_t rtnval = 0x00;
         int i = 0;
 
         if (atomic_read(&acb->ccboutstandingcount) != 0) {
+                /* disable all outbound interrupt */
+                intmask_org = arcmsr_disable_outbound_ints(acb);
                 /* talk to iop 331 outstanding command aborted */
-                arcmsr_abort_allcmd(acb);
-
+                rtnval = arcmsr_abort_allcmd(acb);
                 /* wait for 3 sec for all command aborted*/
                 ssleep(3);
-
-                /* disable all outbound interrupt */
-                intmask_org = arcmsr_disable_outbound_ints(acb);
                 /* clear all outbound posted Q */
                 arcmsr_done4abort_postqueue(acb);
                 for (i = 0; i < ARCMSR_MAX_FREECCB_NUM; i++) {
                         ccb = acb->pccb_pool[i];
                         if (ccb->startdone == ARCMSR_CCB_START) {
-                                ccb->startdone = ARCMSR_CCB_ABORTED;
                                 arcmsr_ccb_complete(ccb, 1);
                         }
                 }
+                atomic_set(&acb->ccboutstandingcount, 0);
                 /* enable all outbound interrupt */
                 arcmsr_enable_outbound_ints(acb, intmask_org);
+                return rtnval;
         }
+        return rtnval;
 }
 
 static int arcmsr_bus_reset(struct scsi_cmnd *cmd)
 {
         struct AdapterControlBlock *acb =
                 (struct AdapterControlBlock *)cmd->device->host->hostdata;
-        int i;
+        int retry = 0;
 
-        acb->num_resets++;
+        if (acb->acb_flags & ACB_F_BUS_RESET)
+                return SUCCESS;
+
+        printk(KERN_NOTICE "arcmsr%d: bus reset ..... \n", acb->adapter_index);
         acb->acb_flags |= ACB_F_BUS_RESET;
-        for (i = 0; i < 400; i++) {
-                if (!atomic_read(&acb->ccboutstandingcount))
+        acb->num_resets++;
+        while (atomic_read(&acb->ccboutstandingcount) != 0 && retry < 4) {
+                arcmsr_interrupt(acb);
+                retry++;
+        }
+
+        if (arcmsr_iop_reset(acb)) {
+                switch (acb->adapter_type) {
+                case ACB_ADAPTER_TYPE_A: {
+                        printk(KERN_NOTICE "arcmsr%d: do hardware bus reset, num_resets = %d num_aborts = %d \n",
+                                acb->adapter_index, acb->num_resets, acb->num_aborts);
+                        arcmsr_hardware_reset(acb);
+                        acb->acb_flags |= ACB_F_FIRMWARE_TRAP;
+                        acb->acb_flags &= ~ACB_F_IOP_INITED;
+                        #ifdef CONFIG_SCSI_ARCMSR_RESET
+                        struct MessageUnit_A __iomem *reg = acb->pmuA;
+                        uint32_t intmask_org, outbound_doorbell;
+                        int retry_count = 0;
+sleep_again:
+                        arcmsr_sleep_for_bus_reset(cmd);
+                        if ((readl(&reg->outbound_msgaddr1) &
+                        ARCMSR_OUTBOUND_MESG1_FIRMWARE_OK) == 0) {
+                        printk(KERN_NOTICE "arcmsr%d: hardware bus reset and return busy, retry=%d \n",
+                        acb->host->host_no, retry_count);
+                        if (retry_count > retrycount) {
+                                printk(KERN_NOTICE "arcmsr%d: hardware bus reset and return busy, retry aborted \n",
+                                acb->host->host_no);
+                        return SUCCESS;
+                        }
+                        retry_count++;
+                        goto sleep_again;
+                        }
+                        acb->acb_flags &= ~ACB_F_FIRMWARE_TRAP;
+                        acb->acb_flags |= ACB_F_IOP_INITED;
+                        acb->acb_flags &= ~ACB_F_BUS_RESET;
+                        printk(KERN_NOTICE "arcmsr%d: hardware bus reset and reset ok \n",
+                                acb->host->host_no);
+                        /* disable all outbound interrupt */
+                        intmask_org = arcmsr_disable_outbound_ints(acb);
+                        arcmsr_get_firmware_spec(acb, 1);
+                        /*start background rebuild*/
+                        arcmsr_start_adapter_bgrb(acb);
+                        /* clear Qbuffer if door bell ringed */
+                        outbound_doorbell = readl(&reg->outbound_doorbell);
+                        writel(outbound_doorbell, &reg->outbound_doorbell); /*clear interrupt */
+                        writel(ARCMSR_INBOUND_DRIVER_DATA_READ_OK, &reg->inbound_doorbell);
+                        /* enable outbound Post Queue,outbound doorbell Interrupt */
+                        arcmsr_enable_outbound_ints(acb, intmask_org);
+                        atomic_set(&acb->rq_map_token, 16);
+                        init_timer(&acb->eternal_timer);
+                        acb->eternal_timer.expires = jiffies + msecs_to_jiffies(20*HZ);
+                        acb->eternal_timer.data = (unsigned long) acb;
+                        acb->eternal_timer.function = &arcmsr_request_device_map;
+                        add_timer(&acb->eternal_timer);
+                        #endif
+                }
                         break;
-                arcmsr_interrupt(acb);/* FIXME: need spinlock */
-                msleep(25);
+                case ACB_ADAPTER_TYPE_B: {
         }
-        arcmsr_iop_reset(acb);
+                }
+        } else {
         acb->acb_flags &= ~ACB_F_BUS_RESET;
+        }
         return SUCCESS;
 }
 
@@ -2277,98 +2692,3 @@ static const char *arcmsr_info(struct Scsi_Host *host)
                         ARCMSR_DRIVER_VERSION);
         return buf;
 }
-#ifdef CONFIG_SCSI_ARCMSR_AER
-static pci_ers_result_t arcmsr_pci_slot_reset(struct pci_dev *pdev)
-{
-        struct Scsi_Host *host = pci_get_drvdata(pdev);
-        struct AdapterControlBlock *acb =
-                (struct AdapterControlBlock *) host->hostdata;
-        uint32_t intmask_org;
-        int i, j;
-
-        if (pci_enable_device(pdev)) {
-                return PCI_ERS_RESULT_DISCONNECT;
-        }
-        pci_set_master(pdev);
-        intmask_org = arcmsr_disable_outbound_ints(acb);
-        acb->acb_flags |= (ACB_F_MESSAGE_WQBUFFER_CLEARED |
-                           ACB_F_MESSAGE_RQBUFFER_CLEARED |
-                           ACB_F_MESSAGE_WQBUFFER_READED);
-        acb->acb_flags &= ~ACB_F_SCSISTOPADAPTER;
-        for (i = 0; i < ARCMSR_MAX_TARGETID; i++)
-                for (j = 0; j < ARCMSR_MAX_TARGETLUN; j++)
-                        acb->devstate[i][j] = ARECA_RAID_GONE;
-
-        arcmsr_wait_firmware_ready(acb);
-        arcmsr_iop_confirm(acb);
-       /* disable all outbound interrupt */
-        arcmsr_get_firmware_spec(acb);
-        /*start background rebuild*/
-        arcmsr_start_adapter_bgrb(acb);
-        /* empty doorbell Qbuffer if door bell ringed */
-        arcmsr_clear_doorbell_queue_buffer(acb);
-        arcmsr_enable_eoi_mode(acb);
-        /* enable outbound Post Queue,outbound doorbell Interrupt */
-        arcmsr_enable_outbound_ints(acb, intmask_org);
-        acb->acb_flags |= ACB_F_IOP_INITED;
-
-        pci_enable_pcie_error_reporting(pdev);
-        return PCI_ERS_RESULT_RECOVERED;
-}
-
-static void arcmsr_pci_ers_need_reset_forepart(struct pci_dev *pdev)
-{
-        struct Scsi_Host *host = pci_get_drvdata(pdev);
-        struct AdapterControlBlock *acb = (struct AdapterControlBlock *)host->hostdata;
-        struct CommandControlBlock *ccb;
-        uint32_t intmask_org;
-        int i = 0;
-
-        if (atomic_read(&acb->ccboutstandingcount) != 0) {
-                /* talk to iop 331 outstanding command aborted */
-                arcmsr_abort_allcmd(acb);
-                /* wait for 3 sec for all command aborted*/
-                ssleep(3);
-                /* disable all outbound interrupt */
-                intmask_org = arcmsr_disable_outbound_ints(acb);
-                /* clear all outbound posted Q */
-                arcmsr_done4abort_postqueue(acb);
-                for (i = 0; i < ARCMSR_MAX_FREECCB_NUM; i++) {
-                        ccb = acb->pccb_pool[i];
-                        if (ccb->startdone == ARCMSR_CCB_START) {
-                                ccb->startdone = ARCMSR_CCB_ABORTED;
-                                arcmsr_ccb_complete(ccb, 1);
-                        }
-                }
-                /* enable all outbound interrupt */
-                arcmsr_enable_outbound_ints(acb, intmask_org);
-        }
-        pci_disable_device(pdev);
-}
-
-static void arcmsr_pci_ers_disconnect_forepart(struct pci_dev *pdev)
-{
-                        struct Scsi_Host *host = pci_get_drvdata(pdev);
-                        struct AdapterControlBlock *acb = \
-                                (struct AdapterControlBlock *)host->hostdata;
-
-                        arcmsr_stop_adapter_bgrb(acb);
-                        arcmsr_flush_adapter_cache(acb);
-}
-
-static pci_ers_result_t arcmsr_pci_error_detected(struct pci_dev *pdev,
-                                                pci_channel_state_t state)
-{
-        switch (state) {
-        case pci_channel_io_frozen:
-                        arcmsr_pci_ers_need_reset_forepart(pdev);
-                        return PCI_ERS_RESULT_NEED_RESET;
-        case pci_channel_io_perm_failure:
-                        arcmsr_pci_ers_disconnect_forepart(pdev);
-                        return PCI_ERS_RESULT_DISCONNECT;
-                        break;
-        default:
-                        return PCI_ERS_RESULT_NEED_RESET;
-          }
-}
-#endif
diff --git a/drivers/scsi/be2iscsi/be_mgmt.c b/drivers/scsi/be2iscsi/be_mgmt.c
index e641922f20bc..350cbeaae160 100644
--- a/drivers/scsi/be2iscsi/be_mgmt.c
+++ b/drivers/scsi/be2iscsi/be_mgmt.c
@@ -167,10 +167,9 @@ unsigned char mgmt_invalidate_icds(struct beiscsi_hba *phba,
                                 &nonemb_cmd.dma);
         if (nonemb_cmd.va == NULL) {
                 SE_DEBUG(DBG_LVL_1,
-                         "Failed to allocate memory for"
-                         "mgmt_invalidate_icds \n");
+                         "Failed to allocate memory for mgmt_invalidate_icds\n");
                 spin_unlock(&ctrl->mbox_lock);
-                return -1;
+                return 0;
         }
         nonemb_cmd.size = sizeof(struct invalidate_commands_params_in);
         req = nonemb_cmd.va;
diff --git a/drivers/scsi/bfa/bfa_core.c b/drivers/scsi/bfa/bfa_core.c
index 0c08e185a766..3a7b3f88932f 100644
--- a/drivers/scsi/bfa/bfa_core.c
+++ b/drivers/scsi/bfa/bfa_core.c
@@ -84,11 +84,32 @@ bfa_cfg_get_meminfo(struct bfa_iocfc_cfg_s *cfg, struct bfa_meminfo_s *meminfo)
         for (i = 0; hal_mods[i]; i++)
                 hal_mods[i]->meminfo(cfg, &km_len, &dm_len);
 
+        dm_len += bfa_port_meminfo();
 
         meminfo->meminfo[BFA_MEM_TYPE_KVA - 1].mem_len = km_len;
         meminfo->meminfo[BFA_MEM_TYPE_DMA - 1].mem_len = dm_len;
 }
 
+static void
+bfa_com_port_attach(struct bfa_s *bfa, struct bfa_meminfo_s *mi)
+{
+        struct bfa_port_s       *port = &bfa->modules.port;
+        uint32_t                dm_len;
+        uint8_t                 *dm_kva;
+        uint64_t                dm_pa;
+
+        dm_len = bfa_port_meminfo();
+        dm_kva = bfa_meminfo_dma_virt(mi);
+        dm_pa  = bfa_meminfo_dma_phys(mi);
+
+        memset(port, 0, sizeof(struct bfa_port_s));
+        bfa_port_attach(port, &bfa->ioc, bfa, bfa->trcmod, bfa->logm);
+        bfa_port_mem_claim(port, dm_kva, dm_pa);
+
+        bfa_meminfo_dma_virt(mi) = dm_kva + dm_len;
+        bfa_meminfo_dma_phys(mi) = dm_pa + dm_len;
+}
+
 /**
  * Use this function to do attach the driver instance with the BFA
  * library. This function will not trigger any HW initialization
@@ -140,6 +161,7 @@ bfa_attach(struct bfa_s *bfa, void *bfad, struct bfa_iocfc_cfg_s *cfg,
         for (i = 0; hal_mods[i]; i++)
                 hal_mods[i]->attach(bfa, bfad, cfg, meminfo, pcidev);
 
+        bfa_com_port_attach(bfa, meminfo);
 }
 
 /**
diff --git a/drivers/scsi/gvp11.c b/drivers/scsi/gvp11.c
index 18b7102bb80e..2ce26eb7a1ec 100644
--- a/drivers/scsi/gvp11.c
+++ b/drivers/scsi/gvp11.c
@@ -1,36 +1,35 @@
 #include <linux/types.h>
-#include <linux/mm.h>
-#include <linux/slab.h>
-#include <linux/blkdev.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/zorro.h>
 
-#include <asm/setup.h>
 #include <asm/page.h>
 #include <asm/pgtable.h>
 #include <asm/amigaints.h>
 #include <asm/amigahw.h>
-#include <linux/zorro.h>
-#include <asm/irq.h>
-#include <linux/spinlock.h>
 
 #include "scsi.h"
-#include <scsi/scsi_host.h>
 #include "wd33c93.h"
 #include "gvp11.h"
 
-#include <linux/stat.h>
 
+#define CHECK_WD33C93
 
-#define DMA(ptr)        ((gvp11_scsiregs *)((ptr)->base))
+struct gvp11_hostdata {
+        struct WD33C93_hostdata wh;
+        struct gvp11_scsiregs *regs;
+};
 
-static irqreturn_t gvp11_intr(int irq, void *_instance)
+static irqreturn_t gvp11_intr(int irq, void *data)
 {
+        struct Scsi_Host *instance = data;
+        struct gvp11_hostdata *hdata = shost_priv(instance);
+        unsigned int status = hdata->regs->CNTR;
         unsigned long flags;
-        unsigned int status;
-        struct Scsi_Host *instance = (struct Scsi_Host *)_instance;
 
-        status = DMA(instance)->CNTR;
         if (!(status & GVP11_DMAC_INT_PENDING))
                 return IRQ_NONE;
 
@@ -50,64 +49,66 @@ void gvp11_setup(char *str, int *ints)
 static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
 {
         struct Scsi_Host *instance = cmd->device->host;
-        struct WD33C93_hostdata *hdata = shost_priv(instance);
+        struct gvp11_hostdata *hdata = shost_priv(instance);
+        struct WD33C93_hostdata *wh = &hdata->wh;
+        struct gvp11_scsiregs *regs = hdata->regs;
         unsigned short cntr = GVP11_DMAC_INT_ENABLE;
         unsigned long addr = virt_to_bus(cmd->SCp.ptr);
         int bank_mask;
         static int scsi_alloc_out_of_range = 0;
 
         /* use bounce buffer if the physical address is bad */
-        if (addr & hdata->dma_xfer_mask) {
-                hdata->dma_bounce_len = (cmd->SCp.this_residual + 511) & ~0x1ff;
+        if (addr & wh->dma_xfer_mask) {
+                wh->dma_bounce_len = (cmd->SCp.this_residual + 511) & ~0x1ff;
 
                 if (!scsi_alloc_out_of_range) {
-                        hdata->dma_bounce_buffer =
-                                kmalloc(hdata->dma_bounce_len, GFP_KERNEL);
-                        hdata->dma_buffer_pool = BUF_SCSI_ALLOCED;
+                        wh->dma_bounce_buffer =
+                                kmalloc(wh->dma_bounce_len, GFP_KERNEL);
+                        wh->dma_buffer_pool = BUF_SCSI_ALLOCED;
                 }
 
                 if (scsi_alloc_out_of_range ||
-                    !hdata->dma_bounce_buffer) {
-                        hdata->dma_bounce_buffer =
-                                amiga_chip_alloc(hdata->dma_bounce_len,
+                    !wh->dma_bounce_buffer) {
+                        wh->dma_bounce_buffer =
+                                amiga_chip_alloc(wh->dma_bounce_len,
                                                  "GVP II SCSI Bounce Buffer");
 
-                        if (!hdata->dma_bounce_buffer) {
-                                hdata->dma_bounce_len = 0;
+                        if (!wh->dma_bounce_buffer) {
+                                wh->dma_bounce_len = 0;
                                 return 1;
                         }
 
-                        hdata->dma_buffer_pool = BUF_CHIP_ALLOCED;
+                        wh->dma_buffer_pool = BUF_CHIP_ALLOCED;
                 }
 
                 /* check if the address of the bounce buffer is OK */
-                addr = virt_to_bus(hdata->dma_bounce_buffer);
+                addr = virt_to_bus(wh->dma_bounce_buffer);
 
-                if (addr & hdata->dma_xfer_mask) {
+                if (addr & wh->dma_xfer_mask) {
                         /* fall back to Chip RAM if address out of range */
-                        if (hdata->dma_buffer_pool == BUF_SCSI_ALLOCED) {
-                                kfree(hdata->dma_bounce_buffer);
+                        if (wh->dma_buffer_pool == BUF_SCSI_ALLOCED) {
+                                kfree(wh->dma_bounce_buffer);
                                 scsi_alloc_out_of_range = 1;
                         } else {
-                                amiga_chip_free(hdata->dma_bounce_buffer);
+                                amiga_chip_free(wh->dma_bounce_buffer);
                         }
 
-                        hdata->dma_bounce_buffer =
-                                amiga_chip_alloc(hdata->dma_bounce_len,
+                        wh->dma_bounce_buffer =
+                                amiga_chip_alloc(wh->dma_bounce_len,
                                                  "GVP II SCSI Bounce Buffer");
 
-                        if (!hdata->dma_bounce_buffer) {
-                                hdata->dma_bounce_len = 0;
+                        if (!wh->dma_bounce_buffer) {
+                                wh->dma_bounce_len = 0;
                                 return 1;
                         }
 
-                        addr = virt_to_bus(hdata->dma_bounce_buffer);
-                        hdata->dma_buffer_pool = BUF_CHIP_ALLOCED;
+                        addr = virt_to_bus(wh->dma_bounce_buffer);
+                        wh->dma_buffer_pool = BUF_CHIP_ALLOCED;
                 }
 
                 if (!dir_in) {
                         /* copy to bounce buffer for a write */
-                        memcpy(hdata->dma_bounce_buffer, cmd->SCp.ptr,
+                        memcpy(wh->dma_bounce_buffer, cmd->SCp.ptr,
                                cmd->SCp.this_residual);
                 }
         }
@@ -116,11 +117,11 @@ static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
         if (!dir_in)
                 cntr |= GVP11_DMAC_DIR_WRITE;
 
-        hdata->dma_dir = dir_in;
-        DMA(cmd->device->host)->CNTR = cntr;
+        wh->dma_dir = dir_in;
+        regs->CNTR = cntr;
 
         /* setup DMA *physical* address */
-        DMA(cmd->device->host)->ACR = addr;
+        regs->ACR = addr;
 
         if (dir_in) {
                 /* invalidate any cache */
@@ -130,12 +131,12 @@ static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
                 cache_push(addr, cmd->SCp.this_residual);
         }
 
-        bank_mask = (~hdata->dma_xfer_mask >> 18) & 0x01c0;
+        bank_mask = (~wh->dma_xfer_mask >> 18) & 0x01c0;
         if (bank_mask)
-                DMA(cmd->device->host)->BANK = bank_mask & (addr >> 18);
+                regs->BANK = bank_mask & (addr >> 18);
 
         /* start DMA */
-        DMA(cmd->device->host)->ST_DMA = 1;
+        regs->ST_DMA = 1;
 
         /* return success */
         return 0;
@@ -144,236 +145,53 @@ static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
 static void dma_stop(struct Scsi_Host *instance, struct scsi_cmnd *SCpnt,
                      int status)
 {
-        struct WD33C93_hostdata *hdata = shost_priv(instance);
+        struct gvp11_hostdata *hdata = shost_priv(instance);
+        struct WD33C93_hostdata *wh = &hdata->wh;
+        struct gvp11_scsiregs *regs = hdata->regs;
 
         /* stop DMA */
-        DMA(instance)->SP_DMA = 1;
+        regs->SP_DMA = 1;
         /* remove write bit from CONTROL bits */
-        DMA(instance)->CNTR = GVP11_DMAC_INT_ENABLE;
+        regs->CNTR = GVP11_DMAC_INT_ENABLE;
 
         /* copy from a bounce buffer, if necessary */
-        if (status && hdata->dma_bounce_buffer) {
-                if (hdata->dma_dir && SCpnt)
-                        memcpy(SCpnt->SCp.ptr, hdata->dma_bounce_buffer,
+        if (status && wh->dma_bounce_buffer) {
+                if (wh->dma_dir && SCpnt)
+                        memcpy(SCpnt->SCp.ptr, wh->dma_bounce_buffer,
                                SCpnt->SCp.this_residual);
 
-                if (hdata->dma_buffer_pool == BUF_SCSI_ALLOCED)
-                        kfree(hdata->dma_bounce_buffer);
-                else
-                        amiga_chip_free(hdata->dma_bounce_buffer);
-
-                hdata->dma_bounce_buffer = NULL;
-                hdata->dma_bounce_len = 0;
-        }
-}
-
-#define CHECK_WD33C93
-
-int __init gvp11_detect(struct scsi_host_template *tpnt)
-{
-        static unsigned char called = 0;
-        struct Scsi_Host *instance;
-        unsigned long address;
-        unsigned int epc;
-        struct zorro_dev *z = NULL;
-        unsigned int default_dma_xfer_mask;
-        struct WD33C93_hostdata *hdata;
-        wd33c93_regs regs;
-        int num_gvp11 = 0;
-#ifdef CHECK_WD33C93
-        volatile unsigned char *sasr_3393, *scmd_3393;
-        unsigned char save_sasr;
-        unsigned char q, qq;
-#endif
-
-        if (!MACH_IS_AMIGA || called)
-                return 0;
-        called = 1;
-
-        tpnt->proc_name = "GVP11";
-        tpnt->proc_info = &wd33c93_proc_info;
-
-        while ((z = zorro_find_device(ZORRO_WILDCARD, z))) {
-                /*
-                 * This should (hopefully) be the correct way to identify
-                 * all the different GVP SCSI controllers (except for the
-                 * SERIES I though).
-                 */
-
-                if (z->id == ZORRO_PROD_GVP_COMBO_030_R3_SCSI ||
-                    z->id == ZORRO_PROD_GVP_SERIES_II)
-                        default_dma_xfer_mask = ~0x00ffffff;
-                else if (z->id == ZORRO_PROD_GVP_GFORCE_030_SCSI ||
-                         z->id == ZORRO_PROD_GVP_A530_SCSI ||
-                         z->id == ZORRO_PROD_GVP_COMBO_030_R4_SCSI)
-                        default_dma_xfer_mask = ~0x01ffffff;
-                else if (z->id == ZORRO_PROD_GVP_A1291 ||
-                         z->id == ZORRO_PROD_GVP_GFORCE_040_SCSI_1)
-                        default_dma_xfer_mask = ~0x07ffffff;
+                if (wh->dma_buffer_pool == BUF_SCSI_ALLOCED)
+                        kfree(wh->dma_bounce_buffer);
                 else
-                        continue;
-
-                /*
-                 * Rumors state that some GVP ram boards use the same product
-                 * code as the SCSI controllers. Therefore if the board-size
-                 * is not 64KB we asume it is a ram board and bail out.
-                 */
-                if (z->resource.end - z->resource.start != 0xffff)
-                        continue;
+                        amiga_chip_free(wh->dma_bounce_buffer);
 
-                address = z->resource.start;
-                if (!request_mem_region(address, 256, "wd33c93"))
-                        continue;
-
-#ifdef CHECK_WD33C93
-
-                /*
-                 * These darn GVP boards are a problem - it can be tough to tell
-                 * whether or not they include a SCSI controller. This is the
-                 * ultimate Yet-Another-GVP-Detection-Hack in that it actually
-                 * probes for a WD33c93 chip: If we find one, it's extremely
-                 * likely that this card supports SCSI, regardless of Product_
-                 * Code, Board_Size, etc.
-                 */
-
-                /* Get pointers to the presumed register locations and save contents */
-
-                sasr_3393 = &(((gvp11_scsiregs *)(ZTWO_VADDR(address)))->SASR);
-                scmd_3393 = &(((gvp11_scsiregs *)(ZTWO_VADDR(address)))->SCMD);
-                save_sasr = *sasr_3393;
-
-                /* First test the AuxStatus Reg */
-
-                q = *sasr_3393; /* read it */
-                if (q & 0x08)   /* bit 3 should always be clear */
-                        goto release;
-                *sasr_3393 = WD_AUXILIARY_STATUS;       /* setup indirect address */
-                if (*sasr_3393 == WD_AUXILIARY_STATUS) {        /* shouldn't retain the write */
-                        *sasr_3393 = save_sasr; /* Oops - restore this byte */
-                        goto release;
-                }
-                if (*sasr_3393 != q) {  /* should still read the same */
-                        *sasr_3393 = save_sasr; /* Oops - restore this byte */
-                        goto release;
-                }
-                if (*scmd_3393 != q)    /* and so should the image at 0x1f */
-                        goto release;
-
-                /*
-                 * Ok, we probably have a wd33c93, but let's check a few other places
-                 * for good measure. Make sure that this works for both 'A and 'B
-                 * chip versions.
-                 */
-
-                *sasr_3393 = WD_SCSI_STATUS;
-                q = *scmd_3393;
-                *sasr_3393 = WD_SCSI_STATUS;
-                *scmd_3393 = ~q;
-                *sasr_3393 = WD_SCSI_STATUS;
-                qq = *scmd_3393;
-                *sasr_3393 = WD_SCSI_STATUS;
-                *scmd_3393 = q;
-                if (qq != q)    /* should be read only */
-                        goto release;
-                *sasr_3393 = 0x1e;      /* this register is unimplemented */
-                q = *scmd_3393;
-                *sasr_3393 = 0x1e;
-                *scmd_3393 = ~q;
-                *sasr_3393 = 0x1e;
-                qq = *scmd_3393;
-                *sasr_3393 = 0x1e;
-                *scmd_3393 = q;
-                if (qq != q || qq != 0xff)      /* should be read only, all 1's */
-                        goto release;
-                *sasr_3393 = WD_TIMEOUT_PERIOD;
-                q = *scmd_3393;
-                *sasr_3393 = WD_TIMEOUT_PERIOD;
-                *scmd_3393 = ~q;
-                *sasr_3393 = WD_TIMEOUT_PERIOD;
-                qq = *scmd_3393;
-                *sasr_3393 = WD_TIMEOUT_PERIOD;
-                *scmd_3393 = q;
-                if (qq != (~q & 0xff))  /* should be read/write */
-                        goto release;
-#endif
-
-                instance = scsi_register(tpnt, sizeof(struct WD33C93_hostdata));
-                if (instance == NULL)
-                        goto release;
-                instance->base = ZTWO_VADDR(address);
-                instance->irq = IRQ_AMIGA_PORTS;
-                instance->unique_id = z->slotaddr;
-
-                hdata = shost_priv(instance);
-                if (gvp11_xfer_mask)
-                        hdata->dma_xfer_mask = gvp11_xfer_mask;
-                else
-                        hdata->dma_xfer_mask = default_dma_xfer_mask;
-
-                DMA(instance)->secret2 = 1;
-                DMA(instance)->secret1 = 0;
-                DMA(instance)->secret3 = 15;
-                while (DMA(instance)->CNTR & GVP11_DMAC_BUSY)
-                        ;
-                DMA(instance)->CNTR = 0;
-
-                DMA(instance)->BANK = 0;
-
-                epc = *(unsigned short *)(ZTWO_VADDR(address) + 0x8000);
-
-                /*
-                 * Check for 14MHz SCSI clock
-                 */
-                regs.SASR = &(DMA(instance)->SASR);
-                regs.SCMD = &(DMA(instance)->SCMD);
-                hdata->no_sync = 0xff;
-                hdata->fast = 0;
-                hdata->dma_mode = CTRL_DMA;
-                wd33c93_init(instance, regs, dma_setup, dma_stop,
-                             (epc & GVP_SCSICLKMASK) ? WD33C93_FS_8_10
-                                                     : WD33C93_FS_12_15);
-
-                if (request_irq(IRQ_AMIGA_PORTS, gvp11_intr, IRQF_SHARED,
-                                "GVP11 SCSI", instance))
-                        goto unregister;
-                DMA(instance)->CNTR = GVP11_DMAC_INT_ENABLE;
-                num_gvp11++;
-                continue;
-
-unregister:
-                scsi_unregister(instance);
-release:
-                release_mem_region(address, 256);
+                wh->dma_bounce_buffer = NULL;
+                wh->dma_bounce_len = 0;
         }
-
-        return num_gvp11;
 }
 
 static int gvp11_bus_reset(struct scsi_cmnd *cmd)
 {
+        struct Scsi_Host *instance = cmd->device->host;
+
         /* FIXME perform bus-specific reset */
 
         /* FIXME 2: shouldn't we no-op this function (return
            FAILED), and fall back to host reset function,
            wd33c93_host_reset ? */
 
-        spin_lock_irq(cmd->device->host->host_lock);
+        spin_lock_irq(instance->host_lock);
         wd33c93_host_reset(cmd);
-        spin_unlock_irq(cmd->device->host->host_lock);
+        spin_unlock_irq(instance->host_lock);
 
         return SUCCESS;
 }
 
-
-#define HOSTS_C
-
-#include "gvp11.h"
-
-static struct scsi_host_template driver_template = {
-        .proc_name              = "GVP11",
+static struct scsi_host_template gvp11_scsi_template = {
+        .module                 = THIS_MODULE,
         .name                   = "GVP Series II SCSI",
-        .detect                 = gvp11_detect,
-        .release                = gvp11_release,
+        .proc_info              = wd33c93_proc_info,
+        .proc_name              = "GVP11",
         .queuecommand           = wd33c93_queuecommand,
         .eh_abort_handler       = wd33c93_abort,
         .eh_bus_reset_handler   = gvp11_bus_reset,
@@ -385,17 +203,230 @@ static struct scsi_host_template driver_template = {
         .use_clustering         = DISABLE_CLUSTERING
 };
 
+static int __devinit check_wd33c93(struct gvp11_scsiregs *regs)
+{
+#ifdef CHECK_WD33C93
+        volatile unsigned char *sasr_3393, *scmd_3393;
+        unsigned char save_sasr;
+        unsigned char q, qq;
 
-#include "scsi_module.c"
+        /*
+         * These darn GVP boards are a problem - it can be tough to tell
+         * whether or not they include a SCSI controller. This is the
+         * ultimate Yet-Another-GVP-Detection-Hack in that it actually
+         * probes for a WD33c93 chip: If we find one, it's extremely
+         * likely that this card supports SCSI, regardless of Product_
+         * Code, Board_Size, etc.
+         */
+
+        /* Get pointers to the presumed register locations and save contents */
+
+        sasr_3393 = &regs->SASR;
+        scmd_3393 = &regs->SCMD;
+        save_sasr = *sasr_3393;
+
+        /* First test the AuxStatus Reg */
+
+        q = *sasr_3393; /* read it */
+        if (q & 0x08)   /* bit 3 should always be clear */
+                return -ENODEV;
+        *sasr_3393 = WD_AUXILIARY_STATUS;       /* setup indirect address */
+        if (*sasr_3393 == WD_AUXILIARY_STATUS) {        /* shouldn't retain the write */
+                *sasr_3393 = save_sasr; /* Oops - restore this byte */
+                return -ENODEV;
+        }
+        if (*sasr_3393 != q) {  /* should still read the same */
+                *sasr_3393 = save_sasr; /* Oops - restore this byte */
+                return -ENODEV;
+        }
+        if (*scmd_3393 != q)    /* and so should the image at 0x1f */
+                return -ENODEV;
+
+        /*
+         * Ok, we probably have a wd33c93, but let's check a few other places
+         * for good measure. Make sure that this works for both 'A and 'B
+         * chip versions.
+         */
+
+        *sasr_3393 = WD_SCSI_STATUS;
+        q = *scmd_3393;
+        *sasr_3393 = WD_SCSI_STATUS;
+        *scmd_3393 = ~q;
+        *sasr_3393 = WD_SCSI_STATUS;
+        qq = *scmd_3393;
+        *sasr_3393 = WD_SCSI_STATUS;
+        *scmd_3393 = q;
+        if (qq != q)    /* should be read only */
+                return -ENODEV;
+        *sasr_3393 = 0x1e;      /* this register is unimplemented */
+        q = *scmd_3393;
+        *sasr_3393 = 0x1e;
+        *scmd_3393 = ~q;
+        *sasr_3393 = 0x1e;
+        qq = *scmd_3393;
+        *sasr_3393 = 0x1e;
+        *scmd_3393 = q;
+        if (qq != q || qq != 0xff)      /* should be read only, all 1's */
+                return -ENODEV;
+        *sasr_3393 = WD_TIMEOUT_PERIOD;
+        q = *scmd_3393;
+        *sasr_3393 = WD_TIMEOUT_PERIOD;
+        *scmd_3393 = ~q;
+        *sasr_3393 = WD_TIMEOUT_PERIOD;
+        qq = *scmd_3393;
+        *sasr_3393 = WD_TIMEOUT_PERIOD;
+        *scmd_3393 = q;
+        if (qq != (~q & 0xff))  /* should be read/write */
+                return -ENODEV;
+#endif /* CHECK_WD33C93 */
 
-int gvp11_release(struct Scsi_Host *instance)
+        return 0;
+}
+
+static int __devinit gvp11_probe(struct zorro_dev *z,
+                                 const struct zorro_device_id *ent)
 {
-#ifdef MODULE
-        DMA(instance)->CNTR = 0;
-        release_mem_region(ZTWO_PADDR(instance->base), 256);
+        struct Scsi_Host *instance;
+        unsigned long address;
+        int error;
+        unsigned int epc;
+        unsigned int default_dma_xfer_mask;
+        struct gvp11_hostdata *hdata;
+        struct gvp11_scsiregs *regs;
+        wd33c93_regs wdregs;
+
+        default_dma_xfer_mask = ent->driver_data;
+
+        /*
+         * Rumors state that some GVP ram boards use the same product
+         * code as the SCSI controllers. Therefore if the board-size
+         * is not 64KB we asume it is a ram board and bail out.
+         */
+        if (zorro_resource_len(z) != 0x10000)
+                return -ENODEV;
+
+        address = z->resource.start;
+        if (!request_mem_region(address, 256, "wd33c93"))
+                return -EBUSY;
+
+        regs = (struct gvp11_scsiregs *)(ZTWO_VADDR(address));
+
+        error = check_wd33c93(regs);
+        if (error)
+                goto fail_check_or_alloc;
+
+        instance = scsi_host_alloc(&gvp11_scsi_template,
+                                   sizeof(struct gvp11_hostdata));
+        if (!instance) {
+                error = -ENOMEM;
+                goto fail_check_or_alloc;
+        }
+
+        instance->irq = IRQ_AMIGA_PORTS;
+        instance->unique_id = z->slotaddr;
+
+        regs->secret2 = 1;
+        regs->secret1 = 0;
+        regs->secret3 = 15;
+        while (regs->CNTR & GVP11_DMAC_BUSY)
+                ;
+        regs->CNTR = 0;
+        regs->BANK = 0;
+
+        wdregs.SASR = &regs->SASR;
+        wdregs.SCMD = &regs->SCMD;
+
+        hdata = shost_priv(instance);
+        if (gvp11_xfer_mask)
+                hdata->wh.dma_xfer_mask = gvp11_xfer_mask;
+        else
+                hdata->wh.dma_xfer_mask = default_dma_xfer_mask;
+
+        hdata->wh.no_sync = 0xff;
+        hdata->wh.fast = 0;
+        hdata->wh.dma_mode = CTRL_DMA;
+        hdata->regs = regs;
+
+        /*
+         * Check for 14MHz SCSI clock
+         */
+        epc = *(unsigned short *)(ZTWO_VADDR(address) + 0x8000);
+        wd33c93_init(instance, wdregs, dma_setup, dma_stop,
+                     (epc & GVP_SCSICLKMASK) ? WD33C93_FS_8_10
+                                             : WD33C93_FS_12_15);
+
+        error = request_irq(IRQ_AMIGA_PORTS, gvp11_intr, IRQF_SHARED,
+                            "GVP11 SCSI", instance);
+        if (error)
+                goto fail_irq;
+
+        regs->CNTR = GVP11_DMAC_INT_ENABLE;
+
+        error = scsi_add_host(instance, NULL);
+        if (error)
+                goto fail_host;
+
+        zorro_set_drvdata(z, instance);
+        scsi_scan_host(instance);
+        return 0;
+
+fail_host:
         free_irq(IRQ_AMIGA_PORTS, instance);
-#endif
-        return 1;
+fail_irq:
+        scsi_host_put(instance);
+fail_check_or_alloc:
+        release_mem_region(address, 256);
+        return error;
+}
+
+static void __devexit gvp11_remove(struct zorro_dev *z)
+{
+        struct Scsi_Host *instance = zorro_get_drvdata(z);
+        struct gvp11_hostdata *hdata = shost_priv(instance);
+
+        hdata->regs->CNTR = 0;
+        scsi_remove_host(instance);
+        free_irq(IRQ_AMIGA_PORTS, instance);
+        scsi_host_put(instance);
+        release_mem_region(z->resource.start, 256);
+}
+
+        /*
+         * This should (hopefully) be the correct way to identify
+         * all the different GVP SCSI controllers (except for the
+         * SERIES I though).
+         */
+
+static struct zorro_device_id gvp11_zorro_tbl[] __devinitdata = {
+        { ZORRO_PROD_GVP_COMBO_030_R3_SCSI,     ~0x00ffffff },
+        { ZORRO_PROD_GVP_SERIES_II,             ~0x00ffffff },
+        { ZORRO_PROD_GVP_GFORCE_030_SCSI,       ~0x01ffffff },
+        { ZORRO_PROD_GVP_A530_SCSI,             ~0x01ffffff },
+        { ZORRO_PROD_GVP_COMBO_030_R4_SCSI,     ~0x01ffffff },
+        { ZORRO_PROD_GVP_A1291,                 ~0x07ffffff },
+        { ZORRO_PROD_GVP_GFORCE_040_SCSI_1,     ~0x07ffffff },
+        { 0 }
+};
+MODULE_DEVICE_TABLE(zorro, gvp11_zorro_tbl);
+
+static struct zorro_driver gvp11_driver = {
+        .name           = "gvp11",
+        .id_table       = gvp11_zorro_tbl,
+        .probe          = gvp11_probe,
+        .remove         = __devexit_p(gvp11_remove),
+};
+
+static int __init gvp11_init(void)
+{
+        return zorro_register_driver(&gvp11_driver);
+}
+module_init(gvp11_init);
+
+static void __exit gvp11_exit(void)
+{
+        zorro_unregister_driver(&gvp11_driver);
 }
+module_exit(gvp11_exit);
 
+MODULE_DESCRIPTION("GVP Series II SCSI");
 MODULE_LICENSE("GPL");
diff --git a/drivers/scsi/gvp11.h b/drivers/scsi/gvp11.h
index e2efdf9601ef..852913cde5dd 100644
--- a/drivers/scsi/gvp11.h
+++ b/drivers/scsi/gvp11.h
@@ -11,9 +11,6 @@
 
 #include <linux/types.h>
 
-int gvp11_detect(struct scsi_host_template *);
-int gvp11_release(struct Scsi_Host *);
-
 #ifndef CMD_PER_LUN
 #define CMD_PER_LUN             2
 #endif
@@ -22,15 +19,13 @@ int gvp11_release(struct Scsi_Host *);
 #define CAN_QUEUE               16
 #endif
 
-#ifndef HOSTS_C
-
 /*
  * if the transfer address ANDed with this results in a non-zero
  * result, then we can't use DMA.
  */
 #define GVP11_XFER_MASK         (0xff000001)
 
-typedef struct {
+struct gvp11_scsiregs {
                  unsigned char  pad1[64];
         volatile unsigned short CNTR;
                  unsigned char  pad2[31];
@@ -46,7 +41,7 @@ typedef struct {
         volatile unsigned short SP_DMA;
         volatile unsigned short secret2; /* store 1 here */
         volatile unsigned short secret3; /* store 15 here */
-} gvp11_scsiregs;
+};
 
 /* bits in CNTR */
 #define GVP11_DMAC_BUSY         (1<<0)
@@ -54,6 +49,4 @@ typedef struct {
 #define GVP11_DMAC_INT_ENABLE   (1<<3)
 #define GVP11_DMAC_DIR_WRITE    (1<<4)
 
-#endif /* else def HOSTS_C */
-
 #endif /* GVP11_H */
diff --git a/drivers/scsi/ipr.c b/drivers/scsi/ipr.c
index 6a6661c35b2f..82ea4a8226b0 100644
--- a/drivers/scsi/ipr.c
+++ b/drivers/scsi/ipr.c
@@ -567,7 +567,8 @@ static void ipr_trc_hook(struct ipr_cmnd *ipr_cmd,
 static void ipr_reinit_ipr_cmnd(struct ipr_cmnd *ipr_cmd)
 {
         struct ipr_ioarcb *ioarcb = &ipr_cmd->ioarcb;
-        struct ipr_ioasa *ioasa = &ipr_cmd->ioasa;
+        struct ipr_ioasa *ioasa = &ipr_cmd->s.ioasa;
+        struct ipr_ioasa64 *ioasa64 = &ipr_cmd->s.ioasa64;
         dma_addr_t dma_addr = ipr_cmd->dma_addr;
 
         memset(&ioarcb->cmd_pkt, 0, sizeof(struct ipr_cmd_pkt));
@@ -576,19 +577,19 @@ static void ipr_reinit_ipr_cmnd(struct ipr_cmnd *ipr_cmd)
         ioarcb->ioadl_len = 0;
         ioarcb->read_ioadl_len = 0;
 
-        if (ipr_cmd->ioa_cfg->sis64)
+        if (ipr_cmd->ioa_cfg->sis64) {
                 ioarcb->u.sis64_addr_data.data_ioadl_addr =
                         cpu_to_be64(dma_addr + offsetof(struct ipr_cmnd, i.ioadl64));
-        else {
+                ioasa64->u.gata.status = 0;
+        } else {
                 ioarcb->write_ioadl_addr =
                         cpu_to_be32(dma_addr + offsetof(struct ipr_cmnd, i.ioadl));
                 ioarcb->read_ioadl_addr = ioarcb->write_ioadl_addr;
+                ioasa->u.gata.status = 0;
         }
 
-        ioasa->ioasc = 0;
-        ioasa->residual_data_len = 0;
-        ioasa->u.gata.status = 0;
-
+        ioasa->hdr.ioasc = 0;
+        ioasa->hdr.residual_data_len = 0;
         ipr_cmd->scsi_cmd = NULL;
         ipr_cmd->qc = NULL;
         ipr_cmd->sense_buffer[0] = 0;
@@ -768,8 +769,8 @@ static void ipr_fail_all_ops(struct ipr_ioa_cfg *ioa_cfg)
         list_for_each_entry_safe(ipr_cmd, temp, &ioa_cfg->pending_q, queue) {
                 list_del(&ipr_cmd->queue);
 
-                ipr_cmd->ioasa.ioasc = cpu_to_be32(IPR_IOASC_IOA_WAS_RESET);
-                ipr_cmd->ioasa.ilid = cpu_to_be32(IPR_DRIVER_ILID);
+                ipr_cmd->s.ioasa.hdr.ioasc = cpu_to_be32(IPR_IOASC_IOA_WAS_RESET);
+                ipr_cmd->s.ioasa.hdr.ilid = cpu_to_be32(IPR_DRIVER_ILID);
 
                 if (ipr_cmd->scsi_cmd)
                         ipr_cmd->done = ipr_scsi_eh_done;
@@ -1040,7 +1041,7 @@ static void ipr_init_res_entry(struct ipr_resource_entry *res,
                 proto = cfgtew->u.cfgte64->proto;
                 res->res_flags = cfgtew->u.cfgte64->res_flags;
                 res->qmodel = IPR_QUEUEING_MODEL64(res);
-                res->type = cfgtew->u.cfgte64->res_type & 0x0f;
+                res->type = cfgtew->u.cfgte64->res_type;
 
                 memcpy(res->res_path, &cfgtew->u.cfgte64->res_path,
                         sizeof(res->res_path));
@@ -1319,7 +1320,7 @@ static void ipr_process_ccn(struct ipr_cmnd *ipr_cmd)
 {
         struct ipr_ioa_cfg *ioa_cfg = ipr_cmd->ioa_cfg;
         struct ipr_hostrcb *hostrcb = ipr_cmd->u.hostrcb;
-        u32 ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+        u32 ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
 
         list_del(&hostrcb->queue);
         list_add_tail(&ipr_cmd->queue, &ioa_cfg->free_q);
@@ -2354,7 +2355,7 @@ static void ipr_process_error(struct ipr_cmnd *ipr_cmd)
 {
         struct ipr_ioa_cfg *ioa_cfg = ipr_cmd->ioa_cfg;
         struct ipr_hostrcb *hostrcb = ipr_cmd->u.hostrcb;
-        u32 ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+        u32 ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
         u32 fd_ioasc;
 
         if (ioa_cfg->sis64)
@@ -4509,11 +4510,16 @@ static int ipr_device_reset(struct ipr_ioa_cfg *ioa_cfg,
         }
 
         ipr_send_blocking_cmd(ipr_cmd, ipr_timeout, IPR_DEVICE_RESET_TIMEOUT);
-        ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+        ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
         list_add_tail(&ipr_cmd->queue, &ioa_cfg->free_q);
-        if (ipr_is_gata(res) && res->sata_port && ioasc != IPR_IOASC_IOA_WAS_RESET)
-                memcpy(&res->sata_port->ioasa, &ipr_cmd->ioasa.u.gata,
-                       sizeof(struct ipr_ioasa_gata));
+        if (ipr_is_gata(res) && res->sata_port && ioasc != IPR_IOASC_IOA_WAS_RESET) {
+                if (ipr_cmd->ioa_cfg->sis64)
+                        memcpy(&res->sata_port->ioasa, &ipr_cmd->s.ioasa64.u.gata,
+                               sizeof(struct ipr_ioasa_gata));
+                else
+                        memcpy(&res->sata_port->ioasa, &ipr_cmd->s.ioasa.u.gata,
+                               sizeof(struct ipr_ioasa_gata));
+        }
 
         LEAVE;
         return (IPR_IOASC_SENSE_KEY(ioasc) ? -EIO : 0);
@@ -4768,7 +4774,7 @@ static int ipr_cancel_op(struct scsi_cmnd * scsi_cmd)
         scmd_printk(KERN_ERR, scsi_cmd, "Aborting command: %02X\n",
                     scsi_cmd->cmnd[0]);
         ipr_send_blocking_cmd(ipr_cmd, ipr_abort_timeout, IPR_CANCEL_ALL_TIMEOUT);
-        ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+        ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
 
         /*
          * If the abort task timed out and we sent a bus reset, we will get
@@ -4812,15 +4818,39 @@ static int ipr_eh_abort(struct scsi_cmnd * scsi_cmd)
 /**
  * ipr_handle_other_interrupt - Handle "other" interrupts
  * @ioa_cfg:    ioa config struct
- * @int_reg:    interrupt register
  *
  * Return value:
  *      IRQ_NONE / IRQ_HANDLED
  **/
-static irqreturn_t ipr_handle_other_interrupt(struct ipr_ioa_cfg *ioa_cfg,
-                                              volatile u32 int_reg)
+static irqreturn_t ipr_handle_other_interrupt(struct ipr_ioa_cfg *ioa_cfg)
 {
         irqreturn_t rc = IRQ_HANDLED;
+        volatile u32 int_reg, int_mask_reg;
+
+        int_mask_reg = readl(ioa_cfg->regs.sense_interrupt_mask_reg32);
+        int_reg = readl(ioa_cfg->regs.sense_interrupt_reg32) & ~int_mask_reg;
+
+        /* If an interrupt on the adapter did not occur, ignore it.
+         * Or in the case of SIS 64, check for a stage change interrupt.
+         */
+        if ((int_reg & IPR_PCII_OPER_INTERRUPTS) == 0) {
+                if (ioa_cfg->sis64) {
+                        int_mask_reg = readl(ioa_cfg->regs.sense_interrupt_mask_reg);
+                        int_reg = readl(ioa_cfg->regs.sense_interrupt_reg) & ~int_mask_reg;
+                        if (int_reg & IPR_PCII_IPL_STAGE_CHANGE) {
+
+                                /* clear stage change */
+                                writel(IPR_PCII_IPL_STAGE_CHANGE, ioa_cfg->regs.clr_interrupt_reg);
+                                int_reg = readl(ioa_cfg->regs.sense_interrupt_reg) & ~int_mask_reg;
+                                list_del(&ioa_cfg->reset_cmd->queue);
+                                del_timer(&ioa_cfg->reset_cmd->timer);
+                                ipr_reset_ioa_job(ioa_cfg->reset_cmd);
+                                return IRQ_HANDLED;
+                        }
+                }
+
+                return IRQ_NONE;
+        }
 
         if (int_reg & IPR_PCII_IOA_TRANS_TO_OPER) {
                 /* Mask the interrupt */
@@ -4881,7 +4911,7 @@ static irqreturn_t ipr_isr(int irq, void *devp)
 {
         struct ipr_ioa_cfg *ioa_cfg = (struct ipr_ioa_cfg *)devp;
         unsigned long lock_flags = 0;
-        volatile u32 int_reg, int_mask_reg;
+        volatile u32 int_reg;
         u32 ioasc;
         u16 cmd_index;
         int num_hrrq = 0;
@@ -4896,33 +4926,6 @@ static irqreturn_t ipr_isr(int irq, void *devp)
                 return IRQ_NONE;
         }
 
-        int_mask_reg = readl(ioa_cfg->regs.sense_interrupt_mask_reg32);
-        int_reg = readl(ioa_cfg->regs.sense_interrupt_reg32) & ~int_mask_reg;
-
-        /* If an interrupt on the adapter did not occur, ignore it.
-         * Or in the case of SIS 64, check for a stage change interrupt.
-         */
-        if (unlikely((int_reg & IPR_PCII_OPER_INTERRUPTS) == 0)) {
-                if (ioa_cfg->sis64) {
-                        int_mask_reg = readl(ioa_cfg->regs.sense_interrupt_mask_reg);
-                        int_reg = readl(ioa_cfg->regs.sense_interrupt_reg) & ~int_mask_reg;
-                        if (int_reg & IPR_PCII_IPL_STAGE_CHANGE) {
-
-                                /* clear stage change */
-                                writel(IPR_PCII_IPL_STAGE_CHANGE, ioa_cfg->regs.clr_interrupt_reg);
-                                int_reg = readl(ioa_cfg->regs.sense_interrupt_reg) & ~int_mask_reg;
-                                list_del(&ioa_cfg->reset_cmd->queue);
-                                del_timer(&ioa_cfg->reset_cmd->timer);
-                                ipr_reset_ioa_job(ioa_cfg->reset_cmd);
-                                spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
-                                return IRQ_HANDLED;
-                        }
-                }
-
-                spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
-                return IRQ_NONE;
-        }
-
         while (1) {
                 ipr_cmd = NULL;
 
@@ -4940,7 +4943,7 @@ static irqreturn_t ipr_isr(int irq, void *devp)
 
                         ipr_cmd = ioa_cfg->ipr_cmnd_list[cmd_index];
 
-                        ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+                        ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
 
                         ipr_trc_hook(ipr_cmd, IPR_TRACE_FINISH, ioasc);
 
@@ -4962,7 +4965,7 @@ static irqreturn_t ipr_isr(int irq, void *devp)
                         /* Clear the PCI interrupt */
                         do {
                                 writel(IPR_PCII_HRRQ_UPDATED, ioa_cfg->regs.clr_interrupt_reg32);
-                                int_reg = readl(ioa_cfg->regs.sense_interrupt_reg32) & ~int_mask_reg;
+                                int_reg = readl(ioa_cfg->regs.sense_interrupt_reg32);
                         } while (int_reg & IPR_PCII_HRRQ_UPDATED &&
                                         num_hrrq++ < IPR_MAX_HRRQ_RETRIES);
 
@@ -4977,7 +4980,7 @@ static irqreturn_t ipr_isr(int irq, void *devp)
         }
 
         if (unlikely(rc == IRQ_NONE))
-                rc = ipr_handle_other_interrupt(ioa_cfg, int_reg);
+                rc = ipr_handle_other_interrupt(ioa_cfg);
 
         spin_unlock_irqrestore(ioa_cfg->host->host_lock, lock_flags);
         return rc;
@@ -5014,6 +5017,10 @@ static int ipr_build_ioadl64(struct ipr_ioa_cfg *ioa_cfg,
 
         ipr_cmd->dma_use_sg = nseg;
 
+        ioarcb->data_transfer_length = cpu_to_be32(length);
+        ioarcb->ioadl_len =
+                cpu_to_be32(sizeof(struct ipr_ioadl64_desc) * ipr_cmd->dma_use_sg);
+
         if (scsi_cmd->sc_data_direction == DMA_TO_DEVICE) {
                 ioadl_flags = IPR_IOADL_FLAGS_WRITE;
                 ioarcb->cmd_pkt.flags_hi |= IPR_FLAGS_HI_WRITE_NOT_READ;
@@ -5135,7 +5142,7 @@ static void ipr_erp_done(struct ipr_cmnd *ipr_cmd)
         struct scsi_cmnd *scsi_cmd = ipr_cmd->scsi_cmd;
         struct ipr_resource_entry *res = scsi_cmd->device->hostdata;
         struct ipr_ioa_cfg *ioa_cfg = ipr_cmd->ioa_cfg;
-        u32 ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+        u32 ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
 
         if (IPR_IOASC_SENSE_KEY(ioasc) > 0) {
                 scsi_cmd->result |= (DID_ERROR << 16);
@@ -5166,7 +5173,7 @@ static void ipr_erp_done(struct ipr_cmnd *ipr_cmd)
 static void ipr_reinit_ipr_cmnd_for_erp(struct ipr_cmnd *ipr_cmd)
 {
         struct ipr_ioarcb *ioarcb = &ipr_cmd->ioarcb;
-        struct ipr_ioasa *ioasa = &ipr_cmd->ioasa;
+        struct ipr_ioasa *ioasa = &ipr_cmd->s.ioasa;
         dma_addr_t dma_addr = ipr_cmd->dma_addr;
 
         memset(&ioarcb->cmd_pkt, 0, sizeof(struct ipr_cmd_pkt));
@@ -5174,8 +5181,8 @@ static void ipr_reinit_ipr_cmnd_for_erp(struct ipr_cmnd *ipr_cmd)
         ioarcb->read_data_transfer_length = 0;
         ioarcb->ioadl_len = 0;
         ioarcb->read_ioadl_len = 0;
-        ioasa->ioasc = 0;
-        ioasa->residual_data_len = 0;
+        ioasa->hdr.ioasc = 0;
+        ioasa->hdr.residual_data_len = 0;
 
         if (ipr_cmd->ioa_cfg->sis64)
                 ioarcb->u.sis64_addr_data.data_ioadl_addr =
@@ -5200,7 +5207,7 @@ static void ipr_reinit_ipr_cmnd_for_erp(struct ipr_cmnd *ipr_cmd)
 static void ipr_erp_request_sense(struct ipr_cmnd *ipr_cmd)
 {
         struct ipr_cmd_pkt *cmd_pkt = &ipr_cmd->ioarcb.cmd_pkt;
-        u32 ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+        u32 ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
 
         if (IPR_IOASC_SENSE_KEY(ioasc) > 0) {
                 ipr_erp_done(ipr_cmd);
@@ -5277,12 +5284,12 @@ static void ipr_dump_ioasa(struct ipr_ioa_cfg *ioa_cfg,
         int i;
         u16 data_len;
         u32 ioasc, fd_ioasc;
-        struct ipr_ioasa *ioasa = &ipr_cmd->ioasa;
+        struct ipr_ioasa *ioasa = &ipr_cmd->s.ioasa;
         __be32 *ioasa_data = (__be32 *)ioasa;
         int error_index;
 
-        ioasc = be32_to_cpu(ioasa->ioasc) & IPR_IOASC_IOASC_MASK;
-        fd_ioasc = be32_to_cpu(ioasa->fd_ioasc) & IPR_IOASC_IOASC_MASK;
+        ioasc = be32_to_cpu(ioasa->hdr.ioasc) & IPR_IOASC_IOASC_MASK;
+        fd_ioasc = be32_to_cpu(ioasa->hdr.fd_ioasc) & IPR_IOASC_IOASC_MASK;
 
         if (0 == ioasc)
                 return;
@@ -5297,7 +5304,7 @@ static void ipr_dump_ioasa(struct ipr_ioa_cfg *ioa_cfg,
 
         if (ioa_cfg->log_level < IPR_MAX_LOG_LEVEL) {
                 /* Don't log an error if the IOA already logged one */
-                if (ioasa->ilid != 0)
+                if (ioasa->hdr.ilid != 0)
                         return;
 
                 if (!ipr_is_gscsi(res))
@@ -5309,10 +5316,11 @@ static void ipr_dump_ioasa(struct ipr_ioa_cfg *ioa_cfg,
 
         ipr_res_err(ioa_cfg, res, "%s\n", ipr_error_table[error_index].error);
 
-        if (sizeof(struct ipr_ioasa) < be16_to_cpu(ioasa->ret_stat_len))
+        data_len = be16_to_cpu(ioasa->hdr.ret_stat_len);
+        if (ioa_cfg->sis64 && sizeof(struct ipr_ioasa64) < data_len)
+                data_len = sizeof(struct ipr_ioasa64);
+        else if (!ioa_cfg->sis64 && sizeof(struct ipr_ioasa) < data_len)
                 data_len = sizeof(struct ipr_ioasa);
-        else
-                data_len = be16_to_cpu(ioasa->ret_stat_len);
 
         ipr_err("IOASA Dump:\n");
 
@@ -5338,8 +5346,8 @@ static void ipr_gen_sense(struct ipr_cmnd *ipr_cmd)
         u32 failing_lba;
         u8 *sense_buf = ipr_cmd->scsi_cmd->sense_buffer;
         struct ipr_resource_entry *res = ipr_cmd->scsi_cmd->device->hostdata;
-        struct ipr_ioasa *ioasa = &ipr_cmd->ioasa;
-        u32 ioasc = be32_to_cpu(ioasa->ioasc);
+        struct ipr_ioasa *ioasa = &ipr_cmd->s.ioasa;
+        u32 ioasc = be32_to_cpu(ioasa->hdr.ioasc);
 
         memset(sense_buf, 0, SCSI_SENSE_BUFFERSIZE);
 
@@ -5382,7 +5390,7 @@ static void ipr_gen_sense(struct ipr_cmnd *ipr_cmd)
 
                 /* Illegal request */
                 if ((IPR_IOASC_SENSE_KEY(ioasc) == 0x05) &&
-                    (be32_to_cpu(ioasa->ioasc_specific) & IPR_FIELD_POINTER_VALID)) {
+                    (be32_to_cpu(ioasa->hdr.ioasc_specific) & IPR_FIELD_POINTER_VALID)) {
                         sense_buf[7] = 10;      /* additional length */
 
                         /* IOARCB was in error */
@@ -5393,10 +5401,10 @@ static void ipr_gen_sense(struct ipr_cmnd *ipr_cmd)
 
                         sense_buf[16] =
                             ((IPR_FIELD_POINTER_MASK &
-                              be32_to_cpu(ioasa->ioasc_specific)) >> 8) & 0xff;
+                              be32_to_cpu(ioasa->hdr.ioasc_specific)) >> 8) & 0xff;
                         sense_buf[17] =
                             (IPR_FIELD_POINTER_MASK &
-                             be32_to_cpu(ioasa->ioasc_specific)) & 0xff;
+                             be32_to_cpu(ioasa->hdr.ioasc_specific)) & 0xff;
                 } else {
                         if (ioasc == IPR_IOASC_MED_DO_NOT_REALLOC) {
                                 if (ipr_is_vset_device(res))
@@ -5428,14 +5436,20 @@ static void ipr_gen_sense(struct ipr_cmnd *ipr_cmd)
  **/
 static int ipr_get_autosense(struct ipr_cmnd *ipr_cmd)
 {
-        struct ipr_ioasa *ioasa = &ipr_cmd->ioasa;
+        struct ipr_ioasa *ioasa = &ipr_cmd->s.ioasa;
+        struct ipr_ioasa64 *ioasa64 = &ipr_cmd->s.ioasa64;
 
-        if ((be32_to_cpu(ioasa->ioasc_specific) & IPR_AUTOSENSE_VALID) == 0)
+        if ((be32_to_cpu(ioasa->hdr.ioasc_specific) & IPR_AUTOSENSE_VALID) == 0)
                 return 0;
 
-        memcpy(ipr_cmd->scsi_cmd->sense_buffer, ioasa->auto_sense.data,
-               min_t(u16, be16_to_cpu(ioasa->auto_sense.auto_sense_len),
-                   SCSI_SENSE_BUFFERSIZE));
+        if (ipr_cmd->ioa_cfg->sis64)
+                memcpy(ipr_cmd->scsi_cmd->sense_buffer, ioasa64->auto_sense.data,
+                       min_t(u16, be16_to_cpu(ioasa64->auto_sense.auto_sense_len),
+                           SCSI_SENSE_BUFFERSIZE));
+        else
+                memcpy(ipr_cmd->scsi_cmd->sense_buffer, ioasa->auto_sense.data,
+                       min_t(u16, be16_to_cpu(ioasa->auto_sense.auto_sense_len),
+                           SCSI_SENSE_BUFFERSIZE));
         return 1;
 }
 
@@ -5455,7 +5469,7 @@ static void ipr_erp_start(struct ipr_ioa_cfg *ioa_cfg,
 {
         struct scsi_cmnd *scsi_cmd = ipr_cmd->scsi_cmd;
         struct ipr_resource_entry *res = scsi_cmd->device->hostdata;
-        u32 ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+        u32 ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
         u32 masked_ioasc = ioasc & IPR_IOASC_IOASC_MASK;
 
         if (!res) {
@@ -5547,9 +5561,9 @@ static void ipr_scsi_done(struct ipr_cmnd *ipr_cmd)
 {
         struct ipr_ioa_cfg *ioa_cfg = ipr_cmd->ioa_cfg;
         struct scsi_cmnd *scsi_cmd = ipr_cmd->scsi_cmd;
-        u32 ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+        u32 ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
 
-        scsi_set_resid(scsi_cmd, be32_to_cpu(ipr_cmd->ioasa.residual_data_len));
+        scsi_set_resid(scsi_cmd, be32_to_cpu(ipr_cmd->s.ioasa.hdr.residual_data_len));
 
         if (likely(IPR_IOASC_SENSE_KEY(ioasc) == 0)) {
                 scsi_dma_unmap(ipr_cmd->scsi_cmd);
@@ -5839,19 +5853,23 @@ static void ipr_sata_done(struct ipr_cmnd *ipr_cmd)
         struct ata_queued_cmd *qc = ipr_cmd->qc;
         struct ipr_sata_port *sata_port = qc->ap->private_data;
         struct ipr_resource_entry *res = sata_port->res;
-        u32 ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+        u32 ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
 
-        memcpy(&sata_port->ioasa, &ipr_cmd->ioasa.u.gata,
-               sizeof(struct ipr_ioasa_gata));
+        if (ipr_cmd->ioa_cfg->sis64)
+                memcpy(&sata_port->ioasa, &ipr_cmd->s.ioasa64.u.gata,
+                       sizeof(struct ipr_ioasa_gata));
+        else
+                memcpy(&sata_port->ioasa, &ipr_cmd->s.ioasa.u.gata,
+                       sizeof(struct ipr_ioasa_gata));
         ipr_dump_ioasa(ioa_cfg, ipr_cmd, res);
 
-        if (be32_to_cpu(ipr_cmd->ioasa.ioasc_specific) & IPR_ATA_DEVICE_WAS_RESET)
+        if (be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc_specific) & IPR_ATA_DEVICE_WAS_RESET)
                 scsi_report_device_reset(ioa_cfg->host, res->bus, res->target);
 
         if (IPR_IOASC_SENSE_KEY(ioasc) > RECOVERED_ERROR)
-                qc->err_mask |= __ac_err_mask(ipr_cmd->ioasa.u.gata.status);
+                qc->err_mask |= __ac_err_mask(sata_port->ioasa.status);
         else
-                qc->err_mask |= ac_err_mask(ipr_cmd->ioasa.u.gata.status);
+                qc->err_mask |= ac_err_mask(sata_port->ioasa.status);
         list_add_tail(&ipr_cmd->queue, &ioa_cfg->free_q);
         ata_qc_complete(qc);
 }
@@ -6520,7 +6538,7 @@ static void ipr_build_mode_sense(struct ipr_cmnd *ipr_cmd,
 static int ipr_reset_cmd_failed(struct ipr_cmnd *ipr_cmd)
 {
         struct ipr_ioa_cfg *ioa_cfg = ipr_cmd->ioa_cfg;
-        u32 ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+        u32 ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
 
         dev_err(&ioa_cfg->pdev->dev,
                 "0x%02X failed with IOASC: 0x%08X\n",
@@ -6544,7 +6562,7 @@ static int ipr_reset_cmd_failed(struct ipr_cmnd *ipr_cmd)
 static int ipr_reset_mode_sense_failed(struct ipr_cmnd *ipr_cmd)
 {
         struct ipr_ioa_cfg *ioa_cfg = ipr_cmd->ioa_cfg;
-        u32 ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+        u32 ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
 
         if (ioasc == IPR_IOASC_IR_INVALID_REQ_TYPE_OR_PKT) {
                 ipr_cmd->job_step = ipr_set_supported_devs;
@@ -6634,7 +6652,7 @@ static int ipr_ioafp_mode_select_page24(struct ipr_cmnd *ipr_cmd)
  **/
 static int ipr_reset_mode_sense_page24_failed(struct ipr_cmnd *ipr_cmd)
 {
-        u32 ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+        u32 ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
 
         if (ioasc == IPR_IOASC_IR_INVALID_REQ_TYPE_OR_PKT) {
                 ipr_cmd->job_step = ipr_ioafp_mode_sense_page28;
@@ -6706,7 +6724,7 @@ static int ipr_init_res_table(struct ipr_cmnd *ipr_cmd)
                 list_move_tail(&res->queue, &old_res);
 
         if (ioa_cfg->sis64)
-                entries = ioa_cfg->u.cfg_table64->hdr64.num_entries;
+                entries = be16_to_cpu(ioa_cfg->u.cfg_table64->hdr64.num_entries);
         else
                 entries = ioa_cfg->u.cfg_table->hdr.num_entries;
 
@@ -6792,6 +6810,7 @@ static int ipr_ioafp_query_ioa_cfg(struct ipr_cmnd *ipr_cmd)
         ioarcb->res_handle = cpu_to_be32(IPR_IOA_RES_HANDLE);
 
         ioarcb->cmd_pkt.cdb[0] = IPR_QUERY_IOA_CONFIG;
+        ioarcb->cmd_pkt.cdb[6] = (ioa_cfg->cfg_table_size >> 16) & 0xff;
         ioarcb->cmd_pkt.cdb[7] = (ioa_cfg->cfg_table_size >> 8) & 0xff;
         ioarcb->cmd_pkt.cdb[8] = ioa_cfg->cfg_table_size & 0xff;
 
@@ -7122,7 +7141,9 @@ static int ipr_reset_next_stage(struct ipr_cmnd *ipr_cmd)
         ipr_dbg("IPL stage = 0x%lx, IPL stage time = %ld\n", stage, stage_time);
 
         /* sanity check the stage_time value */
-        if (stage_time < IPR_IPL_INIT_MIN_STAGE_TIME)
+        if (stage_time == 0)
+                stage_time = IPR_IPL_INIT_DEFAULT_STAGE_TIME;
+        else if (stage_time < IPR_IPL_INIT_MIN_STAGE_TIME)
                 stage_time = IPR_IPL_INIT_MIN_STAGE_TIME;
         else if (stage_time > IPR_LONG_OPERATIONAL_TIMEOUT)
                 stage_time = IPR_LONG_OPERATIONAL_TIMEOUT;
@@ -7165,13 +7186,14 @@ static int ipr_reset_enable_ioa(struct ipr_cmnd *ipr_cmd)
 {
         struct ipr_ioa_cfg *ioa_cfg = ipr_cmd->ioa_cfg;
         volatile u32 int_reg;
+        volatile u64 maskval;
 
         ENTER;
         ipr_cmd->job_step = ipr_ioafp_identify_hrrq;
         ipr_init_ioa_mem(ioa_cfg);
 
         ioa_cfg->allow_interrupts = 1;
-        int_reg = readl(ioa_cfg->regs.sense_interrupt_reg);
+        int_reg = readl(ioa_cfg->regs.sense_interrupt_reg32);
 
         if (int_reg & IPR_PCII_IOA_TRANS_TO_OPER) {
                 writel((IPR_PCII_ERROR_INTERRUPTS | IPR_PCII_HRRQ_UPDATED),
@@ -7183,9 +7205,12 @@ static int ipr_reset_enable_ioa(struct ipr_cmnd *ipr_cmd)
         /* Enable destructive diagnostics on IOA */
         writel(ioa_cfg->doorbell, ioa_cfg->regs.set_uproc_interrupt_reg32);
 
-        writel(IPR_PCII_OPER_INTERRUPTS, ioa_cfg->regs.clr_interrupt_mask_reg32);
-        if (ioa_cfg->sis64)
-                writel(IPR_PCII_IPL_STAGE_CHANGE, ioa_cfg->regs.clr_interrupt_mask_reg);
+        if (ioa_cfg->sis64) {
+                maskval = IPR_PCII_IPL_STAGE_CHANGE;
+                maskval = (maskval << 32) | IPR_PCII_OPER_INTERRUPTS;
+                writeq(maskval, ioa_cfg->regs.clr_interrupt_mask_reg);
+        } else
+                writel(IPR_PCII_OPER_INTERRUPTS, ioa_cfg->regs.clr_interrupt_mask_reg32);
 
         int_reg = readl(ioa_cfg->regs.sense_interrupt_mask_reg);
 
@@ -7332,12 +7357,12 @@ static int ipr_reset_restore_cfg_space(struct ipr_cmnd *ipr_cmd)
         rc = pci_restore_state(ioa_cfg->pdev);
 
         if (rc != PCIBIOS_SUCCESSFUL) {
-                ipr_cmd->ioasa.ioasc = cpu_to_be32(IPR_IOASC_PCI_ACCESS_ERROR);
+                ipr_cmd->s.ioasa.hdr.ioasc = cpu_to_be32(IPR_IOASC_PCI_ACCESS_ERROR);
                 return IPR_RC_JOB_CONTINUE;
         }
 
         if (ipr_set_pcix_cmd_reg(ioa_cfg)) {
-                ipr_cmd->ioasa.ioasc = cpu_to_be32(IPR_IOASC_PCI_ACCESS_ERROR);
+                ipr_cmd->s.ioasa.hdr.ioasc = cpu_to_be32(IPR_IOASC_PCI_ACCESS_ERROR);
                 return IPR_RC_JOB_CONTINUE;
         }
 
@@ -7364,7 +7389,7 @@ static int ipr_reset_restore_cfg_space(struct ipr_cmnd *ipr_cmd)
                 }
         }
 
-        ENTER;
+        LEAVE;
         return IPR_RC_JOB_CONTINUE;
 }
 
@@ -7406,7 +7431,7 @@ static int ipr_reset_start_bist(struct ipr_cmnd *ipr_cmd)
 
         if (rc != PCIBIOS_SUCCESSFUL) {
                 pci_unblock_user_cfg_access(ipr_cmd->ioa_cfg->pdev);
-                ipr_cmd->ioasa.ioasc = cpu_to_be32(IPR_IOASC_PCI_ACCESS_ERROR);
+                ipr_cmd->s.ioasa.hdr.ioasc = cpu_to_be32(IPR_IOASC_PCI_ACCESS_ERROR);
                 rc = IPR_RC_JOB_CONTINUE;
         } else {
                 ipr_cmd->job_step = ipr_reset_bist_done;
@@ -7665,7 +7690,7 @@ static void ipr_reset_ioa_job(struct ipr_cmnd *ipr_cmd)
         struct ipr_ioa_cfg *ioa_cfg = ipr_cmd->ioa_cfg;
 
         do {
-                ioasc = be32_to_cpu(ipr_cmd->ioasa.ioasc);
+                ioasc = be32_to_cpu(ipr_cmd->s.ioasa.hdr.ioasc);
 
                 if (ioa_cfg->reset_cmd != ipr_cmd) {
                         /*
@@ -8048,13 +8073,13 @@ static int __devinit ipr_alloc_cmd_blks(struct ipr_ioa_cfg *ioa_cfg)
                         ioarcb->u.sis64_addr_data.data_ioadl_addr =
                                 cpu_to_be64(dma_addr + offsetof(struct ipr_cmnd, i.ioadl64));
                         ioarcb->u.sis64_addr_data.ioasa_host_pci_addr =
-                                cpu_to_be64(dma_addr + offsetof(struct ipr_cmnd, ioasa));
+                                cpu_to_be64(dma_addr + offsetof(struct ipr_cmnd, s.ioasa64));
                 } else {
                         ioarcb->write_ioadl_addr =
                                 cpu_to_be32(dma_addr + offsetof(struct ipr_cmnd, i.ioadl));
                         ioarcb->read_ioadl_addr = ioarcb->write_ioadl_addr;
                         ioarcb->ioasa_host_pci_addr =
-                                cpu_to_be32(dma_addr + offsetof(struct ipr_cmnd, ioasa));
+                                cpu_to_be32(dma_addr + offsetof(struct ipr_cmnd, s.ioasa));
                 }
                 ioarcb->ioasa_len = cpu_to_be16(sizeof(struct ipr_ioasa));
                 ipr_cmd->cmd_index = i;
diff --git a/drivers/scsi/ipr.h b/drivers/scsi/ipr.h
index 4c267b5e0b96..9ecd2259eb39 100644
--- a/drivers/scsi/ipr.h
+++ b/drivers/scsi/ipr.h
@@ -244,6 +244,7 @@
 #define IPR_RUNTIME_RESET                               0x40000000
 
 #define IPR_IPL_INIT_MIN_STAGE_TIME                     5
+#define IPR_IPL_INIT_DEFAULT_STAGE_TIME                 15
 #define IPR_IPL_INIT_STAGE_UNKNOWN                      0x0
 #define IPR_IPL_INIT_STAGE_TRANSOP                      0xB0000000
 #define IPR_IPL_INIT_STAGE_MASK                         0xff000000
@@ -613,7 +614,7 @@ struct ipr_auto_sense {
         __be32 data[SCSI_SENSE_BUFFERSIZE/sizeof(__be32)];
 };
 
-struct ipr_ioasa {
+struct ipr_ioasa_hdr {
         __be32 ioasc;
 #define IPR_IOASC_SENSE_KEY(ioasc) ((ioasc) >> 24)
 #define IPR_IOASC_SENSE_CODE(ioasc) (((ioasc) & 0x00ff0000) >> 16)
@@ -645,6 +646,25 @@ struct ipr_ioasa {
 #define IPR_FIELD_POINTER_VALID         (0x80000000 >> 8)
 #define IPR_FIELD_POINTER_MASK          0x0000ffff
 
+}__attribute__((packed, aligned (4)));
+
+struct ipr_ioasa {
+        struct ipr_ioasa_hdr hdr;
+
+        union {
+                struct ipr_ioasa_vset vset;
+                struct ipr_ioasa_af_dasd dasd;
+                struct ipr_ioasa_gpdd gpdd;
+                struct ipr_ioasa_gata gata;
+        } u;
+
+        struct ipr_auto_sense auto_sense;
+}__attribute__((packed, aligned (4)));
+
+struct ipr_ioasa64 {
+        struct ipr_ioasa_hdr hdr;
+        u8 fd_res_path[8];
+
         union {
                 struct ipr_ioasa_vset vset;
                 struct ipr_ioasa_af_dasd dasd;
@@ -804,7 +824,7 @@ struct ipr_hostrcb_array_data_entry_enhanced {
 }__attribute__((packed, aligned (4)));
 
 struct ipr_hostrcb_type_ff_error {
-        __be32 ioa_data[502];
+        __be32 ioa_data[758];
 }__attribute__((packed, aligned (4)));
 
 struct ipr_hostrcb_type_01_error {
@@ -1181,7 +1201,7 @@ struct ipr_resource_entry {
         u8 flags;
         __be16 res_flags;
 
-        __be32 type;
+        u8 type;
 
         u8 qmodel;
         struct ipr_std_inq_data std_inq_data;
@@ -1464,7 +1484,10 @@ struct ipr_cmnd {
                 struct ipr_ioadl64_desc ioadl64[IPR_NUM_IOADL_ENTRIES];
                 struct ipr_ata64_ioadl ata_ioadl;
         } i;
-        struct ipr_ioasa ioasa;
+        union {
+                struct ipr_ioasa ioasa;
+                struct ipr_ioasa64 ioasa64;
+        } s;
         struct list_head queue;
         struct scsi_cmnd *scsi_cmd;
         struct ata_queued_cmd *qc;
diff --git a/drivers/scsi/iscsi_tcp.c b/drivers/scsi/iscsi_tcp.c
index bf55d3057413..fec47de72535 100644
--- a/drivers/scsi/iscsi_tcp.c
+++ b/drivers/scsi/iscsi_tcp.c
@@ -601,10 +601,8 @@ static void iscsi_sw_tcp_conn_stop(struct iscsi_cls_conn *cls_conn, int flag)
         set_bit(ISCSI_SUSPEND_BIT, &conn->suspend_rx);
         write_unlock_bh(&tcp_sw_conn->sock->sk->sk_callback_lock);
 
-        if (sk_sleep(sock->sk)) {
-                sock->sk->sk_err = EIO;
-                wake_up_interruptible(sk_sleep(sock->sk));
-        }
+        sock->sk->sk_err = EIO;
+        wake_up_interruptible(sk_sleep(sock->sk));
 
         iscsi_conn_stop(cls_conn, flag);
         iscsi_sw_tcp_release_conn(conn);
diff --git a/drivers/scsi/mac53c94.c b/drivers/scsi/mac53c94.c
index 18735b39b3d3..3ddb4dc62d5d 100644
--- a/drivers/scsi/mac53c94.c
+++ b/drivers/scsi/mac53c94.c
@@ -542,8 +542,11 @@ MODULE_DEVICE_TABLE (of, mac53c94_match);
 
 static struct macio_driver mac53c94_driver = 
 {
-        .name           = "mac53c94",
-        .match_table    = mac53c94_match,
+        .driver = {
+                .name           = "mac53c94",
+                .owner          = THIS_MODULE,
+                .of_match_table = mac53c94_match,
+        },
         .probe          = mac53c94_probe,
         .remove         = mac53c94_remove,
 };
diff --git a/drivers/scsi/mesh.c b/drivers/scsi/mesh.c
index a1c97e88068a..1f784fde2510 100644
--- a/drivers/scsi/mesh.c
+++ b/drivers/scsi/mesh.c
@@ -2036,8 +2036,11 @@ MODULE_DEVICE_TABLE (of, mesh_match);
 
 static struct macio_driver mesh_driver = 
 {
-        .name           = "mesh",
-        .match_table    = mesh_match,
+        .driver = {
+                .name           = "mesh",
+                .owner          = THIS_MODULE,
+                .of_match_table = mesh_match,
+        },
         .probe          = mesh_probe,
         .remove         = mesh_remove,
         .shutdown       = mesh_shutdown,
diff --git a/drivers/scsi/mvme147.c b/drivers/scsi/mvme147.c
index 716d1785cda7..c29d0dbb9660 100644
--- a/drivers/scsi/mvme147.c
+++ b/drivers/scsi/mvme147.c
@@ -16,12 +16,12 @@
 #include <linux/stat.h>
 
 
-static struct Scsi_Host *mvme147_host = NULL;
-
-static irqreturn_t mvme147_intr(int irq, void *dummy)
+static irqreturn_t mvme147_intr(int irq, void *data)
 {
+        struct Scsi_Host *instance = data;
+
         if (irq == MVME147_IRQ_SCSI_PORT)
-                wd33c93_intr(mvme147_host);
+                wd33c93_intr(instance);
         else
                 m147_pcc->dma_intr = 0x89;      /* Ack and enable ints */
         return IRQ_HANDLED;
@@ -29,7 +29,8 @@ static irqreturn_t mvme147_intr(int irq, void *dummy)
 
 static int dma_setup(struct scsi_cmnd *cmd, int dir_in)
 {
-        struct WD33C93_hostdata *hdata = shost_priv(mvme147_host);
+        struct Scsi_Host *instance = cmd->device->host;
+        struct WD33C93_hostdata *hdata = shost_priv(instance);
         unsigned char flags = 0x01;
         unsigned long addr = virt_to_bus(cmd->SCp.ptr);
 
@@ -66,6 +67,7 @@ static void dma_stop(struct Scsi_Host *instance, struct scsi_cmnd *SCpnt,
 int mvme147_detect(struct scsi_host_template *tpnt)
 {
         static unsigned char called = 0;
+        struct Scsi_Host *instance;
         wd33c93_regs regs;
         struct WD33C93_hostdata *hdata;
 
@@ -76,25 +78,25 @@ int mvme147_detect(struct scsi_host_template *tpnt)
         tpnt->proc_name = "MVME147";
         tpnt->proc_info = &wd33c93_proc_info;
 
-        mvme147_host = scsi_register(tpnt, sizeof(struct WD33C93_hostdata));
-        if (!mvme147_host)
+        instance = scsi_register(tpnt, sizeof(struct WD33C93_hostdata));
+        if (!instance)
                 goto err_out;
 
-        mvme147_host->base = 0xfffe4000;
-        mvme147_host->irq = MVME147_IRQ_SCSI_PORT;
+        instance->base = 0xfffe4000;
+        instance->irq = MVME147_IRQ_SCSI_PORT;
         regs.SASR = (volatile unsigned char *)0xfffe4000;
         regs.SCMD = (volatile unsigned char *)0xfffe4001;
-        hdata = shost_priv(mvme147_host);
+        hdata = shost_priv(instance);
         hdata->no_sync = 0xff;
         hdata->fast = 0;
         hdata->dma_mode = CTRL_DMA;
-        wd33c93_init(mvme147_host, regs, dma_setup, dma_stop, WD33C93_FS_8_10);
+        wd33c93_init(instance, regs, dma_setup, dma_stop, WD33C93_FS_8_10);
 
         if (request_irq(MVME147_IRQ_SCSI_PORT, mvme147_intr, 0,
-                        "MVME147 SCSI PORT", mvme147_intr))
+                        "MVME147 SCSI PORT", instance))
                 goto err_unregister;
         if (request_irq(MVME147_IRQ_SCSI_DMA, mvme147_intr, 0,
-                        "MVME147 SCSI DMA", mvme147_intr))
+                        "MVME147 SCSI DMA", instance))
                 goto err_free_irq;
 #if 0   /* Disabled; causes problems booting */
         m147_pcc->scsi_interrupt = 0x10;        /* Assert SCSI bus reset */
@@ -113,7 +115,7 @@ int mvme147_detect(struct scsi_host_template *tpnt)
 err_free_irq:
         free_irq(MVME147_IRQ_SCSI_PORT, mvme147_intr);
 err_unregister:
-        scsi_unregister(mvme147_host);
+        scsi_unregister(instance);
 err_out:
         return 0;
 }
@@ -132,9 +134,6 @@ static int mvme147_bus_reset(struct scsi_cmnd *cmd)
         return SUCCESS;
 }
 
-#define HOSTS_C
-
-#include "mvme147.h"
 
 static struct scsi_host_template driver_template = {
         .proc_name              = "MVME147",
diff --git a/drivers/scsi/osst.c b/drivers/scsi/osst.c
index 8dbf1c3afb7b..d64b7178fa08 100644
--- a/drivers/scsi/osst.c
+++ b/drivers/scsi/osst.c
@@ -3587,7 +3587,7 @@ if (SRpnt) printk(KERN_ERR "%s:A: Not supposed to have SRpnt at line %d\n", name
                 if (i == (-ENOSPC)) {
                         transfer = STp->buffer->writing;        /* FIXME -- check this logic */
                         if (transfer <= do_count) {
-                                filp->f_pos += do_count - transfer;
+                                *ppos += do_count - transfer;
                                 count -= do_count - transfer;
                                 if (STps->drv_block >= 0) {
                                         STps->drv_block += (do_count - transfer) / STp->block_size;
@@ -3625,7 +3625,7 @@ if (SRpnt) printk(KERN_ERR "%s:A: Not supposed to have SRpnt at line %d\n", name
                         goto out;
                 }
 
-                filp->f_pos += do_count;
+                *ppos += do_count;
                 b_point += do_count;
                 count -= do_count;
                 if (STps->drv_block >= 0) {
@@ -3647,7 +3647,7 @@ if (SRpnt) printk(KERN_ERR "%s:A: Not supposed to have SRpnt at line %d\n", name
                 if (STps->drv_block >= 0) {
                         STps->drv_block += blks;
                 }
-                filp->f_pos += count;
+                *ppos += count;
                 count = 0;
         }
 
@@ -3823,7 +3823,7 @@ static ssize_t osst_read(struct file * filp, char __user * buf, size_t count, lo
                         }
                         STp->logical_blk_num += transfer / STp->block_size;
                         STps->drv_block      += transfer / STp->block_size;
-                        filp->f_pos          += transfer;
+                        *ppos          += transfer;
                         buf                  += transfer;
                         total                += transfer;
                 }
@@ -5626,6 +5626,7 @@ static const struct file_operations osst_fops = {
         .open =         os_scsi_tape_open,
         .flush =        os_scsi_tape_flush,
         .release =      os_scsi_tape_close,
+        .llseek =       noop_llseek,
 };
 
 static int osst_supports(struct scsi_device * SDp)
diff --git a/drivers/scsi/scsi_scan.c b/drivers/scsi/scsi_scan.c
index 9798c2c06b93..1c027a97d8b9 100644
--- a/drivers/scsi/scsi_scan.c
+++ b/drivers/scsi/scsi_scan.c
@@ -492,19 +492,20 @@ void scsi_target_reap(struct scsi_target *starget)
         struct Scsi_Host *shost = dev_to_shost(starget->dev.parent);
         unsigned long flags;
         enum scsi_target_state state;
-        int empty;
+        int empty = 0;
 
         spin_lock_irqsave(shost->host_lock, flags);
         state = starget->state;
-        empty = --starget->reap_ref == 0 &&
-                list_empty(&starget->devices) ? 1 : 0;
+        if (--starget->reap_ref == 0 && list_empty(&starget->devices)) {
+                empty = 1;
+                starget->state = STARGET_DEL;
+        }
         spin_unlock_irqrestore(shost->host_lock, flags);
 
         if (!empty)
                 return;
 
         BUG_ON(state == STARGET_DEL);
-        starget->state = STARGET_DEL;
         if (state == STARGET_CREATED)
                 scsi_target_destroy(starget);
         else
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index 829cc37abc41..8802e48bc063 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -97,6 +97,7 @@ MODULE_ALIAS_SCSI_DEVICE(TYPE_RBC);
 #endif
 
 static int  sd_revalidate_disk(struct gendisk *);
+static void sd_unlock_native_capacity(struct gendisk *disk);
 static int  sd_probe(struct device *);
 static int  sd_remove(struct device *);
 static void sd_shutdown(struct device *);
@@ -1101,6 +1102,7 @@ static const struct block_device_operations sd_fops = {
 #endif
         .media_changed          = sd_media_changed,
         .revalidate_disk        = sd_revalidate_disk,
+        .unlock_native_capacity = sd_unlock_native_capacity,
 };
 
 static unsigned int sd_completed_bytes(struct scsi_cmnd *scmd)
@@ -2121,6 +2123,26 @@ static int sd_revalidate_disk(struct gendisk *disk)
 }
 
 /**
+ *      sd_unlock_native_capacity - unlock native capacity
+ *      @disk: struct gendisk to set capacity for
+ *
+ *      Block layer calls this function if it detects that partitions
+ *      on @disk reach beyond the end of the device.  If the SCSI host
+ *      implements ->unlock_native_capacity() method, it's invoked to
+ *      give it a chance to adjust the device capacity.
+ *
+ *      CONTEXT:
+ *      Defined by block layer.  Might sleep.
+ */
+static void sd_unlock_native_capacity(struct gendisk *disk)
+{
+        struct scsi_device *sdev = scsi_disk(disk)->device;
+
+        if (sdev->host->hostt->unlock_native_capacity)
+                sdev->host->hostt->unlock_native_capacity(sdev);
+}
+
+/**
  *      sd_format_disk_name - format disk name
  *      @prefix: name prefix - ie. "sd" for SCSI disks
  *      @index: index of the disk to format name for
diff --git a/drivers/scsi/st.c b/drivers/scsi/st.c
index 3ea1a713ef25..24211d0efa6d 100644
--- a/drivers/scsi/st.c
+++ b/drivers/scsi/st.c
@@ -3962,6 +3962,7 @@ static const struct file_operations st_fops =
         .open =         st_open,
         .flush =        st_flush,
         .release =      st_release,
+        .llseek =       noop_llseek,
 };
 
 static int st_probe(struct device *dev)
diff --git a/drivers/serial/8250_pci.c b/drivers/serial/8250_pci.c
index 01c012da4e26..746a44621d91 100644
--- a/drivers/serial/8250_pci.c
+++ b/drivers/serial/8250_pci.c
@@ -982,6 +982,18 @@ static int skip_tx_en_setup(struct serial_private *priv,
 #define PCI_SUBDEVICE_ID_POCTAL422      0x0408
 #define PCI_VENDOR_ID_ADVANTECH         0x13fe
 #define PCI_DEVICE_ID_ADVANTECH_PCI3620 0x3620
+#define PCI_DEVICE_ID_TITAN_200I        0x8028
+#define PCI_DEVICE_ID_TITAN_400I        0x8048
+#define PCI_DEVICE_ID_TITAN_800I        0x8088
+#define PCI_DEVICE_ID_TITAN_800EH       0xA007
+#define PCI_DEVICE_ID_TITAN_800EHB      0xA008
+#define PCI_DEVICE_ID_TITAN_400EH       0xA009
+#define PCI_DEVICE_ID_TITAN_100E        0xA010
+#define PCI_DEVICE_ID_TITAN_200E        0xA012
+#define PCI_DEVICE_ID_TITAN_400E        0xA013
+#define PCI_DEVICE_ID_TITAN_800E        0xA014
+#define PCI_DEVICE_ID_TITAN_200EI       0xA016
+#define PCI_DEVICE_ID_TITAN_200EISI     0xA017
 
 /* Unknown vendors/cards - this should not be in linux/pci_ids.h */
 #define PCI_SUBDEVICE_ID_UNKNOWN_0x1584 0x1584
@@ -1541,6 +1553,10 @@ enum pci_board_num_t {
         pbn_b3_4_115200,
         pbn_b3_8_115200,
 
+        pbn_b4_bt_2_921600,
+        pbn_b4_bt_4_921600,
+        pbn_b4_bt_8_921600,
+
         /*
          * Board-specific versions.
          */
@@ -1995,6 +2011,25 @@ static struct pciserial_board pci_boards[] __devinitdata = {
                 .uart_offset    = 8,
         },
 
+        [pbn_b4_bt_2_921600] = {
+                .flags          = FL_BASE4,
+                .num_ports      = 2,
+                .base_baud      = 921600,
+                .uart_offset    = 8,
+        },
+        [pbn_b4_bt_4_921600] = {
+                .flags          = FL_BASE4,
+                .num_ports      = 4,
+                .base_baud      = 921600,
+                .uart_offset    = 8,
+        },
+        [pbn_b4_bt_8_921600] = {
+                .flags          = FL_BASE4,
+                .num_ports      = 8,
+                .base_baud      = 921600,
+                .uart_offset    = 8,
+        },
+
         /*
          * Entries following this are board-specific.
          */
@@ -3043,6 +3078,42 @@ static struct pci_device_id serial_pci_tbl[] = {
         {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_800L,
                 PCI_ANY_ID, PCI_ANY_ID, 0, 0,
                 pbn_b0_bt_8_921600 },
+        {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_200I,
+                PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+                pbn_b4_bt_2_921600 },
+        {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_400I,
+                PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+                pbn_b4_bt_4_921600 },
+        {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_800I,
+                PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+                pbn_b4_bt_8_921600 },
+        {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_400EH,
+                PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+                pbn_b0_4_921600 },
+        {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_800EH,
+                PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+                pbn_b0_4_921600 },
+        {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_800EHB,
+                PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+                pbn_b0_4_921600 },
+        {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_100E,
+                PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+                pbn_oxsemi_1_4000000 },
+        {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_200E,
+                PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+                pbn_oxsemi_2_4000000 },
+        {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_400E,
+                PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+                pbn_oxsemi_4_4000000 },
+        {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_800E,
+                PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+                pbn_oxsemi_8_4000000 },
+        {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_200EI,
+                PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+                pbn_oxsemi_2_4000000 },
+        {       PCI_VENDOR_ID_TITAN, PCI_DEVICE_ID_TITAN_200EISI,
+                PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+                pbn_oxsemi_2_4000000 },
 
         {       PCI_VENDOR_ID_SIIG, PCI_DEVICE_ID_SIIG_1S_10x_550,
                 PCI_ANY_ID, PCI_ANY_ID, 0, 0,
diff --git a/drivers/serial/altera_uart.c b/drivers/serial/altera_uart.c
index bcee156d2f2e..0f1189605d21 100644
--- a/drivers/serial/altera_uart.c
+++ b/drivers/serial/altera_uart.c
@@ -89,15 +89,12 @@ static unsigned int altera_uart_tx_empty(struct uart_port *port)
 static unsigned int altera_uart_get_mctrl(struct uart_port *port)
 {
         struct altera_uart *pp = container_of(port, struct altera_uart, port);
-        unsigned long flags;
         unsigned int sigs;
 
-        spin_lock_irqsave(&port->lock, flags);
         sigs =
             (readl(port->membase + ALTERA_UART_STATUS_REG) &
              ALTERA_UART_STATUS_CTS_MSK) ? TIOCM_CTS : 0;
         sigs |= (pp->sigs & TIOCM_RTS);
-        spin_unlock_irqrestore(&port->lock, flags);
 
         return sigs;
 }
@@ -105,49 +102,37 @@ static unsigned int altera_uart_get_mctrl(struct uart_port *port)
 static void altera_uart_set_mctrl(struct uart_port *port, unsigned int sigs)
 {
         struct altera_uart *pp = container_of(port, struct altera_uart, port);
-        unsigned long flags;
 
-        spin_lock_irqsave(&port->lock, flags);
         pp->sigs = sigs;
         if (sigs & TIOCM_RTS)
                 pp->imr |= ALTERA_UART_CONTROL_RTS_MSK;
         else
                 pp->imr &= ~ALTERA_UART_CONTROL_RTS_MSK;
         writel(pp->imr, port->membase + ALTERA_UART_CONTROL_REG);
-        spin_unlock_irqrestore(&port->lock, flags);
 }
 
 static void altera_uart_start_tx(struct uart_port *port)
 {
         struct altera_uart *pp = container_of(port, struct altera_uart, port);
-        unsigned long flags;
 
-        spin_lock_irqsave(&port->lock, flags);
         pp->imr |= ALTERA_UART_CONTROL_TRDY_MSK;
         writel(pp->imr, port->membase + ALTERA_UART_CONTROL_REG);
-        spin_unlock_irqrestore(&port->lock, flags);
 }
 
 static void altera_uart_stop_tx(struct uart_port *port)
 {
         struct altera_uart *pp = container_of(port, struct altera_uart, port);
-        unsigned long flags;
 
-        spin_lock_irqsave(&port->lock, flags);
         pp->imr &= ~ALTERA_UART_CONTROL_TRDY_MSK;
         writel(pp->imr, port->membase + ALTERA_UART_CONTROL_REG);
-        spin_unlock_irqrestore(&port->lock, flags);
 }
 
 static void altera_uart_stop_rx(struct uart_port *port)
 {
         struct altera_uart *pp = container_of(port, struct altera_uart, port);
-        unsigned long flags;
 
-        spin_lock_irqsave(&port->lock, flags);
         pp->imr &= ~ALTERA_UART_CONTROL_RRDY_MSK;
         writel(pp->imr, port->membase + ALTERA_UART_CONTROL_REG);
-        spin_unlock_irqrestore(&port->lock, flags);
 }
 
 static void altera_uart_break_ctl(struct uart_port *port, int break_state)
@@ -272,10 +257,14 @@ static irqreturn_t altera_uart_interrupt(int irq, void *data)
         unsigned int isr;
 
         isr = readl(port->membase + ALTERA_UART_STATUS_REG) & pp->imr;
+
+        spin_lock(&port->lock);
         if (isr & ALTERA_UART_STATUS_RRDY_MSK)
                 altera_uart_rx_chars(pp);
         if (isr & ALTERA_UART_STATUS_TRDY_MSK)
                 altera_uart_tx_chars(pp);
+        spin_unlock(&port->lock);
+
         return IRQ_RETVAL(isr);
 }
 
@@ -402,31 +391,24 @@ int __init early_altera_uart_setup(struct altera_uart_platform_uart *platp)
         return 0;
 }
 
-static void altera_uart_console_putc(struct console *co, const char c)
+static void altera_uart_console_putc(struct uart_port *port, const char c)
 {
-        struct uart_port *port = &(altera_uart_ports + co->index)->port;
-        int i;
+        while (!(readl(port->membase + ALTERA_UART_STATUS_REG) &
+                 ALTERA_UART_STATUS_TRDY_MSK))
+                cpu_relax();
 
-        for (i = 0; i < 0x10000; i++) {
-                if (readl(port->membase + ALTERA_UART_STATUS_REG) &
-                    ALTERA_UART_STATUS_TRDY_MSK)
-                        break;
-        }
         writel(c, port->membase + ALTERA_UART_TXDATA_REG);
-        for (i = 0; i < 0x10000; i++) {
-                if (readl(port->membase + ALTERA_UART_STATUS_REG) &
-                    ALTERA_UART_STATUS_TRDY_MSK)
-                        break;
-        }
 }
 
 static void altera_uart_console_write(struct console *co, const char *s,
                                       unsigned int count)
 {
+        struct uart_port *port = &(altera_uart_ports + co->index)->port;
+
         for (; count; count--, s++) {
-                altera_uart_console_putc(co, *s);
+                altera_uart_console_putc(port, *s);
                 if (*s == '\n')
-                        altera_uart_console_putc(co, '\r');
+                        altera_uart_console_putc(port, '\r');
         }
 }
 
@@ -516,7 +498,7 @@ static int __devinit altera_uart_probe(struct platform_device *pdev)
         return 0;
 }
 
-static int altera_uart_remove(struct platform_device *pdev)
+static int __devexit altera_uart_remove(struct platform_device *pdev)
 {
         struct uart_port *port;
         int i;
diff --git a/drivers/serial/bfin_5xx.c b/drivers/serial/bfin_5xx.c
index 96f7e7484fee..511cbf687877 100644
--- a/drivers/serial/bfin_5xx.c
+++ b/drivers/serial/bfin_5xx.c
@@ -797,7 +797,7 @@ static void bfin_serial_shutdown(struct uart_port *port)
                 gpio_free(uart->rts_pin);
 #endif
 #ifdef CONFIG_SERIAL_BFIN_HARD_CTSRTS
-        if (UART_GET_IER(uart) && EDSSI)
+        if (UART_GET_IER(uart) & EDSSI)
                 free_irq(uart->status_irq, uart);
 #endif
 }
@@ -869,7 +869,12 @@ bfin_serial_set_termios(struct uart_port *port, struct ktermios *termios,
         }
 
         baud = uart_get_baud_rate(port, termios, old, 0, port->uartclk/16);
-        quot = uart_get_divisor(port, baud) - ANOMALY_05000230;
+        quot = uart_get_divisor(port, baud);
+
+        /* If discipline is not IRDA, apply ANOMALY_05000230 */
+        if (termios->c_line != N_IRDA)
+                quot -= ANOMALY_05000230;
+
         spin_lock_irqsave(&uart->port.lock, flags);
 
         UART_SET_ANOMALY_THRESHOLD(uart, USEC_PER_SEC / baud * 15);
diff --git a/drivers/serial/msm_serial.c b/drivers/serial/msm_serial.c
index ecdc0facf7ee..f8c816e7725d 100644
--- a/drivers/serial/msm_serial.c
+++ b/drivers/serial/msm_serial.c
@@ -41,19 +41,6 @@ struct msm_port {
         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);
@@ -320,11 +307,7 @@ 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);
+        msm_serial_set_mnd_regs(port);
 }
 
 static int msm_startup(struct uart_port *port)
@@ -706,6 +689,8 @@ static int __init msm_serial_probe(struct platform_device *pdev)
         if (unlikely(IS_ERR(msm_port->clk)))
                 return PTR_ERR(msm_port->clk);
         port->uartclk = clk_get_rate(msm_port->clk);
+        printk(KERN_INFO "uartclk = %d\n", port->uartclk);
+
 
         resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         if (unlikely(!resource))
diff --git a/drivers/serial/msm_serial.h b/drivers/serial/msm_serial.h
index 689f1fa0e84e..f6ca9ca79e98 100644
--- a/drivers/serial/msm_serial.h
+++ b/drivers/serial/msm_serial.h
@@ -114,4 +114,60 @@
 #define UART_MISR       0x0010
 #define UART_ISR        0x0014
 
+#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);
+}
+
+/*
+ * Setup the MND registers to use the TCXO clock.
+ */
+static inline void msm_serial_set_mnd_regs_tcxo(struct uart_port *port)
+{
+        msm_write(port, 0x06, UART_MREG);
+        msm_write(port, 0xF1, UART_NREG);
+        msm_write(port, 0x0F, UART_DREG);
+        msm_write(port, 0x1A, UART_MNDREG);
+}
+
+/*
+ * Setup the MND registers to use the TCXO clock divided by 4.
+ */
+static inline void msm_serial_set_mnd_regs_tcxoby4(struct uart_port *port)
+{
+        msm_write(port, 0x18, UART_MREG);
+        msm_write(port, 0xF6, UART_NREG);
+        msm_write(port, 0x0F, UART_DREG);
+        msm_write(port, 0x0A, UART_MNDREG);
+}
+
+static inline
+void msm_serial_set_mnd_regs_from_uartclk(struct uart_port *port)
+{
+        if (port->uartclk == 19200000)
+                msm_serial_set_mnd_regs_tcxo(port);
+        else
+                msm_serial_set_mnd_regs_tcxoby4(port);
+}
+
+/*
+ * TROUT has a specific defect that makes it report it's uartclk
+ * as 19.2Mhz (TCXO) when it's actually 4.8Mhz (TCXO/4). This special
+ * cases TROUT to use the right clock.
+ */
+#ifdef CONFIG_MACH_TROUT
+#define msm_serial_set_mnd_regs msm_serial_set_mnd_regs_tcxoby4
+#else
+#define msm_serial_set_mnd_regs msm_serial_set_mnd_regs_from_uartclk
+#endif
+
 #endif  /* __DRIVERS_SERIAL_MSM_SERIAL_H */
diff --git a/drivers/serial/pmac_zilog.c b/drivers/serial/pmac_zilog.c
index cabbdc7ba583..5b9cde79e4ea 100644
--- a/drivers/serial/pmac_zilog.c
+++ b/drivers/serial/pmac_zilog.c
@@ -2005,8 +2005,11 @@ static struct of_device_id pmz_match[] =
 MODULE_DEVICE_TABLE (of, pmz_match);
 
 static struct macio_driver pmz_driver = {
-        .name           = "pmac_zilog",
-        .match_table    = pmz_match,
+        .driver = {
+                .name           = "pmac_zilog",
+                .owner          = THIS_MODULE,
+                .of_match_table = pmz_match,
+        },
         .probe          = pmz_attach,
         .remove         = pmz_detach,
         .suspend        = pmz_suspend,
diff --git a/drivers/serial/s5pv210.c b/drivers/serial/s5pv210.c
index 8dc03837617b..4a789e5361a4 100644
--- a/drivers/serial/s5pv210.c
+++ b/drivers/serial/s5pv210.c
@@ -119,7 +119,7 @@ static int s5p_serial_probe(struct platform_device *pdev)
         return s3c24xx_serial_probe(pdev, s5p_uart_inf[pdev->id]);
 }
 
-static struct platform_driver s5p_serial_drv = {
+static struct platform_driver s5p_serial_driver = {
         .probe          = s5p_serial_probe,
         .remove         = __devexit_p(s3c24xx_serial_remove),
         .driver         = {
@@ -130,19 +130,19 @@ static struct platform_driver s5p_serial_drv = {
 
 static int __init s5pv210_serial_console_init(void)
 {
-        return s3c24xx_serial_initconsole(&s5p_serial_drv, s5p_uart_inf);
+        return s3c24xx_serial_initconsole(&s5p_serial_driver, s5p_uart_inf);
 }
 
 console_initcall(s5pv210_serial_console_init);
 
 static int __init s5p_serial_init(void)
 {
-        return s3c24xx_serial_init(&s5p_serial_drv, *s5p_uart_inf);
+        return s3c24xx_serial_init(&s5p_serial_driver, *s5p_uart_inf);
 }
 
 static void __exit s5p_serial_exit(void)
 {
-        platform_driver_unregister(&s5p_serial_drv);
+        platform_driver_unregister(&s5p_serial_driver);
 }
 
 module_init(s5p_serial_init);
diff --git a/drivers/serial/serial_cs.c b/drivers/serial/serial_cs.c
index dadd686c9801..526307368f8b 100644
--- a/drivers/serial/serial_cs.c
+++ b/drivers/serial/serial_cs.c
@@ -715,6 +715,8 @@ static struct pcmcia_device_id serial_ids[] = {
         PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0057, 0x0021),
         PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0089, 0x110a),
         PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0104, 0x000a),
+        PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0105, 0x0d0a),
+        PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0105, 0x0e0a),
         PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0105, 0xea15),
         PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0109, 0x0501),
         PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0138, 0x110a),
@@ -724,8 +726,6 @@ static struct pcmcia_device_id serial_ids[] = {
         PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x016c, 0x0081),
         PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x021b, 0x0101),
         PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x08a1, 0xc0ab),
-        PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0105, 0x0d0a),
-        PCMCIA_PFC_DEVICE_MANF_CARD(1, 0x0105, 0x0e0a),
         PCMCIA_PFC_DEVICE_PROD_ID123(1, "MEGAHERTZ", "CC/XJEM3288", "DATA/FAX/CELL ETHERNET MODEM", 0xf510db04, 0x04cd2988, 0x46a52d63),
         PCMCIA_PFC_DEVICE_PROD_ID123(1, "MEGAHERTZ", "CC/XJEM3336", "DATA/FAX/CELL ETHERNET MODEM", 0xf510db04, 0x0143b773, 0x46a52d63),
         PCMCIA_PFC_DEVICE_PROD_ID123(1, "MEGAHERTZ", "EM1144T", "PCMCIA MODEM", 0xf510db04, 0x856d66c8, 0xbd6c43ef),
@@ -768,17 +768,26 @@ static struct pcmcia_device_id serial_ids[] = {
         PCMCIA_DEVICE_MANF_CARD(0x00a4, 0x0276),
         PCMCIA_DEVICE_MANF_CARD(0x0101, 0x0039),
         PCMCIA_DEVICE_MANF_CARD(0x0104, 0x0006),
+        PCMCIA_DEVICE_MANF_CARD(0x0105, 0x0101), /* TDK DF2814 */
+        PCMCIA_DEVICE_MANF_CARD(0x0105, 0x100a), /* Xircom CM-56G */
+        PCMCIA_DEVICE_MANF_CARD(0x0105, 0x3e0a), /* TDK DF5660 */
         PCMCIA_DEVICE_MANF_CARD(0x0105, 0x410a),
+        PCMCIA_DEVICE_MANF_CARD(0x0107, 0x0002), /* USRobotics 14,400 */
         PCMCIA_DEVICE_MANF_CARD(0x010b, 0x0d50),
         PCMCIA_DEVICE_MANF_CARD(0x010b, 0x0d51),
         PCMCIA_DEVICE_MANF_CARD(0x010b, 0x0d52),
         PCMCIA_DEVICE_MANF_CARD(0x010b, 0x0d53),
         PCMCIA_DEVICE_MANF_CARD(0x010b, 0xd180),
+        PCMCIA_DEVICE_MANF_CARD(0x0115, 0x3330), /* USRobotics/SUN 14,400 */
+        PCMCIA_DEVICE_MANF_CARD(0x0124, 0x0100), /* Nokia DTP-2 ver II */
+        PCMCIA_DEVICE_MANF_CARD(0x0134, 0x5600), /* LASAT COMMUNICATIONS A/S */
         PCMCIA_DEVICE_MANF_CARD(0x0137, 0x000e),
         PCMCIA_DEVICE_MANF_CARD(0x0137, 0x001b),
         PCMCIA_DEVICE_MANF_CARD(0x0137, 0x0025),
         PCMCIA_DEVICE_MANF_CARD(0x0137, 0x0045),
         PCMCIA_DEVICE_MANF_CARD(0x0137, 0x0052),
+        PCMCIA_DEVICE_MANF_CARD(0x016c, 0x0006), /* Psion 56K+Fax */
+        PCMCIA_DEVICE_MANF_CARD(0x0200, 0x0001), /* MultiMobile */
         PCMCIA_DEVICE_PROD_ID134("ADV", "TECH", "COMpad-32/85", 0x67459937, 0x916d02ba, 0x8fbe92ae),
         PCMCIA_DEVICE_PROD_ID124("GATEWAY2000", "CC3144", "PCMCIA MODEM", 0x506bccae, 0xcb3685f1, 0xbd6c43ef),
         PCMCIA_DEVICE_PROD_ID14("MEGAHERTZ", "PCMCIA MODEM", 0xf510db04, 0xbd6c43ef),
@@ -792,16 +801,21 @@ static struct pcmcia_device_id serial_ids[] = {
         PCMCIA_DEVICE_PROD_ID12("COMPAQ", "PCMCIA 33600 FAX/DATA MODEM", 0xa3a3062c, 0x5a00ce95),
         PCMCIA_DEVICE_PROD_ID12("Computerboards, Inc.", "PCM-COM422", 0xd0b78f51, 0x7e2d49ed),
         PCMCIA_DEVICE_PROD_ID12("Dr. Neuhaus", "FURY CARD 14K4", 0x76942813, 0x8b96ce65),
+        PCMCIA_DEVICE_PROD_ID12("IBM", "ISDN/56K/GSM", 0xb569a6e5, 0xfee5297b),
         PCMCIA_DEVICE_PROD_ID12("Intelligent", "ANGIA FAX/MODEM", 0xb496e65e, 0xf31602a6),
         PCMCIA_DEVICE_PROD_ID12("Intel", "MODEM 2400+", 0x816cc815, 0x412729fb),
+        PCMCIA_DEVICE_PROD_ID12("Intertex", "IX34-PCMCIA", 0xf8a097e3, 0x97880447),
         PCMCIA_DEVICE_PROD_ID12("IOTech Inc ", "PCMCIA Dual RS-232 Serial Port Card", 0x3bd2d898, 0x92abc92f),
         PCMCIA_DEVICE_PROD_ID12("MACRONIX", "FAX/MODEM", 0x668388b3, 0x3f9bdf2f),
         PCMCIA_DEVICE_PROD_ID12("Multi-Tech", "MT1432LT", 0x5f73be51, 0x0b3e2383),
         PCMCIA_DEVICE_PROD_ID12("Multi-Tech", "MT2834LT", 0x5f73be51, 0x4cd7c09e),
         PCMCIA_DEVICE_PROD_ID12("OEM      ", "C288MX     ", 0xb572d360, 0xd2385b7a),
+        PCMCIA_DEVICE_PROD_ID12("Option International", "V34bis GSM/PSTN Data/Fax Modem", 0x9d7cd6f5, 0x5cb8bf41),
         PCMCIA_DEVICE_PROD_ID12("PCMCIA   ", "C336MX     ", 0x99bcafe9, 0xaa25bcab),
         PCMCIA_DEVICE_PROD_ID12("Quatech Inc", "PCMCIA Dual RS-232 Serial Port Card", 0xc4420b35, 0x92abc92f),
         PCMCIA_DEVICE_PROD_ID12("Quatech Inc", "Dual RS-232 Serial Port PC Card", 0xc4420b35, 0x031a380d),
+        PCMCIA_DEVICE_PROD_ID12("Telia", "SurfinBird 560P/A+", 0xe2cdd5e, 0xc9314b38),
+        PCMCIA_DEVICE_PROD_ID1("Smart Serial Port", 0x2d8ce292),
         PCMCIA_PFC_DEVICE_CIS_PROD_ID12(1, "PCMCIA", "EN2218-LAN/MODEM", 0x281f1c5d, 0x570f348e, "cis/PCMLM28.cis"),
         PCMCIA_PFC_DEVICE_CIS_PROD_ID12(1, "PCMCIA", "UE2218-LAN/MODEM", 0x281f1c5d, 0x6fdcacee, "cis/PCMLM28.cis"),
         PCMCIA_PFC_DEVICE_CIS_PROD_ID12(1, "Psion Dacom", "Gold Card V34 Ethernet", 0xf5f025c2, 0x338e8155, "cis/PCMLM28.cis"),
diff --git a/drivers/sfi/sfi_acpi.c b/drivers/sfi/sfi_acpi.c
index 34aba30eb84b..f5b4ca581541 100644
--- a/drivers/sfi/sfi_acpi.c
+++ b/drivers/sfi/sfi_acpi.c
@@ -173,3 +173,44 @@ int sfi_acpi_table_parse(char *signature, char *oem_id, char *oem_table_id,
         sfi_acpi_put_table(table);
         return ret;
 }
+
+static ssize_t sfi_acpi_table_show(struct file *filp, struct kobject *kobj,
+                               struct bin_attribute *bin_attr, char *buf,
+                               loff_t offset, size_t count)
+{
+        struct sfi_table_attr *tbl_attr =
+            container_of(bin_attr, struct sfi_table_attr, attr);
+        struct acpi_table_header *th = NULL;
+        struct sfi_table_key key;
+        ssize_t cnt;
+
+        key.sig = tbl_attr->name;
+        key.oem_id = NULL;
+        key.oem_table_id = NULL;
+
+        th = sfi_acpi_get_table(&key);
+        if (!th)
+                return 0;
+
+        cnt =  memory_read_from_buffer(buf, count, &offset,
+                                        th, th->length);
+        sfi_acpi_put_table(th);
+
+        return cnt;
+}
+
+
+void __init sfi_acpi_sysfs_init(void)
+{
+        u32 tbl_cnt, i;
+        struct sfi_table_attr *tbl_attr;
+
+        tbl_cnt = XSDT_GET_NUM_ENTRIES(xsdt_va, u64);
+        for (i = 0; i < tbl_cnt; i++) {
+                tbl_attr =
+                        sfi_sysfs_install_table(xsdt_va->table_offset_entry[i]);
+                tbl_attr->attr.read = sfi_acpi_table_show;
+        }
+
+        return;
+}
diff --git a/drivers/sfi/sfi_core.c b/drivers/sfi/sfi_core.c
index b204a0929139..ceba593dc84f 100644
--- a/drivers/sfi/sfi_core.c
+++ b/drivers/sfi/sfi_core.c
@@ -67,6 +67,7 @@
 #include <linux/acpi.h>
 #include <linux/init.h>
 #include <linux/sfi.h>
+#include <linux/slab.h>
 
 #include "sfi_core.h"
 
@@ -382,6 +383,104 @@ static __init int sfi_find_syst(void)
         return -1;
 }
 
+static struct kobject *sfi_kobj;
+static struct kobject *tables_kobj;
+
+static ssize_t sfi_table_show(struct file *filp, struct kobject *kobj,
+                               struct bin_attribute *bin_attr, char *buf,
+                               loff_t offset, size_t count)
+{
+        struct sfi_table_attr *tbl_attr =
+            container_of(bin_attr, struct sfi_table_attr, attr);
+        struct sfi_table_header *th = NULL;
+        struct sfi_table_key key;
+        ssize_t cnt;
+
+        key.sig = tbl_attr->name;
+        key.oem_id = NULL;
+        key.oem_table_id = NULL;
+
+        if (strncmp(SFI_SIG_SYST, tbl_attr->name, SFI_SIGNATURE_SIZE)) {
+                th = sfi_get_table(&key);
+                if (!th)
+                        return 0;
+
+                cnt =  memory_read_from_buffer(buf, count, &offset,
+                                                th, th->len);
+                sfi_put_table(th);
+        } else
+                cnt =  memory_read_from_buffer(buf, count, &offset,
+                                        syst_va, syst_va->header.len);
+
+        return cnt;
+}
+
+struct sfi_table_attr __init *sfi_sysfs_install_table(u64 pa)
+{
+        struct sfi_table_attr *tbl_attr;
+        struct sfi_table_header *th;
+        int ret;
+
+        tbl_attr = kzalloc(sizeof(struct sfi_table_attr), GFP_KERNEL);
+        if (!tbl_attr)
+                return NULL;
+
+        th = sfi_map_table(pa);
+        if (!th || !th->sig[0]) {
+                kfree(tbl_attr);
+                return NULL;
+        }
+
+        sysfs_attr_init(&tbl_attr->attr.attr);
+        memcpy(tbl_attr->name, th->sig, SFI_SIGNATURE_SIZE);
+
+        tbl_attr->attr.size = 0;
+        tbl_attr->attr.read = sfi_table_show;
+        tbl_attr->attr.attr.name = tbl_attr->name;
+        tbl_attr->attr.attr.mode = 0400;
+
+        ret = sysfs_create_bin_file(tables_kobj,
+                                  &tbl_attr->attr);
+        if (ret) {
+                kfree(tbl_attr);
+                tbl_attr = NULL;
+        }
+
+        sfi_unmap_table(th);
+        return tbl_attr;
+}
+
+static int __init sfi_sysfs_init(void)
+{
+        int tbl_cnt, i;
+
+        if (sfi_disabled)
+                return 0;
+
+        sfi_kobj = kobject_create_and_add("sfi", firmware_kobj);
+        if (!sfi_kobj)
+                return 0;
+
+        tables_kobj = kobject_create_and_add("tables", sfi_kobj);
+        if (!tables_kobj) {
+                kobject_put(sfi_kobj);
+                return 0;
+        }
+
+        sfi_sysfs_install_table(syst_pa);
+
+        tbl_cnt = SFI_GET_NUM_ENTRIES(syst_va, u64);
+
+        for (i = 0; i < tbl_cnt; i++)
+                sfi_sysfs_install_table(syst_va->pentry[i]);
+
+        sfi_acpi_sysfs_init();
+        kobject_uevent(sfi_kobj, KOBJ_ADD);
+        kobject_uevent(tables_kobj, KOBJ_ADD);
+        pr_info("SFI sysfs interfaces init success\n");
+        return 0;
+}
+
 void __init sfi_init(void)
 {
         if (!acpi_disabled)
@@ -390,7 +489,7 @@ void __init sfi_init(void)
         if (sfi_disabled)
                 return;
 
-        pr_info("Simple Firmware Interface v0.7 http://simplefirmware.org\n");
+        pr_info("Simple Firmware Interface v0.81 http://simplefirmware.org\n");
 
         if (sfi_find_syst() || sfi_parse_syst() || sfi_platform_init())
                 disable_sfi();
@@ -414,3 +513,9 @@ void __init sfi_init_late(void)
 
         sfi_acpi_init();
 }
+
+/*
+ * The reason we put it here becasue we need wait till the /sys/firmware
+ * is setup, then our interface can be registered in /sys/firmware/sfi
+ */
+core_initcall(sfi_sysfs_init);
diff --git a/drivers/sfi/sfi_core.h b/drivers/sfi/sfi_core.h
index da82d39e104d..b7cf220d44ec 100644
--- a/drivers/sfi/sfi_core.h
+++ b/drivers/sfi/sfi_core.h
@@ -61,6 +61,12 @@ struct sfi_table_key{
         char    *oem_table_id;
 };
 
+/* sysfs interface */
+struct sfi_table_attr {
+        struct bin_attribute attr;
+        char name[8];
+};
+
 #define SFI_ANY_KEY { .sig = NULL, .oem_id = NULL, .oem_table_id = NULL }
 
 extern int __init sfi_acpi_init(void);
@@ -68,3 +74,5 @@ extern  struct sfi_table_header *sfi_check_table(u64 paddr,
                                         struct sfi_table_key *key);
 struct sfi_table_header *sfi_get_table(struct sfi_table_key *key);
 extern void sfi_put_table(struct sfi_table_header *table);
+extern struct sfi_table_attr __init *sfi_sysfs_install_table(u64 pa);
+extern void __init sfi_acpi_sysfs_init(void);
diff --git a/drivers/sh/intc.c b/drivers/sh/intc.c
index c585574b9aed..e91a23e5ffd8 100644
--- a/drivers/sh/intc.c
+++ b/drivers/sh/intc.c
@@ -16,6 +16,8 @@
  * License.  See the file "COPYING" in the main directory of this archive
  * for more details.
  */
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/init.h>
 #include <linux/irq.h>
 #include <linux/module.h>
@@ -855,8 +857,8 @@ static void __init intc_register_irq(struct intc_desc *desc,
                 primary = 1;
 
         if (!data[0] && !data[1])
-                pr_warning("intc: missing unique irq mask for "
-                           "irq %d (vect 0x%04x)\n", irq, irq2evt(irq));
+                pr_warning("missing unique irq mask for irq %d (vect 0x%04x)\n",
+                           irq, irq2evt(irq));
 
         data[0] = data[0] ? data[0] : intc_mask_data(desc, d, enum_id, 1);
         data[1] = data[1] ? data[1] : intc_prio_data(desc, d, enum_id, 1);
@@ -952,7 +954,7 @@ int __init register_intc_controller(struct intc_desc *desc)
         struct intc_desc_int *d;
         struct resource *res;
 
-        pr_info("intc: Registered controller '%s' with %u IRQs\n",
+        pr_info("Registered controller '%s' with %u IRQs\n",
                 desc->name, hw->nr_vectors);
 
         d = kzalloc(sizeof(*d), GFP_NOWAIT);
@@ -1148,7 +1150,7 @@ int register_intc_userimask(unsigned long addr)
         if (unlikely(!uimask))
                 return -ENOMEM;
 
-        pr_info("intc: userimask support registered for levels 0 -> %d\n",
+        pr_info("userimask support registered for levels 0 -> %d\n",
                 default_prio_level - 1);
 
         return 0;
@@ -1286,7 +1288,7 @@ static int __init register_intc_sysdevs(void)
         }
 
         if (error)
-                pr_err("intc: sysdev registration error\n");
+                pr_err("sysdev registration error\n");
 
         return error;
 }
diff --git a/drivers/spi/mpc512x_psc_spi.c b/drivers/spi/mpc512x_psc_spi.c
index 28a126d2742b..2534b1ec3edd 100644
--- a/drivers/spi/mpc512x_psc_spi.c
+++ b/drivers/spi/mpc512x_psc_spi.c
@@ -512,29 +512,29 @@ static int __init mpc512x_psc_spi_of_probe(struct of_device *op,
         u64 regaddr64, size64;
         s16 id = -1;
 
-        regaddr_p = of_get_address(op->node, 0, &size64, NULL);
+        regaddr_p = of_get_address(op->dev.of_node, 0, &size64, NULL);
         if (!regaddr_p) {
                 dev_err(&op->dev, "Invalid PSC address\n");
                 return -EINVAL;
         }
-        regaddr64 = of_translate_address(op->node, regaddr_p);
+        regaddr64 = of_translate_address(op->dev.of_node, regaddr_p);
 
         /* get PSC id (0..11, used by port_config) */
         if (op->dev.platform_data == NULL) {
                 const u32 *psc_nump;
 
-                psc_nump = of_get_property(op->node, "cell-index", NULL);
+                psc_nump = of_get_property(op->dev.of_node, "cell-index", NULL);
                 if (!psc_nump || *psc_nump > 11) {
                         dev_err(&op->dev, "mpc512x_psc_spi: Device node %s "
                                 "has invalid cell-index property\n",
-                                op->node->full_name);
+                                op->dev.of_node->full_name);
                         return -EINVAL;
                 }
                 id = *psc_nump;
         }
 
         return mpc512x_psc_spi_do_probe(&op->dev, (u32) regaddr64, (u32) size64,
-                                        irq_of_parse_and_map(op->node, 0), id);
+                                irq_of_parse_and_map(op->dev.of_node, 0), id);
 }
 
 static int __exit mpc512x_psc_spi_of_remove(struct of_device *op)
@@ -550,12 +550,12 @@ static struct of_device_id mpc512x_psc_spi_of_match[] = {
 MODULE_DEVICE_TABLE(of, mpc512x_psc_spi_of_match);
 
 static struct of_platform_driver mpc512x_psc_spi_of_driver = {
-        .match_table = mpc512x_psc_spi_of_match,
         .probe = mpc512x_psc_spi_of_probe,
         .remove = __exit_p(mpc512x_psc_spi_of_remove),
         .driver = {
                 .name = "mpc512x-psc-spi",
                 .owner = THIS_MODULE,
+                .of_match_table = mpc512x_psc_spi_of_match,
         },
 };
 
diff --git a/drivers/spi/spi_ppc4xx.c b/drivers/spi/spi_ppc4xx.c
index 19c0b3b34fce..d53466a249d9 100644
--- a/drivers/spi/spi_ppc4xx.c
+++ b/drivers/spi/spi_ppc4xx.c
@@ -397,7 +397,7 @@ static int __init spi_ppc4xx_of_probe(struct of_device *op,
         struct spi_master *master;
         struct spi_bitbang *bbp;
         struct resource resource;
-        struct device_node *np = op->node;
+        struct device_node *np = op->dev.of_node;
         struct device *dev = &op->dev;
         struct device_node *opbnp;
         int ret;
diff --git a/drivers/ssb/pci.c b/drivers/ssb/pci.c
index 989e2752cc36..6dcda86be6eb 100644
--- a/drivers/ssb/pci.c
+++ b/drivers/ssb/pci.c
@@ -625,9 +625,12 @@ static int ssb_pci_sprom_get(struct ssb_bus *bus,
                 ssb_printk(KERN_ERR PFX "No SPROM available!\n");
                 return -ENODEV;
         }
-
-        bus->sprom_offset = (bus->chipco.dev->id.revision < 31) ?
-                SSB_SPROM_BASE1 : SSB_SPROM_BASE31;
+        if (bus->chipco.dev) {  /* can be unavailible! */
+                bus->sprom_offset = (bus->chipco.dev->id.revision < 31) ?
+                        SSB_SPROM_BASE1 : SSB_SPROM_BASE31;
+        } else {
+                bus->sprom_offset = SSB_SPROM_BASE1;
+        }
 
         buf = kcalloc(SSB_SPROMSIZE_WORDS_R123, sizeof(u16), GFP_KERNEL);
         if (!buf)
diff --git a/drivers/ssb/sprom.c b/drivers/ssb/sprom.c
index 007bc3a03486..4f7cc8d13277 100644
--- a/drivers/ssb/sprom.c
+++ b/drivers/ssb/sprom.c
@@ -185,6 +185,7 @@ bool ssb_is_sprom_available(struct ssb_bus *bus)
         /* this routine differs from specs as we do not access SPROM directly
            on PCMCIA */
         if (bus->bustype == SSB_BUSTYPE_PCI &&
+            bus->chipco.dev &&  /* can be unavailible! */
             bus->chipco.dev->id.revision >= 31)
                 return bus->chipco.capabilities & SSB_CHIPCO_CAP_SPROM;
 
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index b5c3b3013037..984a75440710 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -141,5 +141,11 @@ source "drivers/staging/ti-st/Kconfig"
 
 source "drivers/staging/adis16255/Kconfig"
 
+source "drivers/staging/xgifb/Kconfig"
+
+source "drivers/staging/mrst-touchscreen/Kconfig"
+
+source "drivers/staging/msm/Kconfig"
+
 endif # !STAGING_EXCLUDE_BUILD
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index e330dd5e843d..9fa25133874a 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -51,3 +51,6 @@ obj-$(CONFIG_CRYSTALHD)		+= crystalhd/
 obj-$(CONFIG_CXT1E1)            += cxt1e1/
 obj-$(CONFIG_TI_ST)             += ti-st/
 obj-$(CONFIG_ADIS16255)         += adis16255/
+obj-$(CONFIG_FB_XGI)            += xgifb/
+obj-$(CONFIG_TOUCHSCREEN_MRSTOUCH)      += mrst-touchscreen/
+obj-$(CONFIG_MSM_STAGING)       += msm/
diff --git a/drivers/staging/adis16255/Kconfig b/drivers/staging/adis16255/Kconfig
index a642be66adea..a883c1f4478b 100644
--- a/drivers/staging/adis16255/Kconfig
+++ b/drivers/staging/adis16255/Kconfig
@@ -1,5 +1,5 @@
 config ADIS16255
-        tristate "Ananlog Devices ADIS16250/16255"
+        tristate "Analog Devices ADIS16250/16255"
         depends on SPI && SYSFS
         ---help---
         If you say yes here you get support for the Analog Devices
diff --git a/drivers/staging/adis16255/adis16255.c b/drivers/staging/adis16255/adis16255.c
index 1ba11f00b2e7..55d66e290f7d 100644
--- a/drivers/staging/adis16255/adis16255.c
+++ b/drivers/staging/adis16255/adis16255.c
@@ -361,7 +361,7 @@ err:
 
 /*-------------------------------------------------------------------------*/
 
-static int spi_adis16255_probe(struct spi_device *spi)
+static int __devinit spi_adis16255_probe(struct spi_device *spi)
 {
 
         struct adis16255_init_data *init_data = spi->dev.platform_data;
@@ -421,7 +421,7 @@ err:
         return status;
 }
 
-static int spi_adis16255_remove(struct spi_device *spi)
+static int __devexit spi_adis16255_remove(struct spi_device *spi)
 {
         struct spi_adis16255_data  *spiadis    = dev_get_drvdata(&spi->dev);
 
diff --git a/drivers/staging/batman-adv/device.c b/drivers/staging/batman-adv/device.c
index ad82ec4a4856..7eb6559e0315 100644
--- a/drivers/staging/batman-adv/device.c
+++ b/drivers/staging/batman-adv/device.c
@@ -309,7 +309,7 @@ void bat_device_add_packet(struct device_client *device_client,
         struct device_packet *device_packet;
         unsigned long flags;
 
-        device_packet = kmalloc(sizeof(struct device_packet), GFP_KERNEL);
+        device_packet = kmalloc(sizeof(struct device_packet), GFP_ATOMIC);
 
         if (!device_packet)
                 return;
diff --git a/drivers/staging/batman-adv/main.c b/drivers/staging/batman-adv/main.c
index 9d13979c2d8e..74c70d589a93 100644
--- a/drivers/staging/batman-adv/main.c
+++ b/drivers/staging/batman-adv/main.c
@@ -127,7 +127,10 @@ int init_module(void)
         return 0;
 
 unreg_soft_device:
-        unregister_netdevice(soft_device);
+        unregister_netdev(soft_device);
+        soft_device = NULL;
+        return -ENOMEM;
+
 free_soft_device:
         free_netdev(soft_device);
         soft_device = NULL;
diff --git a/drivers/staging/batman-adv/send.c b/drivers/staging/batman-adv/send.c
index d8536e277a26..ac69ed871a76 100644
--- a/drivers/staging/batman-adv/send.c
+++ b/drivers/staging/batman-adv/send.c
@@ -440,6 +440,9 @@ void send_outstanding_bcast_packet(struct work_struct *work)
         hlist_del(&forw_packet->list);
         spin_unlock_irqrestore(&forw_bcast_list_lock, flags);
 
+        if (atomic_read(&module_state) == MODULE_DEACTIVATING)
+                goto out;
+
         /* rebroadcast packet */
         rcu_read_lock();
         list_for_each_entry_rcu(batman_if, &if_list, list) {
@@ -453,15 +456,15 @@ void send_outstanding_bcast_packet(struct work_struct *work)
 
         forw_packet->num_packets++;
 
-        /* if we still have some more bcasts to send and we are not shutting
-         * down */
-        if ((forw_packet->num_packets < 3) &&
-            (atomic_read(&module_state) != MODULE_DEACTIVATING))
+        /* if we still have some more bcasts to send */
+        if (forw_packet->num_packets < 3) {
                 _add_bcast_packet_to_list(forw_packet, ((5 * HZ) / 1000));
-        else {
-                forw_packet_free(forw_packet);
-                atomic_inc(&bcast_queue_left);
+                return;
         }
+
+out:
+        forw_packet_free(forw_packet);
+        atomic_inc(&bcast_queue_left);
 }
 
 void send_outstanding_bat_packet(struct work_struct *work)
@@ -476,6 +479,9 @@ void send_outstanding_bat_packet(struct work_struct *work)
         hlist_del(&forw_packet->list);
         spin_unlock_irqrestore(&forw_bat_list_lock, flags);
 
+        if (atomic_read(&module_state) == MODULE_DEACTIVATING)
+                goto out;
+
         send_packet(forw_packet);
 
         /**
@@ -483,10 +489,10 @@ void send_outstanding_bat_packet(struct work_struct *work)
          * to determine the queues wake up time unless we are
          * shutting down
          */
-        if ((forw_packet->own) &&
-            (atomic_read(&module_state) != MODULE_DEACTIVATING))
+        if (forw_packet->own)
                 schedule_own_packet(forw_packet->if_incoming);
 
+out:
         /* don't count own packet */
         if (!forw_packet->own)
                 atomic_inc(&batman_queue_left);
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index 8ce307e64b58..aad47326d6dc 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -100,15 +100,6 @@ menuconfig COMEDI_ISA_DRIVERS
 
 if COMEDI_ISA_DRIVERS && ISA
 
-config COMEDI_8255
-        tristate "Generic 8255 support"
-        default N
-        ---help---
-          Enable generic 8255 support.
-
-          To compile this driver as a module, choose M here: the module will be
-          called 8255.
-
 config COMEDI_ACL7225B
         tristate "ADlink NuDAQ ACL-7225b and compatibles support"
         default N
@@ -130,6 +121,7 @@ config COMEDI_PCL711
 
 config COMEDI_PCL724
         tristate "Advantech PCL-722/724/731 and ADlink ACL-7122/7124/PET-48DIO"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for Advantech PCL-724, PCL-722, PCL-731 and
@@ -198,6 +190,7 @@ config COMEDI_PCL818
 
 config COMEDI_PCM3724
         tristate "Advantech PCM-3724 PC/104 card support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for Advantech PCM-3724 PC/104 cards.
@@ -232,18 +225,9 @@ config COMEDI_RTI802
           To compile this driver as a module, choose M here: the module will be
           called rti802.
 
-config COMEDI_DAS08
-        tristate "DAS-08 compatible ISA, PC/104 and PCMCIA card support"
-        default N
-        ---help---
-          Enable support for Keithley Metrabyte/ComputerBoards DAS08
-          and compatible ISA and PC/104 cards
-
-          To compile this driver as a module, choose M here: the module will be
-          called das08.
-
 config COMEDI_DAS16M1
         tristate "MeasurementComputing CIO-DAS16/M1DAS-16 ISA card support"
+        select COMEDI_8255
         select COMEDI_FC
         default N
         ---help---
@@ -254,6 +238,7 @@ config COMEDI_DAS16M1
 
 config COMEDI_DAS16
         tristate "DAS-16 compatible ISA and PC/104 card support"
+        select COMEDI_8255
         select COMEDI_FC
         default N
         ---help---
@@ -385,6 +370,7 @@ config COMEDI_FL512
 
 config COMEDI_AIO_AIO12_8
         tristate "I/O Products PC/104 AIO12-8 Analog I/O Board support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for I/O Products PC/104 AIO12-8 Analog I/O Board
@@ -466,6 +452,7 @@ config COMEDI_NI_ATMIO
 config COMEDI_NI_ATMIO16D
         tristate "NI AT-MIO16/AT-MIO16D series ISA-PNP card support"
         depends on ISAPNP && COMEDI_NI_COMMON
+        select COMEDI_8255
         default N
         ---help---
           Enable support for National Instruments AT-MIO16/AT-MIO16D cards.
@@ -667,6 +654,7 @@ config COMEDI_ADDI_APCI_3XXX
 
 config COMEDI_ADL_PCI6208
         tristate "ADLink PCI-6208A support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for ADLink PCI-6208A cards
@@ -751,6 +739,7 @@ config COMEDI_ADV_PCI1723
 
 config COMEDI_ADV_PCI_DIO
         tristate "Advantech PCI DIO card support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for Advantech PCI DIO cards
@@ -762,6 +751,7 @@ config COMEDI_ADV_PCI_DIO
 
 config COMEDI_AMPLC_DIO200
         tristate "Amplicon PC272E and PCI272 DIO board support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for Amplicon PC272E and PCI272 DIO boards
@@ -771,6 +761,7 @@ config COMEDI_AMPLC_DIO200
 
 config COMEDI_AMPLC_PC236
         tristate "Amplicon PC36AT and PCI236 DIO board support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for Amplicon PC36AT and PCI236 DIO boards
@@ -799,6 +790,7 @@ config COMEDI_AMPLC_PCI224
 
 config COMEDI_AMPLC_PCI230
         tristate "Amplicon PCI230 and PCI260 support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for Amplicon PCI230 and PCI260 Multifunction I/O
@@ -869,6 +861,7 @@ config COMEDI_II_PCI20KC
 
 config COMEDI_DAQBOARD2000
         tristate "IOtech DAQboard/2000 support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for the IOtech DAQboard/2000
@@ -896,6 +889,7 @@ config COMEDI_KE_COUNTER
 
 config COMEDI_CB_PCIDAS64
         tristate "MeasurementComputing PCI-DAS 64xx, 60xx, and 4020 support"
+        select COMEDI_8255
         select COMEDI_FC
         default N
         ---help---
@@ -907,6 +901,7 @@ config COMEDI_CB_PCIDAS64
 
 config COMEDI_CB_PCIDAS
         tristate "MeasurementComputing PCI-DAS support"
+        select COMEDI_8255
         select COMEDI_FC
         default N
         ---help---
@@ -920,6 +915,7 @@ config COMEDI_CB_PCIDAS
 
 config COMEDI_CB_PCIDDA
         tristate "MeasurementComputing PCI-DDA series support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for ComputerBoards/MeasurementComputing PCI-DDA
@@ -931,6 +927,7 @@ config COMEDI_CB_PCIDDA
 
 config COMEDI_CB_PCIDIO
         tristate "MeasurementComputing PCI-DIO series support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for ComputerBoards/MeasurementComputing PCI-DIO series
@@ -941,6 +938,7 @@ config COMEDI_CB_PCIDIO
 
 config COMEDI_CB_PCIMDAS
         tristate "MeasurementComputing PCIM-DAS1602/16 support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for ComputerBoards/MeasurementComputing PCI Migration
@@ -951,6 +949,7 @@ config COMEDI_CB_PCIMDAS
 
 config COMEDI_CB_PCIMDDA
         tristate "MeasurementComputing PCIM-DDA06-16 support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for ComputerBoards/MeasurementComputing PCIM-DDA06-16
@@ -1026,6 +1025,7 @@ config COMEDI_NI_670X
 config COMEDI_NI_PCIDIO
         tristate "NI PCI-DIO32HS, PCI-DIO96, PCI-6533, PCI-6503 support"
         depends on COMEDI_MITE
+        select COMEDI_8255
         default N
         ---help---
           Enable support for National Instruments PCI-DIO-32HS, PXI-6533,
@@ -1058,6 +1058,7 @@ config COMEDI_NI_PCIMIO
 
 config COMEDI_RTD520
         tristate "Real Time Devices PCI4520/DM7520 support"
+        select COMEDI_8255
         default N
         ---help---
           Enable support for Real Time Devices PCI4520/DM7520
@@ -1097,7 +1098,7 @@ endif # COMEDI_PCI_DRIVERS
 
 menuconfig COMEDI_PCMCIA_DRIVERS
         tristate "Comedi PCMCIA drivers"
-        depends on COMEDI && PCMCIA && PCCARD
+        depends on COMEDI && (PCMCIA || PCCARD)
         default N
         ---help---
           Enable comedi PCMCIA and PCCARD drivers to be built
@@ -1142,6 +1143,7 @@ config COMEDI_NI_DAQ_700_CS
 config COMEDI_NI_DAQ_DIO24_CS
         tristate "NI DAQ-Card DIO-24 PCMCIA support"
         depends on COMEDI_NI_COMMON
+        select COMEDI_8255
         default N
         ---help---
           Enable support for the National Instruments PCMCIA DAQ-Card DIO-24
@@ -1162,8 +1164,8 @@ config COMEDI_NI_LABPC_CS
 config COMEDI_NI_MIO_CS
         tristate "NI DAQCard E series PCMCIA support"
         depends on COMEDI_NI_TIO && COMEDI_NI_COMMON
-        default N
         select COMEDI_FC
+        default N
         ---help---
           Enable support for the National Instruments PCMCIA DAQCard E series
           DAQCard-ai-16xe-50, DAQCard-ai-16e-4, DAQCard-6062E, DAQCard-6024E
@@ -1265,7 +1267,8 @@ config COMEDI_MITE
 
 config COMEDI_NI_TIO
         tristate "NI general purpose counter support"
-        select COMEDI_MITE
+        depends on COMEDI_MITE
+        select COMEDI_8255
         default N
         ---help---
           Enable support for National Instruments general purpose counters.
@@ -1278,6 +1281,8 @@ config COMEDI_NI_TIO
 
 config COMEDI_NI_LABPC
         tristate "NI Lab-PC and compatibles ISA and PCI support"
+        depends on COMEDI_MITE
+        select COMEDI_8255
         select COMEDI_FC
         default N
         ---help---
@@ -1291,8 +1296,40 @@ config COMEDI_NI_LABPC
 
 endif # COMEDI_NI_COMMON
 
+config COMEDI_8255
+        tristate "Generic 8255 support"
+        depends on COMEDI
+        default N
+        ---help---
+          Enable generic 8255 support.
+
+          You should enable compilation this driver if you plan to use a board
+          that has an 8255 chip. For multifunction boards, the main driver will
+          configure the 8255 subdevice automatically.
+
+          Note that most PCI 8255 boards do NOT work with this driver, and
+          need a separate driver as a wrapper.
+
+          To compile this driver as a module, choose M here: the module will be
+          called 8255.
+
+config COMEDI_DAS08
+        tristate "DAS-08 compatible support"
+        depends on COMEDI
+        select COMEDI_8255
+        default N
+        ---help---
+          Enable support for DAS08 and compatible ISA, PC/104 and PCI cards.
+
+          Note that PCMCIA DAS08 cards are not directly supported by this
+          driver, and need a separate driver as a wrapper.
+
+          To compile this driver as a module, choose M here: the module will be
+          called das08.
+
 config COMEDI_FC
         tristate "Comedi shared functions for low-level driver support"
+        depends on COMEDI
         default N
         ---help---
           Enable support for shared functions for low-level drivers.
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index aced00e5cd10..aeb2c00875cd 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -83,7 +83,7 @@ static int do_subdinfo_ioctl(struct comedi_device *dev,
 static int do_chaninfo_ioctl(struct comedi_device *dev,
                              struct comedi_chaninfo __user *arg);
 static int do_bufinfo_ioctl(struct comedi_device *dev,
-                            struct comedi_bufinfo __user *arg);
+                            struct comedi_bufinfo __user *arg, void *file);
 static int do_cmd_ioctl(struct comedi_device *dev,
                         struct comedi_cmd __user *arg, void *file);
 static int do_lock_ioctl(struct comedi_device *dev, unsigned int arg,
@@ -169,7 +169,8 @@ static long comedi_unlocked_ioctl(struct file *file, unsigned int cmd,
                 break;
         case COMEDI_BUFINFO:
                 rc = do_bufinfo_ioctl(dev,
-                                      (struct comedi_bufinfo __user *)arg);
+                                      (struct comedi_bufinfo __user *)arg,
+                                      file);
                 break;
         case COMEDI_LOCK:
                 rc = do_lock_ioctl(dev, arg, file);
@@ -563,7 +564,7 @@ static int do_chaninfo_ioctl(struct comedi_device *dev,
 
   */
 static int do_bufinfo_ioctl(struct comedi_device *dev,
-                            struct comedi_bufinfo __user *arg)
+                            struct comedi_bufinfo __user *arg, void *file)
 {
         struct comedi_bufinfo bi;
         struct comedi_subdevice *s;
@@ -576,6 +577,10 @@ static int do_bufinfo_ioctl(struct comedi_device *dev,
                 return -EINVAL;
 
         s = dev->subdevices + bi.subdevice;
+
+        if (s->lock && s->lock != file)
+                return -EACCES;
+
         async = s->async;
 
         if (!async) {
@@ -584,8 +589,17 @@ static int do_bufinfo_ioctl(struct comedi_device *dev,
                 bi.buf_read_ptr = 0;
                 bi.buf_write_count = 0;
                 bi.buf_read_count = 0;
+                bi.bytes_read = 0;
+                bi.bytes_written = 0;
                 goto copyback;
         }
+        if (!s->busy) {
+                bi.bytes_read = 0;
+                bi.bytes_written = 0;
+                goto copyback_position;
+        }
+        if (s->busy != file)
+                return -EACCES;
 
         if (bi.bytes_read && (s->subdev_flags & SDF_CMD_READ)) {
                 bi.bytes_read = comedi_buf_read_alloc(async, bi.bytes_read);
@@ -604,6 +618,7 @@ static int do_bufinfo_ioctl(struct comedi_device *dev,
                 comedi_buf_write_free(async, bi.bytes_written);
         }
 
+copyback_position:
         bi.buf_write_count = async->buf_write_count;
         bi.buf_write_ptr = async->buf_write_ptr;
         bi.buf_read_count = async->buf_read_count;
@@ -1576,6 +1591,19 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
         while (nbytes > 0 && !retval) {
                 set_current_state(TASK_INTERRUPTIBLE);
 
+                if (!(comedi_get_subdevice_runflags(s) & SRF_RUNNING)) {
+                        if (count == 0) {
+                                if (comedi_get_subdevice_runflags(s) &
+                                        SRF_ERROR) {
+                                        retval = -EPIPE;
+                                } else {
+                                        retval = 0;
+                                }
+                                do_become_nonbusy(dev, s);
+                        }
+                        break;
+                }
+
                 n = nbytes;
 
                 m = n;
@@ -1588,16 +1616,6 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
                         n = m;
 
                 if (n == 0) {
-                        if (!(comedi_get_subdevice_runflags(s) & SRF_RUNNING)) {
-                                if (comedi_get_subdevice_runflags(s) &
-                                    SRF_ERROR) {
-                                        retval = -EPIPE;
-                                } else {
-                                        retval = 0;
-                                }
-                                do_become_nonbusy(dev, s);
-                                break;
-                        }
                         if (file->f_flags & O_NONBLOCK) {
                                 retval = -EAGAIN;
                                 break;
diff --git a/drivers/staging/comedi/drivers/Makefile b/drivers/staging/comedi/drivers/Makefile
index 5ccf246e2526..354fb7d29841 100644
--- a/drivers/staging/comedi/drivers/Makefile
+++ b/drivers/staging/comedi/drivers/Makefile
@@ -12,7 +12,6 @@ obj-$(CONFIG_COMEDI_SERIAL2002)		+= serial2002.o
 obj-$(CONFIG_COMEDI_SKEL)               += skel.o
 
 # Comedi ISA drivers
-obj-$(CONFIG_COMEDI_8255)               += 8255.o
 obj-$(CONFIG_COMEDI_ACL7225B)           += acl7225b.o
 obj-$(CONFIG_COMEDI_PCL711)             += pcl711.o
 obj-$(CONFIG_COMEDI_PCL724)             += pcl724.o
@@ -26,7 +25,6 @@ obj-$(CONFIG_COMEDI_PCM3724)		+= pcm3724.o
 obj-$(CONFIG_COMEDI_PCM3730)            += pcm3730.o
 obj-$(CONFIG_COMEDI_RTI800)             += rti800.o
 obj-$(CONFIG_COMEDI_RTI802)             += rti802.o
-obj-$(CONFIG_COMEDI_DAS08)              += das08.o
 obj-$(CONFIG_COMEDI_DAS16M1)            += das16m1.o
 obj-$(CONFIG_COMEDI_DAS16)              += das16.o
 obj-$(CONFIG_COMEDI_DAS800)             += das800.o
@@ -135,4 +133,6 @@ obj-$(CONFIG_COMEDI_NI_TIO)		+= ni_tio.o
 obj-$(CONFIG_COMEDI_NI_TIO)             += ni_tiocmd.o
 obj-$(CONFIG_COMEDI_NI_LABPC)           += ni_labpc.o
 
+obj-$(CONFIG_COMEDI_8255)               += 8255.o
+obj-$(CONFIG_COMEDI_DAS08)              += das08.o
 obj-$(CONFIG_COMEDI_FC)                 += comedi_fc.o
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.c b/drivers/staging/comedi/drivers/addi-data/addi_common.c
index 2c986413a81a..b18e81d8cf8a 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.c
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.c
@@ -68,6 +68,10 @@ You should also find the complete GPL in the COPYING file accompanying this sour
 #include "addi_common.h"
 #include "addi_amcc_s5933.h"
 
+#ifndef ADDIDATA_DRIVER_NAME
+#define ADDIDATA_DRIVER_NAME    "addi_common"
+#endif
+
 /* Update-0.7.57->0.7.68MODULE_AUTHOR("ADDI-DATA GmbH <info@addi-data.com>"); */
 /* Update-0.7.57->0.7.68MODULE_DESCRIPTION("Comedi ADDI-DATA module"); */
 /* Update-0.7.57->0.7.68MODULE_LICENSE("GPL"); */
@@ -2528,7 +2532,7 @@ static const struct addi_board boardtypes[] = {
 #define n_boardtypes (sizeof(boardtypes)/sizeof(struct addi_board))
 
 static struct comedi_driver driver_addi = {
-        .driver_name = "addi_common",
+        .driver_name = ADDIDATA_DRIVER_NAME,
         .module = THIS_MODULE,
         .attach = i_ADDI_Attach,
         .detach = i_ADDI_Detach,
@@ -2570,10 +2574,6 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
         struct pcilst_struct *card = NULL;
         unsigned char pci_bus, pci_slot, pci_func;
         int i_Dma = 0;
-        static char c_Identifier[150];
-
-        sprintf(c_Identifier, "Addi-Data GmbH Comedi %s",
-                this_board->pc_DriverName);
 
         ret = alloc_private(dev, sizeof(struct addi_private));
         if (ret < 0)
@@ -2583,7 +2583,7 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
                 v_pci_card_list_init(this_board->i_VendorId, 1);        /* 1 for displaying the list.. */
                 pci_list_builded = 1;
         }
-        /* printk("comedi%d: addi_common: board=%s",dev->minor,this_board->pc_DriverName); */
+        /* printk("comedi%d: "ADDIDATA_DRIVER_NAME": board=%s",dev->minor,this_board->pc_DriverName); */
 
         if ((this_board->i_Dma) && (it->options[2] == 0)) {
                 i_Dma = 1;
@@ -2648,7 +2648,7 @@ static int i_ADDI_Attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
         if (irq > 0) {
                 if (request_irq(irq, v_ADDI_Interrupt, IRQF_SHARED,
-                                c_Identifier, dev) < 0) {
+                                this_board->pc_DriverName, dev) < 0) {
                         printk(", unable to allocate IRQ %u, DISABLING IT",
                                 irq);
                         irq = 0;        /* Can't use IRQ */
diff --git a/drivers/staging/comedi/drivers/addi_apci_035.c b/drivers/staging/comedi/drivers/addi_apci_035.c
index da454e854c4c..6dfcbe803f2d 100644
--- a/drivers/staging/comedi/drivers/addi_apci_035.c
+++ b/drivers/staging/comedi/drivers/addi_apci_035.c
@@ -2,4 +2,6 @@
 
 #define ADDIDATA_WATCHDOG 2     /*  Or shold it be something else */
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_035"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_1032.c b/drivers/staging/comedi/drivers/addi_apci_1032.c
index fa2056e8aa0e..4722ec834f7b 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1032.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_1032 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_1032"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_1500.c b/drivers/staging/comedi/drivers/addi_apci_1500.c
index 7a5cae599ef8..db3dafdcf691 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1500.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1500.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_1500 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_1500"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_1516.c b/drivers/staging/comedi/drivers/addi_apci_1516.c
index 8d414844009f..f591baff6a0b 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1516.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1516.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_1516 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_1516"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_1564.c b/drivers/staging/comedi/drivers/addi_apci_1564.c
index 0351cdde1026..6f5c923ac226 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1564.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1564.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_1564 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_1564"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_16xx.c b/drivers/staging/comedi/drivers/addi_apci_16xx.c
index 506799041294..1d926add9e6d 100644
--- a/drivers/staging/comedi/drivers/addi_apci_16xx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_16xx.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_16XX 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_16xx"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_1710.c b/drivers/staging/comedi/drivers/addi_apci_1710.c
index c433445913dd..df6ba8ccf56f 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1710.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1710.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_1710 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_1710"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_2016.c b/drivers/staging/comedi/drivers/addi_apci_2016.c
index 271c47c8cad3..7266e412f0a6 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2016.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2016.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_2016 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_2016"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_2032.c b/drivers/staging/comedi/drivers/addi_apci_2032.c
index 5108ea2a3924..f67da94119e8 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2032.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_2032 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_2032"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_2200.c b/drivers/staging/comedi/drivers/addi_apci_2200.c
index e439f835cf4f..bc7f7d653503 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2200.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2200.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_2200 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_2200"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_3001.c b/drivers/staging/comedi/drivers/addi_apci_3001.c
index df97c305828b..d86c4209cb90 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3001.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3001.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_3001 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_3001"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_3120.c b/drivers/staging/comedi/drivers/addi_apci_3120.c
index 9183125ddde4..0b22cf10415d 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3120.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3120.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_3120 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_3120"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_3200.c b/drivers/staging/comedi/drivers/addi_apci_3200.c
index f25a70b3290b..159313997dcf 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3200.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3200.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_3200 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_3200"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_3300.c b/drivers/staging/comedi/drivers/addi_apci_3300.c
index 1ee4778ad45b..733c69abc43a 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3300.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3300.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_3300 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_3300"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_3501.c b/drivers/staging/comedi/drivers/addi_apci_3501.c
index 1049e20237e8..d8a01b154e35 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3501.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3501.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_3501 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_3501"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/addi_apci_3xxx.c b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
index fb9deb7083bd..942bc9e259a8 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3xxx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
@@ -1,3 +1,5 @@
 #define CONFIG_APCI_3XXX 1
 
+#define ADDIDATA_DRIVER_NAME    "addi_apci_3xxx"
+
 #include "addi-data/addi_common.c"
diff --git a/drivers/staging/comedi/drivers/adv_pci_dio.c b/drivers/staging/comedi/drivers/adv_pci_dio.c
index 40eeecf5347f..e424a0c7d34f 100644
--- a/drivers/staging/comedi/drivers/adv_pci_dio.c
+++ b/drivers/staging/comedi/drivers/adv_pci_dio.c
@@ -7,17 +7,17 @@
 */
 /*
 Driver: adv_pci_dio
-Description: Advantech PCI-1730, PCI-1733, PCI-1734, PCI-1736UP,
-             PCI-1750, PCI-1751, PCI-1752, PCI-1753/E, PCI-1754,
-             PCI-1756, PCI-1762
+Description: Advantech PCI-1730, PCI-1733, PCI-1734, PCI-1735U,
+             PCI-1736UP, PCI-1750, PCI-1751, PCI-1752, PCI-1753/E,
+             PCI-1754, PCI-1756, PCI-1762
 Author: Michal Dobes <dobes@tesnet.cz>
 Devices: [Advantech] PCI-1730 (adv_pci_dio), PCI-1733,
-  PCI-1734, PCI-1736UP, PCI-1750,
+  PCI-1734, PCI-1735U, PCI-1736UP, PCI-1750,
   PCI-1751, PCI-1752, PCI-1753,
   PCI-1753+PCI-1753E, PCI-1754, PCI-1756,
   PCI-1760, PCI-1762
 Status: untested
-Updated: Mon, 14 Apr 2008 10:43:08 +0100
+Updated: Tue, 04 May 2010 13:00:00 +0000
 
 This driver supports now only insn interface for DI/DO/DIO.
 
@@ -35,6 +35,7 @@ Configuration options:
 
 #include "comedi_pci.h"
 #include "8255.h"
+#include "8253.h"
 
 #undef PCI_DIO_EXTDEBUG         /* if defined, enable extensive debug logging */
 
@@ -49,7 +50,7 @@ Configuration options:
 
 /* hardware types of the cards */
 enum hw_cards_id {
-        TYPE_PCI1730, TYPE_PCI1733, TYPE_PCI1734, TYPE_PCI1736,
+        TYPE_PCI1730, TYPE_PCI1733, TYPE_PCI1734, TYPE_PCI1735, TYPE_PCI1736,
         TYPE_PCI1750,
         TYPE_PCI1751,
         TYPE_PCI1752,
@@ -67,7 +68,10 @@ enum hw_io_access {
 #define MAX_DI_SUBDEVS  2       /* max number of DI subdevices per card */
 #define MAX_DO_SUBDEVS  2       /* max number of DO subdevices per card */
 #define MAX_DIO_SUBDEVG 2       /* max number of DIO subdevices group per card */
+#define MAX_8254_SUBDEVS   1    /* max number of 8254 counter subdevs per card */
+                                /* (could be more than one 8254 per subdevice) */
 
+#define SIZE_8254          4    /* 8254 IO space length */
 #define SIZE_8255          4    /* 8255 IO space length */
 
 #define PCIDIO_MAINREG     2    /* main I/O region for all Advantech cards? */
@@ -85,6 +89,12 @@ enum hw_io_access {
 #define PCI1734_IDO        0    /* W:   Isolated digital output 0-31 */
 #define PCI173x_BOARDID    4    /* R:   Board I/D switch for 1730/3/4 */
 
+/* Advantech PCI-1735U */
+#define PCI1735_DI         0    /* R:   Digital input  0-31 */
+#define PCI1735_DO         0    /* W:   Digital output 0-31 */
+#define PCI1735_C8254      4    /* R/W: 8254 counter */
+#define PCI1735_BOARDID    8    /* R:   Board I/D switch for 1735U */
+
 /*  Advantech PCI-1736UP */
 #define PCI1736_IDI        0    /* R:   Isolated digital input  0-15 */
 #define PCI1736_IDO        0    /* W:   Isolated digital output 0-15 */
@@ -192,7 +202,8 @@ static int pci_dio_detach(struct comedi_device *dev);
 struct diosubd_data {
         int chans;              /*  num of chans */
         int addr;               /*  PCI address ofset */
-        int regs;               /*  number of registers to read or 8255 subdevices */
+        int regs;               /*  number of registers to read or 8255
+                                    subdevices or 8254 chips */
         unsigned int specflags; /*  addon subdevice flags */
 };
 
@@ -206,6 +217,7 @@ struct dio_boardtype {
         struct diosubd_data sdo[MAX_DO_SUBDEVS];        /*  DO chans */
         struct diosubd_data sdio[MAX_DIO_SUBDEVG];      /*  DIO 8255 chans */
         struct diosubd_data boardid;    /*  card supports board ID switch */
+        struct diosubd_data s8254[MAX_8254_SUBDEVS];    /* 8254 subdevices */
         enum hw_io_access io_access;
 };
 
@@ -214,6 +226,7 @@ static DEFINE_PCI_DEVICE_TABLE(pci_dio_pci_table) = {
         PCI_VENDOR_ID_ADVANTECH, 0x1730, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
         PCI_VENDOR_ID_ADVANTECH, 0x1733, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
         PCI_VENDOR_ID_ADVANTECH, 0x1734, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
+        PCI_VENDOR_ID_ADVANTECH, 0x1735, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
         PCI_VENDOR_ID_ADVANTECH, 0x1736, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
         PCI_VENDOR_ID_ADVANTECH, 0x1750, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
         PCI_VENDOR_ID_ADVANTECH, 0x1751, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0}, {
@@ -235,14 +248,15 @@ static const struct dio_boardtype boardtypes[] = {
          {{16, PCI1730_DO, 2, 0}, {16, PCI1730_IDO, 2, 0}},
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {4, PCI173x_BOARDID, 1, SDF_INTERNAL},
-         IO_8b,
-         },
+         {{0, 0, 0, 0}},
+         IO_8b},
         {"pci1733", PCI_VENDOR_ID_ADVANTECH, 0x1733, PCIDIO_MAINREG,
          TYPE_PCI1733,
          {{0, 0, 0, 0}, {32, PCI1733_IDI, 4, 0}},
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {4, PCI173x_BOARDID, 1, SDF_INTERNAL},
+         {{0, 0, 0, 0}},
          IO_8b},
         {"pci1734", PCI_VENDOR_ID_ADVANTECH, 0x1734, PCIDIO_MAINREG,
          TYPE_PCI1734,
@@ -250,6 +264,15 @@ static const struct dio_boardtype boardtypes[] = {
          {{0, 0, 0, 0}, {32, PCI1734_IDO, 4, 0}},
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {4, PCI173x_BOARDID, 1, SDF_INTERNAL},
+         {{0, 0, 0, 0}},
+         IO_8b},
+        {"pci1735", PCI_VENDOR_ID_ADVANTECH, 0x1735, PCIDIO_MAINREG,
+         TYPE_PCI1735,
+         {{32, PCI1735_DI, 4, 0}, {0, 0, 0, 0}},
+         {{32, PCI1735_DO, 4, 0}, {0, 0, 0, 0}},
+         {{0, 0, 0, 0}, {0, 0, 0, 0}},
+         { 4, PCI1735_BOARDID, 1, SDF_INTERNAL},
+         {{3, PCI1735_C8254, 1, 0}},
          IO_8b},
         {"pci1736", PCI_VENDOR_ID_ADVANTECH, 0x1736, PCI1736_MAINREG,
          TYPE_PCI1736,
@@ -257,14 +280,15 @@ static const struct dio_boardtype boardtypes[] = {
          {{0, 0, 0, 0}, {16, PCI1736_IDO, 2, 0}},
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {4, PCI1736_BOARDID, 1, SDF_INTERNAL},
-         IO_8b,
-         },
+         {{0, 0, 0, 0}},
+         IO_8b},
         {"pci1750", PCI_VENDOR_ID_ADVANTECH, 0x1750, PCIDIO_MAINREG,
          TYPE_PCI1750,
          {{0, 0, 0, 0}, {16, PCI1750_IDI, 2, 0}},
          {{0, 0, 0, 0}, {16, PCI1750_IDO, 2, 0}},
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {0, 0, 0, 0},
+         {{0, 0, 0, 0}},
          IO_8b},
         {"pci1751", PCI_VENDOR_ID_ADVANTECH, 0x1751, PCIDIO_MAINREG,
          TYPE_PCI1751,
@@ -272,6 +296,7 @@ static const struct dio_boardtype boardtypes[] = {
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {{48, PCI1751_DIO, 2, 0}, {0, 0, 0, 0}},
          {0, 0, 0, 0},
+         {{0, 0, 0, 0}},
          IO_8b},
         {"pci1752", PCI_VENDOR_ID_ADVANTECH, 0x1752, PCIDIO_MAINREG,
          TYPE_PCI1752,
@@ -279,6 +304,7 @@ static const struct dio_boardtype boardtypes[] = {
          {{32, PCI1752_IDO, 2, 0}, {32, PCI1752_IDO2, 2, 0}},
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {4, PCI175x_BOARDID, 1, SDF_INTERNAL},
+         {{0, 0, 0, 0}},
          IO_16b},
         {"pci1753", PCI_VENDOR_ID_ADVANTECH, 0x1753, PCIDIO_MAINREG,
          TYPE_PCI1753,
@@ -286,6 +312,7 @@ static const struct dio_boardtype boardtypes[] = {
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {{96, PCI1753_DIO, 4, 0}, {0, 0, 0, 0}},
          {0, 0, 0, 0},
+         {{0, 0, 0, 0}},
          IO_8b},
         {"pci1753e", PCI_VENDOR_ID_ADVANTECH, 0x1753, PCIDIO_MAINREG,
          TYPE_PCI1753E,
@@ -293,6 +320,7 @@ static const struct dio_boardtype boardtypes[] = {
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {{96, PCI1753_DIO, 4, 0}, {96, PCI1753E_DIO, 4, 0}},
          {0, 0, 0, 0},
+         {{0, 0, 0, 0}},
          IO_8b},
         {"pci1754", PCI_VENDOR_ID_ADVANTECH, 0x1754, PCIDIO_MAINREG,
          TYPE_PCI1754,
@@ -300,6 +328,7 @@ static const struct dio_boardtype boardtypes[] = {
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {4, PCI175x_BOARDID, 1, SDF_INTERNAL},
+         {{0, 0, 0, 0}},
          IO_16b},
         {"pci1756", PCI_VENDOR_ID_ADVANTECH, 0x1756, PCIDIO_MAINREG,
          TYPE_PCI1756,
@@ -307,6 +336,7 @@ static const struct dio_boardtype boardtypes[] = {
          {{0, 0, 0, 0}, {32, PCI1756_IDO, 2, 0}},
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {4, PCI175x_BOARDID, 1, SDF_INTERNAL},
+         {{0, 0, 0, 0}},
          IO_16b},
         {"pci1760", PCI_VENDOR_ID_ADVANTECH, 0x1760, 0,
          TYPE_PCI1760,
@@ -314,6 +344,7 @@ static const struct dio_boardtype boardtypes[] = {
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {0, 0, 0, 0},
+         {{0, 0, 0, 0}},
          IO_8b},
         {"pci1762", PCI_VENDOR_ID_ADVANTECH, 0x1762, PCIDIO_MAINREG,
          TYPE_PCI1762,
@@ -321,6 +352,7 @@ static const struct dio_boardtype boardtypes[] = {
          {{0, 0, 0, 0}, {16, PCI1762_RO, 1, 0}},
          {{0, 0, 0, 0}, {0, 0, 0, 0}},
          {4, PCI1762_BOARDID, 1, SDF_INTERNAL},
+         {{0, 0, 0, 0}},
          IO_16b}
 };
 
@@ -440,6 +472,83 @@ static int pci_dio_insn_bits_do_w(struct comedi_device *dev,
 /*
 ==============================================================================
 */
+static int pci_8254_insn_read(struct comedi_device *dev,
+                              struct comedi_subdevice *s,
+                              struct comedi_insn *insn, unsigned int *data)
+{
+        const struct diosubd_data *d = (const struct diosubd_data *)s->private;
+        unsigned int chan, chip, chipchan;
+        unsigned long flags;
+
+        chan = CR_CHAN(insn->chanspec); /* channel on subdevice */
+        chip = chan / 3;                /* chip on subdevice */
+        chipchan = chan - (3 * chip);   /* channel on chip on subdevice */
+        spin_lock_irqsave(&s->spin_lock, flags);
+        data[0] = i8254_read(dev->iobase + d->addr + (SIZE_8254 * chip),
+                        0, chipchan);
+        spin_unlock_irqrestore(&s->spin_lock, flags);
+        return 1;
+}
+
+/*
+==============================================================================
+*/
+static int pci_8254_insn_write(struct comedi_device *dev,
+                               struct comedi_subdevice *s,
+                               struct comedi_insn *insn, unsigned int *data)
+{
+        const struct diosubd_data *d = (const struct diosubd_data *)s->private;
+        unsigned int chan, chip, chipchan;
+        unsigned long flags;
+
+        chan = CR_CHAN(insn->chanspec); /* channel on subdevice */
+        chip = chan / 3;                /* chip on subdevice */
+        chipchan = chan - (3 * chip);   /* channel on chip on subdevice */
+        spin_lock_irqsave(&s->spin_lock, flags);
+        i8254_write(dev->iobase + d->addr + (SIZE_8254 * chip),
+                        0, chipchan, data[0]);
+        spin_unlock_irqrestore(&s->spin_lock, flags);
+        return 1;
+}
+
+/*
+==============================================================================
+*/
+static int pci_8254_insn_config(struct comedi_device *dev,
+                                struct comedi_subdevice *s,
+                                struct comedi_insn *insn, unsigned int *data)
+{
+        const struct diosubd_data *d = (const struct diosubd_data *)s->private;
+        unsigned int chan, chip, chipchan;
+        unsigned long iobase;
+        int ret = 0;
+        unsigned long flags;
+
+        chan = CR_CHAN(insn->chanspec); /* channel on subdevice */
+        chip = chan / 3;                /* chip on subdevice */
+        chipchan = chan - (3 * chip);   /* channel on chip on subdevice */
+        iobase = dev->iobase + d->addr + (SIZE_8254 * chip);
+        spin_lock_irqsave(&s->spin_lock, flags);
+        switch (data[0]) {
+        case INSN_CONFIG_SET_COUNTER_MODE:
+                ret = i8254_set_mode(iobase, 0, chipchan, data[1]);
+                if (ret < 0)
+                        ret = -EINVAL;
+                break;
+        case INSN_CONFIG_8254_READ_STATUS:
+                data[1] = i8254_status(iobase, 0, chipchan);
+                break;
+        default:
+                ret = -EINVAL;
+                break;
+        }
+        spin_unlock_irqrestore(&s->spin_lock, flags);
+        return ret < 0 ? ret : insn->n;
+}
+
+/*
+==============================================================================
+*/
 static int pci1760_unchecked_mbxrequest(struct comedi_device *dev,
                                         unsigned char *omb, unsigned char *imb,
                                         int repeats)
@@ -708,6 +817,15 @@ static int pci_dio_reset(struct comedi_device *dev)
                 outb(0, dev->iobase + PCI1734_IDO + 2);
                 outb(0, dev->iobase + PCI1734_IDO + 3);
                 break;
+        case TYPE_PCI1735:
+                outb(0, dev->iobase + PCI1735_DO);      /*  clear outputs */
+                outb(0, dev->iobase + PCI1735_DO + 1);
+                outb(0, dev->iobase + PCI1735_DO + 2);
+                outb(0, dev->iobase + PCI1735_DO + 3);
+                i8254_set_mode(dev->iobase + PCI1735_C8254, 0, 0, I8254_MODE0);
+                i8254_set_mode(dev->iobase + PCI1735_C8254, 0, 1, I8254_MODE0);
+                i8254_set_mode(dev->iobase + PCI1735_C8254, 0, 2, I8254_MODE0);
+                break;
 
         case TYPE_PCI1736:
                 outb(0, dev->iobase + PCI1736_IDO);
@@ -877,6 +995,26 @@ static int pci_dio_add_do(struct comedi_device *dev, struct comedi_subdevice *s,
 /*
 ==============================================================================
 */
+static int pci_dio_add_8254(struct comedi_device *dev,
+                            struct comedi_subdevice * s,
+                            const struct diosubd_data *d, int subdev)
+{
+        s->type = COMEDI_SUBD_COUNTER;
+        s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+        s->n_chan = d->chans;
+        s->maxdata = 65535;
+        s->len_chanlist = d->chans;
+        s->insn_read = pci_8254_insn_read;
+        s->insn_write = pci_8254_insn_write;
+        s->insn_config = pci_8254_insn_config;
+        s->private = (void *)d;
+
+        return 0;
+}
+
+/*
+==============================================================================
+*/
 static int CheckAndAllocCard(struct comedi_device *dev,
                              struct comedi_devconfig *it,
                              struct pci_dev *pcidev)
@@ -979,6 +1117,9 @@ static int pci_dio_attach(struct comedi_device *dev,
                         n_subdevices += this_board->sdio[i].regs;
                 if (this_board->boardid.chans)
                         n_subdevices++;
+                for (i = 0; i < MAX_8254_SUBDEVS; i++)
+                        if (this_board->s8254[i].chans)
+                                n_subdevices++;
         }
 
         ret = alloc_subdevices(dev, n_subdevices);
@@ -1022,6 +1163,13 @@ static int pci_dio_attach(struct comedi_device *dev,
                 subdev++;
         }
 
+        for (i = 0; i < MAX_8254_SUBDEVS; i++)
+                if (this_board->s8254[i].chans) {
+                        s = dev->subdevices + subdev;
+                        pci_dio_add_8254(dev, s, &this_board->s8254[i], subdev);
+                        subdev++;
+                }
+
         if (this_board->cardtype == TYPE_PCI1760)
                 pci1760_attach(dev, it);
 
@@ -1067,6 +1215,16 @@ static int pci_dio_detach(struct comedi_device *dev)
                         }
                 }
 
+                if (this_board->boardid.chans) {
+                        subdev++;
+                }
+
+                for (i = 0; i < MAX_8254_SUBDEVS; i++) {
+                        if (this_board->s8254[i].chans) {
+                                subdev++;
+                        }
+                }
+
                 for (i = 0; i < dev->n_subdevices; i++) {
                         s = dev->subdevices + i;
                         s->private = NULL;
diff --git a/drivers/staging/comedi/drivers/amplc_dio200.c b/drivers/staging/comedi/drivers/amplc_dio200.c
index 8eb67651486a..bf27617aa62d 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200.c
@@ -460,6 +460,7 @@ struct dio200_subdev_8254 {
         int has_clk_gat_sce;
         unsigned clock_src[3];  /* Current clock sources */
         unsigned gate_src[3];   /* Current gate sources */
+        spinlock_t spinlock;
 };
 
 struct dio200_subdev_intr {
@@ -1042,8 +1043,11 @@ dio200_subdev_8254_read(struct comedi_device *dev, struct comedi_subdevice *s,
 {
         struct dio200_subdev_8254 *subpriv = s->private;
         int chan = CR_CHAN(insn->chanspec);
+        unsigned long flags;
 
+        spin_lock_irqsave(&subpriv->spinlock, flags);
         data[0] = i8254_read(subpriv->iobase, 0, chan);
+        spin_unlock_irqrestore(&subpriv->spinlock, flags);
 
         return 1;
 }
@@ -1057,8 +1061,11 @@ dio200_subdev_8254_write(struct comedi_device *dev, struct comedi_subdevice *s,
 {
         struct dio200_subdev_8254 *subpriv = s->private;
         int chan = CR_CHAN(insn->chanspec);
+        unsigned long flags;
 
+        spin_lock_irqsave(&subpriv->spinlock, flags);
         i8254_write(subpriv->iobase, 0, chan, data[0]);
+        spin_unlock_irqrestore(&subpriv->spinlock, flags);
 
         return 1;
 }
@@ -1151,14 +1158,16 @@ dio200_subdev_8254_config(struct comedi_device *dev, struct comedi_subdevice *s,
                           struct comedi_insn *insn, unsigned int *data)
 {
         struct dio200_subdev_8254 *subpriv = s->private;
-        int ret;
+        int ret = 0;
         int chan = CR_CHAN(insn->chanspec);
+        unsigned long flags;
 
+        spin_lock_irqsave(&subpriv->spinlock, flags);
         switch (data[0]) {
         case INSN_CONFIG_SET_COUNTER_MODE:
                 ret = i8254_set_mode(subpriv->iobase, 0, chan, data[1]);
                 if (ret < 0)
-                        return -EINVAL;
+                        ret = -EINVAL;
                 break;
         case INSN_CONFIG_8254_READ_STATUS:
                 data[1] = i8254_status(subpriv->iobase, 0, chan);
@@ -1166,30 +1175,35 @@ dio200_subdev_8254_config(struct comedi_device *dev, struct comedi_subdevice *s,
         case INSN_CONFIG_SET_GATE_SRC:
                 ret = dio200_set_gate_src(subpriv, chan, data[2]);
                 if (ret < 0)
-                        return -EINVAL;
+                        ret = -EINVAL;
                 break;
         case INSN_CONFIG_GET_GATE_SRC:
                 ret = dio200_get_gate_src(subpriv, chan);
-                if (ret < 0)
-                        return -EINVAL;
+                if (ret < 0) {
+                        ret = -EINVAL;
+                        break;
+                }
                 data[2] = ret;
                 break;
         case INSN_CONFIG_SET_CLOCK_SRC:
                 ret = dio200_set_clock_src(subpriv, chan, data[1]);
                 if (ret < 0)
-                        return -EINVAL;
+                        ret = -EINVAL;
                 break;
         case INSN_CONFIG_GET_CLOCK_SRC:
                 ret = dio200_get_clock_src(subpriv, chan, &data[2]);
-                if (ret < 0)
-                        return -EINVAL;
+                if (ret < 0) {
+                        ret = -EINVAL;
+                        break;
+                }
                 data[1] = ret;
                 break;
         default:
-                return -EINVAL;
+                ret = -EINVAL;
                 break;
         }
-        return insn->n;
+        spin_unlock_irqrestore(&subpriv->spinlock, flags);
+        return ret < 0 ? ret : insn->n;
 }
 
 /*
@@ -1222,6 +1236,7 @@ dio200_subdev_8254_init(struct comedi_device *dev, struct comedi_subdevice *s,
         s->insn_write = dio200_subdev_8254_write;
         s->insn_config = dio200_subdev_8254_config;
 
+        spin_lock_init(&subpriv->spinlock);
         subpriv->iobase = offset + iobase;
         subpriv->has_clk_gat_sce = has_clk_gat_sce;
         if (has_clk_gat_sce) {
diff --git a/drivers/staging/comedi/drivers/ni_mio_cs.c b/drivers/staging/comedi/drivers/ni_mio_cs.c
index cedb02d40f95..3a46f0c0bff9 100644
--- a/drivers/staging/comedi/drivers/ni_mio_cs.c
+++ b/drivers/staging/comedi/drivers/ni_mio_cs.c
@@ -123,7 +123,7 @@ static const struct ni_board_struct ni_boards[] = {
          .adbits = 12,
          .ai_fifo_depth = 1024,
          .alwaysdither = 0,
-         .gainlkup = ai_gain_16,
+         .gainlkup = ai_gain_4,
          .ai_speed = 5000,
          .n_aochan = 2,
          .aobits = 12,
diff --git a/drivers/staging/comedi/drivers/usbdux.c b/drivers/staging/comedi/drivers/usbdux.c
index 86f035d00675..27b4cb2e2ec2 100644
--- a/drivers/staging/comedi/drivers/usbdux.c
+++ b/drivers/staging/comedi/drivers/usbdux.c
@@ -351,8 +351,7 @@ static int usbdux_ai_stop(struct usbduxsub *this_usbduxsub, int do_unlink)
         int ret = 0;
 
         if (!this_usbduxsub) {
-                dev_err(&this_usbduxsub->interface->dev,
-                        "comedi?: usbdux_ai_stop: this_usbduxsub=NULL!\n");
+                pr_err("comedi?: usbdux_ai_stop: this_usbduxsub=NULL!\n");
                 return -EFAULT;
         }
         dev_dbg(&this_usbduxsub->interface->dev, "comedi: usbdux_ai_stop\n");
diff --git a/drivers/staging/dream/synaptics_i2c_rmi.c b/drivers/staging/dream/synaptics_i2c_rmi.c
index 1f020dad6234..3320359408a9 100644
--- a/drivers/staging/dream/synaptics_i2c_rmi.c
+++ b/drivers/staging/dream/synaptics_i2c_rmi.c
@@ -519,7 +519,6 @@ err_input_register_device_failed:
 err_input_dev_alloc_failed:
 err_detect_failed:
 err_power_failed:
-        i2c_set_clientdata(client, NULL);
         kfree(ts);
 err_alloc_data_failed:
 err_check_functionality_failed:
@@ -537,7 +536,6 @@ static int synaptics_ts_remove(struct i2c_client *client)
         else
                 hrtimer_cancel(&ts->timer);
         input_unregister_device(ts->input_dev);
-        i2c_set_clientdata(client, NULL);
         kfree(ts);
         return 0;
 }
diff --git a/drivers/staging/dt3155/allocator.c b/drivers/staging/dt3155/allocator.c
index bd5adbc2a238..d33947b0378f 100644
--- a/drivers/staging/dt3155/allocator.c
+++ b/drivers/staging/dt3155/allocator.c
@@ -176,9 +176,7 @@ int allocator_free_dma(unsigned long address)
         prev = ptr; ptr = ptr->next;
 
         if (!ptr) {
-                printk(KERN_ERR ALL_MSG
-                        "free_dma(0x%08lx) but add. not allocated\n",
-                        ptr->address);
+                pr_err(ALL_MSG "free_dma but add. not allocated\n");
                 return -EINVAL;
         }
         PDEBUGG("freeing: %08lx (%li) next %08lx\n", ptr->address, ptr->size,
diff --git a/drivers/staging/go7007/saa7134-go7007.c b/drivers/staging/go7007/saa7134-go7007.c
index 49f0d31c118a..cf7c34a99459 100644
--- a/drivers/staging/go7007/saa7134-go7007.c
+++ b/drivers/staging/go7007/saa7134-go7007.c
@@ -242,13 +242,13 @@ static void saa7134_go7007_irq_ts_done(struct saa7134_dev *dev,
                 printk(KERN_DEBUG "saa7134-go7007: irq: lost %ld\n",
                                 (status >> 16) & 0x0f);
         if (status & 0x100000) {
-                dma_sync_single(&dev->pci->dev,
-                                saa->bottom_dma, PAGE_SIZE, DMA_FROM_DEVICE);
+                dma_sync_single_for_cpu(&dev->pci->dev,
+                                        saa->bottom_dma, PAGE_SIZE, DMA_FROM_DEVICE);
                 go7007_parse_video_stream(go, saa->bottom, PAGE_SIZE);
                 saa_writel(SAA7134_RS_BA2(5), cpu_to_le32(saa->bottom_dma));
         } else {
-                dma_sync_single(&dev->pci->dev,
-                                saa->top_dma, PAGE_SIZE, DMA_FROM_DEVICE);
+                dma_sync_single_for_cpu(&dev->pci->dev,
+                                        saa->top_dma, PAGE_SIZE, DMA_FROM_DEVICE);
                 go7007_parse_video_stream(go, saa->top, PAGE_SIZE);
                 saa_writel(SAA7134_RS_BA1(5), cpu_to_le32(saa->top_dma));
         }
diff --git a/drivers/staging/go7007/wis-saa7113.c b/drivers/staging/go7007/wis-saa7113.c
index bd925457f8b7..72f5c1f56d19 100644
--- a/drivers/staging/go7007/wis-saa7113.c
+++ b/drivers/staging/go7007/wis-saa7113.c
@@ -289,7 +289,6 @@ static int wis_saa7113_probe(struct i2c_client *client,
         if (write_regs(client, initial_registers) < 0) {
                 printk(KERN_ERR
                         "wis-saa7113: error initializing SAA7113\n");
-                i2c_set_clientdata(client, NULL);
                 kfree(dec);
                 return -ENODEV;
         }
@@ -301,7 +300,6 @@ static int wis_saa7113_remove(struct i2c_client *client)
 {
         struct wis_saa7113 *dec = i2c_get_clientdata(client);
 
-        i2c_set_clientdata(client, NULL);
         kfree(dec);
         return 0;
 }
diff --git a/drivers/staging/go7007/wis-saa7115.c b/drivers/staging/go7007/wis-saa7115.c
index b2eb804c1954..cd950b61cf70 100644
--- a/drivers/staging/go7007/wis-saa7115.c
+++ b/drivers/staging/go7007/wis-saa7115.c
@@ -422,7 +422,6 @@ static int wis_saa7115_probe(struct i2c_client *client,
         if (write_regs(client, initial_registers) < 0) {
                 printk(KERN_ERR
                         "wis-saa7115: error initializing SAA7115\n");
-                i2c_set_clientdata(client, NULL);
                 kfree(dec);
                 return -ENODEV;
         }
@@ -434,7 +433,6 @@ static int wis_saa7115_remove(struct i2c_client *client)
 {
         struct wis_saa7115 *dec = i2c_get_clientdata(client);
 
-        i2c_set_clientdata(client, NULL);
         kfree(dec);
         return 0;
 }
diff --git a/drivers/staging/go7007/wis-sony-tuner.c b/drivers/staging/go7007/wis-sony-tuner.c
index b1013291190f..981c9b311b8b 100644
--- a/drivers/staging/go7007/wis-sony-tuner.c
+++ b/drivers/staging/go7007/wis-sony-tuner.c
@@ -684,7 +684,6 @@ static int wis_sony_tuner_remove(struct i2c_client *client)
 {
         struct wis_sony_tuner *t = i2c_get_clientdata(client);
 
-        i2c_set_clientdata(client, NULL);
         kfree(t);
         return 0;
 }
diff --git a/drivers/staging/go7007/wis-tw2804.c b/drivers/staging/go7007/wis-tw2804.c
index 315268d130dd..ee28a99dc388 100644
--- a/drivers/staging/go7007/wis-tw2804.c
+++ b/drivers/staging/go7007/wis-tw2804.c
@@ -323,7 +323,6 @@ static int wis_tw2804_remove(struct i2c_client *client)
 {
         struct wis_tw2804 *dec = i2c_get_clientdata(client);
 
-        i2c_set_clientdata(client, NULL);
         kfree(dec);
         return 0;
 }
diff --git a/drivers/staging/go7007/wis-tw9903.c b/drivers/staging/go7007/wis-tw9903.c
index 2afea09091b9..80d47269b1c0 100644
--- a/drivers/staging/go7007/wis-tw9903.c
+++ b/drivers/staging/go7007/wis-tw9903.c
@@ -294,7 +294,6 @@ static int wis_tw9903_probe(struct i2c_client *client,
 
         if (write_regs(client, initial_registers) < 0) {
                 printk(KERN_ERR "wis-tw9903: error initializing TW9903\n");
-                i2c_set_clientdata(client, NULL);
                 kfree(dec);
                 return -ENODEV;
         }
@@ -306,7 +305,6 @@ static int wis_tw9903_remove(struct i2c_client *client)
 {
         struct wis_tw9903 *dec = i2c_get_clientdata(client);
 
-        i2c_set_clientdata(client, NULL);
         kfree(dec);
         return 0;
 }
diff --git a/drivers/staging/iio/Documentation/iio_utils.h b/drivers/staging/iio/Documentation/iio_utils.h
index a4555e6f133f..014f6684faba 100644
--- a/drivers/staging/iio/Documentation/iio_utils.h
+++ b/drivers/staging/iio/Documentation/iio_utils.h
@@ -62,9 +62,8 @@ inline int find_type_by_name(const char *name, const char *type)
                                         1) != 0) {
                                 filename = malloc(strlen(iio_dir)
                                                 + strlen(type)
-                                                + 1
                                                 + numstrlen
-                                                + 1);
+                                                + 6);
                                 if (filename == NULL)
                                         return -ENOMEM;
                                 sprintf(filename, "%s%s%d/name",
diff --git a/drivers/staging/iio/adc/max1363_core.c b/drivers/staging/iio/adc/max1363_core.c
index 20e267448d1f..905f8560d31f 100644
--- a/drivers/staging/iio/adc/max1363_core.c
+++ b/drivers/staging/iio/adc/max1363_core.c
@@ -1011,7 +1011,6 @@ error_put_reg:
         if (!IS_ERR(st->reg))
                 regulator_put(st->reg);
 error_free_st:
-        i2c_set_clientdata(client, NULL);
         kfree(st);
 
 error_ret:
@@ -1030,7 +1029,6 @@ static int max1363_remove(struct i2c_client *client)
                 regulator_disable(st->reg);
                 regulator_put(st->reg);
         }
-        i2c_set_clientdata(client, NULL);
         kfree(st);
 
         return 0;
diff --git a/drivers/staging/iio/light/tsl2563.c b/drivers/staging/iio/light/tsl2563.c
index 43aaacff4e74..e4b0a5ef1c1f 100644
--- a/drivers/staging/iio/light/tsl2563.c
+++ b/drivers/staging/iio/light/tsl2563.c
@@ -694,7 +694,6 @@ static int __devinit tsl2563_probe(struct i2c_client *client,
 fail2:
         iio_device_unregister(chip->indio_dev);
 fail1:
-        i2c_set_clientdata(client, NULL);
         kfree(chip);
         return err;
 }
@@ -705,7 +704,6 @@ static int tsl2563_remove(struct i2c_client *client)
 
         iio_device_unregister(chip->indio_dev);
 
-        i2c_set_clientdata(client, NULL);
         kfree(chip);
         return 0;
 }
diff --git a/drivers/staging/iio/ring_sw.c b/drivers/staging/iio/ring_sw.c
index 1f14cd4770e7..294272d0619f 100644
--- a/drivers/staging/iio/ring_sw.c
+++ b/drivers/staging/iio/ring_sw.c
@@ -20,7 +20,7 @@ static inline int __iio_allocate_sw_ring_buffer(struct iio_sw_ring_buffer *ring,
         if ((length == 0) || (bytes_per_datum == 0))
                 return -EINVAL;
         __iio_update_ring_buffer(&ring->buf, bytes_per_datum, length);
-        ring->data = kmalloc(length*ring->buf.bpd, GFP_KERNEL);
+        ring->data = kmalloc(length*ring->buf.bpd, GFP_ATOMIC);
         ring->read_p = NULL;
         ring->write_p = NULL;
         ring->last_written_p = NULL;
diff --git a/drivers/staging/mrst-touchscreen/Kconfig b/drivers/staging/mrst-touchscreen/Kconfig
new file mode 100644
index 000000000000..c2af49217084
--- /dev/null
+++ b/drivers/staging/mrst-touchscreen/Kconfig
@@ -0,0 +1,7 @@
+config TOUCHSCREEN_INTEL_MID
+        tristate "Intel MID platform resistive touchscreen"
+        depends on INTEL_SCU_IPC
+        default y
+        help
+          Say Y here if you have a Intel MID based touchscreen
+          If unsure, say N.
diff --git a/drivers/staging/mrst-touchscreen/Makefile b/drivers/staging/mrst-touchscreen/Makefile
new file mode 100644
index 000000000000..2d638b0d70bf
--- /dev/null
+++ b/drivers/staging/mrst-touchscreen/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_TOUCHSCREEN_INTEL_MID) := intel_mid_touch.o
+
+
diff --git a/drivers/staging/mrst-touchscreen/TODO b/drivers/staging/mrst-touchscreen/TODO
new file mode 100644
index 000000000000..7157028d634a
--- /dev/null
+++ b/drivers/staging/mrst-touchscreen/TODO
@@ -0,0 +1,2 @@
+- Move the driver to not think it is SPI (requires fixing some of the SFI
+  and firmware side)
diff --git a/drivers/staging/mrst-touchscreen/intel-mid-touch.c b/drivers/staging/mrst-touchscreen/intel-mid-touch.c
new file mode 100644
index 000000000000..1db00975a594
--- /dev/null
+++ b/drivers/staging/mrst-touchscreen/intel-mid-touch.c
@@ -0,0 +1,864 @@
+/*
+ * intel_mid_touch.c - Intel MID Resistive Touch Screen Driver
+ *
+ * Copyright (C) 2008 Intel Corp
+ *
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *
+ * 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; ifnot, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ *
+ * Questions/Comments/Bug fixes to Sreedhara (sreedhara.ds@intel.com)
+ *                          Ramesh Agarwal (ramesh.agarwal@intel.com)
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *
+ * TODO:
+ *      kill off mrstouch_debug eventually
+ *      review conversion of r/m/w sequences
+ *      Replace interrupt mutex abuse
+ *      Kill of mrstouchdevp pointer
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/err.h>
+#include <linux/param.h>
+#include <linux/spi/spi.h>
+#include <linux/irq.h>
+#include <linux/delay.h>
+#include <linux/kthread.h>
+#include <asm/intel_scu_ipc.h>
+
+
+#if defined(MRSTOUCH_DEBUG)
+#define mrstouch_debug(fmt, args...)\
+        do { \
+                printk(KERN_DEBUG "\n[MRSTOUCH(%d)] - ", __LINE__); \
+                printk(KERN_DEBUG fmt, ##args); \
+        } while (0);
+#else
+#define mrstouch_debug(fmt, args...)
+#endif
+
+/* PMIC Interrupt registers */
+#define PMIC_REG_ID1   0x00 /*PMIC ID1 register */
+
+/* PMIC Interrupt registers */
+#define PMIC_REG_INT   0x04 /*PMIC interrupt register */
+#define PMIC_REG_MINT  0x05 /*PMIC interrupt mask register */
+
+/* ADC Interrupt registers */
+#define PMIC_REG_ADCINT   0x5F /*ADC interrupt register */
+#define PMIC_REG_MADCINT  0x60 /*ADC interrupt mask register */
+
+/* ADC Control registers */
+#define PMIC_REG_ADCCNTL1    0x61 /*ADC control register */
+
+/* ADC Channel Selection registers */
+#define PMICADDR0     0xA4
+#define END_OF_CHANNEL 0x1F
+
+/* ADC Result register */
+#define PMIC_REG_ADCSNS0H   0x64
+
+/* ADC channels for touch screen */
+#define MRST_TS_CHAN10   0xA /* Touch screen X+ connection */
+#define MRST_TS_CHAN11   0xB /* Touch screen X- connection */
+#define MRST_TS_CHAN12   0xC /* Touch screen Y+ connection */
+#define MRST_TS_CHAN13   0xD /* Touch screen Y- connection */
+
+/* Touch screen coordinate constants */
+#define TOUCH_PRESSURE          50
+#define TOUCH_PRESSURE_FS       100
+
+#define XMOVE_LIMIT     5
+#define YMOVE_LIMIT     5
+#define XYMOVE_CNT      3
+
+#define MAX_10BIT       ((1<<10)-1)
+
+/* Touch screen channel BIAS constants */
+#define XBIAS           0x20
+#define YBIAS           0x40
+#define ZBIAS           0x80
+
+/* Touch screen coordinates */
+#define MIN_X           10
+#define MAX_X           1024
+#define MIN_Y           10
+#define MAX_Y           1024
+#define WAIT_ADC_COMPLETION 10
+
+/* PMIC ADC round robin delays */
+#define ADC_LOOP_DELAY0 0x0 /* Continuous loop */
+#define ADC_LOOP_DELAY1 0x1 /* 4.5  ms approximate */
+
+/* PMIC Vendor Identifiers */
+#define PMIC_VENDOR_FS  0 /* PMIC vendor FreeScale */
+#define PMIC_VENDOR_MAXIM 1 /* PMIC vendor MAXIM */
+#define PMIC_VENDOR_NEC 2 /* PMIC vendor NEC */
+#define MRSTOUCH_MAX_CHANNELS 32 /* Maximum ADC channels */
+
+/* Touch screen device structure */
+struct mrstouch_dev {
+        struct spi_device *spi; /* SPI device associated with touch screen */
+        struct input_dev *input; /* input device for touchscreen*/
+        char            phys[32]; /* Device name */
+        struct task_struct *pendet_thrd; /* PENDET interrupt handler */
+        struct mutex lock; /* Sync between interrupt and PENDET handler */
+        bool            busy; /* Busy flag */
+        u16             asr; /* Address selection register */
+        int             irq;    /* Touch screen IRQ # */
+        uint            vendor;  /* PMIC vendor */
+        uint            rev;  /* PMIC revision */
+        bool            suspended; /* Device suspended status */
+        bool            disabled;  /* Device disabled status */
+        u16             x;  /* X coordinate */
+        u16             y;  /* Y coordinate */
+        bool            pendown;  /* PEN position */
+} ;
+
+
+/* Global Pointer to Touch screen device */
+static struct mrstouch_dev *mrstouchdevp;
+
+/* Utility to read PMIC ID */
+static int mrstouch_pmic_id(uint *vendor, uint *rev)
+{
+        int err;
+        u8 r;
+
+        err = intel_scu_ipc_ioread8(PMIC_REG_ID1, &r);
+        if (err)
+                return err;
+
+        *vendor = r & 0x7;
+        *rev = (r >> 3) & 0x7;
+
+        return 0;
+}
+
+/*
+ * Parse ADC channels to find end of the channel configured by other ADC user
+ * NEC and MAXIM requires 4 channels and FreeScale needs 18 channels
+ */
+static int mrstouch_chan_parse(struct mrstouch_dev *tsdev)
+{
+        int err, i, j, found;
+        u32 r32;
+
+        found = -1;
+
+        for (i = 0; i < MRSTOUCH_MAX_CHANNELS; i++) {
+                if (found >= 0)
+                        break;
+
+                err = intel_scu_ipc_ioread32(PMICADDR0, &r32);
+                if (err)
+                        return err;
+
+                for (j = 0; j < 32; j+= 8) {
+                        if (((r32 >> j) & 0xFF) == END_OF_CHANNEL) {
+                                found = i;
+                                break;
+                        }
+                }
+        }
+        if (found < 0)
+                return 0;
+
+        if (tsdev->vendor == PMIC_VENDOR_FS) {
+                if (found && found > (MRSTOUCH_MAX_CHANNELS - 18))
+                        return -ENOSPC;
+        } else {
+                if (found && found > (MRSTOUCH_MAX_CHANNELS - 4))
+                        return -ENOSPC;
+        }
+        return found;
+}
+
+/* Utility to enable/disable pendet.
+ * pendet set to true enables PENDET interrupt
+ * pendet set to false disables PENDET interrupt
+ * Also clears RND mask bit
+*/
+static int pendet_enable(struct mrstouch_dev *tsdev, bool pendet)
+{
+        u16 reg;
+        u8 r;
+        u8 pendet_enabled = 0;
+        int retry = 0;
+        int err;
+
+        err = intel_scu_ipc_ioread16(PMIC_REG_MADCINT, &reg);
+        if (err)
+                return err;
+
+        if (pendet) {
+                reg &= ~0x0005;
+                reg |= 0x2000; /* Enable pendet */
+        } else
+                reg &= 0xDFFF; /* Disable pendet */
+
+        /* Set MADCINT and update ADCCNTL1 (next reg byte) */
+        err = intel_scu_ipc_iowrite16(PMIC_REG_MADCINT, reg);
+        if (!pendet || err)
+                return err;
+
+        /*
+         * Sometimes even after the register write succeeds
+         * the PMIC register value is not updated. Retry few iterations
+         * to enable pendet.
+         */
+
+        err = intel_scu_ipc_ioread8(PMIC_REG_ADCCNTL1, &r);
+        pendet_enabled = (r >> 5) & 0x01;
+
+        retry = 0;
+        while (!err && !pendet_enabled) {
+                retry++;
+                msleep(10);
+                err = intel_scu_ipc_iowrite8(PMIC_REG_ADCCNTL1, reg >> 8);
+                if (err)
+                        break;
+                err = intel_scu_ipc_ioread8(PMIC_REG_ADCCNTL1, &r);
+                if (err == 0)
+                        pendet_enabled = (r >> 5) & 0x01;
+                if (retry >= 10) {
+                        dev_err(&tsdev->spi->dev, "Touch screen disabled.\n");
+                        return -EIO;
+                }
+        }
+        return 0;
+}
+
+/* To read PMIC ADC touch screen result
+ * Reads ADC storage registers for higher 7 and lower 3 bits
+ * converts the two readings to single value and turns off gain bit
+ */
+static int mrstouch_ts_chan_read(u16 offset, u16 chan, u16 *vp, u16 *vm)
+{
+        int err;
+        u16 result;
+        u32 res;
+
+        result = PMIC_REG_ADCSNS0H + offset;
+
+        if (chan == MRST_TS_CHAN12)
+                result += 4;
+
+        err = intel_scu_ipc_ioread32(result, &res);
+        if (err)
+                return err;
+
+        /* Mash the bits up */
+
+        *vp = (res & 0xFF) << 3;        /* Highest 7 bits */
+        *vp |= (res >> 8) & 0x07;       /* Lower 3 bits */
+        *vp &= 0x3FF;
+
+        res >>= 16;
+
+        *vm = (res & 0xFF) << 3;        /* Highest 7 bits */
+        *vm |= (res >> 8) & 0x07;       /* Lower 3 bits */
+        *vm &= 0x3FF;
+
+        return 0;
+}
+
+/* To configure touch screen channels
+ * Writes touch screen channels to ADC address selection registers
+ */
+static int mrstouch_ts_chan_set(uint offset)
+{
+        int count;
+        u16 chan;
+        u16 reg[5];
+        u8 data[5];
+
+        chan = PMICADDR0 + offset;
+        for (count = 0; count <= 3; count++) {
+                reg[count] = chan++;
+                data[count] = MRST_TS_CHAN10 + count;
+        }
+        reg[count] = chan;
+        data[count] = END_OF_CHANNEL;
+
+        return intel_scu_ipc_writev(reg, data, 5);
+}
+
+/* Initialize ADC */
+static int mrstouch_adc_init(struct mrstouch_dev *tsdev)
+{
+        int err, start;
+        u8 ra, rm;
+
+        err = mrstouch_pmic_id(&tsdev->vendor, &tsdev->rev);
+        if (err) {
+                dev_err(&tsdev->spi->dev, "Unable to read PMIC id\n");
+                return err;
+        }
+
+        start = mrstouch_chan_parse(tsdev);
+        if (start < 0) {
+                dev_err(&tsdev->spi->dev, "Unable to parse channels\n");
+                return start;
+        }
+
+        tsdev->asr = start;
+
+        mrstouch_debug("Channel offset(%d): 0x%X\n", tsdev->asr, tsdev->vendor);
+
+        /* ADC power on, start, enable PENDET and set loop delay
+         * ADC loop delay is set to 4.5 ms approximately
+         * Loop delay more than this results in jitter in adc readings
+         * Setting loop delay to 0 (continous loop) in MAXIM stops PENDET
+         * interrupt generation sometimes.
+         */
+
+        if (tsdev->vendor == PMIC_VENDOR_FS) {
+                ra = 0xE0 | ADC_LOOP_DELAY0;
+                rm = 0x5;
+        } else {
+                /* NEC and MAXIm not consistent with loop delay 0 */
+                ra = 0xE0 | ADC_LOOP_DELAY1;
+                rm = 0x0;
+
+                /* configure touch screen channels */
+                err = mrstouch_ts_chan_set(tsdev->asr);
+                if (err)
+                        return err;
+        }
+        err = intel_scu_ipc_update_register(PMIC_REG_ADCCNTL1, ra, 0xE7);
+        if (err == 0)
+                err = intel_scu_ipc_update_register(PMIC_REG_MADCINT, rm, 0x03);
+        return err;
+}
+
+/* Reports x,y coordinates to event subsystem */
+static void mrstouch_report_xy(struct mrstouch_dev *tsdev, u16 x, u16 y, u16 z)
+{
+        int xdiff, ydiff;
+
+        if (tsdev->pendown && z <= TOUCH_PRESSURE) {
+                /* Pen removed, report button release */
+                mrstouch_debug("BTN REL(%d)", z);
+                input_report_key(tsdev->input, BTN_TOUCH, 0);
+                tsdev->pendown = false;
+        }
+
+        xdiff = abs(x - tsdev->x);
+        ydiff = abs(y - tsdev->y);
+
+        /*
+        if x and y values changes for XYMOVE_CNT readings it is considered
+        as stylus is moving. This is required to differentiate between stylus
+        movement and jitter
+        */
+        if (x < MIN_X || x > MAX_X || y < MIN_Y || y > MAX_Y) {
+                /* Spurious values, release button if touched and return */
+                if (tsdev->pendown) {
+                        mrstouch_debug("BTN REL(%d)", z);
+                        input_report_key(tsdev->input, BTN_TOUCH, 0);
+                        tsdev->pendown = false;
+                }
+                return;
+        } else if (xdiff >= XMOVE_LIMIT || ydiff >= YMOVE_LIMIT) {
+                tsdev->x = x;
+                tsdev->y = y;
+
+                input_report_abs(tsdev->input, ABS_X, x);
+                input_report_abs(tsdev->input, ABS_Y, y);
+                input_sync(tsdev->input);
+        }
+
+
+        if (!tsdev->pendown && z > TOUCH_PRESSURE) {
+                /* Pen touched, report button touch */
+                mrstouch_debug("BTN TCH(%d, %d, %d)", x, y, z);
+                input_report_key(tsdev->input, BTN_TOUCH, 1);
+                tsdev->pendown = true;
+        }
+}
+
+
+/* Utility to start ADC, used by freescale handler */
+static int pendet_mask(void)
+{
+        return  intel_scu_ipc_update_register(PMIC_REG_MADCINT, 0x02, 0x02);
+}
+
+/* Utility to stop ADC, used by freescale handler */
+static int pendet_umask(void)
+{
+        return  intel_scu_ipc_update_register(PMIC_REG_MADCINT, 0x00, 0x02);
+}
+
+/* Utility to read ADC, used by freescale handler */
+static int mrstouch_pmic_fs_adc_read(struct mrstouch_dev *tsdev)
+{
+        int err;
+        u16 x, y, z, result;
+        u16 reg[4];
+        u8 data[4];
+
+        result = PMIC_REG_ADCSNS0H + tsdev->asr;
+
+        reg[0] = result + 4;
+        reg[1] = result + 5;
+        reg[2] = result + 16;
+        reg[3] = result + 17;
+
+        err = intel_scu_ipc_readv(reg, data, 4);
+        if (err)
+                goto ipc_error;
+
+        x = data[0] << 3; /* Higher 7 bits */
+        x |= data[1] & 0x7; /* Lower 3 bits */
+        x &= 0x3FF;
+
+        y = data[2] << 3; /* Higher 7 bits */
+        y |= data[3] & 0x7; /* Lower 3 bits */
+        y &= 0x3FF;
+
+        /* Read Z value */
+        reg[0] = result + 28;
+        reg[1] = result + 29;
+
+        err = intel_scu_ipc_readv(reg, data, 4);
+        if (err)
+                goto ipc_error;
+
+        z = data[0] << 3; /* Higher 7 bits */
+        z |= data[1] & 0x7; /* Lower 3 bits */
+        z &= 0x3FF;
+
+#if defined(MRSTOUCH_PRINT_XYZP)
+        mrstouch_debug("X: %d, Y: %d, Z: %d", x, y, z);
+#endif
+
+        if (z >= TOUCH_PRESSURE_FS) {
+                mrstouch_report_xy(tsdev, x, y, TOUCH_PRESSURE - 1); /* Pen Removed */
+                return TOUCH_PRESSURE - 1;
+        } else {
+                mrstouch_report_xy(tsdev, x, y, TOUCH_PRESSURE + 1); /* Pen Touched */
+                return TOUCH_PRESSURE + 1;
+        }
+
+        return 0;
+
+ipc_error:
+        dev_err(&tsdev->spi->dev, "ipc error during fs_adc read\n");
+        return err;
+}
+
+/* To handle free scale pmic pendet interrupt */
+static int pmic0_pendet(void *dev_id)
+{
+        int err, count;
+        u16 chan;
+        unsigned int touched;
+        struct mrstouch_dev *tsdev = (struct mrstouch_dev *)dev_id;
+        u16 reg[5];
+        u8 data[5];
+
+        chan = PMICADDR0 + tsdev->asr;
+
+        /* Set X BIAS */
+        for (count = 0; count <= 3; count++) {
+                reg[count] = chan++;
+                data[count] = 0x2A;
+        }
+        reg[count] =  chan++; /* Dummy */
+        data[count] = 0;
+
+        err = intel_scu_ipc_writev(reg, data, 5);
+        if (err)
+                goto ipc_error;
+
+        msleep(WAIT_ADC_COMPLETION);
+
+        /* Set Y BIAS */
+        for (count = 0; count <= 3; count++) {
+                reg[count] = chan++;
+                data[count] = 0x4A;
+        }
+        reg[count] = chan++; /* Dummy */
+        data[count] = 0;
+
+        err = intel_scu_ipc_writev(reg, data, 5);
+        if (err)
+                goto ipc_error;
+
+        msleep(WAIT_ADC_COMPLETION);
+
+        /* Set Z BIAS */
+        err = intel_scu_ipc_iowrite32(chan + 2, 0x8A8A8A8A);
+        if (err)
+                goto ipc_error;
+
+        msleep(WAIT_ADC_COMPLETION);
+
+        /*Read touch screen channels till pen removed
+         * Freescale reports constant value of z for all points
+         * z is high when screen is not touched and low when touched
+         * Map high z value to not touched and low z value to pen touched
+         */
+        touched = mrstouch_pmic_fs_adc_read(tsdev);
+        while (touched > TOUCH_PRESSURE) {
+                touched = mrstouch_pmic_fs_adc_read(tsdev);
+                msleep(WAIT_ADC_COMPLETION);
+        }
+
+        /* Clear all TS channels */
+        chan = PMICADDR0 + tsdev->asr;
+        for (count = 0; count <= 4; count++) {
+                reg[count] = chan++;
+                data[count] = 0;
+        }
+        err = intel_scu_ipc_writev(reg, data, 5);
+        if (err)
+                goto ipc_error;
+
+        for (count = 0; count <= 4; count++) {
+                reg[count] = chan++;
+                data[count] = 0;
+        }
+        err = intel_scu_ipc_writev(reg, data, 5);
+        if (err)
+                goto ipc_error;
+
+        err = intel_scu_ipc_iowrite32(chan + 2, 0x00000000);
+        if (err)
+                goto ipc_error;
+
+        return 0;
+
+ipc_error:
+        dev_err(&tsdev->spi->dev, "ipc error during pendet\n");
+        return err;
+}
+
+
+/* To enable X, Y and Z bias values
+ * Enables YPYM for X channels and XPXM for Y channels
+ */
+static int mrstouch_ts_bias_set(uint offset, uint bias)
+{
+        int count;
+        u16 chan, start;
+        u16 reg[4];
+        u8 data[4];
+
+        chan = PMICADDR0 + offset;
+        start = MRST_TS_CHAN10;
+
+        for (count = 0; count <= 3; count++) {
+                reg[count] = chan++;
+                data[count] = bias | (start + count);
+        }
+        return intel_scu_ipc_writev(reg, data, 4);
+}
+
+/* To read touch screen channel values */
+static int mrstouch_adc_read(struct mrstouch_dev *tsdev)
+{
+        int err;
+        u16 xp, xm, yp, ym, zp, zm;
+
+        /* configure Y bias for X channels */
+        err = mrstouch_ts_bias_set(tsdev->asr, YBIAS);
+        if (err)
+                goto ipc_error;
+
+        msleep(WAIT_ADC_COMPLETION);
+
+        /* read x+ and x- channels */
+        err = mrstouch_ts_chan_read(tsdev->asr, MRST_TS_CHAN10, &xp, &xm);
+        if (err)
+                goto ipc_error;
+
+        /* configure x bias for y channels */
+        err = mrstouch_ts_bias_set(tsdev->asr, XBIAS);
+        if (err)
+                goto ipc_error;
+
+        msleep(WAIT_ADC_COMPLETION);
+
+        /* read y+ and y- channels */
+        err = mrstouch_ts_chan_read(tsdev->asr, MRST_TS_CHAN12, &yp, &ym);
+        if (err)
+                goto ipc_error;
+
+        /* configure z bias for x and y channels */
+        err = mrstouch_ts_bias_set(tsdev->asr, ZBIAS);
+        if (err)
+                goto ipc_error;
+
+        msleep(WAIT_ADC_COMPLETION);
+
+        /* read z+ and z- channels */
+        err = mrstouch_ts_chan_read(tsdev->asr, MRST_TS_CHAN10, &zp, &zm);
+        if (err)
+                goto ipc_error;
+
+#if defined(MRSTOUCH_PRINT_XYZP)
+        printk(KERN_INFO "X+: %d, Y+: %d, Z+: %d\n", xp, yp, zp);
+#endif
+
+#if defined(MRSTOUCH_PRINT_XYZM)
+        printk(KERN_INFO "X-: %d, Y-: %d, Z-: %d\n", xm, ym, zm);
+#endif
+
+        mrstouch_report_xy(tsdev, xp, yp, zp); /* report x and y to eventX */
+
+        return zp;
+
+ipc_error:
+        dev_err(&tsdev->spi->dev, "ipc error during adc read\n");
+        return err;
+}
+
+/* PENDET interrupt handler function for NEC and MAXIM */
+static void pmic12_pendet(void *data)
+{
+        unsigned int touched;
+        struct mrstouch_dev *tsdev = (struct mrstouch_dev *)data;
+
+        /* read touch screen channels till pen removed */
+        do {
+                touched = mrstouch_adc_read(tsdev);
+        } while (touched > TOUCH_PRESSURE);
+}
+
+/* Handler to process PENDET interrupt */
+int mrstouch_pendet(void *data)
+{
+        struct mrstouch_dev *tsdev = (struct mrstouch_dev *)data;
+        while (1) {
+                /* Wait for PENDET interrupt */
+                if (mutex_lock_interruptible(&tsdev->lock)) {
+                        msleep(WAIT_ADC_COMPLETION);
+                        continue;
+                }
+
+                if (tsdev->busy)
+                        return 0;
+
+                tsdev->busy = true;
+
+                if (tsdev->vendor == PMIC_VENDOR_NEC ||
+                        tsdev->vendor == PMIC_VENDOR_MAXIM) {
+                        /* PENDET must be disabled in NEC before reading ADC */
+                        pendet_enable(tsdev,false); /* Disbale PENDET */
+                        pmic12_pendet(tsdev);
+                        pendet_enable(tsdev, true); /*Enable PENDET */
+                } else if (tsdev->vendor == PMIC_VENDOR_FS) {
+                        pendet_umask(); /* Stop ADC */
+                        pmic0_pendet(tsdev);
+                        pendet_mask(); /* Stop ADC */
+                } else
+                dev_err(&tsdev->spi->dev, "Unsupported touchscreen: %d\n",
+                                tsdev->vendor);
+
+                tsdev->busy = false;
+
+        }
+        return 0;
+}
+
+/* PENDET interrupt handler */
+static irqreturn_t pendet_intr_handler(int irq, void *handle)
+{
+        struct mrstouch_dev *tsdev = (struct mrstouch_dev *)handle;
+
+        mutex_unlock(&tsdev->lock);
+        return IRQ_HANDLED;
+}
+
+/* Intializes input device and registers with input subsystem */
+static int ts_input_dev_init(struct mrstouch_dev *tsdev, struct spi_device *spi)
+{
+        int err = 0;
+
+        mrstouch_debug("%s", __func__);
+
+        tsdev->input = input_allocate_device();
+        if (!tsdev->input) {
+                dev_err(&tsdev->spi->dev, "Unable to allocate input device.\n");
+                return -EINVAL;
+        }
+
+        tsdev->input->name = "mrst_touchscreen";
+        snprintf(tsdev->phys, sizeof(tsdev->phys),
+                        "%s/input0", dev_name(&spi->dev));
+        tsdev->input->phys = tsdev->phys;
+        tsdev->input->dev.parent = &spi->dev;
+
+        tsdev->input->id.vendor = tsdev->vendor;
+        tsdev->input->id.version = tsdev->rev;
+
+        tsdev->input->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+        tsdev->input->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
+
+        input_set_abs_params(tsdev->input, ABS_X, MIN_X, MIN_Y, 0, 0);
+        input_set_abs_params(tsdev->input, ABS_Y, MIN_X, MIN_Y, 0, 0);
+
+        err = input_register_device(tsdev->input);
+        if (err) {
+                dev_err(&tsdev->spi->dev, "unable to register input device\n");
+                input_free_device(tsdev->input);
+                return err;
+        }
+
+        mrstouch_debug("%s", "mrstouch initialized");
+
+        return 0;
+
+}
+
+/* Probe function for touch screen driver */
+static int __devinit mrstouch_probe(struct spi_device *mrstouch_spi)
+{
+        int err;
+        unsigned int myirq;
+        struct mrstouch_dev *tsdev;
+
+        mrstouch_debug("%s(%p)", __func__, mrstouch_spi);
+
+        mrstouchdevp = NULL;
+        myirq = mrstouch_spi->irq;
+
+        if (!mrstouch_spi->irq) {
+                dev_err(&mrstouch_spi->dev, "no interrupt assigned\n");
+                return -EINVAL;
+        }
+
+        tsdev = kzalloc(sizeof(struct mrstouch_dev), GFP_KERNEL);
+        if (!tsdev) {
+                dev_err(&mrstouch_spi->dev, "unable to allocate memory\n");
+                return -ENOMEM;
+        }
+
+        tsdev->irq = myirq;
+        mrstouchdevp = tsdev;
+
+        err = mrstouch_adc_init(tsdev);
+        if (err) {
+                dev_err(&mrstouch_spi->dev, "ADC init failed\n");
+                goto mrstouch_err_free_mem;
+        }
+
+        dev_set_drvdata(&mrstouch_spi->dev, tsdev);
+        tsdev->spi = mrstouch_spi;
+
+        err = ts_input_dev_init(tsdev, mrstouch_spi);
+        if (err) {
+                dev_err(&tsdev->spi->dev, "ts_input_dev_init failed");
+                goto mrstouch_err_free_mem;
+        }
+
+        mutex_init(&tsdev->lock);
+        mutex_lock(&tsdev->lock)
+
+        mrstouch_debug("Requesting IRQ-%d", myirq);
+        err = request_irq(myirq, pendet_intr_handler,
+                                0, "mrstouch", tsdev);
+        if (err) {
+                dev_err(&tsdev->spi->dev, "unable to allocate irq\n");
+                goto mrstouch_err_free_mem;
+        }
+
+        tsdev->pendet_thrd = kthread_run(mrstouch_pendet,
+                                (void *)tsdev, "pendet handler");
+        if (IS_ERR(tsdev->pendet_thrd)) {
+                dev_err(&tsdev->spi->dev, "kthread_run failed\n");
+                err = PTR_ERR(tsdev->pendet_thrd);
+                goto mrstouch_err_free_mem;
+        }
+        mrstouch_debug("%s", "Driver initialized");
+        return 0;
+
+mrstouch_err_free_mem:
+        kfree(tsdev);
+        return err;
+}
+
+static int mrstouch_suspend(struct spi_device *spi, pm_message_t msg)
+{
+        mrstouch_debug("%s", __func__);
+        mrstouchdevp->suspended = 1;
+        return 0;
+}
+
+static int mrstouch_resume(struct spi_device *spi)
+{
+        mrstouch_debug("%s", __func__);
+        mrstouchdevp->suspended = 0;
+        return 0;
+}
+
+static int mrstouch_remove(struct spi_device *spi)
+{
+        mrstouch_debug("%s", __func__);
+        free_irq(mrstouchdevp->irq, mrstouchdevp);
+        input_unregister_device(mrstouchdevp->input);
+        input_free_device(mrstouchdevp->input);
+        kfree(mrstouchdevp);
+        if (mrstouchdevp->pendet_thrd)
+                kthread_stop(mrstouchdevp->pendet_thrd);
+        return 0;
+}
+
+static struct spi_driver mrstouch_driver = {
+        .driver = {
+                .name   = "pmic_touch",
+                .bus    = &spi_bus_type,
+                .owner  = THIS_MODULE,
+        },
+        .probe          = mrstouch_probe,
+        .suspend        = mrstouch_suspend,
+        .resume         = mrstouch_resume,
+        .remove         = mrstouch_remove,
+};
+
+static int __init mrstouch_module_init(void)
+{
+        int err;
+
+        mrstouch_debug("%s", __func__);
+        err = spi_register_driver(&mrstouch_driver);
+        if (err) {
+                mrstouch_debug("%s(%d)", "SPI PENDET failed", err);
+                return -1;
+        }
+
+        return 0;
+}
+
+static void __exit mrstouch_module_exit(void)
+{
+        mrstouch_debug("%s", __func__);
+        spi_unregister_driver(&mrstouch_driver);
+        return;
+}
+
+module_init(mrstouch_module_init);
+module_exit(mrstouch_module_exit);
+
+MODULE_AUTHOR("Sreedhara Murthy. D.S, sreedhara.ds@intel.com");
+MODULE_DESCRIPTION("Intel Moorestown Resistive Touch Screen Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/msm/Kconfig b/drivers/staging/msm/Kconfig
new file mode 100644
index 000000000000..c57039f2060b
--- /dev/null
+++ b/drivers/staging/msm/Kconfig
@@ -0,0 +1,134 @@
+config MSM_STAGING
+        tristate "MSM Frame Buffer Support"
+        depends on FB && ARCH_MSM && !FB_MSM
+        select FB_BACKLIGHT if FB_MSM_BACKLIGHT
+        select NEW_LEDS
+        select LEDS_CLASS
+        select FB_CFB_FILLRECT
+        select FB_CFB_COPYAREA
+        select FB_CFB_IMAGEBLIT
+        ---help---
+          Support for MSM Framebuffer.
+
+if MSM_STAGING
+
+config FB_MSM_LCDC_HW
+        bool
+        default n
+
+choice
+        prompt "MDP HW version"
+        default FB_MSM_MDP31
+
+config FB_MSM_MDP31
+        select FB_MSM_LCDC_HW
+        bool "MDP HW ver3.1"
+        ---help---
+          Support for MSM MDP HW revision 3.1
+          Say Y here if this is msm8x50 variant platform.
+endchoice
+
+config FB_MSM_LCDC
+        bool
+        default n
+
+config FB_MSM_TVOUT
+        bool
+        default n
+
+config FB_MSM_LCDC_PANEL
+        bool
+        select FB_MSM_LCDC
+        default n
+
+config FB_MSM_LCDC_PRISM_WVGA
+        bool
+        select FB_MSM_LCDC_PANEL
+        default n
+
+config FB_MSM_LCDC_ST1_WXGA
+        bool
+        select FB_MSM_LCDC_PANEL
+        default n
+
+config FB_MSM_LCDC_ST15_WXGA
+        bool
+        select FB_MSM_LCDC_PANEL
+        default n
+
+config FB_MSM_LCDC_WXGA
+        bool
+        select FB_MSM_LCDC_PANEL
+        default n
+
+choice
+        prompt "LCD Panel"
+        default FB_MSM_LCDC_ST15_PANEL
+
+config FB_MSM_LCDC_PRISM_WVGA_PANEL
+        depends on FB_MSM_LCDC_HW
+        bool "LCDC Prism WVGA Panel"
+        select FB_MSM_LCDC_PRISM_WVGA
+        ---help---
+          Support for LCDC Prism WVGA (800x480) panel
+
+
+config FB_MSM_LCDC_ST15_PANEL
+        depends on FB_MSM_LCDC_HW
+        bool "LCDC ST1.5 Panel"
+        select FB_MSM_LCDC_ST15_WXGA
+        ---help---
+          Support for ST1.5 WXGA (1366x768) panel
+
+config FB_MSM_PANEL_NONE
+        bool "NONE"
+        ---help---
+          This will disable LCD panel
+endchoice
+
+choice
+        prompt "Secondary LCD Panel"
+        depends on  FB_MSM_MDP31
+        default FB_MSM_SECONDARY_PANEL_NONE
+
+config FB_MSM_SECONDARY_PANEL_NONE
+        bool "NONE"
+        ---help---
+          No secondary panel
+endchoice
+
+config FB_MSM_TVOUT_NTSC
+        bool
+        select FB_MSM_TVOUT
+        default n
+
+config FB_MSM_TVOUT_PAL
+        bool
+        select FB_MSM_TVOUT
+        default n
+
+choice
+        depends on  (FB_MSM_MDP22 || FB_MSM_MDP31)
+        prompt "TVOut Region"
+        default FB_MSM_TVOUT_NTSC_M
+
+config FB_MSM_TVOUT_NTSC_M
+        bool "NTSC M"
+        select FB_MSM_TVOUT_NTSC
+        ---help---
+          Support for NTSC M region (North American and Korea)
+
+config FB_MSM_TVOUT_NONE
+        bool "NONE"
+        ---help---
+          This will disable TV Out functionality.
+endchoice
+
+config PMEM_KERNEL_SIZE
+        int "PMEM for kernel components (in MB)"
+        default 2
+        depends on ARCH_QSD8X50
+        help
+          Configures the amount of PMEM for use by kernel components
+          (in MB; minimum 2MB)
+endif
diff --git a/drivers/staging/msm/Makefile b/drivers/staging/msm/Makefile
new file mode 100644
index 000000000000..98a0ce177cb2
--- /dev/null
+++ b/drivers/staging/msm/Makefile
@@ -0,0 +1,93 @@
+obj-y := msm_fb.o staging-devices.o memory.o
+
+obj-$(CONFIG_FB_MSM_LOGO) += logo.o
+obj-$(CONFIG_FB_BACKLIGHT) += msm_fb_bl.o
+
+# MDP
+obj-y += mdp.o
+
+ifeq ($(CONFIG_FB_MSM_MDP40),y)
+obj-y += mdp4_util.o
+obj-$(CONFIG_DEBUG_FS) += mdp4_debugfs.o
+else
+obj-y += mdp_hw_init.o
+obj-y += mdp_ppp.o
+ifeq ($(CONFIG_FB_MSM_MDP31),y)
+obj-y += mdp_ppp_v31.o
+obj-$(CONFIG_MDP_PPP_ASYNC_OP) += mdp_ppp_dq.o
+else
+obj-y += mdp_ppp_v20.o
+endif
+endif
+
+ifeq ($(CONFIG_FB_MSM_OVERLAY),y)
+obj-y += mdp4_overlay.o
+obj-y += mdp4_overlay_lcdc.o
+obj-y += mdp4_overlay_mddi.o
+else
+obj-y += mdp_dma_lcdc.o
+endif
+
+obj-y += mdp_dma.o
+obj-y += mdp_dma_s.o
+obj-y += mdp_vsync.o
+obj-y += mdp_cursor.o
+obj-y += mdp_dma_tv.o
+
+# EBI2
+obj-$(CONFIG_FB_MSM_EBI2) += ebi2_lcd.o
+
+# LCDC
+obj-$(CONFIG_FB_MSM_LCDC) += lcdc.o
+
+# MDDI
+msm_mddi-objs := mddi.o mddihost.o mddihosti.o
+obj-$(CONFIG_FB_MSM_MDDI) += msm_mddi.o
+
+# External MDDI
+msm_mddi_ext-objs := mddihost_e.o mddi_ext.o
+obj-$(CONFIG_FB_MSM_EXTMDDI) += msm_mddi_ext.o
+
+# TVEnc
+obj-$(CONFIG_FB_MSM_TVOUT) += tvenc.o
+
+# MSM FB Panel
+obj-y += msm_fb_panel.o
+obj-$(CONFIG_FB_MSM_EBI2_TMD_QVGA_EPSON_QCIF) += ebi2_tmd20.o
+obj-$(CONFIG_FB_MSM_EBI2_TMD_QVGA_EPSON_QCIF) += ebi2_l2f.o
+
+ifeq ($(CONFIG_FB_MSM_MDDI_AUTO_DETECT),y)
+obj-y += mddi_prism.o
+obj-y += mddi_toshiba.o
+obj-y += mddi_toshiba_vga.o
+obj-y += mddi_toshiba_wvga_pt.o
+obj-y += mddi_toshiba_wvga.o
+obj-y += mddi_sharp.o
+else
+obj-$(CONFIG_FB_MSM_MDDI_PRISM_WVGA) += mddi_prism.o
+obj-$(CONFIG_FB_MSM_MDDI_TOSHIBA_COMMON) += mddi_toshiba.o
+obj-$(CONFIG_FB_MSM_MDDI_TOSHIBA_COMMON_VGA) += mddi_toshiba_vga.o
+obj-$(CONFIG_FB_MSM_MDDI_TOSHIBA_WVGA_PORTRAIT) += mddi_toshiba_wvga_pt.o
+obj-$(CONFIG_FB_MSM_MDDI_TOSHIBA_WVGA) += mddi_toshiba_wvga.o
+obj-$(CONFIG_FB_MSM_MDDI_SHARP_QVGA_128x128) += mddi_sharp.o
+endif
+
+obj-$(CONFIG_FB_MSM_LCDC_PANEL) += lcdc_panel.o
+obj-$(CONFIG_FB_MSM_LCDC_PRISM_WVGA) += lcdc_prism.o
+obj-$(CONFIG_FB_MSM_LCDC_EXTERNAL_WXGA) += lcdc_external.o
+obj-$(CONFIG_FB_MSM_LCDC_GORDON_VGA) += lcdc_gordon.o
+obj-$(CONFIG_FB_MSM_LCDC_WXGA) += lcdc_wxga.o
+obj-$(CONFIG_FB_MSM_LCDC_TOSHIBA_WVGA_PT) += lcdc_toshiba_wvga_pt.o
+obj-$(CONFIG_FB_MSM_LCDC_SHARP_WVGA_PT) += lcdc_sharp_wvga_pt.o
+obj-$(CONFIG_FB_MSM_LCDC_GRAPEFRUIT_VGA) += lcdc_grapefruit.o
+obj-$(CONFIG_FB_MSM_LCDC_ST1_WXGA) += lcdc_st1_wxga.o
+obj-$(CONFIG_FB_MSM_LCDC_ST15_WXGA) += lcdc_st15.o
+obj-$(CONFIG_FB_MSM_HDMI_SII_EXTERNAL_720P) += hdmi_sii9022.o
+
+obj-$(CONFIG_FB_MSM_TVOUT_NTSC) += tv_ntsc.o
+obj-$(CONFIG_FB_MSM_TVOUT_PAL) += tv_pal.o
+
+obj-$(CONFIG_FB_MSM_EXTMDDI_SVGA) += mddi_ext_lcd.o
+
+clean:
+        rm *.o .*cmd
diff --git a/drivers/staging/msm/TODO b/drivers/staging/msm/TODO
new file mode 100644
index 000000000000..05107a7d516a
--- /dev/null
+++ b/drivers/staging/msm/TODO
@@ -0,0 +1,3 @@
+- Merge this code with the existing MSM framebuffer
+- General style clean ups.
+
diff --git a/drivers/staging/msm/ebi2_l2f.c b/drivers/staging/msm/ebi2_l2f.c
new file mode 100644
index 000000000000..eea891d8f0f8
--- /dev/null
+++ b/drivers/staging/msm/ebi2_l2f.c
@@ -0,0 +1,569 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+
+#include <linux/memory.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include "linux/proc_fs.h"
+
+#include <linux/delay.h>
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+
+/* The following are for MSM5100 on Gator
+*/
+#ifdef FEATURE_PM1000
+#include "pm1000.h"
+#endif /* FEATURE_PM1000 */
+/* The following are for MSM6050 on Bambi
+*/
+#ifdef FEATURE_PMIC_LCDKBD_LED_DRIVER
+#include "pm.h"
+#endif /* FEATURE_PMIC_LCDKBD_LED_DRIVER */
+
+#ifdef DISP_DEVICE_18BPP
+#undef DISP_DEVICE_18BPP
+#define DISP_DEVICE_16BPP
+#endif
+
+#define QCIF_WIDTH        176
+#define QCIF_HEIGHT       220
+
+static void *DISP_CMD_PORT;
+static void *DISP_DATA_PORT;
+
+#define DISP_CMD_DISON    0xaf
+#define DISP_CMD_DISOFF   0xae
+#define DISP_CMD_DISNOR   0xa6
+#define DISP_CMD_DISINV   0xa7
+#define DISP_CMD_DISCTL   0xca
+#define DISP_CMD_GCP64    0xcb
+#define DISP_CMD_GCP16    0xcc
+#define DISP_CMD_GSSET    0xcd
+#define DISP_GS_2       0x02
+#define DISP_GS_16      0x01
+#define DISP_GS_64      0x00
+#define DISP_CMD_SLPIN    0x95
+#define DISP_CMD_SLPOUT   0x94
+#define DISP_CMD_SD_PSET  0x75
+#define DISP_CMD_MD_PSET  0x76
+#define DISP_CMD_SD_CSET  0x15
+#define DISP_CMD_MD_CSET  0x16
+#define DISP_CMD_DATCTL   0xbc
+#define DISP_DATCTL_666 0x08
+#define DISP_DATCTL_565 0x28
+#define DISP_DATCTL_444 0x38
+#define DISP_CMD_RAMWR    0x5c
+#define DISP_CMD_RAMRD    0x5d
+#define DISP_CMD_PTLIN    0xa8
+#define DISP_CMD_PTLOUT   0xa9
+#define DISP_CMD_ASCSET   0xaa
+#define DISP_CMD_SCSTART  0xab
+#define DISP_CMD_VOLCTL   0xc6
+#define DISP_VOLCTL_TONE 0x80
+#define DISP_CMD_NOp      0x25
+#define DISP_CMD_OSSEL    0xd0
+#define DISP_CMD_3500KSET 0xd1
+#define DISP_CMD_3500KEND 0xd2
+#define DISP_CMD_14MSET   0xd3
+#define DISP_CMD_14MEND   0xd4
+
+#define DISP_CMD_OUT(cmd) outpw(DISP_CMD_PORT, cmd);
+
+#define DISP_DATA_OUT(data) outpw(DISP_DATA_PORT, data);
+
+#define DISP_DATA_IN() inpw(DISP_DATA_PORT);
+
+/* Epson device column number starts at 2
+*/
+#define DISP_SET_RECT(ulhc_row, lrhc_row, ulhc_col, lrhc_col) \
+          DISP_CMD_OUT(DISP_CMD_SD_PSET) \
+          DISP_DATA_OUT((ulhc_row) & 0xFF) \
+          DISP_DATA_OUT((ulhc_row) >> 8) \
+          DISP_DATA_OUT((lrhc_row) & 0xFF) \
+          DISP_DATA_OUT((lrhc_row) >> 8) \
+          DISP_CMD_OUT(DISP_CMD_SD_CSET) \
+          DISP_DATA_OUT(((ulhc_col)+2) & 0xFF) \
+          DISP_DATA_OUT(((ulhc_col)+2) >> 8) \
+          DISP_DATA_OUT(((lrhc_col)+2) & 0xFF) \
+          DISP_DATA_OUT(((lrhc_col)+2) >> 8)
+
+#define DISP_MIN_CONTRAST      0
+#define DISP_MAX_CONTRAST      127
+#define DISP_DEFAULT_CONTRAST  80
+
+#define DISP_MIN_BACKLIGHT     0
+#define DISP_MAX_BACKLIGHT     15
+#define DISP_DEFAULT_BACKLIGHT 2
+
+#define WAIT_SEC(sec) mdelay((sec)/1000)
+
+static word disp_area_start_row;
+static word disp_area_end_row;
+static byte disp_contrast = DISP_DEFAULT_CONTRAST;
+static boolean disp_powered_up;
+static boolean disp_initialized = FALSE;
+/* For some reason the contrast set at init time is not good. Need to do
+ * it again
+ */
+static boolean display_on = FALSE;
+static void epsonQcif_disp_init(struct platform_device *pdev);
+static void epsonQcif_disp_set_contrast(word contrast);
+static void epsonQcif_disp_set_display_area(word start_row, word end_row);
+static int epsonQcif_disp_off(struct platform_device *pdev);
+static int epsonQcif_disp_on(struct platform_device *pdev);
+static void epsonQcif_disp_set_rect(int x, int y, int xres, int yres);
+
+volatile word databack;
+static void epsonQcif_disp_init(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+
+        int i;
+
+        if (disp_initialized)
+                return;
+
+        mfd = platform_get_drvdata(pdev);
+
+        DISP_CMD_PORT = mfd->cmd_port;
+        DISP_DATA_PORT = mfd->data_port;
+
+        /* Sleep in */
+        DISP_CMD_OUT(DISP_CMD_SLPIN);
+
+        /* Display off */
+        DISP_CMD_OUT(DISP_CMD_DISOFF);
+
+        /* Display normal */
+        DISP_CMD_OUT(DISP_CMD_DISNOR);
+
+        /* Set data mode */
+        DISP_CMD_OUT(DISP_CMD_DATCTL);
+        DISP_DATA_OUT(DISP_DATCTL_565);
+
+        /* Set display timing */
+        DISP_CMD_OUT(DISP_CMD_DISCTL);
+        DISP_DATA_OUT(0x1c);    /* p1 */
+        DISP_DATA_OUT(0x02);    /* p1 */
+        DISP_DATA_OUT(0x82);    /* p2 */
+        DISP_DATA_OUT(0x00);    /* p3 */
+        DISP_DATA_OUT(0x00);    /* p4 */
+        DISP_DATA_OUT(0xe0);    /* p5 */
+        DISP_DATA_OUT(0x00);    /* p5 */
+        DISP_DATA_OUT(0xdc);    /* p6 */
+        DISP_DATA_OUT(0x00);    /* p6 */
+        DISP_DATA_OUT(0x02);    /* p7 */
+        DISP_DATA_OUT(0x00);    /* p8 */
+
+        /* Set 64 gray scale level */
+        DISP_CMD_OUT(DISP_CMD_GCP64);
+        DISP_DATA_OUT(0x08);    /* p01 */
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT(0x2a);    /* p02 */
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT(0x4e);    /* p03 */
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT(0x6b);    /* p04 */
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT(0x88);    /* p05 */
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT(0xa3);    /* p06 */
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT(0xba);    /* p07 */
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT(0xd1);    /* p08 */
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT(0xe5);    /* p09 */
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT(0xf3);    /* p10 */
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT(0x03);    /* p11 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x13);    /* p12 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x22);    /* p13 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x2f);    /* p14 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x3b);    /* p15 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x46);    /* p16 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x51);    /* p17 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x5b);    /* p18 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x64);    /* p19 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x6c);    /* p20 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x74);    /* p21 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x7c);    /* p22 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x83);    /* p23 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x8a);    /* p24 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x91);    /* p25 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x98);    /* p26 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x9f);    /* p27 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xa6);    /* p28 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xac);    /* p29 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xb2);    /* p30 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xb7);    /* p31 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xbc);    /* p32 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xc1);    /* p33 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xc6);    /* p34 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xcb);    /* p35 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xd0);    /* p36 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xd4);    /* p37 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xd8);    /* p38 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xdc);    /* p39 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xe0);    /* p40 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xe4);    /* p41 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xe8);    /* p42 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xec);    /* p43 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xf0);    /* p44 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xf4);    /* p45 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xf8);    /* p46 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xfb);    /* p47 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xfe);    /* p48 */
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0x01);    /* p49 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x03);    /* p50 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x05);    /* p51 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x07);    /* p52 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x09);    /* p53 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x0b);    /* p54 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x0d);    /* p55 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x0f);    /* p56 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x11);    /* p57 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x13);    /* p58 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x15);    /* p59 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x17);    /* p60 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x19);    /* p61 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x1b);    /* p62 */
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x1c);    /* p63 */
+        DISP_DATA_OUT(0x02);
+
+        /* Set 16 gray scale level */
+        DISP_CMD_OUT(DISP_CMD_GCP16);
+        DISP_DATA_OUT(0x1a);    /* p01 */
+        DISP_DATA_OUT(0x32);    /* p02 */
+        DISP_DATA_OUT(0x42);    /* p03 */
+        DISP_DATA_OUT(0x4c);    /* p04 */
+        DISP_DATA_OUT(0x58);    /* p05 */
+        DISP_DATA_OUT(0x5f);    /* p06 */
+        DISP_DATA_OUT(0x66);    /* p07 */
+        DISP_DATA_OUT(0x6b);    /* p08 */
+        DISP_DATA_OUT(0x70);    /* p09 */
+        DISP_DATA_OUT(0x74);    /* p10 */
+        DISP_DATA_OUT(0x78);    /* p11 */
+        DISP_DATA_OUT(0x7b);    /* p12 */
+        DISP_DATA_OUT(0x7e);    /* p13 */
+        DISP_DATA_OUT(0x80);    /* p14 */
+        DISP_DATA_OUT(0x82);    /* p15 */
+
+        /* Set DSP column */
+        DISP_CMD_OUT(DISP_CMD_MD_CSET);
+        DISP_DATA_OUT(0xff);
+        DISP_DATA_OUT(0x03);
+        DISP_DATA_OUT(0xff);
+        DISP_DATA_OUT(0x03);
+
+        /* Set DSP page */
+        DISP_CMD_OUT(DISP_CMD_MD_PSET);
+        DISP_DATA_OUT(0xff);
+        DISP_DATA_OUT(0x01);
+        DISP_DATA_OUT(0xff);
+        DISP_DATA_OUT(0x01);
+
+        /* Set ARM column */
+        DISP_CMD_OUT(DISP_CMD_SD_CSET);
+        DISP_DATA_OUT(0x02);
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT((QCIF_WIDTH + 1) & 0xFF);
+        DISP_DATA_OUT((QCIF_WIDTH + 1) >> 8);
+
+        /* Set ARM page */
+        DISP_CMD_OUT(DISP_CMD_SD_PSET);
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT(0x00);
+        DISP_DATA_OUT((QCIF_HEIGHT - 1) & 0xFF);
+        DISP_DATA_OUT((QCIF_HEIGHT - 1) >> 8);
+
+        /* Set 64 gray scales */
+        DISP_CMD_OUT(DISP_CMD_GSSET);
+        DISP_DATA_OUT(DISP_GS_64);
+
+        DISP_CMD_OUT(DISP_CMD_OSSEL);
+        DISP_DATA_OUT(0);
+
+        /* Sleep out */
+        DISP_CMD_OUT(DISP_CMD_SLPOUT);
+
+        WAIT_SEC(40000);
+
+        /* Initialize power IC */
+        DISP_CMD_OUT(DISP_CMD_VOLCTL);
+        DISP_DATA_OUT(DISP_VOLCTL_TONE);
+
+        WAIT_SEC(40000);
+
+        /* Set electronic volume, d'xx */
+        DISP_CMD_OUT(DISP_CMD_VOLCTL);
+        DISP_DATA_OUT(DISP_DEFAULT_CONTRAST);   /* value from 0 to 127 */
+
+        /* Initialize display data */
+        DISP_SET_RECT(0, (QCIF_HEIGHT - 1), 0, (QCIF_WIDTH - 1));
+        DISP_CMD_OUT(DISP_CMD_RAMWR);
+        for (i = 0; i < QCIF_HEIGHT * QCIF_WIDTH; i++)
+                DISP_DATA_OUT(0xffff);
+
+        DISP_CMD_OUT(DISP_CMD_RAMRD);
+        databack = DISP_DATA_IN();
+        databack = DISP_DATA_IN();
+        databack = DISP_DATA_IN();
+        databack = DISP_DATA_IN();
+
+        WAIT_SEC(80000);
+
+        DISP_CMD_OUT(DISP_CMD_DISON);
+
+        disp_area_start_row = 0;
+        disp_area_end_row = QCIF_HEIGHT - 1;
+        disp_powered_up = TRUE;
+        disp_initialized = TRUE;
+        epsonQcif_disp_set_display_area(0, QCIF_HEIGHT - 1);
+        display_on = TRUE;
+}
+
+static void epsonQcif_disp_set_rect(int x, int y, int xres, int yres)
+{
+        if (!disp_initialized)
+                return;
+
+        DISP_SET_RECT(y, y + yres - 1, x, x + xres - 1);
+        DISP_CMD_OUT(DISP_CMD_RAMWR);
+}
+
+static void epsonQcif_disp_set_display_area(word start_row, word end_row)
+{
+        if (!disp_initialized)
+                return;
+
+        if ((start_row == disp_area_start_row)
+            && (end_row == disp_area_end_row))
+                return;
+        disp_area_start_row = start_row;
+        disp_area_end_row = end_row;
+
+        /* Range checking
+         */
+        if (end_row >= QCIF_HEIGHT)
+                end_row = QCIF_HEIGHT - 1;
+        if (start_row > end_row)
+                start_row = end_row;
+
+        /* When display is not the full screen, gray scale is set to
+         ** 2; otherwise it is set to 64.
+         */
+        if ((start_row == 0) && (end_row == (QCIF_HEIGHT - 1))) {
+                /* The whole screen */
+                DISP_CMD_OUT(DISP_CMD_PTLOUT);
+                WAIT_SEC(10000);
+                DISP_CMD_OUT(DISP_CMD_DISOFF);
+                WAIT_SEC(100000);
+                DISP_CMD_OUT(DISP_CMD_GSSET);
+                DISP_DATA_OUT(DISP_GS_64);
+                WAIT_SEC(100000);
+                DISP_CMD_OUT(DISP_CMD_DISON);
+        } else {
+                /* partial screen */
+                DISP_CMD_OUT(DISP_CMD_PTLIN);
+                DISP_DATA_OUT(start_row);
+                DISP_DATA_OUT(start_row >> 8);
+                DISP_DATA_OUT(end_row);
+                DISP_DATA_OUT(end_row >> 8);
+                DISP_CMD_OUT(DISP_CMD_GSSET);
+                DISP_DATA_OUT(DISP_GS_2);
+        }
+}
+
+static int epsonQcif_disp_off(struct platform_device *pdev)
+{
+        if (!disp_initialized)
+                epsonQcif_disp_init(pdev);
+
+        if (display_on) {
+                DISP_CMD_OUT(DISP_CMD_DISOFF);
+                DISP_CMD_OUT(DISP_CMD_SLPIN);
+                display_on = FALSE;
+        }
+
+        return 0;
+}
+
+static int epsonQcif_disp_on(struct platform_device *pdev)
+{
+        if (!disp_initialized)
+                epsonQcif_disp_init(pdev);
+
+        if (!display_on) {
+                DISP_CMD_OUT(DISP_CMD_SLPOUT);
+                WAIT_SEC(40000);
+                DISP_CMD_OUT(DISP_CMD_DISON);
+                epsonQcif_disp_set_contrast(disp_contrast);
+                display_on = TRUE;
+        }
+
+        return 0;
+}
+
+static void epsonQcif_disp_set_contrast(word contrast)
+{
+        if (!disp_initialized)
+                return;
+
+        /* Initialize power IC, d'24 */
+        DISP_CMD_OUT(DISP_CMD_VOLCTL);
+        DISP_DATA_OUT(DISP_VOLCTL_TONE);
+
+        WAIT_SEC(40000);
+
+        /* Set electronic volume, d'xx */
+        DISP_CMD_OUT(DISP_CMD_VOLCTL);
+        if (contrast > 127)
+                contrast = 127;
+        DISP_DATA_OUT(contrast);        /* value from 0 to 127 */
+        disp_contrast = (byte) contrast;
+}                               /* End disp_set_contrast */
+
+static void epsonQcif_disp_clear_screen_area(
+        word start_row, word end_row, word start_column, word end_column) {
+        int32 i;
+
+        /* Clear the display screen */
+        DISP_SET_RECT(start_row, end_row, start_column, end_column);
+        DISP_CMD_OUT(DISP_CMD_RAMWR);
+        i = (end_row - start_row + 1) * (end_column - start_column + 1);
+        for (; i > 0; i--)
+                DISP_DATA_OUT(0xffff);
+}
+
+static int __init epsonQcif_probe(struct platform_device *pdev)
+{
+        msm_fb_add_device(pdev);
+
+        return 0;
+}
+
+static struct platform_driver this_driver = {
+        .probe  = epsonQcif_probe,
+        .driver = {
+                .name   = "ebi2_epson_qcif",
+        },
+};
+
+static struct msm_fb_panel_data epsonQcif_panel_data = {
+        .on = epsonQcif_disp_on,
+        .off = epsonQcif_disp_off,
+        .set_rect = epsonQcif_disp_set_rect,
+};
+
+static struct platform_device this_device = {
+        .name   = "ebi2_epson_qcif",
+        .id     = 0,
+        .dev    = {
+                .platform_data = &epsonQcif_panel_data,
+        }
+};
+
+static int __init epsonQcif_init(void)
+{
+        int ret;
+        struct msm_panel_info *pinfo;
+
+        ret = platform_driver_register(&this_driver);
+        if (!ret) {
+                pinfo = &epsonQcif_panel_data.panel_info;
+                pinfo->xres = QCIF_WIDTH;
+                pinfo->yres = QCIF_HEIGHT;
+                pinfo->type = EBI2_PANEL;
+                pinfo->pdest = DISPLAY_2;
+                pinfo->wait_cycle = 0x808000;
+                pinfo->bpp = 16;
+                pinfo->fb_num = 2;
+                pinfo->lcd.vsync_enable = FALSE;
+
+                ret = platform_device_register(&this_device);
+                if (ret)
+                        platform_driver_unregister(&this_driver);
+        }
+
+        return ret;
+}
+
+module_init(epsonQcif_init);
diff --git a/drivers/staging/msm/ebi2_lcd.c b/drivers/staging/msm/ebi2_lcd.c
new file mode 100644
index 000000000000..b41e1230ceca
--- /dev/null
+++ b/drivers/staging/msm/ebi2_lcd.c
@@ -0,0 +1,250 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/uaccess.h>
+#include <linux/workqueue.h>
+#include <linux/string.h>
+#include <linux/version.h>
+#include <linux/proc_fs.h>
+#include <linux/vmalloc.h>
+#include <linux/debugfs.h>
+
+#include "msm_fb.h"
+
+static int ebi2_lcd_probe(struct platform_device *pdev);
+static int ebi2_lcd_remove(struct platform_device *pdev);
+
+static struct platform_driver ebi2_lcd_driver = {
+        .probe = ebi2_lcd_probe,
+        .remove = ebi2_lcd_remove,
+        .suspend = NULL,
+        .suspend_late = NULL,
+        .resume_early = NULL,
+        .resume = NULL,
+        .shutdown = NULL,
+        .driver = {
+                   .name = "ebi2_lcd",
+                   },
+};
+
+static void *ebi2_base;
+static void *ebi2_lcd_cfg0;
+static void *ebi2_lcd_cfg1;
+static void __iomem *lcd01_base;
+static void __iomem *lcd02_base;
+static int ebi2_lcd_resource_initialized;
+
+static struct platform_device *pdev_list[MSM_FB_MAX_DEV_LIST];
+static int pdev_list_cnt;
+
+static int ebi2_lcd_probe(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+        struct platform_device *mdp_dev = NULL;
+        struct msm_fb_panel_data *pdata = NULL;
+        int rc, i;
+
+        if (pdev->id == 0) {
+                for (i = 0; i < pdev->num_resources; i++) {
+                        if (!strncmp(pdev->resource[i].name, "base", 4)) {
+                                ebi2_base = ioremap(pdev->resource[i].start,
+                                                pdev->resource[i].end -
+                                                pdev->resource[i].start + 1);
+                                if (!ebi2_base) {
+                                        printk(KERN_ERR
+                                                "ebi2_base ioremap failed!\n");
+                                        return -ENOMEM;
+                                }
+                                ebi2_lcd_cfg0 = (void *)(ebi2_base + 0x20);
+                                ebi2_lcd_cfg1 = (void *)(ebi2_base + 0x24);
+                        } else if (!strncmp(pdev->resource[i].name,
+                                                "lcd01", 5)) {
+                                lcd01_base = ioremap(pdev->resource[i].start,
+                                                pdev->resource[i].end -
+                                                pdev->resource[i].start + 1);
+                                if (!lcd01_base) {
+                                        printk(KERN_ERR
+                                                "lcd01_base ioremap failed!\n");
+                                        return -ENOMEM;
+                                }
+                        } else if (!strncmp(pdev->resource[i].name,
+                                                "lcd02", 5)) {
+                                lcd02_base = ioremap(pdev->resource[i].start,
+                                                pdev->resource[i].end -
+                                                pdev->resource[i].start + 1);
+                                if (!lcd02_base) {
+                                        printk(KERN_ERR
+                                                "lcd02_base ioremap failed!\n");
+                                        return -ENOMEM;
+                                }
+                        }
+                }
+                ebi2_lcd_resource_initialized = 1;
+                return 0;
+        }
+
+        if (!ebi2_lcd_resource_initialized)
+                return -EPERM;
+
+        mfd = platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        if (pdev_list_cnt >= MSM_FB_MAX_DEV_LIST)
+                return -ENOMEM;
+
+        if (ebi2_base == NULL)
+                return -ENOMEM;
+
+        mdp_dev = platform_device_alloc("mdp", pdev->id);
+        if (!mdp_dev)
+                return -ENOMEM;
+
+        /* link to the latest pdev */
+        mfd->pdev = mdp_dev;
+        mfd->dest = DISPLAY_LCD;
+
+        /* add panel data */
+        if (platform_device_add_data
+            (mdp_dev, pdev->dev.platform_data,
+             sizeof(struct msm_fb_panel_data))) {
+                printk(KERN_ERR "ebi2_lcd_probe: platform_device_add_data failed!\n");
+                platform_device_put(mdp_dev);
+                return -ENOMEM;
+        }
+
+        /* data chain */
+        pdata = mdp_dev->dev.platform_data;
+        pdata->on = panel_next_on;
+        pdata->off = panel_next_off;
+        pdata->next = pdev;
+
+        /* get/set panel specific fb info */
+        mfd->panel_info = pdata->panel_info;
+
+        if (mfd->panel_info.bpp == 24)
+                mfd->fb_imgType = MDP_RGB_888;
+        else
+                mfd->fb_imgType = MDP_RGB_565;
+
+        /* config msm ebi2 lcd register */
+        if (mfd->panel_info.pdest == DISPLAY_1) {
+                outp32(ebi2_base,
+                       (inp32(ebi2_base) & (~(EBI2_PRIM_LCD_CLR))) |
+                       EBI2_PRIM_LCD_SEL);
+                /*
+                 * current design has one set of cfg0/1 register to control
+                 * both EBI2 channels. so, we're using the PRIM channel to
+                 * configure both.
+                 */
+                outp32(ebi2_lcd_cfg0, mfd->panel_info.wait_cycle);
+                if (mfd->panel_info.bpp == 18)
+                        outp32(ebi2_lcd_cfg1, 0x01000000);
+                else
+                        outp32(ebi2_lcd_cfg1, 0x0);
+        } else {
+#ifdef DEBUG_EBI2_LCD
+                /*
+                 * confliting with QCOM SURF FPGA CS.
+                 * OEM should enable below for their CS mapping
+                 */
+                 outp32(ebi2_base, (inp32(ebi2_base)&(~(EBI2_SECD_LCD_CLR)))
+                                        |EBI2_SECD_LCD_SEL);
+#endif
+        }
+
+        /*
+         * map cs (chip select) address
+         */
+        if (mfd->panel_info.pdest == DISPLAY_1) {
+                mfd->cmd_port = lcd01_base;
+                mfd->data_port =
+                    (void *)((uint32) mfd->cmd_port + EBI2_PRIM_LCD_RS_PIN);
+                mfd->data_port_phys =
+                    (void *)(LCD_PRIM_BASE_PHYS + EBI2_PRIM_LCD_RS_PIN);
+        } else {
+                mfd->cmd_port = lcd01_base;
+                mfd->data_port =
+                    (void *)((uint32) mfd->cmd_port + EBI2_SECD_LCD_RS_PIN);
+                mfd->data_port_phys =
+                    (void *)(LCD_SECD_BASE_PHYS + EBI2_SECD_LCD_RS_PIN);
+        }
+
+        /*
+         * set driver data
+         */
+        platform_set_drvdata(mdp_dev, mfd);
+
+        /*
+         * register in mdp driver
+         */
+        rc = platform_device_add(mdp_dev);
+        if (rc) {
+                goto ebi2_lcd_probe_err;
+        }
+
+        pdev_list[pdev_list_cnt++] = pdev;
+        return 0;
+
+      ebi2_lcd_probe_err:
+        platform_device_put(mdp_dev);
+        return rc;
+}
+
+static int ebi2_lcd_remove(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+
+        mfd = (struct msm_fb_data_type *)platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return 0;
+
+        if (mfd->key != MFD_KEY)
+                return 0;
+
+        iounmap(mfd->cmd_port);
+
+        return 0;
+}
+
+static int ebi2_lcd_register_driver(void)
+{
+        return platform_driver_register(&ebi2_lcd_driver);
+}
+
+static int __init ebi2_lcd_driver_init(void)
+{
+        return ebi2_lcd_register_driver();
+}
+
+module_init(ebi2_lcd_driver_init);
\ No newline at end of file
diff --git a/drivers/staging/msm/ebi2_tmd20.c b/drivers/staging/msm/ebi2_tmd20.c
new file mode 100644
index 000000000000..d66d03978253
--- /dev/null
+++ b/drivers/staging/msm/ebi2_tmd20.c
@@ -0,0 +1,1122 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+
+#include <linux/memory.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include "linux/proc_fs.h"
+
+#include <linux/delay.h>
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+
+/* #define TMD20QVGA_LCD_18BPP */
+#define QVGA_WIDTH        240
+#define QVGA_HEIGHT       320
+
+#ifdef TMD20QVGA_LCD_18BPP
+#define DISP_QVGA_18BPP(x)  ((((x)<<2) & 0x3FC00)|(( (x)<<1)& 0x1FE))
+#define DISP_REG(name)  uint32 register_##name;
+#define OUTPORT(x, y)  outpdw(x, y)
+#define INPORT(x)   inpdw(x)
+#else
+#define DISP_QVGA_18BPP(x)  (x)
+#define DISP_REG(name)  uint16 register_##name;
+#define OUTPORT(x, y)  outpw(x, y)
+#define INPORT(x)   intpw(x)
+#endif
+
+static void *DISP_CMD_PORT;
+static void *DISP_DATA_PORT;
+
+#define DISP_RNTI         0x10
+
+#define DISP_CMD_OUT(cmd) OUTPORT(DISP_CMD_PORT, DISP_QVGA_18BPP(cmd))
+#define DISP_DATA_OUT(data) OUTPORT(DISP_DATA_PORT, data)
+#define DISP_DATA_IN() INPORT(DISP_DATA_PORT)
+
+#if (defined(TMD20QVGA_LCD_18BPP))
+#define DISP_DATA_OUT_16TO18BPP(x) \
+        DISP_DATA_OUT((((x)&0xf800)<<2|((x)&0x80000)>>3) \
+                     | (((x)&0x7e0)<<1) \
+                     | (((x)&0x1F)<<1|((x)&0x10)>>4))
+#else
+#define DISP_DATA_OUT_16TO18BPP(x) \
+        DISP_DATA_OUT(x)
+#endif
+
+#define DISP_WRITE_OUT(addr, data) \
+   register_##addr = DISP_QVGA_18BPP(data); \
+   DISP_CMD_OUT(addr); \
+   DISP_DATA_OUT(register_##addr);
+
+#define DISP_UPDATE_VALUE(addr, bitmask, data) \
+   DISP_WRITE_OUT(##addr, (register_##addr & ~(bitmask)) | (data));
+
+#define DISP_VAL_IF(bitvalue, bitmask) \
+   ((bitvalue) ? (bitmask) : 0)
+
+/* QVGA = 256 x 320 */
+/* actual display is 240 x 320...offset by 0x10 */
+#define DISP_ROW_COL_TO_ADDR(row, col) ((row) * 0x100 + col)
+#define DISP_SET_RECT(ulhc_row, lrhc_row, ulhc_col, lrhc_col) \
+   { \
+   DISP_WRITE_OUT(DISP_HORZ_RAM_ADDR_POS_1_ADDR, (ulhc_col) + tmd20qvga_panel_offset); \
+   DISP_WRITE_OUT(DISP_HORZ_RAM_ADDR_POS_2_ADDR, (lrhc_col) + tmd20qvga_panel_offset); \
+   DISP_WRITE_OUT(DISP_VERT_RAM_ADDR_POS_1_ADDR, (ulhc_row)); \
+   DISP_WRITE_OUT(DISP_VERT_RAM_ADDR_POS_2_ADDR, (lrhc_row)); \
+   DISP_WRITE_OUT(DISP_RAM_ADDR_SET_1_ADDR, (ulhc_col) + tmd20qvga_panel_offset); \
+   DISP_WRITE_OUT(DISP_RAM_ADDR_SET_2_ADDR, (ulhc_row)); \
+   }
+
+#define WAIT_MSEC(msec) mdelay(msec)
+
+/*
+ * TMD QVGA Address
+ */
+/* Display Control */
+#define DISP_START_OSCILLATION_ADDR     0x000
+DISP_REG(DISP_START_OSCILLATION_ADDR)
+#define DISP_DRIVER_OUTPUT_CTL_ADDR     0x001
+    DISP_REG(DISP_DRIVER_OUTPUT_CTL_ADDR)
+#define DISP_LCD_DRIVING_SIG_ADDR     0x002
+    DISP_REG(DISP_LCD_DRIVING_SIG_ADDR)
+#define DISP_ENTRY_MODE_ADDR            0x003
+    DISP_REG(DISP_ENTRY_MODE_ADDR)
+#define DISP_DISPLAY_CTL_1_ADDR         0x007
+    DISP_REG(DISP_DISPLAY_CTL_1_ADDR)
+#define DISP_DISPLAY_CTL_2_ADDR         0x008
+    DISP_REG(DISP_DISPLAY_CTL_2_ADDR)
+
+/* DISPLAY MODE 0x009 partial display not supported */
+#define DISP_POWER_SUPPLY_INTF_ADDR     0x00A
+    DISP_REG(DISP_POWER_SUPPLY_INTF_ADDR)
+
+/* DISPLAY MODE 0x00B xZoom feature is not supported */
+#define DISP_EXT_DISPLAY_CTL_1_ADDR     0x00C
+    DISP_REG(DISP_EXT_DISPLAY_CTL_1_ADDR)
+
+#define DISP_FRAME_CYCLE_CTL_ADDR       0x00D
+    DISP_REG(DISP_FRAME_CYCLE_CTL_ADDR)
+
+#define DISP_EXT_DISPLAY_CTL_2_ADDR     0x00E
+    DISP_REG(DISP_EXT_DISPLAY_CTL_2_ADDR)
+
+#define DISP_EXT_DISPLAY_CTL_3_ADDR     0x00F
+    DISP_REG(DISP_EXT_DISPLAY_CTL_3_ADDR)
+
+#define DISP_LTPS_CTL_1_ADDR            0x012
+    DISP_REG(DISP_LTPS_CTL_1_ADDR)
+#define DISP_LTPS_CTL_2_ADDR            0x013
+    DISP_REG(DISP_LTPS_CTL_2_ADDR)
+#define DISP_LTPS_CTL_3_ADDR            0x014
+    DISP_REG(DISP_LTPS_CTL_3_ADDR)
+#define DISP_LTPS_CTL_4_ADDR            0x018
+    DISP_REG(DISP_LTPS_CTL_4_ADDR)
+#define DISP_LTPS_CTL_5_ADDR            0x019
+    DISP_REG(DISP_LTPS_CTL_5_ADDR)
+#define DISP_LTPS_CTL_6_ADDR            0x01A
+    DISP_REG(DISP_LTPS_CTL_6_ADDR)
+#define DISP_AMP_SETTING_ADDR           0x01C
+    DISP_REG(DISP_AMP_SETTING_ADDR)
+#define DISP_MODE_SETTING_ADDR          0x01D
+    DISP_REG(DISP_MODE_SETTING_ADDR)
+#define DISP_POFF_LN_SETTING_ADDR       0x01E
+    DISP_REG(DISP_POFF_LN_SETTING_ADDR)
+/* Power Contol */
+#define DISP_POWER_CTL_1_ADDR           0x100
+    DISP_REG(DISP_POWER_CTL_1_ADDR)
+#define DISP_POWER_CTL_2_ADDR           0x101
+    DISP_REG(DISP_POWER_CTL_2_ADDR)
+#define DISP_POWER_CTL_3_ADDR           0x102
+    DISP_REG(DISP_POWER_CTL_3_ADDR)
+#define DISP_POWER_CTL_4_ADDR           0x103
+    DISP_REG(DISP_POWER_CTL_4_ADDR)
+#define DISP_POWER_CTL_5_ADDR           0x104
+    DISP_REG(DISP_POWER_CTL_5_ADDR)
+#define DISP_POWER_CTL_6_ADDR           0x105
+    DISP_REG(DISP_POWER_CTL_6_ADDR)
+#define DISP_POWER_CTL_7_ADDR           0x106
+    DISP_REG(DISP_POWER_CTL_7_ADDR)
+/* RAM Access */
+#define DISP_RAM_ADDR_SET_1_ADDR        0x200
+    DISP_REG(DISP_RAM_ADDR_SET_1_ADDR)
+#define DISP_RAM_ADDR_SET_2_ADDR        0x201
+    DISP_REG(DISP_RAM_ADDR_SET_2_ADDR)
+#define DISP_CMD_RAMRD                  DISP_CMD_RAMWR
+#define DISP_CMD_RAMWR                  0x202
+    DISP_REG(DISP_CMD_RAMWR)
+#define DISP_RAM_DATA_MASK_1_ADDR       0x203
+    DISP_REG(DISP_RAM_DATA_MASK_1_ADDR)
+#define DISP_RAM_DATA_MASK_2_ADDR       0x204
+    DISP_REG(DISP_RAM_DATA_MASK_2_ADDR)
+/* Gamma Control, Contrast, Gray Scale Setting */
+#define DISP_GAMMA_CONTROL_1_ADDR       0x300
+    DISP_REG(DISP_GAMMA_CONTROL_1_ADDR)
+#define DISP_GAMMA_CONTROL_2_ADDR       0x301
+    DISP_REG(DISP_GAMMA_CONTROL_2_ADDR)
+#define DISP_GAMMA_CONTROL_3_ADDR       0x302
+    DISP_REG(DISP_GAMMA_CONTROL_3_ADDR)
+#define DISP_GAMMA_CONTROL_4_ADDR       0x303
+    DISP_REG(DISP_GAMMA_CONTROL_4_ADDR)
+#define DISP_GAMMA_CONTROL_5_ADDR       0x304
+    DISP_REG(DISP_GAMMA_CONTROL_5_ADDR)
+/* Coordinate Control */
+#define DISP_VERT_SCROLL_CTL_1_ADDR     0x400
+    DISP_REG(DISP_VERT_SCROLL_CTL_1_ADDR)
+#define DISP_VERT_SCROLL_CTL_2_ADDR     0x401
+    DISP_REG(DISP_VERT_SCROLL_CTL_2_ADDR)
+#define DISP_SCREEN_1_DRV_POS_1_ADDR    0x402
+    DISP_REG(DISP_SCREEN_1_DRV_POS_1_ADDR)
+#define DISP_SCREEN_1_DRV_POS_2_ADDR    0x403
+    DISP_REG(DISP_SCREEN_1_DRV_POS_2_ADDR)
+#define DISP_SCREEN_2_DRV_POS_1_ADDR    0x404
+    DISP_REG(DISP_SCREEN_2_DRV_POS_1_ADDR)
+#define DISP_SCREEN_2_DRV_POS_2_ADDR    0x405
+    DISP_REG(DISP_SCREEN_2_DRV_POS_2_ADDR)
+#define DISP_HORZ_RAM_ADDR_POS_1_ADDR   0x406
+    DISP_REG(DISP_HORZ_RAM_ADDR_POS_1_ADDR)
+#define DISP_HORZ_RAM_ADDR_POS_2_ADDR   0x407
+    DISP_REG(DISP_HORZ_RAM_ADDR_POS_2_ADDR)
+#define DISP_VERT_RAM_ADDR_POS_1_ADDR   0x408
+    DISP_REG(DISP_VERT_RAM_ADDR_POS_1_ADDR)
+#define DISP_VERT_RAM_ADDR_POS_2_ADDR   0x409
+    DISP_REG(DISP_VERT_RAM_ADDR_POS_2_ADDR)
+#define DISP_TMD_700_ADDR               0x700   /*  0x700 */
+    DISP_REG(DISP_TMD_700_ADDR)
+#define DISP_TMD_015_ADDR               0x015   /*  0x700 */
+    DISP_REG(DISP_TMD_015_ADDR)
+#define DISP_TMD_305_ADDR               0x305   /*  0x700 */
+    DISP_REG(DISP_TMD_305_ADDR)
+
+/*
+ * TMD QVGA Bit Definations
+ */
+
+#define DISP_BIT_IB15              0x8000
+#define DISP_BIT_IB14              0x4000
+#define DISP_BIT_IB13              0x2000
+#define DISP_BIT_IB12              0x1000
+#define DISP_BIT_IB11              0x0800
+#define DISP_BIT_IB10              0x0400
+#define DISP_BIT_IB09              0x0200
+#define DISP_BIT_IB08              0x0100
+#define DISP_BIT_IB07              0x0080
+#define DISP_BIT_IB06              0x0040
+#define DISP_BIT_IB05              0x0020
+#define DISP_BIT_IB04              0x0010
+#define DISP_BIT_IB03              0x0008
+#define DISP_BIT_IB02              0x0004
+#define DISP_BIT_IB01              0x0002
+#define DISP_BIT_IB00              0x0001
+/*
+ * Display Control
+ * DISP_START_OSCILLATION_ADDR     Start Oscillation
+ * DISP_DRIVER_OUTPUT_CTL_ADDR     Driver Output Control
+ */
+#define DISP_BITMASK_SS            DISP_BIT_IB08
+#define DISP_BITMASK_NL5           DISP_BIT_IB05
+#define DISP_BITMASK_NL4           DISP_BIT_IB04
+#define DISP_BITMASK_NL3           DISP_BIT_IB03
+#define DISP_BITMASK_NL2           DISP_BIT_IB02
+#define DISP_BITMASK_NL1           DISP_BIT_IB01
+#define DISP_BITMASK_NL0           DISP_BIT_IB00
+/* DISP_LCD_DRIVING_SIG_ADDR       LCD Driving Signal Setting */
+#define DISP_BITMASK_BC            DISP_BIT_IB09
+/* DISP_ENTRY_MODE_ADDR            Entry Mode */
+#define DISP_BITMASK_TRI           DISP_BIT_IB15
+#define DISP_BITMASK_DFM1          DISP_BIT_IB14
+#define DISP_BITMASK_DFM0          DISP_BIT_IB13
+#define DISP_BITMASK_BGR           DISP_BIT_IB12
+#define DISP_BITMASK_HWM0          DISP_BIT_IB08
+#define DISP_BITMASK_ID1           DISP_BIT_IB05
+#define DISP_BITMASK_ID0           DISP_BIT_IB04
+#define DISP_BITMASK_AM            DISP_BIT_IB03
+/* DISP_DISPLAY_CTL_1_ADDR         Display Control (1) */
+#define DISP_BITMASK_COL1          DISP_BIT_IB15
+#define DISP_BITMASK_COL0          DISP_BIT_IB14
+#define DISP_BITMASK_VLE2          DISP_BIT_IB10
+#define DISP_BITMASK_VLE1          DISP_BIT_IB09
+#define DISP_BITMASK_SPT           DISP_BIT_IB08
+#define DISP_BITMASK_PT1           DISP_BIT_IB07
+#define DISP_BITMASK_PT0           DISP_BIT_IB06
+#define DISP_BITMASK_REV           DISP_BIT_IB02
+/* DISP_DISPLAY_CTL_2_ADDR         Display Control (2) */
+#define DISP_BITMASK_FP3           DISP_BIT_IB11
+#define DISP_BITMASK_FP2           DISP_BIT_IB10
+#define DISP_BITMASK_FP1           DISP_BIT_IB09
+#define DISP_BITMASK_FP0           DISP_BIT_IB08
+#define DISP_BITMASK_BP3           DISP_BIT_IB03
+#define DISP_BITMASK_BP2           DISP_BIT_IB02
+#define DISP_BITMASK_BP1           DISP_BIT_IB01
+#define DISP_BITMASK_BP0           DISP_BIT_IB00
+/* DISP_POWER_SUPPLY_INTF_ADDR     Power Supply IC Interface Control */
+#define DISP_BITMASK_CSE           DISP_BIT_IB12
+#define DISP_BITMASK_TE            DISP_BIT_IB08
+#define DISP_BITMASK_IX3           DISP_BIT_IB03
+#define DISP_BITMASK_IX2           DISP_BIT_IB02
+#define DISP_BITMASK_IX1           DISP_BIT_IB01
+#define DISP_BITMASK_IX0           DISP_BIT_IB00
+/* DISP_EXT_DISPLAY_CTL_1_ADDR     External Display Interface Control (1) */
+#define DISP_BITMASK_RM            DISP_BIT_IB08
+#define DISP_BITMASK_DM1           DISP_BIT_IB05
+#define DISP_BITMASK_DM0           DISP_BIT_IB04
+#define DISP_BITMASK_RIM1          DISP_BIT_IB01
+#define DISP_BITMASK_RIM0          DISP_BIT_IB00
+/* DISP_FRAME_CYCLE_CTL_ADDR       Frame Frequency Adjustment Control */
+#define DISP_BITMASK_DIVI1         DISP_BIT_IB09
+#define DISP_BITMASK_DIVI0         DISP_BIT_IB08
+#define DISP_BITMASK_RTNI4         DISP_BIT_IB04
+#define DISP_BITMASK_RTNI3         DISP_BIT_IB03
+#define DISP_BITMASK_RTNI2         DISP_BIT_IB02
+#define DISP_BITMASK_RTNI1         DISP_BIT_IB01
+#define DISP_BITMASK_RTNI0         DISP_BIT_IB00
+/* DISP_EXT_DISPLAY_CTL_2_ADDR     External Display Interface Control (2) */
+#define DISP_BITMASK_DIVE1         DISP_BIT_IB09
+#define DISP_BITMASK_DIVE0         DISP_BIT_IB08
+#define DISP_BITMASK_RTNE7         DISP_BIT_IB07
+#define DISP_BITMASK_RTNE6         DISP_BIT_IB06
+#define DISP_BITMASK_RTNE5         DISP_BIT_IB05
+#define DISP_BITMASK_RTNE4         DISP_BIT_IB04
+#define DISP_BITMASK_RTNE3         DISP_BIT_IB03
+#define DISP_BITMASK_RTNE2         DISP_BIT_IB02
+#define DISP_BITMASK_RTNE1         DISP_BIT_IB01
+#define DISP_BITMASK_RTNE0         DISP_BIT_IB00
+/* DISP_EXT_DISPLAY_CTL_3_ADDR     External Display Interface Control (3) */
+#define DISP_BITMASK_VSPL          DISP_BIT_IB04
+#define DISP_BITMASK_HSPL          DISP_BIT_IB03
+#define DISP_BITMASK_VPL           DISP_BIT_IB02
+#define DISP_BITMASK_EPL           DISP_BIT_IB01
+#define DISP_BITMASK_DPL           DISP_BIT_IB00
+/* DISP_LTPS_CTL_1_ADDR            LTPS Interface Control (1) */
+#define DISP_BITMASK_CLWI3         DISP_BIT_IB11
+#define DISP_BITMASK_CLWI2         DISP_BIT_IB10
+#define DISP_BITMASK_CLWI1         DISP_BIT_IB09
+#define DISP_BITMASK_CLWI0         DISP_BIT_IB08
+#define DISP_BITMASK_CLTI1         DISP_BIT_IB01
+#define DISP_BITMASK_CLTI0         DISP_BIT_IB00
+/* DISP_LTPS_CTL_2_ADDR            LTPS Interface Control (2) */
+#define DISP_BITMASK_OEVBI1        DISP_BIT_IB09
+#define DISP_BITMASK_OEVBI0        DISP_BIT_IB08
+#define DISP_BITMASK_OEVFI1        DISP_BIT_IB01
+#define DISP_BITMASK_OEVFI0        DISP_BIT_IB00
+/* DISP_LTPS_CTL_3_ADDR            LTPS Interface Control (3) */
+#define DISP_BITMASK_SHI1          DISP_BIT_IB01
+#define DISP_BITMASK_SHI0          DISP_BIT_IB00
+/* DISP_LTPS_CTL_4_ADDR            LTPS Interface Control (4) */
+#define DISP_BITMASK_CLWE5         DISP_BIT_IB13
+#define DISP_BITMASK_CLWE4         DISP_BIT_IB12
+#define DISP_BITMASK_CLWE3         DISP_BIT_IB11
+#define DISP_BITMASK_CLWE2         DISP_BIT_IB10
+#define DISP_BITMASK_CLWE1         DISP_BIT_IB09
+#define DISP_BITMASK_CLWE0         DISP_BIT_IB08
+#define DISP_BITMASK_CLTE3         DISP_BIT_IB03
+#define DISP_BITMASK_CLTE2         DISP_BIT_IB02
+#define DISP_BITMASK_CLTE1         DISP_BIT_IB01
+#define DISP_BITMASK_CLTE0         DISP_BIT_IB00
+/* DISP_LTPS_CTL_5_ADDR            LTPS Interface Control (5) */
+#define DISP_BITMASK_OEVBE3        DISP_BIT_IB11
+#define DISP_BITMASK_OEVBE2        DISP_BIT_IB10
+#define DISP_BITMASK_OEVBE1        DISP_BIT_IB09
+#define DISP_BITMASK_OEVBE0        DISP_BIT_IB08
+#define DISP_BITMASK_OEVFE3        DISP_BIT_IB03
+#define DISP_BITMASK_OEVFE2        DISP_BIT_IB02
+#define DISP_BITMASK_OEVFE1        DISP_BIT_IB01
+#define DISP_BITMASK_OEVFE0        DISP_BIT_IB00
+/* DISP_LTPS_CTL_6_ADDR            LTPS Interface Control (6) */
+#define DISP_BITMASK_SHE3          DISP_BIT_IB03
+#define DISP_BITMASK_SHE2          DISP_BIT_IB02
+#define DISP_BITMASK_SHE1          DISP_BIT_IB01
+#define DISP_BITMASK_SHE0          DISP_BIT_IB00
+/* DISP_AMP_SETTING_ADDR           Amplify Setting */
+#define DISP_BITMASK_ABSW1         DISP_BIT_IB01
+#define DISP_BITMASK_ABSW0         DISP_BIT_IB00
+/* DISP_MODE_SETTING_ADDR          Mode Setting */
+#define DISP_BITMASK_DSTB          DISP_BIT_IB02
+#define DISP_BITMASK_STB           DISP_BIT_IB00
+/* DISP_POFF_LN_SETTING_ADDR       Power Off Line Setting */
+#define DISP_BITMASK_POFH3         DISP_BIT_IB03
+#define DISP_BITMASK_POFH2         DISP_BIT_IB02
+#define DISP_BITMASK_POFH1         DISP_BIT_IB01
+#define DISP_BITMASK_POFH0         DISP_BIT_IB00
+
+/* Power Contol */
+/* DISP_POWER_CTL_1_ADDR           Power Control (1) */
+#define DISP_BITMASK_PO            DISP_BIT_IB11
+#define DISP_BITMASK_VCD           DISP_BIT_IB09
+#define DISP_BITMASK_VSC           DISP_BIT_IB08
+#define DISP_BITMASK_CON           DISP_BIT_IB07
+#define DISP_BITMASK_ASW1          DISP_BIT_IB06
+#define DISP_BITMASK_ASW0          DISP_BIT_IB05
+#define DISP_BITMASK_OEV           DISP_BIT_IB04
+#define DISP_BITMASK_OEVE          DISP_BIT_IB03
+#define DISP_BITMASK_FR            DISP_BIT_IB02
+#define DISP_BITMASK_D1            DISP_BIT_IB01
+#define DISP_BITMASK_D0            DISP_BIT_IB00
+/* DISP_POWER_CTL_2_ADDR           Power Control (2) */
+#define DISP_BITMASK_DC4           DISP_BIT_IB15
+#define DISP_BITMASK_DC3           DISP_BIT_IB14
+#define DISP_BITMASK_SAP2          DISP_BIT_IB13
+#define DISP_BITMASK_SAP1          DISP_BIT_IB12
+#define DISP_BITMASK_SAP0          DISP_BIT_IB11
+#define DISP_BITMASK_BT2           DISP_BIT_IB10
+#define DISP_BITMASK_BT1           DISP_BIT_IB09
+#define DISP_BITMASK_BT0           DISP_BIT_IB08
+#define DISP_BITMASK_DC2           DISP_BIT_IB07
+#define DISP_BITMASK_DC1           DISP_BIT_IB06
+#define DISP_BITMASK_DC0           DISP_BIT_IB05
+#define DISP_BITMASK_AP2           DISP_BIT_IB04
+#define DISP_BITMASK_AP1           DISP_BIT_IB03
+#define DISP_BITMASK_AP0           DISP_BIT_IB02
+/* DISP_POWER_CTL_3_ADDR           Power Control (3) */
+#define DISP_BITMASK_VGL4          DISP_BIT_IB10
+#define DISP_BITMASK_VGL3          DISP_BIT_IB09
+#define DISP_BITMASK_VGL2          DISP_BIT_IB08
+#define DISP_BITMASK_VGL1          DISP_BIT_IB07
+#define DISP_BITMASK_VGL0          DISP_BIT_IB06
+#define DISP_BITMASK_VGH4          DISP_BIT_IB04
+#define DISP_BITMASK_VGH3          DISP_BIT_IB03
+#define DISP_BITMASK_VGH2          DISP_BIT_IB02
+#define DISP_BITMASK_VGH1          DISP_BIT_IB01
+#define DISP_BITMASK_VGH0          DISP_BIT_IB00
+/* DISP_POWER_CTL_4_ADDR           Power Control (4) */
+#define DISP_BITMASK_VC2           DISP_BIT_IB02
+#define DISP_BITMASK_VC1           DISP_BIT_IB01
+#define DISP_BITMASK_VC0           DISP_BIT_IB00
+/* DISP_POWER_CTL_5_ADDR           Power Control (5) */
+#define DISP_BITMASK_VRL3          DISP_BIT_IB11
+#define DISP_BITMASK_VRL2          DISP_BIT_IB10
+#define DISP_BITMASK_VRL1          DISP_BIT_IB09
+#define DISP_BITMASK_VRL0          DISP_BIT_IB08
+#define DISP_BITMASK_PON           DISP_BIT_IB04
+#define DISP_BITMASK_VRH3          DISP_BIT_IB03
+#define DISP_BITMASK_VRH2          DISP_BIT_IB02
+#define DISP_BITMASK_VRH1          DISP_BIT_IB01
+#define DISP_BITMASK_VRH0          DISP_BIT_IB00
+/* DISP_POWER_CTL_6_ADDR           Power Control (6) */
+#define DISP_BITMASK_VCOMG         DISP_BIT_IB13
+#define DISP_BITMASK_VDV4          DISP_BIT_IB12
+#define DISP_BITMASK_VDV3          DISP_BIT_IB11
+#define DISP_BITMASK_VDV2          DISP_BIT_IB10
+#define DISP_BITMASK_VDV1          DISP_BIT_IB09
+#define DISP_BITMASK_VDV0          DISP_BIT_IB08
+#define DISP_BITMASK_VCM4          DISP_BIT_IB04
+#define DISP_BITMASK_VCM3          DISP_BIT_IB03
+#define DISP_BITMASK_VCM2          DISP_BIT_IB02
+#define DISP_BITMASK_VCM1          DISP_BIT_IB01
+#define DISP_BITMASK_VCM0          DISP_BIT_IB00
+/* RAM Access */
+/* DISP_RAM_ADDR_SET_1_ADDR        RAM Address Set (1) */
+#define DISP_BITMASK_AD7           DISP_BIT_IB07
+#define DISP_BITMASK_AD6           DISP_BIT_IB06
+#define DISP_BITMASK_AD5           DISP_BIT_IB05
+#define DISP_BITMASK_AD4           DISP_BIT_IB04
+#define DISP_BITMASK_AD3           DISP_BIT_IB03
+#define DISP_BITMASK_AD2           DISP_BIT_IB02
+#define DISP_BITMASK_AD1           DISP_BIT_IB01
+#define DISP_BITMASK_AD0           DISP_BIT_IB00
+/* DISP_RAM_ADDR_SET_2_ADDR        RAM Address Set (2) */
+#define DISP_BITMASK_AD16          DISP_BIT_IB08
+#define DISP_BITMASK_AD15          DISP_BIT_IB07
+#define DISP_BITMASK_AD14          DISP_BIT_IB06
+#define DISP_BITMASK_AD13          DISP_BIT_IB05
+#define DISP_BITMASK_AD12          DISP_BIT_IB04
+#define DISP_BITMASK_AD11          DISP_BIT_IB03
+#define DISP_BITMASK_AD10          DISP_BIT_IB02
+#define DISP_BITMASK_AD9           DISP_BIT_IB01
+#define DISP_BITMASK_AD8           DISP_BIT_IB00
+/*
+ * DISP_CMD_RAMWR       RAM Data Read/Write
+ * Use Data Bit Configuration
+ */
+/* DISP_RAM_DATA_MASK_1_ADDR       RAM Write Data Mask (1) */
+#define DISP_BITMASK_WM11          DISP_BIT_IB13
+#define DISP_BITMASK_WM10          DISP_BIT_IB12
+#define DISP_BITMASK_WM9           DISP_BIT_IB11
+#define DISP_BITMASK_WM8           DISP_BIT_IB10
+#define DISP_BITMASK_WM7           DISP_BIT_IB09
+#define DISP_BITMASK_WM6           DISP_BIT_IB08
+#define DISP_BITMASK_WM5           DISP_BIT_IB05
+#define DISP_BITMASK_WM4           DISP_BIT_IB04
+#define DISP_BITMASK_WM3           DISP_BIT_IB03
+#define DISP_BITMASK_WM2           DISP_BIT_IB02
+#define DISP_BITMASK_WM1           DISP_BIT_IB01
+#define DISP_BITMASK_WM0           DISP_BIT_IB00
+/* DISP_RAM_DATA_MASK_2_ADDR       RAM Write Data Mask (2) */
+#define DISP_BITMASK_WM17          DISP_BIT_IB05
+#define DISP_BITMASK_WM16          DISP_BIT_IB04
+#define DISP_BITMASK_WM15          DISP_BIT_IB03
+#define DISP_BITMASK_WM14          DISP_BIT_IB02
+#define DISP_BITMASK_WM13          DISP_BIT_IB01
+#define DISP_BITMASK_WM12          DISP_BIT_IB00
+/*Gamma Control */
+/* DISP_GAMMA_CONTROL_1_ADDR       Gamma Control (1) */
+#define DISP_BITMASK_PKP12         DISP_BIT_IB10
+#define DISP_BITMASK_PKP11         DISP_BIT_IB08
+#define DISP_BITMASK_PKP10         DISP_BIT_IB09
+#define DISP_BITMASK_PKP02         DISP_BIT_IB02
+#define DISP_BITMASK_PKP01         DISP_BIT_IB01
+#define DISP_BITMASK_PKP00         DISP_BIT_IB00
+/* DISP_GAMMA_CONTROL_2_ADDR       Gamma Control (2) */
+#define DISP_BITMASK_PKP32         DISP_BIT_IB10
+#define DISP_BITMASK_PKP31         DISP_BIT_IB09
+#define DISP_BITMASK_PKP30         DISP_BIT_IB08
+#define DISP_BITMASK_PKP22         DISP_BIT_IB02
+#define DISP_BITMASK_PKP21         DISP_BIT_IB01
+#define DISP_BITMASK_PKP20         DISP_BIT_IB00
+/* DISP_GAMMA_CONTROL_3_ADDR       Gamma Control (3) */
+#define DISP_BITMASK_PKP52         DISP_BIT_IB10
+#define DISP_BITMASK_PKP51         DISP_BIT_IB09
+#define DISP_BITMASK_PKP50         DISP_BIT_IB08
+#define DISP_BITMASK_PKP42         DISP_BIT_IB02
+#define DISP_BITMASK_PKP41         DISP_BIT_IB01
+#define DISP_BITMASK_PKP40         DISP_BIT_IB00
+/* DISP_GAMMA_CONTROL_4_ADDR       Gamma Control (4) */
+#define DISP_BITMASK_PRP12         DISP_BIT_IB10
+#define DISP_BITMASK_PRP11         DISP_BIT_IB08
+#define DISP_BITMASK_PRP10         DISP_BIT_IB09
+#define DISP_BITMASK_PRP02         DISP_BIT_IB02
+#define DISP_BITMASK_PRP01         DISP_BIT_IB01
+#define DISP_BITMASK_PRP00         DISP_BIT_IB00
+/* DISP_GAMMA_CONTROL_5_ADDR       Gamma Control (5) */
+#define DISP_BITMASK_VRP14         DISP_BIT_IB12
+#define DISP_BITMASK_VRP13         DISP_BIT_IB11
+#define DISP_BITMASK_VRP12         DISP_BIT_IB10
+#define DISP_BITMASK_VRP11         DISP_BIT_IB08
+#define DISP_BITMASK_VRP10         DISP_BIT_IB09
+#define DISP_BITMASK_VRP03         DISP_BIT_IB03
+#define DISP_BITMASK_VRP02         DISP_BIT_IB02
+#define DISP_BITMASK_VRP01         DISP_BIT_IB01
+#define DISP_BITMASK_VRP00         DISP_BIT_IB00
+/* DISP_GAMMA_CONTROL_6_ADDR       Gamma Control (6) */
+#define DISP_BITMASK_PKN12         DISP_BIT_IB10
+#define DISP_BITMASK_PKN11         DISP_BIT_IB08
+#define DISP_BITMASK_PKN10         DISP_BIT_IB09
+#define DISP_BITMASK_PKN02         DISP_BIT_IB02
+#define DISP_BITMASK_PKN01         DISP_BIT_IB01
+#define DISP_BITMASK_PKN00         DISP_BIT_IB00
+/* DISP_GAMMA_CONTROL_7_ADDR       Gamma Control (7) */
+#define DISP_BITMASK_PKN32         DISP_BIT_IB10
+#define DISP_BITMASK_PKN31         DISP_BIT_IB08
+#define DISP_BITMASK_PKN30         DISP_BIT_IB09
+#define DISP_BITMASK_PKN22         DISP_BIT_IB02
+#define DISP_BITMASK_PKN21         DISP_BIT_IB01
+#define DISP_BITMASK_PKN20         DISP_BIT_IB00
+/* DISP_GAMMA_CONTROL_8_ADDR       Gamma Control (8) */
+#define DISP_BITMASK_PKN52         DISP_BIT_IB10
+#define DISP_BITMASK_PKN51         DISP_BIT_IB08
+#define DISP_BITMASK_PKN50         DISP_BIT_IB09
+#define DISP_BITMASK_PKN42         DISP_BIT_IB02
+#define DISP_BITMASK_PKN41         DISP_BIT_IB01
+#define DISP_BITMASK_PKN40         DISP_BIT_IB00
+/* DISP_GAMMA_CONTROL_9_ADDR       Gamma Control (9) */
+#define DISP_BITMASK_PRN12         DISP_BIT_IB10
+#define DISP_BITMASK_PRN11         DISP_BIT_IB08
+#define DISP_BITMASK_PRN10         DISP_BIT_IB09
+#define DISP_BITMASK_PRN02         DISP_BIT_IB02
+#define DISP_BITMASK_PRN01         DISP_BIT_IB01
+#define DISP_BITMASK_PRN00         DISP_BIT_IB00
+/* DISP_GAMMA_CONTROL_10_ADDR      Gamma Control (10) */
+#define DISP_BITMASK_VRN14         DISP_BIT_IB12
+#define DISP_BITMASK_VRN13         DISP_BIT_IB11
+#define DISP_BITMASK_VRN12         DISP_BIT_IB10
+#define DISP_BITMASK_VRN11         DISP_BIT_IB08
+#define DISP_BITMASK_VRN10         DISP_BIT_IB09
+#define DISP_BITMASK_VRN03         DISP_BIT_IB03
+#define DISP_BITMASK_VRN02         DISP_BIT_IB02
+#define DISP_BITMASK_VRN01         DISP_BIT_IB01
+#define DISP_BITMASK_VRN00         DISP_BIT_IB00
+/* Coordinate Control */
+/* DISP_VERT_SCROLL_CTL_1_ADDR     Vertical Scroll Control (1) */
+#define DISP_BITMASK_VL18          DISP_BIT_IB08
+#define DISP_BITMASK_VL17          DISP_BIT_IB07
+#define DISP_BITMASK_VL16          DISP_BIT_IB06
+#define DISP_BITMASK_VL15          DISP_BIT_IB05
+#define DISP_BITMASK_VL14          DISP_BIT_IB04
+#define DISP_BITMASK_VL13          DISP_BIT_IB03
+#define DISP_BITMASK_VL12          DISP_BIT_IB02
+#define DISP_BITMASK_VL11          DISP_BIT_IB01
+#define DISP_BITMASK_VL10          DISP_BIT_IB00
+/* DISP_VERT_SCROLL_CTL_2_ADDR     Vertical Scroll Control (2) */
+#define DISP_BITMASK_VL28          DISP_BIT_IB08
+#define DISP_BITMASK_VL27          DISP_BIT_IB07
+#define DISP_BITMASK_VL26          DISP_BIT_IB06
+#define DISP_BITMASK_VL25          DISP_BIT_IB05
+#define DISP_BITMASK_VL24          DISP_BIT_IB04
+#define DISP_BITMASK_VL23          DISP_BIT_IB03
+#define DISP_BITMASK_VL22          DISP_BIT_IB02
+#define DISP_BITMASK_VL21          DISP_BIT_IB01
+#define DISP_BITMASK_VL20          DISP_BIT_IB00
+/* DISP_SCREEN_1_DRV_POS_1_ADDR    First Screen Driving Position (1) */
+#define DISP_BITMASK_SS18          DISP_BIT_IB08
+#define DISP_BITMASK_SS17          DISP_BIT_IB07
+#define DISP_BITMASK_SS16          DISP_BIT_IB06
+#define DISP_BITMASK_SS15          DISP_BIT_IB05
+#define DISP_BITMASK_SS14          DISP_BIT_IB04
+#define DISP_BITMASK_SS13          DISP_BIT_IB03
+#define DISP_BITMASK_SS12          DISP_BIT_IB02
+#define DISP_BITMASK_SS11          DISP_BIT_IB01
+#define DISP_BITMASK_SS10          DISP_BIT_IB00
+/* DISP_SCREEN_1_DRV_POS_2_ADDR    First Screen Driving Position (2) */
+#define DISP_BITMASK_SE18          DISP_BIT_IB08
+#define DISP_BITMASK_SE17          DISP_BIT_IB07
+#define DISP_BITMASK_SE16          DISP_BIT_IB06
+#define DISP_BITMASK_SE15          DISP_BIT_IB05
+#define DISP_BITMASK_SE14          DISP_BIT_IB04
+#define DISP_BITMASK_SE13          DISP_BIT_IB03
+#define DISP_BITMASK_SE12          DISP_BIT_IB02
+#define DISP_BITMASK_SE11          DISP_BIT_IB01
+#define DISP_BITMASK_SE10          DISP_BIT_IB00
+/* DISP_SCREEN_2_DRV_POS_1_ADDR    Second Screen Driving Position (1) */
+#define DISP_BITMASK_SS28          DISP_BIT_IB08
+#define DISP_BITMASK_SS27          DISP_BIT_IB07
+#define DISP_BITMASK_SS26          DISP_BIT_IB06
+#define DISP_BITMASK_SS25          DISP_BIT_IB05
+#define DISP_BITMASK_SS24          DISP_BIT_IB04
+#define DISP_BITMASK_SS23          DISP_BIT_IB03
+#define DISP_BITMASK_SS22          DISP_BIT_IB02
+#define DISP_BITMASK_SS21          DISP_BIT_IB01
+#define DISP_BITMASK_SS20          DISP_BIT_IB00
+/* DISP_SCREEN_3_DRV_POS_2_ADDR    Second Screen Driving Position (2) */
+#define DISP_BITMASK_SE28          DISP_BIT_IB08
+#define DISP_BITMASK_SE27          DISP_BIT_IB07
+#define DISP_BITMASK_SE26          DISP_BIT_IB06
+#define DISP_BITMASK_SE25          DISP_BIT_IB05
+#define DISP_BITMASK_SE24          DISP_BIT_IB04
+#define DISP_BITMASK_SE23          DISP_BIT_IB03
+#define DISP_BITMASK_SE22          DISP_BIT_IB02
+#define DISP_BITMASK_SE21          DISP_BIT_IB01
+#define DISP_BITMASK_SE20          DISP_BIT_IB00
+/* DISP_HORZ_RAM_ADDR_POS_1_ADDR   Horizontal RAM Address Position (1) */
+#define DISP_BITMASK_HSA7          DISP_BIT_IB07
+#define DISP_BITMASK_HSA6          DISP_BIT_IB06
+#define DISP_BITMASK_HSA5          DISP_BIT_IB05
+#define DISP_BITMASK_HSA4          DISP_BIT_IB04
+#define DISP_BITMASK_HSA3          DISP_BIT_IB03
+#define DISP_BITMASK_HSA2          DISP_BIT_IB02
+#define DISP_BITMASK_HSA1          DISP_BIT_IB01
+#define DISP_BITMASK_HSA0          DISP_BIT_IB00
+/* DISP_HORZ_RAM_ADDR_POS_2_ADDR   Horizontal RAM Address Position (2) */
+#define DISP_BITMASK_HEA7          DISP_BIT_IB07
+#define DISP_BITMASK_HEA6          DISP_BIT_IB06
+#define DISP_BITMASK_HEA5          DISP_BIT_IB05
+#define DISP_BITMASK_HEA4          DISP_BIT_IB04
+#define DISP_BITMASK_HEA3          DISP_BIT_IB03
+#define DISP_BITMASK_HEA2          DISP_BIT_IB02
+#define DISP_BITMASK_HEA1          DISP_BIT_IB01
+#define DISP_BITMASK_HEA0          DISP_BIT_IB00
+/* DISP_VERT_RAM_ADDR_POS_1_ADDR   Vertical RAM Address Position (1) */
+#define DISP_BITMASK_VSA8          DISP_BIT_IB08
+#define DISP_BITMASK_VSA7          DISP_BIT_IB07
+#define DISP_BITMASK_VSA6          DISP_BIT_IB06
+#define DISP_BITMASK_VSA5          DISP_BIT_IB05
+#define DISP_BITMASK_VSA4          DISP_BIT_IB04
+#define DISP_BITMASK_VSA3          DISP_BIT_IB03
+#define DISP_BITMASK_VSA2          DISP_BIT_IB02
+#define DISP_BITMASK_VSA1          DISP_BIT_IB01
+#define DISP_BITMASK_VSA0          DISP_BIT_IB00
+/* DISP_VERT_RAM_ADDR_POS_2_ADDR   Vertical RAM Address Position (2) */
+#define DISP_BITMASK_VEA8          DISP_BIT_IB08
+#define DISP_BITMASK_VEA7          DISP_BIT_IB07
+#define DISP_BITMASK_VEA6          DISP_BIT_IB06
+#define DISP_BITMASK_VEA5          DISP_BIT_IB05
+#define DISP_BITMASK_VEA4          DISP_BIT_IB04
+#define DISP_BITMASK_VEA3          DISP_BIT_IB03
+#define DISP_BITMASK_VEA2          DISP_BIT_IB02
+#define DISP_BITMASK_VEA1          DISP_BIT_IB01
+#define DISP_BITMASK_VEA0          DISP_BIT_IB00
+static word disp_area_start_row;
+static word disp_area_end_row;
+static boolean disp_initialized = FALSE;
+/* For some reason the contrast set at init time is not good. Need to do
+* it again
+*/
+static boolean display_on = FALSE;
+
+static uint32 tmd20qvga_lcd_rev;
+uint16 tmd20qvga_panel_offset;
+
+#ifdef DISP_DEVICE_8BPP
+static word convert_8_to_16_tbl[256] = {
+        0x0000, 0x2000, 0x4000, 0x6000, 0x8000, 0xA000, 0xC000, 0xE000,
+        0x0100, 0x2100, 0x4100, 0x6100, 0x8100, 0xA100, 0xC100, 0xE100,
+        0x0200, 0x2200, 0x4200, 0x6200, 0x8200, 0xA200, 0xC200, 0xE200,
+        0x0300, 0x2300, 0x4300, 0x6300, 0x8300, 0xA300, 0xC300, 0xE300,
+        0x0400, 0x2400, 0x4400, 0x6400, 0x8400, 0xA400, 0xC400, 0xE400,
+        0x0500, 0x2500, 0x4500, 0x6500, 0x8500, 0xA500, 0xC500, 0xE500,
+        0x0600, 0x2600, 0x4600, 0x6600, 0x8600, 0xA600, 0xC600, 0xE600,
+        0x0700, 0x2700, 0x4700, 0x6700, 0x8700, 0xA700, 0xC700, 0xE700,
+        0x0008, 0x2008, 0x4008, 0x6008, 0x8008, 0xA008, 0xC008, 0xE008,
+        0x0108, 0x2108, 0x4108, 0x6108, 0x8108, 0xA108, 0xC108, 0xE108,
+        0x0208, 0x2208, 0x4208, 0x6208, 0x8208, 0xA208, 0xC208, 0xE208,
+        0x0308, 0x2308, 0x4308, 0x6308, 0x8308, 0xA308, 0xC308, 0xE308,
+        0x0408, 0x2408, 0x4408, 0x6408, 0x8408, 0xA408, 0xC408, 0xE408,
+        0x0508, 0x2508, 0x4508, 0x6508, 0x8508, 0xA508, 0xC508, 0xE508,
+        0x0608, 0x2608, 0x4608, 0x6608, 0x8608, 0xA608, 0xC608, 0xE608,
+        0x0708, 0x2708, 0x4708, 0x6708, 0x8708, 0xA708, 0xC708, 0xE708,
+        0x0010, 0x2010, 0x4010, 0x6010, 0x8010, 0xA010, 0xC010, 0xE010,
+        0x0110, 0x2110, 0x4110, 0x6110, 0x8110, 0xA110, 0xC110, 0xE110,
+        0x0210, 0x2210, 0x4210, 0x6210, 0x8210, 0xA210, 0xC210, 0xE210,
+        0x0310, 0x2310, 0x4310, 0x6310, 0x8310, 0xA310, 0xC310, 0xE310,
+        0x0410, 0x2410, 0x4410, 0x6410, 0x8410, 0xA410, 0xC410, 0xE410,
+        0x0510, 0x2510, 0x4510, 0x6510, 0x8510, 0xA510, 0xC510, 0xE510,
+        0x0610, 0x2610, 0x4610, 0x6610, 0x8610, 0xA610, 0xC610, 0xE610,
+        0x0710, 0x2710, 0x4710, 0x6710, 0x8710, 0xA710, 0xC710, 0xE710,
+        0x0018, 0x2018, 0x4018, 0x6018, 0x8018, 0xA018, 0xC018, 0xE018,
+        0x0118, 0x2118, 0x4118, 0x6118, 0x8118, 0xA118, 0xC118, 0xE118,
+        0x0218, 0x2218, 0x4218, 0x6218, 0x8218, 0xA218, 0xC218, 0xE218,
+        0x0318, 0x2318, 0x4318, 0x6318, 0x8318, 0xA318, 0xC318, 0xE318,
+        0x0418, 0x2418, 0x4418, 0x6418, 0x8418, 0xA418, 0xC418, 0xE418,
+        0x0518, 0x2518, 0x4518, 0x6518, 0x8518, 0xA518, 0xC518, 0xE518,
+        0x0618, 0x2618, 0x4618, 0x6618, 0x8618, 0xA618, 0xC618, 0xE618,
+        0x0718, 0x2718, 0x4718, 0x6718, 0x8718, 0xA718, 0xC718, 0xE718
+};
+#endif /* DISP_DEVICE_8BPP */
+
+static void tmd20qvga_disp_set_rect(int x, int y, int xres, int yres);
+static void tmd20qvga_disp_init(struct platform_device *pdev);
+static void tmd20qvga_disp_set_contrast(void);
+static void tmd20qvga_disp_set_display_area(word start_row, word end_row);
+static int tmd20qvga_disp_off(struct platform_device *pdev);
+static int tmd20qvga_disp_on(struct platform_device *pdev);
+static void tmd20qvga_set_revId(int);
+
+/* future use */
+void tmd20qvga_disp_clear_screen_area(word start_row, word end_row,
+                                      word start_column, word end_column);
+
+static void tmd20qvga_set_revId(int id)
+{
+
+        tmd20qvga_lcd_rev = id;
+
+        if (tmd20qvga_lcd_rev == 1)
+                tmd20qvga_panel_offset = 0x10;
+        else
+                tmd20qvga_panel_offset = 0;
+}
+
+static void tmd20qvga_disp_init(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+
+        if (disp_initialized)
+                return;
+
+        mfd = platform_get_drvdata(pdev);
+
+        DISP_CMD_PORT = mfd->cmd_port;
+        DISP_DATA_PORT = mfd->data_port;
+
+#ifdef TMD20QVGA_LCD_18BPP
+        tmd20qvga_set_revId(2);
+#else
+        tmd20qvga_set_revId(1);
+#endif
+
+        disp_initialized = TRUE;
+        tmd20qvga_disp_set_contrast();
+        tmd20qvga_disp_set_display_area(0, QVGA_HEIGHT - 1);
+}
+
+static void tmd20qvga_disp_set_rect(int x, int y, int xres, int yres)
+{
+        if (!disp_initialized)
+                return;
+
+        DISP_SET_RECT(y, y + yres - 1, x, x + xres - 1);
+
+        DISP_CMD_OUT(DISP_CMD_RAMWR);
+}
+
+static void tmd20qvga_disp_set_display_area(word start_row, word end_row)
+{
+        word start_driving = start_row;
+        word end_driving = end_row;
+
+        if (!disp_initialized)
+                return;
+
+        /* Range checking
+         */
+        if (end_driving >= QVGA_HEIGHT)
+                end_driving = QVGA_HEIGHT - 1;
+        if (start_driving > end_driving) {
+                /* Probably Backwards Switch */
+                start_driving = end_driving;
+                end_driving = start_row;        /* Has not changed */
+                if (end_driving >= QVGA_HEIGHT)
+                        end_driving = QVGA_HEIGHT - 1;
+        }
+
+        if ((start_driving == disp_area_start_row)
+            && (end_driving == disp_area_end_row))
+                return;
+
+        disp_area_start_row = start_driving;
+        disp_area_end_row = end_driving;
+
+        DISP_WRITE_OUT(DISP_SCREEN_1_DRV_POS_1_ADDR,
+                       DISP_VAL_IF(start_driving & 0x100,
+                                   DISP_BITMASK_SS18) |
+                       DISP_VAL_IF(start_driving & 0x080,
+                                   DISP_BITMASK_SS17) |
+                       DISP_VAL_IF(start_driving & 0x040,
+                                   DISP_BITMASK_SS16) |
+                       DISP_VAL_IF(start_driving & 0x020,
+                                   DISP_BITMASK_SS15) |
+                       DISP_VAL_IF(start_driving & 0x010,
+                                   DISP_BITMASK_SS14) |
+                       DISP_VAL_IF(start_driving & 0x008,
+                                   DISP_BITMASK_SS13) |
+                       DISP_VAL_IF(start_driving & 0x004,
+                                   DISP_BITMASK_SS12) |
+                       DISP_VAL_IF(start_driving & 0x002,
+                                   DISP_BITMASK_SS11) |
+                       DISP_VAL_IF(start_driving & 0x001, DISP_BITMASK_SS10));
+
+        DISP_WRITE_OUT(DISP_SCREEN_1_DRV_POS_2_ADDR,
+                        DISP_VAL_IF(end_driving & 0x100, DISP_BITMASK_SE18) |
+                        DISP_VAL_IF(end_driving & 0x080, DISP_BITMASK_SE17) |
+                        DISP_VAL_IF(end_driving & 0x040, DISP_BITMASK_SE16) |
+                        DISP_VAL_IF(end_driving & 0x020, DISP_BITMASK_SE15) |
+                        DISP_VAL_IF(end_driving & 0x010, DISP_BITMASK_SE14) |
+                        DISP_VAL_IF(end_driving & 0x008, DISP_BITMASK_SE13) |
+                        DISP_VAL_IF(end_driving & 0x004, DISP_BITMASK_SE12) |
+                        DISP_VAL_IF(end_driving & 0x002, DISP_BITMASK_SE11) |
+                        DISP_VAL_IF(end_driving & 0x001, DISP_BITMASK_SE10));
+}
+
+static int tmd20qvga_disp_off(struct platform_device *pdev)
+{
+        if (!disp_initialized)
+                tmd20qvga_disp_init(pdev);
+
+        if (display_on) {
+                if (tmd20qvga_lcd_rev == 2) {
+                        DISP_WRITE_OUT(DISP_POFF_LN_SETTING_ADDR, 0x000A);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0xFFEE);
+                        WAIT_MSEC(40);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0xF812);
+                        WAIT_MSEC(40);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0xE811);
+                        WAIT_MSEC(40);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0xC011);
+                        WAIT_MSEC(40);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0x4011);
+                        WAIT_MSEC(20);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0x0010);
+
+                } else {
+                        DISP_WRITE_OUT(DISP_POFF_LN_SETTING_ADDR, 0x000F);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0x0BFE);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0100);
+                        WAIT_MSEC(40);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0x0BED);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0100);
+                        WAIT_MSEC(40);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0x00CD);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0100);
+                        WAIT_MSEC(20);
+                        DISP_WRITE_OUT(DISP_START_OSCILLATION_ADDR, 0x0);
+                }
+
+                DISP_WRITE_OUT(DISP_MODE_SETTING_ADDR, 0x0004);
+                DISP_WRITE_OUT(DISP_MODE_SETTING_ADDR, 0x0000);
+
+                display_on = FALSE;
+        }
+
+        return 0;
+}
+
+static int tmd20qvga_disp_on(struct platform_device *pdev)
+{
+        if (!disp_initialized)
+                tmd20qvga_disp_init(pdev);
+
+        if (!display_on) {
+                /* Deep Stand-by -> Stand-by */
+                DISP_CMD_OUT(DISP_START_OSCILLATION_ADDR);
+                WAIT_MSEC(1);
+                DISP_CMD_OUT(DISP_START_OSCILLATION_ADDR);
+                WAIT_MSEC(1);
+                DISP_CMD_OUT(DISP_START_OSCILLATION_ADDR);
+                WAIT_MSEC(1);
+
+                /* OFF -> Deep Stan-By -> Stand-by */
+                /* let's change the state from "Stand-by" to "Sleep" */
+                DISP_WRITE_OUT(DISP_MODE_SETTING_ADDR, 0x0005);
+                WAIT_MSEC(1);
+
+                /* Sleep -> Displaying */
+                DISP_WRITE_OUT(DISP_START_OSCILLATION_ADDR, 0x0001);
+                DISP_WRITE_OUT(DISP_DRIVER_OUTPUT_CTL_ADDR, 0x0127);
+                DISP_WRITE_OUT(DISP_LCD_DRIVING_SIG_ADDR, 0x200);
+                /* fast write mode */
+                DISP_WRITE_OUT(DISP_ENTRY_MODE_ADDR, 0x0130);
+                if (tmd20qvga_lcd_rev == 2)
+                        DISP_WRITE_OUT(DISP_TMD_700_ADDR, 0x0003);
+                /* back porch = 14 + front porch = 2 --> 16 lines */
+                if (tmd20qvga_lcd_rev == 2) {
+#ifdef TMD20QVGA_LCD_18BPP
+                        /* 256k color */
+                        DISP_WRITE_OUT(DISP_DISPLAY_CTL_1_ADDR, 0x0000);
+#else
+                        /* 65k color */
+                        DISP_WRITE_OUT(DISP_DISPLAY_CTL_1_ADDR, 0x4000);
+#endif
+                        DISP_WRITE_OUT(DISP_DISPLAY_CTL_2_ADDR, 0x0302);
+                } else {
+#ifdef TMD20QVGA_LCD_18BPP
+                        /* 256k color */
+                        DISP_WRITE_OUT(DISP_DISPLAY_CTL_1_ADDR, 0x0004);
+#else
+                        /* 65k color */
+                        DISP_WRITE_OUT(DISP_DISPLAY_CTL_1_ADDR, 0x4004);
+#endif
+                        DISP_WRITE_OUT(DISP_DISPLAY_CTL_2_ADDR, 0x020E);
+                }
+                /* 16 bit one transfer */
+                if (tmd20qvga_lcd_rev == 2) {
+                        DISP_WRITE_OUT(DISP_EXT_DISPLAY_CTL_1_ADDR, 0x0000);
+                        DISP_WRITE_OUT(DISP_FRAME_CYCLE_CTL_ADDR, 0x0010);
+                        DISP_WRITE_OUT(DISP_LTPS_CTL_1_ADDR, 0x0302);
+                        DISP_WRITE_OUT(DISP_LTPS_CTL_2_ADDR, 0x0102);
+                        DISP_WRITE_OUT(DISP_LTPS_CTL_3_ADDR, 0x0000);
+                        DISP_WRITE_OUT(DISP_TMD_015_ADDR, 0x2000);
+
+                        DISP_WRITE_OUT(DISP_AMP_SETTING_ADDR, 0x0000);
+                        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_1_ADDR, 0x0403);
+                        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_2_ADDR, 0x0304);
+                        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_3_ADDR, 0x0403);
+                        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_4_ADDR, 0x0303);
+                        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_5_ADDR, 0x0101);
+                        DISP_WRITE_OUT(DISP_TMD_305_ADDR, 0);
+
+                        DISP_WRITE_OUT(DISP_SCREEN_1_DRV_POS_1_ADDR, 0x0000);
+                        DISP_WRITE_OUT(DISP_SCREEN_1_DRV_POS_2_ADDR, 0x013F);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_3_ADDR, 0x077D);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_4_ADDR, 0x0005);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_5_ADDR, 0x0000);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_6_ADDR, 0x0015);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0xC010);
+                        WAIT_MSEC(1);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_2_ADDR, 0x0001);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0xFFFE);
+                        WAIT_MSEC(60);
+                } else {
+                        DISP_WRITE_OUT(DISP_EXT_DISPLAY_CTL_1_ADDR, 0x0001);
+                        DISP_WRITE_OUT(DISP_FRAME_CYCLE_CTL_ADDR, 0x0010);
+                        DISP_WRITE_OUT(DISP_LTPS_CTL_1_ADDR, 0x0301);
+                        DISP_WRITE_OUT(DISP_LTPS_CTL_2_ADDR, 0x0001);
+                        DISP_WRITE_OUT(DISP_LTPS_CTL_3_ADDR, 0x0000);
+                        DISP_WRITE_OUT(DISP_AMP_SETTING_ADDR, 0x0000);
+                        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_1_ADDR, 0x0507);
+                        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_2_ADDR, 0x0405);
+                        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_3_ADDR, 0x0607);
+                        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_4_ADDR, 0x0502);
+                        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_5_ADDR, 0x0301);
+                        DISP_WRITE_OUT(DISP_SCREEN_1_DRV_POS_1_ADDR, 0x0000);
+                        DISP_WRITE_OUT(DISP_SCREEN_1_DRV_POS_2_ADDR, 0x013F);
+                        DISP_WRITE_OUT(DISP_POWER_CTL_3_ADDR, 0x0795);
+
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0102);
+                        WAIT_MSEC(1);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_4_ADDR, 0x0450);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0103);
+                        WAIT_MSEC(1);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_5_ADDR, 0x0008);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0104);
+                        WAIT_MSEC(1);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_6_ADDR, 0x0C00);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0105);
+                        WAIT_MSEC(1);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_7_ADDR, 0x0000);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0106);
+                        WAIT_MSEC(1);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0x0801);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0100);
+                        WAIT_MSEC(1);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_2_ADDR, 0x001F);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0101);
+                        WAIT_MSEC(60);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_2_ADDR, 0x009F);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0101);
+                        WAIT_MSEC(10);
+
+                        DISP_WRITE_OUT(DISP_HORZ_RAM_ADDR_POS_1_ADDR, 0x0010);
+                        DISP_WRITE_OUT(DISP_HORZ_RAM_ADDR_POS_2_ADDR, 0x00FF);
+                        DISP_WRITE_OUT(DISP_VERT_RAM_ADDR_POS_1_ADDR, 0x0000);
+                        DISP_WRITE_OUT(DISP_VERT_RAM_ADDR_POS_2_ADDR, 0x013F);
+                        /* RAM starts at address 0x10 */
+                        DISP_WRITE_OUT(DISP_RAM_ADDR_SET_1_ADDR, 0x0010);
+                        DISP_WRITE_OUT(DISP_RAM_ADDR_SET_2_ADDR, 0x0000);
+
+                        /* lcd controller uses internal clock, not ext. vsync */
+                        DISP_CMD_OUT(DISP_CMD_RAMWR);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0x0881);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0100);
+                        WAIT_MSEC(40);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0x0BE1);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0100);
+                        WAIT_MSEC(40);
+
+                        DISP_WRITE_OUT(DISP_POWER_CTL_1_ADDR, 0x0BFF);
+                        DISP_WRITE_OUT(DISP_POWER_SUPPLY_INTF_ADDR, 0x0100);
+                }
+                display_on = TRUE;
+        }
+
+        return 0;
+}
+
+static void tmd20qvga_disp_set_contrast(void)
+{
+#if (defined(TMD20QVGA_LCD_18BPP))
+
+        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_1_ADDR, 0x0403);
+        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_2_ADDR, 0x0302);
+        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_3_ADDR, 0x0403);
+        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_4_ADDR, 0x0303);
+        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_5_ADDR, 0x0F07);
+
+#else
+        int newcontrast = 0x46;
+
+        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_1_ADDR, 0x0403);
+
+        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_2_ADDR,
+                        DISP_VAL_IF(newcontrast & 0x0001, DISP_BITMASK_PKP20) |
+                        DISP_VAL_IF(newcontrast & 0x0002, DISP_BITMASK_PKP21) |
+                        DISP_VAL_IF(newcontrast & 0x0004, DISP_BITMASK_PKP22) |
+                        DISP_VAL_IF(newcontrast & 0x0010, DISP_BITMASK_PKP30) |
+                        DISP_VAL_IF(newcontrast & 0x0020, DISP_BITMASK_PKP31) |
+                        DISP_VAL_IF(newcontrast & 0x0040, DISP_BITMASK_PKP32));
+
+        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_3_ADDR,
+                        DISP_VAL_IF(newcontrast & 0x0010, DISP_BITMASK_PKP40) |
+                        DISP_VAL_IF(newcontrast & 0x0020, DISP_BITMASK_PKP41) |
+                        DISP_VAL_IF(newcontrast & 0x0040, DISP_BITMASK_PKP42) |
+                        DISP_VAL_IF(newcontrast & 0x0001, DISP_BITMASK_PKP50) |
+                        DISP_VAL_IF(newcontrast & 0x0002, DISP_BITMASK_PKP51) |
+                        DISP_VAL_IF(newcontrast & 0x0004, DISP_BITMASK_PKP52));
+
+        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_4_ADDR, 0x0303);
+        DISP_WRITE_OUT(DISP_GAMMA_CONTROL_5_ADDR, 0x0F07);
+
+#endif /* defined(TMD20QVGA_LCD_18BPP) */
+
+}       /* End disp_set_contrast */
+
+void tmd20qvga_disp_clear_screen_area
+    (word start_row, word end_row, word start_column, word end_column) {
+        int32 i;
+
+        /* Clear the display screen */
+        DISP_SET_RECT(start_row, end_row, start_column, end_column);
+        DISP_CMD_OUT(DISP_CMD_RAMWR);
+        i = (end_row - start_row + 1) * (end_column - start_column + 1);
+        for (; i > 0; i--)
+                DISP_DATA_OUT_16TO18BPP(0x0);
+}
+
+static int __init tmd20qvga_probe(struct platform_device *pdev)
+{
+        msm_fb_add_device(pdev);
+
+        return 0;
+}
+
+static struct platform_driver this_driver = {
+        .probe  = tmd20qvga_probe,
+        .driver = {
+                .name   = "ebi2_tmd_qvga",
+        },
+};
+
+static struct msm_fb_panel_data tmd20qvga_panel_data = {
+        .on = tmd20qvga_disp_on,
+        .off = tmd20qvga_disp_off,
+        .set_rect = tmd20qvga_disp_set_rect,
+};
+
+static struct platform_device this_device = {
+        .name   = "ebi2_tmd_qvga",
+        .id     = 0,
+        .dev    = {
+                .platform_data = &tmd20qvga_panel_data,
+        }
+};
+
+static int __init tmd20qvga_init(void)
+{
+        int ret;
+        struct msm_panel_info *pinfo;
+
+        ret = platform_driver_register(&this_driver);
+        if (!ret) {
+                pinfo = &tmd20qvga_panel_data.panel_info;
+                pinfo->xres = 240;
+                pinfo->yres = 320;
+                pinfo->type = EBI2_PANEL;
+                pinfo->pdest = DISPLAY_1;
+                pinfo->wait_cycle = 0x808000;
+#ifdef TMD20QVGA_LCD_18BPP
+                pinfo->bpp = 18;
+#else
+                pinfo->bpp = 16;
+#endif
+                pinfo->fb_num = 2;
+                pinfo->lcd.vsync_enable = TRUE;
+                pinfo->lcd.refx100 = 6000;
+                pinfo->lcd.v_back_porch = 16;
+                pinfo->lcd.v_front_porch = 4;
+                pinfo->lcd.v_pulse_width = 0;
+                pinfo->lcd.hw_vsync_mode = FALSE;
+                pinfo->lcd.vsync_notifier_period = 0;
+
+                ret = platform_device_register(&this_device);
+                if (ret)
+                        platform_driver_unregister(&this_driver);
+        }
+
+        return ret;
+}
+
+module_init(tmd20qvga_init);
diff --git a/drivers/staging/msm/hdmi_sii9022.c b/drivers/staging/msm/hdmi_sii9022.c
new file mode 100644
index 000000000000..6b82b56a77b6
--- /dev/null
+++ b/drivers/staging/msm/hdmi_sii9022.c
@@ -0,0 +1,248 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include "msm_fb.h"
+
+#define DEVICE_NAME "sii9022"
+#define SII9022_DEVICE_ID   0xB0
+
+struct sii9022_i2c_addr_data{
+        u8 addr;
+        u8 data;
+};
+
+/* video mode data */
+static u8 video_mode_data[] = {
+        0x00,
+        0xF9, 0x1C, 0x70, 0x17, 0x72, 0x06, 0xEE, 0x02,
+};
+
+static u8 avi_io_format[] = {
+        0x09,
+        0x00, 0x00,
+};
+
+/* power state */
+static struct sii9022_i2c_addr_data regset0[] = {
+        { 0x60, 0x04 },
+        { 0x63, 0x00 },
+        { 0x1E, 0x00 },
+};
+
+static u8 video_infoframe[] = {
+        0x0C,
+        0xF0, 0x00, 0x68, 0x00, 0x04, 0x00, 0x19, 0x00,
+        0xE9, 0x02, 0x04, 0x01, 0x04, 0x06,
+};
+
+/* configure audio */
+static struct sii9022_i2c_addr_data regset1[] = {
+        { 0x26, 0x90 },
+        { 0x20, 0x90 },
+        { 0x1F, 0x80 },
+        { 0x26, 0x80 },
+        { 0x24, 0x02 },
+        { 0x25, 0x0B },
+        { 0xBC, 0x02 },
+        { 0xBD, 0x24 },
+        { 0xBE, 0x02 },
+};
+
+/* enable audio */
+static u8 misc_infoframe[] = {
+        0xBF,
+        0xC2, 0x84, 0x01, 0x0A, 0x6F, 0x02, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+};
+
+/* set HDMI, active */
+static struct sii9022_i2c_addr_data regset2[] = {
+        { 0x1A, 0x01 },
+        { 0x3D, 0x00 },
+};
+
+static int send_i2c_data(struct i2c_client *client,
+                         struct sii9022_i2c_addr_data *regset,
+                         int size)
+{
+        int i;
+        int rc = 0;
+
+        for (i = 0; i < size; i++) {
+                rc = i2c_smbus_write_byte_data(
+                        client,
+                        regset[i].addr, regset[i].data);
+                if (rc)
+                        break;
+        }
+        return rc;
+}
+
+static int hdmi_sii_enable(struct i2c_client *client)
+{
+        int rc;
+        int retries = 10;
+        int count;
+
+        rc = i2c_smbus_write_byte_data(client, 0xC7, 0x00);
+        if (rc)
+                goto enable_exit;
+
+        do {
+                msleep(1);
+                rc = i2c_smbus_read_byte_data(client, 0x1B);
+        } while ((rc != SII9022_DEVICE_ID) && retries--);
+
+        if (rc != SII9022_DEVICE_ID)
+                return -ENODEV;
+
+        rc = i2c_smbus_write_byte_data(client, 0x1A, 0x11);
+        if (rc)
+                goto enable_exit;
+
+        count = ARRAY_SIZE(video_mode_data);
+        rc = i2c_master_send(client, video_mode_data, count);
+        if (rc != count) {
+                rc = -EIO;
+                goto enable_exit;
+        }
+
+        rc = i2c_smbus_write_byte_data(client, 0x08, 0x20);
+        if (rc)
+                goto enable_exit;
+        count = ARRAY_SIZE(avi_io_format);
+        rc = i2c_master_send(client, avi_io_format, count);
+        if (rc != count) {
+                rc = -EIO;
+                goto enable_exit;
+        }
+
+        rc = send_i2c_data(client, regset0, ARRAY_SIZE(regset0));
+        if (rc)
+                goto enable_exit;
+
+        count = ARRAY_SIZE(video_infoframe);
+        rc = i2c_master_send(client, video_infoframe, count);
+        if (rc != count) {
+                rc = -EIO;
+                goto enable_exit;
+        }
+
+        rc = send_i2c_data(client, regset1, ARRAY_SIZE(regset1));
+        if (rc)
+                goto enable_exit;
+
+        count = ARRAY_SIZE(misc_infoframe);
+        rc = i2c_master_send(client, misc_infoframe, count);
+        if (rc != count) {
+                rc = -EIO;
+                goto enable_exit;
+        }
+
+        rc = send_i2c_data(client, regset2, ARRAY_SIZE(regset2));
+        if (rc)
+                goto enable_exit;
+
+        return 0;
+enable_exit:
+        printk(KERN_ERR "%s: exited rc=%d\n", __func__, rc);
+        return rc;
+}
+
+static const struct i2c_device_id hmdi_sii_id[] = {
+        { DEVICE_NAME, 0 },
+        { }
+};
+
+static int hdmi_sii_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        int rc;
+
+        if (!i2c_check_functionality(client->adapter,
+                                     I2C_FUNC_SMBUS_BYTE | I2C_FUNC_I2C))
+                return -ENODEV;
+        rc = hdmi_sii_enable(client);
+        return rc;
+}
+
+
+static struct i2c_driver hdmi_sii_i2c_driver = {
+        .driver = {
+                .name = DEVICE_NAME,
+                .owner = THIS_MODULE,
+        },
+        .probe = hdmi_sii_probe,
+        .remove =  __exit_p(hdmi_sii_remove),
+        .id_table = hmdi_sii_id,
+};
+
+static int __init hdmi_sii_init(void)
+{
+        int ret;
+        struct msm_panel_info pinfo;
+
+        if (msm_fb_detect_client("hdmi_sii9022"))
+                return 0;
+
+        pinfo.xres = 1280;
+        pinfo.yres = 720;
+        pinfo.type = HDMI_PANEL;
+        pinfo.pdest = DISPLAY_1;
+        pinfo.wait_cycle = 0;
+        pinfo.bpp = 24;
+        pinfo.fb_num = 2;
+        pinfo.clk_rate = 74250000;
+
+        pinfo.lcdc.h_back_porch = 124;
+        pinfo.lcdc.h_front_porch = 110;
+        pinfo.lcdc.h_pulse_width = 136;
+        pinfo.lcdc.v_back_porch = 19;
+        pinfo.lcdc.v_front_porch = 5;
+        pinfo.lcdc.v_pulse_width = 6;
+        pinfo.lcdc.border_clr = 0;
+        pinfo.lcdc.underflow_clr = 0xff;
+        pinfo.lcdc.hsync_skew = 0;
+
+        ret = lcdc_device_register(&pinfo);
+        if (ret) {
+                printk(KERN_ERR "%s: failed to register device\n", __func__);
+                goto init_exit;
+        }
+
+        ret = i2c_add_driver(&hdmi_sii_i2c_driver);
+        if (ret)
+                printk(KERN_ERR "%s: failed to add i2c driver\n", __func__);
+
+init_exit:
+        return ret;
+}
+
+static void __exit hdmi_sii_exit(void)
+{
+        i2c_del_driver(&hdmi_sii_i2c_driver);
+}
+
+module_init(hdmi_sii_init);
+module_exit(hdmi_sii_exit);
+MODULE_LICENSE("GPL v2");
+MODULE_VERSION("0.1");
+MODULE_AUTHOR("Qualcomm Innovation Center, Inc.");
+MODULE_DESCRIPTION("SiI9022 HDMI driver");
+MODULE_ALIAS("platform:hdmi-sii9022");
diff --git a/drivers/staging/msm/lcdc.c b/drivers/staging/msm/lcdc.c
new file mode 100644
index 000000000000..735280ab72cb
--- /dev/null
+++ b/drivers/staging/msm/lcdc.c
@@ -0,0 +1,239 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/uaccess.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/pm_qos_params.h>
+
+#include "msm_fb.h"
+
+static int lcdc_probe(struct platform_device *pdev);
+static int lcdc_remove(struct platform_device *pdev);
+
+static int lcdc_off(struct platform_device *pdev);
+static int lcdc_on(struct platform_device *pdev);
+
+static struct platform_device *pdev_list[MSM_FB_MAX_DEV_LIST];
+static int pdev_list_cnt;
+
+static struct clk *mdp_lcdc_pclk_clk;
+static struct clk *mdp_lcdc_pad_pclk_clk;
+
+int mdp_lcdc_pclk_clk_rate;
+int mdp_lcdc_pad_pclk_clk_rate;
+
+static struct platform_driver lcdc_driver = {
+        .probe = lcdc_probe,
+        .remove = lcdc_remove,
+        .suspend = NULL,
+        .resume = NULL,
+        .shutdown = NULL,
+        .driver = {
+                   .name = "lcdc",
+                   },
+};
+
+static struct lcdc_platform_data *lcdc_pdata;
+
+static int lcdc_off(struct platform_device *pdev)
+{
+        int ret = 0;
+
+        ret = panel_next_off(pdev);
+
+        clk_disable(mdp_lcdc_pclk_clk);
+        clk_disable(mdp_lcdc_pad_pclk_clk);
+
+        if (lcdc_pdata && lcdc_pdata->lcdc_power_save)
+                lcdc_pdata->lcdc_power_save(0);
+
+        if (lcdc_pdata && lcdc_pdata->lcdc_gpio_config)
+                ret = lcdc_pdata->lcdc_gpio_config(0);
+
+//      pm_qos_update_requirement(PM_QOS_SYSTEM_BUS_FREQ , "lcdc",
+//                                      PM_QOS_DEFAULT_VALUE);
+
+        return ret;
+}
+
+static int lcdc_on(struct platform_device *pdev)
+{
+        int ret = 0;
+        struct msm_fb_data_type *mfd;
+        unsigned long panel_pixclock_freq , pm_qos_freq;
+
+        mfd = platform_get_drvdata(pdev);
+        panel_pixclock_freq = mfd->fbi->var.pixclock;
+
+        if (panel_pixclock_freq > 58000000)
+                /* pm_qos_freq should be in Khz */
+                pm_qos_freq = panel_pixclock_freq / 1000 ;
+        else
+                pm_qos_freq = 58000;
+
+//      pm_qos_update_requirement(PM_QOS_SYSTEM_BUS_FREQ , "lcdc",
+//                                              pm_qos_freq);
+        mfd = platform_get_drvdata(pdev);
+
+        clk_enable(mdp_lcdc_pclk_clk);
+        clk_enable(mdp_lcdc_pad_pclk_clk);
+
+        if (lcdc_pdata && lcdc_pdata->lcdc_power_save)
+                lcdc_pdata->lcdc_power_save(1);
+        if (lcdc_pdata && lcdc_pdata->lcdc_gpio_config)
+                ret = lcdc_pdata->lcdc_gpio_config(1);
+
+        clk_set_rate(mdp_lcdc_pclk_clk, mfd->fbi->var.pixclock);
+        clk_set_rate(mdp_lcdc_pad_pclk_clk, mfd->fbi->var.pixclock);
+        mdp_lcdc_pclk_clk_rate = clk_get_rate(mdp_lcdc_pclk_clk);
+        mdp_lcdc_pad_pclk_clk_rate = clk_get_rate(mdp_lcdc_pad_pclk_clk);
+
+        ret = panel_next_on(pdev);
+        return ret;
+}
+
+static int lcdc_probe(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+        struct fb_info *fbi;
+        struct platform_device *mdp_dev = NULL;
+        struct msm_fb_panel_data *pdata = NULL;
+        int rc;
+
+        if (pdev->id == 0) {
+                lcdc_pdata = pdev->dev.platform_data;
+                return 0;
+        }
+
+        mfd = platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        if (pdev_list_cnt >= MSM_FB_MAX_DEV_LIST)
+                return -ENOMEM;
+
+        mdp_dev = platform_device_alloc("mdp", pdev->id);
+        if (!mdp_dev)
+                return -ENOMEM;
+
+        /*
+         * link to the latest pdev
+         */
+        mfd->pdev = mdp_dev;
+        mfd->dest = DISPLAY_LCDC;
+
+        /*
+         * alloc panel device data
+         */
+        if (platform_device_add_data
+            (mdp_dev, pdev->dev.platform_data,
+             sizeof(struct msm_fb_panel_data))) {
+                printk(KERN_ERR "lcdc_probe: platform_device_add_data failed!\n");
+                platform_device_put(mdp_dev);
+                return -ENOMEM;
+        }
+        /*
+         * data chain
+         */
+        pdata = (struct msm_fb_panel_data *)mdp_dev->dev.platform_data;
+        pdata->on = lcdc_on;
+        pdata->off = lcdc_off;
+        pdata->next = pdev;
+
+        /*
+         * get/set panel specific fb info
+         */
+        mfd->panel_info = pdata->panel_info;
+        mfd->fb_imgType = MDP_RGB_565;
+
+        fbi = mfd->fbi;
+        fbi->var.pixclock = mfd->panel_info.clk_rate;
+        fbi->var.left_margin = mfd->panel_info.lcdc.h_back_porch;
+        fbi->var.right_margin = mfd->panel_info.lcdc.h_front_porch;
+        fbi->var.upper_margin = mfd->panel_info.lcdc.v_back_porch;
+        fbi->var.lower_margin = mfd->panel_info.lcdc.v_front_porch;
+        fbi->var.hsync_len = mfd->panel_info.lcdc.h_pulse_width;
+        fbi->var.vsync_len = mfd->panel_info.lcdc.v_pulse_width;
+
+        /*
+         * set driver data
+         */
+        platform_set_drvdata(mdp_dev, mfd);
+
+        /*
+         * register in mdp driver
+         */
+        rc = platform_device_add(mdp_dev);
+        if (rc)
+                goto lcdc_probe_err;
+
+        pdev_list[pdev_list_cnt++] = pdev;
+                return 0;
+
+lcdc_probe_err:
+        platform_device_put(mdp_dev);
+        return rc;
+}
+
+static int lcdc_remove(struct platform_device *pdev)
+{
+//      pm_qos_remove_requirement(PM_QOS_SYSTEM_BUS_FREQ , "lcdc");
+        return 0;
+}
+
+static int lcdc_register_driver(void)
+{
+        return platform_driver_register(&lcdc_driver);
+}
+
+static int __init lcdc_driver_init(void)
+{
+        mdp_lcdc_pclk_clk = clk_get(NULL, "mdp_lcdc_pclk_clk");
+        if (IS_ERR(mdp_lcdc_pclk_clk)) {
+                printk(KERN_ERR "error: can't get mdp_lcdc_pclk_clk!\n");
+                return IS_ERR(mdp_lcdc_pclk_clk);
+        }
+        mdp_lcdc_pad_pclk_clk = clk_get(NULL, "mdp_lcdc_pad_pclk_clk");
+        if (IS_ERR(mdp_lcdc_pad_pclk_clk)) {
+                printk(KERN_ERR "error: can't get mdp_lcdc_pad_pclk_clk!\n");
+                return IS_ERR(mdp_lcdc_pad_pclk_clk);
+        }
+//      pm_qos_add_requirement(PM_QOS_SYSTEM_BUS_FREQ , "lcdc",
+//                              PM_QOS_DEFAULT_VALUE);
+        return lcdc_register_driver();
+}
+
+module_init(lcdc_driver_init);
diff --git a/drivers/staging/msm/lcdc_external.c b/drivers/staging/msm/lcdc_external.c
new file mode 100644
index 000000000000..45ff78527111
--- /dev/null
+++ b/drivers/staging/msm/lcdc_external.c
@@ -0,0 +1,54 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+
+static int __init lcdc_external_init(void)
+{
+        int ret;
+        struct msm_panel_info pinfo;
+
+        if (msm_fb_detect_client("lcdc_external"))
+                return 0;
+
+        pinfo.xres = 1280;
+        pinfo.yres = 720;
+        pinfo.type = LCDC_PANEL;
+        pinfo.pdest = DISPLAY_1;
+        pinfo.wait_cycle = 0;
+        pinfo.bpp = 24;
+        pinfo.fb_num = 2;
+        pinfo.clk_rate = 74250000;
+
+        pinfo.lcdc.h_back_porch = 124;
+        pinfo.lcdc.h_front_porch = 110;
+        pinfo.lcdc.h_pulse_width = 136;
+        pinfo.lcdc.v_back_porch = 19;
+        pinfo.lcdc.v_front_porch = 5;
+        pinfo.lcdc.v_pulse_width = 6;
+        pinfo.lcdc.border_clr = 0;      /* blk */
+        pinfo.lcdc.underflow_clr = 0xff;        /* blue */
+        pinfo.lcdc.hsync_skew = 0;
+
+        ret = lcdc_device_register(&pinfo);
+        if (ret)
+                printk(KERN_ERR "%s: failed to register device!\n", __func__);
+
+        return ret;
+}
+
+module_init(lcdc_external_init);
diff --git a/drivers/staging/msm/lcdc_gordon.c b/drivers/staging/msm/lcdc_gordon.c
new file mode 100644
index 000000000000..399ec8c791ec
--- /dev/null
+++ b/drivers/staging/msm/lcdc_gordon.c
@@ -0,0 +1,446 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/delay.h>
+#include <mach/gpio.h>
+#include "msm_fb.h"
+
+/* registers */
+#define GORDON_REG_NOP          0x00
+#define GORDON_REG_IMGCTL1      0x10
+#define GORDON_REG_IMGCTL2      0x11
+#define GORDON_REG_IMGSET1      0x12
+#define GORDON_REG_IMGSET2      0x13
+#define GORDON_REG_IVBP1        0x14
+#define GORDON_REG_IHBP1        0x15
+#define GORDON_REG_IVNUM1       0x16
+#define GORDON_REG_IHNUM1       0x17
+#define GORDON_REG_IVBP2        0x18
+#define GORDON_REG_IHBP2        0x19
+#define GORDON_REG_IVNUM2       0x1A
+#define GORDON_REG_IHNUM2       0x1B
+#define GORDON_REG_LCDIFCTL1    0x30
+#define GORDON_REG_VALTRAN      0x31
+#define GORDON_REG_AVCTL        0x33
+#define GORDON_REG_LCDIFCTL2    0x34
+#define GORDON_REG_LCDIFCTL3    0x35
+#define GORDON_REG_LCDIFSET1    0x36
+#define GORDON_REG_PCCTL        0x3C
+#define GORDON_REG_TPARAM1      0x40
+#define GORDON_REG_TLCDIF1      0x41
+#define GORDON_REG_TSSPB_ST1    0x42
+#define GORDON_REG_TSSPB_ED1    0x43
+#define GORDON_REG_TSCK_ST1     0x44
+#define GORDON_REG_TSCK_WD1     0x45
+#define GORDON_REG_TGSPB_VST1   0x46
+#define GORDON_REG_TGSPB_VED1   0x47
+#define GORDON_REG_TGSPB_CH1    0x48
+#define GORDON_REG_TGCK_ST1     0x49
+#define GORDON_REG_TGCK_ED1     0x4A
+#define GORDON_REG_TPCTL_ST1    0x4B
+#define GORDON_REG_TPCTL_ED1    0x4C
+#define GORDON_REG_TPCHG_ED1    0x4D
+#define GORDON_REG_TCOM_CH1     0x4E
+#define GORDON_REG_THBP1        0x4F
+#define GORDON_REG_TPHCTL1      0x50
+#define GORDON_REG_EVPH1        0x51
+#define GORDON_REG_EVPL1        0x52
+#define GORDON_REG_EVNH1        0x53
+#define GORDON_REG_EVNL1        0x54
+#define GORDON_REG_TBIAS1       0x55
+#define GORDON_REG_TPARAM2      0x56
+#define GORDON_REG_TLCDIF2      0x57
+#define GORDON_REG_TSSPB_ST2    0x58
+#define GORDON_REG_TSSPB_ED2    0x59
+#define GORDON_REG_TSCK_ST2     0x5A
+#define GORDON_REG_TSCK_WD2     0x5B
+#define GORDON_REG_TGSPB_VST2   0x5C
+#define GORDON_REG_TGSPB_VED2   0x5D
+#define GORDON_REG_TGSPB_CH2    0x5E
+#define GORDON_REG_TGCK_ST2     0x5F
+#define GORDON_REG_TGCK_ED2     0x60
+#define GORDON_REG_TPCTL_ST2    0x61
+#define GORDON_REG_TPCTL_ED2    0x62
+#define GORDON_REG_TPCHG_ED2    0x63
+#define GORDON_REG_TCOM_CH2     0x64
+#define GORDON_REG_THBP2        0x65
+#define GORDON_REG_TPHCTL2      0x66
+#define GORDON_REG_POWCTL       0x80
+
+static int lcdc_gordon_panel_off(struct platform_device *pdev);
+
+static int spi_cs;
+static int spi_sclk;
+static int spi_sdo;
+static int spi_sdi;
+static int spi_dac;
+static unsigned char bit_shift[8] = { (1 << 7), /* MSB */
+        (1 << 6),
+        (1 << 5),
+        (1 << 4),
+        (1 << 3),
+        (1 << 2),
+        (1 << 1),
+        (1 << 0)                               /* LSB */
+};
+
+struct gordon_state_type{
+        boolean disp_initialized;
+        boolean display_on;
+        boolean disp_powered_up;
+};
+
+static struct gordon_state_type gordon_state = { 0 };
+static struct msm_panel_common_pdata *lcdc_gordon_pdata;
+
+static void serigo(uint16 reg, uint8 data)
+{
+        unsigned int tx_val = ((0x00FF & reg) << 8) | data;
+        unsigned char i, val = 0;
+
+        /* Enable the Chip Select */
+        gpio_set_value(spi_cs, 1);
+        udelay(33);
+
+        /* Transmit it in two parts, Higher Byte first, then Lower Byte */
+        val = (unsigned char)((tx_val & 0xFF00) >> 8);
+
+        /* Clock should be Low before entering ! */
+        for (i = 0; i < 8; i++) {
+                /* #1: Drive the Data (High or Low) */
+                if (val & bit_shift[i])
+                        gpio_set_value(spi_sdi, 1);
+                else
+                        gpio_set_value(spi_sdi, 0);
+
+                /* #2: Drive the Clk High and then Low */
+                udelay(33);
+                gpio_set_value(spi_sclk, 1);
+                udelay(33);
+                gpio_set_value(spi_sclk, 0);
+        }
+
+        /* Idle state of SDO (MOSI) is Low */
+        gpio_set_value(spi_sdi, 0);
+        /* ..then Lower Byte */
+        val = (uint8) (tx_val & 0x00FF);
+        /* Before we enter here the Clock should be Low ! */
+
+        for (i = 0; i < 8; i++) {
+                /* #1: Drive the Data (High or Low) */
+                if (val & bit_shift[i])
+                        gpio_set_value(spi_sdi, 1);
+                else
+                        gpio_set_value(spi_sdi, 0);
+
+                /* #2: Drive the Clk High and then Low */
+                udelay(33);
+
+                gpio_set_value(spi_sclk, 1);
+                udelay(33);
+                gpio_set_value(spi_sclk, 0);
+        }
+
+        /* Idle state of SDO (MOSI) is Low */
+        gpio_set_value(spi_sdi, 0);
+
+        /* Now Disable the Chip Select */
+        udelay(33);
+        gpio_set_value(spi_cs, 0);
+}
+
+static void spi_init(void)
+{
+        /* Setting the Default GPIO's */
+        spi_sclk = *(lcdc_gordon_pdata->gpio_num);
+        spi_cs   = *(lcdc_gordon_pdata->gpio_num + 1);
+        spi_sdi  = *(lcdc_gordon_pdata->gpio_num + 2);
+        spi_sdo  = *(lcdc_gordon_pdata->gpio_num + 3);
+
+        /* Set the output so that we dont disturb the slave device */
+        gpio_set_value(spi_sclk, 0);
+        gpio_set_value(spi_sdi, 0);
+
+        /* Set the Chip Select De-asserted */
+        gpio_set_value(spi_cs, 0);
+
+}
+
+static void gordon_disp_powerup(void)
+{
+        if (!gordon_state.disp_powered_up && !gordon_state.display_on) {
+                /* Reset the hardware first */
+                /* Include DAC power up implementation here */
+              gordon_state.disp_powered_up = TRUE;
+        }
+}
+
+static void gordon_init(void)
+{
+        /* Image interface settings */
+        serigo(GORDON_REG_IMGCTL2, 0x00);
+        serigo(GORDON_REG_IMGSET1, 0x00);
+
+        /* Exchange the RGB signal for J510(Softbank mobile) */
+        serigo(GORDON_REG_IMGSET2, 0x12);
+        serigo(GORDON_REG_LCDIFSET1, 0x00);
+
+        /* Pre-charge settings */
+        serigo(GORDON_REG_PCCTL, 0x09);
+        serigo(GORDON_REG_LCDIFCTL2, 0x7B);
+
+        mdelay(1);
+}
+
+static void gordon_disp_on(void)
+{
+        if (gordon_state.disp_powered_up && !gordon_state.display_on) {
+                gordon_init();
+                mdelay(20);
+                /* gordon_dispmode setting */
+                serigo(GORDON_REG_TPARAM1, 0x30);
+                serigo(GORDON_REG_TLCDIF1, 0x00);
+                serigo(GORDON_REG_TSSPB_ST1, 0x8B);
+                serigo(GORDON_REG_TSSPB_ED1, 0x93);
+                serigo(GORDON_REG_TSCK_ST1, 0x88);
+                serigo(GORDON_REG_TSCK_WD1, 0x00);
+                serigo(GORDON_REG_TGSPB_VST1, 0x01);
+                serigo(GORDON_REG_TGSPB_VED1, 0x02);
+                serigo(GORDON_REG_TGSPB_CH1, 0x5E);
+                serigo(GORDON_REG_TGCK_ST1, 0x80);
+                serigo(GORDON_REG_TGCK_ED1, 0x3C);
+                serigo(GORDON_REG_TPCTL_ST1, 0x50);
+                serigo(GORDON_REG_TPCTL_ED1, 0x74);
+                serigo(GORDON_REG_TPCHG_ED1, 0x78);
+                serigo(GORDON_REG_TCOM_CH1, 0x50);
+                serigo(GORDON_REG_THBP1, 0x84);
+                serigo(GORDON_REG_TPHCTL1, 0x00);
+                serigo(GORDON_REG_EVPH1, 0x70);
+                serigo(GORDON_REG_EVPL1, 0x64);
+                serigo(GORDON_REG_EVNH1, 0x56);
+                serigo(GORDON_REG_EVNL1, 0x48);
+                serigo(GORDON_REG_TBIAS1, 0x88);
+
+                /* QVGA settings */
+                serigo(GORDON_REG_TPARAM2, 0x28);
+                serigo(GORDON_REG_TLCDIF2, 0x14);
+                serigo(GORDON_REG_TSSPB_ST2, 0x49);
+                serigo(GORDON_REG_TSSPB_ED2, 0x4B);
+                serigo(GORDON_REG_TSCK_ST2, 0x4A);
+                serigo(GORDON_REG_TSCK_WD2, 0x02);
+                serigo(GORDON_REG_TGSPB_VST2, 0x02);
+                serigo(GORDON_REG_TGSPB_VED2, 0x03);
+                serigo(GORDON_REG_TGSPB_CH2, 0x2F);
+                serigo(GORDON_REG_TGCK_ST2, 0x40);
+                serigo(GORDON_REG_TGCK_ED2, 0x1E);
+                serigo(GORDON_REG_TPCTL_ST2, 0x2C);
+                serigo(GORDON_REG_TPCTL_ED2, 0x3A);
+                serigo(GORDON_REG_TPCHG_ED2, 0x3C);
+                serigo(GORDON_REG_TCOM_CH2, 0x28);
+                serigo(GORDON_REG_THBP2, 0x4D);
+                serigo(GORDON_REG_TPHCTL2, 0x1A);
+
+                /* VGA settings */
+                serigo(GORDON_REG_IVBP1, 0x02);
+                serigo(GORDON_REG_IHBP1, 0x90);
+                serigo(GORDON_REG_IVNUM1, 0xA0);
+                serigo(GORDON_REG_IHNUM1, 0x78);
+
+                /* QVGA settings */
+                serigo(GORDON_REG_IVBP2, 0x02);
+                serigo(GORDON_REG_IHBP2, 0x48);
+                serigo(GORDON_REG_IVNUM2, 0x50);
+                serigo(GORDON_REG_IHNUM2, 0x3C);
+
+                /* Gordon Charge pump settings and ON */
+                serigo(GORDON_REG_POWCTL, 0x03);
+                mdelay(15);
+                serigo(GORDON_REG_POWCTL, 0x07);
+                mdelay(15);
+
+                serigo(GORDON_REG_POWCTL, 0x0F);
+                mdelay(15);
+
+                serigo(GORDON_REG_AVCTL, 0x03);
+                mdelay(15);
+
+                serigo(GORDON_REG_POWCTL, 0x1F);
+                mdelay(15);
+
+                serigo(GORDON_REG_POWCTL, 0x5F);
+                mdelay(15);
+
+                serigo(GORDON_REG_POWCTL, 0x7F);
+                mdelay(15);
+
+                serigo(GORDON_REG_LCDIFCTL1, 0x02);
+                mdelay(15);
+
+                serigo(GORDON_REG_IMGCTL1, 0x00);
+                mdelay(15);
+
+                serigo(GORDON_REG_LCDIFCTL3, 0x00);
+                mdelay(15);
+
+                serigo(GORDON_REG_VALTRAN, 0x01);
+                mdelay(15);
+
+                serigo(GORDON_REG_LCDIFCTL1, 0x03);
+                mdelay(1);
+                gordon_state.display_on = TRUE;
+        }
+}
+
+static int lcdc_gordon_panel_on(struct platform_device *pdev)
+{
+        if (!gordon_state.disp_initialized) {
+                /* Configure reset GPIO that drives DAC */
+                lcdc_gordon_pdata->panel_config_gpio(1);
+                spi_dac = *(lcdc_gordon_pdata->gpio_num + 4);
+                gpio_set_value(spi_dac, 0);
+                udelay(15);
+                gpio_set_value(spi_dac, 1);
+                spi_init();     /* LCD needs SPI */
+                gordon_disp_powerup();
+                gordon_disp_on();
+                gordon_state.disp_initialized = TRUE;
+        }
+        return 0;
+}
+
+static int lcdc_gordon_panel_off(struct platform_device *pdev)
+{
+        if (gordon_state.disp_powered_up && gordon_state.display_on) {
+                serigo(GORDON_REG_LCDIFCTL2, 0x7B);
+                serigo(GORDON_REG_VALTRAN, 0x01);
+                serigo(GORDON_REG_LCDIFCTL1, 0x02);
+                serigo(GORDON_REG_LCDIFCTL3, 0x01);
+                mdelay(20);
+                serigo(GORDON_REG_VALTRAN, 0x01);
+                serigo(GORDON_REG_IMGCTL1, 0x01);
+                serigo(GORDON_REG_LCDIFCTL1, 0x00);
+                mdelay(20);
+
+                serigo(GORDON_REG_POWCTL, 0x1F);
+                mdelay(40);
+
+                serigo(GORDON_REG_POWCTL, 0x07);
+                mdelay(40);
+
+                serigo(GORDON_REG_POWCTL, 0x03);
+                mdelay(40);
+
+                serigo(GORDON_REG_POWCTL, 0x00);
+                mdelay(40);
+                lcdc_gordon_pdata->panel_config_gpio(0);
+                gordon_state.display_on = FALSE;
+                gordon_state.disp_initialized = FALSE;
+        }
+        return 0;
+}
+
+static void lcdc_gordon_set_backlight(struct msm_fb_data_type *mfd)
+{
+                int bl_level = mfd->bl_level;
+
+                if (bl_level <= 1) {
+                        /* keep back light OFF */
+                        serigo(GORDON_REG_LCDIFCTL2, 0x0B);
+                        udelay(15);
+                        serigo(GORDON_REG_VALTRAN, 0x01);
+                } else {
+                        /* keep back light ON */
+                        serigo(GORDON_REG_LCDIFCTL2, 0x7B);
+                        udelay(15);
+                        serigo(GORDON_REG_VALTRAN, 0x01);
+                }
+}
+
+static int __init gordon_probe(struct platform_device *pdev)
+{
+        if (pdev->id == 0) {
+                lcdc_gordon_pdata = pdev->dev.platform_data;
+                return 0;
+        }
+        msm_fb_add_device(pdev);
+        return 0;
+}
+
+static struct platform_driver this_driver = {
+        .probe  = gordon_probe,
+        .driver = {
+                .name   = "lcdc_gordon_vga",
+        },
+};
+
+static struct msm_fb_panel_data gordon_panel_data = {
+        .on = lcdc_gordon_panel_on,
+        .off = lcdc_gordon_panel_off,
+        .set_backlight = lcdc_gordon_set_backlight,
+};
+
+static struct platform_device this_device = {
+        .name   = "lcdc_gordon_vga",
+        .id     = 1,
+        .dev    = {
+                .platform_data = &gordon_panel_data,
+        }
+};
+
+static int __init lcdc_gordon_panel_init(void)
+{
+        int ret;
+        struct msm_panel_info *pinfo;
+
+#ifdef CONFIG_FB_MSM_TRY_MDDI_CATCH_LCDC_PRISM
+        if (msm_fb_detect_client("lcdc_gordon_vga"))
+                return 0;
+#endif
+        ret = platform_driver_register(&this_driver);
+        if (ret)
+                return ret;
+
+        pinfo = &gordon_panel_data.panel_info;
+        pinfo->xres = 480;
+        pinfo->yres = 640;
+        pinfo->type = LCDC_PANEL;
+        pinfo->pdest = DISPLAY_1;
+        pinfo->wait_cycle = 0;
+        pinfo->bpp = 24;
+        pinfo->fb_num = 2;
+        pinfo->clk_rate = 24500000;
+        pinfo->bl_max = 4;
+        pinfo->bl_min = 1;
+
+        pinfo->lcdc.h_back_porch = 84;
+        pinfo->lcdc.h_front_porch = 33;
+        pinfo->lcdc.h_pulse_width = 60;
+        pinfo->lcdc.v_back_porch = 0;
+        pinfo->lcdc.v_front_porch = 2;
+        pinfo->lcdc.v_pulse_width = 2;
+        pinfo->lcdc.border_clr = 0;     /* blk */
+        pinfo->lcdc.underflow_clr = 0xff;       /* blue */
+        pinfo->lcdc.hsync_skew = 0;
+
+        ret = platform_device_register(&this_device);
+        if (ret)
+                platform_driver_unregister(&this_driver);
+
+        return ret;
+}
+
+module_init(lcdc_gordon_panel_init);
diff --git a/drivers/staging/msm/lcdc_grapefruit.c b/drivers/staging/msm/lcdc_grapefruit.c
new file mode 100644
index 000000000000..7284649ea0ae
--- /dev/null
+++ b/drivers/staging/msm/lcdc_grapefruit.c
@@ -0,0 +1,60 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+
+#ifdef CONFIG_FB_MSM_TRY_MDDI_CATCH_LCDC_PRISM
+#include "mddihosti.h"
+#endif
+
+static int __init lcdc_grapefruit_init(void)
+{
+        int ret;
+        struct msm_panel_info pinfo;
+
+#ifdef CONFIG_FB_MSM_TRY_MDDI_CATCH_LCDC_PRISM
+        if (msm_fb_detect_client("lcdc_grapefruit_vga"))
+                return 0;
+#endif
+
+        pinfo.xres = 1024;
+        pinfo.yres = 600;
+        pinfo.type = LCDC_PANEL;
+        pinfo.pdest = DISPLAY_1;
+        pinfo.wait_cycle = 0;
+        pinfo.bpp = 18;
+        pinfo.fb_num = 2;
+        pinfo.clk_rate = 40000000;
+
+        pinfo.lcdc.h_back_porch = 88;
+        pinfo.lcdc.h_front_porch = 40;
+        pinfo.lcdc.h_pulse_width = 128;
+        pinfo.lcdc.v_back_porch = 23;
+        pinfo.lcdc.v_front_porch = 1;
+        pinfo.lcdc.v_pulse_width = 4;
+        pinfo.lcdc.border_clr = 0;      /* blk */
+        pinfo.lcdc.underflow_clr = 0xff;        /* blue */
+        pinfo.lcdc.hsync_skew = 0;
+
+        ret = lcdc_device_register(&pinfo);
+        if (ret)
+                printk(KERN_ERR "%s: failed to register device!\n", __func__);
+
+        return ret;
+}
+
+module_init(lcdc_grapefruit_init);
diff --git a/drivers/staging/msm/lcdc_panel.c b/drivers/staging/msm/lcdc_panel.c
new file mode 100644
index 000000000000..b40974e1f27c
--- /dev/null
+++ b/drivers/staging/msm/lcdc_panel.c
@@ -0,0 +1,88 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+
+static int lcdc_panel_on(struct platform_device *pdev)
+{
+        return 0;
+}
+
+static int lcdc_panel_off(struct platform_device *pdev)
+{
+        return 0;
+}
+
+static int __init lcdc_panel_probe(struct platform_device *pdev)
+{
+        msm_fb_add_device(pdev);
+
+        return 0;
+}
+
+static struct platform_driver this_driver = {
+        .probe  = lcdc_panel_probe,
+        .driver = {
+                .name   = "lcdc_panel",
+        },
+};
+
+static struct msm_fb_panel_data lcdc_panel_data = {
+        .on = lcdc_panel_on,
+        .off = lcdc_panel_off,
+};
+
+static int lcdc_dev_id;
+
+int lcdc_device_register(struct msm_panel_info *pinfo)
+{
+        struct platform_device *pdev = NULL;
+        int ret;
+
+        pdev = platform_device_alloc("lcdc_panel", ++lcdc_dev_id);
+        if (!pdev)
+                return -ENOMEM;
+
+        lcdc_panel_data.panel_info = *pinfo;
+        ret = platform_device_add_data(pdev, &lcdc_panel_data,
+                sizeof(lcdc_panel_data));
+        if (ret) {
+                printk(KERN_ERR
+                  "%s: platform_device_add_data failed!\n", __func__);
+                goto err_device_put;
+        }
+
+        ret = platform_device_add(pdev);
+        if (ret) {
+                printk(KERN_ERR
+                  "%s: platform_device_register failed!\n", __func__);
+                goto err_device_put;
+        }
+
+        return 0;
+
+err_device_put:
+        platform_device_put(pdev);
+        return ret;
+}
+
+static int __init lcdc_panel_init(void)
+{
+        return platform_driver_register(&this_driver);
+}
+
+module_init(lcdc_panel_init);
diff --git a/drivers/staging/msm/lcdc_prism.c b/drivers/staging/msm/lcdc_prism.c
new file mode 100644
index 000000000000..d102c98447c1
--- /dev/null
+++ b/drivers/staging/msm/lcdc_prism.c
@@ -0,0 +1,64 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+
+#ifdef CONFIG_FB_MSM_TRY_MDDI_CATCH_LCDC_PRISM
+#include "mddihosti.h"
+#endif
+
+static int __init lcdc_prism_init(void)
+{
+        int ret;
+        struct msm_panel_info pinfo;
+
+#ifdef CONFIG_FB_MSM_TRY_MDDI_CATCH_LCDC_PRISM
+        ret = msm_fb_detect_client("lcdc_prism_wvga");
+        if (ret == -ENODEV)
+                return 0;
+
+        if (ret && (mddi_get_client_id() != 0))
+                return 0;
+#endif
+
+        pinfo.xres = 800;
+        pinfo.yres = 480;
+        pinfo.type = LCDC_PANEL;
+        pinfo.pdest = DISPLAY_1;
+        pinfo.wait_cycle = 0;
+        pinfo.bpp = 24;
+        pinfo.fb_num = 2;
+        pinfo.clk_rate = 38460000;
+
+        pinfo.lcdc.h_back_porch = 21;
+        pinfo.lcdc.h_front_porch = 81;
+        pinfo.lcdc.h_pulse_width = 60;
+        pinfo.lcdc.v_back_porch = 18;
+        pinfo.lcdc.v_front_porch = 27;
+        pinfo.lcdc.v_pulse_width = 2;
+        pinfo.lcdc.border_clr = 0;      /* blk */
+        pinfo.lcdc.underflow_clr = 0xff;        /* blue */
+        pinfo.lcdc.hsync_skew = 0;
+
+        ret = lcdc_device_register(&pinfo);
+        if (ret)
+                printk(KERN_ERR "%s: failed to register device!\n", __func__);
+
+        return ret;
+}
+
+module_init(lcdc_prism_init);
diff --git a/drivers/staging/msm/lcdc_sharp_wvga_pt.c b/drivers/staging/msm/lcdc_sharp_wvga_pt.c
new file mode 100644
index 000000000000..1f08cf9bc217
--- /dev/null
+++ b/drivers/staging/msm/lcdc_sharp_wvga_pt.c
@@ -0,0 +1,290 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/delay.h>
+#ifdef CONFIG_ARCH_MSM7X30
+#include <linux/mfd/pmic8058.h>
+#endif
+#include <mach/gpio.h>
+#include "msm_fb.h"
+
+static int lcdc_sharp_panel_off(struct platform_device *pdev);
+
+static int spi_cs;
+static int spi_sclk;
+static int spi_mosi;
+static int spi_miso;
+static unsigned char bit_shift[8] = { (1 << 7), /* MSB */
+        (1 << 6),
+        (1 << 5),
+        (1 << 4),
+        (1 << 3),
+        (1 << 2),
+        (1 << 1),
+        (1 << 0)                               /* LSB */
+};
+
+struct sharp_state_type {
+        boolean disp_initialized;
+        boolean display_on;
+        boolean disp_powered_up;
+};
+
+struct sharp_spi_data {
+        u8 addr;
+        u8 data;
+};
+
+static struct sharp_spi_data init_sequence[] = {
+        {  15, 0x01 },
+        {   5, 0x01 },
+        {   7, 0x10 },
+        {   9, 0x1E },
+        {  10, 0x04 },
+        {  17, 0xFF },
+        {  21, 0x8A },
+        {  22, 0x00 },
+        {  23, 0x82 },
+        {  24, 0x24 },
+        {  25, 0x22 },
+        {  26, 0x6D },
+        {  27, 0xEB },
+        {  28, 0xB9 },
+        {  29, 0x3A },
+        {  49, 0x1A },
+        {  50, 0x16 },
+        {  51, 0x05 },
+        {  55, 0x7F },
+        {  56, 0x15 },
+        {  57, 0x7B },
+        {  60, 0x05 },
+        {  61, 0x0C },
+        {  62, 0x80 },
+        {  63, 0x00 },
+        {  92, 0x90 },
+        {  97, 0x01 },
+        {  98, 0xFF },
+        { 113, 0x11 },
+        { 114, 0x02 },
+        { 115, 0x08 },
+        { 123, 0xAB },
+        { 124, 0x04 },
+        {   6, 0x02 },
+        { 133, 0x00 },
+        { 134, 0xFE },
+        { 135, 0x22 },
+        { 136, 0x0B },
+        { 137, 0xFF },
+        { 138, 0x0F },
+        { 139, 0x00 },
+        { 140, 0xFE },
+        { 141, 0x22 },
+        { 142, 0x0B },
+        { 143, 0xFF },
+        { 144, 0x0F },
+        { 145, 0x00 },
+        { 146, 0xFE },
+        { 147, 0x22 },
+        { 148, 0x0B },
+        { 149, 0xFF },
+        { 150, 0x0F },
+        { 202, 0x30 },
+        {  30, 0x01 },
+        {   4, 0x01 },
+        {  31, 0x41 },
+};
+
+static struct sharp_state_type sharp_state = { 0 };
+static struct msm_panel_common_pdata *lcdc_sharp_pdata;
+
+static void sharp_spi_write_byte(u8 val)
+{
+        int i;
+
+        /* Clock should be Low before entering */
+        for (i = 0; i < 8; i++) {
+                /* #1: Drive the Data (High or Low) */
+                if (val & bit_shift[i])
+                        gpio_set_value(spi_mosi, 1);
+                else
+                        gpio_set_value(spi_mosi, 0);
+
+                /* #2: Drive the Clk High and then Low */
+                gpio_set_value(spi_sclk, 1);
+                gpio_set_value(spi_sclk, 0);
+        }
+}
+
+static void serigo(u8 reg, u8 data)
+{
+        /* Enable the Chip Select - low */
+        gpio_set_value(spi_cs, 0);
+        udelay(1);
+
+        /* Transmit register address first, then data */
+        sharp_spi_write_byte(reg);
+
+        /* Idle state of MOSI is Low */
+        gpio_set_value(spi_mosi, 0);
+        udelay(1);
+        sharp_spi_write_byte(data);
+
+        gpio_set_value(spi_mosi, 0);
+        gpio_set_value(spi_cs, 1);
+}
+
+static void sharp_spi_init(void)
+{
+        spi_sclk = *(lcdc_sharp_pdata->gpio_num);
+        spi_cs   = *(lcdc_sharp_pdata->gpio_num + 1);
+        spi_mosi = *(lcdc_sharp_pdata->gpio_num + 2);
+        spi_miso = *(lcdc_sharp_pdata->gpio_num + 3);
+
+        /* Set the output so that we don't disturb the slave device */
+        gpio_set_value(spi_sclk, 0);
+        gpio_set_value(spi_mosi, 0);
+
+        /* Set the Chip Select deasserted (active low) */
+        gpio_set_value(spi_cs, 1);
+}
+
+static void sharp_disp_powerup(void)
+{
+        if (!sharp_state.disp_powered_up && !sharp_state.display_on)
+                sharp_state.disp_powered_up = TRUE;
+}
+
+static void sharp_disp_on(void)
+{
+        int i;
+
+        if (sharp_state.disp_powered_up && !sharp_state.display_on) {
+                for (i = 0; i < ARRAY_SIZE(init_sequence); i++) {
+                        serigo(init_sequence[i].addr,
+                               init_sequence[i].data);
+                }
+                mdelay(10);
+                serigo(31, 0xC1);
+                mdelay(10);
+                serigo(31, 0xD9);
+                serigo(31, 0xDF);
+
+                sharp_state.display_on = TRUE;
+        }
+}
+
+static int lcdc_sharp_panel_on(struct platform_device *pdev)
+{
+        if (!sharp_state.disp_initialized) {
+                lcdc_sharp_pdata->panel_config_gpio(1);
+                sharp_spi_init();
+                sharp_disp_powerup();
+                sharp_disp_on();
+                sharp_state.disp_initialized = TRUE;
+        }
+        return 0;
+}
+
+static int lcdc_sharp_panel_off(struct platform_device *pdev)
+{
+        if (sharp_state.disp_powered_up && sharp_state.display_on) {
+                serigo(4, 0x00);
+                mdelay(40);
+                serigo(31, 0xC1);
+                mdelay(40);
+                serigo(31, 0x00);
+                mdelay(100);
+                sharp_state.display_on = FALSE;
+                sharp_state.disp_initialized = FALSE;
+        }
+        return 0;
+}
+
+static int __init sharp_probe(struct platform_device *pdev)
+{
+        if (pdev->id == 0) {
+                lcdc_sharp_pdata = pdev->dev.platform_data;
+                return 0;
+        }
+        msm_fb_add_device(pdev);
+        return 0;
+}
+
+static struct platform_driver this_driver = {
+        .probe  = sharp_probe,
+        .driver = {
+                .name   = "lcdc_sharp_wvga",
+        },
+};
+
+static struct msm_fb_panel_data sharp_panel_data = {
+        .on = lcdc_sharp_panel_on,
+        .off = lcdc_sharp_panel_off,
+};
+
+static struct platform_device this_device = {
+        .name   = "lcdc_sharp_wvga",
+        .id     = 1,
+        .dev    = {
+                .platform_data = &sharp_panel_data,
+        }
+};
+
+static int __init lcdc_sharp_panel_init(void)
+{
+        int ret;
+        struct msm_panel_info *pinfo;
+
+#ifdef CONFIG_FB_MSM_MDDI_AUTO_DETECT
+        if (msm_fb_detect_client("lcdc_sharp_wvga_pt"))
+                return 0;
+#endif
+
+        ret = platform_driver_register(&this_driver);
+        if (ret)
+                return ret;
+
+        pinfo = &sharp_panel_data.panel_info;
+        pinfo->xres = 480;
+        pinfo->yres = 800;
+        pinfo->type = LCDC_PANEL;
+        pinfo->pdest = DISPLAY_1;
+        pinfo->wait_cycle = 0;
+        pinfo->bpp = 18;
+        pinfo->fb_num = 2;
+        pinfo->clk_rate = 24500000;
+        pinfo->bl_max = 4;
+        pinfo->bl_min = 1;
+
+        pinfo->lcdc.h_back_porch = 20;
+        pinfo->lcdc.h_front_porch = 10;
+        pinfo->lcdc.h_pulse_width = 10;
+        pinfo->lcdc.v_back_porch = 2;
+        pinfo->lcdc.v_front_porch = 2;
+        pinfo->lcdc.v_pulse_width = 2;
+        pinfo->lcdc.border_clr = 0;
+        pinfo->lcdc.underflow_clr = 0xff;
+        pinfo->lcdc.hsync_skew = 0;
+
+        ret = platform_device_register(&this_device);
+        if (ret)
+                platform_driver_unregister(&this_driver);
+
+        return ret;
+}
+
+module_init(lcdc_sharp_panel_init);
diff --git a/drivers/staging/msm/lcdc_st15.c b/drivers/staging/msm/lcdc_st15.c
new file mode 100644
index 000000000000..fed8278eb153
--- /dev/null
+++ b/drivers/staging/msm/lcdc_st15.c
@@ -0,0 +1,237 @@
+/* Copyright (c) 2010, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include "msm_fb.h"
+
+#define DEVICE_NAME "sii9022"
+#define SII9022_DEVICE_ID   0xB0
+
+struct sii9022_i2c_addr_data{
+        u8 addr;
+        u8 data;
+};
+
+/* video mode data */
+static u8 video_mode_data[] = {
+        0x00,
+        0xF9, 0x1C, 0x70, 0x17, 0x72, 0x06, 0xEE, 0x02,
+};
+
+static u8 avi_io_format[] = {
+        0x09,
+        0x00, 0x00,
+};
+
+/* power state */
+static struct sii9022_i2c_addr_data regset0[] = {
+        { 0x60, 0x04 },
+        { 0x63, 0x00 },
+        { 0x1E, 0x00 },
+};
+
+static u8 video_infoframe[] = {
+        0x0C,
+        0xF0, 0x00, 0x68, 0x00, 0x04, 0x00, 0x19, 0x00,
+        0xE9, 0x02, 0x04, 0x01, 0x04, 0x06,
+};
+
+/* configure audio */
+static struct sii9022_i2c_addr_data regset1[] = {
+        { 0x26, 0x90 },
+        { 0x20, 0x90 },
+        { 0x1F, 0x80 },
+        { 0x26, 0x80 },
+        { 0x24, 0x02 },
+        { 0x25, 0x0B },
+        { 0xBC, 0x02 },
+        { 0xBD, 0x24 },
+        { 0xBE, 0x02 },
+};
+
+/* enable audio */
+static u8 misc_infoframe[] = {
+        0xBF,
+        0xC2, 0x84, 0x01, 0x0A, 0x6F, 0x02, 0x00, 0x00,
+        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+};
+
+/* set HDMI, active */
+static struct sii9022_i2c_addr_data regset2[] = {
+        { 0x1A, 0x01 },
+        { 0x3D, 0x00 },
+};
+
+static int send_i2c_data(struct i2c_client *client,
+                         struct sii9022_i2c_addr_data *regset,
+                         int size)
+{
+        int i;
+        int rc = 0;
+
+        for (i = 0; i < size; i++) {
+                rc = i2c_smbus_write_byte_data(
+                        client,
+                        regset[i].addr, regset[i].data);
+                if (rc)
+                        break;
+        }
+        return rc;
+}
+
+static int hdmi_sii_enable(struct i2c_client *client)
+{
+        int rc;
+        int retries = 10;
+        int count;
+
+        rc = i2c_smbus_write_byte_data(client, 0xC7, 0x00);
+        if (rc)
+                goto enable_exit;
+
+        do {
+                msleep(1);
+                rc = i2c_smbus_read_byte_data(client, 0x1B);
+        } while ((rc != SII9022_DEVICE_ID) && retries--);
+
+        if (rc != SII9022_DEVICE_ID)
+                return -ENODEV;
+
+        rc = i2c_smbus_write_byte_data(client, 0x1A, 0x11);
+        if (rc)
+                goto enable_exit;
+
+        count = ARRAY_SIZE(video_mode_data);
+        rc = i2c_master_send(client, video_mode_data, count);
+        if (rc != count) {
+                rc = -EIO;
+                goto enable_exit;
+        }
+
+        rc = i2c_smbus_write_byte_data(client, 0x08, 0x20);
+        if (rc)
+                goto enable_exit;
+        count = ARRAY_SIZE(avi_io_format);
+        rc = i2c_master_send(client, avi_io_format, count);
+        if (rc != count) {
+                rc = -EIO;
+                goto enable_exit;
+        }
+
+        rc = send_i2c_data(client, regset0, ARRAY_SIZE(regset0));
+        if (rc)
+                goto enable_exit;
+
+        count = ARRAY_SIZE(video_infoframe);
+        rc = i2c_master_send(client, video_infoframe, count);
+        if (rc != count) {
+                rc = -EIO;
+                goto enable_exit;
+        }
+
+        rc = send_i2c_data(client, regset1, ARRAY_SIZE(regset1));
+        if (rc)
+                goto enable_exit;
+
+        count = ARRAY_SIZE(misc_infoframe);
+        rc = i2c_master_send(client, misc_infoframe, count);
+        if (rc != count) {
+                rc = -EIO;
+                goto enable_exit;
+        }
+
+        rc = send_i2c_data(client, regset2, ARRAY_SIZE(regset2));
+        if (rc)
+                goto enable_exit;
+
+        return 0;
+enable_exit:
+        printk(KERN_ERR "%s: exited rc=%d\n", __func__, rc);
+        return rc;
+}
+
+static const struct i2c_device_id hmdi_sii_id[] = {
+        { DEVICE_NAME, 0 },
+        { }
+};
+
+static int hdmi_sii_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        int rc;
+
+        if (!i2c_check_functionality(client->adapter,
+                                     I2C_FUNC_SMBUS_BYTE | I2C_FUNC_I2C))
+                return -ENODEV;
+        rc = hdmi_sii_enable(client);
+        return rc;
+}
+
+
+static struct i2c_driver hdmi_sii_i2c_driver = {
+        .driver = {
+                .name = DEVICE_NAME,
+                .owner = THIS_MODULE,
+        },
+        .probe = hdmi_sii_probe,
+        .remove =  __exit_p(hdmi_sii_remove),
+        .id_table = hmdi_sii_id,
+};
+
+static int __init lcdc_st15_init(void)
+{
+        int ret;
+        struct msm_panel_info pinfo;
+
+        if (msm_fb_detect_client("lcdc_st15"))
+                return 0;
+
+        pinfo.xres = 1366;
+        pinfo.yres = 768;
+        pinfo.type = LCDC_PANEL;
+        pinfo.pdest = DISPLAY_1;
+        pinfo.wait_cycle = 0;
+        pinfo.bpp = 24;
+        pinfo.fb_num = 2;
+        pinfo.clk_rate = 74250000;
+
+        pinfo.lcdc.h_back_porch = 120;
+        pinfo.lcdc.h_front_porch = 20;
+        pinfo.lcdc.h_pulse_width = 40;
+        pinfo.lcdc.v_back_porch = 25;
+        pinfo.lcdc.v_front_porch = 1;
+        pinfo.lcdc.v_pulse_width = 7;
+        pinfo.lcdc.border_clr = 0;      /* blk */
+        pinfo.lcdc.underflow_clr = 0xff;        /* blue */
+        pinfo.lcdc.hsync_skew = 0;
+
+        ret = lcdc_device_register(&pinfo);
+        if (ret) {
+                printk(KERN_ERR "%s: failed to register device!\n", __func__);
+                goto init_exit;
+        }
+
+        ret = i2c_add_driver(&hdmi_sii_i2c_driver);
+        if (ret)
+                printk(KERN_ERR "%s: failed to add i2c driver\n", __func__);
+
+init_exit:
+        return ret;
+}
+
+module_init(lcdc_st15_init);
diff --git a/drivers/staging/msm/lcdc_st1_wxga.c b/drivers/staging/msm/lcdc_st1_wxga.c
new file mode 100644
index 000000000000..73760019cf2e
--- /dev/null
+++ b/drivers/staging/msm/lcdc_st1_wxga.c
@@ -0,0 +1,54 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+
+static int __init lcdc_st1_wxga_init(void)
+{
+        int ret;
+        struct msm_panel_info pinfo;
+
+        if (msm_fb_detect_client("lcdc_st1_wxga"))
+                return 0;
+
+        pinfo.xres = 1280;
+        pinfo.yres = 720;
+        pinfo.type = LCDC_PANEL;
+        pinfo.pdest = DISPLAY_1;
+        pinfo.wait_cycle = 0;
+        pinfo.bpp = 18;
+        pinfo.fb_num = 2;
+        pinfo.clk_rate = 74250000;
+
+        pinfo.lcdc.h_back_porch = 124;
+        pinfo.lcdc.h_front_porch = 110;
+        pinfo.lcdc.h_pulse_width = 136;
+        pinfo.lcdc.v_back_porch = 19;
+        pinfo.lcdc.v_front_porch = 5;
+        pinfo.lcdc.v_pulse_width = 6;
+        pinfo.lcdc.border_clr = 0;      /* blk */
+        pinfo.lcdc.underflow_clr = 0xff;        /* blue */
+        pinfo.lcdc.hsync_skew = 0;
+
+        ret = lcdc_device_register(&pinfo);
+        if (ret)
+                printk(KERN_ERR "%s: failed to register device!\n", __func__);
+
+        return ret;
+}
+
+module_init(lcdc_st1_wxga_init);
diff --git a/drivers/staging/msm/lcdc_toshiba_wvga_pt.c b/drivers/staging/msm/lcdc_toshiba_wvga_pt.c
new file mode 100644
index 000000000000..864d7c18913d
--- /dev/null
+++ b/drivers/staging/msm/lcdc_toshiba_wvga_pt.c
@@ -0,0 +1,374 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <mach/gpio.h>
+#include <mach/pmic.h>
+#include "msm_fb.h"
+
+#ifdef CONFIG_FB_MSM_TRY_MDDI_CATCH_LCDC_PRISM
+#include "mddihosti.h"
+#endif
+
+static int spi_cs;
+static int spi_sclk;
+static int spi_mosi;
+static int spi_miso;
+
+struct toshiba_state_type{
+        boolean disp_initialized;
+        boolean display_on;
+        boolean disp_powered_up;
+};
+
+static struct toshiba_state_type toshiba_state = { 0 };
+static struct msm_panel_common_pdata *lcdc_toshiba_pdata;
+
+static void toshiba_spi_write_byte(char dc, uint8 data)
+{
+        uint32 bit;
+        int bnum;
+
+        gpio_set_value(spi_sclk, 0); /* clk low */
+        /* dc: 0 for command, 1 for parameter */
+        gpio_set_value(spi_mosi, dc);
+        udelay(1);      /* at least 20 ns */
+        gpio_set_value(spi_sclk, 1); /* clk high */
+        udelay(1);      /* at least 20 ns */
+        bnum = 8;       /* 8 data bits */
+        bit = 0x80;
+        while (bnum) {
+                gpio_set_value(spi_sclk, 0); /* clk low */
+                if (data & bit)
+                        gpio_set_value(spi_mosi, 1);
+                else
+                        gpio_set_value(spi_mosi, 0);
+                udelay(1);
+                gpio_set_value(spi_sclk, 1); /* clk high */
+                udelay(1);
+                bit >>= 1;
+                bnum--;
+        }
+}
+
+static void toshiba_spi_write(char cmd, uint32 data, int num)
+{
+        char *bp;
+
+        gpio_set_value(spi_cs, 1);      /* cs high */
+
+        /* command byte first */
+        toshiba_spi_write_byte(0, cmd);
+
+        /* followed by parameter bytes */
+        if (num) {
+                bp = (char *)&data;;
+                bp += (num - 1);
+                while (num) {
+                        toshiba_spi_write_byte(1, *bp);
+                        num--;
+                        bp--;
+                }
+        }
+
+        gpio_set_value(spi_cs, 0);      /* cs low */
+        udelay(1);
+}
+
+void toshiba_spi_read_bytes(char cmd, uint32 *data, int num)
+{
+        uint32 dbit, bits;
+        int bnum;
+
+        gpio_set_value(spi_cs, 1);      /* cs high */
+
+        /* command byte first */
+        toshiba_spi_write_byte(0, cmd);
+
+        if (num > 1) {
+                /* extra dc bit */
+                gpio_set_value(spi_sclk, 0); /* clk low */
+                udelay(1);
+                dbit = gpio_get_value(spi_miso);/* dc bit */
+                udelay(1);
+                gpio_set_value(spi_sclk, 1); /* clk high */
+        }
+
+        /* followed by data bytes */
+        bnum = num * 8; /* number of bits */
+        bits = 0;
+        while (bnum) {
+                bits <<= 1;
+                gpio_set_value(spi_sclk, 0); /* clk low */
+                udelay(1);
+                dbit = gpio_get_value(spi_miso);
+                udelay(1);
+                gpio_set_value(spi_sclk, 1); /* clk high */
+                bits |= dbit;
+                bnum--;
+        }
+
+        *data = bits;
+
+        udelay(1);
+        gpio_set_value(spi_cs, 0);      /* cs low */
+        udelay(1);
+}
+
+static void spi_pin_assign(void)
+{
+        /* Setting the Default GPIO's */
+        spi_sclk = *(lcdc_toshiba_pdata->gpio_num);
+        spi_cs   = *(lcdc_toshiba_pdata->gpio_num + 1);
+        spi_mosi  = *(lcdc_toshiba_pdata->gpio_num + 2);
+        spi_miso  = *(lcdc_toshiba_pdata->gpio_num + 3);
+}
+
+static void toshiba_disp_powerup(void)
+{
+        if (!toshiba_state.disp_powered_up && !toshiba_state.display_on) {
+                /* Reset the hardware first */
+                /* Include DAC power up implementation here */
+              toshiba_state.disp_powered_up = TRUE;
+        }
+}
+
+static void toshiba_disp_on(void)
+{
+        uint32  data;
+
+        gpio_set_value(spi_cs, 0);      /* low */
+        gpio_set_value(spi_sclk, 1);    /* high */
+        gpio_set_value(spi_mosi, 0);
+        gpio_set_value(spi_miso, 0);
+
+        if (toshiba_state.disp_powered_up && !toshiba_state.display_on) {
+                toshiba_spi_write(0, 0, 0);
+                mdelay(7);
+                toshiba_spi_write(0, 0, 0);
+                mdelay(7);
+                toshiba_spi_write(0, 0, 0);
+                mdelay(7);
+                toshiba_spi_write(0xba, 0x11, 1);
+                toshiba_spi_write(0x36, 0x00, 1);
+                mdelay(1);
+                toshiba_spi_write(0x3a, 0x60, 1);
+                toshiba_spi_write(0xb1, 0x5d, 1);
+                mdelay(1);
+                toshiba_spi_write(0xb2, 0x33, 1);
+                toshiba_spi_write(0xb3, 0x22, 1);
+                mdelay(1);
+                toshiba_spi_write(0xb4, 0x02, 1);
+                toshiba_spi_write(0xb5, 0x1e, 1); /* vcs -- adjust brightness */
+                mdelay(1);
+                toshiba_spi_write(0xb6, 0x27, 1);
+                toshiba_spi_write(0xb7, 0x03, 1);
+                mdelay(1);
+                toshiba_spi_write(0xb9, 0x24, 1);
+                toshiba_spi_write(0xbd, 0xa1, 1);
+                mdelay(1);
+                toshiba_spi_write(0xbb, 0x00, 1);
+                toshiba_spi_write(0xbf, 0x01, 1);
+                mdelay(1);
+                toshiba_spi_write(0xbe, 0x00, 1);
+                toshiba_spi_write(0xc0, 0x11, 1);
+                mdelay(1);
+                toshiba_spi_write(0xc1, 0x11, 1);
+                toshiba_spi_write(0xc2, 0x11, 1);
+                mdelay(1);
+                toshiba_spi_write(0xc3, 0x3232, 2);
+                mdelay(1);
+                toshiba_spi_write(0xc4, 0x3232, 2);
+                mdelay(1);
+                toshiba_spi_write(0xc5, 0x3232, 2);
+                mdelay(1);
+                toshiba_spi_write(0xc6, 0x3232, 2);
+                mdelay(1);
+                toshiba_spi_write(0xc7, 0x6445, 2);
+                mdelay(1);
+                toshiba_spi_write(0xc8, 0x44, 1);
+                toshiba_spi_write(0xc9, 0x52, 1);
+                mdelay(1);
+                toshiba_spi_write(0xca, 0x00, 1);
+                mdelay(1);
+                toshiba_spi_write(0xec, 0x02a4, 2);     /* 0x02a4 */
+                mdelay(1);
+                toshiba_spi_write(0xcf, 0x01, 1);
+                mdelay(1);
+                toshiba_spi_write(0xd0, 0xc003, 2);     /* c003 */
+                mdelay(1);
+                toshiba_spi_write(0xd1, 0x01, 1);
+                mdelay(1);
+                toshiba_spi_write(0xd2, 0x0028, 2);
+                mdelay(1);
+                toshiba_spi_write(0xd3, 0x0028, 2);
+                mdelay(1);
+                toshiba_spi_write(0xd4, 0x26a4, 2);
+                mdelay(1);
+                toshiba_spi_write(0xd5, 0x20, 1);
+                mdelay(1);
+                toshiba_spi_write(0xef, 0x3200, 2);
+                mdelay(32);
+                toshiba_spi_write(0xbc, 0x80, 1);       /* wvga pass through */
+                toshiba_spi_write(0x3b, 0x00, 1);
+                mdelay(1);
+                toshiba_spi_write(0xb0, 0x16, 1);
+                mdelay(1);
+                toshiba_spi_write(0xb8, 0xfff5, 2);
+                mdelay(1);
+                toshiba_spi_write(0x11, 0, 0);
+                mdelay(5);
+                toshiba_spi_write(0x29, 0, 0);
+                mdelay(5);
+                toshiba_state.display_on = TRUE;
+        }
+
+        data = 0;
+        toshiba_spi_read_bytes(0x04, &data, 3);
+        printk(KERN_INFO "toshiba_disp_on: id=%x\n", data);
+
+}
+
+static int lcdc_toshiba_panel_on(struct platform_device *pdev)
+{
+        if (!toshiba_state.disp_initialized) {
+                /* Configure reset GPIO that drives DAC */
+                if (lcdc_toshiba_pdata->panel_config_gpio)
+                        lcdc_toshiba_pdata->panel_config_gpio(1);
+                toshiba_disp_powerup();
+                toshiba_disp_on();
+                toshiba_state.disp_initialized = TRUE;
+        }
+        return 0;
+}
+
+static int lcdc_toshiba_panel_off(struct platform_device *pdev)
+{
+        if (toshiba_state.disp_powered_up && toshiba_state.display_on) {
+                /* Main panel power off (Deep standby in) */
+
+                toshiba_spi_write(0x28, 0, 0);  /* display off */
+                mdelay(1);
+                toshiba_spi_write(0xb8, 0x8002, 2);     /* output control */
+                mdelay(1);
+                toshiba_spi_write(0x10, 0x00, 1);       /* sleep mode in */
+                mdelay(85);             /* wait 85 msec */
+                toshiba_spi_write(0xb0, 0x00, 1);       /* deep standby in */
+                mdelay(1);
+                if (lcdc_toshiba_pdata->panel_config_gpio)
+                        lcdc_toshiba_pdata->panel_config_gpio(0);
+                toshiba_state.display_on = FALSE;
+                toshiba_state.disp_initialized = FALSE;
+        }
+        return 0;
+}
+
+static void lcdc_toshiba_set_backlight(struct msm_fb_data_type *mfd)
+{
+        int bl_level;
+        int ret = -EPERM;
+
+        bl_level = mfd->bl_level;
+        ret = pmic_set_led_intensity(LED_LCD, bl_level);
+
+        if (ret)
+                printk(KERN_WARNING "%s: can't set lcd backlight!\n",
+                                __func__);
+}
+
+static int __init toshiba_probe(struct platform_device *pdev)
+{
+        if (pdev->id == 0) {
+                lcdc_toshiba_pdata = pdev->dev.platform_data;
+                spi_pin_assign();
+                return 0;
+        }
+        msm_fb_add_device(pdev);
+        return 0;
+}
+
+static struct platform_driver this_driver = {
+        .probe  = toshiba_probe,
+        .driver = {
+                .name   = "lcdc_toshiba_wvga",
+        },
+};
+
+static struct msm_fb_panel_data toshiba_panel_data = {
+        .on = lcdc_toshiba_panel_on,
+        .off = lcdc_toshiba_panel_off,
+        .set_backlight = lcdc_toshiba_set_backlight,
+};
+
+static struct platform_device this_device = {
+        .name   = "lcdc_toshiba_wvga",
+        .id     = 1,
+        .dev    = {
+                .platform_data = &toshiba_panel_data,
+        }
+};
+
+static int __init lcdc_toshiba_panel_init(void)
+{
+        int ret;
+        struct msm_panel_info *pinfo;
+#ifdef CONFIG_FB_MSM_TRY_MDDI_CATCH_LCDC_PRISM
+        if (mddi_get_client_id() != 0)
+                return 0;
+
+        ret = msm_fb_detect_client("lcdc_toshiba_wvga_pt");
+        if (ret)
+                return 0;
+
+#endif
+
+        ret = platform_driver_register(&this_driver);
+        if (ret)
+                return ret;
+
+        pinfo = &toshiba_panel_data.panel_info;
+        pinfo->xres = 480;
+        pinfo->yres = 800;
+        pinfo->type = LCDC_PANEL;
+        pinfo->pdest = DISPLAY_1;
+        pinfo->wait_cycle = 0;
+        pinfo->bpp = 18;
+        pinfo->fb_num = 2;
+        /* 30Mhz mdp_lcdc_pclk and mdp_lcdc_pad_pcl */
+        pinfo->clk_rate = 27648000;
+        pinfo->bl_max = 15;
+        pinfo->bl_min = 1;
+
+        pinfo->lcdc.h_back_porch = 184; /* hsw = 8 + hbp=184 */
+        pinfo->lcdc.h_front_porch = 4;
+        pinfo->lcdc.h_pulse_width = 8;
+        pinfo->lcdc.v_back_porch = 2;   /* vsw=1 + vbp = 2 */
+        pinfo->lcdc.v_front_porch = 3;
+        pinfo->lcdc.v_pulse_width = 1;
+        pinfo->lcdc.border_clr = 0;     /* blk */
+        pinfo->lcdc.underflow_clr = 0xff;       /* blue */
+        pinfo->lcdc.hsync_skew = 0;
+
+        ret = platform_device_register(&this_device);
+        if (ret)
+                platform_driver_unregister(&this_driver);
+
+        return ret;
+}
+
+device_initcall(lcdc_toshiba_panel_init);
diff --git a/drivers/staging/msm/lcdc_wxga.c b/drivers/staging/msm/lcdc_wxga.c
new file mode 100644
index 000000000000..202c92c0ef54
--- /dev/null
+++ b/drivers/staging/msm/lcdc_wxga.c
@@ -0,0 +1,56 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+
+static int __init lcdc_wxga_init(void)
+{
+        int ret;
+        struct msm_panel_info pinfo;
+
+#ifdef CONFIG_FB_MSM_MDDI_AUTO_DETECT
+        if (msm_fb_detect_client("lcdc_wxga"))
+                return 0;
+#endif
+
+        pinfo.xres = 1280;
+        pinfo.yres = 720;
+        pinfo.type = LCDC_PANEL;
+        pinfo.pdest = DISPLAY_1;
+        pinfo.wait_cycle = 0;
+        pinfo.bpp = 24;
+        pinfo.fb_num = 2;
+        pinfo.clk_rate = 74250000;
+
+        pinfo.lcdc.h_back_porch = 124;
+        pinfo.lcdc.h_front_porch = 110;
+        pinfo.lcdc.h_pulse_width = 136;
+        pinfo.lcdc.v_back_porch = 19;
+        pinfo.lcdc.v_front_porch = 5;
+        pinfo.lcdc.v_pulse_width = 6;
+        pinfo.lcdc.border_clr = 0;      /* blk */
+        pinfo.lcdc.underflow_clr = 0xff;        /* blue */
+        pinfo.lcdc.hsync_skew = 0;
+
+        ret = lcdc_device_register(&pinfo);
+        if (ret)
+                printk(KERN_ERR "%s: failed to register device!\n", __func__);
+
+        return ret;
+}
+
+module_init(lcdc_wxga_init);
diff --git a/drivers/staging/msm/logo.c b/drivers/staging/msm/logo.c
new file mode 100644
index 000000000000..7272765f48cd
--- /dev/null
+++ b/drivers/staging/msm/logo.c
@@ -0,0 +1,98 @@
+/* drivers/video/msm/logo.c
+ *
+ * Show Logo in RLE 565 format
+ *
+ * Copyright (C) 2008 Google Incorporated
+ *
+ * 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.
+ *
+ */
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/fb.h>
+#include <linux/vt_kern.h>
+#include <linux/unistd.h>
+#include <linux/syscalls.h>
+
+#include <linux/irq.h>
+#include <asm/system.h>
+
+#define fb_width(fb)    ((fb)->var.xres)
+#define fb_height(fb)   ((fb)->var.yres)
+#define fb_size(fb)     ((fb)->var.xres * (fb)->var.yres * 2)
+
+static void memset16(void *_ptr, unsigned short val, unsigned count)
+{
+        unsigned short *ptr = _ptr;
+        count >>= 1;
+        while (count--)
+                *ptr++ = val;
+}
+
+/* 565RLE image format: [count(2 bytes), rle(2 bytes)] */
+int load_565rle_image(char *filename)
+{
+        struct fb_info *info;
+        int fd, err = 0;
+        unsigned count, max;
+        unsigned short *data, *bits, *ptr;
+
+        info = registered_fb[0];
+        if (!info) {
+                printk(KERN_WARNING "%s: Can not access framebuffer\n",
+                        __func__);
+                return -ENODEV;
+        }
+
+        fd = sys_open(filename, O_RDONLY, 0);
+        if (fd < 0) {
+                printk(KERN_WARNING "%s: Can not open %s\n",
+                        __func__, filename);
+                return -ENOENT;
+        }
+        count = (unsigned)sys_lseek(fd, (off_t)0, 2);
+        if (count == 0) {
+                sys_close(fd);
+                err = -EIO;
+                goto err_logo_close_file;
+        }
+        sys_lseek(fd, (off_t)0, 0);
+        data = kmalloc(count, GFP_KERNEL);
+        if (!data) {
+                printk(KERN_WARNING "%s: Can not alloc data\n", __func__);
+                err = -ENOMEM;
+                goto err_logo_close_file;
+        }
+        if ((unsigned)sys_read(fd, (char *)data, count) != count) {
+                err = -EIO;
+                goto err_logo_free_data;
+        }
+
+        max = fb_width(info) * fb_height(info);
+        ptr = data;
+        bits = (unsigned short *)(info->screen_base);
+        while (count > 3) {
+                unsigned n = ptr[0];
+                if (n > max)
+                        break;
+                memset16(bits, ptr[1], n << 1);
+                bits += n;
+                max -= n;
+                ptr += 2;
+                count -= 4;
+        }
+
+err_logo_free_data:
+        kfree(data);
+err_logo_close_file:
+        sys_close(fd);
+        return err;
+}
+EXPORT_SYMBOL(load_565rle_image);
diff --git a/drivers/staging/msm/mddi.c b/drivers/staging/msm/mddi.c
new file mode 100644
index 000000000000..132eb1adff16
--- /dev/null
+++ b/drivers/staging/msm/mddi.c
@@ -0,0 +1,375 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <mach/hardware.h>
+#include <asm/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/uaccess.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+
+#include "msm_fb.h"
+#include "mddihosti.h"
+#include "mddihost.h"
+#include <mach/gpio.h>
+#include <mach/clk.h>
+
+static int mddi_probe(struct platform_device *pdev);
+static int mddi_remove(struct platform_device *pdev);
+
+static int mddi_off(struct platform_device *pdev);
+static int mddi_on(struct platform_device *pdev);
+
+static int mddi_suspend(struct platform_device *pdev, pm_message_t state);
+static int mddi_resume(struct platform_device *pdev);
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void mddi_early_suspend(struct early_suspend *h);
+static void mddi_early_resume(struct early_suspend *h);
+#endif
+
+static struct platform_device *pdev_list[MSM_FB_MAX_DEV_LIST];
+static int pdev_list_cnt;
+static struct clk *mddi_clk;
+static struct clk *mddi_pclk;
+static struct mddi_platform_data *mddi_pdata;
+
+static struct platform_driver mddi_driver = {
+        .probe = mddi_probe,
+        .remove = mddi_remove,
+#ifndef CONFIG_HAS_EARLYSUSPEND
+#ifdef CONFIG_PM
+        .suspend = mddi_suspend,
+        .resume = mddi_resume,
+#endif
+#endif
+        .suspend_late = NULL,
+        .resume_early = NULL,
+        .shutdown = NULL,
+        .driver = {
+                   .name = "mddi",
+                   },
+};
+
+extern int int_mddi_pri_flag;
+
+static int mddi_off(struct platform_device *pdev)
+{
+        int ret = 0;
+
+        ret = panel_next_off(pdev);
+
+        if (mddi_pdata && mddi_pdata->mddi_power_save)
+                mddi_pdata->mddi_power_save(0);
+
+        return ret;
+}
+
+static int mddi_on(struct platform_device *pdev)
+{
+        int ret = 0;
+        u32 clk_rate;
+        struct msm_fb_data_type *mfd;
+
+        mfd = platform_get_drvdata(pdev);
+
+        if (mddi_pdata && mddi_pdata->mddi_power_save)
+                mddi_pdata->mddi_power_save(1);
+
+        clk_rate = mfd->fbi->var.pixclock;
+        clk_rate = min(clk_rate, mfd->panel_info.clk_max);
+
+        if (mddi_pdata &&
+            mddi_pdata->mddi_sel_clk &&
+            mddi_pdata->mddi_sel_clk(&clk_rate))
+                        printk(KERN_ERR
+                          "%s: can't select mddi io clk targate rate = %d\n",
+                          __func__, clk_rate);
+
+        if (clk_set_min_rate(mddi_clk, clk_rate) < 0)
+                printk(KERN_ERR "%s: clk_set_min_rate failed\n",
+                        __func__);
+
+        ret = panel_next_on(pdev);
+
+        return ret;
+}
+
+static int mddi_resource_initialized;
+
+static int mddi_probe(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+        struct platform_device *mdp_dev = NULL;
+        struct msm_fb_panel_data *pdata = NULL;
+        int rc;
+        resource_size_t size ;
+        u32 clk_rate;
+
+        if ((pdev->id == 0) && (pdev->num_resources >= 0)) {
+                mddi_pdata = pdev->dev.platform_data;
+
+                size =  resource_size(&pdev->resource[0]);
+                msm_pmdh_base =  ioremap(pdev->resource[0].start, size);
+
+                MSM_FB_INFO("primary mddi base phy_addr = 0x%x virt = 0x%x\n",
+                                pdev->resource[0].start, (int) msm_pmdh_base);
+
+                if (unlikely(!msm_pmdh_base))
+                        return -ENOMEM;
+
+                if (mddi_pdata && mddi_pdata->mddi_power_save)
+                        mddi_pdata->mddi_power_save(1);
+
+                mddi_resource_initialized = 1;
+                return 0;
+        }
+
+        if (!mddi_resource_initialized)
+                return -EPERM;
+
+        mfd = platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        if (pdev_list_cnt >= MSM_FB_MAX_DEV_LIST)
+                return -ENOMEM;
+
+        mdp_dev = platform_device_alloc("mdp", pdev->id);
+        if (!mdp_dev)
+                return -ENOMEM;
+
+        /*
+         * link to the latest pdev
+         */
+        mfd->pdev = mdp_dev;
+        mfd->dest = DISPLAY_LCD;
+
+        /*
+         * alloc panel device data
+         */
+        if (platform_device_add_data
+            (mdp_dev, pdev->dev.platform_data,
+             sizeof(struct msm_fb_panel_data))) {
+                printk(KERN_ERR "mddi_probe: platform_device_add_data failed!\n");
+                platform_device_put(mdp_dev);
+                return -ENOMEM;
+        }
+        /*
+         * data chain
+         */
+        pdata = mdp_dev->dev.platform_data;
+        pdata->on = mddi_on;
+        pdata->off = mddi_off;
+        pdata->next = pdev;
+
+        /*
+         * get/set panel specific fb info
+         */
+        mfd->panel_info = pdata->panel_info;
+        mfd->fb_imgType = MDP_RGB_565;
+
+        clk_rate = mfd->panel_info.clk_max;
+        if (mddi_pdata &&
+            mddi_pdata->mddi_sel_clk &&
+            mddi_pdata->mddi_sel_clk(&clk_rate))
+                        printk(KERN_ERR
+                          "%s: can't select mddi io clk targate rate = %d\n",
+                          __func__, clk_rate);
+
+        if (clk_set_max_rate(mddi_clk, clk_rate) < 0)
+                printk(KERN_ERR "%s: clk_set_max_rate failed\n", __func__);
+        mfd->panel_info.clk_rate = mfd->panel_info.clk_min;
+
+        /*
+         * set driver data
+         */
+        platform_set_drvdata(mdp_dev, mfd);
+
+        /*
+         * register in mdp driver
+         */
+        rc = platform_device_add(mdp_dev);
+        if (rc)
+                goto mddi_probe_err;
+
+        pdev_list[pdev_list_cnt++] = pdev;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+        mfd->mddi_early_suspend.level = EARLY_SUSPEND_LEVEL_DISABLE_FB;
+        mfd->mddi_early_suspend.suspend = mddi_early_suspend;
+        mfd->mddi_early_suspend.resume = mddi_early_resume;
+        register_early_suspend(&mfd->mddi_early_suspend);
+#endif
+
+        return 0;
+
+mddi_probe_err:
+        platform_device_put(mdp_dev);
+        return rc;
+}
+
+static int mddi_pad_ctrl;
+static int mddi_power_locked;
+static int mddi_is_in_suspend;
+
+void mddi_disable(int lock)
+{
+        mddi_host_type host_idx = MDDI_HOST_PRIM;
+
+        if (mddi_power_locked)
+                return;
+
+        if (lock)
+                mddi_power_locked = 1;
+
+        if (mddi_host_timer.function)
+                del_timer_sync(&mddi_host_timer);
+
+        mddi_pad_ctrl = mddi_host_reg_in(PAD_CTL);
+        mddi_host_reg_out(PAD_CTL, 0x0);
+
+        if (clk_set_min_rate(mddi_clk, 0) < 0)
+                printk(KERN_ERR "%s: clk_set_min_rate failed\n", __func__);
+
+        clk_disable(mddi_clk);
+        if (mddi_pclk)
+                clk_disable(mddi_pclk);
+        disable_irq(INT_MDDI_PRI);
+
+        if (mddi_pdata && mddi_pdata->mddi_power_save)
+                mddi_pdata->mddi_power_save(0);
+}
+
+static int mddi_suspend(struct platform_device *pdev, pm_message_t state)
+{
+        if (mddi_is_in_suspend)
+                return 0;
+
+        mddi_is_in_suspend = 1;
+        mddi_disable(0);
+        return 0;
+}
+
+static int mddi_resume(struct platform_device *pdev)
+{
+        mddi_host_type host_idx = MDDI_HOST_PRIM;
+
+        if (!mddi_is_in_suspend)
+                return 0;
+
+        mddi_is_in_suspend = 0;
+
+        if (mddi_power_locked)
+                return 0;
+
+        enable_irq(INT_MDDI_PRI);
+        clk_enable(mddi_clk);
+        if (mddi_pclk)
+                clk_enable(mddi_pclk);
+        mddi_host_reg_out(PAD_CTL, mddi_pad_ctrl);
+
+        if (mddi_host_timer.function)
+                mddi_host_timer_service(0);
+
+        return 0;
+}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void mddi_early_suspend(struct early_suspend *h)
+{
+        pm_message_t state;
+        struct msm_fb_data_type *mfd = container_of(h, struct msm_fb_data_type,
+                                                        mddi_early_suspend);
+
+        state.event = PM_EVENT_SUSPEND;
+        mddi_suspend(mfd->pdev, state);
+}
+
+static void mddi_early_resume(struct early_suspend *h)
+{
+        struct msm_fb_data_type *mfd = container_of(h, struct msm_fb_data_type,
+                                                        mddi_early_suspend);
+        mddi_resume(mfd->pdev);
+}
+#endif
+
+static int mddi_remove(struct platform_device *pdev)
+{
+        if (mddi_host_timer.function)
+                del_timer_sync(&mddi_host_timer);
+
+        iounmap(msm_pmdh_base);
+
+        return 0;
+}
+
+static int mddi_register_driver(void)
+{
+        return platform_driver_register(&mddi_driver);
+}
+
+static int __init mddi_driver_init(void)
+{
+        int ret;
+
+        mddi_clk = clk_get(NULL, "mddi_clk");
+        if (IS_ERR(mddi_clk)) {
+                printk(KERN_ERR "can't find mddi_clk \n");
+                return PTR_ERR(mddi_clk);
+        }
+        clk_enable(mddi_clk);
+
+        mddi_pclk = clk_get(NULL, "mddi_pclk");
+        if (IS_ERR(mddi_pclk))
+                mddi_pclk = NULL;
+        else
+                clk_enable(mddi_pclk);
+
+        ret = mddi_register_driver();
+        if (ret) {
+                clk_disable(mddi_clk);
+                clk_put(mddi_clk);
+                if (mddi_pclk) {
+                        clk_disable(mddi_pclk);
+                        clk_put(mddi_pclk);
+                }
+                printk(KERN_ERR "mddi_register_driver() failed!\n");
+                return ret;
+        }
+
+        mddi_init();
+
+        return ret;
+}
+
+module_init(mddi_driver_init);
diff --git a/drivers/staging/msm/mddi_ext.c b/drivers/staging/msm/mddi_ext.c
new file mode 100644
index 000000000000..c0c168c7199d
--- /dev/null
+++ b/drivers/staging/msm/mddi_ext.c
@@ -0,0 +1,320 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <mach/hardware.h>
+#include <asm/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/uaccess.h>
+#include <linux/clk.h>
+#include <mach/clk.h>
+#include <linux/platform_device.h>
+
+#include "msm_fb.h"
+#include "mddihosti.h"
+
+static int mddi_ext_probe(struct platform_device *pdev);
+static int mddi_ext_remove(struct platform_device *pdev);
+
+static int mddi_ext_off(struct platform_device *pdev);
+static int mddi_ext_on(struct platform_device *pdev);
+
+static struct platform_device *pdev_list[MSM_FB_MAX_DEV_LIST];
+static int pdev_list_cnt;
+
+static int mddi_ext_suspend(struct platform_device *pdev, pm_message_t state);
+static int mddi_ext_resume(struct platform_device *pdev);
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void mddi_ext_early_suspend(struct early_suspend *h);
+static void mddi_ext_early_resume(struct early_suspend *h);
+#endif
+
+static struct platform_driver mddi_ext_driver = {
+        .probe = mddi_ext_probe,
+        .remove = mddi_ext_remove,
+#ifndef CONFIG_HAS_EARLYSUSPEND
+#ifdef CONFIG_PM
+        .suspend = mddi_ext_suspend,
+        .resume = mddi_ext_resume,
+#endif
+#endif
+        .resume_early = NULL,
+        .resume = NULL,
+        .shutdown = NULL,
+        .driver = {
+                   .name = "mddi_ext",
+                   },
+};
+
+static struct clk *mddi_ext_clk;
+static struct mddi_platform_data *mddi_ext_pdata;
+
+extern int int_mddi_ext_flag;
+
+static int mddi_ext_off(struct platform_device *pdev)
+{
+        int ret = 0;
+
+        ret = panel_next_off(pdev);
+        mddi_host_stop_ext_display();
+
+        return ret;
+}
+
+static int mddi_ext_on(struct platform_device *pdev)
+{
+        int ret = 0;
+        u32 clk_rate;
+        struct msm_fb_data_type *mfd;
+
+        mfd = platform_get_drvdata(pdev);
+
+        clk_rate = mfd->fbi->var.pixclock;
+        clk_rate = min(clk_rate, mfd->panel_info.clk_max);
+
+        if (mddi_ext_pdata &&
+            mddi_ext_pdata->mddi_sel_clk &&
+            mddi_ext_pdata->mddi_sel_clk(&clk_rate))
+                printk(KERN_ERR
+                          "%s: can't select mddi io clk targate rate = %d\n",
+                          __func__, clk_rate);
+
+        if (clk_set_min_rate(mddi_ext_clk, clk_rate) < 0)
+                printk(KERN_ERR "%s: clk_set_min_rate failed\n",
+                        __func__);
+
+        mddi_host_start_ext_display();
+        ret = panel_next_on(pdev);
+
+        return ret;
+}
+
+static int mddi_ext_resource_initialized;
+
+static int mddi_ext_probe(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+        struct platform_device *mdp_dev = NULL;
+        struct msm_fb_panel_data *pdata = NULL;
+        int rc;
+        resource_size_t size ;
+        u32 clk_rate;
+
+        if ((pdev->id == 0) && (pdev->num_resources >= 0)) {
+                mddi_ext_pdata = pdev->dev.platform_data;
+
+                size =  resource_size(&pdev->resource[0]);
+                msm_emdh_base = ioremap(pdev->resource[0].start, size);
+
+                MSM_FB_INFO("external mddi base address = 0x%x\n",
+                                pdev->resource[0].start);
+
+                if (unlikely(!msm_emdh_base))
+                        return -ENOMEM;
+
+                mddi_ext_resource_initialized = 1;
+                return 0;
+        }
+
+        if (!mddi_ext_resource_initialized)
+                return -EPERM;
+
+        mfd = platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        if (pdev_list_cnt >= MSM_FB_MAX_DEV_LIST)
+                return -ENOMEM;
+
+        mdp_dev = platform_device_alloc("mdp", pdev->id);
+        if (!mdp_dev)
+                return -ENOMEM;
+
+        /*
+         * link to the latest pdev
+         */
+        mfd->pdev = mdp_dev;
+        mfd->dest = DISPLAY_EXT_MDDI;
+
+        /*
+         * alloc panel device data
+         */
+        if (platform_device_add_data
+            (mdp_dev, pdev->dev.platform_data,
+             sizeof(struct msm_fb_panel_data))) {
+                printk(KERN_ERR "mddi_ext_probe: platform_device_add_data failed!\n");
+                platform_device_put(mdp_dev);
+                return -ENOMEM;
+        }
+        /*
+         * data chain
+         */
+        pdata = mdp_dev->dev.platform_data;
+        pdata->on = mddi_ext_on;
+        pdata->off = mddi_ext_off;
+        pdata->next = pdev;
+
+        /*
+         * get/set panel specific fb info
+         */
+        mfd->panel_info = pdata->panel_info;
+        mfd->fb_imgType = MDP_RGB_565;
+
+        clk_rate = mfd->panel_info.clk_max;
+        if (mddi_ext_pdata &&
+            mddi_ext_pdata->mddi_sel_clk &&
+            mddi_ext_pdata->mddi_sel_clk(&clk_rate))
+                        printk(KERN_ERR
+                          "%s: can't select mddi io clk targate rate = %d\n",
+                          __func__, clk_rate);
+
+        if (clk_set_max_rate(mddi_ext_clk, clk_rate) < 0)
+                printk(KERN_ERR "%s: clk_set_max_rate failed\n", __func__);
+        mfd->panel_info.clk_rate = mfd->panel_info.clk_min;
+
+        /*
+         * set driver data
+         */
+        platform_set_drvdata(mdp_dev, mfd);
+
+        /*
+         * register in mdp driver
+         */
+        rc = platform_device_add(mdp_dev);
+        if (rc)
+                goto mddi_ext_probe_err;
+
+        pdev_list[pdev_list_cnt++] = pdev;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+        mfd->mddi_ext_early_suspend.level = EARLY_SUSPEND_LEVEL_DISABLE_FB;
+        mfd->mddi_ext_early_suspend.suspend = mddi_ext_early_suspend;
+        mfd->mddi_ext_early_suspend.resume = mddi_ext_early_resume;
+        register_early_suspend(&mfd->mddi_ext_early_suspend);
+#endif
+
+        return 0;
+
+mddi_ext_probe_err:
+        platform_device_put(mdp_dev);
+        return rc;
+}
+
+static int mddi_ext_is_in_suspend;
+
+static int mddi_ext_suspend(struct platform_device *pdev, pm_message_t state)
+{
+        if (mddi_ext_is_in_suspend)
+                return 0;
+
+        mddi_ext_is_in_suspend = 1;
+
+        if (clk_set_min_rate(mddi_ext_clk, 0) < 0)
+                printk(KERN_ERR "%s: clk_set_min_rate failed\n", __func__);
+
+        clk_disable(mddi_ext_clk);
+        disable_irq(INT_MDDI_EXT);
+
+        return 0;
+}
+
+static int mddi_ext_resume(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+
+        mfd = platform_get_drvdata(pdev);
+
+        if (!mddi_ext_is_in_suspend)
+                return 0;
+
+        mddi_ext_is_in_suspend = 0;
+        enable_irq(INT_MDDI_EXT);
+
+        clk_enable(mddi_ext_clk);
+
+        return 0;
+}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void mddi_ext_early_suspend(struct early_suspend *h)
+{
+        pm_message_t state;
+        struct msm_fb_data_type *mfd = container_of(h, struct msm_fb_data_type,
+                                                        mddi_ext_early_suspend);
+
+        state.event = PM_EVENT_SUSPEND;
+        mddi_ext_suspend(mfd->pdev, state);
+}
+
+static void mddi_ext_early_resume(struct early_suspend *h)
+{
+        struct msm_fb_data_type *mfd = container_of(h, struct msm_fb_data_type,
+                                                        mddi_ext_early_suspend);
+        mddi_ext_resume(mfd->pdev);
+}
+#endif
+
+static int mddi_ext_remove(struct platform_device *pdev)
+{
+        iounmap(msm_emdh_base);
+        return 0;
+}
+
+static int mddi_ext_register_driver(void)
+{
+        return platform_driver_register(&mddi_ext_driver);
+}
+
+static int __init mddi_ext_driver_init(void)
+{
+        int ret;
+
+        mddi_ext_clk = clk_get(NULL, "emdh_clk");
+        if (IS_ERR(mddi_ext_clk)) {
+                printk(KERN_ERR "can't find emdh_clk\n");
+                return PTR_ERR(mddi_ext_clk);
+        }
+        clk_enable(mddi_ext_clk);
+
+        ret = mddi_ext_register_driver();
+        if (ret) {
+                clk_disable(mddi_ext_clk);
+                clk_put(mddi_ext_clk);
+                printk(KERN_ERR "mddi_ext_register_driver() failed!\n");
+                return ret;
+        }
+        mddi_init();
+
+        return ret;
+}
+
+module_init(mddi_ext_driver_init);
diff --git a/drivers/staging/msm/mddi_ext_lcd.c b/drivers/staging/msm/mddi_ext_lcd.c
new file mode 100644
index 000000000000..502e80d17ec7
--- /dev/null
+++ b/drivers/staging/msm/mddi_ext_lcd.c
@@ -0,0 +1,91 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+#include "mddihost.h"
+#include "mddihosti.h"
+
+static int mddi_ext_lcd_on(struct platform_device *pdev);
+static int mddi_ext_lcd_off(struct platform_device *pdev);
+
+static int mddi_ext_lcd_on(struct platform_device *pdev)
+{
+        return 0;
+}
+
+static int mddi_ext_lcd_off(struct platform_device *pdev)
+{
+        return 0;
+}
+
+static int __init mddi_ext_lcd_probe(struct platform_device *pdev)
+{
+        msm_fb_add_device(pdev);
+
+        return 0;
+}
+
+static struct platform_driver this_driver = {
+        .probe  = mddi_ext_lcd_probe,
+        .driver = {
+                .name   = "extmddi_svga",
+        },
+};
+
+static struct msm_fb_panel_data mddi_ext_lcd_panel_data = {
+        .panel_info.xres = 800,
+        .panel_info.yres = 600,
+        .panel_info.type = EXT_MDDI_PANEL,
+        .panel_info.pdest = DISPLAY_1,
+        .panel_info.wait_cycle = 0,
+        .panel_info.bpp = 18,
+        .panel_info.fb_num = 2,
+        .panel_info.clk_rate = 122880000,
+        .panel_info.clk_min  = 120000000,
+        .panel_info.clk_max  = 125000000,
+        .on = mddi_ext_lcd_on,
+        .off = mddi_ext_lcd_off,
+};
+
+static struct platform_device this_device = {
+        .name   = "extmddi_svga",
+        .id     = 0,
+        .dev    = {
+                .platform_data = &mddi_ext_lcd_panel_data,
+        }
+};
+
+static int __init mddi_ext_lcd_init(void)
+{
+        int ret;
+        struct msm_panel_info *pinfo;
+
+        ret = platform_driver_register(&this_driver);
+        if (!ret) {
+                pinfo = &mddi_ext_lcd_panel_data.panel_info;
+                pinfo->lcd.vsync_enable = FALSE;
+                pinfo->mddi.vdopkt = MDDI_DEFAULT_PRIM_PIX_ATTR;
+
+                ret = platform_device_register(&this_device);
+                if (ret)
+                        platform_driver_unregister(&this_driver);
+        }
+
+        return ret;
+}
+
+module_init(mddi_ext_lcd_init);
diff --git a/drivers/staging/msm/mddi_prism.c b/drivers/staging/msm/mddi_prism.c
new file mode 100644
index 000000000000..489d40405a5f
--- /dev/null
+++ b/drivers/staging/msm/mddi_prism.c
@@ -0,0 +1,114 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+#include "mddihost.h"
+#include "mddihosti.h"
+
+static int prism_lcd_on(struct platform_device *pdev);
+static int prism_lcd_off(struct platform_device *pdev);
+
+static int prism_lcd_on(struct platform_device *pdev)
+{
+        /* Set the MDP pixel data attributes for Primary Display */
+        mddi_host_write_pix_attr_reg(0x00C3);
+
+        return 0;
+}
+
+static int prism_lcd_off(struct platform_device *pdev)
+{
+        return 0;
+}
+
+static int __init prism_probe(struct platform_device *pdev)
+{
+        msm_fb_add_device(pdev);
+
+        return 0;
+}
+
+static struct platform_driver this_driver = {
+        .probe  = prism_probe,
+        .driver = {
+                .name   = "mddi_prism_wvga",
+        },
+};
+
+static struct msm_fb_panel_data prism_panel_data = {
+        .on = prism_lcd_on,
+        .off = prism_lcd_off,
+};
+
+static struct platform_device this_device = {
+        .name   = "mddi_prism_wvga",
+        .id     = 0,
+        .dev    = {
+                .platform_data = &prism_panel_data,
+        }
+};
+
+static int __init prism_init(void)
+{
+        int ret;
+        struct msm_panel_info *pinfo;
+
+#ifdef CONFIG_FB_MSM_MDDI_AUTO_DETECT
+        u32 id;
+
+        ret = msm_fb_detect_client("mddi_prism_wvga");
+        if (ret == -ENODEV)
+                return 0;
+
+        if (ret) {
+                id = mddi_get_client_id();
+
+                if (((id >> 16) != 0x4474) || ((id & 0xffff) == 0x8960))
+                        return 0;
+        }
+#endif
+        ret = platform_driver_register(&this_driver);
+        if (!ret) {
+                pinfo = &prism_panel_data.panel_info;
+                pinfo->xres = 800;
+                pinfo->yres = 480;
+                pinfo->type = MDDI_PANEL;
+                pinfo->pdest = DISPLAY_1;
+                pinfo->mddi.vdopkt = MDDI_DEFAULT_PRIM_PIX_ATTR;
+                pinfo->wait_cycle = 0;
+                pinfo->bpp = 18;
+                pinfo->fb_num = 2;
+                pinfo->clk_rate = 153600000;
+                pinfo->clk_min = 150000000;
+                pinfo->clk_max = 160000000;
+                pinfo->lcd.vsync_enable = TRUE;
+                pinfo->lcd.refx100 = 6050;
+                pinfo->lcd.v_back_porch = 23;
+                pinfo->lcd.v_front_porch = 20;
+                pinfo->lcd.v_pulse_width = 105;
+                pinfo->lcd.hw_vsync_mode = TRUE;
+                pinfo->lcd.vsync_notifier_period = 0;
+
+                ret = platform_device_register(&this_device);
+                if (ret)
+                        platform_driver_unregister(&this_driver);
+        }
+
+        return ret;
+}
+
+module_init(prism_init);
diff --git a/drivers/staging/msm/mddi_sharp.c b/drivers/staging/msm/mddi_sharp.c
new file mode 100644
index 000000000000..1da1be4052d0
--- /dev/null
+++ b/drivers/staging/msm/mddi_sharp.c
@@ -0,0 +1,892 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+#include "mddihost.h"
+#include "mddihosti.h"
+
+#define SHARP_QVGA_PRIM 1
+#define SHARP_128X128_SECD 2
+
+extern uint32 mddi_host_core_version;
+static boolean mddi_debug_prim_wait = FALSE;
+static boolean mddi_sharp_vsync_wake = TRUE;
+static boolean mddi_sharp_monitor_refresh_value = TRUE;
+static boolean mddi_sharp_report_refresh_measurements = FALSE;
+static uint32 mddi_sharp_rows_per_second = 13830;       /* 5200000/376 */
+static uint32 mddi_sharp_rows_per_refresh = 338;
+static uint32 mddi_sharp_usecs_per_refresh = 24440;     /* (376+338)/5200000 */
+static boolean mddi_sharp_debug_60hz_refresh = FALSE;
+
+extern mddi_gpio_info_type mddi_gpio;
+extern boolean mddi_vsync_detect_enabled;
+static msm_fb_vsync_handler_type mddi_sharp_vsync_handler;
+static void *mddi_sharp_vsync_handler_arg;
+static uint16 mddi_sharp_vsync_attempts;
+
+static void mddi_sharp_prim_lcd_init(void);
+static void mddi_sharp_sub_lcd_init(void);
+static void mddi_sharp_lcd_set_backlight(struct msm_fb_data_type *mfd);
+static void mddi_sharp_vsync_set_handler(msm_fb_vsync_handler_type handler,
+                                         void *);
+static void mddi_sharp_lcd_vsync_detected(boolean detected);
+static struct msm_panel_common_pdata *mddi_sharp_pdata;
+
+#define REG_SYSCTL    0x0000
+#define REG_INTR    0x0006
+#define REG_CLKCNF    0x000C
+#define REG_CLKDIV1    0x000E
+#define REG_CLKDIV2    0x0010
+
+#define REG_GIOD    0x0040
+#define REG_GIOA    0x0042
+
+#define REG_AGM      0x010A
+#define REG_FLFT    0x0110
+#define REG_FRGT    0x0112
+#define REG_FTOP    0x0114
+#define REG_FBTM    0x0116
+#define REG_FSTRX    0x0118
+#define REG_FSTRY    0x011A
+#define REG_VRAM    0x0202
+#define REG_SSDCTL    0x0330
+#define REG_SSD0    0x0332
+#define REG_PSTCTL1    0x0400
+#define REG_PSTCTL2    0x0402
+#define REG_PTGCTL    0x042A
+#define REG_PTHP    0x042C
+#define REG_PTHB    0x042E
+#define REG_PTHW    0x0430
+#define REG_PTHF    0x0432
+#define REG_PTVP    0x0434
+#define REG_PTVB    0x0436
+#define REG_PTVW    0x0438
+#define REG_PTVF    0x043A
+#define REG_VBLKS    0x0458
+#define REG_VBLKE    0x045A
+#define REG_SUBCTL    0x0700
+#define REG_SUBTCMD    0x0702
+#define REG_SUBTCMDD  0x0704
+#define REG_REVBYTE    0x0A02
+#define REG_REVCNT    0x0A04
+#define REG_REVATTR    0x0A06
+#define REG_REVFMT    0x0A08
+
+#define SHARP_SUB_UNKNOWN 0xffffffff
+#define SHARP_SUB_HYNIX 1
+#define SHARP_SUB_ROHM  2
+
+static uint32 sharp_subpanel_type = SHARP_SUB_UNKNOWN;
+
+static void sub_through_write(int sub_rs, uint32 sub_data)
+{
+        mddi_queue_register_write(REG_SUBTCMDD, sub_data, FALSE, 0);
+
+        /* CS=1,RD=1,WE=1,RS=sub_rs */
+        mddi_queue_register_write(REG_SUBTCMD, 0x000e | sub_rs, FALSE, 0);
+
+        /* CS=0,RD=1,WE=1,RS=sub_rs */
+        mddi_queue_register_write(REG_SUBTCMD, 0x0006 | sub_rs, FALSE, 0);
+
+        /* CS=0,RD=1,WE=0,RS=sub_rs */
+        mddi_queue_register_write(REG_SUBTCMD, 0x0004 | sub_rs, FALSE, 0);
+
+        /* CS=0,RD=1,WE=1,RS=sub_rs */
+        mddi_queue_register_write(REG_SUBTCMD, 0x0006 | sub_rs, FALSE, 0);
+
+        /* CS=1,RD=1,WE=1,RS=sub_rs */
+        mddi_queue_register_write(REG_SUBTCMD, 0x000e | sub_rs, TRUE, 0);
+}
+
+static uint32 sub_through_read(int sub_rs)
+{
+        uint32 sub_data;
+
+        /* CS=1,RD=1,WE=1,RS=sub_rs */
+        mddi_queue_register_write(REG_SUBTCMD, 0x000e | sub_rs, FALSE, 0);
+
+        /* CS=0,RD=1,WE=1,RS=sub_rs */
+        mddi_queue_register_write(REG_SUBTCMD, 0x0006 | sub_rs, FALSE, 0);
+
+        /* CS=0,RD=1,WE=0,RS=sub_rs */
+        mddi_queue_register_write(REG_SUBTCMD, 0x0002 | sub_rs, TRUE, 0);
+
+        mddi_queue_register_read(REG_SUBTCMDD, &sub_data, TRUE, 0);
+
+        /* CS=0,RD=1,WE=1,RS=sub_rs */
+        mddi_queue_register_write(REG_SUBTCMD, 0x0006 | sub_rs, FALSE, 0);
+
+        /* CS=1,RD=1,WE=1,RS=sub_rs */
+        mddi_queue_register_write(REG_SUBTCMD, 0x000e | sub_rs, TRUE, 0);
+
+        return sub_data;
+}
+
+static void serigo(uint32 ssd)
+{
+        uint32 ssdctl;
+
+        mddi_queue_register_read(REG_SSDCTL, &ssdctl, TRUE, 0);
+        ssdctl = ((ssdctl & 0xE7) | 0x02);
+
+        mddi_queue_register_write(REG_SSD0, ssd, FALSE, 0);
+        mddi_queue_register_write(REG_SSDCTL, ssdctl, TRUE, 0);
+
+        do {
+                mddi_queue_register_read(REG_SSDCTL, &ssdctl, TRUE, 0);
+        } while ((ssdctl & 0x0002) != 0);
+
+        if (mddi_debug_prim_wait)
+                mddi_wait(2);
+}
+
+static void mddi_sharp_lcd_powerdown(void)
+{
+        serigo(0x0131);
+        serigo(0x0300);
+        mddi_wait(40);
+        serigo(0x0135);
+        mddi_wait(20);
+        serigo(0x2122);
+        mddi_wait(20);
+        serigo(0x0201);
+        mddi_wait(20);
+        serigo(0x2100);
+        mddi_wait(20);
+        serigo(0x2000);
+        mddi_wait(20);
+
+        mddi_queue_register_write(REG_PSTCTL1, 0x1, TRUE, 0);
+        mddi_wait(100);
+        mddi_queue_register_write(REG_PSTCTL1, 0x0, TRUE, 0);
+        mddi_wait(2);
+        mddi_queue_register_write(REG_SYSCTL, 0x1, TRUE, 0);
+        mddi_wait(2);
+        mddi_queue_register_write(REG_CLKDIV1, 0x3, TRUE, 0);
+        mddi_wait(2);
+        mddi_queue_register_write(REG_SSDCTL, 0x0000, TRUE, 0); /* SSDRESET */
+        mddi_queue_register_write(REG_SYSCTL, 0x0, TRUE, 0);
+        mddi_wait(2);
+}
+
+static void mddi_sharp_lcd_set_backlight(struct msm_fb_data_type *mfd)
+{
+        uint32 regdata;
+        int32 level;
+        int max = mfd->panel_info.bl_max;
+        int min = mfd->panel_info.bl_min;
+
+        if (mddi_sharp_pdata && mddi_sharp_pdata->backlight_level) {
+                level = mddi_sharp_pdata->backlight_level(mfd->bl_level,
+                                                          max,
+                                                          min);
+
+                if (level < 0)
+                        return;
+
+                /* use Rodem GPIO(2:0) to give 8 levels of backlight (7-0) */
+                /* Set lower 3 GPIOs as Outputs (set to 0) */
+                mddi_queue_register_read(REG_GIOA, &regdata, TRUE, 0);
+                mddi_queue_register_write(REG_GIOA, regdata & 0xfff8, TRUE, 0);
+
+                /* Set lower 3 GPIOs as level */
+                mddi_queue_register_read(REG_GIOD, &regdata, TRUE, 0);
+                mddi_queue_register_write(REG_GIOD,
+                          (regdata & 0xfff8) | (0x07 & level), TRUE, 0);
+        }
+}
+
+static void mddi_sharp_prim_lcd_init(void)
+{
+        mddi_queue_register_write(REG_SYSCTL, 0x4000, TRUE, 0);
+        mddi_wait(1);
+        mddi_queue_register_write(REG_SYSCTL, 0x0000, TRUE, 0);
+        mddi_wait(5);
+        mddi_queue_register_write(REG_SYSCTL, 0x0001, FALSE, 0);
+        mddi_queue_register_write(REG_CLKDIV1, 0x000b, FALSE, 0);
+
+        /* new reg write below */
+        if (mddi_sharp_debug_60hz_refresh)
+                mddi_queue_register_write(REG_CLKCNF, 0x070d, FALSE, 0);
+        else
+                mddi_queue_register_write(REG_CLKCNF, 0x0708, FALSE, 0);
+
+        mddi_queue_register_write(REG_SYSCTL, 0x0201, FALSE, 0);
+        mddi_queue_register_write(REG_PTGCTL, 0x0010, FALSE, 0);
+        mddi_queue_register_write(REG_PTHP, 4, FALSE, 0);
+        mddi_queue_register_write(REG_PTHB, 40, FALSE, 0);
+        mddi_queue_register_write(REG_PTHW, 240, FALSE, 0);
+        if (mddi_sharp_debug_60hz_refresh)
+                mddi_queue_register_write(REG_PTHF, 12, FALSE, 0);
+        else
+                mddi_queue_register_write(REG_PTHF, 92, FALSE, 0);
+
+        mddi_wait(1);
+
+        mddi_queue_register_write(REG_PTVP, 1, FALSE, 0);
+        mddi_queue_register_write(REG_PTVB, 2, FALSE, 0);
+        mddi_queue_register_write(REG_PTVW, 320, FALSE, 0);
+        mddi_queue_register_write(REG_PTVF, 15, FALSE, 0);
+
+        mddi_wait(1);
+
+        /* vram_color set REG_AGM???? */
+        mddi_queue_register_write(REG_AGM, 0x0000, TRUE, 0);
+
+        mddi_queue_register_write(REG_SSDCTL, 0x0000, FALSE, 0);
+        mddi_queue_register_write(REG_SSDCTL, 0x0001, TRUE, 0);
+        mddi_wait(1);
+        mddi_queue_register_write(REG_PSTCTL1, 0x0001, TRUE, 0);
+        mddi_wait(10);
+
+        serigo(0x0701);
+        /* software reset */
+        mddi_wait(1);
+        /* Wait over 50us */
+
+        serigo(0x0400);
+        /* DCLK~ACHSYNC~ACVSYNC polarity setting */
+        serigo(0x2900);
+        /* EEPROM start read address setting */
+        serigo(0x2606);
+        /* EEPROM start read register setting */
+        mddi_wait(20);
+        /* Wait over 20ms */
+
+        serigo(0x0503);
+        /* Horizontal timing setting */
+        serigo(0x062C);
+        /* Veritical timing setting */
+        serigo(0x2001);
+        /* power initialize setting(VDC2) */
+        mddi_wait(20);
+        /* Wait over 20ms */
+
+        serigo(0x2120);
+        /* Initialize power setting(CPS) */
+        mddi_wait(20);
+        /* Wait over 20ms */
+
+        serigo(0x2130);
+        /* Initialize power setting(CPS) */
+        mddi_wait(20);
+        /* Wait over 20ms */
+
+        serigo(0x2132);
+        /* Initialize power setting(CPS) */
+        mddi_wait(10);
+        /* Wait over 10ms */
+
+        serigo(0x2133);
+        /* Initialize power setting(CPS) */
+        mddi_wait(20);
+        /* Wait over 20ms */
+
+        serigo(0x0200);
+        /* Panel initialize release(INIT) */
+        mddi_wait(1);
+        /* Wait over 1ms */
+
+        serigo(0x0131);
+        /* Panel setting(CPS) */
+        mddi_wait(1);
+        /* Wait over 1ms */
+
+        mddi_queue_register_write(REG_PSTCTL1, 0x0003, TRUE, 0);
+
+        /* if (FFA LCD is upside down) -> serigo(0x0100); */
+        serigo(0x0130);
+
+        /* Black mask release(display ON) */
+        mddi_wait(1);
+        /* Wait over 1ms */
+
+        if (mddi_sharp_vsync_wake) {
+                mddi_queue_register_write(REG_VBLKS, 0x1001, TRUE, 0);
+                mddi_queue_register_write(REG_VBLKE, 0x1002, TRUE, 0);
+        }
+
+        /* Set the MDP pixel data attributes for Primary Display */
+        mddi_host_write_pix_attr_reg(0x00C3);
+        return;
+
+}
+
+void mddi_sharp_sub_lcd_init(void)
+{
+
+        mddi_queue_register_write(REG_SYSCTL, 0x4000, FALSE, 0);
+        mddi_queue_register_write(REG_SYSCTL, 0x0000, TRUE, 0);
+        mddi_wait(100);
+
+        mddi_queue_register_write(REG_SYSCTL, 0x0001, FALSE, 0);
+        mddi_queue_register_write(REG_CLKDIV1, 0x000b, FALSE, 0);
+        mddi_queue_register_write(REG_CLKCNF, 0x0708, FALSE, 0);
+        mddi_queue_register_write(REG_SYSCTL, 0x0201, FALSE, 0);
+        mddi_queue_register_write(REG_PTGCTL, 0x0010, FALSE, 0);
+        mddi_queue_register_write(REG_PTHP, 4, FALSE, 0);
+        mddi_queue_register_write(REG_PTHB, 40, FALSE, 0);
+        mddi_queue_register_write(REG_PTHW, 128, FALSE, 0);
+        mddi_queue_register_write(REG_PTHF, 92, FALSE, 0);
+        mddi_queue_register_write(REG_PTVP, 1, FALSE, 0);
+        mddi_queue_register_write(REG_PTVB, 2, FALSE, 0);
+        mddi_queue_register_write(REG_PTVW, 128, FALSE, 0);
+        mddi_queue_register_write(REG_PTVF, 15, FALSE, 0);
+
+        /* Now the sub display..... */
+        /* Reset High */
+        mddi_queue_register_write(REG_SUBCTL, 0x0200, FALSE, 0);
+        /* CS=1,RD=1,WE=1,RS=1 */
+        mddi_queue_register_write(REG_SUBTCMD, 0x000f, TRUE, 0);
+        mddi_wait(1);
+        /* Wait 5us */
+
+        if (sharp_subpanel_type == SHARP_SUB_UNKNOWN) {
+                uint32 data;
+
+                sub_through_write(1, 0x05);
+                sub_through_write(1, 0x6A);
+                sub_through_write(1, 0x1D);
+                sub_through_write(1, 0x05);
+                data = sub_through_read(1);
+                if (data == 0x6A) {
+                        sharp_subpanel_type = SHARP_SUB_HYNIX;
+                } else {
+                        sub_through_write(0, 0x36);
+                        sub_through_write(1, 0xA8);
+                        sub_through_write(0, 0x09);
+                        data = sub_through_read(1);
+                        data = sub_through_read(1);
+                        if (data == 0x54) {
+                                sub_through_write(0, 0x36);
+                                sub_through_write(1, 0x00);
+                                sharp_subpanel_type = SHARP_SUB_ROHM;
+                        }
+                }
+        }
+
+        if (sharp_subpanel_type == SHARP_SUB_HYNIX) {
+                sub_through_write(1, 0x00);     /* Display setting 1 */
+                sub_through_write(1, 0x04);
+                sub_through_write(1, 0x01);
+                sub_through_write(1, 0x05);
+                sub_through_write(1, 0x0280);
+                sub_through_write(1, 0x0301);
+                sub_through_write(1, 0x0402);
+                sub_through_write(1, 0x0500);
+                sub_through_write(1, 0x0681);
+                sub_through_write(1, 0x077F);
+                sub_through_write(1, 0x08C0);
+                sub_through_write(1, 0x0905);
+                sub_through_write(1, 0x0A02);
+                sub_through_write(1, 0x0B00);
+                sub_through_write(1, 0x0C00);
+                sub_through_write(1, 0x0D00);
+                sub_through_write(1, 0x0E00);
+                sub_through_write(1, 0x0F00);
+
+                sub_through_write(1, 0x100B);   /* Display setting 2 */
+                sub_through_write(1, 0x1103);
+                sub_through_write(1, 0x1237);
+                sub_through_write(1, 0x1300);
+                sub_through_write(1, 0x1400);
+                sub_through_write(1, 0x1500);
+                sub_through_write(1, 0x1605);
+                sub_through_write(1, 0x1700);
+                sub_through_write(1, 0x1800);
+                sub_through_write(1, 0x192E);
+                sub_through_write(1, 0x1A00);
+                sub_through_write(1, 0x1B00);
+                sub_through_write(1, 0x1C00);
+
+                sub_through_write(1, 0x151A);   /* Power setting */
+
+                sub_through_write(1, 0x2002);   /* Gradation Palette setting */
+                sub_through_write(1, 0x2107);
+                sub_through_write(1, 0x220C);
+                sub_through_write(1, 0x2310);
+                sub_through_write(1, 0x2414);
+                sub_through_write(1, 0x2518);
+                sub_through_write(1, 0x261C);
+                sub_through_write(1, 0x2720);
+                sub_through_write(1, 0x2824);
+                sub_through_write(1, 0x2928);
+                sub_through_write(1, 0x2A2B);
+                sub_through_write(1, 0x2B2E);
+                sub_through_write(1, 0x2C31);
+                sub_through_write(1, 0x2D34);
+                sub_through_write(1, 0x2E37);
+                sub_through_write(1, 0x2F3A);
+                sub_through_write(1, 0x303C);
+                sub_through_write(1, 0x313E);
+                sub_through_write(1, 0x323F);
+                sub_through_write(1, 0x3340);
+                sub_through_write(1, 0x3441);
+                sub_through_write(1, 0x3543);
+                sub_through_write(1, 0x3646);
+                sub_through_write(1, 0x3749);
+                sub_through_write(1, 0x384C);
+                sub_through_write(1, 0x394F);
+                sub_through_write(1, 0x3A52);
+                sub_through_write(1, 0x3B59);
+                sub_through_write(1, 0x3C60);
+                sub_through_write(1, 0x3D67);
+                sub_through_write(1, 0x3E6E);
+                sub_through_write(1, 0x3F7F);
+                sub_through_write(1, 0x4001);
+                sub_through_write(1, 0x4107);
+                sub_through_write(1, 0x420C);
+                sub_through_write(1, 0x4310);
+                sub_through_write(1, 0x4414);
+                sub_through_write(1, 0x4518);
+                sub_through_write(1, 0x461C);
+                sub_through_write(1, 0x4720);
+                sub_through_write(1, 0x4824);
+                sub_through_write(1, 0x4928);
+                sub_through_write(1, 0x4A2B);
+                sub_through_write(1, 0x4B2E);
+                sub_through_write(1, 0x4C31);
+                sub_through_write(1, 0x4D34);
+                sub_through_write(1, 0x4E37);
+                sub_through_write(1, 0x4F3A);
+                sub_through_write(1, 0x503C);
+                sub_through_write(1, 0x513E);
+                sub_through_write(1, 0x523F);
+                sub_through_write(1, 0x5340);
+                sub_through_write(1, 0x5441);
+                sub_through_write(1, 0x5543);
+                sub_through_write(1, 0x5646);
+                sub_through_write(1, 0x5749);
+                sub_through_write(1, 0x584C);
+                sub_through_write(1, 0x594F);
+                sub_through_write(1, 0x5A52);
+                sub_through_write(1, 0x5B59);
+                sub_through_write(1, 0x5C60);
+                sub_through_write(1, 0x5D67);
+                sub_through_write(1, 0x5E6E);
+                sub_through_write(1, 0x5F7E);
+                sub_through_write(1, 0x6000);
+                sub_through_write(1, 0x6107);
+                sub_through_write(1, 0x620C);
+                sub_through_write(1, 0x6310);
+                sub_through_write(1, 0x6414);
+                sub_through_write(1, 0x6518);
+                sub_through_write(1, 0x661C);
+                sub_through_write(1, 0x6720);
+                sub_through_write(1, 0x6824);
+                sub_through_write(1, 0x6928);
+                sub_through_write(1, 0x6A2B);
+                sub_through_write(1, 0x6B2E);
+                sub_through_write(1, 0x6C31);
+                sub_through_write(1, 0x6D34);
+                sub_through_write(1, 0x6E37);
+                sub_through_write(1, 0x6F3A);
+                sub_through_write(1, 0x703C);
+                sub_through_write(1, 0x713E);
+                sub_through_write(1, 0x723F);
+                sub_through_write(1, 0x7340);
+                sub_through_write(1, 0x7441);
+                sub_through_write(1, 0x7543);
+                sub_through_write(1, 0x7646);
+                sub_through_write(1, 0x7749);
+                sub_through_write(1, 0x784C);
+                sub_through_write(1, 0x794F);
+                sub_through_write(1, 0x7A52);
+                sub_through_write(1, 0x7B59);
+                sub_through_write(1, 0x7C60);
+                sub_through_write(1, 0x7D67);
+                sub_through_write(1, 0x7E6E);
+                sub_through_write(1, 0x7F7D);
+
+                sub_through_write(1, 0x1851);   /* Display on */
+
+                mddi_queue_register_write(REG_AGM, 0x0000, TRUE, 0);
+
+                /* 1 pixel / 1 post clock */
+                mddi_queue_register_write(REG_CLKDIV2, 0x3b00, FALSE, 0);
+
+                /* SUB LCD select */
+                mddi_queue_register_write(REG_PSTCTL2, 0x0080, FALSE, 0);
+
+                /* RS=0,command initiate number=0,select master mode */
+                mddi_queue_register_write(REG_SUBCTL, 0x0202, FALSE, 0);
+
+                /* Sub LCD Data transform start */
+                mddi_queue_register_write(REG_PSTCTL1, 0x0003, FALSE, 0);
+
+        } else if (sharp_subpanel_type == SHARP_SUB_ROHM) {
+
+                sub_through_write(0, 0x01);     /* Display setting */
+                sub_through_write(1, 0x00);
+
+                mddi_wait(1);
+                /* Wait 100us  <----- ******* Update 2005/01/24 */
+
+                sub_through_write(0, 0xB6);
+                sub_through_write(1, 0x0C);
+                sub_through_write(1, 0x4A);
+                sub_through_write(1, 0x20);
+                sub_through_write(0, 0x3A);
+                sub_through_write(1, 0x05);
+                sub_through_write(0, 0xB7);
+                sub_through_write(1, 0x01);
+                sub_through_write(0, 0xBA);
+                sub_through_write(1, 0x20);
+                sub_through_write(1, 0x02);
+                sub_through_write(0, 0x25);
+                sub_through_write(1, 0x4F);
+                sub_through_write(0, 0xBB);
+                sub_through_write(1, 0x00);
+                sub_through_write(0, 0x36);
+                sub_through_write(1, 0x00);
+                sub_through_write(0, 0xB1);
+                sub_through_write(1, 0x05);
+                sub_through_write(0, 0xBE);
+                sub_through_write(1, 0x80);
+                sub_through_write(0, 0x26);
+                sub_through_write(1, 0x01);
+                sub_through_write(0, 0x2A);
+                sub_through_write(1, 0x02);
+                sub_through_write(1, 0x81);
+                sub_through_write(0, 0x2B);
+                sub_through_write(1, 0x00);
+                sub_through_write(1, 0x7F);
+
+                sub_through_write(0, 0x2C);
+                sub_through_write(0, 0x11);     /* Sleep mode off */
+
+                mddi_wait(1);
+                /* Wait 100 ms <----- ******* Update 2005/01/24 */
+
+                sub_through_write(0, 0x29);     /* Display on */
+                sub_through_write(0, 0xB3);
+                sub_through_write(1, 0x20);
+                sub_through_write(1, 0xAA);
+                sub_through_write(1, 0xA0);
+                sub_through_write(1, 0x20);
+                sub_through_write(1, 0x30);
+                sub_through_write(1, 0xA6);
+                sub_through_write(1, 0xFF);
+                sub_through_write(1, 0x9A);
+                sub_through_write(1, 0x9F);
+                sub_through_write(1, 0xAF);
+                sub_through_write(1, 0xBC);
+                sub_through_write(1, 0xCF);
+                sub_through_write(1, 0xDF);
+                sub_through_write(1, 0x20);
+                sub_through_write(1, 0x9C);
+                sub_through_write(1, 0x8A);
+
+                sub_through_write(0, 0x002C);   /* Display on */
+
+                /* 1 pixel / 2 post clock */
+                mddi_queue_register_write(REG_CLKDIV2, 0x7b00, FALSE, 0);
+
+                /* SUB LCD select */
+                mddi_queue_register_write(REG_PSTCTL2, 0x0080, FALSE, 0);
+
+                /* RS=1,command initiate number=0,select master mode */
+                mddi_queue_register_write(REG_SUBCTL, 0x0242, FALSE, 0);
+
+                /* Sub LCD Data transform start */
+                mddi_queue_register_write(REG_PSTCTL1, 0x0003, FALSE, 0);
+
+        }
+
+        /* Set the MDP pixel data attributes for Sub Display */
+        mddi_host_write_pix_attr_reg(0x00C0);
+}
+
+void mddi_sharp_lcd_vsync_detected(boolean detected)
+{
+        /* static timetick_type start_time = 0; */
+        static struct timeval start_time;
+        static boolean first_time = TRUE;
+        /* uint32 mdp_cnt_val = 0; */
+        /* timetick_type elapsed_us; */
+        struct timeval now;
+        uint32 elapsed_us;
+        uint32 num_vsyncs;
+
+        if ((detected) || (mddi_sharp_vsync_attempts > 5)) {
+                if ((detected) && (mddi_sharp_monitor_refresh_value)) {
+                        /* if (start_time != 0) */
+                        if (!first_time) {
+                                jiffies_to_timeval(jiffies, &now);
+                                elapsed_us =
+                                    (now.tv_sec - start_time.tv_sec) * 1000000 +
+                                    now.tv_usec - start_time.tv_usec;
+                                /*
+                                * LCD is configured for a refresh every usecs,
+                                * so to determine the number of vsyncs that
+                                * have occurred since the last measurement add
+                                * half that to the time difference and divide
+                                * by the refresh rate.
+                                */
+                                num_vsyncs = (elapsed_us +
+                                              (mddi_sharp_usecs_per_refresh >>
+                                               1)) /
+                                    mddi_sharp_usecs_per_refresh;
+                                /*
+                                 * LCD is configured for * hsyncs (rows) per
+                                 * refresh cycle. Calculate new rows_per_second
+                                 * value based upon these new measurements.
+                                 * MDP can update with this new value.
+                                 */
+                                mddi_sharp_rows_per_second =
+                                    (mddi_sharp_rows_per_refresh * 1000 *
+                                     num_vsyncs) / (elapsed_us / 1000);
+                        }
+                        /* start_time = timetick_get(); */
+                        first_time = FALSE;
+                        jiffies_to_timeval(jiffies, &start_time);
+                        if (mddi_sharp_report_refresh_measurements) {
+                                /* mdp_cnt_val = MDP_LINE_COUNT; */
+                        }
+                }
+                /* if detected = TRUE, client initiated wakeup was detected */
+                if (mddi_sharp_vsync_handler != NULL) {
+                        (*mddi_sharp_vsync_handler)
+                            (mddi_sharp_vsync_handler_arg);
+                        mddi_sharp_vsync_handler = NULL;
+                }
+                mddi_vsync_detect_enabled = FALSE;
+                mddi_sharp_vsync_attempts = 0;
+                /* need to clear this vsync wakeup */
+                if (!mddi_queue_register_write_int(REG_INTR, 0x0000)) {
+                        MDDI_MSG_ERR("Vsync interrupt clear failed!\n");
+                }
+                if (!detected) {
+                        /* give up after 5 failed attempts but show error */
+                        MDDI_MSG_NOTICE("Vsync detection failed!\n");
+                } else if ((mddi_sharp_monitor_refresh_value) &&
+                        (mddi_sharp_report_refresh_measurements)) {
+                        MDDI_MSG_NOTICE("  Lines Per Second=%d!\n",
+                                mddi_sharp_rows_per_second);
+                }
+        } else
+                /* if detected = FALSE, we woke up from hibernation, but did not
+                 * detect client initiated wakeup.
+                 */
+                mddi_sharp_vsync_attempts++;
+}
+
+/* ISR to be executed */
+void mddi_sharp_vsync_set_handler(msm_fb_vsync_handler_type handler, void *arg)
+{
+        boolean error = FALSE;
+        unsigned long flags;
+
+        /* Disable interrupts */
+        spin_lock_irqsave(&mddi_host_spin_lock, flags);
+        /* INTLOCK(); */
+
+        if (mddi_sharp_vsync_handler != NULL)
+                error = TRUE;
+
+        /* Register the handler for this particular GROUP interrupt source */
+        mddi_sharp_vsync_handler = handler;
+        mddi_sharp_vsync_handler_arg = arg;
+
+        /* Restore interrupts */
+        spin_unlock_irqrestore(&mddi_host_spin_lock, flags);
+        /* INTFREE(); */
+
+        if (error)
+                MDDI_MSG_ERR("MDDI: Previous Vsync handler never called\n");
+
+        /* Enable the vsync wakeup */
+        mddi_queue_register_write(REG_INTR, 0x8100, FALSE, 0);
+
+        mddi_sharp_vsync_attempts = 1;
+        mddi_vsync_detect_enabled = TRUE;
+}                               /* mddi_sharp_vsync_set_handler */
+
+static int mddi_sharp_lcd_on(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+
+        mfd = platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        if (mfd->panel.id == SHARP_QVGA_PRIM)
+                mddi_sharp_prim_lcd_init();
+        else
+                mddi_sharp_sub_lcd_init();
+
+        return 0;
+}
+
+static int mddi_sharp_lcd_off(struct platform_device *pdev)
+{
+        mddi_sharp_lcd_powerdown();
+        return 0;
+}
+
+static int __init mddi_sharp_probe(struct platform_device *pdev)
+{
+        if (pdev->id == 0) {
+                mddi_sharp_pdata = pdev->dev.platform_data;
+                return 0;
+        }
+
+        msm_fb_add_device(pdev);
+
+        return 0;
+}
+
+static struct platform_driver this_driver = {
+        .probe  = mddi_sharp_probe,
+        .driver = {
+                .name   = "mddi_sharp_qvga",
+        },
+};
+
+static struct msm_fb_panel_data mddi_sharp_panel_data0 = {
+        .on = mddi_sharp_lcd_on,
+        .off = mddi_sharp_lcd_off,
+        .set_backlight = mddi_sharp_lcd_set_backlight,
+        .set_vsync_notifier = mddi_sharp_vsync_set_handler,
+};
+
+static struct platform_device this_device_0 = {
+        .name   = "mddi_sharp_qvga",
+        .id     = SHARP_QVGA_PRIM,
+        .dev    = {
+                .platform_data = &mddi_sharp_panel_data0,
+        }
+};
+
+static struct msm_fb_panel_data mddi_sharp_panel_data1 = {
+        .on = mddi_sharp_lcd_on,
+        .off = mddi_sharp_lcd_off,
+};
+
+static struct platform_device this_device_1 = {
+        .name   = "mddi_sharp_qvga",
+        .id     = SHARP_128X128_SECD,
+        .dev    = {
+                .platform_data = &mddi_sharp_panel_data1,
+        }
+};
+
+static int __init mddi_sharp_init(void)
+{
+        int ret;
+        struct msm_panel_info *pinfo;
+
+#ifdef CONFIG_FB_MSM_MDDI_AUTO_DETECT
+        u32 id;
+
+        ret = msm_fb_detect_client("mddi_sharp_qvga");
+        if (ret == -ENODEV)
+                return 0;
+
+        if (ret) {
+                id = mddi_get_client_id();
+
+                if (((id >> 16) != 0x0) || ((id & 0xffff) != 0x8835))
+                        return 0;
+        }
+#endif
+        if (mddi_host_core_version > 8) {
+                /* can use faster refresh with newer hw revisions */
+                mddi_sharp_debug_60hz_refresh = TRUE;
+
+                /* Timing variables for tracking vsync */
+                /* dot_clock = 6.00MHz
+                 * horizontal count = 296
+                 * vertical count = 338
+                 * refresh rate = 6000000/(296+338) = 60Hz
+                 */
+                mddi_sharp_rows_per_second = 20270;     /* 6000000/296 */
+                mddi_sharp_rows_per_refresh = 338;
+                mddi_sharp_usecs_per_refresh = 16674;   /* (296+338)/6000000 */
+        } else {
+                /* Timing variables for tracking vsync */
+                /* dot_clock = 5.20MHz
+                 * horizontal count = 376
+                 * vertical count = 338
+                 * refresh rate = 5200000/(376+338) = 41Hz
+                 */
+                mddi_sharp_rows_per_second = 13830;     /* 5200000/376 */
+                mddi_sharp_rows_per_refresh = 338;
+                mddi_sharp_usecs_per_refresh = 24440;   /* (376+338)/5200000 */
+        }
+
+        ret = platform_driver_register(&this_driver);
+        if (!ret) {
+                pinfo = &mddi_sharp_panel_data0.panel_info;
+                pinfo->xres = 240;
+                pinfo->yres = 320;
+                pinfo->type = MDDI_PANEL;
+                pinfo->pdest = DISPLAY_1;
+                pinfo->mddi.vdopkt = MDDI_DEFAULT_PRIM_PIX_ATTR;
+                pinfo->wait_cycle = 0;
+                pinfo->bpp = 18;
+                pinfo->fb_num = 2;
+                pinfo->clk_rate = 122880000;
+                pinfo->clk_min = 120000000;
+                pinfo->clk_max = 125000000;
+                pinfo->lcd.vsync_enable = TRUE;
+                pinfo->lcd.refx100 =
+                        (mddi_sharp_rows_per_second * 100) /
+                        mddi_sharp_rows_per_refresh;
+                pinfo->lcd.v_back_porch = 12;
+                pinfo->lcd.v_front_porch = 6;
+                pinfo->lcd.v_pulse_width = 0;
+                pinfo->lcd.hw_vsync_mode = FALSE;
+                pinfo->lcd.vsync_notifier_period = (1 * HZ);
+                pinfo->bl_max = 7;
+                pinfo->bl_min = 1;
+
+                ret = platform_device_register(&this_device_0);
+                if (ret)
+                        platform_driver_unregister(&this_driver);
+
+                pinfo = &mddi_sharp_panel_data1.panel_info;
+                pinfo->xres = 128;
+                pinfo->yres = 128;
+                pinfo->type = MDDI_PANEL;
+                pinfo->pdest = DISPLAY_2;
+                pinfo->mddi.vdopkt = 0x400;
+                pinfo->wait_cycle = 0;
+                pinfo->bpp = 18;
+                pinfo->clk_rate = 122880000;
+                pinfo->clk_min = 120000000;
+                pinfo->clk_max = 125000000;
+                pinfo->fb_num = 2;
+
+                ret = platform_device_register(&this_device_1);
+                if (ret) {
+                        platform_device_unregister(&this_device_0);
+                        platform_driver_unregister(&this_driver);
+                }
+        }
+
+        if (!ret)
+                mddi_lcd.vsync_detected = mddi_sharp_lcd_vsync_detected;
+
+        return ret;
+}
+
+module_init(mddi_sharp_init);
diff --git a/drivers/staging/msm/mddi_toshiba.c b/drivers/staging/msm/mddi_toshiba.c
new file mode 100644
index 000000000000..e96342d477a2
--- /dev/null
+++ b/drivers/staging/msm/mddi_toshiba.c
@@ -0,0 +1,1741 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+#include "mddihost.h"
+#include "mddihosti.h"
+#include "mddi_toshiba.h"
+
+#define TM_GET_DID(id) ((id) & 0xff)
+#define TM_GET_PID(id) (((id) & 0xff00)>>8)
+
+#define MDDI_CLIENT_CORE_BASE  0x108000
+#define LCD_CONTROL_BLOCK_BASE 0x110000
+#define SPI_BLOCK_BASE         0x120000
+#define PWM_BLOCK_BASE         0x140000
+#define SYSTEM_BLOCK1_BASE     0x160000
+
+#define TTBUSSEL    (MDDI_CLIENT_CORE_BASE|0x18)
+#define DPSET0      (MDDI_CLIENT_CORE_BASE|0x1C)
+#define DPSET1      (MDDI_CLIENT_CORE_BASE|0x20)
+#define DPSUS       (MDDI_CLIENT_CORE_BASE|0x24)
+#define DPRUN       (MDDI_CLIENT_CORE_BASE|0x28)
+#define SYSCKENA    (MDDI_CLIENT_CORE_BASE|0x2C)
+
+#define BITMAP0     (MDDI_CLIENT_CORE_BASE|0x44)
+#define BITMAP1     (MDDI_CLIENT_CORE_BASE|0x48)
+#define BITMAP2     (MDDI_CLIENT_CORE_BASE|0x4C)
+#define BITMAP3     (MDDI_CLIENT_CORE_BASE|0x50)
+#define BITMAP4     (MDDI_CLIENT_CORE_BASE|0x54)
+
+#define SRST        (LCD_CONTROL_BLOCK_BASE|0x00)
+#define PORT_ENB    (LCD_CONTROL_BLOCK_BASE|0x04)
+#define START       (LCD_CONTROL_BLOCK_BASE|0x08)
+#define PORT        (LCD_CONTROL_BLOCK_BASE|0x0C)
+
+#define INTFLG      (LCD_CONTROL_BLOCK_BASE|0x18)
+#define INTMSK      (LCD_CONTROL_BLOCK_BASE|0x1C)
+#define MPLFBUF     (LCD_CONTROL_BLOCK_BASE|0x20)
+
+#define PXL         (LCD_CONTROL_BLOCK_BASE|0x30)
+#define HCYCLE      (LCD_CONTROL_BLOCK_BASE|0x34)
+#define HSW         (LCD_CONTROL_BLOCK_BASE|0x38)
+#define HDE_START   (LCD_CONTROL_BLOCK_BASE|0x3C)
+#define HDE_SIZE    (LCD_CONTROL_BLOCK_BASE|0x40)
+#define VCYCLE      (LCD_CONTROL_BLOCK_BASE|0x44)
+#define VSW         (LCD_CONTROL_BLOCK_BASE|0x48)
+#define VDE_START   (LCD_CONTROL_BLOCK_BASE|0x4C)
+#define VDE_SIZE    (LCD_CONTROL_BLOCK_BASE|0x50)
+#define WAKEUP      (LCD_CONTROL_BLOCK_BASE|0x54)
+#define REGENB      (LCD_CONTROL_BLOCK_BASE|0x5C)
+#define VSYNIF      (LCD_CONTROL_BLOCK_BASE|0x60)
+#define WRSTB       (LCD_CONTROL_BLOCK_BASE|0x64)
+#define RDSTB       (LCD_CONTROL_BLOCK_BASE|0x68)
+#define ASY_DATA    (LCD_CONTROL_BLOCK_BASE|0x6C)
+#define ASY_DATB    (LCD_CONTROL_BLOCK_BASE|0x70)
+#define ASY_DATC    (LCD_CONTROL_BLOCK_BASE|0x74)
+#define ASY_DATD    (LCD_CONTROL_BLOCK_BASE|0x78)
+#define ASY_DATE    (LCD_CONTROL_BLOCK_BASE|0x7C)
+#define ASY_DATF    (LCD_CONTROL_BLOCK_BASE|0x80)
+#define ASY_DATG    (LCD_CONTROL_BLOCK_BASE|0x84)
+#define ASY_DATH    (LCD_CONTROL_BLOCK_BASE|0x88)
+#define ASY_CMDSET  (LCD_CONTROL_BLOCK_BASE|0x8C)
+#define MONI        (LCD_CONTROL_BLOCK_BASE|0xB0)
+#define VPOS        (LCD_CONTROL_BLOCK_BASE|0xC0)
+
+#define SSICTL      (SPI_BLOCK_BASE|0x00)
+#define SSITIME     (SPI_BLOCK_BASE|0x04)
+#define SSITX       (SPI_BLOCK_BASE|0x08)
+#define SSIINTS     (SPI_BLOCK_BASE|0x14)
+
+#define TIMER0LOAD    (PWM_BLOCK_BASE|0x00)
+#define TIMER0CTRL    (PWM_BLOCK_BASE|0x08)
+#define PWM0OFF       (PWM_BLOCK_BASE|0x1C)
+#define TIMER1LOAD    (PWM_BLOCK_BASE|0x20)
+#define TIMER1CTRL    (PWM_BLOCK_BASE|0x28)
+#define PWM1OFF       (PWM_BLOCK_BASE|0x3C)
+#define TIMER2LOAD    (PWM_BLOCK_BASE|0x40)
+#define TIMER2CTRL    (PWM_BLOCK_BASE|0x48)
+#define PWM2OFF       (PWM_BLOCK_BASE|0x5C)
+#define PWMCR         (PWM_BLOCK_BASE|0x68)
+
+#define GPIOIS      (GPIO_BLOCK_BASE|0x08)
+#define GPIOIEV     (GPIO_BLOCK_BASE|0x10)
+#define GPIOIC      (GPIO_BLOCK_BASE|0x20)
+
+#define WKREQ       (SYSTEM_BLOCK1_BASE|0x00)
+#define CLKENB      (SYSTEM_BLOCK1_BASE|0x04)
+#define DRAMPWR     (SYSTEM_BLOCK1_BASE|0x08)
+#define INTMASK     (SYSTEM_BLOCK1_BASE|0x0C)
+#define CNT_DIS     (SYSTEM_BLOCK1_BASE|0x10)
+
+typedef enum {
+        TOSHIBA_STATE_OFF,
+        TOSHIBA_STATE_PRIM_SEC_STANDBY,
+        TOSHIBA_STATE_PRIM_SEC_READY,
+        TOSHIBA_STATE_PRIM_NORMAL_MODE,
+        TOSHIBA_STATE_SEC_NORMAL_MODE
+} mddi_toshiba_state_t;
+
+static uint32 mddi_toshiba_curr_vpos;
+static boolean mddi_toshiba_monitor_refresh_value = FALSE;
+static boolean mddi_toshiba_report_refresh_measurements = FALSE;
+
+boolean mddi_toshiba_61Hz_refresh = TRUE;
+
+/* Modifications to timing to increase refresh rate to > 60Hz.
+ *   20MHz dot clock.
+ *   646 total rows.
+ *   506 total columns.
+ *   refresh rate = 61.19Hz
+ */
+static uint32 mddi_toshiba_rows_per_second = 39526;
+static uint32 mddi_toshiba_usecs_per_refresh = 16344;
+static uint32 mddi_toshiba_rows_per_refresh = 646;
+extern boolean mddi_vsync_detect_enabled;
+
+static msm_fb_vsync_handler_type mddi_toshiba_vsync_handler;
+static void *mddi_toshiba_vsync_handler_arg;
+static uint16 mddi_toshiba_vsync_attempts;
+
+static mddi_toshiba_state_t toshiba_state = TOSHIBA_STATE_OFF;
+
+static struct msm_panel_common_pdata *mddi_toshiba_pdata;
+
+static int mddi_toshiba_lcd_on(struct platform_device *pdev);
+static int mddi_toshiba_lcd_off(struct platform_device *pdev);
+
+static void mddi_toshiba_state_transition(mddi_toshiba_state_t a,
+                                          mddi_toshiba_state_t b)
+{
+        if (toshiba_state != a) {
+                MDDI_MSG_ERR("toshiba state trans. (%d->%d) found %d\n", a, b,
+                             toshiba_state);
+        }
+        toshiba_state = b;
+}
+
+#define GORDON_REG_IMGCTL1      0x10    /* Image interface control 1   */
+#define GORDON_REG_IMGCTL2      0x11    /* Image interface control 2   */
+#define GORDON_REG_IMGSET1      0x12    /* Image interface settings 1  */
+#define GORDON_REG_IMGSET2      0x13    /* Image interface settings 2  */
+#define GORDON_REG_IVBP1        0x14    /* DM0: Vert back porch        */
+#define GORDON_REG_IHBP1        0x15    /* DM0: Horiz back porch       */
+#define GORDON_REG_IVNUM1       0x16    /* DM0: Num of vert lines      */
+#define GORDON_REG_IHNUM1       0x17    /* DM0: Num of pixels per line */
+#define GORDON_REG_IVBP2        0x18    /* DM1: Vert back porch        */
+#define GORDON_REG_IHBP2        0x19    /* DM1: Horiz back porch       */
+#define GORDON_REG_IVNUM2       0x1A    /* DM1: Num of vert lines      */
+#define GORDON_REG_IHNUM2       0x1B    /* DM1: Num of pixels per line */
+#define GORDON_REG_LCDIFCTL1    0x30    /* LCD interface control 1     */
+#define GORDON_REG_VALTRAN      0x31    /* LCD IF ctl: VALTRAN sync flag */
+#define GORDON_REG_AVCTL        0x33
+#define GORDON_REG_LCDIFCTL2    0x34    /* LCD interface control 2     */
+#define GORDON_REG_LCDIFCTL3    0x35    /* LCD interface control 3     */
+#define GORDON_REG_LCDIFSET1    0x36    /* LCD interface settings 1    */
+#define GORDON_REG_PCCTL        0x3C
+#define GORDON_REG_TPARAM1      0x40
+#define GORDON_REG_TLCDIF1      0x41
+#define GORDON_REG_TSSPB_ST1    0x42
+#define GORDON_REG_TSSPB_ED1    0x43
+#define GORDON_REG_TSCK_ST1     0x44
+#define GORDON_REG_TSCK_WD1     0x45
+#define GORDON_REG_TGSPB_VST1   0x46
+#define GORDON_REG_TGSPB_VED1   0x47
+#define GORDON_REG_TGSPB_CH1    0x48
+#define GORDON_REG_TGCK_ST1     0x49
+#define GORDON_REG_TGCK_ED1     0x4A
+#define GORDON_REG_TPCTL_ST1    0x4B
+#define GORDON_REG_TPCTL_ED1    0x4C
+#define GORDON_REG_TPCHG_ED1    0x4D
+#define GORDON_REG_TCOM_CH1     0x4E
+#define GORDON_REG_THBP1        0x4F
+#define GORDON_REG_TPHCTL1      0x50
+#define GORDON_REG_EVPH1        0x51
+#define GORDON_REG_EVPL1        0x52
+#define GORDON_REG_EVNH1        0x53
+#define GORDON_REG_EVNL1        0x54
+#define GORDON_REG_TBIAS1       0x55
+#define GORDON_REG_TPARAM2      0x56
+#define GORDON_REG_TLCDIF2      0x57
+#define GORDON_REG_TSSPB_ST2    0x58
+#define GORDON_REG_TSSPB_ED2    0x59
+#define GORDON_REG_TSCK_ST2     0x5A
+#define GORDON_REG_TSCK_WD2     0x5B
+#define GORDON_REG_TGSPB_VST2   0x5C
+#define GORDON_REG_TGSPB_VED2   0x5D
+#define GORDON_REG_TGSPB_CH2    0x5E
+#define GORDON_REG_TGCK_ST2     0x5F
+#define GORDON_REG_TGCK_ED2     0x60
+#define GORDON_REG_TPCTL_ST2    0x61
+#define GORDON_REG_TPCTL_ED2    0x62
+#define GORDON_REG_TPCHG_ED2    0x63
+#define GORDON_REG_TCOM_CH2     0x64
+#define GORDON_REG_THBP2        0x65
+#define GORDON_REG_TPHCTL2      0x66
+#define GORDON_REG_EVPH2        0x67
+#define GORDON_REG_EVPL2        0x68
+#define GORDON_REG_EVNH2        0x69
+#define GORDON_REG_EVNL2        0x6A
+#define GORDON_REG_TBIAS2       0x6B
+#define GORDON_REG_POWCTL       0x80
+#define GORDON_REG_POWOSC1      0x81
+#define GORDON_REG_POWOSC2      0x82
+#define GORDON_REG_POWSET       0x83
+#define GORDON_REG_POWTRM1      0x85
+#define GORDON_REG_POWTRM2      0x86
+#define GORDON_REG_POWTRM3      0x87
+#define GORDON_REG_POWTRMSEL    0x88
+#define GORDON_REG_POWHIZ       0x89
+
+void serigo(uint16 reg, uint8 data)
+{
+        uint32 mddi_val = 0;
+        mddi_queue_register_read(SSIINTS, &mddi_val, TRUE, 0);
+        if (mddi_val & (1 << 8))
+                mddi_wait(1);
+        /* No De-assert of CS and send 2 bytes */
+        mddi_val = 0x90000 | ((0x00FF & reg) << 8) | data;
+        mddi_queue_register_write(SSITX, mddi_val, TRUE, 0);
+}
+
+void gordon_init(void)
+{
+       /* Image interface settings ***/
+        serigo(GORDON_REG_IMGCTL2, 0x00);
+        serigo(GORDON_REG_IMGSET1, 0x01);
+
+        /* Exchange the RGB signal for J510(Softbank mobile) */
+        serigo(GORDON_REG_IMGSET2, 0x12);
+        serigo(GORDON_REG_LCDIFSET1, 0x00);
+        mddi_wait(2);
+
+        /* Pre-charge settings */
+        serigo(GORDON_REG_PCCTL, 0x09);
+        serigo(GORDON_REG_LCDIFCTL2, 0x1B);
+        mddi_wait(1);
+}
+
+void gordon_disp_on(void)
+{
+        /*gordon_dispmode setting */
+        /*VGA settings */
+        serigo(GORDON_REG_TPARAM1, 0x30);
+        serigo(GORDON_REG_TLCDIF1, 0x00);
+        serigo(GORDON_REG_TSSPB_ST1, 0x8B);
+        serigo(GORDON_REG_TSSPB_ED1, 0x93);
+        mddi_wait(2);
+        serigo(GORDON_REG_TSCK_ST1, 0x88);
+        serigo(GORDON_REG_TSCK_WD1, 0x00);
+        serigo(GORDON_REG_TGSPB_VST1, 0x01);
+        serigo(GORDON_REG_TGSPB_VED1, 0x02);
+        mddi_wait(2);
+        serigo(GORDON_REG_TGSPB_CH1, 0x5E);
+        serigo(GORDON_REG_TGCK_ST1, 0x80);
+        serigo(GORDON_REG_TGCK_ED1, 0x3C);
+        serigo(GORDON_REG_TPCTL_ST1, 0x50);
+        mddi_wait(2);
+        serigo(GORDON_REG_TPCTL_ED1, 0x74);
+        serigo(GORDON_REG_TPCHG_ED1, 0x78);
+        serigo(GORDON_REG_TCOM_CH1, 0x50);
+        serigo(GORDON_REG_THBP1, 0x84);
+        mddi_wait(2);
+        serigo(GORDON_REG_TPHCTL1, 0x00);
+        serigo(GORDON_REG_EVPH1, 0x70);
+        serigo(GORDON_REG_EVPL1, 0x64);
+        serigo(GORDON_REG_EVNH1, 0x56);
+        mddi_wait(2);
+        serigo(GORDON_REG_EVNL1, 0x48);
+        serigo(GORDON_REG_TBIAS1, 0x88);
+        mddi_wait(2);
+        serigo(GORDON_REG_TPARAM2, 0x28);
+        serigo(GORDON_REG_TLCDIF2, 0x14);
+        serigo(GORDON_REG_TSSPB_ST2, 0x49);
+        serigo(GORDON_REG_TSSPB_ED2, 0x4B);
+        mddi_wait(2);
+        serigo(GORDON_REG_TSCK_ST2, 0x4A);
+        serigo(GORDON_REG_TSCK_WD2, 0x02);
+        serigo(GORDON_REG_TGSPB_VST2, 0x02);
+        serigo(GORDON_REG_TGSPB_VED2, 0x03);
+        mddi_wait(2);
+        serigo(GORDON_REG_TGSPB_CH2, 0x2F);
+        serigo(GORDON_REG_TGCK_ST2, 0x40);
+        serigo(GORDON_REG_TGCK_ED2, 0x1E);
+        serigo(GORDON_REG_TPCTL_ST2, 0x2C);
+        mddi_wait(2);
+        serigo(GORDON_REG_TPCTL_ED2, 0x3A);
+        serigo(GORDON_REG_TPCHG_ED2, 0x3C);
+        serigo(GORDON_REG_TCOM_CH2, 0x28);
+        serigo(GORDON_REG_THBP2, 0x4D);
+        mddi_wait(2);
+        serigo(GORDON_REG_TPHCTL2, 0x1A);
+        mddi_wait(2);
+        serigo(GORDON_REG_IVBP1, 0x02);
+        serigo(GORDON_REG_IHBP1, 0x90);
+        serigo(GORDON_REG_IVNUM1, 0xA0);
+        serigo(GORDON_REG_IHNUM1, 0x78);
+        mddi_wait(2);
+        serigo(GORDON_REG_IVBP2, 0x02);
+        serigo(GORDON_REG_IHBP2, 0x48);
+        serigo(GORDON_REG_IVNUM2, 0x50);
+        serigo(GORDON_REG_IHNUM2, 0x3C);
+        mddi_wait(2);
+        serigo(GORDON_REG_POWCTL, 0x03);
+        mddi_wait(15);
+        serigo(GORDON_REG_POWCTL, 0x07);
+        mddi_wait(15);
+        serigo(GORDON_REG_POWCTL, 0x0F);
+        mddi_wait(15);
+        serigo(GORDON_REG_AVCTL, 0x03);
+        mddi_wait(15);
+        serigo(GORDON_REG_POWCTL, 0x1F);
+        mddi_wait(15);
+        serigo(GORDON_REG_POWCTL, 0x5F);
+        mddi_wait(15);
+        serigo(GORDON_REG_POWCTL, 0x7F);
+        mddi_wait(15);
+        serigo(GORDON_REG_LCDIFCTL1, 0x02);
+        mddi_wait(15);
+        serigo(GORDON_REG_IMGCTL1, 0x00);
+        mddi_wait(15);
+        serigo(GORDON_REG_LCDIFCTL3, 0x00);
+        mddi_wait(15);
+        serigo(GORDON_REG_VALTRAN, 0x01);
+        mddi_wait(15);
+        serigo(GORDON_REG_LCDIFCTL1, 0x03);
+        serigo(GORDON_REG_LCDIFCTL1, 0x03);
+        mddi_wait(1);
+}
+
+void gordon_disp_off(void)
+{
+        serigo(GORDON_REG_LCDIFCTL2, 0x7B);
+        serigo(GORDON_REG_VALTRAN, 0x01);
+        serigo(GORDON_REG_LCDIFCTL1, 0x02);
+        serigo(GORDON_REG_LCDIFCTL3, 0x01);
+        mddi_wait(20);
+        serigo(GORDON_REG_VALTRAN, 0x01);
+        serigo(GORDON_REG_IMGCTL1, 0x01);
+        serigo(GORDON_REG_LCDIFCTL1, 0x00);
+        mddi_wait(20);
+        serigo(GORDON_REG_POWCTL, 0x1F);
+        mddi_wait(40);
+        serigo(GORDON_REG_POWCTL, 0x07);
+        mddi_wait(40);
+        serigo(GORDON_REG_POWCTL, 0x03);
+        mddi_wait(40);
+        serigo(GORDON_REG_POWCTL, 0x00);
+        mddi_wait(40);
+}
+
+void gordon_disp_init(void)
+{
+        gordon_init();
+        mddi_wait(20);
+        gordon_disp_on();
+}
+
+static void toshiba_common_initial_setup(struct msm_fb_data_type *mfd)
+{
+        if (TM_GET_PID(mfd->panel.id) == LCD_TOSHIBA_2P4_WVGA_PT) {
+                write_client_reg(DPSET0    , 0x4bec0066, TRUE);
+                write_client_reg(DPSET1    , 0x00000113, TRUE);
+                write_client_reg(DPSUS     , 0x00000000, TRUE);
+                write_client_reg(DPRUN     , 0x00000001, TRUE);
+                mddi_wait(5);
+                write_client_reg(SYSCKENA  , 0x00000001, TRUE);
+                write_client_reg(CLKENB    , 0x0000a0e9, TRUE);
+
+                write_client_reg(GPIODATA  , 0x03FF0000, TRUE);
+                write_client_reg(GPIODIR   , 0x0000024D, TRUE);
+                write_client_reg(GPIOSEL   , 0x00000173, TRUE);
+                write_client_reg(GPIOPC    , 0x03C300C0, TRUE);
+                write_client_reg(WKREQ     , 0x00000000, TRUE);
+                write_client_reg(GPIOIS    , 0x00000000, TRUE);
+                write_client_reg(GPIOIEV   , 0x00000001, TRUE);
+                write_client_reg(GPIOIC    , 0x000003FF, TRUE);
+                write_client_reg(GPIODATA  , 0x00040004, TRUE);
+
+                write_client_reg(GPIODATA  , 0x00080008, TRUE);
+                write_client_reg(DRAMPWR   , 0x00000001, TRUE);
+                write_client_reg(CLKENB    , 0x0000a0eb, TRUE);
+                write_client_reg(PWMCR     , 0x00000000, TRUE);
+                mddi_wait(1);
+
+                write_client_reg(SSICTL    , 0x00060399, TRUE);
+                write_client_reg(SSITIME   , 0x00000100, TRUE);
+                write_client_reg(CNT_DIS   , 0x00000002, TRUE);
+                write_client_reg(SSICTL    , 0x0006039b, TRUE);
+
+                write_client_reg(SSITX     , 0x00000000, TRUE);
+                mddi_wait(7);
+                write_client_reg(SSITX     , 0x00000000, TRUE);
+                mddi_wait(7);
+                write_client_reg(SSITX     , 0x00000000, TRUE);
+                mddi_wait(7);
+
+                write_client_reg(SSITX     , 0x000800BA, TRUE);
+                write_client_reg(SSITX     , 0x00000111, TRUE);
+                write_client_reg(SSITX     , 0x00080036, TRUE);
+                write_client_reg(SSITX     , 0x00000100, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x0008003A, TRUE);
+                write_client_reg(SSITX     , 0x00000160, TRUE);
+                write_client_reg(SSITX     , 0x000800B1, TRUE);
+                write_client_reg(SSITX     , 0x0000015D, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800B2, TRUE);
+                write_client_reg(SSITX     , 0x00000133, TRUE);
+                write_client_reg(SSITX     , 0x000800B3, TRUE);
+                write_client_reg(SSITX     , 0x00000122, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800B4, TRUE);
+                write_client_reg(SSITX     , 0x00000102, TRUE);
+                write_client_reg(SSITX     , 0x000800B5, TRUE);
+                write_client_reg(SSITX     , 0x0000011E, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800B6, TRUE);
+                write_client_reg(SSITX     , 0x00000127, TRUE);
+                write_client_reg(SSITX     , 0x000800B7, TRUE);
+                write_client_reg(SSITX     , 0x00000103, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800B9, TRUE);
+                write_client_reg(SSITX     , 0x00000124, TRUE);
+                write_client_reg(SSITX     , 0x000800BD, TRUE);
+                write_client_reg(SSITX     , 0x000001A1, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800BB, TRUE);
+                write_client_reg(SSITX     , 0x00000100, TRUE);
+                write_client_reg(SSITX     , 0x000800BF, TRUE);
+                write_client_reg(SSITX     , 0x00000101, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800BE, TRUE);
+                write_client_reg(SSITX     , 0x00000100, TRUE);
+                write_client_reg(SSITX     , 0x000800C0, TRUE);
+                write_client_reg(SSITX     , 0x00000111, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800C1, TRUE);
+                write_client_reg(SSITX     , 0x00000111, TRUE);
+                write_client_reg(SSITX     , 0x000800C2, TRUE);
+                write_client_reg(SSITX     , 0x00000111, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800C3, TRUE);
+                write_client_reg(SSITX     , 0x00080132, TRUE);
+                write_client_reg(SSITX     , 0x00000132, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800C4, TRUE);
+                write_client_reg(SSITX     , 0x00080132, TRUE);
+                write_client_reg(SSITX     , 0x00000132, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800C5, TRUE);
+                write_client_reg(SSITX     , 0x00080132, TRUE);
+                write_client_reg(SSITX     , 0x00000132, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800C6, TRUE);
+                write_client_reg(SSITX     , 0x00080132, TRUE);
+                write_client_reg(SSITX     , 0x00000132, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800C7, TRUE);
+                write_client_reg(SSITX     , 0x00080164, TRUE);
+                write_client_reg(SSITX     , 0x00000145, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800C8, TRUE);
+                write_client_reg(SSITX     , 0x00000144, TRUE);
+                write_client_reg(SSITX     , 0x000800C9, TRUE);
+                write_client_reg(SSITX     , 0x00000152, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800CA, TRUE);
+                write_client_reg(SSITX     , 0x00000100, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800EC, TRUE);
+                write_client_reg(SSITX     , 0x00080101, TRUE);
+                write_client_reg(SSITX     , 0x000001FC, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800CF, TRUE);
+                write_client_reg(SSITX     , 0x00000101, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800D0, TRUE);
+                write_client_reg(SSITX     , 0x00080110, TRUE);
+                write_client_reg(SSITX     , 0x00000104, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800D1, TRUE);
+                write_client_reg(SSITX     , 0x00000101, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800D2, TRUE);
+                write_client_reg(SSITX     , 0x00080100, TRUE);
+                write_client_reg(SSITX     , 0x00000128, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800D3, TRUE);
+                write_client_reg(SSITX     , 0x00080100, TRUE);
+                write_client_reg(SSITX     , 0x00000128, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800D4, TRUE);
+                write_client_reg(SSITX     , 0x00080126, TRUE);
+                write_client_reg(SSITX     , 0x000001A4, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800D5, TRUE);
+                write_client_reg(SSITX     , 0x00000120, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800EF, TRUE);
+                write_client_reg(SSITX     , 0x00080132, TRUE);
+                write_client_reg(SSITX     , 0x00000100, TRUE);
+                mddi_wait(1);
+
+                write_client_reg(BITMAP0   , 0x032001E0, TRUE);
+                write_client_reg(BITMAP1   , 0x032001E0, TRUE);
+                write_client_reg(BITMAP2   , 0x014000F0, TRUE);
+                write_client_reg(BITMAP3   , 0x014000F0, TRUE);
+                write_client_reg(BITMAP4   , 0x014000F0, TRUE);
+                write_client_reg(CLKENB    , 0x0000A1EB, TRUE);
+                write_client_reg(PORT_ENB  , 0x00000001, TRUE);
+                write_client_reg(PORT      , 0x00000004, TRUE);
+                write_client_reg(PXL       , 0x00000002, TRUE);
+                write_client_reg(MPLFBUF   , 0x00000000, TRUE);
+                write_client_reg(HCYCLE    , 0x000000FD, TRUE);
+                write_client_reg(HSW       , 0x00000003, TRUE);
+                write_client_reg(HDE_START , 0x00000007, TRUE);
+                write_client_reg(HDE_SIZE  , 0x000000EF, TRUE);
+                write_client_reg(VCYCLE    , 0x00000325, TRUE);
+                write_client_reg(VSW       , 0x00000001, TRUE);
+                write_client_reg(VDE_START , 0x00000003, TRUE);
+                write_client_reg(VDE_SIZE  , 0x0000031F, TRUE);
+                write_client_reg(START     , 0x00000001, TRUE);
+                mddi_wait(32);
+                write_client_reg(SSITX     , 0x000800BC, TRUE);
+                write_client_reg(SSITX     , 0x00000180, TRUE);
+                write_client_reg(SSITX     , 0x0008003B, TRUE);
+                write_client_reg(SSITX     , 0x00000100, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800B0, TRUE);
+                write_client_reg(SSITX     , 0x00000116, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x000800B8, TRUE);
+                write_client_reg(SSITX     , 0x000801FF, TRUE);
+                write_client_reg(SSITX     , 0x000001F5, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX     , 0x00000011, TRUE);
+                mddi_wait(5);
+                write_client_reg(SSITX     , 0x00000029, TRUE);
+                return;
+        }
+
+        if (TM_GET_PID(mfd->panel.id) == LCD_SHARP_2P4_VGA) {
+                write_client_reg(DPSET0, 0x4BEC0066, TRUE);
+                write_client_reg(DPSET1, 0x00000113, TRUE);
+                write_client_reg(DPSUS, 0x00000000, TRUE);
+                write_client_reg(DPRUN, 0x00000001, TRUE);
+                mddi_wait(14);
+                write_client_reg(SYSCKENA, 0x00000001, TRUE);
+                write_client_reg(CLKENB, 0x000000EF, TRUE);
+                write_client_reg(GPIO_BLOCK_BASE, 0x03FF0000, TRUE);
+                write_client_reg(GPIODIR, 0x0000024D, TRUE);
+                write_client_reg(SYSTEM_BLOCK2_BASE, 0x00000173, TRUE);
+                write_client_reg(GPIOPC, 0x03C300C0, TRUE);
+                write_client_reg(SYSTEM_BLOCK1_BASE, 0x00000000, TRUE);
+                write_client_reg(GPIOIS, 0x00000000, TRUE);
+                write_client_reg(GPIOIEV, 0x00000001, TRUE);
+                write_client_reg(GPIOIC, 0x000003FF, TRUE);
+                write_client_reg(GPIO_BLOCK_BASE, 0x00060006, TRUE);
+                write_client_reg(GPIO_BLOCK_BASE, 0x00080008, TRUE);
+                write_client_reg(GPIO_BLOCK_BASE, 0x02000200, TRUE);
+                write_client_reg(DRAMPWR, 0x00000001, TRUE);
+                write_client_reg(TIMER0CTRL, 0x00000060, TRUE);
+                write_client_reg(PWM_BLOCK_BASE, 0x00001388, TRUE);
+                write_client_reg(PWM0OFF, 0x00001387, TRUE);
+                write_client_reg(TIMER1CTRL, 0x00000060, TRUE);
+                write_client_reg(TIMER1LOAD, 0x00001388, TRUE);
+                write_client_reg(PWM1OFF, 0x00001387, TRUE);
+                write_client_reg(TIMER0CTRL, 0x000000E0, TRUE);
+                write_client_reg(TIMER1CTRL, 0x000000E0, TRUE);
+                write_client_reg(PWMCR, 0x00000003, TRUE);
+                mddi_wait(1);
+                write_client_reg(SPI_BLOCK_BASE, 0x00063111, TRUE);
+                write_client_reg(SSITIME, 0x00000100, TRUE);
+                write_client_reg(SPI_BLOCK_BASE, 0x00063113, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX, 0x00000000, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX, 0x00000000, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX, 0x00000000, TRUE);
+                mddi_wait(1);
+                write_client_reg(CLKENB, 0x0000A1EF, TRUE);
+                write_client_reg(START, 0x00000000, TRUE);
+                write_client_reg(WRSTB, 0x0000003F, TRUE);
+                write_client_reg(RDSTB, 0x00000432, TRUE);
+                write_client_reg(PORT_ENB, 0x00000002, TRUE);
+                write_client_reg(VSYNIF, 0x00000000, TRUE);
+                write_client_reg(ASY_DATA, 0x80000000, TRUE);
+                write_client_reg(ASY_DATB, 0x00000001, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+                mddi_wait(10);
+                write_client_reg(ASY_DATA, 0x80000000, TRUE);
+                write_client_reg(ASY_DATB, 0x80000000, TRUE);
+                write_client_reg(ASY_DATC, 0x80000000, TRUE);
+                write_client_reg(ASY_DATD, 0x80000000, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000009, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000008, TRUE);
+                write_client_reg(ASY_DATA, 0x80000007, TRUE);
+                write_client_reg(ASY_DATB, 0x00004005, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+                mddi_wait(20);
+                write_client_reg(ASY_DATA, 0x80000059, TRUE);
+                write_client_reg(ASY_DATB, 0x00000000, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+
+                write_client_reg(VSYNIF, 0x00000001, TRUE);
+                write_client_reg(PORT_ENB, 0x00000001, TRUE);
+        } else {
+                write_client_reg(DPSET0, 0x4BEC0066, TRUE);
+                write_client_reg(DPSET1, 0x00000113, TRUE);
+                write_client_reg(DPSUS, 0x00000000, TRUE);
+                write_client_reg(DPRUN, 0x00000001, TRUE);
+                mddi_wait(14);
+                write_client_reg(SYSCKENA, 0x00000001, TRUE);
+                write_client_reg(CLKENB, 0x000000EF, TRUE);
+                write_client_reg(GPIODATA, 0x03FF0000, TRUE);
+                write_client_reg(GPIODIR, 0x0000024D, TRUE);
+                write_client_reg(GPIOSEL, 0x00000173, TRUE);
+                write_client_reg(GPIOPC, 0x03C300C0, TRUE);
+                write_client_reg(WKREQ, 0x00000000, TRUE);
+                write_client_reg(GPIOIS, 0x00000000, TRUE);
+                write_client_reg(GPIOIEV, 0x00000001, TRUE);
+                write_client_reg(GPIOIC, 0x000003FF, TRUE);
+                write_client_reg(GPIODATA, 0x00060006, TRUE);
+                write_client_reg(GPIODATA, 0x00080008, TRUE);
+                write_client_reg(GPIODATA, 0x02000200, TRUE);
+
+                if (TM_GET_PID(mfd->panel.id) == LCD_TOSHIBA_2P4_WVGA) {
+                        mddi_wait(400);
+                        write_client_reg(DRAMPWR, 0x00000001, TRUE);
+
+                        write_client_reg(CNT_DIS, 0x00000002, TRUE);
+                        write_client_reg(BITMAP0, 0x01E00320, TRUE);
+                        write_client_reg(PORT_ENB, 0x00000001, TRUE);
+                        write_client_reg(PORT, 0x00000004, TRUE);
+                        write_client_reg(PXL, 0x0000003A, TRUE);
+                        write_client_reg(MPLFBUF, 0x00000000, TRUE);
+                        write_client_reg(HCYCLE, 0x00000253, TRUE);
+                        write_client_reg(HSW, 0x00000003, TRUE);
+                        write_client_reg(HDE_START, 0x00000017, TRUE);
+                        write_client_reg(HDE_SIZE, 0x0000018F, TRUE);
+                        write_client_reg(VCYCLE, 0x000001FF, TRUE);
+                        write_client_reg(VSW, 0x00000001, TRUE);
+                        write_client_reg(VDE_START, 0x00000003, TRUE);
+                        write_client_reg(VDE_SIZE, 0x000001DF, TRUE);
+                        write_client_reg(START, 0x00000001, TRUE);
+                        mddi_wait(1);
+                        write_client_reg(TIMER0CTRL, 0x00000060, TRUE);
+                        write_client_reg(TIMER0LOAD, 0x00001388, TRUE);
+                        write_client_reg(TIMER1CTRL, 0x00000060, TRUE);
+                        write_client_reg(TIMER1LOAD, 0x00001388, TRUE);
+                        write_client_reg(PWM1OFF, 0x00000087, TRUE);
+                } else {
+                        write_client_reg(DRAMPWR, 0x00000001, TRUE);
+                        write_client_reg(TIMER0CTRL, 0x00000060, TRUE);
+                        write_client_reg(TIMER0LOAD, 0x00001388, TRUE);
+                        write_client_reg(TIMER1CTRL, 0x00000060, TRUE);
+                        write_client_reg(TIMER1LOAD, 0x00001388, TRUE);
+                        write_client_reg(PWM1OFF, 0x00001387, TRUE);
+                }
+
+                write_client_reg(TIMER0CTRL, 0x000000E0, TRUE);
+                write_client_reg(TIMER1CTRL, 0x000000E0, TRUE);
+                write_client_reg(PWMCR, 0x00000003, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSICTL, 0x00000799, TRUE);
+                write_client_reg(SSITIME, 0x00000100, TRUE);
+                write_client_reg(SSICTL, 0x0000079b, TRUE);
+                write_client_reg(SSITX, 0x00000000, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX, 0x00000000, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX, 0x00000000, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX, 0x000800BA, TRUE);
+                write_client_reg(SSITX, 0x00000111, TRUE);
+                write_client_reg(SSITX, 0x00080036, TRUE);
+                write_client_reg(SSITX, 0x00000100, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800BB, TRUE);
+                write_client_reg(SSITX, 0x00000100, TRUE);
+                write_client_reg(SSITX, 0x0008003A, TRUE);
+                write_client_reg(SSITX, 0x00000160, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800BF, TRUE);
+                write_client_reg(SSITX, 0x00000100, TRUE);
+                write_client_reg(SSITX, 0x000800B1, TRUE);
+                write_client_reg(SSITX, 0x0000015D, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800B2, TRUE);
+                write_client_reg(SSITX, 0x00000133, TRUE);
+                write_client_reg(SSITX, 0x000800B3, TRUE);
+                write_client_reg(SSITX, 0x00000122, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800B4, TRUE);
+                write_client_reg(SSITX, 0x00000102, TRUE);
+                write_client_reg(SSITX, 0x000800B5, TRUE);
+                write_client_reg(SSITX, 0x0000011F, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800B6, TRUE);
+                write_client_reg(SSITX, 0x00000128, TRUE);
+                write_client_reg(SSITX, 0x000800B7, TRUE);
+                write_client_reg(SSITX, 0x00000103, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800B9, TRUE);
+                write_client_reg(SSITX, 0x00000120, TRUE);
+                write_client_reg(SSITX, 0x000800BD, TRUE);
+                write_client_reg(SSITX, 0x00000102, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800BE, TRUE);
+                write_client_reg(SSITX, 0x00000100, TRUE);
+                write_client_reg(SSITX, 0x000800C0, TRUE);
+                write_client_reg(SSITX, 0x00000111, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800C1, TRUE);
+                write_client_reg(SSITX, 0x00000111, TRUE);
+                write_client_reg(SSITX, 0x000800C2, TRUE);
+                write_client_reg(SSITX, 0x00000111, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800C3, TRUE);
+                write_client_reg(SSITX, 0x0008010A, TRUE);
+                write_client_reg(SSITX, 0x0000010A, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800C4, TRUE);
+                write_client_reg(SSITX, 0x00080160, TRUE);
+                write_client_reg(SSITX, 0x00000160, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800C5, TRUE);
+                write_client_reg(SSITX, 0x00080160, TRUE);
+                write_client_reg(SSITX, 0x00000160, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800C6, TRUE);
+                write_client_reg(SSITX, 0x00080160, TRUE);
+                write_client_reg(SSITX, 0x00000160, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800C7, TRUE);
+                write_client_reg(SSITX, 0x00080133, TRUE);
+                write_client_reg(SSITX, 0x00000143, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800C8, TRUE);
+                write_client_reg(SSITX, 0x00000144, TRUE);
+                write_client_reg(SSITX, 0x000800C9, TRUE);
+                write_client_reg(SSITX, 0x00000133, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800CA, TRUE);
+                write_client_reg(SSITX, 0x00000100, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800EC, TRUE);
+                write_client_reg(SSITX, 0x00080102, TRUE);
+                write_client_reg(SSITX, 0x00000118, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800CF, TRUE);
+                write_client_reg(SSITX, 0x00000101, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800D0, TRUE);
+                write_client_reg(SSITX, 0x00080110, TRUE);
+                write_client_reg(SSITX, 0x00000104, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800D1, TRUE);
+                write_client_reg(SSITX, 0x00000101, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800D2, TRUE);
+                write_client_reg(SSITX, 0x00080100, TRUE);
+                write_client_reg(SSITX, 0x0000013A, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800D3, TRUE);
+                write_client_reg(SSITX, 0x00080100, TRUE);
+                write_client_reg(SSITX, 0x0000013A, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800D4, TRUE);
+                write_client_reg(SSITX, 0x00080124, TRUE);
+                write_client_reg(SSITX, 0x0000016E, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX, 0x000800D5, TRUE);
+                write_client_reg(SSITX, 0x00000124, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800ED, TRUE);
+                write_client_reg(SSITX, 0x00080101, TRUE);
+                write_client_reg(SSITX, 0x0000010A, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800D6, TRUE);
+                write_client_reg(SSITX, 0x00000101, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800D7, TRUE);
+                write_client_reg(SSITX, 0x00080110, TRUE);
+                write_client_reg(SSITX, 0x0000010A, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800D8, TRUE);
+                write_client_reg(SSITX, 0x00000101, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800D9, TRUE);
+                write_client_reg(SSITX, 0x00080100, TRUE);
+                write_client_reg(SSITX, 0x00000114, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800DE, TRUE);
+                write_client_reg(SSITX, 0x00080100, TRUE);
+                write_client_reg(SSITX, 0x00000114, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800DF, TRUE);
+                write_client_reg(SSITX, 0x00080112, TRUE);
+                write_client_reg(SSITX, 0x0000013F, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800E0, TRUE);
+                write_client_reg(SSITX, 0x0000010B, TRUE);
+                write_client_reg(SSITX, 0x000800E2, TRUE);
+                write_client_reg(SSITX, 0x00000101, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800E3, TRUE);
+                write_client_reg(SSITX, 0x00000136, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800E4, TRUE);
+                write_client_reg(SSITX, 0x00080100, TRUE);
+                write_client_reg(SSITX, 0x00000103, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800E5, TRUE);
+                write_client_reg(SSITX, 0x00080102, TRUE);
+                write_client_reg(SSITX, 0x00000104, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800E6, TRUE);
+                write_client_reg(SSITX, 0x00000103, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800E7, TRUE);
+                write_client_reg(SSITX, 0x00080104, TRUE);
+                write_client_reg(SSITX, 0x0000010A, TRUE);
+                mddi_wait(2);
+                write_client_reg(SSITX, 0x000800E8, TRUE);
+                write_client_reg(SSITX, 0x00000104, TRUE);
+                write_client_reg(CLKENB, 0x000001EF, TRUE);
+                write_client_reg(START, 0x00000000, TRUE);
+                write_client_reg(WRSTB, 0x0000003F, TRUE);
+                write_client_reg(RDSTB, 0x00000432, TRUE);
+                write_client_reg(PORT_ENB, 0x00000002, TRUE);
+                write_client_reg(VSYNIF, 0x00000000, TRUE);
+                write_client_reg(ASY_DATA, 0x80000000, TRUE);
+                write_client_reg(ASY_DATB, 0x00000001, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+                mddi_wait(10);
+                write_client_reg(ASY_DATA, 0x80000000, TRUE);
+                write_client_reg(ASY_DATB, 0x80000000, TRUE);
+                write_client_reg(ASY_DATC, 0x80000000, TRUE);
+                write_client_reg(ASY_DATD, 0x80000000, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000009, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000008, TRUE);
+                write_client_reg(ASY_DATA, 0x80000007, TRUE);
+                write_client_reg(ASY_DATB, 0x00004005, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+                mddi_wait(20);
+                write_client_reg(ASY_DATA, 0x80000059, TRUE);
+                write_client_reg(ASY_DATB, 0x00000000, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+                write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+                write_client_reg(VSYNIF, 0x00000001, TRUE);
+                write_client_reg(PORT_ENB, 0x00000001, TRUE);
+        }
+
+        mddi_toshiba_state_transition(TOSHIBA_STATE_PRIM_SEC_STANDBY,
+                                      TOSHIBA_STATE_PRIM_SEC_READY);
+}
+
+static void toshiba_prim_start(struct msm_fb_data_type *mfd)
+{
+        if (TM_GET_PID(mfd->panel.id) == LCD_TOSHIBA_2P4_WVGA_PT)
+                return;
+
+        if (TM_GET_PID(mfd->panel.id) == LCD_SHARP_2P4_VGA) {
+                write_client_reg(BITMAP1, 0x01E000F0, TRUE);
+                write_client_reg(BITMAP2, 0x01E000F0, TRUE);
+                write_client_reg(BITMAP3, 0x01E000F0, TRUE);
+                write_client_reg(BITMAP4, 0x00DC00B0, TRUE);
+                write_client_reg(CLKENB, 0x000001EF, TRUE);
+                write_client_reg(PORT_ENB, 0x00000001, TRUE);
+                write_client_reg(PORT, 0x00000016, TRUE);
+                write_client_reg(PXL, 0x00000002, TRUE);
+                write_client_reg(MPLFBUF, 0x00000000, TRUE);
+                write_client_reg(HCYCLE, 0x00000185, TRUE);
+                write_client_reg(HSW, 0x00000018, TRUE);
+                write_client_reg(HDE_START, 0x0000004A, TRUE);
+                write_client_reg(HDE_SIZE, 0x000000EF, TRUE);
+                write_client_reg(VCYCLE, 0x0000028E, TRUE);
+                write_client_reg(VSW, 0x00000004, TRUE);
+                write_client_reg(VDE_START, 0x00000009, TRUE);
+                write_client_reg(VDE_SIZE, 0x0000027F, TRUE);
+                write_client_reg(START, 0x00000001, TRUE);
+                write_client_reg(SYSTEM_BLOCK1_BASE, 0x00000002, TRUE);
+        } else{
+
+                write_client_reg(VSYNIF, 0x00000001, TRUE);
+                write_client_reg(PORT_ENB, 0x00000001, TRUE);
+                write_client_reg(BITMAP1, 0x01E000F0, TRUE);
+                write_client_reg(BITMAP2, 0x01E000F0, TRUE);
+                write_client_reg(BITMAP3, 0x01E000F0, TRUE);
+                write_client_reg(BITMAP4, 0x00DC00B0, TRUE);
+                write_client_reg(CLKENB, 0x000001EF, TRUE);
+                write_client_reg(PORT_ENB, 0x00000001, TRUE);
+                write_client_reg(PORT, 0x00000004, TRUE);
+                write_client_reg(PXL, 0x00000002, TRUE);
+                write_client_reg(MPLFBUF, 0x00000000, TRUE);
+
+                if (mddi_toshiba_61Hz_refresh) {
+                        write_client_reg(HCYCLE, 0x000000FC, TRUE);
+                        mddi_toshiba_rows_per_second = 39526;
+                        mddi_toshiba_rows_per_refresh = 646;
+                        mddi_toshiba_usecs_per_refresh = 16344;
+                } else {
+                        write_client_reg(HCYCLE, 0x0000010b, TRUE);
+                        mddi_toshiba_rows_per_second = 37313;
+                        mddi_toshiba_rows_per_refresh = 646;
+                        mddi_toshiba_usecs_per_refresh = 17313;
+                }
+
+                write_client_reg(HSW, 0x00000003, TRUE);
+                write_client_reg(HDE_START, 0x00000007, TRUE);
+                write_client_reg(HDE_SIZE, 0x000000EF, TRUE);
+                write_client_reg(VCYCLE, 0x00000285, TRUE);
+                write_client_reg(VSW, 0x00000001, TRUE);
+                write_client_reg(VDE_START, 0x00000003, TRUE);
+                write_client_reg(VDE_SIZE, 0x0000027F, TRUE);
+                write_client_reg(START, 0x00000001, TRUE);
+                mddi_wait(10);
+                write_client_reg(SSITX, 0x000800BC, TRUE);
+                write_client_reg(SSITX, 0x00000180, TRUE);
+                write_client_reg(SSITX, 0x0008003B, TRUE);
+                write_client_reg(SSITX, 0x00000100, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX, 0x000800B0, TRUE);
+                write_client_reg(SSITX, 0x00000116, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX, 0x000800B8, TRUE);
+                write_client_reg(SSITX, 0x000801FF, TRUE);
+                write_client_reg(SSITX, 0x000001F5, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX, 0x00000011, TRUE);
+                write_client_reg(SSITX, 0x00000029, TRUE);
+                write_client_reg(WKREQ, 0x00000000, TRUE);
+                write_client_reg(WAKEUP, 0x00000000, TRUE);
+                write_client_reg(INTMSK, 0x00000001, TRUE);
+        }
+
+        mddi_toshiba_state_transition(TOSHIBA_STATE_PRIM_SEC_READY,
+                                      TOSHIBA_STATE_PRIM_NORMAL_MODE);
+}
+
+static void toshiba_sec_start(struct msm_fb_data_type *mfd)
+{
+        if (TM_GET_PID(mfd->panel.id) == LCD_TOSHIBA_2P4_WVGA_PT)
+                return;
+
+        write_client_reg(VSYNIF, 0x00000000, TRUE);
+        write_client_reg(PORT_ENB, 0x00000002, TRUE);
+        write_client_reg(CLKENB, 0x000011EF, TRUE);
+        write_client_reg(BITMAP0, 0x028001E0, TRUE);
+        write_client_reg(BITMAP1, 0x00000000, TRUE);
+        write_client_reg(BITMAP2, 0x00000000, TRUE);
+        write_client_reg(BITMAP3, 0x00000000, TRUE);
+        write_client_reg(BITMAP4, 0x00DC00B0, TRUE);
+        write_client_reg(PORT, 0x00000000, TRUE);
+        write_client_reg(PXL, 0x00000000, TRUE);
+        write_client_reg(MPLFBUF, 0x00000004, TRUE);
+        write_client_reg(HCYCLE, 0x0000006B, TRUE);
+        write_client_reg(HSW, 0x00000003, TRUE);
+        write_client_reg(HDE_START, 0x00000007, TRUE);
+        write_client_reg(HDE_SIZE, 0x00000057, TRUE);
+        write_client_reg(VCYCLE, 0x000000E6, TRUE);
+        write_client_reg(VSW, 0x00000001, TRUE);
+        write_client_reg(VDE_START, 0x00000003, TRUE);
+        write_client_reg(VDE_SIZE, 0x000000DB, TRUE);
+        write_client_reg(ASY_DATA, 0x80000001, TRUE);
+        write_client_reg(ASY_DATB, 0x0000011B, TRUE);
+        write_client_reg(ASY_DATC, 0x80000002, TRUE);
+        write_client_reg(ASY_DATD, 0x00000700, TRUE);
+        write_client_reg(ASY_DATE, 0x80000003, TRUE);
+        write_client_reg(ASY_DATF, 0x00000230, TRUE);
+        write_client_reg(ASY_DATG, 0x80000008, TRUE);
+        write_client_reg(ASY_DATH, 0x00000402, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000001, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000000, TRUE);
+        write_client_reg(ASY_DATA, 0x80000009, TRUE);
+        write_client_reg(ASY_DATB, 0x00000000, TRUE);
+        write_client_reg(ASY_DATC, 0x8000000B, TRUE);
+        write_client_reg(ASY_DATD, 0x00000000, TRUE);
+        write_client_reg(ASY_DATE, 0x8000000C, TRUE);
+        write_client_reg(ASY_DATF, 0x00000000, TRUE);
+        write_client_reg(ASY_DATG, 0x8000000D, TRUE);
+        write_client_reg(ASY_DATH, 0x00000409, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000001, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000000, TRUE);
+        write_client_reg(ASY_DATA, 0x8000000E, TRUE);
+        write_client_reg(ASY_DATB, 0x00000409, TRUE);
+        write_client_reg(ASY_DATC, 0x80000030, TRUE);
+        write_client_reg(ASY_DATD, 0x00000000, TRUE);
+        write_client_reg(ASY_DATE, 0x80000031, TRUE);
+        write_client_reg(ASY_DATF, 0x00000100, TRUE);
+        write_client_reg(ASY_DATG, 0x80000032, TRUE);
+        write_client_reg(ASY_DATH, 0x00000104, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000001, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000000, TRUE);
+        write_client_reg(ASY_DATA, 0x80000033, TRUE);
+        write_client_reg(ASY_DATB, 0x00000400, TRUE);
+        write_client_reg(ASY_DATC, 0x80000034, TRUE);
+        write_client_reg(ASY_DATD, 0x00000306, TRUE);
+        write_client_reg(ASY_DATE, 0x80000035, TRUE);
+        write_client_reg(ASY_DATF, 0x00000706, TRUE);
+        write_client_reg(ASY_DATG, 0x80000036, TRUE);
+        write_client_reg(ASY_DATH, 0x00000707, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000001, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000000, TRUE);
+        write_client_reg(ASY_DATA, 0x80000037, TRUE);
+        write_client_reg(ASY_DATB, 0x00000004, TRUE);
+        write_client_reg(ASY_DATC, 0x80000038, TRUE);
+        write_client_reg(ASY_DATD, 0x00000000, TRUE);
+        write_client_reg(ASY_DATE, 0x80000039, TRUE);
+        write_client_reg(ASY_DATF, 0x00000000, TRUE);
+        write_client_reg(ASY_DATG, 0x8000003A, TRUE);
+        write_client_reg(ASY_DATH, 0x00000001, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000001, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000000, TRUE);
+        write_client_reg(ASY_DATA, 0x80000044, TRUE);
+        write_client_reg(ASY_DATB, 0x0000AF00, TRUE);
+        write_client_reg(ASY_DATC, 0x80000045, TRUE);
+        write_client_reg(ASY_DATD, 0x0000DB00, TRUE);
+        write_client_reg(ASY_DATE, 0x08000042, TRUE);
+        write_client_reg(ASY_DATF, 0x0000DB00, TRUE);
+        write_client_reg(ASY_DATG, 0x80000021, TRUE);
+        write_client_reg(ASY_DATH, 0x00000000, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000001, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000000, TRUE);
+        write_client_reg(PXL, 0x0000000C, TRUE);
+        write_client_reg(VSYNIF, 0x00000001, TRUE);
+        write_client_reg(ASY_DATA, 0x80000022, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000003, TRUE);
+        write_client_reg(START, 0x00000001, TRUE);
+        mddi_wait(60);
+        write_client_reg(PXL, 0x00000000, TRUE);
+        write_client_reg(VSYNIF, 0x00000000, TRUE);
+        write_client_reg(START, 0x00000000, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000000, TRUE);
+        write_client_reg(ASY_DATA, 0x80000050, TRUE);
+        write_client_reg(ASY_DATB, 0x00000000, TRUE);
+        write_client_reg(ASY_DATC, 0x80000051, TRUE);
+        write_client_reg(ASY_DATD, 0x00000E00, TRUE);
+        write_client_reg(ASY_DATE, 0x80000052, TRUE);
+        write_client_reg(ASY_DATF, 0x00000D01, TRUE);
+        write_client_reg(ASY_DATG, 0x80000053, TRUE);
+        write_client_reg(ASY_DATH, 0x00000000, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000001, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000000, TRUE);
+        write_client_reg(ASY_DATA, 0x80000058, TRUE);
+        write_client_reg(ASY_DATB, 0x00000000, TRUE);
+        write_client_reg(ASY_DATC, 0x8000005A, TRUE);
+        write_client_reg(ASY_DATD, 0x00000E01, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000009, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000008, TRUE);
+        write_client_reg(ASY_DATA, 0x80000011, TRUE);
+        write_client_reg(ASY_DATB, 0x00000812, TRUE);
+        write_client_reg(ASY_DATC, 0x80000012, TRUE);
+        write_client_reg(ASY_DATD, 0x00000003, TRUE);
+        write_client_reg(ASY_DATE, 0x80000013, TRUE);
+        write_client_reg(ASY_DATF, 0x00000909, TRUE);
+        write_client_reg(ASY_DATG, 0x80000010, TRUE);
+        write_client_reg(ASY_DATH, 0x00000040, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000001, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000000, TRUE);
+        mddi_wait(40);
+        write_client_reg(ASY_DATA, 0x80000010, TRUE);
+        write_client_reg(ASY_DATB, 0x00000340, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(60);
+        write_client_reg(ASY_DATA, 0x80000010, TRUE);
+        write_client_reg(ASY_DATB, 0x00003340, TRUE);
+        write_client_reg(ASY_DATC, 0x80000007, TRUE);
+        write_client_reg(ASY_DATD, 0x00004007, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000009, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000008, TRUE);
+        mddi_wait(1);
+        write_client_reg(ASY_DATA, 0x80000007, TRUE);
+        write_client_reg(ASY_DATB, 0x00004017, TRUE);
+        write_client_reg(ASY_DATC, 0x8000005B, TRUE);
+        write_client_reg(ASY_DATD, 0x00000000, TRUE);
+        write_client_reg(ASY_DATE, 0x80000059, TRUE);
+        write_client_reg(ASY_DATF, 0x00000011, TRUE);
+        write_client_reg(ASY_CMDSET, 0x0000000D, TRUE);
+        write_client_reg(ASY_CMDSET, 0x0000000C, TRUE);
+        mddi_wait(20);
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        /* LTPS I/F control */
+        write_client_reg(ASY_DATB, 0x00000019, TRUE);
+        /* Direct cmd transfer enable */
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        /* Direct cmd transfer disable */
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(20);
+        /* Index setting of SUB LCDD */
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        /* LTPS I/F control */
+        write_client_reg(ASY_DATB, 0x00000079, TRUE);
+        /* Direct cmd transfer enable */
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        /* Direct cmd transfer disable */
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(20);
+        /* Index setting of SUB LCDD */
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        /* LTPS I/F control */
+        write_client_reg(ASY_DATB, 0x000003FD, TRUE);
+        /* Direct cmd transfer enable */
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        /* Direct cmd transfer disable */
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(20);
+        mddi_toshiba_state_transition(TOSHIBA_STATE_PRIM_SEC_READY,
+                                      TOSHIBA_STATE_SEC_NORMAL_MODE);
+}
+
+static void toshiba_prim_lcd_off(struct msm_fb_data_type *mfd)
+{
+        if (TM_GET_PID(mfd->panel.id) == LCD_SHARP_2P4_VGA) {
+                gordon_disp_off();
+        } else{
+
+                /* Main panel power off (Deep standby in) */
+                write_client_reg(SSITX, 0x000800BC, TRUE);
+                write_client_reg(SSITX, 0x00000100, TRUE);
+                write_client_reg(SSITX, 0x00000028, TRUE);
+                mddi_wait(1);
+                write_client_reg(SSITX, 0x000800B8, TRUE);
+                write_client_reg(SSITX, 0x00000180, TRUE);
+                write_client_reg(SSITX, 0x00000102, TRUE);
+                write_client_reg(SSITX, 0x00000010, TRUE);
+        }
+        write_client_reg(PORT, 0x00000003, TRUE);
+        write_client_reg(REGENB, 0x00000001, TRUE);
+        mddi_wait(1);
+        write_client_reg(PXL, 0x00000000, TRUE);
+        write_client_reg(START, 0x00000000, TRUE);
+        write_client_reg(REGENB, 0x00000001, TRUE);
+        mddi_wait(3);
+        if (TM_GET_PID(mfd->panel.id) != LCD_SHARP_2P4_VGA) {
+                write_client_reg(SSITX, 0x000800B0, TRUE);
+                write_client_reg(SSITX, 0x00000100, TRUE);
+        }
+        mddi_toshiba_state_transition(TOSHIBA_STATE_PRIM_NORMAL_MODE,
+                                      TOSHIBA_STATE_PRIM_SEC_STANDBY);
+}
+
+static void toshiba_sec_lcd_off(struct msm_fb_data_type *mfd)
+{
+        if (TM_GET_PID(mfd->panel.id) == LCD_TOSHIBA_2P4_WVGA_PT)
+                return;
+
+        write_client_reg(VSYNIF, 0x00000000, TRUE);
+        write_client_reg(PORT_ENB, 0x00000002, TRUE);
+        write_client_reg(ASY_DATA, 0x80000007, TRUE);
+        write_client_reg(ASY_DATB, 0x00004016, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        write_client_reg(ASY_DATB, 0x00000019, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        write_client_reg(ASY_DATB, 0x0000000B, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        write_client_reg(ASY_DATB, 0x00000002, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(4);
+        write_client_reg(ASY_DATA, 0x80000010, TRUE);
+        write_client_reg(ASY_DATB, 0x00000300, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(4);
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        write_client_reg(ASY_DATB, 0x00000000, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+        write_client_reg(ASY_DATA, 0x80000007, TRUE);
+        write_client_reg(ASY_DATB, 0x00004004, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+        write_client_reg(PORT, 0x00000000, TRUE);
+        write_client_reg(PXL, 0x00000000, TRUE);
+        write_client_reg(START, 0x00000000, TRUE);
+        write_client_reg(VSYNIF, 0x00000001, TRUE);
+        write_client_reg(PORT_ENB, 0x00000001, TRUE);
+        write_client_reg(REGENB, 0x00000001, TRUE);
+        mddi_toshiba_state_transition(TOSHIBA_STATE_SEC_NORMAL_MODE,
+                                      TOSHIBA_STATE_PRIM_SEC_STANDBY);
+}
+
+static void toshiba_sec_cont_update_start(struct msm_fb_data_type *mfd)
+{
+
+        if (TM_GET_PID(mfd->panel.id) == LCD_TOSHIBA_2P4_WVGA_PT)
+                return;
+
+        write_client_reg(VSYNIF, 0x00000000, TRUE);
+        write_client_reg(PORT_ENB, 0x00000002, TRUE);
+        write_client_reg(INTMASK, 0x00000001, TRUE);
+        write_client_reg(TTBUSSEL, 0x0000000B, TRUE);
+        write_client_reg(MONI, 0x00000008, TRUE);
+        write_client_reg(CLKENB, 0x000000EF, TRUE);
+        write_client_reg(CLKENB, 0x000010EF, TRUE);
+        write_client_reg(CLKENB, 0x000011EF, TRUE);
+        write_client_reg(BITMAP4, 0x00DC00B0, TRUE);
+        write_client_reg(HCYCLE, 0x0000006B, TRUE);
+        write_client_reg(HSW, 0x00000003, TRUE);
+        write_client_reg(HDE_START, 0x00000002, TRUE);
+        write_client_reg(HDE_SIZE, 0x00000057, TRUE);
+        write_client_reg(VCYCLE, 0x000000E6, TRUE);
+        write_client_reg(VSW, 0x00000001, TRUE);
+        write_client_reg(VDE_START, 0x00000003, TRUE);
+        write_client_reg(VDE_SIZE, 0x000000DB, TRUE);
+        write_client_reg(WRSTB, 0x00000015, TRUE);
+        write_client_reg(MPLFBUF, 0x00000004, TRUE);
+        write_client_reg(ASY_DATA, 0x80000021, TRUE);
+        write_client_reg(ASY_DATB, 0x00000000, TRUE);
+        write_client_reg(ASY_DATC, 0x80000022, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000007, TRUE);
+        write_client_reg(PXL, 0x00000089, TRUE);
+        write_client_reg(VSYNIF, 0x00000001, TRUE);
+        mddi_wait(2);
+}
+
+static void toshiba_sec_cont_update_stop(struct msm_fb_data_type *mfd)
+{
+        if (TM_GET_PID(mfd->panel.id) == LCD_TOSHIBA_2P4_WVGA_PT)
+                return;
+
+        write_client_reg(PXL, 0x00000000, TRUE);
+        write_client_reg(VSYNIF, 0x00000000, TRUE);
+        write_client_reg(START, 0x00000000, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000000, TRUE);
+        mddi_wait(3);
+        write_client_reg(SRST, 0x00000002, TRUE);
+        mddi_wait(3);
+        write_client_reg(SRST, 0x00000003, TRUE);
+}
+
+static void toshiba_sec_backlight_on(struct msm_fb_data_type *mfd)
+{
+        if (TM_GET_PID(mfd->panel.id) == LCD_TOSHIBA_2P4_WVGA_PT)
+                return;
+
+        write_client_reg(TIMER0CTRL, 0x00000060, TRUE);
+        write_client_reg(TIMER0LOAD, 0x00001388, TRUE);
+        write_client_reg(PWM0OFF, 0x00000001, TRUE);
+        write_client_reg(TIMER1CTRL, 0x00000060, TRUE);
+        write_client_reg(TIMER1LOAD, 0x00001388, TRUE);
+        write_client_reg(PWM1OFF, 0x00001387, TRUE);
+        write_client_reg(TIMER0CTRL, 0x000000E0, TRUE);
+        write_client_reg(TIMER1CTRL, 0x000000E0, TRUE);
+        write_client_reg(PWMCR, 0x00000003, TRUE);
+}
+
+static void toshiba_sec_sleep_in(struct msm_fb_data_type *mfd)
+{
+        if (TM_GET_PID(mfd->panel.id) == LCD_TOSHIBA_2P4_WVGA_PT)
+                return;
+
+        write_client_reg(VSYNIF, 0x00000000, TRUE);
+        write_client_reg(PORT_ENB, 0x00000002, TRUE);
+        write_client_reg(ASY_DATA, 0x80000007, TRUE);
+        write_client_reg(ASY_DATB, 0x00004016, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        write_client_reg(ASY_DATB, 0x00000019, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        write_client_reg(ASY_DATB, 0x0000000B, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        write_client_reg(ASY_DATB, 0x00000002, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(4);
+        write_client_reg(ASY_DATA, 0x80000010, TRUE);
+        write_client_reg(ASY_DATB, 0x00000300, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(4);
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        write_client_reg(ASY_DATB, 0x00000000, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+        write_client_reg(ASY_DATA, 0x80000007, TRUE);
+        write_client_reg(ASY_DATB, 0x00004004, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+        write_client_reg(PORT, 0x00000000, TRUE);
+        write_client_reg(PXL, 0x00000000, TRUE);
+        write_client_reg(START, 0x00000000, TRUE);
+        write_client_reg(REGENB, 0x00000001, TRUE);
+        /* Sleep in sequence */
+        write_client_reg(ASY_DATA, 0x80000010, TRUE);
+        write_client_reg(ASY_DATB, 0x00000302, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+}
+
+static void toshiba_sec_sleep_out(struct msm_fb_data_type *mfd)
+{
+        if (TM_GET_PID(mfd->panel.id) == LCD_TOSHIBA_2P4_WVGA_PT)
+                return;
+
+        write_client_reg(VSYNIF, 0x00000000, TRUE);
+        write_client_reg(PORT_ENB, 0x00000002, TRUE);
+        write_client_reg(ASY_DATA, 0x80000010, TRUE);
+        write_client_reg(ASY_DATB, 0x00000300, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        /*  Display ON sequence */
+        write_client_reg(ASY_DATA, 0x80000011, TRUE);
+        write_client_reg(ASY_DATB, 0x00000812, TRUE);
+        write_client_reg(ASY_DATC, 0x80000012, TRUE);
+        write_client_reg(ASY_DATD, 0x00000003, TRUE);
+        write_client_reg(ASY_DATE, 0x80000013, TRUE);
+        write_client_reg(ASY_DATF, 0x00000909, TRUE);
+        write_client_reg(ASY_DATG, 0x80000010, TRUE);
+        write_client_reg(ASY_DATH, 0x00000040, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000001, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000000, TRUE);
+        mddi_wait(4);
+        write_client_reg(ASY_DATA, 0x80000010, TRUE);
+        write_client_reg(ASY_DATB, 0x00000340, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(6);
+        write_client_reg(ASY_DATA, 0x80000010, TRUE);
+        write_client_reg(ASY_DATB, 0x00003340, TRUE);
+        write_client_reg(ASY_DATC, 0x80000007, TRUE);
+        write_client_reg(ASY_DATD, 0x00004007, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000009, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000008, TRUE);
+        mddi_wait(1);
+        write_client_reg(ASY_DATA, 0x80000007, TRUE);
+        write_client_reg(ASY_DATB, 0x00004017, TRUE);
+        write_client_reg(ASY_DATC, 0x8000005B, TRUE);
+        write_client_reg(ASY_DATD, 0x00000000, TRUE);
+        write_client_reg(ASY_DATE, 0x80000059, TRUE);
+        write_client_reg(ASY_DATF, 0x00000011, TRUE);
+        write_client_reg(ASY_CMDSET, 0x0000000D, TRUE);
+        write_client_reg(ASY_CMDSET, 0x0000000C, TRUE);
+        mddi_wait(2);
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        write_client_reg(ASY_DATB, 0x00000019, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        write_client_reg(ASY_DATB, 0x00000079, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+        write_client_reg(ASY_DATA, 0x80000059, TRUE);
+        write_client_reg(ASY_DATB, 0x000003FD, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000005, TRUE);
+        write_client_reg(ASY_CMDSET, 0x00000004, TRUE);
+        mddi_wait(2);
+}
+
+static void mddi_toshiba_lcd_set_backlight(struct msm_fb_data_type *mfd)
+{
+        int32 level;
+        int ret = -EPERM;
+        int max = mfd->panel_info.bl_max;
+        int min = mfd->panel_info.bl_min;
+
+        if (mddi_toshiba_pdata && mddi_toshiba_pdata->pmic_backlight) {
+                ret = mddi_toshiba_pdata->pmic_backlight(mfd->bl_level);
+                if (!ret)
+                        return;
+        }
+
+        if (ret && mddi_toshiba_pdata && mddi_toshiba_pdata->backlight_level) {
+                level = mddi_toshiba_pdata->backlight_level(mfd->bl_level,
+                                                                max, min);
+
+                if (level < 0)
+                        return;
+
+                if (TM_GET_PID(mfd->panel.id) == LCD_SHARP_2P4_VGA)
+                        write_client_reg(TIMER0LOAD, 0x00001388, TRUE);
+        } else {
+                if (!max)
+                        level = 0;
+                else
+                        level = (mfd->bl_level * 4999) / max;
+        }
+
+        write_client_reg(PWM0OFF, level, TRUE);
+}
+
+static void mddi_toshiba_vsync_set_handler(msm_fb_vsync_handler_type handler,   /* ISR to be executed */
+                                           void *arg)
+{
+        boolean error = FALSE;
+        unsigned long flags;
+
+        /* Disable interrupts */
+        spin_lock_irqsave(&mddi_host_spin_lock, flags);
+        /* INTLOCK(); */
+
+        if (mddi_toshiba_vsync_handler != NULL) {
+                error = TRUE;
+        } else {
+                /* Register the handler for this particular GROUP interrupt source */
+                mddi_toshiba_vsync_handler = handler;
+                mddi_toshiba_vsync_handler_arg = arg;
+        }
+
+        /* Restore interrupts */
+        spin_unlock_irqrestore(&mddi_host_spin_lock, flags);
+        /* MDDI_INTFREE(); */
+        if (error) {
+                MDDI_MSG_ERR("MDDI: Previous Vsync handler never called\n");
+        } else {
+                /* Enable the vsync wakeup */
+                mddi_queue_register_write(INTMSK, 0x0000, FALSE, 0);
+
+                mddi_toshiba_vsync_attempts = 1;
+                mddi_vsync_detect_enabled = TRUE;
+        }
+}                               /* mddi_toshiba_vsync_set_handler */
+
+static void mddi_toshiba_lcd_vsync_detected(boolean detected)
+{
+        /* static timetick_type start_time = 0; */
+        static struct timeval start_time;
+        static boolean first_time = TRUE;
+        /* uint32 mdp_cnt_val = 0; */
+        /* timetick_type elapsed_us; */
+        struct timeval now;
+        uint32 elapsed_us;
+        uint32 num_vsyncs;
+
+        if ((detected) || (mddi_toshiba_vsync_attempts > 5)) {
+                if ((detected) && (mddi_toshiba_monitor_refresh_value)) {
+                        /* if (start_time != 0) */
+                        if (!first_time) {
+                                jiffies_to_timeval(jiffies, &now);
+                                elapsed_us =
+                                    (now.tv_sec - start_time.tv_sec) * 1000000 +
+                                    now.tv_usec - start_time.tv_usec;
+                                /*
+                                 * LCD is configured for a refresh every usecs,
+                                 *  so to determine the number of vsyncs that
+                                 *  have occurred since the last measurement
+                                 *  add half that to the time difference and
+                                 *  divide by the refresh rate.
+                                 */
+                                num_vsyncs = (elapsed_us +
+                                              (mddi_toshiba_usecs_per_refresh >>
+                                               1)) /
+                                    mddi_toshiba_usecs_per_refresh;
+                                /*
+                                 * LCD is configured for * hsyncs (rows) per
+                                 * refresh cycle. Calculate new rows_per_second
+                                 * value based upon these new measurements.
+                                 * MDP can update with this new value.
+                                 */
+                                mddi_toshiba_rows_per_second =
+                                    (mddi_toshiba_rows_per_refresh * 1000 *
+                                     num_vsyncs) / (elapsed_us / 1000);
+                        }
+                        /* start_time = timetick_get(); */
+                        first_time = FALSE;
+                        jiffies_to_timeval(jiffies, &start_time);
+                        if (mddi_toshiba_report_refresh_measurements) {
+                                (void)mddi_queue_register_read_int(VPOS,
+                                                                   &mddi_toshiba_curr_vpos);
+                                /* mdp_cnt_val = MDP_LINE_COUNT; */
+                        }
+                }
+                /* if detected = TRUE, client initiated wakeup was detected */
+                if (mddi_toshiba_vsync_handler != NULL) {
+                        (*mddi_toshiba_vsync_handler)
+                            (mddi_toshiba_vsync_handler_arg);
+                        mddi_toshiba_vsync_handler = NULL;
+                }
+                mddi_vsync_detect_enabled = FALSE;
+                mddi_toshiba_vsync_attempts = 0;
+                /* need to disable the interrupt wakeup */
+                if (!mddi_queue_register_write_int(INTMSK, 0x0001))
+                        MDDI_MSG_ERR("Vsync interrupt disable failed!\n");
+                if (!detected) {
+                        /* give up after 5 failed attempts but show error */
+                        MDDI_MSG_NOTICE("Vsync detection failed!\n");
+                } else if ((mddi_toshiba_monitor_refresh_value) &&
+                           (mddi_toshiba_report_refresh_measurements)) {
+                        MDDI_MSG_NOTICE("  Last Line Counter=%d!\n",
+                                        mddi_toshiba_curr_vpos);
+                /* MDDI_MSG_NOTICE("  MDP Line Counter=%d!\n",mdp_cnt_val); */
+                        MDDI_MSG_NOTICE("  Lines Per Second=%d!\n",
+                                        mddi_toshiba_rows_per_second);
+                }
+                /* clear the interrupt */
+                if (!mddi_queue_register_write_int(INTFLG, 0x0001))
+                        MDDI_MSG_ERR("Vsync interrupt clear failed!\n");
+        } else {
+                /* if detected = FALSE, we woke up from hibernation, but did not
+                 * detect client initiated wakeup.
+                 */
+                mddi_toshiba_vsync_attempts++;
+        }
+}
+
+static void mddi_toshiba_prim_init(struct msm_fb_data_type *mfd)
+{
+
+        switch (toshiba_state) {
+        case TOSHIBA_STATE_PRIM_SEC_READY:
+                break;
+        case TOSHIBA_STATE_OFF:
+                toshiba_state = TOSHIBA_STATE_PRIM_SEC_STANDBY;
+                toshiba_common_initial_setup(mfd);
+                break;
+        case TOSHIBA_STATE_PRIM_SEC_STANDBY:
+                toshiba_common_initial_setup(mfd);
+                break;
+        case TOSHIBA_STATE_SEC_NORMAL_MODE:
+                toshiba_sec_cont_update_stop(mfd);
+                toshiba_sec_sleep_in(mfd);
+                toshiba_sec_sleep_out(mfd);
+                toshiba_sec_lcd_off(mfd);
+                toshiba_common_initial_setup(mfd);
+                break;
+        default:
+                MDDI_MSG_ERR("mddi_toshiba_prim_init from state %d\n",
+                             toshiba_state);
+        }
+
+        toshiba_prim_start(mfd);
+        if (TM_GET_PID(mfd->panel.id) == LCD_SHARP_2P4_VGA)
+                gordon_disp_init();
+        mddi_host_write_pix_attr_reg(0x00C3);
+}
+
+static void mddi_toshiba_sec_init(struct msm_fb_data_type *mfd)
+{
+
+        switch (toshiba_state) {
+        case TOSHIBA_STATE_PRIM_SEC_READY:
+                break;
+        case TOSHIBA_STATE_PRIM_SEC_STANDBY:
+                toshiba_common_initial_setup(mfd);
+                break;
+        case TOSHIBA_STATE_PRIM_NORMAL_MODE:
+                toshiba_prim_lcd_off(mfd);
+                toshiba_common_initial_setup(mfd);
+                break;
+        default:
+                MDDI_MSG_ERR("mddi_toshiba_sec_init from state %d\n",
+                             toshiba_state);
+        }
+
+        toshiba_sec_start(mfd);
+        toshiba_sec_backlight_on(mfd);
+        toshiba_sec_cont_update_start(mfd);
+        mddi_host_write_pix_attr_reg(0x0400);
+}
+
+static void mddi_toshiba_lcd_powerdown(struct msm_fb_data_type *mfd)
+{
+        switch (toshiba_state) {
+        case TOSHIBA_STATE_PRIM_SEC_READY:
+                mddi_toshiba_prim_init(mfd);
+                mddi_toshiba_lcd_powerdown(mfd);
+                return;
+        case TOSHIBA_STATE_PRIM_SEC_STANDBY:
+                break;
+        case TOSHIBA_STATE_PRIM_NORMAL_MODE:
+                toshiba_prim_lcd_off(mfd);
+                break;
+        case TOSHIBA_STATE_SEC_NORMAL_MODE:
+                toshiba_sec_cont_update_stop(mfd);
+                toshiba_sec_sleep_in(mfd);
+                toshiba_sec_sleep_out(mfd);
+                toshiba_sec_lcd_off(mfd);
+                break;
+        default:
+                MDDI_MSG_ERR("mddi_toshiba_lcd_powerdown from state %d\n",
+                             toshiba_state);
+        }
+}
+
+static int mddi_sharpgordon_firsttime = 1;
+
+static int mddi_toshiba_lcd_on(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+        mfd = platform_get_drvdata(pdev);
+        if (!mfd)
+                return -ENODEV;
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        if (TM_GET_DID(mfd->panel.id) == TOSHIBA_VGA_PRIM)
+                mddi_toshiba_prim_init(mfd);
+        else
+                mddi_toshiba_sec_init(mfd);
+        if (TM_GET_PID(mfd->panel.id) == LCD_SHARP_2P4_VGA) {
+                if (mddi_sharpgordon_firsttime) {
+                        mddi_sharpgordon_firsttime = 0;
+                        write_client_reg(REGENB, 0x00000001, TRUE);
+                }
+        }
+        return 0;
+}
+
+static int mddi_toshiba_lcd_off(struct platform_device *pdev)
+{
+        mddi_toshiba_lcd_powerdown(platform_get_drvdata(pdev));
+        return 0;
+}
+
+static int __init mddi_toshiba_lcd_probe(struct platform_device *pdev)
+{
+        if (pdev->id == 0) {
+                mddi_toshiba_pdata = pdev->dev.platform_data;
+                return 0;
+        }
+
+        msm_fb_add_device(pdev);
+
+        return 0;
+}
+
+static struct platform_driver this_driver = {
+        .probe  = mddi_toshiba_lcd_probe,
+        .driver = {
+                .name   = "mddi_toshiba",
+        },
+};
+
+static struct msm_fb_panel_data toshiba_panel_data = {
+        .on             = mddi_toshiba_lcd_on,
+        .off            = mddi_toshiba_lcd_off,
+};
+
+static int ch_used[3];
+
+int mddi_toshiba_device_register(struct msm_panel_info *pinfo,
+                                        u32 channel, u32 panel)
+{
+        struct platform_device *pdev = NULL;
+        int ret;
+
+        if ((channel >= 3) || ch_used[channel])
+                return -ENODEV;
+
+        if ((channel != TOSHIBA_VGA_PRIM) &&
+            mddi_toshiba_pdata && mddi_toshiba_pdata->panel_num)
+                if (mddi_toshiba_pdata->panel_num() < 2)
+                        return -ENODEV;
+
+        ch_used[channel] = TRUE;
+
+        pdev = platform_device_alloc("mddi_toshiba", (panel << 8)|channel);
+        if (!pdev)
+                return -ENOMEM;
+
+        if (channel == TOSHIBA_VGA_PRIM) {
+                toshiba_panel_data.set_backlight =
+                                mddi_toshiba_lcd_set_backlight;
+
+                if (pinfo->lcd.vsync_enable) {
+                        toshiba_panel_data.set_vsync_notifier =
+                                mddi_toshiba_vsync_set_handler;
+                        mddi_lcd.vsync_detected =
+                                mddi_toshiba_lcd_vsync_detected;
+                }
+        } else {
+                toshiba_panel_data.set_backlight = NULL;
+                toshiba_panel_data.set_vsync_notifier = NULL;
+        }
+
+        toshiba_panel_data.panel_info = *pinfo;
+
+        ret = platform_device_add_data(pdev, &toshiba_panel_data,
+                sizeof(toshiba_panel_data));
+        if (ret) {
+                printk(KERN_ERR
+                  "%s: platform_device_add_data failed!\n", __func__);
+                goto err_device_put;
+        }
+
+        ret = platform_device_add(pdev);
+        if (ret) {
+                printk(KERN_ERR
+                  "%s: platform_device_register failed!\n", __func__);
+                goto err_device_put;
+        }
+
+        return 0;
+
+err_device_put:
+        platform_device_put(pdev);
+        return ret;
+}
+
+static int __init mddi_toshiba_lcd_init(void)
+{
+        return platform_driver_register(&this_driver);
+}
+
+module_init(mddi_toshiba_lcd_init);
diff --git a/drivers/staging/msm/mddi_toshiba.h b/drivers/staging/msm/mddi_toshiba.h
new file mode 100644
index 000000000000..2d22b9a2c413
--- /dev/null
+++ b/drivers/staging/msm/mddi_toshiba.h
@@ -0,0 +1,52 @@
+/* Copyright (c) 2009, Code Aurora Forum. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Code Aurora nor
+ *       the names of its contributors may be used to endorse or promote
+ *       products derived from this software without specific prior written
+ *       permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NON-INFRINGEMENT ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef MDDI_TOSHIBA_H
+#define MDDI_TOSHIBA_H
+
+#define TOSHIBA_VGA_PRIM 1
+#define TOSHIBA_VGA_SECD 2
+
+#define LCD_TOSHIBA_2P4_VGA     0
+#define LCD_TOSHIBA_2P4_WVGA    1
+#define LCD_TOSHIBA_2P4_WVGA_PT 2
+#define LCD_SHARP_2P4_VGA       3
+
+#define GPIO_BLOCK_BASE        0x150000
+#define SYSTEM_BLOCK2_BASE     0x170000
+
+#define GPIODIR     (GPIO_BLOCK_BASE|0x04)
+#define GPIOSEL     (SYSTEM_BLOCK2_BASE|0x00)
+#define GPIOPC      (GPIO_BLOCK_BASE|0x28)
+#define GPIODATA    (GPIO_BLOCK_BASE|0x00)
+
+#define write_client_reg(__X, __Y, __Z) {\
+  mddi_queue_register_write(__X, __Y, TRUE, 0);\
+}
+
+#endif /* MDDI_TOSHIBA_H */
diff --git a/drivers/staging/msm/mddi_toshiba_vga.c b/drivers/staging/msm/mddi_toshiba_vga.c
new file mode 100644
index 000000000000..7e61d3a5b8f1
--- /dev/null
+++ b/drivers/staging/msm/mddi_toshiba_vga.c
@@ -0,0 +1,136 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+#include "mddihost.h"
+#include "mddihosti.h"
+#include "mddi_toshiba.h"
+
+static uint32 read_client_reg(uint32 addr)
+{
+        uint32 val;
+        mddi_queue_register_read(addr, &val, TRUE, 0);
+        return val;
+}
+
+static uint32 toshiba_lcd_gpio_read(void)
+{
+        uint32 val;
+
+        write_client_reg(GPIODIR, 0x0000000C, TRUE);
+        write_client_reg(GPIOSEL, 0x00000000, TRUE);
+        write_client_reg(GPIOSEL, 0x00000000, TRUE);
+        write_client_reg(GPIOPC, 0x03CF00C0, TRUE);
+        val = read_client_reg(GPIODATA) & 0x2C0;
+
+        return val;
+}
+
+static u32 mddi_toshiba_panel_detect(void)
+{
+        mddi_host_type host_idx = MDDI_HOST_PRIM;
+        uint32 lcd_gpio;
+        u32 mddi_toshiba_lcd = LCD_TOSHIBA_2P4_VGA;
+
+        /* Toshiba display requires larger drive_lo value */
+        mddi_host_reg_out(DRIVE_LO, 0x0050);
+
+        lcd_gpio = toshiba_lcd_gpio_read();
+        switch (lcd_gpio) {
+        case 0x0080:
+                mddi_toshiba_lcd = LCD_SHARP_2P4_VGA;
+                break;
+
+        case 0x00C0:
+        default:
+                mddi_toshiba_lcd = LCD_TOSHIBA_2P4_VGA;
+                break;
+        }
+
+        return mddi_toshiba_lcd;
+}
+
+static int __init mddi_toshiba_vga_init(void)
+{
+        int ret;
+        struct msm_panel_info pinfo;
+        u32 panel;
+
+#ifdef CONFIG_FB_MSM_MDDI_AUTO_DETECT
+        u32 id;
+
+        ret = msm_fb_detect_client("mddi_toshiba_vga");
+        if (ret == -ENODEV)
+                return 0;
+
+        if (ret) {
+                id = mddi_get_client_id();
+                if ((id >> 16) != 0xD263)
+                        return 0;
+        }
+#endif
+
+        panel = mddi_toshiba_panel_detect();
+
+        pinfo.xres = 480;
+        pinfo.yres = 640;
+        pinfo.type = MDDI_PANEL;
+        pinfo.pdest = DISPLAY_1;
+        pinfo.mddi.vdopkt = MDDI_DEFAULT_PRIM_PIX_ATTR;
+        pinfo.wait_cycle = 0;
+        pinfo.bpp = 18;
+        pinfo.lcd.vsync_enable = TRUE;
+        pinfo.lcd.refx100 = 6118;
+        pinfo.lcd.v_back_porch = 6;
+        pinfo.lcd.v_front_porch = 0;
+        pinfo.lcd.v_pulse_width = 0;
+        pinfo.lcd.hw_vsync_mode = FALSE;
+        pinfo.lcd.vsync_notifier_period = (1 * HZ);
+        pinfo.bl_max = 99;
+        pinfo.bl_min = 1;
+        pinfo.clk_rate = 122880000;
+        pinfo.clk_min =  120000000;
+        pinfo.clk_max =  200000000;
+        pinfo.fb_num = 2;
+
+        ret = mddi_toshiba_device_register(&pinfo, TOSHIBA_VGA_PRIM, panel);
+        if (ret) {
+                printk(KERN_ERR "%s: failed to register device!\n", __func__);
+                return ret;
+        }
+
+        pinfo.xres = 176;
+        pinfo.yres = 220;
+        pinfo.type = MDDI_PANEL;
+        pinfo.pdest = DISPLAY_2;
+        pinfo.mddi.vdopkt = 0x400;
+        pinfo.wait_cycle = 0;
+        pinfo.bpp = 18;
+        pinfo.clk_rate = 122880000;
+        pinfo.clk_min =  120000000;
+        pinfo.clk_max =  200000000;
+        pinfo.fb_num = 2;
+
+        ret = mddi_toshiba_device_register(&pinfo, TOSHIBA_VGA_SECD, panel);
+        if (ret)
+                printk(KERN_WARNING
+                        "%s: failed to register device!\n", __func__);
+
+        return ret;
+}
+
+module_init(mddi_toshiba_vga_init);
diff --git a/drivers/staging/msm/mddi_toshiba_wvga.c b/drivers/staging/msm/mddi_toshiba_wvga.c
new file mode 100644
index 000000000000..557b0f08faf8
--- /dev/null
+++ b/drivers/staging/msm/mddi_toshiba_wvga.c
@@ -0,0 +1,63 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+#include "mddihost.h"
+#include "mddi_toshiba.h"
+
+static int __init mddi_toshiba_wvga_init(void)
+{
+        int ret;
+        struct msm_panel_info pinfo;
+
+#ifdef CONFIG_FB_MSM_MDDI_AUTO_DETECT
+        if (msm_fb_detect_client("mddi_toshiba_wvga"))
+                return 0;
+#endif
+
+        pinfo.xres = 800;
+        pinfo.yres = 480;
+        pinfo.pdest = DISPLAY_2;
+        pinfo.type = MDDI_PANEL;
+        pinfo.mddi.vdopkt = MDDI_DEFAULT_PRIM_PIX_ATTR;
+        pinfo.wait_cycle = 0;
+        pinfo.bpp = 18;
+        pinfo.lcd.vsync_enable = TRUE;
+        pinfo.lcd.refx100 = 6118;
+        pinfo.lcd.v_back_porch = 6;
+        pinfo.lcd.v_front_porch = 0;
+        pinfo.lcd.v_pulse_width = 0;
+        pinfo.lcd.hw_vsync_mode = FALSE;
+        pinfo.lcd.vsync_notifier_period = (1 * HZ);
+        pinfo.bl_max = 4;
+        pinfo.bl_min = 1;
+        pinfo.clk_rate = 192000000;
+        pinfo.clk_min =  190000000;
+        pinfo.clk_max =  200000000;
+        pinfo.fb_num = 2;
+
+        ret = mddi_toshiba_device_register(&pinfo, TOSHIBA_VGA_PRIM,
+                                           LCD_TOSHIBA_2P4_WVGA);
+        if (ret) {
+                printk(KERN_ERR "%s: failed to register device!\n", __func__);
+                return ret;
+        }
+
+        return ret;
+}
+
+module_init(mddi_toshiba_wvga_init);
diff --git a/drivers/staging/msm/mddi_toshiba_wvga_pt.c b/drivers/staging/msm/mddi_toshiba_wvga_pt.c
new file mode 100644
index 000000000000..fc7d4e0d294f
--- /dev/null
+++ b/drivers/staging/msm/mddi_toshiba_wvga_pt.c
@@ -0,0 +1,64 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "msm_fb.h"
+#include "mddihost.h"
+#include "mddihosti.h"
+#include "mddi_toshiba.h"
+
+static int __init mddi_toshiba_wvga_pt_init(void)
+{
+        int ret;
+        struct msm_panel_info pinfo;
+#ifdef CONFIG_FB_MSM_MDDI_AUTO_DETECT
+        uint id;
+
+        ret = msm_fb_detect_client("mddi_toshiba_wvga_pt");
+        if (ret == -ENODEV)
+                return 0;
+
+        if (ret) {
+                id = mddi_get_client_id();
+                if (id != 0xd2638722)
+                        return 0;
+        }
+#endif
+
+        pinfo.xres = 480;
+        pinfo.yres = 800;
+        pinfo.type = MDDI_PANEL;
+        pinfo.pdest = DISPLAY_1;
+        pinfo.mddi.vdopkt = MDDI_DEFAULT_PRIM_PIX_ATTR;
+        pinfo.wait_cycle = 0;
+        pinfo.bpp = 18;
+        pinfo.lcd.vsync_enable = FALSE;
+        pinfo.bl_max = 15;
+        pinfo.bl_min = 1;
+        pinfo.clk_rate = 192000000;
+        pinfo.clk_min =  190000000;
+        pinfo.clk_max =  200000000;
+        pinfo.fb_num = 2;
+
+        ret = mddi_toshiba_device_register(&pinfo, TOSHIBA_VGA_PRIM,
+                                                LCD_TOSHIBA_2P4_WVGA_PT);
+        if (ret)
+                printk(KERN_ERR "%s: failed to register device!\n", __func__);
+
+        return ret;
+}
+
+module_init(mddi_toshiba_wvga_pt_init);
diff --git a/drivers/staging/msm/mddihost.c b/drivers/staging/msm/mddihost.c
new file mode 100644
index 000000000000..c6c1ee4eda05
--- /dev/null
+++ b/drivers/staging/msm/mddihost.c
@@ -0,0 +1,377 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+
+#include "msm_fb.h"
+#include "mddihost.h"
+#include "mddihosti.h"
+
+#include <linux/clk.h>
+#include <mach/clk.h>
+
+struct semaphore mddi_host_mutex;
+
+struct clk *mddi_io_clk;
+static boolean mddi_host_powered = FALSE;
+static boolean mddi_host_initialized = FALSE;
+extern uint32 *mddi_reg_read_value_ptr;
+
+mddi_lcd_func_type mddi_lcd;
+
+extern mddi_client_capability_type mddi_client_capability_pkt;
+
+#ifdef FEATURE_MDDI_HITACHI
+extern void mddi_hitachi_window_adjust(uint16 x1,
+                                       uint16 x2, uint16 y1, uint16 y2);
+#endif
+
+extern void mddi_toshiba_lcd_init(void);
+
+#ifdef FEATURE_MDDI_S6D0142
+extern void mddi_s6d0142_lcd_init(void);
+extern void mddi_s6d0142_window_adjust(uint16 x1,
+                                       uint16 x2,
+                                       uint16 y1,
+                                       uint16 y2,
+                                       mddi_llist_done_cb_type done_cb);
+#endif
+
+void mddi_init(void)
+{
+        if (mddi_host_initialized)
+                return;
+
+        mddi_host_initialized = TRUE;
+
+        init_MUTEX(&mddi_host_mutex);
+
+        if (!mddi_host_powered) {
+                down(&mddi_host_mutex);
+                mddi_host_init(MDDI_HOST_PRIM);
+                mddi_host_powered = TRUE;
+                up(&mddi_host_mutex);
+                mdelay(10);
+        }
+}
+
+int mddi_host_register_read(uint32 reg_addr,
+     uint32 *reg_value_ptr, boolean wait, mddi_host_type host) {
+        mddi_linked_list_type *curr_llist_ptr;
+        mddi_register_access_packet_type *regacc_pkt_ptr;
+        uint16 curr_llist_idx;
+        int ret = 0;
+
+        if (in_interrupt())
+                MDDI_MSG_CRIT("Called from ISR context\n");
+
+        if (!mddi_host_powered) {
+                MDDI_MSG_ERR("MDDI powered down!\n");
+                mddi_init();
+        }
+
+        down(&mddi_host_mutex);
+
+        mddi_reg_read_value_ptr = reg_value_ptr;
+        curr_llist_idx = mddi_get_reg_read_llist_item(host, TRUE);
+        if (curr_llist_idx == UNASSIGNED_INDEX) {
+                up(&mddi_host_mutex);
+
+                /* need to change this to some sort of wait */
+                MDDI_MSG_ERR("Attempting to queue up more than 1 reg read\n");
+                return -EINVAL;
+        }
+
+        curr_llist_ptr = &llist_extern[host][curr_llist_idx];
+        curr_llist_ptr->link_controller_flags = 0x11;
+        curr_llist_ptr->packet_header_count = 14;
+        curr_llist_ptr->packet_data_count = 0;
+
+        curr_llist_ptr->next_packet_pointer = NULL;
+        curr_llist_ptr->packet_data_pointer = NULL;
+        curr_llist_ptr->reserved = 0;
+
+        regacc_pkt_ptr = &curr_llist_ptr->packet_header.register_pkt;
+
+        regacc_pkt_ptr->packet_length = curr_llist_ptr->packet_header_count;
+        regacc_pkt_ptr->packet_type = 146;      /* register access packet */
+        regacc_pkt_ptr->bClient_ID = 0;
+        regacc_pkt_ptr->read_write_info = 0x8001;
+        regacc_pkt_ptr->register_address = reg_addr;
+
+        /* now adjust pointers */
+        mddi_queue_forward_packets(curr_llist_idx, curr_llist_idx, wait,
+                                   NULL, host);
+        /* need to check if we can write the pointer or not */
+
+        up(&mddi_host_mutex);
+
+        if (wait) {
+                int wait_ret;
+
+                mddi_linked_list_notify_type *llist_notify_ptr;
+                llist_notify_ptr = &llist_extern_notify[host][curr_llist_idx];
+                wait_ret = wait_for_completion_timeout(
+                                        &(llist_notify_ptr->done_comp), 5 * HZ);
+
+                if (wait_ret <= 0)
+                        ret = -EBUSY;
+
+                if (wait_ret < 0)
+                        printk(KERN_ERR "%s: failed to wait for completion!\n",
+                                __func__);
+                else if (!wait_ret)
+                        printk(KERN_ERR "%s: Timed out waiting!\n", __func__);
+        }
+
+        MDDI_MSG_DEBUG("Reg Read value=0x%x\n", *reg_value_ptr);
+
+        return ret;
+}                               /* mddi_host_register_read */
+
+int mddi_host_register_write(uint32 reg_addr,
+     uint32 reg_val, enum mddi_data_packet_size_type packet_size,
+     boolean wait, mddi_llist_done_cb_type done_cb, mddi_host_type host) {
+        mddi_linked_list_type *curr_llist_ptr;
+        mddi_linked_list_type *curr_llist_dma_ptr;
+        mddi_register_access_packet_type *regacc_pkt_ptr;
+        uint16 curr_llist_idx;
+        int ret = 0;
+
+        if (in_interrupt())
+                MDDI_MSG_CRIT("Called from ISR context\n");
+
+        if (!mddi_host_powered) {
+                MDDI_MSG_ERR("MDDI powered down!\n");
+                mddi_init();
+        }
+
+        down(&mddi_host_mutex);
+
+        curr_llist_idx = mddi_get_next_free_llist_item(host, TRUE);
+        curr_llist_ptr = &llist_extern[host][curr_llist_idx];
+        curr_llist_dma_ptr = &llist_dma_extern[host][curr_llist_idx];
+
+        curr_llist_ptr->link_controller_flags = 1;
+        curr_llist_ptr->packet_header_count = 14;
+        curr_llist_ptr->packet_data_count = 4;
+
+        curr_llist_ptr->next_packet_pointer = NULL;
+        curr_llist_ptr->reserved = 0;
+
+        regacc_pkt_ptr = &curr_llist_ptr->packet_header.register_pkt;
+
+        regacc_pkt_ptr->packet_length = curr_llist_ptr->packet_header_count +
+                                        (uint16)packet_size;
+        regacc_pkt_ptr->packet_type = 146;      /* register access packet */
+        regacc_pkt_ptr->bClient_ID = 0;
+        regacc_pkt_ptr->read_write_info = 0x0001;
+        regacc_pkt_ptr->register_address = reg_addr;
+        regacc_pkt_ptr->register_data_list = reg_val;
+
+        MDDI_MSG_DEBUG("Reg Access write reg=0x%x, value=0x%x\n",
+                       regacc_pkt_ptr->register_address,
+                       regacc_pkt_ptr->register_data_list);
+
+        regacc_pkt_ptr = &curr_llist_dma_ptr->packet_header.register_pkt;
+        curr_llist_ptr->packet_data_pointer =
+            (void *)(&regacc_pkt_ptr->register_data_list);
+
+        /* now adjust pointers */
+        mddi_queue_forward_packets(curr_llist_idx, curr_llist_idx, wait,
+                                   done_cb, host);
+
+        up(&mddi_host_mutex);
+
+        if (wait) {
+                int wait_ret;
+
+                mddi_linked_list_notify_type *llist_notify_ptr;
+                llist_notify_ptr = &llist_extern_notify[host][curr_llist_idx];
+                wait_ret = wait_for_completion_timeout(
+                                        &(llist_notify_ptr->done_comp), 5 * HZ);
+
+                if (wait_ret <= 0)
+                        ret = -EBUSY;
+
+                if (wait_ret < 0)
+                        printk(KERN_ERR "%s: failed to wait for completion!\n",
+                                __func__);
+                else if (!wait_ret)
+                        printk(KERN_ERR "%s: Timed out waiting!\n", __func__);
+        }
+
+        return ret;
+}                               /* mddi_host_register_write */
+
+boolean mddi_host_register_read_int
+    (uint32 reg_addr, uint32 *reg_value_ptr, mddi_host_type host) {
+        mddi_linked_list_type *curr_llist_ptr;
+        mddi_register_access_packet_type *regacc_pkt_ptr;
+        uint16 curr_llist_idx;
+
+        if (!in_interrupt())
+                MDDI_MSG_CRIT("Called from TASK context\n");
+
+        if (!mddi_host_powered) {
+                MDDI_MSG_ERR("MDDI powered down!\n");
+                return FALSE;
+        }
+
+        if (down_trylock(&mddi_host_mutex) != 0)
+                return FALSE;
+
+        mddi_reg_read_value_ptr = reg_value_ptr;
+        curr_llist_idx = mddi_get_reg_read_llist_item(host, FALSE);
+        if (curr_llist_idx == UNASSIGNED_INDEX) {
+                up(&mddi_host_mutex);
+                return FALSE;
+        }
+
+        curr_llist_ptr = &llist_extern[host][curr_llist_idx];
+        curr_llist_ptr->link_controller_flags = 0x11;
+        curr_llist_ptr->packet_header_count = 14;
+        curr_llist_ptr->packet_data_count = 0;
+
+        curr_llist_ptr->next_packet_pointer = NULL;
+        curr_llist_ptr->packet_data_pointer = NULL;
+        curr_llist_ptr->reserved = 0;
+
+        regacc_pkt_ptr = &curr_llist_ptr->packet_header.register_pkt;
+
+        regacc_pkt_ptr->packet_length = curr_llist_ptr->packet_header_count;
+        regacc_pkt_ptr->packet_type = 146;      /* register access packet */
+        regacc_pkt_ptr->bClient_ID = 0;
+        regacc_pkt_ptr->read_write_info = 0x8001;
+        regacc_pkt_ptr->register_address = reg_addr;
+
+        /* now adjust pointers */
+        mddi_queue_forward_packets(curr_llist_idx, curr_llist_idx, FALSE,
+                                   NULL, host);
+        /* need to check if we can write the pointer or not */
+
+        up(&mddi_host_mutex);
+
+        return TRUE;
+
+}                               /* mddi_host_register_read */
+
+boolean mddi_host_register_write_int
+    (uint32 reg_addr,
+     uint32 reg_val, mddi_llist_done_cb_type done_cb, mddi_host_type host) {
+        mddi_linked_list_type *curr_llist_ptr;
+        mddi_linked_list_type *curr_llist_dma_ptr;
+        mddi_register_access_packet_type *regacc_pkt_ptr;
+        uint16 curr_llist_idx;
+
+        if (!in_interrupt())
+                MDDI_MSG_CRIT("Called from TASK context\n");
+
+        if (!mddi_host_powered) {
+                MDDI_MSG_ERR("MDDI powered down!\n");
+                return FALSE;
+        }
+
+        if (down_trylock(&mddi_host_mutex) != 0)
+                return FALSE;
+
+        curr_llist_idx = mddi_get_next_free_llist_item(host, FALSE);
+        if (curr_llist_idx == UNASSIGNED_INDEX) {
+                up(&mddi_host_mutex);
+                return FALSE;
+        }
+
+        curr_llist_ptr = &llist_extern[host][curr_llist_idx];
+        curr_llist_dma_ptr = &llist_dma_extern[host][curr_llist_idx];
+
+        curr_llist_ptr->link_controller_flags = 1;
+        curr_llist_ptr->packet_header_count = 14;
+        curr_llist_ptr->packet_data_count = 4;
+
+        curr_llist_ptr->next_packet_pointer = NULL;
+        curr_llist_ptr->reserved = 0;
+
+        regacc_pkt_ptr = &curr_llist_ptr->packet_header.register_pkt;
+
+        regacc_pkt_ptr->packet_length = curr_llist_ptr->packet_header_count + 4;
+        regacc_pkt_ptr->packet_type = 146;      /* register access packet */
+        regacc_pkt_ptr->bClient_ID = 0;
+        regacc_pkt_ptr->read_write_info = 0x0001;
+        regacc_pkt_ptr->register_address = reg_addr;
+        regacc_pkt_ptr->register_data_list = reg_val;
+
+        regacc_pkt_ptr = &curr_llist_dma_ptr->packet_header.register_pkt;
+        curr_llist_ptr->packet_data_pointer =
+            (void *)(&(regacc_pkt_ptr->register_data_list));
+
+        /* now adjust pointers */
+        mddi_queue_forward_packets(curr_llist_idx, curr_llist_idx, FALSE,
+                                   done_cb, host);
+        up(&mddi_host_mutex);
+
+        return TRUE;
+
+}                               /* mddi_host_register_write */
+
+void mddi_wait(uint16 time_ms)
+{
+        mdelay(time_ms);
+}
+
+void mddi_client_lcd_vsync_detected(boolean detected)
+{
+        if (mddi_lcd.vsync_detected)
+                (*mddi_lcd.vsync_detected) (detected);
+}
+
+/* extended version of function includes done callback */
+void mddi_window_adjust_ext(struct msm_fb_data_type *mfd,
+                            uint16 x1,
+                            uint16 x2,
+                            uint16 y1,
+                            uint16 y2, mddi_llist_done_cb_type done_cb)
+{
+#ifdef FEATURE_MDDI_HITACHI
+        if (mfd->panel.id == HITACHI)
+                mddi_hitachi_window_adjust(x1, x2, y1, y2);
+#elif defined(FEATURE_MDDI_S6D0142)
+        if (mfd->panel.id == MDDI_LCD_S6D0142)
+                mddi_s6d0142_window_adjust(x1, x2, y1, y2, done_cb);
+#else
+        /* Do nothing then... except avoid lint/compiler warnings */
+        (void)x1;
+        (void)x2;
+        (void)y1;
+        (void)y2;
+        (void)done_cb;
+#endif
+}
+
+void mddi_window_adjust(struct msm_fb_data_type *mfd,
+                        uint16 x1, uint16 x2, uint16 y1, uint16 y2)
+{
+        mddi_window_adjust_ext(mfd, x1, x2, y1, y2, NULL);
+}
diff --git a/drivers/staging/msm/mddihost.h b/drivers/staging/msm/mddihost.h
new file mode 100644
index 000000000000..20b817841c4a
--- /dev/null
+++ b/drivers/staging/msm/mddihost.h
@@ -0,0 +1,225 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Code Aurora nor
+ *       the names of its contributors may be used to endorse or promote
+ *       products derived from this software without specific prior written
+ *       permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NON-INFRINGEMENT ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef MDDIHOST_H
+#define MDDIHOST_H
+
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include "linux/proc_fs.h"
+#include <linux/types.h>
+#include <linux/dma-mapping.h>
+#include <linux/clk.h>
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/types.h>
+#include <linux/dma-mapping.h>
+
+#include "msm_fb_panel.h"
+
+#undef FEATURE_MDDI_MC4
+#undef FEATURE_MDDI_S6D0142
+#undef FEATURE_MDDI_HITACHI
+#define FEATURE_MDDI_SHARP
+#define FEATURE_MDDI_TOSHIBA
+#undef FEATURE_MDDI_E751
+#define FEATURE_MDDI_CORONA
+#define FEATURE_MDDI_PRISM
+
+#define T_MSM7500
+
+typedef enum {
+        format_16bpp,
+        format_18bpp,
+        format_24bpp
+} mddi_video_format;
+
+typedef enum {
+        MDDI_LCD_NONE = 0,
+        MDDI_LCD_MC4,
+        MDDI_LCD_S6D0142,
+        MDDI_LCD_SHARP,
+        MDDI_LCD_E751,
+        MDDI_LCD_CORONA,
+        MDDI_LCD_HITACHI,
+        MDDI_LCD_TOSHIBA,
+        MDDI_LCD_PRISM,
+        MDDI_LCD_TP2,
+        MDDI_NUM_LCD_TYPES,
+        MDDI_LCD_DEFAULT = MDDI_LCD_TOSHIBA
+} mddi_lcd_type;
+
+typedef enum {
+        MDDI_HOST_PRIM = 0,
+        MDDI_HOST_EXT,
+        MDDI_NUM_HOST_CORES
+} mddi_host_type;
+
+typedef enum {
+        MDDI_DRIVER_RESET,      /* host core registers have not been written. */
+        MDDI_DRIVER_DISABLED,   /* registers written, interrupts disabled. */
+        MDDI_DRIVER_ENABLED     /* registers written, interrupts enabled. */
+} mddi_host_driver_state_type;
+
+typedef enum {
+        MDDI_GPIO_INT_0 = 0,
+        MDDI_GPIO_INT_1,
+        MDDI_GPIO_INT_2,
+        MDDI_GPIO_INT_3,
+        MDDI_GPIO_INT_4,
+        MDDI_GPIO_INT_5,
+        MDDI_GPIO_INT_6,
+        MDDI_GPIO_INT_7,
+        MDDI_GPIO_INT_8,
+        MDDI_GPIO_INT_9,
+        MDDI_GPIO_INT_10,
+        MDDI_GPIO_INT_11,
+        MDDI_GPIO_INT_12,
+        MDDI_GPIO_INT_13,
+        MDDI_GPIO_INT_14,
+        MDDI_GPIO_INT_15,
+        MDDI_GPIO_NUM_INTS
+} mddi_gpio_int_type;
+
+enum mddi_data_packet_size_type {
+        MDDI_DATA_PACKET_4_BYTES  = 4,
+        MDDI_DATA_PACKET_8_BYTES  = 8,
+        MDDI_DATA_PACKET_12_BYTES = 12,
+        MDDI_DATA_PACKET_16_BYTES = 16,
+        MDDI_DATA_PACKET_24_BYTES = 24
+};
+
+typedef struct {
+        uint32 addr;
+        uint32 value;
+} mddi_reg_write_type;
+
+boolean mddi_vsync_set_handler(msm_fb_vsync_handler_type handler, void *arg);
+
+typedef void (*mddi_llist_done_cb_type) (void);
+
+typedef void (*mddi_rev_handler_type) (void *);
+
+boolean mddi_set_rev_handler(mddi_rev_handler_type handler, uint16 pkt_type);
+
+#define MDDI_DEFAULT_PRIM_PIX_ATTR 0xC3
+#define MDDI_DEFAULT_SECD_PIX_ATTR 0xC0
+
+typedef int gpio_int_polarity_type;
+typedef int gpio_int_handler_type;
+
+typedef struct {
+        void (*vsync_detected) (boolean);
+} mddi_lcd_func_type;
+
+extern mddi_lcd_func_type mddi_lcd;
+void mddi_init(void);
+
+void mddi_powerdown(void);
+
+void mddi_host_start_ext_display(void);
+void mddi_host_stop_ext_display(void);
+
+extern spinlock_t mddi_host_spin_lock;
+#ifdef T_MSM7500
+void mddi_reset(void);
+#ifdef FEATURE_DUAL_PROC_MODEM_DISPLAY
+void mddi_host_switch_proc_control(boolean on);
+#endif
+#endif
+void mddi_host_exit_power_collapse(void);
+
+void mddi_queue_splash_screen
+    (void *buf_ptr,
+     boolean clear_area,
+     int16 src_width,
+     int16 src_starting_row,
+     int16 src_starting_column,
+     int16 num_of_rows,
+     int16 num_of_columns, int16 dst_starting_row, int16 dst_starting_column);
+
+void mddi_queue_image
+    (void *buf_ptr,
+     uint8 stereo_video,
+     boolean clear_area,
+     int16 src_width,
+     int16 src_starting_row,
+     int16 src_starting_column,
+     int16 num_of_rows,
+     int16 num_of_columns, int16 dst_starting_row, int16 dst_starting_column);
+
+int mddi_host_register_read
+    (uint32 reg_addr,
+     uint32 *reg_value_ptr, boolean wait, mddi_host_type host_idx);
+int mddi_host_register_write
+    (uint32 reg_addr, uint32 reg_val,
+     enum mddi_data_packet_size_type packet_size,
+     boolean wait, mddi_llist_done_cb_type done_cb, mddi_host_type host);
+boolean mddi_host_register_write_int
+    (uint32 reg_addr,
+     uint32 reg_val, mddi_llist_done_cb_type done_cb, mddi_host_type host);
+boolean mddi_host_register_read_int
+    (uint32 reg_addr, uint32 *reg_value_ptr, mddi_host_type host_idx);
+void mddi_queue_register_write_static
+    (uint32 reg_addr,
+     uint32 reg_val, boolean wait, mddi_llist_done_cb_type done_cb);
+void mddi_queue_static_window_adjust
+    (const mddi_reg_write_type *reg_write,
+     uint16 num_writes, mddi_llist_done_cb_type done_cb);
+
+#define mddi_queue_register_read(reg, val_ptr, wait, sig) \
+        mddi_host_register_read(reg, val_ptr, wait, MDDI_HOST_PRIM)
+#define mddi_queue_register_write(reg, val, wait, sig) \
+        mddi_host_register_write(reg, val, MDDI_DATA_PACKET_4_BYTES,\
+        wait, NULL, MDDI_HOST_PRIM)
+#define mddi_queue_register_write_extn(reg, val, pkt_size, wait, sig) \
+        mddi_host_register_write(reg, val, pkt_size, \
+        wait, NULL, MDDI_HOST_PRIM)
+#define mddi_queue_register_write_int(reg, val) \
+        mddi_host_register_write_int(reg, val, NULL, MDDI_HOST_PRIM)
+#define mddi_queue_register_read_int(reg, val_ptr) \
+        mddi_host_register_read_int(reg, val_ptr, MDDI_HOST_PRIM)
+#define mddi_queue_register_writes(reg_ptr, val, wait, sig) \
+        mddi_host_register_writes(reg_ptr, val, wait, sig, MDDI_HOST_PRIM)
+
+void mddi_wait(uint16 time_ms);
+void mddi_assign_max_pkt_dimensions(uint16 image_cols,
+                                    uint16 image_rows,
+                                    uint16 bpp,
+                                    uint16 *max_cols, uint16 * max_rows);
+uint16 mddi_assign_pkt_height(uint16 pkt_width, uint16 pkt_height, uint16 bpp);
+void mddi_queue_reverse_encapsulation(boolean wait);
+void mddi_disable(int lock);
+#endif /* MDDIHOST_H */
diff --git a/drivers/staging/msm/mddihost_e.c b/drivers/staging/msm/mddihost_e.c
new file mode 100644
index 000000000000..7de5eda71ce3
--- /dev/null
+++ b/drivers/staging/msm/mddihost_e.c
@@ -0,0 +1,63 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+
+#include "msm_fb.h"
+#include "mddihost.h"
+#include "mddihosti.h"
+
+#include <linux/clk.h>
+#include <mach/clk.h>
+
+extern struct semaphore mddi_host_mutex;
+static boolean mddi_host_ext_powered = FALSE;
+
+void mddi_host_start_ext_display(void)
+{
+        down(&mddi_host_mutex);
+
+        if (!mddi_host_ext_powered) {
+                mddi_host_init(MDDI_HOST_EXT);
+
+                mddi_host_ext_powered = TRUE;
+        }
+
+        up(&mddi_host_mutex);
+}
+
+void mddi_host_stop_ext_display(void)
+{
+        down(&mddi_host_mutex);
+
+        if (mddi_host_ext_powered) {
+                mddi_host_powerdown(MDDI_HOST_EXT);
+
+                mddi_host_ext_powered = FALSE;
+        }
+
+        up(&mddi_host_mutex);
+}
diff --git a/drivers/staging/msm/mddihosti.c b/drivers/staging/msm/mddihosti.c
new file mode 100644
index 000000000000..f9d6e91e8d5d
--- /dev/null
+++ b/drivers/staging/msm/mddihosti.c
@@ -0,0 +1,2239 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+
+#include "msm_fb_panel.h"
+#include "mddihost.h"
+#include "mddihosti.h"
+
+#define FEATURE_MDDI_UNDERRUN_RECOVERY
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+static void mddi_read_rev_packet(byte *data_ptr);
+#endif
+
+struct timer_list mddi_host_timer;
+
+#define MDDI_DEFAULT_TIMER_LENGTH 5000  /* 5 seconds */
+uint32 mddi_rtd_frequency = 60000;      /* send RTD every 60 seconds */
+uint32 mddi_client_status_frequency = 60000;    /* get status pkt every 60 secs */
+
+boolean mddi_vsync_detect_enabled = FALSE;
+mddi_gpio_info_type mddi_gpio;
+
+uint32 mddi_host_core_version;
+boolean mddi_debug_log_statistics = FALSE;
+/* #define FEATURE_MDDI_HOST_ENABLE_EARLY_HIBERNATION */
+/* default to TRUE in case MDP does not vote */
+static boolean mddi_host_mdp_active_flag = TRUE;
+static uint32 mddi_log_stats_counter;
+uint32 mddi_log_stats_frequency = 4000;
+
+#define MDDI_DEFAULT_REV_PKT_SIZE            0x20
+
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+static boolean mddi_rev_ptr_workaround = TRUE;
+static uint32 mddi_reg_read_retry;
+static uint32 mddi_reg_read_retry_max = 20;
+static boolean mddi_enable_reg_read_retry = TRUE;
+static boolean mddi_enable_reg_read_retry_once = FALSE;
+
+#define MDDI_MAX_REV_PKT_SIZE                0x60
+
+#define MDDI_CLIENT_CAPABILITY_REV_PKT_SIZE  0x60
+
+#define MDDI_VIDEO_REV_PKT_SIZE              0x40
+#define MDDI_REV_BUFFER_SIZE  MDDI_MAX_REV_PKT_SIZE
+static byte rev_packet_data[MDDI_MAX_REV_PKT_SIZE];
+#endif /* FEATURE_MDDI_DISABLE_REVERSE */
+/* leave these variables so graphics will compile */
+
+#define MDDI_MAX_REV_DATA_SIZE  128
+/*lint -d__align(x) */
+boolean mddi_debug_clear_rev_data = TRUE;
+
+uint32 *mddi_reg_read_value_ptr;
+
+mddi_client_capability_type mddi_client_capability_pkt;
+static boolean mddi_client_capability_request = FALSE;
+
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+
+#define MAX_MDDI_REV_HANDLERS 2
+#define INVALID_PKT_TYPE 0xFFFF
+
+typedef struct {
+        mddi_rev_handler_type handler;  /* ISR to be executed */
+        uint16 pkt_type;
+} mddi_rev_pkt_handler_type;
+static mddi_rev_pkt_handler_type mddi_rev_pkt_handler[MAX_MDDI_REV_HANDLERS] =
+    { {NULL, INVALID_PKT_TYPE}, {NULL, INVALID_PKT_TYPE} };
+
+static boolean mddi_rev_encap_user_request = FALSE;
+static mddi_linked_list_notify_type mddi_rev_user;
+
+spinlock_t mddi_host_spin_lock;
+extern uint32 mdp_in_processing;
+#endif
+
+typedef enum {
+        MDDI_REV_IDLE
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+            , MDDI_REV_REG_READ_ISSUED,
+        MDDI_REV_REG_READ_SENT,
+        MDDI_REV_ENCAP_ISSUED,
+        MDDI_REV_STATUS_REQ_ISSUED,
+        MDDI_REV_CLIENT_CAP_ISSUED
+#endif
+} mddi_rev_link_state_type;
+
+typedef enum {
+        MDDI_LINK_DISABLED,
+        MDDI_LINK_HIBERNATING,
+        MDDI_LINK_ACTIVATING,
+        MDDI_LINK_ACTIVE
+} mddi_host_link_state_type;
+
+typedef struct {
+        uint32 count;
+        uint32 in_count;
+        uint32 disp_req_count;
+        uint32 state_change_count;
+        uint32 ll_done_count;
+        uint32 rev_avail_count;
+        uint32 error_count;
+        uint32 rev_encap_count;
+        uint32 llist_ptr_write_1;
+        uint32 llist_ptr_write_2;
+} mddi_host_int_type;
+
+typedef struct {
+        uint32 fwd_crc_count;
+        uint32 rev_crc_count;
+        uint32 pri_underflow;
+        uint32 sec_underflow;
+        uint32 rev_overflow;
+        uint32 pri_overwrite;
+        uint32 sec_overwrite;
+        uint32 rev_overwrite;
+        uint32 dma_failure;
+        uint32 rtd_failure;
+        uint32 reg_read_failure;
+#ifdef FEATURE_MDDI_UNDERRUN_RECOVERY
+        uint32 pri_underrun_detected;
+#endif
+} mddi_host_stat_type;
+
+typedef struct {
+        uint32 rtd_cnt;
+        uint32 rev_enc_cnt;
+        uint32 vid_cnt;
+        uint32 reg_acc_cnt;
+        uint32 cli_stat_cnt;
+        uint32 cli_cap_cnt;
+        uint32 reg_read_cnt;
+        uint32 link_active_cnt;
+        uint32 link_hibernate_cnt;
+        uint32 vsync_response_cnt;
+        uint32 fwd_crc_cnt;
+        uint32 rev_crc_cnt;
+} mddi_log_params_struct_type;
+
+typedef struct {
+        uint32 rtd_value;
+        uint32 rtd_counter;
+        uint32 client_status_cnt;
+        boolean rev_ptr_written;
+        uint8 *rev_ptr_start;
+        uint8 *rev_ptr_curr;
+        uint32 mddi_rev_ptr_write_val;
+        dma_addr_t rev_data_dma_addr;
+        uint16 rev_pkt_size;
+        mddi_rev_link_state_type rev_state;
+        mddi_host_link_state_type link_state;
+        mddi_host_driver_state_type driver_state;
+        boolean disable_hibernation;
+        uint32 saved_int_reg;
+        uint32 saved_int_en;
+        mddi_linked_list_type *llist_ptr;
+        dma_addr_t llist_dma_addr;
+        mddi_linked_list_type *llist_dma_ptr;
+        uint32 *rev_data_buf;
+        struct completion mddi_llist_avail_comp;
+        boolean mddi_waiting_for_llist_avail;
+        mddi_host_int_type int_type;
+        mddi_host_stat_type stats;
+        mddi_log_params_struct_type log_parms;
+        mddi_llist_info_type llist_info;
+        mddi_linked_list_notify_type llist_notify[MDDI_MAX_NUM_LLIST_ITEMS];
+} mddi_host_cntl_type;
+
+static mddi_host_type mddi_curr_host = MDDI_HOST_PRIM;
+static mddi_host_cntl_type mhctl[MDDI_NUM_HOST_CORES];
+mddi_linked_list_type *llist_extern[MDDI_NUM_HOST_CORES];
+mddi_linked_list_type *llist_dma_extern[MDDI_NUM_HOST_CORES];
+mddi_linked_list_notify_type *llist_extern_notify[MDDI_NUM_HOST_CORES];
+static mddi_log_params_struct_type prev_parms[MDDI_NUM_HOST_CORES];
+
+extern uint32 mdp_total_vdopkts;
+
+static boolean mddi_host_io_clock_on = FALSE;
+static boolean mddi_host_hclk_on = FALSE;
+
+int int_mddi_pri_flag = FALSE;
+int int_mddi_ext_flag = FALSE;
+
+static void mddi_report_errors(uint32 int_reg)
+{
+        mddi_host_type host_idx = mddi_curr_host;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        if (int_reg & MDDI_INT_PRI_UNDERFLOW) {
+                pmhctl->stats.pri_underflow++;
+                MDDI_MSG_ERR("!!! MDDI Primary Underflow !!!\n");
+        }
+        if (int_reg & MDDI_INT_SEC_UNDERFLOW) {
+                pmhctl->stats.sec_underflow++;
+                MDDI_MSG_ERR("!!! MDDI Secondary Underflow !!!\n");
+        }
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+        if (int_reg & MDDI_INT_REV_OVERFLOW) {
+                pmhctl->stats.rev_overflow++;
+                MDDI_MSG_ERR("!!! MDDI Reverse Overflow !!!\n");
+                pmhctl->rev_ptr_curr = pmhctl->rev_ptr_start;
+                mddi_host_reg_out(REV_PTR, pmhctl->mddi_rev_ptr_write_val);
+
+        }
+        if (int_reg & MDDI_INT_CRC_ERROR)
+                MDDI_MSG_ERR("!!! MDDI Reverse CRC Error !!!\n");
+#endif
+        if (int_reg & MDDI_INT_PRI_OVERWRITE) {
+                pmhctl->stats.pri_overwrite++;
+                MDDI_MSG_ERR("!!! MDDI Primary Overwrite !!!\n");
+        }
+        if (int_reg & MDDI_INT_SEC_OVERWRITE) {
+                pmhctl->stats.sec_overwrite++;
+                MDDI_MSG_ERR("!!! MDDI Secondary Overwrite !!!\n");
+        }
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+        if (int_reg & MDDI_INT_REV_OVERWRITE) {
+                pmhctl->stats.rev_overwrite++;
+                /* This will show up normally and is not a problem */
+                MDDI_MSG_DEBUG("MDDI Reverse Overwrite!\n");
+        }
+        if (int_reg & MDDI_INT_RTD_FAILURE) {
+                mddi_host_reg_outm(INTEN, MDDI_INT_RTD_FAILURE, 0);
+                pmhctl->stats.rtd_failure++;
+                MDDI_MSG_ERR("!!! MDDI RTD Failure !!!\n");
+        }
+#endif
+        if (int_reg & MDDI_INT_DMA_FAILURE) {
+                pmhctl->stats.dma_failure++;
+                MDDI_MSG_ERR("!!! MDDI DMA Abort !!!\n");
+        }
+}
+
+static void mddi_host_enable_io_clock(void)
+{
+        if (!MDDI_HOST_IS_IO_CLOCK_ON)
+                MDDI_HOST_ENABLE_IO_CLOCK;
+}
+
+static void mddi_host_enable_hclk(void)
+{
+
+        if (!MDDI_HOST_IS_HCLK_ON)
+                MDDI_HOST_ENABLE_HCLK;
+}
+
+static void mddi_host_disable_io_clock(void)
+{
+#ifndef FEATURE_MDDI_HOST_IO_CLOCK_CONTROL_DISABLE
+        if (MDDI_HOST_IS_IO_CLOCK_ON)
+                MDDI_HOST_DISABLE_IO_CLOCK;
+#endif
+}
+
+static void mddi_host_disable_hclk(void)
+{
+#ifndef FEATURE_MDDI_HOST_HCLK_CONTROL_DISABLE
+        if (MDDI_HOST_IS_HCLK_ON)
+                MDDI_HOST_DISABLE_HCLK;
+#endif
+}
+
+static void mddi_vote_to_sleep(mddi_host_type host_idx, boolean sleep)
+{
+        uint16 vote_mask;
+
+        if (host_idx == MDDI_HOST_PRIM)
+                vote_mask = 0x01;
+        else
+                vote_mask = 0x02;
+}
+
+static void mddi_report_state_change(uint32 int_reg)
+{
+        mddi_host_type host_idx = mddi_curr_host;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        if ((pmhctl->saved_int_reg & MDDI_INT_IN_HIBERNATION) &&
+            (pmhctl->saved_int_reg & MDDI_INT_LINK_ACTIVE)) {
+                /* recover from condition where the io_clock was turned off by the
+                   clock driver during a transition to hibernation. The io_clock
+                   disable is to prevent MDP/MDDI underruns when changing ARM
+                   clock speeds. In the process of halting the ARM, the hclk
+                   divider needs to be set to 1. When it is set to 1, there is
+                   a small time (usecs) when hclk is off or slow, and this can
+                   cause an underrun. To prevent the underrun, clock driver turns
+                   off the MDDI io_clock before making the change. */
+                mddi_host_reg_out(CMD, MDDI_CMD_POWERUP);
+        }
+
+        if (int_reg & MDDI_INT_LINK_ACTIVE) {
+                pmhctl->link_state = MDDI_LINK_ACTIVE;
+                pmhctl->log_parms.link_active_cnt++;
+                pmhctl->rtd_value = mddi_host_reg_in(RTD_VAL);
+                MDDI_MSG_DEBUG("!!! MDDI Active RTD:0x%x!!!\n",
+                               pmhctl->rtd_value);
+                /* now interrupt on hibernation */
+                mddi_host_reg_outm(INTEN,
+                                   (MDDI_INT_IN_HIBERNATION |
+                                    MDDI_INT_LINK_ACTIVE),
+                                   MDDI_INT_IN_HIBERNATION);
+
+#ifdef DEBUG_MDDIHOSTI
+                /* if gpio interrupt is enabled, start polling at fastest
+                 * registered rate
+                 */
+                if (mddi_gpio.polling_enabled) {
+                        timer_reg(&mddi_gpio_poll_timer,
+                mddi_gpio_poll_timer_cb, 0, mddi_gpio.polling_interval, 0);
+                }
+#endif
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+                if (mddi_rev_ptr_workaround) {
+                        /* HW CR: need to reset reverse register stuff */
+                        pmhctl->rev_ptr_written = FALSE;
+                        pmhctl->rev_ptr_curr = pmhctl->rev_ptr_start;
+                }
+#endif
+                /* vote on sleep */
+                mddi_vote_to_sleep(host_idx, FALSE);
+
+                if (host_idx == MDDI_HOST_PRIM) {
+                        if (mddi_vsync_detect_enabled) {
+                                /*
+                                 * Indicate to client specific code that vsync
+                                 * was enabled, but we did not detect a client
+                                 * intiated wakeup. The client specific
+                                 * handler can either reassert vsync detection,
+                                 * or treat this as a valid vsync.
+                                 */
+                                mddi_client_lcd_vsync_detected(FALSE);
+                                pmhctl->log_parms.vsync_response_cnt++;
+                        }
+                }
+        }
+        if (int_reg & MDDI_INT_IN_HIBERNATION) {
+                pmhctl->link_state = MDDI_LINK_HIBERNATING;
+                pmhctl->log_parms.link_hibernate_cnt++;
+                MDDI_MSG_DEBUG("!!! MDDI Hibernating !!!\n");
+                /* now interrupt on link_active */
+#ifdef FEATURE_MDDI_DISABLE_REVERSE
+                mddi_host_reg_outm(INTEN,
+                                   (MDDI_INT_MDDI_IN |
+                                    MDDI_INT_IN_HIBERNATION |
+                                    MDDI_INT_LINK_ACTIVE),
+                                   MDDI_INT_LINK_ACTIVE);
+#else
+                mddi_host_reg_outm(INTEN,
+                                   (MDDI_INT_MDDI_IN |
+                                    MDDI_INT_IN_HIBERNATION |
+                                    MDDI_INT_LINK_ACTIVE),
+                                   (MDDI_INT_MDDI_IN | MDDI_INT_LINK_ACTIVE));
+
+                pmhctl->rtd_counter = mddi_rtd_frequency;
+
+                if (pmhctl->rev_state != MDDI_REV_IDLE) {
+                        /* a rev_encap will not wake up the link, so we do that here */
+                        pmhctl->link_state = MDDI_LINK_ACTIVATING;
+                        mddi_host_reg_out(CMD, MDDI_CMD_LINK_ACTIVE);
+                }
+#endif
+
+                if (pmhctl->disable_hibernation) {
+                        mddi_host_reg_out(CMD, MDDI_CMD_HIBERNATE);
+                        mddi_host_reg_out(CMD, MDDI_CMD_LINK_ACTIVE);
+                        pmhctl->link_state = MDDI_LINK_ACTIVATING;
+                }
+#ifdef FEATURE_MDDI_UNDERRUN_RECOVERY
+                if ((pmhctl->llist_info.transmitting_start_idx !=
+                     UNASSIGNED_INDEX)
+                    &&
+                    ((pmhctl->
+                      saved_int_reg & (MDDI_INT_PRI_LINK_LIST_DONE |
+                                       MDDI_INT_PRI_PTR_READ)) ==
+                     MDDI_INT_PRI_PTR_READ)) {
+                        mddi_linked_list_type *llist_dma;
+                        llist_dma = pmhctl->llist_dma_ptr;
+                        /*
+                         * All indications are that we have not received a
+                         * linked list done interrupt, due to an underrun
+                         * condition. Recovery attempt is to send again.
+                         */
+                        dma_coherent_pre_ops();
+                        /* Write to primary pointer register again */
+                        mddi_host_reg_out(PRI_PTR,
+                                          &llist_dma[pmhctl->llist_info.
+                                                     transmitting_start_idx]);
+                        pmhctl->stats.pri_underrun_detected++;
+                }
+#endif
+
+                /* vote on sleep */
+                if (pmhctl->link_state == MDDI_LINK_HIBERNATING) {
+                        mddi_vote_to_sleep(host_idx, TRUE);
+                }
+
+#ifdef DEBUG_MDDIHOSTI
+                /* need to stop polling timer */
+                if (mddi_gpio.polling_enabled) {
+                        (void) timer_clr(&mddi_gpio_poll_timer, T_NONE);
+                }
+#endif
+        }
+}
+
+void mddi_host_timer_service(unsigned long data)
+{
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+        unsigned long flags;
+#endif
+        mddi_host_type host_idx;
+        mddi_host_cntl_type *pmhctl;
+
+        unsigned long time_ms = MDDI_DEFAULT_TIMER_LENGTH;
+        init_timer(&mddi_host_timer);
+        mddi_host_timer.function = mddi_host_timer_service;
+        mddi_host_timer.data = 0;
+
+        mddi_host_timer.expires = jiffies + ((time_ms * HZ) / 1000);
+        add_timer(&mddi_host_timer);
+
+        for (host_idx = MDDI_HOST_PRIM; host_idx < MDDI_NUM_HOST_CORES;
+             host_idx++) {
+                pmhctl = &(mhctl[host_idx]);
+                mddi_log_stats_counter += (uint32) time_ms;
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+                pmhctl->rtd_counter += (uint32) time_ms;
+                pmhctl->client_status_cnt += (uint32) time_ms;
+
+                if (host_idx == MDDI_HOST_PRIM) {
+                        if (pmhctl->client_status_cnt >=
+                            mddi_client_status_frequency) {
+                                if ((pmhctl->link_state ==
+                                     MDDI_LINK_HIBERNATING)
+                                    && (pmhctl->client_status_cnt >
+                                        mddi_client_status_frequency)) {
+                                        /*
+                                         * special case where we are hibernating
+                                         * and mddi_host_isr is not firing, so
+                                         * kick the link so that the status can
+                                         * be retrieved
+                                         */
+
+                                        /* need to wake up link before issuing
+                                         * rev encap command
+                                         */
+                                        MDDI_MSG_INFO("wake up link!\n");
+                                        spin_lock_irqsave(&mddi_host_spin_lock,
+                                                          flags);
+                                        mddi_host_enable_hclk();
+                                        mddi_host_enable_io_clock();
+                                        pmhctl->link_state =
+                                            MDDI_LINK_ACTIVATING;
+                                        mddi_host_reg_out(CMD,
+                                                          MDDI_CMD_LINK_ACTIVE);
+                                        spin_unlock_irqrestore
+                                            (&mddi_host_spin_lock, flags);
+                                } else
+                                    if ((pmhctl->link_state == MDDI_LINK_ACTIVE)
+                                        && pmhctl->disable_hibernation) {
+                                        /*
+                                         * special case where we have disabled
+                                         * hibernation and mddi_host_isr
+                                         * is not firing, so enable interrupt
+                                         * for no pkts pending, which will
+                                         * generate an interrupt
+                                         */
+                                        MDDI_MSG_INFO("kick isr!\n");
+                                        spin_lock_irqsave(&mddi_host_spin_lock,
+                                                          flags);
+                                        mddi_host_enable_hclk();
+                                        mddi_host_reg_outm(INTEN,
+                                                           MDDI_INT_NO_CMD_PKTS_PEND,
+                                                           MDDI_INT_NO_CMD_PKTS_PEND);
+                                        spin_unlock_irqrestore
+                                            (&mddi_host_spin_lock, flags);
+                                }
+                        }
+                }
+#endif /* #ifndef FEATURE_MDDI_DISABLE_REVERSE */
+        }
+
+        /* Check if logging is turned on */
+        for (host_idx = MDDI_HOST_PRIM; host_idx < MDDI_NUM_HOST_CORES;
+             host_idx++) {
+                mddi_log_params_struct_type *prev_ptr = &(prev_parms[host_idx]);
+                pmhctl = &(mhctl[host_idx]);
+
+                if (mddi_debug_log_statistics) {
+
+                        /* get video pkt count from MDP, since MDDI sw cannot know this */
+                        pmhctl->log_parms.vid_cnt = mdp_total_vdopkts;
+
+                        if (mddi_log_stats_counter >= mddi_log_stats_frequency) {
+                                /* mddi_log_stats_counter = 0; */
+                                if (mddi_debug_log_statistics) {
+                                        MDDI_MSG_NOTICE
+                                            ("MDDI Statistics since last report:\n");
+                                        MDDI_MSG_NOTICE("  Packets sent:\n");
+                                        MDDI_MSG_NOTICE
+                                            ("    %d RTD packet(s)\n",
+                                             pmhctl->log_parms.rtd_cnt -
+                                             prev_ptr->rtd_cnt);
+                                        if (prev_ptr->rtd_cnt !=
+                                            pmhctl->log_parms.rtd_cnt) {
+                                                unsigned long flags;
+                                                spin_lock_irqsave
+                                                    (&mddi_host_spin_lock,
+                                                     flags);
+                                                mddi_host_enable_hclk();
+                                                pmhctl->rtd_value =
+                                                    mddi_host_reg_in(RTD_VAL);
+                                                spin_unlock_irqrestore
+                                                    (&mddi_host_spin_lock,
+                                                     flags);
+                                                MDDI_MSG_NOTICE
+                                                    ("      RTD value=%d\n",
+                                                     pmhctl->rtd_value);
+                                        }
+                                        MDDI_MSG_NOTICE
+                                            ("    %d VIDEO packets\n",
+                                             pmhctl->log_parms.vid_cnt -
+                                             prev_ptr->vid_cnt);
+                                        MDDI_MSG_NOTICE
+                                            ("    %d Register Access packets\n",
+                                             pmhctl->log_parms.reg_acc_cnt -
+                                             prev_ptr->reg_acc_cnt);
+                                        MDDI_MSG_NOTICE
+                                            ("    %d Reverse Encapsulation packet(s)\n",
+                                             pmhctl->log_parms.rev_enc_cnt -
+                                             prev_ptr->rev_enc_cnt);
+                                        if (prev_ptr->rev_enc_cnt !=
+                                            pmhctl->log_parms.rev_enc_cnt) {
+                                                /* report # of reverse CRC errors */
+                                                MDDI_MSG_NOTICE
+                                                    ("      %d reverse CRC errors detected\n",
+                                                     pmhctl->log_parms.
+                                                     rev_crc_cnt -
+                                                     prev_ptr->rev_crc_cnt);
+                                        }
+                                        MDDI_MSG_NOTICE
+                                            ("  Packets received:\n");
+                                        MDDI_MSG_NOTICE
+                                            ("    %d Client Status packets",
+                                             pmhctl->log_parms.cli_stat_cnt -
+                                             prev_ptr->cli_stat_cnt);
+                                        if (prev_ptr->cli_stat_cnt !=
+                                            pmhctl->log_parms.cli_stat_cnt) {
+                                                MDDI_MSG_NOTICE
+                                                    ("      %d forward CRC errors reported\n",
+                                                     pmhctl->log_parms.
+                                                     fwd_crc_cnt -
+                                                     prev_ptr->fwd_crc_cnt);
+                                        }
+                                        MDDI_MSG_NOTICE
+                                            ("    %d Register Access Read packets\n",
+                                             pmhctl->log_parms.reg_read_cnt -
+                                             prev_ptr->reg_read_cnt);
+
+                                        if (pmhctl->link_state ==
+                                            MDDI_LINK_ACTIVE) {
+                                                MDDI_MSG_NOTICE
+                                                    ("  Current Link Status: Active\n");
+                                        } else
+                                            if ((pmhctl->link_state ==
+                                                 MDDI_LINK_HIBERNATING)
+                                                || (pmhctl->link_state ==
+                                                    MDDI_LINK_ACTIVATING)) {
+                                                MDDI_MSG_NOTICE
+                                                    ("  Current Link Status: Hibernation\n");
+                                        } else {
+                                                MDDI_MSG_NOTICE
+                                                    ("  Current Link Status: Inactive\n");
+                                        }
+                                        MDDI_MSG_NOTICE
+                                            ("    Active state entered %d times\n",
+                                             pmhctl->log_parms.link_active_cnt -
+                                             prev_ptr->link_active_cnt);
+                                        MDDI_MSG_NOTICE
+                                            ("    Hibernation state entered %d times\n",
+                                             pmhctl->log_parms.
+                                             link_hibernate_cnt -
+                                             prev_ptr->link_hibernate_cnt);
+                                }
+                        }
+                        prev_parms[host_idx] = pmhctl->log_parms;
+                }
+        }
+        if (mddi_log_stats_counter >= mddi_log_stats_frequency)
+                mddi_log_stats_counter = 0;
+
+        return;
+}                               /* mddi_host_timer_cb */
+
+static void mddi_process_link_list_done(void)
+{
+        mddi_host_type host_idx = mddi_curr_host;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        /* normal forward linked list packet(s) were sent */
+        if (pmhctl->llist_info.transmitting_start_idx == UNASSIGNED_INDEX) {
+                MDDI_MSG_ERR("**** getting LL done, but no list ****\n");
+        } else {
+                uint16 idx;
+
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+                if (pmhctl->rev_state == MDDI_REV_REG_READ_ISSUED) {
+                        /* special case where a register read packet was sent */
+                        pmhctl->rev_state = MDDI_REV_REG_READ_SENT;
+                        if (pmhctl->llist_info.reg_read_idx == UNASSIGNED_INDEX) {
+                                MDDI_MSG_ERR
+                                    ("**** getting LL done, but no list ****\n");
+                        }
+                }
+#endif
+                for (idx = pmhctl->llist_info.transmitting_start_idx;;) {
+                        uint16 next_idx = pmhctl->llist_notify[idx].next_idx;
+                        /* with reg read we don't release the waiting tcb until after
+                         * the reverse encapsulation has completed.
+                         */
+                        if (idx != pmhctl->llist_info.reg_read_idx) {
+                                /* notify task that may be waiting on this completion */
+                                if (pmhctl->llist_notify[idx].waiting) {
+                                        complete(&
+                                                 (pmhctl->llist_notify[idx].
+                                                  done_comp));
+                                }
+                                if (pmhctl->llist_notify[idx].done_cb != NULL) {
+                                        (*(pmhctl->llist_notify[idx].done_cb))
+                                            ();
+                                }
+
+                                pmhctl->llist_notify[idx].in_use = FALSE;
+                                pmhctl->llist_notify[idx].waiting = FALSE;
+                                pmhctl->llist_notify[idx].done_cb = NULL;
+                                if (idx < MDDI_NUM_DYNAMIC_LLIST_ITEMS) {
+                                        /* static LLIST items are configured only once */
+                                        pmhctl->llist_notify[idx].next_idx =
+                                            UNASSIGNED_INDEX;
+                                }
+                                /*
+                                 * currently, all linked list packets are
+                                 * register access, so we can increment the
+                                 * counter for that packet type here.
+                                 */
+                                pmhctl->log_parms.reg_acc_cnt++;
+                        }
+                        if (idx == pmhctl->llist_info.transmitting_end_idx)
+                                break;
+                        idx = next_idx;
+                        if (idx == UNASSIGNED_INDEX)
+                                MDDI_MSG_CRIT("MDDI linked list corruption!\n");
+                }
+
+                pmhctl->llist_info.transmitting_start_idx = UNASSIGNED_INDEX;
+                pmhctl->llist_info.transmitting_end_idx = UNASSIGNED_INDEX;
+
+                if (pmhctl->mddi_waiting_for_llist_avail) {
+                        if (!
+                            (pmhctl->
+                             llist_notify[pmhctl->llist_info.next_free_idx].
+                             in_use)) {
+                                pmhctl->mddi_waiting_for_llist_avail = FALSE;
+                                complete(&(pmhctl->mddi_llist_avail_comp));
+                        }
+                }
+        }
+
+        /* Turn off MDDI_INT_PRI_LINK_LIST_DONE interrupt */
+        mddi_host_reg_outm(INTEN, MDDI_INT_PRI_LINK_LIST_DONE, 0);
+
+}
+
+static void mddi_queue_forward_linked_list(void)
+{
+        uint16 first_pkt_index;
+        mddi_linked_list_type *llist_dma;
+        mddi_host_type host_idx = mddi_curr_host;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+        llist_dma = pmhctl->llist_dma_ptr;
+
+        first_pkt_index = UNASSIGNED_INDEX;
+
+        if (pmhctl->llist_info.transmitting_start_idx == UNASSIGNED_INDEX) {
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+                if (pmhctl->llist_info.reg_read_waiting) {
+                        if (pmhctl->rev_state == MDDI_REV_IDLE) {
+                                /*
+                                 * we have a register read to send and
+                                 * can send it now
+                                 */
+                                pmhctl->rev_state = MDDI_REV_REG_READ_ISSUED;
+                                mddi_reg_read_retry = 0;
+                                first_pkt_index =
+                                    pmhctl->llist_info.waiting_start_idx;
+                                pmhctl->llist_info.reg_read_waiting = FALSE;
+                        }
+                } else
+#endif
+                {
+                        /*
+                         * not register read to worry about, go ahead and write
+                         * anything that may be on the waiting list.
+                         */
+                        first_pkt_index = pmhctl->llist_info.waiting_start_idx;
+                }
+        }
+
+        if (first_pkt_index != UNASSIGNED_INDEX) {
+                pmhctl->llist_info.transmitting_start_idx =
+                    pmhctl->llist_info.waiting_start_idx;
+                pmhctl->llist_info.transmitting_end_idx =
+                    pmhctl->llist_info.waiting_end_idx;
+                pmhctl->llist_info.waiting_start_idx = UNASSIGNED_INDEX;
+                pmhctl->llist_info.waiting_end_idx = UNASSIGNED_INDEX;
+
+                /* write to the primary pointer register */
+                MDDI_MSG_DEBUG("MDDI writing primary ptr with idx=%d\n",
+                               first_pkt_index);
+
+                pmhctl->int_type.llist_ptr_write_2++;
+
+                dma_coherent_pre_ops();
+                mddi_host_reg_out(PRI_PTR, &llist_dma[first_pkt_index]);
+
+                /* enable interrupt when complete */
+                mddi_host_reg_outm(INTEN, MDDI_INT_PRI_LINK_LIST_DONE,
+                                   MDDI_INT_PRI_LINK_LIST_DONE);
+
+        }
+
+}
+
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+static void mddi_read_rev_packet(byte *data_ptr)
+{
+        uint16 i, length;
+        mddi_host_type host_idx = mddi_curr_host;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        uint8 *rev_ptr_overflow =
+            (pmhctl->rev_ptr_start + MDDI_REV_BUFFER_SIZE);
+
+        /* first determine the length and handle invalid lengths */
+        length = *pmhctl->rev_ptr_curr++;
+        if (pmhctl->rev_ptr_curr >= rev_ptr_overflow)
+                pmhctl->rev_ptr_curr = pmhctl->rev_ptr_start;
+        length |= ((*pmhctl->rev_ptr_curr++) << 8);
+        if (pmhctl->rev_ptr_curr >= rev_ptr_overflow)
+                pmhctl->rev_ptr_curr = pmhctl->rev_ptr_start;
+        if (length > (pmhctl->rev_pkt_size - 2)) {
+                MDDI_MSG_ERR("Invalid rev pkt length %d\n", length);
+                /* rev_pkt_size should always be <= rev_ptr_size so limit to packet size */
+                length = pmhctl->rev_pkt_size - 2;
+        }
+
+        /* If the data pointer is NULL, just increment the pmhctl->rev_ptr_curr.
+         * Loop around if necessary. Don't bother reading the data.
+         */
+        if (data_ptr == NULL) {
+                pmhctl->rev_ptr_curr += length;
+                if (pmhctl->rev_ptr_curr >= rev_ptr_overflow)
+                        pmhctl->rev_ptr_curr -= MDDI_REV_BUFFER_SIZE;
+                return;
+        }
+
+        data_ptr[0] = length & 0x0ff;
+        data_ptr[1] = length >> 8;
+        data_ptr += 2;
+        /* copy the data to data_ptr byte-at-a-time */
+        for (i = 0; (i < length) && (pmhctl->rev_ptr_curr < rev_ptr_overflow);
+             i++)
+                *data_ptr++ = *pmhctl->rev_ptr_curr++;
+        if (pmhctl->rev_ptr_curr >= rev_ptr_overflow)
+                pmhctl->rev_ptr_curr = pmhctl->rev_ptr_start;
+        for (; (i < length) && (pmhctl->rev_ptr_curr < rev_ptr_overflow); i++)
+                *data_ptr++ = *pmhctl->rev_ptr_curr++;
+}
+
+static void mddi_process_rev_packets(void)
+{
+        uint32 rev_packet_count;
+        word i;
+        uint32 crc_errors;
+        boolean mddi_reg_read_successful = FALSE;
+        mddi_host_type host_idx = mddi_curr_host;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        pmhctl->log_parms.rev_enc_cnt++;
+        if ((pmhctl->rev_state != MDDI_REV_ENCAP_ISSUED) &&
+            (pmhctl->rev_state != MDDI_REV_STATUS_REQ_ISSUED) &&
+            (pmhctl->rev_state != MDDI_REV_CLIENT_CAP_ISSUED)) {
+                MDDI_MSG_ERR("Wrong state %d for reverse int\n",
+                             pmhctl->rev_state);
+        }
+        /* Turn off MDDI_INT_REV_AVAIL interrupt */
+        mddi_host_reg_outm(INTEN, MDDI_INT_REV_DATA_AVAIL, 0);
+
+        /* Clear rev data avail int */
+        mddi_host_reg_out(INT, MDDI_INT_REV_DATA_AVAIL);
+
+        /* Get Number of packets */
+        rev_packet_count = mddi_host_reg_in(REV_PKT_CNT);
+
+#ifndef T_MSM7500
+        /* Clear out rev packet counter */
+        mddi_host_reg_out(REV_PKT_CNT, 0x0000);
+#endif
+
+#if defined(CONFIG_FB_MSM_MDP31) || defined(CONFIG_FB_MSM_MDP40)
+        if ((pmhctl->rev_state == MDDI_REV_CLIENT_CAP_ISSUED) &&
+            (rev_packet_count > 0) &&
+            (mddi_host_core_version == 0x28 ||
+             mddi_host_core_version == 0x30)) {
+
+                uint32 int_reg;
+                uint32 max_count = 0;
+
+                mddi_host_reg_out(REV_PTR, pmhctl->mddi_rev_ptr_write_val);
+                int_reg = mddi_host_reg_in(INT);
+                while ((int_reg & 0x100000) == 0) {
+                        udelay(3);
+                        int_reg = mddi_host_reg_in(INT);
+                        if (++max_count > 100)
+                                break;
+                }
+        }
+#endif
+
+        /* Get CRC error count */
+        crc_errors = mddi_host_reg_in(REV_CRC_ERR);
+        if (crc_errors != 0) {
+                pmhctl->log_parms.rev_crc_cnt += crc_errors;
+                pmhctl->stats.rev_crc_count += crc_errors;
+                MDDI_MSG_ERR("!!! MDDI %d Reverse CRC Error(s) !!!\n",
+                             crc_errors);
+#ifndef T_MSM7500
+                /* Clear CRC error count */
+                mddi_host_reg_out(REV_CRC_ERR, 0x0000);
+#endif
+                /* also issue an RTD to attempt recovery */
+                pmhctl->rtd_counter = mddi_rtd_frequency;
+        }
+
+        pmhctl->rtd_value = mddi_host_reg_in(RTD_VAL);
+
+        MDDI_MSG_DEBUG("MDDI rev pkt cnt=%d, ptr=0x%x, RTD:0x%x\n",
+                       rev_packet_count,
+                       pmhctl->rev_ptr_curr - pmhctl->rev_ptr_start,
+                       pmhctl->rtd_value);
+
+        if (rev_packet_count >= 1) {
+                mddi_invalidate_cache_lines((uint32 *) pmhctl->rev_ptr_start,
+                                            MDDI_REV_BUFFER_SIZE);
+        }
+        /* order the reads */
+        dma_coherent_post_ops();
+        for (i = 0; i < rev_packet_count; i++) {
+                mddi_rev_packet_type *rev_pkt_ptr;
+
+                mddi_read_rev_packet(rev_packet_data);
+
+                rev_pkt_ptr = (mddi_rev_packet_type *) rev_packet_data;
+
+                if (rev_pkt_ptr->packet_length > pmhctl->rev_pkt_size) {
+                        MDDI_MSG_ERR("!!!invalid packet size: %d\n",
+                                     rev_pkt_ptr->packet_length);
+                }
+
+                MDDI_MSG_DEBUG("MDDI rev pkt 0x%x size 0x%x\n",
+                               rev_pkt_ptr->packet_type,
+                               rev_pkt_ptr->packet_length);
+
+                /* Do whatever you want to do with the data based on the packet type */
+                switch (rev_pkt_ptr->packet_type) {
+                case 66:        /* Client Capability */
+                        {
+                                mddi_client_capability_type
+                                    *client_capability_pkt_ptr;
+
+                                client_capability_pkt_ptr =
+                                    (mddi_client_capability_type *)
+                                    rev_packet_data;
+                                MDDI_MSG_NOTICE
+                                    ("Client Capability: Week=%d, Year=%d\n",
+                                     client_capability_pkt_ptr->
+                                     Week_of_Manufacture,
+                                     client_capability_pkt_ptr->
+                                     Year_of_Manufacture);
+                                memcpy((void *)&mddi_client_capability_pkt,
+                                       (void *)rev_packet_data,
+                                       sizeof(mddi_client_capability_type));
+                                pmhctl->log_parms.cli_cap_cnt++;
+                        }
+                        break;
+
+                case 70:        /* Display Status */
+                        {
+                                mddi_client_status_type *client_status_pkt_ptr;
+
+                                client_status_pkt_ptr =
+                                    (mddi_client_status_type *) rev_packet_data;
+                                if ((client_status_pkt_ptr->crc_error_count !=
+                                     0)
+                                    || (client_status_pkt_ptr->
+                                        reverse_link_request != 0)) {
+                                        MDDI_MSG_ERR
+                                            ("Client Status: RevReq=%d, CrcErr=%d\n",
+                                             client_status_pkt_ptr->
+                                             reverse_link_request,
+                                             client_status_pkt_ptr->
+                                             crc_error_count);
+                                } else {
+                                        MDDI_MSG_DEBUG
+                                            ("Client Status: RevReq=%d, CrcErr=%d\n",
+                                             client_status_pkt_ptr->
+                                             reverse_link_request,
+                                             client_status_pkt_ptr->
+                                             crc_error_count);
+                                }
+                                pmhctl->log_parms.fwd_crc_cnt +=
+                                    client_status_pkt_ptr->crc_error_count;
+                                pmhctl->stats.fwd_crc_count +=
+                                    client_status_pkt_ptr->crc_error_count;
+                                pmhctl->log_parms.cli_stat_cnt++;
+                        }
+                        break;
+
+                case 146:       /* register access packet */
+                        {
+                                mddi_register_access_packet_type
+                                    * regacc_pkt_ptr;
+
+                                regacc_pkt_ptr =
+                                    (mddi_register_access_packet_type *)
+                                    rev_packet_data;
+
+                                MDDI_MSG_DEBUG
+                                    ("Reg Acc parse reg=0x%x, value=0x%x\n",
+                                     regacc_pkt_ptr->register_address,
+                                     regacc_pkt_ptr->register_data_list);
+
+                                /* Copy register value to location passed in */
+                                if (mddi_reg_read_value_ptr) {
+#if defined(T_MSM6280) && !defined(T_MSM7200)
+                                        /* only least significant 16 bits are valid with 6280 */
+                                        *mddi_reg_read_value_ptr =
+                                            regacc_pkt_ptr->
+                                            register_data_list & 0x0000ffff;
+#else
+                                        *mddi_reg_read_value_ptr =
+                                            regacc_pkt_ptr->register_data_list;
+#endif
+                                        mddi_reg_read_successful = TRUE;
+                                        mddi_reg_read_value_ptr = NULL;
+                                }
+
+#ifdef DEBUG_MDDIHOSTI
+                                if ((mddi_gpio.polling_enabled) &&
+                                    (regacc_pkt_ptr->register_address ==
+                                     mddi_gpio.polling_reg)) {
+                                        /*
+                                         * ToDo: need to call Linux GPIO call
+                                         * here...
+                                         */
+                                         mddi_client_lcd_gpio_poll(
+                                         regacc_pkt_ptr->register_data_list);
+                                }
+#endif
+                                pmhctl->log_parms.reg_read_cnt++;
+                        }
+                        break;
+
+                default:        /* any other packet */
+                        {
+                                uint16 hdlr;
+
+                                for (hdlr = 0; hdlr < MAX_MDDI_REV_HANDLERS;
+                                     hdlr++) {
+                                        if (mddi_rev_pkt_handler[hdlr].
+                                            pkt_type ==
+                                            rev_pkt_ptr->packet_type) {
+                                                (*
+                                                 (mddi_rev_pkt_handler[hdlr].
+                                                  handler)) (rev_pkt_ptr);
+                                        /* pmhctl->rev_state = MDDI_REV_IDLE; */
+                                                break;
+                                        }
+                                }
+                                if (hdlr >= MAX_MDDI_REV_HANDLERS)
+                                        MDDI_MSG_ERR("MDDI unknown rev pkt\n");
+                        }
+                        break;
+                }
+        }
+        if ((pmhctl->rev_ptr_curr + pmhctl->rev_pkt_size) >=
+            (pmhctl->rev_ptr_start + MDDI_REV_BUFFER_SIZE)) {
+                pmhctl->rev_ptr_written = FALSE;
+        }
+
+        if (pmhctl->rev_state == MDDI_REV_ENCAP_ISSUED) {
+                pmhctl->rev_state = MDDI_REV_IDLE;
+                if (mddi_rev_user.waiting) {
+                        mddi_rev_user.waiting = FALSE;
+                        complete(&(mddi_rev_user.done_comp));
+                } else if (pmhctl->llist_info.reg_read_idx == UNASSIGNED_INDEX) {
+                        MDDI_MSG_ERR
+                            ("Reverse Encap state, but no reg read in progress\n");
+                } else {
+                        if ((!mddi_reg_read_successful) &&
+                            (mddi_reg_read_retry < mddi_reg_read_retry_max) &&
+                            (mddi_enable_reg_read_retry)) {
+                                /*
+                                 * There is a race condition that can happen
+                                 * where the reverse encapsulation message is
+                                 * sent out by the MDDI host before the register
+                                 * read packet is sent. As a work-around for
+                                 * that problem we issue the reverse
+                                 * encapsulation one more time before giving up.
+                                 */
+                                if (mddi_enable_reg_read_retry_once)
+                                        mddi_reg_read_retry =
+                                            mddi_reg_read_retry_max;
+                                pmhctl->rev_state = MDDI_REV_REG_READ_SENT;
+                                pmhctl->stats.reg_read_failure++;
+                        } else {
+                                uint16 reg_read_idx =
+                                    pmhctl->llist_info.reg_read_idx;
+
+                                mddi_reg_read_retry = 0;
+                                if (pmhctl->llist_notify[reg_read_idx].waiting) {
+                                        complete(&
+                                                 (pmhctl->
+                                                  llist_notify[reg_read_idx].
+                                                  done_comp));
+                                }
+                                pmhctl->llist_info.reg_read_idx =
+                                    UNASSIGNED_INDEX;
+                                if (pmhctl->llist_notify[reg_read_idx].
+                                    done_cb != NULL) {
+                                        (*
+                                         (pmhctl->llist_notify[reg_read_idx].
+                                          done_cb)) ();
+                                }
+                                pmhctl->llist_notify[reg_read_idx].next_idx =
+                                    UNASSIGNED_INDEX;
+                                pmhctl->llist_notify[reg_read_idx].in_use =
+                                    FALSE;
+                                pmhctl->llist_notify[reg_read_idx].waiting =
+                                    FALSE;
+                                pmhctl->llist_notify[reg_read_idx].done_cb =
+                                    NULL;
+                                if (!mddi_reg_read_successful)
+                                        pmhctl->stats.reg_read_failure++;
+                        }
+                }
+        } else if (pmhctl->rev_state == MDDI_REV_CLIENT_CAP_ISSUED) {
+#if defined(CONFIG_FB_MSM_MDP31) || defined(CONFIG_FB_MSM_MDP40)
+                if (mddi_host_core_version == 0x28 ||
+                    mddi_host_core_version == 0x30) {
+                        mddi_host_reg_out(FIFO_ALLOC, 0x00);
+                        pmhctl->rev_ptr_written = TRUE;
+                        mddi_host_reg_out(REV_PTR,
+                                pmhctl->mddi_rev_ptr_write_val);
+                        pmhctl->rev_ptr_curr = pmhctl->rev_ptr_start;
+                        mddi_host_reg_out(CMD, 0xC00);
+                }
+#endif
+
+                if (mddi_rev_user.waiting) {
+                        mddi_rev_user.waiting = FALSE;
+                        complete(&(mddi_rev_user.done_comp));
+                }
+                pmhctl->rev_state = MDDI_REV_IDLE;
+        } else {
+                pmhctl->rev_state = MDDI_REV_IDLE;
+        }
+
+        /* pmhctl->rev_state = MDDI_REV_IDLE; */
+
+        /* Re-enable interrupt */
+        mddi_host_reg_outm(INTEN, MDDI_INT_REV_DATA_AVAIL,
+                           MDDI_INT_REV_DATA_AVAIL);
+
+}
+
+static void mddi_issue_reverse_encapsulation(void)
+{
+        mddi_host_type host_idx = mddi_curr_host;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+        /* Only issue a reverse encapsulation packet if:
+         * 1) another reverse is not in progress (MDDI_REV_IDLE).
+         * 2) a register read has been sent (MDDI_REV_REG_READ_SENT).
+         * 3) forward is not in progress, because of a hw bug in client that
+         *    causes forward crc errors on packet immediately after rev encap.
+         */
+        if (((pmhctl->rev_state == MDDI_REV_IDLE) ||
+             (pmhctl->rev_state == MDDI_REV_REG_READ_SENT)) &&
+            (pmhctl->llist_info.transmitting_start_idx == UNASSIGNED_INDEX) &&
+            (!mdp_in_processing)) {
+                uint32 mddi_command = MDDI_CMD_SEND_REV_ENCAP;
+
+                if ((pmhctl->rev_state == MDDI_REV_REG_READ_SENT) ||
+                    (mddi_rev_encap_user_request == TRUE)) {
+                        mddi_host_enable_io_clock();
+                        if (pmhctl->link_state == MDDI_LINK_HIBERNATING) {
+                                /* need to wake up link before issuing rev encap command */
+                                MDDI_MSG_DEBUG("wake up link!\n");
+                                pmhctl->link_state = MDDI_LINK_ACTIVATING;
+                                mddi_host_reg_out(CMD, MDDI_CMD_LINK_ACTIVE);
+                        } else {
+                                if (pmhctl->rtd_counter >= mddi_rtd_frequency) {
+                                        MDDI_MSG_DEBUG
+                                            ("mddi sending RTD command!\n");
+                                        mddi_host_reg_out(CMD,
+                                                          MDDI_CMD_SEND_RTD);
+                                        pmhctl->rtd_counter = 0;
+                                        pmhctl->log_parms.rtd_cnt++;
+                                }
+                                if (pmhctl->rev_state != MDDI_REV_REG_READ_SENT) {
+                                        /* this is generic reverse request by user, so
+                                         * reset the waiting flag. */
+                                        mddi_rev_encap_user_request = FALSE;
+                                }
+                                /* link is active so send reverse encap to get register read results */
+                                pmhctl->rev_state = MDDI_REV_ENCAP_ISSUED;
+                                mddi_command = MDDI_CMD_SEND_REV_ENCAP;
+                                MDDI_MSG_DEBUG("sending rev encap!\n");
+                        }
+                } else
+                    if ((pmhctl->client_status_cnt >=
+                         mddi_client_status_frequency)
+                        || mddi_client_capability_request) {
+                        mddi_host_enable_io_clock();
+                        if (pmhctl->link_state == MDDI_LINK_HIBERNATING) {
+                                /* only wake up the link if it client status is overdue */
+                                if ((pmhctl->client_status_cnt >=
+                                     (mddi_client_status_frequency * 2))
+                                    || mddi_client_capability_request) {
+                                        /* need to wake up link before issuing rev encap command */
+                                        MDDI_MSG_DEBUG("wake up link!\n");
+                                        pmhctl->link_state =
+                                            MDDI_LINK_ACTIVATING;
+                                        mddi_host_reg_out(CMD,
+                                                          MDDI_CMD_LINK_ACTIVE);
+                                }
+                        } else {
+                                if (pmhctl->rtd_counter >= mddi_rtd_frequency) {
+                                        MDDI_MSG_DEBUG
+                                            ("mddi sending RTD command!\n");
+                                        mddi_host_reg_out(CMD,
+                                                          MDDI_CMD_SEND_RTD);
+                                        pmhctl->rtd_counter = 0;
+                                        pmhctl->log_parms.rtd_cnt++;
+                                }
+                                /* periodically get client status */
+                                MDDI_MSG_DEBUG
+                                    ("mddi sending rev enc! (get status)\n");
+                                if (mddi_client_capability_request) {
+                                        pmhctl->rev_state =
+                                            MDDI_REV_CLIENT_CAP_ISSUED;
+                                        mddi_command = MDDI_CMD_GET_CLIENT_CAP;
+                                        mddi_client_capability_request = FALSE;
+                                } else {
+                                        pmhctl->rev_state =
+                                            MDDI_REV_STATUS_REQ_ISSUED;
+                                        pmhctl->client_status_cnt = 0;
+                                        mddi_command =
+                                            MDDI_CMD_GET_CLIENT_STATUS;
+                                }
+                        }
+                }
+                if ((pmhctl->rev_state == MDDI_REV_ENCAP_ISSUED) ||
+                    (pmhctl->rev_state == MDDI_REV_STATUS_REQ_ISSUED) ||
+                    (pmhctl->rev_state == MDDI_REV_CLIENT_CAP_ISSUED)) {
+                        pmhctl->int_type.rev_encap_count++;
+#if defined(T_MSM6280) && !defined(T_MSM7200)
+                        mddi_rev_pointer_written = TRUE;
+                        mddi_host_reg_out(REV_PTR, mddi_rev_ptr_write_val);
+                        mddi_rev_ptr_curr = mddi_rev_ptr_start;
+                        /* force new rev ptr command */
+                        mddi_host_reg_out(CMD, 0xC00);
+#else
+                        if (!pmhctl->rev_ptr_written) {
+                                MDDI_MSG_DEBUG("writing reverse pointer!\n");
+                                pmhctl->rev_ptr_written = TRUE;
+#if defined(CONFIG_FB_MSM_MDP31) || defined(CONFIG_FB_MSM_MDP40)
+                                if ((pmhctl->rev_state ==
+                                     MDDI_REV_CLIENT_CAP_ISSUED) &&
+                                    (mddi_host_core_version == 0x28 ||
+                                     mddi_host_core_version == 0x30)) {
+                                        pmhctl->rev_ptr_written = FALSE;
+                                        mddi_host_reg_out(FIFO_ALLOC, 0x02);
+                                } else
+                                        mddi_host_reg_out(REV_PTR,
+                                                  pmhctl->
+                                                  mddi_rev_ptr_write_val);
+#else
+                                mddi_host_reg_out(REV_PTR,
+                                                  pmhctl->
+                                                  mddi_rev_ptr_write_val);
+#endif
+                        }
+#endif
+                        if (mddi_debug_clear_rev_data) {
+                                uint16 i;
+                                for (i = 0; i < MDDI_MAX_REV_DATA_SIZE / 4; i++)
+                                        pmhctl->rev_data_buf[i] = 0xdddddddd;
+                                /* clean cache */
+                                mddi_flush_cache_lines(pmhctl->rev_data_buf,
+                                                       MDDI_MAX_REV_DATA_SIZE);
+                        }
+
+                        /* send reverse encapsulation to get needed data */
+                        mddi_host_reg_out(CMD, mddi_command);
+                }
+        }
+
+}
+
+static void mddi_process_client_initiated_wakeup(void)
+{
+        mddi_host_type host_idx = mddi_curr_host;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        /* Disable MDDI_INT Interrupt, we detect client initiated wakeup one
+         * time for each entry into hibernation */
+        mddi_host_reg_outm(INTEN, MDDI_INT_MDDI_IN, 0);
+
+        if (host_idx == MDDI_HOST_PRIM) {
+                if (mddi_vsync_detect_enabled) {
+                        mddi_host_enable_io_clock();
+#ifndef MDDI_HOST_DISP_LISTEN
+                        /* issue command to bring up link */
+                        /* need to do this to clear the vsync condition */
+                        if (pmhctl->link_state == MDDI_LINK_HIBERNATING) {
+                                pmhctl->link_state = MDDI_LINK_ACTIVATING;
+                                mddi_host_reg_out(CMD, MDDI_CMD_LINK_ACTIVE);
+                        }
+#endif
+                        /*
+                         * Indicate to client specific code that vsync was
+                         * enabled, and we did not detect a client initiated
+                         * wakeup. The client specific handler can clear the
+                         * condition if necessary to prevent subsequent
+                         * client initiated wakeups.
+                         */
+                        mddi_client_lcd_vsync_detected(TRUE);
+                        pmhctl->log_parms.vsync_response_cnt++;
+                        MDDI_MSG_NOTICE("MDDI_INT_IN condition\n");
+
+                }
+        }
+
+        if (mddi_gpio.polling_enabled) {
+                mddi_host_enable_io_clock();
+                /* check interrupt status now */
+                (void)mddi_queue_register_read_int(mddi_gpio.polling_reg,
+                                                   &mddi_gpio.polling_val);
+        }
+}
+#endif /* FEATURE_MDDI_DISABLE_REVERSE */
+
+static void mddi_host_isr(void)
+{
+        uint32 int_reg, int_en;
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+        uint32 status_reg;
+#endif
+        mddi_host_type host_idx = mddi_curr_host;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        if (!MDDI_HOST_IS_HCLK_ON) {
+                MDDI_HOST_ENABLE_HCLK;
+                MDDI_MSG_DEBUG("HCLK disabled, but isr is firing\n");
+        }
+        int_reg = mddi_host_reg_in(INT);
+        int_en = mddi_host_reg_in(INTEN);
+        pmhctl->saved_int_reg = int_reg;
+        pmhctl->saved_int_en = int_en;
+        int_reg = int_reg & int_en;
+        pmhctl->int_type.count++;
+
+
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+        status_reg = mddi_host_reg_in(STAT);
+
+        if ((int_reg & MDDI_INT_MDDI_IN) ||
+            ((int_en & MDDI_INT_MDDI_IN) &&
+             ((int_reg == 0) || (status_reg & MDDI_STAT_CLIENT_WAKEUP_REQ)))) {
+                /*
+                 * The MDDI_IN condition will clear itself, and so it is
+                 * possible that MDDI_IN was the reason for the isr firing,
+                 * even though the interrupt register does not have the
+                 * MDDI_IN bit set. To check if this was the case we need to
+                 * look at the status register bit that signifies a client
+                 * initiated wakeup. If the status register bit is set, as well
+                 * as the MDDI_IN interrupt enabled, then we treat this as a
+                 * client initiated wakeup.
+                 */
+                if (int_reg & MDDI_INT_MDDI_IN)
+                        pmhctl->int_type.in_count++;
+                mddi_process_client_initiated_wakeup();
+        }
+#endif
+
+        if (int_reg & MDDI_INT_LINK_STATE_CHANGES) {
+                pmhctl->int_type.state_change_count++;
+                mddi_report_state_change(int_reg);
+        }
+
+        if (int_reg & MDDI_INT_PRI_LINK_LIST_DONE) {
+                pmhctl->int_type.ll_done_count++;
+                mddi_process_link_list_done();
+        }
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+        if (int_reg & MDDI_INT_REV_DATA_AVAIL) {
+                pmhctl->int_type.rev_avail_count++;
+                mddi_process_rev_packets();
+        }
+#endif
+
+        if (int_reg & MDDI_INT_ERROR_CONDITIONS) {
+                pmhctl->int_type.error_count++;
+                mddi_report_errors(int_reg);
+
+                mddi_host_reg_out(INT, int_reg & MDDI_INT_ERROR_CONDITIONS);
+        }
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+        mddi_issue_reverse_encapsulation();
+
+        if ((pmhctl->rev_state != MDDI_REV_ENCAP_ISSUED) &&
+            (pmhctl->rev_state != MDDI_REV_STATUS_REQ_ISSUED))
+#endif
+                /* don't want simultaneous reverse and forward with Eagle */
+                mddi_queue_forward_linked_list();
+
+        if (int_reg & MDDI_INT_NO_CMD_PKTS_PEND) {
+                /* this interrupt is used to kick the isr when hibernation is disabled */
+                mddi_host_reg_outm(INTEN, MDDI_INT_NO_CMD_PKTS_PEND, 0);
+        }
+
+        if ((!mddi_host_mdp_active_flag) &&
+            (!mddi_vsync_detect_enabled) &&
+            (pmhctl->llist_info.transmitting_start_idx == UNASSIGNED_INDEX) &&
+            (pmhctl->llist_info.waiting_start_idx == UNASSIGNED_INDEX) &&
+            (pmhctl->rev_state == MDDI_REV_IDLE)) {
+                if (pmhctl->link_state == MDDI_LINK_HIBERNATING) {
+                        mddi_host_disable_io_clock();
+                        mddi_host_disable_hclk();
+                }
+#ifdef FEATURE_MDDI_HOST_ENABLE_EARLY_HIBERNATION
+                else if ((pmhctl->link_state == MDDI_LINK_ACTIVE) &&
+                         (!pmhctl->disable_hibernation)) {
+                        mddi_host_reg_out(CMD, MDDI_CMD_POWERDOWN);
+                }
+#endif
+        }
+}
+
+static void mddi_host_isr_primary(void)
+{
+        mddi_curr_host = MDDI_HOST_PRIM;
+        mddi_host_isr();
+}
+
+irqreturn_t mddi_pmdh_isr_proxy(int irq, void *ptr)
+{
+        mddi_host_isr_primary();
+        return IRQ_HANDLED;
+}
+
+static void mddi_host_isr_external(void)
+{
+        mddi_curr_host = MDDI_HOST_EXT;
+        mddi_host_isr();
+        mddi_curr_host = MDDI_HOST_PRIM;
+}
+
+irqreturn_t mddi_emdh_isr_proxy(int irq, void *ptr)
+{
+        mddi_host_isr_external();
+        return IRQ_HANDLED;
+}
+
+static void mddi_host_initialize_registers(mddi_host_type host_idx)
+{
+        uint32 pad_reg_val;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        if (pmhctl->driver_state == MDDI_DRIVER_ENABLED)
+                return;
+
+        /* turn on HCLK to MDDI host core */
+        mddi_host_enable_hclk();
+
+        /* MDDI Reset command */
+        mddi_host_reg_out(CMD, MDDI_CMD_RESET);
+
+        /* Version register (= 0x01) */
+        mddi_host_reg_out(VERSION, 0x0001);
+
+        /* Bytes per subframe register */
+        mddi_host_reg_out(BPS, MDDI_HOST_BYTES_PER_SUBFRAME);
+
+        /* Subframes per media frames register (= 0x03) */
+        mddi_host_reg_out(SPM, 0x0003);
+
+        /* Turn Around 1 register (= 0x05) */
+        mddi_host_reg_out(TA1_LEN, 0x0005);
+
+        /* Turn Around 2 register (= 0x0C) */
+        mddi_host_reg_out(TA2_LEN, MDDI_HOST_TA2_LEN);
+
+        /* Drive hi register (= 0x96) */
+        mddi_host_reg_out(DRIVE_HI, 0x0096);
+
+        /* Drive lo register (= 0x32) */
+        mddi_host_reg_out(DRIVE_LO, 0x0032);
+
+        /* Display wakeup count register (= 0x3c) */
+        mddi_host_reg_out(DISP_WAKE, 0x003c);
+
+        /* Reverse Rate Divisor register (= 0x2) */
+        mddi_host_reg_out(REV_RATE_DIV, MDDI_HOST_REV_RATE_DIV);
+
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+        /* Reverse Pointer Size */
+        mddi_host_reg_out(REV_SIZE, MDDI_REV_BUFFER_SIZE);
+
+        /* Rev Encap Size */
+        mddi_host_reg_out(REV_ENCAP_SZ, pmhctl->rev_pkt_size);
+#endif
+
+        /* Periodic Rev Encap */
+        /* don't send periodically */
+        mddi_host_reg_out(CMD, MDDI_CMD_PERIODIC_REV_ENCAP);
+
+        pad_reg_val = mddi_host_reg_in(PAD_CTL);
+        if (pad_reg_val == 0) {
+                /* If we are turning on band gap, need to wait 5us before turning
+                 * on the rest of the PAD */
+                mddi_host_reg_out(PAD_CTL, 0x08000);
+                udelay(5);
+        }
+#ifdef T_MSM7200
+        /* Recommendation from PAD hw team */
+        mddi_host_reg_out(PAD_CTL, 0xa850a);
+#else
+        /* Recommendation from PAD hw team */
+        mddi_host_reg_out(PAD_CTL, 0xa850f);
+#endif
+
+#if defined(CONFIG_FB_MSM_MDP31) || defined(CONFIG_FB_MSM_MDP40)
+        mddi_host_reg_out(PAD_IO_CTL, 0x00320000);
+        mddi_host_reg_out(PAD_CAL, 0x00220020);
+#endif
+
+        mddi_host_core_version = mddi_host_reg_inm(CORE_VER, 0xffff);
+
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+        if (mddi_host_core_version >= 8)
+                mddi_rev_ptr_workaround = FALSE;
+        pmhctl->rev_ptr_curr = pmhctl->rev_ptr_start;
+#endif
+
+        if ((mddi_host_core_version > 8) && (mddi_host_core_version < 0x19))
+                mddi_host_reg_out(TEST, 0x2);
+
+        /* Need an even number for counts */
+        mddi_host_reg_out(DRIVER_START_CNT, 0x60006);
+
+#ifndef T_MSM7500
+        /* Setup defaults for MDP related register */
+        mddi_host_reg_out(MDP_VID_FMT_DES, 0x5666);
+        mddi_host_reg_out(MDP_VID_PIX_ATTR, 0x00C3);
+        mddi_host_reg_out(MDP_VID_CLIENTID, 0);
+#endif
+
+        /* automatically hibernate after 1 empty subframe */
+        if (pmhctl->disable_hibernation)
+                mddi_host_reg_out(CMD, MDDI_CMD_HIBERNATE);
+        else
+                mddi_host_reg_out(CMD, MDDI_CMD_HIBERNATE | 1);
+
+        /* Bring up link if display (client) requests it */
+#ifdef MDDI_HOST_DISP_LISTEN
+        mddi_host_reg_out(CMD, MDDI_CMD_DISP_LISTEN);
+#else
+        mddi_host_reg_out(CMD, MDDI_CMD_DISP_IGNORE);
+#endif
+
+}
+
+void mddi_host_configure_interrupts(mddi_host_type host_idx, boolean enable)
+{
+        unsigned long flags;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        spin_lock_irqsave(&mddi_host_spin_lock, flags);
+
+        /* turn on HCLK to MDDI host core if it has been disabled */
+        mddi_host_enable_hclk();
+        /* Clear MDDI Interrupt enable reg */
+        mddi_host_reg_out(INTEN, 0);
+
+        spin_unlock_irqrestore(&mddi_host_spin_lock, flags);
+
+        if (enable) {
+                pmhctl->driver_state = MDDI_DRIVER_ENABLED;
+
+                if (host_idx == MDDI_HOST_PRIM) {
+                        if (request_irq
+                            (INT_MDDI_PRI, mddi_pmdh_isr_proxy, IRQF_DISABLED,
+                             "PMDH", 0) != 0)
+                                printk(KERN_ERR
+                                       "a mddi: unable to request_irq\n");
+                        else
+                                int_mddi_pri_flag = TRUE;
+                } else {
+                        if (request_irq
+                            (INT_MDDI_EXT, mddi_emdh_isr_proxy, IRQF_DISABLED,
+                             "EMDH", 0) != 0)
+                                printk(KERN_ERR
+                                       "b mddi: unable to request_irq\n");
+                        else
+                                int_mddi_ext_flag = TRUE;
+                }
+
+                /* Set MDDI Interrupt enable reg -- Enable Reverse data avail */
+#ifdef FEATURE_MDDI_DISABLE_REVERSE
+                mddi_host_reg_out(INTEN,
+                                  MDDI_INT_ERROR_CONDITIONS |
+                                  MDDI_INT_LINK_STATE_CHANGES);
+#else
+                /* Reverse Pointer register */
+                pmhctl->rev_ptr_written = FALSE;
+
+                mddi_host_reg_out(INTEN,
+                                  MDDI_INT_REV_DATA_AVAIL |
+                                  MDDI_INT_ERROR_CONDITIONS |
+                                  MDDI_INT_LINK_STATE_CHANGES);
+                pmhctl->rtd_counter = mddi_rtd_frequency;
+                pmhctl->client_status_cnt = 0;
+#endif
+        } else {
+                if (pmhctl->driver_state == MDDI_DRIVER_ENABLED)
+                        pmhctl->driver_state = MDDI_DRIVER_DISABLED;
+        }
+
+}
+
+static void mddi_host_powerup(mddi_host_type host_idx)
+{
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        if (pmhctl->link_state != MDDI_LINK_DISABLED)
+                return;
+
+        /* enable IO_CLK and hclk to MDDI host core */
+        mddi_host_enable_io_clock();
+
+        mddi_host_initialize_registers(host_idx);
+        mddi_host_configure_interrupts(host_idx, TRUE);
+
+        pmhctl->link_state = MDDI_LINK_ACTIVATING;
+
+        /* Link activate command */
+        mddi_host_reg_out(CMD, MDDI_CMD_LINK_ACTIVE);
+
+#ifdef CLKRGM_MDDI_IO_CLOCK_IN_MHZ
+        MDDI_MSG_NOTICE("MDDI Host: Activating Link %d Mbps\n",
+                        CLKRGM_MDDI_IO_CLOCK_IN_MHZ * 2);
+#else
+        MDDI_MSG_NOTICE("MDDI Host: Activating Link\n");
+#endif
+
+        /* Initialize the timer */
+        if (host_idx == MDDI_HOST_PRIM)
+                mddi_host_timer_service(0);
+}
+
+void mddi_host_init(mddi_host_type host_idx)
+/* Write out the MDDI configuration registers */
+{
+        static boolean initialized = FALSE;
+        mddi_host_cntl_type *pmhctl;
+
+        if (host_idx >= MDDI_NUM_HOST_CORES) {
+                MDDI_MSG_ERR("Invalid host core index\n");
+                return;
+        }
+
+        if (!initialized) {
+                uint16 idx;
+                mddi_host_type host;
+                for (host = MDDI_HOST_PRIM; host < MDDI_NUM_HOST_CORES; host++) {
+                        pmhctl = &(mhctl[host]);
+                        initialized = TRUE;
+
+                        pmhctl->llist_ptr =
+                            dma_alloc_coherent(NULL, MDDI_LLIST_POOL_SIZE,
+                                               &(pmhctl->llist_dma_addr),
+                                               GFP_KERNEL);
+                        pmhctl->llist_dma_ptr =
+                            (mddi_linked_list_type *) (void *)pmhctl->
+                            llist_dma_addr;
+#ifdef FEATURE_MDDI_DISABLE_REVERSE
+                        pmhctl->rev_data_buf = NULL;
+                        if (pmhctl->llist_ptr == NULL)
+#else
+                        mddi_rev_user.waiting = FALSE;
+                        init_completion(&(mddi_rev_user.done_comp));
+                        pmhctl->rev_data_buf =
+                            dma_alloc_coherent(NULL, MDDI_MAX_REV_DATA_SIZE,
+                                               &(pmhctl->rev_data_dma_addr),
+                                               GFP_KERNEL);
+                        if ((pmhctl->llist_ptr == NULL)
+                            || (pmhctl->rev_data_buf == NULL))
+#endif
+                        {
+                                MDDI_MSG_CRIT
+                                    ("unable to alloc non-cached memory\n");
+                        }
+                        llist_extern[host] = pmhctl->llist_ptr;
+                        llist_dma_extern[host] = pmhctl->llist_dma_ptr;
+                        llist_extern_notify[host] = pmhctl->llist_notify;
+
+                        for (idx = 0; idx < UNASSIGNED_INDEX; idx++) {
+                                init_completion(&
+                                                (pmhctl->llist_notify[idx].
+                                                 done_comp));
+                        }
+                        init_completion(&(pmhctl->mddi_llist_avail_comp));
+                        spin_lock_init(&mddi_host_spin_lock);
+                        pmhctl->mddi_waiting_for_llist_avail = FALSE;
+                        pmhctl->mddi_rev_ptr_write_val =
+                            (uint32) (void *)(pmhctl->rev_data_dma_addr);
+                        pmhctl->rev_ptr_start = (void *)pmhctl->rev_data_buf;
+
+                        pmhctl->rev_pkt_size = MDDI_DEFAULT_REV_PKT_SIZE;
+                        pmhctl->rev_state = MDDI_REV_IDLE;
+#ifdef IMAGE_MODEM_PROC
+                        /* assume hibernation state is last state from APPS proc, so that
+                         * we don't reinitialize the host core */
+                        pmhctl->link_state = MDDI_LINK_HIBERNATING;
+#else
+                        pmhctl->link_state = MDDI_LINK_DISABLED;
+#endif
+                        pmhctl->driver_state = MDDI_DRIVER_DISABLED;
+                        pmhctl->disable_hibernation = FALSE;
+
+                        /* initialize llist variables */
+                        pmhctl->llist_info.transmitting_start_idx =
+                            UNASSIGNED_INDEX;
+                        pmhctl->llist_info.transmitting_end_idx =
+                            UNASSIGNED_INDEX;
+                        pmhctl->llist_info.waiting_start_idx = UNASSIGNED_INDEX;
+                        pmhctl->llist_info.waiting_end_idx = UNASSIGNED_INDEX;
+                        pmhctl->llist_info.reg_read_idx = UNASSIGNED_INDEX;
+                        pmhctl->llist_info.next_free_idx =
+                            MDDI_FIRST_DYNAMIC_LLIST_IDX;
+                        pmhctl->llist_info.reg_read_waiting = FALSE;
+
+                        mddi_vsync_detect_enabled = FALSE;
+                        mddi_gpio.polling_enabled = FALSE;
+
+                        pmhctl->int_type.count = 0;
+                        pmhctl->int_type.in_count = 0;
+                        pmhctl->int_type.disp_req_count = 0;
+                        pmhctl->int_type.state_change_count = 0;
+                        pmhctl->int_type.ll_done_count = 0;
+                        pmhctl->int_type.rev_avail_count = 0;
+                        pmhctl->int_type.error_count = 0;
+                        pmhctl->int_type.rev_encap_count = 0;
+                        pmhctl->int_type.llist_ptr_write_1 = 0;
+                        pmhctl->int_type.llist_ptr_write_2 = 0;
+
+                        pmhctl->stats.fwd_crc_count = 0;
+                        pmhctl->stats.rev_crc_count = 0;
+                        pmhctl->stats.pri_underflow = 0;
+                        pmhctl->stats.sec_underflow = 0;
+                        pmhctl->stats.rev_overflow = 0;
+                        pmhctl->stats.pri_overwrite = 0;
+                        pmhctl->stats.sec_overwrite = 0;
+                        pmhctl->stats.rev_overwrite = 0;
+                        pmhctl->stats.dma_failure = 0;
+                        pmhctl->stats.rtd_failure = 0;
+                        pmhctl->stats.reg_read_failure = 0;
+#ifdef FEATURE_MDDI_UNDERRUN_RECOVERY
+                        pmhctl->stats.pri_underrun_detected = 0;
+#endif
+
+                        pmhctl->log_parms.rtd_cnt = 0;
+                        pmhctl->log_parms.rev_enc_cnt = 0;
+                        pmhctl->log_parms.vid_cnt = 0;
+                        pmhctl->log_parms.reg_acc_cnt = 0;
+                        pmhctl->log_parms.cli_stat_cnt = 0;
+                        pmhctl->log_parms.cli_cap_cnt = 0;
+                        pmhctl->log_parms.reg_read_cnt = 0;
+                        pmhctl->log_parms.link_active_cnt = 0;
+                        pmhctl->log_parms.link_hibernate_cnt = 0;
+                        pmhctl->log_parms.fwd_crc_cnt = 0;
+                        pmhctl->log_parms.rev_crc_cnt = 0;
+                        pmhctl->log_parms.vsync_response_cnt = 0;
+
+                        prev_parms[host_idx] = pmhctl->log_parms;
+                        mddi_client_capability_pkt.packet_length = 0;
+                }
+
+#ifndef T_MSM7500
+                /* tell clock driver we are user of this PLL */
+                MDDI_HOST_ENABLE_IO_CLOCK;
+#endif
+        }
+
+        mddi_host_powerup(host_idx);
+        pmhctl = &(mhctl[host_idx]);
+}
+
+#ifdef CONFIG_FB_MSM_MDDI_AUTO_DETECT
+static uint32 mddi_client_id;
+
+uint32 mddi_get_client_id(void)
+{
+
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+        mddi_host_type host_idx = MDDI_HOST_PRIM;
+        static boolean client_detection_try = FALSE;
+        mddi_host_cntl_type *pmhctl;
+        unsigned long flags;
+        uint16 saved_rev_pkt_size;
+
+        if (!client_detection_try) {
+                /* Toshiba display requires larger drive_lo value */
+                mddi_host_reg_out(DRIVE_LO, 0x0050);
+
+                pmhctl = &(mhctl[MDDI_HOST_PRIM]);
+
+                saved_rev_pkt_size = pmhctl->rev_pkt_size;
+
+                /* Increase Rev Encap Size */
+                pmhctl->rev_pkt_size = MDDI_CLIENT_CAPABILITY_REV_PKT_SIZE;
+                mddi_host_reg_out(REV_ENCAP_SZ, pmhctl->rev_pkt_size);
+
+                /* disable hibernation temporarily */
+                if (!pmhctl->disable_hibernation)
+                        mddi_host_reg_out(CMD, MDDI_CMD_HIBERNATE);
+
+                mddi_rev_user.waiting = TRUE;
+                INIT_COMPLETION(mddi_rev_user.done_comp);
+
+                spin_lock_irqsave(&mddi_host_spin_lock, flags);
+
+                /* turn on clock(s), if they have been disabled */
+                mddi_host_enable_hclk();
+                mddi_host_enable_io_clock();
+
+                mddi_client_capability_request = TRUE;
+
+                if (pmhctl->rev_state == MDDI_REV_IDLE) {
+                        /* attempt to send the reverse encapsulation now */
+                        mddi_issue_reverse_encapsulation();
+                }
+                spin_unlock_irqrestore(&mddi_host_spin_lock, flags);
+
+                wait_for_completion_killable(&(mddi_rev_user.done_comp));
+
+                /* Set Rev Encap Size back to its original value */
+                pmhctl->rev_pkt_size = saved_rev_pkt_size;
+                mddi_host_reg_out(REV_ENCAP_SZ, pmhctl->rev_pkt_size);
+
+                /* reenable auto-hibernate */
+                if (!pmhctl->disable_hibernation)
+                        mddi_host_reg_out(CMD, MDDI_CMD_HIBERNATE | 1);
+
+                mddi_host_reg_out(DRIVE_LO, 0x0032);
+                client_detection_try = TRUE;
+
+                mddi_client_id = (mddi_client_capability_pkt.Mfr_Name<<16) |
+                                mddi_client_capability_pkt.Product_Code;
+
+                if (!mddi_client_id)
+                        mddi_disable(1);
+        }
+
+#if 0
+        switch (mddi_client_capability_pkt.Mfr_Name) {
+        case 0x4474:
+                if ((mddi_client_capability_pkt.Product_Code != 0x8960) &&
+                    (target == DISPLAY_1)) {
+                        ret = PRISM_WVGA;
+                }
+                break;
+
+        case 0xD263:
+                if (target == DISPLAY_1)
+                        ret = TOSHIBA_VGA_PRIM;
+                else if (target == DISPLAY_2)
+                        ret = TOSHIBA_QCIF_SECD;
+                break;
+
+        case 0:
+                if (mddi_client_capability_pkt.Product_Code == 0x8835) {
+                        if (target == DISPLAY_1)
+                                ret = SHARP_QVGA_PRIM;
+                        else if (target == DISPLAY_2)
+                                ret = SHARP_128x128_SECD;
+                }
+                break;
+
+        default:
+                break;
+        }
+
+        if ((!client_detection_try) && (ret != TOSHIBA_VGA_PRIM)
+            && (ret != TOSHIBA_QCIF_SECD)) {
+                /* Not a Toshiba display, so change drive_lo back to default value */
+                mddi_host_reg_out(DRIVE_LO, 0x0032);
+        }
+#endif
+
+#endif
+
+        return mddi_client_id;
+}
+#endif
+
+void mddi_host_powerdown(mddi_host_type host_idx)
+{
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        if (host_idx >= MDDI_NUM_HOST_CORES) {
+                MDDI_MSG_ERR("Invalid host core index\n");
+                return;
+        }
+
+        if (pmhctl->driver_state == MDDI_DRIVER_RESET) {
+                return;
+        }
+
+        if (host_idx == MDDI_HOST_PRIM) {
+                /* disable timer */
+                del_timer(&mddi_host_timer);
+        }
+
+        mddi_host_configure_interrupts(host_idx, FALSE);
+
+        /* turn on HCLK to MDDI host core if it has been disabled */
+        mddi_host_enable_hclk();
+
+        /* MDDI Reset command */
+        mddi_host_reg_out(CMD, MDDI_CMD_RESET);
+
+        /* Pad Control Register */
+        mddi_host_reg_out(PAD_CTL, 0x0);
+
+        /* disable IO_CLK and hclk to MDDI host core */
+        mddi_host_disable_io_clock();
+        mddi_host_disable_hclk();
+
+        pmhctl->link_state = MDDI_LINK_DISABLED;
+        pmhctl->driver_state = MDDI_DRIVER_RESET;
+
+        MDDI_MSG_NOTICE("MDDI Host: Disabling Link\n");
+
+}
+
+uint16 mddi_get_next_free_llist_item(mddi_host_type host_idx, boolean wait)
+{
+        unsigned long flags;
+        uint16 ret_idx;
+        boolean forced_wait = FALSE;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        ret_idx = pmhctl->llist_info.next_free_idx;
+
+        pmhctl->llist_info.next_free_idx++;
+        if (pmhctl->llist_info.next_free_idx >= MDDI_NUM_DYNAMIC_LLIST_ITEMS)
+                pmhctl->llist_info.next_free_idx = MDDI_FIRST_DYNAMIC_LLIST_IDX;
+        spin_lock_irqsave(&mddi_host_spin_lock, flags);
+        if (pmhctl->llist_notify[ret_idx].in_use) {
+                if (!wait) {
+                        pmhctl->llist_info.next_free_idx = ret_idx;
+                        ret_idx = UNASSIGNED_INDEX;
+                } else {
+                        forced_wait = TRUE;
+                        INIT_COMPLETION(pmhctl->mddi_llist_avail_comp);
+                }
+        }
+        spin_unlock_irqrestore(&mddi_host_spin_lock, flags);
+
+        if (forced_wait) {
+                wait_for_completion_killable(&
+                                                  (pmhctl->
+                                                   mddi_llist_avail_comp));
+                MDDI_MSG_ERR("task waiting on mddi llist item\n");
+        }
+
+        if (ret_idx != UNASSIGNED_INDEX) {
+                pmhctl->llist_notify[ret_idx].waiting = FALSE;
+                pmhctl->llist_notify[ret_idx].done_cb = NULL;
+                pmhctl->llist_notify[ret_idx].in_use = TRUE;
+                pmhctl->llist_notify[ret_idx].next_idx = UNASSIGNED_INDEX;
+        }
+
+        return ret_idx;
+}
+
+uint16 mddi_get_reg_read_llist_item(mddi_host_type host_idx, boolean wait)
+{
+#ifdef FEATURE_MDDI_DISABLE_REVERSE
+        MDDI_MSG_CRIT("No reverse link available\n");
+        (void)wait;
+        return FALSE;
+#else
+        unsigned long flags;
+        uint16 ret_idx;
+        boolean error = FALSE;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        spin_lock_irqsave(&mddi_host_spin_lock, flags);
+        if (pmhctl->llist_info.reg_read_idx != UNASSIGNED_INDEX) {
+                /* need to block here or is this an error condition? */
+                error = TRUE;
+                ret_idx = UNASSIGNED_INDEX;
+        }
+        spin_unlock_irqrestore(&mddi_host_spin_lock, flags);
+
+        if (!error) {
+                ret_idx = pmhctl->llist_info.reg_read_idx =
+                    mddi_get_next_free_llist_item(host_idx, wait);
+                /* clear the reg_read_waiting flag */
+                pmhctl->llist_info.reg_read_waiting = FALSE;
+        }
+
+        if (error)
+                MDDI_MSG_ERR("***** Reg read still in progress! ****\n");
+        return ret_idx;
+#endif
+
+}
+
+void mddi_queue_forward_packets(uint16 first_llist_idx,
+                                uint16 last_llist_idx,
+                                boolean wait,
+                                mddi_llist_done_cb_type llist_done_cb,
+                                mddi_host_type host_idx)
+{
+        unsigned long flags;
+        mddi_linked_list_type *llist;
+        mddi_linked_list_type *llist_dma;
+        mddi_host_cntl_type *pmhctl = &(mhctl[host_idx]);
+
+        if ((first_llist_idx >= UNASSIGNED_INDEX) ||
+            (last_llist_idx >= UNASSIGNED_INDEX)) {
+                MDDI_MSG_ERR("MDDI queueing invalid linked list\n");
+                return;
+        }
+
+        if (pmhctl->link_state == MDDI_LINK_DISABLED)
+                MDDI_MSG_CRIT("MDDI host powered down!\n");
+
+        llist = pmhctl->llist_ptr;
+        llist_dma = pmhctl->llist_dma_ptr;
+
+        /* clean cache so MDDI host can read data */
+        memory_barrier();
+
+        pmhctl->llist_notify[last_llist_idx].waiting = wait;
+        if (wait)
+                INIT_COMPLETION(pmhctl->llist_notify[last_llist_idx].done_comp);
+        pmhctl->llist_notify[last_llist_idx].done_cb = llist_done_cb;
+
+        spin_lock_irqsave(&mddi_host_spin_lock, flags);
+
+        if ((pmhctl->llist_info.transmitting_start_idx == UNASSIGNED_INDEX) &&
+            (pmhctl->llist_info.waiting_start_idx == UNASSIGNED_INDEX) &&
+            (pmhctl->rev_state == MDDI_REV_IDLE)) {
+                /* no packets are currently transmitting */
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+                if (first_llist_idx == pmhctl->llist_info.reg_read_idx) {
+                        /* This is the special case where the packet is a register read. */
+                        pmhctl->rev_state = MDDI_REV_REG_READ_ISSUED;
+                        mddi_reg_read_retry = 0;
+                        /* mddi_rev_reg_read_attempt = 1; */
+                }
+#endif
+                /* assign transmitting index values */
+                pmhctl->llist_info.transmitting_start_idx = first_llist_idx;
+                pmhctl->llist_info.transmitting_end_idx = last_llist_idx;
+
+                /* turn on clock(s), if they have been disabled */
+                mddi_host_enable_hclk();
+                mddi_host_enable_io_clock();
+                pmhctl->int_type.llist_ptr_write_1++;
+                /* Write to primary pointer register */
+                dma_coherent_pre_ops();
+                mddi_host_reg_out(PRI_PTR, &llist_dma[first_llist_idx]);
+
+                /* enable interrupt when complete */
+                mddi_host_reg_outm(INTEN, MDDI_INT_PRI_LINK_LIST_DONE,
+                                   MDDI_INT_PRI_LINK_LIST_DONE);
+
+        } else if (pmhctl->llist_info.waiting_start_idx == UNASSIGNED_INDEX) {
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+                if (first_llist_idx == pmhctl->llist_info.reg_read_idx) {
+                        /*
+                         * we have a register read to send but need to wait
+                         * for current reverse activity to end or there are
+                         * packets currently transmitting
+                         */
+                        /* mddi_rev_reg_read_attempt = 0; */
+                        pmhctl->llist_info.reg_read_waiting = TRUE;
+                }
+#endif
+
+                /* assign waiting index values */
+                pmhctl->llist_info.waiting_start_idx = first_llist_idx;
+                pmhctl->llist_info.waiting_end_idx = last_llist_idx;
+        } else {
+                uint16 prev_end_idx = pmhctl->llist_info.waiting_end_idx;
+#ifndef FEATURE_MDDI_DISABLE_REVERSE
+                if (first_llist_idx == pmhctl->llist_info.reg_read_idx) {
+                        /*
+                         * we have a register read to send but need to wait
+                         * for current reverse activity to end or there are
+                         * packets currently transmitting
+                         */
+                        /* mddi_rev_reg_read_attempt = 0; */
+                        pmhctl->llist_info.reg_read_waiting = TRUE;
+                }
+#endif
+
+                llist = pmhctl->llist_ptr;
+
+                /* clear end flag in previous last packet */
+                llist[prev_end_idx].link_controller_flags = 0;
+                pmhctl->llist_notify[prev_end_idx].next_idx = first_llist_idx;
+
+                /* set the next_packet_pointer of the previous last packet */
+                llist[prev_end_idx].next_packet_pointer =
+                    (void *)(&llist_dma[first_llist_idx]);
+
+                /* clean cache so MDDI host can read data */
+                memory_barrier();
+
+                /* assign new waiting last index value */
+                pmhctl->llist_info.waiting_end_idx = last_llist_idx;
+        }
+
+        spin_unlock_irqrestore(&mddi_host_spin_lock, flags);
+
+}
+
+void mddi_host_write_pix_attr_reg(uint32 value)
+{
+        (void)value;
+}
+
+void mddi_queue_reverse_encapsulation(boolean wait)
+{
+#ifdef FEATURE_MDDI_DISABLE_REVERSE
+        MDDI_MSG_CRIT("No reverse link available\n");
+        (void)wait;
+#else
+        unsigned long flags;
+        boolean error = FALSE;
+        mddi_host_type host_idx = MDDI_HOST_PRIM;
+        mddi_host_cntl_type *pmhctl = &(mhctl[MDDI_HOST_PRIM]);
+
+        spin_lock_irqsave(&mddi_host_spin_lock, flags);
+
+        /* turn on clock(s), if they have been disabled */
+        mddi_host_enable_hclk();
+        mddi_host_enable_io_clock();
+
+        if (wait) {
+                if (!mddi_rev_user.waiting) {
+                        mddi_rev_user.waiting = TRUE;
+                        INIT_COMPLETION(mddi_rev_user.done_comp);
+                } else
+                        error = TRUE;
+        }
+        mddi_rev_encap_user_request = TRUE;
+
+        if (pmhctl->rev_state == MDDI_REV_IDLE) {
+                /* attempt to send the reverse encapsulation now */
+                mddi_host_type orig_host_idx = mddi_curr_host;
+                mddi_curr_host = host_idx;
+                mddi_issue_reverse_encapsulation();
+                mddi_curr_host = orig_host_idx;
+        }
+        spin_unlock_irqrestore(&mddi_host_spin_lock, flags);
+
+        if (error) {
+                MDDI_MSG_ERR("Reverse Encap request already in progress\n");
+        } else if (wait)
+                wait_for_completion_killable(&(mddi_rev_user.done_comp));
+#endif
+}
+
+/* ISR to be executed */
+boolean mddi_set_rev_handler(mddi_rev_handler_type handler, uint16 pkt_type)
+{
+#ifdef FEATURE_MDDI_DISABLE_REVERSE
+        MDDI_MSG_CRIT("No reverse link available\n");
+        (void)handler;
+        (void)pkt_type;
+        return (FALSE);
+#else
+        unsigned long flags;
+        uint16 hdlr;
+        boolean handler_set = FALSE;
+        boolean overwrite = FALSE;
+        mddi_host_type host_idx = MDDI_HOST_PRIM;
+        mddi_host_cntl_type *pmhctl = &(mhctl[MDDI_HOST_PRIM]);
+
+        /* Disable interrupts */
+        spin_lock_irqsave(&mddi_host_spin_lock, flags);
+
+        for (hdlr = 0; hdlr < MAX_MDDI_REV_HANDLERS; hdlr++) {
+                if (mddi_rev_pkt_handler[hdlr].pkt_type == pkt_type) {
+                        mddi_rev_pkt_handler[hdlr].handler = handler;
+                        if (handler == NULL) {
+                                /* clearing handler from table */
+                                mddi_rev_pkt_handler[hdlr].pkt_type =
+                                    INVALID_PKT_TYPE;
+                                handler_set = TRUE;
+                                if (pkt_type == 0x10) { /* video stream packet */
+                                        /* ensure HCLK on to MDDI host core before register write */
+                                        mddi_host_enable_hclk();
+                                        /* No longer getting video, so reset rev encap size to default */
+                                        pmhctl->rev_pkt_size =
+                                            MDDI_DEFAULT_REV_PKT_SIZE;
+                                        mddi_host_reg_out(REV_ENCAP_SZ,
+                                                          pmhctl->rev_pkt_size);
+                                }
+                        } else {
+                                /* already a handler for this packet */
+                                overwrite = TRUE;
+                        }
+                        break;
+                }
+        }
+        if ((hdlr >= MAX_MDDI_REV_HANDLERS) && (handler != NULL)) {
+                /* assigning new handler */
+                for (hdlr = 0; hdlr < MAX_MDDI_REV_HANDLERS; hdlr++) {
+                        if (mddi_rev_pkt_handler[hdlr].pkt_type ==
+                            INVALID_PKT_TYPE) {
+                                if ((pkt_type == 0x10) &&       /* video stream packet */
+                                    (pmhctl->rev_pkt_size <
+                                     MDDI_VIDEO_REV_PKT_SIZE)) {
+                                        /* ensure HCLK on to MDDI host core before register write */
+                                        mddi_host_enable_hclk();
+                                        /* Increase Rev Encap Size */
+                                        pmhctl->rev_pkt_size =
+                                            MDDI_VIDEO_REV_PKT_SIZE;
+                                        mddi_host_reg_out(REV_ENCAP_SZ,
+                                                          pmhctl->rev_pkt_size);
+                                }
+                                mddi_rev_pkt_handler[hdlr].handler = handler;
+                                mddi_rev_pkt_handler[hdlr].pkt_type = pkt_type;
+                                handler_set = TRUE;
+                                break;
+                        }
+                }
+        }
+
+        /* Restore interrupts */
+        spin_unlock_irqrestore(&mddi_host_spin_lock, flags);
+
+        if (overwrite)
+                MDDI_MSG_ERR("Overwriting previous rev packet handler\n");
+
+        return handler_set;
+
+#endif
+}                               /* mddi_set_rev_handler */
+
+void mddi_host_disable_hibernation(boolean disable)
+{
+        mddi_host_type host_idx = MDDI_HOST_PRIM;
+        mddi_host_cntl_type *pmhctl = &(mhctl[MDDI_HOST_PRIM]);
+
+        if (disable) {
+                pmhctl->disable_hibernation = TRUE;
+                /* hibernation will be turned off by isr next time it is entered */
+        } else {
+                if (pmhctl->disable_hibernation) {
+                        unsigned long flags;
+                        spin_lock_irqsave(&mddi_host_spin_lock, flags);
+                        if (!MDDI_HOST_IS_HCLK_ON)
+                                MDDI_HOST_ENABLE_HCLK;
+                        mddi_host_reg_out(CMD, MDDI_CMD_HIBERNATE | 1);
+                        spin_unlock_irqrestore(&mddi_host_spin_lock, flags);
+                        pmhctl->disable_hibernation = FALSE;
+                }
+        }
+}
+
+void mddi_mhctl_remove(mddi_host_type host_idx)
+{
+        mddi_host_cntl_type *pmhctl;
+
+        pmhctl = &(mhctl[host_idx]);
+
+        dma_free_coherent(NULL, MDDI_LLIST_POOL_SIZE, (void *)pmhctl->llist_ptr,
+                          pmhctl->llist_dma_addr);
+
+        dma_free_coherent(NULL, MDDI_MAX_REV_DATA_SIZE,
+                          (void *)pmhctl->rev_data_buf,
+                          pmhctl->rev_data_dma_addr);
+}
diff --git a/drivers/staging/msm/mddihosti.h b/drivers/staging/msm/mddihosti.h
new file mode 100644
index 000000000000..7b26a4253896
--- /dev/null
+++ b/drivers/staging/msm/mddihosti.h
@@ -0,0 +1,547 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Code Aurora nor
+ *       the names of its contributors may be used to endorse or promote
+ *       products derived from this software without specific prior written
+ *       permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NON-INFRINGEMENT ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef MDDIHOSTI_H
+#define MDDIHOSTI_H
+
+#include "msm_fb.h"
+#include "mddihost.h"
+#include <linux/clk.h>
+
+/* Register offsets in MDDI, applies to both msm_pmdh_base and
+ * (u32)msm_emdh_base. */
+#define MDDI_CMD                0x0000
+#define MDDI_VERSION            0x0004
+#define MDDI_PRI_PTR            0x0008
+#define MDDI_BPS                0x0010
+#define MDDI_SPM                0x0014
+#define MDDI_INT                0x0018
+#define MDDI_INTEN              0x001c
+#define MDDI_REV_PTR            0x0020
+#define MDDI_REV_SIZE           0x0024
+#define MDDI_STAT               0x0028
+#define MDDI_REV_RATE_DIV       0x002c
+#define MDDI_REV_CRC_ERR        0x0030
+#define MDDI_TA1_LEN            0x0034
+#define MDDI_TA2_LEN            0x0038
+#define MDDI_TEST               0x0040
+#define MDDI_REV_PKT_CNT        0x0044
+#define MDDI_DRIVE_HI           0x0048
+#define MDDI_DRIVE_LO           0x004c
+#define MDDI_DISP_WAKE          0x0050
+#define MDDI_REV_ENCAP_SZ       0x0054
+#define MDDI_RTD_VAL            0x0058
+#define MDDI_PAD_CTL            0x0068
+#define MDDI_DRIVER_START_CNT   0x006c
+#define MDDI_CORE_VER           0x008c
+#define MDDI_FIFO_ALLOC         0x0090
+#define MDDI_PAD_IO_CTL         0x00a0
+#define MDDI_PAD_CAL            0x00a4
+
+extern u32 mddi_msg_level;
+
+/* No longer need to write to clear these registers */
+#define xxxx_mddi_host_reg_outm(reg, mask, val)  \
+do { \
+        if (host_idx == MDDI_HOST_PRIM) \
+                mddi_host_reg_outm_pmdh(reg, mask, val); \
+        else \
+                mddi_host_reg_outm_emdh(reg, mask, val); \
+} while (0)
+
+#define mddi_host_reg_outm(reg, mask, val) \
+do { \
+        unsigned long __addr; \
+        if (host_idx == MDDI_HOST_PRIM) \
+                __addr = (u32)msm_pmdh_base + MDDI_##reg; \
+        else \
+                __addr = (u32)msm_emdh_base + MDDI_##reg; \
+        writel((readl(__addr) & ~(mask)) | ((val) & (mask)), __addr); \
+} while (0)
+
+#define xxxx_mddi_host_reg_out(reg, val) \
+do { \
+        if (host_idx == MDDI_HOST_PRIM)  \
+                mddi_host_reg_out_pmdh(reg, val); \
+        else \
+                mddi_host_reg_out_emdh(reg, val); \
+        } while (0)
+
+#define mddi_host_reg_out(reg, val) \
+do { \
+        if (host_idx == MDDI_HOST_PRIM) \
+                writel(val, (u32)msm_pmdh_base + MDDI_##reg); \
+        else \
+                writel(val, (u32)msm_emdh_base + MDDI_##reg); \
+} while (0)
+
+#define xxxx_mddi_host_reg_in(reg)  \
+  ((host_idx) ? \
+     mddi_host_reg_in_emdh(reg) : mddi_host_reg_in_pmdh(reg));
+
+#define mddi_host_reg_in(reg) \
+((host_idx) ? \
+        readl((u32)msm_emdh_base + MDDI_##reg) : \
+        readl((u32)msm_pmdh_base + MDDI_##reg)) \
+
+#define xxxx_mddi_host_reg_inm(reg, mask)  \
+  ((host_idx) ? \
+    mddi_host_reg_inm_emdh(reg, mask) : \
+    mddi_host_reg_inm_pmdh(reg, mask);)
+
+#define mddi_host_reg_inm(reg, mask) \
+((host_idx) ? \
+        readl((u32)msm_emdh_base + MDDI_##reg) & (mask) : \
+        readl((u32)msm_pmdh_base + MDDI_##reg) & (mask)) \
+
+/* Using non-cacheable pmem, so do nothing */
+#define mddi_invalidate_cache_lines(addr_start, num_bytes)
+/*
+ * Using non-cacheable pmem, so do nothing with cache
+ * but, ensure write goes out to memory
+ */
+#define mddi_flush_cache_lines(addr_start, num_bytes)  \
+    (void) addr_start; \
+    (void) num_bytes;  \
+    memory_barrier()
+
+/* Since this translates to Remote Procedure Calls to check on clock status
+* just use a local variable to keep track of io_clock */
+#define MDDI_HOST_IS_IO_CLOCK_ON mddi_host_io_clock_on
+#define MDDI_HOST_ENABLE_IO_CLOCK
+#define MDDI_HOST_DISABLE_IO_CLOCK
+#define MDDI_HOST_IS_HCLK_ON mddi_host_hclk_on
+#define MDDI_HOST_ENABLE_HCLK
+#define MDDI_HOST_DISABLE_HCLK
+#define FEATURE_MDDI_HOST_IO_CLOCK_CONTROL_DISABLE
+#define FEATURE_MDDI_HOST_HCLK_CONTROL_DISABLE
+
+#define TRAMP_MDDI_HOST_ISR TRAMP_MDDI_PRI_ISR
+#define TRAMP_MDDI_HOST_EXT_ISR TRAMP_MDDI_EXT_ISR
+#define MDP_LINE_COUNT_BMSK  0x3ff
+#define MDP_SYNC_STATUS  0x000c
+#define MDP_LINE_COUNT      \
+(readl(msm_mdp_base + MDP_SYNC_STATUS) & MDP_LINE_COUNT_BMSK)
+
+/* MDP sends 256 pixel packets, so lower value hibernates more without
+* significantly increasing latency of waiting for next subframe */
+#define MDDI_HOST_BYTES_PER_SUBFRAME  0x3C00
+
+#if defined(CONFIG_FB_MSM_MDP31) || defined(CONFIG_FB_MSM_MDP40)
+#define MDDI_HOST_TA2_LEN       0x001a
+#define MDDI_HOST_REV_RATE_DIV  0x0004
+#else
+#define MDDI_HOST_TA2_LEN       0x000c
+#define MDDI_HOST_REV_RATE_DIV  0x0002
+#endif
+
+#define MDDI_MSG_EMERG(msg, ...)    \
+        if (mddi_msg_level > 0)  \
+                printk(KERN_EMERG msg, ## __VA_ARGS__);
+#define MDDI_MSG_ALERT(msg, ...)    \
+        if (mddi_msg_level > 1)  \
+                printk(KERN_ALERT msg, ## __VA_ARGS__);
+#define MDDI_MSG_CRIT(msg, ...)    \
+        if (mddi_msg_level > 2)  \
+                printk(KERN_CRIT msg, ## __VA_ARGS__);
+#define MDDI_MSG_ERR(msg, ...)    \
+        if (mddi_msg_level > 3)  \
+                printk(KERN_ERR msg, ## __VA_ARGS__);
+#define MDDI_MSG_WARNING(msg, ...)    \
+        if (mddi_msg_level > 4)  \
+                printk(KERN_WARNING msg, ## __VA_ARGS__);
+#define MDDI_MSG_NOTICE(msg, ...)    \
+        if (mddi_msg_level > 5)  \
+                printk(KERN_NOTICE msg, ## __VA_ARGS__);
+#define MDDI_MSG_INFO(msg, ...)    \
+        if (mddi_msg_level > 6)  \
+                printk(KERN_INFO msg, ## __VA_ARGS__);
+#define MDDI_MSG_DEBUG(msg, ...)    \
+        if (mddi_msg_level > 7)  \
+                printk(KERN_DEBUG msg, ## __VA_ARGS__);
+
+#define GCC_PACKED __attribute__((packed))
+typedef struct GCC_PACKED {
+        uint16 packet_length;
+        /* total # of bytes in the packet not including
+                the packet_length field. */
+
+        uint16 packet_type;
+        /* A Packet Type of 70 identifies the packet as
+                a Client status Packet. */
+
+        uint16 bClient_ID;
+        /* This field is reserved for future use and shall
+                be set to zero. */
+
+} mddi_rev_packet_type;
+
+typedef struct GCC_PACKED {
+        uint16 packet_length;
+        /* total # of bytes in the packet not including
+                the packet_length field. */
+
+        uint16 packet_type;
+        /* A Packet Type of 70 identifies the packet as
+                a Client status Packet. */
+
+        uint16 bClient_ID;
+        /* This field is reserved for future use and shall
+                be set to zero. */
+
+        uint16 reverse_link_request;
+        /* 16 bit unsigned integer with number of bytes client
+                needs in the * reverse encapsulation message
+                to transmit data. */
+
+        uint8 crc_error_count;
+        uint8 capability_change;
+        uint16 graphics_busy_flags;
+
+        uint16 parameter_CRC;
+        /* 16-bit CRC of all the bytes in the packet
+                including Packet Length. */
+
+} mddi_client_status_type;
+
+typedef struct GCC_PACKED {
+        uint16 packet_length;
+        /* total # of bytes in the packet not including
+                the packet_length field. */
+
+        uint16 packet_type;
+        /* A Packet Type of 66 identifies the packet as
+                a Client Capability Packet. */
+
+        uint16 bClient_ID;
+        /* This field is reserved for future use and
+                shall be set to zero. */
+
+        uint16 Protocol_Version;
+        uint16 Minimum_Protocol_Version;
+        uint16 Data_Rate_Capability;
+        uint8 Interface_Type_Capability;
+        uint8 Number_of_Alt_Displays;
+        uint16 PostCal_Data_Rate;
+        uint16 Bitmap_Width;
+        uint16 Bitmap_Height;
+        uint16 Display_Window_Width;
+        uint16 Display_Window_Height;
+        uint32 Color_Map_Size;
+        uint16 Color_Map_RGB_Width;
+        uint16 RGB_Capability;
+        uint8 Monochrome_Capability;
+        uint8 Reserved_1;
+        uint16 Y_Cb_Cr_Capability;
+        uint16 Bayer_Capability;
+        uint16 Alpha_Cursor_Image_Planes;
+        uint32 Client_Feature_Capability_Indicators;
+        uint8 Maximum_Video_Frame_Rate_Capability;
+        uint8 Minimum_Video_Frame_Rate_Capability;
+        uint16 Minimum_Sub_frame_Rate;
+        uint16 Audio_Buffer_Depth;
+        uint16 Audio_Channel_Capability;
+        uint16 Audio_Sample_Rate_Capability;
+        uint8 Audio_Sample_Resolution;
+        uint8 Mic_Audio_Sample_Resolution;
+        uint16 Mic_Sample_Rate_Capability;
+        uint8 Keyboard_Data_Format;
+        uint8 pointing_device_data_format;
+        uint16 content_protection_type;
+        uint16 Mfr_Name;
+        uint16 Product_Code;
+        uint16 Reserved_3;
+        uint32 Serial_Number;
+        uint8 Week_of_Manufacture;
+        uint8 Year_of_Manufacture;
+
+        uint16 parameter_CRC;
+        /* 16-bit CRC of all the bytes in the packet including Packet Length. */
+
+} mddi_client_capability_type;
+
+typedef struct GCC_PACKED {
+        uint16 packet_length;
+        /* total # of bytes in the packet not including the packet_length field. */
+
+        uint16 packet_type;
+        /* A Packet Type of 16 identifies the packet as a Video Stream Packet. */
+
+        uint16 bClient_ID;
+        /* This field is reserved for future use and shall be set to zero. */
+
+        uint16 video_data_format_descriptor;
+        /* format of each pixel in the Pixel Data in the present stream in the
+         * present packet.
+         * If bits [15:13] = 000 monochrome
+         * If bits [15:13] = 001 color pixels (palette).
+         * If bits [15:13] = 010 color pixels in raw RGB
+         * If bits [15:13] = 011 data in 4:2:2 Y Cb Cr format
+         * If bits [15:13] = 100 Bayer pixels
+         */
+
+        uint16 pixel_data_attributes;
+        /* interpreted as follows:
+         * Bits [1:0] = 11  pixel data is displayed to both eyes
+         * Bits [1:0] = 10  pixel data is routed to the left eye only.
+         * Bits [1:0] = 01  pixel data is routed to the right eye only.
+         * Bits [1:0] = 00  pixel data is routed to the alternate display.
+         * Bit 2 is 0  Pixel Data is in the standard progressive format.
+         * Bit 2 is 1  Pixel Data is in interlace format.
+         * Bit 3 is 0  Pixel Data is in the standard progressive format.
+         * Bit 3 is 1  Pixel Data is in alternate pixel format.
+         * Bit 4 is 0  Pixel Data is to or from the display frame buffer.
+         * Bit 4 is 1  Pixel Data is to or from the camera.
+         * Bit 5 is 0  pixel data contains the next consecutive row of pixels.
+         * Bit 5 is 1  X Left Edge, Y Top Edge, X Right Edge, Y Bottom Edge,
+         *             X Start, and Y Start parameters are not defined and
+         *             shall be ignored by the client.
+         * Bits [7:6] = 01  Pixel data is written to the offline image buffer.
+         * Bits [7:6] = 00  Pixel data is written to the buffer to refresh display.
+         * Bits [7:6] = 11  Pixel data is written to all image buffers.
+         * Bits [7:6] = 10  Invalid. Reserved for future use.
+         * Bits 8 through 11 alternate display number.
+         * Bits 12 through 14 are reserved for future use and shall be set to zero.
+         * Bit 15 is 1 the row of pixels is the last row of pixels in a frame.
+         */
+
+        uint16 x_left_edge;
+        uint16 y_top_edge;
+        /* X,Y coordinate of the top left edge of the screen window */
+
+        uint16 x_right_edge;
+        uint16 y_bottom_edge;
+        /*  X,Y coordinate of the bottom right edge of the window being updated. */
+
+        uint16 x_start;
+        uint16 y_start;
+        /*  (X Start, Y Start) is the first pixel in the Pixel Data field below. */
+
+        uint16 pixel_count;
+        /*  number of pixels in the Pixel Data field below. */
+
+        uint16 parameter_CRC;
+        /*  16-bit CRC of all bytes from the Packet Length to the Pixel Count. */
+
+        uint16 reserved;
+        /* 16-bit variable to make structure align on 4 byte boundary */
+
+} mddi_video_stream_packet_type;
+
+typedef struct GCC_PACKED {
+        uint16 packet_length;
+        /* total # of bytes in the packet not including the packet_length field. */
+
+        uint16 packet_type;
+        /* A Packet Type of 146 identifies the packet as a Register Access Packet. */
+
+        uint16 bClient_ID;
+        /* This field is reserved for future use and shall be set to zero. */
+
+        uint16 read_write_info;
+        /* Bits 13:0  a 14-bit unsigned integer that specifies the number of
+         *            32-bit Register Data List items to be transferred in the
+         *            Register Data List field.
+         * Bits[15:14] = 00  Write to register(s);
+         * Bits[15:14] = 10  Read from register(s);
+         * Bits[15:14] = 11  Response to a Read.
+         * Bits[15:14] = 01  this value is reserved for future use. */
+
+        uint32 register_address;
+        /* the register address that is to be written to or read from. */
+
+        uint16 parameter_CRC;
+        /* 16-bit CRC of all bytes from the Packet Length to the Register Address. */
+
+        uint32 register_data_list;
+        /* list of 4-byte register data values for/from client registers */
+
+} mddi_register_access_packet_type;
+
+typedef union GCC_PACKED {
+        mddi_video_stream_packet_type video_pkt;
+        mddi_register_access_packet_type register_pkt;
+        /* add 48 byte pad to ensure 64 byte llist struct, that can be
+         * manipulated easily with cache */
+        uint32 alignment_pad[12];       /* 48 bytes */
+} mddi_packet_header_type;
+
+typedef struct GCC_PACKED mddi_host_llist_struct {
+        uint16 link_controller_flags;
+        uint16 packet_header_count;
+        uint16 packet_data_count;
+        void *packet_data_pointer;
+        struct mddi_host_llist_struct *next_packet_pointer;
+        uint16 reserved;
+        mddi_packet_header_type packet_header;
+} mddi_linked_list_type;
+
+typedef struct {
+        struct completion done_comp;
+        mddi_llist_done_cb_type done_cb;
+        uint16 next_idx;
+        boolean waiting;
+        boolean in_use;
+} mddi_linked_list_notify_type;
+
+#define MDDI_LLIST_POOL_SIZE 0x1000
+#define MDDI_MAX_NUM_LLIST_ITEMS (MDDI_LLIST_POOL_SIZE / \
+                 sizeof(mddi_linked_list_type))
+#define UNASSIGNED_INDEX MDDI_MAX_NUM_LLIST_ITEMS
+#define MDDI_FIRST_DYNAMIC_LLIST_IDX 0
+
+/* Static llist items can be used for applications that frequently send
+ * the same set of packets using the linked list interface. */
+/* Here we configure for 6 static linked list items:
+ *  The 1st is used for a the adaptive backlight setting.
+ *  and the remaining 5 are used for sending window adjustments for
+ *  MDDI clients that need windowing info sent separate from video
+ *  packets. */
+#define MDDI_NUM_STATIC_ABL_ITEMS 1
+#define MDDI_NUM_STATIC_WINDOW_ITEMS 5
+#define MDDI_NUM_STATIC_LLIST_ITEMS (MDDI_NUM_STATIC_ABL_ITEMS + \
+                                MDDI_NUM_STATIC_WINDOW_ITEMS)
+#define MDDI_NUM_DYNAMIC_LLIST_ITEMS (MDDI_MAX_NUM_LLIST_ITEMS - \
+                                MDDI_NUM_STATIC_LLIST_ITEMS)
+
+#define MDDI_FIRST_STATIC_LLIST_IDX  MDDI_NUM_DYNAMIC_LLIST_ITEMS
+#define MDDI_FIRST_STATIC_ABL_IDX  MDDI_FIRST_STATIC_LLIST_IDX
+#define MDDI_FIRST_STATIC_WINDOW_IDX  (MDDI_FIRST_STATIC_LLIST_IDX + \
+                                MDDI_NUM_STATIC_ABL_ITEMS)
+
+/* GPIO registers */
+#define VSYNC_WAKEUP_REG          0x80
+#define GPIO_REG                  0x81
+#define GPIO_OUTPUT_REG           0x82
+#define GPIO_INTERRUPT_REG        0x83
+#define GPIO_INTERRUPT_ENABLE_REG 0x84
+#define GPIO_POLARITY_REG         0x85
+
+/* Interrupt Bits */
+#define MDDI_INT_PRI_PTR_READ       0x0001
+#define MDDI_INT_SEC_PTR_READ       0x0002
+#define MDDI_INT_REV_DATA_AVAIL     0x0004
+#define MDDI_INT_DISP_REQ           0x0008
+#define MDDI_INT_PRI_UNDERFLOW      0x0010
+#define MDDI_INT_SEC_UNDERFLOW      0x0020
+#define MDDI_INT_REV_OVERFLOW       0x0040
+#define MDDI_INT_CRC_ERROR          0x0080
+#define MDDI_INT_MDDI_IN            0x0100
+#define MDDI_INT_PRI_OVERWRITE      0x0200
+#define MDDI_INT_SEC_OVERWRITE      0x0400
+#define MDDI_INT_REV_OVERWRITE      0x0800
+#define MDDI_INT_DMA_FAILURE        0x1000
+#define MDDI_INT_LINK_ACTIVE        0x2000
+#define MDDI_INT_IN_HIBERNATION     0x4000
+#define MDDI_INT_PRI_LINK_LIST_DONE 0x8000
+#define MDDI_INT_SEC_LINK_LIST_DONE 0x10000
+#define MDDI_INT_NO_CMD_PKTS_PEND   0x20000
+#define MDDI_INT_RTD_FAILURE        0x40000
+
+#define MDDI_INT_ERROR_CONDITIONS ( \
+        MDDI_INT_PRI_UNDERFLOW | MDDI_INT_SEC_UNDERFLOW | \
+        MDDI_INT_REV_OVERFLOW | MDDI_INT_CRC_ERROR | \
+        MDDI_INT_PRI_OVERWRITE | MDDI_INT_SEC_OVERWRITE | \
+        MDDI_INT_RTD_FAILURE | \
+        MDDI_INT_REV_OVERWRITE | MDDI_INT_DMA_FAILURE)
+
+#define MDDI_INT_LINK_STATE_CHANGES ( \
+        MDDI_INT_LINK_ACTIVE | MDDI_INT_IN_HIBERNATION)
+
+/* Status Bits */
+#define MDDI_STAT_LINK_ACTIVE        0x0001
+#define MDDI_STAT_NEW_REV_PTR        0x0002
+#define MDDI_STAT_NEW_PRI_PTR        0x0004
+#define MDDI_STAT_NEW_SEC_PTR        0x0008
+#define MDDI_STAT_IN_HIBERNATION     0x0010
+#define MDDI_STAT_PRI_LINK_LIST_DONE 0x0020
+#define MDDI_STAT_SEC_LINK_LIST_DONE 0x0040
+#define MDDI_STAT_PENDING_TIMING_PKT 0x0080
+#define MDDI_STAT_PENDING_REV_ENCAP  0x0100
+#define MDDI_STAT_PENDING_POWERDOWN  0x0200
+#define MDDI_STAT_RTD_MEAS_FAIL      0x0800
+#define MDDI_STAT_CLIENT_WAKEUP_REQ  0x1000
+
+/* Command Bits */
+#define MDDI_CMD_POWERDOWN           0x0100
+#define MDDI_CMD_POWERUP             0x0200
+#define MDDI_CMD_HIBERNATE           0x0300
+#define MDDI_CMD_RESET               0x0400
+#define MDDI_CMD_DISP_IGNORE         0x0501
+#define MDDI_CMD_DISP_LISTEN         0x0500
+#define MDDI_CMD_SEND_REV_ENCAP      0x0600
+#define MDDI_CMD_GET_CLIENT_CAP      0x0601
+#define MDDI_CMD_GET_CLIENT_STATUS   0x0602
+#define MDDI_CMD_SEND_RTD            0x0700
+#define MDDI_CMD_LINK_ACTIVE         0x0900
+#define MDDI_CMD_PERIODIC_REV_ENCAP  0x0A00
+
+extern void mddi_host_init(mddi_host_type host);
+extern void mddi_host_powerdown(mddi_host_type host);
+extern uint16 mddi_get_next_free_llist_item(mddi_host_type host, boolean wait);
+extern uint16 mddi_get_reg_read_llist_item(mddi_host_type host, boolean wait);
+extern void mddi_queue_forward_packets(uint16 first_llist_idx,
+                                       uint16 last_llist_idx,
+                                       boolean wait,
+                                       mddi_llist_done_cb_type llist_done_cb,
+                                       mddi_host_type host);
+
+extern void mddi_host_write_pix_attr_reg(uint32 value);
+extern void mddi_client_lcd_gpio_poll(uint32 poll_reg_val);
+extern void mddi_client_lcd_vsync_detected(boolean detected);
+extern void mddi_host_disable_hibernation(boolean disable);
+
+extern mddi_linked_list_type *llist_extern[];
+extern mddi_linked_list_type *llist_dma_extern[];
+extern mddi_linked_list_notify_type *llist_extern_notify[];
+extern struct timer_list mddi_host_timer;
+
+typedef struct {
+        uint16 transmitting_start_idx;
+        uint16 transmitting_end_idx;
+        uint16 waiting_start_idx;
+        uint16 waiting_end_idx;
+        uint16 reg_read_idx;
+        uint16 next_free_idx;
+        boolean reg_read_waiting;
+} mddi_llist_info_type;
+
+extern mddi_llist_info_type mddi_llist;
+
+#define MDDI_GPIO_DEFAULT_POLLING_INTERVAL 200
+typedef struct {
+        uint32 polling_reg;
+        uint32 polling_val;
+        uint32 polling_interval;
+        boolean polling_enabled;
+} mddi_gpio_info_type;
+
+uint32 mddi_get_client_id(void);
+void mddi_mhctl_remove(mddi_host_type host_idx);
+void mddi_host_timer_service(unsigned long data);
+#endif /* MDDIHOSTI_H */
diff --git a/drivers/staging/msm/mdp.c b/drivers/staging/msm/mdp.c
new file mode 100644
index 000000000000..36053afdebe2
--- /dev/null
+++ b/drivers/staging/msm/mdp.c
@@ -0,0 +1,1113 @@
+/* Copyright (c) 2008-2010, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/hrtimer.h>
+#include <linux/clk.h>
+#include <mach/hardware.h>
+#include <linux/io.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/uaccess.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+#ifdef CONFIG_FB_MSM_MDP40
+#include "mdp4.h"
+#endif
+
+static struct clk *mdp_clk;
+static struct clk *mdp_pclk;
+
+struct completion mdp_ppp_comp;
+struct semaphore mdp_ppp_mutex;
+struct semaphore mdp_pipe_ctrl_mutex;
+
+unsigned long mdp_timer_duration = (HZ);   /* 1 sec */
+/* unsigned long mdp_mdp_timer_duration=0; */
+
+boolean mdp_ppp_waiting = FALSE;
+uint32 mdp_tv_underflow_cnt;
+uint32 mdp_lcdc_underflow_cnt;
+
+boolean mdp_current_clk_on = FALSE;
+boolean mdp_is_in_isr = FALSE;
+
+/*
+ * legacy mdp_in_processing is only for DMA2-MDDI
+ * this applies to DMA2 block only
+ */
+uint32 mdp_in_processing = FALSE;
+
+#ifdef CONFIG_FB_MSM_MDP40
+uint32 mdp_intr_mask = MDP4_ANY_INTR_MASK;
+#else
+uint32 mdp_intr_mask = MDP_ANY_INTR_MASK;
+#endif
+
+MDP_BLOCK_TYPE mdp_debug[MDP_MAX_BLOCK];
+
+int32 mdp_block_power_cnt[MDP_MAX_BLOCK];
+
+spinlock_t mdp_spin_lock;
+struct workqueue_struct *mdp_dma_wq;    /*mdp dma wq */
+struct workqueue_struct *mdp_vsync_wq;  /*mdp vsync wq */
+
+static struct workqueue_struct *mdp_pipe_ctrl_wq; /* mdp mdp pipe ctrl wq */
+static struct delayed_work mdp_pipe_ctrl_worker;
+
+#ifdef CONFIG_FB_MSM_MDP40
+struct mdp_dma_data dma2_data;
+struct mdp_dma_data dma_s_data;
+struct mdp_dma_data dma_e_data;
+#else
+static struct mdp_dma_data dma2_data;
+static struct mdp_dma_data dma_s_data;
+static struct mdp_dma_data dma_e_data;
+#endif
+static struct mdp_dma_data dma3_data;
+
+extern ktime_t mdp_dma2_last_update_time;
+
+extern uint32 mdp_dma2_update_time_in_usec;
+extern int mdp_lcd_rd_cnt_offset_slow;
+extern int mdp_lcd_rd_cnt_offset_fast;
+extern int mdp_usec_diff_threshold;
+
+#ifdef CONFIG_FB_MSM_LCDC
+extern int mdp_lcdc_pclk_clk_rate;
+extern int mdp_lcdc_pad_pclk_clk_rate;
+extern int first_pixel_start_x;
+extern int first_pixel_start_y;
+#endif
+
+#ifdef MSM_FB_ENABLE_DBGFS
+struct dentry *mdp_dir;
+#endif
+
+#if defined(CONFIG_PM) && !defined(CONFIG_HAS_EARLYSUSPEND)
+static int mdp_suspend(struct platform_device *pdev, pm_message_t state);
+#else
+#define mdp_suspend NULL
+#endif
+
+struct timeval mdp_dma2_timeval;
+struct timeval mdp_ppp_timeval;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static struct early_suspend early_suspend;
+#endif
+
+#ifndef CONFIG_FB_MSM_MDP22
+DEFINE_MUTEX(mdp_lut_push_sem);
+static int mdp_lut_i;
+static int mdp_lut_hw_update(struct fb_cmap *cmap)
+{
+        int i;
+        u16 *c[3];
+        u16 r, g, b;
+
+        c[0] = cmap->green;
+        c[1] = cmap->blue;
+        c[2] = cmap->red;
+
+        for (i = 0; i < cmap->len; i++) {
+                if (copy_from_user(&r, cmap->red++, sizeof(r)) ||
+                    copy_from_user(&g, cmap->green++, sizeof(g)) ||
+                    copy_from_user(&b, cmap->blue++, sizeof(b)))
+                        return -EFAULT;
+
+#ifdef CONFIG_FB_MSM_MDP40
+                MDP_OUTP(MDP_BASE + 0x94800 +
+#else
+                MDP_OUTP(MDP_BASE + 0x93800 +
+#endif
+                        (0x400*mdp_lut_i) + cmap->start*4 + i*4,
+                                ((g & 0xff) |
+                                 ((b & 0xff) << 8) |
+                                 ((r & 0xff) << 16)));
+        }
+
+        return 0;
+}
+
+static int mdp_lut_push;
+static int mdp_lut_push_i;
+static int mdp_lut_update_nonlcdc(struct fb_info *info, struct fb_cmap *cmap)
+{
+        int ret;
+
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+        ret = mdp_lut_hw_update(cmap);
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+
+        if (ret)
+                return ret;
+
+        mutex_lock(&mdp_lut_push_sem);
+        mdp_lut_push = 1;
+        mdp_lut_push_i = mdp_lut_i;
+        mutex_unlock(&mdp_lut_push_sem);
+
+        mdp_lut_i = (mdp_lut_i + 1)%2;
+
+        return 0;
+}
+
+static int mdp_lut_update_lcdc(struct fb_info *info, struct fb_cmap *cmap)
+{
+        int ret;
+
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+        ret = mdp_lut_hw_update(cmap);
+
+        if (ret) {
+                mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+                return ret;
+        }
+
+        MDP_OUTP(MDP_BASE + 0x90070, (mdp_lut_i << 10) | 0x17);
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+        mdp_lut_i = (mdp_lut_i + 1)%2;
+
+        return 0;
+}
+
+#define MDP_HIST_MAX_BIN 32
+static __u32 mdp_hist_r[MDP_HIST_MAX_BIN];
+static __u32 mdp_hist_g[MDP_HIST_MAX_BIN];
+static __u32 mdp_hist_b[MDP_HIST_MAX_BIN];
+
+#ifdef CONFIG_FB_MSM_MDP40
+struct mdp_histogram mdp_hist;
+struct completion mdp_hist_comp;
+#else
+static struct mdp_histogram mdp_hist;
+static struct completion mdp_hist_comp;
+#endif
+
+static int mdp_do_histogram(struct fb_info *info, struct mdp_histogram *hist)
+{
+        int ret = 0;
+
+        if (!hist->frame_cnt || (hist->bin_cnt == 0) ||
+                                 (hist->bin_cnt > MDP_HIST_MAX_BIN))
+                return -EINVAL;
+
+        INIT_COMPLETION(mdp_hist_comp);
+
+        mdp_hist.bin_cnt = hist->bin_cnt;
+        mdp_hist.r = (hist->r) ? mdp_hist_r : 0;
+        mdp_hist.g = (hist->g) ? mdp_hist_g : 0;
+        mdp_hist.b = (hist->b) ? mdp_hist_b : 0;
+
+#ifdef CONFIG_FB_MSM_MDP40
+        MDP_OUTP(MDP_BASE + 0x95004, hist->frame_cnt);
+        MDP_OUTP(MDP_BASE + 0x95000, 1);
+#else
+        MDP_OUTP(MDP_BASE + 0x94004, hist->frame_cnt);
+        MDP_OUTP(MDP_BASE + 0x94000, 1);
+#endif
+        wait_for_completion_killable(&mdp_hist_comp);
+
+        if (hist->r) {
+                ret = copy_to_user(hist->r, mdp_hist.r, hist->bin_cnt*4);
+                if (ret)
+                        goto hist_err;
+        }
+        if (hist->g) {
+                ret = copy_to_user(hist->g, mdp_hist.g, hist->bin_cnt*4);
+                if (ret)
+                        goto hist_err;
+        }
+        if (hist->b) {
+                ret = copy_to_user(hist->b, mdp_hist.b, hist->bin_cnt*4);
+                if (ret)
+                        goto hist_err;
+        }
+        return 0;
+
+hist_err:
+        printk(KERN_ERR "%s: invalid hist buffer\n", __func__);
+        return ret;
+}
+#endif
+
+/* Returns < 0 on error, 0 on timeout, or > 0 on successful wait */
+
+int mdp_ppp_pipe_wait(void)
+{
+        int ret = 1;
+
+        /* wait 5 seconds for the operation to complete before declaring
+        the MDP hung */
+
+        if (mdp_ppp_waiting == TRUE) {
+                ret = wait_for_completion_interruptible_timeout(&mdp_ppp_comp,
+                                                                5 * HZ);
+
+                if (!ret)
+                        printk(KERN_ERR "%s: Timed out waiting for the MDP.\n",
+                                        __func__);
+        }
+
+        return ret;
+}
+
+static DEFINE_SPINLOCK(mdp_lock);
+static int mdp_irq_mask;
+static int mdp_irq_enabled;
+
+void mdp_enable_irq(uint32 term)
+{
+        unsigned long irq_flags;
+
+        spin_lock_irqsave(&mdp_lock, irq_flags);
+        if (mdp_irq_mask & term) {
+                printk(KERN_ERR "MDP IRQ term-0x%x is already set\n", term);
+        } else {
+                mdp_irq_mask |= term;
+                if (mdp_irq_mask && !mdp_irq_enabled) {
+                        mdp_irq_enabled = 1;
+                        enable_irq(INT_MDP);
+                }
+        }
+        spin_unlock_irqrestore(&mdp_lock, irq_flags);
+}
+
+void mdp_disable_irq(uint32 term)
+{
+        unsigned long irq_flags;
+
+        spin_lock_irqsave(&mdp_lock, irq_flags);
+        if (!(mdp_irq_mask & term)) {
+                printk(KERN_ERR "MDP IRQ term-0x%x is not set\n", term);
+        } else {
+                mdp_irq_mask &= ~term;
+                if (!mdp_irq_mask && mdp_irq_enabled) {
+                        mdp_irq_enabled = 0;
+                        disable_irq(INT_MDP);
+                }
+        }
+        spin_unlock_irqrestore(&mdp_lock, irq_flags);
+}
+
+void mdp_disable_irq_nolock(uint32 term)
+{
+
+        if (!(mdp_irq_mask & term)) {
+                printk(KERN_ERR "MDP IRQ term-0x%x is not set\n", term);
+        } else {
+                mdp_irq_mask &= ~term;
+                if (!mdp_irq_mask && mdp_irq_enabled) {
+                        mdp_irq_enabled = 0;
+                        disable_irq(INT_MDP);
+                }
+        }
+}
+
+void mdp_pipe_kickoff(uint32 term, struct msm_fb_data_type *mfd)
+{
+
+        dmb();  /* memory barrier */
+
+        /* kick off PPP engine */
+        if (term == MDP_PPP_TERM) {
+                if (mdp_debug[MDP_PPP_BLOCK])
+                        jiffies_to_timeval(jiffies, &mdp_ppp_timeval);
+
+                /* let's turn on PPP block */
+                mdp_pipe_ctrl(MDP_PPP_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+                mdp_enable_irq(term);
+                INIT_COMPLETION(mdp_ppp_comp);
+                mdp_ppp_waiting = TRUE;
+                outpdw(MDP_BASE + 0x30, 0x1000);
+                wait_for_completion_killable(&mdp_ppp_comp);
+                mdp_disable_irq(term);
+
+                if (mdp_debug[MDP_PPP_BLOCK]) {
+                        struct timeval now;
+
+                        jiffies_to_timeval(jiffies, &now);
+                        mdp_ppp_timeval.tv_usec =
+                            now.tv_usec - mdp_ppp_timeval.tv_usec;
+                        MSM_FB_INFO("MDP-PPP: %d\n",
+                                    (int)mdp_ppp_timeval.tv_usec);
+                }
+        } else if (term == MDP_DMA2_TERM) {
+                if (mdp_debug[MDP_DMA2_BLOCK]) {
+                        MSM_FB_INFO("MDP-DMA2: %d\n",
+                                    (int)mdp_dma2_timeval.tv_usec);
+                        jiffies_to_timeval(jiffies, &mdp_dma2_timeval);
+                }
+                /* DMA update timestamp */
+                mdp_dma2_last_update_time = ktime_get_real();
+                /* let's turn on DMA2 block */
+#if 0
+                mdp_pipe_ctrl(MDP_DMA2_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+#endif
+#ifdef CONFIG_FB_MSM_MDP22
+                outpdw(MDP_CMD_DEBUG_ACCESS_BASE + 0x0044, 0x0);/* start DMA */
+#else
+                if (mdp_lut_push) {
+                        mutex_lock(&mdp_lut_push_sem);
+                        mdp_lut_push = 0;
+                        MDP_OUTP(MDP_BASE + 0x90070,
+                                        (mdp_lut_push_i << 10) | 0x17);
+                        mutex_unlock(&mdp_lut_push_sem);
+                }
+#ifdef CONFIG_FB_MSM_MDP40
+                outpdw(MDP_BASE + 0x000c, 0x0); /* start DMA */
+#else
+                outpdw(MDP_BASE + 0x0044, 0x0); /* start DMA */
+#endif
+#endif
+#ifdef CONFIG_FB_MSM_MDP40
+        } else if (term == MDP_DMA_S_TERM) {
+                mdp_pipe_ctrl(MDP_DMA_S_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+                outpdw(MDP_BASE + 0x0010, 0x0); /* start DMA */
+        } else if (term == MDP_DMA_E_TERM) {
+                mdp_pipe_ctrl(MDP_DMA_E_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+                outpdw(MDP_BASE + 0x0014, 0x0); /* start DMA */
+        } else if (term == MDP_OVERLAY0_TERM) {
+                mdp_pipe_ctrl(MDP_OVERLAY0_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+                outpdw(MDP_BASE + 0x0004, 0);
+        } else if (term == MDP_OVERLAY1_TERM) {
+                mdp_pipe_ctrl(MDP_OVERLAY1_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+                outpdw(MDP_BASE + 0x0008, 0);
+        }
+#else
+        } else if (term == MDP_DMA_S_TERM) {
+                mdp_pipe_ctrl(MDP_DMA_S_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+                outpdw(MDP_BASE + 0x0048, 0x0); /* start DMA */
+        }
+#endif
+}
+
+static void mdp_pipe_ctrl_workqueue_handler(struct work_struct *work)
+{
+        mdp_pipe_ctrl(MDP_MASTER_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+}
+
+void mdp_pipe_ctrl(MDP_BLOCK_TYPE block, MDP_BLOCK_POWER_STATE state,
+                   boolean isr)
+{
+        boolean mdp_all_blocks_off = TRUE;
+        int i;
+        unsigned long flag;
+
+        spin_lock_irqsave(&mdp_spin_lock, flag);
+        if (MDP_BLOCK_POWER_ON == state) {
+                mdp_block_power_cnt[block]++;
+
+                if (MDP_DMA2_BLOCK == block)
+                        mdp_in_processing = TRUE;
+        } else {
+                mdp_block_power_cnt[block]--;
+
+                if (mdp_block_power_cnt[block] < 0) {
+                        /*
+                        * Master has to serve a request to power off MDP always
+                        * It also has a timer to power off.  So, in case of
+                        * timer expires first and DMA2 finishes later,
+                        * master has to power off two times
+                        * There shouldn't be multiple power-off request for
+                        * other blocks
+                        */
+                        if (block != MDP_MASTER_BLOCK) {
+                                MSM_FB_INFO("mdp_block_power_cnt[block=%d] \
+                                multiple power-off request\n", block);
+                        }
+                        mdp_block_power_cnt[block] = 0;
+                }
+
+                if (MDP_DMA2_BLOCK == block)
+                        mdp_in_processing = FALSE;
+        }
+        spin_unlock_irqrestore(&mdp_spin_lock, flag);
+
+        /*
+         * If it's in isr, we send our request to workqueue.
+         * Otherwise, processing happens in the current context
+         */
+        if (isr) {
+                /* checking all blocks power state */
+                for (i = 0; i < MDP_MAX_BLOCK; i++) {
+                        if (mdp_block_power_cnt[i] > 0)
+                                mdp_all_blocks_off = FALSE;
+                }
+
+                if ((mdp_all_blocks_off) && (mdp_current_clk_on)) {
+                        /* send workqueue to turn off mdp power */
+                        queue_delayed_work(mdp_pipe_ctrl_wq,
+                                           &mdp_pipe_ctrl_worker,
+                                           mdp_timer_duration);
+                }
+        } else {
+                down(&mdp_pipe_ctrl_mutex);
+                /* checking all blocks power state */
+                for (i = 0; i < MDP_MAX_BLOCK; i++) {
+                        if (mdp_block_power_cnt[i] > 0)
+                                mdp_all_blocks_off = FALSE;
+                }
+
+                /*
+                 * find out whether a delayable work item is currently
+                 * pending
+                 */
+
+                if (delayed_work_pending(&mdp_pipe_ctrl_worker)) {
+                        /*
+                         * try to cancel the current work if it fails to
+                         * stop (which means del_timer can't delete it
+                         * from the list, it's about to expire and run),
+                         * we have to let it run. queue_delayed_work won't
+                         * accept the next job which is same as
+                         * queue_delayed_work(mdp_timer_duration = 0)
+                         */
+                        cancel_delayed_work(&mdp_pipe_ctrl_worker);
+                }
+
+                if ((mdp_all_blocks_off) && (mdp_current_clk_on)) {
+                        if (block == MDP_MASTER_BLOCK) {
+                                mdp_current_clk_on = FALSE;
+                                /* turn off MDP clks */
+                                if (mdp_clk != NULL) {
+                                        clk_disable(mdp_clk);
+                                        MSM_FB_DEBUG("MDP CLK OFF\n");
+                                }
+                                if (mdp_pclk != NULL) {
+                                        clk_disable(mdp_pclk);
+                                        MSM_FB_DEBUG("MDP PCLK OFF\n");
+                                }
+                        } else {
+                                /* send workqueue to turn off mdp power */
+                                queue_delayed_work(mdp_pipe_ctrl_wq,
+                                                   &mdp_pipe_ctrl_worker,
+                                                   mdp_timer_duration);
+                        }
+                } else if ((!mdp_all_blocks_off) && (!mdp_current_clk_on)) {
+                        mdp_current_clk_on = TRUE;
+                        /* turn on MDP clks */
+                        if (mdp_clk != NULL) {
+                                clk_enable(mdp_clk);
+                                MSM_FB_DEBUG("MDP CLK ON\n");
+                        }
+                        if (mdp_pclk != NULL) {
+                                clk_enable(mdp_pclk);
+                                MSM_FB_DEBUG("MDP PCLK ON\n");
+                        }
+                }
+                up(&mdp_pipe_ctrl_mutex);
+        }
+}
+
+#ifndef CONFIG_FB_MSM_MDP40
+irqreturn_t mdp_isr(int irq, void *ptr)
+{
+        uint32 mdp_interrupt = 0;
+        struct mdp_dma_data *dma;
+
+        mdp_is_in_isr = TRUE;
+        do {
+                mdp_interrupt = inp32(MDP_INTR_STATUS);
+                outp32(MDP_INTR_CLEAR, mdp_interrupt);
+
+                mdp_interrupt &= mdp_intr_mask;
+
+                if (mdp_interrupt & TV_ENC_UNDERRUN) {
+                        mdp_interrupt &= ~(TV_ENC_UNDERRUN);
+                        mdp_tv_underflow_cnt++;
+                }
+
+                if (!mdp_interrupt)
+                        break;
+
+                /* DMA3 TV-Out Start */
+                if (mdp_interrupt & TV_OUT_DMA3_START) {
+                        /* let's disable TV out interrupt */
+                        mdp_intr_mask &= ~TV_OUT_DMA3_START;
+                        outp32(MDP_INTR_ENABLE, mdp_intr_mask);
+
+                        dma = &dma3_data;
+                        if (dma->waiting) {
+                                dma->waiting = FALSE;
+                                complete(&dma->comp);
+                        }
+                }
+#ifndef CONFIG_FB_MSM_MDP22
+                if (mdp_interrupt & MDP_HIST_DONE) {
+                        outp32(MDP_BASE + 0x94018, 0x3);
+                        outp32(MDP_INTR_CLEAR, MDP_HIST_DONE);
+                        if (mdp_hist.r)
+                                memcpy(mdp_hist.r, MDP_BASE + 0x94100,
+                                                mdp_hist.bin_cnt*4);
+                        if (mdp_hist.g)
+                                memcpy(mdp_hist.g, MDP_BASE + 0x94200,
+                                                mdp_hist.bin_cnt*4);
+                        if (mdp_hist.b)
+                                memcpy(mdp_hist.b, MDP_BASE + 0x94300,
+                                                mdp_hist.bin_cnt*4);
+                        complete(&mdp_hist_comp);
+                }
+
+                /* LCDC UnderFlow */
+                if (mdp_interrupt & LCDC_UNDERFLOW) {
+                        mdp_lcdc_underflow_cnt++;
+                }
+                /* LCDC Frame Start */
+                if (mdp_interrupt & LCDC_FRAME_START) {
+                        /* let's disable LCDC interrupt */
+                        mdp_intr_mask &= ~LCDC_FRAME_START;
+                        outp32(MDP_INTR_ENABLE, mdp_intr_mask);
+
+                        dma = &dma2_data;
+                        if (dma->waiting) {
+                                dma->waiting = FALSE;
+                                complete(&dma->comp);
+                        }
+                }
+
+                /* DMA2 LCD-Out Complete */
+                if (mdp_interrupt & MDP_DMA_S_DONE) {
+                        dma = &dma_s_data;
+                        dma->busy = FALSE;
+                        mdp_pipe_ctrl(MDP_DMA_S_BLOCK, MDP_BLOCK_POWER_OFF,
+                                      TRUE);
+                        complete(&dma->comp);
+                }
+#endif
+
+                /* DMA2 LCD-Out Complete */
+                if (mdp_interrupt & MDP_DMA_P_DONE) {
+                        struct timeval now;
+                        ktime_t now_k;
+
+                        now_k = ktime_get_real();
+                        mdp_dma2_last_update_time.tv.sec =
+                            now_k.tv.sec - mdp_dma2_last_update_time.tv.sec;
+                        mdp_dma2_last_update_time.tv.nsec =
+                            now_k.tv.nsec - mdp_dma2_last_update_time.tv.nsec;
+
+                        if (mdp_debug[MDP_DMA2_BLOCK]) {
+                                jiffies_to_timeval(jiffies, &now);
+                                mdp_dma2_timeval.tv_usec =
+                                    now.tv_usec - mdp_dma2_timeval.tv_usec;
+                        }
+
+                        dma = &dma2_data;
+                        dma->busy = FALSE;
+                        mdp_pipe_ctrl(MDP_DMA2_BLOCK, MDP_BLOCK_POWER_OFF,
+                                      TRUE);
+                        complete(&dma->comp);
+                }
+                /* PPP Complete */
+                if (mdp_interrupt & MDP_PPP_DONE) {
+#ifdef CONFIG_MDP_PPP_ASYNC_OP
+                        mdp_ppp_djob_done();
+#else
+                        mdp_pipe_ctrl(MDP_PPP_BLOCK,
+                                MDP_BLOCK_POWER_OFF, TRUE);
+                        if (mdp_ppp_waiting) {
+                                mdp_ppp_waiting = FALSE;
+                                complete(&mdp_ppp_comp);
+                        }
+#endif
+                }
+        } while (1);
+
+        mdp_is_in_isr = FALSE;
+
+        return IRQ_HANDLED;
+}
+#endif
+
+static void mdp_drv_init(void)
+{
+        int i;
+
+        for (i = 0; i < MDP_MAX_BLOCK; i++) {
+                mdp_debug[i] = 0;
+        }
+
+        /* initialize spin lock and workqueue */
+        spin_lock_init(&mdp_spin_lock);
+        mdp_dma_wq = create_singlethread_workqueue("mdp_dma_wq");
+        mdp_vsync_wq = create_singlethread_workqueue("mdp_vsync_wq");
+        mdp_pipe_ctrl_wq = create_singlethread_workqueue("mdp_pipe_ctrl_wq");
+        INIT_DELAYED_WORK(&mdp_pipe_ctrl_worker,
+                          mdp_pipe_ctrl_workqueue_handler);
+#ifdef CONFIG_MDP_PPP_ASYNC_OP
+        mdp_ppp_dq_init();
+#endif
+
+        /* initialize semaphore */
+        init_completion(&mdp_ppp_comp);
+        init_MUTEX(&mdp_ppp_mutex);
+        init_MUTEX(&mdp_pipe_ctrl_mutex);
+
+        dma2_data.busy = FALSE;
+        dma2_data.waiting = FALSE;
+        init_completion(&dma2_data.comp);
+        init_MUTEX(&dma2_data.mutex);
+        mutex_init(&dma2_data.ov_mutex);
+
+        dma3_data.busy = FALSE;
+        dma3_data.waiting = FALSE;
+        init_completion(&dma3_data.comp);
+        init_MUTEX(&dma3_data.mutex);
+
+        dma_s_data.busy = FALSE;
+        dma_s_data.waiting = FALSE;
+        init_completion(&dma_s_data.comp);
+        init_MUTEX(&dma_s_data.mutex);
+
+        dma_e_data.busy = FALSE;
+        dma_e_data.waiting = FALSE;
+        init_completion(&dma_e_data.comp);
+
+#ifndef CONFIG_FB_MSM_MDP22
+        init_completion(&mdp_hist_comp);
+#endif
+
+        /* initializing mdp power block counter to 0 */
+        for (i = 0; i < MDP_MAX_BLOCK; i++) {
+                mdp_block_power_cnt[i] = 0;
+        }
+
+#ifdef MSM_FB_ENABLE_DBGFS
+        {
+                struct dentry *root;
+                char sub_name[] = "mdp";
+
+                root = msm_fb_get_debugfs_root();
+                if (root != NULL) {
+                        mdp_dir = debugfs_create_dir(sub_name, root);
+
+                        if (mdp_dir) {
+                                msm_fb_debugfs_file_create(mdp_dir,
+                                        "dma2_update_time_in_usec",
+                                        (u32 *) &mdp_dma2_update_time_in_usec);
+                                msm_fb_debugfs_file_create(mdp_dir,
+                                        "vs_rdcnt_slow",
+                                        (u32 *) &mdp_lcd_rd_cnt_offset_slow);
+                                msm_fb_debugfs_file_create(mdp_dir,
+                                        "vs_rdcnt_fast",
+                                        (u32 *) &mdp_lcd_rd_cnt_offset_fast);
+                                msm_fb_debugfs_file_create(mdp_dir,
+                                        "mdp_usec_diff_threshold",
+                                        (u32 *) &mdp_usec_diff_threshold);
+                                msm_fb_debugfs_file_create(mdp_dir,
+                                        "mdp_current_clk_on",
+                                        (u32 *) &mdp_current_clk_on);
+#ifdef CONFIG_FB_MSM_LCDC
+                                msm_fb_debugfs_file_create(mdp_dir,
+                                        "lcdc_start_x",
+                                        (u32 *) &first_pixel_start_x);
+                                msm_fb_debugfs_file_create(mdp_dir,
+                                        "lcdc_start_y",
+                                        (u32 *) &first_pixel_start_y);
+                                msm_fb_debugfs_file_create(mdp_dir,
+                                        "mdp_lcdc_pclk_clk_rate",
+                                        (u32 *) &mdp_lcdc_pclk_clk_rate);
+                                msm_fb_debugfs_file_create(mdp_dir,
+                                        "mdp_lcdc_pad_pclk_clk_rate",
+                                        (u32 *) &mdp_lcdc_pad_pclk_clk_rate);
+#endif
+                        }
+                }
+        }
+#endif
+}
+
+static int mdp_probe(struct platform_device *pdev);
+static int mdp_remove(struct platform_device *pdev);
+
+static struct platform_driver mdp_driver = {
+        .probe = mdp_probe,
+        .remove = mdp_remove,
+#ifndef CONFIG_HAS_EARLYSUSPEND
+        .suspend = mdp_suspend,
+        .resume = NULL,
+#endif
+        .shutdown = NULL,
+        .driver = {
+                /*
+                 * Driver name must match the device name added in
+                 * platform.c.
+                 */
+                .name = "mdp",
+        },
+};
+
+static int mdp_off(struct platform_device *pdev)
+{
+        int ret = 0;
+
+#ifdef MDP_HW_VSYNC
+        struct msm_fb_data_type *mfd = platform_get_drvdata(pdev);
+#endif
+
+        ret = panel_next_off(pdev);
+
+#ifdef MDP_HW_VSYNC
+        mdp_hw_vsync_clk_disable(mfd);
+#endif
+
+        return ret;
+}
+
+static int mdp_on(struct platform_device *pdev)
+{
+#ifdef MDP_HW_VSYNC
+        struct msm_fb_data_type *mfd = platform_get_drvdata(pdev);
+#endif
+
+        int ret = 0;
+
+#ifdef MDP_HW_VSYNC
+        mdp_hw_vsync_clk_enable(mfd);
+#endif
+
+        ret = panel_next_on(pdev);
+
+        return ret;
+}
+
+static int mdp_irq_clk_setup(void)
+{
+        int ret;
+
+#ifdef CONFIG_FB_MSM_MDP40
+        ret = request_irq(INT_MDP, mdp4_isr, IRQF_DISABLED, "MDP", 0);
+#else
+        ret = request_irq(INT_MDP, mdp_isr, IRQF_DISABLED, "MDP", 0);
+#endif
+        if (ret) {
+                printk(KERN_ERR "mdp request_irq() failed!\n");
+                return ret;
+        }
+        disable_irq(INT_MDP);
+
+        mdp_clk = clk_get(NULL, "mdp_clk");
+
+        if (IS_ERR(mdp_clk)) {
+                ret = PTR_ERR(mdp_clk);
+                printk(KERN_ERR "can't get mdp_clk error:%d!\n", ret);
+                free_irq(INT_MDP, 0);
+                return ret;
+        }
+
+        mdp_pclk = clk_get(NULL, "mdp_pclk");
+        if (IS_ERR(mdp_pclk))
+                mdp_pclk = NULL;
+
+
+#ifdef CONFIG_FB_MSM_MDP40
+        /*
+         * mdp_clk should greater than mdp_pclk always
+         */
+        clk_set_rate(mdp_clk, 122880000); /* 122.88 Mhz */
+        printk(KERN_INFO "mdp_clk: mdp_clk=%d mdp_pclk=%d\n",
+                (int)clk_get_rate(mdp_clk), (int)clk_get_rate(mdp_pclk));
+#endif
+
+        return 0;
+}
+
+static struct platform_device *pdev_list[MSM_FB_MAX_DEV_LIST];
+static int pdev_list_cnt;
+static int mdp_resource_initialized;
+static struct msm_panel_common_pdata *mdp_pdata;
+
+static int mdp_probe(struct platform_device *pdev)
+{
+        struct platform_device *msm_fb_dev = NULL;
+        struct msm_fb_data_type *mfd;
+        struct msm_fb_panel_data *pdata = NULL;
+        int rc;
+        resource_size_t  size ;
+#ifdef CONFIG_FB_MSM_MDP40
+        int intf, if_no;
+#else
+        unsigned long flag;
+#endif
+
+        if ((pdev->id == 0) && (pdev->num_resources > 0)) {
+                mdp_pdata = pdev->dev.platform_data;
+
+                size =  resource_size(&pdev->resource[0]);
+                msm_mdp_base = ioremap(pdev->resource[0].start, size);
+
+                MSM_FB_INFO("MDP HW Base phy_Address = 0x%x virt = 0x%x\n",
+                        (int)pdev->resource[0].start, (int)msm_mdp_base);
+
+                if (unlikely(!msm_mdp_base))
+                        return -ENOMEM;
+
+                printk("irq clk setup\n");
+                rc = mdp_irq_clk_setup();
+                printk("irq clk setup done\n");
+                if (rc)
+                        return rc;
+
+                /* initializing mdp hw */
+#ifdef CONFIG_FB_MSM_MDP40
+                mdp4_hw_init();
+#else
+                mdp_hw_init();
+#endif
+
+                mdp_resource_initialized = 1;
+                return 0;
+        }
+
+        if (!mdp_resource_initialized)
+                return -EPERM;
+
+        mfd = platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        if (pdev_list_cnt >= MSM_FB_MAX_DEV_LIST)
+                return -ENOMEM;
+
+        msm_fb_dev = platform_device_alloc("msm_fb", pdev->id);
+        if (!msm_fb_dev)
+                return -ENOMEM;
+
+        /* link to the latest pdev */
+        mfd->pdev = msm_fb_dev;
+
+        /* add panel data */
+        if (platform_device_add_data
+            (msm_fb_dev, pdev->dev.platform_data,
+             sizeof(struct msm_fb_panel_data))) {
+                printk(KERN_ERR "mdp_probe: platform_device_add_data failed!\n");
+                rc = -ENOMEM;
+                goto mdp_probe_err;
+        }
+        /* data chain */
+        pdata = msm_fb_dev->dev.platform_data;
+        pdata->on = mdp_on;
+        pdata->off = mdp_off;
+        pdata->next = pdev;
+
+        switch (mfd->panel.type) {
+        case EXT_MDDI_PANEL:
+        case MDDI_PANEL:
+        case EBI2_PANEL:
+                INIT_WORK(&mfd->dma_update_worker,
+                          mdp_lcd_update_workqueue_handler);
+                INIT_WORK(&mfd->vsync_resync_worker,
+                          mdp_vsync_resync_workqueue_handler);
+                mfd->hw_refresh = FALSE;
+
+                if (mfd->panel.type == EXT_MDDI_PANEL) {
+                        /* 15 fps -> 66 msec */
+                        mfd->refresh_timer_duration = (66 * HZ / 1000);
+                } else {
+                        /* 24 fps -> 42 msec */
+                        mfd->refresh_timer_duration = (42 * HZ / 1000);
+                }
+
+#ifdef CONFIG_FB_MSM_MDP22
+                mfd->dma_fnc = mdp_dma2_update;
+                mfd->dma = &dma2_data;
+#else
+                if (mfd->panel_info.pdest == DISPLAY_1) {
+#ifdef CONFIG_FB_MSM_OVERLAY
+                        mfd->dma_fnc = mdp4_mddi_overlay;
+#else
+                        mfd->dma_fnc = mdp_dma2_update;
+#endif
+                        mfd->dma = &dma2_data;
+                        mfd->lut_update = mdp_lut_update_nonlcdc;
+                        mfd->do_histogram = mdp_do_histogram;
+                } else {
+                        mfd->dma_fnc = mdp_dma_s_update;
+                        mfd->dma = &dma_s_data;
+                }
+#endif
+                if (mdp_pdata)
+                        mfd->vsync_gpio = mdp_pdata->gpio;
+                else
+                        mfd->vsync_gpio = -1;
+
+#ifdef CONFIG_FB_MSM_MDP40
+                if (mfd->panel.type == EBI2_PANEL)
+                        intf = EBI2_INTF;
+                else
+                        intf = MDDI_INTF;
+
+                if (mfd->panel_info.pdest == DISPLAY_1)
+                        if_no = PRIMARY_INTF_SEL;
+                else
+                        if_no = SECONDARY_INTF_SEL;
+
+                mdp4_display_intf_sel(if_no, intf);
+#endif
+                mdp_config_vsync(mfd);
+                break;
+
+        case HDMI_PANEL:
+        case LCDC_PANEL:
+                pdata->on = mdp_lcdc_on;
+                pdata->off = mdp_lcdc_off;
+                mfd->hw_refresh = TRUE;
+                mfd->cursor_update = mdp_hw_cursor_update;
+#ifndef CONFIG_FB_MSM_MDP22
+                mfd->lut_update = mdp_lut_update_lcdc;
+                mfd->do_histogram = mdp_do_histogram;
+#endif
+#ifdef CONFIG_FB_MSM_OVERLAY
+                mfd->dma_fnc = mdp4_lcdc_overlay;
+#else
+                mfd->dma_fnc = mdp_lcdc_update;
+#endif
+
+#ifdef CONFIG_FB_MSM_MDP40
+                if (mfd->panel.type == HDMI_PANEL) {
+                        mfd->dma = &dma_e_data;
+                        mdp4_display_intf_sel(EXTERNAL_INTF_SEL, LCDC_RGB_INTF);
+                } else {
+                        mfd->dma = &dma2_data;
+                        mdp4_display_intf_sel(PRIMARY_INTF_SEL, LCDC_RGB_INTF);
+                }
+#else
+                mfd->dma = &dma2_data;
+                spin_lock_irqsave(&mdp_spin_lock, flag);
+                mdp_intr_mask &= ~MDP_DMA_P_DONE;
+                outp32(MDP_INTR_ENABLE, mdp_intr_mask);
+                spin_unlock_irqrestore(&mdp_spin_lock, flag);
+#endif
+                break;
+
+        case TV_PANEL:
+                pdata->on = mdp_dma3_on;
+                pdata->off = mdp_dma3_off;
+                mfd->hw_refresh = TRUE;
+                mfd->dma_fnc = mdp_dma3_update;
+                mfd->dma = &dma3_data;
+                break;
+
+        default:
+                printk(KERN_ERR "mdp_probe: unknown device type!\n");
+                rc = -ENODEV;
+                goto mdp_probe_err;
+        }
+
+        /* set driver data */
+        platform_set_drvdata(msm_fb_dev, mfd);
+
+        rc = platform_device_add(msm_fb_dev);
+        if (rc) {
+                goto mdp_probe_err;
+        }
+
+        pdev_list[pdev_list_cnt++] = pdev;
+        return 0;
+
+      mdp_probe_err:
+        platform_device_put(msm_fb_dev);
+        return rc;
+}
+
+static void mdp_suspend_sub(void)
+{
+        /* cancel pipe ctrl worker */
+        cancel_delayed_work(&mdp_pipe_ctrl_worker);
+
+        /* for workder can't be cancelled... */
+        flush_workqueue(mdp_pipe_ctrl_wq);
+
+        /* let's wait for PPP completion */
+        while (mdp_block_power_cnt[MDP_PPP_BLOCK] > 0) ;
+
+        /* try to power down */
+        mdp_pipe_ctrl(MDP_MASTER_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+}
+
+#if defined(CONFIG_PM) && !defined(CONFIG_HAS_EARLYSUSPEND)
+static int mdp_suspend(struct platform_device *pdev, pm_message_t state)
+{
+        mdp_suspend_sub();
+        return 0;
+}
+#endif
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void mdp_early_suspend(struct early_suspend *h)
+{
+        mdp_suspend_sub();
+}
+#endif
+
+static int mdp_remove(struct platform_device *pdev)
+{
+        iounmap(msm_mdp_base);
+        return 0;
+}
+
+static int mdp_register_driver(void)
+{
+#ifdef CONFIG_HAS_EARLYSUSPEND
+        early_suspend.level = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1;
+        early_suspend.suspend = mdp_early_suspend;
+        register_early_suspend(&early_suspend);
+#endif
+
+        return platform_driver_register(&mdp_driver);
+}
+
+static int __init mdp_driver_init(void)
+{
+        int ret;
+
+        mdp_drv_init();
+
+        ret = mdp_register_driver();
+        if (ret) {
+                printk(KERN_ERR "mdp_register_driver() failed!\n");
+                return ret;
+        }
+
+#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_FB_MSM_MDP40)
+        mdp4_debugfs_init();
+#endif
+
+        return 0;
+
+}
+
+module_init(mdp_driver_init);
diff --git a/drivers/staging/msm/mdp.h b/drivers/staging/msm/mdp.h
new file mode 100644
index 000000000000..0a5d6ac386ac
--- /dev/null
+++ b/drivers/staging/msm/mdp.h
@@ -0,0 +1,695 @@
+/* Copyright (c) 2008-2010, Code Aurora Forum. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Code Aurora nor
+ *       the names of its contributors may be used to endorse or promote
+ *       products derived from this software without specific prior written
+ *       permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NON-INFRINGEMENT ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef MDP_H
+#define MDP_H
+
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/fb.h>
+#include <linux/hrtimer.h>
+#include "msm_mdp.h"
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+
+#include "msm_fb_panel.h"
+
+#ifdef CONFIG_MDP_PPP_ASYNC_OP
+#include "mdp_ppp_dq.h"
+#endif
+
+#ifdef BIT
+#undef BIT
+#endif
+
+#define BIT(x)  (1<<(x))
+
+#define MDPOP_NOP               0
+#define MDPOP_LR                BIT(0)  /* left to right flip */
+#define MDPOP_UD                BIT(1)  /* up and down flip */
+#define MDPOP_ROT90             BIT(2)  /* rotate image to 90 degree */
+#define MDPOP_ROT180            (MDPOP_UD|MDPOP_LR)
+#define MDPOP_ROT270            (MDPOP_ROT90|MDPOP_UD|MDPOP_LR)
+#define MDPOP_ASCALE            BIT(7)
+#define MDPOP_ALPHAB            BIT(8)  /* enable alpha blending */
+#define MDPOP_TRANSP            BIT(9)  /* enable transparency */
+#define MDPOP_DITHER            BIT(10) /* enable dither */
+#define MDPOP_SHARPENING        BIT(11) /* enable sharpening */
+#define MDPOP_BLUR              BIT(12) /* enable blur */
+#define MDPOP_FG_PM_ALPHA       BIT(13)
+
+struct mdp_table_entry {
+        uint32_t reg;
+        uint32_t val;
+};
+
+extern struct mdp_ccs mdp_ccs_yuv2rgb ;
+extern struct mdp_ccs mdp_ccs_rgb2yuv ;
+
+/*
+ * MDP Image Structure
+ */
+typedef struct mdpImg_ {
+        uint32 imgType;         /* Image type */
+        uint32 *bmy_addr;       /* bitmap or y addr */
+        uint32 *cbcr_addr;      /* cbcr addr */
+        uint32 width;           /* image width */
+        uint32 mdpOp;           /* image opertion (rotation,flip up/down, alpha/tp) */
+        uint32 tpVal;           /* transparency color */
+        uint32 alpha;           /* alpha percentage 0%(0x0) ~ 100%(0x100) */
+        int    sp_value;        /* sharpening strength */
+} MDPIMG;
+
+#ifdef CONFIG_MDP_PPP_ASYNC_OP
+#define MDP_OUTP(addr, data)    mdp_ppp_outdw((uint32_t)(addr), \
+                                         (uint32_t)(data))
+#else
+#define MDP_OUTP(addr, data) outpdw((addr), (data))
+#endif
+
+#define MDP_KTIME2USEC(kt) (kt.tv.sec*1000000 + kt.tv.nsec/1000)
+
+#define MDP_BASE msm_mdp_base
+
+typedef enum {
+        MDP_BC_SCALE_POINT2_POINT4,
+        MDP_BC_SCALE_POINT4_POINT6,
+        MDP_BC_SCALE_POINT6_POINT8,
+        MDP_BC_SCALE_POINT8_1,
+        MDP_BC_SCALE_UP,
+        MDP_PR_SCALE_POINT2_POINT4,
+        MDP_PR_SCALE_POINT4_POINT6,
+        MDP_PR_SCALE_POINT6_POINT8,
+        MDP_PR_SCALE_POINT8_1,
+        MDP_PR_SCALE_UP,
+        MDP_SCALE_BLUR,
+        MDP_INIT_SCALE
+} MDP_SCALE_MODE;
+
+typedef enum {
+        MDP_BLOCK_POWER_OFF,
+        MDP_BLOCK_POWER_ON
+} MDP_BLOCK_POWER_STATE;
+
+typedef enum {
+        MDP_MASTER_BLOCK,
+        MDP_CMD_BLOCK,
+        MDP_PPP_BLOCK,
+        MDP_DMA2_BLOCK,
+        MDP_DMA3_BLOCK,
+        MDP_DMA_S_BLOCK,
+        MDP_DMA_E_BLOCK,
+        MDP_OVERLAY0_BLOCK,
+        MDP_OVERLAY1_BLOCK,
+        MDP_MAX_BLOCK
+} MDP_BLOCK_TYPE;
+
+/* Let's keep Q Factor power of 2 for optimization */
+#define MDP_SCALE_Q_FACTOR 512
+
+#ifdef CONFIG_FB_MSM_MDP31
+#define MDP_MAX_X_SCALE_FACTOR (MDP_SCALE_Q_FACTOR*8)
+#define MDP_MIN_X_SCALE_FACTOR (MDP_SCALE_Q_FACTOR/8)
+#define MDP_MAX_Y_SCALE_FACTOR (MDP_SCALE_Q_FACTOR*8)
+#define MDP_MIN_Y_SCALE_FACTOR (MDP_SCALE_Q_FACTOR/8)
+#else
+#define MDP_MAX_X_SCALE_FACTOR (MDP_SCALE_Q_FACTOR*4)
+#define MDP_MIN_X_SCALE_FACTOR (MDP_SCALE_Q_FACTOR/4)
+#define MDP_MAX_Y_SCALE_FACTOR (MDP_SCALE_Q_FACTOR*4)
+#define MDP_MIN_Y_SCALE_FACTOR (MDP_SCALE_Q_FACTOR/4)
+#endif
+
+/* SHIM Q Factor */
+#define PHI_Q_FACTOR          29
+#define PQF_PLUS_5            (PHI_Q_FACTOR + 5)        /* due to 32 phases */
+#define PQF_PLUS_4            (PHI_Q_FACTOR + 4)
+#define PQF_PLUS_2            (PHI_Q_FACTOR + 2)        /* to get 4.0 */
+#define PQF_MINUS_2           (PHI_Q_FACTOR - 2)        /* to get 0.25 */
+#define PQF_PLUS_5_PLUS_2     (PQF_PLUS_5 + 2)
+#define PQF_PLUS_5_MINUS_2    (PQF_PLUS_5 - 2)
+
+#define MDP_CONVTP(tpVal) (((tpVal&0xF800)<<8)|((tpVal&0x7E0)<<5)|((tpVal&0x1F)<<3))
+
+#define MDPOP_ROTATION (MDPOP_ROT90|MDPOP_LR|MDPOP_UD)
+#define MDP_CHKBIT(val, bit) ((bit) == ((val) & (bit)))
+
+/* overlay interface API defines */
+typedef enum {
+        MORE_IBUF,
+        FINAL_IBUF,
+        COMPLETE_IBUF
+} MDP_IBUF_STATE;
+
+struct mdp_dirty_region {
+        __u32 xoffset;          /* source origin in the x-axis */
+        __u32 yoffset;          /* source origin in the y-axis */
+        __u32 width;            /* number of pixels in the x-axis */
+        __u32 height;           /* number of pixels in the y-axis */
+};
+
+/*
+ * MDP extended data types
+ */
+typedef struct mdp_roi_s {
+        uint32 x;
+        uint32 y;
+        uint32 width;
+        uint32 height;
+        int32 lcd_x;
+        int32 lcd_y;
+        uint32 dst_width;
+        uint32 dst_height;
+} MDP_ROI;
+
+typedef struct mdp_ibuf_s {
+        uint8 *buf;
+        uint32 bpp;
+        uint32 ibuf_type;
+        uint32 ibuf_width;
+        uint32 ibuf_height;
+
+        MDP_ROI roi;
+        MDPIMG mdpImg;
+
+        int32 dma_x;
+        int32 dma_y;
+        uint32 dma_w;
+        uint32 dma_h;
+
+        uint32 vsync_enable;
+        uint32 visible_swapped;
+} MDPIBUF;
+
+struct mdp_dma_data {
+        boolean busy;
+        boolean waiting;
+        struct mutex ov_mutex;
+        struct semaphore mutex;
+        struct completion comp;
+};
+
+#define MDP_CMD_DEBUG_ACCESS_BASE   (MDP_BASE+0x10000)
+
+#define MDP_DMA2_TERM 0x1
+#define MDP_DMA3_TERM 0x2
+#define MDP_PPP_TERM 0x4
+#define MDP_DMA_S_TERM 0x8
+#ifdef CONFIG_FB_MSM_MDP40
+#define MDP_DMA_E_TERM 0x10
+#define MDP_OVERLAY0_TERM 0x20
+#define MDP_OVERLAY1_TERM 0x40
+#endif
+
+#define ACTIVE_START_X_EN BIT(31)
+#define ACTIVE_START_Y_EN BIT(31)
+#define ACTIVE_HIGH 0
+#define ACTIVE_LOW 1
+#define MDP_DMA_S_DONE  BIT(2)
+#define LCDC_FRAME_START    BIT(15)
+#define LCDC_UNDERFLOW      BIT(16)
+
+#ifdef CONFIG_FB_MSM_MDP22
+#define MDP_DMA_P_DONE  BIT(2)
+#else
+#define MDP_DMA_P_DONE  BIT(14)
+#endif
+
+#define MDP_PPP_DONE                            BIT(0)
+#define TV_OUT_DMA3_DONE    BIT(6)
+#define TV_ENC_UNDERRUN     BIT(7)
+#define TV_OUT_DMA3_START   BIT(13)
+#define MDP_HIST_DONE       BIT(20)
+
+#ifdef CONFIG_FB_MSM_MDP22
+#define MDP_ANY_INTR_MASK (MDP_PPP_DONE| \
+                        MDP_DMA_P_DONE| \
+                        TV_ENC_UNDERRUN)
+#else
+#define MDP_ANY_INTR_MASK (MDP_PPP_DONE| \
+                        MDP_DMA_P_DONE| \
+                        MDP_DMA_S_DONE| \
+                        LCDC_UNDERFLOW| \
+                        MDP_HIST_DONE| \
+                        TV_ENC_UNDERRUN)
+#endif
+
+#define MDP_TOP_LUMA       16
+#define MDP_TOP_CHROMA     0
+#define MDP_BOTTOM_LUMA    19
+#define MDP_BOTTOM_CHROMA  3
+#define MDP_LEFT_LUMA      22
+#define MDP_LEFT_CHROMA    6
+#define MDP_RIGHT_LUMA     25
+#define MDP_RIGHT_CHROMA   9
+
+#define CLR_G 0x0
+#define CLR_B 0x1
+#define CLR_R 0x2
+#define CLR_ALPHA 0x3
+
+#define CLR_Y  CLR_G
+#define CLR_CB CLR_B
+#define CLR_CR CLR_R
+
+/* from lsb to msb */
+#define MDP_GET_PACK_PATTERN(a,x,y,z,bit) (((a)<<(bit*3))|((x)<<(bit*2))|((y)<<bit)|(z))
+
+/*
+ * 0x0000 0x0004 0x0008 MDP sync config
+ */
+#ifdef CONFIG_FB_MSM_MDP22
+#define MDP_SYNCFG_HGT_LOC 22
+#define MDP_SYNCFG_VSYNC_EXT_EN BIT(21)
+#define MDP_SYNCFG_VSYNC_INT_EN BIT(20)
+#else
+#define MDP_SYNCFG_HGT_LOC 21
+#define MDP_SYNCFG_VSYNC_EXT_EN BIT(20)
+#define MDP_SYNCFG_VSYNC_INT_EN BIT(19)
+#define MDP_HW_VSYNC
+#endif
+
+/*
+ * 0x0018 MDP VSYNC THREASH
+ */
+#define MDP_PRIM_BELOW_LOC 0
+#define MDP_PRIM_ABOVE_LOC 8
+
+/*
+ * MDP_PRIMARY_VSYNC_OUT_CTRL
+ * 0x0080,84,88 internal vsync pulse config
+ */
+#define VSYNC_PULSE_EN BIT(31)
+#define VSYNC_PULSE_INV BIT(30)
+
+/*
+ * 0x008c MDP VSYNC CONTROL
+ */
+#define DISP0_VSYNC_MAP_VSYNC0 0
+#define DISP0_VSYNC_MAP_VSYNC1 BIT(0)
+#define DISP0_VSYNC_MAP_VSYNC2 BIT(0)|BIT(1)
+
+#define DISP1_VSYNC_MAP_VSYNC0 0
+#define DISP1_VSYNC_MAP_VSYNC1 BIT(2)
+#define DISP1_VSYNC_MAP_VSYNC2 BIT(2)|BIT(3)
+
+#define PRIMARY_LCD_SYNC_EN BIT(4)
+#define PRIMARY_LCD_SYNC_DISABLE 0
+
+#define SECONDARY_LCD_SYNC_EN BIT(5)
+#define SECONDARY_LCD_SYNC_DISABLE 0
+
+#define EXTERNAL_LCD_SYNC_EN BIT(6)
+#define EXTERNAL_LCD_SYNC_DISABLE 0
+
+/*
+ * 0x101f0 MDP VSYNC Threshold
+ */
+#define VSYNC_THRESHOLD_ABOVE_LOC 0
+#define VSYNC_THRESHOLD_BELOW_LOC 16
+#define VSYNC_ANTI_TEAR_EN BIT(31)
+
+/*
+ * 0x10004 command config
+ */
+#define MDP_CMD_DBGBUS_EN BIT(0)
+
+/*
+ * 0x10124 or 0x101d4PPP source config
+ */
+#define PPP_SRC_C0G_8BITS (BIT(1)|BIT(0))
+#define PPP_SRC_C1B_8BITS (BIT(3)|BIT(2))
+#define PPP_SRC_C2R_8BITS (BIT(5)|BIT(4))
+#define PPP_SRC_C3A_8BITS (BIT(7)|BIT(6))
+
+#define PPP_SRC_C0G_6BITS BIT(1)
+#define PPP_SRC_C1B_6BITS BIT(3)
+#define PPP_SRC_C2R_6BITS BIT(5)
+
+#define PPP_SRC_C0G_5BITS BIT(0)
+#define PPP_SRC_C1B_5BITS BIT(2)
+#define PPP_SRC_C2R_5BITS BIT(4)
+
+#define PPP_SRC_C3_ALPHA_EN BIT(8)
+
+#define PPP_SRC_BPP_INTERLVD_1BYTES 0
+#define PPP_SRC_BPP_INTERLVD_2BYTES BIT(9)
+#define PPP_SRC_BPP_INTERLVD_3BYTES BIT(10)
+#define PPP_SRC_BPP_INTERLVD_4BYTES (BIT(10)|BIT(9))
+
+#define PPP_SRC_BPP_ROI_ODD_X BIT(11)
+#define PPP_SRC_BPP_ROI_ODD_Y BIT(12)
+#define PPP_SRC_INTERLVD_2COMPONENTS BIT(13)
+#define PPP_SRC_INTERLVD_3COMPONENTS BIT(14)
+#define PPP_SRC_INTERLVD_4COMPONENTS (BIT(14)|BIT(13))
+
+/*
+ * RGB666 unpack format
+ * TIGHT means R6+G6+B6 together
+ * LOOSE means R6+2 +G6+2+ B6+2 (with MSB)
+ * or 2+R6 +2+G6 +2+B6 (with LSB)
+ */
+#define PPP_SRC_UNPACK_TIGHT BIT(17)
+#define PPP_SRC_UNPACK_LOOSE 0
+#define PPP_SRC_UNPACK_ALIGN_LSB 0
+#define PPP_SRC_UNPACK_ALIGN_MSB BIT(18)
+
+#define PPP_SRC_FETCH_PLANES_INTERLVD 0
+#define PPP_SRC_FETCH_PLANES_PSEUDOPLNR BIT(20)
+
+#define PPP_SRC_WMV9_MODE BIT(21)       /* window media version 9 */
+
+/*
+ * 0x10138 PPP operation config
+ */
+#define PPP_OP_SCALE_X_ON BIT(0)
+#define PPP_OP_SCALE_Y_ON BIT(1)
+
+#define PPP_OP_CONVERT_RGB2YCBCR 0
+#define PPP_OP_CONVERT_YCBCR2RGB BIT(2)
+#define PPP_OP_CONVERT_ON BIT(3)
+
+#define PPP_OP_CONVERT_MATRIX_PRIMARY 0
+#define PPP_OP_CONVERT_MATRIX_SECONDARY BIT(4)
+
+#define PPP_OP_LUT_C0_ON BIT(5)
+#define PPP_OP_LUT_C1_ON BIT(6)
+#define PPP_OP_LUT_C2_ON BIT(7)
+
+/* rotate or blend enable */
+#define PPP_OP_ROT_ON BIT(8)
+
+#define PPP_OP_ROT_90 BIT(9)
+#define PPP_OP_FLIP_LR BIT(10)
+#define PPP_OP_FLIP_UD BIT(11)
+
+#define PPP_OP_BLEND_ON BIT(12)
+
+#define PPP_OP_BLEND_SRCPIXEL_ALPHA 0
+#define PPP_OP_BLEND_DSTPIXEL_ALPHA BIT(13)
+#define PPP_OP_BLEND_CONSTANT_ALPHA BIT(14)
+#define PPP_OP_BLEND_SRCPIXEL_TRANSP (BIT(13)|BIT(14))
+
+#define PPP_OP_BLEND_ALPHA_BLEND_NORMAL 0
+#define PPP_OP_BLEND_ALPHA_BLEND_REVERSE BIT(15)
+
+#define PPP_OP_DITHER_EN BIT(16)
+
+#define PPP_OP_COLOR_SPACE_RGB 0
+#define PPP_OP_COLOR_SPACE_YCBCR BIT(17)
+
+#define PPP_OP_SRC_CHROMA_RGB 0
+#define PPP_OP_SRC_CHROMA_H2V1 BIT(18)
+#define PPP_OP_SRC_CHROMA_H1V2 BIT(19)
+#define PPP_OP_SRC_CHROMA_420 (BIT(18)|BIT(19))
+#define PPP_OP_SRC_CHROMA_COSITE 0
+#define PPP_OP_SRC_CHROMA_OFFSITE BIT(20)
+
+#define PPP_OP_DST_CHROMA_RGB 0
+#define PPP_OP_DST_CHROMA_H2V1 BIT(21)
+#define PPP_OP_DST_CHROMA_H1V2 BIT(22)
+#define PPP_OP_DST_CHROMA_420 (BIT(21)|BIT(22))
+#define PPP_OP_DST_CHROMA_COSITE 0
+#define PPP_OP_DST_CHROMA_OFFSITE BIT(23)
+
+#define PPP_BLEND_CALPHA_TRNASP BIT(24)
+
+#define PPP_OP_BG_CHROMA_RGB 0
+#define PPP_OP_BG_CHROMA_H2V1 BIT(25)
+#define PPP_OP_BG_CHROMA_H1V2 BIT(26)
+#define PPP_OP_BG_CHROMA_420 BIT(25)|BIT(26)
+#define PPP_OP_BG_CHROMA_SITE_COSITE 0
+#define PPP_OP_BG_CHROMA_SITE_OFFSITE BIT(27)
+#define PPP_OP_DEINT_EN BIT(29)
+
+#define PPP_BLEND_BG_USE_ALPHA_SEL      (1 << 0)
+#define PPP_BLEND_BG_ALPHA_REVERSE      (1 << 3)
+#define PPP_BLEND_BG_SRCPIXEL_ALPHA     (0 << 1)
+#define PPP_BLEND_BG_DSTPIXEL_ALPHA     (1 << 1)
+#define PPP_BLEND_BG_CONSTANT_ALPHA     (2 << 1)
+#define PPP_BLEND_BG_CONST_ALPHA_VAL(x) ((x) << 24)
+
+#define PPP_OP_DST_RGB 0
+#define PPP_OP_DST_YCBCR BIT(30)
+/*
+ * 0x10150 PPP destination config
+ */
+#define PPP_DST_C0G_8BIT (BIT(0)|BIT(1))
+#define PPP_DST_C1B_8BIT (BIT(3)|BIT(2))
+#define PPP_DST_C2R_8BIT (BIT(5)|BIT(4))
+#define PPP_DST_C3A_8BIT (BIT(7)|BIT(6))
+
+#define PPP_DST_C0G_6BIT BIT(1)
+#define PPP_DST_C1B_6BIT BIT(3)
+#define PPP_DST_C2R_6BIT BIT(5)
+
+#define PPP_DST_C0G_5BIT BIT(0)
+#define PPP_DST_C1B_5BIT BIT(2)
+#define PPP_DST_C2R_5BIT BIT(4)
+
+#define PPP_DST_C3A_8BIT (BIT(7)|BIT(6))
+#define PPP_DST_C3ALPHA_EN BIT(8)
+
+#define PPP_DST_PACKET_CNT_INTERLVD_2ELEM BIT(9)
+#define PPP_DST_PACKET_CNT_INTERLVD_3ELEM BIT(10)
+#define PPP_DST_PACKET_CNT_INTERLVD_4ELEM (BIT(10)|BIT(9))
+#define PPP_DST_PACKET_CNT_INTERLVD_6ELEM (BIT(11)|BIT(9))
+
+#define PPP_DST_PACK_LOOSE 0
+#define PPP_DST_PACK_TIGHT BIT(13)
+#define PPP_DST_PACK_ALIGN_LSB 0
+#define PPP_DST_PACK_ALIGN_MSB BIT(14)
+
+#define PPP_DST_OUT_SEL_AXI 0
+#define PPP_DST_OUT_SEL_MDDI BIT(15)
+
+#define PPP_DST_BPP_2BYTES BIT(16)
+#define PPP_DST_BPP_3BYTES BIT(17)
+#define PPP_DST_BPP_4BYTES (BIT(17)|BIT(16))
+
+#define PPP_DST_PLANE_INTERLVD 0
+#define PPP_DST_PLANE_PLANAR BIT(18)
+#define PPP_DST_PLANE_PSEUDOPLN BIT(19)
+
+#define PPP_DST_TO_TV BIT(20)
+
+#define PPP_DST_MDDI_PRIMARY 0
+#define PPP_DST_MDDI_SECONDARY BIT(21)
+#define PPP_DST_MDDI_EXTERNAL BIT(22)
+
+/*
+ * 0x10180 DMA config
+ */
+#define DMA_DSTC0G_8BITS (BIT(1)|BIT(0))
+#define DMA_DSTC1B_8BITS (BIT(3)|BIT(2))
+#define DMA_DSTC2R_8BITS (BIT(5)|BIT(4))
+
+#define DMA_DSTC0G_6BITS BIT(1)
+#define DMA_DSTC1B_6BITS BIT(3)
+#define DMA_DSTC2R_6BITS BIT(5)
+
+#define DMA_DSTC0G_5BITS BIT(0)
+#define DMA_DSTC1B_5BITS BIT(2)
+#define DMA_DSTC2R_5BITS BIT(4)
+
+#define DMA_PACK_TIGHT                      BIT(6)
+#define DMA_PACK_LOOSE                      0
+#define DMA_PACK_ALIGN_LSB                  0
+/*
+ * use DMA_PACK_ALIGN_MSB if the upper 6 bits from 8 bits output
+ * from LCDC block maps into 6 pins out to the panel
+ */
+#define DMA_PACK_ALIGN_MSB                  BIT(7)
+#define DMA_PACK_PATTERN_RGB \
+       (MDP_GET_PACK_PATTERN(0, CLR_R, CLR_G, CLR_B, 2)<<8)
+#define DMA_PACK_PATTERN_BGR \
+       (MDP_GET_PACK_PATTERN(0, CLR_B, CLR_G, CLR_R, 2)<<8)
+#define DMA_OUT_SEL_AHB                     0
+#define DMA_OUT_SEL_LCDC                    BIT(20)
+#define DMA_IBUF_FORMAT_RGB888              0
+#define DMA_IBUF_FORMAT_xRGB8888_OR_ARGB8888  BIT(26)
+
+#ifdef CONFIG_FB_MSM_MDP22
+#define DMA_OUT_SEL_MDDI BIT(14)
+#define DMA_AHBM_LCD_SEL_PRIMARY 0
+#define DMA_AHBM_LCD_SEL_SECONDARY BIT(15)
+#define DMA_IBUF_C3ALPHA_EN BIT(16)
+#define DMA_DITHER_EN BIT(17)
+#define DMA_MDDI_DMAOUT_LCD_SEL_PRIMARY 0
+#define DMA_MDDI_DMAOUT_LCD_SEL_SECONDARY BIT(18)
+#define DMA_MDDI_DMAOUT_LCD_SEL_EXTERNAL BIT(19)
+#define DMA_IBUF_FORMAT_RGB565 BIT(20)
+#define DMA_IBUF_FORMAT_RGB888_OR_ARGB8888 0
+#define DMA_IBUF_NONCONTIGUOUS BIT(21)
+#else
+#define DMA_OUT_SEL_MDDI                    BIT(19)
+#define DMA_AHBM_LCD_SEL_PRIMARY            0
+#define DMA_AHBM_LCD_SEL_SECONDARY          0
+#define DMA_IBUF_C3ALPHA_EN                 0
+#define DMA_DITHER_EN                       BIT(24)
+#define DMA_MDDI_DMAOUT_LCD_SEL_PRIMARY     0
+#define DMA_MDDI_DMAOUT_LCD_SEL_SECONDARY   0
+#define DMA_MDDI_DMAOUT_LCD_SEL_EXTERNAL    0
+#define DMA_IBUF_FORMAT_RGB565              BIT(25)
+#define DMA_IBUF_NONCONTIGUOUS 0
+#endif
+
+/*
+ * MDDI Register
+ */
+#define MDDI_VDO_PACKET_DESC  0x5666
+
+#ifdef CONFIG_FB_MSM_MDP40
+#define MDP_INTR_ENABLE         (msm_mdp_base + 0x0050)
+#define MDP_INTR_STATUS         (msm_mdp_base + 0x0054)
+#define MDP_INTR_CLEAR          (msm_mdp_base + 0x0058)
+#define MDP_EBI2_LCD0           (msm_mdp_base + 0x0060)
+#define MDP_EBI2_LCD1           (msm_mdp_base + 0x0064)
+#define MDP_EBI2_PORTMAP_MODE   (msm_mdp_base + 0x0070)
+
+#define MDP_DMA_P_HIST_INTR_STATUS      (msm_mdp_base + 0x95014)
+#define MDP_DMA_P_HIST_INTR_CLEAR       (msm_mdp_base + 0x95018)
+#define MDP_DMA_P_HIST_INTR_ENABLE      (msm_mdp_base + 0x9501C)
+#else
+#define MDP_INTR_ENABLE         (msm_mdp_base + 0x0020)
+#define MDP_INTR_STATUS         (msm_mdp_base + 0x0024)
+#define MDP_INTR_CLEAR          (msm_mdp_base + 0x0028)
+#define MDP_EBI2_LCD0           (msm_mdp_base + 0x003c)
+#define MDP_EBI2_LCD1           (msm_mdp_base + 0x0040)
+#define MDP_EBI2_PORTMAP_MODE   (msm_mdp_base + 0x005c)
+#endif
+
+#define MDP_FULL_BYPASS_WORD43  (msm_mdp_base + 0x101ac)
+
+#define MDP_CSC_PFMVn(n)        (msm_mdp_base + 0x40400 + 4 * (n))
+#define MDP_CSC_PRMVn(n)        (msm_mdp_base + 0x40440 + 4 * (n))
+#define MDP_CSC_PRE_BV1n(n)     (msm_mdp_base + 0x40500 + 4 * (n))
+#define MDP_CSC_PRE_BV2n(n)     (msm_mdp_base + 0x40540 + 4 * (n))
+#define MDP_CSC_POST_BV1n(n)    (msm_mdp_base + 0x40580 + 4 * (n))
+#define MDP_CSC_POST_BV2n(n)    (msm_mdp_base + 0x405c0 + 4 * (n))
+
+#ifdef CONFIG_FB_MSM_MDP31
+#define MDP_CSC_PRE_LV1n(n)     (msm_mdp_base + 0x40600 + 4 * (n))
+#define MDP_CSC_PRE_LV2n(n)     (msm_mdp_base + 0x40640 + 4 * (n))
+#define MDP_CSC_POST_LV1n(n)    (msm_mdp_base + 0x40680 + 4 * (n))
+#define MDP_CSC_POST_LV2n(n)    (msm_mdp_base + 0x406c0 + 4 * (n))
+#define MDP_PPP_SCALE_COEFF_LSBn(n)     (msm_mdp_base + 0x50400 + 8 * (n))
+#define MDP_PPP_SCALE_COEFF_MSBn(n)     (msm_mdp_base + 0x50404 + 8 * (n))
+
+#define SCALE_D0_SET  0
+#define SCALE_D1_SET  BIT(0)
+#define SCALE_D2_SET  BIT(1)
+#define SCALE_U1_SET  (BIT(0)|BIT(1))
+
+#else
+#define MDP_CSC_PRE_LV1n(n)     (msm_mdp_base + 0x40580 + 4 * (n))
+#endif
+
+#define MDP_CURSOR_WIDTH 64
+#define MDP_CURSOR_HEIGHT 64
+#define MDP_CURSOR_SIZE (MDP_CURSOR_WIDTH*MDP_CURSOR_WIDTH*4)
+
+#define MDP_DMA_P_LUT_C0_EN   BIT(0)
+#define MDP_DMA_P_LUT_C1_EN   BIT(1)
+#define MDP_DMA_P_LUT_C2_EN   BIT(2)
+#define MDP_DMA_P_LUT_POST    BIT(4)
+
+void mdp_hw_init(void);
+int mdp_ppp_pipe_wait(void);
+void mdp_pipe_kickoff(uint32 term, struct msm_fb_data_type *mfd);
+void mdp_pipe_ctrl(MDP_BLOCK_TYPE block, MDP_BLOCK_POWER_STATE state,
+                   boolean isr);
+void mdp_set_dma_pan_info(struct fb_info *info, struct mdp_dirty_region *dirty,
+                          boolean sync);
+void mdp_dma_pan_update(struct fb_info *info);
+void mdp_refresh_screen(unsigned long data);
+int mdp_ppp_blit(struct fb_info *info, struct mdp_blit_req *req,
+                struct file **pp_src, struct file **pp_dest);
+void mdp_lcd_update_workqueue_handler(struct work_struct *work);
+void mdp_vsync_resync_workqueue_handler(struct work_struct *work);
+void mdp_dma2_update(struct msm_fb_data_type *mfd);
+void mdp_config_vsync(struct msm_fb_data_type *);
+uint32 mdp_get_lcd_line_counter(struct msm_fb_data_type *mfd);
+enum hrtimer_restart mdp_dma2_vsync_hrtimer_handler(struct hrtimer *ht);
+void mdp_set_scale(MDPIBUF *iBuf,
+                   uint32 dst_roi_width,
+                   uint32 dst_roi_height,
+                   boolean inputRGB, boolean outputRGB, uint32 *pppop_reg_ptr);
+void mdp_init_scale_table(void);
+void mdp_adjust_start_addr(uint8 **src0,
+                           uint8 **src1,
+                           int v_slice,
+                           int h_slice,
+                           int x,
+                           int y,
+                           uint32 width,
+                           uint32 height, int bpp, MDPIBUF *iBuf, int layer);
+void mdp_set_blend_attr(MDPIBUF *iBuf,
+                        uint32 *alpha,
+                        uint32 *tpVal,
+                        uint32 perPixelAlpha, uint32 *pppop_reg_ptr);
+
+int mdp_dma3_on(struct platform_device *pdev);
+int mdp_dma3_off(struct platform_device *pdev);
+void mdp_dma3_update(struct msm_fb_data_type *mfd);
+
+int mdp_lcdc_on(struct platform_device *pdev);
+int mdp_lcdc_off(struct platform_device *pdev);
+void mdp_lcdc_update(struct msm_fb_data_type *mfd);
+int mdp_hw_cursor_update(struct fb_info *info, struct fb_cursor *cursor);
+void mdp_enable_irq(uint32 term);
+void mdp_disable_irq(uint32 term);
+void mdp_disable_irq_nolock(uint32 term);
+uint32_t mdp_get_bytes_per_pixel(uint32_t format);
+
+#ifdef MDP_HW_VSYNC
+void mdp_hw_vsync_clk_enable(struct msm_fb_data_type *mfd);
+void mdp_hw_vsync_clk_disable(struct msm_fb_data_type *mfd);
+#endif
+
+void mdp_dma_s_update(struct msm_fb_data_type *mfd);
+
+/* Added to support flipping */
+void mdp_set_offset_info(struct fb_info *info, uint32 address, uint32 interval);
+
+int get_gem_img(struct mdp_img *img, unsigned long *start,
+                unsigned long *len);
+int get_img(struct mdp_img *img, struct fb_info *info,
+                unsigned long *start, unsigned long *len,
+                struct file **pp_file);
+
+
+/*int get_img(struct msmfb_data *img, struct fb_info *info,
+        unsigned long *start, unsigned long *len, struct file **pp_file);*/
+#endif /* MDP_H */
diff --git a/drivers/staging/msm/mdp4.h b/drivers/staging/msm/mdp4.h
new file mode 100644
index 000000000000..26ec8f12cf64
--- /dev/null
+++ b/drivers/staging/msm/mdp4.h
@@ -0,0 +1,352 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Code Aurora nor
+ *       the names of its contributors may be used to endorse or promote
+ *       products derived from this software without specific prior written
+ *       permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NON-INFRINGEMENT ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef MDP4_H
+#define MDP4_H
+
+extern struct mdp_dma_data dma2_data;
+extern struct mdp_dma_data dma_s_data;
+extern struct mdp_dma_data dma_e_data;
+extern struct mdp_histogram mdp_hist;
+extern struct completion mdp_hist_comp;
+extern boolean mdp_is_in_isr;
+extern uint32 mdp_intr_mask;
+extern spinlock_t mdp_spin_lock;
+
+
+#define MDP4_NONBLOCKING        /* enable non blocking ioctl */
+
+#define MDP4_OVERLAYPROC0_BASE  0x10000
+#define MDP4_OVERLAYPROC1_BASE  0x18000
+
+#define MDP4_VIDEO_BASE 0x20000
+#define MDP4_VIDEO_OFF 0x10000
+
+#define MDP4_RGB_BASE 0x40000
+#define MDP4_RGB_OFF 0x10000
+
+enum {          /* display */
+        PRIMARY_INTF_SEL,
+        SECONDARY_INTF_SEL,
+        EXTERNAL_INTF_SEL
+};
+
+enum {
+        LCDC_RGB_INTF,
+        DTV_INTF = LCDC_RGB_INTF,
+        MDDI_LCDC_INTF,
+        MDDI_INTF,
+        EBI2_INTF
+};
+
+enum {
+        MDDI_PRIMARY_SET,
+        MDDI_SECONDARY_SET,
+        MDDI_EXTERNAL_SET
+};
+
+enum {
+        EBI2_LCD0,
+        EBI2_LCD1
+};
+
+enum {
+        OVERLAY_MODE_NONE,
+        OVERLAY_MODE_BLT
+};
+
+enum {
+        OVERLAY_REFRESH_ON_DEMAND,
+        OVERLAY_REFRESH_VSYNC,
+        OVERLAY_REFRESH_VSYNC_HALF,
+        OVERLAY_REFRESH_VSYNC_QUARTER
+};
+
+enum {
+        OVERLAY_FRAMEBUF,
+        OVERLAY_DIRECTOUT
+};
+
+/* system interrupts */
+#define INTR_OVERLAY0_DONE              BIT(0)
+#define INTR_OVERLAY1_DONE              BIT(1)
+#define INTR_DMA_S_DONE                 BIT(2)
+#define INTR_DMA_E_DONE                 BIT(3)
+#define INTR_DMA_P_DONE                 BIT(4)
+#define INTR_VG1_HISTOGRAM              BIT(5)
+#define INTR_VG2_HISTOGRAM              BIT(6)
+#define INTR_PRIMARY_VSYNC              BIT(7)
+#define INTR_PRIMARY_INTF_UDERRUN       BIT(8)
+#define INTR_EXTERNAL_VSYNC             BIT(9)
+#define INTR_EXTERNAL_INTF_UDERRUN      BIT(10)
+#define INTR_DMA_P_HISTOGRAM            BIT(17)
+
+/* histogram interrupts */
+#define INTR_HIST_DONE                  BIT(0)
+#define INTR_HIST_RESET_SEQ_DONE        BIT(1)
+
+
+#ifdef CONFIG_FB_MSM_OVERLAY
+#define MDP4_ANY_INTR_MASK      (INTR_OVERLAY0_DONE)
+#else
+#define MDP4_ANY_INTR_MASK      (INTR_DMA_P_DONE)
+#endif
+
+enum {
+        OVERLAY_PIPE_RGB1,
+        OVERLAY_PIPE_RGB2,
+};
+
+enum {
+        OVERLAY_PIPE_VG1,       /* video/graphic */
+        OVERLAY_PIPE_VG2
+};
+
+enum {
+        OVERLAY_TYPE_RGB,
+        OVERLAY_TYPE_VG         /* video/graphic */
+};
+
+enum {
+        MDP4_MIXER0,
+        MDP4_MIXER1
+};
+
+#define MDP4_MAX_MIXER  2
+
+enum {
+        OVERLAY_PLANE_INTERLEAVED,
+        OVERLAY_PLANE_PLANAR,
+        OVERLAY_PLANE_PSEUDO_PLANAR
+};
+
+enum {
+        MDP4_MIXER_STAGE_UNUNSED,       /* pipe not used */
+        MDP4_MIXER_STAGE_BASE,
+        MDP4_MIXER_STAGE0,      /* zorder 0 */
+        MDP4_MIXER_STAGE1,      /* zorder 1 */
+        MDP4_MIXER_STAGE2       /* zorder 2 */
+};
+
+#define MDP4_MAX_STAGE  4
+
+enum {
+        MDP4_FRAME_FORMAT_LINEAR,
+        MDP4_FRAME_FORMAT_ARGB_TILE,
+        MDP4_FRAME_FORMAT_VIDEO_SUPERTILE
+};
+
+enum {
+        MDP4_CHROMA_RGB,
+        MDP4_CHROMA_H2V1,
+        MDP4_CHROMA_H1V2,
+        MDP4_CHROMA_420
+};
+
+#define MDP4_BLEND_BG_TRANSP_EN         BIT(9)
+#define MDP4_BLEND_FG_TRANSP_EN         BIT(8)
+#define MDP4_BLEND_BG_MOD_ALPHA         BIT(7)
+#define MDP4_BLEND_BG_INV_ALPHA         BIT(6)
+#define MDP4_BLEND_BG_ALPHA_FG_CONST    (0 << 4)
+#define MDP4_BLEND_BG_ALPHA_BG_CONST    (1 << 4)
+#define MDP4_BLEND_BG_ALPHA_FG_PIXEL    (2 << 4)
+#define MDP4_BLEND_BG_ALPHA_BG_PIXEL    (3 << 4)
+#define MDP4_BLEND_FG_MOD_ALPHA         BIT(3)
+#define MDP4_BLEND_FG_INV_ALPHA         BIT(2)
+#define MDP4_BLEND_FG_ALPHA_FG_CONST    (0 << 0)
+#define MDP4_BLEND_FG_ALPHA_BG_CONST    (1 << 0)
+#define MDP4_BLEND_FG_ALPHA_FG_PIXEL    (2 << 0)
+#define MDP4_BLEND_FG_ALPHA_BG_PIXEL    (3 << 0)
+
+#define MDP4_FORMAT_SOLID_FILL          BIT(22)
+#define MDP4_FORMAT_UNPACK_ALIGN_MSB    BIT(18)
+#define MDP4_FORMAT_UNPACK_TIGHT        BIT(17)
+#define MDP4_FORMAT_90_ROTATED          BIT(12)
+#define MDP4_FORMAT_ALPHA_ENABLE        BIT(8)
+
+#define MDP4_OP_DEINT_ODD_REF   BIT(19)
+#define MDP4_OP_IGC_LUT_EN      BIT(16)
+#define MDP4_OP_DITHER_EN       BIT(15)
+#define MDP4_OP_FLIP_UD         BIT(14)
+#define MDP4_OP_FLIP_LR         BIT(13)
+#define MDP4_OP_CSC_EN          BIT(11)
+#define MDP4_OP_SRC_DATA_YCBCR  BIT(9)
+#define MDP4_OP_SCALEY_FIR              (0 << 4)
+#define MDP4_OP_SCALEY_MN_PHASE         (1 << 4)
+#define MDP4_OP_SCALEY_PIXEL_RPT        (2 << 4)
+#define MDP4_OP_SCALEX_FIR              (0 << 2)
+#define MDP4_OP_SCALEX_MN_PHASE         (1 << 2)
+#define MDP4_OP_SCALEX_PIXEL_RPT        (2 << 2)
+#define MDP4_OP_SCALEY_EN       BIT(1)
+#define MDP4_OP_SCALEX_EN       BIT(0)
+
+#define MDP4_PIPE_PER_MIXER     2
+
+#define MDP4_MAX_PLANE          4
+
+#define MDP4_MAX_VIDEO_PIPE 2
+#define MDP4_MAX_RGB_PIPE 2
+#define MDP4_MAX_OVERLAY_PIPE   16
+
+
+struct mdp4_overlay_pipe {
+        uint32 pipe_type;               /* rgb, video/graphic */
+        uint32 pipe_num;
+        uint32 pipe_ndx;
+        uint32 mixer_num;               /* which mixer used */
+        uint32 mixer_stage;             /* which stage of mixer used */
+        uint32 src_format;
+        uint32 src_width;       /* source img width */
+        uint32 src_height;      /* source img height */
+        uint32 src_w;           /* roi */
+        uint32 src_h;           /* roi */
+        uint32 src_x;           /* roi */
+        uint32 src_y;           /* roi */
+        uint32 dst_w;           /* roi */
+        uint32 dst_h;           /* roi */
+        uint32 dst_x;           /* roi */
+        uint32 dst_y;           /* roi */
+        uint32 op_mode;
+        uint32 transp;
+        uint32 blend_op;
+        uint32 phasex_step;
+        uint32 phasey_step;
+        uint32 alpha;
+        uint32 is_fg;           /* control alpha & color key */
+        uint32 srcp0_addr;      /* interleave, luma */
+        uint32 srcp0_ystride;
+        uint32 srcp1_addr;      /* pseudoplanar, chroma plane */
+        uint32 srcp1_ystride;
+        uint32 srcp2_addr;      /* planar color 2*/
+        uint32 srcp2_ystride;
+        uint32 srcp3_addr;      /* alpha/color 3 */
+        uint32 srcp3_ystride;
+        uint32 fetch_plane;
+        uint32 frame_format;            /* video */
+        uint32 chroma_site;             /* video */
+        uint32 chroma_sample;           /* video */
+        uint32 solid_fill;
+        uint32 vc1_reduce;              /* video */
+        uint32 fatch_planes;            /* video */
+        uint32 unpack_align_msb;/* 0 to LSB, 1 to MSB */
+        uint32 unpack_tight;/* 0 for loose, 1 for tight */
+        uint32 unpack_count;/* 0 = 1 component, 1 = 2 component ... */
+        uint32 rotated_90; /* has been rotated 90 degree */
+        uint32 bpp;     /* byte per pixel */
+        uint32 alpha_enable;/*  source has alpha */
+        /*
+         * number of bits for source component,
+         * 0 = 1 bit, 1 = 2 bits, 2 = 6 bits, 3 = 8 bits
+         */
+        uint32 a_bit;   /* component 3, alpha */
+        uint32 r_bit;   /* component 2, R_Cr */
+        uint32 b_bit;   /* component 1, B_Cb */
+        uint32 g_bit;   /* component 0, G_lumz */
+        /*
+         * unpack pattern
+         * A = C3, R = C2, B = C1, G = C0
+         */
+        uint32 element3; /* 0 = C0, 1 = C1, 2 = C2, 3 = C3 */
+        uint32 element2; /* 0 = C0, 1 = C1, 2 = C2, 3 = C3 */
+        uint32 element1; /* 0 = C0, 1 = C1, 2 = C2, 3 = C3 */
+        uint32 element0; /* 0 = C0, 1 = C1, 2 = C2, 3 = C3 */
+        struct completion comp;
+        struct mdp_overlay req_data;
+};
+
+void mdp4_sw_reset(unsigned long bits);
+void mdp4_display_intf_sel(int output, unsigned long intf);
+void mdp4_overlay_cfg(int layer, int blt_mode, int refresh, int direct_out);
+void mdp4_ebi2_lcd_setup(int lcd, unsigned long base, int ystride);
+void mdp4_mddi_setup(int which, unsigned long id);
+unsigned long mdp4_display_status(void);
+void mdp4_enable_clk_irq(void);
+void mdp4_disable_clk_irq(void);
+void mdp4_dma_p_update(struct msm_fb_data_type *mfd);
+void mdp4_dma_s_update(struct msm_fb_data_type *mfd);
+void mdp_pipe_ctrl(MDP_BLOCK_TYPE block, MDP_BLOCK_POWER_STATE state,
+                   boolean isr);
+void mdp4_pipe_kickoff(uint32 pipe, struct msm_fb_data_type *mfd);
+int mdp4_lcdc_on(struct platform_device *pdev);
+int mdp4_lcdc_off(struct platform_device *pdev);
+void mdp4_lcdc_update(struct msm_fb_data_type *mfd);
+void mdp4_intr_clear_set(ulong clear, ulong set);
+void mdp4_dma_p_cfg(void);
+void mdp4_hw_init(void);
+void mdp4_isr_read(int);
+void mdp4_clear_lcdc(void);
+void mdp4_mixer_blend_init(int mixer_num);
+void mdp4_vg_qseed_init(int vg_num);
+void mdp4_vg_csc_mv_setup(int vp_num);
+void mdp4_vg_csc_pre_bv_setup(int vp_num);
+void mdp4_vg_csc_post_bv_setup(int vp_num);
+void mdp4_vg_csc_pre_lv_setup(int vp_num);
+void mdp4_vg_csc_post_lv_setup(int vp_num);
+irqreturn_t mdp4_isr(int irq, void *ptr);
+void mdp4_overlay_format_to_pipe(uint32 format, struct mdp4_overlay_pipe *pipe);
+uint32 mdp4_overlay_format(struct mdp4_overlay_pipe *pipe);
+uint32 mdp4_overlay_unpack_pattern(struct mdp4_overlay_pipe *pipe);
+uint32 mdp4_overlay_op_mode(struct mdp4_overlay_pipe *pipe);
+void mdp4_lcdc_overlay(struct msm_fb_data_type *mfd);
+void mdp4_overlay_rgb_setup(struct mdp4_overlay_pipe *pipe);
+void mdp4_overlay_reg_flush(struct mdp4_overlay_pipe *pipe, int all);
+void mdp4_mixer_blend_setup(struct mdp4_overlay_pipe *pipe);
+void mdp4_mixer_stage_up(struct mdp4_overlay_pipe *pipe);
+void mdp4_mixer_stage_down(struct mdp4_overlay_pipe *pipe);
+int mdp4_mixer_stage_can_run(struct mdp4_overlay_pipe *pipe);
+void mdp4_overlayproc_cfg(struct mdp4_overlay_pipe *pipe);
+void mdp4_mddi_overlay(struct msm_fb_data_type *mfd);
+int mdp4_overlay_format2type(uint32 format);
+int mdp4_overlay_format2pipe(struct mdp4_overlay_pipe *pipe);
+int mdp4_overlay_get(struct fb_info *info, struct mdp_overlay *req);
+int mdp4_overlay_set(struct fb_info *info, struct mdp_overlay *req);
+int mdp4_overlay_unset(struct fb_info *info, int ndx);
+int mdp4_overlay_play(struct fb_info *info, struct msmfb_overlay_data *req,
+                                struct file **pp_src_file);
+struct mdp4_overlay_pipe *mdp4_overlay_pipe_alloc(void);
+void mdp4_overlay_pipe_free(struct mdp4_overlay_pipe *pipe);
+void mdp4_overlay_dmap_cfg(struct msm_fb_data_type *mfd, int lcdc);
+void mdp4_overlay_dmap_xy(struct mdp4_overlay_pipe *pipe);
+int mdp4_overlay_active(int mixer);
+void mdp4_overlay0_done_lcdc(void);
+void mdp4_overlay0_done_mddi(void);
+void mdp4_mddi_overlay_restore(void);
+void mdp4_mddi_overlay_kickoff(struct msm_fb_data_type *mfd,
+                                struct mdp4_overlay_pipe *pipe);
+void mdp4_rgb_igc_lut_setup(int num);
+void mdp4_vg_igc_lut_setup(int num);
+void mdp4_mixer_gc_lut_setup(int mixer_num);
+
+#ifdef CONFIG_DEBUG_FS
+int mdp4_debugfs_init(void);
+#endif
+
+int mdp_ppp_blit(struct fb_info *info, struct mdp_blit_req *req,
+        struct file **pp_src_file, struct file **pp_dst_file);
+
+#endif /* MDP_H */
diff --git a/drivers/staging/msm/mdp4_debugfs.c b/drivers/staging/msm/mdp4_debugfs.c
new file mode 100644
index 000000000000..844d46775ecd
--- /dev/null
+++ b/drivers/staging/msm/mdp4_debugfs.c
@@ -0,0 +1,181 @@
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/hrtimer.h>
+#include <linux/clk.h>
+#include <mach/hardware.h>
+#include <linux/io.h>
+#include <linux/debugfs.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/uaccess.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+#include "mdp4.h"
+
+
+#define MDP4_DEBUG_BUF  128
+
+
+static char mdp4_debug_buf[MDP4_DEBUG_BUF];
+static ulong mdp4_debug_offset;
+static ulong mdp4_base_addr;
+
+static int mdp4_offset_set(void *data, u64 val)
+{
+        mdp4_debug_offset = (int)val;
+        return 0;
+}
+
+static int mdp4_offset_get(void *data, u64 *val)
+{
+        *val = (u64)mdp4_debug_offset;
+        return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(
+                        mdp4_offset_fops,
+                        mdp4_offset_get,
+                        mdp4_offset_set,
+                        "%llx\n");
+
+
+static int mdp4_debugfs_open(struct inode *inode, struct file *file)
+{
+        /* non-seekable */
+        file->f_mode &= ~(FMODE_LSEEK | FMODE_PREAD | FMODE_PWRITE);
+        return 0;
+}
+
+static int mdp4_debugfs_release(struct inode *inode, struct file *file)
+{
+        return 0;
+}
+
+static ssize_t mdp4_debugfs_write(
+        struct file *file,
+        const char __user *buff,
+        size_t count,
+        loff_t *ppos)
+{
+        int cnt;
+        unsigned int data;
+
+        printk(KERN_INFO "%s: offset=%d count=%d *ppos=%d\n",
+                __func__, (int)mdp4_debug_offset, (int)count, (int)*ppos);
+
+        if (count > sizeof(mdp4_debug_buf))
+                return -EFAULT;
+
+        if (copy_from_user(mdp4_debug_buf, buff, count))
+                return -EFAULT;
+
+
+        mdp4_debug_buf[count] = 0;      /* end of string */
+
+        cnt = sscanf(mdp4_debug_buf, "%x", &data);
+        if (cnt < 1) {
+                printk(KERN_ERR "%s: sscanf failed cnt=%d" , __func__, cnt);
+                return -EINVAL;
+        }
+
+        writel(&data, mdp4_base_addr + mdp4_debug_offset);
+
+        return 0;
+}
+
+static ssize_t mdp4_debugfs_read(
+        struct file *file,
+        char __user *buff,
+        size_t count,
+        loff_t *ppos)
+{
+        int len = 0;
+        unsigned int data;
+
+        printk(KERN_INFO "%s: offset=%d count=%d *ppos=%d\n",
+                __func__, (int)mdp4_debug_offset, (int)count, (int)*ppos);
+
+        if (*ppos)
+                return 0;       /* the end */
+
+        data = readl(mdp4_base_addr + mdp4_debug_offset);
+
+        len = snprintf(mdp4_debug_buf, 4, "%x\n", data);
+
+        if (len > 0) {
+                if (len > count)
+                        len = count;
+                if (copy_to_user(buff, mdp4_debug_buf, len))
+                        return -EFAULT;
+        }
+
+        printk(KERN_INFO "%s: len=%d\n", __func__, len);
+
+        if (len < 0)
+                return 0;
+
+        *ppos += len;   /* increase offset */
+
+        return len;
+}
+
+static const struct file_operations mdp4_debugfs_fops = {
+        .open = mdp4_debugfs_open,
+        .release = mdp4_debugfs_release,
+        .read = mdp4_debugfs_read,
+        .write = mdp4_debugfs_write,
+};
+
+int mdp4_debugfs_init(void)
+{
+        struct dentry *dent = debugfs_create_dir("mdp4", NULL);
+
+        if (IS_ERR(dent)) {
+                printk(KERN_ERR "%s(%d): debugfs_create_dir fail, error %ld\n",
+                        __FILE__, __LINE__, PTR_ERR(dent));
+                return -1;
+        }
+
+        if (debugfs_create_file("offset", 0644, dent, 0, &mdp4_offset_fops)
+                        == NULL) {
+                printk(KERN_ERR "%s(%d): debugfs_create_file: offset fail\n",
+                        __FILE__, __LINE__);
+                return -1;
+        }
+
+        if (debugfs_create_file("regs", 0644, dent, 0, &mdp4_debugfs_fops)
+                        == NULL) {
+                printk(KERN_ERR "%s(%d): debugfs_create_file: regs fail\n",
+                        __FILE__, __LINE__);
+                return -1;
+        }
+
+        mdp4_debug_offset = 0;
+        mdp4_base_addr = (ulong) msm_mdp_base;  /* defined at msm_fb_def.h */
+
+        return 0;
+}
diff --git a/drivers/staging/msm/mdp4_overlay.c b/drivers/staging/msm/mdp4_overlay.c
new file mode 100644
index 000000000000..304bb8297635
--- /dev/null
+++ b/drivers/staging/msm/mdp4_overlay.c
@@ -0,0 +1,1259 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/hrtimer.h>
+#include <linux/clk.h>
+#include <mach/hardware.h>
+#include <linux/io.h>
+#include <linux/debugfs.h>
+#include <linux/fb.h>
+#include <msm_mdp.h>
+#include <linux/file.h>
+#include "android_pmem.h"
+#include <linux/major.h>
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/uaccess.h>
+#include <linux/mutex.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+#include "mdp4.h"
+
+
+struct mdp4_overlay_ctrl {
+        struct mdp4_overlay_pipe plist[MDP4_MAX_OVERLAY_PIPE];
+        struct mdp4_overlay_pipe *stage[MDP4_MAX_MIXER][MDP4_MAX_STAGE];
+} mdp4_overlay_db;
+
+static struct mdp4_overlay_ctrl *ctrl = &mdp4_overlay_db;
+
+
+void mdp4_overlay_dmap_cfg(struct msm_fb_data_type *mfd, int lcdc)
+{
+        uint32  dma2_cfg_reg;
+
+        dma2_cfg_reg = DMA_DITHER_EN;
+
+        if (mfd->fb_imgType == MDP_BGR_565)
+                dma2_cfg_reg |= DMA_PACK_PATTERN_BGR;
+        else
+                dma2_cfg_reg |= DMA_PACK_PATTERN_RGB;
+
+
+        if (mfd->panel_info.bpp == 18) {
+                dma2_cfg_reg |= DMA_DSTC0G_6BITS |      /* 666 18BPP */
+                    DMA_DSTC1B_6BITS | DMA_DSTC2R_6BITS;
+        } else if (mfd->panel_info.bpp == 16) {
+                dma2_cfg_reg |= DMA_DSTC0G_6BITS |      /* 565 16BPP */
+                    DMA_DSTC1B_5BITS | DMA_DSTC2R_5BITS;
+        } else {
+                dma2_cfg_reg |= DMA_DSTC0G_8BITS |      /* 888 16BPP */
+                    DMA_DSTC1B_8BITS | DMA_DSTC2R_8BITS;
+        }
+
+        if (lcdc)
+                dma2_cfg_reg |= DMA_PACK_ALIGN_MSB;
+
+        /* dma2 config register */
+        MDP_OUTP(MDP_BASE + 0x90000, dma2_cfg_reg);
+
+}
+
+void mdp4_overlay_dmap_xy(struct mdp4_overlay_pipe *pipe)
+{
+
+        /* dma_p source */
+        MDP_OUTP(MDP_BASE + 0x90004,
+                        (pipe->src_height << 16 | pipe->src_width));
+        MDP_OUTP(MDP_BASE + 0x90008, pipe->srcp0_addr);
+        MDP_OUTP(MDP_BASE + 0x9000c, pipe->srcp0_ystride);
+
+        /* dma_p dest */
+        MDP_OUTP(MDP_BASE + 0x90010, (pipe->dst_y << 16 | pipe->dst_x));
+}
+
+#define MDP4_VG_PHASE_STEP_DEFAULT      0x20000000
+#define MDP4_VG_PHASE_STEP_SHIFT        29
+
+static int mdp4_leading_0(uint32 num)
+{
+        uint32 bit = 0x80000000;
+        int i;
+
+        for (i = 0; i < 32; i++) {
+                if (bit & num)
+                        return i;
+                bit >>= 1;
+        }
+
+        return i;
+}
+
+static uint32 mdp4_scale_phase_step(int f_num, uint32 src, uint32 dst)
+{
+        uint32 val;
+        int     n;
+
+        n = mdp4_leading_0(src);
+        if (n > f_num)
+                n = f_num;
+        val = src << n; /* maximum to reduce lose of resolution */
+        val /= dst;
+        if (n < f_num) {
+                n = f_num - n;
+                val <<= n;
+        }
+
+        return val;
+}
+
+static void mdp4_scale_setup(struct mdp4_overlay_pipe *pipe)
+{
+
+        pipe->phasex_step = MDP4_VG_PHASE_STEP_DEFAULT;
+        pipe->phasey_step = MDP4_VG_PHASE_STEP_DEFAULT;
+
+        if (pipe->dst_h && pipe->src_h != pipe->dst_h) {
+                if (pipe->dst_h >= pipe->src_h * 8)     /* too much */
+                        return;
+                pipe->op_mode |= MDP4_OP_SCALEY_EN;
+
+                if (pipe->pipe_type == OVERLAY_TYPE_VG) {
+                        if (pipe->dst_h <= (pipe->src_h / 4))
+                                pipe->op_mode |= MDP4_OP_SCALEY_MN_PHASE;
+                        else
+                                pipe->op_mode |= MDP4_OP_SCALEY_FIR;
+                }
+
+                pipe->phasey_step = mdp4_scale_phase_step(29,
+                                        pipe->src_h, pipe->dst_h);
+        }
+
+        if (pipe->dst_w && pipe->src_w != pipe->dst_w) {
+                if (pipe->dst_w >= pipe->src_w * 8)     /* too much */
+                        return;
+                pipe->op_mode |= MDP4_OP_SCALEX_EN;
+
+                if (pipe->pipe_type == OVERLAY_TYPE_VG) {
+                        if (pipe->dst_w <= (pipe->src_w / 4))
+                                pipe->op_mode |= MDP4_OP_SCALEY_MN_PHASE;
+                        else
+                                pipe->op_mode |= MDP4_OP_SCALEY_FIR;
+                }
+
+                pipe->phasex_step = mdp4_scale_phase_step(29,
+                                        pipe->src_w, pipe->dst_w);
+        }
+}
+
+void mdp4_overlay_rgb_setup(struct mdp4_overlay_pipe *pipe)
+{
+        char *rgb_base;
+        uint32 src_size, src_xy, dst_size, dst_xy;
+        uint32 format, pattern;
+
+        rgb_base = MDP_BASE + MDP4_RGB_BASE;
+        rgb_base += (MDP4_RGB_OFF * pipe->pipe_num);
+
+        src_size = ((pipe->src_h << 16) | pipe->src_w);
+        src_xy = ((pipe->src_y << 16) | pipe->src_x);
+        dst_size = ((pipe->dst_h << 16) | pipe->dst_w);
+        dst_xy = ((pipe->dst_y << 16) | pipe->dst_x);
+
+        format = mdp4_overlay_format(pipe);
+        pattern = mdp4_overlay_unpack_pattern(pipe);
+
+        pipe->op_mode |= MDP4_OP_IGC_LUT_EN;
+
+        mdp4_scale_setup(pipe);
+
+        outpdw(rgb_base + 0x0000, src_size);    /* MDP_RGB_SRC_SIZE */
+        outpdw(rgb_base + 0x0004, src_xy);      /* MDP_RGB_SRC_XY */
+        outpdw(rgb_base + 0x0008, dst_size);    /* MDP_RGB_DST_SIZE */
+        outpdw(rgb_base + 0x000c, dst_xy);      /* MDP_RGB_DST_XY */
+
+        outpdw(rgb_base + 0x0010, pipe->srcp0_addr);
+        outpdw(rgb_base + 0x0040, pipe->srcp0_ystride);
+
+        outpdw(rgb_base + 0x0050, format);/* MDP_RGB_SRC_FORMAT */
+        outpdw(rgb_base + 0x0054, pattern);/* MDP_RGB_SRC_UNPACK_PATTERN */
+        outpdw(rgb_base + 0x0058, pipe->op_mode);/* MDP_RGB_OP_MODE */
+        outpdw(rgb_base + 0x005c, pipe->phasex_step);
+        outpdw(rgb_base + 0x0060, pipe->phasey_step);
+
+        /* 16 bytes-burst x 3 req <= 48 bytes */
+        outpdw(rgb_base + 0x1004, 0xc2);        /* MDP_RGB_FETCH_CFG */
+}
+
+void mdp4_overlay_vg_setup(struct mdp4_overlay_pipe *pipe)
+{
+        char *vg_base;
+        uint32 frame_size, src_size, src_xy, dst_size, dst_xy;
+        uint32 format, pattern;
+
+        vg_base = MDP_BASE + MDP4_VIDEO_BASE;
+        vg_base += (MDP4_VIDEO_OFF * pipe->pipe_num);
+
+        frame_size = ((pipe->src_height << 16) | pipe->src_width);
+        src_size = ((pipe->src_h << 16) | pipe->src_w);
+        src_xy = ((pipe->src_y << 16) | pipe->src_x);
+        dst_size = ((pipe->dst_h << 16) | pipe->dst_w);
+        dst_xy = ((pipe->dst_y << 16) | pipe->dst_x);
+
+        format = mdp4_overlay_format(pipe);
+        pattern = mdp4_overlay_unpack_pattern(pipe);
+
+        pipe->op_mode |= (MDP4_OP_CSC_EN | MDP4_OP_SRC_DATA_YCBCR |
+                                MDP4_OP_IGC_LUT_EN);
+
+        mdp4_scale_setup(pipe);
+
+        outpdw(vg_base + 0x0000, src_size);     /* MDP_RGB_SRC_SIZE */
+        outpdw(vg_base + 0x0004, src_xy);       /* MDP_RGB_SRC_XY */
+        outpdw(vg_base + 0x0008, dst_size);     /* MDP_RGB_DST_SIZE */
+        outpdw(vg_base + 0x000c, dst_xy);       /* MDP_RGB_DST_XY */
+        outpdw(vg_base + 0x0048, frame_size);   /* TILE frame size */
+
+        /* luma component plane */
+        outpdw(vg_base + 0x0010, pipe->srcp0_addr);
+
+        /* chroma component plane */
+        outpdw(vg_base + 0x0014, pipe->srcp1_addr);
+
+        outpdw(vg_base + 0x0040,
+                        pipe->srcp1_ystride << 16 | pipe->srcp0_ystride);
+
+        outpdw(vg_base + 0x0050, format);       /* MDP_RGB_SRC_FORMAT */
+        outpdw(vg_base + 0x0054, pattern);      /* MDP_RGB_SRC_UNPACK_PATTERN */
+        outpdw(vg_base + 0x0058, pipe->op_mode);/* MDP_RGB_OP_MODE */
+        outpdw(vg_base + 0x005c, pipe->phasex_step);
+        outpdw(vg_base + 0x0060, pipe->phasey_step);
+
+        if (pipe->op_mode & MDP4_OP_DITHER_EN) {
+                outpdw(vg_base + 0x0068,
+                        pipe->r_bit << 4 | pipe->b_bit << 2 | pipe->g_bit);
+        }
+
+        /* 16 bytes-burst x 3 req <= 48 bytes */
+        outpdw(vg_base + 0x1004, 0xc2); /* MDP_VG_FETCH_CFG */
+}
+
+int mdp4_overlay_format2type(uint32 format)
+{
+        switch (format) {
+        case MDP_RGB_565:
+        case MDP_RGB_888:
+        case MDP_BGR_565:
+        case MDP_ARGB_8888:
+        case MDP_RGBA_8888:
+        case MDP_BGRA_8888:
+                return OVERLAY_TYPE_RGB;
+        case MDP_YCRYCB_H2V1:
+        case MDP_Y_CRCB_H2V1:
+        case MDP_Y_CBCR_H2V1:
+        case MDP_Y_CRCB_H2V2:
+        case MDP_Y_CBCR_H2V2:
+        case MDP_Y_CBCR_H2V2_TILE:
+        case MDP_Y_CRCB_H2V2_TILE:
+                return OVERLAY_TYPE_VG;
+        default:
+                return -ERANGE;
+        }
+
+}
+
+#define C3_ALPHA        3       /* alpha */
+#define C2_R_Cr         2       /* R/Cr */
+#define C1_B_Cb         1       /* B/Cb */
+#define C0_G_Y          0       /* G/luma */
+
+int mdp4_overlay_format2pipe(struct mdp4_overlay_pipe *pipe)
+{
+        switch (pipe->src_format) {
+        case MDP_RGB_565:
+                pipe->frame_format = MDP4_FRAME_FORMAT_LINEAR;
+                pipe->fetch_plane = OVERLAY_PLANE_INTERLEAVED;
+                pipe->a_bit = 0;
+                pipe->r_bit = 1;        /* R, 5 bits */
+                pipe->b_bit = 1;        /* B, 5 bits */
+                pipe->g_bit = 2;        /* G, 6 bits */
+                pipe->alpha_enable = 0;
+                pipe->unpack_tight = 1;
+                pipe->unpack_align_msb = 0;
+                pipe->unpack_count = 2;
+                pipe->element2 = C2_R_Cr;       /* R */
+                pipe->element1 = C0_G_Y;        /* G */
+                pipe->element0 = C1_B_Cb;       /* B */
+                pipe->bpp = 2;  /* 2 bpp */
+                break;
+        case MDP_RGB_888:
+                pipe->frame_format = MDP4_FRAME_FORMAT_LINEAR;
+                pipe->fetch_plane = OVERLAY_PLANE_INTERLEAVED;
+                pipe->a_bit = 0;
+                pipe->r_bit = 3;        /* R, 8 bits */
+                pipe->b_bit = 3;        /* B, 8 bits */
+                pipe->g_bit = 3;        /* G, 8 bits */
+                pipe->alpha_enable = 0;
+                pipe->unpack_tight = 1;
+                pipe->unpack_align_msb = 0;
+                pipe->unpack_count = 2;
+                pipe->element2 = C2_R_Cr;       /* R */
+                pipe->element1 = C0_G_Y;        /* G */
+                pipe->element0 = C1_B_Cb;       /* B */
+                pipe->bpp = 3;  /* 3 bpp */
+                break;
+        case MDP_BGR_565:
+                pipe->frame_format = MDP4_FRAME_FORMAT_LINEAR;
+                pipe->fetch_plane = OVERLAY_PLANE_INTERLEAVED;
+                pipe->a_bit = 0;
+                pipe->r_bit = 1;        /* R, 5 bits */
+                pipe->b_bit = 1;        /* B, 5 bits */
+                pipe->g_bit = 2;        /* G, 6 bits */
+                pipe->alpha_enable = 0;
+                pipe->unpack_tight = 1;
+                pipe->unpack_align_msb = 0;
+                pipe->unpack_count = 2;
+                pipe->element2 = C1_B_Cb;       /* B */
+                pipe->element1 = C0_G_Y;        /* G */
+                pipe->element0 = C2_R_Cr;       /* R */
+                pipe->bpp = 2;  /* 2 bpp */
+                break;
+        case MDP_ARGB_8888:
+                pipe->frame_format = MDP4_FRAME_FORMAT_LINEAR;
+                pipe->fetch_plane = OVERLAY_PLANE_INTERLEAVED;
+                pipe->a_bit = 3;        /* alpha, 4 bits */
+                pipe->r_bit = 3;        /* R, 8 bits */
+                pipe->b_bit = 3;        /* B, 8 bits */
+                pipe->g_bit = 3;        /* G, 8 bits */
+                pipe->alpha_enable = 1;
+                pipe->unpack_tight = 1;
+                pipe->unpack_align_msb = 0;
+                pipe->unpack_count = 3;
+                pipe->element3 = C3_ALPHA;      /* alpha */
+                pipe->element2 = C2_R_Cr;       /* R */
+                pipe->element1 = C0_G_Y;        /* G */
+                pipe->element0 = C1_B_Cb;       /* B */
+                pipe->bpp = 4;          /* 4 bpp */
+                break;
+        case MDP_RGBA_8888:
+                pipe->frame_format = MDP4_FRAME_FORMAT_LINEAR;
+                pipe->fetch_plane = OVERLAY_PLANE_INTERLEAVED;
+                pipe->a_bit = 3;        /* alpha, 4 bits */
+                pipe->r_bit = 3;        /* R, 8 bits */
+                pipe->b_bit = 3;        /* B, 8 bits */
+                pipe->g_bit = 3;        /* G, 8 bits */
+                pipe->alpha_enable = 1;
+                pipe->unpack_tight = 1;
+                pipe->unpack_align_msb = 0;
+                pipe->unpack_count = 3;
+                pipe->element3 = C2_R_Cr;       /* R */
+                pipe->element2 = C0_G_Y;        /* G */
+                pipe->element1 = C1_B_Cb;       /* B */
+                pipe->element0 = C3_ALPHA;      /* alpha */
+                pipe->bpp = 4;          /* 4 bpp */
+                break;
+        case MDP_BGRA_8888:
+                pipe->frame_format = MDP4_FRAME_FORMAT_LINEAR;
+                pipe->fetch_plane = OVERLAY_PLANE_INTERLEAVED;
+                pipe->a_bit = 3;        /* alpha, 4 bits */
+                pipe->r_bit = 3;        /* R, 8 bits */
+                pipe->b_bit = 3;        /* B, 8 bits */
+                pipe->g_bit = 3;        /* G, 8 bits */
+                pipe->alpha_enable = 1;
+                pipe->unpack_tight = 1;
+                pipe->unpack_align_msb = 0;
+                pipe->unpack_count = 3;
+                pipe->element3 = C1_B_Cb;       /* B */
+                pipe->element2 = C0_G_Y;        /* G */
+                pipe->element1 = C2_R_Cr;       /* R */
+                pipe->element0 = C3_ALPHA;      /* alpha */
+                pipe->bpp = 4;          /* 4 bpp */
+                break;
+        case MDP_YCRYCB_H2V1:
+                pipe->frame_format = MDP4_FRAME_FORMAT_LINEAR;
+                pipe->fetch_plane = OVERLAY_PLANE_INTERLEAVED;
+                pipe->a_bit = 0;        /* alpha, 4 bits */
+                pipe->r_bit = 3;        /* R, 8 bits */
+                pipe->b_bit = 3;        /* B, 8 bits */
+                pipe->g_bit = 3;        /* G, 8 bits */
+                pipe->alpha_enable = 0;
+                pipe->unpack_tight = 1;
+                pipe->unpack_align_msb = 0;
+                pipe->unpack_count = 3;
+                pipe->element3 = C0_G_Y;        /* G */
+                pipe->element2 = C2_R_Cr;       /* R */
+                pipe->element1 = C0_G_Y;        /* G */
+                pipe->element0 = C1_B_Cb;       /* B */
+                pipe->bpp = 2;          /* 2 bpp */
+                pipe->chroma_sample = MDP4_CHROMA_H2V1;
+                break;
+        case MDP_Y_CRCB_H2V1:
+        case MDP_Y_CBCR_H2V1:
+        case MDP_Y_CRCB_H2V2:
+        case MDP_Y_CBCR_H2V2:
+                pipe->frame_format = MDP4_FRAME_FORMAT_LINEAR;
+                pipe->fetch_plane = OVERLAY_PLANE_PSEUDO_PLANAR;
+                pipe->a_bit = 0;
+                pipe->r_bit = 3;        /* R, 8 bits */
+                pipe->b_bit = 3;        /* B, 8 bits */
+                pipe->g_bit = 3;        /* G, 8 bits */
+                pipe->alpha_enable = 0;
+                pipe->unpack_tight = 1;
+                pipe->unpack_align_msb = 0;
+                pipe->unpack_count = 1;         /* 2 */
+                pipe->element3 = C0_G_Y;        /* not used */
+                pipe->element2 = C0_G_Y;        /* not used */
+                if (pipe->src_format == MDP_Y_CRCB_H2V1) {
+                        pipe->element1 = C2_R_Cr;       /* R */
+                        pipe->element0 = C1_B_Cb;       /* B */
+                        pipe->chroma_sample = MDP4_CHROMA_H2V1;
+                } else if (pipe->src_format == MDP_Y_CBCR_H2V1) {
+                        pipe->element1 = C1_B_Cb;       /* B */
+                        pipe->element0 = C2_R_Cr;       /* R */
+                        pipe->chroma_sample = MDP4_CHROMA_H2V1;
+                } else if (pipe->src_format == MDP_Y_CRCB_H2V2) {
+                        pipe->element1 = C2_R_Cr;       /* R */
+                        pipe->element0 = C1_B_Cb;       /* B */
+                        pipe->chroma_sample = MDP4_CHROMA_420;
+                } else if (pipe->src_format == MDP_Y_CBCR_H2V2) {
+                        pipe->element1 = C1_B_Cb;       /* B */
+                        pipe->element0 = C2_R_Cr;       /* R */
+                        pipe->chroma_sample = MDP4_CHROMA_420;
+                }
+                pipe->bpp = 2;  /* 2 bpp */
+                break;
+        case MDP_Y_CBCR_H2V2_TILE:
+        case MDP_Y_CRCB_H2V2_TILE:
+                pipe->frame_format = MDP4_FRAME_FORMAT_VIDEO_SUPERTILE;
+                pipe->fetch_plane = OVERLAY_PLANE_PSEUDO_PLANAR;
+                pipe->a_bit = 0;
+                pipe->r_bit = 3;        /* R, 8 bits */
+                pipe->b_bit = 3;        /* B, 8 bits */
+                pipe->g_bit = 3;        /* G, 8 bits */
+                pipe->alpha_enable = 0;
+                pipe->unpack_tight = 1;
+                pipe->unpack_align_msb = 0;
+                pipe->unpack_count = 1;         /* 2 */
+                pipe->element3 = C0_G_Y;        /* not used */
+                pipe->element2 = C0_G_Y;        /* not used */
+                if (pipe->src_format == MDP_Y_CRCB_H2V2_TILE) {
+                        pipe->element1 = C2_R_Cr;       /* R */
+                        pipe->element0 = C1_B_Cb;       /* B */
+                        pipe->chroma_sample = MDP4_CHROMA_420;
+                } else if (pipe->src_format == MDP_Y_CBCR_H2V2_TILE) {
+                        pipe->element1 = C1_B_Cb;       /* B */
+                        pipe->element0 = C2_R_Cr;       /* R */
+                        pipe->chroma_sample = MDP4_CHROMA_420;
+                }
+                pipe->bpp = 2;  /* 2 bpp */
+                break;
+        default:
+                /* not likely */
+                return -ERANGE;
+        }
+
+        return 0;
+}
+
+/*
+ * color_key_convert: output with 12 bits color key
+ */
+static uint32 color_key_convert(int start, int num, uint32 color)
+{
+
+        uint32 data;
+
+        data = (color >> start) & ((1 << num) - 1);
+
+        if (num == 5)
+                data = (data << 7) + (data << 2) + (data >> 3);
+        else if (num == 6)
+                data = (data << 6) + data;
+        else    /* 8 bits */
+                data = (data << 4) + (data >> 4);
+
+        return data;
+
+}
+
+void transp_color_key(int format, uint32 transp,
+                        uint32 *c0, uint32 *c1, uint32 *c2)
+{
+        int b_start, g_start, r_start;
+        int b_num, g_num, r_num;
+
+        switch (format) {
+        case MDP_RGB_565:
+                b_start = 0;
+                g_start = 5;
+                r_start = 11;
+                r_num = 5;
+                g_num = 6;
+                b_num = 5;
+                break;
+        case MDP_RGB_888:
+        case MDP_XRGB_8888:
+        case MDP_ARGB_8888:
+                b_start = 0;
+                g_start = 8;
+                r_start = 16;
+                r_num = 8;
+                g_num = 8;
+                b_num = 8;
+                break;
+        case MDP_BGR_565:
+                b_start = 11;
+                g_start = 5;
+                r_start = 0;
+                r_num = 5;
+                g_num = 6;
+                b_num = 5;
+                break;
+        case MDP_Y_CBCR_H2V2:
+        case MDP_Y_CBCR_H2V1:
+                b_start = 8;
+                g_start = 16;
+                r_start = 0;
+                r_num = 8;
+                g_num = 8;
+                b_num = 8;
+                break;
+        case MDP_Y_CRCB_H2V2:
+        case MDP_Y_CRCB_H2V1:
+                b_start = 0;
+                g_start = 16;
+                r_start = 8;
+                r_num = 8;
+                g_num = 8;
+                b_num = 8;
+                break;
+        default:
+                b_start = 0;
+                g_start = 8;
+                r_start = 16;
+                r_num = 8;
+                g_num = 8;
+                b_num = 8;
+                break;
+        }
+
+        *c0 = color_key_convert(g_start, g_num, transp);
+        *c1 = color_key_convert(b_start, b_num, transp);
+        *c2 = color_key_convert(r_start, r_num, transp);
+}
+
+uint32 mdp4_overlay_format(struct mdp4_overlay_pipe *pipe)
+{
+        uint32  format;
+
+        format = 0;
+
+        if (pipe->solid_fill)
+                format |= MDP4_FORMAT_SOLID_FILL;
+
+        if (pipe->unpack_align_msb)
+                format |= MDP4_FORMAT_UNPACK_ALIGN_MSB;
+
+        if (pipe->unpack_tight)
+                format |= MDP4_FORMAT_UNPACK_TIGHT;
+
+        if (pipe->alpha_enable)
+                format |= MDP4_FORMAT_ALPHA_ENABLE;
+
+        format |= (pipe->unpack_count << 13);
+        format |= ((pipe->bpp - 1) << 9);
+        format |= (pipe->a_bit << 6);
+        format |= (pipe->r_bit << 4);
+        format |= (pipe->b_bit << 2);
+        format |= pipe->g_bit;
+
+        format |= (pipe->frame_format << 29);
+
+        if (pipe->fetch_plane == OVERLAY_PLANE_PSEUDO_PLANAR) {
+                /* video/graphic */
+                format |= (pipe->fetch_plane << 19);
+                format |= (pipe->chroma_site << 28);
+                format |= (pipe->chroma_sample << 26);
+        }
+
+        return format;
+}
+
+uint32 mdp4_overlay_unpack_pattern(struct mdp4_overlay_pipe *pipe)
+{
+        return (pipe->element3 << 24) | (pipe->element2 << 16) |
+                        (pipe->element1 << 8) | pipe->element0;
+}
+
+void mdp4_overlayproc_cfg(struct mdp4_overlay_pipe *pipe)
+{
+        uint32 data;
+        char *overlay_base;
+
+        if (pipe->mixer_num == MDP4_MIXER1)
+                overlay_base = MDP_BASE + MDP4_OVERLAYPROC1_BASE;/* 0x18000 */
+        else
+                overlay_base = MDP_BASE + MDP4_OVERLAYPROC0_BASE;/* 0x10000 */
+
+        /* MDP_OVERLAYPROC_CFG */
+        outpdw(overlay_base + 0x0004, 0x01); /* directout */
+        data = pipe->src_height;
+        data <<= 16;
+        data |= pipe->src_width;
+        outpdw(overlay_base + 0x0008, data); /* ROI, height + width */
+        outpdw(overlay_base + 0x000c, pipe->srcp0_addr);
+        outpdw(overlay_base + 0x0010, pipe->srcp0_ystride);
+        outpdw(overlay_base + 0x0014, 0x4);     /* GC_LUT_EN, 888 */
+}
+
+int mdp4_overlay_active(int mixer)
+{
+        uint32 data, mask, i;
+        int p1, p2;
+
+        data = inpdw(MDP_BASE + 0x10100);
+        p1 = 0;
+        p2 = 0;
+        for (i = 0; i < 8; i++) {
+                mask = data & 0x0f;
+                if (mask) {
+                        if (mask <= 4)
+                                p1++;
+                        else
+                                p2++;
+                }
+                data >>= 4;
+        }
+
+        if (mixer)
+                return p2;
+        else
+                return p1;
+}
+
+void mdp4_mixer_stage_up(struct mdp4_overlay_pipe *pipe)
+{
+        uint32 data, mask, snum, stage, mixer;
+
+        stage = pipe->mixer_stage;
+        mixer = pipe->mixer_num;
+
+        /* MDP_LAYERMIXER_IN_CFG, shard by both mixer 0 and 1  */
+        data = inpdw(MDP_BASE + 0x10100);
+
+        if (mixer == MDP4_MIXER1)
+                stage += 8;
+
+        if (pipe->pipe_type == OVERLAY_TYPE_VG) {/* VG1 and VG2 */
+                snum = 0;
+                snum += (4 * pipe->pipe_num);
+        } else {
+                snum = 8;
+                snum += (4 * pipe->pipe_num);   /* RGB1 and RGB2 */
+        }
+
+        mask = 0x0f;
+        mask <<= snum;
+        stage <<= snum;
+        data &= ~mask;  /* clear old bits */
+
+        data |= stage;
+
+        outpdw(MDP_BASE + 0x10100, data); /* MDP_LAYERMIXER_IN_CFG */
+
+        data = inpdw(MDP_BASE + 0x10100);
+
+        ctrl->stage[pipe->mixer_num][pipe->mixer_stage] = pipe; /* keep it */
+}
+
+void mdp4_mixer_stage_down(struct mdp4_overlay_pipe *pipe)
+{
+        uint32 data, mask, snum, stage, mixer;
+
+        stage = pipe->mixer_stage;
+        mixer = pipe->mixer_num;
+
+        if (pipe != ctrl->stage[mixer][stage])  /* not runing */
+                return;
+
+        /* MDP_LAYERMIXER_IN_CFG, shard by both mixer 0 and 1  */
+        data = inpdw(MDP_BASE + 0x10100);
+
+        if (mixer == MDP4_MIXER1)
+                stage += 8;
+
+        if (pipe->pipe_type == OVERLAY_TYPE_VG) {/* VG1 and VG2 */
+                snum = 0;
+                snum += (4 * pipe->pipe_num);
+        } else {
+                snum = 8;
+                snum += (4 * pipe->pipe_num);   /* RGB1 and RGB2 */
+        }
+
+        mask = 0x0f;
+        mask <<= snum;
+        data &= ~mask;  /* clear old bits */
+
+        outpdw(MDP_BASE + 0x10100, data); /* MDP_LAYERMIXER_IN_CFG */
+
+        data = inpdw(MDP_BASE + 0x10100);
+
+        ctrl->stage[pipe->mixer_num][pipe->mixer_stage] = NULL; /* clear it */
+}
+
+void mdp4_mixer_blend_setup(struct mdp4_overlay_pipe *pipe)
+{
+        unsigned char *overlay_base;
+        uint32 c0, c1, c2, blend_op;
+        int off;
+
+        if (pipe->mixer_num)    /* mixer number, /dev/fb0, /dev/fb1 */
+                overlay_base = MDP_BASE + MDP4_OVERLAYPROC1_BASE;/* 0x18000 */
+        else
+                overlay_base = MDP_BASE + MDP4_OVERLAYPROC0_BASE;/* 0x10000 */
+
+        /* stage 0 to stage 2 */
+        off = 0x20 * (pipe->mixer_stage - MDP4_MIXER_STAGE0);
+
+        blend_op = 0;
+        if (pipe->alpha_enable)         /* ARGB */
+                blend_op = MDP4_BLEND_FG_ALPHA_FG_PIXEL |
+                                MDP4_BLEND_BG_ALPHA_FG_PIXEL;
+        else
+                blend_op = (MDP4_BLEND_BG_ALPHA_BG_CONST |
+                                MDP4_BLEND_FG_ALPHA_FG_CONST);
+
+
+        if (pipe->alpha_enable == 0) {  /* not ARGB */
+                if (pipe->is_fg) {
+                        outpdw(overlay_base + off + 0x108, pipe->alpha);
+                        outpdw(overlay_base + off + 0x10c, 0xff - pipe->alpha);
+                } else {
+                        outpdw(overlay_base + off + 0x108, 0xff - pipe->alpha);
+                        outpdw(overlay_base + off + 0x10c, pipe->alpha);
+                }
+        }
+
+        if (pipe->transp != MDP_TRANSP_NOP) {
+                transp_color_key(pipe->src_format, pipe->transp, &c0, &c1, &c2);
+                if (pipe->is_fg) {
+                        blend_op |= MDP4_BLEND_FG_TRANSP_EN; /* Fg blocked */
+                        /* lower limit */
+                        if (c0 > 0x10)
+                                c0 -= 0x10;
+                        if (c1 > 0x10)
+                                c1 -= 0x10;
+                        if (c2 > 0x10)
+                                c2 -= 0x10;
+                        outpdw(overlay_base + off + 0x110,
+                                                (c1 << 16 | c0));/* low */
+                        outpdw(overlay_base + off + 0x114, c2);/* low */
+                        /* upper limit */
+                        if ((c0 + 0x20) < 0x0fff)
+                                c0 += 0x20;
+                        else
+                                c0 = 0x0fff;
+                        if ((c1 + 0x20) < 0x0fff)
+                                c1 += 0x20;
+                        else
+                                c1 = 0x0fff;
+                        if ((c2 + 0x20) < 0x0fff)
+                                c2 += 0x20;
+                        else
+                                c2 = 0x0fff;
+                        outpdw(overlay_base + off + 0x118,
+                                        (c1 << 16 | c0));/* high */
+                        outpdw(overlay_base + off + 0x11c, c2);/* high */
+                } else {
+                        blend_op |= MDP4_BLEND_BG_TRANSP_EN; /* bg blocked */
+                        /* lower limit */
+                        if (c0 > 0x10)
+                                c0 -= 0x10;
+                        if (c1 > 0x10)
+                                c1 -= 0x10;
+                        if (c2 > 0x10)
+                                c2 -= 0x10;
+                        outpdw(overlay_base + 0x180,
+                                                (c1 << 16 | c0));/* low */
+                        outpdw(overlay_base + 0x184, c2);/* low */
+                        /* upper limit */
+                        if ((c0 + 0x20) < 0x0fff)
+                                c0 += 0x20;
+                        else
+                                c0 = 0x0fff;
+                        if ((c1 + 0x20) < 0x0fff)
+                                c1 += 0x20;
+                        else
+                                c1 = 0x0fff;
+                        if ((c2 + 0x20) < 0x0fff)
+                                c2 += 0x20;
+                        else
+                                c2 = 0x0fff;
+                        outpdw(overlay_base + 0x188,
+                                                (c1 << 16 | c0));/* high */
+                        outpdw(overlay_base + 0x18c, c2);/* high */
+                }
+        }
+        outpdw(overlay_base + off + 0x104, blend_op);
+}
+
+void mdp4_overlay_reg_flush(struct mdp4_overlay_pipe *pipe, int all)
+{
+        uint32 bits = 0;
+
+        if (pipe->mixer_num == MDP4_MIXER1)
+                bits |= 0x02;
+        else
+                bits |= 0x01;
+
+        if (all) {
+                if (pipe->pipe_type == OVERLAY_TYPE_RGB) {
+                        if (pipe->pipe_num == OVERLAY_PIPE_RGB2)
+                                bits |= 0x20;
+                        else
+                                bits |= 0x10;
+                } else {
+                        if (pipe->pipe_num == OVERLAY_PIPE_VG2)
+                                bits |= 0x08;
+                        else
+                                bits |= 0x04;
+                }
+        }
+
+        outpdw(MDP_BASE + 0x18000, bits);       /* MDP_OVERLAY_REG_FLUSH */
+
+        while (inpdw(MDP_BASE + 0x18000) & bits) /* self clear when complete */
+                ;
+}
+
+struct mdp4_overlay_pipe *mdp4_overlay_ndx2pipe(int ndx)
+{
+        struct mdp4_overlay_pipe *pipe;
+
+        if (ndx == 0 || ndx >= MDP4_MAX_OVERLAY_PIPE)
+                return NULL;
+
+        pipe = &ctrl->plist[ndx - 1];   /* ndx start from 1 */
+
+        if (pipe->pipe_ndx == 0)
+                return NULL;
+
+        return pipe;
+}
+
+struct mdp4_overlay_pipe *mdp4_overlay_pipe_alloc(void)
+{
+        int i;
+        struct mdp4_overlay_pipe *pipe;
+
+        pipe = &ctrl->plist[0];
+        for (i = 0; i < MDP4_MAX_OVERLAY_PIPE; i++) {
+                if (pipe->pipe_ndx == 0) {
+                        pipe->pipe_ndx = i + 1; /* start from 1 */
+                        init_completion(&pipe->comp);
+        printk(KERN_INFO "mdp4_overlay_pipe_alloc: pipe=%x ndx=%d\n",
+                                        (int)pipe, pipe->pipe_ndx);
+                        return pipe;
+                }
+                pipe++;
+        }
+
+        return NULL;
+}
+
+
+void mdp4_overlay_pipe_free(struct mdp4_overlay_pipe *pipe)
+{
+        printk(KERN_INFO "mdp4_overlay_pipe_free: pipe=%x ndx=%d\n",
+                                        (int)pipe, pipe->pipe_ndx);
+        memset(pipe, 0, sizeof(*pipe));
+}
+
+static int get_pipe_num(int ptype, int stage)
+{
+        if (ptype == OVERLAY_TYPE_RGB) {
+                if (stage == MDP4_MIXER_STAGE_BASE)
+                        return OVERLAY_PIPE_RGB1;
+                else
+                        return OVERLAY_PIPE_RGB2;
+        } else {
+                if (stage == MDP4_MIXER_STAGE0)
+                        return OVERLAY_PIPE_VG1;
+                else
+                        return OVERLAY_PIPE_VG2;
+        }
+}
+
+int mdp4_overlay_req_check(uint32 id, uint32 z_order, uint32 mixer)
+{
+        struct mdp4_overlay_pipe *pipe;
+
+        pipe = ctrl->stage[mixer][z_order];
+
+        if (pipe == NULL)
+                return 0;
+
+        if (pipe->pipe_ndx == id)       /* same req, recycle */
+                return 0;
+
+        return -EPERM;
+}
+
+static int mdp4_overlay_req2pipe(struct mdp_overlay *req, int mixer,
+                        struct mdp4_overlay_pipe **ppipe)
+{
+        struct mdp4_overlay_pipe *pipe;
+        int ret, ptype;
+
+        if (mixer >= MDP4_MAX_MIXER) {
+                printk(KERN_ERR "mpd_overlay_req2pipe: mixer out of range!\n");
+                return -ERANGE;
+        }
+
+        if (req->z_order < 0 || req->z_order > 2) {
+                printk(KERN_ERR "mpd_overlay_req2pipe: z_order=%d out of range!\n",
+                                req->z_order);
+                return -ERANGE;
+        }
+
+        if (req->src_rect.h == 0 || req->src_rect.w == 0) {
+                printk(KERN_ERR "mpd_overlay_req2pipe: src img of zero size!\n");
+                return -EINVAL;
+        }
+
+        ret = mdp4_overlay_req_check(req->id, req->z_order, mixer);
+        if (ret < 0)
+                return ret;
+
+        ptype = mdp4_overlay_format2type(req->src.format);
+        if (ptype < 0)
+                return ptype;
+
+        if (req->id == MSMFB_NEW_REQUEST)  /* new request */
+                pipe = mdp4_overlay_pipe_alloc();
+        else
+                pipe = mdp4_overlay_ndx2pipe(req->id);
+
+        if (pipe == NULL)
+                return -ENOMEM;
+
+        pipe->src_format = req->src.format;
+        ret = mdp4_overlay_format2pipe(pipe);
+
+        if (ret < 0)
+                return ret;
+
+        /*
+         * base layer == 1, reserved for frame buffer
+         * zorder 0 == stage 0 == 2
+         * zorder 1 == stage 1 == 3
+         * zorder 2 == stage 2 == 4
+         */
+        if (req->id == MSMFB_NEW_REQUEST) {  /* new request */
+                pipe->mixer_stage = req->z_order + MDP4_MIXER_STAGE0;
+                pipe->pipe_type = ptype;
+                pipe->pipe_num = get_pipe_num(ptype, pipe->mixer_stage);
+                printk(KERN_INFO "mpd4_overlay_req2pipe: zorder=%d pipe_num=%d\n",
+                                req->z_order, pipe->pipe_num);
+        }
+
+        pipe->src_width = req->src.width & 0x07ff;      /* source img width */
+        pipe->src_height = req->src.height & 0x07ff;    /* source img height */
+        pipe->src_h = req->src_rect.h & 0x07ff;
+        pipe->src_w = req->src_rect.w & 0x07ff;
+        pipe->src_y = req->src_rect.y & 0x07ff;
+        pipe->src_x = req->src_rect.x & 0x07ff;
+        pipe->dst_h = req->dst_rect.h & 0x07ff;
+        pipe->dst_w = req->dst_rect.w & 0x07ff;
+        pipe->dst_y = req->dst_rect.y & 0x07ff;
+        pipe->dst_x = req->dst_rect.x & 0x07ff;
+
+        if (req->flags & MDP_FLIP_LR)
+                pipe->op_mode |= MDP4_OP_FLIP_LR;
+
+        if (req->flags & MDP_FLIP_UD)
+                pipe->op_mode |= MDP4_OP_FLIP_UD;
+
+        if (req->flags & MDP_DITHER)
+                pipe->op_mode |= MDP4_OP_DITHER_EN;
+
+        if (req->flags & MDP_DEINTERLACE)
+                pipe->op_mode |= MDP4_OP_DEINT_ODD_REF;
+
+        pipe->is_fg = req->is_fg;/* control alpha and color key */
+
+        pipe->alpha = req->alpha & 0x0ff;
+
+        pipe->transp = req->transp_mask;
+
+        *ppipe = pipe;
+
+        return 0;
+}
+
+int get_img(struct msmfb_data *img, struct fb_info *info,
+        unsigned long *start, unsigned long *len, struct file **pp_file)
+{
+        int put_needed, ret = 0;
+        struct file *file;
+#ifdef CONFIG_ANDROID_PMEM
+        unsigned long vstart;
+#endif
+
+#ifdef CONFIG_ANDROID_PMEM
+        if (!get_pmem_file(img->memory_id, start, &vstart, len, pp_file))
+                return 0;
+#endif
+        file = fget_light(img->memory_id, &put_needed);
+        if (file == NULL)
+                return -1;
+
+        if (MAJOR(file->f_dentry->d_inode->i_rdev) == FB_MAJOR) {
+                *start = info->fix.smem_start;
+                *len = info->fix.smem_len;
+                *pp_file = file;
+        } else {
+                ret = -1;
+                fput_light(file, put_needed);
+        }
+        return ret;
+}
+int mdp4_overlay_get(struct fb_info *info, struct mdp_overlay *req)
+{
+        struct mdp4_overlay_pipe *pipe;
+
+        pipe = mdp4_overlay_ndx2pipe(req->id);
+        if (pipe == NULL)
+                return -ENODEV;
+
+        *req = pipe->req_data;
+
+        return 0;
+}
+
+int mdp4_overlay_set(struct fb_info *info, struct mdp_overlay *req)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        int ret, mixer;
+        struct mdp4_overlay_pipe *pipe;
+        int lcdc;
+
+        if (mfd == NULL)
+                return -ENODEV;
+
+        if (req->src.format == MDP_FB_FORMAT)
+                req->src.format = mfd->fb_imgType;
+
+        if (mutex_lock_interruptible(&mfd->dma->ov_mutex))
+                return -EINTR;
+
+        mixer = info->node; /* minor number of char device */
+
+        ret = mdp4_overlay_req2pipe(req, mixer, &pipe);
+        if (ret < 0) {
+                mutex_unlock(&mfd->dma->ov_mutex);
+                return ret;
+        }
+
+        lcdc = inpdw(MDP_BASE + 0xc0000);
+
+        if (lcdc == 0) { /* mddi */
+                /* MDP cmd block enable */
+                mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+        }
+
+        /* return id back to user */
+        req->id = pipe->pipe_ndx;       /* pipe_ndx start from 1 */
+        pipe->req_data = *req;          /* keep original req */
+
+        mutex_unlock(&mfd->dma->ov_mutex);
+
+        return 0;
+}
+
+int mdp4_overlay_unset(struct fb_info *info, int ndx)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        struct mdp4_overlay_pipe *pipe;
+        int lcdc;
+
+        if (mfd == NULL)
+                return -ENODEV;
+
+        if (mutex_lock_interruptible(&mfd->dma->ov_mutex))
+                return -EINTR;
+
+        pipe = mdp4_overlay_ndx2pipe(ndx);
+
+        if (pipe == NULL) {
+                mutex_unlock(&mfd->dma->ov_mutex);
+                return -ENODEV;
+        }
+
+        lcdc = inpdw(MDP_BASE + 0xc0000);
+
+        mdp4_mixer_stage_down(pipe);
+
+        if (lcdc == 0) { /* mddi */
+                /* MDP cmd block disable */
+                mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+        }
+
+        if (lcdc) /* LCDC mode */
+                mdp4_overlay_reg_flush(pipe, 0);
+
+        mdp4_overlay_pipe_free(pipe);
+
+        if (lcdc == 0) { /* mddi */
+                mdp4_mddi_overlay_restore();
+        }
+
+        mutex_unlock(&mfd->dma->ov_mutex);
+
+        return 0;
+}
+
+struct tile_desc {
+        uint32 width;  /* tile's width */
+        uint32 height; /* tile's height */
+        uint32 row_tile_w; /* tiles per row's width */
+        uint32 row_tile_h; /* tiles per row's height */
+};
+
+void tile_samsung(struct tile_desc *tp)
+{
+        /*
+         * each row of samsung tile consists of two tiles in height
+         * and two tiles in width which means width should align to
+         * 64 x 2 bytes and height should align to 32 x 2 bytes.
+         * video decoder generate two tiles in width and one tile
+         * in height which ends up height align to 32 X 1 bytes.
+         */
+        tp->width = 64;         /* 64 bytes */
+        tp->row_tile_w = 2;     /* 2 tiles per row's width */
+        tp->height = 32;        /* 32 bytes */
+        tp->row_tile_h = 1;     /* 1 tiles per row's height */
+}
+
+uint32 tile_mem_size(struct mdp4_overlay_pipe *pipe, struct tile_desc *tp)
+{
+        uint32 tile_w, tile_h;
+        uint32 row_num_w, row_num_h;
+
+
+        tile_w = tp->width * tp->row_tile_w;
+        tile_h = tp->height * tp->row_tile_h;
+
+        row_num_w = (pipe->src_width + tile_w - 1) / tile_w;
+        row_num_h = (pipe->src_height + tile_h - 1) / tile_h;
+
+        return row_num_w * row_num_h * tile_w * tile_h;
+}
+
+int mdp4_overlay_play(struct fb_info *info, struct msmfb_overlay_data *req,
+                struct file **pp_src_file)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        struct msmfb_data *img;
+        struct mdp4_overlay_pipe *pipe;
+        ulong start, addr;
+        ulong len = 0;
+        struct file *p_src_file = 0;
+        int lcdc;
+
+        if (mfd == NULL)
+                return -ENODEV;
+
+        pipe = mdp4_overlay_ndx2pipe(req->id);
+        if (pipe == NULL)
+                return -ENODEV;
+
+        if (mutex_lock_interruptible(&mfd->dma->ov_mutex))
+                return -EINTR;
+
+        img = &req->data;
+        get_img(img, info, &start, &len, &p_src_file);
+        if (len == 0) {
+                mutex_unlock(&mfd->dma->ov_mutex);
+                printk(KERN_ERR "mdp_overlay_play: could not retrieve"
+                                       " image from memory\n");
+                return -1;
+        }
+        *pp_src_file = p_src_file;
+
+        addr = start + img->offset;
+        pipe->srcp0_addr = addr;
+        pipe->srcp0_ystride = pipe->src_width * pipe->bpp;
+
+        if (pipe->fetch_plane == OVERLAY_PLANE_PSEUDO_PLANAR) {
+                if (pipe->frame_format == MDP4_FRAME_FORMAT_VIDEO_SUPERTILE) {
+                        struct tile_desc tile;
+
+                        tile_samsung(&tile);
+                        pipe->srcp1_addr = addr + tile_mem_size(pipe, &tile);
+                } else
+                        pipe->srcp1_addr = addr +
+                                        pipe->src_width * pipe->src_height;
+
+                pipe->srcp0_ystride = pipe->src_width;
+                pipe->srcp1_ystride = pipe->src_width;
+        }
+
+        lcdc = inpdw(MDP_BASE + 0xc0000);
+        lcdc &= 0x01; /* LCDC mode */
+
+        if (pipe->pipe_type == OVERLAY_TYPE_VG)
+                mdp4_overlay_vg_setup(pipe);    /* video/graphic pipe */
+        else
+                mdp4_overlay_rgb_setup(pipe);   /* rgb pipe */
+
+        mdp4_mixer_blend_setup(pipe);
+        mdp4_mixer_stage_up(pipe);
+
+        if (lcdc) { /* LCDC mode */
+                mdp4_overlay_reg_flush(pipe, 1);
+        }
+
+        if (lcdc) { /* LCDC mode */
+                if (pipe->mixer_stage != MDP4_MIXER_STAGE_BASE) { /* done */
+                        mutex_unlock(&mfd->dma->ov_mutex);
+                        return 0;
+                }
+        }
+
+        if (lcdc == 0) { /* MDDI mode */
+#ifdef MDP4_NONBLOCKING
+                if (mfd->panel_power_on)
+#else
+                if (!mfd->dma->busy && mfd->panel_power_on)
+#endif
+                        mdp4_mddi_overlay_kickoff(mfd, pipe);
+        }
+
+        mutex_unlock(&mfd->dma->ov_mutex);
+
+        return 0;
+}
diff --git a/drivers/staging/msm/mdp4_overlay_lcdc.c b/drivers/staging/msm/mdp4_overlay_lcdc.c
new file mode 100644
index 000000000000..a6ab8ec83f55
--- /dev/null
+++ b/drivers/staging/msm/mdp4_overlay_lcdc.c
@@ -0,0 +1,313 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+#include <linux/delay.h>
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/spinlock.h>
+
+#include <linux/fb.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+#include "mdp4.h"
+
+#ifdef CONFIG_FB_MSM_MDP40
+#define LCDC_BASE       0xC0000
+#else
+#define LCDC_BASE       0xE0000
+#endif
+
+int first_pixel_start_x;
+int first_pixel_start_y;
+
+static struct mdp4_overlay_pipe *lcdc_pipe;
+
+int mdp_lcdc_on(struct platform_device *pdev)
+{
+        int lcdc_width;
+        int lcdc_height;
+        int lcdc_bpp;
+        int lcdc_border_clr;
+        int lcdc_underflow_clr;
+        int lcdc_hsync_skew;
+
+        int hsync_period;
+        int hsync_ctrl;
+        int vsync_period;
+        int display_hctl;
+        int display_v_start;
+        int display_v_end;
+        int active_hctl;
+        int active_h_start;
+        int active_h_end;
+        int active_v_start;
+        int active_v_end;
+        int ctrl_polarity;
+        int h_back_porch;
+        int h_front_porch;
+        int v_back_porch;
+        int v_front_porch;
+        int hsync_pulse_width;
+        int vsync_pulse_width;
+        int hsync_polarity;
+        int vsync_polarity;
+        int data_en_polarity;
+        int hsync_start_x;
+        int hsync_end_x;
+        uint8 *buf;
+        int bpp, ptype;
+        uint32 format;
+        struct fb_info *fbi;
+        struct fb_var_screeninfo *var;
+        struct msm_fb_data_type *mfd;
+        struct mdp4_overlay_pipe *pipe;
+        int ret;
+
+        mfd = (struct msm_fb_data_type *)platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        fbi = mfd->fbi;
+        var = &fbi->var;
+
+        /* MDP cmd block enable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+        bpp = fbi->var.bits_per_pixel / 8;
+        buf = (uint8 *) fbi->fix.smem_start;
+        buf += fbi->var.xoffset * bpp +
+                fbi->var.yoffset * fbi->fix.line_length;
+
+        if (bpp == 2)
+                format = MDP_RGB_565;
+        else if (bpp == 3)
+                format = MDP_RGB_888;
+        else
+                format = MDP_ARGB_8888;
+
+
+        if (lcdc_pipe == NULL) {
+                ptype = mdp4_overlay_format2type(format);
+                pipe = mdp4_overlay_pipe_alloc();
+                pipe->pipe_type = ptype;
+                /* use RGB1 pipe */
+                pipe->pipe_num  = OVERLAY_PIPE_RGB1;
+                pipe->mixer_stage  = MDP4_MIXER_STAGE_BASE;
+                pipe->mixer_num  = MDP4_MIXER0;
+                pipe->src_format = format;
+                mdp4_overlay_format2pipe(pipe);
+
+                lcdc_pipe = pipe; /* keep it */
+        } else {
+                pipe = lcdc_pipe;
+        }
+
+        pipe->src_height = fbi->var.yres;
+        pipe->src_width = fbi->var.xres;
+        pipe->src_h = fbi->var.yres;
+        pipe->src_w = fbi->var.xres;
+        pipe->src_y = 0;
+        pipe->src_x = 0;
+        pipe->srcp0_addr = (uint32) buf;
+        pipe->srcp0_ystride = fbi->fix.line_length;
+
+        mdp4_overlay_dmap_xy(pipe);
+        mdp4_overlay_dmap_cfg(mfd, 1);
+
+        mdp4_overlay_rgb_setup(pipe);
+
+        mdp4_mixer_stage_up(pipe);
+
+        mdp4_overlayproc_cfg(pipe);
+
+        /*
+         * LCDC timing setting
+         */
+        h_back_porch = var->left_margin;
+        h_front_porch = var->right_margin;
+        v_back_porch = var->upper_margin;
+        v_front_porch = var->lower_margin;
+        hsync_pulse_width = var->hsync_len;
+        vsync_pulse_width = var->vsync_len;
+        lcdc_border_clr = mfd->panel_info.lcdc.border_clr;
+        lcdc_underflow_clr = mfd->panel_info.lcdc.underflow_clr;
+        lcdc_hsync_skew = mfd->panel_info.lcdc.hsync_skew;
+
+        lcdc_width = mfd->panel_info.xres;
+        lcdc_height = mfd->panel_info.yres;
+        lcdc_bpp = mfd->panel_info.bpp;
+
+        hsync_period =
+            hsync_pulse_width + h_back_porch + lcdc_width + h_front_porch;
+        hsync_ctrl = (hsync_period << 16) | hsync_pulse_width;
+        hsync_start_x = hsync_pulse_width + h_back_porch;
+        hsync_end_x = hsync_period - h_front_porch - 1;
+        display_hctl = (hsync_end_x << 16) | hsync_start_x;
+
+        vsync_period =
+            (vsync_pulse_width + v_back_porch + lcdc_height +
+             v_front_porch) * hsync_period;
+        display_v_start =
+            (vsync_pulse_width + v_back_porch) * hsync_period + lcdc_hsync_skew;
+        display_v_end =
+            vsync_period - (v_front_porch * hsync_period) + lcdc_hsync_skew - 1;
+
+        if (lcdc_width != var->xres) {
+                active_h_start = hsync_start_x + first_pixel_start_x;
+                active_h_end = active_h_start + var->xres - 1;
+                active_hctl =
+                    ACTIVE_START_X_EN | (active_h_end << 16) | active_h_start;
+        } else {
+                active_hctl = 0;
+        }
+
+        if (lcdc_height != var->yres) {
+                active_v_start =
+                    display_v_start + first_pixel_start_y * hsync_period;
+                active_v_end = active_v_start + (var->yres) * hsync_period - 1;
+                active_v_start |= ACTIVE_START_Y_EN;
+        } else {
+                active_v_start = 0;
+                active_v_end = 0;
+        }
+
+
+#ifdef CONFIG_FB_MSM_MDP40
+        hsync_polarity = 1;
+        vsync_polarity = 1;
+        lcdc_underflow_clr |= 0x80000000;       /* enable recovery */
+#else
+        hsync_polarity = 0;
+        vsync_polarity = 0;
+#endif
+        data_en_polarity = 0;
+
+        ctrl_polarity =
+            (data_en_polarity << 2) | (vsync_polarity << 1) | (hsync_polarity);
+
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0x4, hsync_ctrl);
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0x8, vsync_period);
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0xc, vsync_pulse_width * hsync_period);
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0x10, display_hctl);
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0x14, display_v_start);
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0x18, display_v_end);
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0x28, lcdc_border_clr);
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0x2c, lcdc_underflow_clr);
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0x30, lcdc_hsync_skew);
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0x38, ctrl_polarity);
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0x1c, active_hctl);
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0x20, active_v_start);
+        MDP_OUTP(MDP_BASE + LCDC_BASE + 0x24, active_v_end);
+
+        ret = panel_next_on(pdev);
+        if (ret == 0) {
+                /* enable LCDC block */
+                MDP_OUTP(MDP_BASE + LCDC_BASE, 1);
+                mdp_pipe_ctrl(MDP_DMA2_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+        }
+        /* MDP cmd block disable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+
+        return ret;
+}
+
+int mdp_lcdc_off(struct platform_device *pdev)
+{
+        int ret = 0;
+        struct mdp4_overlay_pipe *pipe;
+
+        /* MDP cmd block enable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+        MDP_OUTP(MDP_BASE + LCDC_BASE, 0);
+        /* MDP cmd block disable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+        mdp_pipe_ctrl(MDP_DMA2_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+
+        ret = panel_next_off(pdev);
+
+        /* delay to make sure the last frame finishes */
+        mdelay(100);
+
+        /* dis-engage rgb0 from mixer */
+        pipe = lcdc_pipe;
+        mdp4_mixer_stage_down(pipe);
+
+        return ret;
+}
+
+/*
+ * mdp4_overlay0_done_lcdc: called from isr
+ */
+void mdp4_overlay0_done_lcdc()
+{
+        complete(&lcdc_pipe->comp);
+}
+
+void mdp4_lcdc_overlay(struct msm_fb_data_type *mfd)
+{
+        struct fb_info *fbi = mfd->fbi;
+        uint8 *buf;
+        int bpp;
+        unsigned long flag;
+        struct mdp4_overlay_pipe *pipe;
+
+        if (!mfd->panel_power_on)
+                return;
+
+        /* no need to power on cmd block since it's lcdc mode */
+        bpp = fbi->var.bits_per_pixel / 8;
+        buf = (uint8 *) fbi->fix.smem_start;
+        buf += fbi->var.xoffset * bpp +
+                fbi->var.yoffset * fbi->fix.line_length;
+
+        mutex_lock(&mfd->dma->ov_mutex);
+
+        pipe = lcdc_pipe;
+        pipe->srcp0_addr = (uint32) buf;
+        mdp4_overlay_rgb_setup(pipe);
+        mdp4_overlay_reg_flush(pipe, 1); /* rgb1 and mixer0 */
+
+        /* enable irq */
+        spin_lock_irqsave(&mdp_spin_lock, flag);
+        mdp_enable_irq(MDP_OVERLAY0_TERM);
+        INIT_COMPLETION(lcdc_pipe->comp);
+        mfd->dma->waiting = TRUE;
+        outp32(MDP_INTR_CLEAR, INTR_OVERLAY0_DONE);
+        mdp_intr_mask |= INTR_OVERLAY0_DONE;
+        outp32(MDP_INTR_ENABLE, mdp_intr_mask);
+        spin_unlock_irqrestore(&mdp_spin_lock, flag);
+        wait_for_completion_killable(&lcdc_pipe->comp);
+        mdp_disable_irq(MDP_OVERLAY0_TERM);
+
+        mutex_unlock(&mfd->dma->ov_mutex);
+}
diff --git a/drivers/staging/msm/mdp4_overlay_mddi.c b/drivers/staging/msm/mdp4_overlay_mddi.c
new file mode 100644
index 000000000000..be1b2874185b
--- /dev/null
+++ b/drivers/staging/msm/mdp4_overlay_mddi.c
@@ -0,0 +1,254 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+#include <linux/delay.h>
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/spinlock.h>
+
+#include <linux/fb.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+#include "mdp4.h"
+
+static struct mdp4_overlay_pipe *mddi_pipe;
+static struct mdp4_overlay_pipe *pending_pipe;
+static struct msm_fb_data_type *mddi_mfd;
+
+#define WHOLESCREEN
+
+void mdp4_overlay_update_lcd(struct msm_fb_data_type *mfd)
+{
+        MDPIBUF *iBuf = &mfd->ibuf;
+        uint8 *src;
+        int bpp, ptype;
+        uint32 format;
+        uint32 mddi_ld_param;
+        uint16 mddi_vdo_packet_reg;
+        struct mdp4_overlay_pipe *pipe;
+
+        if (mfd->key != MFD_KEY)
+                return;
+
+        mddi_mfd = mfd;         /* keep it */
+
+        bpp = iBuf->bpp;
+
+        if (bpp == 2)
+                format = MDP_RGB_565;
+        else if (bpp == 3)
+                format = MDP_RGB_888;
+        else
+                format = MDP_ARGB_8888;
+
+        /* MDP cmd block enable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+        if (mddi_pipe == NULL) {
+                ptype = mdp4_overlay_format2type(format);
+                pipe = mdp4_overlay_pipe_alloc();
+                pipe->pipe_type = ptype;
+                /* use RGB1 pipe */
+                pipe->pipe_num  = OVERLAY_PIPE_RGB1;
+                pipe->mixer_num  = MDP4_MIXER0;
+                pipe->src_format = format;
+                mdp4_overlay_format2pipe(pipe);
+
+                mddi_pipe = pipe; /* keep it */
+
+                mddi_ld_param = 0;
+                mddi_vdo_packet_reg = mfd->panel_info.mddi.vdopkt;
+
+                if (mfd->panel_info.type == MDDI_PANEL) {
+                        if (mfd->panel_info.pdest == DISPLAY_1)
+                                mddi_ld_param = 0;
+                        else
+                                mddi_ld_param = 1;
+                } else {
+                        mddi_ld_param = 2;
+                }
+
+                MDP_OUTP(MDP_BASE + 0x00090, mddi_ld_param);
+                MDP_OUTP(MDP_BASE + 0x00094,
+                         (MDDI_VDO_PACKET_DESC << 16) | mddi_vdo_packet_reg);
+        } else {
+                pipe = mddi_pipe;
+        }
+
+
+        src = (uint8 *) iBuf->buf;
+
+#ifdef WHOLESCREEN
+        {
+                struct fb_info *fbi;
+
+                fbi = mfd->fbi;
+                pipe->src_height = fbi->var.yres;
+                pipe->src_width = fbi->var.xres;
+                pipe->src_h = fbi->var.yres;
+                pipe->src_w = fbi->var.xres;
+                pipe->src_y = 0;
+                pipe->src_x = 0;
+                pipe->dst_h = fbi->var.yres;
+                pipe->dst_w = fbi->var.xres;
+                pipe->dst_y = 0;
+                pipe->dst_x = 0;
+                pipe->srcp0_addr = (uint32)src;
+                pipe->srcp0_ystride = fbi->var.xres_virtual * bpp;
+        }
+
+#else
+        if (mdp4_overlay_active(MDP4_MIXER0)) {
+                struct fb_info *fbi;
+
+                fbi = mfd->fbi;
+                pipe->src_height = fbi->var.yres;
+                pipe->src_width = fbi->var.xres;
+                pipe->src_h = fbi->var.yres;
+                pipe->src_w = fbi->var.xres;
+                pipe->src_y = 0;
+                pipe->src_x = 0;
+                pipe->dst_h = fbi->var.yres;
+                pipe->dst_w = fbi->var.xres;
+                pipe->dst_y = 0;
+                pipe->dst_x = 0;
+                pipe->srcp0_addr = (uint32) src;
+                pipe->srcp0_ystride = fbi->var.xres_virtual * bpp;
+        } else {
+                /* starting input address */
+                src += (iBuf->dma_x + iBuf->dma_y * iBuf->ibuf_width) * bpp;
+
+                pipe->src_height = iBuf->dma_h;
+                pipe->src_width = iBuf->dma_w;
+                pipe->src_h = iBuf->dma_h;
+                pipe->src_w = iBuf->dma_w;
+                pipe->src_y = 0;
+                pipe->src_x = 0;
+                pipe->dst_h = iBuf->dma_h;
+                pipe->dst_w = iBuf->dma_w;
+                pipe->dst_y = iBuf->dma_y;
+                pipe->dst_x = iBuf->dma_x;
+                pipe->srcp0_addr = (uint32) src;
+                pipe->srcp0_ystride = iBuf->ibuf_width * bpp;
+        }
+#endif
+
+        pipe->mixer_stage  = MDP4_MIXER_STAGE_BASE;
+
+        mdp4_overlay_rgb_setup(pipe);
+
+        mdp4_mixer_stage_up(pipe);
+
+        mdp4_overlayproc_cfg(pipe);
+
+        mdp4_overlay_dmap_xy(pipe);
+
+        mdp4_overlay_dmap_cfg(mfd, 0);
+
+        /* MDP cmd block disable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+
+}
+
+/*
+ * mdp4_overlay0_done_mddi: called from isr
+ */
+void mdp4_overlay0_done_mddi()
+{
+        if (pending_pipe)
+                complete(&pending_pipe->comp);
+}
+
+void mdp4_mddi_overlay_restore(void)
+{
+        /* mutex holded by caller */
+        mdp4_overlay_update_lcd(mddi_mfd);
+        mdp4_mddi_overlay_kickoff(mddi_mfd, mddi_pipe);
+}
+
+void mdp4_mddi_overlay_kickoff(struct msm_fb_data_type *mfd,
+                                struct mdp4_overlay_pipe *pipe)
+{
+#ifdef MDP4_NONBLOCKING
+        unsigned long flag;
+
+        spin_lock_irqsave(&mdp_spin_lock, flag);
+        if (mfd->dma->busy == TRUE) {
+                INIT_COMPLETION(pipe->comp);
+                pending_pipe = pipe;
+        }
+        spin_unlock_irqrestore(&mdp_spin_lock, flag);
+
+        if (pending_pipe != NULL) {
+                /* wait until DMA finishes the current job */
+                wait_for_completion_killable(&pipe->comp);
+                pending_pipe = NULL;
+        }
+        down(&mfd->sem);
+        mdp_enable_irq(MDP_OVERLAY0_TERM);
+        mfd->dma->busy = TRUE;
+        /* start OVERLAY pipe */
+        mdp_pipe_kickoff(MDP_OVERLAY0_TERM, mfd);
+        up(&mfd->sem);
+#else
+        down(&mfd->sem);
+        mdp_enable_irq(MDP_OVERLAY0_TERM);
+        mfd->dma->busy = TRUE;
+        INIT_COMPLETION(pipe->comp);
+        pending_pipe = pipe;
+
+        /* start OVERLAY pipe */
+        mdp_pipe_kickoff(MDP_OVERLAY0_TERM, mfd);
+        up(&mfd->sem);
+
+        /* wait until DMA finishes the current job */
+        wait_for_completion_killable(&pipe->comp);
+        mdp_disable_irq(MDP_OVERLAY0_TERM);
+#endif
+
+}
+
+void mdp4_mddi_overlay(struct msm_fb_data_type *mfd)
+{
+        mutex_lock(&mfd->dma->ov_mutex);
+
+        if ((mfd) && (!mfd->dma->busy) && (mfd->panel_power_on)) {
+                mdp4_overlay_update_lcd(mfd);
+
+                mdp4_mddi_overlay_kickoff(mfd, mddi_pipe);
+
+        /* signal if pan function is waiting for the update completion */
+                if (mfd->pan_waiting) {
+                        mfd->pan_waiting = FALSE;
+                        complete(&mfd->pan_comp);
+                }
+        }
+
+        mutex_unlock(&mfd->dma->ov_mutex);
+}
diff --git a/drivers/staging/msm/mdp4_util.c b/drivers/staging/msm/mdp4_util.c
new file mode 100644
index 000000000000..fd97f5205992
--- /dev/null
+++ b/drivers/staging/msm/mdp4_util.c
@@ -0,0 +1,1686 @@
+
+/* Copyright (c) 2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/hrtimer.h>
+#include <linux/clk.h>
+#include <mach/hardware.h>
+#include <linux/io.h>
+#include <linux/debugfs.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/uaccess.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+#include "mdp4.h"
+
+void mdp4_sw_reset(ulong bits)
+{
+        bits &= 0x1f;   /* 5 bits */
+        outpdw(MDP_BASE + 0x001c, bits);        /* MDP_SW_RESET */
+
+        while (inpdw(MDP_BASE + 0x001c) & bits) /* self clear when complete */
+                ;
+        MSM_FB_INFO("mdp4_sw_reset: 0x%x\n", (int)bits);
+}
+
+void mdp4_overlay_cfg(int overlayer, int blt_mode, int refresh, int direct_out)
+{
+        ulong bits = 0;
+
+        if (blt_mode)
+                bits |= (1 << 3);
+        refresh &= 0x03;        /* 2 bites */
+        bits |= (refresh << 1);
+        direct_out &= 0x01;
+        bits |= direct_out;
+
+        if (overlayer == MDP4_MIXER0)
+                outpdw(MDP_BASE + 0x10004, bits); /* MDP_OVERLAY0_CFG */
+        else
+                outpdw(MDP_BASE + 0x18004, bits); /* MDP_OVERLAY1_CFG */
+
+        MSM_FB_INFO("mdp4_overlay_cfg: 0x%x\n", (int)inpdw(MDP_BASE + 0x10004));
+}
+
+void mdp4_display_intf_sel(int output, ulong intf)
+{
+        ulong bits, mask;
+
+        bits = inpdw(MDP_BASE + 0x0038);        /* MDP_DISP_INTF_SEL */
+
+        mask = 0x03;    /* 2 bits */
+        intf &= 0x03;   /* 2 bits */
+
+        switch (output) {
+        case EXTERNAL_INTF_SEL:
+                intf <<= 4;
+                mask <<= 4;
+                break;
+        case SECONDARY_INTF_SEL:
+                intf &= 0x02;   /* only MDDI and EBI2 support */
+                intf <<= 2;
+                mask <<= 2;
+                break;
+        default:
+                break;
+        }
+
+
+        bits &= ~mask;
+        bits |= intf;
+
+        outpdw(MDP_BASE + 0x0038, bits);        /* MDP_DISP_INTF_SEL */
+
+  MSM_FB_INFO("mdp4_display_intf_sel: 0x%x\n", (int)inpdw(MDP_BASE + 0x0038));
+}
+
+unsigned long mdp4_display_status(void)
+{
+        return inpdw(MDP_BASE + 0x0018) & 0x3ff;        /* MDP_DISPLAY_STATUS */
+}
+
+void mdp4_ebi2_lcd_setup(int lcd, ulong base, int ystride)
+{
+        /* always use memory map */
+        ystride &= 0x01fff;     /* 13 bits */
+        if (lcd == EBI2_LCD0) {
+                outpdw(MDP_BASE + 0x0060, base);/* MDP_EBI2_LCD0 */
+                outpdw(MDP_BASE + 0x0068, ystride);/* MDP_EBI2_LCD0_YSTRIDE */
+        } else {
+                outpdw(MDP_BASE + 0x0064, base);/* MDP_EBI2_LCD1 */
+                outpdw(MDP_BASE + 0x006c, ystride);/* MDP_EBI2_LCD1_YSTRIDE */
+        }
+}
+
+void mdp4_mddi_setup(int mddi, unsigned long id)
+{
+        ulong   bits;
+
+        if (mddi == MDDI_EXTERNAL_SET)
+                bits = 0x02;
+        else if (mddi == MDDI_SECONDARY_SET)
+                bits = 0x01;
+        else
+                bits = 0;       /* PRIMARY_SET */
+
+        id <<= 16;
+
+        bits |= id;
+
+        outpdw(MDP_BASE + 0x0090, bits); /* MDP_MDDI_PARAM_WR_SEL */
+}
+
+int mdp_ppp_blit(struct fb_info *info, struct mdp_blit_req *req,
+        struct file **pp_src_file, struct file **pp_dst_file)
+{
+
+        /* not implemented yet */
+        return -1;
+}
+
+void mdp4_hw_init(void)
+{
+        ulong bits;
+
+        /* MDP cmd block enable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+#ifdef MDP4_ERROR
+        /*
+         * Issue software reset on DMA_P will casue DMA_P dma engine stall
+         * on LCDC mode. However DMA_P does not stall at MDDI mode.
+         * This need further investigation.
+         */
+        mdp4_sw_reset(0x17);
+#endif
+
+        mdp4_clear_lcdc();
+
+        mdp4_mixer_blend_init(0);
+        mdp4_mixer_blend_init(1);
+        mdp4_vg_qseed_init(0);
+        mdp4_vg_qseed_init(1);
+        mdp4_vg_csc_mv_setup(0);
+        mdp4_vg_csc_mv_setup(1);
+        mdp4_vg_csc_pre_bv_setup(0);
+        mdp4_vg_csc_pre_bv_setup(1);
+        mdp4_vg_csc_post_bv_setup(0);
+        mdp4_vg_csc_post_bv_setup(1);
+        mdp4_vg_csc_pre_lv_setup(0);
+        mdp4_vg_csc_pre_lv_setup(1);
+        mdp4_vg_csc_post_lv_setup(0);
+        mdp4_vg_csc_post_lv_setup(1);
+
+        mdp4_mixer_gc_lut_setup(0);
+        mdp4_mixer_gc_lut_setup(1);
+
+        mdp4_vg_igc_lut_setup(0);
+        mdp4_vg_igc_lut_setup(1);
+
+         mdp4_rgb_igc_lut_setup(0);
+         mdp4_rgb_igc_lut_setup(1);
+
+        outp32(MDP_EBI2_PORTMAP_MODE, 0x3);
+
+        /* system interrupts */
+
+        bits =  mdp_intr_mask;
+        outpdw(MDP_BASE + 0x0050, bits);/* enable specififed interrupts */
+
+        /* histogram */
+        MDP_OUTP(MDP_BASE + 0x95010, 1);        /* auto clear HIST */
+
+        /* enable histogram interrupts */
+        outpdw(MDP_BASE + 0x9501c, INTR_HIST_DONE);
+
+        /* For the max read pending cmd config below, if the MDP clock     */
+        /* is less than the AXI clock, then we must use 3 pending          */
+        /* pending requests.  Otherwise, we should use 8 pending requests. */
+        /* In the future we should do this detection automatically.        */
+
+        /* max read pending cmd config */
+        outpdw(MDP_BASE + 0x004c, 0x02222);     /* 3 pending requests */
+
+        /* dma_p fetch config */
+        outpdw(MDP_BASE + 0x91004, 0x27);       /* burst size of 8 */
+
+#ifndef CONFIG_FB_MSM_OVERLAY
+        /* both REFRESH_MODE and DIRECT_OUT are ignored at BLT mode */
+        mdp4_overlay_cfg(MDP4_MIXER0, OVERLAY_MODE_BLT, 0, 0);
+        mdp4_overlay_cfg(MDP4_MIXER1, OVERLAY_MODE_BLT, 0, 0);
+#endif
+
+        /* MDP cmd block disable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+}
+
+
+void mdp4_clear_lcdc(void)
+{
+        uint32 bits;
+
+        bits = inpdw(MDP_BASE + 0xc0000);
+        if (bits & 0x01) /* enabled already */
+                return;
+
+        outpdw(MDP_BASE + 0xc0004, 0);  /* vsync ctrl out */
+        outpdw(MDP_BASE + 0xc0008, 0);  /* vsync period */
+        outpdw(MDP_BASE + 0xc000c, 0);  /* vsync pusle width */
+        outpdw(MDP_BASE + 0xc0010, 0);  /* lcdc display HCTL */
+        outpdw(MDP_BASE + 0xc0014, 0);  /* lcdc display v start */
+        outpdw(MDP_BASE + 0xc0018, 0);  /* lcdc display v end */
+        outpdw(MDP_BASE + 0xc001c, 0);  /* lcdc active hctl */
+        outpdw(MDP_BASE + 0xc0020, 0);  /* lcdc active v start */
+        outpdw(MDP_BASE + 0xc0024, 0);  /* lcdc active v end */
+        outpdw(MDP_BASE + 0xc0028, 0);  /* lcdc board color */
+        outpdw(MDP_BASE + 0xc002c, 0);  /* lcdc underflow ctrl */
+        outpdw(MDP_BASE + 0xc0030, 0);  /* lcdc hsync skew */
+        outpdw(MDP_BASE + 0xc0034, 0);  /* lcdc test ctl */
+        outpdw(MDP_BASE + 0xc0038, 0);  /* lcdc ctl polarity */
+}
+
+static struct mdp_dma_data overlay1_data;
+static int intr_dma_p;
+static int intr_dma_s;
+static int intr_dma_e;
+static int intr_overlay0;
+static int intr_overlay1;
+
+irqreturn_t mdp4_isr(int irq, void *ptr)
+{
+        uint32 isr, mask, lcdc;
+        struct mdp_dma_data *dma;
+
+        mdp_is_in_isr = TRUE;
+
+        while (1) {
+                isr = inpdw(MDP_INTR_STATUS);
+                if (isr == 0)
+                        break;
+
+                mask = inpdw(MDP_INTR_ENABLE);
+                outpdw(MDP_INTR_CLEAR, isr);
+
+                isr &= mask;
+
+                if (unlikely(isr == 0))
+                        break;
+
+                if (isr & INTR_DMA_P_DONE) {
+                        intr_dma_p++;
+                        lcdc = inpdw(MDP_BASE + 0xc0000);
+                        dma = &dma2_data;
+                        if (lcdc & 0x01) {      /* LCDC enable */
+                                /* disable LCDC interrupt */
+                                mdp_intr_mask &= ~INTR_DMA_P_DONE;
+                                outp32(MDP_INTR_ENABLE, mdp_intr_mask);
+                                dma->waiting = FALSE;
+                        } else {
+                                dma->busy = FALSE;
+                                mdp_pipe_ctrl(MDP_DMA2_BLOCK,
+                                        MDP_BLOCK_POWER_OFF, TRUE);
+                        }
+                        complete(&dma->comp);
+                }
+                if (isr & INTR_DMA_S_DONE) {
+                        intr_dma_s++;
+                        dma = &dma_s_data;
+                        dma->busy = FALSE;
+                        mdp_pipe_ctrl(MDP_DMA_S_BLOCK,
+                                        MDP_BLOCK_POWER_OFF, TRUE);
+                        complete(&dma->comp);
+                }
+                if (isr & INTR_DMA_E_DONE) {
+                        intr_dma_e++;
+                        dma = &dma_e_data;
+                        mdp_intr_mask &= ~INTR_DMA_E_DONE;
+                        outp32(MDP_INTR_ENABLE, mdp_intr_mask);
+                        dma->busy = FALSE;
+
+                        if (dma->waiting) {
+                                dma->waiting = FALSE;
+                                complete(&dma->comp);
+                        }
+                }
+                if (isr & INTR_OVERLAY0_DONE) {
+                        intr_overlay0++;
+                        lcdc = inpdw(MDP_BASE + 0xc0000);
+                        dma = &dma2_data;
+                        if (lcdc & 0x01) {      /* LCDC enable */
+                                /* disable LCDC interrupt */
+                                mdp_intr_mask &= ~INTR_OVERLAY0_DONE;
+                                outp32(MDP_INTR_ENABLE, mdp_intr_mask);
+                                dma->waiting = FALSE;
+                                mdp4_overlay0_done_lcdc();
+                        } else {        /* MDDI */
+                                dma->busy = FALSE;
+#ifdef MDP4_NONBLOCKING
+                                mdp_disable_irq_nolock(MDP_OVERLAY0_TERM);
+#endif
+                                mdp_pipe_ctrl(MDP_OVERLAY0_BLOCK,
+                                        MDP_BLOCK_POWER_OFF, TRUE);
+                                mdp4_overlay0_done_mddi();
+                        }
+                }
+                if (isr & INTR_OVERLAY1_DONE) {
+                        intr_overlay1++;
+                        dma = &overlay1_data;
+                        dma->busy = FALSE;
+                        mdp_pipe_ctrl(MDP_OVERLAY1_BLOCK,
+                                        MDP_BLOCK_POWER_OFF, TRUE);
+                        complete(&dma->comp);
+                }
+                if (isr & INTR_DMA_P_HISTOGRAM) {
+                        isr = inpdw(MDP_DMA_P_HIST_INTR_STATUS);
+                        mask = inpdw(MDP_DMA_P_HIST_INTR_ENABLE);
+                        outpdw(MDP_DMA_P_HIST_INTR_CLEAR, isr);
+                        isr &= mask;
+                        if (isr & INTR_HIST_DONE) {
+                                if (mdp_hist.r)
+                                        memcpy(mdp_hist.r, MDP_BASE + 0x95100,
+                                                        mdp_hist.bin_cnt*4);
+                                if (mdp_hist.g)
+                                        memcpy(mdp_hist.g, MDP_BASE + 0x95200,
+                                                        mdp_hist.bin_cnt*4);
+                                if (mdp_hist.b)
+                                        memcpy(mdp_hist.b, MDP_BASE + 0x95300,
+                                                mdp_hist.bin_cnt*4);
+                                complete(&mdp_hist_comp);
+                        }
+                }
+        }
+
+        mdp_is_in_isr = FALSE;
+
+        return IRQ_HANDLED;
+}
+
+
+/*
+ * QSEED tables
+ */
+
+static uint32 vg_qseed_table0[] = {
+        0x5556aaff, 0x00000000, 0x00000000, 0x00000000
+};
+
+static uint32 vg_qseed_table1[] = {
+        0x76543210, 0xfedcba98
+};
+
+static uint32 vg_qseed_table2[] = {
+        0x02000000, 0x00000000, 0x02060ff2, 0x00000008,
+        0x02090fe4, 0x00000013, 0x020a0fd9, 0x0ffc0021,
+        0x02080fce, 0x0ffa0030, 0x02030fc5, 0x0ff60042,
+        0x01fd0fbe, 0x0ff10054, 0x01f50fb6, 0x0fed0068,
+        0x01e90fb1, 0x0fe60080, 0x01dc0fae, 0x0fe10095,
+        0x01ca0fae, 0x0fda00ae, 0x01b70fad, 0x0fd600c6,
+        0x01a40fad, 0x0fcf00e0, 0x018f0faf, 0x0fc800fa,
+        0x01780fb1, 0x0fc30114, 0x015f0fb5, 0x0fbf012d,
+        0x01490fb7, 0x0fb70149, 0x012d0fbf, 0x0fb5015f,
+        0x01140fc3, 0x0fb10178, 0x00fa0fc8, 0x0faf018f,
+        0x00e00fcf, 0x0fad01a4, 0x00c60fd6, 0x0fad01b7,
+        0x00ae0fda, 0x0fae01ca, 0x00950fe1, 0x0fae01dc,
+        0x00800fe6, 0x0fb101e9, 0x00680fed, 0x0fb601f5,
+        0x00540ff1, 0x0fbe01fd, 0x00420ff6, 0x0fc50203,
+        0x00300ffa, 0x0fce0208, 0x00210ffc, 0x0fd9020a,
+        0x00130000, 0x0fe40209, 0x00080000, 0x0ff20206,
+        0x02000000, 0x00000000, 0x02040ff2, 0x0000000a,
+        0x02040fe4, 0x00000018, 0x02010fda, 0x0ffc0029,
+        0x01fc0fcf, 0x0ffa003b, 0x01f30fc7, 0x0ff60050,
+        0x01e90fc0, 0x0ff20065, 0x01dc0fba, 0x0fee007c,
+        0x01cc0fb6, 0x0fe80096, 0x01ba0fb4, 0x0fe400ae,
+        0x01a70fb4, 0x0fdd00c8, 0x018f0fb5, 0x0fda00e2,
+        0x017a0fb5, 0x0fd400fd, 0x01630fb8, 0x0fce0117,
+        0x014c0fba, 0x0fca0130, 0x01320fbf, 0x0fc70148,
+        0x011b0fc1, 0x0fc10163, 0x01010fc8, 0x0fc00177,
+        0x00e90fcd, 0x0fbd018d, 0x00d10fd1, 0x0fbc01a2,
+        0x00ba0fd7, 0x0fbb01b4, 0x00a30fdd, 0x0fbc01c4,
+        0x008e0fe1, 0x0fbd01d4, 0x00790fe7, 0x0fbe01e2,
+        0x00670feb, 0x0fc001ee, 0x00540ff1, 0x0fc501f6,
+        0x00430ff4, 0x0fcb01fe, 0x00340ff8, 0x0fd10203,
+        0x00260ffb, 0x0fd80207, 0x001a0ffd, 0x0fe10208,
+        0x000f0000, 0x0fea0207, 0x00060000, 0x0ff50205,
+        0x02000000, 0x00000000, 0x02020ff2, 0x0000000c,
+        0x02000fe4, 0x0000001c, 0x01fa0fda, 0x0ffc0030,
+        0x01f10fd0, 0x0ffa0045, 0x01e50fc8, 0x0ff6005d,
+        0x01d60fc3, 0x0ff30074, 0x01c60fbd, 0x0fef008e,
+        0x01b30fba, 0x0fe900aa, 0x019e0fb9, 0x0fe500c4,
+        0x01870fba, 0x0fe000df, 0x016f0fbb, 0x0fdd00f9,
+        0x01580fbc, 0x0fd80114, 0x01400fbf, 0x0fd3012e,
+        0x01280fc2, 0x0fd00146, 0x010f0fc6, 0x0fce015d,
+        0x00f90fc9, 0x0fc90175, 0x00e00fcf, 0x0fc90188,
+        0x00ca0fd4, 0x0fc6019c, 0x00b40fd8, 0x0fc601ae,
+        0x009f0fdd, 0x0fc501bf, 0x008b0fe3, 0x0fc601cc,
+        0x00780fe6, 0x0fc701db, 0x00660feb, 0x0fc801e7,
+        0x00560fef, 0x0fcb01f0, 0x00460ff3, 0x0fcf01f8,
+        0x00380ff6, 0x0fd401fe, 0x002c0ff9, 0x0fd90202,
+        0x00200ffc, 0x0fdf0205, 0x00160ffe, 0x0fe60206,
+        0x000c0000, 0x0fed0207, 0x00050000, 0x0ff70204,
+        0x02000000, 0x00000000, 0x01fe0ff3, 0x0000000f,
+        0x01f60fe5, 0x00000025, 0x01ea0fdb, 0x0ffd003e,
+        0x01db0fd2, 0x0ffb0058, 0x01c80fcc, 0x0ff70075,
+        0x01b50fc7, 0x0ff40090, 0x01a00fc3, 0x0ff000ad,
+        0x01880fc1, 0x0feb00cc, 0x01700fc1, 0x0fe800e7,
+        0x01550fc3, 0x0fe40104, 0x013b0fc5, 0x0fe2011e,
+        0x01240fc6, 0x0fde0138, 0x010c0fca, 0x0fda0150,
+        0x00f40fcd, 0x0fd90166, 0x00dd0fd1, 0x0fd7017b,
+        0x00c80fd4, 0x0fd40190, 0x00b20fd9, 0x0fd401a1,
+        0x009f0fdd, 0x0fd301b1, 0x008c0fe1, 0x0fd301c0,
+        0x007b0fe5, 0x0fd301cd, 0x006a0fea, 0x0fd401d8,
+        0x005c0fec, 0x0fd501e3, 0x004d0ff0, 0x0fd601ed,
+        0x00410ff3, 0x0fd801f4, 0x00340ff7, 0x0fdb01fa,
+        0x002a0ff9, 0x0fdf01fe, 0x00200ffb, 0x0fe30202,
+        0x00180ffd, 0x0fe70204, 0x00100ffe, 0x0fed0205,
+        0x00090000, 0x0ff20205, 0x00040000, 0x0ff90203,
+        0x02000000, 0x00000000, 0x02050ff5, 0x00000006,
+        0x02070fea, 0x0000000f, 0x02080fe1, 0x0ffd001a,
+        0x02070fd8, 0x0ffb0026, 0x02030fd1, 0x0ff80034,
+        0x01fe0fcb, 0x0ff40043, 0x01f60fc5, 0x0ff10054,
+        0x01ee0fc0, 0x0feb0067, 0x01e20fbe, 0x0fe70079,
+        0x01d40fbd, 0x0fe1008e, 0x01c40fbc, 0x0fdd00a3,
+        0x01b40fbb, 0x0fd700ba, 0x01a20fbc, 0x0fd100d1,
+        0x018d0fbd, 0x0fcd00e9, 0x01770fc0, 0x0fc80101,
+        0x01630fc1, 0x0fc1011b, 0x01480fc7, 0x0fbf0132,
+        0x01300fca, 0x0fba014c, 0x01170fce, 0x0fb80163,
+        0x00fd0fd4, 0x0fb5017a, 0x00e20fda, 0x0fb5018f,
+        0x00c80fdd, 0x0fb401a7, 0x00ae0fe4, 0x0fb401ba,
+        0x00960fe8, 0x0fb601cc, 0x007c0fee, 0x0fba01dc,
+        0x00650ff2, 0x0fc001e9, 0x00500ff6, 0x0fc701f3,
+        0x003b0ffa, 0x0fcf01fc, 0x00290ffc, 0x0fda0201,
+        0x00180000, 0x0fe40204, 0x000a0000, 0x0ff20204,
+        0x02000000, 0x00000000, 0x02030ff5, 0x00000008,
+        0x02030fea, 0x00000013, 0x02020fe1, 0x0ffd0020,
+        0x01fc0fd9, 0x0ffc002f, 0x01f60fd2, 0x0ff80040,
+        0x01ed0fcd, 0x0ff50051, 0x01e30fc7, 0x0ff10065,
+        0x01d70fc3, 0x0fec007a, 0x01c60fc2, 0x0fe9008f,
+        0x01b60fc1, 0x0fe300a6, 0x01a20fc1, 0x0fe000bd,
+        0x018f0fc1, 0x0fdb00d5, 0x017b0fc2, 0x0fd500ee,
+        0x01640fc4, 0x0fd20106, 0x014d0fc8, 0x0fce011d,
+        0x01370fc9, 0x0fc90137, 0x011d0fce, 0x0fc8014d,
+        0x01060fd2, 0x0fc40164, 0x00ee0fd5, 0x0fc2017b,
+        0x00d50fdb, 0x0fc1018f, 0x00bd0fe0, 0x0fc101a2,
+        0x00a60fe3, 0x0fc101b6, 0x008f0fe9, 0x0fc201c6,
+        0x007a0fec, 0x0fc301d7, 0x00650ff1, 0x0fc701e3,
+        0x00510ff5, 0x0fcd01ed, 0x00400ff8, 0x0fd201f6,
+        0x002f0ffc, 0x0fd901fc, 0x00200ffd, 0x0fe10202,
+        0x00130000, 0x0fea0203, 0x00080000, 0x0ff50203,
+        0x02000000, 0x00000000, 0x02020ff5, 0x00000009,
+        0x01ff0fea, 0x00000017, 0x01fb0fe2, 0x0ffd0026,
+        0x01f30fda, 0x0ffc0037, 0x01ea0fd3, 0x0ff8004b,
+        0x01df0fce, 0x0ff5005e, 0x01d10fc9, 0x0ff20074,
+        0x01c10fc6, 0x0fed008c, 0x01ae0fc5, 0x0fea00a3,
+        0x019b0fc5, 0x0fe500bb, 0x01850fc6, 0x0fe200d3,
+        0x01700fc6, 0x0fde00ec, 0x015a0fc8, 0x0fd90105,
+        0x01430fca, 0x0fd6011d, 0x012b0fcd, 0x0fd30135,
+        0x01150fcf, 0x0fcf014d, 0x00fc0fd4, 0x0fce0162,
+        0x00e50fd8, 0x0fcc0177, 0x00cf0fdb, 0x0fca018c,
+        0x00b80fe0, 0x0fc9019f, 0x00a20fe5, 0x0fca01af,
+        0x008e0fe8, 0x0fcb01bf, 0x00790fec, 0x0fcb01d0,
+        0x00670fef, 0x0fcd01dd, 0x00550ff4, 0x0fd001e7,
+        0x00440ff7, 0x0fd501f0, 0x00350ffa, 0x0fda01f7,
+        0x00270ffc, 0x0fdf01fe, 0x001b0ffe, 0x0fe70200,
+        0x00100000, 0x0fee0202, 0x00060000, 0x0ff70203,
+        0x02000000, 0x00000000, 0x01ff0ff5, 0x0000000c,
+        0x01f80fea, 0x0000001e, 0x01ef0fe2, 0x0ffd0032,
+        0x01e20fdb, 0x0ffc0047, 0x01d30fd5, 0x0ff9005f,
+        0x01c20fd1, 0x0ff60077, 0x01b00fcd, 0x0ff30090,
+        0x019b0fcb, 0x0fef00ab, 0x01850fcb, 0x0fec00c4,
+        0x016e0fcc, 0x0fe800de, 0x01550fcd, 0x0fe600f8,
+        0x013f0fce, 0x0fe20111, 0x01280fd0, 0x0fdf0129,
+        0x01110fd2, 0x0fdd0140, 0x00f90fd6, 0x0fdb0156,
+        0x00e40fd8, 0x0fd8016c, 0x00cd0fdd, 0x0fd8017e,
+        0x00b80fe0, 0x0fd60192, 0x00a40fe3, 0x0fd601a3,
+        0x00910fe7, 0x0fd501b3, 0x007f0feb, 0x0fd601c0,
+        0x006e0fed, 0x0fd701ce, 0x005d0ff1, 0x0fd701db,
+        0x004f0ff3, 0x0fd901e5, 0x00400ff7, 0x0fdc01ed,
+        0x00330ff9, 0x0fe001f4, 0x00280ffb, 0x0fe301fa,
+        0x001d0ffd, 0x0fe801fe, 0x00140ffe, 0x0fed0201,
+        0x000c0000, 0x0ff20202, 0x00050000, 0x0ff90202,
+        0x02000000, 0x00000000, 0x02040ff7, 0x00000005,
+        0x02070fed, 0x0000000c, 0x02060fe6, 0x0ffe0016,
+        0x02050fdf, 0x0ffc0020, 0x02020fd9, 0x0ff9002c,
+        0x01fe0fd4, 0x0ff60038, 0x01f80fcf, 0x0ff30046,
+        0x01f00fcb, 0x0fef0056, 0x01e70fc8, 0x0feb0066,
+        0x01db0fc7, 0x0fe60078, 0x01cc0fc6, 0x0fe3008b,
+        0x01bf0fc5, 0x0fdd009f, 0x01ae0fc6, 0x0fd800b4,
+        0x019c0fc6, 0x0fd400ca, 0x01880fc9, 0x0fcf00e0,
+        0x01750fc9, 0x0fc900f9, 0x015d0fce, 0x0fc6010f,
+        0x01460fd0, 0x0fc20128, 0x012e0fd3, 0x0fbf0140,
+        0x01140fd8, 0x0fbc0158, 0x00f90fdd, 0x0fbb016f,
+        0x00df0fe0, 0x0fba0187, 0x00c40fe5, 0x0fb9019e,
+        0x00aa0fe9, 0x0fba01b3, 0x008e0fef, 0x0fbd01c6,
+        0x00740ff3, 0x0fc301d6, 0x005d0ff6, 0x0fc801e5,
+        0x00450ffa, 0x0fd001f1, 0x00300ffc, 0x0fda01fa,
+        0x001c0000, 0x0fe40200, 0x000c0000, 0x0ff20202,
+        0x02000000, 0x00000000, 0x02030ff7, 0x00000006,
+        0x02020fee, 0x00000010, 0x02000fe7, 0x0ffe001b,
+        0x01fe0fdf, 0x0ffc0027, 0x01f70fda, 0x0ffa0035,
+        0x01f00fd5, 0x0ff70044, 0x01e70fd0, 0x0ff40055,
+        0x01dd0fcd, 0x0fef0067, 0x01d00fcb, 0x0fec0079,
+        0x01bf0fcb, 0x0fe8008e, 0x01af0fca, 0x0fe500a2,
+        0x019f0fc9, 0x0fe000b8, 0x018c0fca, 0x0fdb00cf,
+        0x01770fcc, 0x0fd800e5, 0x01620fce, 0x0fd400fc,
+        0x014d0fcf, 0x0fcf0115, 0x01350fd3, 0x0fcd012b,
+        0x011d0fd6, 0x0fca0143, 0x01050fd9, 0x0fc8015a,
+        0x00ec0fde, 0x0fc60170, 0x00d30fe2, 0x0fc60185,
+        0x00bb0fe5, 0x0fc5019b, 0x00a30fea, 0x0fc501ae,
+        0x008c0fed, 0x0fc601c1, 0x00740ff2, 0x0fc901d1,
+        0x005e0ff5, 0x0fce01df, 0x004b0ff8, 0x0fd301ea,
+        0x00370ffc, 0x0fda01f3, 0x00260ffd, 0x0fe201fb,
+        0x00170000, 0x0fea01ff, 0x00090000, 0x0ff50202,
+        0x02000000, 0x00000000, 0x02010ff7, 0x00000008,
+        0x01ff0fee, 0x00000013, 0x01fb0fe7, 0x0ffe0020,
+        0x01f60fe0, 0x0ffc002e, 0x01ed0fda, 0x0ffa003f,
+        0x01e40fd6, 0x0ff7004f, 0x01d80fd2, 0x0ff40062,
+        0x01ca0fcf, 0x0ff00077, 0x01bb0fcd, 0x0fed008b,
+        0x01a90fcd, 0x0fe900a1, 0x01960fcd, 0x0fe600b7,
+        0x01830fcd, 0x0fe200ce, 0x016d0fcf, 0x0fde00e6,
+        0x01580fd0, 0x0fdb00fd, 0x01410fd3, 0x0fd80114,
+        0x012c0fd4, 0x0fd4012c, 0x01140fd8, 0x0fd30141,
+        0x00fd0fdb, 0x0fd00158, 0x00e60fde, 0x0fcf016d,
+        0x00ce0fe2, 0x0fcd0183, 0x00b70fe6, 0x0fcd0196,
+        0x00a10fe9, 0x0fcd01a9, 0x008b0fed, 0x0fcd01bb,
+        0x00770ff0, 0x0fcf01ca, 0x00620ff4, 0x0fd201d8,
+        0x004f0ff7, 0x0fd601e4, 0x003f0ffa, 0x0fda01ed,
+        0x002e0ffc, 0x0fe001f6, 0x00200ffe, 0x0fe701fb,
+        0x00130000, 0x0fee01ff, 0x00080000, 0x0ff70201,
+        0x02000000, 0x00000000, 0x01ff0ff7, 0x0000000a,
+        0x01f90fee, 0x00000019, 0x01f10fe7, 0x0ffe002a,
+        0x01e60fe1, 0x0ffd003c, 0x01d90fdc, 0x0ffa0051,
+        0x01cc0fd8, 0x0ff70065, 0x01bb0fd5, 0x0ff5007b,
+        0x01a80fd3, 0x0ff10094, 0x01950fd2, 0x0fef00aa,
+        0x01800fd2, 0x0feb00c3, 0x016a0fd3, 0x0fe900da,
+        0x01540fd3, 0x0fe600f3, 0x013f0fd5, 0x0fe2010a,
+        0x01280fd7, 0x0fe00121, 0x01100fda, 0x0fde0138,
+        0x00fb0fdb, 0x0fdb014f, 0x00e40fdf, 0x0fdb0162,
+        0x00ce0fe2, 0x0fd90177, 0x00b90fe4, 0x0fd8018b,
+        0x00a50fe8, 0x0fd8019b, 0x00910fec, 0x0fd801ab,
+        0x007e0fee, 0x0fd801bc, 0x006c0ff2, 0x0fd901c9,
+        0x005c0ff4, 0x0fda01d6, 0x004b0ff7, 0x0fdd01e1,
+        0x003c0ff9, 0x0fe001eb, 0x002f0ffb, 0x0fe401f2,
+        0x00230ffd, 0x0fe801f8, 0x00180ffe, 0x0fed01fd,
+        0x000e0000, 0x0ff20200, 0x00060000, 0x0ff90201,
+        0x02000000, 0x00000000, 0x02030ff9, 0x00000004,
+        0x02050ff2, 0x00000009, 0x02050fed, 0x0ffe0010,
+        0x02040fe7, 0x0ffd0018, 0x02020fe3, 0x0ffb0020,
+        0x01fe0fdf, 0x0ff9002a, 0x01fa0fdb, 0x0ff70034,
+        0x01f40fd8, 0x0ff30041, 0x01ed0fd6, 0x0ff0004d,
+        0x01e30fd5, 0x0fec005c, 0x01d80fd4, 0x0fea006a,
+        0x01cd0fd3, 0x0fe5007b, 0x01c00fd3, 0x0fe1008c,
+        0x01b10fd3, 0x0fdd009f, 0x01a10fd4, 0x0fd900b2,
+        0x01900fd4, 0x0fd400c8, 0x017b0fd7, 0x0fd100dd,
+        0x01660fd9, 0x0fcd00f4, 0x01500fda, 0x0fca010c,
+        0x01380fde, 0x0fc60124, 0x011e0fe2, 0x0fc5013b,
+        0x01040fe4, 0x0fc30155, 0x00e70fe8, 0x0fc10170,
+        0x00cc0feb, 0x0fc10188, 0x00ad0ff0, 0x0fc301a0,
+        0x00900ff4, 0x0fc701b5, 0x00750ff7, 0x0fcc01c8,
+        0x00580ffb, 0x0fd201db, 0x003e0ffd, 0x0fdb01ea,
+        0x00250000, 0x0fe501f6, 0x000f0000, 0x0ff301fe,
+        0x02000000, 0x00000000, 0x02020ff9, 0x00000005,
+        0x02020ff2, 0x0000000c, 0x02010fed, 0x0ffe0014,
+        0x01fe0fe8, 0x0ffd001d, 0x01fa0fe3, 0x0ffb0028,
+        0x01f40fe0, 0x0ff90033, 0x01ed0fdc, 0x0ff70040,
+        0x01e50fd9, 0x0ff3004f, 0x01db0fd7, 0x0ff1005d,
+        0x01ce0fd7, 0x0fed006e, 0x01c00fd6, 0x0feb007f,
+        0x01b30fd5, 0x0fe70091, 0x01a30fd6, 0x0fe300a4,
+        0x01920fd6, 0x0fe000b8, 0x017e0fd8, 0x0fdd00cd,
+        0x016c0fd8, 0x0fd800e4, 0x01560fdb, 0x0fd600f9,
+        0x01400fdd, 0x0fd20111, 0x01290fdf, 0x0fd00128,
+        0x01110fe2, 0x0fce013f, 0x00f80fe6, 0x0fcd0155,
+        0x00de0fe8, 0x0fcc016e, 0x00c40fec, 0x0fcb0185,
+        0x00ab0fef, 0x0fcb019b, 0x00900ff3, 0x0fcd01b0,
+        0x00770ff6, 0x0fd101c2, 0x005f0ff9, 0x0fd501d3,
+        0x00470ffc, 0x0fdb01e2, 0x00320ffd, 0x0fe201ef,
+        0x001e0000, 0x0fea01f8, 0x000c0000, 0x0ff501ff,
+        0x02000000, 0x00000000, 0x02010ff9, 0x00000006,
+        0x02000ff2, 0x0000000e, 0x01fd0fed, 0x0ffe0018,
+        0x01f80fe8, 0x0ffd0023, 0x01f20fe4, 0x0ffb002f,
+        0x01eb0fe0, 0x0ff9003c, 0x01e10fdd, 0x0ff7004b,
+        0x01d60fda, 0x0ff4005c, 0x01c90fd9, 0x0ff2006c,
+        0x01bc0fd8, 0x0fee007e, 0x01ab0fd8, 0x0fec0091,
+        0x019b0fd8, 0x0fe800a5, 0x018b0fd8, 0x0fe400b9,
+        0x01770fd9, 0x0fe200ce, 0x01620fdb, 0x0fdf00e4,
+        0x014f0fdb, 0x0fdb00fb, 0x01380fde, 0x0fda0110,
+        0x01210fe0, 0x0fd70128, 0x010a0fe2, 0x0fd5013f,
+        0x00f30fe6, 0x0fd30154, 0x00da0fe9, 0x0fd3016a,
+        0x00c30feb, 0x0fd20180, 0x00aa0fef, 0x0fd20195,
+        0x00940ff1, 0x0fd301a8, 0x007b0ff5, 0x0fd501bb,
+        0x00650ff7, 0x0fd801cc, 0x00510ffa, 0x0fdc01d9,
+        0x003c0ffd, 0x0fe101e6, 0x002a0ffe, 0x0fe701f1,
+        0x00190000, 0x0fee01f9, 0x000a0000, 0x0ff701ff,
+        0x02000000, 0x00000000, 0x01ff0ff9, 0x00000008,
+        0x01fb0ff2, 0x00000013, 0x01f50fed, 0x0ffe0020,
+        0x01ed0fe8, 0x0ffd002e, 0x01e30fe4, 0x0ffb003e,
+        0x01d80fe1, 0x0ff9004e, 0x01cb0fde, 0x0ff70060,
+        0x01bc0fdc, 0x0ff40074, 0x01ac0fdb, 0x0ff20087,
+        0x019a0fdb, 0x0fef009c, 0x01870fdb, 0x0fed00b1,
+        0x01740fdb, 0x0fea00c7, 0x01600fdc, 0x0fe700dd,
+        0x014b0fdd, 0x0fe500f3, 0x01350fdf, 0x0fe30109,
+        0x01200fe0, 0x0fe00120, 0x01090fe3, 0x0fdf0135,
+        0x00f30fe5, 0x0fdd014b, 0x00dd0fe7, 0x0fdc0160,
+        0x00c70fea, 0x0fdb0174, 0x00b10fed, 0x0fdb0187,
+        0x009c0fef, 0x0fdb019a, 0x00870ff2, 0x0fdb01ac,
+        0x00740ff4, 0x0fdc01bc, 0x00600ff7, 0x0fde01cb,
+        0x004e0ff9, 0x0fe101d8, 0x003e0ffb, 0x0fe401e3,
+        0x002e0ffd, 0x0fe801ed, 0x00200ffe, 0x0fed01f5,
+        0x00130000, 0x0ff201fb, 0x00080000, 0x0ff901ff,
+        0x02000000, 0x00000000, 0x02060ff2, 0x00000008,
+        0x02090fe4, 0x00000013, 0x020a0fd9, 0x0ffc0021,
+        0x02080fce, 0x0ffa0030, 0x02030fc5, 0x0ff60042,
+        0x01fd0fbe, 0x0ff10054, 0x01f50fb6, 0x0fed0068,
+        0x01e90fb1, 0x0fe60080, 0x01dc0fae, 0x0fe10095,
+        0x01ca0fae, 0x0fda00ae, 0x01b70fad, 0x0fd600c6,
+        0x01a40fad, 0x0fcf00e0, 0x018f0faf, 0x0fc800fa,
+        0x01780fb1, 0x0fc30114, 0x015f0fb5, 0x0fbf012d,
+        0x01490fb7, 0x0fb70149, 0x012d0fbf, 0x0fb5015f,
+        0x01140fc3, 0x0fb10178, 0x00fa0fc8, 0x0faf018f,
+        0x00e00fcf, 0x0fad01a4, 0x00c60fd6, 0x0fad01b7,
+        0x00ae0fda, 0x0fae01ca, 0x00950fe1, 0x0fae01dc,
+        0x00800fe6, 0x0fb101e9, 0x00680fed, 0x0fb601f5,
+        0x00540ff1, 0x0fbe01fd, 0x00420ff6, 0x0fc50203,
+        0x00300ffa, 0x0fce0208, 0x00210ffc, 0x0fd9020a,
+        0x00130000, 0x0fe40209, 0x00080000, 0x0ff20206,
+        0x02000000, 0x00000000, 0x02040ff2, 0x0000000a,
+        0x02040fe4, 0x00000018, 0x02010fda, 0x0ffc0029,
+        0x01fc0fcf, 0x0ffa003b, 0x01f30fc7, 0x0ff60050,
+        0x01e90fc0, 0x0ff20065, 0x01dc0fba, 0x0fee007c,
+        0x01cc0fb6, 0x0fe80096, 0x01ba0fb4, 0x0fe400ae,
+        0x01a70fb4, 0x0fdd00c8, 0x018f0fb5, 0x0fda00e2,
+        0x017a0fb5, 0x0fd400fd, 0x01630fb8, 0x0fce0117,
+        0x014c0fba, 0x0fca0130, 0x01320fbf, 0x0fc70148,
+        0x011b0fc1, 0x0fc10163, 0x01010fc8, 0x0fc00177,
+        0x00e90fcd, 0x0fbd018d, 0x00d10fd1, 0x0fbc01a2,
+        0x00ba0fd7, 0x0fbb01b4, 0x00a30fdd, 0x0fbc01c4,
+        0x008e0fe1, 0x0fbd01d4, 0x00790fe7, 0x0fbe01e2,
+        0x00670feb, 0x0fc001ee, 0x00540ff1, 0x0fc501f6,
+        0x00430ff4, 0x0fcb01fe, 0x00340ff8, 0x0fd10203,
+        0x00260ffb, 0x0fd80207, 0x001a0ffd, 0x0fe10208,
+        0x000f0000, 0x0fea0207, 0x00060000, 0x0ff50205,
+        0x02000000, 0x00000000, 0x02020ff2, 0x0000000c,
+        0x02000fe4, 0x0000001c, 0x01fa0fda, 0x0ffc0030,
+        0x01f10fd0, 0x0ffa0045, 0x01e50fc8, 0x0ff6005d,
+        0x01d60fc3, 0x0ff30074, 0x01c60fbd, 0x0fef008e,
+        0x01b30fba, 0x0fe900aa, 0x019e0fb9, 0x0fe500c4,
+        0x01870fba, 0x0fe000df, 0x016f0fbb, 0x0fdd00f9,
+        0x01580fbc, 0x0fd80114, 0x01400fbf, 0x0fd3012e,
+        0x01280fc2, 0x0fd00146, 0x010f0fc6, 0x0fce015d,
+        0x00f90fc9, 0x0fc90175, 0x00e00fcf, 0x0fc90188,
+        0x00ca0fd4, 0x0fc6019c, 0x00b40fd8, 0x0fc601ae,
+        0x009f0fdd, 0x0fc501bf, 0x008b0fe3, 0x0fc601cc,
+        0x00780fe6, 0x0fc701db, 0x00660feb, 0x0fc801e7,
+        0x00560fef, 0x0fcb01f0, 0x00460ff3, 0x0fcf01f8,
+        0x00380ff6, 0x0fd401fe, 0x002c0ff9, 0x0fd90202,
+        0x00200ffc, 0x0fdf0205, 0x00160ffe, 0x0fe60206,
+        0x000c0000, 0x0fed0207, 0x00050000, 0x0ff70204,
+        0x02000000, 0x00000000, 0x01fe0ff3, 0x0000000f,
+        0x01f60fe5, 0x00000025, 0x01ea0fdb, 0x0ffd003e,
+        0x01db0fd2, 0x0ffb0058, 0x01c80fcc, 0x0ff70075,
+        0x01b50fc7, 0x0ff40090, 0x01a00fc3, 0x0ff000ad,
+        0x01880fc1, 0x0feb00cc, 0x01700fc1, 0x0fe800e7,
+        0x01550fc3, 0x0fe40104, 0x013b0fc5, 0x0fe2011e,
+        0x01240fc6, 0x0fde0138, 0x010c0fca, 0x0fda0150,
+        0x00f40fcd, 0x0fd90166, 0x00dd0fd1, 0x0fd7017b,
+        0x00c80fd4, 0x0fd40190, 0x00b20fd9, 0x0fd401a1,
+        0x009f0fdd, 0x0fd301b1, 0x008c0fe1, 0x0fd301c0,
+        0x007b0fe5, 0x0fd301cd, 0x006a0fea, 0x0fd401d8,
+        0x005c0fec, 0x0fd501e3, 0x004d0ff0, 0x0fd601ed,
+        0x00410ff3, 0x0fd801f4, 0x00340ff7, 0x0fdb01fa,
+        0x002a0ff9, 0x0fdf01fe, 0x00200ffb, 0x0fe30202,
+        0x00180ffd, 0x0fe70204, 0x00100ffe, 0x0fed0205,
+        0x00090000, 0x0ff20205, 0x00040000, 0x0ff90203,
+        0x02000000, 0x00000000, 0x02050ff5, 0x00000006,
+        0x02070fea, 0x0000000f, 0x02080fe1, 0x0ffd001a,
+        0x02070fd8, 0x0ffb0026, 0x02030fd1, 0x0ff80034,
+        0x01fe0fcb, 0x0ff40043, 0x01f60fc5, 0x0ff10054,
+        0x01ee0fc0, 0x0feb0067, 0x01e20fbe, 0x0fe70079,
+        0x01d40fbd, 0x0fe1008e, 0x01c40fbc, 0x0fdd00a3,
+        0x01b40fbb, 0x0fd700ba, 0x01a20fbc, 0x0fd100d1,
+        0x018d0fbd, 0x0fcd00e9, 0x01770fc0, 0x0fc80101,
+        0x01630fc1, 0x0fc1011b, 0x01480fc7, 0x0fbf0132,
+        0x01300fca, 0x0fba014c, 0x01170fce, 0x0fb80163,
+        0x00fd0fd4, 0x0fb5017a, 0x00e20fda, 0x0fb5018f,
+        0x00c80fdd, 0x0fb401a7, 0x00ae0fe4, 0x0fb401ba,
+        0x00960fe8, 0x0fb601cc, 0x007c0fee, 0x0fba01dc,
+        0x00650ff2, 0x0fc001e9, 0x00500ff6, 0x0fc701f3,
+        0x003b0ffa, 0x0fcf01fc, 0x00290ffc, 0x0fda0201,
+        0x00180000, 0x0fe40204, 0x000a0000, 0x0ff20204,
+        0x02000000, 0x00000000, 0x02030ff5, 0x00000008,
+        0x02030fea, 0x00000013, 0x02020fe1, 0x0ffd0020,
+        0x01fc0fd9, 0x0ffc002f, 0x01f60fd2, 0x0ff80040,
+        0x01ed0fcd, 0x0ff50051, 0x01e30fc7, 0x0ff10065,
+        0x01d70fc3, 0x0fec007a, 0x01c60fc2, 0x0fe9008f,
+        0x01b60fc1, 0x0fe300a6, 0x01a20fc1, 0x0fe000bd,
+        0x018f0fc1, 0x0fdb00d5, 0x017b0fc2, 0x0fd500ee,
+        0x01640fc4, 0x0fd20106, 0x014d0fc8, 0x0fce011d,
+        0x01370fc9, 0x0fc90137, 0x011d0fce, 0x0fc8014d,
+        0x01060fd2, 0x0fc40164, 0x00ee0fd5, 0x0fc2017b,
+        0x00d50fdb, 0x0fc1018f, 0x00bd0fe0, 0x0fc101a2,
+        0x00a60fe3, 0x0fc101b6, 0x008f0fe9, 0x0fc201c6,
+        0x007a0fec, 0x0fc301d7, 0x00650ff1, 0x0fc701e3,
+        0x00510ff5, 0x0fcd01ed, 0x00400ff8, 0x0fd201f6,
+        0x002f0ffc, 0x0fd901fc, 0x00200ffd, 0x0fe10202,
+        0x00130000, 0x0fea0203, 0x00080000, 0x0ff50203,
+        0x02000000, 0x00000000, 0x02020ff5, 0x00000009,
+        0x01ff0fea, 0x00000017, 0x01fb0fe2, 0x0ffd0026,
+        0x01f30fda, 0x0ffc0037, 0x01ea0fd3, 0x0ff8004b,
+        0x01df0fce, 0x0ff5005e, 0x01d10fc9, 0x0ff20074,
+        0x01c10fc6, 0x0fed008c, 0x01ae0fc5, 0x0fea00a3,
+        0x019b0fc5, 0x0fe500bb, 0x01850fc6, 0x0fe200d3,
+        0x01700fc6, 0x0fde00ec, 0x015a0fc8, 0x0fd90105,
+        0x01430fca, 0x0fd6011d, 0x012b0fcd, 0x0fd30135,
+        0x01150fcf, 0x0fcf014d, 0x00fc0fd4, 0x0fce0162,
+        0x00e50fd8, 0x0fcc0177, 0x00cf0fdb, 0x0fca018c,
+        0x00b80fe0, 0x0fc9019f, 0x00a20fe5, 0x0fca01af,
+        0x008e0fe8, 0x0fcb01bf, 0x00790fec, 0x0fcb01d0,
+        0x00670fef, 0x0fcd01dd, 0x00550ff4, 0x0fd001e7,
+        0x00440ff7, 0x0fd501f0, 0x00350ffa, 0x0fda01f7,
+        0x00270ffc, 0x0fdf01fe, 0x001b0ffe, 0x0fe70200,
+        0x00100000, 0x0fee0202, 0x00060000, 0x0ff70203,
+        0x02000000, 0x00000000, 0x01ff0ff5, 0x0000000c,
+        0x01f80fea, 0x0000001e, 0x01ef0fe2, 0x0ffd0032,
+        0x01e20fdb, 0x0ffc0047, 0x01d30fd5, 0x0ff9005f,
+        0x01c20fd1, 0x0ff60077, 0x01b00fcd, 0x0ff30090,
+        0x019b0fcb, 0x0fef00ab, 0x01850fcb, 0x0fec00c4,
+        0x016e0fcc, 0x0fe800de, 0x01550fcd, 0x0fe600f8,
+        0x013f0fce, 0x0fe20111, 0x01280fd0, 0x0fdf0129,
+        0x01110fd2, 0x0fdd0140, 0x00f90fd6, 0x0fdb0156,
+        0x00e40fd8, 0x0fd8016c, 0x00cd0fdd, 0x0fd8017e,
+        0x00b80fe0, 0x0fd60192, 0x00a40fe3, 0x0fd601a3,
+        0x00910fe7, 0x0fd501b3, 0x007f0feb, 0x0fd601c0,
+        0x006e0fed, 0x0fd701ce, 0x005d0ff1, 0x0fd701db,
+        0x004f0ff3, 0x0fd901e5, 0x00400ff7, 0x0fdc01ed,
+        0x00330ff9, 0x0fe001f4, 0x00280ffb, 0x0fe301fa,
+        0x001d0ffd, 0x0fe801fe, 0x00140ffe, 0x0fed0201,
+        0x000c0000, 0x0ff20202, 0x00050000, 0x0ff90202,
+        0x02000000, 0x00000000, 0x02040ff7, 0x00000005,
+        0x02070fed, 0x0000000c, 0x02060fe6, 0x0ffe0016,
+        0x02050fdf, 0x0ffc0020, 0x02020fd9, 0x0ff9002c,
+        0x01fe0fd4, 0x0ff60038, 0x01f80fcf, 0x0ff30046,
+        0x01f00fcb, 0x0fef0056, 0x01e70fc8, 0x0feb0066,
+        0x01db0fc7, 0x0fe60078, 0x01cc0fc6, 0x0fe3008b,
+        0x01bf0fc5, 0x0fdd009f, 0x01ae0fc6, 0x0fd800b4,
+        0x019c0fc6, 0x0fd400ca, 0x01880fc9, 0x0fcf00e0,
+        0x01750fc9, 0x0fc900f9, 0x015d0fce, 0x0fc6010f,
+        0x01460fd0, 0x0fc20128, 0x012e0fd3, 0x0fbf0140,
+        0x01140fd8, 0x0fbc0158, 0x00f90fdd, 0x0fbb016f,
+        0x00df0fe0, 0x0fba0187, 0x00c40fe5, 0x0fb9019e,
+        0x00aa0fe9, 0x0fba01b3, 0x008e0fef, 0x0fbd01c6,
+        0x00740ff3, 0x0fc301d6, 0x005d0ff6, 0x0fc801e5,
+        0x00450ffa, 0x0fd001f1, 0x00300ffc, 0x0fda01fa,
+        0x001c0000, 0x0fe40200, 0x000c0000, 0x0ff20202,
+        0x02000000, 0x00000000, 0x02030ff7, 0x00000006,
+        0x02020fee, 0x00000010, 0x02000fe7, 0x0ffe001b,
+        0x01fe0fdf, 0x0ffc0027, 0x01f70fda, 0x0ffa0035,
+        0x01f00fd5, 0x0ff70044, 0x01e70fd0, 0x0ff40055,
+        0x01dd0fcd, 0x0fef0067, 0x01d00fcb, 0x0fec0079,
+        0x01bf0fcb, 0x0fe8008e, 0x01af0fca, 0x0fe500a2,
+        0x019f0fc9, 0x0fe000b8, 0x018c0fca, 0x0fdb00cf,
+        0x01770fcc, 0x0fd800e5, 0x01620fce, 0x0fd400fc,
+        0x014d0fcf, 0x0fcf0115, 0x01350fd3, 0x0fcd012b,
+        0x011d0fd6, 0x0fca0143, 0x01050fd9, 0x0fc8015a,
+        0x00ec0fde, 0x0fc60170, 0x00d30fe2, 0x0fc60185,
+        0x00bb0fe5, 0x0fc5019b, 0x00a30fea, 0x0fc501ae,
+        0x008c0fed, 0x0fc601c1, 0x00740ff2, 0x0fc901d1,
+        0x005e0ff5, 0x0fce01df, 0x004b0ff8, 0x0fd301ea,
+        0x00370ffc, 0x0fda01f3, 0x00260ffd, 0x0fe201fb,
+        0x00170000, 0x0fea01ff, 0x00090000, 0x0ff50202,
+        0x02000000, 0x00000000, 0x02010ff7, 0x00000008,
+        0x01ff0fee, 0x00000013, 0x01fb0fe7, 0x0ffe0020,
+        0x01f60fe0, 0x0ffc002e, 0x01ed0fda, 0x0ffa003f,
+        0x01e40fd6, 0x0ff7004f, 0x01d80fd2, 0x0ff40062,
+        0x01ca0fcf, 0x0ff00077, 0x01bb0fcd, 0x0fed008b,
+        0x01a90fcd, 0x0fe900a1, 0x01960fcd, 0x0fe600b7,
+        0x01830fcd, 0x0fe200ce, 0x016d0fcf, 0x0fde00e6,
+        0x01580fd0, 0x0fdb00fd, 0x01410fd3, 0x0fd80114,
+        0x012c0fd4, 0x0fd4012c, 0x01140fd8, 0x0fd30141,
+        0x00fd0fdb, 0x0fd00158, 0x00e60fde, 0x0fcf016d,
+        0x00ce0fe2, 0x0fcd0183, 0x00b70fe6, 0x0fcd0196,
+        0x00a10fe9, 0x0fcd01a9, 0x008b0fed, 0x0fcd01bb,
+        0x00770ff0, 0x0fcf01ca, 0x00620ff4, 0x0fd201d8,
+        0x004f0ff7, 0x0fd601e4, 0x003f0ffa, 0x0fda01ed,
+        0x002e0ffc, 0x0fe001f6, 0x00200ffe, 0x0fe701fb,
+        0x00130000, 0x0fee01ff, 0x00080000, 0x0ff70201,
+        0x02000000, 0x00000000, 0x01ff0ff7, 0x0000000a,
+        0x01f90fee, 0x00000019, 0x01f10fe7, 0x0ffe002a,
+        0x01e60fe1, 0x0ffd003c, 0x01d90fdc, 0x0ffa0051,
+        0x01cc0fd8, 0x0ff70065, 0x01bb0fd5, 0x0ff5007b,
+        0x01a80fd3, 0x0ff10094, 0x01950fd2, 0x0fef00aa,
+        0x01800fd2, 0x0feb00c3, 0x016a0fd3, 0x0fe900da,
+        0x01540fd3, 0x0fe600f3, 0x013f0fd5, 0x0fe2010a,
+        0x01280fd7, 0x0fe00121, 0x01100fda, 0x0fde0138,
+        0x00fb0fdb, 0x0fdb014f, 0x00e40fdf, 0x0fdb0162,
+        0x00ce0fe2, 0x0fd90177, 0x00b90fe4, 0x0fd8018b,
+        0x00a50fe8, 0x0fd8019b, 0x00910fec, 0x0fd801ab,
+        0x007e0fee, 0x0fd801bc, 0x006c0ff2, 0x0fd901c9,
+        0x005c0ff4, 0x0fda01d6, 0x004b0ff7, 0x0fdd01e1,
+        0x003c0ff9, 0x0fe001eb, 0x002f0ffb, 0x0fe401f2,
+        0x00230ffd, 0x0fe801f8, 0x00180ffe, 0x0fed01fd,
+        0x000e0000, 0x0ff20200, 0x00060000, 0x0ff90201,
+        0x02000000, 0x00000000, 0x02030ff9, 0x00000004,
+        0x02050ff2, 0x00000009, 0x02050fed, 0x0ffe0010,
+        0x02040fe7, 0x0ffd0018, 0x02020fe3, 0x0ffb0020,
+        0x01fe0fdf, 0x0ff9002a, 0x01fa0fdb, 0x0ff70034,
+        0x01f40fd8, 0x0ff30041, 0x01ed0fd6, 0x0ff0004d,
+        0x01e30fd5, 0x0fec005c, 0x01d80fd4, 0x0fea006a,
+        0x01cd0fd3, 0x0fe5007b, 0x01c00fd3, 0x0fe1008c,
+        0x01b10fd3, 0x0fdd009f, 0x01a10fd4, 0x0fd900b2,
+        0x01900fd4, 0x0fd400c8, 0x017b0fd7, 0x0fd100dd,
+        0x01660fd9, 0x0fcd00f4, 0x01500fda, 0x0fca010c,
+        0x01380fde, 0x0fc60124, 0x011e0fe2, 0x0fc5013b,
+        0x01040fe4, 0x0fc30155, 0x00e70fe8, 0x0fc10170,
+        0x00cc0feb, 0x0fc10188, 0x00ad0ff0, 0x0fc301a0,
+        0x00900ff4, 0x0fc701b5, 0x00750ff7, 0x0fcc01c8,
+        0x00580ffb, 0x0fd201db, 0x003e0ffd, 0x0fdb01ea,
+        0x00250000, 0x0fe501f6, 0x000f0000, 0x0ff301fe,
+        0x02000000, 0x00000000, 0x02020ff9, 0x00000005,
+        0x02020ff2, 0x0000000c, 0x02010fed, 0x0ffe0014,
+        0x01fe0fe8, 0x0ffd001d, 0x01fa0fe3, 0x0ffb0028,
+        0x01f40fe0, 0x0ff90033, 0x01ed0fdc, 0x0ff70040,
+        0x01e50fd9, 0x0ff3004f, 0x01db0fd7, 0x0ff1005d,
+        0x01ce0fd7, 0x0fed006e, 0x01c00fd6, 0x0feb007f,
+        0x01b30fd5, 0x0fe70091, 0x01a30fd6, 0x0fe300a4,
+        0x01920fd6, 0x0fe000b8, 0x017e0fd8, 0x0fdd00cd,
+        0x016c0fd8, 0x0fd800e4, 0x01560fdb, 0x0fd600f9,
+        0x01400fdd, 0x0fd20111, 0x01290fdf, 0x0fd00128,
+        0x01110fe2, 0x0fce013f, 0x00f80fe6, 0x0fcd0155,
+        0x00de0fe8, 0x0fcc016e, 0x00c40fec, 0x0fcb0185,
+        0x00ab0fef, 0x0fcb019b, 0x00900ff3, 0x0fcd01b0,
+        0x00770ff6, 0x0fd101c2, 0x005f0ff9, 0x0fd501d3,
+        0x00470ffc, 0x0fdb01e2, 0x00320ffd, 0x0fe201ef,
+        0x001e0000, 0x0fea01f8, 0x000c0000, 0x0ff501ff,
+        0x02000000, 0x00000000, 0x02010ff9, 0x00000006,
+        0x02000ff2, 0x0000000e, 0x01fd0fed, 0x0ffe0018,
+        0x01f80fe8, 0x0ffd0023, 0x01f20fe4, 0x0ffb002f,
+        0x01eb0fe0, 0x0ff9003c, 0x01e10fdd, 0x0ff7004b,
+        0x01d60fda, 0x0ff4005c, 0x01c90fd9, 0x0ff2006c,
+        0x01bc0fd8, 0x0fee007e, 0x01ab0fd8, 0x0fec0091,
+        0x019b0fd8, 0x0fe800a5, 0x018b0fd8, 0x0fe400b9,
+        0x01770fd9, 0x0fe200ce, 0x01620fdb, 0x0fdf00e4,
+        0x014f0fdb, 0x0fdb00fb, 0x01380fde, 0x0fda0110,
+        0x01210fe0, 0x0fd70128, 0x010a0fe2, 0x0fd5013f,
+        0x00f30fe6, 0x0fd30154, 0x00da0fe9, 0x0fd3016a,
+        0x00c30feb, 0x0fd20180, 0x00aa0fef, 0x0fd20195,
+        0x00940ff1, 0x0fd301a8, 0x007b0ff5, 0x0fd501bb,
+        0x00650ff7, 0x0fd801cc, 0x00510ffa, 0x0fdc01d9,
+        0x003c0ffd, 0x0fe101e6, 0x002a0ffe, 0x0fe701f1,
+        0x00190000, 0x0fee01f9, 0x000a0000, 0x0ff701ff,
+        0x02000000, 0x00000000, 0x01ff0ff9, 0x00000008,
+        0x01fb0ff2, 0x00000013, 0x01f50fed, 0x0ffe0020,
+        0x01ed0fe8, 0x0ffd002e, 0x01e30fe4, 0x0ffb003e,
+        0x01d80fe1, 0x0ff9004e, 0x01cb0fde, 0x0ff70060,
+        0x01bc0fdc, 0x0ff40074, 0x01ac0fdb, 0x0ff20087,
+        0x019a0fdb, 0x0fef009c, 0x01870fdb, 0x0fed00b1,
+        0x01740fdb, 0x0fea00c7, 0x01600fdc, 0x0fe700dd,
+        0x014b0fdd, 0x0fe500f3, 0x01350fdf, 0x0fe30109,
+        0x01200fe0, 0x0fe00120, 0x01090fe3, 0x0fdf0135,
+        0x00f30fe5, 0x0fdd014b, 0x00dd0fe7, 0x0fdc0160,
+        0x00c70fea, 0x0fdb0174, 0x00b10fed, 0x0fdb0187,
+        0x009c0fef, 0x0fdb019a, 0x00870ff2, 0x0fdb01ac,
+        0x00740ff4, 0x0fdc01bc, 0x00600ff7, 0x0fde01cb,
+        0x004e0ff9, 0x0fe101d8, 0x003e0ffb, 0x0fe401e3,
+        0x002e0ffd, 0x0fe801ed, 0x00200ffe, 0x0fed01f5,
+        0x00130000, 0x0ff201fb, 0x00080000, 0x0ff901ff
+};
+
+
+#define MDP4_QSEED_TABLE0_OFF 0x8100
+#define MDP4_QSEED_TABLE1_OFF 0x8200
+#define MDP4_QSEED_TABLE2_OFF 0x9000
+
+void mdp4_vg_qseed_init(int vp_num)
+{
+        uint32 *off;
+        int i, voff;
+
+        voff = MDP4_VIDEO_OFF * vp_num;
+        off = (uint32 *)(MDP_BASE + MDP4_VIDEO_BASE + voff +
+                                                MDP4_QSEED_TABLE0_OFF);
+        for (i = 0; i < (sizeof(vg_qseed_table0) / sizeof(uint32)); i++) {
+                outpdw(off, vg_qseed_table0[i]);
+                off++;
+        }
+
+        off = (uint32 *)(MDP_BASE + MDP4_VIDEO_BASE + voff +
+                                                MDP4_QSEED_TABLE1_OFF);
+        for (i = 0; i < (sizeof(vg_qseed_table1) / sizeof(uint32)); i++) {
+                outpdw(off, vg_qseed_table1[i]);
+                off++;
+        }
+
+        off = (uint32 *)(MDP_BASE + MDP4_VIDEO_BASE + voff +
+                                                MDP4_QSEED_TABLE2_OFF);
+        for (i = 0; i < (sizeof(vg_qseed_table2) / sizeof(uint32)); i++) {
+                outpdw(off, vg_qseed_table2[i]);
+                off++;
+        }
+
+}
+
+void mdp4_mixer_blend_init(mixer_num)
+{
+        unsigned char *overlay_base;
+        int off;
+
+        if (mixer_num)  /* mixer number, /dev/fb0, /dev/fb1 */
+                overlay_base = MDP_BASE + MDP4_OVERLAYPROC1_BASE;/* 0x18000 */
+        else
+                overlay_base = MDP_BASE + MDP4_OVERLAYPROC0_BASE;/* 0x10000 */
+
+        /* stage 0 to stage 2 */
+        off = 0;
+        outpdw(overlay_base + off + 0x104, 0x010);
+        outpdw(overlay_base + off + 0x108, 0xff);/* FG */
+        outpdw(overlay_base + off + 0x10c, 0x00);/* BG */
+
+        off += 0x20;
+        outpdw(overlay_base + off + 0x104, 0x010);
+        outpdw(overlay_base + off + 0x108, 0xff);/* FG */
+        outpdw(overlay_base + off + 0x10c, 0x00);/* BG */
+
+        off += 0x20;
+        outpdw(overlay_base + off + 0x104, 0x010);
+        outpdw(overlay_base + off + 0x108, 0xff);/* FG */
+        outpdw(overlay_base + off + 0x10c, 0x00);/* BG */
+}
+
+
+static uint32 csc_matrix_tab[9] = {
+        0x0254, 0x0000, 0x0331,
+        0x0254, 0xff37, 0xfe60,
+        0x0254, 0x0409, 0x0000
+};
+
+static uint32 csc_pre_bv_tab[3] = {0xfff0, 0xff80, 0xff80 };
+static uint32 csc_post_bv_tab[3] = {0, 0, 0 };
+
+static  uint32 csc_pre_lv_tab[6] =  {0, 0xff, 0, 0xff, 0, 0xff };
+static  uint32 csc_post_lv_tab[6] = {0, 0xff, 0, 0xff, 0, 0xff };
+
+#define MDP4_CSC_MV_OFF         0x4400
+#define MDP4_CSC_PRE_BV_OFF     0x4500
+#define MDP4_CSC_POST_BV_OFF    0x4580
+#define MDP4_CSC_PRE_LV_OFF     0x4600
+#define MDP4_CSC_POST_LV_OFF    0x4680
+
+void mdp4_vg_csc_mv_setup(int vp_num)
+{
+        uint32 *off;
+        int i, voff;
+
+        voff = MDP4_VIDEO_OFF * vp_num;
+        off = (uint32 *)(MDP_BASE + MDP4_VIDEO_BASE + voff +
+                                        MDP4_CSC_MV_OFF);
+        for (i = 0; i < 9; i++) {
+                outpdw(off, csc_matrix_tab[i]);
+                off++;
+        }
+}
+
+void mdp4_vg_csc_pre_bv_setup(int vp_num)
+{
+        uint32 *off;
+        int i, voff;
+
+        voff = MDP4_VIDEO_OFF * vp_num;
+        off = (uint32 *)(MDP_BASE + MDP4_VIDEO_BASE + voff +
+                                        MDP4_CSC_PRE_BV_OFF);
+        for (i = 0; i < 3; i++) {
+                outpdw(off, csc_pre_bv_tab[i]);
+                off++;
+        }
+}
+
+void mdp4_vg_csc_post_bv_setup(int vp_num)
+{
+        uint32 *off;
+        int i, voff;
+
+        voff = MDP4_VIDEO_OFF * vp_num;
+        off = (uint32 *)(MDP_BASE + MDP4_VIDEO_BASE + voff +
+                                        MDP4_CSC_POST_BV_OFF);
+        for (i = 0; i < 3; i++) {
+                outpdw(off, csc_post_bv_tab[i]);
+                off++;
+        }
+}
+
+void mdp4_vg_csc_pre_lv_setup(int vp_num)
+{
+        uint32 *off;
+        int i, voff;
+
+        voff = MDP4_VIDEO_OFF * vp_num;
+        off = (uint32 *)(MDP_BASE + MDP4_VIDEO_BASE + voff +
+                                        MDP4_CSC_PRE_LV_OFF);
+
+        for (i = 0; i < 6; i++) {
+                outpdw(off, csc_pre_lv_tab[i]);
+                off++;
+        }
+}
+
+void mdp4_vg_csc_post_lv_setup(int vp_num)
+{
+        uint32 *off;
+        int i, voff;
+
+        voff = MDP4_VIDEO_OFF * vp_num;
+        off = (uint32 *)(MDP_BASE + MDP4_VIDEO_BASE + voff +
+                                        MDP4_CSC_POST_LV_OFF);
+
+        for (i = 0; i < 6; i++) {
+                outpdw(off, csc_post_lv_tab[i]);
+                off++;
+        }
+}
+
+char gc_lut[] = {
+        0x0, 0x1, 0x2, 0x2, 0x3, 0x4, 0x5, 0x6,
+        0x6, 0x7, 0x8, 0x9, 0xA, 0xA, 0xB, 0xC,
+        0xD, 0xD, 0xE, 0xF, 0xF, 0x10, 0x10, 0x11,
+        0x12, 0x12, 0x13, 0x13, 0x14, 0x14, 0x15, 0x15,
+        0x16, 0x16, 0x17, 0x17, 0x17, 0x18, 0x18, 0x19,
+        0x19, 0x19, 0x1A, 0x1A, 0x1B, 0x1B, 0x1B, 0x1C,
+        0x1C, 0x1D, 0x1D, 0x1D, 0x1E, 0x1E, 0x1E, 0x1F,
+        0x1F, 0x1F, 0x20, 0x20, 0x20, 0x21, 0x21, 0x21,
+        0x22, 0x22, 0x22, 0x22, 0x23, 0x23, 0x23, 0x24,
+        0x24, 0x24, 0x25, 0x25, 0x25, 0x25, 0x26, 0x26,
+        0x26, 0x26, 0x27, 0x27, 0x27, 0x28, 0x28, 0x28,
+        0x28, 0x29, 0x29, 0x29, 0x29, 0x2A, 0x2A, 0x2A,
+        0x2A, 0x2B, 0x2B, 0x2B, 0x2B, 0x2B, 0x2C, 0x2C,
+        0x2C, 0x2C, 0x2D, 0x2D, 0x2D, 0x2D, 0x2E, 0x2E,
+        0x2E, 0x2E, 0x2E, 0x2F, 0x2F, 0x2F, 0x2F, 0x30,
+        0x30, 0x30, 0x30, 0x30, 0x31, 0x31, 0x31, 0x31,
+        0x31, 0x32, 0x32, 0x32, 0x32, 0x32, 0x33, 0x33,
+        0x33, 0x33, 0x33, 0x34, 0x34, 0x34, 0x34, 0x34,
+        0x35, 0x35, 0x35, 0x35, 0x35, 0x36, 0x36, 0x36,
+        0x36, 0x36, 0x37, 0x37, 0x37, 0x37, 0x37, 0x37,
+        0x38, 0x38, 0x38, 0x38, 0x38, 0x39, 0x39, 0x39,
+        0x39, 0x39, 0x39, 0x3A, 0x3A, 0x3A, 0x3A, 0x3A,
+        0x3A, 0x3B, 0x3B, 0x3B, 0x3B, 0x3B, 0x3B, 0x3C,
+        0x3C, 0x3C, 0x3C, 0x3C, 0x3C, 0x3D, 0x3D, 0x3D,
+        0x3D, 0x3D, 0x3D, 0x3E, 0x3E, 0x3E, 0x3E, 0x3E,
+        0x3E, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x40,
+        0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x41, 0x41,
+        0x41, 0x41, 0x41, 0x41, 0x42, 0x42, 0x42, 0x42,
+        0x42, 0x42, 0x42, 0x43, 0x43, 0x43, 0x43, 0x43,
+        0x43, 0x43, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44,
+        0x44, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45, 0x45,
+        0x46, 0x46, 0x46, 0x46, 0x46, 0x46, 0x46, 0x47,
+        0x47, 0x47, 0x47, 0x47, 0x47, 0x47, 0x48, 0x48,
+        0x48, 0x48, 0x48, 0x48, 0x48, 0x48, 0x49, 0x49,
+        0x49, 0x49, 0x49, 0x49, 0x49, 0x4A, 0x4A, 0x4A,
+        0x4A, 0x4A, 0x4A, 0x4A, 0x4A, 0x4B, 0x4B, 0x4B,
+        0x4B, 0x4B, 0x4B, 0x4B, 0x4B, 0x4C, 0x4C, 0x4C,
+        0x4C, 0x4C, 0x4C, 0x4C, 0x4D, 0x4D, 0x4D, 0x4D,
+        0x4D, 0x4D, 0x4D, 0x4D, 0x4E, 0x4E, 0x4E, 0x4E,
+        0x4E, 0x4E, 0x4E, 0x4E, 0x4E, 0x4F, 0x4F, 0x4F,
+        0x4F, 0x4F, 0x4F, 0x4F, 0x4F, 0x50, 0x50, 0x50,
+        0x50, 0x50, 0x50, 0x50, 0x50, 0x51, 0x51, 0x51,
+        0x51, 0x51, 0x51, 0x51, 0x51, 0x51, 0x52, 0x52,
+        0x52, 0x52, 0x52, 0x52, 0x52, 0x52, 0x53, 0x53,
+        0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x53, 0x54,
+        0x54, 0x54, 0x54, 0x54, 0x54, 0x54, 0x54, 0x54,
+        0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55,
+        0x55, 0x56, 0x56, 0x56, 0x56, 0x56, 0x56, 0x56,
+        0x56, 0x56, 0x57, 0x57, 0x57, 0x57, 0x57, 0x57,
+        0x57, 0x57, 0x57, 0x58, 0x58, 0x58, 0x58, 0x58,
+        0x58, 0x58, 0x58, 0x58, 0x58, 0x59, 0x59, 0x59,
+        0x59, 0x59, 0x59, 0x59, 0x59, 0x59, 0x5A, 0x5A,
+        0x5A, 0x5A, 0x5A, 0x5A, 0x5A, 0x5A, 0x5A, 0x5A,
+        0x5B, 0x5B, 0x5B, 0x5B, 0x5B, 0x5B, 0x5B, 0x5B,
+        0x5B, 0x5B, 0x5C, 0x5C, 0x5C, 0x5C, 0x5C, 0x5C,
+        0x5C, 0x5C, 0x5C, 0x5C, 0x5D, 0x5D, 0x5D, 0x5D,
+        0x5D, 0x5D, 0x5D, 0x5D, 0x5D, 0x5D, 0x5E, 0x5E,
+        0x5E, 0x5E, 0x5E, 0x5E, 0x5E, 0x5E, 0x5E, 0x5E,
+        0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F,
+        0x5F, 0x5F, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60,
+        0x60, 0x60, 0x60, 0x60, 0x60, 0x61, 0x61, 0x61,
+        0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x62,
+        0x62, 0x62, 0x62, 0x62, 0x62, 0x62, 0x62, 0x62,
+        0x62, 0x62, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
+        0x63, 0x63, 0x63, 0x63, 0x63, 0x64, 0x64, 0x64,
+        0x64, 0x64, 0x64, 0x64, 0x64, 0x64, 0x64, 0x64,
+        0x65, 0x65, 0x65, 0x65, 0x65, 0x65, 0x65, 0x65,
+        0x65, 0x65, 0x65, 0x66, 0x66, 0x66, 0x66, 0x66,
+        0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x67, 0x67,
+        0x67, 0x67, 0x67, 0x67, 0x67, 0x67, 0x67, 0x67,
+        0x67, 0x67, 0x68, 0x68, 0x68, 0x68, 0x68, 0x68,
+        0x68, 0x68, 0x68, 0x68, 0x68, 0x69, 0x69, 0x69,
+        0x69, 0x69, 0x69, 0x69, 0x69, 0x69, 0x69, 0x69,
+        0x69, 0x6A, 0x6A, 0x6A, 0x6A, 0x6A, 0x6A, 0x6A,
+        0x6A, 0x6A, 0x6A, 0x6A, 0x6A, 0x6B, 0x6B, 0x6B,
+        0x6B, 0x6B, 0x6B, 0x6B, 0x6B, 0x6B, 0x6B, 0x6B,
+        0x6B, 0x6C, 0x6C, 0x6C, 0x6C, 0x6C, 0x6C, 0x6C,
+        0x6C, 0x6C, 0x6C, 0x6C, 0x6C, 0x6D, 0x6D, 0x6D,
+        0x6D, 0x6D, 0x6D, 0x6D, 0x6D, 0x6D, 0x6D, 0x6D,
+        0x6D, 0x6E, 0x6E, 0x6E, 0x6E, 0x6E, 0x6E, 0x6E,
+        0x6E, 0x6E, 0x6E, 0x6E, 0x6E, 0x6F, 0x6F, 0x6F,
+        0x6F, 0x6F, 0x6F, 0x6F, 0x6F, 0x6F, 0x6F, 0x6F,
+        0x6F, 0x6F, 0x70, 0x70, 0x70, 0x70, 0x70, 0x70,
+        0x70, 0x70, 0x70, 0x70, 0x70, 0x70, 0x71, 0x71,
+        0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71, 0x71,
+        0x71, 0x71, 0x71, 0x72, 0x72, 0x72, 0x72, 0x72,
+        0x72, 0x72, 0x72, 0x72, 0x72, 0x72, 0x72, 0x72,
+        0x73, 0x73, 0x73, 0x73, 0x73, 0x73, 0x73, 0x73,
+        0x73, 0x73, 0x73, 0x73, 0x73, 0x74, 0x74, 0x74,
+        0x74, 0x74, 0x74, 0x74, 0x74, 0x74, 0x74, 0x74,
+        0x74, 0x74, 0x75, 0x75, 0x75, 0x75, 0x75, 0x75,
+        0x75, 0x75, 0x75, 0x75, 0x75, 0x75, 0x75, 0x75,
+        0x76, 0x76, 0x76, 0x76, 0x76, 0x76, 0x76, 0x76,
+        0x76, 0x76, 0x76, 0x76, 0x76, 0x77, 0x77, 0x77,
+        0x77, 0x77, 0x77, 0x77, 0x77, 0x77, 0x77, 0x77,
+        0x77, 0x77, 0x77, 0x78, 0x78, 0x78, 0x78, 0x78,
+        0x78, 0x78, 0x78, 0x78, 0x78, 0x78, 0x78, 0x78,
+        0x78, 0x79, 0x79, 0x79, 0x79, 0x79, 0x79, 0x79,
+        0x79, 0x79, 0x79, 0x79, 0x79, 0x79, 0x7A, 0x7A,
+        0x7A, 0x7A, 0x7A, 0x7A, 0x7A, 0x7A, 0x7A, 0x7A,
+        0x7A, 0x7A, 0x7A, 0x7A, 0x7A, 0x7B, 0x7B, 0x7B,
+        0x7B, 0x7B, 0x7B, 0x7B, 0x7B, 0x7B, 0x7B, 0x7B,
+        0x7B, 0x7B, 0x7B, 0x7C, 0x7C, 0x7C, 0x7C, 0x7C,
+        0x7C, 0x7C, 0x7C, 0x7C, 0x7C, 0x7C, 0x7C, 0x7C,
+        0x7C, 0x7D, 0x7D, 0x7D, 0x7D, 0x7D, 0x7D, 0x7D,
+        0x7D, 0x7D, 0x7D, 0x7D, 0x7D, 0x7D, 0x7D, 0x7D,
+        0x7E, 0x7E, 0x7E, 0x7E, 0x7E, 0x7E, 0x7E, 0x7E,
+        0x7E, 0x7E, 0x7E, 0x7E, 0x7E, 0x7E, 0x7F, 0x7F,
+        0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0x7F,
+        0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0x80, 0x80, 0x80,
+        0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80,
+        0x80, 0x80, 0x80, 0x80, 0x81, 0x81, 0x81, 0x81,
+        0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81,
+        0x81, 0x81, 0x81, 0x82, 0x82, 0x82, 0x82, 0x82,
+        0x82, 0x82, 0x82, 0x82, 0x82, 0x82, 0x82, 0x82,
+        0x82, 0x82, 0x83, 0x83, 0x83, 0x83, 0x83, 0x83,
+        0x83, 0x83, 0x83, 0x83, 0x83, 0x83, 0x83, 0x83,
+        0x83, 0x83, 0x84, 0x84, 0x84, 0x84, 0x84, 0x84,
+        0x84, 0x84, 0x84, 0x84, 0x84, 0x84, 0x84, 0x84,
+        0x84, 0x85, 0x85, 0x85, 0x85, 0x85, 0x85, 0x85,
+        0x85, 0x85, 0x85, 0x85, 0x85, 0x85, 0x85, 0x85,
+        0x85, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86,
+        0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86,
+        0x86, 0x87, 0x87, 0x87, 0x87, 0x87, 0x87, 0x87,
+        0x87, 0x87, 0x87, 0x87, 0x87, 0x87, 0x87, 0x87,
+        0x87, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88,
+        0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88,
+        0x88, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89,
+        0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89, 0x89,
+        0x89, 0x8A, 0x8A, 0x8A, 0x8A, 0x8A, 0x8A, 0x8A,
+        0x8A, 0x8A, 0x8A, 0x8A, 0x8A, 0x8A, 0x8A, 0x8A,
+        0x8A, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B,
+        0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B,
+        0x8B, 0x8B, 0x8C, 0x8C, 0x8C, 0x8C, 0x8C, 0x8C,
+        0x8C, 0x8C, 0x8C, 0x8C, 0x8C, 0x8C, 0x8C, 0x8C,
+        0x8C, 0x8C, 0x8C, 0x8D, 0x8D, 0x8D, 0x8D, 0x8D,
+        0x8D, 0x8D, 0x8D, 0x8D, 0x8D, 0x8D, 0x8D, 0x8D,
+        0x8D, 0x8D, 0x8D, 0x8D, 0x8E, 0x8E, 0x8E, 0x8E,
+        0x8E, 0x8E, 0x8E, 0x8E, 0x8E, 0x8E, 0x8E, 0x8E,
+        0x8E, 0x8E, 0x8E, 0x8E, 0x8E, 0x8F, 0x8F, 0x8F,
+        0x8F, 0x8F, 0x8F, 0x8F, 0x8F, 0x8F, 0x8F, 0x8F,
+        0x8F, 0x8F, 0x8F, 0x8F, 0x8F, 0x8F, 0x90, 0x90,
+        0x90, 0x90, 0x90, 0x90, 0x90, 0x90, 0x90, 0x90,
+        0x90, 0x90, 0x90, 0x90, 0x90, 0x90, 0x90, 0x91,
+        0x91, 0x91, 0x91, 0x91, 0x91, 0x91, 0x91, 0x91,
+        0x91, 0x91, 0x91, 0x91, 0x91, 0x91, 0x91, 0x91,
+        0x91, 0x92, 0x92, 0x92, 0x92, 0x92, 0x92, 0x92,
+        0x92, 0x92, 0x92, 0x92, 0x92, 0x92, 0x92, 0x92,
+        0x92, 0x92, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93,
+        0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93, 0x93,
+        0x93, 0x93, 0x93, 0x93, 0x94, 0x94, 0x94, 0x94,
+        0x94, 0x94, 0x94, 0x94, 0x94, 0x94, 0x94, 0x94,
+        0x94, 0x94, 0x94, 0x94, 0x94, 0x94, 0x95, 0x95,
+        0x95, 0x95, 0x95, 0x95, 0x95, 0x95, 0x95, 0x95,
+        0x95, 0x95, 0x95, 0x95, 0x95, 0x95, 0x95, 0x95,
+        0x96, 0x96, 0x96, 0x96, 0x96, 0x96, 0x96, 0x96,
+        0x96, 0x96, 0x96, 0x96, 0x96, 0x96, 0x96, 0x96,
+        0x96, 0x96, 0x96, 0x97, 0x97, 0x97, 0x97, 0x97,
+        0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97,
+        0x97, 0x97, 0x97, 0x97, 0x97, 0x98, 0x98, 0x98,
+        0x98, 0x98, 0x98, 0x98, 0x98, 0x98, 0x98, 0x98,
+        0x98, 0x98, 0x98, 0x98, 0x98, 0x98, 0x98, 0x98,
+        0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99,
+        0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99,
+        0x99, 0x99, 0x9A, 0x9A, 0x9A, 0x9A, 0x9A, 0x9A,
+        0x9A, 0x9A, 0x9A, 0x9A, 0x9A, 0x9A, 0x9A, 0x9A,
+        0x9A, 0x9A, 0x9A, 0x9A, 0x9A, 0x9B, 0x9B, 0x9B,
+        0x9B, 0x9B, 0x9B, 0x9B, 0x9B, 0x9B, 0x9B, 0x9B,
+        0x9B, 0x9B, 0x9B, 0x9B, 0x9B, 0x9B, 0x9B, 0x9B,
+        0x9C, 0x9C, 0x9C, 0x9C, 0x9C, 0x9C, 0x9C, 0x9C,
+        0x9C, 0x9C, 0x9C, 0x9C, 0x9C, 0x9C, 0x9C, 0x9C,
+        0x9C, 0x9C, 0x9C, 0x9C, 0x9D, 0x9D, 0x9D, 0x9D,
+        0x9D, 0x9D, 0x9D, 0x9D, 0x9D, 0x9D, 0x9D, 0x9D,
+        0x9D, 0x9D, 0x9D, 0x9D, 0x9D, 0x9D, 0x9D, 0x9E,
+        0x9E, 0x9E, 0x9E, 0x9E, 0x9E, 0x9E, 0x9E, 0x9E,
+        0x9E, 0x9E, 0x9E, 0x9E, 0x9E, 0x9E, 0x9E, 0x9E,
+        0x9E, 0x9E, 0x9F, 0x9F, 0x9F, 0x9F, 0x9F, 0x9F,
+        0x9F, 0x9F, 0x9F, 0x9F, 0x9F, 0x9F, 0x9F, 0x9F,
+        0x9F, 0x9F, 0x9F, 0x9F, 0x9F, 0x9F, 0xA0, 0xA0,
+        0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0,
+        0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0,
+        0xA0, 0xA0, 0xA1, 0xA1, 0xA1, 0xA1, 0xA1, 0xA1,
+        0xA1, 0xA1, 0xA1, 0xA1, 0xA1, 0xA1, 0xA1, 0xA1,
+        0xA1, 0xA1, 0xA1, 0xA1, 0xA1, 0xA1, 0xA2, 0xA2,
+        0xA2, 0xA2, 0xA2, 0xA2, 0xA2, 0xA2, 0xA2, 0xA2,
+        0xA2, 0xA2, 0xA2, 0xA2, 0xA2, 0xA2, 0xA2, 0xA2,
+        0xA2, 0xA2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
+        0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
+        0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA4, 0xA4,
+        0xA4, 0xA4, 0xA4, 0xA4, 0xA4, 0xA4, 0xA4, 0xA4,
+        0xA4, 0xA4, 0xA4, 0xA4, 0xA4, 0xA4, 0xA4, 0xA4,
+        0xA4, 0xA4, 0xA4, 0xA5, 0xA5, 0xA5, 0xA5, 0xA5,
+        0xA5, 0xA5, 0xA5, 0xA5, 0xA5, 0xA5, 0xA5, 0xA5,
+        0xA5, 0xA5, 0xA5, 0xA5, 0xA5, 0xA5, 0xA5, 0xA5,
+        0xA6, 0xA6, 0xA6, 0xA6, 0xA6, 0xA6, 0xA6, 0xA6,
+        0xA6, 0xA6, 0xA6, 0xA6, 0xA6, 0xA6, 0xA6, 0xA6,
+        0xA6, 0xA6, 0xA6, 0xA6, 0xA7, 0xA7, 0xA7, 0xA7,
+        0xA7, 0xA7, 0xA7, 0xA7, 0xA7, 0xA7, 0xA7, 0xA7,
+        0xA7, 0xA7, 0xA7, 0xA7, 0xA7, 0xA7, 0xA7, 0xA7,
+        0xA7, 0xA8, 0xA8, 0xA8, 0xA8, 0xA8, 0xA8, 0xA8,
+        0xA8, 0xA8, 0xA8, 0xA8, 0xA8, 0xA8, 0xA8, 0xA8,
+        0xA8, 0xA8, 0xA8, 0xA8, 0xA8, 0xA8, 0xA8, 0xA9,
+        0xA9, 0xA9, 0xA9, 0xA9, 0xA9, 0xA9, 0xA9, 0xA9,
+        0xA9, 0xA9, 0xA9, 0xA9, 0xA9, 0xA9, 0xA9, 0xA9,
+        0xA9, 0xA9, 0xA9, 0xA9, 0xAA, 0xAA, 0xAA, 0xAA,
+        0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA,
+        0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA,
+        0xAA, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB,
+        0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB,
+        0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAB, 0xAC,
+        0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC,
+        0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAC,
+        0xAC, 0xAC, 0xAC, 0xAC, 0xAC, 0xAD, 0xAD, 0xAD,
+        0xAD, 0xAD, 0xAD, 0xAD, 0xAD, 0xAD, 0xAD, 0xAD,
+        0xAD, 0xAD, 0xAD, 0xAD, 0xAD, 0xAD, 0xAD, 0xAD,
+        0xAD, 0xAD, 0xAD, 0xAE, 0xAE, 0xAE, 0xAE, 0xAE,
+        0xAE, 0xAE, 0xAE, 0xAE, 0xAE, 0xAE, 0xAE, 0xAE,
+        0xAE, 0xAE, 0xAE, 0xAE, 0xAE, 0xAE, 0xAE, 0xAE,
+        0xAE, 0xAF, 0xAF, 0xAF, 0xAF, 0xAF, 0xAF, 0xAF,
+        0xAF, 0xAF, 0xAF, 0xAF, 0xAF, 0xAF, 0xAF, 0xAF,
+        0xAF, 0xAF, 0xAF, 0xAF, 0xAF, 0xAF, 0xAF, 0xB0,
+        0xB0, 0xB0, 0xB0, 0xB0, 0xB0, 0xB0, 0xB0, 0xB0,
+        0xB0, 0xB0, 0xB0, 0xB0, 0xB0, 0xB0, 0xB0, 0xB0,
+        0xB0, 0xB0, 0xB0, 0xB0, 0xB0, 0xB0, 0xB1, 0xB1,
+        0xB1, 0xB1, 0xB1, 0xB1, 0xB1, 0xB1, 0xB1, 0xB1,
+        0xB1, 0xB1, 0xB1, 0xB1, 0xB1, 0xB1, 0xB1, 0xB1,
+        0xB1, 0xB1, 0xB1, 0xB1, 0xB2, 0xB2, 0xB2, 0xB2,
+        0xB2, 0xB2, 0xB2, 0xB2, 0xB2, 0xB2, 0xB2, 0xB2,
+        0xB2, 0xB2, 0xB2, 0xB2, 0xB2, 0xB2, 0xB2, 0xB2,
+        0xB2, 0xB2, 0xB2, 0xB3, 0xB3, 0xB3, 0xB3, 0xB3,
+        0xB3, 0xB3, 0xB3, 0xB3, 0xB3, 0xB3, 0xB3, 0xB3,
+        0xB3, 0xB3, 0xB3, 0xB3, 0xB3, 0xB3, 0xB3, 0xB3,
+        0xB3, 0xB3, 0xB4, 0xB4, 0xB4, 0xB4, 0xB4, 0xB4,
+        0xB4, 0xB4, 0xB4, 0xB4, 0xB4, 0xB4, 0xB4, 0xB4,
+        0xB4, 0xB4, 0xB4, 0xB4, 0xB4, 0xB4, 0xB4, 0xB4,
+        0xB4, 0xB5, 0xB5, 0xB5, 0xB5, 0xB5, 0xB5, 0xB5,
+        0xB5, 0xB5, 0xB5, 0xB5, 0xB5, 0xB5, 0xB5, 0xB5,
+        0xB5, 0xB5, 0xB5, 0xB5, 0xB5, 0xB5, 0xB5, 0xB5,
+        0xB6, 0xB6, 0xB6, 0xB6, 0xB6, 0xB6, 0xB6, 0xB6,
+        0xB6, 0xB6, 0xB6, 0xB6, 0xB6, 0xB6, 0xB6, 0xB6,
+        0xB6, 0xB6, 0xB6, 0xB6, 0xB6, 0xB6, 0xB6, 0xB6,
+        0xB7, 0xB7, 0xB7, 0xB7, 0xB7, 0xB7, 0xB7, 0xB7,
+        0xB7, 0xB7, 0xB7, 0xB7, 0xB7, 0xB7, 0xB7, 0xB7,
+        0xB7, 0xB7, 0xB7, 0xB7, 0xB7, 0xB7, 0xB7, 0xB8,
+        0xB8, 0xB8, 0xB8, 0xB8, 0xB8, 0xB8, 0xB8, 0xB8,
+        0xB8, 0xB8, 0xB8, 0xB8, 0xB8, 0xB8, 0xB8, 0xB8,
+        0xB8, 0xB8, 0xB8, 0xB8, 0xB8, 0xB8, 0xB8, 0xB9,
+        0xB9, 0xB9, 0xB9, 0xB9, 0xB9, 0xB9, 0xB9, 0xB9,
+        0xB9, 0xB9, 0xB9, 0xB9, 0xB9, 0xB9, 0xB9, 0xB9,
+        0xB9, 0xB9, 0xB9, 0xB9, 0xB9, 0xB9, 0xB9, 0xBA,
+        0xBA, 0xBA, 0xBA, 0xBA, 0xBA, 0xBA, 0xBA, 0xBA,
+        0xBA, 0xBA, 0xBA, 0xBA, 0xBA, 0xBA, 0xBA, 0xBA,
+        0xBA, 0xBA, 0xBA, 0xBA, 0xBA, 0xBA, 0xBA, 0xBB,
+        0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB,
+        0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB,
+        0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB, 0xBB,
+        0xBC, 0xBC, 0xBC, 0xBC, 0xBC, 0xBC, 0xBC, 0xBC,
+        0xBC, 0xBC, 0xBC, 0xBC, 0xBC, 0xBC, 0xBC, 0xBC,
+        0xBC, 0xBC, 0xBC, 0xBC, 0xBC, 0xBC, 0xBC, 0xBC,
+        0xBD, 0xBD, 0xBD, 0xBD, 0xBD, 0xBD, 0xBD, 0xBD,
+        0xBD, 0xBD, 0xBD, 0xBD, 0xBD, 0xBD, 0xBD, 0xBD,
+        0xBD, 0xBD, 0xBD, 0xBD, 0xBD, 0xBD, 0xBD, 0xBD,
+        0xBD, 0xBE, 0xBE, 0xBE, 0xBE, 0xBE, 0xBE, 0xBE,
+        0xBE, 0xBE, 0xBE, 0xBE, 0xBE, 0xBE, 0xBE, 0xBE,
+        0xBE, 0xBE, 0xBE, 0xBE, 0xBE, 0xBE, 0xBE, 0xBE,
+        0xBE, 0xBE, 0xBF, 0xBF, 0xBF, 0xBF, 0xBF, 0xBF,
+        0xBF, 0xBF, 0xBF, 0xBF, 0xBF, 0xBF, 0xBF, 0xBF,
+        0xBF, 0xBF, 0xBF, 0xBF, 0xBF, 0xBF, 0xBF, 0xBF,
+        0xBF, 0xBF, 0xC0, 0xC0, 0xC0, 0xC0, 0xC0, 0xC0,
+        0xC0, 0xC0, 0xC0, 0xC0, 0xC0, 0xC0, 0xC0, 0xC0,
+        0xC0, 0xC0, 0xC0, 0xC0, 0xC0, 0xC0, 0xC0, 0xC0,
+        0xC0, 0xC0, 0xC0, 0xC0, 0xC1, 0xC1, 0xC1, 0xC1,
+        0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0xC1,
+        0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0xC1,
+        0xC1, 0xC1, 0xC1, 0xC1, 0xC1, 0xC2, 0xC2, 0xC2,
+        0xC2, 0xC2, 0xC2, 0xC2, 0xC2, 0xC2, 0xC2, 0xC2,
+        0xC2, 0xC2, 0xC2, 0xC2, 0xC2, 0xC2, 0xC2, 0xC2,
+        0xC2, 0xC2, 0xC2, 0xC2, 0xC2, 0xC2, 0xC3, 0xC3,
+        0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3,
+        0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3,
+        0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3, 0xC3,
+        0xC4, 0xC4, 0xC4, 0xC4, 0xC4, 0xC4, 0xC4, 0xC4,
+        0xC4, 0xC4, 0xC4, 0xC4, 0xC4, 0xC4, 0xC4, 0xC4,
+        0xC4, 0xC4, 0xC4, 0xC4, 0xC4, 0xC4, 0xC4, 0xC4,
+        0xC4, 0xC4, 0xC5, 0xC5, 0xC5, 0xC5, 0xC5, 0xC5,
+        0xC5, 0xC5, 0xC5, 0xC5, 0xC5, 0xC5, 0xC5, 0xC5,
+        0xC5, 0xC5, 0xC5, 0xC5, 0xC5, 0xC5, 0xC5, 0xC5,
+        0xC5, 0xC5, 0xC5, 0xC5, 0xC6, 0xC6, 0xC6, 0xC6,
+        0xC6, 0xC6, 0xC6, 0xC6, 0xC6, 0xC6, 0xC6, 0xC6,
+        0xC6, 0xC6, 0xC6, 0xC6, 0xC6, 0xC6, 0xC6, 0xC6,
+        0xC6, 0xC6, 0xC6, 0xC6, 0xC6, 0xC6, 0xC7, 0xC7,
+        0xC7, 0xC7, 0xC7, 0xC7, 0xC7, 0xC7, 0xC7, 0xC7,
+        0xC7, 0xC7, 0xC7, 0xC7, 0xC7, 0xC7, 0xC7, 0xC7,
+        0xC7, 0xC7, 0xC7, 0xC7, 0xC7, 0xC7, 0xC7, 0xC7,
+        0xC8, 0xC8, 0xC8, 0xC8, 0xC8, 0xC8, 0xC8, 0xC8,
+        0xC8, 0xC8, 0xC8, 0xC8, 0xC8, 0xC8, 0xC8, 0xC8,
+        0xC8, 0xC8, 0xC8, 0xC8, 0xC8, 0xC8, 0xC8, 0xC8,
+        0xC8, 0xC8, 0xC8, 0xC9, 0xC9, 0xC9, 0xC9, 0xC9,
+        0xC9, 0xC9, 0xC9, 0xC9, 0xC9, 0xC9, 0xC9, 0xC9,
+        0xC9, 0xC9, 0xC9, 0xC9, 0xC9, 0xC9, 0xC9, 0xC9,
+        0xC9, 0xC9, 0xC9, 0xC9, 0xC9, 0xC9, 0xCA, 0xCA,
+        0xCA, 0xCA, 0xCA, 0xCA, 0xCA, 0xCA, 0xCA, 0xCA,
+        0xCA, 0xCA, 0xCA, 0xCA, 0xCA, 0xCA, 0xCA, 0xCA,
+        0xCA, 0xCA, 0xCA, 0xCA, 0xCA, 0xCA, 0xCA, 0xCA,
+        0xCA, 0xCB, 0xCB, 0xCB, 0xCB, 0xCB, 0xCB, 0xCB,
+        0xCB, 0xCB, 0xCB, 0xCB, 0xCB, 0xCB, 0xCB, 0xCB,
+        0xCB, 0xCB, 0xCB, 0xCB, 0xCB, 0xCB, 0xCB, 0xCB,
+        0xCB, 0xCB, 0xCB, 0xCB, 0xCC, 0xCC, 0xCC, 0xCC,
+        0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC,
+        0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC,
+        0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCD,
+        0xCD, 0xCD, 0xCD, 0xCD, 0xCD, 0xCD, 0xCD, 0xCD,
+        0xCD, 0xCD, 0xCD, 0xCD, 0xCD, 0xCD, 0xCD, 0xCD,
+        0xCD, 0xCD, 0xCD, 0xCD, 0xCD, 0xCD, 0xCD, 0xCD,
+        0xCD, 0xCD, 0xCE, 0xCE, 0xCE, 0xCE, 0xCE, 0xCE,
+        0xCE, 0xCE, 0xCE, 0xCE, 0xCE, 0xCE, 0xCE, 0xCE,
+        0xCE, 0xCE, 0xCE, 0xCE, 0xCE, 0xCE, 0xCE, 0xCE,
+        0xCE, 0xCE, 0xCE, 0xCE, 0xCE, 0xCE, 0xCF, 0xCF,
+        0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF,
+        0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF,
+        0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF,
+        0xCF, 0xCF, 0xD0, 0xD0, 0xD0, 0xD0, 0xD0, 0xD0,
+        0xD0, 0xD0, 0xD0, 0xD0, 0xD0, 0xD0, 0xD0, 0xD0,
+        0xD0, 0xD0, 0xD0, 0xD0, 0xD0, 0xD0, 0xD0, 0xD0,
+        0xD0, 0xD0, 0xD0, 0xD0, 0xD0, 0xD1, 0xD1, 0xD1,
+        0xD1, 0xD1, 0xD1, 0xD1, 0xD1, 0xD1, 0xD1, 0xD1,
+        0xD1, 0xD1, 0xD1, 0xD1, 0xD1, 0xD1, 0xD1, 0xD1,
+        0xD1, 0xD1, 0xD1, 0xD1, 0xD1, 0xD1, 0xD1, 0xD1,
+        0xD1, 0xD1, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2,
+        0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2,
+        0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2,
+        0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD2, 0xD3, 0xD3,
+        0xD3, 0xD3, 0xD3, 0xD3, 0xD3, 0xD3, 0xD3, 0xD3,
+        0xD3, 0xD3, 0xD3, 0xD3, 0xD3, 0xD3, 0xD3, 0xD3,
+        0xD3, 0xD3, 0xD3, 0xD3, 0xD3, 0xD3, 0xD3, 0xD3,
+        0xD3, 0xD3, 0xD4, 0xD4, 0xD4, 0xD4, 0xD4, 0xD4,
+        0xD4, 0xD4, 0xD4, 0xD4, 0xD4, 0xD4, 0xD4, 0xD4,
+        0xD4, 0xD4, 0xD4, 0xD4, 0xD4, 0xD4, 0xD4, 0xD4,
+        0xD4, 0xD4, 0xD4, 0xD4, 0xD4, 0xD4, 0xD4, 0xD5,
+        0xD5, 0xD5, 0xD5, 0xD5, 0xD5, 0xD5, 0xD5, 0xD5,
+        0xD5, 0xD5, 0xD5, 0xD5, 0xD5, 0xD5, 0xD5, 0xD5,
+        0xD5, 0xD5, 0xD5, 0xD5, 0xD5, 0xD5, 0xD5, 0xD5,
+        0xD5, 0xD5, 0xD5, 0xD5, 0xD6, 0xD6, 0xD6, 0xD6,
+        0xD6, 0xD6, 0xD6, 0xD6, 0xD6, 0xD6, 0xD6, 0xD6,
+        0xD6, 0xD6, 0xD6, 0xD6, 0xD6, 0xD6, 0xD6, 0xD6,
+        0xD6, 0xD6, 0xD6, 0xD6, 0xD6, 0xD6, 0xD6, 0xD6,
+        0xD6, 0xD7, 0xD7, 0xD7, 0xD7, 0xD7, 0xD7, 0xD7,
+        0xD7, 0xD7, 0xD7, 0xD7, 0xD7, 0xD7, 0xD7, 0xD7,
+        0xD7, 0xD7, 0xD7, 0xD7, 0xD7, 0xD7, 0xD7, 0xD7,
+        0xD7, 0xD7, 0xD7, 0xD7, 0xD7, 0xD7, 0xD8, 0xD8,
+        0xD8, 0xD8, 0xD8, 0xD8, 0xD8, 0xD8, 0xD8, 0xD8,
+        0xD8, 0xD8, 0xD8, 0xD8, 0xD8, 0xD8, 0xD8, 0xD8,
+        0xD8, 0xD8, 0xD8, 0xD8, 0xD8, 0xD8, 0xD8, 0xD8,
+        0xD8, 0xD8, 0xD8, 0xD9, 0xD9, 0xD9, 0xD9, 0xD9,
+        0xD9, 0xD9, 0xD9, 0xD9, 0xD9, 0xD9, 0xD9, 0xD9,
+        0xD9, 0xD9, 0xD9, 0xD9, 0xD9, 0xD9, 0xD9, 0xD9,
+        0xD9, 0xD9, 0xD9, 0xD9, 0xD9, 0xD9, 0xD9, 0xD9,
+        0xD9, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA,
+        0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA,
+        0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA,
+        0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDA, 0xDB, 0xDB,
+        0xDB, 0xDB, 0xDB, 0xDB, 0xDB, 0xDB, 0xDB, 0xDB,
+        0xDB, 0xDB, 0xDB, 0xDB, 0xDB, 0xDB, 0xDB, 0xDB,
+        0xDB, 0xDB, 0xDB, 0xDB, 0xDB, 0xDB, 0xDB, 0xDB,
+        0xDB, 0xDB, 0xDB, 0xDB, 0xDC, 0xDC, 0xDC, 0xDC,
+        0xDC, 0xDC, 0xDC, 0xDC, 0xDC, 0xDC, 0xDC, 0xDC,
+        0xDC, 0xDC, 0xDC, 0xDC, 0xDC, 0xDC, 0xDC, 0xDC,
+        0xDC, 0xDC, 0xDC, 0xDC, 0xDC, 0xDC, 0xDC, 0xDC,
+        0xDC, 0xDC, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD,
+        0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD,
+        0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD,
+        0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD, 0xDD,
+        0xDD, 0xDE, 0xDE, 0xDE, 0xDE, 0xDE, 0xDE, 0xDE,
+        0xDE, 0xDE, 0xDE, 0xDE, 0xDE, 0xDE, 0xDE, 0xDE,
+        0xDE, 0xDE, 0xDE, 0xDE, 0xDE, 0xDE, 0xDE, 0xDE,
+        0xDE, 0xDE, 0xDE, 0xDE, 0xDE, 0xDE, 0xDE, 0xDF,
+        0xDF, 0xDF, 0xDF, 0xDF, 0xDF, 0xDF, 0xDF, 0xDF,
+        0xDF, 0xDF, 0xDF, 0xDF, 0xDF, 0xDF, 0xDF, 0xDF,
+        0xDF, 0xDF, 0xDF, 0xDF, 0xDF, 0xDF, 0xDF, 0xDF,
+        0xDF, 0xDF, 0xDF, 0xDF, 0xDF, 0xDF, 0xE0, 0xE0,
+        0xE0, 0xE0, 0xE0, 0xE0, 0xE0, 0xE0, 0xE0, 0xE0,
+        0xE0, 0xE0, 0xE0, 0xE0, 0xE0, 0xE0, 0xE0, 0xE0,
+        0xE0, 0xE0, 0xE0, 0xE0, 0xE0, 0xE0, 0xE0, 0xE0,
+        0xE0, 0xE0, 0xE0, 0xE0, 0xE1, 0xE1, 0xE1, 0xE1,
+        0xE1, 0xE1, 0xE1, 0xE1, 0xE1, 0xE1, 0xE1, 0xE1,
+        0xE1, 0xE1, 0xE1, 0xE1, 0xE1, 0xE1, 0xE1, 0xE1,
+        0xE1, 0xE1, 0xE1, 0xE1, 0xE1, 0xE1, 0xE1, 0xE1,
+        0xE1, 0xE1, 0xE1, 0xE2, 0xE2, 0xE2, 0xE2, 0xE2,
+        0xE2, 0xE2, 0xE2, 0xE2, 0xE2, 0xE2, 0xE2, 0xE2,
+        0xE2, 0xE2, 0xE2, 0xE2, 0xE2, 0xE2, 0xE2, 0xE2,
+        0xE2, 0xE2, 0xE2, 0xE2, 0xE2, 0xE2, 0xE2, 0xE2,
+        0xE2, 0xE2, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3,
+        0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3,
+        0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3,
+        0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3, 0xE3,
+        0xE3, 0xE3, 0xE4, 0xE4, 0xE4, 0xE4, 0xE4, 0xE4,
+        0xE4, 0xE4, 0xE4, 0xE4, 0xE4, 0xE4, 0xE4, 0xE4,
+        0xE4, 0xE4, 0xE4, 0xE4, 0xE4, 0xE4, 0xE4, 0xE4,
+        0xE4, 0xE4, 0xE4, 0xE4, 0xE4, 0xE4, 0xE4, 0xE4,
+        0xE4, 0xE5, 0xE5, 0xE5, 0xE5, 0xE5, 0xE5, 0xE5,
+        0xE5, 0xE5, 0xE5, 0xE5, 0xE5, 0xE5, 0xE5, 0xE5,
+        0xE5, 0xE5, 0xE5, 0xE5, 0xE5, 0xE5, 0xE5, 0xE5,
+        0xE5, 0xE5, 0xE5, 0xE5, 0xE5, 0xE5, 0xE5, 0xE5,
+        0xE5, 0xE6, 0xE6, 0xE6, 0xE6, 0xE6, 0xE6, 0xE6,
+        0xE6, 0xE6, 0xE6, 0xE6, 0xE6, 0xE6, 0xE6, 0xE6,
+        0xE6, 0xE6, 0xE6, 0xE6, 0xE6, 0xE6, 0xE6, 0xE6,
+        0xE6, 0xE6, 0xE6, 0xE6, 0xE6, 0xE6, 0xE6, 0xE6,
+        0xE6, 0xE7, 0xE7, 0xE7, 0xE7, 0xE7, 0xE7, 0xE7,
+        0xE7, 0xE7, 0xE7, 0xE7, 0xE7, 0xE7, 0xE7, 0xE7,
+        0xE7, 0xE7, 0xE7, 0xE7, 0xE7, 0xE7, 0xE7, 0xE7,
+        0xE7, 0xE7, 0xE7, 0xE7, 0xE7, 0xE7, 0xE7, 0xE7,
+        0xE7, 0xE8, 0xE8, 0xE8, 0xE8, 0xE8, 0xE8, 0xE8,
+        0xE8, 0xE8, 0xE8, 0xE8, 0xE8, 0xE8, 0xE8, 0xE8,
+        0xE8, 0xE8, 0xE8, 0xE8, 0xE8, 0xE8, 0xE8, 0xE8,
+        0xE8, 0xE8, 0xE8, 0xE8, 0xE8, 0xE8, 0xE8, 0xE8,
+        0xE8, 0xE9, 0xE9, 0xE9, 0xE9, 0xE9, 0xE9, 0xE9,
+        0xE9, 0xE9, 0xE9, 0xE9, 0xE9, 0xE9, 0xE9, 0xE9,
+        0xE9, 0xE9, 0xE9, 0xE9, 0xE9, 0xE9, 0xE9, 0xE9,
+        0xE9, 0xE9, 0xE9, 0xE9, 0xE9, 0xE9, 0xE9, 0xE9,
+        0xE9, 0xE9, 0xEA, 0xEA, 0xEA, 0xEA, 0xEA, 0xEA,
+        0xEA, 0xEA, 0xEA, 0xEA, 0xEA, 0xEA, 0xEA, 0xEA,
+        0xEA, 0xEA, 0xEA, 0xEA, 0xEA, 0xEA, 0xEA, 0xEA,
+        0xEA, 0xEA, 0xEA, 0xEA, 0xEA, 0xEA, 0xEA, 0xEA,
+        0xEA, 0xEA, 0xEB, 0xEB, 0xEB, 0xEB, 0xEB, 0xEB,
+        0xEB, 0xEB, 0xEB, 0xEB, 0xEB, 0xEB, 0xEB, 0xEB,
+        0xEB, 0xEB, 0xEB, 0xEB, 0xEB, 0xEB, 0xEB, 0xEB,
+        0xEB, 0xEB, 0xEB, 0xEB, 0xEB, 0xEB, 0xEB, 0xEB,
+        0xEB, 0xEB, 0xEB, 0xEC, 0xEC, 0xEC, 0xEC, 0xEC,
+        0xEC, 0xEC, 0xEC, 0xEC, 0xEC, 0xEC, 0xEC, 0xEC,
+        0xEC, 0xEC, 0xEC, 0xEC, 0xEC, 0xEC, 0xEC, 0xEC,
+        0xEC, 0xEC, 0xEC, 0xEC, 0xEC, 0xEC, 0xEC, 0xEC,
+        0xEC, 0xEC, 0xEC, 0xEC, 0xED, 0xED, 0xED, 0xED,
+        0xED, 0xED, 0xED, 0xED, 0xED, 0xED, 0xED, 0xED,
+        0xED, 0xED, 0xED, 0xED, 0xED, 0xED, 0xED, 0xED,
+        0xED, 0xED, 0xED, 0xED, 0xED, 0xED, 0xED, 0xED,
+        0xED, 0xED, 0xED, 0xED, 0xED, 0xEE, 0xEE, 0xEE,
+        0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE,
+        0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE,
+        0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE,
+        0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEF, 0xEF,
+        0xEF, 0xEF, 0xEF, 0xEF, 0xEF, 0xEF, 0xEF, 0xEF,
+        0xEF, 0xEF, 0xEF, 0xEF, 0xEF, 0xEF, 0xEF, 0xEF,
+        0xEF, 0xEF, 0xEF, 0xEF, 0xEF, 0xEF, 0xEF, 0xEF,
+        0xEF, 0xEF, 0xEF, 0xEF, 0xEF, 0xEF, 0xEF, 0xEF,
+        0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
+        0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
+        0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
+        0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
+        0xF0, 0xF0, 0xF1, 0xF1, 0xF1, 0xF1, 0xF1, 0xF1,
+        0xF1, 0xF1, 0xF1, 0xF1, 0xF1, 0xF1, 0xF1, 0xF1,
+        0xF1, 0xF1, 0xF1, 0xF1, 0xF1, 0xF1, 0xF1, 0xF1,
+        0xF1, 0xF1, 0xF1, 0xF1, 0xF1, 0xF1, 0xF1, 0xF1,
+        0xF1, 0xF1, 0xF1, 0xF1, 0xF2, 0xF2, 0xF2, 0xF2,
+        0xF2, 0xF2, 0xF2, 0xF2, 0xF2, 0xF2, 0xF2, 0xF2,
+        0xF2, 0xF2, 0xF2, 0xF2, 0xF2, 0xF2, 0xF2, 0xF2,
+        0xF2, 0xF2, 0xF2, 0xF2, 0xF2, 0xF2, 0xF2, 0xF2,
+        0xF2, 0xF2, 0xF2, 0xF2, 0xF2, 0xF2, 0xF3, 0xF3,
+        0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3,
+        0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3,
+        0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3,
+        0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3,
+        0xF4, 0xF4, 0xF4, 0xF4, 0xF4, 0xF4, 0xF4, 0xF4,
+        0xF4, 0xF4, 0xF4, 0xF4, 0xF4, 0xF4, 0xF4, 0xF4,
+        0xF4, 0xF4, 0xF4, 0xF4, 0xF4, 0xF4, 0xF4, 0xF4,
+        0xF4, 0xF4, 0xF4, 0xF4, 0xF4, 0xF4, 0xF4, 0xF4,
+        0xF4, 0xF4, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5,
+        0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5,
+        0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5,
+        0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5,
+        0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF6, 0xF6, 0xF6,
+        0xF6, 0xF6, 0xF6, 0xF6, 0xF6, 0xF6, 0xF6, 0xF6,
+        0xF6, 0xF6, 0xF6, 0xF6, 0xF6, 0xF6, 0xF6, 0xF6,
+        0xF6, 0xF6, 0xF6, 0xF6, 0xF6, 0xF6, 0xF6, 0xF6,
+        0xF6, 0xF6, 0xF6, 0xF6, 0xF6, 0xF6, 0xF6, 0xF6,
+        0xF7, 0xF7, 0xF7, 0xF7, 0xF7, 0xF7, 0xF7, 0xF7,
+        0xF7, 0xF7, 0xF7, 0xF7, 0xF7, 0xF7, 0xF7, 0xF7,
+        0xF7, 0xF7, 0xF7, 0xF7, 0xF7, 0xF7, 0xF7, 0xF7,
+        0xF7, 0xF7, 0xF7, 0xF7, 0xF7, 0xF7, 0xF7, 0xF7,
+        0xF7, 0xF7, 0xF7, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8,
+        0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8,
+        0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8,
+        0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8,
+        0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF9, 0xF9,
+        0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9,
+        0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9,
+        0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9,
+        0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9,
+        0xF9, 0xFA, 0xFA, 0xFA, 0xFA, 0xFA, 0xFA, 0xFA,
+        0xFA, 0xFA, 0xFA, 0xFA, 0xFA, 0xFA, 0xFA, 0xFA,
+        0xFA, 0xFA, 0xFA, 0xFA, 0xFA, 0xFA, 0xFA, 0xFA,
+        0xFA, 0xFA, 0xFA, 0xFA, 0xFA, 0xFA, 0xFA, 0xFA,
+        0xFA, 0xFA, 0xFA, 0xFA, 0xFA, 0xFB, 0xFB, 0xFB,
+        0xFB, 0xFB, 0xFB, 0xFB, 0xFB, 0xFB, 0xFB, 0xFB,
+        0xFB, 0xFB, 0xFB, 0xFB, 0xFB, 0xFB, 0xFB, 0xFB,
+        0xFB, 0xFB, 0xFB, 0xFB, 0xFB, 0xFB, 0xFB, 0xFB,
+        0xFB, 0xFB, 0xFB, 0xFB, 0xFB, 0xFB, 0xFB, 0xFB,
+        0xFB, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC,
+        0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC,
+        0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC,
+        0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFC,
+        0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xFD, 0xFD, 0xFD,
+        0xFD, 0xFD, 0xFD, 0xFD, 0xFD, 0xFD, 0xFD, 0xFD,
+        0xFD, 0xFD, 0xFD, 0xFD, 0xFD, 0xFD, 0xFD, 0xFD,
+        0xFD, 0xFD, 0xFD, 0xFD, 0xFD, 0xFD, 0xFD, 0xFD,
+        0xFD, 0xFD, 0xFD, 0xFD, 0xFD, 0xFD, 0xFD, 0xFD,
+        0xFD, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE,
+        0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE,
+        0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE,
+        0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFE,
+        0xFE, 0xFE, 0xFE, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF,
+        0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+        0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+};
+
+void mdp4_mixer_gc_lut_setup(int mixer_num)
+{
+        unsigned char *base;
+        uint32 data;
+        char val;
+        int i, off;
+
+        if (mixer_num)  /* mixer number, /dev/fb0, /dev/fb1 */
+                base = MDP_BASE + MDP4_OVERLAYPROC1_BASE;/* 0x18000 */
+        else
+                base = MDP_BASE + MDP4_OVERLAYPROC0_BASE;/* 0x10000 */
+
+        base += 0x4000; /* GC_LUT offset */
+
+        off = 0;
+        for (i = 0; i < 4096; i++) {
+                val = gc_lut[i];
+                data = (val << 16 | val << 8 | val); /* R, B, and G are same */
+                outpdw(base + off, data);
+                off += 4;
+        }
+}
+
+uint32 igc_video_lut[] = {       /* non linear */
+        0x0, 0x1, 0x2, 0x4, 0x5, 0x6, 0x7, 0x9,
+        0xA, 0xB, 0xC, 0xE, 0xF, 0x10, 0x12, 0x14,
+        0x15, 0x17, 0x19, 0x1B, 0x1D, 0x1F, 0x21, 0x23,
+        0x25, 0x28, 0x2A, 0x2D, 0x30, 0x32, 0x35, 0x38,
+        0x3B, 0x3E, 0x42, 0x45, 0x48, 0x4C, 0x4F, 0x53,
+        0x57, 0x5B, 0x5F, 0x63, 0x67, 0x6B, 0x70, 0x74,
+        0x79, 0x7E, 0x83, 0x88, 0x8D, 0x92, 0x97, 0x9C,
+        0xA2, 0xA8, 0xAD, 0xB3, 0xB9, 0xBF, 0xC5, 0xCC,
+        0xD2, 0xD8, 0xDF, 0xE6, 0xED, 0xF4, 0xFB, 0x102,
+        0x109, 0x111, 0x118, 0x120, 0x128, 0x130, 0x138, 0x140,
+        0x149, 0x151, 0x15A, 0x162, 0x16B, 0x174, 0x17D, 0x186,
+        0x190, 0x199, 0x1A3, 0x1AC, 0x1B6, 0x1C0, 0x1CA, 0x1D5,
+        0x1DF, 0x1EA, 0x1F4, 0x1FF, 0x20A, 0x215, 0x220, 0x22B,
+        0x237, 0x242, 0x24E, 0x25A, 0x266, 0x272, 0x27F, 0x28B,
+        0x298, 0x2A4, 0x2B1, 0x2BE, 0x2CB, 0x2D8, 0x2E6, 0x2F3,
+        0x301, 0x30F, 0x31D, 0x32B, 0x339, 0x348, 0x356, 0x365,
+        0x374, 0x383, 0x392, 0x3A1, 0x3B1, 0x3C0, 0x3D0, 0x3E0,
+        0x3F0, 0x400, 0x411, 0x421, 0x432, 0x443, 0x454, 0x465,
+        0x476, 0x487, 0x499, 0x4AB, 0x4BD, 0x4CF, 0x4E1, 0x4F3,
+        0x506, 0x518, 0x52B, 0x53E, 0x551, 0x565, 0x578, 0x58C,
+        0x5A0, 0x5B3, 0x5C8, 0x5DC, 0x5F0, 0x605, 0x61A, 0x62E,
+        0x643, 0x659, 0x66E, 0x684, 0x699, 0x6AF, 0x6C5, 0x6DB,
+        0x6F2, 0x708, 0x71F, 0x736, 0x74D, 0x764, 0x77C, 0x793,
+        0x7AB, 0x7C3, 0x7DB, 0x7F3, 0x80B, 0x824, 0x83D, 0x855,
+        0x86F, 0x888, 0x8A1, 0x8BB, 0x8D4, 0x8EE, 0x908, 0x923,
+        0x93D, 0x958, 0x973, 0x98E, 0x9A9, 0x9C4, 0x9DF, 0x9FB,
+        0xA17, 0xA33, 0xA4F, 0xA6C, 0xA88, 0xAA5, 0xAC2, 0xADF,
+        0xAFC, 0xB19, 0xB37, 0xB55, 0xB73, 0xB91, 0xBAF, 0xBCE,
+        0xBEC, 0xC0B, 0xC2A, 0xC4A, 0xC69, 0xC89, 0xCA8, 0xCC8,
+        0xCE8, 0xD09, 0xD29, 0xD4A, 0xD6B, 0xD8C, 0xDAD, 0xDCF,
+        0xDF0, 0xE12, 0xE34, 0xE56, 0xE79, 0xE9B, 0xEBE, 0xEE1,
+        0xF04, 0xF27, 0xF4B, 0xF6E, 0xF92, 0xFB6, 0xFDB, 0xFFF,
+};
+
+void mdp4_vg_igc_lut_setup(int vp_num)
+{
+        unsigned char *base;
+        int i, voff, off;
+        uint32 data, val;
+
+        voff = MDP4_VIDEO_OFF * vp_num;
+        base = MDP_BASE + MDP4_VIDEO_BASE + voff + 0x5000;
+
+        off = 0;
+        for (i = 0; i < 256; i++) {
+                val = igc_video_lut[i];
+                data = (val << 16 | val);       /* color 0 and 1 */
+                outpdw(base + off, data);
+                outpdw(base + off + 0x800, val);        /* color 2 */
+                off += 4;
+        }
+}
+
+uint32 igc_rgb_lut[] = {   /* linear */
+        0x0, 0x10, 0x20, 0x30, 0x40, 0x50, 0x60, 0x70,
+        0x80, 0x91, 0xA1, 0xB1, 0xC1, 0xD1, 0xE1, 0xF1,
+        0x101, 0x111, 0x121, 0x131, 0x141, 0x151, 0x161, 0x171,
+        0x181, 0x191, 0x1A2, 0x1B2, 0x1C2, 0x1D2, 0x1E2, 0x1F2,
+        0x202, 0x212, 0x222, 0x232, 0x242, 0x252, 0x262, 0x272,
+        0x282, 0x292, 0x2A2, 0x2B3, 0x2C3, 0x2D3, 0x2E3, 0x2F3,
+        0x303, 0x313, 0x323, 0x333, 0x343, 0x353, 0x363, 0x373,
+        0x383, 0x393, 0x3A3, 0x3B3, 0x3C4, 0x3D4, 0x3E4, 0x3F4,
+        0x404, 0x414, 0x424, 0x434, 0x444, 0x454, 0x464, 0x474,
+        0x484, 0x494, 0x4A4, 0x4B4, 0x4C4, 0x4D5, 0x4E5, 0x4F5,
+        0x505, 0x515, 0x525, 0x535, 0x545, 0x555, 0x565, 0x575,
+        0x585, 0x595, 0x5A5, 0x5B5, 0x5C5, 0x5D5, 0x5E6, 0x5F6,
+        0x606, 0x616, 0x626, 0x636, 0x646, 0x656, 0x666, 0x676,
+        0x686, 0x696, 0x6A6, 0x6B6, 0x6C6, 0x6D6, 0x6E6, 0x6F7,
+        0x707, 0x717, 0x727, 0x737, 0x747, 0x757, 0x767, 0x777,
+        0x787, 0x797, 0x7A7, 0x7B7, 0x7C7, 0x7D7, 0x7E7, 0x7F7,
+        0x808, 0x818, 0x828, 0x838, 0x848, 0x858, 0x868, 0x878,
+        0x888, 0x898, 0x8A8, 0x8B8, 0x8C8, 0x8D8, 0x8E8, 0x8F8,
+        0x908, 0x919, 0x929, 0x939, 0x949, 0x959, 0x969, 0x979,
+        0x989, 0x999, 0x9A9, 0x9B9, 0x9C9, 0x9D9, 0x9E9, 0x9F9,
+        0xA09, 0xA19, 0xA2A, 0xA3A, 0xA4A, 0xA5A, 0xA6A, 0xA7A,
+        0xA8A, 0xA9A, 0xAAA, 0xABA, 0xACA, 0xADA, 0xAEA, 0xAFA,
+        0xB0A, 0xB1A, 0xB2A, 0xB3B, 0xB4B, 0xB5B, 0xB6B, 0xB7B,
+        0xB8B, 0xB9B, 0xBAB, 0xBBB, 0xBCB, 0xBDB, 0xBEB, 0xBFB,
+        0xC0B, 0xC1B, 0xC2B, 0xC3B, 0xC4C, 0xC5C, 0xC6C, 0xC7C,
+        0xC8C, 0xC9C, 0xCAC, 0xCBC, 0xCCC, 0xCDC, 0xCEC, 0xCFC,
+        0xD0C, 0xD1C, 0xD2C, 0xD3C, 0xD4C, 0xD5D, 0xD6D, 0xD7D,
+        0xD8D, 0xD9D, 0xDAD, 0xDBD, 0xDCD, 0xDDD, 0xDED, 0xDFD,
+        0xE0D, 0xE1D, 0xE2D, 0xE3D, 0xE4D, 0xE5D, 0xE6E, 0xE7E,
+        0xE8E, 0xE9E, 0xEAE, 0xEBE, 0xECE, 0xEDE, 0xEEE, 0xEFE,
+        0xF0E, 0xF1E, 0xF2E, 0xF3E, 0xF4E, 0xF5E, 0xF6E, 0xF7F,
+        0xF8F, 0xF9F, 0xFAF, 0xFBF, 0xFCF, 0xFDF, 0xFEF, 0xFFF,
+};
+
+void mdp4_rgb_igc_lut_setup(int num)
+{
+        unsigned char *base;
+        int i, voff, off;
+        uint32 data, val;
+
+        voff = MDP4_RGB_OFF * num;
+        base = MDP_BASE + MDP4_RGB_BASE + voff + 0x5000;
+
+        off = 0;
+        for (i = 0; i < 256; i++) {
+                val = igc_rgb_lut[i];
+                data = (val << 16 | val);       /* color 0 and 1 */
+                outpdw(base + off, data);
+                outpdw(base + off + 0x800, val);        /* color 2 */
+                off += 4;
+        }
+}
diff --git a/drivers/staging/msm/mdp_cursor.c b/drivers/staging/msm/mdp_cursor.c
new file mode 100644
index 000000000000..7d28f30d9313
--- /dev/null
+++ b/drivers/staging/msm/mdp_cursor.c
@@ -0,0 +1,104 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+
+#include <mach/hardware.h>
+#include <asm/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/spinlock.h>
+
+#include <linux/fb.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+
+static int cursor_enabled;
+
+int mdp_hw_cursor_update(struct fb_info *info, struct fb_cursor *cursor)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        struct fb_image *img = &cursor->image;
+        int calpha_en, transp_en;
+        int alpha;
+        int ret = 0;
+
+        if ((img->width > MDP_CURSOR_WIDTH) ||
+            (img->height > MDP_CURSOR_HEIGHT) ||
+            (img->depth != 32))
+                return -EINVAL;
+
+        if (cursor->set & FB_CUR_SETPOS)
+                MDP_OUTP(MDP_BASE + 0x9004c, (img->dy << 16) | img->dx);
+
+        if (cursor->set & FB_CUR_SETIMAGE) {
+                ret = copy_from_user(mfd->cursor_buf, img->data,
+                                        img->width*img->height*4);
+                if (ret)
+                        return ret;
+
+                if (img->bg_color == 0xffffffff)
+                        transp_en = 0;
+                else
+                        transp_en = 1;
+
+                alpha = (img->fg_color & 0xff000000) >> 24;
+
+                if (alpha)
+                        calpha_en = 0x2; /* xrgb */
+                else
+                        calpha_en = 0x1; /* argb */
+
+                MDP_OUTP(MDP_BASE + 0x90044, (img->height << 16) | img->width);
+                MDP_OUTP(MDP_BASE + 0x90048, mfd->cursor_buf_phys);
+                /* order the writes the cursor_buf before updating the
+                 * hardware */
+//              dma_coherent_pre_ops();
+                MDP_OUTP(MDP_BASE + 0x90060,
+                         (transp_en << 3) | (calpha_en << 1) |
+                         (inp32(MDP_BASE + 0x90060) & 0x1));
+#ifdef CONFIG_FB_MSM_MDP40
+                MDP_OUTP(MDP_BASE + 0x90064, (alpha << 24));
+                MDP_OUTP(MDP_BASE + 0x90068, (0xffffff & img->bg_color));
+                MDP_OUTP(MDP_BASE + 0x9006C, (0xffffff & img->bg_color));
+#else
+                MDP_OUTP(MDP_BASE + 0x90064,
+                         (alpha << 24) | (0xffffff & img->bg_color));
+                MDP_OUTP(MDP_BASE + 0x90068, 0);
+#endif
+        }
+
+        if ((cursor->enable) && (!cursor_enabled)) {
+                cursor_enabled = 1;
+                MDP_OUTP(MDP_BASE + 0x90060, inp32(MDP_BASE + 0x90060) | 0x1);
+        } else if ((!cursor->enable) && (cursor_enabled)) {
+                cursor_enabled = 0;
+                MDP_OUTP(MDP_BASE + 0x90060,
+                         inp32(MDP_BASE + 0x90060) & (~0x1));
+        }
+
+        return 0;
+}
diff --git a/drivers/staging/msm/mdp_dma.c b/drivers/staging/msm/mdp_dma.c
new file mode 100644
index 000000000000..639918b143b4
--- /dev/null
+++ b/drivers/staging/msm/mdp_dma.c
@@ -0,0 +1,561 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/spinlock.h>
+
+#include <linux/fb.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+#include "mddihost.h"
+
+static uint32 mdp_last_dma2_update_width;
+static uint32 mdp_last_dma2_update_height;
+static uint32 mdp_curr_dma2_update_width;
+static uint32 mdp_curr_dma2_update_height;
+
+ktime_t mdp_dma2_last_update_time = { 0 };
+
+int mdp_lcd_rd_cnt_offset_slow = 20;
+int mdp_lcd_rd_cnt_offset_fast = 20;
+int mdp_vsync_usec_wait_line_too_short = 5;
+uint32 mdp_dma2_update_time_in_usec;
+uint32 mdp_total_vdopkts;
+
+extern u32 msm_fb_debug_enabled;
+extern struct workqueue_struct *mdp_dma_wq;
+
+int vsync_start_y_adjust = 4;
+
+static void mdp_dma2_update_lcd(struct msm_fb_data_type *mfd)
+{
+        MDPIBUF *iBuf = &mfd->ibuf;
+        int mddi_dest = FALSE;
+        uint32 outBpp = iBuf->bpp;
+        uint32 dma2_cfg_reg;
+        uint8 *src;
+        uint32 mddi_ld_param;
+        uint16 mddi_vdo_packet_reg;
+        struct msm_fb_panel_data *pdata =
+            (struct msm_fb_panel_data *)mfd->pdev->dev.platform_data;
+        uint32 ystride = mfd->fbi->fix.line_length;
+
+        dma2_cfg_reg = DMA_PACK_TIGHT | DMA_PACK_ALIGN_LSB |
+            DMA_OUT_SEL_AHB | DMA_IBUF_NONCONTIGUOUS;
+
+#ifdef CONFIG_FB_MSM_MDP30
+        /*
+         * Software workaround:  On 7x25/7x27, the MDP will not
+         * respond if dma_w is 1 pixel.  Set the update width to
+         * 2 pixels and adjust the x offset if needed.
+         */
+        if (iBuf->dma_w == 1) {
+                iBuf->dma_w = 2;
+                if (iBuf->dma_x == (iBuf->ibuf_width - 2))
+                        iBuf->dma_x--;
+        }
+#endif
+
+        if (mfd->fb_imgType == MDP_BGR_565)
+                dma2_cfg_reg |= DMA_PACK_PATTERN_BGR;
+        else
+                dma2_cfg_reg |= DMA_PACK_PATTERN_RGB;
+
+        if (outBpp == 4)
+                dma2_cfg_reg |= DMA_IBUF_C3ALPHA_EN;
+
+        if (outBpp == 2)
+                dma2_cfg_reg |= DMA_IBUF_FORMAT_RGB565;
+
+        mddi_ld_param = 0;
+        mddi_vdo_packet_reg = mfd->panel_info.mddi.vdopkt;
+
+        if ((mfd->panel_info.type == MDDI_PANEL) ||
+            (mfd->panel_info.type == EXT_MDDI_PANEL)) {
+                dma2_cfg_reg |= DMA_OUT_SEL_MDDI;
+                mddi_dest = TRUE;
+
+                if (mfd->panel_info.type == MDDI_PANEL) {
+                        mdp_total_vdopkts++;
+                        if (mfd->panel_info.pdest == DISPLAY_1) {
+                                dma2_cfg_reg |= DMA_MDDI_DMAOUT_LCD_SEL_PRIMARY;
+                                mddi_ld_param = 0;
+#ifdef MDDI_HOST_WINDOW_WORKAROUND
+                                mddi_window_adjust(mfd, iBuf->dma_x,
+                                                   iBuf->dma_w - 1, iBuf->dma_y,
+                                                   iBuf->dma_h - 1);
+#endif
+                        } else {
+                                dma2_cfg_reg |=
+                                    DMA_MDDI_DMAOUT_LCD_SEL_SECONDARY;
+                                mddi_ld_param = 1;
+#ifdef MDDI_HOST_WINDOW_WORKAROUND
+                                mddi_window_adjust(mfd, iBuf->dma_x,
+                                                   iBuf->dma_w - 1, iBuf->dma_y,
+                                                   iBuf->dma_h - 1);
+#endif
+                        }
+                } else {
+                        dma2_cfg_reg |= DMA_MDDI_DMAOUT_LCD_SEL_EXTERNAL;
+                        mddi_ld_param = 2;
+                }
+        } else {
+                if (mfd->panel_info.pdest == DISPLAY_1) {
+                        dma2_cfg_reg |= DMA_AHBM_LCD_SEL_PRIMARY;
+                        outp32(MDP_EBI2_LCD0, mfd->data_port_phys);
+                } else {
+                        dma2_cfg_reg |= DMA_AHBM_LCD_SEL_SECONDARY;
+                        outp32(MDP_EBI2_LCD1, mfd->data_port_phys);
+                }
+        }
+
+        dma2_cfg_reg |= DMA_DITHER_EN;
+
+        src = (uint8 *) iBuf->buf;
+        /* starting input address */
+        src += iBuf->dma_x * outBpp + iBuf->dma_y * ystride;
+
+        mdp_curr_dma2_update_width = iBuf->dma_w;
+        mdp_curr_dma2_update_height = iBuf->dma_h;
+
+        /* MDP cmd block enable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+#ifdef CONFIG_FB_MSM_MDP22
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0184,
+                        (iBuf->dma_h << 16 | iBuf->dma_w));
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0188, src);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x018C, ystride);
+#else
+        MDP_OUTP(MDP_BASE + 0x90004, (iBuf->dma_h << 16 | iBuf->dma_w));
+        MDP_OUTP(MDP_BASE + 0x90008, src);
+        MDP_OUTP(MDP_BASE + 0x9000c, ystride);
+#endif
+
+        if (mfd->panel_info.bpp == 18) {
+                dma2_cfg_reg |= DMA_DSTC0G_6BITS |      /* 666 18BPP */
+                    DMA_DSTC1B_6BITS | DMA_DSTC2R_6BITS;
+        } else {
+                dma2_cfg_reg |= DMA_DSTC0G_6BITS |      /* 565 16BPP */
+                    DMA_DSTC1B_5BITS | DMA_DSTC2R_5BITS;
+        }
+
+        if (mddi_dest) {
+#ifdef CONFIG_FB_MSM_MDP22
+                MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0194,
+                         (iBuf->dma_y << 16) | iBuf->dma_x);
+                MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01a0, mddi_ld_param);
+                MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01a4,
+                         (MDDI_VDO_PACKET_DESC << 16) | mddi_vdo_packet_reg);
+#else
+                MDP_OUTP(MDP_BASE + 0x90010, (iBuf->dma_y << 16) | iBuf->dma_x);
+                MDP_OUTP(MDP_BASE + 0x00090, mddi_ld_param);
+                MDP_OUTP(MDP_BASE + 0x00094,
+                         (MDDI_VDO_PACKET_DESC << 16) | mddi_vdo_packet_reg);
+#endif
+        } else {
+                /* setting EBI2 LCDC write window */
+                pdata->set_rect(iBuf->dma_x, iBuf->dma_y, iBuf->dma_w,
+                                iBuf->dma_h);
+        }
+
+        /* dma2 config register */
+#ifdef MDP_HW_VSYNC
+        MDP_OUTP(MDP_BASE + 0x90000, dma2_cfg_reg);
+
+        if ((mfd->use_mdp_vsync) &&
+            (mfd->ibuf.vsync_enable) && (mfd->panel_info.lcd.vsync_enable)) {
+                uint32 start_y;
+
+                if (vsync_start_y_adjust <= iBuf->dma_y)
+                        start_y = iBuf->dma_y - vsync_start_y_adjust;
+                else
+                        start_y =
+                            (mfd->total_lcd_lines - 1) - (vsync_start_y_adjust -
+                                                          iBuf->dma_y);
+
+                /*
+                 * MDP VSYNC clock must be On by now so, we don't have to
+                 * re-enable it
+                 */
+                MDP_OUTP(MDP_BASE + 0x210, start_y);
+                MDP_OUTP(MDP_BASE + 0x20c, 1);  /* enable prim vsync */
+        } else {
+                MDP_OUTP(MDP_BASE + 0x20c, 0);  /* disable prim vsync */
+        }
+#else
+#ifdef CONFIG_FB_MSM_MDP22
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0180, dma2_cfg_reg);
+#else
+        MDP_OUTP(MDP_BASE + 0x90000, dma2_cfg_reg);
+#endif
+#endif /* MDP_HW_VSYNC */
+
+        /* MDP cmd block disable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+}
+
+static ktime_t vt = { 0 };
+int mdp_usec_diff_threshold = 100;
+int mdp_expected_usec_wait;
+
+enum hrtimer_restart mdp_dma2_vsync_hrtimer_handler(struct hrtimer *ht)
+{
+        struct msm_fb_data_type *mfd = NULL;
+
+        mfd = container_of(ht, struct msm_fb_data_type, dma_hrtimer);
+
+        mdp_pipe_kickoff(MDP_DMA2_TERM, mfd);
+
+        if (msm_fb_debug_enabled) {
+                ktime_t t;
+                int usec_diff;
+                int actual_wait;
+
+                t = ktime_get_real();
+
+                actual_wait =
+                    (t.tv.sec - vt.tv.sec) * 1000000 + (t.tv.nsec -
+                                                        vt.tv.nsec) / 1000;
+                usec_diff = actual_wait - mdp_expected_usec_wait;
+
+                if ((mdp_usec_diff_threshold < usec_diff) || (usec_diff < 0))
+                        MSM_FB_DEBUG
+                            ("HRT Diff = %d usec Exp=%d usec  Act=%d usec\n",
+                             usec_diff, mdp_expected_usec_wait, actual_wait);
+        }
+
+        return HRTIMER_NORESTART;
+}
+
+static void mdp_dma_schedule(struct msm_fb_data_type *mfd, uint32 term)
+{
+        /*
+         * dma2 configure VSYNC block
+         * vsync supported on Primary LCD only for now
+         */
+        int32 mdp_lcd_rd_cnt;
+        uint32 usec_wait_time;
+        uint32 start_y;
+
+        /*
+         * ToDo: if we can move HRT timer callback to workqueue, we can
+         * move DMA2 power on under mdp_pipe_kickoff().
+         * This will save a power for hrt time wait.
+         * However if the latency for context switch (hrt irq -> workqueue)
+         * is too big, we will miss the vsync timing.
+         */
+        if (term == MDP_DMA2_TERM)
+                mdp_pipe_ctrl(MDP_DMA2_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+        mdp_dma2_update_time_in_usec =
+            MDP_KTIME2USEC(mdp_dma2_last_update_time);
+
+        if ((!mfd->ibuf.vsync_enable) || (!mfd->panel_info.lcd.vsync_enable)
+            || (mfd->use_mdp_vsync)) {
+                mdp_pipe_kickoff(term, mfd);
+                return;
+        }
+        /* SW vsync logic starts here */
+
+        /* get current rd counter */
+        mdp_lcd_rd_cnt = mdp_get_lcd_line_counter(mfd);
+        if (mdp_dma2_update_time_in_usec != 0) {
+                uint32 num, den;
+
+                /*
+                 * roi width boundary calculation to know the size of pixel
+                 * width that MDP can send faster or slower than LCD read
+                 * pointer
+                 */
+
+                num = mdp_last_dma2_update_width * mdp_last_dma2_update_height;
+                den =
+                    (((mfd->panel_info.lcd.refx100 * mfd->total_lcd_lines) /
+                      1000) * (mdp_dma2_update_time_in_usec / 100)) / 1000;
+
+                if (den == 0)
+                        mfd->vsync_width_boundary[mdp_last_dma2_update_width] =
+                            mfd->panel_info.xres + 1;
+                else
+                        mfd->vsync_width_boundary[mdp_last_dma2_update_width] =
+                            (int)(num / den);
+        }
+
+        if (mfd->vsync_width_boundary[mdp_last_dma2_update_width] >
+            mdp_curr_dma2_update_width) {
+                /* MDP wrp is faster than LCD rdp */
+                mdp_lcd_rd_cnt += mdp_lcd_rd_cnt_offset_fast;
+        } else {
+                /* MDP wrp is slower than LCD rdp */
+                mdp_lcd_rd_cnt -= mdp_lcd_rd_cnt_offset_slow;
+        }
+
+        if (mdp_lcd_rd_cnt < 0)
+                mdp_lcd_rd_cnt = mfd->total_lcd_lines + mdp_lcd_rd_cnt;
+        else if (mdp_lcd_rd_cnt > mfd->total_lcd_lines)
+                mdp_lcd_rd_cnt = mdp_lcd_rd_cnt - mfd->total_lcd_lines - 1;
+
+        /* get wrt pointer position */
+        start_y = mfd->ibuf.dma_y;
+
+        /* measure line difference between start_y and rd counter */
+        if (start_y > mdp_lcd_rd_cnt) {
+                /*
+                 * *100 for lcd_ref_hzx100 was already multiplied by 100
+                 * *1000000 is for usec conversion
+                 */
+
+                if ((start_y - mdp_lcd_rd_cnt) <=
+                    mdp_vsync_usec_wait_line_too_short)
+                        usec_wait_time = 0;
+                else
+                        usec_wait_time =
+                            ((start_y -
+                              mdp_lcd_rd_cnt) * 1000000) /
+                            ((mfd->total_lcd_lines *
+                              mfd->panel_info.lcd.refx100) / 100);
+        } else {
+                if ((start_y + (mfd->total_lcd_lines - mdp_lcd_rd_cnt)) <=
+                    mdp_vsync_usec_wait_line_too_short)
+                        usec_wait_time = 0;
+                else
+                        usec_wait_time =
+                            ((start_y +
+                              (mfd->total_lcd_lines -
+                               mdp_lcd_rd_cnt)) * 1000000) /
+                            ((mfd->total_lcd_lines *
+                              mfd->panel_info.lcd.refx100) / 100);
+        }
+
+        mdp_last_dma2_update_width = mdp_curr_dma2_update_width;
+        mdp_last_dma2_update_height = mdp_curr_dma2_update_height;
+
+        if (usec_wait_time == 0) {
+                mdp_pipe_kickoff(term, mfd);
+        } else {
+                ktime_t wait_time;
+
+                wait_time.tv.sec = 0;
+                wait_time.tv.nsec = usec_wait_time * 1000;
+
+                if (msm_fb_debug_enabled) {
+                        vt = ktime_get_real();
+                        mdp_expected_usec_wait = usec_wait_time;
+                }
+                hrtimer_start(&mfd->dma_hrtimer, wait_time, HRTIMER_MODE_REL);
+        }
+}
+
+#ifdef MDDI_HOST_WINDOW_WORKAROUND
+void mdp_dma2_update(struct msm_fb_data_type *mfd)
+{
+        MDPIBUF *iBuf;
+        uint32 upper_height;
+
+        if (mfd->panel.type == EXT_MDDI_PANEL) {
+                mdp_dma2_update_sub(mfd);
+                return;
+        }
+
+        iBuf = &mfd->ibuf;
+
+        upper_height =
+            (uint32) mddi_assign_pkt_height((uint16) iBuf->dma_w,
+                                            (uint16) iBuf->dma_h, 18);
+
+        if (upper_height >= iBuf->dma_h) {
+                mdp_dma2_update_sub(mfd);
+        } else {
+                MDPIBUF lower_height;
+
+                /* sending the upper region first */
+                lower_height = iBuf->dma_h - upper_height;
+                iBuf->dma_h = upper_height;
+                mdp_dma2_update_sub(mfd);
+
+                /* sending the lower region second */
+                iBuf->dma_h = lower_height;
+                iBuf->dma_y += lower_height;
+                iBuf->vsync_enable = FALSE;
+                mdp_dma2_update_sub(mfd);
+        }
+}
+
+void mdp_dma2_update_sub(struct msm_fb_data_type *mfd)
+#else
+void mdp_dma2_update(struct msm_fb_data_type *mfd)
+#endif
+{
+        down(&mfd->dma->mutex);
+        if ((mfd) && (!mfd->dma->busy) && (mfd->panel_power_on)) {
+                down(&mfd->sem);
+                mfd->ibuf_flushed = TRUE;
+                mdp_dma2_update_lcd(mfd);
+
+                mdp_enable_irq(MDP_DMA2_TERM);
+                mfd->dma->busy = TRUE;
+                INIT_COMPLETION(mfd->dma->comp);
+
+                /* schedule DMA to start */
+                mdp_dma_schedule(mfd, MDP_DMA2_TERM);
+                up(&mfd->sem);
+
+                /* wait until DMA finishes the current job */
+                wait_for_completion_killable(&mfd->dma->comp);
+                mdp_disable_irq(MDP_DMA2_TERM);
+
+        /* signal if pan function is waiting for the update completion */
+                if (mfd->pan_waiting) {
+                        mfd->pan_waiting = FALSE;
+                        complete(&mfd->pan_comp);
+                }
+        }
+        up(&mfd->dma->mutex);
+}
+
+void mdp_lcd_update_workqueue_handler(struct work_struct *work)
+{
+        struct msm_fb_data_type *mfd = NULL;
+
+        mfd = container_of(work, struct msm_fb_data_type, dma_update_worker);
+        if (mfd)
+                mfd->dma_fnc(mfd);
+}
+
+void mdp_set_dma_pan_info(struct fb_info *info, struct mdp_dirty_region *dirty,
+                          boolean sync)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        MDPIBUF *iBuf;
+        int bpp = info->var.bits_per_pixel / 8;
+
+        down(&mfd->sem);
+        iBuf = &mfd->ibuf;
+        iBuf->buf = (uint8 *) info->fix.smem_start;
+        iBuf->buf += info->var.xoffset * bpp +
+                        info->var.yoffset * info->fix.line_length;
+
+        iBuf->ibuf_width = info->var.xres_virtual;
+        iBuf->bpp = bpp;
+
+        iBuf->vsync_enable = sync;
+
+        if (dirty) {
+                /*
+                 * ToDo: dirty region check inside var.xoffset+xres
+                 * <-> var.yoffset+yres
+                 */
+                iBuf->dma_x = dirty->xoffset % info->var.xres;
+                iBuf->dma_y = dirty->yoffset % info->var.yres;
+                iBuf->dma_w = dirty->width;
+                iBuf->dma_h = dirty->height;
+        } else {
+                iBuf->dma_x = 0;
+                iBuf->dma_y = 0;
+                iBuf->dma_w = info->var.xres;
+                iBuf->dma_h = info->var.yres;
+        }
+        mfd->ibuf_flushed = FALSE;
+        up(&mfd->sem);
+}
+
+void mdp_set_offset_info(struct fb_info *info, uint32 addr, uint32 sync)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        MDPIBUF *iBuf;
+
+        int bpp = info->var.bits_per_pixel / 8;
+
+        down(&mfd->sem);
+        iBuf = &mfd->ibuf;
+        iBuf->ibuf_width = info->var.xres_virtual;
+        iBuf->bpp = bpp;
+        iBuf->vsync_enable = sync;
+        iBuf->dma_x = 0;
+        iBuf->dma_y = 0;
+        iBuf->dma_w = info->var.xres;
+        iBuf->dma_h = info->var.yres;
+        iBuf->buf = (uint8 *) addr;
+
+        mfd->ibuf_flushed = FALSE;
+        up(&mfd->sem);
+}
+
+void mdp_dma_pan_update(struct fb_info *info)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        MDPIBUF *iBuf;
+
+        iBuf = &mfd->ibuf;
+
+        if (mfd->sw_currently_refreshing) {
+                /* we need to wait for the pending update */
+                mfd->pan_waiting = TRUE;
+                if (!mfd->ibuf_flushed) {
+                        wait_for_completion_killable(&mfd->pan_comp);
+                }
+                /* waiting for this update to complete */
+                mfd->pan_waiting = TRUE;
+                wait_for_completion_killable(&mfd->pan_comp);
+        } else
+                mfd->dma_fnc(mfd);
+}
+
+void mdp_refresh_screen(unsigned long data)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)data;
+
+        if ((mfd->sw_currently_refreshing) && (mfd->sw_refreshing_enable)) {
+                init_timer(&mfd->refresh_timer);
+                mfd->refresh_timer.function = mdp_refresh_screen;
+                mfd->refresh_timer.data = data;
+
+                if (mfd->dma->busy)
+                        /* come back in 1 msec */
+                        mfd->refresh_timer.expires = jiffies + (HZ / 1000);
+                else
+                        mfd->refresh_timer.expires =
+                            jiffies + mfd->refresh_timer_duration;
+
+                add_timer(&mfd->refresh_timer);
+
+                if (!mfd->dma->busy) {
+                        if (!queue_work(mdp_dma_wq, &mfd->dma_update_worker)) {
+                                MSM_FB_DEBUG("mdp_dma: can't queue_work! -> \
+                        MDP/MDDI/LCD clock speed needs to be increased\n");
+                        }
+                }
+        } else {
+                if (!mfd->hw_refresh)
+                        complete(&mfd->refresher_comp);
+        }
+}
diff --git a/drivers/staging/msm/mdp_dma_lcdc.c b/drivers/staging/msm/mdp_dma_lcdc.c
new file mode 100644
index 000000000000..b57fa1a0ceb0
--- /dev/null
+++ b/drivers/staging/msm/mdp_dma_lcdc.c
@@ -0,0 +1,379 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+#include <linux/delay.h>
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/spinlock.h>
+
+#include <linux/fb.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+#include "mdp4.h"
+
+#ifdef CONFIG_FB_MSM_MDP40
+#define LCDC_BASE       0xC0000
+#define DTV_BASE        0xD0000
+#define DMA_E_BASE      0xB0000
+#else
+#define LCDC_BASE       0xE0000
+#endif
+
+#define DMA_P_BASE      0x90000
+
+extern spinlock_t mdp_spin_lock;
+#ifndef CONFIG_FB_MSM_MDP40
+extern uint32 mdp_intr_mask;
+#endif
+
+int first_pixel_start_x;
+int first_pixel_start_y;
+
+int mdp_lcdc_on(struct platform_device *pdev)
+{
+        int lcdc_width;
+        int lcdc_height;
+        int lcdc_bpp;
+        int lcdc_border_clr;
+        int lcdc_underflow_clr;
+        int lcdc_hsync_skew;
+
+        int hsync_period;
+        int hsync_ctrl;
+        int vsync_period;
+        int display_hctl;
+        int display_v_start;
+        int display_v_end;
+        int active_hctl;
+        int active_h_start;
+        int active_h_end;
+        int active_v_start;
+        int active_v_end;
+        int ctrl_polarity;
+        int h_back_porch;
+        int h_front_porch;
+        int v_back_porch;
+        int v_front_porch;
+        int hsync_pulse_width;
+        int vsync_pulse_width;
+        int hsync_polarity;
+        int vsync_polarity;
+        int data_en_polarity;
+        int hsync_start_x;
+        int hsync_end_x;
+        uint8 *buf;
+        int bpp;
+        uint32 dma2_cfg_reg;
+        struct fb_info *fbi;
+        struct fb_var_screeninfo *var;
+        struct msm_fb_data_type *mfd;
+        uint32 dma_base;
+        uint32 timer_base = LCDC_BASE;
+        uint32 block = MDP_DMA2_BLOCK;
+        int ret;
+
+        mfd = (struct msm_fb_data_type *)platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        fbi = mfd->fbi;
+        var = &fbi->var;
+
+        /* MDP cmd block enable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+        bpp = fbi->var.bits_per_pixel / 8;
+        buf = (uint8 *) fbi->fix.smem_start;
+        buf += fbi->var.xoffset * bpp + fbi->var.yoffset * fbi->fix.line_length;
+
+        dma2_cfg_reg = DMA_PACK_ALIGN_LSB | DMA_DITHER_EN | DMA_OUT_SEL_LCDC;
+
+        if (mfd->fb_imgType == MDP_BGR_565)
+                dma2_cfg_reg |= DMA_PACK_PATTERN_BGR;
+        else
+                dma2_cfg_reg |= DMA_PACK_PATTERN_RGB;
+
+        if (bpp == 2)
+                dma2_cfg_reg |= DMA_IBUF_FORMAT_RGB565;
+        else if (bpp == 3)
+                dma2_cfg_reg |= DMA_IBUF_FORMAT_RGB888;
+        else
+                dma2_cfg_reg |= DMA_IBUF_FORMAT_xRGB8888_OR_ARGB8888;
+
+        switch (mfd->panel_info.bpp) {
+        case 24:
+                dma2_cfg_reg |= DMA_DSTC0G_8BITS |
+                    DMA_DSTC1B_8BITS | DMA_DSTC2R_8BITS;
+                break;
+
+        case 18:
+                dma2_cfg_reg |= DMA_DSTC0G_6BITS |
+                    DMA_DSTC1B_6BITS | DMA_DSTC2R_6BITS;
+                break;
+
+        case 16:
+                dma2_cfg_reg |= DMA_DSTC0G_6BITS |
+                    DMA_DSTC1B_5BITS | DMA_DSTC2R_5BITS;
+                break;
+
+        default:
+                printk(KERN_ERR "mdp lcdc can't support format %d bpp!\n",
+                       mfd->panel_info.bpp);
+                return -ENODEV;
+        }
+
+        /* DMA register config */
+
+        dma_base = DMA_P_BASE;
+
+#ifdef CONFIG_FB_MSM_MDP40
+        if (mfd->panel.type == HDMI_PANEL)
+                dma_base = DMA_E_BASE;
+#endif
+
+        /* starting address */
+        MDP_OUTP(MDP_BASE + dma_base + 0x8, (uint32) buf);
+        /* active window width and height */
+        MDP_OUTP(MDP_BASE + dma_base + 0x4, ((fbi->var.yres) << 16) |
+                                                (fbi->var.xres));
+        /* buffer ystride */
+        MDP_OUTP(MDP_BASE + dma_base + 0xc, fbi->fix.line_length);
+        /* x/y coordinate = always 0 for lcdc */
+        MDP_OUTP(MDP_BASE + dma_base + 0x10, 0);
+        /* dma config */
+        MDP_OUTP(MDP_BASE + dma_base, dma2_cfg_reg);
+
+        /*
+         * LCDC timing setting
+         */
+        h_back_porch = var->left_margin;
+        h_front_porch = var->right_margin;
+        v_back_porch = var->upper_margin;
+        v_front_porch = var->lower_margin;
+        hsync_pulse_width = var->hsync_len;
+        vsync_pulse_width = var->vsync_len;
+        lcdc_border_clr = mfd->panel_info.lcdc.border_clr;
+        lcdc_underflow_clr = mfd->panel_info.lcdc.underflow_clr;
+        lcdc_hsync_skew = mfd->panel_info.lcdc.hsync_skew;
+
+        lcdc_width = mfd->panel_info.xres;
+        lcdc_height = mfd->panel_info.yres;
+        lcdc_bpp = mfd->panel_info.bpp;
+
+        hsync_period =
+            hsync_pulse_width + h_back_porch + lcdc_width + h_front_porch;
+        hsync_ctrl = (hsync_period << 16) | hsync_pulse_width;
+        hsync_start_x = hsync_pulse_width + h_back_porch;
+        hsync_end_x = hsync_period - h_front_porch - 1;
+        display_hctl = (hsync_end_x << 16) | hsync_start_x;
+
+        vsync_period =
+            (vsync_pulse_width + v_back_porch + lcdc_height +
+             v_front_porch) * hsync_period;
+        display_v_start =
+            (vsync_pulse_width + v_back_porch) * hsync_period + lcdc_hsync_skew;
+        display_v_end =
+            vsync_period - (v_front_porch * hsync_period) + lcdc_hsync_skew - 1;
+
+        if (lcdc_width != var->xres) {
+                active_h_start = hsync_start_x + first_pixel_start_x;
+                active_h_end = active_h_start + var->xres - 1;
+                active_hctl =
+                    ACTIVE_START_X_EN | (active_h_end << 16) | active_h_start;
+        } else {
+                active_hctl = 0;
+        }
+
+        if (lcdc_height != var->yres) {
+                active_v_start =
+                    display_v_start + first_pixel_start_y * hsync_period;
+                active_v_end = active_v_start + (var->yres) * hsync_period - 1;
+                active_v_start |= ACTIVE_START_Y_EN;
+        } else {
+                active_v_start = 0;
+                active_v_end = 0;
+        }
+
+
+#ifdef CONFIG_FB_MSM_MDP40
+        if (mfd->panel.type == HDMI_PANEL) {
+                block = MDP_DMA_E_BLOCK;
+                timer_base = DTV_BASE;
+                hsync_polarity = 0;
+                vsync_polarity = 0;
+        } else {
+                hsync_polarity = 1;
+                vsync_polarity = 1;
+        }
+
+        lcdc_underflow_clr |= 0x80000000;       /* enable recovery */
+#else
+        hsync_polarity = 0;
+        vsync_polarity = 0;
+#endif
+        data_en_polarity = 0;
+
+        ctrl_polarity =
+            (data_en_polarity << 2) | (vsync_polarity << 1) | (hsync_polarity);
+
+        MDP_OUTP(MDP_BASE + timer_base + 0x4, hsync_ctrl);
+        MDP_OUTP(MDP_BASE + timer_base + 0x8, vsync_period);
+        MDP_OUTP(MDP_BASE + timer_base + 0xc, vsync_pulse_width * hsync_period);
+        if (timer_base == LCDC_BASE) {
+                MDP_OUTP(MDP_BASE + timer_base + 0x10, display_hctl);
+                MDP_OUTP(MDP_BASE + timer_base + 0x14, display_v_start);
+                MDP_OUTP(MDP_BASE + timer_base + 0x18, display_v_end);
+                MDP_OUTP(MDP_BASE + timer_base + 0x28, lcdc_border_clr);
+                MDP_OUTP(MDP_BASE + timer_base + 0x2c, lcdc_underflow_clr);
+                MDP_OUTP(MDP_BASE + timer_base + 0x30, lcdc_hsync_skew);
+                MDP_OUTP(MDP_BASE + timer_base + 0x38, ctrl_polarity);
+                MDP_OUTP(MDP_BASE + timer_base + 0x1c, active_hctl);
+                MDP_OUTP(MDP_BASE + timer_base + 0x20, active_v_start);
+                MDP_OUTP(MDP_BASE + timer_base + 0x24, active_v_end);
+        } else {
+                MDP_OUTP(MDP_BASE + timer_base + 0x18, display_hctl);
+                MDP_OUTP(MDP_BASE + timer_base + 0x1c, display_v_start);
+                MDP_OUTP(MDP_BASE + timer_base + 0x20, display_v_end);
+                MDP_OUTP(MDP_BASE + timer_base + 0x40, lcdc_border_clr);
+                MDP_OUTP(MDP_BASE + timer_base + 0x44, lcdc_underflow_clr);
+                MDP_OUTP(MDP_BASE + timer_base + 0x48, lcdc_hsync_skew);
+                MDP_OUTP(MDP_BASE + timer_base + 0x50, ctrl_polarity);
+                MDP_OUTP(MDP_BASE + timer_base + 0x2c, active_hctl);
+                MDP_OUTP(MDP_BASE + timer_base + 0x30, active_v_start);
+                MDP_OUTP(MDP_BASE + timer_base + 0x38, active_v_end);
+        }
+
+        ret = panel_next_on(pdev);
+        if (ret == 0) {
+                /* enable LCDC block */
+                MDP_OUTP(MDP_BASE + timer_base, 1);
+                mdp_pipe_ctrl(block, MDP_BLOCK_POWER_ON, FALSE);
+        }
+        /* MDP cmd block disable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+
+        return ret;
+}
+
+int mdp_lcdc_off(struct platform_device *pdev)
+{
+        int ret = 0;
+        struct msm_fb_data_type *mfd;
+        uint32 timer_base = LCDC_BASE;
+        uint32 block = MDP_DMA2_BLOCK;
+
+        mfd = (struct msm_fb_data_type *)platform_get_drvdata(pdev);
+
+#ifdef CONFIG_FB_MSM_MDP40
+        if (mfd->panel.type == HDMI_PANEL) {
+                block = MDP_DMA_E_BLOCK;
+                timer_base = DTV_BASE;
+        }
+#endif
+
+        /* MDP cmd block enable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+        MDP_OUTP(MDP_BASE + timer_base, 0);
+        /* MDP cmd block disable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+        mdp_pipe_ctrl(block, MDP_BLOCK_POWER_OFF, FALSE);
+
+        ret = panel_next_off(pdev);
+
+        /* delay to make sure the last frame finishes */
+        mdelay(100);
+
+        return ret;
+}
+
+void mdp_lcdc_update(struct msm_fb_data_type *mfd)
+{
+        struct fb_info *fbi = mfd->fbi;
+        uint8 *buf;
+        int bpp;
+        unsigned long flag;
+        uint32 dma_base;
+        int irq_block = MDP_DMA2_TERM;
+#ifdef CONFIG_FB_MSM_MDP40
+        int intr = INTR_DMA_P_DONE;
+#endif
+
+        if (!mfd->panel_power_on)
+                return;
+
+        /* no need to power on cmd block since it's lcdc mode */
+
+        if (!mfd->ibuf.visible_swapped) {
+                bpp = fbi->var.bits_per_pixel / 8;
+                buf = (uint8 *) fbi->fix.smem_start;
+                buf += fbi->var.xoffset * bpp +
+                fbi->var.yoffset * fbi->fix.line_length;
+        } else {
+                /* we've done something to update the pointer. */
+                bpp =  mfd->ibuf.bpp;
+                buf = mfd->ibuf.buf;
+        }
+
+        dma_base = DMA_P_BASE;
+
+#ifdef CONFIG_FB_MSM_MDP40
+        if (mfd->panel.type == HDMI_PANEL) {
+                intr = INTR_DMA_E_DONE;
+                irq_block = MDP_DMA_E_TERM;
+                dma_base = DMA_E_BASE;
+        }
+#endif
+
+        /* starting address */
+        MDP_OUTP(MDP_BASE + dma_base + 0x8, (uint32) buf);
+
+        /* enable LCDC irq */
+        spin_lock_irqsave(&mdp_spin_lock, flag);
+        mdp_enable_irq(irq_block);
+        INIT_COMPLETION(mfd->dma->comp);
+        mfd->dma->waiting = TRUE;
+#ifdef CONFIG_FB_MSM_MDP40
+        outp32(MDP_INTR_CLEAR, intr);
+        mdp_intr_mask |= intr;
+        outp32(MDP_INTR_ENABLE, mdp_intr_mask);
+#else
+        outp32(MDP_INTR_CLEAR, LCDC_FRAME_START);
+        mdp_intr_mask |= LCDC_FRAME_START;
+        outp32(MDP_INTR_ENABLE, mdp_intr_mask);
+#endif
+        spin_unlock_irqrestore(&mdp_spin_lock, flag);
+
+        if (mfd->ibuf.vsync_enable)
+                wait_for_completion_killable(&mfd->dma->comp);
+        mdp_disable_irq(irq_block);
+}
diff --git a/drivers/staging/msm/mdp_dma_s.c b/drivers/staging/msm/mdp_dma_s.c
new file mode 100644
index 000000000000..0c34a1010f17
--- /dev/null
+++ b/drivers/staging/msm/mdp_dma_s.c
@@ -0,0 +1,139 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/spinlock.h>
+
+#include <linux/fb.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+
+static void mdp_dma_s_update_lcd(struct msm_fb_data_type *mfd)
+{
+        MDPIBUF *iBuf = &mfd->ibuf;
+        int mddi_dest = FALSE;
+        uint32 outBpp = iBuf->bpp;
+        uint32 dma_s_cfg_reg;
+        uint8 *src;
+        struct msm_fb_panel_data *pdata =
+            (struct msm_fb_panel_data *)mfd->pdev->dev.platform_data;
+
+        dma_s_cfg_reg = DMA_PACK_TIGHT | DMA_PACK_ALIGN_LSB |
+            DMA_OUT_SEL_AHB | DMA_IBUF_NONCONTIGUOUS;
+
+        if (mfd->fb_imgType == MDP_BGR_565)
+                dma_s_cfg_reg |= DMA_PACK_PATTERN_BGR;
+        else
+                dma_s_cfg_reg |= DMA_PACK_PATTERN_RGB;
+
+        if (outBpp == 4)
+                dma_s_cfg_reg |= DMA_IBUF_C3ALPHA_EN;
+
+        if (outBpp == 2)
+                dma_s_cfg_reg |= DMA_IBUF_FORMAT_RGB565;
+
+        if (mfd->panel_info.pdest != DISPLAY_2) {
+                printk(KERN_ERR "error: non-secondary type through dma_s!\n");
+                return;
+        }
+
+        if (mfd->panel_info.type == MDDI_PANEL) {
+                dma_s_cfg_reg |= DMA_OUT_SEL_MDDI;
+                mddi_dest = TRUE;
+        } else {
+                dma_s_cfg_reg |= DMA_AHBM_LCD_SEL_SECONDARY;
+                outp32(MDP_EBI2_LCD1, mfd->data_port_phys);
+        }
+
+        dma_s_cfg_reg |= DMA_DITHER_EN;
+
+        src = (uint8 *) iBuf->buf;
+        /* starting input address */
+        src += (iBuf->dma_x + iBuf->dma_y * iBuf->ibuf_width) * outBpp;
+
+        /* MDP cmd block enable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+        /* PIXELSIZE */
+        MDP_OUTP(MDP_BASE + 0xa0004, (iBuf->dma_h << 16 | iBuf->dma_w));
+        MDP_OUTP(MDP_BASE + 0xa0008, src);      /* ibuf address */
+        MDP_OUTP(MDP_BASE + 0xa000c, iBuf->ibuf_width * outBpp);/* ystride */
+
+        if (mfd->panel_info.bpp == 18) {
+                dma_s_cfg_reg |= DMA_DSTC0G_6BITS |     /* 666 18BPP */
+                    DMA_DSTC1B_6BITS | DMA_DSTC2R_6BITS;
+        } else {
+                dma_s_cfg_reg |= DMA_DSTC0G_6BITS |     /* 565 16BPP */
+                    DMA_DSTC1B_5BITS | DMA_DSTC2R_5BITS;
+        }
+
+        if (mddi_dest) {
+                MDP_OUTP(MDP_BASE + 0xa0010, (iBuf->dma_y << 16) | iBuf->dma_x);
+                MDP_OUTP(MDP_BASE + 0x00090, 1);
+                MDP_OUTP(MDP_BASE + 0x00094,
+                                (MDDI_VDO_PACKET_DESC << 16) |
+                                mfd->panel_info.mddi.vdopkt);
+        } else {
+                /* setting LCDC write window */
+                pdata->set_rect(iBuf->dma_x, iBuf->dma_y, iBuf->dma_w,
+                                iBuf->dma_h);
+        }
+
+        MDP_OUTP(MDP_BASE + 0xa0000, dma_s_cfg_reg);
+
+        /* MDP cmd block disable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+        mdp_pipe_kickoff(MDP_DMA_S_TERM, mfd);
+}
+
+void mdp_dma_s_update(struct msm_fb_data_type *mfd)
+{
+        down(&mfd->dma->mutex);
+        if ((mfd) && (!mfd->dma->busy) && (mfd->panel_power_on)) {
+                down(&mfd->sem);
+                mdp_enable_irq(MDP_DMA_S_TERM);
+                mfd->dma->busy = TRUE;
+                INIT_COMPLETION(mfd->dma->comp);
+                mfd->ibuf_flushed = TRUE;
+                mdp_dma_s_update_lcd(mfd);
+                up(&mfd->sem);
+
+                /* wait until DMA finishes the current job */
+                wait_for_completion_killable(&mfd->dma->comp);
+                mdp_disable_irq(MDP_DMA_S_TERM);
+
+        /* signal if pan function is waiting for the update completion */
+                if (mfd->pan_waiting) {
+                        mfd->pan_waiting = FALSE;
+                        complete(&mfd->pan_comp);
+                }
+        }
+        up(&mfd->dma->mutex);
+}
diff --git a/drivers/staging/msm/mdp_dma_tv.c b/drivers/staging/msm/mdp_dma_tv.c
new file mode 100644
index 000000000000..70989fb32c1d
--- /dev/null
+++ b/drivers/staging/msm/mdp_dma_tv.c
@@ -0,0 +1,142 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/hrtimer.h>
+#include <linux/delay.h>
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/spinlock.h>
+
+#include <linux/fb.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+
+extern spinlock_t mdp_spin_lock;
+extern uint32 mdp_intr_mask;
+
+int mdp_dma3_on(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+        struct fb_info *fbi;
+        uint8 *buf;
+        int bpp;
+        int ret = 0;
+
+        mfd = (struct msm_fb_data_type *)platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        fbi = mfd->fbi;
+        /* MDP cmd block enable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+        bpp = fbi->var.bits_per_pixel / 8;
+        buf = (uint8 *) fbi->fix.smem_start;
+        buf += fbi->var.xoffset * bpp +
+                fbi->var.yoffset * fbi->fix.line_length;
+
+        /* starting address[31..8] of Video frame buffer is CS0 */
+        MDP_OUTP(MDP_BASE + 0xC0008, (uint32) buf >> 3);
+
+        mdp_pipe_ctrl(MDP_DMA3_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+        MDP_OUTP(MDP_BASE + 0xC0004, 0x4c60674); /* flicker filter enabled */
+        MDP_OUTP(MDP_BASE + 0xC0010, 0x20);     /* sobel treshold */
+
+        MDP_OUTP(MDP_BASE + 0xC0018, 0xeb0010); /* Y  Max, Y  min */
+        MDP_OUTP(MDP_BASE + 0xC001C, 0xf00010); /* Cb Max, Cb min */
+        MDP_OUTP(MDP_BASE + 0xC0020, 0xf00010); /* Cb Max, Cb min */
+
+        MDP_OUTP(MDP_BASE + 0xC000C, 0x67686970); /* add a few chars for CC */
+        MDP_OUTP(MDP_BASE + 0xC0000, 0x1);      /* MDP tv out enable */
+
+        /* MDP cmd block disable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+
+        ret = panel_next_on(pdev);
+
+        return ret;
+}
+
+int mdp_dma3_off(struct platform_device *pdev)
+{
+        int ret = 0;
+
+        ret = panel_next_off(pdev);
+        if (ret)
+                return ret;
+
+        /* MDP cmd block enable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+        MDP_OUTP(MDP_BASE + 0xC0000, 0x0);
+        /* MDP cmd block disable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+
+        mdp_pipe_ctrl(MDP_DMA3_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+
+        /* delay to make sure the last frame finishes */
+        mdelay(100);
+
+        return ret;
+}
+
+void mdp_dma3_update(struct msm_fb_data_type *mfd)
+{
+        struct fb_info *fbi = mfd->fbi;
+        uint8 *buf;
+        int bpp;
+        unsigned long flag;
+
+        if (!mfd->panel_power_on)
+                return;
+
+        /* no need to power on cmd block since dma3 is running */
+        bpp = fbi->var.bits_per_pixel / 8;
+        buf = (uint8 *) fbi->fix.smem_start;
+        buf += fbi->var.xoffset * bpp +
+                fbi->var.yoffset * fbi->fix.line_length;
+        MDP_OUTP(MDP_BASE + 0xC0008, (uint32) buf >> 3);
+
+        spin_lock_irqsave(&mdp_spin_lock, flag);
+        mdp_enable_irq(MDP_DMA3_TERM);
+        INIT_COMPLETION(mfd->dma->comp);
+        mfd->dma->waiting = TRUE;
+
+        outp32(MDP_INTR_CLEAR, TV_OUT_DMA3_START);
+        mdp_intr_mask |= TV_OUT_DMA3_START;
+        outp32(MDP_INTR_ENABLE, mdp_intr_mask);
+        spin_unlock_irqrestore(&mdp_spin_lock, flag);
+
+        wait_for_completion_killable(&mfd->dma->comp);
+        mdp_disable_irq(MDP_DMA3_TERM);
+}
diff --git a/drivers/staging/msm/mdp_hw_init.c b/drivers/staging/msm/mdp_hw_init.c
new file mode 100644
index 000000000000..807362ac592c
--- /dev/null
+++ b/drivers/staging/msm/mdp_hw_init.c
@@ -0,0 +1,720 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "mdp.h"
+
+/* mdp primary csc limit vector */
+uint32 mdp_plv[] = { 0x10, 0xeb, 0x10, 0xf0 };
+
+/* Color Coefficient matrix for YUV -> RGB */
+struct mdp_ccs mdp_ccs_yuv2rgb = {
+        MDP_CCS_YUV2RGB,
+        {
+                0x254,
+                0x000,
+                0x331,
+                0x254,
+                0xff38,
+                0xfe61,
+                0x254,
+                0x409,
+                0x000,
+        },
+        {
+#ifdef CONFIG_FB_MSM_MDP31
+                0x1f0,
+                0x180,
+                0x180
+#else
+                0x10,
+                0x80,
+                0x80
+#endif
+        }
+};
+
+/* Color Coefficient matrix for RGB -> YUV */
+struct mdp_ccs mdp_ccs_rgb2yuv = {
+        MDP_CCS_RGB2YUV,
+        {
+                0x83,
+                0x102,
+                0x32,
+                0xffb5,
+                0xff6c,
+                0xe1,
+                0xe1,
+                0xff45,
+                0xffdc,
+        },
+#ifdef CONFIG_FB_MSM_MDP31
+        {
+                0x10,
+                0x80,
+                0x80
+        }
+#endif
+};
+
+static void mdp_load_lut_param(void)
+{
+        outpdw(MDP_BASE + 0x40800, 0x0);
+        outpdw(MDP_BASE + 0x40804, 0x151515);
+        outpdw(MDP_BASE + 0x40808, 0x1d1d1d);
+        outpdw(MDP_BASE + 0x4080c, 0x232323);
+        outpdw(MDP_BASE + 0x40810, 0x272727);
+        outpdw(MDP_BASE + 0x40814, 0x2b2b2b);
+        outpdw(MDP_BASE + 0x40818, 0x2f2f2f);
+        outpdw(MDP_BASE + 0x4081c, 0x333333);
+        outpdw(MDP_BASE + 0x40820, 0x363636);
+        outpdw(MDP_BASE + 0x40824, 0x393939);
+        outpdw(MDP_BASE + 0x40828, 0x3b3b3b);
+        outpdw(MDP_BASE + 0x4082c, 0x3e3e3e);
+        outpdw(MDP_BASE + 0x40830, 0x404040);
+        outpdw(MDP_BASE + 0x40834, 0x434343);
+        outpdw(MDP_BASE + 0x40838, 0x454545);
+        outpdw(MDP_BASE + 0x4083c, 0x474747);
+        outpdw(MDP_BASE + 0x40840, 0x494949);
+        outpdw(MDP_BASE + 0x40844, 0x4b4b4b);
+        outpdw(MDP_BASE + 0x40848, 0x4d4d4d);
+        outpdw(MDP_BASE + 0x4084c, 0x4f4f4f);
+        outpdw(MDP_BASE + 0x40850, 0x515151);
+        outpdw(MDP_BASE + 0x40854, 0x535353);
+        outpdw(MDP_BASE + 0x40858, 0x555555);
+        outpdw(MDP_BASE + 0x4085c, 0x565656);
+        outpdw(MDP_BASE + 0x40860, 0x585858);
+        outpdw(MDP_BASE + 0x40864, 0x5a5a5a);
+        outpdw(MDP_BASE + 0x40868, 0x5b5b5b);
+        outpdw(MDP_BASE + 0x4086c, 0x5d5d5d);
+        outpdw(MDP_BASE + 0x40870, 0x5e5e5e);
+        outpdw(MDP_BASE + 0x40874, 0x606060);
+        outpdw(MDP_BASE + 0x40878, 0x616161);
+        outpdw(MDP_BASE + 0x4087c, 0x636363);
+        outpdw(MDP_BASE + 0x40880, 0x646464);
+        outpdw(MDP_BASE + 0x40884, 0x666666);
+        outpdw(MDP_BASE + 0x40888, 0x676767);
+        outpdw(MDP_BASE + 0x4088c, 0x686868);
+        outpdw(MDP_BASE + 0x40890, 0x6a6a6a);
+        outpdw(MDP_BASE + 0x40894, 0x6b6b6b);
+        outpdw(MDP_BASE + 0x40898, 0x6c6c6c);
+        outpdw(MDP_BASE + 0x4089c, 0x6e6e6e);
+        outpdw(MDP_BASE + 0x408a0, 0x6f6f6f);
+        outpdw(MDP_BASE + 0x408a4, 0x707070);
+        outpdw(MDP_BASE + 0x408a8, 0x717171);
+        outpdw(MDP_BASE + 0x408ac, 0x727272);
+        outpdw(MDP_BASE + 0x408b0, 0x747474);
+        outpdw(MDP_BASE + 0x408b4, 0x757575);
+        outpdw(MDP_BASE + 0x408b8, 0x767676);
+        outpdw(MDP_BASE + 0x408bc, 0x777777);
+        outpdw(MDP_BASE + 0x408c0, 0x787878);
+        outpdw(MDP_BASE + 0x408c4, 0x797979);
+        outpdw(MDP_BASE + 0x408c8, 0x7a7a7a);
+        outpdw(MDP_BASE + 0x408cc, 0x7c7c7c);
+        outpdw(MDP_BASE + 0x408d0, 0x7d7d7d);
+        outpdw(MDP_BASE + 0x408d4, 0x7e7e7e);
+        outpdw(MDP_BASE + 0x408d8, 0x7f7f7f);
+        outpdw(MDP_BASE + 0x408dc, 0x808080);
+        outpdw(MDP_BASE + 0x408e0, 0x818181);
+        outpdw(MDP_BASE + 0x408e4, 0x828282);
+        outpdw(MDP_BASE + 0x408e8, 0x838383);
+        outpdw(MDP_BASE + 0x408ec, 0x848484);
+        outpdw(MDP_BASE + 0x408f0, 0x858585);
+        outpdw(MDP_BASE + 0x408f4, 0x868686);
+        outpdw(MDP_BASE + 0x408f8, 0x878787);
+        outpdw(MDP_BASE + 0x408fc, 0x888888);
+        outpdw(MDP_BASE + 0x40900, 0x898989);
+        outpdw(MDP_BASE + 0x40904, 0x8a8a8a);
+        outpdw(MDP_BASE + 0x40908, 0x8b8b8b);
+        outpdw(MDP_BASE + 0x4090c, 0x8c8c8c);
+        outpdw(MDP_BASE + 0x40910, 0x8d8d8d);
+        outpdw(MDP_BASE + 0x40914, 0x8e8e8e);
+        outpdw(MDP_BASE + 0x40918, 0x8f8f8f);
+        outpdw(MDP_BASE + 0x4091c, 0x8f8f8f);
+        outpdw(MDP_BASE + 0x40920, 0x909090);
+        outpdw(MDP_BASE + 0x40924, 0x919191);
+        outpdw(MDP_BASE + 0x40928, 0x929292);
+        outpdw(MDP_BASE + 0x4092c, 0x939393);
+        outpdw(MDP_BASE + 0x40930, 0x949494);
+        outpdw(MDP_BASE + 0x40934, 0x959595);
+        outpdw(MDP_BASE + 0x40938, 0x969696);
+        outpdw(MDP_BASE + 0x4093c, 0x969696);
+        outpdw(MDP_BASE + 0x40940, 0x979797);
+        outpdw(MDP_BASE + 0x40944, 0x989898);
+        outpdw(MDP_BASE + 0x40948, 0x999999);
+        outpdw(MDP_BASE + 0x4094c, 0x9a9a9a);
+        outpdw(MDP_BASE + 0x40950, 0x9b9b9b);
+        outpdw(MDP_BASE + 0x40954, 0x9c9c9c);
+        outpdw(MDP_BASE + 0x40958, 0x9c9c9c);
+        outpdw(MDP_BASE + 0x4095c, 0x9d9d9d);
+        outpdw(MDP_BASE + 0x40960, 0x9e9e9e);
+        outpdw(MDP_BASE + 0x40964, 0x9f9f9f);
+        outpdw(MDP_BASE + 0x40968, 0xa0a0a0);
+        outpdw(MDP_BASE + 0x4096c, 0xa0a0a0);
+        outpdw(MDP_BASE + 0x40970, 0xa1a1a1);
+        outpdw(MDP_BASE + 0x40974, 0xa2a2a2);
+        outpdw(MDP_BASE + 0x40978, 0xa3a3a3);
+        outpdw(MDP_BASE + 0x4097c, 0xa4a4a4);
+        outpdw(MDP_BASE + 0x40980, 0xa4a4a4);
+        outpdw(MDP_BASE + 0x40984, 0xa5a5a5);
+        outpdw(MDP_BASE + 0x40988, 0xa6a6a6);
+        outpdw(MDP_BASE + 0x4098c, 0xa7a7a7);
+        outpdw(MDP_BASE + 0x40990, 0xa7a7a7);
+        outpdw(MDP_BASE + 0x40994, 0xa8a8a8);
+        outpdw(MDP_BASE + 0x40998, 0xa9a9a9);
+        outpdw(MDP_BASE + 0x4099c, 0xaaaaaa);
+        outpdw(MDP_BASE + 0x409a0, 0xaaaaaa);
+        outpdw(MDP_BASE + 0x409a4, 0xababab);
+        outpdw(MDP_BASE + 0x409a8, 0xacacac);
+        outpdw(MDP_BASE + 0x409ac, 0xadadad);
+        outpdw(MDP_BASE + 0x409b0, 0xadadad);
+        outpdw(MDP_BASE + 0x409b4, 0xaeaeae);
+        outpdw(MDP_BASE + 0x409b8, 0xafafaf);
+        outpdw(MDP_BASE + 0x409bc, 0xafafaf);
+        outpdw(MDP_BASE + 0x409c0, 0xb0b0b0);
+        outpdw(MDP_BASE + 0x409c4, 0xb1b1b1);
+        outpdw(MDP_BASE + 0x409c8, 0xb2b2b2);
+        outpdw(MDP_BASE + 0x409cc, 0xb2b2b2);
+        outpdw(MDP_BASE + 0x409d0, 0xb3b3b3);
+        outpdw(MDP_BASE + 0x409d4, 0xb4b4b4);
+        outpdw(MDP_BASE + 0x409d8, 0xb4b4b4);
+        outpdw(MDP_BASE + 0x409dc, 0xb5b5b5);
+        outpdw(MDP_BASE + 0x409e0, 0xb6b6b6);
+        outpdw(MDP_BASE + 0x409e4, 0xb6b6b6);
+        outpdw(MDP_BASE + 0x409e8, 0xb7b7b7);
+        outpdw(MDP_BASE + 0x409ec, 0xb8b8b8);
+        outpdw(MDP_BASE + 0x409f0, 0xb8b8b8);
+        outpdw(MDP_BASE + 0x409f4, 0xb9b9b9);
+        outpdw(MDP_BASE + 0x409f8, 0xbababa);
+        outpdw(MDP_BASE + 0x409fc, 0xbababa);
+        outpdw(MDP_BASE + 0x40a00, 0xbbbbbb);
+        outpdw(MDP_BASE + 0x40a04, 0xbcbcbc);
+        outpdw(MDP_BASE + 0x40a08, 0xbcbcbc);
+        outpdw(MDP_BASE + 0x40a0c, 0xbdbdbd);
+        outpdw(MDP_BASE + 0x40a10, 0xbebebe);
+        outpdw(MDP_BASE + 0x40a14, 0xbebebe);
+        outpdw(MDP_BASE + 0x40a18, 0xbfbfbf);
+        outpdw(MDP_BASE + 0x40a1c, 0xc0c0c0);
+        outpdw(MDP_BASE + 0x40a20, 0xc0c0c0);
+        outpdw(MDP_BASE + 0x40a24, 0xc1c1c1);
+        outpdw(MDP_BASE + 0x40a28, 0xc1c1c1);
+        outpdw(MDP_BASE + 0x40a2c, 0xc2c2c2);
+        outpdw(MDP_BASE + 0x40a30, 0xc3c3c3);
+        outpdw(MDP_BASE + 0x40a34, 0xc3c3c3);
+        outpdw(MDP_BASE + 0x40a38, 0xc4c4c4);
+        outpdw(MDP_BASE + 0x40a3c, 0xc5c5c5);
+        outpdw(MDP_BASE + 0x40a40, 0xc5c5c5);
+        outpdw(MDP_BASE + 0x40a44, 0xc6c6c6);
+        outpdw(MDP_BASE + 0x40a48, 0xc6c6c6);
+        outpdw(MDP_BASE + 0x40a4c, 0xc7c7c7);
+        outpdw(MDP_BASE + 0x40a50, 0xc8c8c8);
+        outpdw(MDP_BASE + 0x40a54, 0xc8c8c8);
+        outpdw(MDP_BASE + 0x40a58, 0xc9c9c9);
+        outpdw(MDP_BASE + 0x40a5c, 0xc9c9c9);
+        outpdw(MDP_BASE + 0x40a60, 0xcacaca);
+        outpdw(MDP_BASE + 0x40a64, 0xcbcbcb);
+        outpdw(MDP_BASE + 0x40a68, 0xcbcbcb);
+        outpdw(MDP_BASE + 0x40a6c, 0xcccccc);
+        outpdw(MDP_BASE + 0x40a70, 0xcccccc);
+        outpdw(MDP_BASE + 0x40a74, 0xcdcdcd);
+        outpdw(MDP_BASE + 0x40a78, 0xcecece);
+        outpdw(MDP_BASE + 0x40a7c, 0xcecece);
+        outpdw(MDP_BASE + 0x40a80, 0xcfcfcf);
+        outpdw(MDP_BASE + 0x40a84, 0xcfcfcf);
+        outpdw(MDP_BASE + 0x40a88, 0xd0d0d0);
+        outpdw(MDP_BASE + 0x40a8c, 0xd0d0d0);
+        outpdw(MDP_BASE + 0x40a90, 0xd1d1d1);
+        outpdw(MDP_BASE + 0x40a94, 0xd2d2d2);
+        outpdw(MDP_BASE + 0x40a98, 0xd2d2d2);
+        outpdw(MDP_BASE + 0x40a9c, 0xd3d3d3);
+        outpdw(MDP_BASE + 0x40aa0, 0xd3d3d3);
+        outpdw(MDP_BASE + 0x40aa4, 0xd4d4d4);
+        outpdw(MDP_BASE + 0x40aa8, 0xd4d4d4);
+        outpdw(MDP_BASE + 0x40aac, 0xd5d5d5);
+        outpdw(MDP_BASE + 0x40ab0, 0xd6d6d6);
+        outpdw(MDP_BASE + 0x40ab4, 0xd6d6d6);
+        outpdw(MDP_BASE + 0x40ab8, 0xd7d7d7);
+        outpdw(MDP_BASE + 0x40abc, 0xd7d7d7);
+        outpdw(MDP_BASE + 0x40ac0, 0xd8d8d8);
+        outpdw(MDP_BASE + 0x40ac4, 0xd8d8d8);
+        outpdw(MDP_BASE + 0x40ac8, 0xd9d9d9);
+        outpdw(MDP_BASE + 0x40acc, 0xd9d9d9);
+        outpdw(MDP_BASE + 0x40ad0, 0xdadada);
+        outpdw(MDP_BASE + 0x40ad4, 0xdbdbdb);
+        outpdw(MDP_BASE + 0x40ad8, 0xdbdbdb);
+        outpdw(MDP_BASE + 0x40adc, 0xdcdcdc);
+        outpdw(MDP_BASE + 0x40ae0, 0xdcdcdc);
+        outpdw(MDP_BASE + 0x40ae4, 0xdddddd);
+        outpdw(MDP_BASE + 0x40ae8, 0xdddddd);
+        outpdw(MDP_BASE + 0x40aec, 0xdedede);
+        outpdw(MDP_BASE + 0x40af0, 0xdedede);
+        outpdw(MDP_BASE + 0x40af4, 0xdfdfdf);
+        outpdw(MDP_BASE + 0x40af8, 0xdfdfdf);
+        outpdw(MDP_BASE + 0x40afc, 0xe0e0e0);
+        outpdw(MDP_BASE + 0x40b00, 0xe0e0e0);
+        outpdw(MDP_BASE + 0x40b04, 0xe1e1e1);
+        outpdw(MDP_BASE + 0x40b08, 0xe1e1e1);
+        outpdw(MDP_BASE + 0x40b0c, 0xe2e2e2);
+        outpdw(MDP_BASE + 0x40b10, 0xe3e3e3);
+        outpdw(MDP_BASE + 0x40b14, 0xe3e3e3);
+        outpdw(MDP_BASE + 0x40b18, 0xe4e4e4);
+        outpdw(MDP_BASE + 0x40b1c, 0xe4e4e4);
+        outpdw(MDP_BASE + 0x40b20, 0xe5e5e5);
+        outpdw(MDP_BASE + 0x40b24, 0xe5e5e5);
+        outpdw(MDP_BASE + 0x40b28, 0xe6e6e6);
+        outpdw(MDP_BASE + 0x40b2c, 0xe6e6e6);
+        outpdw(MDP_BASE + 0x40b30, 0xe7e7e7);
+        outpdw(MDP_BASE + 0x40b34, 0xe7e7e7);
+        outpdw(MDP_BASE + 0x40b38, 0xe8e8e8);
+        outpdw(MDP_BASE + 0x40b3c, 0xe8e8e8);
+        outpdw(MDP_BASE + 0x40b40, 0xe9e9e9);
+        outpdw(MDP_BASE + 0x40b44, 0xe9e9e9);
+        outpdw(MDP_BASE + 0x40b48, 0xeaeaea);
+        outpdw(MDP_BASE + 0x40b4c, 0xeaeaea);
+        outpdw(MDP_BASE + 0x40b50, 0xebebeb);
+        outpdw(MDP_BASE + 0x40b54, 0xebebeb);
+        outpdw(MDP_BASE + 0x40b58, 0xececec);
+        outpdw(MDP_BASE + 0x40b5c, 0xececec);
+        outpdw(MDP_BASE + 0x40b60, 0xededed);
+        outpdw(MDP_BASE + 0x40b64, 0xededed);
+        outpdw(MDP_BASE + 0x40b68, 0xeeeeee);
+        outpdw(MDP_BASE + 0x40b6c, 0xeeeeee);
+        outpdw(MDP_BASE + 0x40b70, 0xefefef);
+        outpdw(MDP_BASE + 0x40b74, 0xefefef);
+        outpdw(MDP_BASE + 0x40b78, 0xf0f0f0);
+        outpdw(MDP_BASE + 0x40b7c, 0xf0f0f0);
+        outpdw(MDP_BASE + 0x40b80, 0xf1f1f1);
+        outpdw(MDP_BASE + 0x40b84, 0xf1f1f1);
+        outpdw(MDP_BASE + 0x40b88, 0xf2f2f2);
+        outpdw(MDP_BASE + 0x40b8c, 0xf2f2f2);
+        outpdw(MDP_BASE + 0x40b90, 0xf2f2f2);
+        outpdw(MDP_BASE + 0x40b94, 0xf3f3f3);
+        outpdw(MDP_BASE + 0x40b98, 0xf3f3f3);
+        outpdw(MDP_BASE + 0x40b9c, 0xf4f4f4);
+        outpdw(MDP_BASE + 0x40ba0, 0xf4f4f4);
+        outpdw(MDP_BASE + 0x40ba4, 0xf5f5f5);
+        outpdw(MDP_BASE + 0x40ba8, 0xf5f5f5);
+        outpdw(MDP_BASE + 0x40bac, 0xf6f6f6);
+        outpdw(MDP_BASE + 0x40bb0, 0xf6f6f6);
+        outpdw(MDP_BASE + 0x40bb4, 0xf7f7f7);
+        outpdw(MDP_BASE + 0x40bb8, 0xf7f7f7);
+        outpdw(MDP_BASE + 0x40bbc, 0xf8f8f8);
+        outpdw(MDP_BASE + 0x40bc0, 0xf8f8f8);
+        outpdw(MDP_BASE + 0x40bc4, 0xf9f9f9);
+        outpdw(MDP_BASE + 0x40bc8, 0xf9f9f9);
+        outpdw(MDP_BASE + 0x40bcc, 0xfafafa);
+        outpdw(MDP_BASE + 0x40bd0, 0xfafafa);
+        outpdw(MDP_BASE + 0x40bd4, 0xfafafa);
+        outpdw(MDP_BASE + 0x40bd8, 0xfbfbfb);
+        outpdw(MDP_BASE + 0x40bdc, 0xfbfbfb);
+        outpdw(MDP_BASE + 0x40be0, 0xfcfcfc);
+        outpdw(MDP_BASE + 0x40be4, 0xfcfcfc);
+        outpdw(MDP_BASE + 0x40be8, 0xfdfdfd);
+        outpdw(MDP_BASE + 0x40bec, 0xfdfdfd);
+        outpdw(MDP_BASE + 0x40bf0, 0xfefefe);
+        outpdw(MDP_BASE + 0x40bf4, 0xfefefe);
+        outpdw(MDP_BASE + 0x40bf8, 0xffffff);
+        outpdw(MDP_BASE + 0x40bfc, 0xffffff);
+        outpdw(MDP_BASE + 0x40c00, 0x0);
+        outpdw(MDP_BASE + 0x40c04, 0x0);
+        outpdw(MDP_BASE + 0x40c08, 0x0);
+        outpdw(MDP_BASE + 0x40c0c, 0x0);
+        outpdw(MDP_BASE + 0x40c10, 0x0);
+        outpdw(MDP_BASE + 0x40c14, 0x0);
+        outpdw(MDP_BASE + 0x40c18, 0x0);
+        outpdw(MDP_BASE + 0x40c1c, 0x0);
+        outpdw(MDP_BASE + 0x40c20, 0x0);
+        outpdw(MDP_BASE + 0x40c24, 0x0);
+        outpdw(MDP_BASE + 0x40c28, 0x0);
+        outpdw(MDP_BASE + 0x40c2c, 0x0);
+        outpdw(MDP_BASE + 0x40c30, 0x0);
+        outpdw(MDP_BASE + 0x40c34, 0x0);
+        outpdw(MDP_BASE + 0x40c38, 0x0);
+        outpdw(MDP_BASE + 0x40c3c, 0x0);
+        outpdw(MDP_BASE + 0x40c40, 0x10101);
+        outpdw(MDP_BASE + 0x40c44, 0x10101);
+        outpdw(MDP_BASE + 0x40c48, 0x10101);
+        outpdw(MDP_BASE + 0x40c4c, 0x10101);
+        outpdw(MDP_BASE + 0x40c50, 0x10101);
+        outpdw(MDP_BASE + 0x40c54, 0x10101);
+        outpdw(MDP_BASE + 0x40c58, 0x10101);
+        outpdw(MDP_BASE + 0x40c5c, 0x10101);
+        outpdw(MDP_BASE + 0x40c60, 0x10101);
+        outpdw(MDP_BASE + 0x40c64, 0x10101);
+        outpdw(MDP_BASE + 0x40c68, 0x20202);
+        outpdw(MDP_BASE + 0x40c6c, 0x20202);
+        outpdw(MDP_BASE + 0x40c70, 0x20202);
+        outpdw(MDP_BASE + 0x40c74, 0x20202);
+        outpdw(MDP_BASE + 0x40c78, 0x20202);
+        outpdw(MDP_BASE + 0x40c7c, 0x20202);
+        outpdw(MDP_BASE + 0x40c80, 0x30303);
+        outpdw(MDP_BASE + 0x40c84, 0x30303);
+        outpdw(MDP_BASE + 0x40c88, 0x30303);
+        outpdw(MDP_BASE + 0x40c8c, 0x30303);
+        outpdw(MDP_BASE + 0x40c90, 0x30303);
+        outpdw(MDP_BASE + 0x40c94, 0x40404);
+        outpdw(MDP_BASE + 0x40c98, 0x40404);
+        outpdw(MDP_BASE + 0x40c9c, 0x40404);
+        outpdw(MDP_BASE + 0x40ca0, 0x40404);
+        outpdw(MDP_BASE + 0x40ca4, 0x40404);
+        outpdw(MDP_BASE + 0x40ca8, 0x50505);
+        outpdw(MDP_BASE + 0x40cac, 0x50505);
+        outpdw(MDP_BASE + 0x40cb0, 0x50505);
+        outpdw(MDP_BASE + 0x40cb4, 0x50505);
+        outpdw(MDP_BASE + 0x40cb8, 0x60606);
+        outpdw(MDP_BASE + 0x40cbc, 0x60606);
+        outpdw(MDP_BASE + 0x40cc0, 0x60606);
+        outpdw(MDP_BASE + 0x40cc4, 0x70707);
+        outpdw(MDP_BASE + 0x40cc8, 0x70707);
+        outpdw(MDP_BASE + 0x40ccc, 0x70707);
+        outpdw(MDP_BASE + 0x40cd0, 0x70707);
+        outpdw(MDP_BASE + 0x40cd4, 0x80808);
+        outpdw(MDP_BASE + 0x40cd8, 0x80808);
+        outpdw(MDP_BASE + 0x40cdc, 0x80808);
+        outpdw(MDP_BASE + 0x40ce0, 0x90909);
+        outpdw(MDP_BASE + 0x40ce4, 0x90909);
+        outpdw(MDP_BASE + 0x40ce8, 0xa0a0a);
+        outpdw(MDP_BASE + 0x40cec, 0xa0a0a);
+        outpdw(MDP_BASE + 0x40cf0, 0xa0a0a);
+        outpdw(MDP_BASE + 0x40cf4, 0xb0b0b);
+        outpdw(MDP_BASE + 0x40cf8, 0xb0b0b);
+        outpdw(MDP_BASE + 0x40cfc, 0xb0b0b);
+        outpdw(MDP_BASE + 0x40d00, 0xc0c0c);
+        outpdw(MDP_BASE + 0x40d04, 0xc0c0c);
+        outpdw(MDP_BASE + 0x40d08, 0xd0d0d);
+        outpdw(MDP_BASE + 0x40d0c, 0xd0d0d);
+        outpdw(MDP_BASE + 0x40d10, 0xe0e0e);
+        outpdw(MDP_BASE + 0x40d14, 0xe0e0e);
+        outpdw(MDP_BASE + 0x40d18, 0xe0e0e);
+        outpdw(MDP_BASE + 0x40d1c, 0xf0f0f);
+        outpdw(MDP_BASE + 0x40d20, 0xf0f0f);
+        outpdw(MDP_BASE + 0x40d24, 0x101010);
+        outpdw(MDP_BASE + 0x40d28, 0x101010);
+        outpdw(MDP_BASE + 0x40d2c, 0x111111);
+        outpdw(MDP_BASE + 0x40d30, 0x111111);
+        outpdw(MDP_BASE + 0x40d34, 0x121212);
+        outpdw(MDP_BASE + 0x40d38, 0x121212);
+        outpdw(MDP_BASE + 0x40d3c, 0x131313);
+        outpdw(MDP_BASE + 0x40d40, 0x131313);
+        outpdw(MDP_BASE + 0x40d44, 0x141414);
+        outpdw(MDP_BASE + 0x40d48, 0x151515);
+        outpdw(MDP_BASE + 0x40d4c, 0x151515);
+        outpdw(MDP_BASE + 0x40d50, 0x161616);
+        outpdw(MDP_BASE + 0x40d54, 0x161616);
+        outpdw(MDP_BASE + 0x40d58, 0x171717);
+        outpdw(MDP_BASE + 0x40d5c, 0x171717);
+        outpdw(MDP_BASE + 0x40d60, 0x181818);
+        outpdw(MDP_BASE + 0x40d64, 0x191919);
+        outpdw(MDP_BASE + 0x40d68, 0x191919);
+        outpdw(MDP_BASE + 0x40d6c, 0x1a1a1a);
+        outpdw(MDP_BASE + 0x40d70, 0x1b1b1b);
+        outpdw(MDP_BASE + 0x40d74, 0x1b1b1b);
+        outpdw(MDP_BASE + 0x40d78, 0x1c1c1c);
+        outpdw(MDP_BASE + 0x40d7c, 0x1c1c1c);
+        outpdw(MDP_BASE + 0x40d80, 0x1d1d1d);
+        outpdw(MDP_BASE + 0x40d84, 0x1e1e1e);
+        outpdw(MDP_BASE + 0x40d88, 0x1f1f1f);
+        outpdw(MDP_BASE + 0x40d8c, 0x1f1f1f);
+        outpdw(MDP_BASE + 0x40d90, 0x202020);
+        outpdw(MDP_BASE + 0x40d94, 0x212121);
+        outpdw(MDP_BASE + 0x40d98, 0x212121);
+        outpdw(MDP_BASE + 0x40d9c, 0x222222);
+        outpdw(MDP_BASE + 0x40da0, 0x232323);
+        outpdw(MDP_BASE + 0x40da4, 0x242424);
+        outpdw(MDP_BASE + 0x40da8, 0x242424);
+        outpdw(MDP_BASE + 0x40dac, 0x252525);
+        outpdw(MDP_BASE + 0x40db0, 0x262626);
+        outpdw(MDP_BASE + 0x40db4, 0x272727);
+        outpdw(MDP_BASE + 0x40db8, 0x272727);
+        outpdw(MDP_BASE + 0x40dbc, 0x282828);
+        outpdw(MDP_BASE + 0x40dc0, 0x292929);
+        outpdw(MDP_BASE + 0x40dc4, 0x2a2a2a);
+        outpdw(MDP_BASE + 0x40dc8, 0x2b2b2b);
+        outpdw(MDP_BASE + 0x40dcc, 0x2c2c2c);
+        outpdw(MDP_BASE + 0x40dd0, 0x2c2c2c);
+        outpdw(MDP_BASE + 0x40dd4, 0x2d2d2d);
+        outpdw(MDP_BASE + 0x40dd8, 0x2e2e2e);
+        outpdw(MDP_BASE + 0x40ddc, 0x2f2f2f);
+        outpdw(MDP_BASE + 0x40de0, 0x303030);
+        outpdw(MDP_BASE + 0x40de4, 0x313131);
+        outpdw(MDP_BASE + 0x40de8, 0x323232);
+        outpdw(MDP_BASE + 0x40dec, 0x333333);
+        outpdw(MDP_BASE + 0x40df0, 0x333333);
+        outpdw(MDP_BASE + 0x40df4, 0x343434);
+        outpdw(MDP_BASE + 0x40df8, 0x353535);
+        outpdw(MDP_BASE + 0x40dfc, 0x363636);
+        outpdw(MDP_BASE + 0x40e00, 0x373737);
+        outpdw(MDP_BASE + 0x40e04, 0x383838);
+        outpdw(MDP_BASE + 0x40e08, 0x393939);
+        outpdw(MDP_BASE + 0x40e0c, 0x3a3a3a);
+        outpdw(MDP_BASE + 0x40e10, 0x3b3b3b);
+        outpdw(MDP_BASE + 0x40e14, 0x3c3c3c);
+        outpdw(MDP_BASE + 0x40e18, 0x3d3d3d);
+        outpdw(MDP_BASE + 0x40e1c, 0x3e3e3e);
+        outpdw(MDP_BASE + 0x40e20, 0x3f3f3f);
+        outpdw(MDP_BASE + 0x40e24, 0x404040);
+        outpdw(MDP_BASE + 0x40e28, 0x414141);
+        outpdw(MDP_BASE + 0x40e2c, 0x424242);
+        outpdw(MDP_BASE + 0x40e30, 0x434343);
+        outpdw(MDP_BASE + 0x40e34, 0x444444);
+        outpdw(MDP_BASE + 0x40e38, 0x464646);
+        outpdw(MDP_BASE + 0x40e3c, 0x474747);
+        outpdw(MDP_BASE + 0x40e40, 0x484848);
+        outpdw(MDP_BASE + 0x40e44, 0x494949);
+        outpdw(MDP_BASE + 0x40e48, 0x4a4a4a);
+        outpdw(MDP_BASE + 0x40e4c, 0x4b4b4b);
+        outpdw(MDP_BASE + 0x40e50, 0x4c4c4c);
+        outpdw(MDP_BASE + 0x40e54, 0x4d4d4d);
+        outpdw(MDP_BASE + 0x40e58, 0x4f4f4f);
+        outpdw(MDP_BASE + 0x40e5c, 0x505050);
+        outpdw(MDP_BASE + 0x40e60, 0x515151);
+        outpdw(MDP_BASE + 0x40e64, 0x525252);
+        outpdw(MDP_BASE + 0x40e68, 0x535353);
+        outpdw(MDP_BASE + 0x40e6c, 0x545454);
+        outpdw(MDP_BASE + 0x40e70, 0x565656);
+        outpdw(MDP_BASE + 0x40e74, 0x575757);
+        outpdw(MDP_BASE + 0x40e78, 0x585858);
+        outpdw(MDP_BASE + 0x40e7c, 0x595959);
+        outpdw(MDP_BASE + 0x40e80, 0x5b5b5b);
+        outpdw(MDP_BASE + 0x40e84, 0x5c5c5c);
+        outpdw(MDP_BASE + 0x40e88, 0x5d5d5d);
+        outpdw(MDP_BASE + 0x40e8c, 0x5e5e5e);
+        outpdw(MDP_BASE + 0x40e90, 0x606060);
+        outpdw(MDP_BASE + 0x40e94, 0x616161);
+        outpdw(MDP_BASE + 0x40e98, 0x626262);
+        outpdw(MDP_BASE + 0x40e9c, 0x646464);
+        outpdw(MDP_BASE + 0x40ea0, 0x656565);
+        outpdw(MDP_BASE + 0x40ea4, 0x666666);
+        outpdw(MDP_BASE + 0x40ea8, 0x686868);
+        outpdw(MDP_BASE + 0x40eac, 0x696969);
+        outpdw(MDP_BASE + 0x40eb0, 0x6a6a6a);
+        outpdw(MDP_BASE + 0x40eb4, 0x6c6c6c);
+        outpdw(MDP_BASE + 0x40eb8, 0x6d6d6d);
+        outpdw(MDP_BASE + 0x40ebc, 0x6f6f6f);
+        outpdw(MDP_BASE + 0x40ec0, 0x707070);
+        outpdw(MDP_BASE + 0x40ec4, 0x717171);
+        outpdw(MDP_BASE + 0x40ec8, 0x737373);
+        outpdw(MDP_BASE + 0x40ecc, 0x747474);
+        outpdw(MDP_BASE + 0x40ed0, 0x767676);
+        outpdw(MDP_BASE + 0x40ed4, 0x777777);
+        outpdw(MDP_BASE + 0x40ed8, 0x797979);
+        outpdw(MDP_BASE + 0x40edc, 0x7a7a7a);
+        outpdw(MDP_BASE + 0x40ee0, 0x7c7c7c);
+        outpdw(MDP_BASE + 0x40ee4, 0x7d7d7d);
+        outpdw(MDP_BASE + 0x40ee8, 0x7f7f7f);
+        outpdw(MDP_BASE + 0x40eec, 0x808080);
+        outpdw(MDP_BASE + 0x40ef0, 0x828282);
+        outpdw(MDP_BASE + 0x40ef4, 0x838383);
+        outpdw(MDP_BASE + 0x40ef8, 0x858585);
+        outpdw(MDP_BASE + 0x40efc, 0x868686);
+        outpdw(MDP_BASE + 0x40f00, 0x888888);
+        outpdw(MDP_BASE + 0x40f04, 0x898989);
+        outpdw(MDP_BASE + 0x40f08, 0x8b8b8b);
+        outpdw(MDP_BASE + 0x40f0c, 0x8d8d8d);
+        outpdw(MDP_BASE + 0x40f10, 0x8e8e8e);
+        outpdw(MDP_BASE + 0x40f14, 0x909090);
+        outpdw(MDP_BASE + 0x40f18, 0x919191);
+        outpdw(MDP_BASE + 0x40f1c, 0x939393);
+        outpdw(MDP_BASE + 0x40f20, 0x959595);
+        outpdw(MDP_BASE + 0x40f24, 0x969696);
+        outpdw(MDP_BASE + 0x40f28, 0x989898);
+        outpdw(MDP_BASE + 0x40f2c, 0x9a9a9a);
+        outpdw(MDP_BASE + 0x40f30, 0x9b9b9b);
+        outpdw(MDP_BASE + 0x40f34, 0x9d9d9d);
+        outpdw(MDP_BASE + 0x40f38, 0x9f9f9f);
+        outpdw(MDP_BASE + 0x40f3c, 0xa1a1a1);
+        outpdw(MDP_BASE + 0x40f40, 0xa2a2a2);
+        outpdw(MDP_BASE + 0x40f44, 0xa4a4a4);
+        outpdw(MDP_BASE + 0x40f48, 0xa6a6a6);
+        outpdw(MDP_BASE + 0x40f4c, 0xa7a7a7);
+        outpdw(MDP_BASE + 0x40f50, 0xa9a9a9);
+        outpdw(MDP_BASE + 0x40f54, 0xababab);
+        outpdw(MDP_BASE + 0x40f58, 0xadadad);
+        outpdw(MDP_BASE + 0x40f5c, 0xafafaf);
+        outpdw(MDP_BASE + 0x40f60, 0xb0b0b0);
+        outpdw(MDP_BASE + 0x40f64, 0xb2b2b2);
+        outpdw(MDP_BASE + 0x40f68, 0xb4b4b4);
+        outpdw(MDP_BASE + 0x40f6c, 0xb6b6b6);
+        outpdw(MDP_BASE + 0x40f70, 0xb8b8b8);
+        outpdw(MDP_BASE + 0x40f74, 0xbababa);
+        outpdw(MDP_BASE + 0x40f78, 0xbbbbbb);
+        outpdw(MDP_BASE + 0x40f7c, 0xbdbdbd);
+        outpdw(MDP_BASE + 0x40f80, 0xbfbfbf);
+        outpdw(MDP_BASE + 0x40f84, 0xc1c1c1);
+        outpdw(MDP_BASE + 0x40f88, 0xc3c3c3);
+        outpdw(MDP_BASE + 0x40f8c, 0xc5c5c5);
+        outpdw(MDP_BASE + 0x40f90, 0xc7c7c7);
+        outpdw(MDP_BASE + 0x40f94, 0xc9c9c9);
+        outpdw(MDP_BASE + 0x40f98, 0xcbcbcb);
+        outpdw(MDP_BASE + 0x40f9c, 0xcdcdcd);
+        outpdw(MDP_BASE + 0x40fa0, 0xcfcfcf);
+        outpdw(MDP_BASE + 0x40fa4, 0xd1d1d1);
+        outpdw(MDP_BASE + 0x40fa8, 0xd3d3d3);
+        outpdw(MDP_BASE + 0x40fac, 0xd5d5d5);
+        outpdw(MDP_BASE + 0x40fb0, 0xd7d7d7);
+        outpdw(MDP_BASE + 0x40fb4, 0xd9d9d9);
+        outpdw(MDP_BASE + 0x40fb8, 0xdbdbdb);
+        outpdw(MDP_BASE + 0x40fbc, 0xdddddd);
+        outpdw(MDP_BASE + 0x40fc0, 0xdfdfdf);
+        outpdw(MDP_BASE + 0x40fc4, 0xe1e1e1);
+        outpdw(MDP_BASE + 0x40fc8, 0xe3e3e3);
+        outpdw(MDP_BASE + 0x40fcc, 0xe5e5e5);
+        outpdw(MDP_BASE + 0x40fd0, 0xe7e7e7);
+        outpdw(MDP_BASE + 0x40fd4, 0xe9e9e9);
+        outpdw(MDP_BASE + 0x40fd8, 0xebebeb);
+        outpdw(MDP_BASE + 0x40fdc, 0xeeeeee);
+        outpdw(MDP_BASE + 0x40fe0, 0xf0f0f0);
+        outpdw(MDP_BASE + 0x40fe4, 0xf2f2f2);
+        outpdw(MDP_BASE + 0x40fe8, 0xf4f4f4);
+        outpdw(MDP_BASE + 0x40fec, 0xf6f6f6);
+        outpdw(MDP_BASE + 0x40ff0, 0xf8f8f8);
+        outpdw(MDP_BASE + 0x40ff4, 0xfbfbfb);
+        outpdw(MDP_BASE + 0x40ff8, 0xfdfdfd);
+        outpdw(MDP_BASE + 0x40ffc, 0xffffff);
+}
+
+#define   IRQ_EN_1__MDP_IRQ___M    0x00000800
+
+void mdp_hw_init(void)
+{
+        int i;
+
+        /* MDP cmd block enable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+        /* debug interface write access */
+        outpdw(MDP_BASE + 0x60, 1);
+
+        outp32(MDP_INTR_ENABLE, MDP_ANY_INTR_MASK);
+        outp32(MDP_EBI2_PORTMAP_MODE, 0x3);
+        outpdw(MDP_CMD_DEBUG_ACCESS_BASE + 0x01f8, 0x0);
+        outpdw(MDP_CMD_DEBUG_ACCESS_BASE + 0x01fc, 0x0);
+        outpdw(MDP_BASE + 0x60, 0x1);
+        mdp_load_lut_param();
+
+        /*
+         * clear up unused fg/main registers
+         */
+        /* comp.plane 2&3 ystride */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0120, 0x0);
+        /* unpacked pattern */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x012c, 0x0);
+        /* unpacked pattern */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0130, 0x0);
+        /* unpacked pattern */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0134, 0x0);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0158, 0x0);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x15c, 0x0);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0160, 0x0);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0170, 0x0);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0174, 0x0);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x017c, 0x0);
+
+        /* comp.plane 2 */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0114, 0x0);
+        /* comp.plane 3 */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0118, 0x0);
+
+        /* clear up unused bg registers */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01c8, 0);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01d0, 0);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01dc, 0);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01e0, 0);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01e4, 0);
+
+#ifndef CONFIG_FB_MSM_MDP22
+        MDP_OUTP(MDP_BASE + 0xE0000, 0);
+        MDP_OUTP(MDP_BASE + 0x100, 0xffffffff);
+        MDP_OUTP(MDP_BASE + 0x90070, 0);
+        MDP_OUTP(MDP_BASE + 0x94010, 1);
+        MDP_OUTP(MDP_BASE + 0x9401c, 2);
+#endif
+
+        /*
+         * limit vector
+         * pre gets applied before color matrix conversion
+         * post is after ccs
+         */
+        writel(mdp_plv[0], MDP_CSC_PRE_LV1n(0));
+        writel(mdp_plv[1], MDP_CSC_PRE_LV1n(1));
+        writel(mdp_plv[2], MDP_CSC_PRE_LV1n(2));
+        writel(mdp_plv[3], MDP_CSC_PRE_LV1n(3));
+
+#ifdef CONFIG_FB_MSM_MDP31
+        writel(mdp_plv[2], MDP_CSC_PRE_LV1n(4));
+        writel(mdp_plv[3], MDP_CSC_PRE_LV1n(5));
+
+        writel(0, MDP_CSC_POST_LV1n(0));
+        writel(0xff, MDP_CSC_POST_LV1n(1));
+        writel(0, MDP_CSC_POST_LV1n(2));
+        writel(0xff, MDP_CSC_POST_LV1n(3));
+        writel(0, MDP_CSC_POST_LV1n(4));
+        writel(0xff, MDP_CSC_POST_LV1n(5));
+
+        writel(0, MDP_CSC_PRE_LV2n(0));
+        writel(0xff, MDP_CSC_PRE_LV2n(1));
+        writel(0, MDP_CSC_PRE_LV2n(2));
+        writel(0xff, MDP_CSC_PRE_LV2n(3));
+        writel(0, MDP_CSC_PRE_LV2n(4));
+        writel(0xff, MDP_CSC_PRE_LV2n(5));
+
+        writel(mdp_plv[0], MDP_CSC_POST_LV2n(0));
+        writel(mdp_plv[1], MDP_CSC_POST_LV2n(1));
+        writel(mdp_plv[2], MDP_CSC_POST_LV2n(2));
+        writel(mdp_plv[3], MDP_CSC_POST_LV2n(3));
+        writel(mdp_plv[2], MDP_CSC_POST_LV2n(4));
+        writel(mdp_plv[3], MDP_CSC_POST_LV2n(5));
+#endif
+
+        /* primary forward matrix */
+        for (i = 0; i < MDP_CCS_SIZE; i++)
+                writel(mdp_ccs_rgb2yuv.ccs[i], MDP_CSC_PFMVn(i));
+
+#ifdef CONFIG_FB_MSM_MDP31
+        for (i = 0; i < MDP_BV_SIZE; i++)
+                writel(mdp_ccs_rgb2yuv.bv[i], MDP_CSC_POST_BV2n(i));
+
+        writel(0, MDP_CSC_PRE_BV2n(0));
+        writel(0, MDP_CSC_PRE_BV2n(1));
+        writel(0, MDP_CSC_PRE_BV2n(2));
+#endif
+        /* primary reverse matrix */
+        for (i = 0; i < MDP_CCS_SIZE; i++)
+                writel(mdp_ccs_yuv2rgb.ccs[i], MDP_CSC_PRMVn(i));
+
+        for (i = 0; i < MDP_BV_SIZE; i++)
+                writel(mdp_ccs_yuv2rgb.bv[i], MDP_CSC_PRE_BV1n(i));
+
+#ifdef CONFIG_FB_MSM_MDP31
+        writel(0, MDP_CSC_POST_BV1n(0));
+        writel(0, MDP_CSC_POST_BV1n(1));
+        writel(0, MDP_CSC_POST_BV1n(2));
+
+        outpdw(MDP_BASE + 0x30010, 0x03e0);
+        outpdw(MDP_BASE + 0x30014, 0x0360);
+        outpdw(MDP_BASE + 0x30018, 0x0120);
+        outpdw(MDP_BASE + 0x3001c, 0x0140);
+#endif
+        mdp_init_scale_table();
+
+#ifndef CONFIG_FB_MSM_MDP31
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0104,
+                 ((16 << 6) << 16) | (16) << 6);
+#endif
+
+        /* MDP cmd block disable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+}
\ No newline at end of file
diff --git a/drivers/staging/msm/mdp_ppp.c b/drivers/staging/msm/mdp_ppp.c
new file mode 100644
index 000000000000..c35a6aebca14
--- /dev/null
+++ b/drivers/staging/msm/mdp_ppp.c
@@ -0,0 +1,1502 @@
+/* drivers/video/msm/src/drv/mdp/mdp_ppp.c
+ *
+ * Copyright (C) 2007 Google Incorporated
+ * Copyright (c) 2008-2010, Code Aurora Forum. All rights reserved.
+ *
+ * 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.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/fb.h>
+#include <msm_mdp.h>
+#include <linux/file.h>
+#include <linux/major.h>
+
+#include "linux/proc_fs.h"
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+
+#define MDP_IS_IMGTYPE_BAD(x) (((x) >= MDP_IMGTYPE_LIMIT) && \
+                                (((x) < MDP_IMGTYPE2_START) || \
+                                 ((x) >= MDP_IMGTYPE_LIMIT2)))
+
+static uint32_t bytes_per_pixel[] = {
+        [MDP_RGB_565] = 2,
+        [MDP_RGB_888] = 3,
+        [MDP_XRGB_8888] = 4,
+        [MDP_ARGB_8888] = 4,
+        [MDP_RGBA_8888] = 4,
+        [MDP_BGRA_8888] = 4,
+        [MDP_Y_CBCR_H2V1] = 1,
+        [MDP_Y_CBCR_H2V2] = 1,
+        [MDP_Y_CRCB_H2V1] = 1,
+        [MDP_Y_CRCB_H2V2] = 1,
+        [MDP_YCRYCB_H2V1] = 2,
+        [MDP_BGR_565] = 2
+};
+
+extern uint32 mdp_plv[];
+extern struct semaphore mdp_ppp_mutex;
+
+uint32_t mdp_get_bytes_per_pixel(uint32_t format)
+{
+        uint32_t bpp = 0;
+        if (format < ARRAY_SIZE(bytes_per_pixel))
+                bpp = bytes_per_pixel[format];
+
+        BUG_ON(!bpp);
+        return bpp;
+}
+
+static uint32 mdp_conv_matx_rgb2yuv(uint32 input_pixel,
+                                    uint16 *matrix_and_bias_vector,
+                                    uint32 *clamp_vector,
+                                    uint32 *look_up_table)
+{
+        uint8 input_C2, input_C0, input_C1;
+        uint32 output;
+        int32 comp_C2, comp_C1, comp_C0, temp;
+        int32 temp1, temp2, temp3;
+        int32 matrix[9];
+        int32 bias_vector[3];
+        int32 Y_low_limit, Y_high_limit, C_low_limit, C_high_limit;
+        int32 i;
+        uint32 _is_lookup_table_enabled;
+
+        input_C2 = (input_pixel >> 16) & 0xFF;
+        input_C1 = (input_pixel >> 8) & 0xFF;
+        input_C0 = (input_pixel >> 0) & 0xFF;
+
+        comp_C0 = input_C0;
+        comp_C1 = input_C1;
+        comp_C2 = input_C2;
+
+        for (i = 0; i < 9; i++)
+                matrix[i] =
+                    ((int32) (((int32) matrix_and_bias_vector[i]) << 20)) >> 20;
+
+        bias_vector[0] = (int32) (matrix_and_bias_vector[9] & 0xFF);
+        bias_vector[1] = (int32) (matrix_and_bias_vector[10] & 0xFF);
+        bias_vector[2] = (int32) (matrix_and_bias_vector[11] & 0xFF);
+
+        Y_low_limit = (int32) clamp_vector[0];
+        Y_high_limit = (int32) clamp_vector[1];
+        C_low_limit = (int32) clamp_vector[2];
+        C_high_limit = (int32) clamp_vector[3];
+
+        if (look_up_table == 0) /* check for NULL point */
+                _is_lookup_table_enabled = 0;
+        else
+                _is_lookup_table_enabled = 1;
+
+        if (_is_lookup_table_enabled == 1) {
+                comp_C2 = (look_up_table[comp_C2] >> 16) & 0xFF;
+                comp_C1 = (look_up_table[comp_C1] >> 8) & 0xFF;
+                comp_C0 = (look_up_table[comp_C0] >> 0) & 0xFF;
+        }
+        /*
+         * Color Conversion
+         * reorder input colors
+         */
+        temp = comp_C2;
+        comp_C2 = comp_C1;
+        comp_C1 = comp_C0;
+        comp_C0 = temp;
+
+        /* matrix multiplication */
+        temp1 = comp_C0 * matrix[0] + comp_C1 * matrix[1] + comp_C2 * matrix[2];
+        temp2 = comp_C0 * matrix[3] + comp_C1 * matrix[4] + comp_C2 * matrix[5];
+        temp3 = comp_C0 * matrix[6] + comp_C1 * matrix[7] + comp_C2 * matrix[8];
+
+        comp_C0 = temp1 + 0x100;
+        comp_C1 = temp2 + 0x100;
+        comp_C2 = temp3 + 0x100;
+
+        /* take interger part */
+        comp_C0 >>= 9;
+        comp_C1 >>= 9;
+        comp_C2 >>= 9;
+
+        /* post bias (+) */
+        comp_C0 += bias_vector[0];
+        comp_C1 += bias_vector[1];
+        comp_C2 += bias_vector[2];
+
+        /* limit pixel to 8-bit */
+        if (comp_C0 < 0)
+                comp_C0 = 0;
+
+        if (comp_C0 > 255)
+                comp_C0 = 255;
+
+        if (comp_C1 < 0)
+                comp_C1 = 0;
+
+        if (comp_C1 > 255)
+                comp_C1 = 255;
+
+        if (comp_C2 < 0)
+                comp_C2 = 0;
+
+        if (comp_C2 > 255)
+                comp_C2 = 255;
+
+        /* clamp */
+        if (comp_C0 < Y_low_limit)
+                comp_C0 = Y_low_limit;
+
+        if (comp_C0 > Y_high_limit)
+                comp_C0 = Y_high_limit;
+
+        if (comp_C1 < C_low_limit)
+                comp_C1 = C_low_limit;
+
+        if (comp_C1 > C_high_limit)
+                comp_C1 = C_high_limit;
+
+        if (comp_C2 < C_low_limit)
+                comp_C2 = C_low_limit;
+
+        if (comp_C2 > C_high_limit)
+                comp_C2 = C_high_limit;
+
+        output = (comp_C2 << 16) | (comp_C1 << 8) | comp_C0;
+        return output;
+}
+
+uint32 mdp_conv_matx_yuv2rgb(uint32 input_pixel,
+                             uint16 *matrix_and_bias_vector,
+                             uint32 *clamp_vector, uint32 *look_up_table)
+{
+        uint8 input_C2, input_C0, input_C1;
+        uint32 output;
+        int32 comp_C2, comp_C1, comp_C0, temp;
+        int32 temp1, temp2, temp3;
+        int32 matrix[9];
+        int32 bias_vector[3];
+        int32 Y_low_limit, Y_high_limit, C_low_limit, C_high_limit;
+        int32 i;
+        uint32 _is_lookup_table_enabled;
+
+        input_C2 = (input_pixel >> 16) & 0xFF;
+        input_C1 = (input_pixel >> 8) & 0xFF;
+        input_C0 = (input_pixel >> 0) & 0xFF;
+
+        comp_C0 = input_C0;
+        comp_C1 = input_C1;
+        comp_C2 = input_C2;
+
+        for (i = 0; i < 9; i++)
+                matrix[i] =
+                    ((int32) (((int32) matrix_and_bias_vector[i]) << 20)) >> 20;
+
+        bias_vector[0] = (int32) (matrix_and_bias_vector[9] & 0xFF);
+        bias_vector[1] = (int32) (matrix_and_bias_vector[10] & 0xFF);
+        bias_vector[2] = (int32) (matrix_and_bias_vector[11] & 0xFF);
+
+        Y_low_limit = (int32) clamp_vector[0];
+        Y_high_limit = (int32) clamp_vector[1];
+        C_low_limit = (int32) clamp_vector[2];
+        C_high_limit = (int32) clamp_vector[3];
+
+        if (look_up_table == 0) /* check for NULL point */
+                _is_lookup_table_enabled = 0;
+        else
+                _is_lookup_table_enabled = 1;
+
+        /* clamp */
+        if (comp_C0 < Y_low_limit)
+                comp_C0 = Y_low_limit;
+
+        if (comp_C0 > Y_high_limit)
+                comp_C0 = Y_high_limit;
+
+        if (comp_C1 < C_low_limit)
+                comp_C1 = C_low_limit;
+
+        if (comp_C1 > C_high_limit)
+                comp_C1 = C_high_limit;
+
+        if (comp_C2 < C_low_limit)
+                comp_C2 = C_low_limit;
+
+        if (comp_C2 > C_high_limit)
+                comp_C2 = C_high_limit;
+
+        /*
+         * Color Conversion
+         * pre bias (-)
+         */
+        comp_C0 -= bias_vector[0];
+        comp_C1 -= bias_vector[1];
+        comp_C2 -= bias_vector[2];
+
+        /* matrix multiplication */
+        temp1 = comp_C0 * matrix[0] + comp_C1 * matrix[1] + comp_C2 * matrix[2];
+        temp2 = comp_C0 * matrix[3] + comp_C1 * matrix[4] + comp_C2 * matrix[5];
+        temp3 = comp_C0 * matrix[6] + comp_C1 * matrix[7] + comp_C2 * matrix[8];
+
+        comp_C0 = temp1 + 0x100;
+        comp_C1 = temp2 + 0x100;
+        comp_C2 = temp3 + 0x100;
+
+        /* take interger part */
+        comp_C0 >>= 9;
+        comp_C1 >>= 9;
+        comp_C2 >>= 9;
+
+        /* reorder output colors */
+        temp = comp_C0;
+        comp_C0 = comp_C1;
+        comp_C1 = comp_C2;
+        comp_C2 = temp;
+
+        /* limit pixel to 8-bit */
+        if (comp_C0 < 0)
+                comp_C0 = 0;
+
+        if (comp_C0 > 255)
+                comp_C0 = 255;
+
+        if (comp_C1 < 0)
+                comp_C1 = 0;
+
+        if (comp_C1 > 255)
+                comp_C1 = 255;
+
+        if (comp_C2 < 0)
+                comp_C2 = 0;
+
+        if (comp_C2 > 255)
+                comp_C2 = 255;
+
+        /* Look-up table */
+        if (_is_lookup_table_enabled == 1) {
+                comp_C2 = (look_up_table[comp_C2] >> 16) & 0xFF;
+                comp_C1 = (look_up_table[comp_C1] >> 8) & 0xFF;
+                comp_C0 = (look_up_table[comp_C0] >> 0) & 0xFF;
+        }
+
+        output = (comp_C2 << 16) | (comp_C1 << 8) | comp_C0;
+        return output;
+}
+
+static uint32 mdp_calc_tpval(MDPIMG *mdpImg)
+{
+        uint32 tpVal;
+        uint8 plane_tp;
+
+        tpVal = 0;
+        if ((mdpImg->imgType == MDP_RGB_565)
+            || (mdpImg->imgType == MDP_BGR_565)) {
+                /*
+                 * transparent color conversion into 24 bpp
+                 *
+                 * C2R_8BIT
+                 * left shift the entire bit and or it with the upper most bits
+                 */
+                plane_tp = (uint8) ((mdpImg->tpVal & 0xF800) >> 11);
+                tpVal |= ((plane_tp << 3) | ((plane_tp & 0x1C) >> 2)) << 16;
+
+                /* C1B_8BIT */
+                plane_tp = (uint8) (mdpImg->tpVal & 0x1F);
+                tpVal |= ((plane_tp << 3) | ((plane_tp & 0x1C) >> 2)) << 8;
+
+                /* C0G_8BIT */
+                plane_tp = (uint8) ((mdpImg->tpVal & 0x7E0) >> 5);
+                tpVal |= ((plane_tp << 2) | ((plane_tp & 0x30) >> 4));
+        } else {
+                /* 24bit RGB to RBG conversion */
+
+                tpVal = (mdpImg->tpVal & 0xFF00) >> 8;
+                tpVal |= (mdpImg->tpVal & 0xFF) << 8;
+                tpVal |= (mdpImg->tpVal & 0xFF0000);
+        }
+
+        return tpVal;
+}
+
+static uint8 *mdp_get_chroma_addr(MDPIBUF *iBuf)
+{
+        uint8 *dest1;
+
+        dest1 = NULL;
+        switch (iBuf->ibuf_type) {
+        case MDP_Y_CBCR_H2V2:
+        case MDP_Y_CRCB_H2V2:
+        case MDP_Y_CBCR_H2V1:
+        case MDP_Y_CRCB_H2V1:
+                dest1 = (uint8 *) iBuf->buf;
+                dest1 += iBuf->ibuf_width * iBuf->ibuf_height * iBuf->bpp;
+                break;
+
+        default:
+                break;
+        }
+
+        return dest1;
+}
+
+static void mdp_ppp_setbg(MDPIBUF *iBuf)
+{
+        uint8 *bg0_addr;
+        uint8 *bg1_addr;
+        uint32 bg0_ystride, bg1_ystride;
+        uint32 ppp_src_cfg_reg, unpack_pattern;
+        int v_slice, h_slice;
+
+        v_slice = h_slice = 1;
+        bg0_addr = (uint8 *) iBuf->buf;
+        bg1_addr = mdp_get_chroma_addr(iBuf);
+
+        bg0_ystride = iBuf->ibuf_width * iBuf->bpp;
+        bg1_ystride = iBuf->ibuf_width * iBuf->bpp;
+
+        switch (iBuf->ibuf_type) {
+        case MDP_BGR_565:
+        case MDP_RGB_565:
+                /* 888 = 3bytes
+                 * RGB = 3Components
+                 * RGB interleaved
+                 */
+                ppp_src_cfg_reg = PPP_SRC_C2R_5BITS | PPP_SRC_C0G_6BITS |
+                        PPP_SRC_C1B_5BITS | PPP_SRC_BPP_INTERLVD_2BYTES |
+                        PPP_SRC_INTERLVD_3COMPONENTS | PPP_SRC_UNPACK_TIGHT |
+                        PPP_SRC_UNPACK_ALIGN_LSB |
+                        PPP_SRC_FETCH_PLANES_INTERLVD;
+
+                if (iBuf->ibuf_type == MDP_RGB_565)
+                        unpack_pattern =
+                            MDP_GET_PACK_PATTERN(0, CLR_R, CLR_G, CLR_B, 8);
+                else
+                        unpack_pattern =
+                            MDP_GET_PACK_PATTERN(0, CLR_B, CLR_G, CLR_R, 8);
+                break;
+
+        case MDP_RGB_888:
+                /*
+                 * 888 = 3bytes
+                 * RGB = 3Components
+                 * RGB interleaved
+                 */
+                ppp_src_cfg_reg = PPP_SRC_C2R_8BITS | PPP_SRC_C0G_8BITS |
+                PPP_SRC_C1B_8BITS | PPP_SRC_BPP_INTERLVD_3BYTES |
+                PPP_SRC_INTERLVD_3COMPONENTS | PPP_SRC_UNPACK_TIGHT |
+                PPP_SRC_UNPACK_ALIGN_LSB | PPP_SRC_FETCH_PLANES_INTERLVD;
+
+                unpack_pattern =
+                    MDP_GET_PACK_PATTERN(0, CLR_R, CLR_G, CLR_B, 8);
+                break;
+
+        case MDP_BGRA_8888:
+        case MDP_RGBA_8888:
+        case MDP_ARGB_8888:
+        case MDP_XRGB_8888:
+                /*
+                 * 8888 = 4bytes
+                 * ARGB = 4Components
+                 * ARGB interleaved
+                 */
+                ppp_src_cfg_reg = PPP_SRC_C2R_8BITS | PPP_SRC_C0G_8BITS |
+                PPP_SRC_C1B_8BITS | PPP_SRC_C3A_8BITS | PPP_SRC_C3_ALPHA_EN |
+                PPP_SRC_BPP_INTERLVD_4BYTES | PPP_SRC_INTERLVD_4COMPONENTS |
+                PPP_SRC_UNPACK_TIGHT | PPP_SRC_UNPACK_ALIGN_LSB |
+                PPP_SRC_FETCH_PLANES_INTERLVD;
+
+                if (iBuf->ibuf_type == MDP_BGRA_8888)
+                        unpack_pattern =
+                            MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B,
+                                                 8);
+                else if (iBuf->ibuf_type == MDP_RGBA_8888)
+                        unpack_pattern =
+                            MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_B, CLR_G, CLR_R,
+                                                 8);
+                else
+                        unpack_pattern =
+                            MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B,
+                                                 8);
+                break;
+
+        case MDP_Y_CBCR_H2V2:
+        case MDP_Y_CRCB_H2V2:
+                ppp_src_cfg_reg = PPP_SRC_C2R_8BITS |
+                    PPP_SRC_C0G_8BITS |
+                    PPP_SRC_C1B_8BITS |
+                    PPP_SRC_C3A_8BITS |
+                    PPP_SRC_BPP_INTERLVD_2BYTES |
+                    PPP_SRC_INTERLVD_2COMPONENTS |
+                    PPP_SRC_UNPACK_TIGHT |
+                    PPP_SRC_UNPACK_ALIGN_LSB | PPP_SRC_FETCH_PLANES_PSEUDOPLNR;
+
+                if (iBuf->ibuf_type == MDP_Y_CBCR_H2V1)
+                        unpack_pattern =
+                            MDP_GET_PACK_PATTERN(0, 0, CLR_CB, CLR_CR, 8);
+                else
+                        unpack_pattern =
+                            MDP_GET_PACK_PATTERN(0, 0, CLR_CR, CLR_CB, 8);
+                v_slice = h_slice = 2;
+                break;
+
+        case MDP_YCRYCB_H2V1:
+                ppp_src_cfg_reg = PPP_SRC_C2R_8BITS |
+                    PPP_SRC_C0G_8BITS |
+                    PPP_SRC_C1B_8BITS |
+                    PPP_SRC_C3A_8BITS |
+                    PPP_SRC_BPP_INTERLVD_2BYTES |
+                    PPP_SRC_INTERLVD_4COMPONENTS |
+                    PPP_SRC_UNPACK_TIGHT | PPP_SRC_UNPACK_ALIGN_LSB;
+
+                unpack_pattern =
+                    MDP_GET_PACK_PATTERN(CLR_Y, CLR_CR, CLR_Y, CLR_CB, 8);
+                h_slice = 2;
+                break;
+
+        case MDP_Y_CBCR_H2V1:
+        case MDP_Y_CRCB_H2V1:
+                ppp_src_cfg_reg = PPP_SRC_C2R_8BITS |
+                    PPP_SRC_C0G_8BITS |
+                    PPP_SRC_C1B_8BITS |
+                    PPP_SRC_C3A_8BITS |
+                    PPP_SRC_BPP_INTERLVD_2BYTES |
+                    PPP_SRC_INTERLVD_2COMPONENTS |
+                    PPP_SRC_UNPACK_TIGHT |
+                    PPP_SRC_UNPACK_ALIGN_LSB | PPP_SRC_FETCH_PLANES_PSEUDOPLNR;
+
+                if (iBuf->ibuf_type == MDP_Y_CBCR_H2V1)
+                        unpack_pattern =
+                            MDP_GET_PACK_PATTERN(0, 0, CLR_CB, CLR_CR, 8);
+                else
+                        unpack_pattern =
+                            MDP_GET_PACK_PATTERN(0, 0, CLR_CR, CLR_CB, 8);
+                h_slice = 2;
+                break;
+
+        default:
+                return;
+        }
+
+        /* starting input address adjustment */
+        mdp_adjust_start_addr(&bg0_addr, &bg1_addr, v_slice, h_slice,
+                              iBuf->roi.lcd_x, iBuf->roi.lcd_y,
+                              iBuf->ibuf_width, iBuf->ibuf_height, iBuf->bpp,
+                              iBuf, 1);
+
+        /*
+         * 0x01c0: background plane 0 addr
+         * 0x01c4: background plane 1 addr
+         * 0x01c8: background plane 2 addr
+         * 0x01cc: bg y stride for plane 0 and 1
+         * 0x01d0: bg y stride for plane 2
+         * 0x01d4: bg src PPP config
+         * 0x01d8: unpack pattern
+         */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01c0, bg0_addr);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01c4, bg1_addr);
+
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01cc,
+                 (bg1_ystride << 16) | bg0_ystride);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01d4, ppp_src_cfg_reg);
+
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01d8, unpack_pattern);
+}
+
+#define IS_PSEUDOPLNR(img) ((img == MDP_Y_CRCB_H2V2) | \
+                                (img == MDP_Y_CBCR_H2V2) | \
+                                (img == MDP_Y_CRCB_H2V1) | \
+                                (img == MDP_Y_CBCR_H2V1))
+
+#define IMG_LEN(rect_h, w, rect_w, bpp) (((rect_h) * w) * bpp)
+
+#define Y_TO_CRCB_RATIO(format) \
+        ((format == MDP_Y_CBCR_H2V2 || format == MDP_Y_CRCB_H2V2) ?  2 :\
+        (format == MDP_Y_CBCR_H2V1 || format == MDP_Y_CRCB_H2V1) ?  1 : 1)
+
+static void get_len(struct mdp_img *img, struct mdp_rect *rect, uint32_t bpp,
+                        uint32_t *len0, uint32_t *len1)
+{
+        *len0 = IMG_LEN(rect->h, img->width, rect->w, bpp);
+        if (IS_PSEUDOPLNR(img->format))
+                *len1 = *len0/Y_TO_CRCB_RATIO(img->format);
+        else
+                *len1 = 0;
+}
+
+static void flush_imgs(struct mdp_blit_req *req, int src_bpp, int dst_bpp,
+                        struct file *p_src_file, struct file *p_dst_file)
+{
+#ifdef CONFIG_ANDROID_PMEM
+        uint32_t src0_len, src1_len, dst0_len, dst1_len;
+
+        /* flush src images to memory before dma to mdp */
+        get_len(&req->src, &req->src_rect, src_bpp,
+        &src0_len, &src1_len);
+
+        flush_pmem_file(p_src_file,
+        req->src.offset, src0_len);
+
+        if (IS_PSEUDOPLNR(req->src.format))
+                flush_pmem_file(p_src_file,
+                        req->src.offset + src0_len, src1_len);
+
+        get_len(&req->dst, &req->dst_rect, dst_bpp, &dst0_len, &dst1_len);
+        flush_pmem_file(p_dst_file, req->dst.offset, dst0_len);
+
+        if (IS_PSEUDOPLNR(req->dst.format))
+                flush_pmem_file(p_dst_file,
+                        req->dst.offset + dst0_len, dst1_len);
+#endif
+}
+
+static void mdp_start_ppp(struct msm_fb_data_type *mfd, MDPIBUF *iBuf,
+struct mdp_blit_req *req, struct file *p_src_file, struct file *p_dst_file)
+{
+        uint8 *src0, *src1;
+        uint8 *dest0, *dest1;
+        uint16 inpBpp;
+        uint32 dest0_ystride;
+        uint32 src_width;
+        uint32 src_height;
+        uint32 src0_ystride;
+        uint32 dst_roi_width;
+        uint32 dst_roi_height;
+        uint32 ppp_src_cfg_reg, ppp_operation_reg, ppp_dst_cfg_reg;
+        uint32 alpha, tpVal;
+        uint32 packPattern;
+        uint32 dst_packPattern;
+        boolean inputRGB, outputRGB, pseudoplanr_output;
+        int sv_slice, sh_slice;
+        int dv_slice, dh_slice;
+        boolean perPixelAlpha = FALSE;
+        boolean ppp_lookUp_enable = FALSE;
+
+        sv_slice = sh_slice = dv_slice = dh_slice = 1;
+        alpha = tpVal = 0;
+        src_width = iBuf->mdpImg.width;
+        src_height = iBuf->roi.y + iBuf->roi.height;
+        src1 = NULL;
+        dest1 = NULL;
+
+        inputRGB = outputRGB = TRUE;
+        pseudoplanr_output = FALSE;
+        ppp_operation_reg = 0;
+        ppp_dst_cfg_reg = 0;
+        ppp_src_cfg_reg = 0;
+
+        /* Wait for the pipe to clear */
+        do { } while (mdp_ppp_pipe_wait() <= 0);
+
+        /*
+         * destination config
+         */
+        switch (iBuf->ibuf_type) {
+        case MDP_RGB_888:
+                dst_packPattern =
+                    MDP_GET_PACK_PATTERN(0, CLR_R, CLR_G, CLR_B, 8);
+                ppp_dst_cfg_reg =
+                    PPP_DST_C0G_8BIT | PPP_DST_C1B_8BIT | PPP_DST_C2R_8BIT |
+                    PPP_DST_PACKET_CNT_INTERLVD_3ELEM | PPP_DST_PACK_TIGHT |
+                    PPP_DST_PACK_ALIGN_LSB | PPP_DST_OUT_SEL_AXI |
+                    PPP_DST_BPP_3BYTES | PPP_DST_PLANE_INTERLVD;
+                break;
+
+        case MDP_XRGB_8888:
+        case MDP_ARGB_8888:
+        case MDP_RGBA_8888:
+                if (iBuf->ibuf_type == MDP_BGRA_8888)
+                        dst_packPattern =
+                            MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B,
+                                                 8);
+                else if (iBuf->ibuf_type == MDP_RGBA_8888)
+                        dst_packPattern =
+                            MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_B, CLR_G, CLR_R,
+                                                 8);
+                else
+                        dst_packPattern =
+                            MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B,
+                                                 8);
+
+                ppp_dst_cfg_reg = PPP_DST_C0G_8BIT |
+                    PPP_DST_C1B_8BIT |
+                    PPP_DST_C2R_8BIT |
+                    PPP_DST_C3A_8BIT |
+                    PPP_DST_C3ALPHA_EN |
+                    PPP_DST_PACKET_CNT_INTERLVD_4ELEM |
+                    PPP_DST_PACK_TIGHT |
+                    PPP_DST_PACK_ALIGN_LSB |
+                    PPP_DST_OUT_SEL_AXI |
+                    PPP_DST_BPP_4BYTES | PPP_DST_PLANE_INTERLVD;
+                break;
+
+        case MDP_Y_CBCR_H2V2:
+        case MDP_Y_CRCB_H2V2:
+                if (iBuf->ibuf_type == MDP_Y_CBCR_H2V2)
+                        dst_packPattern =
+                            MDP_GET_PACK_PATTERN(0, 0, CLR_CB, CLR_CR, 8);
+                else
+                        dst_packPattern =
+                            MDP_GET_PACK_PATTERN(0, 0, CLR_CR, CLR_CB, 8);
+
+                ppp_dst_cfg_reg = PPP_DST_C2R_8BIT |
+                    PPP_DST_C0G_8BIT |
+                    PPP_DST_C1B_8BIT |
+                    PPP_DST_C3A_8BIT |
+                    PPP_DST_PACKET_CNT_INTERLVD_2ELEM |
+                    PPP_DST_PACK_TIGHT |
+                    PPP_DST_PACK_ALIGN_LSB |
+                    PPP_DST_OUT_SEL_AXI | PPP_DST_BPP_2BYTES;
+
+                ppp_operation_reg |= PPP_OP_DST_CHROMA_420;
+                outputRGB = FALSE;
+                pseudoplanr_output = TRUE;
+                /*
+                 * vertically (y direction) and horizontally (x direction)
+                 * sample reduction by 2
+                 */
+
+                /*
+                 * H2V2(YUV420) Cosite
+                 *
+                 * Y    Y    Y    Y
+                 * CbCr      CbCr
+                 * Y    Y    Y    Y
+                 * Y    Y    Y    Y
+                 * CbCr      CbCr
+                 * Y    Y    Y    Y
+                 */
+                dv_slice = dh_slice = 2;
+
+                /* (x,y) and (width,height) must be even numbern */
+                iBuf->roi.lcd_x = (iBuf->roi.lcd_x / 2) * 2;
+                iBuf->roi.dst_width = (iBuf->roi.dst_width / 2) * 2;
+                iBuf->roi.x = (iBuf->roi.x / 2) * 2;
+                iBuf->roi.width = (iBuf->roi.width / 2) * 2;
+
+                iBuf->roi.lcd_y = (iBuf->roi.lcd_y / 2) * 2;
+                iBuf->roi.dst_height = (iBuf->roi.dst_height / 2) * 2;
+                iBuf->roi.y = (iBuf->roi.y / 2) * 2;
+                iBuf->roi.height = (iBuf->roi.height / 2) * 2;
+                break;
+
+        case MDP_YCRYCB_H2V1:
+                dst_packPattern =
+                    MDP_GET_PACK_PATTERN(CLR_Y, CLR_CR, CLR_Y, CLR_CB, 8);
+                ppp_dst_cfg_reg =
+                    PPP_DST_C2R_8BIT | PPP_DST_C0G_8BIT | PPP_DST_C1B_8BIT |
+                    PPP_DST_C3A_8BIT | PPP_DST_PACKET_CNT_INTERLVD_4ELEM |
+                    PPP_DST_PACK_TIGHT | PPP_DST_PACK_ALIGN_LSB |
+                    PPP_DST_OUT_SEL_AXI | PPP_DST_BPP_2BYTES |
+                    PPP_DST_PLANE_INTERLVD;
+
+                ppp_operation_reg |= PPP_OP_DST_CHROMA_H2V1;
+                outputRGB = FALSE;
+                /*
+                 * horizontally (x direction) sample reduction by 2
+                 *
+                 * H2V1(YUV422) Cosite
+                 *
+                 * YCbCr    Y    YCbCr    Y
+                 * YCbCr    Y    YCbCr    Y
+                 * YCbCr    Y    YCbCr    Y
+                 * YCbCr    Y    YCbCr    Y
+                 */
+                dh_slice = 2;
+
+                /*
+                 * if it's TV-Out/MDP_YCRYCB_H2V1, let's go through the
+                 * preloaded gamma setting of 2.2 when the content is
+                 * non-linear ppp_lookUp_enable = TRUE;
+                 */
+
+                /* x and width must be even number */
+                iBuf->roi.lcd_x = (iBuf->roi.lcd_x / 2) * 2;
+                iBuf->roi.dst_width = (iBuf->roi.dst_width / 2) * 2;
+                iBuf->roi.x = (iBuf->roi.x / 2) * 2;
+                iBuf->roi.width = (iBuf->roi.width / 2) * 2;
+                break;
+
+        case MDP_Y_CBCR_H2V1:
+        case MDP_Y_CRCB_H2V1:
+                if (iBuf->ibuf_type == MDP_Y_CBCR_H2V1)
+                        dst_packPattern =
+                            MDP_GET_PACK_PATTERN(0, 0, CLR_CB, CLR_CR, 8);
+                else
+                        dst_packPattern =
+                            MDP_GET_PACK_PATTERN(0, 0, CLR_CR, CLR_CB, 8);
+
+                ppp_dst_cfg_reg = PPP_DST_C2R_8BIT |
+                    PPP_DST_C0G_8BIT |
+                    PPP_DST_C1B_8BIT |
+                    PPP_DST_C3A_8BIT |
+                    PPP_DST_PACKET_CNT_INTERLVD_2ELEM |
+                    PPP_DST_PACK_TIGHT |
+                    PPP_DST_PACK_ALIGN_LSB |
+                    PPP_DST_OUT_SEL_AXI | PPP_DST_BPP_2BYTES;
+
+                ppp_operation_reg |= PPP_OP_DST_CHROMA_H2V1;
+                outputRGB = FALSE;
+                pseudoplanr_output = TRUE;
+                /* horizontally (x direction) sample reduction by 2 */
+                dh_slice = 2;
+
+                /* x and width must be even number */
+                iBuf->roi.lcd_x = (iBuf->roi.lcd_x / 2) * 2;
+                iBuf->roi.dst_width = (iBuf->roi.dst_width / 2) * 2;
+                iBuf->roi.x = (iBuf->roi.x / 2) * 2;
+                iBuf->roi.width = (iBuf->roi.width / 2) * 2;
+                break;
+
+        case MDP_BGR_565:
+        case MDP_RGB_565:
+        default:
+                if (iBuf->ibuf_type == MDP_RGB_565)
+                        dst_packPattern =
+                            MDP_GET_PACK_PATTERN(0, CLR_R, CLR_G, CLR_B, 8);
+                else
+                        dst_packPattern =
+                            MDP_GET_PACK_PATTERN(0, CLR_B, CLR_G, CLR_R, 8);
+
+                ppp_dst_cfg_reg = PPP_DST_C0G_6BIT |
+                    PPP_DST_C1B_5BIT |
+                    PPP_DST_C2R_5BIT |
+                    PPP_DST_PACKET_CNT_INTERLVD_3ELEM |
+                    PPP_DST_PACK_TIGHT |
+                    PPP_DST_PACK_ALIGN_LSB |
+                    PPP_DST_OUT_SEL_AXI |
+                    PPP_DST_BPP_2BYTES | PPP_DST_PLANE_INTERLVD;
+                break;
+        }
+
+        /* source config */
+        switch (iBuf->mdpImg.imgType) {
+        case MDP_RGB_888:
+                inpBpp = 3;
+                /*
+                 * 565 = 2bytes
+                 * RGB = 3Components
+                 * RGB interleaved
+                 */
+                ppp_src_cfg_reg = PPP_SRC_C2R_8BITS | PPP_SRC_C0G_8BITS |
+                        PPP_SRC_C1B_8BITS | PPP_SRC_BPP_INTERLVD_3BYTES |
+                        PPP_SRC_INTERLVD_3COMPONENTS | PPP_SRC_UNPACK_TIGHT |
+                        PPP_SRC_UNPACK_ALIGN_LSB |
+                        PPP_SRC_FETCH_PLANES_INTERLVD;
+
+                packPattern = MDP_GET_PACK_PATTERN(0, CLR_R, CLR_G, CLR_B, 8);
+
+                ppp_operation_reg |= PPP_OP_COLOR_SPACE_RGB |
+                    PPP_OP_SRC_CHROMA_RGB | PPP_OP_DST_CHROMA_RGB;
+                break;
+
+        case MDP_BGRA_8888:
+        case MDP_RGBA_8888:
+        case MDP_ARGB_8888:
+                perPixelAlpha = TRUE;
+        case MDP_XRGB_8888:
+                inpBpp = 4;
+                /*
+                 * 8888 = 4bytes
+                 * ARGB = 4Components
+                 * ARGB interleaved
+                 */
+                ppp_src_cfg_reg = PPP_SRC_C2R_8BITS | PPP_SRC_C0G_8BITS |
+                        PPP_SRC_C1B_8BITS | PPP_SRC_C3A_8BITS |
+                        PPP_SRC_C3_ALPHA_EN | PPP_SRC_BPP_INTERLVD_4BYTES |
+                        PPP_SRC_INTERLVD_4COMPONENTS | PPP_SRC_UNPACK_TIGHT |
+                        PPP_SRC_UNPACK_ALIGN_LSB |
+                        PPP_SRC_FETCH_PLANES_INTERLVD;
+
+                if (iBuf->mdpImg.imgType == MDP_BGRA_8888)
+                        packPattern =
+                            MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B,
+                                                 8);
+                else if (iBuf->mdpImg.imgType == MDP_RGBA_8888)
+                        packPattern =
+                            MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_B, CLR_G, CLR_R,
+                                                 8);
+                else
+                        packPattern =
+                            MDP_GET_PACK_PATTERN(CLR_ALPHA, CLR_R, CLR_G, CLR_B,
+                                                 8);
+
+                ppp_operation_reg |= PPP_OP_COLOR_SPACE_RGB |
+                    PPP_OP_SRC_CHROMA_RGB | PPP_OP_DST_CHROMA_RGB;
+                break;
+
+        case MDP_Y_CBCR_H2V2:
+        case MDP_Y_CRCB_H2V2:
+                inpBpp = 1;
+                src1 = (uint8 *) iBuf->mdpImg.cbcr_addr;
+
+                /*
+                 * CbCr = 2bytes
+                 * CbCr = 2Components
+                 * Y+CbCr
+                 */
+                ppp_src_cfg_reg = PPP_SRC_C2R_8BITS | PPP_SRC_C0G_8BITS |
+                        PPP_SRC_C1B_8BITS | PPP_SRC_BPP_INTERLVD_2BYTES |
+                        PPP_SRC_INTERLVD_2COMPONENTS | PPP_SRC_UNPACK_TIGHT |
+                        PPP_SRC_UNPACK_ALIGN_LSB |
+                        PPP_SRC_FETCH_PLANES_PSEUDOPLNR;
+
+                if (iBuf->mdpImg.imgType == MDP_Y_CRCB_H2V2)
+                        packPattern =
+                            MDP_GET_PACK_PATTERN(0, 0, CLR_CR, CLR_CB, 8);
+                else
+                        packPattern =
+                            MDP_GET_PACK_PATTERN(0, 0, CLR_CB, CLR_CR, 8);
+
+                ppp_operation_reg |= PPP_OP_COLOR_SPACE_YCBCR |
+                    PPP_OP_SRC_CHROMA_420 |
+                    PPP_OP_SRC_CHROMA_COSITE |
+                    PPP_OP_DST_CHROMA_RGB | PPP_OP_DST_CHROMA_COSITE;
+
+                inputRGB = FALSE;
+                sh_slice = sv_slice = 2;
+                break;
+
+        case MDP_YCRYCB_H2V1:
+                inpBpp = 2;
+                ppp_src_cfg_reg = PPP_SRC_C2R_8BITS |
+                    PPP_SRC_C0G_8BITS |
+                    PPP_SRC_C1B_8BITS |
+                    PPP_SRC_C3A_8BITS |
+                    PPP_SRC_BPP_INTERLVD_2BYTES |
+                    PPP_SRC_INTERLVD_4COMPONENTS |
+                    PPP_SRC_UNPACK_TIGHT | PPP_SRC_UNPACK_ALIGN_LSB;
+
+                packPattern =
+                    MDP_GET_PACK_PATTERN(CLR_Y, CLR_CR, CLR_Y, CLR_CB, 8);
+
+                ppp_operation_reg |= PPP_OP_SRC_CHROMA_H2V1 |
+                    PPP_OP_SRC_CHROMA_COSITE | PPP_OP_DST_CHROMA_COSITE;
+
+                /*
+                 * if it's TV-Out/MDP_YCRYCB_H2V1, let's go through the
+                 * preloaded inverse gamma setting of 2.2 since they're
+                 * symetric when the content is non-linear
+                 * ppp_lookUp_enable = TRUE;
+                 */
+
+                /* x and width must be even number */
+                iBuf->roi.lcd_x = (iBuf->roi.lcd_x / 2) * 2;
+                iBuf->roi.dst_width = (iBuf->roi.dst_width / 2) * 2;
+                iBuf->roi.x = (iBuf->roi.x / 2) * 2;
+                iBuf->roi.width = (iBuf->roi.width / 2) * 2;
+
+                inputRGB = FALSE;
+                sh_slice = 2;
+                break;
+
+        case MDP_Y_CBCR_H2V1:
+        case MDP_Y_CRCB_H2V1:
+                inpBpp = 1;
+                src1 = (uint8 *) iBuf->mdpImg.cbcr_addr;
+
+                ppp_src_cfg_reg = PPP_SRC_C2R_8BITS |
+                    PPP_SRC_C0G_8BITS |
+                    PPP_SRC_C1B_8BITS |
+                    PPP_SRC_C3A_8BITS |
+                    PPP_SRC_BPP_INTERLVD_2BYTES |
+                    PPP_SRC_INTERLVD_2COMPONENTS |
+                    PPP_SRC_UNPACK_TIGHT |
+                    PPP_SRC_UNPACK_ALIGN_LSB | PPP_SRC_FETCH_PLANES_PSEUDOPLNR;
+
+                if (iBuf->mdpImg.imgType == MDP_Y_CBCR_H2V1)
+                        packPattern =
+                            MDP_GET_PACK_PATTERN(0, 0, CLR_CB, CLR_CR, 8);
+                else
+                        packPattern =
+                            MDP_GET_PACK_PATTERN(0, 0, CLR_CR, CLR_CB, 8);
+
+                ppp_operation_reg |= PPP_OP_SRC_CHROMA_H2V1 |
+                    PPP_OP_SRC_CHROMA_COSITE | PPP_OP_DST_CHROMA_COSITE;
+                inputRGB = FALSE;
+                sh_slice = 2;
+                break;
+
+        case MDP_BGR_565:
+        case MDP_RGB_565:
+        default:
+                inpBpp = 2;
+                /*
+                 * 565 = 2bytes
+                 * RGB = 3Components
+                 * RGB interleaved
+                 */
+                ppp_src_cfg_reg = PPP_SRC_C2R_5BITS | PPP_SRC_C0G_6BITS |
+                        PPP_SRC_C1B_5BITS | PPP_SRC_BPP_INTERLVD_2BYTES |
+                        PPP_SRC_INTERLVD_3COMPONENTS | PPP_SRC_UNPACK_TIGHT |
+                        PPP_SRC_UNPACK_ALIGN_LSB |
+                        PPP_SRC_FETCH_PLANES_INTERLVD;
+
+                if (iBuf->mdpImg.imgType == MDP_RGB_565)
+                        packPattern =
+                            MDP_GET_PACK_PATTERN(0, CLR_R, CLR_G, CLR_B, 8);
+                else
+                        packPattern =
+                            MDP_GET_PACK_PATTERN(0, CLR_B, CLR_G, CLR_R, 8);
+
+                ppp_operation_reg |= PPP_OP_COLOR_SPACE_RGB |
+                    PPP_OP_SRC_CHROMA_RGB | PPP_OP_DST_CHROMA_RGB;
+                break;
+
+        }
+
+        if (pseudoplanr_output)
+                ppp_dst_cfg_reg |= PPP_DST_PLANE_PSEUDOPLN;
+
+        /* YCbCr to RGB color conversion flag */
+        if ((!inputRGB) && (outputRGB)) {
+                ppp_operation_reg |= PPP_OP_CONVERT_YCBCR2RGB |
+                    PPP_OP_CONVERT_ON;
+
+                /*
+                 * primary/secondary is sort of misleading term...but
+                 * in mdp2.2/3.0 we only use primary matrix (forward/rev)
+                 * in mdp3.1 we use set1(prim) and set2(secd)
+                 */
+#ifdef CONFIG_FB_MSM_MDP31
+                ppp_operation_reg |= PPP_OP_CONVERT_MATRIX_SECONDARY |
+                                        PPP_OP_DST_RGB;
+                MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0240, 0);
+#endif
+
+                if (ppp_lookUp_enable) {
+                        ppp_operation_reg |= PPP_OP_LUT_C0_ON |
+                            PPP_OP_LUT_C1_ON | PPP_OP_LUT_C2_ON;
+                }
+        }
+        /* RGB to YCbCr color conversion flag */
+        if ((inputRGB) && (!outputRGB)) {
+                ppp_operation_reg |= PPP_OP_CONVERT_RGB2YCBCR |
+                    PPP_OP_CONVERT_ON;
+
+#ifdef CONFIG_FB_MSM_MDP31
+                ppp_operation_reg |= PPP_OP_CONVERT_MATRIX_PRIMARY |
+                                        PPP_OP_DST_YCBCR;
+                MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0240, 0x1e);
+#endif
+
+                if (ppp_lookUp_enable) {
+                        ppp_operation_reg |= PPP_OP_LUT_C0_ON |
+                            PPP_OP_LUT_C1_ON | PPP_OP_LUT_C2_ON;
+                }
+        }
+        /* YCbCr to YCbCr color conversion flag */
+        if ((!inputRGB) && (!outputRGB)) {
+                if ((ppp_lookUp_enable) &&
+                    (iBuf->mdpImg.imgType != iBuf->ibuf_type)) {
+                        ppp_operation_reg |= PPP_OP_LUT_C0_ON;
+                }
+        }
+
+        ppp_src_cfg_reg |= (iBuf->roi.x % 2) ? PPP_SRC_BPP_ROI_ODD_X : 0;
+        ppp_src_cfg_reg |= (iBuf->roi.y % 2) ? PPP_SRC_BPP_ROI_ODD_Y : 0;
+
+        if (req->flags & MDP_DEINTERLACE)
+                ppp_operation_reg |= PPP_OP_DEINT_EN;
+
+        /* Dither at DMA side only since iBuf format is RGB888 */
+        if (iBuf->mdpImg.mdpOp & MDPOP_DITHER)
+                ppp_operation_reg |= PPP_OP_DITHER_EN;
+
+        if (iBuf->mdpImg.mdpOp & MDPOP_ROTATION) {
+                ppp_operation_reg |= PPP_OP_ROT_ON;
+
+                if (iBuf->mdpImg.mdpOp & MDPOP_ROT90) {
+                        ppp_operation_reg |= PPP_OP_ROT_90;
+                }
+                if (iBuf->mdpImg.mdpOp & MDPOP_LR) {
+                        ppp_operation_reg |= PPP_OP_FLIP_LR;
+                }
+                if (iBuf->mdpImg.mdpOp & MDPOP_UD) {
+                        ppp_operation_reg |= PPP_OP_FLIP_UD;
+                }
+        }
+
+        src0_ystride = src_width * inpBpp;
+        dest0_ystride = iBuf->ibuf_width * iBuf->bpp;
+
+        /* no need to care about rotation since it's the real-XY. */
+        dst_roi_width = iBuf->roi.dst_width;
+        dst_roi_height = iBuf->roi.dst_height;
+
+        src0 = (uint8 *) iBuf->mdpImg.bmy_addr;
+        dest0 = (uint8 *) iBuf->buf;
+
+        /* Jumping from Y-Plane to Chroma Plane */
+        dest1 = mdp_get_chroma_addr(iBuf);
+
+        /* first pixel addr calculation */
+        mdp_adjust_start_addr(&src0, &src1, sv_slice, sh_slice, iBuf->roi.x,
+                              iBuf->roi.y, src_width, src_height, inpBpp, iBuf,
+                              0);
+        mdp_adjust_start_addr(&dest0, &dest1, dv_slice, dh_slice,
+                              iBuf->roi.lcd_x, iBuf->roi.lcd_y,
+                              iBuf->ibuf_width, iBuf->ibuf_height, iBuf->bpp,
+                              iBuf, 2);
+
+        /* set scale operation */
+        mdp_set_scale(iBuf, dst_roi_width, dst_roi_height,
+                      inputRGB, outputRGB, &ppp_operation_reg);
+
+        /*
+         * setting background source for blending
+         */
+        mdp_set_blend_attr(iBuf, &alpha, &tpVal, perPixelAlpha,
+                           &ppp_operation_reg);
+
+        if (ppp_operation_reg & PPP_OP_BLEND_ON) {
+                mdp_ppp_setbg(iBuf);
+
+                if (iBuf->ibuf_type == MDP_YCRYCB_H2V1) {
+                        ppp_operation_reg |= PPP_OP_BG_CHROMA_H2V1;
+
+                        if (iBuf->mdpImg.mdpOp & MDPOP_TRANSP) {
+                                tpVal = mdp_conv_matx_rgb2yuv(tpVal,
+                                                      (uint16 *) &
+                                                      mdp_ccs_rgb2yuv,
+                                                      &mdp_plv[0], NULL);
+                        }
+                }
+        }
+
+        /*
+         * 0x0004: enable dbg bus
+         * 0x0100: "don't care" Edge Condit until scaling is on
+         * 0x0104: xrc tile x&y size u7.6 format = 7bit.6bit
+         * 0x0108: src pixel size
+         * 0x010c: component plane 0 starting address
+         * 0x011c: component plane 0 ystride
+         * 0x0124: PPP source config register
+         * 0x0128: unpacked pattern from lsb to msb (eg. RGB->BGR)
+         */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0108, (iBuf->roi.height << 16 |
+                                                      iBuf->roi.width));
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x010c, src0); /* comp.plane 0 */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0110, src1); /* comp.plane 1 */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x011c,
+                 (src0_ystride << 16 | src0_ystride));
+
+        /* setup for rgb 565 */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0124, ppp_src_cfg_reg);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0128, packPattern);
+        /*
+         * 0x0138: PPP destination operation register
+         * 0x014c: constant_alpha|transparent_color
+         * 0x0150: PPP destination config register
+         * 0x0154: PPP packing pattern
+         */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0138, ppp_operation_reg);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x014c, alpha << 24 | tpVal);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0150, ppp_dst_cfg_reg);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0154, dst_packPattern);
+
+        /*
+         * 0x0164: ROI height and width
+         * 0x0168: Component Plane 0 starting addr
+         * 0x016c: Component Plane 1 starting addr
+         * 0x0178: Component Plane 1/0 y stride
+         */
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0164,
+                 (dst_roi_height << 16 | dst_roi_width));
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0168, dest0);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x016c, dest1);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0178,
+                 (dest0_ystride << 16 | dest0_ystride));
+
+        flush_imgs(req, inpBpp, iBuf->bpp, p_src_file, p_dst_file);
+#ifdef CONFIG_MDP_PPP_ASYNC_OP
+        mdp_ppp_process_curr_djob();
+#else
+        mdp_pipe_kickoff(MDP_PPP_TERM, mfd);
+#endif
+}
+
+static int mdp_ppp_verify_req(struct mdp_blit_req *req)
+{
+        u32 src_width, src_height, dst_width, dst_height;
+
+        if (req == NULL)
+                return -1;
+
+        if (MDP_IS_IMGTYPE_BAD(req->src.format) ||
+            MDP_IS_IMGTYPE_BAD(req->dst.format))
+                return -1;
+
+        if ((req->src.width == 0) || (req->src.height == 0) ||
+            (req->src_rect.w == 0) || (req->src_rect.h == 0) ||
+            (req->dst.width == 0) || (req->dst.height == 0) ||
+            (req->dst_rect.w == 0) || (req->dst_rect.h == 0))
+
+                return -1;
+
+        if (((req->src_rect.x + req->src_rect.w) > req->src.width) ||
+            ((req->src_rect.y + req->src_rect.h) > req->src.height))
+                return -1;
+
+        if (((req->dst_rect.x + req->dst_rect.w) > req->dst.width) ||
+            ((req->dst_rect.y + req->dst_rect.h) > req->dst.height))
+                return -1;
+
+        /*
+         * scaling range check
+         */
+        src_width = req->src_rect.w;
+        src_height = req->src_rect.h;
+
+        if (req->flags & MDP_ROT_90) {
+                dst_width = req->dst_rect.h;
+                dst_height = req->dst_rect.w;
+        } else {
+                dst_width = req->dst_rect.w;
+                dst_height = req->dst_rect.h;
+        }
+
+        switch (req->dst.format) {
+        case MDP_Y_CRCB_H2V2:
+        case MDP_Y_CBCR_H2V2:
+                src_width = (src_width / 2) * 2;
+                src_height = (src_height / 2) * 2;
+                dst_width = (src_width / 2) * 2;
+                dst_height = (src_height / 2) * 2;
+                break;
+
+        case MDP_Y_CRCB_H2V1:
+        case MDP_Y_CBCR_H2V1:
+        case MDP_YCRYCB_H2V1:
+                src_width = (src_width / 2) * 2;
+                dst_width = (src_width / 2) * 2;
+                break;
+
+        default:
+                break;
+        }
+
+        if (((MDP_SCALE_Q_FACTOR * dst_width) / src_width >
+             MDP_MAX_X_SCALE_FACTOR)
+            || ((MDP_SCALE_Q_FACTOR * dst_width) / src_width <
+                MDP_MIN_X_SCALE_FACTOR))
+                return -1;
+
+        if (((MDP_SCALE_Q_FACTOR * dst_height) / src_height >
+             MDP_MAX_Y_SCALE_FACTOR)
+            || ((MDP_SCALE_Q_FACTOR * dst_height) / src_height <
+                MDP_MIN_Y_SCALE_FACTOR))
+                return -1;
+
+        return 0;
+}
+
+/**
+ * get_gem_img() - retrieve drm obj's start address and size
+ * @img:        contains drm file descriptor and gem handle
+ * @start:      repository of starting address of drm obj allocated memory
+ * @len:        repository of size of drm obj alloacted memory
+ *
+ **/
+int get_gem_img(struct mdp_img *img, unsigned long *start, unsigned long *len)
+{
+        panic("waaaaaaaah");
+        //return kgsl_gem_obj_addr(img->memory_id, (int)img->priv, start, len);
+}
+
+int get_img(struct mdp_img *img, struct fb_info *info, unsigned long *start,
+            unsigned long *len, struct file **pp_file)
+{
+        int put_needed, ret = 0;
+        struct file *file;
+        unsigned long vstart;
+#ifdef CONFIG_ANDROID_PMEM
+        if (!get_pmem_file(img->memory_id, start, &vstart, len, pp_file))
+                return 0;
+#endif
+        file = fget_light(img->memory_id, &put_needed);
+        if (file == NULL)
+                return -1;
+
+        if (MAJOR(file->f_dentry->d_inode->i_rdev) == FB_MAJOR) {
+                *start = info->fix.smem_start;
+                *len = info->fix.smem_len;
+                *pp_file = file;
+        } else {
+                ret = -1;
+                fput_light(file, put_needed);
+        }
+        return ret;
+}
+
+int mdp_ppp_blit(struct fb_info *info, struct mdp_blit_req *req,
+        struct file **pp_src_file, struct file **pp_dst_file)
+{
+        unsigned long src_start, dst_start;
+        unsigned long src_len = 0;
+        unsigned long dst_len = 0;
+        MDPIBUF iBuf;
+        u32 dst_width, dst_height;
+        struct file *p_src_file = 0 , *p_dst_file = 0;
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+
+        if (req->dst.format == MDP_FB_FORMAT)
+                req->dst.format =  mfd->fb_imgType;
+        if (req->src.format == MDP_FB_FORMAT)
+                req->src.format = mfd->fb_imgType;
+
+        if (req->flags & MDP_BLIT_SRC_GEM) {
+                if (get_gem_img(&req->src, &src_start, &src_len) < 0)
+                        return -1;
+        } else {
+                get_img(&req->src, info, &src_start, &src_len, &p_src_file);
+        }
+        if (src_len == 0) {
+                printk(KERN_ERR "mdp_ppp: could not retrieve image from "
+                       "memory\n");
+                return -1;
+        }
+
+        if (req->flags & MDP_BLIT_DST_GEM) {
+                if (get_gem_img(&req->dst, &dst_start, &dst_len) < 0)
+                        return -1;
+        } else {
+                get_img(&req->dst, info, &dst_start, &dst_len, &p_dst_file);
+        }
+        if (dst_len == 0) {
+                printk(KERN_ERR "mdp_ppp: could not retrieve image from "
+                       "memory\n");
+                return -1;
+        }
+        *pp_src_file = p_src_file;
+        *pp_dst_file = p_dst_file;
+        if (mdp_ppp_verify_req(req)) {
+                printk(KERN_ERR "mdp_ppp: invalid image!\n");
+                return -1;
+        }
+
+        iBuf.ibuf_width = req->dst.width;
+        iBuf.ibuf_height = req->dst.height;
+        iBuf.bpp = bytes_per_pixel[req->dst.format];
+
+        iBuf.ibuf_type = req->dst.format;
+        iBuf.buf = (uint8 *) dst_start;
+        iBuf.buf += req->dst.offset;
+
+        iBuf.roi.lcd_x = req->dst_rect.x;
+        iBuf.roi.lcd_y = req->dst_rect.y;
+        iBuf.roi.dst_width = req->dst_rect.w;
+        iBuf.roi.dst_height = req->dst_rect.h;
+
+        iBuf.roi.x = req->src_rect.x;
+        iBuf.roi.width = req->src_rect.w;
+        iBuf.roi.y = req->src_rect.y;
+        iBuf.roi.height = req->src_rect.h;
+
+        iBuf.mdpImg.width = req->src.width;
+        iBuf.mdpImg.imgType = req->src.format;
+
+        iBuf.mdpImg.bmy_addr = (uint32 *) (src_start + req->src.offset);
+        iBuf.mdpImg.cbcr_addr =
+            (uint32 *) ((uint32) iBuf.mdpImg.bmy_addr +
+                        req->src.width * req->src.height);
+
+        iBuf.mdpImg.mdpOp = MDPOP_NOP;
+
+        /* blending check */
+        if (req->transp_mask != MDP_TRANSP_NOP) {
+                iBuf.mdpImg.mdpOp |= MDPOP_TRANSP;
+                iBuf.mdpImg.tpVal = req->transp_mask;
+                iBuf.mdpImg.tpVal = mdp_calc_tpval(&iBuf.mdpImg);
+        }
+
+        req->alpha &= 0xff;
+        if (req->alpha < MDP_ALPHA_NOP) {
+                iBuf.mdpImg.mdpOp |= MDPOP_ALPHAB;
+                iBuf.mdpImg.alpha = req->alpha;
+        }
+
+        /* rotation check */
+        if (req->flags & MDP_FLIP_LR)
+                iBuf.mdpImg.mdpOp |= MDPOP_LR;
+        if (req->flags & MDP_FLIP_UD)
+                iBuf.mdpImg.mdpOp |= MDPOP_UD;
+        if (req->flags & MDP_ROT_90)
+                iBuf.mdpImg.mdpOp |= MDPOP_ROT90;
+        if (req->flags & MDP_DITHER)
+                iBuf.mdpImg.mdpOp |= MDPOP_DITHER;
+
+        if (req->flags & MDP_BLEND_FG_PREMULT) {
+#ifdef CONFIG_FB_MSM_MDP31
+                iBuf.mdpImg.mdpOp |= MDPOP_FG_PM_ALPHA;
+#else
+                return -EINVAL;
+#endif
+        }
+
+        if (req->flags & MDP_DEINTERLACE) {
+#ifdef CONFIG_FB_MSM_MDP31
+                if ((req->src.format != MDP_Y_CBCR_H2V2) &&
+                        (req->src.format != MDP_Y_CRCB_H2V2))
+#endif
+                return -EINVAL;
+        }
+
+        /* scale check */
+        if (req->flags & MDP_ROT_90) {
+                dst_width = req->dst_rect.h;
+                dst_height = req->dst_rect.w;
+        } else {
+                dst_width = req->dst_rect.w;
+                dst_height = req->dst_rect.h;
+        }
+
+        if ((iBuf.roi.width != dst_width) || (iBuf.roi.height != dst_height))
+                iBuf.mdpImg.mdpOp |= MDPOP_ASCALE;
+
+        if (req->flags & MDP_BLUR) {
+#ifdef CONFIG_FB_MSM_MDP31
+                if (req->flags & MDP_SHARPENING)
+                        printk(KERN_WARNING
+                                "mdp: MDP_SHARPENING is set with MDP_BLUR!\n");
+                req->flags |= MDP_SHARPENING;
+                req->sharpening_strength = -127;
+#else
+                iBuf.mdpImg.mdpOp |= MDPOP_ASCALE | MDPOP_BLUR;
+
+#endif
+        }
+
+        if (req->flags & MDP_SHARPENING) {
+#ifdef CONFIG_FB_MSM_MDP31
+                if ((req->sharpening_strength > 127) ||
+                        (req->sharpening_strength < -127)) {
+                        printk(KERN_ERR
+                                "%s: sharpening strength out of range\n",
+                                __func__);
+                        return -EINVAL;
+                }
+
+                iBuf.mdpImg.mdpOp |= MDPOP_ASCALE | MDPOP_SHARPENING;
+                iBuf.mdpImg.sp_value = req->sharpening_strength & 0xff;
+#else
+                return -EINVAL;
+#endif
+        }
+
+        down(&mdp_ppp_mutex);
+        /* MDP cmd block enable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+#ifdef CONFIG_FB_MSM_MDP31
+        mdp_start_ppp(mfd, &iBuf, req, p_src_file, p_dst_file);
+#else
+        /* bg tile fetching HW workaround */
+        if (((iBuf.mdpImg.mdpOp & (MDPOP_TRANSP | MDPOP_ALPHAB)) ||
+             (req->src.format == MDP_ARGB_8888) ||
+             (req->src.format == MDP_BGRA_8888) ||
+             (req->src.format == MDP_RGBA_8888)) &&
+            (iBuf.mdpImg.mdpOp & MDPOP_ROT90) && (req->dst_rect.w <= 16)) {
+                int dst_h, src_w, i;
+
+                src_w = req->src_rect.w;
+                dst_h = iBuf.roi.dst_height;
+
+                for (i = 0; i < (req->dst_rect.h / 16); i++) {
+                        /* this tile size */
+                        iBuf.roi.dst_height = 16;
+                        iBuf.roi.width =
+                            (16 * req->src_rect.w) / req->dst_rect.h;
+
+                        /* if it's out of scale range... */
+                        if (((MDP_SCALE_Q_FACTOR * iBuf.roi.dst_height) /
+                             iBuf.roi.width) > MDP_MAX_X_SCALE_FACTOR)
+                                iBuf.roi.width =
+                                    (MDP_SCALE_Q_FACTOR * iBuf.roi.dst_height) /
+                                    MDP_MAX_X_SCALE_FACTOR;
+                        else if (((MDP_SCALE_Q_FACTOR * iBuf.roi.dst_height) /
+                                  iBuf.roi.width) < MDP_MIN_X_SCALE_FACTOR)
+                                iBuf.roi.width =
+                                    (MDP_SCALE_Q_FACTOR * iBuf.roi.dst_height) /
+                                    MDP_MIN_X_SCALE_FACTOR;
+
+                        mdp_start_ppp(mfd, &iBuf, req, p_src_file, p_dst_file);
+
+                        /* next tile location */
+                        iBuf.roi.lcd_y += 16;
+                        iBuf.roi.x += iBuf.roi.width;
+
+                        /* this is for a remainder update */
+                        dst_h -= 16;
+                        src_w -= iBuf.roi.width;
+                }
+
+                if ((dst_h < 0) || (src_w < 0))
+                        printk
+                            ("msm_fb: mdp_blt_ex() unexpected result! line:%d\n",
+                             __LINE__);
+
+                /* remainder update */
+                if ((dst_h > 0) && (src_w > 0)) {
+                        u32 tmp_v;
+
+                        iBuf.roi.dst_height = dst_h;
+                        iBuf.roi.width = src_w;
+
+                        if (((MDP_SCALE_Q_FACTOR * iBuf.roi.dst_height) /
+                             iBuf.roi.width) > MDP_MAX_X_SCALE_FACTOR) {
+                                tmp_v =
+                                    (MDP_SCALE_Q_FACTOR * iBuf.roi.dst_height) /
+                                    MDP_MAX_X_SCALE_FACTOR +
+                                    (MDP_SCALE_Q_FACTOR * iBuf.roi.dst_height) %
+                                    MDP_MAX_X_SCALE_FACTOR ? 1 : 0;
+
+                                /* move x location as roi width gets bigger */
+                                iBuf.roi.x -= tmp_v - iBuf.roi.width;
+                                iBuf.roi.width = tmp_v;
+                        } else
+                            if (((MDP_SCALE_Q_FACTOR * iBuf.roi.dst_height) /
+                                 iBuf.roi.width) < MDP_MIN_X_SCALE_FACTOR) {
+                                tmp_v =
+                                    (MDP_SCALE_Q_FACTOR * iBuf.roi.dst_height) /
+                                    MDP_MIN_X_SCALE_FACTOR +
+                                    (MDP_SCALE_Q_FACTOR * iBuf.roi.dst_height) %
+                                    MDP_MIN_X_SCALE_FACTOR ? 1 : 0;
+
+                                /*
+                                 * we don't move x location for continuity of
+                                 * source image
+                                 */
+                                iBuf.roi.width = tmp_v;
+                        }
+
+                        mdp_start_ppp(mfd, &iBuf, req, p_src_file, p_dst_file);
+                }
+        } else {
+                mdp_start_ppp(mfd, &iBuf, req, p_src_file, p_dst_file);
+        }
+#endif
+
+        /* MDP cmd block disable */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+        up(&mdp_ppp_mutex);
+
+        return 0;
+}
diff --git a/drivers/staging/msm/mdp_ppp_dq.c b/drivers/staging/msm/mdp_ppp_dq.c
new file mode 100644
index 000000000000..3dc1c0cc61f9
--- /dev/null
+++ b/drivers/staging/msm/mdp_ppp_dq.c
@@ -0,0 +1,347 @@
+/* Copyright (c) 2009-2010, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include "mdp.h"
+
+static boolean mdp_ppp_intr_flag = FALSE;
+static boolean mdp_ppp_busy_flag = FALSE;
+
+/* Queue to keep track of the completed jobs for cleaning */
+static LIST_HEAD(mdp_ppp_djob_clnrq);
+static DEFINE_SPINLOCK(mdp_ppp_djob_clnrq_lock);
+
+/* Worker to cleanup Display Jobs */
+static struct workqueue_struct *mdp_ppp_djob_clnr;
+
+/* Display Queue (DQ) for MDP PPP Block */
+static LIST_HEAD(mdp_ppp_dq);
+static DEFINE_SPINLOCK(mdp_ppp_dq_lock);
+
+/* Current Display Job for MDP PPP */
+static struct mdp_ppp_djob *curr_djob;
+
+/* Track ret code for the last opeartion */
+static int mdp_ppp_ret_code;
+
+inline int mdp_ppp_get_ret_code(void)
+{
+        return mdp_ppp_ret_code;
+}
+
+/* Push <Reg, Val> pair into DQ (if available) to later
+ * program the MDP PPP Block */
+inline void mdp_ppp_outdw(uint32_t addr, uint32_t data)
+{
+        if (curr_djob) {
+
+                /* get the last node of the list. */
+                struct mdp_ppp_roi_cmd_set *node =
+                        list_entry(curr_djob->roi_cmd_list.prev,
+                                struct mdp_ppp_roi_cmd_set, node);
+
+                /* If a node is already full, create a new one and add it to
+                 * the list (roi_cmd_list).
+                 */
+                if (node->ncmds == MDP_PPP_ROI_NODE_SIZE) {
+                        node = kmalloc(sizeof(struct mdp_ppp_roi_cmd_set),
+                                GFP_KERNEL);
+                        if (!node) {
+                                printk(KERN_ERR
+                                        "MDP_PPP: not enough memory.\n");
+                                mdp_ppp_ret_code = -EINVAL;
+                                return;
+                        }
+
+                        /* no ROI commands initially */
+                        node->ncmds = 0;
+
+                        /* add one node to roi_cmd_list. */
+                        list_add_tail(&node->node, &curr_djob->roi_cmd_list);
+                }
+
+                /* register ROI commands */
+                node->cmd[node->ncmds].reg = addr;
+                node->cmd[node->ncmds].val = data;
+                node->ncmds++;
+        } else
+                /* program MDP PPP block now */
+                outpdw((addr), (data));
+}
+
+/* Initialize DQ */
+inline void mdp_ppp_dq_init(void)
+{
+        mdp_ppp_djob_clnr = create_singlethread_workqueue("MDPDJobClnrThrd");
+}
+
+/* Release resources of a job (DJob). */
+static void mdp_ppp_del_djob(struct mdp_ppp_djob *job)
+{
+        struct mdp_ppp_roi_cmd_set *node, *tmp;
+
+        /* release mem */
+        mdp_ppp_put_img(job->p_src_file, job->p_dst_file);
+
+        /* release roi_cmd_list */
+        list_for_each_entry_safe(node, tmp, &job->roi_cmd_list, node) {
+                list_del(&node->node);
+                kfree(node);
+        }
+
+        /* release job struct */
+        kfree(job);
+}
+
+/* Worker thread to reclaim resources once a display job is done */
+static void mdp_ppp_djob_cleaner(struct work_struct *work)
+{
+        struct mdp_ppp_djob *job;
+
+        MDP_PPP_DEBUG_MSG("mdp ppp display job cleaner started \n");
+
+        /* cleanup display job */
+        job = container_of(work, struct mdp_ppp_djob, cleaner.work);
+        if (likely(work && job))
+                mdp_ppp_del_djob(job);
+}
+
+/* Create a new Display Job (DJob) */
+inline struct mdp_ppp_djob *mdp_ppp_new_djob(void)
+{
+        struct mdp_ppp_djob *job;
+        struct mdp_ppp_roi_cmd_set *node;
+
+        /* create a new djob */
+        job = kmalloc(sizeof(struct mdp_ppp_djob), GFP_KERNEL);
+        if (!job)
+                return NULL;
+
+        /* add the first node to curr_djob->roi_cmd_list */
+        node = kmalloc(sizeof(struct mdp_ppp_roi_cmd_set), GFP_KERNEL);
+        if (!node) {
+                kfree(job);
+                return NULL;
+        }
+
+        /* make this current djob container to keep track of the curr djob not
+         * used in the async path i.e. no sync needed
+         *
+         * Should not contain any references from the past djob
+         */
+        BUG_ON(curr_djob);
+        curr_djob = job;
+        INIT_LIST_HEAD(&curr_djob->roi_cmd_list);
+
+        /* no ROI commands initially */
+        node->ncmds = 0;
+        INIT_LIST_HEAD(&node->node);
+        list_add_tail(&node->node, &curr_djob->roi_cmd_list);
+
+        /* register this djob with the djob cleaner
+         * initializes 'work' data struct
+         */
+        INIT_DELAYED_WORK(&curr_djob->cleaner, mdp_ppp_djob_cleaner);
+        INIT_LIST_HEAD(&curr_djob->entry);
+
+        curr_djob->p_src_file = 0;
+        curr_djob->p_dst_file = 0;
+
+        return job;
+}
+
+/* Undo the effect of mdp_ppp_new_djob() */
+inline void mdp_ppp_clear_curr_djob(void)
+{
+        if (likely(curr_djob)) {
+                mdp_ppp_del_djob(curr_djob);
+                curr_djob = NULL;
+        }
+}
+
+/* Cleanup dirty djobs */
+static void mdp_ppp_flush_dirty_djobs(void *cond)
+{
+        unsigned long flags;
+        struct mdp_ppp_djob *job;
+
+        /* Flush the jobs from the djob clnr queue */
+        while (cond && test_bit(0, (unsigned long *)cond)) {
+
+                /* Until we are done with the cleanup queue */
+                spin_lock_irqsave(&mdp_ppp_djob_clnrq_lock, flags);
+                if (list_empty(&mdp_ppp_djob_clnrq)) {
+                        spin_unlock_irqrestore(&mdp_ppp_djob_clnrq_lock, flags);
+                        break;
+                }
+
+                MDP_PPP_DEBUG_MSG("flushing djobs ... loop \n");
+
+                /* Retrieve the job that needs to be cleaned */
+                job = list_entry(mdp_ppp_djob_clnrq.next,
+                                struct mdp_ppp_djob, entry);
+                list_del_init(&job->entry);
+                spin_unlock_irqrestore(&mdp_ppp_djob_clnrq_lock, flags);
+
+                /* Keep mem state coherent */
+                msm_fb_ensure_mem_coherency_after_dma(job->info, &job->req, 1);
+
+                /* Schedule jobs for cleanup
+                 * A seperate worker thread does this */
+                queue_delayed_work(mdp_ppp_djob_clnr, &job->cleaner,
+                        mdp_timer_duration);
+        }
+}
+
+/* If MDP PPP engine is busy, wait until it is available again */
+void mdp_ppp_wait(void)
+{
+        unsigned long flags;
+        int cond = 1;
+
+        /* keep flushing dirty djobs as long as MDP PPP engine is busy */
+        mdp_ppp_flush_dirty_djobs(&mdp_ppp_busy_flag);
+
+        /* block if MDP PPP engine is still busy */
+        spin_lock_irqsave(&mdp_ppp_dq_lock, flags);
+        if (test_bit(0, (unsigned long *)&mdp_ppp_busy_flag)) {
+
+                /* prepare for the wakeup event */
+                test_and_set_bit(0, (unsigned long *)&mdp_ppp_waiting);
+                INIT_COMPLETION(mdp_ppp_comp);
+                spin_unlock_irqrestore(&mdp_ppp_dq_lock, flags);
+
+                /* block uninterruptibly until available */
+                MDP_PPP_DEBUG_MSG("waiting for mdp... \n");
+                wait_for_completion_killable(&mdp_ppp_comp);
+
+                /* if MDP PPP engine is still free,
+                 * disable INT_MDP if enabled
+                 */
+                spin_lock_irqsave(&mdp_ppp_dq_lock, flags);
+                if (!test_bit(0, (unsigned long *)&mdp_ppp_busy_flag) &&
+                test_and_clear_bit(0, (unsigned long *)&mdp_ppp_intr_flag))
+                        mdp_disable_irq(MDP_PPP_TERM);
+        }
+        spin_unlock_irqrestore(&mdp_ppp_dq_lock, flags);
+
+        /* flush remaining dirty djobs, if any */
+        mdp_ppp_flush_dirty_djobs(&cond);
+}
+
+/* Program MDP PPP block to process this ROI */
+static void mdp_ppp_process_roi(struct list_head *roi_cmd_list)
+{
+
+        /* program PPP engine with registered ROI commands */
+        struct mdp_ppp_roi_cmd_set *node;
+        list_for_each_entry(node, roi_cmd_list, node) {
+                int i = 0;
+                for (; i < node->ncmds; i++) {
+                        MDP_PPP_DEBUG_MSG("%d: reg: 0x%x val: 0x%x \n",
+                                        i, node->cmd[i].reg, node->cmd[i].val);
+                        outpdw(node->cmd[i].reg, node->cmd[i].val);
+                }
+        }
+
+        /* kickoff MDP PPP engine */
+        MDP_PPP_DEBUG_MSG("kicking off mdp \n");
+        outpdw(MDP_BASE + 0x30, 0x1000);
+}
+
+/* Submit this display job to MDP PPP engine */
+static void mdp_ppp_dispatch_djob(struct mdp_ppp_djob *job)
+{
+        /* enable INT_MDP if disabled */
+        if (!test_and_set_bit(0, (unsigned long *)&mdp_ppp_intr_flag))
+                mdp_enable_irq(MDP_PPP_TERM);
+
+        /* turn on PPP and CMD blocks */
+        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+        mdp_pipe_ctrl(MDP_PPP_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+        /* process this ROI */
+        mdp_ppp_process_roi(&job->roi_cmd_list);
+}
+
+/* Enqueue this display job to be cleaned up later in "mdp_ppp_djob_done" */
+static inline void mdp_ppp_enqueue_djob(struct mdp_ppp_djob *job)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&mdp_ppp_dq_lock, flags);
+        list_add_tail(&job->entry, &mdp_ppp_dq);
+        spin_unlock_irqrestore(&mdp_ppp_dq_lock, flags);
+}
+
+/* First enqueue display job for cleanup and dispatch immediately
+ * if MDP PPP engine is free */
+void mdp_ppp_process_curr_djob(void)
+{
+        /* enqueue djob */
+        mdp_ppp_enqueue_djob(curr_djob);
+
+        /* dispatch now if MDP PPP engine is free */
+        if (!test_and_set_bit(0, (unsigned long *)&mdp_ppp_busy_flag))
+                mdp_ppp_dispatch_djob(curr_djob);
+
+        /* done with the current djob */
+        curr_djob = NULL;
+}
+
+/* Called from mdp_isr - cleanup finished job and start with next
+ * if available else set MDP PPP engine free */
+void mdp_ppp_djob_done(void)
+{
+        struct mdp_ppp_djob *curr, *next;
+        unsigned long flags;
+
+        /* dequeue current */
+        spin_lock_irqsave(&mdp_ppp_dq_lock, flags);
+        curr = list_entry(mdp_ppp_dq.next, struct mdp_ppp_djob, entry);
+        list_del_init(&curr->entry);
+        spin_unlock_irqrestore(&mdp_ppp_dq_lock, flags);
+
+        /* cleanup current - enqueue in the djob clnr queue */
+        spin_lock_irqsave(&mdp_ppp_djob_clnrq_lock, flags);
+        list_add_tail(&curr->entry, &mdp_ppp_djob_clnrq);
+        spin_unlock_irqrestore(&mdp_ppp_djob_clnrq_lock, flags);
+
+        /* grab next pending */
+        spin_lock_irqsave(&mdp_ppp_dq_lock, flags);
+        if (!list_empty(&mdp_ppp_dq)) {
+                next = list_entry(mdp_ppp_dq.next, struct mdp_ppp_djob,
+                        entry);
+                spin_unlock_irqrestore(&mdp_ppp_dq_lock, flags);
+
+                /* process next in the queue */
+                mdp_ppp_process_roi(&next->roi_cmd_list);
+        } else {
+                /* no pending display job */
+                spin_unlock_irqrestore(&mdp_ppp_dq_lock, flags);
+
+                /* turn off PPP and CMD blocks - "in_isr" is TRUE */
+                mdp_pipe_ctrl(MDP_PPP_BLOCK, MDP_BLOCK_POWER_OFF, TRUE);
+                mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, TRUE);
+
+                /* notify if waiting */
+                if (test_and_clear_bit(0, (unsigned long *)&mdp_ppp_waiting))
+                        complete(&mdp_ppp_comp);
+
+                /* set free */
+                test_and_clear_bit(0, (unsigned long *)&mdp_ppp_busy_flag);
+        }
+}
diff --git a/drivers/staging/msm/mdp_ppp_dq.h b/drivers/staging/msm/mdp_ppp_dq.h
new file mode 100644
index 000000000000..03e4e9a5f234
--- /dev/null
+++ b/drivers/staging/msm/mdp_ppp_dq.h
@@ -0,0 +1,86 @@
+/* Copyright (c) 2009-2010, Code Aurora Forum. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above
+ *       copyright notice, this list of conditions and the following
+ *       disclaimer in the documentation and/or other materials provided
+ *       with the distribution.
+ *     * Neither the name of Code Aurora Forum, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef MDP_PPP_DQ_H
+#define MDP_PPP_DQ_H
+
+#include "msm_fb_def.h"
+
+#define MDP_PPP_DEBUG_MSG MSM_FB_DEBUG
+
+/* The maximum number of <Reg,Val> pairs in an mdp_ppp_roi_cmd_set structure (a
+ * node)
+ */
+#define MDP_PPP_ROI_NODE_SIZE 32
+
+/* ROI config command (<Reg,Val> pair) for MDP PPP block */
+struct mdp_ppp_roi_cmd {
+        uint32_t reg;
+        uint32_t val;
+};
+
+/* ROI config commands for MDP PPP block are stored in a list of
+ * mdp_ppp_roi_cmd_set structures (nodes).
+ */
+struct mdp_ppp_roi_cmd_set {
+        struct list_head node;
+        uint32_t ncmds; /* number of commands in this set (node). */
+        struct mdp_ppp_roi_cmd cmd[MDP_PPP_ROI_NODE_SIZE];
+};
+
+/* MDP PPP Display Job (DJob) */
+struct mdp_ppp_djob {
+        struct list_head entry;
+        /* One ROI per MDP PPP DJob */
+        struct list_head roi_cmd_list;
+        struct mdp_blit_req req;
+        struct fb_info *info;
+        struct delayed_work cleaner;
+        struct file *p_src_file, *p_dst_file;
+};
+
+extern struct completion mdp_ppp_comp;
+extern boolean mdp_ppp_waiting;
+extern unsigned long mdp_timer_duration;
+
+unsigned int mdp_ppp_async_op_get(void);
+void mdp_ppp_async_op_set(unsigned int flag);
+void msm_fb_ensure_mem_coherency_after_dma(struct fb_info *info,
+        struct mdp_blit_req *req_list, int req_list_count);
+void mdp_ppp_put_img(struct file *p_src_file, struct file *p_dst_file);
+void mdp_ppp_dq_init(void);
+void mdp_ppp_outdw(uint32_t addr, uint32_t data);
+struct mdp_ppp_djob *mdp_ppp_new_djob(void);
+void mdp_ppp_clear_curr_djob(void);
+void mdp_ppp_process_curr_djob(void);
+int mdp_ppp_get_ret_code(void);
+void mdp_ppp_djob_done(void);
+void mdp_ppp_wait(void);
+
+#endif /* MDP_PPP_DQ_H */
diff --git a/drivers/staging/msm/mdp_ppp_v20.c b/drivers/staging/msm/mdp_ppp_v20.c
new file mode 100644
index 000000000000..b5b7271921e0
--- /dev/null
+++ b/drivers/staging/msm/mdp_ppp_v20.c
@@ -0,0 +1,2486 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/fb.h>
+#include "linux/proc_fs.h"
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <asm/div64.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+
+static MDP_SCALE_MODE mdp_curr_up_scale_xy;
+static MDP_SCALE_MODE mdp_curr_down_scale_x;
+static MDP_SCALE_MODE mdp_curr_down_scale_y;
+
+static long long mdp_do_div(long long num, long long den)
+{
+        do_div(num, den);
+        return num;
+}
+
+struct mdp_table_entry mdp_gaussian_blur_table[] = {
+        /* max variance */
+        { 0x5fffc, 0x20000080 },
+        { 0x50280, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50284, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50288, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x5028c, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50290, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50294, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50298, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x5029c, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502a0, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502a4, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502a8, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502ac, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502b0, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502b4, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502b8, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502bc, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502c0, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502c4, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502c8, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502cc, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502d0, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502d4, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502d8, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502dc, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502e0, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502e4, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502e8, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502ec, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502f0, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502f4, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502f8, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x502fc, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50300, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50304, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50308, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x5030c, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50310, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50314, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50318, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x5031c, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50320, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50324, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50328, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x5032c, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50330, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50334, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50338, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x5033c, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50340, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50344, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50348, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x5034c, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50350, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50354, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50358, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x5035c, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50360, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50364, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50368, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x5036c, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50370, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50374, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x50378, 0x20000080 },
+        { 0x5fffc, 0x20000080 },
+        { 0x5037c, 0x20000080 },
+};
+
+static void load_scale_table(
+        struct mdp_table_entry *table, int len)
+{
+        int i;
+        for (i = 0; i < len; i++)
+                MDP_OUTP(MDP_BASE + table[i].reg, table[i].val);
+}
+
+static void mdp_load_pr_upscale_table(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50200, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50204, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50208, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5020c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50210, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50214, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50218, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5021c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50220, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50224, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50228, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5022c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50230, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50234, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50238, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5023c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50240, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50244, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50248, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5024c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50250, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50254, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50258, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5025c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50260, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50264, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50268, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5026c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50270, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50274, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50278, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5027c, 0x0);
+}
+
+static void mdp_load_pr_downscale_table_x_point2TOpoint4(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50280, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50284, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50288, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5028c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50290, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50294, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50298, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5029c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502a0, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502a4, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502a8, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502ac, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502b0, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502b4, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502b8, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502bc, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502c0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502c4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502c8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502cc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502d0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502d4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502d8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502dc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502e0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502e4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502e8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502ec, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502f0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502f4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502f8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502fc, 0x0);
+}
+
+static void mdp_load_pr_downscale_table_y_point2TOpoint4(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50300, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50304, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50308, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5030c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50310, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50314, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50318, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5031c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50320, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50324, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50328, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5032c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50330, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50334, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50338, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5033c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50340, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50344, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50348, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5034c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50350, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50354, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50358, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5035c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50360, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50364, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50368, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5036c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50370, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50374, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50378, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5037c, 0x0);
+}
+
+static void mdp_load_pr_downscale_table_x_point4TOpoint6(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50280, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50284, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50288, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5028c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50290, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50294, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50298, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5029c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502a0, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502a4, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502a8, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502ac, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502b0, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502b4, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502b8, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502bc, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502c0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502c4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502c8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502cc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502d0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502d4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502d8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502dc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502e0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502e4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502e8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502ec, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502f0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502f4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502f8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502fc, 0x0);
+}
+
+static void mdp_load_pr_downscale_table_y_point4TOpoint6(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50300, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50304, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50308, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5030c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50310, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50314, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50318, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5031c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50320, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50324, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50328, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5032c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50330, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50334, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50338, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5033c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50340, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50344, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50348, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5034c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50350, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50354, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50358, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5035c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50360, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50364, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50368, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5036c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50370, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50374, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50378, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5037c, 0x0);
+}
+
+static void mdp_load_pr_downscale_table_x_point6TOpoint8(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50280, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50284, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50288, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5028c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50290, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50294, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50298, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5029c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502a0, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502a4, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502a8, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502ac, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502b0, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502b4, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502b8, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502bc, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502c0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502c4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502c8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502cc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502d0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502d4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502d8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502dc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502e0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502e4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502e8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502ec, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502f0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502f4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502f8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502fc, 0x0);
+}
+
+static void mdp_load_pr_downscale_table_y_point6TOpoint8(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50300, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50304, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50308, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5030c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50310, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50314, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50318, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5031c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50320, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50324, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50328, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5032c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50330, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50334, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50338, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5033c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50340, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50344, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50348, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5034c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50350, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50354, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50358, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5035c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50360, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50364, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50368, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5036c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50370, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50374, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50378, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5037c, 0x0);
+}
+
+static void mdp_load_pr_downscale_table_x_point8TO1(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50280, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50284, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50288, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5028c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50290, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50294, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50298, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5029c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502a0, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502a4, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502a8, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502ac, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502b0, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502b4, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502b8, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x502bc, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502c0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502c4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502c8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502cc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502d0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502d4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502d8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502dc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502e0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502e4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502e8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502ec, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502f0, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502f4, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502f8, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x502fc, 0x0);
+}
+
+static void mdp_load_pr_downscale_table_y_point8TO1(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50300, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50304, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50308, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5030c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50310, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50314, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50318, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5031c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50320, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50324, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50328, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5032c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50330, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50334, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50338, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5033c, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50340, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50344, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50348, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5034c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50350, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50354, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50358, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5035c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50360, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50364, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50368, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5036c, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50370, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50374, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x50378, 0x0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ff);
+        MDP_OUTP(MDP_BASE + 0x5037c, 0x0);
+}
+
+static void mdp_load_bc_upscale_table(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50200, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xff80000d);
+        MDP_OUTP(MDP_BASE + 0x50204, 0x7ec003f9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfec0001c);
+        MDP_OUTP(MDP_BASE + 0x50208, 0x7d4003f3);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe40002b);
+        MDP_OUTP(MDP_BASE + 0x5020c, 0x7b8003ed);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfd80003c);
+        MDP_OUTP(MDP_BASE + 0x50210, 0x794003e8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfcc0004d);
+        MDP_OUTP(MDP_BASE + 0x50214, 0x76c003e4);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfc40005f);
+        MDP_OUTP(MDP_BASE + 0x50218, 0x73c003e0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfb800071);
+        MDP_OUTP(MDP_BASE + 0x5021c, 0x708003de);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfac00085);
+        MDP_OUTP(MDP_BASE + 0x50220, 0x6d0003db);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfa000098);
+        MDP_OUTP(MDP_BASE + 0x50224, 0x698003d9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf98000ac);
+        MDP_OUTP(MDP_BASE + 0x50228, 0x654003d8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf8c000c1);
+        MDP_OUTP(MDP_BASE + 0x5022c, 0x610003d7);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf84000d5);
+        MDP_OUTP(MDP_BASE + 0x50230, 0x5c8003d7);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf7c000e9);
+        MDP_OUTP(MDP_BASE + 0x50234, 0x580003d7);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf74000fd);
+        MDP_OUTP(MDP_BASE + 0x50238, 0x534003d8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf6c00112);
+        MDP_OUTP(MDP_BASE + 0x5023c, 0x4e8003d8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf6800126);
+        MDP_OUTP(MDP_BASE + 0x50240, 0x494003da);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf600013a);
+        MDP_OUTP(MDP_BASE + 0x50244, 0x448003db);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf600014d);
+        MDP_OUTP(MDP_BASE + 0x50248, 0x3f4003dd);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf5c00160);
+        MDP_OUTP(MDP_BASE + 0x5024c, 0x3a4003df);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf5c00172);
+        MDP_OUTP(MDP_BASE + 0x50250, 0x354003e1);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf5c00184);
+        MDP_OUTP(MDP_BASE + 0x50254, 0x304003e3);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf6000195);
+        MDP_OUTP(MDP_BASE + 0x50258, 0x2b0003e6);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf64001a6);
+        MDP_OUTP(MDP_BASE + 0x5025c, 0x260003e8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf6c001b4);
+        MDP_OUTP(MDP_BASE + 0x50260, 0x214003eb);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf78001c2);
+        MDP_OUTP(MDP_BASE + 0x50264, 0x1c4003ee);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf80001cf);
+        MDP_OUTP(MDP_BASE + 0x50268, 0x17c003f1);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf90001db);
+        MDP_OUTP(MDP_BASE + 0x5026c, 0x134003f3);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfa0001e5);
+        MDP_OUTP(MDP_BASE + 0x50270, 0xf0003f6);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfb4001ee);
+        MDP_OUTP(MDP_BASE + 0x50274, 0xac003f9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfcc001f5);
+        MDP_OUTP(MDP_BASE + 0x50278, 0x70003fb);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe4001fb);
+        MDP_OUTP(MDP_BASE + 0x5027c, 0x34003fe);
+}
+
+static void mdp_load_bc_downscale_table_x_point2TOpoint4(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ac00084);
+        MDP_OUTP(MDP_BASE + 0x50280, 0x23400083);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1b000084);
+        MDP_OUTP(MDP_BASE + 0x50284, 0x23000083);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1b400084);
+        MDP_OUTP(MDP_BASE + 0x50288, 0x23000082);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1b400085);
+        MDP_OUTP(MDP_BASE + 0x5028c, 0x23000081);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1b800085);
+        MDP_OUTP(MDP_BASE + 0x50290, 0x23000080);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1bc00086);
+        MDP_OUTP(MDP_BASE + 0x50294, 0x22c0007f);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1c000086);
+        MDP_OUTP(MDP_BASE + 0x50298, 0x2280007f);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1c400086);
+        MDP_OUTP(MDP_BASE + 0x5029c, 0x2280007e);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1c800086);
+        MDP_OUTP(MDP_BASE + 0x502a0, 0x2280007d);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1cc00086);
+        MDP_OUTP(MDP_BASE + 0x502a4, 0x2240007d);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1cc00087);
+        MDP_OUTP(MDP_BASE + 0x502a8, 0x2240007c);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1d000087);
+        MDP_OUTP(MDP_BASE + 0x502ac, 0x2240007b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1d400087);
+        MDP_OUTP(MDP_BASE + 0x502b0, 0x2200007b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1d400088);
+        MDP_OUTP(MDP_BASE + 0x502b4, 0x22400079);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1d800088);
+        MDP_OUTP(MDP_BASE + 0x502b8, 0x22400078);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1dc00088);
+        MDP_OUTP(MDP_BASE + 0x502bc, 0x22400077);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1dc00089);
+        MDP_OUTP(MDP_BASE + 0x502c0, 0x22000077);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1e000089);
+        MDP_OUTP(MDP_BASE + 0x502c4, 0x22000076);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1e400089);
+        MDP_OUTP(MDP_BASE + 0x502c8, 0x22000075);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ec00088);
+        MDP_OUTP(MDP_BASE + 0x502cc, 0x21c00075);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ec00089);
+        MDP_OUTP(MDP_BASE + 0x502d0, 0x21c00074);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1f000089);
+        MDP_OUTP(MDP_BASE + 0x502d4, 0x21c00073);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1f400089);
+        MDP_OUTP(MDP_BASE + 0x502d8, 0x21800073);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1f40008a);
+        MDP_OUTP(MDP_BASE + 0x502dc, 0x21800072);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1f80008a);
+        MDP_OUTP(MDP_BASE + 0x502e0, 0x21800071);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1fc0008a);
+        MDP_OUTP(MDP_BASE + 0x502e4, 0x21800070);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1fc0008b);
+        MDP_OUTP(MDP_BASE + 0x502e8, 0x2180006f);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x2000008c);
+        MDP_OUTP(MDP_BASE + 0x502ec, 0x2140006e);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x2040008c);
+        MDP_OUTP(MDP_BASE + 0x502f0, 0x2140006d);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x2080008c);
+        MDP_OUTP(MDP_BASE + 0x502f4, 0x2100006d);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x20c0008c);
+        MDP_OUTP(MDP_BASE + 0x502f8, 0x2100006c);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x20c0008d);
+        MDP_OUTP(MDP_BASE + 0x502fc, 0x2100006b);
+}
+
+static void mdp_load_bc_downscale_table_y_point2TOpoint4(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ac00084);
+        MDP_OUTP(MDP_BASE + 0x50300, 0x23400083);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1b000084);
+        MDP_OUTP(MDP_BASE + 0x50304, 0x23000083);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1b400084);
+        MDP_OUTP(MDP_BASE + 0x50308, 0x23000082);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1b400085);
+        MDP_OUTP(MDP_BASE + 0x5030c, 0x23000081);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1b800085);
+        MDP_OUTP(MDP_BASE + 0x50310, 0x23000080);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1bc00086);
+        MDP_OUTP(MDP_BASE + 0x50314, 0x22c0007f);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1c000086);
+        MDP_OUTP(MDP_BASE + 0x50318, 0x2280007f);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1c400086);
+        MDP_OUTP(MDP_BASE + 0x5031c, 0x2280007e);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1c800086);
+        MDP_OUTP(MDP_BASE + 0x50320, 0x2280007d);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1cc00086);
+        MDP_OUTP(MDP_BASE + 0x50324, 0x2240007d);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1cc00087);
+        MDP_OUTP(MDP_BASE + 0x50328, 0x2240007c);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1d000087);
+        MDP_OUTP(MDP_BASE + 0x5032c, 0x2240007b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1d400087);
+        MDP_OUTP(MDP_BASE + 0x50330, 0x2200007b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1d400088);
+        MDP_OUTP(MDP_BASE + 0x50334, 0x22400079);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1d800088);
+        MDP_OUTP(MDP_BASE + 0x50338, 0x22400078);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1dc00088);
+        MDP_OUTP(MDP_BASE + 0x5033c, 0x22400077);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1dc00089);
+        MDP_OUTP(MDP_BASE + 0x50340, 0x22000077);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1e000089);
+        MDP_OUTP(MDP_BASE + 0x50344, 0x22000076);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1e400089);
+        MDP_OUTP(MDP_BASE + 0x50348, 0x22000075);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ec00088);
+        MDP_OUTP(MDP_BASE + 0x5034c, 0x21c00075);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ec00089);
+        MDP_OUTP(MDP_BASE + 0x50350, 0x21c00074);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1f000089);
+        MDP_OUTP(MDP_BASE + 0x50354, 0x21c00073);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1f400089);
+        MDP_OUTP(MDP_BASE + 0x50358, 0x21800073);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1f40008a);
+        MDP_OUTP(MDP_BASE + 0x5035c, 0x21800072);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1f80008a);
+        MDP_OUTP(MDP_BASE + 0x50360, 0x21800071);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1fc0008a);
+        MDP_OUTP(MDP_BASE + 0x50364, 0x21800070);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1fc0008b);
+        MDP_OUTP(MDP_BASE + 0x50368, 0x2180006f);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x2000008c);
+        MDP_OUTP(MDP_BASE + 0x5036c, 0x2140006e);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x2040008c);
+        MDP_OUTP(MDP_BASE + 0x50370, 0x2140006d);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x2080008c);
+        MDP_OUTP(MDP_BASE + 0x50374, 0x2100006d);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x20c0008c);
+        MDP_OUTP(MDP_BASE + 0x50378, 0x2100006c);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x20c0008d);
+        MDP_OUTP(MDP_BASE + 0x5037c, 0x2100006b);
+}
+
+static void mdp_load_bc_downscale_table_x_point4TOpoint6(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x740008c);
+        MDP_OUTP(MDP_BASE + 0x50280, 0x33800088);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x800008e);
+        MDP_OUTP(MDP_BASE + 0x50284, 0x33400084);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x8400092);
+        MDP_OUTP(MDP_BASE + 0x50288, 0x33000080);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x9000094);
+        MDP_OUTP(MDP_BASE + 0x5028c, 0x3300007b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x9c00098);
+        MDP_OUTP(MDP_BASE + 0x50290, 0x32400077);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xa40009b);
+        MDP_OUTP(MDP_BASE + 0x50294, 0x32000073);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xb00009d);
+        MDP_OUTP(MDP_BASE + 0x50298, 0x31c0006f);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xbc000a0);
+        MDP_OUTP(MDP_BASE + 0x5029c, 0x3140006b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xc8000a2);
+        MDP_OUTP(MDP_BASE + 0x502a0, 0x31000067);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xd8000a5);
+        MDP_OUTP(MDP_BASE + 0x502a4, 0x30800062);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xe4000a8);
+        MDP_OUTP(MDP_BASE + 0x502a8, 0x2fc0005f);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xec000aa);
+        MDP_OUTP(MDP_BASE + 0x502ac, 0x2fc0005b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf8000ad);
+        MDP_OUTP(MDP_BASE + 0x502b0, 0x2f400057);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x108000b0);
+        MDP_OUTP(MDP_BASE + 0x502b4, 0x2e400054);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x114000b2);
+        MDP_OUTP(MDP_BASE + 0x502b8, 0x2e000050);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x124000b4);
+        MDP_OUTP(MDP_BASE + 0x502bc, 0x2d80004c);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x130000b6);
+        MDP_OUTP(MDP_BASE + 0x502c0, 0x2d000049);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x140000b8);
+        MDP_OUTP(MDP_BASE + 0x502c4, 0x2c800045);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x150000b9);
+        MDP_OUTP(MDP_BASE + 0x502c8, 0x2c000042);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x15c000bd);
+        MDP_OUTP(MDP_BASE + 0x502cc, 0x2b40003e);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x16c000bf);
+        MDP_OUTP(MDP_BASE + 0x502d0, 0x2a80003b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x17c000bf);
+        MDP_OUTP(MDP_BASE + 0x502d4, 0x2a000039);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x188000c2);
+        MDP_OUTP(MDP_BASE + 0x502d8, 0x29400036);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x19c000c4);
+        MDP_OUTP(MDP_BASE + 0x502dc, 0x28800032);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ac000c5);
+        MDP_OUTP(MDP_BASE + 0x502e0, 0x2800002f);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1bc000c7);
+        MDP_OUTP(MDP_BASE + 0x502e4, 0x2740002c);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1cc000c8);
+        MDP_OUTP(MDP_BASE + 0x502e8, 0x26c00029);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1dc000c9);
+        MDP_OUTP(MDP_BASE + 0x502ec, 0x26000027);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ec000cc);
+        MDP_OUTP(MDP_BASE + 0x502f0, 0x25000024);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x200000cc);
+        MDP_OUTP(MDP_BASE + 0x502f4, 0x24800021);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x210000cd);
+        MDP_OUTP(MDP_BASE + 0x502f8, 0x23800020);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x220000ce);
+        MDP_OUTP(MDP_BASE + 0x502fc, 0x2300001d);
+}
+
+static void mdp_load_bc_downscale_table_y_point4TOpoint6(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x740008c);
+        MDP_OUTP(MDP_BASE + 0x50300, 0x33800088);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x800008e);
+        MDP_OUTP(MDP_BASE + 0x50304, 0x33400084);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x8400092);
+        MDP_OUTP(MDP_BASE + 0x50308, 0x33000080);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x9000094);
+        MDP_OUTP(MDP_BASE + 0x5030c, 0x3300007b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x9c00098);
+        MDP_OUTP(MDP_BASE + 0x50310, 0x32400077);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xa40009b);
+        MDP_OUTP(MDP_BASE + 0x50314, 0x32000073);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xb00009d);
+        MDP_OUTP(MDP_BASE + 0x50318, 0x31c0006f);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xbc000a0);
+        MDP_OUTP(MDP_BASE + 0x5031c, 0x3140006b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xc8000a2);
+        MDP_OUTP(MDP_BASE + 0x50320, 0x31000067);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xd8000a5);
+        MDP_OUTP(MDP_BASE + 0x50324, 0x30800062);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xe4000a8);
+        MDP_OUTP(MDP_BASE + 0x50328, 0x2fc0005f);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xec000aa);
+        MDP_OUTP(MDP_BASE + 0x5032c, 0x2fc0005b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf8000ad);
+        MDP_OUTP(MDP_BASE + 0x50330, 0x2f400057);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x108000b0);
+        MDP_OUTP(MDP_BASE + 0x50334, 0x2e400054);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x114000b2);
+        MDP_OUTP(MDP_BASE + 0x50338, 0x2e000050);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x124000b4);
+        MDP_OUTP(MDP_BASE + 0x5033c, 0x2d80004c);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x130000b6);
+        MDP_OUTP(MDP_BASE + 0x50340, 0x2d000049);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x140000b8);
+        MDP_OUTP(MDP_BASE + 0x50344, 0x2c800045);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x150000b9);
+        MDP_OUTP(MDP_BASE + 0x50348, 0x2c000042);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x15c000bd);
+        MDP_OUTP(MDP_BASE + 0x5034c, 0x2b40003e);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x16c000bf);
+        MDP_OUTP(MDP_BASE + 0x50350, 0x2a80003b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x17c000bf);
+        MDP_OUTP(MDP_BASE + 0x50354, 0x2a000039);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x188000c2);
+        MDP_OUTP(MDP_BASE + 0x50358, 0x29400036);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x19c000c4);
+        MDP_OUTP(MDP_BASE + 0x5035c, 0x28800032);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ac000c5);
+        MDP_OUTP(MDP_BASE + 0x50360, 0x2800002f);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1bc000c7);
+        MDP_OUTP(MDP_BASE + 0x50364, 0x2740002c);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1cc000c8);
+        MDP_OUTP(MDP_BASE + 0x50368, 0x26c00029);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1dc000c9);
+        MDP_OUTP(MDP_BASE + 0x5036c, 0x26000027);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1ec000cc);
+        MDP_OUTP(MDP_BASE + 0x50370, 0x25000024);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x200000cc);
+        MDP_OUTP(MDP_BASE + 0x50374, 0x24800021);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x210000cd);
+        MDP_OUTP(MDP_BASE + 0x50378, 0x23800020);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x220000ce);
+        MDP_OUTP(MDP_BASE + 0x5037c, 0x2300001d);
+}
+
+static void mdp_load_bc_downscale_table_x_point6TOpoint8(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe000070);
+        MDP_OUTP(MDP_BASE + 0x50280, 0x4bc00068);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe000078);
+        MDP_OUTP(MDP_BASE + 0x50284, 0x4bc00060);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe000080);
+        MDP_OUTP(MDP_BASE + 0x50288, 0x4b800059);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe000089);
+        MDP_OUTP(MDP_BASE + 0x5028c, 0x4b000052);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe400091);
+        MDP_OUTP(MDP_BASE + 0x50290, 0x4a80004b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe40009a);
+        MDP_OUTP(MDP_BASE + 0x50294, 0x4a000044);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe8000a3);
+        MDP_OUTP(MDP_BASE + 0x50298, 0x4940003d);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfec000ac);
+        MDP_OUTP(MDP_BASE + 0x5029c, 0x48400037);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xff0000b4);
+        MDP_OUTP(MDP_BASE + 0x502a0, 0x47800031);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xff8000bd);
+        MDP_OUTP(MDP_BASE + 0x502a4, 0x4640002b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xc5);
+        MDP_OUTP(MDP_BASE + 0x502a8, 0x45000026);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x8000ce);
+        MDP_OUTP(MDP_BASE + 0x502ac, 0x43800021);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x10000d6);
+        MDP_OUTP(MDP_BASE + 0x502b0, 0x4240001c);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x18000df);
+        MDP_OUTP(MDP_BASE + 0x502b4, 0x40800018);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x24000e6);
+        MDP_OUTP(MDP_BASE + 0x502b8, 0x3f000014);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x30000ee);
+        MDP_OUTP(MDP_BASE + 0x502bc, 0x3d400010);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x40000f5);
+        MDP_OUTP(MDP_BASE + 0x502c0, 0x3b80000c);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x50000fc);
+        MDP_OUTP(MDP_BASE + 0x502c4, 0x39800009);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x6000102);
+        MDP_OUTP(MDP_BASE + 0x502c8, 0x37c00006);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x7000109);
+        MDP_OUTP(MDP_BASE + 0x502cc, 0x35800004);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x840010e);
+        MDP_OUTP(MDP_BASE + 0x502d0, 0x33800002);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x9800114);
+        MDP_OUTP(MDP_BASE + 0x502d4, 0x31400000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xac00119);
+        MDP_OUTP(MDP_BASE + 0x502d8, 0x2f4003fe);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xc40011e);
+        MDP_OUTP(MDP_BASE + 0x502dc, 0x2d0003fc);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xdc00121);
+        MDP_OUTP(MDP_BASE + 0x502e0, 0x2b0003fb);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf400125);
+        MDP_OUTP(MDP_BASE + 0x502e4, 0x28c003fa);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x11000128);
+        MDP_OUTP(MDP_BASE + 0x502e8, 0x268003f9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x12c0012a);
+        MDP_OUTP(MDP_BASE + 0x502ec, 0x244003f9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1480012c);
+        MDP_OUTP(MDP_BASE + 0x502f0, 0x224003f8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1640012e);
+        MDP_OUTP(MDP_BASE + 0x502f4, 0x200003f8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1800012f);
+        MDP_OUTP(MDP_BASE + 0x502f8, 0x1e0003f8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1a00012f);
+        MDP_OUTP(MDP_BASE + 0x502fc, 0x1c0003f8);
+}
+
+static void mdp_load_bc_downscale_table_y_point6TOpoint8(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe000070);
+        MDP_OUTP(MDP_BASE + 0x50300, 0x4bc00068);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe000078);
+        MDP_OUTP(MDP_BASE + 0x50304, 0x4bc00060);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe000080);
+        MDP_OUTP(MDP_BASE + 0x50308, 0x4b800059);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe000089);
+        MDP_OUTP(MDP_BASE + 0x5030c, 0x4b000052);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe400091);
+        MDP_OUTP(MDP_BASE + 0x50310, 0x4a80004b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe40009a);
+        MDP_OUTP(MDP_BASE + 0x50314, 0x4a000044);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe8000a3);
+        MDP_OUTP(MDP_BASE + 0x50318, 0x4940003d);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfec000ac);
+        MDP_OUTP(MDP_BASE + 0x5031c, 0x48400037);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xff0000b4);
+        MDP_OUTP(MDP_BASE + 0x50320, 0x47800031);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xff8000bd);
+        MDP_OUTP(MDP_BASE + 0x50324, 0x4640002b);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xc5);
+        MDP_OUTP(MDP_BASE + 0x50328, 0x45000026);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x8000ce);
+        MDP_OUTP(MDP_BASE + 0x5032c, 0x43800021);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x10000d6);
+        MDP_OUTP(MDP_BASE + 0x50330, 0x4240001c);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x18000df);
+        MDP_OUTP(MDP_BASE + 0x50334, 0x40800018);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x24000e6);
+        MDP_OUTP(MDP_BASE + 0x50338, 0x3f000014);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x30000ee);
+        MDP_OUTP(MDP_BASE + 0x5033c, 0x3d400010);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x40000f5);
+        MDP_OUTP(MDP_BASE + 0x50340, 0x3b80000c);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x50000fc);
+        MDP_OUTP(MDP_BASE + 0x50344, 0x39800009);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x6000102);
+        MDP_OUTP(MDP_BASE + 0x50348, 0x37c00006);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x7000109);
+        MDP_OUTP(MDP_BASE + 0x5034c, 0x35800004);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x840010e);
+        MDP_OUTP(MDP_BASE + 0x50350, 0x33800002);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x9800114);
+        MDP_OUTP(MDP_BASE + 0x50354, 0x31400000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xac00119);
+        MDP_OUTP(MDP_BASE + 0x50358, 0x2f4003fe);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xc40011e);
+        MDP_OUTP(MDP_BASE + 0x5035c, 0x2d0003fc);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xdc00121);
+        MDP_OUTP(MDP_BASE + 0x50360, 0x2b0003fb);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf400125);
+        MDP_OUTP(MDP_BASE + 0x50364, 0x28c003fa);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x11000128);
+        MDP_OUTP(MDP_BASE + 0x50368, 0x268003f9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x12c0012a);
+        MDP_OUTP(MDP_BASE + 0x5036c, 0x244003f9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1480012c);
+        MDP_OUTP(MDP_BASE + 0x50370, 0x224003f8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1640012e);
+        MDP_OUTP(MDP_BASE + 0x50374, 0x200003f8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1800012f);
+        MDP_OUTP(MDP_BASE + 0x50378, 0x1e0003f8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x1a00012f);
+        MDP_OUTP(MDP_BASE + 0x5037c, 0x1c0003f8);
+}
+
+static void mdp_load_bc_downscale_table_x_point8TO1(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50280, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xff80000d);
+        MDP_OUTP(MDP_BASE + 0x50284, 0x7ec003f9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfec0001c);
+        MDP_OUTP(MDP_BASE + 0x50288, 0x7d4003f3);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe40002b);
+        MDP_OUTP(MDP_BASE + 0x5028c, 0x7b8003ed);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfd80003c);
+        MDP_OUTP(MDP_BASE + 0x50290, 0x794003e8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfcc0004d);
+        MDP_OUTP(MDP_BASE + 0x50294, 0x76c003e4);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfc40005f);
+        MDP_OUTP(MDP_BASE + 0x50298, 0x73c003e0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfb800071);
+        MDP_OUTP(MDP_BASE + 0x5029c, 0x708003de);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfac00085);
+        MDP_OUTP(MDP_BASE + 0x502a0, 0x6d0003db);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfa000098);
+        MDP_OUTP(MDP_BASE + 0x502a4, 0x698003d9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf98000ac);
+        MDP_OUTP(MDP_BASE + 0x502a8, 0x654003d8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf8c000c1);
+        MDP_OUTP(MDP_BASE + 0x502ac, 0x610003d7);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf84000d5);
+        MDP_OUTP(MDP_BASE + 0x502b0, 0x5c8003d7);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf7c000e9);
+        MDP_OUTP(MDP_BASE + 0x502b4, 0x580003d7);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf74000fd);
+        MDP_OUTP(MDP_BASE + 0x502b8, 0x534003d8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf6c00112);
+        MDP_OUTP(MDP_BASE + 0x502bc, 0x4e8003d8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf6800126);
+        MDP_OUTP(MDP_BASE + 0x502c0, 0x494003da);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf600013a);
+        MDP_OUTP(MDP_BASE + 0x502c4, 0x448003db);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf600014d);
+        MDP_OUTP(MDP_BASE + 0x502c8, 0x3f4003dd);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf5c00160);
+        MDP_OUTP(MDP_BASE + 0x502cc, 0x3a4003df);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf5c00172);
+        MDP_OUTP(MDP_BASE + 0x502d0, 0x354003e1);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf5c00184);
+        MDP_OUTP(MDP_BASE + 0x502d4, 0x304003e3);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf6000195);
+        MDP_OUTP(MDP_BASE + 0x502d8, 0x2b0003e6);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf64001a6);
+        MDP_OUTP(MDP_BASE + 0x502dc, 0x260003e8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf6c001b4);
+        MDP_OUTP(MDP_BASE + 0x502e0, 0x214003eb);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf78001c2);
+        MDP_OUTP(MDP_BASE + 0x502e4, 0x1c4003ee);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf80001cf);
+        MDP_OUTP(MDP_BASE + 0x502e8, 0x17c003f1);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf90001db);
+        MDP_OUTP(MDP_BASE + 0x502ec, 0x134003f3);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfa0001e5);
+        MDP_OUTP(MDP_BASE + 0x502f0, 0xf0003f6);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfb4001ee);
+        MDP_OUTP(MDP_BASE + 0x502f4, 0xac003f9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfcc001f5);
+        MDP_OUTP(MDP_BASE + 0x502f8, 0x70003fb);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe4001fb);
+        MDP_OUTP(MDP_BASE + 0x502fc, 0x34003fe);
+}
+
+static void mdp_load_bc_downscale_table_y_point8TO1(void)
+{
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0x0);
+        MDP_OUTP(MDP_BASE + 0x50300, 0x7fc00000);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xff80000d);
+        MDP_OUTP(MDP_BASE + 0x50304, 0x7ec003f9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfec0001c);
+        MDP_OUTP(MDP_BASE + 0x50308, 0x7d4003f3);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe40002b);
+        MDP_OUTP(MDP_BASE + 0x5030c, 0x7b8003ed);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfd80003c);
+        MDP_OUTP(MDP_BASE + 0x50310, 0x794003e8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfcc0004d);
+        MDP_OUTP(MDP_BASE + 0x50314, 0x76c003e4);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfc40005f);
+        MDP_OUTP(MDP_BASE + 0x50318, 0x73c003e0);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfb800071);
+        MDP_OUTP(MDP_BASE + 0x5031c, 0x708003de);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfac00085);
+        MDP_OUTP(MDP_BASE + 0x50320, 0x6d0003db);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfa000098);
+        MDP_OUTP(MDP_BASE + 0x50324, 0x698003d9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf98000ac);
+        MDP_OUTP(MDP_BASE + 0x50328, 0x654003d8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf8c000c1);
+        MDP_OUTP(MDP_BASE + 0x5032c, 0x610003d7);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf84000d5);
+        MDP_OUTP(MDP_BASE + 0x50330, 0x5c8003d7);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf7c000e9);
+        MDP_OUTP(MDP_BASE + 0x50334, 0x580003d7);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf74000fd);
+        MDP_OUTP(MDP_BASE + 0x50338, 0x534003d8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf6c00112);
+        MDP_OUTP(MDP_BASE + 0x5033c, 0x4e8003d8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf6800126);
+        MDP_OUTP(MDP_BASE + 0x50340, 0x494003da);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf600013a);
+        MDP_OUTP(MDP_BASE + 0x50344, 0x448003db);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf600014d);
+        MDP_OUTP(MDP_BASE + 0x50348, 0x3f4003dd);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf5c00160);
+        MDP_OUTP(MDP_BASE + 0x5034c, 0x3a4003df);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf5c00172);
+        MDP_OUTP(MDP_BASE + 0x50350, 0x354003e1);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf5c00184);
+        MDP_OUTP(MDP_BASE + 0x50354, 0x304003e3);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf6000195);
+        MDP_OUTP(MDP_BASE + 0x50358, 0x2b0003e6);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf64001a6);
+        MDP_OUTP(MDP_BASE + 0x5035c, 0x260003e8);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf6c001b4);
+        MDP_OUTP(MDP_BASE + 0x50360, 0x214003eb);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf78001c2);
+        MDP_OUTP(MDP_BASE + 0x50364, 0x1c4003ee);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf80001cf);
+        MDP_OUTP(MDP_BASE + 0x50368, 0x17c003f1);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xf90001db);
+        MDP_OUTP(MDP_BASE + 0x5036c, 0x134003f3);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfa0001e5);
+        MDP_OUTP(MDP_BASE + 0x50370, 0xf0003f6);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfb4001ee);
+        MDP_OUTP(MDP_BASE + 0x50374, 0xac003f9);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfcc001f5);
+        MDP_OUTP(MDP_BASE + 0x50378, 0x70003fb);
+        MDP_OUTP(MDP_BASE + 0x5fffc, 0xfe4001fb);
+        MDP_OUTP(MDP_BASE + 0x5037c, 0x34003fe);
+}
+
+static int mdp_get_edge_cond(MDPIBUF *iBuf, uint32 *dup, uint32 *dup2)
+{
+        uint32 reg;
+        uint32 dst_roi_width;   /* Dimensions of DST ROI. */
+        uint32 dst_roi_height;  /* Used to calculate scaling ratios. */
+
+        /*
+         * positions of the luma pixel(relative to the image ) required for
+         * scaling the ROI
+         */
+        int32 luma_interp_point_left = 0; /* left-most luma pixel needed */
+        int32 luma_interp_point_right = 0; /* right-most luma pixel needed */
+        int32 luma_interp_point_top = 0; /* top-most luma pixel needed */
+        int32 luma_interp_point_bottom = 0; /* bottom-most luma pixel needed */
+
+        /*
+         * positions of the chroma pixel(relative to the image ) required for
+         * interpolating a chroma value at all required luma positions
+         */
+        /* left-most chroma pixel needed */
+        int32 chroma_interp_point_left = 0;
+        /* right-most chroma pixel needed */
+        int32 chroma_interp_point_right = 0;
+        /* top-most chroma pixel needed */
+        int32 chroma_interp_point_top = 0;
+        /* bottom-most chroma pixel needed */
+        int32 chroma_interp_point_bottom = 0;
+
+        /*
+         * a rectangular region within the chroma plane of the "image".
+         * Chroma pixels falling inside of this rectangle belongs to the ROI
+         */
+        int32 chroma_bound_left = 0;
+        int32 chroma_bound_right = 0;
+        int32 chroma_bound_top = 0;
+        int32 chroma_bound_bottom = 0;
+
+        /*
+         * number of chroma pixels to replicate on the left, right,
+         * top and bottom edge of the ROI.
+         */
+        int32 chroma_repeat_left = 0;
+        int32 chroma_repeat_right = 0;
+        int32 chroma_repeat_top = 0;
+        int32 chroma_repeat_bottom = 0;
+
+        /*
+         * number of luma pixels to replicate on the left, right,
+         * top and bottom edge of the ROI.
+         */
+        int32 luma_repeat_left = 0;
+        int32 luma_repeat_right = 0;
+        int32 luma_repeat_top = 0;
+        int32 luma_repeat_bottom = 0;
+
+        boolean chroma_edge_enable;
+
+        uint32 _is_scale_enabled = 0;
+        uint32 _is_yuv_offsite_vertical = 0;
+
+        /* fg edge duplicate */
+        reg = 0x0;
+
+        if (iBuf->mdpImg.mdpOp & MDPOP_ASCALE) {        /* if scaling enabled */
+
+                _is_scale_enabled = 1;
+
+                /*
+                 * if rotation mode involves a 90 deg rotation, flip
+                 * dst_roi_width with dst_roi_height.
+                 * Scaling ratios is based on source ROI dimensions, and
+                 * dst ROI dimensions before rotation.
+                 */
+                if (iBuf->mdpImg.mdpOp & MDPOP_ROT90) {
+                        dst_roi_width = iBuf->roi.dst_height;
+                        dst_roi_height = iBuf->roi.dst_width;
+                } else {
+                        dst_roi_width = iBuf->roi.dst_width;
+                        dst_roi_height = iBuf->roi.dst_height;
+                }
+
+                /*
+                 * Find out the luma pixels needed for scaling in the
+                 * x direction (LEFT and RIGHT).  Locations of pixels are
+                 * relative to the ROI. Upper-left corner of ROI corresponds
+                 * to coordinates (0,0). Also set the number of luma pixel
+                 * to repeat.
+                 */
+                if (iBuf->roi.width > 3 * dst_roi_width) {
+                        /* scale factor < 1/3 */
+                        luma_interp_point_left = 0;
+                        luma_interp_point_right = (iBuf->roi.width - 1);
+                        luma_repeat_left = 0;
+                        luma_repeat_right = 0;
+                } else if (iBuf->roi.width == 3 * dst_roi_width) {
+                        /* scale factor == 1/3 */
+                        luma_interp_point_left = 0;
+                        luma_interp_point_right = (iBuf->roi.width - 1) + 1;
+                        luma_repeat_left = 0;
+                        luma_repeat_right = 1;
+                } else if ((iBuf->roi.width > dst_roi_width) &&
+                           (iBuf->roi.width < 3 * dst_roi_width)) {
+                        /* 1/3 < scale factor < 1 */
+                        luma_interp_point_left = -1;
+                        luma_interp_point_right = (iBuf->roi.width - 1) + 1;
+                        luma_repeat_left = 1;
+                        luma_repeat_right = 1;
+                }
+
+                else if (iBuf->roi.width == dst_roi_width) {
+                        /* scale factor == 1 */
+                        luma_interp_point_left = -1;
+                        luma_interp_point_right = (iBuf->roi.width - 1) + 2;
+                        luma_repeat_left = 1;
+                        luma_repeat_right = 2;
+                } else {        /* (iBuf->roi.width < dst_roi_width) */
+                          /* scale factor > 1 */
+                        luma_interp_point_left = -2;
+                        luma_interp_point_right = (iBuf->roi.width - 1) + 2;
+                        luma_repeat_left = 2;
+                        luma_repeat_right = 2;
+                }
+
+                /*
+                 * Find out the number of pixels needed for scaling in the
+                 * y direction (TOP and BOTTOM).  Locations of pixels are
+                 * relative to the ROI. Upper-left corner of ROI corresponds
+                 * to coordinates (0,0). Also set the number of luma pixel
+                 * to repeat.
+                 */
+                if (iBuf->roi.height > 3 * dst_roi_height) {
+                        /* scale factor < 1/3 */
+                        luma_interp_point_top = 0;
+                        luma_interp_point_bottom = (iBuf->roi.height - 1);
+                        luma_repeat_top = 0;
+                        luma_repeat_bottom = 0;
+                } else if (iBuf->roi.height == 3 * dst_roi_height) {
+                        /* scale factor == 1/3 */
+                        luma_interp_point_top = 0;
+                        luma_interp_point_bottom = (iBuf->roi.height - 1) + 1;
+                        luma_repeat_top = 0;
+                        luma_repeat_bottom = 1;
+                } else if ((iBuf->roi.height > dst_roi_height) &&
+                           (iBuf->roi.height < 3 * dst_roi_height)) {
+                        /* 1/3 < scale factor < 1 */
+                        luma_interp_point_top = -1;
+                        luma_interp_point_bottom = (iBuf->roi.height - 1) + 1;
+                        luma_repeat_top = 1;
+                        luma_repeat_bottom = 1;
+                } else if (iBuf->roi.height == dst_roi_height) {
+                        /* scale factor == 1 */
+                        luma_interp_point_top = -1;
+                        luma_interp_point_bottom = (iBuf->roi.height - 1) + 2;
+                        luma_repeat_top = 1;
+                        luma_repeat_bottom = 2;
+                } else {        /* (iBuf->roi.height < dst_roi_height) */
+                         /* scale factor > 1 */
+                        luma_interp_point_top = -2;
+                        luma_interp_point_bottom = (iBuf->roi.height - 1) + 2;
+                        luma_repeat_top = 2;
+                        luma_repeat_bottom = 2;
+                }
+        }                       /* if (iBuf->scale.scale_flag) */
+        else {                  /* scaling disabled */
+                /*
+                 * Since no scaling needed, Tile Fetch does not require any
+                 * more luma pixel than what the ROI contains.
+                 */
+                luma_interp_point_left = (int32) 0;
+                luma_interp_point_right = (int32) (iBuf->roi.width - 1);
+                luma_interp_point_top = (int32) 0;
+                luma_interp_point_bottom = (int32) (iBuf->roi.height - 1);
+
+                luma_repeat_left = 0;
+                luma_repeat_right = 0;
+                luma_repeat_top = 0;
+                luma_repeat_bottom = 0;
+        }
+
+        /* After adding the ROI offsets, we have locations of
+         * luma_interp_points relative to the image.
+         */
+        luma_interp_point_left += (int32) (iBuf->roi.x);
+        luma_interp_point_right += (int32) (iBuf->roi.x);
+        luma_interp_point_top += (int32) (iBuf->roi.y);
+        luma_interp_point_bottom += (int32) (iBuf->roi.y);
+
+        /*
+         * After adding the ROI offsets, we have locations of
+         * chroma_interp_points relative to the image.
+         */
+        chroma_interp_point_left = luma_interp_point_left;
+        chroma_interp_point_right = luma_interp_point_right;
+        chroma_interp_point_top = luma_interp_point_top;
+        chroma_interp_point_bottom = luma_interp_point_bottom;
+
+        chroma_edge_enable = TRUE;
+        /* find out which chroma pixels are needed for chroma upsampling. */
+        switch (iBuf->mdpImg.imgType) {
+                /*
+                 * cosite in horizontal axis
+                 * fully sampled in vertical axis
+                 */
+        case MDP_Y_CBCR_H2V1:
+        case MDP_Y_CRCB_H2V1:
+        case MDP_YCRYCB_H2V1:
+                /* floor( luma_interp_point_left / 2 ); */
+                chroma_interp_point_left = luma_interp_point_left >> 1;
+                /* floor( ( luma_interp_point_right + 1 ) / 2 ); */
+                chroma_interp_point_right = (luma_interp_point_right + 1) >> 1;
+
+                chroma_interp_point_top = luma_interp_point_top;
+                chroma_interp_point_bottom = luma_interp_point_bottom;
+                break;
+
+                /*
+                 * cosite in horizontal axis
+                 * offsite in vertical axis
+                 */
+        case MDP_Y_CBCR_H2V2:
+        case MDP_Y_CRCB_H2V2:
+                /* floor( luma_interp_point_left / 2) */
+                chroma_interp_point_left = luma_interp_point_left >> 1;
+
+                /* floor( ( luma_interp_point_right + 1 )/ 2 ) */
+                chroma_interp_point_right = (luma_interp_point_right + 1) >> 1;
+
+                /* floor( (luma_interp_point_top - 1 ) / 2 ) */
+                chroma_interp_point_top = (luma_interp_point_top - 1) >> 1;
+
+                /* floor( ( luma_interp_point_bottom + 1 ) / 2 ) */
+                chroma_interp_point_bottom =
+                    (luma_interp_point_bottom + 1) >> 1;
+
+                _is_yuv_offsite_vertical = 1;
+                break;
+
+        default:
+                chroma_edge_enable = FALSE;
+                chroma_interp_point_left = luma_interp_point_left;
+                chroma_interp_point_right = luma_interp_point_right;
+                chroma_interp_point_top = luma_interp_point_top;
+                chroma_interp_point_bottom = luma_interp_point_bottom;
+
+                break;
+        }
+
+        /* only if the image type is in YUV domain, we calculate chroma edge */
+        if (chroma_edge_enable) {
+                /* Defines which chroma pixels belongs to the roi */
+                switch (iBuf->mdpImg.imgType) {
+                        /*
+                         * Cosite in horizontal direction, and fully sampled
+                         * in vertical direction.
+                         */
+                case MDP_Y_CBCR_H2V1:
+                case MDP_Y_CRCB_H2V1:
+                case MDP_YCRYCB_H2V1:
+                        /*
+                         * width of chroma ROI is 1/2 of size of luma ROI
+                         * height of chroma ROI same as size of luma ROI
+                         */
+                        chroma_bound_left = iBuf->roi.x / 2;
+
+                        /* there are half as many chroma pixel as luma pixels */
+                        chroma_bound_right =
+                            (iBuf->roi.width + iBuf->roi.x - 1) / 2;
+                        chroma_bound_top = iBuf->roi.y;
+                        chroma_bound_bottom =
+                            (iBuf->roi.height + iBuf->roi.y - 1);
+                        break;
+
+                case MDP_Y_CBCR_H2V2:
+                case MDP_Y_CRCB_H2V2:
+                        /*
+                         * cosite in horizontal dir, and offsite in vertical dir
+                         * width of chroma ROI is 1/2 of size of luma ROI
+                         * height of chroma ROI is 1/2 of size of luma ROI
+                         */
+
+                        chroma_bound_left = iBuf->roi.x / 2;
+                        chroma_bound_right =
+                            (iBuf->roi.width + iBuf->roi.x - 1) / 2;
+                        chroma_bound_top = iBuf->roi.y / 2;
+                        chroma_bound_bottom =
+                            (iBuf->roi.height + iBuf->roi.y - 1) / 2;
+                        break;
+
+                default:
+                        /*
+                         * If no valid chroma sub-sampling format specified,
+                         * assume 4:4:4 ( i.e. fully sampled).  Set ROI
+                         * boundaries for chroma same as ROI boundaries for
+                         * luma.
+                         */
+                        chroma_bound_left = iBuf->roi.x;
+                        chroma_bound_right = iBuf->roi.width + iBuf->roi.x - 1;
+                        chroma_bound_top = iBuf->roi.y;
+                        chroma_bound_bottom =
+                            (iBuf->roi.height + iBuf->roi.y - 1);
+                        break;
+                }
+
+                /*
+                 * Knowing which chroma pixels are needed, and which chroma
+                 * pixels belong to the ROI (i.e. available for fetching ),
+                 * calculate how many chroma pixels Tile Fetch needs to
+                 * duplicate.  If any required chroma pixels falls outside
+                 * of the ROI, Tile Fetch must obtain them by replicating
+                 * pixels.
+                 */
+                if (chroma_bound_left > chroma_interp_point_left)
+                        chroma_repeat_left =
+                            chroma_bound_left - chroma_interp_point_left;
+                else
+                        chroma_repeat_left = 0;
+
+                if (chroma_interp_point_right > chroma_bound_right)
+                        chroma_repeat_right =
+                            chroma_interp_point_right - chroma_bound_right;
+                else
+                        chroma_repeat_right = 0;
+
+                if (chroma_bound_top > chroma_interp_point_top)
+                        chroma_repeat_top =
+                            chroma_bound_top - chroma_interp_point_top;
+                else
+                        chroma_repeat_top = 0;
+
+                if (chroma_interp_point_bottom > chroma_bound_bottom)
+                        chroma_repeat_bottom =
+                            chroma_interp_point_bottom - chroma_bound_bottom;
+                else
+                        chroma_repeat_bottom = 0;
+
+                if (_is_scale_enabled && (iBuf->roi.height == 1)
+                    && _is_yuv_offsite_vertical) {
+                        chroma_repeat_bottom = 3;
+                        chroma_repeat_top = 0;
+                }
+        }
+        /* make sure chroma repeats are non-negative */
+        if ((chroma_repeat_left < 0) || (chroma_repeat_right < 0) ||
+            (chroma_repeat_top < 0) || (chroma_repeat_bottom < 0))
+                return -1;
+
+        /* make sure chroma repeats are no larger than 3 pixels */
+        if ((chroma_repeat_left > 3) || (chroma_repeat_right > 3) ||
+            (chroma_repeat_top > 3) || (chroma_repeat_bottom > 3))
+                return -1;
+
+        /* make sure luma repeats are non-negative */
+        if ((luma_repeat_left < 0) || (luma_repeat_right < 0) ||
+            (luma_repeat_top < 0) || (luma_repeat_bottom < 0))
+                return -1;
+
+        /* make sure luma repeats are no larger than 3 pixels */
+        if ((luma_repeat_left > 3) || (luma_repeat_right > 3) ||
+            (luma_repeat_top > 3) || (luma_repeat_bottom > 3))
+                return -1;
+
+        /* write chroma_repeat_left to register */
+        reg |= (chroma_repeat_left & 3) << MDP_LEFT_CHROMA;
+
+        /* write chroma_repeat_right to register */
+        reg |= (chroma_repeat_right & 3) << MDP_RIGHT_CHROMA;
+
+        /* write chroma_repeat_top to register */
+        reg |= (chroma_repeat_top & 3) << MDP_TOP_CHROMA;
+
+        /* write chroma_repeat_bottom to register */
+        reg |= (chroma_repeat_bottom & 3) << MDP_BOTTOM_CHROMA;
+
+        /* write luma_repeat_left to register */
+        reg |= (luma_repeat_left & 3) << MDP_LEFT_LUMA;
+
+        /* write luma_repeat_right to register */
+        reg |= (luma_repeat_right & 3) << MDP_RIGHT_LUMA;
+
+        /* write luma_repeat_top to register */
+        reg |= (luma_repeat_top & 3) << MDP_TOP_LUMA;
+
+        /* write luma_repeat_bottom to register */
+        reg |= (luma_repeat_bottom & 3) << MDP_BOTTOM_LUMA;
+
+        /* done with reg */
+        *dup = reg;
+
+        /* bg edge duplicate */
+        reg = 0x0;
+
+        switch (iBuf->ibuf_type) {
+        case MDP_Y_CBCR_H2V2:
+        case MDP_Y_CRCB_H2V2:
+                /*
+                 * Edge condition for MDP_Y_CRCB/CBCR_H2V2 cosite only.
+                 * For 420 cosite, 1 chroma replicated on all sides except
+                 * left, so reg 101b8 should be 0x0209. For 420 offsite,
+                 * 1 chroma replicated all sides.
+                 */
+                if (iBuf->roi.lcd_y == 0) {
+                        reg |= BIT(MDP_TOP_CHROMA);
+                }
+
+                if ((iBuf->roi.lcd_y + iBuf->roi.dst_height) ==
+                    iBuf->ibuf_height) {
+                        reg |= BIT(MDP_BOTTOM_CHROMA);
+                }
+
+                if (((iBuf->roi.lcd_x + iBuf->roi.dst_width) ==
+                     iBuf->ibuf_width) && ((iBuf->roi.dst_width % 2) == 0)) {
+                        reg |= BIT(MDP_RIGHT_CHROMA);
+                }
+
+                break;
+
+        case MDP_Y_CBCR_H2V1:
+        case MDP_Y_CRCB_H2V1:
+        case MDP_YCRYCB_H2V1:
+                if (((iBuf->roi.lcd_x + iBuf->roi.dst_width) ==
+                     iBuf->ibuf_width) && ((iBuf->roi.dst_width % 2) == 0)) {
+                        reg |= BIT(MDP_RIGHT_CHROMA);
+                }
+                break;
+        default:
+                break;
+        }
+
+        *dup2 = reg;
+
+        return 0;
+}
+
+#define ADJUST_IP               /* for 1/3 scale factor fix */
+
+static int mdp_calc_scale_params(
+/* ROI origin coordinate for the dimension */
+                                        uint32 org,
+/* src ROI dimension */
+                                        uint32 dim_in,
+/* scaled ROI dimension*/
+                                        uint32 dim_out,
+/* is this ROI width dimension? */
+                                        boolean is_W,
+/* initial phase location address */
+                                        int32 *phase_init_ptr,
+/* phase increment location address */
+                                        uint32 *phase_step_ptr,
+/* ROI start over-fetch location address */
+                                        uint32 *num_repl_beg_ptr,
+/* ROI end over-fetch location address */
+                                        uint32 *num_repl_end_ptr)
+{
+        boolean rpa_on = FALSE;
+        int init_phase = 0;
+        uint32 beg_of = 0;
+        uint32 end_of = 0;
+        uint64 numer = 0;
+        uint64 denom = 0;
+        /*uint64 inverter = 1; */
+        int64 point5 = 1;
+        int64 one = 1;
+        int64 k1, k2, k3, k4;   /* linear equation coefficients */
+        uint64 int_mask;
+        uint64 fract_mask;
+        uint64 Os;
+        int64 Osprime;
+        int64 Od;
+        int64 Odprime;
+        int64 Oreq;
+        uint64 Es;
+        uint64 Ed;
+        uint64 Ereq;
+#ifdef ADJUST_IP
+        int64 IP64;
+        int64 delta;
+#endif
+        uint32 mult;
+
+        /*
+         * The phase accumulator should really be rational for all cases in a
+         * general purpose polyphase scaler for a tiled architecture with
+         * non-zero * origin capability because there is no way to represent
+         * certain scale factors in fixed point regardless of precision.
+         * The error incurred in attempting to use fixed point is most
+         * eggregious for SF where 1/SF is an integral multiple of 1/3.
+         *
+         * However, since the MDP2 has already been committed to HW, we
+         * only use the rational phase accumulator (RPA) when 1/SF is an
+         * integral multiple of 1/3.  This will help minimize regressions in
+         * matching the HW to the C-Sim.
+         */
+        /*
+         * Set the RPA flag for this dimension.
+         *
+         * In order for 1/SF (dim_in/dim_out) to be an integral multiple of
+         * 1/3, dim_out must be an integral multiple of 3.
+         */
+        if (!(dim_out % 3)) {
+                mult = dim_out / 3;
+                rpa_on = (!(dim_in % mult));
+        }
+
+        numer = dim_out;
+        denom = dim_in;
+
+        /*
+         * convert to U30.34 before division
+         *
+         * The K vectors carry 4 extra bits of precision
+         * and are rounded.
+         *
+         * We initially go 5 bits over then round by adding
+         * 1 and right shifting by 1
+         * so final result is U31.33
+         */
+        numer <<= PQF_PLUS_5;
+
+        /* now calculate the scale factor (aka k3) */
+        k3 = ((mdp_do_div(numer, denom) + 1) >> 1);
+
+        /* check scale factor for legal range [0.25 - 4.0] */
+        if (((k3 >> 4) < (1LL << PQF_MINUS_2)) ||
+            ((k3 >> 4) > (1LL << PQF_PLUS_2))) {
+                return -1;
+        }
+
+        /* calculate inverse scale factor (aka k1) for phase init */
+        numer = dim_in;
+        denom = dim_out;
+        numer <<= PQF_PLUS_5;
+        k1 = ((mdp_do_div(numer, denom) + 1) >> 1);
+
+        /*
+         * calculate initial phase and ROI overfetch
+         */
+        /* convert point5 & one to S39.24 (will always be positive) */
+        point5 <<= (PQF_PLUS_4 - 1);
+        one <<= PQF_PLUS_4;
+        k2 = ((k1 - one) >> 1);
+        init_phase = (int)(k2 >> 4);
+        k4 = ((k3 - one) >> 1);
+        if (k3 == one) {
+                /* the simple case; SF = 1.0 */
+                beg_of = 1;
+                end_of = 2;
+        } else {
+                /* calculate the masks */
+                fract_mask = one - 1;
+                int_mask = ~fract_mask;
+
+                if (!rpa_on) {
+                        /*
+                         * FIXED POINT IMPLEMENTATION
+                         */
+                        if (!org) {
+                                /* A fairly simple case; ROI origin = 0 */
+                                if (k1 < one) {
+                                        /* upscaling */
+                                        beg_of = end_of = 2;
+                                }
+                                /* 0.33 <= SF < 1.0 */
+                                else if (k1 < (3LL << PQF_PLUS_4))
+                                        beg_of = end_of = 1;
+                                /* 0.33 == SF */
+                                else if (k1 == (3LL << PQF_PLUS_4)) {
+                                        beg_of = 0;
+                                        end_of = 1;
+                                }
+                                /* 0.25 <= SF < 0.33 */
+                                else
+                                        beg_of = end_of = 0;
+                        } else {
+                                /*
+                                 * The complicated case; ROI origin != 0
+                                 * init_phase needs to be adjusted
+                                 * OF is also position dependent
+                                 */
+
+                                /* map (org - .5) into destination space */
+                                Os = ((uint64) org << 1) - 1;
+                                Od = ((k3 * Os) >> 1) + k4;
+
+                                /* take the ceiling */
+                                Odprime = (Od & int_mask);
+                                if (Odprime != Od)
+                                        Odprime += one;
+
+                                /* now map that back to source space */
+                                Osprime = (k1 * (Odprime >> PQF_PLUS_4)) + k2;
+
+                                /* then floor & decrement to calculate the required
+                                   starting coordinate */
+                                Oreq = (Osprime & int_mask) - one;
+
+                                /* calculate end coord in destination space then map to
+                                   source space */
+                                Ed = Odprime +
+                                    ((uint64) dim_out << PQF_PLUS_4) - one;
+                                Es = (k1 * (Ed >> PQF_PLUS_4)) + k2;
+
+                                /* now floor & increment by 2 to calculate the required
+                                   ending coordinate */
+                                Ereq = (Es & int_mask) + (one << 1);
+
+                                /* calculate initial phase */
+#ifdef ADJUST_IP
+
+                                IP64 = Osprime - Oreq;
+                                delta = ((int64) (org) << PQF_PLUS_4) - Oreq;
+                                IP64 -= delta;
+
+                                /* limit to valid range before the left shift */
+                                delta = (IP64 & (1LL << 63)) ? 4 : -4;
+                                delta <<= PQF_PLUS_4;
+                                while (abs((int)(IP64 >> PQF_PLUS_4)) > 4)
+                                        IP64 += delta;
+
+                                /* right shift to account for extra bits of precision */
+                                init_phase = (int)(IP64 >> 4);
+
+#else /* ADJUST_IP */
+
+                                /* just calculate the real initial phase */
+                                init_phase = (int)((Osprime - Oreq) >> 4);
+
+#endif /* ADJUST_IP */
+
+                                /* calculate the overfetch */
+                                beg_of = org - (uint32) (Oreq >> PQF_PLUS_4);
+                                end_of =
+                                    (uint32) (Ereq >> PQF_PLUS_4) - (org +
+                                                                     dim_in -
+                                                                     1);
+                        }
+                } else {
+                        /*
+                         * RPA IMPLEMENTATION
+                         *
+                         * init_phase needs to be calculated in all RPA_on cases
+                         * because it's a numerator, not a fixed point value.
+                         */
+
+                        /* map (org - .5) into destination space */
+                        Os = ((uint64) org << PQF_PLUS_4) - point5;
+                        Od = mdp_do_div((dim_out * (Os + point5)),
+                                        dim_in) - point5;
+
+                        /* take the ceiling */
+                        Odprime = (Od & int_mask);
+                        if (Odprime != Od)
+                                Odprime += one;
+
+                        /* now map that back to source space */
+                        Osprime =
+                            mdp_do_div((dim_in * (Odprime + point5)),
+                                       dim_out) - point5;
+
+                        /* then floor & decrement to calculate the required
+                           starting coordinate */
+                        Oreq = (Osprime & int_mask) - one;
+
+                        /* calculate end coord in destination space then map to
+                           source space */
+                        Ed = Odprime + ((uint64) dim_out << PQF_PLUS_4) - one;
+                        Es = mdp_do_div((dim_in * (Ed + point5)),
+                                        dim_out) - point5;
+
+                        /* now floor & increment by 2 to calculate the required
+                           ending coordinate */
+                        Ereq = (Es & int_mask) + (one << 1);
+
+                        /* calculate initial phase */
+
+#ifdef ADJUST_IP
+
+                        IP64 = Osprime - Oreq;
+                        delta = ((int64) (org) << PQF_PLUS_4) - Oreq;
+                        IP64 -= delta;
+
+                        /* limit to valid range before the left shift */
+                        delta = (IP64 & (1LL << 63)) ? 4 : -4;
+                        delta <<= PQF_PLUS_4;
+                        while (abs((int)(IP64 >> PQF_PLUS_4)) > 4)
+                                IP64 += delta;
+
+                        /* right shift to account for extra bits of precision */
+                        init_phase = (int)(IP64 >> 4);
+
+#else /* ADJUST_IP */
+
+                        /* just calculate the real initial phase */
+                        init_phase = (int)((Osprime - Oreq) >> 4);
+
+#endif /* ADJUST_IP */
+
+                        /* calculate the overfetch */
+                        beg_of = org - (uint32) (Oreq >> PQF_PLUS_4);
+                        end_of =
+                            (uint32) (Ereq >> PQF_PLUS_4) - (org + dim_in - 1);
+                }
+        }
+
+        /* return the scale parameters */
+        *phase_init_ptr = init_phase;
+        *phase_step_ptr = (uint32) (k1 >> 4);
+        *num_repl_beg_ptr = beg_of;
+        *num_repl_end_ptr = end_of;
+
+        return 0;
+}
+
+static uint8 *mdp_adjust_rot_addr(MDPIBUF *iBuf, uint8 *addr, uint32 uv)
+{
+        uint32 dest_ystride = iBuf->ibuf_width * iBuf->bpp;
+        uint32 h_slice = 1;
+
+        if (uv && ((iBuf->ibuf_type == MDP_Y_CBCR_H2V2) ||
+                (iBuf->ibuf_type == MDP_Y_CRCB_H2V2)))
+                h_slice = 2;
+
+        if (MDP_CHKBIT(iBuf->mdpImg.mdpOp, MDPOP_ROT90) ^
+            MDP_CHKBIT(iBuf->mdpImg.mdpOp, MDPOP_LR)) {
+                addr =
+                    addr + (iBuf->roi.dst_width -
+                            MIN(16, iBuf->roi.dst_width)) * iBuf->bpp;
+        }
+        if (MDP_CHKBIT(iBuf->mdpImg.mdpOp, MDPOP_UD)) {
+                addr =
+                    addr + ((iBuf->roi.dst_height -
+                        MIN(16, iBuf->roi.dst_height))/h_slice) * dest_ystride;
+        }
+
+        return addr;
+}
+
+void mdp_set_scale(MDPIBUF *iBuf,
+                   uint32 dst_roi_width,
+                   uint32 dst_roi_height,
+                   boolean inputRGB, boolean outputRGB, uint32 *pppop_reg_ptr)
+{
+        uint32 dst_roi_width_scale;
+        uint32 dst_roi_height_scale;
+        boolean use_pr;
+        uint32 phasex_step = 0;
+        uint32 phasey_step = 0;
+        int32 phasex_init = 0;
+        int32 phasey_init = 0;
+        uint32 lines_dup = 0;
+        uint32 lines_dup_bg = 0;
+        uint32 dummy;
+        uint32 mdp_blur = 0;
+
+        if (iBuf->mdpImg.mdpOp & MDPOP_ASCALE) {
+                if (iBuf->mdpImg.mdpOp & MDPOP_ROT90) {
+                        dst_roi_width_scale = dst_roi_height;
+                        dst_roi_height_scale = dst_roi_width;
+                } else {
+                        dst_roi_width_scale = dst_roi_width;
+                        dst_roi_height_scale = dst_roi_height;
+                }
+
+                mdp_blur = iBuf->mdpImg.mdpOp & MDPOP_BLUR;
+
+                if ((dst_roi_width_scale != iBuf->roi.width) ||
+                    (dst_roi_height_scale != iBuf->roi.height) ||
+                        mdp_blur) {
+                        *pppop_reg_ptr |=
+                            (PPP_OP_SCALE_Y_ON | PPP_OP_SCALE_X_ON);
+
+                /* let's use SHIM logic to calculate the partial ROI scaling */
+#if 0
+                        phasex_step =
+                            (uint32) mdp_do_div(0x20000000 * iBuf->roi.width,
+                                                dst_roi_width_scale);
+                        phasey_step =
+                            (uint32) mdp_do_div(0x20000000 * iBuf->roi.height,
+                                                dst_roi_height_scale);
+
+/*
+    phasex_step= ((long long) iBuf->roi.width * 0x20000000)/dst_roi_width_scale;
+    phasey_step= ((long long)iBuf->roi.height * 0x20000000)/dst_roi_height_scale;
+*/
+
+                        phasex_init =
+                            (((long long)phasex_step - 0x20000000) >> 1);
+                        phasey_init =
+                            (((long long)phasey_step - 0x20000000) >> 1);
+
+#else
+                        mdp_calc_scale_params(iBuf->roi.x, iBuf->roi.width,
+                                              dst_roi_width_scale, 1,
+                                              &phasex_init, &phasex_step,
+                                              &dummy, &dummy);
+                        mdp_calc_scale_params(iBuf->roi.y, iBuf->roi.height,
+                                              dst_roi_height_scale, 0,
+                                              &phasey_init, &phasey_step,
+                                              &dummy, &dummy);
+#endif
+                        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x013c,
+                                 phasex_init);
+                        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0140,
+                                 phasey_init);
+                        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0144,
+                                 phasex_step);
+                        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0148,
+                                 phasey_step);
+
+                        use_pr = (inputRGB) && (outputRGB);
+
+                        if ((dst_roi_width_scale > iBuf->roi.width) ||
+                            (dst_roi_height_scale > iBuf->roi.height)) {
+                                if ((use_pr)
+                                    && (mdp_curr_up_scale_xy !=
+                                        MDP_PR_SCALE_UP)) {
+                                        mdp_load_pr_upscale_table();
+                                        mdp_curr_up_scale_xy = MDP_PR_SCALE_UP;
+                                } else if ((!use_pr)
+                                           && (mdp_curr_up_scale_xy !=
+                                               MDP_BC_SCALE_UP)) {
+                                        mdp_load_bc_upscale_table();
+                                        mdp_curr_up_scale_xy = MDP_BC_SCALE_UP;
+                                }
+                        }
+
+                        if (mdp_blur) {
+                                load_scale_table(mdp_gaussian_blur_table,
+                                        ARRAY_SIZE(mdp_gaussian_blur_table));
+                                mdp_curr_down_scale_x = MDP_SCALE_BLUR;
+                                mdp_curr_down_scale_y = MDP_SCALE_BLUR;
+                        }
+
+                        /* 0.2 < x <= 1 scaling factor */
+                        if ((dst_roi_width_scale <= iBuf->roi.width) &&
+                                !mdp_blur) {
+                                if (((dst_roi_width_scale * 10) /
+                                     iBuf->roi.width) > 8) {
+                                        if ((use_pr)
+                                            && (mdp_curr_down_scale_x !=
+                                                MDP_PR_SCALE_POINT8_1)) {
+                                                mdp_load_pr_downscale_table_x_point8TO1
+                                                    ();
+                                                mdp_curr_down_scale_x =
+                                                    MDP_PR_SCALE_POINT8_1;
+                                        } else if ((!use_pr)
+                                                   && (mdp_curr_down_scale_x !=
+                                                       MDP_BC_SCALE_POINT8_1)) {
+                                                mdp_load_bc_downscale_table_x_point8TO1
+                                                    ();
+                                                mdp_curr_down_scale_x =
+                                                    MDP_BC_SCALE_POINT8_1;
+                                        }
+                                } else
+                                    if (((dst_roi_width_scale * 10) /
+                                         iBuf->roi.width) > 6) {
+                                        if ((use_pr)
+                                            && (mdp_curr_down_scale_x !=
+                                                MDP_PR_SCALE_POINT6_POINT8)) {
+                                                mdp_load_pr_downscale_table_x_point6TOpoint8
+                                                    ();
+                                                mdp_curr_down_scale_x =
+                                                    MDP_PR_SCALE_POINT6_POINT8;
+                                        } else if ((!use_pr)
+                                                   && (mdp_curr_down_scale_x !=
+                                                       MDP_BC_SCALE_POINT6_POINT8))
+                                        {
+                                                mdp_load_bc_downscale_table_x_point6TOpoint8
+                                                    ();
+                                                mdp_curr_down_scale_x =
+                                                    MDP_BC_SCALE_POINT6_POINT8;
+                                        }
+                                } else
+                                    if (((dst_roi_width_scale * 10) /
+                                         iBuf->roi.width) > 4) {
+                                        if ((use_pr)
+                                            && (mdp_curr_down_scale_x !=
+                                                MDP_PR_SCALE_POINT4_POINT6)) {
+                                                mdp_load_pr_downscale_table_x_point4TOpoint6
+                                                    ();
+                                                mdp_curr_down_scale_x =
+                                                    MDP_PR_SCALE_POINT4_POINT6;
+                                        } else if ((!use_pr)
+                                                   && (mdp_curr_down_scale_x !=
+                                                       MDP_BC_SCALE_POINT4_POINT6))
+                                        {
+                                                mdp_load_bc_downscale_table_x_point4TOpoint6
+                                                    ();
+                                                mdp_curr_down_scale_x =
+                                                    MDP_BC_SCALE_POINT4_POINT6;
+                                        }
+                                } else {
+                                        if ((use_pr)
+                                            && (mdp_curr_down_scale_x !=
+                                                MDP_PR_SCALE_POINT2_POINT4)) {
+                                                mdp_load_pr_downscale_table_x_point2TOpoint4
+                                                    ();
+                                                mdp_curr_down_scale_x =
+                                                    MDP_PR_SCALE_POINT2_POINT4;
+                                        } else if ((!use_pr)
+                                                   && (mdp_curr_down_scale_x !=
+                                                       MDP_BC_SCALE_POINT2_POINT4))
+                                        {
+                                                mdp_load_bc_downscale_table_x_point2TOpoint4
+                                                    ();
+                                                mdp_curr_down_scale_x =
+                                                    MDP_BC_SCALE_POINT2_POINT4;
+                                        }
+                                }
+                        }
+                        /* 0.2 < y <= 1 scaling factor */
+                        if ((dst_roi_height_scale <= iBuf->roi.height) &&
+                                !mdp_blur) {
+                                if (((dst_roi_height_scale * 10) /
+                                     iBuf->roi.height) > 8) {
+                                        if ((use_pr)
+                                            && (mdp_curr_down_scale_y !=
+                                                MDP_PR_SCALE_POINT8_1)) {
+                                                mdp_load_pr_downscale_table_y_point8TO1
+                                                    ();
+                                                mdp_curr_down_scale_y =
+                                                    MDP_PR_SCALE_POINT8_1;
+                                        } else if ((!use_pr)
+                                                   && (mdp_curr_down_scale_y !=
+                                                       MDP_BC_SCALE_POINT8_1)) {
+                                                mdp_load_bc_downscale_table_y_point8TO1
+                                                    ();
+                                                mdp_curr_down_scale_y =
+                                                    MDP_BC_SCALE_POINT8_1;
+                                        }
+                                } else
+                                    if (((dst_roi_height_scale * 10) /
+                                         iBuf->roi.height) > 6) {
+                                        if ((use_pr)
+                                            && (mdp_curr_down_scale_y !=
+                                                MDP_PR_SCALE_POINT6_POINT8)) {
+                                                mdp_load_pr_downscale_table_y_point6TOpoint8
+                                                    ();
+                                                mdp_curr_down_scale_y =
+                                                    MDP_PR_SCALE_POINT6_POINT8;
+                                        } else if ((!use_pr)
+                                                   && (mdp_curr_down_scale_y !=
+                                                       MDP_BC_SCALE_POINT6_POINT8))
+                                        {
+                                                mdp_load_bc_downscale_table_y_point6TOpoint8
+                                                    ();
+                                                mdp_curr_down_scale_y =
+                                                    MDP_BC_SCALE_POINT6_POINT8;
+                                        }
+                                } else
+                                    if (((dst_roi_height_scale * 10) /
+                                         iBuf->roi.height) > 4) {
+                                        if ((use_pr)
+                                            && (mdp_curr_down_scale_y !=
+                                                MDP_PR_SCALE_POINT4_POINT6)) {
+                                                mdp_load_pr_downscale_table_y_point4TOpoint6
+                                                    ();
+                                                mdp_curr_down_scale_y =
+                                                    MDP_PR_SCALE_POINT4_POINT6;
+                                        } else if ((!use_pr)
+                                                   && (mdp_curr_down_scale_y !=
+                                                       MDP_BC_SCALE_POINT4_POINT6))
+                                        {
+                                                mdp_load_bc_downscale_table_y_point4TOpoint6
+                                                    ();
+                                                mdp_curr_down_scale_y =
+                                                    MDP_BC_SCALE_POINT4_POINT6;
+                                        }
+                                } else {
+                                        if ((use_pr)
+                                            && (mdp_curr_down_scale_y !=
+                                                MDP_PR_SCALE_POINT2_POINT4)) {
+                                                mdp_load_pr_downscale_table_y_point2TOpoint4
+                                                    ();
+                                                mdp_curr_down_scale_y =
+                                                    MDP_PR_SCALE_POINT2_POINT4;
+                                        } else if ((!use_pr)
+                                                   && (mdp_curr_down_scale_y !=
+                                                       MDP_BC_SCALE_POINT2_POINT4))
+                                        {
+                                                mdp_load_bc_downscale_table_y_point2TOpoint4
+                                                    ();
+                                                mdp_curr_down_scale_y =
+                                                    MDP_BC_SCALE_POINT2_POINT4;
+                                        }
+                                }
+                        }
+                } else {
+                        iBuf->mdpImg.mdpOp &= ~(MDPOP_ASCALE);
+                }
+        }
+        /* setting edge condition here after scaling check */
+        if (mdp_get_edge_cond(iBuf, &lines_dup, &lines_dup_bg))
+                printk(KERN_ERR "msm_fb: mdp_get_edge_cond() error!\n");
+
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01b8, lines_dup);
+        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x01bc, lines_dup_bg);
+}
+
+void mdp_init_scale_table(void)
+{
+        mdp_curr_up_scale_xy = MDP_INIT_SCALE;
+        mdp_curr_down_scale_x = MDP_INIT_SCALE;
+        mdp_curr_down_scale_y = MDP_INIT_SCALE;
+}
+
+void mdp_adjust_start_addr(uint8 **src0,
+                           uint8 **src1,
+                           int v_slice,
+                           int h_slice,
+                           int x,
+                           int y,
+                           uint32 width,
+                           uint32 height, int bpp, MDPIBUF *iBuf, int layer)
+{
+        *src0 += (x + y * width) * bpp;
+
+        /* if it's dest/bg buffer, we need to adjust it for rotation */
+        if (layer != 0)
+                *src0 = mdp_adjust_rot_addr(iBuf, *src0, 0);
+
+        if (*src1) {
+                /*
+                 * MDP_Y_CBCR_H2V2/MDP_Y_CRCB_H2V2 cosite for now
+                 * we need to shift x direction same as y dir for offsite
+                 */
+                *src1 +=
+                    ((x / h_slice) * h_slice +
+                     ((y == 0) ? 0 : ((y + 1) / v_slice - 1) * width)) * bpp;
+
+                /* if it's dest/bg buffer, we need to adjust it for rotation */
+                if (layer != 0)
+                        *src1 = mdp_adjust_rot_addr(iBuf, *src1, 1);
+        }
+}
+
+void mdp_set_blend_attr(MDPIBUF *iBuf,
+                        uint32 *alpha,
+                        uint32 *tpVal,
+                        uint32 perPixelAlpha, uint32 *pppop_reg_ptr)
+{
+        if (perPixelAlpha) {
+                *pppop_reg_ptr |= PPP_OP_ROT_ON |
+                    PPP_OP_BLEND_ON | PPP_OP_BLEND_SRCPIXEL_ALPHA;
+        } else {
+                if ((iBuf->mdpImg.mdpOp & MDPOP_ALPHAB)
+                    && (iBuf->mdpImg.alpha == 0xff)) {
+                        iBuf->mdpImg.mdpOp &= ~(MDPOP_ALPHAB);
+                }
+
+                if ((iBuf->mdpImg.mdpOp & MDPOP_ALPHAB)
+                    && (iBuf->mdpImg.mdpOp & MDPOP_TRANSP)) {
+                        *pppop_reg_ptr |=
+                            PPP_OP_ROT_ON | PPP_OP_BLEND_ON |
+                            PPP_OP_BLEND_CONSTANT_ALPHA |
+                            PPP_OP_BLEND_ALPHA_BLEND_NORMAL |
+                            PPP_BLEND_CALPHA_TRNASP;
+
+                        *alpha = iBuf->mdpImg.alpha;
+                        *tpVal = iBuf->mdpImg.tpVal;
+                } else {
+                        if (iBuf->mdpImg.mdpOp & MDPOP_TRANSP) {
+                                *pppop_reg_ptr |= PPP_OP_ROT_ON |
+                                    PPP_OP_BLEND_ON |
+                                    PPP_OP_BLEND_SRCPIXEL_TRANSP;
+                                *tpVal = iBuf->mdpImg.tpVal;
+                        } else if (iBuf->mdpImg.mdpOp & MDPOP_ALPHAB) {
+                                *pppop_reg_ptr |= PPP_OP_ROT_ON |
+                                    PPP_OP_BLEND_ON |
+                                    PPP_OP_BLEND_ALPHA_BLEND_NORMAL |
+                                    PPP_OP_BLEND_CONSTANT_ALPHA;
+                                *alpha = iBuf->mdpImg.alpha;
+                        }
+                }
+        }
+}
diff --git a/drivers/staging/msm/mdp_ppp_v31.c b/drivers/staging/msm/mdp_ppp_v31.c
new file mode 100644
index 000000000000..76495dbe4e64
--- /dev/null
+++ b/drivers/staging/msm/mdp_ppp_v31.c
@@ -0,0 +1,828 @@
+/* Copyright (c) 2008-2010, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/fb.h>
+#include "linux/proc_fs.h"
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <asm/div64.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+
+#define MDP_SCALE_COEFF_NUM      32
+#define MDP_SCALE_0P2_TO_0P4_INDEX 0
+#define MDP_SCALE_0P4_TO_0P6_INDEX 32
+#define MDP_SCALE_0P6_TO_0P8_INDEX 64
+#define MDP_SCALE_0P8_TO_8P0_INDEX 96
+#define MDP_SCALE_COEFF_MASK 0x3ff
+
+#define MDP_SCALE_PR  0
+#define MDP_SCALE_FIR 1
+
+static uint32 mdp_scale_0p8_to_8p0_mode;
+static uint32 mdp_scale_0p6_to_0p8_mode;
+static uint32 mdp_scale_0p4_to_0p6_mode;
+static uint32 mdp_scale_0p2_to_0p4_mode;
+
+/* -------- All scaling range, "pixel repeat" -------- */
+static int16 mdp_scale_pixel_repeat_C0[MDP_SCALE_COEFF_NUM] = {
+        0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0
+};
+
+static int16 mdp_scale_pixel_repeat_C1[MDP_SCALE_COEFF_NUM] = {
+        511, 511, 511, 511, 511, 511, 511, 511,
+        511, 511, 511, 511, 511, 511, 511, 511,
+        511, 511, 511, 511, 511, 511, 511, 511,
+        511, 511, 511, 511, 511, 511, 511, 511
+};
+
+static int16 mdp_scale_pixel_repeat_C2[MDP_SCALE_COEFF_NUM] = {
+        0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0
+};
+
+static int16 mdp_scale_pixel_repeat_C3[MDP_SCALE_COEFF_NUM] = {
+        0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0
+};
+
+/* --------------------------- FIR ------------------------------------- */
+/* -------- Downscale, ranging from 0.8x to 8.0x of original size -------- */
+
+static int16 mdp_scale_0p8_to_8p0_C0[MDP_SCALE_COEFF_NUM] = {
+        0, -7, -13, -19, -24, -28, -32, -34, -37, -39,
+        -40, -41, -41, -41, -40, -40, -38, -37, -35, -33,
+        -31, -29, -26, -24, -21, -18, -15, -13, -10, -7,
+        -5, -2
+};
+
+static int16 mdp_scale_0p8_to_8p0_C1[MDP_SCALE_COEFF_NUM] = {
+        511, 507, 501, 494, 485, 475, 463, 450, 436, 422,
+        405, 388, 370, 352, 333, 314, 293, 274, 253, 233,
+        213, 193, 172, 152, 133, 113, 95, 77, 60, 43,
+        28, 13
+};
+
+static int16 mdp_scale_0p8_to_8p0_C2[MDP_SCALE_COEFF_NUM] = {
+        0, 13, 28, 43, 60, 77, 95, 113, 133, 152,
+        172, 193, 213, 233, 253, 274, 294, 314, 333, 352,
+        370, 388, 405, 422, 436, 450, 463, 475, 485, 494,
+        501, 507,
+};
+
+static int16 mdp_scale_0p8_to_8p0_C3[MDP_SCALE_COEFF_NUM] = {
+        0, -2, -5, -7, -10, -13, -15, -18, -21, -24,
+        -26, -29, -31, -33, -35, -37, -38, -40, -40, -41,
+        -41, -41, -40, -39, -37, -34, -32, -28, -24, -19,
+        -13, -7
+};
+
+/* -------- Downscale, ranging from 0.6x to 0.8x of original size -------- */
+
+static int16 mdp_scale_0p6_to_0p8_C0[MDP_SCALE_COEFF_NUM] = {
+        104, 96, 89, 82, 75, 68, 61, 55, 49, 43,
+        38, 33, 28, 24, 20, 16, 12, 9, 6, 4,
+        2, 0, -2, -4, -5, -6, -7, -7, -8, -8,
+        -8, -8
+};
+
+static int16 mdp_scale_0p6_to_0p8_C1[MDP_SCALE_COEFF_NUM] = {
+        303, 303, 302, 300, 298, 296, 293, 289, 286, 281,
+        276, 270, 265, 258, 252, 245, 238, 230, 223, 214,
+        206, 197, 189, 180, 172, 163, 154, 145, 137, 128,
+        120, 112
+};
+
+static int16 mdp_scale_0p6_to_0p8_C2[MDP_SCALE_COEFF_NUM] = {
+        112, 120, 128, 137, 145, 154, 163, 172, 180, 189,
+        197, 206, 214, 223, 230, 238, 245, 252, 258, 265,
+        270, 276, 281, 286, 289, 293, 296, 298, 300, 302,
+        303, 303
+};
+
+static int16 mdp_scale_0p6_to_0p8_C3[MDP_SCALE_COEFF_NUM] = {
+        -8, -8, -8, -8, -7, -7, -6, -5, -4, -2,
+        0, 2, 4, 6, 9, 12, 16, 20, 24, 28,
+        33, 38, 43, 49, 55, 61, 68, 75, 82, 89,
+        96, 104
+};
+
+/* -------- Downscale, ranging from 0.4x to 0.6x of original size -------- */
+
+static int16 mdp_scale_0p4_to_0p6_C0[MDP_SCALE_COEFF_NUM] = {
+        136, 132, 128, 123, 119, 115, 111, 107, 103, 98,
+        95, 91, 87, 84, 80, 76, 73, 69, 66, 62,
+        59, 57, 54, 50, 47, 44, 41, 39, 36, 33,
+        32, 29
+};
+
+static int16 mdp_scale_0p4_to_0p6_C1[MDP_SCALE_COEFF_NUM] = {
+        206, 205, 204, 204, 201, 200, 199, 197, 196, 194,
+        191, 191, 189, 185, 184, 182, 180, 178, 176, 173,
+        170, 168, 165, 162, 160, 157, 155, 152, 148, 146,
+        142, 140
+};
+
+static int16 mdp_scale_0p4_to_0p6_C2[MDP_SCALE_COEFF_NUM] = {
+        140, 142, 146, 148, 152, 155, 157, 160, 162, 165,
+        168, 170, 173, 176, 178, 180, 182, 184, 185, 189,
+        191, 191, 194, 196, 197, 199, 200, 201, 204, 204,
+        205, 206
+};
+
+static int16 mdp_scale_0p4_to_0p6_C3[MDP_SCALE_COEFF_NUM] = {
+        29, 32, 33, 36, 39, 41, 44, 47, 50, 54,
+        57, 59, 62, 66, 69, 73, 76, 80, 84, 87,
+        91, 95, 98, 103, 107, 111, 115, 119, 123, 128,
+        132, 136
+};
+
+/* -------- Downscale, ranging from 0.2x to 0.4x of original size -------- */
+
+static int16 mdp_scale_0p2_to_0p4_C0[MDP_SCALE_COEFF_NUM] = {
+        131, 131, 130, 129, 128, 127, 127, 126, 125, 125,
+        124, 123, 123, 121, 120, 119, 119, 118, 117, 117,
+        116, 115, 115, 114, 113, 112, 111, 110, 109, 109,
+        108, 107
+};
+
+static int16 mdp_scale_0p2_to_0p4_C1[MDP_SCALE_COEFF_NUM] = {
+        141, 140, 140, 140, 140, 139, 138, 138, 138, 137,
+        137, 137, 136, 137, 137, 137, 136, 136, 136, 135,
+        135, 135, 134, 134, 134, 134, 134, 133, 133, 132,
+        132, 132
+};
+
+static int16 mdp_scale_0p2_to_0p4_C2[MDP_SCALE_COEFF_NUM] = {
+        132, 132, 132, 133, 133, 134, 134, 134, 134, 134,
+        135, 135, 135, 136, 136, 136, 137, 137, 137, 136,
+        137, 137, 137, 138, 138, 138, 139, 140, 140, 140,
+        140, 141
+};
+
+static int16 mdp_scale_0p2_to_0p4_C3[MDP_SCALE_COEFF_NUM] = {
+        107, 108, 109, 109, 110, 111, 112, 113, 114, 115,
+        115, 116, 117, 117, 118, 119, 119, 120, 121, 123,
+        123, 124, 125, 125, 126, 127, 127, 128, 129, 130,
+        131, 131
+};
+
+static void mdp_update_scale_table(int index, int16 *c0, int16 *c1,
+                                   int16 *c2, int16 *c3)
+{
+        int i, val;
+
+        for (i = 0; i < MDP_SCALE_COEFF_NUM; i++) {
+                val =
+                    ((MDP_SCALE_COEFF_MASK & c1[i]) << 16) |
+                    (MDP_SCALE_COEFF_MASK & c0[i]);
+                MDP_OUTP(MDP_PPP_SCALE_COEFF_LSBn(index), val);
+                val =
+                    ((MDP_SCALE_COEFF_MASK & c3[i]) << 16) |
+                    (MDP_SCALE_COEFF_MASK & c2[i]);
+                MDP_OUTP(MDP_PPP_SCALE_COEFF_MSBn(index), val);
+                index++;
+        }
+}
+
+void mdp_init_scale_table(void)
+{
+        mdp_scale_0p2_to_0p4_mode = MDP_SCALE_FIR;
+        mdp_update_scale_table(MDP_SCALE_0P2_TO_0P4_INDEX,
+                               mdp_scale_0p2_to_0p4_C0,
+                               mdp_scale_0p2_to_0p4_C1,
+                               mdp_scale_0p2_to_0p4_C2,
+                               mdp_scale_0p2_to_0p4_C3);
+
+        mdp_scale_0p4_to_0p6_mode = MDP_SCALE_FIR;
+        mdp_update_scale_table(MDP_SCALE_0P4_TO_0P6_INDEX,
+                               mdp_scale_0p4_to_0p6_C0,
+                               mdp_scale_0p4_to_0p6_C1,
+                               mdp_scale_0p4_to_0p6_C2,
+                               mdp_scale_0p4_to_0p6_C3);
+
+        mdp_scale_0p6_to_0p8_mode = MDP_SCALE_FIR;
+        mdp_update_scale_table(MDP_SCALE_0P6_TO_0P8_INDEX,
+                               mdp_scale_0p6_to_0p8_C0,
+                               mdp_scale_0p6_to_0p8_C1,
+                               mdp_scale_0p6_to_0p8_C2,
+                               mdp_scale_0p6_to_0p8_C3);
+
+        mdp_scale_0p8_to_8p0_mode = MDP_SCALE_FIR;
+        mdp_update_scale_table(MDP_SCALE_0P8_TO_8P0_INDEX,
+                               mdp_scale_0p8_to_8p0_C0,
+                               mdp_scale_0p8_to_8p0_C1,
+                               mdp_scale_0p8_to_8p0_C2,
+                               mdp_scale_0p8_to_8p0_C3);
+}
+
+static long long mdp_do_div(long long num, long long den)
+{
+        do_div(num, den);
+        return num;
+}
+
+#define SCALER_PHASE_BITS 29
+#define HAL_MDP_PHASE_STEP_2P50    0x50000000
+#define HAL_MDP_PHASE_STEP_1P66    0x35555555
+#define HAL_MDP_PHASE_STEP_1P25    0x28000000
+
+struct phase_val {
+        int phase_init_x;
+        int phase_init_y;
+        int phase_step_x;
+        int phase_step_y;
+};
+
+static void mdp_calc_scaleInitPhase_3p1(uint32 in_w,
+                                        uint32 in_h,
+                                        uint32 out_w,
+                                        uint32 out_h,
+                                        boolean is_rotate,
+                                        boolean is_pp_x,
+                                        boolean is_pp_y, struct phase_val *pval)
+{
+        uint64 dst_ROI_width;
+        uint64 dst_ROI_height;
+        uint64 src_ROI_width;
+        uint64 src_ROI_height;
+
+        /*
+         * phase_step_x, phase_step_y, phase_init_x and phase_init_y
+         * are represented in fixed-point, unsigned 3.29 format
+         */
+        uint32 phase_step_x = 0;
+        uint32 phase_step_y = 0;
+        uint32 phase_init_x = 0;
+        uint32 phase_init_y = 0;
+        uint32 yscale_filter_sel, xscale_filter_sel;
+        uint32 scale_unit_sel_x, scale_unit_sel_y;
+
+        uint64 numerator, denominator;
+        uint64 temp_dim;
+
+        src_ROI_width = in_w;
+        src_ROI_height = in_h;
+        dst_ROI_width = out_w;
+        dst_ROI_height = out_h;
+
+        /* if there is a 90 degree rotation */
+        if (is_rotate) {
+                /* decide whether to use FIR or M/N for scaling */
+
+                /* if down-scaling by a factor smaller than 1/4 */
+                if (src_ROI_width > (4 * dst_ROI_height))
+                        scale_unit_sel_x = 1;   /* use M/N scalar */
+                else
+                        scale_unit_sel_x = 0;   /* use FIR scalar */
+
+                /* if down-scaling by a factor smaller than 1/4 */
+                if (src_ROI_height > (4 * dst_ROI_width))
+                        scale_unit_sel_y = 1;   /* use M/N scalar */
+                else
+                        scale_unit_sel_y = 0;   /* use FIR scalar */
+        } else {
+                /* decide whether to use FIR or M/N for scaling */
+
+                if (src_ROI_width > (4 * dst_ROI_width))
+                        scale_unit_sel_x = 1;   /* use M/N scalar */
+                else
+                        scale_unit_sel_x = 0;   /* use FIR scalar */
+
+                if (src_ROI_height > (4 * dst_ROI_height))
+                        scale_unit_sel_y = 1;   /* use M/N scalar */
+                else
+                        scale_unit_sel_y = 0;   /* use FIR scalar */
+
+        }
+
+        /* if there is a 90 degree rotation */
+        if (is_rotate) {
+                /* swap the width and height of dst ROI */
+                temp_dim = dst_ROI_width;
+                dst_ROI_width = dst_ROI_height;
+                dst_ROI_height = temp_dim;
+        }
+
+        /* calculate phase step for the x direction */
+
+        /* if destination is only 1 pixel wide, the value of phase_step_x
+           is unimportant. Assigning phase_step_x to src ROI width
+           as an arbitrary value. */
+        if (dst_ROI_width == 1)
+                phase_step_x = (uint32) ((src_ROI_width) << SCALER_PHASE_BITS);
+
+        /* if using FIR scalar */
+        else if (scale_unit_sel_x == 0) {
+
+                /* Calculate the quotient ( src_ROI_width - 1 ) / ( dst_ROI_width - 1)
+                   with u3.29 precision. Quotient is rounded up to the larger
+                   29th decimal point. */
+                numerator = (src_ROI_width - 1) << SCALER_PHASE_BITS;
+                denominator = (dst_ROI_width - 1);      /* never equals to 0 because of the "( dst_ROI_width == 1 ) case" */
+                phase_step_x = (uint32) mdp_do_div((numerator + denominator - 1), denominator); /* divide and round up to the larger 29th decimal point. */
+
+        }
+
+        /* if M/N scalar */
+        else if (scale_unit_sel_x == 1) {
+                /* Calculate the quotient ( src_ROI_width ) / ( dst_ROI_width)
+                   with u3.29 precision. Quotient is rounded down to the
+                   smaller 29th decimal point. */
+                numerator = (src_ROI_width) << SCALER_PHASE_BITS;
+                denominator = (dst_ROI_width);
+                phase_step_x = (uint32) mdp_do_div(numerator, denominator);
+        }
+        /* calculate phase step for the y direction */
+
+        /* if destination is only 1 pixel wide, the value of
+           phase_step_x is unimportant. Assigning phase_step_x
+           to src ROI width as an arbitrary value. */
+        if (dst_ROI_height == 1)
+                phase_step_y = (uint32) ((src_ROI_height) << SCALER_PHASE_BITS);
+
+        /* if FIR scalar */
+        else if (scale_unit_sel_y == 0) {
+                /* Calculate the quotient ( src_ROI_height - 1 ) / ( dst_ROI_height - 1)
+                   with u3.29 precision. Quotient is rounded up to the larger
+                   29th decimal point. */
+                numerator = (src_ROI_height - 1) << SCALER_PHASE_BITS;
+                denominator = (dst_ROI_height - 1);     /* never equals to 0 because of the "( dst_ROI_height == 1 )" case */
+                phase_step_y = (uint32) mdp_do_div((numerator + denominator - 1), denominator); /* Quotient is rounded up to the larger 29th decimal point. */
+
+        }
+
+        /* if M/N scalar */
+        else if (scale_unit_sel_y == 1) {
+                /* Calculate the quotient ( src_ROI_height ) / ( dst_ROI_height)
+                   with u3.29 precision. Quotient is rounded down to the smaller
+                   29th decimal point. */
+                numerator = (src_ROI_height) << SCALER_PHASE_BITS;
+                denominator = (dst_ROI_height);
+                phase_step_y = (uint32) mdp_do_div(numerator, denominator);
+        }
+
+        /* decide which set of FIR coefficients to use */
+        if (phase_step_x > HAL_MDP_PHASE_STEP_2P50)
+                xscale_filter_sel = 0;
+        else if (phase_step_x > HAL_MDP_PHASE_STEP_1P66)
+                xscale_filter_sel = 1;
+        else if (phase_step_x > HAL_MDP_PHASE_STEP_1P25)
+                xscale_filter_sel = 2;
+        else
+                xscale_filter_sel = 3;
+
+        if (phase_step_y > HAL_MDP_PHASE_STEP_2P50)
+                yscale_filter_sel = 0;
+        else if (phase_step_y > HAL_MDP_PHASE_STEP_1P66)
+                yscale_filter_sel = 1;
+        else if (phase_step_y > HAL_MDP_PHASE_STEP_1P25)
+                yscale_filter_sel = 2;
+        else
+                yscale_filter_sel = 3;
+
+        /* calculate phase init for the x direction */
+
+        /* if using FIR scalar */
+        if (scale_unit_sel_x == 0) {
+                if (dst_ROI_width == 1)
+                        phase_init_x =
+                            (uint32) ((src_ROI_width - 1) << SCALER_PHASE_BITS);
+                else
+                        phase_init_x = 0;
+
+        }
+        /* M over N scalar  */
+        else if (scale_unit_sel_x == 1)
+                phase_init_x = 0;
+
+        /* calculate phase init for the y direction
+           if using FIR scalar */
+        if (scale_unit_sel_y == 0) {
+                if (dst_ROI_height == 1)
+                        phase_init_y =
+                            (uint32) ((src_ROI_height -
+                                       1) << SCALER_PHASE_BITS);
+                else
+                        phase_init_y = 0;
+
+        }
+        /* M over N scalar   */
+        else if (scale_unit_sel_y == 1)
+                phase_init_y = 0;
+
+        /* write registers */
+        pval->phase_step_x = (uint32) phase_step_x;
+        pval->phase_step_y = (uint32) phase_step_y;
+        pval->phase_init_x = (uint32) phase_init_x;
+        pval->phase_init_y = (uint32) phase_init_y;
+
+        return;
+}
+
+void mdp_set_scale(MDPIBUF *iBuf,
+                   uint32 dst_roi_width,
+                   uint32 dst_roi_height,
+                   boolean inputRGB, boolean outputRGB, uint32 *pppop_reg_ptr)
+{
+        uint32 dst_roi_width_scale;
+        uint32 dst_roi_height_scale;
+        struct phase_val pval;
+        boolean use_pr;
+        uint32 ppp_scale_config = 0;
+
+        if (!inputRGB)
+                ppp_scale_config |= BIT(6);
+
+        if (iBuf->mdpImg.mdpOp & MDPOP_ASCALE) {
+                if (iBuf->mdpImg.mdpOp & MDPOP_ROT90) {
+                        dst_roi_width_scale = dst_roi_height;
+                        dst_roi_height_scale = dst_roi_width;
+                } else {
+                        dst_roi_width_scale = dst_roi_width;
+                        dst_roi_height_scale = dst_roi_height;
+                }
+
+                if ((dst_roi_width_scale != iBuf->roi.width) ||
+                    (dst_roi_height_scale != iBuf->roi.height) ||
+                        (iBuf->mdpImg.mdpOp & MDPOP_SHARPENING)) {
+                        *pppop_reg_ptr |=
+                            (PPP_OP_SCALE_Y_ON | PPP_OP_SCALE_X_ON);
+
+                        mdp_calc_scaleInitPhase_3p1(iBuf->roi.width,
+                                                    iBuf->roi.height,
+                                                    dst_roi_width,
+                                                    dst_roi_height,
+                                                    iBuf->mdpImg.
+                                                    mdpOp & MDPOP_ROT90, 1, 1,
+                                                    &pval);
+
+                        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x013c,
+                                 pval.phase_init_x);
+                        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0140,
+                                 pval.phase_init_y);
+                        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0144,
+                                 pval.phase_step_x);
+                        MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0148,
+                                 pval.phase_step_y);
+
+                        use_pr = (inputRGB) && (outputRGB);
+
+                        /* x-direction */
+                        if ((dst_roi_width_scale == iBuf->roi.width) &&
+                                !(iBuf->mdpImg.mdpOp & MDPOP_SHARPENING)) {
+                                *pppop_reg_ptr &= ~PPP_OP_SCALE_X_ON;
+                        } else
+                            if (((dst_roi_width_scale * 10) / iBuf->roi.width) >
+                                8) {
+                                if ((use_pr)
+                                    && (mdp_scale_0p8_to_8p0_mode !=
+                                        MDP_SCALE_PR)) {
+                                        mdp_scale_0p8_to_8p0_mode =
+                                            MDP_SCALE_PR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P8_TO_8P0_INDEX,
+                                             mdp_scale_pixel_repeat_C0,
+                                             mdp_scale_pixel_repeat_C1,
+                                             mdp_scale_pixel_repeat_C2,
+                                             mdp_scale_pixel_repeat_C3);
+                                } else if ((!use_pr)
+                                           && (mdp_scale_0p8_to_8p0_mode !=
+                                               MDP_SCALE_FIR)) {
+                                        mdp_scale_0p8_to_8p0_mode =
+                                            MDP_SCALE_FIR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P8_TO_8P0_INDEX,
+                                             mdp_scale_0p8_to_8p0_C0,
+                                             mdp_scale_0p8_to_8p0_C1,
+                                             mdp_scale_0p8_to_8p0_C2,
+                                             mdp_scale_0p8_to_8p0_C3);
+                                }
+                                ppp_scale_config |= (SCALE_U1_SET << 2);
+                        } else
+                            if (((dst_roi_width_scale * 10) / iBuf->roi.width) >
+                                6) {
+                                if ((use_pr)
+                                    && (mdp_scale_0p6_to_0p8_mode !=
+                                        MDP_SCALE_PR)) {
+                                        mdp_scale_0p6_to_0p8_mode =
+                                            MDP_SCALE_PR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P6_TO_0P8_INDEX,
+                                             mdp_scale_pixel_repeat_C0,
+                                             mdp_scale_pixel_repeat_C1,
+                                             mdp_scale_pixel_repeat_C2,
+                                             mdp_scale_pixel_repeat_C3);
+                                } else if ((!use_pr)
+                                           && (mdp_scale_0p6_to_0p8_mode !=
+                                               MDP_SCALE_FIR)) {
+                                        mdp_scale_0p6_to_0p8_mode =
+                                            MDP_SCALE_FIR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P6_TO_0P8_INDEX,
+                                             mdp_scale_0p6_to_0p8_C0,
+                                             mdp_scale_0p6_to_0p8_C1,
+                                             mdp_scale_0p6_to_0p8_C2,
+                                             mdp_scale_0p6_to_0p8_C3);
+                                }
+                                ppp_scale_config |= (SCALE_D2_SET << 2);
+                        } else
+                            if (((dst_roi_width_scale * 10) / iBuf->roi.width) >
+                                4) {
+                                if ((use_pr)
+                                    && (mdp_scale_0p4_to_0p6_mode !=
+                                        MDP_SCALE_PR)) {
+                                        mdp_scale_0p4_to_0p6_mode =
+                                            MDP_SCALE_PR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P4_TO_0P6_INDEX,
+                                             mdp_scale_pixel_repeat_C0,
+                                             mdp_scale_pixel_repeat_C1,
+                                             mdp_scale_pixel_repeat_C2,
+                                             mdp_scale_pixel_repeat_C3);
+                                } else if ((!use_pr)
+                                           && (mdp_scale_0p4_to_0p6_mode !=
+                                               MDP_SCALE_FIR)) {
+                                        mdp_scale_0p4_to_0p6_mode =
+                                            MDP_SCALE_FIR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P4_TO_0P6_INDEX,
+                                             mdp_scale_0p4_to_0p6_C0,
+                                             mdp_scale_0p4_to_0p6_C1,
+                                             mdp_scale_0p4_to_0p6_C2,
+                                             mdp_scale_0p4_to_0p6_C3);
+                                }
+                                ppp_scale_config |= (SCALE_D1_SET << 2);
+                        } else
+                            if (((dst_roi_width_scale * 4) / iBuf->roi.width) >=
+                                1) {
+                                if ((use_pr)
+                                    && (mdp_scale_0p2_to_0p4_mode !=
+                                        MDP_SCALE_PR)) {
+                                        mdp_scale_0p2_to_0p4_mode =
+                                            MDP_SCALE_PR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P2_TO_0P4_INDEX,
+                                             mdp_scale_pixel_repeat_C0,
+                                             mdp_scale_pixel_repeat_C1,
+                                             mdp_scale_pixel_repeat_C2,
+                                             mdp_scale_pixel_repeat_C3);
+                                } else if ((!use_pr)
+                                           && (mdp_scale_0p2_to_0p4_mode !=
+                                               MDP_SCALE_FIR)) {
+                                        mdp_scale_0p2_to_0p4_mode =
+                                            MDP_SCALE_FIR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P2_TO_0P4_INDEX,
+                                             mdp_scale_0p2_to_0p4_C0,
+                                             mdp_scale_0p2_to_0p4_C1,
+                                             mdp_scale_0p2_to_0p4_C2,
+                                             mdp_scale_0p2_to_0p4_C3);
+                                }
+                                ppp_scale_config |= (SCALE_D0_SET << 2);
+                        } else
+                                ppp_scale_config |= BIT(0);
+
+                        /* y-direction */
+                        if ((dst_roi_height_scale == iBuf->roi.height) &&
+                                !(iBuf->mdpImg.mdpOp & MDPOP_SHARPENING)) {
+                                *pppop_reg_ptr &= ~PPP_OP_SCALE_Y_ON;
+                        } else if (((dst_roi_height_scale * 10) /
+                                        iBuf->roi.height) > 8) {
+                                if ((use_pr)
+                                    && (mdp_scale_0p8_to_8p0_mode !=
+                                        MDP_SCALE_PR)) {
+                                        mdp_scale_0p8_to_8p0_mode =
+                                            MDP_SCALE_PR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P8_TO_8P0_INDEX,
+                                             mdp_scale_pixel_repeat_C0,
+                                             mdp_scale_pixel_repeat_C1,
+                                             mdp_scale_pixel_repeat_C2,
+                                             mdp_scale_pixel_repeat_C3);
+                                } else if ((!use_pr)
+                                           && (mdp_scale_0p8_to_8p0_mode !=
+                                               MDP_SCALE_FIR)) {
+                                        mdp_scale_0p8_to_8p0_mode =
+                                            MDP_SCALE_FIR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P8_TO_8P0_INDEX,
+                                             mdp_scale_0p8_to_8p0_C0,
+                                             mdp_scale_0p8_to_8p0_C1,
+                                             mdp_scale_0p8_to_8p0_C2,
+                                             mdp_scale_0p8_to_8p0_C3);
+                                }
+                                ppp_scale_config |= (SCALE_U1_SET << 4);
+                        } else
+                            if (((dst_roi_height_scale * 10) /
+                                 iBuf->roi.height) > 6) {
+                                if ((use_pr)
+                                    && (mdp_scale_0p6_to_0p8_mode !=
+                                        MDP_SCALE_PR)) {
+                                        mdp_scale_0p6_to_0p8_mode =
+                                            MDP_SCALE_PR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P6_TO_0P8_INDEX,
+                                             mdp_scale_pixel_repeat_C0,
+                                             mdp_scale_pixel_repeat_C1,
+                                             mdp_scale_pixel_repeat_C2,
+                                             mdp_scale_pixel_repeat_C3);
+                                } else if ((!use_pr)
+                                           && (mdp_scale_0p6_to_0p8_mode !=
+                                               MDP_SCALE_FIR)) {
+                                        mdp_scale_0p6_to_0p8_mode =
+                                            MDP_SCALE_FIR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P6_TO_0P8_INDEX,
+                                             mdp_scale_0p6_to_0p8_C0,
+                                             mdp_scale_0p6_to_0p8_C1,
+                                             mdp_scale_0p6_to_0p8_C2,
+                                             mdp_scale_0p6_to_0p8_C3);
+                                }
+                                ppp_scale_config |= (SCALE_D2_SET << 4);
+                        } else
+                            if (((dst_roi_height_scale * 10) /
+                                 iBuf->roi.height) > 4) {
+                                if ((use_pr)
+                                    && (mdp_scale_0p4_to_0p6_mode !=
+                                        MDP_SCALE_PR)) {
+                                        mdp_scale_0p4_to_0p6_mode =
+                                            MDP_SCALE_PR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P4_TO_0P6_INDEX,
+                                             mdp_scale_pixel_repeat_C0,
+                                             mdp_scale_pixel_repeat_C1,
+                                             mdp_scale_pixel_repeat_C2,
+                                             mdp_scale_pixel_repeat_C3);
+                                } else if ((!use_pr)
+                                           && (mdp_scale_0p4_to_0p6_mode !=
+                                               MDP_SCALE_FIR)) {
+                                        mdp_scale_0p4_to_0p6_mode =
+                                            MDP_SCALE_FIR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P4_TO_0P6_INDEX,
+                                             mdp_scale_0p4_to_0p6_C0,
+                                             mdp_scale_0p4_to_0p6_C1,
+                                             mdp_scale_0p4_to_0p6_C2,
+                                             mdp_scale_0p4_to_0p6_C3);
+                                }
+                                ppp_scale_config |= (SCALE_D1_SET << 4);
+                        } else
+                            if (((dst_roi_height_scale * 4) /
+                                 iBuf->roi.height) >= 1) {
+                                if ((use_pr)
+                                    && (mdp_scale_0p2_to_0p4_mode !=
+                                        MDP_SCALE_PR)) {
+                                        mdp_scale_0p2_to_0p4_mode =
+                                            MDP_SCALE_PR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P2_TO_0P4_INDEX,
+                                             mdp_scale_pixel_repeat_C0,
+                                             mdp_scale_pixel_repeat_C1,
+                                             mdp_scale_pixel_repeat_C2,
+                                             mdp_scale_pixel_repeat_C3);
+                                } else if ((!use_pr)
+                                           && (mdp_scale_0p2_to_0p4_mode !=
+                                               MDP_SCALE_FIR)) {
+                                        mdp_scale_0p2_to_0p4_mode =
+                                            MDP_SCALE_FIR;
+                                        mdp_update_scale_table
+                                            (MDP_SCALE_0P2_TO_0P4_INDEX,
+                                             mdp_scale_0p2_to_0p4_C0,
+                                             mdp_scale_0p2_to_0p4_C1,
+                                             mdp_scale_0p2_to_0p4_C2,
+                                             mdp_scale_0p2_to_0p4_C3);
+                                }
+                                ppp_scale_config |= (SCALE_D0_SET << 4);
+                        } else
+                                ppp_scale_config |= BIT(1);
+
+                        if (iBuf->mdpImg.mdpOp & MDPOP_SHARPENING) {
+                                ppp_scale_config |= BIT(7);
+                                MDP_OUTP(MDP_BASE + 0x50020,
+                                                iBuf->mdpImg.sp_value);
+                        }
+
+                        MDP_OUTP(MDP_BASE + 0x10230, ppp_scale_config);
+                } else {
+                        iBuf->mdpImg.mdpOp &= ~(MDPOP_ASCALE);
+                }
+        }
+}
+
+void mdp_adjust_start_addr(uint8 **src0,
+                           uint8 **src1,
+                           int v_slice,
+                           int h_slice,
+                           int x,
+                           int y,
+                           uint32 width,
+                           uint32 height, int bpp, MDPIBUF *iBuf, int layer)
+{
+        switch (layer) {
+        case 0:
+                MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0200, (y << 16) | (x));
+                MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0208,
+                         (height << 16) | (width));
+                break;
+
+        case 1:
+                /* MDP 3.1 HW bug workaround */
+                if (iBuf->ibuf_type == MDP_YCRYCB_H2V1) {
+                        *src0 += (x + y * width) * bpp;
+                        x = y = 0;
+                        width = iBuf->roi.dst_width;
+                        height = iBuf->roi.dst_height;
+                }
+
+                MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x0204, (y << 16) | (x));
+                MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x020c,
+                         (height << 16) | (width));
+                break;
+
+        case 2:
+                MDP_OUTP(MDP_CMD_DEBUG_ACCESS_BASE + 0x019c, (y << 16) | (x));
+                break;
+        }
+}
+
+void mdp_set_blend_attr(MDPIBUF *iBuf,
+                        uint32 *alpha,
+                        uint32 *tpVal,
+                        uint32 perPixelAlpha, uint32 *pppop_reg_ptr)
+{
+        int bg_alpha;
+
+        *alpha = iBuf->mdpImg.alpha;
+        *tpVal = iBuf->mdpImg.tpVal;
+
+        if (iBuf->mdpImg.mdpOp & MDPOP_FG_PM_ALPHA) {
+                *pppop_reg_ptr |= PPP_OP_ROT_ON |
+                    PPP_OP_BLEND_ON | PPP_OP_BLEND_CONSTANT_ALPHA;
+
+                bg_alpha = PPP_BLEND_BG_USE_ALPHA_SEL |
+                                PPP_BLEND_BG_ALPHA_REVERSE;
+
+                if (perPixelAlpha)
+                        bg_alpha |= PPP_BLEND_BG_SRCPIXEL_ALPHA;
+                else
+                        bg_alpha |= PPP_BLEND_BG_CONSTANT_ALPHA;
+
+                outpdw(MDP_BASE + 0x70010, bg_alpha);
+
+                if (iBuf->mdpImg.mdpOp & MDPOP_TRANSP)
+                        *pppop_reg_ptr |= PPP_BLEND_CALPHA_TRNASP;
+        } else if (perPixelAlpha) {
+                *pppop_reg_ptr |= PPP_OP_ROT_ON |
+                    PPP_OP_BLEND_ON | PPP_OP_BLEND_SRCPIXEL_ALPHA;
+        } else {
+                if ((iBuf->mdpImg.mdpOp & MDPOP_ALPHAB)
+                    && (iBuf->mdpImg.alpha == 0xff)) {
+                        iBuf->mdpImg.mdpOp &= ~(MDPOP_ALPHAB);
+                }
+
+                if ((iBuf->mdpImg.mdpOp & MDPOP_ALPHAB)
+                    || (iBuf->mdpImg.mdpOp & MDPOP_TRANSP)) {
+                        *pppop_reg_ptr |=
+                            PPP_OP_ROT_ON | PPP_OP_BLEND_ON |
+                            PPP_OP_BLEND_CONSTANT_ALPHA |
+                            PPP_OP_BLEND_ALPHA_BLEND_NORMAL;
+                }
+
+                if (iBuf->mdpImg.mdpOp & MDPOP_TRANSP)
+                        *pppop_reg_ptr |= PPP_BLEND_CALPHA_TRNASP;
+        }
+}
diff --git a/drivers/staging/msm/mdp_vsync.c b/drivers/staging/msm/mdp_vsync.c
new file mode 100644
index 000000000000..bbd456044356
--- /dev/null
+++ b/drivers/staging/msm/mdp_vsync.c
@@ -0,0 +1,389 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/hrtimer.h>
+#include <linux/vmalloc.h>
+#include <linux/clk.h>
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/uaccess.h>
+#include <mach/gpio.h>
+
+#include "mdp.h"
+#include "msm_fb.h"
+#include "mddihost.h"
+
+#ifdef CONFIG_FB_MSM_MDP40
+#define MDP_SYNC_CFG_0          0x100
+#define MDP_SYNC_STATUS_0       0x10c
+#define MDP_PRIM_VSYNC_OUT_CTRL 0x118
+#define MDP_PRIM_VSYNC_INIT_VAL 0x128
+#else
+#define MDP_SYNC_CFG_0          0x300
+#define MDP_SYNC_STATUS_0       0x30c
+#define MDP_PRIM_VSYNC_OUT_CTRL 0x318
+#define MDP_PRIM_VSYNC_INIT_VAL 0x328
+#endif
+
+extern mddi_lcd_type mddi_lcd_idx;
+extern spinlock_t mdp_spin_lock;
+extern struct workqueue_struct *mdp_vsync_wq;
+extern int lcdc_mode;
+extern int vsync_mode;
+
+#ifdef MDP_HW_VSYNC
+int vsync_above_th = 4;
+int vsync_start_th = 1;
+int vsync_load_cnt;
+
+struct clk *mdp_vsync_clk;
+
+void mdp_hw_vsync_clk_enable(struct msm_fb_data_type *mfd)
+{
+        if (mfd->use_mdp_vsync)
+                clk_enable(mdp_vsync_clk);
+}
+
+void mdp_hw_vsync_clk_disable(struct msm_fb_data_type *mfd)
+{
+        if (mfd->use_mdp_vsync)
+                clk_disable(mdp_vsync_clk);
+}
+#endif
+
+static void mdp_set_vsync(unsigned long data)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)data;
+        struct msm_fb_panel_data *pdata = NULL;
+
+        pdata = (struct msm_fb_panel_data *)mfd->pdev->dev.platform_data;
+
+        if ((pdata) && (pdata->set_vsync_notifier == NULL))
+                return;
+
+        init_timer(&mfd->vsync_resync_timer);
+        mfd->vsync_resync_timer.function = mdp_set_vsync;
+        mfd->vsync_resync_timer.data = data;
+        mfd->vsync_resync_timer.expires =
+            jiffies + mfd->panel_info.lcd.vsync_notifier_period;
+        add_timer(&mfd->vsync_resync_timer);
+
+        if ((mfd->panel_info.lcd.vsync_enable) && (mfd->panel_power_on)
+            && (!mfd->vsync_handler_pending)) {
+                mfd->vsync_handler_pending = TRUE;
+                if (!queue_work(mdp_vsync_wq, &mfd->vsync_resync_worker)) {
+                        MSM_FB_INFO
+                            ("mdp_set_vsync: can't queue_work! -> needs to increase vsync_resync_timer_duration\n");
+                }
+        } else {
+                MSM_FB_DEBUG
+                    ("mdp_set_vsync failed!  EN:%d  PWR:%d  PENDING:%d\n",
+                     mfd->panel_info.lcd.vsync_enable, mfd->panel_power_on,
+                     mfd->vsync_handler_pending);
+        }
+}
+
+static void mdp_vsync_handler(void *data)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)data;
+
+        if (mfd->use_mdp_vsync) {
+#ifdef MDP_HW_VSYNC
+                if (mfd->panel_power_on)
+                        MDP_OUTP(MDP_BASE + MDP_SYNC_STATUS_0, vsync_load_cnt);
+
+                mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, TRUE);
+#endif
+        } else {
+                mfd->last_vsync_timetick = ktime_get_real();
+        }
+
+        mfd->vsync_handler_pending = FALSE;
+}
+
+irqreturn_t mdp_hw_vsync_handler_proxy(int irq, void *data)
+{
+        /*
+         * ToDo: tried enabling/disabling GPIO MDP HW VSYNC interrupt
+         * but getting inaccurate timing in mdp_vsync_handler()
+         * disable_irq(MDP_HW_VSYNC_IRQ);
+         */
+        mdp_vsync_handler(data);
+
+        return IRQ_HANDLED;
+}
+
+#ifdef MDP_HW_VSYNC
+static void mdp_set_sync_cfg_0(struct msm_fb_data_type *mfd, int vsync_cnt)
+{
+        unsigned long cfg;
+
+        cfg = mfd->total_lcd_lines - 1;
+        cfg <<= MDP_SYNCFG_HGT_LOC;
+        if (mfd->panel_info.lcd.hw_vsync_mode)
+                cfg |= MDP_SYNCFG_VSYNC_EXT_EN;
+        cfg |= (MDP_SYNCFG_VSYNC_INT_EN | vsync_cnt);
+
+        MDP_OUTP(MDP_BASE + MDP_SYNC_CFG_0, cfg);
+}
+#endif
+
+void mdp_config_vsync(struct msm_fb_data_type *mfd)
+{
+
+        /* vsync on primary lcd only for now */
+        if ((mfd->dest != DISPLAY_LCD) || (mfd->panel_info.pdest != DISPLAY_1)
+            || (!vsync_mode)) {
+                goto err_handle;
+        }
+
+        if (mfd->panel_info.lcd.vsync_enable) {
+                mfd->total_porch_lines = mfd->panel_info.lcd.v_back_porch +
+                    mfd->panel_info.lcd.v_front_porch +
+                    mfd->panel_info.lcd.v_pulse_width;
+                mfd->total_lcd_lines =
+                    mfd->panel_info.yres + mfd->total_porch_lines;
+                mfd->lcd_ref_usec_time =
+                    100000000 / mfd->panel_info.lcd.refx100;
+                mfd->vsync_handler_pending = FALSE;
+                mfd->last_vsync_timetick.tv.sec = 0;
+                mfd->last_vsync_timetick.tv.nsec = 0;
+
+#ifdef MDP_HW_VSYNC
+                if (mdp_vsync_clk == NULL)
+                        mdp_vsync_clk = clk_get(NULL, "mdp_vsync_clk");
+
+                if (IS_ERR(mdp_vsync_clk)) {
+                        printk(KERN_ERR "error: can't get mdp_vsync_clk!\n");
+                        mfd->use_mdp_vsync = 0;
+                } else
+                        mfd->use_mdp_vsync = 1;
+
+                if (mfd->use_mdp_vsync) {
+                        uint32 vsync_cnt_cfg, vsync_cnt_cfg_dem;
+                        uint32 mdp_vsync_clk_speed_hz;
+
+                        mdp_vsync_clk_speed_hz = clk_get_rate(mdp_vsync_clk);
+
+                        if (mdp_vsync_clk_speed_hz == 0) {
+                                mfd->use_mdp_vsync = 0;
+                        } else {
+                                /*
+                                 * Do this calculation in 2 steps for
+                                 * rounding uint32 properly.
+                                 */
+                                vsync_cnt_cfg_dem =
+                                    (mfd->panel_info.lcd.refx100 *
+                                     mfd->total_lcd_lines) / 100;
+                                vsync_cnt_cfg =
+                                    (mdp_vsync_clk_speed_hz) /
+                                    vsync_cnt_cfg_dem;
+
+                                /* MDP cmd block enable */
+                                mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON,
+                                              FALSE);
+                                mdp_hw_vsync_clk_enable(mfd);
+
+                                mdp_set_sync_cfg_0(mfd, vsync_cnt_cfg);
+
+                                /*
+                                 * load the last line + 1 to be in the
+                                 * safety zone
+                                 */
+                                vsync_load_cnt = mfd->panel_info.yres;
+
+                                /* line counter init value at the next pulse */
+                                MDP_OUTP(MDP_BASE + MDP_PRIM_VSYNC_INIT_VAL,
+                                                        vsync_load_cnt);
+
+                                /*
+                                 * external vsync source pulse width and
+                                 * polarity flip
+                                 */
+                                MDP_OUTP(MDP_BASE + MDP_PRIM_VSYNC_OUT_CTRL,
+                                                        BIT(30) | BIT(0));
+
+
+                                /* threshold */
+                                MDP_OUTP(MDP_BASE + 0x200,
+                                         (vsync_above_th << 16) |
+                                         (vsync_start_th));
+
+                                mdp_hw_vsync_clk_disable(mfd);
+                                /* MDP cmd block disable */
+                                mdp_pipe_ctrl(MDP_CMD_BLOCK,
+                                              MDP_BLOCK_POWER_OFF, FALSE);
+                        }
+                }
+#else
+                mfd->use_mdp_vsync = 0;
+                hrtimer_init(&mfd->dma_hrtimer, CLOCK_MONOTONIC,
+                             HRTIMER_MODE_REL);
+                mfd->dma_hrtimer.function = mdp_dma2_vsync_hrtimer_handler;
+                mfd->vsync_width_boundary = vmalloc(mfd->panel_info.xres * 4);
+#endif
+
+                mfd->channel_irq = 0;
+                if (mfd->panel_info.lcd.hw_vsync_mode) {
+                        u32 vsync_gpio = mfd->vsync_gpio;
+                        u32 ret;
+
+                        if (vsync_gpio == -1) {
+                                MSM_FB_INFO("vsync_gpio not defined!\n");
+                                goto err_handle;
+                        }
+
+                        ret = gpio_tlmm_config(GPIO_CFG
+                                        (vsync_gpio,
+                                        (mfd->use_mdp_vsync) ? 1 : 0,
+                                        GPIO_INPUT,
+                                        GPIO_PULL_DOWN,
+                                        GPIO_2MA),
+                                        GPIO_ENABLE);
+                        if (ret)
+                                goto err_handle;
+
+                        if (!mfd->use_mdp_vsync) {
+                                mfd->channel_irq = MSM_GPIO_TO_INT(vsync_gpio);
+                                if (request_irq
+                                    (mfd->channel_irq,
+                                     &mdp_hw_vsync_handler_proxy,
+                                     IRQF_TRIGGER_FALLING, "VSYNC_GPIO",
+                                     (void *)mfd)) {
+                                        MSM_FB_INFO
+                                        ("irq=%d failed! vsync_gpio=%d\n",
+                                                mfd->channel_irq,
+                                                vsync_gpio);
+                                        goto err_handle;
+                                }
+                        }
+                }
+
+                mdp_set_vsync((unsigned long)mfd);
+        }
+
+        return;
+
+err_handle:
+        if (mfd->vsync_width_boundary)
+                vfree(mfd->vsync_width_boundary);
+        mfd->panel_info.lcd.vsync_enable = FALSE;
+        printk(KERN_ERR "%s: failed!\n", __func__);
+}
+
+void mdp_vsync_resync_workqueue_handler(struct work_struct *work)
+{
+        struct msm_fb_data_type *mfd = NULL;
+        int vsync_fnc_enabled = FALSE;
+        struct msm_fb_panel_data *pdata = NULL;
+
+        mfd = container_of(work, struct msm_fb_data_type, vsync_resync_worker);
+
+        if (mfd) {
+                if (mfd->panel_power_on) {
+                        pdata =
+                            (struct msm_fb_panel_data *)mfd->pdev->dev.
+                            platform_data;
+
+                        /*
+                         * we need to turn on MDP power if it uses MDP vsync
+                         * HW block in SW mode
+                         */
+                        if ((!mfd->panel_info.lcd.hw_vsync_mode) &&
+                            (mfd->use_mdp_vsync) &&
+                            (pdata) && (pdata->set_vsync_notifier != NULL)) {
+                                /*
+                                 * enable pwr here since we can't enable it in
+                                 * vsync callback in isr mode
+                                 */
+                                mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON,
+                                              FALSE);
+                        }
+
+                        if (pdata->set_vsync_notifier != NULL) {
+                                vsync_fnc_enabled = TRUE;
+                                pdata->set_vsync_notifier(mdp_vsync_handler,
+                                                          (void *)mfd);
+                        }
+                }
+        }
+
+        if ((mfd) && (!vsync_fnc_enabled))
+                mfd->vsync_handler_pending = FALSE;
+}
+
+boolean mdp_hw_vsync_set_handler(msm_fb_vsync_handler_type handler, void *data)
+{
+        /*
+         * ToDo: tried enabling/disabling GPIO MDP HW VSYNC interrupt
+         * but getting inaccurate timing in mdp_vsync_handler()
+         * enable_irq(MDP_HW_VSYNC_IRQ);
+         */
+
+        return TRUE;
+}
+
+uint32 mdp_get_lcd_line_counter(struct msm_fb_data_type *mfd)
+{
+        uint32 elapsed_usec_time;
+        uint32 lcd_line;
+        ktime_t last_vsync_timetick_local;
+        ktime_t curr_time;
+        unsigned long flag;
+
+        if ((!mfd->panel_info.lcd.vsync_enable) || (!vsync_mode))
+                return 0;
+
+        spin_lock_irqsave(&mdp_spin_lock, flag);
+        last_vsync_timetick_local = mfd->last_vsync_timetick;
+        spin_unlock_irqrestore(&mdp_spin_lock, flag);
+
+        curr_time = ktime_get_real();
+        elapsed_usec_time =
+            ((curr_time.tv.sec - last_vsync_timetick_local.tv.sec) * 1000000) +
+            ((curr_time.tv.nsec - last_vsync_timetick_local.tv.nsec) / 1000);
+
+        elapsed_usec_time = elapsed_usec_time % mfd->lcd_ref_usec_time;
+
+        /* lcd line calculation referencing to line counter = 0 */
+        lcd_line =
+            (elapsed_usec_time * mfd->total_lcd_lines) / mfd->lcd_ref_usec_time;
+
+        /* lcd line adjusment referencing to the actual line counter at vsync */
+        lcd_line =
+            (mfd->total_lcd_lines - mfd->panel_info.lcd.v_back_porch +
+             lcd_line) % (mfd->total_lcd_lines + 1);
+
+        if (lcd_line > mfd->total_lcd_lines) {
+                MSM_FB_INFO
+                    ("mdp_get_lcd_line_counter: mdp_lcd_rd_cnt >= mfd->total_lcd_lines error!\n");
+        }
+
+        return lcd_line;
+}
diff --git a/drivers/staging/msm/memory.c b/drivers/staging/msm/memory.c
new file mode 100644
index 000000000000..cc80fdf17d61
--- /dev/null
+++ b/drivers/staging/msm/memory.c
@@ -0,0 +1,214 @@
+/* arch/arm/mach-msm/memory.c
+ *
+ * Copyright (C) 2007 Google, Inc.
+ * Copyright (c) 2009-2010, Code Aurora Forum. All rights reserved.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/mm.h>
+#include <linux/mm_types.h>
+#include <linux/bootmem.h>
+#include <linux/module.h>
+#include <asm/pgtable.h>
+#include <asm/io.h>
+#include <asm/mach/map.h>
+#include "memory_ll.h"
+#include <asm/cacheflush.h>
+#if defined(CONFIG_MSM_NPA_REMOTE)
+#include "npa_remote.h"
+#include <linux/completion.h>
+#include <linux/err.h>
+#endif
+
+int arch_io_remap_pfn_range(struct vm_area_struct *vma, unsigned long addr,
+                            unsigned long pfn, unsigned long size, pgprot_t prot)
+{
+        unsigned long pfn_addr = pfn << PAGE_SHIFT;
+/*
+        if ((pfn_addr >= 0x88000000) && (pfn_addr < 0xD0000000)) {
+                prot = pgprot_device(prot);
+                printk("remapping device %lx\n", prot);
+        }
+*/
+        panic("Memory remap PFN stuff not done\n");
+        return remap_pfn_range(vma, addr, pfn, size, prot);
+}
+
+void *zero_page_strongly_ordered;
+
+static void map_zero_page_strongly_ordered(void)
+{
+        if (zero_page_strongly_ordered)
+                return;
+/*
+        zero_page_strongly_ordered =
+                ioremap_strongly_ordered(page_to_pfn(empty_zero_page)
+                << PAGE_SHIFT, PAGE_SIZE);
+*/
+        panic("Strongly ordered memory functions not implemented\n");
+}
+
+void write_to_strongly_ordered_memory(void)
+{
+        map_zero_page_strongly_ordered();
+        *(int *)zero_page_strongly_ordered = 0;
+}
+EXPORT_SYMBOL(write_to_strongly_ordered_memory);
+
+void flush_axi_bus_buffer(void)
+{
+        __asm__ __volatile__ ("mcr p15, 0, %0, c7, c10, 5" \
+                                    : : "r" (0) : "memory");
+        write_to_strongly_ordered_memory();
+}
+
+#define CACHE_LINE_SIZE 32
+
+/* These cache related routines make the assumption that the associated
+ * physical memory is contiguous. They will operate on all (L1
+ * and L2 if present) caches.
+ */
+void clean_and_invalidate_caches(unsigned long vstart,
+        unsigned long length, unsigned long pstart)
+{
+        unsigned long vaddr;
+
+        for (vaddr = vstart; vaddr < vstart + length; vaddr += CACHE_LINE_SIZE)
+                asm ("mcr p15, 0, %0, c7, c14, 1" : : "r" (vaddr));
+#ifdef CONFIG_OUTER_CACHE
+        outer_flush_range(pstart, pstart + length);
+#endif
+        asm ("mcr p15, 0, %0, c7, c10, 4" : : "r" (0));
+        asm ("mcr p15, 0, %0, c7, c5, 0" : : "r" (0));
+
+        flush_axi_bus_buffer();
+}
+
+void clean_caches(unsigned long vstart,
+        unsigned long length, unsigned long pstart)
+{
+        unsigned long vaddr;
+
+        for (vaddr = vstart; vaddr < vstart + length; vaddr += CACHE_LINE_SIZE)
+                asm ("mcr p15, 0, %0, c7, c10, 1" : : "r" (vaddr));
+#ifdef CONFIG_OUTER_CACHE
+        outer_clean_range(pstart, pstart + length);
+#endif
+        asm ("mcr p15, 0, %0, c7, c10, 4" : : "r" (0));
+        asm ("mcr p15, 0, %0, c7, c5, 0" : : "r" (0));
+
+        flush_axi_bus_buffer();
+}
+
+void invalidate_caches(unsigned long vstart,
+        unsigned long length, unsigned long pstart)
+{
+        unsigned long vaddr;
+
+        for (vaddr = vstart; vaddr < vstart + length; vaddr += CACHE_LINE_SIZE)
+                asm ("mcr p15, 0, %0, c7, c6, 1" : : "r" (vaddr));
+#ifdef CONFIG_OUTER_CACHE
+        outer_inv_range(pstart, pstart + length);
+#endif
+        asm ("mcr p15, 0, %0, c7, c10, 4" : : "r" (0));
+        asm ("mcr p15, 0, %0, c7, c5, 0" : : "r" (0));
+
+        flush_axi_bus_buffer();
+}
+
+void *alloc_bootmem_aligned(unsigned long size, unsigned long alignment)
+{
+        void *unused_addr = NULL;
+        unsigned long addr, tmp_size, unused_size;
+
+        /* Allocate maximum size needed, see where it ends up.
+         * Then free it -- in this path there are no other allocators
+         * so we can depend on getting the same address back
+         * when we allocate a smaller piece that is aligned
+         * at the end (if necessary) and the piece we really want,
+         * then free the unused first piece.
+         */
+
+        tmp_size = size + alignment - PAGE_SIZE;
+        addr = (unsigned long)alloc_bootmem(tmp_size);
+        free_bootmem(__pa(addr), tmp_size);
+
+        unused_size = alignment - (addr % alignment);
+        if (unused_size)
+                unused_addr = alloc_bootmem(unused_size);
+
+        addr = (unsigned long)alloc_bootmem(size);
+        if (unused_size)
+                free_bootmem(__pa(unused_addr), unused_size);
+
+        return (void *)addr;
+}
+
+#if defined(CONFIG_MSM_NPA_REMOTE)
+struct npa_client *npa_memory_client;
+#endif
+
+static int change_memory_power_state(unsigned long start_pfn,
+        unsigned long nr_pages, int state)
+{
+#if defined(CONFIG_MSM_NPA_REMOTE)
+        static atomic_t node_created_flag = ATOMIC_INIT(1);
+#else
+        unsigned long start;
+        unsigned long size;
+        unsigned long virtual;
+#endif
+        int rc = 0;
+
+#if defined(CONFIG_MSM_NPA_REMOTE)
+        if (atomic_dec_and_test(&node_created_flag)) {
+                /* Create NPA 'required' client. */
+                npa_memory_client = npa_create_sync_client(NPA_MEMORY_NODE_NAME,
+                        "memory node", NPA_CLIENT_REQUIRED);
+                if (IS_ERR(npa_memory_client)) {
+                        rc = PTR_ERR(npa_memory_client);
+                        return rc;
+                }
+        }
+
+        rc = npa_issue_required_request(npa_memory_client, state);
+#else
+        if (state == MEMORY_DEEP_POWERDOWN) {
+                /* simulate turning off memory by writing bit pattern into it */
+                start = start_pfn << PAGE_SHIFT;
+                size = nr_pages << PAGE_SHIFT;
+                virtual = __phys_to_virt(start);
+                memset((void *)virtual, 0x27, size);
+        }
+#endif
+        return rc;
+}
+
+int platform_physical_remove_pages(unsigned long start_pfn,
+        unsigned long nr_pages)
+{
+        return change_memory_power_state(start_pfn, nr_pages,
+                MEMORY_DEEP_POWERDOWN);
+}
+
+int platform_physical_add_pages(unsigned long start_pfn,
+        unsigned long nr_pages)
+{
+        return change_memory_power_state(start_pfn, nr_pages, MEMORY_ACTIVE);
+}
+
+int platform_physical_low_power_pages(unsigned long start_pfn,
+        unsigned long nr_pages)
+{
+        return change_memory_power_state(start_pfn, nr_pages,
+                MEMORY_SELF_REFRESH);
+}
diff --git a/drivers/staging/msm/memory_ll.h b/drivers/staging/msm/memory_ll.h
new file mode 100644
index 000000000000..18a239a89a78
--- /dev/null
+++ b/drivers/staging/msm/memory_ll.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2007 Google, Inc.
+ * Copyright (c) 2009-2010, Code Aurora Forum. All rights reserved.
+ *
+ * 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 __ASM_ARCH_MEMORY_LL_H
+#define __ASM_ARCH_MEMORY_LL_H
+
+#define MAX_PHYSMEM_BITS 32
+#define SECTION_SIZE_BITS 25
+
+#define HAS_ARCH_IO_REMAP_PFN_RANGE
+
+#ifndef __ASSEMBLY__
+void *alloc_bootmem_aligned(unsigned long size, unsigned long alignment);
+void clean_and_invalidate_caches(unsigned long, unsigned long, unsigned long);
+void clean_caches(unsigned long, unsigned long, unsigned long);
+void invalidate_caches(unsigned long, unsigned long, unsigned long);
+int platform_physical_remove_pages(unsigned long, unsigned long);
+int platform_physical_add_pages(unsigned long, unsigned long);
+int platform_physical_low_power_pages(unsigned long, unsigned long);
+
+#ifdef CONFIG_ARCH_MSM_ARM11
+void write_to_strongly_ordered_memory(void);
+
+#include <asm/mach-types.h>
+
+#define arch_barrier_extra() do \
+        { if (machine_is_msm7x27_surf() || machine_is_msm7x27_ffa())  \
+                write_to_strongly_ordered_memory(); \
+        } while (0)
+#endif
+
+#ifdef CONFIG_CACHE_L2X0
+extern void l2x0_cache_sync(void);
+#define finish_arch_switch(prev)     do { l2x0_cache_sync(); } while (0)
+#endif
+
+#endif
+
+#ifdef CONFIG_ARCH_MSM_SCORPION
+#define arch_has_speculative_dfetch()   1
+#endif
+
+#endif
+
+/* these correspond to values known by the modem */
+#define MEMORY_DEEP_POWERDOWN   0
+#define MEMORY_SELF_REFRESH     1
+#define MEMORY_ACTIVE           2
+
+#define NPA_MEMORY_NODE_NAME    "/mem/ebi1/cs1"
diff --git a/drivers/staging/msm/msm_fb.c b/drivers/staging/msm/msm_fb.c
new file mode 100644
index 000000000000..af5620e4eee1
--- /dev/null
+++ b/drivers/staging/msm/msm_fb.c
@@ -0,0 +1,2354 @@
+/*
+ *
+ * Core MSM framebuffer driver.
+ *
+ * Copyright (C) 2007 Google Incorporated
+ * Copyright (c) 2008-2010, Code Aurora Forum. All rights reserved.
+ *
+ * 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.
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/fb.h>
+#include "msm_mdp.h"
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <mach/board.h>
+#include <linux/uaccess.h>
+
+#include <linux/workqueue.h>
+#include <linux/string.h>
+#include <linux/version.h>
+#include <linux/proc_fs.h>
+#include <linux/vmalloc.h>
+#include <linux/debugfs.h>
+#include <linux/console.h>
+#include <linux/leds.h>
+#include <asm/dma-mapping.h>
+
+
+#define MSM_FB_C
+#include "msm_fb.h"
+#include "mddihosti.h"
+#include "tvenc.h"
+#include "mdp.h"
+#include "mdp4.h"
+
+#ifdef CONFIG_FB_MSM_LOGO
+#define INIT_IMAGE_FILE "/logo.rle"
+extern int load_565rle_image(char *filename);
+#endif
+
+
+#define pgprot_noncached(prot) \
+       __pgprot_modify(prot, L_PTE_MT_MASK, L_PTE_MT_UNCACHED)
+#define pgprot_writecombine(prot) \
+       __pgprot_modify(prot, L_PTE_MT_MASK, L_PTE_MT_BUFFERABLE)
+#define pgprot_device(prot) \
+       __pgprot_modify(prot, L_PTE_MT_MASK|L_PTE_EXEC, L_PTE_MT_DEV_NONSHARED)
+#define pgprot_writethroughcache(prot) \
+       __pgprot((pgprot_val(prot) & ~L_PTE_MT_MASK) | L_PTE_MT_WRITETHROUGH)
+#define pgprot_writebackcache(prot) \
+       __pgprot((pgprot_val(prot) & ~L_PTE_MT_MASK) | L_PTE_MT_WRITEBACK)
+#define pgprot_writebackwacache(prot) \
+       __pgprot((pgprot_val(prot) & ~L_PTE_MT_MASK) | L_PTE_MT_WRITEALLOC)
+
+static unsigned char *fbram;
+static unsigned char *fbram_phys;
+static int fbram_size;
+
+static struct platform_device *pdev_list[MSM_FB_MAX_DEV_LIST];
+static int pdev_list_cnt;
+
+int vsync_mode = 1;
+
+#define MAX_FBI_LIST 32
+static struct fb_info *fbi_list[MAX_FBI_LIST];
+static int fbi_list_index;
+
+static struct msm_fb_data_type *mfd_list[MAX_FBI_LIST];
+static int mfd_list_index;
+
+static u32 msm_fb_pseudo_palette[16] = {
+        0x00000000, 0xffffffff, 0xffffffff, 0xffffffff,
+        0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff,
+        0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff,
+        0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff
+};
+
+u32 msm_fb_debug_enabled;
+/* Setting msm_fb_msg_level to 8 prints out ALL messages */
+u32 msm_fb_msg_level = 7;
+
+/* Setting mddi_msg_level to 8 prints out ALL messages */
+u32 mddi_msg_level = 5;
+
+extern int32 mdp_block_power_cnt[MDP_MAX_BLOCK];
+extern unsigned long mdp_timer_duration;
+
+static int msm_fb_register(struct msm_fb_data_type *mfd);
+static int msm_fb_open(struct fb_info *info, int user);
+static int msm_fb_release(struct fb_info *info, int user);
+static int msm_fb_pan_display(struct fb_var_screeninfo *var,
+                              struct fb_info *info);
+static int msm_fb_stop_sw_refresher(struct msm_fb_data_type *mfd);
+int msm_fb_resume_sw_refresher(struct msm_fb_data_type *mfd);
+static int msm_fb_check_var(struct fb_var_screeninfo *var,
+                            struct fb_info *info);
+static int msm_fb_set_par(struct fb_info *info);
+static int msm_fb_blank_sub(int blank_mode, struct fb_info *info,
+                            boolean op_enable);
+static int msm_fb_suspend_sub(struct msm_fb_data_type *mfd);
+static int msm_fb_resume_sub(struct msm_fb_data_type *mfd);
+static int msm_fb_ioctl(struct fb_info *info, unsigned int cmd,
+                        unsigned long arg);
+static int msm_fb_mmap(struct fb_info *info, struct vm_area_struct * vma);
+
+#ifdef MSM_FB_ENABLE_DBGFS
+
+#define MSM_FB_MAX_DBGFS 1024
+#define MAX_BACKLIGHT_BRIGHTNESS 255
+
+int msm_fb_debugfs_file_index;
+struct dentry *msm_fb_debugfs_root;
+struct dentry *msm_fb_debugfs_file[MSM_FB_MAX_DBGFS];
+
+struct dentry *msm_fb_get_debugfs_root(void)
+{
+        if (msm_fb_debugfs_root == NULL)
+                msm_fb_debugfs_root = debugfs_create_dir("msm_fb", NULL);
+
+        return msm_fb_debugfs_root;
+}
+
+void msm_fb_debugfs_file_create(struct dentry *root, const char *name,
+                                u32 *var)
+{
+        if (msm_fb_debugfs_file_index >= MSM_FB_MAX_DBGFS)
+                return;
+
+        msm_fb_debugfs_file[msm_fb_debugfs_file_index++] =
+            debugfs_create_u32(name, S_IRUGO | S_IWUSR, root, var);
+}
+#endif
+
+int msm_fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+
+        if (!mfd->cursor_update)
+                return -ENODEV;
+
+        return mfd->cursor_update(info, cursor);
+}
+
+static int msm_fb_resource_initialized;
+
+#ifndef CONFIG_FB_BACKLIGHT
+static int lcd_backlight_registered;
+
+static void msm_fb_set_bl_brightness(struct led_classdev *led_cdev,
+                                        enum led_brightness value)
+{
+        struct msm_fb_data_type *mfd = dev_get_drvdata(led_cdev->dev->parent);
+        int bl_lvl;
+
+        if (value > MAX_BACKLIGHT_BRIGHTNESS)
+                value = MAX_BACKLIGHT_BRIGHTNESS;
+
+        /* This maps android backlight level 0 to 255 into
+           driver backlight level 0 to bl_max with rounding */
+        bl_lvl = (2 * value * mfd->panel_info.bl_max + MAX_BACKLIGHT_BRIGHTNESS)
+                /(2 * MAX_BACKLIGHT_BRIGHTNESS);
+
+        if (!bl_lvl && value)
+                bl_lvl = 1;
+
+        msm_fb_set_backlight(mfd, bl_lvl, 1);
+}
+
+static struct led_classdev backlight_led = {
+        .name           = "lcd-backlight",
+        .brightness     = MAX_BACKLIGHT_BRIGHTNESS,
+        .brightness_set = msm_fb_set_bl_brightness,
+};
+#endif
+
+static struct msm_fb_platform_data *msm_fb_pdata;
+
+int msm_fb_detect_client(const char *name)
+{
+        int ret = -EPERM;
+#ifdef CONFIG_FB_MSM_MDDI_AUTO_DETECT
+        u32 id;
+#endif
+
+        if (msm_fb_pdata && msm_fb_pdata->detect_client) {
+                ret = msm_fb_pdata->detect_client(name);
+
+                /* if it's non mddi panel, we need to pre-scan
+                   mddi client to see if we can disable mddi host */
+
+#ifdef CONFIG_FB_MSM_MDDI_AUTO_DETECT
+                if (!ret && msm_fb_pdata->mddi_prescan)
+                        id = mddi_get_client_id();
+#endif
+        }
+
+        return ret;
+}
+
+static int msm_fb_probe(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+        int rc;
+
+        MSM_FB_DEBUG("msm_fb_probe\n");
+
+        if ((pdev->id == 0) && (pdev->num_resources > 0)) {
+                msm_fb_pdata = pdev->dev.platform_data;
+                fbram_size =
+                        pdev->resource[0].end - pdev->resource[0].start + 1;
+                fbram_phys = (char *)pdev->resource[0].start;
+                fbram = ioremap((unsigned long)fbram_phys, fbram_size);
+
+                if (!fbram) {
+                        printk(KERN_ERR "fbram ioremap failed!\n");
+                        return -ENOMEM;
+                }
+                MSM_FB_INFO("msm_fb_probe:  phy_Addr = 0x%x virt = 0x%x\n",
+                             (int)fbram_phys, (int)fbram);
+
+                msm_fb_resource_initialized = 1;
+                return 0;
+        }
+
+        if (!msm_fb_resource_initialized)
+                return -EPERM;
+
+        mfd = (struct msm_fb_data_type *)platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        if (pdev_list_cnt >= MSM_FB_MAX_DEV_LIST)
+                return -ENOMEM;
+
+        mfd->panel_info.frame_count = 0;
+        mfd->bl_level = mfd->panel_info.bl_max;
+
+        if (mfd->panel_info.type == LCDC_PANEL)
+                mfd->allow_set_offset =
+                msm_fb_pdata->allow_set_offset != NULL ?
+                msm_fb_pdata->allow_set_offset() : 0;
+        else
+                mfd->allow_set_offset = 0;
+
+        rc = msm_fb_register(mfd);
+        if (rc)
+                return rc;
+
+#ifdef CONFIG_FB_BACKLIGHT
+        msm_fb_config_backlight(mfd);
+#else
+        /* android supports only one lcd-backlight/lcd for now */
+        if (!lcd_backlight_registered) {
+                if (led_classdev_register(&pdev->dev, &backlight_led))
+                        printk(KERN_ERR "led_classdev_register failed\n");
+                else
+                        lcd_backlight_registered = 1;
+        }
+#endif
+
+        pdev_list[pdev_list_cnt++] = pdev;
+        return 0;
+}
+
+static int msm_fb_remove(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+
+        MSM_FB_DEBUG("msm_fb_remove\n");
+
+        mfd = (struct msm_fb_data_type *)platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        if (msm_fb_suspend_sub(mfd))
+                printk(KERN_ERR "msm_fb_remove: can't stop the device %d\n", mfd->index);
+
+        if (mfd->channel_irq != 0)
+                free_irq(mfd->channel_irq, (void *)mfd);
+
+        if (mfd->vsync_width_boundary)
+                vfree(mfd->vsync_width_boundary);
+
+        if (mfd->vsync_resync_timer.function)
+                del_timer(&mfd->vsync_resync_timer);
+
+        if (mfd->refresh_timer.function)
+                del_timer(&mfd->refresh_timer);
+
+        if (mfd->dma_hrtimer.function)
+                hrtimer_cancel(&mfd->dma_hrtimer);
+
+        /* remove /dev/fb* */
+        unregister_framebuffer(mfd->fbi);
+
+#ifdef CONFIG_FB_BACKLIGHT
+        /* remove /sys/class/backlight */
+        backlight_device_unregister(mfd->fbi->bl_dev);
+#else
+        if (lcd_backlight_registered) {
+                lcd_backlight_registered = 0;
+                led_classdev_unregister(&backlight_led);
+        }
+#endif
+
+#ifdef MSM_FB_ENABLE_DBGFS
+        if (mfd->sub_dir)
+                debugfs_remove(mfd->sub_dir);
+#endif
+
+        return 0;
+}
+
+#if defined(CONFIG_PM) && !defined(CONFIG_HAS_EARLYSUSPEND)
+static int msm_fb_suspend(struct platform_device *pdev, pm_message_t state)
+{
+        struct msm_fb_data_type *mfd;
+        int ret = 0;
+
+        MSM_FB_DEBUG("msm_fb_suspend\n");
+
+        mfd = (struct msm_fb_data_type *)platform_get_drvdata(pdev);
+
+        if ((!mfd) || (mfd->key != MFD_KEY))
+                return 0;
+
+        acquire_console_sem();
+        fb_set_suspend(mfd->fbi, 1);
+
+        ret = msm_fb_suspend_sub(mfd);
+        if (ret != 0) {
+                printk(KERN_ERR "msm_fb: failed to suspend! %d\n", ret);
+                fb_set_suspend(mfd->fbi, 0);
+        } else {
+                pdev->dev.power.power_state = state;
+        }
+
+        release_console_sem();
+        return ret;
+}
+#else
+#define msm_fb_suspend NULL
+#endif
+
+static int msm_fb_suspend_sub(struct msm_fb_data_type *mfd)
+{
+        int ret = 0;
+
+        if ((!mfd) || (mfd->key != MFD_KEY))
+                return 0;
+
+        /*
+         * suspend this channel
+         */
+        mfd->suspend.sw_refreshing_enable = mfd->sw_refreshing_enable;
+        mfd->suspend.op_enable = mfd->op_enable;
+        mfd->suspend.panel_power_on = mfd->panel_power_on;
+
+        if (mfd->op_enable) {
+                ret =
+                     msm_fb_blank_sub(FB_BLANK_POWERDOWN, mfd->fbi,
+                                      mfd->suspend.op_enable);
+                if (ret) {
+                        MSM_FB_INFO
+                            ("msm_fb_suspend: can't turn off display!\n");
+                        return ret;
+                }
+                mfd->op_enable = FALSE;
+        }
+        /*
+         * try to power down
+         */
+        mdp_pipe_ctrl(MDP_MASTER_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+
+        /*
+         * detach display channel irq if there's any
+         * or wait until vsync-resync completes
+         */
+        if ((mfd->dest == DISPLAY_LCD)) {
+                if (mfd->panel_info.lcd.vsync_enable) {
+                        if (mfd->panel_info.lcd.hw_vsync_mode) {
+                                if (mfd->channel_irq != 0)
+                                        disable_irq(mfd->channel_irq);
+                        } else {
+                                volatile boolean vh_pending;
+                                do {
+                                        vh_pending = mfd->vsync_handler_pending;
+                                } while (vh_pending);
+                        }
+                }
+        }
+
+        return 0;
+}
+
+#if defined(CONFIG_PM) && !defined(CONFIG_HAS_EARLYSUSPEND)
+static int msm_fb_resume(struct platform_device *pdev)
+{
+        /* This resume function is called when interrupt is enabled.
+         */
+        int ret = 0;
+        struct msm_fb_data_type *mfd;
+
+        MSM_FB_DEBUG("msm_fb_resume\n");
+
+        mfd = (struct msm_fb_data_type *)platform_get_drvdata(pdev);
+
+        if ((!mfd) || (mfd->key != MFD_KEY))
+                return 0;
+
+        acquire_console_sem();
+        ret = msm_fb_resume_sub(mfd);
+        pdev->dev.power.power_state = PMSG_ON;
+        fb_set_suspend(mfd->fbi, 1);
+        release_console_sem();
+
+        return ret;
+}
+#else
+#define msm_fb_resume NULL
+#endif
+
+static int msm_fb_resume_sub(struct msm_fb_data_type *mfd)
+{
+        int ret = 0;
+
+        if ((!mfd) || (mfd->key != MFD_KEY))
+                return 0;
+
+        /* attach display channel irq if there's any */
+        if (mfd->channel_irq != 0)
+                enable_irq(mfd->channel_irq);
+
+        /* resume state var recover */
+        mfd->sw_refreshing_enable = mfd->suspend.sw_refreshing_enable;
+        mfd->op_enable = mfd->suspend.op_enable;
+
+        if (mfd->suspend.panel_power_on) {
+                ret =
+                     msm_fb_blank_sub(FB_BLANK_UNBLANK, mfd->fbi,
+                                      mfd->op_enable);
+                if (ret)
+                        MSM_FB_INFO("msm_fb_resume: can't turn on display!\n");
+        }
+
+        return ret;
+}
+
+static struct platform_driver msm_fb_driver = {
+        .probe = msm_fb_probe,
+        .remove = msm_fb_remove,
+#ifndef CONFIG_HAS_EARLYSUSPEND
+        .suspend = msm_fb_suspend,
+        .resume = msm_fb_resume,
+#endif
+        .shutdown = NULL,
+        .driver = {
+                   /* Driver name must match the device name added in platform.c. */
+                   .name = "msm_fb",
+                   },
+};
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void msmfb_early_suspend(struct early_suspend *h)
+{
+        struct msm_fb_data_type *mfd = container_of(h, struct msm_fb_data_type,
+                                                    early_suspend);
+        msm_fb_suspend_sub(mfd);
+}
+
+static void msmfb_early_resume(struct early_suspend *h)
+{
+        struct msm_fb_data_type *mfd = container_of(h, struct msm_fb_data_type,
+                                                    early_suspend);
+        msm_fb_resume_sub(mfd);
+}
+#endif
+
+void msm_fb_set_backlight(struct msm_fb_data_type *mfd, __u32 bkl_lvl, u32 save)
+{
+        struct msm_fb_panel_data *pdata;
+
+        pdata = (struct msm_fb_panel_data *)mfd->pdev->dev.platform_data;
+
+        if ((pdata) && (pdata->set_backlight)) {
+                down(&mfd->sem);
+                if ((bkl_lvl != mfd->bl_level) || (!save)) {
+                        u32 old_lvl;
+
+                        old_lvl = mfd->bl_level;
+                        mfd->bl_level = bkl_lvl;
+                        pdata->set_backlight(mfd);
+
+                        if (!save)
+                                mfd->bl_level = old_lvl;
+                }
+                up(&mfd->sem);
+        }
+}
+
+static int msm_fb_blank_sub(int blank_mode, struct fb_info *info,
+                            boolean op_enable)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        struct msm_fb_panel_data *pdata = NULL;
+        int ret = 0;
+
+        if (!op_enable)
+                return -EPERM;
+
+        pdata = (struct msm_fb_panel_data *)mfd->pdev->dev.platform_data;
+        if ((!pdata) || (!pdata->on) || (!pdata->off)) {
+                printk(KERN_ERR "msm_fb_blank_sub: no panel operation detected!\n");
+                return -ENODEV;
+        }
+
+        switch (blank_mode) {
+        case FB_BLANK_UNBLANK:
+                if (!mfd->panel_power_on) {
+                        mdelay(100);
+                        ret = pdata->on(mfd->pdev);
+                        if (ret == 0) {
+                                mfd->panel_power_on = TRUE;
+
+                                msm_fb_set_backlight(mfd,
+                                                     mfd->bl_level, 0);
+
+/* ToDo: possible conflict with android which doesn't expect sw refresher */
+/*
+          if (!mfd->hw_refresh)
+          {
+            if ((ret = msm_fb_resume_sw_refresher(mfd)) != 0)
+            {
+              MSM_FB_INFO("msm_fb_blank_sub: msm_fb_resume_sw_refresher failed = %d!\n",ret);
+            }
+          }
+*/
+                        }
+                }
+                break;
+
+        case FB_BLANK_VSYNC_SUSPEND:
+        case FB_BLANK_HSYNC_SUSPEND:
+        case FB_BLANK_NORMAL:
+        case FB_BLANK_POWERDOWN:
+        default:
+                if (mfd->panel_power_on) {
+                        int curr_pwr_state;
+
+                        mfd->op_enable = FALSE;
+                        curr_pwr_state = mfd->panel_power_on;
+                        mfd->panel_power_on = FALSE;
+
+                        mdelay(100);
+                        ret = pdata->off(mfd->pdev);
+                        if (ret)
+                                mfd->panel_power_on = curr_pwr_state;
+
+                        msm_fb_set_backlight(mfd, 0, 0);
+                        mfd->op_enable = TRUE;
+                }
+                break;
+        }
+
+        return ret;
+}
+
+static void msm_fb_fillrect(struct fb_info *info,
+                            const struct fb_fillrect *rect)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+
+        cfb_fillrect(info, rect);
+        if (!mfd->hw_refresh && (info->var.yoffset == 0) &&
+                !mfd->sw_currently_refreshing) {
+                struct fb_var_screeninfo var;
+
+                var = info->var;
+                var.reserved[0] = 0x54445055;
+                var.reserved[1] = (rect->dy << 16) | (rect->dx);
+                var.reserved[2] = ((rect->dy + rect->height) << 16) |
+                    (rect->dx + rect->width);
+
+                msm_fb_pan_display(&var, info);
+        }
+}
+
+static void msm_fb_copyarea(struct fb_info *info,
+                            const struct fb_copyarea *area)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+
+        cfb_copyarea(info, area);
+        if (!mfd->hw_refresh && (info->var.yoffset == 0) &&
+                !mfd->sw_currently_refreshing) {
+                struct fb_var_screeninfo var;
+
+                var = info->var;
+                var.reserved[0] = 0x54445055;
+                var.reserved[1] = (area->dy << 16) | (area->dx);
+                var.reserved[2] = ((area->dy + area->height) << 16) |
+                    (area->dx + area->width);
+
+                msm_fb_pan_display(&var, info);
+        }
+}
+
+static void msm_fb_imageblit(struct fb_info *info, const struct fb_image *image)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+
+        cfb_imageblit(info, image);
+        if (!mfd->hw_refresh && (info->var.yoffset == 0) &&
+                !mfd->sw_currently_refreshing) {
+                struct fb_var_screeninfo var;
+
+                var = info->var;
+                var.reserved[0] = 0x54445055;
+                var.reserved[1] = (image->dy << 16) | (image->dx);
+                var.reserved[2] = ((image->dy + image->height) << 16) |
+                    (image->dx + image->width);
+
+                msm_fb_pan_display(&var, info);
+        }
+}
+
+static int msm_fb_blank(int blank_mode, struct fb_info *info)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        return msm_fb_blank_sub(blank_mode, info, mfd->op_enable);
+}
+
+static int msm_fb_set_lut(struct fb_cmap *cmap, struct fb_info *info)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+
+        if (!mfd->lut_update)
+                return -ENODEV;
+
+        mfd->lut_update(info, cmap);
+        return 0;
+}
+
+/*
+ * Custom Framebuffer mmap() function for MSM driver.
+ * Differs from standard mmap() function by allowing for customized
+ * page-protection.
+ */
+static int msm_fb_mmap(struct fb_info *info, struct vm_area_struct * vma)
+{
+        /* Get frame buffer memory range. */
+        unsigned long start = info->fix.smem_start;
+        u32 len = PAGE_ALIGN((start & ~PAGE_MASK) + info->fix.smem_len);
+        unsigned long off = vma->vm_pgoff << PAGE_SHIFT;
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        if (off >= len) {
+                /* memory mapped io */
+                off -= len;
+                if (info->var.accel_flags) {
+                        mutex_unlock(&info->lock);
+                        return -EINVAL;
+                }
+                start = info->fix.mmio_start;
+                len = PAGE_ALIGN((start & ~PAGE_MASK) + info->fix.mmio_len);
+        }
+
+        /* Set VM flags. */
+        start &= PAGE_MASK;
+        if ((vma->vm_end - vma->vm_start + off) > len)
+                return -EINVAL;
+        off += start;
+        vma->vm_pgoff = off >> PAGE_SHIFT;
+        /* This is an IO map - tell maydump to skip this VMA */
+        vma->vm_flags |= VM_IO | VM_RESERVED;
+
+        /* Set VM page protection */
+        if (mfd->mdp_fb_page_protection == MDP_FB_PAGE_PROTECTION_WRITECOMBINE)
+                vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
+        else if (mfd->mdp_fb_page_protection ==
+                        MDP_FB_PAGE_PROTECTION_WRITETHROUGHCACHE)
+                vma->vm_page_prot = pgprot_writethroughcache(vma->vm_page_prot);
+        else if (mfd->mdp_fb_page_protection ==
+                        MDP_FB_PAGE_PROTECTION_WRITEBACKCACHE)
+                vma->vm_page_prot = pgprot_writebackcache(vma->vm_page_prot);
+        else if (mfd->mdp_fb_page_protection ==
+                        MDP_FB_PAGE_PROTECTION_WRITEBACKWACACHE)
+                vma->vm_page_prot = pgprot_writebackwacache(vma->vm_page_prot);
+        else
+                vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+        /* Remap the frame buffer I/O range */
+        if (io_remap_pfn_range(vma, vma->vm_start, off >> PAGE_SHIFT,
+                                vma->vm_end - vma->vm_start,
+                                vma->vm_page_prot))
+                return -EAGAIN;
+
+        return 0;
+}
+
+static struct fb_ops msm_fb_ops = {
+        .owner = THIS_MODULE,
+        .fb_open = msm_fb_open,
+        .fb_release = msm_fb_release,
+        .fb_read = NULL,
+        .fb_write = NULL,
+        .fb_cursor = NULL,
+        .fb_check_var = msm_fb_check_var,       /* vinfo check */
+        .fb_set_par = msm_fb_set_par,   /* set the video mode according to info->var */
+        .fb_setcolreg = NULL,   /* set color register */
+        .fb_blank = msm_fb_blank,       /* blank display */
+        .fb_pan_display = msm_fb_pan_display,   /* pan display */
+        .fb_fillrect = msm_fb_fillrect, /* Draws a rectangle */
+        .fb_copyarea = msm_fb_copyarea, /* Copy data from area to another */
+        .fb_imageblit = msm_fb_imageblit,       /* Draws a image to the display */
+        .fb_rotate = NULL,
+        .fb_sync = NULL,        /* wait for blit idle, optional */
+        .fb_ioctl = msm_fb_ioctl,       /* perform fb specific ioctl (optional) */
+        .fb_mmap = msm_fb_mmap,
+};
+
+static int msm_fb_register(struct msm_fb_data_type *mfd)
+{
+        int ret = -ENODEV;
+        int bpp;
+        struct msm_panel_info *panel_info = &mfd->panel_info;
+        struct fb_info *fbi = mfd->fbi;
+        struct fb_fix_screeninfo *fix;
+        struct fb_var_screeninfo *var;
+        int *id;
+        int fbram_offset;
+
+        /*
+         * fb info initialization
+         */
+        fix = &fbi->fix;
+        var = &fbi->var;
+
+        fix->type_aux = 0;      /* if type == FB_TYPE_INTERLEAVED_PLANES */
+        fix->visual = FB_VISUAL_TRUECOLOR;      /* True Color */
+        fix->ywrapstep = 0;     /* No support */
+        fix->mmio_start = 0;    /* No MMIO Address */
+        fix->mmio_len = 0;      /* No MMIO Address */
+        fix->accel = FB_ACCEL_NONE;/* FB_ACCEL_MSM needes to be added in fb.h */
+
+        var->xoffset = 0,       /* Offset from virtual to visible */
+        var->yoffset = 0,       /* resolution */
+        var->grayscale = 0,     /* No graylevels */
+        var->nonstd = 0,        /* standard pixel format */
+        var->activate = FB_ACTIVATE_VBL,        /* activate it at vsync */
+        var->height = -1,       /* height of picture in mm */
+        var->width = -1,        /* width of picture in mm */
+        var->accel_flags = 0,   /* acceleration flags */
+        var->sync = 0,  /* see FB_SYNC_* */
+        var->rotate = 0,        /* angle we rotate counter clockwise */
+        mfd->op_enable = FALSE;
+
+        switch (mfd->fb_imgType) {
+        case MDP_RGB_565:
+                fix->type = FB_TYPE_PACKED_PIXELS;
+                fix->xpanstep = 1;
+                fix->ypanstep = 1;
+                var->vmode = FB_VMODE_NONINTERLACED;
+                var->blue.offset = 0;
+                var->green.offset = 5;
+                var->red.offset = 11;
+                var->blue.length = 5;
+                var->green.length = 6;
+                var->red.length = 5;
+                var->blue.msb_right = 0;
+                var->green.msb_right = 0;
+                var->red.msb_right = 0;
+                var->transp.offset = 0;
+                var->transp.length = 0;
+                bpp = 2;
+                break;
+
+        case MDP_RGB_888:
+                fix->type = FB_TYPE_PACKED_PIXELS;
+                fix->xpanstep = 1;
+                fix->ypanstep = 1;
+                var->vmode = FB_VMODE_NONINTERLACED;
+                var->blue.offset = 0;
+                var->green.offset = 8;
+                var->red.offset = 16;
+                var->blue.length = 8;
+                var->green.length = 8;
+                var->red.length = 8;
+                var->blue.msb_right = 0;
+                var->green.msb_right = 0;
+                var->red.msb_right = 0;
+                var->transp.offset = 0;
+                var->transp.length = 0;
+                bpp = 3;
+                break;
+
+        case MDP_ARGB_8888:
+                fix->type = FB_TYPE_PACKED_PIXELS;
+                fix->xpanstep = 1;
+                fix->ypanstep = 1;
+                var->vmode = FB_VMODE_NONINTERLACED;
+                var->blue.offset = 0;
+                var->green.offset = 8;
+                var->red.offset = 16;
+                var->blue.length = 8;
+                var->green.length = 8;
+                var->red.length = 8;
+                var->blue.msb_right = 0;
+                var->green.msb_right = 0;
+                var->red.msb_right = 0;
+                var->transp.offset = 24;
+                var->transp.length = 8;
+                bpp = 3;
+                break;
+
+        case MDP_YCRYCB_H2V1:
+                /* ToDo: need to check TV-Out YUV422i framebuffer format */
+                /*       we might need to create new type define */
+                fix->type = FB_TYPE_INTERLEAVED_PLANES;
+                fix->xpanstep = 2;
+                fix->ypanstep = 1;
+                var->vmode = FB_VMODE_NONINTERLACED;
+
+                /* how about R/G/B offset? */
+                var->blue.offset = 0;
+                var->green.offset = 5;
+                var->red.offset = 11;
+                var->blue.length = 5;
+                var->green.length = 6;
+                var->red.length = 5;
+                var->blue.msb_right = 0;
+                var->green.msb_right = 0;
+                var->red.msb_right = 0;
+                var->transp.offset = 0;
+                var->transp.length = 0;
+                bpp = 2;
+                break;
+
+        default:
+                MSM_FB_ERR("msm_fb_init: fb %d unkown image type!\n",
+                           mfd->index);
+                return ret;
+        }
+
+        /* The adreno GPU hardware requires that the pitch be aligned to
+           32 pixels for color buffers, so for the cases where the GPU
+           is writing directly to fb0, the framebuffer pitch
+           also needs to be 32 pixel aligned */
+
+        if (mfd->index == 0)
+                fix->line_length = ALIGN(panel_info->xres * bpp, 32);
+        else
+                fix->line_length = panel_info->xres * bpp;
+
+        fix->smem_len = fix->line_length * panel_info->yres * mfd->fb_page;
+
+        mfd->var_xres = panel_info->xres;
+        mfd->var_yres = panel_info->yres;
+
+        var->pixclock = mfd->panel_info.clk_rate;
+        mfd->var_pixclock = var->pixclock;
+
+        var->xres = panel_info->xres;
+        var->yres = panel_info->yres;
+        var->xres_virtual = panel_info->xres;
+        var->yres_virtual = panel_info->yres * mfd->fb_page;
+        var->bits_per_pixel = bpp * 8,  /* FrameBuffer color depth */
+                /*
+                 * id field for fb app
+                 */
+            id = (int *)&mfd->panel;
+
+#if defined(CONFIG_FB_MSM_MDP22)
+        snprintf(fix->id, sizeof(fix->id), "msmfb22_%x", (__u32) *id);
+#elif defined(CONFIG_FB_MSM_MDP30)
+        snprintf(fix->id, sizeof(fix->id), "msmfb30_%x", (__u32) *id);
+#elif defined(CONFIG_FB_MSM_MDP31)
+        snprintf(fix->id, sizeof(fix->id), "msmfb31_%x", (__u32) *id);
+#elif defined(CONFIG_FB_MSM_MDP40)
+        snprintf(fix->id, sizeof(fix->id), "msmfb40_%x", (__u32) *id);
+#else
+        error CONFIG_FB_MSM_MDP undefined !
+#endif
+         fbi->fbops = &msm_fb_ops;
+        fbi->flags = FBINFO_FLAG_DEFAULT;
+        fbi->pseudo_palette = msm_fb_pseudo_palette;
+
+        mfd->ref_cnt = 0;
+        mfd->sw_currently_refreshing = FALSE;
+        mfd->sw_refreshing_enable = TRUE;
+        mfd->panel_power_on = FALSE;
+
+        mfd->pan_waiting = FALSE;
+        init_completion(&mfd->pan_comp);
+        init_completion(&mfd->refresher_comp);
+        init_MUTEX(&mfd->sem);
+
+        fbram_offset = PAGE_ALIGN((int)fbram)-(int)fbram;
+        fbram += fbram_offset;
+        fbram_phys += fbram_offset;
+        fbram_size -= fbram_offset;
+
+        if (fbram_size < fix->smem_len) {
+                printk(KERN_ERR "error: no more framebuffer memory!\n");
+                return -ENOMEM;
+        }
+
+        fbi->screen_base = fbram;
+        fbi->fix.smem_start = (unsigned long)fbram_phys;
+
+        memset(fbi->screen_base, 0x0, fix->smem_len);
+
+        mfd->op_enable = TRUE;
+        mfd->panel_power_on = FALSE;
+
+        /* cursor memory allocation */
+        if (mfd->cursor_update) {
+                mfd->cursor_buf = dma_alloc_coherent(NULL,
+                                        MDP_CURSOR_SIZE,
+                                        (dma_addr_t *) &mfd->cursor_buf_phys,
+                                        GFP_KERNEL);
+                if (!mfd->cursor_buf)
+                        mfd->cursor_update = 0;
+        }
+
+        if (mfd->lut_update) {
+                ret = fb_alloc_cmap(&fbi->cmap, 256, 0);
+                if (ret)
+                        printk(KERN_ERR "%s: fb_alloc_cmap() failed!\n",
+                                        __func__);
+        }
+
+        if (register_framebuffer(fbi) < 0) {
+                if (mfd->lut_update)
+                        fb_dealloc_cmap(&fbi->cmap);
+
+                if (mfd->cursor_buf)
+                        dma_free_coherent(NULL,
+                                MDP_CURSOR_SIZE,
+                                mfd->cursor_buf,
+                                (dma_addr_t) mfd->cursor_buf_phys);
+
+                mfd->op_enable = FALSE;
+                return -EPERM;
+        }
+
+        fbram += fix->smem_len;
+        fbram_phys += fix->smem_len;
+        fbram_size -= fix->smem_len;
+
+        MSM_FB_INFO
+            ("FrameBuffer[%d] %dx%d size=%d bytes is registered successfully!\n",
+             mfd->index, fbi->var.xres, fbi->var.yres, fbi->fix.smem_len);
+
+#ifdef CONFIG_FB_MSM_LOGO
+        if (!load_565rle_image(INIT_IMAGE_FILE)) ;      /* Flip buffer */
+#endif
+        ret = 0;
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+        mfd->early_suspend.suspend = msmfb_early_suspend;
+        mfd->early_suspend.resume = msmfb_early_resume;
+        mfd->early_suspend.level = EARLY_SUSPEND_LEVEL_DISABLE_FB - 2;
+        register_early_suspend(&mfd->early_suspend);
+#endif
+
+#ifdef MSM_FB_ENABLE_DBGFS
+        {
+                struct dentry *root;
+                struct dentry *sub_dir;
+                char sub_name[2];
+
+                root = msm_fb_get_debugfs_root();
+                if (root != NULL) {
+                        sub_name[0] = (char)(mfd->index + 0x30);
+                        sub_name[1] = '\0';
+                        sub_dir = debugfs_create_dir(sub_name, root);
+                } else {
+                        sub_dir = NULL;
+                }
+
+                mfd->sub_dir = sub_dir;
+
+                if (sub_dir) {
+                        msm_fb_debugfs_file_create(sub_dir, "op_enable",
+                                                   (u32 *) &mfd->op_enable);
+                        msm_fb_debugfs_file_create(sub_dir, "panel_power_on",
+                                                   (u32 *) &mfd->
+                                                   panel_power_on);
+                        msm_fb_debugfs_file_create(sub_dir, "ref_cnt",
+                                                   (u32 *) &mfd->ref_cnt);
+                        msm_fb_debugfs_file_create(sub_dir, "fb_imgType",
+                                                   (u32 *) &mfd->fb_imgType);
+                        msm_fb_debugfs_file_create(sub_dir,
+                                                   "sw_currently_refreshing",
+                                                   (u32 *) &mfd->
+                                                   sw_currently_refreshing);
+                        msm_fb_debugfs_file_create(sub_dir,
+                                                   "sw_refreshing_enable",
+                                                   (u32 *) &mfd->
+                                                   sw_refreshing_enable);
+
+                        msm_fb_debugfs_file_create(sub_dir, "xres",
+                                                   (u32 *) &mfd->panel_info.
+                                                   xres);
+                        msm_fb_debugfs_file_create(sub_dir, "yres",
+                                                   (u32 *) &mfd->panel_info.
+                                                   yres);
+                        msm_fb_debugfs_file_create(sub_dir, "bpp",
+                                                   (u32 *) &mfd->panel_info.
+                                                   bpp);
+                        msm_fb_debugfs_file_create(sub_dir, "type",
+                                                   (u32 *) &mfd->panel_info.
+                                                   type);
+                        msm_fb_debugfs_file_create(sub_dir, "wait_cycle",
+                                                   (u32 *) &mfd->panel_info.
+                                                   wait_cycle);
+                        msm_fb_debugfs_file_create(sub_dir, "pdest",
+                                                   (u32 *) &mfd->panel_info.
+                                                   pdest);
+                        msm_fb_debugfs_file_create(sub_dir, "backbuff",
+                                                   (u32 *) &mfd->panel_info.
+                                                   fb_num);
+                        msm_fb_debugfs_file_create(sub_dir, "clk_rate",
+                                                   (u32 *) &mfd->panel_info.
+                                                   clk_rate);
+                        msm_fb_debugfs_file_create(sub_dir, "frame_count",
+                                                   (u32 *) &mfd->panel_info.
+                                                   frame_count);
+
+
+                        switch (mfd->dest) {
+                        case DISPLAY_LCD:
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "vsync_enable",
+                                (u32 *)&mfd->panel_info.lcd.vsync_enable);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "refx100",
+                                (u32 *) &mfd->panel_info.lcd. refx100);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "v_back_porch",
+                                (u32 *) &mfd->panel_info.lcd.v_back_porch);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "v_front_porch",
+                                (u32 *) &mfd->panel_info.lcd.v_front_porch);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "v_pulse_width",
+                                (u32 *) &mfd->panel_info.lcd.v_pulse_width);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "hw_vsync_mode",
+                                (u32 *) &mfd->panel_info.lcd.hw_vsync_mode);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "vsync_notifier_period", (u32 *)
+                                &mfd->panel_info.lcd.vsync_notifier_period);
+                                break;
+
+                        case DISPLAY_LCDC:
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "h_back_porch",
+                                (u32 *) &mfd->panel_info.lcdc.h_back_porch);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "h_front_porch",
+                                (u32 *) &mfd->panel_info.lcdc.h_front_porch);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "h_pulse_width",
+                                (u32 *) &mfd->panel_info.lcdc.h_pulse_width);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "v_back_porch",
+                                (u32 *) &mfd->panel_info.lcdc.v_back_porch);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "v_front_porch",
+                                (u32 *) &mfd->panel_info.lcdc.v_front_porch);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "v_pulse_width",
+                                (u32 *) &mfd->panel_info.lcdc.v_pulse_width);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "border_clr",
+                                (u32 *) &mfd->panel_info.lcdc.border_clr);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "underflow_clr",
+                                (u32 *) &mfd->panel_info.lcdc.underflow_clr);
+                                msm_fb_debugfs_file_create(sub_dir,
+                                "hsync_skew",
+                                (u32 *) &mfd->panel_info.lcdc.hsync_skew);
+                                break;
+
+                        default:
+                                break;
+                        }
+                }
+        }
+#endif /* MSM_FB_ENABLE_DBGFS */
+
+        return ret;
+}
+
+static int msm_fb_open(struct fb_info *info, int user)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+
+        if (!mfd->ref_cnt) {
+                mdp_set_dma_pan_info(info, NULL, TRUE);
+
+                if (msm_fb_blank_sub(FB_BLANK_UNBLANK, info, mfd->op_enable)) {
+                        printk(KERN_ERR "msm_fb_open: can't turn on display!\n");
+                        return -1;
+                }
+        }
+
+        mfd->ref_cnt++;
+        return 0;
+}
+
+static int msm_fb_release(struct fb_info *info, int user)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        int ret = 0;
+
+        if (!mfd->ref_cnt) {
+                MSM_FB_INFO("msm_fb_release: try to close unopened fb %d!\n",
+                            mfd->index);
+                return -EINVAL;
+        }
+
+        mfd->ref_cnt--;
+
+        if (!mfd->ref_cnt) {
+                if ((ret =
+                     msm_fb_blank_sub(FB_BLANK_POWERDOWN, info,
+                                      mfd->op_enable)) != 0) {
+                        printk(KERN_ERR "msm_fb_release: can't turn off display!\n");
+                        return ret;
+                }
+        }
+
+        return ret;
+}
+
+DECLARE_MUTEX(msm_fb_pan_sem);
+
+static int msm_fb_pan_display(struct fb_var_screeninfo *var,
+                              struct fb_info *info)
+{
+        struct mdp_dirty_region dirty;
+        struct mdp_dirty_region *dirtyPtr = NULL;
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+
+        if ((!mfd->op_enable) || (!mfd->panel_power_on))
+                return -EPERM;
+
+        if (var->xoffset > (info->var.xres_virtual - info->var.xres))
+                return -EINVAL;
+
+        if (var->yoffset > (info->var.yres_virtual - info->var.yres))
+                return -EINVAL;
+
+        if (info->fix.xpanstep)
+                info->var.xoffset =
+                    (var->xoffset / info->fix.xpanstep) * info->fix.xpanstep;
+
+        if (info->fix.ypanstep)
+                info->var.yoffset =
+                    (var->yoffset / info->fix.ypanstep) * info->fix.ypanstep;
+
+        /* "UPDT" */
+        if (var->reserved[0] == 0x54445055) {
+                dirty.xoffset = var->reserved[1] & 0xffff;
+                dirty.yoffset = (var->reserved[1] >> 16) & 0xffff;
+
+                if ((var->reserved[2] & 0xffff) <= dirty.xoffset)
+                        return -EINVAL;
+                if (((var->reserved[2] >> 16) & 0xffff) <= dirty.yoffset)
+                        return -EINVAL;
+
+                dirty.width = (var->reserved[2] & 0xffff) - dirty.xoffset;
+                dirty.height =
+                    ((var->reserved[2] >> 16) & 0xffff) - dirty.yoffset;
+                info->var.yoffset = var->yoffset;
+
+                if (dirty.xoffset < 0)
+                        return -EINVAL;
+
+                if (dirty.yoffset < 0)
+                        return -EINVAL;
+
+                if ((dirty.xoffset + dirty.width) > info->var.xres)
+                        return -EINVAL;
+
+                if ((dirty.yoffset + dirty.height) > info->var.yres)
+                        return -EINVAL;
+
+                if ((dirty.width <= 0) || (dirty.height <= 0))
+                        return -EINVAL;
+
+                dirtyPtr = &dirty;
+        }
+
+        /* Flip */
+        /* A constant value is used to indicate that we should change the DMA
+           output buffer instead of just panning */
+
+        if (var->reserved[0] == 0x466c6970) {
+                unsigned long length, address;
+                struct file *p_src_file;
+                struct mdp_img imgdata;
+                int bpp;
+
+                if (mfd->allow_set_offset) {
+                        imgdata.memory_id = var->reserved[1];
+                        imgdata.priv = var->reserved[2];
+
+                        /* If there is no memory ID then we want to reset back
+                           to the original fb visibility */
+                        if (var->reserved[1]) {
+                                if (var->reserved[4] == MDP_BLIT_SRC_GEM) {
+                                        panic("waaaaaaaaaaaaaah");
+                                        if ( /*get_gem_img(&imgdata,
+                                                (unsigned long *) &address,
+                                                 &length)*/ -1 < 0) {
+                                                return -1;
+                                        }
+                                } else {
+                                        /*get_img(&imgdata, info, &address,
+                                                        &length, &p_src_file);*/
+                                        panic("waaaaaah");
+                                }
+                                mfd->ibuf.visible_swapped = TRUE;
+                        } else {
+                                /* Flip back to the original address
+                                   adjusted for xoffset and yoffset */
+
+                                bpp = info->var.bits_per_pixel / 8;
+                                address = (unsigned long) info->fix.smem_start;
+                                address += info->var.xoffset * bpp +
+                                info->var.yoffset * info->fix.line_length;
+
+                                mfd->ibuf.visible_swapped = FALSE;
+                        }
+
+                        mdp_set_offset_info(info, address,
+                                (var->activate == FB_ACTIVATE_VBL));
+
+                        mfd->dma_fnc(mfd);
+                        return 0;
+                } else
+                        return -EINVAL;
+        }
+
+        down(&msm_fb_pan_sem);
+        mdp_set_dma_pan_info(info, dirtyPtr,
+                             (var->activate == FB_ACTIVATE_VBL));
+        mdp_dma_pan_update(info);
+        up(&msm_fb_pan_sem);
+
+        ++mfd->panel_info.frame_count;
+        return 0;
+}
+
+static int msm_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+
+        if (var->rotate != FB_ROTATE_UR)
+                return -EINVAL;
+        if (var->grayscale != info->var.grayscale)
+                return -EINVAL;
+
+        switch (var->bits_per_pixel) {
+        case 16:
+                if ((var->green.offset != 5) ||
+                        !((var->blue.offset == 11)
+                                || (var->blue.offset == 0)) ||
+                        !((var->red.offset == 11)
+                                || (var->red.offset == 0)) ||
+                        (var->blue.length != 5) ||
+                        (var->green.length != 6) ||
+                        (var->red.length != 5) ||
+                        (var->blue.msb_right != 0) ||
+                        (var->green.msb_right != 0) ||
+                        (var->red.msb_right != 0) ||
+                        (var->transp.offset != 0) ||
+                        (var->transp.length != 0))
+                                return -EINVAL;
+                break;
+
+        case 24:
+                if ((var->blue.offset != 0) ||
+                        (var->green.offset != 8) ||
+                        (var->red.offset != 16) ||
+                        (var->blue.length != 8) ||
+                        (var->green.length != 8) ||
+                        (var->red.length != 8) ||
+                        (var->blue.msb_right != 0) ||
+                        (var->green.msb_right != 0) ||
+                        (var->red.msb_right != 0) ||
+                        !(((var->transp.offset == 0) &&
+                                (var->transp.length == 0)) ||
+                          ((var->transp.offset == 24) &&
+                                (var->transp.length == 8))))
+                                return -EINVAL;
+                break;
+
+        default:
+                return -EINVAL;
+        }
+
+        if ((var->xres_virtual <= 0) || (var->yres_virtual <= 0))
+                return -EINVAL;
+
+        if (info->fix.smem_len <
+                (var->xres_virtual*var->yres_virtual*(var->bits_per_pixel/8)))
+                return -EINVAL;
+
+        if ((var->xres == 0) || (var->yres == 0))
+                return -EINVAL;
+
+        if ((var->xres > mfd->panel_info.xres) ||
+                (var->yres > mfd->panel_info.yres))
+                return -EINVAL;
+
+        if (var->xoffset > (var->xres_virtual - var->xres))
+                return -EINVAL;
+
+        if (var->yoffset > (var->yres_virtual - var->yres))
+                return -EINVAL;
+
+        return 0;
+}
+
+static int msm_fb_set_par(struct fb_info *info)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        struct fb_var_screeninfo *var = &info->var;
+        int old_imgType;
+        int blank = 0;
+
+        old_imgType = mfd->fb_imgType;
+        switch (var->bits_per_pixel) {
+        case 16:
+                if (var->red.offset == 0)
+                        mfd->fb_imgType = MDP_BGR_565;
+                else
+                        mfd->fb_imgType = MDP_RGB_565;
+                break;
+
+        case 24:
+                if ((var->transp.offset == 0) && (var->transp.length == 0))
+                        mfd->fb_imgType = MDP_RGB_888;
+                else if ((var->transp.offset == 24) &&
+                                (var->transp.length == 8)) {
+                        mfd->fb_imgType = MDP_ARGB_8888;
+                        info->var.bits_per_pixel = 32;
+                }
+                break;
+
+        default:
+                return -EINVAL;
+        }
+
+        if ((mfd->var_pixclock != var->pixclock) ||
+                (mfd->hw_refresh && ((mfd->fb_imgType != old_imgType) ||
+                                (mfd->var_pixclock != var->pixclock) ||
+                                (mfd->var_xres != var->xres) ||
+                                (mfd->var_yres != var->yres)))) {
+                mfd->var_xres = var->xres;
+                mfd->var_yres = var->yres;
+                mfd->var_pixclock = var->pixclock;
+                blank = 1;
+        }
+
+        if (blank) {
+                msm_fb_blank_sub(FB_BLANK_POWERDOWN, info, mfd->op_enable);
+                msm_fb_blank_sub(FB_BLANK_UNBLANK, info, mfd->op_enable);
+        }
+
+        return 0;
+}
+
+static int msm_fb_stop_sw_refresher(struct msm_fb_data_type *mfd)
+{
+        if (mfd->hw_refresh)
+                return -EPERM;
+
+        if (mfd->sw_currently_refreshing) {
+                down(&mfd->sem);
+                mfd->sw_currently_refreshing = FALSE;
+                up(&mfd->sem);
+
+                /* wait until the refresher finishes the last job */
+                wait_for_completion_killable(&mfd->refresher_comp);
+        }
+
+        return 0;
+}
+
+int msm_fb_resume_sw_refresher(struct msm_fb_data_type *mfd)
+{
+        boolean do_refresh;
+
+        if (mfd->hw_refresh)
+                return -EPERM;
+
+        down(&mfd->sem);
+        if ((!mfd->sw_currently_refreshing) && (mfd->sw_refreshing_enable)) {
+                do_refresh = TRUE;
+                mfd->sw_currently_refreshing = TRUE;
+        } else {
+                do_refresh = FALSE;
+        }
+        up(&mfd->sem);
+
+        if (do_refresh)
+                mdp_refresh_screen((unsigned long)mfd);
+
+        return 0;
+}
+
+void mdp_ppp_put_img(struct file *p_src_file, struct file *p_dst_file)
+{
+#ifdef CONFIG_ANDROID_PMEM
+        if (p_src_file)
+                put_pmem_file(p_src_file);
+        if (p_dst_file)
+                put_pmem_file(p_dst_file);
+#endif
+}
+
+int mdp_blit(struct fb_info *info, struct mdp_blit_req *req)
+{
+        int ret;
+        struct file *p_src_file = 0, *p_dst_file = 0;
+        if (unlikely(req->src_rect.h == 0 || req->src_rect.w == 0)) {
+                printk(KERN_ERR "mpd_ppp: src img of zero size!\n");
+                return -EINVAL;
+        }
+        if (unlikely(req->dst_rect.h == 0 || req->dst_rect.w == 0))
+                return 0;
+
+        ret = mdp_ppp_blit(info, req, &p_src_file, &p_dst_file);
+        mdp_ppp_put_img(p_src_file, p_dst_file);
+        return ret;
+}
+
+typedef void (*msm_dma_barrier_function_pointer) (void *, size_t);
+
+static inline void msm_fb_dma_barrier_for_rect(struct fb_info *info,
+                        struct mdp_img *img, struct mdp_rect *rect,
+                        msm_dma_barrier_function_pointer dma_barrier_fp
+                        )
+{
+        /*
+         * Compute the start and end addresses of the rectangles.
+         * NOTE: As currently implemented, the data between
+         *       the end of one row and the start of the next is
+         *       included in the address range rather than
+         *       doing multiple calls for each row.
+         */
+
+        char * const pmem_start = info->screen_base;
+/*      int bytes_per_pixel = mdp_get_bytes_per_pixel(img->format);
+        unsigned long start = (unsigned long)pmem_start + img->offset +
+                (img->width * rect->y + rect->x) * bytes_per_pixel;
+        size_t size  = ((rect->h - 1) * img->width + rect->w) * bytes_per_pixel;
+        (*dma_barrier_fp) ((void *) start, size);
+*/
+        panic("waaaaah");
+}
+
+static inline void msm_dma_nc_pre(void)
+{
+        dmb();
+}
+static inline void msm_dma_wt_pre(void)
+{
+        dmb();
+}
+static inline void msm_dma_todevice_wb_pre(void *start, size_t size)
+{
+        #warning this
+//      dma_cache_pre_ops(start, size, DMA_TO_DEVICE);
+}
+
+static inline void msm_dma_fromdevice_wb_pre(void *start, size_t size)
+{
+        #warning this
+//      dma_cache_pre_ops(start, size, DMA_FROM_DEVICE);
+}
+
+static inline void msm_dma_nc_post(void)
+{
+        dmb();
+}
+
+static inline void msm_dma_fromdevice_wt_post(void *start, size_t size)
+{
+        #warning this
+//      dma_cache_post_ops(start, size, DMA_FROM_DEVICE);
+}
+
+static inline void msm_dma_todevice_wb_post(void *start, size_t size)
+{
+        #warning this
+//      dma_cache_post_ops(start, size, DMA_TO_DEVICE);
+}
+
+static inline void msm_dma_fromdevice_wb_post(void *start, size_t size)
+{
+        #warning this
+//      dma_cache_post_ops(start, size, DMA_FROM_DEVICE);
+}
+
+/*
+ * Do the write barriers required to guarantee data is committed to RAM
+ * (from CPU cache or internal buffers) before a DMA operation starts.
+ * NOTE: As currently implemented, the data between
+ *       the end of one row and the start of the next is
+ *       included in the address range rather than
+ *       doing multiple calls for each row.
+*/
+static void msm_fb_ensure_memory_coherency_before_dma(struct fb_info *info,
+                struct mdp_blit_req *req_list,
+                int req_list_count)
+{
+#ifdef CONFIG_ARCH_QSD8X50
+        int i;
+
+        /*
+         * Normally, do the requested barriers for each address
+         * range that corresponds to a rectangle.
+         *
+         * But if at least one write barrier is requested for data
+         * going to or from the device but no address range is
+         * needed for that barrier, then do the barrier, but do it
+         * only once, no matter how many requests there are.
+         */
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        switch (mfd->mdp_fb_page_protection)    {
+        default:
+        case MDP_FB_PAGE_PROTECTION_NONCACHED:
+        case MDP_FB_PAGE_PROTECTION_WRITECOMBINE:
+                /*
+                 * The following barrier is only done at most once,
+                 * since further calls would be redundant.
+                 */
+                for (i = 0; i < req_list_count; i++) {
+                        if (!(req_list[i].flags
+                                & MDP_NO_DMA_BARRIER_START)) {
+                                msm_dma_nc_pre();
+                                break;
+                        }
+                }
+                break;
+
+        case MDP_FB_PAGE_PROTECTION_WRITETHROUGHCACHE:
+                /*
+                 * The following barrier is only done at most once,
+                 * since further calls would be redundant.
+                 */
+                for (i = 0; i < req_list_count; i++) {
+                        if (!(req_list[i].flags
+                                & MDP_NO_DMA_BARRIER_START)) {
+                                msm_dma_wt_pre();
+                                break;
+                        }
+                }
+                break;
+
+        case MDP_FB_PAGE_PROTECTION_WRITEBACKCACHE:
+        case MDP_FB_PAGE_PROTECTION_WRITEBACKWACACHE:
+                for (i = 0; i < req_list_count; i++) {
+                        if (!(req_list[i].flags &
+                                        MDP_NO_DMA_BARRIER_START)) {
+
+                                msm_fb_dma_barrier_for_rect(info,
+                                                &(req_list[i].src),
+                                                &(req_list[i].src_rect),
+                                                msm_dma_todevice_wb_pre
+                                                );
+
+                                msm_fb_dma_barrier_for_rect(info,
+                                                &(req_list[i].dst),
+                                                &(req_list[i].dst_rect),
+                                                msm_dma_todevice_wb_pre
+                                                );
+                        }
+                }
+                break;
+        }
+#else
+        dmb();
+#endif
+}
+
+
+/*
+ * Do the write barriers required to guarantee data will be re-read from RAM by
+ * the CPU after a DMA operation ends.
+ * NOTE: As currently implemented, the data between
+ *       the end of one row and the start of the next is
+ *       included in the address range rather than
+ *       doing multiple calls for each row.
+*/
+static void msm_fb_ensure_memory_coherency_after_dma(struct fb_info *info,
+                struct mdp_blit_req *req_list,
+                int req_list_count)
+{
+#ifdef CONFIG_ARCH_QSD8X50
+        int i;
+
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        switch (mfd->mdp_fb_page_protection)    {
+        default:
+        case MDP_FB_PAGE_PROTECTION_NONCACHED:
+        case MDP_FB_PAGE_PROTECTION_WRITECOMBINE:
+                /*
+                 * The following barrier is only done at most once,
+                 * since further calls would be redundant.
+                 */
+                for (i = 0; i < req_list_count; i++) {
+                        if (!(req_list[i].flags
+                                & MDP_NO_DMA_BARRIER_END)) {
+                                msm_dma_nc_post();
+                                break;
+                        }
+                }
+                break;
+
+        case MDP_FB_PAGE_PROTECTION_WRITETHROUGHCACHE:
+                for (i = 0; i < req_list_count; i++) {
+                        if (!(req_list[i].flags &
+                                        MDP_NO_DMA_BARRIER_END)) {
+
+                                msm_fb_dma_barrier_for_rect(info,
+                                                &(req_list[i].dst),
+                                                &(req_list[i].dst_rect),
+                                                msm_dma_fromdevice_wt_post
+                                                );
+                        }
+                }
+                break;
+        case MDP_FB_PAGE_PROTECTION_WRITEBACKCACHE:
+        case MDP_FB_PAGE_PROTECTION_WRITEBACKWACACHE:
+                for (i = 0; i < req_list_count; i++) {
+                        if (!(req_list[i].flags &
+                                        MDP_NO_DMA_BARRIER_END)) {
+
+                                msm_fb_dma_barrier_for_rect(info,
+                                                &(req_list[i].dst),
+                                                &(req_list[i].dst_rect),
+                                                msm_dma_fromdevice_wb_post
+                                                );
+                        }
+                }
+                break;
+        }
+#else
+        dmb();
+#endif
+}
+
+#ifdef CONFIG_MDP_PPP_ASYNC_OP
+void msm_fb_ensure_mem_coherency_after_dma(struct fb_info *info,
+        struct mdp_blit_req *req_list, int req_list_count)
+{
+        BUG_ON(!info);
+
+        /*
+         * Ensure that CPU cache and other internal CPU state is
+         * updated to reflect any change in memory modified by MDP blit
+         * DMA.
+         */
+        msm_fb_ensure_memory_coherency_after_dma(info,
+                        req_list, req_list_count);
+}
+
+static int msmfb_async_blit(struct fb_info *info, void __user *p)
+{
+        /*
+         * CAUTION: The names of the struct types intentionally *DON'T* match
+         * the names of the variables declared -- they appear to be swapped.
+         * Read the code carefully and you should see that the variable names
+         * make sense.
+         */
+        const int MAX_LIST_WINDOW = 16;
+        struct mdp_blit_req req_list[MAX_LIST_WINDOW];
+        struct mdp_blit_req_list req_list_header;
+
+        int count, i, req_list_count;
+
+        /* Get the count size for the total BLIT request. */
+        if (copy_from_user(&req_list_header, p, sizeof(req_list_header)))
+                return -EFAULT;
+        p += sizeof(req_list_header);
+        count = req_list_header.count;
+        while (count > 0) {
+                /*
+                 * Access the requests through a narrow window to decrease copy
+                 * overhead and make larger requests accessible to the
+                 * coherency management code.
+                 * NOTE: The window size is intended to be larger than the
+                 *       typical request size, but not require more than 2
+                 *       kbytes of stack storage.
+                 */
+                req_list_count = count;
+                if (req_list_count > MAX_LIST_WINDOW)
+                        req_list_count = MAX_LIST_WINDOW;
+                if (copy_from_user(&req_list, p,
+                                sizeof(struct mdp_blit_req)*req_list_count))
+                        return -EFAULT;
+
+                /*
+                 * Ensure that any data CPU may have previously written to
+                 * internal state (but not yet committed to memory) is
+                 * guaranteed to be committed to memory now.
+                 */
+                msm_fb_ensure_memory_coherency_before_dma(info,
+                                req_list, req_list_count);
+
+                /*
+                 * Do the blit DMA, if required -- returning early only if
+                 * there is a failure.
+                 */
+                for (i = 0; i < req_list_count; i++) {
+                        if (!(req_list[i].flags & MDP_NO_BLIT)) {
+                                int ret = 0;
+                                struct mdp_ppp_djob *job = NULL;
+
+                                if (unlikely(req_list[i].src_rect.h == 0 ||
+                                        req_list[i].src_rect.w == 0)) {
+                                        MSM_FB_ERR("mpd_ppp: "
+                                                "src img of zero size!\n");
+                                        return -EINVAL;
+                                }
+
+                                if (unlikely(req_list[i].dst_rect.h == 0 ||
+                                        req_list[i].dst_rect.w == 0))
+                                        continue;
+
+                                /* create a new display job */
+                                job = mdp_ppp_new_djob();
+                                if (unlikely(!job))
+                                        return -ENOMEM;
+
+                                job->info = info;
+                                memcpy(&job->req, &req_list[i],
+                                        sizeof(struct mdp_blit_req));
+
+                                /* Do the actual blit. */
+                                ret = mdp_ppp_blit(info, &job->req,
+                                        &job->p_src_file, &job->p_dst_file);
+
+                                /*
+                                 * Note that early returns don't guarantee
+                                 * memory coherency.
+                                 */
+                                if (ret || mdp_ppp_get_ret_code()) {
+                                        mdp_ppp_clear_curr_djob();
+                                        return ret;
+                                }
+                        }
+                }
+
+                /* Go to next window of requests. */
+                count -= req_list_count;
+                p += sizeof(struct mdp_blit_req)*req_list_count;
+        }
+        return 0;
+}
+#else
+
+/*
+ * NOTE: The userspace issues blit operations in a sequence, the sequence
+ * start with a operation marked START and ends in an operation marked
+ * END. It is guranteed by the userspace that all the blit operations
+ * between START and END are only within the regions of areas designated
+ * by the START and END operations and that the userspace doesnt modify
+ * those areas. Hence it would be enough to perform barrier/cache operations
+ * only on the START and END operations.
+ */
+static int msmfb_blit(struct fb_info *info, void __user *p)
+{
+        /*
+         * CAUTION: The names of the struct types intentionally *DON'T* match
+         * the names of the variables declared -- they appear to be swapped.
+         * Read the code carefully and you should see that the variable names
+         * make sense.
+         */
+        const int MAX_LIST_WINDOW = 16;
+        struct mdp_blit_req req_list[MAX_LIST_WINDOW];
+        struct mdp_blit_req_list req_list_header;
+
+        int count, i, req_list_count;
+
+        /* Get the count size for the total BLIT request. */
+        if (copy_from_user(&req_list_header, p, sizeof(req_list_header)))
+                return -EFAULT;
+        p += sizeof(req_list_header);
+        count = req_list_header.count;
+        while (count > 0) {
+                /*
+                 * Access the requests through a narrow window to decrease copy
+                 * overhead and make larger requests accessible to the
+                 * coherency management code.
+                 * NOTE: The window size is intended to be larger than the
+                 *       typical request size, but not require more than 2
+                 *       kbytes of stack storage.
+                 */
+                req_list_count = count;
+                if (req_list_count > MAX_LIST_WINDOW)
+                        req_list_count = MAX_LIST_WINDOW;
+                if (copy_from_user(&req_list, p,
+                                sizeof(struct mdp_blit_req)*req_list_count))
+                        return -EFAULT;
+
+                /*
+                 * Ensure that any data CPU may have previously written to
+                 * internal state (but not yet committed to memory) is
+                 * guaranteed to be committed to memory now.
+                 */
+                msm_fb_ensure_memory_coherency_before_dma(info,
+                                req_list, req_list_count);
+
+                /*
+                 * Do the blit DMA, if required -- returning early only if
+                 * there is a failure.
+                 */
+                for (i = 0; i < req_list_count; i++) {
+                        if (!(req_list[i].flags & MDP_NO_BLIT)) {
+                                /* Do the actual blit. */
+                                int ret = mdp_blit(info, &(req_list[i]));
+
+                                /*
+                                 * Note that early returns don't guarantee
+                                 * memory coherency.
+                                 */
+                                if (ret)
+                                        return ret;
+                        }
+                }
+
+                /*
+                 * Ensure that CPU cache and other internal CPU state is
+                 * updated to reflect any change in memory modified by MDP blit
+                 * DMA.
+                 */
+                msm_fb_ensure_memory_coherency_after_dma(info,
+                                req_list,
+                                req_list_count);
+
+                /* Go to next window of requests. */
+                count -= req_list_count;
+                p += sizeof(struct mdp_blit_req)*req_list_count;
+        }
+        return 0;
+}
+#endif
+
+#ifdef CONFIG_FB_MSM_OVERLAY
+static int msmfb_overlay_get(struct fb_info *info, void __user *p)
+{
+        struct mdp_overlay req;
+        int ret;
+
+        if (copy_from_user(&req, p, sizeof(req)))
+                return -EFAULT;
+
+        ret = mdp4_overlay_get(info, &req);
+        if (ret) {
+                printk(KERN_ERR "%s: ioctl failed \n",
+                        __func__);
+                return ret;
+        }
+        if (copy_to_user(p, &req, sizeof(req))) {
+                printk(KERN_ERR "%s: copy2user failed \n",
+                        __func__);
+                return -EFAULT;
+        }
+
+        return 0;
+}
+
+static int msmfb_overlay_set(struct fb_info *info, void __user *p)
+{
+        struct mdp_overlay req;
+        int ret;
+
+        if (copy_from_user(&req, p, sizeof(req)))
+                return -EFAULT;
+
+        ret = mdp4_overlay_set(info, &req);
+        if (ret) {
+                printk(KERN_ERR "%s:ioctl failed \n",
+                        __func__);
+                return ret;
+        }
+
+        if (copy_to_user(p, &req, sizeof(req))) {
+                printk(KERN_ERR "%s: copy2user failed \n",
+                        __func__);
+                return -EFAULT;
+        }
+
+        return 0;
+}
+
+static int msmfb_overlay_unset(struct fb_info *info, unsigned long *argp)
+{
+        int     ret, ndx;
+
+        ret = copy_from_user(&ndx, argp, sizeof(ndx));
+        if (ret) {
+                printk(KERN_ERR "%s:msmfb_overlay_unset ioctl failed \n",
+                        __func__);
+                return ret;
+        }
+
+        return mdp4_overlay_unset(info, ndx);
+}
+
+static int msmfb_overlay_play(struct fb_info *info, unsigned long *argp)
+{
+        int     ret;
+        struct msmfb_overlay_data req;
+        struct file *p_src_file = 0;
+
+        ret = copy_from_user(&req, argp, sizeof(req));
+        if (ret) {
+                printk(KERN_ERR "%s:msmfb_overlay_play ioctl failed \n",
+                        __func__);
+                return ret;
+        }
+
+        ret = mdp4_overlay_play(info, &req, &p_src_file);
+
+        if (p_src_file)
+                put_pmem_file(p_src_file);
+
+        return ret;
+}
+
+#endif
+
+DECLARE_MUTEX(msm_fb_ioctl_ppp_sem);
+DEFINE_MUTEX(msm_fb_ioctl_lut_sem);
+DEFINE_MUTEX(msm_fb_ioctl_hist_sem);
+
+/* Set color conversion matrix from user space */
+
+#ifndef CONFIG_FB_MSM_MDP40
+static void msmfb_set_color_conv(struct mdp_ccs *p)
+{
+        int i;
+
+        if (p->direction == MDP_CCS_RGB2YUV) {
+                /* MDP cmd block enable */
+                mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+                /* RGB->YUV primary forward matrix */
+                for (i = 0; i < MDP_CCS_SIZE; i++)
+                        writel(p->ccs[i], MDP_CSC_PFMVn(i));
+
+                #ifdef CONFIG_FB_MSM_MDP31
+                for (i = 0; i < MDP_BV_SIZE; i++)
+                        writel(p->bv[i], MDP_CSC_POST_BV2n(i));
+                #endif
+
+                /* MDP cmd block disable */
+                mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+        } else {
+                /* MDP cmd block enable */
+                mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+
+                /* YUV->RGB primary reverse matrix */
+                for (i = 0; i < MDP_CCS_SIZE; i++)
+                        writel(p->ccs[i], MDP_CSC_PRMVn(i));
+                for (i = 0; i < MDP_BV_SIZE; i++)
+                        writel(p->bv[i], MDP_CSC_PRE_BV1n(i));
+
+                /* MDP cmd block disable */
+                mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
+        }
+}
+#endif
+
+
+static int msm_fb_ioctl(struct fb_info *info, unsigned int cmd,
+                        unsigned long arg)
+{
+        struct msm_fb_data_type *mfd = (struct msm_fb_data_type *)info->par;
+        void __user *argp = (void __user *)arg;
+        struct fb_cursor cursor;
+        struct fb_cmap cmap;
+        struct mdp_histogram hist;
+#ifndef CONFIG_FB_MSM_MDP40
+        struct mdp_ccs ccs_matrix;
+#endif
+        struct mdp_page_protection fb_page_protection;
+        int ret = 0;
+
+        if (!mfd->op_enable)
+                return -EPERM;
+
+        switch (cmd) {
+#ifdef CONFIG_FB_MSM_OVERLAY
+        case MSMFB_OVERLAY_GET:
+                down(&msm_fb_ioctl_ppp_sem);
+                ret = msmfb_overlay_get(info, argp);
+                up(&msm_fb_ioctl_ppp_sem);
+                break;
+        case MSMFB_OVERLAY_SET:
+                down(&msm_fb_ioctl_ppp_sem);
+                ret = msmfb_overlay_set(info, argp);
+                up(&msm_fb_ioctl_ppp_sem);
+                break;
+        case MSMFB_OVERLAY_UNSET:
+                down(&msm_fb_ioctl_ppp_sem);
+                ret = msmfb_overlay_unset(info, argp);
+                up(&msm_fb_ioctl_ppp_sem);
+                break;
+        case MSMFB_OVERLAY_PLAY:
+                down(&msm_fb_ioctl_ppp_sem);
+                ret = msmfb_overlay_play(info, argp);
+                up(&msm_fb_ioctl_ppp_sem);
+                break;
+#endif
+        case MSMFB_BLIT:
+                down(&msm_fb_ioctl_ppp_sem);
+#ifdef CONFIG_MDP_PPP_ASYNC_OP
+                ret = msmfb_async_blit(info, argp);
+                mdp_ppp_wait(); /* Wait for all blits to be finished. */
+#else
+                ret = msmfb_blit(info, argp);
+#endif
+                up(&msm_fb_ioctl_ppp_sem);
+
+                break;
+
+        /* Ioctl for setting ccs matrix from user space */
+        case MSMFB_SET_CCS_MATRIX:
+#ifndef CONFIG_FB_MSM_MDP40
+                ret = copy_from_user(&ccs_matrix, argp, sizeof(ccs_matrix));
+                if (ret) {
+                        printk(KERN_ERR
+                                "%s:MSMFB_SET_CCS_MATRIX ioctl failed \n",
+                                __func__);
+                        return ret;
+                }
+
+                down(&msm_fb_ioctl_ppp_sem);
+                if (ccs_matrix.direction == MDP_CCS_RGB2YUV)
+                        mdp_ccs_rgb2yuv = ccs_matrix;
+                else
+                        mdp_ccs_yuv2rgb = ccs_matrix;
+
+                msmfb_set_color_conv(&ccs_matrix) ;
+                up(&msm_fb_ioctl_ppp_sem);
+#else
+                ret = -EINVAL;
+#endif
+
+                break;
+
+        /* Ioctl for getting ccs matrix to user space */
+        case MSMFB_GET_CCS_MATRIX:
+#ifndef CONFIG_FB_MSM_MDP40
+                ret = copy_from_user(&ccs_matrix, argp, sizeof(ccs_matrix)) ;
+                if (ret) {
+                        printk(KERN_ERR
+                                "%s:MSMFB_GET_CCS_MATRIX ioctl failed \n",
+                                 __func__);
+                        return ret;
+                }
+
+                down(&msm_fb_ioctl_ppp_sem);
+                if (ccs_matrix.direction == MDP_CCS_RGB2YUV)
+                        ccs_matrix = mdp_ccs_rgb2yuv;
+                 else
+                        ccs_matrix =  mdp_ccs_yuv2rgb;
+
+                ret = copy_to_user(argp, &ccs_matrix, sizeof(ccs_matrix));
+
+                if (ret)        {
+                        printk(KERN_ERR
+                                "%s:MSMFB_GET_CCS_MATRIX ioctl failed \n",
+                                 __func__);
+                        return ret ;
+                }
+                up(&msm_fb_ioctl_ppp_sem);
+#else
+                ret = -EINVAL;
+#endif
+
+                break;
+
+#ifdef CONFIG_MDP_PPP_ASYNC_OP
+        case MSMFB_ASYNC_BLIT:
+                down(&msm_fb_ioctl_ppp_sem);
+                ret = msmfb_async_blit(info, argp);
+                up(&msm_fb_ioctl_ppp_sem);
+                break;
+
+        case MSMFB_BLIT_FLUSH:
+                down(&msm_fb_ioctl_ppp_sem);
+                mdp_ppp_wait();
+                up(&msm_fb_ioctl_ppp_sem);
+                break;
+#endif
+
+        case MSMFB_GRP_DISP:
+#ifdef CONFIG_FB_MSM_MDP22
+                {
+                        unsigned long grp_id;
+
+                        ret = copy_from_user(&grp_id, argp, sizeof(grp_id));
+                        if (ret)
+                                return ret;
+
+                        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
+                        writel(grp_id, MDP_FULL_BYPASS_WORD43);
+                        mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF,
+                                      FALSE);
+                        break;
+                }
+#else
+                return -EFAULT;
+#endif
+        case MSMFB_SUSPEND_SW_REFRESHER:
+                if (!mfd->panel_power_on)
+                        return -EPERM;
+
+                mfd->sw_refreshing_enable = FALSE;
+                ret = msm_fb_stop_sw_refresher(mfd);
+                break;
+
+        case MSMFB_RESUME_SW_REFRESHER:
+                if (!mfd->panel_power_on)
+                        return -EPERM;
+
+                mfd->sw_refreshing_enable = TRUE;
+                ret = msm_fb_resume_sw_refresher(mfd);
+                break;
+
+        case MSMFB_CURSOR:
+                ret = copy_from_user(&cursor, argp, sizeof(cursor));
+                if (ret)
+                        return ret;
+
+                ret = msm_fb_cursor(info, &cursor);
+                break;
+
+        case MSMFB_SET_LUT:
+                ret = copy_from_user(&cmap, argp, sizeof(cmap));
+                if (ret)
+                        return ret;
+
+                mutex_lock(&msm_fb_ioctl_lut_sem);
+                ret = msm_fb_set_lut(&cmap, info);
+                mutex_unlock(&msm_fb_ioctl_lut_sem);
+                break;
+
+        case MSMFB_HISTOGRAM:
+                if (!mfd->do_histogram)
+                        return -ENODEV;
+
+                ret = copy_from_user(&hist, argp, sizeof(hist));
+                if (ret)
+                        return ret;
+
+                mutex_lock(&msm_fb_ioctl_hist_sem);
+                ret = mfd->do_histogram(info, &hist);
+                mutex_unlock(&msm_fb_ioctl_hist_sem);
+                break;
+
+        case MSMFB_GET_PAGE_PROTECTION:
+                fb_page_protection.page_protection
+                        = mfd->mdp_fb_page_protection;
+                ret = copy_to_user(argp, &fb_page_protection,
+                                sizeof(fb_page_protection));
+                if (ret)
+                                return ret;
+                break;
+
+        case MSMFB_SET_PAGE_PROTECTION:
+#ifdef CONFIG_ARCH_QSD8X50
+                ret = copy_from_user(&fb_page_protection, argp,
+                                sizeof(fb_page_protection));
+                if (ret)
+                                return ret;
+
+                /* Validate the proposed page protection settings. */
+                switch (fb_page_protection.page_protection)     {
+                case MDP_FB_PAGE_PROTECTION_NONCACHED:
+                case MDP_FB_PAGE_PROTECTION_WRITECOMBINE:
+                case MDP_FB_PAGE_PROTECTION_WRITETHROUGHCACHE:
+                /* Write-back cache (read allocate)  */
+                case MDP_FB_PAGE_PROTECTION_WRITEBACKCACHE:
+                /* Write-back cache (write allocate) */
+                case MDP_FB_PAGE_PROTECTION_WRITEBACKWACACHE:
+                        mfd->mdp_fb_page_protection =
+                                fb_page_protection.page_protection;
+                        break;
+                default:
+                        ret = -EINVAL;
+                        break;
+                }
+#else
+                /*
+                 * Don't allow caching until 7k DMA cache operations are
+                 * available.
+                 */
+                ret = -EINVAL;
+#endif
+                break;
+
+        default:
+                MSM_FB_INFO("MDP: unknown ioctl (cmd=%d) received!\n", cmd);
+                ret = -EINVAL;
+                break;
+        }
+
+        return ret;
+}
+
+static int msm_fb_register_driver(void)
+{
+        return platform_driver_register(&msm_fb_driver);
+}
+
+void msm_fb_add_device(struct platform_device *pdev)
+{
+        struct msm_fb_panel_data *pdata;
+        struct platform_device *this_dev = NULL;
+        struct fb_info *fbi;
+        struct msm_fb_data_type *mfd = NULL;
+        u32 type, id, fb_num;
+
+        if (!pdev)
+                return;
+        id = pdev->id;
+
+        pdata = pdev->dev.platform_data;
+        if (!pdata)
+                return;
+        type = pdata->panel_info.type;
+        fb_num = pdata->panel_info.fb_num;
+
+        if (fb_num <= 0)
+                return;
+
+        if (fbi_list_index >= MAX_FBI_LIST) {
+                printk(KERN_ERR "msm_fb: no more framebuffer info list!\n");
+                return;
+        }
+        /*
+         * alloc panel device data
+         */
+        this_dev = msm_fb_device_alloc(pdata, type, id);
+
+        if (!this_dev) {
+                printk(KERN_ERR
+                "%s: msm_fb_device_alloc failed!\n", __func__);
+                return;
+        }
+
+        /*
+         * alloc framebuffer info + par data
+         */
+        fbi = framebuffer_alloc(sizeof(struct msm_fb_data_type), NULL);
+        if (fbi == NULL) {
+                platform_device_put(this_dev);
+                printk(KERN_ERR "msm_fb: can't alloca framebuffer info data!\n");
+                return;
+        }
+
+        mfd = (struct msm_fb_data_type *)fbi->par;
+        mfd->key = MFD_KEY;
+        mfd->fbi = fbi;
+        mfd->panel.type = type;
+        mfd->panel.id = id;
+        mfd->fb_page = fb_num;
+        mfd->index = fbi_list_index;
+        mfd->mdp_fb_page_protection = MDP_FB_PAGE_PROTECTION_WRITECOMBINE;
+
+        /* link to the latest pdev */
+        mfd->pdev = this_dev;
+
+        mfd_list[mfd_list_index++] = mfd;
+        fbi_list[fbi_list_index++] = fbi;
+
+        /*
+         * set driver data
+         */
+        platform_set_drvdata(this_dev, mfd);
+
+        if (platform_device_add(this_dev)) {
+                printk(KERN_ERR "msm_fb: platform_device_add failed!\n");
+                platform_device_put(this_dev);
+                framebuffer_release(fbi);
+                fbi_list_index--;
+                return;
+        }
+}
+EXPORT_SYMBOL(msm_fb_add_device);
+
+int __init msm_fb_init(void)
+{
+        int rc = -ENODEV;
+
+        if (msm_fb_register_driver())
+                return rc;
+
+#ifdef MSM_FB_ENABLE_DBGFS
+        {
+                struct dentry *root;
+
+                if ((root = msm_fb_get_debugfs_root()) != NULL) {
+                        msm_fb_debugfs_file_create(root,
+                                                   "msm_fb_msg_printing_level",
+                                                   (u32 *) &msm_fb_msg_level);
+                        msm_fb_debugfs_file_create(root,
+                                                   "mddi_msg_printing_level",
+                                                   (u32 *) &mddi_msg_level);
+                        msm_fb_debugfs_file_create(root, "msm_fb_debug_enabled",
+                                                   (u32 *) &msm_fb_debug_enabled);
+                }
+        }
+#endif
+
+        return 0;
+}
+
+module_init(msm_fb_init);
diff --git a/drivers/staging/msm/msm_fb.h b/drivers/staging/msm/msm_fb.h
new file mode 100644
index 000000000000..f93913800475
--- /dev/null
+++ b/drivers/staging/msm/msm_fb.h
@@ -0,0 +1,174 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Code Aurora nor
+ *       the names of its contributors may be used to endorse or promote
+ *       products derived from this software without specific prior written
+ *       permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NON-INFRINGEMENT ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef MSM_FB_H
+#define MSM_FB_H
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include "linux/proc_fs.h"
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+#include <mach/board.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <mach/memory.h>
+#include <linux/semaphore.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+#include <linux/hrtimer.h>
+
+#include <linux/fb.h>
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
+
+#include "msm_fb_panel.h"
+#include "mdp.h"
+
+#define MSM_FB_DEFAULT_PAGE_SIZE 2
+#define MFD_KEY  0x11161126
+#define MSM_FB_MAX_DEV_LIST 32
+
+struct disp_info_type_suspend {
+        boolean op_enable;
+        boolean sw_refreshing_enable;
+        boolean panel_power_on;
+};
+
+struct msm_fb_data_type {
+        __u32 key;
+        __u32 index;
+        __u32 ref_cnt;
+        __u32 fb_page;
+
+        panel_id_type panel;
+        struct msm_panel_info panel_info;
+
+        DISP_TARGET dest;
+        struct fb_info *fbi;
+
+        boolean op_enable;
+        uint32 fb_imgType;
+        boolean sw_currently_refreshing;
+        boolean sw_refreshing_enable;
+        boolean hw_refresh;
+
+        MDPIBUF ibuf;
+        boolean ibuf_flushed;
+        struct timer_list refresh_timer;
+        struct completion refresher_comp;
+
+        boolean pan_waiting;
+        struct completion pan_comp;
+
+        /* vsync */
+        boolean use_mdp_vsync;
+        __u32 vsync_gpio;
+        __u32 total_lcd_lines;
+        __u32 total_porch_lines;
+        __u32 lcd_ref_usec_time;
+        __u32 refresh_timer_duration;
+
+        struct hrtimer dma_hrtimer;
+
+        boolean panel_power_on;
+        struct work_struct dma_update_worker;
+        struct semaphore sem;
+
+        struct timer_list vsync_resync_timer;
+        boolean vsync_handler_pending;
+        struct work_struct vsync_resync_worker;
+
+        ktime_t last_vsync_timetick;
+
+        __u32 *vsync_width_boundary;
+
+        unsigned int pmem_id;
+        struct disp_info_type_suspend suspend;
+
+        __u32 channel_irq;
+
+        struct mdp_dma_data *dma;
+        void (*dma_fnc) (struct msm_fb_data_type *mfd);
+        int (*cursor_update) (struct fb_info *info,
+                              struct fb_cursor *cursor);
+        int (*lut_update) (struct fb_info *info,
+                              struct fb_cmap *cmap);
+        int (*do_histogram) (struct fb_info *info,
+                              struct mdp_histogram *hist);
+        void *cursor_buf;
+        void *cursor_buf_phys;
+
+        void *cmd_port;
+        void *data_port;
+        void *data_port_phys;
+
+        __u32 bl_level;
+
+        struct platform_device *pdev;
+
+        __u32 var_xres;
+        __u32 var_yres;
+        __u32 var_pixclock;
+
+#ifdef MSM_FB_ENABLE_DBGFS
+        struct dentry *sub_dir;
+#endif
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+        struct early_suspend early_suspend;
+        struct early_suspend mddi_early_suspend;
+        struct early_suspend mddi_ext_early_suspend;
+#endif
+        u32 mdp_fb_page_protection;
+        int allow_set_offset;
+};
+
+struct dentry *msm_fb_get_debugfs_root(void);
+void msm_fb_debugfs_file_create(struct dentry *root, const char *name,
+                                u32 *var);
+void msm_fb_set_backlight(struct msm_fb_data_type *mfd, __u32 bkl_lvl,
+                                u32 save);
+
+void msm_fb_add_device(struct platform_device *pdev);
+
+int msm_fb_detect_client(const char *name);
+
+#ifdef CONFIG_FB_BACKLIGHT
+void msm_fb_config_backlight(struct msm_fb_data_type *mfd);
+#endif
+
+#endif /* MSM_FB_H */
diff --git a/drivers/staging/msm/msm_fb_bl.c b/drivers/staging/msm/msm_fb_bl.c
new file mode 100644
index 000000000000..033fc9486e01
--- /dev/null
+++ b/drivers/staging/msm/msm_fb_bl.c
@@ -0,0 +1,79 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/fb.h>
+#include <linux/string.h>
+#include <linux/version.h>
+#include <linux/backlight.h>
+
+#include "msm_fb.h"
+
+static int msm_fb_bl_get_brightness(struct backlight_device *pbd)
+{
+        return pbd->props.brightness;
+}
+
+static int msm_fb_bl_update_status(struct backlight_device *pbd)
+{
+        struct msm_fb_data_type *mfd = bl_get_data(pbd);
+        __u32 bl_lvl;
+
+        bl_lvl = pbd->props.brightness;
+        bl_lvl = mfd->fbi->bl_curve[bl_lvl];
+        msm_fb_set_backlight(mfd, bl_lvl, 1);
+        return 0;
+}
+
+static struct backlight_ops msm_fb_bl_ops = {
+        .get_brightness = msm_fb_bl_get_brightness,
+        .update_status = msm_fb_bl_update_status,
+};
+
+void msm_fb_config_backlight(struct msm_fb_data_type *mfd)
+{
+        struct msm_fb_panel_data *pdata;
+        struct backlight_device *pbd;
+        struct fb_info *fbi;
+        char name[16];
+
+        fbi = mfd->fbi;
+        pdata = (struct msm_fb_panel_data *)mfd->pdev->dev.platform_data;
+
+        if ((pdata) && (pdata->set_backlight)) {
+                snprintf(name, sizeof(name), "msmfb_bl%d", mfd->index);
+                pbd =
+                    backlight_device_register(name, fbi->dev, mfd,
+                                              &msm_fb_bl_ops);
+                if (!IS_ERR(pbd)) {
+                        fbi->bl_dev = pbd;
+                        fb_bl_default_curve(fbi,
+                                            0,
+                                            mfd->panel_info.bl_min,
+                                            mfd->panel_info.bl_max);
+                        pbd->props.max_brightness = FB_BACKLIGHT_LEVELS - 1;
+                        pbd->props.brightness = FB_BACKLIGHT_LEVELS - 1;
+                        backlight_update_status(pbd);
+                } else {
+                        fbi->bl_dev = NULL;
+                        printk(KERN_ERR "msm_fb: backlight_device_register failed!\n");
+                }
+        }
+}
diff --git a/drivers/staging/msm/msm_fb_def.h b/drivers/staging/msm/msm_fb_def.h
new file mode 100644
index 000000000000..6de440937422
--- /dev/null
+++ b/drivers/staging/msm/msm_fb_def.h
@@ -0,0 +1,201 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Code Aurora nor
+ *       the names of its contributors may be used to endorse or promote
+ *       products derived from this software without specific prior written
+ *       permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NON-INFRINGEMENT ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef MSM_FB_DEF_H
+#define MSM_FB_DEF_H
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/fb.h>
+#include "msm_mdp.h"
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/uaccess.h>
+#include <linux/workqueue.h>
+#include <linux/string.h>
+#include <linux/version.h>
+#include <linux/proc_fs.h>
+#include <linux/vmalloc.h>
+#include <linux/debugfs.h>
+#include <linux/console.h>
+
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include "linux/proc_fs.h"
+#include <mach/hardware.h>
+#include <linux/io.h>
+#include <linux/fb.h>
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/platform_device.h>
+
+typedef s64 int64;
+typedef s32 int32;
+typedef s16 int16;
+typedef s8 int8;
+
+typedef u64 uint64;
+typedef u32 uint32;
+typedef u16 uint16;
+typedef u8 uint8;
+
+typedef s32 int4;
+typedef s16 int2;
+typedef s8 int1;
+
+typedef u32 uint4;
+typedef u16 uint2;
+typedef u8 uint1;
+
+typedef u32 dword;
+typedef u16 word;
+typedef u8 byte;
+
+typedef unsigned int boolean;
+
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+#define MSM_FB_ENABLE_DBGFS
+#define FEATURE_MDDI
+
+#define outp32(addr, val) writel(val, addr)
+#define outp16(addr, val) writew(val, addr)
+#define outp8(addr, val) writeb(val, addr)
+#define outp(addr, val) outp32(addr, val)
+
+#ifndef MAX
+#define  MAX( x, y ) (((x) > (y)) ? (x) : (y))
+#endif
+
+#ifndef MIN
+#define  MIN( x, y ) (((x) < (y)) ? (x) : (y))
+#endif
+
+/*--------------------------------------------------------------------------*/
+
+#define inp32(addr) readl(addr)
+#define inp16(addr) readw(addr)
+#define inp8(addr) readb(addr)
+#define inp(addr) inp32(addr)
+
+#define inpw(port)             readw(port)
+#define outpw(port, val)       writew(val, port)
+#define inpdw(port)            readl(port)
+#define outpdw(port, val)      writel(val, port)
+
+
+#define clk_busy_wait(x) msleep_interruptible((x)/1000)
+
+#define memory_barrier()
+
+#define assert(expr) \
+        if(!(expr)) { \
+                printk(KERN_ERR "msm_fb: assertion failed! %s,%s,%s,line=%d\n",\
+                        #expr, __FILE__, __func__, __LINE__); \
+        }
+
+#define ASSERT(x)   assert(x)
+
+#define DISP_EBI2_LOCAL_DEFINE
+#ifdef DISP_EBI2_LOCAL_DEFINE
+#define LCD_PRIM_BASE_PHYS 0x98000000
+#define LCD_SECD_BASE_PHYS 0x9c000000
+#define EBI2_PRIM_LCD_RS_PIN 0x20000
+#define EBI2_SECD_LCD_RS_PIN 0x20000
+
+#define EBI2_PRIM_LCD_CLR 0xC0
+#define EBI2_PRIM_LCD_SEL 0x40
+
+#define EBI2_SECD_LCD_CLR 0x300
+#define EBI2_SECD_LCD_SEL 0x100
+#endif
+
+extern u32 msm_fb_msg_level;
+
+/*
+ * Message printing priorities:
+ * LEVEL 0 KERN_EMERG (highest priority)
+ * LEVEL 1 KERN_ALERT
+ * LEVEL 2 KERN_CRIT
+ * LEVEL 3 KERN_ERR
+ * LEVEL 4 KERN_WARNING
+ * LEVEL 5 KERN_NOTICE
+ * LEVEL 6 KERN_INFO
+ * LEVEL 7 KERN_DEBUG (Lowest priority)
+ */
+#define MSM_FB_EMERG(msg, ...)    \
+        if (msm_fb_msg_level > 0)  \
+                printk(KERN_EMERG msg, ## __VA_ARGS__);
+#define MSM_FB_ALERT(msg, ...)    \
+        if (msm_fb_msg_level > 1)  \
+                printk(KERN_ALERT msg, ## __VA_ARGS__);
+#define MSM_FB_CRIT(msg, ...)    \
+        if (msm_fb_msg_level > 2)  \
+                printk(KERN_CRIT msg, ## __VA_ARGS__);
+#define MSM_FB_ERR(msg, ...)    \
+        if (msm_fb_msg_level > 3)  \
+                printk(KERN_ERR msg, ## __VA_ARGS__);
+#define MSM_FB_WARNING(msg, ...)    \
+        if (msm_fb_msg_level > 4)  \
+                printk(KERN_WARNING msg, ## __VA_ARGS__);
+#define MSM_FB_NOTICE(msg, ...)    \
+        if (msm_fb_msg_level > 5)  \
+                printk(KERN_NOTICE msg, ## __VA_ARGS__);
+#define MSM_FB_INFO(msg, ...)    \
+        if (msm_fb_msg_level > 6)  \
+                printk(KERN_INFO msg, ## __VA_ARGS__);
+#define MSM_FB_DEBUG(msg, ...)    \
+        if (msm_fb_msg_level > 7)  \
+                printk(KERN_DEBUG msg, ## __VA_ARGS__);
+
+#ifdef MSM_FB_C
+unsigned char *msm_mdp_base;
+unsigned char *msm_pmdh_base;
+unsigned char *msm_emdh_base;
+#else
+extern unsigned char *msm_mdp_base;
+extern unsigned char *msm_pmdh_base;
+extern unsigned char *msm_emdh_base;
+#endif
+
+#endif /* MSM_FB_DEF_H */
diff --git a/drivers/staging/msm/msm_fb_panel.c b/drivers/staging/msm/msm_fb_panel.c
new file mode 100644
index 000000000000..b17a239a1bc7
--- /dev/null
+++ b/drivers/staging/msm/msm_fb_panel.c
@@ -0,0 +1,136 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/uaccess.h>
+#include <linux/workqueue.h>
+#include <linux/string.h>
+#include <linux/version.h>
+#include <linux/proc_fs.h>
+#include <linux/vmalloc.h>
+#include <linux/debugfs.h>
+
+#include "msm_fb_panel.h"
+
+int panel_next_on(struct platform_device *pdev)
+{
+        int ret = 0;
+        struct msm_fb_panel_data *pdata;
+        struct msm_fb_panel_data *next_pdata;
+        struct platform_device *next_pdev;
+
+        pdata = (struct msm_fb_panel_data *)pdev->dev.platform_data;
+
+        if (pdata) {
+                next_pdev = pdata->next;
+                if (next_pdev) {
+                        next_pdata =
+                            (struct msm_fb_panel_data *)next_pdev->dev.
+                            platform_data;
+                        if ((next_pdata) && (next_pdata->on))
+                                ret = next_pdata->on(next_pdev);
+                }
+        }
+
+        return ret;
+}
+
+int panel_next_off(struct platform_device *pdev)
+{
+        int ret = 0;
+        struct msm_fb_panel_data *pdata;
+        struct msm_fb_panel_data *next_pdata;
+        struct platform_device *next_pdev;
+
+        pdata = (struct msm_fb_panel_data *)pdev->dev.platform_data;
+
+        if (pdata) {
+                next_pdev = pdata->next;
+                if (next_pdev) {
+                        next_pdata =
+                            (struct msm_fb_panel_data *)next_pdev->dev.
+                            platform_data;
+                        if ((next_pdata) && (next_pdata->on))
+                                ret = next_pdata->off(next_pdev);
+                }
+        }
+
+        return ret;
+}
+
+struct platform_device *msm_fb_device_alloc(struct msm_fb_panel_data *pdata,
+                                                u32 type, u32 id)
+{
+        struct platform_device *this_dev = NULL;
+        char dev_name[16];
+
+        switch (type) {
+        case EBI2_PANEL:
+                snprintf(dev_name, sizeof(dev_name), "ebi2_lcd");
+                break;
+
+        case MDDI_PANEL:
+                snprintf(dev_name, sizeof(dev_name), "mddi");
+                break;
+
+        case EXT_MDDI_PANEL:
+                snprintf(dev_name, sizeof(dev_name), "mddi_ext");
+                break;
+
+        case TV_PANEL:
+                snprintf(dev_name, sizeof(dev_name), "tvenc");
+                break;
+
+        case HDMI_PANEL:
+        case LCDC_PANEL:
+                snprintf(dev_name, sizeof(dev_name), "lcdc");
+                break;
+
+        default:
+                return NULL;
+        }
+
+        if (pdata != NULL)
+                pdata->next = NULL;
+        else
+                return NULL;
+
+        this_dev =
+            platform_device_alloc(dev_name, ((u32) type << 16) | (u32) id);
+
+        if (this_dev) {
+                if (platform_device_add_data
+                    (this_dev, pdata, sizeof(struct msm_fb_panel_data))) {
+                        printk
+                            ("msm_fb_device_alloc: platform_device_add_data failed!\n");
+                        platform_device_put(this_dev);
+                        return NULL;
+                }
+        }
+
+        return this_dev;
+}
diff --git a/drivers/staging/msm/msm_fb_panel.h b/drivers/staging/msm/msm_fb_panel.h
new file mode 100644
index 000000000000..ab458310c3a2
--- /dev/null
+++ b/drivers/staging/msm/msm_fb_panel.h
@@ -0,0 +1,145 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Code Aurora nor
+ *       the names of its contributors may be used to endorse or promote
+ *       products derived from this software without specific prior written
+ *       permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NON-INFRINGEMENT ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef MSM_FB_PANEL_H
+#define MSM_FB_PANEL_H
+
+#include "msm_fb_def.h"
+
+struct msm_fb_data_type;
+
+typedef void (*msm_fb_vsync_handler_type) (void *arg);
+
+/* panel id type */
+typedef struct panel_id_s {
+        uint16 id;
+        uint16 type;
+} panel_id_type;
+
+/* panel type list */
+#define NO_PANEL       0xffff   /* No Panel */
+#define MDDI_PANEL     1        /* MDDI */
+#define EBI2_PANEL     2        /* EBI2 */
+#define LCDC_PANEL     3        /* internal LCDC type */
+#define EXT_MDDI_PANEL 4        /* Ext.MDDI */
+#define TV_PANEL       5        /* TV */
+#define HDMI_PANEL     6        /* HDMI TV */
+
+/* panel class */
+typedef enum {
+        DISPLAY_LCD = 0,        /* lcd = ebi2/mddi */
+        DISPLAY_LCDC,           /* lcdc */
+        DISPLAY_TV,             /* TV Out */
+        DISPLAY_EXT_MDDI,       /* External MDDI */
+} DISP_TARGET;
+
+/* panel device locaiton */
+typedef enum {
+        DISPLAY_1 = 0,          /* attached as first device */
+        DISPLAY_2,              /* attached on second device */
+        MAX_PHYS_TARGET_NUM,
+} DISP_TARGET_PHYS;
+
+/* panel info type */
+struct lcd_panel_info {
+        __u32 vsync_enable;
+        __u32 refx100;
+        __u32 v_back_porch;
+        __u32 v_front_porch;
+        __u32 v_pulse_width;
+        __u32 hw_vsync_mode;
+        __u32 vsync_notifier_period;
+};
+
+struct lcdc_panel_info {
+        __u32 h_back_porch;
+        __u32 h_front_porch;
+        __u32 h_pulse_width;
+        __u32 v_back_porch;
+        __u32 v_front_porch;
+        __u32 v_pulse_width;
+        __u32 border_clr;
+        __u32 underflow_clr;
+        __u32 hsync_skew;
+};
+
+struct mddi_panel_info {
+        __u32 vdopkt;
+};
+
+struct msm_panel_info {
+        __u32 xres;
+        __u32 yres;
+        __u32 bpp;
+        __u32 type;
+        __u32 wait_cycle;
+        DISP_TARGET_PHYS pdest;
+        __u32 bl_max;
+        __u32 bl_min;
+        __u32 fb_num;
+        __u32 clk_rate;
+        __u32 clk_min;
+        __u32 clk_max;
+        __u32 frame_count;
+
+        union {
+                struct mddi_panel_info mddi;
+        };
+
+        union {
+                struct lcd_panel_info lcd;
+                struct lcdc_panel_info lcdc;
+        };
+};
+
+struct msm_fb_panel_data {
+        struct msm_panel_info panel_info;
+        void (*set_rect) (int x, int y, int xres, int yres);
+        void (*set_vsync_notifier) (msm_fb_vsync_handler_type, void *arg);
+        void (*set_backlight) (struct msm_fb_data_type *);
+
+        /* function entry chain */
+        int (*on) (struct platform_device *pdev);
+        int (*off) (struct platform_device *pdev);
+        struct platform_device *next;
+};
+
+/*===========================================================================
+  FUNCTIONS PROTOTYPES
+============================================================================*/
+struct platform_device *msm_fb_device_alloc(struct msm_fb_panel_data *pdata,
+                                                u32 type, u32 id);
+int panel_next_on(struct platform_device *pdev);
+int panel_next_off(struct platform_device *pdev);
+
+int lcdc_device_register(struct msm_panel_info *pinfo);
+
+int mddi_toshiba_device_register(struct msm_panel_info *pinfo,
+                                        u32 channel, u32 panel);
+
+#endif /* MSM_FB_PANEL_H */
diff --git a/drivers/staging/msm/msm_mdp.h b/drivers/staging/msm/msm_mdp.h
new file mode 100644
index 000000000000..2d5323f5b62d
--- /dev/null
+++ b/drivers/staging/msm/msm_mdp.h
@@ -0,0 +1,245 @@
+/* include/linux/msm_mdp.h
+ *
+ * Copyright (C) 2007 Google Incorporated
+ * Copyright (c) 2009-2010, Code Aurora Forum. All rights reserved.
+ *
+ * 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 _MSM_MDP_H_
+#define _MSM_MDP_H_
+
+#include <linux/types.h>
+#include <linux/fb.h>
+
+#define MSMFB_IOCTL_MAGIC 'm'
+#define MSMFB_GRP_DISP          _IOW(MSMFB_IOCTL_MAGIC, 1, unsigned int)
+#define MSMFB_BLIT              _IOW(MSMFB_IOCTL_MAGIC, 2, unsigned int)
+#define MSMFB_SUSPEND_SW_REFRESHER _IOW(MSMFB_IOCTL_MAGIC, 128, unsigned int)
+#define MSMFB_RESUME_SW_REFRESHER _IOW(MSMFB_IOCTL_MAGIC, 129, unsigned int)
+#define MSMFB_CURSOR _IOW(MSMFB_IOCTL_MAGIC, 130, struct fb_cursor)
+#define MSMFB_SET_LUT _IOW(MSMFB_IOCTL_MAGIC, 131, struct fb_cmap)
+#define MSMFB_HISTOGRAM _IOWR(MSMFB_IOCTL_MAGIC, 132, struct mdp_histogram)
+/* new ioctls's for set/get ccs matrix */
+#define MSMFB_GET_CCS_MATRIX  _IOWR(MSMFB_IOCTL_MAGIC, 133, struct mdp_ccs)
+#define MSMFB_SET_CCS_MATRIX  _IOW(MSMFB_IOCTL_MAGIC, 134, struct mdp_ccs)
+#define MSMFB_OVERLAY_SET       _IOWR(MSMFB_IOCTL_MAGIC, 135, \
+                                                struct mdp_overlay)
+#define MSMFB_OVERLAY_UNSET     _IOW(MSMFB_IOCTL_MAGIC, 136, unsigned int)
+#define MSMFB_OVERLAY_PLAY      _IOW(MSMFB_IOCTL_MAGIC, 137, \
+                                                struct msmfb_overlay_data)
+#define MSMFB_GET_PAGE_PROTECTION _IOR(MSMFB_IOCTL_MAGIC, 138, \
+                                        struct mdp_page_protection)
+#define MSMFB_SET_PAGE_PROTECTION _IOW(MSMFB_IOCTL_MAGIC, 139, \
+                                        struct mdp_page_protection)
+#define MSMFB_OVERLAY_GET      _IOR(MSMFB_IOCTL_MAGIC, 140, \
+                                                struct mdp_overlay)
+
+/* new ioctls for async MDP ops */
+#define MSMFB_ASYNC_BLIT _IOW(MSMFB_IOCTL_MAGIC, 141, unsigned int)
+#define MSMFB_BLIT_FLUSH _IOR(MSMFB_IOCTL_MAGIC, 142, unsigned int)
+
+#define MDP_IMGTYPE2_START 0x10000
+
+enum {
+        MDP_RGB_565,      /* RGB 565 planer */
+        MDP_XRGB_8888,    /* RGB 888 padded */
+        MDP_Y_CBCR_H2V2,  /* Y and CbCr, pseudo planer w/ Cb is in MSB */
+        MDP_ARGB_8888,    /* ARGB 888 */
+        MDP_RGB_888,      /* RGB 888 planer */
+        MDP_Y_CRCB_H2V2,  /* Y and CrCb, pseudo planer w/ Cr is in MSB */
+        MDP_YCRYCB_H2V1,  /* YCrYCb interleave */
+        MDP_Y_CRCB_H2V1,  /* Y and CrCb, pseduo planer w/ Cr is in MSB */
+        MDP_Y_CBCR_H2V1,   /* Y and CrCb, pseduo planer w/ Cr is in MSB */
+        MDP_RGBA_8888,    /* ARGB 888 */
+        MDP_BGRA_8888,    /* ABGR 888 */
+        MDP_Y_CRCB_H2V2_TILE,  /* Y and CrCb, pseudo planer tile */
+        MDP_Y_CBCR_H2V2_TILE,  /* Y and CbCr, pseudo planer tile */
+        MDP_IMGTYPE_LIMIT,
+        MDP_BGR_565 = MDP_IMGTYPE2_START,      /* BGR 565 planer */
+        MDP_FB_FORMAT,    /* framebuffer format */
+        MDP_IMGTYPE_LIMIT2 /* Non valid image type after this enum */
+};
+
+enum {
+        PMEM_IMG,
+        FB_IMG,
+};
+
+/* mdp_blit_req flag values */
+#define MDP_ROT_NOP 0
+#define MDP_FLIP_LR 0x1
+#define MDP_FLIP_UD 0x2
+#define MDP_ROT_90 0x4
+#define MDP_ROT_180 (MDP_FLIP_UD|MDP_FLIP_LR)
+#define MDP_ROT_270 (MDP_ROT_90|MDP_FLIP_UD|MDP_FLIP_LR)
+#define MDP_DITHER 0x8
+#define MDP_BLUR 0x10
+#define MDP_BLEND_FG_PREMULT 0x20000
+
+#define MDP_DEINTERLACE         0x80000000
+#define MDP_SHARPENING          0x40000000
+
+#define MDP_NO_DMA_BARRIER_START        0x20000000
+#define MDP_NO_DMA_BARRIER_END          0x10000000
+#define MDP_NO_BLIT                     0x08000000
+#define MDP_BLIT_WITH_DMA_BARRIERS      0x000
+#define MDP_BLIT_WITH_NO_DMA_BARRIERS    \
+        (MDP_NO_DMA_BARRIER_START | MDP_NO_DMA_BARRIER_END)
+#define MDP_TRANSP_NOP 0xffffffff
+#define MDP_ALPHA_NOP 0xff
+
+#define MDP_BLIT_SRC_GEM        0x02000000 /* set for GEM, clear for PMEM */
+#define MDP_BLIT_DST_GEM        0x01000000 /* set for GEM, clear for PMEM */
+
+#define MDP_FB_PAGE_PROTECTION_NONCACHED         (0)
+#define MDP_FB_PAGE_PROTECTION_WRITECOMBINE      (1)
+#define MDP_FB_PAGE_PROTECTION_WRITETHROUGHCACHE (2)
+#define MDP_FB_PAGE_PROTECTION_WRITEBACKCACHE    (3)
+#define MDP_FB_PAGE_PROTECTION_WRITEBACKWACACHE  (4)
+/* Sentinel: Don't use! */
+#define MDP_FB_PAGE_PROTECTION_INVALID           (5)
+/* Count of the number of MDP_FB_PAGE_PROTECTION_... values. */
+#define MDP_NUM_FB_PAGE_PROTECTION_VALUES        (5)
+
+struct mdp_rect {
+        uint32_t x;
+        uint32_t y;
+        uint32_t w;
+        uint32_t h;
+};
+
+struct mdp_img {
+        uint32_t width;
+        uint32_t height;
+        uint32_t format;
+        uint32_t offset;
+        int memory_id;          /* the file descriptor */
+        uint32_t priv;
+};
+
+/*
+ * {3x3} + {3} ccs matrix
+ */
+
+#define MDP_CCS_RGB2YUV         0
+#define MDP_CCS_YUV2RGB         1
+
+#define MDP_CCS_SIZE    9
+#define MDP_BV_SIZE     3
+
+struct mdp_ccs {
+        int direction;                  /* MDP_CCS_RGB2YUV or YUV2RGB */
+        uint16_t ccs[MDP_CCS_SIZE];     /* 3x3 color coefficients */
+        uint16_t bv[MDP_BV_SIZE];       /* 1x3 bias vector */
+};
+
+/* The version of the mdp_blit_req structure so that
+ * user applications can selectively decide which functionality
+ * to include
+ */
+
+#define MDP_BLIT_REQ_VERSION 2
+
+struct mdp_blit_req {
+        struct mdp_img src;
+        struct mdp_img dst;
+        struct mdp_rect src_rect;
+        struct mdp_rect dst_rect;
+        uint32_t alpha;
+        uint32_t transp_mask;
+        uint32_t flags;
+        int sharpening_strength;  /* -127 <--> 127, default 64 */
+};
+
+struct mdp_blit_req_list {
+        uint32_t count;
+        struct mdp_blit_req req[];
+};
+
+struct msmfb_data {
+        uint32_t offset;
+        int memory_id;
+        int id;
+};
+
+#define MSMFB_NEW_REQUEST -1
+
+struct msmfb_overlay_data {
+        uint32_t id;
+        struct msmfb_data data;
+};
+
+struct msmfb_img {
+        uint32_t width;
+        uint32_t height;
+        uint32_t format;
+};
+
+struct mdp_overlay {
+        struct msmfb_img src;
+        struct mdp_rect src_rect;
+        struct mdp_rect dst_rect;
+        uint32_t z_order;       /* stage number */
+        uint32_t is_fg;         /* control alpha & transp */
+        uint32_t alpha;
+        uint32_t transp_mask;
+        uint32_t flags;
+        uint32_t id;
+        uint32_t user_data[8];
+};
+
+struct mdp_histogram {
+        uint32_t frame_cnt;
+        uint32_t bin_cnt;
+        uint32_t *r;
+        uint32_t *g;
+        uint32_t *b;
+};
+
+struct mdp_page_protection {
+        uint32_t page_protection;
+};
+
+
+struct msm_panel_common_pdata {
+        int gpio;
+        int (*backlight_level)(int level, int max, int min);
+        int (*pmic_backlight)(int level);
+        int (*panel_num)(void);
+        void (*panel_config_gpio)(int);
+        int *gpio_num;
+};
+
+struct lcdc_platform_data {
+        int (*lcdc_gpio_config)(int on);
+        void (*lcdc_power_save)(int);
+};
+
+struct tvenc_platform_data {
+        int (*pm_vid_en)(int on);
+};
+
+struct mddi_platform_data {
+        void (*mddi_power_save)(int on);
+        int (*mddi_sel_clk)(u32 *clk_rate);
+};
+
+struct msm_fb_platform_data {
+        int (*detect_client)(const char *name);
+        int mddi_prescan;
+        int (*allow_set_offset)(void);
+};
+
+struct msm_hdmi_platform_data {
+        int irq;
+        int (*cable_detect)(int insert);
+};
+
+#endif /*_MSM_MDP_H_*/
diff --git a/drivers/staging/msm/staging-devices.c b/drivers/staging/msm/staging-devices.c
new file mode 100644
index 000000000000..0f8ec3e26013
--- /dev/null
+++ b/drivers/staging/msm/staging-devices.c
@@ -0,0 +1,323 @@
+#include <linux/kernel.h>
+#include <linux/irq.h>
+#include <linux/gpio.h>
+#include <linux/platform_device.h>
+#include <linux/bootmem.h>
+#include <linux/delay.h>
+
+#include <asm/mach-types.h>
+#include <asm/mach/arch.h>
+#include <asm/io.h>
+#include <asm/setup.h>
+
+#include <mach/board.h>
+#include <mach/irqs.h>
+#include <mach/sirc.h>
+#include <mach/gpio.h>
+
+#include "msm_mdp.h"
+#include "memory_ll.h"
+//#include "android_pmem.h"
+#include <mach/board.h>
+
+#ifdef CONFIG_MSM_SOC_REV_A
+#define MSM_SMI_BASE 0xE0000000
+#else
+#define MSM_SMI_BASE 0x00000000
+#endif
+
+
+#define TOUCHPAD_SUSPEND        34
+#define TOUCHPAD_IRQ            38
+
+#define MSM_PMEM_MDP_SIZE       0x1591000
+
+#ifdef CONFIG_MSM_SOC_REV_A
+#define SMEM_SPINLOCK_I2C       "D:I2C02000021"
+#else
+#define SMEM_SPINLOCK_I2C       "S:6"
+#endif
+
+#define MSM_PMEM_ADSP_SIZE      0x1C00000
+
+#define MSM_FB_SIZE             0x500000
+#define MSM_FB_SIZE_ST15        0x800000
+#define MSM_AUDIO_SIZE          0x80000
+#define MSM_GPU_PHYS_SIZE       SZ_2M
+
+#ifdef CONFIG_MSM_SOC_REV_A
+#define MSM_SMI_BASE            0xE0000000
+#else
+#define MSM_SMI_BASE            0x00000000
+#endif
+
+#define MSM_SHARED_RAM_PHYS     (MSM_SMI_BASE + 0x00100000)
+
+#define MSM_PMEM_SMI_BASE       (MSM_SMI_BASE + 0x02B00000)
+#define MSM_PMEM_SMI_SIZE       0x01500000
+
+#define MSM_FB_BASE             MSM_PMEM_SMI_BASE
+#define MSM_GPU_PHYS_BASE       (MSM_FB_BASE + MSM_FB_SIZE)
+#define MSM_PMEM_SMIPOOL_BASE   (MSM_GPU_PHYS_BASE + MSM_GPU_PHYS_SIZE)
+#define MSM_PMEM_SMIPOOL_SIZE   (MSM_PMEM_SMI_SIZE - MSM_FB_SIZE \
+                                        - MSM_GPU_PHYS_SIZE)
+
+#if defined(CONFIG_FB_MSM_MDP40)
+#define MDP_BASE          0xA3F00000
+#define PMDH_BASE         0xAD600000
+#define EMDH_BASE         0xAD700000
+#define TVENC_BASE        0xAD400000
+#else
+#define MDP_BASE          0xAA200000
+#define PMDH_BASE         0xAA600000
+#define EMDH_BASE         0xAA700000
+#define TVENC_BASE        0xAA400000
+#endif
+
+#define PMEM_KERNEL_EBI1_SIZE   (CONFIG_PMEM_KERNEL_SIZE * 1024 * 1024)
+
+static struct resource msm_fb_resources[] = {
+        {
+                .flags  = IORESOURCE_DMA,
+        }
+};
+
+static struct resource msm_mdp_resources[] = {
+        {
+                .name   = "mdp",
+                .start  = MDP_BASE,
+                .end    = MDP_BASE + 0x000F0000 - 1,
+                .flags  = IORESOURCE_MEM,
+        }
+};
+
+static struct platform_device msm_mdp_device = {
+        .name   = "mdp",
+        .id     = 0,
+        .num_resources  = ARRAY_SIZE(msm_mdp_resources),
+        .resource       = msm_mdp_resources,
+};
+
+static struct platform_device msm_lcdc_device = {
+        .name   = "lcdc",
+        .id     = 0,
+};
+
+static int msm_fb_detect_panel(const char *name)
+{
+        int ret = -EPERM;
+
+        if (machine_is_qsd8x50_ffa() || machine_is_qsd8x50a_ffa()) {
+                if (!strncmp(name, "mddi_toshiba_wvga_pt", 20))
+                        ret = 0;
+                else
+                        ret = -ENODEV;
+        } else if ((machine_is_qsd8x50_surf() || machine_is_qsd8x50a_surf())
+                        && !strcmp(name, "lcdc_external"))
+                ret = 0;
+        else if (0 /*machine_is_qsd8x50_grapefruit() */) {
+                if (!strcmp(name, "lcdc_grapefruit_vga"))
+                        ret = 0;
+                else
+                        ret = -ENODEV;
+        } else if (machine_is_qsd8x50_st1()) {
+                if (!strcmp(name, "lcdc_st1_wxga"))
+                        ret = 0;
+                else
+                        ret = -ENODEV;
+        } else if (machine_is_qsd8x50a_st1_5()) {
+                if (!strcmp(name, "lcdc_st15") ||
+                    !strcmp(name, "hdmi_sii9022"))
+                        ret = 0;
+                else
+                        ret = -ENODEV;
+        }
+
+        return ret;
+}
+
+/* Only allow a small subset of machines to set the offset via
+   FB PAN_DISPLAY */
+
+static int msm_fb_allow_set_offset(void)
+{
+        return (machine_is_qsd8x50_st1() ||
+                machine_is_qsd8x50a_st1_5()) ? 1 : 0;
+}
+
+
+static struct msm_fb_platform_data msm_fb_pdata = {
+        .detect_client = msm_fb_detect_panel,
+        .allow_set_offset = msm_fb_allow_set_offset,
+};
+
+static struct platform_device msm_fb_device = {
+        .name   = "msm_fb",
+        .id     = 0,
+        .num_resources  = ARRAY_SIZE(msm_fb_resources),
+        .resource       = msm_fb_resources,
+        .dev    = {
+                .platform_data = &msm_fb_pdata,
+        }
+};
+
+static void __init qsd8x50_allocate_memory_regions(void)
+{
+        void *addr;
+        unsigned long size;
+        if (machine_is_qsd8x50a_st1_5())
+                size = MSM_FB_SIZE_ST15;
+        else
+                size = MSM_FB_SIZE;
+
+        addr = alloc_bootmem(size); // (void *)MSM_FB_BASE;
+        if (!addr)
+                printk("Failed to allocate bootmem for framebuffer\n");
+
+
+        msm_fb_resources[0].start = __pa(addr);
+        msm_fb_resources[0].end = msm_fb_resources[0].start + size - 1;
+        pr_info(KERN_ERR "using %lu bytes of SMI at %lx physical for fb\n",
+                size, (unsigned long)addr);
+}
+
+static int msm_fb_lcdc_gpio_config(int on)
+{
+//      return 0;
+        if (machine_is_qsd8x50_st1()) {
+                if (on) {
+                        gpio_set_value(32, 1);
+                        mdelay(100);
+                        gpio_set_value(20, 1);
+                        gpio_set_value(17, 1);
+                        gpio_set_value(19, 1);
+                } else {
+                        gpio_set_value(17, 0);
+                        gpio_set_value(19, 0);
+                        gpio_set_value(20, 0);
+                        mdelay(100);
+                        gpio_set_value(32, 0);
+                }
+        } else if (machine_is_qsd8x50a_st1_5()) {
+                if (on) {
+                        gpio_set_value(17, 1);
+                        gpio_set_value(19, 1);
+                        gpio_set_value(20, 1);
+                        gpio_set_value(22, 0);
+                        gpio_set_value(32, 1);
+                        gpio_set_value(155, 1);
+                        //st15_hdmi_power(1);
+                        gpio_set_value(22, 1);
+
+                } else {
+                        gpio_set_value(17, 0);
+                        gpio_set_value(19, 0);
+                        gpio_set_value(22, 0);
+                        gpio_set_value(32, 0);
+                        gpio_set_value(155, 0);
+                //      st15_hdmi_power(0);
+                }
+        }
+        return 0;
+}
+
+
+static struct lcdc_platform_data lcdc_pdata = {
+        .lcdc_gpio_config = msm_fb_lcdc_gpio_config,
+};
+
+static struct msm_gpio msm_fb_st15_gpio_config_data[] = {
+        { GPIO_CFG(17, 0, GPIO_OUTPUT, GPIO_NO_PULL, GPIO_2MA), "lcdc_en0" },
+        { GPIO_CFG(19, 0, GPIO_OUTPUT, GPIO_NO_PULL, GPIO_2MA), "dat_pwr_sv" },
+        { GPIO_CFG(20, 0, GPIO_OUTPUT, GPIO_NO_PULL, GPIO_2MA), "lvds_pwr_dn" },
+        { GPIO_CFG(22, 0, GPIO_OUTPUT, GPIO_NO_PULL, GPIO_2MA), "lcdc_en1" },
+        { GPIO_CFG(32, 0, GPIO_OUTPUT, GPIO_NO_PULL, GPIO_2MA), "lcdc_en2" },
+        { GPIO_CFG(103, 0, GPIO_INPUT, GPIO_NO_PULL, GPIO_2MA), "hdmi_irq" },
+        { GPIO_CFG(155, 0, GPIO_OUTPUT, GPIO_NO_PULL, GPIO_2MA), "hdmi_3v3" },
+};
+
+static struct msm_panel_common_pdata mdp_pdata = {
+        .gpio = 98,
+};
+
+static struct platform_device *devices[] __initdata = {
+        &msm_fb_device,
+};
+
+
+static void __init msm_register_device(struct platform_device *pdev, void *data)
+{
+        int ret;
+
+        pdev->dev.platform_data = data;
+
+        ret = platform_device_register(pdev);
+        if (ret)
+                dev_err(&pdev->dev,
+                          "%s: platform_device_register() failed = %d\n",
+                          __func__, ret);
+}
+
+void __init msm_fb_register_device(char *name, void *data)
+{
+        if (!strncmp(name, "mdp", 3))
+                msm_register_device(&msm_mdp_device, data);
+/*
+        else if (!strncmp(name, "pmdh", 4))
+                msm_register_device(&msm_mddi_device, data);
+        else if (!strncmp(name, "emdh", 4))
+                msm_register_device(&msm_mddi_ext_device, data);
+        else if (!strncmp(name, "ebi2", 4))
+                msm_register_device(&msm_ebi2_lcd_device, data);
+        else if (!strncmp(name, "tvenc", 5))
+                msm_register_device(&msm_tvenc_device, data);
+        else */
+
+        if (!strncmp(name, "lcdc", 4))
+                msm_register_device(&msm_lcdc_device, data);
+        /*else
+                printk(KERN_ERR "%s: unknown device! %s\n", __func__, name);
+*/
+}
+
+static void __init msm_fb_add_devices(void)
+{
+        int rc;
+        msm_fb_register_device("mdp", &mdp_pdata);
+//      msm_fb_register_device("pmdh", &mddi_pdata);
+//      msm_fb_register_device("emdh", &mddi_pdata);
+//      msm_fb_register_device("tvenc", 0);
+
+        if (machine_is_qsd8x50a_st1_5()) {
+/*              rc = st15_hdmi_vreg_init();
+                if (rc)
+                        return;
+*/
+                rc = msm_gpios_request_enable(
+                        msm_fb_st15_gpio_config_data,
+                        ARRAY_SIZE(msm_fb_st15_gpio_config_data));
+                if (rc) {
+                        printk(KERN_ERR "%s: unable to init lcdc gpios\n",
+                               __func__);
+                        return;
+                }
+                msm_fb_register_device("lcdc", &lcdc_pdata);
+        } else
+                msm_fb_register_device("lcdc", 0);
+}
+
+int __init staging_init_pmem(void)
+{
+        qsd8x50_allocate_memory_regions();
+        return 0;
+}
+
+int __init staging_init_devices(void)
+{
+        platform_add_devices(devices, ARRAY_SIZE(devices));
+        msm_fb_add_devices();
+        return 0;
+}
+
+arch_initcall(staging_init_pmem);
+arch_initcall(staging_init_devices);
diff --git a/drivers/staging/msm/tv_ntsc.c b/drivers/staging/msm/tv_ntsc.c
new file mode 100644
index 000000000000..5eb67611661a
--- /dev/null
+++ b/drivers/staging/msm/tv_ntsc.c
@@ -0,0 +1,163 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/uaccess.h>
+#include <linux/clk.h>
+
+#include "msm_fb.h"
+#include "tvenc.h"
+
+#define NTSC_TV_DIMENSION_WIDTH      720
+#define NTSC_TV_DIMENSION_HEIGHT     480
+
+static int ntsc_off(struct platform_device *pdev);
+static int ntsc_on(struct platform_device *pdev);
+
+static int ntsc_on(struct platform_device *pdev)
+{
+        uint32 reg = 0;
+        int ret = 0;
+        struct msm_fb_data_type *mfd;
+
+        mfd = platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        TV_OUT(TV_ENC_CTL, 0);  /* disable TV encoder */
+
+        if (mfd->panel.id == NTSC_M) {
+                /* Cr gain 11, Cb gain C6, y_gain 97 */
+                TV_OUT(TV_GAIN, 0x0081B697);
+        } else {
+                /* Cr gain 11, Cb gain C6, y_gain 97 */
+                TV_OUT(TV_GAIN, 0x008bc4a3);
+                reg |= TVENC_CTL_NTSCJ_MODE;
+        }
+
+        TV_OUT(TV_CGMS, 0x0);
+        /*  NTSC Timing */
+        TV_OUT(TV_SYNC_1, 0x0020009e);
+        TV_OUT(TV_SYNC_2, 0x011306B4);
+        TV_OUT(TV_SYNC_3, 0x0006000C);
+        TV_OUT(TV_SYNC_4, 0x0028020D);
+        TV_OUT(TV_SYNC_5, 0x005E02FB);
+        TV_OUT(TV_SYNC_6, 0x0006000C);
+        TV_OUT(TV_SYNC_7, 0x00000012);
+        TV_OUT(TV_BURST_V1, 0x0013020D);
+        TV_OUT(TV_BURST_V2, 0x0014020C);
+        TV_OUT(TV_BURST_V3, 0x0013020D);
+        TV_OUT(TV_BURST_V4, 0x0014020C);
+        TV_OUT(TV_BURST_H, 0x00AE00F2);
+        TV_OUT(TV_SOL_REQ_ODD, 0x00280208);
+        TV_OUT(TV_SOL_REQ_EVEN, 0x00290209);
+
+        reg |= TVENC_CTL_TV_MODE_NTSC_M_PAL60;
+
+        reg |= TVENC_CTL_Y_FILTER_EN |
+            TVENC_CTL_CR_FILTER_EN |
+            TVENC_CTL_CB_FILTER_EN | TVENC_CTL_SINX_FILTER_EN;
+#ifdef CONFIG_FB_MSM_TVOUT_SVIDEO
+        reg |= TVENC_CTL_S_VIDEO_EN;
+#endif
+
+        TV_OUT(TV_LEVEL, 0x00000000);   /* DC offset to 0. */
+        TV_OUT(TV_OFFSET, 0x008080f0);
+
+#ifdef CONFIG_FB_MSM_MDP31
+        TV_OUT(TV_DAC_INTF, 0x29);
+#endif
+        TV_OUT(TV_ENC_CTL, reg);
+
+        reg |= TVENC_CTL_ENC_EN;
+        TV_OUT(TV_ENC_CTL, reg);
+
+        return ret;
+}
+
+static int ntsc_off(struct platform_device *pdev)
+{
+        TV_OUT(TV_ENC_CTL, 0);  /* disable TV encoder */
+        return 0;
+}
+
+static int __init ntsc_probe(struct platform_device *pdev)
+{
+        msm_fb_add_device(pdev);
+
+        return 0;
+}
+
+static struct platform_driver this_driver = {
+        .probe  = ntsc_probe,
+        .driver = {
+                .name   = "tv_ntsc",
+        },
+};
+
+static struct msm_fb_panel_data ntsc_panel_data = {
+        .panel_info.xres = NTSC_TV_DIMENSION_WIDTH,
+        .panel_info.yres = NTSC_TV_DIMENSION_HEIGHT,
+        .panel_info.type = TV_PANEL,
+        .panel_info.pdest = DISPLAY_1,
+        .panel_info.wait_cycle = 0,
+        .panel_info.bpp = 16,
+        .panel_info.fb_num = 2,
+        .on = ntsc_on,
+        .off = ntsc_off,
+};
+
+static struct platform_device this_device = {
+        .name   = "tv_ntsc",
+        .id     = 0,
+        .dev    = {
+                .platform_data = &ntsc_panel_data,
+        }
+};
+
+static int __init ntsc_init(void)
+{
+        int ret;
+
+        ret = platform_driver_register(&this_driver);
+        if (!ret) {
+                ret = platform_device_register(&this_device);
+                if (ret)
+                        platform_driver_unregister(&this_driver);
+        }
+
+        return ret;
+}
+
+module_init(ntsc_init);
\ No newline at end of file
diff --git a/drivers/staging/msm/tv_pal.c b/drivers/staging/msm/tv_pal.c
new file mode 100644
index 000000000000..204da514660e
--- /dev/null
+++ b/drivers/staging/msm/tv_pal.c
@@ -0,0 +1,213 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/uaccess.h>
+#include <linux/clk.h>
+
+#include "msm_fb.h"
+#include "tvenc.h"
+
+#ifdef CONFIG_FB_MSM_TVOUT_PAL_M
+#define PAL_TV_DIMENSION_WIDTH      720
+#define PAL_TV_DIMENSION_HEIGHT     480
+#else
+#define PAL_TV_DIMENSION_WIDTH      720
+#define PAL_TV_DIMENSION_HEIGHT     576
+#endif
+
+static int pal_on(struct platform_device *pdev)
+{
+        uint32 reg = 0;
+        int ret = 0;
+        struct msm_fb_data_type *mfd;
+
+        mfd = platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        TV_OUT(TV_ENC_CTL, 0);  /* disable TV encoder */
+
+        switch (mfd->panel.id) {
+        case PAL_BDGHIN:
+                /* Cr gain 11, Cb gain C6, y_gain 97 */
+                TV_OUT(TV_GAIN, 0x0088c1a0);
+                TV_OUT(TV_CGMS, 0x00012345);
+                TV_OUT(TV_TEST_MUX, 0x0);
+                /*  PAL Timing */
+                TV_OUT(TV_SYNC_1, 0x00180097);
+                TV_OUT(TV_SYNC_2, 0x011f06c0);
+                TV_OUT(TV_SYNC_3, 0x0005000a);
+                TV_OUT(TV_SYNC_4, 0x00320271);
+                TV_OUT(TV_SYNC_5, 0x005602f9);
+                TV_OUT(TV_SYNC_6, 0x0005000a);
+                TV_OUT(TV_SYNC_7, 0x0000000f);
+                TV_OUT(TV_BURST_V1, 0x0012026e);
+                TV_OUT(TV_BURST_V2, 0x0011026d);
+                TV_OUT(TV_BURST_V3, 0x00100270);
+                TV_OUT(TV_BURST_V4, 0x0013026f);
+                TV_OUT(TV_BURST_H, 0x00af00ea);
+                TV_OUT(TV_SOL_REQ_ODD, 0x0030026e);
+                TV_OUT(TV_SOL_REQ_EVEN, 0x0031026f);
+
+                reg |= TVENC_CTL_TV_MODE_PAL_BDGHIN;
+                break;
+        case PAL_M:
+                /* Cr gain 11, Cb gain C6, y_gain 97 */
+                TV_OUT(TV_GAIN, 0x0081b697);
+                TV_OUT(TV_CGMS, 0x000af317);
+                TV_OUT(TV_TEST_MUX, 0x000001c3);
+                TV_OUT(TV_TEST_MODE, 0x00000002);
+                /*  PAL Timing */
+                TV_OUT(TV_SYNC_1, 0x0020009e);
+                TV_OUT(TV_SYNC_2, 0x011306b4);
+                TV_OUT(TV_SYNC_3, 0x0006000c);
+                TV_OUT(TV_SYNC_4, 0x0028020D);
+                TV_OUT(TV_SYNC_5, 0x005e02fb);
+                TV_OUT(TV_SYNC_6, 0x0006000c);
+                TV_OUT(TV_SYNC_7, 0x00000012);
+                TV_OUT(TV_BURST_V1, 0x0012020b);
+                TV_OUT(TV_BURST_V2, 0x0016020c);
+                TV_OUT(TV_BURST_V3, 0x00150209);
+                TV_OUT(TV_BURST_V4, 0x0013020c);
+                TV_OUT(TV_BURST_H, 0x00bf010b);
+                TV_OUT(TV_SOL_REQ_ODD, 0x00280208);
+                TV_OUT(TV_SOL_REQ_EVEN, 0x00290209);
+
+                reg |= TVENC_CTL_TV_MODE_PAL_M;
+                break;
+        case PAL_N:
+                /* Cr gain 11, Cb gain C6, y_gain 97 */
+                TV_OUT(TV_GAIN, 0x0081b697);
+                TV_OUT(TV_CGMS, 0x000af317);
+                TV_OUT(TV_TEST_MUX, 0x000001c3);
+                TV_OUT(TV_TEST_MODE, 0x00000002);
+                /*  PAL Timing */
+                TV_OUT(TV_SYNC_1, 0x00180097);
+                TV_OUT(TV_SYNC_2, 0x12006c0);
+                TV_OUT(TV_SYNC_3, 0x0005000a);
+                TV_OUT(TV_SYNC_4, 0x00320271);
+                TV_OUT(TV_SYNC_5, 0x005602f9);
+                TV_OUT(TV_SYNC_6, 0x0005000a);
+                TV_OUT(TV_SYNC_7, 0x0000000f);
+                TV_OUT(TV_BURST_V1, 0x0012026e);
+                TV_OUT(TV_BURST_V2, 0x0011026d);
+                TV_OUT(TV_BURST_V3, 0x00100270);
+                TV_OUT(TV_BURST_V4, 0x0013026f);
+                TV_OUT(TV_BURST_H, 0x00af00fa);
+                TV_OUT(TV_SOL_REQ_ODD, 0x0030026e);
+                TV_OUT(TV_SOL_REQ_EVEN, 0x0031026f);
+
+                reg |= TVENC_CTL_TV_MODE_PAL_N;
+                break;
+
+        default:
+                return -ENODEV;
+        }
+
+        reg |= TVENC_CTL_Y_FILTER_EN |
+            TVENC_CTL_CR_FILTER_EN |
+            TVENC_CTL_CB_FILTER_EN | TVENC_CTL_SINX_FILTER_EN;
+#ifdef CONFIG_FB_MSM_TVOUT_SVIDEO
+        reg |= TVENC_CTL_S_VIDEO_EN;
+#endif
+
+        TV_OUT(TV_LEVEL, 0x00000000);   /* DC offset to 0. */
+        TV_OUT(TV_OFFSET, 0x008080f0);
+
+#ifdef CONFIG_FB_MSM_MDP31
+        TV_OUT(TV_DAC_INTF, 0x29);
+#endif
+        TV_OUT(TV_ENC_CTL, reg);
+
+        reg |= TVENC_CTL_ENC_EN;
+        TV_OUT(TV_ENC_CTL, reg);
+
+        return ret;
+}
+
+static int pal_off(struct platform_device *pdev)
+{
+        TV_OUT(TV_ENC_CTL, 0);  /* disable TV encoder */
+        return 0;
+}
+
+static int __init pal_probe(struct platform_device *pdev)
+{
+        msm_fb_add_device(pdev);
+
+        return 0;
+}
+
+static struct platform_driver this_driver = {
+        .probe  = pal_probe,
+        .driver = {
+                .name   = "tv_pal",
+        },
+};
+
+static struct msm_fb_panel_data pal_panel_data = {
+        .panel_info.xres = PAL_TV_DIMENSION_WIDTH,
+        .panel_info.yres = PAL_TV_DIMENSION_HEIGHT,
+        .panel_info.type = TV_PANEL,
+        .panel_info.pdest = DISPLAY_1,
+        .panel_info.wait_cycle = 0,
+        .panel_info.bpp = 16,
+        .panel_info.fb_num = 2,
+        .on = pal_on,
+        .off = pal_off,
+};
+
+static struct platform_device this_device = {
+        .name   = "tv_pal",
+        .id     = 0,
+        .dev    = {
+                .platform_data = &pal_panel_data,
+        }
+};
+
+static int __init pal_init(void)
+{
+        int ret;
+
+        ret = platform_driver_register(&this_driver);
+        if (!ret) {
+                ret = platform_device_register(&this_device);
+                if (ret)
+                        platform_driver_unregister(&this_driver);
+        }
+
+        return ret;
+}
+
+module_init(pal_init);
diff --git a/drivers/staging/msm/tvenc.c b/drivers/staging/msm/tvenc.c
new file mode 100644
index 000000000000..f41c5ac22f25
--- /dev/null
+++ b/drivers/staging/msm/tvenc.c
@@ -0,0 +1,295 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. 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 version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
+ * 02110-1301, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+#include <linux/semaphore.h>
+#include <linux/uaccess.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/pm_qos_params.h>
+
+#define TVENC_C
+#include "tvenc.h"
+#include "msm_fb.h"
+
+static int tvenc_probe(struct platform_device *pdev);
+static int tvenc_remove(struct platform_device *pdev);
+
+static int tvenc_off(struct platform_device *pdev);
+static int tvenc_on(struct platform_device *pdev);
+
+static struct platform_device *pdev_list[MSM_FB_MAX_DEV_LIST];
+static int pdev_list_cnt;
+
+static struct clk *tvenc_clk;
+static struct clk *tvdac_clk;
+
+static struct platform_driver tvenc_driver = {
+        .probe = tvenc_probe,
+        .remove = tvenc_remove,
+        .suspend = NULL,
+//      .suspend_late = NULL,
+//      .resume_early = NULL,
+        .resume = NULL,
+        .shutdown = NULL,
+        .driver = {
+                   .name = "tvenc",
+                   },
+};
+
+static struct tvenc_platform_data *tvenc_pdata;
+
+static int tvenc_off(struct platform_device *pdev)
+{
+        int ret = 0;
+
+        ret = panel_next_off(pdev);
+
+        clk_disable(tvenc_clk);
+        clk_disable(tvdac_clk);
+
+        if (tvenc_pdata && tvenc_pdata->pm_vid_en)
+                ret = tvenc_pdata->pm_vid_en(0);
+
+        //pm_qos_update_requirement(PM_QOS_SYSTEM_BUS_FREQ , "tvenc",
+        //                              PM_QOS_DEFAULT_VALUE);
+
+        if (ret)
+                printk(KERN_ERR "%s: pm_vid_en(off) failed! %d\n",
+                __func__, ret);
+
+        return ret;
+}
+
+static int tvenc_on(struct platform_device *pdev)
+{
+        int ret = 0;
+
+//      pm_qos_update_requirement(PM_QOS_SYSTEM_BUS_FREQ , "tvenc",
+//                              128000);
+        if (tvenc_pdata && tvenc_pdata->pm_vid_en)
+                ret = tvenc_pdata->pm_vid_en(1);
+
+        if (ret) {
+                printk(KERN_ERR "%s: pm_vid_en(on) failed! %d\n",
+                __func__, ret);
+                return ret;
+        }
+
+        clk_enable(tvenc_clk);
+        clk_enable(tvdac_clk);
+
+        ret = panel_next_on(pdev);
+
+        return ret;
+}
+
+void tvenc_gen_test_pattern(struct msm_fb_data_type *mfd)
+{
+        uint32 reg = 0, i;
+
+        reg = readl(MSM_TV_ENC_CTL);
+        reg |= TVENC_CTL_TEST_PATT_EN;
+
+        for (i = 0; i < 3; i++) {
+                TV_OUT(TV_ENC_CTL, 0);  /* disable TV encoder */
+
+                switch (i) {
+                        /*
+                         * TV Encoder - Color Bar Test Pattern
+                         */
+                case 0:
+                        reg |= TVENC_CTL_TPG_CLRBAR;
+                        break;
+                        /*
+                         * TV Encoder - Red Frame Test Pattern
+                         */
+                case 1:
+                        reg |= TVENC_CTL_TPG_REDCLR;
+                        break;
+                        /*
+                         * TV Encoder - Modulated Ramp Test Pattern
+                         */
+                default:
+                        reg |= TVENC_CTL_TPG_MODRAMP;
+                        break;
+                }
+
+                TV_OUT(TV_ENC_CTL, reg);
+                mdelay(5000);
+
+                switch (i) {
+                        /*
+                         * TV Encoder - Color Bar Test Pattern
+                         */
+                case 0:
+                        reg &= ~TVENC_CTL_TPG_CLRBAR;
+                        break;
+                        /*
+                         * TV Encoder - Red Frame Test Pattern
+                         */
+                case 1:
+                        reg &= ~TVENC_CTL_TPG_REDCLR;
+                        break;
+                        /*
+                         * TV Encoder - Modulated Ramp Test Pattern
+                         */
+                default:
+                        reg &= ~TVENC_CTL_TPG_MODRAMP;
+                        break;
+                }
+        }
+}
+
+static int tvenc_resource_initialized;
+
+static int tvenc_probe(struct platform_device *pdev)
+{
+        struct msm_fb_data_type *mfd;
+        struct platform_device *mdp_dev = NULL;
+        struct msm_fb_panel_data *pdata = NULL;
+        int rc;
+
+        if (pdev->id == 0) {
+                tvenc_base = ioremap(pdev->resource[0].start,
+                                        pdev->resource[0].end -
+                                        pdev->resource[0].start + 1);
+                if (!tvenc_base) {
+                        printk(KERN_ERR
+                                "tvenc_base ioremap failed!\n");
+                        return -ENOMEM;
+                }
+                tvenc_pdata = pdev->dev.platform_data;
+                tvenc_resource_initialized = 1;
+                return 0;
+        }
+
+        if (!tvenc_resource_initialized)
+                return -EPERM;
+
+        mfd = platform_get_drvdata(pdev);
+
+        if (!mfd)
+                return -ENODEV;
+
+        if (mfd->key != MFD_KEY)
+                return -EINVAL;
+
+        if (pdev_list_cnt >= MSM_FB_MAX_DEV_LIST)
+                return -ENOMEM;
+
+        if (tvenc_base == NULL)
+                return -ENOMEM;
+
+        mdp_dev = platform_device_alloc("mdp", pdev->id);
+        if (!mdp_dev)
+                return -ENOMEM;
+
+        /*
+         * link to the latest pdev
+         */
+        mfd->pdev = mdp_dev;
+        mfd->dest = DISPLAY_TV;
+
+        /*
+         * alloc panel device data
+         */
+        if (platform_device_add_data
+            (mdp_dev, pdev->dev.platform_data,
+             sizeof(struct msm_fb_panel_data))) {
+                printk(KERN_ERR "tvenc_probe: platform_device_add_data failed!\n");
+                platform_device_put(mdp_dev);
+                return -ENOMEM;
+        }
+        /*
+         * data chain
+         */
+        pdata = mdp_dev->dev.platform_data;
+        pdata->on = tvenc_on;
+        pdata->off = tvenc_off;
+        pdata->next = pdev;
+
+        /*
+         * get/set panel specific fb info
+         */
+        mfd->panel_info = pdata->panel_info;
+        mfd->fb_imgType = MDP_YCRYCB_H2V1;
+
+        /*
+         * set driver data
+         */
+        platform_set_drvdata(mdp_dev, mfd);
+
+        /*
+         * register in mdp driver
+         */
+        rc = platform_device_add(mdp_dev);
+        if (rc)
+                goto tvenc_probe_err;
+
+        pdev_list[pdev_list_cnt++] = pdev;
+        return 0;
+
+tvenc_probe_err:
+        platform_device_put(mdp_dev);
+        return rc;
+}
+
+static int tvenc_remove(struct platform_device *pdev)
+{
+//      pm_qos_remove_requirement(PM_QOS_SYSTEM_BUS_FREQ , "tvenc");
+        return 0;
+}
+
+static int tvenc_register_driver(void)
+{
+        return platform_driver_register(&tvenc_driver);
+}
+
+static int __init tvenc_driver_init(void)
+{
+        tvenc_clk = clk_get(NULL, "tv_enc_clk");
+        tvdac_clk = clk_get(NULL, "tv_dac_clk");
+
+        if (IS_ERR(tvenc_clk)) {
+                printk(KERN_ERR "error: can't get tvenc_clk!\n");
+                return IS_ERR(tvenc_clk);
+        }
+
+        if (IS_ERR(tvdac_clk)) {
+                printk(KERN_ERR "error: can't get tvdac_clk!\n");
+                return IS_ERR(tvdac_clk);
+        }
+
+//      pm_qos_add_requirement(PM_QOS_SYSTEM_BUS_FREQ , "tvenc",
+//                              PM_QOS_DEFAULT_VALUE);
+        return tvenc_register_driver();
+}
+
+module_init(tvenc_driver_init);
diff --git a/drivers/staging/msm/tvenc.h b/drivers/staging/msm/tvenc.h
new file mode 100644
index 000000000000..a682dbebcf7d
--- /dev/null
+++ b/drivers/staging/msm/tvenc.h
@@ -0,0 +1,117 @@
+/* Copyright (c) 2008-2009, Code Aurora Forum. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Code Aurora nor
+ *       the names of its contributors may be used to endorse or promote
+ *       products derived from this software without specific prior written
+ *       permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NON-INFRINGEMENT ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+ * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef TVENC_H
+#define TVENC_H
+
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/time.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/fb.h>
+
+#include <mach/hardware.h>
+#include <linux/io.h>
+
+#include <asm/system.h>
+#include <asm/mach-types.h>
+
+#include "msm_fb_panel.h"
+
+#define NTSC_M          0 /* North America, Korea */
+#define NTSC_J          1 /* Japan */
+#define PAL_BDGHIN      2 /* Non-argentina PAL-N */
+#define PAL_M           3 /* PAL-M */
+#define PAL_N           4 /* Argentina PAL-N */
+
+/* 3.57954545 Mhz */
+#define TVENC_CTL_TV_MODE_NTSC_M_PAL60  0
+/* 3.57961149 Mhz */
+#define TVENC_CTL_TV_MODE_PAL_M             BIT(0)
+/*non-Argintina = 4.3361875 Mhz */
+#define TVENC_CTL_TV_MODE_PAL_BDGHIN        BIT(1)
+/*Argentina = 3.582055625 Mhz */
+#define TVENC_CTL_TV_MODE_PAL_N             (BIT(1)|BIT(0))
+
+#define TVENC_CTL_ENC_EN                    BIT(2)
+#define TVENC_CTL_CC_EN                     BIT(3)
+#define TVENC_CTL_CGMS_EN                   BIT(4)
+#define TVENC_CTL_MACRO_EN                  BIT(5)
+#define TVENC_CTL_Y_FILTER_W_NOTCH          BIT(6)
+#define TVENC_CTL_Y_FILTER_WO_NOTCH         0
+#define TVENC_CTL_Y_FILTER_EN               BIT(7)
+#define TVENC_CTL_CR_FILTER_EN              BIT(8)
+#define TVENC_CTL_CB_FILTER_EN              BIT(9)
+#define TVENC_CTL_SINX_FILTER_EN            BIT(10)
+#define TVENC_CTL_TEST_PATT_EN              BIT(11)
+#define TVENC_CTL_OUTPUT_INV                BIT(12)
+#define TVENC_CTL_PAL60_MODE                BIT(13)
+#define TVENC_CTL_NTSCJ_MODE                BIT(14)
+#define TVENC_CTL_TPG_CLRBAR                0
+#define TVENC_CTL_TPG_MODRAMP               BIT(15)
+#define TVENC_CTL_TPG_REDCLR                BIT(16)
+#define TVENC_CTL_S_VIDEO_EN                BIT(19)
+
+#ifdef TVENC_C
+void *tvenc_base;
+#else
+extern void *tvenc_base;
+#endif
+
+#define TV_OUT(reg, v)  writel(v, tvenc_base + MSM_##reg)
+
+#define MSM_TV_ENC_CTL                  0x00
+#define MSM_TV_LEVEL                    0x04
+#define MSM_TV_GAIN                     0x08
+#define MSM_TV_OFFSET                   0x0c
+#define MSM_TV_CGMS                     0x10
+#define MSM_TV_SYNC_1                   0x14
+#define MSM_TV_SYNC_2                   0x18
+#define MSM_TV_SYNC_3                   0x1c
+#define MSM_TV_SYNC_4                   0x20
+#define MSM_TV_SYNC_5                   0x24
+#define MSM_TV_SYNC_6                   0x28
+#define MSM_TV_SYNC_7                   0x2c
+#define MSM_TV_BURST_V1                 0x30
+#define MSM_TV_BURST_V2                 0x34
+#define MSM_TV_BURST_V3                 0x38
+#define MSM_TV_BURST_V4                 0x3c
+#define MSM_TV_BURST_H                  0x40
+#define MSM_TV_SOL_REQ_ODD              0x44
+#define MSM_TV_SOL_REQ_EVEN             0x48
+#define MSM_TV_DAC_CTL                  0x4c
+#define MSM_TV_TEST_MUX                 0x50
+#define MSM_TV_TEST_MODE                0x54
+#define MSM_TV_TEST_MISR_RESET          0x58
+#define MSM_TV_TEST_EXPORT_MISR         0x5c
+#define MSM_TV_TEST_MISR_CURR_VAL       0x60
+#define MSM_TV_TEST_SOF_CFG             0x64
+#define MSM_TV_DAC_INTF                 0x100
+
+#endif /* TVENC_H */
diff --git a/drivers/staging/phison/Kconfig b/drivers/staging/phison/Kconfig
index d3c65d315a2e..1b56119a7657 100644
--- a/drivers/staging/phison/Kconfig
+++ b/drivers/staging/phison/Kconfig
@@ -1,5 +1,5 @@
 config IDE_PHISON
         tristate "PCIE ATA PS5000 IDE support"
-        depends on PCI && ATA && ATA_SFF
+        depends on PCI && ATA && ATA_SFF && ATA_BMDMA
         ---help---
           This is an experimental driver for PS5000 IDE driver.
diff --git a/drivers/staging/pohmelfs/inode.c b/drivers/staging/pohmelfs/inode.c
index 9286e863b0e7..643b413d9f0f 100644
--- a/drivers/staging/pohmelfs/inode.c
+++ b/drivers/staging/pohmelfs/inode.c
@@ -29,7 +29,6 @@
 #include <linux/slab.h>
 #include <linux/statfs.h>
 #include <linux/writeback.h>
-#include <linux/quotaops.h>
 
 #include "netfs.h"
 
@@ -880,7 +879,7 @@ static struct inode *pohmelfs_alloc_inode(struct super_block *sb)
 /*
  * We want fsync() to work on POHMELFS.
  */
-static int pohmelfs_fsync(struct file *file, struct dentry *dentry, int datasync)
+static int pohmelfs_fsync(struct file *file, int datasync)
 {
         struct inode *inode = file->f_mapping->host;
         struct writeback_control wbc = {
@@ -969,13 +968,6 @@ int pohmelfs_setattr_raw(struct inode *inode, struct iattr *attr)
                 goto err_out_exit;
         }
 
-        if ((attr->ia_valid & ATTR_UID && attr->ia_uid != inode->i_uid) ||
-            (attr->ia_valid & ATTR_GID && attr->ia_gid != inode->i_gid)) {
-                err = dquot_transfer(inode, attr);
-                if (err)
-                        goto err_out_exit;
-        }
-
         err = inode_setattr(inode, attr);
         if (err) {
                 dprintk("%s: ino: %llu, failed to set the attributes.\n", __func__, POHMELFS_I(inode)->ino);
diff --git a/drivers/staging/rt2860/sta_ioctl.c b/drivers/staging/rt2860/sta_ioctl.c
index 112da7a6c417..6b8268d3dc75 100644
--- a/drivers/staging/rt2860/sta_ioctl.c
+++ b/drivers/staging/rt2860/sta_ioctl.c
@@ -2522,6 +2522,8 @@ int rt28xx_sta_ioctl(IN struct net_device *net_dev,
                         Status =
                             copy_to_user(erq->pointer, pAd->nickname,
                                          erq->length);
+                        if (Status)
+                                Status = -EFAULT;
                         break;
                 }
         case SIOCGIWRATE:       /*get default bit rate (bps) */
diff --git a/drivers/staging/sep/sep_driver.c b/drivers/staging/sep/sep_driver.c
index 0332c370fd82..ecbde3467b1b 100644
--- a/drivers/staging/sep/sep_driver.c
+++ b/drivers/staging/sep/sep_driver.c
@@ -594,8 +594,10 @@ static int sep_allocate_data_pool_memory_handler(struct sep_device *sep,
         dbg("SEP Driver:--------> sep_allocate_data_pool_memory_handler start\n");
 
         error = copy_from_user(&command_args, (void *) arg, sizeof(struct sep_driver_alloc_t));
-        if (error)
+        if (error) {
+                error = -EFAULT;
                 goto end_function;
+        }
 
         /* allocate memory */
         if ((sep->data_pool_bytes_allocated + command_args.num_bytes) > SEP_DRIVER_DATA_POOL_SHARED_AREA_SIZE_IN_BYTES) {
@@ -609,8 +611,10 @@ static int sep_allocate_data_pool_memory_handler(struct sep_device *sep,
 
         /* write the memory back to the user space */
         error = copy_to_user((void *) arg, (void *) &command_args, sizeof(struct sep_driver_alloc_t));
-        if (error)
+        if (error) {
+                error = -EFAULT;
                 goto end_function;
+        }
 
         /* set the allocation */
         sep->data_pool_bytes_allocated += command_args.num_bytes;
@@ -661,6 +665,8 @@ static int sep_write_into_data_pool_handler(struct sep_device *sep, unsigned lon
         }
         /* copy the application data */
         error = copy_from_user(virt_address, (void *) app_in_address, num_bytes);
+        if (error)
+                error = -EFAULT;
 end_function:
         dbg("SEP Driver:<-------- sep_write_into_data_pool_handler end\n");
         return error;
@@ -711,6 +717,8 @@ static int sep_read_from_data_pool_handler(struct sep_device *sep, unsigned long
 
         /* copy the application data */
         error = copy_to_user((void *) app_out_address, virt_address, num_bytes);
+        if (error)
+                error = -EFAULT;
 end_function:
         dbg("SEP Driver:<-------- sep_read_from_data_pool_handler end\n");
         return error;
@@ -1448,8 +1456,10 @@ static int sep_create_sync_dma_tables_handler(struct sep_device *sep,
         dbg("SEP Driver:--------> sep_create_sync_dma_tables_handler start\n");
 
         error = copy_from_user(&command_args, (void *) arg, sizeof(struct sep_driver_build_sync_table_t));
-        if (error)
+        if (error) {
+                error = -EFAULT;
                 goto end_function;
+        }
 
         edbg("app_in_address is %08lx\n", command_args.app_in_address);
         edbg("app_out_address is %08lx\n", command_args.app_out_address);
@@ -1799,8 +1809,10 @@ static int sep_create_flow_dma_tables_handler(struct sep_device *sep,
                 goto end_function;
 
         error = copy_from_user(&command_args, (void *) arg, sizeof(struct sep_driver_build_flow_table_t));
-        if (error)
+        if (error) {
+                error = -EFAULT;
                 goto end_function;
+        }
 
         /* create flow tables */
         error = sep_prepare_flow_dma_tables(sep, command_args.num_virtual_buffers, command_args.virt_buff_data_addr, flow_context_ptr, &first_table_data, &last_table_data, command_args.isKernelVirtualAddress);
@@ -1819,8 +1831,10 @@ static int sep_create_flow_dma_tables_handler(struct sep_device *sep,
 
         /* send the parameters to user application */
         error = copy_to_user((void *) arg, &command_args, sizeof(struct sep_driver_build_flow_table_t));
-        if (error)
+        if (error) {
+                error = -EFAULT;
                 goto end_function_with_error;
+        }
 
         /* all the flow created  - update the flow entry with temp id */
         flow_context_ptr->flow_id = SEP_TEMP_FLOW_ID;
@@ -1861,8 +1875,10 @@ static int sep_add_flow_tables_handler(struct sep_device *sep, unsigned long arg
 
         /* get input parameters */
         error = copy_from_user(&command_args, (void *) arg, sizeof(struct sep_driver_add_flow_table_t));
-        if (error)
+        if (error) {
+                error = -EFAULT;
                 goto end_function;
+        }
 
         /* find the flow structure for the flow id */
         flow_context_ptr = sep_find_flow_context(sep, command_args.flow_id);
@@ -1933,6 +1949,8 @@ static int sep_add_flow_tables_handler(struct sep_device *sep, unsigned long arg
 
         /* send the parameters to user application */
         error = copy_to_user((void *) arg, &command_args, sizeof(struct sep_driver_add_flow_table_t));
+        if (error)
+                error = -EFAULT;
 end_function_with_error:
         /* free the allocated tables */
         sep_deallocated_flow_tables(&first_table_data);
@@ -1953,8 +1971,10 @@ static int sep_add_flow_tables_message_handler(struct sep_device *sep, unsigned
         dbg("SEP Driver:--------> sep_add_flow_tables_message_handler start\n");
 
         error = copy_from_user(&command_args, (void *) arg, sizeof(struct sep_driver_add_message_t));
-        if (error)
+        if (error) {
+                error = -EFAULT;
                 goto end_function;
+        }
 
         /* check input */
         if (command_args.message_size_in_bytes > SEP_MAX_ADD_MESSAGE_LENGTH_IN_BYTES) {
@@ -1970,6 +1990,8 @@ static int sep_add_flow_tables_message_handler(struct sep_device *sep, unsigned
         /* copy the message into context */
         flow_context_ptr->message_size_in_bytes = command_args.message_size_in_bytes;
         error = copy_from_user(flow_context_ptr->message, (void *) command_args.message_address, command_args.message_size_in_bytes);
+        if (error)
+                error = -EFAULT;
 end_function:
         dbg("SEP Driver:<-------- sep_add_flow_tables_message_handler end\n");
         return error;
@@ -1994,6 +2016,8 @@ static int sep_get_static_pool_addr_handler(struct sep_device *sep, unsigned lon
 
         /* send the parameters to user application */
         error = copy_to_user((void *) arg, &command_args, sizeof(struct sep_driver_static_pool_addr_t));
+        if (error)
+                error = -EFAULT;
         dbg("SEP Driver:<-------- sep_get_static_pool_addr_handler end\n");
         return error;
 }
@@ -2010,8 +2034,10 @@ static int sep_get_physical_mapped_offset_handler(struct sep_device *sep, unsign
         dbg("SEP Driver:--------> sep_get_physical_mapped_offset_handler start\n");
 
         error = copy_from_user(&command_args, (void *) arg, sizeof(struct sep_driver_get_mapped_offset_t));
-        if (error)
+        if (error) {
+                error = -EFAULT;
                 goto end_function;
+        }
 
         if (command_args.physical_address < sep->shared_bus) {
                 error = -EINVAL;
@@ -2025,6 +2051,8 @@ static int sep_get_physical_mapped_offset_handler(struct sep_device *sep, unsign
 
         /* send the parameters to user application */
         error = copy_to_user((void *) arg, &command_args, sizeof(struct sep_driver_get_mapped_offset_t));
+        if (error)
+                error = -EFAULT;
 end_function:
         dbg("SEP Driver:<-------- sep_get_physical_mapped_offset_handler end\n");
         return error;
@@ -2070,11 +2098,11 @@ static int sep_init_handler(struct sep_device *sep, unsigned long arg)
         error = 0;
 
         error = copy_from_user(&command_args, (void *) arg, sizeof(struct sep_driver_init_t));
-
-        dbg("SEP Driver:--------> sep_init_handler - finished copy_from_user \n");
-
-        if (error)
+        if (error) {
+                error = -EFAULT;
                 goto end_function;
+        }
+        dbg("SEP Driver:--------> sep_init_handler - finished copy_from_user\n");
 
         /* PATCH - configure the DMA to single -burst instead of multi-burst */
         /*sep_configure_dma_burst(); */
diff --git a/drivers/staging/tm6000/Kconfig b/drivers/staging/tm6000/Kconfig
index 5fe759cc2ee9..3657e33e8817 100644
--- a/drivers/staging/tm6000/Kconfig
+++ b/drivers/staging/tm6000/Kconfig
@@ -2,7 +2,8 @@ config VIDEO_TM6000
         tristate "TV Master TM5600/6000/6010 driver"
         depends on VIDEO_DEV && I2C && INPUT && USB && EXPERIMENTAL
         select VIDEO_TUNER
-        select TUNER_XC2028
+        select MEDIA_TUNER_XC2028
+        select MEDIA_TUNER_XC5000
         select VIDEOBUF_VMALLOC
         help
           Support for TM5600/TM6000/TM6010 USB Device
diff --git a/drivers/staging/tm6000/tm6000-alsa.c b/drivers/staging/tm6000/tm6000-alsa.c
index bc89f9d28002..ce081cd44ad4 100644
--- a/drivers/staging/tm6000/tm6000-alsa.c
+++ b/drivers/staging/tm6000/tm6000-alsa.c
@@ -410,5 +410,28 @@ error:
         snd_card_free(card);
         return rc;
 }
-EXPORT_SYMBOL_GPL(tm6000_audio_init);
 
+static int tm6000_audio_fini(struct tm6000_core *dev)
+{
+        return 0;
+}
+
+struct tm6000_ops audio_ops = {
+        .id     = TM6000_AUDIO,
+        .name   = "TM6000 Audio Extension",
+        .init   = tm6000_audio_init,
+        .fini   = tm6000_audio_fini,
+};
+
+static int __init tm6000_alsa_register(void)
+{
+        return tm6000_register_extension(&audio_ops);
+}
+
+static void __exit tm6000_alsa_unregister(void)
+{
+        tm6000_unregister_extension(&audio_ops);
+}
+
+module_init(tm6000_alsa_register);
+module_exit(tm6000_alsa_unregister);
diff --git a/drivers/staging/tm6000/tm6000-cards.c b/drivers/staging/tm6000/tm6000-cards.c
index 6143e20d139d..cedd9044022f 100644
--- a/drivers/staging/tm6000/tm6000-cards.c
+++ b/drivers/staging/tm6000/tm6000-cards.c
@@ -363,13 +363,7 @@ int tm6000_tuner_callback(void *ptr, int component, int command, int arg)
                 tm6000_set_reg(dev, REQ_04_EN_DISABLE_MCU_INT,
                                         0x02, arg);
                 msleep(10);
-                rc = tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
-                                        TM6000_GPIO_CLK, 0);
-                if (rc < 0)
-                        return rc;
-                msleep(10);
-                rc = tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
-                                        TM6000_GPIO_CLK, 1);
+                rc = tm6000_i2c_reset(dev, 10);
                 break;
         case XC2028_TUNER_RESET:
                 /* Reset codes during load firmware */
@@ -423,14 +417,7 @@ int tm6000_tuner_callback(void *ptr, int component, int command, int arg)
                         break;
 
                 case 2:
-                        rc = tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
-                                                TM6000_GPIO_CLK, 0);
-                        if (rc < 0)
-                                return rc;
-                        msleep(100);
-                        rc = tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
-                                                TM6000_GPIO_CLK, 1);
-                        msleep(100);
+                        rc = tm6000_i2c_reset(dev, 100);
                         break;
                 }
         }
@@ -563,7 +550,7 @@ static void tm6000_config_tuner(struct tm6000_core *dev)
 
         switch (dev->tuner_type) {
         case TUNER_XC2028:
-                tun_setup.tuner_callback = tm6000_tuner_callback;;
+                tun_setup.tuner_callback = tm6000_tuner_callback;
                 break;
         case TUNER_XC5000:
                 tun_setup.tuner_callback = tm6000_xc5000_callback;
@@ -692,6 +679,10 @@ static int tm6000_init_dev(struct tm6000_core *dev)
         if (rc < 0)
                 goto err;
 
+        tm6000_add_into_devlist(dev);
+
+        tm6000_init_extension(dev);
+
         if (dev->caps.has_dvb) {
                 dev->dvb = kzalloc(sizeof(*(dev->dvb)), GFP_KERNEL);
                 if (!dev->dvb) {
@@ -921,6 +912,25 @@ static void tm6000_usb_disconnect(struct usb_interface *interface)
         }
 #endif
 
+        if (dev->gpio.power_led) {
+                switch (dev->model) {
+                case TM6010_BOARD_HAUPPAUGE_900H:
+                case TM6010_BOARD_TERRATEC_CINERGY_HYBRID_XE:
+                case TM6010_BOARD_TWINHAN_TU501:
+                        /* Power led off */
+                        tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+                                dev->gpio.power_led, 0x01);
+                        msleep(15);
+                        break;
+                case TM6010_BOARD_BEHOLD_WANDER:
+                case TM6010_BOARD_BEHOLD_VOYAGER:
+                        /* Power led off */
+                        tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+                                dev->gpio.power_led, 0x00);
+                        msleep(15);
+                        break;
+                }
+        }
         tm6000_v4l2_unregister(dev);
 
         tm6000_i2c_unregister(dev);
@@ -931,6 +941,9 @@ static void tm6000_usb_disconnect(struct usb_interface *interface)
 
         usb_put_dev(dev->udev);
 
+        tm6000_remove_from_devlist(dev);
+        tm6000_close_extension(dev);
+
         mutex_unlock(&dev->lock);
         kfree(dev);
 }
diff --git a/drivers/staging/tm6000/tm6000-core.c b/drivers/staging/tm6000/tm6000-core.c
index bfbc53bd2912..27f3f551b545 100644
--- a/drivers/staging/tm6000/tm6000-core.c
+++ b/drivers/staging/tm6000/tm6000-core.c
@@ -153,6 +153,22 @@ int tm6000_get_reg32 (struct tm6000_core *dev, u8 req, u16 value, u16 index)
         return buf[3] | buf[2] << 8 | buf[1] << 16 | buf[0] << 24;
 }
 
+int tm6000_i2c_reset(struct tm6000_core *dev, u16 tsleep)
+{
+        int rc;
+
+        rc = tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, TM6000_GPIO_CLK, 0);
+        if (rc < 0)
+                return rc;
+
+        msleep(tsleep);
+
+        rc = tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN, TM6000_GPIO_CLK, 1);
+        msleep(tsleep);
+
+        return rc;
+}
+
 void tm6000_set_fourcc_format(struct tm6000_core *dev)
 {
         if (dev->dev_type == TM6010) {
@@ -323,6 +339,12 @@ int tm6000_init_analog_mode (struct tm6000_core *dev)
         tm6000_set_standard (dev, &dev->norm);
         tm6000_set_audio_bitrate (dev,48000);
 
+        /* switch dvb led off */
+        if (dev->gpio.dvb_led) {
+                tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+                        dev->gpio.dvb_led, 0x01);
+        }
+
         return 0;
 }
 
@@ -375,6 +397,13 @@ int tm6000_init_digital_mode (struct tm6000_core *dev)
                 tm6000_set_reg (dev, REQ_04_EN_DISABLE_MCU_INT, 0x0020, 0x00);
                 msleep(100);
         }
+
+        /* switch dvb led on */
+        if (dev->gpio.dvb_led) {
+                tm6000_set_reg(dev, REQ_03_SET_GET_MCU_PIN,
+                        dev->gpio.dvb_led, 0x00);
+        }
+
         return 0;
 }
 
@@ -600,3 +629,95 @@ printk("Original value=%d\n",val);
         return val;
 }
 EXPORT_SYMBOL_GPL(tm6000_set_audio_bitrate);
+
+static LIST_HEAD(tm6000_devlist);
+static DEFINE_MUTEX(tm6000_devlist_mutex);
+
+/*
+ * tm6000_realease_resource()
+ */
+
+void tm6000_remove_from_devlist(struct tm6000_core *dev)
+{
+        mutex_lock(&tm6000_devlist_mutex);
+        list_del(&dev->devlist);
+        mutex_unlock(&tm6000_devlist_mutex);
+};
+
+void tm6000_add_into_devlist(struct tm6000_core *dev)
+{
+        mutex_lock(&tm6000_devlist_mutex);
+        list_add_tail(&dev->devlist, &tm6000_devlist);
+        mutex_unlock(&tm6000_devlist_mutex);
+};
+
+/*
+ * Extension interface
+ */
+
+static LIST_HEAD(tm6000_extension_devlist);
+static DEFINE_MUTEX(tm6000_extension_devlist_lock);
+
+int tm6000_register_extension(struct tm6000_ops *ops)
+{
+        struct tm6000_core *dev = NULL;
+
+        mutex_lock(&tm6000_devlist_mutex);
+        mutex_lock(&tm6000_extension_devlist_lock);
+        list_add_tail(&ops->next, &tm6000_extension_devlist);
+        list_for_each_entry(dev, &tm6000_devlist, devlist) {
+                if (dev)
+                        ops->init(dev);
+        }
+        printk(KERN_INFO "tm6000: Initialized (%s) extension\n", ops->name);
+        mutex_unlock(&tm6000_extension_devlist_lock);
+        mutex_unlock(&tm6000_devlist_mutex);
+        return 0;
+}
+EXPORT_SYMBOL(tm6000_register_extension);
+
+void tm6000_unregister_extension(struct tm6000_ops *ops)
+{
+        struct tm6000_core *dev = NULL;
+
+        mutex_lock(&tm6000_devlist_mutex);
+        list_for_each_entry(dev, &tm6000_devlist, devlist) {
+                if (dev)
+                        ops->fini(dev);
+        }
+
+        mutex_lock(&tm6000_extension_devlist_lock);
+        printk(KERN_INFO "tm6000: Remove (%s) extension\n", ops->name);
+        list_del(&ops->next);
+        mutex_unlock(&tm6000_extension_devlist_lock);
+        mutex_unlock(&tm6000_devlist_mutex);
+}
+EXPORT_SYMBOL(tm6000_unregister_extension);
+
+void tm6000_init_extension(struct tm6000_core *dev)
+{
+        struct tm6000_ops *ops = NULL;
+
+        mutex_lock(&tm6000_extension_devlist_lock);
+        if (!list_empty(&tm6000_extension_devlist)) {
+                list_for_each_entry(ops, &tm6000_extension_devlist, next) {
+                        if (ops->init)
+                                ops->init(dev);
+                }
+        }
+        mutex_unlock(&tm6000_extension_devlist_lock);
+}
+
+void tm6000_close_extension(struct tm6000_core *dev)
+{
+        struct tm6000_ops *ops = NULL;
+
+        mutex_lock(&tm6000_extension_devlist_lock);
+        if (!list_empty(&tm6000_extension_devlist)) {
+                list_for_each_entry(ops, &tm6000_extension_devlist, next) {
+                        if (ops->fini)
+                                ops->fini(dev);
+                }
+        }
+        mutex_unlock(&tm6000_extension_devlist_lock);
+}
diff --git a/drivers/staging/tm6000/tm6000-dvb.c b/drivers/staging/tm6000/tm6000-dvb.c
index eafc89c22b6b..261e66acbe46 100644
--- a/drivers/staging/tm6000/tm6000-dvb.c
+++ b/drivers/staging/tm6000/tm6000-dvb.c
@@ -28,6 +28,7 @@
 #include <media/tuner.h>
 
 #include "tuner-xc2028.h"
+#include "xc5000.h"
 
 static void inline print_err_status (struct tm6000_core *dev,
                                      int packet, int status)
@@ -100,7 +101,10 @@ int tm6000_start_stream(struct tm6000_core *dev)
 
         printk(KERN_INFO "tm6000: got start stream request %s\n",__FUNCTION__);
 
-        tm6000_init_digital_mode(dev);
+        if (dev->mode != TM6000_MODE_DIGITAL) {
+                tm6000_init_digital_mode(dev);
+                dev->mode = TM6000_MODE_DIGITAL;
+        }
 
         dvb->bulk_urb = usb_alloc_urb(0, GFP_KERNEL);
         if(dvb->bulk_urb == NULL) {
@@ -254,27 +258,55 @@ int tm6000_dvb_register(struct tm6000_core *dev)
         dvb->adapter.priv = dev;
 
         if (dvb->frontend) {
-                struct xc2028_config cfg = {
-                        .i2c_adap = &dev->i2c_adap,
-                        .i2c_addr = dev->tuner_addr,
-                };
-
-                dvb->frontend->callback = tm6000_tuner_callback;
-                ret = dvb_register_frontend(&dvb->adapter, dvb->frontend);
-                if (ret < 0) {
-                        printk(KERN_ERR
-                                "tm6000: couldn't register frontend\n");
-                        goto adapter_err;
-                }
-
-                if (!dvb_attach(xc2028_attach, dvb->frontend, &cfg)) {
-                        printk(KERN_ERR "tm6000: couldn't register "
-                                        "frontend (xc3028)\n");
-                        ret = -EINVAL;
-                        goto frontend_err;
+                switch (dev->tuner_type) {
+                case TUNER_XC2028: {
+                        struct xc2028_config cfg = {
+                                .i2c_adap = &dev->i2c_adap,
+                                .i2c_addr = dev->tuner_addr,
+                        };
+
+                        dvb->frontend->callback = tm6000_tuner_callback;
+                        ret = dvb_register_frontend(&dvb->adapter, dvb->frontend);
+                        if (ret < 0) {
+                                printk(KERN_ERR
+                                        "tm6000: couldn't register frontend\n");
+                                goto adapter_err;
+                        }
+
+                        if (!dvb_attach(xc2028_attach, dvb->frontend, &cfg)) {
+                                printk(KERN_ERR "tm6000: couldn't register "
+                                                "frontend (xc3028)\n");
+                                ret = -EINVAL;
+                                goto frontend_err;
+                        }
+                        printk(KERN_INFO "tm6000: XC2028/3028 asked to be "
+                                         "attached to frontend!\n");
+                        break;
+                        }
+                case TUNER_XC5000: {
+                        struct xc5000_config cfg = {
+                                .i2c_address = dev->tuner_addr,
+                        };
+
+                        dvb->frontend->callback = tm6000_xc5000_callback;
+                        ret = dvb_register_frontend(&dvb->adapter, dvb->frontend);
+                        if (ret < 0) {
+                                printk(KERN_ERR
+                                        "tm6000: couldn't register frontend\n");
+                                goto adapter_err;
+                        }
+
+                        if (!dvb_attach(xc5000_attach, dvb->frontend, &dev->i2c_adap, &cfg)) {
+                                printk(KERN_ERR "tm6000: couldn't register "
+                                                "frontend (xc5000)\n");
+                                ret = -EINVAL;
+                                goto frontend_err;
+                        }
+                        printk(KERN_INFO "tm6000: XC5000 asked to be "
+                                         "attached to frontend!\n");
+                        break;
+                        }
                 }
-                printk(KERN_INFO "tm6000: XC2028/3028 asked to be "
-                                 "attached to frontend!\n");
         } else {
                 printk(KERN_ERR "tm6000: no frontend found\n");
         }
diff --git a/drivers/staging/tm6000/tm6000-video.c b/drivers/staging/tm6000/tm6000-video.c
index f2b7fe4a3581..56fa371e08c8 100644
--- a/drivers/staging/tm6000/tm6000-video.c
+++ b/drivers/staging/tm6000/tm6000-video.c
@@ -48,7 +48,7 @@
 #define TM6000_MIN_BUF 4
 #define TM6000_DEF_BUF 8
 
-#define TM6000_MAX_ISO_PACKETS  40      /* Max number of ISO packets */
+#define TM6000_MAX_ISO_PACKETS  46      /* Max number of ISO packets */
 
 /* Declare static vars that will be used as parameters */
 static unsigned int vid_limit = 16;     /* Video memory limit, in Mb */
@@ -205,7 +205,11 @@ static int copy_packet(struct urb *urb, u32 header, u8 **ptr, u8 *endp,
                 c = (header >> 24) & 0xff;
 
                 /* split the header fields */
-                size  = (((header & 0x7e) << 1) -1) *4;
+                size  = ((header & 0x7e) << 1);
+
+                if (size > 0)
+                        size -= 4;
+
                 block = (header >> 7) & 0xf;
                 field = (header >> 11) & 0x1;
                 line  = (header >> 12) & 0x1ff;
@@ -307,10 +311,12 @@ static int copy_packet(struct urb *urb, u32 header, u8 **ptr, u8 *endp,
                 case TM6000_URB_MSG_PTS:
                         break;
                 case TM6000_URB_MSG_AUDIO:
-/* Need some code to process audio */
-printk ("%ld: cmd=%s, size=%d\n", jiffies,
+                        /* Need some code to process audio */
+                        printk ("%ld: cmd=%s, size=%d\n", jiffies,
                                 tm6000_msg_type[cmd],size);
                         break;
+                case TM6000_URB_MSG_VBI:
+                        break;
                 default:
                         dprintk (dev, V4L2_DEBUG_ISOC, "cmd=%s, size=%d\n",
                                                 tm6000_msg_type[cmd],size);
@@ -333,14 +339,23 @@ printk ("%ld: cmd=%s, size=%d\n", jiffies,
         return rc;
 }
 
-static int copy_streams(u8 *data, u8 *out_p, unsigned long len,
-                        struct urb *urb, struct tm6000_buffer **buf)
+static int copy_streams(u8 *data, unsigned long len,
+                        struct urb *urb)
 {
         struct tm6000_dmaqueue  *dma_q = urb->context;
         struct tm6000_core *dev= container_of(dma_q,struct tm6000_core,vidq);
         u8 *ptr=data, *endp=data+len;
         unsigned long header=0;
         int rc=0;
+        struct tm6000_buffer *buf;
+        char *outp = NULL;
+
+        get_next_buf(dma_q, &buf);
+        if (buf)
+                outp = videobuf_to_vmalloc(&buf->vb);
+
+        if (!outp)
+                return 0;
 
         for (ptr=data; ptr<endp;) {
                 if (!dev->isoc_ctl.cmd) {
@@ -388,14 +403,14 @@ static int copy_streams(u8 *data, u8 *out_p, unsigned long len,
                 }
 HEADER:
                 /* Copy or continue last copy */
-                rc=copy_packet(urb,header,&ptr,endp,out_p,buf);
+                rc=copy_packet(urb,header,&ptr,endp,outp,&buf);
                 if (rc<0) {
                         buf=NULL;
                         printk(KERN_ERR "tm6000: buffer underrun at %ld\n",
                                         jiffies);
                         return rc;
                 }
-                if (!*buf)
+                if (!buf)
                         return 0;
         }
 
@@ -404,31 +419,40 @@ HEADER:
 /*
  * Identify the tm5600/6000 buffer header type and properly handles
  */
-static int copy_multiplexed(u8 *ptr, u8 *out_p, unsigned long len,
-                        struct urb *urb, struct tm6000_buffer **buf)
+static int copy_multiplexed(u8 *ptr, unsigned long len,
+                        struct urb *urb)
 {
         struct tm6000_dmaqueue  *dma_q = urb->context;
         struct tm6000_core *dev= container_of(dma_q,struct tm6000_core,vidq);
         unsigned int pos=dev->isoc_ctl.pos,cpysize;
         int rc=1;
+        struct tm6000_buffer *buf;
+        char *outp = NULL;
+
+        get_next_buf(dma_q, &buf);
+        if (buf)
+                outp = videobuf_to_vmalloc(&buf->vb);
+
+        if (!outp)
+                return 0;
 
         while (len>0) {
-                cpysize=min(len,(*buf)->vb.size-pos);
-//printk("Copying %d bytes (max=%lu) from %p to %p[%u]\n",cpysize,(*buf)->vb.size,ptr,out_p,pos);
-                memcpy(&out_p[pos], ptr, cpysize);
+                cpysize=min(len,buf->vb.size-pos);
+                //printk("Copying %d bytes (max=%lu) from %p to %p[%u]\n",cpysize,(*buf)->vb.size,ptr,out_p,pos);
+                memcpy(&outp[pos], ptr, cpysize);
                 pos+=cpysize;
                 ptr+=cpysize;
                 len-=cpysize;
-                if (pos >= (*buf)->vb.size) {
+                if (pos >= buf->vb.size) {
                         pos=0;
                         /* Announces that a new buffer were filled */
-                        buffer_filled (dev, dma_q, *buf);
+                        buffer_filled (dev, dma_q, buf);
                         dprintk(dev, V4L2_DEBUG_ISOC, "new buffer filled\n");
-                        get_next_buf (dma_q, buf);
-                        if (!*buf)
+                        get_next_buf (dma_q, &buf);
+                        if (!buf)
                                 break;
-                        out_p = videobuf_to_vmalloc(&((*buf)->vb));
-                        if (!out_p)
+                        outp = videobuf_to_vmalloc(&(buf->vb));
+                        if (!outp)
                                 return rc;
                         pos = 0;
                 }
@@ -487,52 +511,36 @@ static inline int tm6000_isoc_copy(struct urb *urb)
         struct tm6000_dmaqueue  *dma_q = urb->context;
         struct tm6000_core *dev= container_of(dma_q,struct tm6000_core,vidq);
         struct tm6000_buffer *buf;
-        int i, len=0, rc=1;
-        int size;
-        char *outp = NULL, *p;
-        unsigned long copied;
-
-        get_next_buf(dma_q, &buf);
-        if (buf)
-                outp = videobuf_to_vmalloc(&buf->vb);
+        int i, len=0, rc=1, status;
+        char *p;
 
-        if (!outp)
-                return 0;
-
-        size = buf->vb.size;
-
-        copied=0;
-
-        if (urb->status<0) {
-                print_err_status (dev,-1,urb->status);
+        if (urb->status < 0) {
+                print_err_status (dev, -1, urb->status);
                 return 0;
         }
 
         for (i = 0; i < urb->number_of_packets; i++) {
-                int status = urb->iso_frame_desc[i].status;
+                status = urb->iso_frame_desc[i].status;
 
                 if (status<0) {
                         print_err_status (dev,i,status);
                         continue;
                 }
 
-                len=urb->iso_frame_desc[i].actual_length;
+                len = urb->iso_frame_desc[i].actual_length;
 
-//              if (len>=TM6000_URB_MSG_LEN) {
-                        p=urb->transfer_buffer + urb->iso_frame_desc[i].offset;
+                if (len > 0) {
+                        p = urb->transfer_buffer + urb->iso_frame_desc[i].offset;
                         if (!urb->iso_frame_desc[i].status) {
-                                if ((buf->fmt->fourcc)==V4L2_PIX_FMT_TM6000) {
-                                        rc=copy_multiplexed(p, outp, len, urb, &buf);
+                                if ((dev->fourcc)==V4L2_PIX_FMT_TM6000) {
+                                        rc=copy_multiplexed(p, len, urb);
                                         if (rc<=0)
                                                 return rc;
                                 } else {
-                                        copy_streams(p, outp, len, urb, &buf);
+                                        copy_streams(p, len, urb);
                                 }
                         }
-                        copied += len;
-                        if (copied >= size || !buf)
-                                break;
-//              }
+                }
         }
         return rc;
 }
@@ -612,7 +620,7 @@ static void tm6000_uninit_isoc(struct tm6000_core *dev)
 static int tm6000_prepare_isoc(struct tm6000_core *dev, unsigned int framesize)
 {
         struct tm6000_dmaqueue *dma_q = &dev->vidq;
-        int i, j, sb_size, pipe, size, max_packets, num_bufs = 5;
+        int i, j, sb_size, pipe, size, max_packets, num_bufs = 8;
         struct urb *urb;
 
         /* De-allocates all pending stuff */
diff --git a/drivers/staging/tm6000/tm6000.h b/drivers/staging/tm6000/tm6000.h
index 6812d6867d57..7bbaf26dea14 100644
--- a/drivers/staging/tm6000/tm6000.h
+++ b/drivers/staging/tm6000/tm6000.h
@@ -168,6 +168,10 @@ struct tm6000_core {
         struct i2c_adapter              i2c_adap;
         struct i2c_client               i2c_client;
 
+
+        /* extension */
+        struct list_head                devlist;
+
         /* video for linux */
         int                             users;
 
@@ -203,6 +207,16 @@ struct tm6000_core {
         spinlock_t                   slock;
 };
 
+#define TM6000_AUDIO 0x10
+
+struct tm6000_ops {
+        struct list_head        next;
+        char                    *name;
+        int                     id;
+        int (*init)(struct tm6000_core *);
+        int (*fini)(struct tm6000_core *);
+};
+
 struct tm6000_fh {
         struct tm6000_core           *dev;
 
@@ -232,6 +246,8 @@ int tm6000_get_reg (struct tm6000_core *dev, u8 req, u16 value, u16 index);
 int tm6000_get_reg16(struct tm6000_core *dev, u8 req, u16 value, u16 index);
 int tm6000_get_reg32(struct tm6000_core *dev, u8 req, u16 value, u16 index);
 int tm6000_set_reg (struct tm6000_core *dev, u8 req, u16 value, u16 index);
+int tm6000_i2c_reset(struct tm6000_core *dev, u16 tsleep);
+
 int tm6000_init (struct tm6000_core *dev);
 
 int tm6000_init_analog_mode (struct tm6000_core *dev);
@@ -246,6 +262,13 @@ int tm6000_v4l2_unregister(struct tm6000_core *dev);
 int tm6000_v4l2_exit(void);
 void tm6000_set_fourcc_format(struct tm6000_core *dev);
 
+void tm6000_remove_from_devlist(struct tm6000_core *dev);
+void tm6000_add_into_devlist(struct tm6000_core *dev);
+int tm6000_register_extension(struct tm6000_ops *ops);
+void tm6000_unregister_extension(struct tm6000_ops *ops);
+void tm6000_init_extension(struct tm6000_core *dev);
+void tm6000_close_extension(struct tm6000_core *dev);
+
 /* In tm6000-stds.c */
 void tm6000_get_std_res(struct tm6000_core *dev);
 int tm6000_set_standard (struct tm6000_core *dev, v4l2_std_id *norm);
@@ -275,7 +298,7 @@ unsigned int tm6000_v4l2_poll(struct file *file,
 int tm6000_queue_init(struct tm6000_core *dev);
 
 /* In tm6000-alsa.c */
-int tm6000_audio_init(struct tm6000_core *dev, int idx);
+/*int tm6000_audio_init(struct tm6000_core *dev, int idx);*/
 
 
 /* Debug stuff */
diff --git a/drivers/staging/vme/bridges/vme_ca91cx42.c b/drivers/staging/vme/bridges/vme_ca91cx42.c
index 0c82eb47a28d..0f9ea58ff717 100644
--- a/drivers/staging/vme/bridges/vme_ca91cx42.c
+++ b/drivers/staging/vme/bridges/vme_ca91cx42.c
@@ -523,7 +523,7 @@ static int ca91cx42_alloc_resource(struct vme_master_resource *image,
         }
 
         if (image->bus_resource.name == NULL) {
-                image->bus_resource.name = kmalloc(VMENAMSIZ+3, GFP_KERNEL);
+                image->bus_resource.name = kmalloc(VMENAMSIZ+3, GFP_ATOMIC);
                 if (image->bus_resource.name == NULL) {
                         dev_err(ca91cx42_bridge->parent, "Unable to allocate "
                                 "memory for resource name\n");
diff --git a/drivers/staging/vme/bridges/vme_tsi148.c b/drivers/staging/vme/bridges/vme_tsi148.c
index abe88a380b72..f09cac163139 100644
--- a/drivers/staging/vme/bridges/vme_tsi148.c
+++ b/drivers/staging/vme/bridges/vme_tsi148.c
@@ -828,7 +828,7 @@ static int tsi148_alloc_resource(struct vme_master_resource *image,
                 return 0;
 
         if (image->bus_resource.name == NULL) {
-                image->bus_resource.name = kmalloc(VMENAMSIZ+3, GFP_KERNEL);
+                image->bus_resource.name = kmalloc(VMENAMSIZ+3, GFP_ATOMIC);
                 if (image->bus_resource.name == NULL) {
                         dev_err(tsi148_bridge->parent, "Unable to allocate "
                                 "memory for resource name\n");
diff --git a/drivers/staging/wlags49_h2/Kconfig b/drivers/staging/wlags49_h2/Kconfig
index b6fc2ca7d85c..3efcbf8afedf 100644
--- a/drivers/staging/wlags49_h2/Kconfig
+++ b/drivers/staging/wlags49_h2/Kconfig
@@ -1,9 +1,11 @@
 config WLAGS49_H2
         tristate "Agere Systems HERMES II Wireless PC Card Model 0110"
-        depends on WLAN && WIRELESS_EXT && PCMCIA
+        depends on WLAN && PCMCIA
+        select WIRELESS_EXT
         select WEXT_SPY
+        select WEXT_PRIV
         ---help---
-          Driver for wireless cards using Agere's HERMES II chipset
-          which are identified with Manufacture ID: 0156,0003
-          The software is a modified version of wl_lkm_722_abg.tar.gz
-          from the Agere Systems website, addapted for Ubuntu 9.04.
+        Driver for wireless cards using Agere's HERMES II chipset
+        which are identified with Manufacture ID: 0156,0003
+        The software is a modified version of wl_lkm_722_abg.tar.gz
+        from the Agere Systems website, addapted for Ubuntu 9.04.
diff --git a/drivers/staging/wlags49_h25/Kconfig b/drivers/staging/wlags49_h25/Kconfig
index dcc170929c13..bf5664a51cd4 100644
--- a/drivers/staging/wlags49_h25/Kconfig
+++ b/drivers/staging/wlags49_h25/Kconfig
@@ -1,9 +1,11 @@
 config WLAGS49_H25
         tristate "Linksys HERMES II.5 WCF54G_Wireless-G_CompactFlash_Card"
-        depends on WLAN && WIRELESS_EXT && PCMCIA
+        depends on WLAN && PCMCIA
+        select WIRELESS_EXT
         select WEXT_SPY
+        select WEXT_PRIV
         ---help---
-          Driver for wireless cards using Agere's HERMES II.5 chipset
-          which are identified with Manufacture ID: 0156,0004
-          The software is a modified version of wl_lkm_722_abg.tar.gz
-          from the Agere Systems website, addapted for Ubuntu 9.04.
+        Driver for wireless cards using Agere's HERMES II.5 chipset
+        which are identified with Manufacture ID: 0156,0004
+        The software is a modified version of wl_lkm_722_abg.tar.gz
+        from the Agere Systems website, addapted for Ubuntu 9.04.
diff --git a/drivers/staging/xgifb/Kconfig b/drivers/staging/xgifb/Kconfig
new file mode 100644
index 000000000000..bb0ca5974ea0
--- /dev/null
+++ b/drivers/staging/xgifb/Kconfig
@@ -0,0 +1,11 @@
+config FB_XGI
+        tristate "XGI display support"
+        depends on FB && PCI
+        select FB_CFB_FILLRECT
+        select FB_CFB_COPYAREA
+        select FB_CFB_IMAGEBLIT
+        help
+          This driver supports notebooks with XGI Z7,Z9,Z11 PCI chips.
+          Say Y if you have such a graphics card.
+          To compile this driver as a module, choose M here: the
+          module will be called xgifb.ko
diff --git a/drivers/staging/xgifb/Makefile b/drivers/staging/xgifb/Makefile
new file mode 100644
index 000000000000..2a317707de0f
--- /dev/null
+++ b/drivers/staging/xgifb/Makefile
@@ -0,0 +1,4 @@
+obj-$(CONFIG_FB_XGI)  += xgifb.o
+
+xgifb-objs := XGI_main_26.o XGI_accel.o vb_init.o vb_setmode.o vb_util.o vb_ext.o
+
diff --git a/drivers/staging/xgifb/TODO b/drivers/staging/xgifb/TODO
new file mode 100644
index 000000000000..7d71019b84c2
--- /dev/null
+++ b/drivers/staging/xgifb/TODO
@@ -0,0 +1,15 @@
+This drivers still need a lot of work. I can list all cleanups to do but it's
+going to be long. So, I'm writing "cleanups" and not the list.
+
+Arnaud
+
+TODO:
+- clean ups
+- fix build warnings when module
+- sort out dup ids with SiS driver
+- remove useless/wrong/unused #ifdef/code/...
+- fix printk usages
+- get rid of non-linux related stuff
+
+Please send patches to:
+Arnaud Patard <apatard@mandriva.com>
diff --git a/drivers/staging/xgifb/XGI.h b/drivers/staging/xgifb/XGI.h
new file mode 100644
index 000000000000..87803dd032de
--- /dev/null
+++ b/drivers/staging/xgifb/XGI.h
@@ -0,0 +1,10 @@
+#ifndef _XGI_H
+#define _XGI_H
+
+#if 1
+#define TWDEBUG(x)
+#else
+#define TWDEBUG(x) printk(KERN_INFO x "\n");
+#endif
+
+#endif
diff --git a/drivers/staging/xgifb/XGI_accel.c b/drivers/staging/xgifb/XGI_accel.c
new file mode 100644
index 000000000000..86ec3421942f
--- /dev/null
+++ b/drivers/staging/xgifb/XGI_accel.c
@@ -0,0 +1,596 @@
+/*
+ * XGI 300/630/730/540/315/550/650/740 frame buffer driver
+ * for Linux kernels 2.4.x and 2.5.x
+ *
+ * 2D acceleration part
+ *
+ * Based on the X driver's XGI300_accel.c which is
+ *     Copyright Xavier Ducoin <x.ducoin@lectra.com>
+ *     Copyright 2002 by Thomas Winischhofer, Vienna, Austria
+ * and XGI310_accel.c which is
+ *     Copyright 2002 by Thomas Winischhofer, Vienna, Austria
+ *
+ * Author: Thomas Winischhofer <thomas@winischhofer.net>
+ *                      (see http://www.winischhofer.net/
+ *                      for more information and updates)
+ */
+
+//#include <linux/config.h>
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/tty.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/console.h>
+#include <linux/selection.h>
+#include <linux/ioport.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/vt_kern.h>
+#include <linux/capability.h>
+#include <linux/fs.h>
+#include <linux/agp_backend.h>
+
+#include <linux/types.h>
+/*
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+#include <linux/XGIfb.h>
+#else
+#include <video/XGIfb.h>
+#endif
+*/
+#include <asm/io.h>
+
+#ifdef CONFIG_MTRR
+#include <asm/mtrr.h>
+#endif
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+#include <video/fbcon.h>
+#include <video/fbcon-cfb8.h>
+#include <video/fbcon-cfb16.h>
+#include <video/fbcon-cfb24.h>
+#include <video/fbcon-cfb32.h>
+#endif
+
+#include "osdef.h"
+#include "vgatypes.h"
+#include "vb_struct.h"
+#include "XGIfb.h"
+#include "XGI_accel.h"
+
+
+extern struct     video_info xgi_video_info;
+extern int XGIfb_accel;
+
+static const int XGIALUConv[] =
+{
+    0x00,       /* dest = 0;            0,      GXclear,        0 */
+    0x88,       /* dest &= src;         DSa,    GXand,          0x1 */
+    0x44,       /* dest = src & ~dest;  SDna,   GXandReverse,   0x2 */
+    0xCC,       /* dest = src;          S,      GXcopy,         0x3 */
+    0x22,       /* dest &= ~src;        DSna,   GXandInverted,  0x4 */
+    0xAA,       /* dest = dest;         D,      GXnoop,         0x5 */
+    0x66,       /* dest = ^src;         DSx,    GXxor,          0x6 */
+    0xEE,       /* dest |= src;         DSo,    GXor,           0x7 */
+    0x11,       /* dest = ~src & ~dest; DSon,   GXnor,          0x8 */
+    0x99,       /* dest ^= ~src ;       DSxn,   GXequiv,        0x9 */
+    0x55,       /* dest = ~dest;        Dn,     GXInvert,       0xA */
+    0xDD,       /* dest = src|~dest ;   SDno,   GXorReverse,    0xB */
+    0x33,       /* dest = ~src;         Sn,     GXcopyInverted, 0xC */
+    0xBB,       /* dest |= ~src;        DSno,   GXorInverted,   0xD */
+    0x77,       /* dest = ~src|~dest;   DSan,   GXnand,         0xE */
+    0xFF,       /* dest = 0xFF;         1,      GXset,          0xF */
+};
+/* same ROP but with Pattern as Source */
+static const int XGIPatALUConv[] =
+{
+    0x00,       /* dest = 0;            0,      GXclear,        0 */
+    0xA0,       /* dest &= src;         DPa,    GXand,          0x1 */
+    0x50,       /* dest = src & ~dest;  PDna,   GXandReverse,   0x2 */
+    0xF0,       /* dest = src;          P,      GXcopy,         0x3 */
+    0x0A,       /* dest &= ~src;        DPna,   GXandInverted,  0x4 */
+    0xAA,       /* dest = dest;         D,      GXnoop,         0x5 */
+    0x5A,       /* dest = ^src;         DPx,    GXxor,          0x6 */
+    0xFA,       /* dest |= src;         DPo,    GXor,           0x7 */
+    0x05,       /* dest = ~src & ~dest; DPon,   GXnor,          0x8 */
+    0xA5,       /* dest ^= ~src ;       DPxn,   GXequiv,        0x9 */
+    0x55,       /* dest = ~dest;        Dn,     GXInvert,       0xA */
+    0xF5,       /* dest = src|~dest ;   PDno,   GXorReverse,    0xB */
+    0x0F,       /* dest = ~src;         Pn,     GXcopyInverted, 0xC */
+    0xAF,       /* dest |= ~src;        DPno,   GXorInverted,   0xD */
+    0x5F,       /* dest = ~src|~dest;   DPan,   GXnand,         0xE */
+    0xFF,       /* dest = 0xFF;         1,      GXset,          0xF */
+};
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,34)
+static const unsigned char myrops[] = {
+        3, 10, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3
+   };
+#endif
+
+/* 300 series */
+#if 0
+static void
+XGI300Sync(void)
+{
+        XGI300Idle
+}
+#endif
+static void
+XGI310Sync(void)
+{
+        XGI310Idle
+}
+#if 0
+static void
+XGI300SetupForScreenToScreenCopy(int xdir, int ydir, int rop,
+                                unsigned int planemask, int trans_color)
+{
+        XGI300SetupDSTColorDepth(xgi_video_info.DstColor);
+        XGI300SetupSRCPitch(xgi_video_info.video_linelength)
+        XGI300SetupDSTRect(xgi_video_info.video_linelength, 0xFFF)
+
+        if(trans_color != -1) {
+                XGI300SetupROP(0x0A)
+                XGI300SetupSRCTrans(trans_color)
+                XGI300SetupCMDFlag(TRANSPARENT_BITBLT)
+        } else {
+                XGI300SetupROP(XGIALUConv[rop])
+        }
+        if(xdir > 0) {
+                XGI300SetupCMDFlag(X_INC)
+        }
+        if(ydir > 0) {
+                XGI300SetupCMDFlag(Y_INC)
+        }
+}
+
+static void
+XGI300SubsequentScreenToScreenCopy(int src_x, int src_y, int dst_x, int dst_y,
+                                int width, int height)
+{
+        long srcbase, dstbase;
+
+        srcbase = dstbase = 0;
+        if (src_y >= 2048) {
+                srcbase = xgi_video_info.video_linelength * src_y;
+                src_y = 0;
+        }
+        if (dst_y >= 2048) {
+                dstbase = xgi_video_info.video_linelength * dst_y;
+                dst_y = 0;
+        }
+
+        XGI300SetupSRCBase(srcbase);
+        XGI300SetupDSTBase(dstbase);
+
+        if(!(xgi_video_info.CommandReg & X_INC))  {
+                src_x += width-1;
+                dst_x += width-1;
+        }
+        if(!(xgi_video_info.CommandReg & Y_INC))  {
+                src_y += height-1;
+                dst_y += height-1;
+        }
+        XGI300SetupRect(width, height)
+        XGI300SetupSRCXY(src_x, src_y)
+        XGI300SetupDSTXY(dst_x, dst_y)
+        XGI300DoCMD
+}
+
+static void
+XGI300SetupForSolidFill(int color, int rop, unsigned int planemask)
+{
+        XGI300SetupPATFG(color)
+        XGI300SetupDSTRect(xgi_video_info.video_linelength, 0xFFF)
+        XGI300SetupDSTColorDepth(xgi_video_info.DstColor);
+        XGI300SetupROP(XGIPatALUConv[rop])
+        XGI300SetupCMDFlag(PATFG)
+}
+
+static void
+XGI300SubsequentSolidFillRect(int x, int y, int w, int h)
+{
+        long dstbase;
+
+        dstbase = 0;
+        if(y >= 2048) {
+                dstbase = xgi_video_info.video_linelength * y;
+                y = 0;
+        }
+        XGI300SetupDSTBase(dstbase)
+        XGI300SetupDSTXY(x,y)
+        XGI300SetupRect(w,h)
+        XGI300SetupCMDFlag(X_INC | Y_INC | BITBLT)
+        XGI300DoCMD
+}
+#endif
+/* 310/325 series ------------------------------------------------ */
+
+static void
+XGI310SetupForScreenToScreenCopy(int xdir, int ydir, int rop,
+                                unsigned int planemask, int trans_color)
+{
+        XGI310SetupDSTColorDepth(xgi_video_info.DstColor);
+        XGI310SetupSRCPitch(xgi_video_info.video_linelength)
+        XGI310SetupDSTRect(xgi_video_info.video_linelength, 0xFFF)
+        if (trans_color != -1) {
+                XGI310SetupROP(0x0A)
+                XGI310SetupSRCTrans(trans_color)
+                XGI310SetupCMDFlag(TRANSPARENT_BITBLT)
+        } else {
+                XGI310SetupROP(XGIALUConv[rop])
+                /* Set command - not needed, both 0 */
+                /* XGISetupCMDFlag(BITBLT | SRCVIDEO) */
+        }
+        XGI310SetupCMDFlag(xgi_video_info.XGI310_AccelDepth)
+        /* TW: The 310/325 series is smart enough to know the direction */
+}
+
+static void
+XGI310SubsequentScreenToScreenCopy(int src_x, int src_y, int dst_x, int dst_y,
+                                int width, int height)
+{
+        long srcbase, dstbase;
+        int mymin, mymax;
+
+        srcbase = dstbase = 0;
+        mymin = min(src_y, dst_y);
+        mymax = max(src_y, dst_y);
+
+        /* Although the chip knows the direction to use
+         * if the source and destination areas overlap,
+         * that logic fails if we fiddle with the bitmap
+         * addresses. Therefore, we check if the source
+         * and destination blitting areas overlap and
+         * adapt the bitmap addresses synchronously
+         * if the coordinates exceed the valid range.
+         * The the areas do not overlap, we do our
+         * normal check.
+         */
+        if((mymax - mymin) < height) {
+           if((src_y >= 2048) || (dst_y >= 2048)) {
+              srcbase = xgi_video_info.video_linelength * mymin;
+              dstbase = xgi_video_info.video_linelength * mymin;
+              src_y -= mymin;
+              dst_y -= mymin;
+           }
+        } else {
+           if(src_y >= 2048) {
+              srcbase = xgi_video_info.video_linelength * src_y;
+              src_y = 0;
+           }
+           if(dst_y >= 2048) {
+              dstbase = xgi_video_info.video_linelength * dst_y;
+              dst_y = 0;
+           }
+        }
+
+        XGI310SetupSRCBase(srcbase);
+        XGI310SetupDSTBase(dstbase);
+        XGI310SetupRect(width, height)
+        XGI310SetupSRCXY(src_x, src_y)
+        XGI310SetupDSTXY(dst_x, dst_y)
+        XGI310DoCMD
+}
+
+static void
+XGI310SetupForSolidFill(int color, int rop, unsigned int planemask)
+{
+        XGI310SetupPATFG(color)
+        XGI310SetupDSTRect(xgi_video_info.video_linelength, 0xFFF)
+        XGI310SetupDSTColorDepth(xgi_video_info.DstColor);
+        XGI310SetupROP(XGIPatALUConv[rop])
+        XGI310SetupCMDFlag(PATFG | xgi_video_info.XGI310_AccelDepth)
+}
+
+static void
+XGI310SubsequentSolidFillRect(int x, int y, int w, int h)
+{
+        long dstbase;
+
+        dstbase = 0;
+        if(y >= 2048) {
+                dstbase = xgi_video_info.video_linelength * y;
+                y = 0;
+        }
+        XGI310SetupDSTBase(dstbase)
+        XGI310SetupDSTXY(x,y)
+        XGI310SetupRect(w,h)
+        XGI310SetupCMDFlag(BITBLT)
+        XGI310DoCMD
+}
+
+/* --------------------------------------------------------------------- */
+
+/* The exported routines */
+
+int XGIfb_initaccel(void)
+{
+#ifdef XGIFB_USE_SPINLOCKS
+    spin_lock_init(&xgi_video_info.lockaccel);
+#endif
+    return(0);
+}
+
+void XGIfb_syncaccel(void)
+{
+
+    XGI310Sync();
+
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,34)  /* --- KERNEL 2.5.34 and later --- */
+
+int fbcon_XGI_sync(struct fb_info *info)
+{
+    if(!XGIfb_accel) return 0;
+    CRITFLAGS
+
+    XGI310Sync();
+
+   CRITEND
+   return 0;
+}
+
+void fbcon_XGI_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
+{
+   int col=0;
+   CRITFLAGS
+
+
+   if(!rect->width || !rect->height)
+        return;
+
+   if(!XGIfb_accel) {
+        cfb_fillrect(info, rect);
+        return;
+   }
+
+   switch(info->var.bits_per_pixel) {
+                case 8: col = rect->color;
+                        break;
+                case 16: col = ((u32 *)(info->pseudo_palette))[rect->color];
+                         break;
+                case 32: col = ((u32 *)(info->pseudo_palette))[rect->color];
+                         break;
+        }
+
+
+           CRITBEGIN
+           XGI310SetupForSolidFill(col, myrops[rect->rop], 0);
+           XGI310SubsequentSolidFillRect(rect->dx, rect->dy, rect->width, rect->height);
+           CRITEND
+           XGI310Sync();
+
+
+}
+
+void fbcon_XGI_copyarea(struct fb_info *info, const struct fb_copyarea *area)
+{
+   int xdir, ydir;
+   CRITFLAGS
+
+
+   if(!XGIfb_accel) {
+        cfb_copyarea(info, area);
+        return;
+   }
+
+   if(!area->width || !area->height)
+        return;
+
+   if(area->sx < area->dx) xdir = 0;
+   else                    xdir = 1;
+   if(area->sy < area->dy) ydir = 0;
+   else                    ydir = 1;
+
+      CRITBEGIN
+      XGI310SetupForScreenToScreenCopy(xdir, ydir, 3, 0, -1);
+      XGI310SubsequentScreenToScreenCopy(area->sx, area->sy, area->dx, area->dy, area->width, area->height);
+      CRITEND
+      XGI310Sync();
+
+}
+
+#endif
+
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,5,33)  /* ------ KERNEL <2.5.34 ------ */
+
+void fbcon_XGI_bmove(struct display *p, int srcy, int srcx,
+                            int dsty, int dstx, int height, int width)
+{
+        int xdir, ydir;
+        CRITFLAGS
+
+        if(!xgi_video_info.accel) {
+            switch(xgi_video_info.video_bpp) {
+            case 8:
+#ifdef FBCON_HAS_CFB8
+               fbcon_cfb8_bmove(p, srcy, srcx, dsty, dstx, height, width);
+#endif
+               break;
+            case 16:
+#ifdef FBCON_HAS_CFB16
+               fbcon_cfb16_bmove(p, srcy, srcx, dsty, dstx, height, width);
+#endif
+               break;
+            case 32:
+#ifdef FBCON_HAS_CFB32
+               fbcon_cfb32_bmove(p, srcy, srcx, dsty, dstx, height, width);
+#endif
+               break;
+            }
+            return;
+        }
+
+        srcx *= fontwidth(p);
+        srcy *= fontheight(p);
+        dstx *= fontwidth(p);
+        dsty *= fontheight(p);
+        width *= fontwidth(p);
+        height *= fontheight(p);
+
+        if(srcx < dstx) xdir = 0;
+        else            xdir = 1;
+        if(srcy < dsty) ydir = 0;
+        else            ydir = 1;
+
+
+           CRITBEGIN
+           XGI310SetupForScreenToScreenCopy(xdir, ydir, 3, 0, -1);
+           XGI310SubsequentScreenToScreenCopy(srcx, srcy, dstx, dsty, width, height);
+           CRITEND
+           XGI310Sync();
+#if 0
+           printk(KERN_INFO "XGI_bmove sx %d sy %d dx %d dy %d w %d h %d\n",
+                srcx, srcy, dstx, dsty, width, height);
+#endif
+
+}
+
+
+static void fbcon_XGI_clear(struct vc_data *conp, struct display *p,
+                        int srcy, int srcx, int height, int width, int color)
+{
+        CRITFLAGS
+
+        srcx *= fontwidth(p);
+        srcy *= fontheight(p);
+        width *= fontwidth(p);
+        height *= fontheight(p);
+
+
+           CRITBEGIN
+           XGI310SetupForSolidFill(color, 3, 0);
+           XGI310SubsequentSolidFillRect(srcx, srcy, width, height);
+           CRITEND
+           XGI310Sync();
+
+}
+
+void fbcon_XGI_clear8(struct vc_data *conp, struct display *p,
+                        int srcy, int srcx, int height, int width)
+{
+        u32 bgx;
+
+        if(!xgi_video_info.accel) {
+#ifdef FBCON_HAS_CFB8
+            fbcon_cfb8_clear(conp, p, srcy, srcx, height, width);
+#endif
+            return;
+        }
+
+        bgx = attr_bgcol_ec(p, conp);
+        fbcon_XGI_clear(conp, p, srcy, srcx, height, width, bgx);
+}
+
+void fbcon_XGI_clear16(struct vc_data *conp, struct display *p,
+                        int srcy, int srcx, int height, int width)
+{
+        u32 bgx;
+        if(!xgi_video_info.accel) {
+#ifdef FBCON_HAS_CFB16
+            fbcon_cfb16_clear(conp, p, srcy, srcx, height, width);
+#endif
+            return;
+        }
+
+        bgx = ((u_int16_t*)p->dispsw_data)[attr_bgcol_ec(p, conp)];
+        fbcon_XGI_clear(conp, p, srcy, srcx, height, width, bgx);
+}
+
+void fbcon_XGI_clear32(struct vc_data *conp, struct display *p,
+                        int srcy, int srcx, int height, int width)
+{
+        u32 bgx;
+
+        if(!xgi_video_info.accel) {
+#ifdef FBCON_HAS_CFB32
+            fbcon_cfb32_clear(conp, p, srcy, srcx, height, width);
+#endif
+            return;
+        }
+
+        bgx = ((u_int32_t*)p->dispsw_data)[attr_bgcol_ec(p, conp)];
+        fbcon_XGI_clear(conp, p, srcy, srcx, height, width, bgx);
+}
+
+void fbcon_XGI_revc(struct display *p, int srcx, int srcy)
+{
+        CRITFLAGS
+
+        if(!xgi_video_info.accel) {
+            switch(xgi_video_info.video_bpp) {
+            case 16:
+#ifdef FBCON_HAS_CFB16
+               fbcon_cfb16_revc(p, srcx, srcy);
+#endif
+               break;
+            case 32:
+#ifdef FBCON_HAS_CFB32
+               fbcon_cfb32_revc(p, srcx, srcy);
+#endif
+               break;
+            }
+            return;
+        }
+
+        srcx *= fontwidth(p);
+        srcy *= fontheight(p);
+
+
+           CRITBEGIN
+           XGI310SetupForSolidFill(0, 0x0a, 0);
+           XGI310SubsequentSolidFillRect(srcx, srcy, fontwidth(p), fontheight(p));
+           CRITEND
+           XGI310Sync();
+
+}
+
+#ifdef FBCON_HAS_CFB8
+struct display_switch fbcon_XGI8 = {
+        setup:                  fbcon_cfb8_setup,
+        bmove:                  fbcon_XGI_bmove,
+        clear:                  fbcon_XGI_clear8,
+        putc:                   fbcon_cfb8_putc,
+        putcs:                  fbcon_cfb8_putcs,
+        revc:                   fbcon_cfb8_revc,
+        clear_margins:          fbcon_cfb8_clear_margins,
+        fontwidthmask:          FONTWIDTH(4)|FONTWIDTH(8)|FONTWIDTH(12)|FONTWIDTH(16)
+};
+#endif
+#ifdef FBCON_HAS_CFB16
+struct display_switch fbcon_XGI16 = {
+        setup:                  fbcon_cfb16_setup,
+        bmove:                  fbcon_XGI_bmove,
+        clear:                  fbcon_XGI_clear16,
+        putc:                   fbcon_cfb16_putc,
+        putcs:                  fbcon_cfb16_putcs,
+        revc:                   fbcon_XGI_revc,
+        clear_margins:          fbcon_cfb16_clear_margins,
+        fontwidthmask:          FONTWIDTH(4)|FONTWIDTH(8)|FONTWIDTH(12)|FONTWIDTH(16)
+};
+#endif
+#ifdef FBCON_HAS_CFB32
+struct display_switch fbcon_XGI32 = {
+        setup:                  fbcon_cfb32_setup,
+        bmove:                  fbcon_XGI_bmove,
+        clear:                  fbcon_XGI_clear32,
+        putc:                   fbcon_cfb32_putc,
+        putcs:                  fbcon_cfb32_putcs,
+        revc:                   fbcon_XGI_revc,
+        clear_margins:          fbcon_cfb32_clear_margins,
+        fontwidthmask:          FONTWIDTH(4)|FONTWIDTH(8)|FONTWIDTH(12)|FONTWIDTH(16)
+};
+#endif
+
+#endif /* KERNEL VERSION */
+
+
diff --git a/drivers/staging/xgifb/XGI_accel.h b/drivers/staging/xgifb/XGI_accel.h
new file mode 100644
index 000000000000..04e126772bb8
--- /dev/null
+++ b/drivers/staging/xgifb/XGI_accel.h
@@ -0,0 +1,511 @@
+/*
+ * XGI 300/630/730/540/315/550/650/740 frame buffer driver
+ * for Linux kernels 2.4.x and 2.5.x
+ *
+ * 2D acceleration part
+ *
+ * Based on the X driver's XGI300_accel.h which is
+ *     Copyright Xavier Ducoin <x.ducoin@lectra.com>
+ *     Copyright 2002 by Thomas Winischhofer, Vienna, Austria
+ * and XGI310_accel.h which is
+ *     Copyright 2002 by Thomas Winischhofer, Vienna, Austria
+ *
+ * Author:   Thomas Winischhofer <thomas@winischhofer.net>:
+ *                      (see http://www.winischhofer.net/
+ *                      for more information and updates)
+ */
+
+#ifndef _XGIFB_ACCEL_H
+#define _XGIFB_ACCEL_H
+
+/* Guard accelerator accesses with spin_lock_irqsave? Works well without. */
+#undef XGIFB_USE_SPINLOCKS
+
+#ifdef XGIFB_USE_SPINLOCKS
+#include <linux/spinlock.h>
+#define CRITBEGIN  spin_lock_irqsave(&xgi_video_info.lockaccel), critflags);
+#define CRITEND    spin_unlock_irqrestore(&xgi_video_info.lockaccel), critflags);
+#define CRITFLAGS  unsigned long critflags;
+#else
+#define CRITBEGIN
+#define CRITEND
+#define CRITFLAGS
+#endif
+
+/* Definitions for the XGI engine communication. */
+
+#define PATREGSIZE      384  /* Pattern register size. 384 bytes @ 0x8300 */
+#define BR(x)   (0x8200 | (x) << 2)
+#define PBR(x)  (0x8300 | (x) << 2)
+
+/* XGI300 engine commands */
+#define BITBLT                  0x00000000  /* Blit */
+#define COLOREXP                0x00000001  /* Color expand */
+#define ENCOLOREXP              0x00000002  /* Enhanced color expand */
+#define MULTIPLE_SCANLINE       0x00000003  /* ? */
+#define LINE                    0x00000004  /* Draw line */
+#define TRAPAZOID_FILL          0x00000005  /* Fill trapezoid */
+#define TRANSPARENT_BITBLT      0x00000006  /* Transparent Blit */
+
+/* Additional engine commands for 310/325 */
+#define ALPHA_BLEND             0x00000007  /* Alpha blend ? */
+#define A3D_FUNCTION            0x00000008  /* 3D command ? */
+#define CLEAR_Z_BUFFER          0x00000009  /* ? */
+#define GRADIENT_FILL           0x0000000A  /* Gradient fill */
+#define STRETCH_BITBLT          0x0000000B  /* Stretched Blit */
+
+/* source select */
+#define SRCVIDEO                0x00000000  /* source is video RAM */
+#define SRCSYSTEM               0x00000010  /* source is system memory */
+#define SRCCPUBLITBUF           SRCSYSTEM   /* source is CPU-driven BitBuffer (for color expand) */
+#define SRCAGP                  0x00000020  /* source is AGP memory (?) */
+
+/* Pattern flags */
+#define PATFG                   0x00000000  /* foreground color */
+#define PATPATREG               0x00000040  /* pattern in pattern buffer (0x8300) */
+#define PATMONO                 0x00000080  /* mono pattern */
+
+/* blitting direction (300 series only) */
+#define X_INC                   0x00010000
+#define X_DEC                   0x00000000
+#define Y_INC                   0x00020000
+#define Y_DEC                   0x00000000
+
+/* Clipping flags */
+#define NOCLIP                  0x00000000
+#define NOMERGECLIP             0x04000000
+#define CLIPENABLE              0x00040000
+#define CLIPWITHOUTMERGE        0x04040000
+
+/* Transparency */
+#define OPAQUE                  0x00000000
+#define TRANSPARENT             0x00100000
+
+/* ? */
+#define DSTAGP                  0x02000000
+#define DSTVIDEO                0x02000000
+
+/* Line */
+#define LINE_STYLE              0x00800000
+#define NO_RESET_COUNTER        0x00400000
+#define NO_LAST_PIXEL           0x00200000
+
+/* Subfunctions for Color/Enhanced Color Expansion (310/325 only) */
+#define COLOR_TO_MONO           0x00100000
+#define AA_TEXT                 0x00200000
+
+/* Some general registers for 310/325 series */
+#define SRC_ADDR                0x8200
+#define SRC_PITCH               0x8204
+#define AGP_BASE                0x8206 /* color-depth dependent value */
+#define SRC_Y                   0x8208
+#define SRC_X                   0x820A
+#define DST_Y                   0x820C
+#define DST_X                   0x820E
+#define DST_ADDR                0x8210
+#define DST_PITCH               0x8214
+#define DST_HEIGHT              0x8216
+#define RECT_WIDTH              0x8218
+#define RECT_HEIGHT             0x821A
+#define PAT_FGCOLOR             0x821C
+#define PAT_BGCOLOR             0x8220
+#define SRC_FGCOLOR             0x8224
+#define SRC_BGCOLOR             0x8228
+#define MONO_MASK               0x822C
+#define LEFT_CLIP               0x8234
+#define TOP_CLIP                0x8236
+#define RIGHT_CLIP              0x8238
+#define BOTTOM_CLIP             0x823A
+#define COMMAND_READY           0x823C
+#define FIRE_TRIGGER            0x8240
+
+#define PATTERN_REG             0x8300  /* 384 bytes pattern buffer */
+
+/* Line registers */
+#define LINE_X0                 SRC_Y
+#define LINE_X1                 DST_Y
+#define LINE_Y0                 SRC_X
+#define LINE_Y1                 DST_X
+#define LINE_COUNT              RECT_WIDTH
+#define LINE_STYLE_PERIOD       RECT_HEIGHT
+#define LINE_STYLE_0            MONO_MASK
+#define LINE_STYLE_1            0x8230
+#define LINE_XN                 PATTERN_REG
+#define LINE_YN                 PATTERN_REG+2
+
+/* Transparent bitblit registers */
+#define TRANS_DST_KEY_HIGH      PAT_FGCOLOR
+#define TRANS_DST_KEY_LOW       PAT_BGCOLOR
+#define TRANS_SRC_KEY_HIGH      SRC_FGCOLOR
+#define TRANS_SRC_KEY_LOW       SRC_BGCOLOR
+
+/* Queue */
+#define Q_BASE_ADDR             0x85C0  /* Base address of software queue (?) */
+#define Q_WRITE_PTR             0x85C4  /* Current write pointer (?) */
+#define Q_READ_PTR              0x85C8  /* Current read pointer (?) */
+#define Q_STATUS                0x85CC  /* queue status */
+
+
+#define MMIO_IN8(base, offset) \
+        *(volatile u8 *)(((u8*)(base)) + (offset))
+#define MMIO_IN16(base, offset) \
+        *(volatile u16 *)(void *)(((u8*)(base)) + (offset))
+#define MMIO_IN32(base, offset) \
+        *(volatile u32 *)(void *)(((u8*)(base)) + (offset))
+#define MMIO_OUT8(base, offset, val) \
+        *(volatile u8 *)(((u8*)(base)) + (offset)) = (val)
+#define MMIO_OUT16(base, offset, val) \
+        *(volatile u16 *)(void *)(((u8*)(base)) + (offset)) = (val)
+#define MMIO_OUT32(base, offset, val) \
+        *(volatile u32 *)(void *)(((u8*)(base)) + (offset)) = (val)
+
+
+
+/* ------------- XGI 300 series -------------- */
+
+/* Macros to do useful things with the XGI BitBLT engine */
+
+/* BR(16) (0x8420):
+
+   bit 31 2D engine: 1 is idle,
+   bit 30 3D engine: 1 is idle,
+   bit 29 Command queue: 1 is empty
+
+   bits 28:24: Current CPU driven BitBlt buffer stage bit[4:0]
+
+   bits 15:0:  Current command queue length
+
+*/
+
+/* TW: BR(16)+2 = 0x8242 */
+
+int     xgiCmdQueLen;
+
+#define XGI300Idle \
+  { \
+  while( (MMIO_IN16(xgi_video_info.mmio_vbase, BR(16)+2) & 0xE000) != 0xE000){}; \
+  while( (MMIO_IN16(xgi_video_info.mmio_vbase, BR(16)+2) & 0xE000) != 0xE000){}; \
+  while( (MMIO_IN16(xgi_video_info.mmio_vbase, BR(16)+2) & 0xE000) != 0xE000){}; \
+  xgiCmdQueLen=MMIO_IN16(xgi_video_info.mmio_vbase, 0x8240); \
+  }
+/* TW: (do three times, because 2D engine seems quite unsure about whether or not it's idle) */
+
+#define XGI300SetupSRCBase(base) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(0), base);\
+                xgiCmdQueLen --;
+
+#define XGI300SetupSRCPitch(pitch) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT16(xgi_video_info.mmio_vbase, BR(1), pitch);\
+                xgiCmdQueLen --;
+
+#define XGI300SetupSRCXY(x,y) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(2), (x)<<16 | (y) );\
+                xgiCmdQueLen --;
+
+#define XGI300SetupDSTBase(base) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(4), base);\
+                xgiCmdQueLen --;
+
+#define XGI300SetupDSTXY(x,y) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(3), (x)<<16 | (y) );\
+                xgiCmdQueLen --;
+
+#define XGI300SetupDSTRect(x,y) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(5), (y)<<16 | (x) );\
+                xgiCmdQueLen --;
+
+#define XGI300SetupDSTColorDepth(bpp) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT16(xgi_video_info.mmio_vbase, BR(1)+2, bpp);\
+                xgiCmdQueLen --;
+
+#define XGI300SetupRect(w,h) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(6), (h)<<16 | (w) );\
+                xgiCmdQueLen --;
+
+#define XGI300SetupPATFG(color) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(7), color);\
+                xgiCmdQueLen --;
+
+#define XGI300SetupPATBG(color) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(8), color);\
+                xgiCmdQueLen --;
+
+#define XGI300SetupSRCFG(color) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(9), color);\
+                xgiCmdQueLen --;
+
+#define XGI300SetupSRCBG(color) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(10), color);\
+                xgiCmdQueLen --;
+
+/* 0x8224 src colorkey high */
+/* 0x8228 src colorkey low */
+/* 0x821c dest colorkey high */
+/* 0x8220 dest colorkey low */
+#define XGI300SetupSRCTrans(color) \
+                if (xgiCmdQueLen <= 1)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, 0x8224, color);\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, 0x8228, color);\
+                xgiCmdQueLen -= 2;
+
+#define XGI300SetupDSTTrans(color) \
+                if (xgiCmdQueLen <= 1)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, 0x821C, color); \
+                MMIO_OUT32(xgi_video_info.mmio_vbase, 0x8220, color); \
+                xgiCmdQueLen -= 2;
+
+#define XGI300SetupMONOPAT(p0,p1) \
+                if (xgiCmdQueLen <= 1)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(11), p0);\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(12), p1);\
+                xgiCmdQueLen -= 2;
+
+#define XGI300SetupClipLT(left,top) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(13), ((left) & 0xFFFF) | (top)<<16 );\
+                xgiCmdQueLen--;
+
+#define XGI300SetupClipRB(right,bottom) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(14), ((right) & 0xFFFF) | (bottom)<<16 );\
+                xgiCmdQueLen--;
+
+/* General */
+#define XGI300SetupROP(rop) \
+                xgi_video_info.CommandReg = (rop) << 8;
+
+#define XGI300SetupCMDFlag(flags) \
+                xgi_video_info.CommandReg |= (flags);
+
+#define XGI300DoCMD \
+                if (xgiCmdQueLen <= 1)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(15), xgi_video_info.CommandReg); \
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(16), 0);\
+                xgiCmdQueLen -= 2;
+
+/* Line */
+#define XGI300SetupX0Y0(x,y) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(2), (y)<<16 | (x) );\
+                xgiCmdQueLen--;
+
+#define XGI300SetupX1Y1(x,y) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(3), (y)<<16 | (x) );\
+                xgiCmdQueLen--;
+
+#define XGI300SetupLineCount(c) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT16(xgi_video_info.mmio_vbase, BR(6), c);\
+                xgiCmdQueLen--;
+
+#define XGI300SetupStylePeriod(p) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT16(xgi_video_info.mmio_vbase, BR(6)+2, p);\
+                xgiCmdQueLen--;
+
+#define XGI300SetupStyleLow(ls) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(11), ls);\
+                xgiCmdQueLen--;
+
+#define XGI300SetupStyleHigh(ls) \
+                if (xgiCmdQueLen <= 0)  XGI300Idle;\
+                MMIO_OUT32(xgi_video_info.mmio_vbase, BR(12), ls);\
+                xgiCmdQueLen--;
+
+
+
+/* ----------- XGI 310/325 series --------------- */
+
+/* Q_STATUS:
+   bit 31 = 1: All engines idle and all queues empty
+   bit 30 = 1: Hardware Queue (=HW CQ, 2D queue, 3D queue) empty
+   bit 29 = 1: 2D engine is idle
+   bit 28 = 1: 3D engine is idle
+   bit 27 = 1: HW command queue empty
+   bit 26 = 1: 2D queue empty
+   bit 25 = 1: 3D queue empty
+   bit 24 = 1: SW command queue empty
+   bits 23:16: 2D counter 3
+   bits 15:8:  2D counter 2
+   bits 7:0:   2D counter 1
+
+   Where is the command queue length (current amount of commands the queue
+   can accept) on the 310/325 series? (The current implementation is taken
+   from 300 series and certainly wrong...)
+*/
+
+/* TW: FIXME: xgiCmdQueLen is... where....? */
+#define XGI310Idle \
+  { \
+  while( (MMIO_IN16(xgi_video_info.mmio_vbase, Q_STATUS+2) & 0x8000) != 0x8000){}; \
+  while( (MMIO_IN16(xgi_video_info.mmio_vbase, Q_STATUS+2) & 0x8000) != 0x8000){}; \
+  xgiCmdQueLen=MMIO_IN16(xgi_video_info.mmio_vbase, Q_STATUS); \
+  }
+
+#define XGI310SetupSRCBase(base) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, SRC_ADDR, base);\
+      xgiCmdQueLen--;
+
+#define XGI310SetupSRCPitch(pitch) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT16(xgi_video_info.mmio_vbase, SRC_PITCH, pitch);\
+      xgiCmdQueLen--;
+
+#define XGI310SetupSRCXY(x,y) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, SRC_Y, (x)<<16 | (y) );\
+      xgiCmdQueLen--;
+
+#define XGI310SetupDSTBase(base) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, DST_ADDR, base);\
+      xgiCmdQueLen--;
+
+#define XGI310SetupDSTXY(x,y) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, DST_Y, (x)<<16 | (y) );\
+      xgiCmdQueLen--;
+
+#define XGI310SetupDSTRect(x,y) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, DST_PITCH, (y)<<16 | (x) );\
+      xgiCmdQueLen--;
+
+#define XGI310SetupDSTColorDepth(bpp) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT16(xgi_video_info.mmio_vbase, AGP_BASE, bpp);\
+      xgiCmdQueLen--;
+
+#define XGI310SetupRect(w,h) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, RECT_WIDTH, (h)<<16 | (w) );\
+      xgiCmdQueLen--;
+
+#define XGI310SetupPATFG(color) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, PAT_FGCOLOR, color);\
+      xgiCmdQueLen--;
+
+#define XGI310SetupPATBG(color) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, PAT_BGCOLOR, color);\
+      xgiCmdQueLen--;
+
+#define XGI310SetupSRCFG(color) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, SRC_FGCOLOR, color);\
+      xgiCmdQueLen--;
+
+#define XGI310SetupSRCBG(color) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, SRC_BGCOLOR, color);\
+      xgiCmdQueLen--;
+
+#define XGI310SetupSRCTrans(color) \
+      if (xgiCmdQueLen <= 1)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, TRANS_SRC_KEY_HIGH, color);\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, TRANS_SRC_KEY_LOW, color);\
+      xgiCmdQueLen -= 2;
+
+#define XGI310SetupDSTTrans(color) \
+      if (xgiCmdQueLen <= 1)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, TRANS_DST_KEY_HIGH, color); \
+      MMIO_OUT32(xgi_video_info.mmio_vbase, TRANS_DST_KEY_LOW, color); \
+      xgiCmdQueLen -= 2;
+
+#define XGI310SetupMONOPAT(p0,p1) \
+      if (xgiCmdQueLen <= 1)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, MONO_MASK, p0);\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, MONO_MASK+4, p1);\
+      xgiCmdQueLen -= 2;
+
+#define XGI310SetupClipLT(left,top) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, LEFT_CLIP, ((left) & 0xFFFF) | (top)<<16 );\
+      xgiCmdQueLen--;
+
+#define XGI310SetupClipRB(right,bottom) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, RIGHT_CLIP, ((right) & 0xFFFF) | (bottom)<<16 );\
+      xgiCmdQueLen--;
+
+#define XGI310SetupROP(rop) \
+      xgi_video_info.CommandReg = (rop) << 8;
+
+#define XGI310SetupCMDFlag(flags) \
+      xgi_video_info.CommandReg |= (flags);
+
+#define XGI310DoCMD \
+      if (xgiCmdQueLen <= 1)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, COMMAND_READY, xgi_video_info.CommandReg); \
+      MMIO_OUT32(xgi_video_info.mmio_vbase, FIRE_TRIGGER, 0); \
+      xgiCmdQueLen -= 2;
+
+#define XGI310SetupX0Y0(x,y) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, LINE_X0, (y)<<16 | (x) );\
+      xgiCmdQueLen--;
+
+#define XGI310SetupX1Y1(x,y) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, LINE_X1, (y)<<16 | (x) );\
+      xgiCmdQueLen--;
+
+#define XGI310SetupLineCount(c) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT16(xgi_video_info.mmio_vbase, LINE_COUNT, c);\
+      xgiCmdQueLen--;
+
+#define XGI310SetupStylePeriod(p) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT16(xgi_video_info.mmio_vbase, LINE_STYLE_PERIOD, p);\
+      xgiCmdQueLen--;
+
+#define XGI310SetupStyleLow(ls) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, LINE_STYLE_0, ls);\
+      xgiCmdQueLen--;
+
+#define XGI310SetupStyleHigh(ls) \
+      if (xgiCmdQueLen <= 0)  XGI310Idle;\
+      MMIO_OUT32(xgi_video_info.mmio_vbase, LINE_STYLE_1, ls);\
+      xgiCmdQueLen--;
+
+int  XGIfb_initaccel(void);
+void XGIfb_syncaccel(void);
+
+extern struct video_info xgi_video_info;
+
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,5,33)
+void fbcon_XGI_bmove(struct display *p, int srcy, int srcx, int dsty,
+                     int dstx, int height, int width);
+void fbcon_XGI_revc(struct display *p, int srcy, int srcx);
+void fbcon_XGI_clear8(struct vc_data *conp, struct display *p, int srcy,
+                      int srcx, int height, int width);
+void fbcon_XGI_clear16(struct vc_data *conp, struct display *p, int srcy,
+                       int srcx, int height, int width);
+void fbcon_XGI_clear32(struct vc_data *conp, struct display *p, int srcy,
+                       int srcx, int height, int width);
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,34)
+extern int XGIfb_accel;
+void fbcon_XGI_fillrect(struct fb_info *info, const struct fb_fillrect *rect);
+void fbcon_XGI_copyarea(struct fb_info *info, const struct fb_copyarea *area);
+#endif
+
+#endif
diff --git a/drivers/staging/xgifb/XGI_main.h b/drivers/staging/xgifb/XGI_main.h
new file mode 100644
index 000000000000..4f4171e8a68a
--- /dev/null
+++ b/drivers/staging/xgifb/XGI_main.h
@@ -0,0 +1,1023 @@
+#ifndef _XGIFB_MAIN
+#define _XGIFB_MAIN
+
+
+/* ------------------- Constant Definitions ------------------------- */
+
+
+#include "XGIfb.h"
+#include "vb_struct.h"
+#include "vb_def.h"
+
+//#define LINUXBIOS   /* turn this on when compiling for LINUXBIOS */
+#define AGPOFF     /* default is turn off AGP */
+
+#define XGIFAIL(x) do { printk(x "\n"); return -EINVAL; } while(0)
+
+#define VER_MAJOR                 0
+#define VER_MINOR                 8
+#define VER_LEVEL                 1
+
+#define DRIVER_DESC  "XGI Volari Frame Buffer Module Version 0.8.1"
+
+#ifndef PCI_VENDOR_ID_XG
+#define PCI_VENDOR_ID_XG          0x18CA
+#endif
+
+#ifndef PCI_DEVICE_ID_XG_40
+#define PCI_DEVICE_ID_XG_40      0x040
+#endif
+#ifndef PCI_DEVICE_ID_XG_41
+#define PCI_DEVICE_ID_XG_41      0x041
+#endif
+#ifndef PCI_DEVICE_ID_XG_42
+#define PCI_DEVICE_ID_XG_42      0x042
+#endif
+#ifndef PCI_DEVICE_ID_XG_20
+#define PCI_DEVICE_ID_XG_20      0x020
+#endif
+#ifndef PCI_DEVICE_ID_XG_27
+#define PCI_DEVICE_ID_XG_27      0x027
+#endif
+
+
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,8)
+#define XGI_IOTYPE1 void __iomem
+#define XGI_IOTYPE2 __iomem
+#define XGIINITSTATIC static
+#else
+#define XGI_IOTYPE1 unsigned char
+#define XGI_IOTYPE2
+#define XGIINITSTATIC
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+static struct pci_device_id __devinitdata xgifb_pci_table[] = {
+
+        { PCI_VENDOR_ID_XG, PCI_DEVICE_ID_XG_20, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+        { PCI_VENDOR_ID_XG, PCI_DEVICE_ID_XG_27, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 1},
+        { PCI_VENDOR_ID_XG, PCI_DEVICE_ID_XG_40, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 2},
+        { PCI_VENDOR_ID_XG, PCI_DEVICE_ID_XG_42, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 3},
+        { 0 }
+};
+
+MODULE_DEVICE_TABLE(pci, xgifb_pci_table);
+#endif
+/* To be included in fb.h */
+#ifndef FB_ACCEL_XGI_GLAMOUR_2
+#define FB_ACCEL_XGI_GLAMOUR_2  40      /* XGI 315, 650, 740            */
+#endif
+#ifndef FB_ACCEL_XGI_XABRE
+#define FB_ACCEL_XGI_XABRE      41      /* XGI 330 ("Xabre")            */
+#endif
+
+#define MAX_ROM_SCAN              0x10000
+
+#define HW_CURSOR_CAP             0x80
+#define TURBO_QUEUE_CAP           0x40
+#define AGP_CMD_QUEUE_CAP         0x20
+#define VM_CMD_QUEUE_CAP          0x10
+#define MMIO_CMD_QUEUE_CAP        0x08
+
+
+
+/* For 315 series */
+
+#define COMMAND_QUEUE_AREA_SIZE   0x80000 /* 512K */
+#define COMMAND_QUEUE_THRESHOLD   0x1F
+
+
+/* TW */
+#define HW_CURSOR_AREA_SIZE_315   0x4000  /* 16K */
+#define HW_CURSOR_AREA_SIZE_300   0x1000  /* 4K */
+
+#define OH_ALLOC_SIZE             4000
+#define SENTINEL                  0x7fffffff
+
+#define SEQ_ADR                   0x14
+#define SEQ_DATA                  0x15
+#define DAC_ADR                   0x18
+#define DAC_DATA                  0x19
+#define CRTC_ADR                  0x24
+#define CRTC_DATA                 0x25
+#define DAC2_ADR                  (0x16-0x30)
+#define DAC2_DATA                 (0x17-0x30)
+#define VB_PART1_ADR              (0x04-0x30)
+#define VB_PART1_DATA             (0x05-0x30)
+#define VB_PART2_ADR              (0x10-0x30)
+#define VB_PART2_DATA             (0x11-0x30)
+#define VB_PART3_ADR              (0x12-0x30)
+#define VB_PART3_DATA             (0x13-0x30)
+#define VB_PART4_ADR              (0x14-0x30)
+#define VB_PART4_DATA             (0x15-0x30)
+
+#define XGISR                     XGI_Pr.P3c4
+#define XGICR                     XGI_Pr.P3d4
+#define XGIDACA                   XGI_Pr.P3c8
+#define XGIDACD                   XGI_Pr.P3c9
+#define XGIPART1                  XGI_Pr.Part1Port
+#define XGIPART2                  XGI_Pr.Part2Port
+#define XGIPART3                  XGI_Pr.Part3Port
+#define XGIPART4                  XGI_Pr.Part4Port
+#define XGIPART5                  XGI_Pr.Part5Port
+#define XGIDAC2A                  XGIPART5
+#define XGIDAC2D                  (XGIPART5 + 1)
+#define XGIMISCR                  (XGI_Pr.RelIO + 0x1c)
+#define XGIINPSTAT                (XGI_Pr.RelIO + 0x2a)
+
+#define IND_XGI_PASSWORD          0x05  /* SRs */
+#define IND_XGI_COLOR_MODE        0x06
+#define IND_XGI_RAMDAC_CONTROL    0x07
+#define IND_XGI_DRAM_SIZE         0x14
+#define IND_XGI_SCRATCH_REG_16    0x16
+#define IND_XGI_SCRATCH_REG_17    0x17
+#define IND_XGI_SCRATCH_REG_1A    0x1A
+#define IND_XGI_MODULE_ENABLE     0x1E
+#define IND_XGI_PCI_ADDRESS_SET   0x20
+#define IND_XGI_TURBOQUEUE_ADR    0x26
+#define IND_XGI_TURBOQUEUE_SET    0x27
+#define IND_XGI_POWER_ON_TRAP     0x38
+#define IND_XGI_POWER_ON_TRAP2    0x39
+#define IND_XGI_CMDQUEUE_SET      0x26
+#define IND_XGI_CMDQUEUE_THRESHOLD  0x27
+
+#define IND_XGI_SCRATCH_REG_CR30  0x30  /* CRs */
+#define IND_XGI_SCRATCH_REG_CR31  0x31
+#define IND_XGI_SCRATCH_REG_CR32  0x32
+#define IND_XGI_SCRATCH_REG_CR33  0x33
+#define IND_XGI_LCD_PANEL         0x36
+#define IND_XGI_SCRATCH_REG_CR37  0x37
+#define IND_XGI_AGP_IO_PAD        0x48
+
+#define IND_BRI_DRAM_STATUS       0x63 /* PCI config memory size offset */
+
+#define MMIO_QUEUE_PHYBASE        0x85C0
+#define MMIO_QUEUE_WRITEPORT      0x85C4
+#define MMIO_QUEUE_READPORT       0x85C8
+
+#define IND_XGI_CRT2_WRITE_ENABLE_300 0x24
+#define IND_XGI_CRT2_WRITE_ENABLE_315 0x2F
+
+#define XGI_PASSWORD              0x86  /* SR05 */
+#define XGI_INTERLACED_MODE       0x20  /* SR06 */
+#define XGI_8BPP_COLOR_MODE       0x0
+#define XGI_15BPP_COLOR_MODE      0x1
+#define XGI_16BPP_COLOR_MODE      0x2
+#define XGI_32BPP_COLOR_MODE      0x4
+
+#define XGI_DRAM_SIZE_MASK     0xF0  /*SR14 */
+#define XGI_DRAM_SIZE_1MB      0x00
+#define XGI_DRAM_SIZE_2MB      0x01
+#define XGI_DRAM_SIZE_4MB      0x02
+#define XGI_DRAM_SIZE_8MB      0x03
+#define XGI_DRAM_SIZE_16MB     0x04
+#define XGI_DRAM_SIZE_32MB     0x05
+#define XGI_DRAM_SIZE_64MB     0x06
+#define XGI_DRAM_SIZE_128MB    0x07
+#define XGI_DRAM_SIZE_256MB    0x08
+#define XGI_DATA_BUS_MASK      0x02
+#define XGI_DATA_BUS_64        0x00
+#define XGI_DATA_BUS_128       0x01
+#define XGI_DUAL_CHANNEL_MASK  0x0C
+#define XGI_SINGLE_CHANNEL_1_RANK       0x0
+#define XGI_SINGLE_CHANNEL_2_RANK       0x1
+#define XGI_ASYM_DDR                    0x02
+#define XGI_DUAL_CHANNEL_1_RANK         0x3
+
+#define XGI550_DRAM_SIZE_MASK     0x3F  /* 550/650/740 SR14 */
+#define XGI550_DRAM_SIZE_4MB      0x00
+#define XGI550_DRAM_SIZE_8MB      0x01
+#define XGI550_DRAM_SIZE_16MB     0x03
+#define XGI550_DRAM_SIZE_24MB     0x05
+#define XGI550_DRAM_SIZE_32MB     0x07
+#define XGI550_DRAM_SIZE_64MB     0x0F
+#define XGI550_DRAM_SIZE_96MB     0x17
+#define XGI550_DRAM_SIZE_128MB    0x1F
+#define XGI550_DRAM_SIZE_256MB    0x3F
+
+#define XGI_SCRATCH_REG_1A_MASK   0x10
+
+#define XGI_ENABLE_2D             0x40  /* SR1E */
+
+#define XGI_MEM_MAP_IO_ENABLE     0x01  /* SR20 */
+#define XGI_PCI_ADDR_ENABLE       0x80
+
+#define XGI_AGP_CMDQUEUE_ENABLE   0x80  /* 315/650/740 SR26 */
+#define XGI_VRAM_CMDQUEUE_ENABLE  0x40
+#define XGI_MMIO_CMD_ENABLE       0x20
+#define XGI_CMD_QUEUE_SIZE_512k   0x00
+#define XGI_CMD_QUEUE_SIZE_1M     0x04
+#define XGI_CMD_QUEUE_SIZE_2M     0x08
+#define XGI_CMD_QUEUE_SIZE_4M     0x0C
+#define XGI_CMD_QUEUE_RESET       0x01
+#define XGI_CMD_AUTO_CORR         0x02
+
+#define XGI_SIMULTANEOUS_VIEW_ENABLE  0x01  /* CR30 */
+#define XGI_MODE_SELECT_CRT2      0x02
+#define XGI_VB_OUTPUT_COMPOSITE   0x04
+#define XGI_VB_OUTPUT_SVIDEO      0x08
+#define XGI_VB_OUTPUT_SCART       0x10
+#define XGI_VB_OUTPUT_LCD         0x20
+#define XGI_VB_OUTPUT_CRT2        0x40
+#define XGI_VB_OUTPUT_HIVISION    0x80
+
+#define XGI_VB_OUTPUT_DISABLE     0x20  /* CR31 */
+#define XGI_DRIVER_MODE           0x40
+
+#define XGI_VB_COMPOSITE          0x01  /* CR32 */
+#define XGI_VB_SVIDEO             0x02
+#define XGI_VB_SCART              0x04
+#define XGI_VB_LCD                0x08
+#define XGI_VB_CRT2               0x10
+#define XGI_CRT1                  0x20
+#define XGI_VB_HIVISION           0x40
+#define XGI_VB_YPBPR                0x80
+#define XGI_VB_TV                 (XGI_VB_COMPOSITE | XGI_VB_SVIDEO | \
+                                   XGI_VB_SCART | XGI_VB_HIVISION|XGI_VB_YPBPR)
+
+#define XGI_EXTERNAL_CHIP_MASK             0x0E  /* CR37 */
+#define XGI_EXTERNAL_CHIP_XGI301           0x01  /* in CR37 << 1 ! */
+#define XGI_EXTERNAL_CHIP_LVDS             0x02  /* in CR37 << 1 ! */
+#define XGI_EXTERNAL_CHIP_TRUMPION         0x03  /* in CR37 << 1 ! */
+#define XGI_EXTERNAL_CHIP_LVDS_CHRONTEL    0x04  /* in CR37 << 1 ! */
+#define XGI_EXTERNAL_CHIP_CHRONTEL         0x05  /* in CR37 << 1 ! */
+#define XGI310_EXTERNAL_CHIP_LVDS          0x02  /* in CR37 << 1 ! */
+#define XGI310_EXTERNAL_CHIP_LVDS_CHRONTEL 0x03  /* in CR37 << 1 ! */
+
+#define XGI_AGP_2X                0x20  /* CR48 */
+
+#define BRI_DRAM_SIZE_MASK        0x70  /* PCI bridge config data */
+#define BRI_DRAM_SIZE_2MB         0x00
+#define BRI_DRAM_SIZE_4MB         0x01
+#define BRI_DRAM_SIZE_8MB         0x02
+#define BRI_DRAM_SIZE_16MB        0x03
+#define BRI_DRAM_SIZE_32MB        0x04
+#define BRI_DRAM_SIZE_64MB        0x05
+
+#define HW_DEVICE_EXTENSION       XGI_HW_DEVICE_INFO
+#define PHW_DEVICE_EXTENSION      PXGI_HW_DEVICE_INFO
+
+#define SR_BUFFER_SIZE            5
+#define CR_BUFFER_SIZE            5
+
+/* Useful macros */
+#define inXGIREG(base)          inb(base)
+#define outXGIREG(base,val)     outb(val,base)
+#define orXGIREG(base,val)      do { \
+                                  unsigned char __Temp = inb(base); \
+                                  outXGIREG(base, __Temp | (val)); \
+                                } while (0)
+#define andXGIREG(base,val)     do { \
+                                  unsigned char __Temp = inb(base); \
+                                  outXGIREG(base, __Temp & (val)); \
+                                } while (0)
+#define inXGIIDXREG(base,idx,var)   do { \
+                                      outb(idx,base); var=inb((base)+1); \
+                                    } while (0)
+#define outXGIIDXREG(base,idx,val)  do { \
+                                      outb(idx,base); outb((val),(base)+1); \
+                                    } while (0)
+#define orXGIIDXREG(base,idx,val)   do { \
+                                      unsigned char __Temp; \
+                                      outb(idx,base);   \
+                                      __Temp = inb((base)+1)|(val); \
+                                      outXGIIDXREG(base,idx,__Temp); \
+                                    } while (0)
+#define andXGIIDXREG(base,idx,and)  do { \
+                                      unsigned char __Temp; \
+                                      outb(idx,base);   \
+                                      __Temp = inb((base)+1)&(and); \
+                                      outXGIIDXREG(base,idx,__Temp); \
+                                    } while (0)
+#define setXGIIDXREG(base,idx,and,or)   do { \
+                                          unsigned char __Temp; \
+                                          outb(idx,base);   \
+                                          __Temp = (inb((base)+1)&(and))|(or); \
+                                          outXGIIDXREG(base,idx,__Temp); \
+                                        } while (0)
+
+/* ------------------- Global Variables ----------------------------- */
+
+/* Fbcon variables */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+static struct fb_info* fb_info;
+#else
+static struct fb_info XGI_fb_info;
+#endif
+
+
+static int    video_type = FB_TYPE_PACKED_PIXELS;
+
+static struct fb_var_screeninfo default_var = {
+        .xres           = 0,
+        .yres           = 0,
+        .xres_virtual   = 0,
+        .yres_virtual   = 0,
+        .xoffset        = 0,
+        .yoffset        = 0,
+        .bits_per_pixel = 0,
+        .grayscale      = 0,
+        .red            = {0, 8, 0},
+        .green          = {0, 8, 0},
+        .blue           = {0, 8, 0},
+        .transp         = {0, 0, 0},
+        .nonstd         = 0,
+        .activate       = FB_ACTIVATE_NOW,
+        .height         = -1,
+        .width          = -1,
+        .accel_flags    = 0,
+        .pixclock       = 0,
+        .left_margin    = 0,
+        .right_margin   = 0,
+        .upper_margin   = 0,
+        .lower_margin   = 0,
+        .hsync_len      = 0,
+        .vsync_len      = 0,
+        .sync           = 0,
+        .vmode          = FB_VMODE_NONINTERLACED,
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+        .reserved       = {0, 0, 0, 0, 0, 0}
+#endif
+};
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+static struct fb_fix_screeninfo XGIfb_fix = {
+        .id             = "XGI",
+        .type           = FB_TYPE_PACKED_PIXELS,
+        .xpanstep       = 1,
+        .ypanstep       = 1,
+};
+static char myid[20];
+static u32 pseudo_palette[17];
+#endif
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+static struct display XGI_disp;
+
+static struct display_switch XGIfb_sw;
+
+static struct {
+        u16 blue, green, red, pad;
+} XGI_palette[256];
+
+static union {
+#ifdef FBCON_HAS_CFB16
+        u16 cfb16[16];
+#endif
+#ifdef FBCON_HAS_CFB32
+        u32 cfb32[16];
+#endif
+} XGI_fbcon_cmap;
+
+static int XGIfb_inverse = 0;
+#endif
+
+/* display status */
+static int XGIfb_off = 0;
+static int XGIfb_crt1off = 0;
+static int XGIfb_forcecrt1 = -1;
+static int XGIvga_enabled = 0;
+static int XGIfb_userom = 0;
+//static int XGIfb_useoem = -1;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+static int currcon = 0;
+#endif
+
+/* global flags */
+static int XGIfb_registered;
+static int XGIfb_tvmode = 0;
+static int XGIfb_mem = 0;
+static int XGIfb_pdc = 0;
+static int enable_dstn = 0;
+static int XGIfb_ypan = -1;
+
+
+int        XGIfb_accel = 0;
+
+
+static int XGIfb_hwcursor_size = 0;
+static int XGIfb_CRT2_write_enable = 0;
+
+int XGIfb_crt2type  = -1;       /* TW: CRT2 type (for overriding autodetection) */
+int XGIfb_tvplug    = -1;       /* PR: Tv plug type (for overriding autodetection) */
+
+int XGIfb_queuemode = -1;       /* TW: Use MMIO queue mode by default (310/325 series only) */
+
+unsigned char XGIfb_detectedpdc = 0;
+
+unsigned char XGIfb_detectedlcda = 0xff;
+
+
+
+
+/* TW: For ioctl XGIFB_GET_INFO */
+/* XGIfb_info XGIfbinfo; */
+
+/* TW: Hardware extension; contains data on hardware */
+HW_DEVICE_EXTENSION XGIhw_ext;
+
+/* TW: XGI private structure */
+VB_DEVICE_INFO  XGI_Pr;
+
+/* card parameters */
+static unsigned long XGIfb_mmio_size = 0;
+static u8            XGIfb_caps = 0;
+
+typedef enum _XGI_CMDTYPE {
+        MMIO_CMD = 0,
+        AGP_CMD_QUEUE,
+        VM_CMD_QUEUE,
+} XGI_CMDTYPE;
+
+#define MD_XGI300 1
+#define MD_XGI315 2
+
+/* mode table */
+/* NOT const - will be patched for 1280x960 mode number chaos reasons */
+struct _XGIbios_mode {
+        char name[15];
+        u8 mode_no;
+        u16 vesa_mode_no_1;  /* "XGI defined" VESA mode number */
+        u16 vesa_mode_no_2;  /* Real VESA mode numbers */
+        u16 xres;
+        u16 yres;
+        u16 bpp;
+        u16 rate_idx;
+        u16 cols;
+        u16 rows;
+        u8  chipset;
+} XGIbios_mode[] = {
+#define MODE_INDEX_NONE           0  /* TW: index for mode=none */
+        {"none",         0xFF, 0x0000, 0x0000,    0,    0,  0, 0,   0,  0, MD_XGI300|MD_XGI315},  /* TW: for mode "none" */
+        {"320x240x16",   0x56, 0x0000, 0x0000,  320,  240, 16, 1,  40, 15,           MD_XGI315},
+        {"320x480x8",    0x5A, 0x0000, 0x0000,  320,  480,  8, 1,  40, 30,           MD_XGI315},  /* TW: FSTN */
+        {"320x480x16",   0x5B, 0x0000, 0x0000,  320,  480, 16, 1,  40, 30,           MD_XGI315},  /* TW: FSTN */
+        {"640x480x8",    0x2E, 0x0101, 0x0101,  640,  480,  8, 1,  80, 30, MD_XGI300|MD_XGI315},
+        {"640x480x16",   0x44, 0x0111, 0x0111,  640,  480, 16, 1,  80, 30, MD_XGI300|MD_XGI315},
+        {"640x480x24",   0x62, 0x013a, 0x0112,  640,  480, 32, 1,  80, 30, MD_XGI300|MD_XGI315},  /* TW: That's for people who mix up color- and fb depth */
+        {"640x480x32",   0x62, 0x013a, 0x0112,  640,  480, 32, 1,  80, 30, MD_XGI300|MD_XGI315},
+        {"720x480x8",    0x31, 0x0000, 0x0000,  720,  480,  8, 1,  90, 30, MD_XGI300|MD_XGI315},
+        {"720x480x16",   0x33, 0x0000, 0x0000,  720,  480, 16, 1,  90, 30, MD_XGI300|MD_XGI315},
+        {"720x480x24",   0x35, 0x0000, 0x0000,  720,  480, 32, 1,  90, 30, MD_XGI300|MD_XGI315},
+        {"720x480x32",   0x35, 0x0000, 0x0000,  720,  480, 32, 1,  90, 30, MD_XGI300|MD_XGI315},
+        {"720x576x8",    0x32, 0x0000, 0x0000,  720,  576,  8, 1,  90, 36, MD_XGI300|MD_XGI315},
+        {"720x576x16",   0x34, 0x0000, 0x0000,  720,  576, 16, 1,  90, 36, MD_XGI300|MD_XGI315},
+        {"720x576x24",   0x36, 0x0000, 0x0000,  720,  576, 32, 1,  90, 36, MD_XGI300|MD_XGI315},
+        {"720x576x32",   0x36, 0x0000, 0x0000,  720,  576, 32, 1,  90, 36, MD_XGI300|MD_XGI315},
+        {"800x480x8",    0x70, 0x0000, 0x0000,  800,  480,  8, 1, 100, 30, MD_XGI300|MD_XGI315},
+        {"800x480x16",   0x7a, 0x0000, 0x0000,  800,  480, 16, 1, 100, 30, MD_XGI300|MD_XGI315},
+        {"800x480x24",   0x76, 0x0000, 0x0000,  800,  480, 32, 1, 100, 30, MD_XGI300|MD_XGI315},
+        {"800x480x32",   0x76, 0x0000, 0x0000,  800,  480, 32, 1, 100, 30, MD_XGI300|MD_XGI315},
+#define DEFAULT_MODE              21 /* TW: index for 800x600x8 */
+#define DEFAULT_LCDMODE           21 /* TW: index for 800x600x8 */
+#define DEFAULT_TVMODE            21 /* TW: index for 800x600x8 */
+        {"800x600x8",    0x30, 0x0103, 0x0103,  800,  600,  8, 1, 100, 37, MD_XGI300|MD_XGI315},
+        {"800x600x16",   0x47, 0x0114, 0x0114,  800,  600, 16, 1, 100, 37, MD_XGI300|MD_XGI315},
+        {"800x600x24",   0x63, 0x013b, 0x0115,  800,  600, 32, 1, 100, 37, MD_XGI300|MD_XGI315},
+        {"800x600x32",   0x63, 0x013b, 0x0115,  800,  600, 32, 1, 100, 37, MD_XGI300|MD_XGI315},
+        {"1024x576x8",   0x71, 0x0000, 0x0000, 1024,  576,  8, 1, 128, 36, MD_XGI300|MD_XGI315},
+        {"1024x576x16",  0x74, 0x0000, 0x0000, 1024,  576, 16, 1, 128, 36, MD_XGI300|MD_XGI315},
+        {"1024x576x24",  0x77, 0x0000, 0x0000, 1024,  576, 32, 1, 128, 36, MD_XGI300|MD_XGI315},
+        {"1024x576x32",  0x77, 0x0000, 0x0000, 1024,  576, 32, 1, 128, 36, MD_XGI300|MD_XGI315},
+        {"1024x600x8",   0x20, 0x0000, 0x0000, 1024,  600,  8, 1, 128, 37, MD_XGI300          },  /* TW: 300 series only */
+        {"1024x600x16",  0x21, 0x0000, 0x0000, 1024,  600, 16, 1, 128, 37, MD_XGI300          },
+        {"1024x600x24",  0x22, 0x0000, 0x0000, 1024,  600, 32, 1, 128, 37, MD_XGI300          },
+        {"1024x600x32",  0x22, 0x0000, 0x0000, 1024,  600, 32, 1, 128, 37, MD_XGI300          },
+        {"1024x768x8",   0x38, 0x0105, 0x0105, 1024,  768,  8, 1, 128, 48, MD_XGI300|MD_XGI315},
+        {"1024x768x16",  0x4A, 0x0117, 0x0117, 1024,  768, 16, 1, 128, 48, MD_XGI300|MD_XGI315},
+        {"1024x768x24",  0x64, 0x013c, 0x0118, 1024,  768, 32, 1, 128, 48, MD_XGI300|MD_XGI315},
+        {"1024x768x32",  0x64, 0x013c, 0x0118, 1024,  768, 32, 1, 128, 48, MD_XGI300|MD_XGI315},
+        {"1152x768x8",   0x23, 0x0000, 0x0000, 1152,  768,  8, 1, 144, 48, MD_XGI300          },  /* TW: 300 series only */
+        {"1152x768x16",  0x24, 0x0000, 0x0000, 1152,  768, 16, 1, 144, 48, MD_XGI300          },
+        {"1152x768x24",  0x25, 0x0000, 0x0000, 1152,  768, 32, 1, 144, 48, MD_XGI300          },
+        {"1152x768x32",  0x25, 0x0000, 0x0000, 1152,  768, 32, 1, 144, 48, MD_XGI300          },
+        {"1280x720x8",   0x79, 0x0000, 0x0000, 1280,  720,  8, 1, 160, 45, MD_XGI300|MD_XGI315},
+        {"1280x720x16",  0x75, 0x0000, 0x0000, 1280,  720, 16, 1, 160, 45, MD_XGI300|MD_XGI315},
+        {"1280x720x24",  0x78, 0x0000, 0x0000, 1280,  720, 32, 1, 160, 45, MD_XGI300|MD_XGI315},
+        {"1280x720x32",  0x78, 0x0000, 0x0000, 1280,  720, 32, 1, 160, 45, MD_XGI300|MD_XGI315},
+        {"1280x768x8",   0x23, 0x0000, 0x0000, 1280,  768,  8, 1, 160, 48,           MD_XGI315},  /* TW: 310/325 series only */
+        {"1280x768x16",  0x24, 0x0000, 0x0000, 1280,  768, 16, 1, 160, 48,           MD_XGI315},
+        {"1280x768x24",  0x25, 0x0000, 0x0000, 1280,  768, 32, 1, 160, 48,           MD_XGI315},
+        {"1280x768x32",  0x25, 0x0000, 0x0000, 1280,  768, 32, 1, 160, 48,           MD_XGI315},
+#define MODEINDEX_1280x960 48
+        {"1280x960x8",   0x7C, 0x0000, 0x0000, 1280,  960,  8, 1, 160, 60, MD_XGI300|MD_XGI315},  /* TW: Modenumbers being patched */
+        {"1280x960x16",  0x7D, 0x0000, 0x0000, 1280,  960, 16, 1, 160, 60, MD_XGI300|MD_XGI315},
+        {"1280x960x24",  0x7E, 0x0000, 0x0000, 1280,  960, 32, 1, 160, 60, MD_XGI300|MD_XGI315},
+        {"1280x960x32",  0x7E, 0x0000, 0x0000, 1280,  960, 32, 1, 160, 60, MD_XGI300|MD_XGI315},
+        {"1280x1024x8",  0x3A, 0x0107, 0x0107, 1280, 1024,  8, 1, 160, 64, MD_XGI300|MD_XGI315},
+        {"1280x1024x16", 0x4D, 0x011a, 0x011a, 1280, 1024, 16, 1, 160, 64, MD_XGI300|MD_XGI315},
+        {"1280x1024x24", 0x65, 0x013d, 0x011b, 1280, 1024, 32, 1, 160, 64, MD_XGI300|MD_XGI315},
+        {"1280x1024x32", 0x65, 0x013d, 0x011b, 1280, 1024, 32, 1, 160, 64, MD_XGI300|MD_XGI315},
+        {"1400x1050x8",  0x26, 0x0000, 0x0000, 1400, 1050,  8, 1, 175, 65,           MD_XGI315},  /* TW: 310/325 series only */
+        {"1400x1050x16", 0x27, 0x0000, 0x0000, 1400, 1050, 16, 1, 175, 65,           MD_XGI315},
+        {"1400x1050x24", 0x28, 0x0000, 0x0000, 1400, 1050, 32, 1, 175, 65,           MD_XGI315},
+        {"1400x1050x32", 0x28, 0x0000, 0x0000, 1400, 1050, 32, 1, 175, 65,           MD_XGI315},
+        {"1600x1200x8",  0x3C, 0x0130, 0x011c, 1600, 1200,  8, 1, 200, 75, MD_XGI300|MD_XGI315},
+        {"1600x1200x16", 0x3D, 0x0131, 0x011e, 1600, 1200, 16, 1, 200, 75, MD_XGI300|MD_XGI315},
+        {"1600x1200x24", 0x66, 0x013e, 0x011f, 1600, 1200, 32, 1, 200, 75, MD_XGI300|MD_XGI315},
+        {"1600x1200x32", 0x66, 0x013e, 0x011f, 1600, 1200, 32, 1, 200, 75, MD_XGI300|MD_XGI315},
+        {"1920x1440x8",  0x68, 0x013f, 0x0000, 1920, 1440,  8, 1, 240, 75, MD_XGI300|MD_XGI315},
+        {"1920x1440x16", 0x69, 0x0140, 0x0000, 1920, 1440, 16, 1, 240, 75, MD_XGI300|MD_XGI315},
+        {"1920x1440x24", 0x6B, 0x0141, 0x0000, 1920, 1440, 32, 1, 240, 75, MD_XGI300|MD_XGI315},
+        {"1920x1440x32", 0x6B, 0x0141, 0x0000, 1920, 1440, 32, 1, 240, 75, MD_XGI300|MD_XGI315},
+        {"2048x1536x8",  0x6c, 0x0000, 0x0000, 2048, 1536,  8, 1, 256, 96,           MD_XGI315},  /* TW: 310/325 series only */
+        {"2048x1536x16", 0x6d, 0x0000, 0x0000, 2048, 1536, 16, 1, 256, 96,           MD_XGI315},
+        {"2048x1536x24", 0x6e, 0x0000, 0x0000, 2048, 1536, 32, 1, 256, 96,           MD_XGI315},
+        {"2048x1536x32", 0x6e, 0x0000, 0x0000, 2048, 1536, 32, 1, 256, 96,           MD_XGI315},
+        {"\0", 0x00, 0, 0, 0, 0, 0, 0, 0}
+};
+
+/* mode-related variables */
+#ifdef MODULE
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+static int xgifb_mode_idx = 1;
+#else
+static int XGIfb_mode_idx = MODE_INDEX_NONE;  /* Don't use a mode by default if we are a module */
+#endif
+#else
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+static int xgifb_mode_idx = -1;               /* Use a default mode if we are inside the kernel */
+#else
+static int XGIfb_mode_idx = -1;
+#endif
+#endif
+u8  XGIfb_mode_no  = 0;
+u8  XGIfb_rate_idx = 0;
+
+/* TW: CR36 evaluation */
+const USHORT XGI300paneltype[] =
+    { LCD_UNKNOWN,   LCD_800x600,  LCD_1024x768,  LCD_1280x1024,
+      LCD_1280x960,  LCD_640x480,  LCD_1024x600,  LCD_1152x768,
+       LCD_1024x768, LCD_1024x768,  LCD_1024x768,
+      LCD_1024x768,  LCD_1024x768, LCD_1024x768,  LCD_1024x768 };
+
+const USHORT XGI310paneltype[] =
+    { LCD_UNKNOWN,   LCD_800x600,  LCD_1024x768,  LCD_1280x1024,
+      LCD_640x480,   LCD_1024x600, LCD_1152x864,  LCD_1280x960,
+      LCD_1152x768,  LCD_1400x1050,LCD_1280x768,  LCD_1600x1200,
+      LCD_1024x768, LCD_1024x768,  LCD_1024x768 };
+
+static const struct _XGI_crt2type {
+        char name[10];
+        int type_no;
+        int tvplug_no;
+} XGI_crt2type[] = {
+        {"NONE",        0,              -1},
+        {"LCD",         DISPTYPE_LCD,   -1},
+        {"TV",          DISPTYPE_TV,    -1},
+        {"VGA",         DISPTYPE_CRT2,  -1},
+        {"SVIDEO",      DISPTYPE_TV,    TVPLUG_SVIDEO},
+        {"COMPOSITE",   DISPTYPE_TV,    TVPLUG_COMPOSITE},
+        {"SCART",       DISPTYPE_TV,    TVPLUG_SCART},
+        {"none",        0,              -1},
+        {"lcd",         DISPTYPE_LCD,   -1},
+        {"tv",          DISPTYPE_TV,    -1},
+        {"vga",         DISPTYPE_CRT2,  -1},
+        {"svideo",      DISPTYPE_TV,    TVPLUG_SVIDEO},
+        {"composite",   DISPTYPE_TV,    TVPLUG_COMPOSITE},
+        {"scart",       DISPTYPE_TV,    TVPLUG_SCART},
+        {"\0",          -1,             -1}
+};
+
+/* Queue mode selection for 310 series */
+static const struct _XGI_queuemode {
+        char name[6];
+        int type_no;
+} XGI_queuemode[] = {
+        {"AGP",         AGP_CMD_QUEUE},
+        {"VRAM",        VM_CMD_QUEUE},
+        {"MMIO",        MMIO_CMD},
+        {"agp",         AGP_CMD_QUEUE},
+        {"vram",        VM_CMD_QUEUE},
+        {"mmio",        MMIO_CMD},
+        {"\0",          -1}
+};
+
+/* TV standard */
+static const struct _XGI_tvtype {
+        char name[6];
+        int type_no;
+} XGI_tvtype[] = {
+        {"PAL",         1},
+        {"NTSC",        2},
+        {"pal",         1},
+        {"ntsc",        2},
+        {"\0",          -1}
+};
+
+static const struct _XGI_vrate {
+        u16 idx;
+        u16 xres;
+        u16 yres;
+        u16 refresh;
+} XGIfb_vrate[] = {
+        {1,  640,  480, 60}, {2,  640,  480,  72}, {3, 640,   480,  75}, {4,  640, 480,  85},
+        {5,  640,  480,100}, {6,  640,  480, 120}, {7, 640,   480, 160}, {8,  640, 480, 200},
+        {1,  720,  480, 60},
+        {1,  720,  576, 58},
+        {1,  800,  480, 60}, {2,  800,  480,  75}, {3, 800,   480,  85},
+        {1,  800,  600,  60}, {2, 800,   600,  72}, {3,  800, 600,  75},
+        {4,  800,  600, 85}, {5,  800,  600, 100}, {6, 800,   600, 120}, {7,  800, 600, 160},
+        {1, 1024,  768,  60}, {2, 1024,  768,  70}, {3, 1024, 768,  75},
+        {4, 1024,  768, 85}, {5, 1024,  768, 100}, {6, 1024,  768, 120},
+        {1, 1024,  576, 60}, {2, 1024,  576,  75}, {3, 1024,  576,  85},
+        {1, 1024,  600, 60},
+        {1, 1152,  768, 60},
+        {1, 1280,  720, 60}, {2, 1280,  720,  75}, {3, 1280,  720,  85},
+        {1, 1280,  768, 60},
+        {1, 1280, 1024,  60}, {2, 1280, 1024,  75}, {3, 1280, 1024,  85},
+        {1, 1280,  960, 70},
+        {1, 1400, 1050, 60},
+        {1, 1600, 1200, 60}, {2, 1600, 1200,  65}, {3, 1600, 1200,  70}, {4, 1600, 1200,  75},
+        {5, 1600, 1200, 85}, {6, 1600, 1200, 100}, {7, 1600, 1200, 120},
+        {1, 1920, 1440, 60}, {2, 1920, 1440,  65}, {3, 1920, 1440,  70}, {4, 1920, 1440,  75},
+        {5, 1920, 1440, 85}, {6, 1920, 1440, 100},
+        {1, 2048, 1536, 60}, {2, 2048, 1536,  65}, {3, 2048, 1536,  70}, {4, 2048, 1536,  75},
+        {5, 2048, 1536, 85},
+        {0, 0, 0, 0}
+};
+
+static const struct _chswtable {
+    int subsysVendor;
+    int subsysCard;
+    char *vendorName;
+    char *cardName;
+} mychswtable[] = {
+        { 0x1631, 0x1002, "Mitachi", "0x1002" },
+        { 0,      0,      ""       , ""       }
+};
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+/* Offscreen layout */
+typedef struct _XGI_GLYINFO {
+        unsigned char ch;
+        int fontwidth;
+        int fontheight;
+        u8 gmask[72];
+        int ngmask;
+} XGI_GLYINFO;
+#endif
+
+typedef struct _XGI_OH {
+        struct _XGI_OH *poh_next;
+        struct _XGI_OH *poh_prev;
+        unsigned long offset;
+        unsigned long size;
+} XGI_OH;
+
+typedef struct _XGI_OHALLOC {
+        struct _XGI_OHALLOC *poha_next;
+        XGI_OH aoh[1];
+} XGI_OHALLOC;
+
+typedef struct _XGI_HEAP {
+        XGI_OH oh_free;
+        XGI_OH oh_used;
+        XGI_OH *poh_freelist;
+        XGI_OHALLOC *poha_chain;
+        unsigned long max_freesize;
+} XGI_HEAP;
+
+static unsigned long XGIfb_hwcursor_vbase;
+
+static unsigned long XGIfb_heap_start;
+static unsigned long XGIfb_heap_end;
+static unsigned long XGIfb_heap_size;
+static XGI_HEAP      XGIfb_heap;
+
+// Eden Chen
+static const struct _XGI_TV_filter {
+        u8 filter[9][4];
+} XGI_TV_filter[] = {
+        { {{0x00,0x00,0x00,0x40},  /* NTSCFilter_0 */
+           {0x00,0xE0,0x10,0x60},
+           {0x00,0xEE,0x10,0x44},
+           {0x00,0xF4,0x10,0x38},
+           {0xF8,0xF4,0x18,0x38},
+           {0xFC,0xFB,0x14,0x2A},
+           {0x00,0x00,0x10,0x20},
+           {0x00,0x04,0x10,0x18},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* NTSCFilter_1 */
+           {0x00,0xE0,0x10,0x60},
+           {0x00,0xEE,0x10,0x44},
+           {0x00,0xF4,0x10,0x38},
+           {0xF8,0xF4,0x18,0x38},
+           {0xFC,0xFB,0x14,0x2A},
+           {0x00,0x00,0x10,0x20},
+           {0x00,0x04,0x10,0x18},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* NTSCFilter_2 */
+           {0xF5,0xEE,0x1B,0x44},
+           {0xF8,0xF4,0x18,0x38},
+           {0xEB,0x04,0x25,0x18},
+           {0xF1,0x05,0x1F,0x16},
+           {0xF6,0x06,0x1A,0x14},
+           {0xFA,0x06,0x16,0x14},
+           {0x00,0x04,0x10,0x18},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* NTSCFilter_3 */
+           {0xF1,0x04,0x1F,0x18},
+           {0xEE,0x0D,0x22,0x06},
+           {0xF7,0x06,0x19,0x14},
+           {0xF4,0x0B,0x1C,0x0A},
+           {0xFA,0x07,0x16,0x12},
+           {0xF9,0x0A,0x17,0x0C},
+           {0x00,0x07,0x10,0x12},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* NTSCFilter_4 */
+           {0x00,0xE0,0x10,0x60},
+           {0x00,0xEE,0x10,0x44},
+           {0x00,0xF4,0x10,0x38},
+           {0xF8,0xF4,0x18,0x38},
+           {0xFC,0xFB,0x14,0x2A},
+           {0x00,0x00,0x10,0x20},
+           {0x00,0x04,0x10,0x18},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* NTSCFilter_5 */
+           {0xF5,0xEE,0x1B,0x44},
+           {0xF8,0xF4,0x18,0x38},
+           {0xEB,0x04,0x25,0x18},
+           {0xF1,0x05,0x1F,0x16},
+           {0xF6,0x06,0x1A,0x14},
+           {0xFA,0x06,0x16,0x14},
+           {0x00,0x04,0x10,0x18},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* NTSCFilter_6 */
+           {0xEB,0x04,0x25,0x18},
+           {0xE7,0x0E,0x29,0x04},
+           {0xEE,0x0C,0x22,0x08},
+           {0xF6,0x0B,0x1A,0x0A},
+           {0xF9,0x0A,0x17,0x0C},
+           {0xFC,0x0A,0x14,0x0C},
+           {0x00,0x08,0x10,0x10},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* NTSCFilter_7 */
+           {0xEC,0x02,0x24,0x1C},
+           {0xF2,0x04,0x1E,0x18},
+           {0xEB,0x15,0x25,0xF6},
+           {0xF4,0x10,0x1C,0x00},
+           {0xF8,0x0F,0x18,0x02},
+           {0x00,0x04,0x10,0x18},
+           {0x01,0x06,0x0F,0x14},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* PALFilter_0 */
+           {0x00,0xE0,0x10,0x60},
+           {0x00,0xEE,0x10,0x44},
+           {0x00,0xF4,0x10,0x38},
+           {0xF8,0xF4,0x18,0x38},
+           {0xFC,0xFB,0x14,0x2A},
+           {0x00,0x00,0x10,0x20},
+           {0x00,0x04,0x10,0x18},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* PALFilter_1 */
+           {0x00,0xE0,0x10,0x60},
+           {0x00,0xEE,0x10,0x44},
+           {0x00,0xF4,0x10,0x38},
+           {0xF8,0xF4,0x18,0x38},
+           {0xFC,0xFB,0x14,0x2A},
+           {0x00,0x00,0x10,0x20},
+           {0x00,0x04,0x10,0x18},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* PALFilter_2 */
+           {0xF5,0xEE,0x1B,0x44},
+           {0xF8,0xF4,0x18,0x38},
+           {0xF1,0xF7,0x01,0x32},
+           {0xF5,0xFB,0x1B,0x2A},
+           {0xF9,0xFF,0x17,0x22},
+           {0xFB,0x01,0x15,0x1E},
+           {0x00,0x04,0x10,0x18},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* PALFilter_3 */
+           {0xF5,0xFB,0x1B,0x2A},
+           {0xEE,0xFE,0x22,0x24},
+           {0xF3,0x00,0x1D,0x20},
+           {0xF9,0x03,0x17,0x1A},
+           {0xFB,0x02,0x14,0x1E},
+           {0xFB,0x04,0x15,0x18},
+           {0x00,0x06,0x10,0x14},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* PALFilter_4 */
+           {0x00,0xE0,0x10,0x60},
+           {0x00,0xEE,0x10,0x44},
+           {0x00,0xF4,0x10,0x38},
+           {0xF8,0xF4,0x18,0x38},
+           {0xFC,0xFB,0x14,0x2A},
+           {0x00,0x00,0x10,0x20},
+           {0x00,0x04,0x10,0x18},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* PALFilter_5 */
+           {0xF5,0xEE,0x1B,0x44},
+           {0xF8,0xF4,0x18,0x38},
+           {0xF1,0xF7,0x1F,0x32},
+           {0xF5,0xFB,0x1B,0x2A},
+           {0xF9,0xFF,0x17,0x22},
+           {0xFB,0x01,0x15,0x1E},
+           {0x00,0x04,0x10,0x18},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* PALFilter_6 */
+           {0xF5,0xEE,0x1B,0x2A},
+           {0xEE,0xFE,0x22,0x24},
+           {0xF3,0x00,0x1D,0x20},
+           {0xF9,0x03,0x17,0x1A},
+           {0xFB,0x02,0x14,0x1E},
+           {0xFB,0x04,0x15,0x18},
+           {0x00,0x06,0x10,0x14},
+           {0xFF,0xFF,0xFF,0xFF} }},
+        { {{0x00,0x00,0x00,0x40},  /* PALFilter_7 */
+           {0xF5,0xEE,0x1B,0x44},
+           {0xF8,0xF4,0x18,0x38},
+           {0xFC,0xFB,0x14,0x2A},
+           {0xEB,0x05,0x25,0x16},
+           {0xF1,0x05,0x1F,0x16},
+           {0xFA,0x07,0x16,0x12},
+           {0x00,0x07,0x10,0x12},
+           {0xFF,0xFF,0xFF,0xFF} }}
+};
+
+static int           filter = -1;
+static unsigned char filter_tb;
+
+
+/* ---------------------- Routine prototypes ------------------------- */
+
+/* Interface used by the world */
+#ifndef MODULE
+XGIINITSTATIC int __init XGIfb_setup(char *options);
+#endif
+
+/* Interface to the low level console driver */
+
+
+
+/* fbdev routines */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+       int      XGIfb_init(void);
+static int      XGIfb_get_fix(struct fb_fix_screeninfo *fix,
+                              int con,
+                              struct fb_info *info);
+static int      XGIfb_get_var(struct fb_var_screeninfo *var,
+                              int con,
+                              struct fb_info *info);
+static int      XGIfb_set_var(struct fb_var_screeninfo *var,
+                              int con,
+                              struct fb_info *info);
+static void     XGIfb_crtc_to_var(struct fb_var_screeninfo *var);
+static int      XGIfb_get_cmap(struct fb_cmap *cmap,
+                               int kspc,
+                               int con,
+                               struct fb_info *info);
+static int      XGIfb_set_cmap(struct fb_cmap *cmap,
+                               int kspc,
+                               int con,
+                               struct fb_info *info);
+static int      XGIfb_update_var(int con,
+                                 struct fb_info *info);
+static int      XGIfb_switch(int con,
+                             struct fb_info *info);
+static void     XGIfb_blank(int blank,
+                            struct fb_info *info);
+static void     XGIfb_set_disp(int con,
+                               struct fb_var_screeninfo *var,
+                               struct fb_info *info);
+static int      XGI_getcolreg(unsigned regno, unsigned *red, unsigned *green,
+                              unsigned *blue, unsigned *transp,
+                              struct fb_info *fb_info);
+static void     XGIfb_do_install_cmap(int con,
+                                      struct fb_info *info);
+static void     XGI_get_glyph(struct fb_info *info,
+                              XGI_GLYINFO *gly);
+static int      XGIfb_mmap(struct fb_info *info, struct file *file,
+                           struct vm_area_struct *vma);
+static int      XGIfb_ioctl(struct inode *inode, struct file *file,
+                            unsigned int cmd, unsigned long arg, int con,
+                            struct fb_info *info);
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+XGIINITSTATIC int __init xgifb_init(void);
+static int      XGIfb_set_par(struct fb_info *info);
+static int      XGIfb_blank(int blank,
+                            struct fb_info *info);
+/*static int    XGIfb_mmap(struct fb_info *info, struct file *file,
+                           struct vm_area_struct *vma);
+*/
+extern void     fbcon_XGI_fillrect(struct fb_info *info,
+                                   const struct fb_fillrect *rect);
+extern void     fbcon_XGI_copyarea(struct fb_info *info,
+                                   const struct fb_copyarea *area);
+#if 0
+extern void     cfb_imageblit(struct fb_info *info,
+                              const struct fb_image *image);
+#endif
+extern int      fbcon_XGI_sync(struct fb_info *info);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,15)
+static int XGIfb_ioctl(struct fb_info *info, unsigned int cmd,
+                            unsigned long arg);
+#else
+static int      XGIfb_ioctl(struct inode *inode,
+                            struct file *file,
+                            unsigned int cmd,
+                            unsigned long arg,
+                            struct fb_info *info);
+#endif
+
+/*
+extern int      XGIfb_mode_rate_to_dclock(VB_DEVICE_INFO *XGI_Pr,
+                              PXGI_HW_DEVICE_INFO HwDeviceExtension,
+                              unsigned char modeno, unsigned char rateindex);
+extern int      XGIfb_mode_rate_to_ddata(VB_DEVICE_INFO *XGI_Pr, PXGI_HW_DEVICE_INFO HwDeviceExtension,
+                         unsigned char modeno, unsigned char rateindex,
+                         unsigned int *left_margin, unsigned int *right_margin,
+                         unsigned int *upper_margin, unsigned int *lower_margin,
+                         unsigned int *hsync_len, unsigned int *vsync_len,
+                         unsigned int *sync, unsigned int *vmode);
+*/
+#endif
+                        extern   BOOLEAN  XGI_SearchModeID( USHORT ModeNo,USHORT  *ModeIdIndex, PVB_DEVICE_INFO );
+static int      XGIfb_get_fix(struct fb_fix_screeninfo *fix, int con,
+                              struct fb_info *info);
+
+/* Internal 2D accelerator functions */
+extern int      XGIfb_initaccel(void);
+extern void     XGIfb_syncaccel(void);
+
+/* Internal general routines */
+static void     XGIfb_search_mode(const char *name);
+static int      XGIfb_validate_mode(int modeindex);
+static u8       XGIfb_search_refresh_rate(unsigned int rate);
+static int      XGIfb_setcolreg(unsigned regno, unsigned red, unsigned green,
+                        unsigned blue, unsigned transp,
+                        struct fb_info *fb_info);
+static int      XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
+                        struct fb_info *info);
+static void     XGIfb_pre_setmode(void);
+static void     XGIfb_post_setmode(void);
+
+static BOOLEAN  XGIfb_CheckVBRetrace(void);
+static BOOLEAN  XGIfbcheckvretracecrt2(void);
+static BOOLEAN  XGIfbcheckvretracecrt1(void);
+static BOOLEAN  XGIfb_bridgeisslave(void);
+
+struct XGI_memreq {
+        unsigned long offset;
+        unsigned long size;
+};
+
+/* XGI-specific Export functions */
+void            XGI_dispinfo(struct ap_data *rec);
+void            XGI_malloc(struct XGI_memreq *req);
+void            XGI_free(unsigned long base);
+
+/* Internal hardware access routines */
+void            XGIfb_set_reg4(u16 port, unsigned long data);
+u32             XGIfb_get_reg3(u16 port);
+
+/* Chipset-dependent internal routines */
+
+
+static int      XGIfb_get_dram_size(void);
+static void     XGIfb_detect_VB(void);
+static void     XGIfb_get_VB_type(void);
+static int      XGIfb_has_VB(void);
+
+
+/* Internal heap routines */
+static int      XGIfb_heap_init(void);
+static XGI_OH   *XGIfb_poh_new_node(void);
+static XGI_OH   *XGIfb_poh_allocate(unsigned long size);
+static void     XGIfb_delete_node(XGI_OH *poh);
+static void     XGIfb_insert_node(XGI_OH *pohList, XGI_OH *poh);
+static XGI_OH   *XGIfb_poh_free(unsigned long base);
+static void     XGIfb_free_node(XGI_OH *poh);
+
+/* Internal routines to access PCI configuration space */
+BOOLEAN         XGIfb_query_VGA_config_space(PXGI_HW_DEVICE_INFO pXGIhw_ext,
+                        unsigned long offset, unsigned long set, unsigned long *value);
+//BOOLEAN         XGIfb_query_north_bridge_space(PXGI_HW_DEVICE_INFO pXGIhw_ext,
+//                      unsigned long offset, unsigned long set, unsigned long *value);
+
+
+/* Routines from init.c/init301.c */
+extern void     InitTo330Pointer(UCHAR,PVB_DEVICE_INFO pVBInfo);
+extern BOOLEAN  XGIInitNew(PXGI_HW_DEVICE_INFO HwDeviceExtension);
+extern BOOLEAN  XGISetModeNew(PXGI_HW_DEVICE_INFO HwDeviceExtension, USHORT ModeNo);
+//extern void     XGI_SetEnableDstn(VB_DEVICE_INFO *XGI_Pr);
+extern void     XGI_LongWait(VB_DEVICE_INFO *XGI_Pr);
+extern USHORT   XGI_GetRatePtrCRT2( PXGI_HW_DEVICE_INFO pXGIHWDE, USHORT ModeNo,USHORT ModeIdIndex,PVB_DEVICE_INFO pVBInfo );
+/* TW: Chrontel TV functions */
+extern USHORT   XGI_GetCH700x(VB_DEVICE_INFO *XGI_Pr, USHORT tempbx);
+extern void     XGI_SetCH700x(VB_DEVICE_INFO *XGI_Pr, USHORT tempbx);
+extern USHORT   XGI_GetCH701x(VB_DEVICE_INFO *XGI_Pr, USHORT tempbx);
+extern void     XGI_SetCH701x(VB_DEVICE_INFO *XGI_Pr, USHORT tempbx);
+extern void     XGI_SetCH70xxANDOR(VB_DEVICE_INFO *XGI_Pr, USHORT tempax,USHORT tempbh);
+extern void     XGI_DDC2Delay(VB_DEVICE_INFO *XGI_Pr, USHORT delaytime);
+
+/* TW: Sensing routines */
+void            XGI_Sense30x(void);
+int             XGIDoSense(int tempbl, int tempbh, int tempcl, int tempch);
+
+extern XGI21_LVDSCapStruct XGI21_LCDCapList[13];
+#endif
diff --git a/drivers/staging/xgifb/XGI_main_26.c b/drivers/staging/xgifb/XGI_main_26.c
new file mode 100644
index 000000000000..867012b48a01
--- /dev/null
+++ b/drivers/staging/xgifb/XGI_main_26.c
@@ -0,0 +1,3773 @@
+/*
+ * XG20, XG21, XG40, XG42 frame buffer device
+ * for Linux kernels  2.5.x, 2.6.x
+ * Base on TW's sis fbdev code.
+ */
+
+//#include <linux/config.h>
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/tty.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/console.h>
+#include <linux/selection.h>
+#include <linux/ioport.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/vmalloc.h>
+#include <linux/vt_kern.h>
+#include <linux/capability.h>
+#include <linux/fs.h>
+#include <linux/types.h>
+#include <linux/proc_fs.h>
+#include <linux/kernel.h>
+
+#include "osdef.h"
+
+
+#ifndef XGIFB_PAN
+#define XGIFB_PAN
+#endif
+
+#include <asm/io.h>
+#ifdef CONFIG_MTRR
+#include <asm/mtrr.h>
+#endif
+
+#include "XGIfb.h"
+#include "vgatypes.h"
+#include "XGI_main.h"
+#include "vb_util.h"
+
+
+#define Index_CR_GPIO_Reg1 0x48
+#define Index_CR_GPIO_Reg2 0x49
+#define Index_CR_GPIO_Reg3 0x4a
+
+#define GPIOG_EN    (1<<6)
+#define GPIOG_WRITE (1<<6)
+#define GPIOG_READ  (1<<1)
+int XGIfb_GetXG21DefaultLVDSModeIdx(void);
+
+/* -------------------- Macro definitions ---------------------------- */
+
+#undef XGIFBDEBUG
+
+#ifdef XGIFBDEBUG
+#define DPRINTK(fmt, args...) printk(KERN_DEBUG "%s: " fmt, __FUNCTION__ , ## args)
+#else
+#define DPRINTK(fmt, args...)
+#endif
+
+#ifdef XGIFBDEBUG
+static void dumpVGAReg(void)
+{
+    u8 i,reg;
+
+outXGIIDXREG(XGISR, 0x05, 0x86);
+/*
+outXGIIDXREG(XGISR, 0x08, 0x4f);
+outXGIIDXREG(XGISR, 0x0f, 0x20);
+outXGIIDXREG(XGISR, 0x11, 0x4f);
+outXGIIDXREG(XGISR, 0x13, 0x45);
+outXGIIDXREG(XGISR, 0x14, 0x51);
+outXGIIDXREG(XGISR, 0x1e, 0x41);
+outXGIIDXREG(XGISR, 0x1f, 0x0);
+outXGIIDXREG(XGISR, 0x20, 0xa1);
+outXGIIDXREG(XGISR, 0x22, 0xfb);
+outXGIIDXREG(XGISR, 0x26, 0x22);
+outXGIIDXREG(XGISR, 0x3e, 0x07);
+*/
+
+//outXGIIDXREG(XGICR, 0x19, 0x00);
+//outXGIIDXREG(XGICR, 0x1a, 0x3C);
+//outXGIIDXREG(XGICR, 0x22, 0xff);
+//outXGIIDXREG(XGICR, 0x3D, 0x10);
+
+//outXGIIDXREG(XGICR, 0x4a, 0xf3);
+
+//outXGIIDXREG(XGICR, 0x57, 0x0);
+//outXGIIDXREG(XGICR, 0x7a, 0x2c);
+
+//outXGIIDXREG(XGICR, 0x82, 0xcc);
+//outXGIIDXREG(XGICR, 0x8c, 0x0);
+/*
+outXGIIDXREG(XGICR, 0x99, 0x1);
+outXGIIDXREG(XGICR, 0x41, 0x40);
+*/
+
+    for(i=0; i < 0x4f; i++)
+    {
+        inXGIIDXREG(XGISR, i, reg);
+        printk("\no 3c4 %x",i);
+        printk("\ni 3c5 => %x",reg);
+    }
+
+    for(i=0; i < 0xF0; i++)
+    {
+        inXGIIDXREG(XGICR, i, reg);
+        printk("\no 3d4 %x",i);
+        printk("\ni 3d5 => %x",reg);
+    }
+/*
+
+    outXGIIDXREG(XGIPART1,0x2F,1);
+    for(i=1; i < 0x50; i++)
+    {
+        inXGIIDXREG(XGIPART1, i, reg);
+        printk("\no d004 %x",i);
+        printk("\ni d005 => %x",reg);
+    }
+
+    for(i=0; i < 0x50; i++)
+    {
+        inXGIIDXREG(XGIPART2, i, reg);
+        printk("\no d010 %x",i);
+        printk("\ni d011 => %x",reg);
+    }
+    for(i=0; i < 0x50; i++)
+    {
+        inXGIIDXREG(XGIPART3, i, reg);
+        printk("\no d012 %x",i);
+        printk("\ni d013 => %x",reg);
+    }
+    for(i=0; i < 0x50; i++)
+    {
+        inXGIIDXREG(XGIPART4, i, reg);
+        printk("\no d014 %x",i);
+        printk("\ni d015 => %x",reg);
+    }
+*/
+}
+#else
+static inline void dumpVGAReg(void) {}
+#endif
+
+/* data for XGI components */
+struct video_info  xgi_video_info;
+
+
+#if 1
+#define DEBUGPRN(x)
+#else
+#define DEBUGPRN(x) printk(KERN_INFO x "\n");
+#endif
+
+
+/* --------------- Hardware Access Routines -------------------------- */
+
+#ifdef LINUX_KERNEL
+int
+XGIfb_mode_rate_to_dclock(VB_DEVICE_INFO *XGI_Pr, PXGI_HW_DEVICE_INFO HwDeviceExtension,
+                          unsigned char modeno, unsigned char rateindex)
+{
+    USHORT ModeNo = modeno;
+    USHORT ModeIdIndex = 0, ClockIndex = 0;
+    USHORT RefreshRateTableIndex = 0;
+
+    /*ULONG  temp = 0;*/
+    int    Clock;
+    XGI_Pr->ROMAddr  = HwDeviceExtension->pjVirtualRomBase;
+    InitTo330Pointer( HwDeviceExtension->jChipType, XGI_Pr ) ;
+
+    RefreshRateTableIndex = XGI_GetRatePtrCRT2( HwDeviceExtension, ModeNo , ModeIdIndex, XGI_Pr ) ;
+
+/*
+    temp = XGI_SearchModeID( ModeNo , &ModeIdIndex,  XGI_Pr ) ;
+    if(!temp) {
+        printk(KERN_ERR "Could not find mode %x\n", ModeNo);
+        return 65000;
+    }
+
+    RefreshRateTableIndex = XGI_Pr->EModeIDTable[ModeIdIndex].REFindex;
+    RefreshRateTableIndex += (rateindex - 1);
+
+*/
+    ClockIndex = XGI_Pr->RefIndex[RefreshRateTableIndex].Ext_CRTVCLK;
+    if(HwDeviceExtension->jChipType < XGI_315H) {
+       ClockIndex &= 0x3F;
+    }
+    Clock = XGI_Pr->VCLKData[ClockIndex].CLOCK * 1000 ;
+
+    return(Clock);
+}
+
+int
+XGIfb_mode_rate_to_ddata(VB_DEVICE_INFO *XGI_Pr, PXGI_HW_DEVICE_INFO HwDeviceExtension,
+                         unsigned char modeno, unsigned char rateindex,
+                         u32 *left_margin, u32 *right_margin,
+                         u32 *upper_margin, u32 *lower_margin,
+                         u32 *hsync_len, u32 *vsync_len,
+                         u32 *sync, u32 *vmode)
+{
+    USHORT ModeNo = modeno;
+    USHORT ModeIdIndex = 0, index = 0;
+    USHORT RefreshRateTableIndex = 0;
+
+    unsigned short VRE, VBE, VRS, VBS, VDE, VT;
+    unsigned short HRE, HBE, HRS, HBS, HDE, HT;
+    unsigned char  sr_data, cr_data, cr_data2;
+    unsigned long cr_data3;
+    int            A, B, C, D, E, F, temp, j;
+    XGI_Pr->ROMAddr  = HwDeviceExtension->pjVirtualRomBase;
+    InitTo330Pointer( HwDeviceExtension->jChipType, XGI_Pr ) ;
+  RefreshRateTableIndex = XGI_GetRatePtrCRT2( HwDeviceExtension, ModeNo , ModeIdIndex, XGI_Pr ) ;
+/*
+    temp = XGI_SearchModeID( ModeNo, &ModeIdIndex, XGI_Pr);
+    if(!temp) return 0;
+
+    RefreshRateTableIndex = XGI_Pr->EModeIDTable[ModeIdIndex].REFindex;
+    RefreshRateTableIndex += (rateindex - 1);
+*/
+    index = XGI_Pr->RefIndex[RefreshRateTableIndex].Ext_CRT1CRTC;
+
+    sr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[5];
+
+    cr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[0];
+
+    /* Horizontal total */
+    HT = (cr_data & 0xff) |
+         ((unsigned short) (sr_data & 0x03) << 8);
+    A = HT + 5;
+
+    /*cr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[1];
+
+     Horizontal display enable end
+    HDE = (cr_data & 0xff) |
+          ((unsigned short) (sr_data & 0x0C) << 6);*/
+    HDE = (XGI_Pr->RefIndex[RefreshRateTableIndex].XRes >> 3) -1;
+    E = HDE + 1;
+
+    cr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[3];
+
+    /* Horizontal retrace (=sync) start */
+    HRS = (cr_data & 0xff) |
+          ((unsigned short) (sr_data & 0xC0) << 2);
+    F = HRS - E - 3;
+
+    cr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[1];
+
+    /* Horizontal blank start */
+    HBS = (cr_data & 0xff) |
+          ((unsigned short) (sr_data & 0x30) << 4);
+
+    sr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[6];
+
+    cr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[2];
+
+    cr_data2 = XGI_Pr->XGINEWUB_CRT1Table[index].CR[4];
+
+    /* Horizontal blank end */
+    HBE = (cr_data & 0x1f) |
+          ((unsigned short) (cr_data2 & 0x80) >> 2) |
+          ((unsigned short) (sr_data & 0x03) << 6);
+
+    /* Horizontal retrace (=sync) end */
+    HRE = (cr_data2 & 0x1f) | ((sr_data & 0x04) << 3);
+
+    temp = HBE - ((E - 1) & 255);
+    B = (temp > 0) ? temp : (temp + 256);
+
+    temp = HRE - ((E + F + 3) & 63);
+    C = (temp > 0) ? temp : (temp + 64);
+
+    D = B - F - C;
+
+    *left_margin = D * 8;
+    *right_margin = F * 8;
+    *hsync_len = C * 8;
+
+    sr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[14];
+
+    cr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[8];
+
+    cr_data2 = XGI_Pr->XGINEWUB_CRT1Table[index].CR[9];
+
+    /* Vertical total */
+    VT = (cr_data & 0xFF) |
+         ((unsigned short) (cr_data2 & 0x01) << 8) |
+         ((unsigned short)(cr_data2 & 0x20) << 4) |
+         ((unsigned short) (sr_data & 0x01) << 10);
+    A = VT + 2;
+
+    //cr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[10];
+
+    /* Vertical display enable end */
+/*    VDE = (cr_data & 0xff) |
+          ((unsigned short) (cr_data2 & 0x02) << 7) |
+          ((unsigned short) (cr_data2 & 0x40) << 3) |
+          ((unsigned short) (sr_data & 0x02) << 9); */
+    VDE = XGI_Pr->RefIndex[RefreshRateTableIndex].YRes  -1;
+    E = VDE + 1;
+
+    cr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[10];
+
+    /* Vertical retrace (=sync) start */
+    VRS = (cr_data & 0xff) |
+          ((unsigned short) (cr_data2 & 0x04) << 6) |
+          ((unsigned short) (cr_data2 & 0x80) << 2) |
+          ((unsigned short) (sr_data & 0x08) << 7);
+    F = VRS + 1 - E;
+
+    cr_data =  XGI_Pr->XGINEWUB_CRT1Table[index].CR[12];
+
+    cr_data3 = (XGI_Pr->XGINEWUB_CRT1Table[index].CR[14] & 0x80) << 5;
+
+    /* Vertical blank start */
+    VBS = (cr_data & 0xff) |
+          ((unsigned short) (cr_data2 & 0x08) << 5) |
+          ((unsigned short) (cr_data3 & 0x20) << 4) |
+          ((unsigned short) (sr_data & 0x04) << 8);
+
+    cr_data =  XGI_Pr->XGINEWUB_CRT1Table[index].CR[13];
+
+    /* Vertical blank end */
+    VBE = (cr_data & 0xff) |
+          ((unsigned short) (sr_data & 0x10) << 4);
+    temp = VBE - ((E - 1) & 511);
+    B = (temp > 0) ? temp : (temp + 512);
+
+    cr_data = XGI_Pr->XGINEWUB_CRT1Table[index].CR[11];
+
+    /* Vertical retrace (=sync) end */
+    VRE = (cr_data & 0x0f) | ((sr_data & 0x20) >> 1);
+    temp = VRE - ((E + F - 1) & 31);
+    C = (temp > 0) ? temp : (temp + 32);
+
+    D = B - F - C;
+
+    *upper_margin = D;
+    *lower_margin = F;
+    *vsync_len = C;
+
+    if(XGI_Pr->RefIndex[RefreshRateTableIndex].Ext_InfoFlag & 0x8000)
+       *sync &= ~FB_SYNC_VERT_HIGH_ACT;
+    else
+       *sync |= FB_SYNC_VERT_HIGH_ACT;
+
+    if(XGI_Pr->RefIndex[RefreshRateTableIndex].Ext_InfoFlag & 0x4000)
+       *sync &= ~FB_SYNC_HOR_HIGH_ACT;
+    else
+       *sync |= FB_SYNC_HOR_HIGH_ACT;
+
+    *vmode = FB_VMODE_NONINTERLACED;
+    if(XGI_Pr->RefIndex[RefreshRateTableIndex].Ext_InfoFlag & 0x0080)
+       *vmode = FB_VMODE_INTERLACED;
+    else {
+      j = 0;
+      while(XGI_Pr->EModeIDTable[j].Ext_ModeID != 0xff) {
+          if(XGI_Pr->EModeIDTable[j].Ext_ModeID ==
+                          XGI_Pr->RefIndex[RefreshRateTableIndex].ModeID) {
+              if(XGI_Pr->EModeIDTable[j].Ext_ModeFlag & DoubleScanMode) {
+                  *vmode = FB_VMODE_DOUBLE;
+              }
+              break;
+          }
+          j++;
+      }
+    }
+
+#if 0  /* That's bullshit, only the resolution needs to be shifted */
+    if((*vmode & FB_VMODE_MASK) == FB_VMODE_INTERLACED) {
+       *upper_margin <<= 1;
+       *lower_margin <<= 1;
+       *vsync_len <<= 1;
+    } else if((*vmode & FB_VMODE_MASK) == FB_VMODE_DOUBLE) {
+       *upper_margin >>= 1;
+       *lower_margin >>= 1;
+       *vsync_len >>= 1;
+    }
+#endif
+
+    return 1;
+}
+
+#endif
+
+
+
+void XGIRegInit(VB_DEVICE_INFO *XGI_Pr, ULONG BaseAddr)
+{
+   XGI_Pr->RelIO = BaseAddr;
+   XGI_Pr->P3c4 = BaseAddr + 0x14;
+   XGI_Pr->P3d4 = BaseAddr + 0x24;
+   XGI_Pr->P3c0 = BaseAddr + 0x10;
+   XGI_Pr->P3ce = BaseAddr + 0x1e;
+   XGI_Pr->P3c2 = BaseAddr + 0x12;
+   XGI_Pr->P3ca = BaseAddr + 0x1a;
+   XGI_Pr->P3c6 = BaseAddr + 0x16;
+   XGI_Pr->P3c7 = BaseAddr + 0x17;
+   XGI_Pr->P3c8 = BaseAddr + 0x18;
+   XGI_Pr->P3c9 = BaseAddr + 0x19;
+   XGI_Pr->P3da = BaseAddr + 0x2A;
+   XGI_Pr->Part1Port = BaseAddr + XGI_CRT2_PORT_04;   /* Digital video interface registers (LCD) */
+   XGI_Pr->Part2Port = BaseAddr + XGI_CRT2_PORT_10;   /* 301 TV Encoder registers */
+   XGI_Pr->Part3Port = BaseAddr + XGI_CRT2_PORT_12;   /* 301 Macrovision registers */
+   XGI_Pr->Part4Port = BaseAddr + XGI_CRT2_PORT_14;   /* 301 VGA2 (and LCD) registers */
+   XGI_Pr->Part5Port = BaseAddr + XGI_CRT2_PORT_14+2; /* 301 palette address port registers */
+
+}
+
+
+void XGIfb_set_reg4(u16 port, unsigned long data)
+{
+        outl((u32) (data & 0xffffffff), port);
+}
+
+u32 XGIfb_get_reg3(u16 port)
+{
+        u32 data;
+
+        data = inl(port);
+        return (data);
+}
+
+/* ------------ Interface for init & mode switching code ------------- */
+
+BOOLEAN
+XGIfb_query_VGA_config_space(PXGI_HW_DEVICE_INFO pXGIhw_ext,
+        unsigned long offset, unsigned long set, unsigned long *value)
+{
+        static struct pci_dev *pdev = NULL;
+        static unsigned char init = 0, valid_pdev = 0;
+
+        if (!set)
+                DPRINTK("XGIfb: Get VGA offset 0x%lx\n", offset);
+        else
+                DPRINTK("XGIfb: Set offset 0x%lx to 0x%lx\n", offset, *value);
+
+        if (!init) {
+                init = TRUE;
+                pdev = pci_get_device(PCI_VENDOR_ID_XG, xgi_video_info.chip_id, pdev);
+                if (pdev) {
+                        valid_pdev = TRUE;
+                        pci_dev_put(pdev);
+                }
+        }
+
+        if (!valid_pdev) {
+                printk(KERN_DEBUG "XGIfb: Can't find XGI %d VGA device.\n",
+                                xgi_video_info.chip_id);
+                return FALSE;
+        }
+
+        if (set == 0)
+                pci_read_config_dword(pdev, offset, (u32 *)value);
+        else
+                pci_write_config_dword(pdev, offset, (u32)(*value));
+
+        return TRUE;
+}
+
+/*BOOLEAN XGIfb_query_north_bridge_space(PXGI_HW_DEVICE_INFO pXGIhw_ext,
+        unsigned long offset, unsigned long set, unsigned long *value)
+{
+        static struct pci_dev *pdev = NULL;
+        static unsigned char init = 0, valid_pdev = 0;
+        u16 nbridge_id = 0;
+
+        if (!init) {
+                init = TRUE;
+                switch (xgi_video_info.chip) {
+                case XGI_540:
+                        nbridge_id = PCI_DEVICE_ID_XG_540;
+                        break;
+                case XGI_630:
+                        nbridge_id = PCI_DEVICE_ID_XG_630;
+                        break;
+                case XGI_730:
+                        nbridge_id = PCI_DEVICE_ID_XG_730;
+                        break;
+                case XGI_550:
+                        nbridge_id = PCI_DEVICE_ID_XG_550;
+                        break;
+                case XGI_650:
+                        nbridge_id = PCI_DEVICE_ID_XG_650;
+                        break;
+                case XGI_740:
+                        nbridge_id = PCI_DEVICE_ID_XG_740;
+                        break;
+                default:
+                        nbridge_id = 0;
+                        break;
+                }
+
+                pdev = pci_find_device(PCI_VENDOR_ID_SI, nbridge_id, pdev);
+                if (pdev)
+                        valid_pdev = TRUE;
+        }
+
+        if (!valid_pdev) {
+                printk(KERN_DEBUG "XGIfb: Can't find XGI %d North Bridge device.\n",
+                                nbridge_id);
+                return FALSE;
+        }
+
+        if (set == 0)
+                pci_read_config_dword(pdev, offset, (u32 *)value);
+        else
+                pci_write_config_dword(pdev, offset, (u32)(*value));
+
+        return TRUE;
+}
+*/
+/* ------------------ Internal helper routines ----------------- */
+
+static void XGIfb_search_mode(const char *name)
+{
+        int i = 0, j = 0, l;
+
+        if(name == NULL) {
+           printk(KERN_ERR "XGIfb: Internal error, using default mode.\n");
+           xgifb_mode_idx = DEFAULT_MODE;
+           if ((xgi_video_info.chip == XG21) && ((xgi_video_info.disp_state & DISPTYPE_DISP2) == DISPTYPE_LCD))
+           {
+               xgifb_mode_idx = XGIfb_GetXG21DefaultLVDSModeIdx();
+           }
+           return;
+        }
+
+
+        if (!strcmp(name, XGIbios_mode[MODE_INDEX_NONE].name)) {
+           printk(KERN_ERR "XGIfb: Mode 'none' not supported anymore. Using default.\n");
+           xgifb_mode_idx = DEFAULT_MODE;
+           if ((xgi_video_info.chip == XG21) && ((xgi_video_info.disp_state & DISPTYPE_DISP2) == DISPTYPE_LCD))
+           {
+               xgifb_mode_idx = XGIfb_GetXG21DefaultLVDSModeIdx();
+           }
+           return;
+        }
+
+        while(XGIbios_mode[i].mode_no != 0) {
+                l = min(strlen(name), strlen(XGIbios_mode[i].name));
+                if (!strncmp(name, XGIbios_mode[i].name, l)) {
+                        xgifb_mode_idx = i;
+                        j = 1;
+                        break;
+                }
+                i++;
+        }
+        if(!j) printk(KERN_INFO "XGIfb: Invalid mode '%s'\n", name);
+}
+
+static void XGIfb_search_vesamode(unsigned int vesamode)
+{
+        int i = 0, j = 0;
+
+        if(vesamode == 0) {
+
+                printk(KERN_ERR "XGIfb: Mode 'none' not supported anymore. Using default.\n");
+                xgifb_mode_idx = DEFAULT_MODE;
+                if ((xgi_video_info.chip == XG21) && ((xgi_video_info.disp_state & DISPTYPE_DISP2) == DISPTYPE_LCD))
+                {
+                    xgifb_mode_idx = XGIfb_GetXG21DefaultLVDSModeIdx();
+                }
+                return;
+        }
+
+        vesamode &= 0x1dff;  /* Clean VESA mode number from other flags */
+
+        while(XGIbios_mode[i].mode_no != 0) {
+                if( (XGIbios_mode[i].vesa_mode_no_1 == vesamode) ||
+                    (XGIbios_mode[i].vesa_mode_no_2 == vesamode) ) {
+                        xgifb_mode_idx = i;
+                        j = 1;
+                        break;
+                }
+                i++;
+        }
+        if(!j) printk(KERN_INFO "XGIfb: Invalid VESA mode 0x%x'\n", vesamode);
+}
+
+int XGIfb_GetXG21LVDSData(void)
+{
+    u8 tmp;
+    unsigned char *pData;
+    int i,j,k;
+
+    inXGIIDXREG(XGISR,0x1e,tmp);
+    outXGIIDXREG(XGISR, 0x1e, tmp|4);
+
+    pData = xgi_video_info.mmio_vbase+0x20000;
+    if ((pData[0x0]==0x55) && (pData[0x1]==0xAA) && (pData[0x65] & 0x1))
+    {
+        i = pData[ 0x316 ] | ( pData[ 0x317 ] << 8 );
+        j = pData[ i-1 ] ;
+        if ( j == 0xff )
+        {
+          j = 1;
+        }
+        k = 0;
+        do
+        {
+                XGI21_LCDCapList[k].LVDS_Capability = pData[ i ] | ( pData[ i + 1 ] << 8 );
+                XGI21_LCDCapList[k].LVDSHT = pData[ i + 2 ] | ( pData[ i + 3 ] << 8 ) ;
+                XGI21_LCDCapList[k].LVDSVT = pData[ i + 4 ] | ( pData[ i + 5 ] << 8 );
+                XGI21_LCDCapList[k].LVDSHDE = pData[ i + 6 ] | ( pData[ i + 7 ] << 8 );
+                XGI21_LCDCapList[k].LVDSVDE = pData[ i + 8 ] | ( pData[ i + 9 ] << 8 );
+                XGI21_LCDCapList[k].LVDSHFP = pData[ i + 10 ] | ( pData[ i + 11 ] << 8 );
+                XGI21_LCDCapList[k].LVDSVFP = pData[ i + 12 ] | ( pData[ i + 13 ] << 8 );
+                XGI21_LCDCapList[k].LVDSHSYNC = pData[ i + 14 ] | ( pData[ i + 15 ] << 8 );
+                XGI21_LCDCapList[k].LVDSVSYNC = pData[ i + 16 ] | ( pData[ i + 17 ] << 8 );
+                XGI21_LCDCapList[k].VCLKData1 = pData[ i + 18 ] ;
+                XGI21_LCDCapList[k].VCLKData2 = pData[ i + 19 ] ;
+                XGI21_LCDCapList[k].PSC_S1 = pData[ i + 20 ] ;
+                XGI21_LCDCapList[k].PSC_S2 = pData[ i + 21 ] ;
+                XGI21_LCDCapList[k].PSC_S3 = pData[ i + 22 ] ;
+                XGI21_LCDCapList[k].PSC_S4 = pData[ i + 23 ] ;
+                XGI21_LCDCapList[k].PSC_S5 = pData[ i + 24 ] ;
+                i += 25;
+                j--;
+                k++;
+        } while ( (j>0) && ( k < (sizeof(XGI21_LCDCapList)/sizeof(XGI21_LVDSCapStruct)) ) );
+        return 1;
+    }
+    return 0;
+}
+
+int XGIfb_GetXG21DefaultLVDSModeIdx(void)
+{
+
+        int found_mode = 0;
+        int XGIfb_mode_idx = 0;
+
+        found_mode = 0;
+        while( (XGIbios_mode[XGIfb_mode_idx].mode_no != 0) &&
+               (XGIbios_mode[XGIfb_mode_idx].xres <= XGI21_LCDCapList[0].LVDSHDE) )
+        {
+                if( (XGIbios_mode[XGIfb_mode_idx].xres == XGI21_LCDCapList[0].LVDSHDE) &&
+                    (XGIbios_mode[XGIfb_mode_idx].yres == XGI21_LCDCapList[0].LVDSVDE) &&
+                    (XGIbios_mode[XGIfb_mode_idx].bpp == 8))
+                {
+                        XGIfb_mode_no = XGIbios_mode[XGIfb_mode_idx].mode_no;
+                        found_mode = 1;
+                        break;
+                }
+                XGIfb_mode_idx++;
+        }
+        if (!found_mode)
+          XGIfb_mode_idx = 0;
+
+        return (XGIfb_mode_idx);
+}
+
+
+static int XGIfb_validate_mode(int myindex)
+{
+   u16 xres, yres;
+
+    if (xgi_video_info.chip == XG21)
+    {
+        if ((xgi_video_info.disp_state & DISPTYPE_DISP2) == DISPTYPE_LCD)
+        {
+            xres = XGI21_LCDCapList[0].LVDSHDE;
+            yres = XGI21_LCDCapList[0].LVDSVDE;
+            if(XGIbios_mode[myindex].xres > xres)
+                return(-1);
+            if(XGIbios_mode[myindex].yres > yres)
+                return(-1);
+            if ((XGIbios_mode[myindex].xres < xres) && (XGIbios_mode[myindex].yres < yres) )
+            {
+              if (XGIbios_mode[myindex].bpp > 8)
+                  return(-1);
+            }
+
+       }
+       return(myindex);
+
+    }
+
+    /* FIXME: for now, all is valid on XG27 */
+    if (xgi_video_info.chip == XG27)
+            return(myindex);
+
+    if(!(XGIbios_mode[myindex].chipset & MD_XGI315))
+        return(-1);
+
+   switch (xgi_video_info.disp_state & DISPTYPE_DISP2) {
+     case DISPTYPE_LCD:
+        switch (XGIhw_ext.ulCRT2LCDType) {
+        case LCD_640x480:
+                xres =  640; yres =  480;  break;
+        case LCD_800x600:
+                xres =  800; yres =  600;  break;
+        case LCD_1024x600:
+                xres = 1024; yres =  600;  break;
+        case LCD_1024x768:
+                xres = 1024; yres =  768;  break;
+        case LCD_1152x768:
+                xres = 1152; yres =  768;  break;
+        case LCD_1280x960:
+                xres = 1280; yres =  960;  break;
+        case LCD_1280x768:
+                xres = 1280; yres =  768;  break;
+        case LCD_1280x1024:
+                xres = 1280; yres = 1024;  break;
+        case LCD_1400x1050:
+                xres = 1400; yres = 1050;  break;
+        case LCD_1600x1200:
+                xres = 1600; yres = 1200;  break;
+//      case LCD_320x480:                               // TW: FSTN
+//              xres =  320; yres =  480;  break;
+        default:
+                xres =    0; yres =    0;  break;
+        }
+        if(XGIbios_mode[myindex].xres > xres) {
+                return(-1);
+        }
+        if(XGIbios_mode[myindex].yres > yres) {
+                return(-1);
+        }
+        if((XGIhw_ext.ulExternalChip == 0x01) ||   // LVDS
+           (XGIhw_ext.ulExternalChip == 0x05))    // LVDS+Chrontel
+        {
+           switch (XGIbios_mode[myindex].xres) {
+                case 512:
+                        if(XGIbios_mode[myindex].yres != 512) return -1;
+                        if(XGIhw_ext.ulCRT2LCDType == LCD_1024x600) return -1;
+                        break;
+                case 640:
+                        if((XGIbios_mode[myindex].yres != 400) &&
+                           (XGIbios_mode[myindex].yres != 480))
+                                return -1;
+                        break;
+                case 800:
+                        if(XGIbios_mode[myindex].yres != 600) return -1;
+                        break;
+                case 1024:
+                        if((XGIbios_mode[myindex].yres != 600) &&
+                           (XGIbios_mode[myindex].yres != 768))
+                                return -1;
+                        if((XGIbios_mode[myindex].yres == 600) &&
+                           (XGIhw_ext.ulCRT2LCDType != LCD_1024x600))
+                                return -1;
+                        break;
+                case 1152:
+                        if((XGIbios_mode[myindex].yres) != 768) return -1;
+                        if(XGIhw_ext.ulCRT2LCDType != LCD_1152x768) return -1;
+                        break;
+                case 1280:
+                        if((XGIbios_mode[myindex].yres != 768) &&
+                           (XGIbios_mode[myindex].yres != 1024))
+                                return -1;
+                        if((XGIbios_mode[myindex].yres == 768) &&
+                           (XGIhw_ext.ulCRT2LCDType != LCD_1280x768))
+                                return -1;
+                        break;
+                case 1400:
+                        if(XGIbios_mode[myindex].yres != 1050) return -1;
+                        break;
+                case 1600:
+                        if(XGIbios_mode[myindex].yres != 1200) return -1;
+                        break;
+                default:
+                        return -1;
+           }
+        } else {
+           switch (XGIbios_mode[myindex].xres) {
+                case 512:
+                        if(XGIbios_mode[myindex].yres != 512) return -1;
+                        break;
+                case 640:
+                        if((XGIbios_mode[myindex].yres != 400) &&
+                           (XGIbios_mode[myindex].yres != 480))
+                                return -1;
+                        break;
+                case 800:
+                        if(XGIbios_mode[myindex].yres != 600) return -1;
+                        break;
+                case 1024:
+                        if(XGIbios_mode[myindex].yres != 768) return -1;
+                        break;
+                case 1280:
+                        if((XGIbios_mode[myindex].yres != 960) &&
+                           (XGIbios_mode[myindex].yres != 1024))
+                                return -1;
+                        if(XGIbios_mode[myindex].yres == 960) {
+                            if(XGIhw_ext.ulCRT2LCDType == LCD_1400x1050)
+                                return -1;
+                        }
+                        break;
+                case 1400:
+                        if(XGIbios_mode[myindex].yres != 1050) return -1;
+                        break;
+                case 1600:
+                        if(XGIbios_mode[myindex].yres != 1200) return -1;
+                        break;
+                default:
+                        return -1;
+           }
+        }
+        break;
+     case DISPTYPE_TV:
+        switch (XGIbios_mode[myindex].xres) {
+        case 512:
+        case 640:
+        case 800:
+                break;
+        case 720:
+                if (xgi_video_info.TV_type == TVMODE_NTSC) {
+                        if (XGIbios_mode[myindex].yres != 480) {
+                                return(-1);
+                        }
+                } else if (xgi_video_info.TV_type == TVMODE_PAL) {
+                        if (XGIbios_mode[myindex].yres != 576) {
+                                return(-1);
+                        }
+                }
+                // TW: LVDS/CHRONTEL does not support 720
+                if (xgi_video_info.hasVB == HASVB_LVDS_CHRONTEL ||
+                                        xgi_video_info.hasVB == HASVB_CHRONTEL) {
+                                return(-1);
+                }
+                break;
+        case 1024:
+                if (xgi_video_info.TV_type == TVMODE_NTSC) {
+                        if(XGIbios_mode[myindex].bpp == 32) {
+                               return(-1);
+                        }
+                }
+                // TW: LVDS/CHRONTEL only supports < 800 (1024 on 650/Ch7019)
+                if (xgi_video_info.hasVB == HASVB_LVDS_CHRONTEL ||
+                                        xgi_video_info.hasVB == HASVB_CHRONTEL) {
+                    if(xgi_video_info.chip < XGI_315H) {
+                                return(-1);
+                    }
+                }
+                break;
+        default:
+                return(-1);
+        }
+        break;
+     case DISPTYPE_CRT2:
+        if(XGIbios_mode[myindex].xres > 1280) return -1;
+        break;
+     }
+     return(myindex);
+
+}
+
+static void XGIfb_search_crt2type(const char *name)
+{
+        int i = 0;
+
+        if(name == NULL)
+                return;
+
+        while(XGI_crt2type[i].type_no != -1) {
+                if (!strcmp(name, XGI_crt2type[i].name)) {
+                        XGIfb_crt2type = XGI_crt2type[i].type_no;
+                        XGIfb_tvplug = XGI_crt2type[i].tvplug_no;
+                        break;
+                }
+                i++;
+        }
+        if(XGIfb_crt2type < 0)
+                printk(KERN_INFO "XGIfb: Invalid CRT2 type: %s\n", name);
+}
+
+static void XGIfb_search_queuemode(const char *name)
+{
+        int i = 0;
+
+        if(name == NULL)
+                return;
+
+        while (XGI_queuemode[i].type_no != -1) {
+                if (!strcmp(name, XGI_queuemode[i].name)) {
+                        XGIfb_queuemode = XGI_queuemode[i].type_no;
+                        break;
+                }
+                i++;
+        }
+        if (XGIfb_queuemode < 0)
+                printk(KERN_INFO "XGIfb: Invalid queuemode type: %s\n", name);
+}
+
+static u8 XGIfb_search_refresh_rate(unsigned int rate)
+{
+        u16 xres, yres;
+        int i = 0;
+
+        xres = XGIbios_mode[xgifb_mode_idx].xres;
+        yres = XGIbios_mode[xgifb_mode_idx].yres;
+
+        XGIfb_rate_idx = 0;
+        while ((XGIfb_vrate[i].idx != 0) && (XGIfb_vrate[i].xres <= xres)) {
+                if ((XGIfb_vrate[i].xres == xres) && (XGIfb_vrate[i].yres == yres)) {
+                        if (XGIfb_vrate[i].refresh == rate) {
+                                XGIfb_rate_idx = XGIfb_vrate[i].idx;
+                                break;
+                        } else if (XGIfb_vrate[i].refresh > rate) {
+                                if ((XGIfb_vrate[i].refresh - rate) <= 3) {
+                                        DPRINTK("XGIfb: Adjusting rate from %d up to %d\n",
+                                                rate, XGIfb_vrate[i].refresh);
+                                        XGIfb_rate_idx = XGIfb_vrate[i].idx;
+                                        xgi_video_info.refresh_rate = XGIfb_vrate[i].refresh;
+                                } else if (((rate - XGIfb_vrate[i-1].refresh) <= 2)
+                                                && (XGIfb_vrate[i].idx != 1)) {
+                                        DPRINTK("XGIfb: Adjusting rate from %d down to %d\n",
+                                                rate, XGIfb_vrate[i-1].refresh);
+                                        XGIfb_rate_idx = XGIfb_vrate[i-1].idx;
+                                        xgi_video_info.refresh_rate = XGIfb_vrate[i-1].refresh;
+                                }
+                                break;
+                        } else if((rate - XGIfb_vrate[i].refresh) <= 2) {
+                                DPRINTK("XGIfb: Adjusting rate from %d down to %d\n",
+                                                rate, XGIfb_vrate[i].refresh);
+                                XGIfb_rate_idx = XGIfb_vrate[i].idx;
+                                break;
+                        }
+                }
+                i++;
+        }
+        if (XGIfb_rate_idx > 0) {
+                return XGIfb_rate_idx;
+        } else {
+                printk(KERN_INFO
+                        "XGIfb: Unsupported rate %d for %dx%d\n", rate, xres, yres);
+                return 0;
+        }
+}
+
+static void XGIfb_search_tvstd(const char *name)
+{
+        int i = 0;
+
+        if(name == NULL)
+                return;
+
+        while (XGI_tvtype[i].type_no != -1) {
+                if (!strcmp(name, XGI_tvtype[i].name)) {
+                        XGIfb_tvmode = XGI_tvtype[i].type_no;
+                        break;
+                }
+                i++;
+        }
+}
+
+static BOOLEAN XGIfb_bridgeisslave(void)
+{
+   unsigned char usScratchP1_00;
+
+   if(xgi_video_info.hasVB == HASVB_NONE) return FALSE;
+
+   inXGIIDXREG(XGIPART1,0x00,usScratchP1_00);
+   if( (usScratchP1_00 & 0x50) == 0x10)  {
+           return TRUE;
+   } else {
+           return FALSE;
+   }
+}
+
+static BOOLEAN XGIfbcheckvretracecrt1(void)
+{
+   unsigned char temp;
+
+   inXGIIDXREG(XGICR,0x17,temp);
+   if(!(temp & 0x80)) return FALSE;
+
+
+   inXGIIDXREG(XGISR,0x1f,temp);
+   if(temp & 0xc0) return FALSE;
+
+
+   if(inXGIREG(XGIINPSTAT) & 0x08) return TRUE;
+   else                            return FALSE;
+}
+
+static BOOLEAN XGIfbcheckvretracecrt2(void)
+{
+   unsigned char temp;
+   if(xgi_video_info.hasVB == HASVB_NONE) return FALSE;
+   inXGIIDXREG(XGIPART1, 0x30, temp);
+   if(temp & 0x02) return FALSE;
+   else            return TRUE;
+}
+
+static BOOLEAN XGIfb_CheckVBRetrace(void)
+{
+   if(xgi_video_info.disp_state & DISPTYPE_DISP2) {
+      if(XGIfb_bridgeisslave()) {
+         return(XGIfbcheckvretracecrt1());
+      } else {
+         return(XGIfbcheckvretracecrt2());
+      }
+   }
+   return(XGIfbcheckvretracecrt1());
+}
+
+/* ----------- FBDev related routines for all series ----------- */
+
+
+static void XGIfb_bpp_to_var(struct fb_var_screeninfo *var)
+{
+        switch(var->bits_per_pixel) {
+           case 8:
+                var->red.offset = var->green.offset = var->blue.offset = 0;
+                var->red.length = var->green.length = var->blue.length = 6;
+                xgi_video_info.video_cmap_len = 256;
+                break;
+           case 16:
+                var->red.offset = 11;
+                var->red.length = 5;
+                var->green.offset = 5;
+                var->green.length = 6;
+                var->blue.offset = 0;
+                var->blue.length = 5;
+                var->transp.offset = 0;
+                var->transp.length = 0;
+                xgi_video_info.video_cmap_len = 16;
+                break;
+           case 32:
+                var->red.offset = 16;
+                var->red.length = 8;
+                var->green.offset = 8;
+                var->green.length = 8;
+                var->blue.offset = 0;
+                var->blue.length = 8;
+                var->transp.offset = 24;
+                var->transp.length = 8;
+                xgi_video_info.video_cmap_len = 16;
+                break;
+        }
+}
+
+
+
+static int XGIfb_do_set_var(struct fb_var_screeninfo *var, int isactive,
+                      struct fb_info *info)
+{
+
+        unsigned int htotal = var->left_margin + var->xres +
+                var->right_margin + var->hsync_len;
+        unsigned int vtotal = var->upper_margin + var->yres +
+                var->lower_margin + var->vsync_len;
+#if defined(__powerpc__)
+        u8 sr_data, cr_data;
+#endif
+        unsigned int drate = 0, hrate = 0;
+        int found_mode = 0;
+        int old_mode;
+//      unsigned char reg,reg1;
+
+        DEBUGPRN("Inside do_set_var");
+//        printk(KERN_DEBUG "XGIfb:var->yres=%d, var->upper_margin=%d, var->lower_margin=%d, var->vsync_len=%d\n", var->yres,var->upper_margin,var->lower_margin,var->vsync_len);
+
+        info->var.xres_virtual = var->xres_virtual;
+        info->var.yres_virtual = var->yres_virtual;
+        info->var.bits_per_pixel = var->bits_per_pixel;
+
+        if ((var->vmode & FB_VMODE_MASK) == FB_VMODE_NONINTERLACED)
+                vtotal <<= 1;
+        else if ((var->vmode & FB_VMODE_MASK) == FB_VMODE_DOUBLE)
+                vtotal <<= 2;
+        else if ((var->vmode & FB_VMODE_MASK) == FB_VMODE_INTERLACED)
+        {
+//              vtotal <<= 1;
+//              var->yres <<= 1;
+        }
+
+        if(!htotal || !vtotal) {
+                DPRINTK("XGIfb: Invalid 'var' information\n");
+                return -EINVAL;
+        }
+        printk(KERN_DEBUG "XGIfb: var->pixclock=%d, htotal=%d, vtotal=%d\n",
+                var->pixclock,htotal,vtotal);
+
+
+
+        if(var->pixclock && htotal && vtotal) {
+                drate = 1000000000 / var->pixclock;
+                hrate = (drate * 1000) / htotal;
+                xgi_video_info.refresh_rate = (unsigned int) (hrate * 2 / vtotal);
+        } else {
+                xgi_video_info.refresh_rate = 60;
+        }
+
+        printk(KERN_DEBUG "XGIfb: Change mode to %dx%dx%d-%dHz\n",
+                var->xres,var->yres,var->bits_per_pixel,xgi_video_info.refresh_rate);
+
+        old_mode = xgifb_mode_idx;
+        xgifb_mode_idx = 0;
+
+        while( (XGIbios_mode[xgifb_mode_idx].mode_no != 0) &&
+               (XGIbios_mode[xgifb_mode_idx].xres <= var->xres) ) {
+                if( (XGIbios_mode[xgifb_mode_idx].xres == var->xres) &&
+                    (XGIbios_mode[xgifb_mode_idx].yres == var->yres) &&
+                    (XGIbios_mode[xgifb_mode_idx].bpp == var->bits_per_pixel)) {
+                        XGIfb_mode_no = XGIbios_mode[xgifb_mode_idx].mode_no;
+                        found_mode = 1;
+                        break;
+                }
+                xgifb_mode_idx++;
+        }
+
+        if(found_mode)
+                xgifb_mode_idx = XGIfb_validate_mode(xgifb_mode_idx);
+        else
+                xgifb_mode_idx = -1;
+
+        if(xgifb_mode_idx < 0) {
+                printk(KERN_ERR "XGIfb: Mode %dx%dx%d not supported\n", var->xres,
+                       var->yres, var->bits_per_pixel);
+                xgifb_mode_idx = old_mode;
+                return -EINVAL;
+        }
+
+        if(XGIfb_search_refresh_rate(xgi_video_info.refresh_rate) == 0) {
+                XGIfb_rate_idx = XGIbios_mode[xgifb_mode_idx].rate_idx;
+                xgi_video_info.refresh_rate = 60;
+        }
+
+        if(isactive) {
+
+
+                XGIfb_pre_setmode();
+                if(XGISetModeNew( &XGIhw_ext, XGIfb_mode_no) == 0) {
+                        printk(KERN_ERR "XGIfb: Setting mode[0x%x] failed\n", XGIfb_mode_no);
+                        return -EINVAL;
+                }
+        info->fix.line_length = ((info->var.xres_virtual * info->var.bits_per_pixel)>>6);
+
+        outXGIIDXREG(XGISR,IND_XGI_PASSWORD,XGI_PASSWORD);
+
+                outXGIIDXREG(XGICR,0x13,(info->fix.line_length & 0x00ff));
+                outXGIIDXREG(XGISR,0x0E,(info->fix.line_length & 0xff00)>>8);
+
+                XGIfb_post_setmode();
+
+                DPRINTK("XGIfb: Set new mode: %dx%dx%d-%d \n",
+                        XGIbios_mode[xgifb_mode_idx].xres,
+                        XGIbios_mode[xgifb_mode_idx].yres,
+                        XGIbios_mode[xgifb_mode_idx].bpp,
+                        xgi_video_info.refresh_rate);
+
+                xgi_video_info.video_bpp = XGIbios_mode[xgifb_mode_idx].bpp;
+                xgi_video_info.video_vwidth = info->var.xres_virtual;
+                xgi_video_info.video_width = XGIbios_mode[xgifb_mode_idx].xres;
+                xgi_video_info.video_vheight = info->var.yres_virtual;
+                xgi_video_info.video_height = XGIbios_mode[xgifb_mode_idx].yres;
+                xgi_video_info.org_x = xgi_video_info.org_y = 0;
+                xgi_video_info.video_linelength = info->var.xres_virtual * (xgi_video_info.video_bpp >> 3);
+                xgi_video_info.accel = 0;
+                if(XGIfb_accel) {
+                   xgi_video_info.accel = (var->accel_flags & FB_ACCELF_TEXT) ? -1 : 0;
+                }
+                switch(xgi_video_info.video_bpp)
+                {
+                case 8:
+                        xgi_video_info.DstColor = 0x0000;
+                        xgi_video_info.XGI310_AccelDepth = 0x00000000;
+                            xgi_video_info.video_cmap_len = 256;
+#if defined(__powerpc__)
+                inXGIIDXREG (XGICR, 0x4D, cr_data);
+                outXGIIDXREG(XGICR, 0x4D, (cr_data & 0xE0));
+#endif
+                break;
+                case 16:
+                xgi_video_info.DstColor = 0x8000;
+                xgi_video_info.XGI310_AccelDepth = 0x00010000;
+#if defined(__powerpc__)
+                inXGIIDXREG (XGICR, 0x4D, cr_data);
+                outXGIIDXREG(XGICR, 0x4D, ((cr_data & 0xE0) | 0x0B));
+#endif
+                            xgi_video_info.video_cmap_len = 16;
+                break;
+                case 32:
+                xgi_video_info.DstColor = 0xC000;
+                        xgi_video_info.XGI310_AccelDepth = 0x00020000;
+                            xgi_video_info.video_cmap_len = 16;
+#if defined(__powerpc__)
+                inXGIIDXREG (XGICR, 0x4D, cr_data);
+                outXGIIDXREG(XGICR, 0x4D, ((cr_data & 0xE0) | 0x15));
+#endif
+                break;
+                    default:
+                            xgi_video_info.video_cmap_len = 16;
+                        printk(KERN_ERR "XGIfb: Unsupported depth %d", xgi_video_info.video_bpp);
+                            xgi_video_info.accel = 0;
+                            break;
+        }
+        }
+        XGIfb_bpp_to_var(var); /*update ARGB info*/
+        DEBUGPRN("End of do_set_var");
+
+        dumpVGAReg();
+        return 0;
+}
+
+#ifdef XGIFB_PAN
+static int XGIfb_pan_var(struct fb_var_screeninfo *var)
+{
+        unsigned int base;
+
+//      printk("Inside pan_var");
+
+        if (var->xoffset > (var->xres_virtual - var->xres)) {
+//              printk( "Pan: xo: %d xv %d xr %d\n",
+//                      var->xoffset, var->xres_virtual, var->xres);
+                return -EINVAL;
+        }
+        if(var->yoffset > (var->yres_virtual - var->yres)) {
+//              printk( "Pan: yo: %d yv %d yr %d\n",
+//                      var->yoffset, var->yres_virtual, var->yres);
+                return -EINVAL;
+        }
+        base = var->yoffset * var->xres_virtual + var->xoffset;
+
+        /* calculate base bpp dep. */
+        switch(var->bits_per_pixel) {
+        case 16:
+                base >>= 1;
+                break;
+        case 32:
+                break;
+        case 8:
+        default:
+                base >>= 2;
+                break;
+        }
+
+        outXGIIDXREG(XGISR, IND_XGI_PASSWORD, XGI_PASSWORD);
+
+        outXGIIDXREG(XGICR, 0x0D, base & 0xFF);
+        outXGIIDXREG(XGICR, 0x0C, (base >> 8) & 0xFF);
+        outXGIIDXREG(XGISR, 0x0D, (base >> 16) & 0xFF);
+        outXGIIDXREG(XGISR, 0x37, (base >> 24) & 0x03);
+        setXGIIDXREG(XGISR, 0x37, 0xDF, (base >> 21) & 0x04);
+
+        if(xgi_video_info.disp_state & DISPTYPE_DISP2) {
+                orXGIIDXREG(XGIPART1, XGIfb_CRT2_write_enable, 0x01);
+                outXGIIDXREG(XGIPART1, 0x06, (base & 0xFF));
+                outXGIIDXREG(XGIPART1, 0x05, ((base >> 8) & 0xFF));
+                outXGIIDXREG(XGIPART1, 0x04, ((base >> 16) & 0xFF));
+                setXGIIDXREG(XGIPART1, 0x02, 0x7F, ((base >> 24) & 0x01) << 7);
+        }
+//      printk("End of pan_var");
+        return 0;
+}
+#endif
+
+
+void XGI_dispinfo(struct ap_data *rec)
+{
+        rec->minfo.bpp    = xgi_video_info.video_bpp;
+        rec->minfo.xres   = xgi_video_info.video_width;
+        rec->minfo.yres   = xgi_video_info.video_height;
+        rec->minfo.v_xres = xgi_video_info.video_vwidth;
+        rec->minfo.v_yres = xgi_video_info.video_vheight;
+        rec->minfo.org_x  = xgi_video_info.org_x;
+        rec->minfo.org_y  = xgi_video_info.org_y;
+        rec->minfo.vrate  = xgi_video_info.refresh_rate;
+        rec->iobase       = xgi_video_info.vga_base - 0x30;
+        rec->mem_size     = xgi_video_info.video_size;
+        rec->disp_state   = xgi_video_info.disp_state;
+        rec->version      = (VER_MAJOR << 24) | (VER_MINOR << 16) | VER_LEVEL;
+        rec->hasVB        = xgi_video_info.hasVB;
+        rec->TV_type      = xgi_video_info.TV_type;
+        rec->TV_plug      = xgi_video_info.TV_plug;
+        rec->chip         = xgi_video_info.chip;
+}
+
+
+
+
+static int XGIfb_open(struct fb_info *info, int user)
+{
+    return 0;
+}
+
+static int XGIfb_release(struct fb_info *info, int user)
+{
+    return 0;
+}
+
+static int XGIfb_get_cmap_len(const struct fb_var_screeninfo *var)
+{
+        int rc = 16;
+
+        switch(var->bits_per_pixel) {
+        case 8:
+                rc = 256;
+                break;
+        case 16:
+                rc = 16;
+                break;
+        case 32:
+                rc = 16;
+                break;
+        }
+        return rc;
+}
+
+static int XGIfb_setcolreg(unsigned regno, unsigned red, unsigned green, unsigned blue,
+                           unsigned transp, struct fb_info *info)
+{
+        if (regno >= XGIfb_get_cmap_len(&info->var))
+                return 1;
+
+        switch (info->var.bits_per_pixel) {
+        case 8:
+                outXGIREG(XGIDACA, regno);
+                outXGIREG(XGIDACD, (red >> 10));
+                outXGIREG(XGIDACD, (green >> 10));
+                outXGIREG(XGIDACD, (blue >> 10));
+                if (xgi_video_info.disp_state & DISPTYPE_DISP2) {
+                        outXGIREG(XGIDAC2A, regno);
+                        outXGIREG(XGIDAC2D, (red >> 8));
+                        outXGIREG(XGIDAC2D, (green >> 8));
+                        outXGIREG(XGIDAC2D, (blue >> 8));
+                }
+                break;
+        case 16:
+                ((u32 *)(info->pseudo_palette))[regno] =
+                    ((red & 0xf800)) | ((green & 0xfc00) >> 5) | ((blue & 0xf800) >> 11);
+                break;
+        case 32:
+                red >>= 8;
+                green >>= 8;
+                blue >>= 8;
+                ((u32 *) (info->pseudo_palette))[regno] =
+                        (red << 16) | (green << 8) | (blue);
+                break;
+        }
+        return 0;
+}
+
+static int XGIfb_set_par(struct fb_info *info)
+{
+        int err;
+
+//      printk("XGIfb: inside set_par\n");
+        if((err = XGIfb_do_set_var(&info->var, 1, info)))
+                return err;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,10)
+        XGIfb_get_fix(&info->fix, info->currcon, info);
+#else
+        XGIfb_get_fix(&info->fix, -1, info);
+#endif
+//      printk("XGIfb:end of set_par\n");
+        return 0;
+}
+
+static int XGIfb_check_var(struct fb_var_screeninfo *var,
+                           struct fb_info *info)
+{
+        unsigned int htotal =
+                var->left_margin + var->xres + var->right_margin +
+                var->hsync_len;
+        unsigned int vtotal = 0;
+        unsigned int drate = 0, hrate = 0;
+        int found_mode = 0;
+        int refresh_rate, search_idx;
+
+        DEBUGPRN("Inside check_var");
+
+        if((var->vmode & FB_VMODE_MASK) == FB_VMODE_NONINTERLACED) {
+                vtotal = var->upper_margin + var->yres + var->lower_margin +
+                         var->vsync_len;
+                vtotal <<= 1;
+        } else if((var->vmode & FB_VMODE_MASK) == FB_VMODE_DOUBLE) {
+                vtotal = var->upper_margin + var->yres + var->lower_margin +
+                         var->vsync_len;
+                vtotal <<= 2;
+        } else if((var->vmode & FB_VMODE_MASK) == FB_VMODE_INTERLACED) {
+                vtotal = var->upper_margin + (var->yres/2) + var->lower_margin +
+                         var->vsync_len;
+        } else  vtotal = var->upper_margin + var->yres + var->lower_margin +
+                         var->vsync_len;
+
+        if(!(htotal) || !(vtotal)) {
+                XGIFAIL("XGIfb: no valid timing data");
+        }
+
+
+        if(var->pixclock && htotal && vtotal) {
+                drate = 1000000000 / var->pixclock;
+                hrate = (drate * 1000) / htotal;
+                xgi_video_info.refresh_rate = (unsigned int) (hrate * 2 / vtotal);
+        printk(KERN_DEBUG \
+                "%s: pixclock = %d ,htotal=%d, vtotal=%d\n" \
+                "%s: drate=%d, hrate=%d, refresh_rate=%d\n",
+                __func__,var->pixclock, htotal, vtotal,
+                __func__, drate, hrate, xgi_video_info.refresh_rate);
+        } else {
+                xgi_video_info.refresh_rate = 60;
+        }
+
+/*
+        if((var->pixclock) && (htotal)) {
+           drate = 1E12 / var->pixclock;
+           hrate = drate / htotal;
+           refresh_rate = (unsigned int) (hrate / vtotal * 2 + 0.5);
+        } else refresh_rate = 60;
+*/
+        /* TW: Calculation wrong for 1024x600 - force it to 60Hz */
+        if((var->xres == 1024) && (var->yres == 600)) refresh_rate = 60;
+
+        search_idx = 0;
+        while((XGIbios_mode[search_idx].mode_no != 0) &&
+               (XGIbios_mode[search_idx].xres <= var->xres) ) {
+            if((XGIbios_mode[search_idx].xres == var->xres) &&
+               (XGIbios_mode[search_idx].yres == var->yres) &&
+                    (XGIbios_mode[search_idx].bpp == var->bits_per_pixel)) {
+                        if(XGIfb_validate_mode(search_idx) > 0) {
+                    found_mode = 1;
+                    break;
+                }
+            }
+                search_idx++;
+        }
+
+        if(!found_mode) {
+
+                printk(KERN_ERR "XGIfb: %dx%dx%d is no valid mode\n",
+                        var->xres, var->yres, var->bits_per_pixel);
+
+                search_idx = 0;
+                while(XGIbios_mode[search_idx].mode_no != 0) {
+
+                   if( (var->xres <= XGIbios_mode[search_idx].xres) &&
+                       (var->yres <= XGIbios_mode[search_idx].yres) &&
+                       (var->bits_per_pixel == XGIbios_mode[search_idx].bpp) ) {
+                          if(XGIfb_validate_mode(search_idx) > 0) {
+                             found_mode = 1;
+                             break;
+                          }
+                   }
+                   search_idx++;
+                }
+                if(found_mode) {
+                        var->xres = XGIbios_mode[search_idx].xres;
+                        var->yres = XGIbios_mode[search_idx].yres;
+                        printk(KERN_DEBUG "XGIfb: Adapted to mode %dx%dx%d\n",
+                                var->xres, var->yres, var->bits_per_pixel);
+
+                } else {
+                        printk(KERN_ERR "XGIfb: Failed to find similar mode to %dx%dx%d\n",
+                                var->xres, var->yres, var->bits_per_pixel);
+                        return -EINVAL;
+                }
+        }
+
+        /* TW: TODO: Check the refresh rate */
+
+        /* Adapt RGB settings */
+        XGIfb_bpp_to_var(var);
+
+        /* Sanity check for offsets */
+        if (var->xoffset < 0)
+                var->xoffset = 0;
+        if (var->yoffset < 0)
+                var->yoffset = 0;
+
+
+        if(!XGIfb_ypan) {
+                if(var->xres != var->xres_virtual)
+                         var->xres_virtual = var->xres;
+                if(var->yres != var->yres_virtual)
+                        var->yres_virtual = var->yres;
+        }/* else {
+           // TW: Now patch yres_virtual if we use panning
+           // May I do this?
+           var->yres_virtual = xgi_video_info.heapstart / (var->xres * (var->bits_per_pixel >> 3));
+            if(var->yres_virtual <= var->yres) {
+                // TW: Paranoia check
+                var->yres_virtual = var->yres;
+            }
+        }*/
+
+        /* Truncate offsets to maximum if too high */
+        if (var->xoffset > var->xres_virtual - var->xres)
+                var->xoffset = var->xres_virtual - var->xres - 1;
+
+        if (var->yoffset > var->yres_virtual - var->yres)
+                var->yoffset = var->yres_virtual - var->yres - 1;
+
+        /* Set everything else to 0 */
+        var->red.msb_right =
+            var->green.msb_right =
+            var->blue.msb_right =
+            var->transp.offset = var->transp.length = var->transp.msb_right = 0;
+
+        DEBUGPRN("end of check_var");
+        return 0;
+}
+
+#ifdef XGIFB_PAN
+static int XGIfb_pan_display( struct fb_var_screeninfo *var,
+                                 struct fb_info* info)
+{
+        int err;
+
+//      printk("\nInside pan_display:");
+
+        if (var->xoffset > (var->xres_virtual - var->xres))
+                return -EINVAL;
+        if (var->yoffset > (var->yres_virtual - var->yres))
+                return -EINVAL;
+
+        if (var->vmode & FB_VMODE_YWRAP) {
+                if (var->yoffset < 0
+                    || var->yoffset >= info->var.yres_virtual
+                    || var->xoffset) return -EINVAL;
+        } else {
+                if (var->xoffset + info->var.xres > info->var.xres_virtual ||
+                    var->yoffset + info->var.yres > info->var.yres_virtual)
+                        return -EINVAL;
+        }
+
+        if((err = XGIfb_pan_var(var)) < 0) return err;
+
+        info->var.xoffset = var->xoffset;
+        info->var.yoffset = var->yoffset;
+        if (var->vmode & FB_VMODE_YWRAP)
+                info->var.vmode |= FB_VMODE_YWRAP;
+        else
+                info->var.vmode &= ~FB_VMODE_YWRAP;
+
+//      printk(" End of pan_display");
+        return 0;
+}
+#endif
+
+#if 0
+static int XGIfb_mmap(struct fb_info *info, struct file *file,
+                      struct vm_area_struct *vma)
+{
+        unsigned long start;
+        unsigned long off;
+        u32 len, mmio_off;
+
+        DEBUGPRN("inside mmap");
+        if(vma->vm_pgoff > (~0UL >> PAGE_SHIFT))  return -EINVAL;
+
+        off = vma->vm_pgoff << PAGE_SHIFT;
+
+        start = (unsigned long) xgi_video_info.video_base;
+        len = PAGE_ALIGN((start & ~PAGE_MASK) + xgi_video_info.video_size);
+        start &= PAGE_MASK;
+#if 0
+        if (off >= len) {
+                off -= len;
+#endif
+        /* By Jake Page: Treat mmap request with offset beyond heapstart
+         *               as request for mapping the mmio area
+         */
+        #if 1
+        mmio_off = PAGE_ALIGN((start & ~PAGE_MASK) + xgi_video_info.heapstart);
+        if(off >= mmio_off) {
+                off -= mmio_off;
+                if(info->var.accel_flags) return -EINVAL;
+
+                start = (unsigned long) xgi_video_info.mmio_base;
+                len = PAGE_ALIGN((start & ~PAGE_MASK) + XGIfb_mmio_size);
+        }
+        start &= PAGE_MASK;
+        #endif
+        if((vma->vm_end - vma->vm_start + off) > len)   return -EINVAL;
+
+        off += start;
+        vma->vm_pgoff = off >> PAGE_SHIFT;
+        vma->vm_flags |= VM_IO;   /* by Jake Page; is that really needed? */
+
+#if defined(__i386__) || defined(__x86_64__)
+        if (boot_cpu_data.x86 > 3)
+                pgprot_val(vma->vm_page_prot) |= _PAGE_PCD;
+#endif
+        if (io_remap_pfn_range(vma, vma->vm_start, off >> PAGE_SHIFT, vma->vm_end - vma->vm_start,
+                                vma->vm_page_prot))
+                return -EAGAIN;
+
+        DEBUGPRN("end of mmap");
+        return 0;
+}
+#endif
+static int XGIfb_blank(int blank, struct fb_info *info)
+{
+        u8 reg;
+
+        inXGIIDXREG(XGICR, 0x17, reg);
+
+        if(blank > 0)
+                reg &= 0x7f;
+        else
+                reg |= 0x80;
+
+        outXGIIDXREG(XGICR, 0x17, reg);
+        outXGIIDXREG(XGISR, 0x00, 0x01);    /* Synchronous Reset */
+        outXGIIDXREG(XGISR, 0x00, 0x03);    /* End Reset */
+        return(0);
+}
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,15)
+static int XGIfb_ioctl(struct fb_info *info, unsigned int cmd,
+                            unsigned long arg)
+#else
+static int XGIfb_ioctl(struct inode *inode, struct file *file,
+                       unsigned int cmd, unsigned long arg,
+                       struct fb_info *info)
+#endif
+
+{
+        DEBUGPRN("inside ioctl");
+        switch (cmd) {
+           case FBIO_ALLOC:
+                if (!capable(CAP_SYS_RAWIO))
+                        return -EPERM;
+                XGI_malloc((struct XGI_memreq *) arg);
+                break;
+           case FBIO_FREE:
+                if (!capable(CAP_SYS_RAWIO))
+                        return -EPERM;
+                XGI_free(*(unsigned long *) arg);
+                break;
+           case FBIOGET_HWCINFO:
+                {
+                        unsigned long *hwc_offset = (unsigned long *) arg;
+
+                        if (XGIfb_caps & HW_CURSOR_CAP)
+                                *hwc_offset = XGIfb_hwcursor_vbase -
+                                    (unsigned long) xgi_video_info.video_vbase;
+                        else
+                                *hwc_offset = 0;
+
+                        break;
+                }
+           case FBIOPUT_MODEINFO:
+                {
+                        struct mode_info *x = (struct mode_info *)arg;
+
+                        xgi_video_info.video_bpp        = x->bpp;
+                        xgi_video_info.video_width      = x->xres;
+                        xgi_video_info.video_height     = x->yres;
+                        xgi_video_info.video_vwidth     = x->v_xres;
+                        xgi_video_info.video_vheight    = x->v_yres;
+                        xgi_video_info.org_x            = x->org_x;
+                        xgi_video_info.org_y            = x->org_y;
+                        xgi_video_info.refresh_rate     = x->vrate;
+                        xgi_video_info.video_linelength = xgi_video_info.video_vwidth * (xgi_video_info.video_bpp >> 3);
+                        switch(xgi_video_info.video_bpp) {
+                        case 8:
+                                xgi_video_info.DstColor = 0x0000;
+                                xgi_video_info.XGI310_AccelDepth = 0x00000000;
+                                xgi_video_info.video_cmap_len = 256;
+                                break;
+                        case 16:
+                                xgi_video_info.DstColor = 0x8000;
+                                xgi_video_info.XGI310_AccelDepth = 0x00010000;
+                                xgi_video_info.video_cmap_len = 16;
+                                break;
+                        case 32:
+                                xgi_video_info.DstColor = 0xC000;
+                                xgi_video_info.XGI310_AccelDepth = 0x00020000;
+                                xgi_video_info.video_cmap_len = 16;
+                                break;
+                        default:
+                                xgi_video_info.video_cmap_len = 16;
+                                printk(KERN_ERR "XGIfb: Unsupported accel depth %d", xgi_video_info.video_bpp);
+                                xgi_video_info.accel = 0;
+                                break;
+                        }
+
+                        break;
+                }
+           case FBIOGET_DISPINFO:
+                XGI_dispinfo((struct ap_data *)arg);
+                break;
+           case XGIFB_GET_INFO:  /* TW: New for communication with X driver */
+                {
+                        XGIfb_info *x = (XGIfb_info *)arg;
+
+                        //x->XGIfb_id = XGIFB_ID;
+                        x->XGIfb_version = VER_MAJOR;
+                        x->XGIfb_revision = VER_MINOR;
+                        x->XGIfb_patchlevel = VER_LEVEL;
+                        x->chip_id = xgi_video_info.chip_id;
+                        x->memory = xgi_video_info.video_size / 1024;
+                        x->heapstart = xgi_video_info.heapstart / 1024;
+                        x->fbvidmode = XGIfb_mode_no;
+                        x->XGIfb_caps = XGIfb_caps;
+                        x->XGIfb_tqlen = 512; /* yet unused */
+                        x->XGIfb_pcibus = xgi_video_info.pcibus;
+                        x->XGIfb_pcislot = xgi_video_info.pcislot;
+                        x->XGIfb_pcifunc = xgi_video_info.pcifunc;
+                        x->XGIfb_lcdpdc = XGIfb_detectedpdc;
+                        x->XGIfb_lcda = XGIfb_detectedlcda;
+                        break;
+                }
+           case XGIFB_GET_VBRSTATUS:
+                {
+                        unsigned long *vbrstatus = (unsigned long *) arg;
+                        if(XGIfb_CheckVBRetrace()) *vbrstatus = 1;
+                        else                       *vbrstatus = 0;
+                }
+           default:
+                return -EINVAL;
+        }
+        DEBUGPRN("end of ioctl");
+        return 0;
+
+}
+
+
+
+/* ----------- FBDev related routines for all series ---------- */
+
+static int XGIfb_get_fix(struct fb_fix_screeninfo *fix, int con,
+                         struct fb_info *info)
+{
+        DEBUGPRN("inside get_fix");
+        memset(fix, 0, sizeof(struct fb_fix_screeninfo));
+
+        strcpy(fix->id, myid);
+
+        fix->smem_start = xgi_video_info.video_base;
+
+        fix->smem_len = xgi_video_info.video_size;
+
+
+/*        if((!XGIfb_mem) || (XGIfb_mem > (xgi_video_info.video_size/1024))) {
+            if (xgi_video_info.video_size > 0x1000000) {
+                fix->smem_len = 0xD00000;
+            } else if (xgi_video_info.video_size > 0x800000)
+                fix->smem_len = 0x800000;
+            else
+                fix->smem_len = 0x400000;
+        } else
+                fix->smem_len = XGIfb_mem * 1024;
+*/
+        fix->type        = video_type;
+        fix->type_aux    = 0;
+        if(xgi_video_info.video_bpp == 8)
+                fix->visual = FB_VISUAL_PSEUDOCOLOR;
+        else
+                fix->visual = FB_VISUAL_DIRECTCOLOR;
+        fix->xpanstep    = 0;
+#ifdef XGIFB_PAN
+        if(XGIfb_ypan)   fix->ypanstep = 1;
+#endif
+        fix->ywrapstep   = 0;
+        fix->line_length = xgi_video_info.video_linelength;
+        fix->mmio_start  = xgi_video_info.mmio_base;
+        fix->mmio_len    = XGIfb_mmio_size;
+    if(xgi_video_info.chip >= XG40)
+           fix->accel    = FB_ACCEL_XGI_XABRE;
+        else
+           fix->accel    = FB_ACCEL_XGI_GLAMOUR_2;
+
+
+        DEBUGPRN("end of get_fix");
+        return 0;
+}
+
+
+static struct fb_ops XGIfb_ops = {
+        .owner        = THIS_MODULE,
+        .fb_open      = XGIfb_open,
+        .fb_release   = XGIfb_release,
+        .fb_check_var = XGIfb_check_var,
+        .fb_set_par   = XGIfb_set_par,
+        .fb_setcolreg = XGIfb_setcolreg,
+#ifdef XGIFB_PAN
+        .fb_pan_display = XGIfb_pan_display,
+#endif
+        .fb_blank     = XGIfb_blank,
+        .fb_fillrect  = fbcon_XGI_fillrect,
+        .fb_copyarea  = fbcon_XGI_copyarea,
+        .fb_imageblit = cfb_imageblit,
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15)
+        .fb_cursor    = soft_cursor,
+#endif
+        .fb_sync      = fbcon_XGI_sync,
+        .fb_ioctl     = XGIfb_ioctl,
+//      .fb_mmap      = XGIfb_mmap,
+};
+
+/* ---------------- Chip generation dependent routines ---------------- */
+
+
+/* for XGI 315/550/650/740/330 */
+
+static int XGIfb_get_dram_size(void)
+{
+
+        u8  ChannelNum,tmp;
+        u8  reg = 0;
+
+        /* xorg driver sets 32MB * 1 channel */
+        if (xgi_video_info.chip == XG27)
+                outXGIIDXREG(XGISR, IND_XGI_DRAM_SIZE, 0x51);
+
+                inXGIIDXREG(XGISR, IND_XGI_DRAM_SIZE, reg);
+                switch ((reg & XGI_DRAM_SIZE_MASK) >> 4) {
+                   case XGI_DRAM_SIZE_1MB:
+                        xgi_video_info.video_size = 0x100000;
+                        break;
+                   case XGI_DRAM_SIZE_2MB:
+                        xgi_video_info.video_size = 0x200000;
+                        break;
+                   case XGI_DRAM_SIZE_4MB:
+                        xgi_video_info.video_size = 0x400000;
+                        break;
+                   case XGI_DRAM_SIZE_8MB:
+                        xgi_video_info.video_size = 0x800000;
+                        break;
+                   case XGI_DRAM_SIZE_16MB:
+                        xgi_video_info.video_size = 0x1000000;
+                        break;
+                   case XGI_DRAM_SIZE_32MB:
+                        xgi_video_info.video_size = 0x2000000;
+                        break;
+                   case XGI_DRAM_SIZE_64MB:
+                        xgi_video_info.video_size = 0x4000000;
+                        break;
+                   case XGI_DRAM_SIZE_128MB:
+                        xgi_video_info.video_size = 0x8000000;
+                        break;
+                   case XGI_DRAM_SIZE_256MB:
+                        xgi_video_info.video_size = 0x10000000;
+                        break;
+                   default:
+                        return -1;
+                }
+
+                tmp = (reg & 0x0c) >> 2;
+                switch(xgi_video_info.chip)
+                {
+                    case XG20:
+                    case XG21:
+                    case XG27:
+                        ChannelNum = 1;
+                        break;
+
+                    case XG42:
+                        if(reg & 0x04)
+                            ChannelNum = 2;
+                        else
+                            ChannelNum = 1;
+                        break;
+
+                    case XG45:
+                        if(tmp == 1)
+                    ChannelNum = 2;
+                else
+                if(tmp == 2)
+                    ChannelNum = 3;
+                else
+                if(tmp == 3)
+                    ChannelNum = 4;
+                else
+                    ChannelNum = 1;
+                        break;
+
+                    case XG40:
+                    default:
+                if(tmp == 2)
+                    ChannelNum = 2;
+                else
+                if(tmp == 3)
+                    ChannelNum = 3;
+                else
+                    ChannelNum = 1;
+                        break;
+                }
+
+
+                xgi_video_info.video_size = xgi_video_info.video_size * ChannelNum;
+                //PLiad fixed for benchmarking and fb set
+                //xgi_video_info.video_size = 0x200000;//1024x768x16
+                //xgi_video_info.video_size = 0x1000000;//benchmark
+
+                printk("XGIfb: SR14=%x DramSzie %x ChannelNum %x\n",reg,xgi_video_info.video_size ,ChannelNum );
+                return 0;
+
+}
+
+static void XGIfb_detect_VB(void)
+{
+        u8 cr32, temp=0;
+
+        xgi_video_info.TV_plug = xgi_video_info.TV_type = 0;
+
+        switch(xgi_video_info.hasVB) {
+          case HASVB_LVDS_CHRONTEL:
+          case HASVB_CHRONTEL:
+             break;
+          case HASVB_301:
+          case HASVB_302:
+//           XGI_Sense30x(); //Yi-Lin TV Sense?
+             break;
+        }
+
+        inXGIIDXREG(XGICR, IND_XGI_SCRATCH_REG_CR32, cr32);
+
+        if ((cr32 & XGI_CRT1) && !XGIfb_crt1off)
+                XGIfb_crt1off = 0;
+        else {
+                if (cr32 & 0x5F)
+                        XGIfb_crt1off = 1;
+                else
+                        XGIfb_crt1off = 0;
+        }
+
+        if (XGIfb_crt2type != -1)
+                /* TW: Override with option */
+                xgi_video_info.disp_state = XGIfb_crt2type;
+        else if (cr32 & XGI_VB_TV)
+                xgi_video_info.disp_state = DISPTYPE_TV;
+        else if (cr32 & XGI_VB_LCD)
+                xgi_video_info.disp_state = DISPTYPE_LCD;
+        else if (cr32 & XGI_VB_CRT2)
+                xgi_video_info.disp_state = DISPTYPE_CRT2;
+        else
+                xgi_video_info.disp_state = 0;
+
+        if(XGIfb_tvplug != -1)
+                /* PR/TW: Override with option */
+                xgi_video_info.TV_plug = XGIfb_tvplug;
+        else if (cr32 & XGI_VB_HIVISION) {
+                xgi_video_info.TV_type = TVMODE_HIVISION;
+                xgi_video_info.TV_plug = TVPLUG_SVIDEO;
+        }
+        else if (cr32 & XGI_VB_SVIDEO)
+                xgi_video_info.TV_plug = TVPLUG_SVIDEO;
+        else if (cr32 & XGI_VB_COMPOSITE)
+                xgi_video_info.TV_plug = TVPLUG_COMPOSITE;
+        else if (cr32 & XGI_VB_SCART)
+                xgi_video_info.TV_plug = TVPLUG_SCART;
+
+        if(xgi_video_info.TV_type == 0) {
+            /* TW: PAL/NTSC changed for 650 */
+            if((xgi_video_info.chip <= XGI_315PRO) || (xgi_video_info.chip >= XGI_330)) {
+
+                inXGIIDXREG(XGICR, 0x38, temp);
+                if(temp & 0x10)
+                        xgi_video_info.TV_type = TVMODE_PAL;
+                else
+                        xgi_video_info.TV_type = TVMODE_NTSC;
+
+            } else {
+
+                inXGIIDXREG(XGICR, 0x79, temp);
+                if(temp & 0x20)
+                        xgi_video_info.TV_type = TVMODE_PAL;
+                else
+                        xgi_video_info.TV_type = TVMODE_NTSC;
+            }
+        }
+
+        /* TW: Copy forceCRT1 option to CRT1off if option is given */
+        if (XGIfb_forcecrt1 != -1) {
+                if (XGIfb_forcecrt1) XGIfb_crt1off = 0;
+                else                 XGIfb_crt1off = 1;
+        }
+}
+
+static void XGIfb_get_VB_type(void)
+{
+        u8 reg;
+
+        if (!XGIfb_has_VB()) {
+                inXGIIDXREG(XGICR, IND_XGI_SCRATCH_REG_CR37, reg);
+                switch ((reg & XGI_EXTERNAL_CHIP_MASK) >> 1) {
+                   case XGI310_EXTERNAL_CHIP_LVDS:
+                        xgi_video_info.hasVB = HASVB_LVDS;
+                        break;
+                   case XGI310_EXTERNAL_CHIP_LVDS_CHRONTEL:
+                        xgi_video_info.hasVB = HASVB_LVDS_CHRONTEL;
+                        break;
+                   default:
+                        break;
+                }
+        }
+}
+
+
+static int XGIfb_has_VB(void)
+{
+        u8 vb_chipid;
+
+        inXGIIDXREG(XGIPART4, 0x00, vb_chipid);
+        switch (vb_chipid) {
+           case 0x01:
+                xgi_video_info.hasVB = HASVB_301;
+                break;
+           case 0x02:
+                xgi_video_info.hasVB = HASVB_302;
+                break;
+           default:
+                xgi_video_info.hasVB = HASVB_NONE;
+                return FALSE;
+        }
+        return TRUE;
+}
+
+
+
+/* ------------------ Sensing routines ------------------ */
+
+/* TW: Determine and detect attached devices on XGI30x */
+int
+XGIDoSense(int tempbl, int tempbh, int tempcl, int tempch)
+{
+    int temp,i;
+
+    outXGIIDXREG(XGIPART4,0x11,tempbl);
+    temp = tempbh | tempcl;
+    setXGIIDXREG(XGIPART4,0x10,0xe0,temp);
+    for(i=0; i<10; i++) XGI_LongWait(&XGI_Pr);
+    tempch &= 0x7f;
+    inXGIIDXREG(XGIPART4,0x03,temp);
+    temp ^= 0x0e;
+    temp &= tempch;
+    return(temp);
+}
+
+void
+XGI_Sense30x(void)
+{
+  u8 backupP4_0d;
+  u8 testsvhs_tempbl, testsvhs_tempbh;
+  u8 testsvhs_tempcl, testsvhs_tempch;
+  u8 testcvbs_tempbl, testcvbs_tempbh;
+  u8 testcvbs_tempcl, testcvbs_tempch;
+  u8 testvga2_tempbl, testvga2_tempbh;
+  u8 testvga2_tempcl, testvga2_tempch;
+  int myflag, result;
+
+  inXGIIDXREG(XGIPART4,0x0d,backupP4_0d);
+  outXGIIDXREG(XGIPART4,0x0d,(backupP4_0d | 0x04));
+
+
+
+        testvga2_tempbh = 0x00; testvga2_tempbl = 0xd1;
+        testsvhs_tempbh = 0x00; testsvhs_tempbl = 0xb9;
+        testcvbs_tempbh = 0x00; testcvbs_tempbl = 0xb3;
+        if((XGIhw_ext.ujVBChipID != VB_CHIP_301) &&
+           (XGIhw_ext.ujVBChipID != VB_CHIP_302)) {
+              testvga2_tempbh = 0x01; testvga2_tempbl = 0x90;
+              testsvhs_tempbh = 0x01; testsvhs_tempbl = 0x6b;
+              testcvbs_tempbh = 0x01; testcvbs_tempbl = 0x74;
+              if(XGIhw_ext.ujVBChipID == VB_CHIP_301LV ||
+                 XGIhw_ext.ujVBChipID == VB_CHIP_302LV) {
+                 testvga2_tempbh = 0x00; testvga2_tempbl = 0x00;
+                 testsvhs_tempbh = 0x02; testsvhs_tempbl = 0x00;
+                 testcvbs_tempbh = 0x01; testcvbs_tempbl = 0x00;
+              }
+        }
+        if(XGIhw_ext.ujVBChipID != VB_CHIP_301LV &&
+           XGIhw_ext.ujVBChipID != VB_CHIP_302LV) {
+           inXGIIDXREG(XGIPART4,0x01,myflag);
+           if(myflag & 0x04) {
+              testvga2_tempbh = 0x00; testvga2_tempbl = 0xfd;
+              testsvhs_tempbh = 0x00; testsvhs_tempbl = 0xdd;
+              testcvbs_tempbh = 0x00; testcvbs_tempbl = 0xee;
+           }
+        }
+        if((XGIhw_ext.ujVBChipID == VB_CHIP_301LV) ||
+           (XGIhw_ext.ujVBChipID == VB_CHIP_302LV) ) {
+           testvga2_tempbh = 0x00; testvga2_tempbl = 0x00;
+           testvga2_tempch = 0x00; testvga2_tempcl = 0x00;
+           testsvhs_tempch = 0x04; testsvhs_tempcl = 0x08;
+           testcvbs_tempch = 0x08; testcvbs_tempcl = 0x08;
+        } else {
+           testvga2_tempch = 0x0e; testvga2_tempcl = 0x08;
+           testsvhs_tempch = 0x06; testsvhs_tempcl = 0x04;
+           testcvbs_tempch = 0x08; testcvbs_tempcl = 0x04;
+        }
+
+
+    if(testvga2_tempch || testvga2_tempcl || testvga2_tempbh || testvga2_tempbl) {
+        result = XGIDoSense(testvga2_tempbl, testvga2_tempbh,
+                            testvga2_tempcl, testvga2_tempch);
+        if(result) {
+                printk(KERN_INFO "XGIfb: Detected secondary VGA connection\n");
+                orXGIIDXREG(XGICR, 0x32, 0x10);
+        }
+    }
+
+    result = XGIDoSense(testsvhs_tempbl, testsvhs_tempbh,
+                        testsvhs_tempcl, testsvhs_tempch);
+    if(result) {
+        printk(KERN_INFO "XGIfb: Detected TV connected to SVHS output\n");
+        /* TW: So we can be sure that there IS a SVHS output */
+        xgi_video_info.TV_plug = TVPLUG_SVIDEO;
+        orXGIIDXREG(XGICR, 0x32, 0x02);
+    }
+
+    if(!result) {
+        result = XGIDoSense(testcvbs_tempbl, testcvbs_tempbh,
+                            testcvbs_tempcl, testcvbs_tempch);
+        if(result) {
+            printk(KERN_INFO "XGIfb: Detected TV connected to CVBS output\n");
+            /* TW: So we can be sure that there IS a CVBS output */
+            xgi_video_info.TV_plug = TVPLUG_COMPOSITE;
+            orXGIIDXREG(XGICR, 0x32, 0x01);
+        }
+    }
+    XGIDoSense(0, 0, 0, 0);
+
+    outXGIIDXREG(XGIPART4,0x0d,backupP4_0d);
+}
+
+
+
+/* ------------------------ Heap routines -------------------------- */
+
+static int XGIfb_heap_init(void)
+{
+        XGI_OH *poh;
+        u8 temp=0;
+
+        int            agp_enabled = 1;
+        u32            agp_size;
+        unsigned long *cmdq_baseport = 0;
+        unsigned long *read_port = 0;
+        unsigned long *write_port = 0;
+        XGI_CMDTYPE    cmd_type;
+#ifndef AGPOFF
+        struct agp_kern_info  *agp_info;
+        struct agp_memory     *agp;
+        u32            agp_phys;
+#endif
+
+/* TW: The heap start is either set manually using the "mem" parameter, or
+ *     defaults as follows:
+ *     -) If more than 16MB videoRAM available, let our heap start at 12MB.
+ *     -) If more than  8MB videoRAM available, let our heap start at  8MB.
+ *     -) If 4MB or less is available, let it start at 4MB.
+ *     This is for avoiding a clash with X driver which uses the beginning
+ *     of the videoRAM. To limit size of X framebuffer, use Option MaxXFBMem
+ *     in XF86Config-4.
+ *     The heap start can also be specified by parameter "mem" when starting the XGIfb
+ *     driver. XGIfb mem=1024 lets heap starts at 1MB, etc.
+ */
+     if ((!XGIfb_mem) || (XGIfb_mem > (xgi_video_info.video_size/1024))) {
+        if (xgi_video_info.video_size > 0x1000000) {
+                xgi_video_info.heapstart = 0xD00000;
+        } else if (xgi_video_info.video_size > 0x800000) {
+                xgi_video_info.heapstart = 0x800000;
+        } else {
+                xgi_video_info.heapstart = 0x400000;
+        }
+     } else {
+           xgi_video_info.heapstart = XGIfb_mem * 1024;
+     }
+     XGIfb_heap_start =
+               (unsigned long) (xgi_video_info.video_vbase + xgi_video_info.heapstart);
+     printk(KERN_INFO "XGIfb: Memory heap starting at %dK\n",
+                                        (int)(xgi_video_info.heapstart / 1024));
+
+     XGIfb_heap_end = (unsigned long) xgi_video_info.video_vbase + xgi_video_info.video_size;
+     XGIfb_heap_size = XGIfb_heap_end - XGIfb_heap_start;
+
+
+
+        /* TW: Now initialize the 310 series' command queue mode.
+         * On 310/325, there are three queue modes available which
+         * are chosen by setting bits 7:5 in SR26:
+         * 1. MMIO queue mode (bit 5, 0x20). The hardware will keep
+         *    track of the queue, the FIFO, command parsing and so
+         *    on. This is the one comparable to the 300 series.
+         * 2. VRAM queue mode (bit 6, 0x40). In this case, one will
+         *    have to do queue management himself. Register 0x85c4 will
+         *    hold the location of the next free queue slot, 0x85c8
+         *    is the "queue read pointer" whose way of working is
+         *    unknown to me. Anyway, this mode would require a
+         *    translation of the MMIO commands to some kind of
+         *    accelerator assembly and writing these commands
+         *    to the memory location pointed to by 0x85c4.
+         *    We will not use this, as nobody knows how this
+         *    "assembly" works, and as it would require a complete
+         *    re-write of the accelerator code.
+         * 3. AGP queue mode (bit 7, 0x80). Works as 2., but keeps the
+         *    queue in AGP memory space.
+         *
+         * SR26 bit 4 is called "Bypass H/W queue".
+         * SR26 bit 1 is called "Enable Command Queue Auto Correction"
+         * SR26 bit 0 resets the queue
+         * Size of queue memory is encoded in bits 3:2 like this:
+         *    00  (0x00)  512K
+         *    01  (0x04)  1M
+         *    10  (0x08)  2M
+         *    11  (0x0C)  4M
+         * The queue location is to be written to 0x85C0.
+         *
+         */
+        cmdq_baseport = (unsigned long *)(xgi_video_info.mmio_vbase + MMIO_QUEUE_PHYBASE);
+        write_port    = (unsigned long *)(xgi_video_info.mmio_vbase + MMIO_QUEUE_WRITEPORT);
+        read_port     = (unsigned long *)(xgi_video_info.mmio_vbase + MMIO_QUEUE_READPORT);
+
+        DPRINTK("AGP base: 0x%p, read: 0x%p, write: 0x%p\n", cmdq_baseport, read_port, write_port);
+
+        agp_size  = COMMAND_QUEUE_AREA_SIZE;
+
+#ifndef AGPOFF
+        if (XGIfb_queuemode == AGP_CMD_QUEUE) {
+                agp_info = vmalloc(sizeof(*agp_info));
+                memset((void*)agp_info, 0x00, sizeof(*agp_info));
+                agp_copy_info(agp_info);
+
+                agp_backend_acquire();
+
+                agp = agp_allocate_memory(COMMAND_QUEUE_AREA_SIZE/PAGE_SIZE,
+                                          AGP_NORMAL_MEMORY);
+                if (agp == NULL) {
+                        DPRINTK("XGIfb: Allocating AGP buffer failed.\n");
+                        agp_enabled = 0;
+                } else {
+                        if (agp_bind_memory(agp, agp->pg_start) != 0) {
+                                DPRINTK("XGIfb: AGP: Failed to bind memory\n");
+                                /* TODO: Free AGP memory here */
+                                agp_enabled = 0;
+                        } else {
+                                agp_enable(0);
+                        }
+                }
+        }
+#else
+        agp_enabled = 0;
+#endif
+
+        /* TW: Now select the queue mode */
+
+        if ((agp_enabled) && (XGIfb_queuemode == AGP_CMD_QUEUE)) {
+                cmd_type = AGP_CMD_QUEUE;
+                printk(KERN_INFO "XGIfb: Using AGP queue mode\n");
+/*      } else if (XGIfb_heap_size >= COMMAND_QUEUE_AREA_SIZE)  */
+        } else if (XGIfb_queuemode == VM_CMD_QUEUE) {
+                cmd_type = VM_CMD_QUEUE;
+                printk(KERN_INFO "XGIfb: Using VRAM queue mode\n");
+        } else {
+                printk(KERN_INFO "XGIfb: Using MMIO queue mode\n");
+                cmd_type = MMIO_CMD;
+        }
+
+        switch (agp_size) {
+           case 0x80000:
+                temp = XGI_CMD_QUEUE_SIZE_512k;
+                break;
+           case 0x100000:
+                temp = XGI_CMD_QUEUE_SIZE_1M;
+                break;
+           case 0x200000:
+                temp = XGI_CMD_QUEUE_SIZE_2M;
+                break;
+           case 0x400000:
+                temp = XGI_CMD_QUEUE_SIZE_4M;
+                break;
+        }
+
+        switch (cmd_type) {
+           case AGP_CMD_QUEUE:
+#ifndef AGPOFF
+                DPRINTK("XGIfb: AGP buffer base = 0x%lx, offset = 0x%x, size = %dK\n",
+                        agp_info->aper_base, agp->physical, agp_size/1024);
+
+                agp_phys = agp_info->aper_base + agp->physical;
+
+                outXGIIDXREG(XGICR,  IND_XGI_AGP_IO_PAD, 0);
+                outXGIIDXREG(XGICR,  IND_XGI_AGP_IO_PAD, XGI_AGP_2X);
+
+                outXGIIDXREG(XGISR, IND_XGI_CMDQUEUE_THRESHOLD, COMMAND_QUEUE_THRESHOLD);
+
+                outXGIIDXREG(XGISR, IND_XGI_CMDQUEUE_SET, XGI_CMD_QUEUE_RESET);
+
+                *write_port = *read_port;
+
+                temp |= XGI_AGP_CMDQUEUE_ENABLE;
+                outXGIIDXREG(XGISR, IND_XGI_CMDQUEUE_SET, temp);
+
+                *cmdq_baseport = agp_phys;
+
+                XGIfb_caps |= AGP_CMD_QUEUE_CAP;
+#endif
+                break;
+
+           case VM_CMD_QUEUE:
+                XGIfb_heap_end -= COMMAND_QUEUE_AREA_SIZE;
+                XGIfb_heap_size -= COMMAND_QUEUE_AREA_SIZE;
+
+                outXGIIDXREG(XGISR, IND_XGI_CMDQUEUE_THRESHOLD, COMMAND_QUEUE_THRESHOLD);
+
+                outXGIIDXREG(XGISR, IND_XGI_CMDQUEUE_SET, XGI_CMD_QUEUE_RESET);
+
+                *write_port = *read_port;
+
+                temp |= XGI_VRAM_CMDQUEUE_ENABLE;
+                outXGIIDXREG(XGISR, IND_XGI_CMDQUEUE_SET, temp);
+
+                *cmdq_baseport = xgi_video_info.video_size - COMMAND_QUEUE_AREA_SIZE;
+
+                XGIfb_caps |= VM_CMD_QUEUE_CAP;
+
+                DPRINTK("XGIfb: VM Cmd Queue offset = 0x%lx, size is %dK\n",
+                        *cmdq_baseport, COMMAND_QUEUE_AREA_SIZE/1024);
+                break;
+
+           default:  /* MMIO */
+
+//              printk("%s:%d - I'm here\n", __FUNCTION__, __LINE__);
+                /* TW: This previously only wrote XGI_MMIO_CMD_ENABLE
+                 * to IND_XGI_CMDQUEUE_SET. I doubt that this is
+                 * enough. Reserve memory in any way.
+                 */
+// FIXME                XGIfb_heap_end -= COMMAND_QUEUE_AREA_SIZE;
+// FIXME                XGIfb_heap_size -= COMMAND_QUEUE_AREA_SIZE;
+// FIXME
+// FIXME                outXGIIDXREG(XGISR, IND_XGI_CMDQUEUE_THRESHOLD, COMMAND_QUEUE_THRESHOLD);
+// FIXME                outXGIIDXREG(XGISR, IND_XGI_CMDQUEUE_SET, XGI_CMD_QUEUE_RESET);
+// FIXME
+// FIXME                *write_port = *read_port;
+// FIXME
+// FIXME                /* TW: Set Auto_Correction bit */
+// FIXME                temp |= (XGI_MMIO_CMD_ENABLE | XGI_CMD_AUTO_CORR);
+// FIXME                // FIXME outXGIIDXREG(XGISR, IND_XGI_CMDQUEUE_SET, temp);
+// FIXME
+// FIXME                *cmdq_baseport = xgi_video_info.video_size - COMMAND_QUEUE_AREA_SIZE;
+// FIXME
+// FIXME                XGIfb_caps |= MMIO_CMD_QUEUE_CAP;
+// FIXME
+// FIXME                DPRINTK("XGIfb: MMIO Cmd Queue offset = 0x%lx, size is %dK\n",
+// FIXME                        *cmdq_baseport, COMMAND_QUEUE_AREA_SIZE/1024);
+                break;
+        }
+
+
+
+
+     /* TW: Now reserve memory for the HWCursor. It is always located at the very
+            top of the videoRAM, right below the TB memory area (if used). */
+     if (XGIfb_heap_size >= XGIfb_hwcursor_size) {
+                XGIfb_heap_end -= XGIfb_hwcursor_size;
+                XGIfb_heap_size -= XGIfb_hwcursor_size;
+                XGIfb_hwcursor_vbase = XGIfb_heap_end;
+
+                XGIfb_caps |= HW_CURSOR_CAP;
+
+                DPRINTK("XGIfb: Hardware Cursor start at 0x%lx, size is %dK\n",
+                        XGIfb_heap_end, XGIfb_hwcursor_size/1024);
+     }
+
+     XGIfb_heap.poha_chain = NULL;
+     XGIfb_heap.poh_freelist = NULL;
+
+     poh = XGIfb_poh_new_node();
+
+     if(poh == NULL)  return 1;
+
+     poh->poh_next = &XGIfb_heap.oh_free;
+     poh->poh_prev = &XGIfb_heap.oh_free;
+     poh->size = XGIfb_heap_end - XGIfb_heap_start + 1;
+     poh->offset = XGIfb_heap_start - (unsigned long) xgi_video_info.video_vbase;
+
+     DPRINTK("XGIfb: Heap start:0x%p, end:0x%p, len=%dk\n",
+                (char *) XGIfb_heap_start, (char *) XGIfb_heap_end,
+                (unsigned int) poh->size / 1024);
+
+     DPRINTK("XGIfb: First Node offset:0x%x, size:%dk\n",
+                (unsigned int) poh->offset, (unsigned int) poh->size / 1024);
+
+     XGIfb_heap.oh_free.poh_next = poh;
+     XGIfb_heap.oh_free.poh_prev = poh;
+     XGIfb_heap.oh_free.size = 0;
+     XGIfb_heap.max_freesize = poh->size;
+
+     XGIfb_heap.oh_used.poh_next = &XGIfb_heap.oh_used;
+     XGIfb_heap.oh_used.poh_prev = &XGIfb_heap.oh_used;
+     XGIfb_heap.oh_used.size = SENTINEL;
+
+     return 0;
+}
+
+static XGI_OH *XGIfb_poh_new_node(void)
+{
+        int           i;
+        unsigned long cOhs;
+        XGI_OHALLOC   *poha;
+        XGI_OH        *poh;
+
+        if (XGIfb_heap.poh_freelist == NULL) {
+                poha = kmalloc(OH_ALLOC_SIZE, GFP_KERNEL);
+                if(!poha) return NULL;
+
+                poha->poha_next = XGIfb_heap.poha_chain;
+                XGIfb_heap.poha_chain = poha;
+
+                cOhs = (OH_ALLOC_SIZE - sizeof(XGI_OHALLOC)) / sizeof(XGI_OH) + 1;
+
+                poh = &poha->aoh[0];
+                for (i = cOhs - 1; i != 0; i--) {
+                        poh->poh_next = poh + 1;
+                        poh = poh + 1;
+                }
+
+                poh->poh_next = NULL;
+                XGIfb_heap.poh_freelist = &poha->aoh[0];
+        }
+
+        poh = XGIfb_heap.poh_freelist;
+        XGIfb_heap.poh_freelist = poh->poh_next;
+
+        return (poh);
+}
+
+static XGI_OH *XGIfb_poh_allocate(unsigned long size)
+{
+        XGI_OH *pohThis;
+        XGI_OH *pohRoot;
+        int     bAllocated = 0;
+
+        if (size > XGIfb_heap.max_freesize) {
+                DPRINTK("XGIfb: Can't allocate %dk size on offscreen\n",
+                        (unsigned int) size / 1024);
+                return (NULL);
+        }
+
+        pohThis = XGIfb_heap.oh_free.poh_next;
+
+        while (pohThis != &XGIfb_heap.oh_free) {
+                if (size <= pohThis->size) {
+                        bAllocated = 1;
+                        break;
+                }
+                pohThis = pohThis->poh_next;
+        }
+
+        if (!bAllocated) {
+                DPRINTK("XGIfb: Can't allocate %dk size on offscreen\n",
+                        (unsigned int) size / 1024);
+                return (NULL);
+        }
+
+        if (size == pohThis->size) {
+                pohRoot = pohThis;
+                XGIfb_delete_node(pohThis);
+        } else {
+                pohRoot = XGIfb_poh_new_node();
+
+                if (pohRoot == NULL) {
+                        return (NULL);
+                }
+
+                pohRoot->offset = pohThis->offset;
+                pohRoot->size = size;
+
+                pohThis->offset += size;
+                pohThis->size -= size;
+        }
+
+        XGIfb_heap.max_freesize -= size;
+
+        pohThis = &XGIfb_heap.oh_used;
+        XGIfb_insert_node(pohThis, pohRoot);
+
+        return (pohRoot);
+}
+
+static void XGIfb_delete_node(XGI_OH *poh)
+{
+        XGI_OH *poh_prev;
+        XGI_OH *poh_next;
+
+        poh_prev = poh->poh_prev;
+        poh_next = poh->poh_next;
+
+        poh_prev->poh_next = poh_next;
+        poh_next->poh_prev = poh_prev;
+
+}
+
+static void XGIfb_insert_node(XGI_OH *pohList, XGI_OH *poh)
+{
+        XGI_OH *pohTemp;
+
+        pohTemp = pohList->poh_next;
+
+        pohList->poh_next = poh;
+        pohTemp->poh_prev = poh;
+
+        poh->poh_prev = pohList;
+        poh->poh_next = pohTemp;
+}
+
+static XGI_OH *XGIfb_poh_free(unsigned long base)
+{
+        XGI_OH *pohThis;
+        XGI_OH *poh_freed;
+        XGI_OH *poh_prev;
+        XGI_OH *poh_next;
+        unsigned long ulUpper;
+        unsigned long ulLower;
+        int foundNode = 0;
+
+        poh_freed = XGIfb_heap.oh_used.poh_next;
+
+        while(poh_freed != &XGIfb_heap.oh_used) {
+                if(poh_freed->offset == base) {
+                        foundNode = 1;
+                        break;
+                }
+
+                poh_freed = poh_freed->poh_next;
+        }
+
+        if (!foundNode)  return (NULL);
+
+        XGIfb_heap.max_freesize += poh_freed->size;
+
+        poh_prev = poh_next = NULL;
+        ulUpper = poh_freed->offset + poh_freed->size;
+        ulLower = poh_freed->offset;
+
+        pohThis = XGIfb_heap.oh_free.poh_next;
+
+        while (pohThis != &XGIfb_heap.oh_free) {
+                if (pohThis->offset == ulUpper) {
+                        poh_next = pohThis;
+                }
+                        else if ((pohThis->offset + pohThis->size) ==
+                                 ulLower) {
+                        poh_prev = pohThis;
+                }
+                pohThis = pohThis->poh_next;
+        }
+
+        XGIfb_delete_node(poh_freed);
+
+        if (poh_prev && poh_next) {
+                poh_prev->size += (poh_freed->size + poh_next->size);
+                XGIfb_delete_node(poh_next);
+                XGIfb_free_node(poh_freed);
+                XGIfb_free_node(poh_next);
+                return (poh_prev);
+        }
+
+        if (poh_prev) {
+                poh_prev->size += poh_freed->size;
+                XGIfb_free_node(poh_freed);
+                return (poh_prev);
+        }
+
+        if (poh_next) {
+                poh_next->size += poh_freed->size;
+                poh_next->offset = poh_freed->offset;
+                XGIfb_free_node(poh_freed);
+                return (poh_next);
+        }
+
+        XGIfb_insert_node(&XGIfb_heap.oh_free, poh_freed);
+
+        return (poh_freed);
+}
+
+static void XGIfb_free_node(XGI_OH *poh)
+{
+        if(poh == NULL) return;
+
+        poh->poh_next = XGIfb_heap.poh_freelist;
+        XGIfb_heap.poh_freelist = poh;
+
+}
+
+void XGI_malloc(struct XGI_memreq *req)
+{
+        XGI_OH *poh;
+
+        poh = XGIfb_poh_allocate(req->size);
+
+        if(poh == NULL) {
+                req->offset = 0;
+                req->size = 0;
+                DPRINTK("XGIfb: Video RAM allocation failed\n");
+        } else {
+                DPRINTK("XGIfb: Video RAM allocation succeeded: 0x%p\n",
+                        (char *) (poh->offset + (unsigned long) xgi_video_info.video_vbase));
+
+                req->offset = poh->offset;
+                req->size = poh->size;
+        }
+
+}
+
+void XGI_free(unsigned long base)
+{
+        XGI_OH *poh;
+
+        poh = XGIfb_poh_free(base);
+
+        if(poh == NULL) {
+                DPRINTK("XGIfb: XGIfb_poh_free() failed at base 0x%x\n",
+                        (unsigned int) base);
+        }
+}
+
+/* --------------------- SetMode routines ------------------------- */
+
+static void XGIfb_pre_setmode(void)
+{
+        u8 cr30 = 0, cr31 = 0;
+
+        inXGIIDXREG(XGICR, 0x31, cr31);
+        cr31 &= ~0x60;
+
+        switch (xgi_video_info.disp_state & DISPTYPE_DISP2) {
+           case DISPTYPE_CRT2:
+                cr30 = (XGI_VB_OUTPUT_CRT2 | XGI_SIMULTANEOUS_VIEW_ENABLE);
+                cr31 |= XGI_DRIVER_MODE;
+                break;
+           case DISPTYPE_LCD:
+                cr30  = (XGI_VB_OUTPUT_LCD | XGI_SIMULTANEOUS_VIEW_ENABLE);
+                cr31 |= XGI_DRIVER_MODE;
+                break;
+           case DISPTYPE_TV:
+                if (xgi_video_info.TV_type == TVMODE_HIVISION)
+                        cr30 = (XGI_VB_OUTPUT_HIVISION | XGI_SIMULTANEOUS_VIEW_ENABLE);
+                else if (xgi_video_info.TV_plug == TVPLUG_SVIDEO)
+                        cr30 = (XGI_VB_OUTPUT_SVIDEO | XGI_SIMULTANEOUS_VIEW_ENABLE);
+                else if (xgi_video_info.TV_plug == TVPLUG_COMPOSITE)
+                        cr30 = (XGI_VB_OUTPUT_COMPOSITE | XGI_SIMULTANEOUS_VIEW_ENABLE);
+                else if (xgi_video_info.TV_plug == TVPLUG_SCART)
+                        cr30 = (XGI_VB_OUTPUT_SCART | XGI_SIMULTANEOUS_VIEW_ENABLE);
+                cr31 |= XGI_DRIVER_MODE;
+
+                if (XGIfb_tvmode == 1 || xgi_video_info.TV_type == TVMODE_PAL)
+                        cr31 |= 0x01;
+                else
+                        cr31 &= ~0x01;
+                break;
+           default:     /* disable CRT2 */
+                cr30 = 0x00;
+                cr31 |= (XGI_DRIVER_MODE | XGI_VB_OUTPUT_DISABLE);
+        }
+
+        outXGIIDXREG(XGICR, IND_XGI_SCRATCH_REG_CR30, cr30);
+        outXGIIDXREG(XGICR, IND_XGI_SCRATCH_REG_CR31, cr31);
+        outXGIIDXREG(XGICR, IND_XGI_SCRATCH_REG_CR33, (XGIfb_rate_idx & 0x0F));
+
+        if(xgi_video_info.accel) XGIfb_syncaccel();
+
+
+}
+
+static void XGIfb_post_setmode(void)
+{
+        u8 reg;
+        BOOLEAN doit = TRUE;
+#if 0   /* TW: Wrong: Is not in MMIO space, but in RAM */
+        /* Backup mode number to MMIO space */
+        if(xgi_video_info.mmio_vbase) {
+          *(volatile u8 *)(((u8*)xgi_video_info.mmio_vbase) + 0x449) = (unsigned char)XGIfb_mode_no;
+        }
+#endif
+/*      outXGIIDXREG(XGISR,IND_XGI_PASSWORD,XGI_PASSWORD);
+        outXGIIDXREG(XGICR,0x13,0x00);
+        setXGIIDXREG(XGISR,0x0E,0xF0,0x01);
+*test**/
+        if (xgi_video_info.video_bpp == 8) {
+                /* TW: We can't switch off CRT1 on LVDS/Chrontel in 8bpp Modes */
+                if ((xgi_video_info.hasVB == HASVB_LVDS) || (xgi_video_info.hasVB == HASVB_LVDS_CHRONTEL)) {
+                        doit = FALSE;
+                }
+                /* TW: We can't switch off CRT1 on 301B-DH in 8bpp Modes if using LCD */
+                if  (xgi_video_info.disp_state & DISPTYPE_LCD)  {
+                        doit = FALSE;
+                }
+        }
+
+        /* TW: We can't switch off CRT1 if bridge is in slave mode */
+        if(xgi_video_info.hasVB != HASVB_NONE) {
+                inXGIIDXREG(XGIPART1, 0x00, reg);
+
+
+                if((reg & 0x50) == 0x10) {
+                        doit = FALSE;
+                }
+
+        } else XGIfb_crt1off = 0;
+
+        inXGIIDXREG(XGICR, 0x17, reg);
+        if((XGIfb_crt1off) && (doit))
+                reg &= ~0x80;
+        else
+                reg |= 0x80;
+        outXGIIDXREG(XGICR, 0x17, reg);
+
+        andXGIIDXREG(XGISR, IND_XGI_RAMDAC_CONTROL, ~0x04);
+
+        if((xgi_video_info.disp_state & DISPTYPE_TV) && (xgi_video_info.hasVB == HASVB_301)) {
+
+           inXGIIDXREG(XGIPART4, 0x01, reg);
+
+           if(reg < 0xB0) {             /* Set filter for XGI301 */
+
+                switch (xgi_video_info.video_width) {
+                   case 320:
+                        filter_tb = (xgi_video_info.TV_type == TVMODE_NTSC) ? 4 : 12;
+                        break;
+                   case 640:
+                        filter_tb = (xgi_video_info.TV_type == TVMODE_NTSC) ? 5 : 13;
+                        break;
+                   case 720:
+                        filter_tb = (xgi_video_info.TV_type == TVMODE_NTSC) ? 6 : 14;
+                        break;
+                   case 800:
+                        filter_tb = (xgi_video_info.TV_type == TVMODE_NTSC) ? 7 : 15;
+                        break;
+                   default:
+                        filter = -1;
+                        break;
+                }
+
+                orXGIIDXREG(XGIPART1, XGIfb_CRT2_write_enable, 0x01);
+
+                if(xgi_video_info.TV_type == TVMODE_NTSC) {
+
+                        andXGIIDXREG(XGIPART2, 0x3a, 0x1f);
+
+                        if (xgi_video_info.TV_plug == TVPLUG_SVIDEO) {
+
+                                andXGIIDXREG(XGIPART2, 0x30, 0xdf);
+
+                        } else if (xgi_video_info.TV_plug == TVPLUG_COMPOSITE) {
+
+                                orXGIIDXREG(XGIPART2, 0x30, 0x20);
+
+                                switch (xgi_video_info.video_width) {
+                                case 640:
+                                        outXGIIDXREG(XGIPART2, 0x35, 0xEB);
+                                        outXGIIDXREG(XGIPART2, 0x36, 0x04);
+                                        outXGIIDXREG(XGIPART2, 0x37, 0x25);
+                                        outXGIIDXREG(XGIPART2, 0x38, 0x18);
+                                        break;
+                                case 720:
+                                        outXGIIDXREG(XGIPART2, 0x35, 0xEE);
+                                        outXGIIDXREG(XGIPART2, 0x36, 0x0C);
+                                        outXGIIDXREG(XGIPART2, 0x37, 0x22);
+                                        outXGIIDXREG(XGIPART2, 0x38, 0x08);
+                                        break;
+                                case 800:
+                                        outXGIIDXREG(XGIPART2, 0x35, 0xEB);
+                                        outXGIIDXREG(XGIPART2, 0x36, 0x15);
+                                        outXGIIDXREG(XGIPART2, 0x37, 0x25);
+                                        outXGIIDXREG(XGIPART2, 0x38, 0xF6);
+                                        break;
+                                }
+                        }
+
+                } else if(xgi_video_info.TV_type == TVMODE_PAL) {
+
+                        andXGIIDXREG(XGIPART2, 0x3A, 0x1F);
+
+                        if (xgi_video_info.TV_plug == TVPLUG_SVIDEO) {
+
+                                andXGIIDXREG(XGIPART2, 0x30, 0xDF);
+
+                        } else if (xgi_video_info.TV_plug == TVPLUG_COMPOSITE) {
+
+                                orXGIIDXREG(XGIPART2, 0x30, 0x20);
+
+                                switch (xgi_video_info.video_width) {
+                                case 640:
+                                        outXGIIDXREG(XGIPART2, 0x35, 0xF1);
+                                        outXGIIDXREG(XGIPART2, 0x36, 0xF7);
+                                        outXGIIDXREG(XGIPART2, 0x37, 0x1F);
+                                        outXGIIDXREG(XGIPART2, 0x38, 0x32);
+                                        break;
+                                case 720:
+                                        outXGIIDXREG(XGIPART2, 0x35, 0xF3);
+                                        outXGIIDXREG(XGIPART2, 0x36, 0x00);
+                                        outXGIIDXREG(XGIPART2, 0x37, 0x1D);
+                                        outXGIIDXREG(XGIPART2, 0x38, 0x20);
+                                        break;
+                                case 800:
+                                        outXGIIDXREG(XGIPART2, 0x35, 0xFC);
+                                        outXGIIDXREG(XGIPART2, 0x36, 0xFB);
+                                        outXGIIDXREG(XGIPART2, 0x37, 0x14);
+                                        outXGIIDXREG(XGIPART2, 0x38, 0x2A);
+                                        break;
+                                }
+                        }
+                }
+
+                if ((filter >= 0) && (filter <=7)) {
+                        DPRINTK("FilterTable[%d]-%d: %02x %02x %02x %02x\n", filter_tb, filter,
+                                XGI_TV_filter[filter_tb].filter[filter][0],
+                                XGI_TV_filter[filter_tb].filter[filter][1],
+                                XGI_TV_filter[filter_tb].filter[filter][2],
+                                XGI_TV_filter[filter_tb].filter[filter][3]
+                        );
+                        outXGIIDXREG(XGIPART2, 0x35, (XGI_TV_filter[filter_tb].filter[filter][0]));
+                        outXGIIDXREG(XGIPART2, 0x36, (XGI_TV_filter[filter_tb].filter[filter][1]));
+                        outXGIIDXREG(XGIPART2, 0x37, (XGI_TV_filter[filter_tb].filter[filter][2]));
+                        outXGIIDXREG(XGIPART2, 0x38, (XGI_TV_filter[filter_tb].filter[filter][3]));
+                }
+
+             }
+
+        }
+
+}
+
+#ifndef MODULE
+XGIINITSTATIC int __init XGIfb_setup(char *options)
+{
+        char *this_opt;
+
+
+
+        xgi_video_info.refresh_rate = 0;
+
+        printk(KERN_INFO "XGIfb: Options %s\n", options);
+
+        if (!options || !*options)
+                return 0;
+
+        while((this_opt = strsep(&options, ",")) != NULL) {
+
+                if (!*this_opt) continue;
+
+                if (!strncmp(this_opt, "mode:", 5)) {
+                        XGIfb_search_mode(this_opt + 5);
+                } else if (!strncmp(this_opt, "vesa:", 5)) {
+                        XGIfb_search_vesamode(simple_strtoul(this_opt + 5, NULL, 0));
+                } else if (!strncmp(this_opt, "mode:", 5)) {
+                        XGIfb_search_mode(this_opt + 5);
+                } else if (!strncmp(this_opt, "vesa:", 5)) {
+                        XGIfb_search_vesamode(simple_strtoul(this_opt + 5, NULL, 0));
+                } else if (!strncmp(this_opt, "vrate:", 6)) {
+                        xgi_video_info.refresh_rate = simple_strtoul(this_opt + 6, NULL, 0);
+                } else if (!strncmp(this_opt, "rate:", 5)) {
+                        xgi_video_info.refresh_rate = simple_strtoul(this_opt + 5, NULL, 0);
+                } else if (!strncmp(this_opt, "off", 3)) {
+                        XGIfb_off = 1;
+                } else if (!strncmp(this_opt, "crt1off", 7)) {
+                        XGIfb_crt1off = 1;
+                } else if (!strncmp(this_opt, "filter:", 7)) {
+                        filter = (int)simple_strtoul(this_opt + 7, NULL, 0);
+                } else if (!strncmp(this_opt, "forcecrt2type:", 14)) {
+                        XGIfb_search_crt2type(this_opt + 14);
+                } else if (!strncmp(this_opt, "forcecrt1:", 10)) {
+                        XGIfb_forcecrt1 = (int)simple_strtoul(this_opt + 10, NULL, 0);
+                } else if (!strncmp(this_opt, "tvmode:",7)) {
+                        XGIfb_search_tvstd(this_opt + 7);
+                } else if (!strncmp(this_opt, "tvstandard:",11)) {
+                        XGIfb_search_tvstd(this_opt + 7);
+                } else if (!strncmp(this_opt, "mem:",4)) {
+                        XGIfb_mem = simple_strtoul(this_opt + 4, NULL, 0);
+                } else if (!strncmp(this_opt, "dstn", 4)) {
+                        enable_dstn = 1;
+                        /* TW: DSTN overrules forcecrt2type */
+                        XGIfb_crt2type = DISPTYPE_LCD;
+                } else if (!strncmp(this_opt, "queuemode:", 10)) {
+                        XGIfb_search_queuemode(this_opt + 10);
+                } else if (!strncmp(this_opt, "pdc:", 4)) {
+                        XGIfb_pdc = simple_strtoul(this_opt + 4, NULL, 0);
+                        if(XGIfb_pdc & ~0x3c) {
+                           printk(KERN_INFO "XGIfb: Illegal pdc parameter\n");
+                           XGIfb_pdc = 0;
+                        }
+                } else if (!strncmp(this_opt, "noaccel", 7)) {
+                        XGIfb_accel = 0;
+                } else if (!strncmp(this_opt, "noypan", 6)) {
+                        XGIfb_ypan = 0;
+                } else if (!strncmp(this_opt, "userom:", 7)) {
+                        XGIfb_userom = (int)simple_strtoul(this_opt + 7, NULL, 0);
+//              } else if (!strncmp(this_opt, "useoem:", 7)) {
+//                      XGIfb_useoem = (int)simple_strtoul(this_opt + 7, NULL, 0);
+                } else {
+                        XGIfb_search_mode(this_opt);
+//                      printk(KERN_INFO "XGIfb: Invalid option %s\n", this_opt);
+                }
+
+                /* TW: Acceleration only with MMIO mode */
+                if((XGIfb_queuemode != -1) && (XGIfb_queuemode != MMIO_CMD)) {
+                        XGIfb_ypan = 0;
+                        XGIfb_accel = 0;
+                }
+                /* TW: Panning only with acceleration */
+                if(XGIfb_accel == 0) XGIfb_ypan = 0;
+
+        }
+        printk("\nxgifb: outa xgifb_setup 3450");
+        return 0;
+}
+#endif
+
+static unsigned char VBIOS_BUF[65535];
+
+unsigned char* attempt_map_rom(struct pci_dev *dev,void *copy_address)
+{
+    u32 rom_size      = 0;
+    u32 rom_address   = 0;
+    int j;
+
+    /*  Get the size of the expansion rom */
+    pci_write_config_dword(dev, PCI_ROM_ADDRESS, 0xFFFFFFFF);
+    pci_read_config_dword(dev, PCI_ROM_ADDRESS, &rom_size);
+    if ((rom_size & 0x01) == 0)
+    {
+                printk("No ROM\n");
+                return NULL;
+    }
+
+    rom_size &= 0xFFFFF800;
+    rom_size = (~rom_size)+1;
+
+    rom_address = pci_resource_start(dev, 0);
+    if (rom_address == 0 || rom_address == 0xFFFFFFF0)
+    {
+                printk("No suitable rom address found\n"); return NULL;
+    }
+
+    printk("ROM Size is %dK, Address is %x\n", rom_size/1024, rom_address);
+
+    /*  Map ROM */
+    pci_write_config_dword(dev, PCI_ROM_ADDRESS, rom_address | PCI_ROM_ADDRESS_ENABLE);
+
+    /* memcpy(copy_address, rom_address, rom_size); */
+    {
+                unsigned char *virt_addr = ioremap(rom_address, 0x8000000);
+
+                unsigned char *from = (unsigned char *)virt_addr;
+                unsigned char *to = (unsigned char *)copy_address;
+                for (j=0; j<65536 /*rom_size*/; j++) *to++ = *from++;
+        }
+
+        pci_write_config_dword(dev, PCI_ROM_ADDRESS, 0);
+
+    printk("Copy is done\n");
+
+        return copy_address;
+}
+
+int __devinit xgifb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+        u16 reg16;
+        u8  reg, reg1;
+        u8 CR48,CR38;
+        if (XGIfb_off)
+                return -ENXIO;
+
+        XGIfb_registered = 0;
+
+        memset(&XGIhw_ext, 0, sizeof(HW_DEVICE_EXTENSION));
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,3))
+          fb_info = framebuffer_alloc(sizeof(struct fb_info), &pdev->dev);
+          if(!fb_info) return -ENOMEM;
+#else
+          XGI_fb_info = kmalloc( sizeof(struct fb_info), GFP_KERNEL);
+          if(!XGI_fb_info) return -ENOMEM;
+          memset(XGI_fb_info, 0,  sizeof(struct fb_info));
+#endif
+
+        xgi_video_info.chip_id = pdev->device;
+          pci_read_config_byte(pdev, PCI_REVISION_ID,&xgi_video_info.revision_id);
+          pci_read_config_word(pdev, PCI_COMMAND, &reg16);
+          XGIhw_ext.jChipRevision = xgi_video_info.revision_id;
+          XGIvga_enabled = reg16 & 0x01;
+
+          xgi_video_info.pcibus = pdev->bus->number;
+          xgi_video_info.pcislot = PCI_SLOT(pdev->devfn);
+          xgi_video_info.pcifunc = PCI_FUNC(pdev->devfn);
+          xgi_video_info.subsysvendor = pdev->subsystem_vendor;
+          xgi_video_info.subsysdevice = pdev->subsystem_device;
+
+          xgi_video_info.video_base = pci_resource_start(pdev, 0);
+          xgi_video_info.mmio_base = pci_resource_start(pdev, 1);
+          XGIfb_mmio_size =  pci_resource_len(pdev, 1);
+          xgi_video_info.vga_base = pci_resource_start(pdev, 2) + 0x30;
+          XGIhw_ext.pjIOAddress = (PUCHAR)xgi_video_info.vga_base;
+          //XGI_Pr.RelIO  = ioremap(pci_resource_start(pdev, 2), 128) + 0x30;
+          printk("XGIfb: Relocate IO address: %lx [%08lx] \n", (unsigned long)pci_resource_start(pdev, 2), XGI_Pr.RelIO);
+
+          if (pci_enable_device(pdev))
+                  return -EIO;
+
+    XGIRegInit(&XGI_Pr, (ULONG)XGIhw_ext.pjIOAddress);
+
+    outXGIIDXREG(XGISR, IND_XGI_PASSWORD, XGI_PASSWORD);
+    inXGIIDXREG(XGISR, IND_XGI_PASSWORD, reg1);
+
+    if(reg1 != 0xa1) /*I/O error */
+    {
+         printk("\nXGIfb: I/O error!!!");
+         return  -EIO;
+    }
+
+        switch (xgi_video_info.chip_id) {
+           case PCI_DEVICE_ID_XG_20:
+                orXGIIDXREG(XGICR, Index_CR_GPIO_Reg3, GPIOG_EN);
+                inXGIIDXREG(XGICR, Index_CR_GPIO_Reg1, CR48);
+                if (CR48&GPIOG_READ)
+                        xgi_video_info.chip = XG21;
+                else
+                        xgi_video_info.chip = XG20;
+                XGIfb_hwcursor_size = HW_CURSOR_AREA_SIZE_315 * 2;
+                XGIfb_CRT2_write_enable = IND_XGI_CRT2_WRITE_ENABLE_315;
+                break;
+            case PCI_DEVICE_ID_XG_40:
+                xgi_video_info.chip = XG40;
+                XGIfb_hwcursor_size = HW_CURSOR_AREA_SIZE_315 * 2;
+                XGIfb_CRT2_write_enable = IND_XGI_CRT2_WRITE_ENABLE_315;
+                break;
+            case PCI_DEVICE_ID_XG_41:
+                xgi_video_info.chip = XG41;
+                XGIfb_hwcursor_size = HW_CURSOR_AREA_SIZE_315 * 2;
+                XGIfb_CRT2_write_enable = IND_XGI_CRT2_WRITE_ENABLE_315;
+                break;
+            case PCI_DEVICE_ID_XG_42:
+                xgi_video_info.chip = XG42;
+                XGIfb_hwcursor_size = HW_CURSOR_AREA_SIZE_315 * 2;
+                XGIfb_CRT2_write_enable = IND_XGI_CRT2_WRITE_ENABLE_315;
+                break;
+            case PCI_DEVICE_ID_XG_27:
+                xgi_video_info.chip = XG27;
+                XGIfb_hwcursor_size = HW_CURSOR_AREA_SIZE_315 * 2;
+                XGIfb_CRT2_write_enable = IND_XGI_CRT2_WRITE_ENABLE_315;
+                break;
+           default:
+                return -ENODEV;
+        }
+
+        printk("XGIfb:chipid = %x\n",xgi_video_info.chip);
+         XGIhw_ext.jChipType = xgi_video_info.chip;
+
+                switch (xgi_video_info.chip) {
+                   case XG40:
+                   case XG41:
+                   case XG42:
+                   case XG45:
+                   case XG20:
+                   case XG21:
+                   case XG27:
+                   XGIhw_ext.bIntegratedMMEnabled = TRUE;
+                        break;
+
+                   default:
+                        break;
+                }
+
+
+          XGIhw_ext.pDevice = NULL;
+          if ((xgi_video_info.chip == XG21) || (XGIfb_userom))
+          {
+              XGIhw_ext.pjVirtualRomBase = attempt_map_rom(pdev, VBIOS_BUF);
+
+              if(XGIhw_ext.pjVirtualRomBase)
+                        printk(KERN_INFO "XGIfb: Video ROM found and mapped to %p\n",XGIhw_ext.pjVirtualRomBase);
+                else
+                        printk(KERN_INFO "XGIfb: Video ROM not found\n");
+    } else {
+                XGIhw_ext.pjVirtualRomBase = NULL;
+                printk(KERN_INFO "XGIfb: Video ROM usage disabled\n");
+    }
+          XGIhw_ext.pjCustomizedROMImage = NULL;
+          XGIhw_ext.bSkipDramSizing = 0;
+          XGIhw_ext.pQueryVGAConfigSpace = &XGIfb_query_VGA_config_space;
+//      XGIhw_ext.pQueryNorthBridgeSpace = &XGIfb_query_north_bridge_space;
+          strcpy(XGIhw_ext.szVBIOSVer, "0.84");
+
+
+    XGIhw_ext.pSR = vmalloc(sizeof(XGI_DSReg) * SR_BUFFER_SIZE);
+          if (XGIhw_ext.pSR == NULL)
+          {
+                    printk(KERN_ERR "XGIfb: Fatal error: Allocating SRReg space failed.\n");
+                    return -ENODEV;
+          }
+          XGIhw_ext.pSR[0].jIdx = XGIhw_ext.pSR[0].jVal = 0xFF;
+
+          XGIhw_ext.pCR = vmalloc(sizeof(XGI_DSReg) * CR_BUFFER_SIZE);
+          if (XGIhw_ext.pCR == NULL)
+          {
+              vfree(XGIhw_ext.pSR);
+                    printk(KERN_ERR "XGIfb: Fatal error: Allocating CRReg space failed.\n");
+                    return -ENODEV;
+          }
+          XGIhw_ext.pCR[0].jIdx = XGIhw_ext.pCR[0].jVal = 0xFF;
+
+
+
+
+        if (!XGIvga_enabled)
+        {
+                        /* Mapping Max FB Size for 315 Init */
+            XGIhw_ext.pjVideoMemoryAddress = ioremap(xgi_video_info.video_base, 0x10000000);
+            if((xgifb_mode_idx < 0) || ((XGIbios_mode[xgifb_mode_idx].mode_no) != 0xFF))
+            {
+#ifdef LINUXBIOS
+                printk("XGIfb: XGIInit() ...");
+                /* XGIInitNewt for LINUXBIOS only */
+                if(XGIInitNew(&XGIhw_ext))
+                {
+                        printk("OK\n");
+                }
+                else
+                {
+                    printk("Fail\n");
+                }
+#endif
+
+                outXGIIDXREG(XGISR, IND_XGI_PASSWORD, XGI_PASSWORD);
+
+
+            }
+        }
+#ifdef LINUXBIOS
+        else
+        {
+            XGIhw_ext.pjVideoMemoryAddress = ioremap(xgi_video_info.video_base, 0x10000000);
+            if((xgifb_mode_idx < 0) || ((XGIbios_mode[xgifb_mode_idx].mode_no) != 0xFF))
+            {
+
+                outXGIIDXREG(XGISR, IND_XGI_PASSWORD, XGI_PASSWORD);
+
+                // yilin Because no VBIOS DRAM Sizing, Dram size will error.
+                // Set SR13 ,14 temporarily for UDtech
+                outXGIIDXREG(XGISR, 0x13, 0x45);
+                outXGIIDXREG(XGISR, 0x14, 0x51);
+
+
+            }
+        }
+#endif
+        if (XGIfb_get_dram_size())
+        {
+            vfree(XGIhw_ext.pSR);
+            vfree(XGIhw_ext.pCR);
+            printk(KERN_INFO "XGIfb: Fatal error: Unable to determine RAM size.\n");
+            return -ENODEV;
+        }
+
+
+
+          if((xgifb_mode_idx < 0) || ((XGIbios_mode[xgifb_mode_idx].mode_no) != 0xFF))
+          {
+               /* Enable PCI_LINEAR_ADDRESSING and MMIO_ENABLE  */
+        orXGIIDXREG(XGISR, IND_XGI_PCI_ADDRESS_SET, (XGI_PCI_ADDR_ENABLE | XGI_MEM_MAP_IO_ENABLE));
+         /* Enable 2D accelerator engine */
+        orXGIIDXREG(XGISR, IND_XGI_MODULE_ENABLE, XGI_ENABLE_2D);
+    }
+
+          XGIhw_ext.ulVideoMemorySize = xgi_video_info.video_size;
+
+          if (!request_mem_region(xgi_video_info.video_base, xgi_video_info.video_size, "XGIfb FB"))
+          {         printk("unable request memory size %x",xgi_video_info.video_size);
+                    printk(KERN_ERR "XGIfb: Fatal error: Unable to reserve frame buffer memory\n");
+                    printk(KERN_ERR "XGIfb: Is there another framebuffer driver active?\n");
+                    vfree(XGIhw_ext.pSR);
+                    vfree(XGIhw_ext.pCR);
+                    return -ENODEV;
+          }
+
+          if (!request_mem_region(xgi_video_info.mmio_base, XGIfb_mmio_size, "XGIfb MMIO"))
+          {
+                    printk(KERN_ERR "XGIfb: Fatal error: Unable to reserve MMIO region\n");
+                    release_mem_region(xgi_video_info.video_base, xgi_video_info.video_size);
+                    vfree(XGIhw_ext.pSR);
+                    vfree(XGIhw_ext.pCR);
+                    return -ENODEV;
+          }
+
+          xgi_video_info.video_vbase = XGIhw_ext.pjVideoMemoryAddress =
+                ioremap(xgi_video_info.video_base, xgi_video_info.video_size);
+          xgi_video_info.mmio_vbase = ioremap(xgi_video_info.mmio_base, XGIfb_mmio_size);
+
+          printk(KERN_INFO "XGIfb: Framebuffer at 0x%lx, mapped to 0x%p, size %dk\n",
+              xgi_video_info.video_base, xgi_video_info.video_vbase,xgi_video_info.video_size / 1024);
+
+          printk(KERN_INFO "XGIfb: MMIO at 0x%lx, mapped to 0x%p, size %ldk\n",
+              xgi_video_info.mmio_base, xgi_video_info.mmio_vbase,XGIfb_mmio_size / 1024);
+          printk("XGIfb: XGIInitNew() ...");
+          if(XGIInitNew(&XGIhw_ext))
+          {
+                  printk("OK\n");
+          }
+          else
+          {
+                printk("Fail\n");
+          }
+
+          if(XGIfb_heap_init())
+          {
+                    printk(KERN_WARNING "XGIfb: Failed to initialize offscreen memory heap\n");
+          }
+
+
+          xgi_video_info.mtrr = (unsigned int) 0;
+
+          if((xgifb_mode_idx < 0) || ((XGIbios_mode[xgifb_mode_idx].mode_no) != 0xFF))
+          {
+                  xgi_video_info.hasVB = HASVB_NONE;
+        if((xgi_video_info.chip == XG20)||(xgi_video_info.chip == XG27))
+              xgi_video_info.hasVB = HASVB_NONE;
+        else if(xgi_video_info.chip == XG21) {
+            inXGIIDXREG(XGICR,0x38,CR38);
+            if ((CR38&0xE0) == 0xC0) {
+                    xgi_video_info.disp_state = DISPTYPE_LCD;
+                    if (!XGIfb_GetXG21LVDSData()) {
+                            int m;
+                            for (m=0; m < sizeof(XGI21_LCDCapList)/sizeof(XGI21_LVDSCapStruct); m++) {
+                                    if ((XGI21_LCDCapList[m].LVDSHDE == XGIbios_mode[xgifb_mode_idx].xres) &&
+                                        (XGI21_LCDCapList[m].LVDSVDE == XGIbios_mode[xgifb_mode_idx].yres)) {
+                                                XGINew_SetReg1( XGI_Pr.P3d4 , 0x36, m) ;
+                                    }
+                            }
+                    }
+            }
+            else if ((CR38&0xE0) == 0x60)
+                xgi_video_info.hasVB = HASVB_CHRONTEL ;
+            else
+                xgi_video_info.hasVB = HASVB_NONE;
+        }
+        else
+                    XGIfb_get_VB_type();
+
+                    XGIhw_ext.ujVBChipID = VB_CHIP_UNKNOWN;
+
+                    XGIhw_ext.ulExternalChip = 0;
+
+                switch (xgi_video_info.hasVB) {
+                case HASVB_301:
+                        inXGIIDXREG(XGIPART4, 0x01, reg);
+                        if (reg >= 0xE0) {
+                                XGIhw_ext.ujVBChipID = VB_CHIP_302LV;
+                                printk(KERN_INFO "XGIfb: XGI302LV bridge detected (revision 0x%02x)\n",reg);
+                        } else if (reg >= 0xD0) {
+                                XGIhw_ext.ujVBChipID = VB_CHIP_301LV;
+                                printk(KERN_INFO "XGIfb: XGI301LV bridge detected (revision 0x%02x)\n",reg);
+                        }
+                        /* else if (reg >= 0xB0) {
+                                XGIhw_ext.ujVBChipID = VB_CHIP_301B;
+                                inXGIIDXREG(XGIPART4,0x23,reg1);
+                               printk("XGIfb: XGI301B bridge detected\n");
+                        }*/
+                        else {
+                                XGIhw_ext.ujVBChipID = VB_CHIP_301;
+                                printk("XGIfb: XGI301 bridge detected\n");
+                        }
+                        break;
+                case HASVB_302:
+                        inXGIIDXREG(XGIPART4, 0x01, reg);
+                        if (reg >= 0xE0) {
+                                XGIhw_ext.ujVBChipID = VB_CHIP_302LV;
+                                printk(KERN_INFO "XGIfb: XGI302LV bridge detected (revision 0x%02x)\n",reg);
+                        } else if (reg >= 0xD0) {
+                                XGIhw_ext.ujVBChipID = VB_CHIP_301LV;
+                                printk(KERN_INFO "XGIfb: XGI302LV bridge detected (revision 0x%02x)\n",reg);
+                        } else if (reg >= 0xB0) {
+                                inXGIIDXREG(XGIPART4,0x23,reg1);
+
+                                XGIhw_ext.ujVBChipID = VB_CHIP_302B;
+
+                        } else {
+                                XGIhw_ext.ujVBChipID = VB_CHIP_302;
+                                printk(KERN_INFO "XGIfb: XGI302 bridge detected\n");
+                        }
+                        break;
+                case HASVB_LVDS:
+                        XGIhw_ext.ulExternalChip = 0x1;
+                        printk(KERN_INFO "XGIfb: LVDS transmitter detected\n");
+                        break;
+                case HASVB_TRUMPION:
+                        XGIhw_ext.ulExternalChip = 0x2;
+                        printk(KERN_INFO "XGIfb: Trumpion Zurac LVDS scaler detected\n");
+                        break;
+                case HASVB_CHRONTEL:
+                        XGIhw_ext.ulExternalChip = 0x4;
+                        printk(KERN_INFO "XGIfb: Chrontel TV encoder detected\n");
+                        break;
+                case HASVB_LVDS_CHRONTEL:
+                        XGIhw_ext.ulExternalChip = 0x5;
+                        printk(KERN_INFO "XGIfb: LVDS transmitter and Chrontel TV encoder detected\n");
+                        break;
+                default:
+                        printk(KERN_INFO "XGIfb: No or unknown bridge type detected\n");
+                        break;
+                }
+
+                if (xgi_video_info.hasVB != HASVB_NONE) {
+                    XGIfb_detect_VB();
+    }
+
+                if (xgi_video_info.disp_state & DISPTYPE_DISP2) {
+                        if (XGIfb_crt1off)
+                                xgi_video_info.disp_state |= DISPMODE_SINGLE;
+                        else
+                                xgi_video_info.disp_state |= (DISPMODE_MIRROR | DISPTYPE_CRT1);
+                } else {
+                        xgi_video_info.disp_state = DISPMODE_SINGLE | DISPTYPE_CRT1;
+                }
+
+                if (xgi_video_info.disp_state & DISPTYPE_LCD) {
+                    if (!enable_dstn) {
+                        inXGIIDXREG(XGICR, IND_XGI_LCD_PANEL, reg);
+                            reg &= 0x0f;
+                            XGIhw_ext.ulCRT2LCDType = XGI310paneltype[reg];
+
+                    } else {
+                        // TW: FSTN/DSTN
+                        XGIhw_ext.ulCRT2LCDType = LCD_320x480;
+                    }
+                }
+
+                XGIfb_detectedpdc = 0;
+
+                XGIfb_detectedlcda = 0xff;
+#ifndef LINUXBIOS
+
+                /* TW: Try to find about LCDA */
+
+        if((XGIhw_ext.ujVBChipID == VB_CHIP_302B) ||
+               (XGIhw_ext.ujVBChipID == VB_CHIP_301LV) ||
+               (XGIhw_ext.ujVBChipID == VB_CHIP_302LV))
+            {
+               int tmp;
+               inXGIIDXREG(XGICR,0x34,tmp);
+               if(tmp <= 0x13)
+               {
+                  // Currently on LCDA? (Some BIOSes leave CR38)
+                  inXGIIDXREG(XGICR,0x38,tmp);
+                      if((tmp & 0x03) == 0x03)
+                      {
+//                        XGI_Pr.XGI_UseLCDA = TRUE;
+                      }else
+                      {
+                     //  Currently on LCDA? (Some newer BIOSes set D0 in CR35)
+                         inXGIIDXREG(XGICR,0x35,tmp);
+                         if(tmp & 0x01)
+                         {
+//                            XGI_Pr.XGI_UseLCDA = TRUE;
+                           }else
+                           {
+                               inXGIIDXREG(XGICR,0x30,tmp);
+                               if(tmp & 0x20)
+                               {
+                                   inXGIIDXREG(XGIPART1,0x13,tmp);
+                                       if(tmp & 0x04)
+                                       {
+//                                      XGI_Pr.XGI_UseLCDA = TRUE;
+                                       }
+                               }
+                           }
+                        }
+                 }
+
+        }
+
+
+#endif
+
+                if (xgifb_mode_idx >= 0)
+                        xgifb_mode_idx = XGIfb_validate_mode(xgifb_mode_idx);
+
+                if (xgifb_mode_idx < 0) {
+                        switch (xgi_video_info.disp_state & DISPTYPE_DISP2) {
+                           case DISPTYPE_LCD:
+                                xgifb_mode_idx = DEFAULT_LCDMODE;
+                                if (xgi_video_info.chip == XG21)
+                                {
+                                    xgifb_mode_idx = XGIfb_GetXG21DefaultLVDSModeIdx();
+                                }
+                                break;
+                           case DISPTYPE_TV:
+                                xgifb_mode_idx = DEFAULT_TVMODE;
+                                break;
+                           default:
+                                xgifb_mode_idx = DEFAULT_MODE;
+                                break;
+                        }
+                }
+
+                XGIfb_mode_no = XGIbios_mode[xgifb_mode_idx].mode_no;
+
+
+                if( xgi_video_info.refresh_rate == 0)
+                    xgi_video_info.refresh_rate = 60; /*yilin set default refresh rate */
+                if(XGIfb_search_refresh_rate(xgi_video_info.refresh_rate) == 0)
+                {
+                    XGIfb_rate_idx = XGIbios_mode[xgifb_mode_idx].rate_idx;
+                    xgi_video_info.refresh_rate = 60;
+                }
+
+                xgi_video_info.video_bpp = XGIbios_mode[xgifb_mode_idx].bpp;
+                xgi_video_info.video_vwidth = xgi_video_info.video_width = XGIbios_mode[xgifb_mode_idx].xres;
+                xgi_video_info.video_vheight = xgi_video_info.video_height = XGIbios_mode[xgifb_mode_idx].yres;
+                xgi_video_info.org_x = xgi_video_info.org_y = 0;
+                xgi_video_info.video_linelength = xgi_video_info.video_width * (xgi_video_info.video_bpp >> 3);
+                switch(xgi_video_info.video_bpp) {
+                case 8:
+                        xgi_video_info.DstColor = 0x0000;
+                        xgi_video_info.XGI310_AccelDepth = 0x00000000;
+                        xgi_video_info.video_cmap_len = 256;
+                        break;
+                case 16:
+                        xgi_video_info.DstColor = 0x8000;
+                        xgi_video_info.XGI310_AccelDepth = 0x00010000;
+                        xgi_video_info.video_cmap_len = 16;
+                        break;
+                case 32:
+                        xgi_video_info.DstColor = 0xC000;
+                        xgi_video_info.XGI310_AccelDepth = 0x00020000;
+                        xgi_video_info.video_cmap_len = 16;
+                        break;
+                default:
+                        xgi_video_info.video_cmap_len = 16;
+                        printk(KERN_INFO "XGIfb: Unsupported depth %d", xgi_video_info.video_bpp);
+                        break;
+                }
+
+
+
+                printk(KERN_INFO "XGIfb: Default mode is %dx%dx%d (%dHz)\n",
+                        xgi_video_info.video_width, xgi_video_info.video_height, xgi_video_info.video_bpp,
+                        xgi_video_info.refresh_rate);
+
+                default_var.xres = default_var.xres_virtual = xgi_video_info.video_width;
+                default_var.yres = default_var.yres_virtual = xgi_video_info.video_height;
+                default_var.bits_per_pixel = xgi_video_info.video_bpp;
+
+                XGIfb_bpp_to_var(&default_var);
+
+                default_var.pixclock = (u32) (1000000000 /
+                                XGIfb_mode_rate_to_dclock(&XGI_Pr, &XGIhw_ext,
+                                                XGIfb_mode_no, XGIfb_rate_idx));
+
+                if(XGIfb_mode_rate_to_ddata(&XGI_Pr, &XGIhw_ext,
+                         XGIfb_mode_no, XGIfb_rate_idx,
+                         &default_var.left_margin, &default_var.right_margin,
+                         &default_var.upper_margin, &default_var.lower_margin,
+                         &default_var.hsync_len, &default_var.vsync_len,
+                         &default_var.sync, &default_var.vmode)) {
+
+                   if((default_var.vmode & FB_VMODE_MASK) == FB_VMODE_INTERLACED) {
+                      default_var.yres <<= 1;
+                      default_var.yres_virtual <<= 1;
+                   } else if((default_var.vmode & FB_VMODE_MASK) == FB_VMODE_DOUBLE) {
+                      default_var.pixclock >>= 1;
+                      default_var.yres >>= 1;
+                      default_var.yres_virtual >>= 1;
+                   }
+
+                }
+
+
+#if 0
+#ifdef XGIFB_PAN
+                if(XGIfb_ypan) {
+                        default_var.yres_virtual =
+                                xgi_video_info.heapstart / (default_var.xres * (default_var.bits_per_pixel >> 3));
+                        if(default_var.yres_virtual <= default_var.yres) {
+                                default_var.yres_virtual = default_var.yres;
+                        }
+                }
+#endif
+#endif
+
+
+                xgi_video_info.accel = 0;
+                if(XGIfb_accel) {
+                   xgi_video_info.accel = -1;
+                   default_var.accel_flags |= FB_ACCELF_TEXT;
+                   XGIfb_initaccel();
+                }
+
+                fb_info->flags = FBINFO_FLAG_DEFAULT;
+                fb_info->var = default_var;
+                fb_info->fix = XGIfb_fix;
+                fb_info->par = &xgi_video_info;
+                fb_info->screen_base = xgi_video_info.video_vbase;
+                fb_info->fbops = &XGIfb_ops;
+                XGIfb_get_fix(&fb_info->fix, -1, fb_info);
+                fb_info->pseudo_palette = pseudo_palette;
+
+                fb_alloc_cmap(&fb_info->cmap, 256 , 0);
+
+
+#ifdef CONFIG_MTRR
+                xgi_video_info.mtrr = mtrr_add((unsigned int) xgi_video_info.video_base,
+                                (unsigned int) xgi_video_info.video_size,
+                                MTRR_TYPE_WRCOMB, 1);
+                if(xgi_video_info.mtrr) {
+                        printk(KERN_INFO "XGIfb: Added MTRRs\n");
+                }
+#endif
+
+                if(register_framebuffer(fb_info) < 0)
+    {
+                        return -EINVAL;
+    }
+
+                XGIfb_registered = 1;
+
+                printk(KERN_INFO "XGIfb: Installed XGIFB_GET_INFO ioctl (%x)\n", XGIFB_GET_INFO);
+
+/*              printk(KERN_INFO "XGIfb: 2D acceleration is %s, scrolling mode %s\n",
+                     XGIfb_accel ? "enabled" : "disabled",
+                     XGIfb_ypan  ? "ypan" : "redraw");
+*/
+                printk(KERN_INFO "fb%d: %s frame buffer device, Version %d.%d.%02d\n",
+                        fb_info->node, myid, VER_MAJOR, VER_MINOR, VER_LEVEL);
+
+
+        }
+
+        dumpVGAReg();
+
+        return 0;
+}
+
+
+/*****************************************************/
+/*                PCI DEVICE HANDLING                */
+/*****************************************************/
+
+static void __devexit xgifb_remove(struct pci_dev *pdev)
+{
+        /* Unregister the framebuffer */
+//      if(xgi_video_info.registered) {
+                unregister_framebuffer(fb_info);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,3))
+                framebuffer_release(fb_info);
+#else
+                kfree(fb_info);
+#endif
+//      }
+
+        pci_set_drvdata(pdev, NULL);
+
+};
+
+static struct pci_driver xgifb_driver = {
+        .name           = "xgifb",
+        .id_table       = xgifb_pci_table,
+        .probe          = xgifb_probe,
+        .remove         = __devexit_p(xgifb_remove)
+};
+
+XGIINITSTATIC int __init xgifb_init(void)
+{
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,8)
+#ifndef MODULE
+        char *option = NULL;
+
+        if (fb_get_options("xgifb", &option))
+                return -ENODEV;
+        XGIfb_setup(option);
+#endif
+#endif
+        return(pci_register_driver(&xgifb_driver));
+}
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,8)
+#ifndef MODULE
+module_init(xgifb_init);
+#endif
+#endif
+
+/*****************************************************/
+/*                      MODULE                       */
+/*****************************************************/
+
+#ifdef MODULE
+
+static char         *mode = NULL;
+static int          vesa = 0;
+static unsigned int rate = 0;
+static unsigned int crt1off = 1;
+static unsigned int mem = 0;
+static char         *forcecrt2type = NULL;
+static int          forcecrt1 = -1;
+static int          pdc = -1;
+static int          pdc1 = -1;
+static int          noaccel = -1;
+static int          noypan  = -1;
+static int          nomax = -1;
+static int          userom = -1;
+static int          useoem = -1;
+static char         *tvstandard = NULL;
+static int          nocrt2rate = 0;
+static int          scalelcd = -1;
+static char         *specialtiming = NULL;
+static int          lvdshl = -1;
+static int          tvxposoffset = 0, tvyposoffset = 0;
+#if !defined(__i386__) && !defined(__x86_64__)
+static int          resetcard = 0;
+static int          videoram = 0;
+#endif
+
+MODULE_DESCRIPTION("Z7 Z9 Z9S Z11 framebuffer device driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("XGITECH , Others");
+
+
+
+module_param(mem, int, 0);
+module_param(noaccel, int, 0);
+module_param(noypan, int, 0);
+module_param(nomax, int, 0);
+module_param(userom, int, 0);
+module_param(useoem, int, 0);
+module_param(mode, charp, 0);
+module_param(vesa, int, 0);
+module_param(rate, int, 0);
+module_param(forcecrt1, int, 0);
+module_param(forcecrt2type, charp, 0);
+module_param(scalelcd, int, 0);
+module_param(pdc, int, 0);
+module_param(pdc1, int, 0);
+module_param(specialtiming, charp, 0);
+module_param(lvdshl, int, 0);
+module_param(tvstandard, charp, 0);
+module_param(tvxposoffset, int, 0);
+module_param(tvyposoffset, int, 0);
+module_param(filter, int, 0);
+module_param(nocrt2rate, int, 0);
+#if !defined(__i386__) && !defined(__x86_64__)
+module_param(resetcard, int, 0);
+module_param(videoram, int, 0);
+#endif
+
+
+MODULE_PARM_DESC(mem,
+        "\nDetermines the beginning of the video memory heap in KB. This heap is used\n"
+          "for video RAM management for eg. DRM/DRI. On 300 series, the default depends\n"
+          "on the amount of video RAM available. If 8MB of video RAM or less is available,\n"
+          "the heap starts at 4096KB, if between 8 and 16MB are available at 8192KB,\n"
+          "otherwise at 12288KB. On 315 and Xabre series, the heap size is 32KB by default.\n"
+          "The value is to be specified without 'KB' and must match the MaxXFBMem setting\n"
+          "for XFree86 4.x/X.org 6.7 and later.\n");
+
+MODULE_PARM_DESC(noaccel,
+        "\nIf set to anything other than 0, 2D acceleration will be disabled.\n"
+          "(default: 0)\n");
+
+MODULE_PARM_DESC(noypan,
+        "\nIf set to anything other than 0, y-panning will be disabled and scrolling\n"
+          "will be performed by redrawing the screen. (default: 0)\n");
+
+MODULE_PARM_DESC(nomax,
+        "\nIf y-panning is enabled, xgifb will by default use the entire available video\n"
+          "memory for the virtual screen in order to optimize scrolling performance. If\n"
+          "this is set to anything other than 0, xgifb will not do this and thereby \n"
+          "enable the user to positively specify a virtual Y size of the screen using\n"
+          "fbset. (default: 0)\n");
+
+
+
+MODULE_PARM_DESC(mode,
+       "\nSelects the desired default display mode in the format XxYxDepth,\n"
+         "eg. 1024x768x16. Other formats supported include XxY-Depth and\n"
+         "XxY-Depth@Rate. If the parameter is only one (decimal or hexadecimal)\n"
+         "number, it will be interpreted as a VESA mode number. (default: 800x600x8)\n");
+
+MODULE_PARM_DESC(vesa,
+       "\nSelects the desired default display mode by VESA defined mode number, eg.\n"
+         "0x117 (default: 0x0103)\n");
+
+
+MODULE_PARM_DESC(rate,
+        "\nSelects the desired vertical refresh rate for CRT1 (external VGA) in Hz.\n"
+          "If the mode is specified in the format XxY-Depth@Rate, this parameter\n"
+          "will be ignored (default: 60)\n");
+
+MODULE_PARM_DESC(forcecrt1,
+        "\nNormally, the driver autodetects whether or not CRT1 (external VGA) is \n"
+          "connected. With this option, the detection can be overridden (1=CRT1 ON,\n"
+          "0=CRT1 OFF) (default: [autodetected])\n");
+
+MODULE_PARM_DESC(forcecrt2type,
+        "\nIf this option is omitted, the driver autodetects CRT2 output devices, such as\n"
+          "LCD, TV or secondary VGA. With this option, this autodetection can be\n"
+          "overridden. Possible parameters are LCD, TV, VGA or NONE. NONE disables CRT2.\n"
+          "On systems with a SiS video bridge, parameters SVIDEO, COMPOSITE or SCART can\n"
+          "be used instead of TV to override the TV detection. Furthermore, on systems\n"
+          "with a SiS video bridge, SVIDEO+COMPOSITE, HIVISION, YPBPR480I, YPBPR480P,\n"
+          "YPBPR720P and YPBPR1080I are understood. However, whether or not these work\n"
+          "depends on the very hardware in use. (default: [autodetected])\n");
+
+MODULE_PARM_DESC(scalelcd,
+        "\nSetting this to 1 will force the driver to scale the LCD image to the panel's\n"
+          "native resolution. Setting it to 0 will disable scaling; LVDS panels will\n"
+          "show black bars around the image, TMDS panels will probably do the scaling\n"
+          "themselves. Default: 1 on LVDS panels, 0 on TMDS panels\n");
+
+MODULE_PARM_DESC(pdc,
+        "\nThis is for manually selecting the LCD panel delay compensation. The driver\n"
+          "should detect this correctly in most cases; however, sometimes this is not\n"
+          "possible. If you see 'small waves' on the LCD, try setting this to 4, 32 or 24\n"
+          "on a 300 series chipset; 6 on a 315 series chipset. If the problem persists,\n"
+          "try other values (on 300 series: between 4 and 60 in steps of 4; on 315 series:\n"
+          "any value from 0 to 31). (default: autodetected, if LCD is active during start)\n");
+
+MODULE_PARM_DESC(pdc1,
+        "\nThis is same as pdc, but for LCD-via CRT1. Hence, this is for the 315/330\n"
+          "series only. (default: autodetected if LCD is in LCD-via-CRT1 mode during\n"
+          "startup) - Note: currently, this has no effect because LCD-via-CRT1 is not\n"
+          "implemented yet.\n");
+
+MODULE_PARM_DESC(specialtiming,
+        "\nPlease refer to documentation for more information on this option.\n");
+
+MODULE_PARM_DESC(lvdshl,
+        "\nPlease refer to documentation for more information on this option.\n");
+
+MODULE_PARM_DESC(tvstandard,
+        "\nThis allows overriding the BIOS default for the TV standard. Valid choices are\n"
+          "pal, ntsc, palm and paln. (default: [auto; pal or ntsc only])\n");
+
+MODULE_PARM_DESC(tvxposoffset,
+        "\nRelocate TV output horizontally. Possible parameters: -32 through 32.\n"
+          "Default: 0\n");
+
+MODULE_PARM_DESC(tvyposoffset,
+        "\nRelocate TV output vertically. Possible parameters: -32 through 32.\n"
+          "Default: 0\n");
+
+MODULE_PARM_DESC(filter,
+        "\nSelects TV flicker filter type (only for systems with a SiS301 video bridge).\n"
+          "(Possible values 0-7, default: [no filter])\n");
+
+MODULE_PARM_DESC(nocrt2rate,
+        "\nSetting this to 1 will force the driver to use the default refresh rate for\n"
+          "CRT2 if CRT2 type is VGA. (default: 0, use same rate as CRT1)\n");
+
+
+
+
+int __init xgifb_init_module(void)
+{
+        printk("\nXGIfb_init_module");
+        if(mode)
+                XGIfb_search_mode(mode);
+        else if (vesa != -1)
+                XGIfb_search_vesamode(vesa);
+
+        return(xgifb_init());
+}
+
+static void __exit xgifb_remove_module(void)
+{
+        pci_unregister_driver(&xgifb_driver);
+        printk(KERN_DEBUG "xgifb: Module unloaded\n");
+}
+
+module_init(xgifb_init_module);
+module_exit(xgifb_remove_module);
+
+#endif     /*  /MODULE  */
+
+EXPORT_SYMBOL(XGI_malloc);
+EXPORT_SYMBOL(XGI_free);
+
diff --git a/drivers/staging/xgifb/XGIfb.h b/drivers/staging/xgifb/XGIfb.h
new file mode 100644
index 000000000000..41bf163d4e6b
--- /dev/null
+++ b/drivers/staging/xgifb/XGIfb.h
@@ -0,0 +1,215 @@
+#ifndef _LINUX_XGIFB
+#define _LINUX_XGIFB
+#include <linux/spinlock.h>
+#include <asm/ioctl.h>
+#include <asm/types.h>
+
+#define DISPTYPE_CRT1       0x00000008L
+#define DISPTYPE_CRT2       0x00000004L
+#define DISPTYPE_LCD        0x00000002L
+#define DISPTYPE_TV         0x00000001L
+#define DISPTYPE_DISP1      DISPTYPE_CRT1
+#define DISPTYPE_DISP2      (DISPTYPE_CRT2 | DISPTYPE_LCD | DISPTYPE_TV)
+#define DISPMODE_SINGLE     0x00000020L
+#define DISPMODE_MIRROR     0x00000010L
+#define DISPMODE_DUALVIEW   0x00000040L
+
+#define HASVB_NONE              0x00
+#define HASVB_301               0x01
+#define HASVB_LVDS              0x02
+#define HASVB_TRUMPION          0x04
+#define HASVB_LVDS_CHRONTEL     0x10
+#define HASVB_302               0x20
+#define HASVB_303               0x40
+#define HASVB_CHRONTEL          0x80
+
+#ifndef XGIFB_ID
+#define XGIFB_ID          0x53495346    /* Identify myself with 'XGIF' */
+#endif
+
+typedef enum _XGI_CHIP_TYPE {
+    XGI_VGALegacy = 0,
+    XGI_300,
+    XGI_630,
+    XGI_730,
+    XGI_540,
+    XGI_315H,
+    XGI_315,
+    XGI_315PRO,
+    XGI_550,
+    XGI_640,
+    XGI_740,
+    XGI_650,
+    XGI_650M,
+    XGI_330 = 16,
+    XGI_660,
+    XGI_661,
+    XGI_760,
+    XG40 = 32,
+    XG41,
+    XG42,
+    XG45,
+    XG20 = 48,
+    XG21,
+    XG27,
+    MAX_XGI_CHIP
+} XGI_CHIP_TYPE;
+
+typedef enum _TVTYPE {
+        TVMODE_NTSC = 0,
+        TVMODE_PAL,
+        TVMODE_HIVISION,
+        TVTYPE_PALM,    // vicki@030226
+        TVTYPE_PALN,    // vicki@030226
+        TVTYPE_NTSCJ,   // vicki@030226
+        TVMODE_TOTAL
+} XGI_TV_TYPE;
+
+
+typedef struct _XGIFB_INFO XGIfb_info;
+struct _XGIFB_INFO {
+
+unsigned long XGIfb_id;
+        int    chip_id;                 /* PCI ID of detected chip */
+        int    memory;                  /* video memory in KB which XGIfb manages */
+        int    heapstart;               /* heap start (= XGIfb "mem" argument) in KB */
+        unsigned char fbvidmode;        /* current XGIfb mode */
+
+        unsigned char XGIfb_version;
+        unsigned char XGIfb_revision;
+        unsigned char XGIfb_patchlevel;
+
+        unsigned char XGIfb_caps;       /* XGIfb capabilities */
+
+        int    XGIfb_tqlen;             /* turbo queue length (in KB) */
+
+        unsigned int XGIfb_pcibus;      /* The card's PCI ID */
+        unsigned int XGIfb_pcislot;
+        unsigned int XGIfb_pcifunc;
+
+        unsigned char XGIfb_lcdpdc;     /* PanelDelayCompensation */
+
+        unsigned char XGIfb_lcda;       /* Detected status of LCDA for low res/text modes */
+
+        char reserved[235];             /* for future use */
+};
+
+
+
+
+typedef enum _TVPLUGTYPE {      // vicki@030226
+//      TVPLUG_Legacy = 0,
+//      TVPLUG_COMPOSITE,
+//      TVPLUG_SVIDEO,
+//      TVPLUG_SCART,
+//      TVPLUG_TOTAL
+        TVPLUG_UNKNOWN = 0,
+        TVPLUG_COMPOSITE = 1,
+        TVPLUG_SVIDEO = 2,
+        TVPLUG_COMPOSITE_AND_SVIDEO = 3,
+        TVPLUG_SCART = 4,
+        TVPLUG_YPBPR_525i = 5,
+        TVPLUG_YPBPR_525P = 6,
+        TVPLUG_YPBPR_750P = 7,
+        TVPLUG_YPBPR_1080i = 8,
+        TVPLUG_TOTAL
+} XGI_TV_PLUG;
+
+
+struct mode_info {
+        int    bpp;
+        int    xres;
+        int    yres;
+        int    v_xres;
+        int    v_yres;
+        int    org_x;
+        int    org_y;
+        unsigned int  vrate;
+};
+
+struct ap_data {
+        struct mode_info minfo;
+        unsigned long iobase;
+        unsigned int  mem_size;
+        unsigned long disp_state;
+        XGI_CHIP_TYPE chip;
+        unsigned char hasVB;
+        XGI_TV_TYPE TV_type;
+        XGI_TV_PLUG TV_plug;
+        unsigned long version;
+        char reserved[256];
+};
+
+
+
+/*     If changing this, vgatypes.h must also be changed (for X driver)    */
+
+
+/*
+ * NOTE! The ioctl types used to be "size_t" by mistake, but were
+ * really meant to be __u32. Changed to "__u32" even though that
+ * changes the value on 64-bit architectures, because the value
+ * (with a 4-byte size) is also hardwired in vgatypes.h for user
+ * space exports. So "__u32" is actually more compatible, duh!
+ */
+#define XGIFB_GET_INFO          _IOR('n',0xF8,__u32)
+#define XGIFB_GET_VBRSTATUS     _IOR('n',0xF9,__u32)
+
+
+
+struct video_info{
+        int           chip_id;
+        unsigned int  video_size;
+        unsigned long video_base;
+        char  *       video_vbase;
+        unsigned long mmio_base;
+        char  *       mmio_vbase;
+        unsigned long vga_base;
+        unsigned long mtrr;
+        unsigned long heapstart;
+
+        int    video_bpp;
+        int    video_cmap_len;
+        int    video_width;
+        int    video_height;
+        int    video_vwidth;
+        int    video_vheight;
+        int    org_x;
+        int    org_y;
+        int    video_linelength;
+        unsigned int refresh_rate;
+
+        unsigned long disp_state;
+        unsigned char hasVB;
+        unsigned char TV_type;
+        unsigned char TV_plug;
+
+        XGI_CHIP_TYPE chip;
+        unsigned char revision_id;
+
+        unsigned short DstColor;
+        unsigned long  XGI310_AccelDepth;
+        unsigned long  CommandReg;
+
+        spinlock_t     lockaccel;
+
+        unsigned int   pcibus;
+        unsigned int   pcislot;
+        unsigned int   pcifunc;
+
+        int            accel;
+        unsigned short subsysvendor;
+        unsigned short subsysdevice;
+
+        char reserved[236];
+};
+
+
+extern struct video_info xgi_video_info;
+
+#ifdef __KERNEL__
+//extern void xgi_malloc(struct xgi_memreq *req);
+extern void xgi_free(unsigned long base);
+extern void xgi_dispinfo(struct ap_data *rec);
+#endif
+#endif
diff --git a/drivers/staging/xgifb/osdef.h b/drivers/staging/xgifb/osdef.h
new file mode 100644
index 000000000000..4bc7d3a7440c
--- /dev/null
+++ b/drivers/staging/xgifb/osdef.h
@@ -0,0 +1,153 @@
+#ifndef _OSDEF_H_
+#define _OSDEF_H_
+
+/* #define WINCE_HEADER*/
+/*#define WIN2000*/
+/* #define TC */
+#define LINUX_KERNEL
+/* #define LINUX_XF86 */
+
+/**********************************************************************/
+#ifdef LINUX_KERNEL
+//#include <linux/config.h>
+#endif
+
+
+/**********************************************************************/
+#ifdef TC
+#endif
+#ifdef WIN2000
+#endif
+#ifdef WINCE_HEADER
+#endif
+#ifdef LINUX_XF86
+#define LINUX
+#endif
+#ifdef LINUX_KERNEL
+#define LINUX
+#endif
+
+/**********************************************************************/
+#ifdef TC
+#define XGI_SetMemory(MemoryAddress,MemorySize,value) memset(MemoryAddress, value, MemorySize);
+#endif
+#ifdef WIN2000
+#define XGI_SetMemory(MemoryAddress,MemorySize,value) MemFill((PVOID) MemoryAddress,(ULONG) MemorySize,(UCHAR) value);
+#endif
+#ifdef WINCE_HEADER
+#define XGI_SetMemory(MemoryAddress,MemorySize,value) memset(MemoryAddress, value, MemorySize);
+#endif
+#ifdef LINUX_XF86
+#define XGI_SetMemory(MemoryAddress,MemorySize,value) memset(MemoryAddress, value, MemorySize)
+#endif
+#ifdef LINUX_KERNEL
+#define XGI_SetMemory(MemoryAddress,MemorySize,value) memset(MemoryAddress, value, MemorySize)
+#endif
+/**********************************************************************/
+
+/**********************************************************************/
+
+#ifdef TC
+#define XGI_MemoryCopy(Destination,Soruce,Length) memmove(Destination, Soruce, Length);
+#endif
+#ifdef WIN2000
+#define XGI_MemoryCopy(Destination,Soruce,Length)  /*VideoPortMoveMemory((PUCHAR)Destination , Soruce,length);*/
+#endif
+#ifdef WINCE_HEADER
+#define XGI_MemoryCopy(Destination,Soruce,Length) memmove(Destination, Soruce, Length);
+#endif
+#ifdef LINUX_XF86
+#define XGI_MemoryCopy(Destination,Soruce,Length) memcpy(Destination,Soruce,Length)
+#endif
+#ifdef LINUX_KERNEL
+#define XGI_MemoryCopy(Destination,Soruce,Length) memcpy(Destination,Soruce,Length)
+#endif
+
+/**********************************************************************/
+
+#ifdef OutPortByte
+#undef OutPortByte
+#endif /* OutPortByte */
+
+#ifdef OutPortWord
+#undef OutPortWord
+#endif /* OutPortWord */
+
+#ifdef OutPortLong
+#undef OutPortLong
+#endif /* OutPortLong */
+
+#ifdef InPortByte
+#undef InPortByte
+#endif /* InPortByte */
+
+#ifdef InPortWord
+#undef InPortWord
+#endif /* InPortWord */
+
+#ifdef InPortLong
+#undef InPortLong
+#endif /* InPortLong */
+
+/**********************************************************************/
+/*  TC                                                                */
+/**********************************************************************/
+
+#ifdef TC
+#define OutPortByte(p,v) outp((unsigned short)(p),(unsigned char)(v))
+#define OutPortWord(p,v) outp((unsigned short)(p),(unsigned short)(v))
+#define OutPortLong(p,v) outp((unsigned short)(p),(unsigned long)(v))
+#define InPortByte(p)    inp((unsigned short)(p))
+#define InPortWord(p)    inp((unsigned short)(p))
+#define InPortLong(p)    ((inp((unsigned short)(p+2))<<16) | inp((unsigned short)(p)))
+#endif
+
+/**********************************************************************/
+/*  LINUX XF86                                                        */
+/**********************************************************************/
+
+#ifdef LINUX_XF86
+#define OutPortByte(p,v) outb((CARD16)(p),(CARD8)(v))
+#define OutPortWord(p,v) outw((CARD16)(p),(CARD16)(v))
+#define OutPortLong(p,v) outl((CARD16)(p),(CARD32)(v))
+#define InPortByte(p)    inb((CARD16)(p))
+#define InPortWord(p)    inw((CARD16)(p))
+#define InPortLong(p)    inl((CARD16)(p))
+#endif
+
+#ifdef LINUX_KERNEL
+#define OutPortByte(p,v) outb((u8)(v),(p))
+#define OutPortWord(p,v) outw((u16)(v),(p))
+#define OutPortLong(p,v) outl((u32)(v),(p))
+#define InPortByte(p)    inb(p)
+#define InPortWord(p)    inw(p)
+#define InPortLong(p)    inl(p)
+#endif
+
+/**********************************************************************/
+/*  WIN 2000                                                          */
+/**********************************************************************/
+
+#ifdef WIN2000
+#define OutPortByte(p,v) VideoPortWritePortUchar ((PUCHAR) (p), (UCHAR) (v))
+#define OutPortWord(p,v) VideoPortWritePortUshort((PUSHORT) (p), (USHORT) (v))
+#define OutPortLong(p,v) VideoPortWritePortUlong ((PULONG) (p), (ULONG) (v))
+#define InPortByte(p)    VideoPortReadPortUchar  ((PUCHAR) (p))
+#define InPortWord(p)    VideoPortReadPortUshort ((PUSHORT) (p))
+#define InPortLong(p)    VideoPortReadPortUlong  ((PULONG) (p))
+#endif
+
+
+/**********************************************************************/
+/*  WIN CE                                                          */
+/**********************************************************************/
+
+#ifdef WINCE_HEADER
+#define OutPortByte(p,v) WRITE_PORT_UCHAR ((PUCHAR) (p), (UCHAR) (v))
+#define OutPortWord(p,v) WRITE_PORT_USHORT((PUSHORT) (p), (USHORT) (v))
+#define OutPortLong(p,v) WRITE_PORT_ULONG ((PULONG) (p), (ULONG) (v))
+#define InPortByte(p)    READ_PORT_UCHAR  ((PUCHAR) (p))
+#define InPortWord(p)    READ_PORT_USHORT ((PUSHORT) (p))
+#define InPortLong(p)    READ_PORT_ULONG  ((PULONG) (p))
+#endif
+#endif // _OSDEF_H_
diff --git a/drivers/staging/xgifb/vb_def.h b/drivers/staging/xgifb/vb_def.h
new file mode 100644
index 000000000000..17a7ada4926e
--- /dev/null
+++ b/drivers/staging/xgifb/vb_def.h
@@ -0,0 +1,1017 @@
+/* $XFree86: xc/programs/Xserver/hw/xfree86/drivers/xgi/initdef.h,v 1.4 2000/12/02 01:16:17 dawes Exp $ */
+#ifndef _INITDEF_
+#define _INITDEF_
+
+#ifndef NewScratch
+#define NewScratch
+#endif
+/* shampoo */
+#ifdef LINUX_KERNEL
+#define SEQ_ADDRESS_PORT          0x0014
+#define SEQ_DATA_PORT             0x0015
+#define MISC_OUTPUT_REG_READ_PORT 0x001C
+#define MISC_OUTPUT_REG_WRITE_PORT 0x0012
+#define GRAPH_DATA_PORT           0x1F
+#define GRAPH_ADDRESS_PORT        0x1E
+#define XGI_MASK_DUAL_CHIP        0x04  /* SR3A */
+#define CRTC_ADDRESS_PORT_COLOR   0x0024
+#define VIDEO_SUBSYSTEM_ENABLE_PORT 0x0013
+#define PCI_COMMAND             0x04
+#endif
+/* ~shampoo */
+
+
+#define VB_XGI301             0x0001  /*301b*/
+#define VB_XGI301B        0x0002
+#define VB_XGI302B        0x0004
+#define VB_XGI301LV     0x0008 /*301lv*/
+#define VB_XGI302LV     0x0010
+#define VB_XGI301C      0x0020       /* for 301C */
+#define  VB_NoLCD        0x8000
+/*end 301b*/
+
+#define VB_YPbPrInfo     0x07          /*301lv*/
+#define VB_YPbPr525i     0x00
+#define VB_YPbPr525p     0x01
+#define VB_YPbPr750p     0x02
+#define VB_YPbPr1080i    0x03
+
+/* #define CRT1Len 17 */
+#define LVDSCRT1Len             15
+#define CHTVRegDataLen          5
+
+/* #define ModeInfoFlag 0x07 */
+/* #define IsTextMode 0x07 */
+/* #define ModeText 0x00 */
+/* #define ModeCGA 0x01 */
+/* #define ModeEGA 0x02 */
+/* #define ModeVGA 0x03 */
+/* #define Mode15Bpp 0x04 */
+/* #define Mode16Bpp 0x05 */
+/* #define Mode24Bpp 0x06 */
+/* #define Mode32Bpp 0x07 */
+
+/* #define DACInfoFlag 0x18 */
+/* #define MemoryInfoFlag 0x1E0 */
+/* #define MemorySizeShift 0x05 */
+
+#define Charx8Dot               0x0200
+#define LineCompareOff          0x0400
+#define CRT2Mode                0x0800
+#define HalfDCLK                0x1000
+#define NoSupportSimuTV         0x2000
+#define DoubleScanMode          0x8000
+
+#define SupportAllCRT2          0x0078
+#define SupportTV               0x0008
+#define SupportHiVisionTV       0x0010
+#define SupportLCD              0x0020
+#define SupportRAMDAC2          0x0040
+#define NoSupportTV             0x0070
+#define NoSupportHiVisionTV     0x0060
+#define NoSupportLCD            0x0058
+#define SupportCHTV             0x0800
+#define SupportCRT2in301C       0x0100       /* for 301C */
+#define SupportTV1024           0x0800  /*301b*/
+#define SupportYPbPr            0x1000  /*301lv*/
+#define InterlaceMode           0x0080
+#define SyncPP                  0x0000
+#define SyncPN                  0x4000
+#define SyncNP                  0x8000
+/* #define SyncNN 0xc000 */
+#define ECLKindex0              0x0000
+#define ECLKindex1              0x0100
+#define ECLKindex2              0x0200
+#define ECLKindex3              0x0300
+#define ECLKindex4              0x0400
+
+#define SetSimuScanMode         0x0001
+#define SwitchToCRT2            0x0002
+/* #define SetCRT2ToTV 0x009C */
+#define SetCRT2ToAVIDEO         0x0004
+#define SetCRT2ToSVIDEO         0x0008
+#define SetCRT2ToSCART          0x0010
+#define SetCRT2ToLCD            0x0020
+#define SetCRT2ToRAMDAC         0x0040
+#define SetCRT2ToHiVisionTV     0x0080
+#define SetNTSCTV               0x0000
+/* #define SetPALTV 0x0100 */
+#define SetInSlaveMode          0x0200
+#define SetNotSimuMode          0x0400
+#define SetNotSimuTVMode        0x0400
+#define SetDispDevSwitch        0x0800
+#define LoadDACFlag             0x1000
+#define DisableCRT2Display      0x2000
+#define DriverMode              0x4000
+#define HotKeySwitch            0x8000
+#define SetCHTVOverScan         0x8000
+/* #define SetCRT2ToLCDA 0x8000 301b */
+#define PanelRGB18Bit           0x0100
+#define PanelRGB24Bit           0x0000
+
+#define TVOverScan              0x10
+#define TVOverScanShift         4
+#define ClearBufferFlag         0x20
+#define EnableDualEdge          0x01            /*301b*/
+#define SetToLCDA               0x02
+
+#define YPbPrModeInfo           0x38
+/* #define YPbPrMode525i 0x00 */
+/* #define YPbPrMode525p 0x08 */
+/* #define YPbPrMode750p 0x10 */
+/* #define YPbPrMode1080i 0x18 */
+
+#define SetSCARTOutput          0x01
+#define BoardTVType             0x02
+#define  EnablePALMN           0x40
+/* #define ProgrammingCRT2 0x01 */
+/* #define TVSimuMode 0x02 */
+/* #define RPLLDIV2XO 0x04 */
+/* #define LCDVESATiming 0x08 */
+/* #define EnableLVDSDDA 0x10 */
+#define SetDispDevSwitchFlag    0x20
+#define CheckWinDos             0x40
+#define SetJDOSMode             0x80
+
+#define Panel320x480              0x07/*fstn*/
+/* [ycchen] 02/12/03 Modify for Multi-Sync. LCD Support */
+#define PanelResInfo            0x1F    /* CR36 Panel Type/LCDResInfo */
+#define PanelRefInfo            0x60
+#define Panel800x600            0x01
+#define Panel1024x768           0x02
+#define Panel1024x768x75        0x22
+#define Panel1280x1024          0x03
+#define Panel1280x1024x75       0x23
+#define Panel640x480            0x04
+#define Panel1024x600           0x05
+#define Panel1152x864           0x06
+#define Panel1280x960           0x07
+#define Panel1152x768           0x08
+#define Panel1400x1050          0x09
+#define Panel1280x768           0x0A
+#define Panel1600x1200          0x0B
+
+#define PanelRef60Hz            0x00
+#define PanelRef75Hz            0x20
+#define LCDRGB18Bit             0x01
+
+#define ExtChipTrumpion         0x06
+#define ExtChipCH7005           0x08
+#define ExtChipMitacTV          0x0a
+#define LCDNonExpanding         0x10
+#define LCDNonExpandingShift    4
+#define LCDSync                 0x20
+#define LCDSyncBit              0xe0
+#define LCDSyncShift            6
+
+/* #define DDC2DelayTime 300 */
+
+#define CRT2DisplayFlag         0x2000
+/* #define LCDDataLen 8 */
+/* #define HiTVDataLen 12 */
+/* #define TVDataLen 16 */
+/* #define SetPALTV 0x0100 */
+#define HalfDCLK                0x1000
+#define NTSCHT                  1716
+#define NTSCVT                  525
+#define PALHT                   1728
+#define PALVT                   625
+#define StHiTVHT                892
+#define StHiTVVT                1126
+#define StHiTextTVHT            1000
+#define StHiTextTVVT            1126
+#define ExtHiTVHT               2100
+#define ExtHiTVVT               1125
+
+#define St750pTVHT              1716
+#define St750pTVVT               525
+#define Ext750pTVHT             1716
+#define Ext750pTVVT              525
+#define St525pTVHT              1716
+#define St525pTVVT               525
+#define Ext525pTVHT             1716
+#define Ext525pTVVT              525
+#define St525iTVHT              1716
+#define St525iTVVT               525
+#define Ext525iTVHT             1716
+#define Ext525iTVVT              525
+
+#define VCLKStartFreq           25
+#define SoftDramType            0x80
+#define VCLK40                  0x04
+
+#define VCLK162                 0x21
+
+#define LCDRGB18Bit             0x01
+#define LoadDACFlag             0x1000
+#define AfterLockCRT2           0x4000
+#define SetCRT2ToAVIDEO         0x0004
+#define SetCRT2ToSCART          0x0010
+#define Ext2StructSize          5
+
+
+#define YPbPr525iVCLK           0x03B
+#define YPbPr525iVCLK_2         0x03A
+
+#define SwitchToCRT2            0x0002
+/* #define LCDVESATiming 0x08 */
+#define SetSCARTOutput          0x01
+#define AVIDEOSense             0x01
+#define SVIDEOSense             0x02
+#define SCARTSense              0x04
+#define LCDSense                0x08
+#define Monitor1Sense           0x20
+#define Monitor2Sense           0x10
+#define HiTVSense               0x40
+#define BoardTVType             0x02
+#define HotPlugFunction         0x08
+#define StStructSize            0x06
+
+
+#define XGI_CRT2_PORT_00        0x00 - 0x030
+#define XGI_CRT2_PORT_04        0x04 - 0x030
+#define XGI_CRT2_PORT_10        0x10 - 0x30
+#define XGI_CRT2_PORT_12        0x12 - 0x30
+#define XGI_CRT2_PORT_14        0x14 - 0x30
+
+
+#define LCDNonExpanding         0x10
+#define ADR_CRT2PtrData         0x20E
+#define offset_Zurac            0x210
+#define ADR_LVDSDesPtrData      0x212
+#define ADR_LVDSCRT1DataPtr     0x214
+#define ADR_CHTVVCLKPtr         0x216
+#define ADR_CHTVRegDataPtr      0x218
+
+#define LVDSDataLen             6
+/* #define EnableLVDSDDA 0x10 */
+/* #define LVDSDesDataLen 3 */
+#define ActiveNonExpanding      0x40
+#define ActiveNonExpandingShift 6
+/* #define ActivePAL 0x20 */
+#define ActivePALShift          5
+/* #define ModeSwitchStatus 0x0F */
+#define SoftTVType              0x40
+#define SoftSettingAddr         0x52
+#define ModeSettingAddr         0x53
+
+/* #define SelectCRT1Rate 0x4 */
+
+#define _PanelType00             0x00
+#define _PanelType01             0x08
+#define _PanelType02             0x10
+#define _PanelType03             0x18
+#define _PanelType04             0x20
+#define _PanelType05             0x28
+#define _PanelType06             0x30
+#define _PanelType07             0x38
+#define _PanelType08             0x40
+#define _PanelType09             0x48
+#define _PanelType0A             0x50
+#define _PanelType0B             0x58
+#define _PanelType0C             0x60
+#define _PanelType0D             0x68
+#define _PanelType0E             0x70
+#define _PanelType0F             0x78
+
+
+#define PRIMARY_VGA       0     /* 1: XGI is primary vga 0:XGI is secondary vga */
+#define BIOSIDCodeAddr          0x235
+#define OEMUtilIDCodeAddr       0x237
+#define VBModeIDTableAddr       0x239
+#define OEMTVPtrAddr            0x241
+#define PhaseTableAddr          0x243
+#define NTSCFilterTableAddr     0x245
+#define PALFilterTableAddr      0x247
+#define OEMLCDPtr_1Addr         0x249
+#define OEMLCDPtr_2Addr         0x24B
+#define LCDHPosTable_1Addr      0x24D
+#define LCDHPosTable_2Addr      0x24F
+#define LCDVPosTable_1Addr      0x251
+#define LCDVPosTable_2Addr      0x253
+#define OEMLCDPIDTableAddr      0x255
+
+#define VBModeStructSize        5
+#define PhaseTableSize          4
+#define FilterTableSize         4
+#define LCDHPosTableSize        7
+#define LCDVPosTableSize        5
+#define OEMLVDSPIDTableSize     4
+#define LVDSHPosTableSize       4
+#define LVDSVPosTableSize       6
+
+#define VB_ModeID               0
+#define VB_TVTableIndex         1
+#define VB_LCDTableIndex        2
+#define VB_LCDHIndex            3
+#define VB_LCDVIndex            4
+
+#define OEMLCDEnable            0x0001
+#define OEMLCDDelayEnable       0x0002
+#define OEMLCDPOSEnable         0x0004
+#define OEMTVEnable             0x0100
+#define OEMTVDelayEnable        0x0200
+#define OEMTVFlickerEnable      0x0400
+#define OEMTVPhaseEnable        0x0800
+#define OEMTVFilterEnable       0x1000
+
+#define OEMLCDPanelIDSupport    0x0080
+
+/* #define LCDVESATiming 0x0001 //LCD Info CR37 */
+/* #define EnableLVDSDDA 0x0002 */
+#define EnableScalingLCD        0x0008
+#define SetPWDEnable            0x0004
+#define SetLCDtoNonExpanding    0x0010
+/* #define SetLCDPolarity 0x00E0 */
+#define SetLCDDualLink          0x0100
+#define SetLCDLowResolution     0x0200
+#define SetLCDStdMode           0x0400
+#define SetTVStdMode            0x0200
+#define SetTVLowResolution      0x0400
+/* =============================================================
+   for 310
+============================================================== */
+#define SoftDRAMType        0x80
+#define SoftSetting_OFFSET  0x52
+#define SR07_OFFSET  0x7C
+#define SR15_OFFSET  0x7D
+#define SR16_OFFSET  0x81
+#define SR17_OFFSET  0x85
+#define SR19_OFFSET  0x8D
+#define SR1F_OFFSET  0x99
+#define SR21_OFFSET  0x9A
+#define SR22_OFFSET  0x9B
+#define SR23_OFFSET  0x9C
+#define SR24_OFFSET  0x9D
+#define SR25_OFFSET  0x9E
+#define SR31_OFFSET  0x9F
+#define SR32_OFFSET  0xA0
+#define SR33_OFFSET  0xA1
+
+#define CR40_OFFSET  0xA2
+#define SR25_1_OFFSET  0xF6
+#define CR49_OFFSET  0xF7
+
+#define VB310Data_1_2_Offset  0xB6
+#define VB310Data_4_D_Offset  0xB7
+#define VB310Data_4_E_Offset  0xB8
+#define VB310Data_4_10_Offset 0xBB
+
+#define RGBSenseDataOffset    0xBD
+#define YCSenseDataOffset     0xBF
+#define VideoSenseDataOffset  0xC1
+#define OutputSelectOffset    0xF3
+
+#define ECLK_MCLK_DISTANCE  0x14
+#define VBIOSTablePointerStart    0x200
+#define StandTablePtrOffset       VBIOSTablePointerStart+0x02
+#define EModeIDTablePtrOffset     VBIOSTablePointerStart+0x04
+#define CRT1TablePtrOffset        VBIOSTablePointerStart+0x06
+#define ScreenOffsetPtrOffset     VBIOSTablePointerStart+0x08
+#define VCLKDataPtrOffset         VBIOSTablePointerStart+0x0A
+#define MCLKDataPtrOffset         VBIOSTablePointerStart+0x0E
+#define CRT2PtrDataPtrOffset      VBIOSTablePointerStart+0x10
+#define TVAntiFlickPtrOffset      VBIOSTablePointerStart+0x12
+#define TVDelayPtr1Offset         VBIOSTablePointerStart+0x14
+#define TVPhaseIncrPtr1Offset     VBIOSTablePointerStart+0x16
+#define TVYFilterPtr1Offset       VBIOSTablePointerStart+0x18
+#define LCDDelayPtr1Offset        VBIOSTablePointerStart+0x20
+#define TVEdgePtr1Offset          VBIOSTablePointerStart+0x24
+#define CRT2Delay1Offset          VBIOSTablePointerStart+0x28
+#define LCDDataDesOffset     VBIOSTablePointerStart-0x02
+#define LCDDataPtrOffset          VBIOSTablePointerStart+0x2A
+#define LCDDesDataPtrOffset     VBIOSTablePointerStart+0x2C
+#define LCDDataList             VBIOSTablePointerStart+0x22     /* add for GetLCDPtr */
+#define TVDataList              VBIOSTablePointerStart+0x36     /* add for GetTVPtr */
+/*  */
+/* Modify from 310.inc */
+/*  */
+/*  */
+
+
+#define         ShowMsgFlag                  0x20               /* SoftSetting */
+#define         ShowVESAFlag                 0x10
+#define         HotPlugFunction              0x08
+#define         ModeSoftSetting              0x04
+#define         TVSoftSetting                0x02
+#define         LCDSoftSetting               0x01
+
+#define         GatingCRTinLCDA              0x10
+#define         SetHiTVOutput                0x08
+#define         SetYPbPrOutput               0x04
+#define         BoardTVType                  0x02
+#define         SetSCARTOutput               0x01
+
+#define         ModeSettingYPbPr             0x02               /* TVModeSetting, Others as same as CR30 */
+
+/* TVModeSetting same as CR35 */
+
+/* LCDModeSetting same as CR37 */
+
+#define         EnableNewTVFont              0x10               /* MiscCapability */
+
+#define         EnableLCDOutput              0x80               /* LCDCfgSetting */
+
+#define         SoftDRAMType                 0x80               /* DRAMSetting */
+#define         SoftDRAMConfig               0x40
+#define         MosSelDRAMType               0x20
+#define         SDRAM                        000h
+#define         SGRAM                        0x01
+#define         ESDRAM                       0x02
+
+#define         EnableAGPCfgSetting          0x01               /* AGPCfgSetting */
+
+/* ---------------- SetMode Stack */
+#define         CRT1Len                      15
+#define         VCLKLen                      4
+#define         DefThreshold                 0x0100
+#define         ExtRegsSize                  (57+8+37+70+63+28+768+1)/64+1
+
+#define         VGA_XGI315                   0x0001       /* VGA Type Info */
+#define         VGA_SNewis315e                  0x0002       /* 315 series */
+#define         VGA_XGI550                   0x0004
+#define         VGA_XGI640                   0x0008
+#define         VGA_XGI740                   0x0010
+#define         VGA_XGI650                   0x0020
+#define         VGA_XGI650M                  0x0040
+#define         VGA_XGI651                   0x0080
+#define         VGA_XGI340                   0x0001       /* 340 series */
+#define         VGA_XGI330                   0x0001       /* 330 series */
+#define         VGA_XGI660                   0x0001       /* 660 series */
+
+#define         VB_XGI301                    0x0001       /* VB Type Info */
+#define         VB_XGI301B                   0x0002       /* 301 series */
+#define         VB_XGI302B                   0x0004
+#define         VB_NoLCD                     0x8000
+#define         VB_XGI301LV                  0x0008
+#define         VB_XGI302LV                  0x0010
+#define         VB_LVDS_NS                   0x0001       /* 3rd party chip */
+#define         VB_CH7017                    0x0002
+#define         VB_CH7007                    0x0080       /* [Billy] 07/05/03 */
+/* #define VB_LVDS_SI 0x0004 */
+
+#define         ModeInfoFlag                 0x0007
+#define         IsTextMode                   0x0007
+#define         ModeText                     0x0000
+#define         ModeCGA                      0x0001
+#define         ModeEGA                      0x0002       /* 16 colors mode */
+#define         ModeVGA                      0x0003       /* 256 colors mode */
+#define         Mode15Bpp                    0x0004       /* 15 Bpp Color Mode */
+#define         Mode16Bpp                    0x0005       /* 16 Bpp Color Mode */
+#define         Mode24Bpp                    0x0006       /* 24 Bpp Color Mode */
+#define         Mode32Bpp                    0x0007       /* 32 Bpp Color Mode */
+
+#define         DACInfoFlag                  0x0018
+#define         MONODAC                      0x0000
+#define         CGADAC                       0x0008
+#define         EGADAC                       0x0010
+#define         VGADAC                       0x0018
+
+#define         MemoryInfoFlag               0x01e0
+#define         MemorySizeShift              5
+#define         Need1MSize                   0x0000
+#define         Need2MSize                   0x0020
+#define         Need4MSize                   0x0060
+#define         Need8MSize                   0x00e0
+#define         Need16MSize                  0x01e0
+
+#define         Charx8Dot                    0x0200
+#define         LineCompareOff               0x0400
+#define         CRT2Mode                     0x0800
+#define         HalfDCLK                     0x1000
+#define         NoSupportSimuTV              0x2000
+#define         DoubleScanMode               0x8000
+
+/* -------------- Ext_InfoFlag */
+#define         SupportModeInfo              0x0007
+#define         Support256                   0x0003
+#define         Support15Bpp                 0x0004
+#define         Support16Bpp                 0x0005
+#define         Support24Bpp                 0x0006
+#define         Support32Bpp                 0x0007
+
+#define         SupportAllCRT2               0x0078
+#define         SupportTV                    0x0008
+#define         SupportHiVisionTV            0x0010
+#define         SupportLCD                   0x0020
+#define         SupportRAMDAC2               0x0040
+#define         NoSupportTV                  0x0070
+#define         NoSupportHiVisionTV          0x0060
+#define         NoSupportLCD                 0x0058
+#define         SupportTV1024                0x0800       /* 301btest */
+#define         SupportYPbPr                 0x1000       /* 301lv */
+#define         InterlaceMode                0x0080
+#define         SyncPP                       0x0000
+#define         SyncPN                       0x4000
+#define         SyncNP                       0x8000
+#define         SyncNN                       0xC000
+
+/* -------------- SetMode Stack/Scratch */
+#define         SetSimuScanMode              0x0001       /* VBInfo/CR30 & CR31 */
+#define         SwitchToCRT2                 0x0002
+#define         SetCRT2ToTV1                 0x009C
+#define         SetCRT2ToTV                  0x089C
+#define         SetCRT2ToAVIDEO              0x0004
+#define         SetCRT2ToSVIDEO              0x0008
+#define         SetCRT2ToSCART               0x0010
+#define         SetCRT2ToLCD                 0x0020
+#define         SetCRT2ToRAMDAC              0x0040
+#define         SetCRT2ToHiVisionTV          0x0080
+#define         SetCRT2ToLCDA                0x0100
+#define         SetInSlaveMode               0x0200
+#define         SetNotSimuMode               0x0400
+#define         HKEventMode                  0x0800
+#define         SetCRT2ToYPbPr               0x0800
+#define         LoadDACFlag                  0x1000
+#define         DisableCRT2Display           0x2000
+#define         DriverMode                   0x4000
+#define         SetCRT2ToDualEdge            0x8000
+#define         HotKeySwitch                 0x8000
+
+#define         ProgrammingCRT2              0x0001       /* Set Flag */
+#define         EnableVCMode                 0x0002
+#define         SetHKEventMode               0x0004
+#define         ReserveTVOption              0x0008
+#define         DisableRelocateIO            0x0010
+#define         Win9xDOSMode                 0x0020
+#define         JDOSMode                     0x0040
+/* #define SetWin9xforJap 0x0080 // not used now */
+/* #define SetWin9xforKorea 0x0100 // not used now */
+#define         GatingCRT                    0x0800
+#define         DisableChB                   0x1000
+#define         EnableChB                    0x2000
+#define         DisableChA                   0x4000
+#define         EnableChA                    0x8000
+
+#define         SetNTSCTV                    0x0000       /* TV Info */
+#define         SetPALTV                     0x0001
+#define         SetNTSCJ                     0x0002
+#define         SetPALMTV                    0x0004
+#define         SetPALNTV                    0x0008
+#define         SetCHTVUnderScan             0x0000
+/* #define SetCHTVOverScan 0x0010 */
+#define         SetYPbPrMode525i             0x0020
+#define         SetYPbPrMode525p             0x0040
+#define         SetYPbPrMode750p             0x0080
+#define         SetYPbPrMode1080i            0x0100
+#define         SetTVStdMode                 0x0200
+#define         SetTVLowResolution           0x0400
+#define         SetTVSimuMode                0x0800
+#define         TVSimuMode                   0x0800
+#define         RPLLDIV2XO                   0x1000
+#define         NTSC1024x768                 0x2000
+#define         SetTVLockMode                0x4000
+
+#define         LCDVESATiming                0x0001       /* LCD Info/CR37 */
+#define         EnableLVDSDDA                0x0002
+#define         EnableScalingLCD             0x0008
+#define         SetPWDEnable                 0x0004
+#define         SetLCDtoNonExpanding         0x0010
+#define         SetLCDPolarity               0x00e0
+#define         SetLCDDualLink               0x0100
+#define         SetLCDLowResolution          0x0200
+#define         SetLCDStdMode                0x0400
+
+#define         DefaultLCDCap                0x80ea       /* LCD Capability shampoo */
+#define         RLVDSDHL00                   0x0000
+#define         RLVDSDHL01                   0x0001
+#define         RLVDSDHL10                   0x0002       /* default */
+#define         RLVDSDHL11                   0x0003
+#define         EnableLCD24bpp               0x0004       /* default */
+#define         DisableLCD24bpp              0x0000
+#define         RLVDSClkSFT0                 0x0000
+#define         RLVDSClkSFT1                 0x0008       /* default */
+#define         EnableLVDSDCBal              0x0010
+#define         DisableLVDSDCBal             0x0000       /* default */
+#define         SinglePolarity               0x0020       /* default */
+#define         MultiPolarity                0x0000
+#define         LCDPolarity                  0x00c0       /* default: SyncNN */
+#define         LCDSingleLink                0x0000       /* default */
+#define         LCDDualLink                  0x0100
+#define         EnableSpectrum               0x0200
+#define         DisableSpectrum              0x0000       /* default */
+#define         PWDEnable                    0x0400
+#define         PWDDisable                   0x0000       /* default */
+#define         PWMEnable                    0x0800
+#define         PWMDisable                   0x0000       /* default */
+#define         EnableVBCLKDRVLOW            0x4000
+#define         EnableVBCLKDRVHigh           0x0000       /* default */
+#define         EnablePLLSPLOW               0x8000
+#define         EnablePLLSPHigh              0x0000       /* default */
+
+#define         LCDBToA                      0x20               /* LCD SetFlag */
+#define         StLCDBToA                    0x40
+#define         LockLCDBToA                  0x80
+#define         LCDToFull                    0x10
+#define         AVIDEOSense                  0x01               /* CR32 */
+#define         SVIDEOSense                  0x02
+#define         SCARTSense                   0x04
+#define         LCDSense                     0x08
+#define         Monitor2Sense                0x10
+#define         Monitor1Sense                0x20
+#define         HiTVSense                    0x40
+
+#ifdef                   NewScratch
+#define         YPbPrSense                   0x80    /* NEW SCRATCH */
+#endif
+
+#define         TVSense                      0xc7
+
+#define         TVOverScan                   0x10               /* CR35 */
+#define         TVOverScanShift              4
+
+#ifdef                   NewScratch
+#define         NTSCMode                     0x00
+#define         PALMode                      0x00
+#define         NTSCJMode                    0x02
+#define         PALMNMode                    0x0c
+#define         YPbPrMode                    0xe0
+#define         YPbPrMode525i                0x00
+#define         YPbPrMode525p                0x20
+#define         YPbPrMode750p                0x40
+#define         YPbPrMode1080i               0x60
+#else                    /* Old Scratch */
+#define         ClearBufferFlag              0x20
+#endif
+
+
+#define         LCDRGB18Bit                  0x01               /* CR37 */
+#define         LCDNonExpanding              0x10
+#define         LCDNonExpandingShift         4
+#define         LCDSync                      0x20
+#define         LCDSyncBit                   0xe0               /* H/V polarity & sync ID */
+#define         LCDSyncShift                 6
+
+#ifdef                   NewScratch
+#define         ScalingLCD                   0x08
+#else                    /* Old Scratch */
+#define         ExtChipType                  0x0e
+#define         ExtChip301                   0x02
+#define         ExtChipLVDS                  0x04
+#define         ExtChipCH7019                0x06
+#define         ScalingLCD                   0x10
+#endif
+
+#define         EnableDualEdge               0x01               /* CR38 */
+#define         SetToLCDA                    0x02
+#ifdef                   NewScratch
+#define         SetYPbPr                     0x04
+#define         DisableChannelA              0x08
+#define         DisableChannelB              0x10
+#define         ExtChipType                  0xe0
+#define         ExtChip301                   0x20
+#define         ExtChipLVDS                  0x40
+#define         ExtChipCH7019                0x60
+#else                    /* Old Scratch */
+#define         YPbPrSense                   0x04
+#define         SetYPbPr                     0x08
+#define         YPbPrMode                    0x30
+#define         YPbPrMode525i                0x00
+#define         YPbPrMode525p                0x10
+#define         YPbPrMode750p                0x20
+#define         YPbPrMode1080i               0x30
+#define         PALMNMode                    0xc0
+#endif
+
+#define         BacklightControlBit          0x01               /* CR3A */
+#define         Win9xforJap                  0x40
+#define         Win9xforKorea                0x80
+
+#define         ForceMDBits                  0x07               /* CR3B */
+#define         ForceMD_JDOS                 0x00
+#define         ForceMD_640x400T             0x01
+#define         ForceMD_640x350T             0x02
+#define         ForceMD_720x400T             0x03
+#define         ForceMD_640x480E             0x04
+#define         ForceMD_640x400E             0x05
+#define         ForceP1Bit                   0x10
+#define         ForceP2Bit                   0x20
+#define         EnableForceMDinBIOS          0x40
+#define         EnableForceMDinDrv           0x80
+
+#ifdef                   NewScratch                      /* New Scratch */
+/* ---------------------- VUMA Information */
+#define         LCDSettingFromCMOS           0x04               /* CR3C */
+#define         TVSettingFromCMOS            0x08
+#define         DisplayDeviceFromCMOS        0x10
+#define         HKSupportInSBIOS             0x20
+#define         OSDSupportInSBIOS            0x40
+#define         DisableLogo                  0x80
+
+/* ---------------------- HK Evnet Definition */
+#define         HKEvent                      0x0f               /* CR3D */
+#define         HK_ModeSwitch                0x01
+#define         HK_Expanding                 0x02
+#define         HK_OverScan                  0x03
+#define         HK_Brightness                0x04
+#define         HK_Contrast                  0x05
+#define         HK_Mute                      0x06
+#define         HK_Volume                    0x07
+#define         ModeSwitchStatus             0xf0
+#define         ActiveCRT1                   0x10
+#define         ActiveLCD                    0x0020
+#define         ActiveTV                     0x40
+#define         ActiveCRT2                   0x80
+
+#define         TVSwitchStatus               0x1f               /* CR3E */
+#define         ActiveAVideo                 0x01
+#define         ActiveSVideo                 0x02
+#define         ActiveSCART                  0x04
+#define         ActiveHiTV                   0x08
+#define         ActiveYPbPr                  0x10
+
+#define         EnableHKEvent                0x01               /* CR3F */
+#define         EnableOSDEvent               0x02
+#define         StartOSDEvent                0x04
+#define         IgnoreHKEvent                0x08
+#define         IgnoreOSDEvent               0x10
+#else                    /* Old Scratch */
+#define         OSD_SBIOS                    0x02       /* SR17 */
+#define         DisableLogo                  0x04
+#define         SelectKDOS                   0x08
+#define         KorWinMode                   0x10
+#define         KorMode3Bit                  0x0020
+#define         PSCCtrlBit                  0x40
+#define         NPSCCtrlBitShift             6
+#define         BlueScreenBit                0x80
+
+#define         HKEvent                      0x0f       /* CR79 */
+#define         HK_ModeSwitch                0x01
+#define         HK_Expanding                 0x02
+#define         HK_OverScan                  0x03
+#define         HK_Brightness                0x04
+#define         HK_Contrast                  0x05
+#define         HK_Mute                      0x06
+#define         HK_Volume                    0x07
+#define         ActivePAL                    0x0020
+#define         ActivePALShift               5
+#define         ActiveNonExpanding           0x40
+#define         ActiveNonExpandingShift      6
+#define         ActiveOverScan               0x80
+#define         ActiveOverScanShift          7
+
+#define         ModeSwitchStatus             0x0b       /* SR15 */
+#define         ActiveCRT1                   0x01
+#define         ActiveLCD                    0x02
+#define         ActiveCRT2                   0x08
+
+#define         TVSwitchStatus               0xf0       /* SR16 */
+#define         TVConfigShift                3
+#define         ActiveTV                     0x01
+#define         ActiveYPbPr                  0x04
+#define         ActiveAVideo                 0x10
+#define         ActiveSVideo                 0x0020
+#define         ActiveSCART                  0x40
+#define         ActiveHiTV                   0x80
+
+#define         EnableHKEvent                0x01       /* CR7A */
+#define         EnableOSDEvent               0x02
+#define         StartOSDEvent                0x04
+#define         CMOSSupport                  0x08
+#define         HotKeySupport                0x10
+#define         IngoreHKOSDEvent             0x20
+#endif
+
+/* //------------- Misc. Definition */
+#define         SelectCRT1Rate               00h
+/* #define SelectCRT2Rate 04h */
+
+#define         DDC1DelayTime                1000
+#ifdef           TRUMPION
+#define         DDC2DelayTime                15
+#else
+#define         DDC2DelayTime                150
+#endif
+
+#define         R_FACTOR                     04Dh
+#define         G_FACTOR                     097h
+#define         B_FACTOR                     01Ch
+/* --------------------------------------------------------- */
+/* translated from asm code 301def.h */
+/*  */
+/* --------------------------------------------------------- */
+#define         LCDDataLen                   8
+#define         HiTVDataLen                  12
+#define         TVDataLen                    12
+#define         LVDSCRT1Len_H                8
+#define         LVDSCRT1Len_V                7
+#define         LVDSDataLen                  6
+#define         LVDSDesDataLen               6
+#define         LCDDesDataLen                6
+#define         LVDSDesDataLen2              8
+#define         LCDDesDataLen2               8
+#define         CHTVRegLen                   16
+#define         CHLVRegLen                   12
+
+#define         StHiTVHT                     892
+#define         StHiTVVT                     1126
+#define         StHiTextTVHT                 1000
+#define         StHiTextTVVT                 1126
+#define         ExtHiTVHT                    2100
+#define         ExtHiTVVT                    1125
+#define         NTSCHT                       1716
+#define         NTSCVT                        525
+#define         NTSC1024x768HT               1908
+#define         NTSC1024x768VT                525
+#define         PALHT                        1728
+#define         PALVT                         625
+
+#define         YPbPrTV525iHT                1716            /* YPbPr */
+#define         YPbPrTV525iVT                 525
+#define         YPbPrTV525pHT                1716
+#define         YPbPrTV525pVT                 525
+#define         YPbPrTV750pHT                1650
+#define         YPbPrTV750pVT                 750
+
+#define         CRT2VCLKSel                  0xc0
+
+#define         CRT2Delay1                   0x04            /* XGI301 */
+#define         CRT2Delay2                   0x0A            /* 301B,302 */
+
+
+#define         VCLK25_175           0x00
+#define         VCLK28_322           0x01
+#define         VCLK31_5             0x02
+#define         VCLK36               0x03
+#define         VCLK40               0x04
+#define         VCLK43_163           0x05
+#define         VCLK44_9             0x06
+#define         VCLK49_5             0x07
+#define         VCLK50               0x08
+#define         VCLK52_406           0x09
+#define         VCLK56_25            0x0A
+#define         VCLK65               0x0B
+#define         VCLK67_765           0x0C
+#define         VCLK68_179           0x0D
+#define         VCLK72_852           0x0E
+#define         VCLK75               0x0F
+#define         VCLK75_8             0x10
+#define         VCLK78_75            0x11
+#define         VCLK79_411           0x12
+#define         VCLK83_95            0x13
+#define         VCLK84_8             0x14
+#define         VCLK86_6             0x15
+#define         VCLK94_5             0x16
+#define         VCLK104_998          0x17
+#define         VCLK105_882          0x18
+#define         VCLK108_2            0x19
+#define         VCLK109_175          0x1A
+#define         VCLK113_309          0x1B
+#define         VCLK116_406          0x1C
+#define         VCLK132_258          0x1D
+#define         VCLK135_5            0x1E
+#define         VCLK139_054          0x1F
+#define         VCLK157_5            0x20
+#define         VCLK162              0x21
+#define         VCLK175              0x22
+#define         VCLK189              0x23
+#define         VCLK194_4            0x24
+#define         VCLK202_5            0x25
+#define         VCLK229_5            0x26
+#define         VCLK234              0x27
+#define         VCLK252_699          0x28
+#define         VCLK254_817          0x29
+#define         VCLK265_728          0x2A
+#define         VCLK266_952          0x2B
+#define         VCLK269_655          0x2C
+#define         VCLK272_042          0x2D
+#define         VCLK277_015          0x2E
+#define         VCLK286_359          0x2F
+#define         VCLK291_132          0x30
+#define         VCLK291_766          0x31
+#define         VCLK309_789          0x32
+#define         VCLK315_195          0x33
+#define         VCLK323_586          0x34
+#define         VCLK330_615          0x35
+#define         VCLK332_177          0x36
+#define         VCLK340_477          0x37
+#define         VCLK375_847          0x38
+#define         VCLK388_631          0x39
+#define         VCLK125_999          0x51
+#define         VCLK148_5            0x52
+#define         VCLK178_992          0x54
+#define         VCLK217_325          0x55
+#define         VCLK299_505          0x56
+#define         YPbPr750pVCLK        0x57
+
+#define         TVVCLKDIV2              0x3A
+#define         TVVCLK                  0x3B
+#define         HiTVVCLKDIV2          0x3C
+#define         HiTVVCLK              0x3D
+#define         HiTVSimuVCLK          0x3E
+#define         HiTVTextVCLK          0x3F
+#define         VCLK39_77              0x40
+/* #define YPbPr750pVCLK 0x0F */
+#define         YPbPr525pVCLK           0x3A
+/* #define ;;YPbPr525iVCLK 0x3B */
+/* #define ;;YPbPr525iVCLK_2 0x3A */
+#define         NTSC1024VCLK         0x41
+#define         VCLK25_175_41        0x42                  /* ; ScaleLCD */
+#define         VCLK25_175_42        0x43
+#define         VCLK28_322_43        0x44
+#define         VCLK40_44            0x45
+#define         VCLKQVGA_1           0x46                   /* ; QVGA */
+#define         VCLKQVGA_2           0x47
+#define         VCLKQVGA_3           0x48
+#define         VCLK35_2             0x49                    /* ; 800x480 */
+#define         VCLK122_61           0x4A
+#define         VCLK80_350           0x4B
+#define         VCLK107_385          0x4C
+
+#define         CHTVVCLK30_2         0x50                 /* ;;CHTV */
+#define         CHTVVCLK28_1         0x51
+#define         CHTVVCLK43_6         0x52
+#define         CHTVVCLK26_4         0x53
+#define         CHTVVCLK24_6         0x54
+#define         CHTVVCLK47_8         0x55
+#define         CHTVVCLK31_5         0x56
+#define         CHTVVCLK26_2         0x57
+#define         CHTVVCLK39           0x58
+#define         CHTVVCLK36           0x59
+
+#define         CH7007TVVCLK30_2     0x00                 /* [Billy] 2007/05/18 For CH7007 */
+#define         CH7007TVVCLK28_1     0x01
+#define         CH7007TVVCLK43_6     0x02
+#define         CH7007TVVCLK26_4     0x03
+#define         CH7007TVVCLK24_6     0x04
+#define         CH7007TVVCLK47_8     0x05
+#define         CH7007TVVCLK31_5     0x06
+#define         CH7007TVVCLK26_2     0x07
+#define         CH7007TVVCLK39       0x08
+#define         CH7007TVVCLK36       0x09
+
+#define         RES320x200                   0x00
+#define         RES320x240                   0x01
+#define         RES400x300                   0x02
+#define         RES512x384                   0x03
+#define         RES640x400                   0x04
+#define         RES640x480x60                0x05
+#define         RES640x480x72                0x06
+#define         RES640x480x75                0x07
+#define         RES640x480x85                0x08
+#define         RES640x480x100               0x09
+#define         RES640x480x120               0x0A
+#define         RES640x480x160               0x0B
+#define         RES640x480x200               0x0C
+#define         RES800x600x56                0x0D
+#define         RES800x600x60                0x0E
+#define         RES800x600x72                0x0F
+#define         RES800x600x75                0x10
+#define         RES800x600x85                0x11
+#define         RES800x600x100               0x12
+#define         RES800x600x120               0x13
+#define         RES800x600x160               0x14
+#define         RES1024x768x43               0x15
+#define         RES1024x768x60               0x16
+#define         RES1024x768x70               0x17
+#define         RES1024x768x75               0x18
+#define         RES1024x768x85               0x19
+#define         RES1024x768x100              0x1A
+#define         RES1024x768x120              0x1B
+#define         RES1280x1024x43              0x1C
+#define         RES1280x1024x60              0x1D
+#define         RES1280x1024x75              0x1E
+#define         RES1280x1024x85              0x1F
+#define         RES1600x1200x60              0x20
+#define         RES1600x1200x65              0x21
+#define         RES1600x1200x70              0x22
+#define         RES1600x1200x75              0x23
+#define         RES1600x1200x85              0x24
+#define         RES1600x1200x100             0x25
+#define         RES1600x1200x120             0x26
+#define         RES1920x1440x60              0x27
+#define         RES1920x1440x65              0x28
+#define         RES1920x1440x70              0x29
+#define         RES1920x1440x75              0x2A
+#define         RES1920x1440x85              0x2B
+#define         RES1920x1440x100             0x2C
+#define         RES2048x1536x60              0x2D
+#define         RES2048x1536x65              0x2E
+#define         RES2048x1536x70              0x2F
+#define         RES2048x1536x75              0x30
+#define         RES2048x1536x85              0x31
+#define         RES800x480x60                0x32
+#define         RES800x480x75                0x33
+#define         RES800x480x85                0x34
+#define         RES1024x576x60               0x35
+#define         RES1024x576x75               0x36
+#define         RES1024x576x85               0x37
+#define         RES1280x720x60               0x38
+#define         RES1280x720x75               0x39
+#define         RES1280x720x85               0x3A
+#define         RES1280x960x60               0x3B
+#define         RES720x480x60                0x3C
+#define         RES720x576x56                0x3D
+#define         RES856x480x79I               0x3E
+#define         RES856x480x60                0x3F
+#define         RES1280x768x60               0x40
+#define         RES1400x1050x60              0x41
+#define         RES1152x864x60               0x42
+#define         RES1152x864x75               0x43
+#define         RES1024x768x160              0x44
+#define         RES1280x960x75               0x45
+#define         RES1280x960x85               0x46
+#define         RES1280x960x120              0x47
+
+#define         LFBDRAMTrap                  0x30
+#endif
diff --git a/drivers/staging/xgifb/vb_ext.c b/drivers/staging/xgifb/vb_ext.c
new file mode 100644
index 000000000000..49b39ee93a89
--- /dev/null
+++ b/drivers/staging/xgifb/vb_ext.c
@@ -0,0 +1,1370 @@
+#include "osdef.h"
+
+
+
+
+#ifdef WIN2000
+
+#include <dderror.h>
+#include <devioctl.h>
+#include <miniport.h>
+#include <ntddvdeo.h>
+#include <video.h>
+#include "xgiv.h"
+#include "dd_i2c.h"
+#include "tools.h"
+#endif /* WIN2000 */
+
+#ifdef LINUX_XF86
+#include "xf86.h"
+#include "xf86PciInfo.h"
+#include "xgi.h"
+#include "xgi_regs.h"
+#endif
+
+#ifdef LINUX_KERNEL
+#include <linux/version.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include "XGIfb.h"
+/*#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#include <video/XGIfb.h>
+#else
+#include <linux/XGIfb.h>
+#endif*/
+#endif
+
+
+
+#include "vb_def.h"
+#include "vgatypes.h"
+#include "vb_struct.h"
+#include "vb_util.h"
+#include "vb_setmode.h"
+#include "vb_ext.h"
+extern   UCHAR XGI330_SoftSetting;
+extern   UCHAR XGI330_OutputSelect;
+extern   USHORT XGI330_RGBSenseData2;
+extern   USHORT XGI330_YCSenseData2;
+extern   USHORT XGI330_VideoSenseData2;
+#ifdef WIN2000
+extern   UCHAR SenseCHTV(PHW_DEVICE_EXTENSION pHWDE);       /* 2007/05/17 Billy */
+#endif
+void     XGI_GetSenseStatus( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo );
+BOOLEAN  XGINew_GetPanelID(PVB_DEVICE_INFO pVBInfo);
+USHORT   XGINew_SenseLCD(PXGI_HW_DEVICE_INFO,PVB_DEVICE_INFO pVBInfo);
+BOOLEAN  XGINew_GetLCDDDCInfo(PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO pVBInfo);
+void XGISetDPMS( PXGI_HW_DEVICE_INFO pXGIHWDE , ULONG VESA_POWER_STATE ) ;
+BOOLEAN  XGINew_BridgeIsEnable(PXGI_HW_DEVICE_INFO,PVB_DEVICE_INFO pVBInfo );
+BOOLEAN  XGINew_Sense(USHORT tempbx,USHORT tempcx, PVB_DEVICE_INFO pVBInfo);
+BOOLEAN  XGINew_SenseHiTV( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo ) ;
+
+/**************************************************************
+        Dynamic Sense
+*************************************************************/
+
+void XGI_WaitDisplay(void);
+BOOLEAN XGI_Is301C(PVB_DEVICE_INFO);
+BOOLEAN XGI_Is301LV(PVB_DEVICE_INFO);
+
+#ifdef WIN2000
+UCHAR XGI_SenseLCD(PHW_DEVICE_EXTENSION, PVB_DEVICE_INFO);
+UCHAR XGI_GetLCDDDCInfo(PHW_DEVICE_EXTENSION,PVB_DEVICE_INFO);
+
+extern BOOL bGetDdcInfo(
+PHW_DEVICE_EXTENSION  pHWDE,
+ULONG                 ulWhichOne,
+PUCHAR                pjQueryBuffer,
+ULONG                 ulBufferSize
+   );
+
+#endif
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_Is301B */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGINew_Is301B( PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT flag ;
+
+    flag = XGINew_GetReg1( pVBInfo->Part4Port , 0x01 ) ;
+
+    if ( flag > 0x0B0 )
+        return( 0 ) ;   /* 301b */
+    else
+        return( 1 ) ;
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_Is301C */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_Is301C( PVB_DEVICE_INFO pVBInfo )
+{
+    if ( ( XGINew_GetReg1( pVBInfo->Part4Port , 0x01 ) & 0xF0 ) == 0xC0 )
+        return( 1 ) ;
+
+    if ( XGINew_GetReg1( pVBInfo->Part4Port , 0x01 ) >= 0xD0 )
+    {
+        if ( XGINew_GetReg1( pVBInfo->Part4Port , 0x39 ) == 0xE0 )
+            return( 1 ) ;
+    }
+
+    return( 0 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_Is301LV */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_Is301LV( PVB_DEVICE_INFO pVBInfo )
+{
+    if ( XGINew_GetReg1( pVBInfo->Part4Port , 0x01 ) >= 0xD0 )
+    {
+        if ( XGINew_GetReg1( pVBInfo->Part4Port , 0x39 ) == 0xFF )
+        {
+            return( 1 ) ;
+        }
+    }
+    return( 0 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_Sense */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGINew_Sense(  USHORT tempbx , USHORT tempcx, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT temp , i , tempch ;
+
+    temp = tempbx & 0xFF ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x11 , temp ) ;
+    temp = ( tempbx & 0xFF00 ) >> 8 ;
+    temp |= ( tempcx & 0x00FF ) ;
+    XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x10 , ~0x1F , temp ) ;
+
+    for( i = 0 ; i < 10 ; i++ )
+        XGI_LongWait( pVBInfo) ;
+
+    tempch = ( tempcx & 0x7F00 ) >> 8 ;
+    temp = XGINew_GetReg1( pVBInfo->Part4Port , 0x03 ) ;
+    temp = temp ^ ( 0x0E ) ;
+    temp &= tempch ;
+
+    if ( temp > 0 )
+        return( 1 ) ;
+    else
+        return( 0 ) ;
+}
+
+#ifdef WIN2000
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SenseLCD */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+UCHAR XGI_SenseLCD( PHW_DEVICE_EXTENSION pHWDE, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempax , tempbx , tempcx ;
+    UCHAR SoftSetting = XGI330_SoftSetting ;
+
+    if ( pVBInfo->VBType & ( VB_XGI301LV | VB_XGI302LV ) )
+        return( 1 ) ;
+
+
+    if ( SoftSetting & HotPlugFunction )        /* Hot Plug Detection */
+    {
+        XGINew_SetRegAND( pVBInfo->Part4Port , 0x0F , 0x3F ) ;
+        tempbx = 0 ;
+        tempcx = 0x9010 ;
+        if ( XGINew_Sense( tempbx , tempcx, pVBInfo ) )
+            return( 1 ) ;
+
+        return( 0 ) ;
+    }
+    else        /* Get LCD Info from EDID */
+        return(XGI_GetLCDDDCInfo(pHWDE, pVBInfo));
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetLCDDDCInfo */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+UCHAR XGI_GetLCDDDCInfo( PHW_DEVICE_EXTENSION pHWDE , PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR tempah , tempbl , tempbh ;
+    USHORT tempbx , temp ;
+    UCHAR pjEDIDBuf[ 256 ] ;
+    ULONG ulBufferSize = 256 ;
+    UCHAR bMASK_OUTPUTSTATE_CRT2LCD = 2 ; /* 0423 shampoo */
+
+    bGetDdcInfo( pHWDE , MASK_OUTPUTSTATE_CRT2LCD , pjEDIDBuf , ulBufferSize ) ;
+    if ( ( *( ( PULONG )pjEDIDBuf ) == 0xFFFFFF00 ) && ( *( ( PULONG )( pjEDIDBuf + 4 ) ) == 0x00FFFFFF ) )
+    {
+        tempah = Panel1024x768 ;
+        tempbl=( *( pjEDIDBuf + 0x3A ) ) & 0xf0 ;
+
+        if ( tempbl != 0x40 )
+        {
+            tempah = Panel1600x1200 ;
+            if ( tempbl != 0x60 )
+            {
+                tempah = Panel1280x1024 ;
+                tempbh = ( *( pjEDIDBuf + 0x3B ) ) ;
+                if ( tempbh != 0x00 )
+                {
+                    tempah = Panel1280x960 ;
+                    if ( tempbh != 0x0C0 )
+                    {
+                        tempbx = ( ( *( pjEDIDBuf + 0x24 ) ) << 8 ) | ( *( pjEDIDBuf + 0x23 ) ) ;
+                        tempah = Panel1280x1024 ;
+                        if ( !( tempbx & 0x0100 ) )
+                        {
+                            tempah = Panel1024x768 ;
+                            if ( !( tempbx & 0x0E00 ) )
+                            {
+                                tempah = Panel1280x1024 ;
+                            }
+                        }
+                    }
+
+                    if ( tempbx & 0x00FF )
+                    {
+                        temp = ScalingLCD ;
+                        XGINew_SetRegOR( pVBInfo->P3d4 , 0x37 , temp ) ;
+                    }
+                }
+            }
+        }
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x36 , ( ~0x07 ) , tempah ) ;
+        tempah = ( ( *( pjEDIDBuf + 0x47 ) ) & 0x06 ) ;         /* Polarity */
+        tempah = ( tempah ^ 0x06 ) << 4 ;
+        tempah |= LCDSync ;
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x37 , ( ~LCDSyncBit ) , tempah ) ;
+        tempbh= XGINew_GetReg1( pVBInfo->P3d4 , 0x36 ) ;
+        tempbh &= 0x07 ;
+        if ( tempbh == Panel1280x960 )
+            XGINew_SetRegAND( pVBInfo->P3d4 , 0x37 , 0x0E ) ;
+    }
+    else if ( *pjEDIDBuf == 0x20 )
+    {
+        tempah = Panel1024x768 ;
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x36 , ( ~0x07 ) , tempah ) ;
+    }
+    else
+    {
+        return( 0 ) ;
+    }
+
+    return( 1 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_DySense */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_DySense( PHW_DEVICE_EXTENSION pHWDE , PUCHAR ujConnectStatus)
+{
+    UCHAR pre_CRD,pre_SR1E , pre_Part2_0 , pre_Part4_D ;
+    USHORT tempax , tempbx , tempcx , pushax , temp ;
+    VB_DEVICE_INFO VBINF;
+    PVB_DEVICE_INFO pVBInfo = &VBINF;
+    UCHAR OutputSelect = XGI330_OutputSelect ;
+    PXGI_HW_DEVICE_INFO HwDeviceExtension= pHWDE->pXGIHWDE ;
+    UCHAR   bConnectStatus = 0 ;
+    pVBInfo->BaseAddr = HwDeviceExtension->pjIOAddress ;
+    pVBInfo->ROMAddr  = pHWDE->pjVirtualRomBase ;
+
+    pVBInfo->P3c2 = pVBInfo->BaseAddr + 0x12 ;
+    pVBInfo->P3c4 = pVBInfo->BaseAddr + 0x14 ;
+    pVBInfo->P3d4 = pVBInfo->BaseAddr + 0x24 ;
+    pVBInfo->Part2Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_10 ;
+    pVBInfo->Part4Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 ;
+    pushax = XGINew_GetReg1( pVBInfo->P3d4 , 0x17 ) ;   /* 0512 Fix Dysense hanged */
+    temp = ( pushax & 0x00FF ) | 0x80 ;
+    XGINew_SetRegOR( pVBInfo->P3d4 , 0x17 , temp ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x05 , 0x86 ) ;
+    /* beginning of dynamic sense CRT1 */
+
+    pVBInfo->IF_DEF_CH7007 = 0;
+    if (pHWDE->bCH7007)
+    {
+        InitTo330Pointer( pHWDE->pXGIHWDE->jChipType, pVBInfo ) ;
+        HwDeviceExtension->pDevice = (PVOID)pHWDE;
+        pVBInfo->IF_DEF_CH7007 = 1;
+        /* [Billy] 2007/05/14 For CH7007 */
+        if ( pVBInfo->IF_DEF_CH7007 == 1 )
+        {
+           bConnectStatus = SenseCHTV(HwDeviceExtension->pDevice) ; /* 07/05/28 */
+           XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x32 , ~0x03 , (UCHAR)bConnectStatus ) ;
+        }
+    }
+    if(( pHWDE->jChipID >= XG40 ) || ( pHWDE->jChipID >= XG20 ))
+    {
+
+        if ( pHWDE->jChipID >= XG40 )
+            XGINew_SetReg1( pVBInfo->P3d4 , 0x57 , 0x4A ) ;     /* write sense pattern 30->4a */
+        else
+            XGINew_SetReg1( pVBInfo->P3d4 , 0x57 , 0x5F ) ;     /* write sense pattern */
+
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x53 , 0xFF , 0x02 ) ;      /* enable sense DAC */
+        XGI_WaitDisply(pVBInfo) ;
+
+        if(XGINew_GetReg2( pVBInfo->P3c2 ) & 0x10 )
+            bConnectStatus |= Monitor1Sense ;
+
+        XGINew_SetRegAND( pVBInfo->P3d4 , 0x53 , 0xFD ) ;       /* disable sense DAC */
+        XGINew_SetRegAND( pVBInfo->P3d4 , 0x57 , 0x00 ) ;       /* clear sense pattern */
+
+
+        /* ---------- End of dynamic sense CRT1 ----------- */
+
+        /* ---------- beginning of dynamic sense VB ------------ */
+        pre_SR1E = XGINew_GetReg1( pVBInfo->P3c4 , 0x1E ) ;
+        XGINew_SetRegOR( pVBInfo->P3c4 , 0x1E , 0x20 ) ;        /* Enable CRT2,work-a-round for 301B/301LV/302LV */
+        pre_Part2_0 = XGINew_GetReg1( pVBInfo->Part2Port , 0x00 ) ;
+        pre_Part4_D = XGINew_GetReg1( pVBInfo->Part4Port , 0x0D ) ;
+
+        if ( XGI_Is301C( pVBInfo ) )    /* 301C only */
+            XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x0D , ~0x07 , 0x01 ) ;    /* Set Part4 0x0D D[2:0] to 001b */
+
+        /* tempax = 0 ; */
+        if ( !XGI_Is301LV( pVBInfo ) )
+        {
+           tempbx = XGI330_RGBSenseData2 ;
+            tempcx = 0x0E08 ;
+            if(XGINew_Sense( tempbx , tempcx, pVBInfo ) )
+            {
+                bConnectStatus |= Monitor2Sense ;
+                if ( OutputSelect & SetSCARTOutput )
+                {
+                    bConnectStatus ^= ( Monitor2Sense | SCARTSense ) ;
+                }
+            }
+        }
+        if ( XGI_Is301C( pVBInfo ) )    /* 301C only */
+            XGINew_SetRegOR( pVBInfo->Part4Port , 0x0D , 0x04 ) ;       /* Set Part4 0x0D D[2]=1 for dynamic sense */
+
+        if ( ( XGINew_Is301B( pVBInfo ) ) )
+            XGINew_SetRegOR( pVBInfo->Part2Port , 0x00 , 0x0C ) ;    /* ????????? */
+
+        if ( XGINew_SenseHiTV( HwDeviceExtension , pVBInfo) )           /* add by kuku for Dysense HiTV //start */
+        {
+            bConnectStatus|= YPbPrSense ;
+        }
+        else
+        {
+        tempbx = XGI330_YCSenseData2 ;  /* Y/C Sense Data Ptr */
+        tempcx = 0x0604 ;
+        if ( XGINew_Sense( tempbx , tempcx , pVBInfo) )
+            bConnectStatus |= SVIDEOSense ;
+
+        if ( OutputSelect & BoardTVType )
+        {
+            tempbx = XGI330_VideoSenseData2 ;
+            tempcx = 0x0804 ;
+            if ( XGINew_Sense(tempbx , tempcx, pVBInfo) )
+                bConnectStatus|= AVIDEOSense ;
+        }
+        else
+        {
+            if ( !( bConnectStatus & SVIDEOSense ) )
+            {
+                tempbx = XGI330_VideoSenseData2 ;
+                tempcx = 0x0804 ;
+                if ( XGINew_Sense( tempbx , tempcx, pVBInfo ) )
+                    bConnectStatus |= AVIDEOSense ;
+            }
+        }
+        } /* end */
+        /* DySenseVBCnt */
+
+        tempbx = 0 ;
+        tempcx = 0 ;
+        XGINew_Sense(tempbx , tempcx, pVBInfo ) ;
+
+        if ( !( bConnectStatus & Monitor2Sense ) )
+        {
+            if ( XGI_SenseLCD( pHWDE , pVBInfo ) )
+                bConnectStatus |= LCDSense ;
+        }
+
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x32 , ~( AVIDEOSense | SVIDEOSense | LCDSense | Monitor2Sense | Monitor1Sense ) , bConnectStatus ) ;
+
+        XGINew_SetReg1( pVBInfo->Part4Port , 0x0D , pre_Part4_D ) ;
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x00 , pre_Part2_0 ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x1E , pre_SR1E ) ;
+
+        if ( XGI_Is301C( pVBInfo ) )    /* 301C only */
+        {
+            tempax = XGINew_GetReg1( pVBInfo->Part2Port , 0x00 ) ;
+            if ( tempax & 0x20 )
+            {
+                /* Reset VBPro */
+                for( tempcx = 2 ; tempcx > 0 ; tempcx-- )
+                {
+                    tempax ^= 0x20 ;
+                    XGINew_SetReg1( pVBInfo->Part2Port , 0x00 , tempax ) ;
+                }
+            }
+        }
+        /* End of dynamic sense VB */
+    }
+    else
+    {
+        XGI_SenseCRT1(pVBInfo) ;
+        XGI_GetSenseStatus( HwDeviceExtension, pVBInfo ) ;      /* sense CRT2 */
+        bConnectStatus = XGINew_GetReg1( pVBInfo->P3d4 , 0x32 ) ;
+    }
+    temp = pushax & 0x00FF ;            /* 0512 Fix Dysense hanged */
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x17 , temp ) ;
+    if ( bConnectStatus )
+    {
+        *ujConnectStatus = bConnectStatus ;
+        return( 1 ) ;
+    }
+    else
+        return( 0 ) ;
+}
+
+#endif /* WIN2000 */
+
+/* --------------------------------------------------------------------- */
+/* Function : XGISetDPMS */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+VOID XGISetDPMS( PXGI_HW_DEVICE_INFO pXGIHWDE , ULONG VESA_POWER_STATE )
+{
+    USHORT ModeNo, ModeIdIndex ;
+    UCHAR  temp ;
+    VB_DEVICE_INFO VBINF;
+    PVB_DEVICE_INFO pVBInfo = &VBINF;
+    pVBInfo->BaseAddr = (ULONG)pXGIHWDE->pjIOAddress ;
+    pVBInfo->ROMAddr  = pXGIHWDE->pjVirtualRomBase ;
+
+
+    pVBInfo->IF_DEF_LVDS = 0 ;
+    pVBInfo->IF_DEF_CH7005 = 0 ;
+    pVBInfo->IF_DEF_HiVision = 1 ;
+    pVBInfo->IF_DEF_LCDA = 1 ;
+    pVBInfo->IF_DEF_CH7017 = 0 ;
+    pVBInfo->IF_DEF_YPbPr = 1 ;
+    pVBInfo->IF_DEF_CRT2Monitor = 0 ;
+    pVBInfo->IF_DEF_VideoCapture = 0 ;
+    pVBInfo->IF_DEF_ScaleLCD = 0 ;
+    pVBInfo->IF_DEF_OEMUtil = 0 ;
+    pVBInfo->IF_DEF_PWD = 0 ;
+
+    InitTo330Pointer( pXGIHWDE->jChipType,  pVBInfo ) ;
+    ReadVBIOSTablData( pXGIHWDE->jChipType , pVBInfo) ;
+
+    pVBInfo->P3c4 = pVBInfo->BaseAddr + 0x14 ;
+    pVBInfo->P3d4 = pVBInfo->BaseAddr + 0x24 ;
+    pVBInfo->P3c0 = pVBInfo->BaseAddr + 0x10 ;
+    pVBInfo->P3ce = pVBInfo->BaseAddr + 0x1e ;
+    pVBInfo->P3c2 = pVBInfo->BaseAddr + 0x12 ;
+    pVBInfo->P3ca = pVBInfo->BaseAddr + 0x1a ;
+    pVBInfo->P3c6 = pVBInfo->BaseAddr + 0x16 ;
+    pVBInfo->P3c7 = pVBInfo->BaseAddr + 0x17 ;
+    pVBInfo->P3c8 = pVBInfo->BaseAddr + 0x18 ;
+    pVBInfo->P3c9 = pVBInfo->BaseAddr + 0x19 ;
+    pVBInfo->P3da = pVBInfo->BaseAddr + 0x2A ;
+    pVBInfo->Part0Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_00 ;
+    pVBInfo->Part1Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_04 ;
+    pVBInfo->Part2Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_10 ;
+    pVBInfo->Part3Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_12 ;
+    pVBInfo->Part4Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 ;
+    pVBInfo->Part5Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 + 2 ;
+
+    if ( pXGIHWDE->jChipType == XG27 )
+    {
+        if ( ( XGINew_GetReg1( pVBInfo->P3d4 , 0x38 ) & 0xE0 ) == 0xC0 )
+        {
+          if ( XGINew_GetReg1( pVBInfo->P3d4 , 0x30 ) & 0x20 )
+          {
+            pVBInfo->IF_DEF_LVDS = 1 ;
+          }
+        }
+    }
+
+    if ( pVBInfo->IF_DEF_CH7007 == 0 )
+    {
+        XGINew_SetModeScratch ( pXGIHWDE , pVBInfo ) ;
+    }
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x05 , 0x86 ) ;     /* 1.Openkey */
+    XGI_UnLockCRT2( pXGIHWDE , pVBInfo) ;
+    ModeNo = XGINew_GetReg1( pVBInfo->P3d4 , 0x34 ) ;
+    XGI_SearchModeID( ModeNo , &ModeIdIndex, pVBInfo ) ;
+    XGI_GetVGAType( pXGIHWDE , pVBInfo ) ;
+
+    if ( ( pXGIHWDE->ujVBChipID == VB_CHIP_301 ) || ( pXGIHWDE->ujVBChipID == VB_CHIP_302 ) || ( pVBInfo->IF_DEF_CH7007 == 1 ))
+    {
+        XGI_GetVBType( pVBInfo ) ;
+        XGI_GetVBInfo( ModeNo , ModeIdIndex , pXGIHWDE, pVBInfo ) ;
+        XGI_GetTVInfo( ModeNo , ModeIdIndex, pVBInfo ) ;
+        XGI_GetLCDInfo( ModeNo , ModeIdIndex, pVBInfo ) ;
+    }
+
+    if ( VESA_POWER_STATE == 0x00000400 )
+      XGINew_SetReg1( pVBInfo->Part4Port , 0x31 , ( UCHAR )( XGINew_GetReg1( pVBInfo->Part4Port , 0x31 ) & 0xFE ) ) ;
+    else
+      XGINew_SetReg1( pVBInfo->Part4Port , 0x31 , ( UCHAR )( XGINew_GetReg1( pVBInfo->Part4Port , 0x31 ) | 0x01 ) ) ;
+
+    temp = ( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x1f ) ;
+    temp &= 0x3f ;
+    switch ( VESA_POWER_STATE )
+    {
+        case 0x00000000: /* on */
+            if ( ( pXGIHWDE->ujVBChipID == VB_CHIP_301 ) || ( pXGIHWDE->ujVBChipID == VB_CHIP_302 ) )
+            {
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x1f , ( UCHAR )( temp | 0x00 ) ) ;
+                XGI_EnableBridge( pXGIHWDE, pVBInfo ) ;
+            }
+            else
+            {
+               if ( pXGIHWDE->jChipType == XG21 )
+               {
+                 if ( pVBInfo->IF_DEF_LVDS == 1 )
+                 {
+                   XGI_XG21BLSignalVDD( 0x01 , 0x01, pVBInfo ) ; /* LVDS VDD on */
+                   XGI_XG21SetPanelDelay( 2,pVBInfo ) ;
+                 }
+               }
+               if ( pXGIHWDE->jChipType == XG27 )
+               {
+                 if ( pVBInfo->IF_DEF_LVDS == 1 )
+                 {
+                   XGI_XG27BLSignalVDD( 0x01 , 0x01, pVBInfo ) ; /* LVDS VDD on */
+                   XGI_XG21SetPanelDelay( 2,pVBInfo ) ;
+                 }
+               }
+               XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x1F , ~0xC0 , 0x00 ) ;
+               XGINew_SetRegAND( pVBInfo->P3c4 , 0x01 , ~0x20 ) ;               /* CRT on */
+
+               if ( pXGIHWDE->jChipType == XG21 )
+               {
+                 temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x38 ) ;
+                 if ( temp & 0xE0 )
+                 {
+                   XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x09 , ~0x80 , 0x80 ) ;  /* DVO ON */
+                   XGI_SetXG21FPBits( pVBInfo );
+                   XGINew_SetRegAND( pVBInfo->P3d4 , 0x4A , ~0x20 ) ;           /* Enable write GPIOF */
+                   /*XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x48 , ~0x20 , 0x20 ) ;*/      /* LCD Display ON */
+                 }
+                 XGI_XG21BLSignalVDD( 0x20 , 0x20, pVBInfo ) ; /* LVDS signal on */
+                 XGI_DisplayOn( pXGIHWDE, pVBInfo );
+               }
+               if ( pXGIHWDE->jChipType == XG27 )
+               {
+                 temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x38 ) ;
+                 if ( temp & 0xE0 )
+                 {
+                   XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x09 , ~0x80 , 0x80 ) ;  /* DVO ON */
+                   XGI_SetXG27FPBits( pVBInfo );
+                   XGINew_SetRegAND( pVBInfo->P3d4 , 0x4A , ~0x20 ) ;           /* Enable write GPIOF */
+                   /*XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x48 , ~0x20 , 0x20 ) ;*/      /* LCD Display ON */
+                 }
+                 XGI_XG27BLSignalVDD( 0x20 , 0x20, pVBInfo ) ; /* LVDS signal on */
+                 XGI_DisplayOn( pXGIHWDE, pVBInfo );
+               }
+            }
+            break ;
+        case 0x00000100: /* standby */
+            if ( pXGIHWDE->jChipType >= XG21 )
+            {
+                XGI_DisplayOff( pXGIHWDE, pVBInfo );
+            }
+
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x1f , ( UCHAR )( temp | 0x40 ) ) ;
+            break ;
+        case 0x00000200: /* suspend */
+            if ( pXGIHWDE->jChipType == XG21 )
+            {
+                XGI_DisplayOff( pXGIHWDE, pVBInfo );
+                XGI_XG21BLSignalVDD( 0x20 , 0x00, pVBInfo ) ; /* LVDS signal off */
+            }
+            if ( pXGIHWDE->jChipType == XG27 )
+            {
+                XGI_DisplayOff( pXGIHWDE, pVBInfo );
+                XGI_XG27BLSignalVDD( 0x20 , 0x00, pVBInfo ) ; /* LVDS signal off */
+            }
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x1f , ( UCHAR )( temp | 0x80 ) ) ;
+            break ;
+        case 0x00000400: /* off */
+            if ( (pXGIHWDE->ujVBChipID == VB_CHIP_301 ) || ( pXGIHWDE->ujVBChipID == VB_CHIP_302 ) )
+            {
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x1f , ( UCHAR )( temp | 0xc0 ) ) ;
+                XGI_DisableBridge( pXGIHWDE, pVBInfo ) ;
+            }
+            else
+            {
+               if ( pXGIHWDE->jChipType == XG21 )
+               {
+                 XGI_DisplayOff( pXGIHWDE, pVBInfo );
+
+                 XGI_XG21BLSignalVDD( 0x20 , 0x00, pVBInfo ) ; /* LVDS signal off */
+
+                 temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x38 ) ;
+                 if ( temp & 0xE0 )
+                 {
+                   XGINew_SetRegAND( pVBInfo->P3c4 , 0x09 , ~0x80 ) ;           /* DVO Off */
+                   XGINew_SetRegAND( pVBInfo->P3d4 , 0x4A , ~0x20 ) ;           /* Enable write GPIOF */
+                   /*XGINew_SetRegAND( pVBInfo->P3d4 , 0x48 , ~0x20 ) ;*/               /* LCD Display OFF */
+                 }
+               }
+               if ( pXGIHWDE->jChipType == XG27 )
+               {
+                 XGI_DisplayOff( pXGIHWDE, pVBInfo );
+
+                 XGI_XG27BLSignalVDD( 0x20 , 0x00, pVBInfo ) ; /* LVDS signal off */
+
+                 temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x38 ) ;
+                 if ( temp & 0xE0 )
+                 {
+                   XGINew_SetRegAND( pVBInfo->P3c4 , 0x09 , ~0x80 ) ;           /* DVO Off */
+                 }
+               }
+               XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x1F , ~0xC0 , 0xC0 ) ;
+               XGINew_SetRegOR( pVBInfo->P3c4 , 0x01 , 0x20 ) ;         /* CRT Off */
+
+               if ( ( pXGIHWDE->jChipType == XG21 ) && ( pVBInfo->IF_DEF_LVDS == 1 ) )
+               {
+                 XGI_XG21SetPanelDelay( 4,pVBInfo ) ;
+                 XGI_XG21BLSignalVDD( 0x01 , 0x00, pVBInfo ) ; /* LVDS VDD off */
+                 XGI_XG21SetPanelDelay( 5,pVBInfo ) ;
+               }
+               if ( ( pXGIHWDE->jChipType == XG27 ) && ( pVBInfo->IF_DEF_LVDS == 1 ) )
+               {
+                 XGI_XG21SetPanelDelay( 4,pVBInfo ) ;
+                 XGI_XG27BLSignalVDD( 0x01 , 0x00, pVBInfo ) ; /* LVDS VDD off */
+                 XGI_XG21SetPanelDelay( 5,pVBInfo ) ;
+               }
+            }
+            break ;
+
+        default:
+            break ;
+    }
+    XGI_LockCRT2( pXGIHWDE , pVBInfo ) ;
+}
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetSenseStatus */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetSenseStatus( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempax = 0 , tempbx , tempcx , temp ,
+           P2reg0 = 0 , SenseModeNo = 0 , OutputSelect = *pVBInfo->pOutputSelect ,
+           ModeIdIndex , i ;
+    pVBInfo->BaseAddr = (ULONG)HwDeviceExtension->pjIOAddress ;
+
+    if ( pVBInfo->IF_DEF_LVDS == 1 )
+    {
+        tempax = XGINew_GetReg1( pVBInfo->P3c4 , 0x1A ) ;       /* ynlai 02/27/2002 */
+        tempbx = XGINew_GetReg1( pVBInfo->P3c4 , 0x1B ) ;
+        tempax = ( ( tempax & 0xFE ) >> 1 ) | ( tempbx << 8 ) ;
+        if ( tempax == 0x00 )
+        {               /* Get Panel id from DDC */
+            temp = XGINew_GetLCDDDCInfo( HwDeviceExtension, pVBInfo ) ;
+            if ( temp == 1 )
+            {           /* LCD connect */
+                XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x39 , 0xFF , 0x01 ) ;      /* set CR39 bit0="1" */
+                XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x37 , 0xEF , 0x00 ) ;      /* clean CR37 bit4="0" */
+                temp = LCDSense ;
+            }
+            else
+            {           /* LCD don't connect */
+                temp = 0 ;
+            }
+        }
+        else
+        {
+            XGINew_GetPanelID(pVBInfo) ;
+            temp = LCDSense ;
+        }
+
+        tempbx = ~( LCDSense | AVIDEOSense | SVIDEOSense ) ;
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x32 , tempbx , temp ) ;
+    }
+    else
+    {           /* for 301 */
+        if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+        {       /* for HiVision */
+            tempax = XGINew_GetReg1( pVBInfo->P3c4 , 0x38 ) ;
+            temp = tempax & 0x01 ;
+            tempax = XGINew_GetReg1( pVBInfo->P3c4 , 0x3A ) ;
+            temp = temp | ( tempax & 0x02 ) ;
+            XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x32 , 0xA0 , temp ) ;
+        }
+        else
+        {
+            if ( XGI_BridgeIsOn( pVBInfo ) )
+            {
+                P2reg0 = XGINew_GetReg1( pVBInfo->Part2Port , 0x00 ) ;
+                if ( !XGINew_BridgeIsEnable( HwDeviceExtension, pVBInfo ) )
+                {
+                    SenseModeNo = 0x2e ;
+                    /* XGINew_SetReg1( pVBInfo->P3d4 , 0x30 , 0x41 ) ; */
+                    /* XGISetModeNew( HwDeviceExtension , 0x2e ) ; // ynlai InitMode */
+
+                    temp = XGI_SearchModeID( SenseModeNo , &ModeIdIndex, pVBInfo ) ;
+                    XGI_GetVGAType( HwDeviceExtension , pVBInfo) ;
+                    XGI_GetVBType( pVBInfo ) ;
+                    pVBInfo->SetFlag = 0x00 ;
+                    pVBInfo->ModeType = ModeVGA ;
+                    pVBInfo->VBInfo = SetCRT2ToRAMDAC | LoadDACFlag | SetInSlaveMode ;
+                    XGI_GetLCDInfo( 0x2e , ModeIdIndex, pVBInfo ) ;
+                    XGI_GetTVInfo(  0x2e , ModeIdIndex, pVBInfo ) ;
+                    XGI_EnableBridge( HwDeviceExtension, pVBInfo ) ;
+                    XGI_SetCRT2Group301( SenseModeNo , HwDeviceExtension, pVBInfo ) ;
+                    XGI_SetCRT2ModeRegs( 0x2e , HwDeviceExtension, pVBInfo ) ;
+                    /* XGI_DisableBridge( HwDeviceExtension, pVBInfo ) ; */
+                    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x01 , 0xDF , 0x20 ) ;  /* Display Off 0212 */
+                    for( i = 0 ; i < 20 ; i++ )
+                    {
+                        XGI_LongWait(pVBInfo) ;
+                    }
+                }
+                XGINew_SetReg1( pVBInfo->Part2Port , 0x00 , 0x1c ) ;
+                tempax = 0 ;
+                tempbx = *pVBInfo->pRGBSenseData ;
+
+                if ( !( XGINew_Is301B( pVBInfo ) ) )
+                {
+                    tempbx = *pVBInfo->pRGBSenseData2 ;
+                }
+
+                tempcx = 0x0E08 ;
+                if ( XGINew_Sense(tempbx , tempcx, pVBInfo ) )
+                {
+                    if ( XGINew_Sense(tempbx , tempcx, pVBInfo ) )
+                    {
+                        tempax |= Monitor2Sense ;
+                    }
+                }
+
+                if ( pVBInfo->VBType & VB_XGI301C)
+                {
+                    XGINew_SetRegOR( pVBInfo->Part4Port , 0x0d , 0x04 ) ;
+                }
+
+                if ( XGINew_SenseHiTV( HwDeviceExtension , pVBInfo) )           /* add by kuku for Multi-adapter sense HiTV */
+                {
+                    tempax |= HiTVSense ;
+                    if ( ( pVBInfo->VBType & VB_XGI301C ) )
+                    {
+                        tempax ^= ( HiTVSense | YPbPrSense ) ;
+                    }
+                }
+
+                if ( !( tempax & ( HiTVSense | YPbPrSense ) ) )         /* start */
+                {
+
+                    tempbx = *pVBInfo->pYCSenseData ;
+
+                    if ( !( XGINew_Is301B(  pVBInfo ) ) )
+                    {
+                      tempbx=*pVBInfo->pYCSenseData2;
+                    }
+
+                    tempcx = 0x0604 ;
+                    if ( XGINew_Sense(tempbx , tempcx, pVBInfo ) )
+                    {
+                      if ( XGINew_Sense(tempbx , tempcx, pVBInfo ) )
+                      {
+                          tempax |= SVIDEOSense ;
+                      }
+                    }
+
+                    if ( OutputSelect & BoardTVType )
+                    {
+                      tempbx = *pVBInfo->pVideoSenseData ;
+
+                      if ( !( XGINew_Is301B( pVBInfo ) ) )
+                      {
+                          tempbx = *pVBInfo->pVideoSenseData2 ;
+                      }
+
+                      tempcx = 0x0804 ;
+                      if ( XGINew_Sense(tempbx , tempcx, pVBInfo ) )
+                      {
+                          if ( XGINew_Sense(tempbx , tempcx, pVBInfo ) )
+                          {
+                              tempax |= AVIDEOSense ;
+                          }
+                      }
+                    }
+                    else
+                    {
+                      if ( !( tempax & SVIDEOSense ) )
+                      {
+                        tempbx = *pVBInfo->pVideoSenseData ;
+
+                        if ( !( XGINew_Is301B( pVBInfo ) ) )
+                        {
+                            tempbx=*pVBInfo->pVideoSenseData2;
+                        }
+
+                        tempcx = 0x0804 ;
+                        if ( XGINew_Sense(tempbx , tempcx, pVBInfo ) )
+                        {
+                            if ( XGINew_Sense(tempbx , tempcx, pVBInfo ) )
+                            {
+                                tempax |= AVIDEOSense ;
+                            }
+                        }
+                      }
+                    }
+                }
+            } /* end */
+            if ( !( tempax & Monitor2Sense ) )
+            {
+                if ( XGINew_SenseLCD( HwDeviceExtension, pVBInfo ) )
+                {
+                    tempax |= LCDSense ;
+                }
+            }
+            tempbx = 0 ;
+            tempcx = 0 ;
+            XGINew_Sense(tempbx , tempcx, pVBInfo ) ;
+
+            XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x32 , ~0xDF , tempax ) ;
+            XGINew_SetReg1( pVBInfo->Part2Port , 0x00 , P2reg0 ) ;
+
+            if ( !( P2reg0 & 0x20 ) )
+            {
+                pVBInfo->VBInfo = DisableCRT2Display ;
+                /* XGI_SetCRT2Group301( SenseModeNo , HwDeviceExtension, pVBInfo ) ; */
+            }
+        }
+    }
+    XGI_DisableBridge( HwDeviceExtension, pVBInfo ) ;           /* shampoo 0226 */
+
+}
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SenseLCD */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+USHORT XGINew_SenseLCD( PXGI_HW_DEVICE_INFO HwDeviceExtension ,PVB_DEVICE_INFO pVBInfo)
+{
+    /* USHORT SoftSetting ; */
+    USHORT temp ;
+
+    if ( ( HwDeviceExtension->jChipType >= XG20 ) || ( HwDeviceExtension->jChipType >= XG40 ) )
+        temp = 0 ;
+    else
+        temp=XGINew_GetPanelID(pVBInfo) ;
+
+    if( !temp )
+        temp = XGINew_GetLCDDDCInfo( HwDeviceExtension, pVBInfo ) ;
+
+    return( temp ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_GetLCDDDCInfo */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGINew_GetLCDDDCInfo( PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT temp ;
+
+    /* add lcd sense */
+    if ( HwDeviceExtension->ulCRT2LCDType == LCD_UNKNOWN )
+    {
+        return( 0 ) ;
+    }
+    else
+    {
+        temp = ( USHORT )HwDeviceExtension->ulCRT2LCDType ;
+        switch( HwDeviceExtension->ulCRT2LCDType )
+        {
+            case LCD_INVALID:
+            case LCD_800x600:
+            case LCD_1024x768:
+            case LCD_1280x1024:
+                break ;
+
+            case LCD_640x480:
+            case LCD_1024x600:
+            case LCD_1152x864:
+            case LCD_1280x960:
+            case LCD_1152x768:
+                temp = 0 ;
+                break ;
+
+            case LCD_1400x1050:
+            case LCD_1280x768:
+            case LCD_1600x1200:
+                break ;
+
+            case LCD_1920x1440:
+            case LCD_2048x1536:
+                temp = 0 ;
+                break ;
+
+            default:
+                break ;
+        }
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x36 , 0xF0 , temp ) ;
+        return( 1 ) ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGINew_GetPanelID(PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT PanelTypeTable[ 16 ] = { SyncNN | PanelRGB18Bit | Panel800x600  | _PanelType00 ,
+                                    SyncNN | PanelRGB18Bit | Panel1024x768 | _PanelType01 ,
+                                    SyncNN | PanelRGB18Bit | Panel800x600  | _PanelType02 ,
+                                    SyncNN | PanelRGB18Bit | Panel640x480  | _PanelType03 ,
+                                    SyncNN | PanelRGB18Bit | Panel1024x768 | _PanelType04 ,
+                                    SyncNN | PanelRGB18Bit | Panel1024x768 | _PanelType05 ,
+                                    SyncNN | PanelRGB18Bit | Panel1024x768 | _PanelType06 ,
+                                    SyncNN | PanelRGB24Bit | Panel1024x768 | _PanelType07 ,
+                                    SyncNN | PanelRGB18Bit | Panel800x600  | _PanelType08 ,
+                                    SyncNN | PanelRGB18Bit | Panel1024x768 | _PanelType09 ,
+                                    SyncNN | PanelRGB18Bit | Panel800x600  | _PanelType0A ,
+                                    SyncNN | PanelRGB18Bit | Panel1024x768 | _PanelType0B ,
+                                    SyncNN | PanelRGB18Bit | Panel1024x768 | _PanelType0C ,
+                                    SyncNN | PanelRGB24Bit | Panel1024x768 | _PanelType0D ,
+                                    SyncNN | PanelRGB18Bit | Panel1024x768 | _PanelType0E ,
+                                    SyncNN | PanelRGB18Bit | Panel1024x768 | _PanelType0F } ;
+    USHORT tempax , tempbx , temp ;
+    /* USHORT return_flag ; */
+
+    tempax = XGINew_GetReg1( pVBInfo->P3c4 , 0x1A ) ;
+    tempbx = tempax & 0x1E ;
+
+    if ( tempax == 0 )
+        return( 0 ) ;
+    else
+    {
+/*
+        if ( !( tempax & 0x10 ) )
+        {
+            if ( pVBInfo->IF_DEF_LVDS == 1 )
+            {
+                tempbx = 0 ;
+                temp = XGINew_GetReg1( pVBInfo->P3c4 , 0x38 ) ;
+                if ( temp & 0x40 )
+                    tempbx |= 0x08 ;
+                if ( temp & 0x20 )
+                    tempbx |= 0x02 ;
+                if ( temp & 0x01 )
+                    tempbx |= 0x01 ;
+
+                temp = XGINew_GetReg1( pVBInfo->P3c4 , 0x39 ) ;
+                if ( temp & 0x80 )
+                    tempbx |= 0x04 ;
+            }
+            else
+            {
+                return( 0 ) ;
+            }
+        }
+*/
+
+        tempbx = tempbx >> 1 ;
+        temp = tempbx & 0x00F ;
+        XGINew_SetReg1( pVBInfo->P3d4 , 0x36 , temp ) ;
+        tempbx-- ;
+        tempbx = PanelTypeTable[ tempbx ] ;
+
+        temp = ( tempbx & 0xFF00 ) >> 8 ;
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x37 , ~( LCDSyncBit | LCDRGB18Bit ) , temp ) ;
+        return( 1 ) ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_BridgeIsEnable */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGINew_BridgeIsEnable( PXGI_HW_DEVICE_INFO HwDeviceExtension ,PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT flag ;
+
+    if ( XGI_BridgeIsOn( pVBInfo ) == 0 )
+    {
+        flag = XGINew_GetReg1( pVBInfo->Part1Port , 0x0 ) ;
+
+        if ( flag & 0x050 )
+        {
+            return( 1 ) ;
+        }
+        else
+        {
+            return( 0 ) ;
+        }
+
+    }
+    return( 0 ) ;
+}
+
+/* ------------------------------------------------------ */
+/* Function : XGINew_SenseHiTV */
+/* Input : */
+/* Output : */
+/* Description : */
+/* ------------------------------------------------------ */
+BOOLEAN XGINew_SenseHiTV( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempbx , tempcx , temp , i , tempch;
+
+    tempbx = *pVBInfo->pYCSenseData2 ;
+
+    tempcx = 0x0604 ;
+
+    temp = tempbx & 0xFF ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x11 , temp ) ;
+    temp = ( tempbx & 0xFF00 ) >> 8 ;
+    temp |= ( tempcx & 0x00FF ) ;
+    XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x10 , ~0x1F , temp ) ;
+
+    for( i = 0 ; i < 10 ; i++ )
+        XGI_LongWait(pVBInfo) ;
+
+    tempch = ( tempcx & 0xFF00 ) >> 8;
+    temp = XGINew_GetReg1( pVBInfo->Part4Port , 0x03 ) ;
+    temp = temp ^ ( 0x0E ) ;
+    temp &= tempch ;
+
+    if ( temp !=  tempch )
+        return( 0 ) ;
+
+    tempbx = *pVBInfo->pVideoSenseData2 ;
+
+    tempcx = 0x0804 ;
+    temp = tempbx & 0xFF ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x11 , temp ) ;
+    temp = ( tempbx & 0xFF00 ) >> 8 ;
+    temp |= ( tempcx & 0x00FF ) ;
+    XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x10 , ~0x1F , temp ) ;
+
+    for( i = 0 ; i < 10 ; i++ )
+        XGI_LongWait(pVBInfo) ;
+
+    tempch = ( tempcx & 0xFF00 ) >> 8;
+    temp = XGINew_GetReg1( pVBInfo->Part4Port , 0x03 ) ;
+    temp = temp ^ ( 0x0E ) ;
+    temp &= tempch ;
+
+    if ( temp !=  tempch )
+        return( 0 ) ;
+    else
+    {
+      tempbx = 0x3FF ;
+      tempcx = 0x0804 ;
+      temp = tempbx & 0xFF ;
+      XGINew_SetReg1( pVBInfo->Part4Port , 0x11 , temp ) ;
+      temp = ( tempbx & 0xFF00 ) >> 8 ;
+      temp |= ( tempcx & 0x00FF ) ;
+      XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x10 , ~0x1F , temp ) ;
+
+      for( i = 0 ; i < 10 ; i++ )
+          XGI_LongWait(pVBInfo) ;
+
+      tempch = ( tempcx & 0xFF00 ) >> 8;
+      temp = XGINew_GetReg1( pVBInfo->Part4Port , 0x03 ) ;
+      temp = temp ^ ( 0x0E ) ;
+      temp &= tempch ;
+
+      if ( temp != tempch )
+          return( 1 ) ;
+      else
+          return( 0 ) ;
+    }
+}
+
+
+
+/*
+;-----------------------------------------------------------------------------
+;       Description: Get Panel support
+;       O/P        :
+;            BL: Panel ID=81h for no scaler LVDS
+;                    BH: Panel enhanced Mode Count
+;                    CX: Panel H. resolution
+;                    DX: PAnel V. resolution
+;-----------------------------------------------------------------------------
+*/
+void XGI_XG21Fun14Sub70( PVB_DEVICE_INFO pVBInfo , PX86_REGS pBiosArguments )
+{
+
+    USHORT ModeIdIndex;
+    USHORT ModeNo;
+
+    USHORT EModeCount;
+    USHORT lvdstableindex;
+
+    lvdstableindex = XGI_GetLVDSOEMTableIndex( pVBInfo );
+    pBiosArguments->h.bl = 0x81;
+    pBiosArguments->x.cx = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHDE;
+    pBiosArguments->x.dx = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVDE;
+    EModeCount = 0;
+
+    pBiosArguments->x.ax = 0x0014;
+    for( ModeIdIndex = 0 ; ;  ModeIdIndex ++ )
+    {
+        ModeNo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeID;
+        if ( pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeID == 0xFF )
+        {
+            pBiosArguments->h.bh = (UCHAR) EModeCount;
+            return;
+        }
+        if ( !XGI_XG21CheckLVDSMode( ModeNo , ModeIdIndex, pVBInfo) )
+        {
+          continue;
+        }
+        EModeCount++ ;
+    }
+}
+/*(
+;-----------------------------------------------------------------------------
+;
+;       Description: Get Panel mode ID for enhanced mode
+;       I/P        : BH: EModeIndex ( which < Panel enhanced Mode Count )
+;       O/P        :
+;            BL: Mode ID
+;                    CX: H. resolution of the assigned by the index
+;                    DX: V. resolution of the assigned by the index
+;
+;-----------------------------------------------------------------------------
+*/
+void XGI_XG21Fun14Sub71( PVB_DEVICE_INFO pVBInfo , PX86_REGS pBiosArguments )
+{
+
+    USHORT EModeCount;
+    USHORT ModeIdIndex,resindex;
+    USHORT ModeNo;
+    USHORT EModeIndex = pBiosArguments->h.bh;
+
+    EModeCount = 0;
+    for( ModeIdIndex = 0 ; ;  ModeIdIndex ++ )
+    {
+        ModeNo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeID;
+        if ( pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeID == 0xFF )
+        {
+            pBiosArguments->x.ax = 0x0114;
+            return;
+        }
+        if ( !XGI_XG21CheckLVDSMode( ModeNo , ModeIdIndex, pVBInfo) )
+        {
+          continue;
+        }
+        if (EModeCount == EModeIndex)
+        {
+            resindex = XGI_GetResInfo( ModeNo , ModeIdIndex, pVBInfo ) ;
+            pBiosArguments->h.bl = (UCHAR) ModeNo;
+            pBiosArguments->x.cx = pVBInfo->ModeResInfo[ resindex ].HTotal ;                      /* xres->ax */
+            pBiosArguments->x.dx = pVBInfo->ModeResInfo[ resindex ].VTotal ;                      /* yres->bx */
+            pBiosArguments->x.ax = 0x0014;
+        }
+        EModeCount++ ;
+
+    }
+
+}
+/*
+;-----------------------------------------------------------------------------
+;
+;       Description: Validate Panel modes ID support
+;       I/P        :
+;            BL: ModeID
+;       O/P        :
+;                    CX: H. resolution of the assigned by the index
+;                    DX: V. resolution of the assigned by the index
+;
+;-----------------------------------------------------------------------------
+*/
+void XGI_XG21Fun14Sub72( PVB_DEVICE_INFO pVBInfo , PX86_REGS pBiosArguments )
+{
+    USHORT ModeIdIndex,resindex;
+    USHORT ModeNo;
+
+
+    ModeNo = pBiosArguments->h.bl ;
+    XGI_SearchModeID( ModeNo, &ModeIdIndex, pVBInfo);
+    if ( !XGI_XG21CheckLVDSMode( ModeNo , ModeIdIndex, pVBInfo) )
+    {
+        pBiosArguments->x.cx = 0;
+        pBiosArguments->x.dx = 0;
+        pBiosArguments->x.ax = 0x0114;
+        return;
+    }
+    resindex = XGI_GetResInfo( ModeNo , ModeIdIndex, pVBInfo ) ;
+    if ( ModeNo <= 0x13 )
+    {
+        pBiosArguments->x.cx = pVBInfo->StResInfo[ resindex ].HTotal ;
+        pBiosArguments->x.dx = pVBInfo->StResInfo[ resindex ].VTotal ;
+    }
+    else
+    {
+        pBiosArguments->x.cx = pVBInfo->ModeResInfo[ resindex ].HTotal ;                          /* xres->ax */
+        pBiosArguments->x.dx = pVBInfo->ModeResInfo[ resindex ].VTotal ;                          /* yres->bx */
+    }
+
+    pBiosArguments->x.ax = 0x0014;
+
+}
+
+/*
+;-----------------------------------------------------------------------------
+;       Description: Get Customized Panel misc. information support
+;       I/P        : Select
+;                    to get panel horizontal timing
+;                        to get panel vertical timing
+;                        to get channel clock parameter
+;            to get panel misc information
+;
+;       O/P        :
+;                    BL: for input Select = 0 ;
+;                       BX: *Value1 = Horizontal total
+;                       CX: *Value2 = Horizontal front porch
+;                       DX: *Value2 = Horizontal sync width
+;                    BL: for input Select = 1 ;
+;                       BX: *Value1 = Vertical total
+;                       CX: *Value2 = Vertical front porch
+;                       DX: *Value2 = Vertical sync width
+;            BL: for input Select = 2 ;
+;                       BX: Value1 = The first CLK parameter
+;                       CX: Value2 = The second CLK parameter
+;                    BL: for input Select = 4 ;
+;                       BX[15]: *Value1 D[15] VESA V. Polarity
+;                       BX[14]: *Value1 D[14] VESA H. Polarity
+;                       BX[7]: *Value1 D[7] Panel V. Polarity
+;                       BX[6]: *Value1 D[6] Panel H. Polarity
+;-----------------------------------------------------------------------------
+*/
+void XGI_XG21Fun14Sub73( PVB_DEVICE_INFO pVBInfo , PX86_REGS pBiosArguments )
+{
+    UCHAR Select;
+
+    USHORT lvdstableindex;
+
+    lvdstableindex = XGI_GetLVDSOEMTableIndex( pVBInfo );
+    Select = pBiosArguments->h.bl;
+
+    switch (Select)
+    {
+        case 0:
+                pBiosArguments->x.bx = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHT;
+                pBiosArguments->x.cx = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHFP;
+                pBiosArguments->x.dx = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHSYNC;
+                break;
+        case 1:
+                pBiosArguments->x.bx = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVT;
+                pBiosArguments->x.cx = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVFP;
+                pBiosArguments->x.dx = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVSYNC;
+                break;
+        case 2:
+                pBiosArguments->x.bx = pVBInfo->XG21_LVDSCapList[lvdstableindex].VCLKData1;
+                pBiosArguments->x.cx = pVBInfo->XG21_LVDSCapList[lvdstableindex].VCLKData2;
+                break;
+        case 4:
+                pBiosArguments->x.bx = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDS_Capability;
+                break;
+    }
+
+    pBiosArguments->x.ax = 0x0014;
+}
+
+
+void XGI_XG21Fun14( PXGI_HW_DEVICE_INFO pXGIHWDE, PX86_REGS pBiosArguments)
+{
+    VB_DEVICE_INFO VBINF;
+    PVB_DEVICE_INFO pVBInfo = &VBINF;
+
+    pVBInfo->IF_DEF_LVDS = 0 ;
+    pVBInfo->IF_DEF_CH7005 = 0 ;
+    pVBInfo->IF_DEF_HiVision = 1 ;
+    pVBInfo->IF_DEF_LCDA = 1 ;
+    pVBInfo->IF_DEF_CH7017 = 0 ;
+    pVBInfo->IF_DEF_YPbPr = 1 ;
+    pVBInfo->IF_DEF_CRT2Monitor = 0 ;
+    pVBInfo->IF_DEF_VideoCapture = 0 ;
+    pVBInfo->IF_DEF_ScaleLCD = 0 ;
+    pVBInfo->IF_DEF_OEMUtil = 0 ;
+    pVBInfo->IF_DEF_PWD = 0 ;
+
+    InitTo330Pointer( pXGIHWDE->jChipType,  pVBInfo ) ;
+    ReadVBIOSTablData( pXGIHWDE->jChipType , pVBInfo) ;
+
+    pVBInfo->P3c4 = pVBInfo->BaseAddr + 0x14 ;
+    pVBInfo->P3d4 = pVBInfo->BaseAddr + 0x24 ;
+    pVBInfo->P3c0 = pVBInfo->BaseAddr + 0x10 ;
+    pVBInfo->P3ce = pVBInfo->BaseAddr + 0x1e ;
+    pVBInfo->P3c2 = pVBInfo->BaseAddr + 0x12 ;
+    pVBInfo->P3ca = pVBInfo->BaseAddr + 0x1a ;
+    pVBInfo->P3c6 = pVBInfo->BaseAddr + 0x16 ;
+    pVBInfo->P3c7 = pVBInfo->BaseAddr + 0x17 ;
+    pVBInfo->P3c8 = pVBInfo->BaseAddr + 0x18 ;
+    pVBInfo->P3c9 = pVBInfo->BaseAddr + 0x19 ;
+    pVBInfo->P3da = pVBInfo->BaseAddr + 0x2A ;
+    pVBInfo->Part0Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_00 ;
+    pVBInfo->Part1Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_04 ;
+    pVBInfo->Part2Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_10 ;
+    pVBInfo->Part3Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_12 ;
+    pVBInfo->Part4Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 ;
+    pVBInfo->Part5Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 + 2 ;
+
+    switch(pBiosArguments->x.ax)
+    {
+    case 0x1470:
+         XGI_XG21Fun14Sub70( pVBInfo , pBiosArguments ) ;
+         break;
+    case 0x1471:
+         XGI_XG21Fun14Sub71( pVBInfo , pBiosArguments ) ;
+         break;
+    case 0x1472:
+         XGI_XG21Fun14Sub72( pVBInfo , pBiosArguments ) ;
+         break;
+    case 0x1473:
+         XGI_XG21Fun14Sub73( pVBInfo , pBiosArguments ) ;
+         break;
+    }
+}
diff --git a/drivers/staging/xgifb/vb_ext.h b/drivers/staging/xgifb/vb_ext.h
new file mode 100644
index 000000000000..9a72f5ecb713
--- /dev/null
+++ b/drivers/staging/xgifb/vb_ext.h
@@ -0,0 +1,32 @@
+#ifndef  _VBEXT_
+#define  _VBEXT_
+
+struct DWORDREGS {
+    ULONG    Eax, Ebx, Ecx, Edx, Esi, Edi, Ebp;
+};
+
+struct WORDREGS {
+    USHORT    ax, hi_ax, bx, hi_bx, cx, hi_cx, dx, hi_dx, si, hi_si, di ,hi_di, bp, hi_bp;
+};
+
+struct BYTEREGS {
+    UCHAR   al, ah, hi_al, hi_ah, bl, bh, hi_bl, hi_bh, cl, ch, hi_cl, hi_ch, dl, dh, hi_dl, hi_dh;
+};
+
+typedef union   _X86_REGS    {
+    struct  DWORDREGS e;
+    struct  WORDREGS x;
+    struct  BYTEREGS h;
+} X86_REGS, *PX86_REGS;
+
+extern   void     XGI_XG21Fun14( PXGI_HW_DEVICE_INFO pXGIHWDE, PX86_REGS pBiosArguments);
+extern   void     XGISetDPMS( PXGI_HW_DEVICE_INFO pXGIHWDE , ULONG VESA_POWER_STATE ) ;
+extern   void     XGI_GetSenseStatus( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo );
+extern   void     XGINew_SetModeScratch ( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo ) ;
+extern   void     ReadVBIOSTablData( UCHAR ChipType , PVB_DEVICE_INFO pVBInfo);
+extern   USHORT   XGINew_SenseLCD(PXGI_HW_DEVICE_INFO,PVB_DEVICE_INFO pVBInfo);
+#ifdef WIN2000
+extern   BOOLEAN  XGI_DySense( PHW_DEVICE_EXTENSION pHWDE , PUCHAR ujConnectStatus );
+#endif /* WIN2000 */
+
+#endif
diff --git a/drivers/staging/xgifb/vb_init.c b/drivers/staging/xgifb/vb_init.c
new file mode 100644
index 000000000000..b85ca9ba8076
--- /dev/null
+++ b/drivers/staging/xgifb/vb_init.c
@@ -0,0 +1,3444 @@
+#include "osdef.h"
+#include "vgatypes.h"
+
+
+#ifdef LINUX_KERNEL
+#include <linux/version.h>
+#include <linux/types.h>
+#include <linux/delay.h> /* udelay */
+#include "XGIfb.h"
+/*#if LINUX_VERSxION_CODE >= KERNEL_VERSION(2,5,0)
+#include <video/XGIfb.h>
+#else
+#include <linux/XGIfb.h>
+#endif */
+#endif
+
+#ifdef WIN2000
+#include <dderror.h>
+#include <devioctl.h>
+#include <miniport.h>
+#include <ntddvdeo.h>
+#include <video.h>
+#include "xgiv.h"
+#include "dd_i2c.h"
+#include "tools.h"
+#endif
+
+#include "vb_def.h"
+#include "vb_struct.h"
+#include "vb_util.h"
+#include "vb_setmode.h"
+#include "vb_init.h"
+#include "vb_ext.h"
+
+#ifdef LINUX_XF86
+#include "xf86.h"
+#include "xf86PciInfo.h"
+#include "xgi.h"
+#include "xgi_regs.h"
+#endif
+
+#ifdef LINUX_KERNEL
+#include <asm/io.h>
+#include <linux/types.h>
+#endif
+
+
+
+
+UCHAR    XGINew_ChannelAB,XGINew_DataBusWidth;
+
+USHORT XGINew_DRAMType[17][5]={{0x0C,0x0A,0x02,0x40,0x39},{0x0D,0x0A,0x01,0x40,0x48},
+                     {0x0C,0x09,0x02,0x20,0x35},{0x0D,0x09,0x01,0x20,0x44},
+                     {0x0C,0x08,0x02,0x10,0x31},{0x0D,0x08,0x01,0x10,0x40},
+                     {0x0C,0x0A,0x01,0x20,0x34},{0x0C,0x09,0x01,0x08,0x32},
+                     {0x0B,0x08,0x02,0x08,0x21},{0x0C,0x08,0x01,0x08,0x30},
+                     {0x0A,0x08,0x02,0x04,0x11},{0x0B,0x0A,0x01,0x10,0x28},
+                     {0x09,0x08,0x02,0x02,0x01},{0x0B,0x09,0x01,0x08,0x24},
+                     {0x0B,0x08,0x01,0x04,0x20},{0x0A,0x08,0x01,0x02,0x10},
+                     {0x09,0x08,0x01,0x01,0x00}};
+
+USHORT XGINew_SDRDRAM_TYPE[13][5]=
+{
+{ 2,12, 9,64,0x35},
+{ 1,13, 9,64,0x44},
+{ 2,12, 8,32,0x31},
+{ 2,11, 9,32,0x25},
+{ 1,12, 9,32,0x34},
+{ 1,13, 8,32,0x40},
+{ 2,11, 8,16,0x21},
+{ 1,12, 8,16,0x30},
+{ 1,11, 9,16,0x24},
+{ 1,11, 8, 8,0x20},
+{ 2, 9, 8, 4,0x01},
+{ 1,10, 8, 4,0x10},
+{ 1, 9, 8, 2,0x00}
+};
+
+USHORT XGINew_DDRDRAM_TYPE[4][5]=
+{
+{ 2,12, 9,64,0x35},
+{ 2,12, 8,32,0x31},
+{ 2,11, 8,16,0x21},
+{ 2, 9, 8, 4,0x01}
+};
+USHORT XGINew_DDRDRAM_TYPE340[4][5]=
+{
+{ 2,13, 9,64,0x45},
+{ 2,12, 9,32,0x35},
+{ 2,12, 8,16,0x31},
+{ 2,11, 8, 8,0x21}
+};
+USHORT XGINew_DDRDRAM_TYPE20[12][5]=
+{
+{ 2,14,11,128,0x5D},
+{ 2,14,10,64,0x59},
+{ 2,13,11,64,0x4D},
+{ 2,14, 9,32,0x55},
+{ 2,13,10,32,0x49},
+{ 2,12,11,32,0x3D},
+{ 2,14, 8,16,0x51},
+{ 2,13, 9,16,0x45},
+{ 2,12,10,16,0x39},
+{ 2,13, 8, 8,0x41},
+{ 2,12, 9, 8,0x35},
+{ 2,12, 8, 4,0x31}
+};
+
+void     XGINew_SetDRAMSize_340(PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO);
+void     XGINew_SetDRAMSize_310(PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO);
+void     XGINew_SetMemoryClock(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO );
+void     XGINew_SetDRAMModeRegister(PVB_DEVICE_INFO );
+void     XGINew_SetDRAMModeRegister340( PXGI_HW_DEVICE_INFO HwDeviceExtension );
+void     XGINew_SetDRAMDefaultRegister340(PXGI_HW_DEVICE_INFO HwDeviceExtension, ULONG, PVB_DEVICE_INFO );
+UCHAR    XGINew_GetXG20DRAMType( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo);
+BOOLEAN  XGIInitNew( PXGI_HW_DEVICE_INFO HwDeviceExtension) ;
+
+int      XGINew_DDRSizing340( PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO );
+void     XGINew_DisableRefresh( PXGI_HW_DEVICE_INFO ,PVB_DEVICE_INFO) ;
+void     XGINew_CheckBusWidth_310( PVB_DEVICE_INFO) ;
+int      XGINew_SDRSizing(PVB_DEVICE_INFO);
+int      XGINew_DDRSizing( PVB_DEVICE_INFO );
+void     XGINew_EnableRefresh( PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO);
+int      XGINew_RAMType;                  /*int      ModeIDOffset,StandTable,CRT1Table,ScreenOffset,REFIndex;*/
+ULONG    UNIROM;                          /* UNIROM */
+BOOLEAN  ChkLFB( PVB_DEVICE_INFO );
+void     XGINew_Delay15us(ULONG);
+void     SetPowerConsume (PXGI_HW_DEVICE_INFO HwDeviceExtension,ULONG XGI_P3d4Port);
+void     ReadVBIOSTablData( UCHAR ChipType , PVB_DEVICE_INFO pVBInfo);
+void     XGINew_DDR1x_MRS_XG20( ULONG P3c4 , PVB_DEVICE_INFO pVBInfo);
+void     XGINew_SetDRAMModeRegister_XG20( PXGI_HW_DEVICE_INFO HwDeviceExtension );
+void     XGINew_SetDRAMModeRegister_XG27( PXGI_HW_DEVICE_INFO HwDeviceExtension );
+void     XGINew_ChkSenseStatus ( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo ) ;
+void     XGINew_SetModeScratch ( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo ) ;
+void     XGINew_GetXG21Sense(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo) ;
+UCHAR    GetXG21FPBits(PVB_DEVICE_INFO pVBInfo);
+void     XGINew_GetXG27Sense(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo) ;
+UCHAR    GetXG27FPBits(PVB_DEVICE_INFO pVBInfo);
+
+#ifdef WIN2000
+/* [Billy] 2007/05/20 For CH7007 */
+extern  UCHAR CH7007TVReg_UNTSC[][8],CH7007TVReg_ONTSC[][8],CH7007TVReg_UPAL[][8],CH7007TVReg_OPAL[][8];
+extern  UCHAR XGI7007_CHTVVCLKUNTSC[],XGI7007_CHTVVCLKONTSC[],XGI7007_CHTVVCLKUPAL[],XGI7007_CHTVVCLKOPAL[];
+#endif
+
+#ifdef LINUX_KERNEL
+void DelayUS(ULONG MicroSeconds)
+{
+        udelay(MicroSeconds);
+}
+#endif
+
+/* --------------------------------------------------------------------- */
+/* Function : XGIInitNew */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGIInitNew( PXGI_HW_DEVICE_INFO HwDeviceExtension )
+{
+
+    VB_DEVICE_INFO VBINF;
+    PVB_DEVICE_INFO pVBInfo = &VBINF;
+    UCHAR   i , temp = 0 , temp1 ;
+     //       VBIOSVersion[ 5 ] ;
+    PUCHAR  volatile pVideoMemory;
+
+    /* ULONG j, k ; */
+
+    PXGI_DSReg pSR ;
+
+    ULONG Temp ;
+
+    pVBInfo->ROMAddr = HwDeviceExtension->pjVirtualRomBase ;
+
+    pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress ;
+
+    pVBInfo->BaseAddr = (ULONG)HwDeviceExtension->pjIOAddress ;
+
+    pVideoMemory = ( PUCHAR )pVBInfo->ROMAddr;
+
+
+//    Newdebugcode( 0x99 ) ;
+
+
+   /* if ( pVBInfo->ROMAddr == 0 ) */
+   /* return( FALSE ) ; */
+
+    if ( pVBInfo->FBAddr == 0 )
+{
+       printk("\n pVBInfo->FBAddr == 0 ");
+       return( FALSE ) ;
+}
+printk("1");
+    if ( pVBInfo->BaseAddr == 0 )
+{
+       printk("\npVBInfo->BaseAddr == 0 ");
+        return( FALSE ) ;
+}
+printk("2");
+
+    XGINew_SetReg3( ( pVBInfo->BaseAddr + 0x12 ) , 0x67 ) ;     /* 3c2 <- 67 ,ynlai */
+
+    pVBInfo->ISXPDOS = 0 ;
+printk("3");
+
+if ( !HwDeviceExtension->bIntegratedMMEnabled )
+{
+        return( FALSE ) ;       /* alan */
+}
+printk("4");
+
+//    XGI_MemoryCopy( VBIOSVersion , HwDeviceExtension->szVBIOSVer , 4 ) ;
+
+ //   VBIOSVersion[ 4 ] = 0x0 ;
+
+    /* 09/07/99 modify by domao */
+
+    pVBInfo->P3c4 = pVBInfo->BaseAddr + 0x14 ;
+    pVBInfo->P3d4 = pVBInfo->BaseAddr + 0x24 ;
+    pVBInfo->P3c0 = pVBInfo->BaseAddr + 0x10 ;
+    pVBInfo->P3ce = pVBInfo->BaseAddr + 0x1e ;
+    pVBInfo->P3c2 = pVBInfo->BaseAddr + 0x12 ;
+    pVBInfo->P3ca = pVBInfo->BaseAddr + 0x1a ;
+    pVBInfo->P3c6 = pVBInfo->BaseAddr + 0x16 ;
+    pVBInfo->P3c7 = pVBInfo->BaseAddr + 0x17 ;
+    pVBInfo->P3c8 = pVBInfo->BaseAddr + 0x18 ;
+    pVBInfo->P3c9 = pVBInfo->BaseAddr + 0x19 ;
+    pVBInfo->P3da = pVBInfo->BaseAddr + 0x2A ;
+    pVBInfo->Part0Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_00 ;
+    pVBInfo->Part1Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_04 ;
+    pVBInfo->Part2Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_10 ;
+    pVBInfo->Part3Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_12 ;
+    pVBInfo->Part4Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 ;
+    pVBInfo->Part5Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 + 2 ;
+printk("5");
+
+    if ( HwDeviceExtension->jChipType < XG20 )                  /* kuku 2004/06/25 */
+    XGI_GetVBType( pVBInfo ) ;         /* Run XGI_GetVBType before InitTo330Pointer */
+
+    InitTo330Pointer( HwDeviceExtension->jChipType,  pVBInfo ) ;
+
+    /* ReadVBIOSData */
+    ReadVBIOSTablData( HwDeviceExtension->jChipType , pVBInfo) ;
+
+    /* 1.Openkey */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x05 , 0x86 ) ;
+printk("6");
+
+    /* GetXG21Sense (GPIO) */
+    if ( HwDeviceExtension->jChipType == XG21 )
+    {
+        XGINew_GetXG21Sense(HwDeviceExtension, pVBInfo) ;
+    }
+    if ( HwDeviceExtension->jChipType == XG27 )
+    {
+        XGINew_GetXG27Sense(HwDeviceExtension, pVBInfo) ;
+    }
+printk("7");
+
+    /* 2.Reset Extended register */
+
+    for( i = 0x06 ; i < 0x20 ; i++ )
+        XGINew_SetReg1( pVBInfo->P3c4 , i , 0 ) ;
+
+    for( i = 0x21 ; i <= 0x27 ; i++ )
+        XGINew_SetReg1( pVBInfo->P3c4 , i , 0 ) ;
+
+    /* for( i = 0x06 ; i <= 0x27 ; i++ ) */
+    /* XGINew_SetReg1( pVBInfo->P3c4 , i , 0 ) ; */
+
+printk("8");
+
+    if(( HwDeviceExtension->jChipType >= XG20 ) || ( HwDeviceExtension->jChipType >= XG40))
+    {
+        for( i = 0x31 ; i <= 0x3B ; i++ )
+            XGINew_SetReg1( pVBInfo->P3c4 , i , 0 ) ;
+    }
+    else
+    {
+        for( i = 0x31 ; i <= 0x3D ; i++ )
+            XGINew_SetReg1( pVBInfo->P3c4 , i , 0 ) ;
+    }
+printk("9");
+
+    if ( HwDeviceExtension->jChipType == XG42 )                 /* [Hsuan] 2004/08/20 Auto over driver for XG42 */
+      XGINew_SetReg1( pVBInfo->P3c4 , 0x3B , 0xC0 ) ;
+
+    /* for( i = 0x30 ; i <= 0x3F ; i++ ) */
+    /* XGINew_SetReg1( pVBInfo->P3d4 , i , 0 ) ; */
+
+    for( i = 0x79 ; i <= 0x7C ; i++ )
+        XGINew_SetReg1( pVBInfo->P3d4 , i , 0 ) ;               /* shampoo 0208 */
+
+printk("10");
+
+    if ( HwDeviceExtension->jChipType >= XG20 )
+        XGINew_SetReg1( pVBInfo->P3d4 , 0x97 , *pVBInfo->pXGINew_CR97 ) ;
+
+    /* 3.SetMemoryClock
+
+    if ( HwDeviceExtension->jChipType >= XG40 )
+        XGINew_RAMType = ( int )XGINew_GetXG20DRAMType( HwDeviceExtension , pVBInfo) ;
+
+    if ( HwDeviceExtension->jChipType < XG40 )
+        XGINew_SetMemoryClock( HwDeviceExtension , pVBInfo ) ;  */
+
+printk("11");
+
+    /* 4.SetDefExt1Regs begin */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x07 , *pVBInfo->pSR07 ) ;
+    if ( HwDeviceExtension->jChipType == XG27 )
+    {
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x40 , *pVBInfo->pSR40 ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x41 , *pVBInfo->pSR41 ) ;
+    }
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x11 , 0x0F ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x1F , *pVBInfo->pSR1F ) ;
+    /* XGINew_SetReg1( pVBInfo->P3c4 , 0x20 , 0x20 ) ; */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x20 , 0xA0 ) ;     /* alan, 2001/6/26 Frame buffer can read/write SR20 */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x36 , 0x70 ) ;     /* Hsuan, 2006/01/01 H/W request for slow corner chip */
+    if ( HwDeviceExtension->jChipType == XG27 )         /* Alan 12/07/2006 */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x36 , *pVBInfo->pSR36 ) ;
+
+    /* SR11 = 0x0F ; */
+    /* XGINew_SetReg1( pVBInfo->P3c4 , 0x11 , SR11 ) ; */
+
+printk("12");
+
+   if ( HwDeviceExtension->jChipType < XG20 )           /* kuku 2004/06/25 */
+    {
+//    /* Set AGP Rate */
+//    temp1 = XGINew_GetReg1( pVBInfo->P3c4 , 0x3B ) ;
+//    temp1 &= 0x02 ;
+//    if ( temp1 == 0x02 )
+//    {
+//        XGINew_SetReg4( 0xcf8 , 0x80000000 ) ;
+//       ChipsetID = XGINew_GetReg3( 0x0cfc ) ;
+//        XGINew_SetReg4( 0xcf8 , 0x8000002C ) ;
+//        VendorID = XGINew_GetReg3( 0x0cfc ) ;
+//        VendorID &= 0x0000FFFF ;
+//        XGINew_SetReg4( 0xcf8 , 0x8001002C ) ;
+//        GraphicVendorID = XGINew_GetReg3( 0x0cfc ) ;
+//        GraphicVendorID &= 0x0000FFFF;
+//
+//        if ( ChipsetID == 0x7301039 )
+///            XGINew_SetReg1( pVBInfo->P3d4 , 0x5F , 0x09 ) ;
+//
+//        ChipsetID &= 0x0000FFFF ;
+///
+//        if ( ( ChipsetID == 0x700E ) || ( ChipsetID == 0x1022 ) || ( ChipsetID == 0x1106 ) || ( ChipsetID == 0x10DE ) )
+//        {
+//            if ( ChipsetID == 0x1106 )
+//            {
+//                if ( ( VendorID == 0x1019 ) && ( GraphicVendorID == 0x1019 ) )
+//                    XGINew_SetReg1( pVBInfo->P3d4 , 0x5F , 0x0D ) ;
+//                else
+//                    XGINew_SetReg1( pVBInfo->P3d4 , 0x5F , 0x0B ) ;
+//            }
+//            else
+//                XGINew_SetReg1( pVBInfo->P3d4 , 0x5F , 0x0B ) ;
+//        }
+//    }
+
+printk("13");
+
+    if ( HwDeviceExtension->jChipType >= XG40 )
+    {
+        /* Set AGP customize registers (in SetDefAGPRegs) Start */
+        for( i = 0x47 ; i <= 0x4C ; i++ )
+            XGINew_SetReg1( pVBInfo->P3d4 , i , pVBInfo->AGPReg[ i - 0x47 ] ) ;
+
+        for( i = 0x70 ; i <= 0x71 ; i++ )
+            XGINew_SetReg1( pVBInfo->P3d4 , i , pVBInfo->AGPReg[ 6 + i - 0x70 ] ) ;
+
+        for( i = 0x74 ; i <= 0x77 ; i++ )
+            XGINew_SetReg1( pVBInfo->P3d4 , i , pVBInfo->AGPReg[ 8 + i - 0x74 ] ) ;
+        /* Set AGP customize registers (in SetDefAGPRegs) End */
+        /*[Hsuan]2004/12/14 AGP Input Delay Adjustment on 850 */
+//        XGINew_SetReg4( 0xcf8 , 0x80000000 ) ;
+//        ChipsetID = XGINew_GetReg3( 0x0cfc ) ;
+//        if ( ChipsetID == 0x25308086 )
+//            XGINew_SetReg1( pVBInfo->P3d4 , 0x77 , 0xF0 ) ;
+
+        HwDeviceExtension->pQueryVGAConfigSpace( HwDeviceExtension , 0x50 , 0 , &Temp ) ;       /* Get */
+        Temp >>= 20 ;
+        Temp &= 0xF ;
+
+        if ( Temp == 1 )
+            XGINew_SetReg1( pVBInfo->P3d4 , 0x48 , 0x20 ) ;     /* CR48 */
+    }
+printk("14");
+
+    if ( HwDeviceExtension->jChipType < XG40 )
+        XGINew_SetReg1( pVBInfo->P3d4 , 0x49 , pVBInfo->CR49[ 0 ] ) ;
+    }   /* != XG20 */
+
+    /* Set PCI */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x23 , *pVBInfo->pSR23 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x24 , *pVBInfo->pSR24 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x25 , pVBInfo->SR25[ 0 ] ) ;
+printk("15");
+
+    if ( HwDeviceExtension->jChipType < XG20 )          /* kuku 2004/06/25 */
+    {
+    /* Set VB */
+    XGI_UnLockCRT2( HwDeviceExtension, pVBInfo) ;
+    XGINew_SetRegANDOR( pVBInfo->Part0Port , 0x3F , 0xEF , 0x00 ) ;     /* alan, disable VideoCapture */
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x00 , 0x00 ) ;
+    temp1 = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x7B ) ;           /* chk if BCLK>=100MHz */
+    temp = ( UCHAR )( ( temp1 >> 4 ) & 0x0F ) ;
+
+
+        XGINew_SetReg1( pVBInfo->Part1Port , 0x02 , ( *pVBInfo->pCRT2Data_1_2 ) ) ;
+
+printk("16");
+
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x2E , 0x08 ) ;        /* use VB */
+    } /* != XG20 */
+
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x27 , 0x1F ) ;
+
+    if ( ( HwDeviceExtension->jChipType == XG42 ) && XGINew_GetXG20DRAMType( HwDeviceExtension , pVBInfo) != 0 )        /* Not DDR */
+    {
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x31 , ( *pVBInfo->pSR31 & 0x3F ) | 0x40 ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x32 , ( *pVBInfo->pSR32 & 0xFC ) | 0x01 ) ;
+    }
+    else
+    {
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x31 , *pVBInfo->pSR31 ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x32 , *pVBInfo->pSR32 ) ;
+    }
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x33 , *pVBInfo->pSR33 ) ;
+printk("17");
+
+/*
+    if ( HwDeviceExtension->jChipType >= XG40 )
+      SetPowerConsume ( HwDeviceExtension , pVBInfo->P3c4);     */
+
+    if ( HwDeviceExtension->jChipType < XG20 )          /* kuku 2004/06/25 */
+    {
+    if ( XGI_BridgeIsOn( pVBInfo ) == 1 )
+    {
+        if ( pVBInfo->IF_DEF_LVDS == 0 )
+        {
+            XGINew_SetReg1( pVBInfo->Part2Port , 0x00 , 0x1C ) ;
+            XGINew_SetReg1( pVBInfo->Part4Port , 0x0D , *pVBInfo->pCRT2Data_4_D ) ;
+            XGINew_SetReg1( pVBInfo->Part4Port , 0x0E , *pVBInfo->pCRT2Data_4_E ) ;
+            XGINew_SetReg1( pVBInfo->Part4Port , 0x10 , *pVBInfo->pCRT2Data_4_10 ) ;
+            XGINew_SetReg1( pVBInfo->Part4Port , 0x0F , 0x3F ) ;
+        }
+
+        XGI_LockCRT2( HwDeviceExtension, pVBInfo ) ;
+    }
+    }   /* != XG20 */
+printk("18");
+
+    if ( HwDeviceExtension->jChipType < XG40 )
+        XGINew_SetReg1( pVBInfo->P3d4 , 0x83 , 0x00 ) ;
+printk("181");
+
+    if ( HwDeviceExtension->bSkipSense == FALSE )
+    {
+printk("182");
+
+        XGI_SenseCRT1(pVBInfo) ;
+
+printk("183");
+        /* XGINew_DetectMonitor( HwDeviceExtension ) ; */
+pVBInfo->IF_DEF_CH7007 = 0;
+        if ( ( HwDeviceExtension->jChipType == XG21 ) && (pVBInfo->IF_DEF_CH7007) )
+        {
+printk("184");
+           XGI_GetSenseStatus( HwDeviceExtension , pVBInfo ) ;  /* sense CRT2 */
+printk("185");
+
+        }
+        if ( HwDeviceExtension->jChipType == XG21 )
+        {
+printk("186");
+
+          XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x32 , ~Monitor1Sense , Monitor1Sense ) ; /* Z9 default has CRT */
+          temp = GetXG21FPBits( pVBInfo ) ;
+          XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x37 , ~0x01, temp ) ;
+printk("187");
+
+          }
+        if ( HwDeviceExtension->jChipType == XG27 )
+        {
+          XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x32 , ~Monitor1Sense , Monitor1Sense ) ; /* Z9 default has CRT */
+          temp = GetXG27FPBits( pVBInfo ) ;
+          XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x37 , ~0x03, temp ) ;
+        }
+    }
+printk("19");
+
+    if ( HwDeviceExtension->jChipType >= XG40 )
+    {
+        if ( HwDeviceExtension->jChipType >= XG40 )
+        {
+          XGINew_RAMType = ( int )XGINew_GetXG20DRAMType( HwDeviceExtension , pVBInfo ) ;
+         }
+
+        XGINew_SetDRAMDefaultRegister340( HwDeviceExtension ,  pVBInfo->P3d4,  pVBInfo ) ;
+
+        if ( HwDeviceExtension->bSkipDramSizing == TRUE )
+        {
+            pSR = HwDeviceExtension->pSR ;
+            if ( pSR!=NULL )
+            {
+                while( pSR->jIdx != 0xFF )
+                {
+                    XGINew_SetReg1( pVBInfo->P3c4 , pSR->jIdx , pSR->jVal ) ;
+                    pSR++ ;
+                }
+            }
+            /* XGINew_SetDRAMModeRegister340( pVBInfo ) ; */
+        }       /* SkipDramSizing */
+        else
+        {
+#if 0
+           if ( HwDeviceExtension->jChipType == XG20 )
+            {
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , pVBInfo->SR15[0][XGINew_RAMType] ) ;
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , pVBInfo->SR15[1][XGINew_RAMType] ) ;
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x20 , 0x20 ) ;
+            }
+            else
+#endif
+{
+printk("20");
+
+               XGINew_SetDRAMSize_340( HwDeviceExtension , pVBInfo) ;
+}
+printk("21");
+
+        }
+    }           /* XG40 */
+
+printk("22");
+
+
+    /* SetDefExt2Regs begin */
+/*
+    AGP = 1 ;
+    temp =( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x3A ) ;
+    temp &= 0x30 ;
+    if ( temp == 0x30 )
+        AGP = 0 ;
+
+    if ( AGP == 0 )
+        *pVBInfo->pSR21 &= 0xEF ;
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x21 , *pVBInfo->pSR21 ) ;
+    if ( AGP == 1 )
+        *pVBInfo->pSR22 &= 0x20 ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x22 , *pVBInfo->pSR22 ) ;
+*/
+
+//    base = 0x80000000 ;
+//    OutPortLong( 0xcf8 , base ) ;
+//    Temp = ( InPortLong( 0xcfc ) & 0xFFFF ) ;
+//    if ( Temp == 0x1039 )
+//    {
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x22 , ( UCHAR )( ( *pVBInfo->pSR22 ) & 0xFE ) ) ;
+//    }
+//    else
+//    {
+//        XGINew_SetReg1( pVBInfo->P3c4 , 0x22 , *pVBInfo->pSR22 ) ;
+//    }
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x21 , *pVBInfo->pSR21 ) ;
+
+printk("23");
+
+
+    XGINew_ChkSenseStatus ( HwDeviceExtension , pVBInfo ) ;
+    XGINew_SetModeScratch ( HwDeviceExtension , pVBInfo ) ;
+
+printk("24");
+
+
+XGINew_SetReg1( pVBInfo->P3d4 , 0x8c , 0x87);
+XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x31);
+printk("25");
+
+    return( TRUE ) ;
+} /* end of init */
+
+
+
+
+
+/* ============== alan ====================== */
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_GetXG20DRAMType */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+UCHAR XGINew_GetXG20DRAMType( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR data, temp ;
+
+    if ( HwDeviceExtension->jChipType < XG20 )
+    {
+        if ( *pVBInfo->pSoftSetting & SoftDRAMType )
+        {
+            data = *pVBInfo->pSoftSetting & 0x07 ;
+            return( data ) ;
+        }
+        else
+        {
+            data = XGINew_GetReg1( pVBInfo->P3c4 , 0x39 ) & 0x02 ;
+
+            if ( data == 0 )
+                data = ( XGINew_GetReg1( pVBInfo->P3c4 , 0x3A ) & 0x02 ) >> 1 ;
+
+            return( data ) ;
+        }
+    }
+    else if ( HwDeviceExtension->jChipType == XG27 )
+    {
+        if ( *pVBInfo->pSoftSetting & SoftDRAMType )
+        {
+            data = *pVBInfo->pSoftSetting & 0x07 ;
+            return( data ) ;
+        }
+        temp = XGINew_GetReg1( pVBInfo->P3c4 , 0x3B ) ;
+
+        if (( temp & 0x88 )==0x80)              /* SR3B[7][3]MAA15 MAA11 (Power on Trapping) */
+          data = 0 ;                                    /*DDR*/
+        else
+          data = 1 ;                                    /*DDRII*/
+        return( data ) ;
+    }
+    else if ( HwDeviceExtension->jChipType == XG21 )
+    {
+        XGINew_SetRegAND( pVBInfo->P3d4 , 0xB4 , ~0x02 ) ;              /* Independent GPIO control */
+        DelayUS(800);
+        XGINew_SetRegOR( pVBInfo->P3d4 , 0x4A , 0x80 ) ;                /* Enable GPIOH read */
+        temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x48 ) ;                 /* GPIOF 0:DVI 1:DVO */
+// HOTPLUG_SUPPORT
+// for current XG20 & XG21, GPIOH is floating, driver will fix DDR temporarily
+        if ( temp & 0x01 )                                              /* DVI read GPIOH */
+          data = 1 ;                                                    /*DDRII*/
+        else
+          data = 0 ;                                                    /*DDR*/
+//~HOTPLUG_SUPPORT
+        XGINew_SetRegOR( pVBInfo->P3d4 , 0xB4 , 0x02 ) ;
+        return( data ) ;
+    }
+    else
+    {
+        data = XGINew_GetReg1( pVBInfo->P3d4 , 0x97 ) & 0x01 ;
+
+        if ( data == 1 )
+            data ++ ;
+
+        return( data );
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_Get310DRAMType */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+UCHAR XGINew_Get310DRAMType(PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR data ;
+
+  /* index = XGINew_GetReg1( pVBInfo->P3c4 , 0x1A ) ; */
+  /* index &= 07 ; */
+
+    if ( *pVBInfo->pSoftSetting & SoftDRAMType )
+        data = *pVBInfo->pSoftSetting & 0x03 ;
+    else
+        data = XGINew_GetReg1( pVBInfo->P3c4 , 0x3a ) & 0x03 ;
+
+    return( data ) ;
+}
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_Delay15us */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+/*
+void XGINew_Delay15us(ULONG ulMicrsoSec)
+{
+}
+*/
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SDR_MRS */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SDR_MRS(  PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT data ;
+
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x16 ) ;
+    data &= 0x3F ;          /* SR16 D7=0,D6=0 */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x16 , data ) ;   /* enable mode register set(MRS) low */
+    /* XGINew_Delay15us( 0x100 ) ; */
+    data |= 0x80 ;          /* SR16 D7=1,D6=0 */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x16 , data ) ;   /* enable mode register set(MRS) high */
+    /* XGINew_Delay15us( 0x100 ) ; */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DDR1x_MRS_340 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_DDR1x_MRS_340( ULONG P3c4 , PVB_DEVICE_INFO pVBInfo)
+{
+    XGINew_SetReg1( P3c4 , 0x18 , 0x01 ) ;
+    XGINew_SetReg1( P3c4 , 0x19 , 0x20 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x80 ) ;
+
+    if ( *pVBInfo->pXGINew_DRAMTypeDefinition != 0x0C ) /* Samsung F Die */
+    {
+        DelayUS( 3000 ) ;       /* Delay 67 x 3 Delay15us */
+        XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;
+        XGINew_SetReg1( P3c4 , 0x19 , 0x20 ) ;
+        XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;
+        XGINew_SetReg1( P3c4 , 0x16 , 0x80 ) ;
+    }
+
+    DelayUS( 60 ) ;
+    XGINew_SetReg1( P3c4 , 0x18 , pVBInfo->SR15[ 2 ][ XGINew_RAMType ] ) ;      /* SR18 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x01 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , pVBInfo->SR16[ 0 ] ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , pVBInfo->SR16[ 1 ] ) ;
+    DelayUS( 1000 ) ;
+    XGINew_SetReg1( P3c4 , 0x1B , 0x03 ) ;
+    DelayUS( 500 ) ;
+    XGINew_SetReg1( P3c4 , 0x18 , pVBInfo->SR15[ 2 ][ XGINew_RAMType ] ) ;      /* SR18 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x00 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , pVBInfo->SR16[ 2 ] ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , pVBInfo->SR16[ 3 ] ) ;
+    XGINew_SetReg1( P3c4 , 0x1B , 0x00 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DDR2x_MRS_340 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_DDR2x_MRS_340( ULONG P3c4 , PVB_DEVICE_INFO pVBInfo)
+{
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;
+    XGINew_SetReg1( P3c4 , 0x19 , 0x20 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x80 ) ;
+    DelayUS( 60 ) ;
+    XGINew_SetReg1( P3c4 , 0x18 , pVBInfo->SR15[ 2 ][ XGINew_RAMType ] ) ;      /* SR18 */
+    /* XGINew_SetReg1( P3c4 , 0x18 , 0x31 ) ; */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x01 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x05 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x85 ) ;
+    DelayUS( 1000 ) ;
+    XGINew_SetReg1( P3c4 , 0x1B , 0x03 ) ;
+    DelayUS( 500 ) ;
+    /* XGINew_SetReg1( P3c4 , 0x18 , 0x31 ) ; */
+    XGINew_SetReg1( P3c4 , 0x18 , pVBInfo->SR15[ 2 ][ XGINew_RAMType ] ) ;      /* SR18 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x00 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x05 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x85 ) ;
+    XGINew_SetReg1( P3c4 , 0x1B , 0x00 ) ;
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DDRII_Bootup_XG27 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_DDRII_Bootup_XG27( PXGI_HW_DEVICE_INFO HwDeviceExtension ,  ULONG P3c4 , PVB_DEVICE_INFO pVBInfo)
+{
+    ULONG P3d4 = P3c4 + 0x10 ;
+    XGINew_RAMType = ( int )XGINew_GetXG20DRAMType( HwDeviceExtension , pVBInfo ) ;
+    XGINew_SetMemoryClock( HwDeviceExtension , pVBInfo ) ;
+
+   /* Set Double Frequency */
+    /* XGINew_SetReg1( P3d4 , 0x97 , 0x11 ) ; */                /* CR97 */
+    XGINew_SetReg1( P3d4 , 0x97 , *pVBInfo->pXGINew_CR97 ) ;    /* CR97 */
+
+    DelayUS( 200 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;   /* Set SR18 */ //EMRS2
+    XGINew_SetReg1( P3c4 , 0x19 , 0x80 ) ;   /* Set SR19 */
+    XGINew_SetReg1( P3c4 , 0x16 , 0x20 ) ;   /* Set SR16 */
+    DelayUS( 15 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0xA0 ) ;   /* Set SR16 */
+    DelayUS( 15 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;   /* Set SR18 */ //EMRS3
+    XGINew_SetReg1( P3c4 , 0x19 , 0xC0 ) ;   /* Set SR19 */
+    XGINew_SetReg1( P3c4 , 0x16 , 0x20 ) ;   /* Set SR16 */
+    DelayUS( 15 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0xA0 ) ;   /* Set SR16 */
+    DelayUS( 15) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;   /* Set SR18 */ //EMRS1
+    XGINew_SetReg1( P3c4 , 0x19 , 0x40 ) ;   /* Set SR19 */
+    XGINew_SetReg1( P3c4 , 0x16 , 0x20 ) ;   /* Set SR16 */
+    DelayUS( 30 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0xA0 ) ;   /* Set SR16 */
+    DelayUS( 15 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x42 ) ;   /* Set SR18 */ //MRS, DLL Enable
+    XGINew_SetReg1( P3c4 , 0x19 , 0x0A ) ;   /* Set SR19 */
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;   /* Set SR16 */
+    DelayUS( 30 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;   /* Set SR16 */
+    XGINew_SetReg1( P3c4 , 0x16 , 0x80 ) ;   /* Set SR16 */
+    /* DelayUS( 15 ) ; */
+
+    XGINew_SetReg1( P3c4 , 0x1B , 0x04 ) ;   /* Set SR1B */
+    DelayUS( 60 ) ;
+    XGINew_SetReg1( P3c4 , 0x1B , 0x00 ) ;   /* Set SR1B */
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x42 ) ;   /* Set SR18 */ //MRS, DLL Reset
+    XGINew_SetReg1( P3c4 , 0x19 , 0x08 ) ;   /* Set SR19 */
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;   /* Set SR16 */
+
+    DelayUS( 30 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x83 ) ;   /* Set SR16 */
+    DelayUS( 15 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x80 ) ;   /* Set SR18 */ //MRS, ODT
+    XGINew_SetReg1( P3c4 , 0x19 , 0x46 ) ;   /* Set SR19 */
+    XGINew_SetReg1( P3c4 , 0x16 , 0x20 ) ;   /* Set SR16 */
+    DelayUS( 30 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0xA0 ) ;   /* Set SR16 */
+    DelayUS( 15 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;   /* Set SR18 */ //EMRS
+    XGINew_SetReg1( P3c4 , 0x19 , 0x40 ) ;   /* Set SR19 */
+    XGINew_SetReg1( P3c4 , 0x16 , 0x20 ) ;   /* Set SR16 */
+    DelayUS( 30 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0xA0 ) ;   /* Set SR16 */
+    DelayUS( 15 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x1B , 0x04 ) ;   /* Set SR1B refresh control 000:close; 010:open */
+    DelayUS( 200 ) ;
+
+
+}
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DDR2_MRS_XG20 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_DDR2_MRS_XG20( PXGI_HW_DEVICE_INFO HwDeviceExtension ,  ULONG P3c4 , PVB_DEVICE_INFO pVBInfo)
+{
+    ULONG P3d4 = P3c4 + 0x10 ;
+
+    XGINew_RAMType = ( int )XGINew_GetXG20DRAMType( HwDeviceExtension , pVBInfo ) ;
+    XGINew_SetMemoryClock( HwDeviceExtension , pVBInfo ) ;
+
+    XGINew_SetReg1( P3d4 , 0x97 , 0x11 ) ;                      /* CR97 */
+
+    DelayUS( 200 ) ;
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;                      /* EMRS2 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x80 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x05 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x85 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;                      /* EMRS3 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0xC0 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x05 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x85 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;                      /* EMRS1 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x40 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x05 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x85 ) ;
+
+   // XGINew_SetReg1( P3c4 , 0x18 , 0x52 ) ;                    /* MRS1 */
+    XGINew_SetReg1( P3c4 , 0x18 , 0x42 ) ;                      /* MRS1 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x02 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x05 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x85 ) ;
+
+    DelayUS( 15 ) ;
+    XGINew_SetReg1( P3c4 , 0x1B , 0x04 ) ;                      /* SR1B */
+    DelayUS( 30 ) ;
+    XGINew_SetReg1( P3c4 , 0x1B , 0x00 ) ;                      /* SR1B */
+    DelayUS( 100 ) ;
+
+    //XGINew_SetReg1( P3c4 , 0x18 , 0x52 ) ;                    /* MRS2 */
+    XGINew_SetReg1( P3c4 , 0x18 , 0x42 ) ;                      /* MRS1 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x00 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x05 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x85 ) ;
+
+    DelayUS( 200 ) ;
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DDR2_MRS_XG20 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_DDR2_MRS_XG27( PXGI_HW_DEVICE_INFO HwDeviceExtension ,  ULONG P3c4 , PVB_DEVICE_INFO pVBInfo)
+{
+    ULONG P3d4 = P3c4 + 0x10 ;
+
+     XGINew_RAMType = ( int )XGINew_GetXG20DRAMType( HwDeviceExtension , pVBInfo ) ;
+     XGINew_SetMemoryClock( HwDeviceExtension , pVBInfo ) ;
+
+    XGINew_SetReg1( P3d4 , 0x97 , 0x11 ) ;                      /* CR97 */
+    DelayUS( 200 ) ;
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;                      /* EMRS2 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x80 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x16 , 0x10 ) ;
+    DelayUS( 15 ) ;                          ////06/11/23 XG27 A0 for CKE enable
+    XGINew_SetReg1( P3c4 , 0x16 , 0x90 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;                      /* EMRS3 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0xC0 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;
+    DelayUS( 15 ) ;                          ////06/11/22 XG27 A0
+    XGINew_SetReg1( P3c4 , 0x16 , 0x80 ) ;
+
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;                      /* EMRS1 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x40 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;
+    DelayUS( 15 ) ;                          ////06/11/22 XG27 A0
+    XGINew_SetReg1( P3c4 , 0x16 , 0x80 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x42 ) ;                      /* MRS1 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x06 ) ;   ////[Billy]06/11/22 DLL Reset for XG27 Hynix DRAM
+
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;
+    DelayUS( 15 ) ;                          ////06/11/23 XG27 A0
+    XGINew_SetReg1( P3c4 , 0x16 , 0x80 ) ;
+
+    DelayUS( 30 ) ;                          ////06/11/23 XG27 A0 Start Auto-PreCharge
+    XGINew_SetReg1( P3c4 , 0x1B , 0x04 ) ;                      /* SR1B */
+    DelayUS( 60 ) ;
+    XGINew_SetReg1( P3c4 , 0x1B , 0x00 ) ;                      /* SR1B */
+
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x42 ) ;                      /* MRS1 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x04 ) ;   //// DLL without Reset for XG27 Hynix DRAM
+
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;
+    DelayUS( 30 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x80 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x80 );     ////XG27 OCD ON
+    XGINew_SetReg1( P3c4 , 0x19 , 0x46 );
+
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;
+    DelayUS( 30 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x80 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 );
+    XGINew_SetReg1( P3c4 , 0x19 , 0x40 );
+
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;
+    DelayUS( 30 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x80 ) ;
+
+    DelayUS( 15 ) ;                         ////Start Auto-PreCharge
+    XGINew_SetReg1( P3c4 , 0x1B , 0x04 ) ;                      /* SR1B */
+    DelayUS( 200 ) ;
+    XGINew_SetReg1( P3c4 , 0x1B , 0x03 ) ;                      /* SR1B */
+
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DDR1x_DefaultRegister */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_DDR1x_DefaultRegister( PXGI_HW_DEVICE_INFO HwDeviceExtension ,  ULONG Port , PVB_DEVICE_INFO pVBInfo)
+{
+    ULONG P3d4 = Port ,
+           P3c4 = Port - 0x10 ;
+
+    if ( HwDeviceExtension->jChipType >= XG20 )
+    {
+        XGINew_SetMemoryClock( HwDeviceExtension , pVBInfo ) ;
+        XGINew_SetReg1( P3d4 , 0x82 , pVBInfo->CR40[ 11 ][ XGINew_RAMType ] ) ; /* CR82 */
+        XGINew_SetReg1( P3d4 , 0x85 , pVBInfo->CR40[ 12 ][ XGINew_RAMType ] ) ; /* CR85 */
+        XGINew_SetReg1( P3d4 , 0x86 , pVBInfo->CR40[ 13 ][ XGINew_RAMType ] ) ; /* CR86 */
+
+        XGINew_SetReg1( P3d4 , 0x98 , 0x01 ) ;
+        XGINew_SetReg1( P3d4 , 0x9A , 0x02 ) ;
+
+        XGINew_DDR1x_MRS_XG20( P3c4 , pVBInfo) ;
+    }
+    else
+    {
+        XGINew_SetMemoryClock( HwDeviceExtension , pVBInfo ) ;
+
+        switch( HwDeviceExtension->jChipType )
+        {
+            case XG41:
+            case XG42:
+                XGINew_SetReg1( P3d4 , 0x82 , pVBInfo->CR40[ 11 ][ XGINew_RAMType ] ) ; /* CR82 */
+                XGINew_SetReg1( P3d4 , 0x85 , pVBInfo->CR40[ 12 ][ XGINew_RAMType ] ) ; /* CR85 */
+                XGINew_SetReg1( P3d4 , 0x86 , pVBInfo->CR40[ 13 ][ XGINew_RAMType ] ) ; /* CR86 */
+                break ;
+            default:
+                XGINew_SetReg1( P3d4 , 0x82 , 0x88 ) ;
+                XGINew_SetReg1( P3d4 , 0x86 , 0x00 ) ;
+                XGINew_GetReg1( P3d4 , 0x86 ) ;                         /* Insert read command for delay */
+                XGINew_SetReg1( P3d4 , 0x86 , 0x88 ) ;
+                XGINew_GetReg1( P3d4 , 0x86 ) ;
+                XGINew_SetReg1( P3d4 , 0x86 , pVBInfo->CR40[ 13 ][ XGINew_RAMType ] ) ;
+                XGINew_SetReg1( P3d4 , 0x82 , 0x77 ) ;
+                XGINew_SetReg1( P3d4 , 0x85 , 0x00 ) ;
+                XGINew_GetReg1( P3d4 , 0x85 ) ;                         /* Insert read command for delay */
+                XGINew_SetReg1( P3d4 , 0x85 , 0x88 ) ;
+                XGINew_GetReg1( P3d4 , 0x85 ) ;                         /* Insert read command for delay */
+                XGINew_SetReg1( P3d4 , 0x85 , pVBInfo->CR40[ 12 ][ XGINew_RAMType ] ) ; /* CR85 */
+                XGINew_SetReg1( P3d4 , 0x82 , pVBInfo->CR40[ 11 ][ XGINew_RAMType ] ) ; /* CR82 */
+                break ;
+        }
+
+        XGINew_SetReg1( P3d4 , 0x97 , 0x00 ) ;
+        XGINew_SetReg1( P3d4 , 0x98 , 0x01 ) ;
+        XGINew_SetReg1( P3d4 , 0x9A , 0x02 ) ;
+        XGINew_DDR1x_MRS_340( P3c4 , pVBInfo ) ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DDR2x_DefaultRegister */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_DDR2x_DefaultRegister( PXGI_HW_DEVICE_INFO HwDeviceExtension ,  ULONG Port ,PVB_DEVICE_INFO pVBInfo)
+{
+    ULONG P3d4 = Port ,
+           P3c4 = Port - 0x10 ;
+
+    XGINew_SetMemoryClock( HwDeviceExtension , pVBInfo ) ;
+
+    /* 20040906 Hsuan modify CR82, CR85, CR86 for XG42 */
+    switch( HwDeviceExtension->jChipType )
+    {
+       case XG41:
+       case XG42:
+            XGINew_SetReg1( P3d4 , 0x82 , pVBInfo->CR40[ 11 ][ XGINew_RAMType ] ) ;     /* CR82 */
+            XGINew_SetReg1( P3d4 , 0x85 , pVBInfo->CR40[ 12 ][ XGINew_RAMType ] ) ;     /* CR85 */
+            XGINew_SetReg1( P3d4 , 0x86 , pVBInfo->CR40[ 13 ][ XGINew_RAMType ] ) ;     /* CR86 */
+            break ;
+       default:
+         /* keep following setting sequence, each setting in the same reg insert idle */
+         XGINew_SetReg1( P3d4 , 0x82 , 0x88 ) ;
+         XGINew_SetReg1( P3d4 , 0x86 , 0x00 ) ;
+         XGINew_GetReg1( P3d4 , 0x86 ) ;                                /* Insert read command for delay */
+         XGINew_SetReg1( P3d4 , 0x86 , 0x88 ) ;
+         XGINew_SetReg1( P3d4 , 0x82 , 0x77 ) ;
+         XGINew_SetReg1( P3d4 , 0x85 , 0x00 ) ;
+         XGINew_GetReg1( P3d4 , 0x85 ) ;                                /* Insert read command for delay */
+         XGINew_SetReg1( P3d4 , 0x85 , 0x88 ) ;
+         XGINew_GetReg1( P3d4 , 0x85 ) ;                                /* Insert read command for delay */
+         XGINew_SetReg1( P3d4 , 0x85 , pVBInfo->CR40[ 12 ][ XGINew_RAMType ] ) ;        /* CR85 */
+         XGINew_SetReg1( P3d4 , 0x82 , pVBInfo->CR40[ 11 ][ XGINew_RAMType ] ) ;        /* CR82 */
+    }
+    XGINew_SetReg1( P3d4 , 0x97 , 0x11 ) ;
+    if ( HwDeviceExtension->jChipType == XG42 )
+    {
+      XGINew_SetReg1( P3d4 , 0x98 , 0x01 ) ;
+    }
+    else
+    {
+      XGINew_SetReg1( P3d4 , 0x98 , 0x03 ) ;
+    }
+    XGINew_SetReg1( P3d4 , 0x9A , 0x02 ) ;
+
+    XGINew_DDR2x_MRS_340( P3c4 , pVBInfo ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DDR2_DefaultRegister */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_DDR2_DefaultRegister( PXGI_HW_DEVICE_INFO HwDeviceExtension, ULONG Port , PVB_DEVICE_INFO pVBInfo)
+{
+    ULONG P3d4 = Port ,
+           P3c4 = Port - 0x10 ;
+
+    /* keep following setting sequence, each setting in the same reg insert idle */
+    XGINew_SetReg1( P3d4 , 0x82 , 0x77 ) ;
+    XGINew_SetReg1( P3d4 , 0x86 , 0x00 ) ;
+    XGINew_GetReg1( P3d4 , 0x86 ) ;                             /* Insert read command for delay */
+    XGINew_SetReg1( P3d4 , 0x86 , 0x88 ) ;
+    XGINew_GetReg1( P3d4 , 0x86 ) ;                             /* Insert read command for delay */
+    XGINew_SetReg1( P3d4 , 0x86 , pVBInfo->CR40[ 13 ][ XGINew_RAMType ] ) ;     /* CR86 */
+    XGINew_SetReg1( P3d4 , 0x82 , 0x77 ) ;
+    XGINew_SetReg1( P3d4 , 0x85 , 0x00 ) ;
+    XGINew_GetReg1( P3d4 , 0x85 ) ;                             /* Insert read command for delay */
+    XGINew_SetReg1( P3d4 , 0x85 , 0x88 ) ;
+    XGINew_GetReg1( P3d4 , 0x85 ) ;                             /* Insert read command for delay */
+    XGINew_SetReg1( P3d4 , 0x85 , pVBInfo->CR40[ 12 ][ XGINew_RAMType ] ) ;     /* CR85 */
+    if ( HwDeviceExtension->jChipType == XG27 )
+      XGINew_SetReg1( P3d4 , 0x82 , pVBInfo->CR40[ 11 ][ XGINew_RAMType ] ) ;   /* CR82 */
+    else
+    XGINew_SetReg1( P3d4 , 0x82 , 0xA8 ) ;      /* CR82 */
+
+    XGINew_SetReg1( P3d4 , 0x98 , 0x01 ) ;
+    XGINew_SetReg1( P3d4 , 0x9A , 0x02 ) ;
+    if ( HwDeviceExtension->jChipType == XG27 )
+       XGINew_DDRII_Bootup_XG27( HwDeviceExtension ,  P3c4 , pVBInfo) ;
+    else
+    XGINew_DDR2_MRS_XG20( HwDeviceExtension , P3c4, pVBInfo ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetDRAMDefaultRegister340 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SetDRAMDefaultRegister340( PXGI_HW_DEVICE_INFO HwDeviceExtension ,  ULONG Port , PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR temp , temp1 , temp2 , temp3 ,
+          i , j , k ;
+
+    ULONG P3d4 = Port ,
+           P3c4 = Port - 0x10 ;
+
+    XGINew_SetReg1( P3d4 , 0x6D , pVBInfo->CR40[ 8 ][ XGINew_RAMType ] ) ;
+    XGINew_SetReg1( P3d4 , 0x68 , pVBInfo->CR40[ 5 ][ XGINew_RAMType ] ) ;
+    XGINew_SetReg1( P3d4 , 0x69 , pVBInfo->CR40[ 6 ][ XGINew_RAMType ] ) ;
+    XGINew_SetReg1( P3d4 , 0x6A , pVBInfo->CR40[ 7 ][ XGINew_RAMType ] ) ;
+
+    temp2 = 0 ;
+    for( i = 0 ; i < 4 ; i++ )
+    {
+        temp = pVBInfo->CR6B[ XGINew_RAMType ][ i ] ;                   /* CR6B DQS fine tune delay */
+        for( j = 0 ; j < 4 ; j++ )
+        {
+            temp1 = ( ( temp >> ( 2 * j ) ) & 0x03 ) << 2 ;
+            temp2 |= temp1 ;
+            XGINew_SetReg1( P3d4 , 0x6B , temp2 ) ;
+            XGINew_GetReg1( P3d4 , 0x6B ) ;                             /* Insert read command for delay */
+            temp2 &= 0xF0 ;
+            temp2 += 0x10 ;
+        }
+    }
+
+    temp2 = 0 ;
+    for( i = 0 ; i < 4 ; i++ )
+    {
+        temp = pVBInfo->CR6E[ XGINew_RAMType ][ i ] ;                   /* CR6E DQM fine tune delay */
+        for( j = 0 ; j < 4 ; j++ )
+        {
+            temp1 = ( ( temp >> ( 2 * j ) ) & 0x03 ) << 2 ;
+            temp2 |= temp1 ;
+            XGINew_SetReg1( P3d4 , 0x6E , temp2 ) ;
+            XGINew_GetReg1( P3d4 , 0x6E ) ;                             /* Insert read command for delay */
+            temp2 &= 0xF0 ;
+            temp2 += 0x10 ;
+        }
+    }
+
+    temp3 = 0 ;
+    for( k = 0 ; k < 4 ; k++ )
+    {
+        XGINew_SetRegANDOR( P3d4 , 0x6E , 0xFC , temp3 ) ;              /* CR6E_D[1:0] select channel */
+        temp2 = 0 ;
+        for( i = 0 ; i < 8 ; i++ )
+        {
+            temp = pVBInfo->CR6F[ XGINew_RAMType ][ 8 * k + i ] ;       /* CR6F DQ fine tune delay */
+            for( j = 0 ; j < 4 ; j++ )
+            {
+                temp1 = ( temp >> ( 2 * j ) ) & 0x03 ;
+                temp2 |= temp1 ;
+                XGINew_SetReg1( P3d4 , 0x6F , temp2 ) ;
+                XGINew_GetReg1( P3d4 , 0x6F ) ;                         /* Insert read command for delay */
+                temp2 &= 0xF8 ;
+                temp2 += 0x08 ;
+            }
+        }
+        temp3 += 0x01 ;
+    }
+
+    XGINew_SetReg1( P3d4 , 0x80 , pVBInfo->CR40[ 9 ][ XGINew_RAMType ] ) ;      /* CR80 */
+    XGINew_SetReg1( P3d4 , 0x81 , pVBInfo->CR40[ 10 ][ XGINew_RAMType ] ) ;     /* CR81 */
+
+    temp2 = 0x80 ;
+    temp = pVBInfo->CR89[ XGINew_RAMType ][ 0 ] ;                       /* CR89 terminator type select */
+    for( j = 0 ; j < 4 ; j++ )
+    {
+        temp1 = ( temp >> ( 2 * j ) ) & 0x03 ;
+        temp2 |= temp1 ;
+        XGINew_SetReg1( P3d4 , 0x89 , temp2 ) ;
+        XGINew_GetReg1( P3d4 , 0x89 ) ;                         /* Insert read command for delay */
+        temp2 &= 0xF0 ;
+        temp2 += 0x10 ;
+    }
+
+    temp = pVBInfo->CR89[ XGINew_RAMType ][ 1 ] ;
+    temp1 = temp & 0x03 ;
+    temp2 |= temp1 ;
+    XGINew_SetReg1( P3d4 , 0x89 , temp2 ) ;
+
+    temp = pVBInfo->CR40[ 3 ][ XGINew_RAMType ] ;
+    temp1 = temp & 0x0F ;
+    temp2 = ( temp >> 4 ) & 0x07 ;
+    temp3 = temp & 0x80 ;
+    XGINew_SetReg1( P3d4 , 0x45 , temp1 ) ;     /* CR45 */
+    XGINew_SetReg1( P3d4 , 0x99 , temp2 ) ;     /* CR99 */
+    XGINew_SetRegOR( P3d4 , 0x40 , temp3 ) ;    /* CR40_D[7] */
+    XGINew_SetReg1( P3d4 , 0x41 , pVBInfo->CR40[ 0 ][ XGINew_RAMType ] ) ;      /* CR41 */
+
+    if ( HwDeviceExtension->jChipType == XG27 )
+      XGINew_SetReg1( P3d4 , 0x8F , *pVBInfo->pCR8F ) ; /* CR8F */
+
+    for( j = 0 ; j <= 6 ; j++ )
+        XGINew_SetReg1( P3d4 , ( 0x90 + j ) , pVBInfo->CR40[ 14 + j ][ XGINew_RAMType ] ) ;     /* CR90 - CR96 */
+
+    for( j = 0 ; j <= 2 ; j++ )
+        XGINew_SetReg1( P3d4 , ( 0xC3 + j ) , pVBInfo->CR40[ 21 + j ][ XGINew_RAMType ] ) ;     /* CRC3 - CRC5 */
+
+    for( j = 0 ; j < 2 ; j++ )
+        XGINew_SetReg1( P3d4 , ( 0x8A + j ) , pVBInfo->CR40[ 1 + j ][ XGINew_RAMType ] ) ;      /* CR8A - CR8B */
+
+    if ( ( HwDeviceExtension->jChipType == XG41 ) || ( HwDeviceExtension->jChipType == XG42 ) )
+        XGINew_SetReg1( P3d4 , 0x8C , 0x87 ) ;
+
+    XGINew_SetReg1( P3d4 , 0x59 , pVBInfo->CR40[ 4 ][ XGINew_RAMType ] ) ;      /* CR59 */
+
+    XGINew_SetReg1( P3d4 , 0x83 , 0x09 ) ;      /* CR83 */
+    XGINew_SetReg1( P3d4 , 0x87 , 0x00 ) ;      /* CR87 */
+    XGINew_SetReg1( P3d4 , 0xCF , *pVBInfo->pCRCF ) ;   /* CRCF */
+    if ( XGINew_RAMType )
+    {
+      //XGINew_SetReg1( P3c4 , 0x17 , 0xC0 ) ;          /* SR17 DDRII */
+      XGINew_SetReg1( P3c4 , 0x17 , 0x80 ) ;            /* SR17 DDRII */
+      if ( HwDeviceExtension->jChipType == XG27 )
+        XGINew_SetReg1( P3c4 , 0x17 , 0x02 ) ;          /* SR17 DDRII */
+
+    }
+    else
+      XGINew_SetReg1( P3c4 , 0x17 , 0x00 ) ;            /* SR17 DDR */
+    XGINew_SetReg1( P3c4 , 0x1A , 0x87 ) ;              /* SR1A */
+
+    temp = XGINew_GetXG20DRAMType( HwDeviceExtension, pVBInfo) ;
+    if( temp == 0 )
+      XGINew_DDR1x_DefaultRegister( HwDeviceExtension, P3d4, pVBInfo ) ;
+    else
+    {
+      XGINew_SetReg1( P3d4 , 0xB0 , 0x80 ) ;            /* DDRII Dual frequency mode */
+      XGINew_DDR2_DefaultRegister( HwDeviceExtension, P3d4, pVBInfo ) ;
+    }
+    XGINew_SetReg1( P3c4 , 0x1B , pVBInfo->SR15[ 3 ][ XGINew_RAMType ] ) ;      /* SR1B */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DDR_MRS */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_DDR_MRS(PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT data ;
+
+    PUCHAR volatile pVideoMemory = ( PUCHAR )pVBInfo->ROMAddr ;
+
+    /* SR16 <- 1F,DF,2F,AF */
+    /* yriver modified SR16 <- 0F,DF,0F,AF */
+    /* enable DLL of DDR SD/SGRAM , SR16 D4=1 */
+    data = pVideoMemory[ 0xFB ] ;
+    /* data = XGINew_GetReg1( pVBInfo->P3c4 , 0x16 ) ; */
+
+    data &= 0x0F ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x16 , data ) ;
+    data |= 0xC0 ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x16 , data ) ;
+    data &= 0x0F ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x16 , data ) ;
+    data |= 0x80 ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x16 , data ) ;
+    data &= 0x0F ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x16 , data ) ;
+    data |= 0xD0 ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x16 , data ) ;
+    data &= 0x0F ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x16 , data ) ;
+    data |= 0xA0 ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x16 , data ) ;
+/*
+   else {
+     data &= 0x0F;
+     data |= 0x10;
+     XGINew_SetReg1(pVBInfo->P3c4,0x16,data);
+
+     if (!(pVBInfo->SR15[1][XGINew_RAMType] & 0x10))
+     {
+       data &= 0x0F;
+     }
+
+     data |= 0xC0;
+     XGINew_SetReg1(pVBInfo->P3c4,0x16,data);
+
+
+     data &= 0x0F;
+     data |= 0x20;
+     XGINew_SetReg1(pVBInfo->P3c4,0x16,data);
+     if (!(pVBInfo->SR15[1][XGINew_RAMType] & 0x10))
+     {
+       data &= 0x0F;
+     }
+
+     data |= 0x80;
+     XGINew_SetReg1(pVBInfo->P3c4,0x16,data);
+   }
+*/
+}
+
+
+/* check if read cache pointer is correct */
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_VerifyMclk */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_VerifyMclk( PXGI_HW_DEVICE_INFO  HwDeviceExtension , PVB_DEVICE_INFO pVBInfo)
+{
+    PUCHAR pVideoMemory = pVBInfo->FBAddr ;
+    UCHAR i , j ;
+    USHORT Temp , SR21 ;
+
+    pVideoMemory[ 0 ] = 0xaa ;          /* alan */
+    pVideoMemory[ 16 ] = 0x55 ;         /* note: PCI read cache is off */
+
+    if ( ( pVideoMemory[ 0 ] != 0xaa ) || ( pVideoMemory[ 16 ] != 0x55 ) )
+    {
+        for( i = 0 , j = 16 ; i < 2 ; i++ , j += 16 )
+        {
+            SR21 = XGINew_GetReg1( pVBInfo->P3c4 , 0x21 ) ;
+            Temp = SR21 & 0xFB ;        /* disable PCI post write buffer empty gating */
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x21 , Temp ) ;
+
+            Temp = XGINew_GetReg1( pVBInfo->P3c4 , 0x3C ) ;
+            Temp |= 0x01 ;              /* MCLK reset */
+
+
+            Temp = XGINew_GetReg1( pVBInfo->P3c4 , 0x3C ) ;
+            Temp &= 0xFE ;              /* MCLK normal operation */
+
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x21 , SR21 ) ;
+
+            pVideoMemory[ 16 + j ] = j ;
+            if ( pVideoMemory[ 16 + j ] == j )
+            {
+                pVideoMemory[ j ] = j ;
+                break ;
+            }
+        }
+    }
+}
+
+
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetDRAMSize_340 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SetDRAMSize_340( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT  data ;
+
+    pVBInfo->ROMAddr = HwDeviceExtension->pjVirtualRomBase ;
+    pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress ;
+
+    XGISetModeNew( HwDeviceExtension , 0x2e ) ;
+
+
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x21 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x21 , ( USHORT )( data & 0xDF ) ) ;        /* disable read cache */
+    XGI_DisplayOff( HwDeviceExtension, pVBInfo );
+
+    /*data = XGINew_GetReg1( pVBInfo->P3c4 , 0x1 ) ;*/
+    /*data |= 0x20 ;*/
+    /*XGINew_SetReg1( pVBInfo->P3c4 , 0x01 , data ) ;*/                 /* Turn OFF Display */
+    XGINew_DDRSizing340( HwDeviceExtension, pVBInfo ) ;
+    data=XGINew_GetReg1( pVBInfo->P3c4 , 0x21 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x21 , ( USHORT )( data | 0x20 ) ) ;        /* enable read cache */
+
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SetDRAMSize_310( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT data ;
+    pVBInfo->ROMAddr  = HwDeviceExtension->pjVirtualRomBase ,
+    pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress ;
+#ifdef XGI301
+    /* XGINew_SetReg1( pVBInfo->P3d4 , 0x30 , 0x40 ) ; */
+#endif
+
+#ifdef XGI302   /* alan,should change value */
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x30 , 0x4D ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x31 , 0xc0 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x34 , 0x3F ) ;
+#endif
+
+    XGISetModeNew( HwDeviceExtension , 0x2e ) ;
+
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x21 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x21 , ( USHORT )( data & 0xDF ) ) ;        /* disable read cache */
+
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x1 ) ;
+    data |= 0x20 ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x01 , data ) ;             /* Turn OFF Display */
+
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x16 ) ;
+
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x16 , ( USHORT )( data | 0x0F ) ) ;                /* assume lowest speed DRAM */
+
+    XGINew_SetDRAMModeRegister( pVBInfo ) ;
+    XGINew_DisableRefresh( HwDeviceExtension, pVBInfo ) ;
+    XGINew_CheckBusWidth_310( pVBInfo) ;
+    XGINew_VerifyMclk( HwDeviceExtension, pVBInfo ) ;   /* alan 2000/7/3 */
+
+
+
+    if ( XGINew_Get310DRAMType( pVBInfo ) < 2 )
+    {
+        XGINew_SDRSizing( pVBInfo ) ;
+    }
+    else
+    {
+        XGINew_DDRSizing( pVBInfo) ;
+    }
+
+
+
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x16 , pVBInfo->SR15[ 1 ][ XGINew_RAMType ] ) ;     /* restore SR16 */
+
+    XGINew_EnableRefresh(  HwDeviceExtension, pVBInfo ) ;
+    data=XGINew_GetReg1( pVBInfo->P3c4 ,0x21 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x21 , ( USHORT )( data | 0x20 ) ) ;        /* enable read cache */
+}
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetDRAMModeRegister340 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+
+void XGINew_SetDRAMModeRegister340( PXGI_HW_DEVICE_INFO HwDeviceExtension )
+{
+    UCHAR data ;
+    VB_DEVICE_INFO VBINF;
+    PVB_DEVICE_INFO pVBInfo = &VBINF;
+    pVBInfo->ROMAddr = HwDeviceExtension->pjVirtualRomBase ;
+    pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress ;
+    pVBInfo->BaseAddr = (ULONG)HwDeviceExtension->pjIOAddress ;
+    pVBInfo->ISXPDOS = 0 ;
+
+    pVBInfo->P3c4 = pVBInfo->BaseAddr + 0x14 ;
+    pVBInfo->P3d4 = pVBInfo->BaseAddr + 0x24 ;
+    pVBInfo->P3c0 = pVBInfo->BaseAddr + 0x10 ;
+    pVBInfo->P3ce = pVBInfo->BaseAddr + 0x1e ;
+    pVBInfo->P3c2 = pVBInfo->BaseAddr + 0x12 ;
+    pVBInfo->P3ca = pVBInfo->BaseAddr + 0x1a ;
+    pVBInfo->P3c6 = pVBInfo->BaseAddr + 0x16 ;
+    pVBInfo->P3c7 = pVBInfo->BaseAddr + 0x17 ;
+    pVBInfo->P3c8 = pVBInfo->BaseAddr + 0x18 ;
+    pVBInfo->P3c9 = pVBInfo->BaseAddr + 0x19 ;
+    pVBInfo->P3da = pVBInfo->BaseAddr + 0x2A ;
+    pVBInfo->Part0Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_00 ;
+    pVBInfo->Part1Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_04 ;
+    pVBInfo->Part2Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_10 ;
+    pVBInfo->Part3Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_12 ;
+    pVBInfo->Part4Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 ;
+    pVBInfo->Part5Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 + 2 ;
+    if ( HwDeviceExtension->jChipType < XG20 )                  /* kuku 2004/06/25 */
+    XGI_GetVBType( pVBInfo ) ;         /* Run XGI_GetVBType before InitTo330Pointer */
+
+    InitTo330Pointer(HwDeviceExtension->jChipType,pVBInfo);
+
+    ReadVBIOSTablData( HwDeviceExtension->jChipType , pVBInfo) ;
+
+    if ( XGINew_GetXG20DRAMType( HwDeviceExtension, pVBInfo) == 0 )
+    {
+        data = ( XGINew_GetReg1( pVBInfo->P3c4 , 0x39 ) & 0x02 ) >> 1 ;
+        if ( data == 0x01 )
+            XGINew_DDR2x_MRS_340( pVBInfo->P3c4, pVBInfo ) ;
+        else
+            XGINew_DDR1x_MRS_340( pVBInfo->P3c4, pVBInfo ) ;
+    }
+    else
+        XGINew_DDR2_MRS_XG20( HwDeviceExtension, pVBInfo->P3c4, pVBInfo);
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x1B , 0x03 ) ;
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetDRAMModeRegister */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SetDRAMModeRegister( PVB_DEVICE_INFO pVBInfo)
+{
+    if ( XGINew_Get310DRAMType( pVBInfo ) < 2 )
+    {
+      XGINew_SDR_MRS(pVBInfo ) ;
+    }
+    else
+    {
+      /* SR16 <- 0F,CF,0F,8F */
+      XGINew_DDR_MRS( pVBInfo ) ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DisableRefresh */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_DisableRefresh( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT  data ;
+
+
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x1B ) ;
+    data &= 0xF8 ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x1B , data ) ;
+
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_EnableRefresh */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_EnableRefresh( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo)
+{
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x1B , pVBInfo->SR15[ 3 ][ XGINew_RAMType ] ) ;     /* SR1B */
+
+
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DisableChannelInterleaving */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_DisableChannelInterleaving( int index , USHORT XGINew_DDRDRAM_TYPE[][ 5 ] , PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT data ;
+
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x15 ) ;
+    data &= 0x1F ;
+
+    switch( XGINew_DDRDRAM_TYPE[ index ][ 3 ] )
+    {
+        case 64:
+            data |= 0 ;
+            break ;
+        case 32:
+            data |= 0x20 ;
+            break ;
+        case 16:
+            data |= 0x40 ;
+            break ;
+        case 4:
+            data |= 0x60 ;
+            break ;
+        default:
+            break ;
+    }
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x15 , data ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetDRAMSizingType */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SetDRAMSizingType( int index , USHORT DRAMTYPE_TABLE[][ 5 ] ,PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT data ;
+
+    data = DRAMTYPE_TABLE[ index ][ 4 ] ;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x13 , 0x80 , data ) ;
+    DelayUS( 15 ) ;
+   /* should delay 50 ns */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_CheckBusWidth_310 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_CheckBusWidth_310(  PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT data ;
+    PULONG volatile pVideoMemory ;
+
+    pVideoMemory = (PULONG) pVBInfo->FBAddr;
+
+    if ( XGINew_Get310DRAMType( pVBInfo ) < 2 )
+    {
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x00 ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x12 ) ;
+        /* should delay */
+        XGINew_SDR_MRS( pVBInfo ) ;
+
+        XGINew_ChannelAB = 0 ;
+        XGINew_DataBusWidth = 128 ;
+        pVideoMemory[ 0 ] = 0x01234567L ;
+        pVideoMemory[ 1 ] = 0x456789ABL ;
+        pVideoMemory[ 2 ] = 0x89ABCDEFL ;
+        pVideoMemory[ 3 ] = 0xCDEF0123L ;
+        pVideoMemory[ 4 ] = 0x55555555L ;
+        pVideoMemory[ 5 ] = 0x55555555L ;
+        pVideoMemory[ 6 ] = 0xFFFFFFFFL ;
+        pVideoMemory[ 7 ] = 0xFFFFFFFFL ;
+
+        if ( ( pVideoMemory[ 3 ] != 0xCDEF0123L ) || ( pVideoMemory[ 2 ] != 0x89ABCDEFL ) )
+        {
+            /* ChannelA64Bit */
+            XGINew_DataBusWidth = 64 ;
+            XGINew_ChannelAB = 0 ;
+            data=XGINew_GetReg1( pVBInfo->P3c4 , 0x14 ) ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , ( USHORT )( data & 0xFD ) ) ;
+        }
+
+        if ( ( pVideoMemory[ 1 ] != 0x456789ABL ) || ( pVideoMemory[ 0 ] != 0x01234567L ) )
+        {
+            /* ChannelB64Bit */
+            XGINew_DataBusWidth = 64 ;
+            XGINew_ChannelAB = 1 ;
+            data=XGINew_GetReg1( pVBInfo->P3c4 , 0x14 ) ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , ( USHORT )( ( data & 0xFD ) | 0x01 ) ) ;
+        }
+
+        return ;
+    }
+    else
+    {
+        /* DDR Dual channel */
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x00 ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x02 ) ; /* Channel A, 64bit */
+        /* should delay */
+        XGINew_DDR_MRS( pVBInfo ) ;
+
+        XGINew_ChannelAB = 0 ;
+        XGINew_DataBusWidth = 64 ;
+        pVideoMemory[ 0 ] = 0x01234567L ;
+        pVideoMemory[ 1 ] = 0x456789ABL ;
+        pVideoMemory[ 2 ] = 0x89ABCDEFL ;
+        pVideoMemory[ 3 ] = 0xCDEF0123L ;
+        pVideoMemory[ 4 ] = 0x55555555L ;
+        pVideoMemory[ 5 ] = 0x55555555L ;
+        pVideoMemory[ 6 ] = 0xAAAAAAAAL ;
+        pVideoMemory[ 7 ] = 0xAAAAAAAAL ;
+
+        if ( pVideoMemory[ 1 ] == 0x456789ABL )
+        {
+            if ( pVideoMemory[ 0 ] == 0x01234567L )
+            {
+                /* Channel A 64bit */
+                return ;
+            }
+        }
+        else
+        {
+            if ( pVideoMemory[ 0 ] == 0x01234567L )
+            {
+                /* Channel A 32bit */
+                XGINew_DataBusWidth = 32 ;
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x00 ) ;
+                return ;
+            }
+        }
+
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x03 ) ; /* Channel B, 64bit */
+        XGINew_DDR_MRS( pVBInfo);
+
+        XGINew_ChannelAB = 1 ;
+        XGINew_DataBusWidth = 64 ;
+        pVideoMemory[ 0 ] = 0x01234567L ;
+        pVideoMemory[ 1 ] = 0x456789ABL ;
+        pVideoMemory[ 2 ] = 0x89ABCDEFL ;
+        pVideoMemory[ 3 ] = 0xCDEF0123L ;
+        pVideoMemory[ 4 ] = 0x55555555L ;
+        pVideoMemory[ 5 ] = 0x55555555L ;
+        pVideoMemory[ 6 ] = 0xAAAAAAAAL ;
+        pVideoMemory[ 7 ] = 0xAAAAAAAAL ;
+
+        if ( pVideoMemory[ 1 ] == 0x456789ABL )
+        {
+            /* Channel B 64 */
+            if ( pVideoMemory[ 0 ] == 0x01234567L )
+            {
+                /* Channel B 64bit */
+                return ;
+            }
+            else
+            {
+                /* error */
+            }
+        }
+        else
+        {
+            if ( pVideoMemory[ 0 ] == 0x01234567L )
+            {
+                /* Channel B 32 */
+                XGINew_DataBusWidth = 32 ;
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x01 ) ;
+            }
+            else
+            {
+                /* error */
+            }
+        }
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetRank */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int XGINew_SetRank( int index , UCHAR RankNo , UCHAR XGINew_ChannelAB , USHORT DRAMTYPE_TABLE[][ 5 ] , PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT data ;
+    int RankSize ;
+
+    if ( ( RankNo == 2 ) && ( DRAMTYPE_TABLE[ index ][ 0 ] == 2 ) )
+        return 0 ;
+
+    RankSize = DRAMTYPE_TABLE[ index ][ 3 ] / 2 * XGINew_DataBusWidth / 32 ;
+
+    if ( ( RankNo * RankSize ) <= 128 )
+    {
+        data = 0 ;
+
+        while( ( RankSize >>= 1 ) > 0 )
+        {
+            data += 0x10 ;
+        }
+        data |= ( RankNo - 1 ) << 2 ;
+        data |= ( XGINew_DataBusWidth / 64 ) & 2 ;
+        data |= XGINew_ChannelAB ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , data ) ;
+        /* should delay */
+        XGINew_SDR_MRS( pVBInfo ) ;
+        return( 1 ) ;
+    }
+    else
+        return( 0 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetDDRChannel */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int XGINew_SetDDRChannel( int index , UCHAR ChannelNo , UCHAR XGINew_ChannelAB , USHORT DRAMTYPE_TABLE[][ 5 ] , PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT  data ;
+    int RankSize ;
+
+    RankSize = DRAMTYPE_TABLE[index][3]/2 * XGINew_DataBusWidth/32;
+    /* RankSize = DRAMTYPE_TABLE[ index ][ 3 ] ; */
+    if ( ChannelNo * RankSize <= 128 )
+    {
+        data = 0 ;
+        while( ( RankSize >>= 1 ) > 0 )
+        {
+            data += 0x10 ;
+        }
+
+        if ( ChannelNo == 2 )
+            data |= 0x0C ;
+
+        data |= ( XGINew_DataBusWidth / 32 ) & 2 ;
+        data |= XGINew_ChannelAB ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , data ) ;
+        /* should delay */
+        XGINew_DDR_MRS( pVBInfo ) ;
+        return( 1 ) ;
+    }
+    else
+        return( 0 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_CheckColumn */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int XGINew_CheckColumn( int index , USHORT DRAMTYPE_TABLE[][ 5 ], PVB_DEVICE_INFO pVBInfo)
+{
+    int i ;
+    ULONG Increment , Position ;
+
+    /* Increment = 1 << ( DRAMTYPE_TABLE[ index ][ 2 ] + XGINew_DataBusWidth / 64 + 1 ) ; */
+    Increment = 1 << ( 10 + XGINew_DataBusWidth / 64 ) ;
+
+    for( i = 0 , Position = 0 ; i < 2 ; i++ )
+    {
+        *( ( PULONG )( pVBInfo->FBAddr + Position ) ) = Position ;
+        Position += Increment ;
+    }
+
+#ifdef WIN2000  /* chiawen for linux solution */
+    DelayUS( 100 ) ;
+#endif
+
+    for( i = 0 , Position = 0 ; i < 2 ; i++ )
+    {
+        /* if ( pVBInfo->FBAddr[ Position ] != Position ) */
+        if ( ( *( PULONG )( pVBInfo->FBAddr + Position ) ) != Position )
+            return( 0 ) ;
+        Position += Increment ;
+    }
+    return( 1 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_CheckBanks */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int XGINew_CheckBanks( int index , USHORT DRAMTYPE_TABLE[][ 5 ], PVB_DEVICE_INFO pVBInfo)
+{
+    int i ;
+    ULONG Increment , Position ;
+
+    Increment = 1 << ( DRAMTYPE_TABLE[ index ][ 2 ] + XGINew_DataBusWidth / 64 + 2 ) ;
+
+    for( i = 0 , Position = 0 ; i < 4 ; i++ )
+    {
+        /* pVBInfo->FBAddr[ Position ] = Position ; */
+        *( ( PULONG )( pVBInfo->FBAddr + Position ) ) = Position ;
+        Position += Increment ;
+    }
+
+    for( i = 0 , Position = 0 ; i < 4 ; i++ )
+    {
+        /* if (pVBInfo->FBAddr[ Position ] != Position ) */
+        if ( ( *( PULONG )( pVBInfo->FBAddr + Position ) ) != Position )
+            return( 0 ) ;
+        Position += Increment ;
+    }
+    return( 1 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_CheckRank */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int XGINew_CheckRank( int RankNo , int index , USHORT DRAMTYPE_TABLE[][ 5 ], PVB_DEVICE_INFO pVBInfo)
+{
+    int i ;
+    ULONG Increment , Position ;
+
+    Increment = 1 << ( DRAMTYPE_TABLE[ index ][ 2 ] + DRAMTYPE_TABLE[ index ][ 1 ] +
+                  DRAMTYPE_TABLE[ index ][ 0 ] + XGINew_DataBusWidth / 64 + RankNo ) ;
+
+    for( i = 0 , Position = 0 ; i < 2 ; i++ )
+    {
+        /* pVBInfo->FBAddr[ Position ] = Position ; */
+        /* *( ( PULONG )( pVBInfo->FBAddr ) ) = Position ; */
+        *( ( PULONG )( pVBInfo->FBAddr + Position ) ) = Position ;
+        Position += Increment ;
+    }
+
+    for( i = 0 , Position = 0 ; i < 2 ; i++ )
+    {
+        /* if ( pVBInfo->FBAddr[ Position ] != Position ) */
+        /* if ( ( *( PULONG )( pVBInfo->FBAddr ) ) != Position ) */
+        if ( ( *( PULONG )( pVBInfo->FBAddr + Position ) ) != Position )
+            return( 0 ) ;
+        Position += Increment ;
+    }
+    return( 1 );
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_CheckDDRRank */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int XGINew_CheckDDRRank( int RankNo , int index , USHORT DRAMTYPE_TABLE[][ 5 ], PVB_DEVICE_INFO pVBInfo)
+{
+    ULONG Increment , Position ;
+    USHORT data ;
+
+    Increment = 1 << ( DRAMTYPE_TABLE[ index ][ 2 ] + DRAMTYPE_TABLE[ index ][ 1 ] +
+                       DRAMTYPE_TABLE[ index ][ 0 ] + XGINew_DataBusWidth / 64 + RankNo ) ;
+
+    Increment += Increment / 2 ;
+
+    Position = 0;
+    *( ( PULONG )( pVBInfo->FBAddr + Position + 0 ) ) = 0x01234567 ;
+    *( ( PULONG )( pVBInfo->FBAddr + Position + 1 ) ) = 0x456789AB ;
+    *( ( PULONG )( pVBInfo->FBAddr + Position + 2 ) ) = 0x55555555 ;
+    *( ( PULONG )( pVBInfo->FBAddr + Position + 3 ) ) = 0x55555555 ;
+    *( ( PULONG )( pVBInfo->FBAddr + Position + 4 ) ) = 0xAAAAAAAA ;
+    *( ( PULONG )( pVBInfo->FBAddr + Position + 5 ) ) = 0xAAAAAAAA ;
+
+    if ( ( *( PULONG )( pVBInfo->FBAddr + 1 ) ) == 0x456789AB )
+        return( 1 ) ;
+
+    if ( ( *( PULONG )( pVBInfo->FBAddr + 0 ) ) == 0x01234567 )
+        return( 0 ) ;
+
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x14 ) ;
+    data &= 0xF3 ;
+    data |= 0x0E ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , data ) ;
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x15 ) ;
+    data += 0x20 ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x15 , data ) ;
+
+    return( 1 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_CheckRanks */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int XGINew_CheckRanks( int RankNo , int index , USHORT DRAMTYPE_TABLE[][ 5 ], PVB_DEVICE_INFO pVBInfo)
+{
+    int r ;
+
+    for( r = RankNo ; r >= 1 ; r-- )
+    {
+        if ( !XGINew_CheckRank( r , index , DRAMTYPE_TABLE, pVBInfo ) )
+            return( 0 ) ;
+    }
+
+    if ( !XGINew_CheckBanks( index , DRAMTYPE_TABLE, pVBInfo ) )
+        return( 0 ) ;
+
+    if ( !XGINew_CheckColumn( index , DRAMTYPE_TABLE, pVBInfo ) )
+        return( 0 ) ;
+
+    return( 1 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_CheckDDRRanks */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int XGINew_CheckDDRRanks( int RankNo , int index , USHORT DRAMTYPE_TABLE[][ 5 ], PVB_DEVICE_INFO pVBInfo)
+{
+    int r ;
+
+    for( r = RankNo ; r >= 1 ; r-- )
+    {
+        if ( !XGINew_CheckDDRRank( r , index , DRAMTYPE_TABLE, pVBInfo ) )
+            return( 0 ) ;
+    }
+
+    if ( !XGINew_CheckBanks( index , DRAMTYPE_TABLE, pVBInfo ) )
+        return( 0 ) ;
+
+    if ( !XGINew_CheckColumn( index , DRAMTYPE_TABLE, pVBInfo ) )
+        return( 0 ) ;
+
+    return( 1 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int XGINew_SDRSizing(PVB_DEVICE_INFO pVBInfo)
+{
+    int    i ;
+    UCHAR  j ;
+
+    for( i = 0 ; i < 13 ; i++ )
+    {
+        XGINew_SetDRAMSizingType( i , XGINew_SDRDRAM_TYPE , pVBInfo) ;
+
+        for( j = 2 ; j > 0 ; j-- )
+        {
+            if ( !XGINew_SetRank( i , ( UCHAR )j , XGINew_ChannelAB , XGINew_SDRDRAM_TYPE , pVBInfo) )
+                continue ;
+            else
+            {
+                if ( XGINew_CheckRanks( j , i , XGINew_SDRDRAM_TYPE, pVBInfo) )
+                    return( 1 ) ;
+            }
+        }
+    }
+    return( 0 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetDRAMSizeReg */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+USHORT XGINew_SetDRAMSizeReg( int index , USHORT DRAMTYPE_TABLE[][ 5 ], PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT data = 0 , memsize = 0 ;
+    int RankSize ;
+    UCHAR ChannelNo ;
+
+    RankSize = DRAMTYPE_TABLE[ index ][ 3 ] * XGINew_DataBusWidth / 32 ;
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x13 ) ;
+    data &= 0x80 ;
+
+    if ( data == 0x80 )
+        RankSize *= 2 ;
+
+    data = 0 ;
+
+    if( XGINew_ChannelAB == 3 )
+        ChannelNo = 4 ;
+    else
+        ChannelNo = XGINew_ChannelAB ;
+
+    if ( ChannelNo * RankSize <= 256 )
+    {
+        while( ( RankSize >>= 1 ) > 0 )
+        {
+            data += 0x10 ;
+        }
+
+        memsize = data >> 4 ;
+
+        /* [2004/03/25] Vicent, Fix DRAM Sizing Error */
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , ( XGINew_GetReg1( pVBInfo->P3c4 , 0x14 ) & 0x0F ) | ( data & 0xF0 ) ) ;
+
+       /* data |= XGINew_ChannelAB << 2 ; */
+       /* data |= ( XGINew_DataBusWidth / 64 ) << 1 ; */
+       /* XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , data ) ; */
+
+        /* should delay */
+        /* XGINew_SetDRAMModeRegister340( pVBInfo ) ; */
+    }
+    return( memsize ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetDRAMSize20Reg */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+USHORT XGINew_SetDRAMSize20Reg( int index , USHORT DRAMTYPE_TABLE[][ 5 ], PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT data = 0 , memsize = 0 ;
+    int RankSize ;
+    UCHAR ChannelNo ;
+
+    RankSize = DRAMTYPE_TABLE[ index ][ 3 ] * XGINew_DataBusWidth / 8 ;
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x13 ) ;
+    data &= 0x80 ;
+
+    if ( data == 0x80 )
+        RankSize *= 2 ;
+
+    data = 0 ;
+
+    if( XGINew_ChannelAB == 3 )
+        ChannelNo = 4 ;
+    else
+        ChannelNo = XGINew_ChannelAB ;
+
+    if ( ChannelNo * RankSize <= 256 )
+    {
+        while( ( RankSize >>= 1 ) > 0 )
+        {
+            data += 0x10 ;
+        }
+
+        memsize = data >> 4 ;
+
+        /* [2004/03/25] Vicent, Fix DRAM Sizing Error */
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , ( XGINew_GetReg1( pVBInfo->P3c4 , 0x14 ) & 0x0F ) | ( data & 0xF0 ) ) ;
+        DelayUS( 15 ) ;
+
+       /* data |= XGINew_ChannelAB << 2 ; */
+       /* data |= ( XGINew_DataBusWidth / 64 ) << 1 ; */
+       /* XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , data ) ; */
+
+        /* should delay */
+        /* XGINew_SetDRAMModeRegister340( pVBInfo ) ; */
+    }
+    return( memsize ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_ReadWriteRest */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int XGINew_ReadWriteRest( USHORT StopAddr , USHORT StartAddr, PVB_DEVICE_INFO pVBInfo)
+{
+    int i ;
+    ULONG Position = 0 ;
+
+   *( ( PULONG )( pVBInfo->FBAddr + Position ) ) = Position ;
+
+    for( i = StartAddr ; i <= StopAddr ; i++ )
+    {
+        Position = 1 << i ;
+        *( ( PULONG )( pVBInfo->FBAddr + Position ) ) = Position ;
+    }
+
+    DelayUS( 500 ) ;    /* [Vicent] 2004/04/16. Fix #1759 Memory Size error in Multi-Adapter. */
+
+    Position = 0 ;
+
+   if ( ( *( PULONG )( pVBInfo->FBAddr + Position ) ) != Position )
+        return( 0 ) ;
+
+    for( i = StartAddr ; i <= StopAddr ; i++ )
+    {
+        Position = 1 << i ;
+        if ( ( *( PULONG )( pVBInfo->FBAddr + Position ) ) != Position )
+            return( 0 ) ;
+    }
+    return( 1 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_CheckFrequence */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+UCHAR XGINew_CheckFrequence( PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR data ;
+
+    data = XGINew_GetReg1( pVBInfo->P3d4 , 0x97 ) ;
+
+    if ( ( data & 0x10 ) == 0 )
+    {
+        data = XGINew_GetReg1( pVBInfo->P3c4 , 0x39 ) ;
+        data = ( data & 0x02 ) >> 1 ;
+        return( data ) ;
+    }
+    else
+        return( data & 0x01 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_CheckChannel */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_CheckChannel( PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR data;
+
+    switch( HwDeviceExtension->jChipType )
+    {
+      case XG20:
+      case XG21:
+          data = XGINew_GetReg1( pVBInfo->P3d4 , 0x97 ) ;
+          data = data & 0x01;
+          XGINew_ChannelAB = 1 ;                /* XG20 "JUST" one channel */
+
+          if ( data == 0 )  /* Single_32_16 */
+          {
+
+              if (( HwDeviceExtension->ulVideoMemorySize - 1 ) > 0x1000000)
+              {
+
+                XGINew_DataBusWidth = 32 ;      /* 32 bits */
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0xB1 ) ;  /* 22bit + 2 rank + 32bit */
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x52 ) ;
+                DelayUS( 15 ) ;
+
+                if ( XGINew_ReadWriteRest( 24 , 23 , pVBInfo ) == 1 )
+                    return ;
+
+                if (( HwDeviceExtension->ulVideoMemorySize - 1 ) > 0x800000)
+                {
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x31 ) ;  /* 22bit + 1 rank + 32bit */
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x42 ) ;
+                  DelayUS( 15 ) ;
+
+                  if ( XGINew_ReadWriteRest( 23 , 23 , pVBInfo ) == 1 )
+                      return ;
+                }
+              }
+
+              if (( HwDeviceExtension->ulVideoMemorySize - 1 ) > 0x800000)
+              {
+                XGINew_DataBusWidth = 16 ;      /* 16 bits */
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0xB1 ) ;  /* 22bit + 2 rank + 16bit */
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x41 ) ;
+                DelayUS( 15 ) ;
+
+                if ( XGINew_ReadWriteRest( 23 , 22 , pVBInfo ) == 1 )
+                    return ;
+                else
+                    XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x31 ) ;
+                    DelayUS( 15 ) ;
+              }
+
+          }
+          else  /* Dual_16_8 */
+          {
+              if (( HwDeviceExtension->ulVideoMemorySize - 1 ) > 0x800000)
+              {
+
+                XGINew_DataBusWidth = 16 ;      /* 16 bits */
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0xB1 ) ;  /* (0x31:12x8x2) 22bit + 2 rank */
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x41 ) ;  /* 0x41:16Mx16 bit*/
+                DelayUS( 15 ) ;
+
+                if ( XGINew_ReadWriteRest( 23 , 22 , pVBInfo ) == 1 )
+                    return ;
+
+                if (( HwDeviceExtension->ulVideoMemorySize - 1 ) > 0x400000)
+                {
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x31 ) ;  /* (0x31:12x8x2) 22bit + 1 rank */
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x31 ) ;  /* 0x31:8Mx16 bit*/
+                  DelayUS( 15 ) ;
+
+                  if ( XGINew_ReadWriteRest( 22 , 22 , pVBInfo ) == 1 )
+                      return ;
+                }
+              }
+
+
+              if (( HwDeviceExtension->ulVideoMemorySize - 1 ) > 0x400000)
+              {
+                XGINew_DataBusWidth = 8 ;       /* 8 bits */
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0xB1 ) ;  /* (0x31:12x8x2) 22bit + 2 rank */
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x30 ) ;  /* 0x30:8Mx8 bit*/
+                DelayUS( 15 ) ;
+
+                if ( XGINew_ReadWriteRest( 22 , 21 , pVBInfo ) == 1 )
+                    return ;
+                else
+                    XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x31 ) ;  /* (0x31:12x8x2) 22bit + 1 rank */
+                    DelayUS( 15 ) ;
+              }
+          }
+          break ;
+
+      case XG27:
+          XGINew_DataBusWidth = 16 ;    /* 16 bits */
+          XGINew_ChannelAB = 1 ;                /* Single channel */
+          XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x51 ) ;  /* 32Mx16 bit*/
+          break ;
+      case XG41:
+          if ( XGINew_CheckFrequence(pVBInfo) == 1 )
+          {
+              XGINew_DataBusWidth = 32 ;        /* 32 bits */
+              XGINew_ChannelAB = 3 ;            /* Quad Channel */
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0xA1 ) ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x4C ) ;
+
+              if ( XGINew_ReadWriteRest( 25 , 23 , pVBInfo ) == 1 )
+                  return ;
+
+              XGINew_ChannelAB = 2 ;            /* Dual channels */
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x48 ) ;
+
+              if ( XGINew_ReadWriteRest( 24 , 23 , pVBInfo ) == 1 )
+                  return ;
+
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x49 ) ;
+
+              if ( XGINew_ReadWriteRest( 24 , 23 , pVBInfo ) == 1 )
+                  return ;
+
+              XGINew_ChannelAB = 3 ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x21 ) ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x3C ) ;
+
+              if ( XGINew_ReadWriteRest( 24 , 23 , pVBInfo ) == 1 )
+                  return ;
+
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x38 ) ;
+
+              if ( XGINew_ReadWriteRest( 8 , 4 , pVBInfo ) == 1 )
+                  return ;
+              else
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x39 ) ;
+          }
+          else
+          {                                     /* DDR */
+              XGINew_DataBusWidth = 64 ;        /* 64 bits */
+              XGINew_ChannelAB = 2 ;            /* Dual channels */
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0xA1 ) ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x5A ) ;
+
+              if ( XGINew_ReadWriteRest( 25 , 24 , pVBInfo ) == 1 )
+                  return ;
+
+              XGINew_ChannelAB = 1 ;            /* Single channels */
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x52 ) ;
+
+              if ( XGINew_ReadWriteRest( 24 , 23 , pVBInfo ) == 1 )
+                  return ;
+
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x53 ) ;
+
+              if ( XGINew_ReadWriteRest( 24 , 23 , pVBInfo ) == 1 )
+                  return ;
+
+              XGINew_ChannelAB = 2 ;            /* Dual channels */
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x21 ) ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x4A ) ;
+
+              if ( XGINew_ReadWriteRest( 24 , 23 , pVBInfo ) == 1 )
+                  return ;
+
+              XGINew_ChannelAB = 1 ;            /* Single channels */
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x42 ) ;
+
+              if ( XGINew_ReadWriteRest( 8 , 4 , pVBInfo ) == 1 )
+                  return ;
+              else
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x43 ) ;
+          }
+
+          break ;
+
+      case XG42:
+/*
+          XG42 SR14 D[3] Reserve
+                    D[2] = 1, Dual Channel
+                         = 0, Single Channel
+
+          It's Different from Other XG40 Series.
+*/
+          if ( XGINew_CheckFrequence(pVBInfo) == 1 )    /* DDRII, DDR2x */
+          {
+              XGINew_DataBusWidth = 32 ;        /* 32 bits */
+              XGINew_ChannelAB = 2 ;            /* 2 Channel */
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0xA1 ) ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x44 ) ;
+
+              if ( XGINew_ReadWriteRest( 24 , 23 , pVBInfo ) == 1 )
+                  return ;
+
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x21 ) ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x34 ) ;
+              if ( XGINew_ReadWriteRest( 23 , 22 , pVBInfo ) == 1 )
+                  return ;
+
+              XGINew_ChannelAB = 1 ;            /* Single Channel */
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0xA1 ) ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x40 ) ;
+
+              if ( XGINew_ReadWriteRest( 23 , 22 , pVBInfo ) == 1 )
+                  return ;
+              else
+              {
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x21 ) ;
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x30 ) ;
+              }
+          }
+          else
+          {                                     /* DDR */
+              XGINew_DataBusWidth = 64 ;        /* 64 bits */
+              XGINew_ChannelAB = 1 ;            /* 1 channels */
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0xA1 ) ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x52 ) ;
+
+              if ( XGINew_ReadWriteRest( 24 , 23 , pVBInfo ) == 1 )
+                  return ;
+              else
+              {
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x21 ) ;
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x42 ) ;
+              }
+          }
+
+          break ;
+
+      default:  /* XG40 */
+
+          if ( XGINew_CheckFrequence(pVBInfo) == 1 )    /* DDRII */
+          {
+              XGINew_DataBusWidth = 32 ;        /* 32 bits */
+              XGINew_ChannelAB = 3 ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0xA1 ) ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x4C ) ;
+
+              if ( XGINew_ReadWriteRest( 25 , 23 , pVBInfo ) == 1 )
+                  return ;
+
+              XGINew_ChannelAB = 2 ;            /* 2 channels */
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x48 ) ;
+
+              if ( XGINew_ReadWriteRest( 24 , 23 , pVBInfo ) == 1 )
+                  return ;
+
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x21 ) ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x3C ) ;
+
+              if ( XGINew_ReadWriteRest( 24 , 23 , pVBInfo ) == 1 )
+                  XGINew_ChannelAB = 3 ;        /* 4 channels */
+              else
+              {
+                  XGINew_ChannelAB = 2 ;        /* 2 channels */
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x38 ) ;
+              }
+          }
+          else
+          {                                     /* DDR */
+              XGINew_DataBusWidth = 64 ;        /* 64 bits */
+              XGINew_ChannelAB = 2 ;            /* 2 channels */
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0xA1 ) ;
+              XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x5A ) ;
+
+              if ( XGINew_ReadWriteRest( 25 , 24 , pVBInfo ) == 1 )
+                  return ;
+              else
+              {
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x13 , 0x21 ) ;
+                  XGINew_SetReg1( pVBInfo->P3c4 , 0x14 , 0x4A ) ;
+              }
+          }
+          break ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DDRSizing340 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int XGINew_DDRSizing340( PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo)
+{
+    int i ;
+    USHORT memsize , addr ;
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x15 , 0x00 ) ;     /* noninterleaving */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x1C , 0x00 ) ;     /* nontiling */
+    XGINew_CheckChannel( HwDeviceExtension, pVBInfo ) ;
+
+
+    if ( HwDeviceExtension->jChipType >= XG20 )
+    {
+      for( i = 0 ; i < 12 ; i++ )
+      {
+        XGINew_SetDRAMSizingType( i , XGINew_DDRDRAM_TYPE20, pVBInfo ) ;
+        memsize = XGINew_SetDRAMSize20Reg( i , XGINew_DDRDRAM_TYPE20, pVBInfo ) ;
+        if ( memsize == 0 )
+            continue ;
+
+        addr = memsize + ( XGINew_ChannelAB - 2 ) + 20 ;
+        if ( ( HwDeviceExtension->ulVideoMemorySize - 1 ) < ( ULONG )( 1 << addr ) )
+            continue ;
+
+        if ( XGINew_ReadWriteRest( addr , 5, pVBInfo ) == 1 )
+            return( 1 ) ;
+      }
+    }
+    else
+    {
+      for( i = 0 ; i < 4 ; i++ )
+      {
+        XGINew_SetDRAMSizingType( i , XGINew_DDRDRAM_TYPE340, pVBInfo ) ;
+        memsize = XGINew_SetDRAMSizeReg( i , XGINew_DDRDRAM_TYPE340, pVBInfo ) ;
+
+        if ( memsize == 0 )
+            continue ;
+
+        addr = memsize + ( XGINew_ChannelAB - 2 ) + 20 ;
+        if ( ( HwDeviceExtension->ulVideoMemorySize - 1 ) < ( ULONG )( 1 << addr ) )
+            continue ;
+
+        if ( XGINew_ReadWriteRest( addr , 9, pVBInfo ) == 1 )
+            return( 1 ) ;
+      }
+    }
+    return( 0 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DDRSizing */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int XGINew_DDRSizing(PVB_DEVICE_INFO pVBInfo)
+{
+    int    i ;
+    UCHAR  j ;
+
+    for( i = 0 ; i < 4 ; i++ )
+    {
+        XGINew_SetDRAMSizingType( i , XGINew_DDRDRAM_TYPE, pVBInfo ) ;
+        XGINew_DisableChannelInterleaving( i , XGINew_DDRDRAM_TYPE , pVBInfo) ;
+        for( j = 2 ; j > 0 ; j-- )
+        {
+            XGINew_SetDDRChannel( i , j , XGINew_ChannelAB , XGINew_DDRDRAM_TYPE , pVBInfo ) ;
+            if ( !XGINew_SetRank( i , ( UCHAR )j , XGINew_ChannelAB , XGINew_DDRDRAM_TYPE, pVBInfo ) )
+                continue ;
+            else
+            {
+                if ( XGINew_CheckDDRRanks( j , i , XGINew_DDRDRAM_TYPE,  pVBInfo ) )
+                return( 1 ) ;
+            }
+        }
+    }
+    return( 0 ) ;
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetMemoryClock */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SetMemoryClock( PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo)
+{
+
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x28 , pVBInfo->MCLKData[ XGINew_RAMType ].SR28 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x29 , pVBInfo->MCLKData[ XGINew_RAMType ].SR29 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2A , pVBInfo->MCLKData[ XGINew_RAMType ].SR2A ) ;
+
+
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2E , pVBInfo->ECLKData[ XGINew_RAMType ].SR2E ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2F , pVBInfo->ECLKData[ XGINew_RAMType ].SR2F ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x30 , pVBInfo->ECLKData[ XGINew_RAMType ].SR30 ) ;
+
+    /* [Vicent] 2004/07/07, When XG42 ECLK = MCLK = 207MHz, Set SR32 D[1:0] = 10b */
+    /* [Hsuan] 2004/08/20, Modify SR32 value, when MCLK=207MHZ, ELCK=250MHz, Set SR32 D[1:0] = 10b */
+    if ( HwDeviceExtension->jChipType == XG42 )
+    {
+      if ( ( pVBInfo->MCLKData[ XGINew_RAMType ].SR28 == 0x1C ) && ( pVBInfo->MCLKData[ XGINew_RAMType ].SR29 == 0x01 )
+        && ( ( ( pVBInfo->ECLKData[ XGINew_RAMType ].SR2E == 0x1C ) && ( pVBInfo->ECLKData[ XGINew_RAMType ].SR2F == 0x01 ) )
+        || ( ( pVBInfo->ECLKData[ XGINew_RAMType ].SR2E == 0x22 ) && ( pVBInfo->ECLKData[ XGINew_RAMType ].SR2F == 0x01 ) ) ) )
+      {
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x32 , ( ( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x32 ) & 0xFC ) | 0x02 ) ;
+      }
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : ChkLFB */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN ChkLFB( PVB_DEVICE_INFO pVBInfo )
+{
+    if ( LFBDRAMTrap & XGINew_GetReg1( pVBInfo->P3d4 , 0x78 ) )
+        return( TRUE ) ;
+    else
+        return( FALSE );
+}
+
+
+/* --------------------------------------------------------------------- */
+/* input : dx ,valid value : CR or second chip's CR */
+/*  */
+/* SetPowerConsume : */
+/* Description: reduce 40/43 power consumption in first chip or */
+/* in second chip, assume CR A1 D[6]="1" in this case */
+/* output : none */
+/* --------------------------------------------------------------------- */
+void SetPowerConsume ( PXGI_HW_DEVICE_INFO HwDeviceExtension , ULONG XGI_P3d4Port )
+{
+    ULONG   lTemp ;
+    UCHAR   bTemp;
+
+    HwDeviceExtension->pQueryVGAConfigSpace( HwDeviceExtension , 0x08 , 0 , &lTemp ) ; /* Get */
+    if ((lTemp&0xFF)==0)
+    {
+        /* set CR58 D[5]=0 D[3]=0 */
+        XGINew_SetRegAND( XGI_P3d4Port , 0x58 , 0xD7 ) ;
+        bTemp = (UCHAR) XGINew_GetReg1( XGI_P3d4Port , 0xCB ) ;
+        if (bTemp&0x20)
+        {
+            if (!(bTemp&0x10))
+            {
+                XGINew_SetRegANDOR( XGI_P3d4Port , 0x58 , 0xD7 , 0x20 ) ; /* CR58 D[5]=1 D[3]=0 */
+            }
+            else
+            {
+                XGINew_SetRegANDOR( XGI_P3d4Port , 0x58 , 0xD7 , 0x08 ) ; /* CR58 D[5]=0 D[3]=1 */
+            }
+
+        }
+
+    }
+}
+
+
+
+#if defined(LINUX_XF86)||defined(LINUX_KERNEL)
+void XGINew_InitVBIOSData(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo)
+{
+
+        /* ULONG ROMAddr = (ULONG)HwDeviceExtension->pjVirtualRomBase; */
+    pVBInfo->ROMAddr = HwDeviceExtension->pjVirtualRomBase ;
+    pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress ;
+    pVBInfo->BaseAddr = (ULONG)HwDeviceExtension->pjIOAddress ;
+    pVBInfo->ISXPDOS = 0 ;
+
+    pVBInfo->P3c4 = pVBInfo->BaseAddr + 0x14 ;
+    pVBInfo->P3d4 = pVBInfo->BaseAddr + 0x24 ;
+    pVBInfo->P3c0 = pVBInfo->BaseAddr + 0x10 ;
+    pVBInfo->P3ce = pVBInfo->BaseAddr + 0x1e ;
+    pVBInfo->P3c2 = pVBInfo->BaseAddr + 0x12 ;
+    pVBInfo->P3ca = pVBInfo->BaseAddr + 0x1a ;
+    pVBInfo->P3c6 = pVBInfo->BaseAddr + 0x16 ;
+    pVBInfo->P3c7 = pVBInfo->BaseAddr + 0x17 ;
+    pVBInfo->P3c8 = pVBInfo->BaseAddr + 0x18 ;
+    pVBInfo->P3c9 = pVBInfo->BaseAddr + 0x19 ;
+    pVBInfo->P3da = pVBInfo->BaseAddr + 0x2A ;
+    pVBInfo->Part0Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_00 ;
+    pVBInfo->Part1Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_04 ;
+    pVBInfo->Part2Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_10 ;
+    pVBInfo->Part3Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_12 ;
+    pVBInfo->Part4Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 ;
+    pVBInfo->Part5Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 + 2 ;
+    if ( HwDeviceExtension->jChipType < XG20 )                  /* kuku 2004/06/25 */
+    XGI_GetVBType( pVBInfo ) ;         /* Run XGI_GetVBType before InitTo330Pointer */
+
+        switch(HwDeviceExtension->jChipType)
+        {
+        case XG40:
+        case XG41:
+        case XG42:
+        case XG20:
+        case XG21:
+        default:
+                InitTo330Pointer(HwDeviceExtension->jChipType,pVBInfo);
+                return ;
+        }
+
+}
+#endif /* For Linux */
+
+/* --------------------------------------------------------------------- */
+/* Function : ReadVBIOSTablData */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void ReadVBIOSTablData( UCHAR ChipType , PVB_DEVICE_INFO pVBInfo)
+{
+    PUCHAR  volatile pVideoMemory = ( PUCHAR )pVBInfo->ROMAddr ;
+    ULONG   i ;
+    UCHAR   j , k ;
+#if 0
+    ULONG   ii , jj ;
+    i = pVideoMemory[ 0x1CF ] | ( pVideoMemory[ 0x1D0 ] << 8 ) ;                /* UniROM */
+    if ( i != 0 )
+        UNIROM = 1 ;
+
+    ii = 0x90 ;
+    for( jj = 0x00 ; jj < 0x08 ; jj++ )
+    {
+        pVBInfo->MCLKData[ jj ].SR28 = pVideoMemory[ ii ] ;
+        pVBInfo->MCLKData[ jj ].SR29 = pVideoMemory[ ii + 1] ;
+        pVBInfo->MCLKData[ jj ].SR2A = pVideoMemory[ ii + 2] ;
+        pVBInfo->MCLKData[ jj ].CLOCK = pVideoMemory[ ii + 3 ] | ( pVideoMemory[ ii + 4 ] << 8 ) ;
+        ii += 0x05 ;
+    }
+
+    ii = 0xB8 ;
+    for( jj = 0x00 ; jj < 0x08 ; jj++ )
+    {
+        pVBInfo->ECLKData[ jj ].SR2E = pVideoMemory[ ii ] ;
+        pVBInfo->ECLKData[ jj ].SR2F=pVideoMemory[ ii + 1 ] ;
+        pVBInfo->ECLKData[ jj ].SR30= pVideoMemory[ ii + 2 ] ;
+        pVBInfo->ECLKData[ jj ].CLOCK= pVideoMemory[ ii + 3 ] | ( pVideoMemory[ ii + 4 ] << 8 ) ;
+        ii += 0x05 ;
+    }
+
+    /* Volari customize data area start */
+    /* if ( ChipType == XG40 ) */
+    if ( ChipType >= XG40 )
+    {
+        ii = 0xE0 ;
+        for( jj = 0x00 ; jj < 0x03 ; jj++ )
+        {
+            pVBInfo->SR15[ jj ][ 0 ] = pVideoMemory[ ii ] ;             /* SR13, SR14, and SR18 */
+            pVBInfo->SR15[ jj ][ 1 ] = pVideoMemory[ ii + 1 ] ;
+            pVBInfo->SR15[ jj ][ 2 ] = pVideoMemory[ ii + 2 ] ;
+            pVBInfo->SR15[ jj ][ 3 ] = pVideoMemory[ ii + 3 ] ;
+            pVBInfo->SR15[ jj ][ 4 ] = pVideoMemory[ ii + 4 ] ;
+            pVBInfo->SR15[ jj ][ 5 ] = pVideoMemory[ ii + 5 ] ;
+            pVBInfo->SR15[ jj ][ 6 ] = pVideoMemory[ ii + 6 ] ;
+            pVBInfo->SR15[ jj ][ 7 ] = pVideoMemory[ ii + 7 ] ;
+            ii += 0x08 ;
+        }
+        ii = 0x110 ;
+        jj = 0x03 ;
+        pVBInfo->SR15[ jj ][ 0 ] = pVideoMemory[ ii ] ;         /* SR1B */
+        pVBInfo->SR15[ jj ][ 1 ] = pVideoMemory[ ii + 1 ] ;
+        pVBInfo->SR15[ jj ][ 2 ] = pVideoMemory[ ii + 2 ] ;
+        pVBInfo->SR15[ jj ][ 3 ] = pVideoMemory[ ii + 3 ] ;
+        pVBInfo->SR15[ jj ][ 4 ] = pVideoMemory[ ii + 4 ] ;
+        pVBInfo->SR15[ jj ][ 5 ] = pVideoMemory[ ii + 5 ] ;
+        pVBInfo->SR15[ jj ][ 6 ] = pVideoMemory[ ii + 6 ] ;
+        pVBInfo->SR15[ jj ][ 7 ] = pVideoMemory[ ii + 7 ] ;
+
+        *pVBInfo->pSR07 = pVideoMemory[ 0x74 ] ;
+        *pVBInfo->pSR1F = pVideoMemory[ 0x75 ] ;
+        *pVBInfo->pSR21 = pVideoMemory[ 0x76 ] ;
+        *pVBInfo->pSR22 = pVideoMemory[ 0x77 ] ;
+        *pVBInfo->pSR23 = pVideoMemory[ 0x78 ] ;
+        *pVBInfo->pSR24 = pVideoMemory[ 0x79 ] ;
+        pVBInfo->SR25[ 0 ] = pVideoMemory[ 0x7A ] ;
+        *pVBInfo->pSR31 = pVideoMemory[ 0x7B ] ;
+        *pVBInfo->pSR32 = pVideoMemory[ 0x7C ] ;
+        *pVBInfo->pSR33 = pVideoMemory[ 0x7D ] ;
+        ii = 0xF8 ;
+
+        for( jj = 0 ; jj < 3 ; jj++ )
+        {
+            pVBInfo->CR40[ jj ][ 0 ] = pVideoMemory[ ii ] ;
+            pVBInfo->CR40[ jj ][ 1 ] = pVideoMemory[ ii + 1 ] ;
+            pVBInfo->CR40[ jj ][ 2 ] = pVideoMemory[ ii + 2 ] ;
+            pVBInfo->CR40[ jj ][ 3 ] = pVideoMemory[ ii + 3 ] ;
+            pVBInfo->CR40[ jj ][ 4 ] = pVideoMemory[ ii + 4 ] ;
+            pVBInfo->CR40[ jj ][ 5 ] = pVideoMemory[ ii + 5 ] ;
+            pVBInfo->CR40[ jj ][ 6 ] = pVideoMemory[ ii + 6 ] ;
+            pVBInfo->CR40[ jj ][ 7 ] = pVideoMemory[ ii + 7 ] ;
+            ii += 0x08 ;
+        }
+
+        ii = 0x118 ;
+        for( j = 3 ; j < 24 ; j++ )
+        {
+            pVBInfo->CR40[ j ][ 0 ] = pVideoMemory[ ii ] ;
+            pVBInfo->CR40[ j ][ 1 ] = pVideoMemory[ ii + 1 ] ;
+            pVBInfo->CR40[ j ][ 2 ] = pVideoMemory[ ii + 2 ] ;
+            pVBInfo->CR40[ j ][ 3 ] = pVideoMemory[ ii + 3 ] ;
+            pVBInfo->CR40[ j ][ 4 ] = pVideoMemory[ ii + 4 ] ;
+            pVBInfo->CR40[ j ][ 5 ] = pVideoMemory[ ii + 5 ] ;
+            pVBInfo->CR40[ j ][ 6 ] = pVideoMemory[ ii + 6 ] ;
+            pVBInfo->CR40[ j ][ 7 ] = pVideoMemory[ ii + 7 ] ;
+            ii += 0x08 ;
+        }
+
+        i = pVideoMemory[ 0x1C0 ] | ( pVideoMemory[ 0x1C1 ] << 8 ) ;
+
+        for( j = 0 ; j < 8 ; j++ )
+        {
+            for( k = 0 ; k < 4 ; k++ )
+                pVBInfo->CR6B[ j ][ k ] = pVideoMemory[ i + 4 * j + k ] ;
+        }
+
+        i = pVideoMemory[ 0x1C2 ] | ( pVideoMemory[ 0x1C3 ] << 8 ) ;
+
+        for( j = 0 ; j < 8 ; j++ )
+        {
+            for( k = 0 ; k < 4 ; k++ )
+                pVBInfo->CR6E[ j ][ k ] = pVideoMemory[ i + 4 * j + k ] ;
+        }
+
+        i = pVideoMemory[ 0x1C4 ] | ( pVideoMemory[ 0x1C5 ] << 8 ) ;
+        for( j = 0 ; j < 8 ; j++ )
+        {
+            for( k = 0 ; k < 32 ; k++ )
+                pVBInfo->CR6F[ j ][ k ] = pVideoMemory[ i + 32 * j + k ] ;
+        }
+
+        i = pVideoMemory[ 0x1C6 ] | ( pVideoMemory[ 0x1C7 ] << 8 ) ;
+
+        for( j = 0 ; j < 8 ; j++ )
+        {
+            for( k = 0 ; k < 2 ; k++ )
+                pVBInfo->CR89[ j ][ k ] = pVideoMemory[ i + 2 * j + k ] ;
+        }
+
+        i = pVideoMemory[ 0x1C8 ] | ( pVideoMemory[ 0x1C9 ] << 8 ) ;
+        for( j = 0 ; j < 12 ; j++ )
+            pVBInfo->AGPReg[ j ] = pVideoMemory[ i + j ] ;
+
+        i = pVideoMemory[ 0x1CF ] | ( pVideoMemory[ 0x1D0 ] << 8 ) ;
+        for( j = 0 ; j < 4 ; j++ )
+            pVBInfo->SR16[ j ] = pVideoMemory[ i + j ] ;
+
+        if ( ChipType == XG21 )
+        {
+            if (pVideoMemory[ 0x67 ] & 0x80)
+            {
+                *pVBInfo->pDVOSetting = pVideoMemory[ 0x67 ];
+            }
+            if ( (pVideoMemory[ 0x67 ] & 0xC0) == 0xC0 )
+            {
+                *pVBInfo->pCR2E = pVideoMemory[ i + 4 ] ;
+                *pVBInfo->pCR2F = pVideoMemory[ i + 5 ] ;
+                *pVBInfo->pCR46 = pVideoMemory[ i + 6 ] ;
+                *pVBInfo->pCR47 = pVideoMemory[ i + 7 ] ;
+            }
+        }
+
+        if ( ChipType == XG27 )
+        {
+            jj = i+j;
+            for( i = 0 ; i <= 0xB ; i++,jj++ )
+              pVBInfo->pCRD0[i] = pVideoMemory[ jj ] ;
+            for( i = 0x0 ; i <= 0x1 ; i++,jj++ )
+              pVBInfo->pCRDE[i] = pVideoMemory[ jj ] ;
+
+            *pVBInfo->pSR40 = pVideoMemory[ jj ] ;
+            jj++;
+            *pVBInfo->pSR41 = pVideoMemory[ jj ] ;
+
+            if (pVideoMemory[ 0x67 ] & 0x80)
+            {
+                *pVBInfo->pDVOSetting = pVideoMemory[ 0x67 ];
+            }
+            if ( (pVideoMemory[ 0x67 ] & 0xC0) == 0xC0 )
+            {
+                jj++;
+                *pVBInfo->pCR2E = pVideoMemory[ jj ] ;
+                *pVBInfo->pCR2F = pVideoMemory[ jj + 1 ] ;
+                *pVBInfo->pCR46 = pVideoMemory[ jj + 2 ] ;
+                *pVBInfo->pCR47 = pVideoMemory[ jj + 3 ] ;
+            }
+
+        }
+
+        *pVBInfo->pCRCF = pVideoMemory[ 0x1CA ] ;
+        *pVBInfo->pXGINew_DRAMTypeDefinition = pVideoMemory[ 0x1CB ] ;
+        *pVBInfo->pXGINew_I2CDefinition = pVideoMemory[ 0x1D1 ] ;
+        if ( ChipType >= XG20 )
+        {
+           *pVBInfo->pXGINew_CR97 = pVideoMemory[ 0x1D2 ] ;
+           if ( ChipType == XG27 )
+           {
+             *pVBInfo->pSR36 = pVideoMemory[ 0x1D3 ] ;
+             *pVBInfo->pCR8F = pVideoMemory[ 0x1D5 ] ;
+           }
+        }
+
+    }
+#endif
+    /* Volari customize data area end */
+
+    if ( ChipType == XG21 )
+    {
+        pVBInfo->IF_DEF_LVDS = 0 ;
+        if (pVideoMemory[ 0x65 ] & 0x1)
+        {
+            pVBInfo->IF_DEF_LVDS = 1 ;
+            i = pVideoMemory[ 0x316 ] | ( pVideoMemory[ 0x317 ] << 8 );
+            j = pVideoMemory[ i-1 ] ;
+            if ( j != 0xff )
+            {
+              k = 0;
+              do
+              {
+                pVBInfo->XG21_LVDSCapList[k].LVDS_Capability = pVideoMemory[ i ] | ( pVideoMemory[ i + 1 ] << 8 );
+                pVBInfo->XG21_LVDSCapList[k].LVDSHT = pVideoMemory[ i + 2 ] | ( pVideoMemory[ i + 3 ] << 8 ) ;
+                pVBInfo->XG21_LVDSCapList[k].LVDSVT = pVideoMemory[ i + 4 ] | ( pVideoMemory[ i + 5 ] << 8 );
+                pVBInfo->XG21_LVDSCapList[k].LVDSHDE = pVideoMemory[ i + 6 ] | ( pVideoMemory[ i + 7 ] << 8 );
+                pVBInfo->XG21_LVDSCapList[k].LVDSVDE = pVideoMemory[ i + 8 ] | ( pVideoMemory[ i + 9 ] << 8 );
+                pVBInfo->XG21_LVDSCapList[k].LVDSHFP = pVideoMemory[ i + 10 ] | ( pVideoMemory[ i + 11 ] << 8 );
+                pVBInfo->XG21_LVDSCapList[k].LVDSVFP = pVideoMemory[ i + 12 ] | ( pVideoMemory[ i + 13 ] << 8 );
+                pVBInfo->XG21_LVDSCapList[k].LVDSHSYNC = pVideoMemory[ i + 14 ] | ( pVideoMemory[ i + 15 ] << 8 );
+                pVBInfo->XG21_LVDSCapList[k].LVDSVSYNC = pVideoMemory[ i + 16 ] | ( pVideoMemory[ i + 17 ] << 8 );
+                pVBInfo->XG21_LVDSCapList[k].VCLKData1 = pVideoMemory[ i + 18 ] ;
+                pVBInfo->XG21_LVDSCapList[k].VCLKData2 = pVideoMemory[ i + 19 ] ;
+                pVBInfo->XG21_LVDSCapList[k].PSC_S1 = pVideoMemory[ i + 20 ] ;
+                pVBInfo->XG21_LVDSCapList[k].PSC_S2 = pVideoMemory[ i + 21 ] ;
+                pVBInfo->XG21_LVDSCapList[k].PSC_S3 = pVideoMemory[ i + 22 ] ;
+                pVBInfo->XG21_LVDSCapList[k].PSC_S4 = pVideoMemory[ i + 23 ] ;
+                pVBInfo->XG21_LVDSCapList[k].PSC_S5 = pVideoMemory[ i + 24 ] ;
+                i += 25;
+                j--;
+                k++;
+              } while ( (j>0) && ( k < (sizeof(XGI21_LCDCapList)/sizeof(XGI21_LVDSCapStruct)) ) );
+            }
+            else
+            {
+            pVBInfo->XG21_LVDSCapList[0].LVDS_Capability = pVideoMemory[ i ] | ( pVideoMemory[ i + 1 ] << 8 );
+            pVBInfo->XG21_LVDSCapList[0].LVDSHT = pVideoMemory[ i + 2 ] | ( pVideoMemory[ i + 3 ] << 8 ) ;
+            pVBInfo->XG21_LVDSCapList[0].LVDSVT = pVideoMemory[ i + 4 ] | ( pVideoMemory[ i + 5 ] << 8 );
+            pVBInfo->XG21_LVDSCapList[0].LVDSHDE = pVideoMemory[ i + 6 ] | ( pVideoMemory[ i + 7 ] << 8 );
+            pVBInfo->XG21_LVDSCapList[0].LVDSVDE = pVideoMemory[ i + 8 ] | ( pVideoMemory[ i + 9 ] << 8 );
+            pVBInfo->XG21_LVDSCapList[0].LVDSHFP = pVideoMemory[ i + 10 ] | ( pVideoMemory[ i + 11 ] << 8 );
+            pVBInfo->XG21_LVDSCapList[0].LVDSVFP = pVideoMemory[ i + 12 ] | ( pVideoMemory[ i + 13 ] << 8 );
+            pVBInfo->XG21_LVDSCapList[0].LVDSHSYNC = pVideoMemory[ i + 14 ] | ( pVideoMemory[ i + 15 ] << 8 );
+            pVBInfo->XG21_LVDSCapList[0].LVDSVSYNC = pVideoMemory[ i + 16 ] | ( pVideoMemory[ i + 17 ] << 8 );
+            pVBInfo->XG21_LVDSCapList[0].VCLKData1 = pVideoMemory[ i + 18 ] ;
+            pVBInfo->XG21_LVDSCapList[0].VCLKData2 = pVideoMemory[ i + 19 ] ;
+            pVBInfo->XG21_LVDSCapList[0].PSC_S1 = pVideoMemory[ i + 20 ] ;
+            pVBInfo->XG21_LVDSCapList[0].PSC_S2 = pVideoMemory[ i + 21 ] ;
+            pVBInfo->XG21_LVDSCapList[0].PSC_S3 = pVideoMemory[ i + 22 ] ;
+            pVBInfo->XG21_LVDSCapList[0].PSC_S4 = pVideoMemory[ i + 23 ] ;
+            pVBInfo->XG21_LVDSCapList[0].PSC_S5 = pVideoMemory[ i + 24 ] ;
+        }
+        }
+    }
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_DDR1x_MRS_XG20 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_DDR1x_MRS_XG20( ULONG P3c4 , PVB_DEVICE_INFO pVBInfo)
+{
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x01 ) ;
+    XGINew_SetReg1( P3c4 , 0x19 , 0x40 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x80 ) ;
+    DelayUS( 60 ) ;
+
+    XGINew_SetReg1( P3c4 , 0x18 , 0x00 ) ;
+    XGINew_SetReg1( P3c4 , 0x19 , 0x40 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x00 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x80 ) ;
+    DelayUS( 60 ) ;
+    XGINew_SetReg1( P3c4 , 0x18 , pVBInfo->SR15[ 2 ][ XGINew_RAMType ] ) ;      /* SR18 */
+    /* XGINew_SetReg1( P3c4 , 0x18 , 0x31 ) ; */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x01 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x03 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x83 ) ;
+    DelayUS( 1000 ) ;
+    XGINew_SetReg1( P3c4 , 0x1B , 0x03 ) ;
+    DelayUS( 500 ) ;
+    /* XGINew_SetReg1( P3c4 , 0x18 , 0x31 ) ; */
+    XGINew_SetReg1( P3c4 , 0x18 , pVBInfo->SR15[ 2 ][ XGINew_RAMType ] ) ;      /* SR18 */
+    XGINew_SetReg1( P3c4 , 0x19 , 0x00 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x03 ) ;
+    XGINew_SetReg1( P3c4 , 0x16 , 0x83 ) ;
+    XGINew_SetReg1( P3c4 , 0x1B , 0x00 ) ;
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetDRAMModeRegister_XG20 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SetDRAMModeRegister_XG20( PXGI_HW_DEVICE_INFO HwDeviceExtension )
+{
+    VB_DEVICE_INFO VBINF;
+    PVB_DEVICE_INFO pVBInfo = &VBINF;
+    pVBInfo->ROMAddr = HwDeviceExtension->pjVirtualRomBase ;
+    pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress ;
+    pVBInfo->BaseAddr = (ULONG)HwDeviceExtension->pjIOAddress ;
+    pVBInfo->ISXPDOS = 0 ;
+
+    pVBInfo->P3c4 = pVBInfo->BaseAddr + 0x14 ;
+    pVBInfo->P3d4 = pVBInfo->BaseAddr + 0x24 ;
+    pVBInfo->P3c0 = pVBInfo->BaseAddr + 0x10 ;
+    pVBInfo->P3ce = pVBInfo->BaseAddr + 0x1e ;
+    pVBInfo->P3c2 = pVBInfo->BaseAddr + 0x12 ;
+    pVBInfo->P3ca = pVBInfo->BaseAddr + 0x1a ;
+    pVBInfo->P3c6 = pVBInfo->BaseAddr + 0x16 ;
+    pVBInfo->P3c7 = pVBInfo->BaseAddr + 0x17 ;
+    pVBInfo->P3c8 = pVBInfo->BaseAddr + 0x18 ;
+    pVBInfo->P3c9 = pVBInfo->BaseAddr + 0x19 ;
+    pVBInfo->P3da = pVBInfo->BaseAddr + 0x2A ;
+    pVBInfo->Part0Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_00 ;
+    pVBInfo->Part1Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_04 ;
+    pVBInfo->Part2Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_10 ;
+    pVBInfo->Part3Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_12 ;
+    pVBInfo->Part4Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 ;
+    pVBInfo->Part5Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 + 2 ;
+
+    InitTo330Pointer(HwDeviceExtension->jChipType,pVBInfo);
+
+    ReadVBIOSTablData( HwDeviceExtension->jChipType , pVBInfo) ;
+
+    if ( XGINew_GetXG20DRAMType( HwDeviceExtension, pVBInfo) == 0 )
+        XGINew_DDR1x_MRS_XG20( pVBInfo->P3c4, pVBInfo ) ;
+    else
+        XGINew_DDR2_MRS_XG20( HwDeviceExtension , pVBInfo->P3c4 , pVBInfo ) ;
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x1B , 0x03 ) ;
+}
+
+void XGINew_SetDRAMModeRegister_XG27( PXGI_HW_DEVICE_INFO HwDeviceExtension )
+{
+    VB_DEVICE_INFO VBINF;
+    PVB_DEVICE_INFO pVBInfo = &VBINF;
+    pVBInfo->ROMAddr = HwDeviceExtension->pjVirtualRomBase ;
+    pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress ;
+    pVBInfo->BaseAddr = (ULONG)HwDeviceExtension->pjIOAddress ;
+    pVBInfo->ISXPDOS = 0 ;
+
+    pVBInfo->P3c4 = pVBInfo->BaseAddr + 0x14 ;
+    pVBInfo->P3d4 = pVBInfo->BaseAddr + 0x24 ;
+    pVBInfo->P3c0 = pVBInfo->BaseAddr + 0x10 ;
+    pVBInfo->P3ce = pVBInfo->BaseAddr + 0x1e ;
+    pVBInfo->P3c2 = pVBInfo->BaseAddr + 0x12 ;
+    pVBInfo->P3ca = pVBInfo->BaseAddr + 0x1a ;
+    pVBInfo->P3c6 = pVBInfo->BaseAddr + 0x16 ;
+    pVBInfo->P3c7 = pVBInfo->BaseAddr + 0x17 ;
+    pVBInfo->P3c8 = pVBInfo->BaseAddr + 0x18 ;
+    pVBInfo->P3c9 = pVBInfo->BaseAddr + 0x19 ;
+    pVBInfo->P3da = pVBInfo->BaseAddr + 0x2A ;
+    pVBInfo->Part0Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_00 ;
+    pVBInfo->Part1Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_04 ;
+    pVBInfo->Part2Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_10 ;
+    pVBInfo->Part3Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_12 ;
+    pVBInfo->Part4Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 ;
+    pVBInfo->Part5Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 + 2 ;
+
+    InitTo330Pointer(HwDeviceExtension->jChipType,pVBInfo);
+
+    ReadVBIOSTablData( HwDeviceExtension->jChipType , pVBInfo) ;
+
+    if ( XGINew_GetXG20DRAMType( HwDeviceExtension, pVBInfo) == 0 )
+        XGINew_DDR1x_MRS_XG20( pVBInfo->P3c4, pVBInfo ) ;
+    else
+        //XGINew_DDR2_MRS_XG27( HwDeviceExtension , pVBInfo->P3c4 , pVBInfo ) ;
+        XGINew_DDRII_Bootup_XG27( HwDeviceExtension , pVBInfo->P3c4 , pVBInfo) ;
+
+    //XGINew_SetReg1( pVBInfo->P3c4 , 0x1B , 0x03 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x1B , pVBInfo->SR15[ 3 ][ XGINew_RAMType ] ) ;     /* SR1B */
+
+}
+/*
+void XGINew_SetDRAMModeRegister_XG27( PXGI_HW_DEVICE_INFO HwDeviceExtension )
+{
+#ifndef LINUX_XF86
+    UCHAR data ;
+#endif
+    VB_DEVICE_INFO VBINF;
+    PVB_DEVICE_INFO pVBInfo = &VBINF;
+    pVBInfo->ROMAddr = HwDeviceExtension->pjVirtualRomBase ;
+    pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress ;
+    pVBInfo->BaseAddr = HwDeviceExtension->pjIOAddress ;
+    pVBInfo->ISXPDOS = 0 ;
+
+    pVBInfo->P3c4 = pVBInfo->BaseAddr + 0x14 ;
+    pVBInfo->P3d4 = pVBInfo->BaseAddr + 0x24 ;
+    pVBInfo->P3c0 = pVBInfo->BaseAddr + 0x10 ;
+    pVBInfo->P3ce = pVBInfo->BaseAddr + 0x1e ;
+    pVBInfo->P3c2 = pVBInfo->BaseAddr + 0x12 ;
+    pVBInfo->P3ca = pVBInfo->BaseAddr + 0x1a ;
+    pVBInfo->P3c6 = pVBInfo->BaseAddr + 0x16 ;
+    pVBInfo->P3c7 = pVBInfo->BaseAddr + 0x17 ;
+    pVBInfo->P3c8 = pVBInfo->BaseAddr + 0x18 ;
+    pVBInfo->P3c9 = pVBInfo->BaseAddr + 0x19 ;
+    pVBInfo->P3da = pVBInfo->BaseAddr + 0x2A ;
+    pVBInfo->Part0Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_00 ;
+    pVBInfo->Part1Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_04 ;
+    pVBInfo->Part2Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_10 ;
+    pVBInfo->Part3Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_12 ;
+    pVBInfo->Part4Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 ;
+    pVBInfo->Part5Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 + 2 ;
+
+    InitTo330Pointer(HwDeviceExtension->jChipType,pVBInfo);
+
+    ReadVBIOSTablData( HwDeviceExtension->jChipType , pVBInfo) ;
+
+    if ( XGINew_GetXG20DRAMType( HwDeviceExtension, pVBInfo) == 0 )
+        XGINew_DDR1x_MRS_XG20( pVBInfo->P3c4, pVBInfo ) ;
+    else
+        XGINew_DDR2_MRS_XG27( HwDeviceExtension , pVBInfo->P3c4 , pVBInfo ) ;
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x1B , 0x03 ) ;
+}
+*/
+/* -------------------------------------------------------- */
+/* Function : XGINew_ChkSenseStatus */
+/* Input : */
+/* Output : */
+/* Description : */
+/* -------------------------------------------------------- */
+void XGINew_ChkSenseStatus ( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempbx=0 , temp , tempcx , CR3CData;
+
+    temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x32 ) ;
+
+    if ( temp & Monitor1Sense )
+        tempbx |= ActiveCRT1 ;
+    if ( temp & LCDSense )
+        tempbx |= ActiveLCD ;
+    if ( temp & Monitor2Sense )
+        tempbx |= ActiveCRT2 ;
+    if ( temp & TVSense )
+    {
+        tempbx |= ActiveTV ;
+        if ( temp & AVIDEOSense )
+            tempbx |= ( ActiveAVideo << 8 );
+        if ( temp & SVIDEOSense )
+            tempbx |= ( ActiveSVideo << 8 );
+        if ( temp & SCARTSense )
+            tempbx |= ( ActiveSCART << 8 );
+        if ( temp & HiTVSense )
+            tempbx |= ( ActiveHiTV << 8 );
+        if ( temp & YPbPrSense )
+            tempbx |= ( ActiveYPbPr << 8 );
+    }
+
+    tempcx = XGINew_GetReg1( pVBInfo->P3d4 , 0x3d ) ;
+    tempcx |= ( XGINew_GetReg1( pVBInfo->P3d4 , 0x3e ) << 8 ) ;
+
+    if ( tempbx & tempcx )
+    {
+        CR3CData = XGINew_GetReg1( pVBInfo->P3d4 , 0x3c ) ;
+        if ( !( CR3CData & DisplayDeviceFromCMOS ) )
+        {
+            tempcx = 0x1FF0 ;
+            if ( *pVBInfo->pSoftSetting & ModeSoftSetting )
+            {
+                tempbx = 0x1FF0 ;
+            }
+        }
+    }
+    else
+    {
+        tempcx = 0x1FF0 ;
+        if ( *pVBInfo->pSoftSetting & ModeSoftSetting )
+        {
+            tempbx = 0x1FF0 ;
+        }
+    }
+
+    tempbx &= tempcx ;
+    XGINew_SetReg1( pVBInfo->P3d4, 0x3d , ( tempbx & 0x00FF ) ) ;
+    XGINew_SetReg1( pVBInfo->P3d4, 0x3e , ( ( tempbx & 0xFF00 ) >> 8 )) ;
+}
+/* -------------------------------------------------------- */
+/* Function : XGINew_SetModeScratch */
+/* Input : */
+/* Output : */
+/* Description : */
+/* -------------------------------------------------------- */
+void XGINew_SetModeScratch ( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT temp , tempcl = 0 , tempch = 0 , CR31Data , CR38Data;
+
+    temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x3d ) ;
+    temp |= XGINew_GetReg1( pVBInfo->P3d4 , 0x3e ) << 8 ;
+    temp |= ( XGINew_GetReg1( pVBInfo->P3d4 , 0x31 ) & ( DriverMode >> 8) ) << 8 ;
+
+    if ( pVBInfo->IF_DEF_CRT2Monitor == 1)
+    {
+        if ( temp & ActiveCRT2 )
+           tempcl = SetCRT2ToRAMDAC ;
+    }
+
+    if ( temp & ActiveLCD )
+    {
+        tempcl |= SetCRT2ToLCD ;
+        if  ( temp & DriverMode )
+        {
+            if ( temp & ActiveTV )
+            {
+                tempch = SetToLCDA | EnableDualEdge ;
+                temp ^= SetCRT2ToLCD ;
+
+                if ( ( temp >> 8 ) & ActiveAVideo )
+                    tempcl |= SetCRT2ToAVIDEO ;
+                if ( ( temp >> 8 ) & ActiveSVideo )
+                    tempcl |= SetCRT2ToSVIDEO ;
+                if ( ( temp >> 8 ) & ActiveSCART )
+                    tempcl |= SetCRT2ToSCART ;
+
+                if ( pVBInfo->IF_DEF_HiVision == 1 )
+                {
+                    if ( ( temp >> 8 ) & ActiveHiTV )
+                    tempcl |= SetCRT2ToHiVisionTV ;
+                }
+
+                if ( pVBInfo->IF_DEF_YPbPr == 1 )
+                {
+                    if ( ( temp >> 8 ) & ActiveYPbPr )
+                    tempch |= SetYPbPr ;
+                }
+            }
+        }
+    }
+    else
+    {
+        if ( ( temp >> 8 ) & ActiveAVideo )
+           tempcl |= SetCRT2ToAVIDEO ;
+        if ( ( temp >> 8 ) & ActiveSVideo )
+           tempcl |= SetCRT2ToSVIDEO ;
+        if ( ( temp >> 8 ) & ActiveSCART )
+           tempcl |= SetCRT2ToSCART ;
+
+        if ( pVBInfo->IF_DEF_HiVision == 1 )
+        {
+           if ( ( temp >> 8 ) & ActiveHiTV )
+           tempcl |= SetCRT2ToHiVisionTV ;
+        }
+
+        if ( pVBInfo->IF_DEF_YPbPr == 1 )
+        {
+           if ( ( temp >> 8 ) & ActiveYPbPr )
+           tempch |= SetYPbPr ;
+        }
+    }
+
+
+    tempcl |= SetSimuScanMode ;
+    if ( (!( temp & ActiveCRT1 )) && ( ( temp & ActiveLCD ) || ( temp & ActiveTV ) || ( temp & ActiveCRT2 ) ) )
+       tempcl ^= ( SetSimuScanMode | SwitchToCRT2 ) ;
+    if ( ( temp & ActiveLCD ) && ( temp & ActiveTV ) )
+       tempcl ^= ( SetSimuScanMode | SwitchToCRT2 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4, 0x30 , tempcl ) ;
+
+    CR31Data = XGINew_GetReg1( pVBInfo->P3d4 , 0x31 ) ;
+    CR31Data &= ~( SetNotSimuMode >> 8 ) ;
+    if ( !( temp & ActiveCRT1 ) )
+        CR31Data |= ( SetNotSimuMode >> 8 ) ;
+    CR31Data &= ~( DisableCRT2Display >> 8 ) ;
+    if  (!( ( temp & ActiveLCD ) || ( temp & ActiveTV ) || ( temp & ActiveCRT2 ) ) )
+        CR31Data |= ( DisableCRT2Display >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4, 0x31 , CR31Data ) ;
+
+    CR38Data = XGINew_GetReg1( pVBInfo->P3d4 , 0x38 ) ;
+    CR38Data &= ~SetYPbPr ;
+    CR38Data |= tempch ;
+    XGINew_SetReg1( pVBInfo->P3d4, 0x38 , CR38Data ) ;
+
+}
+
+/* -------------------------------------------------------- */
+/* Function : XGINew_GetXG21Sense */
+/* Input : */
+/* Output : */
+/* Description : */
+/* -------------------------------------------------------- */
+void XGINew_GetXG21Sense(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR Temp;
+    PUCHAR  volatile pVideoMemory = ( PUCHAR )pVBInfo->ROMAddr ;
+
+    pVBInfo->IF_DEF_LVDS = 0 ;
+
+#ifdef WIN2000
+   pVBInfo->IF_DEF_CH7007 = 0 ;
+    if ( ( pVideoMemory[ 0x65 ] & 0x02 ) )                      /* For XG21 CH7007 */
+    {
+        /* VideoDebugPrint((0, "ReadVBIOSTablData: pVideoMemory[ 0x65 ] =%x\n",pVideoMemory[ 0x65 ])); */
+        pVBInfo->IF_DEF_CH7007 = 1 ;                            /* [Billy] 07/05/03 */
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x38 , ~0xE0 , 0x60 ) ; /* CH7007 on chip */
+    }
+    else
+#endif
+#if 1
+    if (( pVideoMemory[ 0x65 ] & 0x01 ) )                       /* For XG21 LVDS */
+    {
+        pVBInfo->IF_DEF_LVDS = 1 ;
+        XGINew_SetRegOR( pVBInfo->P3d4 , 0x32 , LCDSense ) ;
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x38 , ~0xE0 , 0xC0 ) ; /* LVDS on chip */
+    }
+    else
+    {
+#endif
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x4A , ~0x03 , 0x03 ) ; /* Enable GPIOA/B read  */
+        Temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x48 ) & 0xC0;
+        if ( Temp == 0xC0 )
+        {                                                               /* DVI & DVO GPIOA/B pull high */
+          XGINew_SenseLCD( HwDeviceExtension, pVBInfo ) ;
+          XGINew_SetRegOR( pVBInfo->P3d4 , 0x32 , LCDSense ) ;
+          XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x4A , ~0x20 , 0x20 ) ;   /* Enable read GPIOF */
+          Temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x48 ) & 0x04 ;
+          if ( !Temp )
+            XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x38 , ~0xE0 , 0x80 ) ; /* TMDS on chip */
+          else
+            XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x38 , ~0xE0 , 0xA0 ) ; /* Only DVO on chip */
+          XGINew_SetRegAND( pVBInfo->P3d4 , 0x4A , ~0x20 ) ;        /* Disable read GPIOF */
+        }
+#if 1
+    }
+#endif
+}
+
+/* -------------------------------------------------------- */
+/* Function : XGINew_GetXG27Sense */
+/* Input : */
+/* Output : */
+/* Description : */
+/* -------------------------------------------------------- */
+void XGINew_GetXG27Sense(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR Temp,bCR4A;
+
+     pVBInfo->IF_DEF_LVDS = 0 ;
+     bCR4A = XGINew_GetReg1( pVBInfo->P3d4 , 0x4A ) ;
+     XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x4A , ~0x07 , 0x07 ) ; /* Enable GPIOA/B/C read  */
+     Temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x48 ) & 0x07;
+     XGINew_SetReg1( pVBInfo->P3d4, 0x4A , bCR4A ) ;
+
+     if ( Temp <= 0x02 )
+     {
+         pVBInfo->IF_DEF_LVDS = 1 ;
+         XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x38 , ~0xE0 , 0xC0 ) ; /* LVDS setting */
+         XGINew_SetReg1( pVBInfo->P3d4, 0x30 , 0x21 ) ;
+     }
+     else
+     {
+       XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x38 , ~0xE0 , 0xA0 ) ; /* TMDS/DVO setting */
+     }
+     XGINew_SetRegOR( pVBInfo->P3d4 , 0x32 , LCDSense ) ;
+
+}
+
+UCHAR GetXG21FPBits(PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR CR38,CR4A,temp;
+
+    CR4A = XGINew_GetReg1( pVBInfo->P3d4 , 0x4A ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x4A , ~0x10 , 0x10 ) ; /* enable GPIOE read */
+    CR38 = XGINew_GetReg1( pVBInfo->P3d4 , 0x38 ) ;
+    temp =0;
+    if ( ( CR38 & 0xE0 ) > 0x80 )
+    {
+        temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x48 ) ;
+        temp &= 0x08;
+        temp >>= 3;
+    }
+
+    XGINew_SetReg1( pVBInfo->P3d4, 0x4A , CR4A ) ;
+
+    return temp;
+}
+
+UCHAR GetXG27FPBits(PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR CR4A,temp;
+
+    CR4A = XGINew_GetReg1( pVBInfo->P3d4 , 0x4A ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x4A , ~0x03 , 0x03 ) ; /* enable GPIOA/B/C read */
+    temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x48 ) ;
+    if ( temp <= 2 )
+    {
+        temp &= 0x03;
+    }
+    else
+    {
+        temp = ((temp&0x04)>>1) || ((~temp)&0x01);
+    }
+    XGINew_SetReg1( pVBInfo->P3d4, 0x4A , CR4A ) ;
+
+    return temp;
+}
+
diff --git a/drivers/staging/xgifb/vb_init.h b/drivers/staging/xgifb/vb_init.h
new file mode 100644
index 000000000000..1f39d9c74cdd
--- /dev/null
+++ b/drivers/staging/xgifb/vb_init.h
@@ -0,0 +1,7 @@
+#ifndef  _VBINIT_
+#define  _VBINIT_
+extern   BOOLEAN    XGIInitNew( PXGI_HW_DEVICE_INFO HwDeviceExtension ) ;
+extern XGI21_LVDSCapStruct  XGI21_LCDCapList[13];
+
+#endif
+
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
new file mode 100644
index 000000000000..bd7f73898644
--- /dev/null
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -0,0 +1,10736 @@
+#include "osdef.h"
+
+#ifdef TC
+#include <stdio.h>
+#include <string.h>
+#include <conio.h>
+#include <dos.h>
+#endif
+
+
+#ifdef LINUX_XF86
+#include "xf86.h"
+#include "xf86PciInfo.h"
+#include "xgi.h"
+#include "xgi_regs.h"
+#endif
+
+#ifdef LINUX_KERNEL
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/version.h>
+#include "XGIfb.h"
+/*#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#include <video/XGIfb.h>
+#else
+#include <linux/XGIfb.h>
+#endif*/
+#endif
+
+#ifdef WIN2000
+#include <dderror.h>
+#include <devioctl.h>
+#include <miniport.h>
+#include <ntddvdeo.h>
+#include <video.h>
+
+#include "xgiv.h"
+#include "dd_i2c.h"
+#include "tools.h"
+#endif
+
+#include "vb_def.h"
+#include "vgatypes.h"
+#include "vb_struct.h"
+#include "vb_util.h"
+#include "vb_table.h"
+
+
+
+#define  IndexMask 0xff
+#ifndef XGI_MASK_DUAL_CHIP
+#define XGI_MASK_DUAL_CHIP        0x04  /* SR3A */
+#endif
+
+
+
+BOOLEAN  XGI_IsLCDDualLink(PVB_DEVICE_INFO pVBInfo);
+BOOLEAN  XGI_SetCRT2Group301(USHORT ModeNo, PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO pVBInfo);
+BOOLEAN  XGI_BacklightByDrv(PVB_DEVICE_INFO pVBInfo);
+
+BOOLEAN  XGI_IsLCDON(PVB_DEVICE_INFO pVBInfo);
+BOOLEAN  XGI_DisableChISLCD(PVB_DEVICE_INFO pVBInfo);
+BOOLEAN  XGI_EnableChISLCD(PVB_DEVICE_INFO pVBInfo);
+BOOLEAN  XGI_AjustCRT2Rate(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,USHORT *i, PVB_DEVICE_INFO pVBInfo);
+BOOLEAN  XGI_SearchModeID( USHORT ModeNo,USHORT  *ModeIdIndex, PVB_DEVICE_INFO pVBInfo);
+BOOLEAN  XGI_GetLCDInfo(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo);
+BOOLEAN  XGISetModeNew( PXGI_HW_DEVICE_INFO HwDeviceExtension , USHORT ModeNo ) ;
+BOOLEAN  XGI_BridgeIsOn(PVB_DEVICE_INFO pVBInfo);
+UCHAR    XGI_GetModePtr( USHORT ModeNo,USHORT ModeIdIndex,PVB_DEVICE_INFO pVBInfo);
+USHORT   XGI_GetOffset(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO pVBInfo);
+USHORT   XGI_GetRatePtrCRT2( PXGI_HW_DEVICE_INFO pXGIHWDE, USHORT ModeNo,USHORT ModeIdIndex,PVB_DEVICE_INFO pVBInfo );
+USHORT   XGI_GetResInfo(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo);
+USHORT   XGI_GetColorDepth(USHORT ModeNo,USHORT ModeIdIndex,PVB_DEVICE_INFO pVBInfo);
+USHORT   XGI_GetVGAHT2(PVB_DEVICE_INFO pVBInfo);
+USHORT   XGI_GetVCLK2Ptr(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO pVBInfo);
+void     XGI_VBLongWait(PVB_DEVICE_INFO pVBInfo);
+void     XGI_SaveCRT2Info(USHORT ModeNo, PVB_DEVICE_INFO pVBInfo);
+void     XGI_GetCRT2Data(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_GetCRT2ResInfo(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_PreSetGroup1(USHORT ModeNo,USHORT ModeIdIndex, PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetGroup1(USHORT ModeNo,USHORT ModeIdIndex, PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetLockRegs(USHORT ModeNo,USHORT ModeIdIndex, PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetLCDRegs(USHORT ModeNo,USHORT ModeIdIndex, PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetGroup2(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetGroup3(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetGroup4(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetGroup5(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo);
+void*    XGI_GetLcdPtr(USHORT BX,  USHORT ModeNo, USHORT ModeIdIndex, USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo);
+void*    XGI_GetTVPtr(USHORT BX, USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_FirePWDEnable(PVB_DEVICE_INFO pVBInfo);
+void     XGI_EnableGatingCRT(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo);
+void     XGI_DisableGatingCRT(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetPanelDelay(USHORT tempbl, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetPanelPower(USHORT tempah,USHORT tempbl, PVB_DEVICE_INFO pVBInfo);
+void     XGI_EnablePWD( PVB_DEVICE_INFO pVBInfo);
+void     XGI_DisablePWD( PVB_DEVICE_INFO pVBInfo);
+void     XGI_AutoThreshold( PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetTap4Regs( PVB_DEVICE_INFO pVBInfo);
+
+void     XGI_DisplayOn(PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO pVBInfo);
+void     XGI_DisplayOff( PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO pVBInfo );
+void     XGI_SetCRT1Group(PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT ModeNo,USHORT ModeIdIndex,PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetXG21CRTC(USHORT ModeNo, USHORT ModeIdIndex, USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetXG21LCD(PVB_DEVICE_INFO pVBInfo,USHORT RefreshRateTableIndex,USHORT ModeNo);
+void     XGI_SetXG27CRTC(USHORT ModeNo, USHORT ModeIdIndex, USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetXG27LCD(PVB_DEVICE_INFO pVBInfo,USHORT RefreshRateTableIndex,USHORT ModeNo);
+void     XGI_UpdateXG21CRTC(USHORT ModeNo, PVB_DEVICE_INFO pVBInfo, USHORT RefreshRateTableIndex);
+void     XGI_WaitDisply(PVB_DEVICE_INFO pVBInfo);
+void     XGI_SenseCRT1(PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetSeqRegs(USHORT ModeNo,USHORT StandTableIndex,USHORT ModeIdIndex,PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetMiscRegs(USHORT StandTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetCRTCRegs(PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT StandTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetATTRegs(USHORT ModeNo,USHORT StandTableIndex,USHORT ModeIdIndex,PVB_DEVICE_INFO pVBInfo );
+void     XGI_SetGRCRegs(USHORT StandTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_ClearExt1Regs(PVB_DEVICE_INFO pVBInfo);
+
+void     XGI_SetSync(USHORT RefreshRateTableIndex,PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetCRT1CRTC(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PVB_DEVICE_INFO pVBInfo,PXGI_HW_DEVICE_INFO HwDeviceExtension);
+void     XGI_SetCRT1Timing_H(PVB_DEVICE_INFO pVBInfo,PXGI_HW_DEVICE_INFO HwDeviceExtension);
+void     XGI_SetCRT1Timing_V(USHORT ModeIdIndex,USHORT ModeNo,PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetCRT1DE(PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetCRT1VCLK(USHORT ModeNo,USHORT ModeIdIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetCRT1FIFO(USHORT ModeNo,PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetCRT1ModeRegs(PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetVCLKState(PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT ModeNo,USHORT RefreshRateTableIndex,PVB_DEVICE_INFO pVBInfo);
+
+void     XGI_LoadDAC(USHORT ModeNo,USHORT ModeIdIndex,PVB_DEVICE_INFO pVBInfo);
+void     XGI_WriteDAC(USHORT dl, USHORT ah, USHORT al, USHORT dh, PVB_DEVICE_INFO pVBInfo);
+/*void     XGI_ClearBuffer(PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT ModeNo,PVB_DEVICE_INFO pVBInfo);*/
+void     XGI_SetLCDAGroup(USHORT ModeNo,USHORT ModeIdIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO  pVBInfo);
+void     XGI_GetLVDSResInfo( USHORT ModeNo,USHORT ModeIdIndex,PVB_DEVICE_INFO  pVBInfo);
+void     XGI_GetLVDSData(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PVB_DEVICE_INFO  pVBInfo);
+void     XGI_ModCRT1Regs(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO  pVBInfo);
+void     XGI_SetLVDSRegs(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PVB_DEVICE_INFO  pVBInfo);
+void     XGI_UpdateModeInfo(PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO  pVBInfo);
+void     XGI_GetVGAType(PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO  pVBInfo);
+void     XGI_GetVBType(PVB_DEVICE_INFO  pVBInfo);
+void     XGI_GetVBInfo(USHORT ModeNo,USHORT ModeIdIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO  pVBInfo);
+void     XGI_GetTVInfo(USHORT ModeNo,USHORT ModeIdIndex,PVB_DEVICE_INFO  pVBInfo);
+void     XGI_SetCRT2ECLK( USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PVB_DEVICE_INFO  pVBInfo);
+void     InitTo330Pointer(UCHAR,PVB_DEVICE_INFO pVBInfo);
+void     XGI_GetLCDSync(USHORT* HSyncWidth, USHORT* VSyncWidth, PVB_DEVICE_INFO pVBInfo);
+void     XGI_DisableBridge(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo);
+void     XGI_EnableBridge(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetCRT2VCLK(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_OEM310Setting(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetDelayComp(PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetLCDCap(PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetLCDCap_A(USHORT tempcx,PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetLCDCap_B(USHORT tempcx,PVB_DEVICE_INFO pVBInfo);
+void     SetSpectrum(PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetAntiFlicker(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetEdgeEnhance(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetPhaseIncr(PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetYFilter(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_GetTVPtrIndex2(USHORT* tempbx,UCHAR* tempcl,UCHAR* tempch, PVB_DEVICE_INFO pVBInfo);
+USHORT   XGI_GetTVPtrIndex(  PVB_DEVICE_INFO pVBInfo );
+void     XGI_SetCRT2ModeRegs(USHORT ModeNo,PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO pVBInfo );
+void     XGI_CloseCRTC(PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO pVBInfo);
+void     XGI_OpenCRTC(PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO pVBInfo);
+void     XGI_GetRAMDAC2DATA(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_UnLockCRT2(PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO pVBInfo);
+void     XGI_LockCRT2(PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO pVBInfo);
+void     XGINew_EnableCRT2(PVB_DEVICE_INFO pVBInfo);
+void     XGINew_LCD_Wait_Time(UCHAR DelayTime, PVB_DEVICE_INFO pVBInfo);
+void     XGI_LongWait(PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetCRT1Offset( USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex , PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO pVBInfo );
+void     XGI_GetLCDVCLKPtr(UCHAR* di_0,UCHAR *di_1, PVB_DEVICE_INFO pVBInfo);
+UCHAR    XGI_GetVCLKPtr(USHORT RefreshRateTableIndex,USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo);
+void     XGI_GetVCLKLen(UCHAR tempal,UCHAR* di_0,UCHAR* di_1, PVB_DEVICE_INFO pVBInfo);
+USHORT   XGI_GetLCDCapPtr(PVB_DEVICE_INFO pVBInfo);
+USHORT   XGI_GetLCDCapPtr1(PVB_DEVICE_INFO pVBInfo);
+XGI301C_Tap4TimingStruct* XGI_GetTap4Ptr(USHORT tempcx, PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetXG21FPBits(PVB_DEVICE_INFO pVBInfo);
+void     XGI_SetXG27FPBits(PVB_DEVICE_INFO pVBInfo);
+UCHAR    XGI_XG21GetPSCValue(PVB_DEVICE_INFO pVBInfo);
+UCHAR    XGI_XG27GetPSCValue(PVB_DEVICE_INFO pVBInfo);
+void     XGI_XG21BLSignalVDD(USHORT tempbh,USHORT tempbl, PVB_DEVICE_INFO pVBInfo);
+void     XGI_XG27BLSignalVDD(USHORT tempbh,USHORT tempbl, PVB_DEVICE_INFO pVBInfo);
+void     XGI_XG21SetPanelDelay(USHORT tempbl, PVB_DEVICE_INFO pVBInfo);
+BOOLEAN  XGI_XG21CheckLVDSMode(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo );
+void     XGI_SetXG21LVDSPara(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo );
+void     XGI_SetXG27LVDSPara(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo );
+UCHAR    XGI_SetDefaultVCLK( PVB_DEVICE_INFO pVBInfo );
+
+extern   void     ReadVBIOSTablData( UCHAR ChipType , PVB_DEVICE_INFO pVBInfo);
+#ifdef WIN2000
+/* [Billy] 2007/05/17 For CH7007 */
+extern  UCHAR CH7007TVReg_UNTSC[][8],CH7007TVReg_ONTSC[][8],CH7007TVReg_UPAL[][8],CH7007TVReg_OPAL[][8];
+extern  UCHAR CH7007TVCRT1UNTSC_H[][10],CH7007TVCRT1ONTSC_H[][10],CH7007TVCRT1UPAL_H[][10],CH7007TVCRT1OPAL_H[][10] ;
+extern  UCHAR CH7007TVCRT1UNTSC_V[][10],CH7007TVCRT1ONTSC_V[][10],CH7007TVCRT1UPAL_V[][10],CH7007TVCRT1OPAL_V[][10] ;
+extern  UCHAR XGI7007_CHTVVCLKUNTSC[],XGI7007_CHTVVCLKONTSC[],XGI7007_CHTVVCLKUPAL[],XGI7007_CHTVVCLKOPAL[];
+
+extern  BOOLEAN XGI_XG21CheckCH7007TVMode(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo ) ;
+extern  void SetCH7007Regs(PXGI_HW_DEVICE_INFO HwDeviceExtension, USHORT ModeNo, USHORT ModeIdIndex, USHORT RefreshRateTableIndex, PVB_DEVICE_INFO  pVBInfo ) ;
+extern  VP_STATUS TurnOnCH7007(PHW_DEVICE_EXTENSION pHWDE) ;
+extern  VP_STATUS TurnOffCH7007(PHW_DEVICE_EXTENSION pHWDE) ;
+extern  BOOLEAN IsCH7007TVMode(PVB_DEVICE_INFO pVBInfo) ;
+#endif
+
+/* USHORT XGINew_flag_clearbuffer; 0: no clear frame buffer 1:clear frame buffer */
+
+
+
+
+
+USHORT XGINew_MDA_DAC[]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+               0x15,0x15,0x15,0x15,0x15,0x15,0x15,0x15,
+               0x15,0x15,0x15,0x15,0x15,0x15,0x15,0x15,
+               0x3F,0x3F,0x3F,0x3F,0x3F,0x3F,0x3F,0x3F,
+               0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+               0x15,0x15,0x15,0x15,0x15,0x15,0x15,0x15,
+               0x15,0x15,0x15,0x15,0x15,0x15,0x15,0x15,
+               0x3F,0x3F,0x3F,0x3F,0x3F,0x3F,0x3F,0x3F};
+
+USHORT XGINew_CGA_DAC[]={0x00,0x10,0x04,0x14,0x01,0x11,0x09,0x15,
+               0x00,0x10,0x04,0x14,0x01,0x11,0x09,0x15,
+               0x2A,0x3A,0x2E,0x3E,0x2B,0x3B,0x2F,0x3F,
+               0x2A,0x3A,0x2E,0x3E,0x2B,0x3B,0x2F,0x3F,
+               0x00,0x10,0x04,0x14,0x01,0x11,0x09,0x15,
+               0x00,0x10,0x04,0x14,0x01,0x11,0x09,0x15,
+               0x2A,0x3A,0x2E,0x3E,0x2B,0x3B,0x2F,0x3F,
+               0x2A,0x3A,0x2E,0x3E,0x2B,0x3B,0x2F,0x3F};
+
+USHORT XGINew_EGA_DAC[]={0x00,0x10,0x04,0x14,0x01,0x11,0x05,0x15,
+               0x20,0x30,0x24,0x34,0x21,0x31,0x25,0x35,
+               0x08,0x18,0x0C,0x1C,0x09,0x19,0x0D,0x1D,
+               0x28,0x38,0x2C,0x3C,0x29,0x39,0x2D,0x3D,
+               0x02,0x12,0x06,0x16,0x03,0x13,0x07,0x17,
+               0x22,0x32,0x26,0x36,0x23,0x33,0x27,0x37,
+               0x0A,0x1A,0x0E,0x1E,0x0B,0x1B,0x0F,0x1F,
+               0x2A,0x3A,0x2E,0x3E,0x2B,0x3B,0x2F,0x3F};
+
+USHORT XGINew_VGA_DAC[]={0x00,0x10,0x04,0x14,0x01,0x11,0x09,0x15,
+               0x2A,0x3A,0x2E,0x3E,0x2B,0x3B,0x2F,0x3F,
+               0x00,0x05,0x08,0x0B,0x0E,0x11,0x14,0x18,
+               0x1C,0x20,0x24,0x28,0x2D,0x32,0x38,0x3F,
+
+               0x00,0x10,0x1F,0x2F,0x3F,0x1F,0x27,0x2F,
+               0x37,0x3F,0x2D,0x31,0x36,0x3A,0x3F,0x00,
+               0x07,0x0E,0x15,0x1C,0x0E,0x11,0x15,0x18,
+               0x1C,0x14,0x16,0x18,0x1A,0x1C,0x00,0x04,
+               0x08,0x0C,0x10,0x08,0x0A,0x0C,0x0E,0x10,
+               0x0B,0x0C,0x0D,0x0F,0x10};
+
+
+/* --------------------------------------------------------------------- */
+/* Function : InitTo330Pointer */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void InitTo330Pointer( UCHAR ChipType ,PVB_DEVICE_INFO pVBInfo)
+{
+    pVBInfo->SModeIDTable = (XGI_StStruct *) XGI330_SModeIDTable ;
+    pVBInfo->StandTable = (XGI_StandTableStruct *) XGI330_StandTable ;
+    pVBInfo->EModeIDTable = (XGI_ExtStruct *) XGI330_EModeIDTable ;
+    pVBInfo->RefIndex = (XGI_Ext2Struct *) XGI330_RefIndex ;
+    pVBInfo->XGINEWUB_CRT1Table = (XGI_CRT1TableStruct *) XGI_CRT1Table ;
+
+    /* add for new UNIVGABIOS */
+    /* XGINew_UBLCDDataTable = (XGI_LCDDataTablStruct *) XGI_LCDDataTable ; */
+    /* XGINew_UBTVDataTable = (XGI_TVDataTablStruct *) XGI_TVDataTable ; */
+
+
+    if ( ChipType >= XG40 )
+    {
+        pVBInfo->MCLKData = (XGI_MCLKDataStruct *) XGI340New_MCLKData ;
+        pVBInfo->ECLKData = (XGI_ECLKDataStruct *) XGI340_ECLKData ;
+    }
+    else
+    {
+        pVBInfo->MCLKData = (XGI_MCLKDataStruct *) XGI330New_MCLKData ;
+        pVBInfo->ECLKData = (XGI_ECLKDataStruct *) XGI330_ECLKData ;
+    }
+
+    pVBInfo->VCLKData = (XGI_VCLKDataStruct *) XGI_VCLKData ;
+    pVBInfo->VBVCLKData = (XGI_VBVCLKDataStruct *) XGI_VBVCLKData ;
+    pVBInfo->ScreenOffset = XGI330_ScreenOffset ;
+    pVBInfo->StResInfo = (XGI_StResInfoStruct *) XGI330_StResInfo ;
+    pVBInfo->ModeResInfo = (XGI_ModeResInfoStruct *) XGI330_ModeResInfo ;
+
+    pVBInfo->pOutputSelect = &XGI330_OutputSelect ;
+    pVBInfo->pSoftSetting = &XGI330_SoftSetting ;
+    pVBInfo->pSR07 = &XGI330_SR07 ;
+    pVBInfo->LCDResInfo = 0 ;
+    pVBInfo->LCDTypeInfo = 0 ;
+    pVBInfo->LCDInfo = 0 ;
+    pVBInfo->VBInfo = 0 ;
+    pVBInfo->TVInfo = 0;
+
+
+    pVBInfo->SR15 = XGI340_SR13 ;
+    pVBInfo->CR40 = XGI340_cr41 ;
+    pVBInfo->SR25 = XGI330_sr25 ;
+    pVBInfo->pSR31 = &XGI330_sr31 ;
+    pVBInfo->pSR32 = &XGI330_sr32 ;
+    pVBInfo->CR6B = XGI340_CR6B ;
+    pVBInfo->CR6E = XGI340_CR6E ;
+    pVBInfo->CR6F = XGI340_CR6F ;
+    pVBInfo->CR89 = XGI340_CR89 ;
+    pVBInfo->AGPReg = XGI340_AGPReg ;
+    pVBInfo->SR16 = XGI340_SR16 ;
+    pVBInfo->pCRCF = &XG40_CRCF ;
+    pVBInfo->pXGINew_DRAMTypeDefinition = &XG40_DRAMTypeDefinition ;
+
+
+    pVBInfo->CR49 = XGI330_CR49 ;
+    pVBInfo->pSR1F = &XGI330_SR1F ;
+    pVBInfo->pSR21 = &XGI330_SR21 ;
+    pVBInfo->pSR22 = &XGI330_SR22 ;
+    pVBInfo->pSR23 = &XGI330_SR23 ;
+    pVBInfo->pSR24 = &XGI330_SR24 ;
+    pVBInfo->pSR33 = &XGI330_SR33 ;
+
+
+
+    pVBInfo->pCRT2Data_1_2 = &XGI330_CRT2Data_1_2 ;
+    pVBInfo->pCRT2Data_4_D = &XGI330_CRT2Data_4_D ;
+    pVBInfo->pCRT2Data_4_E = &XGI330_CRT2Data_4_E ;
+    pVBInfo->pCRT2Data_4_10 = &XGI330_CRT2Data_4_10 ;
+    pVBInfo->pRGBSenseData = &XGI330_RGBSenseData ;
+    pVBInfo->pVideoSenseData = &XGI330_VideoSenseData ;
+    pVBInfo->pYCSenseData = &XGI330_YCSenseData ;
+    pVBInfo->pRGBSenseData2 = &XGI330_RGBSenseData2 ;
+    pVBInfo->pVideoSenseData2 = &XGI330_VideoSenseData2 ;
+    pVBInfo->pYCSenseData2 = &XGI330_YCSenseData2 ;
+
+    pVBInfo->NTSCTiming = XGI330_NTSCTiming ;
+    pVBInfo->PALTiming = XGI330_PALTiming ;
+    pVBInfo->HiTVExtTiming = XGI330_HiTVExtTiming ;
+    pVBInfo->HiTVSt1Timing = XGI330_HiTVSt1Timing ;
+    pVBInfo->HiTVSt2Timing = XGI330_HiTVSt2Timing ;
+    pVBInfo->HiTVTextTiming = XGI330_HiTVTextTiming ;
+    pVBInfo->YPbPr750pTiming = XGI330_YPbPr750pTiming ;
+    pVBInfo->YPbPr525pTiming = XGI330_YPbPr525pTiming ;
+    pVBInfo->YPbPr525iTiming = XGI330_YPbPr525iTiming ;
+    pVBInfo->HiTVGroup3Data = XGI330_HiTVGroup3Data ;
+    pVBInfo->HiTVGroup3Simu = XGI330_HiTVGroup3Simu ;
+    pVBInfo->HiTVGroup3Text = XGI330_HiTVGroup3Text ;
+    pVBInfo->Ren525pGroup3 = XGI330_Ren525pGroup3 ;
+    pVBInfo->Ren750pGroup3 = XGI330_Ren750pGroup3 ;
+
+
+    pVBInfo->TimingH = (XGI_TimingHStruct *) XGI_TimingH ;
+    pVBInfo->TimingV = (XGI_TimingVStruct *) XGI_TimingV ;
+    pVBInfo->UpdateCRT1 = (XGI_XG21CRT1Struct *) XGI_UpdateCRT1Table ;
+
+    pVBInfo->CHTVVCLKUNTSC = XGI330_CHTVVCLKUNTSC ;
+    pVBInfo->CHTVVCLKONTSC = XGI330_CHTVVCLKONTSC ;
+    pVBInfo->CHTVVCLKUPAL = XGI330_CHTVVCLKUPAL ;
+    pVBInfo->CHTVVCLKOPAL = XGI330_CHTVVCLKOPAL ;
+
+    /* 310 customization related */
+    if ( ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) )
+        pVBInfo->LCDCapList = XGI_LCDDLCapList ;
+    else
+        pVBInfo->LCDCapList = XGI_LCDCapList ;
+
+    if ( ( ChipType == XG21 ) || ( ChipType == XG27 ) )
+        pVBInfo->XG21_LVDSCapList = XGI21_LCDCapList ;
+
+    pVBInfo->XGI_TVDelayList = XGI301TVDelayList ;
+    pVBInfo->XGI_TVDelayList2 = XGI301TVDelayList2 ;
+
+
+    pVBInfo->pXGINew_I2CDefinition = &XG40_I2CDefinition ;
+
+    if ( ChipType >= XG20 )
+        pVBInfo->pXGINew_CR97 = &XG20_CR97 ;
+
+    if ( ChipType == XG27 )
+    {
+        pVBInfo->MCLKData = (XGI_MCLKDataStruct *) XGI27New_MCLKData ;
+        pVBInfo->CR40 = XGI27_cr41 ;
+        pVBInfo->pXGINew_CR97 = &XG27_CR97 ;
+        pVBInfo->pSR36 = &XG27_SR36 ;
+        pVBInfo->pCR8F = &XG27_CR8F ;
+        pVBInfo->pCRD0 = XG27_CRD0 ;
+        pVBInfo->pCRDE = XG27_CRDE ;
+        pVBInfo->pSR40 = &XG27_SR40 ;
+        pVBInfo->pSR41 = &XG27_SR41 ;
+
+    }
+
+    if ( ChipType >= XG20 )
+    {
+        pVBInfo->pDVOSetting = &XG21_DVOSetting ;
+        pVBInfo->pCR2E = &XG21_CR2E ;
+        pVBInfo->pCR2F = &XG21_CR2F ;
+        pVBInfo->pCR46 = &XG21_CR46 ;
+        pVBInfo->pCR47 = &XG21_CR47 ;
+    }
+
+}
+
+
+
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGISetModeNew */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGISetModeNew( PXGI_HW_DEVICE_INFO HwDeviceExtension , USHORT ModeNo )
+{
+    USHORT ModeIdIndex ;
+        /* PUCHAR pVBInfo->FBAddr = HwDeviceExtension->pjVideoMemoryAddress ; */
+    VB_DEVICE_INFO VBINF;
+    PVB_DEVICE_INFO pVBInfo = &VBINF;
+    pVBInfo->ROMAddr = HwDeviceExtension->pjVirtualRomBase ;
+    pVBInfo->BaseAddr = (ULONG)HwDeviceExtension->pjIOAddress ;
+    pVBInfo->IF_DEF_LVDS = 0 ;
+    pVBInfo->IF_DEF_CH7005 = 0 ;
+    pVBInfo->IF_DEF_LCDA = 1 ;
+    pVBInfo->IF_DEF_CH7017 = 0 ;
+    pVBInfo->IF_DEF_CH7007 = 0 ;                                /* [Billy] 2007/05/14 */
+    pVBInfo->IF_DEF_VideoCapture = 0 ;
+    pVBInfo->IF_DEF_ScaleLCD = 0 ;
+    pVBInfo->IF_DEF_OEMUtil = 0 ;
+    pVBInfo->IF_DEF_PWD = 0 ;
+
+
+    if ( HwDeviceExtension->jChipType >= XG20 )                 /* kuku 2004/06/25 */
+    {
+        pVBInfo->IF_DEF_YPbPr = 0 ;
+        pVBInfo->IF_DEF_HiVision = 0 ;
+        pVBInfo->IF_DEF_CRT2Monitor = 0 ;
+        pVBInfo->VBType = 0 ;  /*set VBType default 0*/
+    }
+    else if ( HwDeviceExtension->jChipType >= XG40 )
+    {
+        pVBInfo->IF_DEF_YPbPr = 1 ;
+        pVBInfo->IF_DEF_HiVision = 1 ;
+        pVBInfo->IF_DEF_CRT2Monitor = 1 ;
+    }
+    else
+    {
+        pVBInfo->IF_DEF_YPbPr = 1 ;
+        pVBInfo->IF_DEF_HiVision = 1 ;
+        pVBInfo->IF_DEF_CRT2Monitor = 0 ;
+    }
+
+    pVBInfo->P3c4 = pVBInfo->BaseAddr + 0x14 ;
+    pVBInfo->P3d4 = pVBInfo->BaseAddr + 0x24 ;
+    pVBInfo->P3c0 = pVBInfo->BaseAddr + 0x10 ;
+    pVBInfo->P3ce = pVBInfo->BaseAddr + 0x1e ;
+    pVBInfo->P3c2 = pVBInfo->BaseAddr + 0x12 ;
+    pVBInfo->P3cc = pVBInfo->BaseAddr + 0x1C ;
+    pVBInfo->P3ca = pVBInfo->BaseAddr + 0x1a ;
+    pVBInfo->P3c6 = pVBInfo->BaseAddr + 0x16 ;
+    pVBInfo->P3c7 = pVBInfo->BaseAddr + 0x17 ;
+    pVBInfo->P3c8 = pVBInfo->BaseAddr + 0x18 ;
+    pVBInfo->P3c9 = pVBInfo->BaseAddr + 0x19 ;
+    pVBInfo->P3da = pVBInfo->BaseAddr + 0x2A ;
+    pVBInfo->Part0Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_00 ;
+    pVBInfo->Part1Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_04 ;
+    pVBInfo->Part2Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_10 ;
+    pVBInfo->Part3Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_12 ;
+    pVBInfo->Part4Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 ;
+    pVBInfo->Part5Port = pVBInfo->BaseAddr + XGI_CRT2_PORT_14 + 2 ;
+
+    if ( HwDeviceExtension->jChipType == XG21 )  /* for x86 Linux, XG21 LVDS */
+    {
+        if ( ( XGINew_GetReg1( pVBInfo->P3d4 , 0x38 ) & 0xE0 ) == 0xC0 )
+        {
+            pVBInfo->IF_DEF_LVDS = 1 ;
+        }
+    }
+    if ( HwDeviceExtension->jChipType == XG27 )
+    {
+        if ( ( XGINew_GetReg1( pVBInfo->P3d4 , 0x38 ) & 0xE0 ) == 0xC0 )
+        {
+          if ( XGINew_GetReg1( pVBInfo->P3d4 , 0x30 ) & 0x20 )
+          {
+            pVBInfo->IF_DEF_LVDS = 1 ;
+          }
+        }
+    }
+
+    if ( HwDeviceExtension->jChipType < XG20 )                  /* kuku 2004/06/25 */
+    XGI_GetVBType( pVBInfo ) ;
+
+    InitTo330Pointer( HwDeviceExtension->jChipType, pVBInfo ) ;
+#ifdef WIN2000
+    ReadVBIOSTablData( HwDeviceExtension->jChipType , pVBInfo) ;
+#endif
+    if ( ModeNo & 0x80 )
+    {
+        ModeNo = ModeNo & 0x7F ;
+/* XGINew_flag_clearbuffer = 0 ; */
+    }
+/*    else
+    {
+        XGINew_flag_clearbuffer = 1 ;
+    }
+*/
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x05 , 0x86 ) ;
+
+    if ( HwDeviceExtension->jChipType < XG20 )                  /* kuku 2004/06/25 1.Openkey */
+    XGI_UnLockCRT2( HwDeviceExtension , pVBInfo ) ;
+
+    XGI_SearchModeID( ModeNo , &ModeIdIndex, pVBInfo ) ;
+
+    XGI_GetVGAType(HwDeviceExtension,  pVBInfo) ;
+
+    if ( HwDeviceExtension->jChipType < XG20 )                  /* kuku 2004/06/25 */
+    {
+        XGI_GetVBInfo(ModeNo , ModeIdIndex , HwDeviceExtension, pVBInfo ) ;
+        XGI_GetTVInfo(ModeNo , ModeIdIndex, pVBInfo ) ;
+        XGI_GetLCDInfo(ModeNo , ModeIdIndex, pVBInfo ) ;
+        XGI_DisableBridge( HwDeviceExtension,pVBInfo ) ;
+/*        XGI_OpenCRTC( HwDeviceExtension, pVBInfo ) ; */
+
+        if ( pVBInfo->VBInfo & ( SetSimuScanMode | SetCRT2ToLCDA ) )
+        {
+            XGI_SetCRT1Group(HwDeviceExtension , ModeNo , ModeIdIndex, pVBInfo ) ;
+
+            if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+            {
+                XGI_SetLCDAGroup(ModeNo , ModeIdIndex , HwDeviceExtension, pVBInfo ) ;
+            }
+        }
+        else
+        {
+            if ( !( pVBInfo->VBInfo & SwitchToCRT2) )
+            {
+                XGI_SetCRT1Group( HwDeviceExtension , ModeNo , ModeIdIndex, pVBInfo ) ;
+                if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+                {
+                    XGI_SetLCDAGroup( ModeNo , ModeIdIndex , HwDeviceExtension, pVBInfo ) ;
+                }
+            }
+        }
+
+        if ( pVBInfo->VBInfo & ( SetSimuScanMode | SwitchToCRT2 ) )
+        {
+            switch( HwDeviceExtension->ujVBChipID )
+            {
+                case VB_CHIP_301:
+                    XGI_SetCRT2Group301( ModeNo , HwDeviceExtension, pVBInfo ) ;                /*add for CRT2 */
+                    break ;
+
+                case VB_CHIP_302:
+                    XGI_SetCRT2Group301(ModeNo , HwDeviceExtension, pVBInfo ) ;         /*add for CRT2 */
+                    break ;
+
+                default:
+                    break ;
+            }
+        }
+
+        XGI_SetCRT2ModeRegs( ModeNo, HwDeviceExtension,pVBInfo ) ;
+        XGI_OEM310Setting( ModeNo, ModeIdIndex,pVBInfo ) ; /*0212*/
+        XGI_CloseCRTC( HwDeviceExtension, pVBInfo ) ;
+        XGI_EnableBridge( HwDeviceExtension ,pVBInfo) ;
+    }   /* !XG20 */
+    else
+    {
+#ifdef WIN2000
+        if ( pVBInfo->IF_DEF_CH7007 == 1 )
+        {
+
+            VideoDebugPrint((0, "XGISetModeNew: pVBIfo->IF_DEF_CH7007==1\n"));
+            pVBInfo->VBType = VB_CH7007 ;
+            XGI_GetVBInfo(ModeNo , ModeIdIndex , HwDeviceExtension, pVBInfo ) ;
+            XGI_GetTVInfo(ModeNo , ModeIdIndex, pVBInfo ) ;
+            XGI_GetLCDInfo(ModeNo , ModeIdIndex, pVBInfo ) ;
+            if( !(XGI_XG21CheckCH7007TVMode(ModeNo, ModeIdIndex, pVBInfo )) )
+            {
+              return FALSE;
+            }
+        }
+#endif
+
+
+        if ( pVBInfo->IF_DEF_LVDS == 1 )
+        {
+            if ( !XGI_XG21CheckLVDSMode(ModeNo , ModeIdIndex, pVBInfo) )
+            {
+              return FALSE;
+            }
+        }
+
+        if ( ModeNo <= 0x13 )
+        {
+            pVBInfo->ModeType = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag & ModeInfoFlag;
+        }
+        else
+        {
+            pVBInfo->ModeType = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag & ModeInfoFlag;
+        }
+
+        pVBInfo->SetFlag = 0 ;
+        if ( pVBInfo->IF_DEF_CH7007 != 1 )
+        {
+            pVBInfo->VBInfo = DisableCRT2Display ;
+        }
+
+
+        XGI_DisplayOff(HwDeviceExtension, pVBInfo) ;
+
+        XGI_SetCRT1Group(HwDeviceExtension , ModeNo , ModeIdIndex, pVBInfo ) ;
+
+        XGI_DisplayOn( HwDeviceExtension, pVBInfo ) ;
+        /*
+        if( HwDeviceExtension->jChipType == XG21 )
+          XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x09 , ~0x80 , 0x80 ) ;
+        */
+    }
+
+
+/*
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+    }
+    pVBInfo->ModeType = modeflag&ModeInfoFlag ;
+    pVBInfo->SetFlag = 0x00 ;
+    pVBInfo->VBInfo = DisableCRT2Display ;
+    temp = XGINew_CheckMemorySize(  HwDeviceExtension , ModeNo , ModeIdIndex, pVBInfo ) ;
+
+    if ( temp == 0 )
+        return( 0 ) ;
+
+    XGI_DisplayOff( HwDeviceExtension, pVBInfo) ;
+    XGI_SetCRT1Group( HwDeviceExtension , ModeNo , ModeIdIndex, pVBInfo ) ;
+    XGI_DisplayOn( HwDeviceExtension, pVBInfo) ;
+*/
+
+    XGI_UpdateModeInfo( HwDeviceExtension, pVBInfo ) ;
+
+    if ( HwDeviceExtension->jChipType < XG20 )                  /* kuku 2004/06/25 */
+{
+    XGI_LockCRT2( HwDeviceExtension, pVBInfo ) ;
+}
+
+    return( TRUE ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT1Group */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT1Group( PXGI_HW_DEVICE_INFO HwDeviceExtension , USHORT ModeNo , USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT StandTableIndex ,
+           RefreshRateTableIndex ,
+           b3CC ,
+           temp ;
+
+    USHORT XGINew_P3cc =  pVBInfo->P3cc;
+
+    /* XGINew_CRT1Mode = ModeNo ; // SaveModeID */
+    StandTableIndex = XGI_GetModePtr( ModeNo , ModeIdIndex, pVBInfo ) ;
+    /* XGI_SetBIOSData(ModeNo , ModeIdIndex ) ; */
+    /* XGI_ClearBankRegs( ModeNo , ModeIdIndex ) ; */
+    XGI_SetSeqRegs( ModeNo , StandTableIndex , ModeIdIndex, pVBInfo ) ;
+    XGI_SetMiscRegs( StandTableIndex,  pVBInfo ) ;
+    XGI_SetCRTCRegs( HwDeviceExtension , StandTableIndex,  pVBInfo) ;
+    XGI_SetATTRegs( ModeNo , StandTableIndex , ModeIdIndex, pVBInfo ) ;
+    XGI_SetGRCRegs(  StandTableIndex, pVBInfo ) ;
+    XGI_ClearExt1Regs(pVBInfo) ;
+
+/* if ( pVBInfo->IF_DEF_ExpLink ) */
+    if ( HwDeviceExtension->jChipType == XG27 )
+    {
+      if ( pVBInfo->IF_DEF_LVDS == 0 )
+      {
+        XGI_SetDefaultVCLK( pVBInfo ) ;
+      }
+    }
+
+    temp = ~ProgrammingCRT2 ;
+    pVBInfo->SetFlag &= temp ;
+    pVBInfo->SelectCRT2Rate = 0 ;
+
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        if ( pVBInfo->VBInfo & ( SetSimuScanMode | SetCRT2ToLCDA | SetInSlaveMode ) )
+        {
+            pVBInfo->SetFlag |= ProgrammingCRT2 ;
+         }
+    }
+
+    RefreshRateTableIndex = XGI_GetRatePtrCRT2( HwDeviceExtension, ModeNo , ModeIdIndex, pVBInfo ) ;
+    if ( RefreshRateTableIndex != 0xFFFF )
+    {
+        XGI_SetSync( RefreshRateTableIndex, pVBInfo ) ;
+        XGI_SetCRT1CRTC( ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo, HwDeviceExtension ) ;
+        XGI_SetCRT1DE( HwDeviceExtension , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+        XGI_SetCRT1Offset( ModeNo , ModeIdIndex , RefreshRateTableIndex , HwDeviceExtension, pVBInfo ) ;
+        XGI_SetCRT1VCLK( ModeNo , ModeIdIndex , HwDeviceExtension , RefreshRateTableIndex, pVBInfo ) ;
+    }
+
+    if ( ( HwDeviceExtension->jChipType >= XG20 )&&
+         ( HwDeviceExtension->jChipType < XG27 ) ) /* fix H/W DCLK/2 bug */
+    {
+        if ( ( ModeNo == 0x00 ) | (ModeNo == 0x01) )
+        {
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x2B , 0x4E) ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x2C , 0xE9) ;
+            b3CC =(UCHAR) XGINew_GetReg2(XGINew_P3cc) ;
+            XGINew_SetReg3(XGINew_P3cc ,  (b3CC |= 0x0C) ) ;
+        }
+        else if ( ( ModeNo == 0x04) | ( ModeNo == 0x05) | ( ModeNo == 0x0D) )
+        {
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x2B , 0x1B) ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x2C , 0xE3) ;
+            b3CC = (UCHAR)XGINew_GetReg2(XGINew_P3cc) ;
+            XGINew_SetReg3(XGINew_P3cc ,  (b3CC |= 0x0C) ) ;
+        }
+    }
+
+    if ( HwDeviceExtension->jChipType >= XG21 )
+    {
+      temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x38 ) ;
+      if ( temp & 0xA0 )
+      {
+
+        /*XGINew_SetRegAND( pVBInfo->P3d4 , 0x4A , ~0x20 ) ;*/  /* Enable write GPIOF */
+        /*XGINew_SetRegAND( pVBInfo->P3d4 , 0x48 , ~0x20 ) ;*/  /* P. DWN */
+        /* XG21 CRT1 Timing */
+        if ( HwDeviceExtension->jChipType == XG27 )
+          XGI_SetXG27CRTC( ModeNo, ModeIdIndex, RefreshRateTableIndex, pVBInfo );
+        else
+        XGI_SetXG21CRTC( ModeNo, ModeIdIndex, RefreshRateTableIndex, pVBInfo );
+
+        XGI_UpdateXG21CRTC( ModeNo , pVBInfo , RefreshRateTableIndex) ;
+
+        if ( HwDeviceExtension->jChipType == XG27 )
+          XGI_SetXG27LCD( pVBInfo , RefreshRateTableIndex , ModeNo );
+        else
+          XGI_SetXG21LCD( pVBInfo , RefreshRateTableIndex , ModeNo );
+
+        if ( pVBInfo->IF_DEF_LVDS == 1 )
+        {
+          if ( HwDeviceExtension->jChipType == XG27 )
+            XGI_SetXG27LVDSPara(ModeNo,ModeIdIndex, pVBInfo );
+          else
+            XGI_SetXG21LVDSPara(ModeNo,ModeIdIndex, pVBInfo );
+        }
+        /*XGINew_SetRegOR( pVBInfo->P3d4 , 0x48 , 0x20 ) ;*/    /* P. ON */
+      }
+    }
+
+    pVBInfo->SetFlag &= ( ~ProgrammingCRT2 ) ;
+    XGI_SetCRT1FIFO(  ModeNo , HwDeviceExtension,  pVBInfo ) ;
+    XGI_SetCRT1ModeRegs(  HwDeviceExtension , ModeNo , ModeIdIndex , RefreshRateTableIndex , pVBInfo) ;
+
+
+    /* XGI_LoadCharacter(); //dif ifdef TVFont */
+
+    XGI_LoadDAC( ModeNo , ModeIdIndex, pVBInfo ) ;
+    /* XGI_ClearBuffer( HwDeviceExtension , ModeNo, pVBInfo ) ; */
+#ifdef WIN2000
+   if ( pVBInfo->IF_DEF_CH7007 == 1 )  /* [Billy]  2007/05/14  */
+   {
+       VideoDebugPrint((0, "XGI_SetCRT1Group: VBInfo->IF_DEF_CH7007==1\n"));
+       SetCH7007Regs(HwDeviceExtension, ModeNo, ModeIdIndex, RefreshRateTableIndex, pVBInfo ) ; /* 07/05/28 */
+   }
+#endif
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetModePtr */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+UCHAR XGI_GetModePtr( USHORT ModeNo , USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR index ;
+
+    if ( ModeNo <= 0x13 )
+        index = pVBInfo->SModeIDTable[ ModeIdIndex ].St_StTableIndex ;
+    else
+    {
+        if ( pVBInfo->ModeType <= 0x02 )
+            index = 0x1B ;      /* 02 -> ModeEGA */
+        else
+            index = 0x0F ;
+    }
+    return( index ) ;           /* Get pVBInfo->StandTable index */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetBIOSData */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+/*UCHAR XGI_SetBIOSData( USHORT ModeNo , USHORT ModeIdIndex )
+{
+    return( 0 ) ;
+}
+*/
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_ClearBankRegs */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+/*UCHAR XGI_ClearBankRegs( USHORT ModeNo , USHORT ModeIdIndex )
+{
+    return( 0 ) ;
+}
+*/
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetSeqRegs */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetSeqRegs(  USHORT ModeNo , USHORT StandTableIndex , USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR tempah ,
+          SRdata ;
+
+    USHORT i ,
+           modeflag ;
+
+    if ( ModeNo <= 0x13 )
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+    else
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x00 , 0x03 ) ;             /* Set SR0 */
+    tempah=pVBInfo->StandTable[ StandTableIndex ].SR[ 0 ] ;
+
+    i = SetCRT2ToLCDA ;
+    if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+    {
+        tempah |= 0x01 ;
+    }
+    else
+    {
+        if ( pVBInfo->VBInfo & ( SetCRT2ToTV | SetCRT2ToLCD ) )
+        {
+            if ( pVBInfo->VBInfo & SetInSlaveMode )
+                tempah |= 0x01 ;
+        }
+    }
+
+    tempah |= 0x20 ;            /* screen off */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x01 , tempah ) ;           /* Set SR1 */
+
+    for( i = 02 ; i <= 04 ; i++ )
+    {
+        SRdata = pVBInfo->StandTable[ StandTableIndex ].SR[ i - 1 ] ;   /* Get SR2,3,4 from file */
+        XGINew_SetReg1( pVBInfo->P3c4 , i , SRdata ) ;                          /* Set SR2 3 4 */
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetMiscRegs */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetMiscRegs( USHORT StandTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR Miscdata ;
+
+    Miscdata = pVBInfo->StandTable[ StandTableIndex ].MISC ;    /* Get Misc from file */
+/*
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+        {
+            Miscdata |= 0x0C ;
+        }
+    }
+*/
+
+    XGINew_SetReg3( pVBInfo->P3c2 , Miscdata ) ;                /* Set Misc(3c2) */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRTCRegs */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRTCRegs( PXGI_HW_DEVICE_INFO HwDeviceExtension , USHORT StandTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR CRTCdata ;
+    USHORT i ;
+
+    CRTCdata = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x11 ) ;
+    CRTCdata &= 0x7f ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x11 , CRTCdata ) ;         /* Unlock CRTC */
+
+    for( i = 0 ; i <= 0x18 ; i++ )
+    {
+        CRTCdata = pVBInfo->StandTable[ StandTableIndex ].CRTC[ i ] ;   /* Get CRTC from file */
+        XGINew_SetReg1( pVBInfo->P3d4 , i , CRTCdata ) ;                                /* Set CRTC( 3d4 ) */
+    }
+/*
+    if ( ( HwDeviceExtension->jChipType == XGI_630 )&& ( HwDeviceExtension->jChipRevision == 0x30 ) )
+    {
+        if ( pVBInfo->VBInfo & SetInSlaveMode )
+        {
+            if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToTV ) )
+            {
+                XGINew_SetReg1( pVBInfo->P3d4 , 0x18 , 0xFE ) ;
+            }
+        }
+    }
+*/
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetATTRegs( USHORT ModeNo , USHORT StandTableIndex , USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR ARdata ;
+    USHORT i ,
+           modeflag ;
+
+    if ( ModeNo <= 0x13 )
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+    else
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+
+    for( i = 0 ; i <= 0x13 ; i++ )
+    {
+        ARdata = pVBInfo->StandTable[ StandTableIndex ].ATTR[ i ] ;
+        if ( modeflag & Charx8Dot )     /* ifndef Dot9 */
+        {
+            if ( i == 0x13 )
+            {
+                if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+                    ARdata = 0 ;
+                else
+                {
+                    if ( pVBInfo->VBInfo & ( SetCRT2ToTV | SetCRT2ToLCD ) )
+                    {
+                        if ( pVBInfo->VBInfo & SetInSlaveMode )
+                            ARdata = 0 ;
+                    }
+                }
+            }
+        }
+
+        XGINew_GetReg2( pVBInfo->P3da ) ;                       /* reset 3da */
+        XGINew_SetReg3( pVBInfo->P3c0 , i ) ;           /* set index */
+        XGINew_SetReg3( pVBInfo->P3c0 , ARdata ) ;      /* set data */
+    }
+
+    XGINew_GetReg2( pVBInfo->P3da ) ;                   /* reset 3da */
+    XGINew_SetReg3( pVBInfo->P3c0 , 0x14 ) ;            /* set index */
+    XGINew_SetReg3( pVBInfo->P3c0 , 0x00 ) ;            /* set data */
+    XGINew_GetReg2( pVBInfo->P3da ) ;                   /* Enable Attribute */
+    XGINew_SetReg3( pVBInfo->P3c0 , 0x20 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetGRCRegs */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetGRCRegs( USHORT StandTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR GRdata ;
+    USHORT i ;
+
+    for( i = 0 ; i <= 0x08 ; i++ )
+    {
+        GRdata = pVBInfo->StandTable[ StandTableIndex ].GRC[ i ] ;      /* Get GR from file */
+        XGINew_SetReg1( pVBInfo->P3ce , i , GRdata ) ;                  /* Set GR(3ce) */
+    }
+
+    if ( pVBInfo->ModeType > ModeVGA )
+    {
+        GRdata = ( UCHAR )XGINew_GetReg1( pVBInfo->P3ce , 0x05 ) ;
+        GRdata &= 0xBF ;                                                /* 256 color disable */
+        XGINew_SetReg1( pVBInfo->P3ce , 0x05 , GRdata ) ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_ClearExt1Regs */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_ClearExt1Regs(PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT i ;
+
+    for( i = 0x0A ; i <= 0x0E ; i++ )
+        XGINew_SetReg1( pVBInfo->P3c4 , i , 0x00 ) ;    /* Clear SR0A-SR0E */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetDefaultVCLK */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+UCHAR XGI_SetDefaultVCLK( PVB_DEVICE_INFO pVBInfo )
+{
+
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x31 , ~0x30 , 0x20 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2B , pVBInfo->VCLKData[ 0 ].SR2B ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2C , pVBInfo->VCLKData[ 0 ].SR2C ) ;
+
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x31 , ~0x30 , 0x10 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2B , pVBInfo->VCLKData[ 1 ].SR2B ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2C , pVBInfo->VCLKData[ 1 ].SR2C ) ;
+
+    XGINew_SetRegAND( pVBInfo->P3c4 , 0x31 , ~0x30 ) ;
+    return( 0 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetRatePtrCRT2 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+USHORT XGI_GetRatePtrCRT2( PXGI_HW_DEVICE_INFO pXGIHWDE, USHORT ModeNo , USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    SHORT  LCDRefreshIndex[] = { 0x00 , 0x00 , 0x03 , 0x01 } ,
+           LCDARefreshIndex[] = { 0x00 , 0x00 , 0x03 , 0x01 , 0x01 , 0x01 , 0x01 } ;
+
+    USHORT RefreshRateTableIndex , i ,
+         modeflag , index , temp ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+    }
+
+    if ( pVBInfo->IF_DEF_CH7005 == 1 )
+    {
+        if ( pVBInfo->VBInfo & SetCRT2ToTV )
+        {
+            if ( modeflag & HalfDCLK )
+                return( 0 ) ;
+        }
+    }
+
+    if ( ModeNo < 0x14 )
+        return( 0xFFFF ) ;
+
+    index = XGINew_GetReg1( pVBInfo->P3d4 , 0x33 ) ;
+    index = index >> pVBInfo->SelectCRT2Rate ;
+    index &= 0x0F ;
+
+    if ( pVBInfo->LCDInfo & LCDNonExpanding )
+        index = 0 ;
+
+    if ( index > 0 )
+        index-- ;
+
+    if ( pVBInfo->SetFlag & ProgrammingCRT2 )
+    {
+        if ( pVBInfo->IF_DEF_CH7005 == 1 )
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToTV )
+            {
+                index = 0 ;
+            }
+        }
+
+        if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) )
+        {
+            if( pVBInfo->IF_DEF_LVDS == 0 )
+            {
+                if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+                    temp = LCDARefreshIndex[ pVBInfo->LCDResInfo & 0x0F ] ;     /* 301b */
+                else
+                    temp = LCDRefreshIndex[ pVBInfo->LCDResInfo & 0x0F ] ;
+
+                if ( index > temp )
+                {
+                    index = temp ;
+                }
+            }
+            else
+            {
+                index = 0 ;
+            }
+        }
+    }
+
+    RefreshRateTableIndex = pVBInfo->EModeIDTable[ ModeIdIndex ].REFindex ;
+    ModeNo = pVBInfo->RefIndex[ RefreshRateTableIndex ].ModeID ;
+    if ( pXGIHWDE->jChipType >= XG20 )  /* for XG20, XG21, XG27 */
+    {
+      /*
+      if ( pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_InfoFlag & XG2xNotSupport )
+      {
+        index++;
+      }
+      */
+      if ( ( pVBInfo->RefIndex[ RefreshRateTableIndex ].XRes == 800 ) &&
+           ( pVBInfo->RefIndex[ RefreshRateTableIndex ].YRes == 600 ) )
+      {
+        index++;
+      }
+/* Alan 10/19/2007; do the similiar adjustment like XGISearchCRT1Rate() */
+      if ( ( pVBInfo->RefIndex[ RefreshRateTableIndex ].XRes == 1024 ) &&
+           ( pVBInfo->RefIndex[ RefreshRateTableIndex ].YRes == 768 ) )
+      {
+        index++;
+      }
+      if ( ( pVBInfo->RefIndex[ RefreshRateTableIndex ].XRes == 1280 ) &&
+           ( pVBInfo->RefIndex[ RefreshRateTableIndex ].YRes == 1024 ) )
+      {
+        index++;
+      }
+    }
+
+    i = 0 ;
+    do
+    {
+        if ( pVBInfo->RefIndex[ RefreshRateTableIndex + i ].ModeID != ModeNo )
+            break ;
+        temp = pVBInfo->RefIndex[ RefreshRateTableIndex + i ].Ext_InfoFlag ;
+        temp &= ModeInfoFlag ;
+        if ( temp < pVBInfo->ModeType )
+            break ;
+        i++ ;
+        index-- ;
+
+    } while( index != 0xFFFF ) ;
+    if ( !( pVBInfo->VBInfo & SetCRT2ToRAMDAC ) )
+    {
+        if ( pVBInfo->VBInfo & SetInSlaveMode )
+        {
+            temp = pVBInfo->RefIndex[ RefreshRateTableIndex + i - 1 ].Ext_InfoFlag ;
+            if ( temp & InterlaceMode )
+            {
+                i++ ;
+            }
+        }
+    }
+    i-- ;
+    if ( ( pVBInfo->SetFlag & ProgrammingCRT2 ) )
+    {
+        temp = XGI_AjustCRT2Rate( ModeNo , ModeIdIndex , RefreshRateTableIndex , &i, pVBInfo) ;
+    }
+    return( RefreshRateTableIndex + i ) ;                /*return(0x01|(temp1<<1));   */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_AjustCRT2Rate */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_AjustCRT2Rate( USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex , USHORT *i, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempax ,
+           tempbx ,
+           resinfo ,
+           modeflag ,
+           infoflag ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;   /* si+St_ModeFlag */
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+    }
+
+    resinfo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ;
+    tempbx = pVBInfo->RefIndex[ RefreshRateTableIndex + ( *i ) ].ModeID ;
+    tempax = 0 ;
+
+    if ( pVBInfo->IF_DEF_LVDS == 0 )
+    {
+        if ( pVBInfo->VBInfo & SetCRT2ToRAMDAC )
+        {
+            tempax |= SupportRAMDAC2 ;
+
+            if ( pVBInfo->VBType & VB_XGI301C )
+                tempax |= SupportCRT2in301C ;
+        }
+
+        if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) )       /* 301b */
+        {
+            tempax |= SupportLCD ;
+
+            if ( pVBInfo->LCDResInfo != Panel1280x1024 )
+            {
+                if ( pVBInfo->LCDResInfo != Panel1280x960 )
+                {
+                    if ( pVBInfo->LCDInfo & LCDNonExpanding )
+                    {
+                        if ( resinfo >= 9 )
+                        {
+                            tempax = 0 ;
+                            return( 0 ) ;
+                        }
+                    }
+                }
+            }
+        }
+
+        if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )    /* for HiTV */
+        {
+            if ( ( pVBInfo->VBType & VB_XGI301LV ) && ( pVBInfo->VBExtInfo == VB_YPbPr1080i ) )
+            {
+                tempax |= SupportYPbPr ;
+                if ( pVBInfo->VBInfo & SetInSlaveMode )
+                {
+                    if ( resinfo == 4 )
+                        return( 0 ) ;
+
+                    if ( resinfo == 3 )
+                        return( 0 ) ;
+
+                    if ( resinfo > 7 )
+                        return( 0 ) ;
+                }
+            }
+            else
+            {
+                tempax |= SupportHiVisionTV ;
+                if ( pVBInfo->VBInfo & SetInSlaveMode )
+                {
+                    if ( resinfo == 4 )
+                        return( 0 ) ;
+
+                    if ( resinfo == 3 )
+                    {
+                        if ( pVBInfo->SetFlag & TVSimuMode )
+                            return( 0 ) ;
+                    }
+
+                    if ( resinfo > 7 )
+                        return( 0 ) ;
+                }
+            }
+        }
+        else
+        {
+            if ( pVBInfo->VBInfo & ( SetCRT2ToAVIDEO | SetCRT2ToSVIDEO | SetCRT2ToSCART | SetCRT2ToYPbPr | SetCRT2ToHiVisionTV ) )
+            {
+                tempax |= SupportTV ;
+
+                if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+                {
+                    tempax |= SupportTV1024 ;
+                }
+
+                if ( !( pVBInfo->VBInfo & SetPALTV ) )
+                {
+                    if ( modeflag & NoSupportSimuTV )
+                    {
+                        if ( pVBInfo->VBInfo & SetInSlaveMode )
+                        {
+                            if ( !( pVBInfo->VBInfo & SetNotSimuMode ) )
+                            {
+                               return( 0 ) ;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+    else                /* for LVDS */
+    {
+        if ( pVBInfo->IF_DEF_CH7005 == 1 )
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToTV )
+            {
+                tempax |= SupportCHTV ;
+            }
+        }
+
+        if ( pVBInfo->VBInfo & SetCRT2ToLCD )
+        {
+            tempax |= SupportLCD ;
+
+            if ( resinfo > 0x08 )
+                return( 0 ) ;           /* 1024x768 */
+
+            if ( pVBInfo->LCDResInfo < Panel1024x768 )
+            {
+                if ( resinfo > 0x07 )
+                    return( 0 ) ;       /* 800x600 */
+
+                if ( resinfo == 0x04 )
+                    return( 0 ) ;       /* 512x384 */
+            }
+        }
+    }
+
+    for( ; pVBInfo->RefIndex[ RefreshRateTableIndex + ( *i ) ].ModeID == tempbx ; ( *i )-- )
+    {
+        infoflag = pVBInfo->RefIndex[ RefreshRateTableIndex + ( *i ) ].Ext_InfoFlag ;
+        if ( infoflag & tempax )
+        {
+            return( 1 ) ;
+        }
+        if ( ( *i ) == 0 )
+            break ;
+    }
+
+    for( ( *i ) = 0 ; ; ( *i )++ )
+    {
+        infoflag = pVBInfo->RefIndex[ RefreshRateTableIndex + ( *i ) ].Ext_InfoFlag ;
+        if ( pVBInfo->RefIndex[ RefreshRateTableIndex + ( *i ) ].ModeID != tempbx )
+        {
+            return( 0 ) ;
+        }
+
+        if ( infoflag & tempax )
+        {
+            return( 1 ) ;
+        }
+    }
+    return( 1 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetSync */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetSync(USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT sync ,
+           temp ;
+
+    sync = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_InfoFlag >> 8 ;       /* di+0x00 */
+    sync &= 0xC0 ;
+    temp = 0x2F ;
+    temp |= sync ;
+    XGINew_SetReg3( pVBInfo->P3c2 , temp ) ;                            /* Set Misc(3c2) */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT1CRTC */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT1CRTC( USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex,PVB_DEVICE_INFO pVBInfo, PXGI_HW_DEVICE_INFO HwDeviceExtension )
+{
+    UCHAR  index ,
+           data ;
+
+    USHORT i ;
+
+    index = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT1CRTC ;   /* Get index */
+    index = index&IndexMask ;
+
+    data =( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x11 ) ;
+    data &= 0x7F ;
+    XGINew_SetReg1(pVBInfo->P3d4,0x11,data);                            /* Unlock CRTC */
+
+    for( i = 0 ; i < 8 ; i++ )
+        pVBInfo->TimingH[ 0 ].data[ i ] = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ i ] ;
+
+    for( i = 0 ; i < 7 ; i++ )
+        pVBInfo->TimingV[ 0 ].data[ i ] = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ i + 8 ] ;
+
+    XGI_SetCRT1Timing_H( pVBInfo, HwDeviceExtension ) ;
+
+
+
+    XGI_SetCRT1Timing_V( ModeIdIndex , ModeNo, pVBInfo ) ;
+
+
+    if( pVBInfo->ModeType > 0x03 )
+        XGINew_SetReg1( pVBInfo->P3d4 , 0x14 , 0x4F ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT1Timing_H */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT1Timing_H( PVB_DEVICE_INFO pVBInfo, PXGI_HW_DEVICE_INFO HwDeviceExtension )
+{
+    UCHAR data , data1, pushax;
+    USHORT i , j ;
+
+    /* XGINew_SetReg1( pVBInfo->P3d4 , 0x51 , 0 ) ; */
+    /* XGINew_SetReg1( pVBInfo->P3d4 , 0x56 , 0 ) ; */
+    /* XGINew_SetRegANDOR( pVBInfo->P3d4 ,0x11 , 0x7f , 0x00 ) ; */
+
+    data = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x11 ) ;            /* unlock cr0-7 */
+    data &= 0x7F ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x11 , data ) ;
+
+    data = pVBInfo->TimingH[ 0 ].data[ 0 ] ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0 , data ) ;
+
+    for( i = 0x01 ; i <= 0x04 ; i++ )
+    {
+        data = pVBInfo->TimingH[ 0 ].data[ i ] ;
+        XGINew_SetReg1( pVBInfo->P3d4 , ( USHORT )( i + 1 ) , data ) ;
+    }
+
+    for( i = 0x05 ; i <= 0x06 ; i++ )
+    {
+        data = pVBInfo->TimingH[ 0 ].data[ i ];
+        XGINew_SetReg1( pVBInfo->P3c4 ,( USHORT )( i + 6 ) , data ) ;
+    }
+
+    j = ( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x0e ) ;
+    j &= 0x1F ;
+    data = pVBInfo->TimingH[ 0 ].data[ 7 ] ;
+    data &= 0xE0 ;
+    data |= j ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x0e , data ) ;
+
+    if ( HwDeviceExtension->jChipType >= XG20 )
+    {
+        data = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x04 ) ;
+        data = data - 1 ;
+        XGINew_SetReg1( pVBInfo->P3d4 , 0x04 , data ) ;
+        data = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x05 ) ;
+        data1 = data ;
+        data1 &= 0xE0 ;
+        data &= 0x1F ;
+        if ( data == 0 )
+        {
+            pushax = data ;
+            data = ( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x0c ) ;
+            data &= 0xFB ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x0c , data ) ;
+            data = pushax ;
+        }
+        data = data - 1 ;
+        data |= data1 ;
+        XGINew_SetReg1( pVBInfo->P3d4 , 0x05 , data ) ;
+        data = ( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x0e ) ;
+        data = data >> 5 ;
+        data = data + 3 ;
+        if ( data > 7 )
+            data = data - 7 ;
+        data = data << 5 ;
+        XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0e , ~0xE0 , data ) ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT1Timing_V */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT1Timing_V( USHORT ModeIdIndex , USHORT ModeNo,PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR data ;
+    USHORT i , j ;
+
+    /* XGINew_SetReg1( pVBInfo->P3d4 , 0x51 , 0 ) ; */
+    /* XGINew_SetReg1( pVBInfo->P3d4 , 0x56 , 0 ) ; */
+    /* XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x11 , 0x7f , 0x00 ) ; */
+
+    for( i = 0x00 ; i <= 0x01 ; i++ )
+    {
+        data = pVBInfo->TimingV[ 0 ].data[ i ] ;
+        XGINew_SetReg1( pVBInfo->P3d4 , ( USHORT )( i + 6 ) , data ) ;
+    }
+
+    for( i = 0x02 ; i <= 0x03 ; i++ )
+    {
+        data = pVBInfo->TimingV[ 0 ].data[ i ] ;
+        XGINew_SetReg1( pVBInfo->P3d4 , ( USHORT )( i + 0x0e ) , data ) ;
+    }
+
+    for( i = 0x04 ; i <= 0x05 ; i++ )
+    {
+        data = pVBInfo->TimingV[ 0 ].data[ i ] ;
+        XGINew_SetReg1( pVBInfo->P3d4 , ( USHORT )( i + 0x11 ) , data ) ;
+    }
+
+    j = ( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x0a ) ;
+    j &= 0xC0 ;
+    data = pVBInfo->TimingV[ 0 ].data[ 6 ] ;
+    data &= 0x3F ;
+    data |= j ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x0a , data ) ;
+
+    data = pVBInfo->TimingV[ 0 ].data[ 6 ] ;
+    data &= 0x80 ;
+    data = data >> 2 ;
+
+    if ( ModeNo <= 0x13 )
+        i = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+    else
+        i = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+
+    i &= DoubleScanMode ;
+    if ( i )
+        data |= 0x80 ;
+
+    j = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x09 ) ;
+    j &= 0x5F ;
+    data |= j ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x09 , data ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetXG21CRTC */
+/* Input : Stand or enhance CRTC table */
+/* Output : Fill CRT Hsync/Vsync to SR2E/SR2F/SR30/SR33/SR34/SR3F */
+/* Description : Set LCD timing */
+/* --------------------------------------------------------------------- */
+void XGI_SetXG21CRTC(USHORT ModeNo, USHORT ModeIdIndex, USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo)
+{
+  UCHAR StandTableIndex, index, Tempax, Tempbx, Tempcx, Tempdx ;
+  USHORT Temp1, Temp2, Temp3 ;
+
+  if ( ModeNo <= 0x13 )
+  {
+    StandTableIndex = XGI_GetModePtr( ModeNo , ModeIdIndex, pVBInfo ) ;
+    Tempax = pVBInfo->StandTable[ StandTableIndex ].CRTC[ 4 ] ;         /* CR04 HRS */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2E , Tempax ) ;                   /* SR2E [7:0]->HRS */
+    Tempbx = pVBInfo->StandTable[ StandTableIndex ].CRTC[ 5 ] ;         /* Tempbx: CR05 HRE */
+    Tempbx &= 0x1F ;                                                    /* Tempbx: HRE[4:0] */
+    Tempcx = Tempax ;
+    Tempcx &=  0xE0 ;                                                   /* Tempcx: HRS[7:5] */
+    Tempdx = Tempcx | Tempbx ;                                          /* Tempdx(HRE): HRS[7:5]HRE[4:0] */
+    if ( Tempbx < ( Tempax & 0x1F ) )                                   /* IF HRE < HRS */
+      Tempdx |= 0x20 ;                                                  /* Tempdx: HRE = HRE + 0x20 */
+    Tempdx <<= 2 ;                                                      /* Tempdx << 2 */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2F , Tempdx ) ;                   /* SR2F [7:2]->HRE */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x30 , 0xE3 , 00 ) ;
+
+    Tempax = pVBInfo->StandTable[ StandTableIndex ].CRTC[ 16 ] ;        /* Tempax: CR16 VRS */
+    Tempbx = Tempax ;                                                   /* Tempbx=Tempax */
+    Tempax &= 0x01 ;                                                    /* Tempax: VRS[0] */
+    XGINew_SetRegOR( pVBInfo->P3c4 , 0x33 , Tempax ) ;                  /* SR33[0]->VRS */
+    Tempax = pVBInfo->StandTable[ StandTableIndex ].CRTC[ 7 ] ;         /* Tempax: CR7 VRS */
+    Tempdx = Tempbx >> 1 ;                                              /* Tempdx: VRS[7:1] */
+    Tempcx = Tempax & 0x04 ;                                            /* Tempcx: CR7[2] */
+    Tempcx <<= 5 ;                                                      /* Tempcx[7]: VRS[8] */
+    Tempdx |= Tempcx ;                                                  /* Tempdx: VRS[8:1] */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x34 , Tempdx ) ;                   /* SR34[7:0]: VRS[8:1] */
+
+    Temp1 = Tempcx << 1 ;                                               /* Temp1[8]: VRS[8] UCHAR -> USHORT */
+    Temp1 |= Tempbx ;                                                   /* Temp1[8:0]: VRS[8:0] */
+    Tempax &= 0x80 ;                                                    /* Tempax[7]: CR7[7] */
+    Temp2 = Tempax << 2 ;                                               /* Temp2[9]: VRS[9] */
+    Temp1 |= Temp2 ;                                                    /* Temp1[9:0]: VRS[9:0] */
+
+    Tempax = pVBInfo->StandTable[ StandTableIndex ].CRTC[ 17 ] ;        /* CR16 VRE */
+    Tempax &= 0x0F ;                                                    /* Tempax[3:0]: VRE[3:0] */
+    Temp2 = Temp1 & 0x3F0 ;                                             /* Temp2[9:4]: VRS[9:4] */
+    Temp2 |= Tempax ;                                                   /* Temp2[9:0]: VRE[9:0] */
+    Temp3 = Temp1 & 0x0F ;                                              /* Temp3[3:0]: VRS[3:0] */
+    if ( Tempax < Temp3 )                                               /* VRE[3:0]<VRS[3:0] */
+      Temp2 |= 0x10 ;                                                   /* Temp2: VRE + 0x10 */
+    Temp2 &= 0xFF ;                                                     /* Temp2[7:0]: VRE[7:0] */
+    Tempax = (UCHAR)Temp2 ;                                             /* Tempax[7:0]: VRE[7:0] */
+    Tempax <<= 2 ;                                                      /* Tempax << 2: VRE[5:0] */
+    Temp1 &= 0x600 ;                                                    /* Temp1[10:9]: VRS[10:9] */
+    Temp1 >>= 9 ;                                                       /* [10:9]->[1:0] */
+    Tempbx = (UCHAR)Temp1 ;                                             /* Tempbx[1:0]: VRS[10:9] */
+    Tempax |= Tempbx ;                                                  /* VRE[5:0]VRS[10:9] */
+    Tempax &= 0x7F ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x3F , Tempax ) ;                   /* SR3F D[7:2]->VRE D[1:0]->VRS */
+  }
+  else
+  {
+    index = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT1CRTC ;
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 3 ] ;             /* Tempax: CR4 HRS */
+    Tempcx = Tempax ;                                                   /* Tempcx: HRS */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2E , Tempax ) ;                   /* SR2E[7:0]->HRS */
+
+    Tempdx = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 5 ] ;             /* SRB */
+    Tempdx &= 0xC0 ;                                                    /* Tempdx[7:6]: SRB[7:6] */
+    Temp1 = Tempdx ;                                                    /* Temp1[7:6]: HRS[9:8] */
+    Temp1 <<= 2 ;                                                       /* Temp1[9:8]: HRS[9:8] */
+    Temp1 |= Tempax ;                                                   /* Temp1[9:0]: HRS[9:0] */
+
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 4 ] ;             /* CR5 HRE */
+    Tempax &= 0x1F ;                                                    /* Tempax[4:0]: HRE[4:0] */
+
+    Tempbx = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 6 ] ;             /* SRC */
+    Tempbx &= 0x04 ;                                                    /* Tempbx[2]: HRE[5] */
+    Tempbx <<= 3 ;                                                      /* Tempbx[5]: HRE[5] */
+    Tempax |= Tempbx ;                                                  /* Tempax[5:0]: HRE[5:0] */
+
+    Temp2 = Temp1 & 0x3C0 ;                                             /* Temp2[9:6]: HRS[9:6] */
+    Temp2 |= Tempax ;                                                   /* Temp2[9:0]: HRE[9:0] */
+
+    Tempcx &= 0x3F ;                                                    /* Tempcx[5:0]: HRS[5:0] */
+    if( Tempax < Tempcx )                                               /* HRE < HRS */
+      Temp2 |= 0x40 ;                                                   /* Temp2 + 0x40 */
+
+    Temp2 &= 0xFF ;
+    Tempax = (UCHAR)Temp2 ;                                             /* Tempax: HRE[7:0] */
+    Tempax <<= 2 ;                                                      /* Tempax[7:2]: HRE[5:0] */
+    Tempdx >>= 6 ;                                                      /* Tempdx[7:6]->[1:0] HRS[9:8] */
+    Tempax |= Tempdx ;                                                  /* HRE[5:0]HRS[9:8] */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2F , Tempax ) ;                   /* SR2F D[7:2]->HRE, D[1:0]->HRS */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x30 , 0xE3 , 00 ) ;
+
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 10 ] ;            /* CR10 VRS */
+    Tempbx = Tempax ;                                                   /* Tempbx: VRS */
+    Tempax &= 0x01 ;                                                    /* Tempax[0]: VRS[0] */
+    XGINew_SetRegOR( pVBInfo->P3c4 , 0x33 , Tempax ) ;                  /* SR33[0]->VRS[0] */
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 9 ] ;             /* CR7[2][7] VRE */
+    Tempcx = Tempbx >> 1 ;                                              /* Tempcx[6:0]: VRS[7:1] */
+    Tempdx = Tempax & 0x04 ;                                            /* Tempdx[2]: CR7[2] */
+    Tempdx <<= 5 ;                                                      /* Tempdx[7]: VRS[8] */
+    Tempcx |= Tempdx ;                                                  /* Tempcx[7:0]: VRS[8:1] */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x34 , Tempcx ) ;                   /* SR34[8:1]->VRS */
+
+    Temp1 = Tempdx ;                                                    /* Temp1[7]: Tempdx[7] */
+    Temp1 <<= 1 ;                                                       /* Temp1[8]: VRS[8] */
+    Temp1 |= Tempbx ;                                                   /* Temp1[8:0]: VRS[8:0] */
+    Tempax &= 0x80 ;
+    Temp2 = Tempax << 2 ;                                               /* Temp2[9]: VRS[9] */
+    Temp1 |= Temp2 ;                                                    /* Temp1[9:0]: VRS[9:0] */
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 14 ] ;            /* Tempax: SRA */
+    Tempax &= 0x08 ;                                                    /* Tempax[3]: VRS[3] */
+    Temp2 = Tempax ;
+    Temp2 <<= 7 ;                                                       /* Temp2[10]: VRS[10] */
+    Temp1 |= Temp2 ;                                                    /* Temp1[10:0]: VRS[10:0] */
+
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 11 ] ;            /* Tempax: CR11 VRE */
+    Tempax &= 0x0F ;                                                    /* Tempax[3:0]: VRE[3:0] */
+    Tempbx = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 14 ] ;            /* Tempbx: SRA */
+    Tempbx &= 0x20 ;                                                    /* Tempbx[5]: VRE[5] */
+    Tempbx >>= 1 ;                                                      /* Tempbx[4]: VRE[4] */
+    Tempax |= Tempbx ;                                                  /* Tempax[4:0]: VRE[4:0] */
+    Temp2 = Temp1 & 0x7E0 ;                                             /* Temp2[10:5]: VRS[10:5] */
+    Temp2 |= Tempax ;                                                   /* Temp2[10:5]: VRE[10:5] */
+
+    Temp3 = Temp1 & 0x1F ;                                              /* Temp3[4:0]: VRS[4:0] */
+    if ( Tempax < Temp3 )                                               /* VRE < VRS */
+      Temp2 |= 0x20 ;                                                   /* VRE + 0x20 */
+
+    Temp2 &= 0xFF ;
+    Tempax = (UCHAR)Temp2 ;                                             /* Tempax: VRE[7:0] */
+    Tempax <<= 2 ;                                                      /* Tempax[7:0]; VRE[5:0]00 */
+    Temp1 &= 0x600 ;                                                    /* Temp1[10:9]: VRS[10:9] */
+    Temp1 >>= 9 ;                                                       /* Temp1[1:0]: VRS[10:9] */
+    Tempbx = (UCHAR)Temp1 ;
+    Tempax |= Tempbx ;                                                  /* Tempax[7:0]: VRE[5:0]VRS[10:9] */
+    Tempax &= 0x7F ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x3F , Tempax ) ;                   /* SR3F D[7:2]->VRE D[1:0]->VRS */
+  }
+}
+
+void XGI_SetXG27CRTC(USHORT ModeNo, USHORT ModeIdIndex, USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo)
+{
+  USHORT StandTableIndex, index, Tempax, Tempbx, Tempcx, Tempdx ;
+
+  if ( ModeNo <= 0x13 )
+  {
+    StandTableIndex = XGI_GetModePtr( ModeNo , ModeIdIndex, pVBInfo ) ;
+    Tempax = pVBInfo->StandTable[ StandTableIndex ].CRTC[ 4 ] ;         /* CR04 HRS */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2E , Tempax ) ;                   /* SR2E [7:0]->HRS */
+    Tempbx = pVBInfo->StandTable[ StandTableIndex ].CRTC[ 5 ] ;         /* Tempbx: CR05 HRE */
+    Tempbx &= 0x1F ;                                                    /* Tempbx: HRE[4:0] */
+    Tempcx = Tempax ;
+    Tempcx &=  0xE0 ;                                                   /* Tempcx: HRS[7:5] */
+    Tempdx = Tempcx | Tempbx ;                                          /* Tempdx(HRE): HRS[7:5]HRE[4:0] */
+    if ( Tempbx < ( Tempax & 0x1F ) )                                   /* IF HRE < HRS */
+      Tempdx |= 0x20 ;                                                  /* Tempdx: HRE = HRE + 0x20 */
+    Tempdx <<= 2 ;                                                      /* Tempdx << 2 */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2F , Tempdx ) ;                   /* SR2F [7:2]->HRE */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x30 , 0xE3 , 00 ) ;
+
+    Tempax = pVBInfo->StandTable[ StandTableIndex ].CRTC[ 16 ] ;        /* Tempax: CR10 VRS */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x34 , Tempax ) ;                   /* SR34[7:0]->VRS */
+    Tempcx = Tempax ;                                                   /* Tempcx=Tempax=VRS[7:0] */
+    Tempax = pVBInfo->StandTable[ StandTableIndex ].CRTC[ 7 ] ;         /* Tempax[7][2]: CR7[7][2] VRS[9][8] */
+    Tempbx = Tempax ;                                                   /* Tempbx=CR07 */
+    Tempax &= 0x04 ;                                                    /* Tempax[2]: CR07[2] VRS[8] */
+    Tempax >>= 2;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x35 , ~0x01, Tempax ) ;        /* SR35 D[0]->VRS D[8] */
+    Tempcx |= (Tempax << 8) ;                                           /* Tempcx[8] |= VRS[8] */
+    Tempcx |= (Tempbx & 0x80)<<2;                                       /* Tempcx[9] |= VRS[9] */
+
+
+    Tempax = pVBInfo->StandTable[ StandTableIndex ].CRTC[ 17 ] ;        /* CR11 VRE */
+    Tempax &= 0x0F ;                                                    /* Tempax: VRE[3:0] */
+    Tempbx = Tempcx ;                                                   /* Tempbx=Tempcx=VRS[9:0] */
+    Tempbx &= 0x3F0 ;                                                   /* Tempbx[9:4]: VRS[9:4] */
+    Tempbx |= Tempax ;                                                  /* Tempbx[9:0]: VRE[9:0] */
+    if ( Tempax <= (Tempcx & 0x0F) )                                    /* VRE[3:0]<=VRS[3:0] */
+      Tempbx |= 0x10 ;                                                  /* Tempbx: VRE + 0x10 */
+    Tempax = (UCHAR)Tempbx & 0xFF;                                      /* Tempax[7:0]: VRE[7:0] */
+    Tempax <<= 2 ;                                                      /* Tempax << 2: VRE[5:0] */
+    Tempcx = (Tempcx&0x600)>>8;                                         /* Tempcx VRS[10:9] */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x3F , ~0xFC, Tempax ) ;        /* SR3F D[7:2]->VRE D[5:0] */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x35 , ~0x06, Tempcx ) ;        /* SR35 D[2:1]->VRS[10:9] */
+  }
+  else
+  {
+    index = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT1CRTC ;
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 3 ] ;             /* Tempax: CR4 HRS */
+    Tempbx = Tempax ;                                                   /* Tempbx: HRS[7:0] */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2E , Tempax ) ;                   /* SR2E[7:0]->HRS */
+
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 5 ] ;             /* SR0B */
+    Tempax &= 0xC0 ;                                                    /* Tempax[7:6]: SR0B[7:6]: HRS[9:8]*/
+    Tempbx |= (Tempax << 2);                                    /* Tempbx: HRS[9:0] */
+
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 4 ] ;             /* CR5 HRE */
+    Tempax &= 0x1F ;                                                    /* Tempax[4:0]: HRE[4:0] */
+    Tempcx = Tempax ;                                                   /* Tempcx: HRE[4:0] */
+
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 6 ] ;             /* SRC */
+    Tempax &= 0x04 ;                                                    /* Tempax[2]: HRE[5] */
+    Tempax <<= 3 ;                                                          /* Tempax[5]: HRE[5] */
+    Tempcx |= Tempax ;                                                  /* Tempcx[5:0]: HRE[5:0] */
+
+    Tempbx = Tempbx & 0x3C0 ;                                   /* Tempbx[9:6]: HRS[9:6] */
+    Tempbx |= Tempcx ;                                                  /* Tempbx: HRS[9:6]HRE[5:0] */
+
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 3 ] ;             /* Tempax: CR4 HRS */
+    Tempax &= 0x3F ;                                                    /* Tempax: HRS[5:0] */
+    if( Tempcx <= Tempax )                                              /* HRE[5:0] < HRS[5:0] */
+      Tempbx += 0x40 ;                          /* Tempbx= Tempbx + 0x40 : HRE[9:0]*/
+
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 5 ] ;             /* SR0B */
+    Tempax &= 0xC0 ;                                                    /* Tempax[7:6]: SR0B[7:6]: HRS[9:8]*/
+    Tempax >>= 6;                               /* Tempax[1:0]: HRS[9:8]*/
+    Tempax |= ((Tempbx << 2) & 0xFF);           /* Tempax[7:2]: HRE[5:0] */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2F , Tempax ) ;                   /* SR2F [7:2][1:0]: HRE[5:0]HRS[9:8] */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x30 , 0xE3 , 00 ) ;
+
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 10 ] ;            /* CR10 VRS */
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x34 , Tempax ) ;                   /* SR34[7:0]->VRS[7:0] */
+
+    Tempcx = Tempax ;                                                   /* Tempcx <= VRS[7:0] */
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 9 ] ;             /* CR7[7][2] VRS[9][8] */
+    Tempbx = Tempax ;                                                   /* Tempbx <= CR07[7:0] */
+    Tempax = Tempax & 0x04 ;                                    /* Tempax[2]: CR7[2]: VRS[8] */
+    Tempax >>= 2 ;                                                          /* Tempax[0]: VRS[8] */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x35 , ~0x01 , Tempax ) ;       /* SR35[0]: VRS[8] */
+    Tempcx |= (Tempax<<8) ;                                             /* Tempcx <= VRS[8:0] */
+    Tempcx |= ((Tempbx&0x80)<<2) ;                              /* Tempcx <= VRS[9:0] */
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 14 ] ;            /* Tempax: SR0A */
+    Tempax &= 0x08;                             /* SR0A[3] VRS[10] */
+    Tempcx |= (Tempax<<7) ;                                     /* Tempcx <= VRS[10:0] */
+
+
+    Tempax = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 11 ] ;            /* Tempax: CR11 VRE */
+    Tempax &= 0x0F ;                                                    /* Tempax[3:0]: VRE[3:0] */
+    Tempbx = pVBInfo->XGINEWUB_CRT1Table[ index ].CR[ 14 ] ;            /* Tempbx: SR0A */
+    Tempbx &= 0x20 ;                                                    /* Tempbx[5]: SR0A[5]: VRE[4] */
+    Tempbx >>= 1 ;                                                          /* Tempbx[4]: VRE[4] */
+    Tempax |= Tempbx ;                                                  /* Tempax[4:0]: VRE[4:0] */
+    Tempbx = Tempcx ;                                                   /* Tempbx: VRS[10:0] */
+    Tempbx &= 0x7E0 ;                                               /* Tempbx[10:5]: VRS[10:5] */
+    Tempbx |= Tempax ;                                                  /* Tempbx: VRS[10:5]VRE[4:0] */
+
+    if ( Tempbx <= Tempcx )                                             /* VRE <= VRS */
+      Tempbx |= 0x20 ;                                                  /* VRE + 0x20 */
+
+    Tempax = (Tempbx<<2) & 0xFF ;                                       /* Tempax: Tempax[7:0]; VRE[5:0]00 */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x3F , ~0xFC , Tempax ) ;       /* SR3F[7:2]:VRE[5:0] */
+    Tempax = Tempcx >> 8;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x35 , ~0x07 , Tempax ) ;       /* SR35[2:0]:VRS[10:8] */
+  }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetXG21LCD */
+/* Input : */
+/* Output : FCLK duty cycle, FCLK delay compensation */
+/* Description : All values set zero */
+/* --------------------------------------------------------------------- */
+void XGI_SetXG21LCD(PVB_DEVICE_INFO pVBInfo,USHORT RefreshRateTableIndex,USHORT ModeNo)
+{
+  USHORT Data , Temp , b3CC ;
+  USHORT XGI_P3cc ;
+
+  XGI_P3cc = pVBInfo->P3cc ;
+
+  XGINew_SetReg1( pVBInfo->P3d4 , 0x2E , 0x00 ) ;
+  XGINew_SetReg1( pVBInfo->P3d4 , 0x2F , 0x00 ) ;
+  XGINew_SetReg1( pVBInfo->P3d4 , 0x46 , 0x00 ) ;
+  XGINew_SetReg1( pVBInfo->P3d4 , 0x47 , 0x00 ) ;
+  if ( ((*pVBInfo->pDVOSetting)&0xC0) == 0xC0 )
+  {
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x2E , *pVBInfo->pCR2E ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x2F , *pVBInfo->pCR2F ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x46 , *pVBInfo->pCR46 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x47 , *pVBInfo->pCR47 ) ;
+  }
+
+  Temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x37 ) ;
+
+  if ( Temp & 0x01 )
+  {
+    XGINew_SetRegOR( pVBInfo->P3c4 , 0x06 , 0x40 ) ; /* 18 bits FP */
+    XGINew_SetRegOR( pVBInfo->P3c4 , 0x09 , 0x40 ) ;
+  }
+
+  XGINew_SetRegOR( pVBInfo->P3c4 , 0x1E , 0x01 ) ;   /* Negative blank polarity */
+
+  XGINew_SetRegAND( pVBInfo->P3c4 , 0x30 , ~0x20 ) ;
+  XGINew_SetRegAND( pVBInfo->P3c4 , 0x35 , ~0x80 ) ;
+
+  if ( ModeNo <= 0x13 )
+  {
+    b3CC = (UCHAR) XGINew_GetReg2( XGI_P3cc ) ;
+    if ( b3CC & 0x40 )
+      XGINew_SetRegOR( pVBInfo->P3c4 , 0x30 , 0x20 ) ; /* Hsync polarity */
+    if ( b3CC & 0x80 )
+      XGINew_SetRegOR( pVBInfo->P3c4 , 0x35 , 0x80 ) ; /* Vsync polarity */
+  }
+  else
+  {
+    Data = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_InfoFlag ;
+    if ( Data & 0x4000 )
+      XGINew_SetRegOR( pVBInfo->P3c4 , 0x30 , 0x20 ) ; /* Hsync polarity */
+    if ( Data & 0x8000 )
+      XGINew_SetRegOR( pVBInfo->P3c4 , 0x35 , 0x80 ) ; /* Vsync polarity */
+  }
+}
+
+void XGI_SetXG27LCD(PVB_DEVICE_INFO pVBInfo,USHORT RefreshRateTableIndex,USHORT ModeNo)
+{
+  USHORT Data , Temp , b3CC ;
+  USHORT XGI_P3cc ;
+
+  XGI_P3cc = pVBInfo->P3cc ;
+
+  XGINew_SetReg1( pVBInfo->P3d4 , 0x2E , 0x00 ) ;
+  XGINew_SetReg1( pVBInfo->P3d4 , 0x2F , 0x00 ) ;
+  XGINew_SetReg1( pVBInfo->P3d4 , 0x46 , 0x00 ) ;
+  XGINew_SetReg1( pVBInfo->P3d4 , 0x47 , 0x00 ) ;
+
+  Temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x37 ) ;
+  if ( ( Temp & 0x03 ) == 0 )  /* dual 12 */
+  {
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x46 , 0x13 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x47 , 0x13 ) ;
+  }
+
+  if ( ((*pVBInfo->pDVOSetting)&0xC0) == 0xC0 )
+  {
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x2E , *pVBInfo->pCR2E ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x2F , *pVBInfo->pCR2F ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x46 , *pVBInfo->pCR46 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x47 , *pVBInfo->pCR47 ) ;
+  }
+
+  XGI_SetXG27FPBits(pVBInfo);
+
+  XGINew_SetRegOR( pVBInfo->P3c4 , 0x1E , 0x01 ) ;   /* Negative blank polarity */
+
+  XGINew_SetRegAND( pVBInfo->P3c4 , 0x30 , ~0x20 ) ; /* Hsync polarity */
+  XGINew_SetRegAND( pVBInfo->P3c4 , 0x35 , ~0x80 ) ; /* Vsync polarity */
+
+  if ( ModeNo <= 0x13 )
+  {
+    b3CC = (UCHAR) XGINew_GetReg2( XGI_P3cc ) ;
+    if ( b3CC & 0x40 )
+      XGINew_SetRegOR( pVBInfo->P3c4 , 0x30 , 0x20 ) ; /* Hsync polarity */
+    if ( b3CC & 0x80 )
+      XGINew_SetRegOR( pVBInfo->P3c4 , 0x35 , 0x80 ) ; /* Vsync polarity */
+  }
+  else
+  {
+    Data = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_InfoFlag ;
+    if ( Data & 0x4000 )
+      XGINew_SetRegOR( pVBInfo->P3c4 , 0x30 , 0x20 ) ; /* Hsync polarity */
+    if ( Data & 0x8000 )
+      XGINew_SetRegOR( pVBInfo->P3c4 , 0x35 , 0x80 ) ; /* Vsync polarity */
+  }
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_UpdateXG21CRTC */
+/* Input : */
+/* Output : CRT1 CRTC */
+/* Description : Modify CRT1 Hsync/Vsync to fix LCD mode timing */
+/* --------------------------------------------------------------------- */
+void XGI_UpdateXG21CRTC( USHORT ModeNo , PVB_DEVICE_INFO pVBInfo , USHORT RefreshRateTableIndex )
+{
+  int i , index = -1;
+
+  XGINew_SetRegAND( pVBInfo->P3d4 , 0x11 , 0x7F ) ;             /* Unlock CR0~7 */
+  if ( ModeNo <= 0x13 )
+  {
+    for( i = 0 ; i < 12 ; i++ )
+    {
+      if ( ModeNo == pVBInfo->UpdateCRT1[ i ].ModeID )
+        index = i ;
+    }
+  }
+  else
+  {
+    if ( ModeNo == 0x2E && ( pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT1CRTC ==  RES640x480x60 ) )
+      index = 12 ;
+    else if ( ModeNo == 0x2E && ( pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT1CRTC == RES640x480x72 ) )
+      index = 13 ;
+    else if ( ModeNo == 0x2F )
+      index = 14 ;
+    else if ( ModeNo == 0x50 )
+      index = 15 ;
+    else if ( ModeNo == 0x59 )
+      index = 16 ;
+  }
+
+  if( index != -1 )
+  {
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x02 , pVBInfo->UpdateCRT1[ index ].CR02 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x03 , pVBInfo->UpdateCRT1[ index ].CR03 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x15 , pVBInfo->UpdateCRT1[ index ].CR15 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x16 , pVBInfo->UpdateCRT1[ index ].CR16 ) ;
+  }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT1DE */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT1DE( PXGI_HW_DEVICE_INFO HwDeviceExtension , USHORT ModeNo,USHORT ModeIdIndex , USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT resindex ,
+           tempax ,
+           tempbx ,
+           tempcx ,
+           temp ,
+           modeflag ;
+
+    UCHAR data ;
+
+    resindex = XGI_GetResInfo( ModeNo , ModeIdIndex, pVBInfo ) ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+        tempax = pVBInfo->StResInfo[ resindex ].HTotal ;
+        tempbx = pVBInfo->StResInfo[ resindex ].VTotal ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+        tempax = pVBInfo->ModeResInfo[ resindex ].HTotal ;
+        tempbx = pVBInfo->ModeResInfo[ resindex ].VTotal ;
+    }
+
+    if ( modeflag & HalfDCLK )
+        tempax = tempax >> 1 ;
+
+    if ( ModeNo > 0x13 )
+    {
+        if ( modeflag & HalfDCLK )
+            tempax = tempax << 1 ;
+
+        temp = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_InfoFlag ;
+
+        if ( temp & InterlaceMode )
+            tempbx = tempbx >> 1 ;
+
+        if ( modeflag & DoubleScanMode )
+            tempbx = tempbx << 1 ;
+    }
+
+    tempcx = 8 ;
+
+    /* if ( !( modeflag & Charx8Dot ) ) */
+    /* tempcx = 9 ; */
+
+    tempax /= tempcx ;
+    tempax -= 1 ;
+    tempbx -= 1 ;
+    tempcx = tempax ;
+    temp = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x11 ) ;
+    data = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x11 ) ;
+    data &= 0x7F ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x11 , data ) ;             /* Unlock CRTC */
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x01 , ( USHORT )( tempcx & 0xff ) ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x0b , ~0x0c , ( USHORT )( ( tempcx & 0x0ff00 ) >> 10 ) ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x12 , ( USHORT )( tempbx & 0xff ) ) ;
+    tempax = 0 ;
+    tempbx = tempbx >> 8 ;
+
+    if ( tempbx & 0x01 )
+        tempax |= 0x02 ;
+
+    if ( tempbx & 0x02 )
+        tempax |= 0x40 ;
+
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x07 , ~0x42 , tempax ) ;
+    data =( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x07 ) ;
+    data &= 0xFF ;
+    tempax = 0 ;
+
+    if ( tempbx & 0x04 )
+        tempax |= 0x02 ;
+
+    XGINew_SetRegANDOR( pVBInfo->P3d4 ,0x0a , ~0x02 , tempax ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x11 , temp ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetResInfo */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+USHORT  XGI_GetResInfo(USHORT ModeNo , USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT resindex ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        resindex = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ResInfo ;    /* si+St_ResInfo */
+    }
+    else
+    {
+        resindex = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ;   /* si+Ext_ResInfo */
+    }
+    return( resindex ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT1Offset */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT1Offset(  USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex , PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT temp ,
+           ah ,
+           al ,
+           temp2 ,
+           i ,
+           DisplayUnit ;
+
+    /* GetOffset */
+    temp = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeInfo ;
+    temp = temp >> 8 ;
+    temp = pVBInfo->ScreenOffset[ temp ] ;
+
+    temp2 = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_InfoFlag ;
+    temp2 &= InterlaceMode ;
+
+    if ( temp2 )
+        temp = temp << 1;
+
+    temp2 = pVBInfo->ModeType - ModeEGA ;
+
+    switch( temp2 )
+    {
+        case 0:
+            temp2 = 1 ;
+            break ;
+        case 1:
+            temp2 = 2 ;
+            break ;
+        case 2:
+            temp2 = 4 ;
+            break ;
+        case 3:
+            temp2 = 4 ;
+            break ;
+        case 4:
+            temp2 = 6 ;
+            break;
+        case 5:
+            temp2 = 8 ;
+            break ;
+        default:
+            break ;
+    }
+
+    if ( ( ModeNo >= 0x26 ) && ( ModeNo <= 0x28 ) )
+        temp = temp * temp2 + temp2 / 2 ;
+    else
+        temp *= temp2 ;
+
+    /* SetOffset */
+    DisplayUnit = temp ;
+    temp2 = temp ;
+    temp = temp >> 8 ;          /* ah */
+    temp &= 0x0F ;
+    i = XGINew_GetReg1( pVBInfo->P3c4 , 0x0E ) ;
+    i &= 0xF0 ;
+    i |= temp ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x0E , i ) ;
+
+    temp =( UCHAR )temp2 ;
+    temp &= 0xFF ;              /* al */
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x13 , temp ) ;
+
+    /* SetDisplayUnit */
+    temp2 = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_InfoFlag ;
+    temp2 &= InterlaceMode ;
+    if ( temp2 )
+        DisplayUnit >>= 1 ;
+
+    DisplayUnit = DisplayUnit << 5 ;
+    ah = ( DisplayUnit & 0xff00 ) >> 8 ;
+    al = DisplayUnit & 0x00ff ;
+    if ( al == 0 )
+        ah += 1 ;
+    else
+        ah += 2 ;
+
+    if ( HwDeviceExtension->jChipType >= XG20 )
+        if ( ( ModeNo == 0x4A ) | (ModeNo == 0x49 ) )
+            ah -= 1 ;
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x10 , ah ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT1VCLK */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT1VCLK( USHORT ModeNo , USHORT ModeIdIndex ,
+                        PXGI_HW_DEVICE_INFO HwDeviceExtension , USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR index , data ;
+    USHORT vclkindex ;
+
+    if ( pVBInfo->IF_DEF_LVDS == 1 )
+    {
+        index = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRTVCLK ;
+        data = XGINew_GetReg1( pVBInfo->P3c4 , 0x31 ) & 0xCF ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x31 , data ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2B , pVBInfo->VCLKData[ index ].SR2B ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2C , pVBInfo->VCLKData[ index ].SR2C ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2D , 0x01 ) ;
+    }
+    else if ( ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) ) && ( pVBInfo->VBInfo & SetCRT2ToLCDA ) )
+    {
+        vclkindex = XGI_GetVCLK2Ptr( ModeNo , ModeIdIndex , RefreshRateTableIndex , HwDeviceExtension, pVBInfo ) ;
+        data = XGINew_GetReg1( pVBInfo->P3c4 , 0x31 ) & 0xCF ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x31 , data ) ;
+        data = pVBInfo->VBVCLKData[ vclkindex ].Part4_A ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2B , data ) ;
+        data = pVBInfo->VBVCLKData[ vclkindex ].Part4_B ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2C , data ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2D , 0x01 ) ;
+    }
+    else
+    {
+        index = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRTVCLK ;
+        data = XGINew_GetReg1( pVBInfo->P3c4 , 0x31 ) & 0xCF ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x31 , data ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2B , pVBInfo->VCLKData[ index ].SR2B ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2C , pVBInfo->VCLKData[ index ].SR2C ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2D , 0x01 ) ;
+    }
+
+    if ( HwDeviceExtension->jChipType >= XG20 )
+    {
+        if ( pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag & HalfDCLK )
+        {
+        data = XGINew_GetReg1( pVBInfo->P3c4 , 0x2B ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2B , data ) ;
+        data = XGINew_GetReg1( pVBInfo->P3c4 , 0x2C ) ;
+        index = data ;
+        index &= 0xE0 ;
+        data &= 0x1F ;
+        data = data << 1 ;
+        data += 1 ;
+        data |= index ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2C , data ) ;
+        }
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT1FIFO */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT1FIFO( USHORT ModeNo , PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT data ;
+
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x3D ) ;
+    data &= 0xfe ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x3D , data ) ;     /* diable auto-threshold */
+
+    if ( ModeNo > 0x13 )
+    {
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x08 , 0x34 ) ;
+        data = XGINew_GetReg1( pVBInfo->P3c4 , 0x09 ) ;
+        data &= 0xC0 ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x09 , data | 0x30) ;
+        data = XGINew_GetReg1( pVBInfo->P3c4 , 0x3D ) ;
+        data |= 0x01 ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x3D , data ) ;
+    }
+    else
+    {
+        if (HwDeviceExtension->jChipType == XG27)
+        {
+          XGINew_SetReg1( pVBInfo->P3c4 , 0x08 , 0x0E ) ;
+          data = XGINew_GetReg1( pVBInfo->P3c4 , 0x09 ) ;
+          data &= 0xC0 ;
+          XGINew_SetReg1( pVBInfo->P3c4 , 0x09 , data | 0x20 ) ;
+        }
+        else
+        {
+          XGINew_SetReg1( pVBInfo->P3c4 , 0x08 , 0xAE ) ;
+          data = XGINew_GetReg1( pVBInfo->P3c4 , 0x09 ) ;
+          data &= 0xF0 ;
+          XGINew_SetReg1( pVBInfo->P3c4 , 0x09 , data ) ;
+        }
+    }
+
+    if (HwDeviceExtension->jChipType == XG21)
+    {
+        XGI_SetXG21FPBits(pVBInfo);                 /* Fix SR9[7:6] can't read back */
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT1ModeRegs */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT1ModeRegs( PXGI_HW_DEVICE_INFO HwDeviceExtension ,
+                            USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex,PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT data ,
+           data2 ,
+           data3 ,
+           infoflag = 0 ,
+           modeflag ,
+           resindex ,
+           xres ;
+
+    if ( ModeNo > 0x13 )
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+        infoflag = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_InfoFlag ;
+    }
+    else
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;    /* si+St_ModeFlag */
+
+    if ( XGINew_GetReg1( pVBInfo->P3d4 , 0x31 ) & 0x01 )
+      XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x1F , 0x3F , 0x00 ) ;
+
+    if ( ModeNo > 0x13 )
+        data = infoflag ;
+    else
+        data = 0 ;
+
+    data2 = 0 ;
+
+    if ( ModeNo > 0x13 )
+    {
+        if ( pVBInfo->ModeType > 0x02 )
+        {
+            data2 |= 0x02 ;
+            data3 = pVBInfo->ModeType - ModeVGA ;
+            data3 = data3 << 2 ;
+            data2 |= data3 ;
+        }
+    }
+
+    data &= InterlaceMode ;
+
+    if ( data )
+        data2 |= 0x20 ;
+
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x06 , ~0x3F , data2 ) ;
+    /* XGINew_SetReg1(pVBInfo->P3c4,0x06,data2); */
+    resindex = XGI_GetResInfo( ModeNo , ModeIdIndex, pVBInfo ) ;
+    if ( ModeNo <= 0x13 )
+        xres = pVBInfo->StResInfo[ resindex ].HTotal ;
+    else
+        xres = pVBInfo->ModeResInfo[ resindex ].HTotal ;                         /* xres->ax */
+
+    data = 0x0000 ;
+    if ( infoflag & InterlaceMode )
+    {
+        if ( xres == 1024 )
+            data = 0x0035 ;
+        else if ( xres == 1280 )
+            data = 0x0048 ;
+    }
+
+    data2 = data & 0x00FF ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x19 , 0xFF , data2 ) ;
+    data2 = ( data & 0xFF00 ) >> 8 ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x19 , 0xFC , data2 ) ;
+
+    if( modeflag & HalfDCLK )
+        XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x01 , 0xF7 , 0x08 ) ;
+
+    data2 = 0 ;
+
+    if ( modeflag & LineCompareOff )
+        data2 |= 0x08 ;
+
+    if ( ModeNo > 0x13 )
+    {
+        if ( pVBInfo->ModeType == ModeEGA )
+            data2 |= 0x40 ;
+    }
+
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0F , ~0x48 , data2 ) ;
+    data = 0x60 ;
+    if ( pVBInfo->ModeType != ModeText )
+    {
+        data = data ^ 0x60 ;
+        if ( pVBInfo->ModeType != ModeEGA )
+        {
+            data = data ^ 0xA0 ;
+        }
+    }
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x21 , 0x1F , data ) ;
+
+    XGI_SetVCLKState( HwDeviceExtension , ModeNo , RefreshRateTableIndex, pVBInfo) ;
+
+    /* if(modeflag&HalfDCLK)//030305 fix lowresolution bug */
+    /* if(XGINew_IF_DEF_NEW_LOWRES) */
+    /* XGI_VesaLowResolution(ModeNo,ModeIdIndex);//030305 fix lowresolution bug */
+
+    data=XGINew_GetReg1( pVBInfo->P3d4 , 0x31 ) ;
+
+    if (HwDeviceExtension->jChipType == XG27 )
+    {
+        if ( data & 0x40 )
+            data = 0x2c ;
+        else
+            data = 0x6c ;
+        XGINew_SetReg1( pVBInfo->P3d4 , 0x52 , data ) ;
+        XGINew_SetRegOR( pVBInfo->P3d4 , 0x51 , 0x10 ) ;
+    }
+    else
+    if (HwDeviceExtension->jChipType >= XG20 )
+    {
+        if ( data & 0x40 )
+            data = 0x33 ;
+        else
+            data = 0x73 ;
+        XGINew_SetReg1( pVBInfo->P3d4 , 0x52 , data ) ;
+        XGINew_SetReg1( pVBInfo->P3d4 , 0x51 , 0x02 ) ;
+    }
+    else
+    {
+    if ( data & 0x40 )
+        data = 0x2c ;
+    else
+        data = 0x6c ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x52 , data ) ;
+    }
+
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetVCLKState */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetVCLKState(  PXGI_HW_DEVICE_INFO HwDeviceExtension , USHORT ModeNo , USHORT RefreshRateTableIndex,PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT data ,
+           data2 = 0 ;
+    SHORT  VCLK ;
+
+    UCHAR  index ;
+
+    if ( ModeNo <= 0x13 )
+        VCLK = 0 ;
+    else
+    {
+        index = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRTVCLK ;
+        index &= IndexMask ;
+        VCLK = pVBInfo->VCLKData[ index ].CLOCK ;
+    }
+
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x32 ) ;
+    data &= 0xf3 ;
+    if ( VCLK >= 200 )
+        data |= 0x0c ;  /* VCLK > 200 */
+
+    if ( HwDeviceExtension->jChipType >= XG20 )
+        data &= ~0x04 ; /* 2 pixel mode */
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x32 , data ) ;
+
+    if ( HwDeviceExtension->jChipType < XG20 )
+    {
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x1F ) ;
+    data &= 0xE7 ;
+    if ( VCLK < 200 )
+        data |= 0x10 ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x1F , data ) ;
+    }
+
+/*  Jong for Adavantech LCD ripple issue
+    if ( ( VCLK >= 0 ) && ( VCLK < 135 ) )
+        data2 = 0x03 ;
+    else if ( ( VCLK >= 135 ) && ( VCLK < 160 ) )
+        data2 = 0x02 ;
+    else if ( ( VCLK >= 160 ) && ( VCLK < 260 ) )
+        data2 = 0x01 ;
+    else if ( VCLK > 260 )
+          data2 = 0x00 ;
+*/
+    data2 = 0x00 ;
+
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x07 , 0xFC , data2 ) ;
+    if (HwDeviceExtension->jChipType >= XG27 )
+    {
+      XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x40 , 0xFC , data2&0x03 ) ;
+    }
+
+
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_VesaLowResolution */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+/*void XGI_VesaLowResolution( USHORT ModeNo , USHORT ModeIdIndex ,PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT modeflag;
+
+    if ( ModeNo > 0x13 )
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+    else
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+
+    if ( ModeNo > 0x13 )
+    {
+        if ( modeflag & DoubleScanMode )
+        {
+            if ( modeflag & HalfDCLK )
+            {
+                if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+                {
+                    if ( !( pVBInfo->VBInfo & SetCRT2ToRAMDAC ) )
+                    {
+                        if ( pVBInfo->VBInfo & SetInSlaveMode )
+                        {
+                            XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x01 , 0xf7 , 0x00 ) ;
+                            XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0f , 0x7f , 0x00 ) ;
+                            return ;
+                        }
+                    }
+                }
+                XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0f , 0xff , 0x80 ) ;
+                XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x01 , 0xf7 , 0x00 ) ;
+                return ;
+            }
+        }
+    }
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0f , 0x7f , 0x00 ) ;
+}
+*/
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_LoadDAC */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_LoadDAC( USHORT ModeNo , USHORT ModeIdIndex,PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT data , data2 , time ,
+           i  , j , k , m , n , o ,
+           si , di , bx , dl , al , ah , dh ,
+           *table = NULL ;
+
+    if ( ModeNo <= 0x13 )
+        data = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+    else
+        data = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+
+    data &= DACInfoFlag ;
+    time = 64 ;
+
+    if ( data == 0x00 )
+        table = XGINew_MDA_DAC ;
+    else if ( data == 0x08 )
+        table = XGINew_CGA_DAC ;
+    else if ( data == 0x10 )
+        table = XGINew_EGA_DAC ;
+    else if ( data == 0x18 )
+    {
+        time = 256 ;
+        table = XGINew_VGA_DAC ;
+    }
+
+    if ( time == 256 )
+        j = 16 ;
+    else
+        j = time ;
+
+    XGINew_SetReg3( pVBInfo->P3c6 , 0xFF ) ;
+    XGINew_SetReg3( pVBInfo->P3c8 , 0x00 ) ;
+
+    for( i = 0 ; i < j ; i++ )
+    {
+        data = table[ i ] ;
+
+        for( k = 0 ; k < 3 ; k++ )
+        {
+            data2 = 0 ;
+
+            if ( data & 0x01 )
+                data2 = 0x2A ;
+
+            if ( data & 0x02 )
+                data2 += 0x15 ;
+
+            XGINew_SetReg3( pVBInfo->P3c9 , data2 ) ;
+            data = data >> 2 ;
+        }
+    }
+
+    if ( time == 256 )
+    {
+        for( i = 16 ; i < 32 ; i++ )
+        {
+            data = table[ i ] ;
+
+            for( k = 0 ; k < 3 ; k++ )
+                XGINew_SetReg3( pVBInfo->P3c9 , data ) ;
+        }
+
+        si = 32 ;
+
+        for( m = 0 ; m < 9 ; m++ )
+        {
+            di = si ;
+            bx = si + 0x04 ;
+            dl = 0 ;
+
+            for( n = 0 ; n < 3 ; n++ )
+            {
+                for( o = 0 ; o < 5 ; o++ )
+                {
+                    dh = table[ si ] ;
+                    ah = table[ di ] ;
+                    al = table[ bx ] ;
+                    si++ ;
+                    XGI_WriteDAC( dl , ah , al , dh, pVBInfo ) ;
+                }
+
+                si -= 2 ;
+
+                for( o = 0 ; o < 3 ; o++ )
+                {
+                    dh = table[ bx ] ;
+                    ah = table[ di ] ;
+                    al = table[ si ] ;
+                    si-- ;
+                    XGI_WriteDAC( dl , ah , al , dh, pVBInfo ) ;
+                }
+
+                dl++ ;
+            }
+
+            si += 5 ;
+        }
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_WriteDAC */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_WriteDAC( USHORT dl , USHORT ah , USHORT al , USHORT dh,PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT temp , bh , bl ;
+
+    bh = ah ;
+    bl = al ;
+
+    if ( dl != 0 )
+    {
+        temp = bh ;
+        bh = dh ;
+        dh = temp ;
+        if ( dl == 1 )
+        {
+            temp = bl ;
+            bl = dh ;
+            dh = temp ;
+        }
+        else
+        {
+            temp = bl ;
+            bl = bh ;
+            bh = temp ;
+        }
+    }
+    XGINew_SetReg3( pVBInfo->P3c9 , ( USHORT )dh ) ;
+    XGINew_SetReg3( pVBInfo->P3c9 , ( USHORT )bh ) ;
+    XGINew_SetReg3( pVBInfo->P3c9 , ( USHORT )bl ) ;
+}
+
+#if 0
+/* --------------------------------------------------------------------- */
+/* Function : XGI_ClearBuffer */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_ClearBuffer( PXGI_HW_DEVICE_INFO HwDeviceExtension , USHORT ModeNo, PVB_DEVICE_INFO  pVBInfo)
+{
+    PVOID VideoMemoryAddress = ( PVOID )HwDeviceExtension->pjVideoMemoryAddress ;
+    ULONG AdapterMemorySize  = ( ULONG )HwDeviceExtension->ulVideoMemorySize ;
+    PUSHORT pBuffer ;
+#ifndef LINUX_XF86
+    int i ;
+#endif
+
+    if ( pVBInfo->ModeType >= ModeEGA )
+    {
+        if ( ModeNo > 0x13 )
+        {
+            AdapterMemorySize = 0x40000 ;       /* clear 256k */
+            /* GetDRAMSize( HwDeviceExtension ) ; */
+            XGI_SetMemory( VideoMemoryAddress , AdapterMemorySize , 0 ) ;
+        }
+        else
+        {
+/*
+            pBuffer = VideoMemoryAddress ;
+            for( i = 0 ; i < 0x4000 ; i++ )
+                pBuffer[ i ] = 0x0000 ;
+*/
+        }
+    }
+    else
+    {
+        pBuffer = VideoMemoryAddress ;
+        if ( pVBInfo->ModeType < ModeCGA )
+        {
+/*
+            for ( i = 0 ; i < 0x4000 ; i++ )
+                pBuffer[ i ] = 0x0720 ;
+*/
+        }
+        else
+            XGI_SetMemory( VideoMemoryAddress , 0x8000 , 0 ) ;
+    }
+}
+
+#endif
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetLCDAGroup */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetLCDAGroup( USHORT ModeNo , USHORT ModeIdIndex , PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO  pVBInfo )
+{
+    USHORT RefreshRateTableIndex ;
+    /* USHORT temp ; */
+
+    /* pVBInfo->SelectCRT2Rate = 0 ; */
+
+    pVBInfo->SetFlag |= ProgrammingCRT2 ;
+    RefreshRateTableIndex = XGI_GetRatePtrCRT2( HwDeviceExtension, ModeNo , ModeIdIndex, pVBInfo ) ;
+    XGI_GetLVDSResInfo(  ModeNo , ModeIdIndex,  pVBInfo ) ;
+    XGI_GetLVDSData( ModeNo , ModeIdIndex , RefreshRateTableIndex,  pVBInfo);
+    XGI_ModCRT1Regs( ModeNo , ModeIdIndex , RefreshRateTableIndex , HwDeviceExtension, pVBInfo ) ;
+    XGI_SetLVDSRegs( ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+    XGI_SetCRT2ECLK( ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetLVDSResInfo */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetLVDSResInfo( USHORT ModeNo , USHORT ModeIdIndex,PVB_DEVICE_INFO  pVBInfo )
+{
+    USHORT resindex , xres , yres , modeflag ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ResInfo ;    /* si+St_ResInfo */
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ;   /* si+Ext_ResInfo */
+    }
+
+
+    /* if ( ModeNo > 0x13 ) */
+    /* modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ; */
+    /* else */
+    /* modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ; */
+
+    if ( ModeNo <= 0x13 )
+    {
+        resindex = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ResInfo ;    /* si+St_ResInfo */
+    }
+    else
+    {
+        resindex = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ;   /* si+Ext_ResInfo */
+    }
+
+    /* resindex = XGI_GetResInfo( ModeNo , ModeIdIndex, pVBInfo ) ; */
+
+    if ( ModeNo <= 0x13 )
+    {
+        xres = pVBInfo->StResInfo[ resindex ].HTotal ;
+        yres = pVBInfo->StResInfo[ resindex ].VTotal ;
+    }
+    else
+    {
+        xres = pVBInfo->ModeResInfo[ resindex ].HTotal ;
+        yres = pVBInfo->ModeResInfo[ resindex ].VTotal ;
+    }
+    if ( ModeNo > 0x13 )
+    {
+        if ( modeflag & HalfDCLK )
+            xres = xres << 1 ;
+
+        if ( modeflag & DoubleScanMode )
+            yres = yres << 1 ;
+    }
+    /* if ( modeflag & Charx8Dot ) */
+    /* { */
+
+    if ( xres == 720 )
+        xres = 640 ;
+
+    /* } */
+    pVBInfo->VGAHDE = xres ;
+    pVBInfo->HDE = xres ;
+    pVBInfo->VGAVDE = yres ;
+    pVBInfo->VDE = yres ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetLVDSData */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetLVDSData(  USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex, PVB_DEVICE_INFO  pVBInfo )
+{
+    USHORT tempbx ;
+    XGI330_LVDSDataStruct *LCDPtr = NULL ;
+    XGI330_CHTVDataStruct  *TVPtr = NULL ;
+
+    tempbx = 2 ;
+
+    if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) )
+    {
+        LCDPtr = ( XGI330_LVDSDataStruct * )XGI_GetLcdPtr( tempbx, ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo) ;
+        pVBInfo->VGAHT = LCDPtr->VGAHT ;
+        pVBInfo->VGAVT = LCDPtr->VGAVT ;
+        pVBInfo->HT = LCDPtr->LCDHT ;
+        pVBInfo->VT = LCDPtr->LCDVT ;
+    }
+    if ( pVBInfo->IF_DEF_CH7017 == 1 )
+    {
+        if ( pVBInfo->VBInfo & SetCRT2ToTV )
+        {
+            TVPtr = ( XGI330_CHTVDataStruct * )XGI_GetTVPtr( tempbx , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+            pVBInfo->VGAHT = TVPtr->VGAHT ;
+            pVBInfo->VGAVT = TVPtr->VGAVT ;
+            pVBInfo->HT = TVPtr->LCDHT ;
+            pVBInfo->VT = TVPtr->LCDVT ;
+        }
+    }
+
+    if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) )
+    {
+        if ( !( pVBInfo->LCDInfo & ( SetLCDtoNonExpanding | EnableScalingLCD ) ) )
+        {
+            if ( ( pVBInfo->LCDResInfo == Panel1024x768 ) || ( pVBInfo->LCDResInfo == Panel1024x768x75 ) )
+            {
+                pVBInfo->HDE = 1024 ;
+                pVBInfo->VDE = 768 ;
+            }
+            else if ( ( pVBInfo->LCDResInfo == Panel1280x1024 ) || ( pVBInfo->LCDResInfo == Panel1280x1024x75 ) )
+            {
+                pVBInfo->HDE = 1280 ;
+                pVBInfo->VDE = 1024 ;
+            }
+            else if ( pVBInfo->LCDResInfo == Panel1400x1050 )
+            {
+                pVBInfo->HDE = 1400 ;
+                pVBInfo->VDE = 1050 ;
+            }
+            else
+            {
+                pVBInfo->HDE = 1600 ;
+                pVBInfo->VDE = 1200 ;
+            }
+        }
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_ModCRT1Regs */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_ModCRT1Regs( USHORT ModeNo , USHORT ModeIdIndex ,
+                        USHORT RefreshRateTableIndex , PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO  pVBInfo )
+{
+    UCHAR index ;
+    USHORT tempbx , i ;
+    XGI_LVDSCRT1HDataStruct  *LCDPtr = NULL ;
+    XGI_LVDSCRT1VDataStruct  *LCDPtr1 =NULL ;
+    /* XGI330_CHTVDataStruct *TVPtr = NULL ; */
+    XGI_CH7007TV_TimingHStruct *CH7007TV_TimingHPtr = NULL;
+    XGI_CH7007TV_TimingVStruct *CH7007TV_TimingVPtr = NULL;
+
+    if( ModeNo <= 0x13 )
+        index = pVBInfo->SModeIDTable[ ModeIdIndex ].St_CRT2CRTC ;
+    else
+        index = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
+
+    index= index & IndexMask ;
+
+    if ( ( pVBInfo->IF_DEF_ScaleLCD == 0 ) || ( ( pVBInfo->IF_DEF_ScaleLCD == 1 ) && ( !( pVBInfo->LCDInfo & EnableScalingLCD ) ) ) )
+    {
+        tempbx = 0 ;
+
+        if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) )
+        {
+            LCDPtr = ( XGI_LVDSCRT1HDataStruct * )XGI_GetLcdPtr( tempbx , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+
+            for( i = 0 ; i < 8 ; i++ )
+                pVBInfo->TimingH[ 0 ].data[ i ] = LCDPtr[ 0 ].Reg[ i ] ;
+        }
+
+        if ( pVBInfo->IF_DEF_CH7007 == 1 )
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToTV )
+            {
+                CH7007TV_TimingHPtr = ( XGI_CH7007TV_TimingHStruct *)XGI_GetTVPtr( tempbx , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+
+                for( i = 0 ; i < 8 ; i++ )
+                    pVBInfo->TimingH[ 0 ].data[ i ] = CH7007TV_TimingHPtr[ 0 ].data[ i ] ;
+            }
+        }
+
+        /* if ( pVBInfo->IF_DEF_CH7017 == 1 )
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToTV )
+                TVPtr = ( XGI330_CHTVDataStruct *)XGI_GetTVPtr( tempbx , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+        } */
+
+        XGI_SetCRT1Timing_H(pVBInfo,HwDeviceExtension) ;
+
+        if ( pVBInfo->IF_DEF_CH7007 == 1 )
+        {
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x2E , CH7007TV_TimingHPtr[ 0 ].data[ 8 ] ) ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x2F , CH7007TV_TimingHPtr[ 0 ].data[ 9 ] ) ;
+        }
+
+        tempbx = 1 ;
+
+        if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) )
+        {
+            LCDPtr1 = ( XGI_LVDSCRT1VDataStruct * )XGI_GetLcdPtr( tempbx , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+            for( i = 0 ; i < 7 ; i++ )
+                pVBInfo->TimingV[ 0 ].data[ i ] = LCDPtr1[ 0 ].Reg[ i ] ;
+        }
+
+        if ( pVBInfo->IF_DEF_CH7007 == 1 )
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToTV )
+            {
+                CH7007TV_TimingVPtr = ( XGI_CH7007TV_TimingVStruct *)XGI_GetTVPtr( tempbx , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+
+                for( i = 0 ; i < 7 ; i++ )
+                    pVBInfo->TimingV[ 0 ].data[ i ] = CH7007TV_TimingVPtr[ 0 ].data[ i ] ;
+            }
+        }
+        /* if ( pVBInfo->IF_DEF_CH7017 == 1 )
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToTV )
+                TVPtr = ( XGI330_CHTVDataStruct *)XGI_GetTVPtr( tempbx , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+        } */
+
+        XGI_SetCRT1Timing_V( ModeIdIndex , ModeNo , pVBInfo) ;
+
+        if ( pVBInfo->IF_DEF_CH7007 == 1 )
+        {
+            XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x33 , ~0x01 , CH7007TV_TimingVPtr[ 0 ].data[ 7 ]&0x01 ) ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x34 , CH7007TV_TimingVPtr[ 0 ].data[8 ] ) ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x3F , CH7007TV_TimingVPtr[ 0 ].data[9 ] ) ;
+
+        }
+    }
+}
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetLVDSRegs */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetLVDSRegs( USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex, PVB_DEVICE_INFO  pVBInfo )
+{
+    USHORT tempbx , tempax , tempcx , tempdx , push1 , push2 , modeflag ;
+    unsigned long temp , temp1 , temp2 , temp3 , push3 ;
+    XGI330_LCDDataDesStruct  *LCDPtr = NULL ;
+    XGI330_LCDDataDesStruct2  *LCDPtr1 = NULL ;
+
+    if ( ModeNo > 0x13 )
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+    else
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+
+    if ( !( pVBInfo->SetFlag & Win9xDOSMode ) )
+    {
+        if ( ( pVBInfo->IF_DEF_CH7017 == 0 ) || ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) ) )
+        {
+            if ( pVBInfo->IF_DEF_OEMUtil == 1 )
+            {
+                tempbx = 8 ;
+                LCDPtr = ( XGI330_LCDDataDesStruct * )XGI_GetLcdPtr( tempbx , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+            }
+
+            if ( ( pVBInfo->IF_DEF_OEMUtil == 0 ) || ( LCDPtr == 0 ) )
+            {
+                tempbx = 3 ;
+                if ( pVBInfo->LCDInfo & EnableScalingLCD )
+                    LCDPtr1 = ( XGI330_LCDDataDesStruct2 * )XGI_GetLcdPtr( tempbx , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+                else
+                    LCDPtr = ( XGI330_LCDDataDesStruct * )XGI_GetLcdPtr( tempbx , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+            }
+
+            XGI_GetLCDSync( &tempax , &tempbx ,pVBInfo) ;
+            push1 = tempbx ;
+            push2 = tempax ;
+
+            /* GetLCDResInfo */
+            if ( ( pVBInfo->LCDResInfo == Panel1024x768 ) || ( pVBInfo->LCDResInfo == Panel1024x768x75 ) )
+            {
+                tempax = 1024 ;
+                tempbx = 768 ;
+            }
+            else if ( ( pVBInfo->LCDResInfo == Panel1280x1024 ) || ( pVBInfo->LCDResInfo == Panel1280x1024x75 ) )
+            {
+                tempax = 1280 ;
+                tempbx = 1024 ;
+            }
+            else if ( pVBInfo->LCDResInfo == Panel1400x1050 )
+            {
+                tempax = 1400 ;
+                tempbx = 1050 ;
+            }
+            else
+            {
+                tempax = 1600 ;
+                tempbx = 1200 ;
+            }
+
+            if ( pVBInfo->LCDInfo & SetLCDtoNonExpanding )
+            {
+                pVBInfo->HDE=tempax;
+                pVBInfo->VDE=tempbx;
+                pVBInfo->VGAHDE=tempax;
+                pVBInfo->VGAVDE=tempbx;
+            }
+
+            if ( ( pVBInfo->IF_DEF_ScaleLCD == 1 ) && ( pVBInfo->LCDInfo & EnableScalingLCD ) )
+            {
+                tempax=pVBInfo->HDE;
+                tempbx=pVBInfo->VDE;
+            }
+
+            tempax = pVBInfo->HT ;
+
+            if ( pVBInfo->LCDInfo & EnableScalingLCD )
+                tempbx = LCDPtr1->LCDHDES ;
+            else
+                tempbx = LCDPtr->LCDHDES ;
+
+            tempcx = pVBInfo->HDE ;
+            tempbx = tempbx & 0x0fff ;
+            tempcx += tempbx ;
+
+            if ( tempcx >= tempax )
+                tempcx -= tempax ;
+
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x1A , tempbx & 0x07 ) ;
+
+            tempcx = tempcx >> 3 ;
+            tempbx = tempbx >> 3 ;
+
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x16 , ( USHORT )( tempbx & 0xff ) ) ;
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x17 , ( USHORT )( tempcx & 0xff ) ) ;
+
+            tempax = pVBInfo->HT ;
+
+            if ( pVBInfo->LCDInfo & EnableScalingLCD )
+                tempbx = LCDPtr1->LCDHRS ;
+            else
+                tempbx = LCDPtr->LCDHRS ;
+
+            tempcx = push2 ;
+
+            if ( pVBInfo->LCDInfo & EnableScalingLCD )
+                tempcx = LCDPtr1->LCDHSync ;
+
+            tempcx += tempbx ;
+
+            if ( tempcx >= tempax )
+                tempcx -= tempax ;
+
+            tempax = tempbx & 0x07 ;
+            tempax = tempax >> 5 ;
+            tempcx = tempcx >> 3 ;
+            tempbx = tempbx >> 3 ;
+
+            tempcx &= 0x1f ;
+            tempax |= tempcx ;
+
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x15 , tempax ) ;
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x14 , ( USHORT )( tempbx & 0xff ) ) ;
+
+            tempax = pVBInfo->VT ;
+            if ( pVBInfo->LCDInfo & EnableScalingLCD )
+                tempbx = LCDPtr1->LCDVDES ;
+            else
+                tempbx = LCDPtr->LCDVDES ;
+            tempcx = pVBInfo->VDE ;
+
+            tempbx = tempbx & 0x0fff ;
+            tempcx += tempbx ;
+            if ( tempcx >= tempax )
+                tempcx -= tempax ;
+
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x1b , ( USHORT )( tempbx & 0xff ) ) ;
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x1c , ( USHORT )( tempcx & 0xff ) ) ;
+
+            tempbx = ( tempbx >> 8 ) & 0x07 ;
+            tempcx = ( tempcx >> 8 ) & 0x07 ;
+
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x1d , ( USHORT )( ( tempcx << 3 ) | tempbx ) ) ;
+
+            tempax = pVBInfo->VT ;
+            if ( pVBInfo->LCDInfo & EnableScalingLCD )
+                tempbx = LCDPtr1->LCDVRS ;
+            else
+                tempbx = LCDPtr->LCDVRS ;
+
+            /* tempbx = tempbx >> 4 ; */
+              tempcx = push1 ;
+
+            if ( pVBInfo->LCDInfo & EnableScalingLCD )
+                tempcx = LCDPtr1->LCDVSync ;
+
+            tempcx += tempbx ;
+            if ( tempcx >= tempax )
+                tempcx -= tempax ;
+
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x18 , ( USHORT )( tempbx & 0xff ) ) ;
+            XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x19 , ~0x0f , ( USHORT )( tempcx & 0x0f ) ) ;
+
+            tempax = ( ( tempbx >> 8 ) & 0x07 ) << 3 ;
+
+            tempbx = pVBInfo->VGAVDE ;
+            if ( tempbx != pVBInfo->VDE )
+                tempax |= 0x40 ;
+
+            if ( pVBInfo->LCDInfo & EnableLVDSDDA )
+                tempax |= 0x40 ;
+
+            XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x1a , 0x07 , tempax ) ;
+
+            tempcx = pVBInfo->VGAVT ;
+            tempbx = pVBInfo->VDE ;
+            tempax = pVBInfo->VGAVDE ;
+            tempcx -= tempax ;
+
+            temp = tempax ;            /* 0430 ylshieh */
+            temp1 = ( temp << 18 ) / tempbx ;
+
+            tempdx = ( USHORT )( ( temp << 18 ) % tempbx ) ;
+
+            if ( tempdx != 0 )
+            temp1 += 1 ;
+
+            temp2 = temp1 ;
+            push3 = temp2 ;
+
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x37 , ( USHORT )( temp2 & 0xff ) ) ;
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x36 , ( USHORT )( ( temp2 >> 8 ) & 0xff ) ) ;
+
+            tempbx = ( USHORT )( temp2 >> 16 ) ;
+            tempax = tempbx & 0x03 ;
+
+            tempbx = pVBInfo->VGAVDE ;
+            if ( tempbx == pVBInfo->VDE )
+                tempax |= 0x04 ;
+
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x35 , tempax ) ;
+
+            if ( pVBInfo->VBType & VB_XGI301C )
+            {
+                temp2 = push3 ;
+                XGINew_SetReg1( pVBInfo->Part4Port , 0x3c , ( USHORT )( temp2 & 0xff ) ) ;
+                XGINew_SetReg1( pVBInfo->Part4Port , 0x3b , ( USHORT )( ( temp2 >> 8 ) & 0xff ) ) ;
+                tempbx = ( USHORT )( temp2 >> 16 ) ;
+                XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x3a , ~0xc0 , ( USHORT )( ( tempbx & 0xff ) << 6 ) ) ;
+
+                tempcx = pVBInfo->VGAVDE ;
+                if ( tempcx == pVBInfo->VDE )
+                    XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x30 , ~0x0c , 0x00 ) ;
+                else
+                    XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x30 , ~0x0c , 0x08 ) ;
+            }
+
+            tempcx = pVBInfo->VGAHDE ;
+            tempbx = pVBInfo->HDE ;
+
+            temp1 = tempcx << 16 ;
+
+            tempax = ( USHORT )( temp1 / tempbx ) ;
+
+            if ( ( tempbx & 0xffff ) == ( tempcx & 0xffff ) )
+                tempax = 65535 ;
+
+            temp3 = tempax ;
+            temp1 = pVBInfo->VGAHDE << 16 ;
+
+            temp1 /= temp3 ;
+            temp3 = temp3 << 16 ;
+            temp1 -= 1 ;
+
+            temp3 = ( temp3 & 0xffff0000 ) + ( temp1 & 0xffff ) ;
+
+            tempax = ( USHORT )( temp3 & 0xff ) ;
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x1f , tempax ) ;
+
+            temp1 = pVBInfo->VGAVDE << 18 ;
+            temp1 = temp1 / push3 ;
+            tempbx = ( USHORT )( temp1 & 0xffff ) ;
+
+            if ( pVBInfo->LCDResInfo == Panel1024x768 )
+                tempbx -= 1 ;
+
+            tempax = ( ( tempbx >> 8 ) & 0xff ) << 3 ;
+            tempax |= ( USHORT )( ( temp3 >> 8 ) & 0x07 ) ;
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x20 , ( USHORT )( tempax & 0xff ) ) ;
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x21 , ( USHORT )( tempbx & 0xff ) ) ;
+
+            temp3 = temp3 >> 16 ;
+
+            if ( modeflag & HalfDCLK )
+                temp3 = temp3 >> 1 ;
+
+            XGINew_SetReg1(pVBInfo->Part1Port , 0x22 , ( USHORT )( ( temp3 >> 8 ) & 0xff ) ) ;
+            XGINew_SetReg1(pVBInfo->Part1Port , 0x23 , ( USHORT )( temp3 & 0xff ) ) ;
+        }
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT2ECLK */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT2ECLK( USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex, PVB_DEVICE_INFO  pVBInfo )
+{
+    UCHAR di_0 , di_1 , tempal ;
+    int i ;
+
+    tempal = XGI_GetVCLKPtr( RefreshRateTableIndex , ModeNo , ModeIdIndex, pVBInfo ) ;
+    XGI_GetVCLKLen( tempal , &di_0 , &di_1, pVBInfo ) ;
+    XGI_GetLCDVCLKPtr( &di_0 , &di_1, pVBInfo ) ;
+
+    for( i = 0 ; i < 4 ; i++ )
+    {
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x31 , ~0x30 , ( USHORT )( 0x10 * i ) ) ;
+        if ( pVBInfo->IF_DEF_CH7007 == 1 )
+        {
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x2b , di_0 ) ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x2c , di_1 ) ;
+        }
+        else if ( ( !( pVBInfo->VBInfo & SetCRT2ToLCDA ) ) && ( !( pVBInfo->VBInfo & SetInSlaveMode ) ) )
+        {
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x2e , di_0 ) ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x2f , di_1 ) ;
+        }
+        else
+        {
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x2b , di_0 ) ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x2c , di_1 ) ;
+        }
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_UpdateModeInfo */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_UpdateModeInfo( PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO  pVBInfo )
+{
+    USHORT tempcl ,
+           tempch ,
+           temp ,
+           tempbl ,
+           tempax ;
+
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        tempcl = 0 ;
+        tempch = 0 ;
+        temp = XGINew_GetReg1( pVBInfo->P3c4 , 0x01 ) ;
+
+        if ( !( temp & 0x20 ) )
+        {
+            temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x17 ) ;
+            if ( temp & 0x80 )
+            {
+                if ( ( HwDeviceExtension->jChipType >= XG20 ) || ( HwDeviceExtension->jChipType >= XG40 ) )
+                    temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x53 ) ;
+                else
+                    temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x63 ) ;
+
+                if ( !( temp & 0x40 ) )
+                    tempcl |= ActiveCRT1 ;
+            }
+        }
+
+        temp = XGINew_GetReg1( pVBInfo->Part1Port , 0x2e ) ;
+        temp &= 0x0f ;
+
+        if ( !( temp == 0x08 ) )
+        {
+            tempax = XGINew_GetReg1( pVBInfo->Part1Port , 0x13 ) ;      /* Check ChannelA by Part1_13 [2003/10/03] */
+            if ( tempax & 0x04 )
+                tempcl = tempcl | ActiveLCD ;
+
+            temp &= 0x05 ;
+
+            if ( !( tempcl & ActiveLCD ) )
+                if ( temp == 0x01 )
+                    tempcl |= ActiveCRT2 ;
+
+            if ( temp == 0x04 )
+                tempcl |= ActiveLCD ;
+
+            if ( temp == 0x05 )
+            {
+                temp = XGINew_GetReg1( pVBInfo->Part2Port , 0x00 ) ;
+
+                if( !( temp & 0x08 ) )
+                    tempch |= ActiveAVideo ;
+
+                if ( !( temp & 0x04 ) )
+                    tempch |= ActiveSVideo ;
+
+                if ( temp & 0x02 )
+                    tempch |= ActiveSCART ;
+
+                if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+                {
+                    if ( temp & 0x01 )
+                        tempch |= ActiveHiTV ;
+                }
+
+                if ( pVBInfo->VBInfo & SetCRT2ToYPbPr )
+                {
+                    temp = XGINew_GetReg1( pVBInfo->Part2Port , 0x4d ) ;
+
+                    if ( temp & 0x10 )
+                        tempch |= ActiveYPbPr ;
+                }
+
+                if ( tempch != 0 )
+                    tempcl |= ActiveTV ;
+            }
+        }
+
+        temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x3d ) ;
+        if ( tempcl & ActiveLCD )
+        {
+            if ( ( pVBInfo->SetFlag & ReserveTVOption ) )
+            {
+                if ( temp & ActiveTV )
+                    tempcl |= ActiveTV ;
+            }
+        }
+        temp = tempcl ;
+        tempbl = ~ModeSwitchStatus ;
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x3d , tempbl , temp ) ;
+
+        if ( !( pVBInfo->SetFlag & ReserveTVOption ) )
+            XGINew_SetReg1( pVBInfo->P3d4 , 0x3e , tempch ) ;
+    }
+    else
+    {
+        return ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetVGAType */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetVGAType( PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO  pVBInfo)
+{
+    /*
+    if ( HwDeviceExtension->jChipType >= XG20 )
+    {
+        pVBInfo->Set_VGAType = XG20;
+    }
+    else if ( HwDeviceExtension->jChipType >= XG40 )
+    {
+        pVBInfo->Set_VGAType = VGA_XGI340 ;
+    }
+    */
+    pVBInfo->Set_VGAType = HwDeviceExtension->jChipType;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetVBType */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetVBType(PVB_DEVICE_INFO  pVBInfo)
+{
+    USHORT flag , tempbx , tempah ;
+
+    if ( pVBInfo->IF_DEF_CH7007 == 1 )
+    {
+        pVBInfo->VBType = VB_CH7007 ;
+        return;
+    }
+    if ( pVBInfo->IF_DEF_LVDS == 0 )
+    {
+        tempbx = VB_XGI302B ;
+        flag = XGINew_GetReg1( pVBInfo->Part4Port , 0x00 ) ;
+        if ( flag != 0x02 )
+        {
+            tempbx = VB_XGI301 ;
+            flag = XGINew_GetReg1( pVBInfo->Part4Port , 0x01 ) ;
+            if ( flag >= 0xB0 )
+            {
+                tempbx = VB_XGI301B ;
+                if ( flag >= 0xC0 )
+                {
+                    tempbx = VB_XGI301C ;
+                    if ( flag >= 0xD0 )
+                    {
+                        tempbx = VB_XGI301LV ;
+                        if ( flag >= 0xE0 )
+                        {
+                            tempbx = VB_XGI302LV ;
+                            tempah = XGINew_GetReg1( pVBInfo->Part4Port , 0x39 ) ;
+                            if ( tempah != 0xFF )
+                                tempbx = VB_XGI301C ;
+                        }
+                    }
+                }
+
+                if ( tempbx & ( VB_XGI301B | VB_XGI302B ) )
+                {
+                    flag = XGINew_GetReg1( pVBInfo->Part4Port , 0x23 ) ;
+
+                    if ( !( flag & 0x02 ) )
+                        tempbx = tempbx | VB_NoLCD ;
+                }
+            }
+        }
+        pVBInfo->VBType = tempbx ;
+    }
+/*
+    else if ( pVBInfo->IF_DEF_CH7017 == 1 )
+        pVBInfo->VBType = VB_CH7017 ;
+    else  //LVDS
+        pVBInfo->VBType = VB_LVDS_NS ;
+*/
+
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetVBInfo */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetVBInfo( USHORT ModeNo , USHORT ModeIdIndex , PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO  pVBInfo )
+{
+    USHORT tempax ,
+           push ,
+           tempbx ,
+           temp ,
+           modeflag ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+    }
+
+    pVBInfo->SetFlag = 0 ;
+    pVBInfo->ModeType = modeflag & ModeInfoFlag ;
+    tempbx = 0 ;
+
+    if ( pVBInfo->VBType & 0xFFFF )
+    {
+        temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x30 ) ;           /* Check Display Device */
+        tempbx = tempbx | temp ;
+        temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x31 ) ;
+        push = temp ;
+        push = push << 8 ;
+        tempax = temp << 8 ;
+        tempbx = tempbx | tempax ;
+        temp = ( SetCRT2ToDualEdge | SetCRT2ToYPbPr | SetCRT2ToLCDA | SetInSlaveMode | DisableCRT2Display ) ;
+        temp = 0xFFFF ^ temp ;
+        tempbx &= temp ;
+
+        temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x38 ) ;
+
+        if ( pVBInfo->IF_DEF_LCDA == 1 )
+        {
+
+            if ( ( pVBInfo->Set_VGAType >= XG20 ) || ( pVBInfo->Set_VGAType >= XG40 ))
+            {
+                if ( pVBInfo->IF_DEF_LVDS == 0 )
+                {
+                    /* if ( ( pVBInfo->VBType & VB_XGI302B ) || ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) || ( pVBInfo->VBType & VB_XGI301C ) ) */
+                    if ( pVBInfo->VBType & ( VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+                    {
+                        if ( temp & EnableDualEdge )
+                        {
+                            tempbx |= SetCRT2ToDualEdge ;
+
+                            if ( temp & SetToLCDA )
+                                tempbx |= SetCRT2ToLCDA ;
+                        }
+                    }
+                }
+                else if ( pVBInfo->IF_DEF_CH7017 == 1 )
+                {
+                    if ( pVBInfo->VBType & VB_CH7017 )
+                    {
+                        if ( temp & EnableDualEdge )
+                        {
+                            tempbx |= SetCRT2ToDualEdge ;
+
+                            if ( temp & SetToLCDA )
+                                tempbx |= SetCRT2ToLCDA ;
+                        }
+                    }
+                }
+            }
+        }
+
+        if ( pVBInfo->IF_DEF_YPbPr == 1 )
+        {
+            if ( ( ( pVBInfo->IF_DEF_LVDS == 0 ) && ( ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) || ( pVBInfo->VBType & VB_XGI301C ) ) )
+            || ( ( pVBInfo->IF_DEF_CH7017 == 1 ) && ( pVBInfo->VBType&VB_CH7017 ) ) || ( (pVBInfo->IF_DEF_CH7007 == 1) && (pVBInfo->VBType&VB_CH7007) ) )    /* [Billy] 07/05/04 */
+            {
+                if ( temp & SetYPbPr )  /* temp = CR38 */
+                {
+                    if ( pVBInfo->IF_DEF_HiVision == 1 )
+                    {
+                        temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x35 ) ;  /* shampoo add for new scratch */
+                        temp &= YPbPrMode ;
+                        tempbx |= SetCRT2ToHiVisionTV ;
+
+                        if ( temp != YPbPrMode1080i ) {
+                            tempbx &= ( ~SetCRT2ToHiVisionTV ) ;
+                            tempbx |= SetCRT2ToYPbPr ; }
+                    }
+
+                    /* tempbx |= SetCRT2ToYPbPr ; */
+                }
+            }
+        }
+
+        tempax = push ;  /* restore CR31 */
+
+        if ( pVBInfo->IF_DEF_LVDS == 0 )
+        {
+            if ( pVBInfo->IF_DEF_YPbPr == 1 )
+            {
+                if ( pVBInfo->IF_DEF_HiVision == 1 )
+                    temp = 0x09FC ;
+                else
+                    temp = 0x097C ;
+            }
+            else
+            {
+                if ( pVBInfo->IF_DEF_HiVision == 1 )
+                    temp = 0x01FC ;
+                else
+                    temp = 0x017C ;
+            }
+        }
+        else    /* 3nd party chip */
+        {
+            if ( pVBInfo->IF_DEF_CH7017 == 1 )
+                temp = ( SetCRT2ToTV | SetCRT2ToLCD | SetCRT2ToLCDA ) ;
+            else if ( pVBInfo->IF_DEF_CH7007 == 1 )  /* [Billy] 07/05/03 */
+            {
+                temp = SetCRT2ToTV ;
+            }
+            else
+                temp = SetCRT2ToLCD ;
+        }
+
+        if ( !( tempbx & temp ) )
+        {
+            tempax |= DisableCRT2Display ;
+            tempbx = 0 ;
+        }
+
+        if ( pVBInfo->IF_DEF_LCDA == 1 )        /* Select Display Device */
+        {
+            if ( !( pVBInfo->VBType & VB_NoLCD ) )
+            {
+                if ( tempbx & SetCRT2ToLCDA )
+                {
+                    if ( tempbx & SetSimuScanMode )
+                        tempbx &= ( ~( SetCRT2ToLCD | SetCRT2ToRAMDAC | SwitchToCRT2 ) ) ;
+                    else
+                        tempbx &= ( ~( SetCRT2ToLCD | SetCRT2ToRAMDAC | SetCRT2ToTV | SwitchToCRT2 ) ) ;
+                }
+            }
+        }
+
+        /* shampoo add */
+        if ( !( tempbx & ( SwitchToCRT2 | SetSimuScanMode ) ) ) /* for driver abnormal */
+        {
+            if ( pVBInfo->IF_DEF_CRT2Monitor == 1 )
+            {
+                if ( tempbx & SetCRT2ToRAMDAC )
+                {
+                    tempbx &= ( 0xFF00 | SetCRT2ToRAMDAC | SwitchToCRT2 | SetSimuScanMode ) ;
+                    tempbx &= ( 0x00FF | ( ~SetCRT2ToYPbPr ) ) ;
+                }
+            }
+            else
+                tempbx &= ( ~( SetCRT2ToRAMDAC | SetCRT2ToLCD | SetCRT2ToTV ) ) ;
+        }
+
+        if ( !( pVBInfo->VBType & VB_NoLCD ) )
+        {
+            if ( tempbx & SetCRT2ToLCD )
+            {
+                tempbx &= ( 0xFF00 | SetCRT2ToLCD | SwitchToCRT2 | SetSimuScanMode ) ;
+                tempbx &= ( 0x00FF | ( ~SetCRT2ToYPbPr ) ) ;
+            }
+        }
+
+        if ( tempbx & SetCRT2ToSCART )
+        {
+            tempbx &= ( 0xFF00 | SetCRT2ToSCART | SwitchToCRT2 | SetSimuScanMode ) ;
+            tempbx &= ( 0x00FF | ( ~SetCRT2ToYPbPr ) ) ;
+        }
+
+        if ( pVBInfo->IF_DEF_YPbPr == 1 )
+        {
+            if ( tempbx & SetCRT2ToYPbPr )
+                tempbx &= ( 0xFF00 | SwitchToCRT2 | SetSimuScanMode ) ;
+        }
+
+        if ( pVBInfo->IF_DEF_HiVision == 1 )
+        {
+            if ( tempbx & SetCRT2ToHiVisionTV )
+                tempbx &= ( 0xFF00 | SetCRT2ToHiVisionTV | SwitchToCRT2 | SetSimuScanMode ) ;
+        }
+
+        if ( tempax & DisableCRT2Display )      /* Set Display Device Info */
+        {
+            if ( !( tempbx & ( SwitchToCRT2 | SetSimuScanMode ) ) )
+                tempbx = DisableCRT2Display ;
+        }
+
+        if ( !( tempbx & DisableCRT2Display ) )
+        {
+            if ( ( !( tempbx & DriverMode ) ) || ( !( modeflag & CRT2Mode ) ) )
+            {
+                if ( pVBInfo->IF_DEF_LCDA == 1 )
+                {
+                    if ( !( tempbx & SetCRT2ToLCDA ) )
+                        tempbx |= ( SetInSlaveMode | SetSimuScanMode ) ;
+                }
+
+                if ( pVBInfo->IF_DEF_VideoCapture == 1 )
+                {
+                    if ( ( ( HwDeviceExtension->jChipType == XG40 ) && ( pVBInfo->Set_VGAType == XG40 ) )
+                    || ( ( HwDeviceExtension->jChipType == XG41 ) && ( pVBInfo->Set_VGAType == XG41 ) )
+                    || ( ( HwDeviceExtension->jChipType == XG42 ) && ( pVBInfo->Set_VGAType == XG42 ) )
+                    || ( ( HwDeviceExtension->jChipType == XG45 ) && ( pVBInfo->Set_VGAType == XG45 ) ) )
+                    {
+                        if ( ModeNo <= 13 )
+                        {
+                            if ( !( tempbx & SetCRT2ToRAMDAC ) )        /*CRT2 not need to support*/
+                            {
+                                tempbx &= ( 0x00FF | ( ~SetInSlaveMode ) ) ;
+                                pVBInfo->SetFlag |= EnableVCMode ;
+                            }
+                        }
+                    }
+                }
+            }
+
+            /*LCD+TV can't support in slave mode (Force LCDA+TV->LCDB)*/
+            if ( ( tempbx & SetInSlaveMode ) && ( tempbx & SetCRT2ToLCDA ) )
+            {
+                tempbx ^= ( SetCRT2ToLCD | SetCRT2ToLCDA | SetCRT2ToDualEdge ) ;
+                pVBInfo->SetFlag |= ReserveTVOption ;
+            }
+        }
+    }
+
+    pVBInfo->VBInfo = tempbx ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetTVInfo */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetTVInfo( USHORT ModeNo , USHORT ModeIdIndex ,PVB_DEVICE_INFO  pVBInfo )
+{
+    USHORT temp ,
+           tempbx = 0 ,
+           resinfo = 0 ,
+           modeflag ,
+           index1 ;
+
+    tempbx = 0 ;
+    resinfo = 0 ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToTV )
+    {
+        if ( ModeNo <= 0x13 )
+        {
+            modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;       /* si+St_ModeFlag */
+            resinfo = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ResInfo ; /* si+St_ResInfo */
+        }
+        else
+        {
+            modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+            resinfo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ; /* si+Ext_ResInfo */
+        }
+
+        if ( pVBInfo->VBInfo & SetCRT2ToTV )
+        {
+            temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x35 ) ;
+            tempbx = temp;
+            if ( tempbx & SetPALTV )
+            {
+                tempbx &= ( SetCHTVOverScan | SetPALMTV | SetPALNTV | SetPALTV ) ;
+                if ( tempbx & SetPALMTV )
+                    tempbx &= ~SetPALTV ; /* set to NTSC if PAL-M */
+            }
+            else
+                tempbx &= ( SetCHTVOverScan | SetNTSCJ | SetPALTV ) ;
+/*
+            if ( pVBInfo->IF_DEF_LVDS == 0 )
+            {
+                index1 = XGINew_GetReg1( pVBInfo->P3d4 , 0x38 ) ; //PAL-M/PAL-N Info
+                temp2 = ( index1 & 0xC0 ) >> 5 ;                //00:PAL, 01:PAL-M, 10:PAL-N
+                tempbx |= temp2 ;
+                if ( temp2 & 0x02 )          //PAL-M
+                    tempbx &= ( ~SetPALTV ) ;
+            }
+*/
+        }
+
+        if ( pVBInfo->IF_DEF_CH7017 == 1 )
+        {
+            tempbx = XGINew_GetReg1( pVBInfo->P3d4 , 0x35 ) ;
+
+            if ( tempbx & TVOverScan )
+                tempbx |= SetCHTVOverScan ;
+        }
+
+        if ( pVBInfo->IF_DEF_CH7007 == 1 ) /* [Billy] 07/05/04 */
+        {
+            tempbx = XGINew_GetReg1( pVBInfo->P3d4 , 0x35 ) ;
+
+            if ( tempbx & TVOverScan )
+            {
+                tempbx |= SetCHTVOverScan ;
+            }
+        }
+
+
+        if ( pVBInfo->IF_DEF_LVDS == 0 )
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToSCART )
+                tempbx |= SetPALTV ;
+        }
+
+        if ( pVBInfo->IF_DEF_YPbPr == 1 )
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToYPbPr )
+            {
+                index1 = XGINew_GetReg1( pVBInfo->P3d4 , 0x35 ) ;
+                index1 &= YPbPrMode ;
+
+                if ( index1 == YPbPrMode525i )
+                    tempbx |= SetYPbPrMode525i ;
+
+                if ( index1 == YPbPrMode525p )
+                    tempbx = tempbx | SetYPbPrMode525p;
+                if ( index1 == YPbPrMode750p)
+                    tempbx = tempbx | SetYPbPrMode750p;
+            }
+        }
+
+        if ( pVBInfo->IF_DEF_HiVision == 1 )
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+            {
+                tempbx = tempbx | SetYPbPrMode1080i | SetPALTV ;
+            }
+        }
+
+        if ( pVBInfo->IF_DEF_LVDS == 0 )
+        {      /* shampoo */
+            if ( ( pVBInfo->VBInfo & SetInSlaveMode ) && ( !( pVBInfo->VBInfo & SetNotSimuMode ) ) )
+                tempbx |= TVSimuMode ;
+
+            if ( !( tempbx & SetPALTV ) && ( modeflag > 13 ) && ( resinfo == 8 ) ) /* NTSC 1024x768, */
+                tempbx |= NTSC1024x768 ;
+
+            tempbx |= RPLLDIV2XO ;
+
+            if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+            {
+                if ( pVBInfo->VBInfo & SetInSlaveMode )
+                    tempbx &=( ~RPLLDIV2XO ) ;
+            }
+            else
+            {
+                if ( tempbx & ( SetYPbPrMode525p | SetYPbPrMode750p ) )
+                    tempbx &= ( ~RPLLDIV2XO ) ;
+                else if ( !( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) ) )
+                {
+                    if ( tempbx & TVSimuMode )
+                        tempbx &= ( ~RPLLDIV2XO ) ;
+                }
+            }
+        }
+    }
+    pVBInfo->TVInfo = tempbx ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetLCDInfo */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_GetLCDInfo( USHORT ModeNo , USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT temp ,
+           tempax ,
+           tempbx ,
+           modeflag ,
+           resinfo = 0 ,
+           LCDIdIndex ;
+
+    pVBInfo->LCDResInfo = 0 ;
+    pVBInfo->LCDTypeInfo = 0 ;
+    pVBInfo->LCDInfo = 0 ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;  /* si+St_ModeFlag // */
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+        resinfo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ; /* si+Ext_ResInfo// */
+    }
+
+    temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x36 ) ; /* Get LCD Res.Info */
+    tempbx = temp & 0x0F ;
+
+    if ( tempbx == 0 )
+        tempbx = Panel1024x768 ; /* default */
+
+    /* LCD75 [2003/8/22] Vicent */
+    if ( ( tempbx == Panel1024x768 ) || ( tempbx == Panel1280x1024 ) )
+    {
+        if ( pVBInfo->VBInfo & DriverMode )
+        {
+            tempax = XGINew_GetReg1( pVBInfo->P3d4 , 0x33 ) ;
+            if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+                tempax &= 0x0F ;
+            else
+                tempax = tempax >> 4 ;
+
+            if ( ( resinfo == 6 ) || ( resinfo == 9 ) )
+            {
+                if ( tempax >= 3 )
+                    tempbx |= PanelRef75Hz ;
+            }
+            else if ( ( resinfo == 7 ) || ( resinfo == 8 ) )
+            {
+                if ( tempax >= 4 )
+                    tempbx |= PanelRef75Hz ;
+            }
+        }
+    }
+
+    pVBInfo->LCDResInfo = tempbx ;
+
+    /* End of LCD75 */
+
+    if( pVBInfo->IF_DEF_OEMUtil == 1 )
+    {
+        pVBInfo->LCDTypeInfo = ( temp & 0xf0 ) >> 4 ;
+    }
+
+    if ( !( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) ) )
+    {
+        return 0;
+    }
+
+    tempbx = 0 ;
+
+    temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x37 ) ;
+
+    temp &= ( ScalingLCD | LCDNonExpanding | LCDSyncBit | SetPWDEnable ) ;
+
+    if ( ( pVBInfo->IF_DEF_ScaleLCD == 1 ) && ( temp & LCDNonExpanding ) )
+        temp &= ~EnableScalingLCD ;
+
+    tempbx |= temp ;
+
+    LCDIdIndex = XGI_GetLCDCapPtr1(pVBInfo) ;
+
+    tempax = pVBInfo->LCDCapList[ LCDIdIndex ].LCD_Capability ;
+
+    if ( pVBInfo->IF_DEF_LVDS == 0 )    /* shampoo */
+    {
+        if ( ( ( pVBInfo->VBType & VB_XGI302LV ) || ( pVBInfo->VBType & VB_XGI301C ) ) && ( tempax & LCDDualLink ) )
+        {
+            tempbx |= SetLCDDualLink ;
+        }
+    }
+
+    if ( pVBInfo->IF_DEF_CH7017 == 1 )
+    {
+        if ( tempax & LCDDualLink )
+        {
+            tempbx |= SetLCDDualLink ;
+        }
+    }
+
+    if ( pVBInfo->IF_DEF_LVDS == 0 )
+    {
+        if ( ( pVBInfo->LCDResInfo == Panel1400x1050 ) && ( pVBInfo->VBInfo & SetCRT2ToLCD ) && ( ModeNo > 0x13 ) && ( resinfo == 9 ) && ( !( tempbx & EnableScalingLCD ) ) )
+            tempbx |= SetLCDtoNonExpanding ;    /* set to center in 1280x1024 LCDB for Panel1400x1050 */
+    }
+
+/*
+    if ( tempax & LCDBToA )
+    {
+        tempbx |= SetLCDBToA ;
+    }
+*/
+
+    if ( pVBInfo->IF_DEF_ExpLink == 1 )
+    {
+        if ( modeflag & HalfDCLK )
+        {
+            /* if ( !( pVBInfo->LCDInfo&LCDNonExpanding ) ) */
+            if ( !( tempbx & SetLCDtoNonExpanding ) )
+            {
+                tempbx |= EnableLVDSDDA ;
+            }
+            else
+            {
+                if ( ModeNo > 0x13 )
+                {
+                    if ( pVBInfo->LCDResInfo == Panel1024x768 )
+                    {
+                        if ( resinfo == 4 )
+                        {                                /* 512x384  */
+                            tempbx |= EnableLVDSDDA ;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    if ( pVBInfo->VBInfo & SetInSlaveMode )
+    {
+        if ( pVBInfo->VBInfo & SetNotSimuMode )
+        {
+            tempbx |= LCDVESATiming ;
+        }
+    }
+    else
+    {
+        tempbx |= LCDVESATiming ;
+    }
+
+    pVBInfo->LCDInfo = tempbx ;
+
+    if ( pVBInfo->IF_DEF_PWD == 1 )
+    {
+        if ( pVBInfo->LCDInfo & SetPWDEnable )
+        {
+            if ( ( pVBInfo->VBType & VB_XGI302LV ) || ( pVBInfo->VBType & VB_XGI301C ) )
+            {
+                if ( !( tempax & PWDEnable ) )
+                {
+                    pVBInfo->LCDInfo &= ~SetPWDEnable ;
+                }
+            }
+        }
+    }
+
+    if ( pVBInfo->IF_DEF_LVDS == 0 )
+    {
+        if ( tempax & ( LockLCDBToA | StLCDBToA ) )
+        {
+            if ( pVBInfo->VBInfo & SetInSlaveMode )
+            {
+                if ( !( tempax & LockLCDBToA ) )
+                {
+                    if ( ModeNo <= 0x13 )
+                    {
+                        pVBInfo->VBInfo &= ~( SetSimuScanMode | SetInSlaveMode | SetCRT2ToLCD ) ;
+                        pVBInfo->VBInfo |= SetCRT2ToLCDA | SetCRT2ToDualEdge ;
+                    }
+                }
+            }
+        }
+    }
+
+/*
+    if ( pVBInfo->IF_DEF_LVDS == 0 )
+    {
+        if ( tempax & ( LockLCDBToA | StLCDBToA ) )
+        {
+            if ( pVBInfo->VBInfo & SetInSlaveMode )
+            {
+                if ( !( ( !( tempax & LockLCDBToA ) ) && ( ModeNo > 0x13 ) ) )
+                {
+                    pVBInfo->VBInfo&=~(SetSimuScanMode|SetInSlaveMode|SetCRT2ToLCD);
+                    pVBInfo->VBInfo|=SetCRT2ToLCDA|SetCRT2ToDualEdge;
+                }
+            }
+        }
+    }
+*/
+
+    return( 1 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SearchModeID */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_SearchModeID( USHORT ModeNo , USHORT *ModeIdIndex, PVB_DEVICE_INFO pVBInfo )
+{
+
+#ifdef TC
+
+    if ( ModeNo <= 5 )
+        ModeNo |= 1 ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        /* for (*ModeIdIndex=0;*ModeIdIndex<sizeof(pVBInfo->SModeIDTable)/sizeof(XGI_StStruct);(*ModeIdIndex)++) */
+        for( *ModeIdIndex = 0 ; ; ( *ModeIdIndex )++ )
+        {
+            if ( pVBInfo->SModeIDTable[ *ModeIdIndex ].St_ModeID == ModeNo )
+                break ;
+            if ( pVBInfo->SModeIDTable[ *ModeIdIndex ].St_ModeID == 0xFF )
+                return( FALSE ) ;
+        }
+
+        VGA_INFO = ( PUCHAR )MK_FP( 0 , 0x489 ) ;
+
+        if ( ModeNo == 0x07 )
+        {
+            if ( ( *VGA_INFO & 0x10 ) != 0 )
+                ( *ModeIdIndex )++ ; /* 400 lines */
+            /* else 350 lines */
+        }
+
+        if ( ModeNo <= 3 )
+        {
+            if ( ( *VGA_INFO & 0x80 ) == 0 )
+            {
+                ( *ModeIdIndex )++ ;
+                if ( ( *VGA_INFO & 0x10 ) != 0 )
+                    ( *ModeIdIndex )++ ; /* 400 lines */
+                /* else 350 lines */
+            }
+            /* else 200 lines */
+        }
+    }
+    else
+    {
+        /* for (*ModeIdIndex=0;*ModeIdIndex<sizeof(pVBInfo->EModeIDTable)/sizeof(XGI_ExtStruct);(*ModeIdIndex)++) */
+        for( *ModeIdIndex = 0 ; ; ( *ModeIdIndex )++ )
+        {
+            if ( pVBInfo->EModeIDTable[ *ModeIdIndex ].Ext_ModeID == ModeNo )
+                break ;
+            if ( pVBInfo->EModeIDTable[ *ModeIdIndex ].Ext_ModeID == 0xFF )
+                return( FALSE ) ;
+        }
+    }
+
+
+#endif
+
+#ifdef WIN2000
+
+    if ( ModeNo <= 5 )
+        ModeNo |= 1 ;
+    if ( ModeNo <= 0x13 )
+    {
+        /* for (*ModeIdIndex=0;*ModeIdIndex<sizeof(pVBInfo->SModeIDTable)/sizeof(XGI_StStruct);(*ModeIdIndex)++) */
+        for( *ModeIdIndex = 0 ; ; ( *ModeIdIndex )++ )
+        {
+            if ( pVBInfo->SModeIDTable[ *ModeIdIndex ].St_ModeID == ModeNo )
+                break ;
+            if ( pVBInfo->SModeIDTable[ *ModeIdIndex ].St_ModeID == 0xFF )
+                return( FALSE ) ;
+        }
+
+        if ( ModeNo == 0x07 )
+            ( *ModeIdIndex )++ ; /* 400 lines */
+
+        if ( ModeNo <=3 )
+            ( *ModeIdIndex ) += 2 ; /* 400 lines */
+        /* else 350 lines */
+    }
+    else
+    {
+        /* for (*ModeIdIndex=0;*ModeIdIndex<sizeof(pVBInfo->EModeIDTable)/sizeof(XGI_ExtStruct);(*ModeIdIndex)++) */
+        for( *ModeIdIndex = 0 ; ; ( *ModeIdIndex )++ )
+        {
+            if ( pVBInfo->EModeIDTable[ *ModeIdIndex ].Ext_ModeID == ModeNo )
+                break ;
+            if ( pVBInfo->EModeIDTable[ *ModeIdIndex ].Ext_ModeID == 0xFF )
+                return( FALSE ) ;
+        }
+    }
+
+#endif
+
+#ifdef LINUX /* chiawen for linux solution */
+
+    if ( ModeNo <= 5 )
+        ModeNo |= 1 ;
+    if ( ModeNo <= 0x13 )
+    {
+        /* for (*ModeIdIndex=0;*ModeIdIndex<sizeof(pVBInfo->SModeIDTable)/sizeof(XGI_StStruct);(*ModeIdIndex)++) */
+        for( *ModeIdIndex = 0 ; ; ( *ModeIdIndex )++ )
+        {
+            if ( pVBInfo->SModeIDTable[ *ModeIdIndex ].St_ModeID == ModeNo )
+                break ;
+            if ( pVBInfo->SModeIDTable[ *ModeIdIndex ].St_ModeID == 0xFF )
+                return( FALSE ) ;
+        }
+
+        if ( ModeNo == 0x07 )
+            ( *ModeIdIndex )++ ; /* 400 lines */
+
+        if ( ModeNo <= 3 )
+            ( *ModeIdIndex ) += 2 ; /* 400 lines */
+        /* else 350 lines */
+    }
+    else
+    {
+        /* for (*ModeIdIndex=0;*ModeIdIndex<sizeof(pVBInfo->EModeIDTable)/sizeof(XGI_ExtStruct);(*ModeIdIndex)++) */
+        for( *ModeIdIndex = 0 ; ; ( *ModeIdIndex )++ )
+        {
+            if ( pVBInfo->EModeIDTable[ *ModeIdIndex ].Ext_ModeID == ModeNo )
+                break ;
+            if ( pVBInfo->EModeIDTable[ *ModeIdIndex ].Ext_ModeID == 0xFF )
+                return( FALSE ) ;
+        }
+    }
+
+#endif
+
+    return( TRUE ) ;
+}
+
+
+
+
+/* win2000 MM adapter not support standard mode! */
+
+/* --------------------------------------------------------------------- */
+/* Function : */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGINew_CheckMemorySize(PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT ModeNo,USHORT ModeIdIndex,PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT memorysize ,
+           modeflag ,
+           temp ,
+           temp1 ,
+           tmp ;
+
+/*  if ( ( HwDeviceExtension->jChipType == XGI_650 ) ||
+         ( HwDeviceExtension->jChipType == XGI_650M ) )
+    {
+        return( TRUE ) ;
+    } */
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+    }
+    else  {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+    }
+
+    /* ModeType = modeflag&ModeInfoFlag ; // Get mode type */
+
+    memorysize = modeflag & MemoryInfoFlag ;
+    memorysize = memorysize > MemorySizeShift ;
+    memorysize++ ;                                      /* Get memory size */
+
+    temp = XGINew_GetReg1( pVBInfo->P3c4 , 0x14 ) ;     /* Get DRAM Size */
+    tmp = temp ;
+
+    if ( HwDeviceExtension->jChipType == XG40 )
+    {
+        temp = 1 << ( ( temp & 0x0F0 ) >> 4 ) ;         /* memory size per channel SR14[7:4] */
+        if ( ( tmp & 0x0c ) == 0x0C )                   /* Qual channels */
+        {
+            temp <<= 2 ;
+        }
+        else if ( ( tmp & 0x0c ) == 0x08 )              /* Dual channels */
+        {
+            temp <<= 1 ;
+        }
+    }
+    else if ( HwDeviceExtension->jChipType == XG42 )
+    {
+        temp = 1 << ( ( temp & 0x0F0 ) >> 4 ) ;         /* memory size per channel SR14[7:4] */
+        if ( ( tmp & 0x04 ) == 0x04 )                   /* Dual channels */
+        {
+            temp <<= 1 ;
+        }
+    }
+    else if ( HwDeviceExtension->jChipType == XG45 )
+    {
+        temp = 1 << ( ( temp & 0x0F0 ) >> 4 ) ;         /* memory size per channel SR14[7:4] */
+        if ( ( tmp & 0x0c ) == 0x0C )                   /* Qual channels */
+        {
+            temp <<= 2 ;
+        }
+        else if ( ( tmp & 0x0c ) == 0x08 )              /* triple channels */
+        {
+            temp1 = temp ;
+            temp <<= 1 ;
+            temp += temp1 ;
+        }
+        else if ( ( tmp & 0x0c ) == 0x04 )              /* Dual channels */
+        {
+            temp <<= 1 ;
+        }
+    }
+    if ( temp < memorysize )
+        return( FALSE ) ;
+    else
+        return( TRUE ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_IsLowResolution */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+/*void XGINew_IsLowResolution( USHORT ModeNo , USHORT ModeIdIndex, BOOLEAN XGINew_CheckMemorySize(PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT ModeNo,USHORT ModeIdIndex,PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT data ;
+    USHORT ModeFlag ;
+
+    data = XGINew_GetReg1( pVBInfo->P3c4 , 0x0F ) ;
+    data &= 0x7F ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x0F , data ) ;
+
+    if ( ModeNo > 0x13 )
+    {
+        ModeFlag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+        if ( ( ModeFlag & HalfDCLK ) && ( ModeFlag & DoubleScanMode ) )
+        {
+            data = XGINew_GetReg1( pVBInfo->P3c4 , 0x0F ) ;
+            data |= 0x80 ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x0F , data ) ;
+            data = XGINew_GetReg1( pVBInfo->P3c4 , 0x01 ) ;
+            data &= 0xF7 ;
+            XGINew_SetReg1( pVBInfo->P3c4 , 0x01 , data ) ;
+        }
+    }
+}
+
+*/
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_DisplayOn */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_DisplayOn( PXGI_HW_DEVICE_INFO pXGIHWDE , PVB_DEVICE_INFO pVBInfo )
+{
+
+    XGINew_SetRegANDOR(pVBInfo->P3c4,0x01,0xDF,0x00);
+    if ( pXGIHWDE->jChipType == XG21 )
+    {
+       if ( pVBInfo->IF_DEF_LVDS == 1 )
+       {
+         if (!(XGI_XG21GetPSCValue( pVBInfo )&0x1))
+         {
+            XGI_XG21BLSignalVDD( 0x01 , 0x01, pVBInfo ) ; /* LVDS VDD on */
+            XGI_XG21SetPanelDelay( 2,pVBInfo ) ;
+         }
+         if (!(XGI_XG21GetPSCValue( pVBInfo )&0x20))
+         {
+            XGI_XG21BLSignalVDD( 0x20 , 0x20, pVBInfo ) ; /* LVDS signal on */
+         }
+         XGI_XG21SetPanelDelay( 3,pVBInfo ) ;
+         XGI_XG21BLSignalVDD( 0x02 , 0x02, pVBInfo ) ; /* LVDS backlight on */
+       }
+       else
+       {
+            XGI_XG21BLSignalVDD( 0x20 , 0x20, pVBInfo ) ; /* DVO/DVI signal on */
+       }
+
+    }
+
+    if (pVBInfo->IF_DEF_CH7007 == 1) /* [Billy] 07/05/23 For CH7007 */
+    {
+#ifdef WIN2000
+       if ( IsCH7007TVMode( pVBInfo ) )
+       {
+           TurnOnCH7007(pXGIHWDE->pDevice) ; /* 07/05/28 */
+       }
+#endif
+
+    }
+
+
+    if ( pXGIHWDE->jChipType == XG27 )
+    {
+       if ( pVBInfo->IF_DEF_LVDS == 1 )
+       {
+         if (!(XGI_XG27GetPSCValue( pVBInfo )&0x1))
+         {
+            XGI_XG27BLSignalVDD( 0x01 , 0x01, pVBInfo ) ; /* LVDS VDD on */
+            XGI_XG21SetPanelDelay( 2,pVBInfo ) ;
+         }
+         if (!(XGI_XG27GetPSCValue( pVBInfo )&0x20))
+         {
+            XGI_XG27BLSignalVDD( 0x20 , 0x20, pVBInfo ) ; /* LVDS signal on */
+         }
+         XGI_XG21SetPanelDelay( 3,pVBInfo ) ;
+         XGI_XG27BLSignalVDD( 0x02 , 0x02, pVBInfo ) ; /* LVDS backlight on */
+       }
+       else
+       {
+            XGI_XG27BLSignalVDD( 0x20 , 0x20, pVBInfo ) ; /* DVO/DVI signal on */
+       }
+
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_DisplayOff */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_DisplayOff( PXGI_HW_DEVICE_INFO pXGIHWDE , PVB_DEVICE_INFO pVBInfo )
+{
+
+    if ( pXGIHWDE->jChipType == XG21 )
+    {
+       if ( pVBInfo->IF_DEF_LVDS == 1 )
+       {
+         XGI_XG21BLSignalVDD( 0x02 , 0x00, pVBInfo ) ; /* LVDS backlight off */
+         XGI_XG21SetPanelDelay( 3,pVBInfo ) ;
+       }
+       else
+       {
+            XGI_XG21BLSignalVDD( 0x20 , 0x00, pVBInfo ) ; /* DVO/DVI signal off */
+       }
+    }
+
+    if (pVBInfo->IF_DEF_CH7007 == 1) /*[Billy] 07/05/23 For CH7007 */
+    {
+       /* if( IsCH7007TVMode( pVBInfo ) == 0 ) */
+       {
+#ifdef WIN2000
+         TurnOffCH7007(pXGIHWDE->pDevice) ;  /* 07/05/28 */
+#endif
+       }
+    }
+
+
+    if ( pXGIHWDE->jChipType == XG27 )
+    {
+       if ((XGI_XG27GetPSCValue( pVBInfo )&0x2))
+       {
+         XGI_XG27BLSignalVDD( 0x02 , 0x00, pVBInfo ) ; /* LVDS backlight off */
+         XGI_XG21SetPanelDelay( 3,pVBInfo ) ;
+       }
+
+       if ( pVBInfo->IF_DEF_LVDS == 0 )
+       {
+            XGI_XG27BLSignalVDD( 0x20 , 0x00, pVBInfo ) ; /* DVO/DVI signal off */
+       }
+    }
+
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x01 , 0xDF , 0x20 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_WaitDisply */
+/* Input : */
+/* Output : */
+/* Description : chiawen for sensecrt1 */
+/* --------------------------------------------------------------------- */
+void XGI_WaitDisply( PVB_DEVICE_INFO pVBInfo )
+{
+    while( ( XGINew_GetReg2( pVBInfo->P3da ) & 0x01 ) )
+        break ;
+
+    while( !( XGINew_GetReg2( pVBInfo->P3da ) & 0x01 ) )
+        break ;
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SenseCRT1 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+
+void XGI_SenseCRT1( PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR CRTCData[ 17 ] = { 0x5F , 0x4F , 0x50 , 0x82 , 0x55 , 0x81 ,
+                             0x0B , 0x3E , 0xE9 , 0x0B , 0xDF , 0xE7 ,
+                             0x04 , 0x00 , 0x00 , 0x05 , 0x00 } ;
+
+    UCHAR SR01 = 0 , SR1F = 0 , SR07 = 0 , SR06 = 0 ;
+
+    UCHAR CR17 , CR63 , SR31 ;
+    USHORT temp ;
+    UCHAR DAC_TEST_PARMS[ 3 ] = { 0x0F , 0x0F , 0x0F } ;
+
+    int i ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x05 , 0x86 ) ;
+
+    /* [2004/05/06] Vicent to fix XG42 single LCD sense to CRT+LCD */
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x57 , 0x4A ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x53 , ( UCHAR )( XGINew_GetReg1( pVBInfo->P3d4 , 0x53 ) | 0x02 ) ) ;
+
+    SR31 = ( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x31 ) ;
+    CR63 = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x63 ) ;
+    SR01 = ( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x01 ) ;
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x01 , ( UCHAR )( SR01 & 0xDF ) ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x63 , ( UCHAR )( CR63 & 0xBF ) ) ;
+
+    CR17 = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x17 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x17 , ( UCHAR )( CR17 | 0x80 ) ) ;
+
+    SR1F = ( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x1F ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x1F , ( UCHAR )( SR1F | 0x04 ) ) ;
+
+    SR07 = ( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x07 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x07 , ( UCHAR )( SR07 & 0xFB ) ) ;
+    SR06 = ( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x06 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x06 , ( UCHAR )( SR06 & 0xC3 ) ) ;
+
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x11 , 0x00 ) ;
+
+    for( i = 0 ; i < 8 ; i++ )
+        XGINew_SetReg1( pVBInfo->P3d4 , ( USHORT )i , CRTCData[ i ] ) ;
+
+    for( i = 8 ; i < 11 ; i++ )
+        XGINew_SetReg1( pVBInfo->P3d4 , ( USHORT )( i + 8 ) , CRTCData[ i ] ) ;
+
+    for( i = 11 ; i < 13 ; i++ )
+        XGINew_SetReg1( pVBInfo->P3d4 , ( USHORT )( i + 4 ) , CRTCData[ i ] ) ;
+
+    for( i = 13 ; i < 16 ; i++ )
+        XGINew_SetReg1( pVBInfo->P3c4 , ( USHORT )( i - 3 ) , CRTCData[ i ] ) ;
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x0E , ( UCHAR )( CRTCData[ 16 ] & 0xE0 ) ) ;
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x31 , 0x00 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2B , 0x1B ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2C , 0xE1 ) ;
+
+    XGINew_SetReg3( pVBInfo->P3c8 , 0x00 ) ;
+
+    for( i = 0 ; i < 256 ; i++ )
+    {
+        XGINew_SetReg3( ( pVBInfo->P3c8 + 1 ) , ( UCHAR )DAC_TEST_PARMS[ 0 ] ) ;
+        XGINew_SetReg3( ( pVBInfo->P3c8 + 1 ) , ( UCHAR )DAC_TEST_PARMS[ 1 ] ) ;
+        XGINew_SetReg3( ( pVBInfo->P3c8 + 1 ) , ( UCHAR )DAC_TEST_PARMS[ 2 ] ) ;
+    }
+
+    XGI_VBLongWait( pVBInfo ) ;
+    XGI_VBLongWait( pVBInfo ) ;
+    XGI_VBLongWait( pVBInfo ) ;
+
+    XGINew_LCD_Wait_Time( 0x01 , pVBInfo ) ;
+
+    XGI_WaitDisply( pVBInfo ) ;
+    temp = XGINew_GetReg2( pVBInfo->P3c2 ) ;
+
+    if( temp & 0x10 )
+    {
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x32 , 0xDF , 0x20 ) ;
+    }
+    else
+    {
+        XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x32 , 0xDF , 0x00 ) ;
+    }
+
+    /* alan, avoid display something, set BLACK DAC if not restore DAC */
+    XGINew_SetReg3( pVBInfo->P3c8 , 0x00 ) ;
+
+    for( i = 0 ; i < 256 ; i++ )
+    {
+      XGINew_SetReg3( ( pVBInfo->P3c8 + 1 ) , 0 ) ;
+      XGINew_SetReg3( ( pVBInfo->P3c8 + 1 ) , 0 ) ;
+      XGINew_SetReg3( ( pVBInfo->P3c8 + 1 ) , 0 ) ;
+    }
+
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x01 , SR01 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x63 , CR63 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x31 , SR31 ) ;
+
+    /* [2004/05/11] Vicent */
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x53 , ( UCHAR )( XGINew_GetReg1( pVBInfo->P3d4 , 0x53 ) & 0xFD ) ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x1F , ( UCHAR ) SR1F ) ;
+}
+
+
+
+
+
+#ifdef TC
+/* --------------------------------------------------------------------- */
+/* Function : INT1AReturnCode */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+int INT1AReturnCode( union REGS regs )
+{
+    if ( regs.x.cflag )
+    {
+        /* printf( "Error to find pci device!\n" ) ; */
+        return( 1 ) ;
+    }
+
+    switch(regs.h.ah)
+    {
+        case 0: return 0;
+            break ;
+        case 0x81:
+            printf( "Function not support\n" ) ;
+            break ;
+        case 0x83:
+            printf( "bad vendor id\n" ) ;
+            break ;
+        case 0x86:
+            printf( "device not found\n" ) ;
+            break ;
+        case 0x87:
+            printf( "bad register number\n" ) ;
+            break ;
+        case 0x88:
+            printf( "set failed\n" ) ;
+            break ;
+        case 0x89:
+            printf( "buffer too small" ) ;
+            break ;
+        default:
+            break ;
+    }
+    return( 1 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : FindPCIIOBase */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+unsigned FindPCIIOBase( unsigned index , unsigned deviceid )
+{
+    union REGS regs ;
+
+    regs.h.ah = 0xb1 ;  /* PCI_FUNCTION_ID */
+    regs.h.al = 0x02 ;  /* FIND_PCI_DEVICE */
+    regs.x.cx = deviceid ;
+    regs.x.dx = 0x1039 ;
+    regs.x.si = index ; /* find n-th device */
+
+    int86( 0x1A , &regs , &regs ) ;
+
+    if ( INT1AReturnCode( regs ) != 0 )
+        return( 0 ) ;
+
+    /* regs.h.bh bus number */
+    /* regs.h.bl device number */
+    regs.h.ah = 0xb1 ;  /* PCI_FUNCTION_ID */
+    regs.h.al = 0x09 ;  /* READ_CONFIG_WORD */
+    regs.x.cx = deviceid ;
+    regs.x.dx = 0x1039 ;
+    regs.x.di = 0x18 ;  /* register number */
+    int86( 0x1A , &regs , &regs ) ;
+
+    if ( INT1AReturnCode( regs ) != 0 )
+        return( 0 ) ;
+
+    return( regs.x.cx ) ;
+}
+
+#endif
+
+
+
+#ifdef TC
+/* --------------------------------------------------------------------- */
+/* Function : main */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void main(int argc, char *argv[])
+{
+    XGI_HW_DEVICE_INFO HwDeviceExtension ;
+    USHORT temp ;
+    USHORT ModeNo ;
+
+    /* HwDeviceExtension.pjVirtualRomBase =(PUCHAR) MK_FP(0xC000,0); */
+    /* HwDeviceExtension.pjVideoMemoryAddress = (PUCHAR)MK_FP(0xA000,0); */
+
+
+    HwDeviceExtension.pjIOAddress = ( FindPCIIOBase( 0 ,0x6300 ) & 0xFF80 ) + 0x30 ;
+    HwDeviceExtension.jChipType = XGI_340 ;
+
+
+
+    /* HwDeviceExtension.pjIOAddress = ( FindPCIIOBase( 0 , 0x5315 ) & 0xFF80 ) + 0x30 ; */
+
+    HwDeviceExtension.pjIOAddress = ( FindPCIIOBase( 0 , 0x330 ) & 0xFF80 ) + 0x30 ;
+    HwDeviceExtension.jChipType = XGI_340 ;
+
+
+    HwDeviceExtension.ujVBChipID = VB_CHIP_301 ;
+    StrCpy(HwDeviceExtension.szVBIOSVer , "0.84" ) ;
+    HwDeviceExtension.bSkipDramSizing = FALSE ;
+    HwDeviceExtension.ulVideoMemorySize = 0 ;
+
+    if ( argc == 2 )
+    {
+        ModeNo = atoi( argv[ 1 ] ) ;
+    }
+    else
+    {
+        ModeNo = 0x2e ;
+        /* ModeNo = 0x37 ; 1024x768x 4bpp */
+        /* ModeNo = 0x38 ; 1024x768x 8bpp */
+        /* ModeNo = 0x4A ; 1024x768x 16bpp */
+        /* ModeNo = 0x47 ; 800x600x 16bpp */
+    }
+
+    /* XGIInitNew( &HwDeviceExtension ) ; */
+    XGISetModeNew( &HwDeviceExtension , ModeNo ) ;
+}
+#endif
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_WaitDisplay */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_WaitDisplay( PVB_DEVICE_INFO pVBInfo )
+{
+    while( !( XGINew_GetReg2( pVBInfo->P3da ) & 0x01 ) ) ;
+
+    while( XGINew_GetReg2( pVBInfo->P3da ) & 0x01 ) ;
+}
+
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT2Group301 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_SetCRT2Group301( USHORT ModeNo , PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempbx ,
+           ModeIdIndex ,
+           RefreshRateTableIndex ;
+
+    tempbx=pVBInfo->VBInfo ;
+    pVBInfo->SetFlag |= ProgrammingCRT2 ;
+    XGI_SearchModeID( ModeNo , &ModeIdIndex,  pVBInfo ) ;
+    pVBInfo->SelectCRT2Rate = 4 ;
+    RefreshRateTableIndex = XGI_GetRatePtrCRT2( HwDeviceExtension, ModeNo , ModeIdIndex, pVBInfo ) ;
+    XGI_SaveCRT2Info( ModeNo, pVBInfo ) ;
+    XGI_GetCRT2ResInfo( ModeNo , ModeIdIndex, pVBInfo) ;
+    XGI_GetCRT2Data( ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+    XGI_PreSetGroup1( ModeNo , ModeIdIndex , HwDeviceExtension , RefreshRateTableIndex, pVBInfo ) ;
+    XGI_SetGroup1( ModeNo , ModeIdIndex , HwDeviceExtension , RefreshRateTableIndex, pVBInfo ) ;
+    XGI_SetLockRegs( ModeNo , ModeIdIndex , HwDeviceExtension , RefreshRateTableIndex, pVBInfo ) ;
+    XGI_SetGroup2(  ModeNo , ModeIdIndex , RefreshRateTableIndex , HwDeviceExtension, pVBInfo ) ;
+    XGI_SetLCDRegs(ModeNo , ModeIdIndex , HwDeviceExtension , RefreshRateTableIndex, pVBInfo ) ;
+    XGI_SetTap4Regs(pVBInfo) ;
+    XGI_SetGroup3(ModeNo, ModeIdIndex, pVBInfo);
+    XGI_SetGroup4( ModeNo , ModeIdIndex , RefreshRateTableIndex , HwDeviceExtension, pVBInfo ) ;
+    XGI_SetCRT2VCLK( ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+    XGI_SetGroup5( ModeNo , ModeIdIndex, pVBInfo) ;
+    XGI_AutoThreshold( pVBInfo) ;
+    return 1 ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_AutoThreshold */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_AutoThreshold(  PVB_DEVICE_INFO pVBInfo )
+{
+    if ( !( pVBInfo->SetFlag & Win9xDOSMode ) )
+      XGINew_SetRegOR( pVBInfo->Part1Port , 0x01 , 0x40 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SaveCRT2Info */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SaveCRT2Info( USHORT ModeNo , PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT temp1 ,
+           temp2 ;
+
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x34 , ModeNo ) ;  /* reserve CR34 for CRT1 Mode No */
+    temp1 = ( pVBInfo->VBInfo&SetInSlaveMode ) >> 8 ;
+    temp2 = ~( SetInSlaveMode >> 8 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x31 , temp2 , temp1 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetCRT2ResInfo */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetCRT2ResInfo( USHORT ModeNo , USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT xres ,
+           yres ,
+           modeflag ,
+           resindex ;
+
+    resindex = XGI_GetResInfo( ModeNo , ModeIdIndex, pVBInfo) ;
+    if ( ModeNo <= 0x13 )
+    {
+        xres = pVBInfo->StResInfo[ resindex ].HTotal ;
+        yres = pVBInfo->StResInfo[ resindex ].VTotal ;
+     /* modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag; si+St_ResInfo */
+    }
+    else
+    {
+        xres = pVBInfo->ModeResInfo[ resindex ].HTotal ;                        /* xres->ax */
+        yres = pVBInfo->ModeResInfo[ resindex ].VTotal ;                        /* yres->bx */
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex].Ext_ModeFlag ;           /* si+St_ModeFlag */
+
+/*        if ( pVBInfo->IF_DEF_FSTN )
+        {
+            xres *= 2 ;
+            yres *= 2 ;
+        }
+        else
+        {
+*/
+            if ( modeflag & HalfDCLK )
+                xres *= 2;
+
+            if ( modeflag & DoubleScanMode )
+                yres *= 2 ;
+/* } */
+    }
+
+    if ( pVBInfo->VBInfo & SetCRT2ToLCD )
+    {
+        if ( pVBInfo->IF_DEF_LVDS == 0 )
+        {
+            if ( pVBInfo->LCDResInfo == Panel1600x1200 )
+            {
+                if ( !( pVBInfo->LCDInfo & LCDVESATiming ) )
+                {
+                    if ( yres == 1024 )
+                        yres = 1056 ;
+                }
+            }
+
+            if ( pVBInfo->LCDResInfo == Panel1280x1024 )
+            {
+                if ( yres == 400 )
+                    yres = 405 ;
+                else if ( yres == 350 )
+                    yres = 360 ;
+
+                if ( pVBInfo->LCDInfo & LCDVESATiming )
+                {
+                    if ( yres == 360 )
+                        yres = 375 ;
+                }
+            }
+
+            if ( pVBInfo->LCDResInfo == Panel1024x768 )
+            {
+                if ( !( pVBInfo->LCDInfo & LCDVESATiming ) )
+                {
+                    if ( !( pVBInfo->LCDInfo & LCDNonExpanding ) )
+                    {
+                        if ( yres == 350 )
+                            yres = 357 ;
+                        else if ( yres == 400 )
+                            yres = 420 ;
+                        else if ( yres == 480 )
+                            yres = 525 ;
+                    }
+                }
+            }
+        }
+
+        if ( xres == 720 )
+            xres = 640 ;
+    }
+
+    pVBInfo->VGAHDE = xres ;
+    pVBInfo->HDE = xres ;
+    pVBInfo->VGAVDE = yres ;
+    pVBInfo->VDE = yres ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_IsLCDDualLink */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_IsLCDDualLink( PVB_DEVICE_INFO pVBInfo )
+{
+
+    if ( ( ( ( pVBInfo->VBInfo & SetCRT2ToLCD ) | SetCRT2ToLCDA ) ) && ( pVBInfo->LCDInfo & SetLCDDualLink ) ) /* shampoo0129 */
+        return ( 1 ) ;
+
+    return( 0 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetCRT2Data */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetCRT2Data(  USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempax = 0,
+           tempbx ,
+           modeflag ,
+           resinfo ;
+
+    XGI_LCDDataStruct *LCDPtr = NULL ;
+    XGI_TVDataStruct  *TVPtr = NULL ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;      /* si+St_ResInfo */
+        resinfo = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ResInfo ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;     /* si+Ext_ResInfo */
+        resinfo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ;
+    }
+
+    pVBInfo->NewFlickerMode = 0 ;
+    pVBInfo->RVBHRS = 50 ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToRAMDAC )
+    {
+        XGI_GetRAMDAC2DATA( ModeNo , ModeIdIndex , RefreshRateTableIndex,pVBInfo ) ;
+        return ;
+    }
+
+    tempbx = 4 ;
+
+    if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) )
+    {
+        LCDPtr = (XGI_LCDDataStruct* )XGI_GetLcdPtr( tempbx, ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+
+        pVBInfo->RVBHCMAX = LCDPtr->RVBHCMAX ;
+        pVBInfo->RVBHCFACT = LCDPtr->RVBHCFACT ;
+        pVBInfo->VGAHT = LCDPtr->VGAHT ;
+        pVBInfo->VGAVT = LCDPtr->VGAVT ;
+        pVBInfo->HT = LCDPtr->LCDHT ;
+        pVBInfo->VT = LCDPtr->LCDVT ;
+
+        if ( pVBInfo->LCDResInfo == Panel1024x768 )
+        {
+            tempax = 1024 ;
+            tempbx = 768 ;
+
+            if ( !( pVBInfo->LCDInfo & LCDVESATiming ) )
+            {
+                if ( pVBInfo->VGAVDE == 357 )
+                    tempbx = 527 ;
+                else if ( pVBInfo->VGAVDE == 420 )
+                    tempbx = 620 ;
+                else if ( pVBInfo->VGAVDE == 525 )
+                    tempbx = 775 ;
+                else if ( pVBInfo->VGAVDE == 600 )
+                    tempbx = 775 ;
+                /* else if(pVBInfo->VGAVDE==350) tempbx=560; */
+                /* else if(pVBInfo->VGAVDE==400) tempbx=640; */
+                else
+                  tempbx = 768 ;
+            }
+            else
+                tempbx = 768 ;
+        }
+        else if ( pVBInfo->LCDResInfo == Panel1024x768x75 )
+        {
+            tempax = 1024 ;
+            tempbx = 768 ;
+        }
+        else if ( pVBInfo->LCDResInfo == Panel1280x1024 )
+        {
+            tempax = 1280 ;
+            if ( pVBInfo->VGAVDE == 360 )
+                tempbx = 768 ;
+            else if ( pVBInfo->VGAVDE == 375 )
+                tempbx = 800 ;
+            else if ( pVBInfo->VGAVDE == 405 )
+                tempbx = 864 ;
+            else
+                tempbx = 1024 ;
+        }
+        else if ( pVBInfo->LCDResInfo == Panel1280x1024x75 )
+        {
+            tempax = 1280 ;
+            tempbx = 1024 ;
+        }
+        else if ( pVBInfo->LCDResInfo == Panel1280x960 )
+        {
+            tempax = 1280 ;
+            if ( pVBInfo->VGAVDE == 350 )
+                tempbx = 700 ;
+            else if ( pVBInfo->VGAVDE == 400 )
+                tempbx = 800 ;
+            else if ( pVBInfo->VGAVDE == 1024 )
+                tempbx = 960 ;
+            else
+                tempbx = 960 ;
+        }
+        else if ( pVBInfo->LCDResInfo == Panel1400x1050 )
+        {
+            tempax = 1400 ;
+            tempbx = 1050 ;
+
+            if ( pVBInfo->VGAVDE == 1024 )
+            {
+                tempax = 1280 ;
+                tempbx = 1024 ;
+            }
+        }
+        else if ( pVBInfo->LCDResInfo == Panel1600x1200 )
+        {
+            tempax = 1600 ;
+            tempbx = 1200 ;  /* alan 10/14/2003 */
+            if ( !( pVBInfo->LCDInfo & LCDVESATiming ) )
+            {
+              if ( pVBInfo->VGAVDE == 350 )
+                tempbx = 875 ;
+              else if ( pVBInfo->VGAVDE == 400 )
+                tempbx = 1000 ;
+            }
+        }
+
+        if ( pVBInfo->LCDInfo & LCDNonExpanding )
+        {
+            tempax = pVBInfo->VGAHDE ;
+            tempbx = pVBInfo->VGAVDE ;
+        }
+
+        pVBInfo->HDE = tempax ;
+        pVBInfo->VDE = tempbx ;
+        return ;
+    }
+
+    if ( pVBInfo->VBInfo & ( SetCRT2ToTV ) )
+    {
+        tempbx = 4 ;
+        TVPtr = ( XGI_TVDataStruct * )XGI_GetTVPtr( tempbx , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+
+        pVBInfo->RVBHCMAX = TVPtr->RVBHCMAX ;
+        pVBInfo->RVBHCFACT = TVPtr->RVBHCFACT ;
+        pVBInfo->VGAHT = TVPtr->VGAHT ;
+        pVBInfo->VGAVT = TVPtr->VGAVT ;
+        pVBInfo->HDE = TVPtr->TVHDE ;
+        pVBInfo->VDE = TVPtr->TVVDE ;
+        pVBInfo->RVBHRS = TVPtr->RVBHRS ;
+        pVBInfo->NewFlickerMode = TVPtr->FlickerMode ;
+
+        if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+        {
+            if ( resinfo == 0x08 )
+                pVBInfo->NewFlickerMode = 0x40 ;
+            else if ( resinfo == 0x09 )
+                pVBInfo->NewFlickerMode = 0x40 ;
+            else if ( resinfo == 0x12 )
+                pVBInfo->NewFlickerMode = 0x40 ;
+
+            if ( pVBInfo->VGAVDE == 350 )
+                pVBInfo->TVInfo |= TVSimuMode ;
+
+            tempax = ExtHiTVHT ;
+            tempbx = ExtHiTVVT ;
+
+            if ( pVBInfo->VBInfo & SetInSlaveMode )
+            {
+                if ( pVBInfo->TVInfo & TVSimuMode )
+                {
+                    tempax = StHiTVHT ;
+                    tempbx = StHiTVVT ;
+
+                    if ( !( modeflag & Charx8Dot ) )
+                    {
+                        tempax = StHiTextTVHT ;
+                        tempbx = StHiTextTVVT ;
+                    }
+                }
+            }
+        }
+        else if ( pVBInfo->VBInfo & SetCRT2ToYPbPr )
+        {
+            if ( pVBInfo->TVInfo & SetYPbPrMode750p )
+            {
+                tempax = YPbPrTV750pHT ;        /* Ext750pTVHT */
+                tempbx = YPbPrTV750pVT ;        /* Ext750pTVVT */
+            }
+
+            if ( pVBInfo->TVInfo & SetYPbPrMode525p )
+            {
+                tempax = YPbPrTV525pHT ;        /* Ext525pTVHT */
+                tempbx = YPbPrTV525pVT ;        /* Ext525pTVVT */
+            }
+            else if ( pVBInfo->TVInfo & SetYPbPrMode525i )
+            {
+                tempax = YPbPrTV525iHT ;        /* Ext525iTVHT */
+                tempbx = YPbPrTV525iVT ;        /* Ext525iTVVT */
+                if ( pVBInfo->TVInfo & NTSC1024x768 )
+                    tempax = NTSC1024x768HT ;
+            }
+        }
+        else
+        {
+            tempax = PALHT ;
+            tempbx = PALVT ;
+            if ( !( pVBInfo->TVInfo & SetPALTV ) )
+            {
+                tempax = NTSCHT ;
+                tempbx = NTSCVT ;
+                if ( pVBInfo->TVInfo & NTSC1024x768 )
+                    tempax = NTSC1024x768HT ;
+            }
+        }
+
+        pVBInfo->HT = tempax ;
+        pVBInfo->VT = tempbx ;
+        return ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT2VCLK */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT2VCLK( USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR di_0 ,
+          di_1 ,
+          tempal ;
+
+    tempal = XGI_GetVCLKPtr( RefreshRateTableIndex , ModeNo , ModeIdIndex, pVBInfo ) ;
+    XGI_GetVCLKLen( tempal, &di_0 , &di_1, pVBInfo ) ;
+    XGI_GetLCDVCLKPtr( &di_0 , &di_1, pVBInfo ) ;
+
+    if ( pVBInfo->VBType & VB_XGI301 ) /* shampoo 0129 */
+    {                           /* 301 */
+        XGINew_SetReg1(pVBInfo->Part4Port , 0x0A , 0x10 ) ;
+        XGINew_SetReg1(pVBInfo->Part4Port , 0x0B , di_1 ) ;
+        XGINew_SetReg1(pVBInfo->Part4Port , 0x0A , di_0 ) ;
+    }
+    else
+    {                           /* 301b/302b/301lv/302lv */
+        XGINew_SetReg1( pVBInfo->Part4Port , 0x0A , di_0 ) ;
+        XGINew_SetReg1( pVBInfo->Part4Port , 0x0B , di_1 ) ;
+    }
+
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x00 , 0x12 ) ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToRAMDAC )
+        XGINew_SetRegOR( pVBInfo->Part4Port , 0x12 , 0x28 ) ;
+    else
+        XGINew_SetRegOR( pVBInfo->Part4Port , 0x12 , 0x08 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GETLCDVCLKPtr */
+/* Input : */
+/* Output : al -> VCLK Index */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetLCDVCLKPtr( UCHAR* di_0 , UCHAR *di_1, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT index ;
+
+    if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) )
+    {
+        if ( pVBInfo->IF_DEF_ScaleLCD == 1 )
+        {
+            if ( pVBInfo->LCDInfo & EnableScalingLCD )
+                return ;
+        }
+
+        /* index = XGI_GetLCDCapPtr(pVBInfo) ; */
+        index = XGI_GetLCDCapPtr1( pVBInfo) ;
+
+        if ( pVBInfo->VBInfo & SetCRT2ToLCD )
+        {       /* LCDB */
+            *di_0 = pVBInfo->LCDCapList[ index ].LCUCHAR_VCLKData1 ;
+            *di_1 = pVBInfo->LCDCapList[ index ].LCUCHAR_VCLKData2 ;
+        }
+        else
+        {       /* LCDA */
+            *di_0 = pVBInfo->LCDCapList[ index ].LCDA_VCLKData1 ;
+            *di_1 = pVBInfo->LCDCapList[ index ].LCDA_VCLKData2 ;
+        }
+    }
+    return ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetVCLKPtr */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+UCHAR XGI_GetVCLKPtr(USHORT RefreshRateTableIndex,USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo)
+{
+
+    USHORT index ,
+           modeflag ;
+#ifndef LINUX_XF86
+    USHORT tempbx ;
+#endif
+
+    UCHAR tempal ;
+    UCHAR *CHTVVCLKPtr = NULL ;
+
+    if ( ModeNo <= 0x13 )
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;      /* si+St_ResInfo */
+    else
+        modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;    /* si+Ext_ResInfo */
+
+
+    if ( ( pVBInfo->SetFlag & ProgrammingCRT2 ) && ( !( pVBInfo->LCDInfo & EnableScalingLCD ) ) )
+    {   /* {LCDA/LCDB} */
+        index = XGI_GetLCDCapPtr(pVBInfo) ;
+        tempal = pVBInfo->LCDCapList[ index ].LCD_VCLK ;
+
+        if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) )
+            return tempal ;
+
+        /* {TV} */
+        if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV| VB_XGI302LV| VB_XGI301C ) )
+        {
+            if(pVBInfo->VBInfo&SetCRT2ToHiVisionTV)
+           {
+                tempal = HiTVVCLKDIV2;
+                if(!(pVBInfo->TVInfo & RPLLDIV2XO))
+                    tempal = HiTVVCLK;
+                if(pVBInfo->TVInfo & TVSimuMode)
+                {
+                    tempal = HiTVSimuVCLK;
+                    if(!(modeflag & Charx8Dot))
+                        tempal = HiTVTextVCLK;
+
+                 }
+                 return tempal;
+             }
+
+            if ( pVBInfo->TVInfo & SetYPbPrMode750p )
+            {
+                tempal = YPbPr750pVCLK ;
+                return tempal ;
+            }
+
+            if ( pVBInfo->TVInfo & SetYPbPrMode525p )
+            {
+                tempal = YPbPr525pVCLK ;
+                return tempal ;
+            }
+
+            tempal = NTSC1024VCLK ;
+
+            if ( !( pVBInfo->TVInfo & NTSC1024x768 ) )
+            {
+                tempal = TVVCLKDIV2 ;
+                if ( !( pVBInfo->TVInfo & RPLLDIV2XO ) )
+                    tempal = TVVCLK ;
+            }
+
+            if ( pVBInfo->VBInfo & SetCRT2ToTV )
+                return tempal ;
+        }
+        /*else
+        if((pVBInfo->IF_DEF_CH7017==1)&&(pVBInfo->VBType&VB_CH7017))
+        {
+            if(ModeNo<=0x13)
+                *tempal = pVBInfo->SModeIDTable[ModeIdIndex].St_CRT2CRTC;
+            else
+                *tempal = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
+            *tempal = *tempal & 0x1F;
+
+            tempbx = 0;
+            if(pVBInfo->TVInfo & SetPALTV)
+                tempbx = tempbx + 2;
+            if(pVBInfo->TVInfo & SetCHTVOverScan)
+                tempbx++;
+            tempbx = tempbx << 1;
+        }  */
+    }   /* {End of VB} */
+
+    if((pVBInfo->IF_DEF_CH7007==1)&&(pVBInfo->VBType&VB_CH7007)) /* [Billy] 07/05/08 CH7007 */
+    {
+       /* VideoDebugPrint((0, "XGI_GetVCLKPtr: pVBInfo->IF_DEF_CH7007==1\n")); */
+       if ( (pVBInfo->VBInfo & SetCRT2ToTV) )
+       {
+           if( ModeNo <= 0x13 )
+           {
+              tempal = pVBInfo->SModeIDTable[ ModeIdIndex ].St_CRT2CRTC ;
+           }
+           else
+           {
+              tempal = pVBInfo->RefIndex[RefreshRateTableIndex].Ext_CRT2CRTC;
+           }
+
+           tempal = tempal & 0x0F;
+           tempbx = 0;
+
+           if(pVBInfo->TVInfo & SetPALTV)
+           {
+              tempbx = tempbx + 2;
+           }
+           if(pVBInfo->TVInfo & SetCHTVOverScan)
+           {
+              tempbx++;
+           }
+           /** tempbx = tempbx << 1; CH7007 ? **/
+
+/*[Billy]07/05/29 CH7007*/
+           if ( pVBInfo->IF_DEF_CH7007 == 1 )
+           {
+             switch( tempbx )
+             {
+               case 0:
+                   CHTVVCLKPtr = XGI7007_CHTVVCLKUNTSC ;
+                   break ;
+               case 1:
+                   CHTVVCLKPtr = XGI7007_CHTVVCLKONTSC ;
+                   break ;
+               case 2:
+                   CHTVVCLKPtr = XGI7007_CHTVVCLKUPAL ;
+                   break ;
+               case 3:
+                   CHTVVCLKPtr = XGI7007_CHTVVCLKOPAL ;
+                   break ;
+               default:
+                   break ;
+
+             }
+           }
+           /*else
+           {
+            switch( tempbx )
+            {
+               case 0:
+                   CHTVVCLKPtr = pVBInfo->CHTVVCLKUNTSC ;
+                   break ;
+               case 1:
+                   CHTVVCLKPtr = pVBInfo->CHTVVCLKONTSC ;
+                   break ;
+               case 2:
+                   CHTVVCLKPtr = pVBInfo->CHTVVCLKUPAL ;
+                   break ;
+               case 3:
+                   CHTVVCLKPtr = pVBInfo->CHTVVCLKOPAL ;
+                   break ;
+               default:
+                   break ;
+            }
+           }*/
+
+           tempal = CHTVVCLKPtr[ tempal ] ;
+           return tempal ;
+       }
+
+    }
+
+    tempal = ( UCHAR )XGINew_GetReg2( ( pVBInfo->P3ca + 0x02 ) ) ;
+    tempal = tempal >> 2 ;
+    tempal &= 0x03 ;
+
+    if ( ( pVBInfo->LCDInfo & EnableScalingLCD ) && ( modeflag & Charx8Dot ) )  /* for Dot8 Scaling LCD */
+        tempal = tempal ^ tempal ;                   /* ; set to VCLK25MHz always */
+
+    if ( ModeNo <= 0x13 )
+        return tempal ;
+
+    tempal = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRTVCLK ;
+    return tempal ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetVCLKLen */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetVCLKLen(UCHAR tempal,UCHAR* di_0,UCHAR* di_1, PVB_DEVICE_INFO pVBInfo)
+{
+    if ( pVBInfo->IF_DEF_CH7007 == 1 ) /* [Billy] 2007/05/16 */
+    {
+       /* VideoDebugPrint((0, "XGI_GetVCLKLen: pVBInfo->IF_DEF_CH7007==1\n")); */
+        *di_0 = ( UCHAR )XGI_CH7007VCLKData[ tempal ].SR2B ;
+        *di_1 = ( UCHAR )XGI_CH7007VCLKData[ tempal ].SR2C ;
+    }
+    else if ( pVBInfo->VBType & ( VB_XGI301 | VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        if ( ( !( pVBInfo->VBInfo & SetCRT2ToLCDA ) ) && ( pVBInfo->SetFlag & ProgrammingCRT2 ) )
+        {
+            *di_0 = ( UCHAR )XGI_VBVCLKData[ tempal ].SR2B ;
+            *di_1 = XGI_VBVCLKData[ tempal ].SR2C ;
+        }
+    }
+    else
+    {
+        *di_0 = XGI_VCLKData[ tempal ].SR2B ;
+        *di_1 = XGI_VCLKData[ tempal ].SR2C ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT2Offset */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT2Offset( USHORT ModeNo ,
+                                   USHORT ModeIdIndex , USHORT RefreshRateTableIndex , PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT offset ;
+    UCHAR temp ;
+
+    if ( pVBInfo->VBInfo & SetInSlaveMode )
+    {
+        return ;
+    }
+
+    offset = XGI_GetOffset(  ModeNo , ModeIdIndex , RefreshRateTableIndex , HwDeviceExtension, pVBInfo ) ;
+    temp = ( UCHAR )( offset & 0xFF ) ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x07 , temp ) ;
+    temp =( UCHAR)( ( offset & 0xFF00 ) >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x09 , temp ) ;
+    temp =( UCHAR )( ( ( offset >> 3 ) & 0xFF ) + 1 ) ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x03 , temp ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetOffset */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+USHORT XGI_GetOffset(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension,PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT temp ,
+           colordepth ,
+           modeinfo ,
+           index ,
+           infoflag ,
+           ColorDepth[] = { 0x01 , 0x02 , 0x04 } ;
+
+    modeinfo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeInfo ;
+    if ( ModeNo <= 0x14 )
+        infoflag = 0 ;
+    else
+        infoflag = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_InfoFlag ;
+
+
+    index = ( modeinfo >> 8 ) & 0xFF ;
+
+    temp = pVBInfo->ScreenOffset[ index ] ;
+
+    if ( infoflag & InterlaceMode )
+    {
+        temp = temp << 1 ;
+    }
+
+    colordepth = XGI_GetColorDepth( ModeNo , ModeIdIndex, pVBInfo ) ;
+
+    if ( ( ModeNo >= 0x7C ) && ( ModeNo <= 0x7E ) )
+    {
+        temp = ModeNo - 0x7C ;
+        colordepth = ColorDepth[ temp ] ;
+        temp = 0x6B ;
+        if ( infoflag & InterlaceMode )
+        {
+            temp = temp << 1 ;
+        }
+        return( temp * colordepth ) ;
+    }
+    else
+        return( temp * colordepth ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT2FIFO */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT2FIFO( PVB_DEVICE_INFO pVBInfo)
+{
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x01 , 0x3B ) ;                        /* threshold high ,disable auto threshold */
+    XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x02 , ~( 0x3F ) , 0x04 ) ;        /* threshold low default 04h */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_PreSetGroup1 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_PreSetGroup1(USHORT ModeNo , USHORT ModeIdIndex ,PXGI_HW_DEVICE_INFO HwDeviceExtension,
+                       USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempcx = 0 ,
+           CRT1Index = 0 ,
+           resinfo = 0 ;
+
+    if ( ModeNo > 0x13 )
+    {
+        CRT1Index = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT1CRTC ;
+        CRT1Index &= IndexMask ;
+        resinfo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ;
+    }
+
+    XGI_SetCRT2Offset( ModeNo , ModeIdIndex , RefreshRateTableIndex , HwDeviceExtension, pVBInfo ) ;
+    XGI_SetCRT2FIFO(pVBInfo) ;
+    /* XGI_SetCRT2Sync(ModeNo,RefreshRateTableIndex); */
+
+    for( tempcx = 4 ; tempcx < 7 ; tempcx++ )
+    {
+        XGINew_SetReg1( pVBInfo->Part1Port , tempcx , 0x0 ) ;
+    }
+
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x50 , 0x00 ) ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x02 , 0x44 ) ;        /* temp 0206 */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetGroup1 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetGroup1( USHORT ModeNo , USHORT ModeIdIndex ,
+                            PXGI_HW_DEVICE_INFO HwDeviceExtension , USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT temp = 0 ,
+           tempax = 0 ,
+           tempbx = 0 ,
+           tempcx = 0 ,
+           pushbx = 0 ,
+           CRT1Index = 0 ,
+           modeflag ,
+           resinfo = 0 ;
+
+    if ( ModeNo > 0x13 )
+    {
+        CRT1Index = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT1CRTC ;
+        CRT1Index &= IndexMask ;
+        resinfo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ;
+    }
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+    }
+
+    /* bainy change table name */
+    if ( modeflag & HalfDCLK )
+    {
+        temp = ( pVBInfo->VGAHT / 2 - 1 ) & 0x0FF ;                     /* BTVGA2HT 0x08,0x09 */
+        XGINew_SetReg1( pVBInfo->Part1Port , 0x08 , temp ) ;
+        temp = ( ( ( pVBInfo->VGAHT / 2 - 1 ) & 0xFF00 ) >> 8 ) << 4 ;
+        XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x09 , ~0x0F0 , temp ) ;
+        temp = ( pVBInfo->VGAHDE / 2 + 16 ) & 0x0FF ;                   /* BTVGA2HDEE 0x0A,0x0C */
+        XGINew_SetReg1( pVBInfo->Part1Port , 0x0A , temp ) ;
+        tempcx = ( ( pVBInfo->VGAHT - pVBInfo->VGAHDE ) / 2 ) >> 2 ;
+        pushbx = pVBInfo->VGAHDE / 2 + 16 ;
+        tempcx = tempcx >> 1 ;
+        tempbx = pushbx + tempcx ;                                      /* bx BTVGA@HRS 0x0B,0x0C */
+        tempcx += tempbx ;
+
+        if ( pVBInfo->VBInfo & SetCRT2ToRAMDAC )
+        {
+            tempbx = pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 4 ] ;
+            tempbx |= ( ( pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 14 ] & 0xC0 ) << 2 ) ;
+            tempbx = ( tempbx - 3 ) << 3 ;                              /* (VGAHRS-3)*8 */
+            tempcx = pVBInfo->XGINEWUB_CRT1Table[CRT1Index].CR[ 5 ] ;
+            tempcx &= 0x1F ;
+            temp = pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 15 ] ;
+            temp = ( temp & 0x04 ) << ( 5 - 2 ) ;                       /* VGAHRE D[5] */
+            tempcx = ( ( tempcx | temp ) - 3 ) << 3 ;                   /* (VGAHRE-3)*8 */
+        }
+
+        tempbx += 4 ;
+        tempcx += 4 ;
+
+        if ( tempcx > ( pVBInfo->VGAHT / 2 ) )
+            tempcx = pVBInfo->VGAHT / 2 ;
+
+        temp = tempbx & 0x00FF ;
+
+        XGINew_SetReg1( pVBInfo->Part1Port , 0x0B , temp ) ;
+    }
+    else
+    {
+        temp = ( pVBInfo->VGAHT - 1 ) & 0x0FF ;                         /* BTVGA2HT 0x08,0x09 */
+        XGINew_SetReg1( pVBInfo->Part1Port , 0x08 , temp ) ;
+        temp = ( ( ( pVBInfo->VGAHT - 1 ) & 0xFF00 ) >> 8 ) << 4 ;
+        XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x09 , ~0x0F0 , temp ) ;
+        temp = ( pVBInfo->VGAHDE + 16 ) & 0x0FF ;                               /* BTVGA2HDEE 0x0A,0x0C */
+        XGINew_SetReg1( pVBInfo->Part1Port , 0x0A , temp ) ;
+        tempcx = ( pVBInfo->VGAHT - pVBInfo->VGAHDE ) >> 2 ;            /* cx */
+        pushbx = pVBInfo->VGAHDE + 16 ;
+        tempcx = tempcx >> 1 ;
+        tempbx = pushbx + tempcx ;                                      /* bx BTVGA@HRS 0x0B,0x0C */
+        tempcx += tempbx ;
+
+        if ( pVBInfo->VBInfo & SetCRT2ToRAMDAC )
+        {
+            tempbx = pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 3 ] ;
+            tempbx |= ( ( pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 5 ] & 0xC0 ) << 2 ) ;
+            tempbx = ( tempbx - 3 ) << 3 ;                              /* (VGAHRS-3)*8 */
+            tempcx = pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 4 ] ;
+            tempcx &= 0x1F ;
+            temp = pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 6 ] ;
+            temp = ( temp & 0x04 ) << ( 5 - 2 ) ;                       /* VGAHRE D[5] */
+            tempcx = ( ( tempcx | temp ) - 3 ) << 3 ;                   /* (VGAHRE-3)*8 */
+            tempbx += 16 ;
+            tempcx += 16 ;
+        }
+
+        if ( tempcx > pVBInfo->VGAHT )
+            tempcx = pVBInfo->VGAHT ;
+
+        temp = tempbx & 0x00FF ;
+        XGINew_SetReg1( pVBInfo->Part1Port , 0x0B , temp ) ;
+    }
+
+    tempax = ( tempax & 0x00FF ) | ( tempbx & 0xFF00 ) ;
+    tempbx = pushbx ;
+    tempbx = ( tempbx & 0x00FF ) | ( ( tempbx & 0xFF00 ) << 4 ) ;
+    tempax |= ( tempbx & 0xFF00 ) ;
+    temp = ( tempax & 0xFF00 ) >> 8 ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x0C , temp ) ;
+    temp = tempcx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x0D , temp ) ;
+    tempcx = ( pVBInfo->VGAVT - 1 ) ;
+    temp = tempcx & 0x00FF ;
+
+    if ( pVBInfo->IF_DEF_CH7005 == 1 )
+    {
+        if ( pVBInfo->VBInfo & 0x0C )
+        {
+            temp-- ;
+        }
+    }
+
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x0E , temp ) ;
+    tempbx = pVBInfo->VGAVDE - 1 ;
+    temp = tempbx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x0F , temp ) ;
+    temp = ( ( tempbx & 0xFF00 ) << 3 ) >> 8 ;
+    temp |= ( ( tempcx & 0xFF00 ) >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x12 , temp ) ;
+
+    tempax = pVBInfo->VGAVDE ;
+    tempbx = pVBInfo->VGAVDE ;
+    tempcx = pVBInfo->VGAVT ;
+    tempbx = ( pVBInfo->VGAVT + pVBInfo->VGAVDE ) >> 1 ;                                /* BTVGA2VRS 0x10,0x11 */
+    tempcx = ( ( pVBInfo->VGAVT - pVBInfo->VGAVDE ) >> 4 ) + tempbx + 1 ;               /* BTVGA2VRE 0x11 */
+
+    if ( pVBInfo->VBInfo & SetCRT2ToRAMDAC )
+    {
+        tempbx = pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 10 ] ;
+        temp = pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 9 ] ;
+
+        if ( temp & 0x04 )
+            tempbx |= 0x0100 ;
+
+        if ( temp & 0x080 )
+            tempbx |= 0x0200 ;
+
+        temp = pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 14 ] ;
+
+        if ( temp & 0x08 )
+            tempbx |= 0x0400 ;
+
+        temp = pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 11 ] ;
+        tempcx = ( tempcx & 0xFF00 ) | ( temp & 0x00FF ) ;
+    }
+
+    temp = tempbx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x10 , temp ) ;
+    temp = ( ( tempbx & 0xFF00 ) >> 8 ) << 4 ;
+    temp = ( ( tempcx & 0x000F ) | ( temp ) ) ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x11 , temp ) ;
+    tempax = 0 ;
+
+    if ( modeflag & DoubleScanMode )
+        tempax |= 0x80 ;
+
+    if ( modeflag & HalfDCLK )
+        tempax |= 0x40 ;
+
+    XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x2C , ~0x0C0 , tempax ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetLockRegs */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void  XGI_SetLockRegs( USHORT ModeNo , USHORT ModeIdIndex ,
+                                PXGI_HW_DEVICE_INFO HwDeviceExtension , USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT push1 ,
+           push2 ,
+           tempax ,
+           tempbx = 0 ,
+           tempcx ,
+           temp ,
+           resinfo ,
+           modeflag ,
+           CRT1Index ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;   /* si+St_ResInfo */
+        resinfo = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ResInfo ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;  /* si+Ext_ResInfo */
+        resinfo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ;
+        CRT1Index = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT1CRTC ;
+        CRT1Index &= IndexMask;
+    }
+
+    if ( !( pVBInfo->VBInfo & SetInSlaveMode ) )
+    {
+        return ;
+    }
+
+    temp = 0xFF ;                                                       /* set MAX HT */
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x03 , temp ) ;
+    /* if ( modeflag & Charx8Dot ) tempcx = 0x08 ; */
+    /* else */
+    tempcx=0x08;
+
+    if ( pVBInfo->VBType & ( VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+        modeflag |= Charx8Dot ;
+
+    tempax = pVBInfo->VGAHDE ;                                          /* 0x04 Horizontal Display End */
+
+    if ( modeflag & HalfDCLK )
+        tempax = tempax >> 1 ;
+
+    tempax = ( tempax / tempcx ) - 1 ;
+    tempbx |= ( ( tempax & 0x00FF ) << 8 ) ;
+    temp = tempax & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x04 , temp ) ;
+
+    temp = ( tempbx & 0xFF00 ) >> 8 ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToTV )
+    {
+        if ( !( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) ) )
+            temp += 2 ;
+
+        if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+        {
+            if ( pVBInfo->VBType & VB_XGI301LV )
+            {
+                if ( pVBInfo->VBExtInfo == VB_YPbPr1080i )
+                {
+                    if ( resinfo == 7 )
+                        temp -= 2 ;
+                }
+            }
+            else
+                if ( resinfo == 7 )
+                    temp -= 2 ;
+        }
+    }
+
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x05 , temp ) ;                        /* 0x05 Horizontal Display Start */
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x06 , 0x03 ) ;                        /* 0x06 Horizontal Blank end */
+
+    if ( !( pVBInfo->VBInfo & DisableCRT2Display ) )
+    {                                                                   /* 030226 bainy */
+        if ( pVBInfo->VBInfo & SetCRT2ToTV )
+            tempax = pVBInfo->VGAHT ;
+        else
+            tempax = XGI_GetVGAHT2( pVBInfo) ;
+    }
+
+    if ( tempax >= pVBInfo->VGAHT )
+    {
+        tempax = pVBInfo->VGAHT ;
+    }
+
+    if ( modeflag & HalfDCLK )
+    {
+        tempax = tempax >> 1 ;
+    }
+
+    tempax = ( tempax / tempcx ) - 5 ;
+    tempcx = tempax ;                           /* 20030401 0x07 horizontal Retrace Start */
+    if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+    {
+        temp = ( tempbx & 0x00FF ) - 1 ;
+        if ( !( modeflag & HalfDCLK ) )
+        {
+            temp -= 6 ;
+            if ( pVBInfo->TVInfo & TVSimuMode )
+            {
+                temp -= 4 ;
+                if ( ModeNo > 0x13 )
+                    temp -= 10 ;
+            }
+        }
+    }
+    else
+    {
+        /* tempcx = tempbx & 0x00FF ; */
+        tempbx = ( tempbx & 0xFF00 ) >> 8 ;
+        tempcx = ( tempcx + tempbx ) >> 1 ;
+        temp = ( tempcx & 0x00FF ) + 2 ;
+
+        if ( pVBInfo->VBInfo & SetCRT2ToTV )
+        {
+            temp -= 1 ;
+            if ( !( modeflag & HalfDCLK ) )
+            {
+                if ( ( modeflag & Charx8Dot ) )
+                {
+                    temp += 4 ;
+                    if ( pVBInfo->VGAHDE >= 800 )
+                    {
+                        temp -= 6 ;
+                    }
+                }
+            }
+        }
+        else
+        {
+            if ( !( modeflag & HalfDCLK ) )
+            {
+                temp -= 4 ;
+                if ( pVBInfo->LCDResInfo != Panel1280x960 )
+                {
+                    if( pVBInfo->VGAHDE >= 800 )
+                    {
+                        temp -= 7 ;
+                        if ( pVBInfo->ModeType == ModeEGA )
+                        {
+                            if ( pVBInfo->VGAVDE == 1024 )
+                            {
+                                temp += 15 ;
+                                if ( pVBInfo->LCDResInfo != Panel1280x1024 )
+                                {
+                                    temp += 7 ;
+                                }
+                            }
+                        }
+
+                        if ( pVBInfo->VGAHDE >= 1280 )
+                        {
+                            if ( pVBInfo->LCDResInfo != Panel1280x960 )
+                            {
+                                if ( pVBInfo->LCDInfo & LCDNonExpanding )
+                                {
+                                    temp += 28 ;
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x07 , temp ) ;                /* 0x07 Horizontal Retrace Start */
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x08 , 0 ) ;           /* 0x08 Horizontal Retrace End */
+
+    if ( pVBInfo->VBInfo & SetCRT2ToTV )
+    {
+        if ( pVBInfo->TVInfo & TVSimuMode )
+        {
+            if ( ( ModeNo == 0x06 ) || ( ModeNo == 0x10 ) || ( ModeNo == 0x11 ) || ( ModeNo == 0x13 ) || ( ModeNo == 0x0F ) )
+            {
+                XGINew_SetReg1( pVBInfo->Part1Port , 0x07 , 0x5b ) ;
+                XGINew_SetReg1( pVBInfo->Part1Port , 0x08 , 0x03 ) ;
+            }
+
+            if ( ( ModeNo == 0x00 ) || ( ModeNo == 0x01 ) )
+            {
+                if ( pVBInfo->TVInfo & SetNTSCTV )
+                {
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x07 , 0x2A ) ;
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x08 , 0x61 ) ;
+                }
+                else
+                {
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x07 , 0x2A ) ;
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x08 , 0x41 ) ;
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x0C , 0xF0 ) ;
+                }
+            }
+
+            if ( ( ModeNo == 0x02 ) || ( ModeNo == 0x03 ) || ( ModeNo == 0x07 ) )
+            {
+                if ( pVBInfo->TVInfo & SetNTSCTV )
+                {
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x07 , 0x54 ) ;
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x08 , 0x00 ) ;
+                }
+                else
+                {
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x07 , 0x55 ) ;
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x08 , 0x00 ) ;
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x0C , 0xF0 ) ;
+                }
+            }
+
+            if ( ( ModeNo == 0x04 ) || ( ModeNo == 0x05 ) || ( ModeNo == 0x0D ) || ( ModeNo == 0x50 ) )
+            {
+                if ( pVBInfo->TVInfo & SetNTSCTV )
+                {
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x07 , 0x30 ) ;
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x08 , 0x03 ) ;
+                }
+                else
+                {
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x07 , 0x2f ) ;
+                    XGINew_SetReg1( pVBInfo->Part1Port , 0x08 , 0x02 ) ;
+                }
+            }
+        }
+    }
+
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x18 , 0x03 ) ;                                /* 0x18 SR0B */
+    XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x19 , 0xF0 , 0x00 ) ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x09 , 0xFF ) ;                                /* 0x09 Set Max VT */
+
+    tempbx = pVBInfo->VGAVT ;
+    push1 = tempbx ;
+    tempcx = 0x121 ;
+    tempbx = pVBInfo->VGAVDE ;                                                  /* 0x0E Virtical Display End */
+
+    if ( tempbx == 357 )
+        tempbx = 350 ;
+    if ( tempbx == 360 )
+        tempbx =350 ;
+    if ( tempbx == 375 )
+        tempbx = 350 ;
+    if ( tempbx == 405 )
+        tempbx = 400 ;
+    if ( tempbx == 525 )
+        tempbx = 480 ;
+
+    push2 = tempbx ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToLCD )
+    {
+        if ( pVBInfo->LCDResInfo == Panel1024x768 )
+        {
+            if ( !( pVBInfo->LCDInfo & LCDVESATiming ) )
+            {
+                if ( tempbx == 350 )
+                    tempbx += 5 ;
+                if ( tempbx == 480 )
+                    tempbx += 5 ;
+            }
+        }
+    }
+    tempbx-- ;
+    temp = tempbx & 0x00FF ;
+    tempbx-- ;
+    temp = tempbx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x10 ,temp ) ;                         /* 0x10 vertical Blank Start */
+    tempbx = push2 ;
+    tempbx-- ;
+    temp = tempbx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x0E , temp ) ;
+
+    if ( tempbx & 0x0100 )
+    {
+        tempcx |= 0x0002 ;
+    }
+
+    tempax = 0x000B ;
+
+    if ( modeflag & DoubleScanMode )
+    {
+        tempax |= 0x08000 ;
+    }
+
+    if ( tempbx & 0x0200 )
+    {
+        tempcx |= 0x0040 ;
+    }
+
+    temp = ( tempax & 0xFF00 ) >> 8 ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x0B , temp ) ;
+
+    if ( tempbx & 0x0400 )
+    {
+        tempcx |= 0x0600 ;
+    }
+
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x11 , 0x00 ) ;                                /* 0x11 Vertival Blank End */
+
+    tempax = push1 ;
+    tempax -= tempbx ;                                                          /* 0x0C Vertical Retrace Start */
+    tempax = tempax >> 2 ;
+    push1 = tempax ;                                                            /* push ax */
+
+    if ( resinfo != 0x09 )
+    {
+        tempax = tempax << 1 ;
+        tempbx += tempax ;
+    }
+
+    if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+    {
+        if ( pVBInfo->VBType & VB_XGI301LV )
+        {
+            if ( pVBInfo->TVInfo & SetYPbPrMode1080i )
+                tempbx -= 10 ;
+            else
+            {
+                if ( pVBInfo->TVInfo & TVSimuMode )
+                {
+                    if ( pVBInfo->TVInfo & SetPALTV )
+                    {
+                        if ( pVBInfo->VBType & VB_XGI301LV )
+                        {
+                            if ( !( pVBInfo->TVInfo & ( SetYPbPrMode525p | SetYPbPrMode750p | SetYPbPrMode1080i ) ) )
+                                tempbx += 40 ;
+                        }
+                        else
+                            tempbx += 40 ;
+                    }
+                }
+            }
+        }
+        else
+            tempbx -= 10 ;
+    }
+    else
+    {
+        if ( pVBInfo->TVInfo & TVSimuMode )
+        {
+            if ( pVBInfo->TVInfo & SetPALTV )
+            {
+                if ( pVBInfo->VBType & VB_XGI301LV )
+                {
+                    if ( !( pVBInfo->TVInfo & ( SetYPbPrMode525p | SetYPbPrMode750p | SetYPbPrMode1080i ) ) )
+                        tempbx += 40 ;
+                }
+                else
+                    tempbx += 40 ;
+            }
+        }
+    }
+    tempax = push1 ;
+    tempax = tempax >> 2 ;
+    tempax++ ;
+    tempax += tempbx ;
+    push1 = tempax ;                                            /* push ax */
+
+    if ( ( pVBInfo->TVInfo & SetPALTV ) )
+    {
+        if ( tempbx <= 513 )
+        {
+            if ( tempax >= 513 )
+            {
+                tempbx = 513 ;
+            }
+        }
+    }
+
+    temp = tempbx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x0C , temp ) ;
+    tempbx-- ;
+    temp = tempbx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x10 , temp ) ;
+
+    if ( tempbx & 0x0100 )
+    {
+        tempcx |= 0x0008 ;
+    }
+
+    if ( tempbx & 0x0200 )
+    {
+        XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x0B , 0x0FF , 0x20 ) ;
+    }
+
+    tempbx++ ;
+
+    if ( tempbx & 0x0100 )
+    {
+        tempcx |= 0x0004 ;
+    }
+
+    if ( tempbx & 0x0200 )
+    {
+        tempcx |= 0x0080 ;
+    }
+
+    if ( tempbx & 0x0400 )
+    {
+        tempcx |= 0x0C00 ;
+    }
+
+    tempbx = push1 ;                                            /* pop ax */
+    temp = tempbx & 0x00FF ;
+    temp &= 0x0F ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x0D , temp ) ;                /* 0x0D vertical Retrace End */
+
+    if ( tempbx & 0x0010 )
+    {
+        tempcx |= 0x2000 ;
+    }
+
+    temp = tempcx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x0A , temp ) ;                /* 0x0A CR07 */
+    temp = ( tempcx & 0x0FF00 ) >> 8 ;
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x17 , temp ) ;                /* 0x17 SR0A */
+    tempax = modeflag ;
+    temp = ( tempax & 0xFF00 ) >> 8 ;
+
+    temp = ( temp >> 1 ) & 0x09 ;
+
+    if ( pVBInfo->VBType & ( VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+        temp |= 0x01 ;
+
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x16 , temp ) ;                        /* 0x16 SR01 */
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x0F , 0 ) ;                   /* 0x0F CR14 */
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x12 , 0 ) ;                   /* 0x12 CR17 */
+
+    if ( pVBInfo->LCDInfo & LCDRGB18Bit )
+        temp = 0x80 ;
+    else
+        temp = 0x00 ;
+
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x1A , temp ) ;                        /* 0x1A SR0E */
+
+    return ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetGroup2 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetGroup2( USHORT ModeNo, USHORT ModeIdIndex, USHORT RefreshRateTableIndex,
+                    PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT i ,
+           j ,
+           tempax ,
+           tempbx ,
+           tempcx ,
+           temp ,
+           push1 ,
+           push2 ,
+           modeflag ,
+           resinfo ,
+           crt2crtc ;
+    UCHAR *TimingPoint ;
+
+    ULONG longtemp ,
+          tempeax ,
+          tempebx ,
+          temp2 ,
+          tempecx ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;           /* si+St_ResInfo */
+        resinfo = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ResInfo ;
+        crt2crtc = pVBInfo->SModeIDTable[ ModeIdIndex ].St_CRT2CRTC ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;          /* si+Ext_ResInfo */
+        resinfo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ;
+        crt2crtc = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT2CRTC ;
+    }
+
+    tempax = 0 ;
+
+    if ( !( pVBInfo->VBInfo & SetCRT2ToAVIDEO ) )
+        tempax |= 0x0800 ;
+
+    if ( !( pVBInfo->VBInfo & SetCRT2ToSVIDEO ) )
+        tempax |= 0x0400 ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToSCART )
+        tempax |= 0x0200 ;
+
+    if ( !( pVBInfo->TVInfo & SetPALTV ) )
+        tempax |= 0x1000 ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+        tempax |= 0x0100 ;
+
+    if ( pVBInfo->TVInfo & ( SetYPbPrMode525p | SetYPbPrMode750p ) )
+        tempax &= 0xfe00 ;
+
+    tempax = ( tempax & 0xff00 ) >> 8 ;
+
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x0 , tempax ) ;
+    TimingPoint = pVBInfo->NTSCTiming ;
+
+    if ( pVBInfo->TVInfo & SetPALTV )
+    {
+        TimingPoint = pVBInfo->PALTiming ;
+    }
+
+    if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+    {
+        TimingPoint = pVBInfo->HiTVExtTiming ;
+
+        if ( pVBInfo->VBInfo & SetInSlaveMode )
+            TimingPoint = pVBInfo->HiTVSt2Timing ;
+
+        if ( pVBInfo->SetFlag & TVSimuMode )
+            TimingPoint = pVBInfo->HiTVSt1Timing ;
+
+        if ( !(modeflag & Charx8Dot) )
+            TimingPoint = pVBInfo->HiTVTextTiming ;
+    }
+
+    if ( pVBInfo->VBInfo & SetCRT2ToYPbPr )
+    {
+        if ( pVBInfo->TVInfo & SetYPbPrMode525i )
+            TimingPoint = pVBInfo->YPbPr525iTiming ;
+
+        if ( pVBInfo->TVInfo & SetYPbPrMode525p )
+            TimingPoint = pVBInfo->YPbPr525pTiming ;
+
+        if ( pVBInfo->TVInfo & SetYPbPrMode750p )
+            TimingPoint = pVBInfo->YPbPr750pTiming ;
+    }
+
+    for( i = 0x01 , j = 0 ; i <= 0x2D ; i++ , j++ )
+    {
+        XGINew_SetReg1( pVBInfo->Part2Port , i , TimingPoint[ j ] ) ;
+    }
+
+    for( i = 0x39 ; i <= 0x45 ; i++ , j++ )
+    {
+        XGINew_SetReg1( pVBInfo->Part2Port , i , TimingPoint[ j ] ) ;   /* di->temp2[j] */
+    }
+
+    if ( pVBInfo->VBInfo & SetCRT2ToTV )
+    {
+        XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x3A , 0x1F , 0x00 ) ;
+    }
+
+    temp = pVBInfo->NewFlickerMode ;
+    temp &= 0x80 ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x0A , 0xFF , temp ) ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+        tempax = 950 ;
+
+    if ( pVBInfo->TVInfo & SetPALTV )
+        tempax = 520 ;
+    else
+        tempax = 440 ;
+
+    if ( pVBInfo->VDE <= tempax )
+    {
+        tempax -= pVBInfo->VDE ;
+        tempax = tempax >> 2 ;
+        tempax = ( tempax & 0x00FF ) | ( ( tempax & 0x00FF ) << 8 ) ;
+        push1 = tempax ;
+        temp = ( tempax & 0xFF00 ) >> 8 ;
+        temp += ( USHORT )TimingPoint[ 0 ] ;
+
+        if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+        {
+            if ( pVBInfo->VBInfo & ( SetCRT2ToAVIDEO | SetCRT2ToSVIDEO | SetCRT2ToSCART | SetCRT2ToYPbPr ) )
+            {
+                tempcx=pVBInfo->VGAHDE;
+                if ( tempcx >= 1024 )
+                {
+                    temp = 0x17 ;               /* NTSC */
+                    if ( pVBInfo->TVInfo & SetPALTV )
+                        temp = 0x19 ;           /* PAL */
+                }
+            }
+        }
+
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x01 , temp ) ;
+        tempax = push1 ;
+        temp = ( tempax & 0xFF00 ) >> 8 ;
+        temp += TimingPoint[ 1 ] ;
+
+        if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+        {
+            if ( ( pVBInfo->VBInfo & ( SetCRT2ToAVIDEO | SetCRT2ToSVIDEO | SetCRT2ToSCART | SetCRT2ToYPbPr ) ) )
+            {
+                tempcx = pVBInfo->VGAHDE ;
+                if ( tempcx >= 1024 )
+                {
+                    temp = 0x1D ;               /* NTSC */
+                    if ( pVBInfo->TVInfo & SetPALTV )
+                        temp = 0x52 ;           /* PAL */
+                }
+            }
+        }
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x02 , temp ) ;
+    }
+
+    /* 301b */
+    tempcx = pVBInfo->HT ;
+
+    if ( XGI_IsLCDDualLink( pVBInfo ) )
+        tempcx = tempcx >> 1 ;
+
+    tempcx -= 2 ;
+    temp = tempcx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x1B , temp ) ;
+
+    temp = ( tempcx & 0xFF00 ) >> 8 ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x1D , ~0x0F , temp ) ;
+
+    tempcx = pVBInfo->HT >> 1 ;
+    push1 = tempcx ;                            /* push cx */
+    tempcx += 7 ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+    {
+        tempcx -= 4 ;
+    }
+
+    temp = tempcx & 0x00FF ;
+    temp = temp << 4 ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x22 , 0x0F , temp ) ;
+
+    tempbx = TimingPoint[ j ] | ( ( TimingPoint[ j + 1 ] ) << 8 ) ;
+    tempbx += tempcx ;
+    push2 = tempbx ;
+    temp = tempbx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x24 , temp ) ;
+    temp = ( tempbx & 0xFF00 ) >> 8 ;
+    temp = temp << 4 ;
+    XGINew_SetRegANDOR(pVBInfo->Part2Port,0x25,0x0F,temp);
+
+    tempbx=push2;
+    tempbx=tempbx+8;
+    if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+    {
+        tempbx=tempbx-4;
+        tempcx=tempbx;
+    }
+
+    temp = ( tempbx & 0x00FF ) << 4 ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x29 , 0x0F , temp ) ;
+
+    j += 2 ;
+    tempcx += ( TimingPoint[ j ] | ( ( TimingPoint[ j + 1 ] ) << 8 ) ) ;
+    temp = tempcx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x27 , temp ) ;
+    temp = ( ( tempcx & 0xFF00 ) >> 8 ) << 4 ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x28 , 0x0F , temp ) ;
+
+    tempcx += 8 ;
+    if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+    {
+        tempcx -= 4 ;
+    }
+
+    temp = tempcx & 0xFF ;
+    temp = temp << 4 ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x2A , 0x0F , temp ) ;
+
+    tempcx = push1 ;                                    /* pop cx */
+    j += 2 ;
+    temp = TimingPoint[ j ] | ( ( TimingPoint[ j + 1 ] ) << 8 ) ;
+    tempcx -= temp ;
+    temp = tempcx & 0x00FF ;
+    temp = temp << 4 ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x2D , 0x0F ,temp ) ;
+
+    tempcx -= 11 ;
+
+    if ( !( pVBInfo->VBInfo & SetCRT2ToTV ) )
+    {
+        tempax = XGI_GetVGAHT2( pVBInfo) ;
+        tempcx = tempax - 1 ;
+    }
+    temp = tempcx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x2E , temp ) ;
+
+    tempbx = pVBInfo->VDE ;
+
+    if ( pVBInfo->VGAVDE == 360 )
+        tempbx = 746 ;
+    if ( pVBInfo->VGAVDE == 375 )
+        tempbx = 746 ;
+    if ( pVBInfo->VGAVDE == 405 )
+        tempbx = 853 ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToTV )
+    {
+        if ( pVBInfo->VBType & ( VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+        {
+            if ( !( pVBInfo->TVInfo & ( SetYPbPrMode525p | SetYPbPrMode750p ) ) )
+                tempbx = tempbx >> 1 ;
+        }
+        else
+            tempbx = tempbx >> 1 ;
+    }
+
+    tempbx -= 2 ;
+    temp = tempbx & 0x00FF ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+    {
+        if ( pVBInfo->VBType & VB_XGI301LV )
+        {
+            if ( pVBInfo->TVInfo & SetYPbPrMode1080i )
+            {
+                if ( pVBInfo->VBInfo & SetInSlaveMode )
+                {
+                    if ( ModeNo == 0x2f )
+                        temp += 1 ;
+                }
+            }
+        }
+        else
+        {
+            if ( pVBInfo->VBInfo & SetInSlaveMode )
+            {
+                if ( ModeNo == 0x2f )
+                    temp += 1 ;
+            }
+        }
+    }
+
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x2F , temp ) ;
+
+    temp = ( tempcx & 0xFF00 ) >> 8 ;
+    temp |= ( ( tempbx & 0xFF00 ) >> 8 ) << 6 ;
+
+    if ( !( pVBInfo->VBInfo & SetCRT2ToHiVisionTV ) )
+    {
+        if ( pVBInfo->VBType & VB_XGI301LV )
+        {
+            if ( pVBInfo->TVInfo & SetYPbPrMode1080i )
+            {
+                temp |= 0x10 ;
+
+                if ( !( pVBInfo->VBInfo & SetCRT2ToSVIDEO ) )
+                    temp |= 0x20 ;
+            }
+        }
+        else
+        {
+            temp |= 0x10 ;
+            if ( !( pVBInfo->VBInfo & SetCRT2ToSVIDEO ) )
+                temp |= 0x20 ;
+        }
+    }
+
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x30 , temp ) ;
+
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )       /* TV gatingno */
+    {
+        tempbx = pVBInfo->VDE ;
+        tempcx = tempbx - 2 ;
+
+        if ( pVBInfo->VBInfo & SetCRT2ToTV )
+        {
+            if ( !( pVBInfo->TVInfo & ( SetYPbPrMode525p | SetYPbPrMode750p ) ) )
+                tempbx = tempbx >> 1 ;
+        }
+
+        if ( pVBInfo->VBType & ( VB_XGI302LV | VB_XGI301C ) )
+        {
+            temp=0;
+            if( tempcx & 0x0400 )
+                temp |= 0x20 ;
+
+            if ( tempbx & 0x0400 )
+                temp |= 0x40 ;
+
+            XGINew_SetReg1( pVBInfo->Part4Port , 0x10 , temp ) ;
+        }
+
+        temp = ( ( ( tempbx - 3 ) & 0x0300 ) >> 8 ) << 5 ;
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x46 , temp ) ;
+        temp = ( tempbx - 3 ) & 0x00FF ;
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x47 , temp ) ;
+    }
+
+    tempbx = tempbx & 0x00FF ;
+
+    if ( !( modeflag & HalfDCLK ) )
+    {
+        tempcx = pVBInfo->VGAHDE ;
+        if ( tempcx >= pVBInfo->HDE )
+        {
+            tempbx |= 0x2000 ;
+            tempax &= 0x00FF ;
+        }
+    }
+
+    tempcx = 0x0101 ;
+
+    if( pVBInfo->VBInfo & SetCRT2ToTV ) { /*301b*/
+        if(pVBInfo->VGAHDE>=1024)
+        {
+            tempcx=0x1920;
+            if(pVBInfo->VGAHDE>=1280)
+            {
+                tempcx=0x1420;
+                tempbx=tempbx&0xDFFF;
+            }
+        }
+    }
+
+    if ( !( tempbx & 0x2000 ) )
+    {
+        if ( modeflag & HalfDCLK )
+        {
+            tempcx = ( tempcx & 0xFF00 ) | ( ( tempcx & 0x00FF ) << 1 ) ;
+        }
+
+        push1 = tempbx ;
+        tempeax = pVBInfo->VGAHDE ;
+        tempebx = ( tempcx & 0xFF00 ) >> 8 ;
+        longtemp = tempeax * tempebx ;
+        tempecx = tempcx & 0x00FF ;
+        longtemp = longtemp / tempecx ;
+
+        /* 301b */
+        tempecx = 8 * 1024 ;
+
+        if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+        {
+            tempecx = tempecx * 8 ;
+        }
+
+        longtemp = longtemp * tempecx ;
+        tempecx = pVBInfo->HDE ;
+        temp2 = longtemp % tempecx ;
+        tempeax = longtemp / tempecx ;
+        if ( temp2 != 0 )
+        {
+            tempeax += 1 ;
+        }
+
+        tempax = ( USHORT )tempeax ;
+
+        /* 301b */
+        if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+        {
+            tempcx = ( ( tempax & 0xFF00 ) >> 5 ) >> 8 ;
+        }
+        /* end 301b */
+
+        tempbx = push1 ;
+        tempbx =( USHORT )( ( ( tempeax & 0x0000FF00 ) & 0x1F00 ) | ( tempbx & 0x00FF ) ) ;
+        tempax =( USHORT )( ( ( tempeax & 0x000000FF ) << 8 ) | ( tempax & 0x00FF ) ) ;
+        temp = ( tempax & 0xFF00 ) >> 8 ;
+    }
+    else
+    {
+        temp = ( tempax & 0x00FF ) >> 8 ;
+    }
+
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x44 , temp ) ;
+    temp = ( tempbx & 0xFF00 ) >> 8 ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x45 , ~0x03F , temp ) ;
+    temp = tempcx & 0x00FF ;
+
+    if ( tempbx & 0x2000 )
+        temp = 0 ;
+
+    if ( !( pVBInfo->VBInfo & SetCRT2ToLCD ) )
+        temp |= 0x18 ;
+
+    XGINew_SetRegANDOR(pVBInfo->Part2Port,0x46,~0x1F,temp);
+    if ( pVBInfo->TVInfo & SetPALTV )
+    {
+        tempbx = 0x0382 ;
+        tempcx = 0x007e ;
+    }
+    else
+    {
+        tempbx = 0x0369 ;
+        tempcx = 0x0061 ;
+    }
+
+    temp = tempbx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x4b , temp ) ;
+    temp = tempcx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x4c , temp ) ;
+
+    temp = ( ( tempcx & 0xFF00 ) >> 8 ) & 0x03 ;
+    temp = temp << 2 ;
+    temp |= ( ( tempbx & 0xFF00 ) >> 8 ) & 0x03 ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToYPbPr )
+    {
+        temp |= 0x10 ;
+
+        if ( pVBInfo->TVInfo & SetYPbPrMode525p )
+            temp |= 0x20 ;
+
+        if ( pVBInfo->TVInfo & SetYPbPrMode750p )
+            temp |= 0x60 ;
+    }
+
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x4d , temp ) ;
+    temp=XGINew_GetReg1( pVBInfo->Part2Port , 0x43 ) ;          /* 301b change */
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x43 , ( USHORT )( temp - 3 ) ) ;
+
+    if ( !( pVBInfo->TVInfo & ( SetYPbPrMode525p | SetYPbPrMode750p ) ) )
+    {
+        if ( pVBInfo->TVInfo & NTSC1024x768 )
+        {
+            TimingPoint = XGI_NTSC1024AdjTime ;
+            for( i = 0x1c , j = 0 ; i <= 0x30 ; i++ , j++ )
+            {
+                XGINew_SetReg1( pVBInfo->Part2Port , i , TimingPoint[ j ] ) ;
+            }
+            XGINew_SetReg1( pVBInfo->Part2Port , 0x43 , 0x72 ) ;
+        }
+    }
+
+    /* [ycchen] 01/14/03 Modify for 301C PALM Support */
+    if ( pVBInfo->VBType & VB_XGI301C )
+    {
+        if ( pVBInfo->TVInfo & SetPALMTV )
+            XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x4E , ~0x08 , 0x08 ) ;    /* PALM Mode */
+    }
+
+    if ( pVBInfo->TVInfo & SetPALMTV )
+    {
+        tempax = ( UCHAR )XGINew_GetReg1( pVBInfo->Part2Port , 0x01 ) ;
+        tempax-- ;
+        XGINew_SetRegAND( pVBInfo->Part2Port , 0x01 , tempax ) ;
+
+        /* if ( !( pVBInfo->VBType & VB_XGI301C ) ) */
+        XGINew_SetRegAND( pVBInfo->Part2Port , 0x00 , 0xEF ) ;
+    }
+
+    if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+    {
+        if ( !( pVBInfo->VBInfo & SetInSlaveMode ) )
+        {
+            XGINew_SetReg1( pVBInfo->Part2Port , 0x0B , 0x00 ) ;
+        }
+    }
+
+    if ( pVBInfo->VBInfo & SetCRT2ToTV )
+    {
+        return ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetLCDRegs */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void  XGI_SetLCDRegs(USHORT ModeNo,USHORT ModeIdIndex, PXGI_HW_DEVICE_INFO HwDeviceExtension,USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT push1 ,
+           push2 ,
+           pushbx ,
+           tempax ,
+           tempbx ,
+           tempcx ,
+           temp ,
+           tempah ,
+           tempbh ,
+           tempch ,
+           resinfo ,
+           modeflag ,
+           CRT1Index ;
+
+    XGI_LCDDesStruct *LCDBDesPtr = NULL ;
+
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;   /* si+St_ResInfo */
+        resinfo = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ResInfo ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;  /* si+Ext_ResInfo */
+        resinfo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ;
+        CRT1Index = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT1CRTC ;
+        CRT1Index &= IndexMask ;
+    }
+
+    if ( !( pVBInfo->VBInfo & SetCRT2ToLCD ) )
+    {
+        return ;
+    }
+
+    tempbx = pVBInfo->HDE ;                     /* RHACTE=HDE-1 */
+
+    if ( XGI_IsLCDDualLink( pVBInfo ) )
+        tempbx = tempbx >> 1 ;
+
+    tempbx -= 1 ;
+    temp = tempbx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x2C , temp ) ;
+    temp = ( tempbx & 0xFF00 ) >> 8 ;
+    temp = temp << 4 ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x2B , 0x0F , temp ) ;
+    temp = 0x01 ;
+
+    if ( pVBInfo->LCDResInfo == Panel1280x1024 )
+    {
+        if ( pVBInfo->ModeType == ModeEGA )
+        {
+            if ( pVBInfo->VGAHDE >= 1024 )
+            {
+                temp = 0x02 ;
+                if ( pVBInfo->LCDInfo & LCDVESATiming )
+                    temp = 0x01 ;
+            }
+        }
+    }
+
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x0B , temp ) ;
+    tempbx = pVBInfo->VDE ;                     /* RTVACTEO=(VDE-1)&0xFF */
+    push1 = tempbx ;
+    tempbx-- ;
+    temp = tempbx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x03 , temp ) ;
+    temp = ( ( tempbx & 0xFF00 ) >> 8 ) & 0x07 ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x0C , ~0x07 , temp ) ;
+
+    tempcx = pVBInfo->VT - 1 ;
+    push2 = tempcx + 1 ;
+    temp = tempcx & 0x00FF ;                    /* RVTVT=VT-1 */
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x19 , temp ) ;
+    temp = ( tempcx & 0xFF00 ) >> 8 ;
+    temp = temp << 5 ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x1A , temp ) ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x09 , 0xF0 , 0x00 ) ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x0A , 0xF0 , 0x00 ) ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x17 , 0xFB , 0x00 ) ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x18 , 0xDF , 0x00 ) ;
+
+    /* Customized LCDB Des no add */
+    tempbx = 5 ;
+    LCDBDesPtr = ( XGI_LCDDesStruct * )XGI_GetLcdPtr( tempbx , ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+    tempah = pVBInfo->LCDResInfo ;
+    tempah &= PanelResInfo ;
+
+    if ( ( tempah == Panel1024x768 ) || ( tempah == Panel1024x768x75 ) )
+    {
+        tempbx = 1024 ;
+        tempcx = 768 ;
+    }
+    else if ( ( tempah == Panel1280x1024 ) || ( tempah == Panel1280x1024x75 ) )
+    {
+        tempbx = 1280 ;
+        tempcx = 1024 ;
+    }
+    else if ( tempah == Panel1400x1050 )
+    {
+        tempbx = 1400 ;
+        tempcx = 1050 ;
+    }
+    else
+    {
+        tempbx = 1600 ;
+        tempcx = 1200 ;
+    }
+
+    if ( pVBInfo->LCDInfo & EnableScalingLCD )
+    {
+        tempbx = pVBInfo->HDE ;
+        tempcx = pVBInfo->VDE ;
+    }
+
+    pushbx = tempbx ;
+    tempax = pVBInfo->VT ;
+    pVBInfo->LCDHDES = LCDBDesPtr->LCDHDES ;
+    pVBInfo->LCDHRS = LCDBDesPtr->LCDHRS ;
+    pVBInfo->LCDVDES = LCDBDesPtr->LCDVDES ;
+    pVBInfo->LCDVRS = LCDBDesPtr->LCDVRS ;
+    tempbx = pVBInfo->LCDVDES ;
+    tempcx += tempbx ;
+
+    if ( tempcx >= tempax )
+        tempcx -= tempax ;      /* lcdvdes */
+
+    temp = tempbx & 0x00FF ;    /* RVEQ1EQ=lcdvdes */
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x05 , temp ) ;
+    temp = tempcx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x06 , temp ) ;
+    tempch = ( ( tempcx & 0xFF00 ) >> 8 ) & 0x07 ;
+    tempbh = ( ( tempbx & 0xFF00 ) >> 8 ) & 0x07 ;
+    tempah = tempch ;
+    tempah = tempah << 3 ;
+    tempah |= tempbh ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x02 , tempah ) ;
+
+    /* getlcdsync() */
+    XGI_GetLCDSync( &tempax , &tempbx,pVBInfo ) ;
+    tempcx = tempbx ;
+    tempax = pVBInfo->VT ;
+    tempbx = pVBInfo->LCDVRS ;
+
+    /* if ( SetLCD_Info & EnableScalingLCD ) */
+    tempcx += tempbx ;
+    if ( tempcx >= tempax )
+        tempcx -= tempax ;
+
+    temp = tempbx & 0x00FF ;    /* RTVACTEE=lcdvrs */
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x04 , temp ) ;
+    temp = ( tempbx & 0xFF00 ) >> 8 ;
+    temp = temp << 4 ;
+    temp |= ( tempcx & 0x000F ) ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x01 , temp ) ;
+    tempcx = pushbx ;
+    tempax = pVBInfo->HT ;
+    tempbx = pVBInfo->LCDHDES ;
+    tempbx &= 0x0FFF ;
+
+    if ( XGI_IsLCDDualLink(  pVBInfo ) )
+    {
+        tempax = tempax >> 1 ;
+        tempbx = tempbx >> 1 ;
+        tempcx = tempcx >> 1 ;
+    }
+
+    if ( pVBInfo->VBType & VB_XGI302LV )
+        tempbx += 1 ;
+
+    if ( pVBInfo->VBType & VB_XGI301C )  /* tap4 */
+        tempbx += 1 ;
+
+    tempcx += tempbx ;
+
+    if ( tempcx >= tempax )
+        tempcx -= tempax ;
+
+    temp = tempbx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x1F , temp ) ;        /* RHBLKE=lcdhdes */
+    temp = ( ( tempbx & 0xFF00 ) >> 8 ) << 4 ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x20 , temp ) ;
+    temp = tempcx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x23 , temp ) ;        /* RHEQPLE=lcdhdee */
+    temp = ( tempcx & 0xFF00 ) >> 8 ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x25 , temp ) ;
+
+    /* getlcdsync() */
+    XGI_GetLCDSync( &tempax , &tempbx ,pVBInfo) ;
+    tempcx = tempax ;
+    tempax = pVBInfo->HT ;
+    tempbx = pVBInfo->LCDHRS ;
+    /* if ( SetLCD_Info & EnableScalingLCD) */
+    if ( XGI_IsLCDDualLink( pVBInfo) )
+    {
+        tempax = tempax >> 1 ;
+        tempbx = tempbx >> 1 ;
+        tempcx = tempcx >> 1 ;
+    }
+
+    if ( pVBInfo->VBType & VB_XGI302LV )
+        tempbx += 1 ;
+
+    tempcx += tempbx ;
+
+    if ( tempcx >= tempax )
+        tempcx -= tempax ;
+
+    temp = tempbx & 0x00FF ;    /* RHBURSTS=lcdhrs */
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x1C , temp ) ;
+
+    temp = ( tempbx & 0xFF00 ) >> 8 ;
+    temp = temp << 4 ;
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x1D , ~0x0F0 , temp ) ;
+    temp = tempcx & 0x00FF ;    /* RHSYEXP2S=lcdhre */
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x21 , temp ) ;
+
+    if ( !( pVBInfo->LCDInfo & LCDVESATiming ) )
+    {
+        if ( pVBInfo->VGAVDE == 525 )
+        {
+            if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+            {
+                temp = 0xC6 ;
+            }
+            else
+                temp = 0xC4 ;
+
+            XGINew_SetReg1( pVBInfo->Part2Port , 0x2f , temp ) ;
+            XGINew_SetReg1( pVBInfo->Part2Port , 0x30 , 0xB3 ) ;
+        }
+
+        if ( pVBInfo->VGAVDE == 420 )
+        {
+            if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+            {
+                temp = 0x4F ;
+            }
+            else
+                temp = 0x4E ;
+            XGINew_SetReg1( pVBInfo->Part2Port , 0x2f , temp ) ;
+        }
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetTap4Ptr */
+/* Input : */
+/* Output : di -> Tap4 Reg. Setting Pointer */
+/* Description : */
+/* --------------------------------------------------------------------- */
+XGI301C_Tap4TimingStruct* XGI_GetTap4Ptr(USHORT tempcx, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempax ,
+           tempbx ,
+           i ;
+
+    XGI301C_Tap4TimingStruct *Tap4TimingPtr ;
+
+    if ( tempcx == 0 )
+    {
+        tempax = pVBInfo->VGAHDE ;
+        tempbx = pVBInfo->HDE ;
+    }
+    else
+    {
+        tempax = pVBInfo->VGAVDE ;
+        tempbx = pVBInfo->VDE ;
+    }
+
+    if ( tempax < tempbx )
+        return &EnlargeTap4Timing[ 0 ] ;
+    else if( tempax == tempbx )
+        return &NoScaleTap4Timing[ 0 ] ;        /* 1:1 */
+    else
+        Tap4TimingPtr = NTSCTap4Timing ;        /* NTSC */
+
+    if ( pVBInfo->TVInfo & SetPALTV )
+        Tap4TimingPtr = PALTap4Timing ;
+
+
+    if ( pVBInfo->VBInfo & SetCRT2ToYPbPr )
+    {
+        if ( pVBInfo->TVInfo & SetYPbPrMode525i )
+            Tap4TimingPtr = YPbPr525iTap4Timing ;
+        if ( pVBInfo->TVInfo & SetYPbPrMode525p )
+            Tap4TimingPtr = YPbPr525pTap4Timing ;
+        if ( pVBInfo->TVInfo & SetYPbPrMode750p )
+            Tap4TimingPtr = YPbPr750pTap4Timing ;
+    }
+
+    if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+        Tap4TimingPtr = HiTVTap4Timing ;
+
+    i = 0 ;
+    while( Tap4TimingPtr[ i ].DE != 0xFFFF )
+    {
+        if ( Tap4TimingPtr[ i ].DE == tempax )
+            break ;
+        i++ ;
+    }
+    return &Tap4TimingPtr[ i ] ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetTap4Regs */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetTap4Regs( PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT i ,
+           j ;
+
+    XGI301C_Tap4TimingStruct *Tap4TimingPtr ;
+
+    if ( !( pVBInfo->VBType & VB_XGI301C ) )
+        return ;
+
+#ifndef Tap4
+    XGINew_SetRegAND( pVBInfo->Part2Port , 0x4E , 0xEB ) ;      /* Disable Tap4 */
+#else            /* Tap4 Setting */
+
+    Tap4TimingPtr = XGI_GetTap4Ptr( 0 , pVBInfo) ;  /* Set Horizontal Scaling */
+    for( i = 0x80 , j = 0 ; i <= 0xBF ; i++ , j++ )
+        XGINew_SetReg1( pVBInfo->Part2Port , i , Tap4TimingPtr->Reg[ j ] ) ;
+
+    if ( ( pVBInfo->VBInfo & SetCRT2ToTV ) && ( !( pVBInfo->VBInfo & SetCRT2ToHiVisionTV ) ) )
+    {
+        Tap4TimingPtr = XGI_GetTap4Ptr( 1 , pVBInfo);   /* Set Vertical Scaling */
+        for( i = 0xC0 , j = 0 ; i < 0xFF ; i++ , j++ )
+            XGINew_SetReg1( pVBInfo->Part2Port , i , Tap4TimingPtr->Reg[ j ] ) ;
+    }
+
+    if ( ( pVBInfo->VBInfo & SetCRT2ToTV ) && ( !( pVBInfo->VBInfo & SetCRT2ToHiVisionTV ) ) )
+        XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x4E , ~0x14 , 0x04 ) ;        /* Enable V.Scaling */
+    else
+        XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x4E , ~0x14 , 0x10 ) ;        /* Enable H.Scaling */
+#endif
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetGroup3 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetGroup3(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT i;
+    UCHAR *tempdi;
+    USHORT  modeflag;
+
+    if(ModeNo<=0x13)
+    {
+        modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;      /* si+St_ResInfo */
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ModeIdIndex].Ext_ModeFlag;     /* si+Ext_ResInfo */
+    }
+
+
+    XGINew_SetReg1(pVBInfo->Part3Port,0x00,0x00);
+    if(pVBInfo->TVInfo&SetPALTV)
+    {
+        XGINew_SetReg1(pVBInfo->Part3Port,0x13,0xFA);
+        XGINew_SetReg1(pVBInfo->Part3Port,0x14,0xC8);
+    }
+    else
+    {
+        XGINew_SetReg1(pVBInfo->Part3Port,0x13,0xF5);
+        XGINew_SetReg1(pVBInfo->Part3Port,0x14,0xB7);
+    }
+
+    if(!(pVBInfo->VBInfo&SetCRT2ToTV))
+    {
+        return;
+    }
+
+    if(pVBInfo->TVInfo&SetPALMTV)
+    {
+        XGINew_SetReg1(pVBInfo->Part3Port,0x13,0xFA);
+        XGINew_SetReg1(pVBInfo->Part3Port,0x14,0xC8);
+        XGINew_SetReg1(pVBInfo->Part3Port,0x3D,0xA8);
+    }
+
+    if((pVBInfo->VBInfo&SetCRT2ToHiVisionTV)|| (pVBInfo->VBInfo&SetCRT2ToYPbPr))
+    {
+        if(pVBInfo->TVInfo & SetYPbPrMode525i)
+        {
+            return;
+        }
+        tempdi=pVBInfo->HiTVGroup3Data;
+        if(pVBInfo->SetFlag&TVSimuMode)
+        {
+            tempdi=pVBInfo->HiTVGroup3Simu;
+            if(!(modeflag&Charx8Dot))
+            {
+                tempdi=pVBInfo->HiTVGroup3Text;
+            }
+        }
+
+        if(pVBInfo->TVInfo & SetYPbPrMode525p)
+        {
+            tempdi=pVBInfo->Ren525pGroup3;
+        }
+        if(pVBInfo->TVInfo & SetYPbPrMode750p)
+        {
+             tempdi=pVBInfo->Ren750pGroup3;
+        }
+
+        for(i=0;i<=0x3E;i++)
+        {
+            XGINew_SetReg1(pVBInfo->Part3Port,i,tempdi[i]);
+        }
+        if(pVBInfo->VBType&VB_XGI301C)  /* Marcovision */
+        {
+            if(pVBInfo->TVInfo & SetYPbPrMode525p)
+            {
+                XGINew_SetReg1(pVBInfo->Part3Port,0x28,0x3f);
+            }
+        }
+    }
+    return;
+}  /* {end of XGI_SetGroup3} */
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetGroup4 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetGroup4(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempax ,
+           tempcx ,
+           tempbx ,
+           modeflag ,
+           temp ,
+           temp2 ;
+
+    ULONG tempebx ,
+          tempeax ,
+          templong ;
+
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;   /* si+St_ResInfo */
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;  /* si+Ext_ResInfo */
+    }
+
+    temp = pVBInfo->RVBHCFACT ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x13 , temp ) ;
+
+    tempbx = pVBInfo->RVBHCMAX ;
+    temp = tempbx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x14 , temp ) ;
+    temp2 = ( ( tempbx & 0xFF00 ) >> 8 ) << 7 ;
+    tempcx = pVBInfo->VGAHT - 1 ;
+    temp = tempcx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x16 , temp ) ;
+
+    temp =( ( tempcx & 0xFF00 ) >> 8 ) << 3 ;
+    temp2 |= temp ;
+
+    tempcx = pVBInfo->VGAVT - 1 ;
+    if ( !( pVBInfo->VBInfo & SetCRT2ToTV ) )
+    {
+        tempcx -= 5 ;
+    }
+
+    temp = tempcx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x17 , temp ) ;
+    temp = temp2 | ( ( tempcx & 0xFF00 ) >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x15 , temp ) ;
+    XGINew_SetRegOR( pVBInfo->Part4Port , 0x0D , 0x08 ) ;
+    tempcx = pVBInfo->VBInfo ;
+    tempbx = pVBInfo->VGAHDE ;
+
+    if ( modeflag & HalfDCLK )
+    {
+        tempbx = tempbx >> 1 ;
+    }
+
+    if ( XGI_IsLCDDualLink( pVBInfo ) )
+        tempbx = tempbx >> 1 ;
+
+    if(tempcx&SetCRT2ToHiVisionTV)
+    {
+        temp=0;
+        if(tempbx<=1024)
+            temp=0xA0;
+       if(tempbx == 1280)
+          temp = 0xC0;
+    }
+    else if(tempcx&SetCRT2ToTV)
+    {
+         temp=0xA0;
+         if(tempbx <= 800)
+             temp=0x80;
+    }
+    else
+    {
+         temp=0x80;
+         if(pVBInfo->VBInfo&SetCRT2ToLCD)
+         {
+             temp=0;
+             if(tempbx>800)
+                 temp=0x60;
+         }
+    }
+
+    if ( pVBInfo->TVInfo & ( SetYPbPrMode525p | SetYPbPrMode750p ) )
+    {
+        temp = 0x00 ;
+        if ( pVBInfo->VGAHDE == 1280 )
+            temp = 0x40 ;
+        if ( pVBInfo->VGAHDE == 1024 )
+            temp = 0x20 ;
+    }
+    XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x0E , ~0xEF , temp ) ;
+
+    tempebx = pVBInfo->VDE ;
+
+    if ( tempcx & SetCRT2ToHiVisionTV )
+    {
+        if ( !( temp & 0xE000 ) )
+            tempbx = tempbx >> 1 ;
+    }
+
+    tempcx = pVBInfo->RVBHRS ;
+    temp = tempcx & 0x00FF ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x18 , temp );
+
+    tempeax = pVBInfo->VGAVDE ;
+    tempcx |= 0x04000 ;
+
+
+    if ( tempeax <= tempebx )
+    {
+        tempcx=(tempcx&(~0x4000));
+        tempeax = pVBInfo->VGAVDE ;
+    }
+    else
+    {
+        tempeax -= tempebx ;
+    }
+
+
+    templong = ( tempeax * 256 * 1024 ) % tempebx ;
+    tempeax = ( tempeax * 256 * 1024 ) / tempebx ;
+    tempebx = tempeax ;
+
+    if ( templong != 0 )
+    {
+        tempebx++ ;
+    }
+
+
+    temp = ( USHORT )( tempebx & 0x000000FF ) ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x1B , temp ) ;
+
+    temp = ( USHORT )( ( tempebx & 0x0000FF00 ) >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x1A , temp ) ;
+    tempbx = ( USHORT )( tempebx >> 16 ) ;
+    temp = tempbx & 0x00FF ;
+    temp = temp << 4 ;
+    temp |= ( ( tempcx & 0xFF00 ) >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x19 , temp ) ;
+
+    /* 301b */
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        temp = 0x0028 ;
+        XGINew_SetReg1( pVBInfo->Part4Port , 0x1C , temp ) ;
+        tempax = pVBInfo->VGAHDE ;
+        if ( modeflag & HalfDCLK )
+        {
+            tempax = tempax >> 1 ;
+        }
+
+        if ( XGI_IsLCDDualLink( pVBInfo ) )
+            tempax = tempax >> 1 ;
+
+        /* if((pVBInfo->VBInfo&(SetCRT2ToLCD))||((pVBInfo->TVInfo&SetYPbPrMode525p)||(pVBInfo->TVInfo&SetYPbPrMode750p))) { */
+        if ( pVBInfo->VBInfo & SetCRT2ToLCD )
+        {
+            if ( tempax > 800 )
+                tempax -= 800 ;
+        }
+        else
+        {
+            if ( pVBInfo->VGAHDE > 800 )
+            {
+                if ( pVBInfo->VGAHDE == 1024 )
+                    tempax = ( tempax * 25 / 32 ) - 1 ;
+                else
+                    tempax = ( tempax * 20 / 32 ) - 1 ;
+            }
+        }
+        tempax -= 1 ;
+
+/*
+        if ( pVBInfo->VBInfo & ( SetCRT2ToTV | SetCRT2ToHiVisionTV ) )
+        {
+            if ( pVBInfo->VBType & VB_XGI301LV )
+            {
+                if ( !( pVBInfo->TVInfo & ( SetYPbPrMode525p | SetYPbPrMode750p | SetYPbPrMode1080i ) ) )
+                {
+                    if ( pVBInfo->VGAHDE > 800 )
+                    {
+                        if ( pVBInfo->VGAHDE == 1024 )
+                            tempax = ( tempax * 25 / 32 ) - 1 ;
+                        else
+                            tempax = ( tempax * 20 / 32 ) - 1 ;
+                    }
+                }
+            }
+            else
+            {
+                if ( pVBInfo->VGAHDE > 800 )
+                {
+                    if ( pVBInfo->VGAHDE == 1024 )
+                        tempax = ( tempax * 25 / 32 ) - 1 ;
+                    else
+                        tempax = ( tempax * 20 / 32 ) - 1 ;
+                }
+            }
+        }
+*/
+
+        temp = ( tempax & 0xFF00 ) >> 8 ;
+        temp = ( ( temp & 0x0003 ) << 4 ) ;
+        XGINew_SetReg1( pVBInfo->Part4Port , 0x1E , temp ) ;
+        temp = ( tempax & 0x00FF ) ;
+        XGINew_SetReg1( pVBInfo->Part4Port , 0x1D , temp ) ;
+
+        if ( pVBInfo->VBInfo & ( SetCRT2ToTV | SetCRT2ToHiVisionTV ) )
+        {
+            if ( pVBInfo->VGAHDE > 800 )
+            {
+                XGINew_SetRegOR( pVBInfo->Part4Port , 0x1E , 0x08 ) ;
+            }
+        }
+        temp = 0x0036 ;
+
+        if ( pVBInfo->VBInfo & SetCRT2ToTV )
+        {
+            if ( !( pVBInfo->TVInfo & ( NTSC1024x768 | SetYPbPrMode525p | SetYPbPrMode750p | SetYPbPrMode1080i ) ) )
+            {
+                temp |= 0x0001 ;
+                if ( ( pVBInfo->VBInfo & SetInSlaveMode ) && ( !( pVBInfo->TVInfo & TVSimuMode ) ) )
+                    temp &= ( ~0x0001 ) ;
+            }
+        }
+
+        XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x1F , 0x00C0 , temp ) ;
+        tempbx = pVBInfo->HT ;
+        if ( XGI_IsLCDDualLink( pVBInfo ) )
+            tempbx = tempbx >> 1 ;
+        tempbx = ( tempbx >> 1 ) - 2 ;
+        temp = ( ( tempbx & 0x0700 ) >> 8 ) << 3 ;
+        XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x21 , 0x00C0 , temp ) ;
+        temp = tempbx & 0x00FF ;
+        XGINew_SetReg1( pVBInfo->Part4Port , 0x22 , temp ) ;
+    }
+    /* end 301b */
+
+    if ( pVBInfo->ISXPDOS == 0 )
+        XGI_SetCRT2VCLK( ModeNo , ModeIdIndex , RefreshRateTableIndex, pVBInfo ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetGroup5 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetGroup5( USHORT ModeNo , USHORT ModeIdIndex , PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT Pindex ,
+           Pdata ;
+
+    Pindex = pVBInfo->Part5Port ;
+    Pdata = pVBInfo->Part5Port + 1 ;
+    if ( pVBInfo->ModeType == ModeVGA )
+    {
+        if ( !( pVBInfo->VBInfo & ( SetInSlaveMode | LoadDACFlag | CRT2DisplayFlag ) ) )
+        {
+            XGINew_EnableCRT2(pVBInfo) ;
+            /* LoadDAC2(pVBInfo->Part5Port,ModeNo,ModeIdIndex); */
+        }
+    }
+    return ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetLcdPtr */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void* XGI_GetLcdPtr( USHORT BX , USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT i ,
+           tempdx ,
+           tempcx ,
+           tempbx ,
+           tempal ,
+           modeflag ,
+           table ;
+
+    XGI330_LCDDataTablStruct *tempdi = 0 ;
+
+
+    tempbx = BX;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+        tempal = pVBInfo->SModeIDTable[ ModeIdIndex ].St_CRT2CRTC ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+        tempal = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT2CRTC ;
+    }
+
+    tempal = tempal & 0x0f ;
+
+    if ( tempbx <= 1 )          /* ExpLink */
+    {
+        if ( ModeNo <= 0x13 )
+        {
+            tempal = pVBInfo->SModeIDTable[ ModeIdIndex ].St_CRT2CRTC ; /* find no Ext_CRT2CRTC2 */
+        }
+        else
+        {
+            tempal= pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT2CRTC ;
+        }
+
+        if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+        {
+            if ( ModeNo <= 0x13 )
+                tempal = pVBInfo->SModeIDTable[ ModeIdIndex ].St_CRT2CRTC2 ;
+            else
+                tempal= pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT2CRTC2 ;
+        }
+
+        if ( tempbx & 0x01 )
+            tempal = ( tempal >> 4 ) ;
+
+        tempal = ( tempal & 0x0f ) ;
+    }
+
+    tempcx = LCDLenList[ tempbx ] ;     /* mov cl,byte ptr cs:LCDLenList[bx] */
+
+    if ( pVBInfo->LCDInfo & EnableScalingLCD )  /* ScaleLCD */
+    {
+        if ( ( tempbx == 5 ) || ( tempbx ) == 7 )
+            tempcx = LCDDesDataLen2 ;
+        else if ( ( tempbx == 3 ) || ( tempbx == 8 ) )
+            tempcx = LVDSDesDataLen2 ;
+    }
+    /* mov di, word ptr cs:LCDDataList[bx] */
+    /* tempdi=pVideoMemory[LCDDataList+tempbx*2]|(pVideoMemory[LCDDataList+tempbx*2+1]<<8); */
+
+    switch( tempbx )
+    {
+        case 0:
+            tempdi = XGI_EPLLCDCRT1Ptr_H ;
+            break ;
+        case 1:
+            tempdi = XGI_EPLLCDCRT1Ptr_V ;
+             break ;
+        case 2:
+            tempdi = XGI_EPLLCDDataPtr ;
+            break ;
+        case 3:
+            tempdi = XGI_EPLLCDDesDataPtr ;
+            break ;
+        case 4:
+            tempdi = XGI_LCDDataTable ;
+            break ;
+        case 5:
+            tempdi = XGI_LCDDesDataTable ;
+            break ;
+        case 6:
+            tempdi = XGI_EPLCHLCDRegPtr ;
+            break ;
+        case 7:
+        case 8:
+        case 9:
+            tempdi = 0 ;
+            break ;
+        default:
+        break ;
+    }
+
+    if ( tempdi == 0x00 )  /* OEMUtil */
+        return 0 ;
+
+    table = tempbx ;
+    i = 0 ;
+
+    while( tempdi[ i ].PANELID != 0xff )
+    {
+        tempdx = pVBInfo->LCDResInfo ;
+        if ( tempbx & 0x0080 )     /* OEMUtil */
+        {
+            tempbx &= ( ~0x0080 ) ;
+            tempdx = pVBInfo->LCDTypeInfo ;
+        }
+
+        if ( pVBInfo->LCDInfo & EnableScalingLCD )
+        tempdx &= ( ~PanelResInfo ) ;
+
+        if ( tempdi[ i ].PANELID == tempdx )
+        {
+            tempbx = tempdi[ i ].MASK ;
+            tempdx = pVBInfo->LCDInfo ;
+
+            if ( ModeNo <= 0x13 )         /* alan 09/10/2003 */
+                tempdx |= SetLCDStdMode ;
+
+            if ( modeflag & HalfDCLK )
+                tempdx |= SetLCDLowResolution ;
+
+            tempbx &= tempdx;
+            if ( tempbx == tempdi[ i ].CAP )
+                break ;
+        }
+        i++ ;
+    }
+
+    if ( table == 0 )
+    {
+        switch( tempdi[ i ].DATAPTR )
+        {
+            case 0:
+                return &XGI_LVDSCRT11024x768_1_H[ tempal ] ;
+                break ;
+            case 1:
+                return &XGI_LVDSCRT11024x768_2_H[ tempal ] ;
+                break ;
+            case 2:
+                return &XGI_LVDSCRT11280x1024_1_H[ tempal ] ;
+                break ;
+            case 3:
+                return &XGI_LVDSCRT11280x1024_2_H[ tempal ] ;
+                break ;
+            case 4:
+                return &XGI_LVDSCRT11400x1050_1_H[ tempal ] ;
+                break ;
+            case 5:
+                return &XGI_LVDSCRT11400x1050_2_H[ tempal ] ;
+                break ;
+            case 6:
+                return &XGI_LVDSCRT11600x1200_1_H[ tempal ] ;
+                break ;
+            case 7:
+                return &XGI_LVDSCRT11024x768_1_Hx75[ tempal ] ;
+                break ;
+            case 8:
+                return &XGI_LVDSCRT11024x768_2_Hx75[ tempal ] ;
+                break ;
+            case 9:
+                return &XGI_LVDSCRT11280x1024_1_Hx75[ tempal ] ;
+                break ;
+            case 10:
+                return &XGI_LVDSCRT11280x1024_2_Hx75[ tempal ] ;
+                break ;
+            default:
+                break ;
+        }
+    }
+    else if ( table == 1 )
+    {
+        switch( tempdi[ i ].DATAPTR )
+        {
+            case 0:
+                return &XGI_LVDSCRT11024x768_1_V[ tempal ] ;
+                break ;
+            case 1:
+                return &XGI_LVDSCRT11024x768_2_V[ tempal ] ;
+                break ;
+            case 2:
+                return &XGI_LVDSCRT11280x1024_1_V[ tempal ] ;
+                break ;
+            case 3:
+                return &XGI_LVDSCRT11280x1024_2_V[ tempal ] ;
+                break ;
+            case 4:
+                return &XGI_LVDSCRT11400x1050_1_V[ tempal ] ;
+                break ;
+            case 5:
+                return &XGI_LVDSCRT11400x1050_2_V[ tempal ] ;
+                break ;
+            case 6:
+                return &XGI_LVDSCRT11600x1200_1_V[ tempal ] ;
+                break ;
+            case 7:
+                return &XGI_LVDSCRT11024x768_1_Vx75[ tempal ] ;
+                break ;
+            case 8:
+                return &XGI_LVDSCRT11024x768_2_Vx75[ tempal ] ;
+                break ;
+            case 9:
+                return &XGI_LVDSCRT11280x1024_1_Vx75[ tempal ] ;
+                break ;
+            case 10:
+                return &XGI_LVDSCRT11280x1024_2_Vx75[ tempal ] ;
+                break ;
+            default:
+                break ;
+        }
+    }
+    else if ( table == 2 )
+    {
+        switch( tempdi[ i ].DATAPTR )
+        {
+            case 0:
+                return &XGI_LVDS1024x768Data_1[ tempal ] ;
+                break ;
+            case 1:
+                return &XGI_LVDS1024x768Data_2[ tempal ] ;
+                break ;
+            case 2:
+                return &XGI_LVDS1280x1024Data_1[ tempal ] ;
+                break ;
+            case 3:
+                return &XGI_LVDS1280x1024Data_2[ tempal ] ;
+                break ;
+            case 4:
+                return &XGI_LVDS1400x1050Data_1[ tempal ] ;
+                break ;
+            case 5:
+                return &XGI_LVDS1400x1050Data_2[ tempal ] ;
+                break ;
+            case 6:
+                return &XGI_LVDS1600x1200Data_1[ tempal ] ;
+                break ;
+            case 7:
+                return &XGI_LVDSNoScalingData[ tempal ] ;
+                break ;
+            case 8:
+                return &XGI_LVDS1024x768Data_1x75[ tempal ] ;
+                break ;
+            case 9:
+                return &XGI_LVDS1024x768Data_2x75[ tempal ] ;
+                break ;
+            case 10:
+                return &XGI_LVDS1280x1024Data_1x75[ tempal ] ;
+                break ;
+            case 11:
+                return &XGI_LVDS1280x1024Data_2x75[ tempal ] ;
+                break ;
+            case 12:
+                return &XGI_LVDSNoScalingDatax75[ tempal ] ;
+                break ;
+            default:
+                break ;
+        }
+    }
+    else if ( table == 3 )
+    {
+        switch( tempdi[ i ].DATAPTR )
+        {
+            case 0:
+                return &XGI_LVDS1024x768Des_1[ tempal ] ;
+                break ;
+            case 1:
+                return &XGI_LVDS1024x768Des_3[ tempal ] ;
+                break ;
+            case 2:
+                return &XGI_LVDS1024x768Des_2[ tempal ] ;
+                break ;
+            case 3:
+                return &XGI_LVDS1280x1024Des_1[ tempal ] ;
+                break ;
+            case 4:
+                return &XGI_LVDS1280x1024Des_2[ tempal ] ;
+                break ;
+            case 5:
+                return &XGI_LVDS1400x1050Des_1[ tempal ] ;
+                break ;
+            case 6:
+                return &XGI_LVDS1400x1050Des_2[ tempal ] ;
+                break ;
+            case 7:
+                return &XGI_LVDS1600x1200Des_1[ tempal ] ;
+                break ;
+            case 8:
+                return &XGI_LVDSNoScalingDesData[ tempal ] ;
+                break ;
+            case 9:
+                return &XGI_LVDS1024x768Des_1x75[ tempal ] ;
+                break ;
+            case 10:
+                return &XGI_LVDS1024x768Des_3x75[ tempal ] ;
+                break ;
+            case 11:
+                return &XGI_LVDS1024x768Des_2x75[ tempal ] ;
+                break;
+            case 12:
+                return &XGI_LVDS1280x1024Des_1x75[ tempal ] ;
+                break ;
+            case 13:
+                return &XGI_LVDS1280x1024Des_2x75[ tempal ] ;
+                break ;
+            case 14:
+                return &XGI_LVDSNoScalingDesDatax75[ tempal ] ;
+                break ;
+            default:
+                break ;
+        }
+    }
+    else if ( table == 4 )
+    {
+        switch( tempdi[ i ].DATAPTR )
+        {
+            case 0:
+                return &XGI_ExtLCD1024x768Data[ tempal ] ;
+                break ;
+            case 1:
+                return &XGI_StLCD1024x768Data[ tempal ] ;
+                break ;
+            case 2:
+                return &XGI_CetLCD1024x768Data[ tempal ] ;
+                break ;
+            case 3:
+                return &XGI_ExtLCD1280x1024Data[ tempal ] ;
+                break ;
+            case 4:
+                return &XGI_StLCD1280x1024Data[ tempal ] ;
+                break ;
+            case 5:
+                return &XGI_CetLCD1280x1024Data[ tempal ] ;
+                break ;
+            case 6:
+                return &XGI_ExtLCD1400x1050Data[ tempal ] ;
+                break ;
+            case 7:
+                return &XGI_StLCD1400x1050Data[ tempal ] ;
+                break ;
+            case 8:
+                return &XGI_CetLCD1400x1050Data[ tempal ] ;
+                break ;
+            case 9:
+                return &XGI_ExtLCD1600x1200Data[ tempal ] ;
+                break  ;
+            case 10:
+                return &XGI_StLCD1600x1200Data[ tempal ] ;
+                break ;
+            case 11:
+                return &XGI_NoScalingData[ tempal ] ;
+                break ;
+            case 12:
+                return &XGI_ExtLCD1024x768x75Data[ tempal ] ;
+                break ;
+            case 13:
+                return &XGI_ExtLCD1024x768x75Data[ tempal ] ;
+                break ;
+            case 14:
+                return &XGI_CetLCD1024x768x75Data[ tempal ] ;
+                break ;
+            case 15:
+                 return &XGI_ExtLCD1280x1024x75Data[ tempal ] ;
+                break ;
+            case 16:
+                return &XGI_StLCD1280x1024x75Data[ tempal ] ;
+                break;
+            case 17:
+                return &XGI_CetLCD1280x1024x75Data[ tempal ] ;
+                break;
+            case 18:
+                return &XGI_NoScalingDatax75[ tempal ] ;
+                break ;
+            default:
+                break ;
+        }
+    }
+    else if ( table == 5 )
+    {
+        switch( tempdi[ i ].DATAPTR )
+        {
+            case 0:
+                return &XGI_ExtLCDDes1024x768Data[ tempal ] ;
+                break ;
+            case 1:
+                return &XGI_StLCDDes1024x768Data[ tempal ] ;
+                break ;
+            case 2:
+                return &XGI_CetLCDDes1024x768Data[ tempal ] ;
+                break ;
+            case 3:
+                if ( ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) )
+                    return &XGI_ExtLCDDLDes1280x1024Data[ tempal ] ;
+                else
+                    return &XGI_ExtLCDDes1280x1024Data[ tempal ] ;
+                break ;
+            case 4:
+                if ( ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) )
+                    return &XGI_StLCDDLDes1280x1024Data[ tempal ] ;
+                else
+                    return &XGI_StLCDDes1280x1024Data[ tempal ] ;
+                break ;
+            case 5:
+                if ( ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) )
+                    return &XGI_CetLCDDLDes1280x1024Data[ tempal ] ;
+                else
+                    return &XGI_CetLCDDes1280x1024Data[ tempal ] ;
+                break ;
+            case 6:
+                if ( ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) )
+                    return &XGI_ExtLCDDLDes1400x1050Data[ tempal ] ;
+                else
+                    return &XGI_ExtLCDDes1400x1050Data[ tempal ] ;
+                break ;
+            case 7:
+                if ( ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) )
+                    return &XGI_StLCDDLDes1400x1050Data[ tempal ] ;
+                else
+                    return &XGI_StLCDDes1400x1050Data[ tempal ] ;
+                break ;
+            case 8:
+                return &XGI_CetLCDDes1400x1050Data[ tempal ] ;
+                break ;
+            case 9:
+                return &XGI_CetLCDDes1400x1050Data2[ tempal ] ;
+                break ;
+            case 10:
+                if ( ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) )
+                    return &XGI_ExtLCDDLDes1600x1200Data[ tempal ] ;
+                else
+                    return &XGI_ExtLCDDes1600x1200Data[ tempal ] ;
+                break ;
+            case 11:
+                if ( ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) )
+                    return &XGI_StLCDDLDes1600x1200Data[ tempal ] ;
+                else
+                    return &XGI_StLCDDes1600x1200Data[ tempal ] ;
+                break ;
+            case 12:
+                return &XGI_NoScalingDesData[ tempal ] ;
+                break;
+            case 13:
+                return &XGI_ExtLCDDes1024x768x75Data[ tempal ] ;
+                break ;
+            case 14:
+                return &XGI_StLCDDes1024x768x75Data[ tempal ] ;
+                break ;
+            case 15:
+                return &XGI_CetLCDDes1024x768x75Data[ tempal ] ;
+                break ;
+            case 16:
+                if ( ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) )
+                    return &XGI_ExtLCDDLDes1280x1024x75Data[ tempal ] ;
+                else
+                    return &XGI_ExtLCDDes1280x1024x75Data[ tempal ] ;
+                break ;
+            case 17:
+                if ( ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) )
+                    return &XGI_StLCDDLDes1280x1024x75Data[ tempal ] ;
+                else
+                    return &XGI_StLCDDes1280x1024x75Data[ tempal ] ;
+                break ;
+            case 18:
+                if ( ( pVBInfo->VBType & VB_XGI301LV ) || ( pVBInfo->VBType & VB_XGI302LV ) )
+                    return &XGI_CetLCDDLDes1280x1024x75Data[ tempal ] ;
+                else
+                    return &XGI_CetLCDDes1280x1024x75Data[ tempal ] ;
+                break ;
+            case 19:
+                return &XGI_NoScalingDesDatax75[ tempal ] ;
+                break ;
+            default:
+                break ;
+        }
+    }
+    else if ( table == 6 )
+    {
+        switch( tempdi[ i ].DATAPTR )
+        {
+            case 0:
+                return &XGI_CH7017LV1024x768[ tempal ] ;
+                break ;
+            case 1:
+                return &XGI_CH7017LV1400x1050[ tempal ] ;
+                break ;
+            default:
+                break ;
+        }
+    }
+    return 0 ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetTVPtr */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void* XGI_GetTVPtr (USHORT BX,USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT i , tempdx , tempbx , tempal , modeflag , table ;
+    XGI330_TVDataTablStruct *tempdi = 0 ;
+
+    tempbx = BX ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+        tempal = pVBInfo->SModeIDTable[ ModeIdIndex ].St_CRT2CRTC ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+        tempal = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT2CRTC ;
+    }
+
+    tempal = tempal & 0x3f ;
+    table = tempbx ;
+
+    switch( tempbx )
+    {
+        case 0:
+            tempdi = 0 ;        /*EPLCHTVCRT1Ptr_H;*/
+            if ( pVBInfo->IF_DEF_CH7007 == 1 )
+            {
+                tempdi = XGI_EPLCHTVCRT1Ptr;
+            }
+            break ;
+        case 1:
+            tempdi = 0 ;        /*EPLCHTVCRT1Ptr_V;*/
+            if ( pVBInfo->IF_DEF_CH7007 == 1 )
+            {
+                tempdi = XGI_EPLCHTVCRT1Ptr;
+            }
+            break ;
+        case 2:
+            tempdi = XGI_EPLCHTVDataPtr ;
+            break ;
+        case 3:
+            tempdi = 0 ;
+            break ;
+        case 4:
+            tempdi = XGI_TVDataTable ;
+            break ;
+        case 5:
+            tempdi = 0 ;
+            break ;
+        case 6:
+            tempdi = XGI_EPLCHTVRegPtr ;
+            break ;
+        default:
+            break ;
+    }
+
+    if ( tempdi == 0x00 )  /* OEMUtil */
+        return( 0 ) ;
+
+    tempdx = pVBInfo->TVInfo ;
+
+    if ( pVBInfo->VBInfo & SetInSlaveMode )
+        tempdx = tempdx | SetTVLockMode ;
+
+    if ( modeflag & HalfDCLK )
+        tempdx = tempdx | SetTVLowResolution ;
+
+    i = 0 ;
+
+    while( tempdi[ i ].MASK != 0xffff )
+    {
+        if ( ( tempdx & tempdi[ i ].MASK ) == tempdi[ i ].CAP )
+            break ;
+        i++ ;
+    }
+
+    if ( table == 0x00 ) /* 07/05/22 */
+    {
+#ifdef WIN2000
+        if ( pVBInfo->IF_DEF_CH7007 == 1 )
+        {
+          switch( tempdi[ i ].DATAPTR )
+          {
+            case 0:
+                return &CH7007TVCRT1UNTSC_H[ tempal ] ;
+                break ;
+            case 1:
+                return &CH7007TVCRT1ONTSC_H[ tempal ] ;
+                break ;
+            case 2:
+                return &CH7007TVCRT1UPAL_H[ tempal ] ;
+                break ;
+            case 3:
+                return &CH7007TVCRT1OPAL_H[ tempal ] ;
+                break ;
+            default:
+                break ;
+          }
+        }
+#endif
+    }
+    else if ( table == 0x01 )
+    {
+#ifdef WIN2000
+        if ( pVBInfo->IF_DEF_CH7007 == 1 )
+        {
+          switch( tempdi[ i ].DATAPTR )
+          {
+            case 0:
+                return &CH7007TVCRT1UNTSC_V[ tempal ] ;
+                break ;
+            case 1:
+                return &CH7007TVCRT1ONTSC_V[ tempal ] ;
+                break ;
+            case 2:
+                return &CH7007TVCRT1UPAL_V[ tempal ] ;
+                break ;
+            case 3:
+                return &CH7007TVCRT1OPAL_V[ tempal ] ;
+                break ;
+            default:
+                break ;
+          }
+        }
+#endif
+    }
+    else if ( table == 0x04 )
+    {
+        switch( tempdi[ i ].DATAPTR )
+        {
+            case 0:
+                return &XGI_ExtPALData[ tempal ] ;
+                break ;
+            case 1:
+                return &XGI_ExtNTSCData[ tempal ] ;
+                break ;
+            case 2:
+                return &XGI_StPALData[ tempal ] ;
+                break ;
+            case 3:
+                return &XGI_StNTSCData[ tempal ] ;
+                break ;
+            case 4:
+                return &XGI_ExtHiTVData[ tempal ] ;
+                break ;
+            case 5:
+                return &XGI_St2HiTVData[ tempal ] ;
+                break ;
+            case 6:
+                return &XGI_ExtYPbPr525iData[ tempal ] ;
+                break ;
+            case 7:
+                return &XGI_ExtYPbPr525pData[ tempal ] ;
+                break ;
+            case 8:
+                return &XGI_ExtYPbPr750pData[ tempal ] ;
+                break ;
+            case 9:
+                return &XGI_StYPbPr525iData[ tempal ] ;
+                break ;
+            case 10:
+                return &XGI_StYPbPr525pData[ tempal ] ;
+                break ;
+            case 11:
+                return &XGI_StYPbPr750pData[ tempal ] ;
+                break;
+            case 12:    /* avoid system hang */
+                return &XGI_ExtNTSCData[ tempal ] ;
+                break ;
+            case 13:
+                return &XGI_St1HiTVData[ tempal ] ;
+                break ;
+            default:
+                break ;
+        }
+    }
+    else if( table == 0x02 )
+    {
+        switch( tempdi[ i ].DATAPTR )
+        {
+            case 0:
+                return &XGI_CHTVUNTSCData[ tempal ] ;
+                break ;
+            case 1:
+                return &XGI_CHTVONTSCData[ tempal ] ;
+                break ;
+            case 2:
+                return &XGI_CHTVUPALData[ tempal ] ;
+                break ;
+            case 3:
+                return &XGI_CHTVOPALData[ tempal ] ;
+                break ;
+            default:
+                break ;
+        }
+    }
+    else if( table == 0x06 )
+    {
+#ifdef WIN2000
+        if ( pVBInfo->IF_DEF_CH7007 == 1 )
+        {
+          /* VideoDebugPrint((0, "XGI_GetTVPtr: pVBInfo->IF_DEF_CH7007==1\n")); */
+          switch( tempdi[ i ].DATAPTR )
+          {
+            case 0:
+                return &CH7007TVReg_UNTSC[ tempal ] ;
+                break ;
+            case 1:
+                return &CH7007TVReg_ONTSC[ tempal ] ;
+                break ;
+            case 2:
+                return &CH7007TVReg_UPAL[ tempal ] ;
+                break ;
+            case 3:
+                return &CH7007TVReg_OPAL[ tempal ] ;
+                break ;
+            default:
+                break ;
+          }
+        }
+        else
+        {
+            switch( tempdi[ i ].DATAPTR )
+            {
+              case 0:
+                return &XGI_CHTVRegUNTSC[ tempal ] ;
+                break ;
+              case 1:
+                return &XGI_CHTVRegONTSC[ tempal ] ;
+                break ;
+              case 2:
+                return &XGI_CHTVRegUPAL[ tempal ] ;
+                break ;
+              case 3:
+                return &XGI_CHTVRegOPAL[ tempal ] ;
+                break ;
+              default:
+                break ;
+            }
+        }
+#endif
+    }
+    return( 0 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_BacklightByDrv */
+/* Input : */
+/* Output : TRUE -> Skip backlight control */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_BacklightByDrv( PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR tempah ;
+
+    tempah = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x3A ) ;
+    if ( tempah & BacklightControlBit )
+        return TRUE ;
+    else
+        return FALSE ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_FirePWDDisable */
+/* Input : */
+/* Output : */
+/* Description : Turn off VDD & Backlight : Fire disable procedure */
+/* --------------------------------------------------------------------- */
+/*
+void XGI_FirePWDDisable( PVB_DEVICE_INFO pVBInfo )
+{
+    XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x26 , 0x00 , 0xFC ) ;
+}
+*/
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_FirePWDEnable */
+/* Input : */
+/* Output : */
+/* Description : Turn on VDD & Backlight : Fire enable procedure */
+/* --------------------------------------------------------------------- */
+void XGI_FirePWDEnable(PVB_DEVICE_INFO pVBInfo )
+{
+    XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x26 , 0x03 , 0xFC ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_EnableGatingCRT */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_EnableGatingCRT(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo)
+{
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x63 , 0xBF , 0x40 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_DisableGatingCRT */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_DisableGatingCRT(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo)
+{
+
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x63 , 0xBF , 0x00 ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetPanelDelay */
+/* Input : */
+/* Output : */
+/* Description : */
+/* I/P : bl : 1 ; T1 : the duration between CPL on and signal on */
+/* : bl : 2 ; T2 : the duration signal on and Vdd on */
+/* : bl : 3 ; T3 : the duration between CPL off and signal off */
+/* : bl : 4 ; T4 : the duration signal off and Vdd off */
+/* --------------------------------------------------------------------- */
+void XGI_SetPanelDelay(USHORT tempbl, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT index ;
+
+    index = XGI_GetLCDCapPtr(pVBInfo) ;
+
+    if ( tempbl == 1 )
+        XGINew_LCD_Wait_Time( pVBInfo->LCDCapList[ index ].PSC_S1, pVBInfo ) ;
+
+    if ( tempbl == 2 )
+        XGINew_LCD_Wait_Time( pVBInfo->LCDCapList[ index ].PSC_S2, pVBInfo ) ;
+
+    if ( tempbl == 3 )
+        XGINew_LCD_Wait_Time( pVBInfo->LCDCapList[ index ].PSC_S3, pVBInfo ) ;
+
+    if ( tempbl == 4 )
+        XGINew_LCD_Wait_Time( pVBInfo->LCDCapList[ index ].PSC_S4, pVBInfo ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetPanelPower */
+/* Input : */
+/* Output : */
+/* Description : */
+/* I/O : ah = 0011b = 03h ; Backlight on, Power on */
+/* = 0111b = 07h ; Backlight on, Power off */
+/* = 1011b = 0Bh ; Backlight off, Power on */
+/* = 1111b = 0Fh ; Backlight off, Power off */
+/* --------------------------------------------------------------------- */
+void XGI_SetPanelPower(USHORT tempah,USHORT tempbl, PVB_DEVICE_INFO pVBInfo)
+{
+    if ( pVBInfo->VBType & ( VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+        XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x26 , tempbl , tempah ) ;
+    else
+        XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x11 , tempbl , tempah ) ;
+}
+
+UCHAR XG21GPIODataTransfer(UCHAR ujDate)
+{
+    UCHAR  ujRet = 0;
+    UCHAR  i = 0;
+
+    for (i=0; i<8; i++)
+        {
+        ujRet = ujRet << 1;
+        /* ujRet |= GETBITS(ujDate >> i, 0:0); */
+        ujRet |= (ujDate >> i) & 1;
+    }
+
+        return ujRet;
+}
+
+/*----------------------------------------------------------------------------*/
+/* output                                                                     */
+/*      bl[5] : LVDS signal                                                   */
+/*      bl[1] : LVDS backlight                                                */
+/*      bl[0] : LVDS VDD                                                      */
+/*----------------------------------------------------------------------------*/
+UCHAR XGI_XG21GetPSCValue(PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR CR4A,temp;
+
+    CR4A = XGINew_GetReg1( pVBInfo->P3d4 , 0x4A ) ;
+    XGINew_SetRegAND( pVBInfo->P3d4 , 0x4A , ~0x23 ) ; /* enable GPIO write */
+
+    temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x48 ) ;
+
+    temp = XG21GPIODataTransfer(temp);
+    temp &= 0x23;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x4A , CR4A ) ;
+    return temp;
+}
+
+/*----------------------------------------------------------------------------*/
+/* output                                                                     */
+/*      bl[5] : LVDS signal                                                   */
+/*      bl[1] : LVDS backlight                                                */
+/*      bl[0] : LVDS VDD                                                      */
+/*----------------------------------------------------------------------------*/
+UCHAR XGI_XG27GetPSCValue(PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR CR4A,CRB4,temp;
+
+    CR4A = XGINew_GetReg1( pVBInfo->P3d4 , 0x4A ) ;
+    XGINew_SetRegAND( pVBInfo->P3d4 , 0x4A , ~0x0C ) ; /* enable GPIO write */
+
+    temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x48 ) ;
+
+    temp &= 0x0C;
+    temp >>= 2;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x4A , CR4A ) ;
+    CRB4 = XGINew_GetReg1( pVBInfo->P3d4 , 0xB4 ) ;
+    temp |= ((CRB4&0x04)<<3);
+    return temp;
+}
+/*----------------------------------------------------------------------------*/
+/* input                                                                      */
+/*      bl[5] : 1;LVDS signal on                                              */
+/*      bl[1] : 1;LVDS backlight on                                           */
+/*      bl[0] : 1:LVDS VDD on                                                 */
+/*      bh: 100000b : clear bit 5, to set bit5                                */
+/*          000010b : clear bit 1, to set bit1                                */
+/*          000001b : clear bit 0, to set bit0                                */
+/*----------------------------------------------------------------------------*/
+void XGI_XG21BLSignalVDD(USHORT tempbh,USHORT tempbl, PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR CR4A,temp;
+
+    CR4A = XGINew_GetReg1( pVBInfo->P3d4 , 0x4A ) ;
+    tempbh &= 0x23;
+    tempbl &= 0x23;
+    XGINew_SetRegAND( pVBInfo->P3d4 , 0x4A , ~tempbh ) ; /* enable GPIO write */
+
+    if (tempbh&0x20)
+    {
+      temp = (tempbl>>4)&0x02;
+
+      XGINew_SetRegANDOR( pVBInfo->P3d4 , 0xB4 , ~0x02 , temp) ; /* CR B4[1] */
+
+    }
+
+    temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x48 ) ;
+
+    temp = XG21GPIODataTransfer(temp);
+    temp &= ~tempbh;
+    temp |= tempbl;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x48 , temp ) ;
+}
+
+void XGI_XG27BLSignalVDD(USHORT tempbh,USHORT tempbl, PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR CR4A,temp;
+    USHORT tempbh0,tempbl0;
+
+    tempbh0 = tempbh;
+    tempbl0 = tempbl;
+    tempbh0 &= 0x20;
+    tempbl0 &= 0x20;
+    tempbh0 >>= 3;
+    tempbl0 >>= 3;
+
+    if (tempbh&0x20)
+    {
+      temp = (tempbl>>4)&0x02;
+
+      XGINew_SetRegANDOR( pVBInfo->P3d4 , 0xB4 , ~0x02 , temp) ; /* CR B4[1] */
+
+    }
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0xB4 , ~tempbh0 , tempbl0 ) ;
+
+    CR4A = XGINew_GetReg1( pVBInfo->P3d4 , 0x4A ) ;
+    tempbh &= 0x03;
+    tempbl &= 0x03;
+    tempbh <<= 2;
+    tempbl <<= 2;                                       /* GPIOC,GPIOD */
+    XGINew_SetRegAND( pVBInfo->P3d4 , 0x4A , ~tempbh ) ; /* enable GPIO write */
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x48 , ~tempbh , tempbl ) ;
+}
+
+/* --------------------------------------------------------------------- */
+USHORT XGI_GetLVDSOEMTableIndex(PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT index ;
+
+    index = XGINew_GetReg1( pVBInfo->P3d4 , 0x36 ) ;
+    if (index<sizeof(XGI21_LCDCapList)/sizeof(XGI21_LVDSCapStruct))
+    {
+      return index;
+    }
+    return 0;
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_XG21SetPanelDelay */
+/* Input : */
+/* Output : */
+/* Description : */
+/* I/P : bl : 1 ; T1 : the duration between CPL on and signal on */
+/* : bl : 2 ; T2 : the duration signal on and Vdd on */
+/* : bl : 3 ; T3 : the duration between CPL off and signal off */
+/* : bl : 4 ; T4 : the duration signal off and Vdd off */
+/* --------------------------------------------------------------------- */
+void XGI_XG21SetPanelDelay(USHORT tempbl, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT index ;
+
+    index = XGI_GetLVDSOEMTableIndex( pVBInfo );
+    if ( tempbl == 1 )
+        XGINew_LCD_Wait_Time( pVBInfo->XG21_LVDSCapList[ index ].PSC_S1, pVBInfo ) ;
+
+    if ( tempbl == 2 )
+        XGINew_LCD_Wait_Time( pVBInfo->XG21_LVDSCapList[ index ].PSC_S2, pVBInfo ) ;
+
+    if ( tempbl == 3 )
+        XGINew_LCD_Wait_Time( pVBInfo->XG21_LVDSCapList[ index ].PSC_S3, pVBInfo ) ;
+
+    if ( tempbl == 4 )
+        XGINew_LCD_Wait_Time( pVBInfo->XG21_LVDSCapList[ index ].PSC_S4, pVBInfo ) ;
+}
+
+BOOLEAN XGI_XG21CheckLVDSMode(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT xres ,
+           yres ,
+           colordepth ,
+           modeflag ,
+           resindex ,
+           lvdstableindex;
+
+    resindex = XGI_GetResInfo( ModeNo , ModeIdIndex, pVBInfo ) ;
+    if ( ModeNo <= 0x13 )
+    {
+        xres = pVBInfo->StResInfo[ resindex ].HTotal ;
+        yres = pVBInfo->StResInfo[ resindex ].VTotal ;
+        modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;    /* si+St_ResInfo */
+    }
+    else
+    {
+        xres = pVBInfo->ModeResInfo[ resindex ].HTotal ;                          /* xres->ax */
+        yres = pVBInfo->ModeResInfo[ resindex ].VTotal ;                          /* yres->bx */
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex].Ext_ModeFlag ; /* si+St_ModeFlag */
+    }
+
+    if ( !( modeflag & Charx8Dot ) )
+    {
+        xres /= 9;
+        xres *= 8;
+    }
+
+    if ( ModeNo > 0x13 )
+    {
+        if ( ( ModeNo>0x13 ) && ( modeflag & HalfDCLK ) )
+        {
+          xres *=  2 ;
+        }
+        if ( ( ModeNo>0x13 ) && ( modeflag & DoubleScanMode ) )
+        {
+          yres *=  2 ;
+        }
+    }
+
+    lvdstableindex = XGI_GetLVDSOEMTableIndex( pVBInfo );
+    if ( xres > (pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHDE) )
+      return FALSE;
+
+    if ( yres > (pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVDE) )
+      return FALSE;
+
+    if ( ModeNo > 0x13 )
+    {
+      if ( ( xres != (pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHDE) ) ||
+           ( yres != (pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVDE)) )
+      {
+          colordepth = XGI_GetColorDepth( ModeNo , ModeIdIndex, pVBInfo ) ;
+          if ( colordepth > 2 )
+          {
+            return FALSE;
+          }
+      }
+    }
+    return TRUE;
+}
+
+void XGI_SetXG21FPBits(PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR temp;
+
+    temp = XGINew_GetReg1( pVBInfo->P3d4  , 0x37 ) ;  /* D[0] 1: 18bit */
+    temp = ( temp & 1 ) << 6;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x06 , ~0x40 , temp ) ;      /* SR06[6] 18bit Dither */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x09 , ~0xc0 , temp | 0x80 ) ;  /* SR09[7] enable FP output, SR09[6] 1: sigle 18bits, 0: dual 12bits */
+
+}
+
+void XGI_SetXG27FPBits(PVB_DEVICE_INFO pVBInfo)
+{
+    UCHAR temp;
+
+    temp = XGINew_GetReg1( pVBInfo->P3d4  , 0x37 ) ;  /* D[1:0] 01: 18bit, 00: dual 12, 10: single 24 */
+    temp = ( temp & 3 ) << 6;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x06 , ~0xc0 , temp & 0x80 ) ;  /* SR06[7]0: dual 12/1: single 24 [6] 18bit Dither <= 0 h/w recommend */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x09 , ~0xc0 , temp | 0x80 ) ;  /* SR09[7] enable FP output, SR09[6] 1: sigle 18bits, 0: 24bits */
+
+}
+
+void XGI_SetXG21LVDSPara(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR temp,Miscdata;
+    USHORT xres ,
+           yres ,
+           modeflag ,
+           resindex ,
+           lvdstableindex ;
+    USHORT LVDSHT,LVDSHBS,LVDSHRS,LVDSHRE,LVDSHBE;
+    USHORT LVDSVT,LVDSVBS,LVDSVRS,LVDSVRE,LVDSVBE;
+    USHORT value;
+
+    lvdstableindex = XGI_GetLVDSOEMTableIndex( pVBInfo );
+
+    temp = (UCHAR) ( ( pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDS_Capability & (LCDPolarity << 8 ) ) >> 8 );
+    temp &= LCDPolarity;
+    Miscdata =(UCHAR) XGINew_GetReg2(pVBInfo->P3cc) ;
+
+    XGINew_SetReg3( pVBInfo->P3c2 , (Miscdata & 0x3F) | temp ) ;
+
+    temp = (UCHAR) ( pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDS_Capability & LCDPolarity ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x35 , ~0x80 , temp&0x80 ) ;      /* SR35[7] FP VSync polarity */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x30 , ~0x20 , (temp&0x40)>>1 ) ;   /* SR30[5] FP HSync polarity */
+
+    XGI_SetXG21FPBits(pVBInfo);
+    resindex = XGI_GetResInfo( ModeNo , ModeIdIndex, pVBInfo ) ;
+    if ( ModeNo <= 0x13 )
+    {
+        xres = pVBInfo->StResInfo[ resindex ].HTotal ;
+        yres = pVBInfo->StResInfo[ resindex ].VTotal ;
+        modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;    /* si+St_ResInfo */
+    }
+    else
+    {
+        xres = pVBInfo->ModeResInfo[ resindex ].HTotal ;                          /* xres->ax */
+        yres = pVBInfo->ModeResInfo[ resindex ].VTotal ;                          /* yres->bx */
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex].Ext_ModeFlag ; /* si+St_ModeFlag */
+    }
+
+    if (!( modeflag & Charx8Dot ))
+      xres = xres * 8 / 9;
+
+    LVDSHT = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHT;
+
+    LVDSHBS = xres + ( pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHDE - xres ) / 2 ;
+    if ( ( ModeNo<=0x13 ) && ( modeflag & HalfDCLK ) )
+    {
+      LVDSHBS -=  xres/4 ;
+    }
+    if (LVDSHBS > LVDSHT) LVDSHBS -= LVDSHT ;
+
+    LVDSHRS = LVDSHBS + pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHFP ;
+    if (LVDSHRS > LVDSHT) LVDSHRS -= LVDSHT ;
+
+    LVDSHRE = LVDSHRS + pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHSYNC ;
+    if (LVDSHRE > LVDSHT) LVDSHRE -= LVDSHT ;
+
+    LVDSHBE = LVDSHBS + LVDSHT - pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHDE ;
+
+    LVDSVT = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVT;
+
+    LVDSVBS = yres + ( pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVDE - yres ) / 2 ;
+    if ( ( ModeNo>0x13 ) && ( modeflag & DoubleScanMode ) )
+    {
+      LVDSVBS +=  yres/2 ;
+    }
+    if (LVDSVBS > LVDSVT) LVDSVBS -= LVDSVT ;
+
+    LVDSVRS = LVDSVBS + pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVFP ;
+    if (LVDSVRS > LVDSVT) LVDSVRS -= LVDSVT ;
+
+    LVDSVRE = LVDSVRS + pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVSYNC ;
+    if (LVDSVRE > LVDSVT) LVDSVRE -= LVDSVT ;
+
+    LVDSVBE = LVDSVBS + LVDSVT - pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVDE ;
+
+    temp = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x11 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x11 , temp & 0x7f ) ;              /* Unlock CRTC */
+
+    if (!( modeflag & Charx8Dot ))
+    {
+        XGINew_SetRegOR( pVBInfo->P3c4 , 0x1 , 0x1 ) ;
+    }
+
+    /* HT SR0B[1:0] CR00 */
+    value = ( LVDSHT >> 3 ) - 5;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0B , ~0x03 , ( value & 0x300 ) >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x0 , (value & 0xFF) ) ;
+
+    /* HBS SR0B[5:4] CR02 */
+    value = ( LVDSHBS >> 3 ) - 1;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0B , ~0x30 , ( value & 0x300 ) >> 4 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x2 , (value & 0xFF) ) ;
+
+    /* HBE SR0C[1:0] CR05[7] CR03[4:0] */
+    value = ( LVDSHBE >> 3 ) - 1;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0C , ~0x03 , ( value & 0xC0 ) >> 6 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x05 , ~0x80 , ( value & 0x20 ) << 2 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x03 , ~0x1F , value & 0x1F ) ;
+
+    /* HRS SR0B[7:6] CR04 */
+    value = ( LVDSHRS >> 3 ) + 2;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0B , ~0xC0 , ( value & 0x300 ) >> 2 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x4 , (value & 0xFF) ) ;
+
+    /* Panel HRS SR2F[1:0] SR2E[7:0]  */
+    value--;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x2F , ~0x03 , ( value & 0x300 ) >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2E , (value & 0xFF) ) ;
+
+    /* HRE SR0C[2] CR05[4:0] */
+    value = ( LVDSHRE >> 3 ) + 2;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0C , ~0x04 , ( value & 0x20 ) >> 3 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x05 , ~0x1F , value & 0x1F ) ;
+
+    /* Panel HRE SR2F[7:2]  */
+    value--;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x2F , ~0xFC , value << 2 ) ;
+
+    /* VT SR0A[0] CR07[5][0] CR06 */
+    value = LVDSVT - 2 ;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0A , ~0x01 , ( value & 0x400 ) >> 10 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x07 , ~0x20 , ( value & 0x200 ) >> 4 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x07 , ~0x01 , ( value & 0x100 ) >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x06 , (value & 0xFF) ) ;
+
+    /* VBS SR0A[2] CR09[5] CR07[3] CR15 */
+    value = LVDSVBS - 1 ;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0A , ~0x04 , ( value & 0x400 ) >> 8 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x09 , ~0x20 , ( value & 0x200 ) >> 4 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x07 , ~0x08 , ( value & 0x100 ) >> 5 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x15 , (value & 0xFF) ) ;
+
+    /* VBE SR0A[4] CR16 */
+    value = LVDSVBE - 1;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0A , ~0x10 , ( value & 0x100 ) >> 4 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x16 , (value & 0xFF) ) ;
+
+    /* VRS SR0A[3] CR7[7][2] CR10 */
+    value = LVDSVRS - 1 ;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0A , ~0x08 , ( value & 0x400 ) >> 7 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x07 , ~0x80 , ( value & 0x200 ) >> 2 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x07 , ~0x04 , ( value & 0x100 ) >> 6 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x10 , (value & 0xFF) ) ;
+
+    /* Panel VRS SR3F[1:0] SR34[7:0] SR33[0] */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x3F , ~0x03 , ( value & 0x600 ) >> 9 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x34 , (value >> 1) & 0xFF ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x33 , ~0x01 , value & 0x01 ) ;
+
+    /* VRE SR0A[5] CR11[3:0] */
+    value = LVDSVRE - 1;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0A , ~0x20 , ( value & 0x10 ) << 1 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x11 , ~0x0F , value & 0x0F ) ;
+
+    /* Panel VRE SR3F[7:2] */ /* SR3F[7] has to be 0, h/w bug */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x3F , ~0xFC , ( value << 2 ) & 0x7C ) ;
+
+    for ( temp=0, value = 0; temp < 3; temp++)
+    {
+
+        XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x31 , ~0x30 , value ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2B , pVBInfo->XG21_LVDSCapList[lvdstableindex].VCLKData1) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2C , pVBInfo->XG21_LVDSCapList[lvdstableindex].VCLKData2) ;
+        value += 0x10;
+    }
+
+    if (!( modeflag & Charx8Dot ))
+    {
+        XGINew_GetReg2( pVBInfo->P3da ) ;           /* reset 3da */
+        XGINew_SetReg3( pVBInfo->P3c0 , 0x13 ) ;    /* set index */
+        XGINew_SetReg3( pVBInfo->P3c0 , 0x00 ) ;    /* set data, panning = 0, shift left 1 dot*/
+
+        XGINew_GetReg2( pVBInfo->P3da ) ;           /* Enable Attribute */
+        XGINew_SetReg3( pVBInfo->P3c0 , 0x20 ) ;
+
+        XGINew_GetReg2( pVBInfo->P3da ) ;           /* reset 3da */
+    }
+
+
+}
+
+/* no shadow case */
+void XGI_SetXG27LVDSPara(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR temp,Miscdata;
+    USHORT xres ,
+           yres ,
+           modeflag ,
+           resindex ,
+           lvdstableindex ;
+    USHORT LVDSHT,LVDSHBS,LVDSHRS,LVDSHRE,LVDSHBE;
+    USHORT LVDSVT,LVDSVBS,LVDSVRS,LVDSVRE,LVDSVBE;
+    USHORT value;
+
+    lvdstableindex = XGI_GetLVDSOEMTableIndex( pVBInfo );
+    temp = (UCHAR) ( ( pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDS_Capability & (LCDPolarity << 8 ) ) >> 8 );
+    temp &= LCDPolarity;
+    Miscdata =(UCHAR) XGINew_GetReg2(pVBInfo->P3cc) ;
+
+    XGINew_SetReg3( pVBInfo->P3c2 , (Miscdata & 0x3F) | temp ) ;
+
+    temp = (UCHAR) ( pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDS_Capability & LCDPolarity ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x35 , ~0x80 , temp&0x80 ) ;      /* SR35[7] FP VSync polarity */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x30 , ~0x20 , (temp&0x40)>>1 ) ;   /* SR30[5] FP HSync polarity */
+
+    XGI_SetXG27FPBits(pVBInfo);
+    resindex = XGI_GetResInfo( ModeNo , ModeIdIndex, pVBInfo ) ;
+    if ( ModeNo <= 0x13 )
+    {
+        xres = pVBInfo->StResInfo[ resindex ].HTotal ;
+        yres = pVBInfo->StResInfo[ resindex ].VTotal ;
+        modeflag = pVBInfo->SModeIDTable[ModeIdIndex].St_ModeFlag;    /* si+St_ResInfo */
+    }
+    else
+    {
+        xres = pVBInfo->ModeResInfo[ resindex ].HTotal ;                          /* xres->ax */
+        yres = pVBInfo->ModeResInfo[ resindex ].VTotal ;                          /* yres->bx */
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex].Ext_ModeFlag ; /* si+St_ModeFlag */
+    }
+
+    if (!( modeflag & Charx8Dot ))
+      xres = xres * 8 / 9;
+
+    LVDSHT = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHT;
+
+    LVDSHBS = xres + ( pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHDE - xres ) / 2 ;
+    if ( ( ModeNo<=0x13 ) && ( modeflag & HalfDCLK ) )
+    {
+      LVDSHBS -=  xres/4 ;
+    }
+    if (LVDSHBS > LVDSHT) LVDSHBS -= LVDSHT ;
+
+    LVDSHRS = LVDSHBS + pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHFP ;
+    if (LVDSHRS > LVDSHT) LVDSHRS -= LVDSHT ;
+
+    LVDSHRE = LVDSHRS + pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHSYNC ;
+    if (LVDSHRE > LVDSHT) LVDSHRE -= LVDSHT ;
+
+    LVDSHBE = LVDSHBS + LVDSHT - pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSHDE ;
+
+    LVDSVT = pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVT;
+
+    LVDSVBS = yres + ( pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVDE - yres ) / 2 ;
+    if ( ( ModeNo>0x13 ) && ( modeflag & DoubleScanMode ) )
+    {
+      LVDSVBS +=  yres/2 ;
+    }
+    if (LVDSVBS > LVDSVT) LVDSVBS -= LVDSVT ;
+
+    LVDSVRS = LVDSVBS + pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVFP ;
+    if (LVDSVRS > LVDSVT) LVDSVRS -= LVDSVT ;
+
+    LVDSVRE = LVDSVRS + pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVSYNC ;
+    if (LVDSVRE > LVDSVT) LVDSVRE -= LVDSVT ;
+
+    LVDSVBE = LVDSVBS + LVDSVT - pVBInfo->XG21_LVDSCapList[lvdstableindex].LVDSVDE ;
+
+    temp = ( UCHAR )XGINew_GetReg1( pVBInfo->P3d4 , 0x11 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x11 , temp & 0x7f ) ;              /* Unlock CRTC */
+
+    if (!( modeflag & Charx8Dot ))
+    {
+        XGINew_SetRegOR( pVBInfo->P3c4 , 0x1 , 0x1 ) ;
+    }
+
+    /* HT SR0B[1:0] CR00 */
+    value = ( LVDSHT >> 3 ) - 5;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0B , ~0x03 , ( value & 0x300 ) >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x0 , (value & 0xFF) ) ;
+
+    /* HBS SR0B[5:4] CR02 */
+    value = ( LVDSHBS >> 3 ) - 1;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0B , ~0x30 , ( value & 0x300 ) >> 4 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x2 , (value & 0xFF) ) ;
+
+    /* HBE SR0C[1:0] CR05[7] CR03[4:0] */
+    value = ( LVDSHBE >> 3 ) - 1;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0C , ~0x03 , ( value & 0xC0 ) >> 6 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x05 , ~0x80 , ( value & 0x20 ) << 2 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x03 , ~0x1F , value & 0x1F ) ;
+
+    /* HRS SR0B[7:6] CR04 */
+    value = ( LVDSHRS >> 3 ) + 2;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0B , ~0xC0 , ( value & 0x300 ) >> 2 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x4 , (value & 0xFF) ) ;
+
+    /* Panel HRS SR2F[1:0] SR2E[7:0]  */
+    value--;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x2F , ~0x03 , ( value & 0x300 ) >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x2E , (value & 0xFF) ) ;
+
+    /* HRE SR0C[2] CR05[4:0] */
+    value = ( LVDSHRE >> 3 ) + 2;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0C , ~0x04 , ( value & 0x20 ) >> 3 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x05 , ~0x1F , value & 0x1F ) ;
+
+    /* Panel HRE SR2F[7:2]  */
+    value--;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x2F , ~0xFC , value << 2 ) ;
+
+    /* VT SR0A[0] CR07[5][0] CR06 */
+    value = LVDSVT - 2 ;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0A , ~0x01 , ( value & 0x400 ) >> 10 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x07 , ~0x20 , ( value & 0x200 ) >> 4 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x07 , ~0x01 , ( value & 0x100 ) >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x06 , (value & 0xFF) ) ;
+
+    /* VBS SR0A[2] CR09[5] CR07[3] CR15 */
+    value = LVDSVBS - 1 ;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0A , ~0x04 , ( value & 0x400 ) >> 8 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x09 , ~0x20 , ( value & 0x200 ) >> 4 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x07 , ~0x08 , ( value & 0x100 ) >> 5 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x15 , (value & 0xFF) ) ;
+
+    /* VBE SR0A[4] CR16 */
+    value = LVDSVBE - 1;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0A , ~0x10 , ( value & 0x100 ) >> 4 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x16 , (value & 0xFF) ) ;
+
+    /* VRS SR0A[3] CR7[7][2] CR10 */
+    value = LVDSVRS - 1 ;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0A , ~0x08 , ( value & 0x400 ) >> 7 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x07 , ~0x80 , ( value & 0x200 ) >> 2 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x07 , ~0x04 , ( value & 0x100 ) >> 6 ) ;
+    XGINew_SetReg1( pVBInfo->P3d4 , 0x10 , (value & 0xFF) ) ;
+
+    /* Panel VRS SR35[2:0] SR34[7:0] */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x35 , ~0x07 , ( value & 0x700 ) >> 8 ) ;
+    XGINew_SetReg1( pVBInfo->P3c4 , 0x34 , value & 0xFF ) ;
+
+    /* VRE SR0A[5] CR11[3:0] */
+    value = LVDSVRE - 1;
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x0A , ~0x20 , ( value & 0x10 ) << 1 ) ;
+    XGINew_SetRegANDOR( pVBInfo->P3d4 , 0x11 , ~0x0F , value & 0x0F ) ;
+
+    /* Panel VRE SR3F[7:2] */
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x3F , ~0xFC , ( value << 2 ) & 0xFC ) ;
+
+    for ( temp=0, value = 0; temp < 3; temp++)
+    {
+
+        XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x31 , ~0x30 , value ) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2B , pVBInfo->XG21_LVDSCapList[lvdstableindex].VCLKData1) ;
+        XGINew_SetReg1( pVBInfo->P3c4 , 0x2C , pVBInfo->XG21_LVDSCapList[lvdstableindex].VCLKData2) ;
+        value += 0x10;
+    }
+
+    if (!( modeflag & Charx8Dot ))
+    {
+        XGINew_GetReg2( pVBInfo->P3da ) ;           /* reset 3da */
+        XGINew_SetReg3( pVBInfo->P3c0 , 0x13 ) ;    /* set index */
+        XGINew_SetReg3( pVBInfo->P3c0 , 0x00 ) ;    /* set data, panning = 0, shift left 1 dot*/
+
+        XGINew_GetReg2( pVBInfo->P3da ) ;           /* Enable Attribute */
+        XGINew_SetReg3( pVBInfo->P3c0 , 0x20 ) ;
+
+        XGINew_GetReg2( pVBInfo->P3da ) ;           /* reset 3da */
+    }
+
+
+}
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_IsLCDON */
+/* Input : */
+/* Output : FALSE : Skip PSC Control */
+/* TRUE: Disable PSC */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_IsLCDON(PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempax ;
+
+    tempax = pVBInfo->VBInfo ;
+    if ( tempax & SetCRT2ToDualEdge )
+        return FALSE ;
+    else if ( tempax & ( DisableCRT2Display | SwitchToCRT2 | SetSimuScanMode ) )
+        return TRUE ;
+
+    return FALSE ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_EnablePWD */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_EnablePWD(  PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT index ,
+           temp ;
+
+    index = XGI_GetLCDCapPtr(pVBInfo) ;
+    temp = pVBInfo->LCDCapList[ index ].PWD_2B ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x2B , temp ) ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x2C , pVBInfo->LCDCapList[ index ].PWD_2C ) ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x2D , pVBInfo->LCDCapList[ index ].PWD_2D ) ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x2E , pVBInfo->LCDCapList[ index ].PWD_2E ) ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x2F , pVBInfo->LCDCapList[ index ].PWD_2F ) ;
+    XGINew_SetRegOR( pVBInfo->Part4Port , 0x27 , 0x80 ) ;       /* enable PWD */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_DisablePWD */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_DisablePWD( PVB_DEVICE_INFO pVBInfo )
+{
+    XGINew_SetRegAND( pVBInfo->Part4Port , 0x27 , 0x7F ) ;      /* disable PWD */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_DisableChISLCD */
+/* Input : */
+/* Output : FALSE -> Not LCD Mode */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_DisableChISLCD(PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempbx ,
+           tempah ;
+
+    tempbx = pVBInfo->SetFlag & ( DisableChA | DisableChB ) ;
+    tempah = ~( ( USHORT )XGINew_GetReg1( pVBInfo->Part1Port  , 0x2E ) ) ;
+
+    if ( tempbx & ( EnableChA | DisableChA ) )
+    {
+        if ( !( tempah & 0x08 ) )               /* Chk LCDA Mode */
+            return FALSE ;
+    }
+
+    if ( !( tempbx & ( EnableChB | DisableChB ) ) )
+        return FALSE ;
+
+    if ( tempah & 0x01 )       /* Chk LCDB Mode */
+        return TRUE ;
+
+    return FALSE ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_EnableChISLCD */
+/* Input : */
+/* Output : 0 -> Not LCD mode */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_EnableChISLCD(PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempbx ,
+           tempah ;
+
+
+    tempbx = pVBInfo->SetFlag & ( EnableChA | EnableChB ) ;
+    tempah = ~( ( USHORT )XGINew_GetReg1( pVBInfo->Part1Port , 0x2E ) ) ;
+
+    if ( tempbx & ( EnableChA | DisableChA ) )
+    {
+        if ( !( tempah & 0x08 ) )               /* Chk LCDA Mode */
+            return FALSE ;
+    }
+
+    if ( !( tempbx & ( EnableChB | DisableChB ) ) )
+        return FALSE ;
+
+    if ( tempah & 0x01 )       /* Chk LCDB Mode */
+        return TRUE ;
+
+    return FALSE ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetLCDCapPtr */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+USHORT XGI_GetLCDCapPtr(  PVB_DEVICE_INFO pVBInfo )
+{
+    UCHAR tempal ,
+          tempah ,
+          tempbl ,
+          i ;
+
+    tempah = XGINew_GetReg1( pVBInfo->P3d4 , 0x36 ) ;
+    tempal = tempah & 0x0F ;
+    tempah = tempah & 0xF0 ;
+    i = 0 ;
+    tempbl =  pVBInfo->LCDCapList[ i ].LCD_ID ;
+
+    while( tempbl != 0xFF )
+    {
+        if ( tempbl & 0x80 )   /* OEMUtil */
+        {
+            tempal = tempah ;
+            tempbl = tempbl & ~( 0x80 ) ;
+        }
+
+        if ( tempal == tempbl )
+            break ;
+
+        i++ ;
+
+        tempbl = pVBInfo->LCDCapList[ i ].LCD_ID ;
+    }
+
+    return i ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetLCDCapPtr1 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+USHORT XGI_GetLCDCapPtr1( PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempah ,
+           tempal ,
+           tempbl ,
+           i ;
+
+    tempal = pVBInfo->LCDResInfo ;
+    tempah = pVBInfo->LCDTypeInfo ;
+
+    i = 0 ;
+    tempbl =  pVBInfo->LCDCapList[ i ].LCD_ID;
+
+    while( tempbl != 0xFF )
+    {
+        if ( ( tempbl & 0x80 ) && ( tempbl != 0x80 ) )
+        {
+            tempal = tempah ;
+            tempbl &= ~0x80 ;
+        }
+
+        if ( tempal == tempbl )
+            break ;
+
+        i++ ;
+        tempbl = pVBInfo->LCDCapList[ i ].LCD_ID ;
+    }
+
+    if ( tempbl == 0xFF )
+    {
+        pVBInfo->LCDResInfo = Panel1024x768 ;
+        pVBInfo->LCDTypeInfo = 0 ;
+        i = 0 ;
+    }
+
+    return i ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetLCDSync */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetLCDSync( USHORT* HSyncWidth , USHORT* VSyncWidth, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT Index ;
+
+    Index = XGI_GetLCDCapPtr(pVBInfo) ;
+    *HSyncWidth = pVBInfo->LCDCapList[ Index ].LCD_HSyncWidth ;
+    *VSyncWidth = pVBInfo->LCDCapList[ Index ].LCD_VSyncWidth ;
+
+    return ;
+}
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_EnableBridge */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_EnableBridge( PXGI_HW_DEVICE_INFO HwDeviceExtension , PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempbl ,
+           tempah ;
+
+    if ( pVBInfo->SetFlag == Win9xDOSMode )
+    {
+        if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+        {
+            XGI_DisplayOn( HwDeviceExtension, pVBInfo) ;
+            return ;
+        }
+        else  /* LVDS or CH7017 */
+            return ;
+    }
+
+
+    if ( HwDeviceExtension->jChipType < XG40 )
+    {
+        if ( !XGI_DisableChISLCD(pVBInfo) )
+        {
+            if ( ( XGI_EnableChISLCD(pVBInfo) ) || ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) ) )
+            {
+                if ( pVBInfo->LCDInfo & SetPWDEnable )
+                {
+                    XGI_EnablePWD( pVBInfo);
+                }
+                else
+                {
+                    pVBInfo->LCDInfo &= ( ~SetPWDEnable ) ;
+                    if ( pVBInfo->VBType & ( VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+                    {
+                        tempbl = 0xFD ;
+                        tempah = 0x02 ;
+                    }
+                    else
+                    {
+                        tempbl = 0xFB ;
+                        tempah = 0x00 ;
+                    }
+
+                    XGI_SetPanelPower( tempah , tempbl, pVBInfo ) ;
+                    XGI_SetPanelDelay( 1,pVBInfo ) ;
+                }
+            }
+        }
+    }   /* Not 340 */
+
+
+
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        if ( !( pVBInfo->SetFlag & DisableChA ) )
+        {
+            if ( pVBInfo->SetFlag & EnableChA )
+            {
+                XGINew_SetReg1( pVBInfo->Part1Port , 0x1E , 0x20 ) ;  /* Power on */
+            }
+            else
+            {
+                if ( pVBInfo->VBInfo & SetCRT2ToDualEdge ) /* SetCRT2ToLCDA ) */
+                {
+                    XGINew_SetReg1(pVBInfo->Part1Port,0x1E,0x20);  /* Power on */
+                }
+            }
+        }
+
+        if ( !( pVBInfo->SetFlag & DisableChB ) )
+        {
+            if ( ( pVBInfo->SetFlag & EnableChB ) || ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToTV | SetCRT2ToRAMDAC ) ) )
+            {
+                tempah = ( UCHAR )XGINew_GetReg1( pVBInfo->P3c4 , 0x32 ) ;
+                tempah &= 0xDF;
+                if ( pVBInfo->VBInfo & SetInSlaveMode )
+                {
+                    if ( !( pVBInfo->VBInfo & SetCRT2ToRAMDAC ) )
+                        tempah |= 0x20 ;
+                }
+                XGINew_SetReg1( pVBInfo->P3c4 , 0x32 , tempah ) ;
+                XGINew_SetRegOR( pVBInfo->P3c4 , 0x1E , 0x20 ) ;
+
+
+                tempah = ( UCHAR )XGINew_GetReg1( pVBInfo->Part1Port , 0x2E ) ;
+
+                if ( !( tempah & 0x80 ) )
+                    XGINew_SetRegOR( pVBInfo->Part1Port , 0x2E , 0x80 ) ;       /* BVBDOENABLE = 1 */
+
+                XGINew_SetRegAND( pVBInfo->Part1Port , 0x00 , 0x7F ) ;          /* BScreenOFF = 0 */
+            }
+        }
+
+        if ( ( pVBInfo->SetFlag & ( EnableChA | EnableChB ) ) || ( !( pVBInfo->VBInfo & DisableCRT2Display ) ) )
+        {
+            XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x00 , ~0xE0 , 0x20 ) ;   /* shampoo 0129 */
+            if ( pVBInfo->VBType & ( VB_XGI302LV | VB_XGI301C ) )
+            {
+                if ( !XGI_DisableChISLCD(pVBInfo) )
+                {
+                    if ( XGI_EnableChISLCD( pVBInfo) || ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) ) )
+                        XGINew_SetRegAND( pVBInfo->Part4Port ,0x2A , 0x7F ) ;   /* LVDS PLL power on */
+                }
+                XGINew_SetRegAND( pVBInfo->Part4Port , 0x30 , 0x7F ) ;          /* LVDS Driver power on */
+            }
+        }
+
+        tempah = 0x00 ;
+
+        if ( !( pVBInfo->VBInfo & DisableCRT2Display ) )
+        {
+            tempah = 0xc0 ;
+
+            if ( !( pVBInfo->VBInfo & SetSimuScanMode ) )
+            {
+                if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+                {
+                    if ( pVBInfo->VBInfo & SetCRT2ToDualEdge )
+                    {
+                        tempah = tempah & 0x40;
+                        if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+                            tempah = tempah ^ 0xC0 ;
+
+                        if ( pVBInfo->SetFlag & DisableChB )
+                            tempah &= 0xBF ;
+
+                        if ( pVBInfo->SetFlag & DisableChA )
+                            tempah &= 0x7F ;
+
+                        if ( pVBInfo->SetFlag & EnableChB )
+                            tempah |= 0x40 ;
+
+                        if ( pVBInfo->SetFlag & EnableChA )
+                            tempah |= 0x80 ;
+                    }
+                }
+            }
+        }
+
+        XGINew_SetRegOR( pVBInfo->Part4Port , 0x1F , tempah ) ;          /* EnablePart4_1F */
+
+        if ( pVBInfo->SetFlag & Win9xDOSMode )
+        {
+            XGI_DisplayOn( HwDeviceExtension, pVBInfo) ;
+            return ;
+        }
+
+        if ( !( pVBInfo->SetFlag & DisableChA ) )
+        {
+            XGI_VBLongWait( pVBInfo) ;
+            if ( !( pVBInfo->SetFlag & GatingCRT ) )
+            {
+                XGI_DisableGatingCRT( HwDeviceExtension, pVBInfo ) ;
+                XGI_DisplayOn( HwDeviceExtension, pVBInfo) ;
+                XGI_VBLongWait( pVBInfo) ;
+            }
+        }
+    }   /* 301 */
+    else        /* LVDS */
+    {
+        if ( pVBInfo->VBInfo & ( SetCRT2ToTV | SetCRT2ToLCD | SetCRT2ToLCDA ) )
+            XGINew_SetRegOR( pVBInfo->Part1Port , 0x1E , 0x20 ) ;       /* enable CRT2 */
+
+
+
+        tempah = ( UCHAR )XGINew_GetReg1( pVBInfo->Part1Port , 0x2E ) ;
+        if ( !( tempah & 0x80 ) )
+            XGINew_SetRegOR( pVBInfo->Part1Port , 0x2E , 0x80 ) ;       /* BVBDOENABLE = 1 */
+
+        XGINew_SetRegAND(pVBInfo->Part1Port,0x00,0x7F);
+        XGI_DisplayOn( HwDeviceExtension, pVBInfo);
+    } /* End of VB */
+
+
+    if ( HwDeviceExtension->jChipType < XG40 )
+    {
+        if ( !XGI_EnableChISLCD(pVBInfo) )
+        {
+            if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) )
+            {
+                if ( XGI_BacklightByDrv(pVBInfo) )
+                    return ;
+            }
+            else
+                return ;
+        }
+
+        if ( pVBInfo->LCDInfo & SetPWDEnable )
+        {
+            XGI_FirePWDEnable(pVBInfo) ;
+            return ;
+        }
+
+        XGI_SetPanelDelay( 2,pVBInfo ) ;
+
+        if ( pVBInfo->VBType & ( VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+        {
+            tempah = 0x01 ;
+            tempbl = 0xFE ;             /* turn on backlght */
+        }
+        else
+        {
+            tempbl = 0xF7 ;
+            tempah = 0x00 ;
+        }
+        XGI_SetPanelPower( tempah , tempbl , pVBInfo) ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_DisableBridge */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_DisableBridge(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempax ,
+           tempbx ,
+           tempah = 0 ,
+           tempbl = 0 ;
+
+    if ( pVBInfo->SetFlag == Win9xDOSMode )
+        return ;
+
+
+    if ( HwDeviceExtension->jChipType < XG40 )
+    {
+        if ( ( !( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) ) ) || ( XGI_DisableChISLCD(pVBInfo) ) )
+        {
+            if ( !XGI_IsLCDON(pVBInfo) )
+            {
+                if ( pVBInfo->LCDInfo & SetPWDEnable )
+                    XGI_EnablePWD( pVBInfo) ;
+                else
+                {
+                    pVBInfo->LCDInfo &= ~SetPWDEnable ;
+                    XGI_DisablePWD(pVBInfo) ;
+                    if ( pVBInfo->VBType & ( VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+                    {
+                        tempbx = 0xFE ;  /* not 01h */
+                        tempax = 0 ;
+                    }
+                    else
+                    {
+                        tempbx = 0xF7 ;  /* not 08h */
+                        tempax = 0x08 ;
+                    }
+                    XGI_SetPanelPower( tempax , tempbx , pVBInfo) ;
+                    XGI_SetPanelDelay( 3,pVBInfo ) ;
+                }
+            }   /* end if(!XGI_IsLCDON(pVBInfo)) */
+        }
+    }
+
+/*  if ( CH7017 )
+    {
+        if ( !( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2toLCDA ) ) || ( XGI_DisableChISLCD(pVBInfo) ) )
+        {
+            if ( !XGI_IsLCDON(pVBInfo) )
+            {
+                if ( DISCHARGE )
+                {
+                    tempbx = XGINew_GetCH7005( 0x61 ) ;
+                    if ( tempbx < 0x01 )   //first time we power up
+                        XGINew_SetCH7005( 0x0066 ) ;    //and disable power sequence
+                    else
+                        XGINew_SetCH7005( 0x5f66 ) ; //leave VDD on - disable power
+                }
+            }
+        }
+    }        */
+
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B| VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        tempah = 0x3F ;
+        if ( !( pVBInfo->VBInfo & ( DisableCRT2Display | SetSimuScanMode ) ) )
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+            {
+                if ( pVBInfo->VBInfo & SetCRT2ToDualEdge )
+                {
+                    tempah = 0x7F;                      /* Disable Channel A */
+                    if ( !( pVBInfo->VBInfo & SetCRT2ToLCDA ) )
+                        tempah = 0xBF ;         /* Disable Channel B */
+
+                    if ( pVBInfo->SetFlag & DisableChB )
+                        tempah &= 0xBF ;                /* force to disable Cahnnel */
+
+                    if ( pVBInfo->SetFlag & DisableChA )
+                        tempah &= 0x7F ;                /* Force to disable Channel B */
+                }
+            }
+        }
+
+        XGINew_SetRegAND( pVBInfo->Part4Port , 0x1F , tempah ) ;   /* disable part4_1f */
+
+        if ( pVBInfo->VBType & ( VB_XGI302LV | VB_XGI301C ) )
+        {
+            if ( ( ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) ) ) || ( XGI_DisableChISLCD(pVBInfo) ) || ( XGI_IsLCDON(pVBInfo) ) )
+                XGINew_SetRegOR( pVBInfo->Part4Port , 0x30 , 0x80 ) ;  /* LVDS Driver power down */
+        }
+
+        if ( ( pVBInfo->SetFlag & DisableChA ) || ( pVBInfo->VBInfo & ( DisableCRT2Display | SetCRT2ToLCDA | SetSimuScanMode ) ) )
+        {
+            if ( pVBInfo->SetFlag & GatingCRT )
+                XGI_EnableGatingCRT( HwDeviceExtension, pVBInfo ) ;
+            XGI_DisplayOff( HwDeviceExtension, pVBInfo) ;
+        }
+
+        if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+        {
+            if ( ( pVBInfo->SetFlag & DisableChA ) || ( pVBInfo->VBInfo & SetCRT2ToLCDA ) )
+                XGINew_SetRegAND( pVBInfo->Part1Port , 0x1e , 0xdf ) ;  /* Power down */
+        }
+
+        XGINew_SetRegAND( pVBInfo->P3c4 , 0x32 , 0xdf ) ;               /* disable TV as primary VGA swap */
+
+        if ( ( pVBInfo->VBInfo & ( SetSimuScanMode | SetCRT2ToDualEdge  ) ) )
+            XGINew_SetRegAND(pVBInfo->Part2Port,0x00,0xdf);
+
+        if ( ( pVBInfo->SetFlag & DisableChB ) || ( pVBInfo->VBInfo & ( DisableCRT2Display | SetSimuScanMode ) )
+        || ( ( !( pVBInfo->VBInfo & SetCRT2ToLCDA ) ) && ( pVBInfo->VBInfo & ( SetCRT2ToRAMDAC | SetCRT2ToLCD | SetCRT2ToTV ) ) ) )
+            XGINew_SetRegOR( pVBInfo->Part1Port , 0x00 , 0x80 ) ;       /* BScreenOff=1 */
+
+        if ( ( pVBInfo->SetFlag & DisableChB ) || ( pVBInfo->VBInfo & ( DisableCRT2Display | SetSimuScanMode ) )
+        || ( !( pVBInfo->VBInfo & SetCRT2ToLCDA ) ) || ( pVBInfo->VBInfo & ( SetCRT2ToRAMDAC | SetCRT2ToLCD | SetCRT2ToTV ) ) )
+        {
+            tempah= XGINew_GetReg1( pVBInfo->Part1Port , 0x00 ) ;       /* save Part1 index 0 */
+            XGINew_SetRegOR( pVBInfo->Part1Port , 0x00 , 0x10 ) ;       /* BTDAC = 1, avoid VB reset */
+            XGINew_SetRegAND( pVBInfo->Part1Port , 0x1E , 0xDF ) ;      /* disable CRT2 */
+            XGINew_SetReg1( pVBInfo->Part1Port , 0x00 , tempah ) ;      /* restore Part1 index 0 */
+        }
+    }
+    else /* {301} */
+    {
+        if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToTV ) )
+        {
+            XGINew_SetRegOR( pVBInfo->Part1Port , 0x00 , 0x80 ) ;       /* BScreenOff=1 */
+            XGINew_SetRegAND( pVBInfo->Part1Port , 0x1E , 0xDF ) ;      /* Disable CRT2 */
+            XGINew_SetRegAND( pVBInfo->P3c4 , 0x32 , 0xDF ) ;   /* Disable TV asPrimary VGA swap */
+        }
+
+        if ( pVBInfo->VBInfo & ( DisableCRT2Display | SetCRT2ToLCDA | SetSimuScanMode ) )
+            XGI_DisplayOff( HwDeviceExtension, pVBInfo) ;
+    }
+
+
+
+
+    if ( HwDeviceExtension->jChipType < XG40 )
+    {
+        if ( !( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) ) || ( XGI_DisableChISLCD(pVBInfo) ) || ( XGI_IsLCDON(pVBInfo) ) )
+        {
+            if ( pVBInfo->LCDInfo & SetPWDEnable )
+            {
+                if ( pVBInfo->LCDInfo & SetPWDEnable )
+                    XGI_BacklightByDrv(pVBInfo) ;
+                else
+                {
+                    XGI_SetPanelDelay( 4 ,pVBInfo) ;
+                    if ( pVBInfo->VBType & VB_XGI301LV )
+                    {
+                        tempbl = 0xFD ;
+                        tempah = 0x00 ;
+                    }
+                    else
+                    {
+                        tempbl = 0xFB ;
+                        tempah = 0x04 ;
+                    }
+                }
+            }
+            XGI_SetPanelPower( tempah , tempbl , pVBInfo) ;
+        }
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetTVPtrIndex */
+/* Input : */
+/* Output : */
+/* Description : bx 0 : ExtNTSC */
+/* 1 : StNTSC */
+/* 2 : ExtPAL */
+/* 3 : StPAL */
+/* 4 : ExtHiTV */
+/* 5 : StHiTV */
+/* 6 : Ext525i */
+/* 7 : St525i */
+/* 8 : Ext525p */
+/* 9 : St525p */
+/* A : Ext750p */
+/* B : St750p */
+/* --------------------------------------------------------------------- */
+USHORT XGI_GetTVPtrIndex(  PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempbx = 0 ;
+
+    if ( pVBInfo->TVInfo & SetPALTV )
+        tempbx = 2 ;
+    if ( pVBInfo->TVInfo & SetYPbPrMode1080i )
+        tempbx = 4 ;
+    if ( pVBInfo->TVInfo & SetYPbPrMode525i )
+        tempbx = 6 ;
+    if ( pVBInfo->TVInfo & SetYPbPrMode525p )
+        tempbx = 8 ;
+    if ( pVBInfo->TVInfo & SetYPbPrMode750p )
+        tempbx = 10 ;
+    if ( pVBInfo->TVInfo & TVSimuMode )
+        tempbx++ ;
+
+    return tempbx ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_OEM310Setting */
+/* Input : */
+/* Output : */
+/* Description : Customized Param. for 301 */
+/* --------------------------------------------------------------------- */
+void XGI_OEM310Setting( USHORT ModeNo , USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo)
+{
+    if ( pVBInfo->SetFlag & Win9xDOSMode )
+        return ;
+
+    /* GetPart1IO(); */
+    XGI_SetDelayComp(pVBInfo) ;
+
+    if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) )
+        XGI_SetLCDCap(pVBInfo) ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToTV )
+    {
+        /* GetPart2IO() */
+        XGI_SetPhaseIncr(pVBInfo) ;
+        XGI_SetYFilter( ModeNo , ModeIdIndex,pVBInfo ) ;
+        XGI_SetAntiFlicker( ModeNo , ModeIdIndex,pVBInfo ) ;
+
+        if ( pVBInfo->VBType&VB_XGI301)
+            XGI_SetEdgeEnhance( ModeNo , ModeIdIndex ,pVBInfo) ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetDelayComp */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetDelayComp( PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT index ;
+
+    UCHAR  tempah ,
+           tempbl ,
+           tempbh ;
+
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA | SetCRT2ToTV | SetCRT2ToRAMDAC ) )
+        {
+            tempbl = 0;
+            tempbh = 0;
+
+            index = XGI_GetTVPtrIndex(pVBInfo ) ;           /* Get TV Delay */
+            tempbl = pVBInfo->XGI_TVDelayList[ index ] ;
+
+            if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+                tempbl = pVBInfo->XGI_TVDelayList2[ index ] ;
+
+            if ( pVBInfo->VBInfo & SetCRT2ToDualEdge )
+                tempbl = tempbl >> 4 ;
+/*
+            if ( pVBInfo->VBInfo & SetCRT2ToRAMDAC )
+                tempbl = CRT2Delay1 ;                   // Get CRT2 Delay
+
+            if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+                tempbl = CRT2Delay2 ;
+*/
+            if ( pVBInfo->VBInfo & ( SetCRT2ToLCD | SetCRT2ToLCDA ) )
+            {
+                index = XGI_GetLCDCapPtr(pVBInfo) ;             /* Get LCD Delay */
+                tempbh=pVBInfo->LCDCapList[ index ].LCD_DelayCompensation ;
+
+                if ( !( pVBInfo->VBInfo & SetCRT2ToLCDA ) )
+                     tempbl = tempbh ;
+            }
+
+            tempbl  &= 0x0F ;
+            tempbh  &= 0xF0 ;
+            tempah = XGINew_GetReg1( pVBInfo->Part1Port , 0x2D ) ;
+
+            if ( pVBInfo->VBInfo & ( SetCRT2ToRAMDAC | SetCRT2ToLCD | SetCRT2ToTV ) )  /* Channel B */
+            {
+                tempah &= 0xF0 ;
+                tempah |= tempbl ;
+            }
+
+            if ( pVBInfo->VBInfo & SetCRT2ToLCDA )              /* Channel A */
+            {
+                tempah &= 0x0F ;
+                tempah |= tempbh ;
+            }
+            XGINew_SetReg1(pVBInfo->Part1Port,0x2D,tempah);
+        }
+    }
+    else if ( pVBInfo->IF_DEF_LVDS == 1 )
+    {
+        tempbl = 0;
+        tempbh = 0;
+        if ( pVBInfo->VBInfo & SetCRT2ToLCD )
+        {
+            tempah = pVBInfo->LCDCapList[ XGI_GetLCDCapPtr(pVBInfo) ].LCD_DelayCompensation ;           /* / Get LCD Delay */
+            tempah &= 0x0f ;
+            tempah = tempah << 4 ;
+            XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x2D , 0x0f , tempah ) ;
+        }
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetLCDCap */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetLCDCap( PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempcx ;
+
+    tempcx = pVBInfo->LCDCapList[ XGI_GetLCDCapPtr(pVBInfo) ].LCD_Capability ;
+
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        if ( pVBInfo->VBType & ( VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+        {                                       /* 301LV/302LV only */
+            /* Set 301LV Capability */
+            XGINew_SetReg1( pVBInfo->Part4Port , 0x24 , ( UCHAR )( tempcx & 0x1F ) ) ;
+        }
+        /* VB Driving */
+        XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x0D , ~( ( EnableVBCLKDRVLOW | EnablePLLSPLOW ) >> 8 ) , ( USHORT )( ( tempcx & ( EnableVBCLKDRVLOW | EnablePLLSPLOW ) ) >> 8 ) ) ;
+    }
+
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        if ( pVBInfo->VBInfo & SetCRT2ToLCD )
+            XGI_SetLCDCap_B( tempcx,pVBInfo ) ;
+        else if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+            XGI_SetLCDCap_A( tempcx,pVBInfo ) ;
+
+        if ( pVBInfo->VBType & ( VB_XGI302LV | VB_XGI301C ) )
+        {
+            if ( tempcx & EnableSpectrum )
+                SetSpectrum( pVBInfo) ;
+        }
+    }
+    else      /* LVDS,CH7017 */
+        XGI_SetLCDCap_A( tempcx, pVBInfo ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetLCDCap_A */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetLCDCap_A(USHORT tempcx,PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT temp ;
+
+    temp = XGINew_GetReg1( pVBInfo->P3d4 , 0x37 ) ;
+
+    if ( temp & LCDRGB18Bit )
+    {
+        XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x19 , 0x0F , ( USHORT )( 0x20 | ( tempcx & 0x00C0 ) ) ) ; /* Enable Dither */
+        XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x1A , 0x7F , 0x80 ) ;
+    }
+    else
+    {
+        XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x19 , 0x0F , ( USHORT )( 0x30 | ( tempcx & 0x00C0 ) ) ) ;
+        XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x1A , 0x7F , 0x00 ) ;
+    }
+
+/*
+    if ( tempcx & EnableLCD24bpp )      // 24bits
+    {
+        XGINew_SetRegANDOR(pVBInfo->Part1Port,0x19, 0x0F,(USHORT)(0x30|(tempcx&0x00C0)) );
+        XGINew_SetRegANDOR(pVBInfo->Part1Port,0x1A,0x7F,0x00);
+    }
+    else
+    {
+        XGINew_SetRegANDOR(pVBInfo->Part1Port,0x19, 0x0F,(USHORT)(0x20|(tempcx&0x00C0)) );//Enable Dither
+        XGINew_SetRegANDOR(pVBInfo->Part1Port,0x1A,0x7F,0x80);
+    }
+*/
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetLCDCap_B */
+/* Input : cx -> LCD Capability */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetLCDCap_B(USHORT tempcx,PVB_DEVICE_INFO pVBInfo)
+{
+    if ( tempcx & EnableLCD24bpp )      /* 24bits */
+        XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x1A , 0xE0 , ( USHORT )( ( ( tempcx & 0x00ff ) >> 6 ) | 0x0c ) ) ;
+    else
+        XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x1A , 0xE0 , ( USHORT )( ( ( tempcx & 0x00ff ) >> 6 ) | 0x18 ) ) ; /* Enable Dither */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : SetSpectrum */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void SetSpectrum( PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT index ;
+
+    index = XGI_GetLCDCapPtr(pVBInfo) ;
+
+    XGINew_SetRegAND( pVBInfo->Part4Port , 0x30 , 0x8F ) ;   /* disable down spectrum D[4] */
+    XGI_LongWait(pVBInfo) ;
+    XGINew_SetRegOR( pVBInfo->Part4Port , 0x30 , 0x20 ) ;        /* reset spectrum */
+    XGI_LongWait(pVBInfo) ;
+
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x31 , pVBInfo->LCDCapList[ index ].Spectrum_31 ) ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x32 , pVBInfo->LCDCapList[ index ].Spectrum_32 ) ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x33 , pVBInfo->LCDCapList[ index ].Spectrum_33 ) ;
+    XGINew_SetReg1( pVBInfo->Part4Port , 0x34 , pVBInfo->LCDCapList[ index ].Spectrum_34 ) ;
+    XGI_LongWait(pVBInfo) ;
+    XGINew_SetRegOR( pVBInfo->Part4Port , 0x30 , 0x40 ) ; /* enable spectrum */
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetAntiFlicker */
+/* Input : */
+/* Output : */
+/* Description : Set TV Customized Param. */
+/* --------------------------------------------------------------------- */
+void XGI_SetAntiFlicker( USHORT ModeNo , USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempbx ,
+           index ;
+
+    UCHAR tempah ;
+
+    if (pVBInfo->TVInfo & ( SetYPbPrMode525p | SetYPbPrMode750p ) )
+        return ;
+
+    tempbx = XGI_GetTVPtrIndex(pVBInfo ) ;
+    tempbx &= 0xFE ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        index = pVBInfo->SModeIDTable[ ModeIdIndex ].VB_StTVFlickerIndex ;
+    }
+    else
+    {
+        index = pVBInfo->EModeIDTable[ ModeIdIndex ].VB_ExtTVFlickerIndex ;
+    }
+
+    tempbx += index ;
+    tempah = TVAntiFlickList[ tempbx ] ;
+    tempah = tempah << 4 ;
+
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x0A , 0x8F , tempah ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetEdgeEnhance */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetEdgeEnhance( USHORT ModeNo , USHORT ModeIdIndex , PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempbx ,
+           index ;
+
+    UCHAR tempah ;
+
+
+    tempbx = XGI_GetTVPtrIndex(pVBInfo ) ;
+    tempbx &= 0xFE ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        index = pVBInfo->SModeIDTable[ ModeIdIndex ].VB_StTVEdgeIndex ;
+    }
+    else
+    {
+        index = pVBInfo->EModeIDTable[ ModeIdIndex ].VB_ExtTVEdgeIndex ;
+    }
+
+    tempbx += index ;
+    tempah = TVEdgeList[ tempbx ] ;
+    tempah = tempah << 5 ;
+
+    XGINew_SetRegANDOR( pVBInfo->Part2Port , 0x3A , 0x1F , tempah ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetPhaseIncr */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetPhaseIncr( PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempbx ;
+
+    UCHAR tempcl ,
+          tempch ;
+
+    ULONG tempData ;
+
+    XGI_GetTVPtrIndex2( &tempbx , &tempcl , &tempch, pVBInfo ) ; /* bx, cl, ch */
+    tempData = TVPhaseList[ tempbx ] ;
+
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x31 , ( USHORT )( tempData & 0x000000FF ) ) ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x32 , ( USHORT )( ( tempData & 0x0000FF00 ) >> 8 ) ) ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x33 , ( USHORT )( ( tempData & 0x00FF0000 ) >> 16 ) ) ;
+    XGINew_SetReg1( pVBInfo->Part2Port , 0x34 , ( USHORT )( ( tempData & 0xFF000000 ) >> 24 ) ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetYFilter */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_SetYFilter( USHORT ModeNo , USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempbx ,
+           index ;
+
+    UCHAR tempcl ,
+          tempch ,
+          tempal ,
+          *filterPtr ;
+
+    XGI_GetTVPtrIndex2( &tempbx , &tempcl , &tempch, pVBInfo ) ; /* bx, cl, ch */
+
+    switch( tempbx )
+    {
+        case 0x00:
+        case 0x04:
+            filterPtr = NTSCYFilter1 ;
+            break ;
+
+        case 0x01:
+            filterPtr = PALYFilter1 ;
+            break ;
+
+        case 0x02:
+        case 0x05:
+        case 0x0D:
+            filterPtr = PALMYFilter1 ;
+            break ;
+
+        case 0x03:
+            filterPtr = PALNYFilter1 ;
+            break ;
+
+        case 0x08:
+        case 0x0C:
+            filterPtr = NTSCYFilter2 ;
+            break ;
+
+        case 0x0A:
+            filterPtr = PALMYFilter2 ;
+            break ;
+
+        case 0x0B:
+            filterPtr = PALNYFilter2 ;
+            break ;
+
+        case 0x09:
+            filterPtr = PALYFilter2 ;
+            break ;
+
+        default:
+            return ;
+    }
+
+    if ( ModeNo <= 0x13 )
+        tempal = pVBInfo->SModeIDTable[ ModeIdIndex ].VB_StTVYFilterIndex ;
+    else
+        tempal = pVBInfo->EModeIDTable[ ModeIdIndex ].VB_ExtTVYFilterIndex ;
+
+    if ( tempcl == 0 )
+        index = tempal * 4;
+    else
+        index = tempal * 7;
+
+    if ( ( tempcl == 0 ) && ( tempch == 1 ) )
+    {
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x35 , 0 ) ;
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x36 , 0 ) ;
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x37 , 0 ) ;
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x38 , filterPtr[ index++ ] ) ;
+    }
+    else
+    {
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x35 , filterPtr[ index++ ] ) ;
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x36 , filterPtr[ index++ ] ) ;
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x37 , filterPtr[ index++ ] ) ;
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x38 , filterPtr[ index++ ] ) ;
+    }
+
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x48 , filterPtr[ index++ ] ) ;
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x49 , filterPtr[ index++ ] ) ;
+        XGINew_SetReg1( pVBInfo->Part2Port , 0x4A , filterPtr[ index++ ] ) ;
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetTVPtrIndex2 */
+/* Input : */
+/* Output : bx 0 : NTSC */
+/* 1 : PAL */
+/* 2 : PALM */
+/* 3 : PALN */
+/* 4 : NTSC1024x768 */
+/* 5 : PAL-M 1024x768 */
+/* 6-7: reserved */
+/* cl 0 : YFilter1 */
+/* 1 : YFilter2 */
+/* ch 0 : 301A */
+/* 1 : 301B/302B/301LV/302LV */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetTVPtrIndex2(USHORT* tempbx,UCHAR* tempcl,UCHAR* tempch, PVB_DEVICE_INFO pVBInfo)
+{
+    *tempbx = 0 ;
+    *tempcl = 0 ;
+    *tempch = 0 ;
+
+    if ( pVBInfo->TVInfo & SetPALTV )
+        *tempbx = 1 ;
+
+    if ( pVBInfo->TVInfo & SetPALMTV )
+        *tempbx = 2 ;
+
+    if ( pVBInfo->TVInfo & SetPALNTV )
+        *tempbx = 3 ;
+
+    if ( pVBInfo->TVInfo & NTSC1024x768 )
+    {
+        *tempbx = 4 ;
+        if ( pVBInfo->TVInfo & SetPALMTV )
+            *tempbx = 5 ;
+    }
+
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        if ( ( !( pVBInfo->VBInfo & SetInSlaveMode ) ) || ( pVBInfo->TVInfo & TVSimuMode ) )
+        {
+            *tempbx += 8 ;
+            *tempcl += 1 ;
+        }
+    }
+
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+        (*tempch)++ ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_SetCRT2ModeRegs */
+/* Input : */
+/* Output : */
+/* Description : Origin code for crt2group */
+/* --------------------------------------------------------------------- */
+void XGI_SetCRT2ModeRegs(USHORT ModeNo,PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempbl ;
+    SHORT  tempcl ;
+
+    UCHAR  tempah ;
+
+    /* XGINew_SetReg1( pVBInfo->Part1Port , 0x03 , 0x00 ) ; // fix write part1 index 0 BTDRAM bit Bug */
+    tempah=0;
+    if ( !( pVBInfo->VBInfo & DisableCRT2Display ) )
+    {
+        tempah=XGINew_GetReg1( pVBInfo->Part1Port , 0x00 ) ;
+        tempah &= ~0x10 ;       /* BTRAMDAC */
+        tempah |=  0x40 ;       /* BTRAM */
+
+        if ( pVBInfo->VBInfo & ( SetCRT2ToRAMDAC | SetCRT2ToTV | SetCRT2ToLCD ) )
+        {
+            tempah=0x40;        /* BTDRAM */
+            if ( ModeNo > 0x13 )
+            {
+                tempcl = pVBInfo->ModeType ;
+                tempcl -= ModeVGA ;
+                if ( tempcl >= 0 )
+                {
+                    tempah = ( 0x008 >> tempcl ) ;      /* BT Color */
+                    if ( tempah == 0 )
+                        tempah = 1 ;
+                    tempah |= 0x040 ;
+                }
+            }
+            if ( pVBInfo->VBInfo & SetInSlaveMode )
+                tempah ^= 0x50 ;        /* BTDAC */
+        }
+    }
+
+/*      0210 shampoo
+    if ( pVBInfo->VBInfo & DisableCRT2Display )
+    {
+        tempah = 0 ;
+    }
+
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x00 , tempah ) ;
+    if ( pVBInfo->VBInfo & ( SetCRT2ToRAMDAC | SetCRT2ToTV | SetCRT2ToLCD ) )
+    {
+        tempcl = pVBInfo->ModeType ;
+        if ( ModeNo > 0x13 )
+        {
+            tempcl -= ModeVGA ;
+            if ( ( tempcl > 0 ) || ( tempcl == 0 ) )
+            {
+                tempah=(0x008>>tempcl) ;
+                if ( tempah == 0 )
+                    tempah = 1 ;
+                tempah |= 0x040;
+            }
+        }
+        else
+        {
+            tempah = 0x040 ;
+        }
+
+        if ( pVBInfo->VBInfo & SetInSlaveMode )
+        {
+            tempah = ( tempah ^ 0x050 ) ;
+        }
+    }
+*/
+
+    XGINew_SetReg1( pVBInfo->Part1Port , 0x00 , tempah ) ;
+    tempah = 0x08 ;
+    tempbl = 0xf0 ;
+
+    if ( pVBInfo->VBInfo & DisableCRT2Display )
+        XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x2e , tempbl , tempah ) ;
+    else
+    {
+        tempah = 0x00 ;
+        tempbl = 0xff ;
+
+        if ( pVBInfo->VBInfo & ( SetCRT2ToRAMDAC | SetCRT2ToTV | SetCRT2ToLCD | SetCRT2ToLCDA ) )
+        {
+            if ( ( pVBInfo->VBInfo & SetCRT2ToLCDA ) && ( !( pVBInfo->VBInfo & SetSimuScanMode ) ) )
+            {
+                tempbl &= 0xf7 ;
+                tempah |= 0x01 ;
+                XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x2e , tempbl , tempah ) ;
+            }
+            else
+            {
+                if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+                {
+                    tempbl &= 0xf7 ;
+                    tempah |= 0x01 ;
+                }
+
+                if ( pVBInfo->VBInfo & ( SetCRT2ToRAMDAC | SetCRT2ToTV | SetCRT2ToLCD ) )
+                {
+                    tempbl &= 0xf8 ;
+                    tempah = 0x01 ;
+
+                    if ( !( pVBInfo->VBInfo & SetInSlaveMode ) )
+                        tempah |= 0x02 ;
+
+                    if ( !( pVBInfo->VBInfo & SetCRT2ToRAMDAC ) )
+                    {
+                        tempah = tempah ^ 0x05 ;
+                        if ( !( pVBInfo->VBInfo & SetCRT2ToLCD ) )
+                            tempah = tempah ^ 0x01 ;
+                    }
+
+                    if ( !( pVBInfo->VBInfo & SetCRT2ToDualEdge ) )
+                        tempah |= 0x08 ;
+                    XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x2e , tempbl , tempah ) ;
+                }
+                else
+                    XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x2e , tempbl , tempah ) ;
+            }
+        }
+        else
+            XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x2e , tempbl , tempah ) ;
+    }
+
+    if ( pVBInfo->VBInfo & ( SetCRT2ToRAMDAC | SetCRT2ToTV | SetCRT2ToLCD | SetCRT2ToLCDA ) )
+    {
+        tempah &= ( ~0x08 ) ;
+        if ( ( pVBInfo->ModeType == ModeVGA ) && ( !( pVBInfo->VBInfo & SetInSlaveMode ) ) )
+        {
+            tempah |= 0x010 ;
+        }
+        tempah |= 0x080 ;
+
+        if ( pVBInfo->VBInfo & SetCRT2ToTV )
+        {
+            /* if ( !( pVBInfo->TVInfo & ( SetYPbPrMode525p | SetYPbPrMode750p ) ) ) */
+            /* { */
+            tempah |= 0x020 ;
+            if ( ModeNo > 0x13 )
+            {
+                if ( pVBInfo->VBInfo & DriverMode )
+                    tempah = tempah ^ 0x20 ;
+            }
+        /* } */
+        }
+
+        XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x0D , ~0x0BF , tempah ) ;
+        tempah = 0 ;
+
+        if ( pVBInfo->LCDInfo & SetLCDDualLink )
+            tempah |= 0x40 ;
+
+        if ( pVBInfo->VBInfo & SetCRT2ToTV )
+        {
+            /* if ( ( !( pVBInfo->VBInfo & SetCRT2ToHiVisionTV ) ) && ( !( pVBInfo->TVInfo & ( SetYPbPrMode525p | SetYPbPrMode750p ) ) ) ) */
+            /* { */
+                if ( pVBInfo->TVInfo & RPLLDIV2XO )
+                    tempah |= 0x40 ;
+            /* } */
+        }
+
+        if ( ( pVBInfo->LCDResInfo == Panel1280x1024 ) || ( pVBInfo->LCDResInfo == Panel1280x1024x75 ) )
+            tempah |= 0x80 ;
+
+        if ( pVBInfo->LCDResInfo == Panel1280x960 )
+            tempah |= 0x80 ;
+
+        XGINew_SetReg1( pVBInfo->Part4Port , 0x0C , tempah ) ;
+    }
+
+    if ( pVBInfo->VBType & ( VB_XGI301B | VB_XGI302B | VB_XGI301LV | VB_XGI302LV | VB_XGI301C ) )
+    {
+        tempah = 0 ;
+        tempbl = 0xfb ;
+
+        if ( pVBInfo->VBInfo & SetCRT2ToDualEdge )
+        {
+            tempbl=0xff;
+            if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+                tempah |= 0x04 ; /* shampoo 0129 */
+        }
+
+        XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x13 , tempbl , tempah ) ;
+        tempah = 0x00 ;
+        tempbl = 0xcf ;
+        if ( !( pVBInfo->VBInfo & DisableCRT2Display ) )
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToDualEdge )
+                tempah |= 0x30 ;
+        }
+
+        XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x2c , tempbl , tempah ) ;
+        tempah = 0 ;
+        tempbl = 0x3f ;
+
+        if ( !( pVBInfo->VBInfo & DisableCRT2Display ) )
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToDualEdge )
+                tempah |= 0xc0 ;
+        }
+        XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x21 , tempbl , tempah ) ;
+    }
+
+    tempah = 0 ;
+    tempbl = 0x7f ;
+    if ( !( pVBInfo->VBInfo & SetCRT2ToLCDA ) )
+    {
+        tempbl = 0xff ;
+        if ( !( pVBInfo->VBInfo & SetCRT2ToDualEdge ) )
+            tempah |= 0x80 ;
+    }
+
+    XGINew_SetRegANDOR( pVBInfo->Part4Port , 0x23 , tempbl , tempah ) ;
+
+    if ( pVBInfo->VBType & ( VB_XGI302LV | VB_XGI301C ) )
+    {
+        if ( pVBInfo->LCDInfo & SetLCDDualLink )
+        {
+            XGINew_SetRegOR( pVBInfo->Part4Port , 0x27 , 0x20 ) ;
+            XGINew_SetRegOR( pVBInfo->Part4Port , 0x34 , 0x10 ) ;
+        }
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_CloseCRTC */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_CloseCRTC( PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempbx ;
+
+    tempbx = 0 ;
+
+    if ( pVBInfo->VBInfo & SetCRT2ToLCDA )
+        tempbx = 0x08A0 ;
+
+
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_OpenCRTC */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_OpenCRTC( PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempbx ;
+
+    tempbx = 0 ;
+
+
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetRAMDAC2DATA */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_GetRAMDAC2DATA(USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex, PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempax ,
+           tempbx ,
+           temp1 ,
+           temp2 ,
+           modeflag = 0 ,
+           tempcx ,
+           StandTableIndex ,
+           CRT1Index ;
+
+    pVBInfo->RVBHCMAX = 1 ;
+    pVBInfo->RVBHCFACT = 1 ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+        StandTableIndex = XGI_GetModePtr( ModeNo , ModeIdIndex, pVBInfo ) ;
+        tempax = pVBInfo->StandTable[ StandTableIndex ].CRTC[ 0 ] ;
+        tempbx = pVBInfo->StandTable[StandTableIndex ].CRTC[ 6 ] ;
+        temp1 = pVBInfo->StandTable[ StandTableIndex ].CRTC[ 7 ] ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+        CRT1Index = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT1CRTC ;
+        CRT1Index &= IndexMask ;
+        temp1 = ( USHORT )pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 0 ] ;
+        temp2 = ( USHORT )pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 5 ] ;
+        tempax = ( temp1 & 0xFF ) | ( ( temp2 & 0x03 ) << 8 ) ;
+        tempbx = ( USHORT )pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 8 ] ;
+        tempcx = ( USHORT )pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 14 ] << 8 ;
+        tempcx &= 0x0100 ;
+        tempcx = tempcx << 2 ;
+        tempbx |= tempcx;
+        temp1 = ( USHORT )pVBInfo->XGINEWUB_CRT1Table[ CRT1Index ].CR[ 9 ] ;
+    }
+
+    if ( temp1 & 0x01 )
+        tempbx |= 0x0100 ;
+
+    if ( temp1 & 0x20 )
+        tempbx |= 0x0200 ;
+    tempax += 5 ;
+
+    if ( modeflag & Charx8Dot )
+        tempax *= 8 ;
+    else
+        tempax *= 9 ;
+
+    pVBInfo->VGAHT = tempax ;
+    pVBInfo->HT = tempax ;
+    tempbx++ ;
+    pVBInfo->VGAVT = tempbx ;
+    pVBInfo->VT = tempbx ;
+}
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetColorDepth */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+USHORT XGI_GetColorDepth(USHORT ModeNo , USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT ColorDepth[ 6 ] = { 1 , 2 , 4 , 4 , 6 , 8 } ;
+    SHORT  index ;
+    USHORT modeflag ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;
+    }
+
+    index=(modeflag&ModeInfoFlag)-ModeEGA;
+
+    if ( index < 0 )
+        index = 0 ;
+
+    return( ColorDepth[ index ] ) ;
+}
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_UnLockCRT2 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_UnLockCRT2( PXGI_HW_DEVICE_INFO HwDeviceExtension,  PVB_DEVICE_INFO pVBInfo )
+{
+
+    XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x2f , 0xFF , 0x01 ) ;
+
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_LockCRT2 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_LockCRT2( PXGI_HW_DEVICE_INFO HwDeviceExtension,  PVB_DEVICE_INFO pVBInfo )
+{
+
+    XGINew_SetRegANDOR( pVBInfo->Part1Port , 0x2F , 0xFE , 0x00 ) ;
+
+
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_EnableCRT2 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_EnableCRT2( PVB_DEVICE_INFO pVBInfo)
+{
+    XGINew_SetRegANDOR( pVBInfo->P3c4 , 0x1E , 0xFF , 0x20 ) ;
+}
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_LCD_Wait_Time(UCHAR DelayTime, PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT i ,
+           j ;
+
+    ULONG temp ,
+          flag ;
+
+    flag = 0 ;
+//printk("XGINew_LCD_Wait_Time");
+//return;
+    for( i = 0 ; i < DelayTime ; i++ )
+    {
+        for( j = 0 ; j < 66 ; j++ )
+        {
+
+            temp =  XGINew_GetReg3( 0x61 ) ;
+
+            //temp &= 0x10000000;
+            temp &= 0x10;
+            if ( temp == flag )
+                continue ;
+
+            flag = temp ;
+        }
+    }
+}
+
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_BridgeIsOn */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+BOOLEAN XGI_BridgeIsOn( PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT flag ;
+
+    if ( pVBInfo->IF_DEF_LVDS == 1 )
+    {
+        return( 1 ) ;
+    }
+    else
+    {
+        flag = XGINew_GetReg1( pVBInfo->Part4Port , 0x00 ) ;
+        if ( ( flag == 1 ) || ( flag == 2 ) )
+            return( 1 ) ;       /* 301b */
+        else
+            return( 0 ) ;
+    }
+}
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_LongWait */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_LongWait(PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT i ;
+
+    i = XGINew_GetReg1( pVBInfo->P3c4 , 0x1F ) ;
+
+    if ( !( i & 0xC0 ) )
+    {
+        for( i = 0 ; i < 0xFFFF ; i++ )
+        {
+            if ( !( XGINew_GetReg2( pVBInfo->P3da ) & 0x08 ) )
+                break ;
+        }
+
+        for( i = 0 ; i < 0xFFFF ; i++ )
+        {
+            if ( ( XGINew_GetReg2( pVBInfo->P3da ) & 0x08 ) )
+                break ;
+        }
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_VBLongWait */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGI_VBLongWait( PVB_DEVICE_INFO pVBInfo )
+{
+    USHORT tempal ,
+           temp ,
+           i ,
+           j ;
+return ;
+    if ( !( pVBInfo->VBInfo & SetCRT2ToTV ) )
+    {
+        temp = 0 ;
+        for( i = 0 ; i < 3 ; i++ )
+        {
+            for( j = 0 ; j < 100 ; j++ )
+            {
+                tempal = XGINew_GetReg2( pVBInfo->P3da ) ;
+                if ( temp & 0x01 )
+                {                       /* VBWaitMode2 */
+                    if ( ( tempal & 0x08 ) )
+                    {
+                        continue ;
+                    }
+
+                    if ( !( tempal & 0x08 ) )
+                    {
+                        break ;
+                    }
+                }
+                else
+                {                       /* VBWaitMode1 */
+                    if ( !( tempal & 0x08 ) )
+                    {
+                        continue ;
+                    }
+
+                    if ( ( tempal & 0x08 ) )
+                    {
+                        break ;
+                    }
+                }
+            }
+            temp = temp ^ 0x01 ;
+        }
+    }
+    else
+    {
+        XGI_LongWait(pVBInfo) ;
+    }
+    return ;
+}
+
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetVGAHT2 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+USHORT XGI_GetVGAHT2( PVB_DEVICE_INFO pVBInfo )
+{
+    ULONG tempax ,
+          tempbx ;
+
+    tempbx = ( ( pVBInfo->VGAVT - pVBInfo->VGAVDE ) * pVBInfo->RVBHCMAX ) & 0xFFFF ;
+    tempax = ( pVBInfo->VT - pVBInfo->VDE ) * pVBInfo->RVBHCFACT ;
+    tempax = ( tempax * pVBInfo->HT ) /tempbx ;
+
+    return( ( USHORT )tempax ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGI_GetVCLK2Ptr */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+USHORT XGI_GetVCLK2Ptr( USHORT ModeNo , USHORT ModeIdIndex , USHORT RefreshRateTableIndex , PXGI_HW_DEVICE_INFO HwDeviceExtension ,PVB_DEVICE_INFO pVBInfo)
+{
+    USHORT tempbx ;
+
+    USHORT LCDXlat1VCLK[ 4 ] = { VCLK65 + 2 , VCLK65 + 2 , VCLK65 + 2 , VCLK65 + 2 } ;
+    USHORT LCDXlat2VCLK[ 4 ] = { VCLK108_2 + 5 , VCLK108_2 + 5 , VCLK108_2 + 5 , VCLK108_2 + 5 } ;
+    USHORT LVDSXlat1VCLK[ 4 ] = { VCLK40 , VCLK40 , VCLK40 , VCLK40 } ;
+    USHORT LVDSXlat2VCLK[ 4 ] = { VCLK65 + 2 , VCLK65 + 2 , VCLK65 + 2 , VCLK65 + 2 } ;
+    USHORT LVDSXlat3VCLK[ 4 ] = { VCLK65 + 2 , VCLK65 + 2 , VCLK65 + 2 , VCLK65 + 2 } ;
+
+    USHORT CRT2Index , VCLKIndex ;
+    USHORT modeflag , resinfo ;
+    UCHAR *CHTVVCLKPtr = NULL ;
+
+    if ( ModeNo <= 0x13 )
+    {
+        modeflag = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ModeFlag ;      /* si+St_ResInfo */
+        resinfo = pVBInfo->SModeIDTable[ ModeIdIndex ].St_ResInfo ;
+        CRT2Index = pVBInfo->SModeIDTable[ ModeIdIndex ].St_CRT2CRTC ;
+    }
+    else
+    {
+        modeflag = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_ModeFlag ;     /* si+Ext_ResInfo */
+        resinfo = pVBInfo->EModeIDTable[ ModeIdIndex ].Ext_RESINFO ;
+        CRT2Index = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRT2CRTC ;
+    }
+
+    if ( pVBInfo->IF_DEF_LVDS == 0 )
+    {
+        CRT2Index = CRT2Index >> 6 ;        /*  for LCD */
+        if ( ( ( pVBInfo->VBInfo & SetCRT2ToLCD ) | SetCRT2ToLCDA ) )    /*301b*/
+        {
+            if ( pVBInfo->LCDResInfo != Panel1024x768 )
+            {
+                VCLKIndex = LCDXlat2VCLK[ CRT2Index ] ;
+            }
+            else
+            {
+                 VCLKIndex = LCDXlat1VCLK[ CRT2Index ] ;
+            }
+        }
+        else    /* for TV */
+        {
+            if ( pVBInfo->VBInfo & SetCRT2ToTV )
+            {
+                if ( pVBInfo->VBInfo & SetCRT2ToHiVisionTV )
+                {
+                    if ( pVBInfo->SetFlag & RPLLDIV2XO )
+                    {
+                        VCLKIndex = HiTVVCLKDIV2 ;
+
+
+                            VCLKIndex += 25 ;
+
+                    }
+                    else
+                    {
+                        VCLKIndex = HiTVVCLK ;
+
+
+                            VCLKIndex += 25 ;
+
+                    }
+
+                    if ( pVBInfo->SetFlag & TVSimuMode )
+                    {
+                        if( modeflag & Charx8Dot )
+                        {
+                            VCLKIndex = HiTVSimuVCLK ;
+
+
+                                VCLKIndex += 25 ;
+
+                        }
+                        else
+                        {
+                            VCLKIndex = HiTVTextVCLK ;
+
+
+                                VCLKIndex += 25 ;
+
+                        }
+                    }
+
+                    if ( pVBInfo->VBType & VB_XGI301LV )        /* 301lv */
+                    {
+                        if ( !( pVBInfo->VBExtInfo == VB_YPbPr1080i ) )
+                        {
+                            VCLKIndex = YPbPr750pVCLK ;
+                            if ( !( pVBInfo->VBExtInfo == VB_YPbPr750p ) )
+                            {
+                                VCLKIndex = YPbPr525pVCLK ;
+                                if ( !( pVBInfo->VBExtInfo == VB_YPbPr525p ) )
+                                {
+                                    VCLKIndex = YPbPr525iVCLK_2 ;
+                                    if ( !( pVBInfo->SetFlag & RPLLDIV2XO ) )
+                                        VCLKIndex = YPbPr525iVCLK ;
+                                }
+                            }
+                        }
+                    }
+                }
+                else
+                {
+                    if ( pVBInfo->VBInfo & SetCRT2ToTV )
+                    {
+                        if ( pVBInfo->SetFlag & RPLLDIV2XO )
+                        {
+                            VCLKIndex = TVVCLKDIV2 ;
+
+
+                            VCLKIndex += 25 ;
+
+                        }
+                        else
+                        {
+                            VCLKIndex = TVVCLK ;
+
+
+                            VCLKIndex += 25 ;
+
+                        }
+                    }
+                }
+            }
+            else
+            {   /* for CRT2 */
+                VCLKIndex = ( UCHAR )XGINew_GetReg2( ( pVBInfo->P3ca + 0x02 ) ) ;       /* Port 3cch */
+                VCLKIndex = ( ( VCLKIndex >> 2 ) & 0x03 ) ;
+                if ( ModeNo > 0x13 )
+                {
+                    VCLKIndex = pVBInfo->RefIndex[ RefreshRateTableIndex ].Ext_CRTVCLK ;        /* di+Ext_CRTVCLK */
+                    VCLKIndex &= IndexMask ;
+                }
+            }
+        }
+    }
+    else
+    {           /* LVDS */
+        if ( ModeNo <= 0x13 )
+            VCLKIndex = CRT2Index ;
+        else
+            VCLKIndex = CRT2Index ;
+
+        if ( pVBInfo->IF_DEF_CH7005 == 1 )
+        {
+            if ( !( pVBInfo->VBInfo & SetCRT2ToLCD ) )
+            {
+                VCLKIndex &= 0x1f ;
+                tempbx = 0 ;
+
+                if ( pVBInfo->VBInfo & SetPALTV )
+                    tempbx += 2 ;
+
+                if ( pVBInfo->VBInfo & SetCHTVOverScan )
+                    tempbx += 1 ;
+
+                switch( tempbx )
+                {
+                    case 0:
+                        CHTVVCLKPtr = pVBInfo->CHTVVCLKUNTSC ;
+                        break ;
+                    case 1:
+                        CHTVVCLKPtr = pVBInfo->CHTVVCLKONTSC ;
+                        break;
+                    case 2:
+                        CHTVVCLKPtr = pVBInfo->CHTVVCLKUPAL ;
+                        break ;
+                    case 3:
+                        CHTVVCLKPtr = pVBInfo->CHTVVCLKOPAL ;
+                        break ;
+                    default:
+                        break ;
+                }
+
+                VCLKIndex = CHTVVCLKPtr[ VCLKIndex ] ;
+            }
+        }
+        else
+        {
+            VCLKIndex = VCLKIndex >> 6 ;
+            if ( ( pVBInfo->LCDResInfo == Panel800x600 ) || ( pVBInfo->LCDResInfo == Panel320x480 ) )
+                VCLKIndex = LVDSXlat1VCLK[ VCLKIndex ] ;
+            else if ( ( pVBInfo->LCDResInfo == Panel1024x768 ) || ( pVBInfo->LCDResInfo == Panel1024x768x75 ) )
+                VCLKIndex = LVDSXlat2VCLK[ VCLKIndex ] ;
+            else
+                VCLKIndex = LVDSXlat3VCLK[ VCLKIndex ] ;
+        }
+    }
+    /* VCLKIndex = VCLKIndex&IndexMask ; */
+
+
+
+    return( VCLKIndex ) ;
+}
+
diff --git a/drivers/staging/xgifb/vb_setmode.h b/drivers/staging/xgifb/vb_setmode.h
new file mode 100644
index 000000000000..09753d706665
--- /dev/null
+++ b/drivers/staging/xgifb/vb_setmode.h
@@ -0,0 +1,40 @@
+#ifndef  _VBSETMODE_
+#define  _VBSETMODE_
+
+extern   void     InitTo330Pointer(UCHAR,PVB_DEVICE_INFO);
+extern   void     XGI_UnLockCRT2(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO );
+extern   void     XGI_LockCRT2(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO );
+extern   void     XGI_LongWait( PVB_DEVICE_INFO );
+extern   void     XGI_SetCRT2ModeRegs(USHORT ModeNo,PXGI_HW_DEVICE_INFO,  PVB_DEVICE_INFO  );
+extern   void     XGI_DisableBridge(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO );
+extern   void     XGI_EnableBridge(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO );
+extern   void     XGI_DisplayOff( PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO );
+extern   void     XGI_DisplayOn( PXGI_HW_DEVICE_INFO, PVB_DEVICE_INFO );
+extern   void     XGI_GetVBType(PVB_DEVICE_INFO);
+extern   void     XGI_SenseCRT1(PVB_DEVICE_INFO );
+extern   void     XGI_GetVGAType(PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO );
+extern   void     XGI_GetVBInfo(USHORT ModeNo,USHORT ModeIdIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO );
+extern   void     XGI_GetTVInfo(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO );
+extern   void     XGI_SetCRT1Offset(USHORT ModeNo,USHORT ModeIdIndex,USHORT RefreshRateTableIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO );
+extern   void     XGI_SetLCDAGroup(USHORT ModeNo,USHORT ModeIdIndex,PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO );
+extern   void     XGI_WaitDisply( PVB_DEVICE_INFO );
+extern   USHORT   XGI_GetResInfo(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo);
+
+extern   BOOLEAN  XGISetModeNew( PXGI_HW_DEVICE_INFO HwDeviceExtension , USHORT ModeNo ) ;
+
+extern   BOOLEAN  XGI_SearchModeID( USHORT ModeNo,USHORT  *ModeIdIndex, PVB_DEVICE_INFO );
+extern   BOOLEAN  XGI_GetLCDInfo(USHORT ModeNo,USHORT ModeIdIndex,PVB_DEVICE_INFO );
+extern   BOOLEAN  XGI_BridgeIsOn( PVB_DEVICE_INFO );
+extern   BOOLEAN  XGI_SetCRT2Group301(USHORT ModeNo, PXGI_HW_DEVICE_INFO HwDeviceExtension, PVB_DEVICE_INFO);
+extern   USHORT   XGI_GetRatePtrCRT2( PXGI_HW_DEVICE_INFO pXGIHWDE, USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO );
+
+extern   void     XGI_SetXG21FPBits(PVB_DEVICE_INFO pVBInfo);
+extern   void     XGI_SetXG27FPBits(PVB_DEVICE_INFO pVBInfo);
+extern   void     XGI_XG21BLSignalVDD(USHORT tempbh,USHORT tempbl, PVB_DEVICE_INFO pVBInfo);
+extern   void     XGI_XG27BLSignalVDD(USHORT tempbh,USHORT tempbl, PVB_DEVICE_INFO pVBInfo);
+extern   void     XGI_XG21SetPanelDelay(USHORT tempbl, PVB_DEVICE_INFO pVBInfo);
+extern   BOOLEAN  XGI_XG21CheckLVDSMode(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo );
+extern   void     XGI_SetXG21LVDSPara(USHORT ModeNo,USHORT ModeIdIndex, PVB_DEVICE_INFO pVBInfo );
+extern   USHORT XGI_GetLVDSOEMTableIndex(PVB_DEVICE_INFO pVBInfo);
+
+#endif
diff --git a/drivers/staging/xgifb/vb_struct.h b/drivers/staging/xgifb/vb_struct.h
new file mode 100644
index 000000000000..bb25c0e2785e
--- /dev/null
+++ b/drivers/staging/xgifb/vb_struct.h
@@ -0,0 +1,534 @@
+#ifndef _VB_STRUCT_
+#define _VB_STRUCT_
+
+#ifdef _INITNEW_
+#define EXTERN
+#else
+#define EXTERN extern
+#endif
+
+
+
+
+typedef struct _XGI_PanelDelayTblStruct
+{
+ UCHAR timer[2];
+} XGI_PanelDelayTblStruct;
+
+typedef struct _XGI_LCDDataStruct
+{
+ USHORT RVBHCMAX;
+ USHORT RVBHCFACT;
+ USHORT VGAHT;
+ USHORT VGAVT;
+ USHORT LCDHT;
+ USHORT LCDVT;
+} XGI_LCDDataStruct;
+
+
+typedef struct _XGI_LVDSCRT1HDataStruct
+{
+ UCHAR Reg[8];
+} XGI_LVDSCRT1HDataStruct;
+typedef struct _XGI_LVDSCRT1VDataStruct
+{
+ UCHAR Reg[7];
+} XGI_LVDSCRT1VDataStruct;
+
+
+typedef struct _XGI_TVDataStruct
+{
+ USHORT RVBHCMAX;
+ USHORT RVBHCFACT;
+ USHORT VGAHT;
+ USHORT VGAVT;
+ USHORT TVHDE;
+ USHORT TVVDE;
+ USHORT RVBHRS;
+ UCHAR FlickerMode;
+ USHORT HALFRVBHRS;
+ UCHAR RY1COE;
+ UCHAR RY2COE;
+ UCHAR RY3COE;
+ UCHAR RY4COE;
+} XGI_TVDataStruct;
+
+typedef struct _XGI_LVDSDataStruct
+{
+ USHORT VGAHT;
+ USHORT VGAVT;
+ USHORT LCDHT;
+ USHORT LCDVT;
+} XGI_LVDSDataStruct;
+
+typedef struct _XGI_LVDSDesStruct
+{
+ USHORT LCDHDES;
+ USHORT LCDVDES;
+} XGI_LVDSDesStruct;
+
+typedef struct _XGI_LVDSCRT1DataStruct
+{
+ UCHAR CR[15];
+} XGI_LVDSCRT1DataStruct;
+
+/*add for LCDA*/
+
+
+typedef struct _XGI_StStruct
+{
+ UCHAR St_ModeID;
+ USHORT St_ModeFlag;
+ UCHAR St_StTableIndex;
+ UCHAR St_CRT2CRTC;
+ UCHAR St_CRT2CRTC2;
+ UCHAR St_ResInfo;
+ UCHAR VB_StTVFlickerIndex;
+ UCHAR VB_StTVEdgeIndex;
+ UCHAR VB_StTVYFilterIndex;
+} XGI_StStruct;
+
+typedef struct _XGI_StandTableStruct
+{
+ UCHAR CRT_COLS;
+ UCHAR ROWS;
+ UCHAR CHAR_HEIGHT;
+ USHORT CRT_LEN;
+ UCHAR SR[4];
+ UCHAR MISC;
+ UCHAR CRTC[0x19];
+ UCHAR ATTR[0x14];
+ UCHAR GRC[9];
+} XGI_StandTableStruct;
+
+typedef struct _XGI_ExtStruct
+{
+ UCHAR Ext_ModeID;
+ USHORT Ext_ModeFlag;
+ USHORT Ext_ModeInfo;
+ USHORT Ext_Point;
+ USHORT Ext_VESAID;
+ UCHAR Ext_VESAMEMSize;
+ UCHAR Ext_RESINFO;
+ UCHAR VB_ExtTVFlickerIndex;
+ UCHAR VB_ExtTVEdgeIndex;
+ UCHAR VB_ExtTVYFilterIndex;
+ UCHAR REFindex;
+} XGI_ExtStruct;
+
+typedef struct _XGI_Ext2Struct
+{
+ USHORT Ext_InfoFlag;
+ UCHAR Ext_CRT1CRTC;
+ UCHAR Ext_CRTVCLK;
+ UCHAR Ext_CRT2CRTC;
+ UCHAR Ext_CRT2CRTC2;
+ UCHAR  ModeID;
+ USHORT XRes;
+ USHORT YRes;
+ /* USHORT ROM_OFFSET; */
+} XGI_Ext2Struct;
+
+
+typedef struct _XGI_MCLKDataStruct
+{
+ UCHAR SR28,SR29,SR2A;
+ USHORT CLOCK;
+} XGI_MCLKDataStruct;
+
+typedef struct _XGI_ECLKDataStruct
+{
+ UCHAR SR2E,SR2F,SR30;
+ USHORT CLOCK;
+} XGI_ECLKDataStruct;
+
+typedef struct _XGI_VCLKDataStruct
+{
+ UCHAR SR2B,SR2C;
+ USHORT CLOCK;
+} XGI_VCLKDataStruct;
+
+typedef struct _XGI_VBVCLKDataStruct
+{
+ UCHAR Part4_A,Part4_B;
+ USHORT CLOCK;
+} XGI_VBVCLKDataStruct;
+
+typedef struct _XGI_StResInfoStruct
+{
+ USHORT HTotal;
+ USHORT VTotal;
+} XGI_StResInfoStruct;
+
+typedef struct _XGI_ModeResInfoStruct
+{
+ USHORT HTotal;
+ USHORT VTotal;
+ UCHAR  XChar;
+ UCHAR  YChar;
+} XGI_ModeResInfoStruct;
+
+typedef struct _XGI_LCDNBDesStruct
+{
+  UCHAR NB[12];
+} XGI_LCDNBDesStruct;
+ /*add for new UNIVGABIOS*/
+typedef struct _XGI_LCDDesStruct
+{
+ USHORT LCDHDES;
+ USHORT LCDHRS;
+ USHORT LCDVDES;
+ USHORT LCDVRS;
+} XGI_LCDDesStruct;
+
+typedef struct _XGI_LCDDataTablStruct
+{
+ UCHAR  PANELID;
+ USHORT MASK;
+ USHORT CAP;
+ USHORT DATAPTR;
+} XGI_LCDDataTablStruct;
+
+typedef struct _XGI_TVTablDataStruct
+{
+ USHORT MASK;
+ USHORT CAP;
+ USHORT DATAPTR;
+} XGI_TVDataTablStruct;
+
+typedef struct _XGI330_LCDDesDataStruct
+{
+ USHORT LCDHDES;
+ USHORT LCDHRS;
+ USHORT LCDVDES;
+ USHORT LCDVRS;
+} XGI330_LCDDataDesStruct;
+
+
+typedef struct _XGI330_LVDSDataStruct
+{
+ USHORT VGAHT;
+ USHORT VGAVT;
+ USHORT LCDHT;
+ USHORT LCDVT;
+} XGI330_LVDSDataStruct;
+
+typedef struct _XGI330_LCDDesDataStruct2
+{
+ USHORT LCDHDES;
+ USHORT LCDHRS;
+ USHORT LCDVDES;
+ USHORT LCDVRS;
+ USHORT LCDHSync;
+ USHORT LCDVSync;
+} XGI330_LCDDataDesStruct2;
+
+typedef struct _XGI330_LCDDataStruct
+{
+ USHORT RVBHCMAX;
+ USHORT RVBHCFACT;
+ USHORT VGAHT;
+ USHORT VGAVT;
+ USHORT LCDHT;
+ USHORT LCDVT;
+} XGI330_LCDDataStruct;
+
+
+typedef struct _XGI330_TVDataStruct
+{
+ USHORT RVBHCMAX;
+ USHORT RVBHCFACT;
+ USHORT VGAHT;
+ USHORT VGAVT;
+ USHORT TVHDE;
+ USHORT TVVDE;
+ USHORT RVBHRS;
+ UCHAR FlickerMode;
+ USHORT HALFRVBHRS;
+} XGI330_TVDataStruct;
+
+typedef struct _XGI330_LCDDataTablStruct
+{
+ UCHAR  PANELID;
+ USHORT MASK;
+ USHORT CAP;
+ USHORT DATAPTR;
+} XGI330_LCDDataTablStruct;
+
+typedef struct _XGI330_TVDataTablStruct
+{
+ USHORT MASK;
+ USHORT CAP;
+ USHORT DATAPTR;
+} XGI330_TVDataTablStruct;
+
+
+typedef struct _XGI330_CHTVDataStruct
+{
+ USHORT VGAHT;
+ USHORT VGAVT;
+ USHORT LCDHT;
+ USHORT LCDVT;
+} XGI330_CHTVDataStruct;
+
+typedef struct _XGI_TimingHStruct
+{
+  UCHAR data[8];
+} XGI_TimingHStruct;
+
+typedef struct _XGI_TimingVStruct
+{
+  UCHAR data[7];
+} XGI_TimingVStruct;
+
+typedef struct _XGI_CH7007TV_TimingHStruct
+{
+  UCHAR data[10];
+} XGI_CH7007TV_TimingHStruct;
+
+typedef struct _XGI_CH7007TV_TimingVStruct
+{
+  UCHAR data[10];
+} XGI_CH7007TV_TimingVStruct;
+
+typedef struct _XGI_XG21CRT1Struct
+{
+ UCHAR ModeID,CR02,CR03,CR15,CR16;
+} XGI_XG21CRT1Struct;
+
+typedef struct _XGI330_CHTVRegDataStruct
+{
+ UCHAR Reg[16];
+} XGI330_CHTVRegDataStruct;
+
+typedef struct _XGI330_LCDCapStruct
+{
+                UCHAR      LCD_ID;
+                USHORT     LCD_Capability;
+                UCHAR      LCD_SetFlag;
+                UCHAR      LCD_DelayCompensation;
+                UCHAR      LCD_HSyncWidth;
+                UCHAR      LCD_VSyncWidth;
+                UCHAR      LCD_VCLK;
+                UCHAR      LCDA_VCLKData1;
+                UCHAR      LCDA_VCLKData2;
+                UCHAR      LCUCHAR_VCLKData1;
+                UCHAR      LCUCHAR_VCLKData2;
+                UCHAR      PSC_S1;
+                UCHAR      PSC_S2;
+                UCHAR      PSC_S3;
+                UCHAR      PSC_S4;
+                UCHAR      PSC_S5;
+                UCHAR      PWD_2B;
+                UCHAR      PWD_2C;
+                UCHAR      PWD_2D;
+                UCHAR      PWD_2E;
+                UCHAR      PWD_2F;
+                UCHAR      Spectrum_31;
+                UCHAR      Spectrum_32;
+                UCHAR      Spectrum_33;
+                UCHAR      Spectrum_34;
+} XGI330_LCDCapStruct;
+
+typedef struct _XGI21_LVDSCapStruct
+{
+                USHORT     LVDS_Capability;
+                USHORT     LVDSHT;
+                USHORT     LVDSVT;
+                USHORT     LVDSHDE;
+                USHORT     LVDSVDE;
+                USHORT     LVDSHFP;
+                USHORT     LVDSVFP;
+                USHORT     LVDSHSYNC;
+                USHORT     LVDSVSYNC;
+                UCHAR      VCLKData1;
+                UCHAR      VCLKData2;
+                UCHAR      PSC_S1;
+                UCHAR      PSC_S2;
+                UCHAR      PSC_S3;
+                UCHAR      PSC_S4;
+                UCHAR      PSC_S5;
+} XGI21_LVDSCapStruct;
+
+typedef struct _XGI_CRT1TableStruct
+{
+  UCHAR CR[16];
+} XGI_CRT1TableStruct;
+
+
+typedef struct _XGI330_VCLKDataStruct
+{
+    UCHAR SR2B,SR2C;
+    USHORT CLOCK;
+} XGI330_VCLKDataStruct;
+
+typedef struct _XGI301C_Tap4TimingStruct
+{
+    USHORT DE;
+    UCHAR  Reg[64];   /* C0-FF */
+} XGI301C_Tap4TimingStruct;
+
+typedef struct _XGI_New_StandTableStruct
+{
+        UCHAR  CRT_COLS;
+        UCHAR  ROWS;
+        UCHAR  CHAR_HEIGHT;
+        USHORT CRT_LEN;
+        UCHAR  SR[4];
+        UCHAR  MISC;
+        UCHAR  CRTC[0x19];
+        UCHAR  ATTR[0x14];
+        UCHAR  GRC[9];
+} XGI_New_StandTableStruct;
+
+typedef UCHAR DRAM8Type[8];
+typedef UCHAR DRAM4Type[4];
+typedef UCHAR DRAM32Type[32];
+typedef UCHAR DRAM2Type[2];
+
+typedef struct _VB_DEVICE_INFO  VB_DEVICE_INFO;
+typedef VB_DEVICE_INFO *        PVB_DEVICE_INFO;
+
+struct _VB_DEVICE_INFO
+{
+    BOOLEAN  ISXPDOS;
+    ULONG   P3c4,P3d4,P3c0,P3ce,P3c2,P3cc;
+    ULONG   P3ca,P3c6,P3c7,P3c8,P3c9,P3da;
+    ULONG   Part0Port,Part1Port,Part2Port;
+    ULONG   Part3Port,Part4Port,Part5Port;
+    USHORT   RVBHCFACT,RVBHCMAX,RVBHRS;
+    USHORT   VGAVT,VGAHT,VGAVDE,VGAHDE;
+    USHORT   VT,HT,VDE,HDE;
+    USHORT   LCDHRS,LCDVRS,LCDHDES,LCDVDES;
+
+    USHORT   ModeType;
+    USHORT   IF_DEF_LVDS,IF_DEF_TRUMPION,IF_DEF_DSTN;/* ,IF_DEF_FSTN; add for dstn */
+    USHORT   IF_DEF_CRT2Monitor,IF_DEF_VideoCapture;
+    USHORT   IF_DEF_LCDA,IF_DEF_CH7017,IF_DEF_YPbPr,IF_DEF_ScaleLCD,IF_DEF_OEMUtil,IF_DEF_PWD;
+    USHORT   IF_DEF_ExpLink;
+    USHORT   IF_DEF_CH7005,IF_DEF_HiVision;
+    USHORT   IF_DEF_CH7007; /* Billy 2007/05/03 */
+    USHORT   LCDResInfo,LCDTypeInfo, VBType;/*301b*/
+    USHORT   VBInfo,TVInfo,LCDInfo, Set_VGAType;
+    USHORT   VBExtInfo;/*301lv*/
+    USHORT   SetFlag;
+    USHORT   NewFlickerMode;
+    USHORT   SelectCRT2Rate;
+
+    PUCHAR ROMAddr;
+    PUCHAR FBAddr;
+    ULONG BaseAddr;
+    ULONG RelIO;
+
+    DRAM4Type  *CR6B;
+    DRAM4Type  *CR6E;
+    DRAM32Type *CR6F;
+    DRAM2Type  *CR89;
+
+    DRAM8Type  *SR15; /* pointer : point to array */
+    DRAM8Type  *CR40;
+    UCHAR  *pSoftSetting;
+    UCHAR  *pOutputSelect;
+
+    USHORT *pRGBSenseData;
+    USHORT *pRGBSenseData2; /*301b*/
+    USHORT *pVideoSenseData;
+    USHORT *pVideoSenseData2;
+    USHORT *pYCSenseData;
+    USHORT *pYCSenseData2;
+
+    UCHAR  *pSR07;
+    UCHAR  *CR49;
+    UCHAR  *pSR1F;
+    UCHAR  *AGPReg;
+    UCHAR  *SR16;
+    UCHAR  *pSR21;
+    UCHAR  *pSR22;
+    UCHAR  *pSR23;
+    UCHAR  *pSR24;
+    UCHAR  *SR25;
+    UCHAR  *pSR31;
+    UCHAR  *pSR32;
+    UCHAR  *pSR33;
+    UCHAR  *pSR36;      /* alan 12/07/2006 */
+    UCHAR  *pCRCF;
+    UCHAR  *pCRD0;      /* alan 12/07/2006 */
+    UCHAR  *pCRDE;      /* alan 12/07/2006 */
+    UCHAR  *pCR8F;      /* alan 12/07/2006 */
+    UCHAR  *pSR40;      /* alan 12/07/2006 */
+    UCHAR  *pSR41;      /* alan 12/07/2006 */
+    UCHAR  *pDVOSetting;
+    UCHAR  *pCR2E;
+    UCHAR  *pCR2F;
+    UCHAR  *pCR46;
+    UCHAR  *pCR47;
+    UCHAR  *pCRT2Data_1_2;
+    UCHAR  *pCRT2Data_4_D;
+    UCHAR  *pCRT2Data_4_E;
+    UCHAR  *pCRT2Data_4_10;
+    XGI_MCLKDataStruct  *MCLKData;
+    XGI_ECLKDataStruct  *ECLKData;
+
+    UCHAR   *XGI_TVDelayList;
+    UCHAR   *XGI_TVDelayList2;
+    UCHAR   *CHTVVCLKUNTSC;
+    UCHAR   *CHTVVCLKONTSC;
+    UCHAR   *CHTVVCLKUPAL;
+    UCHAR   *CHTVVCLKOPAL;
+    UCHAR   *NTSCTiming;
+    UCHAR   *PALTiming;
+    UCHAR   *HiTVExtTiming;
+    UCHAR   *HiTVSt1Timing;
+    UCHAR   *HiTVSt2Timing;
+    UCHAR   *HiTVTextTiming;
+    UCHAR   *YPbPr750pTiming;
+    UCHAR   *YPbPr525pTiming;
+    UCHAR   *YPbPr525iTiming;
+    UCHAR   *HiTVGroup3Data;
+    UCHAR   *HiTVGroup3Simu;
+    UCHAR   *HiTVGroup3Text;
+    UCHAR   *Ren525pGroup3;
+    UCHAR   *Ren750pGroup3;
+    UCHAR   *ScreenOffset;
+    UCHAR   *pXGINew_DRAMTypeDefinition;
+    UCHAR   *pXGINew_I2CDefinition ;
+    UCHAR   *pXGINew_CR97 ;
+
+    XGI330_LCDCapStruct  *LCDCapList;
+    XGI21_LVDSCapStruct  *XG21_LVDSCapList;
+
+    XGI_TimingHStruct  *TimingH;
+    XGI_TimingVStruct  *TimingV;
+
+    XGI_StStruct          *SModeIDTable;
+    XGI_StandTableStruct  *StandTable;
+    XGI_ExtStruct         *EModeIDTable;
+    XGI_Ext2Struct        *RefIndex;
+    /* XGINew_CRT1TableStruct *CRT1Table; */
+    XGI_CRT1TableStruct    *XGINEWUB_CRT1Table;
+    XGI_VCLKDataStruct    *VCLKData;
+    XGI_VBVCLKDataStruct  *VBVCLKData;
+    XGI_StResInfoStruct   *StResInfo;
+    XGI_ModeResInfoStruct *ModeResInfo;
+    XGI_XG21CRT1Struct    *UpdateCRT1;
+};  /* _VB_DEVICE_INFO */
+
+
+typedef struct
+{
+    USHORT    Horizontal_ACTIVE;
+    USHORT    Horizontal_FP;
+    USHORT    Horizontal_SYNC;
+    USHORT    Horizontal_BP;
+    USHORT    Vertical_ACTIVE;
+    USHORT    Vertical_FP;
+    USHORT    Vertical_SYNC;
+    USHORT    Vertical_BP;
+    double    DCLK;
+    UCHAR     FrameRate;
+    UCHAR     Interlace;
+    USHORT    Margin;
+} TimingInfo;
+
+#define _VB_STRUCT_
+#endif /* _VB_STRUCT_ */
diff --git a/drivers/staging/xgifb/vb_table.h b/drivers/staging/xgifb/vb_table.h
new file mode 100644
index 000000000000..781caefc56b1
--- /dev/null
+++ b/drivers/staging/xgifb/vb_table.h
@@ -0,0 +1,4406 @@
+#define  Tap4
+
+
+XGI_MCLKDataStruct XGI330New_MCLKData[]=
+{
+ { 0x5c,0x23,0x01,166},
+ { 0x5c,0x23,0x01,166},
+ { 0x7C,0x08,0x80,200},
+ { 0x79,0x06,0x80,250},
+ { 0x29,0x01,0x81,300},
+ { 0x29,0x01,0x81,300},
+ { 0x29,0x01,0x81,300},
+ { 0x29,0x01,0x81,300}
+};
+//yilin modify for xgi20
+XGI_MCLKDataStruct XGI340New_MCLKData[]=
+{
+ { 0x16,0x01,0x01,166},
+ { 0x19,0x02,0x01,124},
+ { 0x7C,0x08,0x01,200},
+ { 0x79,0x06,0x01,250},
+ { 0x29,0x01,0x81,301},
+ { 0x5c,0x23,0x01,166},
+ { 0x5c,0x23,0x01,166},
+ { 0x5c,0x23,0x01,166}
+};
+
+XGI_MCLKDataStruct XGI27New_MCLKData[]=
+{
+ { 0x5c,0x23,0x01,166},
+ { 0x19,0x02,0x01,124},
+ { 0x7C,0x08,0x80,200},
+ { 0x79,0x06,0x80,250},
+ { 0x29,0x01,0x81,300},
+ { 0x5c,0x23,0x01,166},
+ { 0x5c,0x23,0x01,166},
+ { 0x5c,0x23,0x01,166}
+};
+
+XGI_ECLKDataStruct XGI330_ECLKData[]=
+{
+ { 0x7c,0x08,0x01,200},
+ { 0x7c,0x08,0x01,200},
+ { 0x7C,0x08,0x80,200},
+ { 0x79,0x06,0x80,250},
+ { 0x29,0x01,0x81,300},
+ { 0x29,0x01,0x81,300},
+ { 0x29,0x01,0x81,300},
+ { 0x29,0x01,0x81,300}
+};
+//yilin modify for xgi20
+XGI_ECLKDataStruct XGI340_ECLKData[]=
+{
+ { 0x5c,0x23,0x01,166},
+ { 0x55,0x84,0x01,123},
+ { 0x7C,0x08,0x01,200},
+ { 0x79,0x06,0x01,250},
+ { 0x29,0x01,0x81,301},
+ { 0x5c,0x23,0x01,166},
+ { 0x5c,0x23,0x01,166},
+ { 0x5c,0x23,0x01,166}
+};
+
+
+
+UCHAR XGI340_SR13[4][8]={
+{0x35,0x45,0xb1,0x00,0x00,0x00,0x00,0x00},/* SR13 */
+{0x41,0x51,0x5c,0x00,0x00,0x00,0x00,0x00},/* SR14 */
+{0x31,0x42,0x42,0x00,0x00,0x00,0x00,0x00},/* SR18 */
+{0x03,0x03,0x03,0x00,0x00,0x00,0x00,0x00}/* SR1B */
+};
+
+UCHAR XGI340_cr41[24][8]=
+{{0x20,0x50,0x60,0x00,0x00,0x00,0x00,0x00},/* 0 CR41 */
+{0xc4,0x40,0x84,0x00,0x00,0x00,0x00,0x00},/* 1 CR8A */
+{0xc4,0x40,0x84,0x00,0x00,0x00,0x00,0x00},/* 2 CR8B */
+{0xb5,0xa4,0xa4,0x00,0x00,0x00,0x00,0x00},
+{0xf0,0xf0,0xf0,0x00,0x00,0x00,0x00,0x00},
+{0x90,0x90,0x24,0x00,0x00,0x00,0x00,0x00},/* 5 CR68 */
+{0x77,0x77,0x44,0x00,0x00,0x00,0x00,0x00},/* 6 CR69 */
+{0x77,0x77,0x44,0x00,0x00,0x00,0x00,0x00},/* 7 CR6A */
+{0xff,0xff,0xff,0x00,0x00,0x00,0x00,0x00},/* 8 CR6D */
+{0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},/* 9 CR80 */
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/* 10 CR81 */
+{0x88,0xa8,0x48,0x00,0x00,0x00,0x00,0x00},/* 11 CR82 */
+{0x44,0x44,0x77,0x00,0x00,0x00,0x00,0x00},/* 12 CR85 */
+{0x48,0x48,0x88,0x00,0x00,0x00,0x00,0x00},/* 13 CR86 */
+{0x54,0x54,0x44,0x00,0x00,0x00,0x00,0x00},/* 14 CR90 */
+{0x54,0x54,0x44,0x00,0x00,0x00,0x00,0x00},/* 15 CR91 */
+{0x0a,0x0a,0x07,0x00,0x00,0x00,0x00,0x00},/* 16 CR92 */
+{0x44,0x44,0x44,0x00,0x00,0x00,0x00,0x00},/* 17 CR93 */
+{0x10,0x10,0x0A,0x00,0x00,0x00,0x00,0x00},/* 18 CR94 */
+{0x11,0x11,0x0a,0x00,0x00,0x00,0x00,0x00},/* 19 CR95 */
+{0x05,0x05,0x05,0x00,0x00,0x00,0x00,0x00},/* 20 CR96 */
+{0xf0,0xf0,0xf0,0x00,0x00,0x00,0x00,0x00},/* 21 CRC3 */
+{0x05,0x00,0x02,0x00,0x00,0x00,0x00,0x00},/* 22 CRC4 */
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}/* 23 CRC5 */
+};
+
+
+UCHAR XGI27_cr41[24][8]=
+{
+{0x20,0x40,0x60,0x00,0x00,0x00,0x00,0x00},/* 0 CR41 */
+{0xC4,0x40,0x84,0x00,0x00,0x00,0x00,0x00},/* 1 CR8A */
+{0xC4,0x40,0x84,0x00,0x00,0x00,0x00,0x00},/* 2 CR8B */
+{0xB5,0x13,0xa4,0x00,0x00,0x00,0x00,0x00},/* 3 CR40[7],CR99[2:0],CR45[3:0]*/
+{0xf0,0xf5,0xf0,0x00,0x00,0x00,0x00,0x00},/* 4 CR59 */
+{0x90,0x90,0x24,0x00,0x00,0x00,0x00,0x00},/* 5 CR68 */
+{0x77,0x67,0x44,0x00,0x00,0x00,0x00,0x00},/* 6 CR69 */
+{0x77,0x77,0x44,0x00,0x00,0x00,0x00,0x00},/* 7 CR6A */
+{0xff,0xff,0xff,0x00,0x00,0x00,0x00,0x00},/* 8 CR6D */
+{0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},/* 9 CR80 */
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/* 10 CR81 */
+{0x88,0xcc,0x48,0x00,0x00,0x00,0x00,0x00},/* 11 CR82 */
+{0x44,0x88,0x77,0x00,0x00,0x00,0x00,0x00},/* 12 CR85 */
+{0x48,0x88,0x88,0x00,0x00,0x00,0x00,0x00},/* 13 CR86 */
+{0x54,0x32,0x44,0x00,0x00,0x00,0x00,0x00},/* 14 CR90 */
+{0x54,0x33,0x44,0x00,0x00,0x00,0x00,0x00},/* 15 CR91 */
+{0x0a,0x07,0x07,0x00,0x00,0x00,0x00,0x00},/* 16 CR92 */
+{0x44,0x63,0x44,0x00,0x00,0x00,0x00,0x00},/* 17 CR93 */
+{0x10,0x14,0x0A,0x00,0x00,0x00,0x00,0x00},/* 18 CR94 */
+{0x11,0x0B,0x0C,0x00,0x00,0x00,0x00,0x00},/* 19 CR95 */
+{0x05,0x22,0x05,0x00,0x00,0x00,0x00,0x00},/* 20 CR96 */
+{0xf0,0xf0,0x00,0x00,0x00,0x00,0x00,0x00},/* 21 CRC3 */
+{0x05,0x00,0x02,0x00,0x00,0x00,0x00,0x00},/* 22 CRC4 */
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}/* 23 CRC5 */
+};
+
+
+#if 0
+UCHAR XGI27_cr41[24][8]=
+{
+{0x20,0x60,0x60,0x00,0x00,0x00,0x00,0x00},/* 0 CR41 */
+{0x04,0x44,0x84,0x00,0x00,0x00,0x00,0x00},/* 1 CR8A */
+{0x04,0x40,0x84,0x00,0x00,0x00,0x00,0x00},/* 2 CR8B */
+{0xb5,0x03,0xa4,0x00,0x00,0x00,0x00,0x00},/* 3 CR40[7],CR99[2:0],CR45[3:0]*/
+{0xf0,0xf5,0xf0,0x00,0x00,0x00,0x00,0x00},/* 4 CR59 */
+{0xa4,0x1C,0x24,0x00,0x00,0x00,0x00,0x00},/* 5 CR68 */
+{0x77,0x77,0x44,0x00,0x00,0x00,0x00,0x00},/* 6 CR69 */
+{0x77,0x77,0x44,0x00,0x00,0x00,0x00,0x00},/* 7 CR6A */
+{0xff,0xff,0xff,0x00,0x00,0x00,0x00,0x00},/* 8 CR6D */
+{0x55,0x55,0x55,0x00,0x00,0x00,0x00,0x00},/* 9 CR80 */
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/* 10 CR81 */
+{0x48,0xa8,0x48,0x00,0x00,0x00,0x00,0x00},/* 11 CR82 */
+{0x77,0x88,0x77,0x00,0x00,0x00,0x00,0x00},/* 12 CR85 */
+{0x88,0x88,0x88,0x00,0x00,0x00,0x00,0x00},/* 13 CR86 */
+{0x44,0x32,0x44,0x00,0x00,0x00,0x00,0x00},/* 14 CR90 */
+{0x44,0x33,0x44,0x00,0x00,0x00,0x00,0x00},/* 15 CR91 */
+{0x07,0x07,0x07,0x00,0x00,0x00,0x00,0x00},/* 16 CR92 */
+{0x44,0x63,0x44,0x00,0x00,0x00,0x00,0x00},/* 17 CR93 */
+{0x0A,0x14,0x0A,0x00,0x00,0x00,0x00,0x00},/* 18 CR94 */
+{0x0C,0x0B,0x0C,0x00,0x00,0x00,0x00,0x00},/* 19 CR95 */
+{0x05,0x22,0x05,0x00,0x00,0x00,0x00,0x00},/* 20 CR96 */
+{0xf0,0xf0,0xf0,0x00,0x00,0x00,0x00,0x00},/* 21 CRC3 */
+{0x03,0x00,0x02,0x00,0x00,0x00,0x00,0x00},/* 22 CRC4 */
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}/* 23 CRC5 */
+};
+#endif
+UCHAR XGI340_CR6B[8][4]={
+{0xaa,0xaa,0xaa,0xaa},
+{0xaa,0xaa,0xaa,0xaa},
+{0xaa,0xaa,0xaa,0xaa},
+{0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00}
+};
+
+UCHAR XGI340_CR6E[8][4]={
+{0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00}
+};
+
+UCHAR XGI340_CR6F[8][32]={
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}
+};
+
+UCHAR XGI340_CR89[8][2]={
+{0x00,0x00},
+{0x00,0x00},
+{0x00,0x00},
+{0x00,0x00},
+{0x00,0x00},
+{0x00,0x00},
+{0x00,0x00},
+{0x00,0x00}
+};
+                         /* CR47,CR48,CR49,CR4A,CR4B,CR4C,CR70,CR71,CR74,CR75,CR76,CR77 */
+UCHAR XGI340_AGPReg[12]={0x28,0x23,0x00,0x20,0x00,0x20,0x00,0x05,0xd0,0x10,0x10,0x00};
+
+UCHAR XGI340_SR16[4]={0x03,0x83,0x03,0x83};
+
+UCHAR XGI330_SR15_1[8][8]={
+{0x0,0x0,0x00,0x00,0x20,0x20,0x00,0x00},
+{0x5,0x15,0x15,0x15,0x15,0x15,0x00,0x00},
+{0xba,0xba,0xba,0xba,0xBA,0xBA,0x00,0x00},
+{0x55,0x57,0x57,0xAB,0xAB,0xAB,0x00,0x00},
+{0x60,0x34,0x34,0x34,0x34,0x34,0x00,0x00},
+{0x0,0x80,0x80,0x80,0x83,0x83,0x00,0x00},
+{0x50,0x50,0x50,0x3C,0x3C,0x3C,0x00,0x00},
+{0x0,0xa5,0xfb,0xf6,0xF6,0xF6,0x00,0x00}
+};
+
+UCHAR XGI330_cr40_1[15][8]={
+{0x66,0x40,0x40,0x28,0x24,0x24,0x00,0x00},
+{0x66,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x0F,0x0F,0x00,0x00},
+{0x00,0xf0,0xf0,0xf0,0xF0,0xF0,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x10,0x10,0x10,0x10,0x20,0x20,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x88,0x88,0x88,0xAA,0xAC,0xAC,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x77,0x77,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
+{0x00,0xA2,0x00,0x00,0xA2,0xA2,0x00,0x00},
+};
+
+UCHAR XGI330_sr25[]={0x00,0x0};
+UCHAR XGI330_sr31=0xc0;
+UCHAR XGI330_sr32=0x11;
+UCHAR XGI330_SR33=0x00;
+UCHAR XG40_CRCF=0x13;
+UCHAR XG40_DRAMTypeDefinition=0xFF ;
+
+XGI_StStruct XGI330_SModeIDTable[]=
+{
+ {0x01,0x9208,0x01,0x00,0x10,0x00,0x00,0x01,0x00},
+ {0x01,0x1210,0x14,0x01,0x00,0x01,0x00,0x01,0x00},
+ {0x01,0x1010,0x17,0x02,0x11,0x00,0x00,0x01,0x01},
+ {0x03,0x8208,0x03,0x00,0x14,0x00,0x00,0x01,0x02},
+ {0x03,0x0210,0x16,0x01,0x04,0x01,0x00,0x01,0x02},
+ {0x03,0x0010,0x18,0x02,0x15,0x00,0x00,0x01,0x03},
+ {0x05,0x9209,0x05,0x00,0x10,0x00,0x00,0x00,0x04},
+ {0x06,0x8209,0x06,0x00,0x14,0x00,0x00,0x00,0x05},
+ {0x07,0x0000,0x07,0x03,0x05,0x03,0x00,0x01,0x03},
+ {0x07,0x0000,0x19,0x02,0x15,0x02,0x00,0x01,0x03},
+ {0x0d,0x920a,0x0d,0x00,0x10,0x00,0x00,0x00,0x04},
+ {0x0e,0x820a,0x0e,0x00,0x14,0x00,0x00,0x00,0x05},
+ {0x0f,0x0202,0x11,0x01,0x04,0x01,0x00,0x00,0x05},
+ {0x10,0x0212,0x12,0x01,0x04,0x01,0x00,0x00,0x05},
+ {0x11,0x0212,0x1a,0x04,0x24,0x04,0x00,0x00,0x05},
+ {0x12,0x0212,0x1b,0x04,0x24,0x04,0x00,0x00,0x05},
+ {0x13,0x021b,0x1c,0x00,0x14,0x00,0x00,0x00,0x04},
+ {0x12,0x0010,0x18,0x02,0x24,0x02,0x00,0x00,0x05},/* St_CRT2CRTC2 not sure */
+ {0x12,0x0210,0x18,0x01,0x24,0x01,0x00,0x00,0x05},/* St_CRT2CRTC2 not sure */
+ {0xff,0x0000,0x00,0x00,0x00,0x00,0x00,0x00,0x00}
+};
+
+
+XGI_ExtStruct  XGI330_EModeIDTable[]=
+{
+ {0x6a,0x2212,0x0407,0x3a81,0x0102,0x08,0x07,0x00,0x00,0x07,0x0e},
+ {0x2e,0x0a1b,0x0306,0x3a57,0x0101,0x08,0x06,0x00,0x00,0x05,0x06},
+ {0x2f,0x0a1b,0x0305,0x3a50,0x0100,0x08,0x05,0x00,0x00,0x05,0x05},
+ {0x30,0x2a1b,0x0407,0x3a81,0x0103,0x08,0x07,0x00,0x00,0x07,0x0e},
+ {0x31,0x0a1b,0x030d,0x3b85,0x0000,0x08,0x0d,0x00,0x00,0x06,0x3d},
+ {0x32,0x0a1b,0x0a0e,0x3b8c,0x0000,0x08,0x0e,0x00,0x00,0x06,0x3e},
+ {0x33,0x0a1d,0x0a0d,0x3b85,0x0000,0x08,0x0d,0x00,0x00,0x06,0x3d},
+ {0x34,0x2a1d,0x0a0e,0x3b8c,0x0000,0x08,0x0e,0x00,0x00,0x06,0x3e},
+ {0x35,0x0a1f,0x0a0d,0x3b85,0x0000,0x08,0x0d,0x00,0x00,0x06,0x3d},
+ {0x36,0x2a1f,0x0a0e,0x3b8c,0x0000,0x08,0x0e,0x00,0x00,0x06,0x3e},
+ {0x37,0x0212,0x0508,0x3aab,0x0104,0x08,0x08,0x00,0x00,0x00,0x16},
+ {0x38,0x0a1b,0x0508,0x3aab,0x0105,0x08,0x08,0x00,0x00,0x00,0x16},
+ {0x3a,0x0e3b,0x0609,0x3adc,0x0107,0x08,0x09,0x00,0x00,0x00,0x1e},
+ {0x3c,0x0e3b,0x070a,0x3af2,0x0130,0x08,0x0a,0x00,0x00,0x00,0x22},      /* mode 1600x1200 add CRT2MODE [2003/10/07] */
+ {0x3d,0x0e7d,0x070a,0x3af2,0x0131,0x08,0x0a,0x00,0x00,0x00,0x22},      /* mode 1600x1200 add CRT2MODE */
+ {0x40,0x9a1c,0x0000,0x3a34,0x010d,0x08,0x00,0x00,0x00,0x04,0x00},
+ {0x41,0x9a1d,0x0000,0x3a34,0x010e,0x08,0x00,0x00,0x00,0x04,0x00},      /* ModeIdIndex = 0x10 */
+ {0x43,0x0a1c,0x0306,0x3a57,0x0110,0x08,0x06,0x00,0x00,0x05,0x06},
+ {0x44,0x0a1d,0x0306,0x3a57,0x0111,0x08,0x06,0x00,0x00,0x05,0x06},
+ {0x46,0x2a1c,0x0407,0x3a81,0x0113,0x08,0x07,0x00,0x00,0x07,0x0e},
+ {0x47,0x2a1d,0x0407,0x3a81,0x0114,0x08,0x07,0x00,0x00,0x07,0x0e},
+ {0x49,0x0a3c,0x0508,0x3aab,0x0116,0x08,0x08,0x00,0x00,0x00,0x16},
+ {0x4a,0x0a3d,0x0508,0x3aab,0x0117,0x08,0x08,0x00,0x00,0x00,0x16},
+ {0x4c,0x0e7c,0x0609,0x3adc,0x0119,0x08,0x09,0x00,0x00,0x00,0x1e},
+ {0x4d,0x0e7d,0x0609,0x3adc,0x011a,0x08,0x09,0x00,0x00,0x00,0x1e},
+ {0x50,0x9a1b,0x0001,0x3a3b,0x0132,0x08,0x01,0x00,0x00,0x04,0x02},
+ {0x51,0xba1b,0x0103,0x3a42,0x0133,0x08,0x03,0x00,0x00,0x07,0x03},
+ {0x52,0x9a1b,0x0204,0x3a49,0x0134,0x08,0x04,0x00,0x00,0x00,0x04},
+ {0x56,0x9a1d,0x0001,0x3a3b,0x0135,0x08,0x01,0x00,0x00,0x04,0x02},
+ {0x57,0xba1d,0x0103,0x3a42,0x0136,0x08,0x03,0x00,0x00,0x07,0x03},
+ {0x58,0x9a1d,0x0204,0x3a49,0x0137,0x08,0x04,0x00,0x00,0x00,0x04},
+ {0x59,0x9a1b,0x0000,0x3a34,0x0138,0x08,0x00,0x00,0x00,0x04,0x00},
+ {0x5A,0x021b,0x0014,0x3b83,0x0138,0x08,0x01,0x00,0x00,0x04,0x3f},      /* ModeIdIndex = 0x20 */
+ {0x5B,0x0a1d,0x0014,0x3b83,0x0135,0x08,0x01,0x00,0x00,0x04,0x3f},
+ {0x5d,0x0a1d,0x0305,0x3a50,0x0139,0x08,0x05,0x00,0x00,0x07,0x05},
+ {0x62,0x0a3f,0x0306,0x3a57,0x013a,0x08,0x06,0x00,0x00,0x05,0x06},
+ {0x63,0x2a3f,0x0407,0x3a81,0x013b,0x08,0x07,0x00,0x00,0x07,0x0e},
+ {0x64,0x0a7f,0x0508,0x3aab,0x013c,0x08,0x08,0x00,0x00,0x00,0x16},
+ {0x65,0x0eff,0x0609,0x3adc,0x013d,0x08,0x09,0x00,0x00,0x00,0x1e},
+ {0x66,0x0eff,0x070a,0x3af2,0x013e,0x08,0x0a,0x00,0x00,0x00,0x22},      /* mode 1600x1200 add CRT2MODE */
+ {0x68,0x067b,0x080b,0x3b17,0x013f,0x08,0x0b,0x00,0x00,0x00,0x29},
+ {0x69,0x06fd,0x080b,0x3b17,0x0140,0x08,0x0b,0x00,0x00,0x00,0x29},
+ {0x6b,0x07ff,0x080b,0x3b17,0x0141,0x10,0x0b,0x00,0x00,0x00,0x29},
+ {0x6c,0x067b,0x090c,0x3b37,0x0000,0x08,0x0c,0x00,0x00,0x00,0x2f},
+ {0x6d,0x06fd,0x090c,0x3b37,0x0000,0x10,0x0c,0x00,0x00,0x00,0x2f},
+ {0x6e,0x07ff,0x090c,0x3b37,0x0000,0x10,0x0c,0x00,0x00,0x00,0x2f},
+ {0x70,0x2a1b,0x0410,0x3b52,0x0000,0x08,0x10,0x00,0x00,0x07,0x34},
+ {0x71,0x0a1b,0x0511,0x3b63,0x0000,0x08,0x11,0x00,0x00,0x00,0x37},
+ {0x74,0x0a1d,0x0511,0x3b63,0x0000,0x08,0x11,0x00,0x00,0x00,0x37},      /* ModeIdIndex = 0x30 */
+ {0x75,0x0a3d,0x0612,0x3b74,0x0000,0x08,0x12,0x00,0x00,0x00,0x3a},
+ {0x76,0x2a1f,0x0410,0x3b52,0x0000,0x08,0x10,0x00,0x00,0x07,0x34},
+ {0x77,0x0a1f,0x0511,0x3b63,0x0000,0x08,0x11,0x00,0x00,0x00,0x37},
+ {0x78,0x0a3f,0x0612,0x3b74,0x0000,0x08,0x12,0x00,0x00,0x00,0x3a},
+ {0x79,0x0a3b,0x0612,0x3b74,0x0000,0x08,0x12,0x00,0x00,0x00,0x3a},
+ {0x7a,0x2a1d,0x0410,0x3b52,0x0000,0x08,0x10,0x00,0x00,0x07,0x34},
+ {0x7b,0x0e3b,0x060f,0x3ad0,0x0000,0x08,0x0f,0x00,0x00,0x00,0x1d},
+ {0x7c,0x0e7d,0x060f,0x3ad0,0x0000,0x08,0x0f,0x00,0x00,0x00,0x1d},
+ {0x7d,0x0eff,0x060f,0x3ad0,0x0000,0x08,0x0f,0x00,0x00,0x00,0x1d},
+ {0x20,0x0e3b,0x0D16,0x49e0,0x0000,0x08,0x16,0x00,0x00,0x00,0x43},
+ {0x21,0x0e7d,0x0D16,0x49e0,0x0000,0x08,0x16,0x00,0x00,0x00,0x43},
+ {0x22,0x0eff,0x0D16,0x49e0,0x0000,0x08,0x16,0x00,0x00,0x00,0x43},
+ {0x23,0x0e3b,0x0614,0x49d5,0x0000,0x08,0x14,0x00,0x00,0x00,0x41},
+ {0x24,0x0e7d,0x0614,0x49d5,0x0000,0x08,0x14,0x00,0x00,0x00,0x41},
+ {0x25,0x0eff,0x0614,0x49d5,0x0000,0x08,0x14,0x00,0x00,0x00,0x41},
+ {0x26,0x063b,0x0c15,0x49dc,0x0000,0x08,0x15,0x00,0x00,0x00,0x42},      /* ModeIdIndex = 0x40 */
+ {0x27,0x067d,0x0c15,0x49dc,0x0000,0x08,0x15,0x00,0x00,0x00,0x42},
+ {0x28,0x06ff,0x0c15,0x49dc,0x0000,0x08,0x15,0x00,0x00,0x00,0x42},
+ {0xff,0x0000,0x0000,0x0000,0x0000,0x00,0x00,0x00,0x00,0x00,0x00}
+};
+
+XGI_StandTableStruct XGI330_StandTable[]=
+{
+/* MD_0_200 */
+ {
+  0x28,0x18,0x08,0x0800,
+  {0x09,0x03,0x00,0x02},
+  0x63,
+  {0x2d,0x27,0x28,0x90,0x2b,0xa0,0xbf,0x1f,
+   0x00,0xc7,0x06,0x07,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x14,0x1f,0x96,0xb9,0xa3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+   0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,
+   0x08,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x10,0x0e,0x00,
+   0xff}
+ },
+/* MD_1_200 */
+ {
+  0x28,0x18,0x08,0x0800,
+  {0x09,0x03,0x00,0x02},
+  0x63,
+  {0x2d,0x27,0x28,0x90,0x2b,0xa0,0xbf,0x1f,
+   0x00,0xc7,0x06,0x07,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x14,0x1f,0x96,0xb9,0xa3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+   0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,
+   0x08,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x10,0x0e,0x00,
+   0xff}
+ },
+/* MD_2_200 */
+ {
+  0x50,0x18,0x08,0x1000,
+  {0x01,0x03,0x00,0x02},
+  0x63,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+   0x00,0xc7,0x06,0x07,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x28,0x1f,0x96,0xb9,0xa3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+   0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,
+   0x08,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x10,0x0e,0x00,
+   0xff}
+ },
+/* MD_3_200 */
+ {
+  0x50,0x18,0x08,0x1000,
+  {0x01,0x03,0x00,0x02},
+  0x63,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+   0x00,0xc7,0x06,0x07,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x28,0x1f,0x96,0xb9,0xa3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+   0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,
+   0x08,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x10,0x0e,0x00,
+   0xff}
+ },
+/* MD_4 */
+ {
+  0x28,0x18,0x08,0x4000,
+  {0x09,0x03,0x00,0x02},
+  0x63,
+  {0x2d,0x27,0x28,0x90,0x2c,0x80,0xbf,0x1f,
+   0x00,0xc1,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x14,0x00,0x96,0xb9,0xa2,
+   0xff},
+  {0x00,0x13,0x15,0x17,0x02,0x04,0x06,0x07,
+   0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,
+   0x01,0x00,0x03,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x30,0x0f,0x00,
+   0xff}
+ },
+/* MD_5 */
+ {
+  0x28,0x18,0x08,0x4000,
+  {0x09,0x03,0x00,0x02},
+  0x63,
+  {0x2d,0x27,0x28,0x90,0x2c,0x80,0xbf,0x1f,
+   0x00,0xc1,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x14,0x00,0x96,0xb9,0xa2,
+   0xff},
+  {0x00,0x13,0x15,0x17,0x02,0x04,0x06,0x07,
+   0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,
+   0x01,0x00,0x03,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x30,0x0f,0x00,
+   0xff}
+ },
+/* MD_6 */
+ {
+  0x50,0x18,0x08,0x4000,
+  {0x01,0x01,0x00,0x06},
+  0x63,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+   0x00,0xc1,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x28,0x00,0x96,0xb9,0xc2,
+   0xff},
+  {0x00,0x17,0x17,0x17,0x17,0x17,0x17,0x17,
+   0x17,0x17,0x17,0x17,0x17,0x17,0x17,0x17,
+   0x01,0x00,0x01,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x00,0x0d,0x00,
+   0xff}
+ },
+/* MD_7 */
+ {
+  0x50,0x18,0x0e,0x1000,
+  {0x00,0x03,0x00,0x03},
+  0xa6,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+   0x00,0x4d,0x0b,0x0c,0x00,0x00,0x00,0x00,
+   0x83,0x85,0x5d,0x28,0x0d,0x63,0xba,0xa3,
+   0xff},
+  {0x00,0x08,0x08,0x08,0x08,0x08,0x08,0x08,
+   0x10,0x18,0x18,0x18,0x18,0x18,0x18,0x18,
+   0x0e,0x00,0x0f,0x08},
+  {0x00,0x00,0x00,0x00,0x00,0x10,0x0a,0x00,
+   0xff}
+ },
+/* MDA_DAC */
+ {
+  0x00,0x00,0x00,0x0000,
+  {0x00,0x00,0x00,0x15},
+  0x15,
+  {0x15,0x15,0x15,0x15,0x15,0x15,0x15,0x15,
+   0x15,0x15,0x15,0x15,0x15,0x15,0x3f,0x3f,
+   0x3f,0x3f,0x3f,0x3f,0x3f,0x3f,0x00,0x00,
+   0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x15,0x15,0x15,
+   0x15,0x15,0x15,0x15,0x15,0x15,0x15,0x15,
+   0x15,0x15,0x15,0x15},
+  {0x15,0x3f,0x3f,0x3f,0x3f,0x3f,0x3f,0x3f,
+   0x3f}
+ },
+/* CGA_DAC */
+ {
+  0x00,0x10,0x04,0x0114,
+  {0x11,0x09,0x15,0x00},
+  0x10,
+  {0x04,0x14,0x01,0x11,0x09,0x15,0x2a,0x3a,
+   0x2e,0x3e,0x2b,0x3b,0x2f,0x3f,0x2a,0x3a,
+   0x2e,0x3e,0x2b,0x3b,0x2f,0x3f,0x00,0x10,
+   0x04},
+  {0x14,0x01,0x11,0x09,0x15,0x00,0x10,0x04,
+   0x14,0x01,0x11,0x09,0x15,0x2a,0x3a,0x2e,
+   0x3e,0x2b,0x3b,0x2f},
+  {0x3f,0x2a,0x3a,0x2e,0x3e,0x2b,0x3b,0x2f,
+   0x3f}
+ },
+/* EGA_DAC */
+ {
+  0x00,0x10,0x04,0x0114,
+  {0x11,0x05,0x15,0x20},
+  0x30,
+  {0x24,0x34,0x21,0x31,0x25,0x35,0x08,0x18,
+   0x0c,0x1c,0x09,0x19,0x0d,0x1d,0x28,0x38,
+   0x2c,0x3c,0x29,0x39,0x2d,0x3d,0x02,0x12,
+   0x06},
+  {0x16,0x03,0x13,0x07,0x17,0x22,0x32,0x26,
+   0x36,0x23,0x33,0x27,0x37,0x0a,0x1a,0x0e,
+   0x1e,0x0b,0x1b,0x0f},
+  {0x1f,0x2a,0x3a,0x2e,0x3e,0x2b,0x3b,0x2f,
+   0x3f}
+ },
+/* VGA_DAC */
+ {
+  0x00,0x10,0x04,0x0114,
+  {0x11,0x09,0x15,0x2a},
+  0x3a,
+  {0x2e,0x3e,0x2b,0x3b,0x2f,0x3f,0x00,0x05,
+   0x08,0x0b,0x0e,0x11,0x14,0x18,0x1c,0x20,
+   0x24,0x28,0x2d,0x32,0x38,0x3f,0x00,0x10,
+   0x1f},
+  {0x2f,0x3f,0x1f,0x27,0x2f,0x37,0x3f,0x2d,
+   0x31,0x36,0x3a,0x3f,0x00,0x07,0x0e,0x15,
+   0x1c,0x0e,0x11,0x15},
+  {0x18,0x1c,0x14,0x16,0x18,0x1a,0x1c,0x00,
+   0x04}
+ },
+ {
+  0x08,0x0c,0x10,0x0a08,
+  {0x0c,0x0e,0x10,0x0b},
+  0x0c,
+  {0x0d,0x0f,0x10,0x10,0x01,0x08,0x00,0x00,
+   0x00,0x00,0x01,0x00,0x02,0x02,0x01,0x00,
+   0x04,0x04,0x01,0x00,0x05,0x02,0x05,0x00,
+   0x06},
+  {0x01,0x06,0x05,0x06,0x00,0x08,0x01,0x08,
+   0x00,0x07,0x02,0x07,0x06,0x07,0x00,0x00,
+   0x00,0x00,0x00,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x00}
+ },
+/* MD_D */
+ {
+  0x28,0x18,0x08,0x2000,
+  {0x09,0x0f,0x00,0x06},
+  0x63,
+  {0x2d,0x27,0x28,0x90,0x2c,0x80,0xbf,0x1f,
+   0x00,0xc0,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x14,0x00,0x96,0xb9,0xe3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+   0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,
+   0x01,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x0f,
+   0xff}
+ },
+/* MD_E */
+ {
+  0x50,0x18,0x08,0x4000,
+  {0x01,0x0f,0x00,0x06},
+  0x63,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+   0x00,0xc0,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x28,0x00,0x96,0xb9,0xe3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+   0x10,0x11,0x12,0x13,0x14,0x15,0x16,0x17,
+   0x01,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x0f,
+   0xff}
+ },
+/* ExtVGATable */
+ {
+  0x00,0x00,0x00,0x0000,
+  {0x01,0x0f,0x00,0x0e},
+  0x23,
+  {0x5f,0x4f,0x50,0x82,0x54,0x80,0x0b,0x3e,
+   0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+   0xea,0x8c,0xdf,0x28,0x40,0xe7,0x04,0xa3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+   0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,
+   0x01,0x00,0x00,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x40,0x05,0x0f,
+   0xff}
+ },
+/* ROM_SAVEPTR */
+ {
+  0x9f,0x3b,0x00,0x00c0,
+  {0x00,0x00,0x00,0x00},
+  0x00,
+  {0x00,0x00,0x00,0x00,0x00,0x00,0xbb,0x3f,
+   0x00,0xc0,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x00,0x00,0x1a,0x00,0xac,0x3e,0x00,0xc0,
+   0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x00,0x00,0x00,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x00}
+ },
+/* MD_F */
+ {
+  0x50,0x18,0x0e,0x8000,
+  {0x01,0x0f,0x00,0x06},
+  0xa2,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+   0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x82,0x84,0x5d,0x28,0x0f,0x63,0xba,0xe3,
+   0xff},
+  {0x00,0x08,0x00,0x00,0x18,0x18,0x00,0x00,
+   0x00,0x08,0x00,0x00,0x00,0x18,0x00,0x00,
+   0x0b,0x00,0x05,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x05,
+   0xff}
+ },
+/* MD_10 */
+ {
+  0x50,0x18,0x0e,0x8000,
+  {0x01,0x0f,0x00,0x06},
+  0xa3,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+   0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x82,0x84,0x5d,0x28,0x0f,0x63,0xba,0xe3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x14,0x07,
+   0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,
+   0x01,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x0f,
+   0xff}
+ },
+/* MD_0_350 */
+ {
+  0x28,0x18,0x0e,0x0800,
+  {0x09,0x03,0x00,0x02},
+  0xa3,
+  {0x2d,0x27,0x28,0x90,0x2b,0xb1,0xbf,0x1f,
+   0x00,0x4d,0x0b,0x0c,0x00,0x00,0x00,0x00,
+   0x83,0x85,0x5d,0x14,0x1f,0x63,0xba,0xa3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x14,0x07,
+   0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,
+   0x08,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x10,0x0e,0x00,
+   0xff}
+ },
+/* MD_1_350 */
+ {
+  0x28,0x18,0x0e,0x0800,
+  {0x09,0x03,0x00,0x02},
+  0xa3,
+  {0x2d,0x27,0x28,0x90,0x2b,0xa0,0xbf,0x1f,
+   0x00,0x4d,0x0b,0x0c,0x00,0x00,0x00,0x00,
+   0x83,0x85,0x5d,0x14,0x1f,0x63,0xba,0xa3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x14,0x07,
+   0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,
+   0x08,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x10,0x0e,0x00,
+   0xff}
+ },
+/* MD_2_350 */
+ {
+  0x50,0x18,0x0e,0x1000,
+  {0x01,0x03,0x00,0x02},
+  0xa3,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+   0x00,0x4d,0x0b,0x0c,0x00,0x00,0x00,0x00,
+   0x83,0x85,0x5d,0x28,0x1f,0x63,0xba,0xa3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x14,0x07,
+   0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,
+   0x08,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x10,0x0e,0x00,
+   0xff}
+ },
+/* MD_3_350 */
+ {
+  0x50,0x18,0x0e,0x1000,
+  {0x01,0x03,0x00,0x02},
+  0xa3,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+   0x00,0x4d,0x0b,0x0c,0x00,0x00,0x00,0x00,
+   0x83,0x85,0x5d,0x28,0x1f,0x63,0xba,0xa3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x14,0x07,
+   0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,
+   0x08,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x10,0x0e,0x00,
+   0xff}
+ },
+/* MD_0_1_400 */
+ {
+  0x28,0x18,0x10,0x0800,
+  {0x08,0x03,0x00,0x02},
+  0x67,
+  {0x2d,0x27,0x28,0x90,0x2b,0xb1,0xbf,0x1f,
+   0x00,0x4f,0x0d,0x0e,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x14,0x1f,0x96,0xb9,0xa3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x14,0x07,
+   0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,
+   0x0c,0x00,0x0f,0x08},
+  {0x00,0x00,0x00,0x00,0x00,0x10,0x0e,0x00,
+   0xff}
+ },
+/* MD_2_3_400 */
+ {
+  0x50,0x18,0x10,0x1000,
+  {0x00,0x03,0x00,0x02},
+  0x67,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+   0x00,0x4f,0x0d,0x0e,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x28,0x1f,0x96,0xb9,0xa3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x14,0x07,
+   0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,
+   0x0c,0x00,0x0f,0x08},
+  {0x00,0x00,0x00,0x00,0x00,0x10,0x0e,0x00,
+   0xff}
+ },
+/* MD_7_400 */
+ {
+  0x50,0x18,0x10,0x1000,
+  {0x00,0x03,0x00,0x02},
+  0x66,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+   0x00,0x4f,0x0d,0x0e,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x28,0x0f,0x96,0xb9,0xa3,
+   0xff},
+  {0x00,0x08,0x08,0x08,0x08,0x08,0x08,0x08,
+   0x10,0x18,0x18,0x18,0x18,0x18,0x18,0x18,
+   0x0e,0x00,0x0f,0x08},
+  {0x00,0x00,0x00,0x00,0x00,0x10,0x0a,0x00,
+   0xff}
+ },
+/* MD_11 */
+ {
+  0x50,0x1d,0x10,0xa000,
+  {0x01,0x0f,0x00,0x06},
+  0xe3,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0x0b,0x3e,
+   0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+   0xe9,0x8b,0xdf,0x28,0x00,0xe7,0x04,0xc3,
+   0xff},
+  {0x00,0x3f,0x3f,0x3f,0x3f,0x3f,0x3f,0x3f,
+   0x3f,0x3f,0x3f,0x3f,0x3f,0x3f,0x3f,0x3f,
+   0x01,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x01,
+   0xff}
+ },
+/* ExtEGATable */
+ {
+  0x50,0x1d,0x10,0xa000,
+  {0x01,0x0f,0x00,0x06},
+  0xe3,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0x0b,0x3e,
+   0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,
+   0xe9,0x8b,0xdf,0x28,0x00,0xe7,0x04,0xe3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x14,0x07,
+   0x38,0x39,0x3a,0x3b,0x3c,0x3d,0x3e,0x3f,
+   0x01,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x0f,
+   0xff}
+ },
+/* MD_13 */
+ {
+  0x28,0x18,0x08,0x2000,
+  {0x01,0x0f,0x00,0x0e},
+  0x63,
+  {0x5f,0x4f,0x50,0x82,0x55,0x81,0xbf,0x1f,
+   0x00,0x41,0x00,0x00,0x00,0x00,0x00,0x00,
+   0x9c,0x8e,0x8f,0x28,0x40,0x96,0xb9,0xa3,
+   0xff},
+  {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,
+   0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f,
+   0x41,0x00,0x0f,0x00},
+  {0x00,0x00,0x00,0x00,0x00,0x40,0x05,0x0f,
+   0xff}
+ }
+};
+
+XGI_TimingHStruct XGI_TimingH[]=
+{{{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}}};
+
+XGI_TimingVStruct XGI_TimingV[]=
+{{{0x00,0x00,0x00,0x00,0x00,0x00,0x00}}};
+
+XGI_XG21CRT1Struct XGI_UpdateCRT1Table[]=
+{
+ {0x01,0x27,0x91,0x8f,0xc0},    /* 00 */
+ {0x03,0x4f,0x83,0x8f,0xc0},    /* 01 */
+ {0x05,0x27,0x91,0x8f,0xc0},    /* 02 */
+ {0x06,0x4f,0x83,0x8f,0xc0},    /* 03 */
+ {0x07,0x4f,0x83,0x8f,0xc0},    /* 04 */
+ {0x0d,0x27,0x91,0x8f,0xc0},    /* 05 */
+ {0x0e,0x4f,0x83,0x8f,0xc0},    /* 06 */
+ {0x0f,0x4f,0x83,0x5d,0xc0},    /* 07 */
+ {0x10,0x4f,0x83,0x5d,0xc0},    /* 08 */
+ {0x11,0x4f,0x83,0xdf,0x0c},    /* 09 */
+ {0x12,0x4f,0x83,0xdf,0x0c},    /* 10 */
+ {0x13,0x4f,0x83,0x8f,0xc0},    /* 11 */
+ {0x2e,0x4f,0x83,0xdf,0x0c},    /* 12 */
+ {0x2e,0x4f,0x87,0xdf,0xc0},    /* 13 */
+ {0x2f,0x4f,0x83,0x8f,0xc0},    /* 14 */
+ {0x50,0x27,0x91,0xdf,0x0c},    /* 15 */
+ {0x59,0x27,0x91,0x8f,0xc0}     /* 16 */
+};
+
+XGI_CRT1TableStruct XGI_CRT1Table[]=
+{
+ {{0x2d,0x28,0x90,0x2c,0x90,0x00,0x04,0x00,
+    0xbf,0x1f,0x9c,0x8e,0x96,0xb9,0x30}}, /* 0x0 */
+ {{0x2d,0x28,0x90,0x2c,0x90,0x00,0x04,0x00,
+    0x0b,0x3e,0xe9,0x8b,0xe7,0x04,0x00}}, /* 0x1 */
+ {{0x3D,0x31,0x81,0x37,0x1F,0x00,0x05,0x00,
+    0x72,0xF0,0x58,0x8C,0x57,0x73,0xA0}}, /* 0x2 */
+ {{0x4F,0x3F,0x93,0x45,0x0D,0x00,0x01,0x00,
+    0x24,0xF5,0x02,0x88,0xFF,0x25,0x90}}, /* 0x3 */
+ {{0x5F,0x50,0x82,0x55,0x81,0x00,0x05,0x00,
+    0xBF,0x1F,0x9C,0x8E,0x96,0xB9,0x30}}, /* 0x4 */
+ {{0x5F,0x50,0x82,0x55,0x81,0x00,0x05,0x00,
+    0x0B,0x3E,0xE9,0x8B,0xE7,0x04,0x00}}, /* 0x5 */
+ {{0x63,0x50,0x86,0x56,0x9B,0x00,0x01,0x00,
+    0x06,0x3E,0xE8,0x8B,0xE7,0xFF,0x10}}, /* 0x6 */
+ {{0x64,0x4F,0x88,0x55,0x9D,0x00,0x01,0x00,
+    0xF2,0x1F,0xE0,0x83,0xDF,0xF3,0x10}}, /* 0x7 */
+ {{0x63,0x4F,0x87,0x5A,0x81,0x00,0x05,0x00,
+    0xFB,0x1F,0xE0,0x83,0xDF,0xFC,0x10}}, /* 0x8 */
+ {{0x65,0x4F,0x89,0x58,0x80,0x00,0x05,0x60,
+    0xFB,0x1F,0xE0,0x83,0xDF,0xFC,0x80}}, /* 0x9 */
+ {{0x65,0x4F,0x89,0x58,0x80,0x00,0x05,0x60,
+    0x01,0x3E,0xE0,0x83,0xDF,0x02,0x80}}, /* 0xa */
+ {{0x67,0x4F,0x8B,0x58,0x81,0x00,0x05,0x60,
+    0x0D,0x3E,0xE0,0x83,0xDF,0x0E,0x90}}, /* 0xb */
+ {{0x65,0x4F,0x89,0x57,0x9F,0x00,0x01,0x00,
+   0xFB,0x1F,0xE6,0x8A,0xDF,0xFC,0x10}}, /* 0xc */
+ {{0x7B,0x63,0x9F,0x6A,0x93,0x00,0x05,0x00,    /* ; 0D (800x600,56Hz) */
+    0x6F,0xF0,0x58,0x8A,0x57,0x70,0xA0}},         /* ; (VCLK 36.0MHz) */
+ {{0x7F,0x63,0x83,0x6C,0x1C,0x00,0x06,0x00,    /* ; 0E (800x600,60Hz) */
+    0x72,0xF0,0x58,0x8C,0x57,0x73,0xA0}},         /* ; (VCLK 40.0MHz) */
+ {{0x7D,0x63,0x81,0x6E,0x1D,0x00,0x06,0x00,    /* ; 0F (800x600,72Hz) */
+    0x98,0xF0,0x7C,0x82,0x57,0x99,0x80}},         /* ; (VCLK 50.0MHz) */
+ {{0x7F,0x63,0x83,0x69,0x13,0x00,0x06,0x00,    /* ; 10 (800x600,75Hz) */
+    0x6F,0xF0,0x58,0x8B,0x57,0x70,0xA0}},         /* ; (VCLK 49.5MHz) */
+ {{0x7E,0x63,0x82,0x6B,0x13,0x00,0x06,0x00,    /* ; 11 (800x600,85Hz) */
+    0x75,0xF0,0x58,0x8B,0x57,0x76,0xA0}},         /* ; (VCLK 56.25MHz) */
+ {{0x81,0x63,0x85,0x6D,0x18,0x00,0x06,0x60,    /* ; 12 (800x600,100Hz) */
+    0x7A,0xF0,0x58,0x8B,0x57,0x7B,0xA0}},         /* ; (VCLK 75.8MHz) */
+ {{0x83,0x63,0x87,0x6E,0x19,0x00,0x06,0x60,    /* ; 13 (800x600,120Hz) */
+    0x81,0xF0,0x58,0x8B,0x57,0x82,0xA0}},         /* ; (VCLK 79.411MHz) */
+ {{0x85,0x63,0x89,0x6F,0x1A,0x00,0x06,0x60,    /* ; 14 (800x600,160Hz) */
+   0x91,0xF0,0x58,0x8B,0x57,0x92,0xA0}},         /* ; (VCLK 105.822MHz) */
+ {{0x99,0x7F,0x9D,0x84,0x1A,0x00,0x02,0x00,
+    0x96,0x1F,0x7F,0x83,0x7F,0x97,0x10}}, /* 0x15 */
+ {{0xA3,0x7F,0x87,0x86,0x97,0x00,0x02,0x00,
+    0x24,0xF5,0x02,0x88,0xFF,0x25,0x90}}, /* 0x16 */
+ {{0xA1,0x7F,0x85,0x86,0x97,0x00,0x02,0x00,
+    0x24,0xF5,0x02,0x88,0xFF,0x25,0x90}}, /* 0x17 */
+ {{0x9F,0x7F,0x83,0x85,0x91,0x00,0x02,0x00,
+    0x1E,0xF5,0x00,0x83,0xFF,0x1F,0x90}}, /* 0x18 */
+ {{0xA7,0x7F,0x8B,0x89,0x95,0x00,0x02,0x00,
+    0x26,0xF5,0x00,0x83,0xFF,0x27,0x90}}, /* 0x19 */
+ {{0xA9,0x7F,0x8D,0x8C,0x9A,0x00,0x02,0x62,
+    0x2C,0xF5,0x00,0x83,0xFF,0x2D,0x14}}, /* 0x1a */
+ {{0xAB,0x7F,0x8F,0x8D,0x9B,0x00,0x02,0x62,
+    0x35,0xF5,0x00,0x83,0xFF,0x36,0x14}}, /* 0x1b */
+ {{0xCF,0x9F,0x93,0xB2,0x01,0x00,0x03,0x00,
+    0x14,0xBA,0x00,0x83,0xFF,0x15,0x00}}, /* 0x1c */
+ {{0xCE,0x9F,0x92,0xA9,0x17,0x00,0x07,0x00,
+    0x28,0x5A,0x00,0x83,0xFF,0x29,0x89}}, /* 0x1d */
+ {{0xCE,0x9F,0x92,0xA5,0x17,0x00,0x07,0x00,
+    0x28,0x5A,0x00,0x83,0xFF,0x29,0x89}}, /* 0x1e */
+ {{0xD3,0x9F,0x97,0xAB,0x1F,0x00,0x07,0x00,
+    0x2E,0x5A,0x00,0x83,0xFF,0x2F,0x89}}, /* 0x1f */
+ {{0x09,0xC7,0x8D,0xD3,0x0B,0x01,0x04,0x00,
+    0xE0,0x10,0xB0,0x83,0xAF,0xE1,0x2F}}, /* 0x20 */
+ {{0x09,0xC7,0x8D,0xD3,0x0B,0x01,0x04,0x00,
+    0xE0,0x10,0xB0,0x83,0xAF,0xE1,0x2F}}, /* 0x21 */
+ {{0x09,0xC7,0x8D,0xD3,0x0B,0x01,0x04,0x00,
+    0xE0,0x10,0xB0,0x83,0xAF,0xE1,0x2F}}, /* 0x22 */
+ {{0x09,0xC7,0x8D,0xD3,0x0B,0x01,0x04,0x00,
+    0xE0,0x10,0xB0,0x83,0xAF,0xE1,0x2F}}, /* 0x23 */
+ {{0x09,0xC7,0x8D,0xD3,0x0B,0x01,0x04,0x00,
+    0xE0,0x10,0xB0,0x83,0xAF,0xE1,0x2F}}, /* 0x24 */
+ {{0x09,0xC7,0x8D,0xD3,0x0B,0x01,0x04,0x00,
+    0xE0,0x10,0xB0,0x83,0xAF,0xE1,0x2F}}, /* 0x25 */
+ {{0x09,0xC7,0x8D,0xD3,0x0B,0x01,0x04,0x00,
+    0xE0,0x10,0xB0,0x83,0xAF,0xE1,0x2F}}, /* 0x26 */
+ {{0x40,0xEF,0x84,0x03,0x1D,0x41,0x01,0x00,
+    0xDA,0x1F,0xA0,0x83,0x9F,0xDB,0x1F}}, /* 0x27 */
+ {{0x43,0xEF,0x87,0x06,0x00,0x41,0x05,0x62,
+    0xD4,0x1F,0xA0,0x83,0x9F,0xD5,0x9F}}, /* 0x28 */
+ {{0x45,0xEF,0x89,0x07,0x01,0x41,0x05,0x62,
+    0xD9,0x1F,0xA0,0x83,0x9F,0xDA,0x9F}}, /* 0x29 */
+ {{0x40,0xEF,0x84,0x03,0x1D,0x41,0x01,0x00,
+    0xDA,0x1F,0xA0,0x83,0x9F,0xDB,0x1F}}, /* 0x2a */
+ {{0x40,0xEF,0x84,0x03,0x1D,0x41,0x01,0x00,
+    0xDA,0x1F,0xA0,0x83,0x9F,0xDB,0x1F}}, /* 0x2b */
+ {{0x40,0xEF,0x84,0x03,0x1D,0x41,0x01,0x00,
+    0xDA,0x1F,0xA0,0x83,0x9F,0xDB,0x1F}}, /* 0x2c */
+ {{0x59,0xFF,0x9D,0x17,0x13,0x41,0x05,0x44,
+    0x33,0xBA,0x00,0x83,0xFF,0x34,0x0F}}, /* 0x2d */
+ {{0x5B,0xFF,0x9F,0x18,0x14,0x41,0x05,0x44,
+    0x38,0xBA,0x00,0x83,0xFF,0x39,0x0F}}, /* 0x2e */
+ {{0x5B,0xFF,0x9F,0x18,0x14,0x41,0x05,0x44,
+    0x3D,0xBA,0x00,0x83,0xFF,0x3E,0x0F}}, /* 0x2f */
+ {{0x5D,0xFF,0x81,0x19,0x95,0x41,0x05,0x44,
+    0x41,0xBA,0x00,0x84,0xFF,0x42,0x0F}}, /* 0x30 */
+ {{0x55,0xFF,0x99,0x0D,0x0C,0x41,0x05,0x00,
+    0x3E,0xBA,0x00,0x84,0xFF,0x3F,0x0F}}, /* 0x31 */
+ {{0x7F,0x63,0x83,0x6C,0x1C,0x00,0x06,0x00,
+    0x72,0xBA,0x27,0x8B,0xDF,0x73,0x80}}, /* 0x32 */
+ {{0x7F,0x63,0x83,0x69,0x13,0x00,0x06,0x00,
+    0x6F,0xBA,0x26,0x89,0xDF,0x6F,0x80}}, /* 0x33 */
+ {{0x7F,0x63,0x82,0x6B,0x13,0x00,0x06,0x00,
+    0x75,0xBA,0x29,0x8C,0xDF,0x75,0x80}}, /* 0x34 */
+ {{0xA3,0x7F,0x87,0x86,0x97,0x00,0x02,0x00,
+    0x24,0xF1,0xAF,0x85,0x3F,0x25,0xB0}}, /* 0x35 */
+ {{0x9F,0x7F,0x83,0x85,0x91,0x00,0x02,0x00,
+    0x1E,0xF1,0xAD,0x81,0x3F,0x1F,0xB0}}, /* 0x36 */
+ {{0xA7,0x7F,0x88,0x89,0x15,0x00,0x02,0x00,
+    0x26,0xF1,0xB1,0x85,0x3F,0x27,0xB0}}, /* 0x37 */
+ {{0xCE,0x9F,0x92,0xA9,0x17,0x00,0x07,0x00,
+    0x28,0xC4,0x7A,0x8E,0xCF,0x29,0xA1}}, /* 0x38 */
+ {{0xCE,0x9F,0x92,0xA5,0x17,0x00,0x07,0x00,
+    0x28,0xD4,0x7A,0x8E,0xCF,0x29,0xA1}}, /* 0x39 */
+ {{0xD3,0x9F,0x97,0xAB,0x1F,0x00,0x07,0x00,
+    0x2E,0xD4,0x7D,0x81,0xCF,0x2F,0xA1}}, /* 0x3a */
+ {{0xDC,0x9F,0x00,0xAB,0x19,0x00,0x07,0x00,
+    0xE6,0xEF,0xC0,0xC3,0xBF,0xE7,0x90}}, /* 0x3b */
+ {{0x6B,0x59,0x8F,0x5E,0x8C,0x00,0x05,0x00,
+    0x0B,0x3E,0xE9,0x8B,0xE7,0x04,0x00}}, /* 0x3c */
+ {{0x7B,0x63,0x9F,0x6A,0x93,0x00,0x05,0x00,
+    0x6F,0xF0,0x58,0x8A,0x57,0x70,0xA0}}, /* 0x3d */
+ {{0x86,0x6A,0x8a,0x74,0x06,0x00,0x02,0x00,
+    0x8c,0x15,0x4f,0x83,0xef,0x8d,0x30}}, /* 0x3e */
+ {{0x81,0x6A,0x85,0x70,0x00,0x00,0x02,0x00,
+    0x0f,0x3e,0xeb,0x8e,0xdf,0x10,0x00}}, /* 0x3f */
+ {{0xCE,0x9F,0x92,0xA9,0x17,0x00,0x07,0x00,
+    0x20,0xF5,0x03,0x88,0xFF,0x21,0x90}}, /* 0x40 */
+ {{0xE6,0xAE,0x8A,0xBD,0x90,0x00,0x03,0x00,
+    0x3D,0x10,0x1A,0x8D,0x19,0x3E,0x2F}}, /* 0x41 */
+ {{0xB9,0x8F,0x9D,0x9B,0x8A,0x00,0x06,0x00,
+    0x7D,0xFF,0x60,0x83,0x5F,0x7E,0x90}}, /* 0x42 */
+ {{0xC3,0x8F,0x87,0x9B,0x0B,0x00,0x07,0x00,
+    0x82,0xFF,0x60,0x83,0x5F,0x83,0x90}},  /* 0x43 */
+ {{0xAD,0x7F,0x91,0x8E,0x9C,0x00,0x02,0x82,
+    0x49,0xF5,0x00,0x83,0xFF,0x4A,0x90}},  /* 0x44 */
+ {{0xCD,0x9F,0x91,0xA7,0x19,0x00,0x07,0x60,
+    0xE6,0xFF,0xC0,0x83,0xBF,0xE7,0x90}},  /* 0x45 */
+ {{0xD3,0x9F,0x97,0xAB,0x1F,0x00,0x07,0x60,
+    0xF1,0xFF,0xC0,0x83,0xBF,0xF2,0x90}},  /* 0x46 */
+ {{0xD7,0x9F,0x9B,0xAC,0x1E,0x00,0x07,0x00,
+    0x03,0xDE,0xC0,0x84,0xBF,0x04,0x90}}  /* 0x47 */
+};
+
+XGI330_CHTVRegDataStruct XGI_CHTVRegUNTSC[] = {
+                /* Index:000h,001h,002h,004h,003h,005h,006h,007h,008h,015h,01Fh,00Ch,00Dh,00Eh,00Fh,010h */
+                {{      0x4A,0x77,0xBB,0x94,0x84,0x48,0xFE,0x50,0x04,0x00,0x80,0x00,0x00,0x00,0x00,0x01  }},/* 00 (640x200,640x400) */
+                {{      0x4A,0x77,0xBB,0x94,0x84,0x48,0xFE,0x50,0x04,0x00,0x80,0x00,0x00,0x00,0x00,0x01  }},/* 01 (640x350) */
+                {{      0x4A,0x77,0xBB,0x94,0x84,0x48,0xFE,0x50,0x04,0x00,0x80,0x00,0x00,0x00,0x00,0x01  }},/* 02 (720x400) */
+                {{      0x4A,0x77,0xBB,0x94,0x84,0x48,0xFE,0x50,0x04,0x00,0x80,0x00,0x00,0x00,0x00,0x01  }},/* 03 (720x350) */
+                {{      0x6A,0x77,0xBB,0x6E,0x84,0x2E,0x02,0x5A,0x04,0x00,0x80,0x20,0x7E,0x80,0x97,0x00  }},/* 04 (640x480) ;;5/6/02 */
+                {{      0xCF,0x77,0xB7,0xC8,0x84,0x3B,0x02,0x5A,0x04,0x00,0x80,0x19,0x88,0xAE,0xA3,0x00  }},/* 05 (800x600) ;;1/12/02 */
+                {{      0xEE,0x77,0xBB,0x66,0x87,0x32,0x01,0x5A,0x04,0x00,0x80,0x1B,0xD4,0x2F,0x6F,0x00  }}/* 06 (1024x768) ;;5/6/02 */
+          };
+
+XGI330_CHTVRegDataStruct XGI_CHTVRegONTSC[]= {
+                /* Index:000h,001h,002h,004h,003h,005h,006h,007h,008h,015h,01Fh,00Ch,00Dh,00Eh,00Fh,010h */
+                {{      0x49,0x77,0xBB,0x7B,0x84,0x34,0x00,0x50,0x04,0x00,0x80,0x00,0x00,0x00,0x00,0x01  }},/* 00 (640x200,640x400) */
+                {{      0x49,0x77,0xBB,0x7B,0x84,0x34,0x00,0x50,0x04,0x00,0x80,0x00,0x00,0x00,0x00,0x01  }},/* 01 (640x350) */
+                {{      0x49,0x77,0xBB,0x7B,0x84,0x34,0x00,0x50,0x04,0x00,0x80,0x00,0x00,0x00,0x00,0x01  }},/* 02 (720x400) */
+                {{      0x49,0x77,0xBB,0x7B,0x84,0x34,0x00,0x50,0x04,0x00,0x80,0x00,0x00,0x00,0x00,0x01  }},/* 03 (720x350) */
+                {{      0x69,0x77,0xBB,0x6E,0x84,0x1E,0x00,0x5A,0x04,0x00,0x80,0x25,0x1A,0x80,0x26,0x00  }},/* 04 (640x480) ;;5/6/02 */
+                {{      0xCE,0x77,0xB7,0xB6,0x83,0x2C,0x02,0x5A,0x04,0x00,0x80,0x1C,0x00,0x82,0x97,0x00  }},/* 05 (800x600) ;;5/6/02 */
+                {{      0xED,0x77,0xBB,0x66,0x8C,0x21,0x02,0x5A,0x04,0x00,0x80,0x1F,0xA0,0x7E,0x73,0x00  }}/* 06 (1024x768) ;;5/6/02 */
+          };
+
+XGI330_CHTVRegDataStruct XGI_CHTVRegUPAL[]=  {
+                /* Index:000h,001h,002h,004h,003h,005h,006h,007h,008h,015h,01Fh,00Ch,00Dh,00Eh,00Fh,010h */
+                {{      0x41,0x7F,0xB7,0x34,0xAD,0x50,0x34,0x83,0x05,0x00,0x80,0x00,0x00,0x00,0x00,0x01  }},/* ; 00 (640x200,640x400) */
+                {{      0x41,0x7F,0xB7,0x80,0x85,0x50,0x00,0x83,0x05,0x00,0x80,0x00,0x00,0x00,0x00,0x01  }},/* ; 01 (640x350) */
+                {{      0x41,0x7F,0xB7,0x34,0xAD,0x50,0x34,0x83,0x05,0x00,0x80,0x00,0x00,0x00,0x00,0x01  }},/* ; 02 (720x400) */
+                {{      0x41,0x7F,0xB7,0x12,0x85,0x50,0x00,0x83,0x05,0x00,0x80,0x00,0x00,0x00,0x00,0x01  }},/* ; 03 (720x350) */
+                {{      0x61,0x7F,0xB7,0x99,0x84,0x35,0x04,0x5A,0x05,0x00,0x80,0x26,0x2A,0x55,0x5D,0x00  }},/* ; 04 (640x480) */
+                {{      0xC3,0x7F,0xB7,0x7A,0x84,0x40,0x02,0x5A,0x05,0x00,0x80,0x1F,0x84,0x3D,0x28,0x00  }},/* ; 05 (800x600) ;;1/12/02 */
+                {{      0xE5,0x7F,0xB7,0x1D,0xA7,0x3E,0x04,0x5A,0x05,0x00,0x80,0x20,0x3E,0xE4,0x22,0x00  }}/* ; 06 (1024x768) ;;1/12/02 */
+          };
+
+XGI330_CHTVRegDataStruct XGI_CHTVRegOPAL[]={
+                /* Index:000,0x01,0x02,0x04,0x03,0x05,0x06,0x07,0x08,0x15,0x1F,0x0C,0x0D,0x0E,0x0F,0x10h */
+                {{      0x41,0x7F,0xB7,0x36,0xAD,0x50,0x34,0x83,0x05,0x00,0x80,0x00,0x00,0x00,0x00,0x01 }},/* 00 (640x200,640x400) */
+                {{      0x41,0x7F,0xB7,0x86,0x85,0x50,0x00,0x83,0x05,0x00,0x80,0x00,0x00,0x00,0x00,0x01 }},/* 01 (640x350) */
+                {{      0x41,0x7F,0xB7,0x36,0xAD,0x50,0x34,0x83,0x05,0x00,0x80,0x00,0x00,0x00,0x00,0x01 }},/* 02 (720x400) */
+                {{      0x41,0x7F,0xB7,0x86,0x85,0x50,0x00,0x83,0x05,0x00,0x80,0x00,0x00,0x00,0x00,0x01 }},/* 03 (720x350) */
+                {{      0x61,0x7F,0xB7,0x99,0x84,0x35,0x04,0x5A,0x05,0x00,0x80,0x26,0x2A,0x55,0x5D,0x00 }},/* 04 (640x480) */
+                {{      0xC1,0x7F,0xB7,0x4D,0x8C,0x1E,0x31,0x5A,0x05,0x00,0x80,0x26,0x78,0x19,0x34,0x00 }},/* 05 (800x600) ;;1/12/02 */
+                {{      0xE4,0x7F,0xB7,0x1E,0xAF,0x29,0x37,0x5A,0x05,0x00,0x80,0x25,0x8C,0xB2,0x2A,0x00 }}/* 06 (1024x768) ;;1/12/02 */
+             };
+
+UCHAR XGI_CH7017LV1024x768[]={0x60,0x02,0x00,0x07,0x40,0xED,0xA3,
+                                        0xC8,0xC7,0xAC,0xE0,0x02};
+UCHAR XGI_CH7017LV1400x1050[]={0x60,0x03,0x11,0x00,0x40,0xE3,0xAD,
+                                        0xDB,0xF6,0xAC,0xE0,0x02};
+
+
+/*add for new UNIVGABIOS*/
+XGI330_LCDDataStruct  XGI_StLCD1024x768Data[]=
+{
+ {   62,  25, 800, 546,1344, 806},
+ {   32,  15, 930, 546,1344, 806},
+ {   62,  25, 800, 546,1344, 806}, /* chiawen for dot9 -> dot8 */
+ {  104,  45, 945, 496,1344, 806},
+ {   62,  25, 800, 546,1344, 806},
+ {   31,  18,1008, 624,1344, 806},
+ {    1,   1,1344, 806,1344, 806}
+};
+
+XGI330_LCDDataStruct  XGI_ExtLCD1024x768Data[]=
+{
+ {   42,  25,1536, 419,1344, 806}, /* { 12, 5, 896, 512,1344, 806}, // alan 09/12/2003 */
+ {   48,  25,1536, 369,1344, 806}, /* { 12, 5, 896, 510,1344, 806}, // alan 09/12/2003 */
+ {   42,  25,1536, 419,1344, 806}, /* { 32, 15,1008, 505,1344, 806}, // alan 09/12/2003 */
+ {   48,  25,1536, 369,1344, 806}, /* { 32, 15,1008, 514,1344, 806}, // alan 09/12/2003 */
+ {   12,   5, 896, 500,1344, 806},
+ {   42,  25,1024, 625,1344, 806},
+ {    1,   1,1344, 806,1344, 806},
+ {   12,   5, 896, 500,1344, 806},
+ {   42,  25,1024, 625,1344, 806},
+ {    1,   1,1344, 806,1344, 806},
+ {   12,   5, 896, 500,1344, 806},
+ {   42,  25,1024, 625,1344, 806},
+ {    1,   1,1344, 806,1344, 806}
+};
+
+/*XGI330_LCDDataStruct  XGI_St2LCD1024x768Data[]=
+{
+ {   62,  25, 800, 546,1344, 806},
+ {   32,  15, 930, 546,1344, 806},
+ {   62,  25, 800, 546,1344, 806},
+ {  104,  45, 945, 496,1344, 806},
+ {   62,  25, 800, 546,1344, 806},
+ {   31,  18,1008, 624,1344, 806},
+ {    1,   1,1344, 806,1344, 806}
+};*/
+
+XGI330_LCDDataStruct  XGI_CetLCD1024x768Data[]=
+{
+        {         1,1,1344,806,1344,806           }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {         1,1,1344,806,1344,806           }, /* 01 (320x350,640x350) */
+        {         1,1,1344,806,1344,806           }, /* 02 (360x400,720x400) */
+        {         1,1,1344,806,1344,806           }, /* 03 (720x350) */
+        {         1,1,1344,806,1344,806           }, /* 04 (640x480x60Hz) */
+        {         1,1,1344,806,1344,806           }, /* 05 (800x600x60Hz) */
+        {         1,1,1344,806,1344,806           }  /* 06 (1024x768x60Hz) */
+};
+
+XGI330_LCDDataStruct  XGI_StLCD1280x1024Data[]=
+{
+ {   22,   5, 800, 510,1650,1088},
+ {   22,   5, 800, 510,1650,1088},
+ {  176,  45, 900, 510,1650,1088},
+ {  176,  45, 900, 510,1650,1088},
+ {   22,   5, 800, 510,1650,1088},
+ {   13,   5,1024, 675,1560,1152},
+ {   16,   9,1266, 804,1688,1072},
+ {    1,   1,1688,1066,1688,1066}
+};
+
+XGI330_LCDDataStruct  XGI_ExtLCD1280x1024Data[]=
+{
+ {  211,  60,1024, 501,1688,1066},
+ {  211,  60,1024, 508,1688,1066},
+ {  211,  60,1024, 501,1688,1066},
+ {  211,  60,1024, 508,1688,1066},
+ {  211,  60,1024, 500,1688,1066},
+ {  211,  75,1024, 625,1688,1066},
+ {  211, 120,1280, 798,1688,1066},
+ {    1,   1,1688,1066,1688,1066}
+};
+
+XGI330_LCDDataStruct  XGI_St2LCD1280x1024Data[]=
+{
+ {   22,   5, 800, 510,1650,1088},
+ {   22,   5, 800, 510,1650,1088},
+ {  176,  45, 900, 510,1650,1088},
+ {  176,  45, 900, 510,1650,1088},
+ {   22,   5, 800, 510,1650,1088},
+ {   13,   5,1024, 675,1560,1152},
+ {   16,   9,1266, 804,1688,1072},
+ {    1,   1,1688,1066,1688,1066}
+};
+
+XGI330_LCDDataStruct  XGI_CetLCD1280x1024Data[]=
+{
+        {         1,1,1688,1066,1688,1066         }, /* 00 (320x200,320x400,640x200,640x400) */
+        {         1,1,1688,1066,1688,1066         }, /* 01 (320x350,640x350) */
+        {         1,1,1688,1066,1688,1066         }, /* 02 (360x400,720x400) */
+        {         1,1,1688,1066,1688,1066         }, /* 03 (720x350) */
+        {         1,1,1688,1066,1688,1066         }, /* 04 (640x480x60Hz) */
+        {         1,1,1688,1066,1688,1066         }, /* 05 (800x600x60Hz) */
+        {         1,1,1688,1066,1688,1066         }, /* 06 (1024x768x60Hz) */
+        {         1,1,1688,1066,1688,1066         }, /* 07 (1280x1024x60Hz) */
+        {         1,1,1688,1066,1688,1066         } /* 08 (1400x1050x60Hz) */
+};
+
+XGI330_LCDDataStruct  XGI_StLCD1400x1050Data[]=
+{
+        {         211,100,2100,408,1688,1066      }, /* 00 (320x200,320x400,640x200,640x400) */
+        {         211,64,1536,358,1688,1066       }, /* 01 (320x350,640x350) */
+        {         211,100,2100,408,1688,1066      }, /* 02 (360x400,720x400) */
+        {         211,64,1536,358,1688,1066       }, /* 03 (720x350) */
+        {         211,48,840,488,1688,1066        }, /* 04 (640x480x60Hz) */
+        {         211,72,1008,609,1688,1066       }, /* 05 (800x600x60Hz) */
+        {         211,128,1400,776,1688,1066      }, /* 06 (1024x768x60Hz) */
+        {         1,1,1688,1066,1688,1066         }, /* 07 (1280x1024x60Hz w/o Scaling) */
+        {         1,1,1688,1066,1688,1066         }  /* 08 (1400x1050x60Hz) */
+};
+
+XGI330_LCDDataStruct  XGI_ExtLCD1400x1050Data[]=
+{
+        {         211,100,2100,408,1688,1066      }, /* 00 (320x200,320x400,640x200,640x400) */
+        {         211,64,1536,358,1688,1066       }, /* 01 (320x350,640x350) */
+        {         211,100,2100,408,1688,1066      }, /* 02 (360x400,720x400) */
+        {         211,64,1536,358,1688,1066       }, /* 03 (720x350) */
+        {         211,48,840,488,1688,1066        }, /* 04 (640x480x60Hz) */
+        {         211,72,1008,609,1688,1066       }, /* 05 (800x600x60Hz) */
+        {         211,128,1400,776,1688,1066      }, /* 06 (1024x768x60Hz) */
+        {         1,1,1688,1066,1688,1066         }, /* 07 (1280x1024x60Hz w/o Scaling) */
+        {         1,1,1688,1066,1688,1066         }  /* 08 (1400x1050x60Hz) */
+};
+
+XGI330_LCDDataStruct  XGI_ExtLCD1600x1200Data[]=
+{
+        {         4,1,1620,420,2160,1250          }, /* { 3,1,2160,425,2160,1250 }, // 00 (320x200,320x400,640x200,640x400) // alan 10/14/2003 */
+        {         27,7,1920,375,2160,1250         }, /* 01 (320x350,640x350) */
+        {         4,1,1620,420,2160,1250          }, /* { 3,1,2160,425,2160,1250 }, // 02 (360x400,720x400) // alan 10/14/2003 */
+        {         27,7,1920,375,2160,1250         }, /* 03 (720x350) */
+        {         27,4,800,500,2160,1250          }, /* 04 (640x480x60Hz) */
+        {         4,1,1080,625,2160,1250          }, /* 05 (800x600x60Hz) */
+        {         5,2,1350,800,2160,1250          }, /* 06 (1024x768x60Hz) */
+        {         27,16,1500,1064,2160,1250       }, /* 07 (1280x1024x60Hz) */
+        {         9,7,1920,1106,2160,1250         }, /* 08 (1400x1050x60Hz) */
+        {         1,1,2160,1250,2160,1250         }  /* 09 (1600x1200x60Hz) ;302lv */
+};
+
+XGI330_LCDDataStruct  XGI_StLCD1600x1200Data[]=
+{
+        {         27,4,800,500,2160,1250          },/* 00 (320x200,320x400,640x200,640x400) */
+        {         27,4,800,500,2160,1250          },/* 01 (320x350,640x350) */
+        {         27,4,800,500,2160,1250          },/* 02 (360x400,720x400) */
+        {         27,4,800,500,2160,1250          },/* 03 (720x350) */
+        {         27,4,800,500,2160,1250          },/* 04 (320x240,640x480) */
+        {         4,1,1080,625,2160,1250          },/* 05 (400x300,800x600) */
+        {         5,2,1350,800,2160,1250          },/* 06 (512x384,1024x768) */
+        {         135,88,1600,1100,2160,1250      },/* 07 (1280x1024) */
+        {         1,1,1800,1500,2160,1250         },/* 08 (1400x1050) */
+        {         1,1,2160,1250,2160,1250         } /* 09 (1600x1200) */
+};
+
+XGI330_LCDDataStruct  XGI_CetLCD1400x1050Data[]=
+{
+        {         1,1,1688,1066,1688,1066         }, /* 00 (320x200,320x400,640x200,640x400) */
+        {         1,1,1688,1066,1688,1066         }, /* 01 (320x350,640x350) */
+        {         1,1,1688,1066,1688,1066         }, /* 02 (360x400,720x400) */
+        {         1,1,1688,1066,1688,1066         }, /* 03 (720x350) */
+        {         1,1,1688,1066,1688,1066         }, /* 04 (640x480x60Hz) */
+        {         1,1,1688,1066,1688,1066         }, /* 05 (800x600x60Hz) */
+        {         1,1,1688,1066,1688,1066         }, /* 06 (1024x768x60Hz) */
+        {         1,1,1688,1066,1688,1066         }, /* 07 (1280x1024x60Hz) */
+        {         1,1,1688,1066,1688,1066         }  /* 08 (1400x1050x60Hz) */
+};
+
+XGI330_LCDDataStruct  XGI_NoScalingData[]=
+{
+ {    1,   1, 800, 449, 800, 449},
+ {    1,   1, 800, 449, 800, 449},
+ {    1,   1, 900, 449, 900, 449},
+ {    1,   1, 900, 449, 900, 449},
+ {    1,   1, 800, 525, 800, 525},
+ {    1,   1,1056, 628,1056, 628},
+ {    1,   1,1344, 806,1344, 806},
+ {    1,   1,1688,1066,1688,1066}
+};
+
+XGI330_LCDDataStruct  XGI_ExtLCD1024x768x75Data[]=
+{
+        {42,25,1536,419,1344,806 }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {48,25,1536,369,1344,806 }, /* ; 01 (320x350,640x350) */
+        {42,25,1536,419,1344,806 }, /* ; 02 (360x400,720x400) */
+        {48,25,1536,369,1344,806 }, /* ; 03 (720x350) */
+        {8,5,1312,500,1312,800   }, /* ; 04 (640x480x75Hz) */
+        {41,25,1024,625,1312,800 }, /* ; 05 (800x600x75Hz) */
+        {1,1,1312,800,1312,800   }  /* ; 06 (1024x768x75Hz) */
+};
+
+XGI330_LCDDataStruct  XGI_StLCD1024x768x75Data[]=
+{
+        {42,25,1536,419,1344,806 }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {48,25,1536,369,1344,806 }, /* ; 01 (320x350,640x350) */
+        {42,25,1536,419,1344,806 }, /* ; 02 (360x400,720x400) */
+        {48,25,1536,369,1344,806 }, /* ; 03 (720x350) */
+        {8,5,1312,500,1312,800   }, /* ; 04 (640x480x75Hz) */
+        {41,25,1024,625,1312,800 }, /* ; 05 (800x600x75Hz) */
+        {1,1,1312,800,1312,800   }  /* ; 06 (1024x768x75Hz) */
+};
+
+XGI330_LCDDataStruct  XGI_CetLCD1024x768x75Data[]=
+{
+        {1,1,1312,800,1312,800}, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {1,1,1312,800,1312,800}, /* ; 01 (320x350,640x350) */
+        {1,1,1312,800,1312,800}, /* ; 02 (360x400,720x400) */
+        {1,1,1312,800,1312,800}, /* ; 03 (720x350) */
+        {1,1,1312,800,1312,800}, /* ; 04 (640x480x75Hz) */
+        {1,1,1312,800,1312,800}, /* ; 05 (800x600x75Hz) */
+        {1,1,1312,800,1312,800} /* ; 06 (1024x768x75Hz) */
+};
+
+XGI330_LCDDataStruct  XGI_ExtLCD1280x1024x75Data[]=
+{
+        {211,60,1024,501,1688,1066   }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {211,60,1024,508,1688,1066   }, /* ; 01 (320x350,640x350) */
+        {211,60,1024,501,1688,1066   }, /* ; 02 (360x400,720x400) */
+        {211,60,1024,508,1688,1066   }, /* ; 03 (720x350) */
+        {211,45,768,498,1688,1066    }, /* ; 04 (640x480x75Hz) */
+        {211,75,1024,625,1688,1066   }, /* ; 05 (800x600x75Hz) */
+        {211,120,1280,798,1688,1066  }, /* ; 06 (1024x768x75Hz) */
+        {1,1,1688,1066,1688,1066     }  /* ; 07 (1280x1024x75Hz) */
+};
+
+XGI330_LCDDataStruct  XGI_StLCD1280x1024x75Data[]=
+{
+        {211,60,1024,501,1688,1066 }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {211,60,1024,508,1688,1066 }, /* ; 01 (320x350,640x350) */
+        {211,60,1024,501,1688,1066 }, /* ; 02 (360x400,720x400) */
+        {211,60,1024,508,1688,1066 }, /* ; 03 (720x350) */
+        {211,45,768,498,1688,1066  }, /* ; 04 (640x480x75Hz) */
+        {211,75,1024,625,1688,1066 }, /* ; 05 (800x600x75Hz) */
+        {211,120,1280,798,1688,1066}, /* ; 06 (1024x768x75Hz) */
+        {1,1,1688,1066,1688,1066   }  /* ; 07 (1280x1024x75Hz) */
+};
+
+XGI330_LCDDataStruct  XGI_CetLCD1280x1024x75Data[]=
+{
+        {1,1,1688,1066,1688,1066}, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {1,1,1688,1066,1688,1066}, /* ; 01 (320x350,640x350) */
+        {1,1,1688,1066,1688,1066}, /* ; 02 (360x400,720x400) */
+        {1,1,1688,1066,1688,1066}, /* ; 03 (720x350) */
+        {1,1,1688,1066,1688,1066}, /* ; 04 (640x480x75Hz) */
+        {1,1,1688,1066,1688,1066}, /* ; 05 (800x600x75Hz) */
+        {1,1,1688,1066,1688,1066}, /* ; 06 (1024x768x75Hz) */
+        {1,1,1688,1066,1688,1066}  /* ; 07 (1280x1024x75Hz) */
+};
+
+XGI330_LCDDataStruct  XGI_NoScalingDatax75[]=
+{
+        {1,1,800,449,800,449    }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {1,1,800,449,800,449    }, /* ; 01 (320x350,640x350) */
+        {1,1,900,449,900,449    }, /* ; 02 (360x400,720x400) */
+        {1,1,900,449,900,449    }, /* ; 03 (720x350) */
+        {1,1,840,500,840,500    }, /* ; 04 (640x480x75Hz) */
+        {1,1,1056,625,1056,625  }, /* ; 05 (800x600x75Hz) */
+        {1,1,1312,800,1312,800  }, /* ; 06 (1024x768x75Hz) */
+        {1,1,1688,1066,1688,1066}, /* ; 07 (1280x1024x75Hz) */
+        {1,1,1688,1066,1688,1066}, /* ; 08 (1400x1050x75Hz) ;;[ycchen] 12/19/02 */
+        {1,1,2160,1250,2160,1250}, /* ; 09 (1600x1200x75Hz) */
+        {1,1,1688,806,1688,806  }  /* ; 0A (1280x768x75Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_ExtLCDDes1024x768Data[]=
+{
+   {  9,1057,0, 771  }, /* ; 00 (320x200,320x400,640x200,640x400) */
+   {  9,1057,0, 771  }, /* ; 01 (320x350,640x350) */
+   {  9,1057,0, 771  }, /* ; 02 (360x400,720x400) */
+   {  9,1057,0, 771  }, /* ; 03 (720x350) */
+   {  9,1057,0, 771  }, /* ; 04 (640x480x60Hz) */
+   {  9,1057,0, 771  }, /* ; 05 (800x600x60Hz) */
+   {  9,1057,805, 770  }  /* ; 06 (1024x768x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_StLCDDes1024x768Data[]=
+{
+        { 9,1057,737,703   }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        { 9,1057,686,651   }, /* ; 01 (320x350,640x350) */
+        { 9,1057,737,703   }, /* ; 02 (360x400,720x400) */
+        { 9,1057,686,651   }, /* ; 03 (720x350) */
+        { 9,1057,776,741   }, /* ; 04 (640x480x60Hz) */
+        { 9,1057, 0 ,771   }, /* ; 05 (800x600x60Hz) */
+        { 9,1057,805,770   }  /* ; 06 (1024x768x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_CetLCDDes1024x768Data[]=
+{
+        {      1152,856,622,587   }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {      1152,856,597,562   }, /* ; 01 (320x350,640x350) */
+        {      1152,856,622,587   }, /* ; 02 (360x400,720x400) */
+        {      1152,856,597,562   }, /* ; 03 (720x350) */
+        {      1152,856,662,627   }, /* ; 04 (640x480x60Hz) */
+        {      1232,936,722,687   }, /* ; 05 (800x600x60Hz) */
+        {      0,1048,805,770   }  /* ; 06 (1024x768x60Hz) */
+};
+
+XGI330_LCDDataDesStruct XGI_ExtLCDDLDes1280x1024Data[]=
+{
+        {      18,1346,981,940     },/* 00 (320x200,320x400,640x200,640x400) */
+        {      18,1346,926,865     },/* 01 (320x350,640x350) */
+        {      18,1346,981,940     },/* 02 (360x400,720x400) */
+        {      18,1346,926,865     },/* 03 (720x350) */
+        {      18,1346,0,1025     },/* 04 (640x480x60Hz) */
+        {      18,1346,0,1025     },/* 05 (800x600x60Hz) */
+        {      18,1346,1065,1024     },/* 06 (1024x768x60Hz) */
+        {      18,1346,1065,1024     }/* 07 (1280x1024x60Hz) */
+};
+
+XGI330_LCDDataDesStruct XGI_StLCDDLDes1280x1024Data[]=
+{
+        {      18,1346,970,907     },/* 00 (320x200,320x400,640x200,640x400) */
+        {      18,1346,917,854     },/* 01 (320x350,640x350) */
+        {      18,1346,970,907     },/* 02 (360x400,720x400) */
+        {      18,1346,917,854     },/* 03 (720x350) */
+        {      18,1346,0,1025     },/* 04 (640x480x60Hz) */
+        {      18,1346,0,1025     },/* 05 (800x600x60Hz) */
+        {      18,1346,1065,1024     },/* 06 (1024x768x60Hz) */
+        {      18,1346,1065,1024     }/* 07 (1280x1024x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_CetLCDDLDes1280x1024Data[]=
+{
+        {      1368,1008,752,711    }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      1368,1008,729,688    }, /* 01 (320x350,640x350) */
+        {      1368,1008,752,711    }, /* 02 (360x400,720x400) */
+        {      1368,1008,729,688    }, /* 03 (720x350) */
+        {      1368,1008,794,753    }, /* 04 (640x480x60Hz) */
+        {      1448,1068,854,813    }, /* 05 (800x600x60Hz) */
+        {      1560,1200,938,897    }, /* 06 (1024x768x60Hz) */
+        {      18,1346,1065,1024    }  /* 07 (1280x1024x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_ExtLCDDes1280x1024Data[]=
+{
+        {      9,1337,981,940    }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {      9,1337,926,884    }, /* ; 01 (320x350,640x350) alan, 2003/09/30 */
+        {      9,1337,981,940    }, /* ; 02 (360x400,720x400) */
+        {      9,1337,926,884    }, /* ; 03 (720x350) alan, 2003/09/30 */
+        {      9,1337,0,1025    }, /* ; 04 (640x480x60Hz) */
+        {      9,1337,0,1025    }, /* ; 05 (800x600x60Hz) */
+        {      9,1337,1065,1024    }, /* ; 06 (1024x768x60Hz) */
+        {      9,1337,1065,1024    }  /* ; 07 (1280x1024x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_StLCDDes1280x1024Data[]=
+{
+        {      9,1337,970,907    }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {      9,1337,917,854    }, /* ; 01 (320x350,640x350) */
+        {      9,1337,970,907    }, /* ; 02 (360x400,720x400) */
+        {      9,1337,917,854    }, /* ; 03 (720x350) */
+        {      9,1337,0,1025    }, /* ; 04 (640x480x60Hz) */
+        {      9,1337,0,1025    }, /* ; 05 (800x600x60Hz) */
+        {      9,1337,1065,1024    }, /* ; 06 (1024x768x60Hz) */
+        {      9,1337,1065,1024    }  /* ; 07 (1280x1024x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_CetLCDDes1280x1024Data[]=
+{
+        {      1368,1008,752,711    }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      1368,1008,729,688    }, /* 01 (320x350,640x350) */
+        {      1368,1008,752,711    }, /* 02 (360x400,720x400) */
+        {      1368,1008,729,688    }, /* 03 (720x350) */
+        {      1368,1008,794,753    }, /* 04 (640x480x60Hz) */
+        {      1448,1068,854,813    }, /* 05 (800x600x60Hz) */
+        {      1560,1200,938,897    }, /* 06 (1024x768x60Hz) */
+        {      9,1337,1065,1024    }  /* 07 (1280x1024x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_StLCDDLDes1400x1050Data[]=
+{
+        {      18,1464,0,1051    }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      18,1464,0,1051    }, /* 01 (320x350,640x350) */
+        {      18,1464,0,1051    }, /* 02 (360x400,720x400) */
+        {      18,1464,0,1051    }, /* 03 (720x350) */
+        {      18,1464,0,1051    }, /* 04 (640x480x60Hz) */
+        {      18,1464,0,1051    }, /* 05 (800x600x60Hz) */
+        {      18,1464,0,1051    }, /* 06 (1024x768x60Hz) */
+        {      1646,1406,1053,1038    }, /* 07 (1280x1024x60Hz) */
+        {      18,1464,0,1051    }  /* 08 (1400x1050x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_ExtLCDDLDes1400x1050Data[]=
+{
+        {      18,1464,0,1051    }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      18,1464,0,1051    }, /* 01 (320x350,640x350) */
+        {      18,1464,0,1051    }, /* 02 (360x400,720x400) */
+        {      18,1464,0,1051    }, /* 03 (720x350) */
+        {      18,1464,0,1051    }, /* 04 (640x480x60Hz) */
+        {      18,1464,0,1051    }, /* 05 (800x600x60Hz) */
+        {      18,1464,0,1051    }, /* 06 (1024x768x60Hz) */
+        {      1646,1406,1053,1038    }, /* 07 (1280x1024x60Hz) */
+        {      18,1464,0,1051    }  /* 08 (1400x1050x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_StLCDDes1400x1050Data[]=
+{
+        {      9,1455,0,1051     },/* 00 (320x200,320x400,640x200,640x400) */
+        {      9,1455,0,1051     },/* 01 (320x350,640x350) */
+        {      9,1455,0,1051     },/* 02 (360x400,720x400) */
+        {      9,1455,0,1051     },/* 03 (720x350) */
+        {      9,1455,0,1051     },/* 04 (640x480x60Hz) */
+        {      9,1455,0,1051     },/* 05 (800x600x60Hz) */
+        {      9,1455,0,1051     },/* 06 (1024x768x60Hz) */
+        {      1637,1397,1053,1038     },/* 07 (1280x1024x60Hz) */
+        {      9,1455,0,1051     } /* 08 (1400x1050x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_ExtLCDDes1400x1050Data[]=
+{
+        {      9,1455,0,1051     },/* 00 (320x200,320x400,640x200,640x400) */
+        {      9,1455,0,1051     },/* 01 (320x350,640x350) */
+        {      9,1455,0,1051     },/* 02 (360x400,720x400) */
+        {      9,1455,0,1051     },/* 03 (720x350) */
+        {      9,1455,0,1051     },/* 04 (640x480x60Hz) */
+        {      9,1455,0,1051     },/* 05 (800x600x60Hz) */
+        {      9,1455,0,1051     },/* 06 (1024x768x60Hz) */
+        {      1637,1397,1053,1038     },/* 07 (1280x1024x60Hz) */
+        {      9,1455,0,1051     } /* 08 (1400x1050x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_CetLCDDes1400x1050Data[]=
+{
+        {      1308,1068,781,766    }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      1308,1068,781,766    }, /* 01 (320x350,640x350) */
+        {      1308,1068,781,766    }, /* 02 (360x400,720x400) */
+        {      1308,1068,781,766    }, /* 03 (720x350) */
+        {      1308,1068,781,766    }, /* 04 (640x480x60Hz) */
+        {      1388,1148,841,826    }, /* 05 (800x600x60Hz) */
+        {      1490,1250,925,910    }, /* 06 (1024x768x60Hz) */
+        {      1646,1406,1053,1038    }, /* 07 (1280x1024x60Hz) */
+        {      18,1464,0,1051    } /* 08 (1400x1050x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_CetLCDDes1400x1050Data2[]=
+{
+        {      0,1448,0,1051    }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      0,1448,0,1051    }, /* 01 (320x350,640x350) */
+        {      0,1448,0,1051    }, /* 02 (360x400,720x400) */
+        {      0,1448,0,1051    }, /* 03 (720x350) */
+        {      0,1448,0,1051    }  /* 04 (640x480x60Hz) */
+};
+
+
+
+XGI330_LCDDataDesStruct  XGI_ExtLCDDLDes1600x1200Data[]=
+{
+        {      18,1682,0,1201    }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      18,1682,0,1201    }, /* 01 (320x350,640x350) */
+        {      18,1682,0,1201    }, /* 02 (360x400,720x400) */
+        {      18,1682,0,1201    }, /* 03 (720x350) */
+        {      18,1682,0,1201    }, /* 04 (640x480x60Hz) */
+        {      18,1682,0,1201    }, /* 05 (800x600x60Hz) */
+        {      18,1682,0,1201    }, /* 06 (1024x768x60Hz) */
+        {      18,1682,0,1201    }, /* 07 (1280x1024x60Hz) */
+        {      18,1682,0,1201    }, /* 08 (1400x1050x60Hz) */
+        {      18,1682,0,1201    }  /* 09 (1600x1200x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_StLCDDLDes1600x1200Data[]=
+{
+        {      18,1682,1150,1101    }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      18,1682,1083,1034    }, /* 01 (320x350,640x350) */
+        {      18,1682,1150,1101    }, /* 02 (360x400,720x400) */
+        {      18,1682,1083,1034    }, /* 03 (720x350) */
+        {      18,1682,0,1201    }, /* 04 (640x480x60Hz) */
+        {      18,1682,0,1201    }, /* 05 (800x600x60Hz) */
+        {      18,1682,0,1201    }, /* 06 (1024x768x60Hz) */
+        {      18,1682,1232,1183    }, /* 07 (1280x1024x60Hz) */
+        {      18,1682,0,1201    }, /* 08 (1400x1050x60Hz) */
+        {      18,1682,0,1201    } /* 09 (1600x1200x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_ExtLCDDes1600x1200Data[]=
+{
+        {      9,1673,0,1201     },/* 00 (320x200,320x400,640x200,640x400) */
+        {      9,1673,0,1201     },/* 01 (320x350,640x350) */
+        {      9,1673,0,1201     },/* 02 (360x400,720x400) */
+        {      9,1673,0,1201     },/* 03 (720x350) */
+        {      9,1673,0,1201     },/* 04 (640x480x60Hz) */
+        {      9,1673,0,1201     },/* 05 (800x600x60Hz) */
+        {      9,1673,0,1201     },/* 06 (1024x768x60Hz) */
+        {      9,1673,0,1201     },/* 07 (1280x1024x60Hz) */
+        {      9,1673,0,1201     },/* 08 (1400x1050x60Hz) */
+        {      9,1673,0,1201     } /* 09 (1600x1200x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_StLCDDes1600x1200Data[]=
+{
+        {      9,1673,1150,1101     },/* 00 (320x200,320x400,640x200,640x400) */
+        {      9,1673,1083,1034     },/* 01 (320x350,640x350) */
+        {      9,1673,1150,1101     },/* 02 (360x400,720x400) */
+        {      9,1673,1083,1034     },/* 03 (720x350) */
+        {      9,1673,0,1201     },/* 04 (640x480x60Hz) */
+        {      9,1673,0,1201     },/* 05 (800x600x60Hz) */
+        {      9,1673,0,1201     },/* 06 (1024x768x60Hz) */
+        {      9,1673,1232,1183     },/* 07 (1280x1024x60Hz) */
+        {      9,1673,0,1201     },/* 08 (1400x1050x60Hz) */
+        {      9,1673,0,1201     } /* 09 (1600x1200x60Hz) */
+};
+
+XGI330_LCDDataDesStruct2  XGI_NoScalingDesData[]=
+{
+        {     9,657,448,405,96,2  }, /* 00 (320x200,320x400,640x200,640x400) */
+        {     9,657,448,355,96,2  }, /* 01 (320x350,640x350) */
+        {     9,657,448,405,96,2  }, /* 02 (360x400,720x400) */
+        {     9,657,448,355,96,2  }, /* 03 (720x350) */
+        {     9,657,1,483,96,2  }, /* 04 (640x480x60Hz) */
+        {     9,849,627,600,128,4  }, /* 05 (800x600x60Hz) */
+        {     9,1057,805,770,0136,6  }, /* 06 (1024x768x60Hz) */
+        {     9,1337,0,1025,112,3  }, /* 07 (1280x1024x60Hz) */
+        {     9,1457,0,1051,112,3  }, /* 08 (1400x1050x60Hz) }, //;[ycchen] 12/19/02 */
+        {     9,1673,0,1201,192,3  }, /* 09 (1600x1200x60Hz) */
+        {     9,1337,0,771,112,6  }  /* 0A (1280x768x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_ExtLCDDes1024x768x75Data[]=                /* ;;1024x768x75Hz */
+{
+        {9,1049,0,769},    /* ; 00 (320x200,320x400,640x200,640x400) */
+        {9,1049,0,769},    /* ; 01 (320x350,640x350) */
+        {9,1049,0,769},    /* ; 02 (360x400,720x400) */
+        {9,1049,0,769},    /* ; 03 (720x350) */
+        {9,1049,0,769},    /* ; 04 (640x480x75Hz) */
+        {9,1049,0,769},    /* ; 05 (800x600x75Hz) */
+        {9,1049,0,769}     /* ; 06 (1024x768x75Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_StLCDDes1024x768x75Data[]=
+{
+        {9,1049,0,769},    /* ; 00 (320x200,320x400,640x200,640x400) */
+        {9,1049,0,769},    /* ; 01 (320x350,640x350) */
+        {9,1049,0,769},    /* ; 02 (360x400,720x400) */
+        {9,1049,0,769},    /* ; 03 (720x350) */
+        {9,1049,0,769},    /* ; 04 (640x480x75Hz) */
+        {9,1049,0,769},    /* ; 05 (800x600x75Hz) */
+        {9,1049,0,769}     /* ; 06 (1024x768x75Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_CetLCDDes1024x768x75Data[]=        /* ;;1024x768x75Hz */
+{
+        {1152,856,622,587},     /* ; 00 (320x200,320x400,640x200,640x400) */
+        {1152,856,597,562},     /* ; 01 (320x350,640x350) */
+        {1192,896,622,587},     /* ; 02 (360x400,720x400) */
+        {1192,896,597,562},     /* ; 03 (720x350) */
+        {1129,857,656,625},     /* ; 04 (640x480x75Hz) */
+        {1209,937,716,685},     /* ; 05 (800x600x75Hz) */
+        {9,1049,0,769}          /* ; 06 (1024x768x75Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_ExtLCDDLDes1280x1024x75Data[]=         /* ;;1280x1024x75Hz */
+{
+        {18,1314,0,1025     },/* ; 00 (320x200,320x400,640x200,640x400) */
+        {18,1314,0,1025     },/* ; 01 (320x350,640x350) */
+        {18,1314,0,1025     },/* ; 02 (360x400,720x400) */
+        {18,1314,0,1025     },/* ; 03 (720x350) */
+        {18,1314,0,1025     },/* ; 04 (640x480x60Hz) */
+        {18,1314,0,1025     },/* ; 05 (800x600x60Hz) */
+        {18,1314,0,1025     },/* ; 06 (1024x768x60Hz) */
+        {18,1314,0,1025     }/* ; 07 (1280x1024x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_StLCDDLDes1280x1024x75Data[]=
+{
+        {18,1314,0,1025     },/* ; 00 (320x200,320x400,640x200,640x400) */
+        {18,1314,0,1025     },/* ; 01 (320x350,640x350) */
+        {18,1314,0,1025     },/* ; 02 (360x400,720x400) */
+        {18,1314,0,1025     },/* ; 03 (720x350) */
+        {18,1314,0,1025     },/* ; 04 (640x480x60Hz) */
+        {18,1314,0,1025     },/* ; 05 (800x600x60Hz) */
+        {18,1314,0,1025     },/* ; 06 (1024x768x60Hz) */
+        {18,1314,0,1025     }/* ; 07 (1280x1024x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_CetLCDDLDes1280x1024x75Data[]=     /* 1280x1024x75Hz */
+{
+        {1368,1008,752,711},    /* ; 00 (320x200,320x400,640x200,640x400) */
+        {1368,1008,729,688},    /* ; 01 (320x350,640x350) */
+        {1408,1048,752,711},    /* ; 02 (360x400,720x400) */
+        {1408,1048,729,688},    /* ; 03 (720x350) */
+        {1377,985,794,753},    /* ; 04 (640x480x75Hz) */
+        {1457,1065,854,813},    /* ; 05 (800x600x75Hz) */
+        {1569,1177,938,897},    /* ; 06 (1024x768x75Hz) */
+        {18,1314,0,1025}          /* ; 07 (1280x1024x75Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_ExtLCDDes1280x1024x75Data[]=         /* ;;1280x1024x75Hz */
+{
+        {9,1305,0,1025},/* ; 00 (320x200,320x400,640x200,640x400) */
+        {9,1305,0,1025},/* ; 01 (320x350,640x350) */
+        {9,1305,0,1025},/* ; 02 (360x400,720x400) */
+        {9,1305,0,1025},/* ; 03 (720x350) */
+        {9,1305,0,1025},/* ; 04 (640x480x60Hz) */
+        {9,1305,0,1025},/* ; 05 (800x600x60Hz) */
+        {9,1305,0,1025},/* ; 06 (1024x768x60Hz) */
+        {9,1305,0,1025} /* ; 07 (1280x1024x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_StLCDDes1280x1024x75Data[]=
+{
+        {9,1305,0,1025},/* ; 00 (320x200,320x400,640x200,640x400) */
+        {9,1305,0,1025},/* ; 01 (320x350,640x350) */
+        {9,1305,0,1025},/* ; 02 (360x400,720x400) */
+        {9,1305,0,1025},/* ; 03 (720x350) */
+        {9,1305,0,1025},/* ; 04 (640x480x60Hz) */
+        {9,1305,0,1025},/* ; 05 (800x600x60Hz) */
+        {9,1305,0,1025},/* ; 06 (1024x768x60Hz) */
+        {9,1305,0,1025} /* ; 07 (1280x1024x60Hz) */
+};
+
+XGI330_LCDDataDesStruct  XGI_CetLCDDes1280x1024x75Data[]=       /* 1280x1024x75Hz */
+{
+        {1368,1008,752,711},    /* ; 00 (320x200,320x400,640x200,640x400) */
+        {1368,1008,729,688},    /* ; 01 (320x350,640x350) */
+        {1408,1048,752,711},    /* ; 02 (360x400,720x400) */
+        {1408,1048,729,688},    /* ; 03 (720x350) */
+        {1377,985,794,753},    /* ; 04 (640x480x75Hz) */
+        {1457,1065,854,813},    /* ; 05 (800x600x75Hz) */
+        {1569,1177,938,897},    /* ; 06 (1024x768x75Hz) */
+        {9,1305,0,1025}           /* ; 07 (1280x1024x75Hz) */
+};
+
+XGI330_LCDDataDesStruct2 XGI_NoScalingDesDatax75[]= /* Scaling LCD 75Hz */
+{
+        {9,657,448,405,96,2},   /* ; 00 (320x200,320x400,640x200,640x400) */
+        {9,657,448,355,96,2},   /* ; 01 (320x350,640x350) */
+        {9,738,448,405,108,2},   /* ; 02 (360x400,720x400) */
+        {9,738,448,355,108,2},   /* ; 03 (720x350) */
+        {9,665,0,481,64,3},   /* ; 04 (640x480x75Hz) */
+        {9,825,0,601,80,3},   /* ; 05 (800x600x75Hz) */
+        {9,1049,0,769,96,3},   /* ; 06 (1024x768x75Hz) */
+        {9,1305,0,1025,144,3},   /* ; 07 (1280x1024x75Hz) */
+        {9,1457,0,1051,112,3},   /* ; 08 (1400x1050x60Hz) ;;[ycchen] 12/19/02 */
+        {9,1673,0,1201,192,3},   /* ; 09 (1600x1200x75Hz) */
+        {9,1337,0,771,112,6}    /* ; 0A (1280x768x60Hz) */
+};
+
+XGI330_TVDataStruct  XGI_StPALData[]=
+{
+ {    1,   1, 864, 525,1270, 400, 100,   0, 760},
+ {    1,   1, 864, 525,1270, 350, 100,   0, 760},
+ {    1,   1, 864, 525,1270, 400,   0,   0, 720},
+ {    1,   1, 864, 525,1270, 350,   0,   0, 720},
+ {    1,   1, 864, 525,1270, 480,  50,   0, 760},
+ {    1,   1, 864, 525,1270, 600,  50,   0,   0}
+};
+
+XGI330_TVDataStruct  XGI_ExtPALData[]=
+{
+ {    2,   1,1080, 463,1270, 500,  50,   0,  50},
+ {   15,   7,1152, 413,1270, 500,  50,   0,  50},
+ {    2,   1,1080, 463,1270, 500,  50,   0,  50},
+ {   15,   7,1152, 413,1270, 500,  50,   0,  50},
+ {    2,   1, 900, 543,1270, 500,   0,   0,  50},
+ {    4,   3,1080, 663,1270, 500, 438,   0, 438},
+ {    1,   1,1125, 831,1270, 500, 686,   0, 686},     /*301b*/
+ {    3,   2,1080, 619,1270, 540, 438,   0, 438}
+};
+
+XGI330_TVDataStruct  XGI_StNTSCData[]=
+{
+ {    1,   1, 858, 525,1270, 400,  50,   0, 760},
+ {    1,   1, 858, 525,1270, 350,  50,   0, 640},
+ {    1,   1, 858, 525,1270, 400,   0,   0, 720},
+ {    1,   1, 858, 525,1270, 350,   0,   0, 720},
+ {    1,   1, 858, 525,1270, 480,   0,   0, 760}
+};
+
+XGI330_TVDataStruct  XGI_ExtNTSCData[]=
+{
+ {    9,  5, 1001, 453,1270, 420, 171,   0, 171},
+ {   12,  5,  858, 403,1270, 420, 171,   0, 171},
+ {    9,  5, 1001, 453,1270, 420, 171,   0, 171},
+ {   12,  5,  858, 403,1270, 420, 171,   0, 171},
+ {  143,  80, 836, 523,1270, 420, 224,   0,   0},
+ {  143, 120,1008, 643,1270, 420,   0,   1,   0},
+ {   1,    1,1120, 821,1516, 420,   0,   1,   0}, /*301b*/
+ {    2,   1, 858, 503,1584, 480,   0,   1,   0},
+ {    3,   2,1001, 533,1270, 420,   0,   0,   0}
+};
+
+XGI330_TVDataStruct  XGI_St1HiTVData[]=
+{
+        {        1,1,892,563,690,800,0,0,0               }, /* 00 (320x200,320x400,640x200,640x400) */
+        {        1,1,892,563,690,700,0,0,0               }, /* 01 (320x350,640x350) */
+        {        1,1,1000,563,785,800,0,0,0              }, /* 02 (360x400,720x400) */
+        {        1,1,1000,563,785,700,0,0,0              }, /* 03 (720x350) */
+        {        1,1,892,563,690,960,0,0,0               }, /* 04 (320x240,640x480) */
+        {        8,5,1050,683,1648,960,0x150,1,0         }  /* 05 (400x300,800x600) */
+};
+
+XGI330_TVDataStruct  XGI_St2HiTVData[]=
+{
+        {        3,1,840,483,1648,960,0x032,0,0          }, /* 00 (320x200,320x400,640x200,640x400) */
+        {        1,1,892,563,690,700,0,0,0               }, /* 01 (320x350,640x350) */
+        {        3,1,840,483,1648,960,0x032,0,0          }, /* 02 (360x400,720x400) */
+        {        1,1,1000,563,785,700,0,0,0              }, /* 03 (720x350) */
+        {        5,2,840,563,1648,960,0x08D,1,0          }, /* 04 (320x240,640x480) */
+        {        8,5,1050,683,1648,960,0x17C,1,0         }  /* 05 (400x300,800x600) */
+
+};
+
+XGI330_TVDataStruct  XGI_ExtHiTVData[]=
+{
+        {        6,1,840,563,1632,960,0,0,0              }, /* 00 (320x200,320x400,640x200,640x400) */
+        {        3,1,960,563,1632,960,0,0,0              }, /* 01 (320x350,640x350) */
+        {        3,1,840,483,1632,960,0,0,0              }, /* 02 (360x400,720x400) */
+        {        3,1,960,563,1632,960,0,0,0              }, /* 03 (720x350) */
+        {        5,1,840,563,1648,960,0x166,1,0          }, /* 04 (320x240,640x480) */
+        {        16,5,1050,683,1648,960,0x143,1,0        }, /* 05 (400x300,800x600) */
+        {        25,12,1260,851,1648,960,0x032,0,0       }, /* 06 (512x384,1024x768) */
+        {        5,4,1575,1124,1648,960,0x128,0,0        }, /* 07 (1280x1024) */
+        {        4,1,1050,563,1548,960,0x143,1,0         }, /* 08 (800x480) */
+        {        5,2,1400,659,1648,960,0x032,0,0         }, /* 09 (1024x576) */
+        {        8,5,1750,803,1648,960,0x128,0,0         }  /* 0A (1280x720) */
+
+};
+
+XGI330_TVDataStruct  XGI_ExtYPbPr525iData[]=
+{
+ {    9,  5, 1001, 453,1270, 420, 171,   0, 171},
+ {   12,  5,  858, 403,1270, 420, 171,   0, 171},
+ {    9,  5, 1001, 453,1270, 420, 171,   0, 171},
+ {   12,  5,  858, 403,1270, 420, 171,   0, 171},
+ {  143,  80, 836, 523,1250, 420, 224,   0,   0},
+ {  143, 120,1008, 643,1250, 420,   0,   1,   0},
+ {   1,    1,1120, 821,1516, 420,   0,   1,   0}, /*301b*/
+ {    2,   1, 858, 503,1584, 480,   0,   1,   0},
+ {    3,   2,1001, 533,1250, 420,   0,   0,   0}
+};
+
+XGI330_TVDataStruct  XGI_StYPbPr525iData[]=
+{
+ {    1,   1, 858, 525,1270, 400,  50,   0, 760},
+ {    1,   1, 858, 525,1270, 350,  50,   0, 640},
+ {    1,   1, 858, 525,1270, 400,   0,   0, 720},
+ {    1,   1, 858, 525,1270, 350,   0,   0, 720},
+ {    1,   1, 858, 525,1270, 480,   0,   0, 760},
+};
+
+XGI330_TVDataStruct  XGI_ExtYPbPr525pData[]=
+{
+ {    9,  5, 1001, 453,1270, 420, 171,   0, 171},
+ {   12,  5,  858, 403,1270, 420, 171,   0, 171},
+ {    9,  5, 1001, 453,1270, 420, 171,   0, 171},
+ {   12,  5,  858, 403,1270, 420, 171,   0, 171},
+ {  143,  80, 836, 523,1270, 420, 224,   0,   0},
+ {  143, 120,1008, 643,1270, 420,   0,   1,   0},
+ {   1,    1,1120, 821,1516, 420,   0,   1,   0}, /*301b*/
+ {    2,   1, 858, 503,1584, 480,   0,   1,   0},
+ {    3,   2,1001, 533,1270, 420,   0,   0,   0}
+ };
+
+XGI330_TVDataStruct  XGI_StYPbPr525pData[]=
+{
+ {    1,   1,1716, 525,1270, 400,  50,   0, 760},
+ {    1,   1,1716, 525,1270, 350,  50,   0, 640},
+ {    1,   1,1716, 525,1270, 400,   0,   0, 720},
+ {    1,   1,1716, 525,1270, 350,   0,   0, 720},
+ {    1,   1,1716, 525,1270, 480,   0,   0, 760},
+};
+
+XGI330_TVDataStruct  XGI_ExtYPbPr750pData[]=
+{
+ {    3,   1, 935, 470,1130, 680,  50,   0,   0},       /* 00 (320x200,320x400,640x200,640x400) */
+ {   24,   7, 935, 420,1130, 680,  50,   0,   0},       /* 01 (320x350,640x350) */
+ {    3,   1, 935, 470,1130, 680,  50,   0,   0},       /* 02 (360x400,720x400) */
+ {   24,   7, 935, 420,1130, 680,  50,   0,   0},       /* 03 (720x350) */
+ {    2,   1,1100, 590,1130, 640,  50,   0,   0},       /* 04 (320x240,640x480) */
+ {    3,   2,1210, 690,1130, 660,  50,   0,   0},       /* 05 (400x300,800x600) */
+ {    1,   1,1375, 878,1130, 640, 638,   0,   0},       /* 06 (1024x768) */
+ {    2,   1, 858, 503,1130, 480,   0,   1,   0},        /* 07 (720x480) */
+ {    5,   4,1815, 570,1130, 660,  50,   0,   0},
+ {    5,   3,1100, 686,1130, 640,  50,   1,   0},
+ {   10,   9,1320, 830,1130, 640,  50,   0,   0}
+};
+
+XGI330_TVDataStruct  XGI_StYPbPr750pData[]=
+{
+ {    1,   1,1650, 750,1280, 400,  50,   0, 760},
+ {    1,   1,1650, 750,1280, 350,  50,   0, 640},
+ {    1,   1,1650, 750,1280, 400,   0,   0, 720},
+ {    1,   1,1650, 750,1280, 350,   0,   0, 720},
+ {    1,   1,1650, 750,1280, 480,   0,   0, 760},
+};
+
+UCHAR XGI330_NTSCTiming[] = {
+  0x17,0x1d,0x03,0x09,0x05,0x06,0x0c,0x0c,
+  0x94,0x49,0x01,0x0a,0x06,0x0d,0x04,0x0a,
+  0x06,0x14,0x0d,0x04,0x0a,0x00,0x85,0x1b,
+  0x0c,0x50,0x00,0x97,0x00,0xda,0x4a,0x17,
+  0x7d,0x05,0x4b,0x00,0x00,0xe2,0x00,0x02,
+  0x03,0x0a,0x65,0x9d,0x08,0x92,0x8f,0x40,
+  0x60,0x80,0x14,0x90,0x8c,0x60,0x14,0x50,
+  0x00,0x40,0x44,0x00,0xdb,0x02,0x3b,0x00};
+
+UCHAR XGI330_PALTiming[] = {
+  0x21,0x5A,0x35,0x6e,0x04,0x38,0x3d,0x70,
+  0x94,0x49,0x01,0x12,0x06,0x3e,0x35,0x6d,
+  0x06,0x14,0x3e,0x35,0x6d,0x00,0x45,0x2b,
+  0x70,0x50,0x00,0x9b,0x00,0xd9,0x5d,0x17,
+  0x7d,0x05,0x45,0x00,0x00,0xe8,0x00,0x02,
+  0x0d,0x00,0x68,0xb0,0x0b,0x92,0x8f,0x40,
+  0x60,0x80,0x14,0x90,0x8c,0x60,0x14,0x63,
+  0x00,0x40,0x3e,0x00,0xe1,0x02,0x28,0x00};
+
+UCHAR XGI330_HiTVExtTiming[] =
+{
+      0x2D,0x60,0x2C,0x5F,0x08,0x31,0x3A,0x64,
+      0x28,0x02,0x01,0x3D,0x06,0x3E,0x35,0x6D,
+      0x06,0x14,0x3E,0x35,0x6D,0x00,0xC5,0x3F,
+      0x64,0x90,0x33,0x8C,0x18,0x36,0x3E,0x13,
+      0x2A,0xDE,0x2A,0x44,0x40,0x2A,0x44,0x40,
+      0x8E,0x8E,0x82,0x07,0x0B,
+      0x92,0x0F,0x40,0x60,0x80,0x14,0x90,0x8C,
+      0x60,0x14,0x3D,0x63,0x4F,
+      0x27,0x00,0xfc,0xff,0x6a,0x00
+
+};
+
+UCHAR XGI330_HiTVSt1Timing[] =
+{
+      0x32,0x65,0x2C,0x5F,0x08,0x31,0x3A,0x65,
+      0x28,0x02,0x01,0x3D,0x06,0x3E,0x35,0x6D,
+      0x06,0x14,0x3E,0x35,0x6D,0x00,0xC5,0x3F,
+      0x65,0x90,0x7B,0xA8,0x03,0xF0,0x87,0x03,
+      0x11,0x15,0x11,0xCF,0x10,0x11,0xCF,0x10,
+      0x35,0x35,0x3B,0x69,0x1D,
+      0x92,0x0F,0x40,0x60,0x80,0x14,0x90,0x8C,
+      0x60,0x04,0x86,0xAF,0x5D,
+      0x0E,0x00,0xfc,0xff,0x2d,0x00
+};
+
+UCHAR XGI330_HiTVSt2Timing[] =
+{
+      0x32,0x65,0x2C,0x5F,0x08,0x31,0x3A,0x64,
+      0x28,0x02,0x01,0x3D,0x06,0x3E,0x35,0x6D,
+      0x06,0x14,0x3E,0x35,0x6D,0x00,0xC5,0x3F,
+      0x64,0x90,0x33,0x8C,0x18,0x36,0x3E,0x13,
+      0x2A,0xDE,0x2A,0x44,0x40,0x2A,0x44,0x40,
+      0x8E,0x8E,0x82,0x07,0x0B,
+      0x92,0x0F,0x40,0x60,0x80,0x14,0x90,0x8C,
+      0x60,0x14,0x3D,0x63,0x4F,
+      0x27,0x00,0xFC,0xff,0x6a,0x00
+};
+
+UCHAR XGI330_HiTVTextTiming[] =
+{
+      0x32,0x65,0x2C,0x5F,0x08,0x31,0x3A,0x65,
+      0x28,0x02,0x01,0x3D,0x06,0x3E,0x35,0x6D,
+      0x06,0x14,0x3E,0x35,0x6D,0x00,0xC5,0x3F,
+      0x65,0x90,0xE7,0xBC,0x03,0x0C,0x97,0x03,
+      0x14,0x78,0x14,0x08,0x20,0x14,0x08,0x20,
+      0xC8,0xC8,0x3B,0xD2,0x26,
+      0x92,0x0F,0x40,0x60,0x80,0x14,0x90,0x8C,
+      0x60,0x04,0x96,0x72,0x5C,
+      0x11,0x00,0xFC,0xFF,0x32,0x00
+};
+
+UCHAR XGI330_YPbPr750pTiming[] =
+{
+      0x30,0x1d,0xe8,0x09,0x09,0xed,0x0c,0x0c,
+      0x98,0x0a,0x01,0x0c,0x06,0x0d,0x04,0x0a,
+      0x06,0x14,0x0d,0x04,0x0a,0x00,0x85,0x3f,
+      0xed,0x50,0x70,0x9f,0x16,0x59,0x60,0x13,
+      0x27,0x0b,0x27,0xfc,0x30,0x27,0x1c,0xb0,
+      0x4b,0x4b,0x6f,0x2f,0x63,
+      0x92,0x0F,0x40,0x60,0x80,0x14,0x90,0x8C,
+      0x60,0x14,0x73,0x00,0x40,
+      0x11,0x00,0xfc,0xff,0x32,0x00
+};
+
+UCHAR XGI330_YPbPr525pTiming[] =
+{
+      0x3E,0x11,0x06,0x09,0x0b,0x0c,0x0c,0x0c,
+      0x98,0x0a,0x01,0x0d,0x06,0x0d,0x04,0x0a,
+      0x06,0x14,0x0d,0x04,0x0a,0x00,0x85,0x3f,
+      0x0c,0x50,0xb2,0x9f,0x16,0x59,0x4f,0x13,
+      0xad,0x11,0xad,0x1d,0x40,0x8a,0x3d,0xb8,
+      0x51,0x5e,0x60,0x49,0x7d,
+      0x92,0x0F,0x40,0x60,0x80,0x14,0x90,0x8C,
+      0x60,0x14,0x4B,0x43,0x41,
+      0x11,0x00,0xFC,0xFF,0x32,0x00
+};
+
+UCHAR XGI330_YPbPr525iTiming[] =
+{
+      0x1B,0x21,0x03,0x09,0x05,0x06,0x0C,0x0C,
+      0x94,0x49,0x01,0x0A,0x06,0x0D,0x04,0x0A,
+      0x06,0x14,0x0D,0x04,0x0A,0x00,0x85,0x1B,
+      0x0C,0x50,0x00,0x97,0x00,0xDA,0x4A,0x17,
+      0x7D,0x05,0x4B,0x00,0x00,0xE2,0x00,0x02,
+      0x03,0x0A,0x65,0x9D,0x08,
+      0x92,0x8F,0x40,0x60,0x80,0x14,0x90,0x8C,
+      0x60,0x14,0x4B,0x00,0x40,
+      0x44,0x00,0xDB,0x02,0x3B,0x00
+
+};
+
+UCHAR XGI330_HiTVGroup3Data[] =
+{
+      0x00,0x1A,0x22,0x63,0x62,0x22,0x08,0x5F,
+      0x05,0x21,0xB2,0xB2,0x55,0x77,0x2A,0xA6,
+      0x25,0x2F,0x47,0xFA,0xC8,0xFF,0x8E,0x20,
+      0x8C,0x6E,0x60,0x2E,0x58,0x48,0x72,0x44,
+      0x56,0x36,0x4F,0x6E,0x3F,0x80,0x00,0x80,
+      0x4F,0x7F,0x03,0xA8,0x7D,0x20,0x1A,0xA9,
+      0x14,0x05,0x03,0x7E,0x64,0x31,0x14,0x75,
+      0x18,0x05,0x18,0x05,0x4C,0xA8,0x01
+};
+
+UCHAR XGI330_HiTVGroup3Simu[] =
+{
+      0x00,0x1A,0x22,0x63,0x62,0x22,0x08,0x95,
+      0xDB,0x20,0xB8,0xB8,0x55,0x47,0x2A,0xA6,
+      0x25,0x2F,0x47,0xFA,0xC8,0xFF,0x8E,0x20,
+      0x8C,0x6E,0x60,0x15,0x26,0xD3,0xE4,0x11,
+      0x56,0x36,0x4F,0x6E,0x3F,0x80,0x00,0x80,
+      0x67,0x36,0x01,0x47,0x0E,0x10,0xBE,0xB4,
+      0x01,0x05,0x03,0x7E,0x65,0x31,0x14,0x75,
+      0x18,0x05,0x18,0x05,0x4C,0xA8,0x01
+};
+
+UCHAR XGI330_HiTVGroup3Text[] =
+{
+      0x00,0x1A,0x22,0x63,0x62,0x22,0x08,0xA7,
+      0xF5,0x20,0xCE,0xCE,0x55,0x47,0x2A,0xA6,
+      0x25,0x2F,0x47,0xFA,0xC8,0xFF,0x8E,0x20,
+      0x8C,0x6E,0x60,0x18,0x2C,0x0C,0x20,0x22,
+      0x56,0x36,0x4F,0x6E,0x3F,0x80,0x00,0x80,
+      0x93,0x3C,0x01,0x50,0x2F,0x10,0xF4,0xCA,
+      0x01,0x05,0x03,0x7E,0x65,0x31,0x14,0x75,
+      0x18,0x05,0x18,0x05,0x4C,0xA8,0x01
+};
+
+UCHAR XGI330_Ren525pGroup3[] =
+{
+  0x00,0x14,0x15,0x25,0x55,0x15,0x0b,0x13,
+  0xB1,0x41,0x62,0x62,0xFF,0xF4,0x45,0xa6,
+  0x25,0x2F,0x67,0xF6,0xbf,0xFF,0x8E,0x20,
+  0xAC,0xDA,0x60,0xFe,0x6A,0x9A,0x06,0x10,
+  0xd1,0x04,0x18,0x0a,0xFF,0x80,0x00,0x80,
+  0x3c,0x77,0x00,0xEF,0xE0,0x10,0xB0,0xE0,
+  0x10,0x4F,0x0F,0x0F,0x05,0x0F,0x08,0x6E,
+  0x1a,0x1F,0x25,0x2a,0x4C,0xAA,0x01
+};
+
+UCHAR XGI330_Ren750pGroup3[] =
+{
+  0x00,0x14,0x15,0x25,0x55,0x15,0x0b,0x7a,
+  0x54,0x41,0xE7,0xE7,0xFF,0xF4,0x45,0xa6,
+  0x25,0x2F,0x67,0xF6,0xbf,0xFF,0x8E,0x20,
+  0xAC,0x6A,0x60,0x2b,0x52,0xCD,0x61,0x10,
+  0x51,0x04,0x18,0x0a,0x1F,0x80,0x00,0x80,
+  0xFF,0xA4,0x04,0x2B,0x94,0x21,0x72,0x94,
+  0x26,0x05,0x01,0x0F,0xed,0x0F,0x0A,0x64,
+  0x18,0x1D,0x23,0x28,0x4C,0xAA,0x01
+};
+
+XGI_PanelDelayTblStruct XGI330_PanelDelayTbl[]=
+{
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}},
+{{0x00,0x00}}
+};
+
+XGI330_LVDSDataStruct  XGI330_LVDS320x480Data_1[]=
+{
+ {848, 433,400,525},
+ {848, 389,400,525},
+ {848, 433,400,525},
+ {848, 389,400,525},
+ {848, 518,400, 525},
+ {1056, 628,400,525},
+ {400, 525,400,525},
+ {800, 449,1000, 644},
+ {800, 525,1000, 635}
+};
+
+XGI330_LVDSDataStruct  XGI330_LVDS800x600Data_1[]=
+{
+ {848, 433,1060, 629},
+ {848, 389,1060, 629},
+ {848, 433,1060, 629},
+ {848, 389,1060, 629},
+ {848, 518,1060, 629},
+ {1056, 628,1056, 628},
+ {1056, 628,1056, 628},
+ {800, 449,1000, 644},
+ {800, 525,1000, 635}
+};
+
+XGI330_LVDSDataStruct  XGI330_LVDS800x600Data_2[]=
+{
+ {1056, 628,1056, 628},
+ {1056, 628,1056, 628},
+ {1056, 628,1056, 628},
+ {1056, 628,1056, 628},
+ {1056, 628,1056, 628},
+ {1056, 628,1056, 628},
+ {1056, 628,1056, 628},
+ {800, 449,1000, 644},
+ {800, 525,1000, 635}
+};
+
+XGI330_LVDSDataStruct  XGI_LVDS1024x768Data_1[]=
+{
+ { 960 , 438 , 1344 , 806 } ,   /* 00 (320x200,320x400,640x200,640x400) */
+ { 960 , 388 , 1344 , 806 } ,   /* 01 (320x350,640x350) */
+ { 1040, 438 , 1344 , 806 } ,   /* 02 (360x400,720x400) */
+ { 1040, 388 , 1344 , 806 } ,   /* 03 (720x350) */
+ { 960 , 518 , 1344 , 806 } ,   /* 04 (320x240,640x480) */
+ {1120 , 638 , 1344 , 806 } ,   /* 05 (400x300,800x600) */
+ {1344 , 806 , 1344 , 806 }     /* 06 (512x384,1024x768) */
+};
+
+
+XGI330_LVDSDataStruct  XGI_LVDS1024x768Data_2[]=
+{
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {800, 449,1280, 801},
+ {800, 525,1280, 813}
+};
+
+XGI330_LVDSDataStruct  XGI_LVDS1280x1024Data_1[]=
+{
+ {1048, 442,1688, 1066},
+ {1048, 392,1688, 1066},
+ {1048, 442,1688, 1066},
+ {1048, 392,1688, 1066},
+ {1048, 522,1688, 1066},
+ {1208, 642,1688, 1066},
+ {1432, 810,1688, 1066},
+ {1688, 1066,1688, 1066}
+};
+
+XGI330_LVDSDataStruct  XGI_LVDS1280x1024Data_2[]=
+{
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {800, 449,1280, 801},
+ {800, 525,1280, 813}
+};
+/*
+XGI330_LVDSDataStruct  XGI_LVDS1280x768Data_1[]=
+{
+ {768,438,1408,806},
+ {768,388,1408,806},
+ {768,438,1408,806},
+ {768,388,1408,806},
+ {768,518,1408,806},
+ {928,638,1408,806},
+ {1408,806,1408,806},
+ {1408,806,1408,806},
+ {1408,806,1408,806}
+};
+
+XGI330_LVDSDataStruct  XGI_LVDS1280x768Data_2[]=
+{
+ {1408, 806,1408, 806},
+ {1408, 806,1408, 806},
+ {1408, 806,1408, 806},
+ {1408, 806,1408, 806},
+ {1408, 806,1408, 806},
+ {1408, 806,1408, 806},
+ {1408, 806,1408, 806},
+ {1408, 806,1408, 806},
+ {1408, 806,1408, 806}
+};
+
+XGI330_LVDSDataStruct  XGI_LVDS1280x768NData_1[]=
+{
+ {704, 438,1344, 806},
+ {704, 388,1344, 806},
+ {704, 438,1344, 806},
+ {704, 388,1344, 806},
+ {704, 518,1344, 806},
+ {864, 638,1344, 806},
+ {1088, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806}
+};
+
+XGI330_LVDSDataStruct  XGI_LVDS1280x768NData_2[]=
+{
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806},
+ {1344, 806,1344, 806}
+};
+
+XGI330_LVDSDataStruct  XGI_LVDS1280x768SData_1[]=
+{
+ {1048,438,1688,806},
+ {1048,388,1688,806},
+ {1148,438,1688,806},
+ {1148,388,1688,806},
+ {1048,518,1688,806},
+ {1208,638,1688,806},
+ {1432,806,1688,806},
+ {1688,806,1688,806},
+ {1688,806,1688,806}
+};
+
+XGI330_LVDSDataStruct  XGI_LVDS1280x768SData_2[]=
+{
+ {1688,806,1688,806},
+ {1688,806,1688,806},
+ {1688,806,1688,806},
+ {1688,806,1688,806},
+ {1688,806,1688,806},
+ {1688,806,1688,806},
+ {1688,806,1688,806},
+ {1688,806,1688,806},
+ {1688,806,1688,806}
+};
+*/
+XGI330_LVDSDataStruct  XGI_LVDS1400x1050Data_1[]=
+{
+ {928,416,1688,1066},
+ {928,366,1688,1066},
+ {928,416,1688,1066},
+ {928,366,1688,1066},
+ {928,496,1688,1066},
+ {1088,616,1688,1066},
+ {1312,784,1688,1066},
+ {1568,1040,1688,1066},
+ {1688,1066,1688,1066}
+};
+
+XGI330_LVDSDataStruct  XGI_LVDS1400x1050Data_2[]=
+{
+ {1688,1066,1688,1066},
+ {1688,1066,1688,1066},
+ {1688,1066,1688,1066},
+ {1688,1066,1688,1066},
+ {1688,1066,1688,1066},
+ {1688,1066,1688,1066},
+ {1688,1066,1688,1066},
+ {1688,1066,1688,1066},
+ {1688,1066,1688,1066}
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1600x1200Data_1[]=
+{      /* ;;[ycchen] 12/05/02 LCDHTxLCDVT=2048x1320 */
+        {        1088,520,2048,1320      },/* 00 (320x200,320x400,640x200,640x400) */
+        {        1088,470,2048,1320      },/* 01 (320x350,640x350) */
+        {        1088,520,2048,1320      },/* 02 (360x400,720x400) */
+        {        1088,470,2048,1320      },/* 03 (720x350) */
+        {        1088,600,2048,1320      },/* 04 (320x240,640x480) */
+        {        1248,720,2048,1320      },/* 05 (400x300,800x600) */
+        {        1472,888,2048,1320      },/* 06 (512x384,1024x768) */
+        {        1728,1144,2048,1320     },/* 07 (640x512,1280x1024) */
+        {        1848,1170,2048,1320     },/* 08 (1400x1050) */
+        {        2048,1320,2048,1320     } /* 09 (1600x1200) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDSNoScalingData[]=
+{
+        {        800,449,800,449             }, /* 00 (320x200,320x400,640x200,640x400) */
+        {        800,449,800,449             }, /* 01 (320x350,640x350) */
+        {        800,449,800,449             }, /* 02 (360x400,720x400) */
+        {        800,449,800,449             }, /* 03 (720x350) */
+        {        800,525,800,525             }, /* 04 (640x480x60Hz) */
+        {        1056,628,1056,628           }, /* 05 (800x600x60Hz) */
+        {        1344,806,1344,806           }, /* 06 (1024x768x60Hz) */
+        {        1688,1066,1688,1066         }, /* 07 (1280x1024x60Hz) */
+        {        1688,1066,1688,1066         }, /* 08 (1400x1050x60Hz) ;;[ycchen] 12/19/02 */
+        {        2160,1250,2160,1250         }, /* 09 (1600x1200x60Hz) */
+        {        1688,806,1688,806           }  /* 0A (1280x768x60Hz) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1024x768Data_1x75[]=
+{
+        {960,438,1312,800  }, /* 00 (320x200,320x400,640x200,640x400) */
+        {960,388,1312,800  }, /* 01 (320x350,640x350) */
+        {1040,438,1312,800 }, /* 02 (360x400,720x400) */
+        {1040,388,1312,800 }, /* 03 (720x350) */
+        {928,512,1312,800  }, /* 04 (320x240,640x480) */
+        {1088,632,1312,800 }, /* 05 (400x300,800x600) */
+        {1312,800,1312,800 }, /* 06 (512x384,1024x768) */
+};
+
+
+XGI330_LVDSDataStruct XGI_LVDS1024x768Data_2x75[]=
+{
+        {1312,800,1312,800}, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {1312,800,1312,800}, /* ; 01 (320x350,640x350) */
+        {1312,800,1312,800}, /* ; 02 (360x400,720x400) */
+        {1312,800,1312,800}, /* ; 03 (720x350) */
+        {1312,800,1312,800}, /* ; 04 (320x240,640x480) */
+        {1312,800,1312,800}, /* ; 05 (400x300,800x600) */
+        {1312,800,1312,800}, /* ; 06 (512x384,1024x768) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1280x1024Data_1x75[]=
+{
+        {1048,442,1688,1066  }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {1048,392,1688,1066  }, /* ; 01 (320x350,640x350) */
+        {1128,442,1688,1066  }, /* ; 02 (360x400,720x400) */
+        {1128,392,1688,1066  }, /* ; 03 (720x350) */
+        {1048,522,1688,1066  }, /* ; 04 (320x240,640x480) */
+        {1208,642,1688,1066  }, /* ; 05 (400x300,800x600) */
+        {1432,810,1688,1066  }, /* ; 06 (512x384,1024x768) */
+        {1688,1066,1688,1066 }, /* ; 06; 07 (640x512,1280x1024) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1280x1024Data_2x75[]=
+{
+        {1688,1066,1688,1066 }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {1688,1066,1688,1066 }, /* ; 01 (320x350,640x350) */
+        {1688,1066,1688,1066 }, /* ; 02 (360x400,720x400) */
+        {1688,1066,1688,1066 }, /* ; 03 (720x350) */
+        {1688,1066,1688,1066 }, /* ; 04 (320x240,640x480) */
+        {1688,1066,1688,1066 }, /* ; 05 (400x300,800x600) */
+        {1688,1066,1688,1066 }, /* ; 06 (512x384,1024x768) */
+        {1688,1066,1688,1066 }, /* ; 06; 07 (640x512,1280x1024) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDSNoScalingDatax75[]=
+{
+        {800,449,800,449     }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {800,449,800,449     }, /* ; 01 (320x350,640x350) */
+        {900,449,900,449     }, /* ; 02 (360x400,720x400) */
+        {900,449,900,449     }, /* ; 03 (720x350) */
+        {800,500,800,500     }, /* ; 04 (640x480x75Hz) */
+        {1056,625,1056,625   }, /* ; 05 (800x600x75Hz) */
+        {1312,800,1312,800   }, /* ; 06 (1024x768x75Hz) */
+        {1688,1066,1688,1066 }, /* ; 07 (1280x1024x75Hz) */
+        {1688,1066,1688,1066 }, /* ; 08 (1400x1050x75Hz) ;;[ycchen] 12/19/02 */
+        {2160,1250,2160,1250 }, /* ; 09 (1600x1200x75Hz) */
+        {1688,806,1688,806   }, /* ; 0A (1280x768x75Hz) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1024x768Des_1[]=
+{
+        {      0,1048,   0, 771     }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      0,1048,   0, 771     }, /* 01 (320x350,640x350) */
+        {      0,1048,   0, 771     }, /* 02 (360x400,720x400) */
+        {      0,1048,   0, 771     }, /* 03 (720x350) */
+        {      0,1048,   0, 771     }, /* 04 (640x480x60Hz) */
+        {      0,1048,   0, 771     }, /* 05 (800x600x60Hz) */
+        {      0,1048, 805, 770     }  /* 06 (1024x768x60Hz) */
+} ;
+
+XGI330_LVDSDataStruct XGI_LVDS1024x768Des_2[]=
+{
+        {      1142, 856, 622, 587     }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      1142, 856, 597, 562     }, /* 01 (320x350,640x350) */
+        {      1142, 856, 622, 587     }, /* 02 (360x400,720x400) */
+        {      1142, 856, 597, 562     }, /* 03 (720x350) */
+        {      1142,1048, 722, 687     }, /* 04 (640x480x60Hz) */
+        {      1232, 936, 722, 687     }, /* 05 (800x600x60Hz) */
+        {         0,1048, 805, 771     }  /* 06 (1024x768x60Hz) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1024x768Des_3[]=
+{
+        {       320,  24, 622, 587     }, /* 00 (320x200,320x400,640x200,640x400) */
+        {       320,  24, 597, 562     }, /* 01 (320x350,640x350) */
+        {       320,  24, 622, 587     }, /* 02 (360x400,720x400) */
+        {       320,  24, 597, 562     }, /* 03 (720x350) */
+        {       320,  24, 722, 687     }  /* 04 (640x480x60Hz) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1280x1024Des_1[]=
+{
+        {      0,1328,    0, 1025     }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      0,1328,    0, 1025     }, /* 01 (320x350,640x350) */
+        {      0,1328,    0, 1025     }, /* 02 (360x400,720x400) */
+        {      0,1328,    0, 1025     }, /* 03 (720x350) */
+        {      0,1328,    0, 1025     }, /* 04 (640x480x60Hz) */
+        {      0,1328,    0, 1025     }, /* 05 (800x600x60Hz) */
+        {      0,1328,    0, 1025     }, /* 06 (1024x768x60Hz) */
+        {      0,1328, 1065, 1024     }  /* 07 (1280x1024x60Hz) */
+};
+
+ /* The Display setting for DE Mode Panel */
+XGI330_LVDSDataStruct XGI_LVDS1280x1024Des_2[]=
+{
+        {      1368,1008,752,711     }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      1368,1008,729,688     }, /* 01 (320x350,640x350) */
+        {      1408,1048,752,711     }, /* 02 (360x400,720x400) */
+        {      1408,1048,729,688     }, /* 03 (720x350) */
+        {      1368,1008,794,753     }, /* 04 (640x480x60Hz) */
+        {      1448,1068,854,813     }, /* 05 (800x600x60Hz) */
+        {      1560,1200,938,897     }, /* 06 (1024x768x60Hz) */
+        {      0000,1328,0,1025     }  /* 07 (1280x1024x60Hz) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1400x1050Des_1[]=
+{
+        {      0,1448,0,1051     }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      0,1448,0,1051     }, /* 01 (320x350,640x350) */
+        {      0,1448,0,1051     }, /* 02 (360x400,720x400) */
+        {      0,1448,0,1051     }, /* 03 (720x350) */
+        {      0,1448,0,1051     }, /* 04 (640x480x60Hz) */
+        {      0,1448,0,1051     }, /* 05 (800x600x60Hz) */
+        {      0,1448,0,1051     }, /* 06 (1024x768x60Hz) */
+        {      0,1448,0,1051     }, /* 07 (1280x1024x60Hz) */
+        {      0,1448,0,1051     }  /* 08 (1400x1050x60Hz) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1400x1050Des_2[]=
+{
+        {      1308,1068, 781, 766     }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      1308,1068, 781, 766     }, /* 01 (320x350,640x350) */
+        {      1308,1068, 781, 766     }, /* 02 (360x400,720x400) */
+        {      1308,1068, 781, 766     }, /* 03 (720x350) */
+        {      1308,1068, 781, 766     }, /* 04 (640x480x60Hz) */
+        {      1388,1148, 841, 826     }, /* 05 (800x600x60Hz) */
+        {      1490,1250, 925, 910     }, /* 06 (1024x768x60Hz) */
+        {      1608,1368,1053,1038     }, /* 07 (1280x1024x60Hz) */
+        {      0,1448,0,1051     }  /* 08 (1400x1050x60Hz) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1600x1200Des_1[]=
+{
+        {      0,1664,0,1201     }, /* 00 (320x200,320x400,640x200,640x400) */
+        {      0,1664,0,1201     }, /* 01 (320x350,640x350) */
+        {      0,1664,0,1201     }, /* 02 (360x400,720x400) */
+        {      0,1664,0,1201     }, /* 03 (720x350) */
+        {      0,1664,0,1201     }, /* 04 (640x480x60Hz) */
+        {      0,1664,0,1201     }, /* 05 (800x600x60Hz) */
+        {      0,1664,0,1201     }, /* 06 (1024x768x60Hz) */
+        {      0,1664,0,1201     }, /* 07 (1280x1024x60Hz) */
+        {      0,1664,0,1201     }, /* 08 (1400x1050x60Hz) */
+        {      0,1664,0,1201     }  /* 09 (1600x1200x60Hz) */
+};
+
+
+
+XGI330_LCDDataDesStruct2  XGI_LVDSNoScalingDesData[]=
+{
+        {     0, 648, 448, 405,  96,   2   }, /* 00 (320x200,320x400,640x200,640x400) */
+        {     0, 648, 448, 355,  96,   2   }, /* 01 (320x350,640x350) */
+        {     0, 648, 448, 405,  96,   2   }, /* 02 (360x400,720x400) */
+        {     0, 648, 448, 355,  96,   2   }, /* 03 (720x350) */
+        {     0, 648,  1, 483,  96,   2   }, /* 04 (640x480x60Hz) */
+        {     0, 840, 627, 600, 128,   4   }, /* 05 (800x600x60Hz) */
+        {     0,1048, 805, 770, 136,   6   }, /* 06 (1024x768x60Hz) */
+        {     0,1328,0,1025, 112,   3   }, /* 07 (1280x1024x60Hz) */
+        {     0,1438,0,1051, 112,   3   }, /* 08 (1400x1050x60Hz) ;;[ycchen] 12/19/02 */
+        {     0,1664,0,1201, 192,   3   }, /* 09 (1600x1200x60Hz) */
+        {     0,1328,0,0771, 112,   6   }  /* 0A (1280x768x60Hz) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1024x768Des_1x75[]=                       /* ; 1024x768 Full-screen */
+{
+        {0,1040,0,769}, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {0,1040,0,769}, /* ; 01 (320x350,640x350) */
+        {0,1040,0,769}, /* ; 02 (360x400,720x400) */
+        {0,1040,0,769}, /* ; 03 (720x350) */
+        {0,1040,0,769}, /* ; 04 (640x480x75Hz) */
+        {0,1040,0,769}, /* ; 05 (800x600x75Hz) */
+        {0,1040,0,769} /* ; 06 (1024x768x75Hz) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1024x768Des_2x75[]= /* ; 1024x768 center-screen (Enh. Mode) */
+{
+        {1142, 856,622,587 }, /* 00 (320x200,320x400,640x200,640x400) */
+        {1142, 856,597,562 }, /* 01 (320x350,640x350) */
+        {1142, 856,622,587 }, /* 02 (360x400,720x400) */
+        {1142, 856,597,562 }, /* 03 (720x350) */
+        {1142,1048,722,687 }, /* 04 (640x480x60Hz) */
+        {1232, 936,722,687 }, /* 05 (800x600x60Hz) */
+        {   0,1048,805,771 }  /* 06 (1024x768x60Hz) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1024x768Des_3x75[]= /* ; 1024x768 center-screen (St.Mode) */
+{
+        {320,24,622,587  }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {320,24,597,562  }, /* ; 01 (320x350,640x350) */
+        {320,24,622,587  }, /* ; 02 (360x400,720x400) */
+        {320,24,597,562  }, /* ; 03 (720x350) */
+        {320,24,722,687  } /* ; 04 (640x480x60Hz) */
+};
+
+XGI330_LVDSDataStruct XGI_LVDS1280x1024Des_1x75[]=
+{
+        {0,1296,0,1025}, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {0,1296,0,1025}, /* ; 01 (320x350,640x350) */
+        {0,1296,0,1025}, /* ; 02 (360x400,720x400) */
+        {0,1296,0,1025}, /* ; 03 (720x350) */
+        {0,1296,0,1025}, /* ; 04 (640x480x75Hz) */
+        {0,1296,0,1025}, /* ; 05 (800x600x75Hz) */
+        {0,1296,0,1025}, /* ; 06 (1024x768x75Hz) */
+        {0,1296,0,1025} /* ; 07 (1280x1024x75Hz) */
+};
+
+/* The Display setting for DE Mode Panel */
+XGI330_LVDSDataStruct XGI_LVDS1280x1024Des_2x75[]=   /* [ycchen] 02/18/03 Set DE as default */
+{
+        {1368,976,752,711 }, /* ; 00 (320x200,320x400,640x200,640x400) */
+        {1368,976,729,688 }, /* ; 01 (320x350,640x350) */
+        {1408,976,752,711 }, /* ; 02 (360x400,720x400) */
+        {1408,976,729,688 }, /* ; 03 (720x350) */
+        {1368,976,794,753 }, /* ; 04 (640x480x75Hz) */
+        {1448,1036,854,813}, /* ; 05 (800x600x75Hz) */
+        {1560,1168,938,897}, /* ; 06 (1024x768x75Hz) */
+        {0,1296,0,1025    } /* ; 07 (1280x1024x75Hz) */
+};
+
+XGI330_LCDDataDesStruct2 XGI_LVDSNoScalingDesDatax75[]=  /* Scaling LCD 75Hz */
+{
+       { 0,648,448,405,96,2  }, /* ; 00 (320x200,320x400,640x200,640x400) */
+       { 0,648,448,355,96,2  }, /* ; 01 (320x350,640x350) */
+       { 0,729,448,405,108,2 }, /* ; 02 (360x400,720x400) */
+       { 0,729,448,355,108,2 }, /* ; 03 (720x350) */
+       { 0,656,0,481,64,3    }, /* ; 04 (640x480x75Hz) */
+       { 0,816,0,601,80,3    }, /* ; 05 (800x600x75Hz) */
+       { 0,1040,0,769,96,3   }, /* ; 06 (1024x768x75Hz) */
+       { 0,1296,0,1025,144,3 }, /* ; 07 (1280x1024x75Hz) */
+       { 0,1448,0,1051,112,3 }, /* ; 08 (1400x1050x75Hz) ;;[ycchen] 12/19/02 */
+       { 0,1664,0,1201,192,3 }, /* ; 09 (1600x1200x75Hz) */
+       { 0,1328,0,771,112,6  }  /* ; 0A (1280x768x75Hz) */
+};
+
+XGI330_LVDSDataStruct  XGI330_LVDS640x480Data_1[]=
+{
+ {800, 449, 800, 449},
+ {800, 449, 800, 449},
+ {800, 449, 800, 449},
+ {800, 449, 800, 449},
+ {800, 525, 800, 525},
+ {1056, 628,1056, 628},
+ {1056, 628,1056, 628},
+ {1056, 628,1056, 628},
+ {1056, 628,1056, 628}
+};
+
+XGI330_CHTVDataStruct  XGI_CHTVUNTSCData[]=
+{
+ {840, 600, 840, 600},
+ {840, 600, 840, 600},
+ {840, 600, 840, 600},
+ {840, 600, 840, 600},
+ {784, 600, 784, 600},
+ {1064, 750,1064, 750}
+};
+
+XGI330_CHTVDataStruct  XGI_CHTVONTSCData[]=
+{
+ {840, 525, 840, 525},
+ {840, 525, 840, 525},
+ {840, 525, 840, 525},
+ {840, 525, 840, 525},
+ {784, 525, 784, 525},
+ {1040, 700,1040, 700}
+};
+
+XGI330_CHTVDataStruct  XGI_CHTVUPALData[]=
+{
+ {1008, 625,1008, 625},
+ {1008, 625,1008, 625},
+ {1008, 625,1008, 625},
+ {1008, 625,1008, 625},
+ {840, 750, 840, 750},
+ {936, 836, 936, 836}
+};
+
+XGI330_CHTVDataStruct  XGI_CHTVOPALData[]=
+{
+ {1008, 625,1008, 625},
+ {1008, 625,1008, 625},
+ {1008, 625,1008, 625},
+ {1008, 625,1008, 625},
+ {840, 625, 840, 625},
+ {960, 750, 960, 750}
+};
+
+XGI_LVDSCRT1HDataStruct  XGI_LVDSCRT11024x768_1_H[]=
+{
+                /* CR00,CR02,CR03,CR04,CR05,SR0B,SR0C,SR0E */
+                {{      0x4B,0x27,0x8F,0x32,0x1B,0x00,0x45,0x00 }}, /* 00 (320x) */
+                {{      0x4B,0x27,0x8F,0x2B,0x03,0x00,0x44,0x00 }}, /* 01 (360x) */
+                {{      0x55,0x31,0x99,0x46,0x1D,0x00,0x55,0x00 }}, /* 02 (400x) */
+                {{      0x63,0x3F,0x87,0x4A,0x93,0x00,0x01,0x00 }}, /* 03 (512x) */
+                {{      0x73,0x4F,0x97,0x55,0x86,0x00,0x05,0x00 }}, /* 04 (640x) */
+                {{      0x73,0x4F,0x97,0x55,0x86,0x00,0x05,0x00 }}, /* 05 (720x) */
+                {{      0x87,0x63,0x8B,0x69,0x1A,0x00,0x26,0x00 }}, /* 06 (800x) */
+                {{      0xA3,0x7F,0x87,0x86,0x97,0x00,0x02,0x00 }} /* 07 (1024x) */
+};
+
+XGI_LVDSCRT1HDataStruct  XGI_LVDSCRT11280x1024_1_H[]=
+{
+                /* CR00,CR02,CR03,CR04,CR05,SR0B,SR0C,SR0E */
+                {{      0x56,0x27,0x9A,0x30,0x1E,0x00,0x05,0x00 }}, /* 00 (320x) */
+                {{      0x56,0x27,0x9A,0x30,0x1E,0x00,0x05,0x00 }}, /* 01 (360x) */
+                {{      0x60,0x31,0x84,0x3A,0x88,0x00,0x01,0x00 }}, /* 02 (400x) */
+                {{      0x6E,0x3F,0x92,0x48,0x96,0x00,0x01,0x00 }}, /* 03 (512x) */
+                {{      0x7E,0x4F,0x82,0x58,0x06,0x00,0x06,0x00 }}, /* 04 (640x) */
+                {{      0x7E,0x4F,0x82,0x58,0x06,0x00,0x06,0x00 }}, /* 05 (720x) */
+                {{      0x92,0x63,0x96,0x6C,0x1A,0x00,0x06,0x00 }}, /* 06 (800x) */
+                {{      0xAE,0x7F,0x92,0x88,0x96,0x00,0x02,0x00 }}, /* 07 (1024x) */
+                {{      0xCE,0x9F,0x92,0xA8,0x16,0x00,0x07,0x00 }} /* 08 (1280x) */
+};
+
+XGI_LVDSCRT1HDataStruct  XGI_LVDSCRT11024x768_2_H[]=
+{
+                /* CR00,CR02,CR03,CR04,CR05,SR0B,SR0C,SR0E */
+                {{      0x63,0x27,0x87,0x3B,0x8C,0x00,0x01,0x00 }}, /* 00 (320x) */
+                {{      0x63,0x27,0x87,0x3B,0x8C,0x00,0x01,0x00 }}, /* 01 (360x) */
+                {{      0x63,0x31,0x87,0x3D,0x8E,0x00,0x01,0x00 }}, /* 02 (400x) */
+                {{      0x63,0x3F,0x87,0x45,0x96,0x00,0x01,0x00 }}, /* 03 (512x) */
+                {{      0xA3,0x4F,0x87,0x6E,0x9F,0x00,0x06,0x00 }}, /* 04 (640x) */
+                {{      0xA3,0x4F,0x87,0x6E,0x9F,0x00,0x06,0x00 }}, /* 05 (720x) */
+                {{      0xA3,0x63,0x87,0x78,0x89,0x00,0x02,0x00 }}, /* 06 (800x) */
+                {{      0xA3,0x7F,0x87,0x86,0x97,0x00,0x02,0x00 }} /* 07 (1024x) */
+};
+
+XGI_LVDSCRT1HDataStruct  XGI_LVDSCRT11280x1024_2_H[]=
+{
+                /* CR00,CR02,CR03,CR04,CR05,SR0B,SR0C,SR0E */
+                {{      0x7E,0x3B,0x9A,0x44,0x12,0x00,0x01,0x00 }}, /* 00 (320x) */
+                {{      0x7E,0x3B,0x9A,0x44,0x12,0x00,0x01,0x00 }}, /* 01 (360x) */
+                {{      0x7E,0x40,0x84,0x49,0x91,0x00,0x01,0x00 }}, /* 02 (400x) */
+                {{      0x7E,0x47,0x93,0x50,0x9E,0x00,0x01,0x00 }}, /* 03 (512x) */
+                {{      0xCE,0x77,0x8A,0x80,0x8E,0x00,0x02,0x00 }}, /* 04 (640x) */
+                {{      0xCE,0x77,0x8A,0x80,0x8E,0x00,0x02,0x00 }}, /* 05 (720x) */
+                {{      0xCE,0x81,0x94,0x8A,0x98,0x00,0x02,0x00 }}, /* 06 (800x) */
+                {{      0xCE,0x8F,0x82,0x98,0x06,0x00,0x07,0x00 }}, /* 07 (1024x) */
+                {{      0xCE,0x9F,0x92,0xA8,0x16,0x00,0x07,0x00 }} /* 08 (1280x) */
+};
+
+XGI_LVDSCRT1HDataStruct XGI_LVDSCRT11400x1050_1_H[]=
+{               /* CR00,CR02,CR03,CR04,CR05,SR0B,SR0C,SR0E */
+                {{      0x47,0x27,0x8B,0x2C,0x1A,0x00,0x05,0x00 }}, /* 00 (320x) */
+                {{      0x47,0x27,0x8B,0x30,0x1E,0x00,0x05,0x00 }}, /* 01 (360x) */
+                {{      0x51,0x31,0x95,0x36,0x04,0x00,0x01,0x00 }}, /* 02 (400x) */
+                {{      0x5F,0x3F,0x83,0x44,0x92,0x00,0x01,0x00 }}, /* 03 (512x) */
+                {{      0x6F,0x4F,0x93,0x54,0x82,0x00,0x05,0x00 }}, /* 04 (640x) */
+                {{      0x6F,0x4F,0x93,0x54,0x82,0x00,0x05,0x00 }}, /* 05 (720x) */
+                {{      0x83,0x63,0x87,0x68,0x16,0x00,0x06,0x00 }}, /* 06 (800x) */
+                {{      0x9F,0x7F,0x83,0x84,0x92,0x00,0x02,0x00 }}, /* 07 (1024x) */
+                {{      0xBF,0x9F,0x83,0xA4,0x12,0x00,0x07,0x00 }}, /* 08 (1280x) */
+                {{      0xCE,0xAE,0x92,0xB3,0x01,0x00,0x03,0x00 }} /* 09 (1400x) */
+};
+
+XGI_LVDSCRT1HDataStruct XGI_LVDSCRT11400x1050_2_H[]=
+{               /* CR00,CR02,CR03,CR04,CR05,SR0B,SR0C,SR0E */
+                {{      0x76,0x3F,0x83,0x45,0x8C,0x00,0x41,0x00 }}, /* 00 (320x) */
+                {{      0x76,0x3F,0x83,0x45,0x8C,0x00,0x41,0x00 }}, /* 01 (360x) */
+                {{      0x76,0x31,0x9A,0x48,0x9F,0x00,0x41,0x00 }}, /* 02 (400x) */
+                {{      0x76,0x3F,0x9A,0x4F,0x96,0x00,0x41,0x00 }}, /* 03 (512x) */
+                {{      0xCE,0x7E,0x82,0x87,0x9E,0x00,0x02,0x00 }}, /* 04 (640x) */
+                {{      0xCE,0x7E,0x82,0x87,0x9E,0x00,0x02,0x00 }}, /* 05 (720x) */
+                {{      0xCE,0x63,0x92,0x96,0x04,0x00,0x07,0x00 }}, /* 06 (800x) */
+                {{      0xCE,0x7F,0x92,0xA4,0x12,0x00,0x07,0x00 }}, /* 07 (1024x) */
+                {{      0xCE,0x9F,0x92,0xB4,0x02,0x00,0x03,0x00 }}, /* 08 (1280x) */
+                {{      0xCE,0xAE,0x92,0xBC,0x0A,0x00,0x03,0x00 }} /* 09 (1400x) */
+};
+
+XGI_LVDSCRT1HDataStruct XGI_LVDSCRT11600x1200_1_H[]=
+/* ;302lv channelA [ycchen] 12/05/02 LCDHT=2048 */
+{   /* ; CR00,CR02,CR03,CR04,CR05,SR0B,SR0C,SR0E */
+                {{      0x5B,0x27,0x9F,0x32,0x0A,0x00,0x01,0x00 }},/* 00 (320x) */
+                {{      0x5B,0x27,0x9F,0x32,0x0A,0x00,0x01,0x00 }},/* 01 (360x) */
+                {{      0x65,0x31,0x89,0x3C,0x94,0x00,0x01,0x00 }},/* 02 (400x) */
+                {{      0x73,0x3F,0x97,0x4A,0x82,0x00,0x05,0x00 }},/* 03 (512x) */
+                {{      0x83,0x4F,0x87,0x51,0x09,0x00,0x06,0x00 }},/* 04 (640x) */
+                {{      0x83,0x4F,0x87,0x51,0x09,0x00,0x06,0x00 }},/* 05 (720x) */
+                {{      0x97,0x63,0x9B,0x65,0x1D,0x00,0x06,0xF0 }},/* 06 (800x) */
+                {{      0xB3,0x7F,0x97,0x81,0x99,0x00,0x02,0x00 }},/* 07 (1024x) */
+                {{      0xD3,0x9F,0x97,0xA1,0x19,0x00,0x07,0x00 }},/* 08 (1280x) */
+                {{      0xE2,0xAE,0x86,0xB9,0x91,0x00,0x03,0x00 }},/* 09 (1400x) */
+                {{      0xFB,0xC7,0x9F,0xC9,0x81,0x00,0x07,0x00 }} /* 0A (1600x) */
+};
+
+XGI_LVDSCRT1VDataStruct XGI_LVDSCRT11024x768_1_V[]=
+{               /* CR06,CR07,CR10,CR11,CR15,CR16,SR0A+CR09(5->7) */
+                {{      0x97,0x1F,0x60,0x87,0x5D,0x83,0x10      }}, /* 00 (x350) */
+                {{      0xB4,0x1F,0x92,0x89,0x8F,0xB5,0x30      }}, /* 01 (x400) */
+                {{      0x04,0x3E,0xE2,0x89,0xDF,0x05,0x00      }}, /* 02 (x480) */
+                {{      0x7C,0xF0,0x5A,0x8F,0x57,0x7D,0xA0      }}, /* 03 (x600) */
+                {{      0x24,0xF5,0x02,0x88,0xFF,0x25,0x90      }} /* 04 (x768) */
+};
+
+XGI_LVDSCRT1VDataStruct XGI_LVDSCRT11024x768_2_V[]=
+{               /* CR06,CR07,CR10,CR11,CR15,CR16,SR0A */
+                {{      0x24,0xBB,0x31,0x87,0x5D,0x25,0x30      }}, /* 00 (x350) */
+                {{      0x24,0xBB,0x4A,0x80,0x8F,0x25,0x30      }}, /* 01 (x400) */
+                {{      0x24,0xBB,0x72,0x88,0xDF,0x25,0x30      }}, /* 02 (x480) */
+                {{      0x24,0xF1,0xAE,0x84,0x57,0x25,0xB0      }}, /* 03 (x600) */
+                {{      0x24,0xF5,0x02,0x88,0xFF,0x25,0x90      }} /* 04 (x768) */
+};
+
+XGI_LVDSCRT1VDataStruct XGI_LVDSCRT11280x1024_1_V[]=
+{               /* CR06,CR07,CR10,CR11,CR15,CR16,SR0A */
+                {{       0x86,0x1F,0x5E,0x82,0x5D,0x87,0x00     }}, /* 00 (x350) */
+                {{       0xB8,0x1F,0x90,0x84,0x8F,0xB9,0x30     }}, /* 01 (x400) */
+                {{       0x08,0x3E,0xE0,0x84,0xDF,0x09,0x00     }}, /* 02 (x480) */
+                {{       0x80,0xF0,0x58,0x8C,0x57,0x81,0xA0     }}, /* 03 (x600) */
+                {{       0x28,0xF5,0x00,0x84,0xFF,0x29,0x90     }}, /* 04 (x768) */
+                {{       0x28,0x5A,0x13,0x87,0xFF,0x29,0xA9     }} /* 05 (x1024) */
+};
+
+XGI_LVDSCRT1VDataStruct XGI_LVDSCRT11280x1024_2_V[]=
+{               /* CR06,CR07,CR10,CR11,CR15,CR16,SR0A */
+                {{      0x28,0xD2,0xAF,0x83,0xAE,0xD8,0xA1      }}, /* 00 (x350) */
+                {{      0x28,0xD2,0xC8,0x8C,0xC7,0xF2,0x81      }}, /* 01 (x400) */
+                {{      0x28,0xD2,0xF0,0x84,0xEF,0x1A,0xB1      }}, /* 02 (x480) */
+                {{      0x28,0xDE,0x2C,0x8F,0x2B,0x56,0x91      }}, /* 03 (x600) */
+                {{      0x28,0xDE,0x80,0x83,0x7F,0xAA,0x91      }}, /* 04 (x768) */
+                {{      0x28,0x5A,0x13,0x87,0xFF,0x29,0xA9      }} /* 05 (x1024) */
+};
+
+XGI_LVDSCRT1VDataStruct XGI_LVDSCRT11400x1050_1_V[]=
+{               /* CR06,CR07,CR10,CR11,CR15,CR16,SR0A */
+                {{      0x6C,0x1F,0x60,0x84,0x5D,0x6D,0x10      }}, /* 00 (x350) */
+                {{      0x9E,0x1F,0x93,0x86,0x8F,0x9F,0x30      }}, /* 01 (x400) */
+                {{      0xEE,0x1F,0xE2,0x86,0xDF,0xEF,0x10      }}, /* 02 (x480) */
+                {{      0x66,0xF0,0x5A,0x8e,0x57,0x67,0xA0      }}, /* 03 (x600) */
+                {{      0x0E,0xF5,0x02,0x86,0xFF,0x0F,0x90      }}, /* 04 (x768) */
+                {{      0x0E,0x5A,0x02,0x86,0xFF,0x0F,0x89      }}, /* 05 (x1024) */
+                {{      0x28,0x10,0x1A,0x80,0x19,0x29,0x0F      }} /* 06 (x1050) */
+};
+
+XGI_LVDSCRT1VDataStruct XGI_LVDSCRT11400x1050_2_V[]=
+{              /* CR06,CR07,CR10,CR11,CR15,CR16,SR0A */
+                {{      0x28,0x92,0xB6,0x83,0xB5,0xCF,0x81      }}, /* 00 (x350) */
+                {{      0x28,0x92,0xD5,0x82,0xD4,0xEE,0x81      }}, /* 01 (x400) */
+                {{      0x28,0x92,0xFD,0x8A,0xFC,0x16,0xB1      }}, /* 02 (x480) */
+                {{      0x28,0xD4,0x39,0x86,0x57,0x29,0x81      }}, /* 03 (x600) */
+                {{      0x28,0xD4,0x8D,0x9A,0xFF,0x29,0xA1      }}, /* 04 (x768) */
+                {{      0x28,0x5A,0x0D,0x9A,0xFF,0x29,0xA9      }}, /* 05 (x1024) */
+                {{      0x28,0x10,0x1A,0x87,0x19,0x29,0x8F      }} /* 06 (x1050) */
+};
+
+XGI_LVDSCRT1VDataStruct XGI_LVDSCRT11600x1200_1_V[]=
+{
+               /* CR06,CR07,CR10,CR11,CR15,CR16,SR0A+CR09(5->7) */
+                {{      0xd4,0x1F,0x81,0x84,0x5D,0xd5,0x10      }}, /* 00 (x350) */
+                {{      0x06,0x3e,0xb3,0x86,0x8F,0x07,0x20      }}, /* 01 (x400) */
+                {{      0x56,0xba,0x03,0x86,0xDF,0x57,0x00      }}, /* 02 (x480) */
+                {{      0xce,0xF0,0x7b,0x8e,0x57,0xcf,0xa0      }}, /* 03 (x600) */
+                {{      0x76,0xF5,0x23,0x86,0xFF,0x77,0x90      }}, /* 04 (x768) */
+                {{      0x76,0x5A,0x23,0x86,0xFF,0x77,0x89      }}, /* 05 (x1024) */
+                {{      0x90,0x10,0x1A,0x8E,0x19,0x91,0x2F      }}, /* 06 (x1050) */
+                {{      0x26,0x11,0xd3,0x86,0xaF,0x27,0x3f      }} /* 07 (x1200) */
+};
+
+XGI_LVDSCRT1HDataStruct XGI_LVDSCRT11024x768_1_Hx75[]=
+{       /* CR00,CR02,CR03,CR04,CR05,SR0B,SR0C,SR0E */
+    {{      0x4B,0x27,0x8F,0x32,0x1B,0x00,0x45,0x00 }},/* ; 00 (320x) */
+    {{      0x4B,0x27,0x8F,0x2B,0x03,0x00,0x44,0x00 }},/* ; 01 (360x) */
+    {{      0x55,0x31,0x99,0x46,0x1D,0x00,0x55,0x00 }},/* ; 02 (400x) */
+    {{      0x63,0x3F,0x87,0x4A,0x93,0x00,0x01,0x00 }},/* ; 03 (512x) */
+    {{      0x6F,0x4F,0x93,0x54,0x80,0x00,0x05,0x00 }},/* ; 04 (640x) */
+    {{      0x6F,0x4F,0x93,0x54,0x80,0x00,0x05,0x00 }},/* ; 05 (720x) */
+    {{      0x83,0x63,0x87,0x68,0x14,0x00,0x26,0x00 }},/* ; 06 (800x) */
+    {{      0x9F,0x7F,0x83,0x85,0x91,0x00,0x02,0x00 }} /* ; 07 (1024x) */
+};
+
+XGI_LVDSCRT1VDataStruct XGI_LVDSCRT11024x768_1_Vx75[]=
+{       /* CR06,CR07,CR10,CR11,CR15,CR16,SR0A+CR09(5->7) */
+    {{      0x97,0x1F,0x60,0x87,0x5D,0x83,0x10      }},/* ; 00 (x350) */
+    {{      0xB4,0x1F,0x92,0x89,0x8F,0xB5,0x30      }},/* ; 01 (x400) */
+    {{      0xFE,0x1F,0xE0,0x84,0xDF,0xFF,0x10      }},/* ; 02 (x480) */
+    {{      0x76,0xF0,0x58,0x8C,0x57,0x77,0xA0      }},/* ; 03 (x600) */
+    {{      0x1E,0xF5,0x00,0x83,0xFF,0x1F,0x90      }} /* ; 04 (x768) */
+};
+
+XGI_LVDSCRT1HDataStruct XGI_LVDSCRT11024x768_2_Hx75[]=
+{       /* CR00,CR02,CR03,CR04,CR05,SR0B,SR0C,SR0E */
+    {{      0x63,0x27,0x87,0x3B,0x8C,0x00,0x01,0x00 }},/* ; 00 (320x) */
+    {{      0x63,0x27,0x87,0x3B,0x8C,0x00,0x01,0x00 }},/* ; 01 (360x) */
+    {{      0x63,0x31,0x87,0x3D,0x8E,0x00,0x01,0x00 }},/* ; 02 (400x) */
+    {{      0x63,0x3F,0x87,0x45,0x96,0x00,0x01,0x00 }},/* ; 03 (512x) */
+    {{      0xA3,0x4F,0x87,0x6E,0x9F,0x00,0x06,0x00 }},/* ; 04 (640x) */
+    {{      0xA3,0x4F,0x87,0x6E,0x9F,0x00,0x06,0x00 }},/* ; 05 (720x) */
+    {{      0xA3,0x63,0x87,0x78,0x89,0x00,0x02,0x00 }},/* ; 06 (800x) */
+    {{      0xA3,0x7F,0x87,0x86,0x97,0x00,0x02,0x00 }} /* ; 07 (1024x) */
+};
+
+XGI_LVDSCRT1VDataStruct XGI_LVDSCRT11024x768_2_Vx75[]=
+{       /* CR06,CR07,CR10,CR11,CR15,CR16,SR0A */
+    {{      0x24,0xBB,0x31,0x87,0x5D,0x25,0x30      }},/* ; 00 (x350) */
+    {{      0x24,0xBB,0x4A,0x80,0x8F,0x25,0x30      }},/* ; 01 (x400) */
+    {{      0x24,0xBB,0x72,0x88,0xDF,0x25,0x30      }},/* ; 02 (x480) */
+    {{      0x24,0xF1,0xAE,0x84,0x57,0x25,0xB0      }},/* ; 03 (x600) */
+    {{      0x24,0xF5,0x02,0x88,0xFF,0x25,0x90      }} /* ; 04 (x768) */
+};
+
+XGI_LVDSCRT1HDataStruct XGI_LVDSCRT11280x1024_1_Hx75[]=
+{      /* CR00,CR02,CR03,CR04,CR05,SR0B,SR0C,SR0E */
+    {{      0x56,0x27,0x9A,0x30,0x1E,0x00,0x05,0x00 }},/* ; 00 (320x) */
+    {{      0x56,0x27,0x9A,0x30,0x1E,0x00,0x05,0x00 }},/* ; 01 (360x) */
+    {{      0x60,0x31,0x84,0x3A,0x88,0x00,0x01,0x00 }},/* ; 02 (400x) */
+    {{      0x6E,0x3F,0x92,0x48,0x96,0x00,0x01,0x00 }},/* ; 03 (512x) */
+    {{      0x7E,0x4F,0x82,0x54,0x06,0x00,0x06,0x00 }},/* ; 04 (640x) */
+    {{      0x7E,0x4F,0x82,0x54,0x06,0x00,0x06,0x00 }},/* ; 05 (720x) */
+    {{      0x92,0x63,0x96,0x68,0x1A,0x00,0x06,0x00 }},/* ; 06 (800x) */
+    {{      0xAE,0x7F,0x92,0x84,0x96,0x00,0x02,0x00 }},/* ; 07 (1024x) */
+    {{      0xCE,0x9F,0x92,0xA5,0x17,0x00,0x07,0x00 }} /* ; 08 (1280x) */
+};
+
+XGI_LVDSCRT1VDataStruct XGI_LVDSCRT11280x1024_1_Vx75[]=
+{       /* CR06,CR07,CR10,CR11,CR15,CR16,SR0A */
+    {{      0x86,0xD1,0xBC,0x80,0xBB,0xE5,0x00      }},/* ; 00 (x350) */
+    {{      0xB8,0x1F,0x90,0x84,0x8F,0xB9,0x30      }},/* ; 01 (x400) */
+    {{      0x08,0x3E,0xE0,0x84,0xDF,0x09,0x00      }},/* ; 02 (x480) */
+    {{      0x80,0xF0,0x58,0x8C,0x57,0x81,0xA0      }},/* ; 03 (x600) */
+    {{      0x28,0xF5,0x00,0x84,0xFF,0x29,0x90      }},/* ; 04 (x768) */
+    {{      0x28,0x5A,0x13,0x87,0xFF,0x29,0xA9      }} /* ; 05 (x1024) */
+};
+
+XGI_LVDSCRT1HDataStruct XGI_LVDSCRT11280x1024_2_Hx75[]=
+{
+        /* CR00,CR02,CR03,CR04,CR05,SR0B,SR0C,SR0E */
+    {{      0x7E,0x3B,0x9A,0x44,0x12,0x00,0x01,0x00 }},/* ; 00 (320x) */
+    {{      0x7E,0x3B,0x9A,0x44,0x12,0x00,0x01,0x00 }},/* ; 01 (360x) */
+    {{      0x7E,0x40,0x84,0x49,0x91,0x00,0x01,0x00 }},/* ; 02 (400x) */
+    {{      0x7E,0x47,0x93,0x50,0x9E,0x00,0x01,0x00 }},/* ; 03 (512x) */
+    {{      0xCE,0x77,0x8A,0x80,0x8E,0x00,0x02,0x00 }},/* ; 04 (640x) */
+    {{      0xCE,0x77,0x8A,0x80,0x8E,0x00,0x02,0x00 }},/* ; 05 (720x) */
+    {{      0xCE,0x81,0x94,0x8A,0x98,0x00,0x02,0x00 }},/* ; 06 (800x) */
+    {{      0xCE,0x8F,0x82,0x98,0x06,0x00,0x07,0x00 }},/* ; 07 (1024x) */
+    {{      0xCE,0x9F,0x92,0xA8,0x16,0x00,0x07,0x00 }} /* ; 08 (1280x) */
+};
+
+XGI_LVDSCRT1VDataStruct XGI_LVDSCRT11280x1024_2_Vx75[]=
+{
+        /* CR06,CR07,CR10,CR11,CR15,CR16,SR0A */
+     {{     0x28,0xD2,0xAF,0x83,0xAE,0xD8,0xA1     }},/* ; 00 (x350) */
+     {{     0x28,0xD2,0xC8,0x8C,0xC7,0xF2,0x81     }},/* ; 01 (x400) */
+     {{     0x28,0xD2,0xF0,0x84,0xEF,0x1A,0xB1     }},/* ; 02 (x480) */
+     {{     0x28,0xDE,0x2C,0x8F,0x2B,0x56,0x91     }},/* ; 03 (x600) */
+     {{     0x28,0xDE,0x80,0x83,0x7F,0xAA,0x91     }},/* ; 04 (x768) */
+     {{     0x28,0x5A,0x13,0x87,0xFF,0x29,0xA9     }} /* ; 05 (x1024) */
+};
+
+XGI_LVDSCRT1DataStruct  XGI_CHTVCRT1UNTSC[]=
+{
+ {{0x64,0x4f,0x88,0x56,0x9f,0x56,0x3e,
+    0xe8,0x84,0x8f,0x57,0x20,0x00,0x01,0x00 }},
+ {{0x64,0x4f,0x88,0x56,0x9f,0x56,0x3e,
+    0xd0,0x82,0x5d,0x57,0x00,0x00,0x01,0x00 }},
+ {{0x64,0x4f,0x88,0x56,0x9f,0x56,0x3e,
+    0xe8,0x84,0x8f,0x57,0x20,0x00,0x01,0x00 }},
+ {{0x64,0x4f,0x88,0x56,0x9f,0x56,0x3e,
+    0xd0,0x82,0x5d,0x57,0x00,0x00,0x01,0x00 }},
+ {{0x5d,0x4f,0x81,0x53,0x9c,0x56,0xba,
+    0x18,0x84,0xdf,0x57,0x00,0x00,0x01,0x00 }},
+ {{0x80,0x63,0x84,0x6c,0x17,0xec,0xf0,
+   0x90,0x8c,0x57,0xed,0x20,0x00,0x06,0x01 }}
+};
+
+XGI_LVDSCRT1DataStruct  XGI_CHTVCRT1ONTSC[]=
+{
+ {{0x64,0x4f,0x88,0x5a,0x9f,0x0b,0x3e,
+    0xc0,0x84,0x8f,0x0c,0x20,0x00,0x01,0x00 }},
+ {{0x64,0x4f,0x88,0x5a,0x9f,0x0b,0x3e,
+    0xb0,0x8d,0x5d,0x0c,0x00,0x00,0x01,0x00 }},
+ {{0x64,0x4f,0x88,0x5a,0x9f,0x0b,0x3e,
+    0xc0,0x84,0x8f,0x0c,0x20,0x00,0x01,0x00 }},
+ {{0x64,0x4f,0x88,0x5a,0x9f,0x0b,0x3e,
+    0xb0,0x8d,0x5d,0x0c,0x00,0x00,0x01,0x00 }},
+ {{0x5d,0x4f,0x81,0x56,0x9c,0x0b,0x3e,
+    0xe8,0x84,0xdf,0x0c,0x00,0x00,0x01,0x00 }},
+ {{0x7d,0x63,0x81,0x6a,0x16,0xba,0xf0,
+   0x7f,0x86,0x57,0xbb,0x00,0x00,0x06,0x01 }}
+};
+
+XGI_LVDSCRT1DataStruct  XGI_CHTVCRT1UPAL[]=
+{
+ {{0x79,0x4f,0x9d,0x5a,0x90,0x6f,0x3e,
+    0xf8,0x83,0x8f,0x70,0x20,0x00,0x05,0x00 }},
+ {{0x79,0x4f,0x9d,0x5a,0x90,0x6f,0x3e,
+    0xde,0x81,0x5d,0x70,0x00,0x00,0x05,0x00 }},
+ {{0x79,0x4f,0x9d,0x5a,0x90,0x6f,0x3e,
+    0xf8,0x83,0x8f,0x70,0x20,0x00,0x05,0x00 }},
+ {{0x79,0x4f,0x9d,0x5a,0x90,0x6f,0x3e,
+    0xde,0x81,0x5d,0x70,0x00,0x00,0x05,0x00 }},
+ {{0x64,0x4f,0x88,0x55,0x80,0xec,0xba,
+    0x50,0x84,0xdf,0xed,0x00,0x00,0x05,0x00 }},
+ {{0x70,0x63,0x94,0x68,0x8d,0x42,0xf1,
+   0xc8,0x8c,0x57,0xe9,0x20,0x00,0x05,0x01 }}
+};
+
+XGI_LVDSCRT1DataStruct  XGI_CHTVCRT1OPAL[]=
+{
+ {{0x79,0x4f,0x9d,0x5a,0x90,0x6f,0x3e,
+    0xf0,0x83,0x8f,0x70,0x20,0x00,0x05,0x00 }},
+ {{0x79,0x4f,0x9d,0x5a,0x90,0x6f,0x3e,
+    0xde,0x81,0x5d,0x70,0x00,0x00,0x05,0x00 }},
+ {{0x79,0x4f,0x9d,0x5a,0x90,0x6f,0x3e,
+    0xf0,0x83,0x8f,0x70,0x20,0x00,0x05,0x00 }},
+ {{0x79,0x4f,0x9d,0x5a,0x90,0x6f,0x3e,
+    0xde,0x81,0x5d,0x70,0x00,0x00,0x05,0x00 }},
+ {{0x64,0x4f,0x88,0x55,0x80,0x6f,0xba,
+    0x20,0x83,0xdf,0x70,0x00,0x00,0x05,0x00 }},
+ {{0x73,0x63,0x97,0x69,0x8e,0xec,0xf0,
+   0x90,0x8c,0x57,0xed,0x20,0x00,0x05,0x01 }}
+};
+
+/*add for new UNIVGABIOS*/
+XGI330_LCDDataTablStruct XGI_LCDDataTable[]=
+{
+  {Panel1024x768,0x0019,0x0001,0},  /* XGI_ExtLCD1024x768Data */
+  {Panel1024x768,0x0019,0x0000,1},  /* XGI_StLCD1024x768Data */
+  {Panel1024x768,0x0018,0x0010,2},  /* XGI_CetLCD1024x768Data */
+  {Panel1280x1024,0x0019,0x0001,3},  /* XGI_ExtLCD1280x1024Data */
+  {Panel1280x1024,0x0019,0x0000,4},  /* XGI_StLCD1280x1024Data */
+  {Panel1280x1024,0x0018,0x0010,5},  /* XGI_CetLCD1280x1024Data */
+  {Panel1400x1050,0x0019,0x0001,6},  /* XGI_ExtLCD1400x1050Data */
+  {Panel1400x1050,0x0019,0x0000,7},  /* XGI_StLCD1400x1050Data */
+  {Panel1400x1050,0x0018,0x0010,8},  /* XGI_CetLCD1400x1050Data */
+  {Panel1600x1200,0x0019,0x0001,9},   /* XGI_ExtLCD1600x1200Data */
+  {Panel1600x1200,0x0019,0x0000,10},  /* XGI_StLCD1600x1200Data */
+  {PanelRef60Hz,0x0008,0x0008,11},      /* XGI_NoScalingData */
+  {Panel1024x768x75,0x0019,0x0001,12},  /* XGI_ExtLCD1024x768x75Data */
+  {Panel1024x768x75,0x0019,0x0000,13},  /* XGI_StLCD1024x768x75Data */
+  {Panel1024x768x75,0x0018,0x0010,14},  /* XGI_CetLCD1024x768x75Data */
+  {Panel1280x1024x75,0x0019,0x0001,15}, /* XGI_ExtLCD1280x1024x75Data */
+  {Panel1280x1024x75,0x0019,0x0000,16}, /* XGI_StLCD1280x1024x75Data */
+  {Panel1280x1024x75,0x0018,0x0010,17}, /* XGI_CetLCD1280x1024x75Data */
+  {PanelRef75Hz,0x0008,0x0008,18},      /* XGI_NoScalingDatax75 */
+  {0xFF,0x0000,0x0000,0}                /* End of table */
+};
+
+XGI330_LCDDataTablStruct XGI_LCDDesDataTable[]=
+{
+  {Panel1024x768,0x0019,0x0001,0}, /* XGI_ExtLCDDes1024x768Data */
+  {Panel1024x768,0x0019,0x0000,1}, /* XGI_StLCDDes1024x768Data */
+  {Panel1024x768,0x0018,0x0010,2}, /* XGI_CetLCDDes1024x768Data */
+  {Panel1280x1024,0x0019,0x0001,3}, /* XGI_ExtLCDDes1280x1024Data */
+  {Panel1280x1024,0x0019,0x0000,4}, /* XGI_StLCDDes1280x1024Data */
+  {Panel1280x1024,0x0018,0x0010,5}, /* XGI_CetLCDDes1280x1024Data */
+  {Panel1400x1050,0x0019,0x0001,6}, /* XGI_ExtLCDDes1400x1050Data */
+  {Panel1400x1050,0x0019,0x0000,7}, /* XGI_StLCDDes1400x1050Data */
+  {Panel1400x1050,0x0418,0x0010,8}, /* XGI_CetLCDDes1400x1050Data */
+  {Panel1400x1050,0x0418,0x0410,9}, /* XGI_CetLCDDes1400x1050Data2 */
+  {Panel1600x1200,0x0019,0x0001,10}, /* XGI_ExtLCDDes1600x1200Data */
+  {Panel1600x1200,0x0019,0x0000,11}, /* XGI_StLCDDes1600x1200Data */
+  {PanelRef60Hz,0x0008,0x0008,12},      /* XGI_NoScalingDesData */
+  {Panel1024x768x75,0x0019,0x0001,13},  /* XGI_ExtLCDDes1024x768x75Data */
+  {Panel1024x768x75,0x0019,0x0000,14},  /* XGI_StLCDDes1024x768x75Data */
+  {Panel1024x768x75,0x0018,0x0010,15},  /* XGI_CetLCDDes1024x768x75Data */
+  {Panel1280x1024x75,0x0019,0x0001,16}, /* XGI_ExtLCDDes1280x1024x75Data */
+  {Panel1280x1024x75,0x0019,0x0000,17}, /* XGI_StLCDDes1280x1024x75Data */
+  {Panel1280x1024x75,0x0018,0x0010,18}, /* XGI_CetLCDDes1280x1024x75Data */
+  {PanelRef75Hz,0x0008,0x0008,19},      /* XGI_NoScalingDesDatax75 */
+  {0xFF,0x0000,0x0000,0}
+};
+
+XGI330_LCDDataTablStruct XGI_EPLLCDCRT1Ptr_H[]=
+{
+  {Panel1024x768,0x0018,0x0000,0}, /* XGI_LVDSCRT11024x768_1_H */
+  {Panel1024x768,0x0018,0x0010,1}, /* XGI_LVDSCRT11024x768_2_H */
+  {Panel1280x1024,0x0018,0x0000,2}, /* XGI_LVDSCRT11280x1024_1_H */
+  {Panel1280x1024,0x0018,0x0010,3}, /* XGI_LVDSCRT11280x1024_2_H */
+  {Panel1400x1050,0x0018,0x0000,4}, /* XGI_LVDSCRT11400x1050_1_H */
+  {Panel1400x1050,0x0018,0x0010,5}, /* XGI_LVDSCRT11400x1050_2_H */
+  {Panel1600x1200,0x0018,0x0000,6},  /* XGI_LVDSCRT11600x1200_1_H */
+  {Panel1024x768x75,0x0018,0x0000,7},   /* XGI_LVDSCRT11024x768_1_Hx75 */
+  {Panel1024x768x75,0x0018,0x0010,8},   /* XGI_LVDSCRT11024x768_2_Hx75 */
+  {Panel1280x1024x75,0x0018,0x0000,9},  /* XGI_LVDSCRT11280x1024_1_Hx75 */
+  {Panel1280x1024x75,0x0018,0x0010,10}, /* XGI_LVDSCRT11280x1024_2_Hx75 */
+  {0xFF,0x0000,0x0000,0}
+};
+
+XGI330_LCDDataTablStruct XGI_EPLLCDCRT1Ptr_V[]=
+{
+  {Panel1024x768,0x0018,0x0000,0}, /* XGI_LVDSCRT11024x768_1_V */
+  {Panel1024x768,0x0018,0x0010,1}, /* XGI_LVDSCRT11024x768_2_V */
+  {Panel1280x1024,0x0018,0x0000,2}, /* XGI_LVDSCRT11280x1024_1_V */
+  {Panel1280x1024,0x0018,0x0010,3}, /* XGI_LVDSCRT11280x1024_2_V */
+  {Panel1400x1050,0x0018,0x0000,4}, /* XGI_LVDSCRT11400x1050_1_V */
+  {Panel1400x1050,0x0018,0x0010,5}, /* XGI_LVDSCRT11400x1050_2_V */
+  {Panel1600x1200,0x0018,0x0000,6}, /* XGI_LVDSCRT11600x1200_1_V */
+  {Panel1024x768x75,0x0018,0x0000,7},   /* XGI_LVDSCRT11024x768_1_Vx75 */
+  {Panel1024x768x75,0x0018,0x0010,8},   /* XGI_LVDSCRT11024x768_2_Vx75 */
+  {Panel1280x1024x75,0x0018,0x0000,9},  /* XGI_LVDSCRT11280x1024_1_Vx75 */
+  {Panel1280x1024x75,0x0018,0x0010,10}, /* XGI_LVDSCRT11280x1024_2_Vx75 */
+  {0xFF,0x0000,0x0000,0}
+};
+
+XGI330_LCDDataTablStruct XGI_EPLLCDDataPtr[]=
+{
+  {Panel1024x768,0x0018,0x0000,0}, /* XGI_LVDS1024x768Data_1 */
+  {Panel1024x768,0x0018,0x0010,1}, /* XGI_LVDS1024x768Data_2 */
+  {Panel1280x1024,0x0018,0x0000,2}, /* XGI_LVDS1280x1024Data_1 */
+  {Panel1280x1024,0x0018,0x0010,3}, /* XGI_LVDS1280x1024Data_2 */
+  {Panel1400x1050,0x0018,0x0000,4}, /* XGI_LVDS1400x1050Data_1 */
+  {Panel1400x1050,0x0018,0x0010,5}, /* XGI_LVDS1400x1050Data_2 */
+  {Panel1600x1200,0x0018,0x0000,6}, /* XGI_LVDS1600x1200Data_1 */
+  {PanelRef60Hz,0x0008,0x0008,7}, /* XGI_LVDSNoScalingData */
+  {Panel1024x768x75,0x0018,0x0000,8},   /* XGI_LVDS1024x768Data_1x75 */
+  {Panel1024x768x75,0x0018,0x0010,9},   /* XGI_LVDS1024x768Data_2x75 */
+  {Panel1280x1024x75,0x0018,0x0000,10}, /* XGI_LVDS1280x1024Data_1x75 */
+  {Panel1280x1024x75,0x0018,0x0010,11}, /* XGI_LVDS1280x1024Data_2x75 */
+  {PanelRef75Hz,0x0008,0x0008,12},      /* XGI_LVDSNoScalingDatax75 */
+  {0xFF,0x0000,0x0000,0}
+};
+
+XGI330_LCDDataTablStruct XGI_EPLLCDDesDataPtr[]=
+{
+  {Panel1024x768,0x0018,0x0000,0}, /* XGI_LVDS1024x768Des_1 */
+  {Panel1024x768,0x0618,0x0410,1}, /* XGI_LVDS1024x768Des_3 */
+  {Panel1024x768,0x0018,0x0010,2}, /* XGI_LVDS1024x768Des_2 */
+  {Panel1280x1024,0x0018,0x0000,3}, /* XGI_LVDS1280x1024Des_1 */
+  {Panel1280x1024,0x0018,0x0010,4}, /* XGI_LVDS1280x1024Des_2 */
+  {Panel1400x1050,0x0018,0x0000,5}, /* XGI_LVDS1400x1050Des_1 */
+  {Panel1400x1050,0x0018,0x0010,6}, /* XGI_LVDS1400x1050Des_2 */
+  {Panel1600x1200,0x0018,0x0000,7}, /* XGI_LVDS1600x1200Des_1 */
+  {PanelRef60Hz,0x0008,0x0008,8},       /* XGI_LVDSNoScalingDesData */
+  {Panel1024x768x75,0x0018,0x0000,9},   /* XGI_LVDS1024x768Des_1x75 */
+  {Panel1024x768x75,0x0618,0x0410,10},  /* XGI_LVDS1024x768Des_3x75 */
+  {Panel1024x768x75,0x0018,0x0010,11},  /* XGI_LVDS1024x768Des_2x75 */
+  {Panel1280x1024x75,0x0018,0x0000,12}, /* XGI_LVDS1280x1024Des_1x75 */
+  {Panel1280x1024x75,0x0018,0x0010,13}, /* XGI_LVDS1280x1024Des_2x75 */
+  {PanelRef75Hz,0x0008,0x0008,14},      /* XGI_LVDSNoScalingDesDatax75 */
+  {0xFF,0x0000,0x0000,0}
+};
+
+XGI330_LCDDataTablStruct XGI_EPLCHLCDRegPtr[]=
+{
+  {Panel1024x768,0x0000,0x0000,0}, /* XGI_CH7017LV1024x768 */
+  {Panel1400x1050,0x0000,0x0000,1}, /* XGI_CH7017LV1400x1050 */
+  {0xFF,0x0000,0x0000,0}
+};
+
+XGI330_TVDataTablStruct XGI_TVDataTable[]=
+{
+ {0x09E1,0x0001,0},     /* XGI_ExtPALData */
+ {0x09E1,0x0000,1},     /* XGI_ExtNTSCData */
+ {0x09E1,0x0801,2},     /* XGI_StPALData */
+ {0x09E1,0x0800,3},     /* XGI_StNTSCData */
+ {0x49E0,0x0100,4},     /* XGI_ExtHiTVData */
+ {0x49E0,0x4100,5},     /* XGI_St2HiTVData */
+ {0x49E0,0x4900,13},    /* XGI_St1HiTVData */
+ {0x09E0,0x0020,6},     /* XGI_ExtYPbPr525iData */
+ {0x09E0,0x0040,7},     /* XGI_ExtYPbPr525pData */
+ {0x09E0,0x0080,8},     /* XGI_ExtYPbPr750pData */
+ {0x09E0,0x0820,9},     /* XGI_StYPbPr525iData */
+ {0x09E0,0x0840,10},    /* XGI_StYPbPr525pData */
+ {0x09E0,0x0880,11},    /* XGI_StYPbPr750pData */
+ {0xffff,0x0000,12}     /* END */
+};
+
+USHORT TVLenList[]=
+{
+   LVDSCRT1Len_H,
+   LVDSCRT1Len_V,
+   LVDSDataLen,
+   0,
+   TVDataLen,
+   0,
+   0,
+   CHTVRegLen
+} ;
+
+/* Chrontel 7017 TV CRT1 Timing List */
+XGI330_TVDataTablStruct XGI_EPLCHTVCRT1Ptr[]=
+{
+  {0x0011,0x0000,0}, /* XGI_CHTVCRT1UNTSC */
+  {0x0011,0x0010,1}, /* XGI_CHTVCRT1ONTSC */
+  {0x0011,0x0001,2}, /* XGI_CHTVCRT1UPAL */
+  {0x0011,0x0011,3}, /* XGI_CHTVCRT1OPAL */
+  {0xFFFF,0x0000,4}
+};
+
+/* ;;Chrontel 7017 TV Timing List */
+XGI330_TVDataTablStruct XGI_EPLCHTVDataPtr[]=
+{
+  {0x0011,0x0000,0}, /* XGI_CHTVUNTSCData */
+  {0x0011,0x0010,1}, /* XGI_CHTVONTSCData */
+  {0x0011,0x0001,2}, /* XGI_CHTVUPALData */
+  {0x0011,0x0011,3}, /* XGI_CHTVOPALData */
+  {0xFFFF,0x0000,4}
+};
+
+/* ;;Chrontel 7017 TV Reg. List */
+XGI330_TVDataTablStruct XGI_EPLCHTVRegPtr[]=
+{
+  {0x0011,0x0000,0}, /* XGI_CHTVRegUNTSC */
+  {0x0011,0x0010,1}, /* XGI_CHTVRegONTSC */
+  {0x0011,0x0001,2}, /* XGI_CHTVRegUPAL */
+  {0x0011,0x0011,3}, /* XGI_CHTVRegOPAL */
+  {0xFFFF,0x0000,4}
+};
+
+USHORT LCDLenList[]=
+{
+   LVDSCRT1Len_H,
+   LVDSCRT1Len_V,
+   LVDSDataLen,
+   LCDDesDataLen,
+   LCDDataLen,
+   LCDDesDataLen,
+   0,
+   LCDDesDataLen,
+   LCDDesDataLen,
+   0
+} ;
+
+XGI330_LCDCapStruct  XGI660_LCDDLCapList[]=  /* 660, Dual link */
+{
+/* LCDCap1024x768 */
+                {Panel1024x768, DefaultLCDCap, 0, 0x014, 0x88, 0x06, VCLK65,
+                 0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
+/* LCDCap1280x1024 */
+                {Panel1280x1024, LCDDualLink+DefaultLCDCap, StLCDBToA, 0x053, 0x70, 0x03, VCLK108_2,
+                 0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCap1400x1050 */
+                {Panel1400x1050, LCDDualLink+DefaultLCDCap, StLCDBToA, 0x053, 0x70, 0x03, VCLK108_2,
+                 0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCap1600x1200 */
+                {Panel1600x1200, LCDDualLink+DefaultLCDCap, LCDToFull, 0x053, 0xC0, 0x03, VCLK162,
+                 0x43, 0x22, 0x70, 0x24, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCap1024x768x75 */
+                {Panel1024x768x75, DefaultLCDCap, 0, 0x014, 0x60, 0, VCLK78_75,
+                 0x2B, 0x61, 0x2B, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
+/* LCDCap1280x1024x75 */
+                {Panel1280x1024x75, LCDDualLink+DefaultLCDCap, StLCDBToA, 0x053, 0x90, 0x03, VCLK135_5,
+                 0x54, 0x42, 0x4A, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCapDefault */
+                {0xFF, DefaultLCDCap, 0, 0x053, 0x88, 0x06, VCLK65,
+                0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10}
+};
+
+XGI330_LCDCapStruct  XGI_LCDDLCapList[]=  /* Dual link only */
+{
+/* LCDCap1024x768 */
+                {Panel1024x768, DefaultLCDCap, 0, 0x012, 0x88, 0x06, VCLK65,
+                0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
+/* LCDCap1280x1024 */
+                {Panel1280x1024, LCDDualLink+DefaultLCDCap, StLCDBToA, 0x012, 0x70, 0x03, VCLK108_2,
+                0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCap1400x1050 */
+                {Panel1400x1050, LCDDualLink+DefaultLCDCap, StLCDBToA, 0x012, 0x70, 0x03, VCLK108_2,
+                 0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCap1600x1200 */
+                {Panel1600x1200, LCDDualLink+DefaultLCDCap, LCDToFull, 0x012, 0xC0, 0x03, VCLK162,
+                 0x43, 0x22, 0x70, 0x24, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCap1024x768x75 */
+                {Panel1024x768x75, DefaultLCDCap, 0, 0x012, 0x60, 0, VCLK78_75,
+                 0x2B, 0x61, 0x2B, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
+/* LCDCap1280x1024x75 */
+                {Panel1280x1024x75, LCDDualLink+DefaultLCDCap, StLCDBToA, 0x012, 0x90, 0x03, VCLK135_5,
+                 0x54, 0x42, 0x4A, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCapDefault */
+                {0xFF, DefaultLCDCap, 0, 0x012, 0x88, 0x06, VCLK65,
+                0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10}
+};
+
+XGI330_LCDCapStruct  XGI660_LCDCapList[]=
+{
+/* LCDCap1024x768 */
+                {Panel1024x768, DefaultLCDCap, 0, 0x014, 0x88, 0x06, VCLK65,
+                0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
+/* LCDCap1280x1024 */
+                {Panel1280x1024, DefaultLCDCap, StLCDBToA, 0x053, 0x70, 0x03, VCLK108_2,
+                0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCap1400x1050 */
+                {Panel1400x1050, DefaultLCDCap, StLCDBToA, 0x053, 0x70, 0x03, VCLK108_2,
+                 0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCap1600x1200 */
+                {Panel1600x1200, DefaultLCDCap, LCDToFull, 0x053, 0xC0, 0x03, VCLK162,
+                 0x5A, 0x23, 0x5A, 0x23, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCap1024x768x75 */
+                {Panel1024x768x75, DefaultLCDCap, 0, 0x014, 0x60, 0, VCLK78_75,
+                 0x2B, 0x61, 0x2B, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
+/* LCDCap1280x1024x75 */
+                {Panel1280x1024x75,+DefaultLCDCap, StLCDBToA, 0x053, 0x90, 0x03, VCLK135_5,
+                 0x54, 0x42, 0x4A, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCapDefault */
+                {0xFF, DefaultLCDCap, 0, 0x053, 0x88, 0x06, VCLK65,
+                0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10}
+};
+
+XGI330_LCDCapStruct  XGI_LCDCapList[]=
+{
+/* LCDCap1024x768 */
+                {Panel1024x768, DefaultLCDCap, 0, 0x012, 0x88, 0x06, VCLK65,
+                0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
+/* LCDCap1280x1024 */
+                {Panel1280x1024, DefaultLCDCap, StLCDBToA, 0x012, 0x70, 0x03, VCLK108_2,
+                0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCap1400x1050 */
+                {Panel1400x1050, DefaultLCDCap, StLCDBToA, 0x012, 0x70, 0x03, VCLK108_2,
+                 0x70, 0x44, 0xF8, 0x2F, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCap1600x1200 */
+                {Panel1600x1200, DefaultLCDCap, LCDToFull, 0x012, 0xC0, 0x03, VCLK162,
+                 0x5A, 0x23, 0x5A, 0x23, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCap1024x768x75 */
+                {Panel1024x768x75, DefaultLCDCap, 0, 0x012, 0x60, 0, VCLK78_75,
+                 0x2B, 0x61, 0x2B, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10},
+/* LCDCap1280x1024x75 */
+                {Panel1280x1024x75, DefaultLCDCap, StLCDBToA, 0x012, 0x90, 0x03, VCLK135_5,
+                 0x54, 0x42, 0x4A, 0x61, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                 0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x30, 0x10},
+/* LCDCapDefault */
+                {0xFF, DefaultLCDCap, 0, 0x012, 0x88, 0x06, VCLK65,
+                0x6C, 0xC3, 0x35, 0x62, 0x02, 0x14, 0x0A, 0x02, 0x00,
+                0x30, 0x10, 0x5A, 0x10, 0x10, 0x0A, 0xC0, 0x28, 0x10}
+};
+
+XGI21_LVDSCapStruct XGI21_LCDCapList[]=
+{
+    {DisableLCD24bpp + LCDPolarity,
+     2160,1250,1600,1200,  64,  1,  192,   3,
+     0x70,0x24,0x20,0x04,0x0A,0x02,0xC8
+    },
+    {DisableLCD24bpp + LCDPolarity,
+     1688,1066,1280,1024,  48,  1,  112,   3,
+     0x70,0x44,0x20,0x04,0x0A,0x02,0xC8
+    },
+    {DisableLCD24bpp + LCDPolarity + (LCDPolarity << 8),
+     1344, 806,1024, 768,  24,  3,  136,   6,
+     0x6C,0x65,0x20,0x04,0x0A,0x02,0xC8
+    },
+    {DisableLCD24bpp + LCDPolarity,
+     1056, 628, 800, 600,  40,   1, 128,   4,
+     0x42,0xE2,0x20,0x14,0x0A,0x02,0x00
+    },
+    {DisableLCD24bpp + LCDPolarity,
+      928, 525, 800, 480,  40,  13,  48,   3,
+     0x52,0xC5,0x20,0x14,0x0A,0x02,0x00
+    },
+    {DisableLCD24bpp + LCDPolarity + (LCDPolarity << 8),
+      800, 525, 640, 480,  16,  10,  96,   2,
+     0x1B,0xE1,0x20,0x04,0x0A,0x02,0xC8
+    }
+
+};
+
+XGI_Ext2Struct XGI330_RefIndex[]=
+{
+{Support32Bpp + SupportAllCRT2 + SyncPN,                        RES320x200,      VCLK25_175, 0x00,0x10,0x59, 320, 200},/* 00 */
+{Support32Bpp + SupportAllCRT2 + SyncPN,                        RES320x200,      VCLK25_175, 0x00,0x10,0x00, 320, 400},/* 01 */
+{Support32Bpp + SupportAllCRT2 + SyncNN,                        RES320x240,      VCLK25_175, 0x04,0x20,0x50, 320, 240},/* 02 */
+{Support32Bpp + SupportAllCRT2 + SyncPP,                        RES400x300,      VCLK40,     0x05,0x32,0x51, 400, 300},/* 03 */
+{Support32Bpp + NoSupportTV + SyncNN + SupportTV1024,           RES512x384,      VCLK65,     0x06,0x43,0x52, 512, 384},/* 04 */
+{Support32Bpp + SupportAllCRT2 + SyncPN,                        RES640x400,      VCLK25_175, 0x00,0x14,0x2f, 640, 400},/* 05 */
+{Support32Bpp + SupportAllCRT2 + SyncNN,                        RES640x480x60,   VCLK25_175, 0x04,0x24,0x2e, 640, 480},/* 06 640x480x60Hz (LCD 640x480x60z) */
+{Support32Bpp + NoSupportHiVisionTV + SyncNN,                   RES640x480x72,   VCLK31_5,   0x04,0x24,0x2e, 640, 480},/* 07 640x480x72Hz (LCD 640x480x70Hz) */
+{Support32Bpp + NoSupportHiVisionTV + SyncNN,                   RES640x480x75,   VCLK31_5,   0x47,0x24,0x2e, 640, 480},/* 08 640x480x75Hz (LCD 640x480x75Hz) */
+{Support32Bpp + SupportRAMDAC2 + SyncNN,                        RES640x480x85,   VCLK36,     0x8A,0x24,0x2e, 640, 480},/* 09 640x480x85Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPN,                        RES640x480x100,  VCLK43_163, 0x00,0x24,0x2e, 640, 480},/* 0a 640x480x100Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPN,                        RES640x480x120,  VCLK52_406, 0x00,0x24,0x2e, 640, 480},/* 0b 640x480x120Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPN,                        RES640x480x160,  VCLK72_852, 0x00,0x24,0x2e, 640, 480},/* 0c 640x480x160Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncNN,                        RES640x480x200,  VCLK86_6,   0x00,0x24,0x2e, 640, 480},/* 0d 640x480x200Hz */
+{Support32Bpp + NoSupportLCD + SyncPP,                          RES800x600x56,   VCLK36,     0x05,0x36,0x6a, 800, 600},/* 0e 800x600x56Hz */
+{Support32Bpp + NoSupportTV + SyncPP,                           RES800x600x60,   VCLK40,     0x05,0x36,0x6a, 800, 600},/* 0f 800x600x60Hz (LCD 800x600x60Hz) */
+{Support32Bpp + NoSupportHiVisionTV + SyncPP,                   RES800x600x72,   VCLK50,     0x48,0x36,0x6a, 800, 600},/* 10 800x600x72Hz (LCD 800x600x70Hz) */
+{Support32Bpp + NoSupportHiVisionTV + SyncPP,                   RES800x600x75,   VCLK49_5,   0x8B,0x36,0x6a, 800, 600},/* 11 800x600x75Hz (LCD 800x600x75Hz) */
+{Support32Bpp + SupportRAMDAC2 + SyncPP,                        RES800x600x85,   VCLK56_25,  0x00,0x36,0x6a, 800, 600},/* 12 800x600x85Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPN,                        RES800x600x100,  VCLK68_179, 0x00,0x36,0x6a, 800, 600},/* 13 800x600x100Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPN,                        RES800x600x120,  VCLK83_95,  0x00,0x36,0x6a, 800, 600},/* 14 800x600x120Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPN,                        RES800x600x160,  VCLK116_406,0x00,0x36,0x6a, 800, 600},/* 15 800x600x160Hz */
+{Support32Bpp + InterlaceMode + SyncPP,                         RES1024x768x43,  VCLK44_9,   0x00,0x47,0x37,1024, 768},/* 16 1024x768x43Hz */
+{Support32Bpp + NoSupportTV + SyncNN + SupportTV1024,           RES1024x768x60,  VCLK65,     0x06,0x47,0x37,1024, 768},/* 17 1024x768x60Hz (LCD 1024x768x60Hz) */
+{Support32Bpp + NoSupportHiVisionTV + SyncNN,                   RES1024x768x70,  VCLK75,     0x49,0x47,0x37,1024, 768},/* 18 1024x768x70Hz (LCD 1024x768x70Hz) */
+{Support32Bpp + NoSupportHiVisionTV + SyncPP,                   RES1024x768x75,  VCLK78_75,  0x00,0x47,0x37,1024, 768},/* 19 1024x768x75Hz (LCD 1024x768x75Hz) */
+{Support32Bpp + SupportRAMDAC2 + SyncPP,                        RES1024x768x85,  VCLK94_5,   0x8C,0x47,0x37,1024, 768},/* 1a 1024x768x85Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPN,                        RES1024x768x100, VCLK113_309,0x00,0x47,0x37,1024, 768},/* 1b 1024x768x100Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPN,                        RES1024x768x120, VCLK139_054,0x00,0x47,0x37,1024, 768},/* 1c 1024x768x120Hz */
+{Support32Bpp + SupportLCD + SyncPP,                            RES1280x960x60,  VCLK108_2,  0x08,0x58,0x7b,1280, 960},/* 1d 1280x960x60Hz */
+{Support32Bpp + InterlaceMode + SyncPP,                         RES1280x1024x43, VCLK78_75,  0x00,0x58,0x3a,1280,1024},/* 1e 1280x1024x43Hz */
+{Support32Bpp + NoSupportTV + SyncPP,                           RES1280x1024x60, VCLK108_2,  0x07,0x58,0x3a,1280,1024},/* 1f 1280x1024x60Hz (LCD 1280x1024x60Hz) */
+{Support32Bpp + NoSupportTV + SyncPP,                           RES1280x1024x75, VCLK135_5,  0x00,0x58,0x3a,1280,1024},/* 20 1280x1024x75Hz (LCD 1280x1024x75Hz) */
+{Support32Bpp + SyncPP,                                         RES1280x1024x85, VCLK157_5,  0x00,0x58,0x3a,1280,1024},/* 21 1280x1024x85Hz */
+{Support32Bpp + SupportLCD + SyncPP + SupportCRT2in301C,        RES1600x1200x60, VCLK162,    0x09,0x7A,0x3c,1600,1200},/* 22 1600x1200x60Hz */
+{Support32Bpp + SyncPP + SupportCRT2in301C,                     RES1600x1200x65, VCLK175,    0x00,0x69,0x3c,1600,1200},/* 23 1600x1200x65Hz */
+{Support32Bpp + SyncPP + SupportCRT2in301C,                     RES1600x1200x70, VCLK189,    0x00,0x69,0x3c,1600,1200},/* 24 1600x1200x70Hz */
+{Support32Bpp + SyncPP + SupportCRT2in301C,                     RES1600x1200x75, VCLK202_5,  0x00,0x69,0x3c,1600,1200},/* 25 1600x1200x75Hz */
+{Support32Bpp + SyncPP,                                         RES1600x1200x85, VCLK229_5,  0x00,0x69,0x3c,1600,1200},/* 26 1600x1200x85Hz */
+{Support32Bpp + SyncPP,                                         RES1600x1200x100,VCLK269_655,0x00,0x69,0x3c,1600,1200},/* 27 1600x1200x100Hz */
+{Support32Bpp + SyncPP,                                         RES1600x1200x120,VCLK323_586,0x00,0x69,0x3c,1600,1200},/* 28 1600x1200x120Hz */
+{Support32Bpp + SupportLCD + SyncNP,                            RES1920x1440x60, VCLK234,    0x00,0x00,0x68,1920,1440},/* 29 1920x1440x60Hz */
+{Support32Bpp + SyncPN,                                         RES1920x1440x65, VCLK254_817,0x00,0x00,0x68,1920,1440},/* 2a 1920x1440x65Hz */
+{Support32Bpp + SyncPN,                                         RES1920x1440x70, VCLK277_015,0x00,0x00,0x68,1920,1440},/* 2b 1920x1440x70Hz */
+{Support32Bpp + SyncPN,                                         RES1920x1440x75, VCLK291_132,0x00,0x00,0x68,1920,1440},/* 2c 1920x1440x75Hz */
+{Support32Bpp + SyncPN,                                         RES1920x1440x85, VCLK330_615,0x00,0x00,0x68,1920,1440},/* 2d 1920x1440x85Hz */
+{Support16Bpp + SyncPN,                                         RES1920x1440x100,VCLK388_631,0x00,0x00,0x68,1920,1440},/* 2e 1920x1440x100Hz */
+{Support32Bpp + SupportLCD + SyncPN,                            RES2048x1536x60, VCLK266_952,0x00,0x00,0x6c,2048,1536},/* 2f 2048x1536x60Hz */
+{Support32Bpp + SyncPN,                                         RES2048x1536x65, VCLK291_766,0x00,0x00,0x6c,2048,1536},/* 30 2048x1536x65Hz */
+{Support32Bpp + SyncPN,                                         RES2048x1536x70, VCLK315_195,0x00,0x00,0x6c,2048,1536},/* 31 2048x1536x70Hz */
+{Support32Bpp + SyncPN,                                         RES2048x1536x75, VCLK340_477,0x00,0x00,0x6c,2048,1536},/* 32 2048x1536x75Hz */
+{Support16Bpp + SyncPN,                                         RES2048x1536x85, VCLK375_847,0x00,0x00,0x6c,2048,1536},/* 33 2048x1536x85Hz */
+{Support32Bpp + SupportHiVisionTV + SupportRAMDAC2 + SyncPP + SupportYPbPr,     RES800x480x60,   VCLK39_77,  0x08,0x00,0x70, 800, 480},/* 34 800x480x60Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPP,                        RES800x480x75,   VCLK49_5,   0x08,0x00,0x70, 800, 480},/* 35 800x480x75Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPP,                        RES800x480x85,   VCLK56_25,  0x08,0x00,0x70, 800, 480},/* 36 800x480x85Hz */
+{Support32Bpp + SupportHiVisionTV + SupportRAMDAC2 + SyncPP + SupportYPbPr,     RES1024x576x60,  VCLK65,     0x09,0x00,0x71,1024, 576},/* 37 1024x576x60Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPP,                        RES1024x576x75,  VCLK78_75,  0x09,0x00,0x71,1024, 576},/* 38 1024x576x75Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPP,                        RES1024x576x85,  VCLK94_5,   0x09,0x00,0x71,1024, 576},/* 39 1024x576x85Hz */
+{Support32Bpp + SupportHiVisionTV + SupportRAMDAC2 + SyncPP + SupportYPbPr,     RES1280x720x60,  VCLK108_2,  0x0A,0x00,0x75,1280, 720},/* 3a 1280x720x60Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPP,                        RES1280x720x75,  VCLK135_5,  0x0A,0x00,0x75,1280, 720},/* 3b 1280x720x75Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPP,                        RES1280x720x85,  VCLK157_5,  0x0A,0x00,0x75,1280, 720},/* 3c 1280x720x85Hz */
+{Support32Bpp + SupportTV + SyncNN,                             RES720x480x60,   VCLK28_322, 0x06,0x00,0x31, 720, 480},/* 3d 720x480x60Hz */
+{Support32Bpp + SupportTV + SyncPP,                             RES720x576x56,   VCLK36,     0x06,0x00,0x32, 720, 576},/* 3e 720x576x56Hz */
+{Support32Bpp + InterlaceMode + NoSupportLCD + SyncPP,          RES856x480x79I,  VCLK35_2,   0x00,0x00,0x00, 856, 480},/* 3f 856x480x79I */
+{Support32Bpp + NoSupportLCD + SyncNN,                          RES856x480x60,   VCLK35_2,   0x00,0x00,0x00, 856, 480},/* 40 856x480x60Hz */
+{Support32Bpp + NoSupportHiVisionTV + SyncPP,                   RES1280x768x60,  VCLK79_411, 0x08,0x48,0x23,1280, 768},/* 41 1280x768x60Hz */
+{Support32Bpp + NoSupportHiVisionTV + SyncPP,                   RES1400x1050x60, VCLK122_61, 0x08,0x69,0x26,1400,1050},/* 42 1400x1050x60Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPP,                        RES1152x864x60,  VCLK80_350, 0x37,0x00,0x20,1152, 864},/* 43 1152x864x60Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPP,                        RES1152x864x75,  VCLK107_385,0x37,0x00,0x20,1152, 864},/* 44 1152x864x75Hz */
+{Support32Bpp + SupportLCD + SupportRAMDAC2 + SyncPP,           RES1280x960x75,  VCLK125_999,0x3A,0x88,0x7b,1280, 960},/* 45 1280x960x75Hz */
+{Support32Bpp + SupportLCD + SupportRAMDAC2 + SyncPP,           RES1280x960x85,  VCLK148_5,  0x0A,0x88,0x7b,1280, 960},/* 46 1280x960x85Hz */
+{Support32Bpp + SupportLCD + SupportRAMDAC2 + SyncPP,           RES1280x960x120, VCLK217_325,0x3A,0x88,0x7b,1280, 960},/* 47 1280x960x120Hz */
+{Support32Bpp + SupportRAMDAC2 + SyncPN,                        RES1024x768x160, VCLK139_054,0x30,0x47,0x37,1024, 768},/* 48 1024x768x160Hz */
+};
+
+
+
+XGI330_VCLKDataStruct XGI330_VCLKData[]=
+{
+ { 0x1b,0xe1, 25}, /* 0x0 */
+ { 0x4e,0xe4, 28}, /* 0x1 */
+ { 0x57,0xe4, 31}, /* 0x2 */
+ { 0xc3,0xc8, 36}, /* 0x3 */
+ { 0x42,0xe2, 40}, /* 0x4 */
+ { 0xfe,0xcd, 43}, /* 0x5 */
+ { 0x5d,0xc4, 44}, /* 0x6 */
+ { 0x52,0xe2, 49}, /* 0x7 */
+ { 0x53,0xe2, 50}, /* 0x8 */
+ { 0x74,0x67, 52}, /* 0x9 */
+ { 0x6d,0x66, 56}, /* 0xa */
+ { 0x6c,0xc3, 65}, /* 0xb */
+ { 0x46,0x44, 67}, /* 0xc */
+ { 0xb1,0x46, 68}, /* 0xd */
+ { 0xd3,0x4a, 72}, /* 0xe */
+ { 0x29,0x61, 75}, /* 0xf */
+ { 0x6e,0x46, 76}, /* 0x10 */
+ { 0x2b,0x61, 78}, /* 0x11 */
+ { 0x31,0x42, 79}, /* 0x12 */
+ { 0xab,0x44, 83}, /* 0x13 */
+ { 0x46,0x25, 84}, /* 0x14 */
+ { 0x78,0x29, 86}, /* 0x15 */
+ { 0x62,0x44, 94}, /* 0x16 */
+ { 0x2b,0x41,104}, /* 0x17 */
+ { 0x3a,0x23,105}, /* 0x18 */
+ { 0x70,0x44,108}, /* 0x19 */
+ { 0x3c,0x23,109}, /* 0x1a */
+ { 0x5e,0x43,113}, /* 0x1b */
+ { 0xbc,0x44,116}, /* 0x1c */
+ { 0xe0,0x46,132}, /* 0x1d */
+ { 0x54,0x42,135}, /* 0x1e */
+ { 0xea,0x2a,139}, /* 0x1f */
+ { 0x41,0x22,157}, /* 0x20 */
+ { 0x70,0x24,162}, /* 0x21 */
+ { 0x30,0x21,175}, /* 0x22 */
+ { 0x4e,0x22,189}, /* 0x23 */
+ { 0xde,0x26,194}, /* 0x24 */
+ { 0x62,0x06,202}, /* 0x25 */
+ { 0x3f,0x03,229}, /* 0x26 */
+ { 0xb8,0x06,234}, /* 0x27 */
+ { 0x34,0x02,253}, /* 0x28 */
+ { 0x58,0x04,255}, /* 0x29 */
+ { 0x24,0x01,265}, /* 0x2a */
+ { 0x9b,0x02,267}, /* 0x2b */
+ { 0x70,0x05,270}, /* 0x2c */
+ { 0x25,0x01,272}, /* 0x2d */
+ { 0x9c,0x02,277}, /* 0x2e */
+ { 0x27,0x01,286}, /* 0x2f */
+ { 0x3c,0x02,291}, /* 0x30 */
+ { 0xef,0x0a,292}, /* 0x31 */
+ { 0xf6,0x0a,310}, /* 0x32 */
+ { 0x95,0x01,315}, /* 0x33 */
+ { 0xf0,0x09,324}, /* 0x34 */
+ { 0xfe,0x0a,331}, /* 0x35 */
+ { 0xf3,0x09,332}, /* 0x36 */
+ { 0xea,0x08,340}, /* 0x37 */
+ { 0xe8,0x07,376}, /* 0x38 */
+ { 0xde,0x06,389}, /* 0x39 */
+ { 0x52,0x2a, 54}, /* 0x3a */
+ { 0x52,0x6a, 27}, /* 0x3b */
+ { 0x62,0x24, 70}, /* 0x3c */
+ { 0x62,0x64, 70}, /* 0x3d */
+ { 0xa8,0x4c, 30}, /* 0x3e */
+ { 0x20,0x26, 33}, /* 0x3f */
+ { 0x31,0xc2, 39}, /* 0x40 */
+ { 0x60,0x36, 30}, /* 0x41 */
+ { 0x40,0x4A, 28}, /* 0x42 */
+ { 0x9F,0x46, 44}, /* 0x43 */
+ { 0x97,0x2C, 26}, /* 0x44 */
+ { 0x44,0xE4, 25}, /* 0x45 */
+ { 0x7E,0x32, 47}, /* 0x46 */
+ { 0x08,0x24, 31}, /* 0x47 */
+ { 0x97,0x2c, 26}, /* 0x48 */
+ { 0xCE,0x3c, 39}, /* 0x49 */
+ { 0x52,0x4A, 36}, /* 0x4a */
+ { 0x2C,0x61, 95}, /* 0x4b */
+ { 0x78,0x27,108}, /* 0x4c */
+ { 0x66,0x43,123},  /* 0x4d */
+ { 0x2c,0x61, 80},  /* 0x4e */
+ { 0x3b,0x61,108}  /* 0x4f */
+};
+
+XGI_VBVCLKDataStruct XGI330_VBVCLKData[]=
+{
+ { 0x1b,0xe1, 25}, /* 0x0 */
+ { 0x4e,0xe4, 28}, /* 0x1 */
+ { 0x57,0xe4, 31}, /* 0x2 */
+ { 0xc3,0xc8, 36}, /* 0x3 */
+ { 0x42,0x47, 40}, /* 0x4 */
+ { 0xfe,0xcd, 43}, /* 0x5 */
+ { 0x5d,0xc4, 44}, /* 0x6 */
+ { 0x52,0x47, 49}, /* 0x7 */
+ { 0x53,0x47, 50}, /* 0x8 */
+ { 0x74,0x67, 52}, /* 0x9 */
+ { 0x6d,0x66, 56}, /* 0xa */
+ { 0x5a,0x64, 65}, /* 0xb */
+ { 0x46,0x44, 67}, /* 0xc */
+ { 0xb1,0x46, 68}, /* 0xd */
+ { 0xd3,0x4a, 72}, /* 0xe */
+ { 0x29,0x61, 75}, /* 0xf */
+ { 0x6d,0x46, 75}, /* 0x10 */
+ { 0x41,0x43, 78}, /* 0x11 */
+ { 0x31,0x42, 79}, /* 0x12 */
+ { 0xab,0x44, 83}, /* 0x13 */
+ { 0x46,0x25, 84}, /* 0x14 */
+ { 0x78,0x29, 86}, /* 0x15 */
+ { 0x62,0x44, 94}, /* 0x16 */
+ { 0x2b,0x22,104}, /* 0x17 */
+ { 0x49,0x24,105}, /* 0x18 */
+ { 0xf8,0x2f,108}, /* 0x19 */
+ { 0x3c,0x23,109}, /* 0x1a */
+ { 0x5e,0x43,113}, /* 0x1b */
+ { 0xbc,0x44,116}, /* 0x1c */
+ { 0xe0,0x46,132}, /* 0x1d */
+ { 0xd4,0x28,135}, /* 0x1e */
+ { 0xea,0x2a,139}, /* 0x1f */
+ { 0x41,0x22,157}, /* 0x20 */
+ { 0x70,0x24,162}, /* 0x21 */
+ { 0x30,0x21,175}, /* 0x22 */
+ { 0x4e,0x22,189}, /* 0x23 */
+ { 0xde,0x26,194}, /* 0x24 */
+ { 0x70,0x07,202}, /* 0x25 */
+ { 0x3f,0x03,229}, /* 0x26 */
+ { 0xb8,0x06,234}, /* 0x27 */
+ { 0x34,0x02,253}, /* 0x28 */
+ { 0x58,0x04,255}, /* 0x29 */
+ { 0x24,0x01,265}, /* 0x2a */
+ { 0x9b,0x02,267}, /* 0x2b */
+ { 0x70,0x05,270}, /* 0x2c */
+ { 0x25,0x01,272}, /* 0x2d */
+ { 0x9c,0x02,277}, /* 0x2e */
+ { 0x27,0x01,286}, /* 0x2f */
+ { 0x3c,0x02,291}, /* 0x30 */
+ { 0xef,0x0a,292}, /* 0x31 */
+ { 0xf6,0x0a,310}, /* 0x32 */
+ { 0x95,0x01,315}, /* 0x33 */
+ { 0xf0,0x09,324}, /* 0x34 */
+ { 0xfe,0x0a,331}, /* 0x35 */
+ { 0xf3,0x09,332}, /* 0x36 */
+ { 0xea,0x08,340}, /* 0x37 */
+ { 0xe8,0x07,376}, /* 0x38 */
+ { 0xde,0x06,389}, /* 0x39 */
+ { 0x52,0x2a, 54}, /* 0x3a */
+ { 0x52,0x6a, 27}, /* 0x3b */
+ { 0x62,0x24, 70}, /* 0x3c */
+ { 0x62,0x64, 70}, /* 0x3d */
+ { 0xa8,0x4c, 30}, /* 0x3e */
+ { 0x20,0x26, 33}, /* 0x3f */
+ { 0x31,0xc2, 39}, /* 0x40 */
+ { 0x2e,0x48, 25}, /* 0x41 */
+ { 0x24,0x46, 25}, /* 0x42 */
+ { 0x26,0x64, 28}, /* 0x43 */
+ { 0x37,0x64, 40}, /* 0x44 */
+ { 0xa1,0x42,108}, /* 0x45 */
+ { 0x37,0x61,100}, /* 0x46 */
+ { 0x78,0x27,108}, /* 0x47 */
+ { 0x5e,0x64,68},  /* 0x48 chiawen for fuj1280x768*/
+ { 0x70,0x44,108}, /* 0x49 chiawen for 1400x1050*/
+};
+
+UCHAR XGI330_ScreenOffset[]={ 0x14,0x19,0x20,0x28,0x32,0x40,0x50,0x64,0x78,0x80,0x2d,0x35,0x57,0x48 };
+
+XGI_StResInfoStruct XGI330_StResInfo[]=
+{
+ { 640,400},
+ { 640,350},
+ { 720,400},
+ { 720,350},
+ { 640,480}
+};
+
+XGI_ModeResInfoStruct XGI330_ModeResInfo[]=
+{
+ {  320, 200, 8, 8},
+ {  320, 240, 8, 8},
+ {  320, 400, 8, 8},
+ {  400, 300, 8, 8},
+ {  512, 384, 8, 8},
+ {  640, 400, 8,16},
+ {  640, 480, 8,16},
+ {  800, 600, 8,16},
+ { 1024, 768, 8,16},
+ { 1280,1024, 8,16},
+ { 1600,1200, 8,16},
+ { 1920,1440, 8,16},
+ { 2048,1536, 8,16},
+ {  720, 480, 8,16},
+ {  720, 576, 8,16},
+ { 1280, 960, 8,16},
+ {  800, 480, 8,16},
+ { 1024, 576, 8,16},
+ { 1280, 720, 8,16},
+ {  856, 480, 8,16},
+ { 1280, 768, 8,16},
+ { 1400,1050, 8,16},
+ { 1152, 864, 8,16}
+};
+
+UCHAR XGI330_OutputSelect =0x40;
+UCHAR XGI330_SoftSetting = 0x30;
+UCHAR XGI330_SR07=0x18;
+UCHAR XGI330New_SR15[8][8]={
+{0x0,0x4,0x60,0x60},
+{0xf,0xf,0xf,0xf},
+{0xba,0xba,0xba,0xba},
+{0xa9,0xa9,0xac,0xac},
+{0xa0,0xa0,0xa0,0xa8},
+{0x0,0x0,0x2,0x2},
+{0x30,0x30,0x40,0x40},
+{0x0,0xa5,0xfb,0xf6}
+};
+
+UCHAR XGI330New_CR40[5][8]={
+{0x77,0x77,0x44,0x44},
+{0x77,0x77,0x44,0x44},
+{0x0,0x0,0x0,0x0},
+{0x5b,0x5b,0xab,0xab},
+{0x0,0x0,0xf0,0xf8}
+};
+
+UCHAR XGI330_CR49[]={0xaa,0x88};
+UCHAR XGI330_SR1F=0x0;
+UCHAR XGI330_SR21=0xa3;
+UCHAR XGI330_650_SR21=0xa7;
+UCHAR XGI330_SR22=0xfb;
+UCHAR XGI330_SR23=0xf6;
+UCHAR XGI330_SR24=0xd;
+
+UCHAR XGI660_SR21=0xa3;/* 2003.0312 */
+UCHAR XGI660_SR22=0xf3;/* 2003.0312 */
+
+UCHAR XGI330_LVDS_SR32=0x00;   /* ynlai for 650 LVDS */
+UCHAR XGI330_LVDS_SR33=0x00;    /* chiawen for 650 LVDS */
+UCHAR XGI330_650_SR31=0x40;
+UCHAR XGI330_650_SR33=0x04;
+UCHAR XGI330_CRT2Data_1_2 = 0x0;
+UCHAR XGI330_CRT2Data_4_D = 0x0;
+UCHAR XGI330_CRT2Data_4_E = 0x0;
+UCHAR XGI330_CRT2Data_4_10 = 0x80;
+USHORT XGI330_RGBSenseData = 0xd1;
+USHORT XGI330_VideoSenseData = 0xb9;
+USHORT XGI330_YCSenseData = 0xb3;
+USHORT XGI330_RGBSenseData2 = 0x0190;     /*301b*/
+USHORT XGI330_VideoSenseData2 = 0x0110;
+USHORT XGI330_YCSenseData2 = 0x016B;
+UCHAR XGI330_NTSCPhase[] = {0x21,0xed,0x8a,0x8};
+UCHAR XGI330_PALPhase[] = {0x2a,0x5,0xd3,0x0};
+UCHAR XGI330_NTSCPhase2[] = {0x21,0xF0,0x7B,0xD6};/*301b*/
+UCHAR XGI330_PALPhase2[] = {0x2a,0x09,0x86,0xe9};
+UCHAR XGI330_PALMPhase[] = {0x21,0xE4,0x2E,0x9B};   /*palmn*/
+UCHAR XGI330_PALNPhase[] = {0x21,0xF4,0x3E,0xBA};
+UCHAR XG40_I2CDefinition = 0x00 ;
+UCHAR XG20_CR97 = 0x10 ;
+
+UCHAR XG21_DVOSetting = 0x00 ;
+UCHAR XG21_CR2E = 0x00 ;
+UCHAR XG21_CR2F = 0x00 ;
+UCHAR XG21_CR46 = 0x00 ;
+UCHAR XG21_CR47 = 0x00 ;
+
+UCHAR XG27_CR97 = 0xC1 ;
+UCHAR XG27_SR36 = 0x30 ;
+UCHAR XG27_CR8F = 0x0C ;
+UCHAR XG27_CRD0[] = {0,0,0,0,0,0,0,0x82,0x00,0x66,0x01,0x00} ;
+UCHAR XG27_CRDE[] = {0,0} ;
+UCHAR XG27_SR40 = 0x04 ;
+UCHAR XG27_SR41 = 0x00 ;
+
+UCHAR XGI330_CHTVVCLKUNTSC[]={0x00 };
+
+UCHAR XGI330_CHTVVCLKONTSC[]={0x00 };
+
+UCHAR XGI330_CHTVVCLKUPAL[]={0x00 };
+
+UCHAR XGI330_CHTVVCLKOPAL[]={0x00 };
+
+UCHAR XGI7007_CHTVVCLKUNTSC[]={CH7007TVVCLK30_2,
+                               CH7007TVVCLK30_2,
+                               CH7007TVVCLK30_2,
+                               CH7007TVVCLK30_2,
+                               CH7007TVVCLK28_1,
+                               CH7007TVVCLK47_8
+                              };
+
+UCHAR XGI7007_CHTVVCLKONTSC[]={CH7007TVVCLK26_4,
+                               CH7007TVVCLK26_4,
+                               CH7007TVVCLK26_4,
+                               CH7007TVVCLK26_4,
+                               CH7007TVVCLK24_6,
+                               CH7007TVVCLK43_6
+                              };
+
+UCHAR XGI7007_CHTVVCLKUPAL[]={CH7007TVVCLK31_5,
+                              CH7007TVVCLK31_5,
+                              CH7007TVVCLK31_5,
+                              CH7007TVVCLK31_5,
+                              CH7007TVVCLK26_2,
+                              CH7007TVVCLK39
+                             };
+
+UCHAR XGI7007_CHTVVCLKOPAL[]={CH7007TVVCLK31_5,
+                              CH7007TVVCLK31_5,
+                              CH7007TVVCLK31_5,
+                              CH7007TVVCLK31_5,
+                              CH7007TVVCLK26_2,
+                              CH7007TVVCLK36
+                             };
+
+XGI330_VCLKDataStruct XGI_CH7007VCLKData[]=
+{
+ { 0x60,0x36,30},  /* 0 30.2 MHZ */
+ { 0x40,0x4A,28},  /* 1 28.19 MHZ */
+ { 0x9F,0x46,44},  /* 2 43.6 MHZ */
+ { 0x97,0x2C,26},  /* 3 26.4 MHZ */
+ { 0x44,0xE4,25},  /* 4 24.6 MHZ */
+ { 0x7E,0x32,47},  /* 5 47.832 MHZ */
+ { 0x8A,0x24,31},  /* 6 31.5 MHZ */
+ { 0x97,0x2C,26},  /* 7 26.2 MHZ */
+ { 0xCE,0x3C,39},  /* 8 39 MHZ   */
+ { 0x52,0x4A,36},  /* 9 36 MHZ   */
+ { 0xFF,0x00,0 }   /* End mark      */
+};
+
+XGI330_VCLKDataStruct XGI_VCLKData[]=
+{
+                /* SR2B,SR2C,SR2D */
+                {      0x1B,0xE1,25               },/* 00 (25.175MHz) */
+
+                {      0x4E,0xE4,28               },/* 01 (28.322MHz) */
+
+                {      0x57,0xE4,31               },/* 02 (31.500MHz) */
+
+                {      0xC3,0xC8,36               },/* 03 (36.000MHz) */
+
+                {      0x42,0xE2,40               },/* 04 (40.000MHz) */
+
+                {      0xFE,0xCD,43               },/* 05 (43.163MHz) */
+
+                {      0x5D,0xC4,44               },/* 06 (44.900MHz) */
+
+                {      0x52,0xE2,49               },/* 07 (49.500MHz) */
+
+                {      0x53,0xE2,50               },/* 08 (50.000MHz) */
+
+                {      0x74,0x67,52               },/* 09 (52.406MHz) */
+
+                {      0x6D,0x66,56               },/* 0A (56.250MHz) */
+
+                {      0x6C,0xC3,65               },/* 0B (65.000MHz) */
+
+                {      0x46,0x44,67               },/* 0C (67.765MHz) */
+
+                {      0xB1,0x46,68               },/* 0D (68.179MHz) */
+
+                {      0xD3,0x4A,72               },/* 0E (72.852MHz) */
+
+                {      0x29,0x61,75              },/* 0F (75.000MHz) */
+
+                {      0x6E,0x46,76               },/* 10 (75.800MHz) */
+
+                {      0x2B,0x61,78               },/* 11 (78.750MHz) */
+
+                {      0x31,0x42,79               },/* 12 (79.411MHz) */
+
+                {      0xAB,0x44,83               },/* 13 (83.950MHz) */
+
+                {      0x46,0x25,84               },/* 14 (84.800MHz) */
+
+                {      0x78,0x29,86               },/* 15 (86.600MHz) */
+
+                {      0x62,0x44,94               },/* 16 (94.500MHz) */
+
+                {      0x2B,0x41,104               },/* 17 (104.998MHz) */
+
+                {      0x3A,0x23,105               },/* 18 (105.882MHz) */
+
+                {      0x70,0x44,108               },/* 19 (107.862MHz) */
+
+                {      0x3C,0x23,109               },/* 1A (109.175MHz) */
+
+                {      0x5E,0x43,113              },/* 1B (113.309MHz) */
+
+                {      0xBC,0x44,116              },/* 1C (116.406MHz) */
+
+                {      0xE0,0x46,132              },/* 1D (132.258MHz) */
+
+                {      0x54,0x42,135               },/* 1E (135.500MHz) */
+
+                {      0x9C,0x22,139               },/* 1F (139.275MHz) */
+
+                {      0x41,0x22,157               },/* 20 (157.500MHz) */
+
+                {      0x70,0x24,162               },/* 21 (161.793MHz) */
+
+                {      0x30,0x21,175               },/* 22 (175.000MHz) */
+
+                {      0x4E,0x22,189              },/* 23 (188.520MHz) */
+
+                {      0xDE,0x26,194              },/* 24 (194.400MHz) */
+
+                {      0x62,0x06,202               },/* 25 (202.500MHz) */
+
+                {      0x3F,0x03,229               },/* 26 (229.500MHz) */
+
+                {      0xB8,0x06,234               },/* 27 (233.178MHz) */
+
+                {      0x34,0x02,253               },/* 28 (252.699MHz) */
+
+                {      0x58,0x04,255               },/* 29 (254.817MHz) */
+
+                {      0x24,0x01,265              },/* 2A (265.728MHz) */
+
+                {      0x9B,0x02,267               },/* 2B (266.952MHz) */
+
+                {      0x70,0x05,270               },/* 2C (269.65567MHz) */
+
+                {      0x25,0x01,272               },/* 2D (272.04199MHz) */
+
+                {      0x9C,0x02,277               },/* 2E (277.015MHz) */
+
+                {      0x27,0x01,286               },/* 2F (286.359985MHz) */
+
+                {      0xB3,0x04,291               },/* 30 (291.13266MHz) */
+
+                {      0xBC,0x05,292               },/* 31 (291.766MHz) */
+
+                {      0xF6,0x0A,310               },/* 32 (309.789459MHz) */
+
+                {      0x95,0x01,315               },/* 33 (315.195MHz) */
+
+                {      0xF0,0x09,324               },/* 34 (323.586792MHz) */
+
+                {      0xFE,0x0A,331               },/* 35 (330.615631MHz) */
+
+                {      0xF3,0x09,332              },/* 36 (332.177612MHz) */
+
+                {      0x5E,0x03,340              },/* 37 (340.477MHz) */
+
+                {      0xE8,0x07,376              },/* 38 (375.847504MHz) */
+
+                {      0xDE, 0x06,389             },/* 39 (388.631439MHz) */
+
+                {      0x52,0x2A,54               },/* 3A (54.000MHz) */
+
+                {      0x52,0x6A,27              },/* 3B (27.000MHz) */
+
+                {      0x62,0x24,70               },/* 3C (70.874991MHz) */
+
+                {      0x62,0x64,70               },/* 3D (70.1048912MHz) */
+
+                {      0xA8,0x4C,30               },/* 3E (30.1048912MHz) */
+
+                {      0x20,0x26,33               },/* 3F (33.7499957MHz) */
+
+                {      0x31,0xc2,39               },/* 40 (39.77MHz) */
+
+                {      0x11,0x21,30               },/* 41 (30MHz) }// NTSC 1024X768 */
+
+                {      0x2E,0x48,25               },/* 42 (25.175MHz) }// ScaleLCD */
+
+                {      0x24,0x46,25               },/* 43 (25.175MHz) */
+
+                {      0x26,0x64,28               },/* 44 (28.322MHz) */
+
+                {      0x37,0x64,40               },/* 45 (40.000MHz) */
+
+                {      0xA1,0x42,108               },/* 46 (95.000MHz) }// QVGA */
+
+                {      0x37,0x61,100               },/* 47 (100.00MHz) */
+
+                {      0x78,0x27,108               },/* 48 (108.200MHz) */
+
+                {      0xBF,0xC8,35               },/* 49 (35.2MHz) */
+
+                {      0x66,0x43,123               },/* 4A (122.61Mhz) */
+
+                {      0x2C,0x61,80               },/* 4B (80.350Mhz) */
+
+                {      0x3B,0x61,108               },/* 4C (107.385Mhz) */
+
+
+/*                {      0x60,0x36,30               },// 4D (30.200MHz)   }// No use
+
+                {      0x60,0x36,30               },// 4E (30.200MHz)   }// No use
+
+                {      0x60,0x36,30               },// 4F (30.200MHz)   }// No use
+
+                {      0x60,0x36,30               },// 50 (30.200MHz)   }// CHTV
+
+                {      0x40,0x4A,28               },// 51 (28.190MHz)
+
+                {      0x9F,0x46,44               },// 52 (43.600MHz)
+
+                {      0x97,0x2C,26               },// 53 (26.400MHz)
+
+                {      0x44,0xE4,25               },// 54 (24.600MHz)
+
+                {      0x7E,0x32,47               },// 55 (47.832MHz)
+
+                {      0x8A,0x24,31               },// 56 (31.500MHz)
+
+                {      0x97,0x2C,26               },// 57 (26.200MHz)
+
+                {      0xCE,0x3C,39               },// 58 (39.000MHz)
+
+                {      0x52,0x4A,36               },// 59 (36.000MHz)
+
+*/
+                {      0x69,0x61,191              }, /* 4D (190.96MHz ) */
+                {      0x4F,0x22,192              }, /* 4E (192.069MHz) */
+                {      0x28,0x26,322              }, /* 4F (322.273MHz) */
+                {      0x5C,0x6B,27               }, /* 50 (27.74HMz) */
+                {      0x57,0x24,126              }, /* 51 (125.999MHz) */
+                {      0x5C,0x42,148              }, /* 52 (148.5MHz) */
+                {      0x42,0x61,120              }, /* 53 (120.839MHz) */
+                {      0x62,0x61,178              }, /* 54 (178.992MHz) */
+                {      0x59,0x22,217              }, /* 55 (217.325MHz) */
+                {      0x29,0x01,300              }, /* 56 (299.505Mhz) */
+                {      0x52,0x63,74               }, /* 57 (74.25MHz) */
+
+
+                {      0xFF,0x00,0                }/* End mark */
+ }  ;
+
+XGI330_VCLKDataStruct XGI_VBVCLKData[]=
+{
+                {      0x1B,0xE1,25               },/* 00 (25.175MHz) */
+
+                {      0x4E,0xE4,28               },/* 01 (28.322MHz) */
+
+                {      0x57,0xE4,31               },/* 02 (31.500MHz) */
+
+                {      0xC3,0xC8,36               },/* 03 (36.000MHz) */
+
+                {      0x42,0x47,40               },/* 04 (40.000MHz) */
+
+                {      0xFE,0xCD,43               },/* 05 (43.163MHz) */
+
+                {      0x5D,0xC4,44               },/* 06 (44.900MHz) */
+
+                {      0x52,0x47,49               },/* 07 (49.500MHz) */
+
+                {      0x53,0x47,50               },/* 08 (50.000MHz) */
+
+                {      0x74,0x67,52               },/* 09 (52.406MHz) */
+
+                {      0x6D,0x66,56               },/* 0A (56.250MHz) */
+
+                {      0x35,0x62,65               },/* 0B (65.000MHz) */
+
+                {      0x46,0x44,67               },/* 0C (67.765MHz) */
+
+                {      0xB1,0x46,68               },/* 0D (68.179MHz) */
+
+                {      0xD3,0x4A,72               },/* 0E (72.852MHz) */
+
+                {      0x29,0x61,75               },/* 0F (75.000MHz) */
+
+                {      0x6D,0x46,75               },/* 10 (75.800MHz) */
+
+                {      0x41,0x43,78               },/* 11 (78.750MHz) */
+
+                {      0x31,0x42,79               },/* 12 (79.411MHz) */
+
+                {      0xAB,0x44,83               },/* 13 (83.950MHz) */
+
+                {      0x46,0x25,84               },/* 14 (84.800MHz) */
+
+                {      0x78,0x29,86               },/* 15 (86.600MHz) */
+
+                {      0x62,0x44,94               },/* 16 (94.500MHz) */
+
+                {      0x2B,0x22,104               },/* 17 (104.998MHz) */
+
+                {      0x49,0x24,105               },/* 18 (105.882MHz) */
+
+                {      0xF8,0x2F,108               },/* 19 (108.279MHz) */
+
+                {      0x3C,0x23,109               },/* 1A (109.175MHz) */
+
+                {      0x5E,0x43,113               },/* 1B (113.309MHz) */
+
+                {      0xBC,0x44,116               },/* 1C (116.406MHz) */
+
+                {      0xE0,0x46,132               },/* 1D (132.258MHz) */
+
+                {      0xD4,0x28,135               },/* 1E (135.220MHz) */
+
+                {      0xEA,0x2A,139               },/* 1F (139.275MHz) */
+
+                {      0x41,0x22,157               },/* 20 (157.500MHz) */
+
+                {      0x70,0x24,162               },/* 21 (161.793MHz) */
+
+                {      0x30,0x21,175               },/* 22 (175.000MHz) */
+
+                {      0x4E,0x22,189               },/* 23 (188.520MHz) */
+
+                {      0xDE,0x26,194               },/* 24 (194.400MHz) */
+
+                {      0x70,0x07,202               },/* 25 (202.500MHz) */
+
+                {      0x3F,0x03,229               },/* 26 (229.500MHz) */
+
+                {      0xB8,0x06,234               },/* 27 (233.178MHz) */
+
+                {      0x34,0x02,253               },/* 28 (252.699997 MHz) */
+
+                {      0x58,0x04,255               },/* 29 (254.817MHz) */
+
+                {      0x24,0x01,265               },/* 2A (265.728MHz) */
+
+                {      0x9B,0x02,267               },/* 2B (266.952MHz) */
+
+                {      0x70,0x05,270               },/* 2C (269.65567 MHz) */
+
+                {      0x25,0x01,272               },/* 2D (272.041992 MHz) */
+
+                {      0x9C,0x02,277               },/* 2E (277.015MHz) */
+
+                {      0x27,0x01,286               },/* 2F (286.359985 MHz) */
+
+                {      0x3C,0x02,291               },/* 30 (291.132660 MHz) */
+
+                {      0xEF,0x0A,292               },/* 31 (291.766MHz) */
+
+                {      0xF6,0x0A,310               },/* 32 (309.789459 MHz) */
+
+                {      0x95,0x01,315               },/* 33 (315.195MHz) */
+
+                {      0xF0,0x09,324               },/* 34 (323.586792 MHz) */
+
+                {      0xFE,0x0A,331               },/* 35 (330.615631 MHz) */
+
+                {      0xF3,0x09,332               },/* 36 (332.177612 MHz) */
+
+                {      0xEA,0x08,340               },/* 37 (340.477MHz) */
+
+                {      0xE8,0x07,376               },/* 38 (375.847504 MHz) */
+
+                {      0xDE,0x06,389               },/* 39 (388.631439 MHz) */
+
+                {      0x52,0x2A,54                },/* 3A (54.000MHz) */
+
+                {      0x52,0x6A,27                },/* 3B (27.000MHz) */
+
+
+                {      0x62,0x24,70                },/* 3C (70.874991MHz) */
+
+
+                {      0x62,0x64,70                },/* 3D (70.1048912MHz) */
+
+                {      0xA8,0x4C,30                },/* 3E (30.1048912MHz) */
+
+                {      0x20,0x26,33                },/* 3F (33.7499957MHz) */
+
+                {      0x31,0xc2,39                },/* 40 (39.77MHz) */
+
+                {      0x11,0x21,30                },/* 41 (30MHz) }// NTSC 1024X768 */
+
+                {      0x2E,0x48,25                },/* 42 (25.175MHz) }// ScaleLCD */
+
+                {      0x24,0x46,25                },/* 43 (25.175MHz) */
+
+                {      0x26,0x64,28                },/* 44 (28.322MHz) */
+
+                {      0x37,0x64,40                },/* 45 (40.000MHz) */
+
+                {      0xA1,0x42,108               },/* 46 (95.000MHz) }// QVGA */
+
+                {      0x37,0x61,100               },/* 47 (100.00MHz) */
+
+                {      0x78,0x27,108               },/* 48 (108.200MHz) */
+
+                {      0xBF,0xC8,35                },/* 49 (35.2MHz) */
+
+                {      0x66,0x43,123               },/* 4A (122.61Mhz) */
+
+                {      0x2C,0x61,80                },/* 4B (80.350Mhz) */
+
+                {      0x3B,0x61,108               },/* 4C (107.385Mhz) */
+
+/*
+                {      0x60,0x36,30               },// 4D (30.200MHz)   }// No use
+
+                {      0x60,0x36,30               },// 4E (30.200MHz)   }// No use
+
+                {      0x60,0x36,30               },// 4F (30.200MHz)   }// No use
+
+                {      0x60,0x36,30               },// 50 (30.200MHz)   }// CHTV
+
+                {      0x40,0x4A,28               },// 51 (28.190MHz)
+
+                {      0x9F,0x46,44               },// 52 (43.600MHz)
+
+                {      0x97,0x2C,26               },// 53 (26.400MHz)
+
+                {      0x44,0xE4,25               },// 54 (24.600MHz)
+
+                {      0x7E,0x32,47               },// 55 (47.832MHz)
+
+                {      0x8A,0x24,31               },// 56 (31.500MHz)
+
+                {      0x97,0x2C,26               },// 57 (26.200MHz)
+
+                {      0xCE,0x3C,39               },// 58 (39.000MHz)
+
+                {      0x52,0x4A,36               },// 59 (36.000MHz)
+*/
+                {      0x69,0x61,191              }, /* 4D (190.96MHz ) */
+                {      0x4F,0x22,192              }, /* 4E (192.069MHz) */
+                {      0x28,0x26,322              }, /* 4F (322.273MHz) */
+                {      0x5C,0x6B,27               }, /* 50 (27.74HMz) */
+                {      0x57,0x24,126              }, /* 51 (125.999MHz) */
+                {      0x5C,0x42,148              }, /* 52 (148.5MHz) */
+                {      0x42,0x61,120              }, /* 53 (120.839MHz) */
+                {      0x62,0x61,178              }, /* 54 (178.992MHz) */
+                {      0x59,0x22,217              }, /* 55 (217.325MHz) */
+                {      0x29,0x01,300              }, /* 56 (299.505Mhz) */
+                {      0x52,0x63,74               }, /* 57 (74.25MHz) */
+
+
+                {      0xFF,0x00,0                }      /* End mark */
+};
+
+UCHAR XGI660_TVDelayList[]=
+{
+          0x44,            /* ; 0 ExtNTSCDelay */
+          0x44,            /* ; 1 StNTSCDelay */
+          0x44,            /* ; 2 ExtPALDelay */
+          0x44,            /* ; 3 StPALDelay */
+          0x44,            /* ; 4 ExtHiTVDelay(1080i) */
+          0x44,            /* ; 5 StHiTVDelay(1080i) */
+          0x44,            /* ; 6 ExtYPbPrDelay(525i) */
+          0x44,            /* ; 7 StYPbPrDealy(525i) */
+          0x44,            /* ; 8 ExtYPbPrDelay(525p) */
+          0x44,            /* ; 9 StYPbPrDealy(525p) */
+          0x44,            /* ; A ExtYPbPrDelay(750p) */
+          0x44             /* ; B StYPbPrDealy(750p) */
+};
+
+UCHAR XGI660_TVDelayList2[]=
+{
+          0x44,           /* ; 0 ExtNTSCDelay */
+          0x44,           /* ; 1 StNTSCDelay */
+          0x44,           /* ; 2 ExtPALDelay */
+          0x44,           /* ; 3 StPALDelay */
+          0x44,           /* ; 4 ExtHiTVDelay */
+          0x44,           /* ; 5 StHiTVDelay */
+          0x44,           /* ; 6 ExtYPbPrDelay(525i) */
+          0x44,           /* ; 7 StYPbPrDealy(525i) */
+          0x44,           /* ; 8 ExtYPbPrDelay(525p) */
+          0x44,           /* ; 9 StYPbPrDealy(525p) */
+          0x44,           /* ; A ExtYPbPrDelay(750p) */
+          0x44            /* ; B StYPbPrDealy(750p) */
+};
+
+UCHAR XGI301TVDelayList[]=
+{
+        0x22,            /* ; 0 ExtNTSCDelay */
+        0x22,            /* ; 1 StNTSCDelay */
+        0x22,            /* ; 2 ExtPALDelay */
+        0x22,            /* ; 3 StPALDelay */
+        0x88,            /* ; 4 ExtHiTVDelay(1080i) */
+        0xBB,            /* ; 5 StHiTVDelay(1080i) */
+        0x22,            /* ; 6 ExtYPbPrDelay(525i) */
+        0x22,            /* ; 7 StYPbPrDealy(525i) */
+        0x22,            /* ; 8 ExtYPbPrDelay(525p) */
+        0x22,            /* ; 9 StYPbPrDealy(525p) */
+        0x22,            /* ; A ExtYPbPrDelay(750p) */
+        0x22            /* B StYPbPrDealy(750p) */
+};
+
+UCHAR XGI301TVDelayList2[]=
+{
+        0x22,           /* ; 0 ExtNTSCDelay */
+        0x22,           /* ; 1 StNTSCDelay */
+        0x22,           /* ; 2 ExtPALDelay */
+        0x22,           /* ; 3 StPALDelay */
+        0x22,           /* ; 4 ExtHiTVDelay */
+        0x22,           /* ; 5 StHiTVDelay */
+        0x22,           /* ; 6 ExtYPbPrDelay(525i) */
+        0x22,           /* ; 7 StYPbPrDealy(525i) */
+        0x22,           /* ; 8 ExtYPbPrDelay(525p) */
+        0x22,           /* ; 9 StYPbPrDealy(525p) */
+        0x22,           /* ; A ExtYPbPrDelay(750p) */
+        0x22            /* ; B StYPbPrDealy(750p) */
+};
+
+
+UCHAR TVAntiFlickList[]=
+{/* NTSCAntiFlicker */
+                      0x04,           /* ; 0 Adaptive */
+                      0x00,           /* ; 1 new anti-flicker ? */
+/* PALAntiFlicker */
+                      0x04,           /* ; 0 Adaptive */
+                      0x08,           /* ; 1 new anti-flicker ? */
+/* HiTVAntiFlicker */
+                      0x04,           /* ; 0 ? */
+                      0x00            /* ; 1 new anti-flicker ? */
+};
+
+
+UCHAR TVEdgeList[]=
+{
+      0x00,            /* ; 0 NTSC No Edge enhance */
+      0x04,            /* ; 1 NTSC Adaptive Edge enhance */
+      0x00,            /* ; 0 PAL No Edge enhance */
+      0x04,            /* ; 1 PAL Adaptive Edge enhance */
+      0x00,            /* ; 0 HiTV */
+      0x00             /* ; 1 HiTV */
+};
+
+ULONG TVPhaseList[]=
+{      0x08BAED21, /* ; 0 NTSC phase */
+       0x00E3052A, /* ; 1 PAL phase */
+       0x9B2EE421, /* ; 2 PAL-M phase */
+       0xBA3EF421, /* ; 3 PAL-N phase */
+       0xA7A28B1E, /* ; 4 NTSC 1024x768 */
+       0xE00A831E, /* ; 5 PAL-M 1024x768 */
+       0x00000000, /* ; 6 reserved */
+       0x00000000, /* ; 7 reserved */
+       0xD67BF021, /* ; 8 NTSC phase */
+       0xE986092A, /* ; 9 PAL phase */
+       0xA4EFE621, /* ; A PAL-M phase */
+       0x4694F621, /* ; B PAL-N phase */
+       0x8BDE711C, /* ; C NTSC 1024x768 */
+       0xE00A831E  /* ; D PAL-M 1024x768 */
+};
+
+UCHAR NTSCYFilter1[]=
+{
+                      0x00,0xF4,0x10,0x38     ,/* 0 : 320x text mode */
+                      0x00,0xF4,0x10,0x38     ,/* 1 : 360x text mode */
+                      0xEB,0x04,0x25,0x18     ,/* 2 : 640x text mode */
+                      0xF1,0x04,0x1F,0x18     ,/* 3 : 720x text mode */
+                      0x00,0xF4,0x10,0x38     ,/* 4 : 320x gra. mode */
+                      0xEB,0x04,0x25,0x18     ,/* 5 : 640x gra. mode */
+                      0xEB,0x15,0x25,0xF6     /* 6 : 800x gra. mode */
+};
+
+UCHAR PALYFilter1[]=
+{
+                      0x00,0xF4,0x10,0x38, /* 0 : 320x text mode */
+                      0x00,0xF4,0x10,0x38     ,/* 1 : 360x text mode */
+                      0xF1,0xF7,0x1F,0x32     ,/* 2 : 640x text mode */
+                      0xF3,0x00,0x1D,0x20     ,/* 3 : 720x text mode */
+                      0x00,0xF4,0x10,0x38     ,/* 4 : 320x gra. mode */
+                      0xF1,0xF7,0x1F,0x32     ,/* 5 : 640x gra. mode */
+                      0xFC,0xFB,0x14,0x2A     /* 6 : 800x gra. mode */
+};
+
+UCHAR PALMYFilter1[]=
+{
+                      0x00,0xF4,0x10,0x38, /* 0 : 320x text mode */
+                      0x00,0xF4,0x10,0x38, /* 1 : 360x text mode */
+                      0xEB,0x04,0x10,0x18, /* 2 : 640x text mode */
+                      0xF7,0x06,0x19,0x14, /* 3 : 720x text mode */
+                      0x00,0xF4,0x10,0x38, /* 4 : 320x gra. mode */
+                      0xEB,0x04,0x25,0x18, /* 5 : 640x gra. mode */
+                      0xEB,0x15,0x25,0xF6, /* 6 : 800x gra. mode */
+                      0xFF,0xFF,0xFF,0xFF  /* End of Table */
+};
+
+UCHAR PALNYFilter1[]=
+{
+                      0x00,0xF4,0x10,0x38, /* 0 : 320x text mode */
+                      0x00,0xF4,0x10,0x38, /* 1 : 360x text mode */
+                      0xEB,0x04,0x10,0x18, /* 2 : 640x text mode */
+                      0xF7,0x06,0x19,0x14, /* 3 : 720x text mode */
+                      0x00,0xF4,0x10,0x38, /* 4 : 320x gra. mode */
+                      0xEB,0x04,0x25,0x18, /* 5 : 640x gra. mode */
+                      0xEB,0x15,0x25,0xF6, /* 6 : 800x gra. mode */
+                      0xFF,0xFF,0xFF,0xFF  /* End of Table */
+};
+
+UCHAR NTSCYFilter2[]=
+{
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 0 : 320x text mode */
+                      0x01,0x02,0xFE,0xF7,0x03,0x27,0x3C, /* 1 : 360x text mode */
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 2 : 640x text mode */
+                      0x01,0x02,0xFE,0xF7,0x03,0x27,0x3C, /* 3 : 720x text mode */
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 4 : 320x gra. mode */
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 5 : 640x gra. mode */
+                      0x01,0x01,0xFC,0xF8,0x08,0x26,0x38, /* 6 : 800x gra. mode */
+                      0xFF,0xFF,0xFC,0x00,0x0F,0x22,0x28  /* 7 : 1024xgra. mode */
+};
+
+UCHAR PALYFilter2[]=
+{
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 0 : 320x text mode */
+                      0x01,0x02,0xFE,0xF7,0x03,0x27,0x3C, /* 1 : 360x text mode */
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 2 : 640x text mode */
+                      0x01,0x02,0xFE,0xF7,0x03,0x27,0x3C, /* 3 : 720x text mode */
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 4 : 320x gra. mode */
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 5 : 640x gra. mode */
+                      0x01,0x01,0xFC,0xF8,0x08,0x26,0x38, /* 6 : 800x gra. mode */
+                      0xFF,0xFF,0xFC,0x00,0x0F,0x22,0x28  /* 7 : 1024xgra. mode */
+};
+
+UCHAR PALMYFilter2[]=
+{
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 0 : 320x text mode */
+                      0x01,0x02,0xFE,0xF7,0x03,0x27,0x3C, /* 1 : 360x text mode */
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 2 : 640x text mode */
+                      0x01,0x02,0xFE,0xF7,0x03,0x27,0x3C, /* 3 : 720x text mode */
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 4 : 320x gra. mode */
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 5 : 640x gra. mode */
+                      0x01,0x01,0xFC,0xF8,0x08,0x26,0x38, /* 6 : 800x gra. mode */
+                      0xFF,0xFF,0xFC,0x00,0x0F,0x22,0x28  /* 7 : 1024xgra. mode */
+};
+
+UCHAR PALNYFilter2[]=
+{
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 0 : 320x text mode */
+                      0x01,0x02,0xFE,0xF7,0x03,0x27,0x3C, /* 1 : 360x text mode */
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 2 : 640x text mode */
+                      0x01,0x02,0xFE,0xF7,0x03,0x27,0x3C, /* 3 : 720x text mode */
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 4 : 320x gra. mode */
+                      0xFF,0x03,0x02,0xF6,0xFC,0x27,0x46, /* 5 : 640x gra. mode */
+                      0x01,0x01,0xFC,0xF8,0x08,0x26,0x38, /* 6 : 800x gra. mode */
+                      0xFF,0xFF,0xFC,0x00,0x0F,0x22,0x28  /* 7 : 1024xgra. mode */
+};
+
+UCHAR XGI_NTSC1024AdjTime[]=
+{
+      0xa7,0x07,0xf2,0x6e,0x17,0x8b,0x73,0x53,
+      0x13,0x40,0x34,0xF4,0x63,0xBB,0xCC,0x7A,
+      0x58,0xe4,0x73,0xd0,0x13
+};
+
+XGI301C_Tap4TimingStruct HiTVTap4Timing[]=
+{
+        {0,{
+        0x00,0x20,0x00,0x00,0x7F,0x20,0x02,0x7F, /* ; C0-C7 */
+        0x7D,0x20,0x04,0x7F,0x7D,0x1F,0x06,0x7E, /* ; C8-CF */
+        0x7C,0x1D,0x09,0x7E,0x7C,0x1B,0x0B,0x7E, /* ; D0-D7 */
+        0x7C,0x19,0x0E,0x7D,0x7C,0x17,0x11,0x7C, /* ; D8-DF */
+        0x7C,0x14,0x14,0x7C,0x7C,0x11,0x17,0x7C, /* ; E0-E7 */
+        0x7D,0x0E,0x19,0x7C,0x7E,0x0B,0x1B,0x7C, /* ; EA-EF */
+        0x7E,0x09,0x1D,0x7C,0x7F,0x06,0x1F,0x7C, /* ; F0-F7 */
+        0x7F,0x04,0x20,0x7D,0x00,0x02,0x20,0x7E  /* ; F8-FF */
+        }
+        }
+};
+
+XGI301C_Tap4TimingStruct EnlargeTap4Timing[]=
+{
+        {0,{
+        0x00,0x20,0x00,0x00,0x7F,0x20,0x02,0x7F, /* ; C0-C7 */
+        0x7D,0x20,0x04,0x7F,0x7D,0x1F,0x06,0x7E, /* ; C8-CF */
+        0x7C,0x1D,0x09,0x7E,0x7C,0x1B,0x0B,0x7E, /* ; D0-D7 */
+        0x7C,0x19,0x0E,0x7D,0x7C,0x17,0x11,0x7C, /* ; D8-DF */
+        0x7C,0x14,0x14,0x7C,0x7C,0x11,0x17,0x7C, /* ; E0-E7 */
+        0x7D,0x0E,0x19,0x7C,0x7E,0x0B,0x1B,0x7C, /* ; EA-EF */
+        0x7E,0x09,0x1D,0x7C,0x7F,0x06,0x1F,0x7C, /* ; F0-F7 */
+        0x7F,0x04,0x20,0x7D,0x00,0x02,0x20,0x7E   /* ; F8-FF */
+        }
+        }
+};
+
+XGI301C_Tap4TimingStruct NoScaleTap4Timing[]=
+{
+        {0,{
+        0x00,0x20,0x00,0x00,0x7F,0x20,0x02,0x7F, /* ; C0-C7 */
+        0x7D,0x20,0x04,0x7F,0x7D,0x1F,0x06,0x7E, /* ; C8-CF */
+        0x7C,0x1D,0x09,0x7E,0x7C,0x1B,0x0B,0x7E, /* ; D0-D7 */
+        0x7C,0x19,0x0E,0x7D,0x7C,0x17,0x11,0x7C, /* ; D8-DF */
+        0x7C,0x14,0x14,0x7C,0x7C,0x11,0x17,0x7C, /* ; E0-E7 */
+        0x7D,0x0E,0x19,0x7C,0x7E,0x0B,0x1B,0x7C, /* ; EA-EF */
+        0x7E,0x09,0x1D,0x7C,0x7F,0x06,0x1F,0x7C, /* ; F0-F7 */
+        0x7F,0x04,0x20,0x7D,0x00,0x02,0x20,0x7E  /* ; F8-FF */
+        }
+        }
+};
+
+XGI301C_Tap4TimingStruct PALTap4Timing[]=
+{
+        {600,  {
+                0x05,0x19,0x05,0x7D,0x03,0x19,0x06,0x7E, /* ; C0-C7 */
+                0x02,0x19,0x08,0x7D,0x01,0x18,0x0A,0x7D, /* ; C8-CF */
+                0x00,0x18,0x0C,0x7C,0x7F,0x17,0x0E,0x7C, /* ; D0-D7 */
+                0x7E,0x16,0x0F,0x7D,0x7E,0x14,0x11,0x7D, /* ; D8-DF */
+                0x7D,0x13,0x13,0x7D,0x7D,0x11,0x14,0x7E, /* ; E0-E7 */
+                0x7D,0x0F,0x16,0x7E,0x7D,0x0E,0x17,0x7E, /* ; EA-EF */
+                0x7D,0x0C,0x18,0x7F,0x7D,0x0A,0x18,0x01, /* ; F0-F7 */
+                0x7D,0x08,0x19,0x02,0x7D,0x06,0x19,0x04  /* ; F8-FF */
+                }
+        },
+        {768,   {
+                0x08,0x12,0x08,0x7E,0x07,0x12,0x09,0x7E, /* ; C0-C7 */
+                0x06,0x12,0x0A,0x7E,0x05,0x11,0x0B,0x7F, /* ; C8-CF */
+                0x04,0x11,0x0C,0x7F,0x03,0x11,0x0C,0x00, /* ; D0-D7 */
+                0x03,0x10,0x0D,0x00,0x02,0x0F,0x0E,0x01, /* ; D8-DF */
+                0x01,0x0F,0x0F,0x01,0x01,0x0E,0x0F,0x02, /* ; E0-E7 */
+                0x00,0x0D,0x10,0x03,0x7F,0x0C,0x11,0x04, /* ; EA-EF */
+                0x7F,0x0C,0x11,0x04,0x7F,0x0B,0x11,0x05, /* ; F0-F7 */
+                0x7E,0x0A,0x12,0x06,0x7E,0x09,0x12,0x07  /* ; F8-FF */
+                }
+        },
+        {0xFFFF,
+                {
+                0x04,0x1A,0x04,0x7E,0x02,0x1B,0x05,0x7E, /* ; C0-C7 */
+                0x01,0x1A,0x07,0x7E,0x00,0x1A,0x09,0x7D, /* ; C8-CF */
+                0x7F,0x19,0x0B,0x7D,0x7E,0x18,0x0D,0x7D, /* ; D0-D7 */
+                0x7D,0x17,0x10,0x7C,0x7D,0x15,0x12,0x7C, /* ; D8-DF */
+                0x7C,0x14,0x14,0x7C,0x7C,0x12,0x15,0x7D, /* ; E0-E7 */
+                0x7C,0x10,0x17,0x7D,0x7C,0x0D,0x18,0x7F, /* ; EA-EF */
+                0x7D,0x0B,0x19,0x7F,0x7D,0x09,0x1A,0x00, /* ; F0-F7 */
+                0x7D,0x07,0x1A,0x02,0x7E,0x05,0x1B,0x02  /* ; F8-FF */
+                }
+        }
+};
+
+XGI301C_Tap4TimingStruct NTSCTap4Timing[]=
+{
+        {480,   {
+                0x04,0x1A,0x04,0x7E,0x03,0x1A,0x06,0x7D, /* ; C0-C7 */
+                0x01,0x1A,0x08,0x7D,0x00,0x19,0x0A,0x7D, /* ; C8-CF */
+                0x7F,0x19,0x0C,0x7C,0x7E,0x18,0x0E,0x7C, /* ; D0-D7 */
+                0x7E,0x17,0x10,0x7B,0x7D,0x15,0x12,0x7C, /* ; D8-DF */
+                0x7D,0x13,0x13,0x7D,0x7C,0x12,0x15,0x7D, /* ; E0-E7 */
+                0x7C,0x10,0x17,0x7D,0x7C,0x0E,0x18,0x7E, /* ; EA-EF */
+                0x7D,0x0C,0x19,0x7E,0x7D,0x0A,0x19,0x00, /* ; F0-F7 */
+                0x7D,0x08,0x1A,0x01,0x7E,0x06,0x1A,0x02  /* ; F8-FF */
+                }
+        },
+        {600,   {
+                0x07,0x14,0x07,0x7E,0x06,0x14,0x09,0x7D, /* ; C0-C7 */
+                0x05,0x14,0x0A,0x7D,0x04,0x13,0x0B,0x7E, /* ; C8-CF */
+                0x03,0x13,0x0C,0x7E,0x02,0x12,0x0D,0x7F, /* ; D0-D7 */
+                0x01,0x12,0x0E,0x7F,0x01,0x11,0x0F,0x7F, /* ; D8-DF */
+                0x01,0x10,0x10,0x00,0x7F,0x0F,0x11,0x01, /* ; E0-E7 */
+                0x7F,0x0E,0x12,0x01,0x7E,0x0D,0x12,0x03, /* ; EA-EF */
+                0x7E,0x0C,0x13,0x03,0x7E,0x0B,0x13,0x04, /* ; F0-F7 */
+                0x7E,0x0A,0x14,0x04,0x7D,0x09,0x14,0x06  /* ; F8-FF */
+                }
+        },
+        {0xFFFF,
+                {
+                0x09,0x0F,0x09,0x7F,0x08,0x0F,0x09,0x00, /* ; C0-C7 */
+                0x07,0x0F,0x0A,0x00,0x06,0x0F,0x0A,0x01, /* ; C8-CF */
+                0x06,0x0E,0x0B,0x01,0x05,0x0E,0x0B,0x02, /* ; D0-D7 */
+                0x04,0x0E,0x0C,0x02,0x04,0x0D,0x0C,0x03, /* ; D8-DF */
+                0x03,0x0D,0x0D,0x03,0x02,0x0C,0x0D,0x05, /* ; E0-E7 */
+                0x02,0x0C,0x0E,0x04,0x01,0x0B,0x0E,0x06, /* ; EA-EF */
+                0x01,0x0B,0x0E,0x06,0x00,0x0A,0x0F,0x07, /* ; F0-F7 */
+                0x00,0x0A,0x0F,0x07,0x00,0x09,0x0F,0x08  /* ; F8-FF */
+                }
+        }
+};
+
+XGI301C_Tap4TimingStruct YPbPr525pTap4Timing[]=
+{
+        {480,   {
+                0x04,0x1A,0x04,0x7E,0x03,0x1A,0x06,0x7D, /* ; C0-C7 */
+                0x01,0x1A,0x08,0x7D,0x00,0x19,0x0A,0x7D, /* ; C8-CF */
+                0x7F,0x19,0x0C,0x7C,0x7E,0x18,0x0E,0x7C, /* ; D0-D7 */
+                0x7E,0x17,0x10,0x7B,0x7D,0x15,0x12,0x7C, /* ; D8-DF */
+                0x7D,0x13,0x13,0x7D,0x7C,0x12,0x15,0x7D, /* ; E0-E7 */
+                0x7C,0x10,0x17,0x7D,0x7C,0x0E,0x18,0x7E, /* ; EA-EF */
+                0x7D,0x0C,0x19,0x7E,0x7D,0x0A,0x19,0x00, /* ; F0-F7 */
+                0x7D,0x08,0x1A,0x01,0x7E,0x06,0x1A,0x02  /* ; F8-FF */
+                }
+        },
+        {600,   {
+                0x07,0x14,0x07,0x7E,0x06,0x14,0x09,0x7D, /* ; C0-C7 */
+                0x05,0x14,0x0A,0x7D,0x04,0x13,0x0B,0x7E, /* ; C8-CF */
+                0x03,0x13,0x0C,0x7E,0x02,0x12,0x0D,0x7F, /* ; D0-D7 */
+                0x01,0x12,0x0E,0x7F,0x01,0x11,0x0F,0x7F, /* ; D8-DF */
+                0x01,0x10,0x10,0x00,0x7F,0x0F,0x11,0x01, /* ; E0-E7 */
+                0x7F,0x0E,0x12,0x01,0x7E,0x0D,0x12,0x03, /* ; EA-EF */
+                0x7E,0x0C,0x13,0x03,0x7E,0x0B,0x13,0x04, /* ; F0-F7 */
+                0x7E,0x0A,0x14,0x04,0x7D,0x09,0x14,0x06  /* ; F8-FF */
+                }
+        },
+        {0xFFFF,
+                {
+                0x09,0x0F,0x09,0x7F,0x08,0x0F,0x09,0x00, /* ; C0-C7 */
+                0x07,0x0F,0x0A,0x00,0x06,0x0F,0x0A,0x01, /* ; C8-CF */
+                0x06,0x0E,0x0B,0x01,0x05,0x0E,0x0B,0x02, /* ; D0-D7 */
+                0x04,0x0E,0x0C,0x02,0x04,0x0D,0x0C,0x03, /* ; D8-DF */
+                0x03,0x0D,0x0D,0x03,0x02,0x0C,0x0D,0x05, /* ; E0-E7 */
+                0x02,0x0C,0x0E,0x04,0x01,0x0B,0x0E,0x06, /* ; EA-EF */
+                0x01,0x0B,0x0E,0x06,0x00,0x0A,0x0F,0x07, /* ; F0-F7 */
+                0x00,0x0A,0x0F,0x07,0x00,0x09,0x0F,0x08  /* ; F8-FF */
+                }
+        }
+};
+
+XGI301C_Tap4TimingStruct YPbPr525iTap4Timing[]=
+{
+        {480,   {
+                0x04,0x1A,0x04,0x7E,0x03,0x1A,0x06,0x7D, /* ; C0-C7 */
+                0x01,0x1A,0x08,0x7D,0x00,0x19,0x0A,0x7D, /* ; C8-CF */
+                0x7F,0x19,0x0C,0x7C,0x7E,0x18,0x0E,0x7C, /* ; D0-D7 */
+                0x7E,0x17,0x10,0x7B,0x7D,0x15,0x12,0x7C, /* ; D8-DF */
+                0x7D,0x13,0x13,0x7D,0x7C,0x12,0x15,0x7D, /* ; E0-E7 */
+                0x7C,0x10,0x17,0x7D,0x7C,0x0E,0x18,0x7E, /* ; EA-EF */
+                0x7D,0x0C,0x19,0x7E,0x7D,0x0A,0x19,0x00, /* ; F0-F7 */
+                0x7D,0x08,0x1A,0x01,0x7E,0x06,0x1A,0x02  /* ; F8-FF */
+                }
+        },
+        {600,   {
+                0x07,0x14,0x07,0x7E,0x06,0x14,0x09,0x7D, /* ; C0-C7 */
+                0x05,0x14,0x0A,0x7D,0x04,0x13,0x0B,0x7E, /* ; C8-CF */
+                0x03,0x13,0x0C,0x7E,0x02,0x12,0x0D,0x7F, /* ; D0-D7 */
+                0x01,0x12,0x0E,0x7F,0x01,0x11,0x0F,0x7F, /* ; D8-DF */
+                0x01,0x10,0x10,0x00,0x7F,0x0F,0x11,0x01, /* ; E0-E7 */
+                0x7F,0x0E,0x12,0x01,0x7E,0x0D,0x12,0x03, /* ; EA-EF */
+                0x7E,0x0C,0x13,0x03,0x7E,0x0B,0x13,0x04, /* ; F0-F7 */
+                0x7E,0x0A,0x14,0x04,0x7D,0x09,0x14,0x06  /* ; F8-FF */
+                }
+        },
+        {0xFFFF,
+                {
+                0x09,0x0F,0x09,0x7F,0x08,0x0F,0x09,0x00, /* ; C0-C7 */
+                0x07,0x0F,0x0A,0x00,0x06,0x0F,0x0A,0x01, /* ; C8-CF */
+                0x06,0x0E,0x0B,0x01,0x05,0x0E,0x0B,0x02, /* ; D0-D7 */
+                0x04,0x0E,0x0C,0x02,0x04,0x0D,0x0C,0x03, /* ; D8-DF */
+                0x03,0x0D,0x0D,0x03,0x02,0x0C,0x0D,0x05, /* ; E0-E7 */
+                0x02,0x0C,0x0E,0x04,0x01,0x0B,0x0E,0x06, /* ; EA-EF */
+                0x01,0x0B,0x0E,0x06,0x00,0x0A,0x0F,0x07, /* ; F0-F7 */
+                0x00,0x0A,0x0F,0x07,0x00,0x09,0x0F,0x08  /* ; F8-FF */
+                }
+        }
+};
+
+XGI301C_Tap4TimingStruct YPbPr750pTap4Timing[]=
+{        {0xFFFF,
+               {
+               0x05,0x19,0x05,0x7D,0x03,0x19,0x06,0x7E, /* ; C0-C7 */
+               0x02,0x19,0x08,0x7D,0x01,0x18,0x0A,0x7D, /* ; C8-CF */
+               0x00,0x18,0x0C,0x7C,0x7F,0x17,0x0E,0x7C, /* ; D0-D7 */
+               0x7E,0x16,0x0F,0x7D,0x7E,0x14,0x11,0x7D, /* ; D8-DF */
+               0x7D,0x13,0x13,0x7D,0x7D,0x11,0x14,0x7E, /* ; E0-E7 */
+               0x7D,0x0F,0x16,0x7E,0x7D,0x0E,0x17,0x7E, /* ; EA-EF */
+               0x7D,0x0C,0x18,0x7F,0x7D,0x0A,0x18,0x01, /* ; F0-F7 */
+               0x7D,0x08,0x19,0x02,0x7D,0x06,0x19,0x04 /* F8-FF */
+               }
+        }
+};
diff --git a/drivers/staging/xgifb/vb_util.c b/drivers/staging/xgifb/vb_util.c
new file mode 100644
index 000000000000..87531b49b739
--- /dev/null
+++ b/drivers/staging/xgifb/vb_util.c
@@ -0,0 +1,263 @@
+#include "osdef.h"
+#include "vb_def.h"
+#include "vgatypes.h"
+#include "vb_struct.h"
+
+#ifdef LINUX_KERNEL
+#include "XGIfb.h"
+#include <asm/io.h>
+#include <linux/types.h>
+#endif
+
+#ifdef TC
+#include <stdio.h>
+#include <string.h>
+#include <conio.h>
+#include <dos.h>
+#endif
+
+#ifdef WIN2000
+#include <dderror.h>
+#include <devioctl.h>
+#include <miniport.h>
+#include <ntddvdeo.h>
+#include <video.h>
+
+#include "xgiv.h"
+#include "dd_i2c.h"
+#include "tools.h"
+#endif
+
+#ifdef LINUX_XF86
+#include "xf86.h"
+#include "xf86PciInfo.h"
+#include "xgi.h"
+#include "xgi_regs.h"
+#endif
+
+
+
+
+void XGINew_SetReg1( ULONG , USHORT , USHORT ) ;
+void XGINew_SetReg2( ULONG , USHORT , USHORT ) ;
+void XGINew_SetReg3( ULONG , USHORT ) ;
+void XGINew_SetReg4( ULONG , ULONG ) ;
+UCHAR XGINew_GetReg1( ULONG , USHORT) ;
+UCHAR XGINew_GetReg2( ULONG ) ;
+ULONG XGINew_GetReg3( ULONG ) ;
+void XGINew_ClearDAC( PUCHAR ) ;
+void     XGINew_SetRegANDOR(ULONG Port,USHORT Index,USHORT DataAND,USHORT DataOR);
+void     XGINew_SetRegOR(ULONG Port,USHORT Index,USHORT DataOR);
+void     XGINew_SetRegAND(ULONG Port,USHORT Index,USHORT DataAND);
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetReg1 */
+/* Input : */
+/* Output : */
+/* Description : SR CRTC GR */
+/* --------------------------------------------------------------------- */
+void XGINew_SetReg1( ULONG port , USHORT index , USHORT data )
+{
+#ifdef LINUX_XF86
+    OutPortByte( ( PUCHAR )(ULONG)port , index ) ;
+    OutPortByte( ( PUCHAR )(ULONG)port + 1 , data ) ;
+#else
+    OutPortByte( port , index ) ;
+    OutPortByte( port + 1 , data ) ;
+#endif
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetReg2 */
+/* Input : */
+/* Output : */
+/* Description : AR( 3C0 ) */
+/* --------------------------------------------------------------------- */
+/*void XGINew_SetReg2( ULONG port , USHORT index , USHORT data )
+{
+    InPortByte( ( PUCHAR )port + 0x3da - 0x3c0 ) ;
+    OutPortByte( XGINew_P3c0 , index ) ;
+    OutPortByte( XGINew_P3c0 , data ) ;
+    OutPortByte( XGINew_P3c0 , 0x20 ) ;
+}*/
+
+
+/* --------------------------------------------------------------------- */
+/* Function : */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SetReg3( ULONG port , USHORT data )
+{
+    OutPortByte( port , data ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetReg4 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SetReg4( ULONG port , ULONG data )
+{
+    OutPortLong( port , data ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_GetReg1 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+UCHAR XGINew_GetReg1( ULONG port , USHORT index )
+{
+    UCHAR data ;
+
+#ifdef LINUX_XF86
+    OutPortByte( ( PUCHAR )(ULONG)port , index ) ;
+    data = InPortByte( ( PUCHAR )(ULONG)port + 1 ) ;
+#else
+    OutPortByte( port , index ) ;
+    data = InPortByte( port + 1 ) ;
+#endif
+
+    return( data ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_GetReg2 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+UCHAR XGINew_GetReg2( ULONG port )
+{
+    UCHAR data ;
+
+    data = InPortByte( port ) ;
+
+    return( data ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_GetReg3 */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+ULONG XGINew_GetReg3( ULONG port )
+{
+    ULONG data ;
+
+    data = InPortLong( port ) ;
+
+    return( data ) ;
+}
+
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetRegANDOR */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SetRegANDOR( ULONG Port , USHORT Index , USHORT DataAND , USHORT DataOR )
+{
+    USHORT temp ;
+
+    temp = XGINew_GetReg1( Port , Index ) ;             /* XGINew_Part1Port index 02 */
+    temp = ( temp & ( DataAND ) ) | DataOR ;
+    XGINew_SetReg1( Port , Index , temp ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetRegAND */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SetRegAND(ULONG Port,USHORT Index,USHORT DataAND)
+{
+    USHORT temp ;
+
+    temp = XGINew_GetReg1( Port , Index ) ;     /* XGINew_Part1Port index 02 */
+    temp &= DataAND ;
+    XGINew_SetReg1( Port , Index , temp ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : XGINew_SetRegOR */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void XGINew_SetRegOR( ULONG Port , USHORT Index , USHORT DataOR )
+{
+    USHORT temp ;
+
+    temp = XGINew_GetReg1( Port , Index ) ;     /* XGINew_Part1Port index 02 */
+    temp |= DataOR ;
+    XGINew_SetReg1( Port , Index , temp ) ;
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : NewDelaySecond */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void NewDelaySeconds( int seconds )
+{
+#ifdef WIN2000
+    int j ;
+#endif
+    int i ;
+
+
+    for( i = 0 ; i < seconds ; i++ )
+    {
+#ifdef TC
+        delay( 1000 ) ;
+#endif
+
+#ifdef WIN2000
+
+        for ( j = 0 ; j < 20000 ; j++ )
+            VideoPortStallExecution( 50 ) ;
+#endif
+
+#ifdef WINCE_HEADER
+#endif
+
+#ifdef LINUX_KERNEL
+#endif
+    }
+}
+
+
+/* --------------------------------------------------------------------- */
+/* Function : Newdebugcode */
+/* Input : */
+/* Output : */
+/* Description : */
+/* --------------------------------------------------------------------- */
+void Newdebugcode( UCHAR code )
+{
+//    OutPortByte ( 0x80 , code ) ;
+    /* OutPortByte ( 0x300 , code ) ; */
+    /* NewDelaySeconds( 0x3 ) ; */
+}
+
+
+
diff --git a/drivers/staging/xgifb/vb_util.h b/drivers/staging/xgifb/vb_util.h
new file mode 100644
index 000000000000..91779d8cfdc6
--- /dev/null
+++ b/drivers/staging/xgifb/vb_util.h
@@ -0,0 +1,15 @@
+#ifndef _VBUTIL_
+#define _VBUTIL_
+extern   void     NewDelaySeconds( int );
+extern   void     Newdebugcode( UCHAR );
+extern   void     XGINew_SetReg1(ULONG, USHORT, USHORT);
+extern   void     XGINew_SetReg3(ULONG, USHORT);
+extern   UCHAR    XGINew_GetReg1(ULONG, USHORT);
+extern   UCHAR    XGINew_GetReg2(ULONG);
+extern   void     XGINew_SetReg4(ULONG, ULONG);
+extern   ULONG    XGINew_GetReg3(ULONG);
+extern   void     XGINew_SetRegOR(ULONG Port,USHORT Index,USHORT DataOR);
+extern   void     XGINew_SetRegAND(ULONG Port,USHORT Index,USHORT DataAND);
+extern   void     XGINew_SetRegANDOR(ULONG Port,USHORT Index,USHORT DataAND,USHORT DataOR);
+#endif
+
diff --git a/drivers/staging/xgifb/vgatypes.h b/drivers/staging/xgifb/vgatypes.h
new file mode 100644
index 000000000000..295ea860ae47
--- /dev/null
+++ b/drivers/staging/xgifb/vgatypes.h
@@ -0,0 +1,325 @@
+
+#ifndef _VGATYPES_
+#define _VGATYPES_
+
+#include "osdef.h"
+
+#ifdef LINUX_XF86
+#include "xf86Version.h"
+#include "xf86Pci.h"
+#endif
+
+#ifdef LINUX_KERNEL  /* We don't want the X driver to depend on kernel source */
+#include <linux/ioctl.h>
+#endif
+
+#ifndef FALSE
+#define FALSE   0
+#endif
+
+#ifndef TRUE
+#define TRUE    1
+#endif
+
+#ifndef NULL
+#define NULL    0
+#endif
+
+#ifndef CHAR
+typedef char CHAR;
+#endif
+
+#ifndef SHORT
+typedef short SHORT;
+#endif
+
+#ifndef LONG
+typedef long  LONG;
+#endif
+
+#ifndef UCHAR
+typedef unsigned char UCHAR;
+#endif
+
+#ifndef USHORT
+typedef unsigned short USHORT;
+#endif
+
+#ifndef ULONG
+typedef unsigned long ULONG;
+#endif
+
+#ifndef PUCHAR
+typedef UCHAR *PUCHAR;
+#endif
+
+#ifndef PUSHORT
+typedef USHORT *PUSHORT;
+#endif
+
+#ifndef PLONGU
+typedef ULONG *PULONG;
+#endif
+
+#ifndef VOID
+typedef void VOID;
+#endif
+
+#ifndef PVOID
+typedef void *PVOID;
+#endif
+
+#ifndef BOOLEAN
+typedef UCHAR BOOLEAN;
+#endif
+/*
+#ifndef bool
+typedef UCHAR bool;
+#endif
+*/
+#ifdef LINUX_KERNEL
+typedef unsigned long XGIIOADDRESS;
+#endif
+
+#ifdef LINUX_XF86
+#if XF86_VERSION_CURRENT < XF86_VERSION_NUMERIC(4,2,0,0,0)
+typedef unsigned char IOADDRESS;
+typedef unsigned char XGIIOADDRESS;
+#else
+typedef IOADDRESS XGIIOADDRESS;
+#endif
+#endif
+
+#ifndef VBIOS_VER_MAX_LENGTH
+#define VBIOS_VER_MAX_LENGTH    4
+#endif
+
+#ifndef WIN2000
+
+#ifndef LINUX_KERNEL   /* For the linux kernel, this is defined in xgifb.h */
+#ifndef XGI_CHIP_TYPE
+typedef enum _XGI_CHIP_TYPE {
+    XGI_VGALegacy = 0,
+#ifdef LINUX_XF86
+    XGI_530,
+    XGI_OLD,
+#endif
+    XGI_300,
+    XGI_630,
+    XGI_640,
+    XGI_315H,
+    XGI_315,
+    XGI_315PRO,
+    XGI_550,
+    XGI_650,
+    XGI_650M,
+    XGI_740,
+    XGI_330,
+    XGI_661,
+    XGI_660,
+    XGI_760,
+    XG40 = 32,
+    XG41,
+    XG42,
+    XG45,
+    XG20 = 48,
+    XG21,
+    XG27,
+    MAX_XGI_CHIP
+} XGI_CHIP_TYPE;
+#endif
+#endif
+
+#ifndef XGI_VB_CHIP_TYPE
+typedef enum _XGI_VB_CHIP_TYPE {
+    VB_CHIP_Legacy = 0,
+    VB_CHIP_301,
+    VB_CHIP_301B,
+    VB_CHIP_301LV,
+    VB_CHIP_302,
+    VB_CHIP_302B,
+    VB_CHIP_302LV,
+    VB_CHIP_301C,
+    VB_CHIP_302ELV,
+    VB_CHIP_UNKNOWN, /* other video bridge or no video bridge */
+    MAX_VB_CHIP
+} XGI_VB_CHIP_TYPE;
+#endif
+
+#ifndef XGI_LCD_TYPE
+typedef enum _XGI_LCD_TYPE {
+    LCD_INVALID = 0,
+    LCD_320x480,       /* FSTN, DSTN */
+    LCD_640x480,
+    LCD_640x480_2,     /* FSTN, DSTN */
+    LCD_640x480_3,     /* FSTN, DSTN */
+    LCD_800x600,
+    LCD_848x480,
+    LCD_1024x600,
+    LCD_1024x768,
+    LCD_1152x768,
+    LCD_1152x864,
+    LCD_1280x720,
+    LCD_1280x768,
+    LCD_1280x800,
+    LCD_1280x960,
+    LCD_1280x1024,
+    LCD_1400x1050,
+    LCD_1600x1200,
+    LCD_1680x1050,
+    LCD_1920x1440,
+    LCD_2048x1536,
+    LCD_CUSTOM,
+    LCD_UNKNOWN
+} XGI_LCD_TYPE;
+#endif
+
+#endif   /* not WIN2000 */
+
+#ifndef PXGI_DSReg
+typedef struct _XGI_DSReg
+{
+  UCHAR  jIdx;
+  UCHAR  jVal;
+} XGI_DSReg, *PXGI_DSReg;
+#endif
+
+#ifndef XGI_HW_DEVICE_INFO
+
+typedef struct _XGI_HW_DEVICE_INFO  XGI_HW_DEVICE_INFO, *PXGI_HW_DEVICE_INFO;
+
+typedef BOOLEAN (*PXGI_QUERYSPACE)   (PXGI_HW_DEVICE_INFO, ULONG, ULONG, ULONG *);
+
+struct _XGI_HW_DEVICE_INFO
+{
+    ULONG  ulExternalChip;       /* NO VB or other video bridge*/
+                                 /* if ujVBChipID = VB_CHIP_UNKNOWN, */
+#ifdef LINUX_XF86
+    PCITAG PciTag;               /* PCI Tag */
+#endif
+
+    PUCHAR  pjVirtualRomBase;    /* ROM image */
+
+    BOOLEAN UseROM;              /* Use the ROM image if provided */
+
+    PVOID   pDevice;
+
+    PUCHAR  pjVideoMemoryAddress;/* base virtual memory address */
+                                 /* of Linear VGA memory */
+
+    ULONG  ulVideoMemorySize;    /* size, in bytes, of the memory on the board */
+
+    PUCHAR pjIOAddress;          /* base I/O address of VGA ports (0x3B0) */
+
+    PUCHAR pjCustomizedROMImage;
+
+    PUCHAR pj2ndVideoMemoryAddress;
+    ULONG  ul2ndVideoMemorySize;
+
+    PUCHAR pj2ndIOAddress;
+/*#ifndef WIN2000
+    XGIIOADDRESS pjIOAddress;   //  base I/O address of VGA ports (0x3B0)
+#endif */
+    UCHAR  jChipType;            /* Used to Identify Graphics Chip */
+                                 /* defined in the data structure type  */
+                                 /* "XGI_CHIP_TYPE" */
+
+    UCHAR  jChipRevision;        /* Used to Identify Graphics Chip Revision */
+
+    UCHAR  ujVBChipID;           /* the ID of video bridge */
+                                 /* defined in the data structure type */
+                                 /* "XGI_VB_CHIP_TYPE" */
+
+    BOOLEAN    bNewScratch;
+
+    ULONG  ulCRT2LCDType;        /* defined in the data structure type */
+
+    ULONG usExternalChip;       /* NO VB or other video bridge (other than  */
+                                 /*  video bridge) */
+
+    BOOLEAN bIntegratedMMEnabled;/* supporting integration MM enable */
+
+    BOOLEAN bSkipDramSizing;     /* True: Skip video memory sizing. */
+
+    BOOLEAN bSkipSense;
+
+    BOOLEAN bIsPowerSaving;     /* True: XGIInit() is invoked by power management,
+                                   otherwise by 2nd adapter's initialzation */
+
+    PXGI_DSReg  pSR;             /* restore SR registers in initial function. */
+                                 /* end data :(idx, val) =  (FF, FF). */
+                                 /* Note : restore SR registers if  */
+                                 /* bSkipDramSizing = TRUE */
+
+    PXGI_DSReg  pCR;             /* restore CR registers in initial function. */
+                                 /* end data :(idx, val) =  (FF, FF) */
+                                 /* Note : restore cR registers if  */
+                                 /* bSkipDramSizing = TRUE */
+/*
+#endif
+*/
+
+    PXGI_QUERYSPACE  pQueryVGAConfigSpace;
+
+    PXGI_QUERYSPACE  pQueryNorthBridgeSpace;
+
+    UCHAR  szVBIOSVer[VBIOS_VER_MAX_LENGTH];
+
+};
+#endif
+
+/* Addtional IOCTL for communication xgifb <> X driver        */
+/* If changing this, xgifb.h must also be changed (for xgifb) */
+
+#ifdef LINUX_XF86  /* We don't want the X driver to depend on the kernel source */
+
+/* ioctl for identifying and giving some info (esp. memory heap start) */
+#define XGIFB_GET_INFO    0x80046ef8  /* Wow, what a terrible hack... */
+
+/* Structure argument for XGIFB_GET_INFO ioctl  */
+typedef struct _XGIFB_INFO xgifb_info, *pxgifb_info;
+
+struct _XGIFB_INFO {
+        CARD32  xgifb_id;               /* for identifying xgifb */
+#ifndef XGIFB_ID
+#define XGIFB_ID          0x53495346    /* Identify myself with 'XGIF' */
+#endif
+        CARD32  chip_id;                /* PCI ID of detected chip */
+        CARD32  memory;                 /* video memory in KB which xgifb manages */
+        CARD32  heapstart;              /* heap start (= xgifb "mem" argument) in KB */
+        CARD8   fbvidmode;              /* current xgifb mode */
+
+        CARD8   xgifb_version;
+        CARD8   xgifb_revision;
+        CARD8   xgifb_patchlevel;
+
+        CARD8   xgifb_caps;             /* xgifb's capabilities */
+
+        CARD32  xgifb_tqlen;            /* turbo queue length (in KB) */
+
+        CARD32  xgifb_pcibus;           /* The card's PCI ID */
+        CARD32  xgifb_pcislot;
+        CARD32  xgifb_pcifunc;
+
+        CARD8   xgifb_lcdpdc;
+
+        CARD8   xgifb_lcda;
+
+        CARD32  xgifb_vbflags;
+        CARD32  xgifb_currentvbflags;
+
+        CARD32  xgifb_scalelcd;
+        CARD32  xgifb_specialtiming;
+
+        CARD8   xgifb_haveemi;
+        CARD8   xgifb_emi30,xgifb_emi31,xgifb_emi32,xgifb_emi33;
+        CARD8   xgifb_haveemilcd;
+
+        CARD8   xgifb_lcdpdca;
+
+        CARD8 reserved[212];            /* for future use */
+};
+#endif
+
+#endif
+
diff --git a/drivers/telephony/ixj.c b/drivers/telephony/ixj.c
index e89304c72568..b53deee25d74 100644
--- a/drivers/telephony/ixj.c
+++ b/drivers/telephony/ixj.c
@@ -5879,20 +5879,13 @@ out:
 static int ixj_build_filter_cadence(IXJ *j, IXJ_FILTER_CADENCE __user * cp)
 {
         IXJ_FILTER_CADENCE *lcp;
-        lcp = kmalloc(sizeof(IXJ_FILTER_CADENCE), GFP_KERNEL);
-        if (lcp == NULL) {
+        lcp = memdup_user(cp, sizeof(IXJ_FILTER_CADENCE));
+        if (IS_ERR(lcp)) {
                 if(ixjdebug & 0x0001) {
-                        printk(KERN_INFO "Could not allocate memory for cadence\n");
+                        printk(KERN_INFO "Could not allocate memory for cadence or could not copy cadence to kernel\n");
                 }
-                return -ENOMEM;
+                return PTR_ERR(lcp);
         }
-        if (copy_from_user(lcp, cp, sizeof(IXJ_FILTER_CADENCE))) {
-                if(ixjdebug & 0x0001) {
-                        printk(KERN_INFO "Could not copy cadence to kernel\n");
-                }
-                kfree(lcp);
-                return -EFAULT;
-        }
         if (lcp->filter > 5) {
                 if(ixjdebug & 0x0001) {
                         printk(KERN_INFO "Cadence out of range\n");
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index 0c2f14ff9696..61d75507d5d0 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -1201,7 +1201,7 @@ made_compressed_probe:
                 if (rcv->urb == NULL) {
                         dev_dbg(&intf->dev,
                                 "out of memory (read urbs usb_alloc_urb)\n");
-                        goto alloc_fail7;
+                        goto alloc_fail6;
                 }
 
                 rcv->urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
@@ -1225,7 +1225,7 @@ made_compressed_probe:
                 if (snd->urb == NULL) {
                         dev_dbg(&intf->dev,
                                 "out of memory (write urbs usb_alloc_urb)");
-                        goto alloc_fail7;
+                        goto alloc_fail8;
                 }
 
                 if (usb_endpoint_xfer_int(epwrite))
@@ -1264,6 +1264,7 @@ made_compressed_probe:
                 i = device_create_file(&intf->dev,
                                                 &dev_attr_iCountryCodeRelDate);
                 if (i < 0) {
+                        device_remove_file(&intf->dev, &dev_attr_wCountryCodes);
                         kfree(acm->country_codes);
                         goto skip_countries;
                 }
@@ -1300,6 +1301,7 @@ alloc_fail8:
                 usb_free_urb(acm->wb[i].urb);
 alloc_fail7:
         acm_read_buffers_free(acm);
+alloc_fail6:
         for (i = 0; i < num_rx_buf; i++)
                 usb_free_urb(acm->ru[i].urb);
         usb_free_urb(acm->ctrlurb);
diff --git a/drivers/usb/core/driver.c b/drivers/usb/core/driver.c
index ded550eda5d9..de98a94d1853 100644
--- a/drivers/usb/core/driver.c
+++ b/drivers/usb/core/driver.c
@@ -1328,6 +1328,7 @@ int usb_resume(struct device *dev, pm_message_t msg)
 
         /* For all other calls, take the device back to full power and
          * tell the PM core in case it was autosuspended previously.
+         * Unbind the interfaces that will need rebinding later.
          */
         } else {
                 status = usb_resume_both(udev, msg);
@@ -1336,6 +1337,7 @@ int usb_resume(struct device *dev, pm_message_t msg)
                         pm_runtime_set_active(dev);
                         pm_runtime_enable(dev);
                         udev->last_busy = jiffies;
+                        do_unbind_rebind(udev, DO_REBIND);
                 }
         }
 
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 649c0c5f7158..591ae9fde199 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -295,6 +295,7 @@ config USB_GADGET_S3C_HSOTG
         boolean "S3C HS/OtG USB Device controller"
         depends on S3C_DEV_USB_HSOTG
         select USB_GADGET_S3C_HSOTG_PIO
+        select USB_GADGET_DUALSPEED
         help
           The Samsung S3C64XX USB2.0 high-speed gadget controller
           integrated into the S3C64XX series SoC.
diff --git a/drivers/usb/gadget/f_audio.c b/drivers/usb/gadget/f_audio.c
index 43bf44514c41..b91115f84b13 100644
--- a/drivers/usb/gadget/f_audio.c
+++ b/drivers/usb/gadget/f_audio.c
@@ -101,7 +101,7 @@ static struct uac_feature_unit_descriptor_0 feature_unit_desc = {
 static struct usb_audio_control mute_control = {
         .list = LIST_HEAD_INIT(mute_control.list),
         .name = "Mute Control",
-        .type = UAC_MUTE_CONTROL,
+        .type = UAC_FU_MUTE,
         /* Todo: add real Mute control code */
         .set = generic_set_cmd,
         .get = generic_get_cmd,
@@ -110,7 +110,7 @@ static struct usb_audio_control mute_control = {
 static struct usb_audio_control volume_control = {
         .list = LIST_HEAD_INIT(volume_control.list),
         .name = "Volume Control",
-        .type = UAC_VOLUME_CONTROL,
+        .type = UAC_FU_VOLUME,
         /* Todo: add real Volume control code */
         .set = generic_set_cmd,
         .get = generic_get_cmd,
diff --git a/drivers/usb/gadget/fsl_qe_udc.c b/drivers/usb/gadget/fsl_qe_udc.c
index 2928523268b5..82506ca297d5 100644
--- a/drivers/usb/gadget/fsl_qe_udc.c
+++ b/drivers/usb/gadget/fsl_qe_udc.c
@@ -2400,7 +2400,7 @@ EXPORT_SYMBOL(usb_gadget_unregister_driver);
 static struct qe_udc __devinit *qe_udc_config(struct of_device *ofdev)
 {
         struct qe_udc *udc;
-        struct device_node *np = ofdev->node;
+        struct device_node *np = ofdev->dev.of_node;
         unsigned int tmp_addr = 0;
         struct usb_device_para __iomem *usbpram;
         unsigned int i;
@@ -2525,7 +2525,7 @@ static void qe_udc_release(struct device *dev)
 static int __devinit qe_udc_probe(struct of_device *ofdev,
                         const struct of_device_id *match)
 {
-        struct device_node *np = ofdev->node;
+        struct device_node *np = ofdev->dev.of_node;
         struct qe_ep *ep;
         unsigned int ret = 0;
         unsigned int i;
diff --git a/drivers/usb/gadget/m66592-udc.h b/drivers/usb/gadget/m66592-udc.h
index 8b960deed680..c3caf1ac73ce 100644
--- a/drivers/usb/gadget/m66592-udc.h
+++ b/drivers/usb/gadget/m66592-udc.h
@@ -537,35 +537,35 @@ struct m66592 {
 /*-------------------------------------------------------------------------*/
 static inline u16 m66592_read(struct m66592 *m66592, unsigned long offset)
 {
-        return inw((unsigned long)m66592->reg + offset);
+        return ioread16(m66592->reg + offset);
 }
 
 static inline void m66592_read_fifo(struct m66592 *m66592,
                 unsigned long offset,
                 void *buf, unsigned long len)
 {
-        unsigned long fifoaddr = (unsigned long)m66592->reg + offset;
+        void __iomem *fifoaddr = m66592->reg + offset;
 
         if (m66592->pdata->on_chip) {
                 len = (len + 3) / 4;
-                insl(fifoaddr, buf, len);
+                ioread32_rep(fifoaddr, buf, len);
         } else {
                 len = (len + 1) / 2;
-                insw(fifoaddr, buf, len);
+                ioread16_rep(fifoaddr, buf, len);
         }
 }
 
 static inline void m66592_write(struct m66592 *m66592, u16 val,
                                 unsigned long offset)
 {
-        outw(val, (unsigned long)m66592->reg + offset);
+        iowrite16(val, m66592->reg + offset);
 }
 
 static inline void m66592_write_fifo(struct m66592 *m66592,
                 unsigned long offset,
                 void *buf, unsigned long len)
 {
-        unsigned long fifoaddr = (unsigned long)m66592->reg + offset;
+        void __iomem *fifoaddr = m66592->reg + offset;
 
         if (m66592->pdata->on_chip) {
                 unsigned long count;
@@ -573,25 +573,25 @@ static inline void m66592_write_fifo(struct m66592 *m66592,
                 int i;
 
                 count = len / 4;
-                outsl(fifoaddr, buf, count);
+                iowrite32_rep(fifoaddr, buf, count);
 
                 if (len & 0x00000003) {
                         pb = buf + count * 4;
                         for (i = 0; i < (len & 0x00000003); i++) {
                                 if (m66592_read(m66592, M66592_CFBCFG)) /* le */
-                                        outb(pb[i], fifoaddr + (3 - i));
+                                        iowrite8(pb[i], fifoaddr + (3 - i));
                                 else
-                                        outb(pb[i], fifoaddr + i);
+                                        iowrite8(pb[i], fifoaddr + i);
                         }
                 }
         } else {
                 unsigned long odd = len & 0x0001;
 
                 len = len / 2;
-                outsw(fifoaddr, buf, len);
+                iowrite16_rep(fifoaddr, buf, len);
                 if (odd) {
                         unsigned char *p = buf + len*2;
-                        outb(*p, fifoaddr);
+                        iowrite8(*p, fifoaddr);
                 }
         }
 }
diff --git a/drivers/usb/gadget/printer.c b/drivers/usb/gadget/printer.c
index 6b8bf8c781c4..43abf55d8c60 100644
--- a/drivers/usb/gadget/printer.c
+++ b/drivers/usb/gadget/printer.c
@@ -794,7 +794,7 @@ printer_write(struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
 }
 
 static int
-printer_fsync(struct file *fd, struct dentry *dentry, int datasync)
+printer_fsync(struct file *fd, int datasync)
 {
         struct printer_dev      *dev = fd->private_data;
         unsigned long           flags;
diff --git a/drivers/usb/gadget/r8a66597-udc.c b/drivers/usb/gadget/r8a66597-udc.c
index 888d8f166c0b..70a817842755 100644
--- a/drivers/usb/gadget/r8a66597-udc.c
+++ b/drivers/usb/gadget/r8a66597-udc.c
@@ -1500,7 +1500,7 @@ static int __exit r8a66597_remove(struct platform_device *pdev)
         struct r8a66597         *r8a66597 = dev_get_drvdata(&pdev->dev);
 
         del_timer_sync(&r8a66597->timer);
-        iounmap((void *)r8a66597->reg);
+        iounmap(r8a66597->reg);
         free_irq(platform_get_irq(pdev, 0), r8a66597);
         r8a66597_free_request(&r8a66597->ep[0].ep, r8a66597->ep0_req);
 #ifdef CONFIG_HAVE_CLK
@@ -1578,7 +1578,7 @@ static int __init r8a66597_probe(struct platform_device *pdev)
         init_timer(&r8a66597->timer);
         r8a66597->timer.function = r8a66597_timer;
         r8a66597->timer.data = (unsigned long)r8a66597;
-        r8a66597->reg = (unsigned long)reg;
+        r8a66597->reg = reg;
 
 #ifdef CONFIG_HAVE_CLK
         if (r8a66597->pdata->on_chip) {
diff --git a/drivers/usb/gadget/r8a66597-udc.h b/drivers/usb/gadget/r8a66597-udc.h
index 9a537aa07968..f763b5190afa 100644
--- a/drivers/usb/gadget/r8a66597-udc.h
+++ b/drivers/usb/gadget/r8a66597-udc.h
@@ -91,7 +91,7 @@ struct r8a66597_ep {
 
 struct r8a66597 {
         spinlock_t              lock;
-        unsigned long           reg;
+        void __iomem            *reg;
 
 #ifdef CONFIG_HAVE_CLK
         struct clk *clk;
@@ -127,7 +127,7 @@ struct r8a66597 {
 
 static inline u16 r8a66597_read(struct r8a66597 *r8a66597, unsigned long offset)
 {
-        return inw(r8a66597->reg + offset);
+        return ioread16(r8a66597->reg + offset);
 }
 
 static inline void r8a66597_read_fifo(struct r8a66597 *r8a66597,
@@ -135,7 +135,7 @@ static inline void r8a66597_read_fifo(struct r8a66597 *r8a66597,
                                       unsigned char *buf,
                                       int len)
 {
-        unsigned long fifoaddr = r8a66597->reg + offset;
+        void __iomem *fifoaddr = r8a66597->reg + offset;
         unsigned int data;
         int i;
 
@@ -144,7 +144,7 @@ static inline void r8a66597_read_fifo(struct r8a66597 *r8a66597,
 
                 /* aligned buf case */
                 if (len >= 4 && !((unsigned long)buf & 0x03)) {
-                        insl(fifoaddr, buf, len / 4);
+                        ioread32_rep(fifoaddr, buf, len / 4);
                         buf += len & ~0x03;
                         len &= 0x03;
                 }
@@ -152,7 +152,7 @@ static inline void r8a66597_read_fifo(struct r8a66597 *r8a66597,
                 /* unaligned buf case */
                 for (i = 0; i < len; i++) {
                         if (!(i & 0x03))
-                                data = inl(fifoaddr);
+                                data = ioread32(fifoaddr);
 
                         buf[i] = (data >> ((i & 0x03) * 8)) & 0xff;
                 }
@@ -161,7 +161,7 @@ static inline void r8a66597_read_fifo(struct r8a66597 *r8a66597,
 
                 /* aligned buf case */
                 if (len >= 2 && !((unsigned long)buf & 0x01)) {
-                        insw(fifoaddr, buf, len / 2);
+                        ioread16_rep(fifoaddr, buf, len / 2);
                         buf += len & ~0x01;
                         len &= 0x01;
                 }
@@ -169,7 +169,7 @@ static inline void r8a66597_read_fifo(struct r8a66597 *r8a66597,
                 /* unaligned buf case */
                 for (i = 0; i < len; i++) {
                         if (!(i & 0x01))
-                                data = inw(fifoaddr);
+                                data = ioread16(fifoaddr);
 
                         buf[i] = (data >> ((i & 0x01) * 8)) & 0xff;
                 }
@@ -179,7 +179,7 @@ static inline void r8a66597_read_fifo(struct r8a66597 *r8a66597,
 static inline void r8a66597_write(struct r8a66597 *r8a66597, u16 val,
                                   unsigned long offset)
 {
-        outw(val, r8a66597->reg + offset);
+        iowrite16(val, r8a66597->reg + offset);
 }
 
 static inline void r8a66597_write_fifo(struct r8a66597 *r8a66597,
@@ -187,21 +187,21 @@ static inline void r8a66597_write_fifo(struct r8a66597 *r8a66597,
                                        unsigned char *buf,
                                        int len)
 {
-        unsigned long fifoaddr = r8a66597->reg + offset;
+        void __iomem *fifoaddr = r8a66597->reg + offset;
         int adj = 0;
         int i;
 
         if (r8a66597->pdata->on_chip) {
                 /* 32-bit access only if buf is 32-bit aligned */
                 if (len >= 4 && !((unsigned long)buf & 0x03)) {
-                        outsl(fifoaddr, buf, len / 4);
+                        iowrite32_rep(fifoaddr, buf, len / 4);
                         buf += len & ~0x03;
                         len &= 0x03;
                 }
         } else {
                 /* 16-bit access only if buf is 16-bit aligned */
                 if (len >= 2 && !((unsigned long)buf & 0x01)) {
-                        outsw(fifoaddr, buf, len / 2);
+                        iowrite16_rep(fifoaddr, buf, len / 2);
                         buf += len & ~0x01;
                         len &= 0x01;
                 }
@@ -216,7 +216,7 @@ static inline void r8a66597_write_fifo(struct r8a66597 *r8a66597,
         }
 
         for (i = 0; i < len; i++)
-                outb(buf[i], fifoaddr + adj - (i & adj));
+                iowrite8(buf[i], fifoaddr + adj - (i & adj));
 }
 
 static inline void r8a66597_mdfy(struct r8a66597 *r8a66597,
diff --git a/drivers/usb/gadget/s3c-hsotg.c b/drivers/usb/gadget/s3c-hsotg.c
index 1f73b485732d..26193eceb323 100644
--- a/drivers/usb/gadget/s3c-hsotg.c
+++ b/drivers/usb/gadget/s3c-hsotg.c
@@ -297,6 +297,12 @@ static void s3c_hsotg_ctrl_epint(struct s3c_hsotg *hsotg,
  */
 static void s3c_hsotg_init_fifo(struct s3c_hsotg *hsotg)
 {
+        unsigned int ep;
+        unsigned int addr;
+        unsigned int size;
+        int timeout;
+        u32 val;
+
         /* the ryu 2.6.24 release ahs
            writel(0x1C0, hsotg->regs + S3C_GRXFSIZ);
            writel(S3C_GNPTXFSIZ_NPTxFStAddr(0x200) |
@@ -310,6 +316,51 @@ static void s3c_hsotg_init_fifo(struct s3c_hsotg *hsotg)
         writel(S3C_GNPTXFSIZ_NPTxFStAddr(2048) |
                S3C_GNPTXFSIZ_NPTxFDep(0x1C0),
                hsotg->regs + S3C_GNPTXFSIZ);
+
+        /* arange all the rest of the TX FIFOs, as some versions of this
+         * block have overlapping default addresses. This also ensures
+         * that if the settings have been changed, then they are set to
+         * known values. */
+
+        /* start at the end of the GNPTXFSIZ, rounded up */
+        addr = 2048 + 1024;
+        size = 768;
+
+        /* currently we allocate TX FIFOs for all possible endpoints,
+         * and assume that they are all the same size. */
+
+        for (ep = 0; ep <= 15; ep++) {
+                val = addr;
+                val |= size << S3C_DPTXFSIZn_DPTxFSize_SHIFT;
+                addr += size;
+
+                writel(val, hsotg->regs + S3C_DPTXFSIZn(ep));
+        }
+
+        /* according to p428 of the design guide, we need to ensure that
+         * all fifos are flushed before continuing */
+
+        writel(S3C_GRSTCTL_TxFNum(0x10) | S3C_GRSTCTL_TxFFlsh |
+               S3C_GRSTCTL_RxFFlsh, hsotg->regs + S3C_GRSTCTL);
+
+        /* wait until the fifos are both flushed */
+        timeout = 100;
+        while (1) {
+                val = readl(hsotg->regs + S3C_GRSTCTL);
+
+                if ((val & (S3C_GRSTCTL_TxFFlsh | S3C_GRSTCTL_RxFFlsh)) == 0)
+                        break;
+
+                if (--timeout == 0) {
+                        dev_err(hsotg->dev,
+                                "%s: timeout flushing fifos (GRSTCTL=%08x)\n",
+                                __func__, val);
+                }
+
+                udelay(1);
+        }
+
+        dev_dbg(hsotg->dev, "FIFOs reset, timeout at %d\n", timeout);
 }
 
 /**
@@ -2574,6 +2625,9 @@ int usb_gadget_register_driver(struct usb_gadget_driver *driver)
         writel(S3C_DCTL_CGOUTNak | S3C_DCTL_CGNPInNAK,
                hsotg->regs + S3C_DCTL);
 
+        /* must be at-least 3ms to allow bus to see disconnect */
+        msleep(3);
+
         /* remove the soft-disconnect and let's go */
         __bic32(hsotg->regs + S3C_DCTL, S3C_DCTL_SftDiscon);
 
@@ -2730,6 +2784,9 @@ static void s3c_hsotg_init(struct s3c_hsotg *hsotg)
 
         writel(0, hsotg->regs + S3C_DAINTMSK);
 
+        /* Be in disconnected state until gadget is registered */
+        __orr32(hsotg->regs + S3C_DCTL, S3C_DCTL_SftDiscon);
+
         if (0) {
                 /* post global nak until we're ready */
                 writel(S3C_DCTL_SGNPInNAK | S3C_DCTL_SGOUTNak,
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index ef3e88f0b3c3..a3ef2a9d9dc2 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -1135,7 +1135,7 @@ MODULE_LICENSE ("GPL");
 
 #ifdef CONFIG_XPS_USB_HCD_XILINX
 #include "ehci-xilinx-of.c"
-#define OF_PLATFORM_DRIVER      ehci_hcd_xilinx_of_driver
+#define XILINX_OF_PLATFORM_DRIVER       ehci_hcd_xilinx_of_driver
 #endif
 
 #ifdef CONFIG_PLAT_ORION
@@ -1159,7 +1159,8 @@ MODULE_LICENSE ("GPL");
 #endif
 
 #if !defined(PCI_DRIVER) && !defined(PLATFORM_DRIVER) && \
-    !defined(PS3_SYSTEM_BUS_DRIVER) && !defined(OF_PLATFORM_DRIVER)
+    !defined(PS3_SYSTEM_BUS_DRIVER) && !defined(OF_PLATFORM_DRIVER) && \
+    !defined(XILINX_OF_PLATFORM_DRIVER)
 #error "missing bus glue for ehci-hcd"
 #endif
 
@@ -1213,10 +1214,20 @@ static int __init ehci_hcd_init(void)
         if (retval < 0)
                 goto clean3;
 #endif
+
+#ifdef XILINX_OF_PLATFORM_DRIVER
+        retval = of_register_platform_driver(&XILINX_OF_PLATFORM_DRIVER);
+        if (retval < 0)
+                goto clean4;
+#endif
         return retval;
 
+#ifdef XILINX_OF_PLATFORM_DRIVER
+        /* of_unregister_platform_driver(&XILINX_OF_PLATFORM_DRIVER); */
+clean4:
+#endif
 #ifdef OF_PLATFORM_DRIVER
-        /* of_unregister_platform_driver(&OF_PLATFORM_DRIVER); */
+        of_unregister_platform_driver(&OF_PLATFORM_DRIVER);
 clean3:
 #endif
 #ifdef PS3_SYSTEM_BUS_DRIVER
@@ -1243,6 +1254,9 @@ module_init(ehci_hcd_init);
 
 static void __exit ehci_hcd_cleanup(void)
 {
+#ifdef XILINX_OF_PLATFORM_DRIVER
+        of_unregister_platform_driver(&XILINX_OF_PLATFORM_DRIVER);
+#endif
 #ifdef OF_PLATFORM_DRIVER
         of_unregister_platform_driver(&OF_PLATFORM_DRIVER);
 #endif
diff --git a/drivers/usb/host/ehci-xilinx-of.c b/drivers/usb/host/ehci-xilinx-of.c
index 013972bbde57..4899f451add9 100644
--- a/drivers/usb/host/ehci-xilinx-of.c
+++ b/drivers/usb/host/ehci-xilinx-of.c
@@ -151,7 +151,7 @@ static const struct hc_driver ehci_xilinx_of_hc_driver = {
 static int __devinit
 ehci_hcd_xilinx_of_probe(struct of_device *op, const struct of_device_id *match)
 {
-        struct device_node *dn = op->node;
+        struct device_node *dn = op->dev.of_node;
         struct usb_hcd *hcd;
         struct ehci_hcd *ehci;
         struct resource res;
diff --git a/drivers/usb/host/isp1362.h b/drivers/usb/host/isp1362.h
index 5151516ea1de..d995351f9bed 100644
--- a/drivers/usb/host/isp1362.h
+++ b/drivers/usb/host/isp1362.h
@@ -65,7 +65,7 @@ static inline void delayed_insw(unsigned int addr, void *buf, int len)
         unsigned short *bp = (unsigned short *)buf;
         while (len--) {
                 DUMMY_DELAY_ACCESS;
-                *bp++ = inw((void *)addr);
+                *bp++ = inw(addr);
         }
 }
 
diff --git a/drivers/usb/host/r8a66597-hcd.c b/drivers/usb/host/r8a66597-hcd.c
index 6db57ab6079d..1a2bb4ce638f 100644
--- a/drivers/usb/host/r8a66597-hcd.c
+++ b/drivers/usb/host/r8a66597-hcd.c
@@ -2404,7 +2404,7 @@ static int __init_or_module r8a66597_remove(struct platform_device *pdev)
 
         del_timer_sync(&r8a66597->rh_timer);
         usb_remove_hcd(hcd);
-        iounmap((void *)r8a66597->reg);
+        iounmap(r8a66597->reg);
 #ifdef CONFIG_HAVE_CLK
         if (r8a66597->pdata->on_chip)
                 clk_put(r8a66597->clk);
@@ -2496,7 +2496,7 @@ static int __devinit r8a66597_probe(struct platform_device *pdev)
         init_timer(&r8a66597->rh_timer);
         r8a66597->rh_timer.function = r8a66597_timer;
         r8a66597->rh_timer.data = (unsigned long)r8a66597;
-        r8a66597->reg = (unsigned long)reg;
+        r8a66597->reg = reg;
 
         /* make sure no interrupts are pending */
         ret = r8a66597_clock_enable(r8a66597);
diff --git a/drivers/usb/host/r8a66597.h b/drivers/usb/host/r8a66597.h
index 228e3fb23854..95d0f5adfdcf 100644
--- a/drivers/usb/host/r8a66597.h
+++ b/drivers/usb/host/r8a66597.h
@@ -112,7 +112,7 @@ struct r8a66597_root_hub {
 
 struct r8a66597 {
         spinlock_t lock;
-        unsigned long reg;
+        void __iomem *reg;
 #ifdef CONFIG_HAVE_CLK
         struct clk *clk;
 #endif
@@ -170,67 +170,67 @@ static inline struct urb *r8a66597_get_urb(struct r8a66597 *r8a66597,
 
 static inline u16 r8a66597_read(struct r8a66597 *r8a66597, unsigned long offset)
 {
-        return inw(r8a66597->reg + offset);
+        return ioread16(r8a66597->reg + offset);
 }
 
 static inline void r8a66597_read_fifo(struct r8a66597 *r8a66597,
                                       unsigned long offset, u16 *buf,
                                       int len)
 {
-        unsigned long fifoaddr = r8a66597->reg + offset;
+        void __iomem *fifoaddr = r8a66597->reg + offset;
         unsigned long count;
 
         if (r8a66597->pdata->on_chip) {
                 count = len / 4;
-                insl(fifoaddr, buf, count);
+                ioread32_rep(fifoaddr, buf, count);
 
                 if (len & 0x00000003) {
-                        unsigned long tmp = inl(fifoaddr);
+                        unsigned long tmp = ioread32(fifoaddr);
                         memcpy((unsigned char *)buf + count * 4, &tmp,
                                len & 0x03);
                 }
         } else {
                 len = (len + 1) / 2;
-                insw(fifoaddr, buf, len);
+                ioread16_rep(fifoaddr, buf, len);
         }
 }
 
 static inline void r8a66597_write(struct r8a66597 *r8a66597, u16 val,
                                   unsigned long offset)
 {
-        outw(val, r8a66597->reg + offset);
+        iowrite16(val, r8a66597->reg + offset);
 }
 
 static inline void r8a66597_write_fifo(struct r8a66597 *r8a66597,
                                        unsigned long offset, u16 *buf,
                                        int len)
 {
-        unsigned long fifoaddr = r8a66597->reg + offset;
+        void __iomem *fifoaddr = r8a66597->reg + offset;
         unsigned long count;
         unsigned char *pb;
         int i;
 
         if (r8a66597->pdata->on_chip) {
                 count = len / 4;
-                outsl(fifoaddr, buf, count);
+                iowrite32_rep(fifoaddr, buf, count);
 
                 if (len & 0x00000003) {
                         pb = (unsigned char *)buf + count * 4;
                         for (i = 0; i < (len & 0x00000003); i++) {
                                 if (r8a66597_read(r8a66597, CFIFOSEL) & BIGEND)
-                                        outb(pb[i], fifoaddr + i);
+                                        iowrite8(pb[i], fifoaddr + i);
                                 else
-                                        outb(pb[i], fifoaddr + 3 - i);
+                                        iowrite8(pb[i], fifoaddr + 3 - i);
                         }
                 }
         } else {
                 int odd = len & 0x0001;
 
                 len = len / 2;
-                outsw(fifoaddr, buf, len);
+                ioread16_rep(fifoaddr, buf, len);
                 if (unlikely(odd)) {
                         buf = &buf[len];
-                        outb((unsigned char)*buf, fifoaddr);
+                        iowrite8((unsigned char)*buf, fifoaddr);
                 }
         }
 }
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index edffd81fc253..11482b6b9381 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -78,6 +78,8 @@ static int xhci_pci_setup(struct usb_hcd *hcd)
                         xhci_dbg(xhci, "QUIRK: Fresco Logic xHC needs configure"
                                         " endpoint cmd after reset endpoint\n");
         }
+        if (pdev->vendor == PCI_VENDOR_ID_NEC)
+                xhci->quirks |= XHCI_NEC_HOST;
 
         /* Make sure the HC is halted. */
         retval = xhci_halt(xhci);
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 36c858e5b529..9012098add6b 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -1071,6 +1071,15 @@ bandwidth_change:
                         xhci_warn(xhci, "Reset device command completion "
                                         "for disabled slot %u\n", slot_id);
                 break;
+        case TRB_TYPE(TRB_NEC_GET_FW):
+                if (!(xhci->quirks & XHCI_NEC_HOST)) {
+                        xhci->error_bitmask |= 1 << 6;
+                        break;
+                }
+                xhci_dbg(xhci, "NEC firmware version %2x.%02x\n",
+                                NEC_FW_MAJOR(event->status),
+                                NEC_FW_MINOR(event->status));
+                break;
         default:
                 /* Skip over unknown commands on the event ring */
                 xhci->error_bitmask |= 1 << 6;
@@ -1079,6 +1088,17 @@ bandwidth_change:
         inc_deq(xhci, xhci->cmd_ring, false);
 }
 
+static void handle_vendor_event(struct xhci_hcd *xhci,
+                union xhci_trb *event)
+{
+        u32 trb_type;
+
+        trb_type = TRB_FIELD_TO_TYPE(event->generic.field[3]);
+        xhci_dbg(xhci, "Vendor specific event TRB type = %u\n", trb_type);
+        if (trb_type == TRB_NEC_CMD_COMP && (xhci->quirks & XHCI_NEC_HOST))
+                handle_cmd_completion(xhci, &event->event_cmd);
+}
+
 static void handle_port_status(struct xhci_hcd *xhci,
                 union xhci_trb *event)
 {
@@ -1659,7 +1679,10 @@ void xhci_handle_event(struct xhci_hcd *xhci)
                         update_ptrs = 0;
                 break;
         default:
-                xhci->error_bitmask |= 1 << 3;
+                if ((event->event_cmd.flags & TRB_TYPE_BITMASK) >= TRB_TYPE(48))
+                        handle_vendor_event(xhci, event);
+                else
+                        xhci->error_bitmask |= 1 << 3;
         }
         /* Any of the above functions may drop and re-acquire the lock, so check
          * to make sure a watchdog timer didn't mark the host as non-responsive.
@@ -2378,6 +2401,12 @@ int xhci_queue_address_device(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
                         false);
 }
 
+int xhci_queue_vendor_command(struct xhci_hcd *xhci,
+                u32 field1, u32 field2, u32 field3, u32 field4)
+{
+        return queue_command(xhci, field1, field2, field3, field4, false);
+}
+
 /* Queue a reset device command TRB */
 int xhci_queue_reset_device(struct xhci_hcd *xhci, u32 slot_id)
 {
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index 40e0a0c221b8..27345cd04da0 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -106,6 +106,33 @@ int xhci_halt(struct xhci_hcd *xhci)
 }
 
 /*
+ * Set the run bit and wait for the host to be running.
+ */
+int xhci_start(struct xhci_hcd *xhci)
+{
+        u32 temp;
+        int ret;
+
+        temp = xhci_readl(xhci, &xhci->op_regs->command);
+        temp |= (CMD_RUN);
+        xhci_dbg(xhci, "// Turn on HC, cmd = 0x%x.\n",
+                        temp);
+        xhci_writel(xhci, temp, &xhci->op_regs->command);
+
+        /*
+         * Wait for the HCHalted Status bit to be 0 to indicate the host is
+         * running.
+         */
+        ret = handshake(xhci, &xhci->op_regs->status,
+                        STS_HALT, 0, XHCI_MAX_HALT_USEC);
+        if (ret == -ETIMEDOUT)
+                xhci_err(xhci, "Host took too long to start, "
+                                "waited %u microseconds.\n",
+                                XHCI_MAX_HALT_USEC);
+        return ret;
+}
+
+/*
  * Reset a halted HC, and set the internal HC state to HC_STATE_HALT.
  *
  * This resets pipelines, timers, counters, state machines, etc.
@@ -116,6 +143,7 @@ int xhci_reset(struct xhci_hcd *xhci)
 {
         u32 command;
         u32 state;
+        int ret;
 
         state = xhci_readl(xhci, &xhci->op_regs->status);
         if ((state & STS_HALT) == 0) {
@@ -130,7 +158,17 @@ int xhci_reset(struct xhci_hcd *xhci)
         /* XXX: Why does EHCI set this here?  Shouldn't other code do this? */
         xhci_to_hcd(xhci)->state = HC_STATE_HALT;
 
-        return handshake(xhci, &xhci->op_regs->command, CMD_RESET, 0, 250 * 1000);
+        ret = handshake(xhci, &xhci->op_regs->command,
+                        CMD_RESET, 0, 250 * 1000);
+        if (ret)
+                return ret;
+
+        xhci_dbg(xhci, "Wait for controller to be ready for doorbell rings\n");
+        /*
+         * xHCI cannot write to any doorbells or operational registers other
+         * than status until the "Controller Not Ready" flag is cleared.
+         */
+        return handshake(xhci, &xhci->op_regs->status, STS_CNR, 0, 250 * 1000);
 }
 
 
@@ -448,17 +486,20 @@ int xhci_run(struct usb_hcd *hcd)
 
         if (NUM_TEST_NOOPS > 0)
                 doorbell = xhci_setup_one_noop(xhci);
+        if (xhci->quirks & XHCI_NEC_HOST)
+                xhci_queue_vendor_command(xhci, 0, 0, 0,
+                                TRB_TYPE(TRB_NEC_GET_FW));
+
+        if (xhci_start(xhci)) {
+                xhci_halt(xhci);
+                return -ENODEV;
+        }
 
-        temp = xhci_readl(xhci, &xhci->op_regs->command);
-        temp |= (CMD_RUN);
-        xhci_dbg(xhci, "// Turn on HC, cmd = 0x%x.\n",
-                        temp);
-        xhci_writel(xhci, temp, &xhci->op_regs->command);
-        /* Flush PCI posted writes */
-        temp = xhci_readl(xhci, &xhci->op_regs->command);
         xhci_dbg(xhci, "// @%p = 0x%x\n", &xhci->op_regs->command, temp);
         if (doorbell)
                 (*doorbell)(xhci);
+        if (xhci->quirks & XHCI_NEC_HOST)
+                xhci_ring_cmd_db(xhci);
 
         xhci_dbg(xhci, "Finished xhci_run\n");
         return 0;
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index dada2fb59261..8b4b7d39f79c 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -925,6 +925,7 @@ union xhci_trb {
 /* TRB bit mask */
 #define TRB_TYPE_BITMASK        (0xfc00)
 #define TRB_TYPE(p)             ((p) << 10)
+#define TRB_FIELD_TO_TYPE(p)    (((p) & TRB_TYPE_BITMASK) >> 10)
 /* TRB type IDs */
 /* bulk, interrupt, isoc scatter/gather, and control data stage */
 #define TRB_NORMAL              1
@@ -992,6 +993,14 @@ union xhci_trb {
 #define TRB_MFINDEX_WRAP        39
 /* TRB IDs 40-47 reserved, 48-63 is vendor-defined */
 
+/* Nec vendor-specific command completion event. */
+#define TRB_NEC_CMD_COMP        48
+/* Get NEC firmware revision. */
+#define TRB_NEC_GET_FW          49
+
+#define NEC_FW_MINOR(p)         (((p) >> 0) & 0xff)
+#define NEC_FW_MAJOR(p)         (((p) >> 8) & 0xff)
+
 /*
  * TRBS_PER_SEGMENT must be a multiple of 4,
  * since the command ring is 64-byte aligned.
@@ -1172,6 +1181,7 @@ struct xhci_hcd {
         unsigned int            quirks;
 #define XHCI_LINK_TRB_QUIRK     (1 << 0)
 #define XHCI_RESET_EP_QUIRK     (1 << 1)
+#define XHCI_NEC_HOST           (1 << 2)
 };
 
 /* For testing purposes */
@@ -1379,6 +1389,8 @@ void xhci_set_hc_event_deq(struct xhci_hcd *xhci);
 int xhci_queue_slot_control(struct xhci_hcd *xhci, u32 trb_type, u32 slot_id);
 int xhci_queue_address_device(struct xhci_hcd *xhci, dma_addr_t in_ctx_ptr,
                 u32 slot_id);
+int xhci_queue_vendor_command(struct xhci_hcd *xhci,
+                u32 field1, u32 field2, u32 field3, u32 field4);
 int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, int slot_id,
                 unsigned int ep_index);
 int xhci_queue_ctrl_tx(struct xhci_hcd *xhci, gfp_t mem_flags, struct urb *urb,
diff --git a/drivers/usb/serial/digi_acceleport.c b/drivers/usb/serial/digi_acceleport.c
index 3edda3ed822a..fd35f73b5721 100644
--- a/drivers/usb/serial/digi_acceleport.c
+++ b/drivers/usb/serial/digi_acceleport.c
@@ -1239,8 +1239,7 @@ static void digi_write_bulk_callback(struct urb *urb)
 
         /* port and serial sanity check */
         if (port == NULL || (priv = usb_get_serial_port_data(port)) == NULL) {
-                dev_err(&port->dev,
-                        "%s: port or port->private is NULL, status=%d\n",
+                pr_err("%s: port or port->private is NULL, status=%d\n",
                         __func__, status);
                 return;
         }
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index 050211afc07e..79dd1ae195e5 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -2005,6 +2005,8 @@ static void ftdi_set_termios(struct tty_struct *tty,
                                 "urb failed to set to rts/cts flow control\n");
                 }
 
+                /* raise DTR/RTS */
+                set_mctrl(port, TIOCM_DTR | TIOCM_RTS);
         } else {
                 /*
                  * Xon/Xoff code
@@ -2052,6 +2054,8 @@ static void ftdi_set_termios(struct tty_struct *tty,
                         }
                 }
 
+                /* lower DTR/RTS */
+                clear_mctrl(port, TIOCM_DTR | TIOCM_RTS);
         }
         return;
 }
diff --git a/drivers/usb/serial/mos7840.c b/drivers/usb/serial/mos7840.c
index f8424d1bfc1b..585b7e663740 100644
--- a/drivers/usb/serial/mos7840.c
+++ b/drivers/usb/serial/mos7840.c
@@ -730,7 +730,6 @@ static void mos7840_bulk_in_callback(struct urb *urb)
         mos7840_port = urb->context;
         if (!mos7840_port) {
                 dbg("%s", "NULL mos7840_port pointer");
-                mos7840_port->read_urb_busy = false;
                 return;
         }
 
diff --git a/drivers/vhost/net.c b/drivers/vhost/net.c
index aa88911c9504..df5b6b971f26 100644
--- a/drivers/vhost/net.c
+++ b/drivers/vhost/net.c
@@ -593,17 +593,17 @@ static long vhost_net_ioctl(struct file *f, unsigned int ioctl,
         int r;
         switch (ioctl) {
         case VHOST_NET_SET_BACKEND:
-                r = copy_from_user(&backend, argp, sizeof backend);
-                if (r < 0)
-                        return r;
+                if (copy_from_user(&backend, argp, sizeof backend))
+                        return -EFAULT;
                 return vhost_net_set_backend(n, backend.index, backend.fd);
         case VHOST_GET_FEATURES:
                 features = VHOST_FEATURES;
-                return copy_to_user(featurep, &features, sizeof features);
+                if (copy_to_user(featurep, &features, sizeof features))
+                        return -EFAULT;
+                return 0;
         case VHOST_SET_FEATURES:
-                r = copy_from_user(&features, featurep, sizeof features);
-                if (r < 0)
-                        return r;
+                if (copy_from_user(&features, featurep, sizeof features))
+                        return -EFAULT;
                 if (features & ~VHOST_FEATURES)
                         return -EOPNOTSUPP;
                 return vhost_net_set_features(n, features);
@@ -637,7 +637,7 @@ const static struct file_operations vhost_net_fops = {
 };
 
 static struct miscdevice vhost_net_misc = {
-        VHOST_NET_MINOR,
+        MISC_DYNAMIC_MINOR,
         "vhost-net",
         &vhost_net_fops,
 };
diff --git a/drivers/vhost/vhost.c b/drivers/vhost/vhost.c
index c6fb8e968f21..3b83382e06eb 100644
--- a/drivers/vhost/vhost.c
+++ b/drivers/vhost/vhost.c
@@ -320,10 +320,8 @@ static long vhost_set_memory(struct vhost_dev *d, struct vhost_memory __user *m)
 {
         struct vhost_memory mem, *newmem, *oldmem;
         unsigned long size = offsetof(struct vhost_memory, regions);
-        long r;
-        r = copy_from_user(&mem, m, size);
-        if (r)
-                return r;
+        if (copy_from_user(&mem, m, size))
+                return -EFAULT;
         if (mem.padding)
                 return -EOPNOTSUPP;
         if (mem.nregions > VHOST_MEMORY_MAX_NREGIONS)
@@ -333,15 +331,16 @@ static long vhost_set_memory(struct vhost_dev *d, struct vhost_memory __user *m)
                 return -ENOMEM;
 
         memcpy(newmem, &mem, size);
-        r = copy_from_user(newmem->regions, m->regions,
-                           mem.nregions * sizeof *m->regions);
-        if (r) {
+        if (copy_from_user(newmem->regions, m->regions,
+                           mem.nregions * sizeof *m->regions)) {
                 kfree(newmem);
-                return r;
+                return -EFAULT;
         }
 
-        if (!memory_access_ok(d, newmem, vhost_has_feature(d, VHOST_F_LOG_ALL)))
+        if (!memory_access_ok(d, newmem, vhost_has_feature(d, VHOST_F_LOG_ALL))) {
+                kfree(newmem);
                 return -EFAULT;
+        }
         oldmem = d->memory;
         rcu_assign_pointer(d->memory, newmem);
         synchronize_rcu();
@@ -374,7 +373,7 @@ static long vhost_set_vring(struct vhost_dev *d, int ioctl, void __user *argp)
         r = get_user(idx, idxp);
         if (r < 0)
                 return r;
-        if (idx > d->nvqs)
+        if (idx >= d->nvqs)
                 return -ENOBUFS;
 
         vq = d->vqs + idx;
@@ -389,9 +388,10 @@ static long vhost_set_vring(struct vhost_dev *d, int ioctl, void __user *argp)
                         r = -EBUSY;
                         break;
                 }
-                r = copy_from_user(&s, argp, sizeof s);
-                if (r < 0)
+                if (copy_from_user(&s, argp, sizeof s)) {
+                        r = -EFAULT;
                         break;
+                }
                 if (!s.num || s.num > 0xffff || (s.num & (s.num - 1))) {
                         r = -EINVAL;
                         break;
@@ -405,9 +405,10 @@ static long vhost_set_vring(struct vhost_dev *d, int ioctl, void __user *argp)
                         r = -EBUSY;
                         break;
                 }
-                r = copy_from_user(&s, argp, sizeof s);
-                if (r < 0)
+                if (copy_from_user(&s, argp, sizeof s)) {
+                        r = -EFAULT;
                         break;
+                }
                 if (s.num > 0xffff) {
                         r = -EINVAL;
                         break;
@@ -419,12 +420,14 @@ static long vhost_set_vring(struct vhost_dev *d, int ioctl, void __user *argp)
         case VHOST_GET_VRING_BASE:
                 s.index = idx;
                 s.num = vq->last_avail_idx;
-                r = copy_to_user(argp, &s, sizeof s);
+                if (copy_to_user(argp, &s, sizeof s))
+                        r = -EFAULT;
                 break;
         case VHOST_SET_VRING_ADDR:
-                r = copy_from_user(&a, argp, sizeof a);
-                if (r < 0)
+                if (copy_from_user(&a, argp, sizeof a)) {
+                        r = -EFAULT;
                         break;
+                }
                 if (a.flags & ~(0x1 << VHOST_VRING_F_LOG)) {
                         r = -EOPNOTSUPP;
                         break;
@@ -477,9 +480,10 @@ static long vhost_set_vring(struct vhost_dev *d, int ioctl, void __user *argp)
                 vq->used = (void __user *)(unsigned long)a.used_user_addr;
                 break;
         case VHOST_SET_VRING_KICK:
-                r = copy_from_user(&f, argp, sizeof f);
-                if (r < 0)
+                if (copy_from_user(&f, argp, sizeof f)) {
+                        r = -EFAULT;
                         break;
+                }
                 eventfp = f.fd == -1 ? NULL : eventfd_fget(f.fd);
                 if (IS_ERR(eventfp)) {
                         r = PTR_ERR(eventfp);
@@ -492,9 +496,10 @@ static long vhost_set_vring(struct vhost_dev *d, int ioctl, void __user *argp)
                         filep = eventfp;
                 break;
         case VHOST_SET_VRING_CALL:
-                r = copy_from_user(&f, argp, sizeof f);
-                if (r < 0)
+                if (copy_from_user(&f, argp, sizeof f)) {
+                        r = -EFAULT;
                         break;
+                }
                 eventfp = f.fd == -1 ? NULL : eventfd_fget(f.fd);
                 if (IS_ERR(eventfp)) {
                         r = PTR_ERR(eventfp);
@@ -510,9 +515,10 @@ static long vhost_set_vring(struct vhost_dev *d, int ioctl, void __user *argp)
                         filep = eventfp;
                 break;
         case VHOST_SET_VRING_ERR:
-                r = copy_from_user(&f, argp, sizeof f);
-                if (r < 0)
+                if (copy_from_user(&f, argp, sizeof f)) {
+                        r = -EFAULT;
                         break;
+                }
                 eventfp = f.fd == -1 ? NULL : eventfd_fget(f.fd);
                 if (IS_ERR(eventfp)) {
                         r = PTR_ERR(eventfp);
@@ -575,9 +581,10 @@ long vhost_dev_ioctl(struct vhost_dev *d, unsigned int ioctl, unsigned long arg)
                 r = vhost_set_memory(d, argp);
                 break;
         case VHOST_SET_LOG_BASE:
-                r = copy_from_user(&p, argp, sizeof p);
-                if (r < 0)
+                if (copy_from_user(&p, argp, sizeof p)) {
+                        r = -EFAULT;
                         break;
+                }
                 if ((u64)(unsigned long)p != p) {
                         r = -EFAULT;
                         break;
diff --git a/drivers/video/Kconfig b/drivers/video/Kconfig
index 1e6fec487973..3d94a1471724 100644
--- a/drivers/video/Kconfig
+++ b/drivers/video/Kconfig
@@ -8,6 +8,9 @@ menu "Graphics support"
 config HAVE_FB_ATMEL
         bool
 
+config HAVE_FB_IMX
+        bool
+
 source "drivers/char/agp/Kconfig"
 
 source "drivers/gpu/vga/Kconfig"
@@ -400,9 +403,6 @@ config FB_SA1100
           If you plan to use the LCD display with your SA-1100 system, say
           Y here.
 
-config HAVE_FB_IMX
-        bool
-
 config FB_IMX
         tristate "Motorola i.MX LCD support"
         depends on FB && (HAVE_FB_IMX || ARCH_MX1 || ARCH_MX2)
diff --git a/drivers/video/aty/mach64_accel.c b/drivers/video/aty/mach64_accel.c
index 51fcc0a2c94a..e45833ce975b 100644
--- a/drivers/video/aty/mach64_accel.c
+++ b/drivers/video/aty/mach64_accel.c
@@ -242,7 +242,7 @@ void atyfb_copyarea(struct fb_info *info, const struct fb_copyarea *area)
 void atyfb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 {
         struct atyfb_par *par = (struct atyfb_par *) info->par;
-        u32 color = rect->color, dx = rect->dx, width = rect->width, rotation = 0;
+        u32 color, dx = rect->dx, width = rect->width, rotation = 0;
 
         if (par->asleep)
                 return;
@@ -253,8 +253,11 @@ void atyfb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
                 return;
         }
 
-        color |= (rect->color << 8);
-        color |= (rect->color << 16);
+        if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
+            info->fix.visual == FB_VISUAL_DIRECTCOLOR)
+                color = ((u32 *)(info->pseudo_palette))[rect->color];
+        else
+                color = rect->color;
 
         if (info->var.bits_per_pixel == 24) {
                 /* In 24 bpp, the engine is in 8 bpp - this requires that all */
diff --git a/drivers/video/backlight/88pm860x_bl.c b/drivers/video/backlight/88pm860x_bl.c
index 68d2518fadaa..38ffc3fbcbe4 100644
--- a/drivers/video/backlight/88pm860x_bl.c
+++ b/drivers/video/backlight/88pm860x_bl.c
@@ -222,6 +222,7 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
         data->port = __check_device(pdata, name);
         if (data->port < 0) {
                 dev_err(&pdev->dev, "wrong platform data is assigned");
+                kfree(data);
                 return -EINVAL;
         }
 
@@ -266,6 +267,7 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
         backlight_update_status(bl);
         return 0;
 out:
+        backlight_device_unregister(bl);
         kfree(data);
         return ret;
 }
diff --git a/drivers/video/backlight/Kconfig b/drivers/video/backlight/Kconfig
index c025c84601b0..e54a337227ea 100644
--- a/drivers/video/backlight/Kconfig
+++ b/drivers/video/backlight/Kconfig
@@ -8,12 +8,13 @@ menuconfig BACKLIGHT_LCD_SUPPORT
           Enable this to be able to choose the drivers for controlling the
           backlight and the LCD panel on some platforms, for example on PDAs.
 
+if BACKLIGHT_LCD_SUPPORT
+
 #
 # LCD
 #
 config LCD_CLASS_DEVICE
         tristate "Lowlevel LCD controls"
-        depends on BACKLIGHT_LCD_SUPPORT
         default m
         help
           This framework adds support for low-level control of LCD.
@@ -24,31 +25,32 @@ config LCD_CLASS_DEVICE
           To have support for your specific LCD panel you will have to
           select the proper drivers which depend on this option.
 
+if LCD_CLASS_DEVICE
+
 config LCD_CORGI
         tristate "LCD Panel support for SHARP corgi/spitz model"
-        depends on LCD_CLASS_DEVICE && SPI_MASTER && PXA_SHARPSL
+        depends on SPI_MASTER && PXA_SHARPSL
         help
           Say y here to support the LCD panels usually found on SHARP
           corgi (C7x0) and spitz (Cxx00) models.
 
 config LCD_L4F00242T03
         tristate "Epson L4F00242T03 LCD"
-        depends on LCD_CLASS_DEVICE && SPI_MASTER && GENERIC_GPIO
+        depends on SPI_MASTER && GENERIC_GPIO
         help
           SPI driver for Epson L4F00242T03. This provides basic support
           for init and powering the LCD up/down through a sysfs interface.
 
 config LCD_LMS283GF05
         tristate "Samsung LMS283GF05 LCD"
-        depends on LCD_CLASS_DEVICE && SPI_MASTER && GENERIC_GPIO
+        depends on SPI_MASTER && GENERIC_GPIO
         help
           SPI driver for Samsung LMS283GF05. This provides basic support
           for powering the LCD up/down through a sysfs interface.
 
 config LCD_LTV350QV
         tristate "Samsung LTV350QV LCD Panel"
-        depends on LCD_CLASS_DEVICE && SPI_MASTER
-        default n
+        depends on SPI_MASTER
         help
           If you have a Samsung LTV350QV LCD panel, say y to include a
           power control driver for it.  The panel starts up in power
@@ -59,60 +61,61 @@ config LCD_LTV350QV
 
 config LCD_ILI9320
         tristate
-        depends on LCD_CLASS_DEVICE && BACKLIGHT_LCD_SUPPORT
-        default n
         help
           If you have a panel based on the ILI9320 controller chip
           then say y to include a power driver for it.
 
 config LCD_TDO24M
         tristate "Toppoly TDO24M  and TDO35S LCD Panels support"
-        depends on LCD_CLASS_DEVICE && SPI_MASTER
-        default n
+        depends on SPI_MASTER
         help
           If you have a Toppoly TDO24M/TDO35S series LCD panel, say y here to
           include the support for it.
 
 config LCD_VGG2432A4
         tristate "VGG2432A4 LCM device support"
-        depends on BACKLIGHT_LCD_SUPPORT && LCD_CLASS_DEVICE && SPI_MASTER
+        depends on SPI_MASTER
         select LCD_ILI9320
-        default n
         help
           If you have a VGG2432A4 panel based on the ILI9320 controller chip
           then say y to include a power driver for it.
 
 config LCD_PLATFORM
         tristate "Platform LCD controls"
-        depends on LCD_CLASS_DEVICE
         help
           This driver provides a platform-device registered LCD power
           control interface.
 
 config LCD_TOSA
         tristate "Sharp SL-6000 LCD Driver"
-        depends on LCD_CLASS_DEVICE && SPI
-        depends on MACH_TOSA
-        default n
+        depends on SPI && MACH_TOSA
         help
           If you have an Sharp SL-6000 Zaurus say Y to enable a driver
           for its LCD.
 
 config LCD_HP700
         tristate "HP Jornada 700 series LCD Driver"
-        depends on LCD_CLASS_DEVICE
         depends on SA1100_JORNADA720_SSP && !PREEMPT
         default y
         help
           If you have an HP Jornada 700 series handheld (710/720/728)
           say Y to enable LCD control driver.
 
+config LCD_S6E63M0
+        tristate "S6E63M0 AMOLED LCD Driver"
+        depends on SPI && BACKLIGHT_CLASS_DEVICE
+        default n
+        help
+          If you have an S6E63M0 LCD Panel, say Y to enable its
+          LCD control driver.
+
+endif # LCD_CLASS_DEVICE
+
 #
 # Backlight
 #
 config BACKLIGHT_CLASS_DEVICE
         tristate "Lowlevel Backlight controls"
-        depends on BACKLIGHT_LCD_SUPPORT
         default m
         help
           This framework adds support for low-level control of the LCD
@@ -121,9 +124,11 @@ config BACKLIGHT_CLASS_DEVICE
           To have support for your specific LCD panel you will have to
           select the proper drivers which depend on this option.
 
+if BACKLIGHT_CLASS_DEVICE
+
 config BACKLIGHT_ATMEL_LCDC
         bool "Atmel LCDC Contrast-as-Backlight control"
-        depends on BACKLIGHT_CLASS_DEVICE && FB_ATMEL
+        depends on FB_ATMEL
         default y if MACH_SAM9261EK || MACH_SAM9G10EK || MACH_SAM9263EK
         help
           This provides a backlight control internal to the Atmel LCDC
@@ -136,8 +141,7 @@ config BACKLIGHT_ATMEL_LCDC
 
 config BACKLIGHT_ATMEL_PWM
         tristate "Atmel PWM backlight control"
-        depends on BACKLIGHT_CLASS_DEVICE && ATMEL_PWM
-        default n
+        depends on ATMEL_PWM
         help
           Say Y here if you want to use the PWM peripheral in Atmel AT91 and
           AVR32 devices. This driver will need additional platform data to know
@@ -146,9 +150,18 @@ config BACKLIGHT_ATMEL_PWM
           To compile this driver as a module, choose M here: the module will be
           called atmel-pwm-bl.
 
+config BACKLIGHT_EP93XX
+        tristate "Cirrus EP93xx Backlight Driver"
+        depends on FB_EP93XX
+        help
+          If you have a LCD backlight connected to the BRIGHT output of
+          the EP93xx, say Y here to enable this driver.
+
+          To compile this driver as a module, choose M here: the module will
+          be called ep93xx_bl.
+
 config BACKLIGHT_GENERIC
         tristate "Generic (aka Sharp Corgi) Backlight Driver"
-        depends on BACKLIGHT_CLASS_DEVICE
         default y
         help
           Say y to enable the generic platform backlight driver previously
@@ -157,7 +170,7 @@ config BACKLIGHT_GENERIC
 
 config BACKLIGHT_LOCOMO
         tristate "Sharp LOCOMO LCD/Backlight Driver"
-        depends on BACKLIGHT_CLASS_DEVICE && SHARP_LOCOMO
+        depends on SHARP_LOCOMO
         default y
         help
           If you have a Sharp Zaurus SL-5500 (Collie) or SL-5600 (Poodle) say y to
@@ -165,7 +178,7 @@ config BACKLIGHT_LOCOMO
 
 config BACKLIGHT_OMAP1
         tristate "OMAP1 PWL-based LCD Backlight"
-        depends on BACKLIGHT_CLASS_DEVICE && ARCH_OMAP1
+        depends on ARCH_OMAP1
         default y
         help
           This driver controls the LCD backlight level and power for
@@ -174,7 +187,7 @@ config BACKLIGHT_OMAP1
 
 config BACKLIGHT_HP680
         tristate "HP Jornada 680 Backlight Driver"
-        depends on BACKLIGHT_CLASS_DEVICE && SH_HP6XX
+        depends on SH_HP6XX
         default y
         help
           If you have a HP Jornada 680, say y to enable the
@@ -182,7 +195,6 @@ config BACKLIGHT_HP680
 
 config BACKLIGHT_HP700
         tristate "HP Jornada 700 series Backlight Driver"
-        depends on BACKLIGHT_CLASS_DEVICE
         depends on SA1100_JORNADA720_SSP && !PREEMPT
         default y
         help
@@ -191,76 +203,70 @@ config BACKLIGHT_HP700
 
 config BACKLIGHT_PROGEAR
         tristate "Frontpath ProGear Backlight Driver"
-        depends on BACKLIGHT_CLASS_DEVICE && PCI && X86
-        default n
+        depends on PCI && X86
         help
           If you have a Frontpath ProGear say Y to enable the
           backlight driver.
 
 config BACKLIGHT_CARILLO_RANCH
         tristate "Intel Carillo Ranch Backlight Driver"
-        depends on BACKLIGHT_CLASS_DEVICE && LCD_CLASS_DEVICE && PCI && X86 && FB_LE80578
-        default n
+        depends on LCD_CLASS_DEVICE && PCI && X86 && FB_LE80578
         help
           If you have a Intel LE80578 (Carillo Ranch) say Y to enable the
           backlight driver.
 
 config BACKLIGHT_PWM
         tristate "Generic PWM based Backlight Driver"
-        depends on BACKLIGHT_CLASS_DEVICE && HAVE_PWM
+        depends on HAVE_PWM
         help
           If you have a LCD backlight adjustable by PWM, say Y to enable
           this driver.
 
 config BACKLIGHT_DA903X
         tristate "Backlight Driver for DA9030/DA9034 using WLED"
-        depends on BACKLIGHT_CLASS_DEVICE && PMIC_DA903X
+        depends on PMIC_DA903X
         help
           If you have a LCD backlight connected to the WLED output of DA9030
           or DA9034 WLED output, say Y here to enable this driver.
 
 config BACKLIGHT_MAX8925
         tristate "Backlight driver for MAX8925"
-        depends on BACKLIGHT_CLASS_DEVICE && MFD_MAX8925
+        depends on MFD_MAX8925
         help
           If you have a LCD backlight connected to the WLED output of MAX8925
           WLED output, say Y here to enable this driver.
 
 config BACKLIGHT_MBP_NVIDIA
        tristate "MacBook Pro Nvidia Backlight Driver"
-       depends on BACKLIGHT_CLASS_DEVICE && X86
-       default n
+       depends on X86
        help
          If you have an Apple Macbook Pro with Nvidia graphics hardware say Y
          to enable a driver for its backlight
 
 config BACKLIGHT_TOSA
         tristate "Sharp SL-6000 Backlight Driver"
-        depends on BACKLIGHT_CLASS_DEVICE && I2C
-        depends on MACH_TOSA && LCD_TOSA
-        default n
+        depends on I2C && MACH_TOSA && LCD_TOSA
         help
           If you have an Sharp SL-6000 Zaurus say Y to enable a driver
           for its backlight
 
 config BACKLIGHT_SAHARA
         tristate "Tabletkiosk Sahara Touch-iT Backlight Driver"
-        depends on BACKLIGHT_CLASS_DEVICE && X86
-        default n
+        depends on X86
         help
           If you have a Tabletkiosk Sahara Touch-iT, say y to enable the
           backlight driver.
 
 config BACKLIGHT_WM831X
         tristate "WM831x PMIC Backlight Driver"
-        depends on BACKLIGHT_CLASS_DEVICE && MFD_WM831X
+        depends on MFD_WM831X
         help
           If you have a backlight driven by the ISINK and DCDC of a
           WM831x PMIC say y to enable the backlight driver for it.
 
 config BACKLIGHT_ADX
         tristate "Avionic Design Xanthos Backlight Driver"
-        depends on BACKLIGHT_CLASS_DEVICE && ARCH_PXA_ADX
+        depends on ARCH_PXA_ADX
         default y
         help
           Say Y to enable the backlight driver on Avionic Design Xanthos-based
@@ -268,7 +274,7 @@ config BACKLIGHT_ADX
 
 config BACKLIGHT_ADP5520
         tristate "Backlight Driver for ADP5520/ADP5501 using WLED"
-        depends on BACKLIGHT_CLASS_DEVICE && PMIC_ADP5520
+        depends on PMIC_ADP5520
         help
           If you have a LCD backlight connected to the BST/BL_SNK output of
           ADP5520 or ADP5501, say Y here to enable this driver.
@@ -276,9 +282,31 @@ config BACKLIGHT_ADP5520
           To compile this driver as a module, choose M here: the module will
           be called adp5520_bl.
 
+config BACKLIGHT_ADP8860
+        tristate "Backlight Driver for ADP8860/ADP8861/ADP8863 using WLED"
+        depends on BACKLIGHT_CLASS_DEVICE && I2C
+        select NEW_LEDS
+        select LEDS_CLASS
+        help
+          If you have a LCD backlight connected to the ADP8860, ADP8861 or
+          ADP8863 say Y here to enable this driver.
+
+          To compile this driver as a module, choose M here: the module will
+          be called adp8860_bl.
+
 config BACKLIGHT_88PM860X
         tristate "Backlight Driver for 88PM8606 using WLED"
-        depends on BACKLIGHT_CLASS_DEVICE && MFD_88PM860X
+        depends on MFD_88PM860X
         help
           Say Y to enable the backlight driver for Marvell 88PM8606.
 
+config BACKLIGHT_PCF50633
+        tristate "Backlight driver for NXP PCF50633 MFD"
+        depends on BACKLIGHT_CLASS_DEVICE && MFD_PCF50633
+        help
+          If you have a backlight driven by a NXP PCF50633 MFD, say Y here to
+          enable its driver.
+
+endif # BACKLIGHT_CLASS_DEVICE
+
+endif # BACKLIGHT_LCD_SUPPORT
diff --git a/drivers/video/backlight/Makefile b/drivers/video/backlight/Makefile
index 09d1f14d6257..44c0f81ad85d 100644
--- a/drivers/video/backlight/Makefile
+++ b/drivers/video/backlight/Makefile
@@ -11,9 +11,11 @@ obj-$(CONFIG_LCD_PLATFORM)	   += platform_lcd.o
 obj-$(CONFIG_LCD_VGG2432A4)        += vgg2432a4.o
 obj-$(CONFIG_LCD_TDO24M)           += tdo24m.o
 obj-$(CONFIG_LCD_TOSA)             += tosa_lcd.o
+obj-$(CONFIG_LCD_S6E63M0)       += s6e63m0.o
 
 obj-$(CONFIG_BACKLIGHT_CLASS_DEVICE) += backlight.o
 obj-$(CONFIG_BACKLIGHT_ATMEL_PWM)    += atmel-pwm-bl.o
+obj-$(CONFIG_BACKLIGHT_EP93XX)  += ep93xx_bl.o
 obj-$(CONFIG_BACKLIGHT_GENERIC) += generic_bl.o
 obj-$(CONFIG_BACKLIGHT_HP700)   += jornada720_bl.o
 obj-$(CONFIG_BACKLIGHT_HP680)   += hp680_bl.o
@@ -30,5 +32,7 @@ obj-$(CONFIG_BACKLIGHT_SAHARA)	+= kb3886_bl.o
 obj-$(CONFIG_BACKLIGHT_WM831X)  += wm831x_bl.o
 obj-$(CONFIG_BACKLIGHT_ADX)    += adx_bl.o
 obj-$(CONFIG_BACKLIGHT_ADP5520) += adp5520_bl.o
+obj-$(CONFIG_BACKLIGHT_ADP8860) += adp8860_bl.o
 obj-$(CONFIG_BACKLIGHT_88PM860X) += 88pm860x_bl.o
+obj-$(CONFIG_BACKLIGHT_PCF50633)        += pcf50633-backlight.o
 
diff --git a/drivers/video/backlight/adp8860_bl.c b/drivers/video/backlight/adp8860_bl.c
new file mode 100644
index 000000000000..3ec24609151e
--- /dev/null
+++ b/drivers/video/backlight/adp8860_bl.c
@@ -0,0 +1,836 @@
+/*
+ * Backlight driver for Analog Devices ADP8860 Backlight Devices
+ *
+ * Copyright 2009-2010 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/pm.h>
+#include <linux/platform_device.h>
+#include <linux/i2c.h>
+#include <linux/fb.h>
+#include <linux/backlight.h>
+#include <linux/leds.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+
+#include <linux/i2c/adp8860.h>
+#define ADP8860_EXT_FEATURES
+#define ADP8860_USE_LEDS
+
+#define ADP8860_MFDVID 0x00 /* Manufacturer and device ID */
+#define ADP8860_MDCR 0x01 /* Device mode and status */
+#define ADP8860_MDCR2 0x02 /* Device mode and Status Register 2 */
+#define ADP8860_INTR_EN 0x03 /* Interrupts enable */
+#define ADP8860_CFGR 0x04 /* Configuration register */
+#define ADP8860_BLSEN 0x05 /* Sink enable backlight or independent */
+#define ADP8860_BLOFF 0x06 /* Backlight off timeout */
+#define ADP8860_BLDIM 0x07 /* Backlight dim timeout */
+#define ADP8860_BLFR 0x08 /* Backlight fade in and out rates */
+#define ADP8860_BLMX1 0x09 /* Backlight (Brightness Level 1-daylight) maximum current */
+#define ADP8860_BLDM1 0x0A /* Backlight (Brightness Level 1-daylight) dim current */
+#define ADP8860_BLMX2 0x0B /* Backlight (Brightness Level 2-office) maximum current */
+#define ADP8860_BLDM2 0x0C /* Backlight (Brightness Level 2-office) dim current */
+#define ADP8860_BLMX3 0x0D /* Backlight (Brightness Level 3-dark) maximum current */
+#define ADP8860_BLDM3 0x0E /* Backlight (Brightness Level 3-dark) dim current */
+#define ADP8860_ISCFR 0x0F /* Independent sink current fade control register */
+#define ADP8860_ISCC 0x10 /* Independent sink current control register */
+#define ADP8860_ISCT1 0x11 /* Independent Sink Current Timer Register LED[7:5] */
+#define ADP8860_ISCT2 0x12 /* Independent Sink Current Timer Register LED[4:1] */
+#define ADP8860_ISCF 0x13 /* Independent sink current fade register */
+#define ADP8860_ISC7 0x14 /* Independent Sink Current LED7 */
+#define ADP8860_ISC6 0x15 /* Independent Sink Current LED6 */
+#define ADP8860_ISC5 0x16 /* Independent Sink Current LED5 */
+#define ADP8860_ISC4 0x17 /* Independent Sink Current LED4 */
+#define ADP8860_ISC3 0x18 /* Independent Sink Current LED3 */
+#define ADP8860_ISC2 0x19 /* Independent Sink Current LED2 */
+#define ADP8860_ISC1 0x1A /* Independent Sink Current LED1 */
+#define ADP8860_CCFG 0x1B /* Comparator configuration */
+#define ADP8860_CCFG2 0x1C /* Second comparator configuration */
+#define ADP8860_L2_TRP 0x1D /* L2 comparator reference */
+#define ADP8860_L2_HYS 0x1E /* L2 hysteresis */
+#define ADP8860_L3_TRP 0x1F /* L3 comparator reference */
+#define ADP8860_L3_HYS 0x20 /* L3 hysteresis */
+#define ADP8860_PH1LEVL 0x21 /* First phototransistor ambient light level-low byte register */
+#define ADP8860_PH1LEVH 0x22 /* First phototransistor ambient light level-high byte register */
+#define ADP8860_PH2LEVL 0x23 /* Second phototransistor ambient light level-low byte register */
+#define ADP8860_PH2LEVH 0x24 /* Second phototransistor ambient light level-high byte register */
+
+#define ADP8860_MANUFID         0x0  /* Analog Devices ADP8860 Manufacturer ID */
+#define ADP8861_MANUFID         0x4  /* Analog Devices ADP8861 Manufacturer ID */
+#define ADP8863_MANUFID         0x2  /* Analog Devices ADP8863 Manufacturer ID */
+
+#define ADP8860_DEVID(x)        ((x) & 0xF)
+#define ADP8860_MANID(x)        ((x) >> 4)
+
+/* MDCR Device mode and status */
+#define INT_CFG                 (1 << 6)
+#define NSTBY                   (1 << 5)
+#define DIM_EN                  (1 << 4)
+#define GDWN_DIS                (1 << 3)
+#define SIS_EN                  (1 << 2)
+#define CMP_AUTOEN              (1 << 1)
+#define BLEN                    (1 << 0)
+
+/* ADP8860_CCFG Main ALS comparator level enable */
+#define L3_EN                   (1 << 1)
+#define L2_EN                   (1 << 0)
+
+#define CFGR_BLV_SHIFT          3
+#define CFGR_BLV_MASK           0x3
+#define ADP8860_FLAG_LED_MASK   0xFF
+
+#define FADE_VAL(in, out)       ((0xF & (in)) | ((0xF & (out)) << 4))
+#define BL_CFGR_VAL(law, blv)   ((((blv) & CFGR_BLV_MASK) << CFGR_BLV_SHIFT) | ((0x3 & (law)) << 1))
+#define ALS_CCFG_VAL(filt)      ((0x7 & filt) << 5)
+
+enum {
+        adp8860,
+        adp8861,
+        adp8863
+};
+
+struct adp8860_led {
+        struct led_classdev     cdev;
+        struct work_struct      work;
+        struct i2c_client       *client;
+        enum led_brightness     new_brightness;
+        int                     id;
+        int                     flags;
+};
+
+struct adp8860_bl {
+        struct i2c_client *client;
+        struct backlight_device *bl;
+        struct adp8860_led *led;
+        struct adp8860_backlight_platform_data *pdata;
+        struct mutex lock;
+        unsigned long cached_daylight_max;
+        int id;
+        int revid;
+        int current_brightness;
+        unsigned en_ambl_sens:1;
+        unsigned gdwn_dis:1;
+};
+
+static int adp8860_read(struct i2c_client *client, int reg, uint8_t *val)
+{
+        int ret;
+
+        ret = i2c_smbus_read_byte_data(client, reg);
+        if (ret < 0) {
+                dev_err(&client->dev, "failed reading at 0x%02x\n", reg);
+                return ret;
+        }
+
+        *val = (uint8_t)ret;
+        return 0;
+}
+
+static int adp8860_write(struct i2c_client *client, u8 reg, u8 val)
+{
+        return i2c_smbus_write_byte_data(client, reg, val);
+}
+
+static int adp8860_set_bits(struct i2c_client *client, int reg, uint8_t bit_mask)
+{
+        struct adp8860_bl *data = i2c_get_clientdata(client);
+        uint8_t reg_val;
+        int ret;
+
+        mutex_lock(&data->lock);
+
+        ret = adp8860_read(client, reg, &reg_val);
+
+        if (!ret && ((reg_val & bit_mask) == 0)) {
+                reg_val |= bit_mask;
+                ret = adp8860_write(client, reg, reg_val);
+        }
+
+        mutex_unlock(&data->lock);
+        return ret;
+}
+
+static int adp8860_clr_bits(struct i2c_client *client, int reg, uint8_t bit_mask)
+{
+        struct adp8860_bl *data = i2c_get_clientdata(client);
+        uint8_t reg_val;
+        int ret;
+
+        mutex_lock(&data->lock);
+
+        ret = adp8860_read(client, reg, &reg_val);
+
+        if (!ret && (reg_val & bit_mask)) {
+                reg_val &= ~bit_mask;
+                ret = adp8860_write(client, reg, reg_val);
+        }
+
+        mutex_unlock(&data->lock);
+        return ret;
+}
+
+/*
+ * Independent sink / LED
+ */
+#if defined(ADP8860_USE_LEDS)
+static void adp8860_led_work(struct work_struct *work)
+{
+        struct adp8860_led *led = container_of(work, struct adp8860_led, work);
+        adp8860_write(led->client, ADP8860_ISC1 - led->id + 1,
+                         led->new_brightness >> 1);
+}
+
+static void adp8860_led_set(struct led_classdev *led_cdev,
+                           enum led_brightness value)
+{
+        struct adp8860_led *led;
+
+        led = container_of(led_cdev, struct adp8860_led, cdev);
+        led->new_brightness = value;
+        schedule_work(&led->work);
+}
+
+static int adp8860_led_setup(struct adp8860_led *led)
+{
+        struct i2c_client *client = led->client;
+        int ret = 0;
+
+        ret = adp8860_write(client, ADP8860_ISC1 - led->id + 1, 0);
+        ret |= adp8860_set_bits(client, ADP8860_ISCC, 1 << (led->id - 1));
+
+        if (led->id > 4)
+                ret |= adp8860_set_bits(client, ADP8860_ISCT1,
+                                (led->flags & 0x3) << ((led->id - 5) * 2));
+        else
+                ret |= adp8860_set_bits(client, ADP8860_ISCT2,
+                                (led->flags & 0x3) << ((led->id - 1) * 2));
+
+        return ret;
+}
+
+static int __devinit adp8860_led_probe(struct i2c_client *client)
+{
+        struct adp8860_backlight_platform_data *pdata =
+                client->dev.platform_data;
+        struct adp8860_bl *data = i2c_get_clientdata(client);
+        struct adp8860_led *led, *led_dat;
+        struct led_info *cur_led;
+        int ret, i;
+
+        led = kzalloc(sizeof(*led) * pdata->num_leds, GFP_KERNEL);
+        if (led == NULL) {
+                dev_err(&client->dev, "failed to alloc memory\n");
+                return -ENOMEM;
+        }
+
+        ret = adp8860_write(client, ADP8860_ISCFR, pdata->led_fade_law);
+        ret = adp8860_write(client, ADP8860_ISCT1,
+                        (pdata->led_on_time & 0x3) << 6);
+        ret |= adp8860_write(client, ADP8860_ISCF,
+                        FADE_VAL(pdata->led_fade_in, pdata->led_fade_out));
+
+        if (ret) {
+                dev_err(&client->dev, "failed to write\n");
+                goto err_free;
+        }
+
+        for (i = 0; i < pdata->num_leds; ++i) {
+                cur_led = &pdata->leds[i];
+                led_dat = &led[i];
+
+                led_dat->id = cur_led->flags & ADP8860_FLAG_LED_MASK;
+
+                if (led_dat->id > 7 || led_dat->id < 1) {
+                        dev_err(&client->dev, "Invalid LED ID %d\n",
+                                led_dat->id);
+                        goto err;
+                }
+
+                if (pdata->bl_led_assign & (1 << (led_dat->id - 1))) {
+                        dev_err(&client->dev, "LED %d used by Backlight\n",
+                                led_dat->id);
+                        goto err;
+                }
+
+                led_dat->cdev.name = cur_led->name;
+                led_dat->cdev.default_trigger = cur_led->default_trigger;
+                led_dat->cdev.brightness_set = adp8860_led_set;
+                led_dat->cdev.brightness = LED_OFF;
+                led_dat->flags = cur_led->flags >> FLAG_OFFT_SHIFT;
+                led_dat->client = client;
+                led_dat->new_brightness = LED_OFF;
+                INIT_WORK(&led_dat->work, adp8860_led_work);
+
+                ret = led_classdev_register(&client->dev, &led_dat->cdev);
+                if (ret) {
+                        dev_err(&client->dev, "failed to register LED %d\n",
+                                led_dat->id);
+                        goto err;
+                }
+
+                ret = adp8860_led_setup(led_dat);
+                if (ret) {
+                        dev_err(&client->dev, "failed to write\n");
+                        i++;
+                        goto err;
+                }
+        }
+
+        data->led = led;
+
+        return 0;
+
+ err:
+        for (i = i - 1; i >= 0; --i) {
+                led_classdev_unregister(&led[i].cdev);
+                cancel_work_sync(&led[i].work);
+        }
+
+ err_free:
+        kfree(led);
+
+        return ret;
+}
+
+static int __devexit adp8860_led_remove(struct i2c_client *client)
+{
+        struct adp8860_backlight_platform_data *pdata =
+                client->dev.platform_data;
+        struct adp8860_bl *data = i2c_get_clientdata(client);
+        int i;
+
+        for (i = 0; i < pdata->num_leds; i++) {
+                led_classdev_unregister(&data->led[i].cdev);
+                cancel_work_sync(&data->led[i].work);
+        }
+
+        kfree(data->led);
+        return 0;
+}
+#else
+static int __devinit adp8860_led_probe(struct i2c_client *client)
+{
+        return 0;
+}
+
+static int __devexit adp8860_led_remove(struct i2c_client *client)
+{
+        return 0;
+}
+#endif
+
+static int adp8860_bl_set(struct backlight_device *bl, int brightness)
+{
+        struct adp8860_bl *data = bl_get_data(bl);
+        struct i2c_client *client = data->client;
+        int ret = 0;
+
+        if (data->en_ambl_sens) {
+                if ((brightness > 0) && (brightness < ADP8860_MAX_BRIGHTNESS)) {
+                        /* Disable Ambient Light auto adjust */
+                        ret |= adp8860_clr_bits(client, ADP8860_MDCR,
+                                        CMP_AUTOEN);
+                        ret |= adp8860_write(client, ADP8860_BLMX1, brightness);
+                } else {
+                        /*
+                         * MAX_BRIGHTNESS -> Enable Ambient Light auto adjust
+                         * restore daylight l1 sysfs brightness
+                         */
+                        ret |= adp8860_write(client, ADP8860_BLMX1,
+                                         data->cached_daylight_max);
+                        ret |= adp8860_set_bits(client, ADP8860_MDCR,
+                                         CMP_AUTOEN);
+                }
+        } else
+                ret |= adp8860_write(client, ADP8860_BLMX1, brightness);
+
+        if (data->current_brightness && brightness == 0)
+                ret |= adp8860_set_bits(client,
+                                ADP8860_MDCR, DIM_EN);
+        else if (data->current_brightness == 0 && brightness)
+                ret |= adp8860_clr_bits(client,
+                                ADP8860_MDCR, DIM_EN);
+
+        if (!ret)
+                data->current_brightness = brightness;
+
+        return ret;
+}
+
+static int adp8860_bl_update_status(struct backlight_device *bl)
+{
+        int brightness = bl->props.brightness;
+        if (bl->props.power != FB_BLANK_UNBLANK)
+                brightness = 0;
+
+        if (bl->props.fb_blank != FB_BLANK_UNBLANK)
+                brightness = 0;
+
+        return adp8860_bl_set(bl, brightness);
+}
+
+static int adp8860_bl_get_brightness(struct backlight_device *bl)
+{
+        struct adp8860_bl *data = bl_get_data(bl);
+
+        return data->current_brightness;
+}
+
+static const struct backlight_ops adp8860_bl_ops = {
+        .update_status  = adp8860_bl_update_status,
+        .get_brightness = adp8860_bl_get_brightness,
+};
+
+static int adp8860_bl_setup(struct backlight_device *bl)
+{
+        struct adp8860_bl *data = bl_get_data(bl);
+        struct i2c_client *client = data->client;
+        struct adp8860_backlight_platform_data *pdata = data->pdata;
+        int ret = 0;
+
+        ret |= adp8860_write(client, ADP8860_BLSEN, ~pdata->bl_led_assign);
+        ret |= adp8860_write(client, ADP8860_BLMX1, pdata->l1_daylight_max);
+        ret |= adp8860_write(client, ADP8860_BLDM1, pdata->l1_daylight_dim);
+
+        if (data->en_ambl_sens) {
+                data->cached_daylight_max = pdata->l1_daylight_max;
+                ret |= adp8860_write(client, ADP8860_BLMX2,
+                                                pdata->l2_office_max);
+                ret |= adp8860_write(client, ADP8860_BLDM2,
+                                                pdata->l2_office_dim);
+                ret |= adp8860_write(client, ADP8860_BLMX3,
+                                                pdata->l3_dark_max);
+                ret |= adp8860_write(client, ADP8860_BLDM3,
+                                                pdata->l3_dark_dim);
+
+                ret |= adp8860_write(client, ADP8860_L2_TRP, pdata->l2_trip);
+                ret |= adp8860_write(client, ADP8860_L2_HYS, pdata->l2_hyst);
+                ret |= adp8860_write(client, ADP8860_L3_TRP, pdata->l3_trip);
+                ret |= adp8860_write(client, ADP8860_L3_HYS, pdata->l3_hyst);
+                ret |= adp8860_write(client, ADP8860_CCFG, L2_EN | L3_EN |
+                                                ALS_CCFG_VAL(pdata->abml_filt));
+        }
+
+        ret |= adp8860_write(client, ADP8860_CFGR,
+                        BL_CFGR_VAL(pdata->bl_fade_law, 0));
+
+        ret |= adp8860_write(client, ADP8860_BLFR, FADE_VAL(pdata->bl_fade_in,
+                        pdata->bl_fade_out));
+
+        ret |= adp8860_set_bits(client, ADP8860_MDCR, BLEN | DIM_EN | NSTBY |
+                        (data->gdwn_dis ? GDWN_DIS : 0));
+
+        return ret;
+}
+
+static ssize_t adp8860_show(struct device *dev, char *buf, int reg)
+{
+        struct adp8860_bl *data = dev_get_drvdata(dev);
+        int error;
+        uint8_t reg_val;
+
+        mutex_lock(&data->lock);
+        error = adp8860_read(data->client, reg, &reg_val);
+        mutex_unlock(&data->lock);
+
+        if (error < 0)
+                return error;
+
+        return sprintf(buf, "%u\n", reg_val);
+}
+
+static ssize_t adp8860_store(struct device *dev, const char *buf,
+                         size_t count, int reg)
+{
+        struct adp8860_bl *data = dev_get_drvdata(dev);
+        unsigned long val;
+        int ret;
+
+        ret = strict_strtoul(buf, 10, &val);
+        if (ret)
+                return ret;
+
+        mutex_lock(&data->lock);
+        adp8860_write(data->client, reg, val);
+        mutex_unlock(&data->lock);
+
+        return count;
+}
+
+static ssize_t adp8860_bl_l3_dark_max_show(struct device *dev,
+                                     struct device_attribute *attr, char *buf)
+{
+        return adp8860_show(dev, buf, ADP8860_BLMX3);
+}
+
+static ssize_t adp8860_bl_l3_dark_max_store(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t count)
+{
+        return adp8860_store(dev, buf, count, ADP8860_BLMX3);
+}
+
+static DEVICE_ATTR(l3_dark_max, 0664, adp8860_bl_l3_dark_max_show,
+                        adp8860_bl_l3_dark_max_store);
+
+static ssize_t adp8860_bl_l2_office_max_show(struct device *dev,
+                struct device_attribute *attr, char *buf)
+{
+        return adp8860_show(dev, buf, ADP8860_BLMX2);
+}
+
+static ssize_t adp8860_bl_l2_office_max_store(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t count)
+{
+        return adp8860_store(dev, buf, count, ADP8860_BLMX2);
+}
+static DEVICE_ATTR(l2_office_max, 0664, adp8860_bl_l2_office_max_show,
+                        adp8860_bl_l2_office_max_store);
+
+static ssize_t adp8860_bl_l1_daylight_max_show(struct device *dev,
+                        struct device_attribute *attr, char *buf)
+{
+        return adp8860_show(dev, buf, ADP8860_BLMX1);
+}
+
+static ssize_t adp8860_bl_l1_daylight_max_store(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t count)
+{
+        struct adp8860_bl *data = dev_get_drvdata(dev);
+
+        strict_strtoul(buf, 10, &data->cached_daylight_max);
+        return adp8860_store(dev, buf, count, ADP8860_BLMX1);
+}
+static DEVICE_ATTR(l1_daylight_max, 0664, adp8860_bl_l1_daylight_max_show,
+                        adp8860_bl_l1_daylight_max_store);
+
+static ssize_t adp8860_bl_l3_dark_dim_show(struct device *dev,
+                        struct device_attribute *attr, char *buf)
+{
+        return adp8860_show(dev, buf, ADP8860_BLDM3);
+}
+
+static ssize_t adp8860_bl_l3_dark_dim_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const char *buf, size_t count)
+{
+        return adp8860_store(dev, buf, count, ADP8860_BLDM3);
+}
+static DEVICE_ATTR(l3_dark_dim, 0664, adp8860_bl_l3_dark_dim_show,
+                        adp8860_bl_l3_dark_dim_store);
+
+static ssize_t adp8860_bl_l2_office_dim_show(struct device *dev,
+                        struct device_attribute *attr, char *buf)
+{
+        return adp8860_show(dev, buf, ADP8860_BLDM2);
+}
+
+static ssize_t adp8860_bl_l2_office_dim_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const char *buf, size_t count)
+{
+        return adp8860_store(dev, buf, count, ADP8860_BLDM2);
+}
+static DEVICE_ATTR(l2_office_dim, 0664, adp8860_bl_l2_office_dim_show,
+                        adp8860_bl_l2_office_dim_store);
+
+static ssize_t adp8860_bl_l1_daylight_dim_show(struct device *dev,
+                                     struct device_attribute *attr, char *buf)
+{
+        return adp8860_show(dev, buf, ADP8860_BLDM1);
+}
+
+static ssize_t adp8860_bl_l1_daylight_dim_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const char *buf, size_t count)
+{
+        return adp8860_store(dev, buf, count, ADP8860_BLDM1);
+}
+static DEVICE_ATTR(l1_daylight_dim, 0664, adp8860_bl_l1_daylight_dim_show,
+                        adp8860_bl_l1_daylight_dim_store);
+
+#ifdef ADP8860_EXT_FEATURES
+static ssize_t adp8860_bl_ambient_light_level_show(struct device *dev,
+                                     struct device_attribute *attr, char *buf)
+{
+        struct adp8860_bl *data = dev_get_drvdata(dev);
+        int error;
+        uint8_t reg_val;
+        uint16_t ret_val;
+
+        mutex_lock(&data->lock);
+        error = adp8860_read(data->client, ADP8860_PH1LEVL, &reg_val);
+        ret_val = reg_val;
+        error |= adp8860_read(data->client, ADP8860_PH1LEVH, &reg_val);
+        mutex_unlock(&data->lock);
+
+        if (error < 0)
+                return error;
+
+        /* Return 13-bit conversion value for the first light sensor */
+        ret_val += (reg_val & 0x1F) << 8;
+
+        return sprintf(buf, "%u\n", ret_val);
+}
+static DEVICE_ATTR(ambient_light_level, 0444,
+                adp8860_bl_ambient_light_level_show, NULL);
+
+static ssize_t adp8860_bl_ambient_light_zone_show(struct device *dev,
+                                     struct device_attribute *attr, char *buf)
+{
+        struct adp8860_bl *data = dev_get_drvdata(dev);
+        int error;
+        uint8_t reg_val;
+
+        mutex_lock(&data->lock);
+        error = adp8860_read(data->client, ADP8860_CFGR, &reg_val);
+        mutex_unlock(&data->lock);
+
+        if (error < 0)
+                return error;
+
+        return sprintf(buf, "%u\n",
+                ((reg_val >> CFGR_BLV_SHIFT) & CFGR_BLV_MASK) + 1);
+}
+
+static ssize_t adp8860_bl_ambient_light_zone_store(struct device *dev,
+                                     struct device_attribute *attr,
+                                     const char *buf, size_t count)
+{
+        struct adp8860_bl *data = dev_get_drvdata(dev);
+        unsigned long val;
+        uint8_t reg_val;
+        int ret;
+
+        ret = strict_strtoul(buf, 10, &val);
+        if (ret)
+                return ret;
+
+        if (val == 0) {
+                /* Enable automatic ambient light sensing */
+                adp8860_set_bits(data->client, ADP8860_MDCR, CMP_AUTOEN);
+        } else if ((val > 0) && (val < 6)) {
+                /* Disable automatic ambient light sensing */
+                adp8860_clr_bits(data->client, ADP8860_MDCR, CMP_AUTOEN);
+
+                /* Set user supplied ambient light zone */
+                mutex_lock(&data->lock);
+                adp8860_read(data->client, ADP8860_CFGR, &reg_val);
+                reg_val &= ~(CFGR_BLV_MASK << CFGR_BLV_SHIFT);
+                reg_val |= val << CFGR_BLV_SHIFT;
+                adp8860_write(data->client, ADP8860_CFGR, reg_val);
+                mutex_unlock(&data->lock);
+        }
+
+        return count;
+}
+static DEVICE_ATTR(ambient_light_zone, 0664,
+                adp8860_bl_ambient_light_zone_show,
+                adp8860_bl_ambient_light_zone_store);
+#endif
+
+static struct attribute *adp8860_bl_attributes[] = {
+        &dev_attr_l3_dark_max.attr,
+        &dev_attr_l3_dark_dim.attr,
+        &dev_attr_l2_office_max.attr,
+        &dev_attr_l2_office_dim.attr,
+        &dev_attr_l1_daylight_max.attr,
+        &dev_attr_l1_daylight_dim.attr,
+#ifdef ADP8860_EXT_FEATURES
+        &dev_attr_ambient_light_level.attr,
+        &dev_attr_ambient_light_zone.attr,
+#endif
+        NULL
+};
+
+static const struct attribute_group adp8860_bl_attr_group = {
+        .attrs = adp8860_bl_attributes,
+};
+
+static int __devinit adp8860_probe(struct i2c_client *client,
+                                        const struct i2c_device_id *id)
+{
+        struct backlight_device *bl;
+        struct adp8860_bl *data;
+        struct adp8860_backlight_platform_data *pdata =
+                client->dev.platform_data;
+        struct backlight_properties props;
+        uint8_t reg_val;
+        int ret;
+
+        if (!i2c_check_functionality(client->adapter,
+                                        I2C_FUNC_SMBUS_BYTE_DATA)) {
+                dev_err(&client->dev, "SMBUS Byte Data not Supported\n");
+                return -EIO;
+        }
+
+        if (!pdata) {
+                dev_err(&client->dev, "no platform data?\n");
+                return -EINVAL;
+        }
+
+        data = kzalloc(sizeof(*data), GFP_KERNEL);
+        if (data == NULL)
+                return -ENOMEM;
+
+        ret = adp8860_read(client, ADP8860_MFDVID, &reg_val);
+        if (ret < 0)
+                goto out2;
+
+        switch (ADP8860_MANID(reg_val)) {
+        case ADP8863_MANUFID:
+                data->gdwn_dis = !!pdata->gdwn_dis;
+        case ADP8860_MANUFID:
+                data->en_ambl_sens = !!pdata->en_ambl_sens;
+                break;
+        case ADP8861_MANUFID:
+                data->gdwn_dis = !!pdata->gdwn_dis;
+                break;
+        default:
+                dev_err(&client->dev, "failed to probe\n");
+                ret = -ENODEV;
+                goto out2;
+        }
+
+        /* It's confirmed that the DEVID field is actually a REVID */
+
+        data->revid = ADP8860_DEVID(reg_val);
+        data->client = client;
+        data->pdata = pdata;
+        data->id = id->driver_data;
+        data->current_brightness = 0;
+        i2c_set_clientdata(client, data);
+
+        memset(&props, 0, sizeof(props));
+        props.max_brightness = ADP8860_MAX_BRIGHTNESS;
+
+        mutex_init(&data->lock);
+
+        bl = backlight_device_register(dev_driver_string(&client->dev),
+                        &client->dev, data, &adp8860_bl_ops, &props);
+        if (IS_ERR(bl)) {
+                dev_err(&client->dev, "failed to register backlight\n");
+                ret = PTR_ERR(bl);
+                goto out2;
+        }
+
+        bl->props.max_brightness =
+                bl->props.brightness = ADP8860_MAX_BRIGHTNESS;
+
+        data->bl = bl;
+
+        if (data->en_ambl_sens)
+                ret = sysfs_create_group(&bl->dev.kobj,
+                        &adp8860_bl_attr_group);
+
+        if (ret) {
+                dev_err(&client->dev, "failed to register sysfs\n");
+                goto out1;
+        }
+
+        ret = adp8860_bl_setup(bl);
+        if (ret) {
+                ret = -EIO;
+                goto out;
+        }
+
+        backlight_update_status(bl);
+
+        dev_info(&client->dev, "%s Rev.%d Backlight\n",
+                client->name, data->revid);
+
+        if (pdata->num_leds)
+                adp8860_led_probe(client);
+
+        return 0;
+
+out:
+        if (data->en_ambl_sens)
+                sysfs_remove_group(&data->bl->dev.kobj,
+                        &adp8860_bl_attr_group);
+out1:
+        backlight_device_unregister(bl);
+out2:
+        kfree(data);
+
+        return ret;
+}
+
+static int __devexit adp8860_remove(struct i2c_client *client)
+{
+        struct adp8860_bl *data = i2c_get_clientdata(client);
+
+        adp8860_clr_bits(client, ADP8860_MDCR, NSTBY);
+
+        if (data->led)
+                adp8860_led_remove(client);
+
+        if (data->en_ambl_sens)
+                sysfs_remove_group(&data->bl->dev.kobj,
+                        &adp8860_bl_attr_group);
+
+        backlight_device_unregister(data->bl);
+        kfree(data);
+
+        return 0;
+}
+
+#ifdef CONFIG_PM
+static int adp8860_i2c_suspend(struct i2c_client *client, pm_message_t message)
+{
+        adp8860_clr_bits(client, ADP8860_MDCR, NSTBY);
+
+        return 0;
+}
+
+static int adp8860_i2c_resume(struct i2c_client *client)
+{
+        adp8860_set_bits(client, ADP8860_MDCR, NSTBY);
+
+        return 0;
+}
+#else
+#define adp8860_i2c_suspend NULL
+#define adp8860_i2c_resume NULL
+#endif
+
+static const struct i2c_device_id adp8860_id[] = {
+        { "adp8860", adp8860 },
+        { "adp8861", adp8861 },
+        { "adp8863", adp8863 },
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, adp8860_id);
+
+static struct i2c_driver adp8860_driver = {
+        .driver = {
+                .name = KBUILD_MODNAME,
+        },
+        .probe    = adp8860_probe,
+        .remove   = __devexit_p(adp8860_remove),
+        .suspend = adp8860_i2c_suspend,
+        .resume  = adp8860_i2c_resume,
+        .id_table = adp8860_id,
+};
+
+static int __init adp8860_init(void)
+{
+        return i2c_add_driver(&adp8860_driver);
+}
+module_init(adp8860_init);
+
+static void __exit adp8860_exit(void)
+{
+        i2c_del_driver(&adp8860_driver);
+}
+module_exit(adp8860_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
+MODULE_DESCRIPTION("ADP8860 Backlight driver");
+MODULE_ALIAS("i2c:adp8860-backlight");
diff --git a/drivers/video/backlight/adx_bl.c b/drivers/video/backlight/adx_bl.c
index 7f4a7c30a98b..fe9af129c5dd 100644
--- a/drivers/video/backlight/adx_bl.c
+++ b/drivers/video/backlight/adx_bl.c
@@ -107,8 +107,8 @@ static int __devinit adx_backlight_probe(struct platform_device *pdev)
         props.max_brightness = 0xff;
         bldev = backlight_device_register(dev_name(&pdev->dev), &pdev->dev,
                                           bl, &adx_backlight_ops, &props);
-        if (!bldev) {
-                ret = -ENOMEM;
+        if (IS_ERR(bldev)) {
+                ret = PTR_ERR(bldev);
                 goto out;
         }
 
diff --git a/drivers/video/backlight/ep93xx_bl.c b/drivers/video/backlight/ep93xx_bl.c
new file mode 100644
index 000000000000..b0cc49184803
--- /dev/null
+++ b/drivers/video/backlight/ep93xx_bl.c
@@ -0,0 +1,160 @@
+/*
+ * Driver for the Cirrus EP93xx lcd backlight
+ *
+ * Copyright (c) 2010 H Hartley Sweeten <hsweeten@visionengravers.com>
+ *
+ * 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.
+ *
+ * This driver controls the pulse width modulated brightness control output,
+ * BRIGHT, on the Cirrus EP9307, EP9312, and EP9315 processors.
+ */
+
+
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/fb.h>
+#include <linux/backlight.h>
+
+#include <mach/hardware.h>
+
+#define EP93XX_RASTER_REG(x)            (EP93XX_RASTER_BASE + (x))
+#define EP93XX_RASTER_BRIGHTNESS        EP93XX_RASTER_REG(0x20)
+
+#define EP93XX_MAX_COUNT                255
+#define EP93XX_MAX_BRIGHT               255
+#define EP93XX_DEF_BRIGHT               128
+
+struct ep93xxbl {
+        void __iomem *mmio;
+        int brightness;
+};
+
+static int ep93xxbl_set(struct backlight_device *bl, int brightness)
+{
+        struct ep93xxbl *ep93xxbl = bl_get_data(bl);
+
+        __raw_writel((brightness << 8) | EP93XX_MAX_COUNT, ep93xxbl->mmio);
+
+        ep93xxbl->brightness = brightness;
+
+        return 0;
+}
+
+static int ep93xxbl_update_status(struct backlight_device *bl)
+{
+        int brightness = bl->props.brightness;
+
+        if (bl->props.power != FB_BLANK_UNBLANK ||
+            bl->props.fb_blank != FB_BLANK_UNBLANK)
+                brightness = 0;
+
+        return ep93xxbl_set(bl, brightness);
+}
+
+static int ep93xxbl_get_brightness(struct backlight_device *bl)
+{
+        struct ep93xxbl *ep93xxbl = bl_get_data(bl);
+
+        return ep93xxbl->brightness;
+}
+
+static const struct backlight_ops ep93xxbl_ops = {
+        .update_status  = ep93xxbl_update_status,
+        .get_brightness = ep93xxbl_get_brightness,
+};
+
+static int __init ep93xxbl_probe(struct platform_device *dev)
+{
+        struct ep93xxbl *ep93xxbl;
+        struct backlight_device *bl;
+        struct backlight_properties props;
+
+        ep93xxbl = devm_kzalloc(&dev->dev, sizeof(*ep93xxbl), GFP_KERNEL);
+        if (!ep93xxbl)
+                return -ENOMEM;
+
+        /*
+         * This register is located in the range already ioremap'ed by
+         * the framebuffer driver.  A MFD driver seems a bit of overkill
+         * to handle this so use the static I/O mapping; this address
+         * is already virtual.
+         *
+         * NOTE: No locking is required; the framebuffer does not touch
+         * this register.
+         */
+        ep93xxbl->mmio = EP93XX_RASTER_BRIGHTNESS;
+
+        memset(&props, 0, sizeof(struct backlight_properties));
+        props.max_brightness = EP93XX_MAX_BRIGHT;
+        bl = backlight_device_register(dev->name, &dev->dev, ep93xxbl,
+                                       &ep93xxbl_ops, &props);
+        if (IS_ERR(bl))
+                return PTR_ERR(bl);
+
+        bl->props.brightness = EP93XX_DEF_BRIGHT;
+
+        platform_set_drvdata(dev, bl);
+
+        ep93xxbl_update_status(bl);
+
+        return 0;
+}
+
+static int ep93xxbl_remove(struct platform_device *dev)
+{
+        struct backlight_device *bl = platform_get_drvdata(dev);
+
+        backlight_device_unregister(bl);
+        platform_set_drvdata(dev, NULL);
+        return 0;
+}
+
+#ifdef CONFIG_PM
+static int ep93xxbl_suspend(struct platform_device *dev, pm_message_t state)
+{
+        struct backlight_device *bl = platform_get_drvdata(dev);
+
+        return ep93xxbl_set(bl, 0);
+}
+
+static int ep93xxbl_resume(struct platform_device *dev)
+{
+        struct backlight_device *bl = platform_get_drvdata(dev);
+
+        backlight_update_status(bl);
+        return 0;
+}
+#else
+#define ep93xxbl_suspend        NULL
+#define ep93xxbl_resume         NULL
+#endif
+
+static struct platform_driver ep93xxbl_driver = {
+        .driver         = {
+                .name   = "ep93xx-bl",
+                .owner  = THIS_MODULE,
+        },
+        .probe          = ep93xxbl_probe,
+        .remove         = __devexit_p(ep93xxbl_remove),
+        .suspend        = ep93xxbl_suspend,
+        .resume         = ep93xxbl_resume,
+};
+
+static int __init ep93xxbl_init(void)
+{
+        return platform_driver_register(&ep93xxbl_driver);
+}
+module_init(ep93xxbl_init);
+
+static void __exit ep93xxbl_exit(void)
+{
+        platform_driver_unregister(&ep93xxbl_driver);
+}
+module_exit(ep93xxbl_exit);
+
+MODULE_DESCRIPTION("EP93xx Backlight Driver");
+MODULE_AUTHOR("H Hartley Sweeten <hsweeten@visionengravers.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:ep93xx-bl");
diff --git a/drivers/video/backlight/l4f00242t03.c b/drivers/video/backlight/l4f00242t03.c
index bcdb12c93efd..9093ef0fa869 100644
--- a/drivers/video/backlight/l4f00242t03.c
+++ b/drivers/video/backlight/l4f00242t03.c
@@ -125,8 +125,7 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
 
         if (priv == NULL) {
                 dev_err(&spi->dev, "No memory for this device.\n");
-                ret = -ENOMEM;
-                goto err;
+                return -ENOMEM;
         }
 
         dev_set_drvdata(&spi->dev, priv);
@@ -139,7 +138,7 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
         if (ret) {
                 dev_err(&spi->dev,
                         "Unable to get the lcd l4f00242t03 reset gpio.\n");
-                return ret;
+                goto err;
         }
 
         ret = gpio_direction_output(pdata->reset_gpio, 1);
@@ -151,7 +150,7 @@ static int __devinit l4f00242t03_probe(struct spi_device *spi)
         if (ret) {
                 dev_err(&spi->dev,
                         "Unable to get the lcd l4f00242t03 data en gpio.\n");
-                return ret;
+                goto err2;
         }
 
         ret = gpio_direction_output(pdata->data_enable_gpio, 0);
@@ -222,9 +221,9 @@ static int __devexit l4f00242t03_remove(struct spi_device *spi)
         gpio_free(pdata->reset_gpio);
 
         if (priv->io_reg)
-                regulator_put(priv->core_reg);
-        if (priv->core_reg)
                 regulator_put(priv->io_reg);
+        if (priv->core_reg)
+                regulator_put(priv->core_reg);
 
         kfree(priv);
 
diff --git a/drivers/video/backlight/max8925_bl.c b/drivers/video/backlight/max8925_bl.c
index b5accc957ad3..b2b2c7ba1f63 100644
--- a/drivers/video/backlight/max8925_bl.c
+++ b/drivers/video/backlight/max8925_bl.c
@@ -162,6 +162,7 @@ static int __devinit max8925_backlight_probe(struct platform_device *pdev)
         backlight_update_status(bl);
         return 0;
 out:
+        backlight_device_unregister(bl);
         kfree(data);
         return ret;
 }
diff --git a/drivers/video/backlight/mbp_nvidia_bl.c b/drivers/video/backlight/mbp_nvidia_bl.c
index 1b5d3fe6bbbc..9fb533f6373e 100644
--- a/drivers/video/backlight/mbp_nvidia_bl.c
+++ b/drivers/video/backlight/mbp_nvidia_bl.c
@@ -141,7 +141,7 @@ static const struct dmi_system_id __initdata mbp_device_table[] = {
                 .callback       = mbp_dmi_match,
                 .ident          = "MacBook 1,1",
                 .matches        = {
-                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Computer, Inc."),
                         DMI_MATCH(DMI_PRODUCT_NAME, "MacBook1,1"),
                 },
                 .driver_data    = (void *)&intel_chipset_data,
@@ -184,6 +184,42 @@ static const struct dmi_system_id __initdata mbp_device_table[] = {
         },
         {
                 .callback       = mbp_dmi_match,
+                .ident          = "MacBookPro 1,1",
+                .matches        = {
+                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro1,1"),
+                },
+                .driver_data    = (void *)&intel_chipset_data,
+        },
+        {
+                .callback       = mbp_dmi_match,
+                .ident          = "MacBookPro 1,2",
+                .matches        = {
+                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro1,2"),
+                },
+                .driver_data    = (void *)&intel_chipset_data,
+        },
+        {
+                .callback       = mbp_dmi_match,
+                .ident          = "MacBookPro 2,1",
+                .matches        = {
+                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro2,1"),
+                },
+                .driver_data    = (void *)&intel_chipset_data,
+        },
+        {
+                .callback       = mbp_dmi_match,
+                .ident          = "MacBookPro 2,2",
+                .matches        = {
+                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro2,2"),
+                },
+                .driver_data    = (void *)&intel_chipset_data,
+        },
+        {
+                .callback       = mbp_dmi_match,
                 .ident          = "MacBookPro 3,1",
                 .matches        = {
                         DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
@@ -238,6 +274,15 @@ static const struct dmi_system_id __initdata mbp_device_table[] = {
         },
         {
                 .callback       = mbp_dmi_match,
+                .ident          = "MacBook 6,1",
+                .matches        = {
+                        DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
+                        DMI_MATCH(DMI_PRODUCT_NAME, "MacBook6,1"),
+                },
+                .driver_data    = (void *)&nvidia_chipset_data,
+        },
+        {
+                .callback       = mbp_dmi_match,
                 .ident          = "MacBookAir 2,1",
                 .matches        = {
                         DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc."),
diff --git a/drivers/video/backlight/pcf50633-backlight.c b/drivers/video/backlight/pcf50633-backlight.c
new file mode 100644
index 000000000000..3c424f7efdcc
--- /dev/null
+++ b/drivers/video/backlight/pcf50633-backlight.c
@@ -0,0 +1,190 @@
+/*
+ *  Copyright (C) 2009-2010, Lars-Peter Clausen <lars@metafoo.de>
+ *      PCF50633 backlight device driver
+ *
+ *  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.
+ *
+ *  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/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+
+#include <linux/backlight.h>
+#include <linux/fb.h>
+
+#include <linux/mfd/pcf50633/core.h>
+#include <linux/mfd/pcf50633/backlight.h>
+
+struct pcf50633_bl {
+        struct pcf50633 *pcf;
+        struct backlight_device *bl;
+
+        unsigned int brightness;
+        unsigned int brightness_limit;
+};
+
+/*
+ * pcf50633_bl_set_brightness_limit
+ *
+ * Update the brightness limit for the pc50633 backlight. The actual brightness
+ * will not go above the limit. This is useful to limit power drain for example
+ * on low battery.
+ *
+ * @dev: Pointer to a pcf50633 device
+ * @limit: The brightness limit. Valid values are 0-63
+ */
+int pcf50633_bl_set_brightness_limit(struct pcf50633 *pcf, unsigned int limit)
+{
+        struct pcf50633_bl *pcf_bl = platform_get_drvdata(pcf->bl_pdev);
+
+        if (!pcf_bl)
+                return -ENODEV;
+
+        pcf_bl->brightness_limit = limit & 0x3f;
+        backlight_update_status(pcf_bl->bl);
+
+    return 0;
+}
+
+static int pcf50633_bl_update_status(struct backlight_device *bl)
+{
+        struct pcf50633_bl *pcf_bl = bl_get_data(bl);
+        unsigned int new_brightness;
+
+
+        if (bl->props.state & (BL_CORE_SUSPENDED | BL_CORE_FBBLANK) ||
+                bl->props.power != FB_BLANK_UNBLANK)
+                new_brightness = 0;
+        else if (bl->props.brightness < pcf_bl->brightness_limit)
+                new_brightness = bl->props.brightness;
+        else
+                new_brightness = pcf_bl->brightness_limit;
+
+
+        if (pcf_bl->brightness == new_brightness)
+                return 0;
+
+        if (new_brightness) {
+                pcf50633_reg_write(pcf_bl->pcf, PCF50633_REG_LEDOUT,
+                                        new_brightness);
+                if (!pcf_bl->brightness)
+                        pcf50633_reg_write(pcf_bl->pcf, PCF50633_REG_LEDENA, 1);
+        } else {
+                pcf50633_reg_write(pcf_bl->pcf, PCF50633_REG_LEDENA, 0);
+        }
+
+        pcf_bl->brightness = new_brightness;
+
+        return 0;
+}
+
+static int pcf50633_bl_get_brightness(struct backlight_device *bl)
+{
+        struct pcf50633_bl *pcf_bl = bl_get_data(bl);
+        return pcf_bl->brightness;
+}
+
+static const struct backlight_ops pcf50633_bl_ops = {
+        .get_brightness = pcf50633_bl_get_brightness,
+        .update_status  = pcf50633_bl_update_status,
+        .options        = BL_CORE_SUSPENDRESUME,
+};
+
+static int __devinit pcf50633_bl_probe(struct platform_device *pdev)
+{
+        int ret;
+        struct pcf50633_bl *pcf_bl;
+        struct device *parent = pdev->dev.parent;
+        struct pcf50633_platform_data *pcf50633_data = parent->platform_data;
+        struct pcf50633_bl_platform_data *pdata = pcf50633_data->backlight_data;
+        struct backlight_properties bl_props;
+
+        pcf_bl = kzalloc(sizeof(*pcf_bl), GFP_KERNEL);
+        if (!pcf_bl)
+                return -ENOMEM;
+
+        bl_props.max_brightness = 0x3f;
+        bl_props.power = FB_BLANK_UNBLANK;
+
+        if (pdata) {
+                bl_props.brightness = pdata->default_brightness;
+                pcf_bl->brightness_limit = pdata->default_brightness_limit;
+        } else {
+                bl_props.brightness = 0x3f;
+                pcf_bl->brightness_limit = 0x3f;
+        }
+
+        pcf_bl->pcf = dev_to_pcf50633(pdev->dev.parent);
+
+        pcf_bl->bl = backlight_device_register(pdev->name, &pdev->dev, pcf_bl,
+                                                &pcf50633_bl_ops, &bl_props);
+
+        if (IS_ERR(pcf_bl->bl)) {
+                ret = PTR_ERR(pcf_bl->bl);
+                goto err_free;
+        }
+
+        platform_set_drvdata(pdev, pcf_bl);
+
+        pcf50633_reg_write(pcf_bl->pcf, PCF50633_REG_LEDDIM, pdata->ramp_time);
+
+        /* Should be different from bl_props.brightness, so we do not exit
+         * update_status early the first time it's called */
+        pcf_bl->brightness = pcf_bl->bl->props.brightness + 1;
+
+        backlight_update_status(pcf_bl->bl);
+
+        return 0;
+
+err_free:
+        kfree(pcf_bl);
+
+        return ret;
+}
+
+static int __devexit pcf50633_bl_remove(struct platform_device *pdev)
+{
+        struct pcf50633_bl *pcf_bl = platform_get_drvdata(pdev);
+
+        backlight_device_unregister(pcf_bl->bl);
+
+        platform_set_drvdata(pdev, NULL);
+
+        kfree(pcf_bl);
+
+        return 0;
+}
+
+static struct platform_driver pcf50633_bl_driver = {
+        .probe =        pcf50633_bl_probe,
+        .remove =       __devexit_p(pcf50633_bl_remove),
+        .driver = {
+                .name = "pcf50633-backlight",
+        },
+};
+
+static int __init pcf50633_bl_init(void)
+{
+        return platform_driver_register(&pcf50633_bl_driver);
+}
+module_init(pcf50633_bl_init);
+
+static void __exit pcf50633_bl_exit(void)
+{
+        platform_driver_unregister(&pcf50633_bl_driver);
+}
+module_exit(pcf50633_bl_exit);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("PCF50633 backlight driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:pcf50633-backlight");
diff --git a/drivers/video/backlight/s6e63m0.c b/drivers/video/backlight/s6e63m0.c
new file mode 100644
index 000000000000..a3128c9cb7ad
--- /dev/null
+++ b/drivers/video/backlight/s6e63m0.c
@@ -0,0 +1,920 @@
+/*
+ * S6E63M0 AMOLED LCD panel driver.
+ *
+ * Author: InKi Dae  <inki.dae@samsung.com>
+ *
+ * Derived from drivers/video/omap/lcd-apollon.c
+ *
+ * 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 <linux/wait.h>
+#include <linux/fb.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/spi/spi.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/lcd.h>
+#include <linux/backlight.h>
+
+#include "s6e63m0_gamma.h"
+
+#define SLEEPMSEC               0x1000
+#define ENDDEF                  0x2000
+#define DEFMASK                 0xFF00
+#define COMMAND_ONLY            0xFE
+#define DATA_ONLY               0xFF
+
+#define MIN_BRIGHTNESS          0
+#define MAX_BRIGHTNESS          10
+
+#define POWER_IS_ON(pwr)        ((pwr) <= FB_BLANK_NORMAL)
+
+struct s6e63m0 {
+        struct device                   *dev;
+        struct spi_device               *spi;
+        unsigned int                    power;
+        unsigned int                    current_brightness;
+        unsigned int                    gamma_mode;
+        unsigned int                    gamma_table_count;
+        struct lcd_device               *ld;
+        struct backlight_device         *bd;
+        struct lcd_platform_data        *lcd_pd;
+};
+
+static const unsigned short SEQ_PANEL_CONDITION_SET[] = {
+        0xF8, 0x01,
+        DATA_ONLY, 0x27,
+        DATA_ONLY, 0x27,
+        DATA_ONLY, 0x07,
+        DATA_ONLY, 0x07,
+        DATA_ONLY, 0x54,
+        DATA_ONLY, 0x9f,
+        DATA_ONLY, 0x63,
+        DATA_ONLY, 0x86,
+        DATA_ONLY, 0x1a,
+        DATA_ONLY, 0x33,
+        DATA_ONLY, 0x0d,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+
+        ENDDEF, 0x0000
+};
+
+static const unsigned short SEQ_DISPLAY_CONDITION_SET[] = {
+        0xf2, 0x02,
+        DATA_ONLY, 0x03,
+        DATA_ONLY, 0x1c,
+        DATA_ONLY, 0x10,
+        DATA_ONLY, 0x10,
+
+        0xf7, 0x03,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+
+        ENDDEF, 0x0000
+};
+
+static const unsigned short SEQ_GAMMA_SETTING[] = {
+        0xfa, 0x00,
+        DATA_ONLY, 0x18,
+        DATA_ONLY, 0x08,
+        DATA_ONLY, 0x24,
+        DATA_ONLY, 0x64,
+        DATA_ONLY, 0x56,
+        DATA_ONLY, 0x33,
+        DATA_ONLY, 0xb6,
+        DATA_ONLY, 0xba,
+        DATA_ONLY, 0xa8,
+        DATA_ONLY, 0xac,
+        DATA_ONLY, 0xb1,
+        DATA_ONLY, 0x9d,
+        DATA_ONLY, 0xc1,
+        DATA_ONLY, 0xc1,
+        DATA_ONLY, 0xb7,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x9c,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x9f,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0xd6,
+
+        0xfa, 0x01,
+
+        ENDDEF, 0x0000
+};
+
+static const unsigned short SEQ_ETC_CONDITION_SET[] = {
+        0xf6, 0x00,
+        DATA_ONLY, 0x8c,
+        DATA_ONLY, 0x07,
+
+        0xb3, 0xc,
+
+        0xb5, 0x2c,
+        DATA_ONLY, 0x12,
+        DATA_ONLY, 0x0c,
+        DATA_ONLY, 0x0a,
+        DATA_ONLY, 0x10,
+        DATA_ONLY, 0x0e,
+        DATA_ONLY, 0x17,
+        DATA_ONLY, 0x13,
+        DATA_ONLY, 0x1f,
+        DATA_ONLY, 0x1a,
+        DATA_ONLY, 0x2a,
+        DATA_ONLY, 0x24,
+        DATA_ONLY, 0x1f,
+        DATA_ONLY, 0x1b,
+        DATA_ONLY, 0x1a,
+        DATA_ONLY, 0x17,
+
+        DATA_ONLY, 0x2b,
+        DATA_ONLY, 0x26,
+        DATA_ONLY, 0x22,
+        DATA_ONLY, 0x20,
+        DATA_ONLY, 0x3a,
+        DATA_ONLY, 0x34,
+        DATA_ONLY, 0x30,
+        DATA_ONLY, 0x2c,
+        DATA_ONLY, 0x29,
+        DATA_ONLY, 0x26,
+        DATA_ONLY, 0x25,
+        DATA_ONLY, 0x23,
+        DATA_ONLY, 0x21,
+        DATA_ONLY, 0x20,
+        DATA_ONLY, 0x1e,
+        DATA_ONLY, 0x1e,
+
+        0xb6, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x11,
+        DATA_ONLY, 0x22,
+        DATA_ONLY, 0x33,
+        DATA_ONLY, 0x44,
+        DATA_ONLY, 0x44,
+        DATA_ONLY, 0x44,
+
+        DATA_ONLY, 0x55,
+        DATA_ONLY, 0x55,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+
+        0xb7, 0x2c,
+        DATA_ONLY, 0x12,
+        DATA_ONLY, 0x0c,
+        DATA_ONLY, 0x0a,
+        DATA_ONLY, 0x10,
+        DATA_ONLY, 0x0e,
+        DATA_ONLY, 0x17,
+        DATA_ONLY, 0x13,
+        DATA_ONLY, 0x1f,
+        DATA_ONLY, 0x1a,
+        DATA_ONLY, 0x2a,
+        DATA_ONLY, 0x24,
+        DATA_ONLY, 0x1f,
+        DATA_ONLY, 0x1b,
+        DATA_ONLY, 0x1a,
+        DATA_ONLY, 0x17,
+
+        DATA_ONLY, 0x2b,
+        DATA_ONLY, 0x26,
+        DATA_ONLY, 0x22,
+        DATA_ONLY, 0x20,
+        DATA_ONLY, 0x3a,
+        DATA_ONLY, 0x34,
+        DATA_ONLY, 0x30,
+        DATA_ONLY, 0x2c,
+        DATA_ONLY, 0x29,
+        DATA_ONLY, 0x26,
+        DATA_ONLY, 0x25,
+        DATA_ONLY, 0x23,
+        DATA_ONLY, 0x21,
+        DATA_ONLY, 0x20,
+        DATA_ONLY, 0x1e,
+        DATA_ONLY, 0x1e,
+
+        0xb8, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x11,
+        DATA_ONLY, 0x22,
+        DATA_ONLY, 0x33,
+        DATA_ONLY, 0x44,
+        DATA_ONLY, 0x44,
+        DATA_ONLY, 0x44,
+
+        DATA_ONLY, 0x55,
+        DATA_ONLY, 0x55,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+
+        0xb9, 0x2c,
+        DATA_ONLY, 0x12,
+        DATA_ONLY, 0x0c,
+        DATA_ONLY, 0x0a,
+        DATA_ONLY, 0x10,
+        DATA_ONLY, 0x0e,
+        DATA_ONLY, 0x17,
+        DATA_ONLY, 0x13,
+        DATA_ONLY, 0x1f,
+        DATA_ONLY, 0x1a,
+        DATA_ONLY, 0x2a,
+        DATA_ONLY, 0x24,
+        DATA_ONLY, 0x1f,
+        DATA_ONLY, 0x1b,
+        DATA_ONLY, 0x1a,
+        DATA_ONLY, 0x17,
+
+        DATA_ONLY, 0x2b,
+        DATA_ONLY, 0x26,
+        DATA_ONLY, 0x22,
+        DATA_ONLY, 0x20,
+        DATA_ONLY, 0x3a,
+        DATA_ONLY, 0x34,
+        DATA_ONLY, 0x30,
+        DATA_ONLY, 0x2c,
+        DATA_ONLY, 0x29,
+        DATA_ONLY, 0x26,
+        DATA_ONLY, 0x25,
+        DATA_ONLY, 0x23,
+        DATA_ONLY, 0x21,
+        DATA_ONLY, 0x20,
+        DATA_ONLY, 0x1e,
+        DATA_ONLY, 0x1e,
+
+        0xba, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x11,
+        DATA_ONLY, 0x22,
+        DATA_ONLY, 0x33,
+        DATA_ONLY, 0x44,
+        DATA_ONLY, 0x44,
+        DATA_ONLY, 0x44,
+
+        DATA_ONLY, 0x55,
+        DATA_ONLY, 0x55,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+        DATA_ONLY, 0x66,
+
+        0xc1, 0x4d,
+        DATA_ONLY, 0x96,
+        DATA_ONLY, 0x1d,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x01,
+        DATA_ONLY, 0xdf,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x03,
+        DATA_ONLY, 0x1f,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x00,
+        DATA_ONLY, 0x03,
+        DATA_ONLY, 0x06,
+        DATA_ONLY, 0x09,
+        DATA_ONLY, 0x0d,
+        DATA_ONLY, 0x0f,
+        DATA_ONLY, 0x12,
+        DATA_ONLY, 0x15,
+        DATA_ONLY, 0x18,
+
+        0xb2, 0x10,
+        DATA_ONLY, 0x10,
+        DATA_ONLY, 0x0b,
+        DATA_ONLY, 0x05,
+
+        ENDDEF, 0x0000
+};
+
+static const unsigned short SEQ_ACL_ON[] = {
+        /* ACL on */
+        0xc0, 0x01,
+
+        ENDDEF, 0x0000
+};
+
+static const unsigned short SEQ_ACL_OFF[] = {
+        /* ACL off */
+        0xc0, 0x00,
+
+        ENDDEF, 0x0000
+};
+
+static const unsigned short SEQ_ELVSS_ON[] = {
+        /* ELVSS on */
+        0xb1, 0x0b,
+
+        ENDDEF, 0x0000
+};
+
+static const unsigned short SEQ_ELVSS_OFF[] = {
+        /* ELVSS off */
+        0xb1, 0x0a,
+
+        ENDDEF, 0x0000
+};
+
+static const unsigned short SEQ_STAND_BY_OFF[] = {
+        0x11, COMMAND_ONLY,
+
+        ENDDEF, 0x0000
+};
+
+static const unsigned short SEQ_STAND_BY_ON[] = {
+        0x10, COMMAND_ONLY,
+
+        ENDDEF, 0x0000
+};
+
+static const unsigned short SEQ_DISPLAY_ON[] = {
+        0x29, COMMAND_ONLY,
+
+        ENDDEF, 0x0000
+};
+
+
+static int s6e63m0_spi_write_byte(struct s6e63m0 *lcd, int addr, int data)
+{
+        u16 buf[1];
+        struct spi_message msg;
+
+        struct spi_transfer xfer = {
+                .len            = 2,
+                .tx_buf         = buf,
+        };
+
+        buf[0] = (addr << 8) | data;
+
+        spi_message_init(&msg);
+        spi_message_add_tail(&xfer, &msg);
+
+        return spi_sync(lcd->spi, &msg);
+}
+
+static int s6e63m0_spi_write(struct s6e63m0 *lcd, unsigned char address,
+        unsigned char command)
+{
+        int ret = 0;
+
+        if (address != DATA_ONLY)
+                ret = s6e63m0_spi_write_byte(lcd, 0x0, address);
+        if (command != COMMAND_ONLY)
+                ret = s6e63m0_spi_write_byte(lcd, 0x1, command);
+
+        return ret;
+}
+
+static int s6e63m0_panel_send_sequence(struct s6e63m0 *lcd,
+        const unsigned short *wbuf)
+{
+        int ret = 0, i = 0;
+
+        while ((wbuf[i] & DEFMASK) != ENDDEF) {
+                if ((wbuf[i] & DEFMASK) != SLEEPMSEC) {
+                        ret = s6e63m0_spi_write(lcd, wbuf[i], wbuf[i+1]);
+                        if (ret)
+                                break;
+                } else
+                        udelay(wbuf[i+1]*1000);
+                i += 2;
+        }
+
+        return ret;
+}
+
+static int _s6e63m0_gamma_ctl(struct s6e63m0 *lcd, const unsigned int *gamma)
+{
+        unsigned int i = 0;
+        int ret = 0;
+
+        /* disable gamma table updating. */
+        ret = s6e63m0_spi_write(lcd, 0xfa, 0x00);
+        if (ret) {
+                dev_err(lcd->dev, "failed to disable gamma table updating.\n");
+                goto gamma_err;
+        }
+
+        for (i = 0 ; i < GAMMA_TABLE_COUNT; i++) {
+                ret = s6e63m0_spi_write(lcd, DATA_ONLY, gamma[i]);
+                if (ret) {
+                        dev_err(lcd->dev, "failed to set gamma table.\n");
+                        goto gamma_err;
+                }
+        }
+
+        /* update gamma table. */
+        ret = s6e63m0_spi_write(lcd, 0xfa, 0x01);
+        if (ret)
+                dev_err(lcd->dev, "failed to update gamma table.\n");
+
+gamma_err:
+        return ret;
+}
+
+static int s6e63m0_gamma_ctl(struct s6e63m0 *lcd, int gamma)
+{
+        int ret = 0;
+
+        ret = _s6e63m0_gamma_ctl(lcd, gamma_table.gamma_22_table[gamma]);
+
+        return ret;
+}
+
+
+static int s6e63m0_ldi_init(struct s6e63m0 *lcd)
+{
+        int ret, i;
+        const unsigned short *init_seq[] = {
+                SEQ_PANEL_CONDITION_SET,
+                SEQ_DISPLAY_CONDITION_SET,
+                SEQ_GAMMA_SETTING,
+                SEQ_ETC_CONDITION_SET,
+                SEQ_ACL_ON,
+                SEQ_ELVSS_ON,
+        };
+
+        for (i = 0; i < ARRAY_SIZE(init_seq); i++) {
+                ret = s6e63m0_panel_send_sequence(lcd, init_seq[i]);
+                if (ret)
+                        break;
+        }
+
+        return ret;
+}
+
+static int s6e63m0_ldi_enable(struct s6e63m0 *lcd)
+{
+        int ret = 0, i;
+        const unsigned short *enable_seq[] = {
+                SEQ_STAND_BY_OFF,
+                SEQ_DISPLAY_ON,
+        };
+
+        for (i = 0; i < ARRAY_SIZE(enable_seq); i++) {
+                ret = s6e63m0_panel_send_sequence(lcd, enable_seq[i]);
+                if (ret)
+                        break;
+        }
+
+        return ret;
+}
+
+static int s6e63m0_ldi_disable(struct s6e63m0 *lcd)
+{
+        int ret;
+
+        ret = s6e63m0_panel_send_sequence(lcd, SEQ_STAND_BY_ON);
+
+        return ret;
+}
+
+static int s6e63m0_power_on(struct s6e63m0 *lcd)
+{
+        int ret = 0;
+        struct lcd_platform_data *pd = NULL;
+        struct backlight_device *bd = NULL;
+
+        pd = lcd->lcd_pd;
+        if (!pd) {
+                dev_err(lcd->dev, "platform data is NULL.\n");
+                return -EFAULT;
+        }
+
+        bd = lcd->bd;
+        if (!bd) {
+                dev_err(lcd->dev, "backlight device is NULL.\n");
+                return -EFAULT;
+        }
+
+        if (!pd->power_on) {
+                dev_err(lcd->dev, "power_on is NULL.\n");
+                return -EFAULT;
+        } else {
+                pd->power_on(lcd->ld, 1);
+                mdelay(pd->power_on_delay);
+        }
+
+        if (!pd->reset) {
+                dev_err(lcd->dev, "reset is NULL.\n");
+                return -EFAULT;
+        } else {
+                pd->reset(lcd->ld);
+                mdelay(pd->reset_delay);
+        }
+
+        ret = s6e63m0_ldi_init(lcd);
+        if (ret) {
+                dev_err(lcd->dev, "failed to initialize ldi.\n");
+                return ret;
+        }
+
+        ret = s6e63m0_ldi_enable(lcd);
+        if (ret) {
+                dev_err(lcd->dev, "failed to enable ldi.\n");
+                return ret;
+        }
+
+        /* set brightness to current value after power on or resume. */
+        ret = s6e63m0_gamma_ctl(lcd, bd->props.brightness);
+        if (ret) {
+                dev_err(lcd->dev, "lcd gamma setting failed.\n");
+                return ret;
+        }
+
+        return 0;
+}
+
+static int s6e63m0_power_off(struct s6e63m0 *lcd)
+{
+        int ret = 0;
+        struct lcd_platform_data *pd = NULL;
+
+        pd = lcd->lcd_pd;
+        if (!pd) {
+                dev_err(lcd->dev, "platform data is NULL.\n");
+                return -EFAULT;
+        }
+
+        ret = s6e63m0_ldi_disable(lcd);
+        if (ret) {
+                dev_err(lcd->dev, "lcd setting failed.\n");
+                return -EIO;
+        }
+
+        mdelay(pd->power_off_delay);
+
+        if (!pd->power_on) {
+                dev_err(lcd->dev, "power_on is NULL.\n");
+                return -EFAULT;
+        } else
+                pd->power_on(lcd->ld, 0);
+
+        return 0;
+}
+
+static int s6e63m0_power(struct s6e63m0 *lcd, int power)
+{
+        int ret = 0;
+
+        if (POWER_IS_ON(power) && !POWER_IS_ON(lcd->power))
+                ret = s6e63m0_power_on(lcd);
+        else if (!POWER_IS_ON(power) && POWER_IS_ON(lcd->power))
+                ret = s6e63m0_power_off(lcd);
+
+        if (!ret)
+                lcd->power = power;
+
+        return ret;
+}
+
+static int s6e63m0_set_power(struct lcd_device *ld, int power)
+{
+        struct s6e63m0 *lcd = lcd_get_data(ld);
+
+        if (power != FB_BLANK_UNBLANK && power != FB_BLANK_POWERDOWN &&
+                power != FB_BLANK_NORMAL) {
+                dev_err(lcd->dev, "power value should be 0, 1 or 4.\n");
+                return -EINVAL;
+        }
+
+        return s6e63m0_power(lcd, power);
+}
+
+static int s6e63m0_get_power(struct lcd_device *ld)
+{
+        struct s6e63m0 *lcd = lcd_get_data(ld);
+
+        return lcd->power;
+}
+
+static int s6e63m0_get_brightness(struct backlight_device *bd)
+{
+        return bd->props.brightness;
+}
+
+static int s6e63m0_set_brightness(struct backlight_device *bd)
+{
+        int ret = 0, brightness = bd->props.brightness;
+        struct s6e63m0 *lcd = bl_get_data(bd);
+
+        if (brightness < MIN_BRIGHTNESS ||
+                brightness > bd->props.max_brightness) {
+                dev_err(&bd->dev, "lcd brightness should be %d to %d.\n",
+                        MIN_BRIGHTNESS, MAX_BRIGHTNESS);
+                return -EINVAL;
+        }
+
+        ret = s6e63m0_gamma_ctl(lcd, bd->props.brightness);
+        if (ret) {
+                dev_err(&bd->dev, "lcd brightness setting failed.\n");
+                return -EIO;
+        }
+
+        return ret;
+}
+
+static struct lcd_ops s6e63m0_lcd_ops = {
+        .set_power = s6e63m0_set_power,
+        .get_power = s6e63m0_get_power,
+};
+
+static const struct backlight_ops s6e63m0_backlight_ops  = {
+        .get_brightness = s6e63m0_get_brightness,
+        .update_status = s6e63m0_set_brightness,
+};
+
+static ssize_t s6e63m0_sysfs_show_gamma_mode(struct device *dev,
+                                      struct device_attribute *attr, char *buf)
+{
+        struct s6e63m0 *lcd = dev_get_drvdata(dev);
+        char temp[10];
+
+        switch (lcd->gamma_mode) {
+        case 0:
+                sprintf(temp, "2.2 mode\n");
+                strcat(buf, temp);
+                break;
+        case 1:
+                sprintf(temp, "1.9 mode\n");
+                strcat(buf, temp);
+                break;
+        case 2:
+                sprintf(temp, "1.7 mode\n");
+                strcat(buf, temp);
+                break;
+        default:
+                dev_info(dev, "gamma mode could be 0:2.2, 1:1.9 or 2:1.7)n");
+                break;
+        }
+
+        return strlen(buf);
+}
+
+static ssize_t s6e63m0_sysfs_store_gamma_mode(struct device *dev,
+                                       struct device_attribute *attr,
+                                       const char *buf, size_t len)
+{
+        struct s6e63m0 *lcd = dev_get_drvdata(dev);
+        struct backlight_device *bd = NULL;
+        int brightness, rc;
+
+        rc = strict_strtoul(buf, 0, (unsigned long *)&lcd->gamma_mode);
+        if (rc < 0)
+                return rc;
+
+        bd = lcd->bd;
+
+        brightness = bd->props.brightness;
+
+        switch (lcd->gamma_mode) {
+        case 0:
+                _s6e63m0_gamma_ctl(lcd, gamma_table.gamma_22_table[brightness]);
+                break;
+        case 1:
+                _s6e63m0_gamma_ctl(lcd, gamma_table.gamma_19_table[brightness]);
+                break;
+        case 2:
+                _s6e63m0_gamma_ctl(lcd, gamma_table.gamma_17_table[brightness]);
+                break;
+        default:
+                dev_info(dev, "gamma mode could be 0:2.2, 1:1.9 or 2:1.7\n");
+                _s6e63m0_gamma_ctl(lcd, gamma_table.gamma_22_table[brightness]);
+                break;
+        }
+        return len;
+}
+
+static DEVICE_ATTR(gamma_mode, 0644,
+                s6e63m0_sysfs_show_gamma_mode, s6e63m0_sysfs_store_gamma_mode);
+
+static ssize_t s6e63m0_sysfs_show_gamma_table(struct device *dev,
+                                      struct device_attribute *attr, char *buf)
+{
+        struct s6e63m0 *lcd = dev_get_drvdata(dev);
+        char temp[3];
+
+        sprintf(temp, "%d\n", lcd->gamma_table_count);
+        strcpy(buf, temp);
+
+        return strlen(buf);
+}
+static DEVICE_ATTR(gamma_table, 0644,
+                s6e63m0_sysfs_show_gamma_table, NULL);
+
+static int __init s6e63m0_probe(struct spi_device *spi)
+{
+        int ret = 0;
+        struct s6e63m0 *lcd = NULL;
+        struct lcd_device *ld = NULL;
+        struct backlight_device *bd = NULL;
+
+        lcd = kzalloc(sizeof(struct s6e63m0), GFP_KERNEL);
+        if (!lcd)
+                return -ENOMEM;
+
+        /* s6e63m0 lcd panel uses 3-wire 9bits SPI Mode. */
+        spi->bits_per_word = 9;
+
+        ret = spi_setup(spi);
+        if (ret < 0) {
+                dev_err(&spi->dev, "spi setup failed.\n");
+                goto out_free_lcd;
+        }
+
+        lcd->spi = spi;
+        lcd->dev = &spi->dev;
+
+        lcd->lcd_pd = (struct lcd_platform_data *)spi->dev.platform_data;
+        if (!lcd->lcd_pd) {
+                dev_err(&spi->dev, "platform data is NULL.\n");
+                goto out_free_lcd;
+        }
+
+        ld = lcd_device_register("s6e63m0", &spi->dev, lcd, &s6e63m0_lcd_ops);
+        if (IS_ERR(ld)) {
+                ret = PTR_ERR(ld);
+                goto out_free_lcd;
+        }
+
+        lcd->ld = ld;
+
+        bd = backlight_device_register("s6e63m0bl-bl", &spi->dev, lcd,
+                &s6e63m0_backlight_ops, NULL);
+        if (IS_ERR(bd)) {
+                ret =  PTR_ERR(bd);
+                goto out_lcd_unregister;
+        }
+
+        bd->props.max_brightness = MAX_BRIGHTNESS;
+        bd->props.brightness = MAX_BRIGHTNESS;
+        lcd->bd = bd;
+
+        /*
+         * it gets gamma table count available so it gets user
+         * know that.
+         */
+        lcd->gamma_table_count =
+            sizeof(gamma_table) / (MAX_GAMMA_LEVEL * sizeof(int));
+
+        ret = device_create_file(&(spi->dev), &dev_attr_gamma_mode);
+        if (ret < 0)
+                dev_err(&(spi->dev), "failed to add sysfs entries\n");
+
+        ret = device_create_file(&(spi->dev), &dev_attr_gamma_table);
+        if (ret < 0)
+                dev_err(&(spi->dev), "failed to add sysfs entries\n");
+
+        /*
+         * if lcd panel was on from bootloader like u-boot then
+         * do not lcd on.
+         */
+        if (!lcd->lcd_pd->lcd_enabled) {
+                /*
+                 * if lcd panel was off from bootloader then
+                 * current lcd status is powerdown and then
+                 * it enables lcd panel.
+                 */
+                lcd->power = FB_BLANK_POWERDOWN;
+
+                s6e63m0_power(lcd, FB_BLANK_UNBLANK);
+        } else
+                lcd->power = FB_BLANK_UNBLANK;
+
+        dev_set_drvdata(&spi->dev, lcd);
+
+        dev_info(&spi->dev, "s6e63m0 panel driver has been probed.\n");
+
+        return 0;
+
+out_lcd_unregister:
+        lcd_device_unregister(ld);
+out_free_lcd:
+        kfree(lcd);
+        return ret;
+}
+
+static int __devexit s6e63m0_remove(struct spi_device *spi)
+{
+        struct s6e63m0 *lcd = dev_get_drvdata(&spi->dev);
+
+        s6e63m0_power(lcd, FB_BLANK_POWERDOWN);
+        lcd_device_unregister(lcd->ld);
+        kfree(lcd);
+
+        return 0;
+}
+
+#if defined(CONFIG_PM)
+unsigned int before_power;
+
+static int s6e63m0_suspend(struct spi_device *spi, pm_message_t mesg)
+{
+        int ret = 0;
+        struct s6e63m0 *lcd = dev_get_drvdata(&spi->dev);
+
+        dev_dbg(&spi->dev, "lcd->power = %d\n", lcd->power);
+
+        before_power = lcd->power;
+
+        /*
+         * when lcd panel is suspend, lcd panel becomes off
+         * regardless of status.
+         */
+        ret = s6e63m0_power(lcd, FB_BLANK_POWERDOWN);
+
+        return ret;
+}
+
+static int s6e63m0_resume(struct spi_device *spi)
+{
+        int ret = 0;
+        struct s6e63m0 *lcd = dev_get_drvdata(&spi->dev);
+
+        /*
+         * after suspended, if lcd panel status is FB_BLANK_UNBLANK
+         * (at that time, before_power is FB_BLANK_UNBLANK) then
+         * it changes that status to FB_BLANK_POWERDOWN to get lcd on.
+         */
+        if (before_power == FB_BLANK_UNBLANK)
+                lcd->power = FB_BLANK_POWERDOWN;
+
+        dev_dbg(&spi->dev, "before_power = %d\n", before_power);
+
+        ret = s6e63m0_power(lcd, before_power);
+
+        return ret;
+}
+#else
+#define s6e63m0_suspend         NULL
+#define s6e63m0_resume          NULL
+#endif
+
+/* Power down all displays on reboot, poweroff or halt. */
+static void s6e63m0_shutdown(struct spi_device *spi)
+{
+        struct s6e63m0 *lcd = dev_get_drvdata(&spi->dev);
+
+        s6e63m0_power(lcd, FB_BLANK_POWERDOWN);
+}
+
+static struct spi_driver s6e63m0_driver = {
+        .driver = {
+                .name   = "s6e63m0",
+                .bus    = &spi_bus_type,
+                .owner  = THIS_MODULE,
+        },
+        .probe          = s6e63m0_probe,
+        .remove         = __devexit_p(s6e63m0_remove),
+        .shutdown       = s6e63m0_shutdown,
+        .suspend        = s6e63m0_suspend,
+        .resume         = s6e63m0_resume,
+};
+
+static int __init s6e63m0_init(void)
+{
+        return spi_register_driver(&s6e63m0_driver);
+}
+
+static void __exit s6e63m0_exit(void)
+{
+        spi_unregister_driver(&s6e63m0_driver);
+}
+
+module_init(s6e63m0_init);
+module_exit(s6e63m0_exit);
+
+MODULE_AUTHOR("InKi Dae <inki.dae@samsung.com>");
+MODULE_DESCRIPTION("S6E63M0 LCD Driver");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/video/backlight/s6e63m0_gamma.h b/drivers/video/backlight/s6e63m0_gamma.h
new file mode 100644
index 000000000000..2c44bdb0696b
--- /dev/null
+++ b/drivers/video/backlight/s6e63m0_gamma.h
@@ -0,0 +1,266 @@
+/* linux/drivers/video/samsung/s6e63m0_brightness.h
+ *
+ * Gamma level definitions.
+ *
+ * Copyright (c) 2009 Samsung Electronics
+ * InKi Dae <inki.dae@samsung.com>
+ *
+ * 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.
+*/
+
+#ifndef _S6E63M0_BRIGHTNESS_H
+#define _S6E63M0_BRIGHTNESS_H
+
+#define MAX_GAMMA_LEVEL         11
+#define GAMMA_TABLE_COUNT       21
+
+/* gamma value: 2.2 */
+static const unsigned int s6e63m0_22_300[] = {
+        0x18, 0x08, 0x24, 0x5f, 0x50, 0x2d, 0xB6,
+        0xB9, 0xA7, 0xAd, 0xB1, 0x9f, 0xbe, 0xC0,
+        0xB5, 0x00, 0xa0, 0x00, 0xa4, 0x00, 0xdb
+};
+
+static const unsigned int s6e63m0_22_280[] = {
+        0x18, 0x08, 0x24, 0x64, 0x56, 0x33, 0xB6,
+        0xBA, 0xA8, 0xAC, 0xB1, 0x9D, 0xC1, 0xC1,
+        0xB7, 0x00, 0x9C, 0x00, 0x9F, 0x00, 0xD6
+};
+
+static const unsigned int s6e63m0_22_260[] = {
+        0x18, 0x08, 0x24, 0x66, 0x58, 0x34, 0xB6,
+        0xBA, 0xA7, 0xAF, 0xB3, 0xA0, 0xC1, 0xC2,
+        0xB7, 0x00, 0x97, 0x00, 0x9A, 0x00, 0xD1
+
+};
+
+static const unsigned int s6e63m0_22_240[] = {
+        0x18, 0x08, 0x24, 0x62, 0x54, 0x30, 0xB9,
+        0xBB, 0xA9, 0xB0, 0xB3, 0xA1, 0xC1, 0xC3,
+        0xB7, 0x00, 0x91, 0x00, 0x95, 0x00, 0xDA
+
+};
+static const unsigned int s6e63m0_22_220[] = {
+        0x18, 0x08, 0x24, 0x63, 0x53, 0x31, 0xB8,
+        0xBC, 0xA9, 0xB0, 0xB5, 0xA2, 0xC4, 0xC4,
+        0xB8, 0x00, 0x8B, 0x00, 0x8E, 0x00, 0xC2
+};
+
+static const unsigned int s6e63m0_22_200[] = {
+        0x18, 0x08, 0x24, 0x66, 0x55, 0x34, 0xBA,
+        0xBD, 0xAB, 0xB1, 0xB5, 0xA3, 0xC5, 0xC6,
+        0xB9, 0x00, 0x85, 0x00, 0x88, 0x00, 0xBA
+};
+
+static const unsigned int s6e63m0_22_170[] = {
+        0x18, 0x08, 0x24, 0x69, 0x54, 0x37, 0xBB,
+        0xBE, 0xAC, 0xB4, 0xB7, 0xA6, 0xC7, 0xC8,
+        0xBC, 0x00, 0x7B, 0x00, 0x7E, 0x00, 0xAB
+};
+
+static const unsigned int s6e63m0_22_140[] = {
+        0x18, 0x08, 0x24, 0x6C, 0x54, 0x3A, 0xBC,
+        0xBF, 0xAC, 0xB7, 0xBB, 0xA9, 0xC9, 0xC9,
+        0xBE, 0x00, 0x71, 0x00, 0x73, 0x00, 0x9E
+};
+
+static const unsigned int s6e63m0_22_110[] = {
+        0x18, 0x08, 0x24, 0x70, 0x51, 0x3E, 0xBF,
+        0xC1, 0xAF, 0xB9, 0xBC, 0xAB, 0xCC, 0xCC,
+        0xC2, 0x00, 0x65, 0x00, 0x67, 0x00, 0x8D
+};
+
+static const unsigned int s6e63m0_22_90[] = {
+        0x18, 0x08, 0x24, 0x73, 0x4A, 0x3D, 0xC0,
+        0xC2, 0xB1, 0xBB, 0xBE, 0xAC, 0xCE, 0xCF,
+        0xC5, 0x00, 0x5D, 0x00, 0x5E, 0x00, 0x82
+};
+
+static const unsigned int s6e63m0_22_30[] = {
+        0x18, 0x08, 0x24, 0x78, 0xEC, 0x3D, 0xC8,
+        0xC2, 0xB6, 0xC4, 0xC7, 0xB6, 0xD5, 0xD7,
+        0xCC, 0x00, 0x39, 0x00, 0x36, 0x00, 0x51
+};
+
+/* gamma value: 1.9 */
+static const unsigned int s6e63m0_19_300[] = {
+        0x18, 0x08, 0x24, 0x61, 0x5F, 0x39, 0xBA,
+        0xBD, 0xAD, 0xB1, 0xB6, 0xA5, 0xC4, 0xC5,
+        0xBC, 0x00, 0xA0, 0x00, 0xA4, 0x00, 0xDB
+};
+
+static const unsigned int s6e63m0_19_280[] = {
+        0x18, 0x08, 0x24, 0x61, 0x60, 0x39, 0xBB,
+        0xBE, 0xAD, 0xB2, 0xB6, 0xA6, 0xC5, 0xC7,
+        0xBD, 0x00, 0x9B, 0x00, 0x9E, 0x00, 0xD5
+};
+
+static const unsigned int s6e63m0_19_260[] = {
+        0x18, 0x08, 0x24, 0x63, 0x61, 0x3B, 0xBA,
+        0xBE, 0xAC, 0xB3, 0xB8, 0xA7, 0xC6, 0xC8,
+        0xBD, 0x00, 0x96, 0x00, 0x98, 0x00, 0xCF
+};
+
+static const unsigned int s6e63m0_19_240[] = {
+        0x18, 0x08, 0x24, 0x67, 0x64, 0x3F, 0xBB,
+        0xBE, 0xAD, 0xB3, 0xB9, 0xA7, 0xC8, 0xC9,
+        0xBE, 0x00, 0x90, 0x00, 0x92, 0x00, 0xC8
+};
+
+static const unsigned int s6e63m0_19_220[] = {
+        0x18, 0x08, 0x24, 0x68, 0x64, 0x40, 0xBC,
+        0xBF, 0xAF, 0xB4, 0xBA, 0xA9, 0xC8, 0xCA,
+        0xBE, 0x00, 0x8B, 0x00, 0x8C, 0x00, 0xC0
+};
+
+static const unsigned int s6e63m0_19_200[] = {
+        0x18, 0x08, 0x24, 0x68, 0x64, 0x3F, 0xBE,
+        0xC0, 0xB0, 0xB6, 0xBB, 0xAB, 0xC8, 0xCB,
+        0xBF, 0x00, 0x85, 0x00, 0x86, 0x00, 0xB8
+};
+
+static const unsigned int s6e63m0_19_170[] = {
+        0x18, 0x08, 0x24, 0x69, 0x64, 0x40, 0xBF,
+        0xC1, 0xB0, 0xB9, 0xBE, 0xAD, 0xCB, 0xCD,
+        0xC2, 0x00, 0x7A, 0x00, 0x7B, 0x00, 0xAA
+};
+
+static const unsigned int s6e63m0_19_140[] = {
+        0x18, 0x08, 0x24, 0x6E, 0x65, 0x45, 0xC0,
+        0xC3, 0xB2, 0xBA, 0xBE, 0xAE, 0xCD, 0xD0,
+        0xC4, 0x00, 0x70, 0x00, 0x70, 0x00, 0x9C
+};
+
+static const unsigned int s6e63m0_19_110[] = {
+        0x18, 0x08, 0x24, 0x6F, 0x65, 0x46, 0xC2,
+        0xC4, 0xB3, 0xBF, 0xC2, 0xB2, 0xCF, 0xD1,
+        0xC6, 0x00, 0x64, 0x00, 0x64, 0x00, 0x8D
+};
+
+static const unsigned int s6e63m0_19_90[] = {
+        0x18, 0x08, 0x24, 0x74, 0x60, 0x4A, 0xC3,
+        0xC6, 0xB5, 0xBF, 0xC3, 0xB2, 0xD2, 0xD3,
+        0xC8, 0x00, 0x5B, 0x00, 0x5B, 0x00, 0x81
+};
+
+static const unsigned int s6e63m0_19_30[] = {
+        0x18, 0x08, 0x24, 0x84, 0x45, 0x4F, 0xCA,
+        0xCB, 0xBC, 0xC9, 0xCB, 0xBC, 0xDA, 0xDA,
+        0xD0, 0x00, 0x35, 0x00, 0x34, 0x00, 0x4E
+};
+
+/* gamma value: 1.7 */
+static const unsigned int s6e63m0_17_300[] = {
+        0x18, 0x08, 0x24, 0x70, 0x70, 0x4F, 0xBF,
+        0xC2, 0xB2, 0xB8, 0xBC, 0xAC, 0xCB, 0xCD,
+        0xC3, 0x00, 0xA0, 0x00, 0xA4, 0x00, 0xDB
+};
+
+static const unsigned int s6e63m0_17_280[] = {
+        0x18, 0x08, 0x24, 0x71, 0x71, 0x50, 0xBF,
+        0xC2, 0xB2, 0xBA, 0xBE, 0xAE, 0xCB, 0xCD,
+        0xC3, 0x00, 0x9C, 0x00, 0x9F, 0x00, 0xD6
+};
+
+static const unsigned int s6e63m0_17_260[] = {
+        0x18, 0x08, 0x24, 0x72, 0x72, 0x50, 0xC0,
+        0xC3, 0xB4, 0xB9, 0xBE, 0xAE, 0xCC, 0xCF,
+        0xC4, 0x00, 0x97, 0x00, 0x9A, 0x00, 0xD1
+};
+
+static const unsigned int s6e63m0_17_240[] = {
+        0x18, 0x08, 0x24, 0x71, 0x72, 0x4F, 0xC2,
+        0xC4, 0xB5, 0xBB, 0xBF, 0xB0, 0xCC, 0xCF,
+        0xC3, 0x00, 0x91, 0x00, 0x95, 0x00, 0xCA
+};
+
+static const unsigned int s6e63m0_17_220[] = {
+        0x18, 0x08, 0x24, 0x71, 0x73, 0x4F, 0xC2,
+        0xC5, 0xB5, 0xBD, 0xC0, 0xB2, 0xCD, 0xD1,
+        0xC5, 0x00, 0x8B, 0x00, 0x8E, 0x00, 0xC2
+};
+
+static const unsigned int s6e63m0_17_200[] = {
+        0x18, 0x08, 0x24, 0x72, 0x75, 0x51, 0xC2,
+        0xC6, 0xB5, 0xBF, 0xC1, 0xB3, 0xCE, 0xD1,
+        0xC6, 0x00, 0x85, 0x00, 0x88, 0x00, 0xBA
+};
+
+static const unsigned int s6e63m0_17_170[] = {
+        0x18, 0x08, 0x24, 0x75, 0x77, 0x54, 0xC3,
+        0xC7, 0xB7, 0xC0, 0xC3, 0xB4, 0xD1, 0xD3,
+        0xC9, 0x00, 0x7B, 0x00, 0x7E, 0x00, 0xAB
+};
+
+static const unsigned int s6e63m0_17_140[] = {
+        0x18, 0x08, 0x24, 0x7B, 0x77, 0x58, 0xC3,
+        0xC8, 0xB8, 0xC2, 0xC6, 0xB6, 0xD3, 0xD4,
+        0xCA, 0x00, 0x71, 0x00, 0x73, 0x00, 0x9E
+};
+
+static const unsigned int s6e63m0_17_110[] = {
+        0x18, 0x08, 0x24, 0x81, 0x7B, 0x5D, 0xC6,
+        0xCA, 0xBB, 0xC3, 0xC7, 0xB8, 0xD6, 0xD8,
+        0xCD, 0x00, 0x65, 0x00, 0x67, 0x00, 0x8D
+};
+
+static const unsigned int s6e63m0_17_90[] = {
+        0x18, 0x08, 0x24, 0x82, 0x7A, 0x5B, 0xC8,
+        0xCB, 0xBD, 0xC5, 0xCA, 0xBA, 0xD6, 0xD8,
+        0xCE, 0x00, 0x5D, 0x00, 0x5E, 0x00, 0x82
+};
+
+static const unsigned int s6e63m0_17_30[] = {
+        0x18, 0x08, 0x24, 0x8F, 0x73, 0x63, 0xD1,
+        0xD0, 0xC5, 0xCC, 0xD1, 0xC2, 0xDE, 0xE0,
+        0xD6, 0x00, 0x39, 0x00, 0x36, 0x00, 0x51
+};
+
+struct s6e63m0_gamma {
+        unsigned int *gamma_22_table[MAX_GAMMA_LEVEL];
+        unsigned int *gamma_19_table[MAX_GAMMA_LEVEL];
+        unsigned int *gamma_17_table[MAX_GAMMA_LEVEL];
+};
+
+static struct s6e63m0_gamma gamma_table = {
+        .gamma_22_table[0] = (unsigned int *)&s6e63m0_22_30,
+        .gamma_22_table[1] = (unsigned int *)&s6e63m0_22_90,
+        .gamma_22_table[2] = (unsigned int *)&s6e63m0_22_110,
+        .gamma_22_table[3] = (unsigned int *)&s6e63m0_22_140,
+        .gamma_22_table[4] = (unsigned int *)&s6e63m0_22_170,
+        .gamma_22_table[5] = (unsigned int *)&s6e63m0_22_200,
+        .gamma_22_table[6] = (unsigned int *)&s6e63m0_22_220,
+        .gamma_22_table[7] = (unsigned int *)&s6e63m0_22_240,
+        .gamma_22_table[8] = (unsigned int *)&s6e63m0_22_260,
+        .gamma_22_table[9] = (unsigned int *)&s6e63m0_22_280,
+        .gamma_22_table[10] = (unsigned int *)&s6e63m0_22_300,
+
+        .gamma_19_table[0] = (unsigned int *)&s6e63m0_19_30,
+        .gamma_19_table[1] = (unsigned int *)&s6e63m0_19_90,
+        .gamma_19_table[2] = (unsigned int *)&s6e63m0_19_110,
+        .gamma_19_table[3] = (unsigned int *)&s6e63m0_19_140,
+        .gamma_19_table[4] = (unsigned int *)&s6e63m0_19_170,
+        .gamma_19_table[5] = (unsigned int *)&s6e63m0_19_200,
+        .gamma_19_table[6] = (unsigned int *)&s6e63m0_19_220,
+        .gamma_19_table[7] = (unsigned int *)&s6e63m0_19_240,
+        .gamma_19_table[8] = (unsigned int *)&s6e63m0_19_260,
+        .gamma_19_table[9] = (unsigned int *)&s6e63m0_19_280,
+        .gamma_19_table[10] = (unsigned int *)&s6e63m0_19_300,
+
+        .gamma_17_table[0] = (unsigned int *)&s6e63m0_17_30,
+        .gamma_17_table[1] = (unsigned int *)&s6e63m0_17_90,
+        .gamma_17_table[2] = (unsigned int *)&s6e63m0_17_110,
+        .gamma_17_table[3] = (unsigned int *)&s6e63m0_17_140,
+        .gamma_17_table[4] = (unsigned int *)&s6e63m0_17_170,
+        .gamma_17_table[5] = (unsigned int *)&s6e63m0_17_200,
+        .gamma_17_table[6] = (unsigned int *)&s6e63m0_17_220,
+        .gamma_17_table[7] = (unsigned int *)&s6e63m0_17_240,
+        .gamma_17_table[8] = (unsigned int *)&s6e63m0_17_260,
+        .gamma_17_table[9] = (unsigned int *)&s6e63m0_17_280,
+        .gamma_17_table[10] = (unsigned int *)&s6e63m0_17_300,
+};
+
+#endif
+
diff --git a/drivers/video/backlight/tosa_bl.c b/drivers/video/backlight/tosa_bl.c
index e03e60bbfd85..2a04b382ec48 100644
--- a/drivers/video/backlight/tosa_bl.c
+++ b/drivers/video/backlight/tosa_bl.c
@@ -119,7 +119,6 @@ static int __devinit tosa_bl_probe(struct i2c_client *client,
 
 err_reg:
         data->bl = NULL;
-        i2c_set_clientdata(client, NULL);
 err_gpio_dir:
         gpio_free(TOSA_GPIO_BL_C20MA);
 err_gpio_bl:
@@ -133,7 +132,6 @@ static int __devexit tosa_bl_remove(struct i2c_client *client)
 
         backlight_device_unregister(data->bl);
         data->bl = NULL;
-        i2c_set_clientdata(client, NULL);
 
         gpio_free(TOSA_GPIO_BL_C20MA);
 
diff --git a/drivers/video/bf54x-lq043fb.c b/drivers/video/bf54x-lq043fb.c
index 23b2a8c0dbfc..b020ba7f1cf2 100644
--- a/drivers/video/bf54x-lq043fb.c
+++ b/drivers/video/bf54x-lq043fb.c
@@ -501,7 +501,9 @@ static irqreturn_t bfin_bf54x_irq_error(int irq, void *dev_id)
 
 static int __devinit bfin_bf54x_probe(struct platform_device *pdev)
 {
+#ifndef NO_BL_SUPPORT
         struct backlight_properties props;
+#endif
         struct bfin_bf54xfb_info *info;
         struct fb_info *fbinfo;
         int ret;
@@ -654,7 +656,8 @@ static int __devinit bfin_bf54x_probe(struct platform_device *pdev)
                 printk(KERN_ERR DRIVER_NAME
                         ": unable to register backlight.\n");
                 ret = -EINVAL;
-                goto out9;
+                unregister_framebuffer(fbinfo);
+                goto out8;
         }
 
         lcd_dev = lcd_device_register(DRIVER_NAME, &pdev->dev, NULL, &bfin_lcd_ops);
@@ -663,8 +666,6 @@ static int __devinit bfin_bf54x_probe(struct platform_device *pdev)
 
         return 0;
 
-out9:
-        unregister_framebuffer(fbinfo);
 out8:
         free_irq(info->irq, info);
 out7:
diff --git a/drivers/video/bfin-t350mcqb-fb.c b/drivers/video/bfin-t350mcqb-fb.c
index c2ec3dcd4e91..7a50272eaab9 100644
--- a/drivers/video/bfin-t350mcqb-fb.c
+++ b/drivers/video/bfin-t350mcqb-fb.c
@@ -420,7 +420,9 @@ static irqreturn_t bfin_t350mcqb_irq_error(int irq, void *dev_id)
 
 static int __devinit bfin_t350mcqb_probe(struct platform_device *pdev)
 {
+#ifndef NO_BL_SUPPORT
         struct backlight_properties props;
+#endif
         struct bfin_t350mcqbfb_info *info;
         struct fb_info *fbinfo;
         int ret;
@@ -550,7 +552,8 @@ static int __devinit bfin_t350mcqb_probe(struct platform_device *pdev)
                 printk(KERN_ERR DRIVER_NAME
                         ": unable to register backlight.\n");
                 ret = -EINVAL;
-                goto out9;
+                unregister_framebuffer(fbinfo);
+                goto out8;
         }
 
         lcd_dev = lcd_device_register(DRIVER_NAME, NULL, &bfin_lcd_ops);
@@ -559,8 +562,6 @@ static int __devinit bfin_t350mcqb_probe(struct platform_device *pdev)
 
         return 0;
 
-out9:
-        unregister_framebuffer(fbinfo);
 out8:
         free_irq(info->irq, info);
 out7:
diff --git a/drivers/video/bw2.c b/drivers/video/bw2.c
index 2c371c07f0da..09f1b9b462f4 100644
--- a/drivers/video/bw2.c
+++ b/drivers/video/bw2.c
@@ -275,7 +275,7 @@ static int __devinit bw2_do_default_mode(struct bw2_par *par,
 
 static int __devinit bw2_probe(struct of_device *op, const struct of_device_id *match)
 {
-        struct device_node *dp = op->node;
+        struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
         struct bw2_par *par;
         int linebytes, err;
diff --git a/drivers/video/cg14.c b/drivers/video/cg14.c
index d12e05b6e63f..e5dc2241194f 100644
--- a/drivers/video/cg14.c
+++ b/drivers/video/cg14.c
@@ -465,7 +465,7 @@ static void cg14_unmap_regs(struct of_device *op, struct fb_info *info,
 
 static int __devinit cg14_probe(struct of_device *op, const struct of_device_id *match)
 {
-        struct device_node *dp = op->node;
+        struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
         struct cg14_par *par;
         int is_8mb, linebytes, i, err;
diff --git a/drivers/video/cg3.c b/drivers/video/cg3.c
index b98f93f7f663..558d73a948a0 100644
--- a/drivers/video/cg3.c
+++ b/drivers/video/cg3.c
@@ -349,7 +349,7 @@ static int __devinit cg3_do_default_mode(struct cg3_par *par)
 static int __devinit cg3_probe(struct of_device *op,
                                const struct of_device_id *match)
 {
-        struct device_node *dp = op->node;
+        struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
         struct cg3_par *par;
         int linebytes, err;
diff --git a/drivers/video/fb_defio.c b/drivers/video/fb_defio.c
index 1105a591dcc1..6b93ef93cb12 100644
--- a/drivers/video/fb_defio.c
+++ b/drivers/video/fb_defio.c
@@ -66,7 +66,7 @@ static int fb_deferred_io_fault(struct vm_area_struct *vma,
         return 0;
 }
 
-int fb_deferred_io_fsync(struct file *file, struct dentry *dentry, int datasync)
+int fb_deferred_io_fsync(struct file *file, int datasync)
 {
         struct fb_info *info = file->private_data;
 
@@ -100,6 +100,16 @@ static int fb_deferred_io_mkwrite(struct vm_area_struct *vma,
         /* protect against the workqueue changing the page list */
         mutex_lock(&fbdefio->lock);
 
+        /*
+         * We want the page to remain locked from ->page_mkwrite until
+         * the PTE is marked dirty to avoid page_mkclean() being called
+         * before the PTE is updated, which would leave the page ignored
+         * by defio.
+         * Do this by locking the page here and informing the caller
+         * about it with VM_FAULT_LOCKED.
+         */
+        lock_page(page);
+
         /* we loop through the pagelist before adding in order
         to keep the pagelist sorted */
         list_for_each_entry(cur, &fbdefio->pagelist, lru) {
@@ -121,7 +131,7 @@ page_already_added:
 
         /* come back after delay to process the deferred IO */
         schedule_delayed_work(&info->deferred_work, fbdefio->delay);
-        return 0;
+        return VM_FAULT_LOCKED;
 }
 
 static const struct vm_operations_struct fb_deferred_io_vm_ops = {
@@ -155,41 +165,25 @@ static void fb_deferred_io_work(struct work_struct *work)
 {
         struct fb_info *info = container_of(work, struct fb_info,
                                                 deferred_work.work);
+        struct list_head *node, *next;
+        struct page *cur;
         struct fb_deferred_io *fbdefio = info->fbdefio;
-        struct page *page, *tmp_page;
-        struct list_head *node, *tmp_node;
-        struct list_head non_dirty;
-
-        INIT_LIST_HEAD(&non_dirty);
 
         /* here we mkclean the pages, then do all deferred IO */
         mutex_lock(&fbdefio->lock);
-        list_for_each_entry_safe(page, tmp_page, &fbdefio->pagelist, lru) {
-                lock_page(page);
-                /*
-                 * The workqueue callback can be triggered after a
-                 * ->page_mkwrite() call but before the PTE has been marked
-                 * dirty. In this case page_mkclean() won't "rearm" the page.
-                 *
-                 * To avoid this, remove those "non-dirty" pages from the
-                 * pagelist before calling the driver's callback, then add
-                 * them back to get processed on the next work iteration.
-                 * At that time, their PTEs will hopefully be dirty for real.
-                 */
-                if (!page_mkclean(page))
-                        list_move_tail(&page->lru, &non_dirty);
-                unlock_page(page);
+        list_for_each_entry(cur, &fbdefio->pagelist, lru) {
+                lock_page(cur);
+                page_mkclean(cur);
+                unlock_page(cur);
         }
 
         /* driver's callback with pagelist */
         fbdefio->deferred_io(info, &fbdefio->pagelist);
 
-        /* clear the list... */
-        list_for_each_safe(node, tmp_node, &fbdefio->pagelist) {
+        /* clear the list */
+        list_for_each_safe(node, next, &fbdefio->pagelist) {
                 list_del(node);
         }
-        /* ... and add back the "non-dirty" pages to the list */
-        list_splice_tail(&non_dirty, &fbdefio->pagelist);
         mutex_unlock(&fbdefio->lock);
 }
 
@@ -218,7 +212,6 @@ EXPORT_SYMBOL_GPL(fb_deferred_io_open);
 void fb_deferred_io_cleanup(struct fb_info *info)
 {
         struct fb_deferred_io *fbdefio = info->fbdefio;
-        struct list_head *node, *tmp_node;
         struct page *page;
         int i;
 
@@ -226,13 +219,6 @@ void fb_deferred_io_cleanup(struct fb_info *info)
         cancel_delayed_work(&info->deferred_work);
         flush_scheduled_work();
 
-        /*  the list may have still some non-dirty pages at this point */
-        mutex_lock(&fbdefio->lock);
-        list_for_each_safe(node, tmp_node, &fbdefio->pagelist) {
-                list_del(node);
-        }
-        mutex_unlock(&fbdefio->lock);
-
         /* clear out the mapping that we setup */
         for (i = 0 ; i < info->fix.smem_len; i += PAGE_SIZE) {
                 page = fb_deferred_io_page(info, i);
diff --git a/drivers/video/leo.c b/drivers/video/leo.c
index 3d7895316eaf..9e8bf7d5e249 100644
--- a/drivers/video/leo.c
+++ b/drivers/video/leo.c
@@ -550,7 +550,7 @@ static void leo_unmap_regs(struct of_device *op, struct fb_info *info,
 static int __devinit leo_probe(struct of_device *op,
                                const struct of_device_id *match)
 {
-        struct device_node *dp = op->node;
+        struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
         struct leo_par *par;
         int linebytes, err;
diff --git a/drivers/video/mb862xx/mb862xxfb.c b/drivers/video/mb862xx/mb862xxfb.c
index 0540de4f5cb4..4e2b8cc3d460 100644
--- a/drivers/video/mb862xx/mb862xxfb.c
+++ b/drivers/video/mb862xx/mb862xxfb.c
@@ -553,7 +553,7 @@ static int mb862xx_gdc_init(struct mb862xxfb_par *par)
 static int __devinit of_platform_mb862xx_probe(struct of_device *ofdev,
                                                const struct of_device_id *id)
 {
-        struct device_node *np = ofdev->node;
+        struct device_node *np = ofdev->dev.of_node;
         struct device *dev = &ofdev->dev;
         struct mb862xxfb_par *par;
         struct fb_info *info;
diff --git a/drivers/video/p9100.c b/drivers/video/p9100.c
index c85dd408a9b8..6552751e81aa 100644
--- a/drivers/video/p9100.c
+++ b/drivers/video/p9100.c
@@ -251,7 +251,7 @@ static void p9100_init_fix(struct fb_info *info, int linebytes, struct device_no
 
 static int __devinit p9100_probe(struct of_device *op, const struct of_device_id *match)
 {
-        struct device_node *dp = op->node;
+        struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
         struct p9100_par *par;
         int linebytes, err;
diff --git a/drivers/video/s3fb.c b/drivers/video/s3fb.c
index d4471b4c0374..dce8c97b4333 100644
--- a/drivers/video/s3fb.c
+++ b/drivers/video/s3fb.c
@@ -71,7 +71,8 @@ static const char * const s3_names[] = {"S3 Unknown", "S3 Trio32", "S3 Trio64",
                         "S3 Trio64UV+", "S3 Trio64V2/DX", "S3 Trio64V2/GX",
                         "S3 Plato/PX", "S3 Aurora64VP", "S3 Virge",
                         "S3 Virge/VX", "S3 Virge/DX", "S3 Virge/GX",
-                        "S3 Virge/GX2", "S3 Virge/GX2P", "S3 Virge/GX2P"};
+                        "S3 Virge/GX2", "S3 Virge/GX2P", "S3 Virge/GX2P",
+                        "S3 Trio3D/1X", "S3 Trio3D/2X", "S3 Trio3D/2X"};
 
 #define CHIP_UNKNOWN            0x00
 #define CHIP_732_TRIO32         0x01
@@ -89,10 +90,14 @@ static const char * const s3_names[] = {"S3 Unknown", "S3 Trio32", "S3 Trio64",
 #define CHIP_356_VIRGE_GX2      0x0D
 #define CHIP_357_VIRGE_GX2P     0x0E
 #define CHIP_359_VIRGE_GX2P     0x0F
+#define CHIP_360_TRIO3D_1X      0x10
+#define CHIP_362_TRIO3D_2X      0x11
+#define CHIP_368_TRIO3D_2X      0x12
 
 #define CHIP_XXX_TRIO           0x80
 #define CHIP_XXX_TRIO64V2_DXGX  0x81
 #define CHIP_XXX_VIRGE_DXGX     0x82
+#define CHIP_36X_TRIO3D_1X_2X   0x83
 
 #define CHIP_UNDECIDED_FLAG     0x80
 #define CHIP_MASK               0xFF
@@ -324,6 +329,7 @@ static void s3fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 
 static void s3_set_pixclock(struct fb_info *info, u32 pixclock)
 {
+        struct s3fb_info *par = info->par;
         u16 m, n, r;
         u8 regval;
         int rv;
@@ -339,7 +345,13 @@ static void s3_set_pixclock(struct fb_info *info, u32 pixclock)
         vga_w(NULL, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
 
         /* Set S3 clock registers */
-        vga_wseq(NULL, 0x12, ((n - 2) | (r << 5)));
+        if (par->chip == CHIP_360_TRIO3D_1X ||
+            par->chip == CHIP_362_TRIO3D_2X ||
+            par->chip == CHIP_368_TRIO3D_2X) {
+                vga_wseq(NULL, 0x12, (n - 2) | ((r & 3) << 6)); /* n and two bits of r */
+                vga_wseq(NULL, 0x29, r >> 2); /* remaining highest bit of r */
+        } else
+                vga_wseq(NULL, 0x12, (n - 2) | (r << 5));
         vga_wseq(NULL, 0x13, m - 2);
 
         udelay(1000);
@@ -456,7 +468,7 @@ static int s3fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 static int s3fb_set_par(struct fb_info *info)
 {
         struct s3fb_info *par = info->par;
-        u32 value, mode, hmul, offset_value, screen_size, multiplex;
+        u32 value, mode, hmul, offset_value, screen_size, multiplex, dbytes;
         u32 bpp = info->var.bits_per_pixel;
 
         if (bpp != 0) {
@@ -518,7 +530,7 @@ static int s3fb_set_par(struct fb_info *info)
         svga_wcrt_mask(0x33, 0x00, 0x08); /* no DDR ?   */
         svga_wcrt_mask(0x43, 0x00, 0x01); /* no DDR ?   */
 
-        svga_wcrt_mask(0x5D, 0x00, 0x28); // Clear strange HSlen bits
+        svga_wcrt_mask(0x5D, 0x00, 0x28); /* Clear strange HSlen bits */
 
 /*      svga_wcrt_mask(0x58, 0x03, 0x03); */
 
@@ -530,10 +542,14 @@ static int s3fb_set_par(struct fb_info *info)
         pr_debug("fb%d: offset register       : %d\n", info->node, offset_value);
         svga_wcrt_multi(s3_offset_regs, offset_value);
 
-        vga_wcrt(NULL, 0x54, 0x18); /* M parameter */
-        vga_wcrt(NULL, 0x60, 0xff); /* N parameter */
-        vga_wcrt(NULL, 0x61, 0xff); /* L parameter */
-        vga_wcrt(NULL, 0x62, 0xff); /* L parameter */
+        if (par->chip != CHIP_360_TRIO3D_1X &&
+            par->chip != CHIP_362_TRIO3D_2X &&
+            par->chip != CHIP_368_TRIO3D_2X) {
+                vga_wcrt(NULL, 0x54, 0x18); /* M parameter */
+                vga_wcrt(NULL, 0x60, 0xff); /* N parameter */
+                vga_wcrt(NULL, 0x61, 0xff); /* L parameter */
+                vga_wcrt(NULL, 0x62, 0xff); /* L parameter */
+        }
 
         vga_wcrt(NULL, 0x3A, 0x35);
         svga_wattr(0x33, 0x00);
@@ -570,6 +586,16 @@ static int s3fb_set_par(struct fb_info *info)
                 vga_wcrt(NULL, 0x66, 0x90);
         }
 
+        if (par->chip == CHIP_360_TRIO3D_1X ||
+            par->chip == CHIP_362_TRIO3D_2X ||
+            par->chip == CHIP_368_TRIO3D_2X) {
+                dbytes = info->var.xres * ((bpp+7)/8);
+                vga_wcrt(NULL, 0x91, (dbytes + 7) / 8);
+                vga_wcrt(NULL, 0x90, (((dbytes + 7) / 8) >> 8) | 0x80);
+
+                vga_wcrt(NULL, 0x66, 0x81);
+        }
+
         svga_wcrt_mask(0x31, 0x00, 0x40);
         multiplex = 0;
         hmul = 1;
@@ -615,11 +641,13 @@ static int s3fb_set_par(struct fb_info *info)
                 break;
         case 3:
                 pr_debug("fb%d: 8 bit pseudocolor\n", info->node);
-                if (info->var.pixclock > 20000) {
-                        svga_wcrt_mask(0x50, 0x00, 0x30);
+                svga_wcrt_mask(0x50, 0x00, 0x30);
+                if (info->var.pixclock > 20000 ||
+                    par->chip == CHIP_360_TRIO3D_1X ||
+                    par->chip == CHIP_362_TRIO3D_2X ||
+                    par->chip == CHIP_368_TRIO3D_2X)
                         svga_wcrt_mask(0x67, 0x00, 0xF0);
-                } else {
-                        svga_wcrt_mask(0x50, 0x00, 0x30);
+                else {
                         svga_wcrt_mask(0x67, 0x10, 0xF0);
                         multiplex = 1;
                 }
@@ -634,7 +662,10 @@ static int s3fb_set_par(struct fb_info *info)
                 } else {
                         svga_wcrt_mask(0x50, 0x10, 0x30);
                         svga_wcrt_mask(0x67, 0x30, 0xF0);
-                        hmul = 2;
+                        if (par->chip != CHIP_360_TRIO3D_1X &&
+                            par->chip != CHIP_362_TRIO3D_2X &&
+                            par->chip != CHIP_368_TRIO3D_2X)
+                                hmul = 2;
                 }
                 break;
         case 5:
@@ -647,7 +678,10 @@ static int s3fb_set_par(struct fb_info *info)
                 } else {
                         svga_wcrt_mask(0x50, 0x10, 0x30);
                         svga_wcrt_mask(0x67, 0x50, 0xF0);
-                        hmul = 2;
+                        if (par->chip != CHIP_360_TRIO3D_1X &&
+                            par->chip != CHIP_362_TRIO3D_2X &&
+                            par->chip != CHIP_368_TRIO3D_2X)
+                                hmul = 2;
                 }
                 break;
         case 6:
@@ -866,6 +900,17 @@ static int __devinit s3_identification(int chip)
                         return CHIP_385_VIRGE_GX;
         }
 
+        if (chip == CHIP_36X_TRIO3D_1X_2X) {
+                switch (vga_rcrt(NULL, 0x2f)) {
+                case 0x00:
+                        return CHIP_360_TRIO3D_1X;
+                case 0x01:
+                        return CHIP_362_TRIO3D_2X;
+                case 0x02:
+                        return CHIP_368_TRIO3D_2X;
+                }
+        }
+
         return CHIP_UNKNOWN;
 }
 
@@ -930,17 +975,32 @@ static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_i
         vga_wcrt(NULL, 0x38, 0x48);
         vga_wcrt(NULL, 0x39, 0xA5);
 
-        /* Find how many physical memory there is on card */
-        /* 0x36 register is accessible even if other registers are locked */
-        regval = vga_rcrt(NULL, 0x36);
-        info->screen_size = s3_memsizes[regval >> 5] << 10;
-        info->fix.smem_len = info->screen_size;
-
+        /* Identify chip type */
         par->chip = id->driver_data & CHIP_MASK;
         par->rev = vga_rcrt(NULL, 0x2f);
         if (par->chip & CHIP_UNDECIDED_FLAG)
                 par->chip = s3_identification(par->chip);
 
+        /* Find how many physical memory there is on card */
+        /* 0x36 register is accessible even if other registers are locked */
+        regval = vga_rcrt(NULL, 0x36);
+        if (par->chip == CHIP_360_TRIO3D_1X ||
+            par->chip == CHIP_362_TRIO3D_2X ||
+            par->chip == CHIP_368_TRIO3D_2X) {
+                switch ((regval & 0xE0) >> 5) {
+                case 0: /* 8MB -- only 4MB usable for display */
+                case 1: /* 4MB with 32-bit bus */
+                case 2: /* 4MB */
+                        info->screen_size = 4 << 20;
+                        break;
+                case 6: /* 2MB */
+                        info->screen_size = 2 << 20;
+                        break;
+                }
+        } else
+                info->screen_size = s3_memsizes[regval >> 5] << 10;
+        info->fix.smem_len = info->screen_size;
+
         /* Find MCLK frequency */
         regval = vga_rseq(NULL, 0x10);
         par->mclk_freq = ((vga_rseq(NULL, 0x11) + 2) * 14318) / ((regval & 0x1F)  + 2);
@@ -1131,6 +1191,7 @@ static struct pci_device_id s3_devices[] __devinitdata = {
         {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A10), .driver_data = CHIP_356_VIRGE_GX2},
         {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A11), .driver_data = CHIP_357_VIRGE_GX2P},
         {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A12), .driver_data = CHIP_359_VIRGE_GX2P},
+        {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A13), .driver_data = CHIP_36X_TRIO3D_1X_2X},
 
         {0, 0, 0, 0, 0, 0, 0}
 };
diff --git a/drivers/video/sh_mobile_lcdcfb.c b/drivers/video/sh_mobile_lcdcfb.c
index e8c769944812..12c451a711e9 100644
--- a/drivers/video/sh_mobile_lcdcfb.c
+++ b/drivers/video/sh_mobile_lcdcfb.c
@@ -991,13 +991,13 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
                 priv->ch[j].lcdc = priv;
                 memcpy(&priv->ch[j].cfg, &pdata->ch[i], sizeof(pdata->ch[i]));
 
-                error = sh_mobile_lcdc_check_interface(&priv->ch[i]);
+                error = sh_mobile_lcdc_check_interface(&priv->ch[j]);
                 if (error) {
                         dev_err(&pdev->dev, "unsupported interface type\n");
                         goto err1;
                 }
-                init_waitqueue_head(&priv->ch[i].frame_end_wait);
-                init_completion(&priv->ch[i].vsync_completion);
+                init_waitqueue_head(&priv->ch[j].frame_end_wait);
+                init_completion(&priv->ch[j].vsync_completion);
                 priv->ch[j].pan_offset = 0;
 
                 switch (pdata->ch[i].chan) {
diff --git a/drivers/video/tcx.c b/drivers/video/tcx.c
index ef7a7bd8b503..cc039b33d2d8 100644
--- a/drivers/video/tcx.c
+++ b/drivers/video/tcx.c
@@ -365,7 +365,7 @@ static void tcx_unmap_regs(struct of_device *op, struct fb_info *info,
 static int __devinit tcx_probe(struct of_device *op,
                                const struct of_device_id *match)
 {
-        struct device_node *dp = op->node;
+        struct device_node *dp = op->dev.of_node;
         struct fb_info *info;
         struct tcx_par *par;
         int linebytes, i, err;
diff --git a/drivers/video/via/viafbdev.c b/drivers/video/via/viafbdev.c
index 2bc40e682f95..1082541358f0 100644
--- a/drivers/video/via/viafbdev.c
+++ b/drivers/video/via/viafbdev.c
@@ -578,14 +578,9 @@ static int viafb_ioctl(struct fb_info *info, u_int cmd, u_long arg)
                 break;
 
         case VIAFB_SET_GAMMA_LUT:
-                viafb_gamma_table = kmalloc(256 * sizeof(u32), GFP_KERNEL);
-                if (!viafb_gamma_table)
-                        return -ENOMEM;
-                if (copy_from_user(viafb_gamma_table, argp,
-                                256 * sizeof(u32))) {
-                        kfree(viafb_gamma_table);
-                        return -EFAULT;
-                }
+                viafb_gamma_table = memdup_user(argp, 256 * sizeof(u32));
+                if (IS_ERR(viafb_gamma_table))
+                        return PTR_ERR(viafb_gamma_table);
                 viafb_set_gamma_table(viafb_bpp, viafb_gamma_table);
                 kfree(viafb_gamma_table);
                 break;
diff --git a/drivers/watchdog/gef_wdt.c b/drivers/watchdog/gef_wdt.c
index ca0f4c6cf5ab..1df284f9c2a1 100644
--- a/drivers/watchdog/gef_wdt.c
+++ b/drivers/watchdog/gef_wdt.c
@@ -273,7 +273,7 @@ static int __devinit gef_wdt_probe(struct of_device *dev,
                 bus_clk = freq;
 
         /* Map devices registers into memory */
-        gef_wdt_regs = of_iomap(dev->node, 0);
+        gef_wdt_regs = of_iomap(dev->dev.of_node, 0);
         if (gef_wdt_regs == NULL)
                 return -ENOMEM;
 
diff --git a/drivers/watchdog/mpc8xxx_wdt.c b/drivers/watchdog/mpc8xxx_wdt.c
index 6622335773bb..4cda64dd309c 100644
--- a/drivers/watchdog/mpc8xxx_wdt.c
+++ b/drivers/watchdog/mpc8xxx_wdt.c
@@ -189,7 +189,7 @@ static int __devinit mpc8xxx_wdt_probe(struct of_device *ofdev,
                                        const struct of_device_id *match)
 {
         int ret;
-        struct device_node *np = ofdev->node;
+        struct device_node *np = ofdev->dev.of_node;
         struct mpc8xxx_wdt_type *wdt_type = match->data;
         u32 freq = fsl_get_sys_freq();
         bool enabled;
diff --git a/drivers/watchdog/rdc321x_wdt.c b/drivers/watchdog/rdc321x_wdt.c
index 69c6adbd8205..428f8a1583e8 100644
--- a/drivers/watchdog/rdc321x_wdt.c
+++ b/drivers/watchdog/rdc321x_wdt.c
@@ -1,7 +1,7 @@
 /*
  * RDC321x watchdog driver
  *
- * Copyright (C) 2007 Florian Fainelli <florian@openwrt.org>
+ * Copyright (C) 2007-2010 Florian Fainelli <florian@openwrt.org>
  *
  * This driver is highly inspired from the cpu5_wdt driver
  *
@@ -36,8 +36,7 @@
 #include <linux/watchdog.h>
 #include <linux/io.h>
 #include <linux/uaccess.h>
-
-#include <asm/rdc321x_defs.h>
+#include <linux/mfd/rdc321x.h>
 
 #define RDC_WDT_MASK    0x80000000 /* Mask */
 #define RDC_WDT_EN      0x00800000 /* Enable bit */
@@ -63,6 +62,8 @@ static struct {
         int default_ticks;
         unsigned long inuse;
         spinlock_t lock;
+        struct pci_dev *sb_pdev;
+        int base_reg;
 } rdc321x_wdt_device;
 
 /* generic helper functions */
@@ -70,14 +71,18 @@ static struct {
 static void rdc321x_wdt_trigger(unsigned long unused)
 {
         unsigned long flags;
+        u32 val;
 
         if (rdc321x_wdt_device.running)
                 ticks--;
 
         /* keep watchdog alive */
         spin_lock_irqsave(&rdc321x_wdt_device.lock, flags);
-        outl(RDC_WDT_EN | inl(RDC3210_CFGREG_DATA),
-                RDC3210_CFGREG_DATA);
+        pci_read_config_dword(rdc321x_wdt_device.sb_pdev,
+                                        rdc321x_wdt_device.base_reg, &val);
+        val |= RDC_WDT_EN;
+        pci_write_config_dword(rdc321x_wdt_device.sb_pdev,
+                                        rdc321x_wdt_device.base_reg, val);
         spin_unlock_irqrestore(&rdc321x_wdt_device.lock, flags);
 
         /* requeue?? */
@@ -105,10 +110,13 @@ static void rdc321x_wdt_start(void)
 
                 /* Clear the timer */
                 spin_lock_irqsave(&rdc321x_wdt_device.lock, flags);
-                outl(RDC_CLS_TMR, RDC3210_CFGREG_ADDR);
+                pci_write_config_dword(rdc321x_wdt_device.sb_pdev,
+                                rdc321x_wdt_device.base_reg, RDC_CLS_TMR);
 
                 /* Enable watchdog and set the timeout to 81.92 us */
-                outl(RDC_WDT_EN | RDC_WDT_CNT, RDC3210_CFGREG_DATA);
+                pci_write_config_dword(rdc321x_wdt_device.sb_pdev,
+                                        rdc321x_wdt_device.base_reg,
+                                        RDC_WDT_EN | RDC_WDT_CNT);
                 spin_unlock_irqrestore(&rdc321x_wdt_device.lock, flags);
 
                 mod_timer(&rdc321x_wdt_device.timer,
@@ -148,7 +156,7 @@ static long rdc321x_wdt_ioctl(struct file *file, unsigned int cmd,
                                 unsigned long arg)
 {
         void __user *argp = (void __user *)arg;
-        unsigned int value;
+        u32 value;
         static const struct watchdog_info ident = {
                 .options = WDIOF_CARDRESET,
                 .identity = "RDC321x WDT",
@@ -162,9 +170,10 @@ static long rdc321x_wdt_ioctl(struct file *file, unsigned int cmd,
         case WDIOC_GETSTATUS:
                 /* Read the value from the DATA register */
                 spin_lock_irqsave(&rdc321x_wdt_device.lock, flags);
-                value = inl(RDC3210_CFGREG_DATA);
+                pci_read_config_dword(rdc321x_wdt_device.sb_pdev,
+                                        rdc321x_wdt_device.base_reg, &value);
                 spin_unlock_irqrestore(&rdc321x_wdt_device.lock, flags);
-                if (copy_to_user(argp, &value, sizeof(int)))
+                if (copy_to_user(argp, &value, sizeof(u32)))
                         return -EFAULT;
                 break;
         case WDIOC_GETSUPPORT:
@@ -219,17 +228,35 @@ static struct miscdevice rdc321x_wdt_misc = {
 static int __devinit rdc321x_wdt_probe(struct platform_device *pdev)
 {
         int err;
+        struct resource *r;
+        struct rdc321x_wdt_pdata *pdata;
+
+        pdata = pdev->dev.platform_data;
+        if (!pdata) {
+                dev_err(&pdev->dev, "no platform data supplied\n");
+                return -ENODEV;
+        }
+
+        r = platform_get_resource_byname(pdev, IORESOURCE_IO, "wdt-reg");
+        if (!r) {
+                dev_err(&pdev->dev, "failed to get wdt-reg resource\n");
+                return -ENODEV;
+        }
+
+        rdc321x_wdt_device.sb_pdev = pdata->sb_pdev;
+        rdc321x_wdt_device.base_reg = r->start;
 
         err = misc_register(&rdc321x_wdt_misc);
         if (err < 0) {
-                printk(KERN_ERR PFX "watchdog misc_register failed\n");
+                dev_err(&pdev->dev, "misc_register failed\n");
                 return err;
         }
 
         spin_lock_init(&rdc321x_wdt_device.lock);
 
         /* Reset the watchdog */
-        outl(RDC_WDT_RST, RDC3210_CFGREG_DATA);
+        pci_write_config_dword(rdc321x_wdt_device.sb_pdev,
+                                rdc321x_wdt_device.base_reg, RDC_WDT_RST);
 
         init_completion(&rdc321x_wdt_device.stop);
         rdc321x_wdt_device.queue = 0;
@@ -240,7 +267,7 @@ static int __devinit rdc321x_wdt_probe(struct platform_device *pdev)
 
         rdc321x_wdt_device.default_ticks = ticks;
 
-        printk(KERN_INFO PFX "watchdog init success\n");
+        dev_info(&pdev->dev, "watchdog init success\n");
 
         return 0;
 }
diff --git a/drivers/watchdog/wm8350_wdt.c b/drivers/watchdog/wm8350_wdt.c
index 89dd7b035295..b68d928c8f90 100644
--- a/drivers/watchdog/wm8350_wdt.c
+++ b/drivers/watchdog/wm8350_wdt.c
@@ -284,7 +284,7 @@ static int __devinit wm8350_wdt_probe(struct platform_device *pdev)
         struct wm8350 *wm8350 = platform_get_drvdata(pdev);
 
         if (!wm8350) {
-                dev_err(wm8350->dev, "No driver data supplied\n");
+                pr_err("No driver data supplied\n");
                 return -ENODEV;
         }
 
diff --git a/drivers/xen/xenbus/xenbus_xs.c b/drivers/xen/xenbus/xenbus_xs.c
index eab33f1dbdf7..7b547f53f65e 100644
--- a/drivers/xen/xenbus/xenbus_xs.c
+++ b/drivers/xen/xenbus/xenbus_xs.c
@@ -499,7 +499,7 @@ int xenbus_printf(struct xenbus_transaction t,
 #define PRINTF_BUFFER_SIZE 4096
         char *printf_buffer;
 
-        printf_buffer = kmalloc(PRINTF_BUFFER_SIZE, GFP_KERNEL);
+        printf_buffer = kmalloc(PRINTF_BUFFER_SIZE, GFP_NOIO | __GFP_HIGH);
         if (printf_buffer == NULL)
                 return -ENOMEM;