TencentOS tiny port to MSP430FR6972

arch/msp430/MSP430X/icc430/data_model.h

@@ -0,0 +1,37 @@
+
+#ifndef DATA_MODEL_H
+#define DATA_MODEL_H
+
+#if __DATA_MODEL__ == __DATA_MODEL_SMALL__
+	#define pushm_x pushm.w
+	#define popm_x  popm.w
+	#define push_x  push.w
+	#define pop_x   pop.w
+	#define mov_x   mov.w
+	#define cmp_x   cmp.w
+#endif
+
+#if __DATA_MODEL__ == __DATA_MODEL_MEDIUM__
+	#define pushm_x pushm.a
+	#define popm_x  popm.a
+	#define push_x  pushx.a
+	#define pop_x   popx.a
+	#define mov_x   mov.w
+	#define cmp_x   cmp.w
+#endif
+
+#if __DATA_MODEL__ == __DATA_MODEL_LARGE__
+	#define pushm_x pushm.a
+	#define popm_x  popm.a
+	#define push_x  pushx.a
+	#define pop_x   popx.a
+	#define mov_x   movx.a
+	#define cmp_x   cmpx.a
+#endif
+
+#ifndef pushm_x
+	#error The assembler options must define one of the following symbols: __DATA_MODEL_SMALL__, __DATA_MODEL_MEDIUM__, or __DATA_MODEL_LARGE__
+#endif
+
+#endif /* DATA_MODEL_H */
+

arch/msp430/MSP430X/icc430/port.h

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------
+ * Tencent is pleased to support the open source community by making TencentOS
+ * available.
+ *
+ * Copyright (C) 2019 THL A29 Limited, a Tencent company. All rights reserved.
+ * If you have downloaded a copy of the TencentOS binary from Tencent, please
+ * note that the TencentOS binary is licensed under the BSD 3-Clause License.
+ *
+ * If you have downloaded a copy of the TencentOS source code from Tencent,
+ * please note that TencentOS source code is licensed under the BSD 3-Clause
+ * License, except for the third-party components listed below which are
+ * subject to different license terms. Your integration of TencentOS into your
+ * own projects may require compliance with the BSD 3-Clause License, as well
+ * as the other licenses applicable to the third-party components included
+ * within TencentOS.
+ *---------------------------------------------------------------------------*/
+
+#ifndef _PORT_H_
+#define  _PORT_H_
+
+__PORT__ void       port_int_disable(void);
+
+__PORT__ void       port_int_enable(void);
+
+__PORT__ cpu_cpsr_t port_cpsr_save(void);
+
+__PORT__ void       port_cpsr_restore(cpu_cpsr_t cpsr);
+
+__PORT__ void       port_cpu_reset(void);
+
+__PORT__ void       port_sched_start(void) __NO_RETURN__;
+
+__PORT__ void       port_context_switch(void);
+
+__PORT__ void       port_irq_context_switch(void);
+
+__PORT__ void       port_systick_config(uint32_t cycle_per_tick);
+
+__PORT__ void       port_systick_priority_set(uint32_t prio);
+
+#if TOS_CFG_TICKLESS_EN > 0u
+
+__PORT__ void       port_systick_resume(void);
+
+__PORT__ void       port_systick_suspend(void);
+
+__PORT__ void       port_systick_reload(uint32_t cycle_per_tick);
+
+__PORT__ void       port_systick_pending_reset(void);
+
+__PORT__ k_time_t   port_systick_max_delay_millisecond(void);
+
+#endif
+
+#if TOS_CFG_PWR_MGR_EN > 0u
+
+__PORT__ void       port_sleep_mode_enter(void);
+
+__PORT__ void       port_stop_mode_enter(void);
+
+__PORT__ void       port_standby_mode_enter(void);
+
+#endif
+
+
+#if TOS_CFG_FAULT_BACKTRACE_EN > 0u
+__PORT__ void       HardFault_Handler(void);
+
+__PORT__ void       port_fault_diagnosis(void);
+#endif
+
+#endif /* _PORT_H_ */
+

arch/msp430/MSP430X/icc430/port_c.c

