/*
 * Floating-point, VMX/Altivec and VSX loads and stores
 * for use in instruction emulation.
 *
 * Copyright 2010 Paul Mackerras, IBM Corp. <paulus@au1.ibm.com>
 *
 *  This program is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  as published by the Free Software Foundation; either version
 *  2 of the License, or (at your option) any later version.
 */

#include <asm/processor.h>
#include <asm/ppc_asm.h>
#include <asm/ppc-opcode.h>
#include <asm/reg.h>
#include <asm/asm-offsets.h>
#include <linux/errno.h>

#ifdef CONFIG_PPC_FPU

#define STKFRM	(PPC_MIN_STKFRM + 16)

	.macro	extab	instr,handler
	.section __ex_table,"a"
	PPC_LONG \instr,\handler
	.previous
	.endm

	.macro	inst32	op
reg = 0
	.rept	32
20:	\op	reg,0,r4
	b	3f
	extab	20b,99f
reg = reg + 1
	.endr
	.endm

/* Get the contents of frN into fr0; N is in r3. */
_GLOBAL(get_fpr)
	mflr	r0
	rlwinm	r3,r3,3,0xf8
	bcl	20,31,1f
	blr			/* fr0 is already in fr0 */
	nop
reg = 1
	.rept	31
	fmr	fr0,reg
	blr
reg = reg + 1
	.endr
1:	mflr	r5
	add	r5,r3,r5
	mtctr	r5
	mtlr	r0
	bctr

/* Put the contents of fr0 into frN; N is in r3. */
_GLOBAL(put_fpr)
	mflr	r0
	rlwinm	r3,r3,3,0xf8
	bcl	20,31,1f
	blr			/* fr0 is already in fr0 */
	nop
reg = 1
	.rept	31
	fmr	reg,fr0
	blr
reg = reg + 1
	.endr
1:	mflr	r5
	add	r5,r3,r5
	mtctr	r5
	mtlr	r0
	bctr

/* Load FP reg N from float at *p.  N is in r3, p in r4. */
_GLOBAL(do_lfs)
	PPC_STLU r1,-STKFRM(r1)
	mflr	r0
	PPC_STL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mfmsr	r6
	ori	r7,r6,MSR_FP
	cmpwi	cr7,r3,0
	MTMSRD(r7)
	isync
	beq	cr7,1f
	stfd	fr0,STKFRM-16(r1)
1:	li	r9,-EFAULT
2:	lfs	fr0,0(r4)
	li	r9,0
3:	bl	put_fpr
	beq	cr7,4f
	lfd	fr0,STKFRM-16(r1)
4:	PPC_LL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mtlr	r0
	MTMSRD(r6)
	isync
	mr	r3,r9
	addi	r1,r1,STKFRM
	blr
	extab	2b,3b

/* Load FP reg N from double at *p.  N is in r3, p in r4. */
_GLOBAL(do_lfd)
	PPC_STLU r1,-STKFRM(r1)
	mflr	r0
	PPC_STL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mfmsr	r6
	ori	r7,r6,MSR_FP
	cmpwi	cr7,r3,0
	MTMSRD(r7)
	isync
	beq	cr7,1f
	stfd	fr0,STKFRM-16(r1)
1:	li	r9,-EFAULT
2:	lfd	fr0,0(r4)
	li	r9,0
3:	beq	cr7,4f
	bl	put_fpr
	lfd	fr0,STKFRM-16(r1)
4:	PPC_LL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mtlr	r0
	MTMSRD(r6)
	isync
	mr	r3,r9
	addi	r1,r1,STKFRM
	blr
	extab	2b,3b

/* Store FP reg N to float at *p.  N is in r3, p in r4. */
_GLOBAL(do_stfs)
	PPC_STLU r1,-STKFRM(r1)
	mflr	r0
	PPC_STL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mfmsr	r6
	ori	r7,r6,MSR_FP
	cmpwi	cr7,r3,0
	MTMSRD(r7)
	isync
	beq	cr7,1f
	stfd	fr0,STKFRM-16(r1)
	bl	get_fpr
1:	li	r9,-EFAULT
2:	stfs	fr0,0(r4)
	li	r9,0
3:	beq	cr7,4f
	lfd	fr0,STKFRM-16(r1)
4:	PPC_LL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mtlr	r0
	MTMSRD(r6)
	isync
	mr	r3,r9
	addi	r1,r1,STKFRM
	blr
	extab	2b,3b

/* Store FP reg N to double at *p.  N is in r3, p in r4. */
_GLOBAL(do_stfd)
	PPC_STLU r1,-STKFRM(r1)
	mflr	r0
	PPC_STL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mfmsr	r6
	ori	r7,r6,MSR_FP
	cmpwi	cr7,r3,0
	MTMSRD(r7)
	isync
	beq	cr7,1f
	stfd	fr0,STKFRM-16(r1)
	bl	get_fpr
1:	li	r9,-EFAULT
2:	stfd	fr0,0(r4)
	li	r9,0
3:	beq	cr7,4f
	lfd	fr0,STKFRM-16(r1)
4:	PPC_LL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mtlr	r0
	MTMSRD(r6)
	isync
	mr	r3,r9
	addi	r1,r1,STKFRM
	blr
	extab	2b,3b

#ifdef CONFIG_ALTIVEC
/* Get the contents of vrN into vr0; N is in r3. */
_GLOBAL(get_vr)
	mflr	r0
	rlwinm	r3,r3,3,0xf8
	bcl	20,31,1f
	blr			/* vr0 is already in vr0 */
	nop
