From 10c4657704464e36e966873ba091c47ba7ae6db2 Mon Sep 17 00:00:00 2001 From: patacongo Date: Mon, 9 Feb 2009 00:11:22 +0000 Subject: Adding M16C support git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@1486 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/sh/include/m16c/irq.h | 30 +++- nuttx/arch/sh/include/m16c/types.h | 2 +- nuttx/arch/sh/src/common/up_assert.c | 171 -------------------- nuttx/arch/sh/src/common/up_initialstate.c | 109 ------------- nuttx/arch/sh/src/common/up_internal.h | 8 + nuttx/arch/sh/src/m16c/Make.defs | 6 +- nuttx/arch/sh/src/m16c/chip.h | 2 +- nuttx/arch/sh/src/m16c/m16c_head.S | 20 +-- nuttx/arch/sh/src/sh1/Make.defs | 3 +- nuttx/arch/sh/src/sh1/sh1_dumpstate.c | 240 +++++++++++++++++++++++++++++ nuttx/arch/sh/src/sh1/sh1_initialstate.c | 109 +++++++++++++ 11 files changed, 404 insertions(+), 296 deletions(-) delete mode 100644 nuttx/arch/sh/src/common/up_initialstate.c create mode 100755 nuttx/arch/sh/src/sh1/sh1_dumpstate.c create mode 100644 nuttx/arch/sh/src/sh1/sh1_initialstate.c (limited to 'nuttx/arch') diff --git a/nuttx/arch/sh/include/m16c/irq.h b/nuttx/arch/sh/include/m16c/irq.h index 2e29f6500..daec5d684 100644 --- a/nuttx/arch/sh/include/m16c/irq.h +++ b/nuttx/arch/sh/include/m16c/irq.h @@ -191,7 +191,7 @@ # define M16C_INT0_IRQ (_LAST_FIXED+24) /* ffd74: INT0 */ # define M16C_INT1_IRQ (_LAST_FIXED+25) /* ffd78: INT1 */ -# define NR_IRQS (_LAST_FIXED+26 /* Total number of supported IRQs */ +# define NR_IRQS (_LAST_FIXED+26) /* Total number of supported IRQs */ #endif #define M16C_SYSTIMER_IRQ M16C_TMRA0_IRQ @@ -226,6 +226,34 @@ extern "C" { ************************************************************************************/ #ifndef __ASSEMBLY__ + +/* Save the current interrupt enable state & disable IRQs */ + +static inline irqstate_t irqsave(void) +{ + irqstate_t flags; + __asm__ __volatile__ + ( + "\tstc flg, %0\n" + "\tfclr I\n" + : "=r" (flags) + : + : "memory"); + return flags; +} + +/* Restore saved IRQ & FIQ state */ + +static inline void irqrestore(irqstate_t flags) +{ + __asm__ __volatile__ + ( + "ldc %0, flg" + : + : "r" (flags) + : "memory"); +} + #endif /************************************************************************************ diff --git a/nuttx/arch/sh/include/m16c/types.h b/nuttx/arch/sh/include/m16c/types.h index df82676c4..4c3cad9bd 100644 --- a/nuttx/arch/sh/include/m16c/types.h +++ b/nuttx/arch/sh/include/m16c/types.h @@ -72,7 +72,7 @@ typedef unsigned long long uint64; * irqsave() */ -typedef unsigned int irqstate_t; +typedef uint16 irqstate_t; #endif /* __ASSEMBLY__ */ diff --git a/nuttx/arch/sh/src/common/up_assert.c b/nuttx/arch/sh/src/common/up_assert.c index f4a1bc080..3cac6606f 100644 --- a/nuttx/arch/sh/src/common/up_assert.c +++ b/nuttx/arch/sh/src/common/up_assert.c @@ -72,177 +72,6 @@ * Private Functions ****************************************************************************/ -/**************************************************************************** - * Name: up_getsp - ****************************************************************************/ - -/**************************************************************************** - * Name: getsp - ****************************************************************************/ - -static inline uint32 up_getsp(void) -{ - uint32 sp; - - __asm__ __volatile__ - ( - "mov r15, %0\n\t" - : "=&z" (sp) - : - : "memory" - ); - return sp; -} - -/**************************************************************************** - * Name: up_stackdump - ****************************************************************************/ - -#ifdef CONFIG_ARCH_STACKDUMP -static void up_stackdump(uint32 sp, uint32 stack_base) -{ - uint32 stack ; - - for (stack = sp & ~0x1f; stack < stack_base; stack += 32) - { - uint32 *ptr = (uint32*)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]); - } -} -#else -# define up_stackdump() -#endif - -/**************************************************************************** - * Name: up_registerdump - ****************************************************************************/ - -#ifdef CONFIG_ARCH_STACKDUMP -static inline void up_registerdump(void) -{ - uint32 *ptr = current_regs; - - /* Are user registers available from interrupt processing? */ - - if (ptr) - { - /* Yes.. dump the interrupt registers */ - - lldbg("PC: %08x SR=%08x\n", - ptr[REG_PC], ptr[REG_SR]); - - lldbg("PR: %08x GBR: %08x MACH: %08x MACL: %08x\n", - ptr[REG_PR], ptr[REG_GBR], ptr[REG_MACH], ptr[REG_MACL]); - - lldbg("R%d: %08x %08x %08x %08x %08x %08x %08x %08x\n", 0, - ptr[REG_R0], ptr[REG_R1], ptr[REG_R2], ptr[REG_R3], - ptr[REG_R4], ptr[REG_R5], ptr[REG_R6], ptr[REG_R7]); - - lldbg("R%d: %08x %08x %08x %08x %08x %08x %08x %08x\n", 8, - ptr[REG_R8], ptr[REG_R9], ptr[REG_R10], ptr[REG_R11], - ptr[REG_R12], ptr[REG_R13], ptr[REG_R14], ptr[REG_R15]); - } -} -#else -# define up_registerdump() -#endif - -/**************************************************************************** - * Name: up_dumpstate - ****************************************************************************/ - -#ifdef CONFIG_ARCH_STACKDUMP -static void up_dumpstate(void) -{ - _TCB *rtcb = (_TCB*)g_readytorun.head; - uint32 sp = up_getsp(); - uint32 ustackbase; - uint32 ustacksize; -#if CONFIG_ARCH_INTERRUPTSTACK > 3 - uint32 istackbase; - uint32 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)rtcb->adj_stack_ptr; - ustacksize = (uint32)rtcb->adj_stack_size; - } - - /* Get the limits on the interrupt stack memory */ - -#if CONFIG_ARCH_INTERRUPTSTACK > 3 - istackbase = (uint32)&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(); -} -#else -# define up_dumpstate() -#endif - /**************************************************************************** * Name: _up_assert ****************************************************************************/ diff --git a/nuttx/arch/sh/src/common/up_initialstate.c b/nuttx/arch/sh/src/common/up_initialstate.c deleted file mode 100644 index d9de84a15..000000000 --- a/nuttx/arch/sh/src/common/up_initialstate.c +++ /dev/null @@ -1,109 +0,0 @@ -/**************************************************************************** - * arch/sh/src/common/up_initialstate.c - * - * Copyright (C) 2008 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 "up_internal.h" -#include "up_arch.h" - -/**************************************************************************** - * Private Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: up_getsr - ****************************************************************************/ - -static inline irqstate_t up_getsr(void) -{ - irqstate_t flags; - - __asm__ __volatile__ - ( - "stc sr, %0\n\t" - : "=&z" (flags) - : - : "memory" - ); - return flags; -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: up_initial_state - * - * Description: - * A new thread is being started and a new TCB - * has been created. This function is called to initialize - * the processor specific portions of the new TCB. - * - * This function must setup the intial architecture registers - * and/or stack so that execution will begin at tcb->start - * on the next context switch. - * - ****************************************************************************/ - -void up_initial_state(_TCB *tcb) -{ - struct xcptcontext *xcp = &tcb->xcp; - - /* Initialize the initial exception register context structure */ - - memset(xcp, 0, sizeof(struct xcptcontext)); - xcp->regs[REG_SP] = (uint32)tcb->adj_stack_ptr; - xcp->regs[REG_PC] = (uint32)tcb->start; -#ifdef CONFIG_SUPPRESS_INTERRUPTS - xcp->regs[REG_SR] = up_getsr() | 0x000000f0; -#else - xcp->regs[REG_SR] = up_getsr() & ~0x000000f0; -#endif -} diff --git a/nuttx/arch/sh/src/common/up_internal.h b/nuttx/arch/sh/src/common/up_internal.h index 9241bb0e7..3c708a387 100644 --- a/nuttx/arch/sh/src/common/up_internal.h +++ b/nuttx/arch/sh/src/common/up_internal.h @@ -209,6 +209,14 @@ extern void up_usbuninitialize(void); # define up_usbuninitialize() #endif +/* Defined in chip-specific logic */ + +#ifdef CONFIG_ARCH_STACKDUMP +extern void up_dumpstate(void); +#else +# define up_dumpstate() +#endif + #endif /* __ASSEMBLY__ */ #endif /* ___ARCH_SH_SRC_COMMON_UP_INTERNAL_H */ diff --git a/nuttx/arch/sh/src/m16c/Make.defs b/nuttx/arch/sh/src/m16c/Make.defs index 7c1645c8b..5bb9d73be 100644 --- a/nuttx/arch/sh/src/m16c/Make.defs +++ b/nuttx/arch/sh/src/m16c/Make.defs @@ -38,9 +38,9 @@ HEAD_ASRC = m16c_head.S CMN_ASRCS = CMN_CSRCS = up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \ up_createstack.c up_doirq.c up_exit.c up_idle.c up_initialize.c \ - up_initialstate.c up_interruptcontext.c up_lowputs.c \ - up_mdelay.c up_puts.c up_releasepending.c up_releasestack.c \ - up_reprioritizertr.c up_udelay.c up_unblocktask.c up_usestack.c + up_interruptcontext.c up_lowputs.c up_mdelay.c up_puts.c \ + up_releasepending.c up_releasestack.c up_reprioritizertr.c \ + up_udelay.c up_unblocktask.c up_usestack.c ifneq ($(CONFIG_DISABLE_SIGNALS),y) CMN_CSRCS += up_schedulesigaction.c up_sigdeliver.c diff --git a/nuttx/arch/sh/src/m16c/chip.h b/nuttx/arch/sh/src/m16c/chip.h index 580823f1b..89f07c97a 100644 --- a/nuttx/arch/sh/src/m16c/chip.h +++ b/nuttx/arch/sh/src/m16c/chip.h @@ -232,4 +232,4 @@ #define M16C_PUR2 0x003fe /* Pull-up control 2 */ #define M16C_PCR 0x003ff /* Port control */ -#endif /* __ARCH_SH_SRC_M16C_CHIP_H */ \ No newline at end of file +#endif /* __ARCH_SH_SRC_M16C_CHIP_H */ diff --git a/nuttx/arch/sh/src/m16c/m16c_head.S b/nuttx/arch/sh/src/m16c/m16c_head.S index 753f98c20..944493911 100644 --- a/nuttx/arch/sh/src/m16c/m16c_head.S +++ b/nuttx/arch/sh/src/m16c/m16c_head.S @@ -91,12 +91,13 @@ * 0x00400 - DATA Size: Determined by linker * BSS Size: Determined by linker * Idle stack Size: CONFIG_IDLETHREAD_STACKSIZE - * Interrupt stack Size: CONFIG_M16C_ISTACKSIZE + * Interrupt stack Size: CONFIG_ARCH_INTERRUPTSTACK * Heap Size: Everything remaining * 0x00bff - (end+1) */ .data +#if 0 .globl _g_stackbase .type _g_stackbase, object _g_stackbase: @@ -107,16 +108,13 @@ _g_stackbase: .globl _g_istackbase .type _g_istackbase, object _g_istackbase: - .word _enbss+CONFIG_IDLETHREAD_STACKSIZE-4 + .word _enbss+CONFIG_IDLETHREAD_STACKSIZE .size _g_istackbase, .-_g_istackbase - - - * The heap extends from _g_heapbase to the end of RAM. - */ +#endif .globl _g_heapbase .type _g_heapbase, object _g_heapbase: - .word _enbss+CONFIG_IDLETHREAD_STACKSIZE+CONFIG_M16C_ISTACKSIZE + .word _enbss+CONFIG_IDLETHREAD_STACKSIZE+CONFIG_ARCH_INTERRUPTSTACK .size _g_heapbase, .-_g_heapbase /************************************************************************************ @@ -346,9 +344,13 @@ _g_heapbase: .type __start, #function __start: -/* Set the istack pointer */ +/* Set the interrupt and user stack pointers */ - ldc #_enbss, isp /* Set idle thread stack pointer to the end of BSS */ + mov.w #_enbss, R0 + ldc R0, usp /* Set the user stack pointer */ + add.w #CONFIG_IDLETHREAD_STACKSIZE, R0 + ldc R0, isp /* Set interrupt thread stack pointer to the end of BSS */ + fset U /* Set bit 7 (U) to select the user stack pointer */ /* Set BCLK speed. At reset, the processor clock (BLCK) defaults to a divisor of 8. * This sets clock to F1 (divide by 1) on XIN: BCLK = XIN frequency. diff --git a/nuttx/arch/sh/src/sh1/Make.defs b/nuttx/arch/sh/src/sh1/Make.defs index faa9d8562..184ab1428 100644 --- a/nuttx/arch/sh/src/sh1/Make.defs +++ b/nuttx/arch/sh/src/sh1/Make.defs @@ -47,7 +47,8 @@ CMN_CSRCS += up_schedulesigaction.c up_sigdeliver.c endif CHIP_ASRCS = sh1_vector.S sh1_saveusercontext.S -CHIP_CSRCS = sh1_lowputc.c sh1_irq.c sh1_timerisr.c sh1_serial.c +CHIP_CSRCS = sh1_lowputc.c sh1_irq.c sh1_timerisr.c sh1_serial.c \ + sh1_initialstate.c sh1_dumpstate.c ifeq ($(CONFIG_USBDEV),y) CHIP_CSRCS += diff --git a/nuttx/arch/sh/src/sh1/sh1_dumpstate.c b/nuttx/arch/sh/src/sh1/sh1_dumpstate.c new file mode 100755 index 000000000..8d41adec4 --- /dev/null +++ b/nuttx/arch/sh/src/sh1/sh1_dumpstate.c @@ -0,0 +1,240 @@ +/**************************************************************************** + * arch/sh/src/sh1/sh1_assert.c + * + * Copyright (C) 2008, 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 "up_internal.h" + +include + +#include +#include + +#include +#include + +#include "up_arch.h" +#include "up_internal.h" + +#ifdef CONFIG_ARCH_STACKDUMP + +/**************************************************************************** + * 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 + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: sh1_getsp + ****************************************************************************/ + +static inline uint32 sh1_getsp(void) +{ + uint32 sp; + + __asm__ __volatile__ + ( + "mov r15, %0\n\t" + : "=&z" (sp) + : + : "memory" + ); + return sp; +} + +/**************************************************************************** + * Name: sh1_stackdump + ****************************************************************************/ + +static void sh1_stackdump(uint32 sp, uint32 stack_base) +{ + uint32 stack ; + + for (stack = sp & ~0x1f; stack < stack_base; stack += 32) + { + uint32 *ptr = (uint32*)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: sh1_registerdump + ****************************************************************************/ + +static inline void sh1_registerdump(void) +{ + uint32 *ptr = current_regs; + + /* Are user registers available from interrupt processing? */ + + if (ptr) + { + /* Yes.. dump the interrupt registers */ + + lldbg("PC: %08x SR=%08x\n", + ptr[REG_PC], ptr[REG_SR]); + + lldbg("PR: %08x GBR: %08x MACH: %08x MACL: %08x\n", + ptr[REG_PR], ptr[REG_GBR], ptr[REG_MACH], ptr[REG_MACL]); + + lldbg("R%d: %08x %08x %08x %08x %08x %08x %08x %08x\n", 0, + ptr[REG_R0], ptr[REG_R1], ptr[REG_R2], ptr[REG_R3], + ptr[REG_R4], ptr[REG_R5], ptr[REG_R6], ptr[REG_R7]); + + lldbg("R%d: %08x %08x %08x %08x %08x %08x %08x %08x\n", 8, + ptr[REG_R8], ptr[REG_R9], ptr[REG_R10], ptr[REG_R11], + ptr[REG_R12], ptr[REG_R13], ptr[REG_R14], ptr[REG_R15]); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_dumpstate + ****************************************************************************/ + +void up_dumpstate(void) +{ + _TCB *rtcb = (_TCB*)g_readytorun.head; + uint32 sp = sh1_getsp(); + uint32 ustackbase; + uint32 ustacksize; +#if CONFIG_ARCH_INTERRUPTSTACK > 3 + uint32 istackbase; + uint32 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)rtcb->adj_stack_ptr; + ustacksize = (uint32)rtcb->adj_stack_size; + } + + /* Get the limits on the interrupt stack memory */ + +#if CONFIG_ARCH_INTERRUPTSTACK > 3 + istackbase = (uint32)&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 */ + + sh1_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 + { + sh1_stackdump(sp, ustackbase); + } + + /* Then dump the registers (if available) */ + + sh1_registerdump(); +} + +#endif /* CONFIG_ARCH_STACKDUMP */ diff --git a/nuttx/arch/sh/src/sh1/sh1_initialstate.c b/nuttx/arch/sh/src/sh1/sh1_initialstate.c new file mode 100644 index 000000000..8ad4290e1 --- /dev/null +++ b/nuttx/arch/sh/src/sh1/sh1_initialstate.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * arch/sh/src/sh1/sh1_initialstate.c + * + * Copyright (C) 2008, 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 "up_internal.h" +#include "up_arch.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_getsr + ****************************************************************************/ + +static inline irqstate_t up_getsr(void) +{ + irqstate_t flags; + + __asm__ __volatile__ + ( + "stc sr, %0\n\t" + : "=&z" (flags) + : + : "memory" + ); + return flags; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_initial_state + * + * Description: + * A new thread is being started and a new TCB + * has been created. This function is called to initialize + * the processor specific portions of the new TCB. + * + * This function must setup the intial architecture registers + * and/or stack so that execution will begin at tcb->start + * on the next context switch. + * + ****************************************************************************/ + +void up_initial_state(_TCB *tcb) +{ + struct xcptcontext *xcp = &tcb->xcp; + + /* Initialize the initial exception register context structure */ + + memset(xcp, 0, sizeof(struct xcptcontext)); + xcp->regs[REG_SP] = (uint32)tcb->adj_stack_ptr; + xcp->regs[REG_PC] = (uint32)tcb->start; +#ifdef CONFIG_SUPPRESS_INTERRUPTS + xcp->regs[REG_SR] = up_getsr() | 0x000000f0; +#else + xcp->regs[REG_SR] = up_getsr() & ~0x000000f0; +#endif +} -- cgit v1.2.3