[U-Boot] [PATCH] video: bmp: Add support for 24bpp BMP files on 16bpp displays

[Stefan Roese <sr@denx.de>]

This patch adds support to load 24bpp BMP files on 16bpp displays. This
will be used by the theadorable board. The "old" bmp command did support
this operartion mode and to not break compatibility with the move to
DM_VIDEO, we need to add this support to the "new" bmp code.

Signed-off-by: Stefan Roese <sr@denx.de>
Cc: Anatolij Gustschin <agust@denx.de>
---
 drivers/video/video_bmp.c | 23 +++++++++++++++++------
 1 file changed, 17 insertions(+), 6 deletions(-)

diff --git a/drivers/video/video_bmp.c b/drivers/video/video_bmp.c
index 2898b0b55d..193f37d275 100644
--- a/drivers/video/video_bmp.c
+++ b/drivers/video/video_bmp.c
@@ -229,11 +229,12 @@ int video_bmp_display(struct udevice *dev, ulong bmp_image, int x, int y,
 	}
 
 	/*
-	 * We support displaying 8bpp BMPs on 16bpp LCDs
+	 * We support displaying 8bpp and 24bpp BMPs on 16bpp LCDs
 	 * and displaying 24bpp BMPs on 32bpp LCDs
-	 * */
+	 */
 	if (bpix != bmp_bpix &&
 	    !(bmp_bpix == 8 && bpix == 16) &&
+	    !(bmp_bpix == 24 && bpix == 16) &&
 	    !(bmp_bpix == 24 && bpix == 32)) {
 		printf("Error: %d bit/pixel mode, but BMP has %d bit/pixel\n",
 		       bpix, get_unaligned_le16(&bmp->header.bit_count));
@@ -318,12 +319,22 @@ int video_bmp_display(struct udevice *dev, ulong bmp_image, int x, int y,
 	case 24:
 		for (i = 0; i < height; ++i) {
 			for (j = 0; j < width; j++) {
-				*(fb++) = *(bmap++);
-				*(fb++) = *(bmap++);
-				*(fb++) = *(bmap++);
-				*(fb++) = 0;
+				if (bpix == 16) {
+					/* 16bit 555RGB format */
+					*(u16 *)fb = ((bmap[2] >> 3) << 10) |
+						((bmap[1] >> 3) << 5) |
+						(bmap[0] >> 3);
+					bmap += 3;
+					fb += 2;
+				} else {
+					*(fb++) = *(bmap++);
+					*(fb++) = *(bmap++);
+					*(fb++) = *(bmap++);
+					*(fb++) = 0;
+				}
 			}
 			fb -= priv->line_length + width * (bpix / 8);
+			bmap += (padded_width - width) * 3;
 		}
 		break;
 #endif /* CONFIG_BMP_24BPP */
-- 
2.20.1