@@ -0,0 +1,116 @@
+#include "tos.h"
+
+__PORT__ void port_cpu_reset(void)
+{
+    WDTCTL = WDTPW | 0xff00; // write a wrong Watchdog timer password will cause a PUC(reset) 
+}
+
+__PORT__ void port_systick_config(uint32_t cycle_per_tick)
+{
+}
+
+__PORT__ void port_systick_priority_set(uint32_t prio)
+{
+}
+
+/* MSP430 */
+__PORT__ void port_setup_systick(void)
+{
+    const unsigned short aclk_frequency_hz = 32768;
+
+    /* Ensure the timer is stopped. */
+    TA0CTL = 0;
+
+    /* Run the timer from the ACLK. */
+    TA0CTL = TASSEL_1;
+
+    /* Clear everything to start with. */
+    TA0CTL |= TACLR;
+
+    /* Set the compare match value according to the tick rate we want. */
+    TA0CCR0 = aclk_frequency_hz / TOS_CFG_CPU_TICK_PER_SECOND;
+
+    /* Enable the interrupts. */
+    TA0CCTL0 = CCIE;
+
+    /* Start up clean. */
+    TA0CTL |= TACLR;
+
+    /* Up mode. */
+    TA0CTL |= MC_1;
+}
+
+/* The MSP430X port uses a callback function to configure its tick interrupt.
+ */
+#pragma vector=TIMER0_A0_VECTOR 
+__PORT__ __interrupt __raw void timer0_isr( void )
+{
+    extern void port_systick_isr( void );
+
+    __bic_SR_register_on_exit( SCG1 + SCG0 + OSCOFF + CPUOFF );
+    port_systick_isr();
+}
+
+
+#if TOS_CFG_TICKLESS_EN > 0u
+
+__PORT__ k_time_t port_systick_max_delay_millisecond(void)
+{
+
+}
+
+__PORT__ void port_systick_resume(void)
+{
+
+}
+
+__PORT__ void port_systick_suspend(void)
+{
+
+}
+
+__PORT__ void port_systick_reload(uint32_t cycle_per_tick)
+{
+
+}
+
+__PORT__ void port_systick_pending_reset(void)
+{
+
+}
+
+#endif
+
+#if TOS_CFG_PWR_MGR_EN > 0u
+
+__PORT__ void port_sleep_mode_enter(void)
+{
+
+}
+
+__PORT__ void port_stop_mode_enter(void)
+{
+
+}
+
+__PORT__ void port_standby_mode_enter(void)
+{
+
+}
+
+#endif
+
+#if TOS_CFG_FAULT_BACKTRACE_EN > 0u
+__PORT__ void port_fault_diagnosis(void)
+{
+    k_fault_log_writer("fault diagnosis does not supported in MSP430\n");
+}
+
+
+/*------------------ 430 IAR Compiler-------------------*/
+#if defined (__ICC430__) // __IAR_SYSTEMS_ICC__
+
+#endif 
+
+#endif /* TOS_CFG_FAULT_BACKTRACE_EN */
+

arch/msp430/MSP430X/icc430/port_config.h

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------
+ * Tencent is pleased to support the open source community by making TencentOS
+ * available.
+ *
+ * Copyright (C) 2019 THL A29 Limited, a Tencent company. All rights reserved.
+ * If you have downloaded a copy of the TencentOS binary from Tencent, please
+ * note that the TencentOS binary is licensed under the BSD 3-Clause License.
+ *
+ * If you have downloaded a copy of the TencentOS source code from Tencent,
+ * please note that TencentOS source code is licensed under the BSD 3-Clause
+ * License, except for the third-party components listed below which are
+ * subject to different license terms. Your integration of TencentOS into your
+ * own projects may require compliance with the BSD 3-Clause License, as well
+ * as the other licenses applicable to the third-party components included
+ * within TencentOS.
+ *---------------------------------------------------------------------------*/
+
+#ifndef _PORT_CONFIG_H_
+#define  _PORT_CONFIG_H_
+
+/* The stack type changes depending on the data model. */
+#if( __DATA_MODEL__ == __DATA_MODEL_SMALL__ )
+    #define TOS_CFG_CPU_ADDR_SIZE               CPU_WORD_SIZE_16
+    #define TOS_CFG_CPU_DATA_SIZE               CPU_WORD_SIZE_16 
+#else
+    #define TOS_CFG_CPU_ADDR_SIZE               CPU_WORD_SIZE_32
+    #define TOS_CFG_CPU_DATA_SIZE               CPU_WORD_SIZE_32 
+#endif
+   
+
+#define TOS_CFG_CPU_STK_GROWTH              CPU_STK_GROWTH_DESCENDING
+// #define TOS_CFG_CPU_HRTIMER_SIZE             CPU_WORD_SIZE_32
+#define TOS_CFG_CPU_HRTIMER_EN               0u
+#define TOS_CFG_CPU_LEAD_ZEROS_ASM_PRESENT  0u
+
+#endif /* _PORT_CONFIG_H_ */
+

