diff --git a/firmware/target/arm/as3525/lcd-as-e200v2-fuze.S b/firmware/target/arm/as3525/lcd-as-e200v2-fuze.S
index 71d997d..e146d86 100644
--- a/firmware/target/arm/as3525/lcd-as-e200v2-fuze.S
+++ b/firmware/target/arm/as3525/lcd-as-e200v2-fuze.S
@@ -25,6 +25,16 @@
#include "cpu.h"

#define DBOP_BUSY (1<<10)
+#define SAVE_PIXEL \
+ mov r6, r0, lsl #16
+
+#define GET_SAVED_PIXEL \
+ orr r6, r6, r0
+
+#define WRITE_PIXELS \
+ str r6, [r3, #0x10]
+
+

/****************************************************************************
* void lcd_write_yuv_420_lines(unsigned char const * const src[3],
@@ -110,12 +120,8 @@ lcd_write_yuv420_lines:
@
orr r0, r0, lr, lsl #11 @ r0 = (r << 11) | b
orr r0, r0, r7, lsl #5 @ r0 = (r << 11) | (g << 5) | b
- strh r0, [r3, #0x10] @ write pixel
-1: @ busy @
- ldr r7, [r3,#0xc] @ r7 = DBOP_STATUS
- tst r7, #DBOP_BUSY @ fifo not empty?
- beq 1b @
- @
+ SAVE_PIXEL @ save pixel, will be written later
+
sub r7, r12, #16 @ r7 = Y = (Y' - 16)*74
add r12, r7, r7, asl #2 @
add r7, r12, r7, asl #5 @
@@ -143,7 +149,8 @@ lcd_write_yuv420_lines:
@
orr r0, r0, lr, lsl #11 @ r0 = (r << 11) | b
orr r0, r0, r7, lsl #5 @ r0 = (r << 11) | (g << 5) | b
- strh r0, [r3, #0x10] @ write pixel
+ GET_SAVED_PIXEL @ get saved pixel
+ WRITE_PIXELS @ write pixels
1: @ busy @
ldr r7, [r3,#0xc] @ r7 = DBOP_STATUS
tst r7, #DBOP_BUSY @ fifo not empty?
@@ -177,11 +184,7 @@ lcd_write_yuv420_lines:
@
orr r0, r0, lr, lsl #11 @ r0 = (r << 11) | b
orr r0, r0, r7, lsl #5 @ r0 = (r << 11) | (g << 5) | b
- strh r0, [r3, #0x10] @ write pixel
-1: @ busy @
- ldr r7, [r3,#0xc] @ r7 = DBOP_STATUS
- tst r7, #DBOP_BUSY @ fifo not empty?
- beq 1b @
+ SAVE_PIXEL @ save pixel, is written later
@
sub r7, r12, #16 @ r7 = Y = (Y' - 16)*74
add r12, r7, r7, asl #2 @
@@ -208,7 +211,8 @@ lcd_write_yuv420_lines:
@
orr r0, r0, lr, lsl #11 @ r0 = (r << 11) | b
orr r0, r0, r7, lsl #5 @ r0 = (r << 11) | (g << 5) | b
- strh r0, [r3, #0x10] @ write pixel
+ GET_SAVED_PIXEL @ get saved pixel
+ WRITE_PIXELS @ write pixels
1: @ busy @
ldr r7, [r3,#0xc] @ r7 = DBOP_STATUS
tst r7, #DBOP_BUSY @ fifo not empty?
@@ -340,11 +344,7 @@ lcd_write_yuv420_lines_odither:
and r7, r7, #0x7e00 @ r0 = pixel = (r & 0xf800) |
orr r11, r11, r7, lsr #4 @ ((g & 0x7e00) >> 4) |
orr r0, r11, r0, lsr #10 @ (b >> 10)
- strh r0, [r3, #0x10] @ write pixel
-1: @ busy @
- ldr r7, [r3,#0xc] @ r7 = DBOP_STATUS
- tst r7, #DBOP_BUSY @ fifo not empty?
- beq 1b @
+ SAVE_PIXEL @ save pixel, is written later
@
sub r7, r12, #16 @ r7 = Y = (Y' - 16)*149
add r12, r7, r7, asl #2 @
@@ -389,7 +389,8 @@ lcd_write_yuv420_lines_odither:
and r7, r7, #0x7e00 @ r0 = pixel = (r & 0xf800) |
orr r11, r11, r7, lsr #4 @ ((g & 0x7e00) >> 4) |
orr r0, r11, r0, lsr #10 @ (b >> 10)
- strh r0, [r3, #0x10] @ write pixel
+ GET_SAVED_PIXEL @ get saved pixel
+ WRITE_PIXELS @ write pixels
1: @ busy @
ldr r7, [r3,#0xc] @ r7 = DBOP_STATUS
tst r7, #DBOP_BUSY @ fifo not empty?
@@ -440,11 +441,7 @@ lcd_write_yuv420_lines_odither:
and r7, r7, #0x7e00 @ r0 = pixel = (r & 0xf800) |
orr r11, r11, r7, lsr #4 @ ((g & 0x7e00) >> 4) |
orr r0, r11, r0, lsr #10 @ (b >> 10)
- strh r0, [r3, #0x10] @ write pixel
-1: @ busy @
- ldr r7, [r3,#0xc] @ r7 = DBOP_STATUS
- tst r7, #DBOP_BUSY @ fifo not empty?
- beq 1b @
+ SAVE_PIXEL @ save pixel, is written later
@
sub r7, r12, #16 @ r7 = Y = (Y' - 16)*149
add r12, r7, r7, asl #2 @
@@ -487,7 +484,8 @@ lcd_write_yuv420_lines_odither:
and r7, r7, #0x7e00 @ r0 = pixel = (r & 0xf800) |
orr r11, r11, r7, lsr #4 @ ((g & 0x7e00) >> 4) |
orr r0, r11, r0, lsr #10 @ (b >> 10)
- strh r0, [r3, #0x10] @ write pixel
+ GET_SAVED_PIXEL @ get saved pixel
+ WRITE_PIXELS @ write pixels
1: @ busy @
ldr r7, [r3,#0xc] @ r7 = DBOP_STATUS
tst r7, #DBOP_BUSY @ fifo not empty?