1*4882a593Smuzhiyun/* 2*4882a593Smuzhiyun * Copyright (C) 2008 Google, Inc. 3*4882a593Smuzhiyun * 4*4882a593Smuzhiyun * This software is licensed under the terms of the GNU General Public 5*4882a593Smuzhiyun * License version 2, as published by the Free Software Foundation, and 6*4882a593Smuzhiyun * may be copied, distributed, and modified under those terms. 7*4882a593Smuzhiyun * 8*4882a593Smuzhiyun * This program is distributed in the hope that it will be useful, 9*4882a593Smuzhiyun * but WITHOUT ANY WARRANTY; without even the implied warranty of 10*4882a593Smuzhiyun * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11*4882a593Smuzhiyun * GNU General Public License for more details. 12*4882a593Smuzhiyun * 13*4882a593Smuzhiyun */ 14*4882a593Smuzhiyun 15*4882a593Smuzhiyun#include <linux/linkage.h> 16*4882a593Smuzhiyun#include <asm/assembler.h> 17*4882a593Smuzhiyun 18*4882a593Smuzhiyun .text 19*4882a593Smuzhiyun 20*4882a593Smuzhiyun .global fiq_glue_end 21*4882a593Smuzhiyun 22*4882a593Smuzhiyun /* fiq stack: r0-r15,cpsr,spsr of interrupted mode */ 23*4882a593Smuzhiyun 24*4882a593SmuzhiyunENTRY(fiq_glue) 25*4882a593Smuzhiyun /* store pc, cpsr from previous mode, reserve space for spsr */ 26*4882a593Smuzhiyun mrs r12, spsr 27*4882a593Smuzhiyun sub lr, lr, #4 28*4882a593Smuzhiyun subs r10, #1 29*4882a593Smuzhiyun bne nested_fiq 30*4882a593Smuzhiyun 31*4882a593Smuzhiyun str r12, [sp, #-8]! 32*4882a593Smuzhiyun str lr, [sp, #-4]! 33*4882a593Smuzhiyun 34*4882a593Smuzhiyun /* store r8-r14 from previous mode */ 35*4882a593Smuzhiyun sub sp, sp, #(7 * 4) 36*4882a593Smuzhiyun ARM( stmia sp, {r8-r14}^ ) 37*4882a593Smuzhiyun nop 38*4882a593Smuzhiyun 39*4882a593Smuzhiyun /* store r0-r7 from previous mode */ 40*4882a593Smuzhiyun stmfd sp!, {r0-r7} 41*4882a593Smuzhiyun 42*4882a593Smuzhiyun /* setup func(data,regs) arguments */ 43*4882a593Smuzhiyun mov r0, r9 44*4882a593Smuzhiyun mov r1, sp 45*4882a593Smuzhiyun mov r3, r8 46*4882a593Smuzhiyun 47*4882a593Smuzhiyun mov r7, sp 48*4882a593Smuzhiyun 49*4882a593Smuzhiyun /* Get sp and lr from non-user modes */ 50*4882a593Smuzhiyun and r4, r12, #MODE_MASK 51*4882a593Smuzhiyun cmp r4, #USR_MODE 52*4882a593Smuzhiyun beq fiq_from_usr_mode 53*4882a593Smuzhiyun 54*4882a593Smuzhiyun mov r7, sp 55*4882a593Smuzhiyun orr r4, r4, #(PSR_I_BIT | PSR_F_BIT) 56*4882a593Smuzhiyun msr cpsr_c, r4 57*4882a593Smuzhiyun 58*4882a593Smuzhiyun THUMB( add r7, r7, #(4 * 8) ) 59*4882a593Smuzhiyun THUMB( stmia r7, {r8-r12} ) 60*4882a593Smuzhiyun THUMB( sub r7, r7, #(4 * 8) ) 61*4882a593Smuzhiyun 62*4882a593Smuzhiyun str sp, [r7, #(4 * 13)] 63*4882a593Smuzhiyun str lr, [r7, #(4 * 14)] 64*4882a593Smuzhiyun mrs r5, spsr 65*4882a593Smuzhiyun str r5, [r7, #(4 * 17)] 66*4882a593Smuzhiyun 67*4882a593Smuzhiyun cmp r4, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT) 68*4882a593Smuzhiyun /* use fiq stack if we reenter this mode */ 69*4882a593Smuzhiyun THUMB( subne r6, r7, #(4 * 3) ) 70*4882a593Smuzhiyun THUMB( movne sp, r6 ) 71*4882a593Smuzhiyun ARM( subne sp, r7, #(4 * 3) ) 72*4882a593Smuzhiyun 73*4882a593Smuzhiyunfiq_from_usr_mode: 74*4882a593Smuzhiyun THUMB( mov r6, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT) ) 75*4882a593Smuzhiyun THUMB( msr cpsr_c, r6 ) 76*4882a593Smuzhiyun ARM( msr cpsr_c, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT) ) 77*4882a593Smuzhiyun mov r2, sp 78*4882a593Smuzhiyun THUMB( sub r6, r7, #12 ) 79*4882a593Smuzhiyun THUMB( mov sp, r6 ) 80*4882a593Smuzhiyun ARM( sub sp, r7, #12 ) 81*4882a593Smuzhiyun stmfd sp!, {r2, ip, lr} 82*4882a593Smuzhiyun /* call func(data,regs) */ 83*4882a593Smuzhiyun blx r3 84*4882a593Smuzhiyun ldmfd sp, {r2, ip, lr} 85*4882a593Smuzhiyun mov sp, r2 86*4882a593Smuzhiyun 87*4882a593Smuzhiyun /* restore/discard saved state */ 88*4882a593Smuzhiyun cmp r4, #USR_MODE 89*4882a593Smuzhiyun beq fiq_from_usr_mode_exit 90*4882a593Smuzhiyun 91*4882a593Smuzhiyun msr cpsr_c, r4 92*4882a593Smuzhiyun ldr sp, [r7, #(4 * 13)] 93*4882a593Smuzhiyun ldr lr, [r7, #(4 * 14)] 94*4882a593Smuzhiyun msr spsr_cxsf, r5 95*4882a593Smuzhiyun 96*4882a593Smuzhiyunfiq_from_usr_mode_exit: 97*4882a593Smuzhiyun THUMB( mov r6, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) ) 98*4882a593Smuzhiyun THUMB( msr cpsr_c, r6 ) 99*4882a593Smuzhiyun ARM( msr cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) ) 100*4882a593Smuzhiyun 101*4882a593Smuzhiyun ldmfd sp!, {r0-r7} 102*4882a593Smuzhiyun ldr lr, [sp, #(4 * 7)] 103*4882a593Smuzhiyun ldr r12, [sp, #(4 * 8)] 104*4882a593Smuzhiyun add sp, sp, #(10 * 4) 105*4882a593Smuzhiyunexit_fiq: 106*4882a593Smuzhiyun msr spsr_cxsf, r12 107*4882a593Smuzhiyun add r10, #1 108*4882a593Smuzhiyun cmp r11, #0 109*4882a593Smuzhiyun moveqs pc, lr 110*4882a593Smuzhiyun bx r11 /* jump to custom fiq return function */ 111*4882a593Smuzhiyun 112*4882a593Smuzhiyunnested_fiq: 113*4882a593Smuzhiyun orr r12, r12, #(PSR_F_BIT) 114*4882a593Smuzhiyun b exit_fiq 115*4882a593Smuzhiyun 116*4882a593Smuzhiyunfiq_glue_end: 117*4882a593Smuzhiyun 118*4882a593SmuzhiyunENTRY(fiq_glue_setup) /* func, data, sp, smc call number */ 119*4882a593Smuzhiyun stmfd sp!, {r4} 120*4882a593Smuzhiyun mrs r4, cpsr 121*4882a593Smuzhiyun THUMB( mov r6, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) ) 122*4882a593Smuzhiyun THUMB( msr cpsr_c, r6 ) 123*4882a593Smuzhiyun ARM( msr cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) ) 124*4882a593Smuzhiyun movs r8, r0 125*4882a593Smuzhiyun mov r9, r1 126*4882a593Smuzhiyun mov sp, r2 127*4882a593Smuzhiyun mov r11, r3 128*4882a593Smuzhiyun moveq r10, #0 129*4882a593Smuzhiyun movne r10, #1 130*4882a593Smuzhiyun msr cpsr_c, r4 131*4882a593Smuzhiyun ldmfd sp!, {r4} 132*4882a593Smuzhiyun bx lr 133*4882a593Smuzhiyun 134