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 24ENTRY(fiq_glue) 25 /* store pc, cpsr from previous mode, reserve space for spsr */ 26 mrs r12, spsr 27 sub lr, lr, #4 28 subs r10, #1 29 bne nested_fiq 30 31 str r12, [sp, #-8]! 32 str lr, [sp, #-4]! 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 66fiq_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 85fiq_from_usr_mode_exit: 86 msr cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) 87 88 ldmfd sp!, {r0-r7} 89 ldr lr, [sp, #(4 * 7)] 90 ldr r12, [sp, #(4 * 8)] 91 add sp, sp, #(10 * 4) 92exit_fiq: 93 msr spsr_cxsf, r12 94 add r10, #1 95 cmp r11, #0 96 moveqs pc, lr 97 bx r11 /* jump to custom fiq return function */ 98 99nested_fiq: 100 orr r12, r12, #(PSR_F_BIT) 101 b exit_fiq 102 103fiq_glue_end: 104 105ENTRY(fiq_glue_setup) /* func, data, sp, smc call number */ 106 stmfd sp!, {r4} 107 mrs r4, cpsr 108 msr cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) 109 movs r8, r0 110 mov r9, r1 111 mov sp, r2 112 mov r11, r3 113 moveq r10, #0 114 movne r10, #1 115 msr cpsr_c, r4 116 ldmfd sp!, {r4} 117 bx lr 118 119