fork
Fork of mbed-rtos by
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?
User | Revision | Line number | New 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 | *---------------------------------------------------------------------------*/ |