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 */