arch/msp430/MSP430X/icc430/port_s.s43

@@ -0,0 +1,148 @@
+ #include "data_model.h"
+ 
+    ;EXPORT
+    PUBLIC  port_int_disable
+    PUBLIC  port_int_enable
+
+    PUBLIC  port_cpsr_save
+    PUBLIC  port_cpsr_restore
+
+    PUBLIC  port_sched_start
+    PUBLIC  port_context_switch
+    PUBLIC  port_irq_context_switch
+    PUBLIC  port_systick_isr
+      
+    ;IMPORT
+    EXTERN  k_curr_task
+    EXTERN  k_next_task
+    
+    EXTERN  port_setup_systick
+    EXTERN  systick_handler
+    EXTERN  irq_context_switch_flag
+    
+/*-----------------------------------------------------------*/    
+
+PORT_SAVE_CONTEXT macro
+	; Save the remaining registers.
+	pushm_x	#12, r15               
+	mov_x	&k_curr_task, r12      
+	mov_x	sp, 0( r12 )           
+	endm
+
+PORT_RESTORE_CONTEXT macro
+
+	mov_x	&k_curr_task, r12      
+	mov_x	@r12, sp               
+	popm_x	#12, r15              
+	nop
+	pop.w	sr                     
+	nop
+	reta
+	endm
+
+/*-----------------------------------------------------------*/
+;The RTOS systick ISR.
+
+
+    RSEG CODE
+    EVEN
+
+port_systick_isr:
+;	/* The sr is not saved in PORT_SAVE_CONTEXT() because port_context_switch() needs
+;	to save it manually before it gets modified (interrupts get disabled).
+;	Entering through this interrupt means the SR is already on the stack, but
+;	this keeps the stack frames identical. */
+    push.w sr
+    
+    PORT_SAVE_CONTEXT
+
+    calla #systick_handler              
+   
+    cmp.w #0x0,irq_context_switch_flag
+    mov.w #0x0, &irq_context_switch_flag
+    
+    jeq skip_context_switch
+    
+    ;k_next_task  -> k_curr_task
+    mov_x &k_next_task,&k_curr_task
+        
+skip_context_switch:
+
+    PORT_RESTORE_CONTEXT 
+    
+    reti
+
+/*-----------------------------------------------------------*/
+
+port_int_disable:
+    dint
+    nop
+    reta
+
+port_int_enable:
+    nop
+    eint
+    reta
+
+port_cpsr_save:
+    mov.w    SR,R12
+    dint
+    nop
+    reta
+
+port_cpsr_restore:
+    nop
+    mov.w   R12,SR
+    nop
+    reta
+
+
+port_context_switch:
+    
+    ;/* The sr needs saving before it is modified. */
+    push.w sr
+    
+    ;/* Now the SR is stacked interrupts can be disabled. */
+    dint
+    nop
+        
+    PORT_SAVE_CONTEXT
+   
+    ;k_next_task  -> k_curr_task
+    mov_x &k_next_task,&k_curr_task
+
+    PORT_RESTORE_CONTEXT 
+    
+    reti 
+
+port_irq_context_switch:
+
+    reti
+
+/*-----------------------------------------------------------*/
+;/*
+;* Start off the scheduler by initialising the RTOS tick timer, then restoring
+;* the context of the first task.
+;*/
+port_sched_start:
+
+/* Interrupts are turned off here, to ensure a tick does not occur
+    before or during the call to port_sched_start().  The stacks of
+    the created tasks contain a status word with interrupts switched on
+    so interrupts will automatically get re-enabled when the first task
+    starts to run. */
+    dint
+    nop
+
+    ;/* Setup the hardware to generate the tick.  Interrupts are disabled
+    ;when this function is called. */
+    calla #port_setup_systick
+
+    ;mov_x &k_next_task,&k_curr_task
+    
+    ;/* Restore the context of the first task that is going to run. */
+    PORT_RESTORE_CONTEXT
+
+/*-----------------------------------------------------------*/
+    END
+

