1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
diff --git a/firmware/target/arm/lcd-c200_c200v2.c b/firmware/target/arm/lcd-c200_c200v2.c
index b3e8f0e..92f6ce6 100644
--- a/firmware/target/arm/lcd-c200_c200v2.c
+++ b/firmware/target/arm/lcd-c200_c200v2.c
@@ -80,13 +80,36 @@ static inline void lcd_wait_write(void)
 /* send LCD data */
 static void lcd_send_data(const fb_data *data, int width)
 {
-    while(width--)
-    {
+#if 0
+    do {
         lcd_wait_write();
         LCD1_DATA = *data >> 8;
         lcd_wait_write();
         LCD1_DATA = *data++ & 0xff;
-    }
+    } while(--width);
+#else
+asm volatile(
+    "mov     r3, #0x70000000    @ LCD1_BASE         \n"
+    "add     r2, r3, #0x3000    @ LCD1_CONTROL      \n"
+
+ "1: ldr     r3, [r2]                               \n"
+    "tst     r3, #0x8000        @ LCD1_BUSY_MASK    \n"
+    "bne     1b                                     \n"
+
+    "ldrh    r4, [%0], #2       @ load pixel        \n"
+
+    "mov     r3, r4, lsr #8                         \n"
+    "strb    r3, [r2, #0x10]    @ LCD1_DATA         \n"
+
+ "2: ldr     r3, [r2]                               \n"
+    "tst     r3, #0x8000        @ LCD1_BUSY_MASK    \n"
+    "bne     2b                                     \n"
+
+    "strb    r4, [r2, #0x10]    @ LCD1_DATA         \n"
+    "subs    %1, %1, #1                             \n"
+    "bne     1b                                     \n"
+    : : "r"(data), "r"(width) :   "r4" );
+#endif
 }

 /* send LCD command */