From b67122892a05ffdaaf0e49835106a1bc44a01607 Mon Sep 17 00:00:00 2001 From: patacongo Date: Thu, 17 Feb 2011 02:12:14 +0000 Subject: More m9s12x stuff git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@3299 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/hc/include/hcs12/irq.h | 54 +++-- nuttx/arch/hc/src/common/up_doirq.c | 118 +++++++++++ nuttx/arch/hc/src/common/up_internal.h | 6 +- nuttx/arch/hc/src/m9s12/Make.defs | 6 +- nuttx/arch/hc/src/m9s12/m9s12_assert.c | 337 ++++++++++++++++++++++++++++++++ nuttx/arch/hc/src/m9s12/m9s12_irq.c | 2 +- nuttx/arch/hc/src/m9s12/m9s12_vectors.S | 77 ++++++-- 7 files changed, 568 insertions(+), 32 deletions(-) create mode 100644 nuttx/arch/hc/src/common/up_doirq.c create mode 100644 nuttx/arch/hc/src/m9s12/m9s12_assert.c 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 + * + * 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 + +#include +#include + +#include +#include +#include + +#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 * * 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 # # 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 + * + * 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 + +#include +#include +#include +#include + +#include +#include +#include + +#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 @@ -287,6 +338,13 @@ vcommon: /************************************************************************************ * .bss ************************************************************************************/ +/************************************************************************************ + * Name: Temporaries used within vcommon + ************************************************************************************/ + + .comm .Lbsave, 1, 1 + .comm .Lspsave, 2, 1 + /************************************************************************************ * Name: up_interruptstack/g_userstack * @@ -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 -- cgit v1.2.3