2 * Copyright (C) 2008 Google, Inc.
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.
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.
15 #include <linux/linkage.h>
16 #include <asm/assembler.h>
22 /* fiq stack: r0-r15,cpsr,spsr of interrupted mode */
25 /* store pc, cpsr from previous mode, reserve space for spsr */
34 /* store r8-r14 from previous mode */
39 /* store r0-r7 from previous mode */
42 /* setup func(data,regs) arguments */
49 /* Get sp and lr from non-user modes */
50 and r4, r12, #MODE_MASK
55 orr r4, r4, #(PSR_I_BIT | PSR_F_BIT)
57 str sp, [r7, #(4 * 13)]
58 str lr, [r7, #(4 * 14)]
60 str r5, [r7, #(4 * 17)]
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)
67 msr cpsr_c, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT)
70 stmfd sp!, {r2, ip, lr}
71 /* call func(data,regs) */
73 ldmfd sp, {r2, ip, lr}
76 /* restore/discard saved state */
78 beq fiq_from_usr_mode_exit
81 ldr sp, [r7, #(4 * 13)]
82 ldr lr, [r7, #(4 * 14)]
85 fiq_from_usr_mode_exit:
86 msr cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT)
89 ldr lr, [sp, #(4 * 7)]
90 ldr r12, [sp, #(4 * 8)]
97 bx r11 /* jump to custom fiq return function */
100 orr r12, r12, #(PSR_F_BIT)
105 ENTRY(fiq_glue_setup) /* func, data, sp, smc call number */
108 msr cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT)