添加支持中断栈部分代码，修改格式

10 files changed, 268 insertions, 186 deletions

diff --git a/libcpu/ti-dsp/c6x/c66xx.h b/libcpu/ti-dsp/c6x/c66xx.h
index 9f3a20399f..97d817a6d2 100644
--- a/libcpu/ti-dsp/c6x/c66xx.h
+++ b/libcpu/ti-dsp/c6x/c66xx.h
@@ -91,14 +91,13 @@ extern cregister volatile unsigned int DNUM;    /* Core number */
 
 #define __dint()				asm(" DINT")
 #define __rint()				asm(" RINT")
-
 #define __system_call()			asm(" SWE")
 #define __enter_idle()			asm(" IDLE")
 #define __nop()					asm(" NOP")
 #define __mfence()				asm(" MFENCE")
 
-#define __SYSREG(ADDR, TYPE) 	(*(volatile TYPE*)(ADDR))
-#define __SYSREGA(ADDR, TYPE) 	((volatile TYPE*)(ADDR))
+#define __SYSREG(ADDR, TYPE)	(*(volatile TYPE*)(ADDR))
+#define __SYSREGA(ADDR, TYPE)	((volatile TYPE*)(ADDR))
 
 extern void rt_hw_enable_exception(void);
 
diff --git a/libcpu/ti-dsp/c6x/context.asm b/libcpu/ti-dsp/c6x/context.asm
index 07affb6a1a..f6972013f5 100644
--- a/libcpu/ti-dsp/c6x/context.asm
+++ b/libcpu/ti-dsp/c6x/context.asm
@@ -12,12 +12,17 @@
 ; context switch for C6678 DSP
 ;-----------------------------------------------------------
 
+	.include "contextinc.asm"
 ;-----------------------------------------------------------
 ; 						macro definition
 ;-----------------------------------------------------------
 DP	.set	B14
 SP	.set	B15
 
+;-----------------------------------------------------------
+; 						extern variable
+;-----------------------------------------------------------
+	.ref rt_system_stack_top
 ;
 ;-----------------------------------------------------------
 ;
@@ -28,7 +33,6 @@ SP	.set	B15
 	.global	rt_interrupt_from_thread
 	.global	rt_interrupt_to_thread
 	.global	rt_thread_switch_interrupt_flag
-
 ;
 ;-----------------------------------------------------------
 ;
@@ -100,18 +104,17 @@ rt_hw_context_switch:
 	STDW	.D2T2	B13:B12,*SP--[1]	; Store PC:CSR
  ||	MVC	.S2	TSR,B5
 
-	MVC	ILC,B11							;
-	MVC	RILC,B10						;
+	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
-
-	MV	B4,A4							;
 	B	rt_hw_context_switch_to
-	NOP	5
+	MV	B4,A4							;
+	NOP	4
 ;}
 
 ;
@@ -138,13 +141,13 @@ rt_hw_context_switch_to:
 	MV	B13,B3							; Restore PC
 	MVC	.S2	B12,CSR						; Restore CSR
 
-	LDDW	*++SP[1],B11:B10
-  	LDDW	*++SP[1],B13:B12
-	LDDW	*++SP[1],A11:A10
-	LDDW	*++SP[1],A13:A12
-	LDDW	*++SP[1],A15:A14
+	LDDW	.D2T2	*++SP[1],B11:B10
+  	LDDW	.D2T2	*++SP[1],B13:B12
+	LDDW	.D2T1	*++SP[1],A11:A10
+	LDDW	.D2T1	*++SP[1],A13:A12
+	LDDW	.D2T1	*++SP[1],A15:A14
 	B	B3                              ; Return to caller
-	ADDAW	SP,2,SP
+	ADDAW	.D2	SP,2,SP
 	NOP	4                               ; Delay slots
 _rt_thread_interrupt_stack:
 	ADDAW	.D1X	SP,30,A15
