diff --git a/common/lcd.c b/common/lcd.c
index efd9a87..7fcac1d 100644
--- a/common/lcd.c
+++ b/common/lcd.c
@@ -385,13 +385,13 @@ int lcd_getbgcolor(void)
 /************************************************************************/
 
 #ifdef CONFIG_LCD_LOGO
+__weak void lcd_logo_set_cmap(void)
+{
+}
+
 void bitmap_plot(int x, int y)
 {
-#ifdef CONFIG_ATMEL_LCD
-	uint *cmap = (uint *)bmp_logo_palette;
-#else
 	ushort *cmap = (ushort *)bmp_logo_palette;
-#endif
 	ushort i, j;
 	uchar *bmap;
 	uchar *fb;
@@ -417,8 +417,6 @@ void bitmap_plot(int x, int y)
 		 */
 #if defined(CONFIG_MPC823)
 		cmap = (ushort *) &(cp->lcd_cmap[BMP_LOGO_OFFSET * sizeof(ushort)]);
-#elif defined(CONFIG_ATMEL_LCD)
-		cmap = (uint *)configuration_get_cmap();
 #else
 		cmap = configuration_get_cmap();
 #endif
@@ -426,25 +424,14 @@ void bitmap_plot(int x, int y)
 		WATCHDOG_RESET();
 
 		/* Set color map */
+#ifdef CONFIG_ATMEL_LCD
+		lcd_logo_set_cmap();
+#else
 		for (i = 0; i < ARRAY_SIZE(bmp_logo_palette); ++i) {
 			ushort colreg = bmp_logo_palette[i];
-#ifdef CONFIG_ATMEL_LCD
-			uint lut_entry;
-#ifdef CONFIG_ATMEL_LCD_BGR555
-			lut_entry = ((colreg & 0x000F) << 11) |
-					((colreg & 0x00F0) <<  2) |
-					((colreg & 0x0F00) >>  7);
-#else /* CONFIG_ATMEL_LCD_RGB565 */
-			lut_entry = ((colreg & 0x000F) << 1) |
-					((colreg & 0x00F0) << 3) |
-					((colreg & 0x0F00) << 4);
-#endif
-			*(cmap + BMP_LOGO_OFFSET) = lut_entry;
-			cmap++;
-#else /* !CONFIG_ATMEL_LCD */
 			*cmap++ = colreg;
-#endif /* CONFIG_ATMEL_LCD */
 		}
+#endif
 
 		WATCHDOG_RESET();
 
diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index c7991cd..2a71eba 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -43,6 +43,32 @@ void fb_put_word(uchar **fb, uchar **from)
 }
 #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)