Submit FS#11843 v17. Integrate YUV-blitting of nano 2G to nano1G/color LCD driver. Additionally refactor RGB and YUV screen updates to use same code fragments and save some binsize. YUV speedup is +3-4%, RGB 1/4 screen +2%.

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

2 files changed, 334 insertions, 273 deletions

diff --git a/firmware/target/arm/ipod/lcd-as-color-nano.S b/firmware/target/arm/ipod/lcd-as-color-nano.S
index d4df4d496a..f6f9cc5be3 100755
--- a/firmware/target/arm/ipod/lcd-as-color-nano.S
+++ b/firmware/target/arm/ipod/lcd-as-color-nano.S
@@ -7,7 +7,7 @@
  *                     \/            \/     \/    \/            \/
  * $Id:$
  *
- * Copyright (C) 2010 by Andree Buschmann
+ * Copyright (C) 2010-2011 by Andree Buschmann
  *
  * Generic asm helper function used by YUV blitting.
  *
@@ -24,129 +24,264 @@
 #include "config.h"
 #include "cpu.h"
 
-    .section    .icode, "ax", %progbits
+/**************************************************************************** 
+ * #define FORCE_FIFO_WAIT
+ *
+ * This is not needed in YUV blitting when the LCD IF is fast enough. In this
+ * case YUV-to-RGB conversion per pixel needs longer than the transfer of a 
+ * pixel via the LCD IF.
+ ****************************************************************************/
+
+#include "config.h"
+
+/* Set FIFO wait for both iPod Color and iPod nano1G until we know for which
+ * devices we can switch this off. */
+#define FORCE_FIFO_WAIT
 
+    .section .icode, "ax", %progbits
+    
 /****************************************************************************
-*  void lcd_yuv_write_inner_loop(unsigned char const * const ysrc,
-*                                unsigned char const * const usrc,
-*                                unsigned char const * const vsrc,
-*                                int width);
-*
-*   YUV- > RGB565 conversion
-*   |R|   |1.000000 -0.000001  1.402000| |Y'|
-*   |G| = |1.000000 -0.334136 -0.714136| |Pb|
-*   |B|   |1.000000  1.772000  0.000000| |Pr|
-*   Scaled, normalized, rounded and tweaked to yield RGB 565:
-*   |R|   |74   0 101| |Y' -  16| >> 9
-*   |G| = |74 -24 -51| |Cb - 128| >> 8
-*   |B|   |74 128   0| |Cr - 128| >> 9
-*
-*/
+ * extern void lcd_write_yuv420_lines(unsigned char const * const src[3],
+ *                                    const unsigned LCD_BASE,
+ *                                    int width,
+ *                                    int stride);
+ *
+ *   Conversion from Motion JPEG and MPEG Y'PbPr to RGB is:
+ *   |R|   |1.164  0.000  1.596| |Y' -  16|
+ *   |G| = |1.164 -0.391 -0.813| |Pb - 128|
+ *   |B|   |1.164  2.018  0.000| |Pr - 128|
+ *
+ *   Scaled, normalized, rounded and tweaked to yield RGB 565:
+ *   |R|   |74   0 101| |Y' -  16| >> 9
+ *   |G| = |74 -24 -51| |Cb - 128| >> 8
+ *   |B|   |74 128   0| |Cr - 128| >> 9
+ *
+ * Converts two lines from YUV to RGB565 and writes to LCD at once. First loop
+ * loads Cb/Cr, calculates the chroma offset and saves them to buffer. Within
+ * the second loop these chroma offset are reloaded from buffer. Within each 
+ * loop two pixels are calculated and written to LCD. 
+ */
     .align      2
