context_iar.S 10.4 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23
;/*
; * Copyright (c) 2006-2018, RT-Thread Development Team
; *
; * SPDX-License-Identifier: Apache-2.0
; *
; * Change Logs:
; * Date           Author       Notes
; * 2009-01-17     Bernard      first version
; * 2009-09-27     Bernard      add protect when contex switch occurs
; * 2012-01-01     aozima       support context switch load/store FPU register.
; * 2013-06-18     aozima       add restore MSP feature.
; * 2013-06-23     aozima       support lazy stack optimized.
; * 2018-07-24     aozima       enhancement hard fault exception handler.
; */

;/**
; * @addtogroup cortex-m33
; */
;/*@{*/

SCB_VTOR        EQU     0xE000ED08               ; Vector Table Offset Register
NVIC_INT_CTRL   EQU     0xE000ED04               ; interrupt control state register
NVIC_SYSPRI2    EQU     0xE000ED20               ; system priority register (2)
24
NVIC_PENDSV_PRI EQU     0xFFFF0000               ; PendSV and SysTick priority value (lowest)
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 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233
NVIC_PENDSVSET  EQU     0x10000000               ; value to trigger PendSV exception

    SECTION    .text:CODE(2)
    THUMB
    REQUIRE8
    PRESERVE8

    IMPORT rt_thread_switch_interrupt_flag
    IMPORT rt_interrupt_from_thread
    IMPORT rt_interrupt_to_thread
    IMPORT rt_trustzone_current_context
    IMPORT rt_trustzone_context_load
    IMPORT rt_trustzone_context_store

;/*
; * rt_base_t rt_hw_interrupt_disable();
; */
    EXPORT rt_hw_interrupt_disable
rt_hw_interrupt_disable:
    MRS     r0, PRIMASK
    CPSID   I
    BX      LR

;/*
; * void rt_hw_interrupt_enable(rt_base_t level);
; */
    EXPORT  rt_hw_interrupt_enable
rt_hw_interrupt_enable:
    MSR     PRIMASK, r0
    BX      LR

;/*
; * void rt_hw_context_switch(rt_uint32 from, rt_uint32 to);
; * r0 --> from
; * r1 --> to
; */
    EXPORT rt_hw_context_switch_interrupt
    EXPORT rt_hw_context_switch
rt_hw_context_switch_interrupt:
rt_hw_context_switch:
    ; set rt_thread_switch_interrupt_flag to 1
    LDR     r2, =rt_thread_switch_interrupt_flag
    LDR     r3, [r2]
    CMP     r3, #1
    BEQ     _reswitch
    MOV     r3, #1
    STR     r3, [r2]

    LDR     r2, =rt_interrupt_from_thread   ; set rt_interrupt_from_thread
    STR     r0, [r2]

_reswitch
    LDR     r2, =rt_interrupt_to_thread     ; set rt_interrupt_to_thread
    STR     r1, [r2]

    LDR     r0, =NVIC_INT_CTRL              ; trigger the PendSV exception (causes context switch)
    LDR     r1, =NVIC_PENDSVSET
    STR     r1, [r0]
    BX      LR

; r0 --> switch from thread stack
; r1 --> switch to thread stack
; psr, pc, lr, r12, r3, r2, r1, r0 are pushed into [from] stack
    EXPORT PendSV_Handler
PendSV_Handler:

    ; disable interrupt to protect context switch
    MRS     r2, PRIMASK
    CPSID   I

    ; get rt_thread_switch_interrupt_flag
    LDR     r0, =rt_thread_switch_interrupt_flag    ; r0 = &rt_thread_switch_interrupt_flag
    LDR     r1, [r0]                                ; r1 = *r1
    CMP     r1, #0x00                               ; compare r1 == 0x00
    BNE     schedule
    MSR     PRIMASK, r2                             ; if r1 == 0x00, do msr PRIMASK, r2
    BX      lr                                      ; if r1 == 0x00, do bx lr

