1 files changed, 374 insertions, 141 deletions

diff --git a/apps/plugins/jpeg.c b/apps/plugins/jpeg.c
index 408f5d1ea1..28c7aebbe8 100644
--- a/apps/plugins/jpeg.c
+++ b/apps/plugins/jpeg.c
@@ -1389,26 +1389,30 @@ INLINE int huff_decode_ac(struct bitstream* bs, struct derived_tbl* tbl)
 }
 
 
-/* a JPEG decoder specialized in decoding only the luminance (b&w) */
-int jpeg_decode(struct jpeg* p_jpeg, unsigned char* p_pixel, int downscale,
-                void (*pf_progress)(int current, int total))
+#ifdef HAVE_LCD_COLOR
+
+/* JPEG decoder variant for YUV decoding, into 3 different planes */
+/*  Note: it keeps the original color subsampling, even if resized. */
+int jpeg_decode(struct jpeg* p_jpeg, unsigned char* p_pixel[3],
+                int downscale, void (*pf_progress)(int current, int total))
 {
     struct bitstream bs; /* bitstream "object" */
     static int block[64]; /* decoded DCT coefficients */
 
     int width, height;
-    int skip_line; /* bytes from one line to the next (skip_line) */
-    int skip_strip, skip_mcu; /* bytes to next DCT row / column */
+    int skip_line[3]; /* bytes from one line to the next (skip_line) */
+    int skip_strip[3], skip_mcu[3]; /* bytes to next DCT row / column */
 
-    int x, y; /* loop counter */
+    int i, x, y; /* loop counter */
 
-    unsigned char* p_byte; /* bitmap pointer */
+    unsigned char* p_line[3] = {p_pixel[0], p_pixel[1], p_pixel[2]};
+    unsigned char* p_byte[3]; /* bitmap pointer */
 
     void (*pf_idct)(unsigned char*, int*, int*, int); /* selected IDCT */
     int k_need; /* AC coefficients needed up to here */
     int zero_need; /* init the block with this many zeros */
 
-    int last_dc_val = 0;
+    int last_dc_val[3] = {0, 0, 0}; // or 128 for chroma?
     int store_offs[4]; /* memory offsets: order of Y11 Y12 Y21 Y22 U V */
     int restart = p_jpeg->restart_interval; /* MCUs until restart marker */
 
@@ -1446,9 +1450,13 @@ int jpeg_decode(struct jpeg* p_jpeg, unsigned char* p_pixel, int downscale,
 
     width  = p_jpeg->x_phys / downscale;
     height = p_jpeg->y_phys / downscale;
-    skip_line = width;
-    skip_strip = skip_line * (height / p_jpeg->y_mbl);
-    skip_mcu = (width/p_jpeg->x_mbl);
+    for (i=0; i<3; i++) /* calculate some strides */
+    {
+        skip_line[i] = width / p_jpeg->subsample_x[i];
+        skip_strip[i] = skip_line[i]
+                        * (height / p_jpeg->y_mbl) / p_jpeg->subsample_y[i];
+        skip_mcu[i] = width/p_jpeg->x_mbl / p_jpeg->subsample_x[i];
+    }
 
     /* prepare offsets about where to store the different blocks */
     store_offs[p_jpeg->store_pos[0]] = 0;
@@ -1458,8 +1466,11 @@ int jpeg_decode(struct jpeg* p_jpeg, unsigned char* p_pixel, int downscale,
 
     for(y=0; y<p_jpeg->y_mbl && bs.next_input_byte <= bs.input_end; y++)
     {
-        p_byte = p_pixel;
-        p_pixel += skip_strip;
+        for (i=0; i<3; i++) // scan line init
+        {
+            p_byte[i] = p_line[i];
+            p_line[i] += skip_strip[i];
+        }
         for (x=0; x<p_jpeg->x_mbl; x++)
         {
             int blkn;
@@ -1477,39 +1488,36 @@ int jpeg_decode(struct jpeg* p_jpeg, unsigned char* p_pixel, int downscale,
                 /* Section F.2.2.1: decode the DC coefficient difference */
                 s = huff_decode_dc(&bs, dctbl);
 
-                if (ci == 0) /* only for Y component */
-                {
-                    last_dc_val += s;
-                    block[0] = last_dc_val; /* output it (assumes zag[0] = 0) */
+                last_dc_val[ci] += s;
+                block[0] = last_dc_val[ci]; /* output it (assumes zag[0] = 0) */
 
-                    /* coefficient buffer must be cleared */
-                    MEMSET(block+1, 0, zero_need*sizeof(block[0]));
+                /* coefficient buffer must be cleared */
+                MEMSET(block+1, 0, zero_need*sizeof(block[0]));
 
-                    /* Section F.2.2.2: decode the AC coefficients */
-                    for (; k < k_need; k++)
-                    {
-                        s = huff_decode_ac(&bs, actbl);
-                        r = s >> 4;
-                        s &= 15;
+                /* Section F.2.2.2: decode the AC coefficients */
+                for (; k < k_need; k++)
+                {
+                    s = huff_decode_ac(&bs, actbl);
+                    r = s >> 4;
+                    s &= 15;
 
-                        if (s)
-                        {
-                            k += r;
-                            check_bit_buffer(&bs, s);
-                            r = get_bits(&bs, s);
-                            block[zag[k]] = HUFF_EXTEND(r, s);
-                        }
-                        else
+                    if (s)
+                    {
+                        k += r;
+                        check_bit_buffer(&bs, s);
+                        r = get_bits(&bs, s);
+                        block[zag[k]] = HUFF_EXTEND(r, s);
+                    }
+                    else
+                    {
+                        if (r != 15)
                         {
-                            if (r != 15)
-                            {
-                                k = 64;
-                                break;
-                            }
-                            k += r;
+                            k = 64;
+                            break;
                         }
-                    }  /* for k */
-                }
+                        k += r;
+                    }
+                }  /* for k */
                 /* In this path we just discard the values */
                 for (; k < 64; k++)
                 {
@@ -1532,17 +1540,25 @@ int jpeg_decode(struct jpeg* p_jpeg, unsigned char* p_pixel, int downscale,
                 }  /* for k */
 
                 if (ci == 0)
-                {   /* only for Y component */
-                    pf_idct(p_byte+store_offs[blkn], block, p_jpeg->qt_idct[ti], 
-                        skip_line);
+                {   /* Y component needs to bother about block store */
+                    pf_idct(p_byte[0]+store_offs[blkn], block,
+                        p_jpeg->qt_idct[ti], skip_line[0]);
+                }
+                else
+                {   /* chroma */
+                    pf_idct(p_byte[ci], block, p_jpeg->qt_idct[ti],
+                        skip_line[ci]);
                 }
             } /* for blkn */
-            p_byte += skip_mcu;
+            p_byte[0] += skip_mcu[0]; // unrolled for (i=0; i<3; i++) loop
+            p_byte[1] += skip_mcu[1];
+            p_byte[2] += skip_mcu[2];
             if (p_jpeg->restart_interval && --restart == 0) 
             {   /* if a restart marker is due: */
                 restart = p_jpeg->restart_interval; /* count again */
                 search_restart(&bs); /* align the bitstream */
-                last_dc_val = 0; /* reset decoder */
+                last_dc_val[0] = last_dc_val[1] = 
+                                 last_dc_val[2] = 0; /* reset decoder */
             }
         } /* for x */
         if (pf_progress != NULL)
@@ -1551,32 +1567,29 @@ int jpeg_decode(struct jpeg* p_jpeg, unsigned char* p_pixel, int downscale,
 
     return 0; /* success */
 }
+#else /* !HAVE_LCD_COLOR */
 
-
-#ifdef HAVE_LCD_COLOR
-
-/* JPEG decoder variant for YUV decoding, into 3 different planes */
-/*  Note: it keeps the original color subsampling, even if resized. */
-int jpeg_decode_color(struct jpeg* p_jpeg, unsigned char* p_pixel[3], 
-        int downscale, void (*pf_progress)(int current, int total))
+/* a JPEG decoder specialized in decoding only the luminance (b&w) */
+int jpeg_decode(struct jpeg* p_jpeg, unsigned char* p_pixel[1], int downscale,
+                void (*pf_progress)(int current, int total))
 {
     struct bitstream bs; /* bitstream "object" */
     static int block[64]; /* decoded DCT coefficients */
 
     int width, height;
-    int skip_line[3]; /* bytes from one line to the next (skip_line) */
-    int skip_strip[3], skip_mcu[3]; /* bytes to next DCT row / column */
+    int skip_line; /* bytes from one line to the next (skip_line) */
+    int skip_strip, skip_mcu; /* bytes to next DCT row / column */
 
-    int i, x, y; /* loop counter */
+    int x, y; /* loop counter */
 
-    unsigned char* p_line[3] = {p_pixel[0], p_pixel[1], p_pixel[2]};
-    unsigned char* p_byte[3]; /* bitmap pointer */
+    unsigned char* p_line = p_pixel[0];
+    unsigned char* p_byte; /* bitmap pointer */
 
     void (*pf_idct)(unsigned char*, int*, int*, int); /* selected IDCT */
     int k_need; /* AC coefficients needed up to here */
     int zero_need; /* init the block with this many zeros */
 
-    int last_dc_val[3] = {0, 0, 0}; // or 128 for chroma?
+    int last_dc_val = 0;
     int store_offs[4]; /* memory offsets: order of Y11 Y12 Y21 Y22 U V */
     int restart = p_jpeg->restart_interval; /* MCUs until restart marker */
 
@@ -1614,13 +1627,9 @@ int jpeg_decode_color(struct jpeg* p_jpeg, unsigned char* p_pixel[3],
 
     width  = p_jpeg->x_phys / downscale;
     height = p_jpeg->y_phys / downscale;
-    for (i=0; i<3; i++) /* calculate some strides */
-    {
-        skip_line[i] = width / p_jpeg->subsample_x[i];
-        skip_strip[i] = skip_line[i]
-                        * (height / p_jpeg->y_mbl) / p_jpeg->subsample_y[i];
-        skip_mcu[i] = width/p_jpeg->x_mbl / p_jpeg->subsample_x[i];
-    }
+    skip_line = width;
+    skip_strip = skip_line * (height / p_jpeg->y_mbl);
+    skip_mcu = (width/p_jpeg->x_mbl);
 
     /* prepare offsets about where to store the different blocks */
     store_offs[p_jpeg->store_pos[0]] = 0;
@@ -1630,11 +1639,8 @@ int jpeg_decode_color(struct jpeg* p_jpeg, unsigned char* p_pixel[3],
 
     for(y=0; y<p_jpeg->y_mbl && bs.next_input_byte <= bs.input_end; y++)
     {
-        for (i=0; i<3; i++) // scan line init
-        {
-            p_byte[i] = p_line[i];
-            p_line[i] += skip_strip[i];
-        }
+        p_byte = p_line;
+        p_line += skip_strip;
         for (x=0; x<p_jpeg->x_mbl; x++)
         {
             int blkn;
@@ -1652,36 +1658,39 @@ int jpeg_decode_color(struct jpeg* p_jpeg, unsigned char* p_pixel[3],
                 /* Section F.2.2.1: decode the DC coefficient difference */
                 s = huff_decode_dc(&bs, dctbl);
 
-                last_dc_val[ci] += s;
-                block[0] = last_dc_val[ci]; /* output it (assumes zag[0] = 0) */
-
-                /* coefficient buffer must be cleared */
-                MEMSET(block+1, 0, zero_need*sizeof(block[0]));
-
-                /* Section F.2.2.2: decode the AC coefficients */
-                for (; k < k_need; k++)
+                if (ci == 0) /* only for Y component */
                 {
-                    s = huff_decode_ac(&bs, actbl);
-                    r = s >> 4;
-                    s &= 15;
+                    last_dc_val += s;
+                    block[0] = last_dc_val; /* output it (assumes zag[0] = 0) */
 
-                    if (s)
-                    {
-                        k += r;
-                        check_bit_buffer(&bs, s);
-                        r = get_bits(&bs, s);
-                        block[zag[k]] = HUFF_EXTEND(r, s);
-                    }
-                    else
+                    /* coefficient buffer must be cleared */
+                    MEMSET(block+1, 0, zero_need*sizeof(block[0]));
+
+                    /* Section F.2.2.2: decode the AC coefficients */
+                    for (; k < k_need; k++)
                     {
-                        if (r != 15)
+                        s = huff_decode_ac(&bs, actbl);
+                        r = s >> 4;
+                        s &= 15;
+
+                        if (s)
                         {
-                            k = 64;
-                            break;
+                            k += r;
+                            check_bit_buffer(&bs, s);
+                            r = get_bits(&bs, s);
+                            block[zag[k]] = HUFF_EXTEND(r, s);
                         }
-                        k += r;
-                    }
-                }  /* for k */
+                        else
+                        {
+                            if (r != 15)
+                            {
+                                k = 64;
+                                break;
+                            }
+                            k += r;
+                        }
+                    }  /* for k */
+                }
                 /* In this path we just discard the values */
                 for (; k < 64; k++)
                 {
@@ -1704,25 +1713,17 @@ int jpeg_decode_color(struct jpeg* p_jpeg, unsigned char* p_pixel[3],
                 }  /* for k */
 
                 if (ci == 0)
-                {   /* Y component needs to bother about block store */
-                    pf_idct(p_byte[0]+store_offs[blkn], block,
-                        p_jpeg->qt_idct[ti], skip_line[0]);
-                }
-                else
-                {   /* chroma */
-                    pf_idct(p_byte[ci], block, p_jpeg->qt_idct[ti], 
-                        skip_line[ci]);
+                {   /* only for Y component */
+                    pf_idct(p_byte+store_offs[blkn], block, p_jpeg->qt_idct[ti], 
+                        skip_line);
                 }
             } /* for blkn */
-            p_byte[0] += skip_mcu[0]; // unrolled for (i=0; i<3; i++) loop
-            p_byte[1] += skip_mcu[1];
-            p_byte[2] += skip_mcu[2];
+            p_byte += skip_mcu;
             if (p_jpeg->restart_interval && --restart == 0) 
             {   /* if a restart marker is due: */
                 restart = p_jpeg->restart_interval; /* count again */
                 search_restart(&bs); /* align the bitstream */
-                last_dc_val[0] = last_dc_val[1] = 
-                                 last_dc_val[2] = 0; /* reset decoder */
+                last_dc_val = 0; /* reset decoder */
             }
         } /* for x */
         if (pf_progress != NULL)
@@ -1731,8 +1732,7 @@ int jpeg_decode_color(struct jpeg* p_jpeg, unsigned char* p_pixel[3],
 
     return 0; /* success */
 }
-
-#endif /* #ifdef HAVE_LCD_COLOR */
+#endif /* !HAVE_LCD_COLOR */
 
 /**************** end JPEG code ********************/
 
@@ -1745,7 +1745,12 @@ int jpeg_decode_color(struct jpeg* p_jpeg, unsigned char* p_pixel[3],
 
 struct t_disp
 {
-    unsigned char* bitmap;
+#ifdef HAVE_LCD_COLOR
+    unsigned char* bitmap[3]; /* Y, Cr, Cb */
+    int csub_x, csub_y;
+#else
+    unsigned char* bitmap[1]; /* Y only */
+#endif
     int width;
     int height;
     int stride;
@@ -1767,6 +1772,151 @@ int root_size;
 
 /************************* Implementation ***************************/
 
+#ifdef HAVE_LCD_COLOR
+
+#if (LCD_DEPTH == 16) && \
+    ((LCD_PIXELFORMAT == RGB565) || (LCD_PIXELFORMAT == RGB565SWAPPED))
+#define RYFAC (31*257)
+#define GYFAC (63*257)
+#define BYFAC (31*257)
+#define RVFAC 11170     /* 31 * 257 *  1.402    */
+#define GVFAC (-11563)  /* 63 * 257 * -0.714136 */
+#define GUFAC (-5572)   /* 63 * 257 * -0.344136 */
+#define BUFAC 14118     /* 31 * 257 *  1.772    */
+#endif
+
+/* Draw a partial YUV colour bitmap */
+void yuv_bitmap_part(unsigned char *src[3], int csub_x, int csub_y,
+                     int src_x, int src_y, int stride,
+                     int x, int y, int width, int height)
+{
+    fb_data *dst, *dst_end;
+
+    /* nothing to draw? */
+    if ((width <= 0) || (height <= 0) || (x >= LCD_WIDTH) || (y >= LCD_HEIGHT)
+        || (x + width <= 0) || (y + height <= 0))
+        return;
+        
+    /* clipping */
+    if (x < 0)
+    {
+        width += x;
+        src_x -= x;
+        x = 0;
+    }
+    if (y < 0)
+    {
+        height += y;
+        src_y -= y;
+        y = 0;
+    }
+    if (x + width > LCD_WIDTH)
+        width = LCD_WIDTH - x;
+    if (y + height > LCD_HEIGHT)
+        height = LCD_HEIGHT - y;
+
+    dst = rb->lcd_framebuffer + LCD_WIDTH * y + x;
+    dst_end = dst + LCD_WIDTH * height;
+    
+    do
+    {
+        fb_data *dst_row = dst;
+        fb_data *row_end = dst_row + width;
+        const unsigned char *ysrc = src[0] + stride * src_y + src_x;
+        int y, u, v;
+        int red, green, blue;
+        unsigned rbits, gbits, bbits;
+
+        if (csub_y) /* colour */
+        {
+            /* upsampling, YUV->RGB conversion and reduction to RGB565 in one go */
+            const unsigned char *usrc = src[1] + (stride/csub_x) * (src_y/csub_y)
+                                               + (src_x/csub_x);
+            const unsigned char *vsrc = src[2] + (stride/csub_x) * (src_y/csub_y)
+                                               + (src_x/csub_x);
+            int xphase = src_x % csub_x;
+            int rc, gc, bc;
+
+            u = *usrc++ - 128;
+            v = *vsrc++ - 128;
+            rc = RVFAC * v + 32639;
+            gc = GVFAC * v + GUFAC * u + 32639;
+            bc = BUFAC * u + 32639;
+            
+            do
+            {
+                y = *ysrc++;
+                red   = RYFAC * y + rc;
+                green = GYFAC * y + gc;
+                blue  = BYFAC * y + bc;
+
+                if ((unsigned)red > (RYFAC*255+32639))
+                {
+                    if (red < 0)
+                        red = 0;
+                    else
+                        red = (RYFAC*255+32639);
+                }
+                if ((unsigned)green > (GYFAC*255+32639))
+                {
+                    if (green < 0)
+                        green = 0;
+                    else
+                        green = (GYFAC*255+32639);
+                }
+                if ((unsigned)blue > (BYFAC*255+32639))
+                {
+                    if (blue < 0)
+                        blue = 0;
+                    else
+                        blue = (BYFAC*255+32639);
+                }
+                rbits = ((unsigned)red) >> 16 ;
+                gbits = ((unsigned)green) >> 16 ;
+                bbits = ((unsigned)blue) >> 16 ;
+#if LCD_PIXELFORMAT == RGB565
+                *dst_row++ = (rbits << 11) | (gbits << 5) | bbits;
+#elif LCD_PIXELFORMAT == RGB565SWAPPED
+                *dst_row++ = swap16((rbits << 11) | (gbits << 5) | bbits);
+#endif
+
+                if (++xphase >= csub_x)
+                {
+                    u = *usrc++ - 128;
+                    v = *vsrc++ - 128;
+                    rc = RVFAC * v + 32639;
+                    gc = GVFAC * v + GUFAC * u + 32639;
+                    bc = BUFAC * u + 32639;
+                    xphase = 0;
+                }
+            }
+            while (dst_row < row_end);
+        }
+        else /* monochrome */
+        {
+            do
+            {
+                y = *ysrc++;
+                red   = RYFAC * y + 32639;  /* blue == red */
+                green = GYFAC * y + 32639;
+                rbits = ((unsigned)red) >> 16;
+                gbits = ((unsigned)green) >> 16;
+#if LCD_PIXELFORMAT == RGB565
+                *dst_row++ = (rbits << 11) | (gbits << 5) | rbits;
+#elif LCD_PIXELFORMAT == RGB565SWAPPED
+                *dst_row++ = swap16((rbits << 11) | (gbits << 5) | rbits);
+#endif
+            }
+            while (dst_row < row_end);
+        }
+        
+        src_y++;
+        dst += LCD_WIDTH;
+    }
+    while (dst < dst_end);    
+}
+#endif
+
 /* switch off overlay, for handling SYS_ events */
 void cleanup(void *parameter)
 {
@@ -1806,10 +1956,18 @@ int scroll_bmp(struct t_disp* pdisp)
             {
                 MYXLCD(scroll_right)(move); /* scroll right */
                 pdisp->x -= move;
+#ifdef HAVE_LCD_COLOR
+                yuv_bitmap_part(
+                    pdisp->bitmap, pdisp->csub_x, pdisp->csub_y, 
+                    pdisp->x, pdisp->y, pdisp->stride,
+                    0, MAX(0, (LCD_HEIGHT-pdisp->height)/2), /* x, y */
+                    move, MIN(LCD_HEIGHT, pdisp->height));   /* w, h */
+#else
                 MYXLCD(gray_bitmap_part)(
-                    pdisp->bitmap, pdisp->x, pdisp->y, pdisp->stride,
+                    pdisp->bitmap[0], pdisp->x, pdisp->y, pdisp->stride,
                     0, MAX(0, (LCD_HEIGHT-pdisp->height)/2), /* x, y */
                     move, MIN(LCD_HEIGHT, pdisp->height));   /* w, h */
+#endif
                 MYLCD_UPDATE();
             }
             break;
@@ -1821,11 +1979,19 @@ int scroll_bmp(struct t_disp* pdisp)
             {
                 MYXLCD(scroll_left)(move); /* scroll left */
                 pdisp->x += move;
+#ifdef HAVE_LCD_COLOR
+                yuv_bitmap_part(
+                    pdisp->bitmap, pdisp->csub_x, pdisp->csub_y,
+                    pdisp->x + LCD_WIDTH - move, pdisp->y, pdisp->stride,
+                    LCD_WIDTH - move, MAX(0, (LCD_HEIGHT-pdisp->height)/2), /* x, y */
+                    move, MIN(LCD_HEIGHT, pdisp->height));   /* w, h */
+#else
                 MYXLCD(gray_bitmap_part)(
-                    pdisp->bitmap, pdisp->x + LCD_WIDTH - move,
+                    pdisp->bitmap[0], pdisp->x + LCD_WIDTH - move,
                     pdisp->y, pdisp->stride,
                     LCD_WIDTH - move, MAX(0, (LCD_HEIGHT-pdisp->height)/2), /* x, y */
                     move, MIN(LCD_HEIGHT, pdisp->height));   /* w, h */
+#endif
                 MYLCD_UPDATE();
             }
             break;
@@ -1837,10 +2003,18 @@ int scroll_bmp(struct t_disp* pdisp)
             {
                 MYXLCD(scroll_down)(move); /* scroll down */
                 pdisp->y -= move;
+#ifdef HAVE_LCD_COLOR
+                yuv_bitmap_part(
+                    pdisp->bitmap, pdisp->csub_x, pdisp->csub_y,
+                    pdisp->x, pdisp->y, pdisp->stride,
+                    MAX(0, (LCD_WIDTH-pdisp->width)/2), 0,   /* x, y */
+                    MIN(LCD_WIDTH, pdisp->width), move);     /* w, h */
+#else
                 MYXLCD(gray_bitmap_part)(
-                    pdisp->bitmap, pdisp->x, pdisp->y, pdisp->stride,
+                    pdisp->bitmap[0], pdisp->x, pdisp->y, pdisp->stride,
                     MAX(0, (LCD_WIDTH-pdisp->width)/2), 0,   /* x, y */
                     MIN(LCD_WIDTH, pdisp->width), move);     /* w, h */
+#endif
                 MYLCD_UPDATE();
             }
             break;
@@ -1852,11 +2026,19 @@ int scroll_bmp(struct t_disp* pdisp)
             {
                 MYXLCD(scroll_up)(move); /* scroll up */
                 pdisp->y += move;
+#ifdef HAVE_LCD_COLOR
+               yuv_bitmap_part(
+                    pdisp->bitmap, pdisp->csub_x, pdisp->csub_y, pdisp->x,
+                    pdisp->y + LCD_HEIGHT - move, pdisp->stride,
+                    MAX(0, (LCD_WIDTH-pdisp->width)/2), LCD_HEIGHT - move, /* x, y */
+                    MIN(LCD_WIDTH, pdisp->width), move);     /* w, h */
+#else
                 MYXLCD(gray_bitmap_part)(
-                    pdisp->bitmap, pdisp->x,
+                    pdisp->bitmap[0], pdisp->x,
                     pdisp->y + LCD_HEIGHT - move, pdisp->stride,
                     MAX(0, (LCD_WIDTH-pdisp->width)/2), LCD_HEIGHT - move, /* x, y */
                     MIN(LCD_WIDTH, pdisp->width), move);     /* w, h */
+#endif
                 MYLCD_UPDATE();
             }
             break;
@@ -1927,35 +2109,50 @@ void align(unsigned char** ppbuf, int* plen, int align)
     *ppbuf = (unsigned char*)aligned;
 }
 
+int jpegmem(struct jpeg *p_jpg, int ds)
+{
+    int size;
+
+    size = (p_jpg->x_phys/ds/p_jpg->subsample_x[0]) 
+         * (p_jpg->y_phys/ds/p_jpg->subsample_y[0]);
+#ifdef HAVE_LCD_COLOR
+    if (p_jpg->blocks > 1) /* colour, add requirements for chroma */
+    {
+        size += (p_jpg->x_phys/ds/p_jpg->subsample_x[1])
+              * (p_jpg->y_phys/ds/p_jpg->subsample_y[1]);
+        size += (p_jpg->x_phys/ds/p_jpg->subsample_x[2])
+              * (p_jpg->y_phys/ds/p_jpg->subsample_y[2]);
+    }
+#endif
+    return size;
+}
 
 /* how far can we zoom in without running out of memory */
-int min_downscale(int x, int y, int bufsize)
+int min_downscale(struct jpeg *p_jpg, int bufsize)
 {
     int downscale = 8;
-
-    if ((x/8) * (y/8) > bufsize)
+    
+    if (jpegmem(p_jpg, 8) > bufsize)
         return 0; /* error, too large, even 1:8 doesn't fit */
-
-    while ((x*2/downscale) * (y*2/downscale) < bufsize
-        && downscale > 1)
-    {
+        
+    while (downscale > 1 && jpegmem(p_jpg, downscale/2) <= bufsize)
         downscale /= 2;
-    }
+        
     return downscale;
 }
 
 
 /* how far can we zoom out, to fit image into the LCD */
-int max_downscale(int x, int y)
+int max_downscale(struct jpeg *p_jpg)
 {
     int downscale = 1;
-
-    while ((x/downscale > LCD_WIDTH || y/downscale > LCD_HEIGHT)
-        && downscale < 8)
+    
+    while (downscale < 8 && (p_jpg->x_size > LCD_WIDTH*downscale
+                          || p_jpg->y_size > LCD_HEIGHT*downscale))
     {
         downscale *= 2;
     }
-
+    
     return downscale;
 }
 
@@ -1970,7 +2167,7 @@ struct t_disp* get_image(struct jpeg* p_jpg, int ds)
 
     struct t_disp* p_disp = &disp[ds]; /* short cut */
 
-    if (p_disp->bitmap != NULL)
+    if (p_disp->bitmap[0] != NULL)
     {
         return p_disp; /* we still have it */
     }
@@ -1978,19 +2175,41 @@ struct t_disp* get_image(struct jpeg* p_jpg, int ds)
     /* assign image buffer */
 
      /* physical size needed for decoding */
-    size = (p_jpg->x_phys/ds) * (p_jpg->y_phys / ds);
+    size = jpegmem(p_jpg, ds);
     if (buf_size <= size)
     {   /* have to discard the current */
         int i;
         for (i=1; i<=8; i++)
-            disp[i].bitmap = NULL; /* invalidate all bitmaps */
+            disp[i].bitmap[0] = NULL; /* invalidate all bitmaps */
         buf = buf_root; /* start again from the beginning of the buffer */
         buf_size = root_size;
     }
+    
+#ifdef HAVE_LCD_COLOR
+    if (p_jpg->blocks > 1) /* colour jpeg */
+    {
+        int i;
 
+        for (i = 1; i < 3; i++)
+        {
+            size = (p_jpg->x_phys / ds / p_jpg->subsample_x[i])
+                 * (p_jpg->y_phys / ds / p_jpg->subsample_y[i]);
+            p_disp->bitmap[i] = buf;
+            buf += size;
+            buf_size -= size;
+        }
+        p_disp->csub_x = p_jpg->subsample_x[1];
+        p_disp->csub_y = p_jpg->subsample_y[1];
+    }
+    else
+    {
+        p_disp->csub_x = p_disp->csub_y = 0;
+        p_disp->bitmap[1] = p_disp->bitmap[2] = buf;
+    }
+#endif
     /* size may be less when decoded (if height is not block aligned) */
     size = (p_jpg->x_phys/ds) * (p_jpg->y_size / ds);
-    p_disp->bitmap = buf;
+    p_disp->bitmap[0] = buf;
     buf += size;
     buf_size -= size;
 
@@ -2000,9 +2219,9 @@ struct t_disp* get_image(struct jpeg* p_jpg, int ds)
     rb->lcd_update();
 
     /* update image properties */
-    p_disp->width = p_jpg->x_size/ds;
+    p_disp->width = p_jpg->x_size / ds;
     p_disp->stride = p_jpg->x_phys / ds; /* use physical size for stride */
-    p_disp->height = p_jpg->y_size/ds;
+    p_disp->height = p_jpg->y_size / ds;
 
     /* the actual decoding */
     time = *rb->current_tick;
@@ -2106,7 +2325,7 @@ int plugin_main(char* filename)
 
     /* allocate JPEG buffer */
     align(&buf, &buf_size, 2); /* 16 bit align */
-    buf_jpeg = (unsigned char*)(((int)buf + 1) & ~1);
+    buf_jpeg = buf;
     buf += filesize;
     buf_size -= filesize;
     buf_root = buf; /* we can start the decompressed images behind it */
@@ -2117,6 +2336,12 @@ int plugin_main(char* filename)
         rb->close(fd);
         return PLUGIN_ERROR;
     }
+    
+#ifdef HAVE_LCD_COLOR
+    rb->lcd_set_foreground(LCD_WHITE);
+    rb->lcd_set_background(LCD_BLACK);
+    rb->lcd_clear_display();
+#endif
 
     rb->snprintf(print, sizeof(print), "loading %d bytes", filesize);
     rb->lcd_puts(0, 0, print);
@@ -2145,10 +2370,8 @@ int plugin_main(char* filename)
     rb->lcd_puts(0, 2, print);
     rb->lcd_update();
 
-    /* check display constraint */
-    ds_max = max_downscale(jpg.x_size, jpg.y_size);
-    /* check memory constraint */
-    ds_min = min_downscale(jpg.x_phys, jpg.y_phys, buf_size);
+    ds_max = max_downscale(&jpg);            /* check display constraint */
+    ds_min = min_downscale(&jpg, buf_size);  /* check memory constraint */
     if (ds_min == 0)
     {
         rb->splash(HZ*2, true, "too large");
@@ -2172,12 +2395,22 @@ int plugin_main(char* filename)
         rb->lcd_update();
 
         MYLCD(clear_display)();
+#ifdef HAVE_LCD_COLOR
+        yuv_bitmap_part(
+            p_disp->bitmap, p_disp->csub_x, p_disp->csub_y,
+            p_disp->x, p_disp->y, p_disp->stride,
+            MAX(0, (LCD_WIDTH - p_disp->width) / 2),
+            MAX(0, (LCD_HEIGHT - p_disp->height) / 2),
+            MIN(LCD_WIDTH, p_disp->width),
+            MIN(LCD_HEIGHT, p_disp->height));
+#else
         MYXLCD(gray_bitmap_part)(
-            p_disp->bitmap, p_disp->x, p_disp->y, p_disp->stride,
+            p_disp->bitmap[0], p_disp->x, p_disp->y, p_disp->stride,
             MAX(0, (LCD_WIDTH - p_disp->width) / 2),
             MAX(0, (LCD_HEIGHT - p_disp->height) / 2),
             MIN(LCD_WIDTH, p_disp->width),
             MIN(LCD_HEIGHT, p_disp->height));
+#endif
         MYLCD_UPDATE();
 
 #ifdef USEGSLIB