Merge branch 'gma500-next' of git://github.com/patjak/drm-gma500 into drm-next

SDVO support for minnowboard

* 'gma500-next' of git://github.com/patjak/drm-gma500:
  drm/gma500/mrst: Add SDVO to output init
  drm/gma500/mrst: Don't blindly guess a mode for LVDS
  drm/gma500/mrst: Setup GMBUS for oaktrail/mrst
  drm/gma500/mrst: Replace WMs and chickenbits with values from EMGD
  drm/gma500/mrst: Add aux register writes to SDVO
  drm/gma500/mrst: Properly route oaktrail hdmi hooks
  drm/gma500/mrst: Add aux register writes when programming pipe
  drm/gma500/mrst: Add SDVO clock calculation
  drm/gma500: Add aux device support for gmbus
  drm/gma500: Add support for aux pci vdc device
  drm/gma500: Add chip specific sdvo masks
  drm/gma500: Add Minnowboard to the IS_MRST() macro

10 files changed, 448 insertions, 257 deletions

diff --git a/drivers/gpu/drm/gma500/cdv_device.c b/drivers/gpu/drm/gma500/cdv_device.c
index 162f686c532d..5a9a6a3063a8 100644
--- a/drivers/gpu/drm/gma500/cdv_device.c
+++ b/drivers/gpu/drm/gma500/cdv_device.c
@@ -634,6 +634,7 @@ const struct psb_ops cdv_chip_ops = {
 	.crtcs = 2,
 	.hdmi_mask = (1 << 0) | (1 << 1),
 	.lvds_mask = (1 << 1),
+	.sdvo_mask = (1 << 0),
 	.cursor_needs_phys = 0,
 	.sgx_offset = MRST_SGX_OFFSET,
 	.chip_setup = cdv_chip_setup,
diff --git a/drivers/gpu/drm/gma500/framebuffer.c b/drivers/gpu/drm/gma500/framebuffer.c
index 01dd7d225762..94b3fec22c28 100644
--- a/drivers/gpu/drm/gma500/framebuffer.c
+++ b/drivers/gpu/drm/gma500/framebuffer.c
@@ -714,7 +714,7 @@ static void psb_setup_outputs(struct drm_device *dev)
 			clone_mask = (1 << INTEL_OUTPUT_ANALOG);
 			break;
 		case INTEL_OUTPUT_SDVO:
-			crtc_mask = ((1 << 0) | (1 << 1));
+			crtc_mask = dev_priv->ops->sdvo_mask;
 			clone_mask = (1 << INTEL_OUTPUT_SDVO);
 			break;
 		case INTEL_OUTPUT_LVDS:
diff --git a/drivers/gpu/drm/gma500/intel_gmbus.c b/drivers/gpu/drm/gma500/intel_gmbus.c
index 62cd42e88f28..566d330aaeea 100644
--- a/drivers/gpu/drm/gma500/intel_gmbus.c
+++ b/drivers/gpu/drm/gma500/intel_gmbus.c
@@ -51,6 +51,9 @@
 #define wait_for(COND, MS) _wait_for(COND, MS, 1)
 #define wait_for_atomic(COND, MS) _wait_for(COND, MS, 0)
 
+#define GMBUS_REG_READ(reg) ioread32(dev_priv->gmbus_reg + (reg))
+#define GMBUS_REG_WRITE(reg, val) iowrite32((val), dev_priv->gmbus_reg + (reg))
+
 /* Intel GPIO access functions */
 
 #define I2C_RISEFALL_TIME 20
@@ -71,7 +74,8 @@ struct intel_gpio {
 void
 gma_intel_i2c_reset(struct drm_device *dev)
 {
-	REG_WRITE(GMBUS0, 0);
+	struct drm_psb_private *dev_priv = dev->dev_private;
+	GMBUS_REG_WRITE(GMBUS0, 0);
 }
 
 static void intel_i2c_quirk_set(struct drm_psb_private *dev_priv, bool enable)
@@ -98,11 +102,10 @@ static void intel_i2c_quirk_set(struct drm_psb_private *dev_priv, bool enable)
 static u32 get_reserved(struct intel_gpio *gpio)
 {
 	struct drm_psb_private *dev_priv = gpio->dev_priv;
-	struct drm_device *dev = dev_priv->dev;
 	u32 reserved = 0;
 
 	/* On most chips, these bits must be preserved in software. */
-	reserved = REG_READ(gpio->reg) &
+	reserved = GMBUS_REG_READ(gpio->reg) &
 				     (GPIO_DATA_PULLUP_DISABLE |
 				      GPIO_CLOCK_PULLUP_DISABLE);
 
@@ -113,29 +116,26 @@ static int get_clock(void *data)
 {
 	struct intel_gpio *gpio = data;
 	struct drm_psb_private *dev_priv = gpio->dev_priv;
-	struct drm_device *dev = dev_priv->dev;
 	u32 reserved = get_reserved(gpio);
-	REG_WRITE(gpio->reg, reserved | GPIO_CLOCK_DIR_MASK);
-	REG_WRITE(gpio->reg, reserved);
-	return (REG_READ(gpio->reg) & GPIO_CLOCK_VAL_IN) != 0;
+	GMBUS_REG_WRITE(gpio->reg, reserved | GPIO_CLOCK_DIR_MASK);
+	GMBUS_REG_WRITE(gpio->reg, reserved);
+	return (GMBUS_REG_READ(gpio->reg) & GPIO_CLOCK_VAL_IN) != 0;
 }
 
 static int get_data(void *data)
 {
 	struct intel_gpio *gpio = data;
 	struct drm_psb_private *dev_priv = gpio->dev_priv;
-	struct drm_device *dev = dev_priv->dev;
 	u32 reserved = get_reserved(gpio);
-	REG_WRITE(gpio->reg, reserved | GPIO_DATA_DIR_MASK);
-	REG_WRITE(gpio->reg, reserved);
-	return (REG_READ(gpio->reg) & GPIO_DATA_VAL_IN) != 0;
+	GMBUS_REG_WRITE(gpio->reg, reserved | GPIO_DATA_DIR_MASK);
+	GMBUS_REG_WRITE(gpio->reg, reserved);
+	return (GMBUS_REG_READ(gpio->reg) & GPIO_DATA_VAL_IN) != 0;
 }
 
 static void set_clock(void *data, int state_high)
 {
 	struct intel_gpio *gpio = data;
 	struct drm_psb_private *dev_priv = gpio->dev_priv;
-	struct drm_device *dev = dev_priv->dev;
 	u32 reserved = get_reserved(gpio);
 	u32 clock_bits;
 
@@ -145,15 +145,14 @@ static void set_clock(void *data, int state_high)
 		clock_bits = GPIO_CLOCK_DIR_OUT | GPIO_CLOCK_DIR_MASK |
 			GPIO_CLOCK_VAL_MASK;
 
-	REG_WRITE(gpio->reg, reserved | clock_bits);
-	REG_READ(gpio->reg); /* Posting */
+	GMBUS_REG_WRITE(gpio->reg, reserved | clock_bits);
+	GMBUS_REG_READ(gpio->reg); /* Posting */
 }
 
 static void set_data(void *data, int state_high)
 {
 	struct intel_gpio *gpio = data;
 	struct drm_psb_private *dev_priv = gpio->dev_priv;
-	struct drm_device *dev = dev_priv->dev;
 	u32 reserved = get_reserved(gpio);
 	u32 data_bits;
 
@@ -163,8 +162,8 @@ static void set_data(void *data, int state_high)
 		data_bits = GPIO_DATA_DIR_OUT | GPIO_DATA_DIR_MASK |
 			GPIO_DATA_VAL_MASK;
 
-	REG_WRITE(gpio->reg, reserved | data_bits);
-	REG_READ(gpio->reg);
+	GMBUS_REG_WRITE(gpio->reg, reserved | data_bits);
+	GMBUS_REG_READ(gpio->reg);
 }
 
 static struct i2c_adapter *
@@ -251,7 +250,6 @@ gmbus_xfer(struct i2c_adapter *adapter,
 					       struct intel_gmbus,
 					       adapter);
 	struct drm_psb_private *dev_priv = adapter->algo_data;
-	struct drm_device *dev = dev_priv->dev;
 	int i, reg_offset;
 
 	if (bus->force_bit)
@@ -260,28 +258,30 @@ gmbus_xfer(struct i2c_adapter *adapter,
 
 	reg_offset = 0;
 
-	REG_WRITE(GMBUS0 + reg_offset, bus->reg0);
+	GMBUS_REG_WRITE(GMBUS0 + reg_offset, bus->reg0);
 
 	for (i = 0; i < num; i++) {
 		u16 len = msgs[i].len;
 		u8 *buf = msgs[i].buf;
 
 		if (msgs[i].flags & I2C_M_RD) {
-			REG_WRITE(GMBUS1 + reg_offset,
-				   GMBUS_CYCLE_WAIT | (i + 1 == num ? GMBUS_CYCLE_STOP : 0) |
-				   (len << GMBUS_BYTE_COUNT_SHIFT) |
-				   (msgs[i].addr << GMBUS_SLAVE_ADDR_SHIFT) |
-				   GMBUS_SLAVE_READ | GMBUS_SW_RDY);
-			REG_READ(GMBUS2+reg_offset);
+			GMBUS_REG_WRITE(GMBUS1 + reg_offset,
+					GMBUS_CYCLE_WAIT |
+					(i + 1 == num ? GMBUS_CYCLE_STOP : 0) |
+					(len << GMBUS_BYTE_COUNT_SHIFT) |
+					(msgs[i].addr << GMBUS_SLAVE_ADDR_SHIFT) |
+					GMBUS_SLAVE_READ | GMBUS_SW_RDY);
+			GMBUS_REG_READ(GMBUS2+reg_offset);
 			do {
 				u32 val, loop = 0;
 
-				if (wait_for(REG_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_RDY), 50))
+				if (wait_for(GMBUS_REG_READ(GMBUS2 + reg_offset) &
+					     (GMBUS_SATOER | GMBUS_HW_RDY), 50))
 					goto timeout;
-				if (REG_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
+				if (GMBUS_REG_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
 					goto clear_err;
 
-				val = REG_READ(GMBUS3 + reg_offset);
+				val = GMBUS_REG_READ(GMBUS3 + reg_offset);
 				do {
 					*buf++ = val & 0xff;
 					val >>= 8;
@@ -295,18 +295,20 @@ gmbus_xfer(struct i2c_adapter *adapter,
 				val |= *buf++ << (8 * loop);
 			} while (--len && ++loop < 4);
 
-			REG_WRITE(GMBUS3 + reg_offset, val);
-			REG_WRITE(GMBUS1 + reg_offset,
+			GMBUS_REG_WRITE(GMBUS3 + reg_offset, val);
+			GMBUS_REG_WRITE(GMBUS1 + reg_offset,
 				   (i + 1 == num ? GMBUS_CYCLE_STOP : GMBUS_CYCLE_WAIT) |
 				   (msgs[i].len << GMBUS_BYTE_COUNT_SHIFT) |
 				   (msgs[i].addr << GMBUS_SLAVE_ADDR_SHIFT) |
 				   GMBUS_SLAVE_WRITE | GMBUS_SW_RDY);
-			REG_READ(GMBUS2+reg_offset);
+			GMBUS_REG_READ(GMBUS2+reg_offset);
 
 			while (len) {
-				if (wait_for(REG_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_RDY), 50))
+				if (wait_for(GMBUS_REG_READ(GMBUS2 + reg_offset) &
+					     (GMBUS_SATOER | GMBUS_HW_RDY), 50))
 					goto timeout;
-				if (REG_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
+				if (GMBUS_REG_READ(GMBUS2 + reg_offset) &
+				    GMBUS_SATOER)
 					goto clear_err;
 
 				val = loop = 0;
@@ -314,14 +316,14 @@ gmbus_xfer(struct i2c_adapter *adapter,
 					val |= *buf++ << (8 * loop);
 				} while (--len && ++loop < 4);
 
-				REG_WRITE(GMBUS3 + reg_offset, val);
-				REG_READ(GMBUS2+reg_offset);
+				GMBUS_REG_WRITE(GMBUS3 + reg_offset, val);
+				GMBUS_REG_READ(GMBUS2+reg_offset);
 			}
 		}
 
-		if (i + 1 < num && wait_for(REG_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_WAIT_PHASE), 50))
+		if (i + 1 < num && wait_for(GMBUS_REG_READ(GMBUS2 + reg_offset) & (GMBUS_SATOER | GMBUS_HW_WAIT_PHASE), 50))
 			goto timeout;
-		if (REG_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
+		if (GMBUS_REG_READ(GMBUS2 + reg_offset) & GMBUS_SATOER)
 			goto clear_err;
 	}
 
@@ -332,20 +334,20 @@ clear_err:
 	 * of resetting the GMBUS controller and so clearing the
 	 * BUS_ERROR raised by the slave's NAK.
 	 */
-	REG_WRITE(GMBUS1 + reg_offset, GMBUS_SW_CLR_INT);
-	REG_WRITE(GMBUS1 + reg_offset, 0);
+	GMBUS_REG_WRITE(GMBUS1 + reg_offset, GMBUS_SW_CLR_INT);
+	GMBUS_REG_WRITE(GMBUS1 + reg_offset, 0);
 
 done:
 	/* Mark the GMBUS interface as disabled. We will re-enable it at the
 	 * start of the next xfer, till then let it sleep.
 	 */
-	REG_WRITE(GMBUS0 + reg_offset, 0);
+	GMBUS_REG_WRITE(GMBUS0 + reg_offset, 0);
 	return i;
 
 timeout:
 	DRM_INFO("GMBUS timed out, falling back to bit banging on pin %d [%s]\n",
 		 bus->reg0 & 0xff, bus->adapter.name);
-	REG_WRITE(GMBUS0 + reg_offset, 0);
+	GMBUS_REG_WRITE(GMBUS0 + reg_offset, 0);
 
 	/* Hardware may not support GMBUS over these pins? Try GPIO bitbanging instead. */
 	bus->force_bit = intel_gpio_create(dev_priv, bus->reg0 & 0xff);
@@ -399,6 +401,11 @@ int gma_intel_setup_gmbus(struct drm_device *dev)
 	if (dev_priv->gmbus == NULL)
 		return -ENOMEM;
 
+	if (IS_MRST(dev))
+		dev_priv->gmbus_reg = dev_priv->aux_reg;
+	else
+		dev_priv->gmbus_reg = dev_priv->vdc_reg;
+
 	for (i = 0; i < GMBUS_NUM_PORTS; i++) {
 		struct intel_gmbus *bus = &dev_priv->gmbus[i];
 
@@ -487,6 +494,7 @@ void gma_intel_teardown_gmbus(struct drm_device *dev)
 		i2c_del_adapter(&bus->adapter);
 	}
 
+	dev_priv->gmbus_reg = NULL; /* iounmap is done in driver_unload */
 	kfree(dev_priv->gmbus);
 	dev_priv->gmbus = NULL;
 }
diff --git a/drivers/gpu/drm/gma500/oaktrail_crtc.c b/drivers/gpu/drm/gma500/oaktrail_crtc.c
index 54c98962b73e..8195e8592107 100644
--- a/drivers/gpu/drm/gma500/oaktrail_crtc.c
+++ b/drivers/gpu/drm/gma500/oaktrail_crtc.c
@@ -26,24 +26,10 @@
 #include "gma_display.h"
 #include "power.h"
 
-struct psb_intel_range_t {
-	int min, max;
-};
-
-struct oaktrail_limit_t {
-	struct psb_intel_range_t dot, m, p1;
-};
-
-struct oaktrail_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_LIMIT_LVDS_100L	0
+#define MRST_LIMIT_LVDS_83	1
+#define MRST_LIMIT_LVDS_100	2
+#define MRST_LIMIT_SDVO		3
 
 #define MRST_DOT_MIN		  19750
 #define MRST_DOT_MAX		  120000
@@ -57,21 +43,40 @@ struct oaktrail_clock_t {
 #define MRST_P1_MAX_0		    7
 #define MRST_P1_MAX_1		    8
 
-static const struct oaktrail_limit_t oaktrail_limits[] = {
+static bool mrst_lvds_find_best_pll(const struct gma_limit_t *limit,
+				    struct drm_crtc *crtc, int target,
+				    int refclk, struct gma_clock_t *best_clock);
+
+static bool mrst_sdvo_find_best_pll(const struct gma_limit_t *limit,
+				    struct drm_crtc *crtc, int target,
+				    int refclk, struct gma_clock_t *best_clock);
+
+static const struct gma_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},
+	 .find_pll = mrst_lvds_find_best_pll,
 	 },
 	{			/* 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},
+	 .find_pll = mrst_lvds_find_best_pll,
 	 },
 	{			/* 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},
+	 .find_pll = mrst_lvds_find_best_pll,
+	 },
+	{			/* MRST_LIMIT_SDVO */
+	 .vco = {.min = 1400000, .max = 2800000},
+	 .n = {.min = 3, .max = 7},
+	 .m = {.min = 80, .max = 137},
+	 .p1 = {.min = 1, .max = 2},
+	 .p2 = {.dot_limit = 200000, .p2_slow = 10, .p2_fast = 10},
+	 .find_pll = mrst_sdvo_find_best_pll,
 	 },
 };
 
@@ -82,9 +87,10 @@ static const u32 oaktrail_m_converts[] = {
 	0x12, 0x09, 0x24, 0x32, 0x39, 0x1c,
 };
 
-static const struct oaktrail_limit_t *oaktrail_limit(struct drm_crtc *crtc)
+static const struct gma_limit_t *mrst_limit(struct drm_crtc *crtc,
+					    int refclk)
 {
-	const struct oaktrail_limit_t *limit = NULL;
+	const struct gma_limit_t *limit = NULL;
 	struct drm_device *dev = crtc->dev;
 	struct drm_psb_private *dev_priv = dev->dev_private;
 
@@ -92,45 +98,100 @@ static const struct oaktrail_limit_t *oaktrail_limit(struct drm_crtc *crtc)
 	    || gma_pipe_has_type(crtc, INTEL_OUTPUT_MIPI)) {
 		switch (dev_priv->core_freq) {
 		case 100:
-			limit = &oaktrail_limits[MRST_LIMIT_LVDS_100L];
+			limit = &mrst_limits[MRST_LIMIT_LVDS_100L];
 			break;
 		case 166:
-			limit = &oaktrail_limits[MRST_LIMIT_LVDS_83];
+			limit = &mrst_limits[MRST_LIMIT_LVDS_83];
 			break;
 		case 200:
-			limit = &oaktrail_limits[MRST_LIMIT_LVDS_100];
+			limit = &mrst_limits[MRST_LIMIT_LVDS_100];
 			break;
 		}
+	} else if (gma_pipe_has_type(crtc, INTEL_OUTPUT_SDVO)) {
+		limit = &mrst_limits[MRST_LIMIT_SDVO];
 	} else {
 		limit = NULL;
-		dev_err(dev->dev, "oaktrail_limit Wrong display type.\n");
+		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 oaktrail_clock(int refclk, struct oaktrail_clock_t *clock)
+static void mrst_lvds_clock(int refclk, struct gma_clock_t *clock)
 {
 	clock->dot = (refclk * clock->m) / (14 * clock->p1);
 }
 
-static void mrstPrintPll(char *prefix, struct oaktrail_clock_t *clock)
+static void mrst_print_pll(struct gma_clock_t *clock)
 {
-	pr_debug("%s: dotclock = %d,  m = %d, p1 = %d.\n",
-	     prefix, clock->dot, clock->m, clock->p1);
+	DRM_DEBUG_DRIVER("dotclock=%d,  m=%d, m1=%d, m2=%d, n=%d, p1=%d, p2=%d\n",
+			 clock->dot, clock->m, clock->m1, clock->m2, clock->n,
+			 clock->p1, clock->p2);
+}
+
+static bool mrst_sdvo_find_best_pll(const struct gma_limit_t *limit,
+				    struct drm_crtc *crtc, int target,
+				    int refclk, struct gma_clock_t *best_clock)
+{
+	struct gma_clock_t clock;
+	u32 target_vco, actual_freq;
+	s32 freq_error, min_error = 100000;
+
+	memset(best_clock, 0, sizeof(*best_clock));
+
+	for (clock.m = limit->m.min; clock.m <= limit->m.max; clock.m++) {
+		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++) {
+				/* p2 value always stored in p2_slow on SDVO */
+				clock.p = clock.p1 * limit->p2.p2_slow;
+				target_vco = target * clock.p;
+
+				/* VCO will increase at this point so break */
+				if (target_vco > limit->vco.max)
+					break;
+
+				if (target_vco < limit->vco.min)
+					continue;
+
+				actual_freq = (refclk * clock.m) /
+					      (clock.n * clock.p);
+				freq_error = 10000 -
+					     ((target * 10000) / actual_freq);
+
+				if (freq_error < -min_error) {
+					/* freq_error will start to decrease at
+					   this point so break */
+					break;
+				}
+
+				if (freq_error < 0)
+					freq_error = -freq_error;
+
+				if (freq_error < min_error) {
+					min_error = freq_error;
+					*best_clock = clock;
+				}
+			}
+		}
+		if (min_error == 0)
+			break;
+	}
+
+	return min_error == 0;
 }
 
 /**
  * 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 oaktrail_clock_t *best_clock)
+static bool mrst_lvds_find_best_pll(const struct gma_limit_t *limit,
+				    struct drm_crtc *crtc, int target,
+				    int refclk, struct gma_clock_t *best_clock)
 {
-	struct oaktrail_clock_t clock;
-	const struct oaktrail_limit_t *limit = oaktrail_limit(crtc);
+	struct gma_clock_t clock;
 	int err = target;
 
 	memset(best_clock, 0, sizeof(*best_clock));
@@ -140,7 +201,7 @@ mrstFindBestPLL(struct drm_crtc *crtc, int target, int refclk,
 		     clock.p1++) {
 			int this_err;
 
-			oaktrail_clock(refclk, &clock);
+			mrst_lvds_clock(refclk, &clock);
 
 			this_err = abs(clock.dot - target);
 			if (this_err < err) {
@@ -149,7 +210,6 @@ mrstFindBestPLL(struct drm_crtc *crtc, int target, int refclk,
 			}
 		}
 	}
-	dev_dbg(crtc->dev->dev, "mrstFindBestPLL err = %d.\n", err);
 	return err != target;
 }
 
@@ -167,8 +227,10 @@ static void oaktrail_crtc_dpms(struct drm_crtc *crtc, int mode)
 	int pipe = gma_crtc->pipe;
 	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	u32 temp;
+	int i;
+	int need_aux = gma_pipe_has_type(crtc, INTEL_OUTPUT_SDVO) ? 1 : 0;
 
-	if (pipe == 1) {
+	if (gma_pipe_has_type(crtc, INTEL_OUTPUT_HDMI)) {
 		oaktrail_crtc_hdmi_dpms(crtc, mode);
 		return;
 	}
@@ -183,35 +245,45 @@ static void oaktrail_crtc_dpms(struct drm_crtc *crtc, int mode)
 	case DRM_MODE_DPMS_ON:
 	case DRM_MODE_DPMS_STANDBY:
 	case DRM_MODE_DPMS_SUSPEND:
-		/* Enable the DPLL */
-		temp = REG_READ(map->dpll);
-		if ((temp & DPLL_VCO_ENABLE) == 0) {
-			REG_WRITE(map->dpll, temp);
-			REG_READ(map->dpll);
-			/* Wait for the clocks to stabilize. */
-			udelay(150);
-			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
-			REG_READ(map->dpll);
-			/* Wait for the clocks to stabilize. */
-			udelay(150);
-			REG_WRITE(map->dpll, temp | DPLL_VCO_ENABLE);
-			REG_READ(map->dpll);
-			/* Wait for the clocks to stabilize. */
-			udelay(150);
-		}
-		/* Enable the pipe */
-		temp = REG_READ(map->conf);
-		if ((temp & PIPEACONF_ENABLE) == 0)
-			REG_WRITE(map->conf, temp | PIPEACONF_ENABLE);
-		/* Enable the plane */
-		temp = REG_READ(map->cntr);
-		if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
-			REG_WRITE(map->cntr,
-				  temp | DISPLAY_PLANE_ENABLE);
-			/* Flush the plane changes */
-			REG_WRITE(map->base, REG_READ(map->base));
-		}
+		for (i = 0; i <= need_aux; i++) {
+			/* Enable the DPLL */
+			temp = REG_READ_WITH_AUX(map->dpll, i);
+			if ((temp & DPLL_VCO_ENABLE) == 0) {
+				REG_WRITE_WITH_AUX(map->dpll, temp, i);
+				REG_READ_WITH_AUX(map->dpll, i);
+				/* Wait for the clocks to stabilize. */
+				udelay(150);
+				REG_WRITE_WITH_AUX(map->dpll,
+						   temp | DPLL_VCO_ENABLE, i);
+				REG_READ_WITH_AUX(map->dpll, i);
+				/* Wait for the clocks to stabilize. */
+				udelay(150);
+				REG_WRITE_WITH_AUX(map->dpll,
+						   temp | DPLL_VCO_ENABLE, i);
+				REG_READ_WITH_AUX(map->dpll, i);
+				/* Wait for the clocks to stabilize. */
+				udelay(150);
+			}
+
+			/* Enable the pipe */
+			temp = REG_READ_WITH_AUX(map->conf, i);
+			if ((temp & PIPEACONF_ENABLE) == 0) {
+				REG_WRITE_WITH_AUX(map->conf,
+						   temp | PIPEACONF_ENABLE, i);
+			}
+
+			/* Enable the plane */
+			temp = REG_READ_WITH_AUX(map->cntr, i);
+			if ((temp & DISPLAY_PLANE_ENABLE) == 0) {
+				REG_WRITE_WITH_AUX(map->cntr,
+						   temp | DISPLAY_PLANE_ENABLE,
+						   i);
+				/* Flush the plane changes */
+				REG_WRITE_WITH_AUX(map->base,
+					REG_READ_WITH_AUX(map->base, i), i);
+			}
 
+		}
 		gma_crtc_load_lut(crtc);
 
 		/* Give the overlay scaler a chance to enable
@@ -223,48 +295,52 @@ static void oaktrail_crtc_dpms(struct drm_crtc *crtc, int mode)
 		 * 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(map->cntr);
-		if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
-			REG_WRITE(map->cntr,
-				  temp & ~DISPLAY_PLANE_ENABLE);
-			/* Flush the plane changes */
-			REG_WRITE(map->base, REG_READ(map->base));
-			REG_READ(map->base);
-		}
+		for (i = 0; i <= need_aux; i++) {
+			/* Disable the VGA plane that we never use */
+			REG_WRITE_WITH_AUX(VGACNTRL, VGA_DISP_DISABLE, i);
+			/* Disable display plane */
+			temp = REG_READ_WITH_AUX(map->cntr, i);
+			if ((temp & DISPLAY_PLANE_ENABLE) != 0) {
+				REG_WRITE_WITH_AUX(map->cntr,
+					temp & ~DISPLAY_PLANE_ENABLE, i);
+				/* Flush the plane changes */
+				REG_WRITE_WITH_AUX(map->base,
+						   REG_READ(map->base), i);
+				REG_READ_WITH_AUX(map->base, i);
+			}
 
-		/* Next, disable display pipes */
-		temp = REG_READ(map->conf);
-		if ((temp & PIPEACONF_ENABLE) != 0) {
-			REG_WRITE(map->conf, temp & ~PIPEACONF_ENABLE);
-			REG_READ(map->conf);
-		}
-		/* Wait for for the pipe disable to take effect. */
-		gma_wait_for_vblank(dev);
+			/* Next, disable display pipes */
+			temp = REG_READ_WITH_AUX(map->conf, i);
+			if ((temp & PIPEACONF_ENABLE) != 0) {
+				REG_WRITE_WITH_AUX(map->conf,
+						   temp & ~PIPEACONF_ENABLE, i);
+				REG_READ_WITH_AUX(map->conf, i);
+			}
+			/* Wait for for the pipe disable to take effect. */
+			gma_wait_for_vblank(dev);
+
+			temp = REG_READ_WITH_AUX(map->dpll, i);
+			if ((temp & DPLL_VCO_ENABLE) != 0) {
+				REG_WRITE_WITH_AUX(map->dpll,
+						   temp & ~DPLL_VCO_ENABLE, i);
+				REG_READ_WITH_AUX(map->dpll, i);
+			}
 
-		temp = REG_READ(map->dpll);
-		if ((temp & DPLL_VCO_ENABLE) != 0) {
-			REG_WRITE(map->dpll, temp & ~DPLL_VCO_ENABLE);
-			REG_READ(map->dpll);
+			/* Wait for the clocks to turn off. */
+			udelay(150);
 		}
-
-		/* Wait for the clocks to turn off. */
-		udelay(150);
 		break;
 	}
 
-	/*Set FIFO Watermarks*/
-	REG_WRITE(DSPARB, 0x3FFF);
-	REG_WRITE(DSPFW1, 0x3F88080A);
-	REG_WRITE(DSPFW2, 0x0b060808);
+	/* Set FIFO Watermarks (values taken from EMGD) */
+	REG_WRITE(DSPARB, 0x3f80);
+	REG_WRITE(DSPFW1, 0x3f8f0404);
+	REG_WRITE(DSPFW2, 0x04040f04);
 	REG_WRITE(DSPFW3, 0x0);
-	REG_WRITE(DSPFW4, 0x08030404);
+	REG_WRITE(DSPFW4, 0x04040404);
 	REG_WRITE(DSPFW5, 0x04040404);
 	REG_WRITE(DSPFW6, 0x78);
-	REG_WRITE(0x70400, REG_READ(0x70400) | 0x4000);
-	/* Must write Bit 14 of the Chicken Bit Register */
+	REG_WRITE(DSPCHICKENBIT, REG_READ(DSPCHICKENBIT) | 0xc040);
 
 	gma_power_end(dev);
 }
@@ -297,7 +373,8 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 	int pipe = gma_crtc->pipe;
 	const struct psb_offset *map = &dev_priv->regmap[pipe];
 	int refclk = 0;
-	struct oaktrail_clock_t clock;
+	struct gma_clock_t clock;
+	const struct gma_limit_t *limit;
 	u32 dpll = 0, fp = 0, dspcntr, pipeconf;
 	bool ok, is_sdvo = false;
 	bool is_lvds = false;
@@ -306,8 +383,10 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 	struct gma_encoder *gma_encoder = NULL;
 	uint64_t scalingType = DRM_MODE_SCALE_FULLSCREEN;
 	struct drm_connector *connector;
+	int i;
+	int need_aux = gma_pipe_has_type(crtc, INTEL_OUTPUT_SDVO) ? 1 : 0;
 
-	if (pipe == 1)
+	if (gma_pipe_has_type(crtc, INTEL_OUTPUT_HDMI))
 		return oaktrail_crtc_hdmi_mode_set(crtc, mode, adjusted_mode, x, y, old_fb);
 
 	if (!gma_power_begin(dev, true))
@@ -340,15 +419,17 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 	}
 
 	/* Disable the VGA plane that we never use */
-	REG_WRITE(VGACNTRL, VGA_DISP_DISABLE);
+	for (i = 0; i <= need_aux; i++)
+		REG_WRITE_WITH_AUX(VGACNTRL, VGA_DISP_DISABLE, i);
 
 	/* Disable the panel fitter if it was on our pipe */
 	if (oaktrail_panel_fitter_pipe(dev) == pipe)
 		REG_WRITE(PFIT_CONTROL, 0);
 
-	REG_WRITE(map->src,
-		  ((mode->crtc_hdisplay - 1) << 16) |
-		  (mode->crtc_vdisplay - 1));
+	for (i = 0; i <= need_aux; i++) {
+		REG_WRITE_WITH_AUX(map->src, ((mode->crtc_hdisplay - 1) << 16) |
+					     (mode->crtc_vdisplay - 1), i);
+	}
 
 	if (gma_encoder)
 		drm_object_property_get_value(&connector->base,
@@ -365,35 +446,39 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 		offsetY = (adjusted_mode->crtc_vdisplay -
 			   mode->crtc_vdisplay) / 2;
 
-		REG_WRITE(map->htotal, (mode->crtc_hdisplay - 1) |
-			((adjusted_mode->crtc_htotal - 1) << 16));
-		REG_WRITE(map->vtotal, (mode->crtc_vdisplay - 1) |
-			((adjusted_mode->crtc_vtotal - 1) << 16));
-		REG_WRITE(map->hblank,
-			(adjusted_mode->crtc_hblank_start - offsetX - 1) |
-			((adjusted_mode->crtc_hblank_end - offsetX - 1) << 16));
-		REG_WRITE(map->hsync,
-			(adjusted_mode->crtc_hsync_start - offsetX - 1) |
-			((adjusted_mode->crtc_hsync_end - offsetX - 1) << 16));
-		REG_WRITE(map->vblank,
-			(adjusted_mode->crtc_vblank_start - offsetY - 1) |
-			((adjusted_mode->crtc_vblank_end - offsetY - 1) << 16));
-		REG_WRITE(map->vsync,
-			(adjusted_mode->crtc_vsync_start - offsetY - 1) |
-			((adjusted_mode->crtc_vsync_end - offsetY - 1) << 16));
+		for (i = 0; i <= need_aux; i++) {
+			REG_WRITE_WITH_AUX(map->htotal, (mode->crtc_hdisplay - 1) |
+				((adjusted_mode->crtc_htotal - 1) << 16), i);
+			REG_WRITE_WITH_AUX(map->vtotal, (mode->crtc_vdisplay - 1) |
+				((adjusted_mode->crtc_vtotal - 1) << 16), i);
+			REG_WRITE_WITH_AUX(map->hblank,
+				(adjusted_mode->crtc_hblank_start - offsetX - 1) |
+				((adjusted_mode->crtc_hblank_end - offsetX - 1) << 16), i);
+			REG_WRITE_WITH_AUX(map->hsync,
+				(adjusted_mode->crtc_hsync_start - offsetX - 1) |
+				((adjusted_mode->crtc_hsync_end - offsetX - 1) << 16), i);
+			REG_WRITE_WITH_AUX(map->vblank,
+				(adjusted_mode->crtc_vblank_start - offsetY - 1) |
+				((adjusted_mode->crtc_vblank_end - offsetY - 1) << 16), i);
+			REG_WRITE_WITH_AUX(map->vsync,
+				(adjusted_mode->crtc_vsync_start - offsetY - 1) |
+				((adjusted_mode->crtc_vsync_end - offsetY - 1) << 16), i);
+		}
 	} else {
-		REG_WRITE(map->htotal, (adjusted_mode->crtc_hdisplay - 1) |
-			((adjusted_mode->crtc_htotal - 1) << 16));
-		REG_WRITE(map->vtotal, (adjusted_mode->crtc_vdisplay - 1) |
-			((adjusted_mode->crtc_vtotal - 1) << 16));
-		REG_WRITE(map->hblank, (adjusted_mode->crtc_hblank_start - 1) |
-			((adjusted_mode->crtc_hblank_end - 1) << 16));
-		REG_WRITE(map->hsync, (adjusted_mode->crtc_hsync_start - 1) |
-			((adjusted_mode->crtc_hsync_end - 1) << 16));
-		REG_WRITE(map->vblank, (adjusted_mode->crtc_vblank_start - 1) |
-			((adjusted_mode->crtc_vblank_end - 1) << 16));
-		REG_WRITE(map->vsync, (adjusted_mode->crtc_vsync_start - 1) |
-			((adjusted_mode->crtc_vsync_end - 1) << 16));
+		for (i = 0; i <= need_aux; i++) {
+			REG_WRITE_WITH_AUX(map->htotal, (adjusted_mode->crtc_hdisplay - 1) |
+				((adjusted_mode->crtc_htotal - 1) << 16), i);
+			REG_WRITE_WITH_AUX(map->vtotal, (adjusted_mode->crtc_vdisplay - 1) |
+				((adjusted_mode->crtc_vtotal - 1) << 16), i);
+			REG_WRITE_WITH_AUX(map->hblank, (adjusted_mode->crtc_hblank_start - 1) |
+				((adjusted_mode->crtc_hblank_end - 1) << 16), i);
+			REG_WRITE_WITH_AUX(map->hsync, (adjusted_mode->crtc_hsync_start - 1) |
+				((adjusted_mode->crtc_hsync_end - 1) << 16), i);
+			REG_WRITE_WITH_AUX(map->vblank, (adjusted_mode->crtc_vblank_start - 1) |
+				((adjusted_mode->crtc_vblank_end - 1) << 16), i);
+			REG_WRITE_WITH_AUX(map->vsync, (adjusted_mode->crtc_vsync_start - 1) |
+				((adjusted_mode->crtc_vsync_end - 1) << 16), i);
+		}
 	}
 
 	/* Flush the plane changes */
@@ -418,21 +503,30 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 	if (is_mipi)
 		goto oaktrail_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);