arch/msp430/common/include/tos_cpu.h

@@ -0,0 +1,121 @@
+#ifndef _TOS_CPU_H_
+#define  _TOS_CPU_H_
+
+typedef struct cpu_context_st {
+    cpu_data_t      _R0; //PC
+    cpu_data_t      _R1; //SP
+    cpu_data_t      _R2; //SR
+    //cpu_data_t      _R3; 
+    cpu_data_t      _R4;
+    cpu_data_t      _R5;
+    cpu_data_t      _R6;
+    cpu_data_t      _R7;
+    cpu_data_t      _R8;
+    cpu_data_t      _R9;
+    cpu_data_t      _R10;
+    cpu_data_t      _R11;
+    cpu_data_t      _R12;
+    cpu_data_t      _R13;
+    cpu_data_t      _R14;
+    cpu_data_t      _R15;
+} cpu_context_t;
+
+__API__ uint32_t        tos_cpu_clz(uint32_t val);
+
+__API__ void            tos_cpu_int_disable(void);
+
+__API__ void            tos_cpu_int_enable(void);
+
+__API__ cpu_cpsr_t      tos_cpu_cpsr_save(void);
+
+__API__ void            tos_cpu_cpsr_restore(cpu_cpsr_t cpsr);
+
+#if (TOS_CFG_CPU_HRTIMER_EN > 0u)
+
+__API__ void            tos_cpu_hrtimer_init(void);
+
+__API__ cpu_hrtimer_t   tos_cpu_hrtimer_read(void);
+
+#endif
+
+__KERNEL__ void         cpu_init(void);
+
+__KERNEL__ void         cpu_reset(void);
+
+__KERNEL__ void         cpu_systick_init(k_cycle_t cycle_per_tick);
+
+__KERNEL__ void         cpu_sched_start(void);
+
+__KERNEL__ void         cpu_context_switch(void);
+
+__KERNEL__ void         cpu_irq_context_switch(void);
+
+#if TOS_CFG_TASK_STACK_DRAUGHT_DEPTH_DETACT_EN > 0u
+
+__KERNEL__ k_err_t cpu_task_stack_draught_depth(k_stack_t *stk_base, size_t stk_size, int *depth);
+
+#endif
+
+__KERNEL__ k_stack_t   *cpu_task_stk_init(void *entry,
+                                                          void *arg,
+                                                          void *exit,
+                                                          k_stack_t *stk_base,
+                                                          size_t stk_size);
+
+#if TOS_CFG_TICKLESS_EN > 0u
+
+__KERNEL__ void         cpu_systick_resume(void);
+
+__KERNEL__ void         cpu_systick_suspend(void);
+
+__KERNEL__ void         cpu_systick_reload_reset(void);
+
+__KERNEL__ void         cpu_systick_pending_reset(void);
+
+__KERNEL__ k_time_t     cpu_systick_max_delay_millisecond(void);
+
+__KERNEL__ void         cpu_systick_expires_set(k_time_t millisecond);
+
+__KERNEL__ void         cpu_systick_reset(void);
+
+#endif
+
+#if TOS_CFG_PWR_MGR_EN > 0u
+
+__KERNEL__ void cpu_sleep_mode_enter(void);
+
+__KERNEL__ void cpu_stop_mode_enter(void);
+
+__KERNEL__ void cpu_standby_mode_enter(void);
+
+#endif
+
+#if TOS_CFG_FAULT_BACKTRACE_EN > 0u
+
+#if defined (TOS_CFG_CPU_ARM_FPU_EN) && (TOS_CFG_CPU_ARM_FPU_EN == 1U)
+
+__KERNEL__ void         cpu_flush_fpu(void);
+
+#endif /* TOS_CFG_CPU_ARM_FPU_EN */
+
+__KERNEL__ void         cpu_fault_diagnosis(void);
+
+#endif
+
+/* Allocates CPU status register word. */
+#define TOS_CPU_CPSR_ALLOC()    cpu_cpsr_t cpu_cpsr = (cpu_cpsr_t)0u
+
+/* Save CPU status word & disable interrupts.*/
+#define TOS_CPU_INT_DISABLE() \
+    do { \
+        cpu_cpsr = tos_cpu_cpsr_save(); \
+    } while (0)
+
+/* Restore CPU status word.                     */
+#define  TOS_CPU_INT_ENABLE() \
+    do { \
+        tos_cpu_cpsr_restore(cpu_cpsr); \
+    } while (0)
+
+#endif /* _TOS_CPU_H_ */
+

