LCD core move buf ptr and address look up function viewport struct

I'm currently running up against the limitations of the lcd_draw functions
I want these functions to be able to be used on any size buffer not
just buffers with a stride matching the underlying device

[DONE] allow the framebuffer to be decoupled from the device framebuffer
[DONE need examples] allow for some simple blit like transformations
[DONE] remove the device framebuffer from the plugin api
[DONE}ditto remote framebuffer
[DONE] remove _viewport_get_framebuffer you can call struct *vp = lcd_set_viewport(NULL) and vp->buffer->fb_ptr

while remote lcds may compile (and work in the sim) its not been tested on targets

[FIXED] backdrops need work to be screen agnostic

[FIXED] screen statusbar is not being combined into the main viewport correctly yet

[FIXED] screen elements are displayed incorrectly  after switch to void*

[FIXED] core didn't restore proper viewport on splash etc.

[NEEDS TESTING] remote lcd garbled data

[FIXED] osd lib garbled screen on bmp_part

[FIXED] grey_set_vp needs to return old viewport like lcd_set_viewport

[FIXED] Viewport update now handles viewports with differing buffers/strides by copying to the main buffer

[FIXED] splash on top of WPS leaves old framebuffer data (doesn't redraw)
[UPDATE] refined this a bit more to have clear_viewport set the clean bit and have skin_render do its own screen clear
scrolling viewports no longer trigger wps refresh
also fixed a bug where guisyncyesno was displaying and then disappearing

[ADDED!] New LCD macros that allow you to create properly size frame buffers in you desired size without wasting bytes
(LCD_ and LCD_REMOTE_)
LCD_STRIDE(w, h) same as STRIDE_MAIN
LCD_FBSTRIDE(w, h) returns target specific stride for a buffer W x H
LCD_NBELEMS(w, h) returns the number of fb_data sized elemenst needed for a buffer W x H
LCD_NATIVE_STRIDE(s) conversion between rockbox native vertical and lcd native stride (2bitH)
test_viewports.c has an example of usage

[FIXED!!] 2bit targets don't respect non-native strides
[FIXED] Few define snags

Change-Id: I0d04c3834e464eca84a5a715743a297a0cefd0af

1 files changed, 124 insertions, 91 deletions

diff --git a/firmware/drivers/lcd-2bit-horz.c b/firmware/drivers/lcd-2bit-horz.c
index 1d256e5f58..35a2be5b2c 100644
--- a/firmware/drivers/lcd-2bit-horz.c
+++ b/firmware/drivers/lcd-2bit-horz.c
@@ -39,8 +39,8 @@
 
 /*** globals ***/
 
-unsigned char lcd_static_framebuffer[LCD_FBHEIGHT][LCD_FBWIDTH] IRAM_LCDFRAMEBUFFER;
-unsigned char *lcd_framebuffer = &lcd_static_framebuffer[0][0];
+static unsigned char lcd_static_framebuffer[LCD_FBHEIGHT][LCD_FBWIDTH] IRAM_LCDFRAMEBUFFER;
+static void *lcd_frameaddress_default(int x, int y);
 
 static const unsigned char pixmask[4] ICONST_ATTR = {
     0xC0, 0x30, 0x0C, 0x03
@@ -49,6 +49,15 @@ static const unsigned char pixmask[4] ICONST_ATTR = {
 static fb_data* lcd_backdrop = NULL;
 static long lcd_backdrop_offset IDATA_ATTR = 0;
 
+/* shouldn't be changed unless you want system-wide framebuffer changes! */
+struct frame_buffer_t lcd_framebuffer_default =
+{
+    .fb_ptr         = &lcd_static_framebuffer[0][0],
+    .get_address_fn = &lcd_frameaddress_default,
+    .stride         = STRIDE_MAIN(LCD_WIDTH, LCD_HEIGHT),
+    .elems          = (LCD_FBWIDTH*LCD_FBHEIGHT),
+};
+
 static struct viewport default_vp =
 {
     .x        = 0,
@@ -57,18 +66,32 @@ static struct viewport default_vp =
     .height   = LCD_HEIGHT,
     .font     = FONT_SYSFIXED,
     .drawmode = DRMODE_SOLID,
+    .buffer   = NULL,
     .fg_pattern = LCD_DEFAULT_FG,
     .bg_pattern = LCD_DEFAULT_BG
 };
 
-static struct viewport* current_vp IBSS_ATTR;
+struct viewport* lcd_current_viewport IBSS_ATTR;
 static unsigned fg_pattern IBSS_ATTR;
 static unsigned bg_pattern IBSS_ATTR;
 
+static void *lcd_frameaddress_default(int x, int y)
+{
+    /* the default expects a buffer the same size as the screen */
+    struct frame_buffer_t *fb = lcd_current_viewport->buffer;
+
+#if defined(LCD_STRIDEFORMAT) && LCD_STRIDEFORMAT == VERTICAL_STRIDE
+    size_t element = (x * LCD_NATIVE_STRIDE(fb->stride)) + y;
+#else
+    size_t element = (y * LCD_NATIVE_STRIDE(fb->stride)) + x;
+#endif
+    return fb->fb_ptr + element;/*(element % fb->elems);*/
+}
+
 /* LCD init */
 void lcd_init(void)
 {
-    /* Initialise the viewport */
+    /* Initialize the viewport */
     lcd_set_viewport(NULL);
 
     lcd_clear_display();
@@ -81,34 +104,34 @@ void lcd_init(void)
 
 void lcd_set_drawmode(int mode)
 {
-    current_vp->drawmode = mode & (DRMODE_SOLID|DRMODE_INVERSEVID);
+    lcd_current_viewport->drawmode = mode & (DRMODE_SOLID|DRMODE_INVERSEVID);
 }
 
 int lcd_get_drawmode(void)
 {
-    return current_vp->drawmode;
+    return lcd_current_viewport->drawmode;
 }
 
 void lcd_set_foreground(unsigned brightness)
 {
-    current_vp->fg_pattern = brightness;
+    lcd_current_viewport->fg_pattern = brightness;
     fg_pattern = 0x55 * (~brightness & 3);
 }
 
 unsigned lcd_get_foreground(void)
 {
-    return current_vp->fg_pattern;
+    return lcd_current_viewport->fg_pattern;
 }
 
 void lcd_set_background(unsigned brightness)
 {
-    current_vp->bg_pattern = brightness;
+    lcd_current_viewport->bg_pattern = brightness;
     bg_pattern = 0x55 * (~brightness & 3);
 }
 
 unsigned lcd_get_background(void)
 {
-    return current_vp->bg_pattern;
+    return lcd_current_viewport->bg_pattern;
 }
 
 void lcd_set_drawinfo(int mode, unsigned fg_brightness, unsigned bg_brightness)
@@ -120,27 +143,27 @@ void lcd_set_drawinfo(int mode, unsigned fg_brightness, unsigned bg_brightness)
 
 int lcd_getwidth(void)
 {
-    return current_vp->width;
+    return lcd_current_viewport->width;
 }
 
 int lcd_getheight(void)
 {
-    return current_vp->height;
+    return lcd_current_viewport->height;
 }
 
 void lcd_setfont(int newfont)
 {
-    current_vp->font = newfont;
+    lcd_current_viewport->font = newfont;
 }
 
 int lcd_getfont(void)
 {
-    return current_vp->font;
+    return lcd_current_viewport->font;
 }
 
 int lcd_getstringsize(const unsigned char *str, int *w, int *h)
 {
-    return font_getstringsize(str, w, h, current_vp->font);
+    return font_getstringsize(str, w, h, lcd_current_viewport->font);
 }
 
 /*** low-level drawing functions ***/
@@ -318,7 +341,7 @@ void lcd_set_backdrop(fb_data* backdrop)
     lcd_backdrop = backdrop;
     if (backdrop)
     {
-        lcd_backdrop_offset = (long)backdrop - (long)lcd_framebuffer;
+        lcd_backdrop_offset = (long)backdrop - (long)FBADDR(0,0);
         lcd_pixelfuncs = lcd_pixelfuncs_backdrop;
         lcd_blockfuncs = lcd_blockfuncs_backdrop;
     }
@@ -349,16 +372,16 @@ static inline void setblock(fb_data *address, unsigned mask, unsigned bits)
 /* Clear the whole display */
 void lcd_clear_display(void)
 {
-    if (current_vp->drawmode & DRMODE_INVERSEVID)
+    if (lcd_current_viewport->drawmode & DRMODE_INVERSEVID)
     {
-        memset(lcd_framebuffer, fg_pattern, FRAMEBUFFER_SIZE);
+        memset(FBADDR(0,0), fg_pattern, FRAMEBUFFER_SIZE);
     }
     else
     {
         if (lcd_backdrop)
-            memcpy(lcd_framebuffer, lcd_backdrop, FRAMEBUFFER_SIZE);
+            memcpy(FBADDR(0,0), lcd_backdrop, FRAMEBUFFER_SIZE);
         else
-            memset(lcd_framebuffer, bg_pattern, FRAMEBUFFER_SIZE);
+            memset(FBADDR(0,0), bg_pattern, FRAMEBUFFER_SIZE);
     }
 
     lcd_scroll_info.lines = 0;
@@ -369,37 +392,39 @@ void lcd_clear_viewport(void)
 {
     int lastmode;
 
-    if (current_vp == &default_vp)
+    if (lcd_current_viewport == &default_vp &&
+           default_vp.buffer == &lcd_framebuffer_default)
     {
         lcd_clear_display();
     }
     else
     {
-        lastmode = current_vp->drawmode;
+        lastmode = lcd_current_viewport->drawmode;
 
         /* Invert the INVERSEVID bit and set basic mode to SOLID */
-        current_vp->drawmode = (~lastmode & DRMODE_INVERSEVID) | 
+        lcd_current_viewport->drawmode = (~lastmode & DRMODE_INVERSEVID) | 
                                DRMODE_SOLID;
 
-        lcd_fillrect(0, 0, current_vp->width, current_vp->height);
+        lcd_fillrect(0, 0, lcd_current_viewport->width, lcd_current_viewport->height);
 
-        current_vp->drawmode = lastmode;
+        lcd_current_viewport->drawmode = lastmode;
 
-        lcd_scroll_stop_viewport(current_vp);
+        lcd_scroll_stop_viewport(lcd_current_viewport);
     }
+    lcd_current_viewport->flags &= ~(VP_FLAG_VP_SET_CLEAN);
 }
 
 /* Set a single pixel */
 void lcd_drawpixel(int x, int y)
 {
-    if (   ((unsigned)x < (unsigned)current_vp->width)
-        && ((unsigned)y < (unsigned)current_vp->height)
+    if (   ((unsigned)x < (unsigned)lcd_current_viewport->width)
+        && ((unsigned)y < (unsigned)lcd_current_viewport->height)
 #if defined(HAVE_VIEWPORT_CLIP)
         && ((unsigned)x < (unsigned)LCD_WIDTH)
         && ((unsigned)y < (unsigned)LCD_HEIGHT)
 #endif
         )
-        lcd_pixelfuncs[current_vp->drawmode](current_vp->x + x, current_vp->y + y);
+        lcd_pixelfuncs[lcd_current_viewport->drawmode](lcd_current_viewport->x + x, lcd_current_viewport->y + y);
 }
 
 /* Draw a line */
@@ -411,7 +436,7 @@ void lcd_drawline(int x1, int y1, int x2, int y2)
     int d, dinc1, dinc2;
     int x, xinc1, xinc2;
     int y, yinc1, yinc2;
-    lcd_pixelfunc_type *pfunc = lcd_pixelfuncs[current_vp->drawmode];
+    lcd_pixelfunc_type *pfunc = lcd_pixelfuncs[lcd_current_viewport->drawmode];
 
     deltay = abs(y2 - y1);
     if (deltay == 0)
@@ -467,14 +492,14 @@ void lcd_drawline(int x1, int y1, int x2, int y2)
 
     for (i = 0; i < numpixels; i++)
     {
-        if (   ((unsigned)x < (unsigned)current_vp->width) 
-            && ((unsigned)y < (unsigned)current_vp->height)
+        if (   ((unsigned)x < (unsigned)lcd_current_viewport->width) 
+            && ((unsigned)y < (unsigned)lcd_current_viewport->height)
 #if defined(HAVE_VIEWPORT_CLIP)
             && ((unsigned)x < (unsigned)LCD_WIDTH)
             && ((unsigned)y < (unsigned)LCD_HEIGHT)
 #endif
             )
-            pfunc(current_vp->x + x, current_vp->y + y);
+            pfunc(lcd_current_viewport->x + x, lcd_current_viewport->y + y);
 
         if (d < 0)
         {
@@ -509,19 +534,19 @@ void lcd_hline(int x1, int x2, int y)
     
     /******************** In viewport clipping **********************/
     /* nothing to draw? */
-    if (((unsigned)y >= (unsigned)current_vp->height) || (x1 >= current_vp->width) 
+    if (((unsigned)y >= (unsigned)lcd_current_viewport->height) || (x1 >= lcd_current_viewport->width) 
         || (x2 < 0))
         return;  
         
     if (x1 < 0)
         x1 = 0;
-    if (x2 >= current_vp->width)
-        x2 = current_vp->width-1;
+    if (x2 >= lcd_current_viewport->width)
+        x2 = lcd_current_viewport->width-1;
 
     /* adjust to viewport */
-    x1 += current_vp->x;
-    x2 += current_vp->x;
-    y += current_vp->y;
+    x1 += lcd_current_viewport->x;
+    x2 += lcd_current_viewport->x;
+    y += lcd_current_viewport->y;
     
 #if defined(HAVE_VIEWPORT_CLIP)
     /********************* Viewport on screen clipping ********************/
@@ -537,7 +562,7 @@ void lcd_hline(int x1, int x2, int y)
         x2 = LCD_WIDTH-1;
 #endif
 
-    bfunc = lcd_blockfuncs[current_vp->drawmode];
+    bfunc = lcd_blockfuncs[lcd_current_viewport->drawmode];
     dst   = FBADDR(x1>>2,y);
     nx    = x2 - (x1 & ~3);
     mask  = 0xFFu >> (2 * (x1 & 3));
@@ -557,6 +582,7 @@ void lcd_vline(int x, int y1, int y2)
 {
     int y;
     unsigned char *dst, *dst_end;
+    int stride_dst;
     unsigned mask;
     lcd_blockfunc_type *bfunc;
 
@@ -570,19 +596,19 @@ void lcd_vline(int x, int y1, int y2)
 
     /******************** In viewport clipping **********************/
     /* nothing to draw? */
-    if (((unsigned)x >= (unsigned)current_vp->width) || (y1 >= current_vp->height)
+    if (((unsigned)x >= (unsigned)lcd_current_viewport->width) || (y1 >= lcd_current_viewport->height)
         || (y2 < 0))
         return;  
         
     if (y1 < 0)
         y1 = 0;
-    if (y2 >= current_vp->height)
-        y2 = current_vp->height-1;
+    if (y2 >= lcd_current_viewport->height)
+        y2 = lcd_current_viewport->height-1;
 
     /* adjust for viewport */
-    y1 += current_vp->y;
-    y2 += current_vp->y;
-    x += current_vp->x;
+    y1 += lcd_current_viewport->y;
+    y2 += lcd_current_viewport->y;
+    x += lcd_current_viewport->x;
     
 #if defined(HAVE_VIEWPORT_CLIP)
     /********************* Viewport on screen clipping ********************/
@@ -598,15 +624,16 @@ void lcd_vline(int x, int y1, int y2)
         y2 = LCD_HEIGHT-1;
 #endif
         
-    bfunc = lcd_blockfuncs[current_vp->drawmode];
+    bfunc = lcd_blockfuncs[lcd_current_viewport->drawmode];
     dst   = FBADDR(x>>2,y1);
+    stride_dst = LCD_FBSTRIDE(lcd_current_viewport->buffer->stride, 0);
     mask  = pixmask[x & 3];
     
-    dst_end = dst + (y2 - y1) * LCD_FBWIDTH;
+    dst_end = dst + (y2 - y1) * stride_dst;
     do
     {
         bfunc(dst, mask, 0xFFu);
-        dst += LCD_FBWIDTH;
+        dst += stride_dst;
     }
     while (dst <= dst_end);
 }
@@ -631,12 +658,13 @@ void lcd_fillrect(int x, int y, int width, int height)
 {
     int nx;
     unsigned char *dst, *dst_end;
+    int stride_dst;
     unsigned mask, mask_right;
     lcd_blockfunc_type *bfunc;
 
     /******************** In viewport clipping **********************/
     /* nothing to draw? */
-    if ((width <= 0) || (height <= 0) || (x >= current_vp->width) || (y >= current_vp->height)
+    if ((width <= 0) || (height <= 0) || (x >= lcd_current_viewport->width) || (y >= lcd_current_viewport->height)
         || (x + width <= 0) || (y + height <= 0))
         return;
 
@@ -650,14 +678,14 @@ void lcd_fillrect(int x, int y, int width, int height)
         height += y;
         y = 0;
     }
-    if (x + width > current_vp->width)
-        width = current_vp->width - x;
-    if (y + height > current_vp->height)
-        height = current_vp->height - y;
+    if (x + width > lcd_current_viewport->width)
+        width = lcd_current_viewport->width - x;
+    if (y + height > lcd_current_viewport->height)
+        height = lcd_current_viewport->height - y;
 
     /* adjust for viewport */
-    x += current_vp->x;
-    y += current_vp->y;
+    x += lcd_current_viewport->x;
+    y += lcd_current_viewport->y;
     
 #if defined(HAVE_VIEWPORT_CLIP)
     /********************* Viewport on screen clipping ********************/
@@ -683,21 +711,22 @@ void lcd_fillrect(int x, int y, int width, int height)
         height = LCD_HEIGHT - y;
 #endif
 
-    bfunc = lcd_blockfuncs[current_vp->drawmode];
-    dst   = FBADDR(x>>2,y);
-    nx    = width - 1 + (x & 3);
-    mask  = 0xFFu >> (2 * (x & 3));
+    bfunc      = lcd_blockfuncs[lcd_current_viewport->drawmode];
+    dst        = FBADDR(x>>2,y);
+    stride_dst = LCD_FBSTRIDE(lcd_current_viewport->buffer->stride, 0);
+    nx         = width - 1 + (x & 3);
+    mask       = 0xFFu >> (2 * (x & 3));
     mask_right = 0xFFu << (2 * (~nx & 3));
     
     for (; nx >= 4; nx -= 4)
     {
         unsigned char *dst_col = dst;
 
-        dst_end = dst_col + height * LCD_FBWIDTH;
+        dst_end = dst_col + height * stride_dst;
         do
         {
             bfunc(dst_col, mask, 0xFFu);
-            dst_col += LCD_FBWIDTH;
+            dst_col += stride_dst;
         }
         while (dst_col < dst_end);
 
@@ -706,11 +735,11 @@ void lcd_fillrect(int x, int y, int width, int height)
     }
     mask &= mask_right;
 
-    dst_end = dst + height * LCD_FBWIDTH;
+    dst_end = dst + height * stride_dst;
     do
     {
         bfunc(dst, mask, 0xFFu);
-        dst += LCD_FBWIDTH;
+        dst += stride_dst;
     }
     while (dst < dst_end);
 }
@@ -731,14 +760,15 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
 {
     const unsigned char *src_end;
     fb_data *dst, *dst_end;
+    int stride_dst;
     unsigned dmask = 0x100; /* bit 8 == sentinel */
     unsigned dst_mask;
-    int drmode = current_vp->drawmode;
+    int drmode = lcd_current_viewport->drawmode;
 
     /******************** Image in viewport clipping **********************/
     /* nothing to draw? */
-    if ((width <= 0) || (height <= 0) || (x >= current_vp->width) ||
-        (y >= current_vp->height) || (x + width <= 0) || (y + height <= 0))
+    if ((width <= 0) || (height <= 0) || (x >= lcd_current_viewport->width) ||
+        (y >= lcd_current_viewport->height) || (x + width <= 0) || (y + height <= 0))
         return;
         
     if (x < 0)
@@ -753,13 +783,13 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
         src_y -= y;
         y = 0;
     }
-    if (x + width > current_vp->width)
-        width = current_vp->width - x;
-    if (y + height > current_vp->height)
-        height = current_vp->height - y;
+    if (x + width > lcd_current_viewport->width)
+        width = lcd_current_viewport->width - x;
+    if (y + height > lcd_current_viewport->height)
+        height = lcd_current_viewport->height - y;
         
-    x += current_vp->x;          /* adjust for viewport */
-    y += current_vp->y;          /* adjust for viewport */
+    x += lcd_current_viewport->x;          /* adjust for viewport */
+    y += lcd_current_viewport->y;          /* adjust for viewport */
     
 #if defined(HAVE_VIEWPORT_CLIP)
     /********************* Viewport on screen clipping ********************/
@@ -792,7 +822,8 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
     src_end = src + width;
     
     dst = FBADDR(x >> 2,y);
-    dst_end = dst + height * LCD_FBWIDTH;
+    stride_dst = LCD_FBSTRIDE(lcd_current_viewport->buffer->stride, 0);
+    dst_end = dst + height * stride_dst;
     dst_mask = pixmask[x & 3];
 
     if (drmode & DRMODE_INVERSEVID)
@@ -825,7 +856,7 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
                 if (data & 0x01)
                     *dst_col ^= dst_mask;
 
-                dst_col += LCD_FBWIDTH;
+                dst_col += stride_dst;
                 UPDATE_SRC;
             }
             while (dst_col < dst_end);
@@ -843,7 +874,7 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
                         *dst_col = block
                                  ^ ((block ^ *(dst_col + bo)) & dst_mask);
                     }
-                    dst_col += LCD_FBWIDTH;
+                    dst_col += stride_dst;
                     UPDATE_SRC;
                 }
                 while (dst_col < dst_end);
@@ -858,7 +889,7 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
                         unsigned block = *dst_col;
                         *dst_col = block ^ ((block ^ bg) & dst_mask);
                     }
-                    dst_col += LCD_FBWIDTH;
+                    dst_col += stride_dst;
                     UPDATE_SRC;
                 }
                 while (dst_col < dst_end);
@@ -874,7 +905,7 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
                     unsigned block = *dst_col;
                     *dst_col = block ^ ((block ^ fg) & dst_mask);
                 }
-                dst_col += LCD_FBWIDTH;
+                dst_col += stride_dst;
                 UPDATE_SRC;
             }
             while (dst_col < dst_end);
@@ -891,7 +922,7 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
                     *dst_col = block ^ ((block ^ ((data & 0x01) ? 
                                fg : *(dst_col + bo))) & dst_mask);
 
-                    dst_col += LCD_FBWIDTH;
+                    dst_col += stride_dst;
                     UPDATE_SRC;
                 }
                 while (dst_col < dst_end);
@@ -905,7 +936,7 @@ void ICODE_ATTR lcd_mono_bitmap_part(const unsigned char *src, int src_x,
                     *dst_col = block ^ ((block ^ ((data & 0x01) ?
                                fg : bg)) & dst_mask);
 
-                    dst_col += LCD_FBWIDTH;
+                    dst_col += stride_dst;
                     UPDATE_SRC;
                 }
                 while (dst_col < dst_end);
@@ -945,12 +976,13 @@ void ICODE_ATTR lcd_bitmap_part(const unsigned char *src, int src_x,
 {
     int  shift, nx;
     unsigned char *dst, *dst_end;
+    int stride_dst;
     unsigned mask, mask_right;
 
     /******************** Image in viewport clipping **********************/
     /* nothing to draw? */
-    if ((width <= 0) || (height <= 0) || (x >= current_vp->width) || 
-        (y >= current_vp->height) || (x + width <= 0) || (y + height <= 0))
+    if ((width <= 0) || (height <= 0) || (x >= lcd_current_viewport->width) || 
+        (y >= lcd_current_viewport->height) || (x + width <= 0) || (y + height <= 0))
         return;
         
     if (x < 0)
@@ -965,14 +997,14 @@ void ICODE_ATTR lcd_bitmap_part(const unsigned char *src, int src_x,
         src_y -= y;
         y = 0;
     }
-    if (x + width > current_vp->width)
-        width = current_vp->width - x;
-    if (y + height > current_vp->height)
-        height = current_vp->height - y;
+    if (x + width > lcd_current_viewport->width)
+        width = lcd_current_viewport->width - x;
+    if (y + height > lcd_current_viewport->height)
+        height = lcd_current_viewport->height - y;
 
     /* adjust for viewport */
-    x += current_vp->x;
-    y += current_vp->y;
+    x += lcd_current_viewport->x;
+    y += lcd_current_viewport->y;
     
 #if defined(HAVE_VIEWPORT_CLIP)
     /********************* Viewport on screen clipping ********************/
@@ -1000,12 +1032,13 @@ void ICODE_ATTR lcd_bitmap_part(const unsigned char *src, int src_x,
         height = LCD_HEIGHT - y;
 #endif
 
-    stride = (stride + 3) >> 2; /* convert to no. of bytes */
+    stride = LCD_FBSTRIDE(stride, 0); /* convert to no. of bytes */
 
     src   += stride * src_y + (src_x >> 2); /* move starting point */
     src_x &= 3;
     x     -= src_x;
     dst   = FBADDR(x>>2,y);
+    stride_dst = LCD_FBSTRIDE(lcd_current_viewport->buffer->stride, 0);
     shift = x & 3;
     nx    = width - 1 + shift + src_x;
 
@@ -1013,7 +1046,7 @@ void ICODE_ATTR lcd_bitmap_part(const unsigned char *src, int src_x,
     mask_right = 0xFFu << (2 * (~nx & 3));
     
     shift *= 2;
-    dst_end = dst + height * LCD_FBWIDTH;
+    dst_end = dst + height * stride_dst;
     do
     {
         const unsigned char *src_row = src;
@@ -1039,7 +1072,7 @@ void ICODE_ATTR lcd_bitmap_part(const unsigned char *src, int src_x,
         setblock(dst_row, mask_row & mask_right, data >> shift);
 
         src += stride;
-        dst += LCD_FBWIDTH;
+        dst += stride_dst;
     }
     while (dst < dst_end);
 }