3 files changed, 18 insertions, 7 deletions

diff --git a/firmware/crt0.S b/firmware/crt0.S
index 7ab7e2cbd8..df2ff5884d 100644
--- a/firmware/crt0.S
+++ b/firmware/crt0.S
@@ -434,7 +434,7 @@ irq_handler:
     /* Chip select 1 - LCD controller */
     move.l  #0xf0000000,%d0 /* CSAR1 - Base = 0xf0000000 */
     move.l  %d0,(0x08c,%a0)
-    moveq.l #0x75,%d0      /* CSMR1 - 64K, Only data access */
+    moveq.l #0x1,%d0      /* CSMR1 - 64K */
     move.l  %d0,(0x090,%a0)
     move.l  #0x00000180,%d0 /* CSCR1 - no wait states, 16 bits, no bursts */
     move.l  %d0,(0x094,%a0)
diff --git a/firmware/drivers/lcd-16bit.c b/firmware/drivers/lcd-16bit.c
index 582050c878..6337fa1ea7 100644
--- a/firmware/drivers/lcd-16bit.c
+++ b/firmware/drivers/lcd-16bit.c
@@ -36,7 +36,7 @@
 #define SCROLLABLE_LINES 26
 
 /*** globals ***/
-fb_data lcd_framebuffer[LCD_HEIGHT][LCD_WIDTH] __attribute__ ((aligned (4)));
+fb_data lcd_framebuffer[LCD_HEIGHT][LCD_WIDTH] __attribute__ ((aligned (16)));
 
 static unsigned fg_pattern IDATA_ATTR = LCD_DEFAULT_FG;
 static unsigned bg_pattern IDATA_ATTR = LCD_DEFAULT_BG;
diff --git a/firmware/drivers/lcd-h300.c b/firmware/drivers/lcd-h300.c
index bb2f9ec777..149062be96 100644
--- a/firmware/drivers/lcd-h300.c
+++ b/firmware/drivers/lcd-h300.c
@@ -89,12 +89,20 @@ inline void lcd_begin_write_gram(void)
     *(volatile unsigned short *)0xf0000000 = R_WRITE_DATA_2_GRAM;
 }
 
-/* called very frequently - inline! */
-inline void lcd_write_data(const unsigned short* p_bytes, int count) ICODE_ATTR;
-inline void lcd_write_data(const unsigned short* p_bytes, int count)
+void lcd_write_data(const unsigned short* p_bytes, int count) ICODE_ATTR;
+void lcd_write_data(const unsigned short* p_bytes, int count)
 {
-    while(count--)
-        *(volatile unsigned short *)0xf0000002 = *p_bytes++;
+    SAR1 = (unsigned long)p_bytes; /* Destination address */
+    while(count)
+    {
+        int cnt = MIN(count, 256);
+        DAR1 = (unsigned long)0xf0000002; /* Destination address */
+        BCR1 = cnt*2;          /* Bytes to transfer */
+        DCR1 = 0x02000000 | DMA_SINC | (2 << 20) | (2 << 17) | DMA_START;
+        while(!(DSR1 & 1)) {};
+        DSR1 = 1;
+        count -= cnt;
+    }
 }
 
 /*** hardware configuration ***/
@@ -131,6 +139,9 @@ void lcd_roll(int lines)
 /* LCD init */
 void lcd_init_device(void)
 {
+    MPARK = 0x81;    /* PARK[1,0]=10 + BCR24BIT */
+    DSR1 = 1;
+
     /* GPO46 is LCD RESET */
     or_l(0x00004000, &GPIO1_OUT);
     or_l(0x00004000, &GPIO1_ENABLE);