lcd-16bit: rewrite mono_bitmap_part

Rewrite the loop to update the framebuffer by rows rather
than by columns. This should (a) be more efficient on the
majority of targets using horizontal stride framebuffers,
(b) skimp a bit on code size and (c) avoid AddressSanitizer
errors caused by reading past the end of the source buffer.

Change-Id: I238d191d63bfdecd25336fd8879a63d9d6ab7087

1 files changed, 72 insertions, 96 deletions

diff --git a/firmware/drivers/lcd-16bit-common.c b/firmware/drivers/lcd-16bit-common.c
index b9c9060216..1b84847929 100644
--- a/firmware/drivers/lcd-16bit-common.c
+++ b/firmware/drivers/lcd-16bit-common.c
@@ -283,12 +283,6 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
                                      int src_y, int stride, int x, int y,
                                      int width, int height)
 {
-    const unsigned char *src_end;
-    fb_data *dst, *dst_col;
-    unsigned dmask = 0x100; /* bit 8 == sentinel */
-    int drmode = lcd_current_viewport->drawmode;
-    int row;
-
     /******************** Image in viewport clipping **********************/
     /* nothing to draw? */
     if ((width <= 0) || (height <= 0) || (x >= lcd_current_viewport->width) ||
@@ -312,22 +306,10 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
     if (y + height > lcd_current_viewport->height)
         height = lcd_current_viewport->height - y;
 
-    /* adjust for viewport */
+    /* convert to viewport coordinates */
     x += lcd_current_viewport->x;
     y += lcd_current_viewport->y;
 
-    /* 'Bugfix' mono_bitmap_part reads ahead in the buffer, While this is a bug
-     * if the height is <= char bit pixels other memory gets read but is not used
-     * the other option is to check in the hot code path but this appears
-     * sufficient, limit to the sim to stop Address Sanitizer errors
-     */
-#if defined(SIMULATOR) && \
-    (!defined(LCD_STRIDEFORMAT) || LCD_STRIDEFORMAT != VERTICAL_STRIDE)
-    /* vertical stride targets don't seem affected by this */
-    if (height <= CHAR_BIT)
-        stride = 0;
-#endif
-
 #if defined(HAVE_VIEWPORT_CLIP)
     /********************* Viewport on screen clipping ********************/
     /* nothing to draw? */
@@ -342,7 +324,6 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
         src_x -= x;
         x = 0;
     }
-
     if (y < 0)
     {
         height += y;
@@ -354,14 +335,17 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
     if (y + height > LCD_HEIGHT)
         height = LCD_HEIGHT - y;
 #endif
-    src += stride * (src_y >> 3) + src_x; /* move starting point */
-    src_y  &= 7;
-    src_end = src + width;
-    dst_col = FBADDR(x, y);
+
+    /* move starting point */
+    src += stride * (src_y >> 3) + src_x;
+    src_y &= 7;
+
+    unsigned dmask = 0;
+    int drmode = lcd_current_viewport->drawmode;
 
     if (drmode & DRMODE_INVERSEVID)
     {
-        dmask = 0x1ff;          /* bit 8 == sentinel */
+        dmask = 0xff;
         drmode &= DRMODE_SOLID; /* mask out inversevid */
     }
 
@@ -369,107 +353,99 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
     if ((drmode & DRMODE_BG) && lcd_backdrop)
         drmode |= DRMODE_INT_BD;
 
-    /* go through each column and update each pixel  */
-    do
+    fb_data* dst = FBADDR(x, y);
+    while(height > 0)
     {
-        const unsigned char *src_col = src++;
-        unsigned data = (*src_col ^ dmask) >> src_y;
+        const unsigned char* src_col = src;
+        const unsigned char* src_end = src + width;
+        fb_data* dst_col = dst;
+
+        unsigned data;
         int fg, bg;
         uintptr_t bo;
 
-        dst = dst_col;
-        dst_col += COL_INC;
-        row = height;
-
-#define UPDATE_SRC  do {                  \
-            data >>= 1;                   \
-            if (data == 0x001) {          \
-                src_col += stride;        \
-                data = *src_col ^ dmask;  \
-            }                             \
-        } while (0)
-
-        switch (drmode)
-        {
-          case DRMODE_COMPLEMENT:
-            do
-            {
-                if (data & 0x01)
-                    *dst = ~(*dst);
+        switch (drmode) {
+        case DRMODE_COMPLEMENT:
+            do {
+                data = (*src_col++ ^ dmask) >> src_y;
+                if(data & 0x01)
+                    *dst_col = ~(*dst_col);
 
-                dst += ROW_INC;
-                UPDATE_SRC;
-            }
-            while (--row);
+                dst_col += COL_INC;
+            } while(src_col != src_end);
             break;
 
-          case DRMODE_BG|DRMODE_INT_BD:
+        case DRMODE_BG|DRMODE_INT_BD:
             bo = lcd_backdrop_offset;
-            do
-            {
-                if (!(data & 0x01))
-                    *dst = *PTR_ADD(dst, bo);
+            do {
+                data = (*src_col++ ^ dmask) >> src_y;
+                if(!(data & 0x01))
+                    *dst_col = *PTR_ADD(dst_col, bo);
 
-                dst += ROW_INC;
-                UPDATE_SRC;
-            }
-            while (--row);
+                dst_col += COL_INC;
+            } while(src_col != src_end);
             break;
 
         case DRMODE_BG:
             bg = lcd_current_viewport->bg_pattern;
-            do
-            {
-                if (!(data & 0x01))
-                    *dst = bg;
+            do {
+                data = (*src_col++ ^ dmask) >> src_y;
+                if(!(data & 0x01))
+                    *dst_col = bg;
 
-                dst += ROW_INC;
-                UPDATE_SRC;
-            }
-            while (--row);
+                dst_col += COL_INC;
+            } while(src_col != src_end);
             break;
 
-          case DRMODE_FG:
+        case DRMODE_FG:
             fg = lcd_current_viewport->fg_pattern;
-            do
-            {
-                if (data & 0x01)
-                    *dst = fg;
+            do {
+                data = (*src_col++ ^ dmask) >> src_y;
+                if(data & 0x01)
+                    *dst_col = fg;
 
-                dst += ROW_INC;
-                UPDATE_SRC;
-            }
-            while (--row);
+                dst_col += COL_INC;
+            } while(src_col != src_end);
             break;
 
-          case DRMODE_SOLID|DRMODE_INT_BD:
+        case DRMODE_SOLID|DRMODE_INT_BD:
             fg = lcd_current_viewport->fg_pattern;
             bo = lcd_backdrop_offset;
-            do
-            {
-                *dst = (data & 0x01) ? fg
-                           : *PTR_ADD(dst, bo);
-                dst += ROW_INC;
-                UPDATE_SRC;
-            }
-            while (--row);
+            do {
+                data = (*src_col++ ^ dmask) >> src_y;
+                if(data & 0x01)
+                    *dst_col = fg;
+                else
+                    *dst_col = *PTR_ADD(dst_col, bo);
+
+                dst_col += COL_INC;
+            } while(src_col != src_end);
             break;
 
-          case DRMODE_SOLID:
+        case DRMODE_SOLID:
             fg = lcd_current_viewport->fg_pattern;
             bg = lcd_current_viewport->bg_pattern;
-            do
-            {
-                *dst = (data & 0x01) ? fg : bg;
-                dst += ROW_INC;
-                UPDATE_SRC;
-            }
-            while (--row);
+            do {
+                data = (*src_col++ ^ dmask) >> src_y;
+                if(data & 0x01)
+                    *dst_col = fg;
+                else
+                    *dst_col = bg;
+
+                dst_col += COL_INC;
+            } while(src_col != src_end);
             break;
         }
+
+        src_y = (src_y + 1) & 7;
+        if(src_y == 0)
+            src += stride;
+
+        dst += ROW_INC;
+        height--;
     }
-    while (src < src_end);
 }
+
 /* Draw a full monochrome bitmap */
 void lcd_mono_bitmap(const unsigned char *src, int x, int y, int width, int height)
 {