Complete rework of the BMP loader: * Support for 4 bit, 15/16 bit and 32 bit BMPs in addition to the already supported 1 bit, 8 bit and 24 bit formats. * Better protection against corrupt BMP files. * Added dithering for 2 bit targets. * Optimised, compact code. There's almost no code size increase for SH1 (only 68 bytes), and even a decrease for coldfire, with all the additional features. * Code policed. * Fixes bug that loading a backdrop worked only once per session.

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@11548 a1c6a512-1295-4272-9138-f99709370657

1 files changed, 340 insertions, 337 deletions

diff --git a/apps/recorder/bmp.c b/apps/recorder/bmp.c
index ab0b8bf..8fe7663 100644
--- a/apps/recorder/bmp.c
+++ b/apps/recorder/bmp.c
@@ -22,6 +22,11 @@
  - New BMP loader function, based on the old one (borrowed a lot of
    calculations and checks there.)
  - Conversion part needs some optimization, doing unneeded calulations now.
+2006-11-18 Jens Arnold: complete rework
+ - All canonical formats supported now (1, 4, 8, 15/16, 24 and 32 bit)
+ - better protection against malformed / non-standard BMPs
+ - code heavily optimised for both size and speed
+ - dithering for 2 bit targets
 */
 
 #include <stdio.h>
@@ -43,98 +48,89 @@
 #pragma pack (push, 2)
 #endif
 
