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 ARM( 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 58 THUMB( add r7, r7, #(4 * 8) ) 59 THUMB( stmia r7, {r8-r12} ) 60 THUMB( sub r7, r7, #(4 * 8) ) 61 62 str sp, [r7, #(4 * 13)] 63 str lr, [r7, #(4 * 14)] 64 mrs r5, spsr 65 str r5, [r7, #(4 * 17)] 66 67 cmp r4, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT) 68 /* use fiq stack if we reenter this mode */ 69 THUMB( subne r6, r7, #(4 * 3) ) 70 THUMB( movne sp, r6 ) 71 ARM( subne sp, r7, #(4 * 3) ) 72 73fiq_from_usr_mode: 74 THUMB( mov r6, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT) ) 75 THUMB( msr cpsr_c, r6 ) 76 ARM( msr cpsr_c, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT) ) 77 mov r2, sp 78 THUMB( sub r6, r7, #12 ) 79 THUMB( mov sp, r6 ) 80 ARM( sub sp, r7, #12 ) 81 stmfd sp!, {r2, ip, lr} 82 /* call func(data,regs) */ 83 blx r3 84 ldmfd sp, {r2, ip, lr} 85 mov sp, r2 86 87 /* restore/discard saved state */ 88 cmp r4, #USR_MODE 89 beq fiq_from_usr_mode_exit 90 91 msr cpsr_c, r4 92 ldr sp, [r7, #(4 * 13)] 93 ldr lr, [r7, #(4 * 14)] 94 msr spsr_cxsf, r5 95 96fiq_from_usr_mode_exit: 97 THUMB( mov r6, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) ) 98 THUMB( msr cpsr_c, r6 ) 99 ARM( msr cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) ) 100 101 ldmfd sp!, {r0-r7} 102 ldr lr, [sp, #(4 * 7)] 103 ldr r12, [sp, #(4 * 8)] 104 add sp, sp, #(10 * 4) 105exit_fiq: 106 msr spsr_cxsf, r12 107 add r10, #1 108 cmp r11, #0 109 moveqs pc, lr 110 bx r11 /* jump to custom fiq return function */ 111 112nested_fiq: 113 orr r12, r12, #(PSR_F_BIT) 114 b exit_fiq 115 116fiq_glue_end: 117 118ENTRY(fiq_glue_setup) /* func, data, sp, smc call number */ 119 stmfd sp!, {r4} 120 mrs r4, cpsr 121 THUMB( mov r6, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) ) 122 THUMB( msr cpsr_c, r6 ) 123 ARM( msr cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) ) 124 movs r8, r0 125 mov r9, r1 126 mov sp, r2 127 mov r11, r3 128 moveq r10, #0 129 movne r10, #1 130 msr cpsr_c, r4 131 ldmfd sp!, {r4} 132 bx lr 133 134