From d85a6b5e74dbbf82ef5ed1518d9c588356a11251 Mon Sep 17 00:00:00 2001 From: patacongo Date: Wed, 13 May 2009 16:19:05 +0000 Subject: 1st cut at lm3s6918 interrupt handling git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@1776 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/include/irq.h | 2 +- nuttx/arch/arm/include/irq_cortexm3.h | 215 +++++++++++++++++++++++ nuttx/arch/arm/include/irq_thumb2.h | 210 ---------------------- nuttx/arch/arm/src/common/up_assert.c | 3 +- nuttx/arch/arm/src/common/up_initialstate.c | 17 +- nuttx/arch/arm/src/common/up_schedulesigaction.c | 40 +++-- nuttx/arch/arm/src/common/up_sigdeliver.c | 14 +- nuttx/arch/arm/src/lm3s/lm3s_vectors.S | 37 ++-- 8 files changed, 295 insertions(+), 243 deletions(-) create mode 100644 nuttx/arch/arm/include/irq_cortexm3.h delete mode 100644 nuttx/arch/arm/include/irq_thumb2.h (limited to 'nuttx/arch/arm') diff --git a/nuttx/arch/arm/include/irq.h b/nuttx/arch/arm/include/irq.h index 1d2c3e122..dabd35f41 100644 --- a/nuttx/arch/arm/include/irq.h +++ b/nuttx/arch/arm/include/irq.h @@ -48,7 +48,7 @@ #include #ifdef __thumb2__ -# include +# include #else # include #endif diff --git a/nuttx/arch/arm/include/irq_cortexm3.h b/nuttx/arch/arm/include/irq_cortexm3.h new file mode 100644 index 000000000..a6c235811 --- /dev/null +++ b/nuttx/arch/arm/include/irq_cortexm3.h @@ -0,0 +1,215 @@ +/**************************************************************************** + * arch/arm/include/irq_cortexm3.h + * + * Copyright (C) 2009 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. + * + ****************************************************************************/ + +/* This file should never be included directed but, rather, only indirectly + * through nuttx/irq.h + */ + +#ifndef __ARCH_ARM_INCLUDE_IRQ_THUMB2_H +#define __ARCH_ARM_INCLUDE_IRQ_THUMB2_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/* IRQ Stack Frame Format: */ + +/* On entry into an IRQ, the hardware automatically saves the following + * registers on the stack in this (address) order: + */ + +#define REG_XPSR (17) /* xPSR */ +#define REG_R15 (16) /* R15 = PC */ +#define REG_R14 (15) /* R14 = LR */ +#define REG_R12 (14) /* R12 */ +#define REG_R3 (13) /* R3 */ +#define REG_R2 (12) /* R2 */ +#define REG_R1 (11) /* R1 */ +#define REG_R0 (10) /* R0 */ + +#define HW_XCPT_REGS (8) +#define HW_XCPT_SIZE (4 * HW_XCPT_REGS) + +/* The following additional registers are stored by the interrupt handling + * logic. + */ + +#define REG_R11 (9) /* R11 */ +#define REG_R10 (8) /* R10 */ +#define REG_R9 (7) /* R9 */ +#define REG_R8 (6) /* R8 */ +#define REG_R7 (5) /* R7 */ +#define REG_R6 (4) /* R6 */ +#define REG_R5 (3) /* R5 */ +#define REG_R4 (2) /* R4 */ +#define REG_PRIMASK (1) /* PRIMASK */ +#define REG_R13 (0) /* R13 = SP at time of interrupt */ + +#define SW_XCPT_REGS (10) +#define SW_XCPT_SIZE (4 * SW_XCPT_REGS) + +#define XCPTCONTEXT_REGS (HW_XCPT_REGS + SW_XCPT_REGS) +#define XCPTCONTEXT_SIZE (HW_XCPT_SIZE + SW_XCPT_SIZE) + +#define REG_A1 REG_R0 +#define REG_A2 REG_R1 +#define REG_A3 REG_R2 +#define REG_A4 REG_R3 +#define REG_V1 REG_R4 +#define REG_V2 REG_R5 +#define REG_V3 REG_R6 +#define REG_V4 REG_R7 +#define REG_V5 REG_R8 +#define REG_V6 REG_R9 +#define REG_V7 REG_R10 +#define REG_SB REG_R9 +#define REG_SL REG_R10 +#define REG_FP REG_R11 +#define REG_IP REG_R12 +#define REG_SP REG_R13 +#define REG_LR REG_R14 +#define REG_PC REG_R15 + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* The following structure is included in the TCB and defines the complete + * state of the thread. + */ + +#ifndef __ASSEMBLY__ +struct xcptcontext +{ + /* The following function pointer is non-zero if there + * are pending signals to be processed. + */ + +#ifndef CONFIG_DISABLE_SIGNALS + void *sigdeliver; /* Actual type is sig_deliver_t */ + + /* These are saved copies of LR, PRIMASK, and xPSR used during + * signal processing. + */ + + uint32 saved_pc; + uint32 saved_primask; + uint32 saved_xpsr; +#endif + + /* Register save area */ + + uint32 regs[XCPTCONTEXT_REGS]; +}; +#endif + +/**************************************************************************** + * Inline functions + ****************************************************************************/ + +#ifndef __ASSEMBLY__ + +/* Save the current interrupt enable state & disable IRQs */ + +static inline irqstate_t irqsave(void) +{ + unsigned short primask; + + /* Return the the current value of primask register and set + * bit 0 of the primask register to disable interrupts + */ + + __asm__ __volatile__ + ( + "\tmrs %0, primask\n" + "\tcpsid i\n" + : "=r" (primask) + : + : "memory"); + return primask; +} + +/* Restore saved IRQ & FIQ state */ + +static inline void irqrestore(irqstate_t primask) +{ + /* If bit 0 of the primask is 0, then we need to restore + * interupts. + */ + + __asm__ __volatile__ + ( + "\ttst %0, #1\n" + "\tbne 1f\n" + "\tcpsie i\n" + "1:\n" + : + : "r" (primask) + : "memory"); +} + +#endif /* __ASSEMBLY__ */ + +/**************************************************************************** + * Public Variables + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifndef __ASSEMBLY__ +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +#undef EXTERN +#ifdef __cplusplus +} +#endif +#endif + +#endif /* __ARCH_ARM_INCLUDE_IRQ_THUMB2_H */ + diff --git a/nuttx/arch/arm/include/irq_thumb2.h b/nuttx/arch/arm/include/irq_thumb2.h deleted file mode 100644 index e138c157a..000000000 --- a/nuttx/arch/arm/include/irq_thumb2.h +++ /dev/null @@ -1,210 +0,0 @@ -/**************************************************************************** - * arch/arm/include/irq_thumb2.h - * - * Copyright (C) 2009 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. - * - ****************************************************************************/ - -/* This file should never be included directed but, rather, only indirectly - * through nuttx/irq.h - */ - -#ifndef __ARCH_ARM_INCLUDE_IRQ_THUMB2_H -#define __ARCH_ARM_INCLUDE_IRQ_THUMB2_H - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/* IRQ Stack Frame Format: */ - -/* On entry into an IRQ, the hardware automatically saves the following - * registers on the stack in this (address) order: - */ - -#define REG_XPSR (16) /* xPSR */ -#define REG_R15 (15) /* R15 = PC */ -#define REG_R14 (14) /* R14 = LR */ -#define REG_R12 (13) /* R12 */ -#define REG_R3 (12) /* R3 */ -#define REG_R2 (11) /* R2 */ -#define REG_R1 (10) /* R1 */ -#define REG_R0 (9) /* R0 */ - -#define HW_XCPT_REGS (8) -#define HW_XCPT_SIZE (4 * HW_XCPT_REGS) - -/* The following additional registers are stored by the interrupt handling - * logic. - */ - -#define REG_R13 (8) /* R13 = SP at time of interrupt */ -#define REG_R11 (7) /* R11 */ -#define REG_R10 (6) /* R10 */ -#define REG_R9 (5) /* R9 */ -#define REG_R8 (4) /* R8 */ -#define REG_R7 (3) /* R7 */ -#define REG_R6 (2) /* R6 */ -#define REG_R5 (1) /* R5 */ -#define REG_R4 (0) /* R4 */ - -#define XCPTCONTEXT_REGS (17) -#define XCPTCONTEXT_SIZE (4 * XCPTCONTEXT_REGS) - -#define REG_A1 REG_R0 -#define REG_A2 REG_R1 -#define REG_A3 REG_R2 -#define REG_A4 REG_R3 -#define REG_V1 REG_R4 -#define REG_V2 REG_R5 -#define REG_V3 REG_R6 -#define REG_V4 REG_R7 -#define REG_V5 REG_R8 -#define REG_V6 REG_R9 -#define REG_V7 REG_R10 -#define REG_SB REG_R9 -#define REG_SL REG_R10 -#define REG_FP REG_R11 -#define REG_IP REG_R12 -#define REG_SP REG_R13 -#define REG_LR REG_R14 -#define REG_PC REG_R15 - -/**************************************************************************** - * Public Types - ****************************************************************************/ - -/* The following structure is included in the TCB and defines the complete - * state of the thread. - */ - -#ifndef __ASSEMBLY__ -struct xcptcontext -{ - /* The following function pointer is non-zero if there - * are pending signals to be processed. - */ - -#ifndef CONFIG_DISABLE_SIGNALS - void *sigdeliver; /* Actual type is sig_deliver_t */ - - /* These are saved copies of LR and CPSR used during - * signal processing. - */ - - uint32 saved_pc; - uint32 saved_xpsr; -#endif - - /* Register save area */ - - uint32 regs[XCPTCONTEXT_REGS]; -}; -#endif - -/**************************************************************************** - * Inline functions - ****************************************************************************/ - -#ifndef __ASSEMBLY__ - -/* Save the current interrupt enable state & disable IRQs */ - -static inline irqstate_t irqsave(void) -{ - unsigned short primask; - - /* Return the the current value of primask register and set - * bit 0 of the primask register to disable interrupts - */ - - __asm__ __volatile__ - ( - "\tmrs %0, primask\n" - "\tcpsid i\n" - : "=r" (primask) - : - : "memory"); - return primask; -} - -/* Restore saved IRQ & FIQ state */ - -static inline void irqrestore(irqstate_t primask) -{ - /* If bit 0 of the primask is 0, then we need to restore - * interupts. - */ - - __asm__ __volatile__ - ( - "\ttst %0, #1\n" - "\tbne 1f\n" - "\tcpsie i\n" - "1:\n" - : - : "r" (primask) - : "memory"); -} - -#endif /* __ASSEMBLY__ */ - -/**************************************************************************** - * Public Variables - ****************************************************************************/ - -/**************************************************************************** - * Public Function Prototypes - ****************************************************************************/ - -#ifndef __ASSEMBLY__ -#ifdef __cplusplus -#define EXTERN extern "C" -extern "C" { -#else -#define EXTERN extern -#endif - -#undef EXTERN -#ifdef __cplusplus -} -#endif -#endif - -#endif /* __ARCH_ARM_INCLUDE_IRQ_THUMB2_H */ - diff --git a/nuttx/arch/arm/src/common/up_assert.c b/nuttx/arch/arm/src/common/up_assert.c index 8460c6b71..88dd0bd49 100644 --- a/nuttx/arch/arm/src/common/up_assert.c +++ b/nuttx/arch/arm/src/common/up_assert.c @@ -134,7 +134,8 @@ static inline void up_registerdump(void) } #ifdef __thumb2__ - lldbg("xPSR: %08x\n", current_regs[REG_XPSR]); + lldbg("xPSR: %08x PRIMASK: %08x\n", + current_regs[REG_XPSR], current_regs[REG_PRIMASK]); #else lldbg("CPSR: %08x\n", current_regs[REG_CPSR]); #endif diff --git a/nuttx/arch/arm/src/common/up_initialstate.c b/nuttx/arch/arm/src/common/up_initialstate.c index 2dd1695ba..a945b4c51 100644 --- a/nuttx/arch/arm/src/common/up_initialstate.c +++ b/nuttx/arch/arm/src/common/up_initialstate.c @@ -81,11 +81,18 @@ void up_initial_state(_TCB *tcb) /* Initialize the initial exception register context structure */ memset(xcp, 0, sizeof(struct xcptcontext)); - xcp->regs[REG_SP] = (uint32)tcb->adj_stack_ptr; - xcp->regs[REG_PC] = (uint32)tcb->start; -#ifdef CONFIG_SUPPRESS_INTERRUPTS - xcp->regs[REG_CPSR] = SVC_MODE | PSR_I_BIT | PSR_F_BIT; + xcp->regs[REG_SP] = (uint32)tcb->adj_stack_ptr; + xcp->regs[REG_PC] = (uint32)tcb->start; + +#ifdef __thumb2__ +# ifdef CONFIG_SUPPRESS_INTERRUPTS + xcp->regs[REG_PRIMASK] = 1; +# endif #else - xcp->regs[REG_CPSR] = SVC_MODE | PSR_F_BIT; +# ifdef CONFIG_SUPPRESS_INTERRUPTS + xcp->regs[REG_CPSR] = SVC_MODE | PSR_I_BIT | PSR_F_BIT; +# else + xcp->regs[REG_CPSR] = SVC_MODE | PSR_F_BIT; +# endif #endif } diff --git a/nuttx/arch/arm/src/common/up_schedulesigaction.c b/nuttx/arch/arm/src/common/up_schedulesigaction.c index 36be3eb24..fb6798468 100644 --- a/nuttx/arch/arm/src/common/up_schedulesigaction.c +++ b/nuttx/arch/arm/src/common/up_schedulesigaction.c @@ -146,16 +146,26 @@ void up_schedule_sigaction(_TCB *tcb, sig_deliver_t sigdeliver) * the signals have been delivered. */ - tcb->xcp.sigdeliver = sigdeliver; - tcb->xcp.saved_pc = current_regs[REG_PC]; - tcb->xcp.saved_cpsr = current_regs[REG_CPSR]; + tcb->xcp.sigdeliver = sigdeliver; + tcb->xcp.saved_pc = current_regs[REG_PC]; +#ifdef __thumb2__ + tcb->xcp.saved_primask = current_regs[REG_PRIMASK]; + tcb->xcp.saved_xpsr = current_regs[REG_XPSR]; +#else + tcb->xcp.saved_cpsr = current_regs[REG_CPSR]; +#endif /* Then set up to vector to the trampoline with interrupts * disabled */ - current_regs[REG_PC] = (uint32)up_sigdeliver; - current_regs[REG_CPSR] = SVC_MODE | PSR_I_BIT | PSR_F_BIT; + current_regs[REG_PC] = (uint32)up_sigdeliver; +#ifdef __thumb2__ + current_regs[REG_PRIMASK] = 1; + current_regs[REG_XPSR] = 0; +#else + current_regs[REG_CPSR] = SVC_MODE | PSR_I_BIT | PSR_F_BIT; +#endif /* And make sure that the saved context in the TCB * is the same as the interrupt return context. @@ -178,16 +188,26 @@ void up_schedule_sigaction(_TCB *tcb, sig_deliver_t sigdeliver) * the signals have been delivered. */ - tcb->xcp.sigdeliver = sigdeliver; - tcb->xcp.saved_pc = tcb->xcp.regs[REG_PC]; - tcb->xcp.saved_cpsr = tcb->xcp.regs[REG_CPSR]; + tcb->xcp.sigdeliver = sigdeliver; + tcb->xcp.saved_pc = tcb->xcp.regs[REG_PC]; +#ifdef __thumb2__ + tcb->xcp.saved_primask = tcb->xcp.regs[REG_PRIMASK]; + tcb->xcp.saved_xpsr = tcb->xcp.regs[REG_XPSR]; +#else + tcb->xcp.saved_cpsr = tcb->xcp.regs[REG_CPSR]; +#endif /* Then set up to vector to the trampoline with interrupts * disabled */ - tcb->xcp.regs[REG_PC] = (uint32)up_sigdeliver; - tcb->xcp.regs[REG_CPSR] = SVC_MODE | PSR_I_BIT | PSR_F_BIT; + tcb->xcp.regs[REG_PC] = (uint32)up_sigdeliver; +#ifdef __thumb2__ + tcb->xcp.regs[REG_PRIMASK] = 1; + tcb->xcp.regs[REG_XPSR] = 0; +#else + tcb->xcp.regs[REG_CPSR] = SVC_MODE | PSR_I_BIT | PSR_F_BIT; +#endif } irqrestore(flags); diff --git a/nuttx/arch/arm/src/common/up_sigdeliver.c b/nuttx/arch/arm/src/common/up_sigdeliver.c index 16227ae21..bf5661696 100644 --- a/nuttx/arch/arm/src/common/up_sigdeliver.c +++ b/nuttx/arch/arm/src/common/up_sigdeliver.c @@ -102,9 +102,13 @@ void up_sigdeliver(void) /* Save the real return state on the stack. */ up_copystate(regs, rtcb->xcp.regs); - regs[REG_PC] = rtcb->xcp.saved_pc; - regs[REG_CPSR] = rtcb->xcp.saved_cpsr; - + regs[REG_PC] = rtcb->xcp.saved_pc; +#ifdef __thumb2__ + regs[REG_PRIMASK] = rtcb->xcp.saved_primask; + regs[REG_XPSR] = rtcb->xcp.saved_xpsr; +#else + regs[REG_CPSR] = rtcb->xcp.saved_cpsr; +#endif /* Get a local copy of the sigdeliver function pointer. * we do this so that we can nullify the sigdeliver * function point in the TCB and accept more signal @@ -117,7 +121,11 @@ void up_sigdeliver(void) /* Then restore the task interrupt statat. */ +#ifdef __thumb2__ + irqrestore((uint16)regs[REG_PRIMASK]); +#else irqrestore(regs[REG_CPSR]); +#endif /* Deliver the signals */ diff --git a/nuttx/arch/arm/src/lm3s/lm3s_vectors.S b/nuttx/arch/arm/src/lm3s/lm3s_vectors.S index 3f196ae92..c17a387ae 100644 --- a/nuttx/arch/arm/src/lm3s/lm3s_vectors.S +++ b/nuttx/arch/arm/src/lm3s/lm3s_vectors.S @@ -38,6 +38,7 @@ ************************************************************************************/ #include +#include /************************************************************************************ * Preprocessor Definitions @@ -64,6 +65,10 @@ .globl dispach_irq + .syntax unified + .thumb + .file "lm3s_vectors.S" + /************************************************************************************ * Macros ************************************************************************************/ @@ -76,8 +81,8 @@ .macro HANDLER, label, irqno \label: - mov r0, \irqno - br lm3s_irqcommon + mov r0, #\irqno + b lm3s_irqcommon .endm /************************************************************************************ @@ -86,7 +91,7 @@ .section .vectors, "ax" .code 16 - .align 0 + .align 2 .globl lm3s_vectors .type lm3s_vectors, function @@ -177,7 +182,6 @@ lm3s_vectors: ************************************************************************************/ .text - .thumb_func .type handlers, function handlers: HANDLER lm3s_reserved, LMSB_IRQ_RESERVED /* Unexpected/reserved vector */ @@ -194,8 +198,8 @@ handlers: #ifdef CONFIG_ARCH_CHIP_LM3S6918 HANDLER lm3s_gpioa, LM3S_IRQ_GPIOA /* Vector 16: GPIO Port A */ HANDLER lm3s_gpiob, LM3S_IRQ_GPIOB /* Vector 17: GPIO Port B */ - HANDLER lm3s_gpiod, LM3S_IRQ_GPIOC /* Vector 18: GPIO Port C */ - HANDLER lm3s_gpioe, LM3S_IRQ_GPIOD /* Vector 19: GPIO Port D */ + HANDLER lm3s_gpioc, LM3S_IRQ_GPIOC /* Vector 18: GPIO Port C */ + HANDLER lm3s_gpiod, LM3S_IRQ_GPIOD /* Vector 19: GPIO Port D */ HANDLER lm3s_gpioe, LM3S_IRQ_GPIOE /* Vector 20: GPIO Port E */ HANDLER lm3s_uart0, LM3S_IRQ_UART0 /* Vector 21: UART 0 */ HANDLER lm3s_uart1, LM3S_IRQ_UART1 /* Vector 22: UART 1 */ @@ -248,9 +252,10 @@ lm3s_irqcommon: /* Complete the context save */ mrs r1, psp /* R1=The process stack pointer */ - mov r3, r1 /* R3=Copy of the process stack pointer */ - add r3, #HW_XCPT_SIZE /* R3=PSP before the interrupt was taken */ - stmdb r1!, {r3-r11} /* Save the remaining registers plus the SP value */ + mov r2, r1 /* R2=Copy of the process stack pointer */ + add r2, #HW_XCPT_SIZE /* R2=PSP before the interrupt was taken */ + mrs r3, primask /* R3=Current PRIMASK setting */ + stmdb r1!, {r2-r11} /* Save the remaining registers plus the SP value */ /* Disable interrupts, select the stack to use for interrupt handling * and call up_doirq to handle the interrupt @@ -286,15 +291,21 @@ lm3s_irqcommon: */ add r1, r0, #(XCPTCONTEXT_SIZE-4) /* r2=offset HW save area */ - ldmia r1, {r4, r11} /* Eight registers in HW save area */ + ldmia r1, {r4-r11} /* Eight registers in HW save area */ ldr r1, [r0, #(4*REG_SP)] /* R1=Value of SP before interrupt */ - stmdb r1!, {r4, r11} /* Eight registers in HW save area */ - mov psp, r1 /* New PSP */ + stmdb r1!, {r4-r11} /* Eight registers in HW save area */ + msr psp, r1 /* New PSP */ /* We simply need to "unwind" the same stack frame that we created */ 1: + stmdb r0!, {r2-r11} /* Recover R4-R11 + 2 temp values */ + + /* Do we need to restore interrupts? */ + + tst r3, #1 /* PRIMASK it 1=1 means that interrupts are masked */ + bne 2f cpsie i /* Restore interrupts */ - stmdb r0!, {r3-r11} /* Recover R4-R11 (R3 does not have the correct value yet) * the remaining registers plus the SP value */ +2: bx r14 /* And return */ .size handler, .-handlers -- cgit v1.2.3