Added missing tegra files.HEADmaster

76 files changed, 30667 insertions, 0 deletions

diff --git a/drivers/staging/gma500/Kconfig b/drivers/staging/gma500/Kconfig
new file mode 100644
index 00000000000..bfe2166acda
--- /dev/null
+++ b/drivers/staging/gma500/Kconfig
@@ -0,0 +1,33 @@
+config DRM_PSB
+        tristate "Intel GMA5/600 KMS Framebuffer"
+        depends on DRM && PCI && X86
+        select FB_CFB_COPYAREA
+        select FB_CFB_FILLRECT
+        select FB_CFB_IMAGEBLIT
+        select DRM_KMS_HELPER
+        select DRM_TTM
+        help
+          Say yes for an experimental 2D KMS framebuffer driver for the
+          Intel GMA500 ('Poulsbo') and other Intel IMG based graphics
+          devices.
+
+config DRM_PSB_MRST
+        bool "Intel GMA600 support (Experimental)"
+        depends on DRM_PSB
+        help
+          Say yes to include support for GMA600 (Intel Moorestown/Oaktrail)
+          platforms with LVDS ports. HDMI and MIPI are not currently
+          supported.
+
+config DRM_PSB_MFLD
+        bool "Intel Medfield support (Experimental)"
+        depends on DRM_PSB
+        help
+          Say yes to include support for Intel Medfield platforms with MIPI
+          interfaces.
+        
+config DRM_PSB_CDV
+        bool "Intel Cedarview support (Experimental)"
+        depends on DRM_PSB
+        help
+          Say yes to include support for Intel Cedarview platforms
diff --git a/drivers/staging/gma500/Makefile b/drivers/staging/gma500/Makefile
new file mode 100644
index 00000000000..c729868b1b1
--- /dev/null
+++ b/drivers/staging/gma500/Makefile
@@ -0,0 +1,52 @@
+#
+#       KMS driver for the GMA500
+#
+ccflags-y += -Iinclude/drm
+
+psb_gfx-y += gem_glue.o \
+          accel_2d.o \
+          backlight.o \
+          framebuffer.o \
+          gem.o \
+          gtt.o \
+          intel_bios.o \
+          intel_i2c.o \
+          intel_opregion.o \
+          mmu.o \
+          power.o \
+          psb_drv.o \
+          psb_intel_display.o \
+          psb_intel_lvds.o \
+          psb_intel_modes.o \
+          psb_intel_sdvo.o \
+          psb_lid.o \
+          psb_irq.o \
+          psb_device.o \
+          mid_bios.o
+
+psb_gfx-$(CONFIG_DRM_PSB_CDV) +=  cdv_device.o \
+          cdv_intel_crt.o \
+          cdv_intel_display.o \
+          cdv_intel_hdmi.o \
+          cdv_intel_lvds.o
+
+psb_gfx-$(CONFIG_DRM_PSB_MRST) += mrst_device.o \
+          mrst_crtc.o \
+          mrst_lvds.o \
+          mrst_hdmi.o \
+          mrst_hdmi_i2c.o
+
+psb_gfx-$(CONFIG_DRM_PSB_MFLD) += mdfld_device.o \
+          mdfld_output.o \
+          mdfld_pyr_cmd.o \
+          mdfld_tmd_vid.o \
+          mdfld_tpo_cmd.o \
+          mdfld_tpo_vid.o \
+          mdfld_dsi_pkg_sender.o \
+          mdfld_dsi_dpi.o \
+          mdfld_dsi_output.o \
+          mdfld_dsi_dbi.o \
+          mdfld_dsi_dbi_dpu.o \
+          mdfld_intel_display.o
+
+obj-$(CONFIG_DRM_PSB) += psb_gfx.o
diff --git a/drivers/staging/gma500/TODO b/drivers/staging/gma500/TODO
new file mode 100644
index 00000000000..fc836158e74
--- /dev/null
+++ b/drivers/staging/gma500/TODO
@@ -0,0 +1,15 @@
+-       Sort out the power management side. Not important for Poulsbo but
+        matters for Moorestown/Medfield
+-       Debug Oaktrail/Moorestown support (single pipe, no BIOS on mrst,
+                                        some other differences)
+-       Add 2D acceleration via console and DRM
+-       Add scrolling acceleration using the GTT to do remapping on the main
+        framebuffer.
+-       HDMI testing
+-       Oaktrail HDMI and other features
+-       Oaktrail MIPI
+-       Medfield needs a lot of further love
+
+As per kernel policy and the in the interest of the safety of various
+kittens there is no support or plans to add hooks for the closed user space
+stuff.
diff --git a/drivers/staging/gma500/accel_2d.c b/drivers/staging/gma500/accel_2d.c
new file mode 100644
index 00000000000..14400fcfe8a
--- /dev/null
+++ b/drivers/staging/gma500/accel_2d.c
@@ -0,0 +1,412 @@
+/**************************************************************************
+ * Copyright (c) 2007-2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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 funded Tungsten Graphics (http://www.tungstengraphics.com) to
+ * develop this driver.
+ *
+ **************************************************************************/
+
+#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/init.h>
+#include <linux/console.h>
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include <drm/drm_crtc.h>
+
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "framebuffer.h"
+
+/**
+ *      psb_spank               -       reset the 2D engine
+ *      @dev_priv: our PSB DRM device
+ *
+ *      Soft reset the graphics engine and then reload the necessary registers.
+ *      We use this at initialisation time but it will become relevant for
+ *      accelerated X later
+ */
+void psb_spank(struct drm_psb_private *dev_priv)
+{
+        PSB_WSGX32(_PSB_CS_RESET_BIF_RESET | _PSB_CS_RESET_DPM_RESET |
+                _PSB_CS_RESET_TA_RESET | _PSB_CS_RESET_USE_RESET |
+                _PSB_CS_RESET_ISP_RESET | _PSB_CS_RESET_TSP_RESET |
+                _PSB_CS_RESET_TWOD_RESET, PSB_CR_SOFT_RESET);
+        PSB_RSGX32(PSB_CR_SOFT_RESET);
+
+        msleep(1);
+
+        PSB_WSGX32(0, PSB_CR_SOFT_RESET);
+        wmb();
+        PSB_WSGX32(PSB_RSGX32(PSB_CR_BIF_CTRL) | _PSB_CB_CTRL_CLEAR_FAULT,
+                   PSB_CR_BIF_CTRL);
+        wmb();
+        (void) PSB_RSGX32(PSB_CR_BIF_CTRL);
+
+        msleep(1);
+        PSB_WSGX32(PSB_RSGX32(PSB_CR_BIF_CTRL) & ~_PSB_CB_CTRL_CLEAR_FAULT,
+                   PSB_CR_BIF_CTRL);
+        (void) PSB_RSGX32(PSB_CR_BIF_CTRL);
+        PSB_WSGX32(dev_priv->gtt.gatt_start, PSB_CR_BIF_TWOD_REQ_BASE);
+}
+
+/**
+ *      psb2_2d_wait_available  -       wait for FIFO room
+ *      @dev_priv: our DRM device
+ *      @size: size (in dwords) of the command we want to issue
+ *
+ *      Wait until there is room to load the FIFO with our data. If the
+ *      device is not responding then reset it
+ */
+static int psb_2d_wait_available(struct drm_psb_private *dev_priv,
+                          unsigned size)
+{
+        uint32_t avail = PSB_RSGX32(PSB_CR_2D_SOCIF);
+        unsigned long t = jiffies + HZ;
+
+        while (avail < size) {
+                avail = PSB_RSGX32(PSB_CR_2D_SOCIF);
+                if (time_after(jiffies, t)) {
+                        psb_spank(dev_priv);
+                        return -EIO;
+                }
+        }
+        return 0;
+}
+
+/**
+ *      psb_2d_submit           -       submit a 2D command
+ *      @dev_priv: our DRM device
+ *      @cmdbuf: command to issue
+ *      @size: length (in dwords)
+ *
+ *      Issue one or more 2D commands to the accelerator. This needs to be
+ *      serialized later when we add the GEM interfaces for acceleration
+ */
+static int psbfb_2d_submit(struct drm_psb_private *dev_priv, uint32_t *cmdbuf,
+                                                                unsigned size)
+{
+        int ret = 0;
+        int i;
+        unsigned submit_size;
+
+        mutex_lock(&dev_priv->mutex_2d);
+        while (size > 0) {
+                submit_size = (size < 0x60) ? size : 0x60;
+                size -= submit_size;
+                ret = psb_2d_wait_available(dev_priv, submit_size);
+                if (ret)
+                        break;
+
+                submit_size <<= 2;
+
+                for (i = 0; i < submit_size; i += 4)
+                        PSB_WSGX32(*cmdbuf++, PSB_SGX_2D_SLAVE_PORT + i);
+
+                (void)PSB_RSGX32(PSB_SGX_2D_SLAVE_PORT + i - 4);
+        }
+        mutex_unlock(&dev_priv->mutex_2d);
+        return ret;
+}
+
+
+/**
+ *      psb_accel_2d_copy_direction     -       compute blit order
+ *      @xdir: X direction of move
+ *      @ydir: Y direction of move
+ *
+ *      Compute the correct order setings to ensure that an overlapping blit
+ *      correctly copies all the pixels.
+ */
+static u32 psb_accel_2d_copy_direction(int xdir, int ydir)
+{
+        if (xdir < 0)
+                return (ydir < 0) ? PSB_2D_COPYORDER_BR2TL :
+                                                PSB_2D_COPYORDER_TR2BL;
+        else
+                return (ydir < 0) ? PSB_2D_COPYORDER_BL2TR :
+                                                PSB_2D_COPYORDER_TL2BR;
+}
+
+/**
+ *      psb_accel_2d_copy               -       accelerated 2D copy
+ *      @dev_priv: our DRM device
+ *      @src_offset in bytes
+ *      @src_stride in bytes
+ *      @src_format psb 2D format defines
+ *      @dst_offset in bytes
+ *      @dst_stride in bytes
+ *      @dst_format psb 2D format defines
+ *      @src_x offset in pixels
+ *      @src_y offset in pixels
+ *      @dst_x offset in pixels
+ *      @dst_y offset in pixels
+ *      @size_x of the copied area
+ *      @size_y of the copied area
+ *
+ *      Format and issue a 2D accelerated copy command.
+ */
+static int psb_accel_2d_copy(struct drm_psb_private *dev_priv,
+                             uint32_t src_offset, uint32_t src_stride,
+                             uint32_t src_format, uint32_t dst_offset,
+                             uint32_t dst_stride, uint32_t dst_format,
+                             uint16_t src_x, uint16_t src_y,
+                             uint16_t dst_x, uint16_t dst_y,
+                             uint16_t size_x, uint16_t size_y)
+{
+        uint32_t blit_cmd;
+        uint32_t buffer[10];
+        uint32_t *buf;
+        uint32_t direction;
+
+        buf = buffer;
+
+        direction =
+            psb_accel_2d_copy_direction(src_x - dst_x, src_y - dst_y);
+
+        if (direction == PSB_2D_COPYORDER_BR2TL ||
+            direction == PSB_2D_COPYORDER_TR2BL) {
+                src_x += size_x - 1;
+                dst_x += size_x - 1;
+        }
+        if (direction == PSB_2D_COPYORDER_BR2TL ||
+            direction == PSB_2D_COPYORDER_BL2TR) {
+                src_y += size_y - 1;
+                dst_y += size_y - 1;
+        }
+
+        blit_cmd =
+            PSB_2D_BLIT_BH |
+            PSB_2D_ROT_NONE |
+            PSB_2D_DSTCK_DISABLE |
+            PSB_2D_SRCCK_DISABLE |
+            PSB_2D_USE_PAT | PSB_2D_ROP3_SRCCOPY | direction;
+
+        *buf++ = PSB_2D_FENCE_BH;
+        *buf++ =
+            PSB_2D_DST_SURF_BH | dst_format | (dst_stride <<
+                                               PSB_2D_DST_STRIDE_SHIFT);
+        *buf++ = dst_offset;
+        *buf++ =
+            PSB_2D_SRC_SURF_BH | src_format | (src_stride <<
+                                               PSB_2D_SRC_STRIDE_SHIFT);
+        *buf++ = src_offset;
+        *buf++ =
+            PSB_2D_SRC_OFF_BH | (src_x << PSB_2D_SRCOFF_XSTART_SHIFT) |
+            (src_y << PSB_2D_SRCOFF_YSTART_SHIFT);
+        *buf++ = blit_cmd;
+        *buf++ =
+            (dst_x << PSB_2D_DST_XSTART_SHIFT) | (dst_y <<
+                                                  PSB_2D_DST_YSTART_SHIFT);
+        *buf++ =
+            (size_x << PSB_2D_DST_XSIZE_SHIFT) | (size_y <<
+                                                  PSB_2D_DST_YSIZE_SHIFT);
+        *buf++ = PSB_2D_FLUSH_BH;
+
+        return psbfb_2d_submit(dev_priv, buffer, buf - buffer);
+}
+
+/**
+ *      psbfb_copyarea_accel    -       copyarea acceleration for /dev/fb
+ *      @info: our framebuffer
+ *      @a: copyarea parameters from the framebuffer core
+ *
+ *      Perform a 2D copy via the accelerator
+ */
+static void psbfb_copyarea_accel(struct fb_info *info,
+                                 const struct fb_copyarea *a)
+{
+        struct psb_fbdev *fbdev = info->par;
+        struct psb_framebuffer *psbfb = &fbdev->pfb;
+        struct drm_device *dev = psbfb->base.dev;
+        struct drm_framebuffer *fb = fbdev->psb_fb_helper.fb;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        uint32_t offset;
+        uint32_t stride;
+        uint32_t src_format;
+        uint32_t dst_format;
+
+        if (!fb)
+                return;
+
+        offset = psbfb->gtt->offset;
+        stride = fb->pitch;
+
+        switch (fb->depth) {
+        case 8:
+                src_format = PSB_2D_SRC_332RGB;
+                dst_format = PSB_2D_DST_332RGB;
+                break;
+        case 15:
+                src_format = PSB_2D_SRC_555RGB;
+                dst_format = PSB_2D_DST_555RGB;
+                break;
+        case 16:
+                src_format = PSB_2D_SRC_565RGB;
+                dst_format = PSB_2D_DST_565RGB;
+                break;
+        case 24:
+        case 32:
+                /* this is wrong but since we don't do blending its okay */
+                src_format = PSB_2D_SRC_8888ARGB;
+                dst_format = PSB_2D_DST_8888ARGB;
+                break;
+        default:
+                /* software fallback */
+                cfb_copyarea(info, a);
+                return;
+        }
+
+        if (!gma_power_begin(dev, false)) {
+                cfb_copyarea(info, a);
+                return;
+        }
+        psb_accel_2d_copy(dev_priv,
+                          offset, stride, src_format,
+                          offset, stride, dst_format,
+                          a->sx, a->sy, a->dx, a->dy, a->width, a->height);
+        gma_power_end(dev);
+}
+
+/**
+ *      psbfb_copyarea  -       2D copy interface
+ *      @info: our framebuffer
+ *      @region: region to copy
+ *
+ *      Copy an area of the framebuffer console either by the accelerator
+ *      or directly using the cfb helpers according to the request
+ */
+void psbfb_copyarea(struct fb_info *info,
+                           const struct fb_copyarea *region)
+{
+        if (unlikely(info->state != FBINFO_STATE_RUNNING))
+                return;
+
+        /* Avoid the 8 pixel erratum */
+        if (region->width == 8 || region->height == 8 ||
+                (info->flags & FBINFO_HWACCEL_DISABLED))
+                return cfb_copyarea(info, region);
+
+        psbfb_copyarea_accel(info, region);
+}
+
+/**
+ *      psbfb_sync      -       synchronize 2D
+ *      @info: our framebuffer
+ *
+ *      Wait for the 2D engine to quiesce so that we can do CPU
+ *      access to the framebuffer again
+ */
+int psbfb_sync(struct fb_info *info)
+{
+        struct psb_fbdev *fbdev = info->par;
+        struct psb_framebuffer *psbfb = &fbdev->pfb;
+        struct drm_device *dev = psbfb->base.dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        unsigned long _end = jiffies + DRM_HZ;
+        int busy = 0;
+
+        mutex_lock(&dev_priv->mutex_2d);
+        /*
+         * First idle the 2D engine.
+         */
+
+        if ((PSB_RSGX32(PSB_CR_2D_SOCIF) == _PSB_C2_SOCIF_EMPTY) &&
+            ((PSB_RSGX32(PSB_CR_2D_BLIT_STATUS) & _PSB_C2B_STATUS_BUSY) == 0))
+                goto out;
+
+        do {
+                busy = (PSB_RSGX32(PSB_CR_2D_SOCIF) != _PSB_C2_SOCIF_EMPTY);
+                cpu_relax();
+        } while (busy && !time_after_eq(jiffies, _end));
+
+        if (busy)
+                busy = (PSB_RSGX32(PSB_CR_2D_SOCIF) != _PSB_C2_SOCIF_EMPTY);
+        if (busy)
+                goto out;
+
+        do {
+                busy = ((PSB_RSGX32(PSB_CR_2D_BLIT_STATUS) &
+                                                _PSB_C2B_STATUS_BUSY) != 0);
+                cpu_relax();
+        } while (busy && !time_after_eq(jiffies, _end));
+        if (busy)
+                busy = ((PSB_RSGX32(PSB_CR_2D_BLIT_STATUS) &
+                                        _PSB_C2B_STATUS_BUSY) != 0);
+
+out:
+        mutex_unlock(&dev_priv->mutex_2d);
+        return (busy) ? -EBUSY : 0;
+}
+
+int psb_accel_ioctl(struct drm_device *dev, void *data, struct drm_file *file)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct drm_psb_2d_op *op = data;
+        u32 *op_ptr = &op->cmd[0];
+        int i;
+        struct drm_gem_object *obj;
+        struct gtt_range *gtt;
+        int err = -EINVAL;
+
+        if (!dev_priv->ops->accel_2d)
+                return -EOPNOTSUPP;
+        if (op->size > PSB_2D_OP_BUFLEN)
+                return -EINVAL;
+
+        /* The GEM object being used. We need to support separate src/dst/etc
+           in the end but for now keep them all the same */
+        obj = drm_gem_object_lookup(dev, file, op->src);
+        if (obj == NULL)
+                return -ENOENT;
+        gtt = container_of(obj, struct gtt_range, gem);
+
+        if (psb_gtt_pin(gtt) < 0)
+                goto bad_2;
+        for (i = 0; i < op->size; i++, op_ptr++) {
+                u32 r = *op_ptr & 0xF0000000;
+                /* Fill in the GTT offsets for the command buffer */
+                if (r == PSB_2D_SRC_SURF_BH ||
+                        r == PSB_2D_DST_SURF_BH || 
+                        r == PSB_2D_MASK_SURF_BH ||
+                        r == PSB_2D_PAT_SURF_BH) {
+                        i++;
+                        op_ptr++;
+                        if (i == op->size)
+                                goto bad;
+                        if (*op_ptr)
+                                goto bad;
+                        *op_ptr = gtt->offset;
+                        continue;
+                }
+        }
+        psbfb_2d_submit(dev_priv, op->cmd, op->size);
+        err = 0;
+bad:
+        psb_gtt_unpin(gtt);
+bad_2:
+        drm_gem_object_unreference(obj);
+        return err;
+}
diff --git a/drivers/staging/gma500/backlight.c b/drivers/staging/gma500/backlight.c
new file mode 100644
index 00000000000..20793951fca
--- /dev/null
+++ b/drivers/staging/gma500/backlight.c
@@ -0,0 +1,49 @@
+/*
+ * GMA500 Backlight Interface
+ *
+ * Copyright (c) 2009-2011, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors: Eric Knopp
+ *
+ */
+
+#include "psb_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_intel_drv.h"
+#include "intel_bios.h"
+#include "power.h"
+
+int gma_backlight_init(struct drm_device *dev)
+{
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        return dev_priv->ops->backlight_init(dev);
+#else
+        return 0;
+#endif
+}
+
+void gma_backlight_exit(struct drm_device *dev)
+{
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        if (dev_priv->backlight_device) {
+                dev_priv->backlight_device->props.brightness = 0;
+                backlight_update_status(dev_priv->backlight_device);
+                backlight_device_unregister(dev_priv->backlight_device);
+        }
+#endif
+}
diff --git a/drivers/staging/gma500/cdv_device.c b/drivers/staging/gma500/cdv_device.c
new file mode 100644
index 00000000000..87614e0d396
--- /dev/null
+++ b/drivers/staging/gma500/cdv_device.c
@@ -0,0 +1,351 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+#include <linux/backlight.h>
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "psb_drm.h"
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include "intel_bios.h"
+#include "cdv_device.h"
+
+#define VGA_SR_INDEX            0x3c4
+#define VGA_SR_DATA             0x3c5
+
+/* FIXME: should check if we are the active VGA device ?? */
+static void cdv_disable_vga(struct drm_device *dev)
+{
+        u8 sr1;
+        u32 vga_reg;
+
+        vga_reg = VGACNTRL;
+
+        outb(1, VGA_SR_INDEX);
+        sr1 = inb(VGA_SR_DATA);
+        outb(sr1 | 1<<5, VGA_SR_DATA);
+        udelay(300);
+
+        REG_WRITE(vga_reg, VGA_DISP_DISABLE);
+        REG_READ(vga_reg);
+}
+
+static int cdv_output_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        cdv_disable_vga(dev);
+
+        cdv_intel_crt_init(dev, &dev_priv->mode_dev);
+        cdv_intel_lvds_init(dev, &dev_priv->mode_dev);
+
+        /* These bits indicate HDMI not SDVO on CDV, but we don't yet support
+           the HDMI interface */
+        if (REG_READ(SDVOB) & SDVO_DETECTED)
+                cdv_hdmi_init(dev, &dev_priv->mode_dev, SDVOB);
+        if (REG_READ(SDVOC) & SDVO_DETECTED)
+                cdv_hdmi_init(dev, &dev_priv->mode_dev, SDVOC);
+        return 0;
+}
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+
+/*
+ *      Poulsbo Backlight Interfaces
+ */
+
+#define BLC_PWM_PRECISION_FACTOR 100    /* 10000000 */
+#define BLC_PWM_FREQ_CALC_CONSTANT 32
+#define MHz 1000000
+
+#define PSB_BLC_PWM_PRECISION_FACTOR    10
+#define PSB_BLC_MAX_PWM_REG_FREQ        0xFFFE
+#define PSB_BLC_MIN_PWM_REG_FREQ        0x2
+
+#define PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR (0xFFFE)
+#define PSB_BACKLIGHT_PWM_CTL_SHIFT     (16)
+
+static int cdv_brightness;
+static struct backlight_device *cdv_backlight_device;
+
+static int cdv_get_brightness(struct backlight_device *bd)
+{
+        /* return locally cached var instead of HW read (due to DPST etc.) */
+        /* FIXME: ideally return actual value in case firmware fiddled with
+           it */
+        return cdv_brightness;
+}
+
+
+static int cdv_backlight_setup(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        unsigned long core_clock;
+        /* u32 bl_max_freq; */
+        /* unsigned long value; */
+        u16 bl_max_freq;
+        uint32_t value;
+        uint32_t blc_pwm_precision_factor;
+
+        /* get bl_max_freq and pol from dev_priv*/
+        if (!dev_priv->lvds_bl) {
+                dev_err(dev->dev, "Has no valid LVDS backlight info\n");
+                return -ENOENT;
+        }
+        bl_max_freq = dev_priv->lvds_bl->freq;
+        blc_pwm_precision_factor = PSB_BLC_PWM_PRECISION_FACTOR;
+
+        core_clock = dev_priv->core_freq;
+
+        value = (core_clock * MHz) / BLC_PWM_FREQ_CALC_CONSTANT;
+        value *= blc_pwm_precision_factor;
+        value /= bl_max_freq;
+        value /= blc_pwm_precision_factor;
+
+        if (value > (unsigned long long)PSB_BLC_MAX_PWM_REG_FREQ ||
+                 value < (unsigned long long)PSB_BLC_MIN_PWM_REG_FREQ)
+                                return -ERANGE;
+        else {
+                /* FIXME */
+        }
+        return 0;
+}
+
+static int cdv_set_brightness(struct backlight_device *bd)
+{
+        int level = bd->props.brightness;
+
+        /* Percentage 1-100% being valid */
+        if (level < 1)
+                level = 1;
+
+        /*cdv_intel_lvds_set_brightness(dev, level); FIXME */
+        cdv_brightness = level;
+        return 0;
+}
+
+static const struct backlight_ops cdv_ops = {
+        .get_brightness = cdv_get_brightness,
+        .update_status  = cdv_set_brightness,
+};
+
+static int cdv_backlight_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int ret;
+        struct backlight_properties props;
+
+        memset(&props, 0, sizeof(struct backlight_properties));
+        props.max_brightness = 100;
+        props.type = BACKLIGHT_PLATFORM;
+
+        cdv_backlight_device = backlight_device_register("psb-bl",
+                                        NULL, (void *)dev, &cdv_ops, &props);
+        if (IS_ERR(cdv_backlight_device))
+                return PTR_ERR(cdv_backlight_device);
+
+        ret = cdv_backlight_setup(dev);
+        if (ret < 0) {
+                backlight_device_unregister(cdv_backlight_device);
+                cdv_backlight_device = NULL;
+                return ret;
+        }
+        cdv_backlight_device->props.brightness = 100;
+        cdv_backlight_device->props.max_brightness = 100;
+        backlight_update_status(cdv_backlight_device);
+        dev_priv->backlight_device = cdv_backlight_device;
+        return 0;
+}
+
+#endif
+
+/*
+ *      Provide the Cedarview specific chip logic and low level methods
+ *      for power management
+ *
+ *      FIXME: we need to implement the apm/ospm base management bits
+ *      for this and the MID devices.
+ */
+
+static inline u32 CDV_MSG_READ32(uint port, uint offset)
+{
+        int mcr = (0x10<<24) | (port << 16) | (offset << 8);
+        uint32_t ret_val = 0;
+        struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+        pci_write_config_dword(pci_root, 0xD0, mcr);
+        pci_read_config_dword(pci_root, 0xD4, &ret_val);
+        pci_dev_put(pci_root);
+        return ret_val;
+}
+
+static inline void CDV_MSG_WRITE32(uint port, uint offset, u32 value)
+{
+        int mcr = (0x11<<24) | (port << 16) | (offset << 8) | 0xF0;
+        struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+        pci_write_config_dword(pci_root, 0xD4, value);
+        pci_write_config_dword(pci_root, 0xD0, mcr);
+        pci_dev_put(pci_root);
+}
+
+#define PSB_APM_CMD                     0x0
+#define PSB_APM_STS                     0x04
+#define PSB_PM_SSC                      0x20
+#define PSB_PM_SSS                      0x30
+#define PSB_PWRGT_GFX_MASK              0x3
+#define CDV_PWRGT_DISPLAY_CNTR          0x000fc00c
+#define CDV_PWRGT_DISPLAY_STS           0x000fc00c
+
+static void cdv_init_pm(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 pwr_cnt;
+        int i;
+
+        dev_priv->apm_base = CDV_MSG_READ32(PSB_PUNIT_PORT,
+                                                        PSB_APMBA) & 0xFFFF;
+        dev_priv->ospm_base = CDV_MSG_READ32(PSB_PUNIT_PORT,
+                                                        PSB_OSPMBA) & 0xFFFF;
+
+        /* Force power on for now */
+        pwr_cnt = inl(dev_priv->apm_base + PSB_APM_CMD);
+        pwr_cnt &= ~PSB_PWRGT_GFX_MASK;
+
+        outl(pwr_cnt, dev_priv->apm_base + PSB_APM_CMD);
+        for (i = 0; i < 5; i++) {
+                u32 pwr_sts = inl(dev_priv->apm_base + PSB_APM_STS);
+                if ((pwr_sts & PSB_PWRGT_GFX_MASK) == 0)
+                        break;
+                udelay(10);
+        }
+        pwr_cnt = inl(dev_priv->ospm_base + PSB_PM_SSC);
+        pwr_cnt &= ~CDV_PWRGT_DISPLAY_CNTR;
+        outl(pwr_cnt, dev_priv->ospm_base + PSB_PM_SSC);
+        for (i = 0; i < 5; i++) {
+                u32 pwr_sts = inl(dev_priv->ospm_base + PSB_PM_SSS);
+                if ((pwr_sts & CDV_PWRGT_DISPLAY_STS) == 0)
+                        break;
+                udelay(10);
+        }
+}
+
+/**
+ *      cdv_save_display_registers      -       save registers lost on suspend
+ *      @dev: our DRM device
+ *
+ *      Save the state we need in order to be able to restore the interface
+ *      upon resume from suspend
+ *
+ *      FIXME: review
+ */
+static int cdv_save_display_registers(struct drm_device *dev)
+{
+        return 0;
+}
+
+/**
+ *      cdv_restore_display_registers   -       restore lost register state
+ *      @dev: our DRM device
+ *
+ *      Restore register state that was lost during suspend and resume.
+ *
+ *      FIXME: review
+ */
+static int cdv_restore_display_registers(struct drm_device *dev)
+{
+        return 0;
+}
+
+static int cdv_power_down(struct drm_device *dev)
+{
+        return 0;
+}
+
+static int cdv_power_up(struct drm_device *dev)
+{
+        return 0;
+}
+
+/* FIXME ? - shared with Poulsbo */
+static void cdv_get_core_freq(struct drm_device *dev)
+{
+        uint32_t clock;
+        struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        pci_write_config_dword(pci_root, 0xD0, 0xD0050300);
+        pci_read_config_dword(pci_root, 0xD4, &clock);
+        pci_dev_put(pci_root);
+
+        switch (clock & 0x07) {
+        case 0:
+                dev_priv->core_freq = 100;
+                break;
+        case 1:
+                dev_priv->core_freq = 133;
+                break;
+        case 2:
+                dev_priv->core_freq = 150;
+                break;
+        case 3:
+                dev_priv->core_freq = 178;
+                break;
+        case 4:
+                dev_priv->core_freq = 200;
+                break;
+        case 5:
+        case 6:
+        case 7:
+                dev_priv->core_freq = 266;
+        default:
+                dev_priv->core_freq = 0;
+        }
+}
+
+static int cdv_chip_setup(struct drm_device *dev)
+{
+        cdv_get_core_freq(dev);
+        gma_intel_opregion_init(dev);
+        psb_intel_init_bios(dev);
+        return 0;
+}
+
+/* CDV is much like Poulsbo but has MID like SGX offsets and PM */
+
+const struct psb_ops cdv_chip_ops = {
+        .name = "Cedartrail",
+        .accel_2d = 0,
+        .pipes = 2,
+        .sgx_offset = MRST_SGX_OFFSET,
+        .chip_setup = cdv_chip_setup,
+
+        .crtc_helper = &cdv_intel_helper_funcs,
+        .crtc_funcs = &cdv_intel_crtc_funcs,
+
+        .output_init = cdv_output_init,
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+        .backlight_init = cdv_backlight_init,
+#endif
+
+        .init_pm = cdv_init_pm,
+        .save_regs = cdv_save_display_registers,
+        .restore_regs = cdv_restore_display_registers,
+        .power_down = cdv_power_down,
+        .power_up = cdv_power_up,
+};
diff --git a/drivers/staging/gma500/cdv_device.h b/drivers/staging/gma500/cdv_device.h
new file mode 100644
index 00000000000..2a88b7beb55
--- /dev/null
+++ b/drivers/staging/gma500/cdv_device.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright © 2011 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ */
+
+extern const struct drm_crtc_helper_funcs cdv_intel_helper_funcs;
+extern const struct drm_crtc_funcs cdv_intel_crtc_funcs;
+extern void cdv_intel_crt_init(struct drm_device *dev,
+                        struct psb_intel_mode_device *mode_dev);
+extern void cdv_intel_lvds_init(struct drm_device *dev,
+                        struct psb_intel_mode_device *mode_dev);
+extern void cdv_hdmi_init(struct drm_device *dev, struct psb_intel_mode_device *mode_dev,
+                        int reg);
+extern struct drm_display_mode *cdv_intel_crtc_mode_get(struct drm_device *dev,
+                                             struct drm_crtc *crtc);
+
+extern inline void cdv_intel_wait_for_vblank(struct drm_device *dev)
+{
+        /* Wait for 20ms, i.e. one cycle at 50hz. */
+        /* FIXME: msleep ?? */
+        mdelay(20);
+}
+
+
diff --git a/drivers/staging/gma500/cdv_intel_crt.c b/drivers/staging/gma500/cdv_intel_crt.c
new file mode 100644
index 00000000000..efda63b97b4
--- /dev/null
+++ b/drivers/staging/gma500/cdv_intel_crt.c
@@ -0,0 +1,326 @@
+/*
+ * Copyright © 2006-2007 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>
+ */
+
+#include <linux/i2c.h>
+#include <drm/drmP.h>
+
+#include "intel_bios.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "power.h"
+#include <linux/pm_runtime.h>
+
+
+static void cdv_intel_crt_dpms(struct drm_encoder *encoder, int mode)
+{
+        struct drm_device *dev = encoder->dev;
+        u32 temp, reg;
+        reg = ADPA;
+
+        temp = REG_READ(reg);
+        temp &= ~(ADPA_HSYNC_CNTL_DISABLE | ADPA_VSYNC_CNTL_DISABLE);
+        temp &= ~ADPA_DAC_ENABLE;
+
+        switch (mode) {
+        case DRM_MODE_DPMS_ON:
+                temp |= ADPA_DAC_ENABLE;
+                break;
+        case DRM_MODE_DPMS_STANDBY:
+                temp |= ADPA_DAC_ENABLE | ADPA_HSYNC_CNTL_DISABLE;
+                break;
+        case DRM_MODE_DPMS_SUSPEND:
+                temp |= ADPA_DAC_ENABLE | ADPA_VSYNC_CNTL_DISABLE;
+                break;
+        case DRM_MODE_DPMS_OFF:
+                temp |= ADPA_HSYNC_CNTL_DISABLE | ADPA_VSYNC_CNTL_DISABLE;
+                break;
+        }
+
+        REG_WRITE(reg, temp);
+}
+
+static int cdv_intel_crt_mode_valid(struct drm_connector *connector,
+                                struct drm_display_mode *mode)
+{
+        int max_clock = 0;
+        if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+                return MODE_NO_DBLESCAN;
+
+        /* The lowest clock for CDV is 20000KHz */
+        if (mode->clock < 20000)
+                return MODE_CLOCK_LOW;
+
+        /* The max clock for CDV is 355 instead of 400 */
+        max_clock = 355000;
+        if (mode->clock > max_clock)
+                return MODE_CLOCK_HIGH;
+
+        if (mode->hdisplay > 1680 || mode->vdisplay > 1050)
+                return MODE_PANEL;
+
+        return MODE_OK;
+}
+
+static bool cdv_intel_crt_mode_fixup(struct drm_encoder *encoder,
+                                 struct drm_display_mode *mode,
+                                 struct drm_display_mode *adjusted_mode)
+{
+        return true;
+}
+
+static void cdv_intel_crt_mode_set(struct drm_encoder *encoder,
+                               struct drm_display_mode *mode,
+                               struct drm_display_mode *adjusted_mode)
+{
+
+        struct drm_device *dev = encoder->dev;
+        struct drm_crtc *crtc = encoder->crtc;
+        struct psb_intel_crtc *psb_intel_crtc =
+                                        to_psb_intel_crtc(crtc);
+        int dpll_md_reg;
+        u32 adpa, dpll_md;
+        u32 adpa_reg;
+
+        if (psb_intel_crtc->pipe == 0)
+                dpll_md_reg = DPLL_A_MD;
+        else
+                dpll_md_reg = DPLL_B_MD;
+
+        adpa_reg = ADPA;
+
+        /*
+         * Disable separate mode multiplier used when cloning SDVO to CRT
+         * XXX this needs to be adjusted when we really are cloning
+         */
+        {
+                dpll_md = REG_READ(dpll_md_reg);
+                REG_WRITE(dpll_md_reg,
+                           dpll_md & ~DPLL_MD_UDI_MULTIPLIER_MASK);
+        }
+
+        adpa = 0;
+        if (adjusted_mode->flags & DRM_MODE_FLAG_PHSYNC)
+                adpa |= ADPA_HSYNC_ACTIVE_HIGH;
+        if (adjusted_mode->flags & DRM_MODE_FLAG_PVSYNC)
+                adpa |= ADPA_VSYNC_ACTIVE_HIGH;
+
+        if (psb_intel_crtc->pipe == 0)
+                adpa |= ADPA_PIPE_A_SELECT;
+        else
+                adpa |= ADPA_PIPE_B_SELECT;
+
+        REG_WRITE(adpa_reg, adpa);
+}
+
+
+/**
+ * Uses CRT_HOTPLUG_EN and CRT_HOTPLUG_STAT to detect CRT presence.
+ *
+ * \return true if CRT is connected.
+ * \return false if CRT is disconnected.
+ */
+static bool cdv_intel_crt_detect_hotplug(struct drm_connector *connector,
+                                                                bool force)
+{
+        struct drm_device *dev = connector->dev;
+        u32 hotplug_en;
+        int i, tries = 0, ret = false;
+        u32 adpa_orig;
+
+        /* disable the DAC when doing the hotplug detection */
+
+        adpa_orig = REG_READ(ADPA);
+
+        REG_WRITE(ADPA, adpa_orig & ~(ADPA_DAC_ENABLE));
+
+        /*
+         * On a CDV thep, CRT detect sequence need to be done twice
+         * to get a reliable result.
+         */
+        tries = 2;
+
+        hotplug_en = REG_READ(PORT_HOTPLUG_EN);
+        hotplug_en &= ~(CRT_HOTPLUG_DETECT_MASK);
+        hotplug_en |= CRT_HOTPLUG_FORCE_DETECT;
+
+        hotplug_en |= CRT_HOTPLUG_ACTIVATION_PERIOD_64;
+        hotplug_en |= CRT_HOTPLUG_VOLTAGE_COMPARE_50;
+
+        for (i = 0; i < tries ; i++) {
+                unsigned long timeout;
+                /* turn on the FORCE_DETECT */
+                REG_WRITE(PORT_HOTPLUG_EN, hotplug_en);
+                timeout = jiffies + msecs_to_jiffies(1000);
+                /* wait for FORCE_DETECT to go off */
+                do {
+                        if (!(REG_READ(PORT_HOTPLUG_EN) &
+                                        CRT_HOTPLUG_FORCE_DETECT))
+                                break;
+                        msleep(1);
+                } while (time_after(timeout, jiffies));
+        }
+
+        if ((REG_READ(PORT_HOTPLUG_STAT) & CRT_HOTPLUG_MONITOR_MASK) !=
+            CRT_HOTPLUG_MONITOR_NONE)
+                ret = true;
+
+        /* Restore the saved ADPA */
+        REG_WRITE(ADPA, adpa_orig);
+        return ret;
+}
+
+static enum drm_connector_status cdv_intel_crt_detect(
+                                struct drm_connector *connector, bool force)
+{
+        if (cdv_intel_crt_detect_hotplug(connector, force))
+                return connector_status_connected;
+        else
+                return connector_status_disconnected;
+}
+
+static void cdv_intel_crt_destroy(struct drm_connector *connector)
+{
+        struct psb_intel_output *intel_output = to_psb_intel_output(connector);
+
+        psb_intel_i2c_destroy(intel_output->ddc_bus);
+        drm_sysfs_connector_remove(connector);
+        drm_connector_cleanup(connector);
+        kfree(connector);
+}
+
+static int cdv_intel_crt_get_modes(struct drm_connector *connector)
+{
+        struct psb_intel_output *intel_output =
+                                to_psb_intel_output(connector);
+        return psb_intel_ddc_get_modes(intel_output);
+}
+
+static int cdv_intel_crt_set_property(struct drm_connector *connector,
+                                  struct drm_property *property,
+                                  uint64_t value)
+{
+        return 0;
+}
+
+/*
+ * Routines for controlling stuff on the analog port
+ */
+
+static const struct drm_encoder_helper_funcs cdv_intel_crt_helper_funcs = {
+        .dpms = cdv_intel_crt_dpms,
+        .mode_fixup = cdv_intel_crt_mode_fixup,
+        .prepare = psb_intel_encoder_prepare,
+        .commit = psb_intel_encoder_commit,
+        .mode_set = cdv_intel_crt_mode_set,
+};
+
+static const struct drm_connector_funcs cdv_intel_crt_connector_funcs = {
+        .dpms = drm_helper_connector_dpms,
+        .detect = cdv_intel_crt_detect,
+        .fill_modes = drm_helper_probe_single_connector_modes,
+        .destroy = cdv_intel_crt_destroy,
+        .set_property = cdv_intel_crt_set_property,
+};
+
+static const struct drm_connector_helper_funcs
+                                cdv_intel_crt_connector_helper_funcs = {
+        .mode_valid = cdv_intel_crt_mode_valid,
+        .get_modes = cdv_intel_crt_get_modes,
+        .best_encoder = psb_intel_best_encoder,
+};
+
+static void cdv_intel_crt_enc_destroy(struct drm_encoder *encoder)
+{
+        drm_encoder_cleanup(encoder);
+}
+
+static const struct drm_encoder_funcs cdv_intel_crt_enc_funcs = {
+        .destroy = cdv_intel_crt_enc_destroy,
+};
+
+void cdv_intel_crt_init(struct drm_device *dev,
+                        struct psb_intel_mode_device *mode_dev)
+{
+
+        struct psb_intel_output *psb_intel_output;
+        struct drm_connector *connector;
+        struct drm_encoder *encoder;
+
+        u32 i2c_reg;
+
+        psb_intel_output = kzalloc(sizeof(struct psb_intel_output), GFP_KERNEL);
+        if (!psb_intel_output)
+                return;
+
+        psb_intel_output->mode_dev = mode_dev;
+        connector = &psb_intel_output->base;
+        drm_connector_init(dev, connector,
+                &cdv_intel_crt_connector_funcs, DRM_MODE_CONNECTOR_VGA);
+
+        encoder = &psb_intel_output->enc;
+        drm_encoder_init(dev, encoder,
+                &cdv_intel_crt_enc_funcs, DRM_MODE_ENCODER_DAC);
+
+        drm_mode_connector_attach_encoder(&psb_intel_output->base,
+                                          &psb_intel_output->enc);
+
+        /* Set up the DDC bus. */
+        i2c_reg = GPIOA;
+        /* Remove the following code for CDV */
+        /*
+        if (dev_priv->crt_ddc_bus != 0)
+                i2c_reg = dev_priv->crt_ddc_bus;
+        }*/
+        psb_intel_output->ddc_bus = psb_intel_i2c_create(dev,
+                                                i2c_reg, "CRTDDC_A");
+        if (!psb_intel_output->ddc_bus) {
+                dev_printk(KERN_ERR, &dev->pdev->dev, "DDC bus registration "
+                           "failed.\n");
+                goto failed_ddc;
+        }
+
+        psb_intel_output->type = INTEL_OUTPUT_ANALOG;
+        /*
+        psb_intel_output->clone_mask = (1 << INTEL_ANALOG_CLONE_BIT);
+        psb_intel_output->crtc_mask = (1 << 0) | (1 << 1);
+        */
+        connector->interlace_allowed = 0;
+        connector->doublescan_allowed = 0;
+
+        drm_encoder_helper_add(encoder, &cdv_intel_crt_helper_funcs);
+        drm_connector_helper_add(connector,
+                                        &cdv_intel_crt_connector_helper_funcs);
+
+        drm_sysfs_connector_add(connector);
+
+        return;
+failed_ddc:
+        drm_encoder_cleanup(&psb_intel_output->enc);
+        drm_connector_cleanup(&psb_intel_output->base);
+        kfree(psb_intel_output);
+        return;
+}
diff --git a/drivers/staging/gma500/cdv_intel_display.c b/drivers/staging/gma500/cdv_intel_display.c
new file mode 100644
index 00000000000..7b97c600eff
--- /dev/null
+++ b/drivers/staging/gma500/cdv_intel_display.c
@@ -0,0 +1,1508 @@
+/*
+ * Copyright © 2006-2011 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors:
+ *      Eric Anholt <eric@anholt.net>
+ */
+
+#include <linux/i2c.h>
+#include <linux/pm_runtime.h>
+
+#include <drm/drmP.h>
+#include "framebuffer.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_intel_display.h"
+#include "power.h"
+#include "cdv_device.h"
+
+
+struct cdv_intel_range_t {
+        int min, max;
+};
+
+struct cdv_intel_p2_t {
+        int dot_limit;
+        int p2_slow, p2_fast;
+};
+
+struct cdv_intel_clock_t {
+        /* given values */
+        int n;
+        int m1, m2;
+        int p1, p2;
+        /* derived values */
+        int dot;
+        int vco;
+        int m;
+        int p;
+};
+
+#define INTEL_P2_NUM                  2
+
+struct cdv_intel_limit_t {
+        struct cdv_intel_range_t dot, vco, n, m, m1, m2, p, p1;
+        struct cdv_intel_p2_t p2;
+};
+
+#define CDV_LIMIT_SINGLE_LVDS_96        0
+#define CDV_LIMIT_SINGLE_LVDS_100       1
+#define CDV_LIMIT_DAC_HDMI_27           2
+#define CDV_LIMIT_DAC_HDMI_96           3
+
+static const struct cdv_intel_limit_t cdv_intel_limits[] = {
+        {                       /* CDV_SIGNLE_LVDS_96MHz */
+         .dot = {.min = 20000, .max = 115500},
+         .vco = {.min = 1800000, .max = 3600000},
+         .n = {.min = 2, .max = 6},
+         .m = {.min = 60, .max = 160},
+         .m1 = {.min = 0, .max = 0},
+         .m2 = {.min = 58, .max = 158},
+         .p = {.min = 28, .max = 140},
+         .p1 = {.min = 2, .max = 10},
+         .p2 = {.dot_limit = 200000,
+                .p2_slow = 14, .p2_fast = 14},
+         },
+        {                       /* CDV_SINGLE_LVDS_100MHz */
+         .dot = {.min = 20000, .max = 115500},
+         .vco = {.min = 1800000, .max = 3600000},
+         .n = {.min = 2, .max = 6},
+         .m = {.min = 60, .max = 160},
+         .m1 = {.min = 0, .max = 0},
+         .m2 = {.min = 58, .max = 158},
+         .p = {.min = 28, .max = 140},
+         .p1 = {.min = 2, .max = 10},
+         /* The single-channel range is 25-112Mhz, and dual-channel
+          * is 80-224Mhz.  Prefer single channel as much as possible.
+          */
+         .p2 = {.dot_limit = 200000, .p2_slow = 14, .p2_fast = 14},
+         },
+        {                       /* CDV_DAC_HDMI_27MHz */
+         .dot = {.min = 20000, .max = 400000},
+         .vco = {.min = 1809000, .max = 3564000},
+         .n = {.min = 1, .max = 1},
+         .m = {.min = 67, .max = 132},
+         .m1 = {.min = 0, .max = 0},
+         .m2 = {.min = 65, .max = 130},
+         .p = {.min = 5, .max = 90},
+         .p1 = {.min = 1, .max = 9},
+         .p2 = {.dot_limit = 225000, .p2_slow = 10, .p2_fast = 5},
+         },
+        {                       /* CDV_DAC_HDMI_96MHz */
+         .dot = {.min = 20000, .max = 400000},
+         .vco = {.min = 1800000, .max = 3600000},
+         .n = {.min = 2, .max = 6},
+         .m = {.min = 60, .max = 160},
+         .m1 = {.min = 0, .max = 0},
+         .m2 = {.min = 58, .max = 158},
+         .p = {.min = 5, .max = 100},
+         .p1 = {.min = 1, .max = 10},
+         .p2 = {.dot_limit = 225000, .p2_slow = 10, .p2_fast = 5},
+         },
+};
+
+#define _wait_for(COND, MS, W) ({ \
+        unsigned long timeout__ = jiffies + msecs_to_jiffies(MS);       \
+        int ret__ = 0;                                                  \
+        while (!(COND)) {                                               \
+                if (time_after(jiffies, timeout__)) {                   \
+                        ret__ = -ETIMEDOUT;                             \
+                        break;                                          \
+                }                                                       \
+                if (W && !in_dbg_master())                              \
+                        msleep(W);                                      \
+        }                                                               \
+        ret__;                                                          \
+})
+
+#define wait_for(COND, MS) _wait_for(COND, MS, 1)
+
+
+static int cdv_sb_read(struct drm_device *dev, u32 reg, u32 *val)
+{
+        int ret;
+
+        ret = wait_for((REG_READ(SB_PCKT) & SB_BUSY) == 0, 1000);
+        if (ret) {
+                DRM_ERROR("timeout waiting for SB to idle before read\n");
+                return ret;
+        }
+
+        REG_WRITE(SB_ADDR, reg);
+        REG_WRITE(SB_PCKT,
+                   SET_FIELD(SB_OPCODE_READ, SB_OPCODE) |
+                   SET_FIELD(SB_DEST_DPLL, SB_DEST) |
+                   SET_FIELD(0xf, SB_BYTE_ENABLE));
+
+        ret = wait_for((REG_READ(SB_PCKT) & SB_BUSY) == 0, 1000);
+        if (ret) {
+                DRM_ERROR("timeout waiting for SB to idle after read\n");
+                return ret;
+        }
+
+        *val = REG_READ(SB_DATA);
+
+        return 0;
+}
+
+static int cdv_sb_write(struct drm_device *dev, u32 reg, u32 val)
+{
+        int ret;
+        static bool dpio_debug = true;
+        u32 temp;
+
+        if (dpio_debug) {
+                if (cdv_sb_read(dev, reg, &temp) == 0)
+                        DRM_DEBUG_KMS("0x%08x: 0x%08x (before)\n", reg, temp);
+                DRM_DEBUG_KMS("0x%08x: 0x%08x\n", reg, val);
+        }
+
+        ret = wait_for((REG_READ(SB_PCKT) & SB_BUSY) == 0, 1000);
+        if (ret) {
+                DRM_ERROR("timeout waiting for SB to idle before write\n");
+                return ret;
+        }
+
+        REG_WRITE(SB_ADDR, reg);
+        REG_WRITE(SB_DATA, val);
+        REG_WRITE(SB_PCKT,
+                   SET_FIELD(SB_OPCODE_WRITE, SB_OPCODE) |
+                   SET_FIELD(SB_DEST_DPLL, SB_DEST) |
+                   SET_FIELD(0xf, SB_BYTE_ENABLE));
+
+        ret = wait_for((REG_READ(SB_PCKT) & SB_BUSY) == 0, 1000);
+        if (ret) {
+                DRM_ERROR("timeout waiting for SB to idle after write\n");
+                return ret;
+        }
+
+        if (dpio_debug) {
+                if (cdv_sb_read(dev, reg, &temp) == 0)
+                        DRM_DEBUG_KMS("0x%08x: 0x%08x (after)\n", reg, temp);
+        }
+
+        return 0;
+}
+
+/* Reset the DPIO configuration register.  The BIOS does this at every
+ * mode set.
+ */
+static void cdv_sb_reset(struct drm_device *dev)
+{
+
+        REG_WRITE(DPIO_CFG, 0);
+        REG_READ(DPIO_CFG);
+        REG_WRITE(DPIO_CFG, DPIO_MODE_SELECT_0 | DPIO_CMN_RESET_N);
+}
+
+/* Unlike most Intel display engines, on Cedarview the DPLL registers
+ * are behind this sideband bus.  They must be programmed while the
+ * DPLL reference clock is on in the DPLL control register, but before
+ * the DPLL is enabled in the DPLL control register.
+ */
+static int
+cdv_dpll_set_clock_cdv(struct drm_device *dev, struct drm_crtc *crtc,
+                               struct cdv_intel_clock_t *clock)
+{
+        struct psb_intel_crtc *psb_crtc =
+                                to_psb_intel_crtc(crtc);
+        int pipe = psb_crtc->pipe;
+        u32 m, n_vco, p;
+        int ret = 0;
+        int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
+        u32 ref_value;
+
+        cdv_sb_reset(dev);
+
+        if ((REG_READ(dpll_reg) & DPLL_SYNCLOCK_ENABLE) == 0) {
+                DRM_ERROR("Attempting to set DPLL with refclk disabled\n");
+                return -EBUSY;
+        }
+
+        /* Follow the BIOS and write the REF/SFR Register. Hardcoded value */
+        ref_value = 0x68A701;
+
+        cdv_sb_write(dev, SB_REF_SFR(pipe), ref_value);
+
+        /* We don't know what the other fields of these regs are, so
+         * leave them in place.
+         */
+        ret = cdv_sb_read(dev, SB_M(pipe), &m);
+        if (ret)
+                return ret;
+        m &= ~SB_M_DIVIDER_MASK;
+        m |= ((clock->m2) << SB_M_DIVIDER_SHIFT);
+        ret = cdv_sb_write(dev, SB_M(pipe), m);
+        if (ret)
+                return ret;
+
+        ret = cdv_sb_read(dev, SB_N_VCO(pipe), &n_vco);
+        if (ret)
+                return ret;
+
+        /* Follow the BIOS to program the N_DIVIDER REG */
+        n_vco &= 0xFFFF;
+        n_vco |= 0x107;
+        n_vco &= ~(SB_N_VCO_SEL_MASK |
+                   SB_N_DIVIDER_MASK |
+                   SB_N_CB_TUNE_MASK);
+
+        n_vco |= ((clock->n) << SB_N_DIVIDER_SHIFT);
+
+        if (clock->vco < 2250000) {
+                n_vco |= (2 << SB_N_CB_TUNE_SHIFT);
+                n_vco |= (0 << SB_N_VCO_SEL_SHIFT);
+        } else if (clock->vco < 2750000) {
+                n_vco |= (1 << SB_N_CB_TUNE_SHIFT);
+                n_vco |= (1 << SB_N_VCO_SEL_SHIFT);
+        } else if (clock->vco < 3300000) {
+                n_vco |= (0 << SB_N_CB_TUNE_SHIFT);
+                n_vco |= (2 << SB_N_VCO_SEL_SHIFT);
+        } else {
+                n_vco |= (0 << SB_N_CB_TUNE_SHIFT);
+                n_vco |= (3 << SB_N_VCO_SEL_SHIFT);
+        }
+
+        ret = cdv_sb_write(dev, SB_N_VCO(pipe), n_vco);
+        if (ret)
+                return ret;
+
+        ret = cdv_sb_read(dev, SB_P(pipe), &p);
+        if (ret)
+                return ret;
+        p &= ~(SB_P2_DIVIDER_MASK | SB_P1_DIVIDER_MASK);
+        p |= SET_FIELD(clock->p1, SB_P1_DIVIDER);
+        switch (clock->p2) {
+        case 5:
+                p |= SET_FIELD(SB_P2_5, SB_P2_DIVIDER);
+                break;
+        case 10:
+                p |= SET_FIELD(SB_P2_10, SB_P2_DIVIDER);
+                break;
+        case 14:
+                p |= SET_FIELD(SB_P2_14, SB_P2_DIVIDER);
+                break;
+        case 7:
+                p |= SET_FIELD(SB_P2_7, SB_P2_DIVIDER);
+                break;
+        default:
+                DRM_ERROR("Bad P2 clock: %d\n", clock->p2);
+                return -EINVAL;
+        }
+        ret = cdv_sb_write(dev, SB_P(pipe), p);
+        if (ret)
+                return ret;
+
+        /* always Program the Lane Register for the Pipe A*/
+        if (pipe == 0) {
+                /* Program the Lane0/1 for HDMI B */
+                u32 lane_reg, lane_value;
+
+                lane_reg = PSB_LANE0;
+                cdv_sb_read(dev, lane_reg, &lane_value);
+                lane_value &= ~(LANE_PLL_MASK);
+                lane_value |= LANE_PLL_ENABLE;
+                cdv_sb_write(dev, lane_reg, lane_value);
+
+                lane_reg = PSB_LANE1;
+                cdv_sb_read(dev, lane_reg, &lane_value);
+                lane_value &= ~(LANE_PLL_MASK);
+                lane_value |= LANE_PLL_ENABLE;
+                cdv_sb_write(dev, lane_reg, lane_value);
+
+                /* Program the Lane2/3 for HDMI C */
+                lane_reg = PSB_LANE2;
+                cdv_sb_read(dev, lane_reg, &lane_value);
+                lane_value &= ~(LANE_PLL_MASK);
+                lane_value |= LANE_PLL_ENABLE;
+                cdv_sb_write(dev, lane_reg, lane_value);
+
+                lane_reg = PSB_LANE3;
+                cdv_sb_read(dev, lane_reg, &lane_value);
+                lane_value &= ~(LANE_PLL_MASK);
+                lane_value |= LANE_PLL_ENABLE;
+                cdv_sb_write(dev, lane_reg, lane_value);
+        }
+
+        return 0;
+}
+
+/*
+ * Returns whether any output on the specified pipe is of the specified type
+ */
+bool cdv_intel_pipe_has_type(struct drm_crtc *crtc, int type)
+{
+        struct drm_device *dev = crtc->dev;
+        struct drm_mode_config *mode_config = &dev->mode_config;
+        struct drm_connector *l_entry;
+
+        list_for_each_entry(l_entry, &mode_config->connector_list, head) {
+                if (l_entry->encoder && l_entry->encoder->crtc == crtc) {
+                        struct psb_intel_output *psb_intel_output =
+                            to_psb_intel_output(l_entry);
+                        if (psb_intel_output->type == type)
+                                return true;
+                }
+        }
+        return false;
+}
+
+static const struct cdv_intel_limit_t *cdv_intel_limit(struct drm_crtc *crtc,
+                                                        int refclk)
+{
+        const struct cdv_intel_limit_t *limit;
+        if (cdv_intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)) {
+                /*
+                 * Now only single-channel LVDS is supported on CDV. If it is
+                 * incorrect, please add the dual-channel LVDS.
+                 */
+                if (refclk == 96000)
+                        limit = &cdv_intel_limits[CDV_LIMIT_SINGLE_LVDS_96];
+                else
+                        limit = &cdv_intel_limits[CDV_LIMIT_SINGLE_LVDS_100];
+        } else {
+                if (refclk == 27000)
+                        limit = &cdv_intel_limits[CDV_LIMIT_DAC_HDMI_27];
+                else
+                        limit = &cdv_intel_limits[CDV_LIMIT_DAC_HDMI_96];
+        }
+        return limit;
+}
+
+/* m1 is reserved as 0 in CDV, n is a ring counter */
+static void cdv_intel_clock(struct drm_device *dev,
+                        int refclk, struct cdv_intel_clock_t *clock)
+{
+        clock->m = clock->m2 + 2;
+        clock->p = clock->p1 * clock->p2;
+        clock->vco = (refclk * clock->m) / clock->n;
+        clock->dot = clock->vco / clock->p;
+}
+
+
+#define INTELPllInvalid(s)   { /* ErrorF (s) */; return false; }
+static bool cdv_intel_PLL_is_valid(struct drm_crtc *crtc,
+                                const struct cdv_intel_limit_t *limit,
+                               struct cdv_intel_clock_t *clock)
+{
+        if (clock->p1 < limit->p1.min || limit->p1.max < clock->p1)
+                INTELPllInvalid("p1 out of range\n");
+        if (clock->p < limit->p.min || limit->p.max < clock->p)
+                INTELPllInvalid("p out of range\n");
+        /* unnecessary to check the range of m(m1/M2)/n again */
+        if (clock->vco < limit->vco.min || limit->vco.max < clock->vco)
+                INTELPllInvalid("vco out of range\n");
+        /* XXX: We may need to be checking "Dot clock"
+         * depending on the multiplier, connector, etc.,
+         * rather than just a single range.
+         */
+        if (clock->dot < limit->dot.min || limit->dot.max < clock->dot)
+                INTELPllInvalid("dot out of range\n");
+
+        return true;
+}
+
+static bool cdv_intel_find_best_PLL(struct drm_crtc *crtc, int target,
+                                int refclk,
+                                struct cdv_intel_clock_t *best_clock)
+{
+        struct drm_device *dev = crtc->dev;
+        struct cdv_intel_clock_t clock;
+        const struct cdv_intel_limit_t *limit = cdv_intel_limit(crtc, refclk);
+        int err = target;
+
+
+        if (cdv_intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
+            (REG_READ(LVDS) & LVDS_PORT_EN) != 0) {
+                /*
+                 * For LVDS, if the panel is on, just rely on its current
+                 * settings for dual-channel.  We haven't figured out how to
+                 * reliably set up different single/dual channel state, if we
+                 * even can.
+                 */
+                if ((REG_READ(LVDS) & LVDS_CLKB_POWER_MASK) ==
+                    LVDS_CLKB_POWER_UP)
+                        clock.p2 = limit->p2.p2_fast;
+                else
+                        clock.p2 = limit->p2.p2_slow;
+        } else {
+                if (target < limit->p2.dot_limit)
+                        clock.p2 = limit->p2.p2_slow;
+                else
+                        clock.p2 = limit->p2.p2_fast;
+        }
+
+        memset(best_clock, 0, sizeof(*best_clock));
+        clock.m1 = 0;
+        /* m1 is reserved as 0 in CDV, n is a ring counter.
+           So skip the m1 loop */
+        for (clock.n = limit->n.min; clock.n <= limit->n.max; clock.n++) {
+                for (clock.m2 = limit->m2.min; clock.m2 <= limit->m2.max;
+                                             clock.m2++) {
+                        for (clock.p1 = limit->p1.min;
+                                        clock.p1 <= limit->p1.max;
+                                        clock.p1++) {
+                                int this_err;
+
+                                cdv_intel_clock(dev, refclk, &clock);
+
+                                if (!cdv_intel_PLL_is_valid(crtc,
+                                                                limit, &clock))
+                                                continue;
+
+                                this_err = abs(clock.dot - target);
+                                if (this_err < err) {
+                                        *best_clock = clock;
+                                        err = this_err;
+                                }
+                        }
+                }
+        }
+
+        return err != target;
+}
+
+int cdv_intel_pipe_set_base(struct drm_crtc *crtc,
+                            int x, int y, struct drm_framebuffer *old_fb)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
+        int pipe = psb_intel_crtc->pipe;
+        unsigned long start, offset;
+        int dspbase = (pipe == 0 ? DSPABASE : DSPBBASE);
+        int dspsurf = (pipe == 0 ? DSPASURF : DSPBSURF);
+        int dspstride = (pipe == 0) ? DSPASTRIDE : DSPBSTRIDE;
+        int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+        u32 dspcntr;
+        int ret = 0;
+
+        if (!gma_power_begin(dev, true))
+                return 0;
+
+        /* no fb bound */
+        if (!crtc->fb) {
+                dev_err(dev->dev, "No FB bound\n");
+                goto psb_intel_pipe_cleaner;
+        }
+
+
+        /* We are displaying this buffer, make sure it is actually loaded
+           into the GTT */
+        ret = psb_gtt_pin(psbfb->gtt);
+        if (ret < 0)
+                goto psb_intel_pipe_set_base_exit;
+        start = psbfb->gtt->offset;
+        offset = y * crtc->fb->pitch + x * (crtc->fb->bits_per_pixel / 8);
+
+        REG_WRITE(dspstride, crtc->fb->pitch);
+
+        dspcntr = REG_READ(dspcntr_reg);
+        dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
+
+        switch (crtc->fb->bits_per_pixel) {
+        case 8:
+                dspcntr |= DISPPLANE_8BPP;
+                break;
+        case 16:
+                if (crtc->fb->depth == 15)
+                        dspcntr |= DISPPLANE_15_16BPP;
+                else
+                        dspcntr |= DISPPLANE_16BPP;
+                break;
+        case 24:
+        case 32:
+                dspcntr |= DISPPLANE_32BPP_NO_ALPHA;
+                break;
+        default:
+                dev_err(dev->dev, "Unknown color depth\n");
+                ret = -EINVAL;
+                goto psb_intel_pipe_set_base_exit;
+        }
+        REG_WRITE(dspcntr_reg, dspcntr);
+
+        dev_dbg(dev->dev,
+                "Writing base %08lX %08lX %d %d\n", start, offset, x, y);
+
+        REG_WRITE(dspbase, offset);
+        REG_READ(dspbase);
+        REG_WRITE(dspsurf, start);
+        REG_READ(dspsurf);
+
+psb_intel_pipe_cleaner:
+        /* If there was a previous display we can now unpin it */
+        if (old_fb)
+                psb_gtt_unpin(to_psb_fb(old_fb)->gtt);
+
+psb_intel_pipe_set_base_exit:
+        gma_power_end(dev);
+        return ret;
+}
+
+/**
+ * Sets the power management mode of the pipe and plane.
+ *
+ * This code should probably grow support for turning the cursor off and back
+ * on appropriately at the same time as we're turning the pipe off/on.
+ */
+static void cdv_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
+        int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+        int dspbase_reg = (pipe == 0) ? DSPABASE : DSPBBASE;
+        int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+        u32 temp;
+        bool enabled;
+
+        /* XXX: When our outputs are all unaware of DPMS modes other than off
+         * and on, we should map those modes to DRM_MODE_DPMS_OFF in the CRTC.
+         */
+        switch (mode) {
+        case DRM_MODE_DPMS_ON:
+        case DRM_MODE_DPMS_STANDBY:
+        case DRM_MODE_DPMS_SUSPEND:
+                /* Enable the DPLL */
+                temp = REG_READ(dpll_reg);
+                if ((temp & DPLL_VCO_ENABLE) == 0) {
+                        REG_WRITE(dpll_reg, temp);
+                        REG_READ(dpll_reg);
+                        /* Wait for the clocks to stabilize. */
+                        udelay(150);
+                        REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+                        REG_READ(dpll_reg);
+                        /* Wait for the clocks to stabilize. */
+                        udelay(150);
+                        REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+                        REG_READ(dpll_reg);
+                        /* Wait for the clocks to stabilize. */
+                        udelay(150);
+                }
+
+                /* Jim Bish - switch plan and pipe per scott */
+                /* Enable the plane */
+                temp = REG_READ(dspcntr_reg);
+                if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
+                        REG_WRITE(dspcntr_reg,
+                                  temp | DISPLAY_PLANE_ENABLE);
+                        /* Flush the plane changes */
+                        REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+                }
+
+                udelay(150);
+
+                /* Enable the pipe */
+                temp = REG_READ(pipeconf_reg);
+                if ((temp & PIPEACONF_ENABLE) == 0)
+                        REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
+
+                psb_intel_crtc_load_lut(crtc);
+
+                /* Give the overlay scaler a chance to enable
+                 * if it's on this pipe */
+                /* psb_intel_crtc_dpms_video(crtc, true); TODO */
+                break;
+        case DRM_MODE_DPMS_OFF:
+                /* Give the overlay scaler a chance to disable
+                 * if it's on this pipe */
+                /* psb_intel_crtc_dpms_video(crtc, FALSE); TODO */
+
+                /* Disable the VGA plane that we never use */
+                REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+
+                /* Jim Bish - changed pipe/plane here as well. */
+
+                /* Wait for vblank for the disable to take effect */
+                cdv_intel_wait_for_vblank(dev);
+
+                /* Next, disable display pipes */
+                temp = REG_READ(pipeconf_reg);
+                if ((temp & PIPEACONF_ENABLE) != 0) {
+                        REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
+                        REG_READ(pipeconf_reg);
+                }
+
+                /* Wait for vblank for the disable to take effect. */
+                cdv_intel_wait_for_vblank(dev);
+
+                udelay(150);
+
+                /* Disable display plane */
+                temp = REG_READ(dspcntr_reg);
+                if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
+                        REG_WRITE(dspcntr_reg,
+                                  temp & ~DISPLAY_PLANE_ENABLE);
+                        /* Flush the plane changes */
+                        REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+                        REG_READ(dspbase_reg);
+                }
+
+                temp = REG_READ(dpll_reg);
+                if ((temp & DPLL_VCO_ENABLE) != 0) {
+                        REG_WRITE(dpll_reg, temp & ~DPLL_VCO_ENABLE);
+                        REG_READ(dpll_reg);
+                }
+
+                /* Wait for the clocks to turn off. */
+                udelay(150);
+                break;
+        }
+        enabled = crtc->enabled && mode != DRM_MODE_DPMS_OFF;
+        /*Set FIFO Watermarks*/
+        REG_WRITE(DSPARB, 0x3F3E);
+}
+
+static void cdv_intel_crtc_prepare(struct drm_crtc *crtc)
+{
+        struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+        crtc_funcs->dpms(crtc, DRM_MODE_DPMS_OFF);
+}
+
+static void cdv_intel_crtc_commit(struct drm_crtc *crtc)
+{
+        struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+        crtc_funcs->dpms(crtc, DRM_MODE_DPMS_ON);
+}
+
+void cdv_intel_encoder_prepare(struct drm_encoder *encoder)
+{
+        struct drm_encoder_helper_funcs *encoder_funcs =
+            encoder->helper_private;
+        /* lvds has its own version of prepare see cdv_intel_lvds_prepare */
+        encoder_funcs->dpms(encoder, DRM_MODE_DPMS_OFF);
+}
+
+void cdv_intel_encoder_commit(struct drm_encoder *encoder)
+{
+        struct drm_encoder_helper_funcs *encoder_funcs =
+            encoder->helper_private;
+        /* lvds has its own version of commit see cdv_intel_lvds_commit */
+        encoder_funcs->dpms(encoder, DRM_MODE_DPMS_ON);
+}
+
+static bool cdv_intel_crtc_mode_fixup(struct drm_crtc *crtc,
+                                  struct drm_display_mode *mode,
+                                  struct drm_display_mode *adjusted_mode)
+{
+        return true;
+}
+
+
+/**
+ * Return the pipe currently connected to the panel fitter,
+ * or -1 if the panel fitter is not present or not in use
+ */
+static int cdv_intel_panel_fitter_pipe(struct drm_device *dev)
+{
+        u32 pfit_control;
+
+        pfit_control = REG_READ(PFIT_CONTROL);
+
+        /* See if the panel fitter is in use */
+        if ((pfit_control & PFIT_ENABLE) == 0)
+                return -1;
+        return (pfit_control >> 29) & 0x3;
+}
+
+static int cdv_intel_crtc_mode_set(struct drm_crtc *crtc,
+                               struct drm_display_mode *mode,
+                               struct drm_display_mode *adjusted_mode,
+                               int x, int y,
+                               struct drm_framebuffer *old_fb)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
+        int dpll_md_reg = (psb_intel_crtc->pipe == 0) ? DPLL_A_MD : DPLL_B_MD;
+        int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+        int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+        int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
+        int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
+        int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
+        int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
+        int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
+        int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
+        int dspsize_reg = (pipe == 0) ? DSPASIZE : DSPBSIZE;
+        int dsppos_reg = (pipe == 0) ? DSPAPOS : DSPBPOS;
+        int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+        int refclk;
+        struct cdv_intel_clock_t clock;
+        u32 dpll = 0, dspcntr, pipeconf;
+        bool ok, is_sdvo = false, is_dvo = false;
+        bool is_crt = false, is_lvds = false, is_tv = false;
+        bool is_hdmi = false;
+        struct drm_mode_config *mode_config = &dev->mode_config;
+        struct drm_connector *connector;
+
+        list_for_each_entry(connector, &mode_config->connector_list, head) {
+                struct psb_intel_output *psb_intel_output =
+                    to_psb_intel_output(connector);
+
+                if (!connector->encoder
+                    || connector->encoder->crtc != crtc)
+                        continue;
+
+                switch (psb_intel_output->type) {
+                case INTEL_OUTPUT_LVDS:
+                        is_lvds = true;
+                        break;
+                case INTEL_OUTPUT_SDVO:
+                        is_sdvo = true;
+                        break;
+                case INTEL_OUTPUT_DVO:
+                        is_dvo = true;
+                        break;
+                case INTEL_OUTPUT_TVOUT:
+                        is_tv = true;
+                        break;
+                case INTEL_OUTPUT_ANALOG:
+                        is_crt = true;
+                        break;
+                case INTEL_OUTPUT_HDMI:
+                        is_hdmi = true;
+                        break;
+                }
+        }
+
+        refclk = 96000;
+
+        /* Hack selection about ref clk for CRT */
+        /* Select 27MHz as the reference clk for HDMI */
+        if (is_crt || is_hdmi)
+                refclk = 27000;
+
+        drm_mode_debug_printmodeline(adjusted_mode);
+
+        ok = cdv_intel_find_best_PLL(crtc, adjusted_mode->clock, refclk,
+                                 &clock);
+        if (!ok) {
+                dev_err(dev->dev, "Couldn't find PLL settings for mode!\n");
+                return 0;
+        }
+
+        dpll = DPLL_VGA_MODE_DIS;
+        if (is_tv) {
+                /* XXX: just matching BIOS for now */
+/*      dpll |= PLL_REF_INPUT_TVCLKINBC; */
+                dpll |= 3;
+        }
+                dpll |= PLL_REF_INPUT_DREFCLK;
+
+        dpll |= DPLL_SYNCLOCK_ENABLE;
+        dpll |= DPLL_VGA_MODE_DIS;
+        if (is_lvds)
+                dpll |= DPLLB_MODE_LVDS;
+        else
+                dpll |= DPLLB_MODE_DAC_SERIAL;
+        /* dpll |= (2 << 11); */
+
+        /* setup pipeconf */
+        pipeconf = REG_READ(pipeconf_reg);
+
+        /* Set up the display plane register */
+        dspcntr = DISPPLANE_GAMMA_ENABLE;
+
+        if (pipe == 0)
+                dspcntr |= DISPPLANE_SEL_PIPE_A;
+        else
+                dspcntr |= DISPPLANE_SEL_PIPE_B;
+
+        dspcntr |= DISPLAY_PLANE_ENABLE;
+        pipeconf |= PIPEACONF_ENABLE;
+
+        REG_WRITE(dpll_reg, dpll | DPLL_VGA_MODE_DIS | DPLL_SYNCLOCK_ENABLE);
+        REG_READ(dpll_reg);
+
+        cdv_dpll_set_clock_cdv(dev, crtc, &clock);
+
+        udelay(150);
+
+
+        /* The LVDS pin pair needs to be on before the DPLLs are enabled.
+         * This is an exception to the general rule that mode_set doesn't turn
+         * things on.
+         */
+        if (is_lvds) {
+                u32 lvds = REG_READ(LVDS);
+
+                lvds |=
+                    LVDS_PORT_EN | LVDS_A0A2_CLKA_POWER_UP |
+                    LVDS_PIPEB_SELECT;
+                /* Set the B0-B3 data pairs corresponding to
+                 * whether we're going to
+                 * set the DPLLs for dual-channel mode or not.
+                 */
+                if (clock.p2 == 7)
+                        lvds |= LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP;
+                else
+                        lvds &= ~(LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP);
+
+                /* It would be nice to set 24 vs 18-bit mode (LVDS_A3_POWER_UP)
+                 * appropriately here, but we need to look more
+                 * thoroughly into how panels behave in the two modes.
+                 */
+
+                REG_WRITE(LVDS, lvds);
+                REG_READ(LVDS);
+        }
+
+        dpll |= DPLL_VCO_ENABLE;
+
+        /* Disable the panel fitter if it was on our pipe */
+        if (cdv_intel_panel_fitter_pipe(dev) == pipe)
+                REG_WRITE(PFIT_CONTROL, 0);
+
+        DRM_DEBUG_KMS("Mode for pipe %c:\n", pipe == 0 ? 'A' : 'B');
+        drm_mode_debug_printmodeline(mode);
+
+        REG_WRITE(dpll_reg,
+                (REG_READ(dpll_reg) & ~DPLL_LOCK) | DPLL_VCO_ENABLE);
+        REG_READ(dpll_reg);
+        /* Wait for the clocks to stabilize. */
+        udelay(150); /* 42 usec w/o calibration, 110 with.  rounded up. */
+
+        if (!(REG_READ(dpll_reg) & DPLL_LOCK)) {
+                dev_err(dev->dev, "Failed to get DPLL lock\n");
+                return -EBUSY;
+        }
+
+        {
+                int sdvo_pixel_multiply = adjusted_mode->clock / mode->clock;
+                REG_WRITE(dpll_md_reg, (0 << DPLL_MD_UDI_DIVIDER_SHIFT) | ((sdvo_pixel_multiply - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT));
+        }
+
+        REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+                  ((adjusted_mode->crtc_htotal - 1) << 16));
+        REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+                  ((adjusted_mode->crtc_hblank_end - 1) << 16));
+        REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+                  ((adjusted_mode->crtc_hsync_end - 1) << 16));
+        REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+                  ((adjusted_mode->crtc_vtotal - 1) << 16));
+        REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+                  ((adjusted_mode->crtc_vblank_end - 1) << 16));
+        REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+                  ((adjusted_mode->crtc_vsync_end - 1) << 16));
+        /* pipesrc and dspsize control the size that is scaled from,
+         * which should always be the user's requested size.
+         */
+        REG_WRITE(dspsize_reg,
+                  ((mode->vdisplay - 1) << 16) | (mode->hdisplay - 1));
+        REG_WRITE(dsppos_reg, 0);
+        REG_WRITE(pipesrc_reg,
+                  ((mode->hdisplay - 1) << 16) | (mode->vdisplay - 1));
+        REG_WRITE(pipeconf_reg, pipeconf);
+        REG_READ(pipeconf_reg);
+
+        cdv_intel_wait_for_vblank(dev);
+
+        REG_WRITE(dspcntr_reg, dspcntr);
+
+        /* Flush the plane changes */
+        {
+                struct drm_crtc_helper_funcs *crtc_funcs =
+                    crtc->helper_private;
+                crtc_funcs->mode_set_base(crtc, x, y, old_fb);
+        }
+
+        cdv_intel_wait_for_vblank(dev);
+
+        return 0;
+}
+
+/** Loads the palette/gamma unit for the CRTC with the prepared values */
+void cdv_intel_crtc_load_lut(struct drm_crtc *crtc)
+{
+        struct drm_device *dev = crtc->dev;
+        struct drm_psb_private *dev_priv =
+                                (struct drm_psb_private *)dev->dev_private;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int palreg = PALETTE_A;
+        int i;
+
+        /* The clocks have to be on to load the palette. */
+        if (!crtc->enabled)
+                return;
+
+        switch (psb_intel_crtc->pipe) {
+        case 0:
+                break;
+        case 1:
+                palreg = PALETTE_B;
+                break;
+        case 2:
+                palreg = PALETTE_C;
+                break;
+        default:
+                dev_err(dev->dev, "Illegal Pipe Number.\n");
+                return;
+        }
+
+        if (gma_power_begin(dev, false)) {
+                for (i = 0; i < 256; i++) {
+                        REG_WRITE(palreg + 4 * i,
+                                  ((psb_intel_crtc->lut_r[i] +
+                                  psb_intel_crtc->lut_adj[i]) << 16) |
+                                  ((psb_intel_crtc->lut_g[i] +
+                                  psb_intel_crtc->lut_adj[i]) << 8) |
+                                  (psb_intel_crtc->lut_b[i] +
+                                  psb_intel_crtc->lut_adj[i]));
+                }
+                gma_power_end(dev);
+        } else {
+                for (i = 0; i < 256; i++) {
+                        dev_priv->save_palette_a[i] =
+                                  ((psb_intel_crtc->lut_r[i] +
+                                  psb_intel_crtc->lut_adj[i]) << 16) |
+                                  ((psb_intel_crtc->lut_g[i] +
+                                  psb_intel_crtc->lut_adj[i]) << 8) |
+                                  (psb_intel_crtc->lut_b[i] +
+                                  psb_intel_crtc->lut_adj[i]);
+                }
+
+        }
+}
+
+/**
+ * Save HW states of giving crtc
+ */
+static void cdv_intel_crtc_save(struct drm_crtc *crtc)
+{
+        struct drm_device *dev = crtc->dev;
+        /* struct drm_psb_private *dev_priv =
+                        (struct drm_psb_private *)dev->dev_private; */
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
+        int pipeA = (psb_intel_crtc->pipe == 0);
+        uint32_t paletteReg;
+        int i;
+
+        if (!crtc_state) {
+                dev_dbg(dev->dev, "No CRTC state found\n");
+                return;
+        }
+
+        crtc_state->saveDSPCNTR = REG_READ(pipeA ? DSPACNTR : DSPBCNTR);
+        crtc_state->savePIPECONF = REG_READ(pipeA ? PIPEACONF : PIPEBCONF);
+        crtc_state->savePIPESRC = REG_READ(pipeA ? PIPEASRC : PIPEBSRC);
+        crtc_state->saveFP0 = REG_READ(pipeA ? FPA0 : FPB0);
+        crtc_state->saveFP1 = REG_READ(pipeA ? FPA1 : FPB1);
+        crtc_state->saveDPLL = REG_READ(pipeA ? DPLL_A : DPLL_B);
+        crtc_state->saveHTOTAL = REG_READ(pipeA ? HTOTAL_A : HTOTAL_B);
+        crtc_state->saveHBLANK = REG_READ(pipeA ? HBLANK_A : HBLANK_B);
+        crtc_state->saveHSYNC = REG_READ(pipeA ? HSYNC_A : HSYNC_B);
+        crtc_state->saveVTOTAL = REG_READ(pipeA ? VTOTAL_A : VTOTAL_B);
+        crtc_state->saveVBLANK = REG_READ(pipeA ? VBLANK_A : VBLANK_B);
+        crtc_state->saveVSYNC = REG_READ(pipeA ? VSYNC_A : VSYNC_B);
+        crtc_state->saveDSPSTRIDE = REG_READ(pipeA ? DSPASTRIDE : DSPBSTRIDE);
+
+        /*NOTE: DSPSIZE DSPPOS only for psb*/
+        crtc_state->saveDSPSIZE = REG_READ(pipeA ? DSPASIZE : DSPBSIZE);
+        crtc_state->saveDSPPOS = REG_READ(pipeA ? DSPAPOS : DSPBPOS);
+
+        crtc_state->saveDSPBASE = REG_READ(pipeA ? DSPABASE : DSPBBASE);
+
+        DRM_DEBUG("(%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x)\n",
+                        crtc_state->saveDSPCNTR,
+                        crtc_state->savePIPECONF,
+                        crtc_state->savePIPESRC,
+                        crtc_state->saveFP0,
+                        crtc_state->saveFP1,
+                        crtc_state->saveDPLL,
+                        crtc_state->saveHTOTAL,
+                        crtc_state->saveHBLANK,
+                        crtc_state->saveHSYNC,
+                        crtc_state->saveVTOTAL,
+                        crtc_state->saveVBLANK,
+                        crtc_state->saveVSYNC,
+                        crtc_state->saveDSPSTRIDE,
+                        crtc_state->saveDSPSIZE,
+                        crtc_state->saveDSPPOS,
+                        crtc_state->saveDSPBASE
+                );
+
+        paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+        for (i = 0; i < 256; ++i)
+                crtc_state->savePalette[i] = REG_READ(paletteReg + (i << 2));
+}
+
+/**
+ * Restore HW states of giving crtc
+ */
+static void cdv_intel_crtc_restore(struct drm_crtc *crtc)
+{
+        struct drm_device *dev = crtc->dev;
+        /* struct drm_psb_private * dev_priv =
+                                (struct drm_psb_private *)dev->dev_private; */
+        struct psb_intel_crtc *psb_intel_crtc =  to_psb_intel_crtc(crtc);
+        struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
+        /* struct drm_crtc_helper_funcs * crtc_funcs = crtc->helper_private; */
+        int pipeA = (psb_intel_crtc->pipe == 0);
+        uint32_t paletteReg;
+        int i;
+
+        if (!crtc_state) {
+                dev_dbg(dev->dev, "No crtc state\n");
+                return;
+        }
+
+        DRM_DEBUG(
+                "current:(%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x)\n",
+                REG_READ(pipeA ? DSPACNTR : DSPBCNTR),
+                REG_READ(pipeA ? PIPEACONF : PIPEBCONF),
+                REG_READ(pipeA ? PIPEASRC : PIPEBSRC),
+                REG_READ(pipeA ? FPA0 : FPB0),
+                REG_READ(pipeA ? FPA1 : FPB1),
+                REG_READ(pipeA ? DPLL_A : DPLL_B),
+                REG_READ(pipeA ? HTOTAL_A : HTOTAL_B),
+                REG_READ(pipeA ? HBLANK_A : HBLANK_B),
+                REG_READ(pipeA ? HSYNC_A : HSYNC_B),
+                REG_READ(pipeA ? VTOTAL_A : VTOTAL_B),
+                REG_READ(pipeA ? VBLANK_A : VBLANK_B),
+                REG_READ(pipeA ? VSYNC_A : VSYNC_B),
+                REG_READ(pipeA ? DSPASTRIDE : DSPBSTRIDE),
+                REG_READ(pipeA ? DSPASIZE : DSPBSIZE),
+                REG_READ(pipeA ? DSPAPOS : DSPBPOS),
+                REG_READ(pipeA ? DSPABASE : DSPBBASE)
+                );
+
+        DRM_DEBUG(
+                "saved: (%x %x %x %x %x %x %x %x %x %x %x %x %x %x %x %x)\n",
+                crtc_state->saveDSPCNTR,
+                crtc_state->savePIPECONF,
+                crtc_state->savePIPESRC,
+                crtc_state->saveFP0,
+                crtc_state->saveFP1,
+                crtc_state->saveDPLL,
+                crtc_state->saveHTOTAL,
+                crtc_state->saveHBLANK,
+                crtc_state->saveHSYNC,
+                crtc_state->saveVTOTAL,
+                crtc_state->saveVBLANK,
+                crtc_state->saveVSYNC,
+                crtc_state->saveDSPSTRIDE,
+                crtc_state->saveDSPSIZE,
+                crtc_state->saveDSPPOS,
+                crtc_state->saveDSPBASE
+                );
+
+
+        if (crtc_state->saveDPLL & DPLL_VCO_ENABLE) {
+                REG_WRITE(pipeA ? DPLL_A : DPLL_B,
+                        crtc_state->saveDPLL & ~DPLL_VCO_ENABLE);
+                REG_READ(pipeA ? DPLL_A : DPLL_B);
+                DRM_DEBUG("write dpll: %x\n",
+                                REG_READ(pipeA ? DPLL_A : DPLL_B));
+                udelay(150);
+        }
+
+        REG_WRITE(pipeA ? FPA0 : FPB0, crtc_state->saveFP0);
+        REG_READ(pipeA ? FPA0 : FPB0);
+
+        REG_WRITE(pipeA ? FPA1 : FPB1, crtc_state->saveFP1);
+        REG_READ(pipeA ? FPA1 : FPB1);
+
+        REG_WRITE(pipeA ? DPLL_A : DPLL_B, crtc_state->saveDPLL);
+        REG_READ(pipeA ? DPLL_A : DPLL_B);
+        udelay(150);
+
+        REG_WRITE(pipeA ? HTOTAL_A : HTOTAL_B, crtc_state->saveHTOTAL);
+        REG_WRITE(pipeA ? HBLANK_A : HBLANK_B, crtc_state->saveHBLANK);
+        REG_WRITE(pipeA ? HSYNC_A : HSYNC_B, crtc_state->saveHSYNC);
+        REG_WRITE(pipeA ? VTOTAL_A : VTOTAL_B, crtc_state->saveVTOTAL);
+        REG_WRITE(pipeA ? VBLANK_A : VBLANK_B, crtc_state->saveVBLANK);
+        REG_WRITE(pipeA ? VSYNC_A : VSYNC_B, crtc_state->saveVSYNC);
+        REG_WRITE(pipeA ? DSPASTRIDE : DSPBSTRIDE, crtc_state->saveDSPSTRIDE);
+
+        REG_WRITE(pipeA ? DSPASIZE : DSPBSIZE, crtc_state->saveDSPSIZE);
+        REG_WRITE(pipeA ? DSPAPOS : DSPBPOS, crtc_state->saveDSPPOS);
+
+        REG_WRITE(pipeA ? PIPEASRC : PIPEBSRC, crtc_state->savePIPESRC);
+        REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
+        REG_WRITE(pipeA ? PIPEACONF : PIPEBCONF, crtc_state->savePIPECONF);
+
+        cdv_intel_wait_for_vblank(dev);
+
+        REG_WRITE(pipeA ? DSPACNTR : DSPBCNTR, crtc_state->saveDSPCNTR);
+        REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
+
+        cdv_intel_wait_for_vblank(dev);
+
+        paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+        for (i = 0; i < 256; ++i)
+                REG_WRITE(paletteReg + (i << 2), crtc_state->savePalette[i]);
+}
+
+static int cdv_intel_crtc_cursor_set(struct drm_crtc *crtc,
+                                 struct drm_file *file_priv,
+                                 uint32_t handle,
+                                 uint32_t width, uint32_t height)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        uint32_t control = (pipe == 0) ? CURACNTR : CURBCNTR;
+        uint32_t base = (pipe == 0) ? CURABASE : CURBBASE;
+        uint32_t temp;
+        size_t addr = 0;
+        struct gtt_range *gt;
+        struct drm_gem_object *obj;
+        int ret;
+
+        /* if we want to turn of the cursor ignore width and height */
+        if (!handle) {
+                /* turn off the cursor */
+                temp = CURSOR_MODE_DISABLE;
+
+                if (gma_power_begin(dev, false)) {
+                        REG_WRITE(control, temp);
+                        REG_WRITE(base, 0);
+                        gma_power_end(dev);
+                }
+
+                /* unpin the old GEM object */
+                if (psb_intel_crtc->cursor_obj) {
+                        gt = container_of(psb_intel_crtc->cursor_obj,
+                                                        struct gtt_range, gem);
+                        psb_gtt_unpin(gt);
+                        drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
+                        psb_intel_crtc->cursor_obj = NULL;
+                }
+
+                return 0;
+        }
+
+        /* Currently we only support 64x64 cursors */
+        if (width != 64 || height != 64) {
+                dev_dbg(dev->dev, "we currently only support 64x64 cursors\n");
+                return -EINVAL;
+        }
+
+        obj = drm_gem_object_lookup(dev, file_priv, handle);
+        if (!obj)
+                return -ENOENT;
+
+        if (obj->size < width * height * 4) {
+                dev_dbg(dev->dev, "buffer is to small\n");
+                return -ENOMEM;
+        }
+
+        gt = container_of(obj, struct gtt_range, gem);
+
+        /* Pin the memory into the GTT */
+        ret = psb_gtt_pin(gt);
+        if (ret) {
+                dev_err(dev->dev, "Can not pin down handle 0x%x\n", handle);
+                return ret;
+        }
+
+        addr = gt->offset;      /* Or resource.start ??? */
+
+        psb_intel_crtc->cursor_addr = addr;
+
+        temp = 0;
+        /* set the pipe for the cursor */
+        temp |= (pipe << 28);
+        temp |= CURSOR_MODE_64_ARGB_AX | MCURSOR_GAMMA_ENABLE;
+
+        if (gma_power_begin(dev, false)) {
+                REG_WRITE(control, temp);
+                REG_WRITE(base, addr);
+                gma_power_end(dev);
+        }
+
+        /* unpin the old GEM object */
+        if (psb_intel_crtc->cursor_obj) {
+                gt = container_of(psb_intel_crtc->cursor_obj,
+                                                        struct gtt_range, gem);
+                psb_gtt_unpin(gt);
+                drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
+                psb_intel_crtc->cursor_obj = obj;
+        }
+        return 0;
+}
+
+static int cdv_intel_crtc_cursor_move(struct drm_crtc *crtc, int x, int y)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        uint32_t temp = 0;
+        uint32_t adder;
+
+
+        if (x < 0) {
+                temp |= (CURSOR_POS_SIGN << CURSOR_X_SHIFT);
+                x = -x;
+        }
+        if (y < 0) {
+                temp |= (CURSOR_POS_SIGN << CURSOR_Y_SHIFT);
+                y = -y;
+        }
+
+        temp |= ((x & CURSOR_POS_MASK) << CURSOR_X_SHIFT);
+        temp |= ((y & CURSOR_POS_MASK) << CURSOR_Y_SHIFT);
+
+        adder = psb_intel_crtc->cursor_addr;
+
+        if (gma_power_begin(dev, false)) {
+                REG_WRITE((pipe == 0) ? CURAPOS : CURBPOS, temp);
+                REG_WRITE((pipe == 0) ? CURABASE : CURBBASE, adder);
+                gma_power_end(dev);
+        }
+        return 0;
+}
+
+static void cdv_intel_crtc_gamma_set(struct drm_crtc *crtc, u16 *red,
+                         u16 *green, u16 *blue, uint32_t start, uint32_t size)
+{
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int i;
+        int end = (start + size > 256) ? 256 : start + size;
+
+        for (i = start; i < end; i++) {
+                psb_intel_crtc->lut_r[i] = red[i] >> 8;
+                psb_intel_crtc->lut_g[i] = green[i] >> 8;
+                psb_intel_crtc->lut_b[i] = blue[i] >> 8;
+        }
+
+        cdv_intel_crtc_load_lut(crtc);
+}
+
+static int cdv_crtc_set_config(struct drm_mode_set *set)
+{
+        int ret = 0;
+        struct drm_device *dev = set->crtc->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (!dev_priv->rpm_enabled)
+                return drm_crtc_helper_set_config(set);
+
+        pm_runtime_forbid(&dev->pdev->dev);
+
+        ret = drm_crtc_helper_set_config(set);
+
+        pm_runtime_allow(&dev->pdev->dev);
+
+        return ret;
+}
+
+/** Derive the pixel clock for the given refclk and divisors for 8xx chips. */
+
+/* FIXME: why are we using this, should it be cdv_ in this tree ? */
+
+static void i8xx_clock(int refclk, struct cdv_intel_clock_t *clock)
+{
+        clock->m = 5 * (clock->m1 + 2) + (clock->m2 + 2);
+        clock->p = clock->p1 * clock->p2;
+        clock->vco = refclk * clock->m / (clock->n + 2);
+        clock->dot = clock->vco / clock->p;
+}
+
+/* Returns the clock of the currently programmed mode of the given pipe. */
+static int cdv_intel_crtc_clock_get(struct drm_device *dev,
+                                struct drm_crtc *crtc)
+{
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        u32 dpll;
+        u32 fp;
+        struct cdv_intel_clock_t clock;
+        bool is_lvds;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (gma_power_begin(dev, false)) {
+                dpll = REG_READ((pipe == 0) ? DPLL_A : DPLL_B);
+                if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
+                        fp = REG_READ((pipe == 0) ? FPA0 : FPB0);
+                else
+                        fp = REG_READ((pipe == 0) ? FPA1 : FPB1);
+                is_lvds = (pipe == 1) && (REG_READ(LVDS) & LVDS_PORT_EN);
+                gma_power_end(dev);
+        } else {
+                dpll = (pipe == 0) ?
+                        dev_priv->saveDPLL_A : dev_priv->saveDPLL_B;
+
+                if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
+                        fp = (pipe == 0) ?
+                                dev_priv->saveFPA0 :
+                                dev_priv->saveFPB0;
+                else
+                        fp = (pipe == 0) ?
+                                dev_priv->saveFPA1 :
+                                dev_priv->saveFPB1;
+
+                is_lvds = (pipe == 1) && (dev_priv->saveLVDS & LVDS_PORT_EN);
+        }
+
+        clock.m1 = (fp & FP_M1_DIV_MASK) >> FP_M1_DIV_SHIFT;
+        clock.m2 = (fp & FP_M2_DIV_MASK) >> FP_M2_DIV_SHIFT;
+        clock.n = (fp & FP_N_DIV_MASK) >> FP_N_DIV_SHIFT;
+
+        if (is_lvds) {
+                clock.p1 =
+                    ffs((dpll &
+                         DPLL_FPA01_P1_POST_DIV_MASK_I830_LVDS) >>
+                        DPLL_FPA01_P1_POST_DIV_SHIFT);
+                if (clock.p1 == 0) {
+                        clock.p1 = 4;
+                        dev_err(dev->dev, "PLL %d\n", dpll);
+                }
+                clock.p2 = 14;
+
+                if ((dpll & PLL_REF_INPUT_MASK) ==
+                    PLLB_REF_INPUT_SPREADSPECTRUMIN) {
+                        /* XXX: might not be 66MHz */
+                        i8xx_clock(66000, &clock);
+                } else
+                        i8xx_clock(48000, &clock);
+        } else {
+                if (dpll & PLL_P1_DIVIDE_BY_TWO)
+                        clock.p1 = 2;
+                else {
+                        clock.p1 =
+                            ((dpll &
+                              DPLL_FPA01_P1_POST_DIV_MASK_I830) >>
+                             DPLL_FPA01_P1_POST_DIV_SHIFT) + 2;
+                }
+                if (dpll & PLL_P2_DIVIDE_BY_4)
+                        clock.p2 = 4;
+                else
+                        clock.p2 = 2;
+
+                i8xx_clock(48000, &clock);
+        }
+
+        /* XXX: It would be nice to validate the clocks, but we can't reuse
+         * i830PllIsValid() because it relies on the xf86_config connector
+         * configuration being accurate, which it isn't necessarily.
+         */
+
+        return clock.dot;
+}
+
+/** Returns the currently programmed mode of the given pipe. */
+struct drm_display_mode *cdv_intel_crtc_mode_get(struct drm_device *dev,
+                                             struct drm_crtc *crtc)
+{
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        struct drm_display_mode *mode;
+        int htot;
+        int hsync;
+        int vtot;
+        int vsync;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (gma_power_begin(dev, false)) {
+                htot = REG_READ((pipe == 0) ? HTOTAL_A : HTOTAL_B);
+                hsync = REG_READ((pipe == 0) ? HSYNC_A : HSYNC_B);
+                vtot = REG_READ((pipe == 0) ? VTOTAL_A : VTOTAL_B);
+                vsync = REG_READ((pipe == 0) ? VSYNC_A : VSYNC_B);
+                gma_power_end(dev);
+        } else {
+                htot = (pipe == 0) ?
+                        dev_priv->saveHTOTAL_A : dev_priv->saveHTOTAL_B;
+                hsync = (pipe == 0) ?
+                        dev_priv->saveHSYNC_A : dev_priv->saveHSYNC_B;
+                vtot = (pipe == 0) ?
+                        dev_priv->saveVTOTAL_A : dev_priv->saveVTOTAL_B;
+                vsync = (pipe == 0) ?
+                        dev_priv->saveVSYNC_A : dev_priv->saveVSYNC_B;
+        }
+
+        mode = kzalloc(sizeof(*mode), GFP_KERNEL);
+        if (!mode)
+                return NULL;
+
+        mode->clock = cdv_intel_crtc_clock_get(dev, crtc);
+        mode->hdisplay = (htot & 0xffff) + 1;
+        mode->htotal = ((htot & 0xffff0000) >> 16) + 1;
+        mode->hsync_start = (hsync & 0xffff) + 1;
+        mode->hsync_end = ((hsync & 0xffff0000) >> 16) + 1;
+        mode->vdisplay = (vtot & 0xffff) + 1;
+        mode->vtotal = ((vtot & 0xffff0000) >> 16) + 1;
+        mode->vsync_start = (vsync & 0xffff) + 1;
+        mode->vsync_end = ((vsync & 0xffff0000) >> 16) + 1;
+
+        drm_mode_set_name(mode);
+        drm_mode_set_crtcinfo(mode, 0);
+
+        return mode;
+}
+
+static void cdv_intel_crtc_destroy(struct drm_crtc *crtc)
+{
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+
+        kfree(psb_intel_crtc->crtc_state);
+        drm_crtc_cleanup(crtc);
+        kfree(psb_intel_crtc);
+}
+
+const struct drm_crtc_helper_funcs cdv_intel_helper_funcs = {
+        .dpms = cdv_intel_crtc_dpms,
+        .mode_fixup = cdv_intel_crtc_mode_fixup,
+        .mode_set = cdv_intel_crtc_mode_set,
+        .mode_set_base = cdv_intel_pipe_set_base,
+        .prepare = cdv_intel_crtc_prepare,
+        .commit = cdv_intel_crtc_commit,
+};
+
+const struct drm_crtc_funcs cdv_intel_crtc_funcs = {
+        .save = cdv_intel_crtc_save,
+        .restore = cdv_intel_crtc_restore,
+        .cursor_set = cdv_intel_crtc_cursor_set,
+        .cursor_move = cdv_intel_crtc_cursor_move,
+        .gamma_set = cdv_intel_crtc_gamma_set,
+        .set_config = cdv_crtc_set_config,
+        .destroy = cdv_intel_crtc_destroy,
+};
+
+/*
+ * Set the default value of cursor control and base register
+ * to zero. This is a workaround for h/w defect on oaktrail
+ */
+void cdv_intel_cursor_init(struct drm_device *dev, int pipe)
+{
+        uint32_t control;
+        uint32_t base;
+
+        switch (pipe) {
+        case 0:
+                control = CURACNTR;
+                base = CURABASE;
+                break;
+        case 1:
+                control = CURBCNTR;
+                base = CURBBASE;
+                break;
+        case 2:
+                control = CURCCNTR;
+                base = CURCBASE;
+                break;
+        default:
+                return;
+        }
+
+        REG_WRITE(control, 0);
+        REG_WRITE(base, 0);
+}
+
diff --git a/drivers/staging/gma500/cdv_intel_hdmi.c b/drivers/staging/gma500/cdv_intel_hdmi.c
new file mode 100644
index 00000000000..cbca2b0c7d5
--- /dev/null
+++ b/drivers/staging/gma500/cdv_intel_hdmi.c
@@ -0,0 +1,376 @@
+/*
+ * Copyright © 2006-2011 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:
+ *      jim liu <jim.liu@intel.com>
+ *
+ * FIXME:
+ *      We should probably make this generic and share it with Medfield
+ */
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_edid.h>
+#include "psb_intel_drv.h"
+#include "psb_drv.h"
+#include "psb_intel_reg.h"
+#include <linux/pm_runtime.h>
+
+/* hdmi control bits */
+#define HDMI_NULL_PACKETS_DURING_VSYNC  (1 << 9)
+#define HDMI_BORDER_ENABLE              (1 << 7)
+#define HDMI_AUDIO_ENABLE               (1 << 6)
+#define HDMI_VSYNC_ACTIVE_HIGH          (1 << 4)
+#define HDMI_HSYNC_ACTIVE_HIGH          (1 << 3)
+/* hdmi-b control bits */
+#define HDMIB_PIPE_B_SELECT             (1 << 30)
+
+
+struct mid_intel_hdmi_priv {
+        u32 hdmi_reg;
+        u32 save_HDMIB;
+        bool has_hdmi_sink;
+        bool has_hdmi_audio;
+        /* Should set this when detect hotplug */
+        bool hdmi_device_connected;
+        struct mdfld_hdmi_i2c *i2c_bus;
+        struct i2c_adapter *hdmi_i2c_adapter;   /* for control functions */
+        struct drm_device *dev;
+};
+
+static void cdv_hdmi_mode_set(struct drm_encoder *encoder,
+                        struct drm_display_mode *mode,
+                        struct drm_display_mode *adjusted_mode)
+{
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+        struct mid_intel_hdmi_priv *hdmi_priv = output->dev_priv;
+        u32 hdmib;
+        struct drm_crtc *crtc = encoder->crtc;
+        struct psb_intel_crtc *intel_crtc = to_psb_intel_crtc(crtc);
+
+        hdmib = (2 << 10);
+
+        if (adjusted_mode->flags & DRM_MODE_FLAG_PVSYNC)
+                hdmib |= HDMI_VSYNC_ACTIVE_HIGH;
+        if (adjusted_mode->flags & DRM_MODE_FLAG_PHSYNC)
+                hdmib |= HDMI_HSYNC_ACTIVE_HIGH;
+
+        if (intel_crtc->pipe == 1)
+                hdmib |= HDMIB_PIPE_B_SELECT;
+
+        if (hdmi_priv->has_hdmi_audio) {
+                hdmib |= HDMI_AUDIO_ENABLE;
+                hdmib |= HDMI_NULL_PACKETS_DURING_VSYNC;
+        }
+
+        REG_WRITE(hdmi_priv->hdmi_reg, hdmib);
+        REG_READ(hdmi_priv->hdmi_reg);
+}
+
+static bool cdv_hdmi_mode_fixup(struct drm_encoder *encoder,
+                                  struct drm_display_mode *mode,
+                                  struct drm_display_mode *adjusted_mode)
+{
+        return true;
+}
+
+static void cdv_hdmi_dpms(struct drm_encoder *encoder, int mode)
+{
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+        struct mid_intel_hdmi_priv *hdmi_priv = output->dev_priv;
+        u32 hdmib;
+
+        hdmib = REG_READ(hdmi_priv->hdmi_reg);
+
+        if (mode != DRM_MODE_DPMS_ON)
+                REG_WRITE(hdmi_priv->hdmi_reg, hdmib & ~HDMIB_PORT_EN);
+        else
+                REG_WRITE(hdmi_priv->hdmi_reg, hdmib | HDMIB_PORT_EN);
+        REG_READ(hdmi_priv->hdmi_reg);
+}
+
+static void cdv_hdmi_save(struct drm_connector *connector)
+{
+        struct drm_device *dev = connector->dev;
+        struct psb_intel_output *output = to_psb_intel_output(connector);
+        struct mid_intel_hdmi_priv *hdmi_priv = output->dev_priv;
+
+        hdmi_priv->save_HDMIB = REG_READ(hdmi_priv->hdmi_reg);
+}
+
+static void cdv_hdmi_restore(struct drm_connector *connector)
+{
+        struct drm_device *dev = connector->dev;
+        struct psb_intel_output *output = to_psb_intel_output(connector);
+        struct mid_intel_hdmi_priv *hdmi_priv = output->dev_priv;
+
+        REG_WRITE(hdmi_priv->hdmi_reg, hdmi_priv->save_HDMIB);
+        REG_READ(hdmi_priv->hdmi_reg);
+}
+
+static enum drm_connector_status cdv_hdmi_detect(
+                                struct drm_connector *connector, bool force)
+{
+        struct psb_intel_output *psb_intel_output =
+                                                to_psb_intel_output(connector);
+        struct mid_intel_hdmi_priv *hdmi_priv = psb_intel_output->dev_priv;
+        struct edid *edid = NULL;
+        enum drm_connector_status status = connector_status_disconnected;
+
+        edid = drm_get_edid(&psb_intel_output->base,
+                         psb_intel_output->hdmi_i2c_adapter);
+
+        hdmi_priv->has_hdmi_sink = false;
+        hdmi_priv->has_hdmi_audio = false;
+        if (edid) {
+                if (edid->input & DRM_EDID_INPUT_DIGITAL) {
+                        status = connector_status_connected;
+                        hdmi_priv->has_hdmi_sink =
+                                                drm_detect_hdmi_monitor(edid);
+                        hdmi_priv->has_hdmi_audio =
+                                                drm_detect_monitor_audio(edid);
+                }
+
+                psb_intel_output->base.display_info.raw_edid = NULL;
+                kfree(edid);
+        }
+        return status;
+}
+
+static int cdv_hdmi_set_property(struct drm_connector *connector,
+                                       struct drm_property *property,
+                                       uint64_t value)
+{
+        struct drm_encoder *encoder = connector->encoder;
+
+        if (!strcmp(property->name, "scaling mode") && encoder) {
+                struct psb_intel_crtc *crtc = to_psb_intel_crtc(encoder->crtc);
+                bool centre;
+                uint64_t curValue;
+
+                if (!crtc)
+                        return -1;
+
+                switch (value) {
+                case DRM_MODE_SCALE_FULLSCREEN:
+                        break;
+                case DRM_MODE_SCALE_NO_SCALE:
+                        break;
+                case DRM_MODE_SCALE_ASPECT:
+                        break;
+                default:
+                        return -1;
+                }
+
+                if (drm_connector_property_get_value(connector,
+                                                        property, &curValue))
+                        return -1;
+
+                if (curValue == value)
+                        return 0;
+
+                if (drm_connector_property_set_value(connector,
+                                                        property, value))
+                        return -1;
+
+                centre = (curValue == DRM_MODE_SCALE_NO_SCALE) ||
+                        (value == DRM_MODE_SCALE_NO_SCALE);
+
+                if (crtc->saved_mode.hdisplay != 0 &&
+                    crtc->saved_mode.vdisplay != 0) {
+                        if (centre) {
+                                if (!drm_crtc_helper_set_mode(encoder->crtc, &crtc->saved_mode,
+                                            encoder->crtc->x, encoder->crtc->y, encoder->crtc->fb))
+                                        return -1;
+                        } else {
+                                struct drm_encoder_helper_funcs *helpers
+                                                    = encoder->helper_private;
+                                helpers->mode_set(encoder, &crtc->saved_mode,
+                                             &crtc->saved_adjusted_mode);
+                        }
+                }
+        }
+        return 0;
+}
+
+/*
+ * Return the list of HDMI DDC modes if available.
+ */
+static int cdv_hdmi_get_modes(struct drm_connector *connector)
+{
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+        struct edid *edid = NULL;
+        int ret = 0;
+
+        edid = drm_get_edid(&psb_intel_output->base,
+                         psb_intel_output->hdmi_i2c_adapter);
+        if (edid) {
+                drm_mode_connector_update_edid_property(&psb_intel_output->
+                                                        base, edid);
+                ret = drm_add_edid_modes(&psb_intel_output->base, edid);
+                kfree(edid);
+        }
+        return ret;
+}
+
+static int cdv_hdmi_mode_valid(struct drm_connector *connector,
+                                 struct drm_display_mode *mode)
+{
+
+        if (mode->clock > 165000)
+                return MODE_CLOCK_HIGH;
+        if (mode->clock < 20000)
+                return MODE_CLOCK_HIGH;
+
+        /* just in case */
+        if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+                return MODE_NO_DBLESCAN;
+
+        /* just in case */
+        if (mode->flags & DRM_MODE_FLAG_INTERLACE)
+                return MODE_NO_INTERLACE;
+
+        /*
+         * FIXME: for now we limit the size to 1680x1050 on CDV, otherwise it
+         * will go beyond the stolen memory size allocated to the framebuffer
+         */
+        if (mode->hdisplay > 1680)
+                return MODE_PANEL;
+        if (mode->vdisplay > 1050)
+                return MODE_PANEL;
+        return MODE_OK;
+}
+
+static void cdv_hdmi_destroy(struct drm_connector *connector)
+{
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+
+        if (psb_intel_output->ddc_bus)
+                psb_intel_i2c_destroy(psb_intel_output->ddc_bus);
+        drm_sysfs_connector_remove(connector);
+        drm_connector_cleanup(connector);
+        kfree(connector);
+}
+
+static const struct drm_encoder_helper_funcs cdv_hdmi_helper_funcs = {
+        .dpms = cdv_hdmi_dpms,
+        .mode_fixup = cdv_hdmi_mode_fixup,
+        .prepare = psb_intel_encoder_prepare,
+        .mode_set = cdv_hdmi_mode_set,
+        .commit = psb_intel_encoder_commit,
+};
+
+static const struct drm_connector_helper_funcs
+                                        cdv_hdmi_connector_helper_funcs = {
+        .get_modes = cdv_hdmi_get_modes,
+        .mode_valid = cdv_hdmi_mode_valid,
+        .best_encoder = psb_intel_best_encoder,
+};
+
+static const struct drm_connector_funcs cdv_hdmi_connector_funcs = {
+        .dpms = drm_helper_connector_dpms,
+        .save = cdv_hdmi_save,
+        .restore = cdv_hdmi_restore,
+        .detect = cdv_hdmi_detect,
+        .fill_modes = drm_helper_probe_single_connector_modes,
+        .set_property = cdv_hdmi_set_property,
+        .destroy = cdv_hdmi_destroy,
+};
+
+void cdv_hdmi_init(struct drm_device *dev,
+                        struct psb_intel_mode_device *mode_dev, int reg)
+{
+        struct psb_intel_output *psb_intel_output;
+        struct drm_connector *connector;
+        struct drm_encoder *encoder;
+        struct mid_intel_hdmi_priv *hdmi_priv;
+        int ddc_bus;
+
+        psb_intel_output = kzalloc(sizeof(struct psb_intel_output) +
+                               sizeof(struct mid_intel_hdmi_priv), GFP_KERNEL);
+        if (!psb_intel_output)
+                return;
+
+        hdmi_priv = (struct mid_intel_hdmi_priv *)(psb_intel_output + 1);
+        psb_intel_output->mode_dev = mode_dev;
+        connector = &psb_intel_output->base;
+        encoder = &psb_intel_output->enc;
+        drm_connector_init(dev, &psb_intel_output->base,
+                           &cdv_hdmi_connector_funcs,
+                           DRM_MODE_CONNECTOR_DVID);
+
+        drm_encoder_init(dev, &psb_intel_output->enc, &psb_intel_lvds_enc_funcs,
+                         DRM_MODE_ENCODER_TMDS);
+
+        drm_mode_connector_attach_encoder(&psb_intel_output->base,
+                                          &psb_intel_output->enc);
+        psb_intel_output->type = INTEL_OUTPUT_HDMI;
+        hdmi_priv->hdmi_reg = reg;
+        hdmi_priv->has_hdmi_sink = false;
+        psb_intel_output->dev_priv = hdmi_priv;
+
+        drm_encoder_helper_add(encoder, &cdv_hdmi_helper_funcs);
+        drm_connector_helper_add(connector,
+                                 &cdv_hdmi_connector_helper_funcs);
+        connector->display_info.subpixel_order = SubPixelHorizontalRGB;
+        connector->interlace_allowed = false;
+        connector->doublescan_allowed = false;
+
+        drm_connector_attach_property(connector,
+            dev->mode_config.scaling_mode_property, DRM_MODE_SCALE_FULLSCREEN);
+
+        switch (reg) {
+        case SDVOB:
+                ddc_bus = GPIOE;
+                break;
+        case SDVOC:
+                ddc_bus = GPIOD;
+                break;
+        default:
+                DRM_ERROR("unknown reg 0x%x for HDMI\n", reg);
+                goto failed_ddc;
+                break;
+        }
+
+        psb_intel_output->ddc_bus = psb_intel_i2c_create(dev,
+                                ddc_bus, (reg == SDVOB) ? "HDMIB" : "HDMIC");
+
+        if (!psb_intel_output->ddc_bus) {
+                dev_err(dev->dev, "No ddc adapter available!\n");
+                goto failed_ddc;
+        }
+        psb_intel_output->hdmi_i2c_adapter =
+                                &(psb_intel_output->ddc_bus->adapter);
+        hdmi_priv->dev = dev;
+        drm_sysfs_connector_add(connector);
+        return;
+
+failed_ddc:
+        drm_encoder_cleanup(&psb_intel_output->enc);
+        drm_connector_cleanup(&psb_intel_output->base);
+        kfree(psb_intel_output);
+}
diff --git a/drivers/staging/gma500/cdv_intel_lvds.c b/drivers/staging/gma500/cdv_intel_lvds.c
new file mode 100644
index 00000000000..988b2d0acf4
--- /dev/null
+++ b/drivers/staging/gma500/cdv_intel_lvds.c
@@ -0,0 +1,721 @@
+/*
+ * Copyright © 2006-2011 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors:
+ *      Eric Anholt <eric@anholt.net>
+ *      Dave Airlie <airlied@linux.ie>
+ *      Jesse Barnes <jesse.barnes@intel.com>
+ */
+
+#include <linux/i2c.h>
+#include <linux/dmi.h>
+#include <drm/drmP.h>
+
+#include "intel_bios.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "power.h"
+#include <linux/pm_runtime.h>
+#include "cdv_device.h"
+
+/**
+ * LVDS I2C backlight control macros
+ */
+#define BRIGHTNESS_MAX_LEVEL 100
+#define BRIGHTNESS_MASK 0xFF
+#define BLC_I2C_TYPE    0x01
+#define BLC_PWM_TYPT    0x02
+
+#define BLC_POLARITY_NORMAL 0
+#define BLC_POLARITY_INVERSE 1
+
+#define PSB_BLC_MAX_PWM_REG_FREQ       (0xFFFE)
+#define PSB_BLC_MIN_PWM_REG_FREQ        (0x2)
+#define PSB_BLC_PWM_PRECISION_FACTOR    (10)
+#define PSB_BACKLIGHT_PWM_CTL_SHIFT     (16)
+#define PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR (0xFFFE)
+
+struct cdv_intel_lvds_priv {
+        /**
+         * Saved LVDO output states
+         */
+        uint32_t savePP_ON;
+        uint32_t savePP_OFF;
+        uint32_t saveLVDS;
+        uint32_t savePP_CONTROL;
+        uint32_t savePP_CYCLE;
+        uint32_t savePFIT_CONTROL;
+        uint32_t savePFIT_PGM_RATIOS;
+        uint32_t saveBLC_PWM_CTL;
+};
+
+/*
+ * Returns the maximum level of the backlight duty cycle field.
+ */
+static u32 cdv_intel_lvds_get_max_backlight(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 retval;
+
+        if (gma_power_begin(dev, false)) {
+                retval = ((REG_READ(BLC_PWM_CTL) &
+                          BACKLIGHT_MODULATION_FREQ_MASK) >>
+                          BACKLIGHT_MODULATION_FREQ_SHIFT) * 2;
+
+                gma_power_end(dev);
+        } else
+                retval = ((dev_priv->saveBLC_PWM_CTL &
+                          BACKLIGHT_MODULATION_FREQ_MASK) >>
+                          BACKLIGHT_MODULATION_FREQ_SHIFT) * 2;
+
+        return retval;
+}
+
+/*
+ * Set LVDS backlight level by I2C command
+ */
+static int cdv_lvds_i2c_set_brightness(struct drm_device *dev,
+                                        unsigned int level)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct psb_intel_i2c_chan *lvds_i2c_bus = dev_priv->lvds_i2c_bus;
+        u8 out_buf[2];
+        unsigned int blc_i2c_brightness;
+
+        struct i2c_msg msgs[] = {
+                {
+                        .addr = lvds_i2c_bus->slave_addr,
+                        .flags = 0,
+                        .len = 2,
+                        .buf = out_buf,
+                }
+        };
+
+        blc_i2c_brightness = BRIGHTNESS_MASK & ((unsigned int)level *
+                             BRIGHTNESS_MASK /
+                             BRIGHTNESS_MAX_LEVEL);
+
+        if (dev_priv->lvds_bl->pol == BLC_POLARITY_INVERSE)
+                blc_i2c_brightness = BRIGHTNESS_MASK - blc_i2c_brightness;
+
+        out_buf[0] = dev_priv->lvds_bl->brightnesscmd;
+        out_buf[1] = (u8)blc_i2c_brightness;
+
+        if (i2c_transfer(&lvds_i2c_bus->adapter, msgs, 1) == 1)
+                return 0;
+
+        DRM_ERROR("I2C transfer error\n");
+        return -1;
+}
+
+
+static int cdv_lvds_pwm_set_brightness(struct drm_device *dev, int level)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        u32 max_pwm_blc;
+        u32 blc_pwm_duty_cycle;
+
+        max_pwm_blc = cdv_intel_lvds_get_max_backlight(dev);
+
+        /*BLC_PWM_CTL Should be initiated while backlight device init*/
+        BUG_ON((max_pwm_blc & PSB_BLC_MAX_PWM_REG_FREQ) == 0);
+
+        blc_pwm_duty_cycle = level * max_pwm_blc / BRIGHTNESS_MAX_LEVEL;
+
+        if (dev_priv->lvds_bl->pol == BLC_POLARITY_INVERSE)
+                blc_pwm_duty_cycle = max_pwm_blc - blc_pwm_duty_cycle;
+
+        blc_pwm_duty_cycle &= PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR;
+        REG_WRITE(BLC_PWM_CTL,
+                  (max_pwm_blc << PSB_BACKLIGHT_PWM_CTL_SHIFT) |
+                  (blc_pwm_duty_cycle));
+
+        return 0;
+}
+
+/*
+ * Set LVDS backlight level either by I2C or PWM
+ */
+void cdv_intel_lvds_set_brightness(struct drm_device *dev, int level)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (!dev_priv->lvds_bl) {
+                DRM_ERROR("NO LVDS Backlight Info\n");
+                return;
+        }
+
+        if (dev_priv->lvds_bl->type == BLC_I2C_TYPE)
+                cdv_lvds_i2c_set_brightness(dev, level);
+        else
+                cdv_lvds_pwm_set_brightness(dev, level);
+}
+
+/**
+ * Sets the backlight level.
+ *
+ * level backlight level, from 0 to cdv_intel_lvds_get_max_backlight().
+ */
+static void cdv_intel_lvds_set_backlight(struct drm_device *dev, int level)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 blc_pwm_ctl;
+
+        if (gma_power_begin(dev, false)) {
+                blc_pwm_ctl =
+                        REG_READ(BLC_PWM_CTL) & ~BACKLIGHT_DUTY_CYCLE_MASK;
+                REG_WRITE(BLC_PWM_CTL,
+                                (blc_pwm_ctl |
+                                (level << BACKLIGHT_DUTY_CYCLE_SHIFT)));
+                gma_power_end(dev);
+        } else {
+                blc_pwm_ctl = dev_priv->saveBLC_PWM_CTL &
+                                ~BACKLIGHT_DUTY_CYCLE_MASK;
+                dev_priv->saveBLC_PWM_CTL = (blc_pwm_ctl |
+                                        (level << BACKLIGHT_DUTY_CYCLE_SHIFT));
+        }
+}
+
+/**
+ * Sets the power state for the panel.
+ */
+static void cdv_intel_lvds_set_power(struct drm_device *dev,
+                                 struct psb_intel_output *output, bool on)
+{
+        u32 pp_status;
+
+        if (!gma_power_begin(dev, true))
+                return;
+
+        if (on) {
+                REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) |
+                          POWER_TARGET_ON);
+                do {
+                        pp_status = REG_READ(PP_STATUS);
+                } while ((pp_status & PP_ON) == 0);
+
+                cdv_intel_lvds_set_backlight(dev,
+                                         output->
+                                         mode_dev->backlight_duty_cycle);
+        } else {
+                cdv_intel_lvds_set_backlight(dev, 0);
+
+                REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) &
+                          ~POWER_TARGET_ON);
+                do {
+                        pp_status = REG_READ(PP_STATUS);
+                } while (pp_status & PP_ON);
+        }
+        gma_power_end(dev);
+}
+
+static void cdv_intel_lvds_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+        if (mode == DRM_MODE_DPMS_ON)
+                cdv_intel_lvds_set_power(dev, output, true);
+        else
+                cdv_intel_lvds_set_power(dev, output, false);
+        /* XXX: We never power down the LVDS pairs. */
+}
+
+static void cdv_intel_lvds_save(struct drm_connector *connector)
+{
+}
+
+static void cdv_intel_lvds_restore(struct drm_connector *connector)
+{
+}
+
+int cdv_intel_lvds_mode_valid(struct drm_connector *connector,
+                                 struct drm_display_mode *mode)
+{
+        struct psb_intel_output *psb_intel_output =
+                                to_psb_intel_output(connector);
+        struct drm_display_mode *fixed_mode =
+            psb_intel_output->mode_dev->panel_fixed_mode;
+
+        /* just in case */
+        if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+                return MODE_NO_DBLESCAN;
+
+        /* just in case */
+        if (mode->flags & DRM_MODE_FLAG_INTERLACE)
+                return MODE_NO_INTERLACE;
+
+        if (fixed_mode) {
+                if (mode->hdisplay > fixed_mode->hdisplay)
+                        return MODE_PANEL;
+                if (mode->vdisplay > fixed_mode->vdisplay)
+                        return MODE_PANEL;
+        }
+        return MODE_OK;
+}
+
+bool cdv_intel_lvds_mode_fixup(struct drm_encoder *encoder,
+                                  struct drm_display_mode *mode,
+                                  struct drm_display_mode *adjusted_mode)
+{
+        struct psb_intel_mode_device *mode_dev =
+            enc_to_psb_intel_output(encoder)->mode_dev;
+        struct drm_device *dev = encoder->dev;
+        struct drm_encoder *tmp_encoder;
+        struct drm_display_mode *panel_fixed_mode = mode_dev->panel_fixed_mode;
+
+        /* Should never happen!! */
+        list_for_each_entry(tmp_encoder, &dev->mode_config.encoder_list,
+                            head) {
+                if (tmp_encoder != encoder
+                    && tmp_encoder->crtc == encoder->crtc) {
+                        printk(KERN_ERR "Can't enable LVDS and another "
+                               "encoder on the same pipe\n");
+                        return false;
+                }
+        }
+
+        /*
+         * If we have timings from the BIOS for the panel, put them in
+         * to the adjusted mode.  The CRTC will be set up for this mode,
+         * with the panel scaling set up to source from the H/VDisplay
+         * of the original mode.
+         */
+        if (panel_fixed_mode != NULL) {
+                adjusted_mode->hdisplay = panel_fixed_mode->hdisplay;
+                adjusted_mode->hsync_start = panel_fixed_mode->hsync_start;
+                adjusted_mode->hsync_end = panel_fixed_mode->hsync_end;
+                adjusted_mode->htotal = panel_fixed_mode->htotal;
+                adjusted_mode->vdisplay = panel_fixed_mode->vdisplay;
+                adjusted_mode->vsync_start = panel_fixed_mode->vsync_start;
+                adjusted_mode->vsync_end = panel_fixed_mode->vsync_end;
+                adjusted_mode->vtotal = panel_fixed_mode->vtotal;
+                adjusted_mode->clock = panel_fixed_mode->clock;
+                drm_mode_set_crtcinfo(adjusted_mode,
+                                      CRTC_INTERLACE_HALVE_V);
+        }
+
+        /*
+         * XXX: It would be nice to support lower refresh rates on the
+         * panels to reduce power consumption, and perhaps match the
+         * user's requested refresh rate.
+         */
+
+        return true;
+}
+
+static void cdv_intel_lvds_prepare(struct drm_encoder *encoder)
+{
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+        struct psb_intel_mode_device *mode_dev = output->mode_dev;
+
+        if (!gma_power_begin(dev, true))
+                return;
+
+        mode_dev->saveBLC_PWM_CTL = REG_READ(BLC_PWM_CTL);
+        mode_dev->backlight_duty_cycle = (mode_dev->saveBLC_PWM_CTL &
+                                          BACKLIGHT_DUTY_CYCLE_MASK);
+
+        cdv_intel_lvds_set_power(dev, output, false);
+
+        gma_power_end(dev);
+}
+
+static void cdv_intel_lvds_commit(struct drm_encoder *encoder)
+{
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+        struct psb_intel_mode_device *mode_dev = output->mode_dev;
+
+        if (mode_dev->backlight_duty_cycle == 0)
+                mode_dev->backlight_duty_cycle =
+                    cdv_intel_lvds_get_max_backlight(dev);
+
+        cdv_intel_lvds_set_power(dev, output, true);
+}
+
+static void cdv_intel_lvds_mode_set(struct drm_encoder *encoder,
+                                struct drm_display_mode *mode,
+                                struct drm_display_mode *adjusted_mode)
+{
+        struct drm_device *dev = encoder->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 pfit_control;
+
+        /*
+         * The LVDS pin pair will already have been turned on in the
+         * cdv_intel_crtc_mode_set since it has a large impact on the DPLL
+         * settings.
+         */
+
+        /*
+         * Enable automatic panel scaling so that non-native modes fill the
+         * screen.  Should be enabled before the pipe is enabled, according to
+         * register description and PRM.
+         */
+        if (mode->hdisplay != adjusted_mode->hdisplay ||
+            mode->vdisplay != adjusted_mode->vdisplay)
+                pfit_control = (PFIT_ENABLE | VERT_AUTO_SCALE |
+                                HORIZ_AUTO_SCALE | VERT_INTERP_BILINEAR |
+                                HORIZ_INTERP_BILINEAR);
+        else
+                pfit_control = 0;
+
+        if (dev_priv->lvds_dither)
+                pfit_control |= PANEL_8TO6_DITHER_ENABLE;
+
+        REG_WRITE(PFIT_CONTROL, pfit_control);
+}
+
+/**
+ * Detect the LVDS connection.
+ *
+ * This always returns CONNECTOR_STATUS_CONNECTED.
+ * This connector should only have
+ * been set up if the LVDS was actually connected anyway.
+ */
+static enum drm_connector_status cdv_intel_lvds_detect(
+                                struct drm_connector *connector, bool force)
+{
+        return connector_status_connected;
+}
+
+/**
+ * Return the list of DDC modes if available, or the BIOS fixed mode otherwise.
+ */
+static int cdv_intel_lvds_get_modes(struct drm_connector *connector)
+{
+        struct drm_device *dev = connector->dev;
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+        struct psb_intel_mode_device *mode_dev =
+                                        psb_intel_output->mode_dev;
+        int ret;
+
+        ret = psb_intel_ddc_get_modes(psb_intel_output);
+
+        if (ret)
+                return ret;
+
+        /* Didn't get an EDID, so
+         * Set wide sync ranges so we get all modes
+         * handed to valid_mode for checking
+         */
+        connector->display_info.min_vfreq = 0;
+        connector->display_info.max_vfreq = 200;
+        connector->display_info.min_hfreq = 0;
+        connector->display_info.max_hfreq = 200;
+        if (mode_dev->panel_fixed_mode != NULL) {
+                struct drm_display_mode *mode =
+                    drm_mode_duplicate(dev, mode_dev->panel_fixed_mode);
+                drm_mode_probed_add(connector, mode);
+                return 1;
+        }
+
+        return 0;
+}
+
+/**
+ * cdv_intel_lvds_destroy - unregister and free LVDS structures
+ * @connector: connector to free
+ *
+ * Unregister the DDC bus for this connector then free the driver private
+ * structure.
+ */
+void cdv_intel_lvds_destroy(struct drm_connector *connector)
+{
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+
+        if (psb_intel_output->ddc_bus)
+                psb_intel_i2c_destroy(psb_intel_output->ddc_bus);
+        drm_sysfs_connector_remove(connector);
+        drm_connector_cleanup(connector);
+        kfree(connector);
+}
+
+int cdv_intel_lvds_set_property(struct drm_connector *connector,
+                                       struct drm_property *property,
+                                       uint64_t value)
+{
+        struct drm_encoder *encoder = connector->encoder;
+
+        if (!strcmp(property->name, "scaling mode") && encoder) {
+                struct psb_intel_crtc *crtc =
+                                        to_psb_intel_crtc(encoder->crtc);
+                uint64_t curValue;
+
+                if (!crtc)
+                        return -1;
+
+                switch (value) {
+                case DRM_MODE_SCALE_FULLSCREEN:
+                        break;
+                case DRM_MODE_SCALE_NO_SCALE:
+                        break;
+                case DRM_MODE_SCALE_ASPECT:
+                        break;
+                default:
+                        return -1;
+                }
+
+                if (drm_connector_property_get_value(connector,
+                                                     property,
+                                                     &curValue))
+                        return -1;
+
+                if (curValue == value)
+                        return 0;
+
+                if (drm_connector_property_set_value(connector,
+                                                        property,
+                                                        value))
+                        return -1;
+
+                if (crtc->saved_mode.hdisplay != 0 &&
+                    crtc->saved_mode.vdisplay != 0) {
+                        if (!drm_crtc_helper_set_mode(encoder->crtc,
+                                                      &crtc->saved_mode,
+                                                      encoder->crtc->x,
+                                                      encoder->crtc->y,
+                                                      encoder->crtc->fb))
+                                return -1;
+                }
+        } else if (!strcmp(property->name, "backlight") && encoder) {
+                if (drm_connector_property_set_value(connector,
+                                                        property,
+                                                        value))
+                        return -1;
+                else {
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+                        struct drm_psb_private *dev_priv =
+                                                encoder->dev->dev_private;
+                        struct backlight_device *bd =
+                                                dev_priv->backlight_device;
+                        bd->props.brightness = value;
+                        backlight_update_status(bd);
+#endif
+                }
+        } else if (!strcmp(property->name, "DPMS") && encoder) {
+                struct drm_encoder_helper_funcs *helpers =
+                                        encoder->helper_private;
+                helpers->dpms(encoder, value);
+        }
+        return 0;
+}
+
+static const struct drm_encoder_helper_funcs
+                                        cdv_intel_lvds_helper_funcs = {
+        .dpms = cdv_intel_lvds_encoder_dpms,
+        .mode_fixup = cdv_intel_lvds_mode_fixup,
+        .prepare = cdv_intel_lvds_prepare,
+        .mode_set = cdv_intel_lvds_mode_set,
+        .commit = cdv_intel_lvds_commit,
+};
+
+static const struct drm_connector_helper_funcs
+                                cdv_intel_lvds_connector_helper_funcs = {
+        .get_modes = cdv_intel_lvds_get_modes,
+        .mode_valid = cdv_intel_lvds_mode_valid,
+        .best_encoder = psb_intel_best_encoder,
+};
+
+static const struct drm_connector_funcs cdv_intel_lvds_connector_funcs = {
+        .dpms = drm_helper_connector_dpms,
+        .save = cdv_intel_lvds_save,
+        .restore = cdv_intel_lvds_restore,
+        .detect = cdv_intel_lvds_detect,
+        .fill_modes = drm_helper_probe_single_connector_modes,
+        .set_property = cdv_intel_lvds_set_property,
+        .destroy = cdv_intel_lvds_destroy,
+};
+
+
+static void cdv_intel_lvds_enc_destroy(struct drm_encoder *encoder)
+{
+        drm_encoder_cleanup(encoder);
+}
+
+const struct drm_encoder_funcs cdv_intel_lvds_enc_funcs = {
+        .destroy = cdv_intel_lvds_enc_destroy,
+};
+
+/**
+ * cdv_intel_lvds_init - setup LVDS connectors on this device
+ * @dev: drm device
+ *
+ * Create the connector, register the LVDS DDC bus, and try to figure out what
+ * modes we can display on the LVDS panel (if present).
+ */
+void cdv_intel_lvds_init(struct drm_device *dev,
+                     struct psb_intel_mode_device *mode_dev)
+{
+        struct psb_intel_output *psb_intel_output;
+        struct cdv_intel_lvds_priv *lvds_priv;
+        struct drm_connector *connector;
+        struct drm_encoder *encoder;
+        struct drm_display_mode *scan;
+        struct drm_crtc *crtc;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 lvds;
+        int pipe;
+
+        psb_intel_output = kzalloc(sizeof(struct psb_intel_output) +
+                        sizeof(struct cdv_intel_lvds_priv), GFP_KERNEL);
+        if (!psb_intel_output)
+                return;
+
+        lvds_priv = (struct cdv_intel_lvds_priv *)(psb_intel_output + 1);
+
+        psb_intel_output->dev_priv = lvds_priv;
+
+        psb_intel_output->mode_dev = mode_dev;
+        connector = &psb_intel_output->base;
+        encoder = &psb_intel_output->enc;
+
+
+        drm_connector_init(dev, &psb_intel_output->base,
+                           &cdv_intel_lvds_connector_funcs,
+                           DRM_MODE_CONNECTOR_LVDS);
+
+        drm_encoder_init(dev, &psb_intel_output->enc,
+                         &cdv_intel_lvds_enc_funcs,
+                         DRM_MODE_ENCODER_LVDS);
+
+
+        drm_mode_connector_attach_encoder(&psb_intel_output->base,
+                                          &psb_intel_output->enc);
+        psb_intel_output->type = INTEL_OUTPUT_LVDS;
+
+        drm_encoder_helper_add(encoder, &cdv_intel_lvds_helper_funcs);
+        drm_connector_helper_add(connector,
+                                 &cdv_intel_lvds_connector_helper_funcs);
+        connector->display_info.subpixel_order = SubPixelHorizontalRGB;
+        connector->interlace_allowed = false;
+        connector->doublescan_allowed = false;
+
+        /*Attach connector properties*/
+        drm_connector_attach_property(connector,
+                                      dev->mode_config.scaling_mode_property,
+                                      DRM_MODE_SCALE_FULLSCREEN);
+        drm_connector_attach_property(connector,
+                                      dev_priv->backlight_property,
+                                      BRIGHTNESS_MAX_LEVEL);
+
+        /**
+         * Set up I2C bus
+         * FIXME: distroy i2c_bus when exit
+         */
+        psb_intel_output->i2c_bus = psb_intel_i2c_create(dev,
+                                                         GPIOB,
+                                                         "LVDSBLC_B");
+        if (!psb_intel_output->i2c_bus) {
+                dev_printk(KERN_ERR,
+                        &dev->pdev->dev, "I2C bus registration failed.\n");
+                goto failed_blc_i2c;
+        }
+        psb_intel_output->i2c_bus->slave_addr = 0x2C;
+        dev_priv->lvds_i2c_bus =  psb_intel_output->i2c_bus;
+
+        /*
+         * LVDS discovery:
+         * 1) check for EDID on DDC
+         * 2) check for VBT data
+         * 3) check to see if LVDS is already on
+         *    if none of the above, no panel
+         * 4) make sure lid is open
+         *    if closed, act like it's not there for now
+         */
+
+        /* Set up the DDC bus. */
+        psb_intel_output->ddc_bus = psb_intel_i2c_create(dev,
+                                                         GPIOC,
+                                                         "LVDSDDC_C");
+        if (!psb_intel_output->ddc_bus) {
+                dev_printk(KERN_ERR, &dev->pdev->dev,
+                           "DDC bus registration " "failed.\n");
+                goto failed_ddc;
+        }
+
+        /*
+         * Attempt to get the fixed panel mode from DDC.  Assume that the
+         * preferred mode is the right one.
+         */
+        psb_intel_ddc_get_modes(psb_intel_output);
+        list_for_each_entry(scan, &connector->probed_modes, head) {
+                if (scan->type & DRM_MODE_TYPE_PREFERRED) {
+                        mode_dev->panel_fixed_mode =
+                            drm_mode_duplicate(dev, scan);
+                        goto out;       /* FIXME: check for quirks */
+                }
+        }
+
+        /* Failed to get EDID, what about VBT? do we need this?*/
+        if (dev_priv->lfp_lvds_vbt_mode) {
+                mode_dev->panel_fixed_mode =
+                        drm_mode_duplicate(dev, dev_priv->lfp_lvds_vbt_mode);
+                if (mode_dev->panel_fixed_mode) {
+                        mode_dev->panel_fixed_mode->type |=
+                                DRM_MODE_TYPE_PREFERRED;
+                        goto out;       /* FIXME: check for quirks */
+                }
+        }
+        /*
+         * If we didn't get EDID, try checking if the panel is already turned
+         * on.  If so, assume that whatever is currently programmed is the
+         * correct mode.
+         */
+        lvds = REG_READ(LVDS);
+        pipe = (lvds & LVDS_PIPEB_SELECT) ? 1 : 0;
+        crtc = psb_intel_get_crtc_from_pipe(dev, pipe);
+
+        if (crtc && (lvds & LVDS_PORT_EN)) {
+                mode_dev->panel_fixed_mode =
+                    cdv_intel_crtc_mode_get(dev, crtc);
+                if (mode_dev->panel_fixed_mode) {
+                        mode_dev->panel_fixed_mode->type |=
+                            DRM_MODE_TYPE_PREFERRED;
+                        goto out;       /* FIXME: check for quirks */
+                }
+        }
+
+        /* If we still don't have a mode after all that, give up. */
+        if (!mode_dev->panel_fixed_mode) {
+                DRM_DEBUG
+                        ("Found no modes on the lvds, ignoring the LVDS\n");
+                goto failed_find;
+        }
+
+out:
+        drm_sysfs_connector_add(connector);
+        return;
+
+failed_find:
+        printk(KERN_ERR "Failed find\n");
+        if (psb_intel_output->ddc_bus)
+                psb_intel_i2c_destroy(psb_intel_output->ddc_bus);
+failed_ddc:
+        printk(KERN_ERR "Failed DDC\n");
+        if (psb_intel_output->i2c_bus)
+                psb_intel_i2c_destroy(psb_intel_output->i2c_bus);
+failed_blc_i2c:
+        printk(KERN_ERR "Failed BLC\n");
+        drm_encoder_cleanup(encoder);
+        drm_connector_cleanup(connector);
+        kfree(connector);
+}
diff --git a/drivers/staging/gma500/displays/hdmi.h b/drivers/staging/gma500/displays/hdmi.h
new file mode 100644
index 00000000000..d58ba9bd010
--- /dev/null
+++ b/drivers/staging/gma500/displays/hdmi.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c)  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:
+ * Thomas Eaton <thomas.g.eaton@intel.com>
+ * Scott Rowe <scott.m.rowe@intel.com>
+ */
+
+#ifndef HDMI_H
+#define HDMI_H
+
+extern void hdmi_init(struct drm_device *dev);
+
+#endif
diff --git a/drivers/staging/gma500/displays/pyr_cmd.h b/drivers/staging/gma500/displays/pyr_cmd.h
new file mode 100644
index 00000000000..84bae5c8c55
--- /dev/null
+++ b/drivers/staging/gma500/displays/pyr_cmd.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c)  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, sublicensen
+ * 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:
+ * Thomas Eaton <thomas.g.eaton@intel.com>
+ * Scott Rowe <scott.m.rowe@intel.com>
+ */
+
+#ifndef PYR_CMD_H
+#define PYR_CMD_H
+
+extern void pyr_cmd_init(struct drm_device *dev, struct panel_funcs *p_funcs);
+
+#endif
+
diff --git a/drivers/staging/gma500/displays/pyr_vid.h b/drivers/staging/gma500/displays/pyr_vid.h
new file mode 100644
index 00000000000..ce98860fa68
--- /dev/null
+++ b/drivers/staging/gma500/displays/pyr_vid.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c)  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, sublicensen
+ * 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:
+ * Thomas Eaton <thomas.g.eaton@intel.com>
+ * Scott Rowe <scott.m.rowe@intel.com>
+*/
+
+#ifndef PYR_VID_H
+#define PYR_VID_H
+
+extern void pyr_vid_init(struct drm_device *dev, struct panel_funcs *p_funcs);
+extern struct drm_display_mode *pyr_vid_get_config_mode(struct drm_device* dev);
+
+#endif
diff --git a/drivers/staging/gma500/displays/tmd_cmd.h b/drivers/staging/gma500/displays/tmd_cmd.h
new file mode 100644
index 00000000000..641e85eedec
--- /dev/null
+++ b/drivers/staging/gma500/displays/tmd_cmd.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c)  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, sublicensen
+ * 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:
+ * Thomas Eaton <thomas.g.eaton@intel.com>
+ * Scott Rowe <scott.m.rowe@intel.com>
+ */
+
+#ifndef TMD_CMD_H
+#define TMD_CMD_H
+
+extern void tmd_cmd_init(struct drm_device *dev, struct panel_funcs *p_funcs);
+extern struct drm_display_mode *tmd_cmd_get_config_mode(struct drm_device *dev);
+
+#endif
diff --git a/drivers/staging/gma500/displays/tmd_vid.h b/drivers/staging/gma500/displays/tmd_vid.h
new file mode 100644
index 00000000000..7a5fa3b935e
--- /dev/null
+++ b/drivers/staging/gma500/displays/tmd_vid.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c)  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, sublicensen
+ * 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:
+ * Thomas Eaton <thomas.g.eaton@intel.com>
+ * Scott Rowe <scott.m.rowe@intel.com>
+*/
+
+#ifndef TMD_VID_H
+#define TMD_VID_H
+
+extern void tmd_vid_init(struct drm_device *dev, struct panel_funcs *p_funcs);
+extern struct drm_display_mode *tmd_vid_get_config_mode(struct drm_device *dev);
+
+#endif
diff --git a/drivers/staging/gma500/displays/tpo_cmd.h b/drivers/staging/gma500/displays/tpo_cmd.h
new file mode 100644
index 00000000000..610552730d7
--- /dev/null
+++ b/drivers/staging/gma500/displays/tpo_cmd.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c)  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, sublicensen
+ * 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:
+ * Thomas Eaton <thomas.g.eaton@intel.com>
+ * Scott Rowe <scott.m.rowe@intel.com>
+*/
+
+#ifndef TPO_CMD_H
+#define TPO_CMD_H
+
+extern void tpo_cmd_init(struct drm_device *dev, struct panel_funcs *p_funcs);
+/* extern struct drm_display_mode * */
+/* tpo_cmd_get_config_mode(struct drm_device *dev); */
+
+#endif
diff --git a/drivers/staging/gma500/displays/tpo_vid.h b/drivers/staging/gma500/displays/tpo_vid.h
new file mode 100644
index 00000000000..c24f05722de
--- /dev/null
+++ b/drivers/staging/gma500/displays/tpo_vid.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c)  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, sublicensen
+ * 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:
+ * Thomas Eaton <thomas.g.eaton@intel.com>
+ * Scott Rowe <scott.m.rowe@intel.com>
+ */
+
+#ifndef TPO_VID_H
+#define TPO_VID_H
+
+extern void tpo_vid_init(struct drm_device *dev, struct panel_funcs *p_funcs);
+
+#endif
diff --git a/drivers/staging/gma500/framebuffer.c b/drivers/staging/gma500/framebuffer.c
new file mode 100644
index 00000000000..ebfde13ec18
--- /dev/null
+++ b/drivers/staging/gma500/framebuffer.c
@@ -0,0 +1,787 @@
+/**************************************************************************
+ * Copyright (c) 2007-2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+#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/init.h>
+#include <linux/console.h>
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include <drm/drm_crtc.h>
+
+#include "psb_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_intel_drv.h"
+#include "framebuffer.h"
+
+#include "mdfld_output.h"
+
+static void psb_user_framebuffer_destroy(struct drm_framebuffer *fb);
+static int psb_user_framebuffer_create_handle(struct drm_framebuffer *fb,
+                                              struct drm_file *file_priv,
+                                              unsigned int *handle);
+
+static const struct drm_framebuffer_funcs psb_fb_funcs = {
+        .destroy = psb_user_framebuffer_destroy,
+        .create_handle = psb_user_framebuffer_create_handle,
+};
+
+#define CMAP_TOHW(_val, _width) ((((_val) << (_width)) + 0x7FFF - (_val)) >> 16)
+
+static int psbfb_setcolreg(unsigned regno, unsigned red, unsigned green,
+                           unsigned blue, unsigned transp,
+                           struct fb_info *info)
+{
+        struct psb_fbdev *fbdev = info->par;
+        struct drm_framebuffer *fb = fbdev->psb_fb_helper.fb;
+        uint32_t v;
+
+        if (!fb)
+                return -ENOMEM;
+
+        if (regno > 255)
+                return 1;
+
+        red = CMAP_TOHW(red, info->var.red.length);
+        blue = CMAP_TOHW(blue, info->var.blue.length);
+        green = CMAP_TOHW(green, info->var.green.length);
+        transp = CMAP_TOHW(transp, info->var.transp.length);
+
+        v = (red << info->var.red.offset) |
+            (green << info->var.green.offset) |
+            (blue << info->var.blue.offset) |
+            (transp << info->var.transp.offset);
+
+        if (regno < 16) {
+                switch (fb->bits_per_pixel) {
+                case 16:
+                        ((uint32_t *) info->pseudo_palette)[regno] = v;
+                        break;
+                case 24:
+                case 32:
+                        ((uint32_t *) info->pseudo_palette)[regno] = v;
+                        break;
+                }
+        }
+
+        return 0;
+}
+
+
+void psbfb_suspend(struct drm_device *dev)
+{
+        struct drm_framebuffer *fb = 0;
+        struct psb_framebuffer *psbfb = to_psb_fb(fb);
+
+        console_lock();
+        mutex_lock(&dev->mode_config.mutex);
+        list_for_each_entry(fb, &dev->mode_config.fb_list, head) {
+                struct fb_info *info = psbfb->fbdev;
+                fb_set_suspend(info, 1);
+                drm_fb_helper_blank(FB_BLANK_POWERDOWN, info);
+        }
+        mutex_unlock(&dev->mode_config.mutex);
+        console_unlock();
+}
+
+void psbfb_resume(struct drm_device *dev)
+{
+        struct drm_framebuffer *fb = 0;
+        struct psb_framebuffer *psbfb = to_psb_fb(fb);
+
+        console_lock();
+        mutex_lock(&dev->mode_config.mutex);
+        list_for_each_entry(fb, &dev->mode_config.fb_list, head) {
+                struct fb_info *info = psbfb->fbdev;
+                fb_set_suspend(info, 0);
+                drm_fb_helper_blank(FB_BLANK_UNBLANK, info);
+        }
+        mutex_unlock(&dev->mode_config.mutex);
+        console_unlock();
+        drm_helper_disable_unused_functions(dev);
+}
+
+static int psbfb_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+        struct psb_framebuffer *psbfb = vma->vm_private_data;
+        struct drm_device *dev = psbfb->base.dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int page_num;
+        int i;
+        unsigned long address;
+        int ret;
+        unsigned long pfn;
+        /* FIXME: assumes fb at stolen base which may not be true */
+        unsigned long phys_addr = (unsigned long)dev_priv->stolen_base;
+
+        page_num = (vma->vm_end - vma->vm_start) >> PAGE_SHIFT;
+        address = (unsigned long)vmf->virtual_address;
+
+        vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+
+        for (i = 0; i < page_num; i++) {
+                pfn = (phys_addr >> PAGE_SHIFT);
+
+                ret = vm_insert_mixed(vma, address, pfn);
+                if (unlikely((ret == -EBUSY) || (ret != 0 && i > 0)))
+                        break;
+                else if (unlikely(ret != 0)) {
+                        ret = (ret == -ENOMEM) ? VM_FAULT_OOM : VM_FAULT_SIGBUS;
+                        return ret;
+                }
+                address += PAGE_SIZE;
+                phys_addr += PAGE_SIZE;
+        }
+        return VM_FAULT_NOPAGE;
+}
+
+static void psbfb_vm_open(struct vm_area_struct *vma)
+{
+}
+
+static void psbfb_vm_close(struct vm_area_struct *vma)
+{
+}
+
+static struct vm_operations_struct psbfb_vm_ops = {
+        .fault  = psbfb_vm_fault,
+        .open   = psbfb_vm_open,
+        .close  = psbfb_vm_close
+};
+
+static int psbfb_mmap(struct fb_info *info, struct vm_area_struct *vma)
+{
+        struct psb_fbdev *fbdev = info->par;
+        struct psb_framebuffer *psbfb = &fbdev->pfb;
+
+        if (vma->vm_pgoff != 0)
+                return -EINVAL;
+        if (vma->vm_pgoff > (~0UL >> PAGE_SHIFT))
+                return -EINVAL;
+
+        if (!psbfb->addr_space)
+                psbfb->addr_space = vma->vm_file->f_mapping;
+        /*
+         * If this is a GEM object then info->screen_base is the virtual
+         * kernel remapping of the object. FIXME: Review if this is
+         * suitable for our mmap work
+         */
+        vma->vm_ops = &psbfb_vm_ops;
+        vma->vm_private_data = (void *)psbfb;
+        vma->vm_flags |= VM_RESERVED | VM_IO |
+                                        VM_MIXEDMAP | VM_DONTEXPAND;
+        return 0;
+}
+
+static int psbfb_ioctl(struct fb_info *info, unsigned int cmd,
+                                                unsigned long arg)
+{
+        return -ENOTTY;
+}
+
+static struct fb_ops psbfb_ops = {
+        .owner = THIS_MODULE,
+        .fb_check_var = drm_fb_helper_check_var,
+        .fb_set_par = drm_fb_helper_set_par,
+        .fb_blank = drm_fb_helper_blank,
+        .fb_setcolreg = psbfb_setcolreg,
+        .fb_fillrect = cfb_fillrect,
+        .fb_copyarea = psbfb_copyarea,
+        .fb_imageblit = cfb_imageblit,
+        .fb_mmap = psbfb_mmap,
+        .fb_sync = psbfb_sync,
+        .fb_ioctl = psbfb_ioctl,
+};
+
+static struct fb_ops psbfb_unaccel_ops = {
+        .owner = THIS_MODULE,
+        .fb_check_var = drm_fb_helper_check_var,
+        .fb_set_par = drm_fb_helper_set_par,
+        .fb_blank = drm_fb_helper_blank,
+        .fb_setcolreg = psbfb_setcolreg,
+        .fb_fillrect = cfb_fillrect,
+        .fb_copyarea = cfb_copyarea,
+        .fb_imageblit = cfb_imageblit,
+        .fb_mmap = psbfb_mmap,
+        .fb_ioctl = psbfb_ioctl,
+};
+
+/**
+ *      psb_framebuffer_init    -       initialize a framebuffer
+ *      @dev: our DRM device
+ *      @fb: framebuffer to set up
+ *      @mode_cmd: mode description
+ *      @gt: backing object
+ *
+ *      Configure and fill in the boilerplate for our frame buffer. Return
+ *      0 on success or an error code if we fail.
+ */
+static int psb_framebuffer_init(struct drm_device *dev,
+                                        struct psb_framebuffer *fb,
+                                        struct drm_mode_fb_cmd *mode_cmd,
+                                        struct gtt_range *gt)
+{
+        int ret;
+
+        if (mode_cmd->pitch & 63)
+                return -EINVAL;
+        switch (mode_cmd->bpp) {
+        case 8:
+        case 16:
+        case 24:
+        case 32:
+                break;
+        default:
+                return -EINVAL;
+        }
+        ret = drm_framebuffer_init(dev, &fb->base, &psb_fb_funcs);
+        if (ret) {
+                dev_err(dev->dev, "framebuffer init failed: %d\n", ret);
+                return ret;
+        }
+        drm_helper_mode_fill_fb_struct(&fb->base, mode_cmd);
+        fb->gtt = gt;
+        return 0;
+}
+
+/**
+ *      psb_framebuffer_create  -       create a framebuffer backed by gt
+ *      @dev: our DRM device
+ *      @mode_cmd: the description of the requested mode
+ *      @gt: the backing object
+ *
+ *      Create a framebuffer object backed by the gt, and fill in the
+ *      boilerplate required
+ *
+ *      TODO: review object references
+ */
+
+static struct drm_framebuffer *psb_framebuffer_create
+                        (struct drm_device *dev,
+                         struct drm_mode_fb_cmd *mode_cmd,
+                         struct gtt_range *gt)
+{
+        struct psb_framebuffer *fb;
+        int ret;
+
+        fb = kzalloc(sizeof(*fb), GFP_KERNEL);
+        if (!fb)
+                return ERR_PTR(-ENOMEM);
+
+        ret = psb_framebuffer_init(dev, fb, mode_cmd, gt);
+        if (ret) {
+                kfree(fb);
+                return ERR_PTR(ret);
+        }
+        return &fb->base;
+}
+
+/**
+ *      psbfb_alloc             -       allocate frame buffer memory
+ *      @dev: the DRM device
+ *      @aligned_size: space needed
+ *
+ *      Allocate the frame buffer. In the usual case we get a GTT range that
+ *      is stolen memory backed and life is simple. If there isn't sufficient
+ *      stolen memory or the system has no stolen memory we allocate a range
+ *      and back it with a GEM object.
+ *
+ *      In this case the GEM object has no handle.
+ *
+ *      FIXME: console speed up - allocate twice the space if room and use
+ *      hardware scrolling for acceleration.
+ */
+static struct gtt_range *psbfb_alloc(struct drm_device *dev, int aligned_size)
+{
+        struct gtt_range *backing;
+        /* Begin by trying to use stolen memory backing */
+        backing = psb_gtt_alloc_range(dev, aligned_size, "fb", 1);
+        if (backing) {
+                if (drm_gem_private_object_init(dev,
+                                        &backing->gem, aligned_size) == 0)
+                        return backing;
+                psb_gtt_free_range(dev, backing);
+        }
+        /* Next try using GEM host memory */
+        backing = psb_gtt_alloc_range(dev, aligned_size, "fb(gem)", 0);
+        if (backing == NULL)
+                return NULL;
+
+        /* Now back it with an object */
+        if (drm_gem_object_init(dev, &backing->gem, aligned_size) != 0) {
+                psb_gtt_free_range(dev, backing);
+                return NULL;
+        }
+        return backing;
+}
+
+/**
+ *      psbfb_create            -       create a framebuffer
+ *      @fbdev: the framebuffer device
+ *      @sizes: specification of the layout
+ *
+ *      Create a framebuffer to the specifications provided
+ */
+static int psbfb_create(struct psb_fbdev *fbdev,
+                                struct drm_fb_helper_surface_size *sizes)
+{
+        struct drm_device *dev = fbdev->psb_fb_helper.dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct fb_info *info;
+        struct drm_framebuffer *fb;
+        struct psb_framebuffer *psbfb = &fbdev->pfb;
+        struct drm_mode_fb_cmd mode_cmd;
+        struct device *device = &dev->pdev->dev;
+        int size;
+        int ret;
+        struct gtt_range *backing;
+
+        mode_cmd.width = sizes->surface_width;
+        mode_cmd.height = sizes->surface_height;
+        mode_cmd.bpp = sizes->surface_bpp;
+
+        /* No 24bit packed */
+        if (mode_cmd.bpp == 24)
+                mode_cmd.bpp = 32;
+
+        /* HW requires pitch to be 64 byte aligned */
+        mode_cmd.pitch =  ALIGN(mode_cmd.width * ((mode_cmd.bpp + 7) / 8), 64);
+        mode_cmd.depth = sizes->surface_depth;
+
+        size = mode_cmd.pitch * mode_cmd.height;
+        size = ALIGN(size, PAGE_SIZE);
+
+        /* Allocate the framebuffer in the GTT with stolen page backing */
+        backing = psbfb_alloc(dev, size);
+        if (backing == NULL)
+                return -ENOMEM;
+
+        mutex_lock(&dev->struct_mutex);
+
+        info = framebuffer_alloc(0, device);
+        if (!info) {
+                ret = -ENOMEM;
+                goto out_err1;
+        }
+        info->par = fbdev;
+
+        ret = psb_framebuffer_init(dev, psbfb, &mode_cmd, backing);
+        if (ret)
+                goto out_unref;
+
+        fb = &psbfb->base;
+        psbfb->fbdev = info;
+
+        fbdev->psb_fb_helper.fb = fb;
+        fbdev->psb_fb_helper.fbdev = info;
+
+        strcpy(info->fix.id, "psbfb");
+
+        info->flags = FBINFO_DEFAULT;
+        /* No 2D engine */
+        if (!dev_priv->ops->accel_2d)
+                info->fbops = &psbfb_unaccel_ops;
+        else
+                info->fbops = &psbfb_ops;
+
+        ret = fb_alloc_cmap(&info->cmap, 256, 0);
+        if (ret) {
+                ret = -ENOMEM;
+                goto out_unref;
+        }
+
+        info->fix.smem_start = dev->mode_config.fb_base;
+        info->fix.smem_len = size;
+
+        if (backing->stolen) {
+                /* Accessed stolen memory directly */
+                info->screen_base = (char *)dev_priv->vram_addr +
+                                                        backing->offset;
+        } else {
+                /* Pin the pages into the GTT and create a mapping to them */
+                psb_gtt_pin(backing);
+                info->screen_base = vm_map_ram(backing->pages, backing->npage,
+                                -1, PAGE_KERNEL);
+                if (info->screen_base == NULL) {
+                        psb_gtt_unpin(backing);
+                        ret = -ENOMEM;
+                        goto out_unref;
+                }
+                psbfb->vm_map = 1;
+        }
+        info->screen_size = size;
+
+        if (dev_priv->gtt.stolen_size) {
+                info->apertures = alloc_apertures(1);
+                if (!info->apertures) {
+                        ret = -ENOMEM;
+                        goto out_unref;
+                }
+                info->apertures->ranges[0].base = dev->mode_config.fb_base;
+                info->apertures->ranges[0].size = dev_priv->gtt.stolen_size;
+        }
+
+        drm_fb_helper_fill_fix(info, fb->pitch, fb->depth);
+        drm_fb_helper_fill_var(info, &fbdev->psb_fb_helper,
+                                sizes->fb_width, sizes->fb_height);
+
+        info->fix.mmio_start = pci_resource_start(dev->pdev, 0);
+        info->fix.mmio_len = pci_resource_len(dev->pdev, 0);
+
+        info->pixmap.size = 64 * 1024;
+        info->pixmap.buf_align = 8;
+        info->pixmap.access_align = 32;
+        info->pixmap.flags = FB_PIXMAP_SYSTEM;
+        info->pixmap.scan_align = 1;
+
+        dev_info(dev->dev, "allocated %dx%d fb\n",
+                                        psbfb->base.width, psbfb->base.height);
+
+        mutex_unlock(&dev->struct_mutex);
+        return 0;
+out_unref:
+        if (backing->stolen)
+                psb_gtt_free_range(dev, backing);
+        else {
+                if (psbfb->vm_map)
+                        vm_unmap_ram(info->screen_base, backing->npage);
+                drm_gem_object_unreference(&backing->gem);
+        }
+out_err1:
+        mutex_unlock(&dev->struct_mutex);
+        psb_gtt_free_range(dev, backing);
+        return ret;
+}
+
+/**
+ *      psb_user_framebuffer_create     -       create framebuffer
+ *      @dev: our DRM device
+ *      @filp: client file
+ *      @cmd: mode request
+ *
+ *      Create a new framebuffer backed by a userspace GEM object
+ */
+static struct drm_framebuffer *psb_user_framebuffer_create
+                        (struct drm_device *dev, struct drm_file *filp,
+                         struct drm_mode_fb_cmd *cmd)
+{
+        struct gtt_range *r;
+        struct drm_gem_object *obj;
+
+        /*
+         *      Find the GEM object and thus the gtt range object that is
+         *      to back this space
+         */
+        obj = drm_gem_object_lookup(dev, filp, cmd->handle);
+        if (obj == NULL)
+                return ERR_PTR(-ENOENT);
+
+        /* Let the core code do all the work */
+        r = container_of(obj, struct gtt_range, gem);
+        return psb_framebuffer_create(dev, cmd, r);
+}
+
+static void psbfb_gamma_set(struct drm_crtc *crtc, u16 red, u16 green,
+                                                        u16 blue, int regno)
+{
+}
+
+static void psbfb_gamma_get(struct drm_crtc *crtc, u16 *red,
+                                        u16 *green, u16 *blue, int regno)
+{
+}
+
+static int psbfb_probe(struct drm_fb_helper *helper,
+                                struct drm_fb_helper_surface_size *sizes)
+{
+        struct psb_fbdev *psb_fbdev = (struct psb_fbdev *)helper;
+        int new_fb = 0;
+        int ret;
+
+        if (!helper->fb) {
+                ret = psbfb_create(psb_fbdev, sizes);
+                if (ret)
+                        return ret;
+                new_fb = 1;
+        }
+        return new_fb;
+}
+
+struct drm_fb_helper_funcs psb_fb_helper_funcs = {
+        .gamma_set = psbfb_gamma_set,
+        .gamma_get = psbfb_gamma_get,
+        .fb_probe = psbfb_probe,
+};
+
+int psb_fbdev_destroy(struct drm_device *dev, struct psb_fbdev *fbdev)
+{
+        struct fb_info *info;
+        struct psb_framebuffer *psbfb = &fbdev->pfb;
+
+        if (fbdev->psb_fb_helper.fbdev) {
+                info = fbdev->psb_fb_helper.fbdev;
+
+                /* If this is our base framebuffer then kill any virtual map
+                   for the framebuffer layer and unpin it */
+                if (psbfb->vm_map) {
+                        vm_unmap_ram(info->screen_base, psbfb->gtt->npage);
+                        psb_gtt_unpin(psbfb->gtt);
+                }
+                unregister_framebuffer(info);
+                if (info->cmap.len)
+                        fb_dealloc_cmap(&info->cmap);
+                framebuffer_release(info);
+        }
+        drm_fb_helper_fini(&fbdev->psb_fb_helper);
+        drm_framebuffer_cleanup(&psbfb->base);
+
+        if (psbfb->gtt)
+                drm_gem_object_unreference(&psbfb->gtt->gem);
+        return 0;
+}
+
+int psb_fbdev_init(struct drm_device *dev)
+{
+        struct psb_fbdev *fbdev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        fbdev = kzalloc(sizeof(struct psb_fbdev), GFP_KERNEL);
+        if (!fbdev) {
+                dev_err(dev->dev, "no memory\n");
+                return -ENOMEM;
+        }
+
+        dev_priv->fbdev = fbdev;
+        fbdev->psb_fb_helper.funcs = &psb_fb_helper_funcs;
+
+        drm_fb_helper_init(dev, &fbdev->psb_fb_helper, dev_priv->ops->crtcs,
+                                                        INTELFB_CONN_LIMIT);
+
+        drm_fb_helper_single_add_all_connectors(&fbdev->psb_fb_helper);
+        drm_fb_helper_initial_config(&fbdev->psb_fb_helper, 32);
+        return 0;
+}
+
+void psb_fbdev_fini(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (!dev_priv->fbdev)
+                return;
+
+        psb_fbdev_destroy(dev, dev_priv->fbdev);
+        kfree(dev_priv->fbdev);
+        dev_priv->fbdev = NULL;
+}
+
+static void psbfb_output_poll_changed(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct psb_fbdev *fbdev = (struct psb_fbdev *)dev_priv->fbdev;
+        drm_fb_helper_hotplug_event(&fbdev->psb_fb_helper);
+}
+
+/**
+ *      psb_user_framebuffer_create_handle - add hamdle to a framebuffer
+ *      @fb: framebuffer
+ *      @file_priv: our DRM file
+ *      @handle: returned handle
+ *
+ *      Our framebuffer object is a GTT range which also contains a GEM
+ *      object. We need to turn it into a handle for userspace. GEM will do
+ *      the work for us
+ */
+static int psb_user_framebuffer_create_handle(struct drm_framebuffer *fb,
+                                              struct drm_file *file_priv,
+                                              unsigned int *handle)
+{
+        struct psb_framebuffer *psbfb = to_psb_fb(fb);
+        struct gtt_range *r = psbfb->gtt;
+        return drm_gem_handle_create(file_priv, &r->gem, handle);
+}
+
+/**
+ *      psb_user_framebuffer_destroy    -       destruct user created fb
+ *      @fb: framebuffer
+ *
+ *      User framebuffers are backed by GEM objects so all we have to do is
+ *      clean up a bit and drop the reference, GEM will handle the fallout
+ */
+static void psb_user_framebuffer_destroy(struct drm_framebuffer *fb)
+{
+        struct psb_framebuffer *psbfb = to_psb_fb(fb);
+        struct gtt_range *r = psbfb->gtt;
+        struct drm_device *dev = fb->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct psb_fbdev *fbdev = dev_priv->fbdev;
+        struct drm_crtc *crtc;
+        int reset = 0;
+
+        /* Should never get stolen memory for a user fb */
+        WARN_ON(r->stolen);
+
+        /* Check if we are erroneously live */
+        list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
+                if (crtc->fb == fb)
+                        reset = 1;
+
+        if (reset)
+                /*
+                 * Now force a sane response before we permit the DRM CRTC
+                 * layer to do stupid things like blank the display. Instead
+                 * we reset this framebuffer as if the user had forced a reset.
+                 * We must do this before the cleanup so that the DRM layer
+                 * doesn't get a chance to stick its oar in where it isn't
+                 * wanted.
+                 */
+                drm_fb_helper_restore_fbdev_mode(&fbdev->psb_fb_helper);
+
+        /* Let DRM do its clean up */
+        drm_framebuffer_cleanup(fb);
+        /*  We are no longer using the resource in GEM */
+        drm_gem_object_unreference_unlocked(&r->gem);
+        kfree(fb);
+}
+
+static const struct drm_mode_config_funcs psb_mode_funcs = {
+        .fb_create = psb_user_framebuffer_create,
+        .output_poll_changed = psbfb_output_poll_changed,
+};
+
+static int psb_create_backlight_property(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct drm_property *backlight;
+
+        if (dev_priv->backlight_property)
+                return 0;
+
+        backlight = drm_property_create(dev, DRM_MODE_PROP_RANGE,
+                                                        "backlight", 2);
+        backlight->values[0] = 0;
+        backlight->values[1] = 100;
+
+        dev_priv->backlight_property = backlight;
+
+        return 0;
+}
+
+static void psb_setup_outputs(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct drm_connector *connector;
+
+        drm_mode_create_scaling_mode_property(dev);
+        psb_create_backlight_property(dev);
+
+        dev_priv->ops->output_init(dev);
+
+        list_for_each_entry(connector, &dev->mode_config.connector_list,
+                            head) {
+                struct psb_intel_output *psb_intel_output =
+                    to_psb_intel_output(connector);
+                struct drm_encoder *encoder = &psb_intel_output->enc;
+                int crtc_mask = 0, clone_mask = 0;
+
+                /* valid crtcs */
+                switch (psb_intel_output->type) {
+                case INTEL_OUTPUT_ANALOG:
+                        crtc_mask = (1 << 0);
+                        clone_mask = (1 << INTEL_OUTPUT_ANALOG);
+                        break;
+                case INTEL_OUTPUT_SDVO:
+                        crtc_mask = ((1 << 0) | (1 << 1));
+                        clone_mask = (1 << INTEL_OUTPUT_SDVO);
+                        break;
+                case INTEL_OUTPUT_LVDS:
+                        if (IS_MRST(dev))
+                                crtc_mask = (1 << 0);
+                        else
+                                crtc_mask = (1 << 1);
+                        clone_mask = (1 << INTEL_OUTPUT_LVDS);
+                        break;
+                case INTEL_OUTPUT_MIPI:
+                        crtc_mask = (1 << 0);
+                        clone_mask = (1 << INTEL_OUTPUT_MIPI);
+                        break;
+                case INTEL_OUTPUT_MIPI2:
+                        crtc_mask = (1 << 2);
+                        clone_mask = (1 << INTEL_OUTPUT_MIPI2);
+                        break;
+                case INTEL_OUTPUT_HDMI:
+                        if (IS_MFLD(dev))
+                                crtc_mask = (1 << 1);
+                        else    /* FIXME: review Oaktrail */
+                                crtc_mask = (1 << 0);   /* Cedarview */
+                        clone_mask = (1 << INTEL_OUTPUT_HDMI);
+                        break;
+                }
+                encoder->possible_crtcs = crtc_mask;
+                encoder->possible_clones =
+                    psb_intel_connector_clones(dev, clone_mask);
+        }
+}
+
+void psb_modeset_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv =
+            (struct drm_psb_private *) dev->dev_private;
+        struct psb_intel_mode_device *mode_dev = &dev_priv->mode_dev;
+        int i;
+
+        drm_mode_config_init(dev);
+
+        dev->mode_config.min_width = 0;
+        dev->mode_config.min_height = 0;
+
+        dev->mode_config.funcs = (void *) &psb_mode_funcs;
+
+        /* set memory base */
+        /* MRST and PSB should use BAR 2*/
+        pci_read_config_dword(dev->pdev, PSB_BSM, (u32 *)
+                                        &(dev->mode_config.fb_base));
+
+        /* num pipes is 2 for PSB but 1 for Mrst */
+        for (i = 0; i < dev_priv->num_pipe; i++)
+                psb_intel_crtc_init(dev, i, mode_dev);
+
+        dev->mode_config.max_width = 2048;
+        dev->mode_config.max_height = 2048;
+
+        psb_setup_outputs(dev);
+}
+
+void psb_modeset_cleanup(struct drm_device *dev)
+{
+        mutex_lock(&dev->struct_mutex);
+
+        drm_kms_helper_poll_fini(dev);
+        psb_fbdev_fini(dev);
+        drm_mode_config_cleanup(dev);
+
+        mutex_unlock(&dev->struct_mutex);
+}
diff --git a/drivers/staging/gma500/framebuffer.h b/drivers/staging/gma500/framebuffer.h
new file mode 100644
index 00000000000..d1b2289447f
--- /dev/null
+++ b/drivers/staging/gma500/framebuffer.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2008-2011, Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors:
+ *      Eric Anholt <eric@anholt.net>
+ *
+ */
+
+#ifndef _FRAMEBUFFER_H_
+#define _FRAMEBUFFER_H_
+
+#include <drm/drmP.h>
+#include <drm/drm_fb_helper.h>
+
+#include "psb_drv.h"
+
+struct psb_framebuffer {
+        struct drm_framebuffer base;
+        struct address_space *addr_space;
+        struct fb_info *fbdev;
+        struct gtt_range *gtt;
+        bool vm_map;            /* True if we must undo a vm_map_ram */
+};
+
+struct psb_fbdev {
+        struct drm_fb_helper psb_fb_helper;
+        struct psb_framebuffer pfb;
+};
+
+#define to_psb_fb(x) container_of(x, struct psb_framebuffer, base)
+
+extern int psb_intel_connector_clones(struct drm_device *dev, int type_mask);
+
+#endif
+
diff --git a/drivers/staging/gma500/gem.c b/drivers/staging/gma500/gem.c
new file mode 100644
index 00000000000..65fdd6b8ab1
--- /dev/null
+++ b/drivers/staging/gma500/gem.c
@@ -0,0 +1,295 @@
+/*
+ *  psb GEM interface
+ *
+ * Copyright (c) 2011, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors: Alan Cox
+ *
+ * TODO:
+ *      -       we need to work out if the MMU is relevant (eg for
+ *              accelerated operations on a GEM object)
+ */
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "psb_drm.h"
+#include "psb_drv.h"
+
+int psb_gem_init_object(struct drm_gem_object *obj)
+{
+        return -EINVAL;
+}
+
+void psb_gem_free_object(struct drm_gem_object *obj)
+{
+        struct gtt_range *gtt = container_of(obj, struct gtt_range, gem);
+        drm_gem_object_release_wrap(obj);
+        /* This must occur last as it frees up the memory of the GEM object */
+        psb_gtt_free_range(obj->dev, gtt);
+}
+
+int psb_gem_get_aperture(struct drm_device *dev, void *data,
+                                struct drm_file *file)
+{
+        return -EINVAL;
+}
+
+/**
+ *      psb_gem_dumb_map_gtt    -       buffer mapping for dumb interface
+ *      @file: our drm client file
+ *      @dev: drm device
+ *      @handle: GEM handle to the object (from dumb_create)
+ *
+ *      Do the necessary setup to allow the mapping of the frame buffer
+ *      into user memory. We don't have to do much here at the moment.
+ */
+int psb_gem_dumb_map_gtt(struct drm_file *file, struct drm_device *dev,
+                         uint32_t handle, uint64_t *offset)
+{
+        int ret = 0;
+        struct drm_gem_object *obj;
+
+        if (!(dev->driver->driver_features & DRIVER_GEM))
+                return -ENODEV;
+
+        mutex_lock(&dev->struct_mutex);
+
+        /* GEM does all our handle to object mapping */
+        obj = drm_gem_object_lookup(dev, file, handle);
+        if (obj == NULL) {
+                ret = -ENOENT;
+                goto unlock;
+        }
+        /* What validation is needed here ? */
+
+        /* Make it mmapable */
+        if (!obj->map_list.map) {
+                ret = gem_create_mmap_offset(obj);
+                if (ret)
+                        goto out;
+        }
+        /* GEM should really work out the hash offsets for us */
+        *offset = (u64)obj->map_list.hash.key << PAGE_SHIFT;
+out:
+        drm_gem_object_unreference(obj);
+unlock:
+        mutex_unlock(&dev->struct_mutex);
+        return ret;
+}
+
+/**
+ *      psb_gem_create          -       create a mappable object
+ *      @file: the DRM file of the client
+ *      @dev: our device
+ *      @size: the size requested
+ *      @handlep: returned handle (opaque number)
+ *
+ *      Create a GEM object, fill in the boilerplate and attach a handle to
+ *      it so that userspace can speak about it. This does the core work
+ *      for the various methods that do/will create GEM objects for things
+ */
+static int psb_gem_create(struct drm_file *file,
+        struct drm_device *dev, uint64_t size, uint32_t *handlep)
+{
+        struct gtt_range *r;
+        int ret;
+        u32 handle;
+
+        size = roundup(size, PAGE_SIZE);
+
+        /* Allocate our object - for now a direct gtt range which is not
+           stolen memory backed */
+        r = psb_gtt_alloc_range(dev, size, "gem", 0);
+        if (r == NULL) {
+                dev_err(dev->dev, "no memory for %lld byte GEM object\n", size);
+                return -ENOSPC;
+        }
+        /* Initialize the extra goodies GEM needs to do all the hard work */
+        if (drm_gem_object_init(dev, &r->gem, size) != 0) {
+                psb_gtt_free_range(dev, r);
+                /* GEM doesn't give an error code and we don't have an
+                   EGEMSUCKS so make something up for now - FIXME */
+                dev_err(dev->dev, "GEM init failed for %lld\n", size);
+                return -ENOMEM;
+        }
+        /* Give the object a handle so we can carry it more easily */
+        ret = drm_gem_handle_create(file, &r->gem, &handle);
+        if (ret) {
+                dev_err(dev->dev, "GEM handle failed for %p, %lld\n",
+                                                        &r->gem, size);
+                drm_gem_object_release(&r->gem);
+                psb_gtt_free_range(dev, r);
+                return ret;
+        }
+        /* We have the initial and handle reference but need only one now */
+        drm_gem_object_unreference(&r->gem);
+        *handlep = handle;
+        return 0;
+}
+
+/**
+ *      psb_gem_dumb_create     -       create a dumb buffer
+ *      @drm_file: our client file
+ *      @dev: our device
+ *      @args: the requested arguments copied from userspace
+ *
+ *      Allocate a buffer suitable for use for a frame buffer of the
+ *      form described by user space. Give userspace a handle by which
+ *      to reference it.
+ */
+int psb_gem_dumb_create(struct drm_file *file, struct drm_device *dev,
+                        struct drm_mode_create_dumb *args)
+{
+        args->pitch = ALIGN(args->width * ((args->bpp + 7) / 8), 64);
+        args->size = args->pitch * args->height;
+        return psb_gem_create(file, dev, args->size, &args->handle);
+}
+
+/**
+ *      psb_gem_dumb_destroy    -       destroy a dumb buffer
+ *      @file: client file
+ *      @dev: our DRM device
+ *      @handle: the object handle
+ *
+ *      Destroy a handle that was created via psb_gem_dumb_create, at least
+ *      we hope it was created that way. i915 seems to assume the caller
+ *      does the checking but that might be worth review ! FIXME
+ */
+int psb_gem_dumb_destroy(struct drm_file *file, struct drm_device *dev,
+                        uint32_t handle)
+{
+        /* No special work needed, drop the reference and see what falls out */
+        return drm_gem_handle_delete(file, handle);
+}
+
+/**
+ *      psb_gem_fault           -       pagefault handler for GEM objects
+ *      @vma: the VMA of the GEM object
+ *      @vmf: fault detail
+ *
+ *      Invoked when a fault occurs on an mmap of a GEM managed area. GEM
+ *      does most of the work for us including the actual map/unmap calls
+ *      but we need to do the actual page work.
+ *
+ *      This code eventually needs to handle faulting objects in and out
+ *      of the GTT and repacking it when we run out of space. We can put
+ *      that off for now and for our simple uses
+ *
+ *      The VMA was set up by GEM. In doing so it also ensured that the
+ *      vma->vm_private_data points to the GEM object that is backing this
+ *      mapping.
+ *
+ *      FIXME
+ */
+int psb_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+        struct drm_gem_object *obj;
+        struct gtt_range *r;
+        int ret;
+        unsigned long pfn;
+        pgoff_t page_offset;
+        struct drm_device *dev;
+        struct drm_psb_private *dev_priv;
+
+        obj = vma->vm_private_data;     /* GEM object */
+        dev = obj->dev;
+        dev_priv = dev->dev_private;
+
+        r = container_of(obj, struct gtt_range, gem);   /* Get the gtt range */
+
+        /* Make sure we don't parallel update on a fault, nor move or remove
+           something from beneath our feet */
+        mutex_lock(&dev->struct_mutex);
+
+        /* For now the mmap pins the object and it stays pinned. As things
+           stand that will do us no harm */
+        if (r->mmapping == 0) {
+                ret = psb_gtt_pin(r);
+                if (ret < 0) {
+                        dev_err(dev->dev, "gma500: pin failed: %d\n", ret);
+                        goto fail;
+                }
+                r->mmapping = 1;
+        }
+
+        /* Page relative to the VMA start - we must calculate this ourselves
+           because vmf->pgoff is the fake GEM offset */
+        page_offset = ((unsigned long) vmf->virtual_address - vma->vm_start)
+                                >> PAGE_SHIFT;
+
+        /* CPU view of the page, don't go via the GART for CPU writes */
+        if (r->stolen)
+                pfn = (dev_priv->stolen_base + r->offset) >> PAGE_SHIFT;
+        else
+                pfn = page_to_pfn(r->pages[page_offset]);
+        ret = vm_insert_pfn(vma, (unsigned long)vmf->virtual_address, pfn);
+
+fail:
+        mutex_unlock(&dev->struct_mutex);
+        switch (ret) {
+        case 0:
+        case -ERESTARTSYS:
+        case -EINTR:
+                return VM_FAULT_NOPAGE;
+        case -ENOMEM:
+                return VM_FAULT_OOM;
+        default:
+                return VM_FAULT_SIGBUS;
+        }
+}
+
+static int psb_gem_create_stolen(struct drm_file *file, struct drm_device *dev,
+                                                int size, u32 *handle)
+{
+        struct gtt_range *gtt = psb_gtt_alloc_range(dev, size, "gem", 1);
+        if (gtt == NULL)
+                return -ENOMEM;
+        if (drm_gem_private_object_init(dev, &gtt->gem, size) != 0)
+                goto free_gtt;
+        if (drm_gem_handle_create(file, &gtt->gem, handle) == 0)
+                return 0;
+free_gtt:
+        psb_gtt_free_range(dev, gtt);
+        return -ENOMEM;
+}
+
+/*
+ *      GEM interfaces for our specific client
+ */
+int psb_gem_create_ioctl(struct drm_device *dev, void *data,
+                                        struct drm_file *file)
+{
+        struct drm_psb_gem_create *args = data;
+        int ret;
+        if (args->flags & PSB_GEM_CREATE_STOLEN) {
+                ret = psb_gem_create_stolen(file, dev, args->size,
+                                                        &args->handle);
+                if (ret == 0)
+                        return 0;
+                /* Fall throguh */
+                args->flags &= ~PSB_GEM_CREATE_STOLEN;
+        }
+        return psb_gem_create(file, dev, args->size, &args->handle);
+}
+
+int psb_gem_mmap_ioctl(struct drm_device *dev, void *data,
+                                        struct drm_file *file)
+{
+        struct drm_psb_gem_mmap *args = data;
+        return dev->driver->dumb_map_offset(file, dev,
+                                                args->handle, &args->offset);
+}
+
diff --git a/drivers/staging/gma500/gem_glue.c b/drivers/staging/gma500/gem_glue.c
new file mode 100644
index 00000000000..daac1212065
--- /dev/null
+++ b/drivers/staging/gma500/gem_glue.c
@@ -0,0 +1,89 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+
+void drm_gem_object_release_wrap(struct drm_gem_object *obj)
+{
+        /* Remove the list map if one is present */
+        if (obj->map_list.map) {
+                struct drm_gem_mm *mm = obj->dev->mm_private;
+                struct drm_map_list *list = &obj->map_list;
+                drm_ht_remove_item(&mm->offset_hash, &list->hash);
+                drm_mm_put_block(list->file_offset_node);
+                kfree(list->map);
+                list->map = NULL;
+        }
+        drm_gem_object_release(obj);
+}
+
+/**
+ *      gem_create_mmap_offset          -       invent an mmap offset
+ *      @obj: our object
+ *
+ *      Standard implementation of offset generation for mmap as is
+ *      duplicated in several drivers. This belongs in GEM.
+ */
+int gem_create_mmap_offset(struct drm_gem_object *obj)
+{
+        struct drm_device *dev = obj->dev;
+        struct drm_gem_mm *mm = dev->mm_private;
+        struct drm_map_list *list;
+        struct drm_local_map *map;
+        int ret;
+
+        list = &obj->map_list;
+        list->map = kzalloc(sizeof(struct drm_map_list), GFP_KERNEL);
+        if (list->map == NULL)
+                return -ENOMEM;
+        map = list->map;
+        map->type = _DRM_GEM;
+        map->size = obj->size;
+        map->handle = obj;
+
+        list->file_offset_node = drm_mm_search_free(&mm->offset_manager,
+                                        obj->size / PAGE_SIZE, 0, 0);
+        if (!list->file_offset_node) {
+                dev_err(dev->dev, "failed to allocate offset for bo %d\n",
+                                                                obj->name);
+                ret = -ENOSPC;
+                goto free_it;
+        }
+        list->file_offset_node = drm_mm_get_block(list->file_offset_node,
+                                        obj->size / PAGE_SIZE, 0);
+        if (!list->file_offset_node) {
+                ret = -ENOMEM;
+                goto free_it;
+        }
+        list->hash.key = list->file_offset_node->start;
+        ret = drm_ht_insert_item(&mm->offset_hash, &list->hash);
+        if (ret) {
+                dev_err(dev->dev, "failed to add to map hash\n");
+                goto free_mm;
+        }
+        return 0;
+
+free_mm:
+        drm_mm_put_block(list->file_offset_node);
+free_it:
+        kfree(list->map);
+        list->map = NULL;
+        return ret;
+}
diff --git a/drivers/staging/gma500/gem_glue.h b/drivers/staging/gma500/gem_glue.h
new file mode 100644
index 00000000000..ce5ce30f74d
--- /dev/null
+++ b/drivers/staging/gma500/gem_glue.h
@@ -0,0 +1,2 @@
+extern void drm_gem_object_release_wrap(struct drm_gem_object *obj);
+extern int gem_create_mmap_offset(struct drm_gem_object *obj);
diff --git a/drivers/staging/gma500/gtt.c b/drivers/staging/gma500/gtt.c
new file mode 100644
index 00000000000..461ead251bb
--- /dev/null
+++ b/drivers/staging/gma500/gtt.c
@@ -0,0 +1,500 @@
+/*
+ * Copyright (c) 2007, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ * Authors: Thomas Hellstrom <thomas-at-tungstengraphics.com>
+ *          Alan Cox <alan@linux.intel.com>
+ */
+
+#include <drm/drmP.h>
+#include "psb_drv.h"
+
+
+/*
+ *      GTT resource allocator - manage page mappings in GTT space
+ */
+
+/**
+ *      psb_gtt_mask_pte        -       generate GTT pte entry
+ *      @pfn: page number to encode
+ *      @type: type of memory in the GTT
+ *
+ *      Set the GTT entry for the appropriate memory type.
+ */
+static inline uint32_t psb_gtt_mask_pte(uint32_t pfn, int type)
+{
+        uint32_t mask = PSB_PTE_VALID;
+
+        if (type & PSB_MMU_CACHED_MEMORY)
+                mask |= PSB_PTE_CACHED;
+        if (type & PSB_MMU_RO_MEMORY)
+                mask |= PSB_PTE_RO;
+        if (type & PSB_MMU_WO_MEMORY)
+                mask |= PSB_PTE_WO;
+
+        return (pfn << PAGE_SHIFT) | mask;
+}
+
+/**
+ *      psb_gtt_entry           -       find the GTT entries for a gtt_range
+ *      @dev: our DRM device
+ *      @r: our GTT range
+ *
+ *      Given a gtt_range object return the GTT offset of the page table
+ *      entries for this gtt_range
+ */
+u32 *psb_gtt_entry(struct drm_device *dev, struct gtt_range *r)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        unsigned long offset;
+
+        offset = r->resource.start - dev_priv->gtt_mem->start;
+
+        return dev_priv->gtt_map + (offset >> PAGE_SHIFT);
+}
+
+/**
+ *      psb_gtt_insert  -       put an object into the GTT
+ *      @dev: our DRM device
+ *      @r: our GTT range
+ *
+ *      Take our preallocated GTT range and insert the GEM object into
+ *      the GTT.
+ *
+ *      FIXME: gtt lock ?
+ */
+static int psb_gtt_insert(struct drm_device *dev, struct gtt_range *r)
+{
+        u32 *gtt_slot, pte;
+        struct page **pages;
+        int i;
+
+        if (r->pages == NULL) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+
+        WARN_ON(r->stolen);     /* refcount these maybe ? */
+
+        gtt_slot = psb_gtt_entry(dev, r);
+        pages = r->pages;
+
+        /* Make sure changes are visible to the GPU */
+        set_pages_array_uc(pages, r->npage);
+
+        /* Write our page entries into the GTT itself */
+        for (i = 0; i < r->npage; i++) {
+                pte = psb_gtt_mask_pte(page_to_pfn(*pages++), 0/*type*/);
+                iowrite32(pte, gtt_slot++);
+        }
+        /* Make sure all the entries are set before we return */
+        ioread32(gtt_slot - 1);
+        return 0;
+}
+
+/**
+ *      psb_gtt_remove  -       remove an object from the GTT
+ *      @dev: our DRM device
+ *      @r: our GTT range
+ *
+ *      Remove a preallocated GTT range from the GTT. Overwrite all the
+ *      page table entries with the dummy page
+ */
+
+static void psb_gtt_remove(struct drm_device *dev, struct gtt_range *r)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 *gtt_slot, pte;
+        int i;
+
+        WARN_ON(r->stolen);
+
+        gtt_slot = psb_gtt_entry(dev, r);
+        pte = psb_gtt_mask_pte(page_to_pfn(dev_priv->scratch_page), 0);
+
+        for (i = 0; i < r->npage; i++)
+                iowrite32(pte, gtt_slot++);
+        ioread32(gtt_slot - 1);
+        set_pages_array_wb(r->pages, r->npage);
+}
+
+/**
+ *      psb_gtt_attach_pages    -       attach and pin GEM pages
+ *      @gt: the gtt range
+ *
+ *      Pin and build an in kernel list of the pages that back our GEM object.
+ *      While we hold this the pages cannot be swapped out
+ */
+static int psb_gtt_attach_pages(struct gtt_range *gt)
+{
+        struct inode *inode;
+        struct address_space *mapping;
+        int i;
+        struct page *p;
+        int pages = gt->gem.size / PAGE_SIZE;
+
+        WARN_ON(gt->pages);
+
+        /* This is the shared memory object that backs the GEM resource */
+        inode = gt->gem.filp->f_path.dentry->d_inode;
+        mapping = inode->i_mapping;
+
+        gt->pages = kmalloc(pages * sizeof(struct page *), GFP_KERNEL);
+        if (gt->pages == NULL)
+                return -ENOMEM;
+        gt->npage = pages;
+
+        for (i = 0; i < pages; i++) {
+                /* FIXME: review flags later */
+                p = read_cache_page_gfp(mapping, i,
+                                        __GFP_COLD | GFP_KERNEL);
+                if (IS_ERR(p))
+                        goto err;
+                gt->pages[i] = p;
+        }
+        return 0;
+
+err:
+        while (i--)
+                page_cache_release(gt->pages[i]);
+        kfree(gt->pages);
+        gt->pages = NULL;
+        return PTR_ERR(p);
+}
+
+/**
+ *      psb_gtt_detach_pages    -       attach and pin GEM pages
+ *      @gt: the gtt range
+ *
+ *      Undo the effect of psb_gtt_attach_pages. At this point the pages
+ *      must have been removed from the GTT as they could now be paged out
+ *      and move bus address.
+ */
+static void psb_gtt_detach_pages(struct gtt_range *gt)
+{
+        int i;
+        for (i = 0; i < gt->npage; i++) {
+                /* FIXME: do we need to force dirty */
+                set_page_dirty(gt->pages[i]);
+                page_cache_release(gt->pages[i]);
+        }
+        kfree(gt->pages);
+        gt->pages = NULL;
+}
+
+/**
+ *      psb_gtt_pin             -       pin pages into the GTT
+ *      @gt: range to pin
+ *
+ *      Pin a set of pages into the GTT. The pins are refcounted so that
+ *      multiple pins need multiple unpins to undo.
+ *
+ *      Non GEM backed objects treat this as a no-op as they are always GTT
+ *      backed objects.
+ */
+int psb_gtt_pin(struct gtt_range *gt)
+{
+        int ret = 0;
+        struct drm_device *dev = gt->gem.dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        mutex_lock(&dev_priv->gtt_mutex);
+
+        if (gt->in_gart == 0 && gt->stolen == 0) {
+                ret = psb_gtt_attach_pages(gt);
+                if (ret < 0)
+                        goto out;
+                ret = psb_gtt_insert(dev, gt);
+                if (ret < 0) {
+                        psb_gtt_detach_pages(gt);
+                        goto out;
+                }
+        }
+        gt->in_gart++;
+out:
+        mutex_unlock(&dev_priv->gtt_mutex);
+        return ret;
+}
+
+/**
+ *      psb_gtt_unpin           -       Drop a GTT pin requirement
+ *      @gt: range to pin
+ *
+ *      Undoes the effect of psb_gtt_pin. On the last drop the GEM object
+ *      will be removed from the GTT which will also drop the page references
+ *      and allow the VM to clean up or page stuff.
+ *
+ *      Non GEM backed objects treat this as a no-op as they are always GTT
+ *      backed objects.
+ */
+void psb_gtt_unpin(struct gtt_range *gt)
+{
+        struct drm_device *dev = gt->gem.dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        mutex_lock(&dev_priv->gtt_mutex);
+
+        WARN_ON(!gt->in_gart);
+
+        gt->in_gart--;
+        if (gt->in_gart == 0 && gt->stolen == 0) {
+                psb_gtt_remove(dev, gt);
+                psb_gtt_detach_pages(gt);
+        }
+        mutex_unlock(&dev_priv->gtt_mutex);
+}
+
+/*
+ *      GTT resource allocator - allocate and manage GTT address space
+ */
+
+/**
+ *      psb_gtt_alloc_range     -       allocate GTT address space
+ *      @dev: Our DRM device
+ *      @len: length (bytes) of address space required
+ *      @name: resource name
+ *      @backed: resource should be backed by stolen pages
+ *
+ *      Ask the kernel core to find us a suitable range of addresses
+ *      to use for a GTT mapping.
+ *
+ *      Returns a gtt_range structure describing the object, or NULL on
+ *      error. On successful return the resource is both allocated and marked
+ *      as in use.
+ */
+struct gtt_range *psb_gtt_alloc_range(struct drm_device *dev, int len,
+                                                const char *name, int backed)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct gtt_range *gt;
+        struct resource *r = dev_priv->gtt_mem;
+        int ret;
+        unsigned long start, end;
+
+        if (backed) {
+                /* The start of the GTT is the stolen pages */
+                start = r->start;
+                end = r->start + dev_priv->gtt.stolen_size - 1;
+        } else {
+                /* The rest we will use for GEM backed objects */
+                start = r->start + dev_priv->gtt.stolen_size;
+                end = r->end;
+        }
+
+        gt = kzalloc(sizeof(struct gtt_range), GFP_KERNEL);
+        if (gt == NULL)
+                return NULL;
+        gt->resource.name = name;
+        gt->stolen = backed;
+        gt->in_gart = backed;
+        /* Ensure this is set for non GEM objects */
+        gt->gem.dev = dev;
+        ret = allocate_resource(dev_priv->gtt_mem, &gt->resource,
+                                len, start, end, PAGE_SIZE, NULL, NULL);
+        if (ret == 0) {
+                gt->offset = gt->resource.start - r->start;
+                return gt;
+        }
+        kfree(gt);
+        return NULL;
+}
+
+/**
+ *      psb_gtt_free_range      -       release GTT address space
+ *      @dev: our DRM device
+ *      @gt: a mapping created with psb_gtt_alloc_range
+ *
+ *      Release a resource that was allocated with psb_gtt_alloc_range. If the
+ *      object has been pinned by mmap users we clean this up here currently.
+ */
+void psb_gtt_free_range(struct drm_device *dev, struct gtt_range *gt)
+{
+        /* Undo the mmap pin if we are destroying the object */
+        if (gt->mmapping) {
+                psb_gtt_unpin(gt);
+                gt->mmapping = 0;
+        }
+        WARN_ON(gt->in_gart && !gt->stolen);
+        release_resource(&gt->resource);
+        kfree(gt);
+}
+
+void psb_gtt_alloc(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        init_rwsem(&dev_priv->gtt.sem);
+}
+
+void psb_gtt_takedown(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (dev_priv->gtt_map) {
+                iounmap(dev_priv->gtt_map);
+                dev_priv->gtt_map = NULL;
+        }
+        if (dev_priv->gtt_initialized) {
+                pci_write_config_word(dev->pdev, PSB_GMCH_CTRL,
+                                      dev_priv->gmch_ctrl);
+                PSB_WVDC32(dev_priv->pge_ctl, PSB_PGETBL_CTL);
+                (void) PSB_RVDC32(PSB_PGETBL_CTL);
+        }
+        if (dev_priv->vram_addr)
+                iounmap(dev_priv->gtt_map);
+}
+
+int psb_gtt_init(struct drm_device *dev, int resume)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        unsigned gtt_pages;
+        unsigned long stolen_size, vram_stolen_size;
+        unsigned i, num_pages;
+        unsigned pfn_base;
+        uint32_t vram_pages;
+        uint32_t dvmt_mode = 0;
+        struct psb_gtt *pg;
+
+        int ret = 0;
+        uint32_t pte;
+
+        mutex_init(&dev_priv->gtt_mutex);
+
+        psb_gtt_alloc(dev);
+        pg = &dev_priv->gtt;
+
+        /* Enable the GTT */
+        pci_read_config_word(dev->pdev, PSB_GMCH_CTRL, &dev_priv->gmch_ctrl);
+        pci_write_config_word(dev->pdev, PSB_GMCH_CTRL,
+                              dev_priv->gmch_ctrl | _PSB_GMCH_ENABLED);
+
+        dev_priv->pge_ctl = PSB_RVDC32(PSB_PGETBL_CTL);
+        PSB_WVDC32(dev_priv->pge_ctl | _PSB_PGETBL_ENABLED, PSB_PGETBL_CTL);
+        (void) PSB_RVDC32(PSB_PGETBL_CTL);
+
+        /* The root resource we allocate address space from */
+        dev_priv->gtt_initialized = 1;
+
+        pg->gtt_phys_start = dev_priv->pge_ctl & PAGE_MASK;
+
+        /*
+         *      FIXME: video mmu has hw bug to access 0x0D0000000,
+         *      then make gatt start at 0x0e000,0000
+         */
+        pg->mmu_gatt_start = 0xE0000000;
+
+        pg->gtt_start = pci_resource_start(dev->pdev, PSB_GTT_RESOURCE);
+        gtt_pages = pci_resource_len(dev->pdev, PSB_GTT_RESOURCE)
+                                                                >> PAGE_SHIFT;
+        /* CDV workaround */
+        if (pg->gtt_start == 0 || gtt_pages == 0) {
+                dev_err(dev->dev, "GTT PCI BAR not initialized.\n");
+                gtt_pages = 64;
+                pg->gtt_start = dev_priv->pge_ctl;
+        }
+
+        pg->gatt_start = pci_resource_start(dev->pdev, PSB_GATT_RESOURCE);
+        pg->gatt_pages = pci_resource_len(dev->pdev, PSB_GATT_RESOURCE)
+                                                                >> PAGE_SHIFT;
+        dev_priv->gtt_mem = &dev->pdev->resource[PSB_GATT_RESOURCE];
+
+        if (pg->gatt_pages == 0 || pg->gatt_start == 0) {
+                static struct resource fudge;   /* Preferably peppermint */
+
+                /* This can occur on CDV SDV systems. Fudge it in this case.
+                   We really don't care what imaginary space is being allocated
+                   at this point */
+                dev_err(dev->dev, "GATT PCI BAR not initialized.\n");
+                pg->gatt_start = 0x40000000;
+                pg->gatt_pages = (128 * 1024 * 1024) >> PAGE_SHIFT;
+                fudge.start = 0x40000000;
+                fudge.end = 0x40000000 + 128 * 1024 * 1024 - 1;
+                fudge.name = "fudge";
+                fudge.flags = IORESOURCE_MEM;
+                dev_priv->gtt_mem = &fudge;
+        }
+
+        pci_read_config_dword(dev->pdev, PSB_BSM, &dev_priv->stolen_base);
+        vram_stolen_size = pg->gtt_phys_start - dev_priv->stolen_base
+                                                                - PAGE_SIZE;
+
+        stolen_size = vram_stolen_size;
+
+        printk(KERN_INFO "Stolen memory information\n");
+        printk(KERN_INFO "       base in RAM: 0x%x\n", dev_priv->stolen_base);
+        printk(KERN_INFO "       size: %luK, calculated by (GTT RAM base) - (Stolen base), seems wrong\n",
+                vram_stolen_size/1024);
+        dvmt_mode = (dev_priv->gmch_ctrl >> 4) & 0x7;
+        printk(KERN_INFO "      the correct size should be: %dM(dvmt mode=%d)\n",
+                (dvmt_mode == 1) ? 1 : (2 << (dvmt_mode - 1)), dvmt_mode);
+
+        if (resume && (gtt_pages != pg->gtt_pages) &&
+            (stolen_size != pg->stolen_size)) {
+                dev_err(dev->dev, "GTT resume error.\n");
+                ret = -EINVAL;
+                goto out_err;
+        }
+
+        pg->gtt_pages = gtt_pages;
+        pg->stolen_size = stolen_size;
+        dev_priv->vram_stolen_size = vram_stolen_size;
+
+        /*
+         *      Map the GTT and the stolen memory area
+         */
+        dev_priv->gtt_map = ioremap_nocache(pg->gtt_phys_start,
+                                                gtt_pages << PAGE_SHIFT);
+        if (!dev_priv->gtt_map) {
+                dev_err(dev->dev, "Failure to map gtt.\n");
+                ret = -ENOMEM;
+                goto out_err;
+        }
+
+        dev_priv->vram_addr = ioremap_wc(dev_priv->stolen_base, stolen_size);
+        if (!dev_priv->vram_addr) {
+                dev_err(dev->dev, "Failure to map stolen base.\n");
+                ret = -ENOMEM;
+                goto out_err;
+        }
+
+        /*
+         * Insert vram stolen pages into the GTT
+         */
+
+        pfn_base = dev_priv->stolen_base >> PAGE_SHIFT;
+        vram_pages = num_pages = vram_stolen_size >> PAGE_SHIFT;
+        printk(KERN_INFO"Set up %d stolen pages starting at 0x%08x, GTT offset %dK\n",
+                num_pages, pfn_base << PAGE_SHIFT, 0);
+        for (i = 0; i < num_pages; ++i) {
+                pte = psb_gtt_mask_pte(pfn_base + i, 0);
+                iowrite32(pte, dev_priv->gtt_map + i);
+        }
+
+        /*
+         * Init rest of GTT to the scratch page to avoid accidents or scribbles
+         */
+
+        pfn_base = page_to_pfn(dev_priv->scratch_page);
+        pte = psb_gtt_mask_pte(pfn_base, 0);
+        for (; i < gtt_pages; ++i)
+                iowrite32(pte, dev_priv->gtt_map + i);
+
+        (void) ioread32(dev_priv->gtt_map + i - 1);
+        return 0;
+
+out_err:
+        psb_gtt_takedown(dev);
+        return ret;
+}
diff --git a/drivers/staging/gma500/gtt.h b/drivers/staging/gma500/gtt.h
new file mode 100644
index 00000000000..e0e1cb6f9bd
--- /dev/null
+++ b/drivers/staging/gma500/gtt.h
@@ -0,0 +1,61 @@
+/**************************************************************************
+ * Copyright (c) 2007-2008, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+#ifndef _PSB_GTT_H_
+#define _PSB_GTT_H_
+
+#include <drm/drmP.h>
+
+/* This wants cleaning up with respect to the psb_dev and un-needed stuff */
+struct psb_gtt {
+        uint32_t gatt_start;
+        uint32_t mmu_gatt_start;
+        uint32_t gtt_start;
+        uint32_t gtt_phys_start;
+        unsigned gtt_pages;
+        unsigned gatt_pages;
+        unsigned long stolen_size;
+        unsigned long vram_stolen_size;
+        struct rw_semaphore sem;
+};
+
+/* Exported functions */
+extern int psb_gtt_init(struct drm_device *dev, int resume);
+extern void psb_gtt_takedown(struct drm_device *dev);
+
+/* Each gtt_range describes an allocation in the GTT area */
+struct gtt_range {
+        struct resource resource;       /* Resource for our allocation */
+        u32 offset;                     /* GTT offset of our object */
+        struct drm_gem_object gem;      /* GEM high level stuff */
+        int in_gart;                    /* Currently in the GART (ref ct) */
+        bool stolen;                    /* Backed from stolen RAM */
+        bool mmapping;                  /* Is mmappable */
+        struct page **pages;            /* Backing pages if present */
+        int npage;                      /* Number of backing pages */
+};
+
+extern struct gtt_range *psb_gtt_alloc_range(struct drm_device *dev, int len,
+                                                const char *name, int backed);
+extern void psb_gtt_kref_put(struct gtt_range *gt);
+extern void psb_gtt_free_range(struct drm_device *dev, struct gtt_range *gt);
+extern int psb_gtt_pin(struct gtt_range *gt);
+extern void psb_gtt_unpin(struct gtt_range *gt);
+
+#endif
diff --git a/drivers/staging/gma500/intel_bios.c b/drivers/staging/gma500/intel_bios.c
new file mode 100644
index 00000000000..096757f9bc8
--- /dev/null
+++ b/drivers/staging/gma500/intel_bios.c
@@ -0,0 +1,303 @@
+/*
+ * Copyright (c) 2006 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors:
+ *    Eric Anholt <eric@anholt.net>
+ *
+ */
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "psb_drm.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "intel_bios.h"
+
+
+static void *find_section(struct bdb_header *bdb, int section_id)
+{
+        u8 *base = (u8 *)bdb;
+        int index = 0;
+        u16 total, current_size;
+        u8 current_id;
+
+        /* skip to first section */
+        index += bdb->header_size;
+        total = bdb->bdb_size;
+
+        /* walk the sections looking for section_id */
+        while (index < total) {
+                current_id = *(base + index);
+                index++;
+                current_size = *((u16 *)(base + index));
+                index += 2;
+                if (current_id == section_id)
+                        return base + index;
+                index += current_size;
+        }
+
+        return NULL;
+}
+
+static void fill_detail_timing_data(struct drm_display_mode *panel_fixed_mode,
+                        struct lvds_dvo_timing *dvo_timing)
+{
+        panel_fixed_mode->hdisplay = (dvo_timing->hactive_hi << 8) |
+                dvo_timing->hactive_lo;
+        panel_fixed_mode->hsync_start = panel_fixed_mode->hdisplay +
+                ((dvo_timing->hsync_off_hi << 8) | dvo_timing->hsync_off_lo);
+        panel_fixed_mode->hsync_end = panel_fixed_mode->hsync_start +
+                dvo_timing->hsync_pulse_width;
+        panel_fixed_mode->htotal = panel_fixed_mode->hdisplay +
+                ((dvo_timing->hblank_hi << 8) | dvo_timing->hblank_lo);
+
+        panel_fixed_mode->vdisplay = (dvo_timing->vactive_hi << 8) |
+                dvo_timing->vactive_lo;
+        panel_fixed_mode->vsync_start = panel_fixed_mode->vdisplay +
+                dvo_timing->vsync_off;
+        panel_fixed_mode->vsync_end = panel_fixed_mode->vsync_start +
+                dvo_timing->vsync_pulse_width;
+        panel_fixed_mode->vtotal = panel_fixed_mode->vdisplay +
+                ((dvo_timing->vblank_hi << 8) | dvo_timing->vblank_lo);
+        panel_fixed_mode->clock = dvo_timing->clock * 10;
+        panel_fixed_mode->type = DRM_MODE_TYPE_PREFERRED;
+
+        /* 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;
+        if (panel_fixed_mode->vsync_end > panel_fixed_mode->vtotal)
+                panel_fixed_mode->vtotal = panel_fixed_mode->vsync_end + 1;
+
+        drm_mode_set_name(panel_fixed_mode);
+}
+
+static void parse_backlight_data(struct drm_psb_private *dev_priv,
+                                struct bdb_header *bdb)
+{
+        struct bdb_lvds_backlight *vbt_lvds_bl = NULL;
+        struct bdb_lvds_backlight *lvds_bl;
+        u8 p_type = 0;
+        void *bl_start = NULL;
+        struct bdb_lvds_options *lvds_opts
+                                = find_section(bdb, BDB_LVDS_OPTIONS);
+
+        dev_priv->lvds_bl = NULL;
+
+        if (lvds_opts)
+                p_type = lvds_opts->panel_type;
+        else
+                return;
+
+        bl_start = find_section(bdb, BDB_LVDS_BACKLIGHT);
+        vbt_lvds_bl = (struct bdb_lvds_backlight *)(bl_start + 1) + p_type;
+
+        lvds_bl = kzalloc(sizeof(*vbt_lvds_bl), GFP_KERNEL);
+        if (!lvds_bl) {
+                dev_err(dev_priv->dev->dev, "out of memory for backlight data\n");
+                return;
+        }
+        memcpy(lvds_bl, vbt_lvds_bl, sizeof(*vbt_lvds_bl));
+        dev_priv->lvds_bl = lvds_bl;
+}
+
+/* Try to find integrated panel data */
+static void parse_lfp_panel_data(struct drm_psb_private *dev_priv,
+                            struct bdb_header *bdb)
+{
+        struct bdb_lvds_options *lvds_options;
+        struct bdb_lvds_lfp_data *lvds_lfp_data;
+        struct bdb_lvds_lfp_data_entry *entry;
+        struct lvds_dvo_timing *dvo_timing;
+        struct drm_display_mode *panel_fixed_mode;
+
+        /* Defaults if we can't find VBT info */
+        dev_priv->lvds_dither = 0;
+        dev_priv->lvds_vbt = 0;
+
+        lvds_options = find_section(bdb, BDB_LVDS_OPTIONS);
+        if (!lvds_options)
+                return;
+
+        dev_priv->lvds_dither = lvds_options->pixel_dither;
+        if (lvds_options->panel_type == 0xff)
+                return;
+
+        lvds_lfp_data = find_section(bdb, BDB_LVDS_LFP_DATA);
+        if (!lvds_lfp_data)
+                return;
+
+
+        entry = &lvds_lfp_data->data[lvds_options->panel_type];
+        dvo_timing = &entry->dvo_timing;
+
+        panel_fixed_mode = kzalloc(sizeof(*panel_fixed_mode),
+                                      GFP_KERNEL);
+        if (panel_fixed_mode == NULL) {
+                dev_err(dev_priv->dev->dev, "out of memory for fixed panel mode\n");
+                return;
+        }
+
+        dev_priv->lvds_vbt = 1;
+        fill_detail_timing_data(panel_fixed_mode, dvo_timing);
+
+        if (panel_fixed_mode->htotal > 0 && panel_fixed_mode->vtotal > 0) {
+                dev_priv->lfp_lvds_vbt_mode = panel_fixed_mode;
+                drm_mode_debug_printmodeline(panel_fixed_mode);
+        } else {
+                dev_dbg(dev_priv->dev->dev, "ignoring invalid LVDS VBT\n");
+                dev_priv->lvds_vbt = 0;
+                kfree(panel_fixed_mode);
+        }
+        return;
+}
+
+/* Try to find sdvo panel data */
+static void parse_sdvo_panel_data(struct drm_psb_private *dev_priv,
+                      struct bdb_header *bdb)
+{
+        struct bdb_sdvo_lvds_options *sdvo_lvds_options;
+        struct lvds_dvo_timing *dvo_timing;
+        struct drm_display_mode *panel_fixed_mode;
+
+        dev_priv->sdvo_lvds_vbt_mode = NULL;
+
+        sdvo_lvds_options = find_section(bdb, BDB_SDVO_LVDS_OPTIONS);
+        if (!sdvo_lvds_options)
+                return;
+
+        dvo_timing = find_section(bdb, BDB_SDVO_PANEL_DTDS);
+        if (!dvo_timing)
+                return;
+
+        panel_fixed_mode = kzalloc(sizeof(*panel_fixed_mode), GFP_KERNEL);
+
+        if (!panel_fixed_mode)
+                return;
+
+        fill_detail_timing_data(panel_fixed_mode,
+                        dvo_timing + sdvo_lvds_options->panel_type);
+
+        dev_priv->sdvo_lvds_vbt_mode = panel_fixed_mode;
+
+        return;
+}
+
+static void parse_general_features(struct drm_psb_private *dev_priv,
+                       struct bdb_header *bdb)
+{
+        struct bdb_general_features *general;
+
+        /* Set sensible defaults in case we can't find the general block */
+        dev_priv->int_tv_support = 1;
+        dev_priv->int_crt_support = 1;
+
+        general = find_section(bdb, BDB_GENERAL_FEATURES);
+        if (general) {
+                dev_priv->int_tv_support = general->int_tv_support;
+                dev_priv->int_crt_support = general->int_crt_support;
+                dev_priv->lvds_use_ssc = general->enable_ssc;
+
+                if (dev_priv->lvds_use_ssc) {
+                        dev_priv->lvds_ssc_freq
+                                = general->ssc_freq ? 100 : 96;
+                }
+        }
+}
+
+/**
+ * psb_intel_init_bios - initialize VBIOS settings & find VBT
+ * @dev: DRM device
+ *
+ * Loads the Video BIOS and checks that the VBT exists.  Sets scratch registers
+ * to appropriate values.
+ *
+ * VBT existence is a sanity check that is relied on by other i830_bios.c code.
+ * Note that it would be better to use a BIOS call to get the VBT, as BIOSes may
+ * feed an updated VBT back through that, compared to what we'll fetch using
+ * this method of groping around in the BIOS data.
+ *
+ * Returns 0 on success, nonzero on failure.
+ */
+bool psb_intel_init_bios(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct pci_dev *pdev = dev->pdev;
+        struct vbt_header *vbt = NULL;
+        struct bdb_header *bdb;
+        u8 __iomem *bios;
+        size_t size;
+        int i;
+
+        bios = pci_map_rom(pdev, &size);
+        if (!bios)
+                return -1;
+
+        /* Scour memory looking for the VBT signature */
+        for (i = 0; i + 4 < size; i++) {
+                if (!memcmp(bios + i, "$VBT", 4)) {
+                        vbt = (struct vbt_header *)(bios + i);
+                        break;
+                }
+        }
+
+        if (!vbt) {
+                dev_err(dev->dev, "VBT signature missing\n");
+                pci_unmap_rom(pdev, bios);
+                return -1;
+        }
+
+        bdb = (struct bdb_header *)(bios + i + vbt->bdb_offset);
+
+        /* Grab useful general definitions */
+        parse_general_features(dev_priv, bdb);
+        parse_lfp_panel_data(dev_priv, bdb);
+        parse_sdvo_panel_data(dev_priv, bdb);
+        parse_backlight_data(dev_priv, bdb);
+
+        pci_unmap_rom(pdev, bios);
+
+        return 0;
+}
+
+/**
+ * Destroy and free VBT data
+ */
+void psb_intel_destroy_bios(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct drm_display_mode *sdvo_lvds_vbt_mode =
+                                dev_priv->sdvo_lvds_vbt_mode;
+        struct drm_display_mode *lfp_lvds_vbt_mode =
+                                dev_priv->lfp_lvds_vbt_mode;
+        struct bdb_lvds_backlight *lvds_bl =
+                                dev_priv->lvds_bl;
+
+        /*free sdvo panel mode*/
+        if (sdvo_lvds_vbt_mode) {
+                dev_priv->sdvo_lvds_vbt_mode = NULL;
+                kfree(sdvo_lvds_vbt_mode);
+        }
+
+        if (lfp_lvds_vbt_mode) {
+                dev_priv->lfp_lvds_vbt_mode = NULL;
+                kfree(lfp_lvds_vbt_mode);
+        }
+
+        if (lvds_bl) {
+                dev_priv->lvds_bl = NULL;
+                kfree(lvds_bl);
+        }
+}
diff --git a/drivers/staging/gma500/intel_bios.h b/drivers/staging/gma500/intel_bios.h
new file mode 100644
index 00000000000..70f1bf01818
--- /dev/null
+++ b/drivers/staging/gma500/intel_bios.h
@@ -0,0 +1,430 @@
+/*
+ * Copyright (c) 2006 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors:
+ *    Eric Anholt <eric@anholt.net>
+ *
+ */
+
+#ifndef _I830_BIOS_H_
+#define _I830_BIOS_H_
+
+#include <drm/drmP.h>
+
+struct vbt_header {
+        u8 signature[20];               /**< Always starts with 'VBT$' */
+        u16 version;                    /**< decimal */
+        u16 header_size;                /**< in bytes */
+        u16 vbt_size;                   /**< in bytes */
+        u8 vbt_checksum;
+        u8 reserved0;
+        u32 bdb_offset;                 /**< from beginning of VBT */
+        u32 aim_offset[4];              /**< from beginning of VBT */
+} __attribute__((packed));
+
+
+struct bdb_header {
+        u8 signature[16];               /**< Always 'BIOS_DATA_BLOCK' */
+        u16 version;                    /**< decimal */
+        u16 header_size;                /**< in bytes */
+        u16 bdb_size;                   /**< in bytes */
+};
+
+/* strictly speaking, this is a "skip" block, but it has interesting info */
+struct vbios_data {
+        u8 type; /* 0 == desktop, 1 == mobile */
+        u8 relstage;
+        u8 chipset;
+        u8 lvds_present:1;
+        u8 tv_present:1;
+        u8 rsvd2:6; /* finish byte */
+        u8 rsvd3[4];
+        u8 signon[155];
+        u8 copyright[61];
+        u16 code_segment;
+        u8 dos_boot_mode;
+        u8 bandwidth_percent;
+        u8 rsvd4; /* popup memory size */
+        u8 resize_pci_bios;
+        u8 rsvd5; /* is crt already on ddc2 */
+} __attribute__((packed));
+
+/*
+ * There are several types of BIOS data blocks (BDBs), each block has
+ * an ID and size in the first 3 bytes (ID in first, size in next 2).
+ * Known types are listed below.
+ */
+#define BDB_GENERAL_FEATURES      1
+#define BDB_GENERAL_DEFINITIONS   2
+#define BDB_OLD_TOGGLE_LIST       3
+#define BDB_MODE_SUPPORT_LIST     4
+#define BDB_GENERIC_MODE_TABLE    5
+#define BDB_EXT_MMIO_REGS         6
+#define BDB_SWF_IO                7
+#define BDB_SWF_MMIO              8
+#define BDB_DOT_CLOCK_TABLE       9
+#define BDB_MODE_REMOVAL_TABLE   10
+#define BDB_CHILD_DEVICE_TABLE   11
+#define BDB_DRIVER_FEATURES      12
+#define BDB_DRIVER_PERSISTENCE   13
+#define BDB_EXT_TABLE_PTRS       14
+#define BDB_DOT_CLOCK_OVERRIDE   15
+#define BDB_DISPLAY_SELECT       16
+/* 17 rsvd */
+#define BDB_DRIVER_ROTATION      18
+#define BDB_DISPLAY_REMOVE       19
+#define BDB_OEM_CUSTOM           20
+#define BDB_EFP_LIST             21 /* workarounds for VGA hsync/vsync */
+#define BDB_SDVO_LVDS_OPTIONS    22
+#define BDB_SDVO_PANEL_DTDS      23
+#define BDB_SDVO_LVDS_PNP_IDS    24
+#define BDB_SDVO_LVDS_POWER_SEQ  25
+#define BDB_TV_OPTIONS           26
+#define BDB_LVDS_OPTIONS         40
+#define BDB_LVDS_LFP_DATA_PTRS   41
+#define BDB_LVDS_LFP_DATA        42
+#define BDB_LVDS_BACKLIGHT       43
+#define BDB_LVDS_POWER           44
+#define BDB_SKIP                254 /* VBIOS private block, ignore */
+
+struct bdb_general_features {
+        /* bits 1 */
+        u8 panel_fitting:2;
+        u8 flexaim:1;
+        u8 msg_enable:1;
+        u8 clear_screen:3;
+        u8 color_flip:1;
+
+        /* bits 2 */
+        u8 download_ext_vbt:1;
+        u8 enable_ssc:1;
+        u8 ssc_freq:1;
+        u8 enable_lfp_on_override:1;
+        u8 disable_ssc_ddt:1;
+        u8 rsvd8:3; /* finish byte */
+
+        /* bits 3 */
+        u8 disable_smooth_vision:1;
+        u8 single_dvi:1;
+        u8 rsvd9:6; /* finish byte */
+
+        /* bits 4 */
+        u8 legacy_monitor_detect;
+
+        /* bits 5 */
+        u8 int_crt_support:1;
+        u8 int_tv_support:1;
+        u8 rsvd11:6; /* finish byte */
+} __attribute__((packed));
+
+struct bdb_general_definitions {
+        /* DDC GPIO */
+        u8 crt_ddc_gmbus_pin;
+
+        /* DPMS bits */
+        u8 dpms_acpi:1;
+        u8 skip_boot_crt_detect:1;
+        u8 dpms_aim:1;
+        u8 rsvd1:5; /* finish byte */
+
+        /* boot device bits */
+        u8 boot_display[2];
+        u8 child_dev_size;
+
+        /* device info */
+        u8 tv_or_lvds_info[33];
+        u8 dev1[33];
+        u8 dev2[33];
+        u8 dev3[33];
+        u8 dev4[33];
+        /* may be another device block here on some platforms */
+};
+
+struct bdb_lvds_options {
+        u8 panel_type;
+        u8 rsvd1;
+        /* LVDS capabilities, stored in a dword */
+        u8 pfit_mode:2;
+        u8 pfit_text_mode_enhanced:1;
+        u8 pfit_gfx_mode_enhanced:1;
+        u8 pfit_ratio_auto:1;
+        u8 pixel_dither:1;
+        u8 lvds_edid:1;
+        u8 rsvd2:1;
+        u8 rsvd4;
+} __attribute__((packed));
+
+struct bdb_lvds_backlight {
+        u8 type:2;
+        u8 pol:1;
+        u8 gpio:3;
+        u8 gmbus:2;
+        u16 freq;
+        u8 minbrightness;
+        u8 i2caddr;
+        u8 brightnesscmd;
+        /*FIXME: more...*/
+} __attribute__((packed));
+
+/* LFP pointer table contains entries to the struct below */
+struct bdb_lvds_lfp_data_ptr {
+        u16 fp_timing_offset; /* offsets are from start of bdb */
+        u8 fp_table_size;
+        u16 dvo_timing_offset;
+        u8 dvo_table_size;
+        u16 panel_pnp_id_offset;
+        u8 pnp_table_size;
+} __attribute__((packed));
+
+struct bdb_lvds_lfp_data_ptrs {
+        u8 lvds_entries; /* followed by one or more lvds_data_ptr structs */
+        struct bdb_lvds_lfp_data_ptr ptr[16];
+} __attribute__((packed));
+
+/* LFP data has 3 blocks per entry */
+struct lvds_fp_timing {
+        u16 x_res;
+        u16 y_res;
+        u32 lvds_reg;
+        u32 lvds_reg_val;
+        u32 pp_on_reg;
+        u32 pp_on_reg_val;
+        u32 pp_off_reg;
+        u32 pp_off_reg_val;
+        u32 pp_cycle_reg;
+        u32 pp_cycle_reg_val;
+        u32 pfit_reg;
+        u32 pfit_reg_val;
+        u16 terminator;
+} __attribute__((packed));
+
+struct lvds_dvo_timing {
+        u16 clock;              /**< In 10khz */
+        u8 hactive_lo;
+        u8 hblank_lo;
+        u8 hblank_hi:4;
+        u8 hactive_hi:4;
+        u8 vactive_lo;
+        u8 vblank_lo;
+        u8 vblank_hi:4;
+        u8 vactive_hi:4;
+        u8 hsync_off_lo;
+        u8 hsync_pulse_width;
+        u8 vsync_pulse_width:4;
+        u8 vsync_off:4;
+        u8 rsvd0:6;
+        u8 hsync_off_hi:2;
+        u8 h_image;
+        u8 v_image;
+        u8 max_hv;
+        u8 h_border;
+        u8 v_border;
+        u8 rsvd1:3;
+        u8 digital:2;
+        u8 vsync_positive:1;
+        u8 hsync_positive:1;
+        u8 rsvd2:1;
+} __attribute__((packed));
+
+struct lvds_pnp_id {
+        u16 mfg_name;
+        u16 product_code;
+        u32 serial;
+        u8 mfg_week;
+        u8 mfg_year;
+} __attribute__((packed));
+
+struct bdb_lvds_lfp_data_entry {
+        struct lvds_fp_timing fp_timing;
+        struct lvds_dvo_timing dvo_timing;
+        struct lvds_pnp_id pnp_id;
+} __attribute__((packed));
+
+struct bdb_lvds_lfp_data {
+        struct bdb_lvds_lfp_data_entry data[16];
+} __attribute__((packed));
+
+struct aimdb_header {
+        char signature[16];
+        char oem_device[20];
+        u16 aimdb_version;
+        u16 aimdb_header_size;
+        u16 aimdb_size;
+} __attribute__((packed));
+
+struct aimdb_block {
+        u8 aimdb_id;
+        u16 aimdb_size;
+} __attribute__((packed));
+
+struct vch_panel_data {
+        u16 fp_timing_offset;
+        u8 fp_timing_size;
+        u16 dvo_timing_offset;
+        u8 dvo_timing_size;
+        u16 text_fitting_offset;
+        u8 text_fitting_size;
+        u16 graphics_fitting_offset;
+        u8 graphics_fitting_size;
+} __attribute__((packed));
+
+struct vch_bdb_22 {
+        struct aimdb_block aimdb_block;
+        struct vch_panel_data panels[16];
+} __attribute__((packed));
+
+struct bdb_sdvo_lvds_options {
+        u8 panel_backlight;
+        u8 h40_set_panel_type;
+        u8 panel_type;
+        u8 ssc_clk_freq;
+        u16 als_low_trip;
+        u16 als_high_trip;
+        u8 sclalarcoeff_tab_row_num;
+        u8 sclalarcoeff_tab_row_size;
+        u8 coefficient[8];
+        u8 panel_misc_bits_1;
+        u8 panel_misc_bits_2;
+        u8 panel_misc_bits_3;
+        u8 panel_misc_bits_4;
+} __attribute__((packed));
+
+
+extern bool psb_intel_init_bios(struct drm_device *dev);
+extern void psb_intel_destroy_bios(struct drm_device *dev);
+
+/*
+ * Driver<->VBIOS interaction occurs through scratch bits in
+ * GR18 & SWF*.
+ */
+
+/* GR18 bits are set on display switch and hotkey events */
+#define GR18_DRIVER_SWITCH_EN   (1<<7) /* 0: VBIOS control, 1: driver control */
+#define GR18_HOTKEY_MASK        0x78 /* See also SWF4 15:0 */
+#define   GR18_HK_NONE          (0x0<<3)
+#define   GR18_HK_LFP_STRETCH   (0x1<<3)
+#define   GR18_HK_TOGGLE_DISP   (0x2<<3)
+#define   GR18_HK_DISP_SWITCH   (0x4<<3) /* see SWF14 15:0 for what to enable */
+#define   GR18_HK_POPUP_DISABLED (0x6<<3)
+#define   GR18_HK_POPUP_ENABLED (0x7<<3)
+#define   GR18_HK_PFIT          (0x8<<3)
+#define   GR18_HK_APM_CHANGE    (0xa<<3)
+#define   GR18_HK_MULTIPLE      (0xc<<3)
+#define GR18_USER_INT_EN        (1<<2)
+#define GR18_A0000_FLUSH_EN     (1<<1)
+#define GR18_SMM_EN             (1<<0)
+
+/* Set by driver, cleared by VBIOS */
+#define SWF00_YRES_SHIFT        16
+#define SWF00_XRES_SHIFT        0
+#define SWF00_RES_MASK          0xffff
+
+/* Set by VBIOS at boot time and driver at runtime */
+#define SWF01_TV2_FORMAT_SHIFT  8
+#define SWF01_TV1_FORMAT_SHIFT  0
+#define SWF01_TV_FORMAT_MASK    0xffff
+
+#define SWF10_VBIOS_BLC_I2C_EN  (1<<29)
+#define SWF10_GTT_OVERRIDE_EN   (1<<28)
+#define SWF10_LFP_DPMS_OVR      (1<<27) /* override DPMS on display switch */
+#define SWF10_ACTIVE_TOGGLE_LIST_MASK (7<<24)
+#define   SWF10_OLD_TOGGLE      0x0
+#define   SWF10_TOGGLE_LIST_1   0x1
+#define   SWF10_TOGGLE_LIST_2   0x2
+#define   SWF10_TOGGLE_LIST_3   0x3
+#define   SWF10_TOGGLE_LIST_4   0x4
+#define SWF10_PANNING_EN        (1<<23)
+#define SWF10_DRIVER_LOADED     (1<<22)
+#define SWF10_EXTENDED_DESKTOP  (1<<21)
+#define SWF10_EXCLUSIVE_MODE    (1<<20)
+#define SWF10_OVERLAY_EN        (1<<19)
+#define SWF10_PLANEB_HOLDOFF    (1<<18)
+#define SWF10_PLANEA_HOLDOFF    (1<<17)
+#define SWF10_VGA_HOLDOFF       (1<<16)
+#define SWF10_ACTIVE_DISP_MASK  0xffff
+#define   SWF10_PIPEB_LFP2      (1<<15)
+#define   SWF10_PIPEB_EFP2      (1<<14)
+#define   SWF10_PIPEB_TV2       (1<<13)
+#define   SWF10_PIPEB_CRT2      (1<<12)
+#define   SWF10_PIPEB_LFP       (1<<11)
+#define   SWF10_PIPEB_EFP       (1<<10)
+#define   SWF10_PIPEB_TV        (1<<9)
+#define   SWF10_PIPEB_CRT       (1<<8)
+#define   SWF10_PIPEA_LFP2      (1<<7)
+#define   SWF10_PIPEA_EFP2      (1<<6)
+#define   SWF10_PIPEA_TV2       (1<<5)
+#define   SWF10_PIPEA_CRT2      (1<<4)
+#define   SWF10_PIPEA_LFP       (1<<3)
+#define   SWF10_PIPEA_EFP       (1<<2)
+#define   SWF10_PIPEA_TV        (1<<1)
+#define   SWF10_PIPEA_CRT       (1<<0)
+
+#define SWF11_MEMORY_SIZE_SHIFT 16
+#define SWF11_SV_TEST_EN        (1<<15)
+#define SWF11_IS_AGP            (1<<14)
+#define SWF11_DISPLAY_HOLDOFF   (1<<13)
+#define SWF11_DPMS_REDUCED      (1<<12)
+#define SWF11_IS_VBE_MODE       (1<<11)
+#define SWF11_PIPEB_ACCESS      (1<<10) /* 0 here means pipe a */
+#define SWF11_DPMS_MASK         0x07
+#define   SWF11_DPMS_OFF        (1<<2)
+#define   SWF11_DPMS_SUSPEND    (1<<1)
+#define   SWF11_DPMS_STANDBY    (1<<0)
+#define   SWF11_DPMS_ON         0
+
+#define SWF14_GFX_PFIT_EN       (1<<31)
+#define SWF14_TEXT_PFIT_EN      (1<<30)
+#define SWF14_LID_STATUS_CLOSED (1<<29) /* 0 here means open */
+#define SWF14_POPUP_EN          (1<<28)
+#define SWF14_DISPLAY_HOLDOFF   (1<<27)
+#define SWF14_DISP_DETECT_EN    (1<<26)
+#define SWF14_DOCKING_STATUS_DOCKED (1<<25) /* 0 here means undocked */
+#define SWF14_DRIVER_STATUS     (1<<24)
+#define SWF14_OS_TYPE_WIN9X     (1<<23)
+#define SWF14_OS_TYPE_WINNT     (1<<22)
+/* 21:19 rsvd */
+#define SWF14_PM_TYPE_MASK      0x00070000
+#define   SWF14_PM_ACPI_VIDEO   (0x4 << 16)
+#define   SWF14_PM_ACPI         (0x3 << 16)
+#define   SWF14_PM_APM_12       (0x2 << 16)
+#define   SWF14_PM_APM_11       (0x1 << 16)
+#define SWF14_HK_REQUEST_MASK   0x0000ffff /* see GR18 6:3 for event type */
+          /* if GR18 indicates a display switch */
+#define   SWF14_DS_PIPEB_LFP2_EN (1<<15)
+#define   SWF14_DS_PIPEB_EFP2_EN (1<<14)
+#define   SWF14_DS_PIPEB_TV2_EN  (1<<13)
+#define   SWF14_DS_PIPEB_CRT2_EN (1<<12)
+#define   SWF14_DS_PIPEB_LFP_EN  (1<<11)
+#define   SWF14_DS_PIPEB_EFP_EN  (1<<10)
+#define   SWF14_DS_PIPEB_TV_EN   (1<<9)
+#define   SWF14_DS_PIPEB_CRT_EN  (1<<8)
+#define   SWF14_DS_PIPEA_LFP2_EN (1<<7)
+#define   SWF14_DS_PIPEA_EFP2_EN (1<<6)
+#define   SWF14_DS_PIPEA_TV2_EN  (1<<5)
+#define   SWF14_DS_PIPEA_CRT2_EN (1<<4)
+#define   SWF14_DS_PIPEA_LFP_EN  (1<<3)
+#define   SWF14_DS_PIPEA_EFP_EN  (1<<2)
+#define   SWF14_DS_PIPEA_TV_EN   (1<<1)
+#define   SWF14_DS_PIPEA_CRT_EN  (1<<0)
+          /* if GR18 indicates a panel fitting request */
+#define   SWF14_PFIT_EN         (1<<0) /* 0 means disable */
+          /* if GR18 indicates an APM change request */
+#define   SWF14_APM_HIBERNATE   0x4
+#define   SWF14_APM_SUSPEND     0x3
+#define   SWF14_APM_STANDBY     0x1
+#define   SWF14_APM_RESTORE     0x0
+
+#endif /* _I830_BIOS_H_ */
diff --git a/drivers/staging/gma500/intel_i2c.c b/drivers/staging/gma500/intel_i2c.c
new file mode 100644
index 00000000000..e33432df510
--- /dev/null
+++ b/drivers/staging/gma500/intel_i2c.c
@@ -0,0 +1,169 @@
+/*
+ * Copyright © 2006-2007 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors:
+ *      Eric Anholt <eric@anholt.net>
+ */
+
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+
+#include "psb_drv.h"
+#include "psb_intel_reg.h"
+
+/*
+ * Intel GPIO access functions
+ */
+
+#define I2C_RISEFALL_TIME 20
+
+static int get_clock(void *data)
+{
+        struct psb_intel_i2c_chan *chan = data;
+        struct drm_device *dev = chan->drm_dev;
+        u32 val;
+
+        val = REG_READ(chan->reg);
+        return (val & GPIO_CLOCK_VAL_IN) != 0;
+}
+
+static int get_data(void *data)
+{
+        struct psb_intel_i2c_chan *chan = data;
+        struct drm_device *dev = chan->drm_dev;
+        u32 val;
+
+        val = REG_READ(chan->reg);
+        return (val & GPIO_DATA_VAL_IN) != 0;
+}
+
+static void set_clock(void *data, int state_high)
+{
+        struct psb_intel_i2c_chan *chan = data;
+        struct drm_device *dev = chan->drm_dev;
+        u32 reserved = 0, clock_bits;
+
+        /* On most chips, these bits must be preserved in software. */
+        reserved =
+                    REG_READ(chan->reg) & (GPIO_DATA_PULLUP_DISABLE |
+                                           GPIO_CLOCK_PULLUP_DISABLE);
+
+        if (state_high)
+                clock_bits = GPIO_CLOCK_DIR_IN | GPIO_CLOCK_DIR_MASK;
+        else
+                clock_bits = GPIO_CLOCK_DIR_OUT | GPIO_CLOCK_DIR_MASK |
+                    GPIO_CLOCK_VAL_MASK;
+        REG_WRITE(chan->reg, reserved | clock_bits);
+        udelay(I2C_RISEFALL_TIME);      /* wait for the line to change state */
+}
+
+static void set_data(void *data, int state_high)
+{
+        struct psb_intel_i2c_chan *chan = data;
+        struct drm_device *dev = chan->drm_dev;
+        u32 reserved = 0, data_bits;
+
+        /* On most chips, these bits must be preserved in software. */
+        reserved =
+                    REG_READ(chan->reg) & (GPIO_DATA_PULLUP_DISABLE |
+                                           GPIO_CLOCK_PULLUP_DISABLE);
+
+        if (state_high)
+                data_bits = GPIO_DATA_DIR_IN | GPIO_DATA_DIR_MASK;
+        else
+                data_bits =
+                    GPIO_DATA_DIR_OUT | GPIO_DATA_DIR_MASK |
+                    GPIO_DATA_VAL_MASK;
+
+        REG_WRITE(chan->reg, reserved | data_bits);
+        udelay(I2C_RISEFALL_TIME);      /* wait for the line to change state */
+}
+
+/**
+ * psb_intel_i2c_create - instantiate an Intel i2c bus using the specified GPIO reg
+ * @dev: DRM device
+ * @output: driver specific output device
+ * @reg: GPIO reg to use
+ * @name: name for this bus
+ *
+ * Creates and registers a new i2c bus with the Linux i2c layer, for use
+ * in output probing and control (e.g. DDC or SDVO control functions).
+ *
+ * Possible values for @reg include:
+ *   %GPIOA
+ *   %GPIOB
+ *   %GPIOC
+ *   %GPIOD
+ *   %GPIOE
+ *   %GPIOF
+ *   %GPIOG
+ *   %GPIOH
+ * see PRM for details on how these different busses are used.
+ */
+struct psb_intel_i2c_chan *psb_intel_i2c_create(struct drm_device *dev,
+                                        const u32 reg, const char *name)
+{
+        struct psb_intel_i2c_chan *chan;
+
+        chan = kzalloc(sizeof(struct psb_intel_i2c_chan), GFP_KERNEL);
+        if (!chan)
+                goto out_free;
+
+        chan->drm_dev = dev;
+        chan->reg = reg;
+        snprintf(chan->adapter.name, I2C_NAME_SIZE, "intel drm %s", name);
+        chan->adapter.owner = THIS_MODULE;
+        chan->adapter.algo_data = &chan->algo;
+        chan->adapter.dev.parent = &dev->pdev->dev;
+        chan->algo.setsda = set_data;
+        chan->algo.setscl = set_clock;
+        chan->algo.getsda = get_data;
+        chan->algo.getscl = get_clock;
+        chan->algo.udelay = 20;
+        chan->algo.timeout = usecs_to_jiffies(2200);
+        chan->algo.data = chan;
+
+        i2c_set_adapdata(&chan->adapter, chan);
+
+        if (i2c_bit_add_bus(&chan->adapter))
+                goto out_free;
+
+        /* JJJ:  raise SCL and SDA? */
+        set_data(chan, 1);
+        set_clock(chan, 1);
+        udelay(20);
+
+        return chan;
+
+out_free:
+        kfree(chan);
+        return NULL;
+}
+
+/**
+ * psb_intel_i2c_destroy - unregister and free i2c bus resources
+ * @output: channel to free
+ *
+ * Unregister the adapter from the i2c layer, then free the structure.
+ */
+void psb_intel_i2c_destroy(struct psb_intel_i2c_chan *chan)
+{
+        if (!chan)
+                return;
+
+        i2c_del_adapter(&chan->adapter);
+        kfree(chan);
+}
diff --git a/drivers/staging/gma500/intel_opregion.c b/drivers/staging/gma500/intel_opregion.c
new file mode 100644
index 00000000000..d2e60376982
--- /dev/null
+++ b/drivers/staging/gma500/intel_opregion.c
@@ -0,0 +1,80 @@
+/*
+ * Copyright 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.
+ *
+ */
+
+#include "psb_drv.h"
+
+struct opregion_header {
+        u8 signature[16];
+        u32 size;
+        u32 opregion_ver;
+        u8 bios_ver[32];
+        u8 vbios_ver[16];
+        u8 driver_ver[16];
+        u32 mboxes;
+        u8 reserved[164];
+} __packed;
+
+struct opregion_apci {
+        /*FIXME: add it later*/
+} __packed;
+
+struct opregion_swsci {
+        /*FIXME: add it later*/
+} __packed;
+
+struct opregion_acpi {
+        /*FIXME: add it later*/
+} __packed;
+
+int gma_intel_opregion_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 opregion_phy;
+        void *base;
+        u32 *lid_state;
+
+        dev_priv->lid_state = NULL;
+
+        pci_read_config_dword(dev->pdev, 0xfc, &opregion_phy);
+        if (opregion_phy == 0)
+                return -ENOTSUPP;
+
+        base = ioremap(opregion_phy, 8*1024);
+        if (!base)
+                return -ENOMEM;
+
+        lid_state = base + 0x01ac;
+
+        dev_priv->lid_state = lid_state;
+        dev_priv->lid_last_state = readl(lid_state);
+        return 0;
+}
+
+int gma_intel_opregion_exit(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        if (dev_priv->lid_state)
+                iounmap(dev_priv->lid_state);
+        return 0;
+}
diff --git a/drivers/staging/gma500/mdfld_device.c b/drivers/staging/gma500/mdfld_device.c
new file mode 100644
index 00000000000..f47aeb7a203
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_device.c
@@ -0,0 +1,714 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+#include <linux/backlight.h>
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include "psb_drm.h"
+#include "psb_drv.h"
+#include "mdfld_output.h"
+#include "mdfld_dsi_output.h"
+#include "mid_bios.h"
+
+/*
+ *      Provide the Medfield specific backlight management
+ */
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+
+static int mdfld_brightness;
+struct backlight_device *mdfld_backlight_device;
+
+static int mfld_set_brightness(struct backlight_device *bd)
+{
+        struct drm_device *dev = bl_get_data(mdfld_backlight_device);
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int level = bd->props.brightness;
+
+        /* Percentage 1-100% being valid */
+        if (level < 1)
+                level = 1;
+
+        if (gma_power_begin(dev, 0)) {
+                /* Calculate and set the brightness value */
+                u32 adjusted_level;
+
+                /* Adjust the backlight level with the percent in
+                 * dev_priv->blc_adj2;
+                 */
+                adjusted_level = level * dev_priv->blc_adj2;
+                adjusted_level = adjusted_level / 100;
+#if 0
+#ifndef CONFIG_MDFLD_DSI_DPU
+                if(!(dev_priv->dsr_fb_update & MDFLD_DSR_MIPI_CONTROL) && 
+                        (dev_priv->dbi_panel_on || dev_priv->dbi_panel_on2)){
+                        mdfld_dsi_dbi_exit_dsr(dev,MDFLD_DSR_MIPI_CONTROL, 0, 0);
+                        dev_dbg(dev->dev, "Out of DSR before set brightness to %d.\n",adjusted_level);
+                }
+#endif
+                mdfld_dsi_brightness_control(dev, 0, adjusted_level);
+
+                if ((dev_priv->dbi_panel_on2) || (dev_priv->dpi_panel_on2))
+                        mdfld_dsi_brightness_control(dev, 2, adjusted_level);
+#endif
+                gma_power_end(dev);
+        }
+        mdfld_brightness = level;
+        return 0;
+}
+
+int psb_get_brightness(struct backlight_device *bd)
+{
+        /* return locally cached var instead of HW read (due to DPST etc.) */
+        /* FIXME: ideally return actual value in case firmware fiddled with
+           it */
+        return mdfld_brightness;
+}
+
+static const struct backlight_ops mfld_ops = {
+        .get_brightness = psb_get_brightness,
+        .update_status  = mfld_set_brightness,
+};
+
+static int mdfld_backlight_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct backlight_properties props;
+        memset(&props, 0, sizeof(struct backlight_properties));
+        props.max_brightness = 100;
+        props.type = BACKLIGHT_PLATFORM;
+
+        mdfld_backlight_device = backlight_device_register("mfld-bl",
+                                        NULL, (void *)dev, &mfld_ops, &props);
+                                        
+        if (IS_ERR(mdfld_backlight_device))
+                return PTR_ERR(mdfld_backlight_device);
+
+        dev_priv->blc_adj1 = 100;
+        dev_priv->blc_adj2 = 100;
+        mdfld_backlight_device->props.brightness = 100;
+        mdfld_backlight_device->props.max_brightness = 100;
+        backlight_update_status(mdfld_backlight_device);
+        dev_priv->backlight_device = mdfld_backlight_device;
+        return 0;
+}
+
+#endif
+
+/*
+ *      Provide the Medfield specific chip logic and low level methods for
+ *      power management.
+ */
+
+static void mdfld_init_pm(struct drm_device *dev)
+{
+        /* No work needed here yet */
+}
+
+/**
+ * mdfld_save_display_registers -       save registers for pipe
+ * @dev: our device
+ * @pipe: pipe to save
+ *
+ * Save the pipe state of the device before we power it off. Keep everything
+ * we need to put it back again
+ */
+static int mdfld_save_display_registers(struct drm_device *dev, int pipe)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int i;
+
+        /* register */
+        u32 dpll_reg = MRST_DPLL_A;
+        u32 fp_reg = MRST_FPA0;
+        u32 pipeconf_reg = PIPEACONF;
+        u32 htot_reg = HTOTAL_A;
+        u32 hblank_reg = HBLANK_A;
+        u32 hsync_reg = HSYNC_A;
+        u32 vtot_reg = VTOTAL_A;
+        u32 vblank_reg = VBLANK_A;
+        u32 vsync_reg = VSYNC_A;
+        u32 pipesrc_reg = PIPEASRC;
+        u32 dspstride_reg = DSPASTRIDE;
+        u32 dsplinoff_reg = DSPALINOFF;
+        u32 dsptileoff_reg = DSPATILEOFF;
+        u32 dspsize_reg = DSPASIZE;
+        u32 dsppos_reg = DSPAPOS;
+        u32 dspsurf_reg = DSPASURF;
+        u32 mipi_reg = MIPI;
+        u32 dspcntr_reg = DSPACNTR;
+        u32 dspstatus_reg = PIPEASTAT;
+        u32 palette_reg = PALETTE_A;
+
+        /* pointer to values */
+        u32 *dpll_val = &dev_priv->saveDPLL_A;
+        u32 *fp_val = &dev_priv->saveFPA0;
+        u32 *pipeconf_val = &dev_priv->savePIPEACONF;
+        u32 *htot_val = &dev_priv->saveHTOTAL_A;
+        u32 *hblank_val = &dev_priv->saveHBLANK_A;
+        u32 *hsync_val = &dev_priv->saveHSYNC_A;
+        u32 *vtot_val = &dev_priv->saveVTOTAL_A;
+        u32 *vblank_val = &dev_priv->saveVBLANK_A;
+        u32 *vsync_val = &dev_priv->saveVSYNC_A;
+        u32 *pipesrc_val = &dev_priv->savePIPEASRC;
+        u32 *dspstride_val = &dev_priv->saveDSPASTRIDE;
+        u32 *dsplinoff_val = &dev_priv->saveDSPALINOFF;
+        u32 *dsptileoff_val = &dev_priv->saveDSPATILEOFF;
+        u32 *dspsize_val = &dev_priv->saveDSPASIZE;
+        u32 *dsppos_val = &dev_priv->saveDSPAPOS;
+        u32 *dspsurf_val = &dev_priv->saveDSPASURF;
+        u32 *mipi_val = &dev_priv->saveMIPI;
+        u32 *dspcntr_val = &dev_priv->saveDSPACNTR;
+        u32 *dspstatus_val = &dev_priv->saveDSPASTATUS;
+        u32 *palette_val = dev_priv->save_palette_a;
+
+        switch (pipe) {
+        case 0:
+                break;
+        case 1:
+                /* register */
+                dpll_reg = MDFLD_DPLL_B;
+                fp_reg = MDFLD_DPLL_DIV0;
+                pipeconf_reg = PIPEBCONF;
+                htot_reg = HTOTAL_B;
+                hblank_reg = HBLANK_B;
+                hsync_reg = HSYNC_B;
+                vtot_reg = VTOTAL_B;
+                vblank_reg = VBLANK_B;
+                vsync_reg = VSYNC_B;
+                pipesrc_reg = PIPEBSRC;
+                dspstride_reg = DSPBSTRIDE;
+                dsplinoff_reg = DSPBLINOFF;
+                dsptileoff_reg = DSPBTILEOFF;
+                dspsize_reg = DSPBSIZE;
+                dsppos_reg = DSPBPOS;
+                dspsurf_reg = DSPBSURF;
+                dspcntr_reg = DSPBCNTR;
+                dspstatus_reg = PIPEBSTAT;
+                palette_reg = PALETTE_B;
+
+                /* values */
+                dpll_val = &dev_priv->saveDPLL_B;
+                fp_val = &dev_priv->saveFPB0;
+                pipeconf_val = &dev_priv->savePIPEBCONF;
+                htot_val = &dev_priv->saveHTOTAL_B;
+                hblank_val = &dev_priv->saveHBLANK_B;
+                hsync_val = &dev_priv->saveHSYNC_B;
+                vtot_val = &dev_priv->saveVTOTAL_B;
+                vblank_val = &dev_priv->saveVBLANK_B;
+                vsync_val = &dev_priv->saveVSYNC_B;
+                pipesrc_val = &dev_priv->savePIPEBSRC;
+                dspstride_val = &dev_priv->saveDSPBSTRIDE;
+                dsplinoff_val = &dev_priv->saveDSPBLINOFF;
+                dsptileoff_val = &dev_priv->saveDSPBTILEOFF;
+                dspsize_val = &dev_priv->saveDSPBSIZE;
+                dsppos_val = &dev_priv->saveDSPBPOS;
+                dspsurf_val = &dev_priv->saveDSPBSURF;
+                dspcntr_val = &dev_priv->saveDSPBCNTR;
+                dspstatus_val = &dev_priv->saveDSPBSTATUS;
+                palette_val = dev_priv->save_palette_b;
+                break;
+        case 2:
+                /* register */
+                pipeconf_reg = PIPECCONF;
+                htot_reg = HTOTAL_C;
+                hblank_reg = HBLANK_C;
+                hsync_reg = HSYNC_C;
+                vtot_reg = VTOTAL_C;
+                vblank_reg = VBLANK_C;
+                vsync_reg = VSYNC_C;
+                pipesrc_reg = PIPECSRC;
+                dspstride_reg = DSPCSTRIDE;
+                dsplinoff_reg = DSPCLINOFF;
+                dsptileoff_reg = DSPCTILEOFF;
+                dspsize_reg = DSPCSIZE;
+                dsppos_reg = DSPCPOS;
+                dspsurf_reg = DSPCSURF;
+                mipi_reg = MIPI_C;
+                dspcntr_reg = DSPCCNTR;
+                dspstatus_reg = PIPECSTAT;
+                palette_reg = PALETTE_C;
+
+                /* pointer to values */
+                pipeconf_val = &dev_priv->savePIPECCONF;
+                htot_val = &dev_priv->saveHTOTAL_C;
+                hblank_val = &dev_priv->saveHBLANK_C;
+                hsync_val = &dev_priv->saveHSYNC_C;
+                vtot_val = &dev_priv->saveVTOTAL_C;
+                vblank_val = &dev_priv->saveVBLANK_C;
+                vsync_val = &dev_priv->saveVSYNC_C;
+                pipesrc_val = &dev_priv->savePIPECSRC;
+                dspstride_val = &dev_priv->saveDSPCSTRIDE;
+                dsplinoff_val = &dev_priv->saveDSPCLINOFF;
+                dsptileoff_val = &dev_priv->saveDSPCTILEOFF;
+                dspsize_val = &dev_priv->saveDSPCSIZE;
+                dsppos_val = &dev_priv->saveDSPCPOS;
+                dspsurf_val = &dev_priv->saveDSPCSURF;
+                mipi_val = &dev_priv->saveMIPI_C;
+                dspcntr_val = &dev_priv->saveDSPCCNTR;
+                dspstatus_val = &dev_priv->saveDSPCSTATUS;
+                palette_val = dev_priv->save_palette_c;
+                break;
+        default:
+                DRM_ERROR("%s, invalid pipe number.\n", __func__);
+                return -EINVAL;
+        }
+
+        /* Pipe & plane A info */
+        *dpll_val = PSB_RVDC32(dpll_reg);
+        *fp_val = PSB_RVDC32(fp_reg);
+        *pipeconf_val = PSB_RVDC32(pipeconf_reg);
+        *htot_val = PSB_RVDC32(htot_reg);
+        *hblank_val = PSB_RVDC32(hblank_reg);
+        *hsync_val = PSB_RVDC32(hsync_reg);
+        *vtot_val = PSB_RVDC32(vtot_reg);
+        *vblank_val = PSB_RVDC32(vblank_reg);
+        *vsync_val = PSB_RVDC32(vsync_reg);
+        *pipesrc_val = PSB_RVDC32(pipesrc_reg);
+        *dspstride_val = PSB_RVDC32(dspstride_reg);
+        *dsplinoff_val = PSB_RVDC32(dsplinoff_reg);
+        *dsptileoff_val = PSB_RVDC32(dsptileoff_reg);
+        *dspsize_val = PSB_RVDC32(dspsize_reg);
+        *dsppos_val = PSB_RVDC32(dsppos_reg);
+        *dspsurf_val = PSB_RVDC32(dspsurf_reg);
+        *dspcntr_val = PSB_RVDC32(dspcntr_reg);
+        *dspstatus_val = PSB_RVDC32(dspstatus_reg);
+
+        /*save palette (gamma) */
+        for (i = 0; i < 256; i++)
+                palette_val[i] = PSB_RVDC32(palette_reg + (i<<2));
+
+        if (pipe == 1) {
+                dev_priv->savePFIT_CONTROL = PSB_RVDC32(PFIT_CONTROL);
+                dev_priv->savePFIT_PGM_RATIOS = PSB_RVDC32(PFIT_PGM_RATIOS);
+                dev_priv->saveHDMIPHYMISCCTL = PSB_RVDC32(HDMIPHYMISCCTL);
+                dev_priv->saveHDMIB_CONTROL = PSB_RVDC32(HDMIB_CONTROL);
+                return 0;
+        }
+        *mipi_val = PSB_RVDC32(mipi_reg);
+        return 0;
+}
+
+/**
+ * mdfld_save_cursor_overlay_registers  -       save cursor overlay info
+ * @dev: our device
+ *
+ * Save the cursor and overlay register state
+ */
+static int mdfld_save_cursor_overlay_registers(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        /* Save cursor regs */
+        dev_priv->saveDSPACURSOR_CTRL = PSB_RVDC32(CURACNTR);
+        dev_priv->saveDSPACURSOR_BASE = PSB_RVDC32(CURABASE);
+        dev_priv->saveDSPACURSOR_POS = PSB_RVDC32(CURAPOS);
+
+        dev_priv->saveDSPBCURSOR_CTRL = PSB_RVDC32(CURBCNTR);
+        dev_priv->saveDSPBCURSOR_BASE = PSB_RVDC32(CURBBASE);
+        dev_priv->saveDSPBCURSOR_POS = PSB_RVDC32(CURBPOS);
+
+        dev_priv->saveDSPCCURSOR_CTRL = PSB_RVDC32(CURCCNTR);
+        dev_priv->saveDSPCCURSOR_BASE = PSB_RVDC32(CURCBASE);
+        dev_priv->saveDSPCCURSOR_POS = PSB_RVDC32(CURCPOS);
+
+        /* HW overlay */
+        dev_priv->saveOV_OVADD = PSB_RVDC32(OV_OVADD);
+        dev_priv->saveOV_OGAMC0 = PSB_RVDC32(OV_OGAMC0);
+        dev_priv->saveOV_OGAMC1 = PSB_RVDC32(OV_OGAMC1);
+        dev_priv->saveOV_OGAMC2 = PSB_RVDC32(OV_OGAMC2);
+        dev_priv->saveOV_OGAMC3 = PSB_RVDC32(OV_OGAMC3);
+        dev_priv->saveOV_OGAMC4 = PSB_RVDC32(OV_OGAMC4);
+        dev_priv->saveOV_OGAMC5 = PSB_RVDC32(OV_OGAMC5);
+
+        dev_priv->saveOV_OVADD_C = PSB_RVDC32(OV_OVADD + OV_C_OFFSET);
+        dev_priv->saveOV_OGAMC0_C = PSB_RVDC32(OV_OGAMC0 + OV_C_OFFSET);
+        dev_priv->saveOV_OGAMC1_C = PSB_RVDC32(OV_OGAMC1 + OV_C_OFFSET);
+        dev_priv->saveOV_OGAMC2_C = PSB_RVDC32(OV_OGAMC2 + OV_C_OFFSET);
+        dev_priv->saveOV_OGAMC3_C = PSB_RVDC32(OV_OGAMC3 + OV_C_OFFSET);
+        dev_priv->saveOV_OGAMC4_C = PSB_RVDC32(OV_OGAMC4 + OV_C_OFFSET);
+        dev_priv->saveOV_OGAMC5_C = PSB_RVDC32(OV_OGAMC5 + OV_C_OFFSET);
+
+        return 0;
+}
+/*
+ * mdfld_restore_display_registers      -       restore the state of a pipe
+ * @dev: our device
+ * @pipe: the pipe to restore
+ *
+ * Restore the state of a pipe to that which was saved by the register save
+ * functions.
+ */
+static int mdfld_restore_display_registers(struct drm_device *dev, int pipe)
+{
+        /* To get  panel out of ULPS mode */
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dsi_config *dsi_config = NULL;
+        u32 i = 0;
+        u32 dpll = 0;
+        u32 timeout = 0;
+        u32 reg_offset = 0;
+
+        /* register */
+        u32 dpll_reg = MRST_DPLL_A;
+        u32 fp_reg = MRST_FPA0;
+        u32 pipeconf_reg = PIPEACONF;
+        u32 htot_reg = HTOTAL_A;
+        u32 hblank_reg = HBLANK_A;
+        u32 hsync_reg = HSYNC_A;
+        u32 vtot_reg = VTOTAL_A;
+        u32 vblank_reg = VBLANK_A;
+        u32 vsync_reg = VSYNC_A;
+        u32 pipesrc_reg = PIPEASRC;
+        u32 dspstride_reg = DSPASTRIDE;
+        u32 dsplinoff_reg = DSPALINOFF;
+        u32 dsptileoff_reg = DSPATILEOFF;
+        u32 dspsize_reg = DSPASIZE;
+        u32 dsppos_reg = DSPAPOS;
+        u32 dspsurf_reg = DSPASURF;
+        u32 dspstatus_reg = PIPEASTAT;
+        u32 mipi_reg = MIPI;
+        u32 dspcntr_reg = DSPACNTR;
+        u32 palette_reg = PALETTE_A;
+
+        /* values */
+        u32 dpll_val = dev_priv->saveDPLL_A & ~DPLL_VCO_ENABLE;
+        u32 fp_val = dev_priv->saveFPA0;
+        u32 pipeconf_val = dev_priv->savePIPEACONF;
+        u32 htot_val = dev_priv->saveHTOTAL_A;
+        u32 hblank_val = dev_priv->saveHBLANK_A;
+        u32 hsync_val = dev_priv->saveHSYNC_A;
+        u32 vtot_val = dev_priv->saveVTOTAL_A;
+        u32 vblank_val = dev_priv->saveVBLANK_A;
+        u32 vsync_val = dev_priv->saveVSYNC_A;
+        u32 pipesrc_val = dev_priv->savePIPEASRC;
+        u32 dspstride_val = dev_priv->saveDSPASTRIDE;
+        u32 dsplinoff_val = dev_priv->saveDSPALINOFF;
+        u32 dsptileoff_val = dev_priv->saveDSPATILEOFF;
+        u32 dspsize_val = dev_priv->saveDSPASIZE;
+        u32 dsppos_val = dev_priv->saveDSPAPOS;
+        u32 dspsurf_val = dev_priv->saveDSPASURF;
+        u32 dspstatus_val = dev_priv->saveDSPASTATUS;
+        u32 mipi_val = dev_priv->saveMIPI;
+        u32 dspcntr_val = dev_priv->saveDSPACNTR;
+        u32 *palette_val = dev_priv->save_palette_a;
+
+        switch (pipe) {
+        case 0:
+                dsi_config = dev_priv->dsi_configs[0];
+                break;
+        case 1:
+                /* register */
+                dpll_reg = MDFLD_DPLL_B;
+                fp_reg = MDFLD_DPLL_DIV0;
+                pipeconf_reg = PIPEBCONF;
+                htot_reg = HTOTAL_B;
+                hblank_reg = HBLANK_B;
+                hsync_reg = HSYNC_B;
+                vtot_reg = VTOTAL_B;
+                vblank_reg = VBLANK_B;
+                vsync_reg = VSYNC_B;
+                pipesrc_reg = PIPEBSRC;
+                dspstride_reg = DSPBSTRIDE;
+                dsplinoff_reg = DSPBLINOFF;
+                dsptileoff_reg = DSPBTILEOFF;
+                dspsize_reg = DSPBSIZE;
+                dsppos_reg = DSPBPOS;
+                dspsurf_reg = DSPBSURF;
+                dspcntr_reg = DSPBCNTR;
+                palette_reg = PALETTE_B;
+                dspstatus_reg = PIPEBSTAT;
+
+                /* values */
+                dpll_val = dev_priv->saveDPLL_B & ~DPLL_VCO_ENABLE;
+                fp_val = dev_priv->saveFPB0;
+                pipeconf_val = dev_priv->savePIPEBCONF;
+                htot_val = dev_priv->saveHTOTAL_B;
+                hblank_val = dev_priv->saveHBLANK_B;
+                hsync_val = dev_priv->saveHSYNC_B;
+                vtot_val = dev_priv->saveVTOTAL_B;
+                vblank_val = dev_priv->saveVBLANK_B;
+                vsync_val = dev_priv->saveVSYNC_B;
+                pipesrc_val = dev_priv->savePIPEBSRC;
+                dspstride_val = dev_priv->saveDSPBSTRIDE;
+                dsplinoff_val = dev_priv->saveDSPBLINOFF;
+                dsptileoff_val = dev_priv->saveDSPBTILEOFF;
+                dspsize_val = dev_priv->saveDSPBSIZE;
+                dsppos_val = dev_priv->saveDSPBPOS;
+                dspsurf_val = dev_priv->saveDSPBSURF;
+                dspcntr_val = dev_priv->saveDSPBCNTR;
+                dspstatus_val = dev_priv->saveDSPBSTATUS;
+                palette_val = dev_priv->save_palette_b;
+                break;
+        case 2:
+                reg_offset = MIPIC_REG_OFFSET;
+
+                /* register */
+                pipeconf_reg = PIPECCONF;
+                htot_reg = HTOTAL_C;
+                hblank_reg = HBLANK_C;
+                hsync_reg = HSYNC_C;
+                vtot_reg = VTOTAL_C;
+                vblank_reg = VBLANK_C;
+                vsync_reg = VSYNC_C;
+                pipesrc_reg = PIPECSRC;
+                dspstride_reg = DSPCSTRIDE;
+                dsplinoff_reg = DSPCLINOFF;
+                dsptileoff_reg = DSPCTILEOFF;
+                dspsize_reg = DSPCSIZE;
+                dsppos_reg = DSPCPOS;
+                dspsurf_reg = DSPCSURF;
+                mipi_reg = MIPI_C;
+                dspcntr_reg = DSPCCNTR;
+                palette_reg = PALETTE_C;
+                dspstatus_reg = PIPECSTAT;
+
+                /* values */
+                pipeconf_val = dev_priv->savePIPECCONF;
+                htot_val = dev_priv->saveHTOTAL_C;
+                hblank_val = dev_priv->saveHBLANK_C;
+                hsync_val = dev_priv->saveHSYNC_C;
+                vtot_val = dev_priv->saveVTOTAL_C;
+                vblank_val = dev_priv->saveVBLANK_C;
+                vsync_val = dev_priv->saveVSYNC_C;
+                pipesrc_val = dev_priv->savePIPECSRC;
+                dspstride_val = dev_priv->saveDSPCSTRIDE;
+                dsplinoff_val = dev_priv->saveDSPCLINOFF;
+                dsptileoff_val = dev_priv->saveDSPCTILEOFF;
+                dspsize_val = dev_priv->saveDSPCSIZE;
+                dsppos_val = dev_priv->saveDSPCPOS;
+                dspsurf_val = dev_priv->saveDSPCSURF;
+                dspstatus_val = dev_priv->saveDSPCSTATUS;
+                mipi_val = dev_priv->saveMIPI_C;
+                dspcntr_val = dev_priv->saveDSPCCNTR;
+                palette_val = dev_priv->save_palette_c;
+
+                dsi_config = dev_priv->dsi_configs[1];
+                break;
+        default:
+                DRM_ERROR("%s, invalid pipe number.\n", __func__);
+                return -EINVAL;
+        }
+
+        /* Make sure VGA plane is off. it initializes to on after reset!*/
+        PSB_WVDC32(0x80000000, VGACNTRL);
+        if (pipe == 1) {
+                PSB_WVDC32(dpll_val & ~DPLL_VCO_ENABLE, dpll_reg);
+                PSB_RVDC32(dpll_reg);
+
+                PSB_WVDC32(fp_val, fp_reg);
+        } else {
+                dpll = PSB_RVDC32(dpll_reg);
+
+                if (!(dpll & DPLL_VCO_ENABLE)) {
+
+                        /* When ungating power of DPLL, needs to wait 0.5us before enable the VCO */
+                        if (dpll & MDFLD_PWR_GATE_EN) {
+                                dpll &= ~MDFLD_PWR_GATE_EN;
+                                PSB_WVDC32(dpll, dpll_reg);
+                                udelay(500);    /* FIXME: 1 ? */
+                        }
+
+                        PSB_WVDC32(fp_val, fp_reg);
+                        PSB_WVDC32(dpll_val, dpll_reg);
+                        /* FIXME_MDFLD PO - change 500 to 1 after PO */
+                        udelay(500);
+
+                        dpll_val |= DPLL_VCO_ENABLE;
+                        PSB_WVDC32(dpll_val, dpll_reg);
+                        PSB_RVDC32(dpll_reg);
+
+                        /* wait for DSI PLL to lock */
+                        while ((timeout < 20000) && !(PSB_RVDC32(pipeconf_reg) & PIPECONF_DSIPLL_LOCK)) {
+                                udelay(150);
+                                timeout++;
+                        }
+
+                        if (timeout == 20000) {
+                                DRM_ERROR("%s, can't lock DSIPLL.\n",
+                                                        __func__);
+                                return -EINVAL;
+                        }
+                }
+        }
+        /* Restore mode */
+        PSB_WVDC32(htot_val, htot_reg);
+        PSB_WVDC32(hblank_val, hblank_reg);
+        PSB_WVDC32(hsync_val, hsync_reg);
+        PSB_WVDC32(vtot_val, vtot_reg);
+        PSB_WVDC32(vblank_val, vblank_reg);
+        PSB_WVDC32(vsync_val, vsync_reg);
+        PSB_WVDC32(pipesrc_val, pipesrc_reg);
+        PSB_WVDC32(dspstatus_val, dspstatus_reg);
+
+        /* Set up the plane */
+        PSB_WVDC32(dspstride_val, dspstride_reg);
+        PSB_WVDC32(dsplinoff_val, dsplinoff_reg);
+        PSB_WVDC32(dsptileoff_val, dsptileoff_reg);
+        PSB_WVDC32(dspsize_val, dspsize_reg);
+        PSB_WVDC32(dsppos_val, dsppos_reg);
+        PSB_WVDC32(dspsurf_val, dspsurf_reg);
+
+        if (pipe == 1) {
+                PSB_WVDC32(dev_priv->savePFIT_CONTROL, PFIT_CONTROL);
+                PSB_WVDC32(dev_priv->savePFIT_PGM_RATIOS, PFIT_PGM_RATIOS);
+                PSB_WVDC32(dev_priv->saveHDMIPHYMISCCTL, HDMIPHYMISCCTL);
+                PSB_WVDC32(dev_priv->saveHDMIB_CONTROL, HDMIB_CONTROL);
+
+        } else {
+                /* Set up pipe related registers */
+                PSB_WVDC32(mipi_val, mipi_reg);
+                /* Setup MIPI adapter + MIPI IP registers */
+                mdfld_dsi_controller_init(dsi_config, pipe);
+                msleep(20);
+        }
+        /* Enable the plane */
+        PSB_WVDC32(dspcntr_val, dspcntr_reg);
+        msleep(20);
+        /* Enable the pipe */
+        PSB_WVDC32(pipeconf_val, pipeconf_reg);
+
+        for (i = 0; i < 256; i++)
+                PSB_WVDC32(palette_val[i], palette_reg + (i<<2));
+        if (pipe == 1)
+                return 0;
+        if (!mdfld_panel_dpi(dev))
+                mdfld_enable_te(dev, pipe);
+        return 0;
+}
+
+/**
+ * mdfld_restore_cursor_overlay_registers       -       restore cursor
+ * @dev: our device
+ *
+ * Restore the cursor and overlay state that was saved earlier
+ */
+static int mdfld_restore_cursor_overlay_registers(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        /* Enable Cursor A */
+        PSB_WVDC32(dev_priv->saveDSPACURSOR_CTRL, CURACNTR);
+        PSB_WVDC32(dev_priv->saveDSPACURSOR_POS, CURAPOS);
+        PSB_WVDC32(dev_priv->saveDSPACURSOR_BASE, CURABASE);
+
+        PSB_WVDC32(dev_priv->saveDSPBCURSOR_CTRL, CURBCNTR);
+        PSB_WVDC32(dev_priv->saveDSPBCURSOR_POS, CURBPOS);
+        PSB_WVDC32(dev_priv->saveDSPBCURSOR_BASE, CURBBASE);
+
+        PSB_WVDC32(dev_priv->saveDSPCCURSOR_CTRL, CURCCNTR);
+        PSB_WVDC32(dev_priv->saveDSPCCURSOR_POS, CURCPOS);
+        PSB_WVDC32(dev_priv->saveDSPCCURSOR_BASE, CURCBASE);
+
+        /* Restore HW overlay */
+        PSB_WVDC32(dev_priv->saveOV_OVADD, OV_OVADD);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC0, OV_OGAMC0);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC1, OV_OGAMC1);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC2, OV_OGAMC2);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC3, OV_OGAMC3);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC4, OV_OGAMC4);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC5, OV_OGAMC5);
+
+        PSB_WVDC32(dev_priv->saveOV_OVADD_C, OV_OVADD + OV_C_OFFSET);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC0_C, OV_OGAMC0 + OV_C_OFFSET);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC1_C, OV_OGAMC1 + OV_C_OFFSET);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC2_C, OV_OGAMC2 + OV_C_OFFSET);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC3_C, OV_OGAMC3 + OV_C_OFFSET);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC4_C, OV_OGAMC4 + OV_C_OFFSET);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC5_C, OV_OGAMC5 + OV_C_OFFSET);
+
+        return 0;
+}
+
+/**
+ *      mdfld_save_display_registers    -       save registers lost on suspend
+ *      @dev: our DRM device
+ *
+ *      Save the state we need in order to be able to restore the interface
+ *      upon resume from suspend
+ */
+static int mdfld_save_registers(struct drm_device *dev)
+{
+        /* FIXME: We need to shut down panels here if using them
+           and once the right bits are merged */
+        mdfld_save_cursor_overlay_registers(dev);
+        mdfld_save_display_registers(dev, 0);
+        mdfld_save_display_registers(dev, 0);
+        mdfld_save_display_registers(dev, 2);
+        mdfld_save_display_registers(dev, 1);
+        mdfld_disable_crtc(dev, 0);
+        mdfld_disable_crtc(dev, 2);
+        mdfld_disable_crtc(dev, 1);
+        return 0;
+}
+
+/**
+ *      mdfld_restore_display_registers -       restore lost register state
+ *      @dev: our DRM device
+ *
+ *      Restore register state that was lost during suspend and resume.
+ */
+static int mdfld_restore_registers(struct drm_device *dev)
+{
+        mdfld_restore_display_registers(dev, 1);
+        mdfld_restore_display_registers(dev, 0);
+        mdfld_restore_display_registers(dev, 2);
+        mdfld_restore_cursor_overlay_registers(dev);
+        return 0;
+}
+
+static int mdfld_power_down(struct drm_device *dev)
+{
+        /* FIXME */
+        return 0;
+}
+
+static int mdfld_power_up(struct drm_device *dev)
+{
+        /* FIXME */
+        return 0;
+}
+
+const struct psb_ops mdfld_chip_ops = {
+        .name = "Medfield",
+        .accel_2d = 0,
+        .pipes = 3,
+        .crtcs = 2,
+        .sgx_offset = MRST_SGX_OFFSET,
+
+        .chip_setup = mid_chip_setup,
+
+        .crtc_helper = &mdfld_helper_funcs,
+        .crtc_funcs = &mdfld_intel_crtc_funcs,
+
+        .output_init = mdfld_output_init,
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+        .backlight_init = mdfld_backlight_init,
+#endif
+
+        .init_pm = mdfld_init_pm,
+        .save_regs = mdfld_save_registers,
+        .restore_regs = mdfld_restore_registers,
+        .power_down = mdfld_power_down,
+        .power_up = mdfld_power_up,
+};
+
diff --git a/drivers/staging/gma500/mdfld_dsi_dbi.c b/drivers/staging/gma500/mdfld_dsi_dbi.c
new file mode 100644
index 00000000000..fd211f3467c
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_dsi_dbi.c
@@ -0,0 +1,761 @@
+/*
+ * Copyright © 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:
+ *  jim liu <jim.liu@intel.com>
+ *  Jackie Li<yaodong.li@intel.com>
+ */
+
+#include "mdfld_dsi_dbi.h"
+#include "mdfld_dsi_dbi_dpu.h"
+#include "mdfld_dsi_pkg_sender.h"
+
+#include "power.h"
+#include <linux/pm_runtime.h>
+
+int enable_gfx_rtpm;
+
+extern struct drm_device *gpDrmDevice;
+extern int gfxrtdelay;
+int enter_dsr;
+struct mdfld_dsi_dbi_output *gdbi_output;
+extern bool gbgfxsuspended;
+extern int enable_gfx_rtpm;
+extern int gfxrtdelay;
+
+#define MDFLD_DSR_MAX_IDLE_COUNT        2
+
+/*
+ * set refreshing area
+ */
+int mdfld_dsi_dbi_update_area(struct mdfld_dsi_dbi_output *dbi_output,
+                                u16 x1, u16 y1, u16 x2, u16 y2)
+{
+        struct mdfld_dsi_pkg_sender *sender =
+                mdfld_dsi_encoder_get_pkg_sender(&dbi_output->base);
+        u8 param[4];
+        u8 cmd;
+        int err;
+
+        if (!sender) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+
+        /* Set column */
+        cmd = DCS_SET_COLUMN_ADDRESS;
+        param[0] = x1 >> 8;
+        param[1] = x1;
+        param[2] = x2 >> 8;
+        param[3] = x2;
+
+        err = mdfld_dsi_send_dcs(sender,
+                                 cmd,
+                                 param,
+                                 4,
+                                 CMD_DATA_SRC_SYSTEM_MEM,
+                                 MDFLD_DSI_QUEUE_PACKAGE);
+        if (err) {
+                dev_err(sender->dev->dev, "DCS 0x%x sent failed\n", cmd);
+                goto err_out;
+        }
+
+        /* Set page */
+        cmd = DCS_SET_PAGE_ADDRESS;
+        param[0] = y1 >> 8;
+        param[1] = y1;
+        param[2] = y2 >> 8;
+        param[3] = y2;
+
+        err = mdfld_dsi_send_dcs(sender,
+                                 cmd,
+                                 param,
+                                 4,
+                                 CMD_DATA_SRC_SYSTEM_MEM,
+                                 MDFLD_DSI_QUEUE_PACKAGE);
+        if (err) {
+                dev_err(sender->dev->dev, "DCS 0x%x sent failed\n", cmd);
+                goto err_out;
+        }
+
+        /*update screen*/
+        err = mdfld_dsi_send_dcs(sender,
+                                 write_mem_start,
+                                 NULL,
+                                 0,
+                                 CMD_DATA_SRC_PIPE,
+                                 MDFLD_DSI_QUEUE_PACKAGE);
+        if (err) {
+                dev_err(sender->dev->dev, "DCS 0x%x sent failed\n", cmd);
+                goto err_out;
+        }
+        mdfld_dsi_cmds_kick_out(sender);
+err_out:
+        return err;
+}
+
+/*
+ * set panel's power state
+ */
+int mdfld_dsi_dbi_update_power(struct mdfld_dsi_dbi_output *dbi_output,
+                                                                int mode)
+{
+        struct drm_device *dev = dbi_output->dev;
+        struct mdfld_dsi_pkg_sender *sender =
+                mdfld_dsi_encoder_get_pkg_sender(&dbi_output->base);
+        u8 param = 0;
+        u32 err = 0;
+
+        if (!sender) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+
+        if (mode == DRM_MODE_DPMS_ON) {
+                /* Exit sleep mode */
+                err = mdfld_dsi_send_dcs(sender,
+                                         DCS_EXIT_SLEEP_MODE,
+                                         NULL,
+                                         0,
+                                         CMD_DATA_SRC_SYSTEM_MEM,
+                                         MDFLD_DSI_QUEUE_PACKAGE);
+                if (err) {
+                        dev_err(dev->dev, "DCS 0x%x sent failed\n",
+                                                DCS_EXIT_SLEEP_MODE);
+                        goto power_err;
+                }
+
+                /* Set display on */
+                err = mdfld_dsi_send_dcs(sender,
+                                         DCS_SET_DISPLAY_ON,
+                                         NULL,
+                                         0,
+                                         CMD_DATA_SRC_SYSTEM_MEM,
+                                         MDFLD_DSI_QUEUE_PACKAGE);
+                if (err) {
+                        dev_err(dev->dev, "DCS 0x%x sent failed\n",
+                                                        DCS_SET_DISPLAY_ON);
+                        goto power_err;
+                }
+
+                /* set tear effect on */
+                err = mdfld_dsi_send_dcs(sender,
+                                         DCS_SET_TEAR_ON,
+                                         &param,
+                                         1,
+                                         CMD_DATA_SRC_SYSTEM_MEM,
+                                         MDFLD_DSI_QUEUE_PACKAGE);
+                if (err) {
+                        dev_err(dev->dev, "DCS 0x%x sent failed\n",
+                                                        set_tear_on);
+                        goto power_err;
+                }
+
+                /**
+                 * FIXME: remove this later
+                 */
+                err = mdfld_dsi_send_dcs(sender,
+                                         DCS_WRITE_MEM_START,
+                                         NULL,
+                                         0,
+                                         CMD_DATA_SRC_PIPE,
+                                         MDFLD_DSI_QUEUE_PACKAGE);
+                if (err) {
+                        dev_err(dev->dev, "DCS 0x%x sent failed\n",
+                                                DCS_WRITE_MEM_START);
+                        goto power_err;
+                }
+        } else {
+                /* Set tear effect off */
+                err = mdfld_dsi_send_dcs(sender,
+                                         DCS_SET_TEAR_OFF,
+                                         NULL,
+                                         0,
+                                         CMD_DATA_SRC_SYSTEM_MEM,
+                                         MDFLD_DSI_QUEUE_PACKAGE);
+                if (err) {
+                        dev_err(dev->dev, "DCS 0x%x sent failed\n",
+                                                        DCS_SET_TEAR_OFF);
+                        goto power_err;
+                }
+
+                /* Turn display off */
+                err = mdfld_dsi_send_dcs(sender,
+                                         DCS_SET_DISPLAY_OFF,
+                                         NULL,
+                                         0,
+                                         CMD_DATA_SRC_SYSTEM_MEM,
+                                         MDFLD_DSI_QUEUE_PACKAGE);
+                if (err) {
+                        dev_err(dev->dev, "DCS 0x%x sent failed\n",
+                                                DCS_SET_DISPLAY_OFF);
+                        goto power_err;
+                }
+
+                /* Now enter sleep mode */
+                err = mdfld_dsi_send_dcs(sender,
+                                         DCS_ENTER_SLEEP_MODE,
+                                         NULL,
+                                         0,
+                                         CMD_DATA_SRC_SYSTEM_MEM,
+                                         MDFLD_DSI_QUEUE_PACKAGE);
+                if (err) {
+                        dev_err(dev->dev, "DCS 0x%x sent failed\n",
+                                                        DCS_ENTER_SLEEP_MODE);
+                        goto power_err;
+                }
+        }
+        mdfld_dsi_cmds_kick_out(sender);
+power_err:
+        return err;
+}
+
+/*
+ * send a generic DCS command with a parameter list
+ */
+int mdfld_dsi_dbi_send_dcs(struct mdfld_dsi_dbi_output *dbi_output,
+                        u8 dcs,  u8 *param, u32 num, u8 data_src)
+{
+        struct mdfld_dsi_pkg_sender *sender =
+                mdfld_dsi_encoder_get_pkg_sender(&dbi_output->base);
+        int ret;
+
+        if (!sender) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+
+        ret = mdfld_dsi_send_dcs(sender,
+                                 dcs,
+                                 param,
+                                 num,
+                                 data_src,
+                                 MDFLD_DSI_SEND_PACKAGE);
+
+        return ret;
+}
+
+/*
+ * Enter DSR
+ */
+void mdfld_dsi_dbi_enter_dsr(struct mdfld_dsi_dbi_output *dbi_output, int pipe)
+{
+        u32 reg_val;
+        struct drm_device *dev = dbi_output->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct drm_crtc *crtc = dbi_output->base.base.crtc;
+        struct psb_intel_crtc *psb_crtc = (crtc) ?
+                                        to_psb_intel_crtc(crtc) : NULL;
+        u32 dpll_reg = MRST_DPLL_A;
+        u32 pipeconf_reg = PIPEACONF;
+        u32 dspcntr_reg = DSPACNTR;
+
+        if (!dbi_output)
+                return;
+
+        /* FIXME check if can go */
+        dev_priv->is_in_idle = true;
+
+        gdbi_output = dbi_output;
+        if ((dbi_output->mode_flags & MODE_SETTING_ON_GOING) ||
+                (psb_crtc && psb_crtc->mode_flags & MODE_SETTING_ON_GOING))
+                return;
+
+        if (pipe == 2) {
+                dpll_reg = MRST_DPLL_A;
+                pipeconf_reg = PIPECCONF;
+                dspcntr_reg = DSPCCNTR;
+        }
+
+        if (!gma_power_begin(dev, true)) {
+                dev_err(dev->dev, "hw begin failed\n");
+                return;
+        }
+        /* Disable te interrupts */
+        mdfld_disable_te(dev, pipe);
+
+        /* Disable plane */
+        reg_val = REG_READ(dspcntr_reg);
+        if (!(reg_val & DISPLAY_PLANE_ENABLE)) {
+                REG_WRITE(dspcntr_reg, reg_val & ~DISPLAY_PLANE_ENABLE);
+                REG_READ(dspcntr_reg);
+        }
+
+        /* Disable pipe */
+        reg_val = REG_READ(pipeconf_reg);
+        if (!(reg_val & DISPLAY_PLANE_ENABLE)) {
+                reg_val &= ~DISPLAY_PLANE_ENABLE;
+                reg_val |= (PIPECONF_PLANE_OFF | PIPECONF_CURSOR_OFF);
+                REG_WRITE(pipeconf_reg, reg_val);
+                REG_READ(pipeconf_reg);
+                mdfldWaitForPipeDisable(dev, pipe);
+        }
+
+        /* Disable DPLL */
+        reg_val = REG_READ(dpll_reg);
+        if (!(reg_val & DPLL_VCO_ENABLE)) {
+                reg_val &= ~DPLL_VCO_ENABLE;
+                REG_WRITE(dpll_reg, reg_val);
+                REG_READ(dpll_reg);
+                udelay(500);
+        }
+
+        gma_power_end(dev);
+        dbi_output->mode_flags |= MODE_SETTING_IN_DSR;
+        if (pipe == 2) {
+                enter_dsr = 1;
+                /* pm_schedule_suspend(&dev->pdev->dev, gfxrtdelay); */
+        }
+}
+
+static void mdfld_dbi_output_exit_dsr(struct mdfld_dsi_dbi_output *dbi_output,
+                        int pipe)
+{
+        struct drm_device *dev = dbi_output->dev;
+        struct drm_crtc *crtc = dbi_output->base.base.crtc;
+        struct psb_intel_crtc *psb_crtc = (crtc) ?
+                                        to_psb_intel_crtc(crtc) : NULL;
+        u32 reg_val;
+        u32 dpll_reg = MRST_DPLL_A;
+        u32 pipeconf_reg = PIPEACONF;
+        u32 dspcntr_reg = DSPACNTR;
+        u32 reg_offset = 0;
+
+        /*if mode setting on-going, back off*/
+        if ((dbi_output->mode_flags & MODE_SETTING_ON_GOING) ||
+                (psb_crtc && psb_crtc->mode_flags & MODE_SETTING_ON_GOING))
+                return;
+
+        if (pipe == 2) {
+                dpll_reg = MRST_DPLL_A;
+                pipeconf_reg = PIPECCONF;
+                dspcntr_reg = DSPCCNTR;
+                reg_offset = MIPIC_REG_OFFSET;
+        }
+
+        if (!gma_power_begin(dev, true)) {
+                dev_err(dev->dev, "hw begin failed\n");
+                return;
+        }
+
+        /* Enable DPLL */
+        reg_val = REG_READ(dpll_reg);
+        if (!(reg_val & DPLL_VCO_ENABLE)) {
+                if (reg_val & MDFLD_PWR_GATE_EN) {
+                        reg_val &= ~MDFLD_PWR_GATE_EN;
+                        REG_WRITE(dpll_reg, reg_val);
+                        REG_READ(dpll_reg);
+                        udelay(500);
+                }
+
+                reg_val |= DPLL_VCO_ENABLE;
+                REG_WRITE(dpll_reg, reg_val);
+                REG_READ(dpll_reg);
+                udelay(500);
+
+                /* Add timeout */
+                while (!(REG_READ(pipeconf_reg) & PIPECONF_DSIPLL_LOCK))
+                        cpu_relax();
+        }
+
+        /* Enable pipe */
+        reg_val = REG_READ(pipeconf_reg);
+        if (!(reg_val & PIPEACONF_ENABLE)) {
+                reg_val |= PIPEACONF_ENABLE;
+                REG_WRITE(pipeconf_reg, reg_val);
+                REG_READ(pipeconf_reg);
+                udelay(500);
+                mdfldWaitForPipeEnable(dev, pipe);
+        }
+
+        /* Enable plane */
+        reg_val = REG_READ(dspcntr_reg);
+        if (!(reg_val & DISPLAY_PLANE_ENABLE)) {
+                reg_val |= DISPLAY_PLANE_ENABLE;
+                REG_WRITE(dspcntr_reg, reg_val);
+                REG_READ(dspcntr_reg);
+                udelay(500);
+        }
+
+        /* Enable TE interrupt on this pipe */
+        mdfld_enable_te(dev, pipe);
+        gma_power_end(dev);
+
+        /*clean IN_DSR flag*/
+        dbi_output->mode_flags &= ~MODE_SETTING_IN_DSR;
+}
+
+/*
+ * Exit from DSR
+ */
+void mdfld_dsi_dbi_exit_dsr(struct drm_device *dev, u32 update_src)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dsr_info *dsr_info = dev_priv->dbi_dsr_info;
+        struct mdfld_dsi_dbi_output **dbi_output;
+        int i;
+        int pipe;
+
+        /* FIXME can go ? */
+        dev_priv->is_in_idle = false;
+        dbi_output = dsr_info->dbi_outputs;
+
+#ifdef CONFIG_PM_RUNTIME
+         if (!enable_gfx_rtpm) {
+/*                pm_runtime_allow(&gpDrmDevice->pdev->dev); */
+/*              schedule_delayed_work(&rtpm_work, 30 * 1000);*/ /* FIXME: HZ ? */
+        }
+#endif
+
+        /* For each output, exit dsr */
+        for (i = 0; i < dsr_info->dbi_output_num; i++) {
+                /* If panel has been turned off, skip */
+                if (!dbi_output[i] || !dbi_output[i]->dbi_panel_on)
+                        continue;
+                pipe = dbi_output[i]->channel_num ? 2 : 0;
+                enter_dsr = 0;
+                mdfld_dbi_output_exit_dsr(dbi_output[i], pipe);
+        }
+        dev_priv->dsr_fb_update |= update_src;
+}
+
+static bool mdfld_dbi_is_in_dsr(struct drm_device *dev)
+{
+        if (REG_READ(MRST_DPLL_A) & DPLL_VCO_ENABLE)
+                return false;
+        if ((REG_READ(PIPEACONF) & PIPEACONF_ENABLE) ||
+           (REG_READ(PIPECCONF) & PIPEACONF_ENABLE))
+                return false;
+        if ((REG_READ(DSPACNTR) & DISPLAY_PLANE_ENABLE) ||
+           (REG_READ(DSPCCNTR) & DISPLAY_PLANE_ENABLE))
+                return false;
+
+        return true;
+}
+
+/* Periodically update dbi panel */
+void mdfld_dbi_update_panel(struct drm_device *dev, int pipe)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dsr_info *dsr_info = dev_priv->dbi_dsr_info;
+        struct mdfld_dsi_dbi_output **dbi_outputs;
+        struct mdfld_dsi_dbi_output *dbi_output;
+        int i;
+        int can_enter_dsr = 0;
+        u32 damage_mask;
+
+        dbi_outputs = dsr_info->dbi_outputs;
+        dbi_output = pipe ? dbi_outputs[1] : dbi_outputs[0];
+
+        if (!dbi_output)
+                return;
+
+        if (pipe == 0)
+                damage_mask = dev_priv->dsr_fb_update & MDFLD_DSR_DAMAGE_MASK_0;
+        else if (pipe == 2)
+                damage_mask = dev_priv->dsr_fb_update & MDFLD_DSR_DAMAGE_MASK_2;
+        else
+                return;
+
+        /* If FB is damaged and panel is on update on-panel FB */
+        if (damage_mask && dbi_output->dbi_panel_on) {
+                dbi_output->dsr_fb_update_done = false;
+
+                if (dbi_output->p_funcs->update_fb)
+                        dbi_output->p_funcs->update_fb(dbi_output, pipe);
+
+                if (dev_priv->dsr_enable && dbi_output->dsr_fb_update_done)
+                        dev_priv->dsr_fb_update &= ~damage_mask;
+
+                /*clean IN_DSR flag*/
+                dbi_output->mode_flags &= ~MODE_SETTING_IN_DSR;
+
+                dbi_output->dsr_idle_count = 0;
+        } else {
+                dbi_output->dsr_idle_count++;
+        }
+
+        switch (dsr_info->dbi_output_num) {
+        case 1:
+                if (dbi_output->dsr_idle_count > MDFLD_DSR_MAX_IDLE_COUNT)
+                        can_enter_dsr = 1;
+                break;
+        case 2:
+                if (dbi_outputs[0]->dsr_idle_count > MDFLD_DSR_MAX_IDLE_COUNT
+                   && dbi_outputs[1]->dsr_idle_count > MDFLD_DSR_MAX_IDLE_COUNT)
+                        can_enter_dsr = 1;
+                break;
+        default:
+                DRM_ERROR("Wrong DBI output number\n");
+        }
+
+        /* Try to enter DSR */
+        if (can_enter_dsr) {
+                for (i = 0; i < dsr_info->dbi_output_num; i++) {
+                        if (!mdfld_dbi_is_in_dsr(dev) && dbi_outputs[i] &&
+                           !(dbi_outputs[i]->mode_flags & MODE_SETTING_ON_GOING)) {
+                                mdfld_dsi_dbi_enter_dsr(dbi_outputs[i],
+                                        dbi_outputs[i]->channel_num ? 2 : 0);
+#if 0
+                                enter_dsr = 1;
+                                pr_err("%s: enter_dsr = 1\n", __func__);
+#endif
+                        }
+                }
+        /*schedule rpm suspend after gfxrtdelay*/
+#ifdef CONFIG_GFX_RTPM
+                if (!dev_priv->rpm_enabled
+                        || !enter_dsr
+        /*              || (REG_READ(HDMIB_CONTROL) & HDMIB_PORT_EN) */
+                        || pm_schedule_suspend(&dev->pdev->dev, gfxrtdelay))
+                        dev_warn(dev->dev,
+                                "Runtime PM schedule suspend failed, rpm %d\n",
+                                        dev_priv->rpm_enabled);
+#endif
+        }
+}
+
+int mdfld_dbi_dsr_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dsr_info *dsr_info = dev_priv->dbi_dsr_info;
+
+        if (!dsr_info || IS_ERR(dsr_info)) {
+                dsr_info = kzalloc(sizeof(struct mdfld_dbi_dsr_info),
+                                                                GFP_KERNEL);
+                if (!dsr_info) {
+                        dev_err(dev->dev, "No memory\n");
+                        return -ENOMEM;
+                }
+                dev_priv->dbi_dsr_info = dsr_info;
+        }
+        return 0;
+}
+
+void mdfld_dbi_dsr_exit(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dsr_info *dsr_info = dev_priv->dbi_dsr_info;
+
+        if (dsr_info) {
+                kfree(dsr_info);
+                dev_priv->dbi_dsr_info = NULL;
+        }
+}
+
+void mdfld_dsi_controller_dbi_init(struct mdfld_dsi_config *dsi_config,
+                                                                int pipe)
+{
+        struct drm_device *dev = dsi_config->dev;
+        u32 reg_offset = pipe ? MIPIC_REG_OFFSET : 0;
+        int lane_count = dsi_config->lane_count;
+        u32 val = 0;
+
+        dev_dbg(dev->dev, "Init DBI interface on pipe %d...\n", pipe);
+
+        /* Un-ready device */
+        REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset), 0x00000000);
+
+        /* Init dsi adapter before kicking off */
+        REG_WRITE((MIPIA_CONTROL_REG + reg_offset), 0x00000018);
+
+        /* TODO: figure out how to setup these registers */
+        REG_WRITE((MIPIA_DPHY_PARAM_REG + reg_offset), 0x150c3408);
+        REG_WRITE((MIPIA_CLK_LANE_SWITCH_TIME_CNT_REG + reg_offset),
+                                                        0x000a0014);
+        REG_WRITE((MIPIA_DBI_BW_CTRL_REG + reg_offset), 0x00000400);
+        REG_WRITE((MIPIA_DBI_FIFO_THROTTLE_REG + reg_offset), 0x00000001);
+        REG_WRITE((MIPIA_HS_LS_DBI_ENABLE_REG + reg_offset), 0x00000000);
+
+        /* Enable all interrupts */
+        REG_WRITE((MIPIA_INTR_EN_REG + reg_offset), 0xffffffff);
+        /* Max value: 20 clock cycles of txclkesc */
+        REG_WRITE((MIPIA_TURN_AROUND_TIMEOUT_REG + reg_offset), 0x0000001f);
+        /* Min 21 txclkesc, max: ffffh */
+        REG_WRITE((MIPIA_DEVICE_RESET_TIMER_REG + reg_offset), 0x0000ffff);
+        /* Min: 7d0 max: 4e20 */
+        REG_WRITE((MIPIA_INIT_COUNT_REG + reg_offset), 0x00000fa0);
+
+        /* Set up func_prg */
+        val |= lane_count;
+        val |= (dsi_config->channel_num << DSI_DBI_VIRT_CHANNEL_OFFSET);
+        val |= DSI_DBI_COLOR_FORMAT_OPTION2;
+        REG_WRITE((MIPIA_DSI_FUNC_PRG_REG + reg_offset), val);
+
+        REG_WRITE((MIPIA_HS_TX_TIMEOUT_REG + reg_offset), 0x3fffff);
+        REG_WRITE((MIPIA_LP_RX_TIMEOUT_REG + reg_offset), 0xffff);
+
+        /* De-assert dbi_stall when half of DBI FIFO is empty */
+        /* REG_WRITE((MIPIA_DBI_FIFO_THROTTLE_REG + reg_offset), 0x00000000); */
+
+        REG_WRITE((MIPIA_HIGH_LOW_SWITCH_COUNT_REG + reg_offset), 0x46);
+        REG_WRITE((MIPIA_EOT_DISABLE_REG + reg_offset), 0x00000000);
+        REG_WRITE((MIPIA_LP_BYTECLK_REG + reg_offset), 0x00000004);
+        REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset), 0x00000001);
+}
+
+#if 0
+/*DBI encoder helper funcs*/
+static const struct drm_encoder_helper_funcs mdfld_dsi_dbi_helper_funcs = {
+        .dpms = mdfld_dsi_dbi_dpms,
+        .mode_fixup = mdfld_dsi_dbi_mode_fixup,
+        .prepare = mdfld_dsi_dbi_prepare,
+        .mode_set = mdfld_dsi_dbi_mode_set,
+        .commit = mdfld_dsi_dbi_commit,
+};
+
+/*DBI encoder funcs*/
+static const struct drm_encoder_funcs mdfld_dsi_dbi_encoder_funcs = {
+        .destroy = drm_encoder_cleanup,
+};
+
+#endif
+
+/*
+ * Init DSI DBI encoder.
+ * Allocate an mdfld_dsi_encoder and attach it to given @dsi_connector
+ * return pointer of newly allocated DBI encoder, NULL on error
+ */
+struct mdfld_dsi_encoder *mdfld_dsi_dbi_init(struct drm_device *dev,
+                                struct mdfld_dsi_connector *dsi_connector,
+                                struct panel_funcs *p_funcs)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dsi_dbi_output *dbi_output = NULL;
+        struct mdfld_dsi_config *dsi_config;
+        struct drm_connector *connector = NULL;
+        struct drm_encoder *encoder = NULL;
+        struct drm_display_mode *fixed_mode = NULL;
+        struct psb_gtt *pg = dev_priv ? (&dev_priv->gtt) : NULL;
+        struct mdfld_dbi_dpu_info *dpu_info = dev_priv ? (dev_priv->dbi_dpu_info) : NULL;
+        struct mdfld_dbi_dsr_info *dsr_info = dev_priv ? (dev_priv->dbi_dsr_info) : NULL;
+        u32 data = 0;
+        int pipe;
+        int ret;
+
+        if (!pg || !dsi_connector || !p_funcs) {
+                WARN_ON(1);
+                return NULL;
+        }
+
+        dsi_config = mdfld_dsi_get_config(dsi_connector);
+        pipe = dsi_connector->pipe;
+
+        /*panel hard-reset*/
+        if (p_funcs->reset) {
+                ret = p_funcs->reset(pipe);
+                if (ret) {
+                        DRM_ERROR("Panel %d hard-reset failed\n", pipe);
+                        return NULL;
+                }
+        }
+        /* Panel drvIC init */
+        if (p_funcs->drv_ic_init)
+                p_funcs->drv_ic_init(dsi_config, pipe);
+
+        /* Panel power mode detect */
+        ret = mdfld_dsi_get_power_mode(dsi_config,
+                                       &data,
+                                       MDFLD_DSI_HS_TRANSMISSION);
+        if (ret) {
+                DRM_ERROR("Panel %d get power mode failed\n", pipe);
+                dsi_connector->status = connector_status_disconnected;
+        } else {
+                DRM_INFO("pipe %d power mode 0x%x\n", pipe, data);
+                dsi_connector->status = connector_status_connected;
+        }
+
+        /*TODO: get panel info from DDB*/
+
+        dbi_output = kzalloc(sizeof(struct mdfld_dsi_dbi_output), GFP_KERNEL);
+        if (!dbi_output) {
+                dev_err(dev->dev, "No memory\n");
+                return NULL;
+        }
+
+        if (dsi_connector->pipe == 0) {
+                dbi_output->channel_num = 0;
+                dev_priv->dbi_output = dbi_output;
+        } else if (dsi_connector->pipe == 2) {
+                dbi_output->channel_num = 1;
+                dev_priv->dbi_output2 = dbi_output;
+        } else {
+                dev_err(dev->dev, "only support 2 DSI outputs\n");
+                goto out_err1;
+        }
+
+        dbi_output->dev = dev;
+        dbi_output->p_funcs = p_funcs;
+        fixed_mode = dsi_config->fixed_mode;
+        dbi_output->panel_fixed_mode = fixed_mode;
+
+        /* Create drm encoder object */
+        connector = &dsi_connector->base.base;
+        encoder = &dbi_output->base.base;
+        /* Review this if we ever get MIPI-HDMI bridges or similar */
+        drm_encoder_init(dev,
+                        encoder,
+                        p_funcs->encoder_funcs,
+                        DRM_MODE_ENCODER_LVDS);
+        drm_encoder_helper_add(encoder, p_funcs->encoder_helper_funcs);
+
+        /* Attach to given connector */
+        drm_mode_connector_attach_encoder(connector, encoder);
+
+        /* Set possible CRTCs and clones */
+        if (dsi_connector->pipe) {
+                encoder->possible_crtcs = (1 << 2);
+                encoder->possible_clones = (1 << 1);
+        } else {
+                encoder->possible_crtcs = (1 << 0);
+                encoder->possible_clones = (1 << 0);
+        }
+
+        dev_priv->dsr_fb_update = 0;
+        dev_priv->dsr_enable = false;
+        dev_priv->exit_idle = mdfld_dsi_dbi_exit_dsr;
+
+        dbi_output->first_boot = true;
+        dbi_output->mode_flags = MODE_SETTING_IN_ENCODER;
+
+        /* Add this output to dpu_info if in DPU mode */
+        if (dpu_info && dsi_connector->status == connector_status_connected) {
+                if (dsi_connector->pipe == 0)
+                        dpu_info->dbi_outputs[0] = dbi_output;
+                else
+                        dpu_info->dbi_outputs[1] = dbi_output;
+
+                dpu_info->dbi_output_num++;
+        } else if (dsi_connector->status == connector_status_connected) {
+                /* Add this output to dsr_info if not */
+                if (dsi_connector->pipe == 0)
+                        dsr_info->dbi_outputs[0] = dbi_output;
+                else
+                        dsr_info->dbi_outputs[1] = dbi_output;
+
+                dsr_info->dbi_output_num++;
+        }
+        return &dbi_output->base;
+out_err1:
+        kfree(dbi_output);
+        return NULL;
+}
diff --git a/drivers/staging/gma500/mdfld_dsi_dbi.h b/drivers/staging/gma500/mdfld_dsi_dbi.h
new file mode 100644
index 00000000000..f0fa986fd93
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_dsi_dbi.h
@@ -0,0 +1,173 @@
+/*
+ * Copyright © 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:
+ * jim liu <jim.liu@intel.com>
+ * Jackie Li<yaodong.li@intel.com>
+ */
+
+#ifndef __MDFLD_DSI_DBI_H__
+#define __MDFLD_DSI_DBI_H__
+
+#include <linux/backlight.h>
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_edid.h>
+
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "power.h"
+
+#include "mdfld_dsi_output.h"
+#include "mdfld_output.h"
+
+/*
+ * DBI encoder which inherits from mdfld_dsi_encoder
+ */
+struct mdfld_dsi_dbi_output {
+        struct mdfld_dsi_encoder base;
+        struct drm_display_mode *panel_fixed_mode;
+        u8 last_cmd;
+        u8 lane_count;
+        u8 channel_num;
+        struct drm_device *dev;
+
+        /* Backlight operations */
+
+        /* DSR timer */
+        u32 dsr_idle_count;
+        bool dsr_fb_update_done;
+
+        /* Mode setting flags */
+        u32 mode_flags;
+
+        /* Panel status */
+        bool dbi_panel_on;
+        bool first_boot;
+        struct panel_funcs *p_funcs;
+
+        /* DPU */
+        u32 *dbi_cb_addr;
+        u32 dbi_cb_phy;
+        spinlock_t cb_lock;
+        u32 cb_write;
+};
+
+#define MDFLD_DSI_DBI_OUTPUT(dsi_encoder) \
+        container_of(dsi_encoder, struct mdfld_dsi_dbi_output, base)
+
+struct mdfld_dbi_dsr_info {
+        int dbi_output_num;
+        struct mdfld_dsi_dbi_output *dbi_outputs[2];
+
+        u32 dsr_idle_count;
+};
+
+#define DBI_CB_TIMEOUT_COUNT    0xffff
+
+/* Offsets */
+#define CMD_MEM_ADDR_OFFSET     0
+
+#define CMD_DATA_SRC_SYSTEM_MEM 0
+#define CMD_DATA_SRC_PIPE       1
+
+static inline int mdfld_dsi_dbi_fifo_ready(struct mdfld_dsi_dbi_output *dbi_output)
+{
+        struct drm_device *dev = dbi_output->dev;
+        u32 retry = DBI_CB_TIMEOUT_COUNT;
+        int reg_offset = (dbi_output->channel_num == 1) ? MIPIC_REG_OFFSET : 0;
+        int ret = 0;
+
+        /* Query the dbi fifo status*/
+        while (retry--) {
+                if (REG_READ(MIPIA_GEN_FIFO_STAT_REG + reg_offset) & (1 << 27))
+                        break;
+        }
+
+        if (!retry) {
+                DRM_ERROR("Timeout waiting for DBI FIFO empty\n");
+                ret = -EAGAIN;
+        }
+        return ret;
+}
+
+static inline int mdfld_dsi_dbi_cmd_sent(struct mdfld_dsi_dbi_output *dbi_output)
+{
+        struct drm_device *dev = dbi_output->dev;
+        u32 retry = DBI_CB_TIMEOUT_COUNT;
+        int reg_offset = (dbi_output->channel_num == 1) ? MIPIC_REG_OFFSET : 0;
+        int ret = 0;
+
+        /* Query the command execution status */
+        while (retry--)
+                if (!(REG_READ(MIPIA_CMD_ADD_REG + reg_offset) & (1 << 0)))
+                        break;
+
+        if (!retry) {
+                DRM_ERROR("Timeout waiting for DBI command status\n");
+                ret = -EAGAIN;
+        }
+
+        return ret;
+}
+
+static inline int mdfld_dsi_dbi_cb_ready(struct mdfld_dsi_dbi_output *dbi_output)
+{
+        int ret = 0;
+
+        /* Query the command execution status*/
+        ret = mdfld_dsi_dbi_cmd_sent(dbi_output);
+        if (ret) {
+                DRM_ERROR("Peripheral is busy\n");
+                ret = -EAGAIN;
+        }
+        /* Query the dbi fifo status*/
+        ret = mdfld_dsi_dbi_fifo_ready(dbi_output);
+        if (ret) {
+                DRM_ERROR("DBI FIFO is not empty\n");
+                ret = -EAGAIN;
+        }
+        return ret;
+}
+
+extern void mdfld_dsi_dbi_output_init(struct drm_device *dev,
+                        struct psb_intel_mode_device *mode_dev, int pipe);
+extern void mdfld_dsi_dbi_exit_dsr(struct drm_device *dev, u32 update_src);
+extern void mdfld_dsi_dbi_enter_dsr(struct mdfld_dsi_dbi_output *dbi_output,
+                        int pipe);
+extern int mdfld_dbi_dsr_init(struct drm_device *dev);
+extern void mdfld_dbi_dsr_exit(struct drm_device *dev);
+extern struct mdfld_dsi_encoder *mdfld_dsi_dbi_init(struct drm_device *dev,
+                        struct mdfld_dsi_connector *dsi_connector,
+                        struct panel_funcs *p_funcs);
+extern int mdfld_dsi_dbi_send_dcs(struct mdfld_dsi_dbi_output *dbi_output,
+                        u8 dcs, u8 *param, u32 num, u8 data_src);
+extern int mdfld_dsi_dbi_update_area(struct mdfld_dsi_dbi_output *dbi_output,
+                        u16 x1, u16 y1, u16 x2, u16 y2);
+extern int mdfld_dsi_dbi_update_power(struct mdfld_dsi_dbi_output *dbi_output,
+                        int mode);
+extern void mdfld_dsi_controller_dbi_init(struct mdfld_dsi_config *dsi_config,
+                        int pipe);
+
+#endif /*__MDFLD_DSI_DBI_H__*/
diff --git a/drivers/staging/gma500/mdfld_dsi_dbi_dpu.c b/drivers/staging/gma500/mdfld_dsi_dbi_dpu.c
new file mode 100644
index 00000000000..a4e2ff442b1
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_dsi_dbi_dpu.c
@@ -0,0 +1,778 @@
+/*
+ * Copyright © 2010-2011 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:
+ * Jim Liu <jim.liu@intel.com>
+ * Jackie Li<yaodong.li@intel.com>
+ */
+
+#include "mdfld_dsi_dbi_dpu.h"
+#include "mdfld_dsi_dbi.h"
+
+/*
+ * NOTE: all mdlfd_x_damage funcs should be called by holding dpu_update_lock
+ */
+
+static int mdfld_cursor_damage(struct mdfld_dbi_dpu_info *dpu_info,
+                           mdfld_plane_t plane,
+                           struct psb_drm_dpu_rect *damaged_rect)
+{
+        int x, y;
+        int new_x, new_y;
+        struct psb_drm_dpu_rect *rect;
+        struct psb_drm_dpu_rect *pipe_rect;
+        int cursor_size;
+        struct mdfld_cursor_info *cursor;
+        mdfld_plane_t fb_plane;
+
+        if (plane == MDFLD_CURSORA) {
+                cursor = &dpu_info->cursors[0];
+                x = dpu_info->cursors[0].x;
+                y = dpu_info->cursors[0].y;
+                cursor_size = dpu_info->cursors[0].size;
+                pipe_rect = &dpu_info->damage_pipea;
+                fb_plane = MDFLD_PLANEA;
+        } else {
+                cursor = &dpu_info->cursors[1];
+                x = dpu_info->cursors[1].x;
+                y = dpu_info->cursors[1].y;
+                cursor_size = dpu_info->cursors[1].size;
+                pipe_rect = &dpu_info->damage_pipec;
+                fb_plane = MDFLD_PLANEC;
+        }
+        new_x = damaged_rect->x;
+        new_y = damaged_rect->y;
+
+        if (x == new_x && y == new_y)
+                return 0;
+
+        rect = &dpu_info->damaged_rects[plane];
+        /* Move to right */
+        if (new_x >= x) {
+                if (new_y > y) {
+                        rect->x = x;
+                        rect->y = y;
+                        rect->width = (new_x + cursor_size) - x;
+                        rect->height = (new_y + cursor_size) - y;
+                        goto cursor_out;
+                } else {
+                        rect->x = x;
+                        rect->y = new_y;
+                        rect->width = (new_x + cursor_size) - x;
+                        rect->height = (y - new_y);
+                        goto cursor_out;
+                }
+        } else {
+                if (new_y > y) {
+                        rect->x = new_x;
+                        rect->y = y;
+                        rect->width = (x + cursor_size) - new_x;
+                        rect->height = new_y - y;
+                        goto cursor_out;
+                } else {
+                        rect->x = new_x;
+                        rect->y = new_y;
+                        rect->width = (x + cursor_size) - new_x;
+                        rect->height = (y + cursor_size) - new_y;
+                }
+        }
+cursor_out:
+        if (new_x < 0)
+                cursor->x = 0;
+        else if (new_x > 864)
+                cursor->x = 864;
+        else
+                cursor->x = new_x;
+
+        if (new_y < 0)
+                cursor->y = 0;
+        else if (new_y > 480)
+                cursor->y = 480;
+        else
+                cursor->y = new_y;
+
+        /*
+         * FIXME: this is a workaround for cursor plane update,
+         * remove it later!
+         */
+        rect->x = 0;
+        rect->y = 0;
+        rect->width = 864;
+        rect->height = 480;
+
+        mdfld_check_boundary(dpu_info, rect);
+        mdfld_dpu_region_extent(pipe_rect, rect);
+
+        /* Update pending status of dpu_info */
+        dpu_info->pending |= (1 << plane);
+        /* Update fb panel as well */
+        dpu_info->pending |= (1 << fb_plane);
+        return 0;
+}
+
+static int mdfld_fb_damage(struct mdfld_dbi_dpu_info *dpu_info,
+                                   mdfld_plane_t plane,
+                                   struct psb_drm_dpu_rect *damaged_rect)
+{
+        struct psb_drm_dpu_rect *rect;
+
+        if (plane == MDFLD_PLANEA)
+                rect = &dpu_info->damage_pipea;
+        else
+                rect = &dpu_info->damage_pipec;
+
+        mdfld_check_boundary(dpu_info, damaged_rect);
+
+        /* Add fb damage area to this pipe */
+        mdfld_dpu_region_extent(rect, damaged_rect);
+
+        /* Update pending status of dpu_info */
+        dpu_info->pending |= (1 << plane);
+        return 0;
+}
+
+/* Do nothing here, right now */
+static int mdfld_overlay_damage(struct mdfld_dbi_dpu_info *dpu_info,
+                                mdfld_plane_t plane,
+                                struct psb_drm_dpu_rect *damaged_rect)
+{
+        return 0;
+}
+
+int mdfld_dbi_dpu_report_damage(struct drm_device *dev,
+                                mdfld_plane_t plane,
+                                struct psb_drm_dpu_rect *rect)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dpu_info *dpu_info = dev_priv->dbi_dpu_info;
+        int ret = 0;
+
+        /* DPU not in use, no damage reporting needed */
+        if (dpu_info == NULL)
+                return 0;
+
+        spin_lock(&dpu_info->dpu_update_lock);
+
+        switch (plane) {
+        case MDFLD_PLANEA:
+        case MDFLD_PLANEC:
+                mdfld_fb_damage(dpu_info, plane, rect);
+                break;
+        case MDFLD_CURSORA:
+        case MDFLD_CURSORC:
+                mdfld_cursor_damage(dpu_info, plane, rect);
+                break;
+        case MDFLD_OVERLAYA:
+        case MDFLD_OVERLAYC:
+                mdfld_overlay_damage(dpu_info, plane, rect);
+                break;
+        default:
+                DRM_ERROR("Invalid plane type %d\n", plane);
+                ret = -EINVAL;
+        }
+        spin_unlock(&dpu_info->dpu_update_lock);
+        return ret;
+}
+
+int mdfld_dbi_dpu_report_fullscreen_damage(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv;
+        struct mdfld_dbi_dpu_info *dpu_info;
+        struct mdfld_dsi_config  *dsi_config;
+        struct psb_drm_dpu_rect rect;
+        int i;
+
+        if (!dev) {
+                DRM_ERROR("Invalid parameter\n");
+                return -EINVAL;
+        }
+
+        dev_priv = dev->dev_private;
+        dpu_info = dev_priv->dbi_dpu_info;
+
+        /* This is fine - we may be in non DPU mode */
+        if (!dpu_info)
+                return -EINVAL;
+
+        for (i = 0; i < dpu_info->dbi_output_num; i++) {
+                dsi_config = dev_priv->dsi_configs[i];
+                if (dsi_config) {
+                        rect.x = rect.y = 0;
+                        rect.width = dsi_config->fixed_mode->hdisplay;
+                        rect.height = dsi_config->fixed_mode->vdisplay;
+                        mdfld_dbi_dpu_report_damage(dev,
+                                    i ? (MDFLD_PLANEC) : (MDFLD_PLANEA),
+                                    &rect);
+                }
+        }
+        /* Exit DSR state */
+        mdfld_dpu_exit_dsr(dev);
+        return 0;
+}
+
+int mdfld_dsi_dbi_dsr_off(struct drm_device *dev,
+                                        struct psb_drm_dpu_rect *rect)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dpu_info *dpu_info = dev_priv->dbi_dpu_info;
+
+        mdfld_dbi_dpu_report_damage(dev, MDFLD_PLANEA, rect);
+
+        /* If dual display mode */
+        if (dpu_info->dbi_output_num == 2)
+                mdfld_dbi_dpu_report_damage(dev, MDFLD_PLANEC, rect);
+
+        /* Force dsi to exit DSR mode */
+        mdfld_dpu_exit_dsr(dev);
+        return 0;
+}
+
+static void mdfld_dpu_cursor_plane_flush(struct mdfld_dbi_dpu_info *dpu_info,
+                                                 mdfld_plane_t plane)
+{
+        struct drm_device *dev = dpu_info->dev;
+        u32 curpos_reg = CURAPOS;
+        u32 curbase_reg = CURABASE;
+        u32 curcntr_reg = CURACNTR;
+        struct mdfld_cursor_info *cursor = &dpu_info->cursors[0];
+
+        if (plane == MDFLD_CURSORC) {
+                curpos_reg = CURCPOS;
+                curbase_reg = CURCBASE;
+                curcntr_reg = CURCCNTR;
+                cursor = &dpu_info->cursors[1];
+        }
+
+        REG_WRITE(curcntr_reg, REG_READ(curcntr_reg));
+        REG_WRITE(curpos_reg,
+                (((cursor->x & CURSOR_POS_MASK) << CURSOR_X_SHIFT) |
+                ((cursor->y & CURSOR_POS_MASK) << CURSOR_Y_SHIFT)));
+        REG_WRITE(curbase_reg, REG_READ(curbase_reg));
+}
+
+static void mdfld_dpu_fb_plane_flush(struct mdfld_dbi_dpu_info *dpu_info,
+                                                 mdfld_plane_t plane)
+{
+        u32 pipesrc_reg = PIPEASRC;
+        u32 dspsize_reg = DSPASIZE;
+        u32 dspoff_reg = DSPALINOFF;
+        u32 dspsurf_reg = DSPASURF;
+        u32 dspstride_reg = DSPASTRIDE;
+        u32 stride;
+        struct psb_drm_dpu_rect *rect = &dpu_info->damage_pipea;
+        struct drm_device *dev = dpu_info->dev;
+
+        if (plane == MDFLD_PLANEC) {
+                pipesrc_reg = PIPECSRC;
+                dspsize_reg = DSPCSIZE;
+                dspoff_reg = DSPCLINOFF;
+                dspsurf_reg = DSPCSURF;
+                dspstride_reg = DSPCSTRIDE;
+                rect = &dpu_info->damage_pipec;
+        }
+
+        stride = REG_READ(dspstride_reg);
+        /* FIXME: should I do the pipe src update here? */
+        REG_WRITE(pipesrc_reg, ((rect->width - 1) << 16) | (rect->height - 1));
+        /* Flush plane */
+        REG_WRITE(dspsize_reg, ((rect->height - 1) << 16) | (rect->width - 1));
+        REG_WRITE(dspoff_reg, ((rect->x * 4) + (rect->y * stride)));
+        REG_WRITE(dspsurf_reg, REG_READ(dspsurf_reg));
+
+        /*
+         * TODO: wait for flip finished and restore the pipesrc reg,
+         * or cursor will be show at a wrong position
+         */
+}
+
+static void mdfld_dpu_overlay_plane_flush(struct mdfld_dbi_dpu_info *dpu_info,
+                                                  mdfld_plane_t plane)
+{
+}
+
+/*
+ * TODO: we are still in dbi normal mode now, we will try to use partial
+ * mode later.
+ */
+static int mdfld_dbi_prepare_cb(struct mdfld_dsi_dbi_output *dbi_output,
+                                struct mdfld_dbi_dpu_info *dpu_info, int pipe)
+{
+        u8 *cb_addr = (u8 *)dbi_output->dbi_cb_addr;
+        u32 *index;
+        struct psb_drm_dpu_rect *rect = pipe ?
+                (&dpu_info->damage_pipec) : (&dpu_info->damage_pipea);
+
+        /* FIXME: lock command buffer, this may lead to a deadlock,
+           as we already hold the dpu_update_lock */
+        if (!spin_trylock(&dbi_output->cb_lock)) {
+                DRM_ERROR("lock command buffer failed, try again\n");
+                return -EAGAIN;
+        }
+
+        index = &dbi_output->cb_write;
+
+        if (*index) {
+                DRM_ERROR("DBI command buffer unclean\n");
+                return -EAGAIN;
+        }
+
+        /* Column address */
+        *(cb_addr + ((*index)++)) = set_column_address;
+        *(cb_addr + ((*index)++)) = rect->x >> 8;
+        *(cb_addr + ((*index)++)) = rect->x;
+        *(cb_addr + ((*index)++)) = (rect->x + rect->width - 1) >> 8;
+        *(cb_addr + ((*index)++)) = (rect->x + rect->width - 1);
+
+        *index = 8;
+
+        /* Page address */
+        *(cb_addr + ((*index)++)) = set_page_addr;
+        *(cb_addr + ((*index)++)) = rect->y >> 8;
+        *(cb_addr + ((*index)++)) = rect->y;
+        *(cb_addr + ((*index)++)) = (rect->y + rect->height - 1) >> 8;
+        *(cb_addr + ((*index)++)) = (rect->y + rect->height - 1);
+
+        *index = 16;
+
+        /*write memory*/
+        *(cb_addr + ((*index)++)) = write_mem_start;
+
+        return 0;
+}
+
+static int mdfld_dbi_flush_cb(struct mdfld_dsi_dbi_output *dbi_output, int pipe)
+{
+        u32 cmd_phy = dbi_output->dbi_cb_phy;
+        u32 *index = &dbi_output->cb_write;
+        int reg_offset = pipe ? MIPIC_REG_OFFSET : 0;
+        struct drm_device *dev = dbi_output->dev;
+
+        if (*index == 0 || !dbi_output)
+                return 0;
+
+        REG_WRITE((MIPIA_CMD_LEN_REG + reg_offset), 0x010505);
+        REG_WRITE((MIPIA_CMD_ADD_REG + reg_offset), cmd_phy | 3);
+
+        *index = 0;
+
+        /* FIXME: unlock command buffer */
+        spin_unlock(&dbi_output->cb_lock);
+        return 0;
+}
+
+static int mdfld_dpu_update_pipe(struct mdfld_dsi_dbi_output *dbi_output,
+                                 struct mdfld_dbi_dpu_info *dpu_info, int pipe)
+{
+        struct drm_device *dev =  dbi_output->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        mdfld_plane_t cursor_plane = MDFLD_CURSORA;
+        mdfld_plane_t fb_plane = MDFLD_PLANEA;
+        mdfld_plane_t overlay_plane = MDFLD_OVERLAYA;
+        int ret = 0;
+        u32 plane_mask = MDFLD_PIPEA_PLANE_MASK;
+
+        /* Damaged rects on this pipe */
+        if (pipe) {
+                cursor_plane = MDFLD_CURSORC;
+                fb_plane = MDFLD_PLANEC;
+                overlay_plane = MDFLD_OVERLAYC;
+                plane_mask = MDFLD_PIPEC_PLANE_MASK;
+        }
+
+        /*update cursor which assigned to @pipe*/
+        if (dpu_info->pending & (1 << cursor_plane))
+                mdfld_dpu_cursor_plane_flush(dpu_info, cursor_plane);
+
+        /*update fb which assigned to @pipe*/
+        if (dpu_info->pending & (1 << fb_plane))
+                mdfld_dpu_fb_plane_flush(dpu_info, fb_plane);
+
+        /* TODO: update overlay */
+        if (dpu_info->pending & (1 << overlay_plane))
+                mdfld_dpu_overlay_plane_flush(dpu_info, overlay_plane);
+
+        /* Flush damage area to panel fb */
+        if (dpu_info->pending & plane_mask) {
+                ret = mdfld_dbi_prepare_cb(dbi_output, dpu_info, pipe);
+                /*
+                 * TODO: remove b_dsr_enable later,
+                 * added it so that text console could boot smoothly
+                 */
+                /* Clean pending flags on this pipe */
+                if (!ret && dev_priv->dsr_enable) {
+                        dpu_info->pending &= ~plane_mask;
+                        /* Reset overlay pipe damage rect */
+                        mdfld_dpu_init_damage(dpu_info, pipe);
+                }
+        }
+        return ret;
+}
+
+static int mdfld_dpu_update_fb(struct drm_device *dev)
+{
+        struct drm_crtc *crtc;
+        struct psb_intel_crtc *psb_crtc;
+        struct mdfld_dsi_dbi_output **dbi_output;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dpu_info *dpu_info = dev_priv->dbi_dpu_info;
+        bool pipe_updated[2];
+        unsigned long irq_flags;
+        u32 dpll_reg = MRST_DPLL_A;
+        u32 dspcntr_reg = DSPACNTR;
+        u32 pipeconf_reg = PIPEACONF;
+        u32 dsplinoff_reg = DSPALINOFF;
+        u32 dspsurf_reg = DSPASURF;
+        u32 mipi_state_reg = MIPIA_INTR_STAT_REG;
+        u32 reg_offset = 0;
+        int pipe;
+        int i;
+        int ret;
+
+        dbi_output = dpu_info->dbi_outputs;
+        pipe_updated[0] = pipe_updated[1] = false;
+
+        if (!gma_power_begin(dev, true))
+                return -EAGAIN;
+
+        /* Try to prevent any new damage reports */
+        if (!spin_trylock_irqsave(&dpu_info->dpu_update_lock, irq_flags))
+                return -EAGAIN;
+
+        for (i = 0; i < dpu_info->dbi_output_num; i++) {
+                crtc = dbi_output[i]->base.base.crtc;
+                psb_crtc = (crtc) ? to_psb_intel_crtc(crtc) : NULL;
+
+                pipe = dbi_output[i]->channel_num ? 2 : 0;
+
+                if (pipe == 2) {
+                        dspcntr_reg = DSPCCNTR;
+                        pipeconf_reg = PIPECCONF;
+                        dsplinoff_reg = DSPCLINOFF;
+                        dspsurf_reg = DSPCSURF;
+                        reg_offset = MIPIC_REG_OFFSET;
+                }
+
+                if (!(REG_READ((MIPIA_GEN_FIFO_STAT_REG + reg_offset))
+                                                        & (1 << 27)) ||
+                        !(REG_READ(dpll_reg) & DPLL_VCO_ENABLE) ||
+                        !(REG_READ(dspcntr_reg) & DISPLAY_PLANE_ENABLE) ||
+                        !(REG_READ(pipeconf_reg) & DISPLAY_PLANE_ENABLE)) {
+                        dev_err(dev->dev,
+                                "DBI FIFO is busy, DSI %d state %x\n",
+                                pipe,
+                                REG_READ(mipi_state_reg + reg_offset));
+                        continue;
+                }
+
+                /*
+                 *      If DBI output is in a exclusive state then the pipe
+                 *      change won't be updated
+                 */
+                if (dbi_output[i]->dbi_panel_on &&
+                   !(dbi_output[i]->mode_flags & MODE_SETTING_ON_GOING) &&
+                   !(psb_crtc &&
+                        psb_crtc->mode_flags & MODE_SETTING_ON_GOING) &&
+                   !(dbi_output[i]->mode_flags & MODE_SETTING_IN_DSR)) {
+                        ret = mdfld_dpu_update_pipe(dbi_output[i],
+                                dpu_info, dbi_output[i]->channel_num ? 2 : 0);
+                        if (!ret)
+                                pipe_updated[i] = true;
+                }
+        }
+
+        for (i = 0; i < dpu_info->dbi_output_num; i++)
+                if (pipe_updated[i])
+                        mdfld_dbi_flush_cb(dbi_output[i],
+                                dbi_output[i]->channel_num ? 2 : 0);
+
+        spin_unlock_irqrestore(&dpu_info->dpu_update_lock, irq_flags);
+        gma_power_end(dev);
+        return 0;
+}
+
+static int __mdfld_dbi_exit_dsr(struct mdfld_dsi_dbi_output *dbi_output,
+                                                                int pipe)
+{
+        struct drm_device *dev = dbi_output->dev;
+        struct drm_crtc *crtc = dbi_output->base.base.crtc;
+        struct psb_intel_crtc *psb_crtc = (crtc) ? to_psb_intel_crtc(crtc)
+                                                                : NULL;
+        u32 reg_val;
+        u32 dpll_reg = MRST_DPLL_A;
+        u32 pipeconf_reg = PIPEACONF;
+        u32 dspcntr_reg = DSPACNTR;
+        u32 dspbase_reg = DSPABASE;
+        u32 dspsurf_reg = DSPASURF;
+        u32 reg_offset = 0;
+
+        if (!dbi_output)
+                return 0;
+
+        /* If mode setting on-going, back off */
+        if ((dbi_output->mode_flags & MODE_SETTING_ON_GOING) ||
+                (psb_crtc && psb_crtc->mode_flags & MODE_SETTING_ON_GOING))
+                return -EAGAIN;
+
+        if (pipe == 2) {
+                dpll_reg = MRST_DPLL_A;
+                pipeconf_reg = PIPECCONF;
+                dspcntr_reg = DSPCCNTR;
+                dspbase_reg = MDFLD_DSPCBASE;
+                dspsurf_reg = DSPCSURF;
+
+                reg_offset = MIPIC_REG_OFFSET;
+        }
+
+        if (!gma_power_begin(dev, true))
+                return -EAGAIN;
+
+        /* Enable DPLL */
+        reg_val = REG_READ(dpll_reg);
+        if (!(reg_val & DPLL_VCO_ENABLE)) {
+
+                if (reg_val & MDFLD_PWR_GATE_EN) {
+                        reg_val &= ~MDFLD_PWR_GATE_EN;
+                        REG_WRITE(dpll_reg, reg_val);
+                        REG_READ(dpll_reg);
+                        udelay(500);
+                }
+
+                reg_val |= DPLL_VCO_ENABLE;
+                REG_WRITE(dpll_reg, reg_val);
+                REG_READ(dpll_reg);
+                udelay(500);
+
+                /* FIXME: add timeout */
+                while (!(REG_READ(pipeconf_reg) & PIPECONF_DSIPLL_LOCK))
+                        cpu_relax();
+        }
+
+        /* Enable pipe */
+        reg_val = REG_READ(pipeconf_reg);
+        if (!(reg_val & PIPEACONF_ENABLE)) {
+                reg_val |= PIPEACONF_ENABLE;
+                REG_WRITE(pipeconf_reg, reg_val);
+                REG_READ(pipeconf_reg);
+                udelay(500);
+                mdfldWaitForPipeEnable(dev, pipe);
+        }
+
+        /* Enable plane */
+        reg_val = REG_READ(dspcntr_reg);
+        if (!(reg_val & DISPLAY_PLANE_ENABLE)) {
+                reg_val |= DISPLAY_PLANE_ENABLE;
+                REG_WRITE(dspcntr_reg, reg_val);
+                REG_READ(dspcntr_reg);
+                udelay(500);
+        }
+
+        gma_power_end(dev);
+
+        /* Clean IN_DSR flag */
+        dbi_output->mode_flags &= ~MODE_SETTING_IN_DSR;
+
+        return 0;
+}
+
+int mdfld_dpu_exit_dsr(struct drm_device *dev)
+{
+        struct mdfld_dsi_dbi_output **dbi_output;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dpu_info *dpu_info = dev_priv->dbi_dpu_info;
+        int i;
+        int pipe;
+
+        dbi_output = dpu_info->dbi_outputs;
+
+        for (i = 0; i < dpu_info->dbi_output_num; i++) {
+                /* If this output is not in DSR mode, don't call exit dsr */
+                if (dbi_output[i]->mode_flags & MODE_SETTING_IN_DSR)
+                        __mdfld_dbi_exit_dsr(dbi_output[i],
+                                        dbi_output[i]->channel_num ? 2 : 0);
+        }
+
+        /* Enable TE interrupt */
+        for (i = 0; i < dpu_info->dbi_output_num; i++) {
+                /* If this output is not in DSR mode, don't call exit dsr */
+                pipe = dbi_output[i]->channel_num ? 2 : 0;
+                if (dbi_output[i]->dbi_panel_on && pipe) {
+                        mdfld_disable_te(dev, 0);
+                        mdfld_enable_te(dev, 2);
+                } else if (dbi_output[i]->dbi_panel_on && !pipe) {
+                        mdfld_disable_te(dev, 2);
+                        mdfld_enable_te(dev, 0);
+                }
+        }
+        return 0;
+}
+
+static int mdfld_dpu_enter_dsr(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dpu_info *dpu_info = dev_priv->dbi_dpu_info;
+        struct mdfld_dsi_dbi_output **dbi_output;
+        int i;
+
+        dbi_output = dpu_info->dbi_outputs;
+
+        for (i = 0; i < dpu_info->dbi_output_num; i++) {
+                /* If output is off or already in DSR state, don't re-enter */
+                if (dbi_output[i]->dbi_panel_on &&
+                   !(dbi_output[i]->mode_flags & MODE_SETTING_IN_DSR)) {
+                        mdfld_dsi_dbi_enter_dsr(dbi_output[i],
+                                dbi_output[i]->channel_num ? 2 : 0);
+                }
+        }
+
+        return 0;
+}
+
+static void mdfld_dbi_dpu_timer_func(unsigned long data)
+{
+        struct drm_device *dev = (struct drm_device *)data;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dpu_info *dpu_info = dev_priv->dbi_dpu_info;
+        struct timer_list *dpu_timer = &dpu_info->dpu_timer;
+        unsigned long flags;
+
+        if (dpu_info->pending) {
+                dpu_info->idle_count = 0;
+                /* Update panel fb with damaged area */
+                mdfld_dpu_update_fb(dev);
+        } else {
+                dpu_info->idle_count++;
+        }
+
+        if (dpu_info->idle_count >= MDFLD_MAX_IDLE_COUNT) {
+                mdfld_dpu_enter_dsr(dev);
+                /* Stop timer by return */
+                return;
+        }
+
+        spin_lock_irqsave(&dpu_info->dpu_timer_lock, flags);
+        if (!timer_pending(dpu_timer)) {
+                dpu_timer->expires = jiffies + MDFLD_DSR_DELAY;
+                add_timer(dpu_timer);
+        }
+        spin_unlock_irqrestore(&dpu_info->dpu_timer_lock, flags);
+}
+
+void mdfld_dpu_update_panel(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dpu_info *dpu_info = dev_priv->dbi_dpu_info;
+
+        if (dpu_info->pending) {
+                dpu_info->idle_count = 0;
+
+                /*update panel fb with damaged area*/
+                mdfld_dpu_update_fb(dev);
+        } else {
+                dpu_info->idle_count++;
+        }
+
+        if (dpu_info->idle_count >= MDFLD_MAX_IDLE_COUNT) {
+                /*enter dsr*/
+                mdfld_dpu_enter_dsr(dev);
+        }
+}
+
+static int mdfld_dbi_dpu_timer_init(struct drm_device *dev,
+                                struct mdfld_dbi_dpu_info *dpu_info)
+{
+        struct timer_list *dpu_timer = &dpu_info->dpu_timer;
+        unsigned long flags;
+
+        spin_lock_init(&dpu_info->dpu_timer_lock);
+        spin_lock_irqsave(&dpu_info->dpu_timer_lock, flags);
+
+        init_timer(dpu_timer);
+
+        dpu_timer->data = (unsigned long)dev;
+        dpu_timer->function = mdfld_dbi_dpu_timer_func;
+        dpu_timer->expires = jiffies + MDFLD_DSR_DELAY;
+
+        spin_unlock_irqrestore(&dpu_info->dpu_timer_lock, flags);
+
+        return 0;
+}
+
+void mdfld_dbi_dpu_timer_start(struct mdfld_dbi_dpu_info *dpu_info)
+{
+        struct timer_list *dpu_timer = &dpu_info->dpu_timer;
+        unsigned long flags;
+
+        spin_lock_irqsave(&dpu_info->dpu_timer_lock, flags);
+        if (!timer_pending(dpu_timer)) {
+                dpu_timer->expires = jiffies + MDFLD_DSR_DELAY;
+                add_timer(dpu_timer);
+        }
+        spin_unlock_irqrestore(&dpu_info->dpu_timer_lock, flags);
+}
+
+int mdfld_dbi_dpu_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dpu_info *dpu_info = dev_priv->dbi_dpu_info;
+
+        if (!dpu_info || IS_ERR(dpu_info)) {
+                dpu_info = kzalloc(sizeof(struct mdfld_dbi_dpu_info),
+                                                                GFP_KERNEL);
+                if (!dpu_info) {
+                        DRM_ERROR("No memory\n");
+                        return -ENOMEM;
+                }
+                dev_priv->dbi_dpu_info = dpu_info;
+        }
+
+        dpu_info->dev = dev;
+
+        dpu_info->cursors[0].size = MDFLD_CURSOR_SIZE;
+        dpu_info->cursors[1].size = MDFLD_CURSOR_SIZE;
+
+        /*init dpu_update_lock*/
+        spin_lock_init(&dpu_info->dpu_update_lock);
+
+        /*init dpu refresh timer*/
+        mdfld_dbi_dpu_timer_init(dev, dpu_info);
+
+        /*init pipe damage area*/
+        mdfld_dpu_init_damage(dpu_info, 0);
+        mdfld_dpu_init_damage(dpu_info, 2);
+
+        return 0;
+}
+
+void mdfld_dbi_dpu_exit(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dbi_dpu_info *dpu_info = dev_priv->dbi_dpu_info;
+
+        if (!dpu_info)
+                return;
+
+        del_timer_sync(&dpu_info->dpu_timer);
+        kfree(dpu_info);
+        dev_priv->dbi_dpu_info = NULL;
+}
+
+
diff --git a/drivers/staging/gma500/mdfld_dsi_dbi_dpu.h b/drivers/staging/gma500/mdfld_dsi_dbi_dpu.h
new file mode 100644
index 00000000000..42367ed48c0
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_dsi_dbi_dpu.h
@@ -0,0 +1,154 @@
+/*
+ * Copyright © 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:
+ * jim liu <jim.liu@intel.com>
+ * Jackie Li<yaodong.li@intel.com>
+ */
+
+#ifndef __MDFLD_DSI_DBI_DPU_H__
+#define __MDFLD_DSI_DBI_DPU_H__
+
+#include "mdfld_dsi_dbi.h"
+
+typedef enum {
+        MDFLD_PLANEA,
+        MDFLD_PLANEC,
+        MDFLD_CURSORA,
+        MDFLD_CURSORC,
+        MDFLD_OVERLAYA,
+        MDFLD_OVERLAYC,
+        MDFLD_PLANE_NUM,
+} mdfld_plane_t;
+
+#define MDFLD_PIPEA_PLANE_MASK  0x15
+#define MDFLD_PIPEC_PLANE_MASK  0x2A
+
+struct mdfld_cursor_info {
+        int x, y;
+        int size;
+};
+
+#define MDFLD_CURSOR_SIZE       64
+
+/*
+ * enter DSR mode if screen has no update for 2 frames.
+ */
+#define MDFLD_MAX_IDLE_COUNT    2
+
+struct mdfld_dbi_dpu_info {
+        struct drm_device *dev;
+        /* Lock */
+        spinlock_t dpu_update_lock;
+
+        /* Cursor postion */
+        struct mdfld_cursor_info cursors[2];
+
+        /* Damaged area for each plane */
+        struct psb_drm_dpu_rect damaged_rects[MDFLD_PLANE_NUM];
+
+        /* Final damaged area */
+        struct psb_drm_dpu_rect damage_pipea;
+        struct psb_drm_dpu_rect damage_pipec;
+
+        /* Pending */
+        u32 pending;
+
+        /* DPU timer */
+        struct timer_list dpu_timer;
+        spinlock_t dpu_timer_lock;
+
+        /* DPU idle count */
+        u32 idle_count;
+
+        /* DSI outputs */
+        struct mdfld_dsi_dbi_output *dbi_outputs[2];
+        int dbi_output_num;
+};
+
+static inline int mdfld_dpu_region_extent(struct psb_drm_dpu_rect *origin,
+                         struct psb_drm_dpu_rect *rect)
+{
+        int x1, y1, x2, y2;
+
+        x1 = origin->x + origin->width;
+        y1 = origin->y + origin->height;
+
+        x2 = rect->x + rect->width;
+        y2 = rect->y + rect->height;
+
+        origin->x = min(origin->x, rect->x);
+        origin->y = min(origin->y, rect->y);
+        origin->width = max(x1, x2) - origin->x;
+        origin->height = max(y1, y2) - origin->y;
+
+        return 0;
+}
+
+static inline void mdfld_check_boundary(struct mdfld_dbi_dpu_info *dpu_info,
+                                struct psb_drm_dpu_rect *rect)
+{
+        if (rect->x < 0)
+                rect->x = 0;
+        if (rect->y < 0)
+                rect->y = 0;
+
+        if (rect->x + rect->width > 864)
+                rect->width = 864 - rect->x;
+        if (rect->y + rect->height > 480)
+                rect->height = 480 - rect->height;
+
+        if (!rect->width)
+                rect->width = 1;
+        if (!rect->height)
+                rect->height = 1;
+}
+
+static inline void mdfld_dpu_init_damage(struct mdfld_dbi_dpu_info *dpu_info,
+                                int pipe)
+{
+        struct psb_drm_dpu_rect *rect;
+
+        if (pipe == 0)
+                rect = &dpu_info->damage_pipea;
+        else
+                rect = &dpu_info->damage_pipec;
+
+        rect->x = 864;
+        rect->y = 480;
+        rect->width = -864;
+        rect->height = -480;
+}
+
+extern int mdfld_dsi_dbi_dsr_off(struct drm_device *dev,
+                                struct psb_drm_dpu_rect *rect);
+extern int mdfld_dbi_dpu_report_damage(struct drm_device *dev,
+                                mdfld_plane_t plane,
+                                struct psb_drm_dpu_rect *rect);
+extern int mdfld_dbi_dpu_report_fullscreen_damage(struct drm_device *dev);
+extern int mdfld_dpu_exit_dsr(struct drm_device *dev);
+extern void mdfld_dbi_dpu_timer_start(struct mdfld_dbi_dpu_info *dpu_info);
+extern int mdfld_dbi_dpu_init(struct drm_device *dev);
+extern void mdfld_dbi_dpu_exit(struct drm_device *dev);
+extern void mdfld_dpu_update_panel(struct drm_device *dev);
+
+#endif /*__MDFLD_DSI_DBI_DPU_H__*/
diff --git a/drivers/staging/gma500/mdfld_dsi_dpi.c b/drivers/staging/gma500/mdfld_dsi_dpi.c
new file mode 100644
index 00000000000..e685f1217ba
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_dsi_dpi.c
@@ -0,0 +1,805 @@
+/*
+ * Copyright © 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:
+ * jim liu <jim.liu@intel.com>
+ * Jackie Li<yaodong.li@intel.com>
+ */
+
+#include "mdfld_dsi_dpi.h"
+#include "mdfld_output.h"
+#include "mdfld_dsi_pkg_sender.h"
+
+
+static void mdfld_wait_for_HS_DATA_FIFO(struct drm_device *dev, u32 pipe)
+{
+        u32 gen_fifo_stat_reg = MIPIA_GEN_FIFO_STAT_REG;
+        int timeout = 0;
+
+        if (pipe == 2)
+                gen_fifo_stat_reg += MIPIC_REG_OFFSET;
+
+        udelay(500);
+
+        /* This will time out after approximately 2+ seconds */
+        while ((timeout < 20000) && (REG_READ(gen_fifo_stat_reg) & DSI_FIFO_GEN_HS_DATA_FULL)) {
+                udelay(100);
+                timeout++;
+        }
+
+        if (timeout == 20000)
+                dev_warn(dev->dev, "MIPI: HS Data FIFO was never cleared!\n");
+}
+
+static void mdfld_wait_for_HS_CTRL_FIFO(struct drm_device *dev, u32 pipe)
+{
+        u32 gen_fifo_stat_reg = MIPIA_GEN_FIFO_STAT_REG;
+        int timeout = 0;
+
+        if (pipe == 2)
+                gen_fifo_stat_reg += MIPIC_REG_OFFSET;
+
+        udelay(500);
+
+        /* This will time out after approximately 2+ seconds */
+        while ((timeout < 20000) && (REG_READ(gen_fifo_stat_reg) & DSI_FIFO_GEN_HS_CTRL_FULL)) {
+                udelay(100);
+                timeout++;
+        }
+        if (timeout == 20000)
+                dev_warn(dev->dev, "MIPI: HS CMD FIFO was never cleared!\n");
+}
+
+static void mdfld_wait_for_DPI_CTRL_FIFO(struct drm_device *dev, u32 pipe)
+{
+        u32 gen_fifo_stat_reg = MIPIA_GEN_FIFO_STAT_REG;
+        int timeout = 0;
+
+        if (pipe == 2)
+                gen_fifo_stat_reg += MIPIC_REG_OFFSET;
+
+        udelay(500);
+
+        /* This will time out after approximately 2+ seconds */
+        while ((timeout < 20000) && ((REG_READ(gen_fifo_stat_reg) & DPI_FIFO_EMPTY)
+                                                        != DPI_FIFO_EMPTY)) {
+                udelay(100);
+                timeout++;
+        }
+
+        if (timeout == 20000)
+                dev_warn(dev->dev, "MIPI: DPI FIFO was never cleared!\n");
+}
+
+static void mdfld_wait_for_SPL_PKG_SENT(struct drm_device *dev, u32 pipe)
+{
+        u32 intr_stat_reg = MIPIA_INTR_STAT_REG;
+        int timeout = 0;
+
+        if (pipe == 2)
+                intr_stat_reg += MIPIC_REG_OFFSET;
+
+        udelay(500);
+
+        /* This will time out after approximately 2+ seconds */
+        while ((timeout < 20000) && (!(REG_READ(intr_stat_reg) & DSI_INTR_STATE_SPL_PKG_SENT))) {
+                udelay(100);
+                timeout++;
+        }
+
+        if (timeout == 20000)
+                dev_warn(dev->dev, "MIPI: SPL_PKT_SENT_INTERRUPT was not sent successfully!\n");
+}
+
+
+/* ************************************************************************* *\
+ * FUNCTION: mdfld_dsi_tpo_ic_init
+ *
+ * DESCRIPTION:  This function is called only by mrst_dsi_mode_set and
+ *               restore_display_registers.  since this function does not
+ *               acquire the mutex, it is important that the calling function
+ *               does!
+\* ************************************************************************* */
+void mdfld_dsi_tpo_ic_init(struct mdfld_dsi_config *dsi_config, u32 pipe)
+{
+        struct drm_device *dev = dsi_config->dev;
+        u32 dcsChannelNumber = dsi_config->channel_num;
+        u32 gen_data_reg = MIPIA_HS_GEN_DATA_REG; 
+        u32 gen_ctrl_reg = MIPIA_HS_GEN_CTRL_REG;
+        u32 gen_ctrl_val = GEN_LONG_WRITE;
+
+        if (pipe == 2) {
+                gen_data_reg = HS_GEN_DATA_REG + MIPIC_REG_OFFSET; 
+                gen_ctrl_reg = HS_GEN_CTRL_REG + MIPIC_REG_OFFSET;
+        }
+
+        gen_ctrl_val |= dcsChannelNumber << DCS_CHANNEL_NUMBER_POS;
+
+        /* Flip page order */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x00008036);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x02 << WORD_COUNTS_POS));
+
+        /* 0xF0 */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x005a5af0);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x03 << WORD_COUNTS_POS));
+
+        /* Write protection key */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x005a5af1);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x03 << WORD_COUNTS_POS));
+
+        /* 0xFC */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x005a5afc);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x03 << WORD_COUNTS_POS));
+
+        /* 0xB7 */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x770000b7);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x00000044);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x05 << WORD_COUNTS_POS));
+
+        /* 0xB6 */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x000a0ab6);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x03 << WORD_COUNTS_POS));
+
+        /* 0xF2 */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x081010f2);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x4a070708);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x000000c5);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x09 << WORD_COUNTS_POS));
+
+        /* 0xF8 */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x024003f8);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x01030a04);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x0e020220);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x00000004);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x0d << WORD_COUNTS_POS));
+
+        /* 0xE2 */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x398fc3e2);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x0000916f);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x06 << WORD_COUNTS_POS));
+
+        /* 0xB0 */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x000000b0);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x02 << WORD_COUNTS_POS));
+
+        /* 0xF4 */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x240242f4);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x78ee2002);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x2a071050);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x507fee10);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x10300710);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x14 << WORD_COUNTS_POS));
+
+        /* 0xBA */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x19fe07ba);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x101c0a31);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x00000010);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x09 << WORD_COUNTS_POS));
+
+        /* 0xBB */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x28ff07bb);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x24280a31);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x00000034);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x09 << WORD_COUNTS_POS));
+
+        /* 0xFB */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x535d05fb);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x1b1a2130);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x221e180e);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x131d2120);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x535d0508);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x1c1a2131);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x231f160d);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x111b2220);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x535c2008);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x1f1d2433);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x2c251a10);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x2c34372d);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x00000023);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x31 << WORD_COUNTS_POS));
+
+        /* 0xFA */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x525c0bfa);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x1c1c232f);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x2623190e);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x18212625);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x545d0d0e);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x1e1d2333);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x26231a10);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x1a222725);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x545d280f);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x21202635);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x31292013);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x31393d33);
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x00000029);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x31 << WORD_COUNTS_POS));
+
+        /* Set DM */
+        mdfld_wait_for_HS_DATA_FIFO(dev, pipe);
+        REG_WRITE(gen_data_reg, 0x000100f7);
+        mdfld_wait_for_HS_CTRL_FIFO(dev, pipe);
+        REG_WRITE(gen_ctrl_reg, gen_ctrl_val | (0x03 << WORD_COUNTS_POS));
+}
+
+static u16 mdfld_dsi_dpi_to_byte_clock_count(int pixel_clock_count,
+                                                int num_lane, int bpp)
+{
+        return (u16)((pixel_clock_count * bpp) / (num_lane * 8)); 
+}
+
+/*
+ * Calculate the dpi time basing on a given drm mode @mode
+ * return 0 on success.
+ * FIXME: I was using proposed mode value for calculation, may need to 
+ * use crtc mode values later 
+ */
+int mdfld_dsi_dpi_timing_calculation(struct drm_display_mode *mode, 
+                        struct mdfld_dsi_dpi_timing *dpi_timing,
+                        int num_lane, int bpp)
+{
+        int pclk_hsync, pclk_hfp, pclk_hbp, pclk_hactive;
+        int pclk_vsync, pclk_vfp, pclk_vbp, pclk_vactive;
+        
+        if(!mode || !dpi_timing) {
+                DRM_ERROR("Invalid parameter\n");
+                return -EINVAL;
+        }
+        
+        pclk_hactive = mode->hdisplay;
+        pclk_hfp = mode->hsync_start - mode->hdisplay;
+        pclk_hsync = mode->hsync_end - mode->hsync_start;
+        pclk_hbp = mode->htotal - mode->hsync_end;
+        
+        pclk_vactive = mode->vdisplay;
+        pclk_vfp = mode->vsync_start - mode->vdisplay;
+        pclk_vsync = mode->vsync_end - mode->vsync_start;
+        pclk_vbp = mode->vtotal - mode->vsync_end;
+
+        /*
+         * byte clock counts were calculated by following formula
+         * bclock_count = pclk_count * bpp / num_lane / 8
+         */
+        dpi_timing->hsync_count = mdfld_dsi_dpi_to_byte_clock_count(pclk_hsync, num_lane, bpp);
+        dpi_timing->hbp_count = mdfld_dsi_dpi_to_byte_clock_count(pclk_hbp, num_lane, bpp);
+        dpi_timing->hfp_count = mdfld_dsi_dpi_to_byte_clock_count(pclk_hfp, num_lane, bpp);
+        dpi_timing->hactive_count = mdfld_dsi_dpi_to_byte_clock_count(pclk_hactive, num_lane, bpp);
+        dpi_timing->vsync_count = mdfld_dsi_dpi_to_byte_clock_count(pclk_vsync, num_lane, bpp);
+        dpi_timing->vbp_count = mdfld_dsi_dpi_to_byte_clock_count(pclk_vbp, num_lane, bpp);
+        dpi_timing->vfp_count = mdfld_dsi_dpi_to_byte_clock_count(pclk_vfp, num_lane, bpp);
+
+        return 0; 
+}
+
+void mdfld_dsi_dpi_controller_init(struct mdfld_dsi_config *dsi_config, int pipe)
+{
+        struct drm_device *dev = dsi_config->dev;
+        u32 reg_offset = pipe ? MIPIC_REG_OFFSET : 0;
+        int lane_count = dsi_config->lane_count;
+        struct mdfld_dsi_dpi_timing dpi_timing;
+        struct drm_display_mode *mode = dsi_config->mode;
+        u32 val = 0;
+        
+        /*un-ready device*/
+        REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset), 0x00000000);
+        
+        /*init dsi adapter before kicking off*/
+        REG_WRITE((MIPIA_CONTROL_REG + reg_offset), 0x00000018);
+        
+        /*enable all interrupts*/
+        REG_WRITE((MIPIA_INTR_EN_REG + reg_offset), 0xffffffff);
+        
+
+        /*set up func_prg*/
+        val |= lane_count;
+        val |= dsi_config->channel_num << DSI_DPI_VIRT_CHANNEL_OFFSET;
+                
+        switch(dsi_config->bpp) {
+        case 16:
+                val |= DSI_DPI_COLOR_FORMAT_RGB565;
+                break;
+        case 18:
+                val |= DSI_DPI_COLOR_FORMAT_RGB666;
+                break;
+        case 24:
+                val |= DSI_DPI_COLOR_FORMAT_RGB888;
+                break;
+        default:
+                DRM_ERROR("unsupported color format, bpp = %d\n", dsi_config->bpp);
+        }
+        REG_WRITE((MIPIA_DSI_FUNC_PRG_REG + reg_offset), val);
+        
+        REG_WRITE((MIPIA_HS_TX_TIMEOUT_REG + reg_offset), 
+                        (mode->vtotal * mode->htotal * dsi_config->bpp / (8 * lane_count)) & DSI_HS_TX_TIMEOUT_MASK);
+        REG_WRITE((MIPIA_LP_RX_TIMEOUT_REG + reg_offset), 0xffff & DSI_LP_RX_TIMEOUT_MASK);
+        
+        /*max value: 20 clock cycles of txclkesc*/
+        REG_WRITE((MIPIA_TURN_AROUND_TIMEOUT_REG + reg_offset), 0x14 & DSI_TURN_AROUND_TIMEOUT_MASK);
+        
+        /*min 21 txclkesc, max: ffffh*/
+        REG_WRITE((MIPIA_DEVICE_RESET_TIMER_REG + reg_offset), 0xffff & DSI_RESET_TIMER_MASK);
+
+        REG_WRITE((MIPIA_DPI_RESOLUTION_REG + reg_offset), mode->vdisplay << 16 | mode->hdisplay);
+        
+        /*set DPI timing registers*/
+        mdfld_dsi_dpi_timing_calculation(mode, &dpi_timing, dsi_config->lane_count, dsi_config->bpp);
+        
+        REG_WRITE((MIPIA_HSYNC_COUNT_REG + reg_offset), dpi_timing.hsync_count & DSI_DPI_TIMING_MASK);
+        REG_WRITE((MIPIA_HBP_COUNT_REG + reg_offset), dpi_timing.hbp_count & DSI_DPI_TIMING_MASK);
+        REG_WRITE((MIPIA_HFP_COUNT_REG + reg_offset), dpi_timing.hfp_count & DSI_DPI_TIMING_MASK);
+        REG_WRITE((MIPIA_HACTIVE_COUNT_REG + reg_offset), dpi_timing.hactive_count & DSI_DPI_TIMING_MASK);
+        REG_WRITE((MIPIA_VSYNC_COUNT_REG + reg_offset), dpi_timing.vsync_count & DSI_DPI_TIMING_MASK);
+        REG_WRITE((MIPIA_VBP_COUNT_REG + reg_offset), dpi_timing.vbp_count & DSI_DPI_TIMING_MASK);
+        REG_WRITE((MIPIA_VFP_COUNT_REG + reg_offset), dpi_timing.vfp_count & DSI_DPI_TIMING_MASK);
+        
+        REG_WRITE((MIPIA_HIGH_LOW_SWITCH_COUNT_REG + reg_offset), 0x46);
+        
+        /*min: 7d0 max: 4e20*/
+        REG_WRITE((MIPIA_INIT_COUNT_REG + reg_offset), 0x000007d0);
+        
+        /*set up video mode*/
+        val = 0;
+        val = dsi_config->video_mode | DSI_DPI_COMPLETE_LAST_LINE;
+        REG_WRITE((MIPIA_VIDEO_MODE_FORMAT_REG + reg_offset), val);
+        
+        REG_WRITE((MIPIA_EOT_DISABLE_REG + reg_offset), 0x00000000);
+        
+        REG_WRITE((MIPIA_LP_BYTECLK_REG + reg_offset), 0x00000004);
+        
+        /*TODO: figure out how to setup these registers*/
+        REG_WRITE((MIPIA_DPHY_PARAM_REG + reg_offset), 0x150c3408);
+        
+        REG_WRITE((MIPIA_CLK_LANE_SWITCH_TIME_CNT_REG + reg_offset), (0xa << 16) | 0x14);
+        /*set device ready*/
+        REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset), 0x00000001);
+}
+
+void mdfld_dsi_dpi_turn_on(struct mdfld_dsi_dpi_output *output, int pipe)
+{
+        struct drm_device *dev = output->dev;
+        u32 reg_offset = 0;
+        
+        if(output->panel_on) 
+                return;
+                
+        if(pipe) 
+                reg_offset = MIPIC_REG_OFFSET;
+
+        /* clear special packet sent bit */
+        if(REG_READ(MIPIA_INTR_STAT_REG + reg_offset) & DSI_INTR_STATE_SPL_PKG_SENT) {
+                REG_WRITE((MIPIA_INTR_STAT_REG + reg_offset), DSI_INTR_STATE_SPL_PKG_SENT);
+        }
+                
+        /*send turn on package*/
+        REG_WRITE((MIPIA_DPI_CONTROL_REG + reg_offset), DSI_DPI_CTRL_HS_TURN_ON);
+        
+        /*wait for SPL_PKG_SENT interrupt*/
+        mdfld_wait_for_SPL_PKG_SENT(dev, pipe);
+        
+        if(REG_READ(MIPIA_INTR_STAT_REG + reg_offset) & DSI_INTR_STATE_SPL_PKG_SENT) {
+                REG_WRITE((MIPIA_INTR_STAT_REG + reg_offset), DSI_INTR_STATE_SPL_PKG_SENT);
+        }
+
+        output->panel_on = 1;
+
+        /* FIXME the following is disabled to WA the X slow start issue for TMD panel */
+        /* if(pipe == 2) */
+        /*      dev_priv->dpi_panel_on2 = true; */
+        /* else if (pipe == 0) */
+        /*      dev_priv->dpi_panel_on = true; */
+}
+
+static void mdfld_dsi_dpi_shut_down(struct mdfld_dsi_dpi_output *output, int pipe)
+{
+        struct drm_device *dev = output->dev;
+        u32 reg_offset = 0;
+        
+        /*if output is on, or mode setting didn't happen, ignore this*/
+        if((!output->panel_on) || output->first_boot) {
+                output->first_boot = 0; 
+                return;
+        }
+        
+        if(pipe)
+                reg_offset = MIPIC_REG_OFFSET;
+
+        /* Wait for dpi fifo to empty */
+        mdfld_wait_for_DPI_CTRL_FIFO(dev, pipe);
+
+        /* Clear the special packet interrupt bit if set */
+        if(REG_READ(MIPIA_INTR_STAT_REG + reg_offset) & DSI_INTR_STATE_SPL_PKG_SENT) {
+                REG_WRITE((MIPIA_INTR_STAT_REG + reg_offset), DSI_INTR_STATE_SPL_PKG_SENT);
+        }
+        
+        if(REG_READ(MIPIA_DPI_CONTROL_REG + reg_offset) == DSI_DPI_CTRL_HS_SHUTDOWN) {
+                dev_warn(dev->dev, "try to send the same package again, abort!");
+                goto shutdown_out;
+        }
+        
+        REG_WRITE((MIPIA_DPI_CONTROL_REG + reg_offset), DSI_DPI_CTRL_HS_SHUTDOWN);
+
+shutdown_out:
+        output->panel_on = 0;
+        output->first_boot = 0;
+
+        /* FIXME the following is disabled to WA the X slow start issue for TMD panel */
+        /* if(pipe == 2) */
+        /*      dev_priv->dpi_panel_on2 = false; */
+        /* else if (pipe == 0) */
+        /*      dev_priv->dpi_panel_on = false;  */
+        /* #ifdef CONFIG_PM_RUNTIME*/ 
+        /*      if (drm_psb_ospm && !enable_gfx_rtpm) { */
+        /*              pm_runtime_allow(&gpDrmDevice->pdev->dev); */
+        /*      schedule_delayed_work(&dev_priv->rtpm_work, 30 * 1000); */
+        /* } */
+        /*if (enable_gfx_rtpm) */
+        /*              pm_schedule_suspend(&dev->pdev->dev, gfxrtdelay); */
+        /* #endif */
+}
+
+void mdfld_dsi_dpi_set_power(struct drm_encoder *encoder, bool on)
+{
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_dpi_output *dpi_output = MDFLD_DSI_DPI_OUTPUT(dsi_encoder);
+        struct mdfld_dsi_config *dsi_config = mdfld_dsi_encoder_get_config(dsi_encoder);
+        int pipe = mdfld_dsi_encoder_get_pipe(dsi_encoder);
+        struct drm_device *dev = dsi_config->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 mipi_reg = MIPI;
+        u32 pipeconf_reg = PIPEACONF;
+        
+        if(pipe) {
+                mipi_reg = MIPI_C;
+                pipeconf_reg = PIPECCONF;
+        }
+        
+        /* Start up display island if it was shutdown */
+        if (!gma_power_begin(dev, true))
+                return;
+
+        if(on) {
+                if (mdfld_get_panel_type(dev, pipe) == TMD_VID){
+                        mdfld_dsi_dpi_turn_on(dpi_output, pipe);
+                } else {
+                        /* Enable mipi port */
+                        REG_WRITE(mipi_reg, (REG_READ(mipi_reg) | (1 << 31)));
+                        REG_READ(mipi_reg);
+
+                        mdfld_dsi_dpi_turn_on(dpi_output, pipe);
+                        mdfld_dsi_tpo_ic_init(dsi_config, pipe);
+                }
+
+                if(pipe == 2) {
+                        dev_priv->dpi_panel_on2 = true;
+                }
+                else {
+                        dev_priv->dpi_panel_on  = true;
+                }
+
+        } else {
+                if (mdfld_get_panel_type(dev, pipe) == TMD_VID) {
+                        mdfld_dsi_dpi_shut_down(dpi_output, pipe);
+                } else {
+                        mdfld_dsi_dpi_shut_down(dpi_output, pipe);
+                        /* Disable mipi port */
+                        REG_WRITE(mipi_reg, (REG_READ(mipi_reg) & ~(1<<31)));
+                        REG_READ(mipi_reg);
+                }
+
+                if(pipe == 2)
+                        dev_priv->dpi_panel_on2 = false;
+                else
+                        dev_priv->dpi_panel_on  = false;
+        }
+        gma_power_end(dev);
+}
+
+void mdfld_dsi_dpi_dpms(struct drm_encoder *encoder, int mode)
+{
+        dev_dbg(encoder->dev->dev, "DPMS %s\n",
+                        (mode == DRM_MODE_DPMS_ON ? "on":"off"));
+
+        if (mode == DRM_MODE_DPMS_ON)
+                mdfld_dsi_dpi_set_power(encoder, true);
+        else {
+                mdfld_dsi_dpi_set_power(encoder, false);
+#if 0 /* FIXME */
+#ifdef CONFIG_PM_RUNTIME
+                if (enable_gfx_rtpm)
+                        pm_schedule_suspend(&gpDrmDevice->pdev->dev, gfxrtdelay);
+#endif
+#endif
+        }
+}
+
+bool mdfld_dsi_dpi_mode_fixup(struct drm_encoder *encoder,
+                                     struct drm_display_mode *mode,
+                                     struct drm_display_mode *adjusted_mode)
+{
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_config *dsi_config = mdfld_dsi_encoder_get_config(dsi_encoder);
+        struct drm_display_mode *fixed_mode = dsi_config->fixed_mode;
+
+        if(fixed_mode) {
+                adjusted_mode->hdisplay = fixed_mode->hdisplay;
+                adjusted_mode->hsync_start = fixed_mode->hsync_start;
+                adjusted_mode->hsync_end = fixed_mode->hsync_end;
+                adjusted_mode->htotal = fixed_mode->htotal;
+                adjusted_mode->vdisplay = fixed_mode->vdisplay;
+                adjusted_mode->vsync_start = fixed_mode->vsync_start;
+                adjusted_mode->vsync_end = fixed_mode->vsync_end;
+                adjusted_mode->vtotal = fixed_mode->vtotal;
+                adjusted_mode->clock = fixed_mode->clock;
+                drm_mode_set_crtcinfo(adjusted_mode, CRTC_INTERLACE_HALVE_V);
+        }
+        
+        return true;
+}
+
+void mdfld_dsi_dpi_prepare(struct drm_encoder *encoder) 
+{
+        mdfld_dsi_dpi_set_power(encoder, false);
+}
+
+void mdfld_dsi_dpi_commit(struct drm_encoder *encoder) 
+{
+        mdfld_dsi_dpi_set_power(encoder, true);
+}
+
+void mdfld_dsi_dpi_mode_set(struct drm_encoder *encoder,
+                                   struct drm_display_mode *mode,
+                                   struct drm_display_mode *adjusted_mode)
+{
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_dpi_output *dpi_output = MDFLD_DSI_DPI_OUTPUT(dsi_encoder);
+        struct mdfld_dsi_config *dsi_config = mdfld_dsi_encoder_get_config(dsi_encoder);
+        struct drm_device *dev = dsi_config->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int pipe = mdfld_dsi_encoder_get_pipe(dsi_encoder);
+        
+        u32 pipeconf_reg = PIPEACONF;
+        u32 dspcntr_reg = DSPACNTR;
+        u32 mipi_reg = MIPI;
+        u32 reg_offset = 0;
+        
+        u32 pipeconf = dev_priv->pipeconf;
+        u32 dspcntr = dev_priv->dspcntr;
+        u32 mipi = MIPI_PORT_EN | PASS_FROM_SPHY_TO_AFE | SEL_FLOPPED_HSTX;
+        
+        dev_dbg(dev->dev, "set mode %dx%d on pipe %d\n",
+                                mode->hdisplay, mode->vdisplay, pipe);
+
+        if(pipe) {
+                pipeconf_reg = PIPECCONF;
+                dspcntr_reg = DSPCCNTR;
+                mipi_reg = MIPI_C;
+                reg_offset = MIPIC_REG_OFFSET;
+        } else {
+                mipi |= 2;
+        }
+        
+        if (!gma_power_begin(dev, true))
+                return;
+
+        /* Set up mipi port FIXME: do at init time */
+        REG_WRITE(mipi_reg, mipi);
+        REG_READ(mipi_reg);
+
+        /* Set up DSI controller DPI interface */
+        mdfld_dsi_dpi_controller_init(dsi_config, pipe);
+
+        if (mdfld_get_panel_type(dev, pipe) != TMD_VID) {
+                /* Turn on DPI interface */
+                mdfld_dsi_dpi_turn_on(dpi_output, pipe);
+        }
+        
+        /* Set up pipe */
+        REG_WRITE(pipeconf_reg, pipeconf);
+        REG_READ(pipeconf_reg);
+        
+        /* Set up display plane */
+        REG_WRITE(dspcntr_reg, dspcntr);
+        REG_READ(dspcntr_reg);
+        
+        msleep(20); /* FIXME: this should wait for vblank */
+        
+        dev_dbg(dev->dev, "State %x, power %d\n",
+                REG_READ(MIPIA_INTR_STAT_REG + reg_offset),
+                dpi_output->panel_on);
+
+        if (mdfld_get_panel_type(dev, pipe) != TMD_VID) {
+                /* Init driver ic */
+                mdfld_dsi_tpo_ic_init(dsi_config, pipe);
+                /* Init backlight */
+                mdfld_dsi_brightness_init(dsi_config, pipe);
+        }
+        gma_power_end(dev);
+}
+
+
+/*
+ * Init DSI DPI encoder. 
+ * Allocate an mdfld_dsi_encoder and attach it to given @dsi_connector
+ * return pointer of newly allocated DPI encoder, NULL on error
+ */ 
+struct mdfld_dsi_encoder *mdfld_dsi_dpi_init(struct drm_device *dev, 
+                                struct mdfld_dsi_connector *dsi_connector,
+                                struct panel_funcs *p_funcs)
+{
+        struct mdfld_dsi_dpi_output *dpi_output = NULL;
+        struct mdfld_dsi_config *dsi_config;
+        struct drm_connector *connector = NULL;
+        struct drm_encoder *encoder = NULL;
+        struct drm_display_mode *fixed_mode = NULL;
+        int pipe;
+        u32 data;
+        int ret;
+
+        if (!dsi_connector || !p_funcs) {
+                WARN_ON(1);
+                return NULL;
+        }
+
+        dsi_config = mdfld_dsi_get_config(dsi_connector);
+        pipe = dsi_connector->pipe;
+
+        /* Panel hard-reset */
+        if (p_funcs->reset) {
+                ret = p_funcs->reset(pipe);
+                if (ret) {
+                        DRM_ERROR("Panel %d hard-reset failed\n", pipe);
+                        return NULL;
+                }
+        }
+
+        /* Panel drvIC init */
+        if (p_funcs->drv_ic_init)
+                p_funcs->drv_ic_init(dsi_config, pipe);
+
+        /* Panel power mode detect */
+        ret = mdfld_dsi_get_power_mode(dsi_config,
+                                        &data,
+                                        MDFLD_DSI_LP_TRANSMISSION);
+        if (ret) {
+                DRM_ERROR("Panel %d get power mode failed\n", pipe);
+                dsi_connector->status = connector_status_disconnected;
+        } else {
+                DRM_INFO("pipe %d power mode 0x%x\n", pipe, data);
+                dsi_connector->status = connector_status_connected;
+        }
+
+        dpi_output = kzalloc(sizeof(struct mdfld_dsi_dpi_output), GFP_KERNEL);
+        if(!dpi_output) {
+                dev_err(dev->dev, "No memory for dsi_dpi_output\n");
+                return NULL;
+        }
+
+        if(dsi_connector->pipe) 
+                dpi_output->panel_on = 0;
+        else
+                dpi_output->panel_on = 0;
+        
+        dpi_output->dev = dev;
+        dpi_output->p_funcs = p_funcs;
+        dpi_output->first_boot = 1;
+        
+        /* Get fixed mode */
+        dsi_config = mdfld_dsi_get_config(dsi_connector);
+        fixed_mode = dsi_config->fixed_mode;
+        
+        /* Create drm encoder object */
+        connector = &dsi_connector->base.base;
+        encoder = &dpi_output->base.base;
+        /*
+         * On existing hardware this will be a panel of some form,
+         * if future devices also have HDMI bridges this will need
+         * revisiting
+         */
+        drm_encoder_init(dev,
+                        encoder,
+                        p_funcs->encoder_funcs,
+                        DRM_MODE_ENCODER_LVDS);
+        drm_encoder_helper_add(encoder,
+                                p_funcs->encoder_helper_funcs);
+        
+        /* Attach to given connector */
+        drm_mode_connector_attach_encoder(connector, encoder);
+        
+        /* Set possible crtcs and clones */
+        if(dsi_connector->pipe) {
+                encoder->possible_crtcs = (1 << 2);
+                encoder->possible_clones = (1 << 1);
+        } else {
+                encoder->possible_crtcs = (1 << 0);
+                encoder->possible_clones = (1 << 0);
+        }
+        return &dpi_output->base;
+}
+
diff --git a/drivers/staging/gma500/mdfld_dsi_dpi.h b/drivers/staging/gma500/mdfld_dsi_dpi.h
new file mode 100644
index 00000000000..ed92d45ee74
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_dsi_dpi.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright © 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:
+ * jim liu <jim.liu@intel.com>
+ * Jackie Li<yaodong.li@intel.com>
+ */
+
+#ifndef __MDFLD_DSI_DPI_H__
+#define __MDFLD_DSI_DPI_H__
+
+#include "mdfld_dsi_output.h"
+#include "mdfld_output.h"
+
+struct mdfld_dsi_dpi_timing {
+        u16 hsync_count;
+        u16 hbp_count;
+        u16 hfp_count;
+        u16 hactive_count;
+        u16 vsync_count;
+        u16 vbp_count;
+        u16 vfp_count;
+};
+
+struct mdfld_dsi_dpi_output {
+        struct mdfld_dsi_encoder base;
+        struct drm_device *dev;
+
+        int panel_on;
+        int first_boot;
+
+        struct panel_funcs *p_funcs;
+};
+
+#define MDFLD_DSI_DPI_OUTPUT(dsi_encoder) \
+        container_of(dsi_encoder, struct mdfld_dsi_dpi_output, base)
+
+extern int mdfld_dsi_dpi_timing_calculation(struct drm_display_mode *mode,
+                        struct mdfld_dsi_dpi_timing *dpi_timing,
+                        int num_lane, int bpp);
+extern struct mdfld_dsi_encoder *mdfld_dsi_dpi_init(struct drm_device *dev,
+                        struct mdfld_dsi_connector *dsi_connector,
+                        struct panel_funcs *p_funcs);
+
+/* Medfield DPI helper functions */
+extern void mdfld_dsi_dpi_dpms(struct drm_encoder *encoder, int mode);
+extern bool mdfld_dsi_dpi_mode_fixup(struct drm_encoder *encoder,
+                        struct drm_display_mode *mode,
+                        struct drm_display_mode *adjusted_mode);
+extern void mdfld_dsi_dpi_prepare(struct drm_encoder *encoder);
+extern void mdfld_dsi_dpi_commit(struct drm_encoder *encoder);
+extern void mdfld_dsi_dpi_mode_set(struct drm_encoder *encoder,
+                        struct drm_display_mode *mode,
+                        struct drm_display_mode *adjusted_mode);
+extern void mdfld_dsi_dpi_turn_on(struct mdfld_dsi_dpi_output *output,
+                        int pipe);
+extern void mdfld_dsi_dpi_controller_init(struct mdfld_dsi_config *si_config,
+                        int pipe);
+#endif /*__MDFLD_DSI_DPI_H__*/
diff --git a/drivers/staging/gma500/mdfld_dsi_output.c b/drivers/staging/gma500/mdfld_dsi_output.c
new file mode 100644
index 00000000000..9050c0f78b1
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_dsi_output.c
@@ -0,0 +1,1013 @@
+/*
+ * Copyright © 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:
+ * jim liu <jim.liu@intel.com>
+ * Jackie Li<yaodong.li@intel.com>
+ */
+
+#include "mdfld_dsi_output.h"
+#include "mdfld_dsi_dbi.h"
+#include "mdfld_dsi_dpi.h"
+#include "mdfld_output.h"
+#include <asm/intel_scu_ipc.h>
+#include "mdfld_dsi_pkg_sender.h"
+#include <linux/pm_runtime.h>
+
+#define MDFLD_DSI_BRIGHTNESS_MAX_LEVEL 100
+
+static int CABC_control = 1;
+static int LABC_control = 1;
+
+module_param (CABC_control, int, 0644);
+module_param (LABC_control, int, 0644);
+
+/**
+ * make these MCS command global 
+ * we don't need 'movl' everytime we send them.
+ * FIXME: these datas were provided by OEM, we should get them from GCT.
+ **/
+static u32 mdfld_dbi_mcs_hysteresis[] = {
+        0x42000f57, 0x8c006400, 0xff00bf00, 0xffffffff,
+        0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff,
+        0x38000aff, 0x82005000, 0xff00ab00, 0xffffffff,
+        0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff,
+        0x000000ff,
+};
+
+static u32 mdfld_dbi_mcs_display_profile[] = {
+        0x50281450, 0x0000c882, 0x00000000, 0x00000000,
+        0x00000000,
+};
+
+static u32 mdfld_dbi_mcs_kbbc_profile[] = {
+        0x00ffcc60, 0x00000000, 0x00000000, 0x00000000,
+}; 
+        
+static u32 mdfld_dbi_mcs_gamma_profile[] = {
+        0x81111158, 0x88888888, 0x88888888,
+}; 
+
+/*
+ * write hysteresis values.
+ */
+static void mdfld_dsi_write_hysteresis (struct mdfld_dsi_config *dsi_config,
+                                                                int pipe)
+{
+        struct mdfld_dsi_pkg_sender *sender = mdfld_dsi_get_pkg_sender(dsi_config);
+
+        if(!sender) {
+                WARN_ON(1);
+                return;
+        }
+        mdfld_dsi_send_mcs_long_hs(sender,
+                                   mdfld_dbi_mcs_hysteresis,
+                                   17,
+                                   MDFLD_DSI_SEND_PACKAGE);
+}
+
+/*
+ * write display profile values.
+ */
+static void mdfld_dsi_write_display_profile(struct mdfld_dsi_config *dsi_config, int pipe)
+{
+        struct mdfld_dsi_pkg_sender *sender = mdfld_dsi_get_pkg_sender(dsi_config);
+
+        if(!sender) {
+                WARN_ON(1);
+                return;
+        }
+        mdfld_dsi_send_mcs_long_hs(sender,
+                                   mdfld_dbi_mcs_display_profile,
+                                   5,
+                                   MDFLD_DSI_SEND_PACKAGE);
+}
+
+/*
+ * write KBBC profile values.
+ */
+static void mdfld_dsi_write_kbbc_profile (struct mdfld_dsi_config * dsi_config, int pipe)
+{
+        struct mdfld_dsi_pkg_sender *sender = mdfld_dsi_get_pkg_sender(dsi_config);
+
+        if(!sender) {
+                WARN_ON(1);
+                return;
+        }
+        mdfld_dsi_send_mcs_long_hs(sender,
+                                   mdfld_dbi_mcs_kbbc_profile,
+                                   4,
+                                   MDFLD_DSI_SEND_PACKAGE);
+}
+
+/*
+ * write gamma setting.
+ */
+static void mdfld_dsi_write_gamma_setting (struct mdfld_dsi_config *dsi_config, int pipe)
+{
+        struct mdfld_dsi_pkg_sender *sender = mdfld_dsi_get_pkg_sender(dsi_config);
+
+        if(!sender) {
+                WARN_ON(1);
+                return;
+        }
+        mdfld_dsi_send_mcs_long_hs(sender,
+                                   mdfld_dbi_mcs_gamma_profile,
+                                   3,
+                                   MDFLD_DSI_SEND_PACKAGE);
+}
+
+/*
+ * Check and see if the generic control or data buffer is empty and ready.
+ */
+void mdfld_dsi_gen_fifo_ready (struct drm_device *dev, u32 gen_fifo_stat_reg, u32 fifo_stat)
+{
+        u32 GEN_BF_time_out_count = 0;
+        
+        /* Check MIPI Adatper command registers */
+        for (GEN_BF_time_out_count = 0; GEN_BF_time_out_count < GEN_FB_TIME_OUT; GEN_BF_time_out_count++)
+        {
+                if ((REG_READ(gen_fifo_stat_reg) & fifo_stat) == fifo_stat)
+                        break;
+                udelay (100);
+        }
+
+        if (GEN_BF_time_out_count == GEN_FB_TIME_OUT)
+                dev_err(dev->dev,
+        "mdfld_dsi_gen_fifo_ready, Timeout. gen_fifo_stat_reg = 0x%x. \n",
+                                                gen_fifo_stat_reg);
+}
+
+/*
+ * Manage the DSI MIPI keyboard and display brightness.
+ * FIXME: this is exported to OSPM code. should work out an specific 
+ * display interface to OSPM. 
+ */
+void mdfld_dsi_brightness_init(struct mdfld_dsi_config *dsi_config, int pipe)
+{
+        struct mdfld_dsi_pkg_sender *sender = mdfld_dsi_get_pkg_sender(dsi_config);
+        struct drm_device *dev = sender->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 gen_ctrl_val;
+        
+        if(!sender) {
+                WARN_ON(1);
+                return;
+        }
+        /* Set default display backlight value to 85% (0xd8)*/
+        mdfld_dsi_send_mcs_short_hs(sender,
+                                    write_display_brightness,
+                                    0xd8,
+                                    1,
+                                    MDFLD_DSI_SEND_PACKAGE);
+
+        /* Set minimum brightness setting of CABC function to 20% (0x33)*/
+        mdfld_dsi_send_mcs_short_hs(sender,
+                                    write_cabc_min_bright,
+                                    0x33,
+                                    1,
+                                    MDFLD_DSI_SEND_PACKAGE);
+
+        mdfld_dsi_write_hysteresis(dsi_config, pipe);
+        mdfld_dsi_write_display_profile (dsi_config, pipe);
+        mdfld_dsi_write_kbbc_profile (dsi_config, pipe);
+        mdfld_dsi_write_gamma_setting (dsi_config, pipe);
+
+        /* Enable backlight or/and LABC */
+        gen_ctrl_val = BRIGHT_CNTL_BLOCK_ON | DISPLAY_DIMMING_ON| BACKLIGHT_ON;
+        if (LABC_control == 1 || CABC_control == 1)
+                gen_ctrl_val |= DISPLAY_DIMMING_ON| DISPLAY_BRIGHTNESS_AUTO | GAMMA_AUTO;
+
+        if (LABC_control == 1)
+                gen_ctrl_val |= AMBIENT_LIGHT_SENSE_ON;
+
+        dev_priv->mipi_ctrl_display = gen_ctrl_val;
+
+        mdfld_dsi_send_mcs_short_hs(sender,
+                                    write_ctrl_display,
+                                    (u8)gen_ctrl_val,
+                                    1,
+                                    MDFLD_DSI_SEND_PACKAGE);
+
+        if (CABC_control == 0)
+                return;
+        mdfld_dsi_send_mcs_short_hs(sender,
+                                    write_ctrl_cabc,
+                                    UI_IMAGE,
+                                    1,
+                                    MDFLD_DSI_SEND_PACKAGE);
+}
+
+/*
+ * Manage the mipi display brightness.
+ * TODO: refine this interface later
+ */
+void mdfld_dsi_brightness_control(struct drm_device *dev, int pipe, int level)
+{
+        struct mdfld_dsi_pkg_sender *sender;
+        struct drm_psb_private *dev_priv;
+        struct mdfld_dsi_config *dsi_config;
+        u32 gen_ctrl_val;
+        int p_type;     
+        
+        if (!dev || (pipe != 0 && pipe != 2)) {
+                dev_err(dev->dev, "Invalid parameter\n");
+                return;
+        }
+
+        p_type = mdfld_get_panel_type(dev, 0);
+
+        dev_priv = dev->dev_private;
+
+        if(pipe)
+                dsi_config = dev_priv->dsi_configs[1];
+        else
+                dsi_config = dev_priv->dsi_configs[0];
+
+        sender = mdfld_dsi_get_pkg_sender(dsi_config);
+
+        if(!sender) {
+                WARN_ON(1);
+                return;
+        }
+
+        gen_ctrl_val = ((level * 0xff) / MDFLD_DSI_BRIGHTNESS_MAX_LEVEL) & 0xff;
+
+        dev_dbg(dev->dev,
+                "pipe = %d, gen_ctrl_val = %d.  \n", pipe, gen_ctrl_val);
+        
+        if(p_type == TMD_VID || p_type == TMD_CMD){
+                /* Set display backlight value */
+                mdfld_dsi_send_mcs_short_hs(sender, 
+                                        tmd_write_display_brightness, 
+                                        (u8)gen_ctrl_val, 
+                                         1, 
+                                        MDFLD_DSI_SEND_PACKAGE);                
+        } else {                        
+                /* Set display backlight value */
+                mdfld_dsi_send_mcs_short_hs(sender,
+                                    write_display_brightness,
+                                    (u8)gen_ctrl_val,
+                                    1,
+                                    MDFLD_DSI_SEND_PACKAGE);
+
+
+                /* Enable backlight control */
+                if (level == 0)
+                        gen_ctrl_val = 0;
+                else 
+                        gen_ctrl_val = dev_priv->mipi_ctrl_display;
+
+                mdfld_dsi_send_mcs_short_hs(sender,
+                                    write_ctrl_display,
+                                   (u8)gen_ctrl_val,
+                                   1,
+                                   MDFLD_DSI_SEND_PACKAGE);
+        }
+}
+
+/*
+ * shut down DSI controller
+ */ 
+void mdfld_dsi_controller_shutdown(struct mdfld_dsi_config * dsi_config, int pipe)
+{
+        struct drm_device * dev;
+        u32 reg_offset = pipe ? MIPIC_REG_OFFSET : 0;
+        int retry = 100;
+        
+        if (!dsi_config) {
+                WARN_ON(1);
+                return;
+        }
+        
+        dev = dsi_config->dev;
+        
+        if (!gma_power_begin(dev, true)) {
+                dev_err(dev->dev, "hw begin failed\n");
+                return;
+        }
+                
+        if(!(REG_READ(MIPIA_DEVICE_READY_REG + reg_offset) &  DSI_DEVICE_READY)) 
+                goto shutdown_out;
+        
+        /* Send shut down package, clean packet send bit first */
+        if(REG_READ(MIPIA_INTR_STAT_REG + reg_offset) & DSI_INTR_STATE_SPL_PKG_SENT) {
+                REG_WRITE((MIPIA_INTR_STAT_REG + reg_offset), 
+                                (REG_READ(MIPIA_INTR_STAT_REG + reg_offset) | DSI_INTR_STATE_SPL_PKG_SENT));
+        }
+        
+        /*send shut down package in HS*/
+        REG_WRITE((MIPIA_DPI_CONTROL_REG + reg_offset), DSI_DPI_CTRL_HS_SHUTDOWN);
+        
+        
+        /*
+         * make sure shut down is sent.
+         * FIXME: add max retry counter
+         */
+        while(!(REG_READ(MIPIA_INTR_STAT_REG + reg_offset) & DSI_INTR_STATE_SPL_PKG_SENT)) {
+                retry--;
+                
+                if(!retry) {
+                        dev_err(dev->dev, "timeout\n");
+                        break;
+                }
+        }
+        
+        /*sleep 1 ms to ensure shutdown finished*/
+        msleep(100);
+        
+        /*un-ready device*/
+        REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset),
+                           (REG_READ(MIPIA_DEVICE_READY_REG + reg_offset) & ~DSI_DEVICE_READY));
+
+shutdown_out:                      
+        gma_power_end(dev);
+}
+
+void mdfld_dsi_controller_startup(struct mdfld_dsi_config * dsi_config, int pipe)
+{
+        struct drm_device * dev;
+        u32 reg_offset = pipe ? MIPIC_REG_OFFSET : 0;
+        int retry = 100;
+        
+        
+        if (!dsi_config) {
+                WARN_ON(1);
+                return;
+        }
+        
+        dev = dsi_config->dev;
+        dev_dbg(dev->dev, "starting up DSI controller on pipe %d...\n", pipe);
+        
+        if (!gma_power_begin(dev, true)) {
+                dev_err(dev->dev, "hw begin failed\n");
+                return;
+        }
+        
+        if((REG_READ(MIPIA_DEVICE_READY_REG + reg_offset) & DSI_DEVICE_READY)) 
+                goto startup_out;
+        
+        /*if config DPI, turn on DPI interface*/
+        if(dsi_config->type == MDFLD_DSI_ENCODER_DPI) {
+                if(REG_READ(MIPIA_INTR_STAT_REG + reg_offset) & DSI_INTR_STATE_SPL_PKG_SENT) {
+                        REG_WRITE((MIPIA_INTR_STAT_REG + reg_offset), DSI_INTR_STATE_SPL_PKG_SENT);
+                }
+                
+                REG_WRITE((MIPIA_DPI_CONTROL_REG + reg_offset), DSI_DPI_CTRL_HS_TURN_ON);
+                
+                /*
+                 * make sure shut down is sent.
+                 * FIXME: add max retry counter
+                 */
+                while(!(REG_READ(MIPIA_INTR_STAT_REG + reg_offset) & DSI_INTR_STATE_SPL_PKG_SENT)) {
+                        retry--;
+                        if(!retry) {
+                                dev_err(dev->dev, "timeout\n");
+                                break;
+                        }
+                }
+                
+                msleep(100);
+        }
+        
+        /*set device ready*/
+        REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset),
+                           (REG_READ(MIPIA_DEVICE_READY_REG + reg_offset) | DSI_DEVICE_READY));
+
+startup_out:    
+        gma_power_end(dev);
+}
+
+
+static int mdfld_dsi_get_panel_status(struct mdfld_dsi_config *dsi_config,
+                                        u8 dcs,
+                                        u32 *data,
+                                        u8 transmission)
+{
+        struct mdfld_dsi_pkg_sender *sender
+                = mdfld_dsi_get_pkg_sender(dsi_config);
+
+        if (!sender || !data) {
+                DRM_ERROR("Invalid parameter\n");
+                return -EINVAL;
+        }
+
+        if (transmission == MDFLD_DSI_HS_TRANSMISSION)
+                return mdfld_dsi_read_mcs_hs(sender, dcs, data, 1);
+        else if (transmission == MDFLD_DSI_LP_TRANSMISSION)
+                return mdfld_dsi_read_mcs_lp(sender, dcs, data, 1);
+        else
+                return -EINVAL;
+}
+
+int mdfld_dsi_get_power_mode(struct mdfld_dsi_config *dsi_config,
+                                u32 *mode,
+                                u8 transmission)
+{
+        if (!dsi_config || !mode) {
+                DRM_ERROR("Invalid parameter\n");
+                return -EINVAL;
+        }
+
+        return mdfld_dsi_get_panel_status(dsi_config, 0x0a, mode, transmission);
+}
+
+int mdfld_dsi_get_diagnostic_result(struct mdfld_dsi_config *dsi_config,
+                                        u32 *result,
+                                        u8 transmission)
+{
+        if (!dsi_config || !result) {
+                DRM_ERROR("Invalid parameter\n");
+                return -EINVAL;
+        }
+
+        return mdfld_dsi_get_panel_status(dsi_config, 0x0f, result,
+                                          transmission);
+}
+
+/*
+ * NOTE: this function was used by OSPM.
+ * TODO: will be removed later, should work out display interfaces for OSPM
+ */
+void mdfld_dsi_controller_init(struct mdfld_dsi_config * dsi_config, int pipe)
+{
+        if(!dsi_config || ((pipe != 0) && (pipe != 2))) {
+                WARN_ON(1);
+                return;
+        }
+
+        if(dsi_config->type)
+                mdfld_dsi_dpi_controller_init(dsi_config, pipe);
+        else
+                mdfld_dsi_controller_dbi_init(dsi_config, pipe);
+}
+
+static void mdfld_dsi_connector_save(struct drm_connector * connector)
+{
+}
+
+static void mdfld_dsi_connector_restore(struct drm_connector * connector)
+{
+}
+
+static enum drm_connector_status mdfld_dsi_connector_detect(struct drm_connector * connector, bool force)
+{
+        struct psb_intel_output *psb_output
+                                        = to_psb_intel_output(connector);
+        struct mdfld_dsi_connector *dsi_connector
+                                        = MDFLD_DSI_CONNECTOR(psb_output);
+        return dsi_connector->status;
+}
+
+static int mdfld_dsi_connector_set_property(struct drm_connector *connector,
+                                        struct drm_property *property,
+                                        uint64_t value)
+{
+        struct drm_encoder *encoder = connector->encoder;
+
+        if (!strcmp(property->name, "scaling mode") && encoder) {
+                struct psb_intel_crtc * psb_crtc = to_psb_intel_crtc(encoder->crtc);
+                bool bTransitionFromToCentered;
+                uint64_t curValue;
+
+                if (!psb_crtc)
+                        goto set_prop_error;
+
+                switch (value) {
+                case DRM_MODE_SCALE_FULLSCREEN:
+                        break;
+                case DRM_MODE_SCALE_NO_SCALE:
+                        break;
+                case DRM_MODE_SCALE_ASPECT:
+                        break;
+                default:
+                        goto set_prop_error;
+                }
+
+                if (drm_connector_property_get_value(connector, property, &curValue))
+                        goto set_prop_error;
+
+                if (curValue == value)
+                        goto set_prop_done;
+
+                if (drm_connector_property_set_value(connector, property, value))
+                        goto set_prop_error;
+
+                bTransitionFromToCentered = (curValue == DRM_MODE_SCALE_NO_SCALE) ||
+                        (value == DRM_MODE_SCALE_NO_SCALE);
+
+                if (psb_crtc->saved_mode.hdisplay != 0 &&
+                    psb_crtc->saved_mode.vdisplay != 0) {
+                        if (bTransitionFromToCentered) {
+                                if (!drm_crtc_helper_set_mode(encoder->crtc, &psb_crtc->saved_mode,
+                                            encoder->crtc->x, encoder->crtc->y, encoder->crtc->fb))
+                                        goto set_prop_error;
+                        } else {
+                                struct drm_encoder_helper_funcs *pEncHFuncs  = encoder->helper_private;
+                                pEncHFuncs->mode_set(encoder, &psb_crtc->saved_mode,
+                                                     &psb_crtc->saved_adjusted_mode);
+                        }
+                }
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+        } else if (!strcmp(property->name, "backlight") && encoder) {
+                struct drm_psb_private *dev_priv = encoder->dev->dev_private;
+                struct backlight_device *psb_bd = dev_priv->backlight_device;
+                dev_dbg(encoder->dev->dev, "backlight level = %d\n", (int)value);
+                if (drm_connector_property_set_value(connector, property, value))
+                        goto set_prop_error;
+                else {
+                        dev_dbg(encoder->dev->dev,
+                                        "set brightness to %d", (int)value);
+                        if (psb_bd) {
+                                psb_bd->props.brightness = value;
+                                backlight_update_status(psb_bd);
+                        }
+                }
+#endif
+        }
+set_prop_done:
+    return 0;
+set_prop_error:
+    return -1;
+}
+
+static void mdfld_dsi_connector_destroy(struct drm_connector *connector)
+{
+        struct psb_intel_output * psb_output = to_psb_intel_output(connector);
+        struct mdfld_dsi_connector * dsi_connector = MDFLD_DSI_CONNECTOR(psb_output);
+        struct mdfld_dsi_pkg_sender * sender;
+        
+        if(!dsi_connector)
+                return;
+        
+        drm_sysfs_connector_remove(connector);
+        drm_connector_cleanup(connector);
+        
+        sender = dsi_connector->pkg_sender;
+
+        mdfld_dsi_pkg_sender_destroy(sender);
+
+        kfree(dsi_connector);
+}
+
+static int mdfld_dsi_connector_get_modes(struct drm_connector * connector)
+{
+        struct psb_intel_output * psb_output = to_psb_intel_output(connector);
+        struct mdfld_dsi_connector * dsi_connector = MDFLD_DSI_CONNECTOR(psb_output);
+        struct mdfld_dsi_config * dsi_config = mdfld_dsi_get_config(dsi_connector);
+        struct drm_display_mode * fixed_mode = dsi_config->fixed_mode;
+        struct drm_display_mode * dup_mode = NULL;
+        struct drm_device * dev = connector->dev;
+        
+        connector->display_info.min_vfreq = 0;
+        connector->display_info.max_vfreq = 200;
+        connector->display_info.min_hfreq = 0;
+        connector->display_info.max_hfreq = 200;
+
+        if(fixed_mode) {
+                dev_dbg(dev->dev, "fixed_mode %dx%d\n",
+                        fixed_mode->hdisplay, fixed_mode->vdisplay);
+                
+                dup_mode = drm_mode_duplicate(dev, fixed_mode);
+                drm_mode_probed_add(connector, dup_mode);
+                return 1;
+        }
+        dev_err(dev->dev, "Didn't get any modes!\n");
+        return 0;
+}
+
+static int mdfld_dsi_connector_mode_valid(struct drm_connector * connector, struct drm_display_mode * mode)
+{
+        struct psb_intel_output * psb_output = to_psb_intel_output(connector);
+        struct mdfld_dsi_connector * dsi_connector = MDFLD_DSI_CONNECTOR(psb_output);
+        struct mdfld_dsi_config * dsi_config = mdfld_dsi_get_config(dsi_connector);
+        struct drm_display_mode * fixed_mode = dsi_config->fixed_mode;
+
+        dev_dbg(connector->dev->dev, "mode %p, fixed mode %p\n",
+                                                        mode, fixed_mode);
+
+        if(mode->flags & DRM_MODE_FLAG_DBLSCAN) 
+                return MODE_NO_DBLESCAN;
+
+        if(mode->flags & DRM_MODE_FLAG_INTERLACE)
+                return MODE_NO_INTERLACE;
+
+        /**
+         * FIXME: current DC has no fitting unit, reject any mode setting request
+         * will figure out a way to do up-scaling(pannel fitting) later.  
+         **/
+        if(fixed_mode) {
+                if(mode->hdisplay != fixed_mode->hdisplay)
+                        return MODE_PANEL;
+
+                if(mode->vdisplay != fixed_mode->vdisplay)
+                        return MODE_PANEL;
+        }
+        dev_dbg(connector->dev->dev, "mode ok\n");
+
+        return MODE_OK;
+}
+
+static void mdfld_dsi_connector_dpms(struct drm_connector *connector, int mode)
+{
+#ifdef CONFIG_PM_RUNTIME
+        struct drm_device * dev = connector->dev;
+        struct drm_psb_private * dev_priv = dev->dev_private;
+        bool panel_on, panel_on2;
+#endif
+        /* First, execute DPMS */
+        drm_helper_connector_dpms(connector, mode);
+
+#ifdef CONFIG_PM_RUNTIME
+        if(mdfld_panel_dpi(dev)) {
+                /* DPI panel */
+                panel_on = dev_priv->dpi_panel_on;
+                panel_on2 = dev_priv->dpi_panel_on2;
+        } else {
+                /* DBI panel */
+                panel_on = dev_priv->dbi_panel_on;
+                panel_on2 = dev_priv->dbi_panel_on2;
+        }
+
+        /* Then check all display panels + monitors status */
+        /* Make sure that the Display (B) sub-system status isn't i3 when
+         * R/W the DC register, otherwise "Fabric error" issue would occur
+         * during S0i3 state. */
+        if(!panel_on && !panel_on2 && !(REG_READ(HDMIB_CONTROL)
+                                                & HDMIB_PORT_EN)) {
+                /* Request rpm idle */
+                if(dev_priv->rpm_enabled)
+                        pm_request_idle(&dev->pdev->dev);
+        }
+        /*
+         * if rpm wasn't enabled yet, try to allow it
+         * FIXME: won't enable rpm for DPI since DPI
+         * CRTC setting is a little messy now.
+         * Enable it later!
+         */
+#if 0
+        if(!dev_priv->rpm_enabled && !mdfld_panel_dpi(dev))
+                ospm_runtime_pm_allow(dev);
+#endif
+#endif
+}
+
+static struct drm_encoder *mdfld_dsi_connector_best_encoder(
+                                        struct drm_connector *connector) 
+{
+        struct psb_intel_output * psb_output = to_psb_intel_output(connector);
+        struct mdfld_dsi_connector * dsi_connector = MDFLD_DSI_CONNECTOR(psb_output);
+        struct mdfld_dsi_config * dsi_config = mdfld_dsi_get_config(dsi_connector);
+        struct mdfld_dsi_encoder * encoder = NULL;
+        
+        if(dsi_config->type == MDFLD_DSI_ENCODER_DBI) 
+                encoder = dsi_config->encoders[MDFLD_DSI_ENCODER_DBI];
+        else if (dsi_config->type == MDFLD_DSI_ENCODER_DPI) 
+                encoder = dsi_config->encoders[MDFLD_DSI_ENCODER_DPI];
+        
+        dev_dbg(connector->dev->dev, "get encoder %p\n", encoder);
+        
+        if(!encoder) {
+                dev_err(connector->dev->dev,
+                        "Invalid encoder for type %d\n", dsi_config->type);
+                return NULL;
+        }
+        dsi_config->encoder = encoder;  
+        return &encoder->base;  
+}
+
+/* DSI connector funcs */
+static const struct drm_connector_funcs mdfld_dsi_connector_funcs = {
+        .dpms = /*drm_helper_connector_dpms*/mdfld_dsi_connector_dpms,
+        .save = mdfld_dsi_connector_save,
+        .restore = mdfld_dsi_connector_restore,
+        .detect = mdfld_dsi_connector_detect,
+        .fill_modes = drm_helper_probe_single_connector_modes,
+        .set_property = mdfld_dsi_connector_set_property,
+        .destroy = mdfld_dsi_connector_destroy,
+};
+
+/* DSI connector helper funcs */
+static const struct drm_connector_helper_funcs mdfld_dsi_connector_helper_funcs = {
+        .get_modes = mdfld_dsi_connector_get_modes,
+        .mode_valid = mdfld_dsi_connector_mode_valid,
+        .best_encoder = mdfld_dsi_connector_best_encoder,
+};
+
+static int mdfld_dsi_get_default_config(struct drm_device * dev, 
+                                                                                struct mdfld_dsi_config * config, int pipe)
+{
+        if(!dev || !config) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+        
+        config->bpp = 24;
+        config->type = mdfld_panel_dpi(dev);
+        config->lane_count = 2;
+        config->channel_num = 0;
+        /*NOTE: video mode is ignored when type is MDFLD_DSI_ENCODER_DBI*/
+        if (mdfld_get_panel_type(dev, pipe) == TMD_VID) {
+                config->video_mode = MDFLD_DSI_VIDEO_NON_BURST_MODE_SYNC_PULSE;
+        } else {
+                config->video_mode = MDFLD_DSI_VIDEO_BURST_MODE;
+        }
+        
+        return 0;
+}
+
+/*
+ * Returns the panel fixed mode from configuration. 
+ */
+struct drm_display_mode *
+mdfld_dsi_get_configuration_mode(struct mdfld_dsi_config * dsi_config, int pipe)
+{
+        struct drm_device *dev = dsi_config->dev;
+        struct drm_display_mode *mode;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_timing_info *ti = &dev_priv->gct_data.DTD;
+        bool use_gct = false;
+
+        mode = kzalloc(sizeof(*mode), GFP_KERNEL);
+        if (!mode) {
+                dev_err(dev->dev, "Out of memory for mode\n");
+                return NULL;
+        }
+        if (use_gct) {
+                dev_dbg(dev->dev, "gct find MIPI panel.\n");
+
+                mode->hdisplay = (ti->hactive_hi << 8) | ti->hactive_lo;
+                mode->vdisplay = (ti->vactive_hi << 8) | ti->vactive_lo;
+                mode->hsync_start = mode->hdisplay + \
+                                ((ti->hsync_offset_hi << 8) | \
+                                ti->hsync_offset_lo);
+                mode->hsync_end = mode->hsync_start + \
+                                ((ti->hsync_pulse_width_hi << 8) | \
+                                ti->hsync_pulse_width_lo);
+                mode->htotal = mode->hdisplay + ((ti->hblank_hi << 8) | \
+                                                                ti->hblank_lo);
+                mode->vsync_start = \
+                        mode->vdisplay + ((ti->vsync_offset_hi << 8) | \
+                                                ti->vsync_offset_lo);
+                mode->vsync_end = \
+                        mode->vsync_start + ((ti->vsync_pulse_width_hi << 8) | \
+                                                ti->vsync_pulse_width_lo);
+                mode->vtotal = mode->vdisplay + \
+                                ((ti->vblank_hi << 8) | ti->vblank_lo);
+                mode->clock = ti->pixel_clock * 10;
+        } else {
+                if(dsi_config->type == MDFLD_DSI_ENCODER_DPI) { 
+                        if (mdfld_get_panel_type(dev, pipe) == TMD_VID) {
+                                mode->hdisplay = 480;
+                                mode->vdisplay = 854;
+                                mode->hsync_start = 487;
+                                mode->hsync_end = 490;
+                                mode->htotal = 499;
+                                mode->vsync_start = 861;
+                                mode->vsync_end = 865;
+                                mode->vtotal = 873;
+                                mode->clock = 33264;
+                        } else {
+                                mode->hdisplay = 864;
+                                mode->vdisplay = 480;
+                                mode->hsync_start = 873;
+                                mode->hsync_end = 876;
+                                mode->htotal = 887;
+                                mode->vsync_start = 487;
+                                mode->vsync_end = 490;
+                                mode->vtotal = 499;
+                                mode->clock = 33264;
+                        }
+                } else if(dsi_config->type == MDFLD_DSI_ENCODER_DBI) {
+                        mode->hdisplay = 864;
+                        mode->vdisplay = 480;
+                        mode->hsync_start = 872;
+                        mode->hsync_end = 876;
+                        mode->htotal = 884;
+                        mode->vsync_start = 482;
+                        mode->vsync_end = 494;
+                        mode->vtotal = 486;
+                        mode->clock = 25777;
+                        
+                }
+        }
+
+        drm_mode_set_name(mode);
+        drm_mode_set_crtcinfo(mode, 0);
+        
+        mode->type |= DRM_MODE_TYPE_PREFERRED;
+
+        return mode;
+}
+
+int mdfld_dsi_panel_reset(int pipe)
+{
+        unsigned gpio;
+        int ret = 0;
+
+        switch (pipe) {
+        case 0:
+                gpio = 128;
+                break;
+        case 2:
+                gpio = 34;
+                break;
+        default:
+                DRM_ERROR("Invalid output\n");
+                return -EINVAL;
+        }
+
+        ret = gpio_request(gpio, "gfx");
+        if (ret) {
+                DRM_ERROR("gpio_rqueset failed\n");
+                return ret;
+        }
+
+        ret = gpio_direction_output(gpio, 1);
+        if (ret) {
+                DRM_ERROR("gpio_direction_output failed\n");
+                goto gpio_error;
+        }
+
+        gpio_get_value(128);
+
+gpio_error:
+        if (gpio_is_valid(gpio))
+                gpio_free(gpio);
+
+        return ret;
+}
+
+/*
+ * MIPI output init
+ * @dev drm device
+ * @pipe pipe number. 0 or 2
+ * @config 
+ * 
+ * Do the initialization of a MIPI output, including create DRM mode objects
+ * initialization of DSI output on @pipe 
+ */
+void mdfld_dsi_output_init(struct drm_device *dev,
+                           int pipe, 
+                           struct mdfld_dsi_config *config,
+                           struct panel_funcs* p_cmd_funcs,
+                           struct panel_funcs* p_vid_funcs)
+{
+        struct mdfld_dsi_config * dsi_config;
+        struct mdfld_dsi_connector * dsi_connector;
+        struct psb_intel_output * psb_output;
+        struct drm_connector * connector;
+        struct mdfld_dsi_encoder * encoder;
+        struct drm_psb_private * dev_priv = dev->dev_private;
+        struct panel_info dsi_panel_info;
+        u32 width_mm, height_mm;
+
+        dev_dbg(dev->dev, "init DSI output on pipe %d\n", pipe);
+        
+        if(!dev || ((pipe != 0) && (pipe != 2))) {
+                WARN_ON(1);
+                return;
+        }
+        
+        /*create a new connetor*/
+        dsi_connector = kzalloc(sizeof(struct mdfld_dsi_connector), GFP_KERNEL);
+        if(!dsi_connector) {
+                DRM_ERROR("No memory");
+                return;
+        }
+        
+        dsi_connector->pipe =  pipe;
+        
+        /*set DSI config*/
+        if(config) { 
+                dsi_config = config;
+        } else {
+                dsi_config = kzalloc(sizeof(struct mdfld_dsi_config), GFP_KERNEL);
+                if(!dsi_config) {
+                        dev_err(dev->dev,
+                                "cannot allocate memory for DSI config\n");
+                        goto dsi_init_err0;
+                }
+                
+                mdfld_dsi_get_default_config(dev, dsi_config, pipe);
+        }
+        
+        dsi_connector->private = dsi_config;
+        
+        dsi_config->changed = 1;
+        dsi_config->dev = dev;
+        
+        /* Init fixed mode basing on DSI config type */
+        if(dsi_config->type == MDFLD_DSI_ENCODER_DBI) {
+                dsi_config->fixed_mode = p_cmd_funcs->get_config_mode(dev);
+                if(p_cmd_funcs->get_panel_info(dev, pipe, &dsi_panel_info))
+                        goto dsi_init_err0;
+        } else if(dsi_config->type == MDFLD_DSI_ENCODER_DPI) {
+                dsi_config->fixed_mode = p_vid_funcs->get_config_mode(dev);
+                if(p_vid_funcs->get_panel_info(dev, pipe, &dsi_panel_info))
+                        goto dsi_init_err0;
+        }
+
+        width_mm = dsi_panel_info.width_mm;
+        height_mm = dsi_panel_info.height_mm;
+
+        dsi_config->mode = dsi_config->fixed_mode;
+        dsi_config->connector = dsi_connector;
+        
+        if(!dsi_config->fixed_mode) {
+                dev_err(dev->dev, "No pannel fixed mode was found\n");
+                goto dsi_init_err0;
+        }
+        
+        if(pipe && dev_priv->dsi_configs[0]) {
+                dsi_config->dvr_ic_inited = 0;
+                dev_priv->dsi_configs[1] = dsi_config;
+        } else if(pipe == 0) {
+                dsi_config->dvr_ic_inited = 1;
+                dev_priv->dsi_configs[0] = dsi_config;
+        } else {
+                dev_err(dev->dev, "Trying to init MIPI1 before MIPI0\n");
+                goto dsi_init_err0;
+        }
+
+        /*init drm connector object*/
+        psb_output = &dsi_connector->base;
+        
+        psb_output->type = (pipe == 0) ? INTEL_OUTPUT_MIPI : INTEL_OUTPUT_MIPI2;
+
+        connector = &psb_output->base;
+        /* Revisit type if MIPI/HDMI bridges ever appear on Medfield */
+        drm_connector_init(dev, connector, &mdfld_dsi_connector_funcs,
+                                                DRM_MODE_CONNECTOR_LVDS);
+        drm_connector_helper_add(connector, &mdfld_dsi_connector_helper_funcs);
+        
+        connector->display_info.subpixel_order = SubPixelHorizontalRGB;
+        connector->display_info.width_mm = width_mm;
+        connector->display_info.height_mm = height_mm;
+        connector->interlace_allowed = false;
+        connector->doublescan_allowed = false;
+        
+        /* Attach properties */
+        drm_connector_attach_property(connector, dev->mode_config.scaling_mode_property, DRM_MODE_SCALE_FULLSCREEN);
+        drm_connector_attach_property(connector, dev_priv->backlight_property, MDFLD_DSI_BRIGHTNESS_MAX_LEVEL);
+
+        /* Init DSI package sender on this output */
+        if (mdfld_dsi_pkg_sender_init(dsi_connector, pipe)) {
+                DRM_ERROR("Package Sender initialization failed on pipe %d\n", pipe);
+                goto dsi_init_err0;
+        }
+
+        /* Init DBI & DPI encoders */
+        if (p_cmd_funcs) {
+                encoder = mdfld_dsi_dbi_init(dev, dsi_connector, p_cmd_funcs);
+                if(!encoder) {
+                        dev_err(dev->dev, "Create DBI encoder failed\n");
+                        goto dsi_init_err1;
+                }
+                encoder->private = dsi_config;
+                dsi_config->encoders[MDFLD_DSI_ENCODER_DBI] = encoder;
+        }
+        
+        if(p_vid_funcs) {
+                encoder = mdfld_dsi_dpi_init(dev, dsi_connector, p_vid_funcs);
+                if(!encoder) {
+                        dev_err(dev->dev, "Create DPI encoder failed\n");
+                        goto dsi_init_err1;
+                }
+                encoder->private = dsi_config;
+                dsi_config->encoders[MDFLD_DSI_ENCODER_DPI] = encoder;
+        }
+        
+        drm_sysfs_connector_add(connector);
+        return;
+        
+        /*TODO: add code to destroy outputs on error*/
+dsi_init_err1:
+        /*destroy sender*/
+        mdfld_dsi_pkg_sender_destroy(dsi_connector->pkg_sender);
+
+        drm_connector_cleanup(connector);
+        kfree(dsi_config->fixed_mode);
+        kfree(dsi_config);
+dsi_init_err0:
+        kfree(dsi_connector);
+}
diff --git a/drivers/staging/gma500/mdfld_dsi_output.h b/drivers/staging/gma500/mdfld_dsi_output.h
new file mode 100644
index 00000000000..4699267efd6
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_dsi_output.h
@@ -0,0 +1,138 @@
+/*
+ * Copyright © 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:
+ * jim liu <jim.liu@intel.com>
+ * Jackie Li<yaodong.li@intel.com>
+ */
+
+#ifndef __MDFLD_DSI_OUTPUT_H__
+#define __MDFLD_DSI_OUTPUT_H__
+
+#include <linux/backlight.h>
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_edid.h>
+
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "power.h"
+#include "mdfld_output.h"
+
+#include <asm/mrst.h>
+
+
+static inline struct mdfld_dsi_config *
+        mdfld_dsi_get_config(struct mdfld_dsi_connector *connector)
+{
+        if (!connector)
+                return NULL;
+        return (struct mdfld_dsi_config *)connector->private;
+}
+
+static inline void *mdfld_dsi_get_pkg_sender(struct mdfld_dsi_config *config)
+{
+        struct mdfld_dsi_connector *dsi_connector;
+
+        if (!config)
+                return NULL;
+
+        dsi_connector = config->connector;
+
+        if (!dsi_connector)
+                return NULL;
+
+        return dsi_connector->pkg_sender;
+}
+
+static inline struct mdfld_dsi_config *
+        mdfld_dsi_encoder_get_config(struct mdfld_dsi_encoder *encoder)
+{
+        if (!encoder)
+                return NULL;
+        return (struct mdfld_dsi_config *)encoder->private;
+}
+
+static inline struct mdfld_dsi_connector *
+        mdfld_dsi_encoder_get_connector(struct mdfld_dsi_encoder *encoder)
+{
+        struct mdfld_dsi_config *config;
+
+        if (!encoder)
+                return NULL;
+
+        config = mdfld_dsi_encoder_get_config(encoder);
+        if (!config)
+                return NULL;
+
+        return config->connector;
+}
+
+static inline void *mdfld_dsi_encoder_get_pkg_sender(
+        struct mdfld_dsi_encoder *encoder)
+{
+        struct mdfld_dsi_config *dsi_config;
+
+        dsi_config = mdfld_dsi_encoder_get_config(encoder);
+        if (!dsi_config)
+                return NULL;
+
+        return mdfld_dsi_get_pkg_sender(dsi_config);
+}
+
+static inline int mdfld_dsi_encoder_get_pipe(struct mdfld_dsi_encoder *encoder)
+{
+        struct mdfld_dsi_connector *connector;
+
+        if (!encoder)
+                return -1;
+
+        connector = mdfld_dsi_encoder_get_connector(encoder);
+        if (!connector)
+                return -1;
+
+        return connector->pipe;
+}
+
+extern void mdfld_dsi_gen_fifo_ready(struct drm_device *dev,
+                                u32 gen_fifo_stat_reg, u32 fifo_stat);
+extern void mdfld_dsi_brightness_init(struct mdfld_dsi_config *dsi_config,
+                                int pipe);
+extern void mdfld_dsi_brightness_control(struct drm_device *dev, int pipe,
+                                int level);
+extern void mdfld_dsi_output_init(struct drm_device *dev, int pipe,
+                                struct mdfld_dsi_config *config,
+                                struct panel_funcs *p_cmd_funcs,
+                                struct panel_funcs *p_vid_funcs);
+extern void mdfld_dsi_controller_init(struct mdfld_dsi_config *dsi_config,
+                                int pipe);
+extern int mdfld_dsi_get_power_mode(struct mdfld_dsi_config *dsi_config,
+                                u32 *mode,
+                                u8 transmission);
+extern int mdfld_dsi_get_diagnostic_result(struct mdfld_dsi_config *dsi_config,
+                                u32 *result,
+                                u8 transmission);
+extern int mdfld_dsi_panel_reset(int pipe);
+
+#endif /*__MDFLD_DSI_OUTPUT_H__*/
diff --git a/drivers/staging/gma500/mdfld_dsi_pkg_sender.c b/drivers/staging/gma500/mdfld_dsi_pkg_sender.c
new file mode 100644
index 00000000000..9b96a5c9abc
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_dsi_pkg_sender.c
@@ -0,0 +1,1484 @@
+/*
+ * Copyright © 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:
+ * Jackie Li<yaodong.li@intel.com>
+ */
+
+#include <linux/freezer.h>
+
+#include "mdfld_dsi_output.h"
+#include "mdfld_dsi_pkg_sender.h"
+#include "mdfld_dsi_dbi.h"
+#include "mdfld_dsi_dpi.h"
+
+#define MDFLD_DSI_DBI_FIFO_TIMEOUT              100
+#define MDFLD_DSI_MAX_RETURN_PACKET_SIZE        512
+#define MDFLD_DSI_READ_MAX_COUNT                5000
+
+static const char * const dsi_errors[] = {
+        "RX SOT Error",
+        "RX SOT Sync Error",
+        "RX EOT Sync Error",
+        "RX Escape Mode Entry Error",
+        "RX LP TX Sync Error",
+        "RX HS Receive Timeout Error",
+        "RX False Control Error",
+        "RX ECC Single Bit Error",
+        "RX ECC Multibit Error",
+        "RX Checksum Error",
+        "RX DSI Data Type Not Recognised",
+        "RX DSI VC ID Invalid",
+        "TX False Control Error",
+        "TX ECC Single Bit Error",
+        "TX ECC Multibit Error",
+        "TX Checksum Error",
+        "TX DSI Data Type Not Recognised",
+        "TX DSI VC ID invalid",
+        "High Contention",
+        "Low contention",
+        "DPI FIFO Under run",
+        "HS TX Timeout",
+        "LP RX Timeout",
+        "Turn Around ACK Timeout",
+        "ACK With No Error",
+        "RX Invalid TX Length",
+        "RX Prot Violation",
+        "HS Generic Write FIFO Full",
+        "LP Generic Write FIFO Full",
+        "Generic Read Data Avail",
+        "Special Packet Sent",
+        "Tearing Effect",
+};
+
+static int wait_for_gen_fifo_empty(struct mdfld_dsi_pkg_sender *sender,
+                                                                u32 mask)
+{
+        struct drm_device *dev = sender->dev;
+        u32 gen_fifo_stat_reg = sender->mipi_gen_fifo_stat_reg;
+        int retry = 0xffff;
+
+        while (retry--) {
+                if ((mask & REG_READ(gen_fifo_stat_reg)) == mask)
+                        return 0;
+                udelay(100);
+        }
+        dev_err(dev->dev, "fifo is NOT empty 0x%08x\n",
+                                        REG_READ(gen_fifo_stat_reg));
+        return -EIO;
+}
+
+static int wait_for_all_fifos_empty(struct mdfld_dsi_pkg_sender *sender)
+{
+        return wait_for_gen_fifo_empty(sender, (1 << 2) | (1 << 10) | (1 << 18)
+                | (1 << 26) | (1 << 27) | (1 << 28));
+}
+
+static int wait_for_lp_fifos_empty(struct mdfld_dsi_pkg_sender *sender)
+{
+        return wait_for_gen_fifo_empty(sender, (1 << 10) | (1 << 26));
+}
+
+static int wait_for_hs_fifos_empty(struct mdfld_dsi_pkg_sender *sender)
+{
+        return wait_for_gen_fifo_empty(sender, (1 << 2) | (1 << 18));
+}
+
+static int wait_for_dbi_fifo_empty(struct mdfld_dsi_pkg_sender *sender)
+{
+        return wait_for_gen_fifo_empty(sender, (1 << 27));
+}
+
+static int handle_dsi_error(struct mdfld_dsi_pkg_sender *sender, u32 mask)
+{
+        u32 intr_stat_reg = sender->mipi_intr_stat_reg;
+        struct drm_device *dev = sender->dev;
+
+        switch (mask) {
+        case (1 << 0):
+        case (1 << 1):
+        case (1 << 2):
+        case (1 << 3):
+        case (1 << 4):
+        case (1 << 5):
+        case (1 << 6):
+        case (1 << 7):
+        case (1 << 8):
+        case (1 << 9):
+        case (1 << 10):
+        case (1 << 11):
+        case (1 << 12):
+        case (1 << 13):
+                break;
+        case (1 << 14):
+                /*wait for all fifo empty*/
+                /*wait_for_all_fifos_empty(sender)*/;
+                break;
+        case (1 << 15):
+                break;
+        case (1 << 16):
+                break;
+        case (1 << 17):
+                break;
+        case (1 << 18):
+        case (1 << 19):
+                /*wait for contention recovery time*/
+                /*mdelay(10);*/
+                /*wait for all fifo empty*/
+                if (0)
+                        wait_for_all_fifos_empty(sender);
+                break;
+        case (1 << 20):
+                break;
+        case (1 << 21):
+                /*wait for all fifo empty*/
+                /*wait_for_all_fifos_empty(sender);*/
+                break;
+        case (1 << 22):
+                break;
+        case (1 << 23):
+        case (1 << 24):
+        case (1 << 25):
+        case (1 << 26):
+        case (1 << 27):
+                /* HS Gen fifo full */
+                REG_WRITE(intr_stat_reg, mask);
+                wait_for_hs_fifos_empty(sender);
+                break;
+        case (1 << 28):
+                /* LP Gen fifo full\n */
+                REG_WRITE(intr_stat_reg, mask);
+                wait_for_lp_fifos_empty(sender);
+                break;
+        case (1 << 29):
+        case (1 << 30):
+        case (1 << 31):
+                break;
+        }
+
+        if (mask & REG_READ(intr_stat_reg))
+                dev_warn(dev->dev, "Cannot clean interrupt 0x%08x\n", mask);
+
+        return 0;
+}
+
+static int dsi_error_handler(struct mdfld_dsi_pkg_sender *sender)
+{
+        struct drm_device *dev = sender->dev;
+        u32 intr_stat_reg = sender->mipi_intr_stat_reg;
+        u32 mask;
+        u32 intr_stat;
+        int i;
+        int err = 0;
+
+        intr_stat = REG_READ(intr_stat_reg);
+
+        for (i = 0; i < 32; i++) {
+                mask = (0x00000001UL) << i;
+                if (intr_stat & mask) {
+                        dev_dbg(dev->dev, "[DSI]: %s\n", dsi_errors[i]);
+                        err = handle_dsi_error(sender, mask);
+                        if (err)
+                                dev_err(dev->dev, "Cannot handle error\n");
+                }
+        }
+        return err;
+}
+
+static inline int dbi_cmd_sent(struct mdfld_dsi_pkg_sender *sender)
+{
+        struct drm_device *dev = sender->dev;
+        u32 retry = 0xffff;
+        u32 dbi_cmd_addr_reg = sender->mipi_cmd_addr_reg;
+
+        /* Query the command execution status */
+        while (retry--) {
+                if (!(REG_READ(dbi_cmd_addr_reg) & (1 << 0)))
+                        break;
+        }
+
+        if (!retry) {
+                dev_err(dev->dev, "Timeout waiting for DBI Command status\n");
+                return -EAGAIN;
+        }
+        return 0;
+}
+
+/*
+ * NOTE: this interface is abandoned expect for write_mem_start DCS
+ * other DCS are sent via generic pkg interfaces
+ */
+static int send_dcs_pkg(struct mdfld_dsi_pkg_sender *sender,
+                        struct mdfld_dsi_pkg *pkg)
+{
+        struct drm_device *dev = sender->dev;
+        struct mdfld_dsi_dcs_pkg *dcs_pkg = &pkg->pkg.dcs_pkg;
+        u32 dbi_cmd_len_reg = sender->mipi_cmd_len_reg;
+        u32 dbi_cmd_addr_reg = sender->mipi_cmd_addr_reg;
+        u32 cb_phy = sender->dbi_cb_phy;
+        u32 index = 0;
+        u8 *cb = (u8 *)sender->dbi_cb_addr;
+        int i;
+        int ret;
+
+        if (!sender->dbi_pkg_support) {
+                dev_err(dev->dev, "Trying to send DCS on a non DBI output, abort!\n");
+                return -ENOTSUPP;
+        }
+
+        /*wait for DBI fifo empty*/
+        wait_for_dbi_fifo_empty(sender);
+
+        *(cb + (index++)) = dcs_pkg->cmd;
+        if (dcs_pkg->param_num) {
+                for (i = 0; i < dcs_pkg->param_num; i++)
+                        *(cb + (index++)) = *(dcs_pkg->param + i);
+        }
+
+        REG_WRITE(dbi_cmd_len_reg, (1 + dcs_pkg->param_num));
+        REG_WRITE(dbi_cmd_addr_reg,
+                (cb_phy << CMD_MEM_ADDR_OFFSET)
+                | (1 << 0)
+                | ((dcs_pkg->data_src == CMD_DATA_SRC_PIPE) ? (1 << 1) : 0));
+
+        ret = dbi_cmd_sent(sender);
+        if (ret) {
+                dev_err(dev->dev, "command 0x%x not complete\n", dcs_pkg->cmd);
+                return -EAGAIN;
+        }
+        return 0;
+}
+
+static int __send_short_pkg(struct mdfld_dsi_pkg_sender *sender,
+                                struct mdfld_dsi_pkg *pkg)
+{
+        struct drm_device *dev = sender->dev;
+        u32 hs_gen_ctrl_reg = sender->mipi_hs_gen_ctrl_reg;
+        u32 lp_gen_ctrl_reg = sender->mipi_lp_gen_ctrl_reg;
+        u32 gen_ctrl_val = 0;
+        struct mdfld_dsi_gen_short_pkg *short_pkg = &pkg->pkg.short_pkg;
+
+        gen_ctrl_val |= short_pkg->cmd << MCS_COMMANDS_POS;
+        gen_ctrl_val |= 0 << DCS_CHANNEL_NUMBER_POS;
+        gen_ctrl_val |= pkg->pkg_type;
+        gen_ctrl_val |= short_pkg->param << MCS_PARAMETER_POS;
+
+        if (pkg->transmission_type == MDFLD_DSI_HS_TRANSMISSION) {
+                /* wait for hs fifo empty */
+                /* wait_for_hs_fifos_empty(sender); */
+                /* Send pkg */
+                REG_WRITE(hs_gen_ctrl_reg, gen_ctrl_val);
+        } else if (pkg->transmission_type == MDFLD_DSI_LP_TRANSMISSION) {
+                /* wait_for_lp_fifos_empty(sender); */
+                /* Send pkg*/
+                REG_WRITE(lp_gen_ctrl_reg, gen_ctrl_val);
+        } else {
+                dev_err(dev->dev, "Unknown transmission type %d\n",
+                                                        pkg->transmission_type);
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+static int __send_long_pkg(struct mdfld_dsi_pkg_sender *sender,
+                                struct mdfld_dsi_pkg *pkg)
+{
+        struct drm_device *dev = sender->dev;
+        u32 hs_gen_ctrl_reg = sender->mipi_hs_gen_ctrl_reg;
+        u32 hs_gen_data_reg = sender->mipi_hs_gen_data_reg;
+        u32 lp_gen_ctrl_reg = sender->mipi_lp_gen_ctrl_reg;
+        u32 lp_gen_data_reg = sender->mipi_lp_gen_data_reg;
+        u32 gen_ctrl_val = 0;
+        u32 *dp;
+        int i;
+        struct mdfld_dsi_gen_long_pkg *long_pkg = &pkg->pkg.long_pkg;
+
+        dp = long_pkg->data;
+
+        /*
+         * Set up word count for long pkg
+         * FIXME: double check word count field.
+         * currently, using the byte counts of the payload as the word count.
+         * ------------------------------------------------------------
+         * | DI |   WC   | ECC|         PAYLOAD              |CHECKSUM|
+         * ------------------------------------------------------------
+         */
+        gen_ctrl_val |= (long_pkg->len << 2) << WORD_COUNTS_POS;
+        gen_ctrl_val |= 0 << DCS_CHANNEL_NUMBER_POS;
+        gen_ctrl_val |= pkg->pkg_type;
+
+        if (pkg->transmission_type == MDFLD_DSI_HS_TRANSMISSION) {
+                /* Wait for hs ctrl and data fifos to be empty */
+                /* wait_for_hs_fifos_empty(sender); */
+                for (i = 0; i < long_pkg->len; i++)
+                        REG_WRITE(hs_gen_data_reg, *(dp + i));
+                REG_WRITE(hs_gen_ctrl_reg, gen_ctrl_val);
+        } else if (pkg->transmission_type == MDFLD_DSI_LP_TRANSMISSION) {
+                /* wait_for_lp_fifos_empty(sender); */
+                for (i = 0; i < long_pkg->len; i++)
+                        REG_WRITE(lp_gen_data_reg, *(dp + i));
+                REG_WRITE(lp_gen_ctrl_reg, gen_ctrl_val);
+        } else {
+                dev_err(dev->dev, "Unknown transmission type %d\n",
+                                                pkg->transmission_type);
+                return -EINVAL;
+        }
+
+        return 0;
+
+}
+
+static int send_mcs_short_pkg(struct mdfld_dsi_pkg_sender *sender,
+                                struct mdfld_dsi_pkg *pkg)
+{
+        return __send_short_pkg(sender, pkg);
+}
+
+static int send_mcs_long_pkg(struct mdfld_dsi_pkg_sender *sender,
+                                struct mdfld_dsi_pkg *pkg)
+{
+        return __send_long_pkg(sender, pkg);
+}
+
+static int send_gen_short_pkg(struct mdfld_dsi_pkg_sender *sender,
+                                struct mdfld_dsi_pkg *pkg)
+{
+        return __send_short_pkg(sender, pkg);
+}
+
+static int send_gen_long_pkg(struct mdfld_dsi_pkg_sender *sender,
+                                struct mdfld_dsi_pkg *pkg)
+{
+        return __send_long_pkg(sender, pkg);
+}
+
+static int send_pkg_prepare(struct mdfld_dsi_pkg_sender *sender,
+                                struct mdfld_dsi_pkg *pkg)
+{
+        u8 cmd;
+        u8 *data;
+
+        switch (pkg->pkg_type) {
+        case MDFLD_DSI_PKG_DCS:
+                cmd = pkg->pkg.dcs_pkg.cmd;
+                break;
+        case MDFLD_DSI_PKG_MCS_SHORT_WRITE_0:
+        case MDFLD_DSI_PKG_MCS_SHORT_WRITE_1:
+                cmd = pkg->pkg.short_pkg.cmd;
+                break;
+        case MDFLD_DSI_PKG_MCS_LONG_WRITE:
+                data = (u8 *)pkg->pkg.long_pkg.data;
+                cmd = *data;
+                break;
+        default:
+                return 0;
+        }
+
+        /* This prevents other package sending while doing msleep */
+        sender->status = MDFLD_DSI_PKG_SENDER_BUSY;
+
+        /* Check panel mode v.s. sending command */
+        if ((sender->panel_mode & MDFLD_DSI_PANEL_MODE_SLEEP) &&
+                cmd != exit_sleep_mode) {
+                dev_err(sender->dev->dev,
+                                "sending 0x%x when panel sleep in\n", cmd);
+                sender->status = MDFLD_DSI_PKG_SENDER_FREE;
+                return -EINVAL;
+        }
+
+        /* Wait for 120 milliseconds in case exit_sleep_mode just be sent */
+        if (cmd == DCS_ENTER_SLEEP_MODE) {
+                /*TODO: replace it with msleep later*/
+                mdelay(120);
+        }
+        return 0;
+}
+
+static int send_pkg_done(struct mdfld_dsi_pkg_sender *sender,
+                                struct mdfld_dsi_pkg *pkg)
+{
+        u8 cmd;
+        u8 *data;
+
+        switch (pkg->pkg_type) {
+        case MDFLD_DSI_PKG_DCS:
+                cmd = pkg->pkg.dcs_pkg.cmd;
+                break;
+        case MDFLD_DSI_PKG_MCS_SHORT_WRITE_0:
+        case MDFLD_DSI_PKG_MCS_SHORT_WRITE_1:
+                cmd = pkg->pkg.short_pkg.cmd;
+                break;
+        case MDFLD_DSI_PKG_MCS_LONG_WRITE:
+                data = (u8 *)pkg->pkg.long_pkg.data;
+                cmd = *data;
+                break;
+        default:
+                return 0;
+        }
+
+        /* Update panel status */
+        if (cmd == DCS_ENTER_SLEEP_MODE) {
+                sender->panel_mode |= MDFLD_DSI_PANEL_MODE_SLEEP;
+                /*TODO: replace it with msleep later*/
+                mdelay(120);
+        } else if (cmd == DCS_EXIT_SLEEP_MODE) {
+                sender->panel_mode &= ~MDFLD_DSI_PANEL_MODE_SLEEP;
+                /*TODO: replace it with msleep later*/
+                mdelay(120);
+        } else if (unlikely(cmd == DCS_SOFT_RESET)) {
+                /*TODO: replace it with msleep later*/
+                mdelay(5);
+        }
+        sender->status = MDFLD_DSI_PKG_SENDER_FREE;
+        return 0;
+
+}
+
+static int do_send_pkg(struct mdfld_dsi_pkg_sender *sender,
+                        struct mdfld_dsi_pkg *pkg)
+{
+        int ret;
+
+        if (sender->status == MDFLD_DSI_PKG_SENDER_BUSY) {
+                dev_err(sender->dev->dev, "sender is busy\n");
+                return -EAGAIN;
+        }
+
+        ret = send_pkg_prepare(sender, pkg);
+        if (ret) {
+                dev_err(sender->dev->dev, "send_pkg_prepare error\n");
+                return ret;
+        }
+
+        switch (pkg->pkg_type) {
+        case MDFLD_DSI_PKG_DCS:
+                ret = send_dcs_pkg(sender, pkg);
+                break;
+        case MDFLD_DSI_PKG_GEN_SHORT_WRITE_0:
+        case MDFLD_DSI_PKG_GEN_SHORT_WRITE_1:
+        case MDFLD_DSI_PKG_GEN_SHORT_WRITE_2:
+        case MDFLD_DSI_PKG_GEN_READ_0:
+        case MDFLD_DSI_PKG_GEN_READ_1:
+        case MDFLD_DSI_PKG_GEN_READ_2:
+                ret = send_gen_short_pkg(sender, pkg);
+                break;
+        case MDFLD_DSI_PKG_GEN_LONG_WRITE:
+                ret = send_gen_long_pkg(sender, pkg);
+                break;
+        case MDFLD_DSI_PKG_MCS_SHORT_WRITE_0:
+        case MDFLD_DSI_PKG_MCS_SHORT_WRITE_1:
+        case MDFLD_DSI_PKG_MCS_READ:
+                ret = send_mcs_short_pkg(sender, pkg);
+                break;
+        case MDFLD_DSI_PKG_MCS_LONG_WRITE:
+                ret = send_mcs_long_pkg(sender, pkg);
+                break;
+        default:
+                dev_err(sender->dev->dev, "Invalid pkg type 0x%x\n",
+                                                        pkg->pkg_type);
+                ret = -EINVAL;
+        }
+        send_pkg_done(sender, pkg);
+        return ret;
+}
+
+static int send_pkg(struct mdfld_dsi_pkg_sender *sender,
+                        struct mdfld_dsi_pkg *pkg)
+{
+        int err ;
+
+        /* Handle DSI error */
+        err = dsi_error_handler(sender);
+        if (err) {
+                dev_err(sender->dev->dev, "Error handling failed\n");
+                err = -EAGAIN;
+                goto send_pkg_err;
+        }
+
+        /* Send pkg */
+        err = do_send_pkg(sender, pkg);
+        if (err) {
+                dev_err(sender->dev->dev, "sent pkg failed\n");
+                err = -EAGAIN;
+                goto send_pkg_err;
+        }
+
+        /* FIXME: should I query complete and fifo empty here? */
+send_pkg_err:
+        return err;
+}
+
+static struct mdfld_dsi_pkg *pkg_sender_get_pkg_locked(
+                                        struct mdfld_dsi_pkg_sender *sender)
+{
+        struct mdfld_dsi_pkg *pkg;
+
+        if (list_empty(&sender->free_list)) {
+                dev_err(sender->dev->dev, "No free pkg left\n");
+                return NULL;
+        }
+        pkg = list_first_entry(&sender->free_list, struct mdfld_dsi_pkg, entry);
+        /* Detach from free list */
+        list_del_init(&pkg->entry);
+        return pkg;
+}
+
+static void pkg_sender_put_pkg_locked(struct mdfld_dsi_pkg_sender *sender,
+                                        struct mdfld_dsi_pkg *pkg)
+{
+        memset(pkg, 0, sizeof(struct mdfld_dsi_pkg));
+        INIT_LIST_HEAD(&pkg->entry);
+        list_add_tail(&pkg->entry, &sender->free_list);
+}
+
+static int mdfld_dbi_cb_init(struct mdfld_dsi_pkg_sender *sender,
+                                        struct psb_gtt *pg, int pipe)
+{
+        unsigned long phys;
+        void *virt_addr = NULL;
+
+        switch (pipe) {
+        case 0:
+                /* FIXME: Doesn't this collide with stolen space ? */
+                phys = pg->gtt_phys_start - 0x1000;
+                break;
+        case 2:
+                phys = pg->gtt_phys_start - 0x800;
+                break;
+        default:
+                dev_err(sender->dev->dev, "Unsupported channel %d\n", pipe);
+                return -EINVAL;
+        }
+
+        virt_addr = ioremap_nocache(phys, 0x800);
+        if (!virt_addr) {
+                dev_err(sender->dev->dev, "Map DBI command buffer error\n");
+                return -ENOMEM;
+        }
+        sender->dbi_cb_phy = phys;
+        sender->dbi_cb_addr = virt_addr;
+        return 0;
+}
+
+static void mdfld_dbi_cb_destroy(struct mdfld_dsi_pkg_sender *sender)
+{
+        if (sender && sender->dbi_cb_addr)
+                iounmap(sender->dbi_cb_addr);
+}
+
+static void pkg_sender_queue_pkg(struct mdfld_dsi_pkg_sender *sender,
+                                        struct mdfld_dsi_pkg *pkg,
+                                        int delay)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&sender->lock, flags);
+
+        if (!delay) {
+                send_pkg(sender, pkg);
+                pkg_sender_put_pkg_locked(sender, pkg);
+        } else {
+                /* Queue it */
+                list_add_tail(&pkg->entry, &sender->pkg_list);
+        }
+        spin_unlock_irqrestore(&sender->lock, flags);
+}
+
+static void process_pkg_list(struct mdfld_dsi_pkg_sender *sender)
+{
+        struct mdfld_dsi_pkg *pkg;
+        unsigned long flags;
+
+        spin_lock_irqsave(&sender->lock, flags);
+
+        while (!list_empty(&sender->pkg_list)) {
+                pkg = list_first_entry(&sender->pkg_list,
+                                        struct mdfld_dsi_pkg, entry);
+                send_pkg(sender, pkg);
+                list_del_init(&pkg->entry);
+                pkg_sender_put_pkg_locked(sender, pkg);
+        }
+
+        spin_unlock_irqrestore(&sender->lock, flags);
+}
+
+static int mdfld_dsi_send_mcs_long(struct mdfld_dsi_pkg_sender *sender,
+        u32 *data, u32 len, u8 transmission, int delay)
+{
+        struct mdfld_dsi_pkg *pkg;
+        unsigned long flags;
+
+        spin_lock_irqsave(&sender->lock, flags);
+        pkg = pkg_sender_get_pkg_locked(sender);
+        spin_unlock_irqrestore(&sender->lock, flags);
+
+        if (!pkg) {
+                dev_err(sender->dev->dev, "No memory\n");
+                return -ENOMEM;
+        }
+        pkg->pkg_type = MDFLD_DSI_PKG_MCS_LONG_WRITE;
+        pkg->transmission_type = transmission;
+        pkg->pkg.long_pkg.data = data;
+        pkg->pkg.long_pkg.len = len;
+        INIT_LIST_HEAD(&pkg->entry);
+
+        pkg_sender_queue_pkg(sender, pkg, delay);
+        return 0;
+}
+
+static int mdfld_dsi_send_mcs_short(struct mdfld_dsi_pkg_sender *sender,
+                                        u8 cmd, u8 param, u8 param_num,
+                                        u8 transmission,
+                                        int delay)
+{
+        struct mdfld_dsi_pkg *pkg;
+        unsigned long flags;
+
+        spin_lock_irqsave(&sender->lock, flags);
+        pkg = pkg_sender_get_pkg_locked(sender);
+        spin_unlock_irqrestore(&sender->lock, flags);
+
+        if (!pkg) {
+                dev_err(sender->dev->dev, "No memory\n");
+                return -ENOMEM;
+        }
+
+        if (param_num) {
+                pkg->pkg_type = MDFLD_DSI_PKG_MCS_SHORT_WRITE_1;
+                pkg->pkg.short_pkg.param = param;
+        } else {
+                pkg->pkg_type = MDFLD_DSI_PKG_MCS_SHORT_WRITE_0;
+                pkg->pkg.short_pkg.param = 0;
+        }
+        pkg->transmission_type = transmission;
+        pkg->pkg.short_pkg.cmd = cmd;
+        INIT_LIST_HEAD(&pkg->entry);
+
+        pkg_sender_queue_pkg(sender, pkg, delay);
+        return 0;
+}
+
+static int mdfld_dsi_send_gen_short(struct mdfld_dsi_pkg_sender *sender,
+                                        u8 param0, u8 param1, u8 param_num,
+                                        u8 transmission,
+                                        int delay)
+{
+        struct mdfld_dsi_pkg *pkg;
+        unsigned long flags;
+
+        spin_lock_irqsave(&sender->lock, flags);
+        pkg = pkg_sender_get_pkg_locked(sender);
+        spin_unlock_irqrestore(&sender->lock, flags);
+
+        if (!pkg) {
+                dev_err(sender->dev->dev, "No pkg memory\n");
+                return -ENOMEM;
+        }
+
+        switch (param_num) {
+        case 0:
+                pkg->pkg_type = MDFLD_DSI_PKG_GEN_SHORT_WRITE_0;
+                pkg->pkg.short_pkg.cmd = 0;
+                pkg->pkg.short_pkg.param = 0;
+                break;
+        case 1:
+                pkg->pkg_type = MDFLD_DSI_PKG_GEN_SHORT_WRITE_1;
+                pkg->pkg.short_pkg.cmd = param0;
+                pkg->pkg.short_pkg.param = 0;
+                break;
+        case 2:
+                pkg->pkg_type = MDFLD_DSI_PKG_GEN_SHORT_WRITE_2;
+                pkg->pkg.short_pkg.cmd = param0;
+                pkg->pkg.short_pkg.param = param1;
+                break;
+        }
+
+        pkg->transmission_type = transmission;
+        INIT_LIST_HEAD(&pkg->entry);
+
+        pkg_sender_queue_pkg(sender, pkg, delay);
+        return 0;
+}
+
+static int mdfld_dsi_send_gen_long(struct mdfld_dsi_pkg_sender *sender,
+                                u32 *data, u32 len, u8 transmission, int delay)
+{
+        struct mdfld_dsi_pkg *pkg;
+        unsigned long flags;
+
+        spin_lock_irqsave(&sender->lock, flags);
+        pkg = pkg_sender_get_pkg_locked(sender);
+        spin_unlock_irqrestore(&sender->lock, flags);
+
+        if (!pkg) {
+                dev_err(sender->dev->dev, "No pkg memory\n");
+                return -ENOMEM;
+        }
+
+        pkg->pkg_type = MDFLD_DSI_PKG_GEN_LONG_WRITE;
+        pkg->transmission_type = transmission;
+        pkg->pkg.long_pkg.data = data;
+        pkg->pkg.long_pkg.len = len;
+
+        INIT_LIST_HEAD(&pkg->entry);
+
+        pkg_sender_queue_pkg(sender, pkg, delay);
+
+        return 0;
+}
+
+static int __read_panel_data(struct mdfld_dsi_pkg_sender *sender,
+                                struct mdfld_dsi_pkg *pkg,
+                                u32 *data,
+                                u16 len)
+{
+        unsigned long flags;
+        struct drm_device *dev = sender->dev;
+        int i;
+        u32 gen_data_reg;
+        int retry = MDFLD_DSI_READ_MAX_COUNT;
+        u8 transmission = pkg->transmission_type;
+
+        /*
+         * do reading.
+         * 0) send out generic read request
+         * 1) polling read data avail interrupt
+         * 2) read data
+         */
+        spin_lock_irqsave(&sender->lock, flags);
+
+        REG_WRITE(sender->mipi_intr_stat_reg, 1 << 29);
+
+        if ((REG_READ(sender->mipi_intr_stat_reg) & (1 << 29)))
+                DRM_ERROR("Can NOT clean read data valid interrupt\n");
+
+        /*send out read request*/
+        send_pkg(sender, pkg);
+
+        pkg_sender_put_pkg_locked(sender, pkg);
+
+        /*polling read data avail interrupt*/
+        while (retry && !(REG_READ(sender->mipi_intr_stat_reg) & (1 << 29))) {
+                udelay(100);
+                retry--;
+        }
+
+        if (!retry) {
+                spin_unlock_irqrestore(&sender->lock, flags);
+                return -ETIMEDOUT;
+        }
+
+        REG_WRITE(sender->mipi_intr_stat_reg, (1 << 29));
+
+        /*read data*/
+        if (transmission == MDFLD_DSI_HS_TRANSMISSION)
+                gen_data_reg = sender->mipi_hs_gen_data_reg;
+        else if (transmission == MDFLD_DSI_LP_TRANSMISSION)
+                gen_data_reg = sender->mipi_lp_gen_data_reg;
+        else {
+                DRM_ERROR("Unknown transmission");
+                spin_unlock_irqrestore(&sender->lock, flags);
+                return -EINVAL;
+        }
+
+        for (i=0; i<len; i++)
+                *(data + i) = REG_READ(gen_data_reg);
+
+        spin_unlock_irqrestore(&sender->lock, flags);
+ 
+        return 0;
+}
+
+static int mdfld_dsi_read_gen(struct mdfld_dsi_pkg_sender *sender,
+                                u8 param0,
+                                u8 param1,
+                                u8 param_num,
+                                u32 *data,
+                                u16 len,
+                                u8 transmission)
+{
+        struct mdfld_dsi_pkg *pkg;
+        unsigned long flags;
+
+        spin_lock_irqsave(&sender->lock, flags);
+
+        pkg = pkg_sender_get_pkg_locked(sender);
+
+        spin_unlock_irqrestore(&sender->lock,flags);
+
+        if (!pkg) {
+                dev_err(sender->dev->dev, "No pkg memory\n");
+                return -ENOMEM;
+        }
+
+        switch (param_num) {
+        case 0:
+                pkg->pkg_type = MDFLD_DSI_PKG_GEN_READ_0;
+                pkg->pkg.short_pkg.cmd = 0;
+                pkg->pkg.short_pkg.param = 0;
+                break;
+        case 1:
+                pkg->pkg_type = MDFLD_DSI_PKG_GEN_READ_1;
+                pkg->pkg.short_pkg.cmd = param0;
+                pkg->pkg.short_pkg.param = 0;
+                break;
+        case 2:
+                pkg->pkg_type = MDFLD_DSI_PKG_GEN_READ_2;
+                pkg->pkg.short_pkg.cmd = param0;
+                pkg->pkg.short_pkg.param = param1;
+                break;
+        }
+
+        pkg->transmission_type = transmission;
+
+        INIT_LIST_HEAD(&pkg->entry);
+
+        return __read_panel_data(sender, pkg, data, len);
+}
+ 
+static int mdfld_dsi_read_mcs(struct mdfld_dsi_pkg_sender *sender,
+                                u8 cmd,
+                                u32 *data,
+                                u16 len,
+                                u8 transmission)
+{
+        struct mdfld_dsi_pkg *pkg;
+        unsigned long flags;
+
+        spin_lock_irqsave(&sender->lock, flags);
+
+        pkg = pkg_sender_get_pkg_locked(sender);
+
+        spin_unlock_irqrestore(&sender->lock, flags);
+ 
+        if (!pkg) {
+                dev_err(sender->dev->dev, "No pkg memory\n");
+                return -ENOMEM;
+        }
+
+        pkg->pkg_type = MDFLD_DSI_PKG_MCS_READ;
+        pkg->pkg.short_pkg.cmd = cmd;
+        pkg->pkg.short_pkg.param = 0;
+
+        pkg->transmission_type = transmission;
+ 
+        INIT_LIST_HEAD(&pkg->entry);
+
+        return __read_panel_data(sender, pkg, data, len);
+}
+
+void dsi_controller_dbi_init(struct mdfld_dsi_config * dsi_config, int pipe)
+{
+        struct drm_device * dev = dsi_config->dev;
+        u32 reg_offset = pipe ? MIPIC_REG_OFFSET : 0;
+        int lane_count = dsi_config->lane_count;
+        u32 val = 0;
+
+        /*un-ready device*/
+        REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset), 0x00000000);
+
+        /*init dsi adapter before kicking off*/
+        REG_WRITE((MIPIA_CONTROL_REG + reg_offset), 0x00000018);
+
+        /*TODO: figure out how to setup these registers*/
+        REG_WRITE((MIPIA_DPHY_PARAM_REG + reg_offset), 0x150c3408);
+        REG_WRITE((MIPIA_CLK_LANE_SWITCH_TIME_CNT_REG + reg_offset), 0x000a0014);
+        REG_WRITE((MIPIA_DBI_BW_CTRL_REG + reg_offset), 0x00000400);
+        REG_WRITE((MIPIA_DBI_FIFO_THROTTLE_REG + reg_offset), 0x00000001);
+        REG_WRITE((MIPIA_HS_LS_DBI_ENABLE_REG + reg_offset), 0x00000000);
+
+        /*enable all interrupts*/
+        REG_WRITE((MIPIA_INTR_EN_REG + reg_offset), 0xffffffff);
+        /*max value: 20 clock cycles of txclkesc*/
+        REG_WRITE((MIPIA_TURN_AROUND_TIMEOUT_REG + reg_offset), 0x0000001f);
+        /*min 21 txclkesc, max: ffffh*/
+        REG_WRITE((MIPIA_DEVICE_RESET_TIMER_REG + reg_offset), 0x0000ffff);
+        /*min: 7d0 max: 4e20*/
+        REG_WRITE((MIPIA_INIT_COUNT_REG + reg_offset), 0x00000fa0);
+
+        /*set up max return packet size*/
+        REG_WRITE((MIPIA_MAX_RETURN_PACK_SIZE_REG + reg_offset),
+                        MDFLD_DSI_MAX_RETURN_PACKET_SIZE);
+
+        /*set up func_prg*/
+        val |= lane_count;
+        val |= (dsi_config->channel_num << DSI_DBI_VIRT_CHANNEL_OFFSET);
+        val |= DSI_DBI_COLOR_FORMAT_OPTION2;
+        REG_WRITE((MIPIA_DSI_FUNC_PRG_REG + reg_offset), val);
+
+        REG_WRITE((MIPIA_HS_TX_TIMEOUT_REG + reg_offset), 0x3fffff);
+        REG_WRITE((MIPIA_LP_RX_TIMEOUT_REG + reg_offset), 0xffff);
+
+        REG_WRITE((MIPIA_HIGH_LOW_SWITCH_COUNT_REG + reg_offset), 0x46);
+        REG_WRITE((MIPIA_EOT_DISABLE_REG + reg_offset), 0x00000000);
+        REG_WRITE((MIPIA_LP_BYTECLK_REG + reg_offset), 0x00000004);
+        REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset), 0x00000001);
+}
+
+void dsi_controller_dpi_init(struct mdfld_dsi_config * dsi_config, int pipe)
+{
+        struct drm_device * dev = dsi_config->dev;
+        u32 reg_offset = pipe ? MIPIC_REG_OFFSET : 0;
+        int lane_count = dsi_config->lane_count;
+        struct mdfld_dsi_dpi_timing dpi_timing;
+        struct drm_display_mode * mode = dsi_config->mode;
+        u32 val = 0;
+
+        /*un-ready device*/
+        REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset), 0x00000000);
+
+        /*init dsi adapter before kicking off*/
+        REG_WRITE((MIPIA_CONTROL_REG + reg_offset), 0x00000018);
+
+        /*enable all interrupts*/
+        REG_WRITE((MIPIA_INTR_EN_REG + reg_offset), 0xffffffff);
+
+        /*set up func_prg*/
+        val |= lane_count;
+        val |= dsi_config->channel_num << DSI_DPI_VIRT_CHANNEL_OFFSET;
+
+        switch(dsi_config->bpp) {
+        case 16:
+                val |= DSI_DPI_COLOR_FORMAT_RGB565;
+                break;
+        case 18:
+                val |= DSI_DPI_COLOR_FORMAT_RGB666;
+                break;
+        case 24:
+                val |= DSI_DPI_COLOR_FORMAT_RGB888;
+                break;
+        default:
+                DRM_ERROR("unsupported color format, bpp = %d\n", dsi_config->bpp);
+        }
+
+        REG_WRITE((MIPIA_DSI_FUNC_PRG_REG + reg_offset), val);
+
+        REG_WRITE((MIPIA_HS_TX_TIMEOUT_REG + reg_offset),
+                        (mode->vtotal * mode->htotal * dsi_config->bpp / (8 * lane_count)) & DSI_HS_TX_TIMEOUT_MASK);
+        REG_WRITE((MIPIA_LP_RX_TIMEOUT_REG + reg_offset), 0xffff & DSI_LP_RX_TIMEOUT_MASK);
+
+        /*max value: 20 clock cycles of txclkesc*/
+        REG_WRITE((MIPIA_TURN_AROUND_TIMEOUT_REG + reg_offset), 0x14 & DSI_TURN_AROUND_TIMEOUT_MASK);
+
+        /*min 21 txclkesc, max: ffffh*/
+        REG_WRITE((MIPIA_DEVICE_RESET_TIMER_REG + reg_offset), 0xffff & DSI_RESET_TIMER_MASK);
+
+        REG_WRITE((MIPIA_DPI_RESOLUTION_REG + reg_offset), mode->vdisplay << 16 | mode->hdisplay);
+
+        /*set DPI timing registers*/
+        mdfld_dsi_dpi_timing_calculation(mode, &dpi_timing, dsi_config->lane_count, dsi_config->bpp);
+
+        REG_WRITE((MIPIA_HSYNC_COUNT_REG + reg_offset), dpi_timing.hsync_count & DSI_DPI_TIMING_MASK);
+        REG_WRITE((MIPIA_HBP_COUNT_REG + reg_offset), dpi_timing.hbp_count & DSI_DPI_TIMING_MASK);
+        REG_WRITE((MIPIA_HFP_COUNT_REG + reg_offset), dpi_timing.hfp_count & DSI_DPI_TIMING_MASK);
+        REG_WRITE((MIPIA_HACTIVE_COUNT_REG + reg_offset), dpi_timing.hactive_count & DSI_DPI_TIMING_MASK);
+        REG_WRITE((MIPIA_VSYNC_COUNT_REG + reg_offset), dpi_timing.vsync_count & DSI_DPI_TIMING_MASK);
+        REG_WRITE((MIPIA_VBP_COUNT_REG + reg_offset), dpi_timing.vbp_count & DSI_DPI_TIMING_MASK);
+        REG_WRITE((MIPIA_VFP_COUNT_REG + reg_offset), dpi_timing.vfp_count & DSI_DPI_TIMING_MASK);
+
+        REG_WRITE((MIPIA_HIGH_LOW_SWITCH_COUNT_REG + reg_offset), 0x46);
+
+        /*min: 7d0 max: 4e20*/
+        REG_WRITE((MIPIA_INIT_COUNT_REG + reg_offset), 0x000007d0);
+
+        /*set up video mode*/
+        val = dsi_config->video_mode | DSI_DPI_COMPLETE_LAST_LINE;
+        REG_WRITE((MIPIA_VIDEO_MODE_FORMAT_REG + reg_offset), val);
+
+        REG_WRITE((MIPIA_EOT_DISABLE_REG + reg_offset), 0x00000000);
+
+        REG_WRITE((MIPIA_LP_BYTECLK_REG + reg_offset), 0x00000004);
+
+        /*TODO: figure out how to setup these registers*/
+        REG_WRITE((MIPIA_DPHY_PARAM_REG + reg_offset), 0x150c3408);
+
+        REG_WRITE((MIPIA_CLK_LANE_SWITCH_TIME_CNT_REG + reg_offset), (0xa << 16) | 0x14);
+
+        /*set device ready*/
+        REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset), 0x00000001);
+}
+
+static void dsi_controller_init(struct mdfld_dsi_config * dsi_config, int pipe)
+{
+        if (!dsi_config || ((pipe != 0) && (pipe != 2))) {
+                DRM_ERROR("Invalid parameters\n");
+                return;
+        }
+
+        if (dsi_config->type == MDFLD_DSI_ENCODER_DPI)
+                dsi_controller_dpi_init(dsi_config, pipe);
+        else if (dsi_config->type == MDFLD_DSI_ENCODER_DBI)
+                dsi_controller_dbi_init(dsi_config, pipe);
+        else
+                DRM_ERROR("Bad DSI encoder type\n");
+}
+
+void mdfld_dsi_cmds_kick_out(struct mdfld_dsi_pkg_sender *sender)
+{
+        process_pkg_list(sender);
+}
+
+int mdfld_dsi_send_dcs(struct mdfld_dsi_pkg_sender *sender,
+                        u8 dcs, u8 *param, u32 param_num, u8 data_src,
+                        int delay)
+{
+        struct mdfld_dsi_pkg *pkg;
+        u32 cb_phy = sender->dbi_cb_phy;
+        struct drm_device *dev = sender->dev;
+        u32 index = 0;
+        u8 *cb = (u8 *)sender->dbi_cb_addr;
+        unsigned long flags;
+        int retry;
+        u8 *dst = NULL;
+        u32 len;
+
+        if (!sender) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+
+        if (!sender->dbi_pkg_support) {
+                dev_err(dev->dev, "No DBI pkg sending on this sender\n");
+                return -ENOTSUPP;
+        }
+
+        if (param_num > MDFLD_MAX_DCS_PARAM) {
+                dev_err(dev->dev, "Sender only supports up to %d DCS params\n",
+                                                        MDFLD_MAX_DCS_PARAM);
+                return -EINVAL;
+        }
+
+        /*
+         * If dcs is write_mem_start, send it directly using DSI adapter
+         * interface
+         */
+        if (dcs == DCS_WRITE_MEM_START) {
+                if (!spin_trylock(&sender->lock))
+                        return -EAGAIN;
+
+                /*
+                 * query whether DBI FIFO is empty,
+                 * if not wait it becoming empty
+                 */
+                retry = MDFLD_DSI_DBI_FIFO_TIMEOUT;
+                while (retry &&
+                    !(REG_READ(sender->mipi_gen_fifo_stat_reg) & (1 << 27))) {
+                        udelay(500);
+                        retry--;
+                }
+
+                /* If DBI FIFO timeout, drop this frame */
+                if (!retry) {
+                        spin_unlock(&sender->lock);
+                        return 0;
+                }
+
+                *(cb + (index++)) = write_mem_start;
+
+                REG_WRITE(sender->mipi_cmd_len_reg, 1);
+                REG_WRITE(sender->mipi_cmd_addr_reg,
+                                        cb_phy | (1 << 0) | (1 << 1));
+
+                retry = MDFLD_DSI_DBI_FIFO_TIMEOUT;
+                while (retry &&
+                        (REG_READ(sender->mipi_cmd_addr_reg) & (1 << 0))) {
+                        udelay(1);
+                        retry--;
+                }
+
+                spin_unlock(&sender->lock);
+                return 0;
+        }
+
+        /* Get a free pkg */
+        spin_lock_irqsave(&sender->lock, flags);
+        pkg = pkg_sender_get_pkg_locked(sender);
+        spin_unlock_irqrestore(&sender->lock, flags);
+
+        if (!pkg) {
+                dev_err(dev->dev, "No packages memory\n");
+                return -ENOMEM;
+        }
+
+        dst = pkg->pkg.dcs_pkg.param;
+        memcpy(dst, param, param_num);
+
+        pkg->pkg_type = MDFLD_DSI_PKG_DCS;
+        pkg->transmission_type = MDFLD_DSI_DCS;
+        pkg->pkg.dcs_pkg.cmd = dcs;
+        pkg->pkg.dcs_pkg.param_num = param_num;
+        pkg->pkg.dcs_pkg.data_src = data_src;
+
+        INIT_LIST_HEAD(&pkg->entry);
+
+        if (param_num == 0)
+                return mdfld_dsi_send_mcs_short_hs(sender, dcs, 0, 0, delay);
+        else if (param_num == 1)
+                return mdfld_dsi_send_mcs_short_hs(sender, dcs,
+                                                        param[0], 1, delay);
+        else if (param_num > 1) {
+                len = (param_num + 1) / 4;
+                if ((param_num + 1) % 4)
+                        len++;
+                return mdfld_dsi_send_mcs_long_hs(sender,
+                                (u32 *)&pkg->pkg.dcs_pkg, len, delay);
+        }
+        return 0;
+}
+
+int mdfld_dsi_send_mcs_short_hs(struct mdfld_dsi_pkg_sender *sender,
+                                u8 cmd, u8 param, u8 param_num, int delay)
+{
+        if (!sender) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+        return mdfld_dsi_send_mcs_short(sender, cmd, param, param_num,
+                                        MDFLD_DSI_HS_TRANSMISSION, delay);
+}
+
+int mdfld_dsi_send_mcs_short_lp(struct mdfld_dsi_pkg_sender *sender,
+                                u8 cmd, u8 param, u8 param_num, int delay)
+{
+        if (!sender) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+        return mdfld_dsi_send_mcs_short(sender, cmd, param, param_num,
+                                        MDFLD_DSI_LP_TRANSMISSION, delay);
+}
+
+int mdfld_dsi_send_mcs_long_hs(struct mdfld_dsi_pkg_sender *sender,
+                                u32 *data,
+                                u32 len,
+                                int delay)
+{
+        if (!sender || !data || !len) {
+                DRM_ERROR("Invalid parameters\n");
+                return -EINVAL;
+        }
+        return mdfld_dsi_send_mcs_long(sender, data, len,
+                                        MDFLD_DSI_HS_TRANSMISSION, delay);
+}
+
+int mdfld_dsi_send_mcs_long_lp(struct mdfld_dsi_pkg_sender *sender,
+                                u32 *data,
+                                u32 len,
+                                int delay)
+{
+        if (!sender || !data || !len) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+        return mdfld_dsi_send_mcs_long(sender, data, len,
+                                MDFLD_DSI_LP_TRANSMISSION, delay);
+}
+
+int mdfld_dsi_send_gen_short_hs(struct mdfld_dsi_pkg_sender *sender,
+                                u8 param0, u8 param1, u8 param_num, int delay)
+{
+        if (!sender) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+        return mdfld_dsi_send_gen_short(sender, param0, param1, param_num,
+                                        MDFLD_DSI_HS_TRANSMISSION, delay);
+}
+
+int mdfld_dsi_send_gen_short_lp(struct mdfld_dsi_pkg_sender *sender,
+                                u8 param0, u8 param1, u8 param_num, int delay)
+{
+        if (!sender || param_num < 0 || param_num > 2) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+        return mdfld_dsi_send_gen_short(sender, param0, param1, param_num,
+                                        MDFLD_DSI_LP_TRANSMISSION, delay);
+}
+
+int mdfld_dsi_send_gen_long_hs(struct mdfld_dsi_pkg_sender *sender,
+                                u32 *data,
+                                u32 len,
+                                int delay)
+{
+        if (!sender || !data || !len) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+        return mdfld_dsi_send_gen_long(sender, data, len,
+                                        MDFLD_DSI_HS_TRANSMISSION, delay);
+}
+
+int mdfld_dsi_send_gen_long_lp(struct mdfld_dsi_pkg_sender *sender,
+                                u32 *data,
+                                u32 len,
+                                int delay)
+{
+        if (!sender || !data || !len) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+        return mdfld_dsi_send_gen_long(sender, data, len,
+                                        MDFLD_DSI_LP_TRANSMISSION, delay);
+}
+
+int mdfld_dsi_read_gen_hs(struct mdfld_dsi_pkg_sender *sender,
+                        u8 param0,
+                        u8 param1,
+                        u8 param_num,
+                        u32 *data,
+                        u16 len)
+{
+        if (!sender || !data || param_num < 0 || param_num > 2
+                || !data || !len) {
+                DRM_ERROR("Invalid parameters\n");
+                return -EINVAL;
+        }
+
+        return mdfld_dsi_read_gen(sender, param0, param1, param_num,
+                                data, len, MDFLD_DSI_HS_TRANSMISSION);
+
+}
+
+int mdfld_dsi_read_gen_lp(struct mdfld_dsi_pkg_sender *sender,
+                        u8 param0,
+                        u8 param1,
+                        u8 param_num,
+                        u32 *data,
+                        u16 len)
+{
+        if (!sender || !data || param_num < 0 || param_num > 2
+                || !data || !len) {
+                DRM_ERROR("Invalid parameters\n");
+                return -EINVAL;
+        }
+
+        return mdfld_dsi_read_gen(sender, param0, param1, param_num,
+                                data, len, MDFLD_DSI_LP_TRANSMISSION);
+}
+
+int mdfld_dsi_read_mcs_hs(struct mdfld_dsi_pkg_sender *sender,
+                        u8 cmd,
+                        u32 *data,
+                        u16 len)
+{
+        if (!sender || !data || !len) {
+                DRM_ERROR("Invalid parameters\n");
+                return -EINVAL;
+        }
+
+        return mdfld_dsi_read_mcs(sender, cmd, data, len,
+                                MDFLD_DSI_HS_TRANSMISSION);
+}
+
+int mdfld_dsi_read_mcs_lp(struct mdfld_dsi_pkg_sender *sender,
+                        u8 cmd,
+                        u32 *data,
+                        u16 len)
+{
+        if (!sender || !data || !len) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+
+        return mdfld_dsi_read_mcs(sender, cmd, data, len,
+                                MDFLD_DSI_LP_TRANSMISSION);
+}
+ 
+int mdfld_dsi_pkg_sender_init(struct mdfld_dsi_connector *dsi_connector,
+                                                                int pipe)
+{
+        int ret;
+        struct mdfld_dsi_pkg_sender *pkg_sender;
+        struct mdfld_dsi_config *dsi_config =
+                                        mdfld_dsi_get_config(dsi_connector);
+        struct drm_device *dev = dsi_config->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct psb_gtt *pg = &dev_priv->gtt;
+        int i;
+        struct mdfld_dsi_pkg *pkg, *tmp;
+        u32 mipi_val = 0;
+
+        if (!dsi_connector) {
+                WARN_ON(1);
+                return -EINVAL;
+        }
+
+        pkg_sender = dsi_connector->pkg_sender;
+
+        if (!pkg_sender || IS_ERR(pkg_sender)) {
+                pkg_sender = kzalloc(sizeof(struct mdfld_dsi_pkg_sender),
+                                                                GFP_KERNEL);
+                if (!pkg_sender) {
+                        dev_err(dev->dev, "Create DSI pkg sender failed\n");
+                        return -ENOMEM;
+                }
+
+                dsi_connector->pkg_sender = (void *)pkg_sender;
+        }
+
+        pkg_sender->dev = dev;
+        pkg_sender->dsi_connector = dsi_connector;
+        pkg_sender->pipe = pipe;
+        pkg_sender->pkg_num = 0;
+        pkg_sender->panel_mode = 0;
+        pkg_sender->status = MDFLD_DSI_PKG_SENDER_FREE;
+
+        /* Init dbi command buffer*/
+
+        if (dsi_config->type == MDFLD_DSI_ENCODER_DBI) {
+                pkg_sender->dbi_pkg_support = 1;
+                ret = mdfld_dbi_cb_init(pkg_sender, pg, pipe);
+                if (ret) {
+                        dev_err(dev->dev, "DBI command buffer map failed\n");
+                        goto mapping_err;
+                }
+        }
+
+        /* Init regs */
+        if (pipe == 0) {
+                pkg_sender->dpll_reg = MRST_DPLL_A;
+                pkg_sender->dspcntr_reg = DSPACNTR;
+                pkg_sender->pipeconf_reg = PIPEACONF;
+                pkg_sender->dsplinoff_reg = DSPALINOFF;
+                pkg_sender->dspsurf_reg = DSPASURF;
+                pkg_sender->pipestat_reg = PIPEASTAT;
+
+                pkg_sender->mipi_intr_stat_reg = MIPIA_INTR_STAT_REG;
+                pkg_sender->mipi_lp_gen_data_reg = MIPIA_LP_GEN_DATA_REG;
+                pkg_sender->mipi_hs_gen_data_reg = MIPIA_HS_GEN_DATA_REG;
+                pkg_sender->mipi_lp_gen_ctrl_reg = MIPIA_LP_GEN_CTRL_REG;
+                pkg_sender->mipi_hs_gen_ctrl_reg = MIPIA_HS_GEN_CTRL_REG;
+                pkg_sender->mipi_gen_fifo_stat_reg = MIPIA_GEN_FIFO_STAT_REG;
+                pkg_sender->mipi_data_addr_reg = MIPIA_DATA_ADD_REG;
+                pkg_sender->mipi_data_len_reg = MIPIA_DATA_LEN_REG;
+                pkg_sender->mipi_cmd_addr_reg = MIPIA_CMD_ADD_REG;
+                pkg_sender->mipi_cmd_len_reg = MIPIA_CMD_LEN_REG;
+        } else if (pipe == 2) {
+                pkg_sender->dpll_reg = MRST_DPLL_A;
+                pkg_sender->dspcntr_reg = DSPCCNTR;
+                pkg_sender->pipeconf_reg = PIPECCONF;
+                pkg_sender->dsplinoff_reg = DSPCLINOFF;
+                pkg_sender->dspsurf_reg = DSPCSURF;
+                pkg_sender->pipestat_reg = PIPECSTAT;
+
+                pkg_sender->mipi_intr_stat_reg =
+                                MIPIA_INTR_STAT_REG + MIPIC_REG_OFFSET;
+                pkg_sender->mipi_lp_gen_data_reg =
+                                MIPIA_LP_GEN_DATA_REG + MIPIC_REG_OFFSET;
+                pkg_sender->mipi_hs_gen_data_reg =
+                                MIPIA_HS_GEN_DATA_REG + MIPIC_REG_OFFSET;
+                pkg_sender->mipi_lp_gen_ctrl_reg =
+                                MIPIA_LP_GEN_CTRL_REG + MIPIC_REG_OFFSET;
+                pkg_sender->mipi_hs_gen_ctrl_reg =
+                                MIPIA_HS_GEN_CTRL_REG + MIPIC_REG_OFFSET;
+                pkg_sender->mipi_gen_fifo_stat_reg =
+                                MIPIA_GEN_FIFO_STAT_REG + MIPIC_REG_OFFSET;
+                pkg_sender->mipi_data_addr_reg =
+                                MIPIA_DATA_ADD_REG + MIPIC_REG_OFFSET;
+                pkg_sender->mipi_data_len_reg =
+                                MIPIA_DATA_LEN_REG + MIPIC_REG_OFFSET;
+                pkg_sender->mipi_cmd_addr_reg =
+                                MIPIA_CMD_ADD_REG + MIPIC_REG_OFFSET;
+                pkg_sender->mipi_cmd_len_reg =
+                                MIPIA_CMD_LEN_REG + MIPIC_REG_OFFSET;
+        }
+
+        /* Init pkg list */
+        INIT_LIST_HEAD(&pkg_sender->pkg_list);
+        INIT_LIST_HEAD(&pkg_sender->free_list);
+
+        spin_lock_init(&pkg_sender->lock);
+
+        /* Allocate free pkg pool */
+        for (i = 0; i < MDFLD_MAX_PKG_NUM; i++) {
+                pkg = kzalloc(sizeof(struct mdfld_dsi_pkg), GFP_KERNEL);
+                if (!pkg) {
+                        dev_err(dev->dev, "Out of memory allocating pkg pool");
+                        ret = -ENOMEM;
+                        goto pkg_alloc_err;
+                }
+                INIT_LIST_HEAD(&pkg->entry);
+                list_add_tail(&pkg->entry, &pkg_sender->free_list);
+        }
+
+        /*
+         * For video mode, don't enable DPI timing output here,
+         * will init the DPI timing output during mode setting.
+         */
+        if (dsi_config->type == MDFLD_DSI_ENCODER_DPI)
+                mipi_val = PASS_FROM_SPHY_TO_AFE | SEL_FLOPPED_HSTX;
+        else if (dsi_config->type == MDFLD_DSI_ENCODER_DBI)
+                mipi_val = PASS_FROM_SPHY_TO_AFE | SEL_FLOPPED_HSTX
+                        | TE_TRIGGER_GPIO_PIN;
+        else
+                DRM_ERROR("Bad DSI encoder type\n");
+
+        if (pipe == 0) {
+                mipi_val |= 0x2;
+                REG_WRITE(MIPI, mipi_val);
+                REG_READ(MIPI);
+        } else if (pipe == 2) {
+                REG_WRITE(MIPI_C, mipi_val);
+                REG_READ(MIPI_C);
+        }
+
+        /*do dsi controller init*/
+        dsi_controller_init(dsi_config, pipe);
+        
+        return 0;
+
+pkg_alloc_err:
+        list_for_each_entry_safe(pkg, tmp, &pkg_sender->free_list, entry) {
+                list_del(&pkg->entry);
+                kfree(pkg);
+        }
+
+        /* Free mapped command buffer */
+        mdfld_dbi_cb_destroy(pkg_sender);
+mapping_err:
+        kfree(pkg_sender);
+        dsi_connector->pkg_sender = NULL;
+        return ret;
+}
+
+void mdfld_dsi_pkg_sender_destroy(struct mdfld_dsi_pkg_sender *sender)
+{
+        struct mdfld_dsi_pkg *pkg, *tmp;
+
+        if (!sender || IS_ERR(sender))
+                return;
+
+        /* Free pkg pool */
+        list_for_each_entry_safe(pkg, tmp, &sender->free_list, entry) {
+                list_del(&pkg->entry);
+                kfree(pkg);
+        }
+        /* Free pkg list */
+        list_for_each_entry_safe(pkg, tmp, &sender->pkg_list, entry) {
+                list_del(&pkg->entry);
+                kfree(pkg);
+        }
+        mdfld_dbi_cb_destroy(sender);   /* free mapped command buffer */
+        kfree(sender);
+}
diff --git a/drivers/staging/gma500/mdfld_dsi_pkg_sender.h b/drivers/staging/gma500/mdfld_dsi_pkg_sender.h
new file mode 100644
index 00000000000..f24abc70068
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_dsi_pkg_sender.h
@@ -0,0 +1,184 @@
+/*
+ * Copyright © 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:
+ * Jackie Li<yaodong.li@intel.com>
+ */
+#ifndef __MDFLD_DSI_PKG_SENDER_H__
+#define __MDFLD_DSI_PKG_SENDER_H__
+
+#include <linux/kthread.h>
+
+#define MDFLD_MAX_DCS_PARAM     8
+#define MDFLD_MAX_PKG_NUM       2048
+
+enum {
+        MDFLD_DSI_PKG_DCS,
+        MDFLD_DSI_PKG_GEN_SHORT_WRITE_0 = 0x03,
+        MDFLD_DSI_PKG_GEN_SHORT_WRITE_1 = 0x13,
+        MDFLD_DSI_PKG_GEN_SHORT_WRITE_2 = 0x23,
+        MDFLD_DSI_PKG_GEN_READ_0 = 0x04,
+        MDFLD_DSI_PKG_GEN_READ_1 = 0x14,
+        MDFLD_DSI_PKG_GEN_READ_2 = 0x24,
+        MDFLD_DSI_PKG_GEN_LONG_WRITE = 0x29,
+        MDFLD_DSI_PKG_MCS_SHORT_WRITE_0 = 0x05,
+        MDFLD_DSI_PKG_MCS_SHORT_WRITE_1 = 0x15,
+        MDFLD_DSI_PKG_MCS_READ = 0x06,
+        MDFLD_DSI_PKG_MCS_LONG_WRITE = 0x39,
+};
+
+enum {
+        MDFLD_DSI_LP_TRANSMISSION,
+        MDFLD_DSI_HS_TRANSMISSION,
+        MDFLD_DSI_DCS,
+};
+
+enum {
+        MDFLD_DSI_PANEL_MODE_SLEEP = 0x1,
+};
+
+enum {
+        MDFLD_DSI_PKG_SENDER_FREE = 0x0,
+        MDFLD_DSI_PKG_SENDER_BUSY = 0x1,
+};
+
+enum {
+        MDFLD_DSI_SEND_PACKAGE,
+        MDFLD_DSI_QUEUE_PACKAGE,
+};
+
+struct mdfld_dsi_gen_short_pkg {
+        u8 cmd;
+        u8 param;
+};
+
+struct mdfld_dsi_gen_long_pkg {
+        u32 *data;
+        u32 len;
+};
+
+struct mdfld_dsi_dcs_pkg {
+        u8 cmd;
+        u8 param[MDFLD_MAX_DCS_PARAM];
+        u32 param_num;
+        u8 data_src;
+};
+
+struct mdfld_dsi_pkg {
+        u8 pkg_type;
+        u8 transmission_type;
+
+        union {
+                struct mdfld_dsi_gen_short_pkg short_pkg;
+                struct mdfld_dsi_gen_long_pkg long_pkg;
+                struct mdfld_dsi_dcs_pkg dcs_pkg;
+        } pkg;
+
+        struct list_head entry;
+};
+
+struct mdfld_dsi_pkg_sender {
+        struct drm_device *dev;
+        struct mdfld_dsi_connector *dsi_connector;
+        u32 status;
+
+        u32 panel_mode;
+
+        int pipe;
+
+        spinlock_t lock;
+        struct list_head pkg_list;
+        struct list_head free_list;
+
+        u32 pkg_num;
+
+        int dbi_pkg_support;
+
+        u32 dbi_cb_phy;
+        void *dbi_cb_addr;
+
+        /* Registers */
+        u32 dpll_reg;
+        u32 dspcntr_reg;
+        u32 pipeconf_reg;
+        u32 pipestat_reg;
+        u32 dsplinoff_reg;
+        u32 dspsurf_reg;
+
+        u32 mipi_intr_stat_reg;
+        u32 mipi_lp_gen_data_reg;
+        u32 mipi_hs_gen_data_reg;
+        u32 mipi_lp_gen_ctrl_reg;
+        u32 mipi_hs_gen_ctrl_reg;
+        u32 mipi_gen_fifo_stat_reg;
+        u32 mipi_data_addr_reg;
+        u32 mipi_data_len_reg;
+        u32 mipi_cmd_addr_reg;
+        u32 mipi_cmd_len_reg;
+};
+
+/* DCS definitions */
+#define DCS_SOFT_RESET                  0x01
+#define DCS_ENTER_SLEEP_MODE            0x10
+#define DCS_EXIT_SLEEP_MODE             0x11
+#define DCS_SET_DISPLAY_OFF             0x28
+#define DCS_SET_DISPLAY_ON              0x29
+#define DCS_SET_COLUMN_ADDRESS          0x2a
+#define DCS_SET_PAGE_ADDRESS            0x2b
+#define DCS_WRITE_MEM_START             0x2c
+#define DCS_SET_TEAR_OFF                0x34
+#define DCS_SET_TEAR_ON                 0x35
+
+extern int mdfld_dsi_pkg_sender_init(struct mdfld_dsi_connector *dsi_connector,
+                        int pipe);
+extern void mdfld_dsi_pkg_sender_destroy(struct mdfld_dsi_pkg_sender *sender);
+extern int mdfld_dsi_send_dcs(struct mdfld_dsi_pkg_sender *sender, u8 dcs,
+                        u8 *param, u32 param_num, u8 data_src, int delay);
+extern int mdfld_dsi_send_mcs_short_hs(struct mdfld_dsi_pkg_sender *sender,
+                        u8 cmd, u8 param, u8 param_num, int delay);
+extern int mdfld_dsi_send_mcs_short_lp(struct mdfld_dsi_pkg_sender *sender,
+                        u8 cmd, u8 param, u8 param_num, int delay);
+extern int mdfld_dsi_send_mcs_long_hs(struct mdfld_dsi_pkg_sender *sender,
+                        u32 *data, u32 len, int delay);
+extern int mdfld_dsi_send_mcs_long_lp(struct mdfld_dsi_pkg_sender *sender,
+                        u32 *data, u32 len, int delay);
+extern int mdfld_dsi_send_gen_short_hs(struct mdfld_dsi_pkg_sender *sender,
+                        u8 param0, u8 param1, u8 param_num, int delay);
+extern int mdfld_dsi_send_gen_short_lp(struct mdfld_dsi_pkg_sender *sender,
+                        u8 param0, u8 param1, u8 param_num, int delay);
+extern int mdfld_dsi_send_gen_long_hs(struct mdfld_dsi_pkg_sender *sender,
+                        u32 *data, u32 len, int delay);
+extern int mdfld_dsi_send_gen_long_lp(struct mdfld_dsi_pkg_sender *sender,
+                        u32 *data, u32 len, int delay);
+
+extern int mdfld_dsi_read_gen_hs(struct mdfld_dsi_pkg_sender *sender,
+                        u8 param0, u8 param1, u8 param_num, u32 *data, u16 len);
+extern int mdfld_dsi_read_gen_lp(struct mdfld_dsi_pkg_sender *sender,
+                        u8 param0, u8 param1, u8 param_num, u32 *data, u16 len);
+extern int mdfld_dsi_read_mcs_hs(struct mdfld_dsi_pkg_sender *sender,
+                        u8 cmd, u32 *data, u16 len);
+extern int mdfld_dsi_read_mcs_lp(struct mdfld_dsi_pkg_sender *sender,
+                        u8 cmd, u32 *data, u16 len);
+
+extern void mdfld_dsi_cmds_kick_out(struct mdfld_dsi_pkg_sender *sender);
+
+#endif /* __MDFLD_DSI_PKG_SENDER_H__ */
diff --git a/drivers/staging/gma500/mdfld_intel_display.c b/drivers/staging/gma500/mdfld_intel_display.c
new file mode 100644
index 00000000000..aa2ff559383
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_intel_display.c
@@ -0,0 +1,1402 @@
+/*
+ * Copyright © 2006-2011 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>
+ */
+
+#include "framebuffer.h"
+#include "psb_intel_display.h"
+#include "mdfld_dsi_dbi.h"
+#include "mdfld_dsi_dpi.h"
+#include "mdfld_dsi_dbi_dpu.h"
+
+#include <linux/pm_runtime.h>
+
+#ifdef MIN
+#undef MIN
+#endif
+
+#define MIN(x, y) (((x) < (y)) ? (x) : (y))
+
+/* Hardcoded currently */
+static int ksel = KSEL_CRYSTAL_19;
+
+extern void mdfld_save_display(struct drm_device *dev);
+extern bool gbgfxsuspended;
+
+struct psb_intel_range_t {
+        int min, max;
+};
+
+struct mdfld_limit_t {
+        struct psb_intel_range_t dot, m, p1;
+};
+
+struct mdfld_intel_clock_t {
+        /* given values */
+        int n;
+        int m1, m2;
+        int p1, p2;
+        /* derived values */
+        int dot;
+        int vco;
+        int m;
+        int p;
+};
+
+
+
+#define COUNT_MAX 0x10000000
+
+void mdfldWaitForPipeDisable(struct drm_device *dev, int pipe)
+{
+        int count, temp;
+        u32 pipeconf_reg = PIPEACONF;
+        
+        switch (pipe) {
+        case 0:
+                break;
+        case 1:
+                pipeconf_reg = PIPEBCONF;
+                break;
+        case 2:
+                pipeconf_reg = PIPECCONF;
+                break;
+        default:
+                DRM_ERROR("Illegal Pipe Number. \n");
+                return;
+        }
+
+        /* FIXME JLIU7_PO */
+        psb_intel_wait_for_vblank(dev);
+        return;
+
+        /* Wait for for the pipe disable to take effect. */
+        for (count = 0; count < COUNT_MAX; count++) {
+                temp = REG_READ(pipeconf_reg);
+                if ((temp & PIPEACONF_PIPE_STATE) == 0)
+                        break;
+        }
+}
+
+void mdfldWaitForPipeEnable(struct drm_device *dev, int pipe)
+{
+        int count, temp;
+        u32 pipeconf_reg = PIPEACONF;
+        
+        switch (pipe) {
+        case 0:
+                break;
+        case 1:
+                pipeconf_reg = PIPEBCONF;
+                break;
+        case 2:
+                pipeconf_reg = PIPECCONF;
+                break;
+        default:
+                dev_err(dev->dev, "Illegal Pipe Number.\n");
+                return;
+        }
+
+        /* FIXME JLIU7_PO */
+        psb_intel_wait_for_vblank(dev);
+        return;
+
+        /* Wait for for the pipe enable to take effect. */
+        for (count = 0; count < COUNT_MAX; count++) {
+                temp = REG_READ(pipeconf_reg);
+                if ((temp & PIPEACONF_PIPE_STATE) == 1)
+                        break;
+        }
+}
+
+
+static int mdfld_intel_crtc_cursor_set(struct drm_crtc *crtc,
+                                 struct drm_file *file_priv,
+                                 uint32_t handle,
+                                 uint32_t width, uint32_t height)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        uint32_t control = CURACNTR;
+        uint32_t base = CURABASE;
+        uint32_t temp;
+        size_t addr = 0;
+        struct gtt_range *gt;
+        struct drm_gem_object *obj;
+        int ret;
+
+        switch (pipe) {
+        case 0:
+                break;
+        case 1:
+                control = CURBCNTR;
+                base = CURBBASE;
+                break;
+        case 2:
+                control = CURCCNTR;
+                base = CURCBASE;
+                break;
+        default:
+                dev_err(dev->dev, "Illegal Pipe Number. \n");
+                return -EINVAL;
+        }
+        
+#if 1 /* FIXME_JLIU7 can't enalbe cursorB/C HW issue. need to remove after HW fix */
+        if (pipe != 0)
+                return 0;
+#endif 
+        /* if we want to turn of the cursor ignore width and height */
+        if (!handle) {
+                dev_dbg(dev->dev, "cursor off\n");
+                /* turn off the cursor */
+                temp = 0;
+                temp |= CURSOR_MODE_DISABLE;
+
+                if (gma_power_begin(dev, true)) {
+                        REG_WRITE(control, temp);
+                        REG_WRITE(base, 0);
+                        gma_power_end(dev);
+                }
+                /* Unpin the old GEM object */
+                if (psb_intel_crtc->cursor_obj) {
+                        gt = container_of(psb_intel_crtc->cursor_obj,
+                                                        struct gtt_range, gem);
+                        psb_gtt_unpin(gt);
+                        drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
+                        psb_intel_crtc->cursor_obj = NULL;
+                }
+                return 0;
+        }
+
+        /* Currently we only support 64x64 cursors */
+        if (width != 64 || height != 64) {
+                DRM_ERROR("we currently only support 64x64 cursors\n");
+                return -EINVAL;
+        }
+
+        obj = drm_gem_object_lookup(dev, file_priv, handle);
+        if (!obj)
+                return -ENOENT;
+
+        if (obj->size < width * height * 4) {
+                dev_dbg(dev->dev, "buffer is to small\n");
+                return -ENOMEM;
+        }
+
+        gt = container_of(obj, struct gtt_range, gem);
+
+        /* Pin the memory into the GTT */
+        ret = psb_gtt_pin(gt);
+        if (ret) {
+                dev_err(dev->dev, "Can not pin down handle 0x%x\n", handle);
+                return ret;
+        }
+
+
+        addr = gt->offset;      /* Or resource.start ??? */
+
+        psb_intel_crtc->cursor_addr = addr;
+
+        temp = 0;
+        /* set the pipe for the cursor */
+        temp |= (pipe << 28);
+        temp |= CURSOR_MODE_64_ARGB_AX | MCURSOR_GAMMA_ENABLE;
+
+        if (gma_power_begin(dev, true)) {
+                REG_WRITE(control, temp);
+                REG_WRITE(base, addr);
+                gma_power_end(dev);
+        }
+        /* unpin the old GEM object */
+        if (psb_intel_crtc->cursor_obj) {
+                gt = container_of(psb_intel_crtc->cursor_obj,
+                                                        struct gtt_range, gem);
+                psb_gtt_unpin(gt);
+                drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
+                psb_intel_crtc->cursor_obj = obj;
+        }
+        return 0;
+}
+
+static int mdfld_intel_crtc_cursor_move(struct drm_crtc *crtc, int x, int y)
+{
+        struct drm_device *dev = crtc->dev;
+        struct drm_psb_private * dev_priv = (struct drm_psb_private *)dev->dev_private;
+        struct mdfld_dbi_dpu_info *dpu_info = dev_priv->dbi_dpu_info;
+        struct psb_drm_dpu_rect rect;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        uint32_t pos = CURAPOS;
+        uint32_t base = CURABASE;
+        uint32_t temp = 0;
+        uint32_t addr;
+
+        switch (pipe) {
+        case 0:
+                if (dpu_info) {
+                        rect.x = x;
+                        rect.y = y;
+                
+                        mdfld_dbi_dpu_report_damage(dev, MDFLD_CURSORA, &rect);
+                        mdfld_dpu_exit_dsr(dev);
+                } else if (!(dev_priv->dsr_fb_update & MDFLD_DSR_CURSOR_0))
+                        mdfld_dsi_dbi_exit_dsr(dev, MDFLD_DSR_CURSOR_0);
+                break;
+        case 1:
+                pos = CURBPOS;
+                base = CURBBASE;
+                break;
+        case 2:
+                if (dpu_info) {
+                        mdfld_dbi_dpu_report_damage(dev, MDFLD_CURSORC, &rect);
+                        mdfld_dpu_exit_dsr(dev);
+                } else if (!(dev_priv->dsr_fb_update & MDFLD_DSR_CURSOR_2))
+                        mdfld_dsi_dbi_exit_dsr(dev, MDFLD_DSR_CURSOR_2);
+                pos = CURCPOS;
+                base = CURCBASE;
+                break;
+        default:
+                DRM_ERROR("Illegal Pipe Number. \n");
+                return -EINVAL;
+        }
+                
+#if 1 /* FIXME_JLIU7 can't enalbe cursorB/C HW issue. need to remove after HW fix */
+        if (pipe != 0)
+                return 0;
+#endif 
+        if (x < 0) {
+                temp |= (CURSOR_POS_SIGN << CURSOR_X_SHIFT);
+                x = -x;
+        }
+        if (y < 0) {
+                temp |= (CURSOR_POS_SIGN << CURSOR_Y_SHIFT);
+                y = -y;
+        }
+
+        temp |= ((x & CURSOR_POS_MASK) << CURSOR_X_SHIFT);
+        temp |= ((y & CURSOR_POS_MASK) << CURSOR_Y_SHIFT);
+
+        addr = psb_intel_crtc->cursor_addr;
+
+        if (gma_power_begin(dev, true)) {
+                REG_WRITE(pos, temp);
+                REG_WRITE(base, addr);
+                gma_power_end(dev);
+        }
+
+        return 0;
+}
+
+const struct drm_crtc_funcs mdfld_intel_crtc_funcs = {
+        .cursor_set = mdfld_intel_crtc_cursor_set,
+        .cursor_move = mdfld_intel_crtc_cursor_move,
+        .gamma_set = psb_intel_crtc_gamma_set,
+        .set_config = drm_crtc_helper_set_config,
+        .destroy = psb_intel_crtc_destroy,
+};
+
+static struct drm_device globle_dev;
+
+void mdfld__intel_plane_set_alpha(int enable)
+{
+        struct drm_device *dev = &globle_dev;
+        int dspcntr_reg = DSPACNTR;
+        u32 dspcntr;
+
+        dspcntr = REG_READ(dspcntr_reg);
+
+        if (enable) {
+                dspcntr &= ~DISPPLANE_32BPP_NO_ALPHA;
+                dspcntr |= DISPPLANE_32BPP;
+        } else {
+                dspcntr &= ~DISPPLANE_32BPP;
+                dspcntr |= DISPPLANE_32BPP_NO_ALPHA;
+        }
+
+        REG_WRITE(dspcntr_reg, dspcntr);
+}
+
+int mdfld__intel_pipe_set_base(struct drm_crtc *crtc, int x, int y, struct drm_framebuffer *old_fb)
+{
+        struct drm_device *dev = crtc->dev;
+        /* struct drm_i915_master_private *master_priv; */
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
+        int pipe = psb_intel_crtc->pipe;
+        unsigned long start, offset;
+        int dsplinoff = DSPALINOFF;
+        int dspsurf = DSPASURF;
+        int dspstride = DSPASTRIDE;
+        int dspcntr_reg = DSPACNTR;
+        u32 dspcntr;
+        int ret = 0;
+
+        memcpy(&globle_dev, dev, sizeof(struct drm_device));
+
+        if (!gma_power_begin(dev, true))
+                return 0;
+
+        /* no fb bound */
+        if (!crtc->fb) {
+                dev_err(dev->dev, "No FB bound\n");
+                goto psb_intel_pipe_cleaner;
+        }
+
+        switch (pipe) {
+        case 0:
+                dsplinoff = DSPALINOFF;
+                break;
+        case 1:
+                dsplinoff = DSPBLINOFF;
+                dspsurf = DSPBSURF;
+                dspstride = DSPBSTRIDE;
+                dspcntr_reg = DSPBCNTR;
+                break;
+        case 2:
+                dsplinoff = DSPCLINOFF;
+                dspsurf = DSPCSURF;
+                dspstride = DSPCSTRIDE;
+                dspcntr_reg = DSPCCNTR;
+                break;
+        default:
+                dev_err(dev->dev, "Illegal Pipe Number.\n");
+                return -EINVAL;
+        }
+
+        ret = psb_gtt_pin(psbfb->gtt);
+        if (ret < 0)
+                goto psb_intel_pipe_set_base_exit;
+
+        start = psbfb->gtt->offset;
+        offset = y * crtc->fb->pitch + x * (crtc->fb->bits_per_pixel / 8);
+
+        REG_WRITE(dspstride, crtc->fb->pitch);
+        dspcntr = REG_READ(dspcntr_reg);
+        dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
+
+        switch (crtc->fb->bits_per_pixel) {
+        case 8:
+                dspcntr |= DISPPLANE_8BPP;
+                break;
+        case 16:
+                if (crtc->fb->depth == 15)
+                        dspcntr |= DISPPLANE_15_16BPP;
+                else
+                        dspcntr |= DISPPLANE_16BPP;
+                break;
+        case 24:
+        case 32:
+                dspcntr |= DISPPLANE_32BPP_NO_ALPHA;
+                break;
+        default:
+                dev_err(dev->dev, "Unknown color depth\n");
+                ret = -EINVAL;
+                goto psb_intel_pipe_set_base_exit;
+        }
+        REG_WRITE(dspcntr_reg, dspcntr);
+
+        dev_dbg(dev->dev, "Writing base %08lX %08lX %d %d\n",
+                                                start, offset, x, y);
+
+        REG_WRITE(dsplinoff, offset);
+        REG_READ(dsplinoff);
+        REG_WRITE(dspsurf, start);
+        REG_READ(dspsurf);
+
+psb_intel_pipe_cleaner:
+        /* If there was a previous display we can now unpin it */
+        if (old_fb)
+                psb_gtt_unpin(to_psb_fb(old_fb)->gtt);
+
+psb_intel_pipe_set_base_exit:
+        gma_power_end(dev);
+        return ret;
+}
+
+/**
+ * Disable the pipe, plane and pll.
+ *
+ */
+void mdfld_disable_crtc (struct drm_device *dev, int pipe)
+{
+        int dpll_reg = MRST_DPLL_A;
+        int dspcntr_reg = DSPACNTR;
+        int dspbase_reg = MRST_DSPABASE;
+        int pipeconf_reg = PIPEACONF;
+        u32 gen_fifo_stat_reg = GEN_FIFO_STAT_REG;
+        u32 temp;
+
+        switch (pipe) {
+        case 0:
+                break;
+        case 1:
+                dpll_reg = MDFLD_DPLL_B;
+                dspcntr_reg = DSPBCNTR;
+                dspbase_reg = DSPBSURF;
+                pipeconf_reg = PIPEBCONF;
+                break;
+        case 2:
+                dpll_reg = MRST_DPLL_A;
+                dspcntr_reg = DSPCCNTR;
+                dspbase_reg = MDFLD_DSPCBASE;
+                pipeconf_reg = PIPECCONF;
+                gen_fifo_stat_reg = GEN_FIFO_STAT_REG + MIPIC_REG_OFFSET;
+                break;
+        default:
+                dev_err(dev->dev, "Illegal Pipe Number. \n");
+                return;
+        }
+
+        if (pipe != 1)
+                mdfld_dsi_gen_fifo_ready (dev, gen_fifo_stat_reg, HS_CTRL_FIFO_EMPTY | HS_DATA_FIFO_EMPTY);
+
+        /* Disable display plane */
+        temp = REG_READ(dspcntr_reg);
+        if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
+                REG_WRITE(dspcntr_reg,
+                          temp & ~DISPLAY_PLANE_ENABLE);
+                /* Flush the plane changes */
+                REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+                REG_READ(dspbase_reg);
+        }
+
+        /* FIXME_JLIU7 MDFLD_PO revisit */
+        /* Wait for vblank for the disable to take effect */
+// MDFLD_PO_JLIU7               psb_intel_wait_for_vblank(dev);
+
+        /* Next, disable display pipes */
+        temp = REG_READ(pipeconf_reg);
+        if ((temp & PIPEACONF_ENABLE) != 0) {
+                temp &= ~PIPEACONF_ENABLE;
+                temp |= PIPECONF_PLANE_OFF | PIPECONF_CURSOR_OFF;
+                REG_WRITE(pipeconf_reg, temp);
+                REG_READ(pipeconf_reg);
+
+                /* Wait for for the pipe disable to take effect. */
+                mdfldWaitForPipeDisable(dev, pipe);
+        }
+
+        temp = REG_READ(dpll_reg);
+        if (temp & DPLL_VCO_ENABLE) {
+                if (((pipe != 1) && !((REG_READ(PIPEACONF) | REG_READ(PIPECCONF)) & PIPEACONF_ENABLE))
+                                || (pipe == 1)){
+                        temp &= ~(DPLL_VCO_ENABLE);
+                        REG_WRITE(dpll_reg, temp);
+                        REG_READ(dpll_reg);
+                        /* Wait for the clocks to turn off. */
+                        /* FIXME_MDFLD PO may need more delay */
+                        udelay(500);
+
+                        if (!(temp & MDFLD_PWR_GATE_EN)) {
+                                /* gating power of DPLL */
+                                REG_WRITE(dpll_reg, temp | MDFLD_PWR_GATE_EN);
+                                /* FIXME_MDFLD PO - change 500 to 1 after PO */
+                                udelay(5000);
+                        }
+                }
+        }
+
+}
+
+/**
+ * Sets the power management mode of the pipe and plane.
+ *
+ * This code should probably grow support for turning the cursor off and back
+ * on appropriately at the same time as we're turning the pipe off/on.
+ */
+static void mdfld_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+        struct drm_device *dev = crtc->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        int dpll_reg = MRST_DPLL_A;
+        int dspcntr_reg = DSPACNTR;
+        int dspbase_reg = MRST_DSPABASE;
+        int pipeconf_reg = PIPEACONF;
+        u32 pipestat_reg = PIPEASTAT;
+        u32 gen_fifo_stat_reg = GEN_FIFO_STAT_REG;
+        u32 pipeconf = dev_priv->pipeconf;
+        u32 dspcntr = dev_priv->dspcntr;
+        u32 mipi_enable_reg = MIPIA_DEVICE_READY_REG;
+        u32 temp;
+        bool enabled;
+        int timeout = 0;
+
+        if (!gma_power_begin(dev, true))
+                return;
+
+         /* Ignore if system is already in DSR and in suspended state. */
+        if(/*gbgfxsuspended */0 && dev_priv->dispstatus == false && mode == 3){
+            if(dev_priv->rpm_enabled && pipe == 1){
+        //          dev_priv->is_mipi_on = false;
+                  pm_request_idle(&dev->pdev->dev);
+            }
+            return;
+        }else if(mode == 0) {
+                //do not need to set gbdispstatus=true in crtc.
+                //this will be set in encoder such as mdfld_dsi_dbi_dpms
+            //gbdispstatus = true;
+        }
+
+
+/* FIXME_JLIU7 MDFLD_PO replaced w/ the following function */
+/* mdfld_dbi_dpms (struct drm_device *dev, int pipe, bool enabled) */
+
+        switch (pipe) {
+        case 0:
+                break;
+        case 1:
+                dpll_reg = DPLL_B;
+                dspcntr_reg = DSPBCNTR;
+                dspbase_reg = MRST_DSPBBASE;
+                pipeconf_reg = PIPEBCONF;
+                pipeconf = dev_priv->pipeconf1;
+                dspcntr = dev_priv->dspcntr1;
+                dpll_reg = MDFLD_DPLL_B;
+                break;
+        case 2:
+                dpll_reg = MRST_DPLL_A;
+                dspcntr_reg = DSPCCNTR;
+                dspbase_reg = MDFLD_DSPCBASE;
+                pipeconf_reg = PIPECCONF;
+                pipestat_reg = PIPECSTAT;
+                pipeconf = dev_priv->pipeconf2;
+                dspcntr = dev_priv->dspcntr2;
+                gen_fifo_stat_reg = GEN_FIFO_STAT_REG + MIPIC_REG_OFFSET;
+                mipi_enable_reg = MIPIA_DEVICE_READY_REG + MIPIC_REG_OFFSET;
+                break;
+        default:
+                dev_err(dev->dev, "Illegal Pipe Number.\n");
+                return;
+        }
+
+        /* XXX: When our outputs are all unaware of DPMS modes other than off
+         * and on, we should map those modes to DRM_MODE_DPMS_OFF in the CRTC.
+         */
+        switch (mode) {
+        case DRM_MODE_DPMS_ON:
+        case DRM_MODE_DPMS_STANDBY:
+        case DRM_MODE_DPMS_SUSPEND:
+                /* Enable the DPLL */
+                temp = REG_READ(dpll_reg);
+
+                if ((temp & DPLL_VCO_ENABLE) == 0) {
+                        /* When ungating power of DPLL, needs to wait 0.5us before enable the VCO */
+                        if (temp & MDFLD_PWR_GATE_EN) {
+                                temp &= ~MDFLD_PWR_GATE_EN;
+                                REG_WRITE(dpll_reg, temp);
+                                /* FIXME_MDFLD PO - change 500 to 1 after PO */
+                                udelay(500);
+                        }
+
+                        REG_WRITE(dpll_reg, temp);
+                        REG_READ(dpll_reg);
+                        /* FIXME_MDFLD PO - change 500 to 1 after PO */
+                        udelay(500);
+                        
+                        REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+                        REG_READ(dpll_reg);
+
+                        /**
+                         * wait for DSI PLL to lock
+                         * NOTE: only need to poll status of pipe 0 and pipe 1,
+                         * since both MIPI pipes share the same PLL.
+                         */
+                        while ((pipe != 2) && (timeout < 20000) && !(REG_READ(pipeconf_reg) & PIPECONF_DSIPLL_LOCK)) {
+                                udelay(150);
+                                timeout ++;
+                        }
+                }
+
+                /* Enable the plane */
+                temp = REG_READ(dspcntr_reg);
+                if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
+                        REG_WRITE(dspcntr_reg,
+                                temp | DISPLAY_PLANE_ENABLE);
+                        /* Flush the plane changes */
+                        REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+                }
+
+                /* Enable the pipe */
+                temp = REG_READ(pipeconf_reg);
+                if ((temp & PIPEACONF_ENABLE) == 0) {
+                        REG_WRITE(pipeconf_reg, pipeconf);
+
+                        /* Wait for for the pipe enable to take effect. */
+                        mdfldWaitForPipeEnable(dev, pipe);
+                }
+
+                /*workaround for sighting 3741701 Random X blank display*/
+                /*perform w/a in video mode only on pipe A or C*/
+                if ((pipe == 0 || pipe == 2) &&
+                        (mdfld_panel_dpi(dev) == true)) {
+                        REG_WRITE(pipestat_reg, REG_READ(pipestat_reg));
+                        msleep(100);
+                        if(PIPE_VBLANK_STATUS & REG_READ(pipestat_reg)) {
+                                printk(KERN_ALERT "OK");
+                        } else {
+                                printk(KERN_ALERT "STUCK!!!!");
+                                /*shutdown controller*/
+                                temp = REG_READ(dspcntr_reg);
+                                REG_WRITE(dspcntr_reg, temp & ~DISPLAY_PLANE_ENABLE);
+                                REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+                                /*mdfld_dsi_dpi_shut_down(dev, pipe);*/
+                                REG_WRITE(0xb048, 1);
+                                msleep(100);
+                                temp = REG_READ(pipeconf_reg);
+                                temp &= ~PIPEACONF_ENABLE;
+                                REG_WRITE(pipeconf_reg, temp);
+                                msleep(100); /*wait for pipe disable*/
+                        /*printk(KERN_ALERT "70008 is %x\n", REG_READ(0x70008));
+                        printk(KERN_ALERT "b074 is %x\n", REG_READ(0xb074));*/
+                                REG_WRITE(mipi_enable_reg, 0);
+                                msleep(100);
+                        printk(KERN_ALERT "70008 is %x\n", REG_READ(0x70008));
+                        printk(KERN_ALERT "b074 is %x\n", REG_READ(0xb074));
+                                REG_WRITE(0xb004, REG_READ(0xb004));
+                                /* try to bring the controller back up again*/
+                                REG_WRITE(mipi_enable_reg, 1);
+                                temp = REG_READ(dspcntr_reg);
+                                REG_WRITE(dspcntr_reg, temp | DISPLAY_PLANE_ENABLE);
+                                REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+                                /*mdfld_dsi_dpi_turn_on(dev, pipe);*/
+                                REG_WRITE(0xb048, 2);
+                                msleep(100);
+                                temp = REG_READ(pipeconf_reg);
+                                temp |= PIPEACONF_ENABLE;
+                                REG_WRITE(pipeconf_reg, temp);
+                        }
+                }
+
+                psb_intel_crtc_load_lut(crtc);
+
+                /* Give the overlay scaler a chance to enable
+                   if it's on this pipe */
+                /* psb_intel_crtc_dpms_video(crtc, true); TODO */
+
+                break;
+        case DRM_MODE_DPMS_OFF:
+                /* Give the overlay scaler a chance to disable
+                 * if it's on this pipe */
+                /* psb_intel_crtc_dpms_video(crtc, FALSE); TODO */
+                if (pipe != 1)
+                        mdfld_dsi_gen_fifo_ready (dev, gen_fifo_stat_reg, HS_CTRL_FIFO_EMPTY | HS_DATA_FIFO_EMPTY);
+
+                /* Disable the VGA plane that we never use */
+                REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+
+                /* Disable display plane */
+                temp = REG_READ(dspcntr_reg);
+                if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
+                        REG_WRITE(dspcntr_reg,
+                                  temp & ~DISPLAY_PLANE_ENABLE);
+                        /* Flush the plane changes */
+                        REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+                        REG_READ(dspbase_reg);
+                }
+
+                /* FIXME_JLIU7 MDFLD_PO revisit */
+                /* Wait for vblank for the disable to take effect */
+// MDFLD_PO_JLIU7               psb_intel_wait_for_vblank(dev);
+
+                /* Next, disable display pipes */
+                temp = REG_READ(pipeconf_reg);
+                if ((temp & PIPEACONF_ENABLE) != 0) {
+                        temp &= ~PIPEACONF_ENABLE;
+                        temp |= PIPECONF_PLANE_OFF | PIPECONF_CURSOR_OFF;
+                        REG_WRITE(pipeconf_reg, temp);
+//                      REG_WRITE(pipeconf_reg, 0);
+                        REG_READ(pipeconf_reg);
+
+                        /* Wait for for the pipe disable to take effect. */
+                        mdfldWaitForPipeDisable(dev, pipe);
+                }
+
+                temp = REG_READ(dpll_reg);
+                if (temp & DPLL_VCO_ENABLE) {
+                        if (((pipe != 1) && !((REG_READ(PIPEACONF) | REG_READ(PIPECCONF)) & PIPEACONF_ENABLE))
+                                        || (pipe == 1)){
+                                temp &= ~(DPLL_VCO_ENABLE);
+                                REG_WRITE(dpll_reg, temp);
+                                REG_READ(dpll_reg);
+                                /* Wait for the clocks to turn off. */
+                                /* FIXME_MDFLD PO may need more delay */
+                                udelay(500);
+#if 0 /* MDFLD_PO_JLIU7 */      
+                if (!(temp & MDFLD_PWR_GATE_EN)) {
+                        /* gating power of DPLL */
+                        REG_WRITE(dpll_reg, temp | MDFLD_PWR_GATE_EN);
+                        /* FIXME_MDFLD PO - change 500 to 1 after PO */
+                        udelay(5000);
+                }
+#endif  /* MDFLD_PO_JLIU7 */    
+                        }
+                }
+                break;
+        }
+
+        enabled = crtc->enabled && mode != DRM_MODE_DPMS_OFF;
+
+#if 0                           /* JB: Add vblank support later */
+        if (enabled)
+                dev_priv->vblank_pipe |= (1 << pipe);
+        else
+                dev_priv->vblank_pipe &= ~(1 << pipe);
+#endif
+
+        gma_power_end(dev);
+}
+
+
+#define MDFLD_LIMT_DPLL_19          0
+#define MDFLD_LIMT_DPLL_25          1
+#define MDFLD_LIMT_DPLL_83          2
+#define MDFLD_LIMT_DPLL_100         3
+#define MDFLD_LIMT_DSIPLL_19        4
+#define MDFLD_LIMT_DSIPLL_25        5
+#define MDFLD_LIMT_DSIPLL_83        6
+#define MDFLD_LIMT_DSIPLL_100       7
+
+#define MDFLD_DOT_MIN             19750  /* FIXME_MDFLD JLIU7 need to find out  min & max for MDFLD */
+#define MDFLD_DOT_MAX             120000
+#define MDFLD_DPLL_M_MIN_19         113
+#define MDFLD_DPLL_M_MAX_19         155
+#define MDFLD_DPLL_P1_MIN_19        2
+#define MDFLD_DPLL_P1_MAX_19        10
+#define MDFLD_DPLL_M_MIN_25         101
+#define MDFLD_DPLL_M_MAX_25         130
+#define MDFLD_DPLL_P1_MIN_25        2
+#define MDFLD_DPLL_P1_MAX_25        10
+#define MDFLD_DPLL_M_MIN_83         64
+#define MDFLD_DPLL_M_MAX_83         64
+#define MDFLD_DPLL_P1_MIN_83        2
+#define MDFLD_DPLL_P1_MAX_83        2
+#define MDFLD_DPLL_M_MIN_100        64
+#define MDFLD_DPLL_M_MAX_100        64
+#define MDFLD_DPLL_P1_MIN_100       2
+#define MDFLD_DPLL_P1_MAX_100       2
+#define MDFLD_DSIPLL_M_MIN_19       131
+#define MDFLD_DSIPLL_M_MAX_19       175
+#define MDFLD_DSIPLL_P1_MIN_19      3
+#define MDFLD_DSIPLL_P1_MAX_19      8
+#define MDFLD_DSIPLL_M_MIN_25       97
+#define MDFLD_DSIPLL_M_MAX_25       140
+#define MDFLD_DSIPLL_P1_MIN_25      3
+#define MDFLD_DSIPLL_P1_MAX_25      9
+#define MDFLD_DSIPLL_M_MIN_83       33
+#define MDFLD_DSIPLL_M_MAX_83       92
+#define MDFLD_DSIPLL_P1_MIN_83      2
+#define MDFLD_DSIPLL_P1_MAX_83      3
+#define MDFLD_DSIPLL_M_MIN_100      97
+#define MDFLD_DSIPLL_M_MAX_100      140
+#define MDFLD_DSIPLL_P1_MIN_100     3
+#define MDFLD_DSIPLL_P1_MAX_100     9
+
+static const struct mdfld_limit_t mdfld_limits[] = {
+        {                       /* MDFLD_LIMT_DPLL_19 */
+         .dot = {.min = MDFLD_DOT_MIN, .max = MDFLD_DOT_MAX},
+         .m = {.min = MDFLD_DPLL_M_MIN_19, .max = MDFLD_DPLL_M_MAX_19},
+         .p1 = {.min = MDFLD_DPLL_P1_MIN_19, .max = MDFLD_DPLL_P1_MAX_19},
+         },
+        {                       /* MDFLD_LIMT_DPLL_25 */
+         .dot = {.min = MDFLD_DOT_MIN, .max = MDFLD_DOT_MAX},
+         .m = {.min = MDFLD_DPLL_M_MIN_25, .max = MDFLD_DPLL_M_MAX_25},
+         .p1 = {.min = MDFLD_DPLL_P1_MIN_25, .max = MDFLD_DPLL_P1_MAX_25},
+         },
+        {                       /* MDFLD_LIMT_DPLL_83 */
+         .dot = {.min = MDFLD_DOT_MIN, .max = MDFLD_DOT_MAX},
+         .m = {.min = MDFLD_DPLL_M_MIN_83, .max = MDFLD_DPLL_M_MAX_83},
+         .p1 = {.min = MDFLD_DPLL_P1_MIN_83, .max = MDFLD_DPLL_P1_MAX_83},
+         },
+        {                       /* MDFLD_LIMT_DPLL_100 */
+         .dot = {.min = MDFLD_DOT_MIN, .max = MDFLD_DOT_MAX},
+         .m = {.min = MDFLD_DPLL_M_MIN_100, .max = MDFLD_DPLL_M_MAX_100},
+         .p1 = {.min = MDFLD_DPLL_P1_MIN_100, .max = MDFLD_DPLL_P1_MAX_100},
+         },
+        {                       /* MDFLD_LIMT_DSIPLL_19 */
+         .dot = {.min = MDFLD_DOT_MIN, .max = MDFLD_DOT_MAX},
+         .m = {.min = MDFLD_DSIPLL_M_MIN_19, .max = MDFLD_DSIPLL_M_MAX_19},
+         .p1 = {.min = MDFLD_DSIPLL_P1_MIN_19, .max = MDFLD_DSIPLL_P1_MAX_19},
+         },
+        {                       /* MDFLD_LIMT_DSIPLL_25 */
+         .dot = {.min = MDFLD_DOT_MIN, .max = MDFLD_DOT_MAX},
+         .m = {.min = MDFLD_DSIPLL_M_MIN_25, .max = MDFLD_DSIPLL_M_MAX_25},
+         .p1 = {.min = MDFLD_DSIPLL_P1_MIN_25, .max = MDFLD_DSIPLL_P1_MAX_25},
+         },
+        {                       /* MDFLD_LIMT_DSIPLL_83 */
+         .dot = {.min = MDFLD_DOT_MIN, .max = MDFLD_DOT_MAX},
+         .m = {.min = MDFLD_DSIPLL_M_MIN_83, .max = MDFLD_DSIPLL_M_MAX_83},
+         .p1 = {.min = MDFLD_DSIPLL_P1_MIN_83, .max = MDFLD_DSIPLL_P1_MAX_83},
+         },
+        {                       /* MDFLD_LIMT_DSIPLL_100 */
+         .dot = {.min = MDFLD_DOT_MIN, .max = MDFLD_DOT_MAX},
+         .m = {.min = MDFLD_DSIPLL_M_MIN_100, .max = MDFLD_DSIPLL_M_MAX_100},
+         .p1 = {.min = MDFLD_DSIPLL_P1_MIN_100, .max = MDFLD_DSIPLL_P1_MAX_100},
+         },
+};
+
+#define MDFLD_M_MIN         21
+#define MDFLD_M_MAX         180
+static const u32 mdfld_m_converts[] = {
+/* M configuration table from 9-bit LFSR table */
+        224, 368, 440, 220, 366, 439, 219, 365, 182, 347, /* 21 - 30 */
+        173, 342, 171, 85, 298, 149, 74, 37, 18, 265,   /* 31 - 40 */
+        388, 194, 353, 432, 216, 108, 310, 155, 333, 166, /* 41 - 50 */
+        83, 41, 276, 138, 325, 162, 337, 168, 340, 170, /* 51 - 60 */
+        341, 426, 469, 234, 373, 442, 221, 110, 311, 411, /* 61 - 70 */
+        461, 486, 243, 377, 188, 350, 175, 343, 427, 213, /* 71 - 80 */
+        106, 53, 282, 397, 354, 227, 113, 56, 284, 142, /* 81 - 90 */
+        71, 35, 273, 136, 324, 418, 465, 488, 500, 506, /* 91 - 100 */
+        253, 126, 63, 287, 399, 455, 483, 241, 376, 444, /* 101 - 110 */
+        478, 495, 503, 251, 381, 446, 479, 239, 375, 443, /* 111 - 120 */
+        477, 238, 119, 315, 157, 78, 295, 147, 329, 420, /* 121 - 130 */
+        210, 105, 308, 154, 77, 38, 275, 137, 68, 290, /* 131 - 140 */
+        145, 328, 164, 82, 297, 404, 458, 485, 498, 249, /* 141 - 150 */
+        380, 190, 351, 431, 471, 235, 117, 314, 413, 206, /* 151 - 160 */
+        103, 51, 25, 12, 262, 387, 193, 96, 48, 280, /* 161 - 170 */
+        396, 198, 99, 305, 152, 76, 294, 403, 457, 228, /* 171 - 180 */
+};
+
+static const struct mdfld_limit_t *mdfld_limit(struct drm_crtc *crtc)
+{
+        const struct mdfld_limit_t *limit = NULL;
+        struct drm_device *dev = crtc->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (psb_intel_pipe_has_type(crtc, INTEL_OUTPUT_MIPI)
+            || psb_intel_pipe_has_type(crtc, INTEL_OUTPUT_MIPI2)) {
+                if ((ksel == KSEL_CRYSTAL_19) || (ksel == KSEL_BYPASS_19))
+                        limit = &mdfld_limits[MDFLD_LIMT_DSIPLL_19];
+                else if (ksel == KSEL_BYPASS_25) 
+                        limit = &mdfld_limits[MDFLD_LIMT_DSIPLL_25];
+                else if ((ksel == KSEL_BYPASS_83_100) && (dev_priv->core_freq == 166))
+                        limit = &mdfld_limits[MDFLD_LIMT_DSIPLL_83];
+                else if ((ksel == KSEL_BYPASS_83_100) &&
+                         (dev_priv->core_freq == 100 || dev_priv->core_freq == 200))
+                        limit = &mdfld_limits[MDFLD_LIMT_DSIPLL_100];
+        } else if (psb_intel_pipe_has_type(crtc, INTEL_OUTPUT_HDMI)) {
+                if ((ksel == KSEL_CRYSTAL_19) || (ksel == KSEL_BYPASS_19))
+                        limit = &mdfld_limits[MDFLD_LIMT_DPLL_19];
+                else if (ksel == KSEL_BYPASS_25) 
+                        limit = &mdfld_limits[MDFLD_LIMT_DPLL_25];
+                else if ((ksel == KSEL_BYPASS_83_100) && (dev_priv->core_freq == 166))
+                        limit = &mdfld_limits[MDFLD_LIMT_DPLL_83];
+                else if ((ksel == KSEL_BYPASS_83_100) &&
+                         (dev_priv->core_freq == 100 || dev_priv->core_freq == 200))
+                        limit = &mdfld_limits[MDFLD_LIMT_DPLL_100];
+        } else {
+                limit = NULL;
+                dev_err(dev->dev, "mdfld_limit Wrong display type.\n");
+        }
+
+        return limit;
+}
+
+/** Derive the pixel clock for the given refclk and divisors for 8xx chips. */
+static void mdfld_clock(int refclk, struct mdfld_intel_clock_t *clock)
+{
+        clock->dot = (refclk * clock->m) / clock->p1;
+}
+
+/**
+ * Returns a set of divisors for the desired target clock with the given refclk,
+ * or FALSE.  Divisor values are the actual divisors for
+ */
+static bool
+mdfldFindBestPLL(struct drm_crtc *crtc, int target, int refclk,
+                struct mdfld_intel_clock_t *best_clock)
+{
+        struct mdfld_intel_clock_t clock;
+        const struct mdfld_limit_t *limit = mdfld_limit(crtc);
+        int err = target;
+
+        memset(best_clock, 0, sizeof(*best_clock));
+
+        for (clock.m = limit->m.min; clock.m <= limit->m.max; clock.m++) {
+                for (clock.p1 = limit->p1.min; clock.p1 <= limit->p1.max;
+                     clock.p1++) {
+                        int this_err;
+
+                        mdfld_clock(refclk, &clock);
+
+                        this_err = abs(clock.dot - target);
+                        if (this_err < err) {
+                                *best_clock = clock;
+                                err = this_err;
+                        }
+                }
+        }
+        return err != target;
+}
+
+/**
+ * Return the pipe currently connected to the panel fitter,
+ * or -1 if the panel fitter is not present or not in use
+ */
+static int mdfld_panel_fitter_pipe(struct drm_device *dev)
+{
+        u32 pfit_control;
+
+        pfit_control = REG_READ(PFIT_CONTROL);
+
+        /* See if the panel fitter is in use */
+        if ((pfit_control & PFIT_ENABLE) == 0)
+                return -1;
+        return (pfit_control >> 29) & 3;
+}
+
+static int mdfld_crtc_mode_set(struct drm_crtc *crtc,
+                              struct drm_display_mode *mode,
+                              struct drm_display_mode *adjusted_mode,
+                              int x, int y,
+                              struct drm_framebuffer *old_fb)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int pipe = psb_intel_crtc->pipe;
+        int fp_reg = MRST_FPA0;
+        int dpll_reg = MRST_DPLL_A;
+        int dspcntr_reg = DSPACNTR;
+        int pipeconf_reg = PIPEACONF;
+        int htot_reg = HTOTAL_A;
+        int hblank_reg = HBLANK_A;
+        int hsync_reg = HSYNC_A;
+        int vtot_reg = VTOTAL_A;
+        int vblank_reg = VBLANK_A;
+        int vsync_reg = VSYNC_A;
+        int dspsize_reg = DSPASIZE; 
+        int dsppos_reg = DSPAPOS; 
+        int pipesrc_reg = PIPEASRC;
+        u32 *pipeconf = &dev_priv->pipeconf;
+        u32 *dspcntr = &dev_priv->dspcntr;
+        int refclk = 0;
+        int clk_n = 0, clk_p2 = 0, clk_byte = 1, clk = 0, m_conv = 0, clk_tmp = 0;
+        struct mdfld_intel_clock_t clock;
+        bool ok;
+        u32 dpll = 0, fp = 0;
+        bool is_crt = false, is_lvds = false, is_tv = false;
+        bool is_mipi = false, is_mipi2 = false, is_hdmi = false;
+        struct drm_mode_config *mode_config = &dev->mode_config;
+        struct psb_intel_output *psb_intel_output = NULL;
+        uint64_t scalingType = DRM_MODE_SCALE_FULLSCREEN;
+        struct drm_encoder *encoder;
+        struct drm_connector *connector;
+        int timeout = 0;
+
+        dev_dbg(dev->dev, "pipe = 0x%x \n", pipe);
+
+        switch (pipe) {
+        case 0:
+                break;
+        case 1:
+                fp_reg = FPB0;
+                dpll_reg = DPLL_B;
+                dspcntr_reg = DSPBCNTR;
+                pipeconf_reg = PIPEBCONF;
+                htot_reg = HTOTAL_B;
+                hblank_reg = HBLANK_B;
+                hsync_reg = HSYNC_B;
+                vtot_reg = VTOTAL_B;
+                vblank_reg = VBLANK_B;
+                vsync_reg = VSYNC_B;
+                dspsize_reg = DSPBSIZE; 
+                dsppos_reg = DSPBPOS; 
+                pipesrc_reg = PIPEBSRC;
+                pipeconf = &dev_priv->pipeconf1;
+                dspcntr = &dev_priv->dspcntr1;
+                fp_reg = MDFLD_DPLL_DIV0;
+                dpll_reg = MDFLD_DPLL_B;
+                break;
+        case 2:
+                dpll_reg = MRST_DPLL_A;
+                dspcntr_reg = DSPCCNTR;
+                pipeconf_reg = PIPECCONF;
+                htot_reg = HTOTAL_C;
+                hblank_reg = HBLANK_C;
+                hsync_reg = HSYNC_C;
+                vtot_reg = VTOTAL_C;
+                vblank_reg = VBLANK_C;
+                vsync_reg = VSYNC_C;
+                dspsize_reg = DSPCSIZE; 
+                dsppos_reg = DSPCPOS; 
+                pipesrc_reg = PIPECSRC;
+                pipeconf = &dev_priv->pipeconf2;
+                dspcntr = &dev_priv->dspcntr2;
+                break;
+        default:
+                DRM_ERROR("Illegal Pipe Number. \n");
+                return 0;
+        }
+
+        dev_dbg(dev->dev, "adjusted_hdisplay = %d\n",
+                 adjusted_mode->hdisplay);
+        dev_dbg(dev->dev, "adjusted_vdisplay = %d\n",
+                 adjusted_mode->vdisplay);
+        dev_dbg(dev->dev, "adjusted_hsync_start = %d\n",
+                 adjusted_mode->hsync_start);
+        dev_dbg(dev->dev, "adjusted_hsync_end = %d\n",
+                 adjusted_mode->hsync_end);
+        dev_dbg(dev->dev, "adjusted_htotal = %d\n",
+                 adjusted_mode->htotal);
+        dev_dbg(dev->dev, "adjusted_vsync_start = %d\n",
+                 adjusted_mode->vsync_start);
+        dev_dbg(dev->dev, "adjusted_vsync_end = %d\n",
+                 adjusted_mode->vsync_end);
+        dev_dbg(dev->dev, "adjusted_vtotal = %d\n",
+                 adjusted_mode->vtotal);
+        dev_dbg(dev->dev, "adjusted_clock = %d\n",
+                 adjusted_mode->clock);
+        dev_dbg(dev->dev, "hdisplay = %d\n",
+                 mode->hdisplay);
+        dev_dbg(dev->dev, "vdisplay = %d\n",
+                 mode->vdisplay);
+
+        if (!gma_power_begin(dev, true))
+                return 0;
+
+        memcpy(&psb_intel_crtc->saved_mode, mode, sizeof(struct drm_display_mode));
+        memcpy(&psb_intel_crtc->saved_adjusted_mode, adjusted_mode, sizeof(struct drm_display_mode));
+
+        list_for_each_entry(connector, &mode_config->connector_list, head) {
+                        
+                encoder = connector->encoder;
+                
+                if(!encoder)
+                        continue;
+
+                if (encoder->crtc != crtc)
+                        continue;
+
+                psb_intel_output = to_psb_intel_output(connector);
+                
+                dev_dbg(dev->dev, "output->type = 0x%x \n", psb_intel_output->type);
+
+                switch (psb_intel_output->type) {
+                case INTEL_OUTPUT_LVDS:
+                        is_lvds = true;
+                        break;
+                case INTEL_OUTPUT_TVOUT:
+                        is_tv = true;
+                        break;
+                case INTEL_OUTPUT_ANALOG:
+                        is_crt = true;
+                        break;
+                case INTEL_OUTPUT_MIPI:
+                        is_mipi = true;
+                        break;
+                case INTEL_OUTPUT_MIPI2:
+                        is_mipi2 = true;
+                        break;
+                case INTEL_OUTPUT_HDMI:
+                        is_hdmi = true;
+                        break;
+                }
+        }
+
+        /* Disable the VGA plane that we never use */
+        REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+
+        /* Disable the panel fitter if it was on our pipe */
+        if (mdfld_panel_fitter_pipe(dev) == pipe)
+                REG_WRITE(PFIT_CONTROL, 0);
+
+        /* pipesrc and dspsize control the size that is scaled from,
+         * which should always be the user's requested size.
+         */
+        if (pipe == 1) {
+                /* FIXME: To make HDMI display with 864x480 (TPO), 480x864 (PYR) or 480x854 (TMD), set the sprite
+                 * width/height and souce image size registers with the adjusted mode for pipe B. */
+
+                /* The defined sprite rectangle must always be completely contained within the displayable
+                 * area of the screen image (frame buffer). */
+                REG_WRITE(dspsize_reg, ((MIN(mode->crtc_vdisplay, adjusted_mode->crtc_vdisplay) - 1) << 16)
+                                | (MIN(mode->crtc_hdisplay, adjusted_mode->crtc_hdisplay) - 1));
+                /* Set the CRTC with encoder mode. */
+                REG_WRITE(pipesrc_reg, ((mode->crtc_hdisplay - 1) << 16)
+                                 | (mode->crtc_vdisplay - 1));
+        } else {
+                REG_WRITE(dspsize_reg, ((mode->crtc_vdisplay - 1) << 16) | (mode->crtc_hdisplay - 1));
+                REG_WRITE(pipesrc_reg, ((mode->crtc_hdisplay - 1) << 16) | (mode->crtc_vdisplay - 1));
+        }
+
+        REG_WRITE(dsppos_reg, 0);
+
+        if (psb_intel_output)
+                drm_connector_property_get_value(&psb_intel_output->base,
+                        dev->mode_config.scaling_mode_property, &scalingType);
+
+        if (scalingType == DRM_MODE_SCALE_NO_SCALE) {
+                /*Moorestown doesn't have register support for centering so we need to
+                  mess with the h/vblank and h/vsync start and ends to get centering*/
+                int offsetX = 0, offsetY = 0;
+
+                offsetX = (adjusted_mode->crtc_hdisplay - mode->crtc_hdisplay) / 2;
+                offsetY = (adjusted_mode->crtc_vdisplay - mode->crtc_vdisplay) / 2;
+
+                REG_WRITE(htot_reg, (mode->crtc_hdisplay - 1) |
+                        ((adjusted_mode->crtc_htotal - 1) << 16));
+                REG_WRITE(vtot_reg, (mode->crtc_vdisplay - 1) |
+                        ((adjusted_mode->crtc_vtotal - 1) << 16));
+                REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - offsetX - 1) |
+                        ((adjusted_mode->crtc_hblank_end - offsetX - 1) << 16));
+                REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - offsetX - 1) |
+                        ((adjusted_mode->crtc_hsync_end - offsetX - 1) << 16));
+                REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - offsetY - 1) |
+                        ((adjusted_mode->crtc_vblank_end - offsetY - 1) << 16));
+                REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - offsetY - 1) |
+                        ((adjusted_mode->crtc_vsync_end - offsetY - 1) << 16));
+        } else {
+                REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+                        ((adjusted_mode->crtc_htotal - 1) << 16));
+                REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+                        ((adjusted_mode->crtc_vtotal - 1) << 16));
+                REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+                        ((adjusted_mode->crtc_hblank_end - 1) << 16));
+                REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+                        ((adjusted_mode->crtc_hsync_end - 1) << 16));
+                REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+                        ((adjusted_mode->crtc_vblank_end - 1) << 16));
+                REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+                        ((adjusted_mode->crtc_vsync_end - 1) << 16));
+        }
+
+        /* Flush the plane changes */
+        {
+                struct drm_crtc_helper_funcs *crtc_funcs =
+                    crtc->helper_private;
+                crtc_funcs->mode_set_base(crtc, x, y, old_fb);
+        }
+
+        /* setup pipeconf */
+        *pipeconf = PIPEACONF_ENABLE; /* FIXME_JLIU7 REG_READ(pipeconf_reg); */
+
+        /* Set up the display plane register */
+        *dspcntr = REG_READ(dspcntr_reg);
+        *dspcntr |= pipe << DISPPLANE_SEL_PIPE_POS;
+        *dspcntr |= DISPLAY_PLANE_ENABLE;
+/* MDFLD_PO_JLIU7       dspcntr |= DISPPLANE_BOTTOM; */
+/* MDFLD_PO_JLIU7       dspcntr |= DISPPLANE_GAMMA_ENABLE; */
+
+        if (is_mipi2)
+        {
+                goto mrst_crtc_mode_set_exit;
+        }
+/* FIXME JLIU7 Add MDFLD HDMI supports */
+/* FIXME_MDFLD JLIU7 DSIPLL clock *= 8? */
+/* FIXME_MDFLD JLIU7 need to revist for dual MIPI supports */
+        clk = adjusted_mode->clock;
+
+        if (is_hdmi) {
+                if ((ksel == KSEL_CRYSTAL_19) || (ksel == KSEL_BYPASS_19))
+                {
+                        refclk = 19200;
+
+                        if (is_mipi || is_mipi2)
+                        {
+                                clk_n = 1, clk_p2 = 8;
+                        } else if (is_hdmi) {
+                                clk_n = 1, clk_p2 = 10;
+                        }
+                } else if (ksel == KSEL_BYPASS_25) { 
+                        refclk = 25000;
+
+                        if (is_mipi || is_mipi2)
+                        {
+                                clk_n = 1, clk_p2 = 8;
+                        } else if (is_hdmi) {
+                                clk_n = 1, clk_p2 = 10;
+                        }
+                } else if ((ksel == KSEL_BYPASS_83_100) && (dev_priv->core_freq == 166)) {
+                        refclk = 83000;
+
+                        if (is_mipi || is_mipi2)
+                        {
+                                clk_n = 4, clk_p2 = 8;
+                        } else if (is_hdmi) {
+                                clk_n = 4, clk_p2 = 10;
+                        }
+                } else if ((ksel == KSEL_BYPASS_83_100) &&
+                           (dev_priv->core_freq == 100 || dev_priv->core_freq == 200)) {
+                        refclk = 100000;
+                        if (is_mipi || is_mipi2)
+                        {
+                                clk_n = 4, clk_p2 = 8;
+                        } else if (is_hdmi) {
+                                clk_n = 4, clk_p2 = 10;
+                        }
+                }
+
+                if (is_mipi)
+                        clk_byte = dev_priv->bpp / 8;
+                else if (is_mipi2)
+                        clk_byte = dev_priv->bpp2 / 8;
+        
+                clk_tmp = clk * clk_n * clk_p2 * clk_byte;
+
+                dev_dbg(dev->dev, "clk = %d, clk_n = %d, clk_p2 = %d. \n", clk, clk_n, clk_p2);
+                dev_dbg(dev->dev, "adjusted_mode->clock = %d, clk_tmp = %d. \n", adjusted_mode->clock, clk_tmp);
+
+                ok = mdfldFindBestPLL(crtc, clk_tmp, refclk, &clock);
+
+                if (!ok) {
+                        dev_err(dev->dev, 
+                           "mdfldFindBestPLL fail in mdfld_crtc_mode_set. \n");
+                } else {
+                        m_conv = mdfld_m_converts[(clock.m - MDFLD_M_MIN)];
+
+                        dev_dbg(dev->dev, "dot clock = %d,"
+                                 "m = %d, p1 = %d, m_conv = %d. \n", clock.dot, clock.m,
+                                 clock.p1, m_conv);
+                }
+
+                dpll = REG_READ(dpll_reg);
+
+                if (dpll & DPLL_VCO_ENABLE) {
+                        dpll &= ~DPLL_VCO_ENABLE;
+                        REG_WRITE(dpll_reg, dpll);
+                        REG_READ(dpll_reg);
+
+                        /* FIXME jliu7 check the DPLL lock bit PIPEACONF[29] */
+                        /* FIXME_MDFLD PO - change 500 to 1 after PO */
+                        udelay(500);
+
+                        /* reset M1, N1 & P1 */
+                        REG_WRITE(fp_reg, 0);
+                        dpll &= ~MDFLD_P1_MASK;
+                        REG_WRITE(dpll_reg, dpll);
+                        /* FIXME_MDFLD PO - change 500 to 1 after PO */
+                        udelay(500);
+                }
+
+                /* When ungating power of DPLL, needs to wait 0.5us before enable the VCO */
+                if (dpll & MDFLD_PWR_GATE_EN) {
+                        dpll &= ~MDFLD_PWR_GATE_EN;
+                        REG_WRITE(dpll_reg, dpll);
+                        /* FIXME_MDFLD PO - change 500 to 1 after PO */
+                        udelay(500);
+                }       
+
+                dpll = 0; 
+
+#if 0 /* FIXME revisit later */
+                if ((ksel == KSEL_CRYSTAL_19) || (ksel == KSEL_BYPASS_19) || (ksel == KSEL_BYPASS_25)) {
+                        dpll &= ~MDFLD_INPUT_REF_SEL;   
+                } else if (ksel == KSEL_BYPASS_83_100) { 
+                        dpll |= MDFLD_INPUT_REF_SEL;    
+                }
+#endif /* FIXME revisit later */
+
+                if (is_hdmi)
+                        dpll |= MDFLD_VCO_SEL;  
+
+                fp = (clk_n / 2) << 16;
+                fp |= m_conv; 
+
+                /* compute bitmask from p1 value */
+                dpll |= (1 << (clock.p1 - 2)) << 17;
+
+#if 0 /* 1080p30 & 720p */
+                dpll = 0x00050000;
+                fp = 0x000001be;
+#endif 
+#if 0 /* 480p */
+                dpll = 0x02010000;
+                fp = 0x000000d2;
+#endif 
+        } else {
+#if 0 /*DBI_TPO_480x864*/
+                dpll = 0x00020000;
+                fp = 0x00000156; 
+#endif /* DBI_TPO_480x864 */ /* get from spec. */
+
+                dpll = 0x00800000;
+                fp = 0x000000c1;
+}
+
+        REG_WRITE(fp_reg, fp);
+        REG_WRITE(dpll_reg, dpll);
+        /* FIXME_MDFLD PO - change 500 to 1 after PO */
+        udelay(500);
+
+        dpll |= DPLL_VCO_ENABLE;
+        REG_WRITE(dpll_reg, dpll);
+        REG_READ(dpll_reg);
+
+        /* wait for DSI PLL to lock */
+        while ((timeout < 20000) && !(REG_READ(pipeconf_reg) & PIPECONF_DSIPLL_LOCK)) {
+                udelay(150);
+                timeout ++;
+        }
+
+        if (is_mipi)
+                goto mrst_crtc_mode_set_exit;
+
+        dev_dbg(dev->dev, "is_mipi = 0x%x \n", is_mipi);
+
+        REG_WRITE(pipeconf_reg, *pipeconf);
+        REG_READ(pipeconf_reg);
+
+        /* Wait for for the pipe enable to take effect. */
+//FIXME_JLIU7 HDMI      mrstWaitForPipeEnable(dev);
+
+        REG_WRITE(dspcntr_reg, *dspcntr);
+        psb_intel_wait_for_vblank(dev);
+
+mrst_crtc_mode_set_exit:
+
+        gma_power_end(dev);
+
+        return 0;
+}
+
+static void mdfld_crtc_prepare(struct drm_crtc *crtc)
+{
+        struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+        crtc_funcs->dpms(crtc, DRM_MODE_DPMS_OFF);
+}
+
+static void mdfld_crtc_commit(struct drm_crtc *crtc)
+{
+        struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+        crtc_funcs->dpms(crtc, DRM_MODE_DPMS_ON);
+}
+
+static bool mdfld_crtc_mode_fixup(struct drm_crtc *crtc,
+                                  struct drm_display_mode *mode,
+                                  struct drm_display_mode *adjusted_mode)
+{
+        return true;
+}
+
+const struct drm_crtc_helper_funcs mdfld_helper_funcs = {
+        .dpms = mdfld_crtc_dpms,
+        .mode_fixup = mdfld_crtc_mode_fixup,
+        .mode_set = mdfld_crtc_mode_set,
+        .mode_set_base = mdfld__intel_pipe_set_base,
+        .prepare = mdfld_crtc_prepare,
+        .commit = mdfld_crtc_commit,
+};
diff --git a/drivers/staging/gma500/mdfld_msic.h b/drivers/staging/gma500/mdfld_msic.h
new file mode 100644
index 00000000000..a7ad6547249
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_msic.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright © 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:
+ *      Jim Liu <jim.liu@intel.com>
+ */
+
+#define MSIC_PCI_DEVICE_ID      0x831
+
+int msic_regsiter_driver(void);
+int msic_unregister_driver(void);
+extern void hpd_notify_um(void);
diff --git a/drivers/staging/gma500/mdfld_output.c b/drivers/staging/gma500/mdfld_output.c
new file mode 100644
index 00000000000..ee55f87ba1f
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_output.c
@@ -0,0 +1,170 @@
+/*
+ * Copyright (c)  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, sublicensen
+ * 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:
+ * Thomas Eaton <thomas.g.eaton@intel.com>
+ * Scott Rowe <scott.m.rowe@intel.com>
+*/
+
+#include <linux/init.h>
+#include "mdfld_dsi_dbi.h"
+#include "mdfld_dsi_dpi.h"
+#include "mdfld_dsi_output.h"
+#include "mdfld_output.h"
+#include "mdfld_dsi_dbi_dpu.h"
+
+#include "displays/tpo_cmd.h"
+#include "displays/tpo_vid.h"
+#include "displays/tmd_cmd.h"
+#include "displays/tmd_vid.h"
+#include "displays/pyr_cmd.h"
+#include "displays/pyr_vid.h"
+/* #include "displays/hdmi.h" */
+
+static int mdfld_dual_mipi;
+static int mdfld_hdmi;
+static int mdfld_dpu;
+
+module_param(mdfld_dual_mipi, int, 0600);
+MODULE_PARM_DESC(mdfld_dual_mipi, "Enable dual MIPI configuration");
+module_param(mdfld_hdmi, int, 0600);
+MODULE_PARM_DESC(mdfld_hdmi, "Enable Medfield HDMI");
+module_param(mdfld_dpu, int, 0600);
+MODULE_PARM_DESC(mdfld_dpu, "Enable Medfield DPU");
+
+/* For now a single type per device is all we cope with */
+int mdfld_get_panel_type(struct drm_device *dev, int pipe)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        return dev_priv->panel_id;
+}
+
+int mdfld_panel_dpi(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        switch (dev_priv->panel_id) {
+        case TMD_VID:
+        case TPO_VID:
+        case PYR_VID:
+                return true;
+        case TMD_CMD:
+        case TPO_CMD:
+        case PYR_CMD:
+        default:
+                return false;
+        }
+}
+
+static int init_panel(struct drm_device *dev, int mipi_pipe, int p_type)
+{
+        struct panel_funcs *p_cmd_funcs;
+        struct panel_funcs *p_vid_funcs;
+
+        /* Oh boy ... FIXME */
+        p_cmd_funcs = kzalloc(sizeof(struct panel_funcs), GFP_KERNEL);
+        if (p_cmd_funcs == NULL)
+                return -ENODEV;
+        p_vid_funcs = kzalloc(sizeof(struct panel_funcs), GFP_KERNEL);
+        if (p_vid_funcs == NULL) {
+                kfree(p_cmd_funcs);
+                return -ENODEV;
+        }
+
+        switch (p_type) {
+        case TPO_CMD:
+                tpo_cmd_init(dev, p_cmd_funcs);
+                mdfld_dsi_output_init(dev, mipi_pipe, NULL, p_cmd_funcs, NULL);
+                break;
+        case TPO_VID:
+                tpo_vid_init(dev, p_vid_funcs);
+                mdfld_dsi_output_init(dev, mipi_pipe, NULL, NULL, p_vid_funcs);
+                break;
+        case TMD_CMD:
+                /*tmd_cmd_init(dev, p_cmd_funcs); */
+                mdfld_dsi_output_init(dev, mipi_pipe, NULL, p_cmd_funcs, NULL);
+                break;
+        case TMD_VID:
+                tmd_vid_init(dev, p_vid_funcs);
+                mdfld_dsi_output_init(dev, mipi_pipe, NULL, NULL, p_vid_funcs);
+                break;
+        case PYR_CMD:
+                pyr_cmd_init(dev, p_cmd_funcs);
+                mdfld_dsi_output_init(dev, mipi_pipe, NULL, p_cmd_funcs, NULL);
+                break;
+        case PYR_VID:
+                mdfld_dsi_output_init(dev, mipi_pipe, NULL, NULL, p_vid_funcs);
+                break;
+        case TPO:       /* TPO panel supports both cmd & vid interfaces */
+                tpo_cmd_init(dev, p_cmd_funcs);
+                tpo_vid_init(dev, p_vid_funcs);
+                mdfld_dsi_output_init(dev, mipi_pipe, NULL, p_cmd_funcs,
+                                      p_vid_funcs);
+                break;
+        case TMD:
+                break;
+        case PYR:
+                break;
+#if 0
+        case HDMI:
+                dev_dbg(dev->dev, "Initializing HDMI");
+                mdfld_hdmi_init(dev, &dev_priv->mode_dev);
+                break;
+#endif
+        default:
+                dev_err(dev->dev, "Unsupported interface %d", p_type);
+                return -ENODEV;
+        }
+        return 0;
+}
+
+int mdfld_output_init(struct drm_device *dev)
+{
+        int type;
+
+        /* MIPI panel 1 */
+        type = mdfld_get_panel_type(dev, 0);
+        dev_info(dev->dev, "panel 1: type is %d\n", type);
+        init_panel(dev, 0, type);
+
+        if (mdfld_dual_mipi) {
+                /* MIPI panel 2 */
+                type = mdfld_get_panel_type(dev, 2);
+                dev_info(dev->dev, "panel 2: type is %d\n", type);
+                init_panel(dev, 2, type);
+        }
+        if (mdfld_hdmi)
+                /* HDMI panel */
+                init_panel(dev, 0, HDMI);
+        return 0;
+}
+
+void mdfld_output_setup(struct drm_device *dev)
+{
+        /* FIXME: this is not the right place for this stuff ! */
+        if (IS_MFLD(dev)) {
+                if (mdfld_dpu)
+                        mdfld_dbi_dpu_init(dev);
+                else
+                        mdfld_dbi_dsr_init(dev);
+        }
+}
\ No newline at end of file
diff --git a/drivers/staging/gma500/mdfld_output.h b/drivers/staging/gma500/mdfld_output.h
new file mode 100644
index 00000000000..daf33e7df9d
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_output.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c)  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, sublicensen
+ * 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:
+ * Thomas Eaton <thomas.g.eaton@intel.com>
+ * Scott Rowe <scott.m.rowe@intel.com>
+*/
+
+#ifndef MDFLD_OUTPUT_H
+#define MDFLD_OUTPUT_H
+
+int mdfld_output_init(struct drm_device *dev);
+int mdfld_panel_dpi(struct drm_device *dev);
+int mdfld_get_panel_type(struct drm_device *dev, int pipe);
+void mdfld_disable_crtc (struct drm_device *dev, int pipe);
+
+extern const struct drm_crtc_helper_funcs mdfld_helper_funcs;
+extern const struct drm_crtc_funcs mdfld_intel_crtc_funcs;
+
+extern void mdfld_output_setup(struct drm_device *dev);
+
+#endif
diff --git a/drivers/staging/gma500/mdfld_pyr_cmd.c b/drivers/staging/gma500/mdfld_pyr_cmd.c
new file mode 100644
index 00000000000..523f2d8fe4f
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_pyr_cmd.c
@@ -0,0 +1,558 @@
+/*
+ * Copyright (c)  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, sublicensen
+ * 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:
+ * Thomas Eaton <thomas.g.eaton@intel.com>
+ * Scott Rowe <scott.m.rowe@intel.com>
+*/
+
+#include "mdfld_dsi_dbi.h"
+#include "mdfld_dsi_dpi.h"
+#include "mdfld_dsi_output.h"
+#include "mdfld_output.h"
+#include "mdfld_dsi_dbi_dpu.h"
+#include "mdfld_dsi_pkg_sender.h"
+
+#include "displays/pyr_cmd.h"
+
+static struct drm_display_mode *pyr_cmd_get_config_mode(struct drm_device *dev)
+{
+        struct drm_display_mode *mode;
+
+        mode = kzalloc(sizeof(*mode), GFP_KERNEL);
+        if (!mode) {
+                dev_err(dev->dev, "Out of memory\n");
+                return NULL;
+        }
+
+        dev_dbg(dev->dev, "hdisplay is %d\n", mode->hdisplay);
+        dev_dbg(dev->dev, "vdisplay is %d\n", mode->vdisplay);
+        dev_dbg(dev->dev, "HSS is %d\n", mode->hsync_start);
+        dev_dbg(dev->dev, "HSE is %d\n", mode->hsync_end);
+        dev_dbg(dev->dev, "htotal is %d\n", mode->htotal);
+        dev_dbg(dev->dev, "VSS is %d\n", mode->vsync_start);
+        dev_dbg(dev->dev, "VSE is %d\n", mode->vsync_end);
+        dev_dbg(dev->dev, "vtotal is %d\n", mode->vtotal);
+        dev_dbg(dev->dev, "clock is %d\n", mode->clock);
+
+        mode->hdisplay = 480;
+        mode->vdisplay = 864;
+        mode->hsync_start = 487;
+        mode->hsync_end = 490;
+        mode->htotal = 499;
+        mode->vsync_start = 874;
+        mode->vsync_end = 878;
+        mode->vtotal = 886;
+        mode->clock = 25777;
+
+        drm_mode_set_name(mode);
+        drm_mode_set_crtcinfo(mode, 0);
+
+        mode->type |= DRM_MODE_TYPE_PREFERRED;
+
+        return mode;
+}
+
+static bool pyr_dsi_dbi_mode_fixup(struct drm_encoder *encoder,
+                                struct drm_display_mode *mode,
+                                struct drm_display_mode *adjusted_mode)
+{
+        struct drm_device *dev = encoder->dev;
+        struct drm_display_mode *fixed_mode = pyr_cmd_get_config_mode(dev);
+
+        if (fixed_mode) {
+                adjusted_mode->hdisplay = fixed_mode->hdisplay;
+                adjusted_mode->hsync_start = fixed_mode->hsync_start;
+                adjusted_mode->hsync_end = fixed_mode->hsync_end;
+                adjusted_mode->htotal = fixed_mode->htotal;
+                adjusted_mode->vdisplay = fixed_mode->vdisplay;
+                adjusted_mode->vsync_start = fixed_mode->vsync_start;
+                adjusted_mode->vsync_end = fixed_mode->vsync_end;
+                adjusted_mode->vtotal = fixed_mode->vtotal;
+                adjusted_mode->clock = fixed_mode->clock;
+                drm_mode_set_crtcinfo(adjusted_mode, CRTC_INTERLACE_HALVE_V);
+                kfree(fixed_mode);
+        }
+        return true;
+}
+
+static void pyr_dsi_dbi_set_power(struct drm_encoder *encoder, bool on)
+{
+        int ret = 0;
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_dbi_output *dbi_output =
+                                MDFLD_DSI_DBI_OUTPUT(dsi_encoder);
+        struct drm_device *dev = encoder->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 reg_offset = 0;
+        int pipe = (dbi_output->channel_num == 0) ? 0 : 2;
+
+        dev_dbg(dev->dev, "pipe %d : %s, panel on: %s\n", pipe,
+                        on ? "On" : "Off",
+                        dbi_output->dbi_panel_on ? "True" : "False");
+
+        if (pipe == 2) {
+                if (on)
+                        dev_priv->dual_mipi = true;
+                else
+                        dev_priv->dual_mipi = false;
+
+                reg_offset = MIPIC_REG_OFFSET;
+        } else {
+                if (!on)
+                        dev_priv->dual_mipi = false;
+        }
+
+        if (!gma_power_begin(dev, true)) {
+                dev_err(dev->dev, "hw begin failed\n");
+                return;
+        }
+
+
+        if (on) {
+                if (dbi_output->dbi_panel_on)
+                        goto out_err;
+
+                ret = mdfld_dsi_dbi_update_power(dbi_output, DRM_MODE_DPMS_ON);
+                if (ret) {
+                        dev_err(dev->dev, "power on error\n");
+                        goto out_err;
+                }
+
+                dbi_output->dbi_panel_on = true;
+
+                if (pipe == 2) {
+                        dev_priv->dbi_panel_on2 = true;
+                } else {
+                        dev_priv->dbi_panel_on = true;
+                        mdfld_enable_te(dev, 0);
+                }
+        } else {
+                if (!dbi_output->dbi_panel_on && !dbi_output->first_boot)
+                        goto out_err;
+
+                dbi_output->dbi_panel_on = false;
+                dbi_output->first_boot = false;
+
+                if (pipe == 2) {
+                        dev_priv->dbi_panel_on2 = false;
+                        mdfld_disable_te(dev, 2);
+                } else {
+                        dev_priv->dbi_panel_on = false;
+                        mdfld_disable_te(dev, 0);
+
+                        if (dev_priv->dbi_panel_on2)
+                                mdfld_enable_te(dev, 2);
+                }
+
+                ret = mdfld_dsi_dbi_update_power(dbi_output, DRM_MODE_DPMS_OFF);
+                if (ret) {
+                        dev_err(dev->dev, "power on error\n");
+                        goto out_err;
+                }
+        }
+
+out_err:
+        gma_power_end(dev);
+
+        if (ret)
+                dev_err(dev->dev, "failed\n");
+}
+
+static void pyr_dsi_controller_dbi_init(struct mdfld_dsi_config *dsi_config,
+                                                                int pipe)
+{
+        struct drm_device *dev = dsi_config->dev;
+        u32 reg_offset = pipe ? MIPIC_REG_OFFSET : 0;
+        int lane_count = dsi_config->lane_count;
+        u32 val = 0;
+
+        dev_dbg(dev->dev, "Init DBI interface on pipe %d...\n", pipe);
+
+        /* Un-ready device */
+        REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset), 0x00000000);
+
+        /* Init dsi adapter before kicking off */
+        REG_WRITE((MIPIA_CONTROL_REG + reg_offset), 0x00000018);
+
+        /* TODO: figure out how to setup these registers */
+        REG_WRITE((MIPIA_DPHY_PARAM_REG + reg_offset), 0x150c600F);
+        REG_WRITE((MIPIA_CLK_LANE_SWITCH_TIME_CNT_REG + reg_offset),
+                                                                0x000a0014);
+        REG_WRITE((MIPIA_DBI_BW_CTRL_REG + reg_offset), 0x00000400);
+        REG_WRITE((MIPIA_HS_LS_DBI_ENABLE_REG + reg_offset), 0x00000000);
+
+        /* Enable all interrupts */
+        REG_WRITE((MIPIA_INTR_EN_REG + reg_offset), 0xffffffff);
+        /* Max value: 20 clock cycles of txclkesc */
+        REG_WRITE((MIPIA_TURN_AROUND_TIMEOUT_REG + reg_offset), 0x0000001f);
+        /* Min 21 txclkesc, max: ffffh */
+        REG_WRITE((MIPIA_DEVICE_RESET_TIMER_REG + reg_offset), 0x0000ffff);
+        /* Min: 7d0 max: 4e20 */
+        REG_WRITE((MIPIA_INIT_COUNT_REG + reg_offset), 0x00000fa0);
+
+        /* Set up func_prg */
+        val |= lane_count;
+        val |= (dsi_config->channel_num << DSI_DBI_VIRT_CHANNEL_OFFSET);
+        val |= DSI_DBI_COLOR_FORMAT_OPTION2;
+        REG_WRITE((MIPIA_DSI_FUNC_PRG_REG + reg_offset), val);
+
+        REG_WRITE((MIPIA_HS_TX_TIMEOUT_REG + reg_offset), 0x3fffff);
+        REG_WRITE((MIPIA_LP_RX_TIMEOUT_REG + reg_offset), 0xffff);
+
+        /* De-assert dbi_stall when half of DBI FIFO is empty */
+        /* REG_WRITE((MIPIA_DBI_FIFO_THROTTLE_REG + reg_offset), 0x00000000); */
+
+        REG_WRITE((MIPIA_HIGH_LOW_SWITCH_COUNT_REG + reg_offset), 0x46);
+        REG_WRITE((MIPIA_EOT_DISABLE_REG + reg_offset), 0x00000002);
+        REG_WRITE((MIPIA_LP_BYTECLK_REG + reg_offset), 0x00000004);
+        REG_WRITE((MIPIA_DEVICE_READY_REG + reg_offset), 0x00000001);
+}
+
+static void pyr_dsi_dbi_mode_set(struct drm_encoder *encoder,
+                                struct drm_display_mode *mode,
+                                struct drm_display_mode *adjusted_mode)
+{
+        int ret = 0;
+        struct drm_device *dev = encoder->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_dbi_output *dsi_output =
+                                        MDFLD_DSI_DBI_OUTPUT(dsi_encoder);
+        struct mdfld_dsi_config *dsi_config =
+                                mdfld_dsi_encoder_get_config(dsi_encoder);
+        struct mdfld_dsi_connector *dsi_connector = dsi_config->connector;
+        int pipe = dsi_connector->pipe;
+        u8 param = 0;
+
+        /* Regs */
+        u32 mipi_reg = MIPI;
+        u32 dspcntr_reg = DSPACNTR;
+        u32 pipeconf_reg = PIPEACONF;
+        u32 reg_offset = 0;
+
+        /* Values */
+        u32 dspcntr_val = dev_priv->dspcntr;
+        u32 pipeconf_val = dev_priv->pipeconf;
+        u32 h_active_area = mode->hdisplay;
+        u32 v_active_area = mode->vdisplay;
+        u32 mipi_val = (PASS_FROM_SPHY_TO_AFE | SEL_FLOPPED_HSTX |
+                                                        TE_TRIGGER_GPIO_PIN);
+
+        dev_dbg(dev->dev, "mipi_val =0x%x\n", mipi_val);
+
+        dev_dbg(dev->dev, "type %s\n", (pipe == 2) ? "MIPI2" : "MIPI");
+        dev_dbg(dev->dev, "h %d v %d\n", mode->hdisplay, mode->vdisplay);
+
+        if (pipe == 2) {
+                mipi_reg = MIPI_C;
+                dspcntr_reg = DSPCCNTR;
+                pipeconf_reg = PIPECCONF;
+
+                reg_offset = MIPIC_REG_OFFSET;
+
+                dspcntr_val = dev_priv->dspcntr2;
+                pipeconf_val = dev_priv->pipeconf2;
+        } else {
+                mipi_val |= 0x2; /* Two lanes for port A and C respectively */
+        }
+
+        if (!gma_power_begin(dev, true)) {
+                dev_err(dev->dev, "hw begin failed\n");
+                return;
+        }
+
+        /* Set up pipe related registers */
+        REG_WRITE(mipi_reg, mipi_val);
+        REG_READ(mipi_reg);
+
+        pyr_dsi_controller_dbi_init(dsi_config, pipe);
+
+        msleep(20);
+
+        REG_WRITE(dspcntr_reg, dspcntr_val);
+        REG_READ(dspcntr_reg);
+
+        /* 20ms delay before sending exit_sleep_mode */
+        msleep(20);
+
+        /* Send exit_sleep_mode DCS */
+        ret = mdfld_dsi_dbi_send_dcs(dsi_output, exit_sleep_mode, NULL,
+                                                0, CMD_DATA_SRC_SYSTEM_MEM);
+        if (ret) {
+                dev_err(dev->dev, "sent exit_sleep_mode faild\n");
+                goto out_err;
+        }
+
+        /*send set_tear_on DCS*/
+        ret = mdfld_dsi_dbi_send_dcs(dsi_output, set_tear_on,
+                                        &param, 1, CMD_DATA_SRC_SYSTEM_MEM);
+        if (ret) {
+                dev_err(dev->dev, "%s - sent set_tear_on faild\n", __func__);
+                goto out_err;
+        }
+
+        /* Do some init stuff */
+        mdfld_dsi_brightness_init(dsi_config, pipe);
+        mdfld_dsi_gen_fifo_ready(dev, (MIPIA_GEN_FIFO_STAT_REG + reg_offset),
+                                HS_CTRL_FIFO_EMPTY | HS_DATA_FIFO_EMPTY);
+
+        REG_WRITE(pipeconf_reg, pipeconf_val | PIPEACONF_DSR);
+        REG_READ(pipeconf_reg);
+
+        /* TODO: this looks ugly, try to move it to CRTC mode setting */
+        if (pipe == 2)
+                dev_priv->pipeconf2 |= PIPEACONF_DSR;
+        else
+                dev_priv->pipeconf |= PIPEACONF_DSR;
+
+        dev_dbg(dev->dev, "pipeconf %x\n",  REG_READ(pipeconf_reg));
+
+        ret = mdfld_dsi_dbi_update_area(dsi_output, 0, 0,
+                                h_active_area - 1, v_active_area - 1);
+        if (ret) {
+                dev_err(dev->dev, "update area failed\n");
+                goto out_err;
+        }
+
+out_err:
+        gma_power_end(dev);
+
+        if (ret)
+                dev_err(dev->dev, "mode set failed\n");
+        else
+                dev_dbg(dev->dev, "mode set done successfully\n");
+}
+
+static void pyr_dsi_dbi_prepare(struct drm_encoder *encoder)
+{
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_dbi_output *dbi_output =
+                                        MDFLD_DSI_DBI_OUTPUT(dsi_encoder);
+
+        dbi_output->mode_flags |= MODE_SETTING_IN_ENCODER;
+        dbi_output->mode_flags &= ~MODE_SETTING_ENCODER_DONE;
+
+        pyr_dsi_dbi_set_power(encoder, false);
+}
+
+static void pyr_dsi_dbi_commit(struct drm_encoder *encoder)
+{
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_dbi_output *dbi_output =
+                                        MDFLD_DSI_DBI_OUTPUT(dsi_encoder);
+        struct drm_device *dev = dbi_output->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct psb_drm_dpu_rect rect;
+
+        pyr_dsi_dbi_set_power(encoder, true);
+
+        dbi_output->mode_flags &= ~MODE_SETTING_IN_ENCODER;
+
+        rect.x = rect.y = 0;
+        rect.width = 864;
+        rect.height = 480;
+
+        if (dbi_output->channel_num == 1) {
+                dev_priv->dsr_fb_update |= MDFLD_DSR_2D_3D_2;
+                /* If DPU enabled report a fullscreen damage */
+                mdfld_dbi_dpu_report_damage(dev, MDFLD_PLANEC, &rect);
+        } else {
+                dev_priv->dsr_fb_update |= MDFLD_DSR_2D_3D_0;
+                mdfld_dbi_dpu_report_damage(dev, MDFLD_PLANEA, &rect);
+        }
+        dbi_output->mode_flags |= MODE_SETTING_ENCODER_DONE;
+}
+
+static void pyr_dsi_dbi_dpms(struct drm_encoder *encoder, int mode)
+{
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_dbi_output *dbi_output =
+                                        MDFLD_DSI_DBI_OUTPUT(dsi_encoder);
+        struct drm_device *dev = dbi_output->dev;
+
+        dev_dbg(dev->dev, "%s\n",  (mode == DRM_MODE_DPMS_ON ? "on" : "off"));
+
+        if (mode == DRM_MODE_DPMS_ON)
+                pyr_dsi_dbi_set_power(encoder, true);
+        else
+                pyr_dsi_dbi_set_power(encoder, false);
+}
+
+/*
+ * Update the DBI MIPI Panel Frame Buffer.
+ */
+static void pyr_dsi_dbi_update_fb(struct mdfld_dsi_dbi_output *dbi_output,
+                                                                int pipe)
+{
+        struct mdfld_dsi_pkg_sender *sender =
+                mdfld_dsi_encoder_get_pkg_sender(&dbi_output->base);
+        struct drm_device *dev = dbi_output->dev;
+        struct drm_crtc *crtc = dbi_output->base.base.crtc;
+        struct psb_intel_crtc *psb_crtc = (crtc) ?
+                                to_psb_intel_crtc(crtc) : NULL;
+
+        u32 dpll_reg = MRST_DPLL_A;
+        u32 dspcntr_reg = DSPACNTR;
+        u32 pipeconf_reg = PIPEACONF;
+        u32 dsplinoff_reg = DSPALINOFF;
+        u32 dspsurf_reg = DSPASURF;
+        u32 hs_gen_ctrl_reg = HS_GEN_CTRL_REG;
+        u32 gen_fifo_stat_reg = GEN_FIFO_STAT_REG;
+        u32 reg_offset = 0;
+
+        u32 intr_status;
+        u32 fifo_stat_reg_val;
+        u32 dpll_reg_val;
+        u32 dspcntr_reg_val;
+        u32 pipeconf_reg_val;
+
+        /* If mode setting on-going, back off */
+        if ((dbi_output->mode_flags & MODE_SETTING_ON_GOING) ||
+                (psb_crtc && psb_crtc->mode_flags & MODE_SETTING_ON_GOING) ||
+                !(dbi_output->mode_flags & MODE_SETTING_ENCODER_DONE))
+                return;
+
+        /*
+         * Look for errors here.  In particular we're checking for whatever
+         * error status might have appeared during the last frame transmit
+         * (memory write).
+         *
+         * Normally, the bits we're testing here would be set infrequently,
+         * if at all.  However, one panel (at least) returns at least one
+         * error bit on most frames.  So we've disabled the kernel message
+         * for now.
+         *
+         * Still clear whatever error bits are set, except don't clear the
+         * ones that would make the Penwell DSI controller reset if we
+         * cleared them.
+         */
+        intr_status = REG_READ(INTR_STAT_REG);
+        if ((intr_status & 0x26FFFFFF) != 0) {
+                /* dev_err(dev->dev, "DSI status: 0x%08X\n", intr_status); */
+                intr_status &= 0x26F3FFFF;
+                REG_WRITE(INTR_STAT_REG, intr_status);
+        }
+
+        if (pipe == 2) {
+                dspcntr_reg = DSPCCNTR;
+                pipeconf_reg = PIPECCONF;
+                dsplinoff_reg = DSPCLINOFF;
+                dspsurf_reg = DSPCSURF;
+
+                hs_gen_ctrl_reg = HS_GEN_CTRL_REG + MIPIC_REG_OFFSET;
+                gen_fifo_stat_reg = GEN_FIFO_STAT_REG + MIPIC_REG_OFFSET,
+
+                reg_offset = MIPIC_REG_OFFSET;
+        }
+
+        if (!gma_power_begin(dev, true)) {
+                dev_err(dev->dev, "hw begin failed\n");
+                return;
+        }
+
+        fifo_stat_reg_val = REG_READ(MIPIA_GEN_FIFO_STAT_REG + reg_offset);
+        dpll_reg_val = REG_READ(dpll_reg);
+        dspcntr_reg_val = REG_READ(dspcntr_reg);
+        pipeconf_reg_val = REG_READ(pipeconf_reg);
+
+        if (!(fifo_stat_reg_val & (1 << 27)) ||
+                (dpll_reg_val & DPLL_VCO_ENABLE) ||
+                !(dspcntr_reg_val & DISPLAY_PLANE_ENABLE) ||
+                !(pipeconf_reg_val & DISPLAY_PLANE_ENABLE)) {
+                goto update_fb_out0;
+        }
+
+        /* Refresh plane changes */
+        REG_WRITE(dsplinoff_reg, REG_READ(dsplinoff_reg));
+        REG_WRITE(dspsurf_reg, REG_READ(dspsurf_reg));
+        REG_READ(dspsurf_reg);
+
+        mdfld_dsi_send_dcs(sender,
+                           write_mem_start,
+                           NULL,
+                           0,
+                           CMD_DATA_SRC_PIPE,
+                           MDFLD_DSI_SEND_PACKAGE);
+
+        /*
+         * The idea here is to transmit a Generic Read command after the
+         * Write Memory Start/Continue commands finish.  This asks for
+         * the panel to return an "ACK No Errors," or (if it has errors
+         * to report) an Error Report.  This allows us to monitor the
+         * panel's perception of the health of the DSI.
+         */
+        mdfld_dsi_gen_fifo_ready(dev, gen_fifo_stat_reg,
+                                HS_CTRL_FIFO_EMPTY | HS_DATA_FIFO_EMPTY);
+        REG_WRITE(hs_gen_ctrl_reg, (1 << WORD_COUNTS_POS) | GEN_READ_0);
+
+        dbi_output->dsr_fb_update_done = true;
+update_fb_out0:
+        gma_power_end(dev);
+}
+
+/*
+ * TODO: will be removed later, should work out display interfaces for power
+ */
+void pyr_dsi_adapter_init(struct mdfld_dsi_config *dsi_config, int pipe)
+{
+        if (!dsi_config || (pipe != 0 && pipe != 2)) {
+                WARN_ON(1);
+                return;
+        }
+        pyr_dsi_controller_dbi_init(dsi_config, pipe);
+}
+
+static int pyr_cmd_get_panel_info(struct drm_device *dev, int pipe,
+                                                        struct panel_info *pi)
+{
+        if (!dev || !pi)
+                return -EINVAL;
+
+        pi->width_mm = PYR_PANEL_WIDTH;
+        pi->height_mm = PYR_PANEL_HEIGHT;
+
+        return 0;
+}
+
+/* PYR DBI encoder helper funcs */
+static const struct drm_encoder_helper_funcs pyr_dsi_dbi_helper_funcs = {
+        .dpms = pyr_dsi_dbi_dpms,
+        .mode_fixup = pyr_dsi_dbi_mode_fixup,
+        .prepare = pyr_dsi_dbi_prepare,
+        .mode_set = pyr_dsi_dbi_mode_set,
+        .commit = pyr_dsi_dbi_commit,
+};
+
+/* PYR DBI encoder funcs */
+static const struct drm_encoder_funcs mdfld_dsi_dbi_encoder_funcs = {
+        .destroy = drm_encoder_cleanup,
+};
+
+void pyr_cmd_init(struct drm_device *dev, struct panel_funcs *p_funcs)
+{
+        p_funcs->encoder_funcs = &mdfld_dsi_dbi_encoder_funcs;
+        p_funcs->encoder_helper_funcs = &pyr_dsi_dbi_helper_funcs;
+        p_funcs->get_config_mode = &pyr_cmd_get_config_mode;
+        p_funcs->update_fb = pyr_dsi_dbi_update_fb;
+        p_funcs->get_panel_info = pyr_cmd_get_panel_info;
+}
diff --git a/drivers/staging/gma500/mdfld_tmd_vid.c b/drivers/staging/gma500/mdfld_tmd_vid.c
new file mode 100644
index 00000000000..affdc09c676
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_tmd_vid.c
@@ -0,0 +1,206 @@
+/*
+ * Copyright © 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:
+ * Jim Liu <jim.liu@intel.com>
+ * Jackie Li<yaodong.li@intel.com>
+ * Gideon Eaton <eaton.
+ * Scott Rowe <scott.m.rowe@intel.com>
+ */
+
+#include "mdfld_dsi_dbi.h"
+#include "mdfld_dsi_dpi.h"
+#include "mdfld_dsi_output.h"
+#include "mdfld_output.h"
+
+#include "mdfld_dsi_pkg_sender.h"
+
+#include "displays/tmd_vid.h"
+
+/* FIXME: static ? */
+struct drm_display_mode *tmd_vid_get_config_mode(struct drm_device *dev)
+{
+        struct drm_display_mode *mode;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_timing_info *ti = &dev_priv->gct_data.DTD;
+        bool use_gct = false; /*Disable GCT for now*/
+
+        mode = kzalloc(sizeof(*mode), GFP_KERNEL);
+        if (!mode) {
+                dev_err(dev->dev, "Out of memory\n");
+                return NULL;
+        }
+
+        if (use_gct) {
+                dev_dbg(dev->dev, "gct find MIPI panel.\n");
+
+                mode->hdisplay = (ti->hactive_hi << 8) | ti->hactive_lo;
+                mode->vdisplay = (ti->vactive_hi << 8) | ti->vactive_lo;
+                mode->hsync_start = mode->hdisplay +
+                                ((ti->hsync_offset_hi << 8) |
+                                ti->hsync_offset_lo);
+                mode->hsync_end = mode->hsync_start +
+                                ((ti->hsync_pulse_width_hi << 8) |
+                                ti->hsync_pulse_width_lo);
+                mode->htotal = mode->hdisplay + ((ti->hblank_hi << 8) |
+                                                                ti->hblank_lo);
+                mode->vsync_start = \
+                        mode->vdisplay + ((ti->vsync_offset_hi << 8) |
+                                                ti->vsync_offset_lo);
+                mode->vsync_end = \
+                        mode->vsync_start + ((ti->vsync_pulse_width_hi << 8) | \
+                                                ti->vsync_pulse_width_lo);
+                mode->vtotal = mode->vdisplay +
+                                ((ti->vblank_hi << 8) | ti->vblank_lo);
+                mode->clock = ti->pixel_clock * 10;
+
+                dev_dbg(dev->dev, "hdisplay is %d\n", mode->hdisplay);
+                dev_dbg(dev->dev, "vdisplay is %d\n", mode->vdisplay);
+                dev_dbg(dev->dev, "HSS is %d\n", mode->hsync_start);
+                dev_dbg(dev->dev, "HSE is %d\n", mode->hsync_end);
+                dev_dbg(dev->dev, "htotal is %d\n", mode->htotal);
+                dev_dbg(dev->dev, "VSS is %d\n", mode->vsync_start);
+                dev_dbg(dev->dev, "VSE is %d\n", mode->vsync_end);
+                dev_dbg(dev->dev, "vtotal is %d\n", mode->vtotal);
+                dev_dbg(dev->dev, "clock is %d\n", mode->clock);
+        } else {
+                mode->hdisplay = 480;
+                mode->vdisplay = 854;
+                mode->hsync_start = 487;
+                mode->hsync_end = 490;
+                mode->htotal = 499;
+                mode->vsync_start = 861;
+                mode->vsync_end = 865;
+                mode->vtotal = 873;
+                mode->clock = 33264;
+        }
+        drm_mode_set_name(mode);
+        drm_mode_set_crtcinfo(mode, 0);
+
+        mode->type |= DRM_MODE_TYPE_PREFERRED;
+
+        return mode;
+}
+
+static int tmd_vid_get_panel_info(struct drm_device *dev,
+                                int pipe,
+                                struct panel_info *pi)
+{
+        if (!dev || !pi)
+                return -EINVAL;
+
+        pi->width_mm = TMD_PANEL_WIDTH;
+        pi->height_mm = TMD_PANEL_HEIGHT;
+
+        return 0;
+}
+
+/*
+ *      mdfld_init_TMD_MIPI     -       initialise a TMD interface
+ *      @dsi_config: configuration
+ *      @pipe: pipe to configure
+ *
+ *      This function is called only by mrst_dsi_mode_set and
+ *      restore_display_registers.  since this function does not
+ *      acquire the mutex, it is important that the calling function
+ *      does!
+ */
+
+
+static void mdfld_dsi_tmd_drv_ic_init(struct mdfld_dsi_config *dsi_config,
+                                      int pipe)
+{
+        static u32 tmd_cmd_mcap_off[] = {0x000000b2};
+        static u32 tmd_cmd_enable_lane_switch[] = {0x000101ef};
+        static u32 tmd_cmd_set_lane_num[] = {0x006360ef};
+        static u32 tmd_cmd_pushing_clock0[] = {0x00cc2fef};
+        static u32 tmd_cmd_pushing_clock1[] = {0x00dd6eef};
+        static u32 tmd_cmd_set_mode[] = {0x000000b3};
+        static u32 tmd_cmd_set_sync_pulse_mode[] = {0x000961ef};
+        static u32 tmd_cmd_set_column[] = {0x0100002a, 0x000000df};
+        static u32 tmd_cmd_set_page[] = {0x0300002b, 0x00000055};
+        static u32 tmd_cmd_set_video_mode[] = {0x00000153};
+        /*no auto_bl,need add in furture*/
+        static u32 tmd_cmd_enable_backlight[] = {0x00005ab4};
+        static u32 tmd_cmd_set_backlight_dimming[] = {0x00000ebd};
+
+        struct mdfld_dsi_pkg_sender *sender
+                        = mdfld_dsi_get_pkg_sender(dsi_config);
+
+        DRM_INFO("Enter mdfld init TMD MIPI display.\n");
+
+        if (!sender) {
+                DRM_ERROR("Cannot get sender\n");
+                return;
+        }
+
+        if (dsi_config->dvr_ic_inited)
+                return;
+
+        msleep(3);
+
+        mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_mcap_off, 1, 0);
+        mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_enable_lane_switch, 1, 0);
+        mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_set_lane_num, 1, 0);
+        mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_pushing_clock0, 1, 0);
+        mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_pushing_clock1, 1, 0);
+        mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_set_mode, 1, 0);
+        mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_set_sync_pulse_mode, 1, 0);
+        mdfld_dsi_send_mcs_long_lp(sender, tmd_cmd_set_column, 2, 0);
+        mdfld_dsi_send_mcs_long_lp(sender, tmd_cmd_set_page, 2, 0);
+        mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_set_video_mode, 1, 0);
+        mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_enable_backlight, 1, 0);
+        mdfld_dsi_send_gen_long_lp(sender, tmd_cmd_set_backlight_dimming, 1, 0);
+
+        dsi_config->dvr_ic_inited = 1;
+}
+
+/* TMD DPI encoder helper funcs */
+static const struct drm_encoder_helper_funcs
+                                        mdfld_tpo_dpi_encoder_helper_funcs = {
+        .dpms = mdfld_dsi_dpi_dpms,
+        .mode_fixup = mdfld_dsi_dpi_mode_fixup,
+        .prepare = mdfld_dsi_dpi_prepare,
+        .mode_set = mdfld_dsi_dpi_mode_set,
+        .commit = mdfld_dsi_dpi_commit,
+};
+
+/* TMD DPI encoder funcs */
+static const struct drm_encoder_funcs mdfld_tpo_dpi_encoder_funcs = {
+        .destroy = drm_encoder_cleanup,
+};
+
+void tmd_vid_init(struct drm_device *dev, struct panel_funcs *p_funcs)
+{
+        if (!dev || !p_funcs) {
+                dev_err(dev->dev, "Invalid parameters\n");
+                return;
+        }
+
+        p_funcs->encoder_funcs = &mdfld_tpo_dpi_encoder_funcs;
+        p_funcs->encoder_helper_funcs = &mdfld_tpo_dpi_encoder_helper_funcs;
+        p_funcs->get_config_mode = &tmd_vid_get_config_mode;
+        p_funcs->update_fb = NULL;
+        p_funcs->get_panel_info = tmd_vid_get_panel_info;
+        p_funcs->reset = mdfld_dsi_panel_reset;
+        p_funcs->drv_ic_init = mdfld_dsi_tmd_drv_ic_init;
+}
diff --git a/drivers/staging/gma500/mdfld_tpo_cmd.c b/drivers/staging/gma500/mdfld_tpo_cmd.c
new file mode 100644
index 00000000000..c7f7c9c19bc
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_tpo_cmd.c
@@ -0,0 +1,509 @@
+/*
+ * Copyright (c)  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, sublicensen
+ * 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:
+ * Thomas Eaton <thomas.g.eaton@intel.com>
+ * Scott Rowe <scott.m.rowe@intel.com>
+ */
+
+#include "mdfld_dsi_dbi.h"
+#include "mdfld_dsi_dpi.h"
+#include "mdfld_dsi_output.h"
+#include "mdfld_output.h"
+#include "mdfld_dsi_dbi_dpu.h"
+#include "mdfld_dsi_pkg_sender.h"
+
+#include "displays/tpo_cmd.h"
+
+static struct drm_display_mode *tpo_cmd_get_config_mode(struct drm_device *dev)
+{
+        struct drm_display_mode *mode;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_timing_info *ti = &dev_priv->gct_data.DTD;
+        bool use_gct = false;
+
+        mode = kzalloc(sizeof(*mode), GFP_KERNEL);
+        if (!mode)
+                return NULL;
+
+        if (use_gct) {
+                dev_dbg(dev->dev, "gct find MIPI panel.\n");
+
+                mode->hdisplay = (ti->hactive_hi << 8) | ti->hactive_lo;
+                mode->vdisplay = (ti->vactive_hi << 8) | ti->vactive_lo;
+                mode->hsync_start = mode->hdisplay + \
+                                ((ti->hsync_offset_hi << 8) | \
+                                ti->hsync_offset_lo);
+                mode->hsync_end = mode->hsync_start + \
+                                ((ti->hsync_pulse_width_hi << 8) | \
+                                ti->hsync_pulse_width_lo);
+                mode->htotal = mode->hdisplay + ((ti->hblank_hi << 8) | \
+                                                                ti->hblank_lo);
+                mode->vsync_start = \
+                        mode->vdisplay + ((ti->vsync_offset_hi << 8) | \
+                                                ti->vsync_offset_lo);
+                mode->vsync_end = \
+                        mode->vsync_start + ((ti->vsync_pulse_width_hi << 8) | \
+                                                ti->vsync_pulse_width_lo);
+                mode->vtotal = mode->vdisplay + \
+                                ((ti->vblank_hi << 8) | ti->vblank_lo);
+                mode->clock = ti->pixel_clock * 10;
+
+                dev_dbg(dev->dev, "hdisplay is %d\n", mode->hdisplay);
+                dev_dbg(dev->dev, "vdisplay is %d\n", mode->vdisplay);
+                dev_dbg(dev->dev, "HSS is %d\n", mode->hsync_start);
+                dev_dbg(dev->dev, "HSE is %d\n", mode->hsync_end);
+                dev_dbg(dev->dev, "htotal is %d\n", mode->htotal);
+                dev_dbg(dev->dev, "VSS is %d\n", mode->vsync_start);
+                dev_dbg(dev->dev, "VSE is %d\n", mode->vsync_end);
+                dev_dbg(dev->dev, "vtotal is %d\n", mode->vtotal);
+                dev_dbg(dev->dev, "clock is %d\n", mode->clock);
+        } else {
+                mode->hdisplay = 864;
+                mode->vdisplay = 480;
+                mode->hsync_start = 872;
+                mode->hsync_end = 876;
+                mode->htotal = 884;
+                mode->vsync_start = 482;
+                mode->vsync_end = 494;
+                mode->vtotal = 486;
+                mode->clock = 25777;
+        }
+
+        drm_mode_set_name(mode);
+        drm_mode_set_crtcinfo(mode, 0);
+
+        mode->type |= DRM_MODE_TYPE_PREFERRED;
+
+        return mode;
+}
+
+static bool mdfld_dsi_dbi_mode_fixup(struct drm_encoder *encoder,
+                                     struct drm_display_mode *mode,
+                                     struct drm_display_mode *adjusted_mode)
+{
+        struct drm_device *dev = encoder->dev;
+        struct drm_display_mode *fixed_mode = tpo_cmd_get_config_mode(dev);
+
+        if (fixed_mode) {
+                adjusted_mode->hdisplay = fixed_mode->hdisplay;
+                adjusted_mode->hsync_start = fixed_mode->hsync_start;
+                adjusted_mode->hsync_end = fixed_mode->hsync_end;
+                adjusted_mode->htotal = fixed_mode->htotal;
+                adjusted_mode->vdisplay = fixed_mode->vdisplay;
+                adjusted_mode->vsync_start = fixed_mode->vsync_start;
+                adjusted_mode->vsync_end = fixed_mode->vsync_end;
+                adjusted_mode->vtotal = fixed_mode->vtotal;
+                adjusted_mode->clock = fixed_mode->clock;
+                drm_mode_set_crtcinfo(adjusted_mode, CRTC_INTERLACE_HALVE_V);
+                kfree(fixed_mode);
+        }
+        return true;
+}
+
+static void mdfld_dsi_dbi_set_power(struct drm_encoder *encoder, bool on)
+{
+        int ret = 0;
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_dbi_output *dbi_output =
+                                MDFLD_DSI_DBI_OUTPUT(dsi_encoder);
+        struct mdfld_dsi_config *dsi_config =
+                mdfld_dsi_encoder_get_config(dsi_encoder);
+        struct mdfld_dsi_pkg_sender *sender =
+                mdfld_dsi_encoder_get_pkg_sender(dsi_encoder);
+        struct drm_device *dev = encoder->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 reg_offset = 0;
+        int pipe = (dbi_output->channel_num == 0) ? 0 : 2;
+        u32 data = 0;
+
+        dev_dbg(dev->dev, "pipe %d : %s, panel on: %s\n",
+                        pipe, on ? "On" : "Off",
+                        dbi_output->dbi_panel_on ? "True" : "False");
+
+        if (pipe == 2) {
+                if (on)
+                        dev_priv->dual_mipi = true;
+                else
+                        dev_priv->dual_mipi = false;
+                reg_offset = MIPIC_REG_OFFSET;
+        } else {
+                if (!on)
+                        dev_priv->dual_mipi = false;
+        }
+
+        if (!gma_power_begin(dev, true)) {
+                dev_err(dev->dev, "hw begin failed\n");
+                return;
+        }
+
+        if (on) {
+                if (dbi_output->dbi_panel_on)
+                        goto out_err;
+
+                ret = mdfld_dsi_dbi_update_power(dbi_output, DRM_MODE_DPMS_ON);
+                if (ret) {
+                        dev_err(dev->dev, "power on error\n");
+                        goto out_err;
+                }
+
+                dbi_output->dbi_panel_on = true;
+
+                if (pipe == 2)
+                        dev_priv->dbi_panel_on2 = true;
+                else
+                        dev_priv->dbi_panel_on = true;
+                mdfld_enable_te(dev, pipe);
+        } else {
+                if (!dbi_output->dbi_panel_on && !dbi_output->first_boot)
+                        goto out_err;
+
+                dbi_output->dbi_panel_on = false;
+                dbi_output->first_boot = false;
+
+                if (pipe == 2)
+                        dev_priv->dbi_panel_on2 = false;
+                else
+                        dev_priv->dbi_panel_on = false;
+
+                mdfld_disable_te(dev, pipe);
+
+                ret = mdfld_dsi_dbi_update_power(dbi_output, DRM_MODE_DPMS_OFF);
+                if (ret) {
+                        dev_err(dev->dev, "power on error\n");
+                        goto out_err;
+                }
+        }
+
+        /*
+         * FIXME: this is a WA for TPO panel crash on DPMS on & off around
+         * 83 times. the root cause of this issue is that Booster in
+         * drvIC crashed. Add this WA so that we can resume the driver IC
+         * once we found that booster has a fault
+         */
+        mdfld_dsi_get_power_mode(dsi_config,
+                                &data,
+                                MDFLD_DSI_HS_TRANSMISSION);
+
+        if (on && data && !(data & (1 << 7))) {
+                /* Soft reset */
+                mdfld_dsi_send_dcs(sender,
+                                   DCS_SOFT_RESET,
+                                   NULL,
+                                   0,
+                                   CMD_DATA_SRC_PIPE,
+                                   MDFLD_DSI_SEND_PACKAGE);
+
+                /* Init drvIC */
+                if (dbi_output->p_funcs->drv_ic_init)
+                        dbi_output->p_funcs->drv_ic_init(dsi_config,
+                                                         pipe);
+        }
+ 
+out_err:
+        gma_power_end(dev);
+        if (ret)
+                dev_err(dev->dev, "failed\n");
+}
+
+
+static void mdfld_dsi_dbi_mode_set(struct drm_encoder *encoder,
+                                   struct drm_display_mode *mode,
+                                   struct drm_display_mode *adjusted_mode)
+{
+        int ret = 0;
+        struct drm_device *dev = encoder->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_dbi_output *dsi_output =
+                                        MDFLD_DSI_DBI_OUTPUT(dsi_encoder);
+        struct mdfld_dsi_config *dsi_config =
+                                mdfld_dsi_encoder_get_config(dsi_encoder);
+        struct mdfld_dsi_connector *dsi_connector = dsi_config->connector;
+        int pipe = dsi_connector->pipe;
+        u8 param = 0;
+
+        /* Regs */
+        u32 mipi_reg = MIPI;
+        u32 dspcntr_reg = DSPACNTR;
+        u32 pipeconf_reg = PIPEACONF;
+        u32 reg_offset = 0;
+
+        /* Values */
+        u32 dspcntr_val = dev_priv->dspcntr;
+        u32 pipeconf_val = dev_priv->pipeconf;
+        u32 h_active_area = mode->hdisplay;
+        u32 v_active_area = mode->vdisplay;
+        u32 mipi_val;
+
+        mipi_val = (PASS_FROM_SPHY_TO_AFE | SEL_FLOPPED_HSTX |
+                                                TE_TRIGGER_GPIO_PIN);
+
+        dev_dbg(dev->dev, "mipi_val =0x%x\n", mipi_val);
+
+        dev_dbg(dev->dev, "type %s\n", (pipe == 2) ? "MIPI2" : "MIPI");
+        dev_dbg(dev->dev, "h %d v %d\n", mode->hdisplay, mode->vdisplay);
+
+        if (pipe == 2) {
+                mipi_reg = MIPI_C;
+                dspcntr_reg = DSPCCNTR;
+                pipeconf_reg = PIPECCONF;
+
+                reg_offset = MIPIC_REG_OFFSET;
+
+                dspcntr_val = dev_priv->dspcntr2;
+                pipeconf_val = dev_priv->pipeconf2;
+        } else {
+                mipi_val |= 0x2; /*two lanes for port A and C respectively*/
+        }
+
+        if (!gma_power_begin(dev, true)) {
+                dev_err(dev->dev, "hw begin failed\n");
+                return;
+        }
+
+        REG_WRITE(dspcntr_reg, dspcntr_val);
+        REG_READ(dspcntr_reg);
+
+        /* 20ms delay before sending exit_sleep_mode */
+        msleep(20);
+
+        /* Send exit_sleep_mode DCS */
+        ret = mdfld_dsi_dbi_send_dcs(dsi_output, DCS_EXIT_SLEEP_MODE,
+                                        NULL, 0, CMD_DATA_SRC_SYSTEM_MEM);
+        if (ret) {
+                dev_err(dev->dev, "sent exit_sleep_mode faild\n");
+                goto out_err;
+        }
+
+        /* Send set_tear_on DCS */
+        ret = mdfld_dsi_dbi_send_dcs(dsi_output, DCS_SET_TEAR_ON,
+                                        &param, 1, CMD_DATA_SRC_SYSTEM_MEM);
+        if (ret) {
+                dev_err(dev->dev, "%s - sent set_tear_on faild\n", __func__);
+                goto out_err;
+        }
+
+        /* Do some init stuff */
+        REG_WRITE(pipeconf_reg, pipeconf_val | PIPEACONF_DSR);
+        REG_READ(pipeconf_reg);
+
+        /* TODO: this looks ugly, try to move it to CRTC mode setting*/
+        if (pipe == 2)
+                dev_priv->pipeconf2 |= PIPEACONF_DSR;
+        else
+                dev_priv->pipeconf |= PIPEACONF_DSR;
+
+        dev_dbg(dev->dev, "pipeconf %x\n",  REG_READ(pipeconf_reg));
+
+        ret = mdfld_dsi_dbi_update_area(dsi_output, 0, 0,
+                                h_active_area - 1, v_active_area - 1);
+        if (ret) {
+                dev_err(dev->dev, "update area failed\n");
+                goto out_err;
+        }
+
+out_err:
+        gma_power_end(dev);
+
+        if (ret)
+                dev_err(dev->dev, "mode set failed\n");
+}
+
+static void mdfld_dsi_dbi_prepare(struct drm_encoder *encoder)
+{
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_dbi_output *dbi_output
+                                = MDFLD_DSI_DBI_OUTPUT(dsi_encoder);
+
+        dbi_output->mode_flags |= MODE_SETTING_IN_ENCODER;
+        dbi_output->mode_flags &= ~MODE_SETTING_ENCODER_DONE;
+
+        mdfld_dsi_dbi_set_power(encoder, false);
+}
+
+static void mdfld_dsi_dbi_commit(struct drm_encoder *encoder)
+{
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_dbi_output *dbi_output =
+                                        MDFLD_DSI_DBI_OUTPUT(dsi_encoder);
+        struct drm_device *dev = dbi_output->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct psb_drm_dpu_rect rect;
+
+        mdfld_dsi_dbi_set_power(encoder, true);
+        dbi_output->mode_flags &= ~MODE_SETTING_IN_ENCODER;
+
+        rect.x = rect.y = 0;
+        rect.width = 864;
+        rect.height = 480;
+
+        if (dbi_output->channel_num == 1) {
+                dev_priv->dsr_fb_update |= MDFLD_DSR_2D_3D_2;
+                /*if dpu enabled report a fullscreen damage*/
+                mdfld_dbi_dpu_report_damage(dev, MDFLD_PLANEC, &rect);
+        } else {
+                dev_priv->dsr_fb_update |= MDFLD_DSR_2D_3D_0;
+                mdfld_dbi_dpu_report_damage(dev, MDFLD_PLANEA, &rect);
+        }
+        dbi_output->mode_flags |= MODE_SETTING_ENCODER_DONE;
+}
+
+static void mdfld_dsi_dbi_dpms(struct drm_encoder *encoder, int mode)
+{
+        struct mdfld_dsi_encoder *dsi_encoder = MDFLD_DSI_ENCODER(encoder);
+        struct mdfld_dsi_dbi_output *dbi_output
+                                = MDFLD_DSI_DBI_OUTPUT(dsi_encoder);
+        struct drm_device *dev = dbi_output->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        static bool bdispoff;
+
+        dev_dbg(dev->dev, "%s\n", (mode == DRM_MODE_DPMS_ON ? "on" : "off"));
+
+        if (mode == DRM_MODE_DPMS_ON) {
+                /*
+                 * FIXME: in case I am wrong!
+                 * we don't need to exit dsr here to wake up plane/pipe/pll
+                 * if everything goes right, hw_begin will resume them all
+                 * during set_power.
+                 */
+                if (bdispoff /* FIXME && gbgfxsuspended */) {
+                        mdfld_dsi_dbi_exit_dsr(dev, MDFLD_DSR_2D_3D);
+                        bdispoff = false;
+                        dev_priv->dispstatus = true;
+                }
+
+                mdfld_dsi_dbi_set_power(encoder, true);
+                /* FIXME if (gbgfxsuspended)
+                        gbgfxsuspended = false; */
+        } else {
+                /*
+                 * I am not sure whether this is the perfect place to
+                 * turn rpm on since we still have a lot of CRTC turnning
+                 * on work to do.
+                 */
+                bdispoff = true;
+                dev_priv->dispstatus = false;
+                mdfld_dsi_dbi_set_power(encoder, false);
+        }
+}
+
+
+/*
+ * Update the DBI MIPI Panel Frame Buffer.
+ */
+static void mdfld_dsi_dbi_update_fb(struct mdfld_dsi_dbi_output *dbi_output,
+                                                                int pipe)
+{
+        struct mdfld_dsi_pkg_sender *sender =
+                mdfld_dsi_encoder_get_pkg_sender(&dbi_output->base);
+        struct drm_device *dev = dbi_output->dev;
+        struct drm_crtc *crtc = dbi_output->base.base.crtc;
+        struct psb_intel_crtc *psb_crtc = (crtc) ?
+                                        to_psb_intel_crtc(crtc) : NULL;
+        u32 dpll_reg = MRST_DPLL_A;
+        u32 dspcntr_reg = DSPACNTR;
+        u32 pipeconf_reg = PIPEACONF;
+        u32 dsplinoff_reg = DSPALINOFF;
+        u32 dspsurf_reg = DSPASURF;
+        u32 reg_offset = 0;
+
+        /* If mode setting on-going, back off */
+        if ((dbi_output->mode_flags & MODE_SETTING_ON_GOING) ||
+                (psb_crtc && psb_crtc->mode_flags & MODE_SETTING_ON_GOING) ||
+                !(dbi_output->mode_flags & MODE_SETTING_ENCODER_DONE))
+                return;
+
+        if (pipe == 2) {
+                dspcntr_reg = DSPCCNTR;
+                pipeconf_reg = PIPECCONF;
+                dsplinoff_reg = DSPCLINOFF;
+                dspsurf_reg = DSPCSURF;
+                reg_offset = MIPIC_REG_OFFSET;
+        }
+
+        if (!gma_power_begin(dev, true)) {
+                dev_err(dev->dev, "hw begin failed\n");
+                return;
+        }
+
+        /* Check DBI FIFO status */
+        if (!(REG_READ(dpll_reg) & DPLL_VCO_ENABLE) ||
+           !(REG_READ(dspcntr_reg) & DISPLAY_PLANE_ENABLE) ||
+           !(REG_READ(pipeconf_reg) & DISPLAY_PLANE_ENABLE))
+                goto update_fb_out0;
+
+        /* Refresh plane changes */
+        REG_WRITE(dsplinoff_reg, REG_READ(dsplinoff_reg));
+        REG_WRITE(dspsurf_reg, REG_READ(dspsurf_reg));
+        REG_READ(dspsurf_reg);
+
+        mdfld_dsi_send_dcs(sender,
+                           DCS_WRITE_MEM_START,
+                           NULL,
+                           0,
+                           CMD_DATA_SRC_PIPE,
+                           MDFLD_DSI_SEND_PACKAGE);
+
+        dbi_output->dsr_fb_update_done = true;
+update_fb_out0:
+        gma_power_end(dev);
+}
+
+static int tpo_cmd_get_panel_info(struct drm_device *dev,
+                                int pipe,
+                                struct panel_info *pi)
+{
+        if (!dev || !pi)
+                return -EINVAL;
+
+        pi->width_mm = TPO_PANEL_WIDTH;
+        pi->height_mm = TPO_PANEL_HEIGHT;
+
+        return 0;
+}
+
+
+/* TPO DBI encoder helper funcs */
+static const struct drm_encoder_helper_funcs mdfld_dsi_dbi_helper_funcs = {
+        .dpms = mdfld_dsi_dbi_dpms,
+        .mode_fixup = mdfld_dsi_dbi_mode_fixup,
+        .prepare = mdfld_dsi_dbi_prepare,
+        .mode_set = mdfld_dsi_dbi_mode_set,
+        .commit = mdfld_dsi_dbi_commit,
+};
+
+/* TPO DBI encoder funcs */
+static const struct drm_encoder_funcs mdfld_dsi_dbi_encoder_funcs = {
+        .destroy = drm_encoder_cleanup,
+};
+
+void tpo_cmd_init(struct drm_device *dev, struct panel_funcs *p_funcs)
+{
+        p_funcs->encoder_funcs = &mdfld_dsi_dbi_encoder_funcs;
+        p_funcs->encoder_helper_funcs = &mdfld_dsi_dbi_helper_funcs;
+        p_funcs->get_config_mode = &tpo_cmd_get_config_mode;
+        p_funcs->update_fb = mdfld_dsi_dbi_update_fb;
+        p_funcs->get_panel_info = tpo_cmd_get_panel_info;
+        p_funcs->reset = mdfld_dsi_panel_reset;
+        p_funcs->drv_ic_init = mdfld_dsi_brightness_init;
+}
diff --git a/drivers/staging/gma500/mdfld_tpo_vid.c b/drivers/staging/gma500/mdfld_tpo_vid.c
new file mode 100644
index 00000000000..95490175176
--- /dev/null
+++ b/drivers/staging/gma500/mdfld_tpo_vid.c
@@ -0,0 +1,140 @@
+/*
+ * Copyright © 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:
+ * jim liu <jim.liu@intel.com>
+ * Jackie Li<yaodong.li@intel.com>
+ */
+
+#include "mdfld_dsi_dbi.h"
+#include "mdfld_dsi_dpi.h"
+#include "mdfld_dsi_output.h"
+#include "mdfld_output.h"
+
+#include "mdfld_dsi_pkg_sender.h"
+
+#include "displays/tpo_vid.h"
+
+static struct drm_display_mode *tpo_vid_get_config_mode(struct drm_device *dev)
+{
+        struct drm_display_mode *mode;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_timing_info *ti = &dev_priv->gct_data.DTD;
+        bool use_gct = false;
+
+        mode = kzalloc(sizeof(*mode), GFP_KERNEL);
+        if (!mode) {
+                dev_err(dev->dev, "out of memory\n");
+                return NULL;
+        }
+
+        if (use_gct) {
+                mode->hdisplay = (ti->hactive_hi << 8) | ti->hactive_lo;
+                mode->vdisplay = (ti->vactive_hi << 8) | ti->vactive_lo;
+                mode->hsync_start = mode->hdisplay + \
+                                ((ti->hsync_offset_hi << 8) | \
+                                ti->hsync_offset_lo);
+                mode->hsync_end = mode->hsync_start + \
+                                ((ti->hsync_pulse_width_hi << 8) | \
+                                ti->hsync_pulse_width_lo);
+                mode->htotal = mode->hdisplay + ((ti->hblank_hi << 8) | \
+                                                                ti->hblank_lo);
+                mode->vsync_start = \
+                        mode->vdisplay + ((ti->vsync_offset_hi << 8) | \
+                                                ti->vsync_offset_lo);
+                mode->vsync_end = \
+                        mode->vsync_start + ((ti->vsync_pulse_width_hi << 8) | \
+                                                ti->vsync_pulse_width_lo);
+                mode->vtotal = mode->vdisplay + \
+                                ((ti->vblank_hi << 8) | ti->vblank_lo);
+                mode->clock = ti->pixel_clock * 10;
+
+                dev_dbg(dev->dev, "hdisplay is %d\n", mode->hdisplay);
+                dev_dbg(dev->dev, "vdisplay is %d\n", mode->vdisplay);
+                dev_dbg(dev->dev, "HSS is %d\n", mode->hsync_start);
+                dev_dbg(dev->dev, "HSE is %d\n", mode->hsync_end);
+                dev_dbg(dev->dev, "htotal is %d\n", mode->htotal);
+                dev_dbg(dev->dev, "VSS is %d\n", mode->vsync_start);
+                dev_dbg(dev->dev, "VSE is %d\n", mode->vsync_end);
+                dev_dbg(dev->dev, "vtotal is %d\n", mode->vtotal);
+                dev_dbg(dev->dev, "clock is %d\n", mode->clock);
+        } else {
+                mode->hdisplay = 864;
+                mode->vdisplay = 480;
+                mode->hsync_start = 873;
+                mode->hsync_end = 876;
+                mode->htotal = 887;
+                mode->vsync_start = 487;
+                mode->vsync_end = 490;
+                mode->vtotal = 499;
+                mode->clock = 33264;
+        }
+
+        drm_mode_set_name(mode);
+        drm_mode_set_crtcinfo(mode, 0);
+
+        mode->type |= DRM_MODE_TYPE_PREFERRED;
+
+        return mode;
+}
+
+static int tpo_vid_get_panel_info(struct drm_device *dev,
+                                int pipe,
+                                struct panel_info *pi)
+{
+        if (!dev || !pi)
+                return -EINVAL;
+
+        pi->width_mm = TPO_PANEL_WIDTH;
+        pi->height_mm = TPO_PANEL_HEIGHT;
+
+        return 0;
+}
+
+/*TPO DPI encoder helper funcs*/
+static const struct drm_encoder_helper_funcs
+                                        mdfld_tpo_dpi_encoder_helper_funcs = {
+        .dpms = mdfld_dsi_dpi_dpms,
+        .mode_fixup = mdfld_dsi_dpi_mode_fixup,
+        .prepare = mdfld_dsi_dpi_prepare,
+        .mode_set = mdfld_dsi_dpi_mode_set,
+        .commit = mdfld_dsi_dpi_commit,
+};
+
+/*TPO DPI encoder funcs*/
+static const struct drm_encoder_funcs mdfld_tpo_dpi_encoder_funcs = {
+        .destroy = drm_encoder_cleanup,
+};
+
+void tpo_vid_init(struct drm_device *dev, struct panel_funcs *p_funcs)
+{
+        if (!dev || !p_funcs) {
+                dev_err(dev->dev, "tpo_vid_init: Invalid parameters\n");
+                return;
+        }
+
+        p_funcs->encoder_funcs = &mdfld_tpo_dpi_encoder_funcs;
+        p_funcs->encoder_helper_funcs = &mdfld_tpo_dpi_encoder_helper_funcs;
+        p_funcs->get_config_mode = &tpo_vid_get_config_mode;
+        p_funcs->update_fb = NULL;
+        p_funcs->get_panel_info = tpo_vid_get_panel_info;
+}
diff --git a/drivers/staging/gma500/medfield.h b/drivers/staging/gma500/medfield.h
new file mode 100644
index 00000000000..09e9687431f
--- /dev/null
+++ b/drivers/staging/gma500/medfield.h
@@ -0,0 +1,268 @@
+/*
+ * Copyright © 2011 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.
+ */
+
+/* Medfield DSI controller registers */
+
+#define MIPIA_DEVICE_READY_REG                          0xb000
+#define MIPIA_INTR_STAT_REG                             0xb004
+#define MIPIA_INTR_EN_REG                               0xb008
+#define MIPIA_DSI_FUNC_PRG_REG                          0xb00c
+#define MIPIA_HS_TX_TIMEOUT_REG                         0xb010
+#define MIPIA_LP_RX_TIMEOUT_REG                         0xb014
+#define MIPIA_TURN_AROUND_TIMEOUT_REG                   0xb018
+#define MIPIA_DEVICE_RESET_TIMER_REG                    0xb01c
+#define MIPIA_DPI_RESOLUTION_REG                        0xb020
+#define MIPIA_DBI_FIFO_THROTTLE_REG                     0xb024
+#define MIPIA_HSYNC_COUNT_REG                           0xb028
+#define MIPIA_HBP_COUNT_REG                             0xb02c
+#define MIPIA_HFP_COUNT_REG                             0xb030
+#define MIPIA_HACTIVE_COUNT_REG                         0xb034
+#define MIPIA_VSYNC_COUNT_REG                           0xb038
+#define MIPIA_VBP_COUNT_REG                             0xb03c
+#define MIPIA_VFP_COUNT_REG                             0xb040
+#define MIPIA_HIGH_LOW_SWITCH_COUNT_REG                 0xb044
+#define MIPIA_DPI_CONTROL_REG                           0xb048
+#define MIPIA_DPI_DATA_REG                              0xb04c
+#define MIPIA_INIT_COUNT_REG                            0xb050
+#define MIPIA_MAX_RETURN_PACK_SIZE_REG                  0xb054
+#define MIPIA_VIDEO_MODE_FORMAT_REG                     0xb058
+#define MIPIA_EOT_DISABLE_REG                           0xb05c
+#define MIPIA_LP_BYTECLK_REG                            0xb060
+#define MIPIA_LP_GEN_DATA_REG                           0xb064
+#define MIPIA_HS_GEN_DATA_REG                           0xb068
+#define MIPIA_LP_GEN_CTRL_REG                           0xb06c
+#define MIPIA_HS_GEN_CTRL_REG                           0xb070
+#define MIPIA_GEN_FIFO_STAT_REG                         0xb074
+#define MIPIA_HS_LS_DBI_ENABLE_REG                      0xb078
+#define MIPIA_DPHY_PARAM_REG                            0xb080
+#define MIPIA_DBI_BW_CTRL_REG                           0xb084
+#define MIPIA_CLK_LANE_SWITCH_TIME_CNT_REG              0xb088
+
+#define DSI_DEVICE_READY                                (0x1)
+#define DSI_POWER_STATE_ULPS_ENTER                      (0x2 << 1)
+#define DSI_POWER_STATE_ULPS_EXIT                       (0x1 << 1)
+#define DSI_POWER_STATE_ULPS_OFFSET                     (0x1)
+
+
+#define DSI_ONE_DATA_LANE                               (0x1)
+#define DSI_TWO_DATA_LANE                               (0x2)
+#define DSI_THREE_DATA_LANE                             (0X3)
+#define DSI_FOUR_DATA_LANE                              (0x4)
+#define DSI_DPI_VIRT_CHANNEL_OFFSET                     (0x3)
+#define DSI_DBI_VIRT_CHANNEL_OFFSET                     (0x5)
+#define DSI_DPI_COLOR_FORMAT_RGB565                     (0x01 << 7)
+#define DSI_DPI_COLOR_FORMAT_RGB666                     (0x02 << 7)
+#define DSI_DPI_COLOR_FORMAT_RGB666_UNPACK              (0x03 << 7)
+#define DSI_DPI_COLOR_FORMAT_RGB888                     (0x04 << 7)
+#define DSI_DBI_COLOR_FORMAT_OPTION2                    (0x05 << 13)
+
+#define DSI_INTR_STATE_RXSOTERROR                       1
+
+#define DSI_INTR_STATE_SPL_PKG_SENT                     (1 << 30)
+#define DSI_INTR_STATE_TE                               (1 << 31)
+
+#define DSI_HS_TX_TIMEOUT_MASK                          (0xffffff)
+
+#define DSI_LP_RX_TIMEOUT_MASK                          (0xffffff)
+
+#define DSI_TURN_AROUND_TIMEOUT_MASK                    (0x3f)
+
+#define DSI_RESET_TIMER_MASK                            (0xffff)
+
+#define DSI_DBI_FIFO_WM_HALF                            (0x0)
+#define DSI_DBI_FIFO_WM_QUARTER                         (0x1)
+#define DSI_DBI_FIFO_WM_LOW                             (0x2)
+
+#define DSI_DPI_TIMING_MASK                             (0xffff)
+
+#define DSI_INIT_TIMER_MASK                             (0xffff)
+
+#define DSI_DBI_RETURN_PACK_SIZE_MASK                   (0x3ff)
+
+#define DSI_LP_BYTECLK_MASK                             (0x0ffff)
+
+#define DSI_HS_CTRL_GEN_SHORT_W0                        (0x03)
+#define DSI_HS_CTRL_GEN_SHORT_W1                        (0x13)
+#define DSI_HS_CTRL_GEN_SHORT_W2                        (0x23)
+#define DSI_HS_CTRL_GEN_R0                              (0x04)
+#define DSI_HS_CTRL_GEN_R1                              (0x14)
+#define DSI_HS_CTRL_GEN_R2                              (0x24)
+#define DSI_HS_CTRL_GEN_LONG_W                          (0x29)
+#define DSI_HS_CTRL_MCS_SHORT_W0                        (0x05)
+#define DSI_HS_CTRL_MCS_SHORT_W1                        (0x15)
+#define DSI_HS_CTRL_MCS_R0                              (0x06)
+#define DSI_HS_CTRL_MCS_LONG_W                          (0x39)
+#define DSI_HS_CTRL_VC_OFFSET                           (0x06)
+#define DSI_HS_CTRL_WC_OFFSET                           (0x08)
+
+#define DSI_FIFO_GEN_HS_DATA_FULL                       (1 << 0)
+#define DSI_FIFO_GEN_HS_DATA_HALF_EMPTY                 (1 << 1)
+#define DSI_FIFO_GEN_HS_DATA_EMPTY                      (1 << 2)
+#define DSI_FIFO_GEN_LP_DATA_FULL                       (1 << 8)
+#define DSI_FIFO_GEN_LP_DATA_HALF_EMPTY                 (1 << 9)
+#define DSI_FIFO_GEN_LP_DATA_EMPTY                      (1 << 10)
+#define DSI_FIFO_GEN_HS_CTRL_FULL                       (1 << 16)
+#define DSI_FIFO_GEN_HS_CTRL_HALF_EMPTY                 (1 << 17)
+#define DSI_FIFO_GEN_HS_CTRL_EMPTY                      (1 << 18)
+#define DSI_FIFO_GEN_LP_CTRL_FULL                       (1 << 24)
+#define DSI_FIFO_GEN_LP_CTRL_HALF_EMPTY                 (1 << 25)
+#define DSI_FIFO_GEN_LP_CTRL_EMPTY                      (1 << 26)
+#define DSI_FIFO_DBI_EMPTY                              (1 << 27)
+#define DSI_FIFO_DPI_EMPTY                              (1 << 28)
+
+#define DSI_DBI_HS_LP_SWITCH_MASK                       (0x1)
+
+#define DSI_HS_LP_SWITCH_COUNTER_OFFSET                 (0x0)
+#define DSI_LP_HS_SWITCH_COUNTER_OFFSET                 (0x16)
+
+#define DSI_DPI_CTRL_HS_SHUTDOWN                        (0x00000001)
+#define DSI_DPI_CTRL_HS_TURN_ON                         (0x00000002)
+
+/* Medfield DSI adapter registers */
+#define MIPIA_CONTROL_REG                               0xb104
+#define MIPIA_DATA_ADD_REG                              0xb108
+#define MIPIA_DATA_LEN_REG                              0xb10c
+#define MIPIA_CMD_ADD_REG                               0xb110
+#define MIPIA_CMD_LEN_REG                               0xb114
+
+/*dsi power modes*/
+#define DSI_POWER_MODE_DISPLAY_ON       (1 << 2)
+#define DSI_POWER_MODE_NORMAL_ON        (1 << 3)
+#define DSI_POWER_MODE_SLEEP_OUT        (1 << 4)
+#define DSI_POWER_MODE_PARTIAL_ON       (1 << 5)
+#define DSI_POWER_MODE_IDLE_ON          (1 << 6)
+
+enum {
+        MDFLD_DSI_ENCODER_DBI = 0,
+        MDFLD_DSI_ENCODER_DPI,
+};
+
+enum {
+        MDFLD_DSI_VIDEO_NON_BURST_MODE_SYNC_PULSE = 1,
+        MDFLD_DSI_VIDEO_NON_BURST_MODE_SYNC_EVENTS = 2,
+        MDFLD_DSI_VIDEO_BURST_MODE = 3,
+};
+
+#define DSI_DPI_COMPLETE_LAST_LINE                      (1 << 2)
+#define DSI_DPI_DISABLE_BTA                             (1 << 3)
+/* Panel types */
+enum {
+        TPO_CMD,
+        TPO_VID,
+        TMD_CMD,
+        TMD_VID,
+        PYR_CMD,
+        PYR_VID,
+        TPO,
+        TMD,
+        PYR,
+        HDMI,
+        GCT_DETECT
+};
+
+/* Junk that belongs elsewhere */
+#define TPO_PANEL_WIDTH         84
+#define TPO_PANEL_HEIGHT        46
+#define TMD_PANEL_WIDTH         39
+#define TMD_PANEL_HEIGHT        71
+#define PYR_PANEL_WIDTH         53
+#define PYR_PANEL_HEIGHT        95
+
+/* Panel interface */
+struct panel_info {
+        u32 width_mm;
+        u32 height_mm;
+};
+
+struct mdfld_dsi_dbi_output;
+
+struct mdfld_dsi_connector_state {
+        u32 mipi_ctrl_reg;
+};
+
+struct mdfld_dsi_encoder_state {
+
+};
+
+struct mdfld_dsi_connector {
+        /*
+         * This is ugly, but I have to use connector in it! :-(
+         * FIXME: use drm_connector instead.
+         */
+        struct psb_intel_output base;
+
+        int pipe;
+        void *private;
+        void *pkg_sender;
+
+        /* Connection status */
+        enum drm_connector_status status;
+};
+
+struct mdfld_dsi_encoder {
+        struct drm_encoder base;
+        void *private;
+};
+
+/*
+ * DSI config, consists of one DSI connector, two DSI encoders.
+ * DRM will pick up on DSI encoder basing on differents configs.
+ */
+struct mdfld_dsi_config {
+        struct drm_device *dev;
+        struct drm_display_mode *fixed_mode;
+        struct drm_display_mode *mode;
+
+        struct mdfld_dsi_connector *connector;
+        struct mdfld_dsi_encoder *encoders[DRM_CONNECTOR_MAX_ENCODER];
+        struct mdfld_dsi_encoder *encoder;
+
+        int changed;
+
+        int bpp;
+        int type;
+        int lane_count;
+        /*Virtual channel number for this encoder*/
+        int channel_num;
+        /*video mode configure*/
+        int video_mode;
+
+        int dvr_ic_inited;
+};
+
+#define MDFLD_DSI_CONNECTOR(psb_output) \
+                (container_of(psb_output, struct mdfld_dsi_connector, base))
+
+#define MDFLD_DSI_ENCODER(encoder) \
+                (container_of(encoder, struct mdfld_dsi_encoder, base))
+
+struct panel_funcs {
+        const struct drm_encoder_funcs *encoder_funcs;
+        const struct drm_encoder_helper_funcs *encoder_helper_funcs;
+        struct drm_display_mode *(*get_config_mode) (struct drm_device *);
+        void (*update_fb) (struct mdfld_dsi_dbi_output *, int);
+        int (*get_panel_info) (struct drm_device *, int, struct panel_info *);
+        int (*reset)(int pipe);
+        void (*drv_ic_init)(struct mdfld_dsi_config *dsi_config, int pipe);
+};
+
diff --git a/drivers/staging/gma500/mid_bios.c b/drivers/staging/gma500/mid_bios.c
new file mode 100644
index 00000000000..8cfe301f8fb
--- /dev/null
+++ b/drivers/staging/gma500/mid_bios.c
@@ -0,0 +1,269 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+/* TODO
+ * - Split functions by vbt type
+ * - Make them all take drm_device
+ * - Check ioremap failures
+ */
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "psb_drm.h"
+#include "psb_drv.h"
+#include "mid_bios.h"
+#include "mdfld_output.h"
+
+static int panel_id = GCT_DETECT;
+module_param_named(panel_id, panel_id, int, 0600);
+MODULE_PARM_DESC(panel_id, "Panel Identifier");
+
+
+static void mid_get_fuse_settings(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+        uint32_t fuse_value = 0;
+        uint32_t fuse_value_tmp = 0;
+
+#define FB_REG06 0xD0810600
+#define FB_MIPI_DISABLE  (1 << 11)
+#define FB_REG09 0xD0810900
+#define FB_REG09 0xD0810900
+#define FB_SKU_MASK  0x7000
+#define FB_SKU_SHIFT 12
+#define FB_SKU_100 0
+#define FB_SKU_100L 1
+#define FB_SKU_83 2
+        pci_write_config_dword(pci_root, 0xD0, FB_REG06);
+        pci_read_config_dword(pci_root, 0xD4, &fuse_value);
+
+        /* FB_MIPI_DISABLE doesn't mean LVDS on with Medfield */
+        if (IS_MRST(dev))
+                dev_priv->iLVDS_enable = fuse_value & FB_MIPI_DISABLE;
+
+        DRM_INFO("internal display is %s\n",
+                 dev_priv->iLVDS_enable ? "LVDS display" : "MIPI display");
+
+         /* Prevent runtime suspend at start*/
+         if (dev_priv->iLVDS_enable) {
+                dev_priv->is_lvds_on = true;
+                dev_priv->is_mipi_on = false;
+        } else {
+                dev_priv->is_mipi_on = true;
+                dev_priv->is_lvds_on = false;
+        }
+
+        dev_priv->video_device_fuse = fuse_value;
+
+        pci_write_config_dword(pci_root, 0xD0, FB_REG09);
+        pci_read_config_dword(pci_root, 0xD4, &fuse_value);
+
+        dev_dbg(dev->dev, "SKU values is 0x%x.\n", fuse_value);
+        fuse_value_tmp = (fuse_value & FB_SKU_MASK) >> FB_SKU_SHIFT;
+
+        dev_priv->fuse_reg_value = fuse_value;
+
+        switch (fuse_value_tmp) {
+        case FB_SKU_100:
+                dev_priv->core_freq = 200;
+                break;
+        case FB_SKU_100L:
+                dev_priv->core_freq = 100;
+                break;
+        case FB_SKU_83:
+                dev_priv->core_freq = 166;
+                break;
+        default:
+                dev_warn(dev->dev, "Invalid SKU values, SKU value = 0x%08x\n",
+                                                                fuse_value_tmp);
+                dev_priv->core_freq = 0;
+        }
+        dev_dbg(dev->dev, "LNC core clk is %dMHz.\n", dev_priv->core_freq);
+        pci_dev_put(pci_root);
+}
+
+/*
+ *      Get the revison ID, B0:D2:F0;0x08
+ */
+static void mid_get_pci_revID(struct drm_psb_private *dev_priv)
+{
+        uint32_t platform_rev_id = 0;
+        struct pci_dev *pci_gfx_root = pci_get_bus_and_slot(0, PCI_DEVFN(2, 0));
+
+        pci_read_config_dword(pci_gfx_root, 0x08, &platform_rev_id);
+        dev_priv->platform_rev_id = (uint8_t) platform_rev_id;
+        pci_dev_put(pci_gfx_root);
+        dev_dbg(dev_priv->dev->dev, "platform_rev_id is %x\n",
+                                        dev_priv->platform_rev_id);
+}
+
+static void mid_get_vbt_data(struct drm_psb_private *dev_priv)
+{
+        struct drm_device *dev = dev_priv->dev;
+        struct mrst_vbt *vbt = &dev_priv->vbt_data;
+        u32 addr;
+        u16 new_size;
+        u8 *vbt_virtual;
+        u8 bpi;
+        u8 number_desc = 0;
+        struct mrst_timing_info *dp_ti = &dev_priv->gct_data.DTD;
+        struct gct_r10_timing_info ti;
+        void *pGCT;
+        struct pci_dev *pci_gfx_root = pci_get_bus_and_slot(0, PCI_DEVFN(2, 0));
+
+        /* Get the address of the platform config vbt, B0:D2:F0;0xFC */
+        pci_read_config_dword(pci_gfx_root, 0xFC, &addr);
+        pci_dev_put(pci_gfx_root);
+
+        dev_dbg(dev->dev, "drm platform config address is %x\n", addr);
+
+        /* check for platform config address == 0. */
+        /* this means fw doesn't support vbt */
+
+        if (addr == 0) {
+                vbt->size = 0;
+                return;
+        }
+
+        /* get the virtual address of the vbt */
+        vbt_virtual = ioremap(addr, sizeof(*vbt));
+
+        memcpy(vbt, vbt_virtual, sizeof(*vbt));
+        iounmap(vbt_virtual); /* Free virtual address space */
+
+        dev_dbg(dev->dev, "GCT revision is %x\n", vbt->revision);
+
+        switch (vbt->revision) {
+        case 0:
+                vbt->mrst_gct = ioremap(addr + sizeof(*vbt) - 4,
+                                        vbt->size - sizeof(*vbt) + 4);
+                pGCT = vbt->mrst_gct;
+                bpi = ((struct mrst_gct_v1 *)pGCT)->PD.BootPanelIndex;
+                dev_priv->gct_data.bpi = bpi;
+                dev_priv->gct_data.pt =
+                        ((struct mrst_gct_v1 *)pGCT)->PD.PanelType;
+                memcpy(&dev_priv->gct_data.DTD,
+                        &((struct mrst_gct_v1 *)pGCT)->panel[bpi].DTD,
+                                sizeof(struct mrst_timing_info));
+                dev_priv->gct_data.Panel_Port_Control =
+                  ((struct mrst_gct_v1 *)pGCT)->panel[bpi].Panel_Port_Control;
+                dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
+                        ((struct mrst_gct_v1 *)pGCT)->panel[bpi].Panel_MIPI_Display_Descriptor;
+                break;
+        case 1:
+                vbt->mrst_gct = ioremap(addr + sizeof(*vbt) - 4,
+                                        vbt->size - sizeof(*vbt) + 4);
+                pGCT = vbt->mrst_gct;
+                bpi = ((struct mrst_gct_v2 *)pGCT)->PD.BootPanelIndex;
+                dev_priv->gct_data.bpi = bpi;
+                dev_priv->gct_data.pt =
+                        ((struct mrst_gct_v2 *)pGCT)->PD.PanelType;
+                memcpy(&dev_priv->gct_data.DTD,
+                        &((struct mrst_gct_v2 *)pGCT)->panel[bpi].DTD,
+                                sizeof(struct mrst_timing_info));
+                dev_priv->gct_data.Panel_Port_Control =
+                  ((struct mrst_gct_v2 *)pGCT)->panel[bpi].Panel_Port_Control;
+                dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
+                        ((struct mrst_gct_v2 *)pGCT)->panel[bpi].Panel_MIPI_Display_Descriptor;
+                break;
+        case 0x10:
+                /*header definition changed from rev 01 (v2) to rev 10h. */
+                /*so, some values have changed location*/
+                new_size = vbt->checksum; /*checksum contains lo size byte*/
+                /*LSB of mrst_gct contains hi size byte*/
+                new_size |= ((0xff & (unsigned int)vbt->mrst_gct)) << 8;
+
+                vbt->checksum = vbt->size; /*size contains the checksum*/
+                if (new_size > 0xff)
+                        vbt->size = 0xff; /*restrict size to 255*/
+                else
+                        vbt->size = new_size;
+
+                /* number of descriptors defined in the GCT */
+                number_desc = ((0xff00 & (unsigned int)vbt->mrst_gct)) >> 8;
+                bpi = ((0xff0000 & (unsigned int)vbt->mrst_gct)) >> 16;
+                vbt->mrst_gct = ioremap(addr + GCT_R10_HEADER_SIZE,
+                                GCT_R10_DISPLAY_DESC_SIZE * number_desc);
+                pGCT = vbt->mrst_gct;
+                pGCT = (u8 *)pGCT + (bpi*GCT_R10_DISPLAY_DESC_SIZE);
+                dev_priv->gct_data.bpi = bpi; /*save boot panel id*/
+
+                /*copy the GCT display timings into a temp structure*/
+                memcpy(&ti, pGCT, sizeof(struct gct_r10_timing_info));
+
+                /*now copy the temp struct into the dev_priv->gct_data*/
+                dp_ti->pixel_clock = ti.pixel_clock;
+                dp_ti->hactive_hi = ti.hactive_hi;
+                dp_ti->hactive_lo = ti.hactive_lo;
+                dp_ti->hblank_hi = ti.hblank_hi;
+                dp_ti->hblank_lo = ti.hblank_lo;
+                dp_ti->hsync_offset_hi = ti.hsync_offset_hi;
+                dp_ti->hsync_offset_lo = ti.hsync_offset_lo;
+                dp_ti->hsync_pulse_width_hi = ti.hsync_pulse_width_hi;
+                dp_ti->hsync_pulse_width_lo = ti.hsync_pulse_width_lo;
+                dp_ti->vactive_hi = ti.vactive_hi;
+                dp_ti->vactive_lo = ti.vactive_lo;
+                dp_ti->vblank_hi = ti.vblank_hi;
+                dp_ti->vblank_lo = ti.vblank_lo;
+                dp_ti->vsync_offset_hi = ti.vsync_offset_hi;
+                dp_ti->vsync_offset_lo = ti.vsync_offset_lo;
+                dp_ti->vsync_pulse_width_hi = ti.vsync_pulse_width_hi;
+                dp_ti->vsync_pulse_width_lo = ti.vsync_pulse_width_lo;
+
+                /* Move the MIPI_Display_Descriptor data from GCT to dev priv */
+                dev_priv->gct_data.Panel_MIPI_Display_Descriptor =
+                                                        *((u8 *)pGCT + 0x0d);
+                dev_priv->gct_data.Panel_MIPI_Display_Descriptor |=
+                                                (*((u8 *)pGCT + 0x0e)) << 8;
+                break;
+        default:
+                dev_err(dev->dev, "Unknown revision of GCT!\n");
+                vbt->size = 0;
+        }
+        if (IS_MFLD(dev_priv->dev)) {
+                if (panel_id == GCT_DETECT) {
+                        if (dev_priv->gct_data.bpi == 2) {
+                                dev_info(dev->dev, "[GFX] PYR Panel Detected\n");
+                                dev_priv->panel_id = PYR_CMD;
+                                panel_id = PYR_CMD;
+                        } else if (dev_priv->gct_data.bpi == 0) {
+                                dev_info(dev->dev, "[GFX] TMD Panel Detected.\n");
+                                dev_priv->panel_id = TMD_VID;
+                                panel_id = TMD_VID;
+                        } else {
+                                dev_info(dev->dev, "[GFX] Default Panel (TPO)\n");
+                                dev_priv->panel_id = TPO_CMD;
+                                panel_id = TPO_CMD;
+                        }
+                } else {
+                        dev_info(dev->dev, "[GFX] Panel Parameter Passed in through cmd line\n");
+                        dev_priv->panel_id = panel_id;
+                }
+        }
+}
+
+int mid_chip_setup(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        mid_get_fuse_settings(dev);
+        mid_get_vbt_data(dev_priv);
+        mid_get_pci_revID(dev_priv);
+        return 0;
+}
diff --git a/drivers/staging/gma500/mid_bios.h b/drivers/staging/gma500/mid_bios.h
new file mode 100644
index 00000000000..00e7d564b7e
--- /dev/null
+++ b/drivers/staging/gma500/mid_bios.h
@@ -0,0 +1,21 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+extern int mid_chip_setup(struct drm_device *dev);
+
diff --git a/drivers/staging/gma500/mmu.c b/drivers/staging/gma500/mmu.c
new file mode 100644
index 00000000000..c904d73b1de
--- /dev/null
+++ b/drivers/staging/gma500/mmu.c
@@ -0,0 +1,858 @@
+/**************************************************************************
+ * Copyright (c) 2007, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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 <drm/drmP.h>
+#include "psb_drv.h"
+#include "psb_reg.h"
+
+/*
+ * Code for the SGX MMU:
+ */
+
+/*
+ * clflush on one processor only:
+ * clflush should apparently flush the cache line on all processors in an
+ * SMP system.
+ */
+
+/*
+ * kmap atomic:
+ * The usage of the slots must be completely encapsulated within a spinlock, and
+ * no other functions that may be using the locks for other purposed may be
+ * called from within the locked region.
+ * Since the slots are per processor, this will guarantee that we are the only
+ * user.
+ */
+
+/*
+ * TODO: Inserting ptes from an interrupt handler:
+ * This may be desirable for some SGX functionality where the GPU can fault in
+ * needed pages. For that, we need to make an atomic insert_pages function, that
+ * may fail.
+ * If it fails, the caller need to insert the page using a workqueue function,
+ * but on average it should be fast.
+ */
+
+struct psb_mmu_driver {
+        /* protects driver- and pd structures. Always take in read mode
+         * before taking the page table spinlock.
+         */
+        struct rw_semaphore sem;
+
+        /* protects page tables, directory tables and pt tables.
+         * and pt structures.
+         */
+        spinlock_t lock;
+
+        atomic_t needs_tlbflush;
+
+        uint8_t __iomem *register_map;
+        struct psb_mmu_pd *default_pd;
+        /*uint32_t bif_ctrl;*/
+        int has_clflush;
+        int clflush_add;
+        unsigned long clflush_mask;
+
+        struct drm_psb_private *dev_priv;
+};
+
+struct psb_mmu_pd;
+
+struct psb_mmu_pt {
+        struct psb_mmu_pd *pd;
+        uint32_t index;
+        uint32_t count;
+        struct page *p;
+        uint32_t *v;
+};
+
+struct psb_mmu_pd {
+        struct psb_mmu_driver *driver;
+        int hw_context;
+        struct psb_mmu_pt **tables;
+        struct page *p;
+        struct page *dummy_pt;
+        struct page *dummy_page;
+        uint32_t pd_mask;
+        uint32_t invalid_pde;
+        uint32_t invalid_pte;
+};
+
+static inline uint32_t psb_mmu_pt_index(uint32_t offset)
+{
+        return (offset >> PSB_PTE_SHIFT) & 0x3FF;
+}
+
+static inline uint32_t psb_mmu_pd_index(uint32_t offset)
+{
+        return offset >> PSB_PDE_SHIFT;
+}
+
+static inline void psb_clflush(void *addr)
+{
+        __asm__ __volatile__("clflush (%0)\n" : : "r"(addr) : "memory");
+}
+
+static inline void psb_mmu_clflush(struct psb_mmu_driver *driver,
+                                   void *addr)
+{
+        if (!driver->has_clflush)
+                return;
+
+        mb();
+        psb_clflush(addr);
+        mb();
+}
+
+static void psb_page_clflush(struct psb_mmu_driver *driver, struct page* page)
+{
+        uint32_t clflush_add = driver->clflush_add >> PAGE_SHIFT;
+        uint32_t clflush_count = PAGE_SIZE / clflush_add;
+        int i;
+        uint8_t *clf;
+
+        clf = kmap_atomic(page, KM_USER0);
+        mb();
+        for (i = 0; i < clflush_count; ++i) {
+                psb_clflush(clf);
+                clf += clflush_add;
+        }
+        mb();
+        kunmap_atomic(clf, KM_USER0);
+}
+
+static void psb_pages_clflush(struct psb_mmu_driver *driver,
+                                struct page *page[], unsigned long num_pages)
+{
+        int i;
+
+        if (!driver->has_clflush)
+                return ;
+
+        for (i = 0; i < num_pages; i++)
+                psb_page_clflush(driver, *page++);
+}
+
+static void psb_mmu_flush_pd_locked(struct psb_mmu_driver *driver,
+                                    int force)
+{
+        atomic_set(&driver->needs_tlbflush, 0);
+}
+
+static void psb_mmu_flush_pd(struct psb_mmu_driver *driver, int force)
+{
+        down_write(&driver->sem);
+        psb_mmu_flush_pd_locked(driver, force);
+        up_write(&driver->sem);
+}
+
+void psb_mmu_flush(struct psb_mmu_driver *driver, int rc_prot)
+{
+        if (rc_prot)
+                down_write(&driver->sem);
+        if (rc_prot)
+                up_write(&driver->sem);
+}
+
+void psb_mmu_set_pd_context(struct psb_mmu_pd *pd, int hw_context)
+{
+        /*ttm_tt_cache_flush(&pd->p, 1);*/
+        psb_pages_clflush(pd->driver, &pd->p, 1);
+        down_write(&pd->driver->sem);
+        wmb();
+        psb_mmu_flush_pd_locked(pd->driver, 1);
+        pd->hw_context = hw_context;
+        up_write(&pd->driver->sem);
+
+}
+
+static inline unsigned long psb_pd_addr_end(unsigned long addr,
+                                            unsigned long end)
+{
+
+        addr = (addr + PSB_PDE_MASK + 1) & ~PSB_PDE_MASK;
+        return (addr < end) ? addr : end;
+}
+
+static inline uint32_t psb_mmu_mask_pte(uint32_t pfn, int type)
+{
+        uint32_t mask = PSB_PTE_VALID;
+
+        if (type & PSB_MMU_CACHED_MEMORY)
+                mask |= PSB_PTE_CACHED;
+        if (type & PSB_MMU_RO_MEMORY)
+                mask |= PSB_PTE_RO;
+        if (type & PSB_MMU_WO_MEMORY)
+                mask |= PSB_PTE_WO;
+
+        return (pfn << PAGE_SHIFT) | mask;
+}
+
+struct psb_mmu_pd *psb_mmu_alloc_pd(struct psb_mmu_driver *driver,
+                                    int trap_pagefaults, int invalid_type)
+{
+        struct psb_mmu_pd *pd = kmalloc(sizeof(*pd), GFP_KERNEL);
+        uint32_t *v;
+        int i;
+
+        if (!pd)
+                return NULL;
+
+        pd->p = alloc_page(GFP_DMA32);
+        if (!pd->p)
+                goto out_err1;
+        pd->dummy_pt = alloc_page(GFP_DMA32);
+        if (!pd->dummy_pt)
+                goto out_err2;
+        pd->dummy_page = alloc_page(GFP_DMA32);
+        if (!pd->dummy_page)
+                goto out_err3;
+
+        if (!trap_pagefaults) {
+                pd->invalid_pde =
+                    psb_mmu_mask_pte(page_to_pfn(pd->dummy_pt),
+                                     invalid_type);
+                pd->invalid_pte =
+                    psb_mmu_mask_pte(page_to_pfn(pd->dummy_page),
+                                     invalid_type);
+        } else {
+                pd->invalid_pde = 0;
+                pd->invalid_pte = 0;
+        }
+
+        v = kmap(pd->dummy_pt);
+        for (i = 0; i < (PAGE_SIZE / sizeof(uint32_t)); ++i)
+                v[i] = pd->invalid_pte;
+
+        kunmap(pd->dummy_pt);
+
+        v = kmap(pd->p);
+        for (i = 0; i < (PAGE_SIZE / sizeof(uint32_t)); ++i)
+                v[i] = pd->invalid_pde;
+
+        kunmap(pd->p);
+
+        clear_page(kmap(pd->dummy_page));
+        kunmap(pd->dummy_page);
+
+        pd->tables = vmalloc_user(sizeof(struct psb_mmu_pt *) * 1024);
+        if (!pd->tables)
+                goto out_err4;
+
+        pd->hw_context = -1;
+        pd->pd_mask = PSB_PTE_VALID;
+        pd->driver = driver;
+
+        return pd;
+
+out_err4:
+        __free_page(pd->dummy_page);
+out_err3:
+        __free_page(pd->dummy_pt);
+out_err2:
+        __free_page(pd->p);
+out_err1:
+        kfree(pd);
+        return NULL;
+}
+
+void psb_mmu_free_pt(struct psb_mmu_pt *pt)
+{
+        __free_page(pt->p);
+        kfree(pt);
+}
+
+void psb_mmu_free_pagedir(struct psb_mmu_pd *pd)
+{
+        struct psb_mmu_driver *driver = pd->driver;
+        struct psb_mmu_pt *pt;
+        int i;
+
+        down_write(&driver->sem);
+        if (pd->hw_context != -1)
+                psb_mmu_flush_pd_locked(driver, 1);
+
+        /* Should take the spinlock here, but we don't need to do that
+           since we have the semaphore in write mode. */
+
+        for (i = 0; i < 1024; ++i) {
+                pt = pd->tables[i];
+                if (pt)
+                        psb_mmu_free_pt(pt);
+        }
+
+        vfree(pd->tables);
+        __free_page(pd->dummy_page);
+        __free_page(pd->dummy_pt);
+        __free_page(pd->p);
+        kfree(pd);
+        up_write(&driver->sem);
+}
+
+static struct psb_mmu_pt *psb_mmu_alloc_pt(struct psb_mmu_pd *pd)
+{
+        struct psb_mmu_pt *pt = kmalloc(sizeof(*pt), GFP_KERNEL);
+        void *v;
+        uint32_t clflush_add = pd->driver->clflush_add >> PAGE_SHIFT;
+        uint32_t clflush_count = PAGE_SIZE / clflush_add;
+        spinlock_t *lock = &pd->driver->lock;
+        uint8_t *clf;
+        uint32_t *ptes;
+        int i;
+
+        if (!pt)
+                return NULL;
+
+        pt->p = alloc_page(GFP_DMA32);
+        if (!pt->p) {
+                kfree(pt);
+                return NULL;
+        }
+
+        spin_lock(lock);
+
+        v = kmap_atomic(pt->p, KM_USER0);
+        clf = (uint8_t *) v;
+        ptes = (uint32_t *) v;
+        for (i = 0; i < (PAGE_SIZE / sizeof(uint32_t)); ++i)
+                *ptes++ = pd->invalid_pte;
+
+
+        if (pd->driver->has_clflush && pd->hw_context != -1) {
+                mb();
+                for (i = 0; i < clflush_count; ++i) {
+                        psb_clflush(clf);
+                        clf += clflush_add;
+                }
+                mb();
+        }
+
+        kunmap_atomic(v, KM_USER0);
+        spin_unlock(lock);
+
+        pt->count = 0;
+        pt->pd = pd;
+        pt->index = 0;
+
+        return pt;
+}
+
+struct psb_mmu_pt *psb_mmu_pt_alloc_map_lock(struct psb_mmu_pd *pd,
+                                             unsigned long addr)
+{
+        uint32_t index = psb_mmu_pd_index(addr);
+        struct psb_mmu_pt *pt;
+        uint32_t *v;
+        spinlock_t *lock = &pd->driver->lock;
+
+        spin_lock(lock);
+        pt = pd->tables[index];
+        while (!pt) {
+                spin_unlock(lock);
+                pt = psb_mmu_alloc_pt(pd);
+                if (!pt)
+                        return NULL;
+                spin_lock(lock);
+
+                if (pd->tables[index]) {
+                        spin_unlock(lock);
+                        psb_mmu_free_pt(pt);
+                        spin_lock(lock);
+                        pt = pd->tables[index];
+                        continue;
+                }
+
+                v = kmap_atomic(pd->p, KM_USER0);
+                pd->tables[index] = pt;
+                v[index] = (page_to_pfn(pt->p) << 12) | pd->pd_mask;
+                pt->index = index;
+                kunmap_atomic((void *) v, KM_USER0);
+
+                if (pd->hw_context != -1) {
+                        psb_mmu_clflush(pd->driver, (void *) &v[index]);
+                        atomic_set(&pd->driver->needs_tlbflush, 1);
+                }
+        }
+        pt->v = kmap_atomic(pt->p, KM_USER0);
+        return pt;
+}
+
+static struct psb_mmu_pt *psb_mmu_pt_map_lock(struct psb_mmu_pd *pd,
+                                              unsigned long addr)
+{
+        uint32_t index = psb_mmu_pd_index(addr);
+        struct psb_mmu_pt *pt;
+        spinlock_t *lock = &pd->driver->lock;
+
+        spin_lock(lock);
+        pt = pd->tables[index];
+        if (!pt) {
+                spin_unlock(lock);
+                return NULL;
+        }
+        pt->v = kmap_atomic(pt->p, KM_USER0);
+        return pt;
+}
+
+static void psb_mmu_pt_unmap_unlock(struct psb_mmu_pt *pt)
+{
+        struct psb_mmu_pd *pd = pt->pd;
+        uint32_t *v;
+
+        kunmap_atomic(pt->v, KM_USER0);
+        if (pt->count == 0) {
+                v = kmap_atomic(pd->p, KM_USER0);
+                v[pt->index] = pd->invalid_pde;
+                pd->tables[pt->index] = NULL;
+
+                if (pd->hw_context != -1) {
+                        psb_mmu_clflush(pd->driver,
+                                        (void *) &v[pt->index]);
+                        atomic_set(&pd->driver->needs_tlbflush, 1);
+                }
+                kunmap_atomic(pt->v, KM_USER0);
+                spin_unlock(&pd->driver->lock);
+                psb_mmu_free_pt(pt);
+                return;
+        }
+        spin_unlock(&pd->driver->lock);
+}
+
+static inline void psb_mmu_set_pte(struct psb_mmu_pt *pt,
+                                   unsigned long addr, uint32_t pte)
+{
+        pt->v[psb_mmu_pt_index(addr)] = pte;
+}
+
+static inline void psb_mmu_invalidate_pte(struct psb_mmu_pt *pt,
+                                          unsigned long addr)
+{
+        pt->v[psb_mmu_pt_index(addr)] = pt->pd->invalid_pte;
+}
+
+
+void psb_mmu_mirror_gtt(struct psb_mmu_pd *pd,
+                        uint32_t mmu_offset, uint32_t gtt_start,
+                        uint32_t gtt_pages)
+{
+        uint32_t *v;
+        uint32_t start = psb_mmu_pd_index(mmu_offset);
+        struct psb_mmu_driver *driver = pd->driver;
+        int num_pages = gtt_pages;
+
+        down_read(&driver->sem);
+        spin_lock(&driver->lock);
+
+        v = kmap_atomic(pd->p, KM_USER0);
+        v += start;
+
+        while (gtt_pages--) {
+                *v++ = gtt_start | pd->pd_mask;
+                gtt_start += PAGE_SIZE;
+        }
+
+        /*ttm_tt_cache_flush(&pd->p, num_pages);*/
+        psb_pages_clflush(pd->driver, &pd->p, num_pages);
+        kunmap_atomic(v, KM_USER0);
+        spin_unlock(&driver->lock);
+
+        if (pd->hw_context != -1)
+                atomic_set(&pd->driver->needs_tlbflush, 1);
+
+        up_read(&pd->driver->sem);
+        psb_mmu_flush_pd(pd->driver, 0);
+}
+
+struct psb_mmu_pd *psb_mmu_get_default_pd(struct psb_mmu_driver *driver)
+{
+        struct psb_mmu_pd *pd;
+
+        /* down_read(&driver->sem); */
+        pd = driver->default_pd;
+        /* up_read(&driver->sem); */
+
+        return pd;
+}
+
+/* Returns the physical address of the PD shared by sgx/msvdx */
+uint32_t psb_get_default_pd_addr(struct psb_mmu_driver *driver)
+{
+        struct psb_mmu_pd *pd;
+
+        pd = psb_mmu_get_default_pd(driver);
+        return page_to_pfn(pd->p) << PAGE_SHIFT;
+}
+
+void psb_mmu_driver_takedown(struct psb_mmu_driver *driver)
+{
+        psb_mmu_free_pagedir(driver->default_pd);
+        kfree(driver);
+}
+
+struct psb_mmu_driver *psb_mmu_driver_init(uint8_t __iomem * registers,
+                                        int trap_pagefaults,
+                                        int invalid_type,
+                                        struct drm_psb_private *dev_priv)
+{
+        struct psb_mmu_driver *driver;
+
+        driver = kmalloc(sizeof(*driver), GFP_KERNEL);
+
+        if (!driver)
+                return NULL;
+        driver->dev_priv = dev_priv;
+
+        driver->default_pd = psb_mmu_alloc_pd(driver, trap_pagefaults,
+                                              invalid_type);
+        if (!driver->default_pd)
+                goto out_err1;
+
+        spin_lock_init(&driver->lock);
+        init_rwsem(&driver->sem);
+        down_write(&driver->sem);
+        driver->register_map = registers;
+        atomic_set(&driver->needs_tlbflush, 1);
+
+        driver->has_clflush = 0;
+
+        if (boot_cpu_has(X86_FEATURE_CLFLSH)) {
+                uint32_t tfms, misc, cap0, cap4, clflush_size;
+
+                /*
+                 * clflush size is determined at kernel setup for x86_64
+                 *  but not for i386. We have to do it here.
+                 */
+
+                cpuid(0x00000001, &tfms, &misc, &cap0, &cap4);
+                clflush_size = ((misc >> 8) & 0xff) * 8;
+                driver->has_clflush = 1;
+                driver->clflush_add =
+                    PAGE_SIZE * clflush_size / sizeof(uint32_t);
+                driver->clflush_mask = driver->clflush_add - 1;
+                driver->clflush_mask = ~driver->clflush_mask;
+        }
+
+        up_write(&driver->sem);
+        return driver;
+
+out_err1:
+        kfree(driver);
+        return NULL;
+}
+
+static void psb_mmu_flush_ptes(struct psb_mmu_pd *pd,
+                               unsigned long address, uint32_t num_pages,
+                               uint32_t desired_tile_stride,
+                               uint32_t hw_tile_stride)
+{
+        struct psb_mmu_pt *pt;
+        uint32_t rows = 1;
+        uint32_t i;
+        unsigned long addr;
+        unsigned long end;
+        unsigned long next;
+        unsigned long add;
+        unsigned long row_add;
+        unsigned long clflush_add = pd->driver->clflush_add;
+        unsigned long clflush_mask = pd->driver->clflush_mask;
+
+        if (!pd->driver->has_clflush) {
+                /*ttm_tt_cache_flush(&pd->p, num_pages);*/
+                psb_pages_clflush(pd->driver, &pd->p, num_pages);
+                return;
+        }
+
+        if (hw_tile_stride)
+                rows = num_pages / desired_tile_stride;
+        else
+                desired_tile_stride = num_pages;
+
+        add = desired_tile_stride << PAGE_SHIFT;
+        row_add = hw_tile_stride << PAGE_SHIFT;
+        mb();
+        for (i = 0; i < rows; ++i) {
+
+                addr = address;
+                end = addr + add;
+
+                do {
+                        next = psb_pd_addr_end(addr, end);
+                        pt = psb_mmu_pt_map_lock(pd, addr);
+                        if (!pt)
+                                continue;
+                        do {
+                                psb_clflush(&pt->v
+                                            [psb_mmu_pt_index(addr)]);
+                        } while (addr +=
+                                 clflush_add,
+                                 (addr & clflush_mask) < next);
+
+                        psb_mmu_pt_unmap_unlock(pt);
+                } while (addr = next, next != end);
+                address += row_add;
+        }
+        mb();
+}
+
+void psb_mmu_remove_pfn_sequence(struct psb_mmu_pd *pd,
+                                 unsigned long address, uint32_t num_pages)
+{
+        struct psb_mmu_pt *pt;
+        unsigned long addr;
+        unsigned long end;
+        unsigned long next;
+        unsigned long f_address = address;
+
+        down_read(&pd->driver->sem);
+
+        addr = address;
+        end = addr + (num_pages << PAGE_SHIFT);
+
+        do {
+                next = psb_pd_addr_end(addr, end);
+                pt = psb_mmu_pt_alloc_map_lock(pd, addr);
+                if (!pt)
+                        goto out;
+                do {
+                        psb_mmu_invalidate_pte(pt, addr);
+                        --pt->count;
+                } while (addr += PAGE_SIZE, addr < next);
+                psb_mmu_pt_unmap_unlock(pt);
+
+        } while (addr = next, next != end);
+
+out:
+        if (pd->hw_context != -1)
+                psb_mmu_flush_ptes(pd, f_address, num_pages, 1, 1);
+
+        up_read(&pd->driver->sem);
+
+        if (pd->hw_context != -1)
+                psb_mmu_flush(pd->driver, 0);
+
+        return;
+}
+
+void psb_mmu_remove_pages(struct psb_mmu_pd *pd, unsigned long address,
+                          uint32_t num_pages, uint32_t desired_tile_stride,
+                          uint32_t hw_tile_stride)
+{
+        struct psb_mmu_pt *pt;
+        uint32_t rows = 1;
+        uint32_t i;
+        unsigned long addr;
+        unsigned long end;
+        unsigned long next;
+        unsigned long add;
+        unsigned long row_add;
+        unsigned long f_address = address;
+
+        if (hw_tile_stride)
+                rows = num_pages / desired_tile_stride;
+        else
+                desired_tile_stride = num_pages;
+
+        add = desired_tile_stride << PAGE_SHIFT;
+        row_add = hw_tile_stride << PAGE_SHIFT;
+
+        /* down_read(&pd->driver->sem); */
+
+        /* Make sure we only need to flush this processor's cache */
+
+        for (i = 0; i < rows; ++i) {
+
+                addr = address;
+                end = addr + add;
+
+                do {
+                        next = psb_pd_addr_end(addr, end);
+                        pt = psb_mmu_pt_map_lock(pd, addr);
+                        if (!pt)
+                                continue;
+                        do {
+                                psb_mmu_invalidate_pte(pt, addr);
+                                --pt->count;
+
+                        } while (addr += PAGE_SIZE, addr < next);
+                        psb_mmu_pt_unmap_unlock(pt);
+
+                } while (addr = next, next != end);
+                address += row_add;
+        }
+        if (pd->hw_context != -1)
+                psb_mmu_flush_ptes(pd, f_address, num_pages,
+                                   desired_tile_stride, hw_tile_stride);
+
+        /* up_read(&pd->driver->sem); */
+
+        if (pd->hw_context != -1)
+                psb_mmu_flush(pd->driver, 0);
+}
+
+int psb_mmu_insert_pfn_sequence(struct psb_mmu_pd *pd, uint32_t start_pfn,
+                                unsigned long address, uint32_t num_pages,
+                                int type)
+{
+        struct psb_mmu_pt *pt;
+        uint32_t pte;
+        unsigned long addr;
+        unsigned long end;
+        unsigned long next;
+        unsigned long f_address = address;
+        int ret = 0;
+
+        down_read(&pd->driver->sem);
+
+        addr = address;
+        end = addr + (num_pages << PAGE_SHIFT);
+
+        do {
+                next = psb_pd_addr_end(addr, end);
+                pt = psb_mmu_pt_alloc_map_lock(pd, addr);
+                if (!pt) {
+                        ret = -ENOMEM;
+                        goto out;
+                }
+                do {
+                        pte = psb_mmu_mask_pte(start_pfn++, type);
+                        psb_mmu_set_pte(pt, addr, pte);
+                        pt->count++;
+                } while (addr += PAGE_SIZE, addr < next);
+                psb_mmu_pt_unmap_unlock(pt);
+
+        } while (addr = next, next != end);
+
+out:
+        if (pd->hw_context != -1)
+                psb_mmu_flush_ptes(pd, f_address, num_pages, 1, 1);
+
+        up_read(&pd->driver->sem);
+
+        if (pd->hw_context != -1)
+                psb_mmu_flush(pd->driver, 1);
+
+        return ret;
+}
+
+int psb_mmu_insert_pages(struct psb_mmu_pd *pd, struct page **pages,
+                         unsigned long address, uint32_t num_pages,
+                         uint32_t desired_tile_stride,
+                         uint32_t hw_tile_stride, int type)
+{
+        struct psb_mmu_pt *pt;
+        uint32_t rows = 1;
+        uint32_t i;
+        uint32_t pte;
+        unsigned long addr;
+        unsigned long end;
+        unsigned long next;
+        unsigned long add;
+        unsigned long row_add;
+        unsigned long f_address = address;
+        int ret = 0;
+
+        if (hw_tile_stride) {
+                if (num_pages % desired_tile_stride != 0)
+                        return -EINVAL;
+                rows = num_pages / desired_tile_stride;
+        } else {
+                desired_tile_stride = num_pages;
+        }
+
+        add = desired_tile_stride << PAGE_SHIFT;
+        row_add = hw_tile_stride << PAGE_SHIFT;
+
+        down_read(&pd->driver->sem);
+
+        for (i = 0; i < rows; ++i) {
+
+                addr = address;
+                end = addr + add;
+
+                do {
+                        next = psb_pd_addr_end(addr, end);
+                        pt = psb_mmu_pt_alloc_map_lock(pd, addr);
+                        if (!pt) {
+                                ret = -ENOMEM;
+                                goto out;
+                        }
+                        do {
+                                pte =
+                                    psb_mmu_mask_pte(page_to_pfn(*pages++),
+                                                     type);
+                                psb_mmu_set_pte(pt, addr, pte);
+                                pt->count++;
+                        } while (addr += PAGE_SIZE, addr < next);
+                        psb_mmu_pt_unmap_unlock(pt);
+
+                } while (addr = next, next != end);
+
+                address += row_add;
+        }
+out:
+        if (pd->hw_context != -1)
+                psb_mmu_flush_ptes(pd, f_address, num_pages,
+                                   desired_tile_stride, hw_tile_stride);
+
+        up_read(&pd->driver->sem);
+
+        if (pd->hw_context != -1)
+                psb_mmu_flush(pd->driver, 1);
+
+        return ret;
+}
+
+int psb_mmu_virtual_to_pfn(struct psb_mmu_pd *pd, uint32_t virtual,
+                           unsigned long *pfn)
+{
+        int ret;
+        struct psb_mmu_pt *pt;
+        uint32_t tmp;
+        spinlock_t *lock = &pd->driver->lock;
+
+        down_read(&pd->driver->sem);
+        pt = psb_mmu_pt_map_lock(pd, virtual);
+        if (!pt) {
+                uint32_t *v;
+
+                spin_lock(lock);
+                v = kmap_atomic(pd->p, KM_USER0);
+                tmp = v[psb_mmu_pd_index(virtual)];
+                kunmap_atomic(v, KM_USER0);
+                spin_unlock(lock);
+
+                if (tmp != pd->invalid_pde || !(tmp & PSB_PTE_VALID) ||
+                    !(pd->invalid_pte & PSB_PTE_VALID)) {
+                        ret = -EINVAL;
+                        goto out;
+                }
+                ret = 0;
+                *pfn = pd->invalid_pte >> PAGE_SHIFT;
+                goto out;
+        }
+        tmp = pt->v[psb_mmu_pt_index(virtual)];
+        if (!(tmp & PSB_PTE_VALID)) {
+                ret = -EINVAL;
+        } else {
+                ret = 0;
+                *pfn = tmp >> PAGE_SHIFT;
+        }
+        psb_mmu_pt_unmap_unlock(pt);
+out:
+        up_read(&pd->driver->sem);
+        return ret;
+}
diff --git a/drivers/staging/gma500/mrst.h b/drivers/staging/gma500/mrst.h
new file mode 100644
index 00000000000..b563dbc7310
--- /dev/null
+++ b/drivers/staging/gma500/mrst.h
@@ -0,0 +1,252 @@
+/**************************************************************************
+ * Copyright (c) 2007-2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+/* MID device specific descriptors */
+
+struct mrst_vbt {
+        s8 signature[4];        /*4 bytes,"$GCT" */
+        u8 revision;
+        u8 size;
+        u8 checksum;
+        void *mrst_gct;
+} __packed;
+
+struct mrst_timing_info {
+        u16 pixel_clock;
+        u8 hactive_lo;
+        u8 hblank_lo;
+        u8 hblank_hi:4;
+        u8 hactive_hi:4;
+        u8 vactive_lo;
+        u8 vblank_lo;
+        u8 vblank_hi:4;
+        u8 vactive_hi:4;
+        u8 hsync_offset_lo;
+        u8 hsync_pulse_width_lo;
+        u8 vsync_pulse_width_lo:4;
+        u8 vsync_offset_lo:4;
+        u8 vsync_pulse_width_hi:2;
+        u8 vsync_offset_hi:2;
+        u8 hsync_pulse_width_hi:2;
+        u8 hsync_offset_hi:2;
+        u8 width_mm_lo;
+        u8 height_mm_lo;
+        u8 height_mm_hi:4;
+        u8 width_mm_hi:4;
+        u8 hborder;
+        u8 vborder;
+        u8 unknown0:1;
+        u8 hsync_positive:1;
+        u8 vsync_positive:1;
+        u8 separate_sync:2;
+        u8 stereo:1;
+        u8 unknown6:1;
+        u8 interlaced:1;
+} __packed;
+
+struct gct_r10_timing_info {
+        u16 pixel_clock;
+        u32 hactive_lo:8;
+        u32 hactive_hi:4;
+        u32 hblank_lo:8;
+        u32 hblank_hi:4;
+        u32 hsync_offset_lo:8;
+        u16 hsync_offset_hi:2;
+        u16 hsync_pulse_width_lo:8;
+        u16 hsync_pulse_width_hi:2;
+        u16 hsync_positive:1;
+        u16 rsvd_1:3;
+        u8  vactive_lo:8;
+        u16 vactive_hi:4;
+        u16 vblank_lo:8;
+        u16 vblank_hi:4;
+        u16 vsync_offset_lo:4;
+        u16 vsync_offset_hi:2;
+        u16 vsync_pulse_width_lo:4;
+        u16 vsync_pulse_width_hi:2;
+        u16 vsync_positive:1;
+        u16 rsvd_2:3;
+} __packed;
+
+struct mrst_panel_descriptor_v1 {
+        u32 Panel_Port_Control; /* 1 dword, Register 0x61180 if LVDS */
+                                /* 0x61190 if MIPI */
+        u32 Panel_Power_On_Sequencing;/*1 dword,Register 0x61208,*/
+        u32 Panel_Power_Off_Sequencing;/*1 dword,Register 0x6120C,*/
+        u32 Panel_Power_Cycle_Delay_and_Reference_Divisor;/* 1 dword */
+                                                /* Register 0x61210 */
+        struct mrst_timing_info DTD;/*18 bytes, Standard definition */
+        u16 Panel_Backlight_Inverter_Descriptor;/* 16 bits, as follows */
+                                /* Bit 0, Frequency, 15 bits,0 - 32767Hz */
+                        /* Bit 15, Polarity, 1 bit, 0: Normal, 1: Inverted */
+        u16 Panel_MIPI_Display_Descriptor;
+                        /*16 bits, Defined as follows: */
+                        /* if MIPI, 0x0000 if LVDS */
+                        /* Bit 0, Type, 2 bits, */
+                        /* 0: Type-1, */
+                        /* 1: Type-2, */
+                        /* 2: Type-3, */
+                        /* 3: Type-4 */
+                        /* Bit 2, Pixel Format, 4 bits */
+                        /* Bit0: 16bpp (not supported in LNC), */
+                        /* Bit1: 18bpp loosely packed, */
+                        /* Bit2: 18bpp packed, */
+                        /* Bit3: 24bpp */
+                        /* Bit 6, Reserved, 2 bits, 00b */
+                        /* Bit 8, Minimum Supported Frame Rate, 6 bits, 0 - 63Hz */
+                        /* Bit 14, Reserved, 2 bits, 00b */
+} __packed;
+
+struct mrst_panel_descriptor_v2 {
+        u32 Panel_Port_Control; /* 1 dword, Register 0x61180 if LVDS */
+                                /* 0x61190 if MIPI */
+        u32 Panel_Power_On_Sequencing;/*1 dword,Register 0x61208,*/
+        u32 Panel_Power_Off_Sequencing;/*1 dword,Register 0x6120C,*/
+        u8 Panel_Power_Cycle_Delay_and_Reference_Divisor;/* 1 byte */
+                                                /* Register 0x61210 */
+        struct mrst_timing_info DTD;/*18 bytes, Standard definition */
+        u16 Panel_Backlight_Inverter_Descriptor;/*16 bits, as follows*/
+                                /*Bit 0, Frequency, 16 bits, 0 - 32767Hz*/
+        u8 Panel_Initial_Brightness;/* [7:0] 0 - 100% */
+                        /*Bit 7, Polarity, 1 bit,0: Normal, 1: Inverted*/
+        u16 Panel_MIPI_Display_Descriptor;
+                        /*16 bits, Defined as follows: */
+                        /* if MIPI, 0x0000 if LVDS */
+                        /* Bit 0, Type, 2 bits, */
+                        /* 0: Type-1, */
+                        /* 1: Type-2, */
+                        /* 2: Type-3, */
+                        /* 3: Type-4 */
+                        /* Bit 2, Pixel Format, 4 bits */
+                        /* Bit0: 16bpp (not supported in LNC), */
+                        /* Bit1: 18bpp loosely packed, */
+                        /* Bit2: 18bpp packed, */
+                        /* Bit3: 24bpp */
+                        /* Bit 6, Reserved, 2 bits, 00b */
+                        /* Bit 8, Minimum Supported Frame Rate, 6 bits, 0 - 63Hz */
+                        /* Bit 14, Reserved, 2 bits, 00b */
+} __packed;
+
+union mrst_panel_rx {
+        struct {
+                u16 NumberOfLanes:2; /*Num of Lanes, 2 bits,0 = 1 lane,*/
+                        /* 1 = 2 lanes, 2 = 3 lanes, 3 = 4 lanes. */
+                u16 MaxLaneFreq:3; /* 0: 100MHz, 1: 200MHz, 2: 300MHz, */
+                /*3: 400MHz, 4: 500MHz, 5: 600MHz, 6: 700MHz, 7: 800MHz.*/
+                u16 SupportedVideoTransferMode:2; /*0: Non-burst only */
+                                        /* 1: Burst and non-burst */
+                                        /* 2/3: Reserved */
+                u16 HSClkBehavior:1; /*0: Continuous, 1: Non-continuous*/
+                u16 DuoDisplaySupport:1; /*1 bit,0: No, 1: Yes*/
+                u16 ECC_ChecksumCapabilities:1;/*1 bit,0: No, 1: Yes*/
+                u16 BidirectionalCommunication:1;/*1 bit,0: No, 1: Yes */
+                u16 Rsvd:5;/*5 bits,00000b */
+        } panelrx;
+        u16 panel_receiver;
+} __packed;
+
+struct mrst_gct_v1 {
+        union { /*8 bits,Defined as follows: */
+                struct {
+                        u8 PanelType:4; /*4 bits, Bit field for panels*/
+                                        /* 0 - 3: 0 = LVDS, 1 = MIPI*/
+                                        /*2 bits,Specifies which of the*/
+                        u8 BootPanelIndex:2;
+                                        /* 4 panels to use by default*/
+                        u8 BootMIPI_DSI_RxIndex:2;/*Specifies which of*/
+                                        /* the 4 MIPI DSI receivers to use*/
+                } PD;
+                u8 PanelDescriptor;
+        };
+        struct mrst_panel_descriptor_v1 panel[4];/*panel descrs,38 bytes each*/
+        union mrst_panel_rx panelrx[4]; /* panel receivers*/
+} __packed;
+
+struct mrst_gct_v2 {
+        union { /*8 bits,Defined as follows: */
+                struct {
+                        u8 PanelType:4; /*4 bits, Bit field for panels*/
+                                        /* 0 - 3: 0 = LVDS, 1 = MIPI*/
+                                        /*2 bits,Specifies which of the*/
+                        u8 BootPanelIndex:2;
+                                        /* 4 panels to use by default*/
+                        u8 BootMIPI_DSI_RxIndex:2;/*Specifies which of*/
+                                        /* the 4 MIPI DSI receivers to use*/
+                } PD;
+                u8 PanelDescriptor;
+        };
+        struct mrst_panel_descriptor_v2 panel[4];/*panel descrs,38 bytes each*/
+        union mrst_panel_rx panelrx[4]; /* panel receivers*/
+} __packed;
+
+struct mrst_gct_data {
+        u8 bpi; /* boot panel index, number of panel used during boot */
+        u8 pt; /* panel type, 4 bit field, 0=lvds, 1=mipi */
+        struct mrst_timing_info DTD; /* timing info for the selected panel */
+        u32 Panel_Port_Control;
+        u32 PP_On_Sequencing;/*1 dword,Register 0x61208,*/
+        u32 PP_Off_Sequencing;/*1 dword,Register 0x6120C,*/
+        u32 PP_Cycle_Delay;
+        u16 Panel_Backlight_Inverter_Descriptor;
+        u16 Panel_MIPI_Display_Descriptor;
+} __packed;
+
+#define MODE_SETTING_IN_CRTC            0x1
+#define MODE_SETTING_IN_ENCODER         0x2
+#define MODE_SETTING_ON_GOING           0x3
+#define MODE_SETTING_IN_DSR             0x4
+#define MODE_SETTING_ENCODER_DONE       0x8
+
+#define GCT_R10_HEADER_SIZE             16
+#define GCT_R10_DISPLAY_DESC_SIZE       28
+
+/*
+ *      Moorestown HDMI interfaces
+ */
+
+struct mrst_hdmi_dev {
+        struct pci_dev *dev;
+        void __iomem *regs;
+        unsigned int mmio, mmio_len;
+        int dpms_mode;
+        struct hdmi_i2c_dev *i2c_dev;
+
+        /* register state */
+        u32 saveDPLL_CTRL;
+        u32 saveDPLL_DIV_CTRL;
+        u32 saveDPLL_ADJUST;
+        u32 saveDPLL_UPDATE;
+        u32 saveDPLL_CLK_ENABLE;
+        u32 savePCH_HTOTAL_B;
+        u32 savePCH_HBLANK_B;
+        u32 savePCH_HSYNC_B;
+        u32 savePCH_VTOTAL_B;
+        u32 savePCH_VBLANK_B;
+        u32 savePCH_VSYNC_B;
+        u32 savePCH_PIPEBCONF;
+        u32 savePCH_PIPEBSRC;
+};
+
+extern void mrst_hdmi_setup(struct drm_device *dev);
+extern void mrst_hdmi_teardown(struct drm_device *dev);
+extern int  mrst_hdmi_i2c_init(struct pci_dev *dev);
+extern void mrst_hdmi_i2c_exit(struct pci_dev *dev);
+extern void mrst_hdmi_save(struct drm_device *dev);
+extern void mrst_hdmi_restore(struct drm_device *dev);
+extern void mrst_hdmi_init(struct drm_device *dev, struct psb_intel_mode_device *mode_dev);
diff --git a/drivers/staging/gma500/mrst_crtc.c b/drivers/staging/gma500/mrst_crtc.c
new file mode 100644
index 00000000000..72464dd0f23
--- /dev/null
+++ b/drivers/staging/gma500/mrst_crtc.c
@@ -0,0 +1,610 @@
+/*
+ * Copyright © 2009 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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/i2c.h>
+#include <linux/pm_runtime.h>
+
+#include <drm/drmP.h>
+#include "framebuffer.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_intel_display.h"
+#include "power.h"
+
+struct psb_intel_range_t {
+        int min, max;
+};
+
+struct mrst_limit_t {
+        struct psb_intel_range_t dot, m, p1;
+};
+
+struct mrst_clock_t {
+        /* derived values */
+        int dot;
+        int m;
+        int p1;
+};
+
+#define MRST_LIMIT_LVDS_100L        0
+#define MRST_LIMIT_LVDS_83          1
+#define MRST_LIMIT_LVDS_100         2
+
+#define MRST_DOT_MIN              19750
+#define MRST_DOT_MAX              120000
+#define MRST_M_MIN_100L             20
+#define MRST_M_MIN_100              10
+#define MRST_M_MIN_83               12
+#define MRST_M_MAX_100L             34
+#define MRST_M_MAX_100              17
+#define MRST_M_MAX_83               20
+#define MRST_P1_MIN                 2
+#define MRST_P1_MAX_0               7
+#define MRST_P1_MAX_1               8
+
+static const struct mrst_limit_t mrst_limits[] = {
+        {                       /* MRST_LIMIT_LVDS_100L */
+         .dot = {.min = MRST_DOT_MIN, .max = MRST_DOT_MAX},
+         .m = {.min = MRST_M_MIN_100L, .max = MRST_M_MAX_100L},
+         .p1 = {.min = MRST_P1_MIN, .max = MRST_P1_MAX_1},
+         },
+        {                       /* MRST_LIMIT_LVDS_83L */
+         .dot = {.min = MRST_DOT_MIN, .max = MRST_DOT_MAX},
+         .m = {.min = MRST_M_MIN_83, .max = MRST_M_MAX_83},
+         .p1 = {.min = MRST_P1_MIN, .max = MRST_P1_MAX_0},
+         },
+        {                       /* MRST_LIMIT_LVDS_100 */
+         .dot = {.min = MRST_DOT_MIN, .max = MRST_DOT_MAX},
+         .m = {.min = MRST_M_MIN_100, .max = MRST_M_MAX_100},
+         .p1 = {.min = MRST_P1_MIN, .max = MRST_P1_MAX_1},
+         },
+};
+
+#define MRST_M_MIN          10
+static const u32 mrst_m_converts[] = {
+        0x2B, 0x15, 0x2A, 0x35, 0x1A, 0x0D, 0x26, 0x33, 0x19, 0x2C,
+        0x36, 0x3B, 0x1D, 0x2E, 0x37, 0x1B, 0x2D, 0x16, 0x0B, 0x25,
+        0x12, 0x09, 0x24, 0x32, 0x39, 0x1c,
+};
+
+static const struct mrst_limit_t *mrst_limit(struct drm_crtc *crtc)
+{
+        const struct mrst_limit_t *limit = NULL;
+        struct drm_device *dev = crtc->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (psb_intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS)
+            || psb_intel_pipe_has_type(crtc, INTEL_OUTPUT_MIPI)) {
+                switch (dev_priv->core_freq) {
+                case 100:
+                        limit = &mrst_limits[MRST_LIMIT_LVDS_100L];
+                        break;
+                case 166:
+                        limit = &mrst_limits[MRST_LIMIT_LVDS_83];
+                        break;
+                case 200:
+                        limit = &mrst_limits[MRST_LIMIT_LVDS_100];
+                        break;
+                }
+        } else {
+                limit = NULL;
+                dev_err(dev->dev, "mrst_limit Wrong display type.\n");
+        }
+
+        return limit;
+}
+
+/** Derive the pixel clock for the given refclk and divisors for 8xx chips. */
+static void mrst_clock(int refclk, struct mrst_clock_t *clock)
+{
+        clock->dot = (refclk * clock->m) / (14 * clock->p1);
+}
+
+void mrstPrintPll(char *prefix, struct mrst_clock_t *clock)
+{
+        pr_debug("%s: dotclock = %d,  m = %d, p1 = %d.\n",
+             prefix, clock->dot, clock->m, clock->p1);
+}
+
+/**
+ * Returns a set of divisors for the desired target clock with the given refclk,
+ * or FALSE.  Divisor values are the actual divisors for
+ */
+static bool
+mrstFindBestPLL(struct drm_crtc *crtc, int target, int refclk,
+                struct mrst_clock_t *best_clock)
+{
+        struct mrst_clock_t clock;
+        const struct mrst_limit_t *limit = mrst_limit(crtc);
+        int err = target;
+
+        memset(best_clock, 0, sizeof(*best_clock));
+
+        for (clock.m = limit->m.min; clock.m <= limit->m.max; clock.m++) {
+                for (clock.p1 = limit->p1.min; clock.p1 <= limit->p1.max;
+                     clock.p1++) {
+                        int this_err;
+
+                        mrst_clock(refclk, &clock);
+
+                        this_err = abs(clock.dot - target);
+                        if (this_err < err) {
+                                *best_clock = clock;
+                                err = this_err;
+                        }
+                }
+        }
+        dev_dbg(crtc->dev->dev, "mrstFindBestPLL err = %d.\n", err);
+        return err != target;
+}
+
+/**
+ * Sets the power management mode of the pipe and plane.
+ *
+ * This code should probably grow support for turning the cursor off and back
+ * on appropriately at the same time as we're turning the pipe off/on.
+ */
+static void mrst_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        int dpll_reg = (pipe == 0) ? MRST_DPLL_A : DPLL_B;
+        int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+        int dspbase_reg = (pipe == 0) ? MRST_DSPABASE : DSPBBASE;
+        int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+        u32 temp;
+        bool enabled;
+
+        if (!gma_power_begin(dev, true))
+                return;
+
+        /* XXX: When our outputs are all unaware of DPMS modes other than off
+         * and on, we should map those modes to DRM_MODE_DPMS_OFF in the CRTC.
+         */
+        switch (mode) {
+        case DRM_MODE_DPMS_ON:
+        case DRM_MODE_DPMS_STANDBY:
+        case DRM_MODE_DPMS_SUSPEND:
+                /* Enable the DPLL */
+                temp = REG_READ(dpll_reg);
+                if ((temp & DPLL_VCO_ENABLE) == 0) {
+                        REG_WRITE(dpll_reg, temp);
+                        REG_READ(dpll_reg);
+                        /* Wait for the clocks to stabilize. */
+                        udelay(150);
+                        REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+                        REG_READ(dpll_reg);
+                        /* Wait for the clocks to stabilize. */
+                        udelay(150);
+                        REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+                        REG_READ(dpll_reg);
+                        /* Wait for the clocks to stabilize. */
+                        udelay(150);
+                }
+                /* Enable the pipe */
+                temp = REG_READ(pipeconf_reg);
+                if ((temp & PIPEACONF_ENABLE) == 0)
+                        REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
+                /* Enable the plane */
+                temp = REG_READ(dspcntr_reg);
+                if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
+                        REG_WRITE(dspcntr_reg,
+                                  temp | DISPLAY_PLANE_ENABLE);
+                        /* Flush the plane changes */
+                        REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+                }
+
+                psb_intel_crtc_load_lut(crtc);
+
+                /* Give the overlay scaler a chance to enable
+                   if it's on this pipe */
+                /* psb_intel_crtc_dpms_video(crtc, true); TODO */
+                break;
+        case DRM_MODE_DPMS_OFF:
+                /* Give the overlay scaler a chance to disable
+                 * if it's on this pipe */
+                /* psb_intel_crtc_dpms_video(crtc, FALSE); TODO */
+
+                /* Disable the VGA plane that we never use */
+                REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+                /* Disable display plane */
+                temp = REG_READ(dspcntr_reg);
+                if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
+                        REG_WRITE(dspcntr_reg,
+                                  temp & ~DISPLAY_PLANE_ENABLE);
+                        /* Flush the plane changes */
+                        REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+                        REG_READ(dspbase_reg);
+                }
+
+                /* Next, disable display pipes */
+                temp = REG_READ(pipeconf_reg);
+                if ((temp & PIPEACONF_ENABLE) != 0) {
+                        REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
+                        REG_READ(pipeconf_reg);
+                }
+                /* Wait for for the pipe disable to take effect. */
+                psb_intel_wait_for_vblank(dev);
+
+                temp = REG_READ(dpll_reg);
+                if ((temp & DPLL_VCO_ENABLE) != 0) {
+                        REG_WRITE(dpll_reg, temp & ~DPLL_VCO_ENABLE);
+                        REG_READ(dpll_reg);
+                }
+
+                /* Wait for the clocks to turn off. */
+                udelay(150);
+                break;
+        }
+
+        enabled = crtc->enabled && mode != DRM_MODE_DPMS_OFF;
+
+        /*Set FIFO Watermarks*/
+        REG_WRITE(DSPARB, 0x3FFF);
+        REG_WRITE(DSPFW1, 0x3F88080A);
+        REG_WRITE(DSPFW2, 0x0b060808);
+        REG_WRITE(DSPFW3, 0x0);
+        REG_WRITE(DSPFW4, 0x08030404);
+        REG_WRITE(DSPFW5, 0x04040404);
+        REG_WRITE(DSPFW6, 0x78);
+        REG_WRITE(0x70400, REG_READ(0x70400) | 0x4000);
+        /* Must write Bit 14 of the Chicken Bit Register */
+
+        gma_power_end(dev);
+}
+
+/**
+ * Return the pipe currently connected to the panel fitter,
+ * or -1 if the panel fitter is not present or not in use
+ */
+static int mrst_panel_fitter_pipe(struct drm_device *dev)
+{
+        u32 pfit_control;
+
+        pfit_control = REG_READ(PFIT_CONTROL);
+
+        /* See if the panel fitter is in use */
+        if ((pfit_control & PFIT_ENABLE) == 0)
+                return -1;
+        return (pfit_control >> 29) & 3;
+}
+
+static int mrst_crtc_mode_set(struct drm_crtc *crtc,
+                              struct drm_display_mode *mode,
+                              struct drm_display_mode *adjusted_mode,
+                              int x, int y,
+                              struct drm_framebuffer *old_fb)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int pipe = psb_intel_crtc->pipe;
+        int fp_reg = (pipe == 0) ? MRST_FPA0 : FPB0;
+        int dpll_reg = (pipe == 0) ? MRST_DPLL_A : DPLL_B;
+        int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+        int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+        int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
+        int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
+        int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
+        int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
+        int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
+        int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
+        int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+        int refclk = 0;
+        struct mrst_clock_t clock;
+        u32 dpll = 0, fp = 0, dspcntr, pipeconf;
+        bool ok, is_sdvo = false;
+        bool is_crt = false, is_lvds = false, is_tv = false;
+        bool is_mipi = false;
+        struct drm_mode_config *mode_config = &dev->mode_config;
+        struct psb_intel_output *psb_intel_output = NULL;
+        uint64_t scalingType = DRM_MODE_SCALE_FULLSCREEN;
+        struct drm_encoder *encoder;
+
+        if (!gma_power_begin(dev, true))
+                return 0;
+
+        memcpy(&psb_intel_crtc->saved_mode,
+                mode,
+                sizeof(struct drm_display_mode));
+        memcpy(&psb_intel_crtc->saved_adjusted_mode,
+                adjusted_mode,
+                sizeof(struct drm_display_mode));
+
+        list_for_each_entry(encoder, &mode_config->encoder_list, head) {
+
+                if (encoder->crtc != crtc)
+                        continue;
+
+                psb_intel_output = enc_to_psb_intel_output(encoder);
+                switch (psb_intel_output->type) {
+                case INTEL_OUTPUT_LVDS:
+                        is_lvds = true;
+                        break;
+                case INTEL_OUTPUT_SDVO:
+                        is_sdvo = true;
+                        break;
+                case INTEL_OUTPUT_TVOUT:
+                        is_tv = true;
+                        break;
+                case INTEL_OUTPUT_ANALOG:
+                        is_crt = true;
+                        break;
+                case INTEL_OUTPUT_MIPI:
+                        is_mipi = true;
+                        break;
+                }
+        }
+
+        /* Disable the VGA plane that we never use */
+        REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+
+        /* Disable the panel fitter if it was on our pipe */
+        if (mrst_panel_fitter_pipe(dev) == pipe)
+                REG_WRITE(PFIT_CONTROL, 0);
+
+        REG_WRITE(pipesrc_reg,
+                  ((mode->crtc_hdisplay - 1) << 16) |
+                  (mode->crtc_vdisplay - 1));
+
+        if (psb_intel_output)
+                drm_connector_property_get_value(&psb_intel_output->base,
+                        dev->mode_config.scaling_mode_property, &scalingType);
+
+        if (scalingType == DRM_MODE_SCALE_NO_SCALE) {
+                /* Moorestown doesn't have register support for centering so
+                 * we need to mess with the h/vblank and h/vsync start and
+                 * ends to get centering */
+                int offsetX = 0, offsetY = 0;
+
+                offsetX = (adjusted_mode->crtc_hdisplay -
+                           mode->crtc_hdisplay) / 2;
+                offsetY = (adjusted_mode->crtc_vdisplay -
+                           mode->crtc_vdisplay) / 2;
+
+                REG_WRITE(htot_reg, (mode->crtc_hdisplay - 1) |
+                        ((adjusted_mode->crtc_htotal - 1) << 16));
+                REG_WRITE(vtot_reg, (mode->crtc_vdisplay - 1) |
+                        ((adjusted_mode->crtc_vtotal - 1) << 16));
+                REG_WRITE(hblank_reg,
+                        (adjusted_mode->crtc_hblank_start - offsetX - 1) |
+                        ((adjusted_mode->crtc_hblank_end - offsetX - 1) << 16));
+                REG_WRITE(hsync_reg,
+                        (adjusted_mode->crtc_hsync_start - offsetX - 1) |
+                        ((adjusted_mode->crtc_hsync_end - offsetX - 1) << 16));
+                REG_WRITE(vblank_reg,
+                        (adjusted_mode->crtc_vblank_start - offsetY - 1) |
+                        ((adjusted_mode->crtc_vblank_end - offsetY - 1) << 16));
+                REG_WRITE(vsync_reg,
+                        (adjusted_mode->crtc_vsync_start - offsetY - 1) |
+                        ((adjusted_mode->crtc_vsync_end - offsetY - 1) << 16));
+        } else {
+                REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+                        ((adjusted_mode->crtc_htotal - 1) << 16));
+                REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+                        ((adjusted_mode->crtc_vtotal - 1) << 16));
+                REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+                        ((adjusted_mode->crtc_hblank_end - 1) << 16));
+                REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+                        ((adjusted_mode->crtc_hsync_end - 1) << 16));
+                REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+                        ((adjusted_mode->crtc_vblank_end - 1) << 16));
+                REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+                        ((adjusted_mode->crtc_vsync_end - 1) << 16));
+        }
+
+        /* Flush the plane changes */
+        {
+                struct drm_crtc_helper_funcs *crtc_funcs =
+                    crtc->helper_private;
+                crtc_funcs->mode_set_base(crtc, x, y, old_fb);
+        }
+
+        /* setup pipeconf */
+        pipeconf = REG_READ(pipeconf_reg);
+
+        /* Set up the display plane register */
+        dspcntr = REG_READ(dspcntr_reg);
+        dspcntr |= DISPPLANE_GAMMA_ENABLE;
+
+        if (pipe == 0)
+                dspcntr |= DISPPLANE_SEL_PIPE_A;
+        else
+                dspcntr |= DISPPLANE_SEL_PIPE_B;
+
+        dev_priv->dspcntr = dspcntr |= DISPLAY_PLANE_ENABLE;
+        dev_priv->pipeconf = pipeconf |= PIPEACONF_ENABLE;
+
+        if (is_mipi)
+                goto mrst_crtc_mode_set_exit;
+
+        refclk = dev_priv->core_freq * 1000;
+
+        dpll = 0;               /*BIT16 = 0 for 100MHz reference */
+
+        ok = mrstFindBestPLL(crtc, adjusted_mode->clock, refclk, &clock);
+
+        if (!ok) {
+                dev_dbg(dev->dev, "mrstFindBestPLL fail in mrst_crtc_mode_set.\n");
+        } else {
+                dev_dbg(dev->dev, "mrst_crtc_mode_set pixel clock = %d,"
+                         "m = %x, p1 = %x.\n", clock.dot, clock.m,
+                         clock.p1);
+        }
+
+        fp = mrst_m_converts[(clock.m - MRST_M_MIN)] << 8;
+
+        dpll |= DPLL_VGA_MODE_DIS;
+
+
+        dpll |= DPLL_VCO_ENABLE;
+
+        if (is_lvds)
+                dpll |= DPLLA_MODE_LVDS;
+        else
+                dpll |= DPLLB_MODE_DAC_SERIAL;
+
+        if (is_sdvo) {
+                int sdvo_pixel_multiply =
+                    adjusted_mode->clock / mode->clock;
+
+                dpll |= DPLL_DVO_HIGH_SPEED;
+                dpll |=
+                    (sdvo_pixel_multiply -
+                     1) << SDVO_MULTIPLIER_SHIFT_HIRES;
+        }
+
+
+        /* compute bitmask from p1 value */
+        dpll |= (1 << (clock.p1 - 2)) << 17;
+
+        dpll |= DPLL_VCO_ENABLE;
+
+        mrstPrintPll("chosen", &clock);
+
+        if (dpll & DPLL_VCO_ENABLE) {
+                REG_WRITE(fp_reg, fp);
+                REG_WRITE(dpll_reg, dpll & ~DPLL_VCO_ENABLE);
+                REG_READ(dpll_reg);
+                /* Check the DPLLA lock bit PIPEACONF[29] */
+                udelay(150);
+        }
+
+        REG_WRITE(fp_reg, fp);
+        REG_WRITE(dpll_reg, dpll);
+        REG_READ(dpll_reg);
+        /* Wait for the clocks to stabilize. */
+        udelay(150);
+
+        /* write it again -- the BIOS does, after all */
+        REG_WRITE(dpll_reg, dpll);
+        REG_READ(dpll_reg);
+        /* Wait for the clocks to stabilize. */
+        udelay(150);
+
+        REG_WRITE(pipeconf_reg, pipeconf);
+        REG_READ(pipeconf_reg);
+        psb_intel_wait_for_vblank(dev);
+
+        REG_WRITE(dspcntr_reg, dspcntr);
+        psb_intel_wait_for_vblank(dev);
+
+mrst_crtc_mode_set_exit:
+        gma_power_end(dev);
+        return 0;
+}
+
+static bool mrst_crtc_mode_fixup(struct drm_crtc *crtc,
+                                  struct drm_display_mode *mode,
+                                  struct drm_display_mode *adjusted_mode)
+{
+        return true;
+}
+
+int mrst_pipe_set_base(struct drm_crtc *crtc,
+                            int x, int y, struct drm_framebuffer *old_fb)
+{
+        struct drm_device *dev = crtc->dev;
+        /* struct drm_i915_master_private *master_priv; */
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
+        int pipe = psb_intel_crtc->pipe;
+        unsigned long start, offset;
+        /* FIXME: check if we need this surely MRST is pipe 0 only */
+        int dspbase = (pipe == 0 ? DSPALINOFF : DSPBBASE);
+        int dspsurf = (pipe == 0 ? DSPASURF : DSPBSURF);
+        int dspstride = (pipe == 0) ? DSPASTRIDE : DSPBSTRIDE;
+        int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+        u32 dspcntr;
+        int ret = 0;
+
+        /* no fb bound */
+        if (!crtc->fb) {
+                dev_dbg(dev->dev, "No FB bound\n");
+                return 0;
+        }
+
+        if (!gma_power_begin(dev, true))
+                return 0;
+
+        start = psbfb->gtt->offset;
+        offset = y * crtc->fb->pitch + x * (crtc->fb->bits_per_pixel / 8);
+
+        REG_WRITE(dspstride, crtc->fb->pitch);
+
+        dspcntr = REG_READ(dspcntr_reg);
+        dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
+
+        switch (crtc->fb->bits_per_pixel) {
+        case 8:
+                dspcntr |= DISPPLANE_8BPP;
+                break;
+        case 16:
+                if (crtc->fb->depth == 15)
+                        dspcntr |= DISPPLANE_15_16BPP;
+                else
+                        dspcntr |= DISPPLANE_16BPP;
+                break;
+        case 24:
+        case 32:
+                dspcntr |= DISPPLANE_32BPP_NO_ALPHA;
+                break;
+        default:
+                dev_err(dev->dev, "Unknown color depth\n");
+                ret = -EINVAL;
+                goto pipe_set_base_exit;
+        }
+        REG_WRITE(dspcntr_reg, dspcntr);
+
+        if (0 /* FIXMEAC - check what PSB needs */) {
+                REG_WRITE(dspbase, offset);
+                REG_READ(dspbase);
+                REG_WRITE(dspsurf, start);
+                REG_READ(dspsurf);
+        } else {
+                REG_WRITE(dspbase, start + offset);
+                REG_READ(dspbase);
+        }
+
+pipe_set_base_exit:
+        gma_power_end(dev);
+        return ret;
+}
+
+static void mrst_crtc_prepare(struct drm_crtc *crtc)
+{
+        struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+        crtc_funcs->dpms(crtc, DRM_MODE_DPMS_OFF);
+}
+
+static void mrst_crtc_commit(struct drm_crtc *crtc)
+{
+        struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+        crtc_funcs->dpms(crtc, DRM_MODE_DPMS_ON);
+}
+
+const struct drm_crtc_helper_funcs mrst_helper_funcs = {
+        .dpms = mrst_crtc_dpms,
+        .mode_fixup = mrst_crtc_mode_fixup,
+        .mode_set = mrst_crtc_mode_set,
+        .mode_set_base = mrst_pipe_set_base,
+        .prepare = mrst_crtc_prepare,
+        .commit = mrst_crtc_commit,
+};
+
diff --git a/drivers/staging/gma500/mrst_device.c b/drivers/staging/gma500/mrst_device.c
new file mode 100644
index 00000000000..6707fafbfa1
--- /dev/null
+++ b/drivers/staging/gma500/mrst_device.c
@@ -0,0 +1,634 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+#include <linux/backlight.h>
+#include <linux/module.h>
+#include <linux/dmi.h>
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "psb_drm.h"
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include <asm/mrst.h>
+#include <asm/intel_scu_ipc.h>
+#include "mid_bios.h"
+
+static int devtype;
+
+module_param_named(type, devtype, int, 0600);
+MODULE_PARM_DESC(type, "Moorestown/Oaktrail device type");
+
+#define DEVICE_MOORESTOWN               1
+#define DEVICE_OAKTRAIL                 2
+#define DEVICE_MOORESTOWN_MM            3
+
+static int mrst_device_ident(struct drm_device *dev)
+{
+        /* User forced */
+        if (devtype)
+                return devtype;
+        if (dmi_match(DMI_PRODUCT_NAME, "OakTrail") ||
+                dmi_match(DMI_PRODUCT_NAME, "OakTrail platform"))
+                return DEVICE_OAKTRAIL;
+#if defined(CONFIG_X86_MRST)
+        if (dmi_match(DMI_PRODUCT_NAME, "MM") ||
+                dmi_match(DMI_PRODUCT_NAME, "MM 10"))
+                return DEVICE_MOORESTOWN_MM;
+        if (mrst_identify_cpu())
+                return DEVICE_MOORESTOWN;
+#endif
+        return DEVICE_OAKTRAIL;
+}
+
+
+/* IPC message and command defines used to enable/disable mipi panel voltages */
+#define IPC_MSG_PANEL_ON_OFF    0xE9
+#define IPC_CMD_PANEL_ON        1
+#define IPC_CMD_PANEL_OFF       0
+
+static int mrst_output_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        if (dev_priv->iLVDS_enable)
+                mrst_lvds_init(dev, &dev_priv->mode_dev);
+        else
+                dev_err(dev->dev, "DSI is not supported\n");
+        if (dev_priv->hdmi_priv)
+                mrst_hdmi_init(dev, &dev_priv->mode_dev);
+        return 0;
+}
+
+/*
+ *      Provide the low level interfaces for the Moorestown backlight
+ */
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+
+#define MRST_BLC_MAX_PWM_REG_FREQ           0xFFFF
+#define BLC_PWM_PRECISION_FACTOR 100    /* 10000000 */
+#define BLC_PWM_FREQ_CALC_CONSTANT 32
+#define MHz 1000000
+#define BLC_ADJUSTMENT_MAX 100
+
+static struct backlight_device *mrst_backlight_device;
+static int mrst_brightness;
+
+static int mrst_set_brightness(struct backlight_device *bd)
+{
+        struct drm_device *dev = bl_get_data(mrst_backlight_device);
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int level = bd->props.brightness;
+        u32 blc_pwm_ctl;
+        u32 max_pwm_blc;
+
+        /* Percentage 1-100% being valid */
+        if (level < 1)
+                level = 1;
+
+        if (gma_power_begin(dev, 0)) {
+                /* Calculate and set the brightness value */
+                max_pwm_blc = REG_READ(BLC_PWM_CTL) >> 16;
+                blc_pwm_ctl = level * max_pwm_blc / 100;
+
+                /* Adjust the backlight level with the percent in
+                 * dev_priv->blc_adj1;
+                 */
+                blc_pwm_ctl = blc_pwm_ctl * dev_priv->blc_adj1;
+                blc_pwm_ctl = blc_pwm_ctl / 100;
+
+                /* Adjust the backlight level with the percent in
+                 * dev_priv->blc_adj2;
+                 */
+                blc_pwm_ctl = blc_pwm_ctl * dev_priv->blc_adj2;
+                blc_pwm_ctl = blc_pwm_ctl / 100;
+
+                /* force PWM bit on */
+                REG_WRITE(BLC_PWM_CTL2, (0x80000000 | REG_READ(BLC_PWM_CTL2)));
+                REG_WRITE(BLC_PWM_CTL, (max_pwm_blc << 16) | blc_pwm_ctl);
+                gma_power_end(dev);
+        }
+        mrst_brightness = level;
+        return 0;
+}
+
+static int mrst_get_brightness(struct backlight_device *bd)
+{
+        /* return locally cached var instead of HW read (due to DPST etc.) */
+        /* FIXME: ideally return actual value in case firmware fiddled with
+           it */
+        return mrst_brightness;
+}
+
+static int device_backlight_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        unsigned long core_clock;
+        u16 bl_max_freq;
+        uint32_t value;
+        uint32_t blc_pwm_precision_factor;
+
+        dev_priv->blc_adj1 = BLC_ADJUSTMENT_MAX;
+        dev_priv->blc_adj2 = BLC_ADJUSTMENT_MAX;
+        bl_max_freq = 256;
+        /* this needs to be set elsewhere */
+        blc_pwm_precision_factor = BLC_PWM_PRECISION_FACTOR;
+
+        core_clock = dev_priv->core_freq;
+
+        value = (core_clock * MHz) / BLC_PWM_FREQ_CALC_CONSTANT;
+        value *= blc_pwm_precision_factor;
+        value /= bl_max_freq;
+        value /= blc_pwm_precision_factor;
+
+        if (value > (unsigned long long)MRST_BLC_MAX_PWM_REG_FREQ)
+                        return -ERANGE;
+
+        if (gma_power_begin(dev, false)) {
+                REG_WRITE(BLC_PWM_CTL2, (0x80000000 | REG_READ(BLC_PWM_CTL2)));
+                REG_WRITE(BLC_PWM_CTL, value | (value << 16));
+                gma_power_end(dev);
+        }
+        return 0;
+}
+
+static const struct backlight_ops mrst_ops = {
+        .get_brightness = mrst_get_brightness,
+        .update_status  = mrst_set_brightness,
+};
+
+int mrst_backlight_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int ret;
+        struct backlight_properties props;
+
+        memset(&props, 0, sizeof(struct backlight_properties));
+        props.max_brightness = 100;
+        props.type = BACKLIGHT_PLATFORM;
+
+        mrst_backlight_device = backlight_device_register("mrst-bl",
+                                        NULL, (void *)dev, &mrst_ops, &props);
+
+        if (IS_ERR(mrst_backlight_device))
+                return PTR_ERR(mrst_backlight_device);
+
+        ret = device_backlight_init(dev);
+        if (ret < 0) {
+                backlight_device_unregister(mrst_backlight_device);
+                return ret;
+        }
+        mrst_backlight_device->props.brightness = 100;
+        mrst_backlight_device->props.max_brightness = 100;
+        backlight_update_status(mrst_backlight_device);
+        dev_priv->backlight_device = mrst_backlight_device;
+        return 0;
+}
+
+#endif
+
+/*
+ *      Provide the Moorestown specific chip logic and low level methods
+ *      for power management
+ */
+
+static void mrst_init_pm(struct drm_device *dev)
+{
+}
+
+/**
+ *      mrst_save_display_registers     -       save registers lost on suspend
+ *      @dev: our DRM device
+ *
+ *      Save the state we need in order to be able to restore the interface
+ *      upon resume from suspend
+ */
+static int mrst_save_display_registers(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int i;
+        u32 pp_stat;
+
+        /* Display arbitration control + watermarks */
+        dev_priv->saveDSPARB = PSB_RVDC32(DSPARB);
+        dev_priv->saveDSPFW1 = PSB_RVDC32(DSPFW1);
+        dev_priv->saveDSPFW2 = PSB_RVDC32(DSPFW2);
+        dev_priv->saveDSPFW3 = PSB_RVDC32(DSPFW3);
+        dev_priv->saveDSPFW4 = PSB_RVDC32(DSPFW4);
+        dev_priv->saveDSPFW5 = PSB_RVDC32(DSPFW5);
+        dev_priv->saveDSPFW6 = PSB_RVDC32(DSPFW6);
+        dev_priv->saveCHICKENBIT = PSB_RVDC32(DSPCHICKENBIT);
+
+        /* Pipe & plane A info */
+        dev_priv->savePIPEACONF = PSB_RVDC32(PIPEACONF);
+        dev_priv->savePIPEASRC = PSB_RVDC32(PIPEASRC);
+        dev_priv->saveFPA0 = PSB_RVDC32(MRST_FPA0);
+        dev_priv->saveFPA1 = PSB_RVDC32(MRST_FPA1);
+        dev_priv->saveDPLL_A = PSB_RVDC32(MRST_DPLL_A);
+        dev_priv->saveHTOTAL_A = PSB_RVDC32(HTOTAL_A);
+        dev_priv->saveHBLANK_A = PSB_RVDC32(HBLANK_A);
+        dev_priv->saveHSYNC_A = PSB_RVDC32(HSYNC_A);
+        dev_priv->saveVTOTAL_A = PSB_RVDC32(VTOTAL_A);
+        dev_priv->saveVBLANK_A = PSB_RVDC32(VBLANK_A);
+        dev_priv->saveVSYNC_A = PSB_RVDC32(VSYNC_A);
+        dev_priv->saveBCLRPAT_A = PSB_RVDC32(BCLRPAT_A);
+        dev_priv->saveDSPACNTR = PSB_RVDC32(DSPACNTR);
+        dev_priv->saveDSPASTRIDE = PSB_RVDC32(DSPASTRIDE);
+        dev_priv->saveDSPAADDR = PSB_RVDC32(DSPABASE);
+        dev_priv->saveDSPASURF = PSB_RVDC32(DSPASURF);
+        dev_priv->saveDSPALINOFF = PSB_RVDC32(DSPALINOFF);
+        dev_priv->saveDSPATILEOFF = PSB_RVDC32(DSPATILEOFF);
+
+        /* Save cursor regs */
+        dev_priv->saveDSPACURSOR_CTRL = PSB_RVDC32(CURACNTR);
+        dev_priv->saveDSPACURSOR_BASE = PSB_RVDC32(CURABASE);
+        dev_priv->saveDSPACURSOR_POS = PSB_RVDC32(CURAPOS);
+
+        /* Save palette (gamma) */
+        for (i = 0; i < 256; i++)
+                dev_priv->save_palette_a[i] = PSB_RVDC32(PALETTE_A + (i << 2));
+
+        if (dev_priv->hdmi_priv)
+                mrst_hdmi_save(dev);
+
+        /* Save performance state */
+        dev_priv->savePERF_MODE = PSB_RVDC32(MRST_PERF_MODE);
+
+        /* LVDS state */
+        dev_priv->savePP_CONTROL = PSB_RVDC32(PP_CONTROL);
+        dev_priv->savePFIT_PGM_RATIOS = PSB_RVDC32(PFIT_PGM_RATIOS);
+        dev_priv->savePFIT_AUTO_RATIOS = PSB_RVDC32(PFIT_AUTO_RATIOS);
+        dev_priv->saveBLC_PWM_CTL = PSB_RVDC32(BLC_PWM_CTL);
+        dev_priv->saveBLC_PWM_CTL2 = PSB_RVDC32(BLC_PWM_CTL2);
+        dev_priv->saveLVDS = PSB_RVDC32(LVDS);
+        dev_priv->savePFIT_CONTROL = PSB_RVDC32(PFIT_CONTROL);
+        dev_priv->savePP_ON_DELAYS = PSB_RVDC32(LVDSPP_ON);
+        dev_priv->savePP_OFF_DELAYS = PSB_RVDC32(LVDSPP_OFF);
+        dev_priv->savePP_DIVISOR = PSB_RVDC32(PP_CYCLE);
+
+        /* HW overlay */
+        dev_priv->saveOV_OVADD = PSB_RVDC32(OV_OVADD);
+        dev_priv->saveOV_OGAMC0 = PSB_RVDC32(OV_OGAMC0);
+        dev_priv->saveOV_OGAMC1 = PSB_RVDC32(OV_OGAMC1);
+        dev_priv->saveOV_OGAMC2 = PSB_RVDC32(OV_OGAMC2);
+        dev_priv->saveOV_OGAMC3 = PSB_RVDC32(OV_OGAMC3);
+        dev_priv->saveOV_OGAMC4 = PSB_RVDC32(OV_OGAMC4);
+        dev_priv->saveOV_OGAMC5 = PSB_RVDC32(OV_OGAMC5);
+
+        /* DPST registers */
+        dev_priv->saveHISTOGRAM_INT_CONTROL_REG =
+                                        PSB_RVDC32(HISTOGRAM_INT_CONTROL);
+        dev_priv->saveHISTOGRAM_LOGIC_CONTROL_REG =
+                                        PSB_RVDC32(HISTOGRAM_LOGIC_CONTROL);
+        dev_priv->savePWM_CONTROL_LOGIC = PSB_RVDC32(PWM_CONTROL_LOGIC);
+
+        if (dev_priv->iLVDS_enable) {
+                /* Shut down the panel */
+                PSB_WVDC32(0, PP_CONTROL);
+
+                do {
+                        pp_stat = PSB_RVDC32(PP_STATUS);
+                } while (pp_stat & 0x80000000);
+
+                /* Turn off the plane */
+                PSB_WVDC32(0x58000000, DSPACNTR);
+                /* Trigger the plane disable */
+                PSB_WVDC32(0, DSPASURF);
+
+                /* Wait ~4 ticks */
+                msleep(4);
+
+                /* Turn off pipe */
+                PSB_WVDC32(0x0, PIPEACONF);
+                /* Wait ~8 ticks */
+                msleep(8);
+
+                /* Turn off PLLs */
+                PSB_WVDC32(0, MRST_DPLL_A);
+        }
+        return 0;
+}
+
+/**
+ *      mrst_restore_display_registers  -       restore lost register state
+ *      @dev: our DRM device
+ *
+ *      Restore register state that was lost during suspend and resume.
+ */
+static int mrst_restore_display_registers(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 pp_stat;
+        int i;
+
+        /* Display arbitration + watermarks */
+        PSB_WVDC32(dev_priv->saveDSPARB, DSPARB);
+        PSB_WVDC32(dev_priv->saveDSPFW1, DSPFW1);
+        PSB_WVDC32(dev_priv->saveDSPFW2, DSPFW2);
+        PSB_WVDC32(dev_priv->saveDSPFW3, DSPFW3);
+        PSB_WVDC32(dev_priv->saveDSPFW4, DSPFW4);
+        PSB_WVDC32(dev_priv->saveDSPFW5, DSPFW5);
+        PSB_WVDC32(dev_priv->saveDSPFW6, DSPFW6);
+        PSB_WVDC32(dev_priv->saveCHICKENBIT, DSPCHICKENBIT);
+
+        /* Make sure VGA plane is off. it initializes to on after reset!*/
+        PSB_WVDC32(0x80000000, VGACNTRL);
+
+        /* set the plls */
+        PSB_WVDC32(dev_priv->saveFPA0, MRST_FPA0);
+        PSB_WVDC32(dev_priv->saveFPA1, MRST_FPA1);
+
+        /* Actually enable it */
+        PSB_WVDC32(dev_priv->saveDPLL_A, MRST_DPLL_A);
+        DRM_UDELAY(150);
+
+        /* Restore mode */
+        PSB_WVDC32(dev_priv->saveHTOTAL_A, HTOTAL_A);
+        PSB_WVDC32(dev_priv->saveHBLANK_A, HBLANK_A);
+        PSB_WVDC32(dev_priv->saveHSYNC_A, HSYNC_A);
+        PSB_WVDC32(dev_priv->saveVTOTAL_A, VTOTAL_A);
+        PSB_WVDC32(dev_priv->saveVBLANK_A, VBLANK_A);
+        PSB_WVDC32(dev_priv->saveVSYNC_A, VSYNC_A);
+        PSB_WVDC32(dev_priv->savePIPEASRC, PIPEASRC);
+        PSB_WVDC32(dev_priv->saveBCLRPAT_A, BCLRPAT_A);
+
+        /* Restore performance mode*/
+        PSB_WVDC32(dev_priv->savePERF_MODE, MRST_PERF_MODE);
+
+        /* Enable the pipe*/
+        if (dev_priv->iLVDS_enable)
+                PSB_WVDC32(dev_priv->savePIPEACONF, PIPEACONF);
+
+        /* Set up the plane*/
+        PSB_WVDC32(dev_priv->saveDSPALINOFF, DSPALINOFF);
+        PSB_WVDC32(dev_priv->saveDSPASTRIDE, DSPASTRIDE);
+        PSB_WVDC32(dev_priv->saveDSPATILEOFF, DSPATILEOFF);
+
+        /* Enable the plane */
+        PSB_WVDC32(dev_priv->saveDSPACNTR, DSPACNTR);
+        PSB_WVDC32(dev_priv->saveDSPASURF, DSPASURF);
+
+        /* Enable Cursor A */
+        PSB_WVDC32(dev_priv->saveDSPACURSOR_CTRL, CURACNTR);
+        PSB_WVDC32(dev_priv->saveDSPACURSOR_POS, CURAPOS);
+        PSB_WVDC32(dev_priv->saveDSPACURSOR_BASE, CURABASE);
+
+        /* Restore palette (gamma) */
+        for (i = 0; i < 256; i++)
+                PSB_WVDC32(dev_priv->save_palette_a[i], PALETTE_A + (i << 2));
+
+        if (dev_priv->hdmi_priv)
+                mrst_hdmi_restore(dev);
+
+        if (dev_priv->iLVDS_enable) {
+                PSB_WVDC32(dev_priv->saveBLC_PWM_CTL2, BLC_PWM_CTL2);
+                PSB_WVDC32(dev_priv->saveLVDS, LVDS); /*port 61180h*/
+                PSB_WVDC32(dev_priv->savePFIT_CONTROL, PFIT_CONTROL);
+                PSB_WVDC32(dev_priv->savePFIT_PGM_RATIOS, PFIT_PGM_RATIOS);
+                PSB_WVDC32(dev_priv->savePFIT_AUTO_RATIOS, PFIT_AUTO_RATIOS);
+                PSB_WVDC32(dev_priv->saveBLC_PWM_CTL, BLC_PWM_CTL);
+                PSB_WVDC32(dev_priv->savePP_ON_DELAYS, LVDSPP_ON);
+                PSB_WVDC32(dev_priv->savePP_OFF_DELAYS, LVDSPP_OFF);
+                PSB_WVDC32(dev_priv->savePP_DIVISOR, PP_CYCLE);
+                PSB_WVDC32(dev_priv->savePP_CONTROL, PP_CONTROL);
+        }
+
+        /* Wait for cycle delay */
+        do {
+                pp_stat = PSB_RVDC32(PP_STATUS);
+        } while (pp_stat & 0x08000000);
+
+        /* Wait for panel power up */
+        do {
+                pp_stat = PSB_RVDC32(PP_STATUS);
+        } while (pp_stat & 0x10000000);
+
+        /* Restore HW overlay */
+        PSB_WVDC32(dev_priv->saveOV_OVADD, OV_OVADD);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC0, OV_OGAMC0);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC1, OV_OGAMC1);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC2, OV_OGAMC2);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC3, OV_OGAMC3);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC4, OV_OGAMC4);
+        PSB_WVDC32(dev_priv->saveOV_OGAMC5, OV_OGAMC5);
+
+        /* DPST registers */
+        PSB_WVDC32(dev_priv->saveHISTOGRAM_INT_CONTROL_REG,
+                                                HISTOGRAM_INT_CONTROL);
+        PSB_WVDC32(dev_priv->saveHISTOGRAM_LOGIC_CONTROL_REG,
+                                                HISTOGRAM_LOGIC_CONTROL);
+        PSB_WVDC32(dev_priv->savePWM_CONTROL_LOGIC, PWM_CONTROL_LOGIC);
+
+        return 0;
+}
+
+/**
+ *      mrst_power_down -       power down the display island
+ *      @dev: our DRM device
+ *
+ *      Power down the display interface of our device
+ */
+static int mrst_power_down(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 pwr_mask ;
+        u32 pwr_sts;
+
+        pwr_mask = PSB_PWRGT_DISPLAY_MASK;
+        outl(pwr_mask, dev_priv->ospm_base + PSB_PM_SSC);
+
+        while (true) {
+                pwr_sts = inl(dev_priv->ospm_base + PSB_PM_SSS);
+                if ((pwr_sts & pwr_mask) == pwr_mask)
+                        break;
+                else
+                        udelay(10);
+        }
+        return 0;
+}
+
+/*
+ * mrst_power_up
+ *
+ * Restore power to the specified island(s) (powergating)
+ */
+static int mrst_power_up(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 pwr_mask = PSB_PWRGT_DISPLAY_MASK;
+        u32 pwr_sts, pwr_cnt;
+
+        pwr_cnt = inl(dev_priv->ospm_base + PSB_PM_SSC);
+        pwr_cnt &= ~pwr_mask;
+        outl(pwr_cnt, (dev_priv->ospm_base + PSB_PM_SSC));
+
+        while (true) {
+                pwr_sts = inl(dev_priv->ospm_base + PSB_PM_SSS);
+                if ((pwr_sts & pwr_mask) == 0)
+                        break;
+                else
+                        udelay(10);
+        }
+        return 0;
+}
+
+#if defined(CONFIG_X86_MRST)
+static void mrst_lvds_cache_bl(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        intel_scu_ipc_ioread8(0x28, &(dev_priv->saveBKLTCNT));
+        intel_scu_ipc_ioread8(0x29, &(dev_priv->saveBKLTREQ));
+        intel_scu_ipc_ioread8(0x2A, &(dev_priv->saveBKLTBRTL));
+}
+
+static void mrst_mm_bl_power(struct drm_device *dev, bool on)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (on) {
+                intel_scu_ipc_iowrite8(0x2A, dev_priv->saveBKLTBRTL);
+                intel_scu_ipc_iowrite8(0x28, dev_priv->saveBKLTCNT);
+                intel_scu_ipc_iowrite8(0x29, dev_priv->saveBKLTREQ);
+        } else {
+                intel_scu_ipc_iowrite8(0x2A, 0);
+                intel_scu_ipc_iowrite8(0x28, 0);
+                intel_scu_ipc_iowrite8(0x29, 0);
+        }
+}
+
+static const struct psb_ops mrst_mm_chip_ops = {
+        .name = "Moorestown MM ",
+        .accel_2d = 1,
+        .pipes = 1,
+        .crtcs = 1,
+        .sgx_offset = MRST_SGX_OFFSET,
+
+        .crtc_helper = &mrst_helper_funcs,
+        .crtc_funcs = &psb_intel_crtc_funcs,
+
+        .output_init = mrst_output_init,
+
+        .lvds_bl_power = mrst_mm_bl_power,
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+        .backlight_init = mrst_backlight_init,
+#endif
+
+        .init_pm = mrst_init_pm,
+        .save_regs = mrst_save_display_registers,
+        .restore_regs = mrst_restore_display_registers,
+        .power_down = mrst_power_down,
+        .power_up = mrst_power_up,
+
+        .i2c_bus = 0,
+};
+
+#endif
+
+static void oaktrail_teardown(struct drm_device *dev)
+{
+        mrst_hdmi_teardown(dev);
+}
+
+static const struct psb_ops oaktrail_chip_ops = {
+        .name = "Oaktrail",
+        .accel_2d = 1,
+        .pipes = 2,
+        .crtcs = 2,
+        .sgx_offset = MRST_SGX_OFFSET,
+
+        .chip_setup = mid_chip_setup,
+        .chip_teardown = oaktrail_teardown,
+        .crtc_helper = &mrst_helper_funcs,
+        .crtc_funcs = &psb_intel_crtc_funcs,
+
+        .output_init = mrst_output_init,
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+        .backlight_init = mrst_backlight_init,
+#endif
+
+        .init_pm = mrst_init_pm,
+        .save_regs = mrst_save_display_registers,
+        .restore_regs = mrst_restore_display_registers,
+        .power_down = mrst_power_down,
+        .power_up = mrst_power_up,
+
+        .i2c_bus = 1,
+};
+
+/**
+ *      mrst_chip_setup         -       perform the initial chip init
+ *      @dev: Our drm_device
+ *
+ *      Figure out which incarnation we are and then scan the firmware for
+ *      tables and information.
+ */
+static int mrst_chip_setup(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        switch (mrst_device_ident(dev)) {
+        case DEVICE_OAKTRAIL:
+                /* Dual CRTC, PC compatible, HDMI, I2C #2 */
+                dev_priv->ops = &oaktrail_chip_ops;
+                mrst_hdmi_setup(dev);
+                return mid_chip_setup(dev);
+#if defined(CONFIG_X86_MRST)
+        case DEVICE_MOORESTOWN_MM:
+                /* Single CRTC, No HDMI, I2C #0, BL control */
+                mrst_lvds_cache_bl(dev);
+                dev_priv->ops = &mrst_mm_chip_ops;
+                return mid_chip_setup(dev);
+        case DEVICE_MOORESTOWN:
+                /* Dual CRTC, No HDMI(?), I2C #1 */
+                return mid_chip_setup(dev);
+#endif
+        default:
+                dev_err(dev->dev, "unsupported device type.\n");
+                return -ENODEV;
+        }
+}
+
+const struct psb_ops mrst_chip_ops = {
+        .name = "Moorestown",
+        .accel_2d = 1,
+        .pipes = 2,
+        .crtcs = 2,
+        .sgx_offset = MRST_SGX_OFFSET,
+
+        .chip_setup = mrst_chip_setup,
+        .crtc_helper = &mrst_helper_funcs,
+        .crtc_funcs = &psb_intel_crtc_funcs,
+
+        .output_init = mrst_output_init,
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+        .backlight_init = mrst_backlight_init,
+#endif
+
+        .init_pm = mrst_init_pm,
+        .save_regs = mrst_save_display_registers,
+        .restore_regs = mrst_restore_display_registers,
+        .power_down = mrst_power_down,
+        .power_up = mrst_power_up,
+
+        .i2c_bus = 2,
+};
+
diff --git a/drivers/staging/gma500/mrst_hdmi.c b/drivers/staging/gma500/mrst_hdmi.c
new file mode 100644
index 00000000000..e66607eb3d3
--- /dev/null
+++ b/drivers/staging/gma500/mrst_hdmi.c
@@ -0,0 +1,852 @@
+/*
+ * Copyright © 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:
+ *      Li Peng <peng.li@intel.com>
+ */
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_drv.h"
+
+#define HDMI_READ(reg)          readl(hdmi_dev->regs + (reg))
+#define HDMI_WRITE(reg, val)    writel(val, hdmi_dev->regs + (reg))
+
+#define HDMI_HCR        0x1000
+#define HCR_ENABLE_HDCP         (1 << 5)
+#define HCR_ENABLE_AUDIO        (1 << 2)
+#define HCR_ENABLE_PIXEL        (1 << 1)
+#define HCR_ENABLE_TMDS         (1 << 0)
+
+#define HDMI_HICR       0x1004
+#define HDMI_HSR        0x1008
+#define HDMI_HISR       0x100C
+#define HDMI_DETECT_HDP         (1 << 0)
+
+#define HDMI_VIDEO_REG  0x3000
+#define HDMI_UNIT_EN            (1 << 7)
+#define HDMI_MODE_OUTPUT        (1 << 0)
+#define HDMI_HBLANK_A   0x3100
+
+#define HDMI_AUDIO_CTRL 0x4000
+#define HDMI_ENABLE_AUDIO       (1 << 0)
+
+#define PCH_HTOTAL_B    0x3100
+#define PCH_HBLANK_B    0x3104
+#define PCH_HSYNC_B     0x3108
+#define PCH_VTOTAL_B    0x310C
+#define PCH_VBLANK_B    0x3110
+#define PCH_VSYNC_B     0x3114
+#define PCH_PIPEBSRC    0x311C
+
+#define PCH_PIPEB_DSL   0x3800
+#define PCH_PIPEB_SLC   0x3804
+#define PCH_PIPEBCONF   0x3808
+#define PCH_PIPEBSTAT   0x3824
+
+#define CDVO_DFT        0x5000
+#define CDVO_SLEWRATE   0x5004
+#define CDVO_STRENGTH   0x5008
+#define CDVO_RCOMP      0x500C
+
+#define DPLL_CTRL       0x6000
+#define DPLL_PDIV_SHIFT         16
+#define DPLL_PDIV_MASK          (0xf << 16)
+#define DPLL_PWRDN              (1 << 4)
+#define DPLL_RESET              (1 << 3)
+#define DPLL_FASTEN             (1 << 2)
+#define DPLL_ENSTAT             (1 << 1)
+#define DPLL_DITHEN             (1 << 0)
+
+#define DPLL_DIV_CTRL   0x6004
+#define DPLL_CLKF_MASK          0xffffffc0
+#define DPLL_CLKR_MASK          (0x3f)
+
+#define DPLL_CLK_ENABLE 0x6008
+#define DPLL_EN_DISP            (1 << 31)
+#define DPLL_SEL_HDMI           (1 << 8)
+#define DPLL_EN_HDMI            (1 << 1)
+#define DPLL_EN_VGA             (1 << 0)
+
+#define DPLL_ADJUST     0x600C
+#define DPLL_STATUS     0x6010
+#define DPLL_UPDATE     0x6014
+#define DPLL_DFT        0x6020
+
+struct intel_range {
+        int     min, max;
+};
+
+struct mrst_hdmi_limit {
+        struct intel_range vco, np, nr, nf;
+};
+
+struct mrst_hdmi_clock {
+        int np;
+        int nr;
+        int nf;
+        int dot;
+};
+
+#define VCO_MIN         320000
+#define VCO_MAX         1650000
+#define NP_MIN          1
+#define NP_MAX          15
+#define NR_MIN          1
+#define NR_MAX          64
+#define NF_MIN          2
+#define NF_MAX          4095
+
+static const struct mrst_hdmi_limit mrst_hdmi_limit = {
+        .vco = { .min = VCO_MIN,                .max = VCO_MAX },
+        .np  = { .min = NP_MIN,                 .max = NP_MAX  },
+        .nr  = { .min = NR_MIN,                 .max = NR_MAX  },
+        .nf  = { .min = NF_MIN,                 .max = NF_MAX  },
+};
+
+static void wait_for_vblank(struct drm_device *dev)
+{
+        /* FIXME: Can we do this as a sleep ? */
+        /* Wait for 20ms, i.e. one cycle at 50hz. */
+        mdelay(20);
+}
+
+static void scu_busy_loop(void *scu_base)
+{
+        u32 status = 0;
+        u32 loop_count = 0;
+
+        status = readl(scu_base + 0x04);
+        while (status & 1) {
+                udelay(1); /* scu processing time is in few u secods */
+                status = readl(scu_base + 0x04);
+                loop_count++;
+                /* break if scu doesn't reset busy bit after huge retry */
+                if (loop_count > 1000) {
+                        DRM_DEBUG_KMS("SCU IPC timed out");
+                        return;
+                }
+        }
+}
+
+static void mrst_hdmi_reset(struct drm_device *dev)
+{
+        void *base;
+        /* FIXME: at least make these defines */
+        unsigned int scu_ipc_mmio = 0xff11c000;
+        int scu_len = 1024;
+
+        base = ioremap((resource_size_t)scu_ipc_mmio, scu_len);
+        if (base == NULL) {
+                DRM_ERROR("failed to map SCU mmio\n");
+                return;
+        }
+
+        /* scu ipc: assert hdmi controller reset */
+        writel(0xff11d118, base + 0x0c);
+        writel(0x7fffffdf, base + 0x80);
+        writel(0x42005, base + 0x0);
+        scu_busy_loop(base);
+
+        /* scu ipc: de-assert hdmi controller reset */
+        writel(0xff11d118, base + 0x0c);
+        writel(0x7fffffff, base + 0x80);
+        writel(0x42005, base + 0x0);
+        scu_busy_loop(base);
+
+        iounmap(base);
+}
+
+static void mrst_hdmi_audio_enable(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+
+        HDMI_WRITE(HDMI_HCR, 0x67);
+        HDMI_READ(HDMI_HCR);
+
+        HDMI_WRITE(0x51a8, 0x10);
+        HDMI_READ(0x51a8);
+
+        HDMI_WRITE(HDMI_AUDIO_CTRL, 0x1);
+        HDMI_READ(HDMI_AUDIO_CTRL);
+}
+
+static void mrst_hdmi_audio_disable(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+
+        HDMI_WRITE(0x51a8, 0x0);
+        HDMI_READ(0x51a8);
+
+        HDMI_WRITE(HDMI_AUDIO_CTRL, 0x0);
+        HDMI_READ(HDMI_AUDIO_CTRL);
+
+        HDMI_WRITE(HDMI_HCR, 0x47);
+        HDMI_READ(HDMI_HCR);
+}
+
+void mrst_crtc_hdmi_dpms(struct drm_crtc *crtc, int mode)
+{
+        struct drm_device *dev = crtc->dev;
+        u32 temp;
+
+        switch (mode) {
+        case DRM_MODE_DPMS_OFF:
+                /* Disable VGACNTRL */
+                REG_WRITE(VGACNTRL, 0x80000000);
+
+                /* Disable plane */
+                temp = REG_READ(DSPBCNTR);
+                if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
+                        REG_WRITE(DSPBCNTR, temp & ~DISPLAY_PLANE_ENABLE);
+                        REG_READ(DSPBCNTR);
+                        /* Flush the plane changes */
+                        REG_WRITE(DSPBSURF, REG_READ(DSPBSURF));
+                        REG_READ(DSPBSURF);
+                }
+
+                /* Disable pipe B */
+                temp = REG_READ(PIPEBCONF);
+                if ((temp & PIPEACONF_ENABLE) != 0) {
+                        REG_WRITE(PIPEBCONF, temp & ~PIPEACONF_ENABLE);
+                        REG_READ(PIPEBCONF);
+                }
+
+                /* Disable LNW Pipes, etc */
+                temp = REG_READ(PCH_PIPEBCONF);
+                if ((temp & PIPEACONF_ENABLE) != 0) {
+                        REG_WRITE(PCH_PIPEBCONF, temp & ~PIPEACONF_ENABLE);
+                        REG_READ(PCH_PIPEBCONF);
+                }
+                /* wait for pipe off */
+                udelay(150);
+                /* Disable dpll */
+                temp = REG_READ(DPLL_CTRL);
+                if ((temp & DPLL_PWRDN) == 0) {
+                        REG_WRITE(DPLL_CTRL, temp | (DPLL_PWRDN | DPLL_RESET));
+                        REG_WRITE(DPLL_STATUS, 0x1);
+                }
+                /* wait for dpll off */
+                udelay(150);
+                break;
+        case DRM_MODE_DPMS_ON:
+        case DRM_MODE_DPMS_STANDBY:
+        case DRM_MODE_DPMS_SUSPEND:
+                /* Enable dpll */
+                temp = REG_READ(DPLL_CTRL);
+                if ((temp & DPLL_PWRDN) != 0) {
+                        REG_WRITE(DPLL_CTRL, temp & ~(DPLL_PWRDN | DPLL_RESET));
+                        temp = REG_READ(DPLL_CLK_ENABLE);
+                        REG_WRITE(DPLL_CLK_ENABLE, temp | DPLL_EN_DISP | DPLL_SEL_HDMI | DPLL_EN_HDMI);
+                        REG_READ(DPLL_CLK_ENABLE);
+                }
+                /* wait for dpll warm up */
+                udelay(150);
+
+                /* Enable pipe B */
+                temp = REG_READ(PIPEBCONF);
+                if ((temp & PIPEACONF_ENABLE) == 0) {
+                        REG_WRITE(PIPEBCONF, temp | PIPEACONF_ENABLE);
+                        REG_READ(PIPEBCONF);
+                }
+
+                /* Enable LNW Pipe B */
+                temp = REG_READ(PCH_PIPEBCONF);
+                if ((temp & PIPEACONF_ENABLE) == 0) {
+                        REG_WRITE(PCH_PIPEBCONF, temp | PIPEACONF_ENABLE);
+                        REG_READ(PCH_PIPEBCONF);
+                }
+                wait_for_vblank(dev);
+
+                /* Enable plane */
+                temp = REG_READ(DSPBCNTR);
+                if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
+                        REG_WRITE(DSPBCNTR, temp | DISPLAY_PLANE_ENABLE);
+                        /* Flush the plane changes */
+                        REG_WRITE(DSPBSURF, REG_READ(DSPBSURF));
+                        REG_READ(DSPBSURF);
+                }
+                psb_intel_crtc_load_lut(crtc);
+        }
+        /* DSPARB */
+        REG_WRITE(DSPARB, 0x00003fbf);
+        /* FW1 */
+        REG_WRITE(0x70034, 0x3f880a0a);
+        /* FW2 */
+        REG_WRITE(0x70038, 0x0b060808);
+        /* FW4 */
+        REG_WRITE(0x70050, 0x08030404);
+        /* FW5 */
+        REG_WRITE(0x70054, 0x04040404);
+        /* LNC Chicken Bits */
+        REG_WRITE(0x70400, 0x4000);
+}
+
+
+static void mrst_hdmi_dpms(struct drm_encoder *encoder, int mode)
+{
+        static int dpms_mode = -1;
+
+        struct drm_device *dev = encoder->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+        u32 temp;
+
+        if (dpms_mode == mode)
+                return;
+
+        if (mode != DRM_MODE_DPMS_ON)
+                temp = 0x0;
+        else
+                temp = 0x99;
+
+        dpms_mode = mode;
+        HDMI_WRITE(HDMI_VIDEO_REG, temp);
+}
+
+static unsigned int htotal_calculate(struct drm_display_mode *mode)
+{
+        u32 htotal, new_crtc_htotal;
+
+        htotal = (mode->crtc_hdisplay - 1) | ((mode->crtc_htotal - 1) << 16);
+
+        /*
+         * 1024 x 768  new_crtc_htotal = 0x1024;
+         * 1280 x 1024 new_crtc_htotal = 0x0c34;
+         */
+        new_crtc_htotal = (mode->crtc_htotal - 1) * 200 * 1000 / mode->clock;
+
+        return (mode->crtc_hdisplay - 1) | (new_crtc_htotal << 16);
+}
+
+static void mrst_hdmi_find_dpll(struct drm_crtc *crtc, int target,
+                                int refclk, struct mrst_hdmi_clock *best_clock)
+{
+        int np_min, np_max, nr_min, nr_max;
+        int np, nr, nf;
+
+        np_min = DIV_ROUND_UP(mrst_hdmi_limit.vco.min, target * 10);
+        np_max = mrst_hdmi_limit.vco.max / (target * 10);
+        if (np_min < mrst_hdmi_limit.np.min)
+                np_min = mrst_hdmi_limit.np.min;
+        if (np_max > mrst_hdmi_limit.np.max)
+                np_max = mrst_hdmi_limit.np.max;
+
+        nr_min = DIV_ROUND_UP((refclk * 1000), (target * 10 * np_max));
+        nr_max = DIV_ROUND_UP((refclk * 1000), (target * 10 * np_min));
+        if (nr_min < mrst_hdmi_limit.nr.min)
+                nr_min = mrst_hdmi_limit.nr.min;
+        if (nr_max > mrst_hdmi_limit.nr.max)
+                nr_max = mrst_hdmi_limit.nr.max;
+
+        np = DIV_ROUND_UP((refclk * 1000), (target * 10 * nr_max));
+        nr = DIV_ROUND_UP((refclk * 1000), (target * 10 * np));
+        nf = DIV_ROUND_CLOSEST((target * 10 * np * nr), refclk);
+        DRM_DEBUG_KMS("np, nr, nf %d %d %d\n", np, nr, nf);
+
+        /*
+         * 1024 x 768  np = 1; nr = 0x26; nf = 0x0fd8000;
+         * 1280 x 1024 np = 1; nr = 0x17; nf = 0x1034000;
+         */
+        best_clock->np = np;
+        best_clock->nr = nr - 1;
+        best_clock->nf = (nf << 14);
+}
+
+int mrst_crtc_hdmi_mode_set(struct drm_crtc *crtc,
+                            struct drm_display_mode *mode,
+                            struct drm_display_mode *adjusted_mode,
+                            int x, int y,
+                            struct drm_framebuffer *old_fb)
+{
+        struct drm_device *dev = crtc->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+        int pipe = 1;
+        int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
+        int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
+        int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
+        int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
+        int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
+        int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
+        int dspsize_reg = (pipe == 0) ? DSPASIZE : DSPBSIZE;
+        int dsppos_reg = (pipe == 0) ? DSPAPOS : DSPBPOS;
+        int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+        int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+        int refclk;
+        struct mrst_hdmi_clock clock;
+        u32 dspcntr, pipeconf, dpll, temp;
+        int dspcntr_reg = DSPBCNTR;
+
+        /* Disable the VGA plane that we never use */
+        REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+
+        /* XXX: Disable the panel fitter if it was on our pipe */
+
+        /* Disable dpll if necessary */
+        dpll = REG_READ(DPLL_CTRL);
+        if ((dpll & DPLL_PWRDN) == 0) {
+                REG_WRITE(DPLL_CTRL, dpll | (DPLL_PWRDN | DPLL_RESET));
+                REG_WRITE(DPLL_DIV_CTRL, 0x00000000);
+                REG_WRITE(DPLL_STATUS, 0x1);
+        }
+        udelay(150);
+
+        /* reset controller: FIXME - can we sort out the ioremap mess ? */
+        iounmap(hdmi_dev->regs);
+        mrst_hdmi_reset(dev);
+
+        /* program and enable dpll */
+        refclk = 25000;
+        mrst_hdmi_find_dpll(crtc, adjusted_mode->clock, refclk, &clock);
+
+        /* Setting DPLL */
+        dpll = REG_READ(DPLL_CTRL);
+        dpll &= ~DPLL_PDIV_MASK;
+        dpll &= ~(DPLL_PWRDN | DPLL_RESET);
+        REG_WRITE(DPLL_CTRL, 0x00000008);
+        REG_WRITE(DPLL_DIV_CTRL, ((clock.nf << 6) | clock.nr));
+        REG_WRITE(DPLL_ADJUST, ((clock.nf >> 14) - 1));
+        REG_WRITE(DPLL_CTRL, (dpll | (clock.np << DPLL_PDIV_SHIFT) | DPLL_ENSTAT | DPLL_DITHEN));
+        REG_WRITE(DPLL_UPDATE, 0x80000000);
+        REG_WRITE(DPLL_CLK_ENABLE, 0x80050102);
+        udelay(150);
+
+        hdmi_dev->regs = ioremap(hdmi_dev->mmio, hdmi_dev->mmio_len);
+        if (hdmi_dev->regs == NULL) {
+                DRM_ERROR("failed to do hdmi mmio mapping\n");
+                return -ENOMEM;
+        }
+
+        /* configure HDMI */
+        HDMI_WRITE(0x1004, 0x1fd);
+        HDMI_WRITE(0x2000, 0x1);
+        HDMI_WRITE(0x2008, 0x0);
+        HDMI_WRITE(0x3130, 0x8);
+        HDMI_WRITE(0x101c, 0x1800810);
+
+        temp = htotal_calculate(adjusted_mode);
+        REG_WRITE(htot_reg, temp);
+        REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) | ((adjusted_mode->crtc_hblank_end - 1) << 16));
+        REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) | ((adjusted_mode->crtc_hsync_end - 1) << 16));
+        REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) | ((adjusted_mode->crtc_vtotal - 1) << 16));
+        REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) | ((adjusted_mode->crtc_vblank_end - 1) << 16));
+        REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) | ((adjusted_mode->crtc_vsync_end - 1) << 16));
+        REG_WRITE(pipesrc_reg,
+                ((mode->crtc_hdisplay - 1) << 16) |  (mode->crtc_vdisplay - 1));
+
+        REG_WRITE(PCH_HTOTAL_B, (adjusted_mode->crtc_hdisplay - 1) | ((adjusted_mode->crtc_htotal - 1) << 16));
+        REG_WRITE(PCH_HBLANK_B, (adjusted_mode->crtc_hblank_start - 1) | ((adjusted_mode->crtc_hblank_end - 1) << 16));
+        REG_WRITE(PCH_HSYNC_B, (adjusted_mode->crtc_hsync_start - 1) | ((adjusted_mode->crtc_hsync_end - 1) << 16));
+        REG_WRITE(PCH_VTOTAL_B, (adjusted_mode->crtc_vdisplay - 1) | ((adjusted_mode->crtc_vtotal - 1) << 16));
+        REG_WRITE(PCH_VBLANK_B, (adjusted_mode->crtc_vblank_start - 1) | ((adjusted_mode->crtc_vblank_end - 1) << 16));
+        REG_WRITE(PCH_VSYNC_B, (adjusted_mode->crtc_vsync_start - 1) | ((adjusted_mode->crtc_vsync_end - 1) << 16));
+        REG_WRITE(PCH_PIPEBSRC,
+                ((mode->crtc_hdisplay - 1) << 16) |  (mode->crtc_vdisplay - 1));
+
+        temp = adjusted_mode->crtc_hblank_end - adjusted_mode->crtc_hblank_start;
+        HDMI_WRITE(HDMI_HBLANK_A, ((adjusted_mode->crtc_hdisplay - 1) << 16) |  temp);
+
+        REG_WRITE(dspsize_reg,
+                        ((mode->vdisplay - 1) << 16) | (mode->hdisplay - 1));
+        REG_WRITE(dsppos_reg, 0);
+
+        /* Flush the plane changes */
+        {
+                struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+                crtc_funcs->mode_set_base(crtc, x, y, old_fb);
+        }
+
+        /* Set up the display plane register */
+        dspcntr = REG_READ(dspcntr_reg);
+        dspcntr |= DISPPLANE_GAMMA_ENABLE;
+        dspcntr |= DISPPLANE_SEL_PIPE_B;
+        dspcntr |= DISPLAY_PLANE_ENABLE;
+
+        /* setup pipeconf */
+        pipeconf = REG_READ(pipeconf_reg);
+        pipeconf |= PIPEACONF_ENABLE;
+
+        REG_WRITE(pipeconf_reg, pipeconf);
+        REG_READ(pipeconf_reg);
+
+        REG_WRITE(PCH_PIPEBCONF, pipeconf);
+        REG_READ(PCH_PIPEBCONF);
+        wait_for_vblank(dev);
+
+        REG_WRITE(dspcntr_reg, dspcntr);
+        wait_for_vblank(dev);
+
+        return 0;
+}
+
+static int mrst_hdmi_mode_valid(struct drm_connector *connector,
+                                struct drm_display_mode *mode)
+{
+        if (mode->clock > 165000)
+                return MODE_CLOCK_HIGH;
+        if (mode->clock < 20000)
+                return MODE_CLOCK_LOW;
+
+        if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+                return MODE_NO_DBLESCAN;
+
+        return MODE_OK;
+}
+
+static bool mrst_hdmi_mode_fixup(struct drm_encoder *encoder,
+                                 struct drm_display_mode *mode,
+                                 struct drm_display_mode *adjusted_mode)
+{
+        return true;
+}
+
+static enum drm_connector_status
+mrst_hdmi_detect(struct drm_connector *connector, bool force)
+{
+        enum drm_connector_status status;
+        struct drm_device *dev = connector->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+        u32 temp;
+
+        temp = HDMI_READ(HDMI_HSR);
+        DRM_DEBUG_KMS("HDMI_HSR %x\n", temp);
+
+        if ((temp & HDMI_DETECT_HDP) != 0)
+                status = connector_status_connected;
+        else
+                status = connector_status_disconnected;
+
+        return status;
+}
+
+static const unsigned char raw_edid[] = {
+        0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x10, 0xac, 0x2f, 0xa0,
+        0x53, 0x55, 0x33, 0x30, 0x16, 0x13, 0x01, 0x03, 0x0e, 0x3a, 0x24, 0x78,
+        0xea, 0xe9, 0xf5, 0xac, 0x51, 0x30, 0xb4, 0x25, 0x11, 0x50, 0x54, 0xa5,
+        0x4b, 0x00, 0x81, 0x80, 0xa9, 0x40, 0x71, 0x4f, 0xb3, 0x00, 0x01, 0x01,
+        0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x28, 0x3c, 0x80, 0xa0, 0x70, 0xb0,
+        0x23, 0x40, 0x30, 0x20, 0x36, 0x00, 0x46, 0x6c, 0x21, 0x00, 0x00, 0x1a,
+        0x00, 0x00, 0x00, 0xff, 0x00, 0x47, 0x4e, 0x37, 0x32, 0x31, 0x39, 0x35,
+        0x52, 0x30, 0x33, 0x55, 0x53, 0x0a, 0x00, 0x00, 0x00, 0xfc, 0x00, 0x44,
+        0x45, 0x4c, 0x4c, 0x20, 0x32, 0x37, 0x30, 0x39, 0x57, 0x0a, 0x20, 0x20,
+        0x00, 0x00, 0x00, 0xfd, 0x00, 0x38, 0x4c, 0x1e, 0x53, 0x11, 0x00, 0x0a,
+        0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x00, 0x8d
+};
+
+static int mrst_hdmi_get_modes(struct drm_connector *connector)
+{
+        struct drm_device *dev = connector->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct i2c_adapter *i2c_adap;
+        struct edid *edid;
+        struct drm_display_mode *mode, *t;
+        int i = 0, ret = 0;
+
+        i2c_adap = i2c_get_adapter(3);
+        if (i2c_adap == NULL) {
+                DRM_ERROR("No ddc adapter available!\n");
+                edid = (struct edid *)raw_edid;
+        } else {
+                edid = (struct edid *)raw_edid;
+                /* FIXME ? edid = drm_get_edid(connector, i2c_adap); */
+        }
+
+        if (edid) {
+                drm_mode_connector_update_edid_property(connector, edid);
+                ret = drm_add_edid_modes(connector, edid);
+                connector->display_info.raw_edid = NULL;
+        }
+
+        /*
+         * prune modes that require frame buffer bigger than stolen mem
+         */
+        list_for_each_entry_safe(mode, t, &connector->probed_modes, head) {
+                if ((mode->hdisplay * mode->vdisplay * 4) >= dev_priv->vram_stolen_size) {
+                        i++;
+                        drm_mode_remove(connector, mode);
+                }
+        }
+        return ret - i;
+}
+
+static void mrst_hdmi_mode_set(struct drm_encoder *encoder,
+                               struct drm_display_mode *mode,
+                               struct drm_display_mode *adjusted_mode)
+{
+        struct drm_device *dev = encoder->dev;
+
+        mrst_hdmi_audio_enable(dev);
+        return;
+}
+
+static void mrst_hdmi_destroy(struct drm_connector *connector)
+{
+        return;
+}
+
+static const struct drm_encoder_helper_funcs mrst_hdmi_helper_funcs = {
+        .dpms = mrst_hdmi_dpms,
+        .mode_fixup = mrst_hdmi_mode_fixup,
+        .prepare = psb_intel_encoder_prepare,
+        .mode_set = mrst_hdmi_mode_set,
+        .commit = psb_intel_encoder_commit,
+};
+
+static const struct drm_connector_helper_funcs
+                                        mrst_hdmi_connector_helper_funcs = {
+        .get_modes = mrst_hdmi_get_modes,
+        .mode_valid = mrst_hdmi_mode_valid,
+        .best_encoder = psb_intel_best_encoder,
+};
+
+static const struct drm_connector_funcs mrst_hdmi_connector_funcs = {
+        .dpms = drm_helper_connector_dpms,
+        .detect = mrst_hdmi_detect,
+        .fill_modes = drm_helper_probe_single_connector_modes,
+        .destroy = mrst_hdmi_destroy,
+};
+
+static void mrst_hdmi_enc_destroy(struct drm_encoder *encoder)
+{
+        drm_encoder_cleanup(encoder);
+}
+
+static const struct drm_encoder_funcs mrst_hdmi_enc_funcs = {
+        .destroy = mrst_hdmi_enc_destroy,
+};
+
+void mrst_hdmi_init(struct drm_device *dev,
+                                        struct psb_intel_mode_device *mode_dev)
+{
+        struct psb_intel_output *psb_intel_output;
+        struct drm_connector *connector;
+        struct drm_encoder *encoder;
+
+        psb_intel_output = kzalloc(sizeof(struct psb_intel_output), GFP_KERNEL);
+        if (!psb_intel_output)
+                return;
+
+        psb_intel_output->mode_dev = mode_dev;
+        connector = &psb_intel_output->base;
+        encoder = &psb_intel_output->enc;
+        drm_connector_init(dev, &psb_intel_output->base,
+                           &mrst_hdmi_connector_funcs,
+                           DRM_MODE_CONNECTOR_DVID);
+
+        drm_encoder_init(dev, &psb_intel_output->enc,
+                         &mrst_hdmi_enc_funcs,
+                         DRM_MODE_ENCODER_TMDS);
+
+        drm_mode_connector_attach_encoder(&psb_intel_output->base,
+                                          &psb_intel_output->enc);
+
+        psb_intel_output->type = INTEL_OUTPUT_HDMI;
+        drm_encoder_helper_add(encoder, &mrst_hdmi_helper_funcs);
+        drm_connector_helper_add(connector, &mrst_hdmi_connector_helper_funcs);
+
+        connector->display_info.subpixel_order = SubPixelHorizontalRGB;
+        connector->interlace_allowed = false;
+        connector->doublescan_allowed = false;
+        drm_sysfs_connector_add(connector);
+
+        return;
+}
+
+static DEFINE_PCI_DEVICE_TABLE(hdmi_ids) = {
+        { PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x080d) },
+        {}
+};
+
+void mrst_hdmi_setup(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct pci_dev *pdev;
+        struct mrst_hdmi_dev *hdmi_dev;
+        int ret;
+
+        pdev = pci_get_device(PCI_VENDOR_ID_INTEL, 0x080d, NULL);
+        if (!pdev)
+                return;
+
+        hdmi_dev = kzalloc(sizeof(struct mrst_hdmi_dev), GFP_KERNEL);
+        if (!hdmi_dev) {
+                dev_err(dev->dev, "failed to allocate memory\n");
+                goto out;
+        }
+
+
+        ret = pci_enable_device(pdev);
+        if (ret) {
+                dev_err(dev->dev, "failed to enable hdmi controller\n");
+                goto free;
+        }
+
+        hdmi_dev->mmio = pci_resource_start(pdev, 0);
+        hdmi_dev->mmio_len = pci_resource_len(pdev, 0);
+        hdmi_dev->regs = ioremap(hdmi_dev->mmio, hdmi_dev->mmio_len);
+        if (!hdmi_dev->regs) {
+                dev_err(dev->dev, "failed to map hdmi mmio\n");
+                goto free;
+        }
+
+        hdmi_dev->dev = pdev;
+        pci_set_drvdata(pdev, hdmi_dev);
+
+        /* Initialize i2c controller */
+        ret = mrst_hdmi_i2c_init(hdmi_dev->dev);
+        if (ret)
+                dev_err(dev->dev, "HDMI I2C initialization failed\n");
+
+        dev_priv->hdmi_priv = hdmi_dev;
+        mrst_hdmi_audio_disable(dev);
+        return;
+
+free:
+        kfree(hdmi_dev);
+out:
+        return;
+}
+
+void mrst_hdmi_teardown(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+        struct pci_dev *pdev;
+
+        if (hdmi_dev) {
+                pdev = hdmi_dev->dev;
+                pci_set_drvdata(pdev, NULL);
+                mrst_hdmi_i2c_exit(pdev);
+                iounmap(hdmi_dev->regs);
+                kfree(hdmi_dev);
+                pci_dev_put(pdev);
+        }
+}
+
+/* save HDMI register state */
+void mrst_hdmi_save(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+        int i;
+
+        /* dpll */
+        hdmi_dev->saveDPLL_CTRL = PSB_RVDC32(DPLL_CTRL);
+        hdmi_dev->saveDPLL_DIV_CTRL = PSB_RVDC32(DPLL_DIV_CTRL);
+        hdmi_dev->saveDPLL_ADJUST = PSB_RVDC32(DPLL_ADJUST);
+        hdmi_dev->saveDPLL_UPDATE = PSB_RVDC32(DPLL_UPDATE);
+        hdmi_dev->saveDPLL_CLK_ENABLE = PSB_RVDC32(DPLL_CLK_ENABLE);
+
+        /* pipe B */
+        dev_priv->savePIPEBCONF = PSB_RVDC32(PIPEBCONF);
+        dev_priv->savePIPEBSRC  = PSB_RVDC32(PIPEBSRC);
+        dev_priv->saveHTOTAL_B  = PSB_RVDC32(HTOTAL_B);
+        dev_priv->saveHBLANK_B  = PSB_RVDC32(HBLANK_B);
+        dev_priv->saveHSYNC_B   = PSB_RVDC32(HSYNC_B);
+        dev_priv->saveVTOTAL_B  = PSB_RVDC32(VTOTAL_B);
+        dev_priv->saveVBLANK_B  = PSB_RVDC32(VBLANK_B);
+        dev_priv->saveVSYNC_B   = PSB_RVDC32(VSYNC_B);
+
+        hdmi_dev->savePCH_PIPEBCONF = PSB_RVDC32(PCH_PIPEBCONF);
+        hdmi_dev->savePCH_PIPEBSRC = PSB_RVDC32(PCH_PIPEBSRC);
+        hdmi_dev->savePCH_HTOTAL_B = PSB_RVDC32(PCH_HTOTAL_B);
+        hdmi_dev->savePCH_HBLANK_B = PSB_RVDC32(PCH_HBLANK_B);
+        hdmi_dev->savePCH_HSYNC_B  = PSB_RVDC32(PCH_HSYNC_B);
+        hdmi_dev->savePCH_VTOTAL_B = PSB_RVDC32(PCH_VTOTAL_B);
+        hdmi_dev->savePCH_VBLANK_B = PSB_RVDC32(PCH_VBLANK_B);
+        hdmi_dev->savePCH_VSYNC_B  = PSB_RVDC32(PCH_VSYNC_B);
+
+        /* plane */
+        dev_priv->saveDSPBCNTR = PSB_RVDC32(DSPBCNTR);
+        dev_priv->saveDSPBSTRIDE = PSB_RVDC32(DSPBSTRIDE);
+        dev_priv->saveDSPBADDR = PSB_RVDC32(DSPBBASE);
+        dev_priv->saveDSPBSURF = PSB_RVDC32(DSPBSURF);
+        dev_priv->saveDSPBLINOFF = PSB_RVDC32(DSPBLINOFF);
+        dev_priv->saveDSPBTILEOFF = PSB_RVDC32(DSPBTILEOFF);
+
+        /* cursor B */
+        dev_priv->saveDSPBCURSOR_CTRL = PSB_RVDC32(CURBCNTR);
+        dev_priv->saveDSPBCURSOR_BASE = PSB_RVDC32(CURBBASE);
+        dev_priv->saveDSPBCURSOR_POS = PSB_RVDC32(CURBPOS);
+
+        /* save palette */
+        for (i = 0; i < 256; i++)
+                dev_priv->save_palette_b[i] = PSB_RVDC32(PALETTE_B + (i << 2));
+}
+
+/* restore HDMI register state */
+void mrst_hdmi_restore(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_hdmi_dev *hdmi_dev = dev_priv->hdmi_priv;
+        int i;
+
+        /* dpll */
+        PSB_WVDC32(hdmi_dev->saveDPLL_CTRL, DPLL_CTRL);
+        PSB_WVDC32(hdmi_dev->saveDPLL_DIV_CTRL, DPLL_DIV_CTRL);
+        PSB_WVDC32(hdmi_dev->saveDPLL_ADJUST, DPLL_ADJUST);
+        PSB_WVDC32(hdmi_dev->saveDPLL_UPDATE, DPLL_UPDATE);
+        PSB_WVDC32(hdmi_dev->saveDPLL_CLK_ENABLE, DPLL_CLK_ENABLE);
+        DRM_UDELAY(150);
+
+        /* pipe */
+        PSB_WVDC32(dev_priv->savePIPEBSRC, PIPEBSRC);
+        PSB_WVDC32(dev_priv->saveHTOTAL_B, HTOTAL_B);
+        PSB_WVDC32(dev_priv->saveHBLANK_B, HBLANK_B);
+        PSB_WVDC32(dev_priv->saveHSYNC_B,  HSYNC_B);
+        PSB_WVDC32(dev_priv->saveVTOTAL_B, VTOTAL_B);
+        PSB_WVDC32(dev_priv->saveVBLANK_B, VBLANK_B);
+        PSB_WVDC32(dev_priv->saveVSYNC_B,  VSYNC_B);
+
+        PSB_WVDC32(hdmi_dev->savePCH_PIPEBSRC, PCH_PIPEBSRC);
+        PSB_WVDC32(hdmi_dev->savePCH_HTOTAL_B, PCH_HTOTAL_B);
+        PSB_WVDC32(hdmi_dev->savePCH_HBLANK_B, PCH_HBLANK_B);
+        PSB_WVDC32(hdmi_dev->savePCH_HSYNC_B,  PCH_HSYNC_B);
+        PSB_WVDC32(hdmi_dev->savePCH_VTOTAL_B, PCH_VTOTAL_B);
+        PSB_WVDC32(hdmi_dev->savePCH_VBLANK_B, PCH_VBLANK_B);
+        PSB_WVDC32(hdmi_dev->savePCH_VSYNC_B,  PCH_VSYNC_B);
+
+        PSB_WVDC32(dev_priv->savePIPEBCONF, PIPEBCONF);
+        PSB_WVDC32(hdmi_dev->savePCH_PIPEBCONF, PCH_PIPEBCONF);
+
+        /* plane */
+        PSB_WVDC32(dev_priv->saveDSPBLINOFF, DSPBLINOFF);
+        PSB_WVDC32(dev_priv->saveDSPBSTRIDE, DSPBSTRIDE);
+        PSB_WVDC32(dev_priv->saveDSPBTILEOFF, DSPBTILEOFF);
+        PSB_WVDC32(dev_priv->saveDSPBCNTR, DSPBCNTR);
+        PSB_WVDC32(dev_priv->saveDSPBSURF, DSPBSURF);
+
+        /* cursor B */
+        PSB_WVDC32(dev_priv->saveDSPBCURSOR_CTRL, CURBCNTR);
+        PSB_WVDC32(dev_priv->saveDSPBCURSOR_POS, CURBPOS);
+        PSB_WVDC32(dev_priv->saveDSPBCURSOR_BASE, CURBBASE);
+
+        /* restore palette */
+        for (i = 0; i < 256; i++)
+                PSB_WVDC32(dev_priv->save_palette_b[i], PALETTE_B + (i << 2));
+}
diff --git a/drivers/staging/gma500/mrst_hdmi_i2c.c b/drivers/staging/gma500/mrst_hdmi_i2c.c
new file mode 100644
index 00000000000..351b9d897b9
--- /dev/null
+++ b/drivers/staging/gma500/mrst_hdmi_i2c.c
@@ -0,0 +1,327 @@
+/*
+ * Copyright © 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:
+ *      Li Peng <peng.li@intel.com>
+ */
+
+#include <linux/mutex.h>
+#include <linux/pci.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include "psb_drv.h"
+
+#define HDMI_READ(reg)          readl(hdmi_dev->regs + (reg))
+#define HDMI_WRITE(reg, val)    writel(val, hdmi_dev->regs + (reg))
+
+#define HDMI_HCR        0x1000
+#define HCR_DETECT_HDP          (1 << 6)
+#define HCR_ENABLE_HDCP         (1 << 5)
+#define HCR_ENABLE_AUDIO        (1 << 2)
+#define HCR_ENABLE_PIXEL        (1 << 1)
+#define HCR_ENABLE_TMDS         (1 << 0)
+#define HDMI_HICR       0x1004
+#define HDMI_INTR_I2C_ERROR     (1 << 4)
+#define HDMI_INTR_I2C_FULL      (1 << 3)
+#define HDMI_INTR_I2C_DONE      (1 << 2)
+#define HDMI_INTR_HPD           (1 << 0)
+#define HDMI_HSR        0x1008
+#define HDMI_HISR       0x100C
+#define HDMI_HI2CRDB0   0x1200
+#define HDMI_HI2CHCR    0x1240
+#define HI2C_HDCP_WRITE         (0 << 2)
+#define HI2C_HDCP_RI_READ       (1 << 2)
+#define HI2C_HDCP_READ          (2 << 2)
+#define HI2C_EDID_READ          (3 << 2)
+#define HI2C_READ_CONTINUE      (1 << 1)
+#define HI2C_ENABLE_TRANSACTION (1 << 0)
+
+#define HDMI_ICRH       0x1100
+#define HDMI_HI2CTDR0   0x1244
+#define HDMI_HI2CTDR1   0x1248
+
+#define I2C_STAT_INIT           0
+#define I2C_READ_DONE           1
+#define I2C_TRANSACTION_DONE    2
+
+struct hdmi_i2c_dev {
+        struct i2c_adapter *adap;
+        struct mutex i2c_lock;
+        struct completion complete;
+        int status;
+        struct i2c_msg *msg;
+        int buf_offset;
+};
+
+static void hdmi_i2c_irq_enable(struct mrst_hdmi_dev *hdmi_dev)
+{
+        u32 temp;
+
+        temp = HDMI_READ(HDMI_HICR);
+        temp |= (HDMI_INTR_I2C_ERROR | HDMI_INTR_I2C_FULL | HDMI_INTR_I2C_DONE);
+        HDMI_WRITE(HDMI_HICR, temp);
+        HDMI_READ(HDMI_HICR);
+}
+
+static void hdmi_i2c_irq_disable(struct mrst_hdmi_dev *hdmi_dev)
+{
+        HDMI_WRITE(HDMI_HICR, 0x0);
+        HDMI_READ(HDMI_HICR);
+}
+
+static int xfer_read(struct i2c_adapter *adap, struct i2c_msg *pmsg)
+{
+        struct mrst_hdmi_dev *hdmi_dev = i2c_get_adapdata(adap);
+        struct hdmi_i2c_dev *i2c_dev = hdmi_dev->i2c_dev;
+        u32 temp;
+
+        i2c_dev->status = I2C_STAT_INIT;
+        i2c_dev->msg = pmsg;
+        i2c_dev->buf_offset = 0;
+        INIT_COMPLETION(i2c_dev->complete);
+
+        /* Enable I2C transaction */
+        temp = ((pmsg->len) << 20) | HI2C_EDID_READ | HI2C_ENABLE_TRANSACTION;
+        HDMI_WRITE(HDMI_HI2CHCR, temp);
+        HDMI_READ(HDMI_HI2CHCR);
+
+        while (i2c_dev->status != I2C_TRANSACTION_DONE)
+                wait_for_completion_interruptible_timeout(&i2c_dev->complete,
+                                                                10 * HZ);
+
+        return 0;
+}
+
+static int xfer_write(struct i2c_adapter *adap, struct i2c_msg *pmsg)
+{
+        /*
+         * XXX: i2c write seems isn't useful for EDID probe, don't do anything
+         */
+        return 0;
+}
+
+static int mrst_hdmi_i2c_access(struct i2c_adapter *adap,
+                                struct i2c_msg *pmsg,
+                                int num)
+{
+        struct mrst_hdmi_dev *hdmi_dev = i2c_get_adapdata(adap);
+        struct hdmi_i2c_dev *i2c_dev = hdmi_dev->i2c_dev;
+        int i, err = 0;
+
+        mutex_lock(&i2c_dev->i2c_lock);
+
+        /* Enable i2c unit */
+        HDMI_WRITE(HDMI_ICRH, 0x00008760);
+
+        /* Enable irq */
+        hdmi_i2c_irq_enable(hdmi_dev);
+        for (i = 0; i < num; i++) {
+                if (pmsg->len && pmsg->buf) {
+                        if (pmsg->flags & I2C_M_RD)
+                                err = xfer_read(adap, pmsg);
+                        else
+                                err = xfer_write(adap, pmsg);
+                }
+                pmsg++;         /* next message */
+        }
+
+        /* Disable irq */
+        hdmi_i2c_irq_disable(hdmi_dev);
+
+        mutex_unlock(&i2c_dev->i2c_lock);
+
+        return i;
+}
+
+static u32 mrst_hdmi_i2c_func(struct i2c_adapter *adapter)
+{
+        return I2C_FUNC_I2C | I2C_FUNC_10BIT_ADDR;
+}
+
+static const struct i2c_algorithm mrst_hdmi_i2c_algorithm = {
+        .master_xfer    = mrst_hdmi_i2c_access,
+        .functionality  = mrst_hdmi_i2c_func,
+};
+
+static struct i2c_adapter mrst_hdmi_i2c_adapter = {
+        .name           = "mrst_hdmi_i2c",
+        .nr             = 3,
+        .owner          = THIS_MODULE,
+        .class          = I2C_CLASS_DDC,
+        .algo           = &mrst_hdmi_i2c_algorithm,
+};
+
+static void hdmi_i2c_read(struct mrst_hdmi_dev *hdmi_dev)
+{
+        struct hdmi_i2c_dev *i2c_dev = hdmi_dev->i2c_dev;
+        struct i2c_msg *msg = i2c_dev->msg;
+        u8 *buf = msg->buf;
+        u32 temp;
+        int i, offset;
+
+        offset = i2c_dev->buf_offset;
+        for (i = 0; i < 0x10; i++) {
+                temp = HDMI_READ(HDMI_HI2CRDB0 + (i * 4));
+                memcpy(buf + (offset + i * 4), &temp, 4);
+        }
+        i2c_dev->buf_offset += (0x10 * 4);
+
+        /* clearing read buffer full intr */
+        temp = HDMI_READ(HDMI_HISR);
+        HDMI_WRITE(HDMI_HISR, temp | HDMI_INTR_I2C_FULL);
+        HDMI_READ(HDMI_HISR);
+
+        /* continue read transaction */
+        temp = HDMI_READ(HDMI_HI2CHCR);
+        HDMI_WRITE(HDMI_HI2CHCR, temp | HI2C_READ_CONTINUE);
+        HDMI_READ(HDMI_HI2CHCR);
+
+        i2c_dev->status = I2C_READ_DONE;
+        return;
+}
+
+static void hdmi_i2c_transaction_done(struct mrst_hdmi_dev *hdmi_dev)
+{
+        struct hdmi_i2c_dev *i2c_dev = hdmi_dev->i2c_dev;
+        u32 temp;
+
+        /* clear transaction done intr */
+        temp = HDMI_READ(HDMI_HISR);
+        HDMI_WRITE(HDMI_HISR, temp | HDMI_INTR_I2C_DONE);
+        HDMI_READ(HDMI_HISR);
+
+
+        temp = HDMI_READ(HDMI_HI2CHCR);
+        HDMI_WRITE(HDMI_HI2CHCR, temp & ~HI2C_ENABLE_TRANSACTION);
+        HDMI_READ(HDMI_HI2CHCR);
+
+        i2c_dev->status = I2C_TRANSACTION_DONE;
+        return;
+}
+
+static irqreturn_t mrst_hdmi_i2c_handler(int this_irq, void *dev)
+{
+        struct mrst_hdmi_dev *hdmi_dev = dev;
+        struct hdmi_i2c_dev *i2c_dev = hdmi_dev->i2c_dev;
+        u32 stat;
+
+        stat = HDMI_READ(HDMI_HISR);
+
+        if (stat & HDMI_INTR_HPD) {
+                HDMI_WRITE(HDMI_HISR, stat | HDMI_INTR_HPD);
+                HDMI_READ(HDMI_HISR);
+        }
+
+        if (stat & HDMI_INTR_I2C_FULL)
+                hdmi_i2c_read(hdmi_dev);
+
+        if (stat & HDMI_INTR_I2C_DONE)
+                hdmi_i2c_transaction_done(hdmi_dev);
+
+        complete(&i2c_dev->complete);
+
+        return IRQ_HANDLED;
+}
+
+/*
+ * choose alternate function 2 of GPIO pin 52, 53,
+ * which is used by HDMI I2C logic
+ */
+static void mrst_hdmi_i2c_gpio_fix(void)
+{
+        void *base;
+        unsigned int gpio_base = 0xff12c000;
+        int gpio_len = 0x1000;
+        u32 temp;
+
+        base = ioremap((resource_size_t)gpio_base, gpio_len);
+        if (base == NULL) {
+                DRM_ERROR("gpio ioremap fail\n");
+                return;
+        }
+
+        temp = readl(base + 0x44);
+        DRM_DEBUG_DRIVER("old gpio val %x\n", temp);
+        writel((temp | 0x00000a00), (base +  0x44));
+        temp = readl(base + 0x44);
+        DRM_DEBUG_DRIVER("new gpio val %x\n", temp);
+
+        iounmap(base);
+}
+
+int mrst_hdmi_i2c_init(struct pci_dev *dev)
+{
+        struct mrst_hdmi_dev *hdmi_dev;
+        struct hdmi_i2c_dev *i2c_dev;
+        int ret;
+
+        hdmi_dev = pci_get_drvdata(dev);
+
+        i2c_dev = kzalloc(sizeof(struct hdmi_i2c_dev), GFP_KERNEL);
+        if (i2c_dev == NULL) {
+                DRM_ERROR("Can't allocate interface\n");
+                ret = -ENOMEM;
+                goto exit;
+        }
+
+        i2c_dev->adap = &mrst_hdmi_i2c_adapter;
+        i2c_dev->status = I2C_STAT_INIT;
+        init_completion(&i2c_dev->complete);
+        mutex_init(&i2c_dev->i2c_lock);
+        i2c_set_adapdata(&mrst_hdmi_i2c_adapter, hdmi_dev);
+        hdmi_dev->i2c_dev = i2c_dev;
+
+        /* Enable HDMI I2C function on gpio */
+        mrst_hdmi_i2c_gpio_fix();
+
+        /* request irq */
+        ret = request_irq(dev->irq, mrst_hdmi_i2c_handler, IRQF_SHARED,
+                          mrst_hdmi_i2c_adapter.name, hdmi_dev);
+        if (ret) {
+                DRM_ERROR("Failed to request IRQ for I2C controller\n");
+                goto err;
+        }
+
+        /* Adapter registration */
+        ret = i2c_add_numbered_adapter(&mrst_hdmi_i2c_adapter);
+        return ret;
+
+err:
+        kfree(i2c_dev);
+exit:
+        return ret;
+}
+
+void mrst_hdmi_i2c_exit(struct pci_dev *dev)
+{
+        struct mrst_hdmi_dev *hdmi_dev;
+        struct hdmi_i2c_dev *i2c_dev;
+
+        hdmi_dev = pci_get_drvdata(dev);
+        if (i2c_del_adapter(&mrst_hdmi_i2c_adapter))
+                DRM_DEBUG_DRIVER("Failed to delete hdmi-i2c adapter\n");
+
+        i2c_dev = hdmi_dev->i2c_dev;
+        kfree(i2c_dev);
+        free_irq(dev->irq, hdmi_dev);
+}
diff --git a/drivers/staging/gma500/mrst_lvds.c b/drivers/staging/gma500/mrst_lvds.c
new file mode 100644
index 00000000000..e7999a2a379
--- /dev/null
+++ b/drivers/staging/gma500/mrst_lvds.c
@@ -0,0 +1,407 @@
+/*
+ * Copyright © 2006-2009 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors:
+ *      Eric Anholt <eric@anholt.net>
+ *      Dave Airlie <airlied@linux.ie>
+ *      Jesse Barnes <jesse.barnes@intel.com>
+ */
+
+#include <linux/i2c.h>
+#include <drm/drmP.h>
+#include <asm/mrst.h>
+
+#include "intel_bios.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "power.h"
+#include <linux/pm_runtime.h>
+
+/* The max/min PWM frequency in BPCR[31:17] - */
+/* The smallest number is 1 (not 0) that can fit in the
+ * 15-bit field of the and then*/
+/* shifts to the left by one bit to get the actual 16-bit
+ * value that the 15-bits correspond to.*/
+#define MRST_BLC_MAX_PWM_REG_FREQ           0xFFFF
+#define BRIGHTNESS_MAX_LEVEL 100
+
+/**
+ * Sets the power state for the panel.
+ */
+static void mrst_lvds_set_power(struct drm_device *dev,
+                                struct psb_intel_output *output, bool on)
+{
+        u32 pp_status;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (!gma_power_begin(dev, true))
+                return;
+
+        if (on) {
+                REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) |
+                          POWER_TARGET_ON);
+                do {
+                        pp_status = REG_READ(PP_STATUS);
+                } while ((pp_status & (PP_ON | PP_READY)) == PP_READY);
+                dev_priv->is_lvds_on = true;
+                if (dev_priv->ops->lvds_bl_power)
+                        dev_priv->ops->lvds_bl_power(dev, true);
+        } else {
+                if (dev_priv->ops->lvds_bl_power)
+                        dev_priv->ops->lvds_bl_power(dev, false);
+                REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) &
+                          ~POWER_TARGET_ON);
+                do {
+                        pp_status = REG_READ(PP_STATUS);
+                } while (pp_status & PP_ON);
+                dev_priv->is_lvds_on = false;
+                pm_request_idle(&dev->pdev->dev);
+        }
+        gma_power_end(dev);
+}
+
+static void mrst_lvds_dpms(struct drm_encoder *encoder, int mode)
+{
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+
+        if (mode == DRM_MODE_DPMS_ON)
+                mrst_lvds_set_power(dev, output, true);
+        else
+                mrst_lvds_set_power(dev, output, false);
+
+        /* XXX: We never power down the LVDS pairs. */
+}
+
+static void mrst_lvds_mode_set(struct drm_encoder *encoder,
+                               struct drm_display_mode *mode,
+                               struct drm_display_mode *adjusted_mode)
+{
+        struct psb_intel_mode_device *mode_dev =
+                                enc_to_psb_intel_output(encoder)->mode_dev;
+        struct drm_device *dev = encoder->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 lvds_port;
+        uint64_t v = DRM_MODE_SCALE_FULLSCREEN;
+
+        if (!gma_power_begin(dev, true))
+                return;
+
+        /*
+         * The LVDS pin pair will already have been turned on in the
+         * psb_intel_crtc_mode_set since it has a large impact on the DPLL
+         * settings.
+         */
+        lvds_port = (REG_READ(LVDS) &
+                    (~LVDS_PIPEB_SELECT)) |
+                    LVDS_PORT_EN |
+                    LVDS_BORDER_EN;
+
+        /* If the firmware says dither on Moorestown, or the BIOS does
+           on Oaktrail then enable dithering */
+        if (mode_dev->panel_wants_dither || dev_priv->lvds_dither)
+                lvds_port |= MRST_PANEL_8TO6_DITHER_ENABLE;
+
+        REG_WRITE(LVDS, lvds_port);
+
+        drm_connector_property_get_value(
+                &enc_to_psb_intel_output(encoder)->base,
+                dev->mode_config.scaling_mode_property,
+                &v);
+
+        if (v == DRM_MODE_SCALE_NO_SCALE)
+                REG_WRITE(PFIT_CONTROL, 0);
+        else if (v == DRM_MODE_SCALE_ASPECT) {
+                if ((mode->vdisplay != adjusted_mode->crtc_vdisplay) ||
+                    (mode->hdisplay != adjusted_mode->crtc_hdisplay)) {
+                        if ((adjusted_mode->crtc_hdisplay * mode->vdisplay) ==
+                            (mode->hdisplay * adjusted_mode->crtc_vdisplay))
+                                REG_WRITE(PFIT_CONTROL, PFIT_ENABLE);
+                        else if ((adjusted_mode->crtc_hdisplay *
+                                mode->vdisplay) > (mode->hdisplay *
+                                adjusted_mode->crtc_vdisplay))
+                                REG_WRITE(PFIT_CONTROL, PFIT_ENABLE |
+                                          PFIT_SCALING_MODE_PILLARBOX);
+                        else
+                                REG_WRITE(PFIT_CONTROL, PFIT_ENABLE |
+                                          PFIT_SCALING_MODE_LETTERBOX);
+                } else
+                        REG_WRITE(PFIT_CONTROL, PFIT_ENABLE);
+        } else /*(v == DRM_MODE_SCALE_FULLSCREEN)*/
+                REG_WRITE(PFIT_CONTROL, PFIT_ENABLE);
+
+        gma_power_end(dev);
+}
+
+static void mrst_lvds_prepare(struct drm_encoder *encoder)
+{
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+        struct psb_intel_mode_device *mode_dev = output->mode_dev;
+
+        if (!gma_power_begin(dev, true))
+                return;
+
+        mode_dev->saveBLC_PWM_CTL = REG_READ(BLC_PWM_CTL);
+        mode_dev->backlight_duty_cycle = (mode_dev->saveBLC_PWM_CTL &
+                                          BACKLIGHT_DUTY_CYCLE_MASK);
+        mrst_lvds_set_power(dev, output, false);
+        gma_power_end(dev);
+}
+
+static u32 mrst_lvds_get_max_backlight(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 ret;
+
+        if (gma_power_begin(dev, false)) {
+                ret = ((REG_READ(BLC_PWM_CTL) &
+                          BACKLIGHT_MODULATION_FREQ_MASK) >>
+                          BACKLIGHT_MODULATION_FREQ_SHIFT) * 2;
+
+                gma_power_end(dev);
+        } else
+                ret = ((dev_priv->saveBLC_PWM_CTL &
+                          BACKLIGHT_MODULATION_FREQ_MASK) >>
+                          BACKLIGHT_MODULATION_FREQ_SHIFT) * 2;
+
+        return ret;
+}
+
+static void mrst_lvds_commit(struct drm_encoder *encoder)
+{
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+        struct psb_intel_mode_device *mode_dev = output->mode_dev;
+
+        if (mode_dev->backlight_duty_cycle == 0)
+                mode_dev->backlight_duty_cycle =
+                                        mrst_lvds_get_max_backlight(dev);
+        mrst_lvds_set_power(dev, output, true);
+}
+
+static const struct drm_encoder_helper_funcs mrst_lvds_helper_funcs = {
+        .dpms = mrst_lvds_dpms,
+        .mode_fixup = psb_intel_lvds_mode_fixup,
+        .prepare = mrst_lvds_prepare,
+        .mode_set = mrst_lvds_mode_set,
+        .commit = mrst_lvds_commit,
+};
+
+static struct drm_display_mode lvds_configuration_modes[] = {
+        /* hard coded fixed mode for TPO LTPS LPJ040K001A */
+        { DRM_MODE("800x480",  DRM_MODE_TYPE_DRIVER, 33264, 800, 836,
+                   846, 1056, 0, 480, 489, 491, 525, 0, 0) },
+        /* hard coded fixed mode for LVDS 800x480 */
+        { DRM_MODE("800x480",  DRM_MODE_TYPE_DRIVER, 30994, 800, 801,
+                   802, 1024, 0, 480, 481, 482, 525, 0, 0) },
+        /* hard coded fixed mode for Samsung 480wsvga LVDS 1024x600@75 */
+        { DRM_MODE("1024x600", DRM_MODE_TYPE_DRIVER, 53990, 1024, 1072,
+                   1104, 1184, 0, 600, 603, 604, 608, 0, 0) },
+        /* hard coded fixed mode for Samsung 480wsvga LVDS 1024x600@75 */
+        { DRM_MODE("1024x600", DRM_MODE_TYPE_DRIVER, 53990, 1024, 1104,
+                   1136, 1184, 0, 600, 603, 604, 608, 0, 0) },
+        /* hard coded fixed mode for Sharp wsvga LVDS 1024x600 */
+        { DRM_MODE("1024x600", DRM_MODE_TYPE_DRIVER, 48885, 1024, 1124,
+                   1204, 1312, 0, 600, 607, 610, 621, 0, 0) },
+        /* hard coded fixed mode for LVDS 1024x768 */
+        { DRM_MODE("1024x768", DRM_MODE_TYPE_DRIVER, 65000, 1024, 1048,
+                   1184, 1344, 0, 768, 771, 777, 806, 0, 0) },
+        /* hard coded fixed mode for LVDS 1366x768 */
+        { DRM_MODE("1366x768", DRM_MODE_TYPE_DRIVER, 77500, 1366, 1430,
+                   1558, 1664, 0, 768, 769, 770, 776, 0, 0) },
+};
+
+/* Returns the panel fixed mode from configuration. */
+
+static struct drm_display_mode *
+mrst_lvds_get_configuration_mode(struct drm_device *dev)
+{
+        struct drm_display_mode *mode = NULL;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct mrst_timing_info *ti = &dev_priv->gct_data.DTD;
+
+        if (dev_priv->vbt_data.size != 0x00) { /*if non-zero, then use vbt*/
+                mode = kzalloc(sizeof(*mode), GFP_KERNEL);
+                if (!mode)
+                        return NULL;
+
+                mode->hdisplay = (ti->hactive_hi << 8) | ti->hactive_lo;
+                mode->vdisplay = (ti->vactive_hi << 8) | ti->vactive_lo;
+                mode->hsync_start = mode->hdisplay + \
+                                ((ti->hsync_offset_hi << 8) | \
+                                ti->hsync_offset_lo);
+                mode->hsync_end = mode->hsync_start + \
+                                ((ti->hsync_pulse_width_hi << 8) | \
+                                ti->hsync_pulse_width_lo);
+                mode->htotal = mode->hdisplay + ((ti->hblank_hi << 8) | \
+                                                        ti->hblank_lo);
+                mode->vsync_start = \
+                        mode->vdisplay + ((ti->vsync_offset_hi << 4) | \
+                                                ti->vsync_offset_lo);
+                mode->vsync_end = \
+                        mode->vsync_start + ((ti->vsync_pulse_width_hi << 4) | \
+                                                ti->vsync_pulse_width_lo);
+                mode->vtotal = mode->vdisplay + \
+                                ((ti->vblank_hi << 8) | ti->vblank_lo);
+                mode->clock = ti->pixel_clock * 10;
+#if 0
+                printk(KERN_INFO "hdisplay is %d\n", mode->hdisplay);
+                printk(KERN_INFO "vdisplay is %d\n", mode->vdisplay);
+                printk(KERN_INFO "HSS is %d\n", mode->hsync_start);
+                printk(KERN_INFO "HSE is %d\n", mode->hsync_end);
+                printk(KERN_INFO "htotal is %d\n", mode->htotal);
+                printk(KERN_INFO "VSS is %d\n", mode->vsync_start);
+                printk(KERN_INFO "VSE is %d\n", mode->vsync_end);
+                printk(KERN_INFO "vtotal is %d\n", mode->vtotal);
+                printk(KERN_INFO "clock is %d\n", mode->clock);
+#endif
+        } else
+                mode = drm_mode_duplicate(dev, &lvds_configuration_modes[2]);
+
+        drm_mode_set_name(mode);
+        drm_mode_set_crtcinfo(mode, 0);
+
+        return mode;
+}
+
+/**
+ * mrst_lvds_init - setup LVDS connectors on this device
+ * @dev: drm device
+ *
+ * Create the connector, register the LVDS DDC bus, and try to figure out what
+ * modes we can display on the LVDS panel (if present).
+ */
+void mrst_lvds_init(struct drm_device *dev,
+                    struct psb_intel_mode_device *mode_dev)
+{
+        struct psb_intel_output *psb_intel_output;
+        struct drm_connector *connector;
+        struct drm_encoder *encoder;
+        struct drm_psb_private *dev_priv =
+                                (struct drm_psb_private *) dev->dev_private;
+        struct edid *edid;
+        int ret = 0;
+        struct i2c_adapter *i2c_adap;
+        struct drm_display_mode *scan;  /* *modes, *bios_mode; */
+
+        psb_intel_output = kzalloc(sizeof(struct psb_intel_output), GFP_KERNEL);
+        if (!psb_intel_output)
+                return;
+
+        psb_intel_output->mode_dev = mode_dev;
+        connector = &psb_intel_output->base;
+        encoder = &psb_intel_output->enc;
+        dev_priv->is_lvds_on = true;
+        drm_connector_init(dev, &psb_intel_output->base,
+                           &psb_intel_lvds_connector_funcs,
+                           DRM_MODE_CONNECTOR_LVDS);
+
+        drm_encoder_init(dev, &psb_intel_output->enc, &psb_intel_lvds_enc_funcs,
+                         DRM_MODE_ENCODER_LVDS);
+
+        drm_mode_connector_attach_encoder(&psb_intel_output->base,
+                                          &psb_intel_output->enc);
+        psb_intel_output->type = INTEL_OUTPUT_LVDS;
+
+        drm_encoder_helper_add(encoder, &mrst_lvds_helper_funcs);
+        drm_connector_helper_add(connector,
+                                 &psb_intel_lvds_connector_helper_funcs);
+        connector->display_info.subpixel_order = SubPixelHorizontalRGB;
+        connector->interlace_allowed = false;
+        connector->doublescan_allowed = false;
+
+        drm_connector_attach_property(connector,
+                                        dev->mode_config.scaling_mode_property,
+                                        DRM_MODE_SCALE_FULLSCREEN);
+        drm_connector_attach_property(connector,
+                                        dev_priv->backlight_property,
+                                        BRIGHTNESS_MAX_LEVEL);
+
+        mode_dev->panel_wants_dither = false;
+        if (dev_priv->vbt_data.size != 0x00)
+                mode_dev->panel_wants_dither = (dev_priv->gct_data.
+                        Panel_Port_Control & MRST_PANEL_8TO6_DITHER_ENABLE);
+
+        /*
+         * LVDS discovery:
+         * 1) check for EDID on DDC
+         * 2) check for VBT data
+         * 3) check to see if LVDS is already on
+         *    if none of the above, no panel
+         * 4) make sure lid is open
+         *    if closed, act like it's not there for now
+         */
+
+        i2c_adap = i2c_get_adapter(dev_priv->ops->i2c_bus);
+
+        if (i2c_adap == NULL)
+                dev_err(dev->dev, "No ddc adapter available!\n");
+        /*
+         * Attempt to get the fixed panel mode from DDC.  Assume that the
+         * preferred mode is the right one.
+         */
+        if (i2c_adap) {
+                edid = drm_get_edid(connector, i2c_adap);
+                if (edid) {
+                        drm_mode_connector_update_edid_property(connector,
+                                                                        edid);
+                        ret = drm_add_edid_modes(connector, edid);
+                        kfree(edid);
+                }
+
+                list_for_each_entry(scan, &connector->probed_modes, head) {
+                        if (scan->type & DRM_MODE_TYPE_PREFERRED) {
+                                mode_dev->panel_fixed_mode =
+                                    drm_mode_duplicate(dev, scan);
+                                goto out;       /* FIXME: check for quirks */
+                        }
+                }
+        }
+        /*
+         * If we didn't get EDID, try geting panel timing
+         * from configuration data
+         */
+        mode_dev->panel_fixed_mode = mrst_lvds_get_configuration_mode(dev);
+
+        if (mode_dev->panel_fixed_mode) {
+                mode_dev->panel_fixed_mode->type |= DRM_MODE_TYPE_PREFERRED;
+                goto out;       /* FIXME: check for quirks */
+        }
+
+        /* If we still don't have a mode after all that, give up. */
+        if (!mode_dev->panel_fixed_mode) {
+                dev_err(dev->dev, "Found no modes on the lvds, ignoring the LVDS\n");
+                goto failed_find;
+        }
+
+out:
+        drm_sysfs_connector_add(connector);
+        return;
+
+failed_find:
+        dev_dbg(dev->dev, "No LVDS modes found, disabling.\n");
+        if (psb_intel_output->ddc_bus)
+                psb_intel_i2c_destroy(psb_intel_output->ddc_bus);
+
+/* failed_ddc: */
+
+        drm_encoder_cleanup(encoder);
+        drm_connector_cleanup(connector);
+        kfree(connector);
+}
+
diff --git a/drivers/staging/gma500/power.c b/drivers/staging/gma500/power.c
new file mode 100644
index 00000000000..972bea7c1af
--- /dev/null
+++ b/drivers/staging/gma500/power.c
@@ -0,0 +1,316 @@
+/**************************************************************************
+ * Copyright (c) 2009-2011, Intel Corporation.
+ * 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, 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:
+ *    Benjamin Defnet <benjamin.r.defnet@intel.com>
+ *    Rajesh Poornachandran <rajesh.poornachandran@intel.com>
+ * Massively reworked
+ *    Alan Cox <alan@linux.intel.com>
+ */
+
+#include "power.h"
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+
+static struct mutex power_mutex;        /* Serialize power ops */
+static spinlock_t power_ctrl_lock;      /* Serialize power claim */
+
+/**
+ *      gma_power_init          -       initialise power manager
+ *      @dev: our device
+ *
+ *      Set up for power management tracking of our hardware.
+ */
+void gma_power_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        /* FIXME: Move APM/OSPM base into relevant device code */
+        dev_priv->apm_base = dev_priv->apm_reg & 0xffff;
+        dev_priv->ospm_base &= 0xffff;
+
+        dev_priv->display_power = true; /* We start active */
+        dev_priv->display_count = 0;    /* Currently no users */
+        dev_priv->suspended = false;    /* And not suspended */
+        spin_lock_init(&power_ctrl_lock);
+        mutex_init(&power_mutex);
+
+        dev_priv->ops->init_pm(dev);
+}
+
+/**
+ *      gma_power_uninit        -       end power manager
+ *      @dev: device to end for
+ *
+ *      Undo the effects of gma_power_init
+ */
+void gma_power_uninit(struct drm_device *dev)
+{
+        pm_runtime_disable(&dev->pdev->dev);
+        pm_runtime_set_suspended(&dev->pdev->dev);
+}
+
+/**
+ *      gma_suspend_display     -       suspend the display logic
+ *      @dev: our DRM device
+ *
+ *      Suspend the display logic of the graphics interface
+ */
+static void gma_suspend_display(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (dev_priv->suspended)
+                return;
+        dev_priv->ops->save_regs(dev);
+        dev_priv->ops->power_down(dev);
+        dev_priv->display_power = false;
+}
+
+/**
+ *      gma_resume_display      -       resume display side logic
+ *
+ *      Resume the display hardware restoring state and enabling
+ *      as necessary.
+ */
+static void gma_resume_display(struct pci_dev *pdev)
+{
+        struct drm_device *dev = pci_get_drvdata(pdev);
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (dev_priv->suspended == false)
+                return;
+
+        /* turn on the display power island */
+        dev_priv->ops->power_up(dev);
+        dev_priv->suspended = false;
+        dev_priv->display_power = true;
+
+        PSB_WVDC32(dev_priv->pge_ctl | _PSB_PGETBL_ENABLED, PSB_PGETBL_CTL);
+        pci_write_config_word(pdev, PSB_GMCH_CTRL,
+                        dev_priv->gmch_ctrl | _PSB_GMCH_ENABLED);
+        dev_priv->ops->restore_regs(dev);
+}
+
+/**
+ *      gma_suspend_pci         -       suspend PCI side
+ *      @pdev: PCI device
+ *
+ *      Perform the suspend processing on our PCI device state
+ */
+static void gma_suspend_pci(struct pci_dev *pdev)
+{
+        struct drm_device *dev = pci_get_drvdata(pdev);
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int bsm, vbt;
+
+        if (dev_priv->suspended)
+                return;
+
+        pci_save_state(pdev);
+        pci_read_config_dword(pdev, 0x5C, &bsm);
+        dev_priv->saveBSM = bsm;
+        pci_read_config_dword(pdev, 0xFC, &vbt);
+        dev_priv->saveVBT = vbt;
+        pci_read_config_dword(pdev, PSB_PCIx_MSI_ADDR_LOC, &dev_priv->msi_addr);
+        pci_read_config_dword(pdev, PSB_PCIx_MSI_DATA_LOC, &dev_priv->msi_data);
+
+        pci_disable_device(pdev);
+        pci_set_power_state(pdev, PCI_D3hot);
+
+        dev_priv->suspended = true;
+}
+
+/**
+ *      gma_resume_pci          -       resume helper
+ *      @dev: our PCI device
+ *
+ *      Perform the resume processing on our PCI device state - rewrite
+ *      register state and re-enable the PCI device
+ */
+static bool gma_resume_pci(struct pci_dev *pdev)
+{
+        struct drm_device *dev = pci_get_drvdata(pdev);
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int ret;
+
+        if (!dev_priv->suspended)
+                return true;
+
+        pci_set_power_state(pdev, PCI_D0);
+        pci_restore_state(pdev);
+        pci_write_config_dword(pdev, 0x5c, dev_priv->saveBSM);
+        pci_write_config_dword(pdev, 0xFC, dev_priv->saveVBT);
+        /* restoring MSI address and data in PCIx space */
+        pci_write_config_dword(pdev, PSB_PCIx_MSI_ADDR_LOC, dev_priv->msi_addr);
+        pci_write_config_dword(pdev, PSB_PCIx_MSI_DATA_LOC, dev_priv->msi_data);
+        ret = pci_enable_device(pdev);
+
+        if (ret != 0)
+                dev_err(&pdev->dev, "pci_enable failed: %d\n", ret);
+        else
+                dev_priv->suspended = false;
+        return !dev_priv->suspended;
+}
+
+/**
+ *      gma_power_suspend               -       bus callback for suspend
+ *      @pdev: our PCI device
+ *      @state: suspend type
+ *
+ *      Called back by the PCI layer during a suspend of the system. We
+ *      perform the necessary shut down steps and save enough state that
+ *      we can undo this when resume is called.
+ */
+int gma_power_suspend(struct device *_dev)
+{
+        struct pci_dev *pdev = container_of(_dev, struct pci_dev, dev);
+        struct drm_device *dev = pci_get_drvdata(pdev);
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        mutex_lock(&power_mutex);
+        if (!dev_priv->suspended) {
+                if (dev_priv->display_count) {
+                        mutex_unlock(&power_mutex);
+                        return -EBUSY;
+                }
+                psb_irq_uninstall(dev);
+                gma_suspend_display(dev);
+                gma_suspend_pci(pdev);
+        }
+        mutex_unlock(&power_mutex);
+        return 0;
+}
+
+/**
+ *      gma_power_resume                -       resume power
+ *      @pdev: PCI device
+ *
+ *      Resume the PCI side of the graphics and then the displays
+ */
+int gma_power_resume(struct device *_dev)
+{
+        struct pci_dev *pdev = container_of(_dev, struct pci_dev, dev);
+        struct drm_device *dev = pci_get_drvdata(pdev);
+
+        mutex_lock(&power_mutex);
+        gma_resume_pci(pdev);
+        gma_resume_display(pdev);
+        psb_irq_preinstall(dev);
+        psb_irq_postinstall(dev);
+        mutex_unlock(&power_mutex);
+        return 0;
+}
+
+/**
+ *      gma_power_is_on         -       returne true if power is on
+ *      @dev: our DRM device
+ *
+ *      Returns true if the display island power is on at this moment
+ */
+bool gma_power_is_on(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        return dev_priv->display_power;
+}
+
+/**
+ *      gma_power_begin         -       begin requiring power
+ *      @dev: our DRM device
+ *      @force_on: true to force power on
+ *
+ *      Begin an action that requires the display power island is enabled.
+ *      We refcount the islands.
+ */
+bool gma_power_begin(struct drm_device *dev, bool force_on)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int ret;
+        unsigned long flags;
+
+        spin_lock_irqsave(&power_ctrl_lock, flags);
+        /* Power already on ? */
+        if (dev_priv->display_power) {
+                dev_priv->display_count++;
+                pm_runtime_get(&dev->pdev->dev);
+                spin_unlock_irqrestore(&power_ctrl_lock, flags);
+                return true;
+        }
+        if (force_on == false)
+                goto out_false;
+
+        /* Ok power up needed */
+        ret = gma_resume_pci(dev->pdev);
+        if (ret == 0) {
+                psb_irq_preinstall(dev);
+                psb_irq_postinstall(dev);
+                pm_runtime_get(&dev->pdev->dev);
+                dev_priv->display_count++;
+                spin_unlock_irqrestore(&power_ctrl_lock, flags);
+                return true;
+        }
+out_false:
+        spin_unlock_irqrestore(&power_ctrl_lock, flags);
+        return false;
+}
+
+/**
+ *      gma_power_end           -       end use of power
+ *      @dev: Our DRM device
+ *
+ *      Indicate that one of our gma_power_begin() requested periods when
+ *      the diplay island power is needed has completed.
+ */
+void gma_power_end(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        unsigned long flags;
+        spin_lock_irqsave(&power_ctrl_lock, flags);
+        dev_priv->display_count--;
+        WARN_ON(dev_priv->display_count < 0);
+        spin_unlock_irqrestore(&power_ctrl_lock, flags);
+        pm_runtime_put(&dev->pdev->dev);
+}
+
+int psb_runtime_suspend(struct device *dev)
+{
+        return gma_power_suspend(dev);
+}
+
+int psb_runtime_resume(struct device *dev)
+{
+        return 0;
+}
+
+int psb_runtime_idle(struct device *dev)
+{
+        struct drm_device *drmdev = pci_get_drvdata(to_pci_dev(dev));
+        struct drm_psb_private *dev_priv = drmdev->dev_private;
+        if (dev_priv->display_count)
+                return 0;
+        else
+                return 1;
+}
diff --git a/drivers/staging/gma500/power.h b/drivers/staging/gma500/power.h
new file mode 100644
index 00000000000..1969d2ecb32
--- /dev/null
+++ b/drivers/staging/gma500/power.h
@@ -0,0 +1,67 @@
+/**************************************************************************
+ * Copyright (c) 2009-2011, Intel Corporation.
+ * 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, 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:
+ *    Benjamin Defnet <benjamin.r.defnet@intel.com>
+ *    Rajesh Poornachandran <rajesh.poornachandran@intel.com>
+ * Massively reworked
+ *    Alan Cox <alan@linux.intel.com>
+ */
+#ifndef _PSB_POWERMGMT_H_
+#define _PSB_POWERMGMT_H_
+
+#include <linux/pci.h>
+#include <drm/drmP.h>
+
+void gma_power_init(struct drm_device *dev);
+void gma_power_uninit(struct drm_device *dev);
+
+/*
+ * The kernel bus power management  will call these functions
+ */
+int gma_power_suspend(struct device *dev);
+int gma_power_resume(struct device *dev);
+
+/*
+ * These are the functions the driver should use to wrap all hw access
+ * (i.e. register reads and writes)
+ */
+bool gma_power_begin(struct drm_device *dev, bool force);
+void gma_power_end(struct drm_device *dev);
+
+/*
+ * Use this function to do an instantaneous check for if the hw is on.
+ * Only use this in cases where you know the mutex is already held such
+ * as in irq install/uninstall and you need to
+ * prevent a deadlock situation.  Otherwise use gma_power_begin().
+ */
+bool gma_power_is_on(struct drm_device *dev);
+
+/*
+ * GFX-Runtime PM callbacks
+ */
+int psb_runtime_suspend(struct device *dev);
+int psb_runtime_resume(struct device *dev);
+int psb_runtime_idle(struct device *dev);
+
+#endif /*_PSB_POWERMGMT_H_*/
diff --git a/drivers/staging/gma500/psb_device.c b/drivers/staging/gma500/psb_device.c
new file mode 100644
index 00000000000..46591323595
--- /dev/null
+++ b/drivers/staging/gma500/psb_device.c
@@ -0,0 +1,353 @@
+/**************************************************************************
+ * Copyright (c) 2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+#include <linux/backlight.h>
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "psb_drm.h"
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include "intel_bios.h"
+
+
+static int psb_output_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        psb_intel_lvds_init(dev, &dev_priv->mode_dev);
+        psb_intel_sdvo_init(dev, SDVOB);
+        return 0;
+}
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+
+/*
+ *      Poulsbo Backlight Interfaces
+ */
+
+#define BLC_PWM_PRECISION_FACTOR 100    /* 10000000 */
+#define BLC_PWM_FREQ_CALC_CONSTANT 32
+#define MHz 1000000
+
+#define PSB_BLC_PWM_PRECISION_FACTOR    10
+#define PSB_BLC_MAX_PWM_REG_FREQ        0xFFFE
+#define PSB_BLC_MIN_PWM_REG_FREQ        0x2
+
+#define PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR (0xFFFE)
+#define PSB_BACKLIGHT_PWM_CTL_SHIFT     (16)
+
+static int psb_brightness;
+static struct backlight_device *psb_backlight_device;
+
+static int psb_get_brightness(struct backlight_device *bd)
+{
+        /* return locally cached var instead of HW read (due to DPST etc.) */
+        /* FIXME: ideally return actual value in case firmware fiddled with
+           it */
+        return psb_brightness;
+}
+
+
+static int psb_backlight_setup(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        unsigned long core_clock;
+        /* u32 bl_max_freq; */
+        /* unsigned long value; */
+        u16 bl_max_freq;
+        uint32_t value;
+        uint32_t blc_pwm_precision_factor;
+
+        /* get bl_max_freq and pol from dev_priv*/
+        if (!dev_priv->lvds_bl) {
+                dev_err(dev->dev, "Has no valid LVDS backlight info\n");
+                return -ENOENT;
+        }
+        bl_max_freq = dev_priv->lvds_bl->freq;
+        blc_pwm_precision_factor = PSB_BLC_PWM_PRECISION_FACTOR;
+
+        core_clock = dev_priv->core_freq;
+
+        value = (core_clock * MHz) / BLC_PWM_FREQ_CALC_CONSTANT;
+        value *= blc_pwm_precision_factor;
+        value /= bl_max_freq;
+        value /= blc_pwm_precision_factor;
+
+        if (value > (unsigned long long)PSB_BLC_MAX_PWM_REG_FREQ ||
+                 value < (unsigned long long)PSB_BLC_MIN_PWM_REG_FREQ)
+                                return -ERANGE;
+        else {
+                value &= PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR;
+                REG_WRITE(BLC_PWM_CTL,
+                        (value << PSB_BACKLIGHT_PWM_CTL_SHIFT) | (value));
+        }
+        return 0;
+}
+
+static int psb_set_brightness(struct backlight_device *bd)
+{
+        struct drm_device *dev = bl_get_data(psb_backlight_device);
+        int level = bd->props.brightness;
+
+        /* Percentage 1-100% being valid */
+        if (level < 1)
+                level = 1;
+
+        psb_intel_lvds_set_brightness(dev, level);
+        psb_brightness = level;
+        return 0;
+}
+
+static const struct backlight_ops psb_ops = {
+        .get_brightness = psb_get_brightness,
+        .update_status  = psb_set_brightness,
+};
+
+static int psb_backlight_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        int ret;
+        struct backlight_properties props;
+
+        memset(&props, 0, sizeof(struct backlight_properties));
+        props.max_brightness = 100;
+        props.type = BACKLIGHT_PLATFORM;
+
+        psb_backlight_device = backlight_device_register("psb-bl",
+                                        NULL, (void *)dev, &psb_ops, &props);
+        if (IS_ERR(psb_backlight_device))
+                return PTR_ERR(psb_backlight_device);
+
+        ret = psb_backlight_setup(dev);
+        if (ret < 0) {
+                backlight_device_unregister(psb_backlight_device);
+                psb_backlight_device = NULL;
+                return ret;
+        }
+        psb_backlight_device->props.brightness = 100;
+        psb_backlight_device->props.max_brightness = 100;
+        backlight_update_status(psb_backlight_device);
+        dev_priv->backlight_device = psb_backlight_device;
+        return 0;
+}
+
+#endif
+
+/*
+ *      Provide the Poulsbo specific chip logic and low level methods
+ *      for power management
+ */
+
+static void psb_init_pm(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        u32 gating = PSB_RSGX32(PSB_CR_CLKGATECTL);
+        gating &= ~3;   /* Disable 2D clock gating */
+        gating |= 1;
+        PSB_WSGX32(gating, PSB_CR_CLKGATECTL);
+        PSB_RSGX32(PSB_CR_CLKGATECTL);
+}
+
+/**
+ *      psb_save_display_registers      -       save registers lost on suspend
+ *      @dev: our DRM device
+ *
+ *      Save the state we need in order to be able to restore the interface
+ *      upon resume from suspend
+ */
+static int psb_save_display_registers(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct drm_crtc *crtc;
+        struct drm_connector *connector;
+
+        /* Display arbitration control + watermarks */
+        dev_priv->saveDSPARB = PSB_RVDC32(DSPARB);
+        dev_priv->saveDSPFW1 = PSB_RVDC32(DSPFW1);
+        dev_priv->saveDSPFW2 = PSB_RVDC32(DSPFW2);
+        dev_priv->saveDSPFW3 = PSB_RVDC32(DSPFW3);
+        dev_priv->saveDSPFW4 = PSB_RVDC32(DSPFW4);
+        dev_priv->saveDSPFW5 = PSB_RVDC32(DSPFW5);
+        dev_priv->saveDSPFW6 = PSB_RVDC32(DSPFW6);
+        dev_priv->saveCHICKENBIT = PSB_RVDC32(DSPCHICKENBIT);
+
+        /* Save crtc and output state */
+        mutex_lock(&dev->mode_config.mutex);
+        list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+                if (drm_helper_crtc_in_use(crtc))
+                        crtc->funcs->save(crtc);
+        }
+
+        list_for_each_entry(connector, &dev->mode_config.connector_list, head)
+                connector->funcs->save(connector);
+
+        mutex_unlock(&dev->mode_config.mutex);
+        return 0;
+}
+
+/**
+ *      psb_restore_display_registers   -       restore lost register state
+ *      @dev: our DRM device
+ *
+ *      Restore register state that was lost during suspend and resume.
+ */
+static int psb_restore_display_registers(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct drm_crtc *crtc;
+        struct drm_connector *connector;
+        int pp_stat;
+
+        /* Display arbitration + watermarks */
+        PSB_WVDC32(dev_priv->saveDSPARB, DSPARB);
+        PSB_WVDC32(dev_priv->saveDSPFW1, DSPFW1);
+        PSB_WVDC32(dev_priv->saveDSPFW2, DSPFW2);
+        PSB_WVDC32(dev_priv->saveDSPFW3, DSPFW3);
+        PSB_WVDC32(dev_priv->saveDSPFW4, DSPFW4);
+        PSB_WVDC32(dev_priv->saveDSPFW5, DSPFW5);
+        PSB_WVDC32(dev_priv->saveDSPFW6, DSPFW6);
+        PSB_WVDC32(dev_priv->saveCHICKENBIT, DSPCHICKENBIT);
+
+        /*make sure VGA plane is off. it initializes to on after reset!*/
+        PSB_WVDC32(0x80000000, VGACNTRL);
+
+        mutex_lock(&dev->mode_config.mutex);
+        list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
+                if (drm_helper_crtc_in_use(crtc))
+                        crtc->funcs->restore(crtc);
+
+        list_for_each_entry(connector, &dev->mode_config.connector_list, head)
+                connector->funcs->restore(connector);
+
+        mutex_unlock(&dev->mode_config.mutex);
+
+        if (dev_priv->iLVDS_enable) {
+                /*shutdown the panel*/
+                PSB_WVDC32(0, PP_CONTROL);
+                do {
+                        pp_stat = PSB_RVDC32(PP_STATUS);
+                } while (pp_stat & 0x80000000);
+
+                /* Turn off the plane */
+                PSB_WVDC32(0x58000000, DSPACNTR);
+                PSB_WVDC32(0, DSPASURF);/*trigger the plane disable*/
+                /* Wait ~4 ticks */
+                msleep(4);
+                /* Turn off pipe */
+                PSB_WVDC32(0x0, PIPEACONF);
+                /* Wait ~8 ticks */
+                msleep(8);
+
+                /* Turn off PLLs */
+                PSB_WVDC32(0, MRST_DPLL_A);
+        } else {
+                PSB_WVDC32(DPI_SHUT_DOWN, DPI_CONTROL_REG);
+                PSB_WVDC32(0x0, PIPEACONF);
+                PSB_WVDC32(0x2faf0000, BLC_PWM_CTL);
+                while (REG_READ(0x70008) & 0x40000000)
+                        cpu_relax();
+                while ((PSB_RVDC32(GEN_FIFO_STAT_REG) & DPI_FIFO_EMPTY)
+                        != DPI_FIFO_EMPTY)
+                        cpu_relax();
+                PSB_WVDC32(0, DEVICE_READY_REG);
+        }
+        return 0;
+}
+
+static int psb_power_down(struct drm_device *dev)
+{
+        return 0;
+}
+
+static int psb_power_up(struct drm_device *dev)
+{
+        return 0;
+}
+
+static void psb_get_core_freq(struct drm_device *dev)
+{
+        uint32_t clock;
+        struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        /*pci_write_config_dword(pci_root, 0xD4, 0x00C32004);*/
+        /*pci_write_config_dword(pci_root, 0xD0, 0xE0033000);*/
+
+        pci_write_config_dword(pci_root, 0xD0, 0xD0050300);
+        pci_read_config_dword(pci_root, 0xD4, &clock);
+        pci_dev_put(pci_root);
+
+        switch (clock & 0x07) {
+        case 0:
+                dev_priv->core_freq = 100;
+                break;
+        case 1:
+                dev_priv->core_freq = 133;
+                break;
+        case 2:
+                dev_priv->core_freq = 150;
+                break;
+        case 3:
+                dev_priv->core_freq = 178;
+                break;
+        case 4:
+                dev_priv->core_freq = 200;
+                break;
+        case 5:
+        case 6:
+        case 7:
+                dev_priv->core_freq = 266;
+        default:
+                dev_priv->core_freq = 0;
+        }
+}
+
+static int psb_chip_setup(struct drm_device *dev)
+{
+        psb_get_core_freq(dev);
+        gma_intel_opregion_init(dev);
+        psb_intel_init_bios(dev);
+        return 0;
+}
+
+const struct psb_ops psb_chip_ops = {
+        .name = "Poulsbo",
+        .accel_2d = 1,
+        .pipes = 2,
+        .crtcs = 2,
+        .sgx_offset = PSB_SGX_OFFSET,
+        .chip_setup = psb_chip_setup,
+
+        .crtc_helper = &psb_intel_helper_funcs,
+        .crtc_funcs = &psb_intel_crtc_funcs,
+
+        .output_init = psb_output_init,
+
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+        .backlight_init = psb_backlight_init,
+#endif
+
+        .init_pm = psb_init_pm,
+        .save_regs = psb_save_display_registers,
+        .restore_regs = psb_restore_display_registers,
+        .power_down = psb_power_down,
+        .power_up = psb_power_up,
+};
+
diff --git a/drivers/staging/gma500/psb_drm.h b/drivers/staging/gma500/psb_drm.h
new file mode 100644
index 00000000000..0da84683568
--- /dev/null
+++ b/drivers/staging/gma500/psb_drm.h
@@ -0,0 +1,219 @@
+/**************************************************************************
+ * Copyright (c) 2007-2011, Intel Corporation.
+ * All Rights Reserved.
+ * Copyright (c) 2008, Tungsten Graphics Inc.  Cedar Park, TX., USA.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+#ifndef _PSB_DRM_H_
+#define _PSB_DRM_H_
+
+#define PSB_NUM_PIPE 3
+
+#define PSB_GPU_ACCESS_READ         (1ULL << 32)
+#define PSB_GPU_ACCESS_WRITE        (1ULL << 33)
+#define PSB_GPU_ACCESS_MASK         (PSB_GPU_ACCESS_READ | PSB_GPU_ACCESS_WRITE)
+
+#define PSB_BO_FLAG_COMMAND         (1ULL << 52)
+
+/*
+ * Feedback components:
+ */
+
+struct drm_psb_sizes_arg {
+        u32 ta_mem_size;
+        u32 mmu_size;
+        u32 pds_size;
+        u32 rastgeom_size;
+        u32 tt_size;
+        u32 vram_size;
+};
+
+struct drm_psb_dpst_lut_arg {
+        uint8_t lut[256];
+        int output_id;
+};
+
+#define PSB_DC_CRTC_SAVE 0x01
+#define PSB_DC_CRTC_RESTORE 0x02
+#define PSB_DC_OUTPUT_SAVE 0x04
+#define PSB_DC_OUTPUT_RESTORE 0x08
+#define PSB_DC_CRTC_MASK 0x03
+#define PSB_DC_OUTPUT_MASK 0x0C
+
+struct drm_psb_dc_state_arg {
+        u32 flags;
+        u32 obj_id;
+};
+
+struct drm_psb_mode_operation_arg {
+        u32 obj_id;
+        u16 operation;
+        struct drm_mode_modeinfo mode;
+        void *data;
+};
+
+struct drm_psb_stolen_memory_arg {
+        u32 base;
+        u32 size;
+};
+
+/*Display Register Bits*/
+#define REGRWBITS_PFIT_CONTROLS                 (1 << 0)
+#define REGRWBITS_PFIT_AUTOSCALE_RATIOS         (1 << 1)
+#define REGRWBITS_PFIT_PROGRAMMED_SCALE_RATIOS  (1 << 2)
+#define REGRWBITS_PIPEASRC                      (1 << 3)
+#define REGRWBITS_PIPEBSRC                      (1 << 4)
+#define REGRWBITS_VTOTAL_A                      (1 << 5)
+#define REGRWBITS_VTOTAL_B                      (1 << 6)
+#define REGRWBITS_DSPACNTR      (1 << 8)
+#define REGRWBITS_DSPBCNTR      (1 << 9)
+#define REGRWBITS_DSPCCNTR      (1 << 10)
+
+/*Overlay Register Bits*/
+#define OV_REGRWBITS_OVADD                      (1 << 0)
+#define OV_REGRWBITS_OGAM_ALL                   (1 << 1)
+
+#define OVC_REGRWBITS_OVADD                  (1 << 2)
+#define OVC_REGRWBITS_OGAM_ALL                  (1 << 3)
+
+struct drm_psb_register_rw_arg {
+        u32 b_force_hw_on;
+
+        u32 display_read_mask;
+        u32 display_write_mask;
+
+        struct {
+                u32 pfit_controls;
+                u32 pfit_autoscale_ratios;
+                u32 pfit_programmed_scale_ratios;
+                u32 pipeasrc;
+                u32 pipebsrc;
+                u32 vtotal_a;
+                u32 vtotal_b;
+        } display;
+
+        u32 overlay_read_mask;
+        u32 overlay_write_mask;
+
+        struct {
+                u32 OVADD;
+                u32 OGAMC0;
+                u32 OGAMC1;
+                u32 OGAMC2;
+                u32 OGAMC3;
+                u32 OGAMC4;
+                u32 OGAMC5;
+                u32 IEP_ENABLED;
+                u32 IEP_BLE_MINMAX;
+                u32 IEP_BSSCC_CONTROL;
+                u32 b_wait_vblank;
+        } overlay;
+
+        u32 sprite_enable_mask;
+        u32 sprite_disable_mask;
+
+        struct {
+                u32 dspa_control;
+                u32 dspa_key_value;
+                u32 dspa_key_mask;
+                u32 dspc_control;
+                u32 dspc_stride;
+                u32 dspc_position;
+                u32 dspc_linear_offset;
+                u32 dspc_size;
+                u32 dspc_surface;
+        } sprite;
+
+        u32 subpicture_enable_mask;
+        u32 subpicture_disable_mask;
+};
+
+/* Controlling the kernel modesetting buffers */
+
+#define DRM_PSB_SIZES           0x07
+#define DRM_PSB_FUSE_REG        0x08
+#define DRM_PSB_DC_STATE        0x0A
+#define DRM_PSB_ADB             0x0B
+#define DRM_PSB_MODE_OPERATION  0x0C
+#define DRM_PSB_STOLEN_MEMORY   0x0D
+#define DRM_PSB_REGISTER_RW     0x0E
+
+/*
+ * NOTE: Add new commands here, but increment
+ * the values below and increment their
+ * corresponding defines where they're
+ * defined elsewhere.
+ */
+
+#define DRM_PSB_GEM_CREATE      0x10
+#define DRM_PSB_2D_OP           0x11
+#define DRM_PSB_GEM_MMAP        0x12
+#define DRM_PSB_DPST            0x1B
+#define DRM_PSB_GAMMA           0x1C
+#define DRM_PSB_DPST_BL         0x1D
+#define DRM_PSB_GET_PIPE_FROM_CRTC_ID 0x1F
+
+#define PSB_MODE_OPERATION_MODE_VALID   0x01
+#define PSB_MODE_OPERATION_SET_DC_BASE  0x02
+
+struct drm_psb_get_pipe_from_crtc_id_arg {
+        /** ID of CRTC being requested **/
+        u32 crtc_id;
+
+        /** pipe of requested CRTC **/
+        u32 pipe;
+};
+
+/* FIXME: move this into a medfield header once we are sure it isn't needed for an
+   ioctl  */
+struct psb_drm_dpu_rect {  
+        int x, y;             
+        int width, height;    
+};  
+
+struct drm_psb_gem_create {
+        __u64 size;
+        __u32 handle;
+        __u32 flags;
+#define PSB_GEM_CREATE_STOLEN           1       /* Stolen memory can be used */
+};
+
+#define PSB_2D_OP_BUFLEN                16
+
+struct drm_psb_2d_op {
+        __u32 src;              /* Handles, only src supported right now */
+        __u32 dst;
+        __u32 mask;
+        __u32 pat;
+        __u32 size;             /* In dwords of command */
+        __u32 spare;            /* And bumps array to u64 align */
+        __u32 cmd[PSB_2D_OP_BUFLEN];
+};
+
+struct drm_psb_gem_mmap {
+        __u32 handle;
+        __u32 pad;
+        /**
+         * Fake offset to use for subsequent mmap call
+         *
+         * This is a fixed-size type for 32/64 compatibility.
+         */
+        __u64 offset;
+};
+
+#endif
diff --git a/drivers/staging/gma500/psb_drv.c b/drivers/staging/gma500/psb_drv.c
new file mode 100644
index 00000000000..b2cdce7b97e
--- /dev/null
+++ b/drivers/staging/gma500/psb_drv.c
@@ -0,0 +1,1231 @@
+/**************************************************************************
+ * Copyright (c) 2007-2011, Intel Corporation.
+ * All Rights Reserved.
+ * Copyright (c) 2008, Tungsten Graphics, Inc. Cedar Park, TX., USA.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+#include <drm/drmP.h>
+#include <drm/drm.h>
+#include "psb_drm.h"
+#include "psb_drv.h"
+#include "framebuffer.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include "intel_bios.h"
+#include "mid_bios.h"
+#include "mdfld_dsi_dbi.h"
+#include <drm/drm_pciids.h>
+#include "power.h"
+#include <linux/cpu.h>
+#include <linux/notifier.h>
+#include <linux/spinlock.h>
+#include <linux/pm_runtime.h>
+#include <acpi/video.h>
+
+static int drm_psb_trap_pagefaults;
+
+int drm_psb_no_fb;
+
+static int psb_probe(struct pci_dev *pdev, const struct pci_device_id *ent);
+
+MODULE_PARM_DESC(no_fb, "Disable FBdev");
+MODULE_PARM_DESC(trap_pagefaults, "Error and reset on MMU pagefaults");
+module_param_named(no_fb, drm_psb_no_fb, int, 0600);
+module_param_named(trap_pagefaults, drm_psb_trap_pagefaults, int, 0600);
+
+
+static DEFINE_PCI_DEVICE_TABLE(pciidlist) = {
+        { 0x8086, 0x8108, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &psb_chip_ops },
+        { 0x8086, 0x8109, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &psb_chip_ops },
+#if defined(CONFIG_DRM_PSB_MRST)
+        { 0x8086, 0x4100, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mrst_chip_ops},
+        { 0x8086, 0x4101, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mrst_chip_ops},
+        { 0x8086, 0x4102, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mrst_chip_ops},
+        { 0x8086, 0x4103, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mrst_chip_ops},
+        { 0x8086, 0x4104, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mrst_chip_ops},
+        { 0x8086, 0x4105, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mrst_chip_ops},
+        { 0x8086, 0x4106, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mrst_chip_ops},
+        { 0x8086, 0x4107, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mrst_chip_ops},
+#endif
+#if defined(CONFIG_DRM_PSB_MFLD)
+        { 0x8086, 0x0130, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mdfld_chip_ops},
+        { 0x8086, 0x0131, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mdfld_chip_ops},
+        { 0x8086, 0x0132, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mdfld_chip_ops},
+        { 0x8086, 0x0133, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mdfld_chip_ops},
+        { 0x8086, 0x0134, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mdfld_chip_ops},
+        { 0x8086, 0x0135, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mdfld_chip_ops},
+        { 0x8086, 0x0136, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mdfld_chip_ops},
+        { 0x8086, 0x0137, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &mdfld_chip_ops},
+#endif
+#if defined(CONFIG_DRM_PSB_CDV)
+        { 0x8086, 0x0be0, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+        { 0x8086, 0x0be1, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+        { 0x8086, 0x0be2, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+        { 0x8086, 0x0be3, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+        { 0x8086, 0x0be4, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+        { 0x8086, 0x0be5, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+        { 0x8086, 0x0be6, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+        { 0x8086, 0x0be7, PCI_ANY_ID, PCI_ANY_ID, 0, 0, (long) &cdv_chip_ops},
+#endif
+        { 0, 0, 0}
+};
+MODULE_DEVICE_TABLE(pci, pciidlist);
+
+/*
+ * Standard IOCTLs.
+ */
+
+#define DRM_IOCTL_PSB_SIZES     \
+                DRM_IOR(DRM_PSB_SIZES + DRM_COMMAND_BASE, \
+                        struct drm_psb_sizes_arg)
+#define DRM_IOCTL_PSB_FUSE_REG  \
+                DRM_IOWR(DRM_PSB_FUSE_REG + DRM_COMMAND_BASE, uint32_t)
+#define DRM_IOCTL_PSB_DC_STATE  \
+                DRM_IOW(DRM_PSB_DC_STATE + DRM_COMMAND_BASE, \
+                        struct drm_psb_dc_state_arg)
+#define DRM_IOCTL_PSB_ADB       \
+                DRM_IOWR(DRM_PSB_ADB + DRM_COMMAND_BASE, uint32_t)
+#define DRM_IOCTL_PSB_MODE_OPERATION    \
+                DRM_IOWR(DRM_PSB_MODE_OPERATION + DRM_COMMAND_BASE, \
+                         struct drm_psb_mode_operation_arg)
+#define DRM_IOCTL_PSB_STOLEN_MEMORY     \
+                DRM_IOWR(DRM_PSB_STOLEN_MEMORY + DRM_COMMAND_BASE, \
+                         struct drm_psb_stolen_memory_arg)
+#define DRM_IOCTL_PSB_REGISTER_RW       \
+                DRM_IOWR(DRM_PSB_REGISTER_RW + DRM_COMMAND_BASE, \
+                         struct drm_psb_register_rw_arg)
+#define DRM_IOCTL_PSB_DPST      \
+                DRM_IOWR(DRM_PSB_DPST + DRM_COMMAND_BASE, \
+                         uint32_t)
+#define DRM_IOCTL_PSB_GAMMA     \
+                DRM_IOWR(DRM_PSB_GAMMA + DRM_COMMAND_BASE, \
+                         struct drm_psb_dpst_lut_arg)
+#define DRM_IOCTL_PSB_DPST_BL   \
+                DRM_IOWR(DRM_PSB_DPST_BL + DRM_COMMAND_BASE, \
+                         uint32_t)
+#define DRM_IOCTL_PSB_GET_PIPE_FROM_CRTC_ID     \
+                DRM_IOWR(DRM_PSB_GET_PIPE_FROM_CRTC_ID + DRM_COMMAND_BASE, \
+                         struct drm_psb_get_pipe_from_crtc_id_arg)
+#define DRM_IOCTL_PSB_GEM_CREATE        \
+                DRM_IOWR(DRM_PSB_GEM_CREATE + DRM_COMMAND_BASE, \
+                         struct drm_psb_gem_create)
+#define DRM_IOCTL_PSB_2D_OP     \
+                DRM_IOW(DRM_PSB_2D_OP + DRM_COMMAND_BASE, \
+                         struct drm_psb_2d_op)
+#define DRM_IOCTL_PSB_GEM_MMAP  \
+                DRM_IOWR(DRM_PSB_GEM_MMAP + DRM_COMMAND_BASE, \
+                         struct drm_psb_gem_mmap)
+
+static int psb_sizes_ioctl(struct drm_device *dev, void *data,
+                           struct drm_file *file_priv);
+static int psb_dc_state_ioctl(struct drm_device *dev, void * data,
+                              struct drm_file *file_priv);
+static int psb_adb_ioctl(struct drm_device *dev, void *data,
+                         struct drm_file *file_priv);
+static int psb_mode_operation_ioctl(struct drm_device *dev, void *data,
+                                    struct drm_file *file_priv);
+static int psb_stolen_memory_ioctl(struct drm_device *dev, void *data,
+                                   struct drm_file *file_priv);
+static int psb_register_rw_ioctl(struct drm_device *dev, void *data,
+                                 struct drm_file *file_priv);
+static int psb_dpst_ioctl(struct drm_device *dev, void *data,
+                          struct drm_file *file_priv);
+static int psb_gamma_ioctl(struct drm_device *dev, void *data,
+                           struct drm_file *file_priv);
+static int psb_dpst_bl_ioctl(struct drm_device *dev, void *data,
+                             struct drm_file *file_priv);
+
+#define PSB_IOCTL_DEF(ioctl, func, flags) \
+        [DRM_IOCTL_NR(ioctl) - DRM_COMMAND_BASE] = {ioctl, flags, func}
+
+static struct drm_ioctl_desc psb_ioctls[] = {
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_SIZES, psb_sizes_ioctl, DRM_AUTH),
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_DC_STATE, psb_dc_state_ioctl, DRM_AUTH),
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_ADB, psb_adb_ioctl, DRM_AUTH),
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_MODE_OPERATION, psb_mode_operation_ioctl,
+                      DRM_AUTH),
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_STOLEN_MEMORY, psb_stolen_memory_ioctl,
+                      DRM_AUTH),
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_REGISTER_RW, psb_register_rw_ioctl,
+                      DRM_AUTH),
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_DPST, psb_dpst_ioctl, DRM_AUTH),
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_GAMMA, psb_gamma_ioctl, DRM_AUTH),
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_DPST_BL, psb_dpst_bl_ioctl, DRM_AUTH),
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_GET_PIPE_FROM_CRTC_ID,
+                                        psb_intel_get_pipe_from_crtc_id, 0),
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_GEM_CREATE, psb_gem_create_ioctl,
+                                                DRM_UNLOCKED | DRM_AUTH),
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_2D_OP, psb_accel_ioctl,
+                                                DRM_UNLOCKED| DRM_AUTH),
+        PSB_IOCTL_DEF(DRM_IOCTL_PSB_GEM_MMAP, psb_gem_mmap_ioctl,
+                                                DRM_UNLOCKED | DRM_AUTH),
+};
+
+static void psb_lastclose(struct drm_device *dev)
+{
+        return;
+}
+
+static void psb_do_takedown(struct drm_device *dev)
+{
+        /* FIXME: do we need to clean up the gtt here ? */
+}
+
+static int psb_do_init(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct psb_gtt *pg = &dev_priv->gtt;
+
+        uint32_t stolen_gtt;
+
+        int ret = -ENOMEM;
+
+        if (pg->mmu_gatt_start & 0x0FFFFFFF) {
+                dev_err(dev->dev, "Gatt must be 256M aligned. This is a bug.\n");
+                ret = -EINVAL;
+                goto out_err;
+        }
+
+
+        stolen_gtt = (pg->stolen_size >> PAGE_SHIFT) * 4;
+        stolen_gtt = (stolen_gtt + PAGE_SIZE - 1) >> PAGE_SHIFT;
+        stolen_gtt =
+            (stolen_gtt < pg->gtt_pages) ? stolen_gtt : pg->gtt_pages;
+
+        dev_priv->gatt_free_offset = pg->mmu_gatt_start +
+            (stolen_gtt << PAGE_SHIFT) * 1024;
+
+        if (1 || drm_debug) {
+                uint32_t core_id = PSB_RSGX32(PSB_CR_CORE_ID);
+                uint32_t core_rev = PSB_RSGX32(PSB_CR_CORE_REVISION);
+                DRM_INFO("SGX core id = 0x%08x\n", core_id);
+                DRM_INFO("SGX core rev major = 0x%02x, minor = 0x%02x\n",
+                         (core_rev & _PSB_CC_REVISION_MAJOR_MASK) >>
+                         _PSB_CC_REVISION_MAJOR_SHIFT,
+                         (core_rev & _PSB_CC_REVISION_MINOR_MASK) >>
+                         _PSB_CC_REVISION_MINOR_SHIFT);
+                DRM_INFO
+                    ("SGX core rev maintenance = 0x%02x, designer = 0x%02x\n",
+                     (core_rev & _PSB_CC_REVISION_MAINTENANCE_MASK) >>
+                     _PSB_CC_REVISION_MAINTENANCE_SHIFT,
+                     (core_rev & _PSB_CC_REVISION_DESIGNER_MASK) >>
+                     _PSB_CC_REVISION_DESIGNER_SHIFT);
+        }
+
+
+        spin_lock_init(&dev_priv->irqmask_lock);
+        mutex_init(&dev_priv->mutex_2d);
+
+        PSB_WSGX32(0x00000000, PSB_CR_BIF_BANK0);
+        PSB_WSGX32(0x00000000, PSB_CR_BIF_BANK1);
+        PSB_RSGX32(PSB_CR_BIF_BANK1);
+        PSB_WSGX32(PSB_RSGX32(PSB_CR_BIF_CTRL) | _PSB_MMU_ER_MASK,
+                                                        PSB_CR_BIF_CTRL);
+        psb_spank(dev_priv);
+
+        /* mmu_gatt ?? */
+        PSB_WSGX32(pg->gatt_start, PSB_CR_BIF_TWOD_REQ_BASE);
+        return 0;
+out_err:
+        psb_do_takedown(dev);
+        return ret;
+}
+
+static int psb_driver_unload(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        /* Kill vblank etc here */
+
+        gma_backlight_exit(dev);
+
+        if (drm_psb_no_fb == 0)
+                psb_modeset_cleanup(dev);
+
+        if (dev_priv) {
+                psb_lid_timer_takedown(dev_priv);
+                gma_intel_opregion_exit(dev);
+
+                if (dev_priv->ops->chip_teardown)
+                        dev_priv->ops->chip_teardown(dev);
+                psb_do_takedown(dev);
+
+
+                if (dev_priv->pf_pd) {
+                        psb_mmu_free_pagedir(dev_priv->pf_pd);
+                        dev_priv->pf_pd = NULL;
+                }
+                if (dev_priv->mmu) {
+                        struct psb_gtt *pg = &dev_priv->gtt;
+
+                        down_read(&pg->sem);
+                        psb_mmu_remove_pfn_sequence(
+                                psb_mmu_get_default_pd
+                                (dev_priv->mmu),
+                                pg->mmu_gatt_start,
+                                dev_priv->vram_stolen_size >> PAGE_SHIFT);
+                        up_read(&pg->sem);
+                        psb_mmu_driver_takedown(dev_priv->mmu);
+                        dev_priv->mmu = NULL;
+                }
+                psb_gtt_takedown(dev);
+                if (dev_priv->scratch_page) {
+                        __free_page(dev_priv->scratch_page);
+                        dev_priv->scratch_page = NULL;
+                }
+                if (dev_priv->vdc_reg) {
+                        iounmap(dev_priv->vdc_reg);
+                        dev_priv->vdc_reg = NULL;
+                }
+                if (dev_priv->sgx_reg) {
+                        iounmap(dev_priv->sgx_reg);
+                        dev_priv->sgx_reg = NULL;
+                }
+
+                kfree(dev_priv);
+                dev->dev_private = NULL;
+
+                /*destroy VBT data*/
+                psb_intel_destroy_bios(dev);
+        }
+
+        gma_power_uninit(dev);
+
+        return 0;
+}
+
+
+static int psb_driver_load(struct drm_device *dev, unsigned long chipset)
+{
+        struct drm_psb_private *dev_priv;
+        unsigned long resource_start;
+        struct psb_gtt *pg;
+        unsigned long irqflags;
+        int ret = -ENOMEM;
+        uint32_t tt_pages;
+        struct drm_connector *connector;
+        struct psb_intel_output *psb_intel_output;
+
+        dev_priv = kzalloc(sizeof(*dev_priv), GFP_KERNEL);
+        if (dev_priv == NULL)
+                return -ENOMEM;
+
+        dev_priv->ops = (struct psb_ops *)chipset;
+        dev_priv->dev = dev;
+        dev->dev_private = (void *) dev_priv;
+
+        dev_priv->num_pipe = dev_priv->ops->pipes;
+
+        resource_start = pci_resource_start(dev->pdev, PSB_MMIO_RESOURCE);
+
+        dev_priv->vdc_reg =
+            ioremap(resource_start + PSB_VDC_OFFSET, PSB_VDC_SIZE);
+        if (!dev_priv->vdc_reg)
+                goto out_err;
+
+        dev_priv->sgx_reg = ioremap(resource_start + dev_priv->ops->sgx_offset,
+                                                        PSB_SGX_SIZE);
+        if (!dev_priv->sgx_reg)
+                goto out_err;
+
+        ret = dev_priv->ops->chip_setup(dev);
+        if (ret)
+                goto out_err;
+
+        /* Init OSPM support */
+        gma_power_init(dev);
+
+        ret = -ENOMEM;
+
+        dev_priv->scratch_page = alloc_page(GFP_DMA32 | __GFP_ZERO);
+        if (!dev_priv->scratch_page)
+                goto out_err;
+
+        set_pages_uc(dev_priv->scratch_page, 1);
+
+        ret = psb_gtt_init(dev, 0);
+        if (ret)
+                goto out_err;
+
+        dev_priv->mmu = psb_mmu_driver_init((void *)0,
+                                        drm_psb_trap_pagefaults, 0,
+                                        dev_priv);
+        if (!dev_priv->mmu)
+                goto out_err;
+
+        pg = &dev_priv->gtt;
+
+        tt_pages = (pg->gatt_pages < PSB_TT_PRIV0_PLIMIT) ?
+                (pg->gatt_pages) : PSB_TT_PRIV0_PLIMIT;
+
+
+        dev_priv->pf_pd = psb_mmu_alloc_pd(dev_priv->mmu, 1, 0);
+        if (!dev_priv->pf_pd)
+                goto out_err;
+
+        psb_mmu_set_pd_context(psb_mmu_get_default_pd(dev_priv->mmu), 0);
+        psb_mmu_set_pd_context(dev_priv->pf_pd, 1);
+
+        ret = psb_do_init(dev);
+        if (ret)
+                return ret;
+
+        PSB_WSGX32(0x20000000, PSB_CR_PDS_EXEC_BASE);
+        PSB_WSGX32(0x30000000, PSB_CR_BIF_3D_REQ_BASE);
+
+/*      igd_opregion_init(&dev_priv->opregion_dev); */
+        acpi_video_register();
+        if (dev_priv->lid_state)
+                psb_lid_timer_init(dev_priv);
+
+        ret = drm_vblank_init(dev, dev_priv->num_pipe);
+        if (ret)
+                goto out_err;
+
+        /*
+         * Install interrupt handlers prior to powering off SGX or else we will
+         * crash.
+         */
+        dev_priv->vdc_irq_mask = 0;
+        dev_priv->pipestat[0] = 0;
+        dev_priv->pipestat[1] = 0;
+        dev_priv->pipestat[2] = 0;
+        spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+        PSB_WVDC32(0xFFFFFFFF, PSB_HWSTAM);
+        PSB_WVDC32(0x00000000, PSB_INT_ENABLE_R);
+        PSB_WVDC32(0xFFFFFFFF, PSB_INT_MASK_R);
+        spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+        if (drm_core_check_feature(dev, DRIVER_MODESET))
+                drm_irq_install(dev);
+
+        dev->vblank_disable_allowed = 1;
+
+        dev->max_vblank_count = 0xffffff; /* only 24 bits of frame count */
+
+        dev->driver->get_vblank_counter = psb_get_vblank_counter;
+
+#if defined(CONFIG_DRM_PSB_MFLD)
+        /* FIXME: this is not the right place for this stuff ! */
+        mdfld_output_setup(dev);
+#endif
+        if (drm_psb_no_fb == 0) {
+                psb_modeset_init(dev);
+                psb_fbdev_init(dev);
+                drm_kms_helper_poll_init(dev);
+        }
+
+        /* Only add backlight support if we have LVDS output */
+        list_for_each_entry(connector, &dev->mode_config.connector_list,
+                            head) {
+                psb_intel_output = to_psb_intel_output(connector);
+
+                switch (psb_intel_output->type) {
+                case INTEL_OUTPUT_LVDS:
+                case INTEL_OUTPUT_MIPI:
+                        ret = gma_backlight_init(dev);
+                        break;
+                }
+        }
+
+        if (ret)
+                return ret;
+#if 0
+        /*enable runtime pm at last*/
+        pm_runtime_enable(&dev->pdev->dev);
+        pm_runtime_set_active(&dev->pdev->dev);
+#endif
+        /*Intel drm driver load is done, continue doing pvr load*/
+        return 0;
+out_err:
+        psb_driver_unload(dev);
+        return ret;
+}
+
+int psb_driver_device_is_agp(struct drm_device *dev)
+{
+        return 0;
+}
+
+
+static int psb_sizes_ioctl(struct drm_device *dev, void *data,
+                           struct drm_file *file_priv)
+{
+        struct drm_psb_private *dev_priv = psb_priv(dev);
+        struct drm_psb_sizes_arg *arg =
+                (struct drm_psb_sizes_arg *) data;
+
+        *arg = dev_priv->sizes;
+        return 0;
+}
+
+static int psb_dc_state_ioctl(struct drm_device *dev, void * data,
+                                struct drm_file *file_priv)
+{
+        uint32_t flags;
+        uint32_t obj_id;
+        struct drm_mode_object *obj;
+        struct drm_connector *connector;
+        struct drm_crtc *crtc;
+        struct drm_psb_dc_state_arg *arg =
+                (struct drm_psb_dc_state_arg *)data;
+
+
+        /* Double check MRST case */
+        if (IS_MRST(dev) || IS_MFLD(dev))
+                return -EOPNOTSUPP;
+
+        flags = arg->flags;
+        obj_id = arg->obj_id;
+
+        if (flags & PSB_DC_CRTC_MASK) {
+                obj = drm_mode_object_find(dev, obj_id,
+                                DRM_MODE_OBJECT_CRTC);
+                if (!obj) {
+                        dev_dbg(dev->dev, "Invalid CRTC object.\n");
+                        return -EINVAL;
+                }
+
+                crtc = obj_to_crtc(obj);
+
+                mutex_lock(&dev->mode_config.mutex);
+                if (drm_helper_crtc_in_use(crtc)) {
+                        if (flags & PSB_DC_CRTC_SAVE)
+                                crtc->funcs->save(crtc);
+                        else
+                                crtc->funcs->restore(crtc);
+                }
+                mutex_unlock(&dev->mode_config.mutex);
+
+                return 0;
+        } else if (flags & PSB_DC_OUTPUT_MASK) {
+                obj = drm_mode_object_find(dev, obj_id,
+                                DRM_MODE_OBJECT_CONNECTOR);
+                if (!obj) {
+                        dev_dbg(dev->dev, "Invalid connector id.\n");
+                        return -EINVAL;
+                }
+
+                connector = obj_to_connector(obj);
+                if (flags & PSB_DC_OUTPUT_SAVE)
+                        connector->funcs->save(connector);
+                else
+                        connector->funcs->restore(connector);
+
+                return 0;
+        }
+        return -EINVAL;
+}
+
+static inline void get_brightness(struct backlight_device *bd)
+{
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+        if (bd) {
+                bd->props.brightness = bd->ops->get_brightness(bd);
+                backlight_update_status(bd);
+        }
+#endif
+}
+
+static int psb_dpst_bl_ioctl(struct drm_device *dev, void *data,
+                       struct drm_file *file_priv)
+{
+        struct drm_psb_private *dev_priv = psb_priv(dev);
+        uint32_t *arg = data;
+
+        dev_priv->blc_adj2 = *arg;
+        get_brightness(dev_priv->backlight_device);
+        return 0;
+}
+
+static int psb_adb_ioctl(struct drm_device *dev, void *data,
+                        struct drm_file *file_priv)
+{
+        struct drm_psb_private *dev_priv = psb_priv(dev);
+        uint32_t *arg = data;
+
+        dev_priv->blc_adj1 = *arg;
+        get_brightness(dev_priv->backlight_device);
+        return 0;
+}
+
+/* return the current mode to the dpst module */
+static int psb_dpst_ioctl(struct drm_device *dev, void *data,
+                          struct drm_file *file_priv)
+{
+        struct drm_psb_private *dev_priv = psb_priv(dev);
+        uint32_t *arg = data;
+        uint32_t x;
+        uint32_t y;
+        uint32_t reg;
+
+        if (!gma_power_begin(dev, 0))
+                return -EIO;
+
+        reg = PSB_RVDC32(PIPEASRC);
+
+        gma_power_end(dev);
+
+        /* horizontal is the left 16 bits */
+        x = reg >> 16;
+        /* vertical is the right 16 bits */
+        y = reg & 0x0000ffff;
+
+        /* the values are the image size minus one */
+        x++;
+        y++;
+
+        *arg = (x << 16) | y;
+
+        return 0;
+}
+static int psb_gamma_ioctl(struct drm_device *dev, void *data,
+                           struct drm_file *file_priv)
+{
+        struct drm_psb_dpst_lut_arg *lut_arg = data;
+        struct drm_mode_object *obj;
+        struct drm_crtc *crtc;
+        struct drm_connector *connector;
+        struct psb_intel_crtc *psb_intel_crtc;
+        int i = 0;
+        int32_t obj_id;
+
+        obj_id = lut_arg->output_id;
+        obj = drm_mode_object_find(dev, obj_id, DRM_MODE_OBJECT_CONNECTOR);
+        if (!obj) {
+                dev_dbg(dev->dev, "Invalid Connector object.\n");
+                return -EINVAL;
+        }
+
+        connector = obj_to_connector(obj);
+        crtc = connector->encoder->crtc;
+        psb_intel_crtc = to_psb_intel_crtc(crtc);
+
+        for (i = 0; i < 256; i++)
+                psb_intel_crtc->lut_adj[i] = lut_arg->lut[i];
+
+        psb_intel_crtc_load_lut(crtc);
+
+        return 0;
+}
+
+static int psb_mode_operation_ioctl(struct drm_device *dev, void *data,
+                                struct drm_file *file_priv)
+{
+        uint32_t obj_id;
+        uint16_t op;
+        struct drm_mode_modeinfo *umode;
+        struct drm_display_mode *mode = NULL;
+        struct drm_psb_mode_operation_arg *arg;
+        struct drm_mode_object *obj;
+        struct drm_connector *connector;
+        struct drm_framebuffer *drm_fb;
+        struct psb_framebuffer *psb_fb;
+        struct drm_connector_helper_funcs *connector_funcs;
+        int ret = 0;
+        int resp = MODE_OK;
+        struct drm_psb_private *dev_priv = psb_priv(dev);
+
+        arg = (struct drm_psb_mode_operation_arg *)data;
+        obj_id = arg->obj_id;
+        op = arg->operation;
+
+        switch (op) {
+        case PSB_MODE_OPERATION_SET_DC_BASE:
+                obj = drm_mode_object_find(dev, obj_id, DRM_MODE_OBJECT_FB);
+                if (!obj) {
+                        dev_dbg(dev->dev, "Invalid FB id %d\n", obj_id);
+                        return -EINVAL;
+                }
+
+                drm_fb = obj_to_fb(obj);
+                psb_fb = to_psb_fb(drm_fb);
+
+                if (gma_power_begin(dev, 0)) {
+                        REG_WRITE(DSPASURF, psb_fb->gtt->offset);
+                        REG_READ(DSPASURF);
+                        gma_power_end(dev);
+                } else {
+                        dev_priv->saveDSPASURF = psb_fb->gtt->offset;
+                }
+
+                return 0;
+        case PSB_MODE_OPERATION_MODE_VALID:
+                umode = &arg->mode;
+
+                mutex_lock(&dev->mode_config.mutex);
+
+                obj = drm_mode_object_find(dev, obj_id,
+                                        DRM_MODE_OBJECT_CONNECTOR);
+                if (!obj) {
+                        ret = -EINVAL;
+                        goto mode_op_out;
+                }
+
+                connector = obj_to_connector(obj);
+
+                mode = drm_mode_create(dev);
+                if (!mode) {
+                        ret = -ENOMEM;
+                        goto mode_op_out;
+                }
+
+                /* drm_crtc_convert_umode(mode, umode); */
+                {
+                        mode->clock = umode->clock;
+                        mode->hdisplay = umode->hdisplay;
+                        mode->hsync_start = umode->hsync_start;
+                        mode->hsync_end = umode->hsync_end;
+                        mode->htotal = umode->htotal;
+                        mode->hskew = umode->hskew;
+                        mode->vdisplay = umode->vdisplay;
+                        mode->vsync_start = umode->vsync_start;
+                        mode->vsync_end = umode->vsync_end;
+                        mode->vtotal = umode->vtotal;
+                        mode->vscan = umode->vscan;
+                        mode->vrefresh = umode->vrefresh;
+                        mode->flags = umode->flags;
+                        mode->type = umode->type;
+                        strncpy(mode->name, umode->name, DRM_DISPLAY_MODE_LEN);
+                        mode->name[DRM_DISPLAY_MODE_LEN-1] = 0;
+                }
+
+                connector_funcs = (struct drm_connector_helper_funcs *)
+                                   connector->helper_private;
+
+                if (connector_funcs->mode_valid) {
+                        resp = connector_funcs->mode_valid(connector, mode);
+                        arg->data = (void *)resp;
+                }
+
+                /*do some clean up work*/
+                if (mode)
+                        drm_mode_destroy(dev, mode);
+mode_op_out:
+                mutex_unlock(&dev->mode_config.mutex);
+                return ret;
+
+        default:
+                dev_dbg(dev->dev, "Unsupported psb mode operation\n");
+                return -EOPNOTSUPP;
+        }
+
+        return 0;
+}
+
+static int psb_stolen_memory_ioctl(struct drm_device *dev, void *data,
+                                   struct drm_file *file_priv)
+{
+        struct drm_psb_private *dev_priv = psb_priv(dev);
+        struct drm_psb_stolen_memory_arg *arg = data;
+
+        arg->base = dev_priv->stolen_base;
+        arg->size = dev_priv->vram_stolen_size;
+
+        return 0;
+}
+
+/* FIXME: needs Medfield changes */
+static int psb_register_rw_ioctl(struct drm_device *dev, void *data,
+                                 struct drm_file *file_priv)
+{
+        struct drm_psb_private *dev_priv = psb_priv(dev);
+        struct drm_psb_register_rw_arg *arg = data;
+        bool usage = arg->b_force_hw_on ? true : false;
+
+        if (arg->display_write_mask != 0) {
+                if (gma_power_begin(dev, usage)) {
+                        if (arg->display_write_mask & REGRWBITS_PFIT_CONTROLS)
+                                PSB_WVDC32(arg->display.pfit_controls,
+                                           PFIT_CONTROL);
+                        if (arg->display_write_mask &
+                            REGRWBITS_PFIT_AUTOSCALE_RATIOS)
+                                PSB_WVDC32(arg->display.pfit_autoscale_ratios,
+                                           PFIT_AUTO_RATIOS);
+                        if (arg->display_write_mask &
+                            REGRWBITS_PFIT_PROGRAMMED_SCALE_RATIOS)
+                                PSB_WVDC32(
+                                   arg->display.pfit_programmed_scale_ratios,
+                                   PFIT_PGM_RATIOS);
+                        if (arg->display_write_mask & REGRWBITS_PIPEASRC)
+                                PSB_WVDC32(arg->display.pipeasrc,
+                                           PIPEASRC);
+                        if (arg->display_write_mask & REGRWBITS_PIPEBSRC)
+                                PSB_WVDC32(arg->display.pipebsrc,
+                                           PIPEBSRC);
+                        if (arg->display_write_mask & REGRWBITS_VTOTAL_A)
+                                PSB_WVDC32(arg->display.vtotal_a,
+                                           VTOTAL_A);
+                        if (arg->display_write_mask & REGRWBITS_VTOTAL_B)
+                                PSB_WVDC32(arg->display.vtotal_b,
+                                           VTOTAL_B);
+                        gma_power_end(dev);
+                } else {
+                        if (arg->display_write_mask & REGRWBITS_PFIT_CONTROLS)
+                                dev_priv->savePFIT_CONTROL =
+                                                arg->display.pfit_controls;
+                        if (arg->display_write_mask &
+                            REGRWBITS_PFIT_AUTOSCALE_RATIOS)
+                                dev_priv->savePFIT_AUTO_RATIOS =
+                                        arg->display.pfit_autoscale_ratios;
+                        if (arg->display_write_mask &
+                            REGRWBITS_PFIT_PROGRAMMED_SCALE_RATIOS)
+                                dev_priv->savePFIT_PGM_RATIOS =
+                                   arg->display.pfit_programmed_scale_ratios;
+                        if (arg->display_write_mask & REGRWBITS_PIPEASRC)
+                                dev_priv->savePIPEASRC = arg->display.pipeasrc;
+                        if (arg->display_write_mask & REGRWBITS_PIPEBSRC)
+                                dev_priv->savePIPEBSRC = arg->display.pipebsrc;
+                        if (arg->display_write_mask & REGRWBITS_VTOTAL_A)
+                                dev_priv->saveVTOTAL_A = arg->display.vtotal_a;
+                        if (arg->display_write_mask & REGRWBITS_VTOTAL_B)
+                                dev_priv->saveVTOTAL_B = arg->display.vtotal_b;
+                }
+        }
+
+        if (arg->display_read_mask != 0) {
+                if (gma_power_begin(dev, usage)) {
+                        if (arg->display_read_mask &
+                            REGRWBITS_PFIT_CONTROLS)
+                                arg->display.pfit_controls =
+                                                PSB_RVDC32(PFIT_CONTROL);
+                        if (arg->display_read_mask &
+                            REGRWBITS_PFIT_AUTOSCALE_RATIOS)
+                                arg->display.pfit_autoscale_ratios =
+                                                PSB_RVDC32(PFIT_AUTO_RATIOS);
+                        if (arg->display_read_mask &
+                            REGRWBITS_PFIT_PROGRAMMED_SCALE_RATIOS)
+                                arg->display.pfit_programmed_scale_ratios =
+                                                PSB_RVDC32(PFIT_PGM_RATIOS);
+                        if (arg->display_read_mask & REGRWBITS_PIPEASRC)
+                                arg->display.pipeasrc = PSB_RVDC32(PIPEASRC);
+                        if (arg->display_read_mask & REGRWBITS_PIPEBSRC)
+                                arg->display.pipebsrc = PSB_RVDC32(PIPEBSRC);
+                        if (arg->display_read_mask & REGRWBITS_VTOTAL_A)
+                                arg->display.vtotal_a = PSB_RVDC32(VTOTAL_A);
+                        if (arg->display_read_mask & REGRWBITS_VTOTAL_B)
+                                arg->display.vtotal_b = PSB_RVDC32(VTOTAL_B);
+                        gma_power_end(dev);
+                } else {
+                        if (arg->display_read_mask &
+                            REGRWBITS_PFIT_CONTROLS)
+                                arg->display.pfit_controls =
+                                                dev_priv->savePFIT_CONTROL;
+                        if (arg->display_read_mask &
+                            REGRWBITS_PFIT_AUTOSCALE_RATIOS)
+                                arg->display.pfit_autoscale_ratios =
+                                                dev_priv->savePFIT_AUTO_RATIOS;
+                        if (arg->display_read_mask &
+                            REGRWBITS_PFIT_PROGRAMMED_SCALE_RATIOS)
+                                arg->display.pfit_programmed_scale_ratios =
+                                                dev_priv->savePFIT_PGM_RATIOS;
+                        if (arg->display_read_mask & REGRWBITS_PIPEASRC)
+                                arg->display.pipeasrc = dev_priv->savePIPEASRC;
+                        if (arg->display_read_mask & REGRWBITS_PIPEBSRC)
+                                arg->display.pipebsrc = dev_priv->savePIPEBSRC;
+                        if (arg->display_read_mask & REGRWBITS_VTOTAL_A)
+                                arg->display.vtotal_a = dev_priv->saveVTOTAL_A;
+                        if (arg->display_read_mask & REGRWBITS_VTOTAL_B)
+                                arg->display.vtotal_b = dev_priv->saveVTOTAL_B;
+                }
+        }
+
+        if (arg->overlay_write_mask != 0) {
+                if (gma_power_begin(dev, usage)) {
+                        if (arg->overlay_write_mask & OV_REGRWBITS_OGAM_ALL) {
+                                PSB_WVDC32(arg->overlay.OGAMC5, OV_OGAMC5);
+                                PSB_WVDC32(arg->overlay.OGAMC4, OV_OGAMC4);
+                                PSB_WVDC32(arg->overlay.OGAMC3, OV_OGAMC3);
+                                PSB_WVDC32(arg->overlay.OGAMC2, OV_OGAMC2);
+                                PSB_WVDC32(arg->overlay.OGAMC1, OV_OGAMC1);
+                                PSB_WVDC32(arg->overlay.OGAMC0, OV_OGAMC0);
+                        }
+                        if (arg->overlay_write_mask & OVC_REGRWBITS_OGAM_ALL) {
+                                PSB_WVDC32(arg->overlay.OGAMC5, OVC_OGAMC5);
+                                PSB_WVDC32(arg->overlay.OGAMC4, OVC_OGAMC4);
+                                PSB_WVDC32(arg->overlay.OGAMC3, OVC_OGAMC3);
+                                PSB_WVDC32(arg->overlay.OGAMC2, OVC_OGAMC2);
+                                PSB_WVDC32(arg->overlay.OGAMC1, OVC_OGAMC1);
+                                PSB_WVDC32(arg->overlay.OGAMC0, OVC_OGAMC0);
+                        }
+
+                        if (arg->overlay_write_mask & OV_REGRWBITS_OVADD) {
+                                PSB_WVDC32(arg->overlay.OVADD, OV_OVADD);
+
+                                if (arg->overlay.b_wait_vblank) {
+                                        /* Wait for 20ms.*/
+                                        unsigned long vblank_timeout = jiffies
+                                                                + HZ/50;
+                                        uint32_t temp;
+                                        while (time_before_eq(jiffies,
+                                                        vblank_timeout)) {
+                                                temp = PSB_RVDC32(OV_DOVASTA);
+                                                if ((temp & (0x1 << 31)) != 0)
+                                                        break;
+                                                cpu_relax();
+                                        }
+                                }
+                        }
+                        if (arg->overlay_write_mask & OVC_REGRWBITS_OVADD) {
+                                PSB_WVDC32(arg->overlay.OVADD, OVC_OVADD);
+                                if (arg->overlay.b_wait_vblank) {
+                                        /* Wait for 20ms.*/
+                                        unsigned long vblank_timeout =
+                                                        jiffies + HZ/50;
+                                        uint32_t temp;
+                                        while (time_before_eq(jiffies,
+                                                        vblank_timeout)) {
+                                                temp = PSB_RVDC32(OVC_DOVCSTA);
+                                                if ((temp & (0x1 << 31)) != 0)
+                                                        break;
+                                                cpu_relax();
+                                        }
+                                }
+                        }
+                        gma_power_end(dev);
+                } else {
+                        if (arg->overlay_write_mask & OV_REGRWBITS_OGAM_ALL) {
+                                dev_priv->saveOV_OGAMC5 = arg->overlay.OGAMC5;
+                                dev_priv->saveOV_OGAMC4 = arg->overlay.OGAMC4;
+                                dev_priv->saveOV_OGAMC3 = arg->overlay.OGAMC3;
+                                dev_priv->saveOV_OGAMC2 = arg->overlay.OGAMC2;
+                                dev_priv->saveOV_OGAMC1 = arg->overlay.OGAMC1;
+                                dev_priv->saveOV_OGAMC0 = arg->overlay.OGAMC0;
+                        }
+                        if (arg->overlay_write_mask & OVC_REGRWBITS_OGAM_ALL) {
+                                dev_priv->saveOVC_OGAMC5 = arg->overlay.OGAMC5;
+                                dev_priv->saveOVC_OGAMC4 = arg->overlay.OGAMC4;
+                                dev_priv->saveOVC_OGAMC3 = arg->overlay.OGAMC3;
+                                dev_priv->saveOVC_OGAMC2 = arg->overlay.OGAMC2;
+                                dev_priv->saveOVC_OGAMC1 = arg->overlay.OGAMC1;
+                                dev_priv->saveOVC_OGAMC0 = arg->overlay.OGAMC0;
+                        }
+                        if (arg->overlay_write_mask & OV_REGRWBITS_OVADD)
+                                dev_priv->saveOV_OVADD = arg->overlay.OVADD;
+                        if (arg->overlay_write_mask & OVC_REGRWBITS_OVADD)
+                                dev_priv->saveOVC_OVADD = arg->overlay.OVADD;
+                }
+        }
+
+        if (arg->overlay_read_mask != 0) {
+                if (gma_power_begin(dev, usage)) {
+                        if (arg->overlay_read_mask & OV_REGRWBITS_OGAM_ALL) {
+                                arg->overlay.OGAMC5 = PSB_RVDC32(OV_OGAMC5);
+                                arg->overlay.OGAMC4 = PSB_RVDC32(OV_OGAMC4);
+                                arg->overlay.OGAMC3 = PSB_RVDC32(OV_OGAMC3);
+                                arg->overlay.OGAMC2 = PSB_RVDC32(OV_OGAMC2);
+                                arg->overlay.OGAMC1 = PSB_RVDC32(OV_OGAMC1);
+                                arg->overlay.OGAMC0 = PSB_RVDC32(OV_OGAMC0);
+                        }
+                        if (arg->overlay_read_mask & OVC_REGRWBITS_OGAM_ALL) {
+                                arg->overlay.OGAMC5 = PSB_RVDC32(OVC_OGAMC5);
+                                arg->overlay.OGAMC4 = PSB_RVDC32(OVC_OGAMC4);
+                                arg->overlay.OGAMC3 = PSB_RVDC32(OVC_OGAMC3);
+                                arg->overlay.OGAMC2 = PSB_RVDC32(OVC_OGAMC2);
+                                arg->overlay.OGAMC1 = PSB_RVDC32(OVC_OGAMC1);
+                                arg->overlay.OGAMC0 = PSB_RVDC32(OVC_OGAMC0);
+                        }
+                        if (arg->overlay_read_mask & OV_REGRWBITS_OVADD)
+                                arg->overlay.OVADD = PSB_RVDC32(OV_OVADD);
+                        if (arg->overlay_read_mask & OVC_REGRWBITS_OVADD)
+                                arg->overlay.OVADD = PSB_RVDC32(OVC_OVADD);
+                        gma_power_end(dev);
+                } else {
+                        if (arg->overlay_read_mask & OV_REGRWBITS_OGAM_ALL) {
+                                arg->overlay.OGAMC5 = dev_priv->saveOV_OGAMC5;
+                                arg->overlay.OGAMC4 = dev_priv->saveOV_OGAMC4;
+                                arg->overlay.OGAMC3 = dev_priv->saveOV_OGAMC3;
+                                arg->overlay.OGAMC2 = dev_priv->saveOV_OGAMC2;
+                                arg->overlay.OGAMC1 = dev_priv->saveOV_OGAMC1;
+                                arg->overlay.OGAMC0 = dev_priv->saveOV_OGAMC0;
+                        }
+                        if (arg->overlay_read_mask & OVC_REGRWBITS_OGAM_ALL) {
+                                arg->overlay.OGAMC5 = dev_priv->saveOVC_OGAMC5;
+                                arg->overlay.OGAMC4 = dev_priv->saveOVC_OGAMC4;
+                                arg->overlay.OGAMC3 = dev_priv->saveOVC_OGAMC3;
+                                arg->overlay.OGAMC2 = dev_priv->saveOVC_OGAMC2;
+                                arg->overlay.OGAMC1 = dev_priv->saveOVC_OGAMC1;
+                                arg->overlay.OGAMC0 = dev_priv->saveOVC_OGAMC0;
+                        }
+                        if (arg->overlay_read_mask & OV_REGRWBITS_OVADD)
+                                arg->overlay.OVADD = dev_priv->saveOV_OVADD;
+                        if (arg->overlay_read_mask & OVC_REGRWBITS_OVADD)
+                                arg->overlay.OVADD = dev_priv->saveOVC_OVADD;
+                }
+        }
+
+        if (arg->sprite_enable_mask != 0) {
+                if (gma_power_begin(dev, usage)) {
+                        PSB_WVDC32(0x1F3E, DSPARB);
+                        PSB_WVDC32(arg->sprite.dspa_control
+                                        | PSB_RVDC32(DSPACNTR), DSPACNTR);
+                        PSB_WVDC32(arg->sprite.dspa_key_value, DSPAKEYVAL);
+                        PSB_WVDC32(arg->sprite.dspa_key_mask, DSPAKEYMASK);
+                        PSB_WVDC32(PSB_RVDC32(DSPASURF), DSPASURF);
+                        PSB_RVDC32(DSPASURF);
+                        PSB_WVDC32(arg->sprite.dspc_control, DSPCCNTR);
+                        PSB_WVDC32(arg->sprite.dspc_stride, DSPCSTRIDE);
+                        PSB_WVDC32(arg->sprite.dspc_position, DSPCPOS);
+                        PSB_WVDC32(arg->sprite.dspc_linear_offset, DSPCLINOFF);
+                        PSB_WVDC32(arg->sprite.dspc_size, DSPCSIZE);
+                        PSB_WVDC32(arg->sprite.dspc_surface, DSPCSURF);
+                        PSB_RVDC32(DSPCSURF);
+                        gma_power_end(dev);
+                }
+        }
+
+        if (arg->sprite_disable_mask != 0) {
+                if (gma_power_begin(dev, usage)) {
+                        PSB_WVDC32(0x3F3E, DSPARB);
+                        PSB_WVDC32(0x0, DSPCCNTR);
+                        PSB_WVDC32(arg->sprite.dspc_surface, DSPCSURF);
+                        PSB_RVDC32(DSPCSURF);
+                        gma_power_end(dev);
+                }
+        }
+
+        if (arg->subpicture_enable_mask != 0) {
+                if (gma_power_begin(dev, usage)) {
+                        uint32_t temp;
+                        if (arg->subpicture_enable_mask & REGRWBITS_DSPACNTR) {
+                                temp =  PSB_RVDC32(DSPACNTR);
+                                temp &= ~DISPPLANE_PIXFORMAT_MASK;
+                                temp &= ~DISPPLANE_BOTTOM;
+                                temp |= DISPPLANE_32BPP;
+                                PSB_WVDC32(temp, DSPACNTR);
+
+                                temp =  PSB_RVDC32(DSPABASE);
+                                PSB_WVDC32(temp, DSPABASE);
+                                PSB_RVDC32(DSPABASE);
+                                temp =  PSB_RVDC32(DSPASURF);
+                                PSB_WVDC32(temp, DSPASURF);
+                                PSB_RVDC32(DSPASURF);
+                        }
+                        if (arg->subpicture_enable_mask & REGRWBITS_DSPBCNTR) {
+                                temp =  PSB_RVDC32(DSPBCNTR);
+                                temp &= ~DISPPLANE_PIXFORMAT_MASK;
+                                temp &= ~DISPPLANE_BOTTOM;
+                                temp |= DISPPLANE_32BPP;
+                                PSB_WVDC32(temp, DSPBCNTR);
+
+                                temp =  PSB_RVDC32(DSPBBASE);
+                                PSB_WVDC32(temp, DSPBBASE);
+                                PSB_RVDC32(DSPBBASE);
+                                temp =  PSB_RVDC32(DSPBSURF);
+                                PSB_WVDC32(temp, DSPBSURF);
+                                PSB_RVDC32(DSPBSURF);
+                        }
+                        if (arg->subpicture_enable_mask & REGRWBITS_DSPCCNTR) {
+                                temp =  PSB_RVDC32(DSPCCNTR);
+                                temp &= ~DISPPLANE_PIXFORMAT_MASK;
+                                temp &= ~DISPPLANE_BOTTOM;
+                                temp |= DISPPLANE_32BPP;
+                                PSB_WVDC32(temp, DSPCCNTR);
+
+                                temp =  PSB_RVDC32(DSPCBASE);
+                                PSB_WVDC32(temp, DSPCBASE);
+                                PSB_RVDC32(DSPCBASE);
+                                temp =  PSB_RVDC32(DSPCSURF);
+                                PSB_WVDC32(temp, DSPCSURF);
+                                PSB_RVDC32(DSPCSURF);
+                        }
+                        gma_power_end(dev);
+                }
+        }
+
+        if (arg->subpicture_disable_mask != 0) {
+                if (gma_power_begin(dev, usage)) {
+                        uint32_t temp;
+                        if (arg->subpicture_disable_mask & REGRWBITS_DSPACNTR) {
+                                temp =  PSB_RVDC32(DSPACNTR);
+                                temp &= ~DISPPLANE_PIXFORMAT_MASK;
+                                temp |= DISPPLANE_32BPP_NO_ALPHA;
+                                PSB_WVDC32(temp, DSPACNTR);
+
+                                temp =  PSB_RVDC32(DSPABASE);
+                                PSB_WVDC32(temp, DSPABASE);
+                                PSB_RVDC32(DSPABASE);
+                                temp =  PSB_RVDC32(DSPASURF);
+                                PSB_WVDC32(temp, DSPASURF);
+                                PSB_RVDC32(DSPASURF);
+                        }
+                        if (arg->subpicture_disable_mask & REGRWBITS_DSPBCNTR) {
+                                temp =  PSB_RVDC32(DSPBCNTR);
+                                temp &= ~DISPPLANE_PIXFORMAT_MASK;
+                                temp |= DISPPLANE_32BPP_NO_ALPHA;
+                                PSB_WVDC32(temp, DSPBCNTR);
+
+                                temp =  PSB_RVDC32(DSPBBASE);
+                                PSB_WVDC32(temp, DSPBBASE);
+                                PSB_RVDC32(DSPBBASE);
+                                temp =  PSB_RVDC32(DSPBSURF);
+                                PSB_WVDC32(temp, DSPBSURF);
+                                PSB_RVDC32(DSPBSURF);
+                        }
+                        if (arg->subpicture_disable_mask & REGRWBITS_DSPCCNTR) {
+                                temp =  PSB_RVDC32(DSPCCNTR);
+                                temp &= ~DISPPLANE_PIXFORMAT_MASK;
+                                temp |= DISPPLANE_32BPP_NO_ALPHA;
+                                PSB_WVDC32(temp, DSPCCNTR);
+
+                                temp =  PSB_RVDC32(DSPCBASE);
+                                PSB_WVDC32(temp, DSPCBASE);
+                                PSB_RVDC32(DSPCBASE);
+                                temp =  PSB_RVDC32(DSPCSURF);
+                                PSB_WVDC32(temp, DSPCSURF);
+                                PSB_RVDC32(DSPCSURF);
+                        }
+                        gma_power_end(dev);
+                }
+        }
+
+        return 0;
+}
+
+static int psb_driver_open(struct drm_device *dev, struct drm_file *priv)
+{
+        return 0;
+}
+
+static void psb_driver_close(struct drm_device *dev, struct drm_file *priv)
+{
+}
+
+static long psb_unlocked_ioctl(struct file *filp, unsigned int cmd,
+                               unsigned long arg)
+{
+        struct drm_file *file_priv = filp->private_data;
+        struct drm_device *dev = file_priv->minor->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        static unsigned int runtime_allowed;
+
+        if (runtime_allowed == 1 && dev_priv->is_lvds_on) {
+                runtime_allowed++;
+                pm_runtime_allow(&dev->pdev->dev);
+                dev_priv->rpm_enabled = 1;
+        }
+        return drm_ioctl(filp, cmd, arg);
+        /* FIXME: do we need to wrap the other side of this */
+}
+
+
+/* When a client dies:
+ *    - Check for and clean up flipped page state
+ */
+void psb_driver_preclose(struct drm_device *dev, struct drm_file *priv)
+{
+}
+
+static void psb_remove(struct pci_dev *pdev)
+{
+        struct drm_device *dev = pci_get_drvdata(pdev);
+        drm_put_dev(dev);
+}
+
+static const struct dev_pm_ops psb_pm_ops = {
+        .resume = gma_power_resume,
+        .suspend = gma_power_suspend,
+        .runtime_suspend = psb_runtime_suspend,
+        .runtime_resume = psb_runtime_resume,
+        .runtime_idle = psb_runtime_idle,
+};
+
+static struct vm_operations_struct psb_gem_vm_ops = {
+        .fault = psb_gem_fault,
+        .open = drm_gem_vm_open,
+        .close = drm_gem_vm_close,
+};
+
+static struct drm_driver driver = {
+        .driver_features = DRIVER_HAVE_IRQ | DRIVER_IRQ_SHARED | \
+                           DRIVER_IRQ_VBL | DRIVER_MODESET | DRIVER_GEM ,
+        .load = psb_driver_load,
+        .unload = psb_driver_unload,
+
+        .ioctls = psb_ioctls,
+        .num_ioctls = DRM_ARRAY_SIZE(psb_ioctls),
+        .device_is_agp = psb_driver_device_is_agp,
+        .irq_preinstall = psb_irq_preinstall,
+        .irq_postinstall = psb_irq_postinstall,
+        .irq_uninstall = psb_irq_uninstall,
+        .irq_handler = psb_irq_handler,
+        .enable_vblank = psb_enable_vblank,
+        .disable_vblank = psb_disable_vblank,
+        .get_vblank_counter = psb_get_vblank_counter,
+        .lastclose = psb_lastclose,
+        .open = psb_driver_open,
+        .preclose = psb_driver_preclose,
+        .postclose = psb_driver_close,
+        .reclaim_buffers = drm_core_reclaim_buffers,
+
+        .gem_init_object = psb_gem_init_object,
+        .gem_free_object = psb_gem_free_object,
+        .gem_vm_ops = &psb_gem_vm_ops,
+        .dumb_create = psb_gem_dumb_create,
+        .dumb_map_offset = psb_gem_dumb_map_gtt,
+        .dumb_destroy = psb_gem_dumb_destroy,
+
+        .fops = {
+                 .owner = THIS_MODULE,
+                 .open = drm_open,
+                 .release = drm_release,
+                 .unlocked_ioctl = psb_unlocked_ioctl,
+                 .mmap = drm_gem_mmap,
+                 .poll = drm_poll,
+                 .fasync = drm_fasync,
+                 .read = drm_read,
+         },
+        .name = DRIVER_NAME,
+        .desc = DRIVER_DESC,
+        .date = PSB_DRM_DRIVER_DATE,
+        .major = PSB_DRM_DRIVER_MAJOR,
+        .minor = PSB_DRM_DRIVER_MINOR,
+        .patchlevel = PSB_DRM_DRIVER_PATCHLEVEL
+};
+
+static struct pci_driver psb_pci_driver = {
+        .name = DRIVER_NAME,
+        .id_table = pciidlist,
+        .probe = psb_probe,
+        .remove = psb_remove,
+        .driver.pm = &psb_pm_ops,
+};
+
+static int psb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+        /* MLD Added this from Inaky's patch */
+        if (pci_enable_msi(pdev))
+                dev_warn(&pdev->dev, "Enable MSI failed!\n");
+        return drm_get_pci_dev(pdev, ent, &driver);
+}
+
+static int __init psb_init(void)
+{
+        return drm_pci_init(&driver, &psb_pci_driver);
+}
+
+static void __exit psb_exit(void)
+{
+        drm_pci_exit(&driver, &psb_pci_driver);
+}
+
+late_initcall(psb_init);
+module_exit(psb_exit);
+
+MODULE_AUTHOR("Alan Cox <alan@linux.intel.com> and others");
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/gma500/psb_drv.h b/drivers/staging/gma500/psb_drv.h
new file mode 100644
index 00000000000..fd4732dd783
--- /dev/null
+++ b/drivers/staging/gma500/psb_drv.h
@@ -0,0 +1,948 @@
+/**************************************************************************
+ * Copyright (c) 2007-2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ **************************************************************************/
+
+#ifndef _PSB_DRV_H_
+#define _PSB_DRV_H_
+
+#include <linux/kref.h>
+
+#include <drm/drmP.h>
+#include "drm_global.h"
+#include "gem_glue.h"
+#include "psb_drm.h"
+#include "psb_reg.h"
+#include "psb_intel_drv.h"
+#include "gtt.h"
+#include "power.h"
+#include "mrst.h"
+#include "medfield.h"
+
+/* Append new drm mode definition here, align with libdrm definition */
+#define DRM_MODE_SCALE_NO_SCALE         2
+
+enum {
+        CHIP_PSB_8108 = 0,              /* Poulsbo */
+        CHIP_PSB_8109 = 1,              /* Poulsbo */
+        CHIP_MRST_4100 = 2,             /* Moorestown/Oaktrail */
+        CHIP_MFLD_0130 = 3,             /* Medfield */
+};
+
+#define IS_MRST(dev) (((dev)->pci_device & 0xfffc) == 0x4100)
+#define IS_MFLD(dev) (((dev)->pci_device & 0xfff8) == 0x0130)
+
+/*
+ * Driver definitions
+ */
+
+#define DRIVER_NAME "gma500"
+#define DRIVER_DESC "DRM driver for the Intel GMA500"
+
+#define PSB_DRM_DRIVER_DATE "2011-06-06"
+#define PSB_DRM_DRIVER_MAJOR 1
+#define PSB_DRM_DRIVER_MINOR 0
+#define PSB_DRM_DRIVER_PATCHLEVEL 0
+
+/*
+ *      Hardware offsets
+ */
+#define PSB_VDC_OFFSET           0x00000000
+#define PSB_VDC_SIZE             0x000080000
+#define MRST_MMIO_SIZE           0x0000C0000
+#define MDFLD_MMIO_SIZE          0x000100000
+#define PSB_SGX_SIZE             0x8000
+#define PSB_SGX_OFFSET           0x00040000
+#define MRST_SGX_OFFSET          0x00080000
+/*
+ *      PCI resource identifiers
+ */
+#define PSB_MMIO_RESOURCE        0
+#define PSB_GATT_RESOURCE        2
+#define PSB_GTT_RESOURCE         3
+/*
+ *      PCI configuration
+ */
+#define PSB_GMCH_CTRL            0x52
+#define PSB_BSM                  0x5C
+#define _PSB_GMCH_ENABLED        0x4
+#define PSB_PGETBL_CTL           0x2020
+#define _PSB_PGETBL_ENABLED      0x00000001
+#define PSB_SGX_2D_SLAVE_PORT    0x4000
+
+/* To get rid of */
+#define PSB_TT_PRIV0_LIMIT       (256*1024*1024)
+#define PSB_TT_PRIV0_PLIMIT      (PSB_TT_PRIV0_LIMIT >> PAGE_SHIFT)
+
+/*
+ *      SGX side MMU definitions (these can probably go)
+ */
+
+/*
+ *      Flags for external memory type field.
+ */
+#define PSB_MMU_CACHED_MEMORY     0x0001        /* Bind to MMU only */
+#define PSB_MMU_RO_MEMORY         0x0002        /* MMU RO memory */
+#define PSB_MMU_WO_MEMORY         0x0004        /* MMU WO memory */
+/*
+ *      PTE's and PDE's
+ */
+#define PSB_PDE_MASK              0x003FFFFF
+#define PSB_PDE_SHIFT             22
+#define PSB_PTE_SHIFT             12
+/*
+ *      Cache control
+ */
+#define PSB_PTE_VALID             0x0001        /* PTE / PDE valid */
+#define PSB_PTE_WO                0x0002        /* Write only */
+#define PSB_PTE_RO                0x0004        /* Read only */
+#define PSB_PTE_CACHED            0x0008        /* CPU cache coherent */
+
+/*
+ *      VDC registers and bits
+ */
+#define PSB_MSVDX_CLOCKGATING     0x2064
+#define PSB_TOPAZ_CLOCKGATING     0x2068
+#define PSB_HWSTAM                0x2098
+#define PSB_INSTPM                0x20C0
+#define PSB_INT_IDENTITY_R        0x20A4
+#define _MDFLD_PIPEC_EVENT_FLAG   (1<<2)
+#define _MDFLD_PIPEC_VBLANK_FLAG  (1<<3)
+#define _PSB_DPST_PIPEB_FLAG      (1<<4)
+#define _MDFLD_PIPEB_EVENT_FLAG   (1<<4)
+#define _PSB_VSYNC_PIPEB_FLAG     (1<<5)
+#define _PSB_DPST_PIPEA_FLAG      (1<<6)
+#define _PSB_PIPEA_EVENT_FLAG     (1<<6)
+#define _PSB_VSYNC_PIPEA_FLAG     (1<<7)
+#define _MDFLD_MIPIA_FLAG         (1<<16)
+#define _MDFLD_MIPIC_FLAG         (1<<17)
+#define _PSB_IRQ_SGX_FLAG         (1<<18)
+#define _PSB_IRQ_MSVDX_FLAG       (1<<19)
+#define _LNC_IRQ_TOPAZ_FLAG       (1<<20)
+
+/* This flag includes all the display IRQ bits excepts the vblank irqs. */
+#define _MDFLD_DISP_ALL_IRQ_FLAG (_MDFLD_PIPEC_EVENT_FLAG | \
+                                  _MDFLD_PIPEB_EVENT_FLAG | \
+                                  _PSB_PIPEA_EVENT_FLAG | \
+                                  _PSB_VSYNC_PIPEA_FLAG | \
+                                  _MDFLD_MIPIA_FLAG | \
+                                  _MDFLD_MIPIC_FLAG)
+#define PSB_INT_IDENTITY_R        0x20A4
+#define PSB_INT_MASK_R            0x20A8
+#define PSB_INT_ENABLE_R          0x20A0
+
+#define _PSB_MMU_ER_MASK      0x0001FF00
+#define _PSB_MMU_ER_HOST      (1 << 16)
+#define GPIOA                   0x5010
+#define GPIOB                   0x5014
+#define GPIOC                   0x5018
+#define GPIOD                   0x501c
+#define GPIOE                   0x5020
+#define GPIOF                   0x5024
+#define GPIOG                   0x5028
+#define GPIOH                   0x502c
+#define GPIO_CLOCK_DIR_MASK             (1 << 0)
+#define GPIO_CLOCK_DIR_IN               (0 << 1)
+#define GPIO_CLOCK_DIR_OUT              (1 << 1)
+#define GPIO_CLOCK_VAL_MASK             (1 << 2)
+#define GPIO_CLOCK_VAL_OUT              (1 << 3)
+#define GPIO_CLOCK_VAL_IN               (1 << 4)
+#define GPIO_CLOCK_PULLUP_DISABLE       (1 << 5)
+#define GPIO_DATA_DIR_MASK              (1 << 8)
+#define GPIO_DATA_DIR_IN                (0 << 9)
+#define GPIO_DATA_DIR_OUT               (1 << 9)
+#define GPIO_DATA_VAL_MASK              (1 << 10)
+#define GPIO_DATA_VAL_OUT               (1 << 11)
+#define GPIO_DATA_VAL_IN                (1 << 12)
+#define GPIO_DATA_PULLUP_DISABLE        (1 << 13)
+
+#define VCLK_DIVISOR_VGA0   0x6000
+#define VCLK_DIVISOR_VGA1   0x6004
+#define VCLK_POST_DIV       0x6010
+
+#define PSB_COMM_2D (PSB_ENGINE_2D << 4)
+#define PSB_COMM_3D (PSB_ENGINE_3D << 4)
+#define PSB_COMM_TA (PSB_ENGINE_TA << 4)
+#define PSB_COMM_HP (PSB_ENGINE_HP << 4)
+#define PSB_COMM_USER_IRQ (1024 >> 2)
+#define PSB_COMM_USER_IRQ_LOST (PSB_COMM_USER_IRQ + 1)
+#define PSB_COMM_FW (2048 >> 2)
+
+#define PSB_UIRQ_VISTEST               1
+#define PSB_UIRQ_OOM_REPLY             2
+#define PSB_UIRQ_FIRE_TA_REPLY         3
+#define PSB_UIRQ_FIRE_RASTER_REPLY     4
+
+#define PSB_2D_SIZE (256*1024*1024)
+#define PSB_MAX_RELOC_PAGES 1024
+
+#define PSB_LOW_REG_OFFS 0x0204
+#define PSB_HIGH_REG_OFFS 0x0600
+
+#define PSB_NUM_VBLANKS 2
+
+
+#define PSB_2D_SIZE (256*1024*1024)
+#define PSB_MAX_RELOC_PAGES 1024
+
+#define PSB_LOW_REG_OFFS 0x0204
+#define PSB_HIGH_REG_OFFS 0x0600
+
+#define PSB_NUM_VBLANKS 2
+#define PSB_WATCHDOG_DELAY (DRM_HZ * 2)
+#define PSB_LID_DELAY (DRM_HZ / 10)
+
+#define MDFLD_PNW_B0 0x04
+#define MDFLD_PNW_C0 0x08
+
+#define MDFLD_DSR_2D_3D_0       (1 << 0)
+#define MDFLD_DSR_2D_3D_2       (1 << 1)
+#define MDFLD_DSR_CURSOR_0      (1 << 2)
+#define MDFLD_DSR_CURSOR_2      (1 << 3)
+#define MDFLD_DSR_OVERLAY_0     (1 << 4)
+#define MDFLD_DSR_OVERLAY_2     (1 << 5)
+#define MDFLD_DSR_MIPI_CONTROL  (1 << 6)
+#define MDFLD_DSR_DAMAGE_MASK_0 ((1 << 0) | (1 << 2) | (1 << 4))
+#define MDFLD_DSR_DAMAGE_MASK_2 ((1 << 1) | (1 << 3) | (1 << 5))
+#define MDFLD_DSR_2D_3D         (MDFLD_DSR_2D_3D_0 | MDFLD_DSR_2D_3D_2)
+
+#define MDFLD_DSR_RR            45
+#define MDFLD_DPU_ENABLE        (1 << 31)
+#define MDFLD_DSR_FULLSCREEN    (1 << 30)
+#define MDFLD_DSR_DELAY         (DRM_HZ / MDFLD_DSR_RR)
+
+#define PSB_PWR_STATE_ON                1
+#define PSB_PWR_STATE_OFF               2
+
+#define PSB_PMPOLICY_NOPM               0
+#define PSB_PMPOLICY_CLOCKGATING        1
+#define PSB_PMPOLICY_POWERDOWN          2
+
+#define PSB_PMSTATE_POWERUP             0
+#define PSB_PMSTATE_CLOCKGATED          1
+#define PSB_PMSTATE_POWERDOWN           2
+#define PSB_PCIx_MSI_ADDR_LOC           0x94
+#define PSB_PCIx_MSI_DATA_LOC           0x98
+
+/* Medfield crystal settings */
+#define KSEL_CRYSTAL_19 1
+#define KSEL_BYPASS_19 5
+#define KSEL_BYPASS_25 6
+#define KSEL_BYPASS_83_100 7
+
+struct opregion_header;
+struct opregion_acpi;
+struct opregion_swsci;
+struct opregion_asle;
+
+struct psb_intel_opregion {
+        struct opregion_header *header;
+        struct opregion_acpi *acpi;
+        struct opregion_swsci *swsci;
+        struct opregion_asle *asle;
+        int enabled;
+};
+
+struct psb_ops;
+
+struct drm_psb_private {
+        struct drm_device *dev;
+        const struct psb_ops *ops;
+
+        struct psb_gtt gtt;
+
+        /* GTT Memory manager */
+        struct psb_gtt_mm *gtt_mm;
+        struct page *scratch_page;
+        u32 *gtt_map;
+        uint32_t stolen_base;
+        void *vram_addr;
+        unsigned long vram_stolen_size;
+        int gtt_initialized;
+        u16 gmch_ctrl;          /* Saved GTT setup */
+        u32 pge_ctl;
+
+        struct mutex gtt_mutex;
+        struct resource *gtt_mem;       /* Our PCI resource */
+
+        struct psb_mmu_driver *mmu;
+        struct psb_mmu_pd *pf_pd;
+
+        /*
+         * Register base
+         */
+
+        uint8_t *sgx_reg;
+        uint8_t *vdc_reg;
+        uint32_t gatt_free_offset;
+
+        /*
+         * Fencing / irq.
+         */
+
+        uint32_t vdc_irq_mask;
+        uint32_t pipestat[PSB_NUM_PIPE];
+
+        spinlock_t irqmask_lock;
+
+        /*
+         * Power
+         */
+
+        bool suspended;
+        bool display_power;
+        int display_count;
+
+        /*
+         * Modesetting
+         */
+        struct psb_intel_mode_device mode_dev;
+
+        struct drm_crtc *plane_to_crtc_mapping[PSB_NUM_PIPE];
+        struct drm_crtc *pipe_to_crtc_mapping[PSB_NUM_PIPE];
+        uint32_t num_pipe;
+
+        /*
+         * OSPM info (Power management base) (can go ?)
+         */
+        uint32_t ospm_base;
+
+        /*
+         * Sizes info
+         */
+
+        struct drm_psb_sizes_arg sizes;
+
+        u32 fuse_reg_value;
+        u32 video_device_fuse;
+
+        /* PCI revision ID for B0:D2:F0 */
+        uint8_t platform_rev_id;
+
+        /*
+         * LVDS info
+         */
+        int backlight_duty_cycle;       /* restore backlight to this value */
+        bool panel_wants_dither;
+        struct drm_display_mode *panel_fixed_mode;
+        struct drm_display_mode *lfp_lvds_vbt_mode;
+        struct drm_display_mode *sdvo_lvds_vbt_mode;
+
+        struct bdb_lvds_backlight *lvds_bl; /* LVDS backlight info from VBT */
+        struct psb_intel_i2c_chan *lvds_i2c_bus;
+
+        /* Feature bits from the VBIOS */
+        unsigned int int_tv_support:1;
+        unsigned int lvds_dither:1;
+        unsigned int lvds_vbt:1;
+        unsigned int int_crt_support:1;
+        unsigned int lvds_use_ssc:1;
+        int lvds_ssc_freq;
+        bool is_lvds_on;
+        bool is_mipi_on;
+        u32 mipi_ctrl_display;
+
+        unsigned int core_freq;
+        uint32_t iLVDS_enable;
+
+        /* Runtime PM state */
+        int rpm_enabled;
+
+        /* MID specific */
+        struct mrst_vbt vbt_data;
+        struct mrst_gct_data gct_data;
+
+        /* MIPI Panel type etc */
+        int panel_id;
+        bool dual_mipi;         /* dual display - DPI & DBI */
+        bool dpi_panel_on;      /* The DPI panel power is on */
+        bool dpi_panel_on2;     /* The DPI panel power is on */
+        bool dbi_panel_on;      /* The DBI panel power is on */
+        bool dbi_panel_on2;     /* The DBI panel power is on */
+        u32 dsr_fb_update;      /* DSR FB update counter */
+
+        /* Moorestown HDMI state */
+        struct mrst_hdmi_dev *hdmi_priv;
+
+        /* Moorestown pipe config register value cache */
+        uint32_t pipeconf;
+        uint32_t pipeconf1;
+        uint32_t pipeconf2;
+
+        /* Moorestown plane control register value cache */
+        uint32_t dspcntr;
+        uint32_t dspcntr1;
+        uint32_t dspcntr2;
+
+        /* Moorestown MM backlight cache */
+        uint8_t saveBKLTCNT;
+        uint8_t saveBKLTREQ;
+        uint8_t saveBKLTBRTL;
+
+        /*
+         * Register state
+         */
+        uint32_t saveDSPACNTR;
+        uint32_t saveDSPBCNTR;
+        uint32_t savePIPEACONF;
+        uint32_t savePIPEBCONF;
+        uint32_t savePIPEASRC;
+        uint32_t savePIPEBSRC;
+        uint32_t saveFPA0;
+        uint32_t saveFPA1;
+        uint32_t saveDPLL_A;
+        uint32_t saveDPLL_A_MD;
+        uint32_t saveHTOTAL_A;
+        uint32_t saveHBLANK_A;
+        uint32_t saveHSYNC_A;
+        uint32_t saveVTOTAL_A;
+        uint32_t saveVBLANK_A;
+        uint32_t saveVSYNC_A;
+        uint32_t saveDSPASTRIDE;
+        uint32_t saveDSPASIZE;
+        uint32_t saveDSPAPOS;
+        uint32_t saveDSPABASE;
+        uint32_t saveDSPASURF;
+        uint32_t saveDSPASTATUS;
+        uint32_t saveFPB0;
+        uint32_t saveFPB1;
+        uint32_t saveDPLL_B;
+        uint32_t saveDPLL_B_MD;
+        uint32_t saveHTOTAL_B;
+        uint32_t saveHBLANK_B;
+        uint32_t saveHSYNC_B;
+        uint32_t saveVTOTAL_B;
+        uint32_t saveVBLANK_B;
+        uint32_t saveVSYNC_B;
+        uint32_t saveDSPBSTRIDE;
+        uint32_t saveDSPBSIZE;
+        uint32_t saveDSPBPOS;
+        uint32_t saveDSPBBASE;
+        uint32_t saveDSPBSURF;
+        uint32_t saveDSPBSTATUS;
+        uint32_t saveVCLK_DIVISOR_VGA0;
+        uint32_t saveVCLK_DIVISOR_VGA1;
+        uint32_t saveVCLK_POST_DIV;
+        uint32_t saveVGACNTRL;
+        uint32_t saveADPA;
+        uint32_t saveLVDS;
+        uint32_t saveDVOA;
+        uint32_t saveDVOB;
+        uint32_t saveDVOC;
+        uint32_t savePP_ON;
+        uint32_t savePP_OFF;
+        uint32_t savePP_CONTROL;
+        uint32_t savePP_CYCLE;
+        uint32_t savePFIT_CONTROL;
+        uint32_t savePaletteA[256];
+        uint32_t savePaletteB[256];
+        uint32_t saveBLC_PWM_CTL2;
+        uint32_t saveBLC_PWM_CTL;
+        uint32_t saveCLOCKGATING;
+        uint32_t saveDSPARB;
+        uint32_t saveDSPATILEOFF;
+        uint32_t saveDSPBTILEOFF;
+        uint32_t saveDSPAADDR;
+        uint32_t saveDSPBADDR;
+        uint32_t savePFIT_AUTO_RATIOS;
+        uint32_t savePFIT_PGM_RATIOS;
+        uint32_t savePP_ON_DELAYS;
+        uint32_t savePP_OFF_DELAYS;
+        uint32_t savePP_DIVISOR;
+        uint32_t saveBSM;
+        uint32_t saveVBT;
+        uint32_t saveBCLRPAT_A;
+        uint32_t saveBCLRPAT_B;
+        uint32_t saveDSPALINOFF;
+        uint32_t saveDSPBLINOFF;
+        uint32_t savePERF_MODE;
+        uint32_t saveDSPFW1;
+        uint32_t saveDSPFW2;
+        uint32_t saveDSPFW3;
+        uint32_t saveDSPFW4;
+        uint32_t saveDSPFW5;
+        uint32_t saveDSPFW6;
+        uint32_t saveCHICKENBIT;
+        uint32_t saveDSPACURSOR_CTRL;
+        uint32_t saveDSPBCURSOR_CTRL;
+        uint32_t saveDSPACURSOR_BASE;
+        uint32_t saveDSPBCURSOR_BASE;
+        uint32_t saveDSPACURSOR_POS;
+        uint32_t saveDSPBCURSOR_POS;
+        uint32_t save_palette_a[256];
+        uint32_t save_palette_b[256];
+        uint32_t saveOV_OVADD;
+        uint32_t saveOV_OGAMC0;
+        uint32_t saveOV_OGAMC1;
+        uint32_t saveOV_OGAMC2;
+        uint32_t saveOV_OGAMC3;
+        uint32_t saveOV_OGAMC4;
+        uint32_t saveOV_OGAMC5;
+        uint32_t saveOVC_OVADD;
+        uint32_t saveOVC_OGAMC0;
+        uint32_t saveOVC_OGAMC1;
+        uint32_t saveOVC_OGAMC2;
+        uint32_t saveOVC_OGAMC3;
+        uint32_t saveOVC_OGAMC4;
+        uint32_t saveOVC_OGAMC5;
+
+        /* MSI reg save */
+        uint32_t msi_addr;
+        uint32_t msi_data;
+
+        /* Medfield specific register save state */
+        uint32_t saveHDMIPHYMISCCTL;
+        uint32_t saveHDMIB_CONTROL;
+        uint32_t saveDSPCCNTR;
+        uint32_t savePIPECCONF;
+        uint32_t savePIPECSRC;
+        uint32_t saveHTOTAL_C;
+        uint32_t saveHBLANK_C;
+        uint32_t saveHSYNC_C;
+        uint32_t saveVTOTAL_C;
+        uint32_t saveVBLANK_C;
+        uint32_t saveVSYNC_C;
+        uint32_t saveDSPCSTRIDE;
+        uint32_t saveDSPCSIZE;
+        uint32_t saveDSPCPOS;
+        uint32_t saveDSPCSURF;
+        uint32_t saveDSPCSTATUS;
+        uint32_t saveDSPCLINOFF;
+        uint32_t saveDSPCTILEOFF;
+        uint32_t saveDSPCCURSOR_CTRL;
+        uint32_t saveDSPCCURSOR_BASE;
+        uint32_t saveDSPCCURSOR_POS;
+        uint32_t save_palette_c[256];
+        uint32_t saveOV_OVADD_C;
+        uint32_t saveOV_OGAMC0_C;
+        uint32_t saveOV_OGAMC1_C;
+        uint32_t saveOV_OGAMC2_C;
+        uint32_t saveOV_OGAMC3_C;
+        uint32_t saveOV_OGAMC4_C;
+        uint32_t saveOV_OGAMC5_C;
+
+        /* DSI register save */
+        uint32_t saveDEVICE_READY_REG;
+        uint32_t saveINTR_EN_REG;
+        uint32_t saveDSI_FUNC_PRG_REG;
+        uint32_t saveHS_TX_TIMEOUT_REG;
+        uint32_t saveLP_RX_TIMEOUT_REG;
+        uint32_t saveTURN_AROUND_TIMEOUT_REG;
+        uint32_t saveDEVICE_RESET_REG;
+        uint32_t saveDPI_RESOLUTION_REG;
+        uint32_t saveHORIZ_SYNC_PAD_COUNT_REG;
+        uint32_t saveHORIZ_BACK_PORCH_COUNT_REG;
+        uint32_t saveHORIZ_FRONT_PORCH_COUNT_REG;
+        uint32_t saveHORIZ_ACTIVE_AREA_COUNT_REG;
+        uint32_t saveVERT_SYNC_PAD_COUNT_REG;
+        uint32_t saveVERT_BACK_PORCH_COUNT_REG;
+        uint32_t saveVERT_FRONT_PORCH_COUNT_REG;
+        uint32_t saveHIGH_LOW_SWITCH_COUNT_REG;
+        uint32_t saveINIT_COUNT_REG;
+        uint32_t saveMAX_RET_PAK_REG;
+        uint32_t saveVIDEO_FMT_REG;
+        uint32_t saveEOT_DISABLE_REG;
+        uint32_t saveLP_BYTECLK_REG;
+        uint32_t saveHS_LS_DBI_ENABLE_REG;
+        uint32_t saveTXCLKESC_REG;
+        uint32_t saveDPHY_PARAM_REG;
+        uint32_t saveMIPI_CONTROL_REG;
+        uint32_t saveMIPI;
+        uint32_t saveMIPI_C;
+
+        /* DPST register save */
+        uint32_t saveHISTOGRAM_INT_CONTROL_REG;
+        uint32_t saveHISTOGRAM_LOGIC_CONTROL_REG;
+        uint32_t savePWM_CONTROL_LOGIC;
+
+        /*
+         * DSI info. 
+         */
+        void * dbi_dsr_info;    
+        void * dbi_dpu_info;
+        void * dsi_configs[2];
+        /*
+         * LID-Switch
+         */
+        spinlock_t lid_lock;
+        struct timer_list lid_timer;
+        struct psb_intel_opregion opregion;
+        u32 *lid_state;
+        u32 lid_last_state;
+
+        /*
+         * Watchdog
+         */
+
+        uint32_t apm_reg;
+        uint16_t apm_base;
+
+        /*
+         * Used for modifying backlight from
+         * xrandr -- consider removing and using HAL instead
+         */
+        struct backlight_device *backlight_device;
+        struct drm_property *backlight_property;
+        uint32_t blc_adj1;
+        uint32_t blc_adj2;
+
+        void *fbdev;
+        /* DPST state */
+        uint32_t dsr_idle_count;
+        bool is_in_idle;
+        bool dsr_enable;
+        void (*exit_idle)(struct drm_device *dev, u32 update_src);
+
+        /* 2D acceleration */
+        struct mutex mutex_2d;
+
+        /* FIXME: Arrays anyone ? */
+        struct mdfld_dsi_encoder *encoder0;     
+        struct mdfld_dsi_encoder *encoder2;     
+        struct mdfld_dsi_dbi_output * dbi_output;
+        struct mdfld_dsi_dbi_output * dbi_output2;
+        u32 bpp;
+        u32 bpp2;
+        
+        bool dispstatus;
+};
+
+
+/*
+ *      Operations for each board type
+ */
+ 
+struct psb_ops {
+        const char *name;
+        unsigned int accel_2d:1;
+        int pipes;              /* Number of output pipes */
+        int crtcs;              /* Number of CRTCs */
+        int sgx_offset;         /* Base offset of SGX device */
+
+        /* Sub functions */
+        struct drm_crtc_helper_funcs const *crtc_helper;
+        struct drm_crtc_funcs const *crtc_funcs;
+
+        /* Setup hooks */
+        int (*chip_setup)(struct drm_device *dev);
+        void (*chip_teardown)(struct drm_device *dev);
+
+        /* Display management hooks */
+        int (*output_init)(struct drm_device *dev);
+        /* Power management hooks */
+        void (*init_pm)(struct drm_device *dev);
+        int (*save_regs)(struct drm_device *dev);
+        int (*restore_regs)(struct drm_device *dev);
+        int (*power_up)(struct drm_device *dev);
+        int (*power_down)(struct drm_device *dev);
+
+        void (*lvds_bl_power)(struct drm_device *dev, bool on);
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+        /* Backlight */
+        int (*backlight_init)(struct drm_device *dev);
+#endif
+        int i2c_bus;            /* I2C bus identifier for Moorestown */
+};
+
+
+
+struct psb_mmu_driver;
+
+extern int drm_crtc_probe_output_modes(struct drm_device *dev, int, int);
+extern int drm_pick_crtcs(struct drm_device *dev);
+
+static inline struct drm_psb_private *psb_priv(struct drm_device *dev)
+{
+        return (struct drm_psb_private *) dev->dev_private;
+}
+
+/*
+ * MMU stuff.
+ */
+
+extern struct psb_mmu_driver *psb_mmu_driver_init(uint8_t __iomem * registers,
+                                        int trap_pagefaults,
+                                        int invalid_type,
+                                        struct drm_psb_private *dev_priv);
+extern void psb_mmu_driver_takedown(struct psb_mmu_driver *driver);
+extern struct psb_mmu_pd *psb_mmu_get_default_pd(struct psb_mmu_driver
+                                                 *driver);
+extern void psb_mmu_mirror_gtt(struct psb_mmu_pd *pd, uint32_t mmu_offset,
+                               uint32_t gtt_start, uint32_t gtt_pages);
+extern struct psb_mmu_pd *psb_mmu_alloc_pd(struct psb_mmu_driver *driver,
+                                           int trap_pagefaults,
+                                           int invalid_type);
+extern void psb_mmu_free_pagedir(struct psb_mmu_pd *pd);
+extern void psb_mmu_flush(struct psb_mmu_driver *driver, int rc_prot);
+extern void psb_mmu_remove_pfn_sequence(struct psb_mmu_pd *pd,
+                                        unsigned long address,
+                                        uint32_t num_pages);
+extern int psb_mmu_insert_pfn_sequence(struct psb_mmu_pd *pd,
+                                       uint32_t start_pfn,
+                                       unsigned long address,
+                                       uint32_t num_pages, int type);
+extern int psb_mmu_virtual_to_pfn(struct psb_mmu_pd *pd, uint32_t virtual,
+                                  unsigned long *pfn);
+
+/*
+ * Enable / disable MMU for different requestors.
+ */
+
+
+extern void psb_mmu_set_pd_context(struct psb_mmu_pd *pd, int hw_context);
+extern int psb_mmu_insert_pages(struct psb_mmu_pd *pd, struct page **pages,
+                                unsigned long address, uint32_t num_pages,
+                                uint32_t desired_tile_stride,
+                                uint32_t hw_tile_stride, int type);
+extern void psb_mmu_remove_pages(struct psb_mmu_pd *pd,
+                                 unsigned long address, uint32_t num_pages,
+                                 uint32_t desired_tile_stride,
+                                 uint32_t hw_tile_stride);
+/*
+ *psb_irq.c
+ */
+
+extern irqreturn_t psb_irq_handler(DRM_IRQ_ARGS);
+extern int psb_irq_enable_dpst(struct drm_device *dev);
+extern int psb_irq_disable_dpst(struct drm_device *dev);
+extern void psb_irq_preinstall(struct drm_device *dev);
+extern int psb_irq_postinstall(struct drm_device *dev);
+extern void psb_irq_uninstall(struct drm_device *dev);
+extern void psb_irq_turn_on_dpst(struct drm_device *dev);
+extern void psb_irq_turn_off_dpst(struct drm_device *dev);
+
+extern void psb_irq_uninstall_islands(struct drm_device *dev, int hw_islands);
+extern int psb_vblank_wait2(struct drm_device *dev, unsigned int *sequence);
+extern int psb_vblank_wait(struct drm_device *dev, unsigned int *sequence);
+extern int psb_enable_vblank(struct drm_device *dev, int crtc);
+extern void psb_disable_vblank(struct drm_device *dev, int crtc);
+void
+psb_enable_pipestat(struct drm_psb_private *dev_priv, int pipe, u32 mask);
+
+void
+psb_disable_pipestat(struct drm_psb_private *dev_priv, int pipe, u32 mask);
+
+extern u32 psb_get_vblank_counter(struct drm_device *dev, int crtc);
+
+extern int mdfld_enable_te(struct drm_device *dev, int pipe);
+extern void mdfld_disable_te(struct drm_device *dev, int pipe);
+
+/*
+ * intel_opregion.c
+ */
+extern int gma_intel_opregion_init(struct drm_device *dev);
+extern int gma_intel_opregion_exit(struct drm_device *dev);
+
+/*
+ * framebuffer.c
+ */
+extern int psbfb_probed(struct drm_device *dev);
+extern int psbfb_remove(struct drm_device *dev,
+                        struct drm_framebuffer *fb);
+/*
+ * accel_2d.c
+ */
+extern void psbfb_copyarea(struct fb_info *info,
+                                        const struct fb_copyarea *region);
+extern int psbfb_sync(struct fb_info *info);
+extern void psb_spank(struct drm_psb_private *dev_priv);
+extern int psb_accel_ioctl(struct drm_device *dev, void *data,
+                                                        struct drm_file *file);
+
+/*
+ * psb_reset.c
+ */
+
+extern void psb_lid_timer_init(struct drm_psb_private *dev_priv);
+extern void psb_lid_timer_takedown(struct drm_psb_private *dev_priv);
+extern void psb_print_pagefault(struct drm_psb_private *dev_priv);
+
+/* modesetting */
+extern void psb_modeset_init(struct drm_device *dev);
+extern void psb_modeset_cleanup(struct drm_device *dev);
+extern int psb_fbdev_init(struct drm_device *dev);
+
+/* backlight.c */
+int gma_backlight_init(struct drm_device *dev);
+void gma_backlight_exit(struct drm_device *dev);
+
+/* mrst_crtc.c */
+extern const struct drm_crtc_helper_funcs mrst_helper_funcs;
+
+/* mrst_lvds.c */
+extern void mrst_lvds_init(struct drm_device *dev,
+                    struct psb_intel_mode_device *mode_dev);
+
+/* psb_intel_display.c */
+extern const struct drm_crtc_helper_funcs psb_intel_helper_funcs;
+extern const struct drm_crtc_funcs psb_intel_crtc_funcs;
+
+/* psb_intel_lvds.c */
+extern const struct drm_connector_helper_funcs
+                                        psb_intel_lvds_connector_helper_funcs;
+extern const struct drm_connector_funcs psb_intel_lvds_connector_funcs;
+
+/* gem.c */
+extern int psb_gem_init_object(struct drm_gem_object *obj);
+extern void psb_gem_free_object(struct drm_gem_object *obj);
+extern int psb_gem_get_aperture(struct drm_device *dev, void *data,
+                        struct drm_file *file);
+extern int psb_gem_dumb_create(struct drm_file *file, struct drm_device *dev,
+                        struct drm_mode_create_dumb *args);
+extern int psb_gem_dumb_destroy(struct drm_file *file, struct drm_device *dev,
+                        uint32_t handle);
+extern int psb_gem_dumb_map_gtt(struct drm_file *file, struct drm_device *dev,
+                        uint32_t handle, uint64_t *offset);
+extern int psb_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
+extern int psb_gem_create_ioctl(struct drm_device *dev, void *data,
+                        struct drm_file *file);
+extern int psb_gem_mmap_ioctl(struct drm_device *dev, void *data,
+                                        struct drm_file *file);
+
+/* psb_device.c */
+extern const struct psb_ops psb_chip_ops;
+
+/* mrst_device.c */
+extern const struct psb_ops mrst_chip_ops;
+
+/* mdfld_device.c */
+extern const struct psb_ops mdfld_chip_ops;
+
+/* cdv_device.c */
+extern const struct psb_ops cdv_chip_ops;
+
+/*
+ * Debug print bits setting
+ */
+#define PSB_D_GENERAL (1 << 0)
+#define PSB_D_INIT    (1 << 1)
+#define PSB_D_IRQ     (1 << 2)
+#define PSB_D_ENTRY   (1 << 3)
+/* debug the get H/V BP/FP count */
+#define PSB_D_HV      (1 << 4)
+#define PSB_D_DBI_BF  (1 << 5)
+#define PSB_D_PM      (1 << 6)
+#define PSB_D_RENDER  (1 << 7)
+#define PSB_D_REG     (1 << 8)
+#define PSB_D_MSVDX   (1 << 9)
+#define PSB_D_TOPAZ   (1 << 10)
+
+extern int drm_psb_no_fb;
+extern int drm_idle_check_interval;
+
+/*
+ *      Utilities
+ */
+
+static inline u32 MRST_MSG_READ32(uint port, uint offset)
+{
+        int mcr = (0xD0<<24) | (port << 16) | (offset << 8);
+        uint32_t ret_val = 0;
+        struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+        pci_write_config_dword(pci_root, 0xD0, mcr);
+        pci_read_config_dword(pci_root, 0xD4, &ret_val);
+        pci_dev_put(pci_root);
+        return ret_val;
+}
+static inline void MRST_MSG_WRITE32(uint port, uint offset, u32 value)
+{
+        int mcr = (0xE0<<24) | (port << 16) | (offset << 8) | 0xF0;
+        struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+        pci_write_config_dword(pci_root, 0xD4, value);
+        pci_write_config_dword(pci_root, 0xD0, mcr);
+        pci_dev_put(pci_root);
+}
+static inline u32 MDFLD_MSG_READ32(uint port, uint offset)
+{
+        int mcr = (0x10<<24) | (port << 16) | (offset << 8);
+        uint32_t ret_val = 0;
+        struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+        pci_write_config_dword(pci_root, 0xD0, mcr);
+        pci_read_config_dword(pci_root, 0xD4, &ret_val);
+        pci_dev_put(pci_root);
+        return ret_val;
+}
+static inline void MDFLD_MSG_WRITE32(uint port, uint offset, u32 value)
+{
+        int mcr = (0x11<<24) | (port << 16) | (offset << 8) | 0xF0;
+        struct pci_dev *pci_root = pci_get_bus_and_slot(0, 0);
+        pci_write_config_dword(pci_root, 0xD4, value);
+        pci_write_config_dword(pci_root, 0xD0, mcr);
+        pci_dev_put(pci_root);
+}
+
+static inline uint32_t REGISTER_READ(struct drm_device *dev, uint32_t reg)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        return ioread32(dev_priv->vdc_reg + reg);
+}
+
+#define REG_READ(reg)          REGISTER_READ(dev, (reg))
+
+static inline void REGISTER_WRITE(struct drm_device *dev, uint32_t reg,
+                                      uint32_t val)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        iowrite32((val), dev_priv->vdc_reg + (reg));
+}
+
+#define REG_WRITE(reg, val)     REGISTER_WRITE(dev, (reg), (val))
+
+static inline void REGISTER_WRITE16(struct drm_device *dev,
+                                        uint32_t reg, uint32_t val)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        iowrite16((val), dev_priv->vdc_reg + (reg));
+}
+
+#define REG_WRITE16(reg, val)     REGISTER_WRITE16(dev, (reg), (val))
+
+static inline void REGISTER_WRITE8(struct drm_device *dev,
+                                       uint32_t reg, uint32_t val)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        iowrite8((val), dev_priv->vdc_reg + (reg));
+}
+
+#define REG_WRITE8(reg, val)            REGISTER_WRITE8(dev, (reg), (val))
+
+#define PSB_WVDC32(_val, _offs)         iowrite32(_val, dev_priv->vdc_reg + (_offs))
+#define PSB_RVDC32(_offs)               ioread32(dev_priv->vdc_reg + (_offs))
+
+/* #define TRAP_SGX_PM_FAULT 1 */
+#ifdef TRAP_SGX_PM_FAULT
+#define PSB_RSGX32(_offs)                                               \
+({                                                                      \
+        if (inl(dev_priv->apm_base + PSB_APM_STS) & 0x3) {              \
+                printk(KERN_ERR                                         \
+                        "access sgx when it's off!! (READ) %s, %d\n",   \
+               __FILE__, __LINE__);                                     \
+                melay(1000);                                            \
+        }                                                               \
+        ioread32(dev_priv->sgx_reg + (_offs));                          \
+})
+#else
+#define PSB_RSGX32(_offs)               ioread32(dev_priv->sgx_reg + (_offs))
+#endif
+#define PSB_WSGX32(_val, _offs)         iowrite32(_val, dev_priv->sgx_reg + (_offs))
+
+#define MSVDX_REG_DUMP 0
+
+#define PSB_WMSVDX32(_val, _offs)       iowrite32(_val, dev_priv->msvdx_reg + (_offs))
+#define PSB_RMSVDX32(_offs)             ioread32(dev_priv->msvdx_reg + (_offs))
+
+#endif
diff --git a/drivers/staging/gma500/psb_intel_display.c b/drivers/staging/gma500/psb_intel_display.c
new file mode 100644
index 00000000000..4afa671f974
--- /dev/null
+++ b/drivers/staging/gma500/psb_intel_display.c
@@ -0,0 +1,1433 @@
+/*
+ * Copyright © 2006-2011 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors:
+ *      Eric Anholt <eric@anholt.net>
+ */
+
+#include <linux/i2c.h>
+#include <linux/pm_runtime.h>
+
+#include <drm/drmP.h>
+#include "framebuffer.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_intel_display.h"
+#include "power.h"
+
+#include "mdfld_output.h"
+
+struct psb_intel_clock_t {
+        /* given values */
+        int n;
+        int m1, m2;
+        int p1, p2;
+        /* derived values */
+        int dot;
+        int vco;
+        int m;
+        int p;
+};
+
+struct psb_intel_range_t {
+        int min, max;
+};
+
+struct psb_intel_p2_t {
+        int dot_limit;
+        int p2_slow, p2_fast;
+};
+
+#define INTEL_P2_NUM                  2
+
+struct psb_intel_limit_t {
+        struct psb_intel_range_t dot, vco, n, m, m1, m2, p, p1;
+        struct psb_intel_p2_t p2;
+};
+
+#define I8XX_DOT_MIN              25000
+#define I8XX_DOT_MAX             350000
+#define I8XX_VCO_MIN             930000
+#define I8XX_VCO_MAX            1400000
+#define I8XX_N_MIN                    3
+#define I8XX_N_MAX                   16
+#define I8XX_M_MIN                   96
+#define I8XX_M_MAX                  140
+#define I8XX_M1_MIN                  18
+#define I8XX_M1_MAX                  26
+#define I8XX_M2_MIN                   6
+#define I8XX_M2_MAX                  16
+#define I8XX_P_MIN                    4
+#define I8XX_P_MAX                  128
+#define I8XX_P1_MIN                   2
+#define I8XX_P1_MAX                  33
+#define I8XX_P1_LVDS_MIN              1
+#define I8XX_P1_LVDS_MAX              6
+#define I8XX_P2_SLOW                  4
+#define I8XX_P2_FAST                  2
+#define I8XX_P2_LVDS_SLOW             14
+#define I8XX_P2_LVDS_FAST             14        /* No fast option */
+#define I8XX_P2_SLOW_LIMIT       165000
+
+#define I9XX_DOT_MIN              20000
+#define I9XX_DOT_MAX             400000
+#define I9XX_VCO_MIN            1400000
+#define I9XX_VCO_MAX            2800000
+#define I9XX_N_MIN                    3
+#define I9XX_N_MAX                    8
+#define I9XX_M_MIN                   70
+#define I9XX_M_MAX                  120
+#define I9XX_M1_MIN                  10
+#define I9XX_M1_MAX                  20
+#define I9XX_M2_MIN                   5
+#define I9XX_M2_MAX                   9
+#define I9XX_P_SDVO_DAC_MIN           5
+#define I9XX_P_SDVO_DAC_MAX          80
+#define I9XX_P_LVDS_MIN               7
+#define I9XX_P_LVDS_MAX              98
+#define I9XX_P1_MIN                   1
+#define I9XX_P1_MAX                   8
+#define I9XX_P2_SDVO_DAC_SLOW                10
+#define I9XX_P2_SDVO_DAC_FAST                 5
+#define I9XX_P2_SDVO_DAC_SLOW_LIMIT      200000
+#define I9XX_P2_LVDS_SLOW                    14
+#define I9XX_P2_LVDS_FAST                     7
+#define I9XX_P2_LVDS_SLOW_LIMIT          112000
+
+#define INTEL_LIMIT_I8XX_DVO_DAC    0
+#define INTEL_LIMIT_I8XX_LVDS       1
+#define INTEL_LIMIT_I9XX_SDVO_DAC   2
+#define INTEL_LIMIT_I9XX_LVDS       3
+
+static const struct psb_intel_limit_t psb_intel_limits[] = {
+        {                       /* INTEL_LIMIT_I8XX_DVO_DAC */
+         .dot = {.min = I8XX_DOT_MIN, .max = I8XX_DOT_MAX},
+         .vco = {.min = I8XX_VCO_MIN, .max = I8XX_VCO_MAX},
+         .n = {.min = I8XX_N_MIN, .max = I8XX_N_MAX},
+         .m = {.min = I8XX_M_MIN, .max = I8XX_M_MAX},
+         .m1 = {.min = I8XX_M1_MIN, .max = I8XX_M1_MAX},
+         .m2 = {.min = I8XX_M2_MIN, .max = I8XX_M2_MAX},
+         .p = {.min = I8XX_P_MIN, .max = I8XX_P_MAX},
+         .p1 = {.min = I8XX_P1_MIN, .max = I8XX_P1_MAX},
+         .p2 = {.dot_limit = I8XX_P2_SLOW_LIMIT,
+                .p2_slow = I8XX_P2_SLOW, .p2_fast = I8XX_P2_FAST},
+         },
+        {                       /* INTEL_LIMIT_I8XX_LVDS */
+         .dot = {.min = I8XX_DOT_MIN, .max = I8XX_DOT_MAX},
+         .vco = {.min = I8XX_VCO_MIN, .max = I8XX_VCO_MAX},
+         .n = {.min = I8XX_N_MIN, .max = I8XX_N_MAX},
+         .m = {.min = I8XX_M_MIN, .max = I8XX_M_MAX},
+         .m1 = {.min = I8XX_M1_MIN, .max = I8XX_M1_MAX},
+         .m2 = {.min = I8XX_M2_MIN, .max = I8XX_M2_MAX},
+         .p = {.min = I8XX_P_MIN, .max = I8XX_P_MAX},
+         .p1 = {.min = I8XX_P1_LVDS_MIN, .max = I8XX_P1_LVDS_MAX},
+         .p2 = {.dot_limit = I8XX_P2_SLOW_LIMIT,
+                .p2_slow = I8XX_P2_LVDS_SLOW, .p2_fast = I8XX_P2_LVDS_FAST},
+         },
+        {                       /* INTEL_LIMIT_I9XX_SDVO_DAC */
+         .dot = {.min = I9XX_DOT_MIN, .max = I9XX_DOT_MAX},
+         .vco = {.min = I9XX_VCO_MIN, .max = I9XX_VCO_MAX},
+         .n = {.min = I9XX_N_MIN, .max = I9XX_N_MAX},
+         .m = {.min = I9XX_M_MIN, .max = I9XX_M_MAX},
+         .m1 = {.min = I9XX_M1_MIN, .max = I9XX_M1_MAX},
+         .m2 = {.min = I9XX_M2_MIN, .max = I9XX_M2_MAX},
+         .p = {.min = I9XX_P_SDVO_DAC_MIN, .max = I9XX_P_SDVO_DAC_MAX},
+         .p1 = {.min = I9XX_P1_MIN, .max = I9XX_P1_MAX},
+         .p2 = {.dot_limit = I9XX_P2_SDVO_DAC_SLOW_LIMIT,
+                .p2_slow = I9XX_P2_SDVO_DAC_SLOW, .p2_fast =
+                I9XX_P2_SDVO_DAC_FAST},
+         },
+        {                       /* INTEL_LIMIT_I9XX_LVDS */
+         .dot = {.min = I9XX_DOT_MIN, .max = I9XX_DOT_MAX},
+         .vco = {.min = I9XX_VCO_MIN, .max = I9XX_VCO_MAX},
+         .n = {.min = I9XX_N_MIN, .max = I9XX_N_MAX},
+         .m = {.min = I9XX_M_MIN, .max = I9XX_M_MAX},
+         .m1 = {.min = I9XX_M1_MIN, .max = I9XX_M1_MAX},
+         .m2 = {.min = I9XX_M2_MIN, .max = I9XX_M2_MAX},
+         .p = {.min = I9XX_P_LVDS_MIN, .max = I9XX_P_LVDS_MAX},
+         .p1 = {.min = I9XX_P1_MIN, .max = I9XX_P1_MAX},
+         /* The single-channel range is 25-112Mhz, and dual-channel
+          * is 80-224Mhz.  Prefer single channel as much as possible.
+          */
+         .p2 = {.dot_limit = I9XX_P2_LVDS_SLOW_LIMIT,
+                .p2_slow = I9XX_P2_LVDS_SLOW, .p2_fast = I9XX_P2_LVDS_FAST},
+         },
+};
+
+static const struct psb_intel_limit_t *psb_intel_limit(struct drm_crtc *crtc)
+{
+        const struct psb_intel_limit_t *limit;
+
+        if (psb_intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS))
+                limit = &psb_intel_limits[INTEL_LIMIT_I9XX_LVDS];
+        else
+                limit = &psb_intel_limits[INTEL_LIMIT_I9XX_SDVO_DAC];
+        return limit;
+}
+
+/** Derive the pixel clock for the given refclk and divisors for 8xx chips. */
+
+static void i8xx_clock(int refclk, struct psb_intel_clock_t *clock)
+{
+        clock->m = 5 * (clock->m1 + 2) + (clock->m2 + 2);
+        clock->p = clock->p1 * clock->p2;
+        clock->vco = refclk * clock->m / (clock->n + 2);
+        clock->dot = clock->vco / clock->p;
+}
+
+/** Derive the pixel clock for the given refclk and divisors for 9xx chips. */
+
+static void i9xx_clock(int refclk, struct psb_intel_clock_t *clock)
+{
+        clock->m = 5 * (clock->m1 + 2) + (clock->m2 + 2);
+        clock->p = clock->p1 * clock->p2;
+        clock->vco = refclk * clock->m / (clock->n + 2);
+        clock->dot = clock->vco / clock->p;
+}
+
+static void psb_intel_clock(struct drm_device *dev, int refclk,
+                        struct psb_intel_clock_t *clock)
+{
+        return i9xx_clock(refclk, clock);
+}
+
+/**
+ * Returns whether any output on the specified pipe is of the specified type
+ */
+bool psb_intel_pipe_has_type(struct drm_crtc *crtc, int type)
+{
+        struct drm_device *dev = crtc->dev;
+        struct drm_mode_config *mode_config = &dev->mode_config;
+        struct drm_connector *l_entry;
+
+        list_for_each_entry(l_entry, &mode_config->connector_list, head) {
+                if (l_entry->encoder && l_entry->encoder->crtc == crtc) {
+                        struct psb_intel_output *psb_intel_output =
+                            to_psb_intel_output(l_entry);
+                        if (psb_intel_output->type == type)
+                                return true;
+                }
+        }
+        return false;
+}
+
+#define INTELPllInvalid(s)   { /* ErrorF (s) */; return false; }
+/**
+ * Returns whether the given set of divisors are valid for a given refclk with
+ * the given connectors.
+ */
+
+static bool psb_intel_PLL_is_valid(struct drm_crtc *crtc,
+                               struct psb_intel_clock_t *clock)
+{
+        const struct psb_intel_limit_t *limit = psb_intel_limit(crtc);
+
+        if (clock->p1 < limit->p1.min || limit->p1.max < clock->p1)
+                INTELPllInvalid("p1 out of range\n");
+        if (clock->p < limit->p.min || limit->p.max < clock->p)
+                INTELPllInvalid("p out of range\n");
+        if (clock->m2 < limit->m2.min || limit->m2.max < clock->m2)
+                INTELPllInvalid("m2 out of range\n");
+        if (clock->m1 < limit->m1.min || limit->m1.max < clock->m1)
+                INTELPllInvalid("m1 out of range\n");
+        if (clock->m1 <= clock->m2)
+                INTELPllInvalid("m1 <= m2\n");
+        if (clock->m < limit->m.min || limit->m.max < clock->m)
+                INTELPllInvalid("m out of range\n");
+        if (clock->n < limit->n.min || limit->n.max < clock->n)
+                INTELPllInvalid("n out of range\n");
+        if (clock->vco < limit->vco.min || limit->vco.max < clock->vco)
+                INTELPllInvalid("vco out of range\n");
+        /* XXX: We may need to be checking "Dot clock"
+         * depending on the multiplier, connector, etc.,
+         * rather than just a single range.
+         */
+        if (clock->dot < limit->dot.min || limit->dot.max < clock->dot)
+                INTELPllInvalid("dot out of range\n");
+
+        return true;
+}
+
+/**
+ * Returns a set of divisors for the desired target clock with the given
+ * refclk, or FALSE.  The returned values represent the clock equation:
+ * reflck * (5 * (m1 + 2) + (m2 + 2)) / (n + 2) / p1 / p2.
+ */
+static bool psb_intel_find_best_PLL(struct drm_crtc *crtc, int target,
+                                int refclk,
+                                struct psb_intel_clock_t *best_clock)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_clock_t clock;
+        const struct psb_intel_limit_t *limit = psb_intel_limit(crtc);
+        int err = target;
+
+        if (psb_intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS) &&
+            (REG_READ(LVDS) & LVDS_PORT_EN) != 0) {
+                /*
+                 * For LVDS, if the panel is on, just rely on its current
+                 * settings for dual-channel.  We haven't figured out how to
+                 * reliably set up different single/dual channel state, if we
+                 * even can.
+                 */
+                if ((REG_READ(LVDS) & LVDS_CLKB_POWER_MASK) ==
+                    LVDS_CLKB_POWER_UP)
+                        clock.p2 = limit->p2.p2_fast;
+                else
+                        clock.p2 = limit->p2.p2_slow;
+        } else {
+                if (target < limit->p2.dot_limit)
+                        clock.p2 = limit->p2.p2_slow;
+                else
+                        clock.p2 = limit->p2.p2_fast;
+        }
+
+        memset(best_clock, 0, sizeof(*best_clock));
+
+        for (clock.m1 = limit->m1.min; clock.m1 <= limit->m1.max;
+             clock.m1++) {
+                for (clock.m2 = limit->m2.min;
+                     clock.m2 < clock.m1 && clock.m2 <= limit->m2.max;
+                     clock.m2++) {
+                        for (clock.n = limit->n.min;
+                             clock.n <= limit->n.max; clock.n++) {
+                                for (clock.p1 = limit->p1.min;
+                                     clock.p1 <= limit->p1.max;
+                                     clock.p1++) {
+                                        int this_err;
+
+                                        psb_intel_clock(dev, refclk, &clock);
+
+                                        if (!psb_intel_PLL_is_valid
+                                            (crtc, &clock))
+                                                continue;
+
+                                        this_err = abs(clock.dot - target);
+                                        if (this_err < err) {
+                                                *best_clock = clock;
+                                                err = this_err;
+                                        }
+                                }
+                        }
+                }
+        }
+
+        return err != target;
+}
+
+void psb_intel_wait_for_vblank(struct drm_device *dev)
+{
+        /* Wait for 20ms, i.e. one cycle at 50hz. */
+        mdelay(20);
+}
+
+int psb_intel_pipe_set_base(struct drm_crtc *crtc,
+                            int x, int y, struct drm_framebuffer *old_fb)
+{
+        struct drm_device *dev = crtc->dev;
+        /* struct drm_i915_master_private *master_priv; */
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        struct psb_framebuffer *psbfb = to_psb_fb(crtc->fb);
+        int pipe = psb_intel_crtc->pipe;
+        unsigned long start, offset;
+        int dspbase = (pipe == 0 ? DSPABASE : DSPBBASE);
+        int dspsurf = (pipe == 0 ? DSPASURF : DSPBSURF);
+        int dspstride = (pipe == 0) ? DSPASTRIDE : DSPBSTRIDE;
+        int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+        u32 dspcntr;
+        int ret = 0;
+
+        if (!gma_power_begin(dev, true))
+                return 0;
+
+        /* no fb bound */
+        if (!crtc->fb) {
+                dev_dbg(dev->dev, "No FB bound\n");
+                goto psb_intel_pipe_cleaner;
+        }
+
+        /* We are displaying this buffer, make sure it is actually loaded
+           into the GTT */
+        ret = psb_gtt_pin(psbfb->gtt);
+        if (ret < 0)
+                goto psb_intel_pipe_set_base_exit;
+        start = psbfb->gtt->offset;
+
+        offset = y * crtc->fb->pitch + x * (crtc->fb->bits_per_pixel / 8);
+
+        REG_WRITE(dspstride, crtc->fb->pitch);
+
+        dspcntr = REG_READ(dspcntr_reg);
+        dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
+
+        switch (crtc->fb->bits_per_pixel) {
+        case 8:
+                dspcntr |= DISPPLANE_8BPP;
+                break;
+        case 16:
+                if (crtc->fb->depth == 15)
+                        dspcntr |= DISPPLANE_15_16BPP;
+                else
+                        dspcntr |= DISPPLANE_16BPP;
+                break;
+        case 24:
+        case 32:
+                dspcntr |= DISPPLANE_32BPP_NO_ALPHA;
+                break;
+        default:
+                dev_err(dev->dev, "Unknown color depth\n");
+                ret = -EINVAL;
+                psb_gtt_unpin(psbfb->gtt);
+                goto psb_intel_pipe_set_base_exit;
+        }
+        REG_WRITE(dspcntr_reg, dspcntr);
+
+
+        if (0 /* FIXMEAC - check what PSB needs */) {
+                REG_WRITE(dspbase, offset);
+                REG_READ(dspbase);
+                REG_WRITE(dspsurf, start);
+                REG_READ(dspsurf);
+        } else {
+                REG_WRITE(dspbase, start + offset);
+                REG_READ(dspbase);
+        }
+
+psb_intel_pipe_cleaner:
+        /* If there was a previous display we can now unpin it */
+        if (old_fb)
+                psb_gtt_unpin(to_psb_fb(old_fb)->gtt);
+
+psb_intel_pipe_set_base_exit:
+        gma_power_end(dev);
+        return ret;
+}
+
+/**
+ * Sets the power management mode of the pipe and plane.
+ *
+ * This code should probably grow support for turning the cursor off and back
+ * on appropriately at the same time as we're turning the pipe off/on.
+ */
+static void psb_intel_crtc_dpms(struct drm_crtc *crtc, int mode)
+{
+        struct drm_device *dev = crtc->dev;
+        /* struct drm_i915_master_private *master_priv; */
+        /* struct drm_i915_private *dev_priv = dev->dev_private; */
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
+        int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+        int dspbase_reg = (pipe == 0) ? DSPABASE : DSPBBASE;
+        int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+        u32 temp;
+        bool enabled;
+
+        /* XXX: When our outputs are all unaware of DPMS modes other than off
+         * and on, we should map those modes to DRM_MODE_DPMS_OFF in the CRTC.
+         */
+        switch (mode) {
+        case DRM_MODE_DPMS_ON:
+        case DRM_MODE_DPMS_STANDBY:
+        case DRM_MODE_DPMS_SUSPEND:
+                /* Enable the DPLL */
+                temp = REG_READ(dpll_reg);
+                if ((temp & DPLL_VCO_ENABLE) == 0) {
+                        REG_WRITE(dpll_reg, temp);
+                        REG_READ(dpll_reg);
+                        /* Wait for the clocks to stabilize. */
+                        udelay(150);
+                        REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+                        REG_READ(dpll_reg);
+                        /* Wait for the clocks to stabilize. */
+                        udelay(150);
+                        REG_WRITE(dpll_reg, temp | DPLL_VCO_ENABLE);
+                        REG_READ(dpll_reg);
+                        /* Wait for the clocks to stabilize. */
+                        udelay(150);
+                }
+
+                /* Enable the pipe */
+                temp = REG_READ(pipeconf_reg);
+                if ((temp & PIPEACONF_ENABLE) == 0)
+                        REG_WRITE(pipeconf_reg, temp | PIPEACONF_ENABLE);
+
+                /* Enable the plane */
+                temp = REG_READ(dspcntr_reg);
+                if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
+                        REG_WRITE(dspcntr_reg,
+                                  temp | DISPLAY_PLANE_ENABLE);
+                        /* Flush the plane changes */
+                        REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+                }
+
+                psb_intel_crtc_load_lut(crtc);
+
+                /* Give the overlay scaler a chance to enable
+                 * if it's on this pipe */
+                /* psb_intel_crtc_dpms_video(crtc, true); TODO */
+                break;
+        case DRM_MODE_DPMS_OFF:
+                /* Give the overlay scaler a chance to disable
+                 * if it's on this pipe */
+                /* psb_intel_crtc_dpms_video(crtc, FALSE); TODO */
+
+                /* Disable the VGA plane that we never use */
+                REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+
+                /* Disable display plane */
+                temp = REG_READ(dspcntr_reg);
+                if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
+                        REG_WRITE(dspcntr_reg,
+                                  temp & ~DISPLAY_PLANE_ENABLE);
+                        /* Flush the plane changes */
+                        REG_WRITE(dspbase_reg, REG_READ(dspbase_reg));
+                        REG_READ(dspbase_reg);
+                }
+
+                /* Next, disable display pipes */
+                temp = REG_READ(pipeconf_reg);
+                if ((temp & PIPEACONF_ENABLE) != 0) {
+                        REG_WRITE(pipeconf_reg, temp & ~PIPEACONF_ENABLE);
+                        REG_READ(pipeconf_reg);
+                }
+
+                /* Wait for vblank for the disable to take effect. */
+                psb_intel_wait_for_vblank(dev);
+
+                temp = REG_READ(dpll_reg);
+                if ((temp & DPLL_VCO_ENABLE) != 0) {
+                        REG_WRITE(dpll_reg, temp & ~DPLL_VCO_ENABLE);
+                        REG_READ(dpll_reg);
+                }
+
+                /* Wait for the clocks to turn off. */
+                udelay(150);
+                break;
+        }
+
+        enabled = crtc->enabled && mode != DRM_MODE_DPMS_OFF;
+
+        /*Set FIFO Watermarks*/
+        REG_WRITE(DSPARB, 0x3F3E);
+}
+
+static void psb_intel_crtc_prepare(struct drm_crtc *crtc)
+{
+        struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+        crtc_funcs->dpms(crtc, DRM_MODE_DPMS_OFF);
+}
+
+static void psb_intel_crtc_commit(struct drm_crtc *crtc)
+{
+        struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+        crtc_funcs->dpms(crtc, DRM_MODE_DPMS_ON);
+}
+
+void psb_intel_encoder_prepare(struct drm_encoder *encoder)
+{
+        struct drm_encoder_helper_funcs *encoder_funcs =
+            encoder->helper_private;
+        /* lvds has its own version of prepare see psb_intel_lvds_prepare */
+        encoder_funcs->dpms(encoder, DRM_MODE_DPMS_OFF);
+}
+
+void psb_intel_encoder_commit(struct drm_encoder *encoder)
+{
+        struct drm_encoder_helper_funcs *encoder_funcs =
+            encoder->helper_private;
+        /* lvds has its own version of commit see psb_intel_lvds_commit */
+        encoder_funcs->dpms(encoder, DRM_MODE_DPMS_ON);
+}
+
+static bool psb_intel_crtc_mode_fixup(struct drm_crtc *crtc,
+                                  struct drm_display_mode *mode,
+                                  struct drm_display_mode *adjusted_mode)
+{
+        return true;
+}
+
+
+/**
+ * Return the pipe currently connected to the panel fitter,
+ * or -1 if the panel fitter is not present or not in use
+ */
+static int psb_intel_panel_fitter_pipe(struct drm_device *dev)
+{
+        u32 pfit_control;
+
+        pfit_control = REG_READ(PFIT_CONTROL);
+
+        /* See if the panel fitter is in use */
+        if ((pfit_control & PFIT_ENABLE) == 0)
+                return -1;
+        /* Must be on PIPE 1 for PSB */
+        return 1;
+}
+
+static int psb_intel_crtc_mode_set(struct drm_crtc *crtc,
+                               struct drm_display_mode *mode,
+                               struct drm_display_mode *adjusted_mode,
+                               int x, int y,
+                               struct drm_framebuffer *old_fb)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
+        int pipe = psb_intel_crtc->pipe;
+        int fp_reg = (pipe == 0) ? FPA0 : FPB0;
+        int dpll_reg = (pipe == 0) ? DPLL_A : DPLL_B;
+        int dspcntr_reg = (pipe == 0) ? DSPACNTR : DSPBCNTR;
+        int pipeconf_reg = (pipe == 0) ? PIPEACONF : PIPEBCONF;
+        int htot_reg = (pipe == 0) ? HTOTAL_A : HTOTAL_B;
+        int hblank_reg = (pipe == 0) ? HBLANK_A : HBLANK_B;
+        int hsync_reg = (pipe == 0) ? HSYNC_A : HSYNC_B;
+        int vtot_reg = (pipe == 0) ? VTOTAL_A : VTOTAL_B;
+        int vblank_reg = (pipe == 0) ? VBLANK_A : VBLANK_B;
+        int vsync_reg = (pipe == 0) ? VSYNC_A : VSYNC_B;
+        int dspsize_reg = (pipe == 0) ? DSPASIZE : DSPBSIZE;
+        int dsppos_reg = (pipe == 0) ? DSPAPOS : DSPBPOS;
+        int pipesrc_reg = (pipe == 0) ? PIPEASRC : PIPEBSRC;
+        int refclk;
+        struct psb_intel_clock_t clock;
+        u32 dpll = 0, fp = 0, dspcntr, pipeconf;
+        bool ok, is_sdvo = false, is_dvo = false;
+        bool is_crt = false, is_lvds = false, is_tv = false;
+        struct drm_mode_config *mode_config = &dev->mode_config;
+        struct drm_connector *connector;
+
+        /* No scan out no play */
+        if (crtc->fb == NULL) {
+                crtc_funcs->mode_set_base(crtc, x, y, old_fb);
+                return 0;
+        }
+
+        list_for_each_entry(connector, &mode_config->connector_list, head) {
+                struct psb_intel_output *psb_intel_output =
+                    to_psb_intel_output(connector);
+
+                if (!connector->encoder
+                    || connector->encoder->crtc != crtc)
+                        continue;
+
+                switch (psb_intel_output->type) {
+                case INTEL_OUTPUT_LVDS:
+                        is_lvds = true;
+                        break;
+                case INTEL_OUTPUT_SDVO:
+                        is_sdvo = true;
+                        break;
+                case INTEL_OUTPUT_DVO:
+                        is_dvo = true;
+                        break;
+                case INTEL_OUTPUT_TVOUT:
+                        is_tv = true;
+                        break;
+                case INTEL_OUTPUT_ANALOG:
+                        is_crt = true;
+                        break;
+                }
+        }
+
+        refclk = 96000;
+
+        ok = psb_intel_find_best_PLL(crtc, adjusted_mode->clock, refclk,
+                                 &clock);
+        if (!ok) {
+                dev_err(dev->dev, "Couldn't find PLL settings for mode!\n");
+                return 0;
+        }
+
+        fp = clock.n << 16 | clock.m1 << 8 | clock.m2;
+
+        dpll = DPLL_VGA_MODE_DIS;
+        if (is_lvds) {
+                dpll |= DPLLB_MODE_LVDS;
+                dpll |= DPLL_DVO_HIGH_SPEED;
+        } else
+                dpll |= DPLLB_MODE_DAC_SERIAL;
+        if (is_sdvo) {
+                int sdvo_pixel_multiply =
+                            adjusted_mode->clock / mode->clock;
+                dpll |= DPLL_DVO_HIGH_SPEED;
+                dpll |=
+                    (sdvo_pixel_multiply - 1) << SDVO_MULTIPLIER_SHIFT_HIRES;
+        }
+
+        /* compute bitmask from p1 value */
+        dpll |= (1 << (clock.p1 - 1)) << 16;
+        switch (clock.p2) {
+        case 5:
+                dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_5;
+                break;
+        case 7:
+                dpll |= DPLLB_LVDS_P2_CLOCK_DIV_7;
+                break;
+        case 10:
+                dpll |= DPLL_DAC_SERIAL_P2_CLOCK_DIV_10;
+                break;
+        case 14:
+                dpll |= DPLLB_LVDS_P2_CLOCK_DIV_14;
+                break;
+        }
+
+        if (is_tv) {
+                /* XXX: just matching BIOS for now */
+/*      dpll |= PLL_REF_INPUT_TVCLKINBC; */
+                dpll |= 3;
+        }
+        dpll |= PLL_REF_INPUT_DREFCLK;
+
+        /* setup pipeconf */
+        pipeconf = REG_READ(pipeconf_reg);
+
+        /* Set up the display plane register */
+        dspcntr = DISPPLANE_GAMMA_ENABLE;
+
+        if (pipe == 0)
+                dspcntr |= DISPPLANE_SEL_PIPE_A;
+        else
+                dspcntr |= DISPPLANE_SEL_PIPE_B;
+
+        dspcntr |= DISPLAY_PLANE_ENABLE;
+        pipeconf |= PIPEACONF_ENABLE;
+        dpll |= DPLL_VCO_ENABLE;
+
+
+        /* Disable the panel fitter if it was on our pipe */
+        if (psb_intel_panel_fitter_pipe(dev) == pipe)
+                REG_WRITE(PFIT_CONTROL, 0);
+
+        drm_mode_debug_printmodeline(mode);
+
+        if (dpll & DPLL_VCO_ENABLE) {
+                REG_WRITE(fp_reg, fp);
+                REG_WRITE(dpll_reg, dpll & ~DPLL_VCO_ENABLE);
+                REG_READ(dpll_reg);
+                udelay(150);
+        }
+
+        /* The LVDS pin pair needs to be on before the DPLLs are enabled.
+         * This is an exception to the general rule that mode_set doesn't turn
+         * things on.
+         */
+        if (is_lvds) {
+                u32 lvds = REG_READ(LVDS);
+
+                lvds &= ~LVDS_PIPEB_SELECT;
+                if (pipe == 1)
+                        lvds |= LVDS_PIPEB_SELECT;
+
+                lvds |= LVDS_PORT_EN | LVDS_A0A2_CLKA_POWER_UP;
+                /* Set the B0-B3 data pairs corresponding to
+                 * whether we're going to
+                 * set the DPLLs for dual-channel mode or not.
+                 */
+                lvds &= ~(LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP);
+                if (clock.p2 == 7)
+                        lvds |= LVDS_B0B3_POWER_UP | LVDS_CLKB_POWER_UP;
+
+                /* It would be nice to set 24 vs 18-bit mode (LVDS_A3_POWER_UP)
+                 * appropriately here, but we need to look more
+                 * thoroughly into how panels behave in the two modes.
+                 */
+
+                REG_WRITE(LVDS, lvds);
+                REG_READ(LVDS);
+        }
+
+        REG_WRITE(fp_reg, fp);
+        REG_WRITE(dpll_reg, dpll);
+        REG_READ(dpll_reg);
+        /* Wait for the clocks to stabilize. */
+        udelay(150);
+
+        /* write it again -- the BIOS does, after all */
+        REG_WRITE(dpll_reg, dpll);
+
+        REG_READ(dpll_reg);
+        /* Wait for the clocks to stabilize. */
+        udelay(150);
+
+        REG_WRITE(htot_reg, (adjusted_mode->crtc_hdisplay - 1) |
+                  ((adjusted_mode->crtc_htotal - 1) << 16));
+        REG_WRITE(hblank_reg, (adjusted_mode->crtc_hblank_start - 1) |
+                  ((adjusted_mode->crtc_hblank_end - 1) << 16));
+        REG_WRITE(hsync_reg, (adjusted_mode->crtc_hsync_start - 1) |
+                  ((adjusted_mode->crtc_hsync_end - 1) << 16));
+        REG_WRITE(vtot_reg, (adjusted_mode->crtc_vdisplay - 1) |
+                  ((adjusted_mode->crtc_vtotal - 1) << 16));
+        REG_WRITE(vblank_reg, (adjusted_mode->crtc_vblank_start - 1) |
+                  ((adjusted_mode->crtc_vblank_end - 1) << 16));
+        REG_WRITE(vsync_reg, (adjusted_mode->crtc_vsync_start - 1) |
+                  ((adjusted_mode->crtc_vsync_end - 1) << 16));
+        /* pipesrc and dspsize control the size that is scaled from,
+         * which should always be the user's requested size.
+         */
+        REG_WRITE(dspsize_reg,
+                  ((mode->vdisplay - 1) << 16) | (mode->hdisplay - 1));
+        REG_WRITE(dsppos_reg, 0);
+        REG_WRITE(pipesrc_reg,
+                  ((mode->hdisplay - 1) << 16) | (mode->vdisplay - 1));
+        REG_WRITE(pipeconf_reg, pipeconf);
+        REG_READ(pipeconf_reg);
+
+        psb_intel_wait_for_vblank(dev);
+
+        REG_WRITE(dspcntr_reg, dspcntr);
+
+        /* Flush the plane changes */
+        crtc_funcs->mode_set_base(crtc, x, y, old_fb);
+
+        psb_intel_wait_for_vblank(dev);
+
+        return 0;
+}
+
+/** Loads the palette/gamma unit for the CRTC with the prepared values */
+void psb_intel_crtc_load_lut(struct drm_crtc *crtc)
+{
+        struct drm_device *dev = crtc->dev;
+        struct drm_psb_private *dev_priv =
+                                (struct drm_psb_private *)dev->dev_private;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int palreg = PALETTE_A;
+        int i;
+
+        /* The clocks have to be on to load the palette. */
+        if (!crtc->enabled)
+                return;
+
+        switch (psb_intel_crtc->pipe) {
+        case 0:
+                break;
+        case 1:
+                palreg = PALETTE_B;
+                break;
+        case 2:
+                palreg = PALETTE_C;
+                break;
+        default:
+                dev_err(dev->dev, "Illegal Pipe Number.\n");
+                return;
+        }
+
+        if (gma_power_begin(dev, false)) {
+                for (i = 0; i < 256; i++) {
+                        REG_WRITE(palreg + 4 * i,
+                                  ((psb_intel_crtc->lut_r[i] +
+                                  psb_intel_crtc->lut_adj[i]) << 16) |
+                                  ((psb_intel_crtc->lut_g[i] +
+                                  psb_intel_crtc->lut_adj[i]) << 8) |
+                                  (psb_intel_crtc->lut_b[i] +
+                                  psb_intel_crtc->lut_adj[i]));
+                }
+                gma_power_end(dev);
+        } else {
+                for (i = 0; i < 256; i++) {
+                        dev_priv->save_palette_a[i] =
+                                  ((psb_intel_crtc->lut_r[i] +
+                                  psb_intel_crtc->lut_adj[i]) << 16) |
+                                  ((psb_intel_crtc->lut_g[i] +
+                                  psb_intel_crtc->lut_adj[i]) << 8) |
+                                  (psb_intel_crtc->lut_b[i] +
+                                  psb_intel_crtc->lut_adj[i]);
+                }
+
+        }
+}
+
+/**
+ * Save HW states of giving crtc
+ */
+static void psb_intel_crtc_save(struct drm_crtc *crtc)
+{
+        struct drm_device *dev = crtc->dev;
+        /* struct drm_psb_private *dev_priv =
+                        (struct drm_psb_private *)dev->dev_private; */
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
+        int pipeA = (psb_intel_crtc->pipe == 0);
+        uint32_t paletteReg;
+        int i;
+
+        if (!crtc_state) {
+                dev_err(dev->dev, "No CRTC state found\n");
+                return;
+        }
+
+        crtc_state->saveDSPCNTR = REG_READ(pipeA ? DSPACNTR : DSPBCNTR);
+        crtc_state->savePIPECONF = REG_READ(pipeA ? PIPEACONF : PIPEBCONF);
+        crtc_state->savePIPESRC = REG_READ(pipeA ? PIPEASRC : PIPEBSRC);
+        crtc_state->saveFP0 = REG_READ(pipeA ? FPA0 : FPB0);
+        crtc_state->saveFP1 = REG_READ(pipeA ? FPA1 : FPB1);
+        crtc_state->saveDPLL = REG_READ(pipeA ? DPLL_A : DPLL_B);
+        crtc_state->saveHTOTAL = REG_READ(pipeA ? HTOTAL_A : HTOTAL_B);
+        crtc_state->saveHBLANK = REG_READ(pipeA ? HBLANK_A : HBLANK_B);
+        crtc_state->saveHSYNC = REG_READ(pipeA ? HSYNC_A : HSYNC_B);
+        crtc_state->saveVTOTAL = REG_READ(pipeA ? VTOTAL_A : VTOTAL_B);
+        crtc_state->saveVBLANK = REG_READ(pipeA ? VBLANK_A : VBLANK_B);
+        crtc_state->saveVSYNC = REG_READ(pipeA ? VSYNC_A : VSYNC_B);
+        crtc_state->saveDSPSTRIDE = REG_READ(pipeA ? DSPASTRIDE : DSPBSTRIDE);
+
+        /*NOTE: DSPSIZE DSPPOS only for psb*/
+        crtc_state->saveDSPSIZE = REG_READ(pipeA ? DSPASIZE : DSPBSIZE);
+        crtc_state->saveDSPPOS = REG_READ(pipeA ? DSPAPOS : DSPBPOS);
+
+        crtc_state->saveDSPBASE = REG_READ(pipeA ? DSPABASE : DSPBBASE);
+
+        paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+        for (i = 0; i < 256; ++i)
+                crtc_state->savePalette[i] = REG_READ(paletteReg + (i << 2));
+}
+
+/**
+ * Restore HW states of giving crtc
+ */
+static void psb_intel_crtc_restore(struct drm_crtc *crtc)
+{
+        struct drm_device *dev = crtc->dev;
+        /* struct drm_psb_private * dev_priv =
+                                (struct drm_psb_private *)dev->dev_private; */
+        struct psb_intel_crtc *psb_intel_crtc =  to_psb_intel_crtc(crtc);
+        struct psb_intel_crtc_state *crtc_state = psb_intel_crtc->crtc_state;
+        /* struct drm_crtc_helper_funcs * crtc_funcs = crtc->helper_private; */
+        int pipeA = (psb_intel_crtc->pipe == 0);
+        uint32_t paletteReg;
+        int i;
+
+        if (!crtc_state) {
+                dev_err(dev->dev, "No crtc state\n");
+                return;
+        }
+
+        if (crtc_state->saveDPLL & DPLL_VCO_ENABLE) {
+                REG_WRITE(pipeA ? DPLL_A : DPLL_B,
+                        crtc_state->saveDPLL & ~DPLL_VCO_ENABLE);
+                REG_READ(pipeA ? DPLL_A : DPLL_B);
+                udelay(150);
+        }
+
+        REG_WRITE(pipeA ? FPA0 : FPB0, crtc_state->saveFP0);
+        REG_READ(pipeA ? FPA0 : FPB0);
+
+        REG_WRITE(pipeA ? FPA1 : FPB1, crtc_state->saveFP1);
+        REG_READ(pipeA ? FPA1 : FPB1);
+
+        REG_WRITE(pipeA ? DPLL_A : DPLL_B, crtc_state->saveDPLL);
+        REG_READ(pipeA ? DPLL_A : DPLL_B);
+        udelay(150);
+
+        REG_WRITE(pipeA ? HTOTAL_A : HTOTAL_B, crtc_state->saveHTOTAL);
+        REG_WRITE(pipeA ? HBLANK_A : HBLANK_B, crtc_state->saveHBLANK);
+        REG_WRITE(pipeA ? HSYNC_A : HSYNC_B, crtc_state->saveHSYNC);
+        REG_WRITE(pipeA ? VTOTAL_A : VTOTAL_B, crtc_state->saveVTOTAL);
+        REG_WRITE(pipeA ? VBLANK_A : VBLANK_B, crtc_state->saveVBLANK);
+        REG_WRITE(pipeA ? VSYNC_A : VSYNC_B, crtc_state->saveVSYNC);
+        REG_WRITE(pipeA ? DSPASTRIDE : DSPBSTRIDE, crtc_state->saveDSPSTRIDE);
+
+        REG_WRITE(pipeA ? DSPASIZE : DSPBSIZE, crtc_state->saveDSPSIZE);
+        REG_WRITE(pipeA ? DSPAPOS : DSPBPOS, crtc_state->saveDSPPOS);
+
+        REG_WRITE(pipeA ? PIPEASRC : PIPEBSRC, crtc_state->savePIPESRC);
+        REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
+        REG_WRITE(pipeA ? PIPEACONF : PIPEBCONF, crtc_state->savePIPECONF);
+
+        psb_intel_wait_for_vblank(dev);
+
+        REG_WRITE(pipeA ? DSPACNTR : DSPBCNTR, crtc_state->saveDSPCNTR);
+        REG_WRITE(pipeA ? DSPABASE : DSPBBASE, crtc_state->saveDSPBASE);
+
+        psb_intel_wait_for_vblank(dev);
+
+        paletteReg = pipeA ? PALETTE_A : PALETTE_B;
+        for (i = 0; i < 256; ++i)
+                REG_WRITE(paletteReg + (i << 2), crtc_state->savePalette[i]);
+}
+
+static int psb_intel_crtc_cursor_set(struct drm_crtc *crtc,
+                                 struct drm_file *file_priv,
+                                 uint32_t handle,
+                                 uint32_t width, uint32_t height)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        uint32_t control = (pipe == 0) ? CURACNTR : CURBCNTR;
+        uint32_t base = (pipe == 0) ? CURABASE : CURBBASE;
+        uint32_t temp;
+        size_t addr = 0;
+        struct gtt_range *gt;
+        struct drm_gem_object *obj;
+        int ret;
+
+        /* if we want to turn of the cursor ignore width and height */
+        if (!handle) {
+                /* turn off the cursor */
+                temp = CURSOR_MODE_DISABLE;
+
+                if (gma_power_begin(dev, false)) {
+                        REG_WRITE(control, temp);
+                        REG_WRITE(base, 0);
+                        gma_power_end(dev);
+                }
+
+                /* Unpin the old GEM object */
+                if (psb_intel_crtc->cursor_obj) {
+                        gt = container_of(psb_intel_crtc->cursor_obj,
+                                                        struct gtt_range, gem);
+                        psb_gtt_unpin(gt);
+                        drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
+                        psb_intel_crtc->cursor_obj = NULL;
+                }
+
+                return 0;
+        }
+
+        /* Currently we only support 64x64 cursors */
+        if (width != 64 || height != 64) {
+                dev_dbg(dev->dev, "we currently only support 64x64 cursors\n");
+                return -EINVAL;
+        }
+
+        obj = drm_gem_object_lookup(dev, file_priv, handle);
+        if (!obj)
+                return -ENOENT;
+
+        if (obj->size < width * height * 4) {
+                dev_dbg(dev->dev, "buffer is to small\n");
+                return -ENOMEM;
+        }
+
+        gt = container_of(obj, struct gtt_range, gem);
+
+        /* Pin the memory into the GTT */
+        ret = psb_gtt_pin(gt);
+        if (ret) {
+                dev_err(dev->dev, "Can not pin down handle 0x%x\n", handle);
+                return ret;
+        }
+
+
+        addr = gt->offset;      /* Or resource.start ??? */
+
+        psb_intel_crtc->cursor_addr = addr;
+
+        temp = 0;
+        /* set the pipe for the cursor */
+        temp |= (pipe << 28);
+        temp |= CURSOR_MODE_64_ARGB_AX | MCURSOR_GAMMA_ENABLE;
+
+        if (gma_power_begin(dev, false)) {
+                REG_WRITE(control, temp);
+                REG_WRITE(base, addr);
+                gma_power_end(dev);
+        }
+
+        /* unpin the old bo */
+        if (psb_intel_crtc->cursor_obj) {
+                gt = container_of(psb_intel_crtc->cursor_obj,
+                                                        struct gtt_range, gem);
+                psb_gtt_unpin(gt);
+                drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
+                psb_intel_crtc->cursor_obj = obj;
+        }
+        return 0;
+}
+
+static int psb_intel_crtc_cursor_move(struct drm_crtc *crtc, int x, int y)
+{
+        struct drm_device *dev = crtc->dev;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        uint32_t temp = 0;
+        uint32_t addr;
+
+
+        if (x < 0) {
+                temp |= (CURSOR_POS_SIGN << CURSOR_X_SHIFT);
+                x = -x;
+        }
+        if (y < 0) {
+                temp |= (CURSOR_POS_SIGN << CURSOR_Y_SHIFT);
+                y = -y;
+        }
+
+        temp |= ((x & CURSOR_POS_MASK) << CURSOR_X_SHIFT);
+        temp |= ((y & CURSOR_POS_MASK) << CURSOR_Y_SHIFT);
+
+        addr = psb_intel_crtc->cursor_addr;
+
+        if (gma_power_begin(dev, false)) {
+                REG_WRITE((pipe == 0) ? CURAPOS : CURBPOS, temp);
+                REG_WRITE((pipe == 0) ? CURABASE : CURBBASE, addr);
+                gma_power_end(dev);
+        }
+        return 0;
+}
+
+void psb_intel_crtc_gamma_set(struct drm_crtc *crtc, u16 *red,
+                         u16 *green, u16 *blue, uint32_t type, uint32_t size)
+{
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int i;
+
+        if (size != 256)
+                return;
+
+        for (i = 0; i < 256; i++) {
+                psb_intel_crtc->lut_r[i] = red[i] >> 8;
+                psb_intel_crtc->lut_g[i] = green[i] >> 8;
+                psb_intel_crtc->lut_b[i] = blue[i] >> 8;
+        }
+
+        psb_intel_crtc_load_lut(crtc);
+}
+
+static int psb_crtc_set_config(struct drm_mode_set *set)
+{
+        int ret;
+        struct drm_device *dev = set->crtc->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (!dev_priv->rpm_enabled)
+                return drm_crtc_helper_set_config(set);
+
+        pm_runtime_forbid(&dev->pdev->dev);
+        ret = drm_crtc_helper_set_config(set);
+        pm_runtime_allow(&dev->pdev->dev);
+        return ret;
+}
+
+/* Returns the clock of the currently programmed mode of the given pipe. */
+static int psb_intel_crtc_clock_get(struct drm_device *dev,
+                                struct drm_crtc *crtc)
+{
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        u32 dpll;
+        u32 fp;
+        struct psb_intel_clock_t clock;
+        bool is_lvds;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (gma_power_begin(dev, false)) {
+                dpll = REG_READ((pipe == 0) ? DPLL_A : DPLL_B);
+                if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
+                        fp = REG_READ((pipe == 0) ? FPA0 : FPB0);
+                else
+                        fp = REG_READ((pipe == 0) ? FPA1 : FPB1);
+                is_lvds = (pipe == 1) && (REG_READ(LVDS) & LVDS_PORT_EN);
+                gma_power_end(dev);
+        } else {
+                dpll = (pipe == 0) ?
+                        dev_priv->saveDPLL_A : dev_priv->saveDPLL_B;
+
+                if ((dpll & DISPLAY_RATE_SELECT_FPA1) == 0)
+                        fp = (pipe == 0) ?
+                                dev_priv->saveFPA0 :
+                                dev_priv->saveFPB0;
+                else
+                        fp = (pipe == 0) ?
+                                dev_priv->saveFPA1 :
+                                dev_priv->saveFPB1;
+
+                is_lvds = (pipe == 1) && (dev_priv->saveLVDS & LVDS_PORT_EN);
+        }
+
+        clock.m1 = (fp & FP_M1_DIV_MASK) >> FP_M1_DIV_SHIFT;
+        clock.m2 = (fp & FP_M2_DIV_MASK) >> FP_M2_DIV_SHIFT;
+        clock.n = (fp & FP_N_DIV_MASK) >> FP_N_DIV_SHIFT;
+
+        if (is_lvds) {
+                clock.p1 =
+                    ffs((dpll &
+                         DPLL_FPA01_P1_POST_DIV_MASK_I830_LVDS) >>
+                        DPLL_FPA01_P1_POST_DIV_SHIFT);
+                clock.p2 = 14;
+
+                if ((dpll & PLL_REF_INPUT_MASK) ==
+                    PLLB_REF_INPUT_SPREADSPECTRUMIN) {
+                        /* XXX: might not be 66MHz */
+                        i8xx_clock(66000, &clock);
+                } else
+                        i8xx_clock(48000, &clock);
+        } else {
+                if (dpll & PLL_P1_DIVIDE_BY_TWO)
+                        clock.p1 = 2;
+                else {
+                        clock.p1 =
+                            ((dpll &
+                              DPLL_FPA01_P1_POST_DIV_MASK_I830) >>
+                             DPLL_FPA01_P1_POST_DIV_SHIFT) + 2;
+                }
+                if (dpll & PLL_P2_DIVIDE_BY_4)
+                        clock.p2 = 4;
+                else
+                        clock.p2 = 2;
+
+                i8xx_clock(48000, &clock);
+        }
+
+        /* XXX: It would be nice to validate the clocks, but we can't reuse
+         * i830PllIsValid() because it relies on the xf86_config connector
+         * configuration being accurate, which it isn't necessarily.
+         */
+
+        return clock.dot;
+}
+
+/** Returns the currently programmed mode of the given pipe. */
+struct drm_display_mode *psb_intel_crtc_mode_get(struct drm_device *dev,
+                                             struct drm_crtc *crtc)
+{
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        int pipe = psb_intel_crtc->pipe;
+        struct drm_display_mode *mode;
+        int htot;
+        int hsync;
+        int vtot;
+        int vsync;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+
+        if (gma_power_begin(dev, false)) {
+                htot = REG_READ((pipe == 0) ? HTOTAL_A : HTOTAL_B);
+                hsync = REG_READ((pipe == 0) ? HSYNC_A : HSYNC_B);
+                vtot = REG_READ((pipe == 0) ? VTOTAL_A : VTOTAL_B);
+                vsync = REG_READ((pipe == 0) ? VSYNC_A : VSYNC_B);
+                gma_power_end(dev);
+        } else {
+                htot = (pipe == 0) ?
+                        dev_priv->saveHTOTAL_A : dev_priv->saveHTOTAL_B;
+                hsync = (pipe == 0) ?
+                        dev_priv->saveHSYNC_A : dev_priv->saveHSYNC_B;
+                vtot = (pipe == 0) ?
+                        dev_priv->saveVTOTAL_A : dev_priv->saveVTOTAL_B;
+                vsync = (pipe == 0) ?
+                        dev_priv->saveVSYNC_A : dev_priv->saveVSYNC_B;
+        }
+
+        mode = kzalloc(sizeof(*mode), GFP_KERNEL);
+        if (!mode)
+                return NULL;
+
+        mode->clock = psb_intel_crtc_clock_get(dev, crtc);
+        mode->hdisplay = (htot & 0xffff) + 1;
+        mode->htotal = ((htot & 0xffff0000) >> 16) + 1;
+        mode->hsync_start = (hsync & 0xffff) + 1;
+        mode->hsync_end = ((hsync & 0xffff0000) >> 16) + 1;
+        mode->vdisplay = (vtot & 0xffff) + 1;
+        mode->vtotal = ((vtot & 0xffff0000) >> 16) + 1;
+        mode->vsync_start = (vsync & 0xffff) + 1;
+        mode->vsync_end = ((vsync & 0xffff0000) >> 16) + 1;
+
+        drm_mode_set_name(mode);
+        drm_mode_set_crtcinfo(mode, 0);
+
+        return mode;
+}
+
+void psb_intel_crtc_destroy(struct drm_crtc *crtc)
+{
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        struct gtt_range *gt;
+
+        /* Unpin the old GEM object */
+        if (psb_intel_crtc->cursor_obj) {
+                gt = container_of(psb_intel_crtc->cursor_obj,
+                                                struct gtt_range, gem);
+                psb_gtt_unpin(gt);
+                drm_gem_object_unreference(psb_intel_crtc->cursor_obj);
+                psb_intel_crtc->cursor_obj = NULL;
+        }
+        kfree(psb_intel_crtc->crtc_state);
+        drm_crtc_cleanup(crtc);
+        kfree(psb_intel_crtc);
+}
+
+const struct drm_crtc_helper_funcs psb_intel_helper_funcs = {
+        .dpms = psb_intel_crtc_dpms,
+        .mode_fixup = psb_intel_crtc_mode_fixup,
+        .mode_set = psb_intel_crtc_mode_set,
+        .mode_set_base = psb_intel_pipe_set_base,
+        .prepare = psb_intel_crtc_prepare,
+        .commit = psb_intel_crtc_commit,
+};
+
+const struct drm_crtc_funcs psb_intel_crtc_funcs = {
+        .save = psb_intel_crtc_save,
+        .restore = psb_intel_crtc_restore,
+        .cursor_set = psb_intel_crtc_cursor_set,
+        .cursor_move = psb_intel_crtc_cursor_move,
+        .gamma_set = psb_intel_crtc_gamma_set,
+        .set_config = psb_crtc_set_config,
+        .destroy = psb_intel_crtc_destroy,
+};
+
+/*
+ * Set the default value of cursor control and base register
+ * to zero. This is a workaround for h/w defect on Oaktrail
+ */
+static void psb_intel_cursor_init(struct drm_device *dev, int pipe)
+{
+        u32 control[3] = { CURACNTR, CURBCNTR, CURCCNTR };
+        u32 base[3] = { CURABASE, CURBBASE, CURCBASE };
+
+        REG_WRITE(control[pipe], 0);
+        REG_WRITE(base[pipe], 0);
+}
+
+void psb_intel_crtc_init(struct drm_device *dev, int pipe,
+                     struct psb_intel_mode_device *mode_dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct psb_intel_crtc *psb_intel_crtc;
+        int i;
+        uint16_t *r_base, *g_base, *b_base;
+
+        /* We allocate a extra array of drm_connector pointers
+         * for fbdev after the crtc */
+        psb_intel_crtc =
+            kzalloc(sizeof(struct psb_intel_crtc) +
+                    (INTELFB_CONN_LIMIT * sizeof(struct drm_connector *)),
+                    GFP_KERNEL);
+        if (psb_intel_crtc == NULL)
+                return;
+
+        psb_intel_crtc->crtc_state =
+                kzalloc(sizeof(struct psb_intel_crtc_state), GFP_KERNEL);
+        if (!psb_intel_crtc->crtc_state) {
+                dev_err(dev->dev, "Crtc state error: No memory\n");
+                kfree(psb_intel_crtc);
+                return;
+        }
+
+        /* Set the CRTC operations from the chip specific data */
+        drm_crtc_init(dev, &psb_intel_crtc->base, dev_priv->ops->crtc_funcs);
+
+        drm_mode_crtc_set_gamma_size(&psb_intel_crtc->base, 256);
+        psb_intel_crtc->pipe = pipe;
+        psb_intel_crtc->plane = pipe;
+
+        r_base = psb_intel_crtc->base.gamma_store;
+        g_base = r_base + 256;
+        b_base = g_base + 256;
+        for (i = 0; i < 256; i++) {
+                psb_intel_crtc->lut_r[i] = i;
+                psb_intel_crtc->lut_g[i] = i;
+                psb_intel_crtc->lut_b[i] = i;
+                r_base[i] = i << 8;
+                g_base[i] = i << 8;
+                b_base[i] = i << 8;
+
+                psb_intel_crtc->lut_adj[i] = 0;
+        }
+
+        psb_intel_crtc->mode_dev = mode_dev;
+        psb_intel_crtc->cursor_addr = 0;
+
+        drm_crtc_helper_add(&psb_intel_crtc->base,
+                                                dev_priv->ops->crtc_helper);
+
+        /* Setup the array of drm_connector pointer array */
+        psb_intel_crtc->mode_set.crtc = &psb_intel_crtc->base;
+        BUG_ON(pipe >= ARRAY_SIZE(dev_priv->plane_to_crtc_mapping) ||
+               dev_priv->plane_to_crtc_mapping[psb_intel_crtc->plane] != NULL);
+        dev_priv->plane_to_crtc_mapping[psb_intel_crtc->plane] =
+                                                        &psb_intel_crtc->base;
+        dev_priv->pipe_to_crtc_mapping[psb_intel_crtc->pipe] =
+                                                        &psb_intel_crtc->base;
+        psb_intel_crtc->mode_set.connectors =
+            (struct drm_connector **) (psb_intel_crtc + 1);
+        psb_intel_crtc->mode_set.num_connectors = 0;
+        psb_intel_cursor_init(dev, pipe);
+}
+
+int psb_intel_get_pipe_from_crtc_id(struct drm_device *dev, void *data,
+                                struct drm_file *file_priv)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        struct drm_psb_get_pipe_from_crtc_id_arg *pipe_from_crtc_id = data;
+        struct drm_mode_object *drmmode_obj;
+        struct psb_intel_crtc *crtc;
+
+        if (!dev_priv) {
+                dev_err(dev->dev, "called with no initialization\n");
+                return -EINVAL;
+        }
+
+        drmmode_obj = drm_mode_object_find(dev, pipe_from_crtc_id->crtc_id,
+                        DRM_MODE_OBJECT_CRTC);
+
+        if (!drmmode_obj) {
+                dev_err(dev->dev, "no such CRTC id\n");
+                return -EINVAL;
+        }
+
+        crtc = to_psb_intel_crtc(obj_to_crtc(drmmode_obj));
+        pipe_from_crtc_id->pipe = crtc->pipe;
+
+        return 0;
+}
+
+struct drm_crtc *psb_intel_get_crtc_from_pipe(struct drm_device *dev, int pipe)
+{
+        struct drm_crtc *crtc = NULL;
+
+        list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
+                struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+                if (psb_intel_crtc->pipe == pipe)
+                        break;
+        }
+        return crtc;
+}
+
+int psb_intel_connector_clones(struct drm_device *dev, int type_mask)
+{
+        int index_mask = 0;
+        struct drm_connector *connector;
+        int entry = 0;
+
+        list_for_each_entry(connector, &dev->mode_config.connector_list,
+                            head) {
+                struct psb_intel_output *psb_intel_output =
+                    to_psb_intel_output(connector);
+                if (type_mask & (1 << psb_intel_output->type))
+                        index_mask |= (1 << entry);
+                entry++;
+        }
+        return index_mask;
+}
+
+
+void psb_intel_modeset_cleanup(struct drm_device *dev)
+{
+        drm_mode_config_cleanup(dev);
+}
+
+
+/* current intel driver doesn't take advantage of encoders
+   always give back the encoder for the connector
+*/
+struct drm_encoder *psb_intel_best_encoder(struct drm_connector *connector)
+{
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+
+        return &psb_intel_output->enc;
+}
+
diff --git a/drivers/staging/gma500/psb_intel_display.h b/drivers/staging/gma500/psb_intel_display.h
new file mode 100644
index 00000000000..535b49a5e40
--- /dev/null
+++ b/drivers/staging/gma500/psb_intel_display.h
@@ -0,0 +1,28 @@
+/* copyright (c) 2008, Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors:
+ * Eric Anholt <eric@anholt.net>
+ */
+
+#ifndef _INTEL_DISPLAY_H_
+#define _INTEL_DISPLAY_H_
+
+bool psb_intel_pipe_has_type(struct drm_crtc *crtc, int type);
+void psb_intel_crtc_gamma_set(struct drm_crtc *crtc, u16 *red,
+                         u16 *green, u16 *blue, uint32_t type, uint32_t size);
+void psb_intel_crtc_destroy(struct drm_crtc *crtc);
+
+#endif
diff --git a/drivers/staging/gma500/psb_intel_drv.h b/drivers/staging/gma500/psb_intel_drv.h
new file mode 100644
index 00000000000..36b554b5c33
--- /dev/null
+++ b/drivers/staging/gma500/psb_intel_drv.h
@@ -0,0 +1,230 @@
+/*
+ * Copyright (c) 2009-2011, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ */
+
+#ifndef __INTEL_DRV_H__
+#define __INTEL_DRV_H__
+
+#include <linux/i2c.h>
+#include <linux/i2c-algo-bit.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_crtc_helper.h>
+#include <linux/gpio.h>
+
+/*
+ * Display related stuff
+ */
+
+/* store information about an Ixxx DVO */
+/* The i830->i865 use multiple DVOs with multiple i2cs */
+/* the i915, i945 have a single sDVO i2c bus - which is different */
+#define MAX_OUTPUTS 6
+/* maximum connectors per crtcs in the mode set */
+#define INTELFB_CONN_LIMIT 4
+
+#define INTEL_I2C_BUS_DVO 1
+#define INTEL_I2C_BUS_SDVO 2
+
+/* these are outputs from the chip - integrated only
+ * external chips are via DVO or SDVO output */
+#define INTEL_OUTPUT_UNUSED 0
+#define INTEL_OUTPUT_ANALOG 1
+#define INTEL_OUTPUT_DVO 2
+#define INTEL_OUTPUT_SDVO 3
+#define INTEL_OUTPUT_LVDS 4
+#define INTEL_OUTPUT_TVOUT 5
+#define INTEL_OUTPUT_HDMI 6
+#define INTEL_OUTPUT_MIPI 7
+#define INTEL_OUTPUT_MIPI2 8
+
+#define INTEL_DVO_CHIP_NONE 0
+#define INTEL_DVO_CHIP_LVDS 1
+#define INTEL_DVO_CHIP_TMDS 2
+#define INTEL_DVO_CHIP_TVOUT 4
+
+/*
+ * Hold information useally put on the device driver privates here,
+ * since it needs to be shared across multiple of devices drivers privates.
+ */
+struct psb_intel_mode_device {
+
+        /*
+         * Abstracted memory manager operations
+         */
+         size_t(*bo_offset) (struct drm_device *dev, void *bo);
+
+        /*
+         * Cursor (Can go ?)
+         */
+        int cursor_needs_physical;
+
+        /*
+         * LVDS info
+         */
+        int backlight_duty_cycle;       /* restore backlight to this value */
+        bool panel_wants_dither;
+        struct drm_display_mode *panel_fixed_mode;
+        struct drm_display_mode *panel_fixed_mode2;
+        struct drm_display_mode *vbt_mode;      /* if any */
+
+        uint32_t saveBLC_PWM_CTL;
+};
+
+struct psb_intel_i2c_chan {
+        /* for getting at dev. private (mmio etc.) */
+        struct drm_device *drm_dev;
+        u32 reg;                /* GPIO reg */
+        struct i2c_adapter adapter;
+        struct i2c_algo_bit_data algo;
+        u8 slave_addr;
+};
+
+struct psb_intel_output {
+        struct drm_connector base;
+
+        struct drm_encoder enc;
+        int type;
+
+        struct psb_intel_i2c_chan *i2c_bus;     /* for control functions */
+        struct psb_intel_i2c_chan *ddc_bus;     /* for DDC only stuff */
+        bool load_detect_temp;
+        void *dev_priv;
+
+        struct psb_intel_mode_device *mode_dev;
+        struct i2c_adapter *hdmi_i2c_adapter;   /* for control functions */
+};
+
+struct psb_intel_crtc_state {
+        uint32_t saveDSPCNTR;
+        uint32_t savePIPECONF;
+        uint32_t savePIPESRC;
+        uint32_t saveDPLL;
+        uint32_t saveFP0;
+        uint32_t saveFP1;
+        uint32_t saveHTOTAL;
+        uint32_t saveHBLANK;
+        uint32_t saveHSYNC;
+        uint32_t saveVTOTAL;
+        uint32_t saveVBLANK;
+        uint32_t saveVSYNC;
+        uint32_t saveDSPSTRIDE;
+        uint32_t saveDSPSIZE;
+        uint32_t saveDSPPOS;
+        uint32_t saveDSPBASE;
+        uint32_t savePalette[256];
+};
+
+struct psb_intel_crtc {
+        struct drm_crtc base;
+        int pipe;
+        int plane;
+        uint32_t cursor_addr;
+        u8 lut_r[256], lut_g[256], lut_b[256];
+        u8 lut_adj[256];
+        struct psb_intel_framebuffer *fbdev_fb;
+        /* a mode_set for fbdev users on this crtc */
+        struct drm_mode_set mode_set;
+
+        /* GEM object that holds our cursor */
+        struct drm_gem_object *cursor_obj;
+
+        struct drm_display_mode saved_mode;
+        struct drm_display_mode saved_adjusted_mode;
+
+        struct psb_intel_mode_device *mode_dev;
+
+        /*crtc mode setting flags*/
+        u32 mode_flags;
+
+        /* Saved Crtc HW states */
+        struct psb_intel_crtc_state *crtc_state;
+};
+
+#define to_psb_intel_crtc(x)    \
+                container_of(x, struct psb_intel_crtc, base)
+#define to_psb_intel_output(x)  \
+                container_of(x, struct psb_intel_output, base)
+#define enc_to_psb_intel_output(x)      \
+                container_of(x, struct psb_intel_output, enc)
+#define to_psb_intel_framebuffer(x)     \
+                container_of(x, struct psb_intel_framebuffer, base)
+
+struct psb_intel_i2c_chan *psb_intel_i2c_create(struct drm_device *dev,
+                                        const u32 reg, const char *name);
+void psb_intel_i2c_destroy(struct psb_intel_i2c_chan *chan);
+int psb_intel_ddc_get_modes(struct psb_intel_output *psb_intel_output);
+extern bool psb_intel_ddc_probe(struct psb_intel_output *psb_intel_output);
+
+extern void psb_intel_crtc_init(struct drm_device *dev, int pipe,
+                            struct psb_intel_mode_device *mode_dev);
+extern void psb_intel_crt_init(struct drm_device *dev);
+extern void psb_intel_sdvo_init(struct drm_device *dev, int output_device);
+extern void psb_intel_dvo_init(struct drm_device *dev);
+extern void psb_intel_tv_init(struct drm_device *dev);
+extern void psb_intel_lvds_init(struct drm_device *dev,
+                            struct psb_intel_mode_device *mode_dev);
+extern void psb_intel_lvds_set_brightness(struct drm_device *dev, int level);
+extern void mrst_lvds_init(struct drm_device *dev,
+                           struct psb_intel_mode_device *mode_dev);
+extern void mrst_wait_for_INTR_PKT_SENT(struct drm_device *dev);
+extern void mrst_dsi_init(struct drm_device *dev,
+                           struct psb_intel_mode_device *mode_dev);
+extern void mid_dsi_init(struct drm_device *dev,
+                    struct psb_intel_mode_device *mode_dev, int dsi_num);
+
+extern void psb_intel_crtc_load_lut(struct drm_crtc *crtc);
+extern void psb_intel_encoder_prepare(struct drm_encoder *encoder);
+extern void psb_intel_encoder_commit(struct drm_encoder *encoder);
+
+extern struct drm_encoder *psb_intel_best_encoder(struct drm_connector
+                                              *connector);
+
+extern struct drm_display_mode *psb_intel_crtc_mode_get(struct drm_device *dev,
+                                                    struct drm_crtc *crtc);
+extern void psb_intel_wait_for_vblank(struct drm_device *dev);
+extern int psb_intel_get_pipe_from_crtc_id(struct drm_device *dev, void *data,
+                                struct drm_file *file_priv);
+extern struct drm_crtc *psb_intel_get_crtc_from_pipe(struct drm_device *dev,
+                                                 int pipe);
+extern struct drm_connector *psb_intel_sdvo_find(struct drm_device *dev,
+                                             int sdvoB);
+extern int psb_intel_sdvo_supports_hotplug(struct drm_connector *connector);
+extern void psb_intel_sdvo_set_hotplug(struct drm_connector *connector,
+                                   int enable);
+extern int intelfb_probe(struct drm_device *dev);
+extern int intelfb_remove(struct drm_device *dev,
+                          struct drm_framebuffer *fb);
+extern struct drm_framebuffer *psb_intel_framebuffer_create(struct drm_device
+                                                        *dev, struct
+                                                        drm_mode_fb_cmd
+                                                        *mode_cmd,
+                                                        void *mm_private);
+extern bool psb_intel_lvds_mode_fixup(struct drm_encoder *encoder,
+                                      struct drm_display_mode *mode,
+                                      struct drm_display_mode *adjusted_mode);
+extern int psb_intel_lvds_mode_valid(struct drm_connector *connector,
+                                     struct drm_display_mode *mode);
+extern int psb_intel_lvds_set_property(struct drm_connector *connector,
+                                        struct drm_property *property,
+                                        uint64_t value);
+extern void psb_intel_lvds_destroy(struct drm_connector *connector);
+extern const struct drm_encoder_funcs psb_intel_lvds_enc_funcs;
+
+extern void mdfldWaitForPipeDisable(struct drm_device *dev, int pipe);
+extern void mdfldWaitForPipeEnable(struct drm_device *dev, int pipe);
+
+#endif                          /* __INTEL_DRV_H__ */
diff --git a/drivers/staging/gma500/psb_intel_lvds.c b/drivers/staging/gma500/psb_intel_lvds.c
new file mode 100644
index 00000000000..c6436da6073
--- /dev/null
+++ b/drivers/staging/gma500/psb_intel_lvds.c
@@ -0,0 +1,849 @@
+/*
+ * Copyright © 2006-2007 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors:
+ *      Eric Anholt <eric@anholt.net>
+ *      Dave Airlie <airlied@linux.ie>
+ *      Jesse Barnes <jesse.barnes@intel.com>
+ */
+
+#include <linux/i2c.h>
+#include <drm/drmP.h>
+
+#include "intel_bios.h"
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "power.h"
+#include <linux/pm_runtime.h>
+
+/*
+ * LVDS I2C backlight control macros
+ */
+#define BRIGHTNESS_MAX_LEVEL 100
+#define BRIGHTNESS_MASK 0xFF
+#define BLC_I2C_TYPE    0x01
+#define BLC_PWM_TYPT    0x02
+
+#define BLC_POLARITY_NORMAL 0
+#define BLC_POLARITY_INVERSE 1
+
+#define PSB_BLC_MAX_PWM_REG_FREQ       (0xFFFE)
+#define PSB_BLC_MIN_PWM_REG_FREQ        (0x2)
+#define PSB_BLC_PWM_PRECISION_FACTOR    (10)
+#define PSB_BACKLIGHT_PWM_CTL_SHIFT     (16)
+#define PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR (0xFFFE)
+
+struct psb_intel_lvds_priv {
+        /*
+         * Saved LVDO output states
+         */
+        uint32_t savePP_ON;
+        uint32_t savePP_OFF;
+        uint32_t saveLVDS;
+        uint32_t savePP_CONTROL;
+        uint32_t savePP_CYCLE;
+        uint32_t savePFIT_CONTROL;
+        uint32_t savePFIT_PGM_RATIOS;
+        uint32_t saveBLC_PWM_CTL;
+};
+
+
+/*
+ * Returns the maximum level of the backlight duty cycle field.
+ */
+static u32 psb_intel_lvds_get_max_backlight(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 retVal;
+
+        if (gma_power_begin(dev, false)) {
+                retVal = ((REG_READ(BLC_PWM_CTL) &
+                          BACKLIGHT_MODULATION_FREQ_MASK) >>
+                          BACKLIGHT_MODULATION_FREQ_SHIFT) * 2;
+
+                gma_power_end(dev);
+        } else
+                retVal = ((dev_priv->saveBLC_PWM_CTL &
+                          BACKLIGHT_MODULATION_FREQ_MASK) >>
+                          BACKLIGHT_MODULATION_FREQ_SHIFT) * 2;
+
+        return retVal;
+}
+
+/*
+ * Set LVDS backlight level by I2C command
+ *
+ * FIXME: at some point we need to both track this for PM and also
+ * disable runtime pm on MRST if the brightness is nil (ie blanked)
+ */
+static int psb_lvds_i2c_set_brightness(struct drm_device *dev,
+                                        unsigned int level)
+{
+        struct drm_psb_private *dev_priv =
+                (struct drm_psb_private *)dev->dev_private;
+
+        struct psb_intel_i2c_chan *lvds_i2c_bus = dev_priv->lvds_i2c_bus;
+        u8 out_buf[2];
+        unsigned int blc_i2c_brightness;
+
+        struct i2c_msg msgs[] = {
+                {
+                        .addr = lvds_i2c_bus->slave_addr,
+                        .flags = 0,
+                        .len = 2,
+                        .buf = out_buf,
+                }
+        };
+
+        blc_i2c_brightness = BRIGHTNESS_MASK & ((unsigned int)level *
+                             BRIGHTNESS_MASK /
+                             BRIGHTNESS_MAX_LEVEL);
+
+        if (dev_priv->lvds_bl->pol == BLC_POLARITY_INVERSE)
+                blc_i2c_brightness = BRIGHTNESS_MASK - blc_i2c_brightness;
+
+        out_buf[0] = dev_priv->lvds_bl->brightnesscmd;
+        out_buf[1] = (u8)blc_i2c_brightness;
+
+        if (i2c_transfer(&lvds_i2c_bus->adapter, msgs, 1) == 1) {
+                dev_dbg(dev->dev, "I2C set brightness.(command, value) (%d, %d)\n",
+                        dev_priv->lvds_bl->brightnesscmd,
+                        blc_i2c_brightness);
+                return 0;
+        }
+
+        dev_err(dev->dev, "I2C transfer error\n");
+        return -1;
+}
+
+
+static int psb_lvds_pwm_set_brightness(struct drm_device *dev, int level)
+{
+        struct drm_psb_private *dev_priv =
+                        (struct drm_psb_private *)dev->dev_private;
+
+        u32 max_pwm_blc;
+        u32 blc_pwm_duty_cycle;
+
+        max_pwm_blc = psb_intel_lvds_get_max_backlight(dev);
+
+        /*BLC_PWM_CTL Should be initiated while backlight device init*/
+        BUG_ON((max_pwm_blc & PSB_BLC_MAX_PWM_REG_FREQ) == 0);
+
+        blc_pwm_duty_cycle = level * max_pwm_blc / BRIGHTNESS_MAX_LEVEL;
+
+        if (dev_priv->lvds_bl->pol == BLC_POLARITY_INVERSE)
+                blc_pwm_duty_cycle = max_pwm_blc - blc_pwm_duty_cycle;
+
+        blc_pwm_duty_cycle &= PSB_BACKLIGHT_PWM_POLARITY_BIT_CLEAR;
+        REG_WRITE(BLC_PWM_CTL,
+                  (max_pwm_blc << PSB_BACKLIGHT_PWM_CTL_SHIFT) |
+                  (blc_pwm_duty_cycle));
+
+        return 0;
+}
+
+/*
+ * Set LVDS backlight level either by I2C or PWM
+ */
+void psb_intel_lvds_set_brightness(struct drm_device *dev, int level)
+{
+        /*u32 blc_pwm_ctl;*/
+        struct drm_psb_private *dev_priv =
+                        (struct drm_psb_private *)dev->dev_private;
+
+        dev_dbg(dev->dev, "backlight level is %d\n", level);
+
+        if (!dev_priv->lvds_bl) {
+                dev_err(dev->dev, "NO LVDS Backlight Info\n");
+                return;
+        }
+
+        if (dev_priv->lvds_bl->type == BLC_I2C_TYPE)
+                psb_lvds_i2c_set_brightness(dev, level);
+        else
+                psb_lvds_pwm_set_brightness(dev, level);
+}
+
+/*
+ * Sets the backlight level.
+ *
+ * level: backlight level, from 0 to psb_intel_lvds_get_max_backlight().
+ */
+static void psb_intel_lvds_set_backlight(struct drm_device *dev, int level)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 blc_pwm_ctl;
+
+        if (gma_power_begin(dev, false)) {
+                blc_pwm_ctl =
+                        REG_READ(BLC_PWM_CTL) & ~BACKLIGHT_DUTY_CYCLE_MASK;
+                REG_WRITE(BLC_PWM_CTL,
+                                (blc_pwm_ctl |
+                                (level << BACKLIGHT_DUTY_CYCLE_SHIFT)));
+                gma_power_end(dev);
+        } else {
+                blc_pwm_ctl = dev_priv->saveBLC_PWM_CTL &
+                                ~BACKLIGHT_DUTY_CYCLE_MASK;
+                dev_priv->saveBLC_PWM_CTL = (blc_pwm_ctl |
+                                        (level << BACKLIGHT_DUTY_CYCLE_SHIFT));
+        }
+}
+
+/*
+ * Sets the power state for the panel.
+ */
+static void psb_intel_lvds_set_power(struct drm_device *dev,
+                                 struct psb_intel_output *output, bool on)
+{
+        u32 pp_status;
+
+        if (!gma_power_begin(dev, true))
+                return;
+
+        if (on) {
+                REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) |
+                          POWER_TARGET_ON);
+                do {
+                        pp_status = REG_READ(PP_STATUS);
+                } while ((pp_status & PP_ON) == 0);
+
+                psb_intel_lvds_set_backlight(dev,
+                                         output->
+                                         mode_dev->backlight_duty_cycle);
+        } else {
+                psb_intel_lvds_set_backlight(dev, 0);
+
+                REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) &
+                          ~POWER_TARGET_ON);
+                do {
+                        pp_status = REG_READ(PP_STATUS);
+                } while (pp_status & PP_ON);
+        }
+
+        gma_power_end(dev);
+}
+
+static void psb_intel_lvds_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+
+        if (mode == DRM_MODE_DPMS_ON)
+                psb_intel_lvds_set_power(dev, output, true);
+        else
+                psb_intel_lvds_set_power(dev, output, false);
+
+        /* XXX: We never power down the LVDS pairs. */
+}
+
+static void psb_intel_lvds_save(struct drm_connector *connector)
+{
+        struct drm_device *dev = connector->dev;
+        struct drm_psb_private *dev_priv =
+                (struct drm_psb_private *)dev->dev_private;
+        struct psb_intel_output *psb_intel_output =
+                to_psb_intel_output(connector);
+        struct psb_intel_lvds_priv *lvds_priv =
+                (struct psb_intel_lvds_priv *)psb_intel_output->dev_priv;
+
+        lvds_priv->savePP_ON = REG_READ(LVDSPP_ON);
+        lvds_priv->savePP_OFF = REG_READ(LVDSPP_OFF);
+        lvds_priv->saveLVDS = REG_READ(LVDS);
+        lvds_priv->savePP_CONTROL = REG_READ(PP_CONTROL);
+        lvds_priv->savePP_CYCLE = REG_READ(PP_CYCLE);
+        /*lvds_priv->savePP_DIVISOR = REG_READ(PP_DIVISOR);*/
+        lvds_priv->saveBLC_PWM_CTL = REG_READ(BLC_PWM_CTL);
+        lvds_priv->savePFIT_CONTROL = REG_READ(PFIT_CONTROL);
+        lvds_priv->savePFIT_PGM_RATIOS = REG_READ(PFIT_PGM_RATIOS);
+
+        /*TODO: move backlight_duty_cycle to psb_intel_lvds_priv*/
+        dev_priv->backlight_duty_cycle = (dev_priv->saveBLC_PWM_CTL &
+                                                BACKLIGHT_DUTY_CYCLE_MASK);
+
+        /*
+         * If the light is off at server startup,
+         * just make it full brightness
+         */
+        if (dev_priv->backlight_duty_cycle == 0)
+                dev_priv->backlight_duty_cycle =
+                psb_intel_lvds_get_max_backlight(dev);
+
+        dev_dbg(dev->dev, "(0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x)\n",
+                        lvds_priv->savePP_ON,
+                        lvds_priv->savePP_OFF,
+                        lvds_priv->saveLVDS,
+                        lvds_priv->savePP_CONTROL,
+                        lvds_priv->savePP_CYCLE,
+                        lvds_priv->saveBLC_PWM_CTL);
+}
+
+static void psb_intel_lvds_restore(struct drm_connector *connector)
+{
+        struct drm_device *dev = connector->dev;
+        u32 pp_status;
+
+        /*struct drm_psb_private *dev_priv =
+                                (struct drm_psb_private *)dev->dev_private;*/
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+        struct psb_intel_lvds_priv *lvds_priv =
+                (struct psb_intel_lvds_priv *)psb_intel_output->dev_priv;
+
+        dev_dbg(dev->dev, "(0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x)\n",
+                        lvds_priv->savePP_ON,
+                        lvds_priv->savePP_OFF,
+                        lvds_priv->saveLVDS,
+                        lvds_priv->savePP_CONTROL,
+                        lvds_priv->savePP_CYCLE,
+                        lvds_priv->saveBLC_PWM_CTL);
+
+        REG_WRITE(BLC_PWM_CTL, lvds_priv->saveBLC_PWM_CTL);
+        REG_WRITE(PFIT_CONTROL, lvds_priv->savePFIT_CONTROL);
+        REG_WRITE(PFIT_PGM_RATIOS, lvds_priv->savePFIT_PGM_RATIOS);
+        REG_WRITE(LVDSPP_ON, lvds_priv->savePP_ON);
+        REG_WRITE(LVDSPP_OFF, lvds_priv->savePP_OFF);
+        /*REG_WRITE(PP_DIVISOR, lvds_priv->savePP_DIVISOR);*/
+        REG_WRITE(PP_CYCLE, lvds_priv->savePP_CYCLE);
+        REG_WRITE(PP_CONTROL, lvds_priv->savePP_CONTROL);
+        REG_WRITE(LVDS, lvds_priv->saveLVDS);
+
+        if (lvds_priv->savePP_CONTROL & POWER_TARGET_ON) {
+                REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) |
+                        POWER_TARGET_ON);
+                do {
+                        pp_status = REG_READ(PP_STATUS);
+                } while ((pp_status & PP_ON) == 0);
+        } else {
+                REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) &
+                        ~POWER_TARGET_ON);
+                do {
+                        pp_status = REG_READ(PP_STATUS);
+                } while (pp_status & PP_ON);
+        }
+}
+
+int psb_intel_lvds_mode_valid(struct drm_connector *connector,
+                                 struct drm_display_mode *mode)
+{
+        struct psb_intel_output *psb_intel_output =
+                                to_psb_intel_output(connector);
+        struct drm_display_mode *fixed_mode =
+            psb_intel_output->mode_dev->panel_fixed_mode;
+
+        if (psb_intel_output->type == INTEL_OUTPUT_MIPI2)
+                fixed_mode = psb_intel_output->mode_dev->panel_fixed_mode2;
+
+        /* just in case */
+        if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+                return MODE_NO_DBLESCAN;
+
+        /* just in case */
+        if (mode->flags & DRM_MODE_FLAG_INTERLACE)
+                return MODE_NO_INTERLACE;
+
+        if (fixed_mode) {
+                if (mode->hdisplay > fixed_mode->hdisplay)
+                        return MODE_PANEL;
+                if (mode->vdisplay > fixed_mode->vdisplay)
+                        return MODE_PANEL;
+        }
+        return MODE_OK;
+}
+
+bool psb_intel_lvds_mode_fixup(struct drm_encoder *encoder,
+                                  struct drm_display_mode *mode,
+                                  struct drm_display_mode *adjusted_mode)
+{
+        struct psb_intel_mode_device *mode_dev =
+            enc_to_psb_intel_output(encoder)->mode_dev;
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_crtc *psb_intel_crtc =
+                                to_psb_intel_crtc(encoder->crtc);
+        struct drm_encoder *tmp_encoder;
+        struct drm_display_mode *panel_fixed_mode = mode_dev->panel_fixed_mode;
+        struct psb_intel_output *psb_intel_output =
+                                        enc_to_psb_intel_output(encoder);
+
+        if (psb_intel_output->type == INTEL_OUTPUT_MIPI2)
+                panel_fixed_mode = mode_dev->panel_fixed_mode2;
+
+        /* FIXME: review for Medfield */
+        /* PSB requires the LVDS is on pipe B, MRST has only one pipe anyway */
+        if (!IS_MRST(dev) && psb_intel_crtc->pipe == 0) {
+                printk(KERN_ERR "Can't support LVDS on pipe A\n");
+                return false;
+        }
+        if (IS_MRST(dev) && psb_intel_crtc->pipe != 0) {
+                printk(KERN_ERR "Must use PIPE A\n");
+                return false;
+        }
+        /* Should never happen!! */
+        list_for_each_entry(tmp_encoder, &dev->mode_config.encoder_list,
+                            head) {
+                if (tmp_encoder != encoder
+                    && tmp_encoder->crtc == encoder->crtc) {
+                        printk(KERN_ERR "Can't enable LVDS and another "
+                               "encoder on the same pipe\n");
+                        return false;
+                }
+        }
+
+        /*
+         * If we have timings from the BIOS for the panel, put them in
+         * to the adjusted mode.  The CRTC will be set up for this mode,
+         * with the panel scaling set up to source from the H/VDisplay
+         * of the original mode.
+         */
+        if (panel_fixed_mode != NULL) {
+                adjusted_mode->hdisplay = panel_fixed_mode->hdisplay;
+                adjusted_mode->hsync_start = panel_fixed_mode->hsync_start;
+                adjusted_mode->hsync_end = panel_fixed_mode->hsync_end;
+                adjusted_mode->htotal = panel_fixed_mode->htotal;
+                adjusted_mode->vdisplay = panel_fixed_mode->vdisplay;
+                adjusted_mode->vsync_start = panel_fixed_mode->vsync_start;
+                adjusted_mode->vsync_end = panel_fixed_mode->vsync_end;
+                adjusted_mode->vtotal = panel_fixed_mode->vtotal;
+                adjusted_mode->clock = panel_fixed_mode->clock;
+                drm_mode_set_crtcinfo(adjusted_mode,
+                                      CRTC_INTERLACE_HALVE_V);
+        }
+
+        /*
+         * XXX: It would be nice to support lower refresh rates on the
+         * panels to reduce power consumption, and perhaps match the
+         * user's requested refresh rate.
+         */
+
+        return true;
+}
+
+static void psb_intel_lvds_prepare(struct drm_encoder *encoder)
+{
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+        struct psb_intel_mode_device *mode_dev = output->mode_dev;
+
+        if (!gma_power_begin(dev, true))
+                return;
+
+        mode_dev->saveBLC_PWM_CTL = REG_READ(BLC_PWM_CTL);
+        mode_dev->backlight_duty_cycle = (mode_dev->saveBLC_PWM_CTL &
+                                          BACKLIGHT_DUTY_CYCLE_MASK);
+
+        psb_intel_lvds_set_power(dev, output, false);
+
+        gma_power_end(dev);
+}
+
+static void psb_intel_lvds_commit(struct drm_encoder *encoder)
+{
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_output *output = enc_to_psb_intel_output(encoder);
+        struct psb_intel_mode_device *mode_dev = output->mode_dev;
+
+        if (mode_dev->backlight_duty_cycle == 0)
+                mode_dev->backlight_duty_cycle =
+                    psb_intel_lvds_get_max_backlight(dev);
+
+        psb_intel_lvds_set_power(dev, output, true);
+}
+
+static void psb_intel_lvds_mode_set(struct drm_encoder *encoder,
+                                struct drm_display_mode *mode,
+                                struct drm_display_mode *adjusted_mode)
+{
+        struct drm_device *dev = encoder->dev;
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        u32 pfit_control;
+
+        /*
+         * The LVDS pin pair will already have been turned on in the
+         * psb_intel_crtc_mode_set since it has a large impact on the DPLL
+         * settings.
+         */
+
+        /*
+         * Enable automatic panel scaling so that non-native modes fill the
+         * screen.  Should be enabled before the pipe is enabled, according to
+         * register description and PRM.
+         */
+        if (mode->hdisplay != adjusted_mode->hdisplay ||
+            mode->vdisplay != adjusted_mode->vdisplay)
+                pfit_control = (PFIT_ENABLE | VERT_AUTO_SCALE |
+                                HORIZ_AUTO_SCALE | VERT_INTERP_BILINEAR |
+                                HORIZ_INTERP_BILINEAR);
+        else
+                pfit_control = 0;
+
+        if (dev_priv->lvds_dither)
+                pfit_control |= PANEL_8TO6_DITHER_ENABLE;
+
+        REG_WRITE(PFIT_CONTROL, pfit_control);
+}
+
+/*
+ * Detect the LVDS connection.
+ *
+ * This always returns CONNECTOR_STATUS_CONNECTED.
+ * This connector should only have
+ * been set up if the LVDS was actually connected anyway.
+ */
+static enum drm_connector_status psb_intel_lvds_detect(struct drm_connector
+                                                   *connector, bool force)
+{
+        return connector_status_connected;
+}
+
+/*
+ * Return the list of DDC modes if available, or the BIOS fixed mode otherwise.
+ */
+static int psb_intel_lvds_get_modes(struct drm_connector *connector)
+{
+        struct drm_device *dev = connector->dev;
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+        struct psb_intel_mode_device *mode_dev =
+                                        psb_intel_output->mode_dev;
+        int ret = 0;
+
+        if (!IS_MRST(dev))
+                ret = psb_intel_ddc_get_modes(psb_intel_output);
+
+        if (ret)
+                return ret;
+
+        /* Didn't get an EDID, so
+         * Set wide sync ranges so we get all modes
+         * handed to valid_mode for checking
+         */
+        connector->display_info.min_vfreq = 0;
+        connector->display_info.max_vfreq = 200;
+        connector->display_info.min_hfreq = 0;
+        connector->display_info.max_hfreq = 200;
+
+        if (mode_dev->panel_fixed_mode != NULL) {
+                struct drm_display_mode *mode =
+                    drm_mode_duplicate(dev, mode_dev->panel_fixed_mode);
+                drm_mode_probed_add(connector, mode);
+                return 1;
+        }
+
+        return 0;
+}
+
+/**
+ * psb_intel_lvds_destroy - unregister and free LVDS structures
+ * @connector: connector to free
+ *
+ * Unregister the DDC bus for this connector then free the driver private
+ * structure.
+ */
+void psb_intel_lvds_destroy(struct drm_connector *connector)
+{
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+
+        if (psb_intel_output->ddc_bus)
+                psb_intel_i2c_destroy(psb_intel_output->ddc_bus);
+        drm_sysfs_connector_remove(connector);
+        drm_connector_cleanup(connector);
+        kfree(connector);
+}
+
+int psb_intel_lvds_set_property(struct drm_connector *connector,
+                                       struct drm_property *property,
+                                       uint64_t value)
+{
+        struct drm_encoder *encoder = connector->encoder;
+
+        if (!encoder)
+                return -1;
+
+        if (!strcmp(property->name, "scaling mode")) {
+                struct psb_intel_crtc *crtc =
+                                        to_psb_intel_crtc(encoder->crtc);
+                uint64_t curval;
+
+                if (!crtc)
+                        goto set_prop_error;
+
+                switch (value) {
+                case DRM_MODE_SCALE_FULLSCREEN:
+                        break;
+                case DRM_MODE_SCALE_NO_SCALE:
+                        break;
+                case DRM_MODE_SCALE_ASPECT:
+                        break;
+                default:
+                        goto set_prop_error;
+                }
+
+                if (drm_connector_property_get_value(connector,
+                                                     property,
+                                                     &curval))
+                        goto set_prop_error;
+
+                if (curval == value)
+                        goto set_prop_done;
+
+                if (drm_connector_property_set_value(connector,
+                                                        property,
+                                                        value))
+                        goto set_prop_error;
+
+                if (crtc->saved_mode.hdisplay != 0 &&
+                    crtc->saved_mode.vdisplay != 0) {
+                        if (!drm_crtc_helper_set_mode(encoder->crtc,
+                                                      &crtc->saved_mode,
+                                                      encoder->crtc->x,
+                                                      encoder->crtc->y,
+                                                      encoder->crtc->fb))
+                                goto set_prop_error;
+                }
+        } else if (!strcmp(property->name, "backlight")) {
+                if (drm_connector_property_set_value(connector,
+                                                        property,
+                                                        value))
+                        goto set_prop_error;
+                else {
+#ifdef CONFIG_BACKLIGHT_CLASS_DEVICE
+                        struct drm_psb_private *devp = encoder->dev->dev_private;
+                        struct backlight_device *bd = devp->backlight_device;
+                        if (bd) {
+                                bd->props.brightness = value;
+                                backlight_update_status(bd);
+                        }
+#endif
+                }
+        } else if (!strcmp(property->name, "DPMS")) {
+                struct drm_encoder_helper_funcs *hfuncs
+                                                = encoder->helper_private;
+                hfuncs->dpms(encoder, value);
+        }
+
+set_prop_done:
+        return 0;
+set_prop_error:
+        return -1;
+}
+
+static const struct drm_encoder_helper_funcs psb_intel_lvds_helper_funcs = {
+        .dpms = psb_intel_lvds_encoder_dpms,
+        .mode_fixup = psb_intel_lvds_mode_fixup,
+        .prepare = psb_intel_lvds_prepare,
+        .mode_set = psb_intel_lvds_mode_set,
+        .commit = psb_intel_lvds_commit,
+};
+
+const struct drm_connector_helper_funcs
+                                psb_intel_lvds_connector_helper_funcs = {
+        .get_modes = psb_intel_lvds_get_modes,
+        .mode_valid = psb_intel_lvds_mode_valid,
+        .best_encoder = psb_intel_best_encoder,
+};
+
+const struct drm_connector_funcs psb_intel_lvds_connector_funcs = {
+        .dpms = drm_helper_connector_dpms,
+        .save = psb_intel_lvds_save,
+        .restore = psb_intel_lvds_restore,
+        .detect = psb_intel_lvds_detect,
+        .fill_modes = drm_helper_probe_single_connector_modes,
+        .set_property = psb_intel_lvds_set_property,
+        .destroy = psb_intel_lvds_destroy,
+};
+
+
+static void psb_intel_lvds_enc_destroy(struct drm_encoder *encoder)
+{
+        drm_encoder_cleanup(encoder);
+}
+
+const struct drm_encoder_funcs psb_intel_lvds_enc_funcs = {
+        .destroy = psb_intel_lvds_enc_destroy,
+};
+
+
+
+/**
+ * psb_intel_lvds_init - setup LVDS connectors on this device
+ * @dev: drm device
+ *
+ * Create the connector, register the LVDS DDC bus, and try to figure out what
+ * modes we can display on the LVDS panel (if present).
+ */
+void psb_intel_lvds_init(struct drm_device *dev,
+                     struct psb_intel_mode_device *mode_dev)
+{
+        struct psb_intel_output *psb_intel_output;
+        struct psb_intel_lvds_priv *lvds_priv;
+        struct drm_connector *connector;
+        struct drm_encoder *encoder;
+        struct drm_display_mode *scan;  /* *modes, *bios_mode; */
+        struct drm_crtc *crtc;
+        struct drm_psb_private *dev_priv =
+                                (struct drm_psb_private *)dev->dev_private;
+        u32 lvds;
+        int pipe;
+
+        psb_intel_output = kzalloc(sizeof(struct psb_intel_output), GFP_KERNEL);
+        if (!psb_intel_output)
+                return;
+
+        lvds_priv = kzalloc(sizeof(struct psb_intel_lvds_priv), GFP_KERNEL);
+        if (!lvds_priv) {
+                kfree(psb_intel_output);
+                dev_err(dev->dev, "LVDS private allocation error\n");
+                return;
+        }
+
+        psb_intel_output->dev_priv = lvds_priv;
+
+        psb_intel_output->mode_dev = mode_dev;
+        connector = &psb_intel_output->base;
+        encoder = &psb_intel_output->enc;
+        drm_connector_init(dev, &psb_intel_output->base,
+                           &psb_intel_lvds_connector_funcs,
+                           DRM_MODE_CONNECTOR_LVDS);
+
+        drm_encoder_init(dev, &psb_intel_output->enc,
+                         &psb_intel_lvds_enc_funcs,
+                         DRM_MODE_ENCODER_LVDS);
+
+        drm_mode_connector_attach_encoder(&psb_intel_output->base,
+                                          &psb_intel_output->enc);
+        psb_intel_output->type = INTEL_OUTPUT_LVDS;
+
+        drm_encoder_helper_add(encoder, &psb_intel_lvds_helper_funcs);
+        drm_connector_helper_add(connector,
+                                 &psb_intel_lvds_connector_helper_funcs);
+        connector->display_info.subpixel_order = SubPixelHorizontalRGB;
+        connector->interlace_allowed = false;
+        connector->doublescan_allowed = false;
+
+        /*Attach connector properties*/
+        drm_connector_attach_property(connector,
+                                      dev->mode_config.scaling_mode_property,
+                                      DRM_MODE_SCALE_FULLSCREEN);
+        drm_connector_attach_property(connector,
+                                      dev_priv->backlight_property,
+                                      BRIGHTNESS_MAX_LEVEL);
+
+        /*
+         * Set up I2C bus
+         * FIXME: distroy i2c_bus when exit
+         */
+        psb_intel_output->i2c_bus = psb_intel_i2c_create(dev,
+                                                         GPIOB,
+                                                         "LVDSBLC_B");
+        if (!psb_intel_output->i2c_bus) {
+                dev_printk(KERN_ERR,
+                        &dev->pdev->dev, "I2C bus registration failed.\n");
+                goto failed_blc_i2c;
+        }
+        psb_intel_output->i2c_bus->slave_addr = 0x2C;
+        dev_priv->lvds_i2c_bus =  psb_intel_output->i2c_bus;
+
+        /*
+         * LVDS discovery:
+         * 1) check for EDID on DDC
+         * 2) check for VBT data
+         * 3) check to see if LVDS is already on
+         *    if none of the above, no panel
+         * 4) make sure lid is open
+         *    if closed, act like it's not there for now
+         */
+
+        /* Set up the DDC bus. */
+        psb_intel_output->ddc_bus = psb_intel_i2c_create(dev,
+                                                         GPIOC,
+                                                         "LVDSDDC_C");
+        if (!psb_intel_output->ddc_bus) {
+                dev_printk(KERN_ERR, &dev->pdev->dev,
+                           "DDC bus registration " "failed.\n");
+                goto failed_ddc;
+        }
+
+        /*
+         * Attempt to get the fixed panel mode from DDC.  Assume that the
+         * preferred mode is the right one.
+         */
+        psb_intel_ddc_get_modes(psb_intel_output);
+        list_for_each_entry(scan, &connector->probed_modes, head) {
+                if (scan->type & DRM_MODE_TYPE_PREFERRED) {
+                        mode_dev->panel_fixed_mode =
+                            drm_mode_duplicate(dev, scan);
+                        goto out;       /* FIXME: check for quirks */
+                }
+        }
+
+        /* Failed to get EDID, what about VBT? do we need this? */
+        if (mode_dev->vbt_mode)
+                mode_dev->panel_fixed_mode =
+                    drm_mode_duplicate(dev, mode_dev->vbt_mode);
+
+        if (!mode_dev->panel_fixed_mode)
+                if (dev_priv->lfp_lvds_vbt_mode)
+                        mode_dev->panel_fixed_mode =
+                                drm_mode_duplicate(dev,
+                                        dev_priv->lfp_lvds_vbt_mode);
+
+        /*
+         * If we didn't get EDID, try checking if the panel is already turned
+         * on.  If so, assume that whatever is currently programmed is the
+         * correct mode.
+         */
+        lvds = REG_READ(LVDS);
+        pipe = (lvds & LVDS_PIPEB_SELECT) ? 1 : 0;
+        crtc = psb_intel_get_crtc_from_pipe(dev, pipe);
+
+        if (crtc && (lvds & LVDS_PORT_EN)) {
+                mode_dev->panel_fixed_mode =
+                    psb_intel_crtc_mode_get(dev, crtc);
+                if (mode_dev->panel_fixed_mode) {
+                        mode_dev->panel_fixed_mode->type |=
+                            DRM_MODE_TYPE_PREFERRED;
+                        goto out;       /* FIXME: check for quirks */
+                }
+        }
+
+        /* If we still don't have a mode after all that, give up. */
+        if (!mode_dev->panel_fixed_mode) {
+                dev_err(dev->dev, "Found no modes on the lvds, ignoring the LVDS\n");
+                goto failed_find;
+        }
+
+        /*
+         * Blacklist machines with BIOSes that list an LVDS panel without
+         * actually having one.
+         */
+out:
+        drm_sysfs_connector_add(connector);
+        return;
+
+failed_find:
+        if (psb_intel_output->ddc_bus)
+                psb_intel_i2c_destroy(psb_intel_output->ddc_bus);
+failed_ddc:
+        if (psb_intel_output->i2c_bus)
+                psb_intel_i2c_destroy(psb_intel_output->i2c_bus);
+failed_blc_i2c:
+        drm_encoder_cleanup(encoder);
+        drm_connector_cleanup(connector);
+        kfree(connector);
+}
+
diff --git a/drivers/staging/gma500/psb_intel_modes.c b/drivers/staging/gma500/psb_intel_modes.c
new file mode 100644
index 00000000000..bde1aff9619
--- /dev/null
+++ b/drivers/staging/gma500/psb_intel_modes.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2007 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authers: Jesse Barnes <jesse.barnes@intel.com>
+ */
+
+#include <linux/i2c.h>
+#include <linux/fb.h>
+#include <drm/drmP.h>
+#include "psb_intel_drv.h"
+
+/**
+ * psb_intel_ddc_probe
+ *
+ */
+bool psb_intel_ddc_probe(struct psb_intel_output *psb_intel_output)
+{
+        u8 out_buf[] = { 0x0, 0x0 };
+        u8 buf[2];
+        int ret;
+        struct i2c_msg msgs[] = {
+                {
+                 .addr = 0x50,
+                 .flags = 0,
+                 .len = 1,
+                 .buf = out_buf,
+                 },
+                {
+                 .addr = 0x50,
+                 .flags = I2C_M_RD,
+                 .len = 1,
+                 .buf = buf,
+                 }
+        };
+
+        ret = i2c_transfer(&psb_intel_output->ddc_bus->adapter, msgs, 2);
+        if (ret == 2)
+                return true;
+
+        return false;
+}
+
+/**
+ * psb_intel_ddc_get_modes - get modelist from monitor
+ * @connector: DRM connector device to use
+ *
+ * Fetch the EDID information from @connector using the DDC bus.
+ */
+int psb_intel_ddc_get_modes(struct psb_intel_output *psb_intel_output)
+{
+        struct edid *edid;
+        int ret = 0;
+
+        edid =
+            drm_get_edid(&psb_intel_output->base,
+                         &psb_intel_output->ddc_bus->adapter);
+        if (edid) {
+                drm_mode_connector_update_edid_property(&psb_intel_output->
+                                                        base, edid);
+                ret = drm_add_edid_modes(&psb_intel_output->base, edid);
+                kfree(edid);
+        }
+        return ret;
+}
diff --git a/drivers/staging/gma500/psb_intel_reg.h b/drivers/staging/gma500/psb_intel_reg.h
new file mode 100644
index 00000000000..1ac16aa791c
--- /dev/null
+++ b/drivers/staging/gma500/psb_intel_reg.h
@@ -0,0 +1,1235 @@
+/*
+ * Copyright (c) 2009, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ */
+#ifndef __PSB_INTEL_REG_H__
+#define __PSB_INTEL_REG_H__
+
+#define BLC_PWM_CTL             0x61254
+#define BLC_PWM_CTL2            0x61250
+#define BLC_PWM_CTL_C           0x62254
+#define BLC_PWM_CTL2_C          0x62250
+#define BACKLIGHT_MODULATION_FREQ_SHIFT         (17)
+/*
+ * This is the most significant 15 bits of the number of backlight cycles in a
+ * complete cycle of the modulated backlight control.
+ *
+ * The actual value is this field multiplied by two.
+ */
+#define BACKLIGHT_MODULATION_FREQ_MASK  (0x7fff << 17)
+#define BLM_LEGACY_MODE                 (1 << 16)
+/*
+ * This is the number of cycles out of the backlight modulation cycle for which
+ * the backlight is on.
+ *
+ * This field must be no greater than the number of cycles in the complete
+ * backlight modulation cycle.
+ */
+#define BACKLIGHT_DUTY_CYCLE_SHIFT      (0)
+#define BACKLIGHT_DUTY_CYCLE_MASK       (0xffff)
+
+#define I915_GCFGC                      0xf0
+#define I915_LOW_FREQUENCY_ENABLE       (1 << 7)
+#define I915_DISPLAY_CLOCK_190_200_MHZ  (0 << 4)
+#define I915_DISPLAY_CLOCK_333_MHZ      (4 << 4)
+#define I915_DISPLAY_CLOCK_MASK         (7 << 4)
+
+#define I855_HPLLCC                     0xc0
+#define I855_CLOCK_CONTROL_MASK         (3 << 0)
+#define I855_CLOCK_133_200              (0 << 0)
+#define I855_CLOCK_100_200              (1 << 0)
+#define I855_CLOCK_100_133              (2 << 0)
+#define I855_CLOCK_166_250              (3 << 0)
+
+/* I830 CRTC registers */
+#define HTOTAL_A                0x60000
+#define HBLANK_A                0x60004
+#define HSYNC_A                 0x60008
+#define VTOTAL_A                0x6000c
+#define VBLANK_A                0x60010
+#define VSYNC_A                 0x60014
+#define PIPEASRC                0x6001c
+#define BCLRPAT_A               0x60020
+#define VSYNCSHIFT_A            0x60028
+
+#define HTOTAL_B                0x61000
+#define HBLANK_B                0x61004
+#define HSYNC_B                 0x61008
+#define VTOTAL_B                0x6100c
+#define VBLANK_B                0x61010
+#define VSYNC_B                 0x61014
+#define PIPEBSRC                0x6101c
+#define BCLRPAT_B               0x61020
+#define VSYNCSHIFT_B            0x61028
+
+#define HTOTAL_C                0x62000
+#define HBLANK_C                0x62004
+#define HSYNC_C                 0x62008
+#define VTOTAL_C                0x6200c
+#define VBLANK_C                0x62010
+#define VSYNC_C                 0x62014
+#define PIPECSRC                0x6201c
+#define BCLRPAT_C               0x62020
+#define VSYNCSHIFT_C            0x62028
+
+#define PP_STATUS               0x61200
+# define PP_ON                          (1 << 31)
+/*
+ * Indicates that all dependencies of the panel are on:
+ *
+ * - PLL enabled
+ * - pipe enabled
+ * - LVDS/DVOB/DVOC on
+ */
+#define PP_READY                        (1 << 30)
+#define PP_SEQUENCE_NONE                (0 << 28)
+#define PP_SEQUENCE_ON                  (1 << 28)
+#define PP_SEQUENCE_OFF                 (2 << 28)
+#define PP_SEQUENCE_MASK                0x30000000
+#define PP_CONTROL              0x61204
+#define POWER_TARGET_ON                 (1 << 0)
+
+#define LVDSPP_ON               0x61208
+#define LVDSPP_OFF              0x6120c
+#define PP_CYCLE                0x61210
+
+#define PFIT_CONTROL            0x61230
+#define PFIT_ENABLE                     (1 << 31)
+#define PFIT_PIPE_MASK                  (3 << 29)
+#define PFIT_PIPE_SHIFT                 29
+#define PFIT_SCALING_MODE_PILLARBOX     (1 << 27)
+#define PFIT_SCALING_MODE_LETTERBOX     (3 << 26)
+#define VERT_INTERP_DISABLE             (0 << 10)
+#define VERT_INTERP_BILINEAR            (1 << 10)
+#define VERT_INTERP_MASK                (3 << 10)
+#define VERT_AUTO_SCALE                 (1 << 9)
+#define HORIZ_INTERP_DISABLE            (0 << 6)
+#define HORIZ_INTERP_BILINEAR           (1 << 6)
+#define HORIZ_INTERP_MASK               (3 << 6)
+#define HORIZ_AUTO_SCALE                (1 << 5)
+#define PANEL_8TO6_DITHER_ENABLE        (1 << 3)
+
+#define PFIT_PGM_RATIOS         0x61234
+#define PFIT_VERT_SCALE_MASK                    0xfff00000
+#define PFIT_HORIZ_SCALE_MASK                   0x0000fff0
+
+#define PFIT_AUTO_RATIOS        0x61238
+
+#define DPLL_A                  0x06014
+#define DPLL_B                  0x06018
+#define DPLL_VCO_ENABLE                 (1 << 31)
+#define DPLL_DVO_HIGH_SPEED             (1 << 30)
+#define DPLL_SYNCLOCK_ENABLE            (1 << 29)
+#define DPLL_VGA_MODE_DIS               (1 << 28)
+#define DPLLB_MODE_DAC_SERIAL           (1 << 26)       /* i915 */
+#define DPLLB_MODE_LVDS                 (2 << 26)       /* i915 */
+#define DPLL_MODE_MASK                  (3 << 26)
+#define DPLL_DAC_SERIAL_P2_CLOCK_DIV_10 (0 << 24)       /* i915 */
+#define DPLL_DAC_SERIAL_P2_CLOCK_DIV_5  (1 << 24)       /* i915 */
+#define DPLLB_LVDS_P2_CLOCK_DIV_14      (0 << 24)       /* i915 */
+#define DPLLB_LVDS_P2_CLOCK_DIV_7       (1 << 24)       /* i915 */
+#define DPLL_P2_CLOCK_DIV_MASK          0x03000000      /* i915 */
+#define DPLL_FPA01_P1_POST_DIV_MASK     0x00ff0000      /* i915 */
+#define DPLL_LOCK                       (1 << 15)       /* CDV */
+
+/*
+ *  The i830 generation, in DAC/serial mode, defines p1 as two plus this
+ * bitfield, or just 2 if PLL_P1_DIVIDE_BY_TWO is set.
+ */
+# define DPLL_FPA01_P1_POST_DIV_MASK_I830       0x001f0000
+/*
+ * The i830 generation, in LVDS mode, defines P1 as the bit number set within
+ * this field (only one bit may be set).
+ */
+#define DPLL_FPA01_P1_POST_DIV_MASK_I830_LVDS   0x003f0000
+#define DPLL_FPA01_P1_POST_DIV_SHIFT    16
+#define PLL_P2_DIVIDE_BY_4              (1 << 23)       /* i830, required
+                                                         * in DVO non-gang */
+# define PLL_P1_DIVIDE_BY_TWO           (1 << 21)       /* i830 */
+#define PLL_REF_INPUT_DREFCLK           (0 << 13)
+#define PLL_REF_INPUT_TVCLKINA          (1 << 13)       /* i830 */
+#define PLL_REF_INPUT_TVCLKINBC         (2 << 13)       /* SDVO
+                                                                 * TVCLKIN */
+#define PLLB_REF_INPUT_SPREADSPECTRUMIN (3 << 13)
+#define PLL_REF_INPUT_MASK              (3 << 13)
+#define PLL_LOAD_PULSE_PHASE_SHIFT      9
+/*
+ * Parallel to Serial Load Pulse phase selection.
+ * Selects the phase for the 10X DPLL clock for the PCIe
+ * digital display port. The range is 4 to 13; 10 or more
+ * is just a flip delay. The default is 6
+ */
+#define PLL_LOAD_PULSE_PHASE_MASK       (0xf << PLL_LOAD_PULSE_PHASE_SHIFT)
+#define DISPLAY_RATE_SELECT_FPA1        (1 << 8)
+
+/*
+ * SDVO multiplier for 945G/GM. Not used on 965.
+ *
+ * DPLL_MD_UDI_MULTIPLIER_MASK
+ */
+#define SDVO_MULTIPLIER_MASK            0x000000ff
+#define SDVO_MULTIPLIER_SHIFT_HIRES     4
+#define SDVO_MULTIPLIER_SHIFT_VGA       0
+
+/*
+ * PLL_MD
+ */
+/* Pipe A SDVO/UDI clock multiplier/divider register for G965. */
+#define DPLL_A_MD               0x0601c
+/* Pipe B SDVO/UDI clock multiplier/divider register for G965. */
+#define DPLL_B_MD               0x06020
+/*
+ * UDI pixel divider, controlling how many pixels are stuffed into a packet.
+ *
+ * Value is pixels minus 1.  Must be set to 1 pixel for SDVO.
+ */
+#define DPLL_MD_UDI_DIVIDER_MASK        0x3f000000
+#define DPLL_MD_UDI_DIVIDER_SHIFT       24
+/* UDI pixel divider for VGA, same as DPLL_MD_UDI_DIVIDER_MASK. */
+#define DPLL_MD_VGA_UDI_DIVIDER_MASK    0x003f0000
+#define DPLL_MD_VGA_UDI_DIVIDER_SHIFT   16
+/*
+ * SDVO/UDI pixel multiplier.
+ *
+ * SDVO requires that the bus clock rate be between 1 and 2 Ghz, and the bus
+ * clock rate is 10 times the DPLL clock.  At low resolution/refresh rate
+ * modes, the bus rate would be below the limits, so SDVO allows for stuffing
+ * dummy bytes in the datastream at an increased clock rate, with both sides of
+ * the link knowing how many bytes are fill.
+ *
+ * So, for a mode with a dotclock of 65Mhz, we would want to double the clock
+ * rate to 130Mhz to get a bus rate of 1.30Ghz.  The DPLL clock rate would be
+ * set to 130Mhz, and the SDVO multiplier set to 2x in this register and
+ * through an SDVO command.
+ *
+ * This register field has values of multiplication factor minus 1, with
+ * a maximum multiplier of 5 for SDVO.
+ */
+#define DPLL_MD_UDI_MULTIPLIER_MASK     0x00003f00
+#define DPLL_MD_UDI_MULTIPLIER_SHIFT    8
+/*
+ * SDVO/UDI pixel multiplier for VGA, same as DPLL_MD_UDI_MULTIPLIER_MASK.
+ * This best be set to the default value (3) or the CRT won't work. No,
+ * I don't entirely understand what this does...
+ */
+#define DPLL_MD_VGA_UDI_MULTIPLIER_MASK 0x0000003f
+#define DPLL_MD_VGA_UDI_MULTIPLIER_SHIFT 0
+
+#define DPLL_TEST               0x606c
+#define DPLLB_TEST_SDVO_DIV_1           (0 << 22)
+#define DPLLB_TEST_SDVO_DIV_2           (1 << 22)
+#define DPLLB_TEST_SDVO_DIV_4           (2 << 22)
+#define DPLLB_TEST_SDVO_DIV_MASK        (3 << 22)
+#define DPLLB_TEST_N_BYPASS             (1 << 19)
+#define DPLLB_TEST_M_BYPASS             (1 << 18)
+#define DPLLB_INPUT_BUFFER_ENABLE       (1 << 16)
+#define DPLLA_TEST_N_BYPASS             (1 << 3)
+#define DPLLA_TEST_M_BYPASS             (1 << 2)
+#define DPLLA_INPUT_BUFFER_ENABLE       (1 << 0)
+
+#define ADPA                    0x61100
+#define ADPA_DAC_ENABLE                 (1 << 31)
+#define ADPA_DAC_DISABLE                0
+#define ADPA_PIPE_SELECT_MASK           (1 << 30)
+#define ADPA_PIPE_A_SELECT              0
+#define ADPA_PIPE_B_SELECT              (1 << 30)
+#define ADPA_USE_VGA_HVPOLARITY         (1 << 15)
+#define ADPA_SETS_HVPOLARITY            0
+#define ADPA_VSYNC_CNTL_DISABLE         (1 << 11)
+#define ADPA_VSYNC_CNTL_ENABLE          0
+#define ADPA_HSYNC_CNTL_DISABLE         (1 << 10)
+#define ADPA_HSYNC_CNTL_ENABLE          0
+#define ADPA_VSYNC_ACTIVE_HIGH          (1 << 4)
+#define ADPA_VSYNC_ACTIVE_LOW           0
+#define ADPA_HSYNC_ACTIVE_HIGH          (1 << 3)
+#define ADPA_HSYNC_ACTIVE_LOW           0
+
+#define FPA0                    0x06040
+#define FPA1                    0x06044
+#define FPB0                    0x06048
+#define FPB1                    0x0604c
+#define FP_N_DIV_MASK                   0x003f0000
+#define FP_N_DIV_SHIFT                  16
+#define FP_M1_DIV_MASK                  0x00003f00
+#define FP_M1_DIV_SHIFT                 8
+#define FP_M2_DIV_MASK                  0x0000003f
+#define FP_M2_DIV_SHIFT                 0
+
+#define PORT_HOTPLUG_EN         0x61110
+#define SDVOB_HOTPLUG_INT_EN            (1 << 26)
+#define SDVOC_HOTPLUG_INT_EN            (1 << 25)
+#define TV_HOTPLUG_INT_EN               (1 << 18)
+#define CRT_HOTPLUG_INT_EN              (1 << 9)
+#define CRT_HOTPLUG_FORCE_DETECT        (1 << 3)
+/* CDV.. */
+#define CRT_HOTPLUG_ACTIVATION_PERIOD_64        (1 << 8)
+#define CRT_HOTPLUG_DAC_ON_TIME_2M              (0 << 7)
+#define CRT_HOTPLUG_DAC_ON_TIME_4M              (1 << 7)
+#define CRT_HOTPLUG_VOLTAGE_COMPARE_40          (0 << 5)
+#define CRT_HOTPLUG_VOLTAGE_COMPARE_50          (1 << 5)
+#define CRT_HOTPLUG_VOLTAGE_COMPARE_60          (2 << 5)
+#define CRT_HOTPLUG_VOLTAGE_COMPARE_70          (3 << 5)
+#define CRT_HOTPLUG_VOLTAGE_COMPARE_MASK        (3 << 5)
+#define CRT_HOTPLUG_DETECT_DELAY_1G             (0 << 4)
+#define CRT_HOTPLUG_DETECT_DELAY_2G             (1 << 4)
+#define CRT_HOTPLUG_DETECT_VOLTAGE_325MV        (0 << 2)
+#define CRT_HOTPLUG_DETECT_VOLTAGE_475MV        (1 << 2)
+#define CRT_HOTPLUG_DETECT_MASK                 0x000000F8
+
+#define PORT_HOTPLUG_STAT       0x61114
+#define CRT_HOTPLUG_INT_STATUS          (1 << 11)
+#define TV_HOTPLUG_INT_STATUS           (1 << 10)
+#define CRT_HOTPLUG_MONITOR_MASK        (3 << 8)
+#define CRT_HOTPLUG_MONITOR_COLOR       (3 << 8)
+#define CRT_HOTPLUG_MONITOR_MONO        (2 << 8)
+#define CRT_HOTPLUG_MONITOR_NONE        (0 << 8)
+#define SDVOC_HOTPLUG_INT_STATUS        (1 << 7)
+#define SDVOB_HOTPLUG_INT_STATUS        (1 << 6)
+
+#define SDVOB                   0x61140
+#define SDVOC                   0x61160
+#define SDVO_ENABLE                     (1 << 31)
+#define SDVO_PIPE_B_SELECT              (1 << 30)
+#define SDVO_STALL_SELECT               (1 << 29)
+#define SDVO_INTERRUPT_ENABLE           (1 << 26)
+
+/**
+ * 915G/GM SDVO pixel multiplier.
+ *
+ * Programmed value is multiplier - 1, up to 5x.
+ *
+ * DPLL_MD_UDI_MULTIPLIER_MASK
+ */
+#define SDVO_PORT_MULTIPLY_MASK         (7 << 23)
+#define SDVO_PORT_MULTIPLY_SHIFT        23
+#define SDVO_PHASE_SELECT_MASK          (15 << 19)
+#define SDVO_PHASE_SELECT_DEFAULT       (6 << 19)
+#define SDVO_CLOCK_OUTPUT_INVERT        (1 << 18)
+#define SDVOC_GANG_MODE                 (1 << 16)
+#define SDVO_BORDER_ENABLE              (1 << 7)
+#define SDVOB_PCIE_CONCURRENCY          (1 << 3)
+#define SDVO_DETECTED                   (1 << 2)
+/* Bits to be preserved when writing */
+#define SDVOB_PRESERVE_MASK             ((1 << 17) | (1 << 16) | (1 << 14))
+#define SDVOC_PRESERVE_MASK             (1 << 17)
+
+/*
+ * This register controls the LVDS output enable, pipe selection, and data
+ * format selection.
+ *
+ * All of the clock/data pairs are force powered down by power sequencing.
+ */
+#define LVDS                    0x61180
+/*
+ * Enables the LVDS port.  This bit must be set before DPLLs are enabled, as
+ * the DPLL semantics change when the LVDS is assigned to that pipe.
+ */
+#define LVDS_PORT_EN                    (1 << 31)
+/* Selects pipe B for LVDS data.  Must be set on pre-965. */
+#define LVDS_PIPEB_SELECT               (1 << 30)
+
+/* Turns on border drawing to allow centered display. */
+#define LVDS_BORDER_EN                  (1 << 15)
+
+/*
+ * Enables the A0-A2 data pairs and CLKA, containing 18 bits of color data per
+ * pixel.
+ */
+#define LVDS_A0A2_CLKA_POWER_MASK       (3 << 8)
+#define LVDS_A0A2_CLKA_POWER_DOWN       (0 << 8)
+#define LVDS_A0A2_CLKA_POWER_UP         (3 << 8)
+/*
+ * Controls the A3 data pair, which contains the additional LSBs for 24 bit
+ * mode.  Only enabled if LVDS_A0A2_CLKA_POWER_UP also indicates it should be
+ * on.
+ */
+#define LVDS_A3_POWER_MASK              (3 << 6)
+#define LVDS_A3_POWER_DOWN              (0 << 6)
+#define LVDS_A3_POWER_UP                (3 << 6)
+/*
+ * Controls the CLKB pair.  This should only be set when LVDS_B0B3_POWER_UP
+ * is set.
+ */
+#define LVDS_CLKB_POWER_MASK            (3 << 4)
+#define LVDS_CLKB_POWER_DOWN            (0 << 4)
+#define LVDS_CLKB_POWER_UP              (3 << 4)
+/*
+ * Controls the B0-B3 data pairs.  This must be set to match the DPLL p2
+ * setting for whether we are in dual-channel mode.  The B3 pair will
+ * additionally only be powered up when LVDS_A3_POWER_UP is set.
+ */
+#define LVDS_B0B3_POWER_MASK            (3 << 2)
+#define LVDS_B0B3_POWER_DOWN            (0 << 2)
+#define LVDS_B0B3_POWER_UP              (3 << 2)
+
+#define PIPEACONF               0x70008
+#define PIPEACONF_ENABLE                (1 << 31)
+#define PIPEACONF_DISABLE               0
+#define PIPEACONF_DOUBLE_WIDE           (1 << 30)
+#define PIPECONF_ACTIVE                 (1 << 30)
+#define I965_PIPECONF_ACTIVE            (1 << 30)
+#define PIPECONF_DSIPLL_LOCK            (1 << 29)
+#define PIPEACONF_SINGLE_WIDE           0
+#define PIPEACONF_PIPE_UNLOCKED         0
+#define PIPEACONF_DSR                   (1 << 26)
+#define PIPEACONF_PIPE_LOCKED           (1 << 25)
+#define PIPEACONF_PALETTE               0
+#define PIPECONF_FORCE_BORDER           (1 << 25)
+#define PIPEACONF_GAMMA                 (1 << 24)
+#define PIPECONF_PROGRESSIVE            (0 << 21)
+#define PIPECONF_INTERLACE_W_FIELD_INDICATION   (6 << 21)
+#define PIPECONF_INTERLACE_FIELD_0_ONLY         (7 << 21)
+#define PIPECONF_PLANE_OFF              (1 << 19)
+#define PIPECONF_CURSOR_OFF             (1 << 18)
+
+#define PIPEBCONF               0x71008
+#define PIPEBCONF_ENABLE                (1 << 31)
+#define PIPEBCONF_DISABLE               0
+#define PIPEBCONF_DOUBLE_WIDE           (1 << 30)
+#define PIPEBCONF_DISABLE               0
+#define PIPEBCONF_GAMMA                 (1 << 24)
+#define PIPEBCONF_PALETTE               0
+
+#define PIPECCONF               0x72008
+
+#define PIPEBGCMAXRED           0x71010
+#define PIPEBGCMAXGREEN         0x71014
+#define PIPEBGCMAXBLUE          0x71018
+
+#define PIPEASTAT               0x70024
+#define PIPEBSTAT               0x71024
+#define PIPECSTAT               0x72024
+#define PIPE_VBLANK_INTERRUPT_STATUS            (1UL << 1)
+#define PIPE_START_VBLANK_INTERRUPT_STATUS      (1UL << 2)
+#define PIPE_VBLANK_CLEAR                       (1 << 1)
+#define PIPE_VBLANK_STATUS                      (1 << 1)
+#define PIPE_TE_STATUS                          (1UL << 6)
+#define PIPE_DPST_EVENT_STATUS                  (1UL << 7)
+#define PIPE_VSYNC_CLEAR                        (1UL << 9)
+#define PIPE_VSYNC_STATUS                       (1UL << 9)
+#define PIPE_HDMI_AUDIO_UNDERRUN_STATUS         (1UL << 10)
+#define PIPE_HDMI_AUDIO_BUFFER_DONE_STATUS      (1UL << 11)
+#define PIPE_VBLANK_INTERRUPT_ENABLE            (1UL << 17)
+#define PIPE_START_VBLANK_INTERRUPT_ENABLE      (1UL << 18)
+#define PIPE_TE_ENABLE                          (1UL << 22)
+#define PIPE_DPST_EVENT_ENABLE                  (1UL << 23)
+#define PIPE_VSYNC_ENABL                        (1UL << 25)
+#define PIPE_HDMI_AUDIO_UNDERRUN                (1UL << 26)
+#define PIPE_HDMI_AUDIO_BUFFER_DONE             (1UL << 27)
+#define PIPE_HDMI_AUDIO_INT_MASK                (PIPE_HDMI_AUDIO_UNDERRUN | \
+                                                PIPE_HDMI_AUDIO_BUFFER_DONE)
+#define PIPE_EVENT_MASK ((1 << 29)|(1 << 28)|(1 << 27)|(1 << 26)|(1 << 24)|(1 << 23)|(1 << 22)|(1 << 21)|(1 << 20)|(1 << 16))
+#define PIPE_VBLANK_MASK ((1 << 25)|(1 << 24)|(1 << 18)|(1 << 17))
+#define HISTOGRAM_INT_CONTROL           0x61268
+#define HISTOGRAM_BIN_DATA              0X61264
+#define HISTOGRAM_LOGIC_CONTROL         0x61260
+#define PWM_CONTROL_LOGIC               0x61250
+#define PIPE_HOTPLUG_INTERRUPT_STATUS           (1UL << 10)
+#define HISTOGRAM_INTERRUPT_ENABLE              (1UL << 31)
+#define HISTOGRAM_LOGIC_ENABLE                  (1UL << 31)
+#define PWM_LOGIC_ENABLE                        (1UL << 31)
+#define PWM_PHASEIN_ENABLE                      (1UL << 25)
+#define PWM_PHASEIN_INT_ENABLE                  (1UL << 24)
+#define PWM_PHASEIN_VB_COUNT                    0x00001f00
+#define PWM_PHASEIN_INC                         0x0000001f
+#define HISTOGRAM_INT_CTRL_CLEAR                (1UL << 30)
+#define DPST_YUV_LUMA_MODE                      0
+
+struct dpst_ie_histogram_control {
+        union {
+                uint32_t data;
+                struct {
+                        uint32_t bin_reg_index:7;
+                        uint32_t reserved:4;
+                        uint32_t bin_reg_func_select:1;
+                        uint32_t sync_to_phase_in:1;
+                        uint32_t alt_enhancement_mode:2;
+                        uint32_t reserved1:1;
+                        uint32_t sync_to_phase_in_count:8;
+                        uint32_t histogram_mode_select:1;
+                        uint32_t reserved2:4;
+                        uint32_t ie_pipe_assignment:1;
+                        uint32_t ie_mode_table_enabled:1;
+                        uint32_t ie_histogram_enable:1;
+                };
+        };
+};
+
+struct dpst_guardband {
+        union {
+                uint32_t data;
+                struct {
+                        uint32_t guardband:22;
+                        uint32_t guardband_interrupt_delay:8;
+                        uint32_t interrupt_status:1;
+                        uint32_t interrupt_enable:1;
+                };
+        };
+};
+
+#define PIPEAFRAMEHIGH          0x70040
+#define PIPEAFRAMEPIXEL         0x70044
+#define PIPEBFRAMEHIGH          0x71040
+#define PIPEBFRAMEPIXEL         0x71044
+#define PIPECFRAMEHIGH          0x72040
+#define PIPECFRAMEPIXEL         0x72044
+#define PIPE_FRAME_HIGH_MASK    0x0000ffff
+#define PIPE_FRAME_HIGH_SHIFT   0
+#define PIPE_FRAME_LOW_MASK     0xff000000
+#define PIPE_FRAME_LOW_SHIFT    24
+#define PIPE_PIXEL_MASK         0x00ffffff
+#define PIPE_PIXEL_SHIFT        0
+
+#define DSPARB                  0x70030
+#define DSPFW1                  0x70034
+#define DSPFW2                  0x70038
+#define DSPFW3                  0x7003c
+#define DSPFW4                  0x70050
+#define DSPFW5                  0x70054
+#define DSPFW6                  0x70058
+#define DSPCHICKENBIT           0x70400
+#define DSPACNTR                0x70180
+#define DSPBCNTR                0x71180
+#define DSPCCNTR                0x72180
+#define DISPLAY_PLANE_ENABLE                    (1 << 31)
+#define DISPLAY_PLANE_DISABLE                   0
+#define DISPPLANE_GAMMA_ENABLE                  (1 << 30)
+#define DISPPLANE_GAMMA_DISABLE                 0
+#define DISPPLANE_PIXFORMAT_MASK                (0xf << 26)
+#define DISPPLANE_8BPP                          (0x2 << 26)
+#define DISPPLANE_15_16BPP                      (0x4 << 26)
+#define DISPPLANE_16BPP                         (0x5 << 26)
+#define DISPPLANE_32BPP_NO_ALPHA                (0x6 << 26)
+#define DISPPLANE_32BPP                         (0x7 << 26)
+#define DISPPLANE_STEREO_ENABLE                 (1 << 25)
+#define DISPPLANE_STEREO_DISABLE                0
+#define DISPPLANE_SEL_PIPE_MASK                 (1 << 24)
+#define DISPPLANE_SEL_PIPE_POS                  24
+#define DISPPLANE_SEL_PIPE_A                    0
+#define DISPPLANE_SEL_PIPE_B                    (1 << 24)
+#define DISPPLANE_SRC_KEY_ENABLE                (1 << 22)
+#define DISPPLANE_SRC_KEY_DISABLE               0
+#define DISPPLANE_LINE_DOUBLE                   (1 << 20)
+#define DISPPLANE_NO_LINE_DOUBLE                0
+#define DISPPLANE_STEREO_POLARITY_FIRST         0
+#define DISPPLANE_STEREO_POLARITY_SECOND        (1 << 18)
+/* plane B only */
+#define DISPPLANE_ALPHA_TRANS_ENABLE            (1 << 15)
+#define DISPPLANE_ALPHA_TRANS_DISABLE           0
+#define DISPPLANE_SPRITE_ABOVE_DISPLAYA         0
+#define DISPPLANE_SPRITE_ABOVE_OVERLAY          (1)
+#define DISPPLANE_BOTTOM                        (4)
+
+#define DSPABASE                0x70184
+#define DSPALINOFF              0x70184
+#define DSPASTRIDE              0x70188
+
+#define DSPBBASE                0x71184
+#define DSPBLINOFF              0X71184
+#define DSPBADDR                DSPBBASE
+#define DSPBSTRIDE              0x71188
+
+#define DSPCBASE                0x72184
+#define DSPCLINOFF              0x72184
+#define DSPCSTRIDE              0x72188
+
+#define DSPAKEYVAL              0x70194
+#define DSPAKEYMASK             0x70198
+
+#define DSPAPOS                 0x7018C /* reserved */
+#define DSPASIZE                0x70190
+#define DSPBPOS                 0x7118C
+#define DSPBSIZE                0x71190
+#define DSPCPOS                 0x7218C
+#define DSPCSIZE                0x72190
+
+#define DSPASURF                0x7019C
+#define DSPATILEOFF             0x701A4
+
+#define DSPBSURF                0x7119C
+#define DSPBTILEOFF             0x711A4
+
+#define DSPCSURF                0x7219C
+#define DSPCTILEOFF             0x721A4
+#define DSPCKEYMAXVAL           0x721A0
+#define DSPCKEYMINVAL           0x72194
+#define DSPCKEYMSK              0x72198
+
+#define VGACNTRL                0x71400
+#define VGA_DISP_DISABLE                (1 << 31)
+#define VGA_2X_MODE                     (1 << 30)
+#define VGA_PIPE_B_SELECT               (1 << 29)
+
+/*
+ * Overlay registers
+ */
+#define OV_C_OFFSET             0x08000
+#define OV_OVADD                0x30000
+#define OV_DOVASTA              0x30008
+# define OV_PIPE_SELECT                 ((1 << 6)|(1 << 7))
+# define OV_PIPE_SELECT_POS             6
+# define OV_PIPE_A                      0
+# define OV_PIPE_C                      1
+#define OV_OGAMC5               0x30010
+#define OV_OGAMC4               0x30014
+#define OV_OGAMC3               0x30018
+#define OV_OGAMC2               0x3001C
+#define OV_OGAMC1               0x30020
+#define OV_OGAMC0               0x30024
+#define OVC_OVADD               0x38000
+#define OVC_DOVCSTA             0x38008
+#define OVC_OGAMC5              0x38010
+#define OVC_OGAMC4              0x38014
+#define OVC_OGAMC3              0x38018
+#define OVC_OGAMC2              0x3801C
+#define OVC_OGAMC1              0x38020
+#define OVC_OGAMC0              0x38024
+
+/*
+ * Some BIOS scratch area registers.  The 845 (and 830?) store the amount
+ * of video memory available to the BIOS in SWF1.
+ */
+#define SWF0                    0x71410
+#define SWF1                    0x71414
+#define SWF2                    0x71418
+#define SWF3                    0x7141c
+#define SWF4                    0x71420
+#define SWF5                    0x71424
+#define SWF6                    0x71428
+
+/*
+ * 855 scratch registers.
+ */
+#define SWF00                   0x70410
+#define SWF01                   0x70414
+#define SWF02                   0x70418
+#define SWF03                   0x7041c
+#define SWF04                   0x70420
+#define SWF05                   0x70424
+#define SWF06                   0x70428
+
+#define SWF10                   SWF0
+#define SWF11                   SWF1
+#define SWF12                   SWF2
+#define SWF13                   SWF3
+#define SWF14                   SWF4
+#define SWF15                   SWF5
+#define SWF16                   SWF6
+
+#define SWF30                   0x72414
+#define SWF31                   0x72418
+#define SWF32                   0x7241c
+
+
+/*
+ * Palette registers
+ */
+#define PALETTE_A               0x0a000
+#define PALETTE_B               0x0a800
+#define PALETTE_C               0x0ac00
+
+/* Cursor A & B regs */
+#define CURACNTR                0x70080
+#define CURSOR_MODE_DISABLE             0x00
+#define CURSOR_MODE_64_32B_AX           0x07
+#define CURSOR_MODE_64_ARGB_AX          ((1 << 5) | CURSOR_MODE_64_32B_AX)
+#define MCURSOR_GAMMA_ENABLE            (1 << 26)
+#define CURABASE                0x70084
+#define CURAPOS                 0x70088
+#define CURSOR_POS_MASK                 0x007FF
+#define CURSOR_POS_SIGN                 0x8000
+#define CURSOR_X_SHIFT                  0
+#define CURSOR_Y_SHIFT                  16
+#define CURBCNTR                0x700c0
+#define CURBBASE                0x700c4
+#define CURBPOS                 0x700c8
+#define CURCCNTR                0x700e0
+#define CURCBASE                0x700e4
+#define CURCPOS                 0x700e8
+
+/*
+ * Interrupt Registers
+ */
+#define IER                     0x020a0
+#define IIR                     0x020a4
+#define IMR                     0x020a8
+#define ISR                     0x020ac
+
+/*
+ * MOORESTOWN delta registers
+ */
+#define MRST_DPLL_A             0x0f014
+#define MDFLD_DPLL_B            0x0f018
+#define MDFLD_INPUT_REF_SEL             (1 << 14)
+#define MDFLD_VCO_SEL                   (1 << 16)
+#define DPLLA_MODE_LVDS                 (2 << 26)       /* mrst */
+#define MDFLD_PLL_LATCHEN               (1 << 28)
+#define MDFLD_PWR_GATE_EN               (1 << 30)
+#define MDFLD_P1_MASK                   (0x1FF << 17)
+#define MRST_FPA0               0x0f040
+#define MRST_FPA1               0x0f044
+#define MDFLD_DPLL_DIV0         0x0f048
+#define MDFLD_DPLL_DIV1         0x0f04c
+#define MRST_PERF_MODE          0x020f4
+
+/*
+ * MEDFIELD HDMI registers
+ */
+#define HDMIPHYMISCCTL          0x61134
+#define HDMI_PHY_POWER_DOWN             0x7f
+#define HDMIB_CONTROL           0x61140
+#define HDMIB_PORT_EN                   (1 << 31)
+#define HDMIB_PIPE_B_SELECT             (1 << 30)
+#define HDMIB_NULL_PACKET               (1 << 9)
+#define HDMIB_HDCP_PORT                 (1 << 5)
+
+/* #define LVDS                 0x61180 */
+#define MRST_PANEL_8TO6_DITHER_ENABLE   (1 << 25)
+#define MRST_PANEL_24_DOT_1_FORMAT      (1 << 24)
+#define LVDS_A3_POWER_UP_0_OUTPUT       (1 << 6)
+
+#define MIPI                    0x61190
+#define MIPI_C                  0x62190
+#define MIPI_PORT_EN                    (1 << 31)
+/* Turns on border drawing to allow centered display. */
+#define SEL_FLOPPED_HSTX                (1 << 23)
+#define PASS_FROM_SPHY_TO_AFE           (1 << 16)
+#define MIPI_BORDER_EN                  (1 << 15)
+#define MIPIA_3LANE_MIPIC_1LANE         0x1
+#define MIPIA_2LANE_MIPIC_2LANE         0x2
+#define TE_TRIGGER_DSI_PROTOCOL         (1 << 2)
+#define TE_TRIGGER_GPIO_PIN             (1 << 3)
+#define MIPI_TE_COUNT           0x61194
+
+/* #define PP_CONTROL   0x61204 */
+#define POWER_DOWN_ON_RESET             (1 << 1)
+
+/* #define PFIT_CONTROL 0x61230 */
+#define PFIT_PIPE_SELECT                (3 << 29)
+#define PFIT_PIPE_SELECT_SHIFT          (29)
+
+/* #define BLC_PWM_CTL          0x61254 */
+#define MRST_BACKLIGHT_MODULATION_FREQ_SHIFT    (16)
+#define MRST_BACKLIGHT_MODULATION_FREQ_MASK     (0xffff << 16)
+
+/* #define PIPEACONF 0x70008 */
+#define PIPEACONF_PIPE_STATE            (1 << 30)
+/* #define DSPACNTR             0x70180 */
+
+#define MRST_DSPABASE           0x7019c
+#define MRST_DSPBBASE           0x7119c
+#define MDFLD_DSPCBASE          0x7219c
+
+/*
+ * Moorestown registers.
+ */
+
+/*
+ *      MIPI IP registers
+ */
+#define MIPIC_REG_OFFSET                0x800
+
+#define DEVICE_READY_REG                0xb000
+#define LP_OUTPUT_HOLD                          (1 << 16)
+#define EXIT_ULPS_DEV_READY                     0x3
+#define LP_OUTPUT_HOLD_RELEASE                  0x810000
+# define ENTERING_ULPS                          (2 << 1)
+# define EXITING_ULPS                           (1 << 1)
+# define ULPS_MASK                              (3 << 1)
+# define BUS_POSSESSION                         (1 << 3)
+#define INTR_STAT_REG                   0xb004
+#define RX_SOT_ERROR                            (1 << 0)
+#define RX_SOT_SYNC_ERROR                       (1 << 1)
+#define RX_ESCAPE_MODE_ENTRY_ERROR              (1 << 3)
+#define RX_LP_TX_SYNC_ERROR                     (1 << 4)
+#define RX_HS_RECEIVE_TIMEOUT_ERROR             (1 << 5)
+#define RX_FALSE_CONTROL_ERROR                  (1 << 6)
+#define RX_ECC_SINGLE_BIT_ERROR                 (1 << 7)
+#define RX_ECC_MULTI_BIT_ERROR                  (1 << 8)
+#define RX_CHECKSUM_ERROR                       (1 << 9)
+#define RX_DSI_DATA_TYPE_NOT_RECOGNIZED         (1 << 10)
+#define RX_DSI_VC_ID_INVALID                    (1 << 11)
+#define TX_FALSE_CONTROL_ERROR                  (1 << 12)
+#define TX_ECC_SINGLE_BIT_ERROR                 (1 << 13)
+#define TX_ECC_MULTI_BIT_ERROR                  (1 << 14)
+#define TX_CHECKSUM_ERROR                       (1 << 15)
+#define TX_DSI_DATA_TYPE_NOT_RECOGNIZED         (1 << 16)
+#define TX_DSI_VC_ID_INVALID                    (1 << 17)
+#define HIGH_CONTENTION                         (1 << 18)
+#define LOW_CONTENTION                          (1 << 19)
+#define DPI_FIFO_UNDER_RUN                      (1 << 20)
+#define HS_TX_TIMEOUT                           (1 << 21)
+#define LP_RX_TIMEOUT                           (1 << 22)
+#define TURN_AROUND_ACK_TIMEOUT                 (1 << 23)
+#define ACK_WITH_NO_ERROR                       (1 << 24)
+#define HS_GENERIC_WR_FIFO_FULL                 (1 << 27)
+#define LP_GENERIC_WR_FIFO_FULL                 (1 << 28)
+#define SPL_PKT_SENT                            (1 << 30)
+#define INTR_EN_REG                     0xb008
+#define DSI_FUNC_PRG_REG                0xb00c
+#define DPI_CHANNEL_NUMBER_POS                  0x03
+#define DBI_CHANNEL_NUMBER_POS                  0x05
+#define FMT_DPI_POS                             0x07
+#define FMT_DBI_POS                             0x0A
+#define DBI_DATA_WIDTH_POS                      0x0D
+
+/* DPI PIXEL FORMATS */
+#define RGB_565_FMT                             0x01    /* RGB 565 FORMAT */
+#define RGB_666_FMT                             0x02    /* RGB 666 FORMAT */
+#define LRGB_666_FMT                            0x03    /* RGB LOOSELY PACKED
+                                                         * 666 FORMAT
+                                                         */
+#define RGB_888_FMT                             0x04    /* RGB 888 FORMAT */
+#define VIRTUAL_CHANNEL_NUMBER_0                0x00    /* Virtual channel 0 */
+#define VIRTUAL_CHANNEL_NUMBER_1                0x01    /* Virtual channel 1 */
+#define VIRTUAL_CHANNEL_NUMBER_2                0x02    /* Virtual channel 2 */
+#define VIRTUAL_CHANNEL_NUMBER_3                0x03    /* Virtual channel 3 */
+
+#define DBI_NOT_SUPPORTED                       0x00    /* command mode
+                                                         * is not supported
+                                                         */
+#define DBI_DATA_WIDTH_16BIT                    0x01    /* 16 bit data */
+#define DBI_DATA_WIDTH_9BIT                     0x02    /* 9 bit data */
+#define DBI_DATA_WIDTH_8BIT                     0x03    /* 8 bit data */
+#define DBI_DATA_WIDTH_OPT1                     0x04    /* option 1 */
+#define DBI_DATA_WIDTH_OPT2                     0x05    /* option 2 */
+
+#define HS_TX_TIMEOUT_REG               0xb010
+#define LP_RX_TIMEOUT_REG               0xb014
+#define TURN_AROUND_TIMEOUT_REG         0xb018
+#define DEVICE_RESET_REG                0xb01C
+#define DPI_RESOLUTION_REG              0xb020
+#define RES_V_POS                               0x10
+#define DBI_RESOLUTION_REG              0xb024 /* Reserved for MDFLD */
+#define HORIZ_SYNC_PAD_COUNT_REG        0xb028
+#define HORIZ_BACK_PORCH_COUNT_REG      0xb02C
+#define HORIZ_FRONT_PORCH_COUNT_REG     0xb030
+#define HORIZ_ACTIVE_AREA_COUNT_REG     0xb034
+#define VERT_SYNC_PAD_COUNT_REG         0xb038
+#define VERT_BACK_PORCH_COUNT_REG       0xb03c
+#define VERT_FRONT_PORCH_COUNT_REG      0xb040
+#define HIGH_LOW_SWITCH_COUNT_REG       0xb044
+#define DPI_CONTROL_REG                 0xb048
+#define DPI_SHUT_DOWN                           (1 << 0)
+#define DPI_TURN_ON                             (1 << 1)
+#define DPI_COLOR_MODE_ON                       (1 << 2)
+#define DPI_COLOR_MODE_OFF                      (1 << 3)
+#define DPI_BACK_LIGHT_ON                       (1 << 4)
+#define DPI_BACK_LIGHT_OFF                      (1 << 5)
+#define DPI_LP                                  (1 << 6)
+#define DPI_DATA_REG                    0xb04c
+#define DPI_BACK_LIGHT_ON_DATA                  0x07
+#define DPI_BACK_LIGHT_OFF_DATA                 0x17
+#define INIT_COUNT_REG                  0xb050
+#define MAX_RET_PAK_REG                 0xb054
+#define VIDEO_FMT_REG                   0xb058
+#define COMPLETE_LAST_PCKT                      (1 << 2)
+#define EOT_DISABLE_REG                 0xb05c
+#define ENABLE_CLOCK_STOPPING                   (1 << 1)
+#define LP_BYTECLK_REG                  0xb060
+#define LP_GEN_DATA_REG                 0xb064
+#define HS_GEN_DATA_REG                 0xb068
+#define LP_GEN_CTRL_REG                 0xb06C
+#define HS_GEN_CTRL_REG                 0xb070
+#define DCS_CHANNEL_NUMBER_POS          0x6
+#define MCS_COMMANDS_POS                0x8
+#define WORD_COUNTS_POS                 0x8
+#define MCS_PARAMETER_POS                       0x10
+#define GEN_FIFO_STAT_REG               0xb074
+#define HS_DATA_FIFO_FULL                       (1 << 0)
+#define HS_DATA_FIFO_HALF_EMPTY                 (1 << 1)
+#define HS_DATA_FIFO_EMPTY                      (1 << 2)
+#define LP_DATA_FIFO_FULL                       (1 << 8)
+#define LP_DATA_FIFO_HALF_EMPTY                 (1 << 9)
+#define LP_DATA_FIFO_EMPTY                      (1 << 10)
+#define HS_CTRL_FIFO_FULL                       (1 << 16)
+#define HS_CTRL_FIFO_HALF_EMPTY                 (1 << 17)
+#define HS_CTRL_FIFO_EMPTY                      (1 << 18)
+#define LP_CTRL_FIFO_FULL                       (1 << 24)
+#define LP_CTRL_FIFO_HALF_EMPTY                 (1 << 25)
+#define LP_CTRL_FIFO_EMPTY                      (1 << 26)
+#define DBI_FIFO_EMPTY                          (1 << 27)
+#define DPI_FIFO_EMPTY                          (1 << 28)
+#define HS_LS_DBI_ENABLE_REG            0xb078
+#define TXCLKESC_REG                    0xb07c
+#define DPHY_PARAM_REG                  0xb080
+#define DBI_BW_CTRL_REG                 0xb084
+#define CLK_LANE_SWT_REG                0xb088
+
+/*
+ * MIPI Adapter registers
+ */
+#define MIPI_CONTROL_REG                0xb104
+#define MIPI_2X_CLOCK_BITS                      ((1 << 0) | (1 << 1))
+#define MIPI_DATA_ADDRESS_REG           0xb108
+#define MIPI_DATA_LENGTH_REG            0xb10C
+#define MIPI_COMMAND_ADDRESS_REG        0xb110
+#define MIPI_COMMAND_LENGTH_REG         0xb114
+#define MIPI_READ_DATA_RETURN_REG0      0xb118
+#define MIPI_READ_DATA_RETURN_REG1      0xb11C
+#define MIPI_READ_DATA_RETURN_REG2      0xb120
+#define MIPI_READ_DATA_RETURN_REG3      0xb124
+#define MIPI_READ_DATA_RETURN_REG4      0xb128
+#define MIPI_READ_DATA_RETURN_REG5      0xb12C
+#define MIPI_READ_DATA_RETURN_REG6      0xb130
+#define MIPI_READ_DATA_RETURN_REG7      0xb134
+#define MIPI_READ_DATA_VALID_REG        0xb138
+
+/* DBI COMMANDS */
+#define soft_reset                      0x01
+/*
+ *      The display module performs a software reset.
+ *      Registers are written with their SW Reset default values.
+ */
+#define get_power_mode                  0x0a
+/*
+ *      The display module returns the current power mode
+ */
+#define get_address_mode                0x0b
+/*
+ *      The display module returns the current status.
+ */
+#define get_pixel_format                0x0c
+/*
+ *      This command gets the pixel format for the RGB image data
+ *      used by the interface.
+ */
+#define get_display_mode                0x0d
+/*
+ *      The display module returns the Display Image Mode status.
+ */
+#define get_signal_mode                 0x0e
+/*
+ *      The display module returns the Display Signal Mode.
+ */
+#define get_diagnostic_result           0x0f
+/*
+ *      The display module returns the self-diagnostic results following
+ *      a Sleep Out command.
+ */
+#define enter_sleep_mode                0x10
+/*
+ *      This command causes the display module to enter the Sleep mode.
+ *      In this mode, all unnecessary blocks inside the display module are
+ *      disabled except interface communication. This is the lowest power
+ *      mode the display module supports.
+ */
+#define exit_sleep_mode                 0x11
+/*
+ *      This command causes the display module to exit Sleep mode.
+ *      All blocks inside the display module are enabled.
+ */
+#define enter_partial_mode              0x12
+/*
+ *      This command causes the display module to enter the Partial Display
+ *      Mode. The Partial Display Mode window is described by the
+ *      set_partial_area command.
+ */
+#define enter_normal_mode               0x13
+/*
+ *      This command causes the display module to enter the Normal mode.
+ *      Normal Mode is defined as Partial Display mode and Scroll mode are off
+ */
+#define exit_invert_mode                0x20
+/*
+ *      This command causes the display module to stop inverting the image
+ *      data on the display device. The frame memory contents remain unchanged.
+ *      No status bits are changed.
+ */
+#define enter_invert_mode               0x21
+/*
+ *      This command causes the display module to invert the image data only on
+ *      the display device. The frame memory contents remain unchanged.
+ *      No status bits are changed.
+ */
+#define set_gamma_curve                 0x26
+/*
+ *      This command selects the desired gamma curve for the display device.
+ *      Four fixed gamma curves are defined in section DCS spec.
+ */
+#define set_display_off                 0x28
+/* ************************************************************************* *\
+This command causes the display module to stop displaying the image data
+on the display device. The frame memory contents remain unchanged.
+No status bits are changed.
+\* ************************************************************************* */
+#define set_display_on                  0x29
+/* ************************************************************************* *\
+This command causes the display module to start displaying the image data
+on the display device. The frame memory contents remain unchanged.
+No status bits are changed.
+\* ************************************************************************* */
+#define set_column_address              0x2a
+/*
+ *      This command defines the column extent of the frame memory accessed by
+ *      the hostprocessor with the read_memory_continue and
+ *      write_memory_continue commands.
+ *      No status bits are changed.
+ */
+#define set_page_addr                   0x2b
+/*
+ *      This command defines the page extent of the frame memory accessed by
+ *      the host processor with the write_memory_continue and
+ *      read_memory_continue command.
+ *      No status bits are changed.
+ */
+#define write_mem_start                 0x2c
+/*
+ *      This command transfers image data from the host processor to the
+ *      display modules frame memory starting at the pixel location specified
+ *      by preceding set_column_address and set_page_address commands.
+ */
+#define set_partial_area                0x30
+/*
+ *      This command defines the Partial Display mode s display area.
+ *      There are two parameters associated with this command, the first
+ *      defines the Start Row (SR) and the second the End Row (ER). SR and ER
+ *      refer to the Frame Memory Line Pointer.
+ */
+#define set_scroll_area                 0x33
+/*
+ *      This command defines the display modules Vertical Scrolling Area.
+ */
+#define set_tear_off                    0x34
+/*
+ *      This command turns off the display modules Tearing Effect output
+ *      signal on the TE signal line.
+ */
+#define set_tear_on                     0x35
+/*
+ *      This command turns on the display modules Tearing Effect output signal
+ *      on the TE signal line.
+ */
+#define set_address_mode                0x36
+/*
+ *      This command sets the data order for transfers from the host processor
+ *      to display modules frame memory,bits B[7:5] and B3, and from the
+ *      display modules frame memory to the display device, bits B[2:0] and B4.
+ */
+#define set_scroll_start                0x37
+/*
+ *      This command sets the start of the vertical scrolling area in the frame
+ *      memory. The vertical scrolling area is fully defined when this command
+ *      is used with the set_scroll_area command The set_scroll_start command
+ *      has one parameter, the Vertical Scroll Pointer. The VSP defines the
+ *      line in the frame memory that is written to the display device as the
+ *      first line of the vertical scroll area.
+ */
+#define exit_idle_mode                  0x38
+/*
+ *      This command causes the display module to exit Idle mode.
+ */
+#define enter_idle_mode                 0x39
+/*
+ *      This command causes the display module to enter Idle Mode.
+ *      In Idle Mode, color expression is reduced. Colors are shown on the
+ *      display device using the MSB of each of the R, G and B color
+ *      components in the frame memory
+ */
+#define set_pixel_format                0x3a
+/*
+ *      This command sets the pixel format for the RGB image data used by the
+ *      interface.
+ *      Bits D[6:4]  DPI Pixel Format Definition
+ *      Bits D[2:0]  DBI Pixel Format Definition
+ *      Bits D7 and D3 are not used.
+ */
+#define DCS_PIXEL_FORMAT_3bpp           0x1
+#define DCS_PIXEL_FORMAT_8bpp           0x2
+#define DCS_PIXEL_FORMAT_12bpp          0x3
+#define DCS_PIXEL_FORMAT_16bpp          0x5
+#define DCS_PIXEL_FORMAT_18bpp          0x6
+#define DCS_PIXEL_FORMAT_24bpp          0x7
+
+#define write_mem_cont                  0x3c
+
+/*
+ *      This command transfers image data from the host processor to the
+ *      display module's frame memory continuing from the pixel location
+ *      following the previous write_memory_continue or write_memory_start
+ *      command.
+ */
+#define set_tear_scanline               0x44
+/*
+ *      This command turns on the display modules Tearing Effect output signal
+ *      on the TE signal line when the display module reaches line N.
+ */
+#define get_scanline                    0x45
+/*
+ *      The display module returns the current scanline, N, used to update the
+ *       display device. The total number of scanlines on a display device is
+ *      defined as VSYNC + VBP + VACT + VFP.The first scanline is defined as
+ *      the first line of V Sync and is denoted as Line 0.
+ *      When in Sleep Mode, the value returned by get_scanline is undefined.
+ */
+
+/* MCS or Generic COMMANDS */
+/* MCS/generic data type */
+#define GEN_SHORT_WRITE_0       0x03  /* generic short write, no parameters */
+#define GEN_SHORT_WRITE_1       0x13  /* generic short write, 1 parameters */
+#define GEN_SHORT_WRITE_2       0x23  /* generic short write, 2 parameters */
+#define GEN_READ_0              0x04  /* generic read, no parameters */
+#define GEN_READ_1              0x14  /* generic read, 1 parameters */
+#define GEN_READ_2              0x24  /* generic read, 2 parameters */
+#define GEN_LONG_WRITE          0x29  /* generic long write */
+#define MCS_SHORT_WRITE_0       0x05  /* MCS short write, no parameters */
+#define MCS_SHORT_WRITE_1       0x15  /* MCS short write, 1 parameters */
+#define MCS_READ                0x06  /* MCS read, no parameters */
+#define MCS_LONG_WRITE          0x39  /* MCS long write */
+/* MCS/generic commands */
+/* TPO MCS */
+#define write_display_profile           0x50
+#define write_display_brightness        0x51
+#define write_ctrl_display              0x53
+#define write_ctrl_cabc                 0x55
+  #define UI_IMAGE              0x01
+  #define STILL_IMAGE           0x02
+  #define MOVING_IMAGE          0x03
+#define write_hysteresis                0x57
+#define write_gamma_setting             0x58
+#define write_cabc_min_bright           0x5e
+#define write_kbbc_profile              0x60
+/* TMD MCS */
+#define tmd_write_display_brightness 0x8c
+
+/*
+ *      This command is used to control ambient light, panel backlight
+ *      brightness and gamma settings.
+ */
+#define BRIGHT_CNTL_BLOCK_ON    (1 << 5)
+#define AMBIENT_LIGHT_SENSE_ON  (1 << 4)
+#define DISPLAY_DIMMING_ON      (1 << 3)
+#define BACKLIGHT_ON            (1 << 2)
+#define DISPLAY_BRIGHTNESS_AUTO (1 << 1)
+#define GAMMA_AUTO              (1 << 0)
+
+/* DCS Interface Pixel Formats */
+#define DCS_PIXEL_FORMAT_3BPP   0x1
+#define DCS_PIXEL_FORMAT_8BPP   0x2
+#define DCS_PIXEL_FORMAT_12BPP  0x3
+#define DCS_PIXEL_FORMAT_16BPP  0x5
+#define DCS_PIXEL_FORMAT_18BPP  0x6
+#define DCS_PIXEL_FORMAT_24BPP  0x7
+/* ONE PARAMETER READ DATA */
+#define addr_mode_data          0xfc
+#define diag_res_data           0x00
+#define disp_mode_data          0x23
+#define pxl_fmt_data            0x77
+#define pwr_mode_data           0x74
+#define sig_mode_data           0x00
+/* TWO PARAMETERS READ DATA */
+#define scanline_data1          0xff
+#define scanline_data2          0xff
+#define NON_BURST_MODE_SYNC_PULSE       0x01    /* Non Burst Mode
+                                                 * with Sync Pulse
+                                                 */
+#define NON_BURST_MODE_SYNC_EVENTS      0x02    /* Non Burst Mode
+                                                 * with Sync events
+                                                 */
+#define BURST_MODE                      0x03    /* Burst Mode */
+#define DBI_COMMAND_BUFFER_SIZE         0x240   /* 0x32 */    /* 0x120 */
+                                                /* Allocate at least
+                                                 * 0x100 Byte with 32
+                                                 * byte alignment
+                                                 */
+#define DBI_DATA_BUFFER_SIZE            0x120   /* Allocate at least
+                                                 * 0x100 Byte with 32
+                                                 * byte alignment
+                                                 */
+#define DBI_CB_TIME_OUT                 0xFFFF
+
+#define GEN_FB_TIME_OUT                 2000
+
+#define SKU_83                          0x01
+#define SKU_100                         0x02
+#define SKU_100L                        0x04
+#define SKU_BYPASS                      0x08
+
+/* Some handy macros for playing with bitfields. */
+#define PSB_MASK(high, low) (((1<<((high)-(low)+1))-1)<<(low))
+#define SET_FIELD(value, field) (((value) << field ## _SHIFT) & field ## _MASK)
+#define GET_FIELD(word, field) (((word)  & field ## _MASK) >> field ## _SHIFT)
+
+#define _PIPE(pipe, a, b) ((a) + (pipe)*((b)-(a)))
+
+/* PCI config space */
+
+#define SB_PCKT         0x02100 /* cedarview */
+# define SB_OPCODE_MASK                         PSB_MASK(31, 16)
+# define SB_OPCODE_SHIFT                        16
+# define SB_OPCODE_READ                         0
+# define SB_OPCODE_WRITE                        1
+# define SB_DEST_MASK                           PSB_MASK(15, 8)
+# define SB_DEST_SHIFT                          8
+# define SB_DEST_DPLL                           0x88
+# define SB_BYTE_ENABLE_MASK                    PSB_MASK(7, 4)
+# define SB_BYTE_ENABLE_SHIFT                   4
+# define SB_BUSY                                (1 << 0)
+
+
+/* 32-bit value read/written from the DPIO reg. */
+#define SB_DATA         0x02104 /* cedarview */
+/* 32-bit address of the DPIO reg to be read/written. */
+#define SB_ADDR         0x02108 /* cedarview */
+#define DPIO_CFG        0x02110 /* cedarview */
+# define DPIO_MODE_SELECT_1                     (1 << 3)
+# define DPIO_MODE_SELECT_0                     (1 << 2)
+# define DPIO_SFR_BYPASS                        (1 << 1)
+/* reset is active low */
+# define DPIO_CMN_RESET_N                       (1 << 0)
+
+/* Cedarview sideband registers */
+#define _SB_M_A                 0x8008
+#define _SB_M_B                 0x8028
+#define SB_M(pipe) _PIPE(pipe, _SB_M_A, _SB_M_B)
+# define SB_M_DIVIDER_MASK                      (0xFF << 24)
+# define SB_M_DIVIDER_SHIFT                     24
+
+#define _SB_N_VCO_A             0x8014
+#define _SB_N_VCO_B             0x8034
+#define SB_N_VCO(pipe) _PIPE(pipe, _SB_N_VCO_A, _SB_N_VCO_B)
+#define SB_N_VCO_SEL_MASK                       PSB_MASK(31, 30)
+#define SB_N_VCO_SEL_SHIFT                      30
+#define SB_N_DIVIDER_MASK                       PSB_MASK(29, 26)
+#define SB_N_DIVIDER_SHIFT                      26
+#define SB_N_CB_TUNE_MASK                       PSB_MASK(25, 24)
+#define SB_N_CB_TUNE_SHIFT                      24
+
+#define _SB_REF_A               0x8018
+#define _SB_REF_B               0x8038
+#define SB_REF_SFR(pipe)        _PIPE(pipe, _SB_REF_A, _SB_REF_B)
+
+#define _SB_P_A                 0x801c
+#define _SB_P_B                 0x803c
+#define SB_P(pipe) _PIPE(pipe, _SB_P_A, _SB_P_B)
+#define SB_P2_DIVIDER_MASK                      PSB_MASK(31, 30)
+#define SB_P2_DIVIDER_SHIFT                     30
+#define SB_P2_10                                0 /* HDMI, DP, DAC */
+#define SB_P2_5                         1 /* DAC */
+#define SB_P2_14                                2 /* LVDS single */
+#define SB_P2_7                         3 /* LVDS double */
+#define SB_P1_DIVIDER_MASK                      PSB_MASK(15, 12)
+#define SB_P1_DIVIDER_SHIFT                     12
+
+#define PSB_LANE0               0x120
+#define PSB_LANE1               0x220
+#define PSB_LANE2               0x2320
+#define PSB_LANE3               0x2420
+
+#define LANE_PLL_MASK           (0x7 << 20)
+#define LANE_PLL_ENABLE         (0x3 << 20)
+
+
+#endif
diff --git a/drivers/staging/gma500/psb_intel_sdvo.c b/drivers/staging/gma500/psb_intel_sdvo.c
new file mode 100644
index 00000000000..a4bad1af4b7
--- /dev/null
+++ b/drivers/staging/gma500/psb_intel_sdvo.c
@@ -0,0 +1,1293 @@
+/*
+ * Copyright (c) 2006-2007 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors:
+ *      Eric Anholt <eric@anholt.net>
+ */
+
+#include <linux/i2c.h>
+#include <linux/delay.h>
+/* #include <drm/drm_crtc.h> */
+#include <drm/drmP.h>
+#include "psb_drv.h"
+#include "psb_intel_drv.h"
+#include "psb_intel_reg.h"
+#include "psb_intel_sdvo_regs.h"
+
+struct psb_intel_sdvo_priv {
+        struct psb_intel_i2c_chan *i2c_bus;
+        int slaveaddr;
+        int output_device;
+
+        u16 active_outputs;
+
+        struct psb_intel_sdvo_caps caps;
+        int pixel_clock_min, pixel_clock_max;
+
+        int save_sdvo_mult;
+        u16 save_active_outputs;
+        struct psb_intel_sdvo_dtd save_input_dtd_1, save_input_dtd_2;
+        struct psb_intel_sdvo_dtd save_output_dtd[16];
+        u32 save_SDVOX;
+        u8 in_out_map[4];
+
+        u8 by_input_wiring;
+        u32 active_device;
+};
+
+/**
+ * Writes the SDVOB or SDVOC with the given value, but always writes both
+ * SDVOB and SDVOC to work around apparent hardware issues (according to
+ * comments in the BIOS).
+ */
+void psb_intel_sdvo_write_sdvox(struct psb_intel_output *psb_intel_output,
+                                u32 val)
+{
+        struct drm_device *dev = psb_intel_output->base.dev;
+        struct psb_intel_sdvo_priv *sdvo_priv = psb_intel_output->dev_priv;
+        u32 bval = val, cval = val;
+        int i;
+
+        if (sdvo_priv->output_device == SDVOB)
+                cval = REG_READ(SDVOC);
+        else
+                bval = REG_READ(SDVOB);
+        /*
+         * Write the registers twice for luck. Sometimes,
+         * writing them only once doesn't appear to 'stick'.
+         * The BIOS does this too. Yay, magic
+         */
+        for (i = 0; i < 2; i++) {
+                REG_WRITE(SDVOB, bval);
+                REG_READ(SDVOB);
+                REG_WRITE(SDVOC, cval);
+                REG_READ(SDVOC);
+        }
+}
+
+static bool psb_intel_sdvo_read_byte(
+                                struct psb_intel_output *psb_intel_output,
+                                u8 addr, u8 *ch)
+{
+        struct psb_intel_sdvo_priv *sdvo_priv = psb_intel_output->dev_priv;
+        u8 out_buf[2];
+        u8 buf[2];
+        int ret;
+
+        struct i2c_msg msgs[] = {
+                {
+                 .addr = sdvo_priv->i2c_bus->slave_addr,
+                 .flags = 0,
+                 .len = 1,
+                 .buf = out_buf,
+                 },
+                {
+                 .addr = sdvo_priv->i2c_bus->slave_addr,
+                 .flags = I2C_M_RD,
+                 .len = 1,
+                 .buf = buf,
+                 }
+        };
+
+        out_buf[0] = addr;
+        out_buf[1] = 0;
+
+        ret = i2c_transfer(&sdvo_priv->i2c_bus->adapter, msgs, 2);
+        if (ret == 2) {
+                *ch = buf[0];
+                return true;
+        }
+
+        return false;
+}
+
+static bool psb_intel_sdvo_write_byte(
+                        struct psb_intel_output *psb_intel_output,
+                        int addr, u8 ch)
+{
+        u8 out_buf[2];
+        struct i2c_msg msgs[] = {
+                {
+                 .addr = psb_intel_output->i2c_bus->slave_addr,
+                 .flags = 0,
+                 .len = 2,
+                 .buf = out_buf,
+                 }
+        };
+
+        out_buf[0] = addr;
+        out_buf[1] = ch;
+
+        if (i2c_transfer(&psb_intel_output->i2c_bus->adapter, msgs, 1) == 1)
+                return true;
+        return false;
+}
+
+#define SDVO_CMD_NAME_ENTRY(cmd) {cmd, #cmd}
+/** Mapping of command numbers to names, for debug output */
+static const struct _sdvo_cmd_name {
+        u8 cmd;
+        char *name;
+} sdvo_cmd_names[] = {
+SDVO_CMD_NAME_ENTRY(SDVO_CMD_RESET),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_DEVICE_CAPS),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_FIRMWARE_REV),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_TRAINED_INPUTS),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_ACTIVE_OUTPUTS),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_ACTIVE_OUTPUTS),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_IN_OUT_MAP),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_IN_OUT_MAP),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_ATTACHED_DISPLAYS),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_HOT_PLUG_SUPPORT),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_ACTIVE_HOT_PLUG),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_ACTIVE_HOT_PLUG),
+            SDVO_CMD_NAME_ENTRY
+            (SDVO_CMD_GET_INTERRUPT_EVENT_SOURCE),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_TARGET_INPUT),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_TARGET_OUTPUT),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_INPUT_TIMINGS_PART1),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_INPUT_TIMINGS_PART2),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_INPUT_TIMINGS_PART1),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_INPUT_TIMINGS_PART2),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_INPUT_TIMINGS_PART1),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_OUTPUT_TIMINGS_PART1),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_OUTPUT_TIMINGS_PART2),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_OUTPUT_TIMINGS_PART1),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_OUTPUT_TIMINGS_PART2),
+            SDVO_CMD_NAME_ENTRY
+            (SDVO_CMD_CREATE_PREFERRED_INPUT_TIMING),
+            SDVO_CMD_NAME_ENTRY
+            (SDVO_CMD_GET_PREFERRED_INPUT_TIMING_PART1),
+            SDVO_CMD_NAME_ENTRY
+            (SDVO_CMD_GET_PREFERRED_INPUT_TIMING_PART2),
+            SDVO_CMD_NAME_ENTRY
+            (SDVO_CMD_GET_INPUT_PIXEL_CLOCK_RANGE),
+            SDVO_CMD_NAME_ENTRY
+            (SDVO_CMD_GET_OUTPUT_PIXEL_CLOCK_RANGE),
+            SDVO_CMD_NAME_ENTRY
+            (SDVO_CMD_GET_SUPPORTED_CLOCK_RATE_MULTS),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_CLOCK_RATE_MULT),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_CLOCK_RATE_MULT),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_SUPPORTED_TV_FORMATS),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_TV_FORMAT),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_TV_FORMAT),
+            SDVO_CMD_NAME_ENTRY
+            (SDVO_CMD_SET_TV_RESOLUTION_SUPPORT),
+            SDVO_CMD_NAME_ENTRY(SDVO_CMD_SET_CONTROL_BUS_SWITCH),};
+
+#define SDVO_NAME(dev_priv) \
+                 ((dev_priv)->output_device == SDVOB ? "SDVOB" : "SDVOC")
+#define SDVO_PRIV(output)   ((struct psb_intel_sdvo_priv *) (output)->dev_priv)
+
+static void psb_intel_sdvo_write_cmd(struct psb_intel_output *psb_intel_output,
+                                     u8 cmd,
+                                     void *args,
+                                     int args_len)
+{
+        struct psb_intel_sdvo_priv *sdvo_priv = psb_intel_output->dev_priv;
+        int i;
+
+        if (0) {
+                printk(KERN_DEBUG "%s: W: %02X ", SDVO_NAME(sdvo_priv), cmd);
+                for (i = 0; i < args_len; i++)
+                        printk(KERN_CONT "%02X ", ((u8 *) args)[i]);
+                for (; i < 8; i++)
+                        printk(KERN_CONT "   ");
+                for (i = 0;
+                     i <
+                     sizeof(sdvo_cmd_names) / sizeof(sdvo_cmd_names[0]);
+                     i++) {
+                        if (cmd == sdvo_cmd_names[i].cmd) {
+                                printk(KERN_CONT
+                                        "(%s)", sdvo_cmd_names[i].name);
+                                break;
+                        }
+                }
+                if (i ==
+                    sizeof(sdvo_cmd_names) / sizeof(sdvo_cmd_names[0]))
+                        printk(KERN_CONT "(%02X)", cmd);
+                printk(KERN_CONT "\n");
+        }
+
+        for (i = 0; i < args_len; i++) {
+                psb_intel_sdvo_write_byte(psb_intel_output,
+                                        SDVO_I2C_ARG_0 - i,
+                                        ((u8 *) args)[i]);
+        }
+
+        psb_intel_sdvo_write_byte(psb_intel_output, SDVO_I2C_OPCODE, cmd);
+}
+
+static const char *const cmd_status_names[] = {
+        "Power on",
+        "Success",
+        "Not supported",
+        "Invalid arg",
+        "Pending",
+        "Target not specified",
+        "Scaling not supported"
+};
+
+static u8 psb_intel_sdvo_read_response(
+                                struct psb_intel_output *psb_intel_output,
+                                void *response, int response_len)
+{
+        struct psb_intel_sdvo_priv *sdvo_priv = psb_intel_output->dev_priv;
+        int i;
+        u8 status;
+        u8 retry = 50;
+
+        while (retry--) {
+                /* Read the command response */
+                for (i = 0; i < response_len; i++) {
+                        psb_intel_sdvo_read_byte(psb_intel_output,
+                                             SDVO_I2C_RETURN_0 + i,
+                                             &((u8 *) response)[i]);
+                }
+
+                /* read the return status */
+                psb_intel_sdvo_read_byte(psb_intel_output,
+                                         SDVO_I2C_CMD_STATUS,
+                                         &status);
+
+                if (0) {
+                        pr_debug("%s: R: ", SDVO_NAME(sdvo_priv));
+                        for (i = 0; i < response_len; i++)
+                                printk(KERN_CONT "%02X ", ((u8 *) response)[i]);
+                        for (; i < 8; i++)
+                                printk("   ");
+                        if (status <= SDVO_CMD_STATUS_SCALING_NOT_SUPP)
+                                printk(KERN_CONT "(%s)",
+                                         cmd_status_names[status]);
+                        else
+                                printk(KERN_CONT "(??? %d)", status);
+                        printk(KERN_CONT "\n");
+                }
+
+                if (status != SDVO_CMD_STATUS_PENDING)
+                        return status;
+
+                mdelay(50);
+        }
+
+        return status;
+}
+
+int psb_intel_sdvo_get_pixel_multiplier(struct drm_display_mode *mode)
+{
+        if (mode->clock >= 100000)
+                return 1;
+        else if (mode->clock >= 50000)
+                return 2;
+        else
+                return 4;
+}
+
+/**
+ * Don't check status code from this as it switches the bus back to the
+ * SDVO chips which defeats the purpose of doing a bus switch in the first
+ * place.
+ */
+void psb_intel_sdvo_set_control_bus_switch(
+                                struct psb_intel_output *psb_intel_output,
+                                u8 target)
+{
+        psb_intel_sdvo_write_cmd(psb_intel_output,
+                                 SDVO_CMD_SET_CONTROL_BUS_SWITCH,
+                                 &target,
+                                 1);
+}
+
+static bool psb_intel_sdvo_set_target_input(
+                                struct psb_intel_output *psb_intel_output,
+                                bool target_0, bool target_1)
+{
+        struct psb_intel_sdvo_set_target_input_args targets = { 0 };
+        u8 status;
+
+        if (target_0 && target_1)
+                return SDVO_CMD_STATUS_NOTSUPP;
+
+        if (target_1)
+                targets.target_1 = 1;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output, SDVO_CMD_SET_TARGET_INPUT,
+                             &targets, sizeof(targets));
+
+        status = psb_intel_sdvo_read_response(psb_intel_output, NULL, 0);
+
+        return status == SDVO_CMD_STATUS_SUCCESS;
+}
+
+/**
+ * Return whether each input is trained.
+ *
+ * This function is making an assumption about the layout of the response,
+ * which should be checked against the docs.
+ */
+static bool psb_intel_sdvo_get_trained_inputs(struct psb_intel_output
+                                          *psb_intel_output, bool *input_1,
+                                          bool *input_2)
+{
+        struct psb_intel_sdvo_get_trained_inputs_response response;
+        u8 status;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output, SDVO_CMD_GET_TRAINED_INPUTS,
+                             NULL, 0);
+        status =
+            psb_intel_sdvo_read_response(psb_intel_output, &response,
+                                     sizeof(response));
+        if (status != SDVO_CMD_STATUS_SUCCESS)
+                return false;
+
+        *input_1 = response.input0_trained;
+        *input_2 = response.input1_trained;
+        return true;
+}
+
+static bool psb_intel_sdvo_get_active_outputs(struct psb_intel_output
+                                          *psb_intel_output, u16 *outputs)
+{
+        u8 status;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output, SDVO_CMD_GET_ACTIVE_OUTPUTS,
+                             NULL, 0);
+        status =
+            psb_intel_sdvo_read_response(psb_intel_output, outputs,
+                                     sizeof(*outputs));
+
+        return status == SDVO_CMD_STATUS_SUCCESS;
+}
+
+static bool psb_intel_sdvo_set_active_outputs(struct psb_intel_output
+                                          *psb_intel_output, u16 outputs)
+{
+        u8 status;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output, SDVO_CMD_SET_ACTIVE_OUTPUTS,
+                             &outputs, sizeof(outputs));
+        status = psb_intel_sdvo_read_response(psb_intel_output, NULL, 0);
+        return status == SDVO_CMD_STATUS_SUCCESS;
+}
+
+static bool psb_intel_sdvo_set_encoder_power_state(struct psb_intel_output
+                                               *psb_intel_output, int mode)
+{
+        u8 status, state = SDVO_ENCODER_STATE_ON;
+
+        switch (mode) {
+        case DRM_MODE_DPMS_ON:
+                state = SDVO_ENCODER_STATE_ON;
+                break;
+        case DRM_MODE_DPMS_STANDBY:
+                state = SDVO_ENCODER_STATE_STANDBY;
+                break;
+        case DRM_MODE_DPMS_SUSPEND:
+                state = SDVO_ENCODER_STATE_SUSPEND;
+                break;
+        case DRM_MODE_DPMS_OFF:
+                state = SDVO_ENCODER_STATE_OFF;
+                break;
+        }
+
+        psb_intel_sdvo_write_cmd(psb_intel_output,
+                             SDVO_CMD_SET_ENCODER_POWER_STATE, &state,
+                             sizeof(state));
+        status = psb_intel_sdvo_read_response(psb_intel_output, NULL, 0);
+
+        return status == SDVO_CMD_STATUS_SUCCESS;
+}
+
+static bool psb_intel_sdvo_get_input_pixel_clock_range(struct psb_intel_output
+                                                   *psb_intel_output,
+                                                   int *clock_min,
+                                                   int *clock_max)
+{
+        struct psb_intel_sdvo_pixel_clock_range clocks;
+        u8 status;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output,
+                             SDVO_CMD_GET_INPUT_PIXEL_CLOCK_RANGE, NULL,
+                             0);
+
+        status =
+            psb_intel_sdvo_read_response(psb_intel_output, &clocks,
+                                     sizeof(clocks));
+
+        if (status != SDVO_CMD_STATUS_SUCCESS)
+                return false;
+
+        /* Convert the values from units of 10 kHz to kHz. */
+        *clock_min = clocks.min * 10;
+        *clock_max = clocks.max * 10;
+
+        return true;
+}
+
+static bool psb_intel_sdvo_set_target_output(
+                                struct psb_intel_output *psb_intel_output,
+                                u16 outputs)
+{
+        u8 status;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output, SDVO_CMD_SET_TARGET_OUTPUT,
+                             &outputs, sizeof(outputs));
+
+        status = psb_intel_sdvo_read_response(psb_intel_output, NULL, 0);
+        return status == SDVO_CMD_STATUS_SUCCESS;
+}
+
+static bool psb_intel_sdvo_get_timing(struct psb_intel_output *psb_intel_output,
+                                  u8 cmd, struct psb_intel_sdvo_dtd *dtd)
+{
+        u8 status;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output, cmd, NULL, 0);
+        status = psb_intel_sdvo_read_response(psb_intel_output, &dtd->part1,
+                                          sizeof(dtd->part1));
+        if (status != SDVO_CMD_STATUS_SUCCESS)
+                return false;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output, cmd + 1, NULL, 0);
+        status = psb_intel_sdvo_read_response(psb_intel_output, &dtd->part2,
+                                          sizeof(dtd->part2));
+        if (status != SDVO_CMD_STATUS_SUCCESS)
+                return false;
+
+        return true;
+}
+
+static bool psb_intel_sdvo_get_input_timing(
+                                struct psb_intel_output *psb_intel_output,
+                                struct psb_intel_sdvo_dtd *dtd)
+{
+        return psb_intel_sdvo_get_timing(psb_intel_output,
+                                     SDVO_CMD_GET_INPUT_TIMINGS_PART1,
+                                     dtd);
+}
+
+static bool psb_intel_sdvo_set_timing(
+                                struct psb_intel_output *psb_intel_output,
+                                u8 cmd,
+                                struct psb_intel_sdvo_dtd *dtd)
+{
+        u8 status;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output, cmd, &dtd->part1,
+                             sizeof(dtd->part1));
+        status = psb_intel_sdvo_read_response(psb_intel_output, NULL, 0);
+        if (status != SDVO_CMD_STATUS_SUCCESS)
+                return false;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output, cmd + 1, &dtd->part2,
+                             sizeof(dtd->part2));
+        status = psb_intel_sdvo_read_response(psb_intel_output, NULL, 0);
+        if (status != SDVO_CMD_STATUS_SUCCESS)
+                return false;
+
+        return true;
+}
+
+static bool psb_intel_sdvo_set_input_timing(
+                                struct psb_intel_output *psb_intel_output,
+                                struct psb_intel_sdvo_dtd *dtd)
+{
+        return psb_intel_sdvo_set_timing(psb_intel_output,
+                                     SDVO_CMD_SET_INPUT_TIMINGS_PART1,
+                                     dtd);
+}
+
+static bool psb_intel_sdvo_set_output_timing(
+                                struct psb_intel_output *psb_intel_output,
+                                struct psb_intel_sdvo_dtd *dtd)
+{
+        return psb_intel_sdvo_set_timing(psb_intel_output,
+                                     SDVO_CMD_SET_OUTPUT_TIMINGS_PART1,
+                                     dtd);
+}
+
+static int psb_intel_sdvo_get_clock_rate_mult(struct psb_intel_output
+                                                *psb_intel_output)
+{
+        u8 response, status;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output,
+                                 SDVO_CMD_GET_CLOCK_RATE_MULT,
+                                 NULL,
+                                 0);
+
+        status = psb_intel_sdvo_read_response(psb_intel_output, &response, 1);
+
+        if (status != SDVO_CMD_STATUS_SUCCESS) {
+                DRM_DEBUG("Couldn't get SDVO clock rate multiplier\n");
+                return SDVO_CLOCK_RATE_MULT_1X;
+        } else {
+                DRM_DEBUG("Current clock rate multiplier: %d\n", response);
+        }
+
+        return response;
+}
+
+static bool psb_intel_sdvo_set_clock_rate_mult(struct psb_intel_output
+                                                *psb_intel_output, u8 val)
+{
+        u8 status;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output,
+                                SDVO_CMD_SET_CLOCK_RATE_MULT,
+                                &val,
+                                1);
+
+        status = psb_intel_sdvo_read_response(psb_intel_output, NULL, 0);
+        if (status != SDVO_CMD_STATUS_SUCCESS)
+                return false;
+
+        return true;
+}
+
+static bool psb_sdvo_set_current_inoutmap(struct psb_intel_output *output,
+                                          u32 in0outputmask,
+                                          u32 in1outputmask)
+{
+        u8 byArgs[4];
+        u8 status;
+        int i;
+        struct psb_intel_sdvo_priv *sdvo_priv = output->dev_priv;
+
+        /* Make all fields of the  args/ret to zero */
+        memset(byArgs, 0, sizeof(byArgs));
+
+        /* Fill up the argument values; */
+        byArgs[0] = (u8) (in0outputmask & 0xFF);
+        byArgs[1] = (u8) ((in0outputmask >> 8) & 0xFF);
+        byArgs[2] = (u8) (in1outputmask & 0xFF);
+        byArgs[3] = (u8) ((in1outputmask >> 8) & 0xFF);
+
+
+        /*save inoutmap arg here*/
+        for (i = 0; i < 4; i++)
+                sdvo_priv->in_out_map[i] = byArgs[0];
+
+        psb_intel_sdvo_write_cmd(output, SDVO_CMD_SET_IN_OUT_MAP, byArgs, 4);
+        status = psb_intel_sdvo_read_response(output, NULL, 0);
+
+        if (status != SDVO_CMD_STATUS_SUCCESS)
+                return false;
+        return true;
+}
+
+
+static void psb_intel_sdvo_set_iomap(struct psb_intel_output *output)
+{
+        u32 dwCurrentSDVOIn0 = 0;
+        u32 dwCurrentSDVOIn1 = 0;
+        u32 dwDevMask = 0;
+
+
+        struct psb_intel_sdvo_priv *sdvo_priv = output->dev_priv;
+
+        /* Please DO NOT change the following code. */
+        /* SDVOB_IN0 or SDVOB_IN1 ==> sdvo_in0 */
+        /* SDVOC_IN0 or SDVOC_IN1 ==> sdvo_in1 */
+        if (sdvo_priv->by_input_wiring & (SDVOB_IN0 | SDVOC_IN0)) {
+                switch (sdvo_priv->active_device) {
+                case SDVO_DEVICE_LVDS:
+                        dwDevMask = SDVO_OUTPUT_LVDS0 | SDVO_OUTPUT_LVDS1;
+                        break;
+                case SDVO_DEVICE_TMDS:
+                        dwDevMask = SDVO_OUTPUT_TMDS0 | SDVO_OUTPUT_TMDS1;
+                        break;
+                case SDVO_DEVICE_TV:
+                        dwDevMask =
+                        SDVO_OUTPUT_YPRPB0 | SDVO_OUTPUT_SVID0 |
+                        SDVO_OUTPUT_CVBS0 | SDVO_OUTPUT_YPRPB1 |
+                        SDVO_OUTPUT_SVID1 | SDVO_OUTPUT_CVBS1 |
+                        SDVO_OUTPUT_SCART0 | SDVO_OUTPUT_SCART1;
+                        break;
+                case SDVO_DEVICE_CRT:
+                        dwDevMask = SDVO_OUTPUT_RGB0 | SDVO_OUTPUT_RGB1;
+                        break;
+                }
+                dwCurrentSDVOIn0 = (sdvo_priv->active_outputs & dwDevMask);
+        } else if (sdvo_priv->by_input_wiring & (SDVOB_IN1 | SDVOC_IN1)) {
+                switch (sdvo_priv->active_device) {
+                case SDVO_DEVICE_LVDS:
+                        dwDevMask = SDVO_OUTPUT_LVDS0 | SDVO_OUTPUT_LVDS1;
+                        break;
+                case SDVO_DEVICE_TMDS:
+                        dwDevMask = SDVO_OUTPUT_TMDS0 | SDVO_OUTPUT_TMDS1;
+                        break;
+                case SDVO_DEVICE_TV:
+                        dwDevMask =
+                        SDVO_OUTPUT_YPRPB0 | SDVO_OUTPUT_SVID0 |
+                        SDVO_OUTPUT_CVBS0 | SDVO_OUTPUT_YPRPB1 |
+                        SDVO_OUTPUT_SVID1 | SDVO_OUTPUT_CVBS1 |
+                        SDVO_OUTPUT_SCART0 | SDVO_OUTPUT_SCART1;
+                        break;
+                case SDVO_DEVICE_CRT:
+                        dwDevMask = SDVO_OUTPUT_RGB0 | SDVO_OUTPUT_RGB1;
+                        break;
+                }
+                dwCurrentSDVOIn1 = (sdvo_priv->active_outputs & dwDevMask);
+        }
+
+        psb_sdvo_set_current_inoutmap(output, dwCurrentSDVOIn0,
+                                          dwCurrentSDVOIn1);
+}
+
+
+static bool psb_intel_sdvo_mode_fixup(struct drm_encoder *encoder,
+                                  struct drm_display_mode *mode,
+                                  struct drm_display_mode *adjusted_mode)
+{
+        /* Make the CRTC code factor in the SDVO pixel multiplier.  The SDVO
+         * device will be told of the multiplier during mode_set.
+         */
+        adjusted_mode->clock *= psb_intel_sdvo_get_pixel_multiplier(mode);
+        return true;
+}
+
+static void psb_intel_sdvo_mode_set(struct drm_encoder *encoder,
+                                struct drm_display_mode *mode,
+                                struct drm_display_mode *adjusted_mode)
+{
+        struct drm_device *dev = encoder->dev;
+        struct drm_crtc *crtc = encoder->crtc;
+        struct psb_intel_crtc *psb_intel_crtc = to_psb_intel_crtc(crtc);
+        struct psb_intel_output *psb_intel_output =
+                                        enc_to_psb_intel_output(encoder);
+        struct psb_intel_sdvo_priv *sdvo_priv = psb_intel_output->dev_priv;
+        u16 width, height;
+        u16 h_blank_len, h_sync_len, v_blank_len, v_sync_len;
+        u16 h_sync_offset, v_sync_offset;
+        u32 sdvox;
+        struct psb_intel_sdvo_dtd output_dtd;
+        int sdvo_pixel_multiply;
+
+        if (!mode)
+                return;
+
+        psb_intel_sdvo_set_target_output(psb_intel_output, 0);
+
+        width = mode->crtc_hdisplay;
+        height = mode->crtc_vdisplay;
+
+        /* do some mode translations */
+        h_blank_len = mode->crtc_hblank_end - mode->crtc_hblank_start;
+        h_sync_len = mode->crtc_hsync_end - mode->crtc_hsync_start;
+
+        v_blank_len = mode->crtc_vblank_end - mode->crtc_vblank_start;
+        v_sync_len = mode->crtc_vsync_end - mode->crtc_vsync_start;
+
+        h_sync_offset = mode->crtc_hsync_start - mode->crtc_hblank_start;
+        v_sync_offset = mode->crtc_vsync_start - mode->crtc_vblank_start;
+
+        output_dtd.part1.clock = mode->clock / 10;
+        output_dtd.part1.h_active = width & 0xff;
+        output_dtd.part1.h_blank = h_blank_len & 0xff;
+        output_dtd.part1.h_high = (((width >> 8) & 0xf) << 4) |
+            ((h_blank_len >> 8) & 0xf);
+        output_dtd.part1.v_active = height & 0xff;
+        output_dtd.part1.v_blank = v_blank_len & 0xff;
+        output_dtd.part1.v_high = (((height >> 8) & 0xf) << 4) |
+            ((v_blank_len >> 8) & 0xf);
+
+        output_dtd.part2.h_sync_off = h_sync_offset;
+        output_dtd.part2.h_sync_width = h_sync_len & 0xff;
+        output_dtd.part2.v_sync_off_width = (v_sync_offset & 0xf) << 4 |
+            (v_sync_len & 0xf);
+        output_dtd.part2.sync_off_width_high =
+            ((h_sync_offset & 0x300) >> 2) | ((h_sync_len & 0x300) >> 4) |
+            ((v_sync_offset & 0x30) >> 2) | ((v_sync_len & 0x30) >> 4);
+
+        output_dtd.part2.dtd_flags = 0x18;
+        if (mode->flags & DRM_MODE_FLAG_PHSYNC)
+                output_dtd.part2.dtd_flags |= 0x2;
+        if (mode->flags & DRM_MODE_FLAG_PVSYNC)
+                output_dtd.part2.dtd_flags |= 0x4;
+
+        output_dtd.part2.sdvo_flags = 0;
+        output_dtd.part2.v_sync_off_high = v_sync_offset & 0xc0;
+        output_dtd.part2.reserved = 0;
+
+        /* Set the output timing to the screen */
+        psb_intel_sdvo_set_target_output(psb_intel_output,
+                                     sdvo_priv->active_outputs);
+
+        /* Set the input timing to the screen. Assume always input 0. */
+        psb_intel_sdvo_set_target_input(psb_intel_output, true, false);
+
+        psb_intel_sdvo_set_output_timing(psb_intel_output, &output_dtd);
+
+        /* We would like to use i830_sdvo_create_preferred_input_timing() to
+         * provide the device with a timing it can support, if it supports that
+         * feature.  However, presumably we would need to adjust the CRTC to
+         * output the preferred timing, and we don't support that currently.
+         */
+        psb_intel_sdvo_set_input_timing(psb_intel_output, &output_dtd);
+
+        switch (psb_intel_sdvo_get_pixel_multiplier(mode)) {
+        case 1:
+                psb_intel_sdvo_set_clock_rate_mult(psb_intel_output,
+                                               SDVO_CLOCK_RATE_MULT_1X);
+                break;
+        case 2:
+                psb_intel_sdvo_set_clock_rate_mult(psb_intel_output,
+                                               SDVO_CLOCK_RATE_MULT_2X);
+                break;
+        case 4:
+                psb_intel_sdvo_set_clock_rate_mult(psb_intel_output,
+                                               SDVO_CLOCK_RATE_MULT_4X);
+                break;
+        }
+
+        /* Set the SDVO control regs. */
+        sdvox = REG_READ(sdvo_priv->output_device);
+        switch (sdvo_priv->output_device) {
+        case SDVOB:
+                sdvox &= SDVOB_PRESERVE_MASK;
+                break;
+        case SDVOC:
+                sdvox &= SDVOC_PRESERVE_MASK;
+                break;
+        }
+        sdvox |= (9 << 19) | SDVO_BORDER_ENABLE;
+        if (psb_intel_crtc->pipe == 1)
+                sdvox |= SDVO_PIPE_B_SELECT;
+
+        sdvo_pixel_multiply = psb_intel_sdvo_get_pixel_multiplier(mode);
+
+        psb_intel_sdvo_write_sdvox(psb_intel_output, sdvox);
+
+         psb_intel_sdvo_set_iomap(psb_intel_output);
+}
+
+static void psb_intel_sdvo_dpms(struct drm_encoder *encoder, int mode)
+{
+        struct drm_device *dev = encoder->dev;
+        struct psb_intel_output *psb_intel_output =
+                                        enc_to_psb_intel_output(encoder);
+        struct psb_intel_sdvo_priv *sdvo_priv = psb_intel_output->dev_priv;
+        u32 temp;
+
+        if (mode != DRM_MODE_DPMS_ON) {
+                psb_intel_sdvo_set_active_outputs(psb_intel_output, 0);
+                if (0)
+                        psb_intel_sdvo_set_encoder_power_state(
+                                                        psb_intel_output,
+                                                        mode);
+
+                if (mode == DRM_MODE_DPMS_OFF) {
+                        temp = REG_READ(sdvo_priv->output_device);
+                        if ((temp & SDVO_ENABLE) != 0) {
+                                psb_intel_sdvo_write_sdvox(psb_intel_output,
+                                                       temp &
+                                                       ~SDVO_ENABLE);
+                        }
+                }
+        } else {
+                bool input1, input2;
+                int i;
+                u8 status;
+
+                temp = REG_READ(sdvo_priv->output_device);
+                if ((temp & SDVO_ENABLE) == 0)
+                        psb_intel_sdvo_write_sdvox(psb_intel_output,
+                                               temp | SDVO_ENABLE);
+                for (i = 0; i < 2; i++)
+                        psb_intel_wait_for_vblank(dev);
+
+                status =
+                    psb_intel_sdvo_get_trained_inputs(psb_intel_output,
+                                                        &input1,
+                                                        &input2);
+
+
+                /* Warn if the device reported failure to sync.
+                 * A lot of SDVO devices fail to notify of sync, but it's
+                 * a given it the status is a success, we succeeded.
+                 */
+                if (status == SDVO_CMD_STATUS_SUCCESS && !input1) {
+                        DRM_DEBUG
+                            ("First %s output reported failure to sync\n",
+                             SDVO_NAME(sdvo_priv));
+                }
+
+                if (0)
+                        psb_intel_sdvo_set_encoder_power_state(
+                                                        psb_intel_output,
+                                                        mode);
+                psb_intel_sdvo_set_active_outputs(psb_intel_output,
+                                              sdvo_priv->active_outputs);
+        }
+        return;
+}
+
+static void psb_intel_sdvo_save(struct drm_connector *connector)
+{
+        struct drm_device *dev = connector->dev;
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+        struct psb_intel_sdvo_priv *sdvo_priv = psb_intel_output->dev_priv;
+        /*int o;*/
+
+        sdvo_priv->save_sdvo_mult =
+            psb_intel_sdvo_get_clock_rate_mult(psb_intel_output);
+        psb_intel_sdvo_get_active_outputs(psb_intel_output,
+                                      &sdvo_priv->save_active_outputs);
+
+        if (sdvo_priv->caps.sdvo_inputs_mask & 0x1) {
+                psb_intel_sdvo_set_target_input(psb_intel_output,
+                                                true,
+                                                false);
+                psb_intel_sdvo_get_input_timing(psb_intel_output,
+                                            &sdvo_priv->save_input_dtd_1);
+        }
+
+        if (sdvo_priv->caps.sdvo_inputs_mask & 0x2) {
+                psb_intel_sdvo_set_target_input(psb_intel_output,
+                                                false,
+                                                true);
+                psb_intel_sdvo_get_input_timing(psb_intel_output,
+                                            &sdvo_priv->save_input_dtd_2);
+        }
+        sdvo_priv->save_SDVOX = REG_READ(sdvo_priv->output_device);
+
+        /*TODO: save the in_out_map state*/
+}
+
+static void psb_intel_sdvo_restore(struct drm_connector *connector)
+{
+        struct drm_device *dev = connector->dev;
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+        struct psb_intel_sdvo_priv *sdvo_priv = psb_intel_output->dev_priv;
+        /*int o;*/
+        int i;
+        bool input1, input2;
+        u8 status;
+
+        psb_intel_sdvo_set_active_outputs(psb_intel_output, 0);
+
+        if (sdvo_priv->caps.sdvo_inputs_mask & 0x1) {
+                psb_intel_sdvo_set_target_input(psb_intel_output, true, false);
+                psb_intel_sdvo_set_input_timing(psb_intel_output,
+                                            &sdvo_priv->save_input_dtd_1);
+        }
+
+        if (sdvo_priv->caps.sdvo_inputs_mask & 0x2) {
+                psb_intel_sdvo_set_target_input(psb_intel_output, false, true);
+                psb_intel_sdvo_set_input_timing(psb_intel_output,
+                                            &sdvo_priv->save_input_dtd_2);
+        }
+
+        psb_intel_sdvo_set_clock_rate_mult(psb_intel_output,
+                                       sdvo_priv->save_sdvo_mult);
+
+        REG_WRITE(sdvo_priv->output_device, sdvo_priv->save_SDVOX);
+
+        if (sdvo_priv->save_SDVOX & SDVO_ENABLE) {
+                for (i = 0; i < 2; i++)
+                        psb_intel_wait_for_vblank(dev);
+                status =
+                    psb_intel_sdvo_get_trained_inputs(psb_intel_output,
+                                                        &input1,
+                                                        &input2);
+                if (status == SDVO_CMD_STATUS_SUCCESS && !input1)
+                        DRM_DEBUG
+                            ("First %s output reported failure to sync\n",
+                             SDVO_NAME(sdvo_priv));
+        }
+
+        psb_intel_sdvo_set_active_outputs(psb_intel_output,
+                                      sdvo_priv->save_active_outputs);
+
+        /*TODO: restore in_out_map*/
+        psb_intel_sdvo_write_cmd(psb_intel_output,
+                                 SDVO_CMD_SET_IN_OUT_MAP,
+                                 sdvo_priv->in_out_map,
+                                 4);
+
+        psb_intel_sdvo_read_response(psb_intel_output, NULL, 0);
+}
+
+static int psb_intel_sdvo_mode_valid(struct drm_connector *connector,
+                                 struct drm_display_mode *mode)
+{
+        struct psb_intel_output *psb_intel_output =
+                                to_psb_intel_output(connector);
+        struct psb_intel_sdvo_priv *sdvo_priv = psb_intel_output->dev_priv;
+
+        if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+                return MODE_NO_DBLESCAN;
+
+        if (sdvo_priv->pixel_clock_min > mode->clock)
+                return MODE_CLOCK_LOW;
+
+        if (sdvo_priv->pixel_clock_max < mode->clock)
+                return MODE_CLOCK_HIGH;
+
+        return MODE_OK;
+}
+
+static bool psb_intel_sdvo_get_capabilities(
+                                struct psb_intel_output *psb_intel_output,
+                                struct psb_intel_sdvo_caps *caps)
+{
+        u8 status;
+
+        psb_intel_sdvo_write_cmd(psb_intel_output,
+                                 SDVO_CMD_GET_DEVICE_CAPS,
+                                 NULL,
+                                 0);
+        status = psb_intel_sdvo_read_response(psb_intel_output,
+                                                caps,
+                                                sizeof(*caps));
+        if (status != SDVO_CMD_STATUS_SUCCESS)
+                return false;
+
+        return true;
+}
+
+struct drm_connector *psb_intel_sdvo_find(struct drm_device *dev, int sdvoB)
+{
+        struct drm_connector *connector = NULL;
+        struct psb_intel_output *iout = NULL;
+        struct psb_intel_sdvo_priv *sdvo;
+
+        /* find the sdvo connector */
+        list_for_each_entry(connector, &dev->mode_config.connector_list,
+                            head) {
+                iout = to_psb_intel_output(connector);
+
+                if (iout->type != INTEL_OUTPUT_SDVO)
+                        continue;
+
+                sdvo = iout->dev_priv;
+
+                if (sdvo->output_device == SDVOB && sdvoB)
+                        return connector;
+
+                if (sdvo->output_device == SDVOC && !sdvoB)
+                        return connector;
+
+        }
+
+        return NULL;
+}
+
+int psb_intel_sdvo_supports_hotplug(struct drm_connector *connector)
+{
+        u8 response[2];
+        u8 status;
+        struct psb_intel_output *psb_intel_output;
+
+        if (!connector)
+                return 0;
+
+        psb_intel_output = to_psb_intel_output(connector);
+
+        psb_intel_sdvo_write_cmd(psb_intel_output,
+                                 SDVO_CMD_GET_HOT_PLUG_SUPPORT,
+                                 NULL,
+                                 0);
+        status = psb_intel_sdvo_read_response(psb_intel_output,
+                                                &response,
+                                                2);
+
+        if (response[0] != 0)
+                return 1;
+
+        return 0;
+}
+
+void psb_intel_sdvo_set_hotplug(struct drm_connector *connector, int on)
+{
+        u8 response[2];
+        u8 status;
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+
+        psb_intel_sdvo_write_cmd(psb_intel_output,
+                                 SDVO_CMD_GET_ACTIVE_HOT_PLUG,
+                                 NULL,
+                                 0);
+        psb_intel_sdvo_read_response(psb_intel_output, &response, 2);
+
+        if (on) {
+                psb_intel_sdvo_write_cmd(psb_intel_output,
+                                     SDVO_CMD_GET_HOT_PLUG_SUPPORT, NULL,
+                                     0);
+                status = psb_intel_sdvo_read_response(psb_intel_output,
+                                                      &response,
+                                                      2);
+
+                psb_intel_sdvo_write_cmd(psb_intel_output,
+                                     SDVO_CMD_SET_ACTIVE_HOT_PLUG,
+                                     &response, 2);
+        } else {
+                response[0] = 0;
+                response[1] = 0;
+                psb_intel_sdvo_write_cmd(psb_intel_output,
+                                     SDVO_CMD_SET_ACTIVE_HOT_PLUG,
+                                     &response, 2);
+        }
+
+        psb_intel_sdvo_write_cmd(psb_intel_output,
+                                 SDVO_CMD_GET_ACTIVE_HOT_PLUG,
+                                 NULL,
+                                 0);
+        psb_intel_sdvo_read_response(psb_intel_output, &response, 2);
+}
+
+static enum drm_connector_status psb_intel_sdvo_detect(struct drm_connector
+                                                   *connector, bool force)
+{
+        u8 response[2];
+        u8 status;
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+
+        psb_intel_sdvo_write_cmd(psb_intel_output,
+                                 SDVO_CMD_GET_ATTACHED_DISPLAYS,
+                                 NULL,
+                                 0);
+        status = psb_intel_sdvo_read_response(psb_intel_output, &response, 2);
+
+        DRM_DEBUG("SDVO response %d %d\n", response[0], response[1]);
+        if ((response[0] != 0) || (response[1] != 0))
+                return connector_status_connected;
+        else
+                return connector_status_disconnected;
+}
+
+static int psb_intel_sdvo_get_modes(struct drm_connector *connector)
+{
+        struct psb_intel_output *psb_intel_output =
+                                        to_psb_intel_output(connector);
+
+        /* set the bus switch and get the modes */
+        psb_intel_sdvo_set_control_bus_switch(psb_intel_output,
+                                          SDVO_CONTROL_BUS_DDC2);
+        psb_intel_ddc_get_modes(psb_intel_output);
+
+        if (list_empty(&connector->probed_modes))
+                return 0;
+        return 1;
+}
+
+static void psb_intel_sdvo_destroy(struct drm_connector *connector)
+{
+        struct psb_intel_output *psb_intel_output =
+                                to_psb_intel_output(connector);
+
+        if (psb_intel_output->i2c_bus)
+                psb_intel_i2c_destroy(psb_intel_output->i2c_bus);
+        drm_sysfs_connector_remove(connector);
+        drm_connector_cleanup(connector);
+        kfree(psb_intel_output);
+}
+
+static const struct drm_encoder_helper_funcs psb_intel_sdvo_helper_funcs = {
+        .dpms = psb_intel_sdvo_dpms,
+        .mode_fixup = psb_intel_sdvo_mode_fixup,
+        .prepare = psb_intel_encoder_prepare,
+        .mode_set = psb_intel_sdvo_mode_set,
+        .commit = psb_intel_encoder_commit,
+};
+
+static const struct drm_connector_funcs psb_intel_sdvo_connector_funcs = {
+        .dpms = drm_helper_connector_dpms,
+        .save = psb_intel_sdvo_save,
+        .restore = psb_intel_sdvo_restore,
+        .detect = psb_intel_sdvo_detect,
+        .fill_modes = drm_helper_probe_single_connector_modes,
+        .destroy = psb_intel_sdvo_destroy,
+};
+
+static const struct drm_connector_helper_funcs
+                                psb_intel_sdvo_connector_helper_funcs = {
+        .get_modes = psb_intel_sdvo_get_modes,
+        .mode_valid = psb_intel_sdvo_mode_valid,
+        .best_encoder = psb_intel_best_encoder,
+};
+
+void psb_intel_sdvo_enc_destroy(struct drm_encoder *encoder)
+{
+        drm_encoder_cleanup(encoder);
+}
+
+static const struct drm_encoder_funcs psb_intel_sdvo_enc_funcs = {
+        .destroy = psb_intel_sdvo_enc_destroy,
+};
+
+
+void psb_intel_sdvo_init(struct drm_device *dev, int output_device)
+{
+        struct drm_connector *connector;
+        struct psb_intel_output *psb_intel_output;
+        struct psb_intel_sdvo_priv *sdvo_priv;
+        struct psb_intel_i2c_chan *i2cbus = NULL;
+        int connector_type;
+        u8 ch[0x40];
+        int i;
+        int encoder_type, output_id;
+
+        psb_intel_output =
+            kcalloc(sizeof(struct psb_intel_output) +
+                    sizeof(struct psb_intel_sdvo_priv), 1, GFP_KERNEL);
+        if (!psb_intel_output)
+                return;
+
+        connector = &psb_intel_output->base;
+
+        drm_connector_init(dev, connector, &psb_intel_sdvo_connector_funcs,
+                           DRM_MODE_CONNECTOR_Unknown);
+        drm_connector_helper_add(connector,
+                                 &psb_intel_sdvo_connector_helper_funcs);
+        sdvo_priv = (struct psb_intel_sdvo_priv *) (psb_intel_output + 1);
+        psb_intel_output->type = INTEL_OUTPUT_SDVO;
+
+        connector->interlace_allowed = 0;
+        connector->doublescan_allowed = 0;
+
+        /* setup the DDC bus. */
+        if (output_device == SDVOB)
+                i2cbus =
+                    psb_intel_i2c_create(dev, GPIOE, "SDVOCTRL_E for SDVOB");
+        else
+                i2cbus =
+                    psb_intel_i2c_create(dev, GPIOE, "SDVOCTRL_E for SDVOC");
+
+        if (!i2cbus)
+                goto err_connector;
+
+        sdvo_priv->i2c_bus = i2cbus;
+
+        if (output_device == SDVOB) {
+                output_id = 1;
+                sdvo_priv->by_input_wiring = SDVOB_IN0;
+                sdvo_priv->i2c_bus->slave_addr = 0x38;
+        } else {
+                output_id = 2;
+                sdvo_priv->i2c_bus->slave_addr = 0x39;
+        }
+
+        sdvo_priv->output_device = output_device;
+        psb_intel_output->i2c_bus = i2cbus;
+        psb_intel_output->dev_priv = sdvo_priv;
+
+
+        /* Read the regs to test if we can talk to the device */
+        for (i = 0; i < 0x40; i++) {
+                if (!psb_intel_sdvo_read_byte(psb_intel_output, i, &ch[i])) {
+                        dev_dbg(dev->dev, "No SDVO device found on SDVO%c\n",
+                                  output_device == SDVOB ? 'B' : 'C');
+                        goto err_i2c;
+                }
+        }
+
+        psb_intel_sdvo_get_capabilities(psb_intel_output, &sdvo_priv->caps);
+
+        memset(&sdvo_priv->active_outputs, 0,
+               sizeof(sdvo_priv->active_outputs));
+
+        /* TODO, CVBS, SVID, YPRPB & SCART outputs. */
+        if (sdvo_priv->caps.output_flags & SDVO_OUTPUT_RGB0) {
+                sdvo_priv->active_outputs = SDVO_OUTPUT_RGB0;
+                sdvo_priv->active_device = SDVO_DEVICE_CRT;
+                connector->display_info.subpixel_order =
+                    SubPixelHorizontalRGB;
+                encoder_type = DRM_MODE_ENCODER_DAC;
+                connector_type = DRM_MODE_CONNECTOR_VGA;
+        } else if (sdvo_priv->caps.output_flags & SDVO_OUTPUT_RGB1) {
+                sdvo_priv->active_outputs = SDVO_OUTPUT_RGB1;
+                sdvo_priv->active_outputs = SDVO_DEVICE_CRT;
+                connector->display_info.subpixel_order =
+                    SubPixelHorizontalRGB;
+                encoder_type = DRM_MODE_ENCODER_DAC;
+                connector_type = DRM_MODE_CONNECTOR_VGA;
+        } else if (sdvo_priv->caps.output_flags & SDVO_OUTPUT_TMDS0) {
+                sdvo_priv->active_outputs = SDVO_OUTPUT_TMDS0;
+                sdvo_priv->active_device = SDVO_DEVICE_TMDS;
+                connector->display_info.subpixel_order =
+                    SubPixelHorizontalRGB;
+                encoder_type = DRM_MODE_ENCODER_TMDS;
+                connector_type = DRM_MODE_CONNECTOR_DVID;
+        } else if (sdvo_priv->caps.output_flags & SDVO_OUTPUT_TMDS1) {
+                sdvo_priv->active_outputs = SDVO_OUTPUT_TMDS1;
+                sdvo_priv->active_device = SDVO_DEVICE_TMDS;
+                connector->display_info.subpixel_order =
+                    SubPixelHorizontalRGB;
+                encoder_type = DRM_MODE_ENCODER_TMDS;
+                connector_type = DRM_MODE_CONNECTOR_DVID;
+        } else {
+                unsigned char bytes[2];
+
+                memcpy(bytes, &sdvo_priv->caps.output_flags, 2);
+                dev_dbg(dev->dev, "%s: No active RGB or TMDS outputs (0x%02x%02x)\n",
+                     SDVO_NAME(sdvo_priv), bytes[0], bytes[1]);
+                goto err_i2c;
+        }
+
+        drm_encoder_init(dev, &psb_intel_output->enc, &psb_intel_sdvo_enc_funcs,
+                         encoder_type);
+        drm_encoder_helper_add(&psb_intel_output->enc,
+                               &psb_intel_sdvo_helper_funcs);
+        connector->connector_type = connector_type;
+
+        drm_mode_connector_attach_encoder(&psb_intel_output->base,
+                                          &psb_intel_output->enc);
+        drm_sysfs_connector_add(connector);
+
+        /* Set the input timing to the screen. Assume always input 0. */
+        psb_intel_sdvo_set_target_input(psb_intel_output, true, false);
+
+        psb_intel_sdvo_get_input_pixel_clock_range(psb_intel_output,
+                                               &sdvo_priv->pixel_clock_min,
+                                               &sdvo_priv->
+                                               pixel_clock_max);
+
+
+        dev_dbg(dev->dev, "%s device VID/DID: %02X:%02X.%02X, "
+                  "clock range %dMHz - %dMHz, "
+                  "input 1: %c, input 2: %c, "
+                  "output 1: %c, output 2: %c\n",
+                  SDVO_NAME(sdvo_priv),
+                  sdvo_priv->caps.vendor_id, sdvo_priv->caps.device_id,
+                  sdvo_priv->caps.device_rev_id,
+                  sdvo_priv->pixel_clock_min / 1000,
+                  sdvo_priv->pixel_clock_max / 1000,
+                  (sdvo_priv->caps.sdvo_inputs_mask & 0x1) ? 'Y' : 'N',
+                  (sdvo_priv->caps.sdvo_inputs_mask & 0x2) ? 'Y' : 'N',
+                  /* check currently supported outputs */
+                  sdvo_priv->caps.output_flags &
+                  (SDVO_OUTPUT_TMDS0 | SDVO_OUTPUT_RGB0) ? 'Y' : 'N',
+                  sdvo_priv->caps.output_flags &
+                  (SDVO_OUTPUT_TMDS1 | SDVO_OUTPUT_RGB1) ? 'Y' : 'N');
+
+        psb_intel_output->ddc_bus = i2cbus;
+
+        return;
+
+err_i2c:
+        psb_intel_i2c_destroy(psb_intel_output->i2c_bus);
+err_connector:
+        drm_connector_cleanup(connector);
+        kfree(psb_intel_output);
+
+        return;
+}
diff --git a/drivers/staging/gma500/psb_intel_sdvo_regs.h b/drivers/staging/gma500/psb_intel_sdvo_regs.h
new file mode 100644
index 00000000000..96862ea65ab
--- /dev/null
+++ b/drivers/staging/gma500/psb_intel_sdvo_regs.h
@@ -0,0 +1,338 @@
+/*
+ * SDVO command definitions and structures.
+ *
+ * Copyright (c) 2008, Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors:
+ *      Eric Anholt <eric@anholt.net>
+ */
+
+#define SDVO_OUTPUT_FIRST   (0)
+#define SDVO_OUTPUT_TMDS0   (1 << 0)
+#define SDVO_OUTPUT_RGB0    (1 << 1)
+#define SDVO_OUTPUT_CVBS0   (1 << 2)
+#define SDVO_OUTPUT_SVID0   (1 << 3)
+#define SDVO_OUTPUT_YPRPB0  (1 << 4)
+#define SDVO_OUTPUT_SCART0  (1 << 5)
+#define SDVO_OUTPUT_LVDS0   (1 << 6)
+#define SDVO_OUTPUT_TMDS1   (1 << 8)
+#define SDVO_OUTPUT_RGB1    (1 << 9)
+#define SDVO_OUTPUT_CVBS1   (1 << 10)
+#define SDVO_OUTPUT_SVID1   (1 << 11)
+#define SDVO_OUTPUT_YPRPB1  (1 << 12)
+#define SDVO_OUTPUT_SCART1  (1 << 13)
+#define SDVO_OUTPUT_LVDS1   (1 << 14)
+#define SDVO_OUTPUT_LAST    (14)
+
+struct psb_intel_sdvo_caps {
+        u8 vendor_id;
+        u8 device_id;
+        u8 device_rev_id;
+        u8 sdvo_version_major;
+        u8 sdvo_version_minor;
+        unsigned int sdvo_inputs_mask:2;
+        unsigned int smooth_scaling:1;
+        unsigned int sharp_scaling:1;
+        unsigned int up_scaling:1;
+        unsigned int down_scaling:1;
+        unsigned int stall_support:1;
+        unsigned int pad:1;
+        u16 output_flags;
+} __packed;
+
+/** This matches the EDID DTD structure, more or less */
+struct psb_intel_sdvo_dtd {
+        struct {
+                u16 clock;      /**< pixel clock, in 10kHz units */
+                u8 h_active;    /**< lower 8 bits (pixels) */
+                u8 h_blank;     /**< lower 8 bits (pixels) */
+                u8 h_high;      /**< upper 4 bits each h_active, h_blank */
+                u8 v_active;    /**< lower 8 bits (lines) */
+                u8 v_blank;     /**< lower 8 bits (lines) */
+                u8 v_high;      /**< upper 4 bits each v_active, v_blank */
+        } part1;
+
+        struct {
+                u8 h_sync_off;
+                        /**< lower 8 bits, from hblank start */
+                u8 h_sync_width;/**< lower 8 bits (pixels) */
+        /** lower 4 bits each vsync offset, vsync width */
+                u8 v_sync_off_width;
+        /**
+         * 2 high bits of hsync offset, 2 high bits of hsync width,
+         * bits 4-5 of vsync offset, and 2 high bits of vsync width.
+         */
+                u8 sync_off_width_high;
+                u8 dtd_flags;
+                u8 sdvo_flags;
+        /** bits 6-7 of vsync offset at bits 6-7 */
+                u8 v_sync_off_high;
+                u8 reserved;
+        } part2;
+} __packed;
+
+struct psb_intel_sdvo_pixel_clock_range {
+        u16 min;                /**< pixel clock, in 10kHz units */
+        u16 max;                /**< pixel clock, in 10kHz units */
+} __packed;
+
+struct psb_intel_sdvo_preferred_input_timing_args {
+        u16 clock;
+        u16 width;
+        u16 height;
+} __packed;
+
+/* I2C registers for SDVO */
+#define SDVO_I2C_ARG_0                          0x07
+#define SDVO_I2C_ARG_1                          0x06
+#define SDVO_I2C_ARG_2                          0x05
+#define SDVO_I2C_ARG_3                          0x04
+#define SDVO_I2C_ARG_4                          0x03
+#define SDVO_I2C_ARG_5                          0x02
+#define SDVO_I2C_ARG_6                          0x01
+#define SDVO_I2C_ARG_7                          0x00
+#define SDVO_I2C_OPCODE                         0x08
+#define SDVO_I2C_CMD_STATUS                     0x09
+#define SDVO_I2C_RETURN_0                       0x0a
+#define SDVO_I2C_RETURN_1                       0x0b
+#define SDVO_I2C_RETURN_2                       0x0c
+#define SDVO_I2C_RETURN_3                       0x0d
+#define SDVO_I2C_RETURN_4                       0x0e
+#define SDVO_I2C_RETURN_5                       0x0f
+#define SDVO_I2C_RETURN_6                       0x10
+#define SDVO_I2C_RETURN_7                       0x11
+#define SDVO_I2C_VENDOR_BEGIN                   0x20
+
+/* Status results */
+#define SDVO_CMD_STATUS_POWER_ON                0x0
+#define SDVO_CMD_STATUS_SUCCESS                 0x1
+#define SDVO_CMD_STATUS_NOTSUPP                 0x2
+#define SDVO_CMD_STATUS_INVALID_ARG             0x3
+#define SDVO_CMD_STATUS_PENDING                 0x4
+#define SDVO_CMD_STATUS_TARGET_NOT_SPECIFIED    0x5
+#define SDVO_CMD_STATUS_SCALING_NOT_SUPP        0x6
+
+/* SDVO commands, argument/result registers */
+
+#define SDVO_CMD_RESET                                  0x01
+
+/** Returns a struct psb_intel_sdvo_caps */
+#define SDVO_CMD_GET_DEVICE_CAPS                        0x02
+
+#define SDVO_CMD_GET_FIRMWARE_REV                       0x86
+# define SDVO_DEVICE_FIRMWARE_MINOR                     SDVO_I2C_RETURN_0
+# define SDVO_DEVICE_FIRMWARE_MAJOR                     SDVO_I2C_RETURN_1
+# define SDVO_DEVICE_FIRMWARE_PATCH                     SDVO_I2C_RETURN_2
+
+/**
+ * Reports which inputs are trained (managed to sync).
+ *
+ * Devices must have trained within 2 vsyncs of a mode change.
+ */
+#define SDVO_CMD_GET_TRAINED_INPUTS                     0x03
+struct psb_intel_sdvo_get_trained_inputs_response {
+        unsigned int input0_trained:1;
+        unsigned int input1_trained:1;
+        unsigned int pad:6;
+} __packed;
+
+/** Returns a struct psb_intel_sdvo_output_flags of active outputs. */
+#define SDVO_CMD_GET_ACTIVE_OUTPUTS                     0x04
+
+/**
+ * Sets the current set of active outputs.
+ *
+ * Takes a struct psb_intel_sdvo_output_flags.
+ * Must be preceded by a SET_IN_OUT_MAP
+ * on multi-output devices.
+ */
+#define SDVO_CMD_SET_ACTIVE_OUTPUTS                     0x05
+
+/**
+ * Returns the current mapping of SDVO inputs to outputs on the device.
+ *
+ * Returns two struct psb_intel_sdvo_output_flags structures.
+ */
+#define SDVO_CMD_GET_IN_OUT_MAP                         0x06
+
+/**
+ * Sets the current mapping of SDVO inputs to outputs on the device.
+ *
+ * Takes two struct i380_sdvo_output_flags structures.
+ */
+#define SDVO_CMD_SET_IN_OUT_MAP                         0x07
+
+/**
+ * Returns a struct psb_intel_sdvo_output_flags of attached displays.
+ */
+#define SDVO_CMD_GET_ATTACHED_DISPLAYS                  0x0b
+
+/**
+ * Returns a struct psb_intel_sdvo_ouptut_flags of displays supporting hot plugging.
+ */
+#define SDVO_CMD_GET_HOT_PLUG_SUPPORT                   0x0c
+
+/**
+ * Takes a struct psb_intel_sdvo_output_flags.
+ */
+#define SDVO_CMD_SET_ACTIVE_HOT_PLUG                    0x0d
+
+/**
+ * Returns a struct psb_intel_sdvo_output_flags of displays with hot plug
+ * interrupts enabled.
+ */
+#define SDVO_CMD_GET_ACTIVE_HOT_PLUG                    0x0e
+
+#define SDVO_CMD_GET_INTERRUPT_EVENT_SOURCE             0x0f
+struct psb_intel_sdvo_get_interrupt_event_source_response {
+        u16 interrupt_status;
+        unsigned int ambient_light_interrupt:1;
+        unsigned int pad:7;
+} __packed;
+
+/**
+ * Selects which input is affected by future input commands.
+ *
+ * Commands affected include SET_INPUT_TIMINGS_PART[12],
+ * GET_INPUT_TIMINGS_PART[12], GET_PREFERRED_INPUT_TIMINGS_PART[12],
+ * GET_INPUT_PIXEL_CLOCK_RANGE, and CREATE_PREFERRED_INPUT_TIMINGS.
+ */
+#define SDVO_CMD_SET_TARGET_INPUT                       0x10
+struct psb_intel_sdvo_set_target_input_args {
+        unsigned int target_1:1;
+        unsigned int pad:7;
+} __packed;
+
+/**
+ * Takes a struct psb_intel_sdvo_output_flags of which outputs are targeted by
+ * future output commands.
+ *
+ * Affected commands inclue SET_OUTPUT_TIMINGS_PART[12],
+ * GET_OUTPUT_TIMINGS_PART[12], and GET_OUTPUT_PIXEL_CLOCK_RANGE.
+ */
+#define SDVO_CMD_SET_TARGET_OUTPUT                      0x11
+
+#define SDVO_CMD_GET_INPUT_TIMINGS_PART1                0x12
+#define SDVO_CMD_GET_INPUT_TIMINGS_PART2                0x13
+#define SDVO_CMD_SET_INPUT_TIMINGS_PART1                0x14
+#define SDVO_CMD_SET_INPUT_TIMINGS_PART2                0x15
+#define SDVO_CMD_SET_OUTPUT_TIMINGS_PART1               0x16
+#define SDVO_CMD_SET_OUTPUT_TIMINGS_PART2               0x17
+#define SDVO_CMD_GET_OUTPUT_TIMINGS_PART1               0x18
+#define SDVO_CMD_GET_OUTPUT_TIMINGS_PART2               0x19
+/* Part 1 */
+# define SDVO_DTD_CLOCK_LOW                             SDVO_I2C_ARG_0
+# define SDVO_DTD_CLOCK_HIGH                            SDVO_I2C_ARG_1
+# define SDVO_DTD_H_ACTIVE                              SDVO_I2C_ARG_2
+# define SDVO_DTD_H_BLANK                               SDVO_I2C_ARG_3
+# define SDVO_DTD_H_HIGH                                SDVO_I2C_ARG_4
+# define SDVO_DTD_V_ACTIVE                              SDVO_I2C_ARG_5
+# define SDVO_DTD_V_BLANK                               SDVO_I2C_ARG_6
+# define SDVO_DTD_V_HIGH                                SDVO_I2C_ARG_7
+/* Part 2 */
+# define SDVO_DTD_HSYNC_OFF                             SDVO_I2C_ARG_0
+# define SDVO_DTD_HSYNC_WIDTH                           SDVO_I2C_ARG_1
+# define SDVO_DTD_VSYNC_OFF_WIDTH                       SDVO_I2C_ARG_2
+# define SDVO_DTD_SYNC_OFF_WIDTH_HIGH                   SDVO_I2C_ARG_3
+# define SDVO_DTD_DTD_FLAGS                             SDVO_I2C_ARG_4
+# define SDVO_DTD_DTD_FLAG_INTERLACED                           (1 << 7)
+# define SDVO_DTD_DTD_FLAG_STEREO_MASK                          (3 << 5)
+# define SDVO_DTD_DTD_FLAG_INPUT_MASK                           (3 << 3)
+# define SDVO_DTD_DTD_FLAG_SYNC_MASK                            (3 << 1)
+# define SDVO_DTD_SDVO_FLAS                             SDVO_I2C_ARG_5
+# define SDVO_DTD_SDVO_FLAG_STALL                               (1 << 7)
+# define SDVO_DTD_SDVO_FLAG_CENTERED                            (0 << 6)
+# define SDVO_DTD_SDVO_FLAG_UPPER_LEFT                          (1 << 6)
+# define SDVO_DTD_SDVO_FLAG_SCALING_MASK                        (3 << 4)
+# define SDVO_DTD_SDVO_FLAG_SCALING_NONE                        (0 << 4)
+# define SDVO_DTD_SDVO_FLAG_SCALING_SHARP                       (1 << 4)
+# define SDVO_DTD_SDVO_FLAG_SCALING_SMOOTH                      (2 << 4)
+# define SDVO_DTD_VSYNC_OFF_HIGH                        SDVO_I2C_ARG_6
+
+/**
+ * Generates a DTD based on the given width, height, and flags.
+ *
+ * This will be supported by any device supporting scaling or interlaced
+ * modes.
+ */
+#define SDVO_CMD_CREATE_PREFERRED_INPUT_TIMING          0x1a
+# define SDVO_PREFERRED_INPUT_TIMING_CLOCK_LOW          SDVO_I2C_ARG_0
+# define SDVO_PREFERRED_INPUT_TIMING_CLOCK_HIGH         SDVO_I2C_ARG_1
+# define SDVO_PREFERRED_INPUT_TIMING_WIDTH_LOW          SDVO_I2C_ARG_2
+# define SDVO_PREFERRED_INPUT_TIMING_WIDTH_HIGH         SDVO_I2C_ARG_3
+# define SDVO_PREFERRED_INPUT_TIMING_HEIGHT_LOW         SDVO_I2C_ARG_4
+# define SDVO_PREFERRED_INPUT_TIMING_HEIGHT_HIGH        SDVO_I2C_ARG_5
+# define SDVO_PREFERRED_INPUT_TIMING_FLAGS              SDVO_I2C_ARG_6
+# define SDVO_PREFERRED_INPUT_TIMING_FLAGS_INTERLACED           (1 << 0)
+# define SDVO_PREFERRED_INPUT_TIMING_FLAGS_SCALED               (1 << 1)
+
+#define SDVO_CMD_GET_PREFERRED_INPUT_TIMING_PART1       0x1b
+#define SDVO_CMD_GET_PREFERRED_INPUT_TIMING_PART2       0x1c
+
+/** Returns a struct psb_intel_sdvo_pixel_clock_range */
+#define SDVO_CMD_GET_INPUT_PIXEL_CLOCK_RANGE            0x1d
+/** Returns a struct psb_intel_sdvo_pixel_clock_range */
+#define SDVO_CMD_GET_OUTPUT_PIXEL_CLOCK_RANGE           0x1e
+
+/** Returns a byte bitfield containing SDVO_CLOCK_RATE_MULT_* flags */
+#define SDVO_CMD_GET_SUPPORTED_CLOCK_RATE_MULTS         0x1f
+
+/** Returns a byte containing a SDVO_CLOCK_RATE_MULT_* flag */
+#define SDVO_CMD_GET_CLOCK_RATE_MULT                    0x20
+/** Takes a byte containing a SDVO_CLOCK_RATE_MULT_* flag */
+#define SDVO_CMD_SET_CLOCK_RATE_MULT                    0x21
+# define SDVO_CLOCK_RATE_MULT_1X                                (1 << 0)
+# define SDVO_CLOCK_RATE_MULT_2X                                (1 << 1)
+# define SDVO_CLOCK_RATE_MULT_4X                                (1 << 3)
+
+#define SDVO_CMD_GET_SUPPORTED_TV_FORMATS               0x27
+
+#define SDVO_CMD_GET_TV_FORMAT                          0x28
+
+#define SDVO_CMD_SET_TV_FORMAT                          0x29
+
+#define SDVO_CMD_GET_SUPPORTED_POWER_STATES             0x2a
+#define SDVO_CMD_GET_ENCODER_POWER_STATE                0x2b
+#define SDVO_CMD_SET_ENCODER_POWER_STATE                0x2c
+# define SDVO_ENCODER_STATE_ON                                  (1 << 0)
+# define SDVO_ENCODER_STATE_STANDBY                             (1 << 1)
+# define SDVO_ENCODER_STATE_SUSPEND                             (1 << 2)
+# define SDVO_ENCODER_STATE_OFF                                 (1 << 3)
+
+#define SDVO_CMD_SET_TV_RESOLUTION_SUPPORT              0x93
+
+#define SDVO_CMD_SET_CONTROL_BUS_SWITCH                 0x7a
+# define SDVO_CONTROL_BUS_PROM                          0x0
+# define SDVO_CONTROL_BUS_DDC1                          0x1
+# define SDVO_CONTROL_BUS_DDC2                          0x2
+# define SDVO_CONTROL_BUS_DDC3                          0x3
+
+/* SDVO Bus & SDVO Inputs wiring details*/
+/* Bit 0: Is SDVOB connected to In0 (1 = yes, 0 = no*/
+/* Bit 1: Is SDVOB connected to In1 (1 = yes, 0 = no*/
+/* Bit 2: Is SDVOC connected to In0 (1 = yes, 0 = no*/
+/* Bit 3: Is SDVOC connected to In1 (1 = yes, 0 = no*/
+#define SDVOB_IN0 0x01
+#define SDVOB_IN1 0x02
+#define SDVOC_IN0 0x04
+#define SDVOC_IN1 0x08
+
+#define SDVO_DEVICE_NONE 0x00
+#define        SDVO_DEVICE_CRT 0x01
+#define        SDVO_DEVICE_TV 0x02
+#define        SDVO_DEVICE_LVDS 0x04
+#define        SDVO_DEVICE_TMDS 0x08
+
diff --git a/drivers/staging/gma500/psb_irq.c b/drivers/staging/gma500/psb_irq.c
new file mode 100644
index 00000000000..4a0fa42893f
--- /dev/null
+++ b/drivers/staging/gma500/psb_irq.c
@@ -0,0 +1,616 @@
+/**************************************************************************
+ * Copyright (c) 2007, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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 funded Tungsten Graphics (http://www.tungstengraphics.com) to
+ * develop this driver.
+ *
+ **************************************************************************/
+/*
+ */
+
+#include <drm/drmP.h>
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include "power.h"
+#include "mdfld_output.h"
+
+/*
+ * inline functions
+ */
+
+static inline u32
+psb_pipestat(int pipe)
+{
+        if (pipe == 0)
+                return PIPEASTAT;
+        if (pipe == 1)
+                return PIPEBSTAT;
+        if (pipe == 2)
+                return PIPECSTAT;
+        BUG();
+}
+
+static inline u32
+mid_pipe_event(int pipe)
+{
+        if (pipe == 0)
+                return _PSB_PIPEA_EVENT_FLAG;
+        if (pipe == 1)
+                return _MDFLD_PIPEB_EVENT_FLAG;
+        if (pipe == 2)
+                return _MDFLD_PIPEC_EVENT_FLAG;
+        BUG();
+}
+
+static inline u32
+mid_pipe_vsync(int pipe)
+{
+        if (pipe == 0)
+                return _PSB_VSYNC_PIPEA_FLAG;
+        if (pipe == 1)
+                return _PSB_VSYNC_PIPEB_FLAG;
+        if (pipe == 2)
+                return _MDFLD_PIPEC_VBLANK_FLAG;
+        BUG();
+}
+
+static inline u32
+mid_pipeconf(int pipe)
+{
+        if (pipe == 0)
+                return PIPEACONF;
+        if (pipe == 1)
+                return PIPEBCONF;
+        if (pipe == 2)
+                return PIPECCONF;
+        BUG();
+}
+
+void
+psb_enable_pipestat(struct drm_psb_private *dev_priv, int pipe, u32 mask)
+{
+        if ((dev_priv->pipestat[pipe] & mask) != mask) {
+                u32 reg = psb_pipestat(pipe);
+                dev_priv->pipestat[pipe] |= mask;
+                /* Enable the interrupt, clear any pending status */
+                if (gma_power_begin(dev_priv->dev, false)) {
+                        u32 writeVal = PSB_RVDC32(reg);
+                        writeVal |= (mask | (mask >> 16));
+                        PSB_WVDC32(writeVal, reg);
+                        (void) PSB_RVDC32(reg);
+                        gma_power_end(dev_priv->dev);
+                }
+        }
+}
+
+void
+psb_disable_pipestat(struct drm_psb_private *dev_priv, int pipe, u32 mask)
+{
+        if ((dev_priv->pipestat[pipe] & mask) != 0) {
+                u32 reg = psb_pipestat(pipe);
+                dev_priv->pipestat[pipe] &= ~mask;
+                if (gma_power_begin(dev_priv->dev, false)) {
+                        u32 writeVal = PSB_RVDC32(reg);
+                        writeVal &= ~mask;
+                        PSB_WVDC32(writeVal, reg);
+                        (void) PSB_RVDC32(reg);
+                        gma_power_end(dev_priv->dev);
+                }
+        }
+}
+
+void mid_enable_pipe_event(struct drm_psb_private *dev_priv, int pipe)
+{
+        if (gma_power_begin(dev_priv->dev, false)) {
+                u32 pipe_event = mid_pipe_event(pipe);
+                dev_priv->vdc_irq_mask |= pipe_event;
+                PSB_WVDC32(~dev_priv->vdc_irq_mask, PSB_INT_MASK_R);
+                PSB_WVDC32(dev_priv->vdc_irq_mask, PSB_INT_ENABLE_R);
+                gma_power_end(dev_priv->dev);
+        }
+}
+
+void mid_disable_pipe_event(struct drm_psb_private *dev_priv, int pipe)
+{
+        if (dev_priv->pipestat[pipe] == 0) {
+                if (gma_power_begin(dev_priv->dev, false)) {
+                        u32 pipe_event = mid_pipe_event(pipe);
+                        dev_priv->vdc_irq_mask &= ~pipe_event;
+                        PSB_WVDC32(~dev_priv->vdc_irq_mask, PSB_INT_MASK_R);
+                        PSB_WVDC32(dev_priv->vdc_irq_mask, PSB_INT_ENABLE_R);
+                        gma_power_end(dev_priv->dev);
+                }
+        }
+}
+
+/**
+ * Display controller interrupt handler for vsync/vblank.
+ *
+ */
+static void mid_vblank_handler(struct drm_device *dev, uint32_t pipe)
+{
+        drm_handle_vblank(dev, pipe);
+}
+
+
+/**
+ * Display controller interrupt handler for pipe event.
+ *
+ */
+#define WAIT_STATUS_CLEAR_LOOP_COUNT 0xffff
+static void mid_pipe_event_handler(struct drm_device *dev, uint32_t pipe)
+{
+        struct drm_psb_private *dev_priv =
+            (struct drm_psb_private *) dev->dev_private;
+
+        uint32_t pipe_stat_val = 0;
+        uint32_t pipe_stat_reg = psb_pipestat(pipe);
+        uint32_t pipe_enable = dev_priv->pipestat[pipe];
+        uint32_t pipe_status = dev_priv->pipestat[pipe] >> 16;
+        uint32_t i = 0;
+
+        spin_lock(&dev_priv->irqmask_lock);
+
+        pipe_stat_val = PSB_RVDC32(pipe_stat_reg);
+        pipe_stat_val &= pipe_enable | pipe_status;
+        pipe_stat_val &= pipe_stat_val >> 16;
+
+        spin_unlock(&dev_priv->irqmask_lock);
+
+        /* clear the 2nd level interrupt status bits */
+        /**
+        * FIXME: shouldn't use while loop here. However, the interrupt
+        * status 'sticky' bits cannot be cleared by setting '1' to that
+        * bit once...
+        */
+        for (i = 0; i < WAIT_STATUS_CLEAR_LOOP_COUNT; i++) {
+                PSB_WVDC32(PSB_RVDC32(pipe_stat_reg), pipe_stat_reg);
+                (void) PSB_RVDC32(pipe_stat_reg);
+
+                if ((PSB_RVDC32(pipe_stat_reg) & pipe_status) == 0)
+                        break;
+        }
+
+        if (i == WAIT_STATUS_CLEAR_LOOP_COUNT)
+                dev_err(dev->dev,
+        "%s, can't clear the status bits in pipe_stat_reg, its value = 0x%x.\n",
+                        __func__, PSB_RVDC32(pipe_stat_reg));
+
+        if (pipe_stat_val & PIPE_VBLANK_STATUS)
+                mid_vblank_handler(dev, pipe);
+
+        if (pipe_stat_val & PIPE_TE_STATUS)
+                drm_handle_vblank(dev, pipe);
+}
+
+/*
+ * Display controller interrupt handler.
+ */
+static void psb_vdc_interrupt(struct drm_device *dev, uint32_t vdc_stat)
+{
+        if (vdc_stat & _PSB_PIPEA_EVENT_FLAG)
+                mid_pipe_event_handler(dev, 0);
+}
+
+irqreturn_t psb_irq_handler(DRM_IRQ_ARGS)
+{
+        struct drm_device *dev = (struct drm_device *) arg;
+        struct drm_psb_private *dev_priv =
+            (struct drm_psb_private *) dev->dev_private;
+
+        uint32_t vdc_stat, dsp_int = 0, sgx_int = 0;
+        int handled = 0;
+
+        spin_lock(&dev_priv->irqmask_lock);
+
+        vdc_stat = PSB_RVDC32(PSB_INT_IDENTITY_R);
+
+        if (vdc_stat & _MDFLD_DISP_ALL_IRQ_FLAG)
+                dsp_int = 1;
+
+        if (vdc_stat & _PSB_IRQ_SGX_FLAG)
+                sgx_int = 1;
+
+        vdc_stat &= dev_priv->vdc_irq_mask;
+        spin_unlock(&dev_priv->irqmask_lock);
+
+        if (dsp_int && gma_power_is_on(dev)) {
+                psb_vdc_interrupt(dev, vdc_stat);
+                handled = 1;
+        }
+
+        if (sgx_int) {
+                /* Not expected - we have it masked, shut it up */
+                u32 s, s2;
+                s = PSB_RSGX32(PSB_CR_EVENT_STATUS);
+                s2 = PSB_RSGX32(PSB_CR_EVENT_STATUS2);
+                PSB_WSGX32(s, PSB_CR_EVENT_HOST_CLEAR);
+                PSB_WSGX32(s2, PSB_CR_EVENT_HOST_CLEAR2);
+                /* if s & _PSB_CE_TWOD_COMPLETE we have 2D done but
+                   we may as well poll even if we add that ! */
+                handled = 1;
+        }
+
+        PSB_WVDC32(vdc_stat, PSB_INT_IDENTITY_R);
+        (void) PSB_RVDC32(PSB_INT_IDENTITY_R);
+        DRM_READMEMORYBARRIER();
+
+        if (!handled)
+                return IRQ_NONE;
+
+        return IRQ_HANDLED;
+}
+
+void psb_irq_preinstall(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv =
+            (struct drm_psb_private *) dev->dev_private;
+        unsigned long irqflags;
+
+        spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+        if (gma_power_is_on(dev))
+                PSB_WVDC32(0xFFFFFFFF, PSB_HWSTAM);
+        if (dev->vblank_enabled[0])
+                dev_priv->vdc_irq_mask |= _PSB_PIPEA_EVENT_FLAG;
+        if (dev->vblank_enabled[1])
+                dev_priv->vdc_irq_mask |= _MDFLD_PIPEB_EVENT_FLAG;
+        if (dev->vblank_enabled[2])
+                dev_priv->vdc_irq_mask |= _MDFLD_PIPEC_EVENT_FLAG;
+
+        /*This register is safe even if display island is off*/
+        PSB_WVDC32(~dev_priv->vdc_irq_mask, PSB_INT_MASK_R);
+        spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+}
+
+int psb_irq_postinstall(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv =
+            (struct drm_psb_private *) dev->dev_private;
+        unsigned long irqflags;
+
+        spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+        /* This register is safe even if display island is off */
+        PSB_WVDC32(dev_priv->vdc_irq_mask, PSB_INT_ENABLE_R);
+        PSB_WVDC32(0xFFFFFFFF, PSB_HWSTAM);
+
+        if (dev->vblank_enabled[0])
+                psb_enable_pipestat(dev_priv, 0, PIPE_VBLANK_INTERRUPT_ENABLE);
+        else
+                psb_disable_pipestat(dev_priv, 0, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+        if (dev->vblank_enabled[1])
+                psb_enable_pipestat(dev_priv, 1, PIPE_VBLANK_INTERRUPT_ENABLE);
+        else
+                psb_disable_pipestat(dev_priv, 1, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+        if (dev->vblank_enabled[2])
+                psb_enable_pipestat(dev_priv, 2, PIPE_VBLANK_INTERRUPT_ENABLE);
+        else
+                psb_disable_pipestat(dev_priv, 2, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+        spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+        return 0;
+}
+
+void psb_irq_uninstall(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv =
+            (struct drm_psb_private *) dev->dev_private;
+        unsigned long irqflags;
+
+        spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+        PSB_WVDC32(0xFFFFFFFF, PSB_HWSTAM);
+
+        if (dev->vblank_enabled[0])
+                psb_disable_pipestat(dev_priv, 0, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+        if (dev->vblank_enabled[1])
+                psb_disable_pipestat(dev_priv, 1, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+        if (dev->vblank_enabled[2])
+                psb_disable_pipestat(dev_priv, 2, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+        dev_priv->vdc_irq_mask &= _PSB_IRQ_SGX_FLAG |
+                                  _PSB_IRQ_MSVDX_FLAG |
+                                  _LNC_IRQ_TOPAZ_FLAG;
+
+        /* These two registers are safe even if display island is off */
+        PSB_WVDC32(~dev_priv->vdc_irq_mask, PSB_INT_MASK_R);
+        PSB_WVDC32(dev_priv->vdc_irq_mask, PSB_INT_ENABLE_R);
+
+        wmb();
+
+        /* This register is safe even if display island is off */
+        PSB_WVDC32(PSB_RVDC32(PSB_INT_IDENTITY_R), PSB_INT_IDENTITY_R);
+        spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+}
+
+void psb_irq_turn_on_dpst(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv =
+                (struct drm_psb_private *) dev->dev_private;
+        u32 hist_reg;
+        u32 pwm_reg;
+
+        if (gma_power_begin(dev, false)) {
+                PSB_WVDC32(1 << 31, HISTOGRAM_LOGIC_CONTROL);
+                hist_reg = PSB_RVDC32(HISTOGRAM_LOGIC_CONTROL);
+                PSB_WVDC32(1 << 31, HISTOGRAM_INT_CONTROL);
+                hist_reg = PSB_RVDC32(HISTOGRAM_INT_CONTROL);
+
+                PSB_WVDC32(0x80010100, PWM_CONTROL_LOGIC);
+                pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
+                PSB_WVDC32(pwm_reg | PWM_PHASEIN_ENABLE
+                                                | PWM_PHASEIN_INT_ENABLE,
+                                                           PWM_CONTROL_LOGIC);
+                pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
+
+                psb_enable_pipestat(dev_priv, 0, PIPE_DPST_EVENT_ENABLE);
+
+                hist_reg = PSB_RVDC32(HISTOGRAM_INT_CONTROL);
+                PSB_WVDC32(hist_reg | HISTOGRAM_INT_CTRL_CLEAR,
+                                                        HISTOGRAM_INT_CONTROL);
+                pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
+                PSB_WVDC32(pwm_reg | 0x80010100 | PWM_PHASEIN_ENABLE,
+                                                        PWM_CONTROL_LOGIC);
+
+                gma_power_end(dev);
+        }
+}
+
+int psb_irq_enable_dpst(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv =
+                (struct drm_psb_private *) dev->dev_private;
+        unsigned long irqflags;
+
+        spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+        /* enable DPST */
+        mid_enable_pipe_event(dev_priv, 0);
+        psb_irq_turn_on_dpst(dev);
+
+        spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+        return 0;
+}
+
+void psb_irq_turn_off_dpst(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv =
+            (struct drm_psb_private *) dev->dev_private;
+        u32 hist_reg;
+        u32 pwm_reg;
+
+        if (gma_power_begin(dev, false)) {
+                PSB_WVDC32(0x00000000, HISTOGRAM_INT_CONTROL);
+                hist_reg = PSB_RVDC32(HISTOGRAM_INT_CONTROL);
+
+                psb_disable_pipestat(dev_priv, 0, PIPE_DPST_EVENT_ENABLE);
+
+                pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
+                PSB_WVDC32(pwm_reg & !(PWM_PHASEIN_INT_ENABLE),
+                                                        PWM_CONTROL_LOGIC);
+                pwm_reg = PSB_RVDC32(PWM_CONTROL_LOGIC);
+
+                gma_power_end(dev);
+        }
+}
+
+int psb_irq_disable_dpst(struct drm_device *dev)
+{
+        struct drm_psb_private *dev_priv =
+            (struct drm_psb_private *) dev->dev_private;
+        unsigned long irqflags;
+
+        spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+        mid_disable_pipe_event(dev_priv, 0);
+        psb_irq_turn_off_dpst(dev);
+
+        spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+
+        return 0;
+}
+
+#ifdef PSB_FIXME
+static int psb_vblank_do_wait(struct drm_device *dev,
+                              unsigned int *sequence, atomic_t *counter)
+{
+        unsigned int cur_vblank;
+        int ret = 0;
+        DRM_WAIT_ON(ret, dev->vbl_queue, 3 * DRM_HZ,
+                    (((cur_vblank = atomic_read(counter))
+                      - *sequence) <= (1 << 23)));
+        *sequence = cur_vblank;
+
+        return ret;
+}
+#endif
+
+/*
+ * It is used to enable VBLANK interrupt
+ */
+int psb_enable_vblank(struct drm_device *dev, int pipe)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        unsigned long irqflags;
+        uint32_t reg_val = 0;
+        uint32_t pipeconf_reg = mid_pipeconf(pipe);
+
+#if defined(CONFIG_DRM_PSB_MFLD)
+        /* Medfield is different - we should perhaps extract out vblank
+           and blacklight etc ops */
+        if (IS_MFLD(dev) && !mdfld_panel_dpi(dev))
+                return mdfld_enable_te(dev, pipe);
+#endif
+        if (gma_power_begin(dev, false)) {
+                reg_val = REG_READ(pipeconf_reg);
+                gma_power_end(dev);
+        }
+
+        if (!(reg_val & PIPEACONF_ENABLE))
+                return -EINVAL;
+
+        spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+        mid_enable_pipe_event(dev_priv, pipe);
+        psb_enable_pipestat(dev_priv, pipe, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+        spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+
+        return 0;
+}
+
+/*
+ * It is used to disable VBLANK interrupt
+ */
+void psb_disable_vblank(struct drm_device *dev, int pipe)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        unsigned long irqflags;
+
+#if defined(CONFIG_DRM_PSB_MFLD)
+        if (IS_MFLD(dev) && !mdfld_panel_dpi(dev))
+                mdfld_disable_te(dev, pipe);
+#endif
+        spin_lock_irqsave(&dev_priv->irqmask_lock, irqflags);
+
+        mid_disable_pipe_event(dev_priv, pipe);
+        psb_disable_pipestat(dev_priv, pipe, PIPE_VBLANK_INTERRUPT_ENABLE);
+
+        spin_unlock_irqrestore(&dev_priv->irqmask_lock, irqflags);
+}
+
+/**
+ *      mdfld_enable_te         -       enable TE events
+ *      @dev: our DRM device
+ *      @pipe: which pipe to work on
+ *
+ *      Enable TE events on a Medfield display pipe. Medfield specific.
+ */
+int mdfld_enable_te(struct drm_device *dev, int pipe)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        unsigned long flags;
+        uint32_t reg_val = 0;
+        uint32_t pipeconf_reg = mid_pipeconf(pipe);
+
+        if (gma_power_begin(dev, false)) {
+                reg_val = REG_READ(pipeconf_reg);
+                gma_power_end(dev);
+        }
+
+        if (!(reg_val & PIPEACONF_ENABLE))
+                return -EINVAL;
+
+        spin_lock_irqsave(&dev_priv->irqmask_lock, flags);
+
+        mid_enable_pipe_event(dev_priv, pipe);
+        psb_enable_pipestat(dev_priv, pipe, PIPE_TE_ENABLE);
+
+        spin_unlock_irqrestore(&dev_priv->irqmask_lock, flags);
+
+        return 0;
+}
+
+/**
+ *      mdfld_disable_te                -       disable TE events
+ *      @dev: our DRM device
+ *      @pipe: which pipe to work on
+ *
+ *      Disable TE events on a Medfield display pipe. Medfield specific.
+ */
+void mdfld_disable_te(struct drm_device *dev, int pipe)
+{
+        struct drm_psb_private *dev_priv = dev->dev_private;
+        unsigned long flags;
+
+        spin_lock_irqsave(&dev_priv->irqmask_lock, flags);
+
+        mid_disable_pipe_event(dev_priv, pipe);
+        psb_disable_pipestat(dev_priv, pipe, PIPE_TE_ENABLE);
+
+        spin_unlock_irqrestore(&dev_priv->irqmask_lock, flags);
+}
+
+/* Called from drm generic code, passed a 'crtc', which
+ * we use as a pipe index
+ */
+u32 psb_get_vblank_counter(struct drm_device *dev, int pipe)
+{
+        uint32_t high_frame = PIPEAFRAMEHIGH;
+        uint32_t low_frame = PIPEAFRAMEPIXEL;
+        uint32_t pipeconf_reg = PIPEACONF;
+        uint32_t reg_val = 0;
+        uint32_t high1 = 0, high2 = 0, low = 0, count = 0;
+
+        switch (pipe) {
+        case 0:
+                break;
+        case 1:
+                high_frame = PIPEBFRAMEHIGH;
+                low_frame = PIPEBFRAMEPIXEL;
+                pipeconf_reg = PIPEBCONF;
+                break;
+        case 2:
+                high_frame = PIPECFRAMEHIGH;
+                low_frame = PIPECFRAMEPIXEL;
+                pipeconf_reg = PIPECCONF;
+                break;
+        default:
+                dev_err(dev->dev, "%s, invalid pipe.\n", __func__);
+                return 0;
+        }
+
+        if (!gma_power_begin(dev, false))
+                return 0;
+
+        reg_val = REG_READ(pipeconf_reg);
+
+        if (!(reg_val & PIPEACONF_ENABLE)) {
+                dev_err(dev->dev, "trying to get vblank count for disabled pipe %d\n",
+                                                                pipe);
+                goto psb_get_vblank_counter_exit;
+        }
+
+        /*
+         * High & low register fields aren't synchronized, so make sure
+         * we get a low value that's stable across two reads of the high
+         * register.
+         */
+        do {
+                high1 = ((REG_READ(high_frame) & PIPE_FRAME_HIGH_MASK) >>
+                         PIPE_FRAME_HIGH_SHIFT);
+                low =  ((REG_READ(low_frame) & PIPE_FRAME_LOW_MASK) >>
+                        PIPE_FRAME_LOW_SHIFT);
+                high2 = ((REG_READ(high_frame) & PIPE_FRAME_HIGH_MASK) >>
+                         PIPE_FRAME_HIGH_SHIFT);
+        } while (high1 != high2);
+
+        count = (high1 << 8) | low;
+
+psb_get_vblank_counter_exit:
+
+        gma_power_end(dev);
+
+        return count;
+}
+
diff --git a/drivers/staging/gma500/psb_irq.h b/drivers/staging/gma500/psb_irq.h
new file mode 100644
index 00000000000..216fda38b57
--- /dev/null
+++ b/drivers/staging/gma500/psb_irq.h
@@ -0,0 +1,45 @@
+/**************************************************************************
+ * Copyright (c) 2009-2011, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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.
+ *
+ * Authors:
+ *    Benjamin Defnet <benjamin.r.defnet@intel.com>
+ *    Rajesh Poornachandran <rajesh.poornachandran@intel.com>
+ *
+ **************************************************************************/
+
+#ifndef _SYSIRQ_H_
+#define _SYSIRQ_H_
+
+#include <drm/drmP.h>
+
+bool sysirq_init(struct drm_device *dev);
+void sysirq_uninit(struct drm_device *dev);
+
+void psb_irq_preinstall(struct drm_device *dev);
+int  psb_irq_postinstall(struct drm_device *dev);
+void psb_irq_uninstall(struct drm_device *dev);
+irqreturn_t psb_irq_handler(DRM_IRQ_ARGS);
+
+int psb_irq_enable_dpst(struct drm_device *dev);
+int psb_irq_disable_dpst(struct drm_device *dev);
+void psb_irq_turn_on_dpst(struct drm_device *dev);
+void psb_irq_turn_off_dpst(struct drm_device *dev);
+int  psb_enable_vblank(struct drm_device *dev, int pipe);
+void psb_disable_vblank(struct drm_device *dev, int pipe);
+u32  psb_get_vblank_counter(struct drm_device *dev, int pipe);
+
+#endif /* _SYSIRQ_H_ */
diff --git a/drivers/staging/gma500/psb_lid.c b/drivers/staging/gma500/psb_lid.c
new file mode 100644
index 00000000000..af328516561
--- /dev/null
+++ b/drivers/staging/gma500/psb_lid.c
@@ -0,0 +1,90 @@
+/**************************************************************************
+ * Copyright (c) 2007, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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.
+ *
+ * Authors: Thomas Hellstrom <thomas-at-tungstengraphics-dot-com>
+ **************************************************************************/
+
+#include <drm/drmP.h>
+#include "psb_drv.h"
+#include "psb_reg.h"
+#include "psb_intel_reg.h"
+#include <linux/spinlock.h>
+
+static void psb_lid_timer_func(unsigned long data)
+{
+        struct drm_psb_private * dev_priv = (struct drm_psb_private *)data;
+        struct drm_device *dev = (struct drm_device *)dev_priv->dev;
+        struct timer_list *lid_timer = &dev_priv->lid_timer;
+        unsigned long irq_flags;
+        u32 *lid_state = dev_priv->lid_state;
+        u32 pp_status;
+
+        if (readl(lid_state) == dev_priv->lid_last_state)
+                goto lid_timer_schedule;
+
+        if ((readl(lid_state)) & 0x01) {
+                /*lid state is open*/
+                REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) | POWER_TARGET_ON);
+                do {
+                        pp_status = REG_READ(PP_STATUS);
+                } while ((pp_status & PP_ON) == 0);
+
+                /*FIXME: should be backlight level before*/
+                psb_intel_lvds_set_brightness(dev, 100);
+        } else {
+                psb_intel_lvds_set_brightness(dev, 0);
+
+                REG_WRITE(PP_CONTROL, REG_READ(PP_CONTROL) & ~POWER_TARGET_ON);
+                do {
+                        pp_status = REG_READ(PP_STATUS);
+                } while ((pp_status & PP_ON) == 0);
+        }
+                /* printk(KERN_INFO"%s: lid: closed\n", __FUNCTION__); */
+
+        dev_priv->lid_last_state =  readl(lid_state);
+
+lid_timer_schedule:
+        spin_lock_irqsave(&dev_priv->lid_lock, irq_flags);
+        if (!timer_pending(lid_timer)) {
+                lid_timer->expires = jiffies + PSB_LID_DELAY;
+                add_timer(lid_timer);
+        }
+        spin_unlock_irqrestore(&dev_priv->lid_lock, irq_flags);
+}
+
+void psb_lid_timer_init(struct drm_psb_private *dev_priv)
+{
+        struct timer_list *lid_timer = &dev_priv->lid_timer;
+        unsigned long irq_flags;
+
+        spin_lock_init(&dev_priv->lid_lock);
+        spin_lock_irqsave(&dev_priv->lid_lock, irq_flags);
+
+        init_timer(lid_timer);
+
+        lid_timer->data = (unsigned long)dev_priv;
+        lid_timer->function = psb_lid_timer_func;
+        lid_timer->expires = jiffies + PSB_LID_DELAY;
+
+        add_timer(lid_timer);
+        spin_unlock_irqrestore(&dev_priv->lid_lock, irq_flags);
+}
+
+void psb_lid_timer_takedown(struct drm_psb_private *dev_priv)
+{
+        del_timer_sync(&dev_priv->lid_timer);
+}
+
diff --git a/drivers/staging/gma500/psb_reg.h b/drivers/staging/gma500/psb_reg.h
new file mode 100644
index 00000000000..b81c7c1e9c2
--- /dev/null
+++ b/drivers/staging/gma500/psb_reg.h
@@ -0,0 +1,582 @@
+/**************************************************************************
+ *
+ * Copyright (c) (2005-2007) Imagination Technologies Limited.
+ * Copyright (c) 2007, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * 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..
+ *
+ **************************************************************************/
+
+#ifndef _PSB_REG_H_
+#define _PSB_REG_H_
+
+#define PSB_CR_CLKGATECTL               0x0000
+#define _PSB_C_CLKGATECTL_AUTO_MAN_REG          (1 << 24)
+#define _PSB_C_CLKGATECTL_USE_CLKG_SHIFT        (20)
+#define _PSB_C_CLKGATECTL_USE_CLKG_MASK         (0x3 << 20)
+#define _PSB_C_CLKGATECTL_DPM_CLKG_SHIFT        (16)
+#define _PSB_C_CLKGATECTL_DPM_CLKG_MASK         (0x3 << 16)
+#define _PSB_C_CLKGATECTL_TA_CLKG_SHIFT         (12)
+#define _PSB_C_CLKGATECTL_TA_CLKG_MASK          (0x3 << 12)
+#define _PSB_C_CLKGATECTL_TSP_CLKG_SHIFT        (8)
+#define _PSB_C_CLKGATECTL_TSP_CLKG_MASK         (0x3 << 8)
+#define _PSB_C_CLKGATECTL_ISP_CLKG_SHIFT        (4)
+#define _PSB_C_CLKGATECTL_ISP_CLKG_MASK         (0x3 << 4)
+#define _PSB_C_CLKGATECTL_2D_CLKG_SHIFT         (0)
+#define _PSB_C_CLKGATECTL_2D_CLKG_MASK          (0x3 << 0)
+#define _PSB_C_CLKGATECTL_CLKG_ENABLED          (0)
+#define _PSB_C_CLKGATECTL_CLKG_DISABLED         (1)
+#define _PSB_C_CLKGATECTL_CLKG_AUTO             (2)
+
+#define PSB_CR_CORE_ID                  0x0010
+#define _PSB_CC_ID_ID_SHIFT                     (16)
+#define _PSB_CC_ID_ID_MASK                      (0xFFFF << 16)
+#define _PSB_CC_ID_CONFIG_SHIFT                 (0)
+#define _PSB_CC_ID_CONFIG_MASK                  (0xFFFF << 0)
+
+#define PSB_CR_CORE_REVISION            0x0014
+#define _PSB_CC_REVISION_DESIGNER_SHIFT         (24)
+#define _PSB_CC_REVISION_DESIGNER_MASK          (0xFF << 24)
+#define _PSB_CC_REVISION_MAJOR_SHIFT            (16)
+#define _PSB_CC_REVISION_MAJOR_MASK             (0xFF << 16)
+#define _PSB_CC_REVISION_MINOR_SHIFT            (8)
+#define _PSB_CC_REVISION_MINOR_MASK             (0xFF << 8)
+#define _PSB_CC_REVISION_MAINTENANCE_SHIFT      (0)
+#define _PSB_CC_REVISION_MAINTENANCE_MASK       (0xFF << 0)
+
+#define PSB_CR_DESIGNER_REV_FIELD1      0x0018
+
+#define PSB_CR_SOFT_RESET               0x0080
+#define _PSB_CS_RESET_TSP_RESET         (1 << 6)
+#define _PSB_CS_RESET_ISP_RESET         (1 << 5)
+#define _PSB_CS_RESET_USE_RESET         (1 << 4)
+#define _PSB_CS_RESET_TA_RESET          (1 << 3)
+#define _PSB_CS_RESET_DPM_RESET         (1 << 2)
+#define _PSB_CS_RESET_TWOD_RESET        (1 << 1)
+#define _PSB_CS_RESET_BIF_RESET                 (1 << 0)
+
+#define PSB_CR_DESIGNER_REV_FIELD2      0x001C
+
+#define PSB_CR_EVENT_HOST_ENABLE2       0x0110
+
+#define PSB_CR_EVENT_STATUS2            0x0118
+
+#define PSB_CR_EVENT_HOST_CLEAR2        0x0114
+#define _PSB_CE2_BIF_REQUESTER_FAULT            (1 << 4)
+
+#define PSB_CR_EVENT_STATUS             0x012C
+
+#define PSB_CR_EVENT_HOST_ENABLE        0x0130
+
+#define PSB_CR_EVENT_HOST_CLEAR         0x0134
+#define _PSB_CE_MASTER_INTERRUPT                (1 << 31)
+#define _PSB_CE_TA_DPM_FAULT                    (1 << 28)
+#define _PSB_CE_TWOD_COMPLETE                   (1 << 27)
+#define _PSB_CE_DPM_OUT_OF_MEMORY_ZLS           (1 << 25)
+#define _PSB_CE_DPM_TA_MEM_FREE                 (1 << 24)
+#define _PSB_CE_PIXELBE_END_RENDER              (1 << 18)
+#define _PSB_CE_SW_EVENT                        (1 << 14)
+#define _PSB_CE_TA_FINISHED                     (1 << 13)
+#define _PSB_CE_TA_TERMINATE                    (1 << 12)
+#define _PSB_CE_DPM_REACHED_MEM_THRESH          (1 << 3)
+#define _PSB_CE_DPM_OUT_OF_MEMORY_GBL           (1 << 2)
+#define _PSB_CE_DPM_OUT_OF_MEMORY_MT            (1 << 1)
+#define _PSB_CE_DPM_3D_MEM_FREE                 (1 << 0)
+
+
+#define PSB_USE_OFFSET_MASK             0x0007FFFF
+#define PSB_USE_OFFSET_SIZE             (PSB_USE_OFFSET_MASK + 1)
+#define PSB_CR_USE_CODE_BASE0           0x0A0C
+#define PSB_CR_USE_CODE_BASE1           0x0A10
+#define PSB_CR_USE_CODE_BASE2           0x0A14
+#define PSB_CR_USE_CODE_BASE3           0x0A18
+#define PSB_CR_USE_CODE_BASE4           0x0A1C
+#define PSB_CR_USE_CODE_BASE5           0x0A20
+#define PSB_CR_USE_CODE_BASE6           0x0A24
+#define PSB_CR_USE_CODE_BASE7           0x0A28
+#define PSB_CR_USE_CODE_BASE8           0x0A2C
+#define PSB_CR_USE_CODE_BASE9           0x0A30
+#define PSB_CR_USE_CODE_BASE10          0x0A34
+#define PSB_CR_USE_CODE_BASE11          0x0A38
+#define PSB_CR_USE_CODE_BASE12          0x0A3C
+#define PSB_CR_USE_CODE_BASE13          0x0A40
+#define PSB_CR_USE_CODE_BASE14          0x0A44
+#define PSB_CR_USE_CODE_BASE15          0x0A48
+#define PSB_CR_USE_CODE_BASE(_i)        (0x0A0C + ((_i) << 2))
+#define _PSB_CUC_BASE_DM_SHIFT                  (25)
+#define _PSB_CUC_BASE_DM_MASK                   (0x3 << 25)
+#define _PSB_CUC_BASE_ADDR_SHIFT                (0)     /* 1024-bit aligned address? */
+#define _PSB_CUC_BASE_ADDR_ALIGNSHIFT           (7)
+#define _PSB_CUC_BASE_ADDR_MASK                 (0x1FFFFFF << 0)
+#define _PSB_CUC_DM_VERTEX                      (0)
+#define _PSB_CUC_DM_PIXEL                       (1)
+#define _PSB_CUC_DM_RESERVED                    (2)
+#define _PSB_CUC_DM_EDM                         (3)
+
+#define PSB_CR_PDS_EXEC_BASE            0x0AB8
+#define _PSB_CR_PDS_EXEC_BASE_ADDR_SHIFT        (20)    /* 1MB aligned address */
+#define _PSB_CR_PDS_EXEC_BASE_ADDR_ALIGNSHIFT   (20)
+
+#define PSB_CR_EVENT_KICKER             0x0AC4
+#define _PSB_CE_KICKER_ADDRESS_SHIFT            (4)     /* 128-bit aligned address */
+
+#define PSB_CR_EVENT_KICK               0x0AC8
+#define _PSB_CE_KICK_NOW                        (1 << 0)
+
+#define PSB_CR_BIF_DIR_LIST_BASE1       0x0C38
+
+#define PSB_CR_BIF_CTRL                 0x0C00
+#define _PSB_CB_CTRL_CLEAR_FAULT                (1 << 4)
+#define _PSB_CB_CTRL_INVALDC                    (1 << 3)
+#define _PSB_CB_CTRL_FLUSH                      (1 << 2)
+
+#define PSB_CR_BIF_INT_STAT             0x0C04
+
+#define PSB_CR_BIF_FAULT                0x0C08
+#define _PSB_CBI_STAT_PF_N_RW                   (1 << 14)
+#define _PSB_CBI_STAT_FAULT_SHIFT               (0)
+#define _PSB_CBI_STAT_FAULT_MASK                (0x3FFF << 0)
+#define _PSB_CBI_STAT_FAULT_CACHE               (1 << 1)
+#define _PSB_CBI_STAT_FAULT_TA                  (1 << 2)
+#define _PSB_CBI_STAT_FAULT_VDM                 (1 << 3)
+#define _PSB_CBI_STAT_FAULT_2D                  (1 << 4)
+#define _PSB_CBI_STAT_FAULT_PBE                 (1 << 5)
+#define _PSB_CBI_STAT_FAULT_TSP                 (1 << 6)
+#define _PSB_CBI_STAT_FAULT_ISP                 (1 << 7)
+#define _PSB_CBI_STAT_FAULT_USSEPDS             (1 << 8)
+#define _PSB_CBI_STAT_FAULT_HOST                (1 << 9)
+
+#define PSB_CR_BIF_BANK0                0x0C78
+#define PSB_CR_BIF_BANK1                0x0C7C
+#define PSB_CR_BIF_DIR_LIST_BASE0       0x0C84
+#define PSB_CR_BIF_TWOD_REQ_BASE        0x0C88
+#define PSB_CR_BIF_3D_REQ_BASE          0x0CAC
+
+#define PSB_CR_2D_SOCIF                 0x0E18
+#define _PSB_C2_SOCIF_FREESPACE_SHIFT           (0)
+#define _PSB_C2_SOCIF_FREESPACE_MASK            (0xFF << 0)
+#define _PSB_C2_SOCIF_EMPTY                     (0x80 << 0)
+
+#define PSB_CR_2D_BLIT_STATUS           0x0E04
+#define _PSB_C2B_STATUS_BUSY                    (1 << 24)
+#define _PSB_C2B_STATUS_COMPLETE_SHIFT          (0)
+#define _PSB_C2B_STATUS_COMPLETE_MASK           (0xFFFFFF << 0)
+
+/*
+ * 2D defs.
+ */
+
+/*
+ * 2D Slave Port Data : Block Header's Object Type
+ */
+
+#define PSB_2D_CLIP_BH                  (0x00000000)
+#define PSB_2D_PAT_BH                   (0x10000000)
+#define PSB_2D_CTRL_BH                  (0x20000000)
+#define PSB_2D_SRC_OFF_BH               (0x30000000)
+#define PSB_2D_MASK_OFF_BH              (0x40000000)
+#define PSB_2D_RESERVED1_BH             (0x50000000)
+#define PSB_2D_RESERVED2_BH             (0x60000000)
+#define PSB_2D_FENCE_BH                 (0x70000000)
+#define PSB_2D_BLIT_BH                  (0x80000000)
+#define PSB_2D_SRC_SURF_BH              (0x90000000)
+#define PSB_2D_DST_SURF_BH              (0xA0000000)
+#define PSB_2D_PAT_SURF_BH              (0xB0000000)
+#define PSB_2D_SRC_PAL_BH               (0xC0000000)
+#define PSB_2D_PAT_PAL_BH               (0xD0000000)
+#define PSB_2D_MASK_SURF_BH             (0xE0000000)
+#define PSB_2D_FLUSH_BH                 (0xF0000000)
+
+/*
+ * Clip Definition block (PSB_2D_CLIP_BH)
+ */
+#define PSB_2D_CLIPCOUNT_MAX            (1)
+#define PSB_2D_CLIPCOUNT_MASK           (0x00000000)
+#define PSB_2D_CLIPCOUNT_CLRMASK        (0xFFFFFFFF)
+#define PSB_2D_CLIPCOUNT_SHIFT          (0)
+/* clip rectangle min & max */
+#define PSB_2D_CLIP_XMAX_MASK           (0x00FFF000)
+#define PSB_2D_CLIP_XMAX_CLRMASK        (0xFF000FFF)
+#define PSB_2D_CLIP_XMAX_SHIFT          (12)
+#define PSB_2D_CLIP_XMIN_MASK           (0x00000FFF)
+#define PSB_2D_CLIP_XMIN_CLRMASK        (0x00FFF000)
+#define PSB_2D_CLIP_XMIN_SHIFT          (0)
+/* clip rectangle offset */
+#define PSB_2D_CLIP_YMAX_MASK           (0x00FFF000)
+#define PSB_2D_CLIP_YMAX_CLRMASK        (0xFF000FFF)
+#define PSB_2D_CLIP_YMAX_SHIFT          (12)
+#define PSB_2D_CLIP_YMIN_MASK           (0x00000FFF)
+#define PSB_2D_CLIP_YMIN_CLRMASK        (0x00FFF000)
+#define PSB_2D_CLIP_YMIN_SHIFT          (0)
+
+/*
+ * Pattern Control (PSB_2D_PAT_BH)
+ */
+#define PSB_2D_PAT_HEIGHT_MASK          (0x0000001F)
+#define PSB_2D_PAT_HEIGHT_SHIFT         (0)
+#define PSB_2D_PAT_WIDTH_MASK           (0x000003E0)
+#define PSB_2D_PAT_WIDTH_SHIFT          (5)
+#define PSB_2D_PAT_YSTART_MASK          (0x00007C00)
+#define PSB_2D_PAT_YSTART_SHIFT         (10)
+#define PSB_2D_PAT_XSTART_MASK          (0x000F8000)
+#define PSB_2D_PAT_XSTART_SHIFT         (15)
+
+/*
+ * 2D Control block (PSB_2D_CTRL_BH)
+ */
+/* Present Flags */
+#define PSB_2D_SRCCK_CTRL               (0x00000001)
+#define PSB_2D_DSTCK_CTRL               (0x00000002)
+#define PSB_2D_ALPHA_CTRL               (0x00000004)
+/* Colour Key Colour (SRC/DST)*/
+#define PSB_2D_CK_COL_MASK              (0xFFFFFFFF)
+#define PSB_2D_CK_COL_CLRMASK           (0x00000000)
+#define PSB_2D_CK_COL_SHIFT             (0)
+/* Colour Key Mask (SRC/DST)*/
+#define PSB_2D_CK_MASK_MASK             (0xFFFFFFFF)
+#define PSB_2D_CK_MASK_CLRMASK          (0x00000000)
+#define PSB_2D_CK_MASK_SHIFT            (0)
+/* Alpha Control (Alpha/RGB)*/
+#define PSB_2D_GBLALPHA_MASK            (0x000FF000)
+#define PSB_2D_GBLALPHA_CLRMASK         (0xFFF00FFF)
+#define PSB_2D_GBLALPHA_SHIFT           (12)
+#define PSB_2D_SRCALPHA_OP_MASK         (0x00700000)
+#define PSB_2D_SRCALPHA_OP_CLRMASK      (0xFF8FFFFF)
+#define PSB_2D_SRCALPHA_OP_SHIFT        (20)
+#define PSB_2D_SRCALPHA_OP_ONE          (0x00000000)
+#define PSB_2D_SRCALPHA_OP_SRC          (0x00100000)
+#define PSB_2D_SRCALPHA_OP_DST          (0x00200000)
+#define PSB_2D_SRCALPHA_OP_SG           (0x00300000)
+#define PSB_2D_SRCALPHA_OP_DG           (0x00400000)
+#define PSB_2D_SRCALPHA_OP_GBL          (0x00500000)
+#define PSB_2D_SRCALPHA_OP_ZERO         (0x00600000)
+#define PSB_2D_SRCALPHA_INVERT          (0x00800000)
+#define PSB_2D_SRCALPHA_INVERT_CLR      (0xFF7FFFFF)
+#define PSB_2D_DSTALPHA_OP_MASK         (0x07000000)
+#define PSB_2D_DSTALPHA_OP_CLRMASK      (0xF8FFFFFF)
+#define PSB_2D_DSTALPHA_OP_SHIFT        (24)
+#define PSB_2D_DSTALPHA_OP_ONE          (0x00000000)
+#define PSB_2D_DSTALPHA_OP_SRC          (0x01000000)
+#define PSB_2D_DSTALPHA_OP_DST          (0x02000000)
+#define PSB_2D_DSTALPHA_OP_SG           (0x03000000)
+#define PSB_2D_DSTALPHA_OP_DG           (0x04000000)
+#define PSB_2D_DSTALPHA_OP_GBL          (0x05000000)
+#define PSB_2D_DSTALPHA_OP_ZERO         (0x06000000)
+#define PSB_2D_DSTALPHA_INVERT          (0x08000000)
+#define PSB_2D_DSTALPHA_INVERT_CLR      (0xF7FFFFFF)
+
+#define PSB_2D_PRE_MULTIPLICATION_ENABLE        (0x10000000)
+#define PSB_2D_PRE_MULTIPLICATION_CLRMASK       (0xEFFFFFFF)
+#define PSB_2D_ZERO_SOURCE_ALPHA_ENABLE         (0x20000000)
+#define PSB_2D_ZERO_SOURCE_ALPHA_CLRMASK        (0xDFFFFFFF)
+
+/*
+ *Source Offset (PSB_2D_SRC_OFF_BH)
+ */
+#define PSB_2D_SRCOFF_XSTART_MASK       ((0x00000FFF) << 12)
+#define PSB_2D_SRCOFF_XSTART_SHIFT      (12)
+#define PSB_2D_SRCOFF_YSTART_MASK       (0x00000FFF)
+#define PSB_2D_SRCOFF_YSTART_SHIFT      (0)
+
+/*
+ * Mask Offset (PSB_2D_MASK_OFF_BH)
+ */
+#define PSB_2D_MASKOFF_XSTART_MASK      ((0x00000FFF) << 12)
+#define PSB_2D_MASKOFF_XSTART_SHIFT     (12)
+#define PSB_2D_MASKOFF_YSTART_MASK      (0x00000FFF)
+#define PSB_2D_MASKOFF_YSTART_SHIFT     (0)
+
+/*
+ * 2D Fence (see PSB_2D_FENCE_BH): bits 0:27 are ignored
+ */
+
+/*
+ *Blit Rectangle (PSB_2D_BLIT_BH)
+ */
+
+#define PSB_2D_ROT_MASK                 (3 << 25)
+#define PSB_2D_ROT_CLRMASK              (~PSB_2D_ROT_MASK)
+#define PSB_2D_ROT_NONE                 (0 << 25)
+#define PSB_2D_ROT_90DEGS               (1 << 25)
+#define PSB_2D_ROT_180DEGS              (2 << 25)
+#define PSB_2D_ROT_270DEGS              (3 << 25)
+
+#define PSB_2D_COPYORDER_MASK           (3 << 23)
+#define PSB_2D_COPYORDER_CLRMASK        (~PSB_2D_COPYORDER_MASK)
+#define PSB_2D_COPYORDER_TL2BR          (0 << 23)
+#define PSB_2D_COPYORDER_BR2TL          (1 << 23)
+#define PSB_2D_COPYORDER_TR2BL          (2 << 23)
+#define PSB_2D_COPYORDER_BL2TR          (3 << 23)
+
+#define PSB_2D_DSTCK_CLRMASK            (0xFF9FFFFF)
+#define PSB_2D_DSTCK_DISABLE            (0x00000000)
+#define PSB_2D_DSTCK_PASS               (0x00200000)
+#define PSB_2D_DSTCK_REJECT             (0x00400000)
+
+#define PSB_2D_SRCCK_CLRMASK            (0xFFE7FFFF)
+#define PSB_2D_SRCCK_DISABLE            (0x00000000)
+#define PSB_2D_SRCCK_PASS               (0x00080000)
+#define PSB_2D_SRCCK_REJECT             (0x00100000)
+
+#define PSB_2D_CLIP_ENABLE              (0x00040000)
+
+#define PSB_2D_ALPHA_ENABLE             (0x00020000)
+
+#define PSB_2D_PAT_CLRMASK              (0xFFFEFFFF)
+#define PSB_2D_PAT_MASK                 (0x00010000)
+#define PSB_2D_USE_PAT                  (0x00010000)
+#define PSB_2D_USE_FILL                 (0x00000000)
+/*
+ * Tungsten Graphics note on rop codes: If rop A and rop B are
+ * identical, the mask surface will not be read and need not be
+ * set up.
+ */
+
+#define PSB_2D_ROP3B_MASK               (0x0000FF00)
+#define PSB_2D_ROP3B_CLRMASK            (0xFFFF00FF)
+#define PSB_2D_ROP3B_SHIFT              (8)
+/* rop code A */
+#define PSB_2D_ROP3A_MASK               (0x000000FF)
+#define PSB_2D_ROP3A_CLRMASK            (0xFFFFFF00)
+#define PSB_2D_ROP3A_SHIFT              (0)
+
+#define PSB_2D_ROP4_MASK                (0x0000FFFF)
+/*
+ *      DWORD0: (Only pass if Pattern control == Use Fill Colour)
+ *      Fill Colour RGBA8888
+ */
+#define PSB_2D_FILLCOLOUR_MASK          (0xFFFFFFFF)
+#define PSB_2D_FILLCOLOUR_SHIFT         (0)
+/*
+ *      DWORD1: (Always Present)
+ *      X Start (Dest)
+ *      Y Start (Dest)
+ */
+#define PSB_2D_DST_XSTART_MASK          (0x00FFF000)
+#define PSB_2D_DST_XSTART_CLRMASK       (0xFF000FFF)
+#define PSB_2D_DST_XSTART_SHIFT         (12)
+#define PSB_2D_DST_YSTART_MASK          (0x00000FFF)
+#define PSB_2D_DST_YSTART_CLRMASK       (0xFFFFF000)
+#define PSB_2D_DST_YSTART_SHIFT         (0)
+/*
+ *      DWORD2: (Always Present)
+ *      X Size (Dest)
+ *      Y Size (Dest)
+ */
+#define PSB_2D_DST_XSIZE_MASK           (0x00FFF000)
+#define PSB_2D_DST_XSIZE_CLRMASK        (0xFF000FFF)
+#define PSB_2D_DST_XSIZE_SHIFT          (12)
+#define PSB_2D_DST_YSIZE_MASK           (0x00000FFF)
+#define PSB_2D_DST_YSIZE_CLRMASK        (0xFFFFF000)
+#define PSB_2D_DST_YSIZE_SHIFT          (0)
+
+/*
+ * Source Surface (PSB_2D_SRC_SURF_BH)
+ */
+/*
+ * WORD 0
+ */
+
+#define PSB_2D_SRC_FORMAT_MASK          (0x00078000)
+#define PSB_2D_SRC_1_PAL                (0x00000000)
+#define PSB_2D_SRC_2_PAL                (0x00008000)
+#define PSB_2D_SRC_4_PAL                (0x00010000)
+#define PSB_2D_SRC_8_PAL                (0x00018000)
+#define PSB_2D_SRC_8_ALPHA              (0x00020000)
+#define PSB_2D_SRC_4_ALPHA              (0x00028000)
+#define PSB_2D_SRC_332RGB               (0x00030000)
+#define PSB_2D_SRC_4444ARGB             (0x00038000)
+#define PSB_2D_SRC_555RGB               (0x00040000)
+#define PSB_2D_SRC_1555ARGB             (0x00048000)
+#define PSB_2D_SRC_565RGB               (0x00050000)
+#define PSB_2D_SRC_0888ARGB             (0x00058000)
+#define PSB_2D_SRC_8888ARGB             (0x00060000)
+#define PSB_2D_SRC_8888UYVY             (0x00068000)
+#define PSB_2D_SRC_RESERVED             (0x00070000)
+#define PSB_2D_SRC_1555ARGB_LOOKUP      (0x00078000)
+
+
+#define PSB_2D_SRC_STRIDE_MASK          (0x00007FFF)
+#define PSB_2D_SRC_STRIDE_CLRMASK       (0xFFFF8000)
+#define PSB_2D_SRC_STRIDE_SHIFT         (0)
+/*
+ *  WORD 1 - Base Address
+ */
+#define PSB_2D_SRC_ADDR_MASK            (0x0FFFFFFC)
+#define PSB_2D_SRC_ADDR_CLRMASK         (0x00000003)
+#define PSB_2D_SRC_ADDR_SHIFT           (2)
+#define PSB_2D_SRC_ADDR_ALIGNSHIFT      (2)
+
+/*
+ * Pattern Surface (PSB_2D_PAT_SURF_BH)
+ */
+/*
+ *  WORD 0
+ */
+
+#define PSB_2D_PAT_FORMAT_MASK          (0x00078000)
+#define PSB_2D_PAT_1_PAL                (0x00000000)
+#define PSB_2D_PAT_2_PAL                (0x00008000)
+#define PSB_2D_PAT_4_PAL                (0x00010000)
+#define PSB_2D_PAT_8_PAL                (0x00018000)
+#define PSB_2D_PAT_8_ALPHA              (0x00020000)
+#define PSB_2D_PAT_4_ALPHA              (0x00028000)
+#define PSB_2D_PAT_332RGB               (0x00030000)
+#define PSB_2D_PAT_4444ARGB             (0x00038000)
+#define PSB_2D_PAT_555RGB               (0x00040000)
+#define PSB_2D_PAT_1555ARGB             (0x00048000)
+#define PSB_2D_PAT_565RGB               (0x00050000)
+#define PSB_2D_PAT_0888ARGB             (0x00058000)
+#define PSB_2D_PAT_8888ARGB             (0x00060000)
+
+#define PSB_2D_PAT_STRIDE_MASK          (0x00007FFF)
+#define PSB_2D_PAT_STRIDE_CLRMASK       (0xFFFF8000)
+#define PSB_2D_PAT_STRIDE_SHIFT         (0)
+/*
+ *  WORD 1 - Base Address
+ */
+#define PSB_2D_PAT_ADDR_MASK            (0x0FFFFFFC)
+#define PSB_2D_PAT_ADDR_CLRMASK         (0x00000003)
+#define PSB_2D_PAT_ADDR_SHIFT           (2)
+#define PSB_2D_PAT_ADDR_ALIGNSHIFT      (2)
+
+/*
+ * Destination Surface (PSB_2D_DST_SURF_BH)
+ */
+/*
+ * WORD 0
+ */
+
+#define PSB_2D_DST_FORMAT_MASK          (0x00078000)
+#define PSB_2D_DST_332RGB               (0x00030000)
+#define PSB_2D_DST_4444ARGB             (0x00038000)
+#define PSB_2D_DST_555RGB               (0x00040000)
+#define PSB_2D_DST_1555ARGB             (0x00048000)
+#define PSB_2D_DST_565RGB               (0x00050000)
+#define PSB_2D_DST_0888ARGB             (0x00058000)
+#define PSB_2D_DST_8888ARGB             (0x00060000)
+#define PSB_2D_DST_8888AYUV             (0x00070000)
+
+#define PSB_2D_DST_STRIDE_MASK          (0x00007FFF)
+#define PSB_2D_DST_STRIDE_CLRMASK       (0xFFFF8000)
+#define PSB_2D_DST_STRIDE_SHIFT         (0)
+/*
+ * WORD 1 - Base Address
+ */
+#define PSB_2D_DST_ADDR_MASK            (0x0FFFFFFC)
+#define PSB_2D_DST_ADDR_CLRMASK         (0x00000003)
+#define PSB_2D_DST_ADDR_SHIFT           (2)
+#define PSB_2D_DST_ADDR_ALIGNSHIFT      (2)
+
+/*
+ * Mask Surface (PSB_2D_MASK_SURF_BH)
+ */
+/*
+ * WORD 0
+ */
+#define PSB_2D_MASK_STRIDE_MASK         (0x00007FFF)
+#define PSB_2D_MASK_STRIDE_CLRMASK      (0xFFFF8000)
+#define PSB_2D_MASK_STRIDE_SHIFT        (0)
+/*
+ *  WORD 1 - Base Address
+ */
+#define PSB_2D_MASK_ADDR_MASK           (0x0FFFFFFC)
+#define PSB_2D_MASK_ADDR_CLRMASK        (0x00000003)
+#define PSB_2D_MASK_ADDR_SHIFT          (2)
+#define PSB_2D_MASK_ADDR_ALIGNSHIFT     (2)
+
+/*
+ * Source Palette (PSB_2D_SRC_PAL_BH)
+ */
+
+#define PSB_2D_SRCPAL_ADDR_SHIFT        (0)
+#define PSB_2D_SRCPAL_ADDR_CLRMASK      (0xF0000007)
+#define PSB_2D_SRCPAL_ADDR_MASK         (0x0FFFFFF8)
+#define PSB_2D_SRCPAL_BYTEALIGN         (1024)
+
+/*
+ * Pattern Palette (PSB_2D_PAT_PAL_BH)
+ */
+
+#define PSB_2D_PATPAL_ADDR_SHIFT        (0)
+#define PSB_2D_PATPAL_ADDR_CLRMASK      (0xF0000007)
+#define PSB_2D_PATPAL_ADDR_MASK         (0x0FFFFFF8)
+#define PSB_2D_PATPAL_BYTEALIGN         (1024)
+
+/*
+ * Rop3 Codes (2 LS bytes)
+ */
+
+#define PSB_2D_ROP3_SRCCOPY             (0xCCCC)
+#define PSB_2D_ROP3_PATCOPY             (0xF0F0)
+#define PSB_2D_ROP3_WHITENESS           (0xFFFF)
+#define PSB_2D_ROP3_BLACKNESS           (0x0000)
+#define PSB_2D_ROP3_SRC                 (0xCC)
+#define PSB_2D_ROP3_PAT                 (0xF0)
+#define PSB_2D_ROP3_DST                 (0xAA)
+
+/*
+ * Sizes.
+ */
+
+#define PSB_SCENE_HW_COOKIE_SIZE        16
+#define PSB_TA_MEM_HW_COOKIE_SIZE       16
+
+/*
+ * Scene stuff.
+ */
+
+#define PSB_NUM_HW_SCENES               2
+
+/*
+ * Scheduler completion actions.
+ */
+
+#define PSB_RASTER_BLOCK                0
+#define PSB_RASTER                      1
+#define PSB_RETURN                      2
+#define PSB_TA                          3
+
+/* Power management */
+#define PSB_PUNIT_PORT                  0x04
+#define PSB_OSPMBA                      0x78
+#define PSB_APMBA                       0x7a
+#define PSB_APM_CMD                     0x0
+#define PSB_APM_STS                     0x04
+#define PSB_PWRGT_VID_ENC_MASK          0x30
+#define PSB_PWRGT_VID_DEC_MASK          0xc
+#define PSB_PWRGT_GL3_MASK              0xc0
+
+#define PSB_PM_SSC                      0x20
+#define PSB_PM_SSS                      0x30
+#define PSB_PWRGT_DISPLAY_MASK          0xc /*on a different BA than video/gfx*/
+#define MDFLD_PWRGT_DISPLAY_A_CNTR      0x0000000c
+#define MDFLD_PWRGT_DISPLAY_B_CNTR      0x0000c000
+#define MDFLD_PWRGT_DISPLAY_C_CNTR      0x00030000
+#define MDFLD_PWRGT_DISP_MIPI_CNTR      0x000c0000
+#define MDFLD_PWRGT_DISPLAY_CNTR    (MDFLD_PWRGT_DISPLAY_A_CNTR | MDFLD_PWRGT_DISPLAY_B_CNTR | MDFLD_PWRGT_DISPLAY_C_CNTR | MDFLD_PWRGT_DISP_MIPI_CNTR) /* 0x000fc00c */
+/* Display SSS register bits are different in A0 vs. B0 */
+#define PSB_PWRGT_GFX_MASK              0x3
+#define MDFLD_PWRGT_DISPLAY_A_STS       0x000000c0
+#define MDFLD_PWRGT_DISPLAY_B_STS       0x00000300
+#define MDFLD_PWRGT_DISPLAY_C_STS       0x00000c00
+#define PSB_PWRGT_GFX_MASK_B0           0xc3
+#define MDFLD_PWRGT_DISPLAY_A_STS_B0    0x0000000c
+#define MDFLD_PWRGT_DISPLAY_B_STS_B0    0x0000c000
+#define MDFLD_PWRGT_DISPLAY_C_STS_B0    0x00030000
+#define MDFLD_PWRGT_DISP_MIPI_STS       0x000c0000
+#define MDFLD_PWRGT_DISPLAY_STS_A0    (MDFLD_PWRGT_DISPLAY_A_STS | MDFLD_PWRGT_DISPLAY_B_STS | MDFLD_PWRGT_DISPLAY_C_STS | MDFLD_PWRGT_DISP_MIPI_STS) /* 0x000fc00c */
+#define MDFLD_PWRGT_DISPLAY_STS_B0    (MDFLD_PWRGT_DISPLAY_A_STS_B0 | MDFLD_PWRGT_DISPLAY_B_STS_B0 | MDFLD_PWRGT_DISPLAY_C_STS_B0 | MDFLD_PWRGT_DISP_MIPI_STS) /* 0x000fc00c */
+#endif