1 files changed, 62 insertions, 14 deletions

diff --git a/drivers/video/pxafb.c b/drivers/video/pxafb.c
index 4e8f68b7c5b3..0031a5fefa26 100644
--- a/drivers/video/pxafb.c
+++ b/drivers/video/pxafb.c
@@ -1065,13 +1065,73 @@ static int __init pxafb_map_video_memory(struct pxafb_info *fbi)
 	return fbi->map_cpu ? 0 : -ENOMEM;
 }
 
+static void pxafb_decode_mode_info(struct pxafb_info *fbi,
+				   struct pxafb_mode_info *modes,
+				   unsigned int num_modes)
+{
+	unsigned int i, smemlen;
+
+	pxafb_setmode(&fbi->fb.var, &modes[0]);
+
+	for (i = 0; i < num_modes; i++) {
+		smemlen = modes[i].xres * modes[i].yres * modes[i].bpp / 8;
+		if (smemlen > fbi->fb.fix.smem_len)
+			fbi->fb.fix.smem_len = smemlen;
+	}
+}
+
+static int pxafb_decode_mach_info(struct pxafb_info *fbi,
+				  struct pxafb_mach_info *inf)
+{
+	unsigned int lcd_conn = inf->lcd_conn;
+
+	fbi->cmap_inverse	= inf->cmap_inverse;
+	fbi->cmap_static	= inf->cmap_static;
+
+	switch (lcd_conn & 0xf) {
+	case LCD_TYPE_MONO_STN:
+		fbi->lccr0 = LCCR0_CMS;
+		break;
+	case LCD_TYPE_MONO_DSTN:
+		fbi->lccr0 = LCCR0_CMS | LCCR0_SDS;
+		break;
+	case LCD_TYPE_COLOR_STN:
+		fbi->lccr0 = 0;
+		break;
+	case LCD_TYPE_COLOR_DSTN:
+		fbi->lccr0 = LCCR0_SDS;
+		break;
+	case LCD_TYPE_COLOR_TFT:
+		fbi->lccr0 = LCCR0_PAS;
+		break;
+	case LCD_TYPE_SMART_PANEL:
+		fbi->lccr0 = LCCR0_LCDT | LCCR0_PAS;
+		break;
+	default:
+		/* fall back to backward compatibility way */
+		fbi->lccr0 = inf->lccr0;
+		fbi->lccr3 = inf->lccr3;
+		fbi->lccr4 = inf->lccr4;
+		return -EINVAL;
+	}
+
+	if (lcd_conn == LCD_MONO_STN_8BPP)
+		fbi->lccr0 |= LCCR0_DPD;
+
+	fbi->lccr3 = LCCR3_Acb((inf->lcd_conn >> 10) & 0xff);
+	fbi->lccr3 |= (lcd_conn & LCD_BIAS_ACTIVE_LOW) ? LCCR3_OEP : 0;
+	fbi->lccr3 |= (lcd_conn & LCD_PCLK_EDGE_FALL)  ? LCCR3_PCP : 0;
+
+	pxafb_decode_mode_info(fbi, inf->modes, inf->num_modes);
+	return 0;
+}
+
 static struct pxafb_info * __init pxafb_init_fbinfo(struct device *dev)
 {
 	struct pxafb_info *fbi;
 	void *addr;
 	struct pxafb_mach_info *inf = dev->platform_data;
 	struct pxafb_mode_info *mode = inf->modes;
-	int i, smemlen;
 
 	/* Alloc the pxafb_info and pseudo_palette in one step */
 	fbi = kmalloc(sizeof(struct pxafb_info) + sizeof(u32) * 16, GFP_KERNEL);
@@ -1111,22 +1171,10 @@ static struct pxafb_info * __init pxafb_init_fbinfo(struct device *dev)
 	addr = addr + sizeof(struct pxafb_info);
 	fbi->fb.pseudo_palette	= addr;
 
-	pxafb_setmode(&fbi->fb.var, mode);
-
-	fbi->cmap_inverse	= inf->cmap_inverse;
-	fbi->cmap_static	= inf->cmap_static;
-
-	fbi->lccr0		= inf->lccr0;
-	fbi->lccr3		= inf->lccr3;
-	fbi->lccr4		= inf->lccr4;
 	fbi->state		= C_STARTUP;
 	fbi->task_state		= (u_char)-1;
 
-	for (i = 0; i < inf->num_modes; i++) {
-		smemlen = mode[i].xres * mode[i].yres * mode[i].bpp / 8;
-		if (smemlen > fbi->fb.fix.smem_len)
-			fbi->fb.fix.smem_len = smemlen;
-	}
+	pxafb_decode_mach_info(fbi, inf);
 
 	init_waitqueue_head(&fbi->ctrlr_wait);
 	INIT_WORK(&fbi->task, pxafb_task);