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

;
; 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

;-----------------------------------------------------------
;                      extern variable
;-----------------------------------------------------------
	.ref rt_system_stack_top
;
;-----------------------------------------------------------
;

;-----------------------------------------------------------
;                      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]   B	_rt_thread_interrupt_stack
 	NOP	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
	NOP		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