@@ -204,6 +207,7 @@ _rt_thread_interrupt_stack:
 ; 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
@@ -285,14 +289,26 @@ rt_interrupt_context_restore:
  ||	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
+	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
@@ -302,10 +318,10 @@ rt_preempt_context_restore:
     MVKH	rt_interrupt_to_thread,B10
     LDW		*B10,B11
     NOP		3
-    STW		SP,*A10	    					; store sp in preempted tasks's TCB
-	MV		B11,A4							;
-	B		rt_hw_context_switch_to
-	NOP		5
+    STW		SP,*A10	    				; store sp in preempted tasks's TCB
+    B		rt_hw_context_switch_to
+	MV		B11,A4						;
+	NOP		4
 ;}
 
 	.end
diff --git a/libcpu/ti-dsp/c6x/contextinc.asm b/libcpu/ti-dsp/c6x/contextinc.asm
new file mode 100644
index 0000000000..18926aa088
--- /dev/null
+++ b/libcpu/ti-dsp/c6x/contextinc.asm
@@ -0,0 +1,187 @@
+;
+; 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
+;
+
+;-----------------------------------------------------------
+;					macro definition
+;-----------------------------------------------------------
+SAVE_ALL	.macro  __rp, __tsr
+	STW	.D2T2	B0,*SP--[2]				; save original B0
+	MVKL	.S2	rt_system_stack_top,B0
+	MVKH	.S2	rt_system_stack_top,B0
+	LDW	.D2T2	*B0,B1					; system stack
+
+	NOP	3
+	STW	.D2T2	B1,*+SP[1]				; save original B1
+	XOR	.D2	SP,B1,B0					; (SP ^ KSP, check current stack types)
+	LDW	.D2T2	*+SP[1],B1				; restore B0/B1
+	LDW	.D2T2	*++SP[2],B0
+	SHR	.S2	B0,12,B0					; 0 if already using system stack
+ [B0]	STDW	.D2T2	SP:DP,*--B1[1]	; thread: save thread sp/dp system stack
+ [B0]	MV	.S2	B1,SP					; and switch to system stack
+||[!B0] STDW	.D2T2	SP:DP,*--SP[1]	; kernel: nest interrupt save(not support)
+
+	SUBAW	.D2	SP,2,SP
+
+	ADD	.D1X	SP,-8,A15
+ ||	STDW	.D2T1	A15:A14,*SP--[16]	; save A15:A14
+
+	STDW	.D2T2	B13:B12,*SP--[1]
+ ||	STDW	.D1T1	A13:A12,*A15--[1]
+ ||	MVC	.S2	__rp,B13
+	STDW	.D2T2	B11:B10,*SP--[1]
+ ||	STDW	.D1T1	A11:A10,*A15--[1]
+ ||	MVC	.S2	CSR,B12
+
+	STDW	.D2T2	B9:B8,*SP--[1]
+ ||	STDW	.D1T1	A9:A8,*A15--[1]
+ ||	MVC	.S2	RILC,B11
+	STDW	.D2T2	B7:B6,*SP--[1]
+ ||	STDW	.D1T1	A7:A6,*A15--[1]
+ ||	MVC	.S2	ILC,B10
+	STDW	.D2T2	B5:B4,*SP--[1]
+ ||	STDW	.D1T1	A5:A4,*A15--[1]
+	STDW	.D2T2	B3:B2,*SP--[1]
+ ||	STDW	.D1T1	A3:A2,*A15--[1]
+ ||	MVC	.S2	__tsr,B5
+	STDW	.D2T2	B1:B0,*SP--[1]
+ ||	STDW	.D1T1	A1:A0,*A15--[1]
+ ||	MV	.S1X	B5,A5
+
+	STDW	.D2T2	B31:B30,*SP--[1]
+ ||	STDW	.D1T1	A31:A30,*A15--[1]
+ || MVKL	1,A4
+
+	STDW	.D2T2	B29:B28,*SP--[1]
+ ||	STDW	.D1T1	A29:A28,*A15--[1]
+	STDW	.D2T2	B27:B26,*SP--[1]
+ ||	STDW	.D1T1	A27:A26,*A15--[1]
+	STDW	.D2T2	B25:B24,*SP--[1]
+ ||	STDW	.D1T1	A25:A24,*A15--[1]
+	STDW	.D2T2	B23:B22,*SP--[1]
+ ||	STDW	.D1T1	A23:A22,*A15--[1]
+	STDW	.D2T2	B21:B20,*SP--[1]
+ ||	STDW	.D1T1	A21:A20,*A15--[1]
+	STDW	.D2T2	B19:B18,*SP--[1]
+ ||	STDW	.D1T1	A19:A18,*A15--[1]
+	STDW	.D2T2	B17:B16,*SP--[1]
+ ||	STDW	.D1T1	A17:A16,*A15--[1]
+
+	STDW	.D2T2	B13:B12,*SP--[1]	; save PC and CSR
+	STDW	.D2T2	B11:B10,*SP--[1]	; save RILC and ILC
+	STDW	.D2T1	A5:A4,*SP--[1]		; save TSR and orig A4
+			.endm
+
+RESTORE_ALL	.macro  __rp, __tsr
+	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,__tsr
+	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,__rp
+
+	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
+	LDDW	.D2T2	*+SP[1],SP:DP
+	NOP		4
+			.endm
+
+THREAD_SAVE_ALL	.macro  __rp, __tsr
+	STDW	.D2T2	SP:DP,*--SP[1]
+	SUBAW	.D2	SP,2,SP
+
+	ADD	.D1X	SP,-8,A15
+ ||	STDW	.D2T1	A15:A14,*SP--[16]	; save A15:A14
+
+	STDW	.D2T2	B13:B12,*SP--[1]
+ ||	STDW	.D1T1	A13:A12,*A15--[1]
+ ||	MVC	.S2	__rp,B13
+	STDW	.D2T2	B11:B10,*SP--[1]
+ ||	STDW	.D1T1	A11:A10,*A15--[1]
+ ||	MVC	.S2	CSR,B12
+
+	STDW	.D2T2	B9:B8,*SP--[1]
+ ||	STDW	.D1T1	A9:A8,*A15--[1]
+ ||	MVC	.S2	RILC,B11
+	STDW	.D2T2	B7:B6,*SP--[1]
+ ||	STDW	.D1T1	A7:A6,*A15--[1]
+ ||	MVC	.S2	ILC,B10
+	STDW	.D2T2	B5:B4,*SP--[1]
+ ||	STDW	.D1T1	A5:A4,*A15--[1]
+	STDW	.D2T2	B3:B2,*SP--[1]
+ ||	STDW	.D1T1	A3:A2,*A15--[1]
+ ||	MVC	.S2	__tsr,B5
+	STDW	.D2T2	B1:B0,*SP--[1]
+ ||	STDW	.D1T1	A1:A0,*A15--[1]
+ ||	MV	.S1X	B5,A5
+
+	STDW	.D2T2	B31:B30,*SP--[1]
+ ||	STDW	.D1T1	A31:A30,*A15--[1]
+ || MVKL	1,A4
+	STDW	.D2T2	B29:B28,*SP--[1]
+ ||	STDW	.D1T1	A29:A28,*A15--[1]
+	STDW	.D2T2	B27:B26,*SP--[1]
+ ||	STDW	.D1T1	A27:A26,*A15--[1]
+	STDW	.D2T2	B25:B24,*SP--[1]
+ ||	STDW	.D1T1	A25:A24,*A15--[1]
+	STDW	.D2T2	B23:B22,*SP--[1]
+ ||	STDW	.D1T1	A23:A22,*A15--[1]
+	STDW	.D2T2	B21:B20,*SP--[1]
+ ||	STDW	.D1T1	A21:A20,*A15--[1]
+	STDW	.D2T2	B19:B18,*SP--[1]
+ ||	STDW	.D1T1	A19:A18,*A15--[1]
+	STDW	.D2T2	B17:B16,*SP--[1]
+ ||	STDW	.D1T1	A17:A16,*A15--[1]
+
+	STDW	.D2T2	B13:B12,*SP--[1]	; save PC and CSR
+	STDW	.D2T2	B11:B10,*SP--[1]	; save RILC and ILC
+	STDW	.D2T1	A5:A4,*SP--[1]		; save TSR and orig A4
+				.endm
diff --git a/libcpu/ti-dsp/c6x/cpuport.c b/libcpu/ti-dsp/c6x/cpuport.c
index a00a2e9046..716e8a16fa 100644
--- a/libcpu/ti-dsp/c6x/cpuport.c
+++ b/libcpu/ti-dsp/c6x/cpuport.c
@@ -27,7 +27,6 @@ RT_WEAK void rt_hw_cpu_shutdown(void)
 
     RT_ASSERT(0);
 }
