rt-thread/libcpu/ti-dsp/c6x/context.asm

352 lines
9.8 KiB
NASM
Raw Normal View History

;
; Copyright (c) 2021, Shenzhen Academy of Aerospace Technology
;
; SPDX-License-Identifier: Apache-2.0
;
; Change Logs:
; Date Author Notes
; 2021-11-16 Dystopia the first version
;
;-----------------------------------------------------------
; context switch for C6000 DSP
;-----------------------------------------------------------
.include "contextinc.asm"
;-----------------------------------------------------------
; macro definition
;-----------------------------------------------------------
DP .set B14
SP .set B15
;
;-----------------------------------------------------------
;
;-----------------------------------------------------------
; global variable
;-----------------------------------------------------------
.global rt_interrupt_from_thread
.global rt_interrupt_to_thread
.global rt_thread_switch_interrupt_flag
;
;-----------------------------------------------------------
;
.sect ".text"
;-----------------------------------------------------------
; void rt_hw_enable_exception(void)
;-----------------------------------------------------------
.global rt_hw_enable_exception
rt_hw_enable_exception:
DINT
MVC .S2 TSR,B0
MVC .S2 B3,NRP
MVK .L2 0xC,B1
OR .D2 B0,B1,B0
MVC .S2 B0,TSR ; Set GEE and XEN in TSR
B .S2 NRP
NOP 5
;-----------------------------------------------------------
; rt_base_t rt_hw_interrupt_enable(void)
;-----------------------------------------------------------
.global rt_hw_interrupt_disable
rt_hw_interrupt_disable:
;{
MVC CSR,B4
MV B4,A4
AND 1,B4,B0
[!B0] CLR B4,1,1,B4
[B0] SET B4,1,1,B4
CLR B4,0,0,B4
MVC B4,CSR
B B3
NOP 5
;}
;-----------------------------------------------------------
; void rt_hw_interrupt_enable(rt_base_t scr)
;-----------------------------------------------------------
.global rt_hw_interrupt_enable
rt_hw_interrupt_enable:
;{
MVC A4,CSR
B B3
NOP 5
;}
;-----------------------------------------------------------
; rt_uint32_t rt_hw_get_current_dp(void)
;-----------------------------------------------------------
.global rt_hw_get_current_dp
rt_hw_get_current_dp:
;{
B B3
MV B14, A4
NOP 4
;}
;-----------------------------------------------------------
; rt_int32_t __fls(rt_int32_t val)
;-----------------------------------------------------------
.global __fls
__fls:
;{
B B3
LMBD .L1 1,A4,A4
NOP 4
;}
;-----------------------------------------------------------
; rt_int32_t __ffs(rt_int32_t val)
;-----------------------------------------------------------
.global __ffs
__ffs:
;{
BITR .M1 A4,A4
B B3
LMBD .L1 1,A4,A4
NOP 4
;}
;
;-----------------------------------------------------------
;
;
; void rt_hw_context_switch(rt_uint32 from, rt_uint32 to);
; A4 --> from
; B4 --> to
;
.global rt_hw_context_switch
rt_hw_context_switch:
; {
SUBAW .D2 SP,2,SP
ADD .D1X SP,-8,A15
|| STDW .D2T1 A15:A14,*SP--[3] ; Store A15:A14
STDW .D2T2 B13:B12,*SP--[1] ; Store B13:B12
|| STDW .D1T1 A13:A12,*A15--[1] ; Store A13:A12
|| MV B3,B13
STDW .D2T2 B11:B10,*SP--[1] ; Store B11:B10
|| STDW .D1T1 A11:A10,*A15--[1] ; Store A11:A10
|| MVC .S2 CSR,B12
STDW .D2T2 B13:B12,*SP--[1] ; Store PC:CSR
|| MVC .S2 TSR,B5
MVC .S2 ILC,B11
MVC .S2 RILC,B10
STDW .D2T2 B11:B10,*SP--[1] ; Store RILC:ILC
|| MV .S1X B5,A3
ZERO A2 ;
STDW .D2T1 A3:A2,*SP--[1] ; Store TSR:stack type
STW SP,*A4 ; Save thread's stack pointer
B rt_hw_context_switch_to
MV B4,A4
NOP 4
;}
;
; void rt_hw_context_switch_to(rt_uint32 to);
; A4 --> to
;
.global rt_hw_context_switch_to
rt_hw_context_switch_to:
;{
LDW *A4,SP
NOP 4
LDDW .D2T2 *++SP[1],B9:B8 ; get TSR (B9) and stack frame type (B8)
LDDW .D2T2 *++SP[1],B11:B10 ; get RILC (B11) and ILC (B10)
LDDW .D2T2 *++SP[1],B13:B12 ; get PC (B13) and CSR (B12)
NOP 2
MV B8,B0
[B0] BNOP _rt_thread_interrupt_stack, 5
;
; this maybe do better
;
LDDW .D2T2 *++SP[1],B11:B10
|| MVC .S2 B11,RILC ; Restore RILC
LDDW .D2T2 *++SP[1],B13:B12
|| MVC .S2 B10,ILC ; Restore ILC
LDDW .D2T1 *++SP[1],A11:A10
|| MV B13,B3 ; Restore PC
LDDW .D2T1 *++SP[1],A13:A12
|| MVC .S2 B12,CSR ; Restore CSR
LDDW .D2T1 *++SP[1],A15:A14
B B3 ; Return to caller
ADDAW .D2 SP,2,SP
NOP 4 ; Delay slots
_rt_thread_interrupt_stack:
ADDAW .D1X SP,30,A15
LDDW .D1T1 *++A15[1],A17:A16
|| LDDW .D2T2 *++SP[1],B17:B16
LDDW .D1T1 *++A15[1],A19:A18
|| LDDW .D2T2 *++SP[1],B19:B18
LDDW .D1T1 *++A15[1],A21:A20
|| LDDW .D2T2 *++SP[1],B21:B20
LDDW .D1T1 *++A15[1],A23:A22
|| LDDW .D2T2 *++SP[1],B23:B22
LDDW .D1T1 *++A15[1],A25:A24
|| LDDW .D2T2 *++SP[1],B25:B24
LDDW .D1T1 *++A15[1],A27:A26
|| LDDW .D2T2 *++SP[1],B27:B26
LDDW .D1T1 *++A15[1],A29:A28
|| LDDW .D2T2 *++SP[1],B29:B28
LDDW .D1T1 *++A15[1],A31:A30
|| LDDW .D2T2 *++SP[1],B31:B30
LDDW .D1T1 *++A15[1],A1:A0
|| LDDW .D2T2 *++SP[1],B1:B0
LDDW .D1T1 *++A15[1],A3:A2
|| LDDW .D2T2 *++SP[1],B3:B2
|| MVC .S2 B9,ITSR ; Restore ITSR
LDDW .D1T1 *++A15[1],A5:A4
|| LDDW .D2T2 *++SP[1],B5:B4
|| MVC .S2 B11,RILC ; Restore RILC
LDDW .D1T1 *++A15[1],A7:A6
|| LDDW .D2T2 *++SP[1],B7:B6
|| MVC .S2 B10,ILC ; Restore ILC
LDDW .D1T1 *++A15[1],A9:A8
|| LDDW .D2T2 *++SP[1],B9:B8
|| MVC .S2 B13,IRP ; Restore IPR
LDDW .D1T1 *++A15[1],A11:A10
|| LDDW .D2T2 *++SP[1],B11:B10
|| MVC .S2 B12,CSR ; Restore CSR
LDDW .D1T1 *++A15[1],A13:A12
|| LDDW .D2T2 *++SP[1],B13:B12
MV .D2X A15,SP
LDDW .D2T1 *++SP[1],A15:A14
B IRP ; Return to point of interrupt
LDDW .D2T2 *+SP[1],SP:DP
NOP 4 ; Delay slots
;}
;
;-----------------------------------------------------------
;
;
; void rt_hw_context_switch_interrupt(rt_uint32_t from, rt_uint32_t to)
; A4 --> from
; B4 --> to
;{
.global rt_hw_context_switch_interrupt
rt_hw_context_switch_interrupt:
SUB B15,0x8,B15
STW B4,*B15[2]
STW A4,*B15[1]
LDW *+B14(rt_thread_switch_interrupt_flag),B4
NOP 4
CMPEQ 1,B4,B0
[ B0] BNOP _reswitch,5
MVK 1,B4
STW B4,*+B14(rt_thread_switch_interrupt_flag)
MV A4,B4
STW B4,*+B14(rt_interrupt_from_thread)
_reswitch:
LDW *B15[2],B4
NOP 4
STW B4,*+B14(rt_interrupt_to_thread)
ADD 8,B15,B15
BNOP B3,5
;}
;-----------------------------------------------------------
;
;void rt_interrupt_context_restore(void)
;
.global rt_interrupt_context_restore
rt_interrupt_context_restore:
;{
MVKL rt_thread_switch_interrupt_flag,A3
MVKH rt_thread_switch_interrupt_flag,A3
LDW *A3,A1
NOP 4
CMPEQ 1,A1,A2
[A2] BNOP rt_preempt_context_restore,5
LDDW .D2T2 *++SP[1],B9:B8 ; get TSR (B9)
LDDW .D2T2 *++SP[1],B11:B10 ; get RILC (B11) and ILC (B10)
LDDW .D2T2 *++SP[1],B13:B12 ; get PC (B13) and CSR (B12)
ADDAW .D1X SP,30,A15
LDDW .D1T1 *++A15[1],A17:A16
|| LDDW .D2T2 *++SP[1],B17:B16
LDDW .D1T1 *++A15[1],A19:A18
|| LDDW .D2T2 *++SP[1],B19:B18
LDDW .D1T1 *++A15[1],A21:A20
|| LDDW .D2T2 *++SP[1],B21:B20
LDDW .D1T1 *++A15[1],A23:A22
|| LDDW .D2T2 *++SP[1],B23:B22
LDDW .D1T1 *++A15[1],A25:A24
|| LDDW .D2T2 *++SP[1],B25:B24
LDDW .D1T1 *++A15[1],A27:A26
|| LDDW .D2T2 *++SP[1],B27:B26
LDDW .D1T1 *++A15[1],A29:A28
|| LDDW .D2T2 *++SP[1],B29:B28
LDDW .D1T1 *++A15[1],A31:A30
|| LDDW .D2T2 *++SP[1],B31:B30
LDDW .D1T1 *++A15[1],A1:A0
|| LDDW .D2T2 *++SP[1],B1:B0
LDDW .D1T1 *++A15[1],A3:A2
|| LDDW .D2T2 *++SP[1],B3:B2
|| MVC .S2 B9,ITSR
LDDW .D1T1 *++A15[1],A5:A4
|| LDDW .D2T2 *++SP[1],B5:B4
|| MVC .S2 B11,RILC
LDDW .D1T1 *++A15[1],A7:A6
|| LDDW .D2T2 *++SP[1],B7:B6
|| MVC .S2 B10,ILC
LDDW .D1T1 *++A15[1],A9:A8
|| LDDW .D2T2 *++SP[1],B9:B8
|| MVC .S2 B13,IRP
LDDW .D1T1 *++A15[1],A11:A10
|| LDDW .D2T2 *++SP[1],B11:B10
|| MVC .S2 B12,CSR
LDDW .D1T1 *++A15[1],A13:A12
|| LDDW .D2T2 *++SP[1],B13:B12
MV .D2X A15,SP
|| MVKL .S1 rt_system_stack_top,A15
MVKH .S1 rt_system_stack_top,A15
|| ADDAW .D1X SP,6,A14
STW .D1T1 A14,*A15 ; save system stack pointer
LDDW .D2T1 *++SP[1],A15:A14
B .S2 IRP ; return from interruption
LDDW .D2T2 *+SP[1],SP:DP
NOP 4
rt_preempt_context_restore:
ZERO A12
STW A12,*A3 ; clear rt_thread_switch_interrupt_flag
;
; restore saved registers by system stack
;
RESTORE_ALL IRP,ITSR
;
; store registers to thread stack
;
THREAD_SAVE_ALL IRP,ITSR
MVKL rt_interrupt_from_thread,A11
MVKH rt_interrupt_from_thread,A11
LDW *A11,A10
NOP
MVKL rt_interrupt_to_thread,B10
MVKH rt_interrupt_to_thread,B10
LDW *B10,B11
NOP 3
STW SP,*A10 ; store sp in preempted tasks's TCB
B rt_hw_context_switch_to
MV B11,A4
NOP 4
;}
.end