More m9s12x stuff

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

7 files changed, 568 insertions, 32 deletions

diff --git a/nuttx/arch/hc/include/hcs12/irq.h b/nuttx/arch/hc/include/hcs12/irq.h
index 3149c543a..fb3c767e2 100755
--- a/nuttx/arch/hc/include/hcs12/irq.h
+++ b/nuttx/arch/hc/include/hcs12/irq.h
@@ -62,7 +62,8 @@
  *                TMPH
  *                TMPL
  *                FRAMEH
- *                FRAMEL <-- SP after interrupt
+ *                FRAMEL
+ *                SP    <-- SP after interrupt
  *                CCR
  *                B
  *                A
@@ -76,6 +77,7 @@
  ************************************************************************************/
 
 /* Byte offsets */
+/* PPAGE register (only in banked mode) */
 
 #ifndef CONFIG_HCS12_NONBANKED
 #  define REG_PPAGE          0
@@ -84,6 +86,8 @@
 #  define REG_FIRST_SOFTREG  0
 #endif
 
+/* Soft registers (as configured) */
+
 #if CONFIG_HCS12_MSOFTREGS > 2
 #  error "Need to save more registers"
 #elif CONFIG_HCS12_MSOFTREGS == 2
@@ -108,20 +112,42 @@
 #  define REG_FRAMEH         (REG_FIRST_HARDREG+6)
 #  define REG_FRAMEL         (REG_FIRST_HARDREG+7)
 
-#define REG_CCR              (REG_FIRST_HARDREG+8)
-#define REG_BA               (REG_FIRST_HARDREG+9)
-#  define REG_B              (REG_FIRST_HARDREG+9)
-#  define REG_A              (REG_FIRST_HARDREG+10)
-#define REG_X                (REG_FIRST_HARDREG+11)
-#  define REG_XH             (REG_FIRST_HARDREG+11)
-#  define REG_XL             (REG_FIRST_HARDREG+12)
-#define REG_Y                (REG_FIRST_HARDREG+13)
-#  define REG_YH             (REG_FIRST_HARDREG+13)
-#  define REG_YL             (REG_FIRST_HARDREG+14)
-#define REG_PC               (REG_FIRST_HARDREG+15)
-#  define REG_PCH            (REG_FIRST_HARDREG+15)
-#  define REG_PCL            (REG_FIRST_HARDREG+16)
+/* Stack pointer before the interrupt */
+
+#define REG_SP               (REG_FIRST_HARDREG+8)
+#  define REG_SPH            (REG_FIRST_HARDREG+8)
+#  define REG_SPL            (REG_FIRST_HARDREG+9)
+
+/* On entry into an I- or X-interrupt, into an SWI, or into an undefined instruction
+ * interrupt, the stack frame created by hardware looks like:
+ *
+ * Low Address       <-- SP after interrupt
+ *              CCR
+ *              B
+ *              A
+ *              XH
+ *              XL
+ *              YH
+ *              YL
+ *              PCH
+ * High Address PCL  <-- SP before interrupt
+ */
 
