/* SPDX-License-Identifier: GPL-2.0-or-later */
/*
 *
 * Copyright (C) IBM Corporation, 2012
 *
 * Author: Anton Blanchard <anton@au.ibm.com>
 */
#include <asm/ppc_asm.h>

#ifndef SELFTEST_CASE
/* 0 == don't use VMX, 1 == use VMX */
#define SELFTEST_CASE	0
#endif

#ifdef __BIG_ENDIAN__
#define LVS(VRT,RA,RB)		lvsl	VRT,RA,RB
#define VPERM(VRT,VRA,VRB,VRC)	vperm	VRT,VRA,VRB,VRC
#else
#define LVS(VRT,RA,RB)		lvsr	VRT,RA,RB
#define VPERM(VRT,VRA,VRB,VRC)	vperm	VRT,VRB,VRA,VRC
#endif

_GLOBAL(memcpy_power7)
	cmpldi	r5,16
	cmpldi	cr1,r5,4096
	std	r3,-STACKFRAMESIZE+STK_REG(R31)(r1)
	blt	.Lshort_copy

#ifdef CONFIG_ALTIVEC
test_feature = SELFTEST_CASE
BEGIN_FTR_SECTION
	bgt	cr1, .Lvmx_copy
END_FTR_SECTION_IFSET(CPU_FTR_ALTIVEC)
#endif

.Lnonvmx_copy:
	/* Get the source 8B aligned */
	neg	r6,r4
	mtocrf	0x01,r6
	clrldi	r6,r6,(64-3)

	bf	cr7*4+3,1f
	lbz	r0,0(r4)
	addi	r4,r4,1
	stb	r0,0(r3)
	addi	r3,r3,1

1:	bf	cr7*4+2,2f
	lhz	r0,0(r4)
	addi	r4,r4,2
	sth	r0,0(r3)
	addi	r3,r3,2

2:	bf	cr7*4+1,3f
	lwz	r0,0(r4)
	addi	r4,r4,4
	stw	r0,0(r3)
	addi	r3,r3,4

3:	sub	r5,r5,r6
	cmpldi	r5,128
	blt	5f

	mflr	r0
	stdu	r1,-STACKFRAMESIZE(r1)
	std	r14,STK_REG(R14)(r1)
	std	r15,STK_REG(R15)(r1)
	std	r16,STK_REG(R16)(r1)
	std	r17,STK_REG(R17)(r1)
	std	r18,STK_REG(R18)(r1)
	std	r19,STK_REG(R19)(r1)
	std	r20,STK_REG(R20)(r1)
	std	r21,STK_REG(R21)(r1)
	std	r22,STK_REG(R22)(r1)
	std	r0,STACKFRAMESIZE+16(r1)

	srdi	r6,r5,7
	mtctr	r6

	/* Now do cacheline (128B) sized loads and stores. */
	.align	5
4:
	ld	r0,0(r4)
	ld	r6,8(r4)
	ld	r7,16(r4)
	ld	r8,24(r4)
	ld	r9,32(r4)
	ld	r10,40(r4)
	ld	r11,48(r4)
	ld	r12,56(r4)
	ld	r14,64(r4)
	ld	r15,72(r4)
	ld	r16,80(r4)
	ld	r17,88(r4)
	ld	r18,96(r4)
	ld	r19,104(r4)
	ld	r20,112(r4)
	ld	r21,120(r4)
	addi	r4,r4,128
	std	r0,0(r3)
	std	r6,8(r3)
	std	r7,16(r3)
	std	r8,24(r3)
	std	r9,32(r3)
	std	r10,40(r3)
	std	r11,48(r3)
	std	r12,56(r3)
	std	r14,64(r3)
	std	r15,72(r3)
	std	r16,80(r3)
	std	r17,88(r3)
	std	r18,96(r3)
	std	r19,104(r3)
	std	r20,112(r3)
	std	r21,120(r3)
	addi	r3,r3,128
	bdnz	4b

	clrldi	r5,r5,(64-7)

	ld	r14,STK_REG(R14)(r1)
	ld	r15,STK_REG(R15)(r1)
	ld	r16,STK_REG(R16)(r1)
	ld	r17,STK_REG(R17)(r1)
	ld	r18,STK_REG(R18)(r1)
	ld	r19,STK_REG(R19)(r1)
	ld	r20,STK_REG(R20)(r1)
	ld	r21,STK_REG(R21)(r1)
	ld	r22,STK_REG(R22)(r1)
	addi	r1,r1,STACKFRAMESIZE

	/* Up to 127B to go */
