1 files changed, 51 insertions, 0 deletions
diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index 3cf008ce6b..5add1369ef 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -11,6 +11,7 @@
 #include <asm/arch/gpio.h>
 #include <asm/arch/clk.h>
 #include <lcd.h>
+#include <bmp_layout.h>
 #include <atmel_lcdc.h>
 
 /* configurable parameters */
@@ -29,6 +30,46 @@
 #define lcdc_readl(mmio, reg)		__raw_readl((mmio)+(reg))
 #define lcdc_writel(mmio, reg, val)	__raw_writel((val), (mmio)+(reg))
 
+ushort *configuration_get_cmap(void)
+{
+	return (ushort *)(panel_info.mmio + ATMEL_LCDC_LUT(0));
+}
+
+#if defined(CONFIG_BMP_16BPP) && defined(CONFIG_ATMEL_LCD_BGR555)
+void fb_put_word(uchar **fb, uchar **from)
+{
+	*(*fb)++ = (((*from)[0] & 0x1f) << 2) | ((*from)[1] & 0x03);
+	*(*fb)++ = ((*from)[0] & 0xe0) | (((*from)[1] & 0x7c) >> 2);
+	*from += 2;
+}
+#endif
+
+#ifdef CONFIG_LCD_LOGO
+#include <bmp_logo.h>
+void lcd_logo_set_cmap(void)
+{
+	int i;
+	uint lut_entry;
+	ushort colreg;
+	uint *cmap = (uint *)configuration_get_cmap();
+
+	for (i = 0; i < BMP_LOGO_COLORS; ++i) {
+		colreg = bmp_logo_palette[i];
+#ifdef CONFIG_ATMEL_LCD_BGR555
+		lut_entry = ((colreg & 0x000F) << 11) |
+				((colreg & 0x00F0) <<  2) |
+				((colreg & 0x0F00) >>  7);
+#else
+		lut_entry = ((colreg & 0x000F) << 1) |
+				((colreg & 0x00F0) << 3) |
+				((colreg & 0x0F00) << 4);
+#endif
+		*(cmap + BMP_LOGO_OFFSET) = lut_entry;
+		cmap++;
+	}
+}
+#endif
+
 void lcd_setcolreg(ushort regno, ushort red, ushort green, ushort blue)
 {
 #if defined(CONFIG_ATMEL_LCD_BGR555)
@@ -40,6 +81,16 @@ void lcd_setcolreg(ushort regno, ushort red, ushort green, ushort blue)
 #endif
 }
 
+void lcd_set_cmap(bmp_image_t *bmp, unsigned colors)
+{
+	int i;
+
+	for (i = 0; i < colors; ++i) {
+		bmp_color_table_entry_t cte = bmp->color_table[i];
+		lcd_setcolreg(i, cte.red, cte.green, cte.blue);
+	}
+}
+
 void lcd_ctrl_init(void *lcdbase)
 {
 	unsigned long value;