More context switching logic for m9s12

git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@3300 42af7a65-404d-4744-a932-0658087f49c3

arch/hc/include/hcs12/irq.h

@@ -147,8 +147,9 @@
 #  define REG_PCH            (REG_FIRST_HARDREG+17)
 #  define REG_PCL            (REG_FIRST_HARDREG+18)
 
+#define TOTALFRAME_SIZE      (REG_FIRST_HARDREG+17)
 #define INTFRAME_SIZE        9
-#define XCPTCONTEXT_REGS     (REG_FIRST_HARDREG+17)
+#define XCPTCONTEXT_REGS     TOTALFRAME_SIZE
 
 /************************************************************************************
  * Public Types

arch/hc/src/m9s12/Make.defs

@@ -41,5 +41,5 @@ CMN_CSRCS	= up_allocateheap.c up_createstack.c up_doirq.c up_idle.c up_initializ
 			  up_modifyreg32.c up_modifyreg8.c up_puts.c up_releasestack.c \
 			  up_udelay.c up_usestack.c
 
-CHIP_ASRCS	= m9s12_start.S m9s12_lowputc.S
+CHIP_ASRCS	= m9s12_start.S m9s12_lowputc.S m9s12_saveusercontext.S
 CHIP_CSRCS	= m9s12_assert.c m9s12_serial.c m9s12_irq.c

arch/hc/src/m9s12/m9s12_saveusercontext.S

@@ -0,0 +1,181 @@
+/**************************************************************************
+ * arch/arm/src/m9s12/m9s12_saveusercontext.S
+ *
+ *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ **************************************************************************/
+
+/**************************************************************************
+ * Included Files
+ **************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <arch/irq.h>
+
+#include "up_internal.h"
+#include "m9s12_internal.h"
+
+/**************************************************************************
+ * Private Definitions
+ **************************************************************************/
+
+/**************************************************************************
+ * Private Types
+ **************************************************************************/
+
+/**************************************************************************
+ * Private Function Prototypes
+ **************************************************************************/
+
+/**************************************************************************
+ * Global Variables
+ **************************************************************************/
+
+/**************************************************************************
+ * Private Variables
+ **************************************************************************/
+
+/**************************************************************************
+ * Private Functions
+ **************************************************************************/
+
+/**************************************************************************
+ * Public Functions
+ **************************************************************************/
+
+/**************************************************************************
+ * Name: up_saveusercontext
+ *
+ * Description:
+ *	 Create this state save strucure:
+ *   Low Address  [PPAGE]
+ *                [soft regisers]
+ *                XYH
+ *                XYL
+ *                ZH
+ *                ZL
+ *                TMPH
+ *                TMPL
+ *                FRAMEH
+ *                FRAMEL
+ *                SP    <-- SP after interrupt
+ *                CCR
+ *                B
+ *                A
+ *                XH
+ *                XL
+ *                YH
+ *                YL
+ *                PCH
+ *   High Address PCL
+ *
+ *   On entry:
+ *    D=Pointer to save save structure
+ *    TOS=return address
+ *
+ **************************************************************************/
+
+	.text
+	.globl	up_saveusercontext
+	.type	up_saveusercontext, function
+up_saveusercontext:
+	/* Exchange D with X.  Now X points to the save structure. */
+
+	xgdx
+
+	/* Save he PPAGE register */
+
+#ifndef CONFIG_HCS12_NONBANKED
+	movb	HCS12_MMC_PPAGE, 1, x+
+#endif
+
+	/* Save the soft registers */
+
+#if CONFIG_HCS12_MSOFTREGS > 2
+#  error "Need to save more registers"
+#endif
+#if CONFIG_HCS12_MSOFTREGS > 1
+	movw	_.d2, 2, x+
+#endif
+#if CONFIG_HCS12_MSOFTREGS > 0
+	movw	_.d1, 2, x+
+#endif
+
+	/* It is not necessary to save the value of _.tmp, _.z, or _.xy */
+
+	ldd		#0
+	std		2, x+			/* Save _.xy = 0 */
+	std		2, x+			/* Save _.z = 0 */
+	std		2, x+			/* Save _.tmp = 0 */
+
+	/* Save _.frame */
+
+	movw	_.frame, 2, x+
+
+	/* Save the value of the stack "before" this function was called */
+
+	tfr		sp, d			/* D = current SP */
+	addd	#(TOTALFRAME_SIZE-INTFRAME_SIZE)
+	std		2, x+			/* Save the value of SP on entry */
+
+	/* Save the CCR */
+
+	tpa						/* A = CCR */
+	staa	1, x+			/* Save CCR in the structure */
+
+	/* D (A:B) is the return value.  Save 1 as the new return value as it
+	 * will appear after a context switch back to the current thread.
+	 */
+
+	ldd		#1
+	std		2, x+			/* Save D = 1 */
+
+	/* X, Y do not need to be preserved.  Write zeros to these locations */
+
+	ldd		#0
+	std		2, x+			/* Save X = 0 */
+	std		2, x+			/* Save Y = 0 */
+
+	/* Fetch the 2-byte return address from the stack and save it at the
+	 * end of the state save area
+	 */
+
+	movw	0, sp, 2, x+	/* Save PCH and PCL */
+
+#if __INT__ == 32 /* 32-bit ABI */
+	ldx		#0
+#endif
+	clra
+	clrb
+	rts
+	.size	up_saveusercontext, . - up_saveusercontext
+	.end