5:	srdi	r6,r5,4
	mtocrf	0x01,r6

6:	bf	cr7*4+1,7f
	ld	r0,0(r4)
	ld	r6,8(r4)
	ld	r7,16(r4)
	ld	r8,24(r4)
	ld	r9,32(r4)
	ld	r10,40(r4)
	ld	r11,48(r4)
	ld	r12,56(r4)
	addi	r4,r4,64
	std	r0,0(r3)
	std	r6,8(r3)
	std	r7,16(r3)
	std	r8,24(r3)
	std	r9,32(r3)
	std	r10,40(r3)
	std	r11,48(r3)
	std	r12,56(r3)
	addi	r3,r3,64

	/* Up to 63B to go */
7:	bf	cr7*4+2,8f
	ld	r0,0(r4)
	ld	r6,8(r4)
	ld	r7,16(r4)
	ld	r8,24(r4)
	addi	r4,r4,32
	std	r0,0(r3)
	std	r6,8(r3)
	std	r7,16(r3)
	std	r8,24(r3)
	addi	r3,r3,32

	/* Up to 31B to go */
8:	bf	cr7*4+3,9f
	ld	r0,0(r4)
	ld	r6,8(r4)
	addi	r4,r4,16
	std	r0,0(r3)
	std	r6,8(r3)
	addi	r3,r3,16

9:	clrldi	r5,r5,(64-4)

	/* Up to 15B to go */
.Lshort_copy:
	mtocrf	0x01,r5
	bf	cr7*4+0,12f
	lwz	r0,0(r4)	/* Less chance of a reject with word ops */
	lwz	r6,4(r4)
	addi	r4,r4,8
	stw	r0,0(r3)
	stw	r6,4(r3)
	addi	r3,r3,8

12:	bf	cr7*4+1,13f
	lwz	r0,0(r4)
	addi	r4,r4,4
	stw	r0,0(r3)
	addi	r3,r3,4

13:	bf	cr7*4+2,14f
	lhz	r0,0(r4)
	addi	r4,r4,2
	sth	r0,0(r3)
	addi	r3,r3,2

14:	bf	cr7*4+3,15f
	lbz	r0,0(r4)
	stb	r0,0(r3)

15:	ld	r3,-STACKFRAMESIZE+STK_REG(R31)(r1)
	blr

.Lunwind_stack_nonvmx_copy:
	addi	r1,r1,STACKFRAMESIZE
	b	.Lnonvmx_copy

.Lvmx_copy:
#ifdef CONFIG_ALTIVEC
	mflr	r0
	std	r4,-STACKFRAMESIZE+STK_REG(R30)(r1)
	std	r5,-STACKFRAMESIZE+STK_REG(R29)(r1)
	std	r0,16(r1)
	stdu	r1,-STACKFRAMESIZE(r1)
	bl	CFUNC(enter_vmx_ops)
	cmpwi	cr1,r3,0
	ld	r0,STACKFRAMESIZE+16(r1)
	ld	r3,STK_REG(R31)(r1)
	ld	r4,STK_REG(R30)(r1)
	ld	r5,STK_REG(R29)(r1)
	mtlr	r0

	/*
	 * We prefetch both the source and destination using enhanced touch
	 * instructions. We use a stream ID of 0 for the load side and
	 * 1 for the store side.
	 */
	clrrdi	r6,r4,7
	clrrdi	r9,r3,7
	ori	r9,r9,1		/* stream=1 */

	srdi	r7,r5,7		/* length in cachelines, capped at 0x3FF */
	cmpldi	r7,0x3FF
	ble	1f
	li	r7,0x3FF
