From 44a68c09402a4e702870da55df40b99f01995401 Mon Sep 17 00:00:00 2001 From: patacongo Date: Wed, 18 May 2011 17:16:28 +0000 Subject: Add PIC32 register definitions and assertion logic git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@3623 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/mips/include/mips32/cp0.h | 1 + nuttx/arch/mips/include/mips32/irq.h | 249 ++++++++++++++++++++++++++++- nuttx/arch/mips/include/mips32/registers.h | 2 +- nuttx/arch/mips/include/pic32mx/cp0.h | 15 +- nuttx/arch/mips/src/common/up_internal.h | 41 +++-- nuttx/arch/mips/src/mips32/up_assert.c | 163 +++++++++++++++++++ nuttx/arch/mips/src/mips32/up_dumpstate.c | 240 +++++++++++++++++++++++++++ nuttx/arch/mips/src/pic32mx/Make.defs | 15 +- nuttx/arch/mips/src/pic32mx/pic32mx-head.S | 2 +- nuttx/arch/mips/src/pic32mx/pic32mx-irq.c | 153 ++++++++++++++++++ 10 files changed, 843 insertions(+), 38 deletions(-) create mode 100644 nuttx/arch/mips/src/mips32/up_assert.c create mode 100644 nuttx/arch/mips/src/mips32/up_dumpstate.c create mode 100755 nuttx/arch/mips/src/pic32mx/pic32mx-irq.c (limited to 'nuttx/arch/mips') diff --git a/nuttx/arch/mips/include/mips32/cp0.h b/nuttx/arch/mips/include/mips32/cp0.h index 54b0a7b8e..b0832f207 100755 --- a/nuttx/arch/mips/include/mips32/cp0.h +++ b/nuttx/arch/mips/include/mips32/cp0.h @@ -225,6 +225,7 @@ # define CP0_STATUS_IM6 (0x40 << CP0_STATUS_IM_SHIFT) # define CP0_STATUS_IM_TIMER (0x80 << CP0_STATUS_IM_SHIFT) /* IM7 = Hardware/Timer/Perf interrupts */ # define CP0_STATUS_IM7 (0x80 << CP0_STATUS_IM_SHIFT) +# define CP0_STATUS_IM_ALL (0xff << CP0_STATUS_IM_SHIFT) #define CP0_STATUS_IMPL_SHIFT (16) /* Bits 16-17: Implementation dependent */ #define CP0_STATUS_IMPL_MASK (3 << CP0_STATUS_IMPL_SHIFT) #define CP0_STATUS_NMI (1 << 19) /* Bit 19: Reset exception due to an NMI */ diff --git a/nuttx/arch/mips/include/mips32/irq.h b/nuttx/arch/mips/include/mips32/irq.h index 1dea3c795..8d1e2a036 100755 --- a/nuttx/arch/mips/include/mips32/irq.h +++ b/nuttx/arch/mips/include/mips32/irq.h @@ -44,15 +44,260 @@ * Included Files ****************************************************************************/ +#include #include /**************************************************************************** * Definitions ****************************************************************************/ +/* Configuration ************************************************************/ +/* The global pointer (GP) does not need to be saved in the "normal," flat + * NuttX build. However, it would be necessary to save the GP if this is + * a KERNEL build or if NXFLAT is supported. + */ + +#undef MIPS32_SAVE_GP +#if defined(CONFIG_NUTTX_KERNEL) || defined(CONFIG_NXFLAT) +# define MIPS32_SAVE_GP 1 +#endif + +/* Register save state structure ********************************************/ +/* Co processor registers */ + +#define REG_MFLO_NDX 0 +#define REG_MFHI_NDX 1 +#define REG_EPC_NDX 2 + +/* General pupose registers */ +/* $0: Zero register does not need to be saved */ +/* $1: at_reg, assembler temporary */ + +#define REG_R1_NDX 3 + +/* $2-$3 = v0-v1: Return value registers */ + +#define REG_R2_NDX 4 +#define REG_R3_NDX 5 + +/* $4-$7 = a0-a3: Argument registers */ + +#define REG_R4_NDX 6 +#define REG_R5_NDX 7 +#define REG_R6_NDX 8 +#define REG_R7_NDX 9 + +/* $8-$15 = t0-t7: Volatile registers */ + +#define REG_R8_NDX 10 +#define REG_R9_NDX 11 +#define REG_R10_NDX 12 +#define REG_R11_NDX 13 +#define REG_R12_NDX 14 +#define REG_R13_NDX 15 +#define REG_R14_NDX 16 +#define REG_R15_NDX 17 + +/* $16-$23 = s0-s7: Static registers */ + +#define REG_R16_NDX 18 +#define REG_R17_NDX 19 +#define REG_R18_NDX 20 +#define REG_R19_NDX 21 +#define REG_R20_NDX 22 +#define REG_R21_NDX 23 +#define REG_R22_NDX 24 +#define REG_R23_NDX 25 + +/* $24-25 = t8-t9: More Volatile registers */ + +#define REG_R24_NDX 26 +#define REG_R25_NDX 27 + +/* $26-$27 = ko-k1: Reserved for use in exeption handers. These do not need + * to be saved. + */ + +/* $28 = gp: Only needs to be saved under conditions where there are + * multiple, per-thread values for the GP. + */ + +#ifdef MIPS32_SAVE_GP + +# define REG_R28_NDX 28 + +/* $29 = sp: The value of the stack pointer on return from the exception */ + +# define REG_R29_NDX 29 + +/* $30 = either s8 or fp: Depends if a frame pointer is used or not */ + +# define REG_R30_NDX 30 + +/* $31 = ra: Return address */ + +# define REG_R31_NDX 31 +# define XCPTCONTEXT_REGS 32 +#else + +/* $29 = sp: The value of the stack pointer on return from the exception */ + +# define REG_R29_NDX 28 + +/* $30 = either s8 or fp: Depends if a frame pointer is used or not */ + +# define REG_R30_NDX 29 + +/* $31 = ra: Return address */ + +# define REG_R31_NDX 30 +# define XCPTCONTEXT_REGS 31 +#endif +#define XCPTCONTEXT_SIZE (4 * XCPTCONTEXT_REGS) + +/* In assembly language, values have to be referenced as byte address + * offsets. But in C, it is more convenient to reference registers as + * register save table offsets. + */ + +#ifdef __ASSEMBLY__ +# define REG_MFLO (4*REG_MFLO_NDX) +# define REG_MFHI (4*REG_MFHI_NDX) +# define REG_EPC (4*REG_EPC_NDX) +# define REG_R1 (4*REG_R1_NDX) +# define REG_R2 (4*REG_R2_NDX) +# define REG_R3 (4*REG_R3_NDX) +# define REG_R4 (4*REG_R4_NDX) +# define REG_R5 (4*REG_R5_NDX) +# define REG_R6 (4*REG_R6_NDX) +# define REG_R7 (4*REG_R7_NDX) +# define REG_R8 (4*REG_R8_NDX) +# define REG_R9 (4*REG_R9_NDX) +# define REG_R10 (4*REG_R10_NDX) +# define REG_R11 (4*REG_R11_NDX) +# define REG_R12 (4*REG_R12_NDX) +# define REG_R13 (4*REG_R13_NDX) +# define REG_R14 (4*REG_R14_NDX) +# define REG_R15 (4*REG_R15_NDX) +# define REG_R16 (4*REG_R16_NDX) +# define REG_R17 (4*REG_R17_NDX) +# define REG_R18 (4*REG_R18_NDX) +# define REG_R19 (4*REG_R19_NDX) +# define REG_R20 (4*REG_R20_NDX) +# define REG_R21 (4*REG_R21_NDX) +# define REG_R22 (4*REG_R22_NDX) +# define REG_R23 (4*REG_R23_NDX) +# define REG_R24 (4*REG_R24_NDX) +# define REG_R25 (4*REG_R25_NDX) +# ifdef MIPS32_SAVE_GP +# define REG_R28 (4*REG_R28_NDX) +# endif +# define REG_R29 (4*REG_R29_NDX) +# define REG_R30 (4*REG_R30_NDX) +# define REG_R31 (4*REG_R31_NDX) +#else +# define REG_MFLO REG_MFLO_NDX +# define REG_MFHI REG_MFHI_NDX +# define REG_EPC REG_EPC_NDX +# define REG_R1 REG_R1_NDX +# define REG_R2 REG_R2_NDX +# define REG_R3 REG_R3_NDX +# define REG_R4 REG_R4_NDX +# define REG_R5 REG_R5_NDX +# define REG_R6 REG_R6_NDX +# define REG_R7 REG_R7_NDX +# define REG_R8 REG_R8_NDX +# define REG_R9 REG_R9_NDX +# define REG_R10 REG_R10_NDX +# define REG_R11 REG_R11_NDX +# define REG_R12 REG_R12_NDX +# define REG_R13 REG_R13_NDX +# define REG_R14 REG_R14_NDX +# define REG_R15 REG_R15_NDX +# define REG_R16 REG_R16_NDX +# define REG_R17 REG_R17_NDX +# define REG_R18 REG_R18_NDX +# define REG_R19 REG_R19_NDX +# define REG_R20 REG_R20_NDX +# define REG_R21 REG_R21_NDX +# define REG_R22 REG_R22_NDX +# define REG_R23 REG_R23_NDX +# define REG_R24 REG_R24_NDX +# define REG_R25 REG_R25_NDX +# ifdef MIPS32_SAVE_GP +# define REG_R28 REG_R28_NDX +# endif +# define REG_R29 REG_R29_NDX +# define REG_R30 REG_R30_NDX +# define REG_R31 REG_R31_NDX +#endif + +/* Now define more user friendly alternative name that can be used either + * in assembly or C contexts. + */ + +/* $1: at_reg, assembler temporary */ + +#define REG_AT REG_R1 + +/* $2-$3 = v0-v1: Return value registers */ + +#define REG_V0 REG_R2 +#define REG_V1 REG_R3 + +/* $4-$7 = a0-a3: Argument registers */ + +#define REG_A0 REG_R4 +#define REG_A1 REG_R5 +#define REG_A2 REG_R6 +#define REG_A3 REG_R7 + +/* $8-$15 = t0-t7: Volatile registers */ + +#define REG_T0 REG_R8 +#define REG_T1 REG_R9 +#define REG_T2 REG_R10 +#define REG_T3 REG_R11 +#define REG_T4 REG_R12 +#define REG_T5 REG_R13 +#define REG_T6 REG_R14 +#define REG_T7 REG_R15 + +/* $16-$23 = s0-s7: Static registers */ + +#define REG_S0 REG_R16 +#define REG_S1 REG_R17 +#define REG_S2 REG_R18 +#define REG_S3 REG_R19 +#define REG_S4 REG_R20 +#define REG_S5 REG_R21 +#define REG_S6 REG_R22 +#define REG_S7 REG_R23 + +/* $24-25 = t8-t9: More Volatile registers */ + +#define REG_T8 REG_R24 +#define REG_T9 REG_R25 + +/* $28 = gp: Only needs to be saved under conditions where there are + * multiple, per-thread values for the GP. + */ + +#ifdef MIPS32_SAVE_GP +# define REG_GP REG_R28 +#endif + +/* $29 = sp: The value of the stack pointer on return from the exception */ + +#define REG_SP REG_R29 + +/* $30 = either s8 or fp: Depends if a frame pointer is used or not */ + +#define REG_S8 REG_R30 +#define REG_FP REG_R30 -/* Lots of missing logic here */ +/* $31 = ra: Return address */ -#define XCPTCONTEXT_REGS 1 +#define REG_RA REG_R31 /**************************************************************************** * Public Types diff --git a/nuttx/arch/mips/include/mips32/registers.h b/nuttx/arch/mips/include/mips32/registers.h index 37f404cf8..997bc51d6 100755 --- a/nuttx/arch/mips/include/mips32/registers.h +++ b/nuttx/arch/mips/include/mips32/registers.h @@ -62,7 +62,7 @@ #define v0 $2 #define v1 $3 -/* Argument registers: First few parameters for a function */ +/* Argument registers: First four parameters to a function */ #define a0 $4 #define a1 $5 diff --git a/nuttx/arch/mips/include/pic32mx/cp0.h b/nuttx/arch/mips/include/pic32mx/cp0.h index 5bd50b371..d0feadb6b 100755 --- a/nuttx/arch/mips/include/pic32mx/cp0.h +++ b/nuttx/arch/mips/include/pic32mx/cp0.h @@ -136,24 +136,15 @@ #undef CP0_STATUS_PX #undef CP0_STATUS_MX -/* 2. The following field is of a different width +/* 2. The following field is of a different width. Apparently, it + * excludes the software interrupt bits. + * * CP0_STATUS_IM Bits 8-15: Interrupt Mask * Vs. * CP0_STATUS_IPL Bits 10-15: Interrupt priority level * Bitss 8-9 reserved */ -#undef CP0_STATUS_IM_SHIFT -#undef CP0_STATUS_IM_MASK -#undef CP0_STATUS_IM0 -#undef CP0_STATUS_IM1 -#undef CP0_STATUS_IM2 -#undef CP0_STATUS_IM3 -#undef CP0_STATUS_IM4 -#undef CP0_STATUS_IM5 -#undef CP0_STATUS_IM6 -#undef CP0_STATUS_IM7 - #define CP0_STATUS_IPL_SHIFT (10) /* Bits 10-15: Interrupt priority level */ #define CP0_STATUS_IPL_MASK (0x3f << CP0_STATUS_IPL_SHIFT) diff --git a/nuttx/arch/mips/src/common/up_internal.h b/nuttx/arch/mips/src/common/up_internal.h index e4079fd55..1c8d4ed37 100755 --- a/nuttx/arch/mips/src/common/up_internal.h +++ b/nuttx/arch/mips/src/common/up_internal.h @@ -121,21 +121,30 @@ extern void g_intstackbase; * following way: * * - The linker script defines, for example, the symbol_sdata. - * - The declareion extern uint32_t _sdata; makes C happy. C will believe + * - The declaration extern uint32_t _sdata; makes C happy. C will believe * that the value _sdata is the address of a uint32_t variable _data (it is * not!). * - We can recoved the linker value then by simply taking the address of * of _data. like: uint32_t *pdata = &_sdata; */ -extern uint32_t _stext; /* Start of .text */ -extern uint32_t _etext; /* End_1 of .text + .rodata */ -extern const uint32_t _eronly; /* End+1 of read only section (.text + .rodata) */ -extern uint32_t _sdata; /* Start of .data */ -extern uint32_t _edata; /* End+1 of .data */ -extern uint32_t _sbss; /* Start of .bss */ -extern uint32_t _ebss; /* End+1 of .bss */ -#endif +extern uint32_t _stext; /* Start of .text */ +extern uint32_t _etext; /* End+1 of .text + .rodata */ +extern const uint32_t _data_loaddr; /* Start of .data in FLASH */ +extern uint32_t _sdata; /* Start of .data */ +extern uint32_t _edata; /* End+1 of .data */ +extern uint32_t _sbss; /* Start of .bss */ +extern uint32_t _ebss; /* End+1 of .bss */ +#ifdef CONFIG_PIC32MX_RAMFUNCS +extern uint32_t _sramfunc; /* Start of ramfuncs */ +extern uint32_t _eramfunc; /* End+1 of ramfuncs */ +extern uint32_t _ramfunc_loadaddr; /* Start of ramfuncs in FLASH */ +extern uint32_t _ramfunc_sizeof; /* Size of ramfuncs */ +extern uint32_t _bmxdkpba_address; /* BMX register setting */ +extern uint32_t _bmxdudba_address; /* BMX register setting */ +extern uint32_t _bmxdupba_address; /* BMX register setting */ +#endif /* CONFIG_PIC32MX_RAMFUNCS */ +#endif /* __ASSEMBLY__ */ /**************************************************************************** * Inline Functions @@ -167,15 +176,13 @@ extern void up_lowputs(const char *str); extern uint32_t *up_doirq(int irq, uint32_t *regs); -/* Defined in up_vectors.S */ +/* Defined in up_dumpstate.c */ -extern void up_vectorundefinsn(void); -extern void up_vectorswi(void); -extern void up_vectorprefetch(void); -extern void up_vectordata(void); -extern void up_vectoraddrexcptn(void); -extern void up_vectorirq(void); -extern void up_vectorfiq(void); +#ifdef CONFIG_ARCH_STACKDUMP +extern void up_dumpstate(void); +#else +# define up_dumpstate() +#endif /* Defined in up_allocateheap.c */ diff --git a/nuttx/arch/mips/src/mips32/up_assert.c b/nuttx/arch/mips/src/mips32/up_assert.c new file mode 100644 index 000000000..c10a99471 --- /dev/null +++ b/nuttx/arch/mips/src/mips32/up_assert.c @@ -0,0 +1,163 @@ +/**************************************************************************** + * arch/mips/src/arm/up_assert.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Output debug info if stack dump is selected -- even if debug is not + * selected. + */ + +#ifdef CONFIG_ARCH_STACKDUMP +# undef lldbg +# define lldbg lib_lowprintf +#endif + +/* The following is just intended to keep some ugliness out of the mainline + * code. We are going to print the task name if: + * + * CONFIG_TASK_NAME_SIZE > 0 && <-- The task has a name + * (defined(CONFIG_DEBUG) || <-- And the debug is enabled (lldbg used) + * defined(CONFIG_ARCH_STACKDUMP) <-- Or lib_lowprintf() is used + */ + +#undef CONFIG_PRINT_TASKNAME +#if CONFIG_TASK_NAME_SIZE > 0 && (defined(CONFIG_DEBUG) || defined(CONFIG_ARCH_STACKDUMP)) +# define CONFIG_PRINT_TASKNAME 1 +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: _up_assert + ****************************************************************************/ + +static void _up_assert(int errorcode) /* __attribute__ ((noreturn)) */ +{ + /* Are we in an interrupt handler or the idle task? */ + + if (current_regs || ((_TCB*)g_readytorun.head)->pid == 0) + { + (void)irqsave(); + for(;;) + { +#ifdef CONFIG_ARCH_LEDS + up_ledon(LED_PANIC); + up_mdelay(250); + up_ledoff(LED_PANIC); + up_mdelay(250); +#endif + } + } + else + { + exit(errorcode); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_assert + ****************************************************************************/ + +void up_assert(const uint8_t *filename, int lineno) +{ +#ifdef CONFIG_PRINT_TASKNAME + _TCB *rtcb = (_TCB*)g_readytorun.head; +#endif + + up_ledon(LED_ASSERTION); +#ifdef CONFIG_PRINT_TASKNAME + lldbg("Assertion failed at file:%s line: %d task: %s\n", + filename, lineno, rtcb->name); +#else + lldbg("Assertion failed at file:%s line: %d\n", + filename, lineno); +#endif + up_dumpstate(); + _up_assert(EXIT_FAILURE); +} + +/**************************************************************************** + * Name: up_assert_code + ****************************************************************************/ + +void up_assert_code(const uint8_t *filename, int lineno, int errorcode) +{ +#ifdef CONFIG_PRINT_TASKNAME + _TCB *rtcb = (_TCB*)g_readytorun.head; +#endif + + up_ledon(LED_ASSERTION); + +#ifdef CONFIG_PRINT_TASKNAME + lldbg("Assertion failed at file:%s line: %d task: %s error code: %d\n", + filename, lineno, rtcb->name, errorcode); +#else + lldbg("Assertion failed at file:%s line: %d error code: %d\n", + filename, lineno, errorcode); +#endif + up_dumpstate(); + _up_assert(errorcode); +} diff --git a/nuttx/arch/mips/src/mips32/up_dumpstate.c b/nuttx/arch/mips/src/mips32/up_dumpstate.c new file mode 100644 index 000000000..83495e3a2 --- /dev/null +++ b/nuttx/arch/mips/src/mips32/up_dumpstate.c @@ -0,0 +1,240 @@ +/**************************************************************************** + * arch/mips/src/arm/up_dumpstate.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" + +#ifdef CONFIG_ARCH_STACKDUMP + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Output debug info if stack dump is selected -- even if debug is not + * selected. + */ + +#undef lldbg +#define lldbg lib_lowprintf + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_getsp + ****************************************************************************/ + +/* I don't know if the builtin to get SP is enabled */ + +static inline uint32_t up_getsp(void) +{ + register uint32_t sp; + __asm__ + ( + "\tadd %0, $0, $29\n" + : "=r"(sp) + ); + return sp; +} + +/**************************************************************************** + * Name: up_stackdump + ****************************************************************************/ + +static void up_stackdump(uint32_t sp, uint32_t stack_base) +{ + uint32_t stack ; + + for (stack = sp & ~0x1f; stack < stack_base; stack += 32) + { + uint32_t *ptr = (uint32_t*)stack; + lldbg("%08x: %08x %08x %08x %08x %08x %08x %08x %08x\n", + stack, ptr[0], ptr[1], ptr[2], ptr[3], + ptr[4], ptr[5], ptr[6], ptr[7]); + } +} + +/**************************************************************************** + * Name: up_registerdump + ****************************************************************************/ + +static inline void up_registerdump(void) +{ + /* Are user registers available from interrupt processing? */ + + if (current_regs) + { + lldbg("MFLO:%08x MFHI:%08x EPC:%08x\n", + current_regs[REG_MFLO], current_regs[REG_MFHI], current_regs[REG_EPC]); + lldbg("AT:%08x V0:$08x V1:%08x A0:%08x A1:%08x A2:%08x A3:%08x\n", + current_regs[REG_AT], current_regs[REG_V0], current_regs[REG_V1], + current_regs[REG_A0], current_regs[REG_A1], current_regs[REG_A2], + current_regs[REG_A3]); + lldbg("T0:%08x T1:$08x T2:%08x T3:%08x T4:%08x T5:%08x T6:%08x T7:%08x\n", + current_regs[REG_T0], current_regs[REG_T1], current_regs[REG_T2], + current_regs[REG_T3], current_regs[REG_T4], current_regs[REG_T5], + current_regs[REG_T6], current_regs[REG_T7]); + lldbg("S0:%08x S1:$08x S2:%08x S3:%08x S4:%08x S5:%08x S6:%08x S7:%08x\n", + current_regs[REG_S0], current_regs[REG_S1], current_regs[REG_S2], + current_regs[REG_S3], current_regs[REG_S4], current_regs[REG_S5], + current_regs[REG_S6], current_regs[REG_S7]); +#ifdef MIPS32_SAVE_GP + lldbg("T8:%08x T9:$08x GP:%08x SP:%08x FP:%08x RA:%08x\n", + current_regs[REG_T8], current_regs[REG_T9], current_regs[REG_GP], + current_regs[REG_SP], current_regs[REG_FP], current_regs[REG_RA]); +#else + lldbg("T8:%08x T9:$08x SP:%08x FP:%08x RA:%08x\n", + current_regs[REG_T8], current_regs[REG_T9], current_regs[REG_SP], + current_regs[REG_FP], current_regs[REG_RA]); +#endif + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_dumpstate + ****************************************************************************/ + +void up_dumpstate(void) +{ + _TCB *rtcb = (_TCB*)g_readytorun.head; + uint32_t sp = up_getsp(); + uint32_t ustackbase; + uint32_t ustacksize; +#if CONFIG_ARCH_INTERRUPTSTACK > 3 + uint32_t istackbase; + uint32_t istacksize; +#endif + + /* Get the limits on the user stack memory */ + + if (rtcb->pid == 0) + { + ustackbase = g_heapbase - 4; + ustacksize = CONFIG_IDLETHREAD_STACKSIZE; + } + else + { + ustackbase = (uint32_t)rtcb->adj_stack_ptr; + ustacksize = (uint32_t)rtcb->adj_stack_size; + } + + /* Get the limits on the interrupt stack memory */ + +#if CONFIG_ARCH_INTERRUPTSTACK > 3 + istackbase = (uint32_t)&g_userstack; + istacksize = (CONFIG_ARCH_INTERRUPTSTACK & ~3) - 4; + + /* Show interrupt stack info */ + + lldbg("sp: %08x\n", sp); + lldbg("IRQ stack:\n"); + lldbg(" base: %08x\n", istackbase); + lldbg(" size: %08x\n", istacksize); + + /* Does the current stack pointer lie within the interrupt + * stack? + */ + + if (sp <= istackbase && sp > istackbase - istacksize) + { + /* Yes.. dump the interrupt stack */ + + up_stackdump(sp, istackbase); + + /* Extract the user stack pointer which should lie + * at the base of the interrupt stack. + */ + + sp = g_userstack; + lldbg("sp: %08x\n", sp); + } + + /* Show user stack info */ + + lldbg("User stack:\n"); + lldbg(" base: %08x\n", ustackbase); + lldbg(" size: %08x\n", ustacksize); +#else + lldbg("sp: %08x\n", sp); + lldbg("stack base: %08x\n", ustackbase); + lldbg("stack size: %08x\n", ustacksize); +#endif + + /* Dump the user stack if the stack pointer lies within the allocated user + * stack memory. + */ + + if (sp > ustackbase || sp <= ustackbase - ustacksize) + { +#if !defined(CONFIG_ARCH_INTERRUPTSTACK) || CONFIG_ARCH_INTERRUPTSTACK < 4 + lldbg("ERROR: Stack pointer is not within allocated stack\n"); +#endif + } + else + { + up_stackdump(sp, ustackbase); + } + + /* Then dump the registers (if available) */ + + up_registerdump(); +} + +#endif /* CONFIG_ARCH_STACKDUMP */ diff --git a/nuttx/arch/mips/src/pic32mx/Make.defs b/nuttx/arch/mips/src/pic32mx/Make.defs index 05e4898ca..912a3bb41 100755 --- a/nuttx/arch/mips/src/pic32mx/Make.defs +++ b/nuttx/arch/mips/src/pic32mx/Make.defs @@ -40,10 +40,11 @@ HEAD_ASRC = pic32mx-head.S # Common MIPS files CMN_ASRCS = -CMN_CSRCS = up_allocateheap.c up_createstack.c up_exit.c up_idle.c \ - up_initialize.c up_interruptcontext.c up_irq.c up_lowputs.c \ - up_mdelay.c up_modifyreg8.c up_modifyreg16.c up_modifyreg32.c \ - up_puts.c up_releasestack.c up_udelay.c up_usestack.c +CMN_CSRCS = up_allocateheap.c up_assert.c up_createstack.c up_exit.c \ + up_idle.c up_initialize.c up_interruptcontext.c up_irq.c \ + up_lowputs.c up_mdelay.c up_modifyreg8.c up_modifyreg16.c \ + up_modifyreg32.c up_puts.c up_releasestack.c up_udelay.c \ + up_usestack.c # Configuration dependent common files @@ -51,10 +52,14 @@ ifeq ($(CONFIG_NET),y) CMN_CSRCS += up_etherstub.c endif +ifeq ($(CONFIG_ARCH_STACKDUMP),y) +CMN_CSRCS += up_dumpstate.c +endif + # Required PIC32MX files CHIP_ASRCS = -CHIP_CSRCS = +CHIP_CSRCS = pic32mx-irq.c # Configuration-dependent PIC32MX files diff --git a/nuttx/arch/mips/src/pic32mx/pic32mx-head.S b/nuttx/arch/mips/src/pic32mx/pic32mx-head.S index 967e784cb..e18136551 100644 --- a/nuttx/arch/mips/src/pic32mx/pic32mx-head.S +++ b/nuttx/arch/mips/src/pic32mx/pic32mx-head.S @@ -216,7 +216,7 @@ __start: * initializing bus matrix registers. */ -#ifdef CONFIG_PIC32MX_RAMFUNCs +#ifdef CONFIG_PIC32MX_RAMFUNCS la t1, _ramfunc_sizeof beqz t1, .Lnoramfuncs nop diff --git a/nuttx/arch/mips/src/pic32mx/pic32mx-irq.c b/nuttx/arch/mips/src/pic32mx/pic32mx-irq.c new file mode 100755 index 000000000..19ecfb8e7 --- /dev/null +++ b/nuttx/arch/mips/src/pic32mx/pic32mx-irq.c @@ -0,0 +1,153 @@ +/**************************************************************************** + * arch/mips/src/pic32mx/pic32mx-irq.c + * arch/mips/src/chip/pic32mx-irq.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" +#include "pic32mx-internal.h" + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +volatile uint32_t *current_regs; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_irqinitialize + ****************************************************************************/ + +void up_irqinitialize(void) +{ + /* Disable all interrupts */ +#warning "Missing logic" + + /* currents_regs is non-NULL only while processing an interrupt */ + + current_regs = NULL; + + /* Attach processor exceptions */ + + /* Initialize logic to support a second level of interrupt decoding for + * IOPORT pins. + */ + +#ifdef CONFIG_GPIO_IRQ + lpc17_gpioirqinitialize(); +#endif + + /* And finally, enable interrupts */ + +#ifndef CONFIG_SUPPRESS_INTERRUPTS + irqrestore(CP0_STATUS_IM_ALL); +#endif +} + +/**************************************************************************** + * Name: up_disable_irq + * + * Description: + * Disable the IRQ specified by 'irq' + * + ****************************************************************************/ + +void up_disable_irq(int irq) +{ +#warning "Missing logic" +} + +/**************************************************************************** + * Name: up_enable_irq + * + * Description: + * Enable the IRQ specified by 'irq' + * + ****************************************************************************/ + +void up_enable_irq(int irq) +{ +#warning "Missing logic" +} + +/**************************************************************************** + * Name: up_prioritize_irq + * + * Description: + * Set the priority of an IRQ. + * + * Since this API is not supported on all architectures, it should be + * avoided in common implementations where possible. + * + ****************************************************************************/ + +#ifdef CONFIG_ARCH_IRQPRIO +int up_prioritize_irq(int irq, int priority) +{ +#warning "Missing logic" + return -ENOSYS; +} +#endif -- cgit v1.2.3