/* $NetBSD: copyinout.S,v 1.16 2021/02/11 08:35:11 ryo Exp $ */

/*-
 * Copyright (c) 2014 The NetBSD Foundation, Inc.
 * All rights reserved.
 *
 * This code is derived from software contributed to The NetBSD Foundation
 * by Matt Thomas of 3am Software Foundry.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "opt_gprof.h"
#include <sys/errno.h>
#include <aarch64/asm.h>
#include "assym.h"

RCSID("$NetBSD: copyinout.S,v 1.16 2021/02/11 08:35:11 ryo Exp $");

#ifdef ARMV81_PAN
#define PAN_ENABLE	\
	adrl	x9, _C_LABEL(aarch64_pan_enabled)	; \
	ldr	w9, [x9]				; \
	cbz	w9, 666f				; \
	msr	pan, #1					; \
666:
#define PAN_DISABLE	\
	adrl	x9, _C_LABEL(aarch64_pan_enabled)	; \
	ldr	w9, [x9]				; \
	cbz	w9, 666f				; \
	msr	pan, #0					; \
666:
#else
#define PAN_ENABLE	/* nothing */
#define PAN_DISABLE	/* nothing */
#endif

	ARMV8_DEFINE_OPTIONS

	.macro enter_cpu_onfault
	stp	fp, lr, [sp, #-16]!	/* save fp, lr */
	mov	fp, sp			/* link frame pointer */

	stp	x19, x20, [sp, #-16]!	/* save x19, x20 */
	mov	x19, x0			/* x19 = arg0 */
	mov	x20, x1			/* x20 = arg1 */

	/* if (cpu_set_onfault(fb) != 0) return -1 */
	sub	sp, sp, #FB_T_SIZE	/* allocate struct faultbuf */
	mov	x0, sp			/* x0 = faultbuf */
	stp	x2, x3, [sp, #-16]!	/* save x2, x3 */
	bl	cpu_set_onfault
	ldp	x2, x3, [sp], #16	/* restore x2, x3 */
	mov	x8, x0			/* x8 = cpu_set_onfault() */
	cbnz	x0, 9f			/* return if error */

	mov	x0, x19			/* x0 = x19 = arg0 */
	mov	x1, x20			/* x1 = x20 = arg1 */

	PAN_DISABLE			/* disable PAN */
	.endm

	.macro exit_cpu_onfault
	/* curlwp->l_md.md_onfault = NULL */
	mrs	x0, tpidr_el1			/* x0 = curlwp */
	str	xzr, [x0, #L_MD_ONFAULT]	/* lwp->l_md_onfault = NULL */
9:
	PAN_ENABLE				/* enable PAN */
	add	sp, sp, #FB_T_SIZE		/* pop stack */
	ldp	x19, x20, [sp], #16		/* restore x19, x20 */
	ldp	fp, lr, [sp], #16		/* restore fp, lr */
	mov	x0, x8				/* x0 = return value */
	.endm


/* LINTSTUB: int copyin(const void *uaddr, void *kaddr, size_t len); */

ENTRY(copyin)
	enter_cpu_onfault

	/*
	 * How much to copy?  Less than 32 bytes?  ...
	 */
	lsr	x4, x2, #5			/* x4 = len / 32 */
	cbz	x4, .Lcopyin_last_suboword

	/*
	 * Subtract the amount of data we copied to get a octword aligned
	 * from the length: length -= 32 - offset -> length += offset - 32;
	 */
	and	x5, x0, #31
	cbz	x5, .Lcopyin_loop

	add	x2, x2, x5
	sub	x2, x2, #32
	lsr	x4, x2, #5			/* update x4 = len / 32 */

	/* XXX: not considered alignment of destination */
	tbz	x0, #0, .Lcopyin_hword_aligned
	ldtrb	w5, [x0]
	add	x0, x0, #1
	strb	w5, [x1], #1
.Lcopyin_hword_aligned:
	tbz	x0, #1, .Lcopyin_word_aligned
	ldtrh	w5, [x0]
	add	x0, x0, #2
	strh	w5, [x1], #2
.Lcopyin_word_aligned:
	tbz	x0, #2, .Lcopyin_dword_aligned
	ldtr	w5, [x0]
	add	x0, x0, #4
	str	w5, [x1], #4
.Lcopyin_dword_aligned:
	tbz	x0, #3, .Lcopyin_qword_aligned
	ldtr	x5, [x0]
	add	x0, x0, #8
	str	x5, [x1], #8
.Lcopyin_qword_aligned:
	tbz	x0, #4, .Lcopyin_loop0
	ldtr	x5, [x0, #0]
	ldtr	x6, [x0, #8]
	add	x0, x0, #16
	stp	x5, x6, [x1], #16
.Lcopyin_loop0:
	cbz	x4, .Lcopyin_last_suboword
.Lcopyin_loop:
	ldtr	x5, [x0, #0]
	ldtr	x6, [x0, #8]
	ldtr	x7, [x0, #16]
	ldtr	x8, [x0, #24]
	add	x0, x0, #32
	stp	x5, x6, [x1], #16
	stp	x7, x8, [x1], #16
	sub	x4, x4, #1
	cbnz	x4, .Lcopyin_loop
.Lcopyin_last_suboword:
	tbz	x2, #4, .Lcopyin_last_subqword
	ldtr	x5, [x0, #0]
	ldtr	x6, [x0, #8]
	add	x0, x0, #16
	stp	x5, x6, [x1], #16
.Lcopyin_last_subqword:
	tbz	x2, #3, .Lcopyin_last_subdword
	ldtr	x5, [x0]
	add	x0, x0, #8
	str	x5, [x1], #8
.Lcopyin_last_subdword:
	tbz	x2, #2, .Lcopyin_last_subword
	ldtr	w5, [x0]
	add	x0, x0, #4
	str	w5, [x1], #4
.Lcopyin_last_subword:
	tbz	x2, #1, .Lcopyin_last_subhword
	ldtrh	w5, [x0]
	add	x0, x0, #2
	strh	w5, [x1], #2
.Lcopyin_last_subhword:
	tbz	x2, #0, .Lcopyin_done
	ldtrb	w5, [x0]
	strb	w5, [x1]
.Lcopyin_done:
	mov	x8, #0

	exit_cpu_onfault
	ret
END(copyin)


/* LINTSTUB: int copyout(const void *kaddr, void *uaddr, size_t len); */

ENTRY(copyout)
	enter_cpu_onfault

	/*
	 * How much to copy?  Less than 32 bytes?  ...
	 */
	lsr	x4, x2, #5			/* x4 = len / 32 */
	cbz	x4, .Lcopyout_last_suboword

	/*
	 * Subtract the amount of data we copied to get a octword aligned
	 * from the length: length -= 32 - offset -> length += offset - 32;
	 */
	and	x5, x0, #31
	cbz	x5, .Lcopyout_loop

	add	x2, x2, x5
	sub	x2, x2, #32
	lsr	x4, x2, #5			/* update x4 = len / 32 */

	/* XXX: not considered alignment of destination */
	tbz	x0, #0, .Lcopyout_hword_aligned
	ldrb	w5, [x0], #1
	sttrb	w5, [x1]
	add	x1, x1, #1
.Lcopyout_hword_aligned:
	tbz	x0, #1, .Lcopyout_word_aligned
	ldrh	w5, [x0], #2
	sttrh	w5, [x1]
	add	x1, x1, #2
.Lcopyout_word_aligned:
	tbz	x0, #2, .Lcopyout_dword_aligned
	ldr	w5, [x0], #4
	sttr	w5, [x1]
	add	x1, x1, #4
.Lcopyout_dword_aligned:
	tbz	x0, #3, .Lcopyout_qword_aligned
	ldr	x5, [x0], #8
	sttr	x5, [x1]
	add	x1, x1, #8
.Lcopyout_qword_aligned:
	tbz	x0, #4, .Lcopyout_loop0
	ldp	x5, x6, [x0], #16
	sttr	x5, [x1, #0]
	sttr	x6, [x1, #8]
	add	x1, x1, #16
.Lcopyout_loop0:
	cbz	x4, .Lcopyout_last_suboword
.Lcopyout_loop:
	ldp	x5, x6, [x0], #16
	ldp	x7, x8, [x0], #16
	sttr	x5, [x1, #0]
	sttr	x6, [x1, #8]
	sttr	x7, [x1, #16]
	sttr	x8, [x1, #24]
	add	x1, x1, #32
	sub	x4, x4, #1
	cbnz	x4, .Lcopyout_loop
.Lcopyout_last_suboword:
	tbz	x2, #4, .Lcopyout_last_subqword
	ldp	x5, x6, [x0], #16
	sttr	x5, [x1, #0]
	sttr	x6, [x1, #8]
	add	x1, x1, #16
.Lcopyout_last_subqword:
	tbz	x2, #3, .Lcopyout_last_subdword
	ldr	x5, [x0], #8
	sttr	x5, [x1]
	add	x1, x1, #8
.Lcopyout_last_subdword:
	tbz	x2, #2, .Lcopyout_last_subword
	ldr	w5, [x0], #4
	sttr	w5, [x1]
	add	x1, x1, #4
.Lcopyout_last_subword:
	tbz	x2, #1, .Lcopyout_last_subhword
	ldrh	w5, [x0], #2
	sttrh	w5, [x1]
	add	x1, x1, #2
.Lcopyout_last_subhword:
	tbz	x2, #0, .Lcopyout_done
	ldrb	w5, [x0]
	sttrb	w5, [x1]
.Lcopyout_done:
	mov	x8, #0

	exit_cpu_onfault
	ret
END(copyout)


/* LINTSTUB: int copyinstr(const void *uaddr, void *kaddr, size_t len, size_t *done); */

ENTRY(copyinstr)
	enter_cpu_onfault

	mov	x8, #0			/* error = 0 */

	mov	x4, xzr			/* i = 0 */
	cbz	x2, copyinstr_done	/* if (len == 0) goto done */
copyinstr_loop:
	ldtrb	w5, [x0]		/* ch = *uaddr++ */
	add	x0, x0, #1
	strb	w5, [x1], #1		/* *kaddr++ = ch */
	add	x4, x4, #1		/* i++ */
	cbz	w5, copyinstr_done	/* if (ch == '\0') goto done */

	cmp	x4, x2			/* if (i < len) goto loop */
	bcc	copyinstr_loop
	mov	x8, #ENAMETOOLONG	/* error = ENAMETOOLONG */

copyinstr_done:
	cbz	x3, 1f			/* if (done != NULL) *done = i */
	str	x4, [x3]
1:
	exit_cpu_onfault
	ret
END(copyinstr)


/* LINTSTUB: int copyoutstr(const void *kaddr, void *uaddr, size_t len, size_t *done); */

ENTRY(copyoutstr)
	enter_cpu_onfault

	mov	x8, #0			/* error = 0 */

	mov	x4, xzr			/* i = 0 */
	cbz	x2, copyoutstr_done	/* if (len == 0) goto done */
copyoutstr_loop:
	ldrb	w5, [x0], #1		/* ch = *kaddr++ */
	sttrb	w5, [x1]		/* *uaddr++ = ch */
	add	x1, x1, #1
	add	x4, x4, #1		/* i++ */
	cbz	w5, copyoutstr_done	/* if (ch == '\0') goto done */

	cmp	x4, x2			/* if (i < len) goto loop */
	bcc	copyoutstr_loop
	mov	x8, #ENAMETOOLONG	/* error = ENAMETOOLONG */

copyoutstr_done:
	cbz	x3, 1f			/* if (done != NULL) *done = i */
	str	x4, [x3]
1:
	exit_cpu_onfault
	ret
END(copyoutstr)


/* LINTSTUB: int kcopy(const void *src, void *dst, size_t len); */

ENTRY(kcopy)
	enter_cpu_onfault

	mov	x3, x0			/* swap src and dst for memcpy */
	mov	x0, x1
	mov	x1, x3
	bl	_C_LABEL(memcpy)
	mov	x8, #0			/* error = 0 */

	exit_cpu_onfault
	ret
END(kcopy)


/* LINTSTUB: int _ucas_32(volatile uint32_t *uptr, uint32_t old, uint32_t new, uint32_t *retp); */
ENTRY(_ucas_32)
	tbnz	x0, AARCH64_ADDRTOP_TAG_BIT, 3f	/* not userspace */
	ands	x8, x0, #3
	cbnz	x8, 3f				/* not aligned */

	enter_cpu_onfault

1:	ldxr	w4, [x0]		/* load old value */
	cmp	w4, w1			/*   compare? */
	b.ne	2f			/*     return if different */
	stxr	w5, w2, [x0]		/* store new value */
	cbnz	w5, 1b			/*   succeed? nope, try again. */
2:	str	w4, [x3]
	mov	x8, #0			/* error = 0 */

	exit_cpu_onfault
	ret
3:
	mov	x0, #EFAULT
	ret
END(_ucas_32)

/* LINTSTUB: int _ucas_64(volatile uint64_t *uptr, uint64_t old, uint64_t new, uint64_t *retp); */
ENTRY(_ucas_64)
	tbnz	x0, AARCH64_ADDRTOP_TAG_BIT, 3f	/* not userspace */
	ands	x8, x0, #7
	cbnz	x8, 3f				/* not aligned */

	enter_cpu_onfault

1:	ldxr	x4, [x0]		/* load old value */
	cmp	x4, x1			/*   compare? */
	b.ne	2f			/*     return if different */
	stxr	w5, x2, [x0]		/* store new value */
	cbnz	w5, 1b			/*   succeed? nope, try again. */
2:	str	x4, [x3]
	mov	x8, #0			/* error = 0 */

	exit_cpu_onfault
	ret
3:
	mov	x0, #EFAULT
	ret
END(_ucas_64)