reg = 1
	.rept	31
	vor	vr0,reg,reg	/* assembler doesn't know vmr? */
	blr
reg = reg + 1
	.endr
1:	mflr	r5
	add	r5,r3,r5
	mtctr	r5
	mtlr	r0
	bctr

/* Put the contents of vr0 into vrN; N is in r3. */
_GLOBAL(put_vr)
	mflr	r0
	rlwinm	r3,r3,3,0xf8
	bcl	20,31,1f
	blr			/* vr0 is already in vr0 */
	nop
reg = 1
	.rept	31
	vor	reg,vr0,vr0
	blr
reg = reg + 1
	.endr
1:	mflr	r5
	add	r5,r3,r5
	mtctr	r5
	mtlr	r0
	bctr

/* Load vector reg N from *p.  N is in r3, p in r4. */
_GLOBAL(do_lvx)
	PPC_STLU r1,-STKFRM(r1)
	mflr	r0
	PPC_STL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mfmsr	r6
	oris	r7,r6,MSR_VEC@h
	cmpwi	cr7,r3,0
	li	r8,STKFRM-16
	MTMSRD(r7)
	isync
	beq	cr7,1f
	stvx	vr0,r1,r8
1:	li	r9,-EFAULT
2:	lvx	vr0,0,r4
	li	r9,0
3:	beq	cr7,4f
	bl	put_vr
	lvx	vr0,r1,r8
4:	PPC_LL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mtlr	r0
	MTMSRD(r6)
	isync
	mr	r3,r9
	addi	r1,r1,STKFRM
	blr
	extab	2b,3b

/* Store vector reg N to *p.  N is in r3, p in r4. */
_GLOBAL(do_stvx)
	PPC_STLU r1,-STKFRM(r1)
	mflr	r0
	PPC_STL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mfmsr	r6
	oris	r7,r6,MSR_VEC@h
	cmpwi	cr7,r3,0
	li	r8,STKFRM-16
	MTMSRD(r7)
	isync
	beq	cr7,1f
	stvx	vr0,r1,r8
	bl	get_vr
1:	li	r9,-EFAULT
2:	stvx	vr0,0,r4
	li	r9,0
3:	beq	cr7,4f
	lvx	vr0,r1,r8
4:	PPC_LL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mtlr	r0
	MTMSRD(r6)
	isync
	mr	r3,r9
	addi	r1,r1,STKFRM
	blr
	extab	2b,3b
#endif /* CONFIG_ALTIVEC */

#ifdef CONFIG_VSX
/* Get the contents of vsrN into vsr0; N is in r3. */
_GLOBAL(get_vsr)
	mflr	r0
	rlwinm	r3,r3,3,0x1f8
	bcl	20,31,1f
	blr			/* vsr0 is already in vsr0 */
	nop
reg = 1
	.rept	63
	XXLOR(0,reg,reg)
	blr
reg = reg + 1
	.endr
1:	mflr	r5
	add	r5,r3,r5
	mtctr	r5
	mtlr	r0
	bctr

/* Put the contents of vsr0 into vsrN; N is in r3. */
_GLOBAL(put_vsr)
	mflr	r0
	rlwinm	r3,r3,3,0x1f8
	bcl	20,31,1f
	blr			/* vr0 is already in vr0 */
	nop
reg = 1
	.rept	63
	XXLOR(reg,0,0)
	blr
reg = reg + 1
	.endr
1:	mflr	r5
	add	r5,r3,r5
	mtctr	r5
	mtlr	r0
	bctr

/* Load VSX reg N from vector doubleword *p.  N is in r3, p in r4. */
_GLOBAL(do_lxvd2x)
	PPC_STLU r1,-STKFRM(r1)
	mflr	r0
	PPC_STL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mfmsr	r6
	oris	r7,r6,MSR_VSX@h
	cmpwi	cr7,r3,0
	li	r8,STKFRM-16
	MTMSRD(r7)
	isync
	beq	cr7,1f
	STXVD2X(0,r1,r8)
1:	li	r9,-EFAULT
2:	LXVD2X(0,0,r4)
	li	r9,0
3:	beq	cr7,4f
	bl	put_vsr
	LXVD2X(0,r1,r8)
4:	PPC_LL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mtlr	r0
	MTMSRD(r6)
	isync
	mr	r3,r9
	addi	r1,r1,STKFRM
	blr
	extab	2b,3b

/* Store VSX reg N to vector doubleword *p.  N is in r3, p in r4. */
_GLOBAL(do_stxvd2x)
	PPC_STLU r1,-STKFRM(r1)
	mflr	r0
	PPC_STL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mfmsr	r6
	oris	r7,r6,MSR_VSX@h
	cmpwi	cr7,r3,0
	li	r8,STKFRM-16
	MTMSRD(r7)
	isync
	beq	cr7,1f
	STXVD2X(0,r1,r8)
	bl	get_vsr
1:	li	r9,-EFAULT
2:	STXVD2X(0,0,r4)
	li	r9,0
3:	beq	cr7,4f
	LXVD2X(0,r1,r8)
4:	PPC_LL	r0,STKFRM+PPC_LR_STKOFF(r1)
	mtlr	r0
	MTMSRD(r6)
	isync
	mr	r3,r9
	addi	r1,r1,STKFRM
	blr
	extab	2b,3b

#endif /* CONFIG_VSX */

#endif	/* CONFIG_PPC_FPU */