diff --git a/bsp/m16c62p/vectors.s34 b/bsp/m16c62p/vectors.s34
index f80ed655c44717c3728c9e82003992a96c15f342..1594485c0db10549ea584a9337623552a59a932c 100644
--- a/bsp/m16c62p/vectors.s34
+++ b/bsp/m16c62p/vectors.s34
@@ -23,6 +23,7 @@
 
     EXTERN  rt_hw_timer_handler
     EXTERN  rt_hw_uart0_receive_handler
+    EXTERN  os_context_switch
     
     PUBLIC  RelocatableVectTbl
 
@@ -30,7 +31,7 @@
 
 RelocatableVectTbl:
     ORG     0
-    DC32    0                           ; Vector  0: BRK
+    DC32    os_context_switch           ; Vector  0: BRK
     DC32    0                           ; Vector  1: Reserved
     DC32    0                           ; Vector  2: Reserved
     DC32    0                           ; Vector  3: Reserved
diff --git a/libcpu/m16c/context.asm b/libcpu/m16c/context.asm
index 6a1c1e1490b031a879f12ac218ae82b13ae583b1..f131b94eff91e5388bff47fb30105031c0882eef 100644
--- a/libcpu/m16c/context.asm
+++ b/libcpu/m16c/context.asm
@@ -10,15 +10,12 @@
  * Change Logs:
  * Date           Author       Notes
  * 2010-04-09     fify         the first version
+ * 2010-04-19     fify         rewrite rt_hw_interrupt_disable/enable fuction
  *
  * For       : Renesas M16C
  * Toolchain : IAR's EW for M16C v3.401
 */
 
-;********************************************************************************************************
-;                                           PUBLIC FUNCTIONS
-;********************************************************************************************************
-
     RSEG        CSTACK
 
     RSEG        ISTACK
@@ -40,34 +37,51 @@
     PUBLIC    rt_hw_context_switch_interrupt
     PUBLIC    rt_hw_timer_handler
     PUBLIC    rt_hw_uart0_receive_handler
+    PUBLIC    os_context_switch
 
 rt_hw_interrupt_disable
-    FCLR    I
+    STC      FLG, R0    ;fify 20100419
+    FCLR     I
     RTS
 
 rt_hw_interrupt_enable
-    FSET    I
+    LDC      R0, FLG    ;fify 20100419
     RTS
     
-;/*
-; * void rt_hw_context_switch_to(rt_uint32 to);
-; * r0 --> to
-; * this fucntion is used to perform the first thread switch
-; */
+    .EVEN
+os_context_switch:
+    PUSHM    R0,R1,R2,R3,A0,A1,SB,FB
+    CMP.W    #0,rt_thread_switch_interrput_flag
+    JEQ      exit
+    MOV.W    #0, rt_thread_switch_interrput_flag
+    MOV.W    rt_interrupt_from_thread, A0
+    STC      ISP, [A0]    
+    MOV.W    rt_interrupt_to_thread, A0
+    LDC      [A0], ISP
+exit
+    POPM     R0,R1,R2,R3,A0,A1,SB,FB             ; Restore registers from the new task's stack
+    REIT                                         ; Return from interrup    
+    
+/*
+ * void rt_hw_context_switch_to(rt_uint32 to);
+ * r0 --> to
+ * this fucntion is used to perform the first thread switch
+ */
 rt_hw_context_switch_to
-    MOV.W   R0, A0
-    LDC     [A0], ISP
-    POPM    R0,R1,R2,R3,A0,A1,SB,FB
-    REIT
-
-rt_hw_context_switch
-    PUSHM    R0,R1,R2,R3,A0,A1,SB,FB 
     MOV.W    R0, A0
-    STC      ISP, [A0]
-    MOV.W    R1, A0
     LDC      [A0], ISP
-    POPM     R0,R1,R2,R3,A0,A1,SB,FB         ; Restore all processor registers from the new task's stack
+    POPM     R0,R1,R2,R3,A0,A1,SB,FB
     REIT
+
+rt_hw_context_switch
+    CMP.W    #1,rt_thread_switch_interrput_flag
+    JEQ      jump1
+    MOV.W    #1,rt_thread_switch_interrput_flag
+    MOV.W    R0, rt_interrupt_from_thread
+jump1
+    MOV.W    R1, rt_interrupt_to_thread
+    INT      #0
+    RTS
   
 rt_hw_context_switch_interrupt
     CMP.W    #1,rt_thread_switch_interrput_flag
@@ -85,8 +99,8 @@ rt_hw_context_switch_interrupt_do
     
     MOV.W    rt_interrupt_to_thread, A0
     LDC      [A0], ISP
-    POPM     R0,R1,R2,R3,A0,A1,SB,FB         ; Restore all processor registers from the new task's stack
-    RTS                                      ; Normal return
+    POPM     R0,R1,R2,R3,A0,A1,SB,FB             ; Restore all processor registers from the new task's stack
+    REIT
     
     .EVEN
 rt_hw_timer_handler:
@@ -98,7 +112,7 @@ rt_hw_timer_handler:
     CMP.W    #1,rt_thread_switch_interrput_flag
     JEQ      rt_hw_context_switch_interrupt_do
     
-    POPM     R0,R1,R2,R3,A0,A1,SB,FB             ; Restore registers from the new task's stack
+    POPM     R0,R1,R2,R3,A0,A1,SB,FB             ; Restore current task's registers
     REIT                                         ; Return from interrup
 
     .EVEN
@@ -111,7 +125,7 @@ rt_hw_uart0_receive_handler:
     CMP.W    #1, rt_thread_switch_interrput_flag
     JEQ      rt_hw_context_switch_interrupt_do
     
-    POPM     R0,R1,R2,R3,A0,A1,SB,FB             ; Restore registers from the new task's stack
+    POPM     R0,R1,R2,R3,A0,A1,SB,FB             ; Restore current task's registers
     REIT                                         ; Return from interrup
 
     END