+#define REG_CCR              (REG_FIRST_HARDREG+10)
+#define REG_BA               (REG_FIRST_HARDREG+11)
+#  define REG_B              (REG_FIRST_HARDREG+11)
+#  define REG_A              (REG_FIRST_HARDREG+12)
+#define REG_X                (REG_FIRST_HARDREG+13)
+#  define REG_XH             (REG_FIRST_HARDREG+13)
+#  define REG_XL             (REG_FIRST_HARDREG+14)
+#define REG_Y                (REG_FIRST_HARDREG+15)
+#  define REG_YH             (REG_FIRST_HARDREG+15)
+#  define REG_YL             (REG_FIRST_HARDREG+16)
+#define REG_PC               (REG_FIRST_HARDREG+17)
+#  define REG_PCH            (REG_FIRST_HARDREG+17)
+#  define REG_PCL            (REG_FIRST_HARDREG+18)
+
+#define INTFRAME_SIZE        9
 #define XCPTCONTEXT_REGS     (REG_FIRST_HARDREG+17)
 
 /************************************************************************************
diff --git a/nuttx/arch/hc/src/common/up_doirq.c b/nuttx/arch/hc/src/common/up_doirq.c
new file mode 100644
index 000000000..d3de013fa
--- /dev/null
+++ b/nuttx/arch/hc/src/common/up_doirq.c
@@ -0,0 +1,118 @@
+/****************************************************************************
+ * arch/hc/src/common/up_doirq.c
+ *
+ *   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 <stdint.h>
+#include <assert.h>
+
+#include <nuttx/irq.h>
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+
+#include "up_arch.h"
+#include "os_internal.h"
+#include "up_internal.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+uint8_t *up_doirq(int irq, uint8_t *regs)
+{
+  up_ledon(LED_INIRQ);
+#ifdef CONFIG_SUPPRESS_INTERRUPTS
+  PANIC(OSERR_ERREXCEPTION);
+#else
+  /* Nested interrupts are not supported in this implementation.  If you want
+   * implemented nested interrupts, you would have to (1) change the way that
+   * current regs is handled and (2) the design associated with
+   * CONFIG_ARCH_INTERRUPTSTACK.
+   */
+
+  /* Current regs non-zero indicates that we are processing an interrupt;
+   * current_regs is also used to manage interrupt level context switches.
+   */
+
+  DEBUGASSERT(current_regs == NULL);
+  current_regs = regs;
+
+  /* Mask and acknowledge the interrupt */
+
+  up_maskack_irq(irq);
+
+  /* Deliver the IRQ */
+
+  irq_dispatch(irq, regs);
+
+  /* If a context switch occurred while processing the interrupt then
+   * current_regs may have change value.  If we return any value different
+   * from the input regs, then the lower level will know that a context
+   * switch occurred during interrupt processing.
+   */
+
+  regs = current_regs;
+
+  /* Indicate that we are no long in an interrupt handler */
+
+  current_regs = NULL;
+
+  /* Unmask the last interrupt (global interrupts are still disabled) */
+
+  up_enable_irq(irq);
+#endif
+  up_ledoff(LED_INIRQ);
+  return regs;
+}
diff --git a/nuttx/arch/hc/src/common/up_internal.h b/nuttx/arch/hc/src/common/up_internal.h
index 1e79e743c..b32087312 100755
--- a/nuttx/arch/hc/src/common/up_internal.h
+++ b/nuttx/arch/hc/src/common/up_internal.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  * arch/hc/src/common/up_internal.h
  *
- *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
  *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -101,7 +101,7 @@ typedef void (*up_vector_t)(void);
  * structure.  If is non-NULL only during interrupt processing.
  */
 
-extern uint16_t *current_regs;
+extern uint8_t *current_regs;
 
 /* This is the beginning of heap as provided from processor-specific logic.
  * This is the first address in RAM after the loaded program+bss+idle stack.
@@ -139,7 +139,7 @@ extern void up_switchcontext(uint32_t *saveregs, uint32_t *restoreregs);
 
 /* Interrupt handling */
 
-extern uint16_t *up_doirq(int irq, uint32_t *regs);
+extern uint8_t *up_doirq(int irq, uint8_t *regs);
 extern void up_maskack_irq(int irq);
 
 /* Signal handling */
diff --git a/nuttx/arch/hc/src/m9s12/Make.defs b/nuttx/arch/hc/src/m9s12/Make.defs
index 3f5a7ddd6..37288b2dc 100755
--- a/nuttx/arch/hc/src/m9s12/Make.defs
+++ b/nuttx/arch/hc/src/m9s12/Make.defs
@@ -1,7 +1,7 @@
 ############################################################################
 # arch/arm/src/m9s12/Make.defs
 #
-#   Copyright (C) 2009,2011 Gregory Nutt. All rights reserved.
+#   Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
 #   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
 #
 # Redistribution and use in source and binary forms, with or without
@@ -36,10 +36,10 @@
 HEAD_ASRC	= m9s12_vectors.S
 
 CMN_ASRCS	= 