arch/hc/src/m9s12/m9s12_vectors.S

@@ -51,6 +51,7 @@
 
 	.globl		__start
 	.globl		up_doirq
+	.globl		up_fullcontextrestore
 	.file		"m9s12_vectors.S"
 
 /************************************************************************************
@@ -204,9 +205,11 @@ handlers:
 	HANDLER villegal, HCS12_IRQ_VILLEGAL 		/* Any reserved vector */
 
 /************************************************************************************
- *	Common IRQ handling logic
+ * Name: vcommon
  *
  * Description:
+ *	 Common IRQ handling logic
+ *
  *   On entry in to vcommon: (1) The interrupt stack fram is in place, and (2) the
  *   IRQ number is in B.
  *
@@ -214,6 +217,7 @@ handlers:
  *   instruction interrupt, the stack frame created by hardware looks like:
  *
  *   Low Address        <-- SP after interrupt
+ *                CCR
  *                B
  *                A
  *                XH
@@ -237,6 +241,7 @@ handlers:
  *                FRAMEH
  *                FRAMEL
  *                SP    <-- SP after interrupt
+ *                CCR
  *                B
  *                A
  *                XH
@@ -302,10 +307,8 @@ vcommon:
 	/* Check if the return value in d is the same as regs parameter passed in the TOS */
 
 	cpd		.Lspsave
-	beq		.Lnoswitch
+	bne		up_fullcontextrestore
-#warning "Missing Logic"
 
-.Lnoswitch:
 	/* Restore registers and return */
  
 	/* Restore the PPAGE register */
@@ -335,6 +338,82 @@ vcommon:
 	rti
 	.size	handlers, .-handlers
 
+/************************************************************************************
+ * Name: up_fullcontextrestore
+ *
+ * Description:
+ *   Given a pointer to a register save block that was previously created by either
+ *   interrupt handler or by up_saveusercontext(), restore the context of the saved
+ *   thread, thereby completing a context switch.
+ *
+ *   Low Address  [PPAGE]
+ *                [soft regisers]
+ *                XYH
+ *                XYL
+ *                ZH
+ *                ZL
+ *                TMPH
+ *                TMPL
+ *                FRAMEH
+ *                FRAMEL
+ *                SP
+ *                CCR
+ *                B
+ *                A
+ *                XH
+ *                XL
+ *                YH
+ *                YL
+ *                PCH
+ *   High Address PCL
+ *
+ * On entry:
+ *   D = Address of the context switch save block
+ *
+ ************************************************************************************/
+
+up_fullcontextrestore:
+	/* Make sure that interrupts are dissabled */
+
+	orcc	#0x50
+
+	/* Exchange D with X.  Now X points to the save structure. */
+
+	xgdx
+
+	/* Recover PPAGE */
+
+#ifndef CONFIG_HCS12_NONBANKED
+	movb	1, x+, HCS12_MMC_PPAGE
+#endif
+
+	/* Recover _.xy, _.z, _.tmp, _.frame */
+
+	movw	2, x+, _.xy
+	movw	2, x+, _.z
+	movw	2, x+, _.tmp
+	movw	2, x+, _.frame
+
+	/* Recover SP "before" the interrupt occurred */
+
+	ldd		2, x+
+	tfr		d, sp
+
+	/* Now, create a new interrupt return frame */
+
+	ldab	#(INTFRAME_SIZE-1)	/* Offset to PCL */
+	abx							/* X now points to last byte */
+
+	/* Copy the interrupt frame onto the stack */
+
+	movw	2, -sp, 2, -x		/* Copy the PC */
+	movw	2, -sp, 2, -x		/* Copy Y */
+	movw	2, -sp, 2, -x		/* Copy X */
+	movw	2, -sp, 2, -x		/* Copy A:B */
+	movw	1, -sp, 1, -x		/* Copy CCR */
+	rti							/* And return from interrupt */
+	.size	up_fullcontextrestore, .-up_fullcontextrestore
+
 /************************************************************************************
  * .bss
  ************************************************************************************/