atmel_lcdfb: support new-style palette format

[sfrench/cifs-2.6.git] / drivers / video / atmel_lcdfb.c

diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index 8bfee6aecc8bc9308a9d7dd3d2444183131043cf..2226aca44c5744a5f658efd448d6ddbcef642ca2 100644 (file)
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -684,14 +684,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;