119 lines
2.5 KiB
ArmAsm
119 lines
2.5 KiB
ArmAsm
|
/*
|
||
|
|
* 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
|
||
|
|
|