HAL_CM4.c

00001 /**
00002  * @file    HAL_CM3.c
00003  * @brief   HAL for Cortex-M4
00004  *
00005  * DAPLink Interface Firmware
00006  * Copyright (c) 2009-2016, ARM Limited, All Rights Reserved
00007  * SPDX-License-Identifier: Apache-2.0
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License"); you may
00010  * not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  * http://www.apache.org/licenses/LICENSE-2.0
00014  *
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
00017  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
00020  */
00021 
00022 #include "rt_TypeDef.h"
00023 #include "RTX_Config.h"
00024 #include "rt_System.h"
00025 #include "rt_HAL_CM.h"
00026 #include "rt_Task.h"
00027 #include "rt_List.h"
00028 #include "rt_MemBox.h"
00029 
00030 
00031 /*----------------------------------------------------------------------------
00032  *      Global Variables
00033  *---------------------------------------------------------------------------*/
00034 
00035 BIT dbg_msg;
00036 
00037 /*----------------------------------------------------------------------------
00038  *      Functions
00039  *---------------------------------------------------------------------------*/
00040 
00041 
00042 /*--------------------------- rt_set_PSP ------------------------------------*/
00043 
00044 __asm void rt_set_PSP (U32 stack) {
00045         MSR     PSP,R0
00046         BX      LR
00047 }
00048 
00049 
00050 /*--------------------------- rt_get_PSP ------------------------------------*/
00051 
00052 __asm U32 rt_get_PSP (void) {
00053         MRS     R0,PSP
00054         BX      LR
00055 }
00056 
00057 
00058 /*--------------------------- os_set_env ------------------------------------*/
00059 
00060 __asm void os_set_env (void) {
00061    /* Switch to Unprivileged/Privileged Thread mode, use PSP. */
00062         MOV     R0,SP                   ; PSP = MSP
00063         MSR     PSP,R0
00064         LDR     R0,=__cpp(&os_flags)
00065         LDRB    R0,[R0]
00066         LSLS    R0,#31
00067         MOVNE   R0,#0x02                ; Privileged Thread mode, use PSP
00068         MOVEQ   R0,#0x03                ; Unprivileged Thread mode, use PSP
00069         MSR     CONTROL,R0
00070         BX      LR
00071 
00072         ALIGN
00073 }
00074 
00075 
00076 /*--------------------------- _alloc_box ------------------------------------*/
00077 
00078 __asm void *_alloc_box (void *box_mem) {
00079    /* Function wrapper for Unprivileged/Privileged mode. */
00080         LDR     R12,=__cpp(rt_alloc_box)
00081         MRS     R3,IPSR
00082         LSLS    R3,#24
00083         BXNE    R12
00084         MRS     R3,CONTROL
00085         LSLS    R3,#31
00086         BXEQ    R12
00087         SVC     0
00088         BX      LR
00089 
00090         ALIGN
00091 }
00092 
00093 
00094 /*--------------------------- _free_box -------------------------------------*/
00095 
00096 __asm int _free_box (void *box_mem, void *box) {
00097    /* Function wrapper for Unprivileged/Privileged mode. */
00098         LDR     R12,=__cpp(rt_free_box)
00099         MRS     R3,IPSR
00100         LSLS    R3,#24
00101         BXNE    R12
00102         MRS     R3,CONTROL
00103         LSLS    R3,#31
00104         BXEQ    R12
00105         SVC     0
00106         BX      LR
00107 
00108         ALIGN
00109 }
00110 
00111 
00112 /*-------------------------- SVC_Handler ------------------------------------*/
00113 
00114 __asm void SVC_Handler (void) {
00115         PRESERVE8
00116 
00117 #ifdef  IFX_XMC4XXX
00118         EXPORT  SVC_Handler_Veneer
00119 SVC_Handler_Veneer
00120 #endif
00121 
00122         IMPORT  SVC_Count
00123         IMPORT  SVC_Table
00124         IMPORT  rt_stk_check
00125 
00126         MRS     R0,PSP                  ; Read PSP
00127         LDR     R1,[R0,#24]             ; Read Saved PC from Stack
00128         LDRB    R1,[R1,#-2]             ; Load SVC Number
00129         CBNZ    R1,SVC_User
00130 
00131         PUSH    {R4,LR}                 ; Save EXC_RETURN
00132         LDM     R0,{R0-R3,R12}          ; Read R0-R3,R12 from stack
00133         BLX     R12                     ; Call SVC Function
00134         POP     {R4,LR}                 ; Restore EXC_RETURN
00135 
00136         MRS     R12,PSP                 ; Read PSP
00137         LDR     R3,=__cpp(&os_tsk)
00138         LDM     R3,{R1,R2}              ; os_tsk.run, os_tsk.new
00139         CMP     R1,R2
00140         BEQ     SVC_Exit                ; no task switch
00141 
00142         CBZ     R1,SVC_Restore          ; Runtask deleted?
00143 
00144         PUSH    {R2,R3}
00145         TST     LR,#0x10                ; is it extended frame?
00146         VSTMDBEQ R12!,{S16-S31}         ; yes, stack also VFP hi-regs
00147         MOVEQ   R3,#0x03                ; os_tsk->ret_upd val
00148         MOVNE   R3,#0x01
00149 
00150         STRB    R3,[R1,#TCB_RETUPD]     ; os_tsk.run->ret_upd = val
00151         STMDB   R12!,{R4-R11}           ; Save Old context
00152         STR     R12,[R1,#TCB_TSTACK]    ; Update os_tsk.run->tsk_stack
00153         BL      rt_stk_check            ; Check for Stack overflow
00154         POP     {R2,R3}
00155 
00156 SVC_Restore
00157         STR     R2,[R3]                 ; os_tsk.run = os_tsk.new
00158 
00159         LDR     R12,[R2,#TCB_TSTACK]    ; os_tsk.new->tsk_stack
00160         LDMIA   R12!,{R4-R11}           ; Restore New Context
00161         LDRB    R3,[R2,#TCB_RETUPD]     ; Update ret_val?
00162 
00163         TST     R3,#0x02                ; VFP Active?
00164         VLDMIANE R12!,{S16-S31}         ; restore VFP hi-registers
00165         MVNNE   LR,#:NOT:0xFFFFFFED     ; set EXC_RETURN value
00166         MVNEQ   LR,#:NOT:0xFFFFFFFD
00167         MSR     PSP,R12                 ; Write PSP
00168 
00169         TST     R3,#0x01
00170 #ifdef  IFX_XMC4XXX
00171         PUSHEQ  {LR}
00172         POPEQ   {PC}
00173 #else
00174         BXEQ    LR                      ; RETI
00175 #endif
00176 
00177         LDRB    R0,[R2,#TCB_RETVAL]     ; Write os_tsk.new->ret_val
00178 SVC_Exit
00179         STR     R0,[R12]                ; Function return value
00180 #ifdef  IFX_XMC4XXX
00181         PUSH    {LR}
00182         POP     {PC}
00183 #else
00184         BX      LR
00185 #endif
00186 
00187         /*------------------- User SVC ------------------------------*/
00188 
00189 SVC_User
00190         PUSH    {R4,LR}                 ; Save Registers
00191         LDR     R2,=SVC_Count
00192         LDR     R2,[R2]
00193         CMP     R1,R2
00194         BHI     SVC_Done                ; Overflow
00195 
00196         LDR     R4,=SVC_Table-4
00197         LDR     R4,[R4,R1,LSL #2]       ; Load SVC Function Address
00198 
00199         LDM     R0,{R0-R3,R12}          ; Read R0-R3,R12 from stack
00200         BLX     R4                      ; Call SVC Function
00201 
00202         MRS     R12,PSP
00203         STM     R12,{R0-R3}             ; Function return values
00204 SVC_Done
00205         POP     {R4,PC}                 ; RETI
00206 
00207         ALIGN
00208 }
00209 
00210 
00211 /*-------------------------- PendSV_Handler ---------------------------------*/
00212 
00213 __asm void PendSV_Handler (void) {
00214         PRESERVE8
00215 
00216 #ifdef  IFX_XMC4XXX
00217         EXPORT  PendSV_Handler_Veneer
00218 PendSV_Handler_Veneer
00219 #endif
00220 
00221         PUSH    {R4,LR}                 ; Save EXC_RETURN
00222         BL      __cpp(rt_pop_req)
00223 
00224 Sys_Switch
00225         POP     {R4,LR}                 ; Restore EXC_RETURN
00226 
00227         LDR     R3,=__cpp(&os_tsk)
00228         LDM     R3,{R1,R2}              ; os_tsk.run, os_tsk.new
00229         CMP     R1,R2
00230 #ifdef  IFX_XMC4XXX
00231         PUSHEQ  {LR}
00232         POPEQ   {PC}
00233 #else
00234         BXEQ    LR                      ; RETI, no task switch
00235 #endif
00236 
00237         PUSH    {R2,R3}
00238         MRS     R12,PSP                 ; Read PSP
00239         TST     LR,#0x10                ; is it extended frame?
00240         VSTMDBEQ R12!,{S16-S31}         ; yes, stack also VFP hi-regs
00241         MOVEQ   R3,#0x02                ; os_tsk->ret_upd val
00242         MOVNE   R3,#0x00
00243 
00244         STRB    R3,[R1,#TCB_RETUPD]     ; os_tsk.run->ret_upd = 0
00245         STMDB   R12!,{R4-R11}           ; Save Old context
00246         STR     R12,[R1,#TCB_TSTACK]    ; Update os_tsk.run->tsk_stack
00247         BL      rt_stk_check            ; Check for Stack overflow
00248 
00249         POP     {R2,R3}
00250         STR     R2,[R3]                 ; os_tsk.run = os_tsk.new
00251 
00252         LDR     R12,[R2,#TCB_TSTACK]    ; os_tsk.new->tsk_stack
00253         LDMIA   R12!,{R4-R11}           ; Restore New Context
00254         LDRB    R3,[R2,#TCB_RETUPD]     ; Update ret_val?
00255 
00256         TST     R3,#0x02                ; VFP Active?
00257         VLDMIANE R12!,{S16-S31}         ; restore VFP hi-regs
00258         MVNNE   LR,#:NOT:0xFFFFFFED     ; set EXC_RETURN value
00259         MVNEQ   LR,#:NOT:0xFFFFFFFD
00260         MSR     PSP,R12                 ; Write PSP
00261 
00262         TST     R3,#0x01
00263         LDRBNE  R3,[R2,#TCB_RETVAL]     ; Write os_tsk.new->ret_val
00264         STRNE   R3,[R12]
00265 #ifdef  IFX_XMC4XXX
00266         PUSH    {LR}
00267         POP     {PC}
00268 #else
00269         BX      LR                      ; Return to Thread Mode
00270 #endif
00271 
00272         ALIGN
00273 }
00274 
00275 
00276 /*-------------------------- SysTick_Handler --------------------------------*/
00277 
00278 __asm void SysTick_Handler (void) {
00279         PRESERVE8
00280 
00281 #ifdef  IFX_XMC4XXX
00282         EXPORT  SysTick_Handler_Veneer
00283 SysTick_Handler_Veneer
00284 #endif
00285 
00286         PUSH    {R4,LR}                 ; Save EXC_RETURN
00287         BL      __cpp(rt_systick)
00288         B       Sys_Switch
00289 
00290         ALIGN
00291 }
00292 
00293 
00294 /*-------------------------- OS_Tick_Handler --------------------------------*/
00295 
00296 __asm void OS_Tick_Handler (void) {
00297         PRESERVE8
00298 
00299         PUSH    {R4,LR}                 ; Save EXC_RETURN
00300         BL      __cpp(os_tick_irqack)
00301         BL      __cpp(rt_systick)
00302         B       Sys_Switch
00303 
00304         ALIGN
00305 }
00306 
00307 
00308 /*--------------------------- rt_init_stack ---------------------------------*/
00309 
00310 void rt_init_stack (P_TCB p_TCB, FUNCP task_body) {
00311   /* Prepare TCB and saved context for a first time start of a task. */
00312   U32 *stk,i,size;
00313 
00314   /* Prepare a complete interrupt frame for first task start */
00315   size = p_TCB->priv_stack >> 2;
00316   if (size == 0) {
00317     size = (U16)os_stackinfo >> 2;
00318   }
00319 
00320   /* Write to the top of stack. */
00321   stk = &p_TCB->stack[size];
00322 
00323   /* Auto correct to 8-byte stack alignment. */
00324   if ((U32)stk & 0x04) {
00325     stk--;
00326   }
00327 
00328   stk -= 16;
00329 
00330   /* Default xPSR and initial PC */
00331   stk[15] = INITIAL_xPSR;
00332   stk[14] = (U32)task_body;
00333 
00334   /* Clear R1-R12,LR registers. */
00335   for (i = 0; i < 14; i++) {
00336     stk[i] = 0;
00337   }
00338 
00339   /* Assign a void pointer to R0. */
00340   stk[8] = (U32)p_TCB->msg;
00341 
00342   /* Initial Task stack pointer. */
00343   p_TCB->tsk_stack = (U32)stk;
00344 
00345   /* Task entry point. */
00346   p_TCB->ptask = task_body;
00347 
00348   /* Set a magic word for checking of stack overflow. */
00349   p_TCB->stack[0] = MAGIC_WORD;
00350 }
00351 
00352 
00353 /*--------------------------- dbg_init --------------------------------------*/
00354 
00355 void dbg_init (void) {
00356   if ((DEMCR & DEMCR_TRCENA)     &&
00357       (ITM_CONTROL & ITM_ITMENA) &&
00358       (ITM_ENABLE & (1UL << 31))) {
00359     dbg_msg = __TRUE;
00360   }
00361 }
00362 
00363 
00364 /*--------------------------- dbg_task_notify -------------------------------*/
00365 
00366 void dbg_task_notify (P_TCB p_tcb, BOOL create) {
00367   while (ITM_PORT31_U32 == 0);
00368   ITM_PORT31_U32 = (U32)p_tcb->ptask;
00369   while (ITM_PORT31_U32 == 0);
00370   ITM_PORT31_U16 = (create << 8) | p_tcb->task_id;
00371 }
00372 
00373 
00374 /*--------------------------- dbg_task_switch -------------------------------*/
00375 
00376 void dbg_task_switch (U32 task_id) {
00377   while (ITM_PORT31_U32 == 0);
00378   ITM_PORT31_U8 = task_id;
00379 }
00380 
00381 
00382 /*----------------------------------------------------------------------------
00383  * end of file
00384  *---------------------------------------------------------------------------*/
00385