First version
[3rdparty/ote_partner/tlk.git] / arch / arm / arm / monitor_vectors.S
1 /*
2  * Copyright (c) 2012-2014, NVIDIA CORPORATION. All rights reserved
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining
5  * a copy of this software and associated documentation files
6  * (the "Software"), to deal in the Software without restriction,
7  * including without limitation the rights to use, copy, modify, merge,
8  * publish, distribute, sublicense, and/or sell copies of the Software,
9  * and to permit persons to whom the Software is furnished to do so,
10  * subject to the following conditions:
11  *
12  * The above copyright notice and this permission notice shall be
13  * included in all copies or substantial portions of the Software.
14  *
15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
18  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
19  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
20  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
21  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22  */
23
24 #include <arch/arm/monitor_vectors.h>
25 #include <lib/monitor/monitor_vector.h>
26
27 #define L1_CACHE_ALIGN          5
28
29 /* loads a 32-bit value into a register */
30 .macro mov32, reg, val
31         movw    \reg, #:lower16:\val
32         movt    \reg, #:upper16:\val
33 .endm
34
35 /*****************************************************************************/
36 /* The monitor entry table stuff                                             */
37 /*****************************************************************************/
38 .p2align 5
39 .globl monitor_vector_base
40 monitor_vector_base:
41         nop                             /* RESET        */
42         b       mon_undef_entry         /* UNDEF        */
43         b       mon_swi_entry           /* SWI          */
44         b       mon_inst_abort_entry    /* IABORT       */
45         b       mon_data_abort_entry    /* DABORT       */
46         nop                             /* reserved     */
47         b       mon_irq_entry           /* IRQ          */
48         b       mon_fiq_entry           /* FIQ          */
49
50 mon_undef_entry:
51         b       .
52
53 mon_swi_entry:
54         b       handle_exception
55
56 mon_inst_abort_entry:
57         b       .
58
59 mon_data_abort_entry:
60         b       .
61
62 mon_irq_entry:
63         b       .
64
65 mon_fiq_entry:
66         b       .
67
68 /*
69  * Go to secure world, handle the exception and then switch back to non-secure
70  * world.
71  *
72  */
73 handle_exception:
74         stmfd   sp!, {r1-r2}            @ create scratch regs
75
76         SWITCH_SCR_TO_SECURE r1
77
78         /* check if is mmu is currently enabled */
79         mrc     p15, 0, r1, c1, c0, 0
80         tst     r1, #0x1
81         bne     cont_handle_exc         @ enabled, just continue
82
83         /* it won't be on exit from LP1, so handle it now */
84         adr     r1, mon_p2v_offset
85         ldr     r2, [r1]
86         add     sp, sp, r2              @ convert sp to virt
87
88         ldr     r2, =monitor_vector_base
89         mcr     p15, 0, r2, c12, c0, 1  @ reload virt mvbar
90
91         mrc     p15, 0, r2, c1, c0, 0
92         orr     r2, r2, #1
93         mcr     p15, 0, r2, c1, c0, 0   @ enable mmu
94
95         ldr     r1, =cont_handle_exc
96         mov     pc, r1                  @ convert pc to virt
97
98 cont_handle_exc:
99         SAVE_NONSECURE_STATE r1, r2     @ save NS state
100         RESTORE_SECURE_STATE r1, r2     @ restore S state
101
102         /* if returning from NS irq, skip CPU save (r1/r2 are still scratch) */
103         mov32   r1, 0x32000005
104         cmp     r0, r1
105         moveq   r0, #0                  @ no incoming SMC
106         ldmia   sp!, {r1-r2}            @ restore scratch regs
107         beq     return_go_nonsecure
108
109         /* if returning from FS req, skip CPU save (r1/r2 are still scratch) */
110         stmfd   sp!, {r1-r2}            @ create scratch regs
111         mov32   r1, 0x32000009
112         cmp     r0, r1
113         moveq   r0, #0                  @ no incoming SMC
114         adreq   r0, fs_return_value
115         ldmia   sp!, {r1-r2}            @ restore scratch regs
116         streq   r1, [r0]                @ return value for the FS request
117         moveq   r0, #0
118         beq     return_go_nonsecure
119
120         /* save tz_monitor_frame to sp_mon (frame in r0) */
121         SAVE_MON_FRAME_TO_STACK
122         mov     r0, sp
123
124 return_go_nonsecure:
125         /* frame in r0 (register not part of restored CPU state) */
126         adr     r1, secure_exit_mode
127         ldr     r2, [r1]                @ get previous exit mode
128         msr     cpsr_cfsx, r2           @ switch to it
129         ldmia   sp!, { r4-r12, pc }     @ restore CPU state from stack (and return)
130
131 /*
132  * Go to nonsecure world
133  *
134  */
135 .globl go_nonsecure
136 go_nonsecure:
137         /* r0 = smc_type, r1 = frame ptr arg */
138         stmfd   sp!, { r4-r12, lr }     @ save CPU state to stack
139
140         adr     r2, secure_exit_mode
141         mrs     r3, cpsr
142         str     r3, [r2]                @ save current mode
143         cpsid   iaf                     @ disable intrs
144
145         SAVE_SECURE_STATE r2, r3
146         RESTORE_NONSECURE_STATE r2, r3
147         SWITCH_SCR_TO_NONSECURE r2
148
149         /* completions always restore from the stack */
150         mov32   r2, SMC_TOS_COMPLETION
151         cmp     r0, r2
152         beq     restore_stack
153
154         /* for INITIAL_NS_RETURN and PREEMPTs, restore from arg */
155         RESTORE_MON_FRAME_FROM_ARG r1
156         movs    pc, r14
157
158 restore_stack:
159         /* restore from monitor stack */
160         RESTORE_MON_FRAME_FROM_STACK
161         movs    pc, r14
162
163
164 .align L1_CACHE_ALIGN
165
166 /* save area for NS and S worlds */
167 secure_state:
168         .rept   SAVED_STATE_WORDS
169         .long   0
170         .endr
171
172 nonsecure_state:
173         .rept   SAVED_STATE_WORDS
174         .long   0
175         .endr
176
177 /* cpsr mode from last secure world exit */
178 secure_exit_mode:
179         .long   0
180
181 .globl  fs_return_value
182 fs_return_value:
183         .long   0
184
185 .globl  mon_stack_top
186 mon_stack_top:
187         .long   0
188
189 .globl  mon_p2v_offset
190 mon_p2v_offset:
191         .long   0