2 files changed, 54 insertions, 1 deletions

diff --git a/firmware/drivers/lcd.S b/firmware/drivers/lcd.S
index 3ab993c1d0..c082637f3e 100755
--- a/firmware/drivers/lcd.S
+++ b/firmware/drivers/lcd.S
@@ -19,8 +19,9 @@
  ****************************************************************************/
 
 #include "config.h"
-#include "sh7034.h"
+#include "cpu.h"
 
+#if CONFIG_CPU == SH7034
 #define LCDR (PBDR_ADDR+1)
 
 #ifdef HAVE_LCD_CHARCELLS
@@ -304,4 +305,55 @@ _lcd_write_data:
 
 .end:
     .size   _lcd_write_command,.end-_lcd_write_command
+#elif CONFIG_CPU == MCF5249
+    .section    .icode,"ax",@progbits
+
+    .align      2
+    .global     lcd_write_command
+    .type       lcd_write_command,@function
+
+lcd_write_command:
+        move.l  (4,%sp),%d0
+        lea MBAR2,%a1
+        move.l   #~8,%d1
+        and.l   %d1,(0xb4,%a1)
+        move.w  %d0,0xf0000000
+        rts
+
+    .align      2
+    .global     lcd_write_command_ex
+    .type       lcd_write_command_ex,@function
 
+lcd_write_command_ex:
+        lea MBAR2,%a1
+        
+        move.l  (4,%sp),%d0     /* Command */
+
+        move.l   #~8,%d1        /* Set A0 = 0 */
+        and.l   %d1,(0xb4,%a1)
+        move.w  %d0,0xf0000000  /* Write to LCD */
+
+        move.l  (8,%sp),%d0     /* Data */
+
+        not.l   %d1             /* Set A0 = 1 */
+        or.l    %d1,(0xb4,%a1)
+        move.w  %d0,0xf0000000  /* Write to LCD */
+        rts
+
+    .align      2
+    .global     lcd_write_data
+    .type       lcd_write_data,@function
+
+lcd_write_data:
+        move.l  (4,%sp),%a0       /* Data pointer */
+        move.l  (8,%sp),%d0     /* Length */
+        lea MBAR2,%a1
+        moveq   #8,%d1
+        or.l    %d1,(0xb4,%a1)
+.loop:
+        move.b  (%a0)+,%d1
+        move.w  %d1,0xf0000000
+        subq.l  #1,%d0
+        bne     .loop
+        rts
+#endif
diff --git a/firmware/export/lcd.h b/firmware/export/lcd.h
index b78474719a..6809aaf4ae 100644
--- a/firmware/export/lcd.h
+++ b/firmware/export/lcd.h
@@ -44,6 +44,7 @@ extern void lcd_scroll_speed( int speed );
 extern void lcd_scroll_delay( int ms );
 extern void lcd_set_contrast(int val);
 extern void lcd_write_command( int byte );
+extern void lcd_write_command_ex( int cmd, unsigned char data );
 extern void lcd_write_data( const unsigned char* p_bytes, int count );
 extern int  lcd_default_contrast(void);