1 files changed, 30 insertions, 11 deletions

diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index 63409c122ae8..ced4419e56e2 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -100,8 +100,11 @@ static int atmel_bl_update_status(struct backlight_device *bl)
 		brightness = 0;
 
 	lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_VAL, brightness);
-	lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR,
+	if (contrast_ctr & ATMEL_LCDC_POL_POSITIVE)
+		lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR,
 			brightness ? contrast_ctr : 0);
+	else
+		lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, contrast_ctr);
 
 	bl->props.fb_blank = bl->props.power = sinfo->bl_power = power;
 
@@ -397,7 +400,7 @@ static int atmel_lcdfb_check_var(struct fb_var_screeninfo *var,
 	var->lower_margin = min_t(u32, var->lower_margin,
 			ATMEL_LCDC_VFP);
 	var->right_margin = min_t(u32, var->right_margin,
-			(ATMEL_LCDC_HFP >> ATMEL_LCDC_HFP_OFFSET) + 1);
+			(ATMEL_LCDC_HFP >> ATMEL_LCDC_HFP_OFFSET) + 2);
 	var->hsync_len = min_t(u32, var->hsync_len,
 			(ATMEL_LCDC_HPW >> ATMEL_LCDC_HPW_OFFSET) + 1);
 	var->left_margin = min_t(u32, var->left_margin,
@@ -582,7 +585,7 @@ static int atmel_lcdfb_set_par(struct fb_info *info)
 	lcdc_writel(sinfo, ATMEL_LCDC_TIM1, value);
 
 	/* Horizontal timing */
-	value = (info->var.right_margin - 1) << ATMEL_LCDC_HFP_OFFSET;
+	value = (info->var.right_margin - 2) << ATMEL_LCDC_HFP_OFFSET;
 	value |= (info->var.hsync_len - 1) << ATMEL_LCDC_HPW_OFFSET;
 	value |= (info->var.left_margin - 1);
 	dev_dbg(info->device, "  * LCDTIM2 = %08lx\n", value);
@@ -682,14 +685,30 @@ static int atmel_lcdfb_setcolreg(unsigned int regno, unsigned int red,
 
 	case FB_VISUAL_PSEUDOCOLOR:
 		if (regno < 256) {
-			val  = ((red   >> 11) & 0x001f);
-			val |= ((green >>  6) & 0x03e0);
-			val |= ((blue  >>  1) & 0x7c00);
-
-			/*
-			 * TODO: intensity bit. Maybe something like
-			 *   ~(red[10] ^ green[10] ^ blue[10]) & 1
-			 */
+			if (cpu_is_at91sam9261() || cpu_is_at91sam9263()
+			    || cpu_is_at91sam9rl()) {
+				/* old style I+BGR:555 */
+				val  = ((red   >> 11) & 0x001f);
+				val |= ((green >>  6) & 0x03e0);
+				val |= ((blue  >>  1) & 0x7c00);
+
+				/*
+				 * TODO: intensity bit. Maybe something like
+				 *   ~(red[10] ^ green[10] ^ blue[10]) & 1
+				 */
+			} else {
+				/* new style BGR:565 / RGB:565 */
+				if (sinfo->lcd_wiring_mode ==
+				    ATMEL_LCDC_WIRING_RGB) {
+					val  = ((blue >> 11) & 0x001f);
+					val |= ((red  >>  0) & 0xf800);
+				} else {
+					val  = ((red  >> 11) & 0x001f);
+					val |= ((blue >>  0) & 0xf800);
+				}
+
+				val |= ((green >>  5) & 0x07e0);
+			}
 
 			lcdc_writel(sinfo, ATMEL_LCDC_LUT(regno), val);
 			ret = 0;