ARM: integrator: support CLCD RGB5551 layout for 16bpp

When the CLCD is in anything but RGB888 mode, its outputs are configured
for RGB5551 output.  Integrator/CP supports RGB565 via an external
multiplexer, which we configure for RGB5551 or RGB565 based only on the
bits per pixel.  So when userspace asks for the RGB555 layout, the mux
remains in RGB565 mode, and we produce incorrect colours.  Fix this.

Note that Integrator doesn't support BGR565 mode, but does support
BGR5551.

Acked-by: Catalin Marinas <catalin.marinas@arm.com>
Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>

1 files changed, 8 insertions, 6 deletions

diff --git a/arch/arm/mach-integrator/integrator_cp.c b/arch/arm/mach-integrator/integrator_cp.c
index e9327da1382..860090247b3 100644
--- a/arch/arm/mach-integrator/integrator_cp.c
+++ b/arch/arm/mach-integrator/integrator_cp.c
@@ -478,14 +478,16 @@ static struct clcd_panel vga = {
  */
 static void cp_clcd_enable(struct clcd_fb *fb)
 {
-        u32 val;
+        struct fb_var_screeninfo *var = &fb->fb.var;
+        u32 val = CM_CTRL_STATIC1 | CM_CTRL_STATIC2;
 
-        if (fb->fb.var.bits_per_pixel <= 8)
-                val = CM_CTRL_LCDMUXSEL_VGA_8421BPP;
+        if (var->bits_per_pixel <= 8 ||
+            (var->bits_per_pixel == 16 && var->green.length == 5))
+                /* Pseudocolor, RGB555, BGR555 */
+                val |= CM_CTRL_LCDMUXSEL_VGA555_TFT555;
         else if (fb->fb.var.bits_per_pixel <= 16)
-                val = CM_CTRL_LCDMUXSEL_VGA_16BPP
-                        | CM_CTRL_LCDEN0 | CM_CTRL_LCDEN1
-                        | CM_CTRL_STATIC1 | CM_CTRL_STATIC2;
+                /* truecolor RGB565 */
+                val |= CM_CTRL_LCDMUXSEL_VGA565_TFT555;
         else
                 val = 0; /* no idea for this, don't trust the docs */