Project5_Software / RTOS

Versie 0.2 Het versturen via de NRF werkt nog niet helemaal omdat er per 4 bytes verstuurd moet worden. Wordt gefixt d.m.v. dynamic stuff!

Dependencies:   BufferedSerial

Dependents:   rtos_basic rtos_basic

Fork of mbed-rtos by mbed official

rtx/TARGET_CORTEX_A/ARM/HAL_CA9.c@49:77c8e4604045, 2014-11-06 (annotated)

Committer:
mbed_official
Date:
Thu Nov 06 13:00:11 2014 +0000
Revision:
49:77c8e4604045
Parent:
rtx_ca/ARM/HAL_CA9.c@48:e9a2c7cb57a4 
Synchronized with git revision 7b90c2ba137baaf9769219e0e8a7b8e8d1299c4f



Full URL: https://github.com/mbedmicro/mbed/commit/7b90c2ba137baaf9769219e0e8a7b8e8d1299c4f/



This target is not yet tested, so it can't be released as part of the official

SDK build for now.

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 48:e9a2c7cb57a4 18 * - Neither the name of ARM nor the names of its contributors may be used
mbed_official 48:e9a2c7cb57a4 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 48:e9a2c7cb57a4 22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
mbed_official 48:e9a2c7cb57a4 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 48:e9a2c7cb57a4 27 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
mbed_official 48:e9a2c7cb57a4 28 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
mbed_official 48:e9a2c7cb57a4 29 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
mbed_official 48:e9a2c7cb57a4 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 *---------------------------------------------------------------------------*/