video: fix bug in cfb_console code

FILL_15BIT_555RGB macro extension for pixel swapping
by commit bed53753dd1d7e6bcbea4339be0fb7760214cc35
introduced a bug in cfb_console:

Bitmaps with odd-numbered width won't be rendered
correctly and even U-Boot crashes are observed on
some platforms while repeated rendering of such
bitmaps with "bmp display". Also if a bitmap is
rendered to an odd-numbered x starting position,
the same problem occurs. This patch is an attempt
to fix it.

Signed-off-by: Anatolij Gustschin <agust@denx.de>

diff --git a/drivers/video/cfb_console.c b/drivers/video/cfb_console.c
index 97a37ba50c7514f23206e71e2deadc82fba3ed99..d313e9098c932569ae07100f252a20e59bd87bfc 100644 (file)
--- a/drivers/video/cfb_console.c
+++ b/drivers/video/cfb_console.c
@@ -751,24 +751,10 @@ void video_puts (const char *s)
        fb ++;                                          \
 }
 
-#if !defined(VIDEO_FB_16BPP_PIXEL_SWAP)
 #define FILL_15BIT_555RGB(r,g,b) {                     \
        *(unsigned short *)fb = SWAP16((unsigned short)(((r>>3)<<10) | ((g>>3)<<5) | (b>>3))); \
        fb += 2;                                        \
 }
-#else
-static int tgl;
-static unsigned short p0;
-#define FILL_15BIT_555RGB(r,g,b) {                     \
-       if (!tgl++) {                                   \
-               p0 = SWAP16((unsigned short)(((r>>3)<<10) | ((g>>3)<<5) | (b>>3))); \
-       } else {                                        \
-               tgl=0;                                  \
-               *(unsigned long *)(fb-2) = (SWAP16((unsigned short)(((r>>3)<<10) | ((g>>3)<<5) | (b>>3)))<<16) | p0; \
-       }                                               \
-       fb += 2;                                        \
-}
-#endif
 
 #define FILL_16BIT_565RGB(r,g,b) {                     \
        *(unsigned short *)fb = SWAP16((unsigned short)((((r)>>3)<<11) | (((g)>>2)<<5) | ((b)>>3))); \
@@ -796,6 +782,20 @@ static unsigned short p0;
 }
 #endif
 
+#if defined(VIDEO_FB_16BPP_PIXEL_SWAP)
+static void inline fill_555rgb_pswap(uchar *fb, int x,
+                                    u8 r, u8 g, u8 b)
+{
+       ushort *dst = (ushort *)fb;
+       ushort color = (ushort)(((r >> 3) << 10) |
+                               ((g >> 3) << 5) |
+                               (b >> 3));
+       if (x & 1)
+               *(--dst) = color;
+       else
+               *(++dst) = color;
+}
+#endif
 
 /*
  * Display the BMP file located at address bmp_image.
@@ -927,11 +927,20 @@ int video_display_bitmap (ulong bmp_image, int x, int y)
                        break;
                case GDF_15BIT_555RGB:
                        while (ycount--) {
+#if defined(VIDEO_FB_16BPP_PIXEL_SWAP)
+                               int xpos = x;
+#endif
                                WATCHDOG_RESET ();
                                xcount = width;
                                while (xcount--) {
                                        cte = bmp->color_table[*bmap++];
+#if !defined(VIDEO_FB_16BPP_PIXEL_SWAP)
                                        FILL_15BIT_555RGB (cte.red, cte.green, cte.blue);
+#else
+                                       fill_555rgb_pswap (fb, xpos++, cte.red,
+                                                          cte.green, cte.blue);
+                                       fb += 2;
+#endif
                                }
                                bmap += padded_line;
                                fb -= (VIDEO_VISIBLE_COLS + width) * VIDEO_PIXEL_SIZE;
@@ -993,10 +1002,19 @@ int video_display_bitmap (ulong bmp_image, int x, int y)
                        break;
                case GDF_15BIT_555RGB:
                        while (ycount--) {
+#if defined(VIDEO_FB_16BPP_PIXEL_SWAP)
+                               int xpos = x;
+#endif
                                WATCHDOG_RESET ();
                                xcount = width;
                                while (xcount--) {
+#if !defined(VIDEO_FB_16BPP_PIXEL_SWAP)
                                        FILL_15BIT_555RGB (bmap[2], bmap[1], bmap[0]);
+#else
+                                       fill_555rgb_pswap (fb, xpos++, bmap[2],
+                                                          bmap[1], bmap[0]);
+                                       fb += 2;
+#endif
                                        bmap += 3;
                                }
                                bmap += padded_line;
@@ -1103,6 +1121,9 @@ void logo_plot (void *screen, int width, int x, int y)
        }
 
        while (ycount--) {
+#if defined(VIDEO_FB_16BPP_PIXEL_SWAP)
+               int xpos = x;
+#endif
                xcount = VIDEO_LOGO_WIDTH;
                while (xcount--) {
                        r = logo_red[*source - VIDEO_LOGO_LUT_OFFSET];
@@ -1121,15 +1142,7 @@ void logo_plot (void *screen, int width, int x, int y)
                                *(unsigned short *) dest =
                                        SWAP16 ((unsigned short) (((r >> 3) << 10) | ((g >> 3) << 5) | (b >> 3)));
 #else
-                               {
-                                       if (!tgl++) {
-                                               p0 = SWAP16 ((unsigned short) (((r >> 3) << 10) | ((g >> 3) << 5) | (b >> 3)));
-                                       } else {
-                                               *(unsigned long *)(dest-2) =
-                                                       (SWAP16 ((unsigned short) (((r >> 3) << 10) | ((g >> 3) << 5) | (b >> 3)))<<16) | p0;
-                                               tgl=0;
-                                       }
-                               }
+                               fill_555rgb_pswap (dest, xpos++, r, g, b);
 #endif
                                break;
                        case GDF_16BIT_565RGB: