diff --git a/libcpu/m16c/m16c62p/context_iar.asm b/libcpu/m16c/m16c62p/context_iar.asm
index f326817ffb713060bd0d5a2d81fb153f04f72512..bd75341f4b5cdc6a2aa1c1080b15e1e6f5e7e37f 100644
--- a/libcpu/m16c/m16c62p/context_iar.asm
+++ b/libcpu/m16c/m16c62p/context_iar.asm
@@ -23,15 +23,12 @@
 
     RSEG    CODE(1)
 
-    EXTERN  rt_thread_switch_interrput_flag
     EXTERN  rt_interrupt_from_thread
     EXTERN  rt_interrupt_to_thread
 
     PUBLIC  rt_hw_interrupt_disable
     PUBLIC  rt_hw_interrupt_enable
     PUBLIC  rt_hw_context_switch_to
-    PUBLIC  rt_hw_context_switch
-    PUBLIC  rt_hw_context_switch_interrupt
     PUBLIC  os_context_switch
 
 rt_hw_interrupt_disable:
@@ -66,19 +63,4 @@ rt_hw_context_switch_to:
     POPM    R0,R1,R2,R3,A0,A1,SB,FB
     REIT
 
-rt_hw_context_switch:
-    MOV.W   R0, rt_interrupt_from_thread
-    MOV.W   R1, rt_interrupt_to_thread
-    INT     #0                                   ;software interrupt 0
-    RTS
-  
-rt_hw_context_switch_interrupt:
-    CMP.W   #1, rt_thread_switch_interrput_flag
-    JEQ     jump
-    MOV.W   #1, rt_thread_switch_interrput_flag
-    MOV.W   R0, rt_interrupt_from_thread
-jump
-    MOV.W   R1, rt_interrupt_to_thread
-    RTS
-
     END
diff --git a/libcpu/m16c/m16c62p/cpuport.c b/libcpu/m16c/m16c62p/cpuport.c
index bab2ab5a22d29c7f330fda9a1dfd5005fdfdf16a..58c1400a2524b501126a9b9a1c83e16220d3486c 100644
--- a/libcpu/m16c/m16c62p/cpuport.c
+++ b/libcpu/m16c/m16c62p/cpuport.c
@@ -19,7 +19,7 @@ extern volatile rt_uint8_t rt_interrupt_nest;
 /* switch flag on interrupt and thread pointer to save switch record */
 rt_uint32_t rt_interrupt_from_thread;
 rt_uint32_t rt_interrupt_to_thread;
-rt_uint32_t rt_thread_switch_interrput_flag;
+rt_uint8_t rt_thread_switch_interrput_flag;
 
 /**
  * This function will initialize hardware interrupt
@@ -71,3 +71,20 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_ad
     /* return task's current stack address */
     return (rt_uint8_t *)pstk16;
 }
+
+void rt_hw_context_switch(rt_uint32_t from, rt_uint32_t to)
+{
+    rt_interrupt_from_thread = from;
+    rt_interrupt_to_thread = to;
+    asm("INT #0");    
+}
+
+void rt_hw_context_switch_interrupt(rt_uint32_t from, rt_uint32_t to)
+{
+    if (rt_thread_switch_interrput_flag != 1)
+    {
+        rt_thread_switch_interrput_flag = 1;
+        rt_interrupt_from_thread = from;        
+    }
+    rt_interrupt_to_thread = to;  
+}
\ No newline at end of file