1:	lis	r0,0x0E00	/* depth=7 */
	sldi	r7,r7,7
	or	r7,r7,r0
	ori	r10,r7,1	/* stream=1 */

	DCBT_SETUP_STREAMS(r6, r7, r9, r10, r8)

	beq	cr1,.Lunwind_stack_nonvmx_copy

	/*
	 * If source and destination are not relatively aligned we use a
	 * slower permute loop.
	 */
	xor	r6,r4,r3
	rldicl.	r6,r6,0,(64-4)
	bne	.Lvmx_unaligned_copy

	/* Get the destination 16B aligned */
	neg	r6,r3
	mtocrf	0x01,r6
	clrldi	r6,r6,(64-4)

	bf	cr7*4+3,1f
	lbz	r0,0(r4)
	addi	r4,r4,1
	stb	r0,0(r3)
	addi	r3,r3,1

1:	bf	cr7*4+2,2f
	lhz	r0,0(r4)
	addi	r4,r4,2
	sth	r0,0(r3)
	addi	r3,r3,2

2:	bf	cr7*4+1,3f
	lwz	r0,0(r4)
	addi	r4,r4,4
	stw	r0,0(r3)
	addi	r3,r3,4

3:	bf	cr7*4+0,4f
	ld	r0,0(r4)
	addi	r4,r4,8
	std	r0,0(r3)
	addi	r3,r3,8

4:	sub	r5,r5,r6

	/* Get the desination 128B aligned */
	neg	r6,r3
	srdi	r7,r6,4
	mtocrf	0x01,r7
	clrldi	r6,r6,(64-7)

	li	r9,16
	li	r10,32
	li	r11,48

	bf	cr7*4+3,5f
	lvx	v1,0,r4
	addi	r4,r4,16
	stvx	v1,0,r3
	addi	r3,r3,16

5:	bf	cr7*4+2,6f
	lvx	v1,0,r4
	lvx	v0,r4,r9
	addi	r4,r4,32
	stvx	v1,0,r3
	stvx	v0,r3,r9
	addi	r3,r3,32

6:	bf	cr7*4+1,7f
	lvx	v3,0,r4
	lvx	v2,r4,r9
	lvx	v1,r4,r10
	lvx	v0,r4,r11
	addi	r4,r4,64
	stvx	v3,0,r3
	stvx	v2,r3,r9
	stvx	v1,r3,r10
	stvx	v0,r3,r11
	addi	r3,r3,64

7:	sub	r5,r5,r6
	srdi	r6,r5,7

	std	r14,STK_REG(R14)(r1)
	std	r15,STK_REG(R15)(r1)
	std	r16,STK_REG(R16)(r1)

	li	r12,64
	li	r14,80
	li	r15,96
	li	r16,112

	mtctr	r6

	/*
	 * Now do cacheline sized loads and stores. By this stage the
	 * cacheline stores are also cacheline aligned.
	 */
	.align	5
8:
	lvx	v7,0,r4
	lvx	v6,r4,r9
	lvx	v5,r4,r10
	lvx	v4,r4,r11
	lvx	v3,r4,r12
	lvx	v2,r4,r14
	lvx	v1,r4,r15
	lvx	v0,r4,r16
	addi	r4,r4,128
	stvx	v7,0,r3
	stvx	v6,r3,r9
	stvx	v5,r3,r10
	stvx	v4,r3,r11
	stvx	v3,r3,r12
	stvx	v2,r3,r14
	stvx	v1,r3,r15
	stvx	v0,r3,r16
	addi	r3,r3,128
	bdnz	8b

	ld	r14,STK_REG(R14)(r1)
	ld	r15,STK_REG(R15)(r1)
	ld	r16,STK_REG(R16)(r1)

	/* Up to 127B to go */
	clrldi	r5,r5,(64-7)
	srdi	r6,r5,4
	mtocrf	0x01,r6

	bf	cr7*4+1,9f
	lvx	v3,0,r4
	lvx	v2,r4,r9
	lvx	v1,r4,r10
	lvx	v0,r4,r11
	addi	r4,r4,64
	stvx	v3,0,r3
	stvx	v2,r3,r9
	stvx	v1,r3,r10
	stvx	v0,r3,r11
	addi	r3,r3,64

