Arrow / Mbed OS DAPLink Reset

Repostiory containing DAPLink source code with Reset Pin workaround for HANI_IOT board.

Upstream: https://github.com/ARMmbed/DAPLink

source/rtos/cm3/HAL_CM3.c@0:01f31e923fe2, 2020-04-07 (annotated)

Committer:
Pawel Zarembski
Date:
Tue Apr 07 12:55:42 2020 +0200
Revision:
0:01f31e923fe2
hani: DAPLink with reset workaround

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Pawel Zarembski 0:01f31e923fe2 1 /**
Pawel Zarembski 0:01f31e923fe2 2 * @file HAL_CM3.c
Pawel Zarembski 0:01f31e923fe2 3 * @brief HAL for Cortex-M3
Pawel Zarembski 0:01f31e923fe2 4 *
Pawel Zarembski 0:01f31e923fe2 5 * DAPLink Interface Firmware
Pawel Zarembski 0:01f31e923fe2 6 * Copyright (c) 2009-2016, ARM Limited, All Rights Reserved
Pawel Zarembski 0:01f31e923fe2 7 * SPDX-License-Identifier: Apache-2.0
Pawel Zarembski 0:01f31e923fe2 8 *
Pawel Zarembski 0:01f31e923fe2 9 * Licensed under the Apache License, Version 2.0 (the "License"); you may
Pawel Zarembski 0:01f31e923fe2 10 * not use this file except in compliance with the License.
Pawel Zarembski 0:01f31e923fe2 11 * You may obtain a copy of the License at
Pawel Zarembski 0:01f31e923fe2 12 *
Pawel Zarembski 0:01f31e923fe2 13 * http://www.apache.org/licenses/LICENSE-2.0
Pawel Zarembski 0:01f31e923fe2 14 *
Pawel Zarembski 0:01f31e923fe2 15 * Unless required by applicable law or agreed to in writing, software
Pawel Zarembski 0:01f31e923fe2 16 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
Pawel Zarembski 0:01f31e923fe2 17 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
Pawel Zarembski 0:01f31e923fe2 18 * See the License for the specific language governing permissions and
Pawel Zarembski 0:01f31e923fe2 19 * limitations under the License.
Pawel Zarembski 0:01f31e923fe2 20 */
Pawel Zarembski 0:01f31e923fe2 21
Pawel Zarembski 0:01f31e923fe2 22 #include "rt_TypeDef.h"
Pawel Zarembski 0:01f31e923fe2 23 #include "RTX_Config.h"
Pawel Zarembski 0:01f31e923fe2 24 #include "rt_System.h"
Pawel Zarembski 0:01f31e923fe2 25 #include "rt_HAL_CM.h"
Pawel Zarembski 0:01f31e923fe2 26 #include "rt_Task.h"
Pawel Zarembski 0:01f31e923fe2 27 #include "rt_List.h"
Pawel Zarembski 0:01f31e923fe2 28 #include "rt_MemBox.h"
Pawel Zarembski 0:01f31e923fe2 29
Pawel Zarembski 0:01f31e923fe2 30
Pawel Zarembski 0:01f31e923fe2 31 /*----------------------------------------------------------------------------
Pawel Zarembski 0:01f31e923fe2 32 * Global Variables
Pawel Zarembski 0:01f31e923fe2 33 *---------------------------------------------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 34
Pawel Zarembski 0:01f31e923fe2 35 BIT dbg_msg;
Pawel Zarembski 0:01f31e923fe2 36
Pawel Zarembski 0:01f31e923fe2 37 /*----------------------------------------------------------------------------
Pawel Zarembski 0:01f31e923fe2 38 * Functions
Pawel Zarembski 0:01f31e923fe2 39 *---------------------------------------------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 40
Pawel Zarembski 0:01f31e923fe2 41
Pawel Zarembski 0:01f31e923fe2 42 /*--------------------------- rt_set_PSP ------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 43
Pawel Zarembski 0:01f31e923fe2 44 __asm void rt_set_PSP (U32 stack) {
Pawel Zarembski 0:01f31e923fe2 45 MSR PSP,R0
Pawel Zarembski 0:01f31e923fe2 46 BX LR
Pawel Zarembski 0:01f31e923fe2 47 }
Pawel Zarembski 0:01f31e923fe2 48
Pawel Zarembski 0:01f31e923fe2 49
Pawel Zarembski 0:01f31e923fe2 50 /*--------------------------- rt_get_PSP ------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 51
Pawel Zarembski 0:01f31e923fe2 52 __asm U32 rt_get_PSP (void) {
Pawel Zarembski 0:01f31e923fe2 53 MRS R0,PSP
Pawel Zarembski 0:01f31e923fe2 54 BX LR
Pawel Zarembski 0:01f31e923fe2 55 }
Pawel Zarembski 0:01f31e923fe2 56
Pawel Zarembski 0:01f31e923fe2 57
Pawel Zarembski 0:01f31e923fe2 58 /*--------------------------- os_set_env ------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 59
Pawel Zarembski 0:01f31e923fe2 60 __asm void os_set_env (void) {
Pawel Zarembski 0:01f31e923fe2 61 /* Switch to Unprivileged/Privileged Thread mode, use PSP. */
Pawel Zarembski 0:01f31e923fe2 62 MOV R0,SP ; PSP = MSP
Pawel Zarembski 0:01f31e923fe2 63 MSR PSP,R0
Pawel Zarembski 0:01f31e923fe2 64 LDR R0,=__cpp(&os_flags)
Pawel Zarembski 0:01f31e923fe2 65 LDRB R0,[R0]
Pawel Zarembski 0:01f31e923fe2 66 LSLS R0,#31
Pawel Zarembski 0:01f31e923fe2 67 MOVNE R0,#0x02 ; Privileged Thread mode, use PSP
Pawel Zarembski 0:01f31e923fe2 68 MOVEQ R0,#0x03 ; Unprivileged Thread mode, use PSP
Pawel Zarembski 0:01f31e923fe2 69 MSR CONTROL,R0
Pawel Zarembski 0:01f31e923fe2 70 BX LR
Pawel Zarembski 0:01f31e923fe2 71
Pawel Zarembski 0:01f31e923fe2 72 ALIGN
Pawel Zarembski 0:01f31e923fe2 73 }
Pawel Zarembski 0:01f31e923fe2 74
Pawel Zarembski 0:01f31e923fe2 75
Pawel Zarembski 0:01f31e923fe2 76 /*--------------------------- _alloc_box ------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 77
Pawel Zarembski 0:01f31e923fe2 78 __asm void *_alloc_box (void *box_mem) {
Pawel Zarembski 0:01f31e923fe2 79 /* Function wrapper for Unprivileged/Privileged mode. */
Pawel Zarembski 0:01f31e923fe2 80 LDR R12,=__cpp(rt_alloc_box)
Pawel Zarembski 0:01f31e923fe2 81 MRS R3,IPSR
Pawel Zarembski 0:01f31e923fe2 82 LSLS R3,#24
Pawel Zarembski 0:01f31e923fe2 83 BXNE R12
Pawel Zarembski 0:01f31e923fe2 84 MRS R3,CONTROL
Pawel Zarembski 0:01f31e923fe2 85 LSLS R3,#31
Pawel Zarembski 0:01f31e923fe2 86 BXEQ R12
Pawel Zarembski 0:01f31e923fe2 87 SVC 0
Pawel Zarembski 0:01f31e923fe2 88 BX LR
Pawel Zarembski 0:01f31e923fe2 89
Pawel Zarembski 0:01f31e923fe2 90 ALIGN
Pawel Zarembski 0:01f31e923fe2 91 }
Pawel Zarembski 0:01f31e923fe2 92
Pawel Zarembski 0:01f31e923fe2 93
Pawel Zarembski 0:01f31e923fe2 94 /*--------------------------- _free_box -------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 95
Pawel Zarembski 0:01f31e923fe2 96 __asm int _free_box (void *box_mem, void *box) {
Pawel Zarembski 0:01f31e923fe2 97 /* Function wrapper for Unprivileged/Privileged mode. */
Pawel Zarembski 0:01f31e923fe2 98 LDR R12,=__cpp(rt_free_box)
Pawel Zarembski 0:01f31e923fe2 99 MRS R3,IPSR
Pawel Zarembski 0:01f31e923fe2 100 LSLS R3,#24
Pawel Zarembski 0:01f31e923fe2 101 BXNE R12
Pawel Zarembski 0:01f31e923fe2 102 MRS R3,CONTROL
Pawel Zarembski 0:01f31e923fe2 103 LSLS R3,#31
Pawel Zarembski 0:01f31e923fe2 104 BXEQ R12
Pawel Zarembski 0:01f31e923fe2 105 SVC 0
Pawel Zarembski 0:01f31e923fe2 106 BX LR
Pawel Zarembski 0:01f31e923fe2 107
Pawel Zarembski 0:01f31e923fe2 108 ALIGN
Pawel Zarembski 0:01f31e923fe2 109 }
Pawel Zarembski 0:01f31e923fe2 110
Pawel Zarembski 0:01f31e923fe2 111
Pawel Zarembski 0:01f31e923fe2 112 /*-------------------------- SVC_Handler ------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 113
Pawel Zarembski 0:01f31e923fe2 114 __asm void SVC_Handler (void) {
Pawel Zarembski 0:01f31e923fe2 115 PRESERVE8
Pawel Zarembski 0:01f31e923fe2 116
Pawel Zarembski 0:01f31e923fe2 117 #ifdef IFX_XMC4XXX
Pawel Zarembski 0:01f31e923fe2 118 EXPORT SVC_Handler_Veneer
Pawel Zarembski 0:01f31e923fe2 119 SVC_Handler_Veneer
Pawel Zarembski 0:01f31e923fe2 120 #endif
Pawel Zarembski 0:01f31e923fe2 121
Pawel Zarembski 0:01f31e923fe2 122 IMPORT SVC_Count
Pawel Zarembski 0:01f31e923fe2 123 IMPORT SVC_Table
Pawel Zarembski 0:01f31e923fe2 124 IMPORT rt_stk_check
Pawel Zarembski 0:01f31e923fe2 125
Pawel Zarembski 0:01f31e923fe2 126 MRS R0,PSP ; Read PSP
Pawel Zarembski 0:01f31e923fe2 127 LDR R1,[R0,#24] ; Read Saved PC from Stack
Pawel Zarembski 0:01f31e923fe2 128 LDRB R1,[R1,#-2] ; Load SVC Number
Pawel Zarembski 0:01f31e923fe2 129 CBNZ R1,SVC_User
Pawel Zarembski 0:01f31e923fe2 130
Pawel Zarembski 0:01f31e923fe2 131 LDM R0,{R0-R3,R12} ; Read R0-R3,R12 from stack
Pawel Zarembski 0:01f31e923fe2 132 BLX R12 ; Call SVC Function
Pawel Zarembski 0:01f31e923fe2 133
Pawel Zarembski 0:01f31e923fe2 134 MRS R12,PSP ; Read PSP
Pawel Zarembski 0:01f31e923fe2 135 LDR R3,=__cpp(&os_tsk)
Pawel Zarembski 0:01f31e923fe2 136 LDM R3,{R1,R2} ; os_tsk.run, os_tsk.new
Pawel Zarembski 0:01f31e923fe2 137 CMP R1,R2
Pawel Zarembski 0:01f31e923fe2 138 BEQ SVC_Exit ; no task switch
Pawel Zarembski 0:01f31e923fe2 139
Pawel Zarembski 0:01f31e923fe2 140 CBZ R1,SVC_Restore ; Runtask deleted?
Pawel Zarembski 0:01f31e923fe2 141
Pawel Zarembski 0:01f31e923fe2 142 PUSH {R2,R3}
Pawel Zarembski 0:01f31e923fe2 143 MOV R3,#1
Pawel Zarembski 0:01f31e923fe2 144 STRB R3,[R1,#TCB_RETUPD] ; os_tsk.run->ret_upd = 1
Pawel Zarembski 0:01f31e923fe2 145 STMDB R12!,{R4-R11} ; Save Old context
Pawel Zarembski 0:01f31e923fe2 146 STR R12,[R1,#TCB_TSTACK] ; Update os_tsk.run->tsk_stack
Pawel Zarembski 0:01f31e923fe2 147 BL rt_stk_check ; Check for Stack overflow
Pawel Zarembski 0:01f31e923fe2 148 POP {R2,R3}
Pawel Zarembski 0:01f31e923fe2 149
Pawel Zarembski 0:01f31e923fe2 150 SVC_Restore
Pawel Zarembski 0:01f31e923fe2 151 STR R2,[R3] ; os_tsk.run = os_tsk.new
Pawel Zarembski 0:01f31e923fe2 152
Pawel Zarembski 0:01f31e923fe2 153 LDR R12,[R2,#TCB_TSTACK] ; os_tsk.new->tsk_stack
Pawel Zarembski 0:01f31e923fe2 154 LDMIA R12!,{R4-R11} ; Restore New Context
Pawel Zarembski 0:01f31e923fe2 155 LDRB R3,[R2,#TCB_RETUPD] ; Update ret_val?
Pawel Zarembski 0:01f31e923fe2 156 MSR PSP,R12 ; Write PSP
Pawel Zarembski 0:01f31e923fe2 157
Pawel Zarembski 0:01f31e923fe2 158 CBZ R3,SVC_Return
Pawel Zarembski 0:01f31e923fe2 159 LDRB R0,[R2,#TCB_RETVAL] ; Write os_tsk.new->ret_val
Pawel Zarembski 0:01f31e923fe2 160
Pawel Zarembski 0:01f31e923fe2 161 SVC_Exit
Pawel Zarembski 0:01f31e923fe2 162 STR R0,[R12] ; Function return value
Pawel Zarembski 0:01f31e923fe2 163
Pawel Zarembski 0:01f31e923fe2 164 SVC_Return
Pawel Zarembski 0:01f31e923fe2 165 MVN LR,#:NOT:0xFFFFFFFD ; set EXC_RETURN value
Pawel Zarembski 0:01f31e923fe2 166 #ifdef IFX_XMC4XXX
Pawel Zarembski 0:01f31e923fe2 167 PUSH {LR}
Pawel Zarembski 0:01f31e923fe2 168 POP {PC}
Pawel Zarembski 0:01f31e923fe2 169 #else
Pawel Zarembski 0:01f31e923fe2 170 BX LR
Pawel Zarembski 0:01f31e923fe2 171 #endif
Pawel Zarembski 0:01f31e923fe2 172
Pawel Zarembski 0:01f31e923fe2 173 /*------------------- User SVC ------------------------------*/
Pawel Zarembski 0:01f31e923fe2 174
Pawel Zarembski 0:01f31e923fe2 175 SVC_User
Pawel Zarembski 0:01f31e923fe2 176 PUSH {R4,LR} ; Save Registers
Pawel Zarembski 0:01f31e923fe2 177 LDR R2,=SVC_Count
Pawel Zarembski 0:01f31e923fe2 178 LDR R2,[R2]
Pawel Zarembski 0:01f31e923fe2 179 CMP R1,R2
Pawel Zarembski 0:01f31e923fe2 180 BHI SVC_Done ; Overflow
Pawel Zarembski 0:01f31e923fe2 181
Pawel Zarembski 0:01f31e923fe2 182 LDR R4,=SVC_Table-4
Pawel Zarembski 0:01f31e923fe2 183 LDR R4,[R4,R1,LSL #2] ; Load SVC Function Address
Pawel Zarembski 0:01f31e923fe2 184
Pawel Zarembski 0:01f31e923fe2 185 LDM R0,{R0-R3,R12} ; Read R0-R3,R12 from stack
Pawel Zarembski 0:01f31e923fe2 186 BLX R4 ; Call SVC Function
Pawel Zarembski 0:01f31e923fe2 187
Pawel Zarembski 0:01f31e923fe2 188 MRS R12,PSP
Pawel Zarembski 0:01f31e923fe2 189 STM R12,{R0-R3} ; Function return values
Pawel Zarembski 0:01f31e923fe2 190 SVC_Done
Pawel Zarembski 0:01f31e923fe2 191 POP {R4,PC} ; RETI
Pawel Zarembski 0:01f31e923fe2 192
Pawel Zarembski 0:01f31e923fe2 193 ALIGN
Pawel Zarembski 0:01f31e923fe2 194 }
Pawel Zarembski 0:01f31e923fe2 195
Pawel Zarembski 0:01f31e923fe2 196
Pawel Zarembski 0:01f31e923fe2 197 /*-------------------------- PendSV_Handler ---------------------------------*/
Pawel Zarembski 0:01f31e923fe2 198
Pawel Zarembski 0:01f31e923fe2 199 __asm void PendSV_Handler (void) {
Pawel Zarembski 0:01f31e923fe2 200 PRESERVE8
Pawel Zarembski 0:01f31e923fe2 201
Pawel Zarembski 0:01f31e923fe2 202 #ifdef IFX_XMC4XXX
Pawel Zarembski 0:01f31e923fe2 203 EXPORT PendSV_Handler_Veneer
Pawel Zarembski 0:01f31e923fe2 204 PendSV_Handler_Veneer
Pawel Zarembski 0:01f31e923fe2 205 #endif
Pawel Zarembski 0:01f31e923fe2 206
Pawel Zarembski 0:01f31e923fe2 207 BL __cpp(rt_pop_req)
Pawel Zarembski 0:01f31e923fe2 208
Pawel Zarembski 0:01f31e923fe2 209 Sys_Switch
Pawel Zarembski 0:01f31e923fe2 210 LDR R3,=__cpp(&os_tsk)
Pawel Zarembski 0:01f31e923fe2 211 LDM R3,{R1,R2} ; os_tsk.run, os_tsk.new
Pawel Zarembski 0:01f31e923fe2 212 CMP R1,R2
Pawel Zarembski 0:01f31e923fe2 213 BEQ Sys_Exit
Pawel Zarembski 0:01f31e923fe2 214
Pawel Zarembski 0:01f31e923fe2 215 PUSH {R2,R3}
Pawel Zarembski 0:01f31e923fe2 216 MOV R3,#0
Pawel Zarembski 0:01f31e923fe2 217 STRB R3,[R1,#TCB_RETUPD] ; os_tsk.run->ret_upd = 0
Pawel Zarembski 0:01f31e923fe2 218 MRS R12,PSP ; Read PSP
Pawel Zarembski 0:01f31e923fe2 219 STMDB R12!,{R4-R11} ; Save Old context
Pawel Zarembski 0:01f31e923fe2 220 STR R12,[R1,#TCB_TSTACK] ; Update os_tsk.run->tsk_stack
Pawel Zarembski 0:01f31e923fe2 221 BL rt_stk_check ; Check for Stack overflow
Pawel Zarembski 0:01f31e923fe2 222
Pawel Zarembski 0:01f31e923fe2 223 POP {R2,R3}
Pawel Zarembski 0:01f31e923fe2 224 STR R2,[R3] ; os_tsk.run = os_tsk.new
Pawel Zarembski 0:01f31e923fe2 225
Pawel Zarembski 0:01f31e923fe2 226 LDR R12,[R2,#TCB_TSTACK] ; os_tsk.new->tsk_stack
Pawel Zarembski 0:01f31e923fe2 227 LDMIA R12!,{R4-R11} ; Restore New Context
Pawel Zarembski 0:01f31e923fe2 228 MSR PSP,R12 ; Write PSP
Pawel Zarembski 0:01f31e923fe2 229
Pawel Zarembski 0:01f31e923fe2 230 LDRB R3,[R2,#TCB_RETUPD] ; Update ret_val?
Pawel Zarembski 0:01f31e923fe2 231 CBZ R3,Sys_Exit
Pawel Zarembski 0:01f31e923fe2 232 LDRB R3,[R2,#TCB_RETVAL] ; Write os_tsk.new->ret_val
Pawel Zarembski 0:01f31e923fe2 233 STR R3,[R12]
Pawel Zarembski 0:01f31e923fe2 234 Sys_Exit
Pawel Zarembski 0:01f31e923fe2 235 MVN LR,#:NOT:0xFFFFFFFD ; set EXC_RETURN value
Pawel Zarembski 0:01f31e923fe2 236 #ifdef IFX_XMC4XXX
Pawel Zarembski 0:01f31e923fe2 237 PUSH {LR}
Pawel Zarembski 0:01f31e923fe2 238 POP {PC}
Pawel Zarembski 0:01f31e923fe2 239 #else
Pawel Zarembski 0:01f31e923fe2 240 BX LR ; Return to Thread Mode
Pawel Zarembski 0:01f31e923fe2 241 #endif
Pawel Zarembski 0:01f31e923fe2 242
Pawel Zarembski 0:01f31e923fe2 243 ALIGN
Pawel Zarembski 0:01f31e923fe2 244 }
Pawel Zarembski 0:01f31e923fe2 245
Pawel Zarembski 0:01f31e923fe2 246
Pawel Zarembski 0:01f31e923fe2 247 /*-------------------------- SysTick_Handler --------------------------------*/
Pawel Zarembski 0:01f31e923fe2 248
Pawel Zarembski 0:01f31e923fe2 249 __asm void SysTick_Handler (void) {
Pawel Zarembski 0:01f31e923fe2 250 PRESERVE8
Pawel Zarembski 0:01f31e923fe2 251
Pawel Zarembski 0:01f31e923fe2 252 #ifdef IFX_XMC4XXX
Pawel Zarembski 0:01f31e923fe2 253 EXPORT SysTick_Handler_Veneer
Pawel Zarembski 0:01f31e923fe2 254 SysTick_Handler_Veneer
Pawel Zarembski 0:01f31e923fe2 255 #endif
Pawel Zarembski 0:01f31e923fe2 256
Pawel Zarembski 0:01f31e923fe2 257 BL __cpp(rt_systick)
Pawel Zarembski 0:01f31e923fe2 258 B Sys_Switch
Pawel Zarembski 0:01f31e923fe2 259
Pawel Zarembski 0:01f31e923fe2 260 ALIGN
Pawel Zarembski 0:01f31e923fe2 261 }
Pawel Zarembski 0:01f31e923fe2 262
Pawel Zarembski 0:01f31e923fe2 263
Pawel Zarembski 0:01f31e923fe2 264 /*-------------------------- OS_Tick_Handler --------------------------------*/
Pawel Zarembski 0:01f31e923fe2 265
Pawel Zarembski 0:01f31e923fe2 266 __asm void OS_Tick_Handler (void) {
Pawel Zarembski 0:01f31e923fe2 267 PRESERVE8
Pawel Zarembski 0:01f31e923fe2 268
Pawel Zarembski 0:01f31e923fe2 269 BL __cpp(os_tick_irqack)
Pawel Zarembski 0:01f31e923fe2 270 BL __cpp(rt_systick)
Pawel Zarembski 0:01f31e923fe2 271 B Sys_Switch
Pawel Zarembski 0:01f31e923fe2 272
Pawel Zarembski 0:01f31e923fe2 273 ALIGN
Pawel Zarembski 0:01f31e923fe2 274 }
Pawel Zarembski 0:01f31e923fe2 275
Pawel Zarembski 0:01f31e923fe2 276
Pawel Zarembski 0:01f31e923fe2 277 /*--------------------------- rt_init_stack ---------------------------------*/
Pawel Zarembski 0:01f31e923fe2 278
Pawel Zarembski 0:01f31e923fe2 279 void rt_init_stack (P_TCB p_TCB, FUNCP task_body) {
Pawel Zarembski 0:01f31e923fe2 280 /* Prepare TCB and saved context for a first time start of a task. */
Pawel Zarembski 0:01f31e923fe2 281 U32 *stk,i,size;
Pawel Zarembski 0:01f31e923fe2 282
Pawel Zarembski 0:01f31e923fe2 283 /* Prepare a complete interrupt frame for first task start */
Pawel Zarembski 0:01f31e923fe2 284 size = p_TCB->priv_stack >> 2;
Pawel Zarembski 0:01f31e923fe2 285 if (size == 0) {
Pawel Zarembski 0:01f31e923fe2 286 size = (U16)os_stackinfo >> 2;
Pawel Zarembski 0:01f31e923fe2 287 }
Pawel Zarembski 0:01f31e923fe2 288
Pawel Zarembski 0:01f31e923fe2 289 /* Write to the top of stack. */
Pawel Zarembski 0:01f31e923fe2 290 stk = &p_TCB->stack[size];
Pawel Zarembski 0:01f31e923fe2 291
Pawel Zarembski 0:01f31e923fe2 292 /* Auto correct to 8-byte ARM stack alignment. */
Pawel Zarembski 0:01f31e923fe2 293 if ((U32)stk & 0x04) {
Pawel Zarembski 0:01f31e923fe2 294 stk--;
Pawel Zarembski 0:01f31e923fe2 295 }
Pawel Zarembski 0:01f31e923fe2 296
Pawel Zarembski 0:01f31e923fe2 297 stk -= 16;
Pawel Zarembski 0:01f31e923fe2 298
Pawel Zarembski 0:01f31e923fe2 299 /* Default xPSR and initial PC */
Pawel Zarembski 0:01f31e923fe2 300 stk[15] = INITIAL_xPSR;
Pawel Zarembski 0:01f31e923fe2 301 stk[14] = (U32)task_body;
Pawel Zarembski 0:01f31e923fe2 302
Pawel Zarembski 0:01f31e923fe2 303 /* Clear R1-R12,LR registers. */
Pawel Zarembski 0:01f31e923fe2 304 for (i = 0; i < 14; i++) {
Pawel Zarembski 0:01f31e923fe2 305 stk[i] = 0;
Pawel Zarembski 0:01f31e923fe2 306 }
Pawel Zarembski 0:01f31e923fe2 307
Pawel Zarembski 0:01f31e923fe2 308 /* Assign a void pointer to R0. */
Pawel Zarembski 0:01f31e923fe2 309 stk[8] = (U32)p_TCB->msg;
Pawel Zarembski 0:01f31e923fe2 310
Pawel Zarembski 0:01f31e923fe2 311 /* Initial Task stack pointer. */
Pawel Zarembski 0:01f31e923fe2 312 p_TCB->tsk_stack = (U32)stk;
Pawel Zarembski 0:01f31e923fe2 313
Pawel Zarembski 0:01f31e923fe2 314 /* Task entry point. */
Pawel Zarembski 0:01f31e923fe2 315 p_TCB->ptask = task_body;
Pawel Zarembski 0:01f31e923fe2 316
Pawel Zarembski 0:01f31e923fe2 317 /* Set a magic word for checking of stack overflow. */
Pawel Zarembski 0:01f31e923fe2 318 p_TCB->stack[0] = MAGIC_WORD;
Pawel Zarembski 0:01f31e923fe2 319 }
Pawel Zarembski 0:01f31e923fe2 320
Pawel Zarembski 0:01f31e923fe2 321
Pawel Zarembski 0:01f31e923fe2 322 /*--------------------------- dbg_init --------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 323
Pawel Zarembski 0:01f31e923fe2 324 void dbg_init (void) {
Pawel Zarembski 0:01f31e923fe2 325 if ((DEMCR & DEMCR_TRCENA) &&
Pawel Zarembski 0:01f31e923fe2 326 (ITM_CONTROL & ITM_ITMENA) &&
Pawel Zarembski 0:01f31e923fe2 327 (ITM_ENABLE & (1UL << 31))) {
Pawel Zarembski 0:01f31e923fe2 328 dbg_msg = __TRUE;
Pawel Zarembski 0:01f31e923fe2 329 }
Pawel Zarembski 0:01f31e923fe2 330 }
Pawel Zarembski 0:01f31e923fe2 331
Pawel Zarembski 0:01f31e923fe2 332
Pawel Zarembski 0:01f31e923fe2 333 /*--------------------------- dbg_task_notify -------------------------------*/
Pawel Zarembski 0:01f31e923fe2 334
Pawel Zarembski 0:01f31e923fe2 335 void dbg_task_notify (P_TCB p_tcb, BOOL create) {
Pawel Zarembski 0:01f31e923fe2 336 while (ITM_PORT31_U32 == 0);
Pawel Zarembski 0:01f31e923fe2 337 ITM_PORT31_U32 = (U32)p_tcb->ptask;
Pawel Zarembski 0:01f31e923fe2 338 while (ITM_PORT31_U32 == 0);
Pawel Zarembski 0:01f31e923fe2 339 ITM_PORT31_U16 = (create << 8) | p_tcb->task_id;
Pawel Zarembski 0:01f31e923fe2 340 }
Pawel Zarembski 0:01f31e923fe2 341
Pawel Zarembski 0:01f31e923fe2 342
Pawel Zarembski 0:01f31e923fe2 343 /*--------------------------- dbg_task_switch -------------------------------*/
Pawel Zarembski 0:01f31e923fe2 344
Pawel Zarembski 0:01f31e923fe2 345 void dbg_task_switch (U32 task_id) {
Pawel Zarembski 0:01f31e923fe2 346 while (ITM_PORT31_U32 == 0);
Pawel Zarembski 0:01f31e923fe2 347 ITM_PORT31_U8 = task_id;
Pawel Zarembski 0:01f31e923fe2 348 }
Pawel Zarembski 0:01f31e923fe2 349
Pawel Zarembski 0:01f31e923fe2 350
Pawel Zarembski 0:01f31e923fe2 351 /*----------------------------------------------------------------------------
Pawel Zarembski 0:01f31e923fe2 352 * end of file
Pawel Zarembski 0:01f31e923fe2 353 *---------------------------------------------------------------------------*/
Pawel Zarembski 0:01f31e923fe2 354