[GitHub/LineageOS/android_kernel_motorola_exynos9610.git] / arch / arm / common / fiq_glue.S

/*
 * Copyright (C) 2008 Google, Inc.
 *
 * This software is licensed under the terms of the GNU General Public
 * License version 2, as published by the Free Software Foundation, and
 * may be copied, distributed, and modified under those terms.
 *
 * 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.
 *
 */

#include <linux/linkage.h>
#include <asm/assembler.h>

		.text

		.global fiq_glue_end

		/* fiq stack: r0-r15,cpsr,spsr of interrupted mode */

ENTRY(fiq_glue)
		/* store pc, cpsr from previous mode, reserve space for spsr */
		mrs	r12, spsr
		sub	lr, lr, #4
		subs	r10, #1
		bne	nested_fiq

		str	r12, [sp, #-8]!
		str	lr, [sp, #-4]!

		/* store r8-r14 from previous mode */
		sub	sp, sp, #(7 * 4)
		stmia	sp, {r8-r14}^
		nop

		/* store r0-r7 from previous mode */
		stmfd	sp!, {r0-r7}

		/* setup func(data,regs) arguments */
		mov	r0, r9
		mov	r1, sp
		mov	r3, r8

		mov	r7, sp

		/* Get sp and lr from non-user modes */
		and	r4, r12, #MODE_MASK
		cmp	r4, #USR_MODE
		beq	fiq_from_usr_mode

		mov	r7, sp
		orr	r4, r4, #(PSR_I_BIT | PSR_F_BIT)
		msr	cpsr_c, r4
		str	sp, [r7, #(4 * 13)]
		str	lr, [r7, #(4 * 14)]
		mrs	r5, spsr
		str	r5, [r7, #(4 * 17)]

		cmp	r4, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT)
		/* use fiq stack if we reenter this mode */
		subne	sp, r7, #(4 * 3)

fiq_from_usr_mode:
		msr	cpsr_c, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT)
		mov	r2, sp
		sub	sp, r7, #12
		stmfd	sp!, {r2, ip, lr}
		/* call func(data,regs) */
		blx	r3
		ldmfd	sp, {r2, ip, lr}
		mov	sp, r2

		/* restore/discard saved state */
		cmp	r4, #USR_MODE
		beq	fiq_from_usr_mode_exit

		msr	cpsr_c, r4
		ldr	sp, [r7, #(4 * 13)]
		ldr	lr, [r7, #(4 * 14)]
		msr	spsr_cxsf, r5

fiq_from_usr_mode_exit:
		msr	cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT)

		ldmfd	sp!, {r0-r7}
		ldr	lr, [sp, #(4 * 7)]
		ldr	r12, [sp, #(4 * 8)]
		add	sp, sp, #(10 * 4)
exit_fiq:
		msr	spsr_cxsf, r12
		add	r10, #1
		cmp	r11, #0
		moveqs	pc, lr
		bx	r11 /* jump to custom fiq return function */

nested_fiq:
		orr	r12, r12, #(PSR_F_BIT)
		b	exit_fiq

fiq_glue_end:

ENTRY(fiq_glue_setup) /* func, data, sp, smc call number */
		stmfd		sp!, {r4}
		mrs		r4, cpsr
		msr		cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT)
		movs		r8, r0
		mov		r9, r1
		mov		sp, r2
		mov		r11, r3
		moveq		r10, #0
		movne		r10, #1
		msr		cpsr_c, r4
		ldmfd		sp!, {r4}
		bx		lr
Commit	Line	Data
47fe2f8d AH	1	/*
	2	* Copyright (C) 2008 Google, Inc.
	3	*
	4	* This software is licensed under the terms of the GNU General Public
	5	* License version 2, as published by the Free Software Foundation, and
	6	* may be copied, distributed, and modified under those terms.
	7	*
	8	* This program is distributed in the hope that it will be useful,
	9	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	10	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	11	* GNU General Public License for more details.
	12	*
	13	*/
	14
	15	#include <linux/linkage.h>
	16	#include <asm/assembler.h>
	17
	18	.text
	19
	20	.global fiq_glue_end
	21
	22	/* fiq stack: r0-r15,cpsr,spsr of interrupted mode */
	23
	24	ENTRY(fiq_glue)
e329aaf3	25	/* store pc, cpsr from previous mode, reserve space for spsr */
47fe2f8d	26	mrs r12, spsr
e329aaf3	27	sub lr, lr, #4
47fe2f8d AH	28	subs r10, #1
	29	bne nested_fiq
	30
e329aaf3 AH	31	str r12, [sp, #-8]!
e329aaf3 AH	32	str lr, [sp, #-4]!
47fe2f8d AH	33
	34	/* store r8-r14 from previous mode */
	35	sub sp, sp, #(7 * 4)
	36	stmia sp, {r8-r14}^
	37	nop
	38
	39	/* store r0-r7 from previous mode */
	40	stmfd sp!, {r0-r7}
	41
	42	/* setup func(data,regs) arguments */
	43	mov r0, r9
	44	mov r1, sp
	45	mov r3, r8
	46
	47	mov r7, sp
	48
	49	/* Get sp and lr from non-user modes */
	50	and r4, r12, #MODE_MASK
	51	cmp r4, #USR_MODE
	52	beq fiq_from_usr_mode
	53
	54	mov r7, sp
	55	orr r4, r4, #(PSR_I_BIT \| PSR_F_BIT)
	56	msr cpsr_c, r4
	57	str sp, [r7, #(4 * 13)]
	58	str lr, [r7, #(4 * 14)]
	59	mrs r5, spsr
	60	str r5, [r7, #(4 * 17)]
	61
	62	cmp r4, #(SVC_MODE \| PSR_I_BIT \| PSR_F_BIT)
	63	/* use fiq stack if we reenter this mode */
	64	subne sp, r7, #(4 * 3)
	65
	66	fiq_from_usr_mode:
	67	msr cpsr_c, #(SVC_MODE \| PSR_I_BIT \| PSR_F_BIT)
	68	mov r2, sp
	69	sub sp, r7, #12
	70	stmfd sp!, {r2, ip, lr}
	71	/* call func(data,regs) */
	72	blx r3
	73	ldmfd sp, {r2, ip, lr}
	74	mov sp, r2
	75
	76	/* restore/discard saved state */
	77	cmp r4, #USR_MODE
	78	beq fiq_from_usr_mode_exit
	79
	80	msr cpsr_c, r4
	81	ldr sp, [r7, #(4 * 13)]
	82	ldr lr, [r7, #(4 * 14)]
	83	msr spsr_cxsf, r5
	84
	85	fiq_from_usr_mode_exit:
	86	msr cpsr_c, #(FIQ_MODE \| PSR_I_BIT \| PSR_F_BIT)
	87
	88	ldmfd sp!, {r0-r7}
e329aaf3 AH	89	ldr lr, [sp, #(4 * 7)]
	90	ldr r12, [sp, #(4 * 8)]
	91	add sp, sp, #(10 * 4)
47fe2f8d AH	92	exit_fiq:
	93	msr spsr_cxsf, r12
	94	add r10, #1
e329aaf3 AH	95	cmp r11, #0
	96	moveqs pc, lr
	97	bx r11 /* jump to custom fiq return function */
47fe2f8d AH	98
	99	nested_fiq:
	100	orr r12, r12, #(PSR_F_BIT)
	101	b exit_fiq
	102
	103	fiq_glue_end:
	104
e329aaf3 AH	105	ENTRY(fiq_glue_setup) /* func, data, sp, smc call number */
	106	stmfd sp!, {r4}
	107	mrs r4, cpsr
47fe2f8d AH	108	msr cpsr_c, #(FIQ_MODE \| PSR_I_BIT \| PSR_F_BIT)
	109	movs r8, r0
	110	mov r9, r1
	111	mov sp, r2
e329aaf3	112	mov r11, r3
47fe2f8d AH	113	moveq r10, #0
47fe2f8d AH	114	movne r10, #1
e329aaf3 AH	115	msr cpsr_c, r4
e329aaf3 AH	116	ldmfd sp!, {r4}
47fe2f8d AH	117	bx lr
47fe2f8d AH	118