9:	bf	cr7*4+2,10f
	lvx	v1,0,r4
	lvx	v0,r4,r9
	addi	r4,r4,32
	stvx	v1,0,r3
	stvx	v0,r3,r9
	addi	r3,r3,32

10:	bf	cr7*4+3,11f
	lvx	v1,0,r4
	addi	r4,r4,16
	stvx	v1,0,r3
	addi	r3,r3,16

	/* Up to 15B to go */
11:	clrldi	r5,r5,(64-4)
	mtocrf	0x01,r5
	bf	cr7*4+0,12f
	ld	r0,0(r4)
	addi	r4,r4,8
	std	r0,0(r3)
	addi	r3,r3,8

12:	bf	cr7*4+1,13f
	lwz	r0,0(r4)
	addi	r4,r4,4
	stw	r0,0(r3)
	addi	r3,r3,4

13:	bf	cr7*4+2,14f
	lhz	r0,0(r4)
	addi	r4,r4,2
	sth	r0,0(r3)
	addi	r3,r3,2

14:	bf	cr7*4+3,15f
	lbz	r0,0(r4)
	stb	r0,0(r3)

15:	addi	r1,r1,STACKFRAMESIZE
	ld	r3,-STACKFRAMESIZE+STK_REG(R31)(r1)
	b	CFUNC(exit_vmx_ops)		/* tail call optimise */

.Lvmx_unaligned_copy:
	/* Get the destination 16B aligned */
	neg	r6,r3
	mtocrf	0x01,r6
	clrldi	r6,r6,(64-4)

	bf	cr7*4+3,1f
	lbz	r0,0(r4)
	addi	r4,r4,1
	stb	r0,0(r3)
	addi	r3,r3,1

1:	bf	cr7*4+2,2f
	lhz	r0,0(r4)
	addi	r4,r4,2
	sth	r0,0(r3)
	addi	r3,r3,2

2:	bf	cr7*4+1,3f
	lwz	r0,0(r4)
	addi	r4,r4,4
	stw	r0,0(r3)
	addi	r3,r3,4

3:	bf	cr7*4+0,4f
	lwz	r0,0(r4)	/* Less chance of a reject with word ops */
	lwz	r7,4(r4)
	addi	r4,r4,8
	stw	r0,0(r3)
	stw	r7,4(r3)
	addi	r3,r3,8

4:	sub	r5,r5,r6

	/* Get the desination 128B aligned */
	neg	r6,r3
	srdi	r7,r6,4
	mtocrf	0x01,r7
	clrldi	r6,r6,(64-7)

	li	r9,16
	li	r10,32
	li	r11,48

	LVS(v16,0,r4)		/* Setup permute control vector */
	lvx	v0,0,r4
	addi	r4,r4,16

	bf	cr7*4+3,5f
	lvx	v1,0,r4
	VPERM(v8,v0,v1,v16)
	addi	r4,r4,16
	stvx	v8,0,r3
	addi	r3,r3,16
	vor	v0,v1,v1

5:	bf	cr7*4+2,6f
	lvx	v1,0,r4
	VPERM(v8,v0,v1,v16)
	lvx	v0,r4,r9
	VPERM(v9,v1,v0,v16)
	addi	r4,r4,32
	stvx	v8,0,r3
	stvx	v9,r3,r9
	addi	r3,r3,32

6:	bf	cr7*4+1,7f
	lvx	v3,0,r4
	VPERM(v8,v0,v3,v16)
	lvx	v2,r4,r9
	VPERM(v9,v3,v2,v16)
	lvx	v1,r4,r10
	VPERM(v10,v2,v1,v16)
	lvx	v0,r4,r11
	VPERM(v11,v1,v0,v16)
	addi	r4,r4,64
	stvx	v8,0,r3
	stvx	v9,r3,r9
	stvx	v10,r3,r10
	stvx	v11,r3,r11
	addi	r3,r3,64

