| /* |
| * 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 |
| |