From 5bbce5453fee636e7fbb087ffee7691edfd25318 Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 19 May 2009 17:54:01 +0000 Subject: Move share-able Cortex-M3 file from lm3s subdirectory git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@1797 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/src/common/up_internal.h | 41 ++++++-- nuttx/arch/arm/src/cortexm3/up_context.S | 132 ++++++++++++++++++++++++++ nuttx/arch/arm/src/cortexm3/up_hardfault.c | 141 ++++++++++++++++++++++++++++ nuttx/arch/arm/src/cortexm3/up_svcall.c | 145 ++++++++++++++++++++++++++++ nuttx/arch/arm/src/lm3s/Make.defs | 10 +- nuttx/arch/arm/src/lm3s/lm3s_context.S | 133 -------------------------- nuttx/arch/arm/src/lm3s/lm3s_hardfault.c | 142 ---------------------------- nuttx/arch/arm/src/lm3s/lm3s_internal.h | 41 -------- nuttx/arch/arm/src/lm3s/lm3s_irq.c | 4 +- nuttx/arch/arm/src/lm3s/lm3s_svcall.c | 146 ----------------------------- 10 files changed, 457 insertions(+), 478 deletions(-) create mode 100644 nuttx/arch/arm/src/cortexm3/up_context.S create mode 100644 nuttx/arch/arm/src/cortexm3/up_hardfault.c create mode 100644 nuttx/arch/arm/src/cortexm3/up_svcall.c delete mode 100644 nuttx/arch/arm/src/lm3s/lm3s_context.S delete mode 100644 nuttx/arch/arm/src/lm3s/lm3s_hardfault.c delete mode 100644 nuttx/arch/arm/src/lm3s/lm3s_svcall.c (limited to 'nuttx') diff --git a/nuttx/arch/arm/src/common/up_internal.h b/nuttx/arch/arm/src/common/up_internal.h index d5c350e1e..2fd44bb93 100644 --- a/nuttx/arch/arm/src/common/up_internal.h +++ b/nuttx/arch/arm/src/common/up_internal.h @@ -118,6 +118,26 @@ extern uint32 g_heapbase; #if CONFIG_ARCH_INTERRUPTSTACK > 3 extern uint32 g_userstack; #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 */ #endif /**************************************************************************** @@ -134,25 +154,28 @@ extern uint32 g_userstack; extern void up_boot(void); extern void up_copystate(uint32 *dest, uint32 *src); -extern void up_dataabort(uint32 *regs); extern void up_decodeirq(uint32 *regs); -#ifdef CONFIG_ARCH_CORTEXM3 -extern uint32 *up_doirq(int irq, uint32 *regs); -#else -extern void up_doirq(int irq, uint32 *regs); -#endif extern void up_fullcontextrestore(uint32 *regs) __attribute__ ((noreturn)); extern void up_irqinitialize(void); -extern void up_prefetchabort(uint32 *regs); extern int up_saveusercontext(uint32 *regs); extern void up_sigdeliver(void); -extern void up_syscall(uint32 *regs); extern int up_timerisr(int irq, uint32 *regs); -extern void up_undefinedinsn(uint32 *regs); extern void up_lowputc(char ch); extern void up_puts(const char *str); extern void up_lowputs(const char *str); +#ifdef CONFIG_ARCH_CORTEXM3 +extern uint32 *up_doirq(int irq, uint32 *regs); +extern int up_svcall(int irq, FAR void *context); +extern int up_hardfault(int irq, FAR void *context); +#else +extern void up_doirq(int irq, uint32 *regs); +extern void up_dataabort(uint32 *regs); +extern void up_prefetchabort(uint32 *regs); +extern void up_syscall(uint32 *regs); +extern void up_undefinedinsn(uint32 *regs); +#endif + /* Defined in up_vectors.S */ extern void up_vectorundefinsn(void); diff --git a/nuttx/arch/arm/src/cortexm3/up_context.S b/nuttx/arch/arm/src/cortexm3/up_context.S new file mode 100644 index 000000000..ab3018d6f --- /dev/null +++ b/nuttx/arch/arm/src/cortexm3/up_context.S @@ -0,0 +1,132 @@ +/************************************************************************************ + * arch/arm/src/cortexm3/up_context.S + * + * 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 "nvic.h" + +/************************************************************************************ + * Preprocessor Definitions + ************************************************************************************/ + +/************************************************************************************ + * Global Symbols + ************************************************************************************/ + + .syntax unified + .thumb + .file "up_context.S" + +/************************************************************************************ + * Macros + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************** + * Name: up_saveusercontext + * + * Description: + * Save the current thread context. Full prototype is: + * + * int up_saveusercontext(uint32 *regs); + * + * Return: + * 0: Normal return + * 1: Context switch return + * + **************************************************************************/ + + .text + .thumb_func + .globl up_saveusercontext + .type up_saveusercontext, function +up_saveusercontext: + + /* Perform the System call with R0=0 and R1=regs */ + + mov r1, r0 /* R1: regs */ + mov r0, #0 /* R0: 0 means save context (also return value) */ + 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 + * force R0 to be 1. + */ + + add r2, r1, #(4*REG_R0) + mov r3, #1 + str r3, [r2, #0] + bx lr /* "normal" return with r0=0 or + * context switch with r0=1 */ + .size up_saveusercontext, .-up_saveusercontext + + +/************************************************************************** + * Name: up_fullcontextrestore + * + * Description: + * Restore the current thread context. Full prototype is: + * + * void up_fullcontextrestore(uint32 *regs) __attribute__ ((noreturn)); + * + * Return: + * None + * + **************************************************************************/ + + .thumb_func + .globl up_fullcontextrestore + .type up_fullcontextrestore, function +up_fullcontextrestore: + + /* Perform the System call with R0=1 and R1=regs */ + + mov r1, r0 /* R1: regs */ + mov r0, #1 /* R0: 1 means restore context */ + svc 0 /* Force synchronous SVCall (or Hard Fault) */ + + /* This call should not return */ + + bx lr /* Unnecessary ... will not return */ + .size up_fullcontextrestore, .-up_fullcontextrestore + .end + diff --git a/nuttx/arch/arm/src/cortexm3/up_hardfault.c b/nuttx/arch/arm/src/cortexm3/up_hardfault.c new file mode 100644 index 000000000..fdd405130 --- /dev/null +++ b/nuttx/arch/arm/src/cortexm3/up_hardfault.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * arch/arm/src/cortexm3/up_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 "nvic.h" +#include "up_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: up_hardfault + * + * Description: + * This is Hard Fault exception handler. It also catches SVC call + * exceptions that are performed in bad contexts. + * + ****************************************************************************/ + +int up_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) + { + sllvdbg("Forward SVCall\n"); + return up_svcall(irq, context); + } + } + + (void)irqsave(); + dbg("PANIC!!! Hard fault: %08x\n", getreg32(NVIC_HFAULTS)); + PANIC(OSERR_UNEXPECTEDISR); + return OK; +} diff --git a/nuttx/arch/arm/src/cortexm3/up_svcall.c b/nuttx/arch/arm/src/cortexm3/up_svcall.c new file mode 100644 index 000000000..3a346881f --- /dev/null +++ b/nuttx/arch/arm/src/cortexm3/up_svcall.c @@ -0,0 +1,145 @@ +/**************************************************************************** + * arch/arm/src/cortexm3/up_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 "up_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#undef DEBUG_SVCALL /* Define to debug SVCall */ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_svcall + * + * Description: + * This is SVCall exception handler that performs context switching + * + ****************************************************************************/ + +int up_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/Make.defs b/nuttx/arch/arm/src/lm3s/Make.defs index 3e0ab3488..42bb065d2 100644 --- a/nuttx/arch/arm/src/lm3s/Make.defs +++ b/nuttx/arch/arm/src/lm3s/Make.defs @@ -35,18 +35,18 @@ HEAD_ASRC = lm3s_vectors.S -CMN_ASRCS = +CMN_ASRCS = up_context.S CMN_CSRCS = up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \ up_createstack.c up_mdelay.c up_udelay.c up_exit.c \ up_idle.c up_initialize.c up_initialstate.c up_interruptcontext.c \ up_modifyreg8.c up_modifyreg16.c up_modifyreg32.c \ up_releasepending.c up_releasestack.c up_reprioritizertr.c \ up_schedulesigaction.c up_sigdeliver.c up_unblocktask.c \ - up_usestack.c up_doirq.c + up_usestack.c up_doirq.c up_hardfault.c up_svcall.c -CHIP_ASRCS = lm3s_context.S -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 \ +CHIP_ASRCS = +CHIP_CSRCS = lm3s_start.c lm3s_syscontrol.c lm3s_irq.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 deleted file mode 100644 index efef06630..000000000 --- a/nuttx/arch/arm/src/lm3s/lm3s_context.S +++ /dev/null @@ -1,133 +0,0 @@ -/************************************************************************************ - * arch/arm/src/lm3s/lm3s_context.S - * arch/arm/src/chip/lm3s_context.S - * - * 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 "nvic.h" - -/************************************************************************************ - * Preprocessor Definitions - ************************************************************************************/ - -/************************************************************************************ - * Global Symbols - ************************************************************************************/ - - .syntax unified - .thumb - .file "lm3s_context.S" - -/************************************************************************************ - * Macros - ************************************************************************************/ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************** - * Name: up_saveusercontext - * - * Description: - * Save the current thread context. Full prototype is: - * - * int up_saveusercontext(uint32 *regs); - * - * Return: - * 0: Normal return - * 1: Context switch return - * - **************************************************************************/ - - .text - .thumb_func - .globl up_saveusercontext - .type up_saveusercontext, function -up_saveusercontext: - - /* Perform the System call with R0=0 and R1=regs */ - - mov r1, r0 /* R1: regs */ - mov r0, #0 /* R0: 0 means save context (also return value) */ - 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 - * force R0 to be 1. - */ - - add r2, r1, #(4*REG_R0) - mov r3, #1 - str r3, [r2, #0] - bx lr /* "normal" return with r0=0 or - * context switch with r0=1 */ - .size up_saveusercontext, .-up_saveusercontext - - -/************************************************************************** - * Name: up_fullcontextrestore - * - * Description: - * Restore the current thread context. Full prototype is: - * - * void up_fullcontextrestore(uint32 *regs) __attribute__ ((noreturn)); - * - * Return: - * None - * - **************************************************************************/ - - .thumb_func - .globl up_fullcontextrestore - .type up_fullcontextrestore, function -up_fullcontextrestore: - - /* Perform the System call with R0=1 and R1=regs */ - - mov r1, r0 /* R1: regs */ - mov r0, #1 /* R0: 1 means restore context */ - svc 0 /* Force synchronous SVCall (or Hard Fault) */ - - /* This call should not return */ - - bx lr /* Unnecessary ... will not return */ - .size up_fullcontextrestore, .-up_fullcontextrestore - .end - diff --git a/nuttx/arch/arm/src/lm3s/lm3s_hardfault.c b/nuttx/arch/arm/src/lm3s/lm3s_hardfault.c deleted file mode 100644 index 7a3e60623..000000000 --- a/nuttx/arch/arm/src/lm3s/lm3s_hardfault.c +++ /dev/null @@ -1,142 +0,0 @@ -/**************************************************************************** - * 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 "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) - { - sllvdbg("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 86d9514d2..0408fe81d 100644 --- a/nuttx/arch/arm/src/lm3s/lm3s_internal.h +++ b/nuttx/arch/arm/src/lm3s/lm3s_internal.h @@ -213,26 +213,6 @@ 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 ****************************************************************************/ @@ -280,27 +260,6 @@ EXTERN void up_clockconfig(void); EXTERN int lm3s_configgpio(uint32 cfgset); -/**************************************************************************** - * 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 Hard Fault exception handler. It also catches SVC call - * exceptions that are performed in bad contexts. - * - ****************************************************************************/ - -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 374c4bfc7..a2da7663e 100644 --- a/nuttx/arch/arm/src/lm3s/lm3s_irq.c +++ b/nuttx/arch/arm/src/lm3s/lm3s_irq.c @@ -315,8 +315,8 @@ void up_irqinitialize(void) * under certain conditions. */ - irq_attach(LM3S_IRQ_SVCALL, lm3s_svcall); - irq_attach(LM3S_IRQ_HARDFAULT, lm3s_hardfault); + irq_attach(LM3S_IRQ_SVCALL, up_svcall); + irq_attach(LM3S_IRQ_HARDFAULT, up_hardfault); /* Set the priority of the SVCall interrupt */ diff --git a/nuttx/arch/arm/src/lm3s/lm3s_svcall.c b/nuttx/arch/arm/src/lm3s/lm3s_svcall.c deleted file mode 100644 index d6c7bf517..000000000 --- a/nuttx/arch/arm/src/lm3s/lm3s_svcall.c +++ /dev/null @@ -1,146 +0,0 @@ -/**************************************************************************** - * 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; -} -- cgit v1.2.3