arch/msp430/common/include/tos_cpu_def.h

@@ -0,0 +1,17 @@
+#ifndef _TOS_CPU_DEF_H_
+#define  _TOS_CPU_DEF_H_
+
+enum CPU_WORD_SIZE {
+    CPU_WORD_SIZE_08,
+    CPU_WORD_SIZE_16,
+    CPU_WORD_SIZE_32,
+    CPU_WORD_SIZE_64,
+};
+
+enum CPU_STK_GROWTH {
+    CPU_STK_GROWTH_ASCENDING,
+    CPU_STK_GROWTH_DESCENDING,
+};
+
+#endif /* _TOS_CPU_DEF_H_ */
+

arch/msp430/common/include/tos_cpu_types.h

@@ -0,0 +1,20 @@
+#ifndef _TOS_CPU_TYPES_H_
+#define  _TOS_CPU_TYPES_H_
+
+#if( __DATA_MODEL__ == __DATA_MODEL_SMALL__ )
+/* CPU data type based on data bus size.          */
+typedef uint16_t    cpu_data_t;
+/* CPU address type based on address bus size.          */
+typedef uint16_t    cpu_addr_t;
+#else
+/* CPU data type based on data bus size.          */
+typedef uint32_t    cpu_data_t;
+/* CPU address type based on address bus size.          */
+typedef uint32_t    cpu_addr_t;
+#endif
+
+//typedef cpu_addr_t  size_t;
+typedef cpu_addr_t  cpu_cpsr_t;
+
+#endif
+

arch/msp430/common/include/tos_fault.h

@@ -0,0 +1,10 @@
+#ifndef _TOS_FAULT_H_
+#define _TOS_FAULT_H_
+
+#if TOS_CFG_FAULT_BACKTRACE_EN > 0u
+
+#error "unsupport now"
+
+#endif
+
+#endif /* _TOS_FAULT_H_ */

arch/msp430/common/tos_cpu.c