+	refclk = is_sdvo ? 96000 : dev_priv->core_freq * 1000;
+	limit = mrst_limit(crtc, refclk);
+	ok = limit->find_pll(limit, crtc, adjusted_mode->clock,
+			     refclk, &clock);
 
-	if (!ok) {
-		dev_dbg(dev->dev, "mrstFindBestPLL fail in oaktrail_crtc_mode_set.\n");
-	} else {
-		dev_dbg(dev->dev, "oaktrail_crtc_mode_set pixel clock = %d,"
-			 "m = %x, p1 = %x.\n", clock.dot, clock.m,
-			 clock.p1);
+	if (is_sdvo) {
+		/* Convert calculated values to register values */
+		clock.p1 = (1L << (clock.p1 - 1));
+		clock.m -= 2;
+		clock.n = (1L << (clock.n - 1));
 	}
 
-	fp = oaktrail_m_converts[(clock.m - MRST_M_MIN)] << 8;
+	if (!ok)
+		DRM_ERROR("Failed to find proper PLL settings");
+
+	mrst_print_pll(&clock);
+
+	if (is_sdvo)
+		fp = clock.n << 16 | clock.m;
+	else
+		fp = oaktrail_m_converts[(clock.m - MRST_M_MIN)] << 8;
 
 	dpll |= DPLL_VGA_MODE_DIS;
 
@@ -456,38 +550,43 @@ static int oaktrail_crtc_mode_set(struct drm_crtc *crtc,
 
 
 	/* compute bitmask from p1 value */
-	dpll |= (1 << (clock.p1 - 2)) << 17;
+	if (is_sdvo)
+		dpll |= clock.p1 << 16; // dpll |= (1 << (clock.p1 - 1)) << 16;
+	else
+		dpll |= (1 << (clock.p1 - 2)) << 17;
 
 	dpll |= DPLL_VCO_ENABLE;
 
-	mrstPrintPll("chosen", &clock);
-
 	if (dpll & DPLL_VCO_ENABLE) {
-		REG_WRITE(map->fp0, fp);
-		REG_WRITE(map->dpll, dpll & ~DPLL_VCO_ENABLE);
-		REG_READ(map->dpll);
-		/* Check the DPLLA lock bit PIPEACONF[29] */
-		udelay(150);
+		for (i = 0; i <= need_aux; i++) {
+			REG_WRITE_WITH_AUX(map->fp0, fp, i);
+			REG_WRITE_WITH_AUX(map->dpll, dpll & ~DPLL_VCO_ENABLE, i);
+			REG_READ_WITH_AUX(map->dpll, i);
+			/* Check the DPLLA lock bit PIPEACONF[29] */
+			udelay(150);
+		}
 	}
 
-	REG_WRITE(map->fp0, fp);
-	REG_WRITE(map->dpll, dpll);
-	REG_READ(map->dpll);
-	/* Wait for the clocks to stabilize. */
-	udelay(150);
+	for (i = 0; i <= need_aux; i++) {
+		REG_WRITE_WITH_AUX(map->fp0, fp, i);
+		REG_WRITE_WITH_AUX(map->dpll, dpll, i);
+		REG_READ_WITH_AUX(map->dpll, i);
+		/* Wait for the clocks to stabilize. */
+		udelay(150);
 
-	/* write it again -- the BIOS does, after all */
-	REG_WRITE(map->dpll, dpll);
-	REG_READ(map->dpll);
-	/* Wait for the clocks to stabilize. */
-	udelay(150);
+		/* write it again -- the BIOS does, after all */
+		REG_WRITE_WITH_AUX(map->dpll, dpll, i);
+		REG_READ_WITH_AUX(map->dpll, i);
+		/* Wait for the clocks to stabilize. */
+		udelay(150);
 
-	REG_WRITE(map->conf, pipeconf);
-	REG_READ(map->conf);
-	gma_wait_for_vblank(dev);
+		REG_WRITE_WITH_AUX(map->conf, pipeconf, i);
+		REG_READ_WITH_AUX(map->conf, i);
+		gma_wait_for_vblank(dev);
 
-	REG_WRITE(map->cntr, dspcntr);
-	gma_wait_for_vblank(dev);
+		REG_WRITE_WITH_AUX(map->cntr, dspcntr, i);
+		gma_wait_for_vblank(dev);
+	}
 
 oaktrail_crtc_mode_set_exit:
 	gma_power_end(dev);
@@ -565,3 +664,9 @@ const struct drm_crtc_helper_funcs oaktrail_helper_funcs = {
 	.commit = gma_crtc_commit,
 };
 
+/* Not used yet */
+const struct gma_clock_funcs mrst_clock_funcs = {
+	.clock = mrst_lvds_clock,
+	.limit = mrst_limit,
+	.pll_is_valid = gma_pll_is_valid,
+};
diff --git a/drivers/gpu/drm/gma500/oaktrail_device.c b/drivers/gpu/drm/gma500/oaktrail_device.c
index 08747fd7105c..27d3875d895b 100644
--- a/drivers/gpu/drm/gma500/oaktrail_device.c
+++ b/drivers/gpu/drm/gma500/oaktrail_device.c
@@ -40,6 +40,9 @@ static int oaktrail_output_init(struct drm_device *dev)
 		dev_err(dev->dev, "DSI is not supported\n");
 	if (dev_priv->hdmi_priv)
 		oaktrail_hdmi_init(dev, &dev_priv->mode_dev);
+
+	psb_intel_sdvo_init(dev, SDVOB);
+
 	return 0;
 }
 
@@ -526,6 +529,7 @@ static int oaktrail_chip_setup(struct drm_device *dev)
 		psb_intel_opregion_init(dev);
 		psb_intel_init_bios(dev);
 	}
+	gma_intel_setup_gmbus(dev);
 	oaktrail_hdmi_setup(dev);
 	return 0;
 }
@@ -534,6 +538,7 @@ static void oaktrail_teardown(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 
+	gma_intel_teardown_gmbus(dev);
 	oaktrail_hdmi_teardown(dev);
 	if (!dev_priv->has_gct)
 		psb_intel_destroy_bios(dev);
@@ -546,6 +551,7 @@ const struct psb_ops oaktrail_chip_ops = {
 	.crtcs = 2,
 	.hdm