nios2: Library functions

Add optimised library functions for nios2.

Signed-off-by: Ley Foon Tan <lftan@altera.com>

1 files changed, 81 insertions, 0 deletions

diff --git a/arch/nios2/lib/memset.c b/arch/nios2/lib/memset.c
new file mode 100644
index 0000000..65e9780
--- /dev/null
+++ b/arch/nios2/lib/memset.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2011 Tobias Klauser <tklauser@distanz.ch>
+ * Copyright (C) 2004 Microtronix Datacom Ltd
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#include <linux/types.h>
+#include <linux/string.h>
+
+#ifdef __HAVE_ARCH_MEMSET
+void *memset(void *s, int c, size_t count)
+{
+	int destptr, charcnt, dwordcnt, fill8reg, wrkrega;
+
+	if (!count)
+		return s;
+
+	c &= 0xFF;
+
+	if (count <= 8) {
+		char *xs = (char *) s;
+
+		while (count--)
+			*xs++ = c;
+		return s;
+	}
+
+	__asm__ __volatile__ (
+		/* fill8 %3, %5 (c & 0xff) */
+		"	slli	%4, %5, 8\n"
+		"	or	%4, %4, %5\n"
+		"	slli    %3, %4, 16\n"
+		"	or	%3, %3, %4\n"
+		/* Word-align %0 (s) if necessary */
+		"	andi	%4, %0, 0x01\n"
+		"	beq	%4, zero, 1f\n"
+		"	addi	%1, %1, -1\n"
+		"	stb	%3, 0(%0)\n"
+		"	addi	%0, %0, 1\n"
+		"1:	mov	%2, %1\n"
+		/* Dword-align %0 (s) if necessary */
+		"	andi	%4, %0, 0x02\n"
+		"	beq	%4, zero, 2f\n"
+		"	addi	%1, %1, -2\n"
+		"	sth	%3, 0(%0)\n"
+		"	addi	%0, %0, 2\n"
+		"	mov	%2, %1\n"
+		/* %1 and %2 are how many more bytes to set */
+		"2:	srli	%2, %2, 2\n"
+		/* %2 is how many dwords to set */
+		"3:	stw	%3, 0(%0)\n"
+		"	addi	%0, %0, 4\n"
+		"	addi    %2, %2, -1\n"
+		"	bne	%2, zero, 3b\n"
+		/* store residual word and/or byte if necessary */
+		"	andi	%4, %1, 0x02\n"
+		"	beq	%4, zero, 4f\n"
+		"	sth	%3, 0(%0)\n"
+		"	addi	%0, %0, 2\n"
+		/* store residual byte if necessary */
+		"4:	andi	%4, %1, 0x01\n"
+		"	beq	%4, zero, 5f\n"
+		"	stb	%3, 0(%0)\n"
+		"5:\n"
+		: "=r" (destptr),	/* %0  Output */
+		  "=r" (charcnt),	/* %1  Output */
+		  "=r" (dwordcnt),	/* %2  Output */
+		  "=r" (fill8reg),	/* %3  Output */
+		  "=r" (wrkrega)	/* %4  Output */
+		: "r" (c),		/* %5  Input */
+		  "0" (s),		/* %0  Input/Output */
+		  "1" (count)		/* %1  Input/Output */
+		: "memory"		/* clobbered */
+	);
+
+	return s;
+}
+#endif /* __HAVE_ARCH_MEMSET */