@@ -0,0 +1,294 @@
+#include <tos.h>
+
+uint8_t irq_context_switch_flag = 0;
+
+__API__ uint32_t tos_cpu_clz(uint32_t val)
+{
+#if defined(TOS_CFG_CPU_LEAD_ZEROS_ASM_PRESENT) && (TOS_CFG_CPU_LEAD_ZEROS_ASM_PRESENT == 0u)
+    uint32_t nbr_lead_zeros = 0;
+
+    if (!(val & 0XFFFF0000)) {
+        val <<= 16;
+        nbr_lead_zeros += 16;
+    }
+
+    if (!(val & 0XFF000000)) {
+        val <<= 8;
+        nbr_lead_zeros += 8;
+    }
+
+    if (!(val & 0XF0000000)) {
+        val <<= 4;
+        nbr_lead_zeros += 4;
+    }
+
+    if (!(val & 0XC0000000)) {
+        val <<= 2;
+        nbr_lead_zeros += 2;
+    }
+
+    if (!(val & 0X80000000)) {
+        nbr_lead_zeros += 1;
+    }
+
+    if (!val) {
+        nbr_lead_zeros += 1;
+    }
+
+    return (nbr_lead_zeros);
+#else
+    return port_clz(val);
+#endif
+}
+
+__API__ void tos_cpu_int_disable(void)
+{
+    port_int_disable();
+}
+
+__API__ void tos_cpu_int_enable(void)
+{
+    port_int_enable();
+}
+
+__API__ cpu_cpsr_t tos_cpu_cpsr_save(void)
+{
+    return port_cpsr_save();
+}
+
+__API__ void tos_cpu_cpsr_restore(cpu_cpsr_t cpsr)
+{
+    port_cpsr_restore(cpsr);
+}
+
+__KERNEL__ void cpu_init(void)
+{
+
+}
+
+__KERNEL__ void cpu_reset(void)
+{
+    port_cpu_reset();
+}
+
+__KERNEL__ void cpu_sched_start(void)
+{
+    port_sched_start();
+}
+
+__KERNEL__ void cpu_context_switch(void)
+{
+    port_context_switch();
+}
+
+__KERNEL__ void cpu_irq_context_switch(void)
+{
+    irq_context_switch_flag = 1;//port_irq_context_switch();
+}
+
+__KERNEL__ void cpu_systick_init(k_cycle_t cycle_per_tick)
+{
+    port_systick_priority_set(TOS_CFG_CPU_SYSTICK_PRIO);
+    port_systick_config(cycle_per_tick);
+}
+
+#if TOS_CFG_TICKLESS_EN > 0u
+
+/**
+ * @brief Set value to systick reload value register
+ *
+ * @param cycles The value set to register
+ *
+ * @return None
+ */
+__STATIC_INLINE__ void cpu_systick_reload(k_cycle_t cycle_per_tick)
+{
+    port_systick_reload(cycle_per_tick);
+}
+
+/**
+ * @brief Enable systick interrupt
+ *
+ * @return None
+ */
+__KERNEL__ void cpu_systick_resume(void)
+{
+    port_systick_resume();
+}
+
+/**
+ * @brief Disable systick interrupt
+ *
+ * @return None
+ */
+__KERNEL__ void cpu_systick_suspend(void)
+{
+    port_systick_suspend();
+}
+
+__KERNEL__ k_time_t cpu_systick_max_delay_millisecond(void)
+{
+    return port_systick_max_delay_millisecond();
+}
+
+__KERNEL__ void cpu_systick_expires_set(k_time_t millisecond)
+{
+    k_cycle_t cycles;
+
+    cycles = (k_cycle_t)((uint64_t)millisecond * TOS_CFG_CPU_CLOCK / K_TIME_MILLISEC_PER_SEC); /* CLOCK means cycle per second */
+
+    cpu_systick_reload(cycles - 12); /* interrupt delay */
+}
+
+__KERNEL__ void cpu_systick_pending_reset(void)
+{
+    port_systick_pending_reset();
+}
+
+__KERNEL__ void cpu_systick_reset(void)
+{
+    cpu_systick_reload(k_cpu_cycle_per_tick);
+}
+
+#endif /* TOS_CFG_TICKLESS_EN */
+
+#if TOS_CFG_PWR_MGR_EN > 0u
+
+__KERNEL__ void cpu_sleep_mode_enter(void)
+{
+    __bis_SR_register( LPM4_bits + GIE );
+    __no_operation();
+}
+
+__KERNEL__ void cpu_stop_mode_enter(void)
+{
+
+}
+
+__KERNEL__ void cpu_standby_mode_enter(void)
+{
+
+}
+
+#endif /* TOS_CFG_PWR_MGR_EN */
+
+__KERNEL__ k_stack_t *cpu_task_stk_init(void *entry,
+                                              void *arg,
+                                              void *exit,
+                                              k_stack_t *stk_base,
+                                              size_t stk_size)
+{
+    cpu_data_t *sp;
+    uint16_t *pus_top_of_stack;
+    uint32_t *pul_top_of_stack;
+
+    #define PORT_BYTE_ALIGNMENT_MASK	( 0x0001 )
+    
+    /* The stack type changes depending on the data model. */
+
+    sp = (cpu_data_t *)&( stk_base[ stk_size - ( uint32_t ) 1 ] );
+    sp = ( cpu_data_t * ) ( ( ( cpu_data_t ) sp ) & ( ~( ( cpu_data_t ) PORT_BYTE_ALIGNMENT_MASK ) ) ); 
+    
+    /* cpu_data_t is either 16 bits or 32 bits depending on the data model.
+    Some stacked items do not change size depending on the data model so have
+    to be explicitly cast to the correct size so this function will work
+    whichever data model is being used. */
+
+    if( sizeof( cpu_data_t ) == sizeof( uint16_t ) )
+    {
+        /* Make room for a 20 bit value stored as a 32 bit value. */
+        pus_top_of_stack = ( uint16_t * ) sp;
+        pus_top_of_stack--;
+        pul_top_of_stack = ( uint32_t * ) pus_top_of_stack;
+    }
+    else
+    {
+        pul_top_of_stack = ( uint32_t * ) sp;
+    }
+
+    /* PC - Interrupt return pointer */
+    *pul_top_of_stack = ( uint32_t ) entry;           
+        
+    pus_top_of_stack = ( uint16_t * ) pul_top_of_stack;
+    pus_top_of_stack--;
+    
+    /* R2 - SR.GIE - bit8,golbal interrupt enable */
+    *pus_top_of_stack = 0x08;
+    /* SR size is 16-bits */                       
+    pus_top_of_stack -= ( sizeof( cpu_data_t ) / 2 );
+    
+
+    /* From here on the size of stacked items depends on the memory model. */
+    sp = ( cpu_data_t * ) pus_top_of_stack;
+    
+#if 0 // enable for debug
+    *sp = ( cpu_data_t ) 0xffff;
+    sp--;
+    *sp = ( cpu_data_t ) 0xeeee;
+    sp--;
+    *sp = ( cpu_data_t ) 0xdddd;
+    sp--;
+    *sp = ( cpu_data_t ) arg;
+    sp--;
+    *sp = ( cpu_data_t ) 0xbbbb;
+    sp--;
+    *sp = ( cpu_data_t ) 0xaaaa;
+    sp--;
+    *sp = ( cpu_data_t ) 0x9999;
+    sp--;
+    *sp = ( cpu_data_t ) 0x8888;
+    sp--;	
+    *sp = ( cpu_data_t ) 0x5555;
+    sp--;
+    *sp = ( cpu_data_t ) 0x6666;
+    sp--;
+    *sp = ( cpu_data_t ) 0x5555;
+    sp--;
+    *sp = ( cpu_data_t ) 0x4444;
+#else
+    sp -= 3;
+    *sp = ( cpu_data_t ) arg;
+    sp -= 8;// R11-R4
+#endif   
+        
+    return (k_stack_t *)sp;
+}
+
+#if TOS_CFG_TASK_STACK_DRAUGHT_DEPTH_DETACT_EN > 0u
+
+__KERNEL__ k_err_t cpu_task_stack_draught_depth(k_stack_t *stk_base, size_t stk_size, int *depth)
+{
+    uint8_t *slot;
+    uint8_t *sp, *bp;
+    int the_depth = 0;
+
+    bp = (uint8_t *)&stk_base[0];
+
+    sp = &stk_base[stk_size];
+    sp = (uint8_t *)((cpu_addr_t)sp & 0xFFFFFFF8);
+
+    for (slot = sp - 1; slot >= bp; --slot) {
+        if (*slot != 0xCC) {
+            the_depth = sp - slot;
+        }
+    }
+
+    *depth = the_depth;
+    if (the_depth == stk_size) {
+        return K_ERR_TASK_STK_OVERFLOW;
+    }
+
+    return K_ERR_NONE;
+}
+
+#endif
+
+#if TOS_CFG_FAULT_BACKTRACE_EN > 0u
+
+__KERNEL__ void cpu_fault_diagnosis(void)
+{
+    port_fault_diagnosis();
+}
+
+#endif /* TOS_CFG_FAULT_BACKTRACE_EN */
+