save texit address in to thread stack in m16c

git-svn-id: https://rt-thread.googlecode.com/svn/trunk@2308 bbd45198-f89e-11dd-88c7-29a3b14d5316

libcpu/m16c/m16c62p/cpuport.c

 /*
  * File      : cpuport.c
  * This file is part of RT-Thread RTOS
- * COPYRIGHT (C) 2009 - 2011, RT-Thread Development Team
+ * COPYRIGHT (C) 2009 - 2012, RT-Thread Development Team
  *
  * The license and distribution terms for this file may be
  * found in the file LICENSE in this distribution or at
@@ -10,6 +10,7 @@
  * Change Logs:
  * Date           Author       Notes
  * 2011-02-23     Bernard      the first version
+ * 2012-09-25     lgnq         save texit address in to thread stack
  */
 
 #include <rtthread.h>
@@ -27,9 +28,9 @@ rt_uint8_t rt_thread_switch_interrupt_flag;
 void rt_hw_interrupt_init(void)
 {
     /* init interrupt nest, and context in thread sp */
-    rt_interrupt_nest = 0;
-    rt_interrupt_from_thread = 0;
-    rt_interrupt_to_thread = 0;
+    rt_interrupt_nest               = 0;
+    rt_interrupt_from_thread        = 0;
+    rt_interrupt_to_thread          = 0;
     rt_thread_switch_interrupt_flag = 0;
 }
 
@@ -43,30 +44,35 @@ void rt_hw_interrupt_init(void)
  *
  * @return stack address
  */
-rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_addr, void *texit)
+rt_uint8_t *rt_hw_stack_init(void       *tentry,
+                             void       *parameter,
+                             rt_uint8_t *stack_addr,
+                             void       *texit)
 {
-    rt_uint16_t  *pstk16;
-    rt_uint16_t   flag;
+    rt_uint16_t *pstk16;
+    rt_uint16_t  flag;
 
-    flag       = 0x0040;
-    pstk16     = (rt_uint16_t *)stack_addr;
-    pstk16--;
+    flag      = 0x0040;
+    pstk16    = (rt_uint16_t *)stack_addr;
+
+    *pstk16-- = (rt_uint32_t)texit >> 16L;
+    *pstk16-- = (rt_uint32_t)texit & 0x0000FFFFL;
 
     /* Simulate ISR entry */
-    *pstk16--  = (flag&0x00FF)                                  /* ... The lowest byte of the FLAG register     */
-               | (((rt_uint32_t)tentry>>8)&0x00000F00)          /* ... The highest nibble of the PC register    */
-               | ((flag<<4)&0xF000);                            /* ... The highest nibble of the FLAG register  */
-    *pstk16--  = (((rt_uint32_t)tentry)&0x0000FFFF);            /* ... The lowest bytes of the PC register      */
-    
+    *pstk16-- = (flag&0x00FF) |                            /* The lowest byte of the FLAG register    */
+                (((rt_uint32_t)tentry>>8)&0x00000F00) |    /* The highest nibble of the PC register   */
+                ((flag<<4)&0xF000);                        /* The highest nibble of the FLAG register */
+    *pstk16-- = (((rt_uint32_t)tentry)&0x0000FFFF);        /* The lowest bytes of the PC register     */
+
     /* Save registers onto stack frame */
-    *pstk16--  = (rt_uint16_t)0xFBFB;                           /* ... FB register                              */
-    *pstk16--  = (rt_uint16_t)0x3B3B;                           /* ... SB register                              */
-    *pstk16--  = (rt_uint16_t)0xA1A1;                           /* ... A1 register                              */
-    *pstk16--  = (rt_uint16_t)0xA0A0;                           /* ... A0 register                              */
-    *pstk16--  = (rt_uint16_t)0x3333;                           /* ... R3 register                              */
-    *pstk16--  = (rt_uint32_t)parameter >> 16L;                 /* ... Pass argument in R2 register             */
-    *pstk16--  = (rt_uint32_t)parameter & 0x0000FFFFL;          /* ... Pass argument in R1 register             */
-    *pstk16    = (rt_uint16_t)0x0000;                           /* ... R0 register                              */
+    *pstk16-- = (rt_uint16_t)0xFBFB;                       /* FB register                             */
+    *pstk16-- = (rt_uint16_t)0x3B3B;                       /* SB register                             */
+    *pstk16-- = (rt_uint16_t)0xA1A1;                       /* A1 register                             */
+    *pstk16-- = (rt_uint16_t)0xA0A0;                       /* A0 register                             */
+    *pstk16-- = (rt_uint16_t)0x3333;                       /* R3 register                             */
+    *pstk16-- = (rt_uint32_t)parameter >> 16L;             /* Pass argument in R2 register            */
+    *pstk16-- = (rt_uint32_t)parameter & 0x0000FFFFL;      /* Pass argument in R1 register            */
+    *pstk16   = (rt_uint16_t)0x0000;                       /* R0 register                             */
 
     /* return task's current stack address */
     return (rt_uint8_t *)pstk16;
@@ -75,8 +81,8 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_ad
 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");    
+    rt_interrupt_to_thread   = to;
+    asm("INT #0");
 }
 
 void rt_hw_context_switch_interrupt(rt_uint32_t from, rt_uint32_t to)
@@ -84,27 +90,27 @@ void rt_hw_context_switch_interrupt(rt_uint32_t from, rt_uint32_t to)
     if (rt_thread_switch_interrupt_flag != 1)
     {
         rt_thread_switch_interrupt_flag = 1;
-        rt_interrupt_from_thread = from;        
+        rt_interrupt_from_thread        = from;
     }
-    rt_interrupt_to_thread = to;  
+    rt_interrupt_to_thread = to;
 }
 
 #if defined(__GNUC__)
 rt_base_t rt_hw_interrupt_disable(void)
 {
-	register rt_uint16_t temp;
-	
-	asm("STC  FLG, %0":"=r" (temp));
-	asm("FCLR I");
-	
-	return (rt_base_t)temp;
+    register rt_uint16_t temp;
+
+    asm("STC  FLG, %0":"=r" (temp));
+    asm("FCLR I");
+
+    return (rt_base_t)temp;
 }
 
 void rt_hw_interrupt_enable(rt_base_t level)
 {
-	register rt_uint16_t temp;
-	
-	temp = level & 0xffff;	
-	asm("LDC %0, FLG": :"r" (temp));	
+    register rt_uint16_t temp;
+
+    temp = level & 0xffff;
+    asm("LDC %0, FLG": :"r" (temp));
 }
 #endif
\ No newline at end of file