4180 LabTeam - Spring '18 / mbed-rtos

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   Lab2_Part4

Fork of mbed-rtos by mbed official

rtx/TARGET_CORTEX_A/TOOLCHAIN_ARM/HAL_CA9.c@92:bc9729798a19, 2015-09-25 (annotated)

Committer:
mbed_official
Date:
Fri Sep 25 13:30:34 2015 +0100
Revision:
92:bc9729798a19
Parent:
68:d3d0e710b443 
Synchronized with git revision e8c24ba90dd5507bdb7c1b46dd3aba8cfabb762b



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



RZ_A1H - Modify to support NEON for RTOS.

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 92:bc9729798a19 6 * Rev.: 8 April 2015
mbed_official 48:e9a2c7cb57a4 7 *----------------------------------------------------------------------------
mbed_official 48:e9a2c7cb57a4 8 *
mbed_official 92:bc9729798a19 9 * Copyright (c) 2012 - 2015 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 92:bc9729798a19 117 IMPORT scheduler_suspended ; flag set by rt_suspend, cleared by rt_resume, read by SVC_Handler
mbed_official 48:e9a2c7cb57a4 118
mbed_official 48:e9a2c7cb57a4 119 Mode_SVC EQU 0x13
mbed_official 48:e9a2c7cb57a4 120
mbed_official 48:e9a2c7cb57a4 121 SRSFD SP!, #Mode_SVC ; Push LR_SVC and SPRS_SVC onto SVC mode stack
mbed_official 48:e9a2c7cb57a4 122 PUSH {R4} ; Push R4 so we can use it as a temp
mbed_official 48:e9a2c7cb57a4 123
mbed_official 48:e9a2c7cb57a4 124 MRS R4,SPSR ; Get SPSR
mbed_official 48:e9a2c7cb57a4 125 TST R4,#CPSR_T_BIT ; Check Thumb Bit
mbed_official 48:e9a2c7cb57a4 126 LDRNEH R4,[LR,#-2] ; Thumb: Load Halfword
mbed_official 48:e9a2c7cb57a4 127 BICNE R4,R4,#0xFF00 ; Extract SVC Number
mbed_official 48:e9a2c7cb57a4 128 LDREQ R4,[LR,#-4] ; ARM: Load Word
mbed_official 48:e9a2c7cb57a4 129 BICEQ R4,R4,#0xFF000000 ; Extract SVC Number
mbed_official 48:e9a2c7cb57a4 130
mbed_official 48:e9a2c7cb57a4 131 /* Lock out systick and re-enable interrupts */
mbed_official 48:e9a2c7cb57a4 132 PUSH {R0-R3,R12,LR}
mbed_official 48:e9a2c7cb57a4 133
mbed_official 48:e9a2c7cb57a4 134 AND R12, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 48:e9a2c7cb57a4 135 SUB SP, SP, R12 ; Adjust stack
mbed_official 48:e9a2c7cb57a4 136 PUSH {R12, LR} ; Store stack adjustment and dummy LR to SVC stack
mbed_official 48:e9a2c7cb57a4 137
mbed_official 48:e9a2c7cb57a4 138 BLX rt_tsk_lock
mbed_official 48:e9a2c7cb57a4 139 CPSIE i
mbed_official 48:e9a2c7cb57a4 140
mbed_official 48:e9a2c7cb57a4 141 POP {R12, LR} ; Get stack adjustment & discard dummy LR
mbed_official 48:e9a2c7cb57a4 142 ADD SP, SP, R12 ; Unadjust stack
mbed_official 48:e9a2c7cb57a4 143
mbed_official 48:e9a2c7cb57a4 144 POP {R0-R3,R12,LR}
mbed_official 48:e9a2c7cb57a4 145
mbed_official 48:e9a2c7cb57a4 146 CMP R4,#0
mbed_official 48:e9a2c7cb57a4 147 BNE SVC_User
mbed_official 48:e9a2c7cb57a4 148
mbed_official 48:e9a2c7cb57a4 149 MRS R4,SPSR
mbed_official 48:e9a2c7cb57a4 150 PUSH {R4} ; Push R4 so we can use it as a temp
mbed_official 48:e9a2c7cb57a4 151 AND R4, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 48:e9a2c7cb57a4 152 SUB SP, SP, R4 ; Adjust stack
mbed_official 48:e9a2c7cb57a4 153 PUSH {R4, LR} ; Store stack adjustment and dummy LR
mbed_official 48:e9a2c7cb57a4 154 BLX R12
mbed_official 48:e9a2c7cb57a4 155 POP {R4, LR} ; Get stack adjustment & discard dummy LR
mbed_official 48:e9a2c7cb57a4 156 ADD SP, SP, R4 ; Unadjust stack
mbed_official 48:e9a2c7cb57a4 157 POP {R4} ; Restore R4
mbed_official 48:e9a2c7cb57a4 158 MSR SPSR_CXSF,R4
mbed_official 48:e9a2c7cb57a4 159
mbed_official 48:e9a2c7cb57a4 160 /* Here we will be in SVC mode (even if coming in from PendSV_Handler or OS_Tick_Handler) */
mbed_official 48:e9a2c7cb57a4 161 Sys_Switch
mbed_official 48:e9a2c7cb57a4 162 LDR LR,=__cpp(&os_tsk)
mbed_official 92:bc9729798a19 163 LDM LR,{R4,LR} ; os_tsk.run, os_tsk.new_tsk
mbed_official 48:e9a2c7cb57a4 164 CMP R4,LR
mbed_official 48:e9a2c7cb57a4 165 BNE switching
mbed_official 48:e9a2c7cb57a4 166
mbed_official 48:e9a2c7cb57a4 167 PUSH {R0-R3,R12,LR}
mbed_official 48:e9a2c7cb57a4 168
mbed_official 48:e9a2c7cb57a4 169 AND R12, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 48:e9a2c7cb57a4 170 SUB SP, SP, R12 ; Adjust stack
mbed_official 48:e9a2c7cb57a4 171 PUSH {R12, LR} ; Store stack adjustment and dummy LR to SVC stack
mbed_official 48:e9a2c7cb57a4 172
mbed_official 48:e9a2c7cb57a4 173 CPSID i
mbed_official 92:bc9729798a19 174 ; Do not unlock scheduler if it has just been suspended by rt_suspend()
mbed_official 92:bc9729798a19 175 LDR R1,=scheduler_suspended
mbed_official 92:bc9729798a19 176 LDRB R0, [R1]
mbed_official 92:bc9729798a19 177 CMP R0, #1
mbed_official 92:bc9729798a19 178 BEQ dont_unlock
mbed_official 48:e9a2c7cb57a4 179 BLX rt_tsk_unlock
mbed_official 92:bc9729798a19 180 dont_unlock
mbed_official 48:e9a2c7cb57a4 181
mbed_official 48:e9a2c7cb57a4 182 POP {R12, LR} ; Get stack adjustment & discard dummy LR
mbed_official 48:e9a2c7cb57a4 183 ADD SP, SP, R12 ; Unadjust stack
mbed_official 48:e9a2c7cb57a4 184
mbed_official 48:e9a2c7cb57a4 185 POP {R0-R3,R12,LR}
mbed_official 48:e9a2c7cb57a4 186 POP {R4}
mbed_official 48:e9a2c7cb57a4 187 RFEFD SP! ; Return from exception, no task switch
mbed_official 48:e9a2c7cb57a4 188
mbed_official 48:e9a2c7cb57a4 189 switching
mbed_official 48:e9a2c7cb57a4 190 CLREX
mbed_official 48:e9a2c7cb57a4 191 CMP R4,#0
mbed_official 48:e9a2c7cb57a4 192 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 193 BEQ SVC_Next ; Runtask deleted?
mbed_official 48:e9a2c7cb57a4 194
mbed_official 48:e9a2c7cb57a4 195
mbed_official 48:e9a2c7cb57a4 196 PUSH {R8-R11} //R4 and LR already stacked
mbed_official 48:e9a2c7cb57a4 197 MOV R10,R4 ; Preserve os_tsk.run
mbed_official 92:bc9729798a19 198 MOV R11,LR ; Preserve os_tsk.new_tsk
mbed_official 48:e9a2c7cb57a4 199
mbed_official 48:e9a2c7cb57a4 200 ADD R8,SP,#16 ; Unstack R4,LR
mbed_official 48:e9a2c7cb57a4 201 LDMIA R8,{R4,LR}
mbed_official 48:e9a2c7cb57a4 202
mbed_official 48:e9a2c7cb57a4 203 SUB SP,SP,#4 ; Make space on the stack for the next instn
mbed_official 48:e9a2c7cb57a4 204 STMIA SP,{SP}^ ; Put User SP onto stack
mbed_official 48:e9a2c7cb57a4 205 POP {R8} ; Pop User SP into R8
mbed_official 48:e9a2c7cb57a4 206
mbed_official 48:e9a2c7cb57a4 207 MRS R9,SPSR
mbed_official 48:e9a2c7cb57a4 208 STMDB R8!,{R9} ; User CPSR
mbed_official 48:e9a2c7cb57a4 209 STMDB R8!,{LR} ; User PC
mbed_official 48:e9a2c7cb57a4 210 STMDB R8,{LR}^ ; User LR
mbed_official 48:e9a2c7cb57a4 211 SUB R8,R8,#4 ; No writeback for store of User LR
mbed_official 48:e9a2c7cb57a4 212 STMDB R8!,{R0-R3,R12} ; User R0-R3,R12
mbed_official 48:e9a2c7cb57a4 213 MOV R3,R10 ; os_tsk.run
mbed_official 92:bc9729798a19 214 MOV LR,R11 ; os_tsk.new_tsk
mbed_official 48:e9a2c7cb57a4 215 POP {R9-R12}
mbed_official 48:e9a2c7cb57a4 216 ADD SP,SP,#12 ; Fix up SP for unstack of R4, LR & SPSR
mbed_official 48:e9a2c7cb57a4 217 STMDB R8!,{R4-R7,R9-R12} ; User R4-R11
mbed_official 48:e9a2c7cb57a4 218
mbed_official 92:bc9729798a19 219 //If applicable, stack VFP/NEON state
mbed_official 48:e9a2c7cb57a4 220 MRC p15,0,R1,c1,c0,2 ; VFP/NEON access enabled? (CPACR)
mbed_official 48:e9a2c7cb57a4 221 AND R2,R1,#0x00F00000
mbed_official 48:e9a2c7cb57a4 222 CMP R2,#0x00F00000
mbed_official 48:e9a2c7cb57a4 223 BNE no_outgoing_vfp
mbed_official 48:e9a2c7cb57a4 224 VMRS R2,FPSCR
mbed_official 48:e9a2c7cb57a4 225 STMDB R8!,{R2,R4} ; Push FPSCR, maintain 8-byte alignment
mbed_official 92:bc9729798a19 226 VSTMDB R8!,{D0-D15}
mbed_official 92:bc9729798a19 227 VSTMDB R8!,{D16-D31}
mbed_official 92:bc9729798a19 228 LDRB R2,[R3,#TCB_STACKF] ; Record in TCB that NEON/D32 state is stacked
mbed_official 92:bc9729798a19 229 ORR R2,#4
mbed_official 48:e9a2c7cb57a4 230 STRB R2,[R3,#TCB_STACKF]
mbed_official 48:e9a2c7cb57a4 231
mbed_official 48:e9a2c7cb57a4 232 no_outgoing_vfp
mbed_official 48:e9a2c7cb57a4 233 STR R8,[R3,#TCB_TSTACK]
mbed_official 48:e9a2c7cb57a4 234 MOV R4,LR
mbed_official 48:e9a2c7cb57a4 235
mbed_official 48:e9a2c7cb57a4 236 PUSH {R4} ; Push R4 so we can use it as a temp
mbed_official 48:e9a2c7cb57a4 237 AND R4, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 48:e9a2c7cb57a4 238 SUB SP, SP, R4 ; Adjust stack
mbed_official 48:e9a2c7cb57a4 239 PUSH {R4, LR} ; Store stack adjustment and dummy LR to SVC stack
mbed_official 48:e9a2c7cb57a4 240
mbed_official 48:e9a2c7cb57a4 241 BLX rt_stk_check
mbed_official 48:e9a2c7cb57a4 242
mbed_official 48:e9a2c7cb57a4 243 POP {R4, LR} ; Get stack adjustment & discard dummy LR
mbed_official 48:e9a2c7cb57a4 244 ADD SP, SP, R4 ; Unadjust stack
mbed_official 48:e9a2c7cb57a4 245 POP {R4} ; Restore R4
mbed_official 48:e9a2c7cb57a4 246
mbed_official 48:e9a2c7cb57a4 247 MOV LR,R4
mbed_official 48:e9a2c7cb57a4 248
mbed_official 92:bc9729798a19 249 SVC_Next //R4 == os_tsk.run, LR == os_tsk.new_tsk, R0-R3, R5-R12 corruptible
mbed_official 92:bc9729798a19 250 LDR R1,=__cpp(&os_tsk) ; os_tsk.run = os_tsk.new_tsk
mbed_official 48:e9a2c7cb57a4 251 STR LR,[R1]
mbed_official 48:e9a2c7cb57a4 252 LDRB R1,[LR,#TCB_TID] ; os_tsk.run->task_id
mbed_official 48:e9a2c7cb57a4 253 LSL R1,#8 ; Store PROCID
mbed_official 48:e9a2c7cb57a4 254 MCR p15,0,R1,c13,c0,1 ; Write CONTEXTIDR
mbed_official 48:e9a2c7cb57a4 255
mbed_official 48:e9a2c7cb57a4 256 LDR R0,[LR,#TCB_TSTACK] ; os_tsk.run->tsk_stack
mbed_official 48:e9a2c7cb57a4 257
mbed_official 92:bc9729798a19 258 //Does incoming task have VFP/NEON state in stack?
mbed_official 48:e9a2c7cb57a4 259 LDRB R3,[LR,#TCB_STACKF]
mbed_official 92:bc9729798a19 260 ANDS R3, R3, #0x6
mbed_official 48:e9a2c7cb57a4 261 MRC p15,0,R1,c1,c0,2 ; Read CPACR
mbed_official 92:bc9729798a19 262 ANDEQ R1,R1,#0xFF0FFFFF ; Disable VFP/NEON access if incoming task does not have stacked VFP/NEON state
mbed_official 92:bc9729798a19 263 ORRNE R1,R1,#0x00F00000 ; Enable VFP/NEON access if incoming task does have stacked VFP/NEON state
mbed_official 48:e9a2c7cb57a4 264 MCR p15,0,R1,c1,c0,2 ; Write CPACR
mbed_official 48:e9a2c7cb57a4 265 BEQ no_incoming_vfp
mbed_official 92:bc9729798a19 266 ISB ; We only need the sync if we enabled, otherwise we will context switch before next VFP/NEON instruction anyway
mbed_official 92:bc9729798a19 267 VLDMIA R0!,{D16-D31}
mbed_official 92:bc9729798a19 268 VLDMIA R0!,{D0-D15}
mbed_official 48:e9a2c7cb57a4 269 LDR R2,[R0]
mbed_official 48:e9a2c7cb57a4 270 VMSR FPSCR,R2
mbed_official 48:e9a2c7cb57a4 271 ADD R0,R0,#8
mbed_official 48:e9a2c7cb57a4 272
mbed_official 48:e9a2c7cb57a4 273 no_incoming_vfp
mbed_official 48:e9a2c7cb57a4 274 LDR R1,[R0,#60] ; Restore User CPSR
mbed_official 48:e9a2c7cb57a4 275 MSR SPSR_CXSF,R1
mbed_official 48:e9a2c7cb57a4 276 LDMIA R0!,{R4-R11} ; Restore User R4-R11
mbed_official 48:e9a2c7cb57a4 277 ADD R0,R0,#4 ; Restore User R1-R3,R12
mbed_official 48:e9a2c7cb57a4 278 LDMIA R0!,{R1-R3,R12}
mbed_official 48:e9a2c7cb57a4 279 LDMIA R0,{LR}^ ; Restore User LR
mbed_official 48:e9a2c7cb57a4 280 ADD R0,R0,#4 ; No writeback for load to user LR
mbed_official 48:e9a2c7cb57a4 281 LDMIA R0!,{LR} ; Restore User PC
mbed_official 48:e9a2c7cb57a4 282 ADD R0,R0,#4 ; Correct User SP for unstacked user CPSR
mbed_official 48:e9a2c7cb57a4 283
mbed_official 48:e9a2c7cb57a4 284 PUSH {R0} ; Push R0 onto stack
mbed_official 48:e9a2c7cb57a4 285 LDMIA SP,{SP}^ ; Get R0 off stack into User SP
mbed_official 48:e9a2c7cb57a4 286 ADD SP,SP,#4 ; Put SP back
mbed_official 48:e9a2c7cb57a4 287
mbed_official 48:e9a2c7cb57a4 288 LDR R0,[R0,#-32] ; Restore R0
mbed_official 48:e9a2c7cb57a4 289
mbed_official 48:e9a2c7cb57a4 290 PUSH {R0-R3,R12,LR}
mbed_official 48:e9a2c7cb57a4 291
mbed_official 48:e9a2c7cb57a4 292 AND R12, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 48:e9a2c7cb57a4 293 SUB SP, SP, R12 ; Adjust stack
mbed_official 48:e9a2c7cb57a4 294 PUSH {R12, LR} ; Store stack adjustment and dummy LR to SVC stack
mbed_official 48:e9a2c7cb57a4 295
mbed_official 48:e9a2c7cb57a4 296 CPSID i
mbed_official 48:e9a2c7cb57a4 297 BLX rt_tsk_unlock
mbed_official 48:e9a2c7cb57a4 298
mbed_official 48:e9a2c7cb57a4 299 POP {R12, LR} ; Get stack adjustment & discard dummy LR
mbed_official 48:e9a2c7cb57a4 300 ADD SP, SP, R12 ; Unadjust stack
mbed_official 48:e9a2c7cb57a4 301
mbed_official 48:e9a2c7cb57a4 302 POP {R0-R3,R12,LR}
mbed_official 48:e9a2c7cb57a4 303
mbed_official 48:e9a2c7cb57a4 304 MOVS PC,LR ; Return from exception
mbed_official 48:e9a2c7cb57a4 305
mbed_official 48:e9a2c7cb57a4 306
mbed_official 48:e9a2c7cb57a4 307 /*------------------- User SVC -------------------------------*/
mbed_official 48:e9a2c7cb57a4 308
mbed_official 48:e9a2c7cb57a4 309 SVC_User
mbed_official 48:e9a2c7cb57a4 310 LDR R12,=SVC_Count
mbed_official 48:e9a2c7cb57a4 311 LDR R12,[R12]
mbed_official 48:e9a2c7cb57a4 312 CMP R4,R12 ; Check for overflow
mbed_official 48:e9a2c7cb57a4 313 BHI SVC_Done
mbed_official 48:e9a2c7cb57a4 314
mbed_official 48:e9a2c7cb57a4 315 LDR R12,=SVC_Table-4
mbed_official 48:e9a2c7cb57a4 316 LDR R12,[R12,R4,LSL #2] ; Load SVC Function Address
mbed_official 48:e9a2c7cb57a4 317 MRS R4,SPSR ; Save SPSR
mbed_official 48:e9a2c7cb57a4 318 PUSH {R4} ; Push R4 so we can use it as a temp
mbed_official 48:e9a2c7cb57a4 319 AND R4, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 48:e9a2c7cb57a4 320 SUB SP, SP, R4 ; Adjust stack
mbed_official 48:e9a2c7cb57a4 321 PUSH {R4, LR} ; Store stack adjustment and dummy LR
mbed_official 48:e9a2c7cb57a4 322 BLX R12 ; Call SVC Function
mbed_official 48:e9a2c7cb57a4 323 POP {R4, LR} ; Get stack adjustment & discard dummy LR
mbed_official 48:e9a2c7cb57a4 324 ADD SP, SP, R4 ; Unadjust stack
mbed_official 48:e9a2c7cb57a4 325 POP {R4} ; Restore R4
mbed_official 48:e9a2c7cb57a4 326 MSR SPSR_CXSF,R4 ; Restore SPSR
mbed_official 48:e9a2c7cb57a4 327
mbed_official 48:e9a2c7cb57a4 328 SVC_Done
mbed_official 48:e9a2c7cb57a4 329 PUSH {R0-R3,R12,LR}
mbed_official 48:e9a2c7cb57a4 330
mbed_official 48:e9a2c7cb57a4 331 PUSH {R4} ; Push R4 so we can use it as a temp
mbed_official 48:e9a2c7cb57a4 332 AND R4, SP, #4 ; Ensure stack is 8-byte aligned
mbed_official 48:e9a2c7cb57a4 333 SUB SP, SP, R4 ; Adjust stack
mbed_official 48:e9a2c7cb57a4 334 PUSH {R4, LR} ; Store stack adjustment and dummy LR
mbed_official 48:e9a2c7cb57a4 335
mbed_official 48:e9a2c7cb57a4 336 CPSID i
mbed_official 48:e9a2c7cb57a4 337 BLX rt_tsk_unlock
mbed_official 48:e9a2c7cb57a4 338
mbed_official 48:e9a2c7cb57a4 339 POP {R4, LR} ; Get stack adjustment & discard dummy LR
mbed_official 48:e9a2c7cb57a4 340 ADD SP, SP, R4 ; Unadjust stack
mbed_official 48:e9a2c7cb57a4 341 POP {R4} ; Restore R4
mbed_official 48:e9a2c7cb57a4 342
mbed_official 48:e9a2c7cb57a4 343 POP {R0-R3,R12,LR}
mbed_official 48:e9a2c7cb57a4 344 POP {R4}
mbed_official 48:e9a2c7cb57a4 345 RFEFD SP! ; Return from exception
mbed_official 48:e9a2c7cb57a4 346 }
mbed_official 48:e9a2c7cb57a4 347 #pragma pop
mbed_official 48:e9a2c7cb57a4 348
mbed_official 48:e9a2c7cb57a4 349 #pragma push
mbed_official 48:e9a2c7cb57a4 350 #pragma arm
mbed_official 48:e9a2c7cb57a4 351 __asm void PendSV_Handler (U32 IRQn) {
mbed_official 48:e9a2c7cb57a4 352 ARM
mbed_official 48:e9a2c7cb57a4 353
mbed_official 48:e9a2c7cb57a4 354 IMPORT rt_tsk_lock
mbed_official 92:bc9729798a19 355 IMPORT IRQNestLevel ; Flag indicates whether inside an ISR, and the depth of nesting. 0 = not in ISR.
mbed_official 92:bc9729798a19 356 IMPORT seen_id0_active ; Flag used to workaround GIC 390 errata 733075 - set in startup_Renesas_RZ_A1.s
mbed_official 48:e9a2c7cb57a4 357
mbed_official 48:e9a2c7cb57a4 358 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 359
mbed_official 48:e9a2c7cb57a4 360 //Disable systick interrupts, then write EOIR. We want interrupts disabled before we enter the context switcher.
mbed_official 48:e9a2c7cb57a4 361 PUSH {R0, R1}
mbed_official 48:e9a2c7cb57a4 362 BLX rt_tsk_lock
mbed_official 48:e9a2c7cb57a4 363 POP {R0, R1}
mbed_official 48:e9a2c7cb57a4 364 LDR R1, =__cpp(&GICInterface_BASE)
mbed_official 48:e9a2c7cb57a4 365 LDR R1, [R1, #0]
mbed_official 48:e9a2c7cb57a4 366 STR R0, [R1, #0x10]
mbed_official 48:e9a2c7cb57a4 367
mbed_official 92:bc9729798a19 368 ; If it was interrupt ID0, clear the seen flag, otherwise return as normal
mbed_official 92:bc9729798a19 369 CMP R0, #0
mbed_official 92:bc9729798a19 370 LDREQ R1, =seen_id0_active
mbed_official 92:bc9729798a19 371 STRBEQ R0, [R1] ; Clear the seen flag, using R0 (which is 0), to save loading another register
mbed_official 92:bc9729798a19 372
mbed_official 48:e9a2c7cb57a4 373 LDR R0, =IRQNestLevel ; Get address of nesting counter
mbed_official 48:e9a2c7cb57a4 374 LDR R1, [R0]
mbed_official 48:e9a2c7cb57a4 375 SUB R1, R1, #1 ; Decrement nesting counter
mbed_official 48:e9a2c7cb57a4 376 STR R1, [R0]
mbed_official 48:e9a2c7cb57a4 377
mbed_official 48:e9a2c7cb57a4 378 BLX __cpp(rt_pop_req)
mbed_official 48:e9a2c7cb57a4 379
mbed_official 48:e9a2c7cb57a4 380 POP {R1, LR} ; Get stack adjustment & discard dummy LR
mbed_official 48:e9a2c7cb57a4 381 ADD SP, SP, R1 ; Unadjust stack
mbed_official 48:e9a2c7cb57a4 382
mbed_official 48:e9a2c7cb57a4 383 LDR R0,[SP,#24]
mbed_official 48:e9a2c7cb57a4 384 MSR SPSR_CXSF,R0
mbed_official 48:e9a2c7cb57a4 385 POP {R0-R3,R12} ; Leave SPSR & LR on the stack
mbed_official 48:e9a2c7cb57a4 386 PUSH {R4}
mbed_official 48:e9a2c7cb57a4 387 B Sys_Switch
mbed_official 48:e9a2c7cb57a4 388 }
mbed_official 48:e9a2c7cb57a4 389 #pragma pop
mbed_official 48:e9a2c7cb57a4 390
mbed_official 48:e9a2c7cb57a4 391
mbed_official 48:e9a2c7cb57a4 392 #pragma push
mbed_official 48:e9a2c7cb57a4 393 #pragma arm
mbed_official 48:e9a2c7cb57a4 394 __asm void OS_Tick_Handler (U32 IRQn) {
mbed_official 48:e9a2c7cb57a4 395 ARM
mbed_official 48:e9a2c7cb57a4 396
mbed_official 48:e9a2c7cb57a4 397 IMPORT rt_tsk_lock
mbed_official 92:bc9729798a19 398 IMPORT IRQNestLevel ; Flag indicates whether inside an ISR, and the depth of nesting. 0 = not in ISR.
mbed_official 92:bc9729798a19 399 IMPORT seen_id0_active ; Flag used to workaround GIC 390 errata 733075 - set in startup_Renesas_RZ_A1.s
mbed_official 48:e9a2c7cb57a4 400
mbed_official 48:e9a2c7cb57a4 401 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 402
mbed_official 48:e9a2c7cb57a4 403 PUSH {R0, R1}
mbed_official 48:e9a2c7cb57a4 404 BLX rt_tsk_lock
mbed_official 48:e9a2c7cb57a4 405 POP {R0, R1}
mbed_official 48:e9a2c7cb57a4 406 LDR R1, =__cpp(&GICInterface_BASE)
mbed_official 48:e9a2c7cb57a4 407 LDR R1, [R1, #0]
mbed_official 48:e9a2c7cb57a4 408 STR R0, [R1, #0x10]
mbed_official 48:e9a2c7cb57a4 409
mbed_official 92:bc9729798a19 410 ; If it was interrupt ID0, clear the seen flag, otherwise return as normal
mbed_official 92:bc9729798a19 411 CMP R0, #0
mbed_official 92:bc9729798a19 412 LDREQ R1, =seen_id0_active
mbed_official 92:bc9729798a19 413 STRBEQ R0, [R1] ; Clear the seen flag, using R0 (which is 0), to save loading another register
mbed_official 92:bc9729798a19 414
mbed_official 48:e9a2c7cb57a4 415 LDR R0, =IRQNestLevel ; Get address of nesting counter
mbed_official 48:e9a2c7cb57a4 416 LDR R1, [R0]
mbed_official 48:e9a2c7cb57a4 417 SUB R1, R1, #1 ; Decrement nesting counter
mbed_official 48:e9a2c7cb57a4 418 STR R1, [R0]
mbed_official 48:e9a2c7cb57a4 419
mbed_official 48:e9a2c7cb57a4 420 BLX __cpp(os_tick_irqack)
mbed_official 48:e9a2c7cb57a4 421 BLX __cpp(rt_systick)
mbed_official 48:e9a2c7cb57a4 422
mbed_official 48:e9a2c7cb57a4 423 POP {R1, LR} ; Get stack adjustment & discard dummy LR
mbed_official 48:e9a2c7cb57a4 424 ADD SP, SP, R1 ; Unadjust stack
mbed_official 48:e9a2c7cb57a4 425
mbed_official 48:e9a2c7cb57a4 426 LDR R0,[SP,#24]
mbed_official 48:e9a2c7cb57a4 427 MSR SPSR_CXSF,R0
mbed_official 48:e9a2c7cb57a4 428 POP {R0-R3,R12} ; Leave SPSR & LR on the stack
mbed_official 48:e9a2c7cb57a4 429 PUSH {R4}
mbed_official 48:e9a2c7cb57a4 430 B Sys_Switch
mbed_official 48:e9a2c7cb57a4 431 }
mbed_official 48:e9a2c7cb57a4 432 #pragma pop
mbed_official 48:e9a2c7cb57a4 433
mbed_official 48:e9a2c7cb57a4 434
mbed_official 48:e9a2c7cb57a4 435 /*----------------------------------------------------------------------------
mbed_official 48:e9a2c7cb57a4 436 * end of file
mbed_official 48:e9a2c7cb57a4 437 *---------------------------------------------------------------------------*/