-CMN_CSRCS	= up_allocateheap.c up_createstack.c up_idle.c up_initialize.c \
+CMN_CSRCS	= up_allocateheap.c up_createstack.c up_doirq.c up_idle.c up_initialize.c \
 			  up_interruptcontext.c up_mdelay.c up_modifyreg16.c \
 			  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_CSRCS	= m9s12_serial.c m9s12_irq.c
+CHIP_CSRCS	= m9s12_assert.c m9s12_serial.c m9s12_irq.c
diff --git a/nuttx/arch/hc/src/m9s12/m9s12_assert.c b/nuttx/arch/hc/src/m9s12/m9s12_assert.c
new file mode 100644
index 000000000..7b167d183
--- /dev/null
+++ b/nuttx/arch/hc/src/m9s12/m9s12_assert.c
@@ -0,0 +1,337 @@
+/****************************************************************************
+ * arch/hc/src/m9s12/m9s12_assert.c
+ *
+ *   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 <stdint.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <debug.h>
+
+#include <nuttx/irq.h>
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+
+#include "up_arch.h"
+#include "os_internal.h"
+#include "up_internal.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Output debug info if stack dump is selected -- even if 
+ * debug is not selected.
+ */
+
+#ifdef CONFIG_ARCH_STACKDUMP
+# undef  lldbg
+# define lldbg lib_lowprintf
+#endif
+
+/* The following is just intended to keep some ugliness out of the mainline
+ * code.  We are going to print the task name if:
+ *
+ *  CONFIG_TASK_NAME_SIZE > 0 &&    <-- The task has a name
+ *  (defined(CONFIG_DEBUG)    ||    <-- And the debug is enabled (lldbg used)
+ *   defined(CONFIG_ARCH_STACKDUMP) <-- Or lib_lowprintf() is used
+ */
+
+#undef CONFIG_PRINT_TASKNAME
+#if CONFIG_TASK_NAME_SIZE > 0 && (defined(CONFIG_DEBUG) || defined(CONFIG_ARCH_STACKDUMP))
+#  define CONFIG_PRINT_TASKNAME 1
+#endif
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: up_getsp
+ ****************************************************************************/
+
+/* I don't know if the builtin to get SP is enabled */
+
+static inline uint16_t up_getsp(void)
+{
+  uint16_t spval;
+  __asm__
+  (
+    "\tsts spval\n"
+	: "=A"(spval) :
+  );
+  return spval;
+}
+
+/****************************************************************************
+ * Name: up_stackdump
+ ****************************************************************************/
+
+#ifdef CONFIG_ARCH_STACKDUMP
+static void up_stackdump(uint16_t sp, uint16_t stack_base)
+{
+  uint16_t stack;
+
+  for (stack = sp; stack < stack_base; stack += 16)
+    {
+      uint8_t *ptr = (uint8_t*)stack;
+      lldbg("%04x: %02x %02x %02x %02x %02x %02x %02x %02x"
+            "   %02x %02x %02x %02x %02x %02x %02x %02x\n",
+             stack, ptr[0], ptr[1], ptr[2], ptr[3], ptr[4], ptr[5], ptr[6], ptr[7],
+             ptr[8], ptr[9], ptr[10], ptr[11], ptr[12], ptr[13], ptr[14], ptr[15]);
+    }
+}
+#else
+# define up_stackdump()
+#endif
+
+/****************************************************************************
+ * Name: up_registerdump
+ ****************************************************************************/
+
+#ifdef CONFIG_ARCH_STACKDUMP
+static inline void up_registerdump(void)
+{
+  /* Are user registers available from interrupt processing? */
+
+  if (current_regs)
+    {
+      lldbg("A:%02x B:%02x X:%02x%02x Y:%02x%02x PC:%02x%02x CCR:%02x\n",
+             current_regs[REG_A], current_regs[REG_B], current_regs[REG_XH],
+             current_regs[REG_XL], current_regs[REG_YH], current_regs[REG_YL],
+             current_regs[REG_PCH], current_regs[REG_PCL], current_regs[REG_CCR]);
+      lldbg("SP:%02x%02x FRAME:%02x%02x TMP:%02x%02x Z:%02x%02x XY:%02x\n",
+             current_regs[REG_SPH], current_regs[REG_SPL],
+             current_regs[REG_FRAMEH], current_regs[REG_FRAMEL],
+             current_regs[REG_TMPL], current_regs[REG_TMPH], current_regs[REG_ZL],
+             current_regs[REG_ZH], current_regs[REG_XY], current_regs[REG_XY+1]);
+
+#if CONFIG_HCS12_MSOFTREGS > 2
+#  error "Need to save more registers"
+#elif CONFIG_HCS12_MSOFTREGS == 2
+      lldbg("SOFTREGS: %02x%02x :%02x%02x\n",
+            current_regs[REG_SOFTREG1], current_regs[REG_SOFTREG1+1],
+            current_regs[REG_SOFTREG2], current_regs[REG_SOFTREG2+1]);
+#elif CONFIG_HCS12_MSOFTREGS == 1
+      lldbg("SOFTREGS: %02x%02x\n", current_regs[REG_SOFTREG1],
+            current_regs[REG_SOFTREG1+1]);
+#endif
+
+#ifndef CONFIG_HCS12_NONBANKED
+      lldbg("PPAGE: %02x\n", current_regs[REG_PPAGE],);
+#endif
+    }
+}
+#else
+# define up_registerdump()
+#endif
+
+/****************************************************************************
+ * Name: up_dumpstate
+ ****************************************************************************/
+
+#ifdef CONFIG_ARCH_STACKDUMP
+static void up_dumpstate(void)
+{
+  _TCB    *rtcb = (_TCB*)g_readytorun.head;
+  uint16_t sp   = up_getsp();
+  uint16_t ustackbase;
+  uint16_t ustacksize;
+#if CONFIG_ARCH_INTERRUPTSTACK > 3
+  uint16_t istackbase;
+  uint16_t istacksize;
+#endif
+
+  /* Get the limits on the user stack memory */
+
+  if (rtcb->pid == 0)
+    {
+      ustackbase = g_heapbase - 4;
+      ustacksize = CONFIG_IDLETHREAD_STACKSIZE;
+    }
+  else
+    {
+      ustackbase = (uint16_t)rtcb->adj_stack_ptr;
+      ustacksize = (uint16_t)rtcb->adj_stack_size;
+    }
+
+  /* Get the limits on the interrupt stack memory */
+
+#if CONFIG_ARCH_INTERRUPTSTACK > 3
+  istackbase = (uint16_t)&g_userstack;
+  istacksize = (CONFIG_ARCH_INTERRUPTSTACK & ~3) - 4;
+
+  /* Show interrupt stack info */
+
+  lldbg("sp:     %04x\n", sp);
+  lldbg("IRQ stack:\n");
+  lldbg("  base: %04x\n", istackbase);
+  lldbg("  size: %04x\n", istacksize);
+
+  /* Does the current stack pointer lie within the interrupt
+   * stack?
+   */
+
+  if (sp <= istackbase && sp > istackbase - istacksize)
+    {
+      /* Yes.. dump the interrupt stack */
+
+      up_stackdump(sp, istackbase);
+
+      /* Extract the user stack pointer which should lie
+       * at the base of the interrupt stack.
+       */
+
+      sp = g_userstack;
+      lldbg("sp:     %04x\n", sp);
+    }
+
+  /* Show user stack info */
+
+  lldbg("User stack:\n");
+  lldbg("  base: %04x\n", ustackbase);
+  lldbg("  size: %04x\n", ustacksize);
+#else
+  lldbg("sp:         %04x\n", sp);
+  lldbg("stack base: %04x\n", ustackbase);
+  lldbg("stack size: %04x\n", ustacksize);
+#endif
+
+  /* Dump the user stack if the stack pointer lies within the allocated user
+   * stack memory.
+   */
+
+  if (sp > ustackbase || sp <= ustackbase - ustacksize)
+    {
+#if !defined(CONFIG_ARCH_INTERRUPTSTACK) || CONFIG_ARCH_INTERRUPTSTACK < 4
+      lldbg("ERROR: Stack pointer is not within allocated stack\n");
+#endif
+    }
+  else
+    {
+      up_stackdump(sp, ustackbase);
+    }
+
+  /* Then dump the registers (if available) */
+
+  up_registerdump();
+}
+#else
+# define up_dumpstate()
+#endif
+
+/****************************************************************************
+ * Name: _up_assert
+ ****************************************************************************/
+
+static void _up_assert(int errorcode) /* __attribute__ ((noreturn)) */
+{
+  /* Are we in an interrupt handler or the idle task? */
+
+  if (current_regs || ((_TCB*)g_readytorun.head)->pid == 0)
+    {
+       (void)irqsave();
+        for(;;)
+          {
+#ifdef CONFIG_ARCH_LEDS
+            up_ledon(LED_PANIC);
+            up_mdelay(250);
+            up_ledoff(LED_PANIC);
+            up_mdelay(250);
+#endif
+          }
+    }
+  else
+    {
+      exit(errorcode);
+    }
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: up_assert
+ ****************************************************************************/
+
+void up_assert(const uint8_t *filename, int lineno)
+{
+#ifdef CONFIG_PRINT_TASKNAME
+  _TCB *rtcb = (_TCB*)g_readytorun.head;
+#endif
+
+  up_ledon(LED_ASSERTION);
+#ifdef CONFIG_PRINT_TASKNAME
+  lldbg("Assertion failed at file:%s line: %d task: %s\n",
+        filename, lineno, rtcb->name);
+#else
+  lldbg("Assertion failed at file:%s line: %d\n",
+        filename, lineno);
+#endif
+  up_dumpstate();
+  _up_assert(EXIT_FAILURE);
+}
+
+/****************************************************************************
+ * Name: up_assert_code
+ ****************************************************************************/
+
+void up_assert_code(const uint8_t *filename, int lineno, int errorcode)
+{
+#ifdef CONFIG_PRINT_TASKNAME
+  _TCB *rtcb = (_TCB*)g_readytorun.head;
+#endif
+
+  up_ledon(LED_ASSERTION);
+
+#ifdef CONFIG_PRINT_TASKNAME
+  lldbg("Assertion failed at file:%s line: %d task: %s error code: %d\n",
+        filename, lineno, rtcb->name, errorcode);
+#else
+  lldbg("Assertion failed at file:%s line: %d error code: %d\n",
+        filename, lineno, errorcode);
+#endif
+  up_dumpstate();
+  _up_assert(errorcode);
+}
diff --git a/nuttx/arch/hc/src/m9s12/m9s12_irq.c b/nuttx/arch/hc/src/m9s12/m9s12_irq.c
index 73c5ef801..20a86e1ef 100755
--- a/nuttx/arch/hc/src/m9s12/m9s12_irq.c
+++ b/nuttx/arch/hc/src/m9s12/m9s12_irq.c
@@ -60,7 +60,7 @@
  * Public Data
  ****************************************************************************/
 
-uint16_t *current_regs;
+uint8_t *current_regs;
 
 /****************************************************************************
  * Private Data
diff --git a/nuttx/arch/hc/src/m9s12/m9s12_vectors.S b/nuttx/arch/hc/src/m9s12/m9s12_vectors.S
index f332c49a1..e69a47512 100755
--- a/nuttx/arch/hc/src/m9s12/m9s12_vectors.S
+++ b/nuttx/arch/hc/src/m9s12/m9s12_vectors.S
@@ -50,6 +50,7 @@
  ************************************************************************************/
 
 	.globl		__start
+	.globl		up_doirq
 	.file		"m9s12_vectors.S"
 
 /************************************************************************************
@@ -73,7 +74,8 @@
  
 	.macro	HANDLER, label, irqno
 \label:
-#warning "Missing Logic"
+	ldab	#\irqno							/* Load B=IRQ number */
+	bra		vcommon							/* Jump to common logic */
 	.endm
 
 /************************************************************************************
@@ -205,9 +207,24 @@ handlers:
  *	Common IRQ handling logic
  *
  * Description:
+ *   On entry in to vcommon: (1) The interrupt stack fram is in place, and (2) the
+ *   IRQ number is in B.
+ *
  *   On entry into an I- or X-interrupt, into an SWI, or into an undefined
  *   instruction interrupt, the stack frame created by hardware looks like:
  *
+ *   Low Address        <-- SP after interrupt
+ *                B
+ *                A
+ *                XH
+ *                XL
+ *                YH
+ *                YL
+ *                PCH
+ *   High Address PCL   <-- SP before interrupt
+ *
+ *   This function will create the following stack frame and will call up_doirq():
+ *
  *   Low Address        <-- SP after state save
  *                [PPAGE]
  *                [soft regisers]
@@ -218,8 +235,8 @@ handlers:
  *                TMPH
  *                TMPL
  *                FRAMEH
- *                FRAMEL <-- SP after interrupt
- *                CCR
+ *                FRAMEL
+ *                SP    <-- SP after interrupt
  *                B
  *                A
  *                XH
@@ -227,11 +244,22 @@ handlers:
  *                YH
  *                YL
  *                PCH
- *   High Address PCL    <-- SP before interrupt
+ *   High Address PCL   <-- SP before interrupt
  *
  ************************************************************************************/
 
 vcommon:
+	/* Save the IRQ number currently in B */
+
+	stab	.Lbsave
+
+	/* Save the SP at the time of the interrupt */
+
+	tfr		sp, d
+	addd	#INTFRAME_SIZE
+	staa	1, -sp
+	stab	1, -sp
+
 	/* Save the rest of the frame */
 
 	movw	_.frame, 2, -sp
@@ -257,9 +285,27 @@ vcommon:
 	movb	HCS12_MMC_PPAGE, 1, -sp
 #endif
 
-    /* Handle the IRQ */
+    /* Handle the IRQ:  AB=irqno, TOS=register save structure.  Return value in d */
+	/* SP now points o the bottom of the frame - 1 */
+
+	tfr		sp, x
+	inx
+	std		.Lspsave
+	std		2, -sp
+
+	/* Recover the IRQ number and call up_doirq() */
+
+	ldab	.Lbsave
+	bsr		up_doirq
+	leas	2, sp
+
+	/* Check if the return value in d is the same as regs parameter passed in the TOS */
+
+	cpd		.Lspsave
+	beq		.Lnoswitch
 #warning "Missing Logic"
 
+.Lnoswitch:
 	/* Restore registers and return */
  
 	/* Restore the PPAGE register */
@@ -277,6 +323,11 @@ vcommon:
 	movw	2, sp+, _.d2
 #endif
 
+	/* Skip over the saved stack pointer */
+
+	ins
+	ins
+
 	movw	2, sp+, _.xy
 	movw	2, sp+, _.z
 	movw	2, sp+, _.tmp
@@ -288,6 +339,13 @@ vcommon:
  * .bss
  ************************************************************************************/
 /************************************************************************************
+ *	Name: Temporaries used within vcommon
+ ************************************************************************************/
+
+	.comm	.Lbsave, 1, 1
+	.comm	.Lspsave, 2, 1
+
+/************************************************************************************
  *	Name: up_interruptstack/g_userstack
  *
  * Description:
@@ -297,12 +355,9 @@ vcommon:
  ************************************************************************************/
 
 #if CONFIG_ARCH_INTERRUPTSTACK > 1
-		.bss
-		.align	2
-up_interruptstack:
-		.skip	(CONFIG_ARCH_INTERRUPTSTACK & ~1)
+	.comm	.up_interruptstack:, CONFIG_ARCH_INTERRUPTSTACK, 1
 up_interruptstack_base:
-		.size	up_interruptstack, .-up_interruptstack
+	.size	up_interruptstack, .-up_interruptstack
 #endif
-		.end
+	.end