diff --git a/nuttx/arch/arm/include/armv7-m/irq.h b/nuttx/arch/arm/include/armv7-m/irq.h index 87647ee2dc..92e3063554 100644 --- a/nuttx/arch/arm/include/armv7-m/irq.h +++ b/nuttx/arch/arm/include/armv7-m/irq.h @@ -71,80 +71,33 @@ #define REG_R9 (7) /* R9 */ #define REG_R10 (8) /* R10 */ #define REG_R11 (9) /* R11 */ +#define REG_EXC_RETURN (10) /* EXC_RETURN */ +#define SW_INT_REGS (11) -#ifdef CONFIG_NUTTX_KERNEL -# define REG_EXC_RETURN (10) /* EXC_RETURN */ -# define SW_INT_REGS (11) -#else -# define SW_INT_REGS (10) -#endif +#ifdef CONFIG_ARCH_FPU /* If the MCU supports a floating point unit, then it will be necessary - * to save the state of the FPU status register and data registers on - * each context switch. These registers are not saved during interrupt - * level processing, however. So, as a consequence, floating point - * operations may NOT be performed in interrupt handlers. - * - * The FPU provides an extension register file containing 32 single- - * precision registers. These can be viewed as: - * - * - Sixteen 64-bit doubleword registers, D0-D15 - * - Thirty-two 32-bit single-word registers, S0-S31 - * S<2n> maps to the least significant half of D - * S<2n+1> maps to the most significant half of D. + * to save the state of the non-volatile registers before calling code + * that may save and overwrite them. */ -#ifdef CONFIG_ARCH_FPU -# define REG_D0 (SW_INT_REGS+0) /* D0 */ -# define REG_S0 (SW_INT_REGS+0) /* S0 */ -# define REG_S1 (SW_INT_REGS+1) /* S1 */ -# define REG_D1 (SW_INT_REGS+2) /* D1 */ -# define REG_S2 (SW_INT_REGS+2) /* S2 */ -# define REG_S3 (SW_INT_REGS+3) /* S3 */ -# define REG_D2 (SW_INT_REGS+4) /* D2 */ -# define REG_S4 (SW_INT_REGS+4) /* S4 */ -# define REG_S5 (SW_INT_REGS+5) /* S5 */ -# define REG_D3 (SW_INT_REGS+6) /* D3 */ -# define REG_S6 (SW_INT_REGS+6) /* S6 */ -# define REG_S7 (SW_INT_REGS+7) /* S7 */ -# define REG_D4 (SW_INT_REGS+8) /* D4 */ -# define REG_S8 (SW_INT_REGS+8) /* S8 */ -# define REG_S9 (SW_INT_REGS+9) /* S9 */ -# define REG_D5 (SW_INT_REGS+10) /* D5 */ -# define REG_S10 (SW_INT_REGS+10) /* S10 */ -# define REG_S11 (SW_INT_REGS+11) /* S11 */ -# define REG_D6 (SW_INT_REGS+12) /* D6 */ -# define REG_S12 (SW_INT_REGS+12) /* S12 */ -# define REG_S13 (SW_INT_REGS+13) /* S13 */ -# define REG_D7 (SW_INT_REGS+14) /* D7 */ -# define REG_S14 (SW_INT_REGS+14) /* S14 */ -# define REG_S15 (SW_INT_REGS+15) /* S15 */ -# define REG_D8 (SW_INT_REGS+16) /* D8 */ -# define REG_S16 (SW_INT_REGS+16) /* S16 */ -# define REG_S17 (SW_INT_REGS+17) /* S17 */ -# define REG_D9 (SW_INT_REGS+18) /* D9 */ -# define REG_S18 (SW_INT_REGS+18) /* S18 */ -# define REG_S19 (SW_INT_REGS+19) /* S19 */ -# define REG_D10 (SW_INT_REGS+20) /* D10 */ -# define REG_S20 (SW_INT_REGS+20) /* S20 */ -# define REG_S21 (SW_INT_REGS+21) /* S21 */ -# define REG_D11 (SW_INT_REGS+22) /* D11 */ -# define REG_S22 (SW_INT_REGS+22) /* S22 */ -# define REG_S23 (SW_INT_REGS+23) /* S23 */ -# define REG_D12 (SW_INT_REGS+24) /* D12 */ -# define REG_S24 (SW_INT_REGS+24) /* S24 */ -# define REG_S25 (SW_INT_REGS+25) /* S25 */ -# define REG_D13 (SW_INT_REGS+26) /* D13 */ -# define REG_S26 (SW_INT_REGS+26) /* S26 */ -# define REG_S27 (SW_INT_REGS+27) /* S27 */ -# define REG_D14 (SW_INT_REGS+28) /* D14 */ -# define REG_S28 (SW_INT_REGS+28) /* S28 */ -# define REG_S29 (SW_INT_REGS+29) /* S29 */ -# define REG_D15 (SW_INT_REGS+30) /* D15 */ -# define REG_S30 (SW_INT_REGS+30) /* S30 */ -# define REG_S31 (SW_INT_REGS+31) /* S31 */ -# define REG_FPSCR (SW_INT_REGS+32) /* Floating point status and control */ -# define SW_FPU_REGS (33) +# define REG_S16 (SW_INT_REGS+0) /* S16 */ +# define REG_S17 (SW_INT_REGS+1) /* S17 */ +# define REG_S18 (SW_INT_REGS+2) /* S18 */ +# define REG_S19 (SW_INT_REGS+3) /* S19 */ +# define REG_S20 (SW_INT_REGS+4) /* S20 */ +# define REG_S21 (SW_INT_REGS+5) /* S21 */ +# define REG_S22 (SW_INT_REGS+6) /* S22 */ +# define REG_S23 (SW_INT_REGS+7) /* S23 */ +# define REG_S24 (SW_INT_REGS+8) /* S24 */ +# define REG_S25 (SW_INT_REGS+9) /* S25 */ +# define REG_S26 (SW_INT_REGS+10) /* S26 */ +# define REG_S27 (SW_INT_REGS+11) /* S27 */ +# define REG_S28 (SW_INT_REGS+12) /* S28 */ +# define REG_S29 (SW_INT_REGS+13) /* S29 */ +# define REG_S30 (SW_INT_REGS+14) /* S30 */ +# define REG_S31 (SW_INT_REGS+15) /* S31 */ +# define SW_FPU_REGS (16) #else # define SW_FPU_REGS (0) #endif @@ -166,12 +119,41 @@ #define REG_R14 (SW_XCPT_REGS+5) /* R14 = LR */ #define REG_R15 (SW_XCPT_REGS+6) /* R15 = PC */ #define REG_XPSR (SW_XCPT_REGS+7) /* xPSR */ +#define HW_INT_REGS (8) + +#ifdef CONFIG_ARCH_FPU + +/* If the FPU is enabled, the hardware also saves the volatile FP registers. + */ -#define HW_XCPT_REGS (8) +# define REG_S0 (SW_XCPT_REGS+8) /* S0 */ +# define REG_S1 (SW_XCPT_REGS+9) /* S1 */ +# define REG_S2 (SW_XCPT_REGS+10) /* S2 */ +# define REG_S3 (SW_XCPT_REGS+11) /* S3 */ +# define REG_S4 (SW_XCPT_REGS+12) /* S4 */ +# define REG_S5 (SW_XCPT_REGS+13) /* S5 */ +# define REG_S6 (SW_XCPT_REGS+14) /* S6 */ +# define REG_S7 (SW_XCPT_REGS+15) /* S7 */ +# define REG_S8 (SW_XCPT_REGS+16) /* S8 */ +# define REG_S9 (SW_XCPT_REGS+17) /* S9 */ +# define REG_S10 (SW_XCPT_REGS+18) /* S10 */ +# define REG_S11 (SW_XCPT_REGS+19) /* S11 */ +# define REG_S12 (SW_XCPT_REGS+20) /* S12 */ +# define REG_S13 (SW_XCPT_REGS+21) /* S13 */ +# define REG_S14 (SW_XCPT_REGS+22) /* S14 */ +# define REG_S15 (SW_XCPT_REGS+23) /* S15 */ +# define REG_FPSCR (SW_XCPT_REGS+24) /* FPSCR */ +# define REG_FPReserved (SW_XCPT_REGS+25) /* Reserved */ +# define HW_FPU_REGS (18) +#else +# define HW_FPU_REGS (0) +#endif + +#define HW_XCPT_REGS (HW_INT_REGS + HW_FPU_REGS) #define HW_XCPT_SIZE (4 * HW_XCPT_REGS) #define XCPTCONTEXT_REGS (HW_XCPT_REGS + SW_XCPT_REGS) -#define XCPTCONTEXT_SIZE (HW_XCPT_SIZE + SW_XCPT_SIZE) +#define XCPTCONTEXT_SIZE (4 * XCPTCONTEXT_REGS) /* Alternate register names */ @@ -364,6 +346,30 @@ static inline void setipsr(uint32_t ipsr) : "memory"); } +/* Get/set CONTROL */ + +static inline uint32_t getcontrol(void) +{ + uint32_t control; + __asm__ __volatile__ + ( + "\tmrs %0, control\n" + : "=r" (control) + : + : "memory"); + return control; +} + +static inline void setcontrol(uint32_t control) +{ + __asm__ __volatile__ + ( + "\tmsr control, %0\n" + : + : "r" (control) + : "memory"); +} + #endif /* __ASSEMBLY__ */ /**************************************************************************** diff --git a/nuttx/arch/arm/src/armv7-m/exc_return.h b/nuttx/arch/arm/src/armv7-m/exc_return.h index f32fd7a066..cffbd3e2ee 100644 --- a/nuttx/arch/arm/src/armv7-m/exc_return.h +++ b/nuttx/arch/arm/src/armv7-m/exc_return.h @@ -1,8 +1,8 @@ /************************************************************************************ * arch/arm/src/armv7-m/exc_return.h * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt + * Copyright (C) 2011-2012 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 @@ -48,38 +48,65 @@ /* The processor saves an EXC_RETURN value to the LR on exception entry. The * exception mechanism relies on this value to detect when the processor has - * completed an exception handler. Bits[31:4] of an EXC_RETURN value are - * 0xfffffffX. When the processor loads a value matching this pattern to the - * PC it detects that the operation is a not a normal branch operation and, - * instead, that the exception is complete. Therefore, it starts the exception - * return sequence. Bits[3:0] of the EXC_RETURN value indicate the required - * return stack and processor mode: + * completed an exception handler. + * + * Bits [31:28] of an EXC_RETURN value are always 1. When the processor loads a + * value matching this pattern to the PC it detects that the operation is a not + * a normal branch operation and instead, that the exception is complete. + * Therefore, it starts the exception return sequence. + * + * Bits[4:0] of the EXC_RETURN value indicate the required return stack and eventual + * processor mode. The remaining bits of the EXC_RETURN value should be set to 1. + */ + +/* EXC_RETURN_BASE: Bits that are always set in an EXC_RETURN value. */ + +#define EXC_RETURN_BASE 0xffffffe1 + +/* EXC_RETURN_PROCESS_STACK: The exception saved (and will restore) the hardware + * context using the process stack pointer (if not set, the context was saved + * using the main stack pointer) + */ + +#define EXC_RETURN_PROCESS_STACK (1 << 2) + +/* EXC_RETURN_THREAD_MODE: The exception will return to thread mode (if not set, + * return stays in handler mode) + */ + +#define EXC_RETURN_THREAD_MODE (1 << 3) + +/* EXC_RETURN_STD_CONTEXT: The state saved on the stack does not include the + * volatile FP registers and FPSCR. If this bit is clear, the state does include + * these registers. */ + +#define EXC_RETURN_STD_CONTEXT (1 << 4) /* EXC_RETURN_HANDLER: Return to handler mode. Exception return gets state from * the main stack. Execution uses MSP after return. */ -#define EXC_RETURN_HANDLER 0xfffffff1 +#define EXC_RETURN_HANDLER 0xfffffff1 /* EXC_RETURN_PRIVTHR: Return to privileged thread mode. Exception return gets * state from the main stack. Execution uses MSP after return. */ -#define EXC_RETURN_PRIVTHR 0xfffffff9 +#define EXC_RETURN_PRIVTHR 0xfffffff9 /* EXC_RETURN_UNPRIVTHR: Return to unprivileged thread mode. Exception return gets * state from the process stack. Execution uses PSP after return. */ -#define EXC_RETURN_UNPRIVTHR 0xfffffffd +#define EXC_RETURN_UNPRIVTHR 0xfffffffd /* In the kernel build is not selected, then all threads run in privileged thread * mode. */ #ifdef CONFIG_NUTTX_KERNEL -# define EXC_RETURN 0xfffffff9 +# define EXC_RETURN 0xfffffff9 #endif /************************Th************************************************************ diff --git a/nuttx/arch/arm/src/armv7-m/nvic.h b/nuttx/arch/arm/src/armv7-m/nvic.h index b0b08ddefc..07d9cde6f0 100644 --- a/nuttx/arch/arm/src/armv7-m/nvic.h +++ b/nuttx/arch/arm/src/armv7-m/nvic.h @@ -373,6 +373,7 @@ #define NVIC_ISAR4 (ARMV7M_NVIC_BASE + NVIC_ISAR4_OFFSET) #define NVIC_CPACR (ARMV7M_NVIC_BASE + NVIC_CPACR_OFFSET) #define NVIC_STIR (ARMV7M_NVIC_BASE + NVIC_STIR_OFFSET) +#define NVIC_FPCCR (ARMV7M_NVIC_BASE + NVIC_FPCCR_OFFSET) #define NVIC_PID4 (ARMV7M_NVIC_BASE + NVIC_PID4_OFFSET) #define NVIC_PID5 (ARMV7M_NVIC_BASE + NVIC_PID5_OFFSET) #define NVIC_PID6 (ARMV7M_NVIC_BASE + NVIC_PID6_OFFSET) diff --git a/nuttx/arch/arm/src/armv7-m/up_exception.S b/nuttx/arch/arm/src/armv7-m/up_exception.S new file mode 100644 index 0000000000..091542ea36 --- /dev/null +++ b/nuttx/arch/arm/src/armv7-m/up_exception.S @@ -0,0 +1,236 @@ +/************************************************************************************ + * arch/arm/src/stm32/up_exception.S + * arch/arm/src/chip/up_exception.S + * + * Copyright (C) 2009-2012 Gregory Nutt. All rights reserved. + * Copyright (C) 2012 Michael Smith. 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 "exc_return.h" + +#include "chip.h" + +/************************************************************************************ + * Global Symbols + ************************************************************************************/ + + .globl exception_common + + .syntax unified + .thumb + .file "up_exception.S" + +/************************************************************************************ + * .text + ************************************************************************************/ + +/* Common exception handling logic. On entry here, the return stack is on either + * the PSP or the MSP and looks like the following: + * + * REG_XPSR + * REG_R15 + * REG_R14 + * REG_R12 + * REG_R3 + * REG_R2 + * REG_R1 + * MSP->REG_R0 + * + * And + * IPSR contains the IRQ number + * R14 Contains the EXC_RETURN value + * We are in handler mode and the current SP is the MSP + * + * If CONFIG_ARCH_FPU is defined, the volatile FP registers and FPSCR are on the + * return stack immediately above REG_XPSR. + */ + + .text + .type exception_common, function + .thumb_func +exception_common: + + mrs r0, ipsr /* R0=exception number */ + + /* Complete the context save */ + + /* The EXC_RETURN value tells us whether the context is on the MSP or PSP */ + + tst r14, #EXC_RETURN_PROCESS_STACK /* nonzero if context on process stack */ + ite eq /* next two instructions conditional */ + mrseq r1, msp /* R1=The main stack pointer */ + mrsne r1, psp /* R1=The process stack pointer */ + + mov r2, r1 /* R2=Copy of the main/process stack pointer */ + add r2, #HW_XCPT_SIZE /* R2=MSP/PSP before the interrupt was taken */ + /* (ignoring the xPSR[9] alignment bit) */ + mrs r3, primask /* R3=Current PRIMASK setting */ + +#ifdef CONFIG_ARCH_FPU + + /* Save the non-volatile FP registers here. + * + * This routine is the only point where we can save these registers; either before + * or after calling up_doirq. The compiler is free to use them at any time as long + * as they are restored before returning, so we can't assume that we can get at the + * true values of these registers in any routine called from here. + * + * XXX we could do all this saving lazily on the context switch side if we knew where to put + * the registers. + */ + + vstmdb r1!, {s16-s31} /* Save the non-volatile FP context */ + +#endif + + stmdb r1!, {r2-r11,r14} /* Save the remaining registers plus the SP/PRIMASK values */ + + /* Disable interrupts, select the stack to use for interrupt handling + * and call up_doirq to handle the interrupt + */ + + cpsid i /* Disable further interrupts */ + + /* 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 use the stack that was current when the interrupt was taken. + */ + +#if CONFIG_ARCH_INTERRUPTSTACK > 3 + ldr sp, =g_intstackbase + push r1 /* Save the MSP on the interrupt stack */ + bl up_doirq /* R0=IRQ, R1=register save area on stack */ + pop r1 /* Recover R1=main stack pointer */ +#else + msr msp, r1 /* We are using the main stack pointer */ + bl up_doirq /* R0=IRQ, R1=register save area on stack */ + mrs r1, msp /* Recover R1=main stack pointer */ +#endif + + /* 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 + * as current stack pointer, then things are relatively easy. + */ + + cmp r0, r1 /* Context switch? */ + beq 1f /* Branch if no context switch */ + + /* 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 within a TCB structure. We'll have to copy some + * values to the stack. + */ + + /* Copy the hardware-saved context to the stack, and restore the software + * saved context directly. + * + * XXX In the normal case, it appears that this entire operation is unnecessary; + * context switch time would be improved if we could work out when the stack + * is dirty and avoid the work... + */ + add r1, r0, #SW_XCPT_SIZE /* R1=Address of HW save area in reg array */ + ldmia r1!, {r4-r11} /* Fetch eight registers in HW save area */ +#ifdef CONFIG_ARCH_FPU + vldmia r1!, {s0-s15} /* Fetch sixteen FP registers in HW save area */ + ldmia r1, {r2-r3} /* Fetch FPSCR and Reserved in HW save area */ +#endif + ldr r1, [r0, #(4*REG_SP)] /* R1=Value of SP before interrupt */ +#ifdef CONFIG_ARCH_FPU + stmdb r1!, {r2-r3} /* Store FPSCR and Reserved on the return stack */ + vstmdb r1!, {s0-s15} /* Store sixteen FP registers on the return stack */ +#endif + stmdb r1!, {r4-r11} /* Store eight registers on the return stack */ + ldmia r0!, {r2-r11,r14} /* Recover R4-R11, r14 + 2 temp values */ +#ifdef CONFIG_ARCH_FPU + vldmia r0, {s16-s31} /* Recover S16-S31 */ +#endif + + b 2f /* Re-join common logic */ + +1: + /* We are returning with no context switch. We simply need to "unwind" + * the same stack frame that we created at entry. + */ + + ldmia r1!, {r2-r11,r14} /* Recover R4-R11, r14 + 2 temp values */ +#ifdef CONFIG_ARCH_FPU + vldmia r1!, {s16-s31} /* Recover S16-S31 */ +#endif + +2: + /* The EXC_RETURN value tells us whether we are returning on the MSP or PSP + */ + + tst r14, #EXC_RETURN_PROCESS_STACK /* nonzero if context on process stack */ + ite eq /* next two instructions conditional */ + msreq msp, r1 /* R1=The main stack pointer */ + msrne psp, r1 /* R1=The process stack pointer */ + + /* Restore the interrupt state */ + + msr primask, r3 /* Restore interrupts */ + + /* Always return with R14 containing the special value that will: (1) + * return to thread mode, and (2) select the correct stack. + */ + + bx r14 /* And return */ + + .size exception_common, .-exception_common + +/************************************************************************************ + * Name: up_interruptstack/g_intstackbase + * + * Description: + * Shouldn't happen + * + ************************************************************************************/ + +#if CONFIG_ARCH_INTERRUPTSTACK > 3 + .bss + .global g_intstackbase + .align 4 +up_interruptstack: + .skip (CONFIG_ARCH_INTERRUPTSTACK & ~3) +g_intstackbase: + .size up_interruptstack, .-up_interruptstack +#endif + + .end + diff --git a/nuttx/arch/arm/src/armv7-m/up_initialstate.c b/nuttx/arch/arm/src/armv7-m/up_initialstate.c index 6a13f038b1..81a4dc9eab 100644 --- a/nuttx/arch/arm/src/armv7-m/up_initialstate.c +++ b/nuttx/arch/arm/src/armv7-m/up_initialstate.c @@ -1,8 +1,8 @@ /**************************************************************************** * arch/arm/src/armv7-m/up_initialstate.c * - * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt + * Copyright (C) 2009, 2011-2 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 @@ -124,8 +124,43 @@ void up_initial_state(_TCB *tcb) #ifdef CONFIG_NXFLAT tcb->entry.main = (main_t)((uint32_t)tcb->entry.main | 1); #endif +#endif /* CONFIG_PIC */ + +#ifdef CONFIG_ARMV7M_CMNVECTOR + /* Set privileged- or unprivileged-mode, depending on how NuttX is + * configured and what kind of thread is being started. + * + * If the kernel build is not selected, then all threads run in + * privileged thread mode. + * + * If FPU support is not configured, set the bit that indicates that + * the context does not include the volatile FP registers. + */ + + xcp->regs[REG_EXC_RETURN] = EXC_RETURN_BASE | EXC_RETURN_THREAD_MODE; + +#ifndef CONFIG_ARCH_FPU + + xcp->regs[REG_EXC_RETURN] |= EXC_RETURN_STD_CONTEXT; + +#else + + xcp->regs[REG_FPSCR] = 0; // XXX initial FPSCR should be configurable + xcp->regs[REG_FPReserved] = 0; + +#endif + +#ifdef CONFIG_NUTTX_KERNEL + if ((tcb->flags & TCB_FLAG_TTYPE_MASK) != TCB_FLAG_TTYPE_KERNEL) + { + /* It is a normal task or a pthread. Set user mode */ + + xcp->regs[REG_EXC_RETURN] = EXC_RETURN_PROCESS_STACK; + } #endif +#else /* CONFIG_ARMV7M_CMNVECTOR */ + /* Set privileged- or unprivileged-mode, depending on how NuttX is * configured and what kind of thread is being started. * @@ -147,11 +182,11 @@ void up_initial_state(_TCB *tcb) xcp->regs[REG_EXC_RETURN] = EXC_RETURN_UNPRIVTHR; } #endif +#endif /* CONFIG_ARMV7M_CMNVECTOR */ /* Enable or disable interrupts, based on user configuration */ -# ifdef CONFIG_SUPPRESS_INTERRUPTS +#ifdef CONFIG_SUPPRESS_INTERRUPTS xcp->regs[REG_PRIMASK] = 1; -# endif +#endif } - diff --git a/nuttx/arch/arm/src/armv7-m/up_svcall.c b/nuttx/arch/arm/src/armv7-m/up_svcall.c index 85430c0adf..949efb1af0 100644 --- a/nuttx/arch/arm/src/armv7-m/up_svcall.c +++ b/nuttx/arch/arm/src/armv7-m/up_svcall.c @@ -1,8 +1,8 @@ /**************************************************************************** * arch/arm/src/armv7-m/up_svcall.c * - * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt + * Copyright (C) 2009, 2011-2012 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 @@ -265,14 +265,14 @@ int up_svcall(int irq, FAR void *context) * R1 = saveregs * * In this case, we simply need to copy the current regsters to the - * save regiser space references in the saved R1 and return. + * save register space references in the saved R1 and return. */ case SYS_save_context: { DEBUGASSERT(regs[REG_R1] != 0); memcpy((uint32_t*)regs[REG_R1], regs, XCPTCONTEXT_SIZE); -#ifdef CONFIG_ARCH_FPU +#if defined(CONFIG_ARCH_FPU) && !defined(CONFIG_ARMV7M_CMNVECTOR) up_savefpu((uint32_t*)regs[REG_R1]); #endif } @@ -324,7 +324,7 @@ int up_svcall(int irq, FAR void *context) } break; - /* This is not an architecture-specify system call. If NuttX is built + /* This is not an architecture-specific system call. If NuttX is built * as a standalone kernel with a system call interface, then all of the * additional system calls must be handled as in the default case. */ diff --git a/nuttx/arch/arm/src/armv7-m/up_vectors.c b/nuttx/arch/arm/src/armv7-m/up_vectors.c new file mode 100644 index 0000000000..b85ac92462 --- /dev/null +++ b/nuttx/arch/arm/src/armv7-m/up_vectors.c @@ -0,0 +1,95 @@ +/**************************************************************************** + * arch/arm/src/armv7-m/up_vectors.c + * + * Copyright (C) 2012 Michael Smith. All rights reserved. + * + * 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 "chip.h" + +/************************************************************************************ + * Preprocessor Definitions + ************************************************************************************/ + +#define IDLE_STACK ((unsigned)&_ebss+CONFIG_IDLETHREAD_STACKSIZE-4) + +#ifndef ARMV7M_PERIPHERAL_INTERRUPTS +# error ARMV7M_PERIPHERAL_INTERRUPTS must be defined to the number of I/O interrupts to be supported +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/* Chip-specific entrypoint */ + +extern void __start(void); + +/* Common exception entrypoint */ + +extern void exception_common(void); + +/************************************************************************************ + * Public data + ************************************************************************************/ + +/* Provided by the linker script to indicate the end of the BSS */ + +extern char _ebss; + +/* The v7m vector table consists of an array of function pointers, with the first + * slot (vector zero) used to hold the initial stack pointer. + * + * As all exceptions (interrupts) are routed via exception_common, we just need to + * fill this array with pointers to it. + * + * Note that the [ ... ] desginated initialiser is a GCC extension. + */ + +unsigned _vectors[] __attribute__((section(".vectors"))) = + { + /* Initial stack */ + + IDLE_STACK, + + /* Reset exception handler */ + + (unsigned)&__start, + + /* Vectors 2 - n point directly at the generic handler */ + + [2 ... (15 + ARMV7M_PERIPHERAL_INTERRUPTS)] = (unsigned)&exception_common + }; diff --git a/nuttx/arch/arm/src/common/up_internal.h b/nuttx/arch/arm/src/common/up_internal.h index 79dc3f7d9c..f0f3d06ef9 100644 --- a/nuttx/arch/arm/src/common/up_internal.h +++ b/nuttx/arch/arm/src/common/up_internal.h @@ -100,7 +100,7 @@ */ #if defined(CONFIG_ARCH_CORTEXM3) || defined(CONFIG_ARCH_CORTEXM4) -# ifdef CONFIG_ARCH_FPU +# if defined(CONFIG_ARCH_FPU) && !defined(CONFIG_ARMV7M_CMNVECTOR) # define up_savestate(regs) \ do { \ up_copystate(regs, (uint32_t*)current_regs); \ diff --git a/nuttx/arch/arm/src/stm32/Make.defs b/nuttx/arch/arm/src/stm32/Make.defs index 213ac0be99..351680685f 100644 --- a/nuttx/arch/arm/src/stm32/Make.defs +++ b/nuttx/arch/arm/src/stm32/Make.defs @@ -33,7 +33,11 @@ # ############################################################################ +ifeq ($(CONFIG_ARMV7M_CMNVECTOR),y) +HEAD_ASRC = +else HEAD_ASRC = stm32_vectors.S +endif CMN_ASRCS = up_saveusercontext.S up_fullcontextrestore.S up_switchcontext.S CMN_CSRCS = up_assert.c up_blocktask.c up_copystate.c \ @@ -44,6 +48,11 @@ CMN_CSRCS = up_assert.c up_blocktask.c up_copystate.c \ up_schedulesigaction.c up_sigdeliver.c up_unblocktask.c \ up_usestack.c up_doirq.c up_hardfault.c up_svcall.c +ifeq ($(CONFIG_ARMV7M_CMNVECTOR),y) +CMN_ASRCS += up_exception.S stm32_vectors.S +CMN_CSRCS += up_vectors.c +endif + ifeq ($(CONFIG_DEBUG_STACK),y) CMN_CSRCS += up_checkstack.c endif @@ -59,6 +68,10 @@ CHIP_CSRCS = stm32_allocateheap.c stm32_start.c stm32_rcc.c stm32_lse.c \ stm32_spi.c stm32_usbdev.c stm32_sdio.c stm32_tim.c stm32_i2c.c \ stm32_waste.c +ifeq ($(CONFIG_ARMV7M_CMNVECTOR),y) +CHIP_ASRCS += stm32_vectors.S +endif + ifneq ($(CONFIG_IDLE_CUSTOM),y) CHIP_CSRCS += stm32_idle.c endif diff --git a/nuttx/arch/arm/src/stm32/chip.h b/nuttx/arch/arm/src/stm32/chip.h index 5a560edfe1..3bceac77b0 100644 --- a/nuttx/arch/arm/src/stm32/chip.h +++ b/nuttx/arch/arm/src/stm32/chip.h @@ -49,7 +49,7 @@ /* Include the chip pin configuration file */ #if defined(CONFIG_STM32_STM32F10XX) -# if defined(CONFIG_ARCH_CHIP_STM32F103ZET6) +# if defined(CONFIG_ARCH_CHIP_STM32F103ZET6) # include "chip/stm32f103ze_pinmap.h" # elif defined(CONFIG_ARCH_CHIP_STM32F103RET6) # include "chip/stm32f103re_pinmap.h" @@ -63,7 +63,21 @@ #elif defined(CONFIG_STM32_STM32F40XX) # include "chip/stm32f40xxx_pinmap.h" #else -# error "Unsupported STM32 chip" +# error "No pinmap file for this STM32 chip" +#endif + +/* If the common ARMv7-M vector handling logic is used, then include the + * required vector definitions as well. + */ + +#ifdef CONFIG_ARMV7M_CMNVECTOR +# if defined(CONFIG_STM32_STM32F10XX) +# include "chip/stm32f10xxx_vectors.h" +# elif defined(CONFIG_STM32_STM32F40XX) +# include "chip/stm32f40xxx_vectors.h" +# else +# error "No vector file for this STM32 family" +# endif #endif /* Include only the mchip emory map. */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f10xxx_vectors.h b/nuttx/arch/arm/src/stm32/chip/stm32f10xxx_vectors.h index 0d6b965d29..8ce98c9887 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f10xxx_vectors.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f10xxx_vectors.h @@ -36,7 +36,6 @@ /************************************************************************************ * Pre-processor definitions ************************************************************************************/ - /* This file is included by stm32_vectors.S. It provides the macro VECTOR that * supplies ach STM32F10xxx vector in terms of a (lower-case) ISR label and an * (upper-case) IRQ number as defined in arch/arm/include/stm32/stm32f10xxx_irq.h. @@ -46,6 +45,18 @@ #ifdef CONFIG_STM32_CONNECTIVITY_LINE +/* If the common ARMv7-M vector handling is used, then all it needs is the following + * definition that provides the number of supported vectors. + */ + +#ifdef CONFIG_ARMV7M_CMNVECTOR + +/* Reserve 68 interrupt table entries for I/O interrupts. */ + +# define ARMV7M_PERIPHERAL_INTERRUPTS 68 + +#else + VECTOR(stm32_wwdg, STM32_IRQ_WWDG) /* Vector 16+0: Window Watchdog interrupt */ VECTOR(stm32_pvd, STM32_IRQ_PVD) /* Vector 16+1: PVD through EXTI Line detection interrupt */ VECTOR(stm32_tamper, STM32_IRQ_TAMPER) /* Vector 16+2: Tamper interrupt */ @@ -107,6 +118,19 @@ VECTOR(stm32_can2rx1, STM32_IRQ_CAN2RX1) /* Vector 16+65: CAN2 RX1 inter VECTOR(stm32_can2sce, STM32_IRQ_CAN2SCE) /* Vector 16+66: CAN2 SCE interrupt */ VECTOR(stm32_otgfs, STM32_IRQ_OTGFS) /* Vector 16+67: USB On The Go FS global interrupt */ +#endif /* CONFIG_ARMV7M_CMNVECTOR */ +#else /* CONFIG_STM32_CONNECTIVITY_LINE */ + +/* If the common ARMv7-M vector handling is used, then all it needs is the following + * definition that provides the number of supported vectors. + */ + +#ifdef CONFIG_ARMV7M_CMNVECTOR + +/* Reserve 60 interrupt table entries for I/O interrupts. */ + +# define ARMV7M_PERIPHERAL_INTERRUPTS 60 + #else VECTOR(stm32_wwdg, STM32_IRQ_WWDG) /* Vector 16+0: Window Watchdog interrupt */ @@ -169,4 +193,6 @@ VECTOR(stm32_dma2ch1, STM32_IRQ_DMA2CH1) /* Vector 16+56: DMA2 Channel 1 VECTOR(stm32_dma2ch2, STM32_IRQ_DMA2CH2) /* Vector 16+57: DMA2 Channel 2 global interrupt */ VECTOR(stm32_dma2ch3, STM32_IRQ_DMA2CH3) /* Vector 16+58: DMA2 Channel 3 global interrupt */ VECTOR(stm32_dma2ch45, STM32_IRQ_DMA2CH45) /* Vector 16+59: DMA2 Channel 4&5 global interrupt */ -#endif + +#endif /* CONFIG_ARMV7M_CMNVECTOR */ +#endif /* CONFIG_STM32_CONNECTIVITY_LINE */ diff --git a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h index 2bc949fec6..0965a7224a 100644 --- a/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h +++ b/nuttx/arch/arm/src/stm32/chip/stm32f40xxx_vectors.h @@ -1,7 +1,7 @@ /************************************************************************************ * arch/arm/src/stm32/chip/stm32f40xxx_vectors.h * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -44,6 +44,18 @@ * the interrupt vectors and handlers in their final form. */ +/* If the common ARMv7-M vector handling is used, then all it needs is the following + * definition that provides the number of supported vectors. + */ + +#ifdef CONFIG_ARMV7M_CMNVECTOR + +/* Reserve 84 interrupt table entries for I/O interrupts. */ + +# define ARMV7M_PERIPHERAL_INTERRUPTS 82 + +#else + VECTOR(stm32_wwdg, STM32_IRQ_WWDG) /* Vector 16+0: Window Watchdog interrupt */ VECTOR(stm32_pvd, STM32_IRQ_PVD) /* Vector 16+1: PVD through EXTI Line detection interrupt */ VECTOR(stm32_tamper, STM32_IRQ_TAMPER) /* Vector 16+2: Tamper and time stamp interrupts */ @@ -127,3 +139,4 @@ VECTOR(stm32_cryp, STM32_IRQ_CRYP) /* Vector 16+79: CRYP crypto gl VECTOR(stm32_hash, STM32_IRQ_HASH) /* Vector 16+80: Hash and Rng global interrupt */ VECTOR(stm32_fpu, STM32_IRQ_FPU) /* Vector 16+81: FPU global interrupt */ +#endif /* CONFIG_ARMV7M_CMNVECTOR */ diff --git a/nuttx/arch/arm/src/stm32/stm32_start.c b/nuttx/arch/arm/src/stm32/stm32_start.c index b1b2af297b..5f4a4c9c94 100644 --- a/nuttx/arch/arm/src/stm32/stm32_start.c +++ b/nuttx/arch/arm/src/stm32/stm32_start.c @@ -71,22 +71,55 @@ * Public Functions ****************************************************************************/ - /**************************************************************************** +/**************************************************************************** * Name: stm32_fpuconfig * * Description: - * Configure the FPU. The the MCU has an FPU, then enable full access - * to coprocessors CP10 and CP11. + * Configure the FPU. + * + * 1. The MCU has an FPU, then enable full access to coprocessors CP10 and + * CP11. + * + * if the common ARMv-7M interrupt vector handling is used (via + * CONFIG_ARMV7M_CMNVECTOR=y), then lazy floating point register saving is + * disabled and this function will also: + * + * 2. Ensure that FPCCR.LSPEN is disabled, so that we don't have to contend + * with the lazy FP context save behaviour. Clear FPCCR.ASPEN since we + * are going to turn on CONTROL.FPCA for all contexts. + * + * 3. Set CONTROL.FPCA so that we always get the extended context frame + * with the volatile FP registers stacked above the basic context. * ****************************************************************************/ #ifdef CONFIG_ARCH_FPU +#ifdef CONFIG_ARMV7M_CMNVECTOR + +# define stm32_fpuconfig() \ +{ \ + uint32_t regval;\ + regval = getcontrol(); \ + regval |= 1<<2; \ + setcontrol(regval); \ + regval = getreg32(NVIC_FPCCR); \ + regval &= ~((1 << 31) | (1 << 30)); \ + putreg32(regval, NVIC_FPCCR); \ + regval = getreg32(NVIC_CPACR); \ + regval |= ((3 << (2*10)) | (3 << (2*11))); \ + putreg32(regval, NVIC_CPACR); \ +} + +#else + # define stm32_fpuconfig() \ { \ uint32_t regval = getreg32(NVIC_CPACR); \ regval |= ((3 << (2*10)) | (3 << (2*11))); \ putreg32(regval, NVIC_CPACR); \ } +#endif + #else # define stm32_fpuconfig() #endif diff --git a/nuttx/arch/arm/src/stm32/stm32_vectors.S b/nuttx/arch/arm/src/stm32/stm32_vectors.S index ee4220ccc5..91f530f058 100644 --- a/nuttx/arch/arm/src/stm32/stm32_vectors.S +++ b/nuttx/arch/arm/src/stm32/stm32_vectors.S @@ -2,7 +2,7 @@ * arch/arm/src/stm32/stm32_vectors.S * arch/arm/src/chip/stm32_vectors.S * - * Copyright (C) 2009-2011 Gregory Nutt. All rights reserved. + * Copyright (C) 2009-2012 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -40,14 +40,23 @@ #include + #include #include "chip.h" /************************************************************************************ - * Preprocessor Definitions + * Configuration ************************************************************************************/ +/* Check if common ARMv7 interrupt vectoring is used (see + * arch/arm/src/armv7-m/up_vectors.S) + */ + +#ifndef CONFIG_ARMV7M_CMNVECTOR +/************************************************************************************ + * Preprocessor Definitions + ************************************************************************************/ /* Memory Map: * * 0x0800:0000 - Beginning of FLASH. Address of vectors (if not using bootloader) @@ -334,6 +343,7 @@ up_interruptstack: g_intstackbase: .size up_interruptstack, .-up_interruptstack #endif +#endif /* CONFIG_ARMV7M_CMNVECTOR */ /************************************************************************************ * .rodata diff --git a/nuttx/configs/stm3240g-eval/README.txt b/nuttx/configs/stm3240g-eval/README.txt index 3c4f7e77c4..af6e71dadb 100755 --- a/nuttx/configs/stm3240g-eval/README.txt +++ b/nuttx/configs/stm3240g-eval/README.txt @@ -16,6 +16,8 @@ Contents - Ethernet - PWM - CAN + - FPU + - STM3240G-EVAL-specific Configuration Options - Configurations Development Environment @@ -289,6 +291,57 @@ Configuration Options: CONFIG_CAN_REGDEBUG - If CONFIG_DEBUG is set, this will generate an dump of all CAN registers. +FPU +=== + +FPU Configuration Options +------------------------- + +There are two version of the FPU support built into the STM32 port. + +1. Lazy Floating Point Register Save. + + This is an untested implementation that saves and restores FPU registers + only on context switches. This means: (1) floating point registers are + not stored on each context switch and, hence, possibly better interrupt + performance. But, (2) since floating point registers are not saved, + you cannot use floating point operations within interrupt handlers. + + This logic can be enabled by simply adding the following to your .config + file: + + CONFIG_ARCH_FPU=y + +2. Non-Lazy Floating Point Register Save + + Mike Smith has contributed an extensive re-write of the ARMv7-M exception + handling logic. This includes verified support for the FPU. These changes + have not yet been incorporated into the mainline and are still considered + experimental. These FPU logic can be enabled with: + + CONFIG_ARCH_FPU=y + CONFIG_ARMV7M_CMNVECTOR=y + + You will probably also changes to the ld.script in if this option is selected. + This should work: + + -ENTRY(_stext) + +ENTRY(__start) /* Treat __start as the anchor for dead code stripping */ + +EXTERN(_vectors) /* Force the vectors to be included in the output */ + +CFLAGS +------ + +To used the FPU, you will also have to modify the CFLAGS to enable compiler +support for the ARMv7-M FPU. As of this writing, there are not many GCC +toolchains that will support the ARMv7-M FPU. + +As a minimum, you will need to CFLAG options to (1) enable hardware floating +point code generation, and to (2) select the FPU implementation. You might +try something like the following in the Make.defs file: + +ARCHCPUFLAGS = -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard + STM3240G-EVAL-specific Configuration Options ============================================ diff --git a/nuttx/configs/stm32f4discovery/README.txt b/nuttx/configs/stm32f4discovery/README.txt index 7492d03b02..31b950bd6e 100755 --- a/nuttx/configs/stm32f4discovery/README.txt +++ b/nuttx/configs/stm32f4discovery/README.txt @@ -15,6 +15,7 @@ Contents - PWM - UARTs - Timer Inputs/Outputs + - FPU - STM32F4Discovery-specific Configuration Options - Configurations @@ -308,6 +309,57 @@ If enabled (by setting CONFIG_QENCODER=y), then quadrature encoder will user TIM2 (see nsh/defconfig) and input pins PA15, and PA1 for CH1 and CH2, respectively (see include board.h). +FPU +=== + +FPU Configuration Options +------------------------- + +There are two version of the FPU support built into the STM32 port. + +1. Lazy Floating Point Register Save. + + This is an untested implementation that saves and restores FPU registers + only on context switches. This means: (1) floating point registers are + not stored on each context switch and, hence, possibly better interrupt + performance. But, (2) since floating point registers are not saved, + you cannot use floating point operations within interrupt handlers. + + This logic can be enabled by simply adding the following to your .config + file: + + CONFIG_ARCH_FPU=y + +2. Non-Lazy Floating Point Register Save + + Mike Smith has contributed an extensive re-write of the ARMv7-M exception + handling logic. This includes verified support for the FPU. These changes + have not yet been incorporated into the mainline and are still considered + experimental. These FPU logic can be enabled with: + + CONFIG_ARCH_FPU=y + CONFIG_ARMV7M_CMNVECTOR=y + + You will probably also changes to the ld.script in if this option is selected. + This should work: + + -ENTRY(_stext) + +ENTRY(__start) /* Treat __start as the anchor for dead code stripping */ + +EXTERN(_vectors) /* Force the vectors to be included in the output */ + +CFLAGS +------ + +To used the FPU, you will also have to modify the CFLAGS to enable compiler +support for the ARMv7-M FPU. As of this writing, there are not many GCC +toolchains that will support the ARMv7-M FPU. + +As a minimum, you will need to CFLAG options to (1) enable hardware floating +point code generation, and to (2) select the FPU implementation. You might +try something like the following in the Make.defs file: + +ARCHCPUFLAGS = -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard + STM32F4Discovery-specific Configuration Options ===============================================