1#include <k_config.h>
2
3;******************************************************************************
4;                            EXTERN PARAMETERS
5;******************************************************************************
6    EXTERN g_active_task
7    EXTERN g_preferred_ready_task
8    EXTERN krhino_stack_ovf_check
9    EXTERN krhino_task_sched_stats_get
10
11;******************************************************************************
12;                            EXPORT FUNCTIONS
13;******************************************************************************
14    PUBLIC cpu_intrpt_save
15    PUBLIC cpu_intrpt_restore
16    PUBLIC cpu_task_switch
17    PUBLIC cpu_intrpt_switch
18    PUBLIC cpu_first_task_start
19    PUBLIC _first_task_restore
20
21    PUBLIC PendSV_Handler
22
23;******************************************************************************
24;                                 EQUATES
25;******************************************************************************
26SCB_ICSR              EQU      0xE000ED04 ; Interrupt Control and State Register.
27SCB_VTOR              EQU      0xE000ED08 ; Vector Table Offset Register.
28ICSR_PENDSVSET        EQU      0x10000000 ; Value to trigger PendSV exception.
29
30SHPR3_PRI_14          EQU      0xE000ED22 ; System Handler Priority Register 3 (PendSV).
31PRI_LVL_PENDSV        EQU      0xFF       ; PendSV priority level (lowest).
32SHPR3_PRI_15          EQU      0xE000ED23 ; System Handler Priority Register 3 (SysTick).
33PRI_LVL_SYSTICK       EQU      0xFF       ; SYstick priority level (lowest).
34
35;******************************************************************************
36;                        CODE GENERATION DIRECTIVES
37;******************************************************************************
38    SECTION   .text:CODE(2)
39    THUMB
40    REQUIRE8
41    PRESERVE8
42
43;******************************************************************************
44; Functions:
45;     size_t cpu_intrpt_save(void);
46;     void cpu_intrpt_restore(size_t cpsr);
47;******************************************************************************
48cpu_intrpt_save:
49    MRS     R0, PRIMASK
50    CPSID   I
51    BX      LR
52
53cpu_intrpt_restore:
54    MSR     PRIMASK, R0
55    BX      LR
56
57;******************************************************************************
58; Functions:
59;     void cpu_intrpt_switch(void);
60;     void cpu_task_switch(void);
61;******************************************************************************
62cpu_task_switch:
63    LDR     R0, =SCB_ICSR
64    LDR     R1, =ICSR_PENDSVSET
65    STR     R1, [R0]
66    BX      LR
67
68cpu_intrpt_switch:
69    LDR     R0, =SCB_ICSR
70    LDR     R1, =ICSR_PENDSVSET
71    STR     R1, [R0]
72    BX      LR
73
74;******************************************************************************
75; Functions:
76;     void cpu_first_task_start(void);
77;******************************************************************************
78cpu_first_task_start:
79    ;set PendSV prority to the lowest
80    LDR     R0, =SHPR3_PRI_14
81    LDR     R1, =PRI_LVL_PENDSV
82    STRB    R1, [R0]
83
84    ;set Systick prority to the lowest
85    LDR     R0, =SHPR3_PRI_15
86    LDR     R1, =PRI_LVL_SYSTICK
87    STRB    R1, [R0]
88
89    ;indicate PendSV_Handler branch to _pendsv_handler_nosave
90    MOVS    R0, #0
91    MSR     PSP, R0
92
93    ;make PendSV exception pending
94    LDR     R0, =SCB_ICSR
95    LDR     R1, =ICSR_PENDSVSET
96    STR     R1, [R0]
97
98    ;goto PendSV_Handler
99    CPSIE   I
100    B       .
101
102;******************************************************************************
103; Functions:
104;     void krhino_pendsv_handler(void);
105;******************************************************************************
106PendSV_Handler:
107    CPSID   I
108    MRS     R0, PSP
109    ;branch if cpu_first_task_start
110    CMP     R0, #0
111    BEQ     _first_task_restore
112
113    ;hardware saved R0~R3,R12,LR,PC,xPSR
114
115    ;save context
116    #if defined(__ARMVFP__)
117    ;if the switchout task use FPU, save the FPU regs
118    TST     LR, #0x10
119    IT      EQ
120    VSTMDBEQ R0!, {D8 - D15}
121    ;hardware saved D0~D7, FPSCR
122    #endif
123
124    SUBS    R0, R0, #0x24
125    STM     R0, {R4-R11, LR}
126
127    ;g_active_task->task_stack = context region
128    LDR     R1, =g_active_task
129    LDR     R1, [R1]
130    STR     R0, [R1]
131
132#if (RHINO_CONFIG_TASK_STACK_OVF_CHECK > 0)
133    BL      krhino_stack_ovf_check
134#endif
135#if (RHINO_CONFIG_SYS_STATS > 0)
136    BL      krhino_task_sched_stats_get
137#endif
138
139_pendsv_handler_nosave:
140    LDR     R0, =g_active_task
141    LDR     R1, =g_preferred_ready_task
142    LDR     R2, [R1]
143    STR     R2, [R0]
144    ;R0 = g_active_task->task_stack = context region
145    LDR     R0, [R2]
146
147    ;restore context
148    LDM     R0, {R4-R11, LR}
149    ADDS    R0, R0, #0x24
150
151    #if defined(__ARMVFP__)
152    ;if the switchin task use FPU, save the FPU regs
153    TST     LR, #0x10
154    IT      EQ
155    VLDMIAEQ  R0!, {D8 - D15}
156    ;hardware will restore D0~D7, FPSCR
157    #endif
158
159    ;return stack = PSP
160    MSR     PSP, R0
161
162    CPSIE   I
163    ;hardware restore R0~R3,R12,LR,PC,xPSR
164    BX      LR
165
166_first_task_restore:
167    ;set MSP to the base of system stack
168    LDR     R0, =SCB_VTOR
169    LDR     R0, [R0]
170    LDR     R0, [R0]
171    MSR     MSP, R0
172
173    B       _pendsv_handler_nosave
174
175    END
176
177