/*
 * FP/SIMD state saving and restoring macros
 *
 * Copyright (C) 2012 ARM Ltd.
 * Author: Catalin Marinas <catalin.marinas@arm.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

.macro fpsimd_save state, tmpnr
	stp	q0, q1, [\state, #16 * 0]
	stp	q2, q3, [\state, #16 * 2]
	stp	q4, q5, [\state, #16 * 4]
	stp	q6, q7, [\state, #16 * 6]
	stp	q8, q9, [\state, #16 * 8]
	stp	q10, q11, [\state, #16 * 10]
	stp	q12, q13, [\state, #16 * 12]
	stp	q14, q15, [\state, #16 * 14]
	stp	q16, q17, [\state, #16 * 16]
	stp	q18, q19, [\state, #16 * 18]
	stp	q20, q21, [\state, #16 * 20]
	stp	q22, q23, [\state, #16 * 22]
	stp	q24, q25, [\state, #16 * 24]
	stp	q26, q27, [\state, #16 * 26]
	stp	q28, q29, [\state, #16 * 28]
	stp	q30, q31, [\state, #16 * 30]!
	mrs	x\tmpnr, fpsr
	str	w\tmpnr, [\state, #16 * 2]
	mrs	x\tmpnr, fpcr
	str	w\tmpnr, [\state, #16 * 2 + 4]
.endm

.macro fpsimd_restore_fpcr state, tmp
	/*
	 * Writes to fpcr may be self-synchronising, so avoid restoring
	 * the register if it hasn't changed.
	 */
	mrs	\tmp, fpcr
	cmp	\tmp, \state
	b.eq	9999f
	msr	fpcr, \state
9999:
.endm

/* Clobbers \state */
.macro fpsimd_restore state, tmpnr
	ldp	q0, q1, [\state, #16 * 0]
	ldp	q2, q3, [\state, #16 * 2]
	ldp	q4, q5, [\state, #16 * 4]
	ldp	q6, q7, [\state, #16 * 6]
	ldp	q8, q9, [\state, #16 * 8]
	ldp	q10, q11, [\state, #16 * 10]
	ldp	q12, q13, [\state, #16 * 12]
	ldp	q14, q15, [\state, #16 * 14]
	ldp	q16, q17, [\state, #16 * 16]
	ldp	q18, q19, [\state, #16 * 18]
	ldp	q20, q21, [\state, #16 * 20]
	ldp	q22, q23, [\state, #16 * 22]
	ldp	q24, q25, [\state, #16 * 24]
	ldp	q26, q27, [\state, #16 * 26]
	ldp	q28, q29, [\state, #16 * 28]
	ldp	q30, q31, [\state, #16 * 30]!
	ldr	w\tmpnr, [\state, #16 * 2]
	msr	fpsr, x\tmpnr
	ldr	w\tmpnr, [\state, #16 * 2 + 4]
	fpsimd_restore_fpcr x\tmpnr, \state
.endm

.macro fpsimd_save_partial state, numnr, tmpnr1, tmpnr2
	mrs	x\tmpnr1, fpsr
	str	w\numnr, [\state, #8]
	mrs	x\tmpnr2, fpcr
	stp	w\tmpnr1, w\tmpnr2, [\state]
	adr	x\tmpnr1, 0f
	add	\state, \state, x\numnr, lsl #4
	sub	x\tmpnr1, x\tmpnr1, x\numnr, lsl #1
	br	x\tmpnr1
	stp	q30, q31, [\state, #-16 * 30 - 16]
	stp	q28, q29, [\state, #-16 * 28 - 16]
	stp	q26, q27, [\state, #-16 * 26 - 16]
	stp	q24, q25, [\state, #-16 * 24 - 16]
	stp	q22, q23, [\state, #-16 * 22 - 16]
	stp	q20, q21, [\state, #-16 * 20 - 16]
	stp	q18, q19, [\state, #-16 * 18 - 16]
	stp	q16, q17, [\state, #-16 * 16 - 16]
	stp	q14, q15, [\state, #-16 * 14 - 16]
	stp	q12, q13, [\state, #-16 * 12 - 16]
	stp	q10, q11, [\state, #-16 * 10 - 16]
	stp	q8, q9, [\state, #-16 * 8 - 16]
	stp	q6, q7, [\state, #-16 * 6 - 16]
	stp	q4, q5, [\state, #-16 * 4 - 16]
	stp	q2, q3, [\state, #-16 * 2 - 16]
	stp	q0, q1, [\state, #-16 * 0 - 16]
0:
.endm

.macro fpsimd_restore_partial state, tmpnr1, tmpnr2
	ldp	w\tmpnr1, w\tmpnr2, [\state]
	msr	fpsr, x\tmpnr1
	fpsimd_restore_fpcr x\tmpnr2, x\tmpnr1
	adr	x\tmpnr1, 0f
	ldr	w\tmpnr2, [\state, #8]
	add	\state, \state, x\tmpnr2, lsl #4
	sub	x\tmpnr1, x\tmpnr1, x\tmpnr2, lsl #1
	br	x\tmpnr1
	ldp	q30, q31, [\state, #-16 * 30 - 16]
	ldp	q28, q29, [\state, #-16 * 28 - 16]
	ldp	q26, q27, [\state, #-16 * 26 - 16]
	ldp	q24, q25, [\state, #-16 * 24 - 16]
	ldp	q22, q23, [\state, #-16 * 22 - 16]
	ldp	q20, q21, [\state, #-16 * 20 - 16]
	ldp	q18, q19, [\state, #-16 * 18 - 16]
	ldp	q16, q17, [\state, #-16 * 16 - 16]
	ldp	q14, q15, [\state, #-16 * 14 - 16]
	ldp	q12, q13, [\state, #-16 * 12 - 16]
	ldp	q10, q11, [\state, #-16 * 10 - 16]
	ldp	q8, q9, [\state, #-16 * 8 - 16]
	ldp	q6, q7, [\state, #-16 * 6 - 16]
	ldp	q4, q5, [\state, #-16 * 4 - 16]
	ldp	q2, q3, [\state, #-16 * 2 - 16]
	ldp	q0, q1, [\state, #-16 * 0 - 16]
0:
.endm