1/*
2 * Copyright (c) 2020, Arm Limited. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7#include <asm_macros.S>
8
9	.syntax unified
10	.global	memset
11
12/* -----------------------------------------------------------------------
13 * void *memset(void *dst, int val, size_t count)
14 *
15 * Copy the value of 'val' (converted to an unsigned char) into
16 * each of the first 'count' characters of the object pointed to by 'dst'.
17 *
18 * Returns the value of 'dst'.
19 * -----------------------------------------------------------------------
20 */
21func memset
22	mov	r12, r0			/* keep r0 */
23	tst	r0, #3
24	beq	aligned			/* 4-bytes aligned */
25
26	/* Unaligned 'dst' */
27unaligned:
28	subs	r2, r2, #1
29	strbhs	r1, [r12], #1
30	bxls	lr			/* return if 0 */
31	tst	r12, #3
32	bne	unaligned		/* continue while unaligned */
33
34	/* 4-bytes aligned */
35aligned:bfi	r1, r1, #8, #8		/* propagate 'val' */
36	bfi	r1, r1, #16, #16
37
38	mov	r3, r1
39
40	cmp	r2, #16
41	blo	less_16			/* < 16 */
42
43	push	{r4, lr}
44	mov	r4, r1
45	mov	lr, r1
46
47write_32:
48	subs	r2, r2, #32
49	stmiahs	r12!, {r1, r3, r4, lr}
50	stmiahs	r12!, {r1, r3, r4, lr}
51	bhi	write_32		/* write 32 bytes in a loop */
52	popeq	{r4, pc}		/* return if 0 */
53	lsls	r2, r2, #28		/* C = r2[4]; N = r2[3]; Z = r2[3:0] */
54	stmiacs	r12!, {r1, r3, r4, lr}	/* write 16 bytes */
55	popeq	{r4, pc}		/* return if 16 */
56	stmiami	r12!, {r1, r3}		/* write 8 bytes */
57	lsls	r2, r2, #2		/* C = r2[2]; N = r2[1]; Z = r2[1:0] */
58	strcs	r1, [r12], #4		/* write 4 bytes */
59	popeq	{r4, pc}		/* return if 8 or 4 */
60	strhmi	r1, [r12], #2		/* write 2 bytes */
61	lsls	r2, r2, #1		/* N = Z = r2[0] */
62	strbmi	r1, [r12]		/* write 1 byte */
63	pop	{r4, pc}
64
65less_16:lsls	r2, r2, #29		/* C = r2[3]; N = r2[2]; Z = r2[2:0] */
66	stmiacs	r12!, {r1, r3}		/* write 8 bytes */
67	bxeq	lr			/* return if 8 */
68	strmi	r1, [r12], #4		/* write 4 bytes */
69	lsls	r2, r2, #2		/* C = r2[1]; N = Z = r2[0] */
70	strhcs	r1, [r12], #2		/* write 2 bytes */
71	strbmi	r1, [r12]		/* write 1 byte */
72	bx	lr
73
74endfunc memset
75