1 files changed, 136 insertions, 25 deletions

diff --git a/minui/graphics.cpp b/minui/graphics.cpp
index c0eea9e38..4c7dd3f5c 100644
--- a/minui/graphics.cpp
+++ b/minui/graphics.cpp
@@ -52,6 +52,10 @@ static unsigned char gr_current_r = 255;
 static unsigned char gr_current_g = 255;
 static unsigned char gr_current_b = 255;
 static unsigned char gr_current_a = 255;
+static unsigned char rgb_555[2];
+static unsigned char gr_current_r5 = 31;
+static unsigned char gr_current_g5 = 63;
+static unsigned char gr_current_b5 = 31;
 
 static GRSurface* gr_draw = NULL;
 
@@ -71,6 +75,30 @@ void gr_font_size(int *x, int *y)
     *y = gr_font->cheight;
 }
 
+void blend_16bpp(unsigned char* px, unsigned r5, unsigned g5, unsigned b5, unsigned char a)
+{
+    unsigned char orig[2];
+    orig[0] = px[0];
+    orig[1] = px[1];
+
+    /* This code is a little easier to read
+    unsigned oldred = (orig[1] >> 3);
+    unsigned oldgreen = (((orig[0] >> 5) << 3) + (orig[1] & 0x7));
+    unsigned oldblue = (orig[0] & 0x1F);
+
+    unsigned newred = (oldred * (255-a) + r5 * a) / 255;
+    unsigned newgreen = (oldgreen * (255-a) + g5 * a) / 255;
+    unsigned newblue = (oldblue * (255-a) + b5 * a) / 255;
+    */
+
+    unsigned newred = ((orig[1] >> 3) * (255-a) + r5 * a) / 255;
+    unsigned newgreen = ((((orig[0] >> 5) << 3) + (orig[1] & 0x7)) * (255-a) + g5 * a) / 255;
+    unsigned newblue = ((orig[0] & 0x1F) * (255-a) + b5 * a) / 255;
+
+    *px++ = (newgreen << 5) + (newblue);
+    *px++ = (newred << 3) + (newgreen >> 3);
+}
+
 static void text_blend(unsigned char* src_p, int src_row_bytes,
                        unsigned char* dst_p, int dst_row_bytes,
                        int width, int height)
@@ -82,20 +110,30 @@ static void text_blend(unsigned char* src_p, int src_row_bytes,
             unsigned char a = *sx++;
             if (gr_current_a < 255) a = ((int)a * gr_current_a) / 255;
             if (a == 255) {
-                *px++ = gr_current_r;
-                *px++ = gr_current_g;
-                *px++ = gr_current_b;
-                px++;
+                if (gr_draw->pixel_bytes == 2) {
+                    *px++ = rgb_555[0];
+                    *px++ = rgb_555[1];
+                } else {
+                    *px++ = gr_current_r;
+                    *px++ = gr_current_g;
+                    *px++ = gr_current_b;
+                    px++;
+                }
             } else if (a > 0) {
-                *px = (*px * (255-a) + gr_current_r * a) / 255;
-                ++px;
-                *px = (*px * (255-a) + gr_current_g * a) / 255;
-                ++px;
-                *px = (*px * (255-a) + gr_current_b * a) / 255;
-                ++px;
-                ++px;
+                if (gr_draw->pixel_bytes == 2) {
+                    blend_16bpp(px, gr_current_r5, gr_current_g5, gr_current_b5, a);
+                    px += gr_draw->pixel_bytes;
+                } else {
+                    *px = (*px * (255-a) + gr_current_r * a) / 255;
+                    ++px;
+                    *px = (*px * (255-a) + gr_current_g * a) / 255;
+                    ++px;
+                    *px = (*px * (255-a) + gr_current_b * a) / 255;
+                    ++px;
+                    ++px;
+                }
             } else {
-                px += 4;
+                px += gr_draw->pixel_bytes;
             }
         }
         src_p += src_row_bytes;
@@ -155,6 +193,16 @@ void gr_texticon(int x, int y, GRSurface* icon) {
                icon->width, icon->height);
 }
 