-/* Struct from original code. */
-struct Fileheader {
-    uint16_t Type;        /* signature - 'BM' */
-    uint32_t Size;         /* file size in bytes */
-    uint16_t Reserved1;   /* 0 */
-    uint16_t Reserved2;   /* 0 */
-    uint32_t OffBits;      /* offset to bitmap */
-    uint32_t StructSize;   /* size of this struct (40) */
-    uint32_t Width;        /* bmap width in pixels */
-    uint32_t Height;       /* bmap height in pixels */
-    uint16_t Planes;      /* num planes - always 1 */
-    uint16_t BitCount;    /* bits per pixel */
-    uint32_t Compression;  /* compression flag */
-    uint32_t SizeImage;    /* image size in bytes */
-    int32_t XPelsPerMeter;         /* horz resolution */
-    int32_t YPelsPerMeter;         /* vert resolution */
-    uint32_t ClrUsed;      /* 0 -> color table size */
-    uint32_t ClrImportant; /* important color count */
+/* BMP header structure */
+struct bmp_header {
+    uint16_t type;          /* signature - 'BM' */
+    uint32_t size;          /* file size in bytes */
+    uint16_t reserved1;     /* 0 */
+    uint16_t reserved2;     /* 0 */
+    uint32_t off_bits;      /* offset to bitmap */
+    uint32_t struct_size;   /* size of this struct (40) */
+    uint32_t width;         /* bmap width in pixels */
+    uint32_t height;        /* bmap height in pixels */
+    uint16_t planes;        /* num planes - always 1 */
+    uint16_t bit_count;     /* bits per pixel */
+    uint32_t compression;   /* compression flag */
+    uint32_t size_image;    /* image size in bytes */
+    int32_t x_pels_per_meter;   /* horz resolution */
+    int32_t y_pels_per_meter;   /* vert resolution */
+    uint32_t clr_used;      /* 0 -> color table size */
+    uint32_t clr_important; /* important color count */
 } STRUCT_PACKED;
 
-struct rgb_quad { /* Little endian */
-  unsigned char blue;
-  unsigned char green;
-  unsigned char red;
-  unsigned char reserved;
-} STRUCT_PACKED;
+union rgb_union {
+    struct { /* Little endian */
+        unsigned char blue;
+        unsigned char green;
+        unsigned char red;
+        unsigned char reserved;
+    };
+    uint32_t raw;
+};
 
-/* big endian functions */
-static uint16_t readshort(uint16_t *value) {
-    unsigned char* bytes = (unsigned char*) value;
-    return bytes[0] | (bytes[1] << 8);
-}
+/* masks for supported BI_BITFIELDS encodings (16/32 bit), little endian */
+static const unsigned char bitfields[3][12] = {
+    { 0x00,0x7c,0x00,0,  0xe0,0x03,0x00,0,  0x1f,0x00,0x00,0 }, /* 15 bit */
+    { 0x00,0xf8,0x00,0,  0xe0,0x07,0x00,0,  0x1f,0x00,0x00,0 }, /* 16 bit */
+    { 0x00,0x00,0xff,0,  0x00,0xff,0x00,0,  0xff,0x00,0x00,0 }, /* 32 bit */
+};
 
-static uint32_t readlong(uint32_t *value) {
-    unsigned char* bytes = (unsigned char*) value;
-    return (long)bytes[0] | ((long)bytes[1] << 8) |
-        ((long)bytes[2] << 16) | ((long)bytes[3] << 24);
-}
+#if LCD_DEPTH > 1
+/* canonical ordered dither matrix */
+static const unsigned char dither_matrix[16][16] = {
+    {   0,192, 48,240, 12,204, 60,252,  3,195, 51,243, 15,207, 63,255 },
+    { 128, 64,176,112,140, 76,188,124,131, 67,179,115,143, 79,191,127 },
+    {  32,224, 16,208, 44,236, 28,220, 35,227, 19,211, 47,239, 31,223 },
+    { 160, 96,144, 80,172,108,156, 92,163, 99,147, 83,175,111,159, 95 },
+    {   8,200, 56,248,  4,196, 52,244, 11,203, 59,251,  7,199, 55,247 },
+    { 136, 72,184,120,132, 68,180,116,139, 75,187,123,135, 71,183,119 },
+    {  40,232, 24,216, 36,228, 20,212, 43,235, 27,219, 39,231, 23,215 },
+    { 168,104,152, 88,164,100,148, 84,171,107,155, 91,167,103,151, 87 },
+    {   2,194, 50,242, 14,206, 62,254,  1,193, 49,241, 13,205, 61,253 },
+    { 130, 66,178,114,142, 78,190,126,129, 65,177,113,141, 77,189,125 },
+    {  34,226, 18,210, 46,238, 30,222, 33,225, 17,209, 45,237, 29,221 },
+    { 162, 98,146, 82,174,110,158, 94,161, 97,145, 81,173,109,157, 93 },
+    {  10,202, 58,250,  6,198, 54,246,  9,201, 57,249,  5,197, 53,245 },
+    { 138, 74,186,122,134, 70,182,118,137, 73,185,121,133, 69,181,117 },
+    {  42,234, 26,218, 38,230, 22,214, 41,233, 25,217, 37,229, 21,213 },
+    { 170,106,154, 90,166,102,150, 86,169,105,153, 89,165,101,149, 85 }
+};
+#endif
 
-unsigned char brightness(struct rgb_quad color)
+/* little endian functions */
+static inline unsigned readshort(uint16_t *value)
 {
-    return (3 * (unsigned int)color.red + 6 * (unsigned int)color.green
-              + (unsigned int)color.blue) / 10;
+    unsigned char* bytes = (unsigned char*) value;
+    return (unsigned)bytes[0] | ((unsigned)bytes[1] << 8);
 }
 
-/* Function to get a pixel from a line. (Tomas: maybe a better way?) */
-inline int getpix(int px, unsigned char *bmpbuf) {
-    int a = (px / 8);
-    int b = (7 - (px % 8));
-
-    return (bmpbuf[a] & (1 << b)) != 0;
+static inline uint32_t readlong(uint32_t *value)
+{
+    unsigned char* bytes = (unsigned char*) value;
+    return (uint32_t)bytes[0] | ((uint32_t)bytes[1] << 8) |
+           ((uint32_t)bytes[2] << 16) | ((uint32_t)bytes[3] << 24);
 }
 
-#if LCD_DEPTH == 16
-/* 24 -> lcd depth dither */
-static fb_data dither_24_to_lcd(struct rgb_quad rgb, int row, int col)
+static inline unsigned brightness(union rgb_union color)
 {
-    static const unsigned char dith[][16] =
-    {
-        {   /* 5 bit */
-            0,6,1,7,
-            4,2,5,3,
-            1,7,0,6,
-            5,3,4,2
-        },
-        {   /* 6 bit */
-            0,3,0,3,
-            2,1,2,1,
-            0,3,0,3,
-            2,1,2,1
-        },
-    };
-
-    const unsigned elm = (row & 3) + ((col & 3) << 2);
-    unsigned b, g, r;
-
-    b = ((unsigned)rgb.blue  + dith[0][elm]) >> (8-LCD_BLUE_BITS);
-    if (b > LCD_MAX_BLUE)  b = LCD_MAX_BLUE;
-    g = ((unsigned)rgb.green + dith[1][elm]) >> (8-LCD_GREEN_BITS);
-    if (g > LCD_MAX_GREEN) g = LCD_MAX_GREEN;
-    r = ((unsigned)rgb.red   + dith[0][elm]) >> (8-LCD_RED_BITS);
-    if (r > LCD_MAX_RED)   r = LCD_MAX_RED;
-
-    return LCD_RGBPACK_LCD(r, g, b);
+    return (3 * (unsigned)color.red + 6 * (unsigned)color.green
+              + (unsigned)color.blue) / 10;
 }
-#endif /* LCD_DEPTH == 16 */
-
 
 /******************************************************************************
  * read_bmp_file()
  *
- * Reads a monochrome BMP file and puts the data in rockbox format in *bitmap.
+ * Reads a BMP file and puts the data in rockbox format in *bitmap.
  *
  *****************************************************************************/
 int read_bmp_file(char* filename,
@@ -142,352 +138,359 @@ int read_bmp_file(char* filename,
                   int maxsize,
                   int format)
 {
-    struct Fileheader fh;
-    int width, height, PaddedWidth, PaddedHeight, dst_width;
+    struct bmp_header bmph;
+    int width, height, padded_width;
+    int dst_height, dst_width;
     int fd, row, col, ret;
-    struct rgb_quad palette[256];
-    int invert_pixel = 0;
-    int numcolors;
-    int depth;
-    int totalsize;
-    char *bitmap = bm->data;
-    unsigned char bmpbuf[LCD_WIDTH*sizeof(struct rgb_quad)]; /* Buffer for one line */
-#if LCD_DEPTH != 1
+    int depth, numcolors, compression, totalsize;
+
+    unsigned char *bitmap = bm->data;
+    uint32_t bmpbuf[LCD_WIDTH]; /* Buffer for one line */
+    uint32_t palette[256];
+#if LCD_DEPTH > 1
     bool transparent = false;
-#if LCD_DEPTH == 16
-    /* Should adapt dithering to all native depths */
     bool dither = false;
-    if(format & FORMAT_DITHER) {
-        dither = true;
-        format &= ~FORMAT_DITHER;
-    }
-#endif
-    if(format & FORMAT_TRANSPARENT) {
+
+    if (format & FORMAT_TRANSPARENT) {
         transparent = true;
         format &= ~FORMAT_TRANSPARENT;
     }
+    if (format & FORMAT_DITHER) {
+        dither = true;
+        format &= ~FORMAT_DITHER;
+    }
 #else
-    format = FORMAT_MONO;
+
+    (void)format;
 #endif
 
     fd = open(filename, O_RDONLY);
 
     /* Exit if file opening failed */
     if (fd < 0) {
-        DEBUGF("error - can't open '%s' open returned: %d\n", filename, fd);
-        return (fd * 10) - 1;
+        DEBUGF("read_bmp_file: can't open '%s', rc: %d\n", filename, fd);
+        return fd * 10 - 1;
     }
 
     /* read fileheader */
-    ret = read(fd, &fh, sizeof(struct Fileheader));
-    if(ret < 0) {
+    ret = read(fd, &bmph, sizeof(struct bmp_header));
+    if (ret < 0) {
         close(fd);
-        return (ret * 10 - 2);
+        return ret * 10 - 2;
     }
 
-    if(ret != sizeof(struct Fileheader)) {
-        DEBUGF("error - can't read Fileheader structure.");
+    if (ret != sizeof(struct bmp_header)) {
+        DEBUGF("read_bmp_file: can't read BMP header.");
         close(fd);
         return -3;
     }
 
-    /* Exit if too wide */
-    if (readlong(&fh.Width) > LCD_WIDTH) {
-        DEBUGF("error - Bitmap is too wide (%d pixels, max is %d)\n",
-                        readlong(&fh.Width), LCD_WIDTH);
+    width = readlong(&bmph.width);
+    if (width > LCD_WIDTH) {
+        DEBUGF("read_bmp_file: Bitmap too wide (%d pixels, max is %d)\n",
+                        width, LCD_WIDTH);
         close(fd);
-        return -5;
+        return -4;
     }
 
-    /* Exit if too high */
-    if (readlong(&fh.Height) > LCD_HEIGHT) {
-        DEBUGF("error - Bitmap is too high (%d pixels, max is %d)\n",
-                        readlong(&fh.Height), LCD_HEIGHT);
+    height = readlong(&bmph.height);
+    if (height > LCD_HEIGHT) {
+        DEBUGF("read_bmp_file: Bitmap too high (%d pixels, max is %d)\n",
+                        height, LCD_HEIGHT);
         close(fd);
-        return -6;
+        return -5;
     }
 
-    /* Calculate image size */
-    height = readlong(&fh.Height);
-    width = readlong(&fh.Width);
-    depth = readshort(&fh.BitCount);
-
-    /* 4-byte boundary aligned */
-    PaddedWidth = ((width * depth + 31) / 8) & ~3;
+    depth = readshort(&bmph.bit_count);
+    padded_width = ((width * depth + 31) / 8) & ~3;  /* 4-byte boundary aligned */
 
 #if LCD_DEPTH > 1
-        if(format == FORMAT_ANY) {
-            if(depth == 1)
-                format = FORMAT_MONO;
-            else
-                format = FORMAT_NATIVE;
-        }
+    if (format == FORMAT_ANY) {
+        if (depth == 1)
+            format = FORMAT_MONO;
+        else
+            format = FORMAT_NATIVE;
+    }
+    bm->format = format;
 #endif
+    /* returning image size */
+    bm->width = width;
+    bm->height = height;
 
-    /* PaddedHeight is for rockbox format. */
-    if(format == FORMAT_MONO) {
-        PaddedHeight = (height + 7) / 8;
-        dst_width = width;
-        totalsize = PaddedHeight * dst_width;
-    } else {
+#if LCD_DEPTH > 1
+    if (format == FORMAT_NATIVE) {
 #if LCD_DEPTH == 2
 #if LCD_PIXELFORMAT == VERTICAL_PACKING
-        PaddedHeight = (height + 3) / 4;
-        dst_width = width;
-#else
-        PaddedHeight = height;
-        dst_width = (width + 3) / 4;
-#endif
-#else
-        PaddedHeight = height;
-        dst_width = width;
-#endif
-        totalsize = PaddedHeight * dst_width * sizeof(fb_data);
+        dst_width  = width;
+        dst_height = (height + 3) / 4;
+#else /* LCD_PIXELFORMAT == HORIZONTAL_PACKING */
+        dst_width  = (width + 3) / 4;
+        dst_height = height;
+#endif /* LCD_PIXELFORMAT */
+#elif LCD_DEPTH == 16
+        dst_width  = width;
+        dst_height = height;
+#endif /* LCD_DEPTH */
+        totalsize  = dst_width * dst_height * sizeof(fb_data);
+    } else
+#endif /* LCD_DEPTH > 1 */
+    {
+        dst_width  = width;
+        dst_height = (height + 7) / 8;
+        totalsize  = dst_width * dst_height;
     }
 
     /* Check if this fits the buffer */
-
     if (totalsize > maxsize) {
-        DEBUGF("error - Bitmap is too large to fit the supplied buffer: "
-               "%d bytes.\n", (PaddedHeight * dst_width));
+        DEBUGF("read_bmp_file: Bitmap too large for buffer: "
+               "%d bytes.\n", totalsize);
         close(fd);
-        return -7;
+        return -6;
     }
 
-    if (depth <= 8)
-    {
-        numcolors = readlong(&fh.ClrUsed);
+    compression = readlong(&bmph.compression);
+    if (depth <= 8) {
+        numcolors = readlong(&bmph.clr_used);
         if (numcolors == 0)
             numcolors = 1 << depth;
-
-        if(read(fd, palette, numcolors * sizeof(struct rgb_quad))
-           != numcolors * (int)sizeof(struct rgb_quad))
+    } else
+        numcolors = (compression == 3) ? 3 : 0;
+        
+    if (numcolors > 0 && numcolors <= 256) {
+        if (read(fd, palette, numcolors * sizeof(uint32_t))
+            != numcolors * (int)sizeof(uint32_t))
         {
-            DEBUGF("error - Can't read bitmap's color palette\n");
+            DEBUGF("read_bmp_file: Can't read color palette\n");
             close(fd);
-            return -8;
+            return -7;
         }
     }
 
-    /* Use the darker palette color as foreground on mono bitmaps */
-    if(depth == 1) {
-        if(brightness(palette[0]) < brightness(palette[1]))
-           invert_pixel = 1;
+    switch (depth) {
+      case 16:
+#if LCD_DEPTH >= 16
+        /* don't dither 16 bit BMP to LCD with same or larger depth */
+        dither = false;
+#endif
+        if (compression == 0) { /* BI_RGB, i.e. 15 bit */
+            depth = 15;
+            break;
+        }   /* else fall through */
+
+      case 32:
+        if (compression == 3) { /* BI_BITFIELDS */
+            if (!memcmp(palette, bitfields[0], 12)) {    /* 15 bit */
+                depth = 15;
+                break;
+            }
+            if (!memcmp(palette, bitfields[1], 12)       /* 16 bit */
+                || !memcmp(palette, bitfields[2], 12))   /* 32 bit */
+            {
+                break;
+            }
+        }   /* else fall through */
+
+      default:
+        if (compression != 0) { /* not BI_RGB */
+            DEBUGF("read_bmp_file: Unsupported compression (type %d)\n",
+                   compression);
+            close(fd);
+            return -8;
+        }
+        break;
     }
 
     /* Search to the beginning of the image data */
-    lseek(fd, (off_t)readlong(&fh.OffBits), SEEK_SET);
-
-#if LCD_DEPTH == 2
-    if(format == FORMAT_NATIVE)
-        memset(bitmap, 0, totalsize);
-#endif
+    lseek(fd, (off_t)readlong(&bmph.off_bits), SEEK_SET);
 
-#if LCD_DEPTH > 1
-    fb_data *dest = (fb_data *)bitmap;
+#if LCD_DEPTH >= 8
+    if (format == FORMAT_MONO)
 #endif
+        memset(bitmap, 0, totalsize);
 
     /* loop to read rows and put them to buffer */
-    for (row = 0; row < height; row++) {
+    for (row = height - 1; row >= 0; row--) {
+        unsigned data, mask;
         unsigned char *p;
+        uint16_t *p2;
+        uint32_t *rp;
+        union rgb_union *qp;
+        union rgb_union q0, q1;
 
         /* read one row */
-        ret = read(fd, bmpbuf, PaddedWidth);
-        if (ret != PaddedWidth) {
-            DEBUGF("error reading image, read returned: %d expected was: "
-                   "%d\n", ret, PaddedWidth);
+        ret = read(fd, bmpbuf, padded_width);
+        if (ret != padded_width) {
+            DEBUGF("read_bmp_file: error reading image, read returned: %d "
+                   "expected: %d\n", ret, padded_width);
             close(fd);
             return -9;
         }
 
-        switch(depth) {
-        case 1:
-#if LCD_DEPTH > 1
-            if(format == FORMAT_MONO) {
-#endif
-                /* Mono -> Mono */
-                for (col = 0; col < width; col++) {
-                    ret = getpix(col, bmpbuf) ^ invert_pixel;
-                    if (ret) {
-                        bitmap[width * ((height - row - 1) / 8) + col]
-                            |= 1 << ((height - row - 1) % 8);
-                    } else {
-                        bitmap[width * ((height - row - 1) / 8) + col]
-                            &= ~ 1 << ((height - row - 1) % 8);
-                    }
-                }
-#if LCD_DEPTH == 2
-#if LCD_PIXELFORMAT == VERTICAL_PACKING
-            } else {
-                /* Mono -> 2gray (iriver H1xx) */
-                for (col = 0; col < width; col++) {
-                    ret = getpix(col, bmpbuf) ^ invert_pixel;
-
-                    if (ret)
-                        dest[((height - row - 1)/4) * width + col] |=
-                            0xC0 >> (2 * (~(height - row - 1) & 3));
-                }
+        /* convert whole line in-place to XRGB8888 (little endian) */
+        rp = bmpbuf + width;
+        switch (depth) {
+          case 1:
+            q0.raw = palette[0];
+            q1.raw = palette[1];
+            p = (unsigned char*)bmpbuf + (width + 7) / 8;
+            mask = 0x80 >> ((width + 7) & 7);
+            while (p > (unsigned char*)bmpbuf) {
+                data = *(--p);
+                for (; mask <= 0x80; mask <<= 1)
+                    *(--rp) = (data & mask) ? q1.raw : q0.raw;
+                mask = 0x01;
             }
-#else
-            } else {
-                /* Mono -> 2gray (ipod) */
-                for (col = 0; col < width; col++) {
-                    ret = getpix(col, bmpbuf) ^ invert_pixel;
-
-                    if (ret)
-                        dest[(height - row - 1) * dst_width + col/4] |=
-                            0xC0 >> (2 * (col & 3));
-                }
-            }
-#endif
-#elif LCD_DEPTH == 16
-            } else {
-                /* Mono -> RGB16 */
-                for (col = 0; col < width; col++) {
-                    ret = getpix(col, bmpbuf);
-                    unsigned short rgb16 = LCD_RGBPACK(palette[ret].red,
-                                                       palette[ret].green,
-                                                       palette[ret].blue);
-                    dest[width * (height - row - 1) + col] = rgb16;
-                }
-            }
-#endif
             break;
 
+          case 4:
+            if (width & 1)
+                rp++;
+            p = (unsigned char*)bmpbuf + (width + 1) / 2;
+            while (p > (unsigned char*)bmpbuf) {
+                data = *(--p);
+                *(--rp) = palette[data & 0x0f];
+                *(--rp) = palette[data >> 4];
+            }
+            break;
 
-        case 8:
-            p = bmpbuf;
-#if LCD_DEPTH > 1
-            if(format == FORMAT_MONO) {
-#endif
-                /* 8-bit RGB24 palette -> mono */
-                for (col = 0; col < width; col++) {
-                    struct rgb_quad rgb = palette[*p];
-                    ret = brightness(rgb);
-                    if (ret > 96) {
-                        bitmap[width * ((height - row - 1) / 8) + col]
-                            &= ~ 1 << ((height - row - 1) % 8);
-                    } else {
-                        bitmap[width * ((height - row - 1) / 8) + col]
-                            |= 1 << ((height - row - 1) % 8);
-                    }
-                    p++;
-                }
-#if LCD_DEPTH == 2
-#if LCD_PIXELFORMAT == VERTICAL_PACKING
-            } else {
-                /* 8-bit RGB24 palette -> 2gray (iriver H1xx) */
-                for (col = 0; col < width; col++) {
-                    struct rgb_quad rgb = palette[*p];
-                    ret = brightness(rgb);
-
-                    dest[((height - row - 1)/4) * width + col] |=
-                        (~ret & 0xC0) >> (2 * (~(height - row - 1) & 3));
-                    p++;
+          case 8:
+            p = (unsigned char*)bmpbuf + width;
+            while (p > (unsigned char*)bmpbuf)
+                *(--rp) = palette[*(--p)];
+            break;
+            
+          case 15:
+          case 16:
+            p2 = (uint16_t *)bmpbuf + width;
+            while (p2 > (uint16_t *)bmpbuf) {
+                unsigned component, rgb;
+
+                data = letoh16(*(--p2));
+                /* blue */
+                component = (data << 3) & 0xf8;
+#ifdef ROCKBOX_BIG_ENDIAN
+                rgb = (component | (component >> 5)) << 8;
+                /* green */
+                data >>= 2;
+                if (depth == 15) {
+                    component = data & 0xf8;
+                    rgb |= component | (component >> 5);
+                } else {
+                    data >>= 1;
+                    component = data & 0xfc;
+                    rgb |= component | (component >> 6);
                 }
-            }
-#else
-            } else {
-                /* 8-bit RGB24 palette -> 2gray (ipod) */
-                for (col = 0; col < width; col++) {
-                    struct rgb_quad rgb = palette[*p];
-                    ret = brightness(rgb);
-
-                    dest[(height - row - 1) * dst_width + col/4] |=
-                        (~ret & 0xC0) >> (2 * (col & 3));
-                    p++;
+                /* red */
+                data >>= 5;
+                component = data & 0xf8;
+                rgb = (rgb << 8) | component | (component >> 5);
+                *(--rp) = rgb << 8;
+#else /* little endian */
+                rgb = component | (component >> 5);
+                /* green */
+                data >>= 2;
+                if (depth == 15) {
+                    component = data & 0xf8;
+                    rgb |= (component | (component >> 5)) << 8;
+                } else {
+                    data >>= 1;
+                    component = data & 0xfc;
+                    rgb |= (component | (component >> 6)) << 8;
                 }
-            }
+                /* red */
+                data >>= 5;
+                component = data & 0xf8;
+                rgb |= (component | (component >> 5)) << 16;
+                *(--rp) = rgb;
 #endif
-#elif LCD_DEPTH == 16
-            } else {
-                /* 8-bit RGB24 palette -> RGB16 */
-                for (col = 0; col < width; col++, p++) {
-                    struct rgb_quad rgb = palette[*p];
-                    dest[width * (height - row - 1) + col] = dither ?
-                        dither_24_to_lcd(rgb, row, col) :
-                        (fb_data)LCD_RGBPACK(rgb.red, rgb.green, rgb.blue);
-                }
             }
-#endif
             break;
 
-        case 24:
-            p = bmpbuf;
+          case 24:
+            p = (unsigned char*)bmpbuf + 3 * width;
+            while (p > (unsigned char*)bmpbuf) {
+                data = *(--p);
+                data = (data << 8) | *(--p);
+                data = (data << 8) | *(--p);
+                *(--rp) = htole32(data);
+            }
+            break;
+
+          case 32: /* already in desired format */
+            break;
+        }
+        
+        /* Convert to destination format */
+        qp = (union rgb_union *)bmpbuf;
 #if LCD_DEPTH > 1
-            if(format == FORMAT_MONO) {
-#endif
-                /* RGB24 -> mono */
-                for (col = 0; col < width; col++) {
-                    struct rgb_quad rgb;
-                    rgb.red = p[2];
-                    rgb.green = p[1];
-                    rgb.blue = p[0];
-                    ret = brightness(rgb);
-                    if (ret > 96) {
-                        bitmap[width * ((height - row - 1) / 8) + col]
-                            &= ~ 1 << ((height - row - 1) % 8);
-                    } else {
-                        bitmap[width * ((height - row - 1) / 8) + col]
-                            |= 1 << ((height - row - 1) % 8);
-                    }
-                    p += 3;
-                }
+        if (format == FORMAT_NATIVE) {
 #if LCD_DEPTH == 2
 #if LCD_PIXELFORMAT == VERTICAL_PACKING
-            } else {
-                /* RGB24 -> 2gray (iriver H1xx) */
-                for (col = 0; col < width; col++) {
-                    struct rgb_quad rgb;
-                    rgb.red = p[2];
-                    rgb.green = p[1];
-                    rgb.blue = p[0];
-                    ret = brightness(rgb);
-
-                    dest[((height - row - 1)/4) * width + col] |=
-                        (~ret & 0xC0) >> (2 * (~(height - row - 1) & 3));
-                    p += 3;
-                }
+            /* iriver H1x0 */
+            fb_data *dest = (fb_data *)bitmap + dst_width * (row / 4);
+            int shift = 2 * (row & 3);
+            int delta = 127;
+            unsigned bright;
+
+            for (col = 0; col < width; col++) {
+                if (dither)
+                    delta = dither_matrix[row & 0xf][col & 0xf];
+                bright = (257 * (3 * brightness(*qp++) + delta)) >> 16;
+                *dest++ |= (~bright & 3) << shift;
             }
-#else
-            } else {
-                /* RGB24 -> 2gray (ipod) */
-                for (col = 0; col < width; col++) {
-                    struct rgb_quad rgb;
-                    rgb.red = p[2];
-                    rgb.green = p[1];
-                    rgb.blue = p[0];
-                    ret = brightness(rgb);
-
-                    dest[(height - row - 1) * dst_width + col/4] |=
-                        (~ret & 0xC0) >> (2 * (col & 3));
-                    p += 3;
+#else /* LCD_PIXELFORMAT == HORIZONTAL_PACKING */
+            /* greyscale iPods */
+            fb_data *dest = (fb_data *)bitmap + dst_width * row;
+            int shift = 6;
+            int delta = 127;
+            unsigned bright;
+            unsigned data = 0;
+            
+            for (col = 0; col < width; col++) {
+                if (dither)
+                    delta = dither_matrix[row & 0xf][col & 0xf];
+                bright = (257 * (3 * brightness(*qp++) + delta)) >> 16;
+                data |= (~bright & 3) << shift;
+                shift -= 2;
+                if (shift < 0) {
+                    *dest++ = data;
+                    data = 0;
+                    shift = 6;
                 }
             }
-#endif
+            if (shift < 6)
+                *dest++ = data;
+#endif /* LCD_PIXELFORMAT */
 #elif LCD_DEPTH == 16
-            } else {
-                /* RGB24 -> RGB16 */
-                for (col = 0; col < width; col++, p += 3) {
-                    dest[width * (height - row - 1) + col] = dither ?
-                        dither_24_to_lcd(*(struct rgb_quad *)p, row, col) :
-                        (fb_data)LCD_RGBPACK(p[2], p[1], p[0]);
-                }
+            /* iriver h300, colour iPods, X5 */
+            fb_data *dest = (fb_data *)bitmap + dst_width * row;
+            int delta = 127;
+            unsigned r, g, b;
+
+            for (col = 0; col < width; col++) {
+                if (dither)
+                    delta = dither_matrix[row & 0xf][col & 0xf];
+                q0 = *qp++;
+                r = (257 * (31 * q0.red + delta)) >> 16;
+                g = (257 * (63 * q0.green + delta)) >> 16;
+                b = (257 * (31 * q0.blue + delta)) >> 16;
+                *dest++ = LCD_RGBPACK_LCD(r, g, b);
             }
-#endif
-            break;
+#endif /* LCD_DEPTH */
+        } else
+#endif /* LCD_DEPTH > 1 */
+        {
+            p = bitmap + dst_width * (row / 8);
+            mask = 1 << (row & 7);
+
+            for (col = 0; col < width; col++, p++)
+                if (brightness(*qp++) < 128)
+                    *p |= mask;
         }
     }
 
     close(fd);
 
-    /* returning image size: */
-    bm->width = width;
-    bm->height = height;
-#if LCD_DEPTH > 1
-    bm->format = format;
-#endif
-
-DEBUGF("totalsize: %d\n", totalsize);
+    DEBUGF("totalsize: %d\n", totalsize);
     return totalsize; /* return the used buffer size. */
 }