schedule
    PUSH    {r2}                                    ; store interrupt state

    ; clear rt_thread_switch_interrupt_flag to 0
    MOV     r1, #0x00                               ; r1 = 0x00
    STR     r1, [r0]                                ; *r0 = r1

    ; skip register save at the first time
    LDR     r0, =rt_interrupt_from_thread           ; r0 = &rt_interrupt_from_thread
    LDR     r1, [r0]                                ; r1 = *r0
    CBZ     r1, switch_to_thread                    ; if r1 == 0, goto switch_to_thread

    ; Whether TrustZone thread stack exists
    LDR     r1,  =rt_trustzone_current_context      ; r1 = &rt_secure_current_context
    LDR     r1, [r1]                                ; r1 = *r1
    CBZ     r1, contex_ns_store                     ; if r1 == 0, goto contex_ns_store

    ;call TrustZone fun, Save TrustZone stack
    STMFD   sp!, {r0-r1, lr}                        ; push register
    MOV     r0, r1                                  ; r0 = rt_secure_current_context
    BL rt_trustzone_context_store                   ; call TrustZone store fun
    LDMFD   sp!, {r0-r1, lr}                        ; pop register

    ; check break from TrustZone
    MOV     r2, lr                                  ; r2 = lr
    TST     r2, #0x40                               ; if EXC_RETURN[6] is 1, TrustZone stack was used
    BEQ     contex_ns_store                         ; if r2 & 0x40 == 0, goto contex_ns_store

    ; push PSPLIM CONTROL PSP LR current_context to stack
    MRS     r3, psplim                              ; r3 = psplim
    MRS     r4, control                             ; r4 = control
    MRS     r5, psp                                 ; r5 = psp
    STMFD   r5!, {r1-r4}                            ; push to thread stack

    ; update from thread stack pointer
    LDR     r0, [r0]                                ; r0 = rt_thread_switch_interrupt_flag
    STR     r5, [r0]                                ; *r0 = r5
    b switch_to_thread                              ; goto switch_to_thread

contex_ns_store

    MRS     r1, psp                                 ; get from thread stack pointer

#if defined ( __ARMVFP__ )
    TST     lr, #0x10                               ; if(!EXC_RETURN[4])
    BNE     skip_push_fpu
    VSTMDB  r1!, {d8 - d15}                         ; push FPU register s16~s31
skip_push_fpu
#endif

    STMFD   r1!, {r4 - r11}         ; push r4 - r11 register

    LDR     r2,  =rt_trustzone_current_context      ; r2 = &rt_secure_current_context
    LDR     r2, [r2]                                ; r2 = *r2
    MOV     r3, lr                                  ; r3 = lr
    MRS     r4, psplim                              ; r4 = psplim
    MRS     r5, control                             ; r5 = control
    STMFD   r1!, {r2-r5}                            ; push to thread stack

    LDR     r0, [r0]
    STR     r1, [r0]                                ; update from thread stack pointer

switch_to_thread

    LDR     r1, =rt_interrupt_to_thread
    LDR     r1, [r1]
    LDR     r1, [r1]                ; load thread stack pointer

    ; update current TrustZone context
    LDMFD   r1!, {r2-r5}                            ; pop thread stack
    MSR     psplim, r4                              ; psplim = r4
    MSR     control, r5                             ; control = r5
    MOV     lr, r3                                  ; lr = r3
    LDR     r6,  =rt_trustzone_current_context      ; r6 = &rt_secure_current_context
    STR     r2, [r6]                                ; *r6 = r2
    MOV     r0, r2                                  ; r0 = r2

    ; Whether TrustZone thread stack exists
    CBZ     r0, contex_ns_load                      ; if r0 == 0, goto contex_ns_load
    PUSH    {r1, r3}                                ; push lr, thread_stack
    BL rt_trustzone_context_load                    ; call TrustZone load fun
    POP     {r1, r3}                                ; pop lr, thread_stack
    MOV     lr, r3                                  ; lr = r1
    TST     r3, #0x40                               ; if EXC_RETURN[6] is 1, TrustZone stack was used
    BEQ     contex_ns_load                          ; if r1 & 0x40 == 0, goto contex_ns_load
    B pendsv_exit