7:	sub	r5,r5,r6
	srdi	r6,r5,7

	std	r14,STK_REG(R14)(r1)
	std	r15,STK_REG(R15)(r1)
	std	r16,STK_REG(R16)(r1)

	li	r12,64
	li	r14,80
	li	r15,96
	li	r16,112

	mtctr	r6

	/*
	 * Now do cacheline sized loads and stores. By this stage the
	 * cacheline stores are also cacheline aligned.
	 */
	.align	5
8:
	lvx	v7,0,r4
	VPERM(v8,v0,v7,v16)
	lvx	v6,r4,r9
	VPERM(v9,v7,v6,v16)
	lvx	v5,r4,r10
	VPERM(v10,v6,v5,v16)
	lvx	v4,r4,r11
	VPERM(v11,v5,v4,v16)
	lvx	v3,r4,r12
	VPERM(v12,v4,v3,v16)
	lvx	v2,r4,r14
	VPERM(v13,v3,v2,v16)
	lvx	v1,r4,r15
	VPERM(v14,v2,v1,v16)
	lvx	v0,r4,r16
	VPERM(v15,v1,v0,v16)
	addi	r4,r4,128
	stvx	v8,0,r3
	stvx	v9,r3,r9
	stvx	v10,r3,r10
	stvx	v11,r3,r11
	stvx	v12,r3,r12
	stvx	v13,r3,r14
	stvx	v14,r3,r15
	stvx	v15,r3,r16
	addi	r3,r3,128
	bdnz	8b

	ld	r14,STK_REG(R14)(r1)
	ld	r15,STK_REG(R15)(r1)
	ld	r16,STK_REG(R16)(r1)

	/* Up to 127B to go */
	clrldi	r5,r5,(64-7)
	srdi	r6,r5,4
	mtocrf	0x01,r6

	bf	cr7*4+1,9f
	lvx	v3,0,r4
	VPERM(v8,v0,v3,v16)
	lvx	v2,r4,r9
	VPERM(v9,v3,v2,v16)
	lvx	v1,r4,r10
	VPERM(v10,v2,v1,v16)
	lvx	v0,r4,r11
	VPERM(v11,v1,v0,v16)
	addi	r4,r4,64
	stvx	v8,0,r3
	stvx	v9,r3,r9
	stvx	v10,r3,r10
	stvx	v11,r3,r11
	addi	r3,r3,64

9:	bf	cr7*4+2,10f
	lvx	v1,0,r4
	VPERM(v8,v0,v1,v16)
	lvx	v0,r4,r9
	VPERM(v9,v1,v0,v16)
	addi	r4,r4,32
	stvx	v8,0,r3
	stvx	v9,r3,r9
	addi	r3,r3,32

10:	bf	cr7*4+3,11f
	lvx	v1,0,r4
	VPERM(v8,v0,v1,v16)
	addi	r4,r4,16
	stvx	v8,0,r3
	addi	r3,r3,16

	/* Up to 15B to go */
11:	clrldi	r5,r5,(64-4)
	addi	r4,r4,-16	/* Unwind the +16 load offset */
	mtocrf	0x01,r5
	bf	cr7*4+0,12f
	lwz	r0,0(r4)	/* Less chance of a reject with word ops */
	lwz	r6,4(r4)
	addi	r4,r4,8
	stw	r0,0(r3)
	stw	r6,4(r3)
	addi	r3,r3,8

12:	bf	cr7*4+1,13f
	lwz	r0,0(r4)
	addi	r4,r4,4
	stw	r0,0(r3)
	addi	r3,r3,4

13:	bf	cr7*4+2,14f
	lhz	r0,0(r4)
	addi	r4,r4,2
	sth	r0,0(r3)
	addi	r3,r3,2

14:	bf	cr7*4+3,15f
	lbz	r0,0(r4)
	stb	r0,0(r3)

15:	addi	r1,r1,STACKFRAMESIZE
	ld	r3,-STACKFRAMESIZE+STK_REG(R31)(r1)
	b	CFUNC(exit_vmx_ops)		/* tail call optimise */
#endif /* CONFIG_ALTIVEC */