Fuzev2: Reduce code duplication by reusing Fuzev1 code.

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

5 files changed, 25 insertions, 1098 deletions

diff --git a/firmware/SOURCES b/firmware/SOURCES
index 2ee04f2..a61331f 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -1252,7 +1252,7 @@ target/arm/as3525/sansa-clip/powermgmt-clip.c
 #ifdef SANSA_E200V2
 #ifndef SIMULATOR
 target/arm/as3525/sansa-e200v2/lcd-e200v2.c
-target/arm/as3525/lcd-as-e200v2-fuze.S
+target/arm/as3525/lcd-as-e200v2-fuze-fuzev2.S
 target/arm/as3525/button-e200v2-fuze.c
 target/arm/as3525/backlight-e200v2-fuze.c
 target/arm/as3525/dbop-as3525.c
@@ -1290,7 +1290,7 @@ target/arm/as3525/sansa-m200v4/powermgmt-m200v4.c
 #ifndef SIMULATOR
 target/arm/as3525/button-e200v2-fuze.c
 target/arm/as3525/sansa-fuze/lcd-fuze.c
-target/arm/as3525/lcd-as-e200v2-fuze.S
+target/arm/as3525/lcd-as-e200v2-fuze-fuzev2.S
 target/arm/as3525/backlight-e200v2-fuze.c
 target/arm/as3525/dbop-as3525.c
 #ifndef BOOTLOADER
@@ -1303,9 +1303,10 @@ target/arm/as3525/sansa-fuze/powermgmt-fuze.c
 #ifdef SANSA_FUZEV2
 #ifndef SIMULATOR
 target/arm/as3525/sansa-fuzev2/lcd-fuzev2.c
-target/arm/as3525/sansa-fuzev2/lcd-as-fuzev2.S
+target/arm/as3525/lcd-as-e200v2-fuze-fuzev2.S
 target/arm/as3525/sansa-fuzev2/backlight-fuzev2.c
 target/arm/as3525/sansa-fuzev2/button-fuzev2.c
+target/arm/as3525/dbop-as3525.c
 #ifndef BOOTLOADER
 target/arm/powermgmt-ascodec.c
 target/arm/as3525/sansa-fuzev2/powermgmt-fuzev2.c
diff --git a/firmware/target/arm/as3525/dbop-as3525.c b/firmware/target/arm/as3525/dbop-as3525.c
index 1730290..5ca551c 100644
--- a/firmware/target/arm/as3525/dbop-as3525.c
+++ b/firmware/target/arm/as3525/dbop-as3525.c
@@ -30,6 +30,8 @@
 #define DBOP_PRECHARGE 0xF0FF
 #endif
 
+#if CONFIG_CPU == AS3525
+/* doesn't work with the new ams sansas so far and is not needed */
 static short int dbop_input_value = 0;
 
 /* read the DBOP data pins */
@@ -77,13 +79,24 @@ unsigned short dbop_debug(void)
     return dbop_input_value;
 }
 
