mbed official / mbed-rtos

Official mbed Real Time Operating System based on the RTX implementation of the CMSIS-RTOS API open standard.

Dependents:   denki-yohou_b TestY201 Network-RTOS NTPClient_HelloWorld ... more

Deprecated

This is the mbed 2 rtos library. mbed OS 5 integrates the mbed library with mbed-rtos. With this, we have provided thread safety for all mbed APIs. If you'd like to learn about using mbed OS 5, please see the docs.

rtx/TARGET_CORTEX_A/TOOLCHAIN_IAR/HAL_CA9_asm.s@103:5a85840ab54e, 2016-02-18 (annotated)

Committer:
mbed_official
Date:
Thu Feb 18 09:45:27 2016 +0000
Revision:
103:5a85840ab54e
Synchronized with git revision b57f7d56840134d072ca567460a86b77fb7adcf8



Full URL: https://github.com/mbedmicro/mbed/commit/b57f7d56840134d072ca567460a86b77fb7adcf8/



Support of export function to the IAR.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 103:5a85840ab54e 1 /*----------------------------------------------------------------------------
mbed_official 103:5a85840ab54e 2 * RL-ARM - RTX
mbed_official 103:5a85840ab54e 3 *----------------------------------------------------------------------------
mbed_official 103:5a85840ab54e 4 * Name: HAL_CA9.c
mbed_official 103:5a85840ab54e 5 * Purpose: Hardware Abstraction Layer for Cortex-A9
mbed_official 103:5a85840ab54e 6 * Rev.: 8 April 2015
mbed_official 103:5a85840ab54e 7 *----------------------------------------------------------------------------
mbed_official 103:5a85840ab54e 8 *
mbed_official 103:5a85840ab54e 9 * Copyright (c) 2012 - 2015 ARM Limited
mbed_official 103:5a85840ab54e 10 * All rights reserved.
mbed_official 103:5a85840ab54e 11 * Redistribution and use in source and binary forms, with or without
mbed_official 103:5a85840ab54e 12 * modification, are permitted provided that the following conditions are met:
mbed_official 103:5a85840ab54e 13 * - Redistributions of source code must retain the above copyright
mbed_official 103:5a85840ab54e 14 * notice, this list of conditions and the following disclaimer.
mbed_official 103:5a85840ab54e 15 * - Redistributions in binary form must reproduce the above copyright
mbed_official 103:5a85840ab54e 16 * notice, this list of conditions and the following disclaimer in the
mbed_official 103:5a85840ab54e 17 * documentation and/or other materials provided with the distribution.
mbed_official 103:5a85840ab54e 18 * - Neither the name of ARM nor the names of its contributors may be used
mbed_official 103:5a85840ab54e 19 * to endorse or promote products derived from this software without
mbed_official 103:5a85840ab54e 20 * specific prior written permission.
mbed_official 103:5a85840ab54e 21 *
mbed_official 103:5a85840ab54e 22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
mbed_official 103:5a85840ab54e 23 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
mbed_official 103:5a85840ab54e 24 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
mbed_official 103:5a85840ab54e 25 * ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
mbed_official 103:5a85840ab54e 26 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
mbed_official 103:5a85840ab54e 27 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
mbed_official 103:5a85840ab54e 28 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
mbed_official 103:5a85840ab54e 29 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
mbed_official 103:5a85840ab54e 30 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
mbed_official 103:5a85840ab54e 31 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
mbed_official 103:5a85840ab54e 32 * POSSIBILITY OF SUCH DAMAGE.
mbed_official 103:5a85840ab54e 33 *---------------------------------------------------------------------------*/
mbed_official 103:5a85840ab54e 34
mbed_official 103:5a85840ab54e 35 PUBLIC rt_set_PSP
mbed_official 103:5a85840ab54e 36 PUBLIC rt_get_PSP
mbed_official 103:5a85840ab54e 37 PUBLIC _alloc_box
mbed_official 103:5a85840ab54e 38 PUBLIC _free_box
mbed_official 103:5a85840ab54e 39 PUBLIC SWI_Handler
mbed_official 103:5a85840ab54e 40 PUBLIC PendSV_Handler
mbed_official 103:5a85840ab54e 41 PUBLIC OS_Tick_Handler
mbed_official 103:5a85840ab54e 42
mbed_official 103:5a85840ab54e 43 /* macro defines form rt_HAL_CA.h */
mbed_official 103:5a85840ab54e 44 #define CPSR_T_BIT 0x20
mbed_official 103:5a85840ab54e 45 #define CPSR_I_BIT 0x80
mbed_official 103:5a85840ab54e 46 #define CPSR_F_BIT 0x40
mbed_official 103:5a85840ab54e 47
mbed_official 103:5a85840ab54e 48 #define MODE_USR 0x10
mbed_official 103:5a85840ab54e 49 #define MODE_FIQ 0x11
mbed_official 103:5a85840ab54e 50 #define MODE_IRQ 0x12
mbed_official 103:5a85840ab54e 51 #define MODE_SVC 0x13
mbed_official 103:5a85840ab54e 52 #define MODE_ABT 0x17
mbed_official 103:5a85840ab54e 53 #define MODE_UND 0x1B
mbed_official 103:5a85840ab54e 54 #define MODE_SYS 0x1F
mbed_official 103:5a85840ab54e 55
mbed_official 103:5a85840ab54e 56 /* macro defines form rt_TypeDef.h */
mbed_official 103:5a85840ab54e 57 #define TCB_TID 3 /* 'task id' offset */
mbed_official 103:5a85840ab54e 58 #define TCB_STACKF 37 /* 'stack_frame' offset */
mbed_official 103:5a85840ab54e 59 #ifndef __LARGE_PRIV_STACK
mbed_official 103:5a85840ab54e 60 #define TCB_TSTACK 40 /* 'tsk_stack' offset */
mbed_official 103:5a85840ab54e 61 #else
mbed_official 103:5a85840ab54e 62 #define TCB_TSTACK 44 /* 'tsk_stack' offset for LARGE_STACK */
mbed_official 103:5a85840ab54e 63 #endif
mbed_official 103:5a85840ab54e 64
mbed_official 103:5a85840ab54e 65
mbed_official 103:5a85840ab54e 66 IMPORT rt_alloc_box
mbed_official 103:5a85840ab54e 67 IMPORT rt_free_box
mbed_official 103:5a85840ab54e 68 IMPORT os_tsk
mbed_official 103:5a85840ab54e 69 IMPORT GICInterface_BASE
mbed_official 103:5a85840ab54e 70 IMPORT rt_pop_req
mbed_official 103:5a85840ab54e 71 IMPORT os_tick_irqack
mbed_official 103:5a85840ab54e 72 IMPORT rt_systick
mbed_official 103:5a85840ab54e 73
mbed_official 103:5a85840ab54e 74 SECTION `.text`:CODE:ROOT(2)
mbed_official 103:5a85840ab54e 75
mbed_official 103:5a85840ab54e 76 /*----------------------------------------------------------------------------
mbed_official 103:5a85840ab54e 77 * Functions
mbed_official 103:5a85840ab54e 78 *---------------------------------------------------------------------------*/
mbed_official 103:5a85840ab54e 79
mbed_official 103:5a85840ab54e 80 //For A-class, set USR/SYS stack
mbed_official 103:5a85840ab54e 81 //__asm void rt_set_PSP (U32 stack) {
mbed_official 103:5a85840ab54e 82 rt_set_PSP:
mbed_official 103:5a85840ab54e 83 ARM
mbed_official 103:5a85840ab54e 84
mbed_official 103:5a85840ab54e 85 MRS R1, CPSR
mbed_official 103:5a85840ab54e 86 CPS #MODE_SYS ;no effect in USR mode
mbed_official 103:5a85840ab54e 87 ISB
mbed_official 103:5a85840ab54e 88 MOV SP, R0
mbed_official 103:5a85840ab54e 89 MSR CPSR_c, R1 ;no effect in USR mode
mbed_official 103:5a85840ab54e 90 ISB
mbed_official 103:5a85840ab54e 91 BX LR
mbed_official 103:5a85840ab54e 92
mbed_official 103:5a85840ab54e 93 //}
mbed_official 103:5a85840ab54e 94
mbed_official 103:5a85840ab54e 95 //For A-class, get USR/SYS stack
mbed_official 103:5a85840ab54e 96 //__asm U32 rt_get_PSP (void) {
mbed_official 103:5a85840ab54e 97 rt_get_PSP:
mbed_official 103:5a85840ab54e 98 ARM
mbed_official 103:5a85840ab54e 99
mbed_official 103:5a85840ab54e 100 MRS R1, CPSR
mbed_official 103:5a85840ab54e 101 CPS #MODE_SYS ;no effect in USR mode
mbed_official 103:5a85840ab54e 102 ISB
mbed_official 103:5a85840ab54e 103 MOV R0, SP
mbed_official 103:5a85840ab54e 104 MSR CPSR_c, R1 ;no effect in USR mode
mbed_official 103:5a85840ab54e 105 ISB
mbed_official 103:5a85840ab54e 106 BX LR
mbed_official 103:5a85840ab54e 107 //}
mbed_official 103:5a85840ab54e 108
mbed_official 103:5a85840ab54e 109 /*--------------------------- _alloc_box ------------------------------------*/
mbed_official 103:5a85840ab54e 110 //__asm void *_alloc_box (void *box_mem) {
mbed_official 103:5a85840ab54e 111 _alloc_box:
mbed_official 103:5a85840ab54e 112 /* Function wrapper for Unprivileged/Privileged mode. */
mbed_official 103:5a85840ab54e 113 ARM
mbed_official 103:5a85840ab54e 114
mbed_official 103:5a85840ab54e 115 LDR R12,=(rt_alloc_box)
mbed_official 103:5a85840ab54e 116 MRS R2, CPSR
mbed_official 103:5a85840ab54e 117 LSLS R2, R2,#28
mbed_official 103:5a85840ab54e 118 BXNE R12
mbed_official 103:5a85840ab54e 119 SVC 0
mbed_official 103:5a85840ab54e 120 BX LR
mbed_official 103:5a85840ab54e 121 //}
mbed_official 103:5a85840ab54e 122
mbed_official 103:5a85840ab54e 123
mbed_official 103:5a85840ab54e 124 /*--------------------------- _free_box -------------------------------------*/
mbed_official 103:5a85840ab54e 125 //__asm int _free_box (void *box_mem, void *box) {
mbed_official 103:5a85840ab54e 126 _free_box:
mbed_official 103:5a85840ab54e 127 /* Function wrapper for Unprivileged/Privileged mode. */
mbed_official 103:5a85840ab54e 128
mbed_official 103:5a85840ab54e 129 LDR R12,=(rt_free_box)
mbed_official 103:5a85840ab54e 130 MRS R2, CPSR
mbed_official 103:5a85840ab54e 131 LSLS R2, R2,#28
mbed_official 103:5a85840ab54e 132 BXNE R12
mbed_official 103:5a85840ab54e 133 SVC 0
mbed_official 103:5a85840ab54e 134 BX LR
mbed_official 103:5a85840ab54e 135
mbed_official 103:5a85840ab54e 136 //}
mbed_official 103:5a85840ab54e 137
mbed_official 103:5a85840ab54e 138 /*-------------------------- SWI_Handler -----------------------------------*/
mbed_official 103:5a85840ab54e 139
mbed_official 103:5a85840ab54e 140 //#pragma push
mbed_official 103:5a85840ab54e 141 //#pragma arm
mbed_official 103:5a85840ab54e 142 //__asm void SWI_Handler (void) {
mbed_official 103:5a85840ab54e 143 SWI_Handler:
mbed_official 103:5a85840ab54e 144 PRESERVE8
mbed_official 103:5a85840ab54e 145 ARM
mbed_official 103:5a85840ab54e 146
mbed_official 103:5a85840ab54e 147 IMPORT rt_tsk_lock
mbed_official 103:5a85840ab54e 148 IMPORT rt_tsk_unlock
mbed_official 103:5a85840ab54e 149 IMPORT SVC_Count
mbed_official 103:5a85840ab54e 150 IMPORT SVC_Table
mbed_official 103:5a85840ab54e 151 IMPORT rt_stk_check
mbed_official 103:5a85840ab54e 152 IMPORT FPUEnable
mbed_official 103:5a85840ab54e 153 IMPORT scheduler_suspended ; flag set by rt_suspend, cleared by rt_resume, read by SWI_Handler
mbed_official 103:5a85840ab54e 154
mbed_official 103:5a85840ab54e 155 Mode_SVC EQU 0x13
mbed_official 103:5a85840ab54e 156
mbed_official 103:5a85840ab54e 157 SRSDB #Mode_SVC! ; Push LR_SVC and SPRS_SVC onto SVC mode stack
mbed_official 103:5a85840ab54e 158 STR R4,[SP,#-0x4]! ; Push R4 so we can use it as a temp
mbed_official 103:5a85840ab54e 159
mbed_official 103:5a85840ab54e 160 MRS R4,SPSR ; Get SPSR
mbed_official 103:5a85840ab54e 161 TST R4,#CPSR_T_BIT ; Check Thumb Bit
mbed_official 103:5a85840ab54e 162 LDRNEH R4,[LR,#-2] ; Thumb: Load Halfword
mbed_official 103:5a85840ab54e 163 BICNE R4,R4,#0xFF00 ; Extract SVC Number
mbed_official 103:5a85840ab54e 164 LDREQ R4,[LR,#-4] ; ARM: Load Word
mbed_official 103:5a85840ab54e 165 BICEQ R4,R4,#0xFF000000 ; Extract SVC Number
mbed_official 103:5a85840ab54e 166
mbed_official 103:5a85840ab54e 167 /* Lock out systick and re-enable interrupts */
mbed_official 103:5a85840ab54e 168 STMDB SP!,{R0-R3,R12,LR}
mbed_official 103:5a85840ab54e 169
mbed_official 103:5a85840ab54e 170 AND R12, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 103:5a85840ab54e 171 SUB SP, SP, R12 ; Adjust stack
mbed_official 103:5a85840ab54e 172 STMDB SP!,{R12, LR} ; Store stack adjustment and dummy LR to SVC stack
mbed_official 103:5a85840ab54e 173
mbed_official 103:5a85840ab54e 174 BLX rt_tsk_lock
mbed_official 103:5a85840ab54e 175 CPSIE i
mbed_official 103:5a85840ab54e 176
mbed_official 103:5a85840ab54e 177 LDMIA SP!,{R12,LR} ; Get stack adjustment & discard dummy LR
mbed_official 103:5a85840ab54e 178 ADD SP, SP, R12 ; Unadjust stack
mbed_official 103:5a85840ab54e 179
mbed_official 103:5a85840ab54e 180 LDMIA SP!,{R0-R3,R12,LR}
mbed_official 103:5a85840ab54e 181
mbed_official 103:5a85840ab54e 182 CMP R4,#0
mbed_official 103:5a85840ab54e 183 BNE SVC_User
mbed_official 103:5a85840ab54e 184
mbed_official 103:5a85840ab54e 185 MRS R4,SPSR
mbed_official 103:5a85840ab54e 186 STR R4,[SP,#-0x4]! ; Push R4 so we can use it as a temp
mbed_official 103:5a85840ab54e 187 AND R4, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 103:5a85840ab54e 188 SUB SP, SP, R4 ; Adjust stack
mbed_official 103:5a85840ab54e 189 STMDB SP!,{R4, LR} ; Store stack adjustment and dummy LR
mbed_official 103:5a85840ab54e 190 BLX R12
mbed_official 103:5a85840ab54e 191 LDMIA SP!,{R4, LR} ; Get stack adjustment & discard dummy LR
mbed_official 103:5a85840ab54e 192 ADD SP, SP, R4 ; Unadjust stack
mbed_official 103:5a85840ab54e 193 LDR R4,[SP],#0x4 ; Restore R4
mbed_official 103:5a85840ab54e 194 MSR SPSR_CXSF,R4
mbed_official 103:5a85840ab54e 195
mbed_official 103:5a85840ab54e 196 /* Here we will be in SVC mode (even if coming in from PendSV_Handler or OS_Tick_Handler) */
mbed_official 103:5a85840ab54e 197 Sys_Switch:
mbed_official 103:5a85840ab54e 198 LDR LR,=(os_tsk)
mbed_official 103:5a85840ab54e 199 LDMIA LR,{R4,LR} ; os_tsk.run, os_tsk.new
mbed_official 103:5a85840ab54e 200 CMP R4,LR
mbed_official 103:5a85840ab54e 201 BNE switching
mbed_official 103:5a85840ab54e 202
mbed_official 103:5a85840ab54e 203 STMDB SP!,{R0-R3,R12,LR}
mbed_official 103:5a85840ab54e 204
mbed_official 103:5a85840ab54e 205 AND R12, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 103:5a85840ab54e 206 SUB SP, SP, R12 ; Adjust stack
mbed_official 103:5a85840ab54e 207 STMDB SP!,{R12,LR} ; Store stack adjustment and dummy LR to SVC stack
mbed_official 103:5a85840ab54e 208
mbed_official 103:5a85840ab54e 209 CPSID i
mbed_official 103:5a85840ab54e 210 ; Do not unlock scheduler if it has just been suspended by rt_suspend()
mbed_official 103:5a85840ab54e 211 LDR R1,=scheduler_suspended
mbed_official 103:5a85840ab54e 212 LDRB R0, [R1]
mbed_official 103:5a85840ab54e 213 CMP R0, #1
mbed_official 103:5a85840ab54e 214 BEQ dont_unlock
mbed_official 103:5a85840ab54e 215 BLX rt_tsk_unlock
mbed_official 103:5a85840ab54e 216 dont_unlock:
mbed_official 103:5a85840ab54e 217
mbed_official 103:5a85840ab54e 218 LDMIA SP!,{R12,LR} ; Get stack adjustment & discard dummy LR
mbed_official 103:5a85840ab54e 219 ADD SP, SP, R12 ; Unadjust stack
mbed_official 103:5a85840ab54e 220
mbed_official 103:5a85840ab54e 221 LDMIA SP!,{R0-R3,R12,LR}
mbed_official 103:5a85840ab54e 222 LDR R4,[SP],#0x4
mbed_official 103:5a85840ab54e 223 RFEFD SP! ; Return from exception, no task switch
mbed_official 103:5a85840ab54e 224
mbed_official 103:5a85840ab54e 225 switching:
mbed_official 103:5a85840ab54e 226 CLREX
mbed_official 103:5a85840ab54e 227 CMP R4,#0
mbed_official 103:5a85840ab54e 228 ADDEQ SP,SP,#12 ; Original R4, LR & SPSR do not need to be popped when we are paging in a different task
mbed_official 103:5a85840ab54e 229 BEQ SVC_Next ; Runtask deleted?
mbed_official 103:5a85840ab54e 230
mbed_official 103:5a85840ab54e 231
mbed_official 103:5a85840ab54e 232 STMDB SP!,{R8-R11} //R4 and LR already stacked
mbed_official 103:5a85840ab54e 233 MOV R10,R4 ; Preserve os_tsk.run
mbed_official 103:5a85840ab54e 234 MOV R11,LR ; Preserve os_tsk.new
mbed_official 103:5a85840ab54e 235
mbed_official 103:5a85840ab54e 236 ADD R8,SP,#16 ; Unstack R4,LR
mbed_official 103:5a85840ab54e 237 LDMIA R8,{R4,LR}
mbed_official 103:5a85840ab54e 238
mbed_official 103:5a85840ab54e 239 SUB SP,SP,#4 ; Make space on the stack for the next instn
mbed_official 103:5a85840ab54e 240 STMIA SP,{SP}^ ; Put User SP onto stack
mbed_official 103:5a85840ab54e 241 LDR R8,[SP],#0x4 ; Pop User SP into R8
mbed_official 103:5a85840ab54e 242
mbed_official 103:5a85840ab54e 243 MRS R9,SPSR
mbed_official 103:5a85840ab54e 244 STMDB R8!,{R9} ; User CPSR
mbed_official 103:5a85840ab54e 245 STMDB R8!,{LR} ; User PC
mbed_official 103:5a85840ab54e 246 STMDB R8,{LR}^ ; User LR
mbed_official 103:5a85840ab54e 247 SUB R8,R8,#4 ; No writeback for store of User LR
mbed_official 103:5a85840ab54e 248 STMDB R8!,{R0-R3,R12} ; User R0-R3,R12
mbed_official 103:5a85840ab54e 249 MOV R3,R10 ; os_tsk.run
mbed_official 103:5a85840ab54e 250 MOV LR,R11 ; os_tsk.new
mbed_official 103:5a85840ab54e 251 LDMIA SP!,{R9-R12}
mbed_official 103:5a85840ab54e 252 ADD SP,SP,#12 ; Fix up SP for unstack of R4, LR & SPSR
mbed_official 103:5a85840ab54e 253 STMDB R8!,{R4-R7,R9-R12} ; User R4-R11
mbed_official 103:5a85840ab54e 254
mbed_official 103:5a85840ab54e 255 //If applicable, stack VFP/NEON state
mbed_official 103:5a85840ab54e 256 MRC p15,0,R1,c1,c0,2 ; VFP/NEON access enabled? (CPACR)
mbed_official 103:5a85840ab54e 257 AND R2,R1,#0x00F00000
mbed_official 103:5a85840ab54e 258 CMP R2,#0x00F00000
mbed_official 103:5a85840ab54e 259 BNE no_outgoing_vfp
mbed_official 103:5a85840ab54e 260 VMRS R2,FPSCR
mbed_official 103:5a85840ab54e 261 STMDB R8!,{R2,R4} ; Push FPSCR, maintain 8-byte alignment
mbed_official 103:5a85840ab54e 262 //IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
mbed_official 103:5a85840ab54e 263 VSTMDB R8!,{D0-D15}
mbed_official 103:5a85840ab54e 264 VSTMDB R8!,{D16-D31}
mbed_official 103:5a85840ab54e 265 LDRB R2,[R3,#TCB_STACKF] ; Record in TCB that NEON/D32 state is stacked
mbed_official 103:5a85840ab54e 266 ORR R2,R2,#4
mbed_official 103:5a85840ab54e 267 STRB R2,[R3,#TCB_STACKF]
mbed_official 103:5a85840ab54e 268 //ENDIF
mbed_official 103:5a85840ab54e 269
mbed_official 103:5a85840ab54e 270 no_outgoing_vfp:
mbed_official 103:5a85840ab54e 271 STR R8,[R3,#TCB_TSTACK]
mbed_official 103:5a85840ab54e 272 MOV R4,LR
mbed_official 103:5a85840ab54e 273
mbed_official 103:5a85840ab54e 274 STR R4,[SP,#-0x4]! ; Push R4 so we can use it as a temp
mbed_official 103:5a85840ab54e 275 AND R4, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 103:5a85840ab54e 276 SUB SP, SP, R4 ; Adjust stack
mbed_official 103:5a85840ab54e 277 STMDB SP!,{R4, LR} ; Store stack adjustment and dummy LR to SVC stack
mbed_official 103:5a85840ab54e 278
mbed_official 103:5a85840ab54e 279 BLX rt_stk_check
mbed_official 103:5a85840ab54e 280
mbed_official 103:5a85840ab54e 281 LDMIA SP!,{R4, LR} ; Get stack adjustment & discard dummy LR
mbed_official 103:5a85840ab54e 282 ADD SP, SP, R4 ; Unadjust stack
mbed_official 103:5a85840ab54e 283 LDR R4,[SP],#0x4 ; Restore R4
mbed_official 103:5a85840ab54e 284
mbed_official 103:5a85840ab54e 285 MOV LR,R4
mbed_official 103:5a85840ab54e 286
mbed_official 103:5a85840ab54e 287 SVC_Next: //R4 == os_tsk.run, LR == os_tsk.new, R0-R3, R5-R12 corruptible
mbed_official 103:5a85840ab54e 288 LDR R1,=(os_tsk) ; os_tsk.run = os_tsk.new
mbed_official 103:5a85840ab54e 289 STR LR,[R1]
mbed_official 103:5a85840ab54e 290 LDRB R1,[LR,#TCB_TID] ; os_tsk.run->task_id
mbed_official 103:5a85840ab54e 291 LSL R1,R1,#8 ; Store PROCID
mbed_official 103:5a85840ab54e 292 MCR p15,0,R1,c13,c0,1 ; Write CONTEXTIDR
mbed_official 103:5a85840ab54e 293
mbed_official 103:5a85840ab54e 294 LDR R0,[LR,#TCB_TSTACK] ; os_tsk.run->tsk_stack
mbed_official 103:5a85840ab54e 295
mbed_official 103:5a85840ab54e 296 //Does incoming task have VFP/NEON state in stack?
mbed_official 103:5a85840ab54e 297 LDRB R3,[LR,#TCB_STACKF]
mbed_official 103:5a85840ab54e 298 ANDS R3, R3, #0x6
mbed_official 103:5a85840ab54e 299 MRC p15,0,R1,c1,c0,2 ; Read CPACR
mbed_official 103:5a85840ab54e 300 BICEQ R1,R1,#0x00F00000 ; Disable VFP/NEON access if incoming task does not have stacked VFP/NEON state
mbed_official 103:5a85840ab54e 301 ORRNE R1,R1,#0x00F00000 ; Enable VFP/NEON access if incoming task does have stacked VFP/NEON state
mbed_official 103:5a85840ab54e 302 MCR p15,0,R1,c1,c0,2 ; Write CPACR
mbed_official 103:5a85840ab54e 303 BEQ no_incoming_vfp
mbed_official 103:5a85840ab54e 304 ISB ; We only need the sync if we enabled, otherwise we will context switch before next VFP/NEON instruction anyway
mbed_official 103:5a85840ab54e 305 //IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
mbed_official 103:5a85840ab54e 306 VLDMIA R0!,{D16-D31}
mbed_official 103:5a85840ab54e 307 //ENDIF
mbed_official 103:5a85840ab54e 308 VLDMIA R0!,{D0-D15}
mbed_official 103:5a85840ab54e 309 LDR R2,[R0]
mbed_official 103:5a85840ab54e 310 VMSR FPSCR,R2
mbed_official 103:5a85840ab54e 311 ADD R0,R0,#8
mbed_official 103:5a85840ab54e 312
mbed_official 103:5a85840ab54e 313 no_incoming_vfp:
mbed_official 103:5a85840ab54e 314 LDR R1,[R0,#60] ; Restore User CPSR
mbed_official 103:5a85840ab54e 315 MSR SPSR_CXSF,R1
mbed_official 103:5a85840ab54e 316 LDMIA R0!,{R4-R11} ; Restore User R4-R11
mbed_official 103:5a85840ab54e 317 ADD R0,R0,#4 ; Restore User R1-R3,R12
mbed_official 103:5a85840ab54e 318 LDMIA R0!,{R1-R3,R12}
mbed_official 103:5a85840ab54e 319 LDMIA R0,{LR}^ ; Restore User LR
mbed_official 103:5a85840ab54e 320 ADD R0,R0,#4 ; No writeback for load to user LR
mbed_official 103:5a85840ab54e 321 LDMIA R0!,{LR} ; Restore User PC
mbed_official 103:5a85840ab54e 322 ADD R0,R0,#4 ; Correct User SP for unstacked user CPSR
mbed_official 103:5a85840ab54e 323
mbed_official 103:5a85840ab54e 324 STR R0,[SP,#-0x4]! ; Push R0 onto stack
mbed_official 103:5a85840ab54e 325 LDMIA SP,{SP}^ ; Get R0 off stack into User SP
mbed_official 103:5a85840ab54e 326 ADD SP,SP,#4 ; Put SP back
mbed_official 103:5a85840ab54e 327
mbed_official 103:5a85840ab54e 328 LDR R0,[R0,#-32] ; Restore R0
mbed_official 103:5a85840ab54e 329
mbed_official 103:5a85840ab54e 330 STMDB SP!,{R0-R3,R12,LR}
mbed_official 103:5a85840ab54e 331
mbed_official 103:5a85840ab54e 332 AND R12, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 103:5a85840ab54e 333 SUB SP, SP, R12 ; Adjust stack
mbed_official 103:5a85840ab54e 334 STMDB sp!,{R12, LR} ; Store stack adjustment and dummy LR to SVC stack
mbed_official 103:5a85840ab54e 335
mbed_official 103:5a85840ab54e 336 CPSID i
mbed_official 103:5a85840ab54e 337 BLX rt_tsk_unlock
mbed_official 103:5a85840ab54e 338
mbed_official 103:5a85840ab54e 339 LDMIA sp!,{R12, LR} ; Get stack adjustment & discard dummy LR
mbed_official 103:5a85840ab54e 340 ADD SP, SP, R12 ; Unadjust stack
mbed_official 103:5a85840ab54e 341
mbed_official 103:5a85840ab54e 342 LDMIA SP!,{R0-R3,R12,LR}
mbed_official 103:5a85840ab54e 343
mbed_official 103:5a85840ab54e 344 MOVS PC,LR ; Return from exception
mbed_official 103:5a85840ab54e 345
mbed_official 103:5a85840ab54e 346
mbed_official 103:5a85840ab54e 347 /*------------------- User SVC -------------------------------*/
mbed_official 103:5a85840ab54e 348
mbed_official 103:5a85840ab54e 349 SVC_User:
mbed_official 103:5a85840ab54e 350 LDR R12,=SVC_Count
mbed_official 103:5a85840ab54e 351 LDR R12,[R12]
mbed_official 103:5a85840ab54e 352 CMP R4,R12 ; Check for overflow
mbed_official 103:5a85840ab54e 353 BHI SVC_Done
mbed_official 103:5a85840ab54e 354
mbed_official 103:5a85840ab54e 355 LDR R12,=SVC_Table-4
mbed_official 103:5a85840ab54e 356 LDR R12,[R12,R4,LSL #2] ; Load SVC Function Address
mbed_official 103:5a85840ab54e 357 MRS R4,SPSR ; Save SPSR
mbed_official 103:5a85840ab54e 358 STR R4,[SP,#-0x4]! ; Push R4 so we can use it as a temp
mbed_official 103:5a85840ab54e 359 AND R4, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 103:5a85840ab54e 360 SUB SP, SP, R4 ; Adjust stack
mbed_official 103:5a85840ab54e 361 STMDB SP!,{R4, LR} ; Store stack adjustment and dummy LR
mbed_official 103:5a85840ab54e 362 BLX R12 ; Call SVC Function
mbed_official 103:5a85840ab54e 363 LDMIA SP!,{R4, LR} ; Get stack adjustment & discard dummy LR
mbed_official 103:5a85840ab54e 364 ADD SP, SP, R4 ; Unadjust stack
mbed_official 103:5a85840ab54e 365 LDR R4,[SP],#0x4 ; Restore R4
mbed_official 103:5a85840ab54e 366 MSR SPSR_CXSF,R4 ; Restore SPSR
mbed_official 103:5a85840ab54e 367
mbed_official 103:5a85840ab54e 368 SVC_Done:
mbed_official 103:5a85840ab54e 369 STMDB sp!,{R0-R3,R12,LR}
mbed_official 103:5a85840ab54e 370
mbed_official 103:5a85840ab54e 371 STR R4,[sp,#-0x4]! ; Push R4 so we can use it as a temp
mbed_official 103:5a85840ab54e 372 AND R4, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 103:5a85840ab54e 373 SUB SP, SP, R4 ; Adjust stack
mbed_official 103:5a85840ab54e 374 STMDB SP!,{R4, LR} ; Store stack adjustment and dummy LR
mbed_official 103:5a85840ab54e 375
mbed_official 103:5a85840ab54e 376 CPSID i
mbed_official 103:5a85840ab54e 377 BLX rt_tsk_unlock
mbed_official 103:5a85840ab54e 378
mbed_official 103:5a85840ab54e 379 LDMIA SP!,{R4, LR} ; Get stack adjustment & discard dummy LR
mbed_official 103:5a85840ab54e 380 ADD SP, SP, R4 ; Unadjust stack
mbed_official 103:5a85840ab54e 381 LDR R4,[SP],#0x4 ; Restore R4
mbed_official 103:5a85840ab54e 382
mbed_official 103:5a85840ab54e 383 LDMIA SP!,{R0-R3,R12,LR}
mbed_official 103:5a85840ab54e 384 LDR R4,[SP],#0x4
mbed_official 103:5a85840ab54e 385 RFEFD SP! ; Return from exception
mbed_official 103:5a85840ab54e 386 //}
mbed_official 103:5a85840ab54e 387 //#pragma pop
mbed_official 103:5a85840ab54e 388
mbed_official 103:5a85840ab54e 389 //#pragma push
mbed_official 103:5a85840ab54e 390 //#pragma arm
mbed_official 103:5a85840ab54e 391 //__asm void PendSV_Handler (U32 IRQn) {
mbed_official 103:5a85840ab54e 392 PendSV_Handler:
mbed_official 103:5a85840ab54e 393 ARM
mbed_official 103:5a85840ab54e 394
mbed_official 103:5a85840ab54e 395 IMPORT rt_tsk_lock
mbed_official 103:5a85840ab54e 396 IMPORT IRQNestLevel ; Flag indicates whether inside an ISR, and the depth of nesting. 0 = not in ISR.
mbed_official 103:5a85840ab54e 397 IMPORT seen_id0_active ; Flag used to workaround GIC 390 errata 733075 - set in startup_Renesas_RZ_A1.s
mbed_official 103:5a85840ab54e 398
mbed_official 103:5a85840ab54e 399 ADD SP,SP,#8 //fix up stack pointer (R0 has been pushed and will never be popped, R1 was pushed for stack alignment)
mbed_official 103:5a85840ab54e 400
mbed_official 103:5a85840ab54e 401 //Disable systick interrupts, then write EOIR. We want interrupts disabled before we enter the context switcher.
mbed_official 103:5a85840ab54e 402 STMDB SP!,{R0, R1}
mbed_official 103:5a85840ab54e 403 BLX rt_tsk_lock
mbed_official 103:5a85840ab54e 404 LDMIA SP!,{R0, R1}
mbed_official 103:5a85840ab54e 405 LDR R1,=(GICInterface_BASE)
mbed_official 103:5a85840ab54e 406 LDR R1, [R1, #0]
mbed_official 103:5a85840ab54e 407 STR R0, [R1, #0x10]
mbed_official 103:5a85840ab54e 408
mbed_official 103:5a85840ab54e 409 ; If it was interrupt ID0, clear the seen flag, otherwise return as normal
mbed_official 103:5a85840ab54e 410 CMP R0, #0
mbed_official 103:5a85840ab54e 411 LDREQ R1, =seen_id0_active
mbed_official 103:5a85840ab54e 412 STRBEQ R0, [R1] ; Clear the seen flag, using R0 (which is 0), to save loading another register
mbed_official 103:5a85840ab54e 413
mbed_official 103:5a85840ab54e 414 LDR R0, =IRQNestLevel ; Get address of nesting counter
mbed_official 103:5a85840ab54e 415 LDR R1, [R0]
mbed_official 103:5a85840ab54e 416 SUB R1, R1, #1 ; Decrement nesting counter
mbed_official 103:5a85840ab54e 417 STR R1, [R0]
mbed_official 103:5a85840ab54e 418
mbed_official 103:5a85840ab54e 419 BLX (rt_pop_req)
mbed_official 103:5a85840ab54e 420
mbed_official 103:5a85840ab54e 421 LDMIA SP!,{R1, LR} ; Get stack adjustment & discard dummy LR
mbed_official 103:5a85840ab54e 422 ADD SP, SP, R1 ; Unadjust stack
mbed_official 103:5a85840ab54e 423
mbed_official 103:5a85840ab54e 424 LDR R0,[SP,#24]
mbed_official 103:5a85840ab54e 425 MSR SPSR_CXSF,R0
mbed_official 103:5a85840ab54e 426 LDMIA SP!,{R0-R3,R12} ; Leave SPSR & LR on the stack
mbed_official 103:5a85840ab54e 427 STR R4,[SP,#-0x4]!
mbed_official 103:5a85840ab54e 428 B Sys_Switch
mbed_official 103:5a85840ab54e 429 //}
mbed_official 103:5a85840ab54e 430 //#pragma pop
mbed_official 103:5a85840ab54e 431
mbed_official 103:5a85840ab54e 432
mbed_official 103:5a85840ab54e 433 //#pragma push
mbed_official 103:5a85840ab54e 434 //#pragma arm
mbed_official 103:5a85840ab54e 435 //__asm void OS_Tick_Handler (U32 IRQn) {
mbed_official 103:5a85840ab54e 436 OS_Tick_Handler:
mbed_official 103:5a85840ab54e 437 ARM
mbed_official 103:5a85840ab54e 438
mbed_official 103:5a85840ab54e 439 IMPORT rt_tsk_lock
mbed_official 103:5a85840ab54e 440 IMPORT IRQNestLevel ; Flag indicates whether inside an ISR, and the depth of nesting. 0 = not in ISR.
mbed_official 103:5a85840ab54e 441 IMPORT seen_id0_active ; Flag used to workaround GIC 390 errata 733075 - set in startup_Renesas_RZ_A1.s
mbed_official 103:5a85840ab54e 442
mbed_official 103:5a85840ab54e 443 ADD SP,SP,#8 //fix up stack pointer (R0 has been pushed and will never be popped, R1 was pushed for stack alignment)
mbed_official 103:5a85840ab54e 444
mbed_official 103:5a85840ab54e 445 STMDB SP!,{R0, R1}
mbed_official 103:5a85840ab54e 446 BLX rt_tsk_lock
mbed_official 103:5a85840ab54e 447 LDMIA SP!,{R0, R1}
mbed_official 103:5a85840ab54e 448 LDR R1, =(GICInterface_BASE)
mbed_official 103:5a85840ab54e 449 LDR R1, [R1, #0]
mbed_official 103:5a85840ab54e 450 STR R0, [R1, #0x10]
mbed_official 103:5a85840ab54e 451
mbed_official 103:5a85840ab54e 452 ; If it was interrupt ID0, clear the seen flag, otherwise return as normal
mbed_official 103:5a85840ab54e 453 CMP R0, #0
mbed_official 103:5a85840ab54e 454 LDREQ R1, =seen_id0_active
mbed_official 103:5a85840ab54e 455 STRBEQ R0, [R1] ; Clear the seen flag, using R0 (which is 0), to save loading another register
mbed_official 103:5a85840ab54e 456
mbed_official 103:5a85840ab54e 457 LDR R0, =IRQNestLevel ; Get address of nesting counter
mbed_official 103:5a85840ab54e 458 LDR R1, [R0]
mbed_official 103:5a85840ab54e 459 SUB R1, R1, #1 ; Decrement nesting counter
mbed_official 103:5a85840ab54e 460 STR R1, [R0]
mbed_official 103:5a85840ab54e 461
mbed_official 103:5a85840ab54e 462 BLX (os_tick_irqack)
mbed_official 103:5a85840ab54e 463 BLX (rt_systick)
mbed_official 103:5a85840ab54e 464
mbed_official 103:5a85840ab54e 465 LDMIA SP!,{R1, LR} ; Get stack adjustment & discard dummy LR
mbed_official 103:5a85840ab54e 466 ADD SP, SP, R1 ; Unadjust stack
mbed_official 103:5a85840ab54e 467
mbed_official 103:5a85840ab54e 468 LDR R0,[SP,#24]
mbed_official 103:5a85840ab54e 469 MSR SPSR_CXSF,R0
mbed_official 103:5a85840ab54e 470 LDMIA SP!,{R0-R3,R12} ; Leave SPSR & LR on the stack
mbed_official 103:5a85840ab54e 471 STR R4,[SP,#-0x4]!
mbed_official 103:5a85840ab54e 472 B Sys_Switch
mbed_official 103:5a85840ab54e 473 //}
mbed_official 103:5a85840ab54e 474 //#pragma pop
mbed_official 103:5a85840ab54e 475
mbed_official 103:5a85840ab54e 476
mbed_official 103:5a85840ab54e 477 END
mbed_official 103:5a85840ab54e 478 /*----------------------------------------------------------------------------
mbed_official 103:5a85840ab54e 479 * end of file
mbed_official 103:5a85840ab54e 480 *---------------------------------------------------------------------------*/