1;
2; Copyright (c) 2021, Shenzhen Academy of Aerospace Technology
3;
4; SPDX-License-Identifier: Apache-2.0
5;
6; Change Logs:
7; Date           Author       Notes
8; 2021-11-16     Dystopia     the first version
9;
10
11;-----------------------------------------------------------
12;               context switch for C6000 DSP
13;-----------------------------------------------------------
14
15    .include "contextinc.asm"
16;-----------------------------------------------------------
17;                      macro definition
18;-----------------------------------------------------------
19DP  .set    B14
20SP  .set    B15
21;
22;-----------------------------------------------------------
23;
24
25;-----------------------------------------------------------
26;                      global variable
27;-----------------------------------------------------------
28    .global rt_interrupt_from_thread
29    .global rt_interrupt_to_thread
30    .global rt_thread_switch_interrupt_flag
31;
32;-----------------------------------------------------------
33;
34    .sect   ".text"
35;-----------------------------------------------------------
36; void rt_hw_enable_exception(void)
37;-----------------------------------------------------------
38    .global rt_hw_enable_exception
39rt_hw_enable_exception:
40    DINT
41    MVC .S2 TSR,B0
42    MVC .S2 B3,NRP
43    MVK .L2 0xC,B1
44    OR  .D2 B0,B1,B0
45    MVC .S2 B0,TSR    ; Set GEE and XEN in TSR
46    B   .S2 NRP
47    NOP 5
48
49;-----------------------------------------------------------
50; rt_base_t rt_hw_interrupt_enable(void)
51;-----------------------------------------------------------
52    .global rt_hw_interrupt_disable
53rt_hw_interrupt_disable:
54;{
55    MVC CSR,B4
56    MV  B4,A4
57    AND 1,B4,B0
58 [!B0]  CLR B4,1,1,B4
59 [B0]   SET B4,1,1,B4
60    CLR B4,0,0,B4
61    MVC B4,CSR
62    B   B3
63    NOP 5
64;}
65
66;-----------------------------------------------------------
67; void rt_hw_interrupt_enable(rt_base_t scr)
68;-----------------------------------------------------------
69    .global rt_hw_interrupt_enable
70rt_hw_interrupt_enable:
71;{
72    MVC A4,CSR
73    B   B3
74    NOP 5
75;}
76
77;-----------------------------------------------------------
78; rt_uint32_t rt_hw_get_current_dp(void)
79;-----------------------------------------------------------
80    .global rt_hw_get_current_dp
81rt_hw_get_current_dp:
82;{
83    B   B3
84    MV  B14,    A4
85    NOP 4
86;}
87
88;-----------------------------------------------------------
89; rt_int32_t __fls(rt_int32_t val)
90;-----------------------------------------------------------
91    .global __fls
92__fls:
93;{
94    B   B3
95    LMBD  .L1  1,A4,A4
96    NOP 4
97;}
98
99;-----------------------------------------------------------
100; rt_int32_t __ffs(rt_int32_t val)
101;-----------------------------------------------------------
102    .global __ffs
103__ffs:
104;{
105    BITR  .M1  A4,A4
106    B   B3
107    LMBD  .L1  1,A4,A4
108    NOP 4
109;}
110
111;
112;-----------------------------------------------------------
113;
114
115;
116; void rt_hw_context_switch(rt_uint32 from, rt_uint32 to);
117; A4 --> from
118; B4 --> to
119;
120    .global rt_hw_context_switch
121rt_hw_context_switch:
122; {
123    SUBAW   .D2 SP,2,SP
124    ADD .D1X    SP,-8,A15
125 || STDW    .D2T1   A15:A14,*SP--[3]    ; Store A15:A14
126
127    STDW    .D2T2   B13:B12,*SP--[1]    ; Store B13:B12
128 || STDW    .D1T1   A13:A12,*A15--[1]   ; Store A13:A12
129 || MV      B3,B13
130    STDW    .D2T2   B11:B10,*SP--[1]    ; Store B11:B10
131 || STDW    .D1T1   A11:A10,*A15--[1]   ; Store A11:A10
132 || MVC .S2 CSR,B12
133    STDW    .D2T2   B13:B12,*SP--[1]    ; Store PC:CSR
134 || MVC .S2 TSR,B5
135
136    MVC .S2 ILC,B11
137    MVC .S2 RILC,B10
138    STDW    .D2T2   B11:B10,*SP--[1]    ; Store RILC:ILC
139 || MV  .S1X    B5,A3
140
141    ZERO    A2                          ;
142    STDW    .D2T1   A3:A2,*SP--[1]      ; Store TSR:stack type
143    STW SP,*A4                          ; Save thread's stack pointer
144    B   rt_hw_context_switch_to
145    MV  B4,A4
146    NOP 4
147;}
148
149;
150; void rt_hw_context_switch_to(rt_uint32 to);
151; A4 --> to
152;
153    .global rt_hw_context_switch_to
154rt_hw_context_switch_to:
155;{
156    LDW *A4,SP
157    NOP 4
158    LDDW    .D2T2   *++SP[1],B9:B8      ; get TSR (B9) and stack frame type (B8)
159    LDDW    .D2T2   *++SP[1],B11:B10    ; get RILC (B11) and ILC (B10)
160    LDDW    .D2T2   *++SP[1],B13:B12    ; get PC (B13) and CSR (B12)
161    NOP 2
162    MV  B8,B0
163 [B0]  BNOP _rt_thread_interrupt_stack, 5
164;
165; this maybe do better
166;
167    LDDW    .D2T2   *++SP[1],B11:B10
168 || MVC .S2 B11,RILC                    ; Restore RILC
169    LDDW    .D2T2   *++SP[1],B13:B12
170 || MVC .S2 B10,ILC                     ; Restore ILC
171    LDDW    .D2T1   *++SP[1],A11:A10
172 || MV  B13,B3                          ; Restore PC
173    LDDW    .D2T1   *++SP[1],A13:A12
174 || MVC .S2 B12,CSR                     ; Restore CSR
175    LDDW    .D2T1   *++SP[1],A15:A14
176    B   B3                              ; Return to caller
177    ADDAW   .D2 SP,2,SP
178    NOP 4                               ; Delay slots
179_rt_thread_interrupt_stack:
180    ADDAW   .D1X    SP,30,A15
181    LDDW    .D1T1   *++A15[1],A17:A16
182 || LDDW    .D2T2   *++SP[1],B17:B16
183    LDDW    .D1T1   *++A15[1],A19:A18
184 || LDDW    .D2T2   *++SP[1],B19:B18
185    LDDW    .D1T1   *++A15[1],A21:A20
186 || LDDW    .D2T2   *++SP[1],B21:B20
187    LDDW    .D1T1   *++A15[1],A23:A22
188 || LDDW    .D2T2   *++SP[1],B23:B22
189    LDDW    .D1T1   *++A15[1],A25:A24
190 || LDDW    .D2T2   *++SP[1],B25:B24
191    LDDW    .D1T1   *++A15[1],A27:A26
192 || LDDW    .D2T2   *++SP[1],B27:B26
193    LDDW    .D1T1   *++A15[1],A29:A28
194 || LDDW    .D2T2   *++SP[1],B29:B28
195    LDDW    .D1T1   *++A15[1],A31:A30
196 || LDDW    .D2T2   *++SP[1],B31:B30
197
198    LDDW    .D1T1   *++A15[1],A1:A0
199 || LDDW    .D2T2   *++SP[1],B1:B0
200
201    LDDW    .D1T1   *++A15[1],A3:A2
202 || LDDW    .D2T2   *++SP[1],B3:B2
203 || MVC .S2 B9,ITSR                     ; Restore ITSR
204    LDDW    .D1T1   *++A15[1],A5:A4
205 || LDDW    .D2T2   *++SP[1],B5:B4
206 || MVC .S2 B11,RILC                    ; Restore RILC
207    LDDW    .D1T1   *++A15[1],A7:A6
208 || LDDW    .D2T2   *++SP[1],B7:B6
209 || MVC .S2 B10,ILC                     ; Restore ILC
210
211    LDDW    .D1T1   *++A15[1],A9:A8
212 || LDDW    .D2T2   *++SP[1],B9:B8
213 || MVC .S2 B13,IRP                     ; Restore IPR
214
215    LDDW    .D1T1   *++A15[1],A11:A10
216 || LDDW    .D2T2   *++SP[1],B11:B10
217 || MVC .S2 B12,CSR                     ; Restore CSR
218
219    LDDW    .D1T1   *++A15[1],A13:A12
220 || LDDW    .D2T2   *++SP[1],B13:B12
221
222    MV  .D2X    A15,SP
223    LDDW    .D2T1   *++SP[1],A15:A14
224    B   IRP                             ; Return to point of interrupt
225    LDDW    .D2T2   *+SP[1],SP:DP
226    NOP 4                               ; Delay slots
227;}
228
229;
230;-----------------------------------------------------------
231;
232
233;
234; void rt_hw_context_switch_interrupt(rt_uint32_t from, rt_uint32_t to)
235; A4 --> from
236; B4 --> to
237;{
238    .global rt_hw_context_switch_interrupt
239rt_hw_context_switch_interrupt:
240    SUB     B15,0x8,B15
241    STW     B4,*B15[2]
242    STW     A4,*B15[1]
243    LDW     *+B14(rt_thread_switch_interrupt_flag),B4
244    NOP     4
245    CMPEQ   1,B4,B0
246    [ B0]  BNOP _reswitch,5
247    MVK     1,B4
248    STW     B4,*+B14(rt_thread_switch_interrupt_flag)
249    MV          A4,B4
250    STW     B4,*+B14(rt_interrupt_from_thread)
251_reswitch:
252    LDW     *B15[2],B4
253    NOP     4
254    STW     B4,*+B14(rt_interrupt_to_thread)
255    ADD     8,B15,B15
256    BNOP    B3,5
257;}
258
259;-----------------------------------------------------------
260;
261;void rt_interrupt_context_restore(void)
262;
263    .global rt_interrupt_context_restore
264rt_interrupt_context_restore:
265;{
266    MVKL    rt_thread_switch_interrupt_flag,A3
267    MVKH    rt_thread_switch_interrupt_flag,A3
268    LDW     *A3,A1
269    NOP     4
270    CMPEQ   1,A1,A2
271 [A2]  BNOP  rt_preempt_context_restore,5
272    LDDW    .D2T2   *++SP[1],B9:B8      ; get TSR (B9)
273    LDDW    .D2T2   *++SP[1],B11:B10    ; get RILC (B11) and ILC (B10)
274    LDDW    .D2T2   *++SP[1],B13:B12    ; get PC (B13) and CSR (B12)
275
276    ADDAW   .D1X    SP,30,A15
277
278    LDDW    .D1T1   *++A15[1],A17:A16
279 || LDDW    .D2T2   *++SP[1],B17:B16
280    LDDW    .D1T1   *++A15[1],A19:A18
281 || LDDW    .D2T2   *++SP[1],B19:B18
282    LDDW    .D1T1   *++A15[1],A21:A20
283 || LDDW    .D2T2   *++SP[1],B21:B20
284    LDDW    .D1T1   *++A15[1],A23:A22
285 || LDDW    .D2T2   *++SP[1],B23:B22
286    LDDW    .D1T1   *++A15[1],A25:A24
287 || LDDW    .D2T2   *++SP[1],B25:B24
288    LDDW    .D1T1   *++A15[1],A27:A26
289 || LDDW    .D2T2   *++SP[1],B27:B26
290    LDDW    .D1T1   *++A15[1],A29:A28
291 || LDDW    .D2T2   *++SP[1],B29:B28
292    LDDW    .D1T1   *++A15[1],A31:A30
293 || LDDW    .D2T2   *++SP[1],B31:B30
294
295    LDDW    .D1T1   *++A15[1],A1:A0
296 || LDDW    .D2T2   *++SP[1],B1:B0
297    LDDW    .D1T1   *++A15[1],A3:A2
298 || LDDW    .D2T2   *++SP[1],B3:B2
299 || MVC .S2 B9,ITSR
300    LDDW    .D1T1   *++A15[1],A5:A4
301 || LDDW    .D2T2   *++SP[1],B5:B4
302 || MVC .S2 B11,RILC
303    LDDW    .D1T1   *++A15[1],A7:A6
304 || LDDW    .D2T2   *++SP[1],B7:B6
305 || MVC .S2 B10,ILC
306    LDDW    .D1T1   *++A15[1],A9:A8
307 || LDDW    .D2T2   *++SP[1],B9:B8
308 || MVC .S2 B13,IRP
309
310    LDDW    .D1T1   *++A15[1],A11:A10
311 || LDDW    .D2T2   *++SP[1],B11:B10
312 || MVC .S2 B12,CSR
313    LDDW    .D1T1   *++A15[1],A13:A12
314 || LDDW    .D2T2   *++SP[1],B13:B12
315
316    MV  .D2X    A15,SP
317 || MVKL    .S1 rt_system_stack_top,A15
318    MVKH    .S1 rt_system_stack_top,A15
319 || ADDAW   .D1X    SP,6,A14
320    STW .D1T1   A14,*A15                ; save system stack pointer
321
322    LDDW    .D2T1   *++SP[1],A15:A14
323    B   .S2 IRP                         ; return from interruption
324    LDDW    .D2T2   *+SP[1],SP:DP
325    NOP     4
326rt_preempt_context_restore:
327    ZERO    A12
328    STW A12,*A3                         ; clear rt_thread_switch_interrupt_flag
329;
330; restore saved registers by system stack
331;
332    RESTORE_ALL IRP,ITSR
333;
334; store registers to thread stack
335;
336    THREAD_SAVE_ALL IRP,ITSR
337
338    MVKL    rt_interrupt_from_thread,A11
339    MVKH    rt_interrupt_from_thread,A11
340    LDW     *A11,A10
341    NOP
342    MVKL    rt_interrupt_to_thread,B10
343    MVKH    rt_interrupt_to_thread,B10
344    LDW     *B10,B11
345    NOP     3
346    STW     SP,*A10                     ; store sp in preempted tasks's TCB
347    B       rt_hw_context_switch_to
348    MV      B11,A4
349    NOP     4
350;}
351    .end
352