-//-----------------------------------------------------------------------------
 
 /*------------ nested_exception_handler() function ---------------------------
  * DESCRIPTION: Function handles Nested Exception
@@ -40,8 +39,6 @@ void nested_exception_handler(void)
 {
 	for ( ; ; );
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_nmi_handler() function --------------------------------------
  * DESCRIPTION: Function handles NMI interrupt
@@ -53,8 +50,6 @@ void hw_nmi_handler(struct rt_hw_exp_stack_register *regs)
 {
 	for ( ; ; );
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_bad_handler() function --------------------------------------
  * DESCRIPTION: Function handles Bad interrupt
@@ -66,8 +61,6 @@ void hw_bad_handler(void)
 {
 	for ( ; ; );
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_int4_handler() function -------------------------------------
  * DESCRIPTION: Function handles INT4 interrupt
@@ -79,8 +72,6 @@ void hw_int4_handler(void)
 {
 
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_int5_handler() function -------------------------------------
  * DESCRIPTION: Function handles INT5 interrupt
@@ -92,8 +83,6 @@ void hw_int5_handler(void)
 {
 
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_int6_handler() function -------------------------------------
  * DESCRIPTION: Function handles INT6 interrupt
@@ -105,8 +94,6 @@ void hw_int6_handler(void)
 {
 
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_int7_handler() function -------------------------------------
  * DESCRIPTION: Function handles INT7 interrupt
@@ -118,8 +105,6 @@ void hw_int7_handler(void)
 {
 
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_int8_handler() function -------------------------------------
  * DESCRIPTION: Function handles INT8 interrupt
@@ -131,8 +116,6 @@ void hw_int8_handler(void)
 {
 
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_int9_handler() function -------------------------------------
  * DESCRIPTION: Function handles INT9 interrupt
@@ -144,8 +127,6 @@ void hw_int9_handler(void)
 {
 
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_int10_handler() function ------------------------------------
  * DESCRIPTION: Function handles INT10 interrupt
@@ -157,8 +138,6 @@ void hw_int10_handler(void)
 {
 
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_int11_handler() function ------------------------------------
  * DESCRIPTION: Function handles INT11 interrupt
@@ -170,8 +149,6 @@ void hw_int11_handler(void)
 {
 
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_int12_handler() function ------------------------------------
  * DESCRIPTION: Function handles INT12 interrupt
@@ -181,9 +158,8 @@ void hw_int11_handler(void)
 -----------------------------------------------------------------------------*/
 void hw_int12_handler(void)
 {
+	
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_int13_handler() function ------------------------------------
  * DESCRIPTION: Function handles INT13 interrupt
@@ -195,8 +171,6 @@ void hw_int13_handler(void)
 {
 
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------------ hw_int14_handler() function ------------------------------
  * DESCRIPTION: Function handles INT14 interrupt
@@ -209,8 +183,6 @@ void hw_int14_handler(void)
 extern void rt_hw_systick_isr();
 	rt_hw_systick_isr();
 }
-//-----------------------------------------------------------------------------
-
 
 /*------------ hw_int15_handler() function ------------------------------------
  * DESCRIPTION: Function handles INT15 interrupt
diff --git a/libcpu/ti-dsp/c6x/interrupt.c b/libcpu/ti-dsp/c6x/interrupt.c
index 69205e0d38..d78de95abb 100644
--- a/libcpu/ti-dsp/c6x/interrupt.c
+++ b/libcpu/ti-dsp/c6x/interrupt.c
@@ -8,7 +8,6 @@
  * 2021-11-16     Dystopia     the first version
  */
 
-
 #include "c66xx.h"
 #include "interrupt.h"
 
diff --git a/libcpu/ti-dsp/c6x/inct.asm b/libcpu/ti-dsp/c6x/intexc.asm
index 4f866e4800..d3ed54dc3b 100644
--- a/libcpu/ti-dsp/c6x/inct.asm
+++ b/libcpu/ti-dsp/c6x/intexc.asm
@@ -9,7 +9,7 @@
 ;
 
 ;-----------------------------------------------------------
-; interrupt handler for C6678 DSP
+; interrupt and execption handler for C6678 DSP
 ;-----------------------------------------------------------
 
 ;-----------------------------------------------------------
@@ -20,7 +20,7 @@ SP	.set	B15
 ;
 ;-----------------------------------------------------------
 ;
-
+	.include "contextinc.asm"
 ;-----------------------------------------------------------
 ; 						global function
 ;-----------------------------------------------------------
@@ -65,121 +65,14 @@ SP	.set	B15
 ;-----------------------------------------------------------
 ;
 
-;
 ;-----------------------------------------------------------
-;
-
+; 						extern variable
 ;-----------------------------------------------------------
-; 						macro definition
+	.ref rt_system_stack_top
+;
 ;-----------------------------------------------------------
-SAVE_ALL	.macro  __rp, __tsr
-	STDW	.D2T2	SP:DP,*--SP[1]
-	SUBAW	.D2	SP,2,SP
-	ADD	.D1X	SP,-8,A15
- ||	STDW	.D2T1	A15:A14,*SP--[16]	; save A15:A14
-
-	STDW	.D2T2	B13:B12,*SP--[1]
- ||	STDW	.D1T1	A13:A12,*A15--[1]
- ||	MVC	.S2	__rp,B13
-
-	STDW	.D2T2	B11:B10,*SP--[1]
- ||	STDW	.D1T1	A11:A10,*A15--[1]
- ||	MVC	.S2	CSR,B12
-
-	STDW	.D2T2	B9:B8,*SP--[1]
- ||	STDW	.D1T1	A9:A8,*A15--[1]
- ||	MVC	.S2	RILC,B11
-	STDW	.D2T2	B7:B6,*SP--[1]
- ||	STDW	.D1T1	A7:A6,*A15--[1]
- ||	MVC	.S2	ILC,B10
-
-	STDW	.D2T2	B5:B4,*SP--[1]
- ||	STDW	.D1T1	A5:A4,*A15--[1]
-
-	STDW	.D2T2	B3:B2,*SP--[1]
- ||	STDW	.D1T1	A3:A2,*A15--[1]
- ||	MVC	.S2	__tsr,B5
-
-	STDW	.D2T2	B1:B0,*SP--[1]
- ||	STDW	.D1T1	A1:A0,*A15--[1]
- ||	MV	.S1X	B5,A5
-
-	STDW	.D2T2	B31:B30,*SP--[1]
- ||	STDW	.D1T1	A31:A30,*A15--[1]
- || MVKL	1,A4
-
-	STDW	.D2T2	B29:B28,*SP--[1]
- ||	STDW	.D1T1	A29:A28,*A15--[1]
-	STDW	.D2T2	B27:B26,*SP--[1]
- ||	STDW	.D1T1	A27:A26,*A15--[1]
-	STDW	.D2T2	B25:B24,*SP--[1]
- ||	STDW	.D1T1	A25:A24,*A15--[1]
-	STDW	.D2T2	B23:B22,*SP--[1]
- ||	STDW	.D1T1	A23:A22,*A15--[1]
-	STDW	.D2T2	B21:B20,*SP--[1]
- ||	STDW	.D1T1	A21:A20,*A15--[1]
-	STDW	.D2T2	B19:B18,*SP--[1]
- ||	STDW	.D1T1	A19:A18,*A15--[1]
-	STDW	.D2T2	B17:B16,*SP--[1]
- ||	STDW	.D1T1	A17:A16,*A15--[1]
-
-	STDW	.D2T2	B13:B12,*SP--[1]	; save PC and CSR
-
-	STDW	.D2T2	B11:B10,*SP--[1]	; save RILC and ILC
-	STDW	.D2T1	A5:A4,*SP--[1]		; save TSR and orig A4
-			.endm
-
-RESTORE_ALL	.macro  __rp, __tsr
-	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,__tsr
-	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,__rp
-
-	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
- 	LDDW	.D2T1	*++SP[1],A15:A14
-	B	.S2	__rp		; return from interruption
-	LDDW	.D2T2	*+SP[1],SP:DP
-	NOP		4
-			.endm
 ;-----------------------------------------------------------
 ; 				interrupt macro definition
 ;-----------------------------------------------------------
@@ -190,8 +83,8 @@ RT_INTERRUPT_ENTRY	.macro
 RT_CALL_INT .macro __isr
 	CALLP	__isr,B3
 	B	.S1	rt_interrupt_context_restore
-	NOP    	5
-					.endm
+	NOP	5
+			.endm
 ;-----------------------------------------------------------
 ; 				execption macro definition
 ;-----------------------------------------------------------
@@ -201,6 +94,8 @@ RT_EXECPTION_ENTRY	.macro
 
 RT_EXECPTION_EXIT	.macro
 	RESTORE_ALL NRP,NTSR
+	B	.S2	NRP				; return from interruption
+	NOP	5
 					.endm
 
 ;
diff --git a/libcpu/ti-dsp/c6x/stack.asm b/libcpu/ti-dsp/c6x/stack.asm
index 4415a9767c..6a523a1c95 100644
--- a/libcpu/ti-dsp/c6x/stack.asm
+++ b/libcpu/ti-dsp/c6x/stack.asm
@@ -13,7 +13,7 @@
 ;-----------------------------------------------------------
 
 ;-----------------------------------------------------------
-; 						macro definition
+;					macro definition
 ;-----------------------------------------------------------
 ADDRESS_MSK .set    0xFFFFFFF0
 
@@ -23,6 +23,10 @@ ADDRESS_MSK .set    0xFFFFFFF0
 
     .sect   ".text"
 ;
+;-----------------------------------------------------------
+;
+
+;
 ; rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_addr, void *texit)
 ; tentry  --> A4
 ; parameter --> B4