contex_ns_load
    LDMFD   r1!, {r4 - r11}                         ; pop r4 - r11 register

#if defined ( __ARMVFP__ )
    TST     lr, #0x10                               ; if(!EXC_RETURN[4])
    BNE     skip_pop_fpu
    VLDMIA  r1!, {d8 - d15}                         ; pop FPU register s16~s31
skip_pop_fpu
#endif

pendsv_exit
    MSR     psp, r1                                 ; update stack pointer
    ; restore interrupt
    POP    {r2}
    MSR     PRIMASK, r2

    BX      lr

;/*
; * void rt_hw_context_switch_to(rt_uint32 to);
; * r0 --> to
; */
    EXPORT rt_hw_context_switch_to
rt_hw_context_switch_to:
    LDR     r1, =rt_interrupt_to_thread
    STR     r0, [r1]

#if defined ( __ARMVFP__ )
    ; CLEAR CONTROL.FPCA
    MRS     r2, CONTROL             ; read
    BIC     r2, r2, #0x04           ; modify
    MSR     CONTROL, r2             ; write-back
#endif

    ; set from thread to 0
    LDR     r1, =rt_interrupt_from_thread
    MOV     r0, #0x0
    STR     r0, [r1]

    ; set interrupt flag to 1
    LDR     r1, =rt_thread_switch_interrupt_flag
    MOV     r0, #1
    STR     r0, [r1]

234
    ; set the PendSV and SysTick exception priority
235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300
    LDR     r0, =NVIC_SYSPRI2
    LDR     r1, =NVIC_PENDSV_PRI
    LDR.W   r2, [r0,#0x00]       ; read
    ORR     r1,r1,r2             ; modify
    STR     r1, [r0]             ; write-back

    LDR     r0, =NVIC_INT_CTRL      ; trigger the PendSV exception (causes context switch)
    LDR     r1, =NVIC_PENDSVSET
    STR     r1, [r0]

    ; restore MSP
    LDR     r0, =SCB_VTOR
    LDR     r0, [r0]
    LDR     r0, [r0]
    NOP
    MSR     msp, r0

    ; enable interrupts at processor level
    CPSIE   F
    CPSIE   I

    ; never reach here!

; compatible with old version
    EXPORT rt_hw_interrupt_thread_switch
rt_hw_interrupt_thread_switch:
    BX      lr

    IMPORT rt_hw_hard_fault_exception
    EXPORT HardFault_Handler
HardFault_Handler:

    ; get current context
    MRS     r0, msp                                 ; get fault context from handler.
    TST     lr, #0x04                               ; if(!EXC_RETURN[2])
    BEQ     get_sp_done
    MRS     r0, psp                                 ; get fault context from thread.
get_sp_done

    STMFD   r0!, {r4 - r11}                         ; push r4 - r11 register
    
    LDR     r2,  =rt_trustzone_current_context      ; r2 = &rt_secure_current_context
    LDR     r2, [r2]                                ; r2 = *r2
    MOV     r3, lr                                  ; r3 = lr
    MRS     r4, psplim                              ; r4 = psplim
    MRS     r5, control                             ; r5 = control
    STMFD   r0!, {r2-r5}                            ; push to thread stack

    STMFD   r0!, {lr}                               ; push exec_return register

    TST     lr, #0x04                               ; if(!EXC_RETURN[2])
    BEQ     update_msp
    MSR     psp, r0                                 ; update stack pointer to PSP.
    B       update_done
update_msp
    MSR     msp, r0                                 ; update stack pointer to MSP.
update_done

    PUSH    {lr}
    BL      rt_hw_hard_fault_exception
    POP     {lr}

    ORR     lr, lr, #0x04
    BX      lr

    END