2 files changed, 49 insertions, 0 deletions

diff --git a/firmware/common/memcpy.S b/firmware/common/memcpy.S
index 2fb9f6a5a7..e129b99442 100644
--- a/firmware/common/memcpy.S
+++ b/firmware/common/memcpy.S
@@ -16,9 +16,11 @@
  * KIND, either express or implied.
  *
  ****************************************************************************/
+#include "config.h"
 
     .section    .icode,"ax",@progbits
 
+#if CONFIG_CPU == SH7034
     .align      2
     .global     _memcpy
     .type       _memcpy,@function
@@ -168,4 +170,27 @@ _memcpy:
     mov     r7,r0       /* return dest start address */
 .end:
     .size   _memcpy,.end-_memcpy
+#elif CONFIG_CPU == MCF5249
+    .align      2
+    .global     memcpy
+    .type       memcpy,@function
 
+/* Copies <length> bytes of data in memory from <source> to <dest>
+ * This version is not optimized at all
+ */
+memcpy:
+        move.l  (4,%sp),%a1    /* Destination */
+        move.l  (8,%sp),%a0     /* Source */
+        move.l  (12,%sp),%d1    /* Length */
+
+        cmp.l   #0,%d1
+        bra.b   .byteloopend
+
+.byteloop:
+        move.b  (%a0)+,(%a1)+
+        subq.l  #1,%d1
+.byteloopend:
+        bne.b   .byteloop
+
+        rts
+#endif
diff --git a/firmware/common/memset.S b/firmware/common/memset.S
index 038915c475..bce8936089 100644
--- a/firmware/common/memset.S
+++ b/firmware/common/memset.S
@@ -16,10 +16,12 @@
  * KIND, either express or implied.
  *
  ****************************************************************************/
+#include "config.h"
 
     .section    .icode,"ax",@progbits
 
     .align      2
+#if CONFIG_CPU == SH7034        
     .global     _memset
     .type       _memset,@function
 
@@ -105,4 +107,26 @@ _memset:
 
 .end:
     .size   _memset,.end-_memset
+#elif CONFIG_CPU == MCF5249
+    .global     memset
+    .type       memset,@function
 
+/* Fills a memory region with specified byte value
+ * This version is not optimized at all
+ */
+memset:
+        move.l  (4,%sp),%a0     /* Start address */
+        move.l  (8,%sp),%d0    /* Value */
+        move.l  (12,%sp),%d1    /* Length */
+        lea.l   (%d1,%a0),%a1 /* a1 = a0+d1 */
+
+        bra.b   .byteloopend
+        
+.byteloop:
+        move.b  %d0,(%a0)+
+.byteloopend:
+        cmp.l   %a0,%a1
+        bne.b   .byteloop
+        
+        rts
+#endif