From e0178ffa9a25af4cd0f8a767a12be2d58aa3720a Mon Sep 17 00:00:00 2001 From: patacongo Date: Mon, 18 May 2009 21:08:43 +0000 Subject: Progress on Cortex-M3 interrupts git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@1789 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/include/irq_cortexm3.h | 71 ++++++++++++++- nuttx/arch/arm/include/lm3s/irq.h | 20 ++--- nuttx/arch/arm/src/common/up_assert.c | 20 ++++- nuttx/arch/arm/src/lm3s/Make.defs | 4 +- nuttx/arch/arm/src/lm3s/lm3s_context.S | 16 +--- nuttx/arch/arm/src/lm3s/lm3s_hardfault.c | 142 ++++++++++++++++++++++++++++++ nuttx/arch/arm/src/lm3s/lm3s_internal.h | 37 +++++++- nuttx/arch/arm/src/lm3s/lm3s_irq.c | 76 +++++++++------- nuttx/arch/arm/src/lm3s/lm3s_pendsv.c | 119 ------------------------- nuttx/arch/arm/src/lm3s/lm3s_start.c | 8 -- nuttx/arch/arm/src/lm3s/lm3s_svcall.c | 146 +++++++++++++++++++++++++++++++ nuttx/arch/arm/src/lm3s/lm3s_vectors.S | 86 +++++++++++------- 12 files changed, 516 insertions(+), 229 deletions(-) create mode 100644 nuttx/arch/arm/src/lm3s/lm3s_hardfault.c delete mode 100644 nuttx/arch/arm/src/lm3s/lm3s_pendsv.c create mode 100644 nuttx/arch/arm/src/lm3s/lm3s_svcall.c (limited to 'nuttx/arch/arm') diff --git a/nuttx/arch/arm/include/irq_cortexm3.h b/nuttx/arch/arm/include/irq_cortexm3.h index 8c149d1d6..a08b7e2f8 100644 --- a/nuttx/arch/arm/include/irq_cortexm3.h +++ b/nuttx/arch/arm/include/irq_cortexm3.h @@ -148,7 +148,7 @@ struct xcptcontext #ifndef __ASSEMBLY__ -/* Save the current interrupt enable state & disable IRQs */ +/* Save the current primask state & disable IRQs */ static inline irqstate_t irqsave(void) { @@ -168,7 +168,7 @@ static inline irqstate_t irqsave(void) return primask; } -/* Restore saved IRQ & FIQ state */ +/* Restore saved primask state */ static inline void irqrestore(irqstate_t primask) { @@ -187,18 +187,81 @@ static inline void irqrestore(irqstate_t primask) : "memory"); } -/* Get the basepri register */ +/* Get/set the primask register */ + +static inline ubyte getprimask(void) +{ + uint32 primask; + __asm__ __volatile__ + ( + "\tmrs %0, primask\n" + : "=r" (primask) + : + : "memory"); + return (ubyte)primask; +} + +static inline void setprimask(uint32 primask) +{ + __asm__ __volatile__ + ( + "\tmsr primask, %0\n" + : + : "r" (primask) + : "memory"); +} + +/* Get/set the basepri register */ + +static inline ubyte getbasepri(void) +{ + uint32 basepri; + __asm__ __volatile__ + ( + "\tmrs %0, basepri\n" + : "=r" (basepri) + : + : "memory"); + return (ubyte)basepri; +} static inline void setbasepri(uint32 basepri) { __asm__ __volatile__ ( - "\msr basepri, %0\n" + "\tmsr basepri, %0\n" : : "r" (basepri) : "memory"); } +/* Get IPSR */ + +static inline uint32 getipsr(void) +{ + uint32 ipsr; + __asm__ __volatile__ + ( + "\tmrs %0, ipsr\n" + : "=r" (ipsr) + : + : "memory"); + return ipsr; +} + +/* SVC system call */ + +static inline void svcall(uint32 cmd, uint32 arg) +{ + __asm__ __volatile__ + ( + "\tmov r0, %0\n" + "\tmov r1, %1\n" + "\tsvc 0\n" + : + : "r" (cmd), "r" (arg) + : "memory"); +} #endif /* __ASSEMBLY__ */ /**************************************************************************** diff --git a/nuttx/arch/arm/include/lm3s/irq.h b/nuttx/arch/arm/include/lm3s/irq.h index 68df2e4b6..1c838790a 100644 --- a/nuttx/arch/arm/include/lm3s/irq.h +++ b/nuttx/arch/arm/include/lm3s/irq.h @@ -59,19 +59,19 @@ /* Processor Exceptions (vectors 0-15) */ -#define LMSB_IRQ_RESERVED (0) /* Reserved vector (only used with CONFIG_DEBUG) */ +#define LM3S_IRQ_RESERVED (0) /* Reserved vector (only used with CONFIG_DEBUG) */ /* Vector 0: Reset stack pointer value */ /* Vector 1: Reset (not handler as an IRQ) */ -#define LMSB_IRQ_NMI (2) /* Vector 2: Non-Maskable Interrupt (NMI) */ -#define LMSB_IRQ_HARDFAULT (3) /* Vector 3: Hard fault */ -#define LMSB_IRQ_MPU (4) /* Vector 4: Memory management (MPU) */ -#define LMSB_IRQ_BUSFAULT (5) /* Vector 5: Bus fault */ -#define LMSB_IRQ_USAGEFAULT (6) /* Vector 6: Usage fault */ -#define LMSB_IRQ_SVCALL (11) /* Vector 11: SVC call */ -#define LMSB_IRQ_DBGMONITOR (12) /* Vector 12: Debug Monitor */ +#define LM3S_IRQ_NMI (2) /* Vector 2: Non-Maskable Interrupt (NMI) */ +#define LM3S_IRQ_HARDFAULT (3) /* Vector 3: Hard fault */ +#define LM3S_IRQ_MPU (4) /* Vector 4: Memory management (MPU) */ +#define LM3S_IRQ_BUSFAULT (5) /* Vector 5: Bus fault */ +#define LM3S_IRQ_USAGEFAULT (6) /* Vector 6: Usage fault */ +#define LM3S_IRQ_SVCALL (11) /* Vector 11: SVC call */ +#define LM3S_IRQ_DBGMONITOR (12) /* Vector 12: Debug Monitor */ /* Vector 13: Reserved */ -#define LMSB_IRQ_PENDSV (14) /* Vector 14: Pendable system service request */ -#define LMSB_IRQ_SYSTICK (15) /* Vector 15: System tick */ +#define LM3S_IRQ_PENDSV (14) /* Vector 14: Pendable system service request */ +#define LM3S_IRQ_SYSTICK (15) /* Vector 15: System tick */ /* External interrupts (vectors >= 16) */ diff --git a/nuttx/arch/arm/src/common/up_assert.c b/nuttx/arch/arm/src/common/up_assert.c index 88dd0bd49..dcb254e60 100644 --- a/nuttx/arch/arm/src/common/up_assert.c +++ b/nuttx/arch/arm/src/common/up_assert.c @@ -121,6 +121,22 @@ static inline void up_registerdump(void) if (current_regs) { +#ifdef __thumb2__ + /* Yes.. dump the interrupt registers */ + + lldbg("R0: %08x %08x %08x %08x %08x %08x %08x %08x\n", + current_regs[REG_R0], current_regs[REG_R1], + current_regs[REG_R2], current_regs[REG_R3], + current_regs[REG_R4], current_regs[REG_R5], + current_regs[REG_R6], current_regs[REG_R7]); + lldbg("R8: %08x %08x %08x %08x %08x %08x %08x %08x\n", + current_regs[REG_R8], current_regs[REG_R9], + current_regs[REG_R10], current_regs[REG_R11], + current_regs[REG_R12], current_regs[REG_R13], + current_regs[REG_R14], current_regs[REG_R15]); + lldbg("xPSR: %08x PRIMASK: %08x\n", + current_regs[REG_XPSR], current_regs[REG_PRIMASK]); +#else int regs; /* Yes.. dump the interrupt registers */ @@ -133,10 +149,6 @@ static inline void up_registerdump(void) ptr[4], ptr[5], ptr[6], ptr[7]); } -#ifdef __thumb2__ - 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/lm3s/Make.defs b/nuttx/arch/arm/src/lm3s/Make.defs index f033f8826..3eb33dd58 100644 --- a/nuttx/arch/arm/src/lm3s/Make.defs +++ b/nuttx/arch/arm/src/lm3s/Make.defs @@ -46,8 +46,8 @@ CMN_CSRCS = up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \ up_undefinedinsn.c up_usestack.c up_doirq.c CHIP_ASRCS = lm3s_context.S -CHIP_CSRCS = lm3s_start.c lm3s_syscontrol.c lm3s_irq.c lm3s_pendsv.c \ - lm3s_gpio.c lm3s_gpioirq.c lm3s_timerisr.c lm3s_lowputc.c \ +CHIP_CSRCS = lm3s_start.c lm3s_syscontrol.c lm3s_irq.c lm3s_svcall.c \ + lm3s_hardfault.c lm3s_gpio.c lm3s_gpioirq.c lm3s_timerisr.c lm3s_lowputc.c \ lm3s_serial.c ifdef CONFIG_NET diff --git a/nuttx/arch/arm/src/lm3s/lm3s_context.S b/nuttx/arch/arm/src/lm3s/lm3s_context.S index 9f74408d6..7641b0109 100644 --- a/nuttx/arch/arm/src/lm3s/lm3s_context.S +++ b/nuttx/arch/arm/src/lm3s/lm3s_context.S @@ -84,12 +84,9 @@ up_saveusercontext: /* Perform the System call with R0=0 and R1=regs */ - ldr r2, nvic_intctrl /* R2: Address NVIC INTCTRL registers */ - ldr r3, [r2, #0] /* R3: Content of NVIC INCTRL */ - orr r3, r3, #NVIC_INTCTRL_PENDSVSET mov r1, r0 /* R1: regs */ mov r0, #0 /* R0: 0 means save context (also return value) */ - str r3, [r2, #0] /* Force Pend SV */ + svc 0 /* Force synchronous SVCall (or Hard Fault) */ /* There are two return conditions. On the first return, R0 (the * return value will be zero. On the second return we need to @@ -124,22 +121,13 @@ up_fullcontextrestore: /* Perform the System call with R0=1 and R1=regs */ - ldr r2, nvic_intctrl /* R2: Address NVIC INTCTRL registers */ - ldr r3, [r2, #0] /* R3: Content of NVIC INCTRL */ - orr r3, r3, #NVIC_INTCTRL_PENDSVSET mov r1, r0 /* R1: regs */ mov r0, #1 /* R0: 1 means restore context */ - str r3, [r2, #0] /* Force Pend SV */ + svc 0 /* Force synchronous SVCall (or Hard Fault) */ /* This call should not return */ bx lr /* Unnecessary ... will not return */ .size up_fullcontextrestore, .-up_fullcontextrestore - - .align 2 - .type nvic_intctrl, object -nvic_intctrl: - .word NVIC_INTCTRL - .size nvic_intctrl, .-nvic_intctrl .end diff --git a/nuttx/arch/arm/src/lm3s/lm3s_hardfault.c b/nuttx/arch/arm/src/lm3s/lm3s_hardfault.c new file mode 100644 index 000000000..64382c328 --- /dev/null +++ b/nuttx/arch/arm/src/lm3s/lm3s_hardfault.c @@ -0,0 +1,142 @@ +/**************************************************************************** + * arch/arm/src/lm3s/lm3s_hardfault.c + * arch/arm/src/chip/lm3s_hardfault.c + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include + +#include + +#include "up_arch.h" +#include "os_internal.h" +#include "cortexm3_nvic.h" +#include "lm3s_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#undef DEBUG_HARDFAULTS /* Define to debug hard faults */ + +#define INSN_SVC0 0xdf00 /* insn: svc 0 */ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: lm3s_hardfault + * + * Description: + * This is Hard Fault exception handler. It also catches SVC call + * exceptions that are performed in bad contexts. + * + ****************************************************************************/ + +int lm3s_hardfault(int irq, FAR void *context) +{ + uint32 *regs = (uint32*)context; + uint16 *pc; + uint16 insn; + + /* Dump some hard fault info */ + +#ifdef DEBUG_HARDFAULTS + lldbg("Hard Fault:\n"); + lldbg(" IRQ: %d regs: %p\n", irq, regs); + lldbg(" BASEPRI: %08x PRIMASK: %08x IPSR: %08x\n", + getbasepri(), getprimask(), getipsr()); + lldbg(" CFAULTS: %08x HFAULTS: %08x DFAULTS: %08x BFAULTADDR: %08x AFAULTS: %08x\n", + getreg32(NVIC_CFAULTS), getreg32(NVIC_HFAULTS), + getreg32(NVIC_DFAULTS), getreg32(NVIC_BFAULT_ADDR), + getreg32(NVIC_AFAULTS)); + lldbg(" R0: %08x %08x %08x %08x %08x %08x %08x %08x\n", + regs[REG_R0], regs[REG_R1], regs[REG_R2], regs[REG_R3], + regs[REG_R4], regs[REG_R5], regs[REG_R6], regs[REG_R7]); + lldbg(" R8: %08x %08x %08x %08x %08x %08x %08x %08x\n", + regs[REG_R8], regs[REG_R9], regs[REG_R10], regs[REG_R11], + regs[REG_R12], regs[REG_R13], regs[REG_R14], regs[REG_R15]); + lldbg(" PSR=%08x\n", regs[REG_XPSR]); +#endif + + /* Get the value of the program counter where the fault occurred */ + + pc = (uint16*)regs[REG_PC] - 1; + if ((void*)pc >= (void*)&_stext && (void*)pc < (void*)&_etext) + { + /* Fetch the instruction that caused the Hard fault */ + + insn = *pc; + +#ifdef DEBUG_HARDFAULTS + lldbg(" PC: %p INSN: %04x\n", pc, insn); +#endif + + /* If this was the instruction 'svc 0', then forward processing + * to the SVCall handler + */ + + if (insn == INSN_SVC0) + { + llvdbg("Forward SVCall\n"); + return lm3s_svcall(LM3S_IRQ_SVCALL, context); + } + } + + (void)irqsave(); + dbg("PANIC!!! Hard fault: %08x\n", getreg32(NVIC_HFAULTS)); + PANIC(OSERR_UNEXPECTEDISR); + return OK; +} diff --git a/nuttx/arch/arm/src/lm3s/lm3s_internal.h b/nuttx/arch/arm/src/lm3s/lm3s_internal.h index 4d99cb492..86d9514d2 100644 --- a/nuttx/arch/arm/src/lm3s/lm3s_internal.h +++ b/nuttx/arch/arm/src/lm3s/lm3s_internal.h @@ -213,6 +213,26 @@ extern "C" { #define EXTERN extern #endif +/* These 'addresses' of these values are setup by the linker script. They are + * not actual uint32 storage locations! They are only used meaningfully in the + * following way: + * + * - The linker script defines, for example, the symbol_sdata. + * - The declareion extern uint32 _sdata; makes C happy. C will believe + * that the value _sdata is the address of a uint32 variable _data (it is + * not!). + * - We can recoved the linker value then by simply taking the address of + * of _data. like: uint32 *pdata = &_sdata; + */ + +extern uint32 _stext; /* Start of .text */ +extern uint32 _etext; /* End_1 of .text + .rodata) */ +extern const uint32 _eronly; /* End+1 of read only section (.text + .rodata) */ +extern uint32 _sdata; /* Start of .data */ +extern uint32 _edata; /* End+1 of .data */ +extern uint32 _sbss; /* Start of .bss */ +extern uint32 _ebss; /* End+1 of .bss */ + /**************************************************************************** * Public Function Prototypes ****************************************************************************/ @@ -261,14 +281,25 @@ EXTERN void up_clockconfig(void); EXTERN int lm3s_configgpio(uint32 cfgset); /**************************************************************************** - * Name: lm3s_pendsv + * Name: lm3s_svcall + * + * Description: + * This is SVCall exception handler that performs context switching + * + ****************************************************************************/ + +EXTERN int lm3s_svcall(int irq, FAR void *context); + +/**************************************************************************** + * Name: lm3s_hardfault * * Description: - * This is PendSV exception handler that performs context switching + * This is Hard Fault exception handler. It also catches SVC call + * exceptions that are performed in bad contexts. * ****************************************************************************/ -EXTERN int lm3s_pendsv(int irq, FAR void *context); +EXTERN int lm3s_hardfault(int irq, FAR void *context); /**************************************************************************** * Name: lm3s_gpiowrite diff --git a/nuttx/arch/arm/src/lm3s/lm3s_irq.c b/nuttx/arch/arm/src/lm3s/lm3s_irq.c index c8307aac3..d75d33304 100644 --- a/nuttx/arch/arm/src/lm3s/lm3s_irq.c +++ b/nuttx/arch/arm/src/lm3s/lm3s_irq.c @@ -45,6 +45,7 @@ #include #include +#include #include "up_arch.h" #include "os_internal.h" @@ -55,9 +56,11 @@ * Definitions ****************************************************************************/ -/* Enable debug features that are probably on desireable during bringup */ +/* Enable NVIC debug features that are probably only desireable during + * bringup + */ -#define LM2S_IRQ_DEBUG 1 +#undef LM2S_IRQ_DEBUG /* Get a 32-bit version of the default priority */ @@ -92,9 +95,17 @@ uint32 *current_regs; #if defined(LM2S_IRQ_DEBUG) && defined (CONFIG_DEBUG) static void lm3s_dumpnvic(const char *msg, int irq) { + irqstate_t flags; + + flags = irqsave(); slldbg("NVIC (%s, irq=%d):\n", msg, irq); slldbg(" INTCTRL: %08x VECTAB: %08x\n", getreg32(NVIC_INTCTRL), getreg32(NVIC_VECTAB)); +#if 0 + slldbg(" SYSH ENABLE MEMFAULT: %08x BUSFAULT: %08x USGFAULT: %08x SYSTICK: %08x\n", + getreg32(NVIC_SYSHCON_MEMFAULTENA), getreg32(NVIC_SYSHCON_BUSFAULTENA), + getreg32(NVIC_SYSHCON_USGFAULTENA), getreg32(NVIC_SYSTICK_CTRL_ENABLE)); +#endif slldbg(" IRQ ENABLE: %08x %08x\n", getreg32(NVIC_IRQ0_31_ENABLE), getreg32(NVIC_IRQ32_63_ENABLE)); slldbg(" SYSH_PRIO: %08x %08x %08x\n", @@ -109,14 +120,15 @@ static void lm3s_dumpnvic(const char *msg, int irq) slldbg(" %08x %08x %08x %08x\n", getreg32(NVIC_IRQ32_35_PRIORITY), getreg32(NVIC_IRQ36_39_PRIORITY), getreg32(NVIC_IRQ40_43_PRIORITY), getreg32(NVIC_IRQ44_47_PRIORITY)); + irqrestore(flags); } #else # define lm3s_dumpnvic(msg, irq) #endif /**************************************************************************** - * Name: lm3s_nmi, lm3s_hardfault, lm3s_mpu, lm3s_busfault, lm3s_usagefault, - * lm3s_svcall, lm3s_dbgmonitor, lm3s_pendsv, lm3s_reserved + * Name: lm3s_nmi, lm3s_mpu, lm3s_busfault, lm3s_usagefault, lm3s_pendsv, + * lm3s_dbgmonitor, lm3s_pendsv, lm3s_reserved * * Description: * Handlers for various execptions. None are handled and all are fatal @@ -134,14 +146,6 @@ static int lm3s_nmi(int irq, FAR void *context) return 0; } -static int lm3s_hardfault(int irq, FAR void *context) -{ - (void)irqsave(); - dbg("PANIC!!! Hard fault received\n"); - PANIC(OSERR_UNEXPECTEDISR); - return 0; -} - static int lm3s_mpu(int irq, FAR void *context) { (void)irqsave(); @@ -166,10 +170,10 @@ static int lm3s_usagefault(int irq, FAR void *context) return 0; } -static int lm3s_svcall(int irq, FAR void *context) +static int lm3s_pendsv(int irq, FAR void *context) { (void)irqsave(); - dbg("PANIC!!! SVCALL received\n"); + dbg("PANIC!!! PendSV received\n"); PANIC(OSERR_UNEXPECTEDISR); return 0; } @@ -202,7 +206,7 @@ static int lm3s_reserved(int irq, FAR void *context) static int lml3s_irqinfo(int irq, uint32 *regaddr, uint32 *bit) { - DEBUGASSERT(irq >= LMSB_IRQ_MPU && irq < NR_IRQS); + DEBUGASSERT(irq >= LM3S_IRQ_NMI && irq < NR_IRQS); /* Check for external interrupt */ @@ -229,19 +233,19 @@ static int lml3s_irqinfo(int irq, uint32 *regaddr, uint32 *bit) else { *regaddr = NVIC_SYSHCON; - if (irq == LMSB_IRQ_MPU) + if (irq == LM3S_IRQ_MPU) { *bit = NVIC_SYSHCON_MEMFAULTENA; } - else if (irq == LMSB_IRQ_BUSFAULT) + else if (irq == LM3S_IRQ_BUSFAULT) { *bit = NVIC_SYSHCON_BUSFAULTENA; } - else if (irq == LMSB_IRQ_USAGEFAULT) + else if (irq == LM3S_IRQ_USAGEFAULT) { *bit = NVIC_SYSHCON_USGFAULTENA; } - else if (irq == LMSB_IRQ_SYSTICK) + else if (irq == LM3S_IRQ_SYSTICK) { *regaddr = NVIC_SYSTICK_CTRL; *bit = NVIC_SYSTICK_CTRL_ENABLE; @@ -304,27 +308,31 @@ void up_irqinitialize(void) } #endif - /* Attach the PendSV exception handler and set it to the minimum - * prioirity. The PendSV exception is used for performing - * context switches. + /* Attach the SVCall and Hard Fault exception handlers. The SVCall + * exception is used for performing context switches; The Hard Fault + * must also be caught because a SVCall may show up as a Hard Fault + * under certain conditions. */ - irq_attach(LMSB_IRQ_PENDSV, lm3s_pendsv); + irq_attach(LM3S_IRQ_SVCALL, lm3s_svcall); + irq_attach(LM3S_IRQ_HARDFAULT, lm3s_hardfault); + + /* Set the priority of the SVCall interrupt */ + #ifdef CONFIG_ARCH_IRQPRIO -/* up_prioritize_irq(LMSB_IRQ_PENDSV, NVIC_SYSH_PRIORITY_MIN); */ +/* up_prioritize_irq(LM3S_IRQ_PENDSV, NVIC_SYSH_PRIORITY_MIN); */ #endif /* Attach all other processor exceptions (except reset and sys tick) */ #ifdef CONFIG_DEBUG - irq_attach(LMSB_IRQ_NMI, lm3s_nmi); - irq_attach(LMSB_IRQ_HARDFAULT, lm3s_hardfault); - irq_attach(LMSB_IRQ_MPU, lm3s_mpu); - irq_attach(LMSB_IRQ_BUSFAULT, lm3s_busfault); - irq_attach(LMSB_IRQ_USAGEFAULT, lm3s_usagefault); - irq_attach(LMSB_IRQ_SVCALL, lm3s_svcall); - irq_attach(LMSB_IRQ_DBGMONITOR, lm3s_dbgmonitor); - irq_attach(LMSB_IRQ_RESERVED, lm3s_reserved); + irq_attach(LM3S_IRQ_NMI, lm3s_nmi); + irq_attach(LM3S_IRQ_MPU, lm3s_mpu); + irq_attach(LM3S_IRQ_BUSFAULT, lm3s_busfault); + irq_attach(LM3S_IRQ_USAGEFAULT, lm3s_usagefault); + irq_attach(LM3S_IRQ_PENDSV, lm3s_pendsv); + irq_attach(LM3S_IRQ_DBGMONITOR, lm3s_dbgmonitor); + irq_attach(LM3S_IRQ_RESERVED, lm3s_reserved); #endif lm3s_dumpnvic("inital", NR_IRQS); @@ -339,7 +347,7 @@ void up_irqinitialize(void) /* And finally, enable interrupts */ - setbasepri(0); + setbasepri(NVIC_SYSH_PRIORITY_MAX); irqrestore(0); #endif } @@ -425,7 +433,7 @@ int up_prioritize_irq(int irq, int priority) uint32 regval; int shift; - DEBUGASSERT(irq >= LMSB_IRQ_MPU && irq < NR_IRQS && (unsigned)priority <= NVIC_SYSH_PRIORITY_MIN); + DEBUGASSERT(irq >= LM3S_IRQ_MPU && irq < NR_IRQS && (unsigned)priority <= NVIC_SYSH_PRIORITY_MIN); if (irq < LM3S_IRQ_INTERRUPTS) { diff --git a/nuttx/arch/arm/src/lm3s/lm3s_pendsv.c b/nuttx/arch/arm/src/lm3s/lm3s_pendsv.c deleted file mode 100644 index ef2b189c8..000000000 --- a/nuttx/arch/arm/src/lm3s/lm3s_pendsv.c +++ /dev/null @@ -1,119 +0,0 @@ -/**************************************************************************** - * arch/arm/src/lm3s/lm3s_pendsv.c - * arch/arm/src/chip/lm3s_pendsv.c - * - * 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include - -#include - -#include "os_internal.h" -#include "lm3s_internal.h" - -/**************************************************************************** - * Private Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: lm3xs_pendxv - * - * Description: - * This is PendSV exception handler that performs context switching - * - ****************************************************************************/ - -int lm3s_pendsv(int irq, FAR void *context) -{ - uint32 *svregs = (uint32*)context; - uint32 *tcbregs = (uint32*)svregs[REG_R1]; - - DEBUGASSERT(svregs && svregs == current_regs && tcbregs); - - /* The PendSV software interrupt is called with R0 = PendSV command and R1 = - * the TCB register save area. - */ - - svdbg("Command: %d regs: %08x\n", svregs[REG_R0], tcbregs); - switch (svregs[REG_R0]) - { - /* R0=0: This is a save context command. In this case, we simply need - * to copy the svregs to the tdbregs and return. - */ - - case 0: - memcpy(tcbregs, svregs, XCPTCONTEXT_SIZE); - break; - - /* R1=1: This a restore context command. In this case, we simply need to - * set current_regs to tcbrgs. svregs == current_regs is the normal exception - * turn. By setting current_regs = tcbregs, we force the return through - * the saved context. - */ - - case 1: - current_regs = tcbregs; - break; - - default: - PANIC(OSERR_INTERNAL); - break; - } - - return OK; -} diff --git a/nuttx/arch/arm/src/lm3s/lm3s_start.c b/nuttx/arch/arm/src/lm3s/lm3s_start.c index df1a48c84..848c41074 100644 --- a/nuttx/arch/arm/src/lm3s/lm3s_start.c +++ b/nuttx/arch/arm/src/lm3s/lm3s_start.c @@ -62,14 +62,6 @@ * Public Data ****************************************************************************/ -/* These values are setup by the linker script */ - -extern const uint32 _eronly; /* End of read only (.text + .rodata) */ -extern uint32 _sdata; /* Start of .data */ -extern uint32 _edata; /* End+1 of .data */ -extern uint32 _sbss; /* Start of .bss */ -extern uint32 _ebss; /* End+1 of .bss */ - extern void lm3s_vectors(void); /**************************************************************************** diff --git a/nuttx/arch/arm/src/lm3s/lm3s_svcall.c b/nuttx/arch/arm/src/lm3s/lm3s_svcall.c new file mode 100644 index 000000000..d6c7bf517 --- /dev/null +++ b/nuttx/arch/arm/src/lm3s/lm3s_svcall.c @@ -0,0 +1,146 @@ +/**************************************************************************** + * arch/arm/src/lm3s/lm3s_svcall.c + * arch/arm/src/chip/lm3s_svcall.c + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include + +#include + +#include "os_internal.h" +#include "lm3s_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#undef DEBUG_SVCALL /* Define to debug SVCall */ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: lm3s_svcall + * + * Description: + * This is SVCall exception handler that performs context switching + * + ****************************************************************************/ + +int lm3s_svcall(int irq, FAR void *context) +{ + uint32 *svregs = (uint32*)context; + uint32 *tcbregs = (uint32*)svregs[REG_R1]; + + DEBUGASSERT(svregs && svregs == current_regs && tcbregs); + + /* The SVCall software interrupt is called with R0 = SVC command and R1 = + * the TCB register save area. + */ + + sllvdbg("Command: %d svregs: %p tcbregs: %08x\n", svregs[REG_R0], svregs, tcbregs); + +#ifdef DEBUG_SVCALL + lldbg("SVCall Entry:\n"); + lldbg(" R0: %08x %08x %08x %08x %08x %08x %08x %08x\n", + svregs[REG_R0], svregs[REG_R1], svregs[REG_R2], svregs[REG_R3], + svregs[REG_R4], svregs[REG_R5], svregs[REG_R6], svregs[REG_R7]); + lldbg(" R8: %08x %08x %08x %08x %08x %08x %08x %08x\n", + svregs[REG_R8], svregs[REG_R9], svregs[REG_R10], svregs[REG_R11], + svregs[REG_R12], svregs[REG_R13], svregs[REG_R14], svregs[REG_R15]); + lldbg(" PSR=%08x\n", svregs[REG_XPSR]); +#endif + + /* Handle the SVCall according to the command in R0 */ + + switch (svregs[REG_R0]) + { + /* R0=0: This is a save context command. In this case, we simply need + * to copy the svregs to the tdbregs and return. + */ + + case 0: + memcpy(tcbregs, svregs, XCPTCONTEXT_SIZE); + break; + + /* R1=1: This a restore context command. In this case, we simply need to + * set current_regs to tcbrgs. svregs == current_regs is the normal exception + * turn. By setting current_regs = tcbregs, we force the return through + * the saved context. + */ + + case 1: + current_regs = tcbregs; + break; + + default: + PANIC(OSERR_INTERNAL); + break; + } + +#ifdef DEBUG_SVCALL + lldbg("SVCall Return:\n"); + lldbg(" R0: %08x %08x %08x %08x %08x %08x %08x %08x\n", + current_regs[REG_R0], current_regs[REG_R1], current_regs[REG_R2], current_regs[REG_R3], + current_regs[REG_R4], current_regs[REG_R5], current_regs[REG_R6], current_regs[REG_R7]); + lldbg(" R8: %08x %08x %08x %08x %08x %08x %08x %08x\n", + current_regs[REG_R8], current_regs[REG_R9], current_regs[REG_R10], current_regs[REG_R11], + current_regs[REG_R12], current_regs[REG_R13], current_regs[REG_R14], current_regs[REG_R15]); + lldbg(" PSR=%08x\n", current_regs[REG_XPSR]); +#endif + + return OK; +} diff --git a/nuttx/arch/arm/src/lm3s/lm3s_vectors.S b/nuttx/arch/arm/src/lm3s/lm3s_vectors.S index f25086c91..5be36a90e 100644 --- a/nuttx/arch/arm/src/lm3s/lm3s_vectors.S +++ b/nuttx/arch/arm/src/lm3s/lm3s_vectors.S @@ -60,6 +60,13 @@ #define IDLE_STACK (_ebss+CONFIG_IDLETHREAD_STACKSIZE-4) #define HEAP_BASE (_ebss+CONFIG_IDLETHREAD_STACKSIZE-4) +/* The Cortex-M3 return from interrupt is unusual. We provide the following special + * address to the BX instruction. The particular value also forces a return to + * thread mode and covers state from the main stack point, the MSP (vs. the MSP). + */ + +#define EXC_RETURN 0xfffffff9 + /************************************************************************************ * Global Symbols ************************************************************************************/ @@ -82,6 +89,7 @@ */ .macro HANDLER, label, irqno + .thumb_func \label: mov r0, #\irqno b lm3s_irqcommon @@ -98,7 +106,9 @@ .type lm3s_vectors, function lm3s_vectors: + /* Processor Exceptions */ + .word IDLE_STACK /* Vector 0: Reset stack pointer */ .word __start /* Vector 1: Reset vector */ .word lm3s_nmi /* Vector 2: Non-Maskable Interrupt (NMI) */ @@ -185,17 +195,18 @@ lm3s_vectors: .text .type handlers, function + .thumb_func handlers: - HANDLER lm3s_reserved, LMSB_IRQ_RESERVED /* Unexpected/reserved vector */ - HANDLER lm3s_nmi, LMSB_IRQ_NMI /* Vector 2: Non-Maskable Interrupt (NMI) */ - HANDLER lm3s_hardfault, LMSB_IRQ_HARDFAULT /* Vector 3: Hard fault */ - HANDLER lm3s_mpu, LMSB_IRQ_MPU /* Vector 4: Memory management (MPU) */ - HANDLER lm3s_busfault, LMSB_IRQ_BUSFAULT /* Vector 5: Bus fault */ - HANDLER lm3s_usagefault, LMSB_IRQ_USAGEFAULT /* Vector 6: Usage fault */ - HANDLER lm3s_svcall, LMSB_IRQ_SVCALL /* Vector 11: SVC call */ - HANDLER lm3s_dbgmonitor, LMSB_IRQ_DBGMONITOR /* Vector 12: Debug Monitor */ - HANDLER lm3s_pendsv, LMSB_IRQ_PENDSV /* Vector 14: Penable system service request */ - HANDLER lm3s_systick, LMSB_IRQ_SYSTICK /* Vector 15: System tick */ + HANDLER lm3s_reserved, LM3S_IRQ_RESERVED /* Unexpected/reserved vector */ + HANDLER lm3s_nmi, LM3S_IRQ_NMI /* Vector 2: Non-Maskable Interrupt (NMI) */ + HANDLER lm3s_hardfault, LM3S_IRQ_HARDFAULT /* Vector 3: Hard fault */ + HANDLER lm3s_mpu, LM3S_IRQ_MPU /* Vector 4: Memory management (MPU) */ + HANDLER lm3s_busfault, LM3S_IRQ_BUSFAULT /* Vector 5: Bus fault */ + HANDLER lm3s_usagefault, LM3S_IRQ_USAGEFAULT /* Vector 6: Usage fault */ + HANDLER lm3s_svcall, LM3S_IRQ_SVCALL /* Vector 11: SVC call */ + HANDLER lm3s_dbgmonitor, LM3S_IRQ_DBGMONITOR /* Vector 12: Debug Monitor */ + HANDLER lm3s_pendsv, LM3S_IRQ_PENDSV /* Vector 14: Penable system service request */ + HANDLER lm3s_systick, LM3S_IRQ_SYSTICK /* Vector 15: System tick */ #ifdef CONFIG_ARCH_CHIP_LM3S6918 HANDLER lm3s_gpioa, LM3S_IRQ_GPIOA /* Vector 16: GPIO Port A */ @@ -245,17 +256,18 @@ handlers: * REG_R3 * REG_R2 * REG_R1 - * PSP->REG_R0 + * MSP->REG_R0 * * and R0 contains the IRQ number */ lm3s_irqcommon: + /* Complete the context save */ - mrs r1, psp /* R1=The process stack pointer */ - mov r2, r1 /* R2=Copy of the process stack pointer */ - add r2, #HW_XCPT_SIZE /* R2=PSP before the interrupt was taken */ + mrs r1, msp /* R1=The main stack pointer */ + mov r2, r1 /* R2=Copy of the main stack pointer */ + add r2, #HW_XCPT_SIZE /* R2=MSP before the interrupt was taken */ mrs r3, primask /* R3=Current PRIMASK setting */ stmdb r1!, {r2-r11} /* Save the remaining registers plus the SP value */ @@ -267,17 +279,19 @@ lm3s_irqcommon: /* If CONFIG_ARCH_INTERRUPTSTACK is defined, we will use a special interrupt * stack pointer. The way that this is done here prohibits nested interrupts! - * Otherwise, we will re-use the process stack for interrupt level processing. + * Otherwise, we will re-use the main stack for interrupt level processing. */ #ifdef CONFIG_ARCH_INTERRUPTSTACK ld sp, #up_interruptstack_base + str r1, [sp, #-4]! /* Save the MSP on the interrupt stack */ + bl up_doirq /* R0=IRQ, R1=register save (msp) */ + ldr r1, [sp, #+4]! /* Recover R1=main stack pointer */ #else - mov sp, r1 /* Use the process stack pointer */ + mov sp, r1 /* We are using the main stack pointer */ + bl up_doirq /* R0=IRQ, R1=register save (msp) */ + mov r1, sp /* Recover R1=main stack pointer */ #endif - stmdb sp!, {r1, r14} /* Save the process SP and the return LR */ - bl up_doirq /* R0=IRQ, R1=register save */ - ldmia sp!, {r1, r14} /* Recover the process SP and the return LR */ /* On return from up_doirq, R0 will hold a pointer to register context * array to use for the interrupt return. If that return value is the same @@ -287,27 +301,38 @@ lm3s_irqcommon: cmp r0, r1 beq 1f /* Branch if no context switch */ - /* We are returning from a context switch. This is different because in this - * case, the register save structure does not lie on the stack but, rather, - * is within a TCB structure. We'll have to copy some values to the stack. + /* We are returning with a pending context switch. This case is different + * because in this case, the register save structure does not lie on the + * stack but, rather, are within a TCB structure. We'll have to copy some + * values to the stack. */ - add r1, r0, #(XCPTCONTEXT_SIZE-4) /* r2=offset HW save area */ + add r1, r0, #SW_XCPT_REGS /* r2=offset 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 */ - msr psp, r1 /* New PSP */ + ldmia r0!, {r2-r11} /* Recover R4-R11 + 2 temp values */ + b 2f /* Re-join common logic */ - /* We simply need to "unwind" the same stack frame that we created */ + /* We are returning with no context switch. We simply need to "unwind" + * the same stack frame that we created + */ 1: - stmdb r0!, {r2-r11} /* Recover R4-R11 + 2 temp values */ + ldmia r1!, {r2-r11} /* Recover R4-R11 + 2 temp values */ +2: + msr msp, r1 /* Recover the return MSP value */ /* Do we need to restore interrupts? */ - tst r3, #1 /* PRIMASK it 1=1 means that interrupts are masked */ - bne 2f + tst r3, #1 /* PRIMASK bit 1=1 means that interrupts are masked */ + bne 3f cpsie i /* Restore interrupts */ -2: + + /* Always return with R14 containing the special value that will: (1) + * return to thread mode, and (2) continue to use the MSP + */ +3: + ldr r14, =EXC_RETURN /* Load the special value */ bx r14 /* And return */ .size handler, .-handlers @@ -323,9 +348,8 @@ lm3s_irqcommon: .bss .align 4 up_interruptstack: - .skip ((CONFIG_ARCH_INTERRUPTSTACK & ~3) - 4) + .skip (CONFIG_ARCH_INTERRUPTSTACK & ~3) up_interruptstack_base: - .skip 4 .size up_interruptstack, .-up_interruptstack #endif -- cgit v1.2.3