-    .global     lcd_yuv_write_inner_loop
-    .type       lcd_yuv_write_inner_loop, %function
-
-lcd_yuv_write_inner_loop:
-                                        @ r0 = ysrc
-                                        @ r1 = usrc
-                                        @ r2 = vsrc
-                                        @ r3 = width
-    stmfd sp!, { r4-r11, lr }           @ save regs
-    mov r4, #0x70000000                 @ r4 = LCD2_BLOCK_CTRL - 0x20
-    add r4, r4, #0x8a00                 @
-    add r5, r4, #0x100                  @ r5 = LCD2_BLOCK_DATA
-10:                                     @ loop
-
-    ldrb r7, [r1], #1                   @ *usrc++
-    ldrb r8, [r2], #1                   @ *vsrc++
-
-    sub r7, r7, #128                    @ Cb -= 128
-    sub r8, r8, #128                    @ Cr -= 128
-
-    add r10, r8, r8, asl #2             @ Cr*101
-    add r10, r10, r8, asl #5
-    add r10, r10, r8, asl #6
-
-    add r11, r8, r8, asl #1             @ Cr*51 + Cb*24
-    add r11, r11, r11, asl #4
-    add r11, r11, r7, asl #3
-    add r11, r11, r7, asl #4
-
-    add r12, r7, #2                     @ r12 = bu = (Cb*128 + 256) >> 9
-    mov r12, r12, asr #2
-    add r10, r10, #256                  @ r10 = rv = (Cr*101 + 256) >> 9
-    mov r10, r10, asr #9
-    rsb r11, r11, #128                  @ r11 = guv = (-r11 + 128) >> 8
-    mov r11, r11, asr #8
-
-@ pixel_1
-    ldrb r7, [r0], #1                   @ *ysrc++
-    sub r7, r7, #16                     @ Y = (Y' - 16) * 37
-    add r8, r7, r7, asl #2
-    add r7, r8, r7, asl #5
-
-    add r9, r10, r7, asr #8             @ R = (Y >> 8) + rv
-    add r8, r11, r7, asr #7             @ G = (Y >> 7) + guv
-    add r7, r12, r7, asr #8             @ B = (Y >> 8) + bu
-
-    cmp r9, #31                         @ clamp R
-    mvnhi r9, r9, asr #31
-    andhi r9, r9, #31
-
-    cmp r8, #63                         @ clamp G
-    mvnhi r8, r8, asr #31
-    andhi r8, r8, #63
-
-    cmp r7, #31                         @ clamp B
-    mvnhi r7, r7, asr #31
-    andhi r7, r7, #31
-
-    orr r6, r7, r8, lsl #5              @ pack pixel
-    orr r6, r6, r9, lsl #11
-
-    mov r7, r6, lsl #8                  @ swap bytes
-    and r7, r7, #0xff00
-    add r6, r7, r6, lsr #8
-
-@ pixel_2
-    ldrb r7, [r0], #1                   @ *ysrc++
-    sub r7, r7, #16                     @ Y = (Y' - 16) * 37
-    add r8, r7, r7, asl #2
-    add r7, r8, r7, asl #5
-
-    add r9, r10, r7, asr #8             @ R = (Y >> 8) + rv
-    add r8, r11, r7, asr #7             @ G = (Y >> 7) + guv
-    add r7, r12, r7, asr #8             @ B = (Y >> 8) + bu
-
-    cmp r9, #31                         @ clamp R
-    mvnhi r9, r9, asr #31
-    andhi r9, r9, #31
-
-    cmp r8, #63                         @ clamp G
-    mvnhi r8, r8, asr #31
-    andhi r8, r8, #63
-
-    cmp r7, #31                         @ clamp B
-    mvnhi r7, r7, asr #31
-    andhi r7, r7, #31
-
-    orr r7, r7, r8, lsl #5              @ pack pixel
-    orr r7, r7, r9, lsl #11
-
-    orr r6, r6, r7, lsl #24             @ swap bytes and add pixels simultaneously
-    mov r7, r7, lsr #8
-    orr r6, r6, r7, lsl #16
-#if 1
-11:                                     @ while (!(LCD2_BLOCK_CTRL & LCD2_BLOCK_TXOK));
-    ldr r11, [r4, #0x20]                @
-    tst r11, #0x1000000                 @
-    beq 11b                             @
+    .global     lcd_write_yuv420_lines
+    .type       lcd_write_yuv420_lines, %function
+lcd_write_yuv420_lines:
+                                      /* r0 = src = yuv_src */
+                                      /* r1 = dst = LCD_BASE */
+                                      /* r2 = width */
+                                      /* r3 = stride */                
+    stmfd       sp!, { r4-r10, lr }   /* save non-scratch */
+    ldmia       r0, { r9, r10, r12 }  /* r9 = yuv_src[0] = Y'_p */
+                                      /* r10 = yuv_src[1] = Cb_p */
+                                      /* r12 = yuv_src[2] = Cr_p */
+    add         r3, r9, r3            /* r3 = &ysrc[stride] */
+    add         r4, r2, r2, asr #1    /* chroma buffer lenght = width/2 *3 */
+    mov         r4, r4, asl #2        /*   use words for str/ldm possibility */
+    add         r4, r4, #19           /*   plus room for 4 additional words, */
+    bic         r4, r4, #3            /*   rounded up to multiples of 4 byte */
+    sub         sp, sp, r4            /*   and allocate on stack */
+    stmia       sp, {r1-r4}           /* LCD_BASE, width, &ysrc[stride], stack_alloc */
+
+    mov         r7, r2                /* r7 = loop count */
+    add         r8, sp, #16           /* chroma buffer */
+    add         lr, r1, #0x100        /* LCD data port = LCD2_BASE + 0x100 */
+
+    /* 1st loop start */
+10:                                   /* loop start */
+
+    ldrb        r0, [r10], #1         /* r0 = *usrc++ = *Cb_p++ */
+    ldrb        r1, [r12], #1         /* r1 = *vsrc++ = *Cr_p++ */
+
+    sub         r0, r0, #128          /* r0 = Cb-128 */
+    sub         r1, r1, #128          /* r1 = Cr-128 */
+
+    add         r2, r1, r1, asl #1    /* r2 = Cr*51 + Cb*24 */
+    add         r2, r2, r2, asl #4   
+    add         r2, r2, r0, asl #3   
+    add         r2, r2, r0, asl #4   
+
+    add         r4, r1, r1, asl #2    /* r1 = Cr*101 */
+    add         r4, r4, r1, asl #5
+    add         r1, r4, r1, asl #6
+
+    add         r1, r1, #256          /* r1 = rv = (r1 + 256) >> 9 */
+    mov         r1, r1, asr #9
+    rsb         r2, r2, #128          /* r2 = guv = (-r2 + 128) >> 8 */
+    mov         r2, r2, asr #8       
+    add         r0, r0, #2            /* r0 = bu = (Cb*128 + 256) >> 9 */
+    mov         r0, r0, asr #2       
+    stmia       r8!, {r0-r2}          /* store r0, r1 and r2 to chroma buffer */
+
+    /* 1st loop, first pixel */
+    ldrb        r5, [r9], #1          /* r5 = *ysrc++ = *Y'_p++ */
+    sub         r5, r5, #16           /* r5 = (Y'-16) * 74 */
+    add         r3, r5, r5, asl #2
+    add         r5, r3, r5, asl #5
+
+    add         r6, r1, r5, asr #8    /* r6 = r = (Y >> 9) + rv */
+    add         r3, r2, r5, asr #7    /* r3 = g = (Y >> 8) + guv */
+    add         r4, r0, r5, asr #8    /* r4 = b = (Y >> 9) + bu */
+
+    orr         r5, r6, r4            /* check if clamping is needed... */
+    orr         r5, r5, r3, asr #1    /* ...at all */
+    cmp         r5, #31                 
+    bls         15f                   /* -> no clamp */
+    cmp         r6, #31               /* clamp r */
+    mvnhi       r6, r6, asr #31         
+    andhi       r6, r6, #31             
+    cmp         r3, #63               /* clamp g */
+    mvnhi       r3, r3, asr #31
+    andhi       r3, r3, #63
+    cmp         r4, #31               /* clamp b */
+    mvnhi       r4, r4, asr #31         
+    andhi       r4, r4, #31          
+15:                                   /* no clamp */
+
+    /* calculate pixel_1 and save to r4 for later pixel packing */
+    orr         r4, r4, r3, lsl #5    /* pixel_1 = r<<11 | g<<5 | b */
+    orr         r4, r4, r6, lsl #11   /* r4 = pixel_1 */
+
+    /* 1st loop, second pixel */
+    ldrb        r5, [r9], #1          /* r5 = *ysrc++ = *Y'_p++ */
+    sub         r5, r5, #16           /* r5 = (Y'-16) * 74 */
+    add         r3, r5, r5, asl #2
+    add         r5, r3, r5, asl #5
+
+    add         r6, r1, r5, asr #8    /* r6 = r = (Y >> 9) + rv */
+    add         r3, r2, r5, asr #7    /* r3 = g = (Y >> 8) + guv */
+    add         r5, r0, r5, asr #8    /* r5 = b = (Y >> 9) + bu */   
+
+    orr         r0, r6, r5            /* check if clamping is needed... */
+    orr         r0, r0, r3, asr #1    /* ...at all */
+    cmp         r0, #31                 
+    bls         15f                   /* -> no clamp */
+    cmp         r6, #31               /* clamp r */
+    mvnhi       r6, r6, asr #31         
+    andhi       r6, r6, #31             
+    cmp         r3, #63               /* clamp g */
+    mvnhi       r3, r3, asr #31
+    andhi       r3, r3, #63
+    cmp         r5, #31               /* clamp b */
+    mvnhi       r5, r5, asr #31         
+    andhi       r5, r5, #31          
+15:                                   /* no clamp */
+
+    /* calculate pixel_2 and pack with pixel_1 before writing */
+    orr         r5, r5, r3, lsl #5    /* pixel_2 = r<<11 | g<<5 | b */
+    orr         r5, r5, r6, lsl #11   /* r5 = pixel_2 */
+#ifdef FORCE_FIFO_WAIT
+    /* wait for FIFO half full */
+.fifo_wait1:
+    ldr         r3, [lr, #-0xE0]      /* while !(LCD2_BLOCK_CTRL & 0x1000000); */
+    tst         r3, #0x1000000
+    beq         .fifo_wait1
 #endif
-    str r6, [r5]                        @ send two pixels
 
-    subs r3, r3, #2                     @ decrease width
-    bgt 10b                             @ loop
+    mov         r3, r4, lsl #8        /* swap pixel_1 */
+    and         r3, r3, #0xff00
+    add         r4, r3, r4, lsr #8
+    
+    orr         r4, r4, r5, lsl #24   /* swap pixel_2 and pack with pixel_1 */
+    mov         r5, r5, lsr #8
+    orr         r4, r4, r5, lsl #16
+
+    str         r4, [lr]              /* write pixel_1 and pixel_2 */
+
+    subs        r7, r7, #2            /* check for loop end */
+    bgt         10b                   /* back to beginning  */
+    /* 1st loop end */
+
+    /* Reload several registers for pointer rewinding for next loop */
+    add         r8, sp, #16           /* chroma buffer */
+    ldmia       sp, { r1, r7, r9}     /* r1  = LCD_BASE */
+                                      /* r7  = loop count */
+                                      /* r9 = &ysrc[stride] */   
+
+    /* 2nd loop start */
+20:                                   /* loop start */
+    /* restore r0 (bu), r1 (rv) and r2 (guv) from chroma buffer */
+    ldmia       r8!, {r0-r2}
+
+    /* 2nd loop, first pixel */
+    ldrb        r5, [r9], #1          /* r5 = *ysrc++ = *Y'_p++ */
+    sub         r5, r5, #16           /* r5 = (Y'-16) * 74 */
+    add         r3, r5, r5, asl #2
+    add         r5, r3, r5, asl #5
+
+    add         r6, r1, r5, asr #8    /* r6 = r = (Y >> 9) + rv */
+    add         r3, r2, r5, asr #7    /* r3 = g = (Y >> 8) + guv */
+    add         r4, r0, r5, asr #8    /* r4 = b = (Y >> 9) + bu */
+
+    orr         r5, r6, r4            /* check if clamping is needed... */
+    orr         r5, r5, r3, asr #1    /* ...at all */
+    cmp         r5, #31                 
+    bls         15f                   /* -> no clamp */
+    cmp         r6, #31               /* clamp r */
+    mvnhi       r6, r6, asr #31         
+    andhi       r6, r6, #31             
+    cmp         r3, #63               /* clamp g */
+    mvnhi       r3, r3, asr #31
+    andhi       r3, r3, #63
+    cmp         r4, #31               /* clamp b */
+    mvnhi       r4, r4, asr #31         
+    andhi       r4, r4, #31          
+15:                                   /* no clamp */
+    /* calculate pixel_1 and save to r4 for later pixel packing */
+    orr         r4, r4, r3, lsl #5    /* pixel_1 = r<<11 | g<<5 | b */
+    orr         r4, r4, r6, lsl #11   /* r4 = pixel_1 */
+
+    /* 2nd loop, second pixel */
+    ldrb        r5, [r9], #1          /* r5 = *ysrc++ = *Y'_p++ */
+    sub         r5, r5, #16           /* r5 = (Y'-16) * 74 */
+    add         r3, r5, r5, asl #2
+    add         r5, r3, r5, asl #5
+
+    add         r6, r1, r5, asr #8    /* r6 = r = (Y >> 9) + rv */
+    add         r3, r2, r5, asr #7    /* r3 = g = (Y >> 8) + guv */
+    add         r5, r0, r5, asr #8    /* r5 = b = (Y >> 9) + bu */
+
+    orr         r0, r6, r5            /* check if clamping is needed... */
+    orr         r0, r0, r3, asr #1    /* ...at all */
+    cmp         r0, #31                 
+    bls         15f                   /* -> no clamp */
+    cmp         r6, #31               /* clamp r */
+    mvnhi       r6, r6, asr #31         
+    andhi       r6, r6, #31             
+    cmp         r3, #63               /* clamp g */
+    mvnhi       r3, r3, asr #31
+    andhi       r3, r3, #63
+    cmp         r5, #31               /* clamp b */
+    mvnhi       r5, r5, asr #31         
+    andhi       r5, r5, #31          
+15:                                   /* no clamp */
+
+    /* calculate pixel_2 and pack with pixel_1 before writing */
+    orr         r5, r5, r3, lsl #5    /* pixel_2 = r<<11 | g<<5 | b */
+    orr         r5, r5, r6, lsl #11   /* r5 = pixel_2 */
+#ifdef FORCE_FIFO_WAIT
+    /* wait for FIFO half full */
+.fifo_wait2:
+    ldr         r3, [lr, #-0xE0]      /* while !(LCD2_BLOCK_CTRL & 0x1000000); */
+    tst         r3, #0x1000000
+    beq         .fifo_wait2
+#endif
+
+    mov         r3, r4, lsl #8        /* swap pixel_1 */
+    and         r3, r3, #0xff00
+    add         r4, r3, r4, lsr #8
+    
+    orr         r4, r4, r5, lsl #24   /* swap pixel_2 and pack with pixel_1 */
+    mov         r5, r5, lsr #8
+    orr         r4, r4, r5, lsl #16
+    
+    str         r4, [lr]              /* write pixel_1 and pixel_2 */
+
+    subs        r7, r7, #2            /* check for loop end */
+    bgt         20b                   /* back to beginning  */
+    /* 2nd loop end */
+
+    ldr         r3, [sp, #12]
+    add         sp, sp, r3            /* deallocate buffer */
+    ldmpc       regs=r4-r10           /* restore registers */
 
-    ldmpc regs=r4-r11                   @ restore regs
-    .ltorg                              @ dump constant pool
-    .size   lcd_yuv_write_inner_loop, .-lcd_yuv_write_inner_loop
+    .ltorg
+    .size   lcd_write_yuv420_lines, .-lcd_write_yuv420_lines
diff --git a/firmware/target/arm/ipod/lcd-color_nano.c b/firmware/target/arm/ipod/lcd-color_nano.c
index e3b9ea8eb6..8cff7bda21 100644
--- a/firmware/target/arm/ipod/lcd-color_nano.c
+++ b/firmware/target/arm/ipod/lcd-color_nano.c
@@ -32,6 +32,9 @@
 #include "system.h"
 #include "hwcompat.h"
 
+/*** macros ***/
+#define SWAP_INT(X,Y) {int tmp=X; X=Y; Y=tmp;}
+
 /* LCD command codes for HD66789R */
 #define LCD_CNTL_RAM_ADDR_SET           0x21
 #define LCD_CNTL_WRITE_TO_GRAM          0x22
@@ -120,25 +123,11 @@ void lcd_init_device(void)
 #endif
 }
 
-/*** update functions ***/
-extern void lcd_yuv_write_inner_loop(unsigned char const * const ysrc,
-                                     unsigned char const * const usrc,
-                                     unsigned char const * const vsrc,
-                                     int width);
-
-#define CSUB_X 2
-#define CSUB_Y 2
-
-/* Performance function to blit a YUV bitmap directly to the LCD */
-void lcd_blit_yuv(unsigned char * const src[3],
-                  int src_x, int src_y, int stride,
-                  int x, int y, int width, int height)
+/* Helper function to set up drawing region and start drawing */
+static void lcd_setup_drawing_region(int x, int y, int width, int height)
 {
-    int h;
     int y0, x0, y1, x1;
-
-    width = (width + 1) & ~1;
-
+    
     /* calculate the drawing region */
 #if CONFIG_LCD == LCD_IPODNANO
     y0 = x;                         /* start horiz */
@@ -159,21 +148,8 @@ void lcd_blit_yuv(unsigned char * const src[3],
         lcd_cmd_data(0x15, y1);      /* end vert */
         lcd_cmd_data(0x16, x1);      /* end horiz */
     } else {
-        /* swap max horiz < start horiz */
-        if (y1 < y0) {
-            int t;
-            t = y0;
-            y0 = y1;
-            y1 = t;
-        }
-
-        /* swap max vert < start vert */
-        if (x1 < x0) {
-            int t;
-            t = x0;
-            x0 = x1;
-            x1 = t;
-        }
+        if (y1 < y0) SWAP_INT(y0,y1) /* swap max horiz < start horiz */
+        if (x1 < x0) SWAP_INT(x0,x1) /* swap max vert  < start vert  */
 
         /* max horiz << 8 | start horiz */
         lcd_cmd_data(LCD_CNTL_HORIZ_RAM_ADDR_POS, (y1 << 8) | y0);
@@ -194,167 +170,117 @@ void lcd_blit_yuv(unsigned char * const src[3],
         LCD2_PORT = LCD2_CMD_MASK;
         LCD2_PORT = (LCD2_CMD_MASK|LCD_CNTL_WRITE_TO_GRAM);
     }
+}
 
-    const int stride_div_csub_x = stride/CSUB_X;
-
-    h=0;
-    while (1)
-    {
-        /* upsampling, YUV->RGB conversion and reduction to RGB565 in one go */
-        const unsigned char *ysrc = src[0] + stride * src_y + src_x;
-
-        const int uvoffset = stride_div_csub_x * (src_y/CSUB_Y) +
-                             (src_x/CSUB_X);
+/* Line write helper function for lcd_yuv_blit. Writes two lines of yuv420. */
+extern void lcd_write_yuv420_lines(unsigned char const * const src[3],
+                                   const unsigned int lcd_baseadress,
+                                   int width,
+                                   int stride);
 
-        const unsigned char *usrc = src[1] + uvoffset;
-        const unsigned char *vsrc = src[2] + uvoffset;
+/* Performance function to blit a YUV bitmap directly to the LCD */
+void lcd_blit_yuv(unsigned char * const src[3],
+                  int src_x, int src_y, int stride,
+                  int x, int y, int width, int height)
+{
+    int z;
+    unsigned char const * yuv_src[3];
 
-        int pixels_to_write;
+    width  = (width  + 1) & ~1; /* ensure width is even  */
+    height = (height + 1) & ~1; /* ensure height is even */
 
-        if (h==0)
-        {
-            while (!(LCD2_BLOCK_CTRL & LCD2_BLOCK_READY));
-            LCD2_BLOCK_CONFIG = 0;
+    lcd_setup_drawing_region(x, y, width, height);
 
-            if (height == 0) break;
+    z = stride * src_y;
+    yuv_src[0] = src[0] + z + src_x;
+    yuv_src[1] = src[1] + (z >> 2) + (src_x >> 1);
+    yuv_src[2] = src[2] + (yuv_src[1] - src[1]);
 
-            pixels_to_write = (width * height) * 2;
-            h = height;
+    while (height > 0) {
+        int r, h, pixels_to_write;
 
-            /* calculate how much we can do in one go */
-            if (pixels_to_write > 0x10000)
-            {
-                h = (0x10000/2) / width;
-                pixels_to_write = (width * h) * 2;
-            }
+        pixels_to_write = (width * height) * 2;
+        h = height;
 
-            height -= h;
-            LCD2_BLOCK_CTRL = 0x10000080;
-            LCD2_BLOCK_CONFIG = 0xc0010000 | (pixels_to_write - 1);
-            LCD2_BLOCK_CTRL = 0x34000000;
+        /* calculate how much we can do in one go */
+        if (pixels_to_write > 0x10000) {
+            h = ((0x10000/2) / width) & ~1; /* ensure h is even */
+            pixels_to_write = (width * h) * 2;
         }
-
-        lcd_yuv_write_inner_loop(ysrc,usrc,vsrc,width);
-
-        src_y++;
-        h--;
+        
+        LCD2_BLOCK_CTRL   = 0x10000080;
+        LCD2_BLOCK_CONFIG = 0xc0010000 | (pixels_to_write - 1);
+        LCD2_BLOCK_CTRL   = 0x34000000;
+
+        r = h>>1; /* lcd_write_yuv420_lines writes two lines at once */
+        do {
+            lcd_write_yuv420_lines(yuv_src, LCD2_BASE, width, stride);
+            yuv_src[0] += stride << 1;
+            yuv_src[1] += stride >> 1;
+            yuv_src[2] += stride >> 1;
+        } while (--r > 0);
+        
+        /* transfer of pixels_to_write bytes finished */
+        while (!(LCD2_BLOCK_CTRL & LCD2_BLOCK_READY));
+        LCD2_BLOCK_CONFIG = 0;
+        
+        height -= h;
     }
-
-    while (!(LCD2_BLOCK_CTRL & LCD2_BLOCK_READY));
-    LCD2_BLOCK_CONFIG = 0;
 }
 
+/* Helper function writes 'count' consecutive pixels from src to LCD IF */
+static void lcd_write_line(int count, unsigned long *src)
+{
+    do {
+        while (!(LCD2_BLOCK_CTRL & LCD2_BLOCK_TXOK)); /* FIFO wait */
+        LCD2_BLOCK_DATA = *src++;                     /* output 2 pixels */
+        count -= 2;
+    } while (count > 0);
+}
 
 /* Update a fraction of the display. */
 void lcd_update_rect(int x, int y, int width, int height)
 {
-    int y0, x0, y1, x1;
-    int newx,newwidth;
     unsigned long *addr;
 
-    /* Ensure x and width are both even - so we can read 32-bit aligned 
-       data from lcd_framebuffer */
-    newx=x&~1;
-    newwidth=width&~1;
-    if (newx+newwidth < x+width) { newwidth+=2; }
-    x=newx; width=newwidth;
+    /* Ensure both x and width are even to be able to read 32-bit aligned 
+     * data from lcd_framebuffer */
+    x     =  x & ~1;            /* use the smaller even number */
+    width = (width + 1) & ~1;   /* use the bigger even number  */
 
-    /* calculate the drawing region */
-#if CONFIG_LCD == LCD_IPODNANO
-    y0 = x;                         /* start horiz */
-    x0 = y;                         /* start vert */
-    y1 = (x + width) - 1;           /* max horiz */
-    x1 = (y + height) - 1;          /* max vert */
-#elif CONFIG_LCD == LCD_IPODCOLOR
-    y0 = y;                         /* start vert */
-    x0 = (LCD_WIDTH - 1) - x;       /* start horiz */
-    y1 = (y + height) - 1;          /* end vert */
-    x1 = (x0 - width) + 1;          /* end horiz */
-#endif
-    /* setup the drawing region */
-    if (lcd_type == 0) {
-        lcd_cmd_data(0x12, y0);      /* start vert */
-        lcd_cmd_data(0x13, x0);      /* start horiz */
-        lcd_cmd_data(0x15, y1);      /* end vert */
-        lcd_cmd_data(0x16, x1);      /* end horiz */
-    } else {
-        /* swap max horiz < start horiz */
-        if (y1 < y0) {
-            int t;
-            t = y0;
-            y0 = y1;
-            y1 = t;
-        }
-
-        /* swap max vert < start vert */
-        if (x1 < x0) {
-            int t;
-            t = x0;
-            x0 = x1;
-            x1 = t;
-        }
-
-        /* max horiz << 8 | start horiz */
-        lcd_cmd_data(LCD_CNTL_HORIZ_RAM_ADDR_POS, (y1 << 8) | y0);
-        /* max vert << 8 | start vert */
-        lcd_cmd_data(LCD_CNTL_VERT_RAM_ADDR_POS, (x1 << 8) | x0);
-
-        /* start vert = max vert */
-#if CONFIG_LCD == LCD_IPODCOLOR
-        x0 = x1;
-#endif
-
-        /* position cursor (set AD0-AD15) */
-        /* start vert << 8 | start horiz */
-        lcd_cmd_data(LCD_CNTL_RAM_ADDR_SET, ((x0 << 8) | y0));
-
-        /* start drawing */
-        lcd_wait_write();
-        LCD2_PORT = LCD2_CMD_MASK;
-        LCD2_PORT = (LCD2_CMD_MASK|LCD_CNTL_WRITE_TO_GRAM);
-    }
+    lcd_setup_drawing_region(x, y, width, height);
 
     addr = (unsigned long*)&lcd_framebuffer[y][x];
 
     while (height > 0) {
-        int c, r;
-        int h, pixels_to_write;
+        int r, h, pixels_to_write;
 
         pixels_to_write = (width * height) * 2;
         h = height;
 
         /* calculate how much we can do in one go */
         if (pixels_to_write > 0x10000) {
-            h = (0x10000/2) / width;
+            h = ((0x10000/2) / width) & ~1; /* ensure h is even */
             pixels_to_write = (width * h) * 2;
         }
 
-        LCD2_BLOCK_CTRL = 0x10000080;
+        LCD2_BLOCK_CTRL   = 0x10000080;
         LCD2_BLOCK_CONFIG = 0xc0010000 | (pixels_to_write - 1);
-        LCD2_BLOCK_CTRL = 0x34000000;
+        LCD2_BLOCK_CTRL   = 0x34000000;
 
         if (LCD_WIDTH == width) {
-            /* for each row and column in a single loop */
-            for (r = 0; r < h*width; r += 2) {
-                    while (!(LCD2_BLOCK_CTRL & LCD2_BLOCK_TXOK));
-    
-                    /* output 2 pixels */
-                    LCD2_BLOCK_DATA = *addr++;
-            }
+            /* for each row and column in a single call */
+            lcd_write_line(h*width, addr);
+            addr += LCD_WIDTH/2*h;
         } else {
             /* for each row */
             for (r = 0; r < h; r++) {
-                /* for each column */
-                for (c = 0; c < width; c += 2) {
-                    while (!(LCD2_BLOCK_CTRL & LCD2_BLOCK_TXOK));
-    
-                    /* output 2 pixels */
-                    LCD2_BLOCK_DATA = *addr++;
-                }
-                addr += (LCD_WIDTH - width)/2;
+                lcd_write_line(width, addr);
+                addr += LCD_WIDTH/2;
             }
         }
 
+        /* transfer of pixels_to_write bytes finished */
         while (!(LCD2_BLOCK_CTRL & LCD2_BLOCK_READY));
         LCD2_BLOCK_CONFIG = 0;