lcd: Support colour lookup table on 16bpp display in BMP images

For 16-bit-per-pixel displays it is useful to support 8 bit-per-pixel
images to reduce image size. Add support for this when drawing BMP images.

Signed-off-by: Simon Glass <sjg@chromium.org>
diff --git a/common/lcd.c b/common/lcd.c
index c7f2830..5a52fe4 100644
--- a/common/lcd.c
+++ b/common/lcd.c
@@ -578,6 +578,8 @@
 	unsigned long width, height, byte_width;
 	unsigned long pwidth = panel_info.vl_col;
 	unsigned colors, bpix, bmp_bpix;
+	int hdr_size;
+	struct bmp_color_table_entry *palette = bmp->color_table;
 
 	if (!bmp || !(bmp->header.signature[0] == 'B' &&
 		bmp->header.signature[1] == 'M')) {
@@ -589,6 +591,8 @@
 	width = get_unaligned_le32(&bmp->header.width);
 	height = get_unaligned_le32(&bmp->header.height);
 	bmp_bpix = get_unaligned_le16(&bmp->header.bit_count);
+	hdr_size = get_unaligned_le16(&bmp->header.size);
+	debug("hdr_size=%d, bmp_bpix=%d\n", hdr_size, bmp_bpix);
 
 	colors = 1 << bmp_bpix;
 
@@ -613,8 +617,8 @@
 		return 1;
 	}
 
-	debug("Display-bmp: %d x %d  with %d colors\n",
-		(int)width, (int)height, (int)colors);
+	debug("Display-bmp: %d x %d  with %d colors, display %d\n",
+	      (int)width, (int)height, (int)colors, 1 << bpix);
 
 	if (bmp_bpix == 8)
 		lcd_set_cmap(bmp, colors);
@@ -641,6 +645,7 @@
 		cmap_base = configuration_get_cmap();
 #ifdef CONFIG_LCD_BMP_RLE8
 		u32 compression = get_unaligned_le32(&bmp->header.compression);
+		debug("compressed %d %d\n", compression, BMP_BI_RLE8);
 		if (compression == BMP_BI_RLE8) {
 			if (bpix != 16) {
 				/* TODO implement render code for bpix != 16 */
@@ -663,7 +668,19 @@
 				if (bpix != 16) {
 					fb_put_byte(&fb, &bmap);
 				} else {
-					*(uint16_t *)fb = cmap_base[*(bmap++)];
+					struct bmp_color_table_entry *entry;
+					uint val;
+
+					if (cmap_base) {
+						val = cmap_base[*bmap];
+					} else {
+						entry = &palette[*bmap];
+						val = entry->blue >> 3 |
+							entry->green >> 2 << 5 |
+							entry->red >> 3 << 11;
+					}
+					*(uint16_t *)fb = val;
+					bmap++;
 					fb += sizeof(uint16_t) / sizeof(*fb);
 				}
 			}