2 files changed, 99 insertions, 1 deletions

diff --git a/firmware/common/strlen.c b/firmware/common/strlen.c
index 4249e14c78..932567c181 100644
--- a/firmware/common/strlen.c
+++ b/firmware/common/strlen.c
@@ -1,4 +1,4 @@
-/* 
+/*
 FUNCTION
         <<strlen>>---character string length
         
@@ -55,6 +55,10 @@ QUICKREF
 
 size_t
 _DEFUN (strlen, (str),
+        _CONST char *str) __attribute__ ((section (".icode")));
+
+size_t
+_DEFUN (strlen, (str),
         _CONST char *str)
 {
 #if defined(PREFER_SIZE_OVER_SPEED) || defined(__OPTIMIZE_SIZE__)
diff --git a/firmware/common/strlen_a.S b/firmware/common/strlen_a.S
new file mode 100755
index 0000000000..34837605ac
--- /dev/null
+++ b/firmware/common/strlen_a.S
@@ -0,0 +1,94 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2005 by Jens Arnold
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include "config.h"
+
+    .section    .icode,"ax",@progbits
+
+    .align      2
+    .global     _strlen
+    .type       _strlen,@function
+
+/* Works out the length of a string
+ * This version is optimized for speed
+ *
+ * arguments:
+ *  r4 - start address
+ *
+ * return value:
+ *  r0 - string length
+ *
+ * register usage:
+ *  r0 - current address
+ *  r1 - current value (byte/long)
+ *  r2 - mask for alignment / zero (for cmp/str)
+ *  r4 - start address
+ *
+ */
+
+_strlen:
+    mov     r4,r0       /* r0 = start address */
+    tst     #3,r0       /* long aligned? */
+    bt      .start_l    /* yes, jump directly to the longword loop */
+
+    /* not long aligned: check the first 3 bytes */
+    mov.b   @r0+,r1     /* fetch first byte */
+    tst     r1,r1       /* byte == 0 ? */
+    bt      .hitzero    /* yes, string end found */
+    mov.b   @r0+,r1     /* fetch second byte */
+    mov     #3,r2       /* prepare mask: r2 = 0..00000011b */
+    tst     r1,r1       /* byte == 0 ? */
+    bt      .hitzero    /* yes, string end found */
+    mov.b   @r0+,r1     /* fetch third byte */
+    not     r2,r2       /* prepare mask: r2 = 1..11111100b */
+    tst     r1,r1       /* byte == 0 ? */
+    bt      .hitzero    /* yes, string end found */
+
+    /* not yet found, fall through into longword loop */
+    and     r2,r0       /* align down to long bound */
+
+    /* main loop: check longwords */
+.start_l:
+    mov     #0,r2       /* zero longword for cmp/str */
+.loop_l:
+    mov.l   @r0+,r1     /* fetch long word */
+    cmp/str r1,r2       /* any zero byte within? */
+    bf      .loop_l     /* no, loop */
+    add     #-4,r0      /* set address back to start of this longword */
+
+    /* the last longword contains the string end: figure out the byte */
+    mov.b   @r0+,r1     /* fetch first byte */    
+    tst     r1,r1       /* byte == 0 ? */
+    bt      .hitzero    /* yes, string end found */
+    mov.b   @r0+,r1     /* fetch second byte */
+    tst     r1,r1       /* byte == 0 ? */
+    bt      .hitzero    /* yes, string end found */
+    mov.b   @r0+,r1     /* fetch third byte */
+    tst     r1,r1       /* byte == 0 ? */
+    bt      .hitzero    /* yes, string end found */
+    rts                 /* must be the fourth byte */
+    sub     r4,r0       /* len = string_end - string_start */
+
+.hitzero:
+    add     #-1,r0      /* undo address increment */
+    rts
+    sub     r4,r0       /* len = string_end - string_start */
+
+.end:
+    .size   _strlen,.end-_strlen
+