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
45
46
47
48
49
50
51
52
53
diff --git a/firmware/target/arm/lcd-c200_c200v2.c b/firmware/target/arm/lcd-c200_c200v2.c
index b3e8f0e..39b1509 100644
--- a/firmware/target/arm/lcd-c200_c200v2.c
+++ b/firmware/target/arm/lcd-c200_c200v2.c
@@ -78,15 +78,19 @@ static inline void lcd_wait_write(void)
 }

 /* send LCD data */
-static void lcd_send_data(const fb_data *data, int width)
+static void lcd_send_pixel(const fb_data data)
 {
-    while(width--)
-    {
-        lcd_wait_write();
-        LCD1_DATA = *data >> 8;
-        lcd_wait_write();
-        LCD1_DATA = *data++ & 0xff;
-    }
+    lcd_wait_write();
+    LCD1_DATA = data >> 8;
+    lcd_wait_write();
+    LCD1_DATA = data & 0xff;
+}
+
+static inline void lcd_send_data(const fb_data *data, int width)
+{
+    do {
+        lcd_send_pixel(*data++);
+    } while(--width);
 }

 /* send LCD command */
@@ -131,12 +135,16 @@ static inline void lcd_delay(int delay)
 /* send LCD data */
 static void lcd_send_data(const fb_data *data, int width)
 {
-    while(width--)
-    {
+    do {
         DBOP_DOUT = *data << 8 | *data >> 8;
         data++;
-        while ((DBOP_STAT & (1<<10)) == 0);
-    }
+
+        /* Wait if push fifo is full */
+        while ((DBOP_STAT & (1<<6)) != 0);
+    } while(--width);
+
+    /* While push fifo is not empty */
+    while ((DBOP_STAT & (1<<10)) == 0);
 }

 /* send LCD command */