+#endif
+
 static inline void dbop_set_mode(int mode)
 {
     int delay = 10;
-    if (mode == 32 && (!(DBOP_CTRL & (1<<13|1<<14))))
-        DBOP_CTRL |= (1<<13|1<<14);
-    else if (mode == 16 && (DBOP_CTRL & (1<<13|1<<14)))
-        DBOP_CTRL &= ~(1<<14|1<<13);
+    unsigned long ctrl = DBOP_CTRL;
+    int curr_mode = (DBOP_CTRL >> 13) & 0x3; // bits 14:13
+#ifdef SANSA_FUZEV2
+    if (mode == 32 && curr_mode != 1<<1)
+        DBOP_CTRL = (ctrl & ~(1<<13)) | (1<<14); // 2 serial half words
+    else if (mode == 16 && curr_mode != 1<<0)
+        DBOP_CTRL = (ctrl & ~(1<<14)) | (1<<13); // 2 serial bytes
+#else
+    if (mode == 32 && curr_mode == 0)
+        DBOP_CTRL = ctrl | (1<<13|1<<14); /* 2 serial half words */
+    else if (mode == 16 && curr_mode == (1<<1|1<<0))
+        DBOP_CTRL =  ctrl & ~(1<<14|1<<13); /* 1 serial half word */
+#endif
     else
         return;
     while(delay--) asm volatile("nop");
diff --git a/firmware/target/arm/as3525/lcd-as-e200v2-fuze.S b/firmware/target/arm/as3525/lcd-as-e200v2-fuze.S
deleted file mode 100644
index 2725c92..0000000
--- a/firmware/target/arm/as3525/lcd-as-e200v2-fuze.S
+++ /dev/null
@@ -1,500 +0,0 @@
-/***************************************************************************
- *             __________               __   ___.
- *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
- *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
- *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
- *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
- *                     \/            \/     \/    \/            \/
- * $Id$
- *
- * Copyright (C) 2007 by Jens Arnold
- * Heavily based on lcd-as-memframe.c by Michael Sevakis
- * Adapted for Sansa Fuze/e200v2 by Rafaël Carré
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * as published by the Free Software Foundation; either version 2
- * of the License, or (at your option) any later version.
- *
- * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
- * KIND, either express or implied.
- *
- ****************************************************************************/
-
-#include "config.h"
-#include "cpu.h"
-
-#define DBOP_BUSY (1<<10)
-
-/****************************************************************************
- * void lcd_write_yuv_420_lines(unsigned char const * const src[3],
- *                              int width,
- *                              int stride);
- *
- *   |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
- *
- * Write four RGB565 pixels in the following order on each loop:
- * 1 3 + > down
- * 2 4 \/ left
- */
-    .section    .icode, "ax", %progbits
-    .align      2
-    .global     lcd_write_yuv420_lines
-    .type       lcd_write_yuv420_lines, %function
-lcd_write_yuv420_lines:
-                                        @ r0 = yuv_src
-                                        @ r1 = width
-                                        @ r2 = stride
-    stmfd       sp!, { r4-r11, lr }     @ save non-scratch
-
-    mov         r3, #0xC8000000         @
-    orr         r3, r3, #0x120000       @ r3 = DBOP_BASE
-
-    ldmia       r0, { r4, r5, r6 }      @ r4 = yuv_src[0] = Y'_p
-                                        @ r5 = yuv_src[1] = Cb_p
-                                        @ r6 = yuv_src[2] = Cr_p
-                                        @ r0 = scratch
-    ldr         r12, [r3, #8]           @
-    sub         r2, r2, #1              @ stride -= 1
-    orr         r12, r12, #3<<13        @
-    str         r12, [r3, #8]           @ DBOP_CTRL |= (1<<13|1<<14) (32bit mode)
-10: @ loop line                         @
-    ldrb        r7, [r4], #1            @ r7 = *Y'_p++;
-    ldrb        r8, [r5], #1            @ r8 = *Cb_p++;
-    ldrb        r9, [r6], #1            @ r9 = *Cr_p++;
-                                        @
-    sub         r7, r7, #16             @ r7 = Y = (Y' - 16)*74
-    add         r12, r7, r7, asl #2     @ actually (Y' - 16)*37 and shift right
-    add         r7, r12, r7, asl #5     @ by one less when adding - same for all
-                                        @
-    sub         r8, r8, #128            @ Cb -= 128
-    sub         r9, r9, #128            @ Cr -= 128
-                                        @
-    add         r10, r9, r9, asl #1     @ r10 = Cr*51 + Cb*24
-    add         r10, r10, r10, asl #4   @
-    add         r10, r10, r8, asl #3    @
-    add         r10, r10, r8, asl #4    @
-                                        @
-    add         lr, r9, r9, asl #2      @ r9 = Cr*101
-    add         lr, lr, r9, asl #5      @
-    add         r9, lr, r9, asl #6      @
-                                        @
-    add         r8, r8, #2              @ r8 = bu = (Cb*128 + 128) >> 8
-    mov         r8, r8, asr #2          @
-    add         r9, r9, #256            @ r9 = rv = (r9 + 256) >> 9
-    mov         r9, r9, asr #9          @
-    rsb         r10, r10, #128          @ r10 = guv = (-r10 + 128) >> 8
-    mov         r10, r10, asr #8        @
-                                        @ compute R, G, and B
-    add         r0, r8, r7, asr #8      @ r0  = b = (Y >> 9) + bu
-    add         lr, r9, r7, asr #8      @ lr = r = (Y >> 9) + rv
-    add         r7, r10, r7, asr #7     @ r7  = g = (Y >> 8) + guv
-                                        @
-    orr         r12, r0, lr             @ check if clamping is needed...
-    orr         r12, r12, r7, asr #1    @ ...at all
-    cmp         r12, #31                @
-    bls         15f @ no clamp          @
-    cmp         r0, #31                 @ clamp b
-    mvnhi       r0, r0, asr #31         @
-    andhi       r0, r0, #31             @
-    cmp         lr, #31                 @ clamp r
-    mvnhi       lr, lr, asr #31         @
-    andhi       lr, lr, #31             @
-    cmp         r7, #63                 @ clamp g
-    mvnhi       r7, r7, asr #31         @
-    andhi       r7, r7, #63             @
-15: @ no clamp                          @
-                                        @
-    ldrb        r12, [r4, r2]           @ r12 = Y' = *(Y'_p + stride)
-                                        @
-    orr         r0, r0, lr, lsl #11     @ r0 = (r << 11) | b
-    orr         r11, r0, r7, lsl #5      @ r0 = (r << 11) | (g << 5) | b
-                                        @
-    sub         r7, r12, #16            @ r7 = Y = (Y' - 16)*74
-    add         r12, r7, r7, asl #2     @
-    add         r7, r12, r7, asl #5     @
-                                        @ compute R, G, and B
-    add         r0, r8, r7, asr #8      @ r0  = b = (Y >> 9) + bu
-    add         lr, r9, r7, asr #8      @ lr = r = (Y >> 9) + rv
-    add         r7, r10, r7, asr #7     @ r7  = g = (Y >> 8) + guv
-                                        @
-    orr         r12, r0, lr             @ check if clamping is needed...
-    orr         r12, r12, r7, asr #1    @ ...at all
-    cmp         r12, #31                @
-    bls         15f @ no clamp          @
-    cmp         r0, #31                 @ clamp b
-    mvnhi       r0, r0, asr #31         @
-    andhi       r0, r0, #31             @
-    cmp         lr, #31                 @ clamp r
-    mvnhi       lr, lr, asr #31         @
-    andhi       lr, lr, #31             @
-    cmp         r7, #63                 @ clamp g
-    mvnhi       r7, r7, asr #31         @
-    andhi       r7, r7, #63             @
-15: @ no clamp                          @
-                                        @
-    ldrb        r12, [r4], #1           @ r12 = Y' = *(Y'_p++)
-                                        @
-    orr         r0, r0, lr, lsl #11     @ r0 = (r << 11) | b
-    orr         r0, r0, r7, lsl #5      @ r0 = (r << 11) | (g << 5) | b
-    orr         r0, r11, r0, lsl#16     @ pack with 2nd pixel
-    str         r0, [r3, #0x10]         @ write pixel
-                                        @
-    sub         r7, r12, #16            @ r7 = Y = (Y' - 16)*74
-    add         r12, r7, r7, asl #2     @
-    add         r7, r12, r7, asl #5     @
-                                        @ compute R, G, and B
-    add         r0, r8, r7, asr #8      @ r0  = b = (Y >> 9) + bu
-    add         lr, r9, r7, asr #8      @ lr = r = (Y >> 9) + rv
-    add         r7, r10, r7, asr #7     @ r7  = g = (Y >> 8) + guv
-                                        @
-    orr         r12, r0, lr             @ check if clamping is needed...
-    orr         r12, r12, r7, asr #1    @ ...at all
-    cmp         r12, #31                @
-    bls         15f @ no clamp          @
-    cmp         r0, #31                 @ clamp b
-    mvnhi       r0, r0, asr #31         @
-    andhi       r0, r0, #31             @
-    cmp         lr, #31                 @ clamp r
-    mvnhi       lr, lr, asr #31         @
-    andhi       lr, lr, #31             @
-    cmp         r7, #63                 @ clamp g
-    mvnhi       r7, r7, asr #31         @
-    andhi       r7, r7, #63             @
-15: @ no clamp                          @
-                                        @
-    ldrb        r12, [r4, r2]           @ r12 = Y' = *(Y'_p + stride)
-                                        @
-                                        @
-    orr         r0, r0, lr, lsl #11     @ r0 = (r << 11) | b
-    orr         r11, r0, r7, lsl #5      @ r0 = (r << 11) | (g << 5) | b
-                                        @
-    sub         r7, r12, #16            @ r7 = Y = (Y' - 16)*74
-    add         r12, r7, r7, asl #2     @
-    add         r7, r12, r7, asl #5     @
-                                        @ compute R, G, and B
-    add         r0, r8, r7, asr #8      @ r0  = b = (Y >> 9) + bu
-    add         lr, r9, r7, asr #8      @ lr = r = (Y >> 9) + rv
-    add         r7, r10, r7, asr #7     @ r7  = g = (Y >> 8) + guv
-                                        @
-    orr         r12, r0, lr             @ check if clamping is needed...
-    orr         r12, r12, r7, asr #1    @ ...at all
-    cmp         r12, #31                @
-    bls         15f @ no clamp          @
-    cmp         r0, #31                 @ clamp b
-    mvnhi       r0, r0, asr #31         @
-    andhi       r0, r0, #31             @
-    cmp         lr, #31                 @ clamp r
-    mvnhi       lr, lr, asr #31         @
-    andhi       lr, lr, #31             @
-    cmp         r7, #63                 @ clamp g
-    mvnhi       r7, r7, asr #31         @
-    andhi       r7, r7, #63             @
-15: @ no clamp                          @
-                                        @
-    orr         r0, r0, lr, lsl #11     @ r0 = (r << 11) | b
-    orr         r0, r0, r7, lsl #5      @ r0 = (r << 11) | (g << 5) | b
-    orr         r0, r11, r0, lsl#16     @ pack with 2nd pixel
-    str         r0, [r3, #0x10]         @ write pixel
-                                        @
-    subs        r1, r1, #2              @ subtract block from width
-    bgt         10b @ loop line         @
-                                        @
-1: @ busy
-    @ writing at max 110*32 its (LCD_WIDTH/2), the fifo is bigger
-    @ so polling fifo empty after the loops is save
-    ldr         r7, [r3,#0xc]           @ r7 = DBOP_STATUS
-    tst         r7, #DBOP_BUSY          @ fifo not empty?
-    beq         1b                      @
-
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
-    bx          lr                      @
-    .ltorg                              @ dump constant pool
-    .size   lcd_write_yuv420_lines, .-lcd_write_yuv420_lines
-
-/****************************************************************************
- * void lcd_write_yuv_420_lines_odither(unsigned char const * const src[3],
- *                                      int width,
- *                                      int stride,
- *                                      int x_screen,
- *                                      int y_screen);
- *
- *   |R|   |1.000000 -0.000001  1.402000| |Y'|
- *   |G| = |1.000000 -0.334136 -0.714136| |Pb|
- *   |B|   |1.000000  1.772000  0.000000| |Pr|
- *   Red scaled at twice g & b but at same precision to place it in correct
- *   bit position after multiply and leave instruction count lower.
- *   |R|   |258   0  408| |Y' -  16|
- *   |G| = |149 -49 -104| |Cb - 128|
- *   |B|   |149 258    0| |Cr - 128|
- *
- * Write four RGB565 pixels in the following order on each loop:
- * 1 3 + > down
- * 2 4 \/ left
- *
- * Kernel pattern (raw|rotated|use order):
- * 5 3 4 2   2 6 3 7     row0   row2          > down
- * 1 7 0 6 | 4 0 5 1 | 2 4 6 0 3 5 7 1 col0     left
- * 4 2 5 3 | 3 7 2 6 | 3 5 7 1 2 4 6 0 col2  \/
- * 0 6 1 7   5 1 4 0
- */
-    .section    .icode, "ax", %progbits
-    .align      2
-    .global     lcd_write_yuv420_lines_odither
-    .type       lcd_write_yuv420_lines_odither, %function
-lcd_write_yuv420_lines_odither:
-                                        @ r0 = yuv_src
-                                        @ r1 = width
-                                        @ r2 = stride
-                                        @ r3 = x_screen
-                                        @ [sp] = y_screen
-    stmfd       sp!, { r4-r11, lr }     @ save non-scratch
-    ldmia       r0, { r4, r5, r6 }      @ r4 = yuv_src[0] = Y'_p
-                                        @ r5 = yuv_src[1] = Cb_p
-                                        @ r6 = yuv_src[2] = Cr_p
-                                        @
-    ldr         r14, [sp, #40]          @ Line up pattern and kernel quadrant
-    sub         r2, r2, #1              @ stride =- 1
-    eor         r14, r14, r3            @
-    and         r14, r14, #0x2          @
-    mov         r14, r14, lsl #6        @ 0x00 or 0x80
-
-    mov         r3, #0xC8000000         @
-    orr         r3, r3, #0x120000       @ r3 = DBOP_BASE, need to be redone
-                                        @ due to lack of registers
-    ldr         r12, [r3, #8]           @
-    orr         r12, r12, #3<<13        @ DBOP_CTRL |= (1<<13|1<<14) 
-    str         r12, [r3, #8]           @ (32bit mode)
-10: @ loop line                         @
-                                        @
-    ldrb        r7, [r4], #1            @ r7 = *Y'_p++;
-    ldrb        r8, [r5], #1            @ r8 = *Cb_p++;
-    ldrb        r9, [r6], #1            @ r9 = *Cr_p++;
-                                        @
-    eor         r14, r14, #0x80         @ flip pattern quadrant
-                                        @
-    sub         r7, r7, #16             @ r7 = Y = (Y' - 16)*149
-    add         r12, r7, r7, asl #2     @
-    add         r12, r12, r12, asl #4   @
-    add         r7, r12, r7, asl #6     @
-                                        @
-    sub         r8, r8, #128            @ Cb -= 128
-    sub         r9, r9, #128            @ Cr -= 128
-                                        @
-    add         r10, r8, r8, asl #4     @ r10 = guv = Cr*104 + Cb*49
-    add         r10, r10, r8, asl #5    @
-    add         r10, r10, r9, asl #3    @
-    add         r10, r10, r9, asl #5    @
-    add         r10, r10, r9, asl #6    @
-                                        @
-    mov         r8, r8, asl #1          @ r8 = bu = Cb*258
-    add         r8, r8, r8, asl #7      @
-                                        @
-    add         r9, r9, r9, asl #1      @ r9 = rv = Cr*408
-    add         r9, r9, r9, asl #4      @
-    mov         r9, r9, asl #3          @
-                                        @
-                                        @ compute R, G, and B
-    add         r0, r8, r7              @ r0  = b' = Y + bu
-    add         r11, r9, r7, asl #1     @ r11 = r' = Y*2 + rv
-    rsb         r7, r10, r7             @ r7  = g' = Y + guv
-                                        @
-                                        @ r8 = bu, r9 = rv, r10 = guv
-                                        @
-    sub         r12, r0, r0, lsr #5     @ r0 = 31/32*b + b/256
-    add         r0, r12, r0, lsr #8     @
-                                        @
-    sub         r12, r11, r11, lsr #5   @ r11 = 31/32*r + r/256
-    add         r11, r12, r11, lsr #8   @
-                                        @
-    sub         r12, r7, r7, lsr #6     @ r7 = 63/64*g + g/256
-    add         r7, r12, r7, lsr #8     @
-                                        @
-    add         r12, r14, #0x100        @
-                                        @
-    add         r0, r0, r12             @ b = r0 + delta
-    add         r11, r11, r12, lsl #1   @ r = r11 + delta*2
-    add         r7, r7, r12, lsr #1     @ g = r7 + delta/2
-                                        @
-    orr         r12, r0, r11, asr #1    @ check if clamping is needed...
-    orr         r12, r12, r7            @ ...at all
-    movs        r12, r12, asr #15       @
-    beq         15f @ no clamp          @
-    movs        r12, r0, asr #15        @ clamp b
-    mvnne       r0, r12, lsr #15        @
-    andne       r0, r0, #0x7c00         @ mask b only if clamped
-    movs        r12, r11, asr #16       @ clamp r
-    mvnne       r11, r12, lsr #16       @
-    movs        r12, r7, asr #15        @ clamp g
-    mvnne       r7, r12, lsr #15        @
-15: @ no clamp                          @
-                                        @
-    ldrb        r12, [r4, r2]           @ r12 = Y' = *(Y'_p + stride)
-                                        @
-    and         r11, r11, #0xf800       @ pack pixel
-    and         r7, r7, #0x7e00         @ r0 = pixel = (r & 0xf800) |
-    orr         r11, r11, r7, lsr #4    @              ((g & 0x7e00) >> 4) |
-    orr         r3, r11, r0, lsr #10    @              (b >> 10)
-                                        @ save pixel
-    sub         r7, r12, #16            @ r7 = Y = (Y' - 16)*149
-    add         r12, r7, r7, asl #2     @
-    add         r12, r12, r12, asl #4   @
-    add         r7, r12, r7, asl #6     @
-                                        @ compute R, G, and B
-    add         r0, r8, r7              @ r0  = b' = Y + bu
-    add         r11, r9, r7, asl #1     @ r11 = r' = Y*2 + rv
-    rsb         r7, r10, r7             @ r7  = g' = Y + guv
-                                        @
-    sub         r12, r0, r0, lsr #5     @ r0  = 31/32*b' + b'/256
-    add         r0, r12, r0, lsr #8     @
-                                        @
-    sub         r12, r11, r11, lsr #5   @ r11 = 31/32*r' + r'/256
-    add         r11, r12, r11, lsr #8   @
-                                        @
-    sub         r12, r7, r7, lsr #6     @ r7  = 63/64*g' + g'/256
-    add         r7, r12, r7, lsr #8     @
-                                        @
-    add         r12, r14, #0x200        @
-                                        @
-    add         r0, r0, r12             @ b = r0 + delta
-    add         r11, r11, r12, lsl #1   @ r = r11 + delta*2
-    add         r7, r7, r12, lsr #1     @ g = r7 + delta/2
-                                        @
-    orr         r12, r0, r11, asr #1    @ check if clamping is needed...
-    orr         r12, r12, r7            @ ...at all
-    movs        r12, r12, asr #15       @
-    beq         15f @ no clamp          @
-    movs        r12, r0, asr #15        @ clamp b
-    mvnne       r0, r12, lsr #15        @
-    andne       r0, r0, #0x7c00         @ mask b only if clamped
-    movs        r12, r11, asr #16       @ clamp r
-    mvnne       r11, r12, lsr #16       @
-    movs        r12, r7, asr #15        @ clamp g
-    mvnne       r7, r12, lsr #15        @
-15: @ no clamp                          @
-                                        @
-    ldrb        r12, [r4], #1           @ r12 = Y' = *(Y'_p++)
-
-    and         r11, r11, #0xf800       @ pack pixel
-    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)
-    orr         r3, r3, r0, lsl#16      @ pack with 2nd pixel
-    mov         r0, #0xC8000000         @
-    orr         r0, r0, #0x120000       @ r3 = DBOP_BASE
-
-    str         r3, [r0, #0x10]         @ write pixel
-                                        @
-    sub         r7, r12, #16            @ r7 = Y = (Y' - 16)*149
-    add         r12, r7, r7, asl #2     @
-    add         r12, r12, r12, asl #4   @
-    add         r7, r12, r7, asl #6     @
-                                        @ compute R, G, and B
-    add         r0, r8, r7              @ r0  = b' = Y + bu
-    add         r11, r9, r7, asl #1     @ r11 = r' = Y*2 + rv
-    rsb         r7, r10, r7             @ r7  = g' = Y + guv
-                                        @
-                                        @ r8 = bu, r9 = rv, r10 = guv
-                                        @
-    sub         r12, r0, r0, lsr #5     @ r0  = 31/32*b' + b'/256
-    add         r0, r12, r0, lsr #8     @
-                                        @
-    sub         r12, r11, r11, lsr #5   @ r11 = 31/32*r' + r'/256
-    add         r11, r12, r11, lsr #8   @
-                                        @
-    sub         r12, r7, r7, lsr #6     @ r7  = 63/64*g' + g'/256
-    add         r7, r12, r7, lsr #8     @
-                                        @
-    add         r12, r14, #0x300        @
-                                        @
-    add         r0, r0, r12             @ b = r0 + delta
-    add         r11, r11, r12, lsl #1   @ r = r11 + delta*2
-    add         r7, r7, r12, lsr #1     @ g = r7 + delta/2
-                                        @
-    orr         r12, r0, r11, asr #1    @ check if clamping is needed...
-    orr         r12, r12, r7            @ ...at all
-    movs        r12, r12, asr #15       @
-    beq         15f @ no clamp          @
-    movs        r12, r0, asr #15        @ clamp b
-    mvnne       r0, r12, lsr #15        @
-    andne       r0, r0, #0x7c00         @ mask b only if clamped
-    movs        r12, r11, asr #16       @ clamp r
-    mvnne       r11, r12, lsr #16       @
-    movs        r12, r7, asr #15        @ clamp g
-    mvnne       r7, r12, lsr #15        @
-15: @ no clamp                          @
-                                        @
-    ldrb        r12, [r4, r2]           @ r12 = Y' = *(Y'_p + stride)
-                                        @
-    and         r11, r11, #0xf800       @ pack pixel
-    and         r7, r7, #0x7e00         @ r0 = pixel = (r & 0xf800) |
-    orr         r11, r11, r7, lsr #4    @              ((g & 0x7e00) >> 4) |
-    orr         r3, r11, r0, lsr #10    @              (b >> 10)
-                                        @ save pixel
-                                        @
-    sub         r7, r12, #16            @ r7 = Y = (Y' - 16)*149
-    add         r12, r7, r7, asl #2     @
-    add         r12, r12, r12, asl #4   @
-    add         r7, r12, r7, asl #6     @
-                                        @ compute R, G, and B
-    add         r0, r8, r7              @ r0  = b' = Y + bu
-    add         r11, r9, r7, asl #1     @ r11 = r' = Y*2 + rv
-    rsb         r7, r10, r7             @ r7  = g' = Y + guv
-                                        @
-    sub         r12, r0, r0, lsr #5     @ r0 = 31/32*b + b/256
-    add         r0, r12, r0, lsr #8     @
-                                        @
-    sub         r12, r11, r11, lsr #5   @ r11 = 31/32*r + r/256
-    add         r11, r12, r11, lsr #8   @
-                                        @
-    sub         r12, r7, r7, lsr #6     @ r7 = 63/64*g + g/256
-    add         r7, r12, r7, lsr #8     @
-                                        @
-    @ This element is zero - use r14    @
-                                        @
-    add         r0, r0, r14             @ b = r0 + delta
-    add         r11, r11, r14, lsl #1   @ r = r11 + delta*2
-    add         r7, r7, r14, lsr #1     @ g = r7 + delta/2
-                                        @
-    orr         r12, r0, r11, asr #1    @ check if clamping is needed...
-    orr         r12, r12, r7            @ ...at all
-    movs        r12, r12, asr #15       @
-    beq         15f @ no clamp          @
-    movs        r12, r0, asr #15        @ clamp b
-    mvnne       r0, r12, lsr #15        @
-    andne       r0, r0, #0x7c00         @ mask b only if clamped
-    movs        r12, r11, asr #16       @ clamp r
-    mvnne       r11, r12, lsr #16       @
-    movs        r12, r7, asr #15        @ clamp g
-    mvnne       r7, r12, lsr #15        @
-15: @ no clamp                          @
-                                        @
-    and         r11, r11, #0xf800       @ pack pixel
-    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)
-    orr         r3, r3, r0, lsl#16      @ pack with 2nd pixel
-    mov         r0, #0xC8000000         @
-    orr         r0, r0, #0x120000       @ r3 = DBOP_BASE
-
-    str         r3, [r0, #0x10]         @ write pixel
-                                        @
-    subs        r1, r1, #2              @ subtract block from width
-    bgt         10b @ loop line         @
-                                        @
-1: @ busy                               @
-    @ writing at max 110*32 its (LCD_WIDTH/2), the fifo is bigger
-    @ so polling fifo empty after the loops is save
-    ldr         r7, [r0,#0xc]           @ r7 = DBOP_STATUS
-    tst         r7, #DBOP_BUSY          @ fifo not empty?
-    beq         1b                      @
-
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
-    .ltorg                              @ dump constant pool
-    .size   lcd_write_yuv420_lines_odither, .-lcd_write_yuv420_lines_odither
diff --git a/firmware/target/arm/as3525/sansa-fuzev2/lcd-as-fuzev2.S b/firmware/target/arm/as3525/sansa-fuzev2/lcd-as-fuzev2.S
deleted file mode 100644
index c760eea..0000000
--- a/firmware/target/arm/as3525/sansa-fuzev2/lcd-as-fuzev2.S
+++ /dev/null
@@ -1,530 +0,0 @@
-/***************************************************************************
- *             __________               __   ___.
- *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
- *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
- *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
- *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
- *                     \/            \/     \/    \/            \/
- * $Id$
- *
- * Copyright (C) 2007 by Jens Arnold
- * Heavily based on lcd-as-memframe.c by Michael Sevakis
- * Adapted for Sansa Fuzev2 by Rafaël Carré
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * as published by the Free Software Foundation; either version 2
- * of the License, or (at your option) any later version.
- *
- * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
- * KIND, either express or implied.
- *
- ****************************************************************************/
-
-#include "config.h"
-#include "cpu.h"
-
-#define DBOP_BUSY (1<<10)
-
-/****************************************************************************
- * void lcd_write_yuv_420_lines(unsigned char const * const src[3],
- *                              int width,
- *                              int stride);
- *
- *   |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
- *
- * Write four RGB565 pixels in the following order on each loop:
- * 1 3 + > down
- * 2 4 \/ left
- */
-    .section    .icode, "ax", %progbits
-    .align      2
-    .global     lcd_write_yuv420_lines
-    .type       lcd_write_yuv420_lines, %function
-lcd_write_yuv420_lines:
-                                        @ r0 = yuv_src
-                                        @ r1 = width
-                                        @ r2 = stride
-    stmfd       sp!, { r4-r11, lr }     @ save non-scratch
-
-    mov         r3, #0xC8000000         @
-    orr         r3, r3, #0x120000       @ r3 = DBOP_BASE
-
-    ldmia       r0, { r4, r5, r6 }      @ r4 = yuv_src[0] = Y'_p
-                                        @ r5 = yuv_src[1] = Cb_p
-                                        @ r6 = yuv_src[2] = Cr_p
-                                        @ r0 = scratch
-
-    sub         r2, r2, #1              @ stride -= 1
-
-10: @ loop line                         @
-    ldrb        r7, [r4], #1            @ r7 = *Y'_p++;
-    ldrb        r8, [r5], #1            @ r8 = *Cb_p++;
-    ldrb        r9, [r6], #1            @ r9 = *Cr_p++;
-                                        @
-    sub         r7, r7, #16             @ r7 = Y = (Y' - 16)*74
-    add         r12, r7, r7, asl #2     @ actually (Y' - 16)*37 and shift right
-    add         r7, r12, r7, asl #5     @ by one less when adding - same for all
-                                        @
-    sub         r8, r8, #128            @ Cb -= 128
-    sub         r9, r9, #128            @ Cr -= 128
-                                        @
-    add         r10, r9, r9, asl #1     @ r10 = Cr*51 + Cb*24
-    add         r10, r10, r10, asl #4   @
-    add         r10, r10, r8, asl #3    @
-    add         r10, r10, r8, asl #4    @
-                                        @
-    add         lr, r9, r9, asl #2      @ r9 = Cr*101
-    add         lr, lr, r9, asl #5      @
-    add         r9, lr, r9, asl #6      @
-                                        @
-    add         r8, r8, #2              @ r8 = bu = (Cb*128 + 128) >> 8
-    mov         r8, r8, asr #2          @
-    add         r9, r9, #256            @ r9 = rv = (r9 + 256) >> 9
-    mov         r9, r9, asr #9          @
-    rsb         r10, r10, #128          @ r10 = guv = (-r10 + 128) >> 8
-    mov         r10, r10, asr #8        @
-                                        @ compute R, G, and B
-    add         r0, r8, r7, asr #8      @ r0  = b = (Y >> 9) + bu
-    add         lr, r9, r7, asr #8      @ lr = r = (Y >> 9) + rv
-    add         r7, r10, r7, asr #7     @ r7  = g = (Y >> 8) + guv
-                                        @
-    orr         r12, r0, lr             @ check if clamping is needed...
-    orr         r12, r12, r7, asr #1    @ ...at all
-    cmp         r12, #31                @
-    bls         15f @ no clamp          @
-    cmp         r0, #31                 @ clamp b
-    mvnhi       r0, r0, asr #31         @
-    andhi       r0, r0, #31             @
-    cmp         lr, #31                 @ clamp r
-    mvnhi       lr, lr, asr #31         @
-    andhi       lr, lr, #31             @
-    cmp         r7, #63                 @ clamp g
-    mvnhi       r7, r7, asr #31         @
-    andhi       r7, r7, #63             @
-15: @ no clamp                          @
-                                        @
-    ldrb        r12, [r4, r2]           @ r12 = Y' = *(Y'_p + stride)
-                                        @
-    orr         r0, r0, lr, lsl #11     @ r0 = (r << 11) | b
-    orr         r11, r0, r7, lsl #5     @ r11 = (r << 11) | (g << 5) | b
-                                        @
-    mov         r0, r11, lsr #8         @
-    bic         r11, r11, #0xff00       @
-    orr         r11, r0, r11, lsl #8    @ swap bytes
-                                        @
-    sub         r7, r12, #16            @ r7 = Y = (Y' - 16)*74
-    add         r12, r7, r7, asl #2     @
-    add         r7, r12, r7, asl #5     @
-                                        @ compute R, G, and B
-    add         r0, r8, r7, asr #8      @ r0  = b = (Y >> 9) + bu
-    add         lr, r9, r7, asr #8      @ lr = r = (Y >> 9) + rv
-    add         r7, r10, r7, asr #7     @ r7  = g = (Y >> 8) + guv
-                                        @
-    orr         r12, r0, lr             @ check if clamping is needed...
-    orr         r12, r12, r7, asr #1    @ ...at all
-    cmp         r12, #31                @
-    bls         15f @ no clamp          @
-    cmp         r0, #31                 @ clamp b
-    mvnhi       r0, r0, asr #31         @
-    andhi       r0, r0, #31             @
-    cmp         lr, #31                 @ clamp r
-    mvnhi       lr, lr, asr #31         @
-    andhi       lr, lr, #31             @
-    cmp         r7, #63                 @ clamp g
-    mvnhi       r7, r7, asr #31         @
-    andhi       r7, r7, #63             @
-15: @ no clamp                          @
-                                        @
-    ldrb        r12, [r4], #1           @ r12 = Y' = *(Y'_p++)
-                                        @
-    orr         r0, r0, lr, lsl #11     @ r0 = (r << 11) | b
-    orr         r0, r0, r7, lsl #5      @ r0 = (r << 11) | (g << 5) | b
-                                        @
-    mov         r7, r0, lsr #8          @
-    bic         r7, r7, #0xff00         @
-    orr         r0, r7, r0, lsl #8      @ swap bytes
-                                        @
-    orr         r0, r11, r0, lsl#16     @ pack with 2nd pixel
-    str         r0, [r3, #0x10]         @ write pixel
-                                        @
-    sub         r7, r12, #16            @ r7 = Y = (Y' - 16)*74
-    add         r12, r7, r7, asl #2     @
-    add         r7, r12, r7, asl #5     @
-                                        @ compute R, G, and B
-    add         r0, r8, r7, asr #8      @ r0  = b = (Y >> 9) + bu
-    add         lr, r9, r7, asr #8      @ lr = r = (Y >> 9) + rv
-    add         r7, r10, r7, asr #7     @ r7  = g = (Y >> 8) + guv
-                                        @
-    orr         r12, r0, lr             @ check if clamping is needed...
-    orr         r12, r12, r7, asr #1    @ ...at all
-    cmp         r12, #31                @
-    bls         15f @ no clamp          @
-    cmp         r0, #31                 @ clamp b
-    mvnhi       r0, r0, asr #31         @
-    andhi       r0, r0, #31             @
-    cmp         lr, #31                 @ clamp r
-    mvnhi       lr, lr, asr #31         @
-    andhi       lr, lr, #31             @
-    cmp         r7, #63                 @ clamp g
-    mvnhi       r7, r7, asr #31         @
-    andhi       r7, r7, #63             @
-15: @ no clamp                          @
-                                        @
-    ldrb        r12, [r4, r2]           @ r12 = Y' = *(Y'_p + stride)
-                                        @
-                                        @
-    orr         r0, r0, lr, lsl #11     @ r0 = (r << 11) | b
-    orr         r11, r0, r7, lsl #5     @ r0 = (r << 11) | (g << 5) | b
-                                        @
-    mov         r0, r11, lsr #8         @
-    bic         r11, r11, #0xff00       @
-    orr         r11, r0, r11, lsl #8    @ swap bytes
-                                        @
-    sub         r7, r12, #16            @ r7 = Y = (Y' - 16)*74
-    add         r12, r7, r7, asl #2     @
-    add         r7, r12, r7, asl #5     @
-                                        @ compute R, G, and B
-    add         r0, r8, r7, asr #8      @ r0  = b = (Y >> 9) + bu
-    add         lr, r9, r7, asr #8      @ lr = r = (Y >> 9) + rv
-    add         r7, r10, r7, asr #7     @ r7  = g = (Y >> 8) + guv
-                                        @
-    orr         r12, r0, lr             @ check if clamping is needed...
-    orr         r12, r12, r7, asr #1    @ ...at all
-    cmp         r12, #31                @
-    bls         15f @ no clamp          @
-    cmp         r0, #31                 @ clamp b
-    mvnhi       r0, r0, asr #31         @
-    andhi       r0, r0, #31             @
-    cmp         lr, #31                 @ clamp r
-    mvnhi       lr, lr, asr #31         @
-    andhi       lr, lr, #31             @
-    cmp         r7, #63                 @ clamp g
-    mvnhi       r7, r7, asr #31         @
-    andhi       r7, r7, #63             @
-15: @ no clamp                          @
-                                        @
-    orr         r0, r0, lr, lsl #11     @ r0 = (r << 11) | b
-    orr         r0, r0, r7, lsl #5      @ r0 = (r << 11) | (g << 5) | b
-                                        @
-    mov         r7, r0, lsr #8          @
-    bic         r7, r7, #0xff00         @
-    orr         r0, r7, r0, lsl #8      @ swap bytes
-                                        @
-    orr         r0, r11, r0, lsl#16     @ pack with 2nd pixel
-    str         r0, [r3, #0x10]         @ write pixel
-                                        @
-    subs        r1, r1, #2              @ subtract block from width
-    bgt         10b @ loop line         @
-                                        @
-1: @ busy
-    @ writing at max 110*32 its (LCD_WIDTH/2), the fifo is bigger
-    @ so polling fifo empty after the loops is save
-    ldr         r7, [r3,#0xc]           @ r7 = DBOP_STATUS
-    tst         r7, #DBOP_BUSY          @ fifo not empty?
-    beq         1b                      @
-
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
-    bx          lr                      @
-    .ltorg                              @ dump constant pool
-    .size   lcd_write_yuv420_lines, .-lcd_write_yuv420_lines
-
-/****************************************************************************
- * void lcd_write_yuv_420_lines_odither(unsigned char const * const src[3],
- *                                      int width,
- *                                      int stride,
- *                                      int x_screen,
- *                                      int y_screen);
- *
- *   |R|   |1.000000 -0.000001  1.402000| |Y'|
- *   |G| = |1.000000 -0.334136 -0.714136| |Pb|
- *   |B|   |1.000000  1.772000  0.000000| |Pr|
- *   Red scaled at twice g & b but at same precision to place it in correct
- *   bit position after multiply and leave instruction count lower.
- *   |R|   |258   0  408| |Y' -  16|
- *   |G| = |149 -49 -104| |Cb - 128|
- *   |B|   |149 258    0| |Cr - 128|
- *
- * Write four RGB565 pixels in the following order on each loop:
- * 1 3 + > down
- * 2 4 \/ left
- *
- * Kernel pattern (raw|rotated|use order):
- * 5 3 4 2   2 6 3 7     row0   row2          > down
- * 1 7 0 6 | 4 0 5 1 | 2 4 6 0 3 5 7 1 col0     left
- * 4 2 5 3 | 3 7 2 6 | 3 5 7 1 2 4 6 0 col2  \/
- * 0 6 1 7   5 1 4 0
- */
-    .section    .icode, "ax", %progbits
-    .align      2
-    .global     lcd_write_yuv420_lines_odither
-    .type       lcd_write_yuv420_lines_odither, %function
-lcd_write_yuv420_lines_odither:
-                                        @ r0 = yuv_src
-                                        @ r1 = width
-                                        @ r2 = stride
-                                        @ r3 = x_screen
-                                        @ [sp] = y_screen
-    stmfd       sp!, { r4-r11, lr }     @ save non-scratch
-    ldmia       r0, { r4, r5, r6 }      @ r4 = yuv_src[0] = Y'_p
-                                        @ r5 = yuv_src[1] = Cb_p
-                                        @ r6 = yuv_src[2] = Cr_p
-                                        @
-    ldr         r14, [sp, #40]          @ Line up pattern and kernel quadrant
-    sub         r2, r2, #1              @ stride =- 1
-    eor         r14, r14, r3            @
-    and         r14, r14, #0x2          @
-    mov         r14, r14, lsl #6        @ 0x00 or 0x80
-
-    mov         r3, #0xC8000000         @
-    orr         r3, r3, #0x120000       @ r3 = DBOP_BASE, need to be redone
-                                        @ due to lack of registers
-10: @ loop line                         @
-                                        @
-    ldrb        r7, [r4], #1            @ r7 = *Y'_p++;
-    ldrb        r8, [r5], #1            @ r8 = *Cb_p++;
-    ldrb        r9, [r6], #1            @ r9 = *Cr_p++;
-                                        @
-    eor         r14, r14, #0x80         @ flip pattern quadrant
-                                        @
-    sub         r7, r7, #16             @ r7 = Y = (Y' - 16)*149
-    add         r12, r7, r7, asl #2     @
-    add         r12, r12, r12, asl #4   @
-    add         r7, r12, r7, asl #6     @
-                                        @
-    sub         r8, r8, #128            @ Cb -= 128
-    sub         r9, r9, #128            @ Cr -= 128
-                                        @
-    add         r10, r8, r8, asl #4     @ r10 = guv = Cr*104 + Cb*49
-    add         r10, r10, r8, asl #5    @
-    add         r10, r10, r9, asl #3    @
-    add         r10, r10, r9, asl #5    @
-    add         r10, r10, r9, asl #6    @
-                                        @
-    mov         r8, r8, asl #1          @ r8 = bu = Cb*258
-    add         r8, r8, r8, asl #7      @
-                                        @
-    add         r9, r9, r9, asl #1      @ r9 = rv = Cr*408
-    add         r9, r9, r9, asl #4      @
-    mov         r9, r9, asl #3          @
-                                        @
-                                        @ compute R, G, and B
-    add         r0, r8, r7              @ r0  = b' = Y + bu
-    add         r11, r9, r7, asl #1     @ r11 = r' = Y*2 + rv
-    rsb         r7, r10, r7             @ r7  = g' = Y + guv
-                                        @
-                                        @ r8 = bu, r9 = rv, r10 = guv
-                                        @
-    sub         r12, r0, r0, lsr #5     @ r0 = 31/32*b + b/256
-    add         r0, r12, r0, lsr #8     @
-                                        @
-    sub         r12, r11, r11, lsr #5   @ r11 = 31/32*r + r/256
-    add         r11, r12, r11, lsr #8   @
-                                        @
-    sub         r12, r7, r7, lsr #6     @ r7 = 63/64*g + g/256
-    add         r7, r12, r7, lsr #8     @
-                                        @
-    add         r12, r14, #0x100        @
-                                        @
-    add         r0, r0, r12             @ b = r0 + delta
-    add         r11, r11, r12, lsl #1   @ r = r11 + delta*2
-    add         r7, r7, r12, lsr #1     @ g = r7 + delta/2
-                                        @
-    orr         r12, r0, r11, asr #1    @ check if clamping is needed...
-    orr         r12, r12, r7            @ ...at all
-    movs        r12, r12, asr #15       @
-    beq         15f @ no clamp          @
-    movs        r12, r0, asr #15        @ clamp b
-    mvnne       r0, r12, lsr #15        @
-    andne       r0, r0, #0x7c00         @ mask b only if clamped
-    movs        r12, r11, asr #16       @ clamp r
-    mvnne       r11, r12, lsr #16       @
-    movs        r12, r7, asr #15        @ clamp g
-    mvnne       r7, r12, lsr #15        @
-15: @ no clamp                          @
-                                        @
-    ldrb        r12, [r4, r2]           @ r12 = Y' = *(Y'_p + stride)
-                                        @
-    and         r11, r11, #0xf800       @ pack pixel
-    and         r7, r7, #0x7e00         @ r0 = pixel = (r & 0xf800) |
-    orr         r11, r11, r7, lsr #4    @              ((g & 0x7e00) >> 4) |
-    orr         r3, r11, r0, lsr #10    @              (b >> 10)
-                                        @
-    mov         r7, r3, lsr #8          @
-    bic         r3, r3, #0xff00         @
-    orr         r3, r7, r3, lsl #8      @ swap pixel
-                                        @ save pixel
-    sub         r7, r12, #16            @ r7 = Y = (Y' - 16)*149
-    add         r12, r7, r7, asl #2     @
-    add         r12, r12, r12, asl #4   @
-    add         r7, r12, r7, asl #6     @
-                                        @ compute R, G, and B
-    add         r0, r8, r7              @ r0  = b' = Y + bu
-    add         r11, r9, r7, asl #1     @ r11 = r' = Y*2 + rv
-    rsb         r7, r10, r7             @ r7  = g' = Y + guv
-                                        @
-    sub         r12, r0, r0, lsr #5     @ r0  = 31/32*b' + b'/256
-    add         r0, r12, r0, lsr #8     @
-                                        @
-    sub         r12, r11, r11, lsr #5   @ r11 = 31/32*r' + r'/256
-    add         r11, r12, r11, lsr #8   @
-                                        @
-    sub         r12, r7, r7, lsr #6     @ r7  = 63/64*g' + g'/256
-    add         r7, r12, r7, lsr #8     @
-                                        @
-    add         r12, r14, #0x200        @
-                                        @
-    add         r0, r0, r12             @ b = r0 + delta
-    add         r11, r11, r12, lsl #1   @ r = r11 + delta*2
-    add         r7, r7, r12, lsr #1     @ g = r7 + delta/2
-                                        @
-    orr         r12, r0, r11, asr #1    @ check if clamping is needed...
-    orr         r12, r12, r7            @ ...at all
-    movs        r12, r12, asr #15       @
-    beq         15f @ no clamp          @
-    movs        r12, r0, asr #15        @ clamp b
-    mvnne       r0, r12, lsr #15        @
-    andne       r0, r0, #0x7c00         @ mask b only if clamped
-    movs        r12, r11, asr #16       @ clamp r
-    mvnne       r11, r12, lsr #16       @
-    movs        r12, r7, asr #15        @ clamp g
-    mvnne       r7, r12, lsr #15        @
-15: @ no clamp                          @
-                                        @
-    ldrb        r12, [r4], #1           @ r12 = Y' = *(Y'_p++)
-
-    and         r11, r11, #0xf800       @ pack pixel
-    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)
-                                        @
-    mov         r7, r0, lsr #8          @
-    bic         r0, r0, #0xff00         @
-    orr         r0, r7, r0, lsl #8      @ swap pixel
-    orr         r3, r3, r0, lsl#16      @ pack with 2nd pixel
-    mov         r0, #0xC8000000         @
-    orr         r0, r0, #0x120000       @ r3 = DBOP_BASE
-
-    str         r3, [r0, #0x10]         @ write pixel
-                                        @
-    sub         r7, r12, #16            @ r7 = Y = (Y' - 16)*149
-    add         r12, r7, r7, asl #2     @
-    add         r12, r12, r12, asl #4   @
-    add         r7, r12, r7, asl #6     @
-                                        @ compute R, G, and B
-    add         r0, r8, r7              @ r0  = b' = Y + bu
-    add         r11, r9, r7, asl #1     @ r11 = r' = Y*2 + rv
-    rsb         r7, r10, r7             @ r7  = g' = Y + guv
-                                        @
-                                        @ r8 = bu, r9 = rv, r10 = guv
-                                        @
-    sub         r12, r0, r0, lsr #5     @ r0  = 31/32*b' + b'/256
-    add         r0, r12, r0, lsr #8     @
-                                        @
-    sub         r12, r11, r11, lsr #5   @ r11 = 31/32*r' + r'/256
-    add         r11, r12, r11, lsr #8   @
-                                        @
-    sub         r12, r7, r7, lsr #6     @ r7  = 63/64*g' + g'/256
-    add         r7, r12, r7, lsr #8     @
-                                        @
-    add         r12, r14, #0x300        @
-                                        @
-    add         r0, r0, r12             @ b = r0 + delta
-    add         r11, r11, r12, lsl #1   @ r = r11 + delta*2
-    add         r7, r7, r12, lsr #1     @ g = r7 + delta/2
-                                        @
-    orr         r12, r0, r11, asr #1    @ check if clamping is needed...
-    orr         r12, r12, r7            @ ...at all
-    movs        r12, r12, asr #15       @
-    beq         15f @ no clamp          @
-    movs        r12, r0, asr #15        @ clamp b
-    mvnne       r0, r12, lsr #15        @
-    andne       r0, r0, #0x7c00         @ mask b only if clamped
-    movs        r12, r11, asr #16       @ clamp r
-    mvnne       r11, r12, lsr #16       @
-    movs        r12, r7, asr #15        @ clamp g
-    mvnne       r7, r12, lsr #15        @
-15: @ no clamp                          @
-                                        @
-    ldrb        r12, [r4, r2]           @ r12 = Y' = *(Y'_p + stride)
-                                        @
-    and         r11, r11, #0xf800       @ pack pixel
-    and         r7, r7, #0x7e00         @ r0 = pixel = (r & 0xf800) |
-    orr         r11, r11, r7, lsr #4    @              ((g & 0x7e00) >> 4) |
-    orr         r3, r11, r0, lsr #10    @              (b >> 10)
-                                        @
-    mov         r7, r3, lsr #8          @
-    bic         r3, r3, #0xff00         @
-    orr         r3, r7, r3, lsl #8      @ swap pixel
-                                        @ save pixel
-                                        @
-    sub         r7, r12, #16            @ r7 = Y = (Y' - 16)*149
-    add         r12, r7, r7, asl #2     @
-    add         r12, r12, r12, asl #4   @
-    add         r7, r12, r7, asl #6     @
-                                        @ compute R, G, and B
-    add         r0, r8, r7              @ r0  = b' = Y + bu
-    add         r11, r9, r7, asl #1     @ r11 = r' = Y*2 + rv
-    rsb         r7, r10, r7             @ r7  = g' = Y + guv
-                                        @
-    sub         r12, r0, r0, lsr #5     @ r0 = 31/32*b + b/256
-    add         r0, r12, r0, lsr #8     @
-                                        @
-    sub         r12, r11, r11, lsr #5   @ r11 = 31/32*r + r/256
-    add         r11, r12, r11, lsr #8   @
-                                        @
-    sub         r12, r7, r7, lsr #6     @ r7 = 63/64*g + g/256
-    add         r7, r12, r7, lsr #8     @
-                                        @
-    @ This element is zero - use r14    @
-                                        @
-    add         r0, r0, r14             @ b = r0 + delta
-    add         r11, r11, r14, lsl #1   @ r = r11 + delta*2
-    add         r7, r7, r14, lsr #1     @ g = r7 + delta/2
-                                        @
-    orr         r12, r0, r11, asr #1    @ check if clamping is needed...
-    orr         r12, r12, r7            @ ...at all
-    movs        r12, r12, asr #15       @
-    beq         15f @ no clamp          @
-    movs        r12, r0, asr #15        @ clamp b
-    mvnne       r0, r12, lsr #15        @
-    andne       r0, r0, #0x7c00         @ mask b only if clamped
-    movs        r12, r11, asr #16       @ clamp r
-    mvnne       r11, r12, lsr #16       @
-    movs        r12, r7, asr #15        @ clamp g
-    mvnne       r7, r12, lsr #15        @
-15: @ no clamp                          @
-                                        @
-    and         r11, r11, #0xf800       @ pack pixel
-    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)
-                                        @
-    mov         r7, r0, lsr #8          @
-    bic         r0, r0, #0xff00         @
-    orr         r0, r7, r0, lsl #8      @ swap pixel
-    orr         r3, r3, r0, lsl#16      @ pack with 2nd pixel
-    mov         r0, #0xC8000000         @
-    orr         r0, r0, #0x120000       @ r3 = DBOP_BASE
-
-    str         r3, [r0, #0x10]         @ write pixel
-                                        @
-    subs        r1, r1, #2              @ subtract block from width
-    bgt         10b @ loop line         @
-                                        @
-1: @ busy                               @
-    @ writing at max 110*32 its (LCD_WIDTH/2), the fifo is bigger
-    @ so polling fifo empty after the loops is save
-    ldr         r7, [r0,#0xc]           @ r7 = DBOP_STATUS
-    tst         r7, #DBOP_BUSY          @ fifo not empty?
-    beq         1b                      @
-
-    ldmfd       sp!, { r4-r11, pc }     @ restore registers and return
-    .ltorg                              @ dump constant pool
-    .size   lcd_write_yuv420_lines_odither, .-lcd_write_yuv420_lines_odither
diff --git a/firmware/target/arm/as3525/sansa-fuzev2/lcd-fuzev2.c b/firmware/target/arm/as3525/sansa-fuzev2/lcd-fuzev2.c
index 5fa26c0..d17149e 100644
--- a/firmware/target/arm/as3525/sansa-fuzev2/lcd-fuzev2.c
+++ b/firmware/target/arm/as3525/sansa-fuzev2/lcd-fuzev2.c
@@ -29,6 +29,7 @@
 #include "debug.h"
 #include "system.h"
 #include "clock-target.h"
+#include "dbop-as3525.h"
 
 /* The controller is unknown, but some registers appear to be the same as the
    HD66789R */
@@ -113,67 +114,13 @@ static void as3525_dbop_init(void)
     DBOP_TIMPOL_23 = 0xA12FE037;
 }
 
-static inline void dbop_set_mode(int mode)
-{
-    unsigned long ctrl = DBOP_CTRL;
-    int words = (ctrl >> 13) & 3; // bits 14:13
-    if (mode == 32 && words != 2)
-        DBOP_CTRL = (ctrl & ~(1<<13)) | (1<<14); // 4 serial words
-    else if (mode == 16 && words != 1)
-        DBOP_CTRL = (ctrl & ~(1<<14)) | (1<<13); // 2 serial words
-    else
-        return;
-    lcd_delay(10);
-}
-
-static void dbop_write_data(const int16_t* p_bytes, int count)
-{
-    const int32_t *data;
-    if ((intptr_t)p_bytes & 0x3 || count == 1)
-    {   /* need to do a single 16bit write beforehand if the address is
-         * not word aligned or count is 1, switch to 16bit mode if needed */
-        dbop_set_mode(16);
-        DBOP_DOUT16 = *p_bytes++;
-        if (!(--count))
-            return;
-    }
-    /* from here, 32bit transfers are save
-     * set it to transfer 4*(outputwidth) units at a time,
-     * if bit 12 is set it only does 2 halfwords though (we never set it)
-     * switch to 32bit output if needed */
-    dbop_set_mode(32);
-    data = (int32_t*)p_bytes;
-
-    while (count > 1)
-    {
-        DBOP_DOUT32 = *data++;
-        count -= 2;
-
-        /* Wait if push fifo is full */
-        while ((DBOP_STAT & (1<<6)) != 0);
-    }
-    /* While push fifo is not empty */
-    while ((DBOP_STAT & (1<<10)) == 0);
-
-    /* due to the 32bit alignment requirement or uneven count,
-     * we possibly need to do a 16bit transfer at the end also */
-    if (count > 0)
-        dbop_write_data((int16_t*)data, 1);
-}
 
 static void lcd_write_cmd(unsigned short cmd)
 {
-    volatile int i;
-    lcd_delay(0x20);
-
-    DBOP_CTRL |= 1<<13;
-    DBOP_CTRL &= ~(1<<14);  // 2 serial words
-    DBOP_CTRL &= ~(1<<12);  // 8 bit data width
+    unsigned short data = swap16(cmd);
     DBOP_TIMPOL_23 = 0xA12F0036;
-    DBOP_DOUT = swap16(cmd);
-
-    while ((DBOP_STAT & (1<<10)) == 0);
-    for(i=0;i<0x20;i++) asm volatile ("nop\n");
+    dbop_write_data(&data, 1);
+    lcd_delay(32);
     DBOP_TIMPOL_23 = 0xA12FE037;
 }
 
@@ -403,8 +350,6 @@ void lcd_blit_yuv(unsigned char * const src[3],
             lcd_window_y(y, y + 1);
 
             lcd_write_cmd(R_WRITE_DATA_2_GRAM);
-
-            dbop_set_mode(32);
             lcd_write_yuv420_lines_odither(yuv_src, width, stride, x, y);
             yuv_src[0] += stride << 1; /* Skip down two luma lines */
             yuv_src[1] += stride >> 1; /* Skip down one chroma line */
@@ -420,8 +365,6 @@ void lcd_blit_yuv(unsigned char * const src[3],
             lcd_window_y(y, y + 1);
 
             lcd_write_cmd(R_WRITE_DATA_2_GRAM);
-
-            dbop_set_mode(32);
             lcd_write_yuv420_lines(yuv_src, width, stride);
             yuv_src[0] += stride << 1; /* Skip down two luma lines */
             yuv_src[1] += stride >> 1; /* Skip down one chroma line */