diff --git a/libcpu/ti-dsp/c6x/trap.c b/libcpu/ti-dsp/c6x/trap.c
index eb1863fa9c..f919467f2b 100644
--- a/libcpu/ti-dsp/c6x/trap.c
+++ b/libcpu/ti-dsp/c6x/trap.c
@@ -14,8 +14,18 @@
 #include <rthw.h>
 #include <rtthread.h>
 
+#include <rtdef.h>
+
+#define RT_SYS_STACK_SIZE	4096
+
+rt_uint8_t rt_system_stack[RT_SYS_STACK_SIZE];
+rt_uint8_t *rt_system_stack_top;
+
 void rt_trap_init(void)
 {
+	rt_system_stack_top = &rt_system_stack[RT_SYS_STACK_SIZE-1];
+	rt_system_stack_top  = (rt_uint8_t *)RT_ALIGN_DOWN((rt_uint32_t)rt_system_stack_top, 8);
+
 	ack_exception(EXCEPT_TYPE_NXF);
 	ack_exception(EXCEPT_TYPE_EXC);
 	ack_exception(EXCEPT_TYPE_IXF);
@@ -319,7 +329,7 @@ int rt_hw_process_exception(struct rt_hw_exp_stack_register *regs)
 	int ie_num = 9; /* default is unknown exception */
 
 	while ((type = get_except_type()) != 0) {
-		type_num = fls(type) - 1;
+		type_num = __fls(type) - 1;
 
 		switch(type_num) {
 		case EXCEPT_TYPE_NXF:					/* NMI exception   */
diff --git a/libcpu/ti-dsp/c6x/trap.h b/libcpu/ti-dsp/c6x/trap.h
index 5494fa900b..6d4312e243 100644
--- a/libcpu/ti-dsp/c6x/trap.h
+++ b/libcpu/ti-dsp/c6x/trap.h
@@ -67,8 +67,8 @@ struct rt_exception_info {
 #define BKPT_OPCODE		0x56454314       /* illegal opcode */
 #define INTC_MEXPMASK	__SYSREGA(0x018000e0, unsigned int)
 
-#define __ffs(a)   (_lmbd(1, _bitr(a)))
-#define fls(a)     (!(a) ? 0 : (32 - _lmbd(1, (a))))
+#define __ffs(a)	(_lmbd(1, _bitr(a)))
+#define __fls(a)	(!(a) ? 0 : (32 - _lmbd(1, (a))))
 
 void rt_trap_init(void);
 
diff --git a/libcpu/ti-dsp/c6x/vector.asm b/libcpu/ti-dsp/c6x/vector.asm
index 47eb8781f5..c942029d86 100644
--- a/libcpu/ti-dsp/c6x/vector.asm
+++ b/libcpu/ti-dsp/c6x/vector.asm
@@ -75,21 +75,21 @@ __name:
 	.align 32
 	.global vector
 vector:
-	VEC_RESET	  _c_int00
-	IRQVEC NMI,   _nmi_handler
-	IRQVEC AINT,  _bad_handler
-	IRQVEC MSGINT,_bad_handler
-	IRQVEC INT4,  _int4_handler
-	IRQVEC INT5,  _int5_handler
-	IRQVEC INT6,  _int6_handler
-	IRQVEC INT7,  _int7_handler
-	IRQVEC INT8,  _int8_handler
-	IRQVEC INT9,  _int9_handler
-	IRQVEC INT10, _int10_handler
-	IRQVEC INT11, _int11_handler
-	IRQVEC INT12, _int12_handler
-	IRQVEC INT13, _int13_handler
-	IRQVEC INT14, _int14_handler
-	IRQVEC INT15, _int15_handler
+	VEC_RESET		_c_int00
+	IRQVEC NMI,		_nmi_handler
+	IRQVEC AINT,	_bad_handler
+	IRQVEC MSGINT,	_bad_handler
+	IRQVEC INT4,	_int4_handler
+	IRQVEC INT5,	_int5_handler
+	IRQVEC INT6,	_int6_handler
+	IRQVEC INT7,	_int7_handler
+	IRQVEC INT8,	_int8_handler
+	IRQVEC INT9,	_int9_handler
+	IRQVEC INT10,	_int10_handler
+	IRQVEC INT11,	_int11_handler
+	IRQVEC INT12,	_int12_handler
+	IRQVEC INT13,	_int13_handler
+	IRQVEC INT14,	_int14_handler
+	IRQVEC INT15,	_int15_handler
 
 	.end