+void gr_convert_rgb_555()
+{
+    gr_current_r5 = (((gr_current_r & 0xFF) * 0x1F) + 0x7F) / 0xFF;
+    gr_current_g5 = (((gr_current_g & 0xFF) * 0x3F) + 0x7F) / 0xFF;
+    gr_current_b5 = (((gr_current_b & 0xFF) * 0x1F) + 0x7F) / 0xFF;
+
+    rgb_555[0] = (gr_current_g5 << 5) + (gr_current_b5);
+    rgb_555[1] = (gr_current_r5 << 3) + (gr_current_g5 >> 3);
+}
+
 void gr_color(unsigned char r, unsigned char g, unsigned char b, unsigned char a)
 {
 #if defined(RECOVERY_ABGR) || defined(RECOVERY_BGRA)
@@ -168,10 +216,19 @@ void gr_color(unsigned char r, unsigned char g, unsigned char b, unsigned char a
     gr_current_b = b;
     gr_current_a = a;
 #endif
+    if (gr_draw->pixel_bytes == 2) {
+        gr_convert_rgb_555();
+    }
 }
 
 void gr_clear()
 {
+    if (gr_draw->pixel_bytes == 2) {
+        gr_fill(0, 0, gr_fb_width(), gr_fb_height());
+        return;
+    }
+
+    // This code only works on 32bpp devices
     if (gr_current_r == gr_current_g && gr_current_r == gr_current_b) {
         memset(gr_draw->data, gr_current_r, gr_draw->height * gr_draw->row_bytes);
     } else {
@@ -204,10 +261,15 @@ void gr_fill(int x1, int y1, int x2, int y2)
         for (y = y1; y < y2; ++y) {
             unsigned char* px = p;
             for (x = x1; x < x2; ++x) {
-                *px++ = gr_current_r;
-                *px++ = gr_current_g;
-                *px++ = gr_current_b;
-                px++;
+                if (gr_draw->pixel_bytes == 2) {
+                    *px++ = rgb_555[0];
+                    *px++ = rgb_555[1];
+                } else {
+                    *px++ = gr_current_r;
+                    *px++ = gr_current_g;
+                    *px++ = gr_current_b;
+                    px++;
+                }
             }
             p += gr_draw->row_bytes;
         }
@@ -216,25 +278,74 @@ void gr_fill(int x1, int y1, int x2, int y2)
         for (y = y1; y < y2; ++y) {
             unsigned char* px = p;
             for (x = x1; x < x2; ++x) {
-                *px = (*px * (255-gr_current_a) + gr_current_r * gr_current_a) / 255;
-                ++px;
-                *px = (*px * (255-gr_current_a) + gr_current_g * gr_current_a) / 255;
-                ++px;
-                *px = (*px * (255-gr_current_a) + gr_current_b * gr_current_a) / 255;
-                ++px;
-                ++px;
+                if (gr_draw->pixel_bytes == 2) {
+                    blend_16bpp(px, gr_current_r5, gr_current_g5, gr_current_b5, gr_current_a);
+                    px += gr_draw->row_bytes;
+                } else {
+                    *px = (*px * (255-gr_current_a) + gr_current_r * gr_current_a) / 255;
+                    ++px;
+                    *px = (*px * (255-gr_current_a) + gr_current_g * gr_current_a) / 255;
+                    ++px;
+                    *px = (*px * (255-gr_current_a) + gr_current_b * gr_current_a) / 255;
+                    ++px;
+                    ++px;
+                }
             }
             p += gr_draw->row_bytes;
         }
     }
 }
 
+void gr_blit_32to16(GRSurface* source, int sx, int sy, int w, int h, int dx, int dy) {
+    dx += overscan_offset_x;
+    dy += overscan_offset_y;
+
+    if (outside(dx, dy) || outside(dx+w-1, dy+h-1)) return;
+
+    unsigned char* src_p = source->data + sy*source->row_bytes + sx*source->pixel_bytes;
+    unsigned char* dst_p = gr_draw->data + dy*gr_draw->row_bytes + dx*gr_draw->pixel_bytes;
+
+    int i, j;
+    for (i = 0; i < h; ++i) {
+        unsigned char* spx = src_p;
+        unsigned char* dpx = dst_p;
+
+        for (j = 0; j < w; ++j) {
+            unsigned a = spx[3];
+
+            if (a == 0) {
+                spx += source->pixel_bytes;
+                dpx += gr_draw->pixel_bytes;
+            } else {
+                unsigned r5 = (((*spx++ & 0xFF) * 0x1F) + 0x7F) / 0xFF;
+                unsigned g5 = (((*spx++ & 0xFF) * 0x3F) + 0x7F) / 0xFF;
+                unsigned b5 = (((*spx++ & 0xFF) * 0x1F) + 0x7F) / 0xFF;
+                spx++;
+                if (a == 255) {
+                    *dpx++ = (g5 << 5) + (b5);
+                    *dpx++ = (r5 << 3) + (g5 >> 3);
+                } else {
+                    blend_16bpp(dpx, r5, g5, b5, a);
+                    spx += source->pixel_bytes;
+                }
+            }
+        }
+        src_p += source->row_bytes;
+        dst_p += gr_draw->row_bytes;
+    }
+}
+
 void gr_blit(GRSurface* source, int sx, int sy, int w, int h, int dx, int dy) {
     if (source == NULL) return;
 
     if (gr_draw->pixel_bytes != source->pixel_bytes) {
-        printf("gr_blit: source has wrong format\n");
-        return;
+        if (gr_draw->pixel_bytes == 2 && source->pixel_bytes == 4) {
+            gr_blit_32to16(source, sx, sy, w, h, dx, dy);
+            return;
+        } else {
+            printf("gr_blit: source has wrong format\n");
+            return;
+        }
     }
 
     dx += overscan_offset_x;