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_ARM/HAL_CA9.c@68:d3d0e710b443, 2015-03-12 (annotated)

Committer:
mbed_official
Date:
Thu Mar 12 14:31:19 2015 +0000
Revision:
68:d3d0e710b443
Parent:
67:63988a2238f7
Child:
92:bc9729798a19
Synchronized with git revision 051854181516992fb498d51f9ee6e70cbad9e083



Full URL: https://github.com/mbedmicro/mbed/commit/051854181516992fb498d51f9ee6e70cbad9e083/



Fix ksdk mcu HAL - stopbit

Who changed what in which revision?

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