1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
| /*
| * 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)
| ARM( 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
|
| THUMB( add r7, r7, #(4 * 8) )
| THUMB( stmia r7, {r8-r12} )
| THUMB( sub r7, r7, #(4 * 8) )
|
| 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 */
| THUMB( subne r6, r7, #(4 * 3) )
| THUMB( movne sp, r6 )
| ARM( subne sp, r7, #(4 * 3) )
|
| fiq_from_usr_mode:
| THUMB( mov r6, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT) )
| THUMB( msr cpsr_c, r6 )
| ARM( msr cpsr_c, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT) )
| mov r2, sp
| THUMB( sub r6, r7, #12 )
| THUMB( mov sp, r6 )
| ARM( 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:
| THUMB( mov r6, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) )
| THUMB( msr cpsr_c, r6 )
| ARM( 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
| THUMB( mov r6, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) )
| THUMB( msr cpsr_c, r6 )
| ARM( 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
|
|
