diff options
author | patacongo <patacongo@42af7a65-404d-4744-a932-0658087f49c3> | 2007-04-28 19:39:18 +0000 |
---|---|---|
committer | patacongo <patacongo@42af7a65-404d-4744-a932-0658087f49c3> | 2007-04-28 19:39:18 +0000 |
commit | cfa89740728582ffd77b868c732c45cef8c20ea9 (patch) | |
tree | 14e026aaf0cc6b424581434e0986f367d69adfc7 /nuttx/arch/arm/src | |
parent | 02755ff439dd361643294af1c10937845d848f69 (diff) | |
download | px4-nuttx-cfa89740728582ffd77b868c732c45cef8c20ea9.tar.gz px4-nuttx-cfa89740728582ffd77b868c732c45cef8c20ea9.tar.bz2 px4-nuttx-cfa89740728582ffd77b868c732c45cef8c20ea9.zip |
Common ARM support
git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@187 42af7a65-404d-4744-a932-0658087f49c3
Diffstat (limited to 'nuttx/arch/arm/src')
58 files changed, 10409 insertions, 0 deletions
diff --git a/nuttx/arch/arm/src/Makefile b/nuttx/arch/arm/src/Makefile new file mode 100644 index 000000000..25c17b0bd --- /dev/null +++ b/nuttx/arch/arm/src/Makefile @@ -0,0 +1,124 @@ +############################################################################ +# Makefile +# +# Copyright (C) 2007 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 Gregory Nutt 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs +-include chip/Make.defs + +MKDEP = $(TOPDIR)/tools/mkdeps.sh +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(TOPDIR)/sched + +HEAD_AOBJ = $(HEAD_ASRC:.S=$(OBJEXT)) + +ASRCS = $(CHIP_ASRCS) $(CMN_ASRCS) +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = $(CHIP_CSRCS) $(CMN_CSRCS) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +LDFLAGS = $(ARCHSCRIPT) +EXTRA_LIBS = + +LINKLIBS = +LDPATHES = $(addprefix -L$(TOPDIR)/,$(dir $(LINKLIBS))) +LDLIBS = $(patsubst lib%,-l%,$(basename $(notdir $(LINKLIBS)))) + +BOARDDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src/board + +LIBGCC = ${shell $(CC) -print-libgcc-file-name} + +VPATH = chip:common + +all: $(HEAD_OBJ) libarch$(LIBEXT) + +$(AOBJS) $(HEAD_AOBJ): %$(OBJEXT): %.S + $(CC) -c $(CFLAGS) -D__ASSEMBLY__ $< -o $@ + +$(COBJS): %$(OBJEXT): %.c + $(CC) -c $(CFLAGS) $< -o $@ + +libarch$(LIBEXT): $(OBJS) + @( for obj in $(OBJS) ; do \ + $(AR) $@ $${obj} || \ + { echo "$(AR) $@ $obj FAILED!" ; exit 1 ; } ; \ + done ; ) + +board/libboard$(LIBEXT): + $(MAKE) -C board TOPDIR=$(TOPDIR) libboard$(LIBEXT) + +nuttx: $(HEAD_AOBJ) board/libboard$(LIBEXT) + $(LD) --entry=__start $(LDFLAGS) $(LDPATHES) -L$(BOARDDIR) -o $(TOPDIR)/$@ $(HEAD_AOBJ) \ + --start-group $(LDLIBS) -lboard --end-group $(EXTRA_LIBS) $(LIBGCC) + @$(NM) $(TOPDIR)/$@ | \ + grep -v '\(compiled\)\|\(\$(OBJEXT)$$\)\|\( [aUw] \)\|\(\.\.ng$$\)\|\(LASH[RL]DI\)' | \ + sort > $(TOPDIR)/System.map + @export vflashstart=`$(OBJDUMP) --all-headers $(TOPDIR)/$@ | grep _vflashstart | cut -d' ' -f1`; \ + if [ ! -z "$$vflashstart" ]; then \ + $(OBJCOPY) --adjust-section-vma=.vector=0x$$vflashstart $(TOPDIR)/$@ $(TOPDIR)/$@.flashimage; \ + mv $(TOPDIR)/$@.flashimage $(TOPDIR)/$@; \ + fi +ifeq ($(CONFIG_RRLOAD_BINARY),y) + @$(TOPDIR)/tools/mkimage.sh --Prefix $(CROSSDEV) $(TOPDIR)/$@ $(TOPDIR)/$@.rr + @if [ -w /tftpboot ] ; then \ + cp -f $(TOPDIR)/$@.rr /tftpboot/$@.rr.${CONFIG_ARCH}; \ + fi +endif + +.depend: Makefile $(SRCS) + @if [ -e board/Makefile ]; then \ + $(MAKE) -C board TOPDIR=$(TOPDIR) depend ; \ + if + $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + @if [ -e board/Makefile ]; then \ + $(MAKE) -C board TOPDIR=$(TOPDIR) clean ; \ + fi + @rm -f libarch$(LIBEXT) *~ + @if [ ! -z "$(OBJEXT)" ]; then rm -f *$(OBJEXT); fi + +distclean: clean + @if [ -e board/Makefile ]; then \ + $(MAKE) -C board TOPDIR=$(TOPDIR) distclean ; \ + fi + @rm -f Make.dep .depend + +-include Make.dep diff --git a/nuttx/arch/arm/src/c5471/Make.defs b/nuttx/arch/arm/src/c5471/Make.defs new file mode 100644 index 000000000..54d337938 --- /dev/null +++ b/nuttx/arch/arm/src/c5471/Make.defs @@ -0,0 +1,48 @@ +############################################################################ +# c5471/Make.defs +# +# Copyright (C) 2007 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 Gregory Nutt 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. +# +############################################################################ + +HEAD_ASRC = up_nommuhead.S + +CMN_ASRCS = up_saveusercontext.S up_fullcontextrestore.S +CMN_CSRCS = up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \ + up_createstack.c up_dataabort.c up_delay.c up_exit.c up_idle.c \ + up_initialize.c up_initialstate.c up_interruptcontext.c \ + up_prefetchabort.c up_releasepending.c up_releasestack.c \ + up_reprioritizertr.c up_schedulesigaction.c up_sigdeliver.c \ + up_syscall.c up_unblocktask.c up_undefinedinsn.c up_usestack.c + +CHIP_ASRCS = c5471_lowputc.S c5471_vectors.S +CHIP_CSRCS = c5471_doirq.c c5471_irq.c c5471_serial.c c5471_timerisr.c \ + c5471_watchdog.c diff --git a/nuttx/arch/arm/src/c5471/c5471_doirq.c b/nuttx/arch/arm/src/c5471/c5471_doirq.c new file mode 100644 index 000000000..19d898b4c --- /dev/null +++ b/nuttx/arch/arm/src/c5471/c5471_doirq.c @@ -0,0 +1,104 @@ +/************************************************************ + * c5471/c5471_doirq.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <nuttx/irq.h> +#include <nuttx/arch.h> +#include <assert.h> +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Definitions + ************************************************************/ + +/************************************************************ + * Public Data + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Functions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +void c5471_doirq(int irq, uint32* regs) +{ + up_ledon(LED_INIRQ); +#ifdef CONFIG_SUPPRESS_INTERRUPTS + PANIC(OSERR_ERREXCEPTION); +#else + if ((unsigned)irq < NR_IRQS) + { + /* Current regs non-zero indicates that we are processing + * an interrupt; current_regs is also used to manage + * interrupt level context switches. + */ + + current_regs = regs; + + /* Mask and acknowledge the interrupt */ + + up_maskack_irq(irq); + + /* Deliver the IRQ */ + + irq_dispatch(irq, regs); + + /* Indicate that we are no long in an interrupt handler */ + + current_regs = NULL; + + /* Unmask the last interrupt (global interrupts are still + * disabled. + */ + + up_enable_irq(irq); + } + up_ledoff(LED_INIRQ); +#endif +} diff --git a/nuttx/arch/arm/src/c5471/c5471_irq.c b/nuttx/arch/arm/src/c5471/c5471_irq.c new file mode 100644 index 000000000..a9ab7b166 --- /dev/null +++ b/nuttx/arch/arm/src/c5471/c5471_irq.c @@ -0,0 +1,242 @@ +/************************************************************ + * c5471/c5471_irq.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <nuttx/irq.h> +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Definitions + ************************************************************/ + +#define ILR_EDGESENSITIVE 0x00000020 +#define ILR_PRIORITY 0x0000001E + +/************************************************************ + * Public Data + ************************************************************/ + +uint32 *current_regs; + +/************************************************************ + * Private Data + ************************************************************/ + +/* The value of _vflashstart is defined in ld.script. It + * could be hard-coded because we know that correct IRAM + * area is 0xffc00000. + */ + +extern int _svectors; /* Type does not matter */ + +/* The C5471 has FLASH at the low end of memory. The + * rrload bootloaer will catch all interrupts and re-vector + * them to vectors stored in IRAM. The following table is + * used to initialize those vectors. + */ + +static up_vector_t g_vectorinittab[] = +{ + (up_vector_t)NULL, + up_vectorundefinsn, + up_vectorswi, + up_vectorprefetch, + up_vectordata, + up_vectoraddrexcptn, + up_vectorirq, + up_vectorfiq +}; +#define NVECTORS ((sizeof(g_vectorinittab)) / sizeof(up_vector_t)) + +/************************************************************ + * Private Functions + ************************************************************/ + +/************************************************************ + * Name: up_ackirq + * + * Description: + * Acknowlede the IRQ.Bit 0 of the Interrupt Control + * Register == New IRQ agreement (NEW_IRQ_AGR). Reset IRQ + * output. Clear source IRQ register. Enables a new IRQ + * generation. Reset by internal logic. + * + ************************************************************/ + +static inline void up_ackirq(unsigned int irq) +{ + uint32 reg; + reg = getreg32(SRC_IRQ_REG); /* Insure appropriate IT_REG bit clears */ + putreg32(reg | 0x00000001, INT_CTRL_REG); /* write the NEW_IRQ_AGR bit. */ +} + +/************************************************************ + * Name: up_ackfiq + * + * Description: + * Acknowledge the FIQ. Bit 1 of the Interrupt Control + * Register == New FIQ agreement (NEW_FIQ_AGR). Reset FIQ + * output. Clear source FIQ register. Enables a new FIQ + * generation. Reset by internal logic. + * + ************************************************************/ + +static inline void up_ackfiq(unsigned int irq) +{ + uint32 reg; + reg = getreg32(SRC_FIQ_REG); /* Insure appropriate IT_REG bit clears */ + putreg32(reg | 0x00000002, INT_CTRL_REG); /* write the NEW_FIQ_AGR bit. */ +} + +/************************************************************ + * Name: up_vectorinitialize + ************************************************************/ + +static inline void up_vectorinitialize(void) +{ + up_vector_t *src = g_vectorinittab; + up_vector_t *dest = (up_vector_t*)&_svectors; + int i; + + for (i = 0; i < NVECTORS; i++) + { + *dest++ = *src++; + } +} + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_irqinitialize + ************************************************************/ + +void up_irqinitialize(void) +{ + /* Disable all interrupts. */ + + putreg32(0x0000ffff, MASK_IT_REG); + + /* Clear any pending interrupts */ + + up_ackirq(0); + up_ackfiq(0); + putreg32(0x00000000, IT_REG); + + /* Override hardware defaults */ + + putreg32(ILR_EDGESENSITIVE | ILR_PRIORITY, ILR_IRQ2_REG); + putreg32(ILR_EDGESENSITIVE | ILR_PRIORITY, ILR_IRQ4_REG); + putreg32(ILR_PRIORITY, ILR_IRQ6_REG); + putreg32(ILR_EDGESENSITIVE | ILR_PRIORITY, ILR_IRQ15_REG); + + /* Initialize hardware interrupt vectors */ + + up_vectorinitialize(); + current_regs = NULL; + + /* And finally, enable interrupts */ + +#ifndef CONFIG_SUPPRESS_INTERRUPTS + irqrestore(SVC_MODE | PSR_F_BIT); +#endif +} + +/************************************************************ + * Name: up_disable_irq + * + * Description: + * Disable the IRQ specified by 'irq' + * + ************************************************************/ + +void up_disable_irq(int irq) +{ + if ((unsigned)irq < NR_IRQS) + { + uint32 reg = getreg32(MASK_IT_REG); + putreg32(reg | (1 << irq), MASK_IT_REG); + } +} + +/************************************************************ + * Name: up_enable_irq + * + * Description: + * Enable the IRQ specified by 'irq' + * + ************************************************************/ + +void up_enable_irq(int irq) +{ + if ((unsigned)irq < NR_IRQS) + { + uint32 reg = getreg32(MASK_IT_REG); + putreg32(reg & ~(1 << irq), MASK_IT_REG); + } +} + +/************************************************************ + * Name: up_maskack_irq + * + * Description: + * Mask the IRQ and acknowledge it + * + ************************************************************/ + +void up_maskack_irq(int irq) +{ + uint32 reg = getreg32(INT_CTRL_REG); + + /* Mask the interrupt */ + + reg = getreg32(MASK_IT_REG); + putreg32(reg | (1 << irq), MASK_IT_REG); + + /* Set the NEW_IRQ_AGR bit. This clears the IRQ src register + * enables generation of a new IRQ. + */ + + reg = getreg32(INT_CTRL_REG); + putreg32(reg | 0x00000001, INT_CTRL_REG); /* write the NEW_IRQ_AGR bit. */ +} diff --git a/nuttx/arch/arm/src/c5471/c5471_lowputc.S b/nuttx/arch/arm/src/c5471/c5471_lowputc.S new file mode 100644 index 000000000..78154fd8c --- /dev/null +++ b/nuttx/arch/arm/src/c5471/c5471_lowputc.S @@ -0,0 +1,127 @@ +/************************************************************************** + * c5471/c5471_lowputc.S + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include "up_internal.h" +#include "up_arch.h" + +/************************************************************************** + * Private Definitions + **************************************************************************/ + +/************************************************************************** + * Private Types + **************************************************************************/ + +/************************************************************************** + * Private Function Prototypes + **************************************************************************/ + +/************************************************************************** + * Global Variables + **************************************************************************/ + +/************************************************************************** + * Private Variables + **************************************************************************/ + +/************************************************************************** + * Private Functions + **************************************************************************/ + +/************************************************************************** + * Public Functions + **************************************************************************/ + +/************************************************************************** + * Name: up_lowputc + **************************************************************************/ + +/* This assembly language version has the advantage that it can does not + * require a C stack and uses only r0-r1. Hence it can be used during + * early boot phases. + */ + + .text + .global up_lowputc + .type up_lowputc, function +up_lowputc: + /* On entry, r0 holds the character to be printed */ + +#ifdef CONFIG_SERIAL_IRDA_CONSOLE + ldr r2, =UART_IRDA_BASE /* r2=IRDA UART base */ +#else + ldr r2, =UART_MODEM_BASE /* r2=Modem UART base */ +#endif + + /* Poll bit 0 of the UART_SSR register. When the bit + * is clear, the TX FIFO is no longer full + */ + +1: ldr r1, [r2, #UART_SSR_OFFS] + tst r1, #UART_SSR_TXFULL + bne 1b + + /* Send the character by writing it into the UART_THR + * register. + */ + + str r0, [r2, #UART_THR_OFFS] + + /* Wait for the tranmsit holding regiser (THR) to be + * emptied. This is detemined when bit 6 of the LSR + * is set. + */ + +2: ldr r1, [r2, #UART_LSR_OFFS] + tst r1, #0x00000020 + beq 2b + + /* If the character that we just sent was a linefeed, + * then send a carriage return as well. + */ + + teq r0, #'\n' + moveq r0, #'\r' + beq 1b + + /* And return */ + + mov pc, lr + diff --git a/nuttx/arch/arm/src/c5471/c5471_serial.c b/nuttx/arch/arm/src/c5471/c5471_serial.c new file mode 100644 index 000000000..13197816b --- /dev/null +++ b/nuttx/arch/arm/src/c5471/c5471_serial.c @@ -0,0 +1,811 @@ +/************************************************************ + * c5471/c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471c5471_serial.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <unistd.h> +#include <semaphore.h> +#include <string.h> +#include <errno.h> +#include <debug.h> +#include <nuttx/irq.h> +#include <nuttx/arch.h> +#include <nuttx/serial.h> +#include <arch/serial.h> +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Definitions + ************************************************************/ + +#define BASE_BAUD 115200 + +#if defined(CONFIG_UART_IRDA_HWFLOWCONTROL) || defined(CONFIG_UART_MODEM_HWFLOWCONTROL) +# define CONFIG_UART_HWFLOWCONTROL +#endif + +/************************************************************ + * Private Types + ************************************************************/ + +struct uart_regs_s +{ + uint32 ier; + uint32 lcr; + uint32 fcr; +#ifdef CONFIG_UART_HWFLOWCONTROL + uint32 efr; + uint32 tcr; +#endif +}; + +struct up_dev_s +{ + unsigned int uartbase; /* Base address of UART + * registers */ + unsigned int baud_base; /* Base baud for conversions */ + unsigned int baud; /* Configured baud */ + ubyte xmit_fifo_size; /* Size of transmit FIFO */ + ubyte irq; /* IRQ associated with + * this UART */ + ubyte parity; /* 0=none, 1=odd, 2=even */ + ubyte bits; /* Number of bits (7 or 8) */ +#ifdef CONFIG_UART_HWFLOWCONTROL + boolean flowcontrol; /* TRUE: Hardware flow control + * is enabled. */ +#endif + boolean stopbits2; /* TRUE: Configure with 2 + * stop bits instead of 1 */ + struct uart_regs_s regs; /* Shadow copy of readonly regs */ +}; + +/************************************************************ + * Private Function Prototypes + ************************************************************/ + +static int up_setup(struct uart_dev_s *dev); +static void up_shutdown(struct uart_dev_s *dev); +static int up_interrupt(int irq, void *context); +static int up_ioctl(struct file *filep, int cmd, unsigned long arg); +static int up_receive(struct uart_dev_s *dev, unsigned int *status); +static void up_rxint(struct uart_dev_s *dev, boolean enable); +static boolean up_rxfifonotempty(struct uart_dev_s *dev); +static void up_send(struct uart_dev_s *dev, int ch); +static void up_txint(struct uart_dev_s *dev, boolean enable); +static boolean up_txfifonotfull(struct uart_dev_s *dev); +static boolean up_txfifoempty(struct uart_dev_s *dev); + +/************************************************************ + * Private Variables + ************************************************************/ + +struct uart_ops_s g_uart_ops = +{ + .setup = up_setup, + .shutdown = up_shutdown, + .handler = up_interrupt, + .ioctl = up_ioctl, + .receive = up_receive, + .rxint = up_rxint, + .rxfifonotempty = up_rxfifonotempty, + .send = up_send, + .txint = up_txint, + .txfifonotfull = up_txfifonotfull, + .txfifoempty = up_txfifoempty, +}; + +/* I/O buffers */ + +static char g_irdarxbuffer[CONFIG_UART_IRDA_RXBUFSIZE]; +static char g_irdatxbuffer[CONFIG_UART_IRDA_TXBUFSIZE]; +static char g_modemrxbuffer[CONFIG_UART_MODEM_RXBUFSIZE]; +static char g_modemtxbuffer[CONFIG_UART_MODEM_TXBUFSIZE]; + +/* This describes the state of the C5471 serial IRDA port. */ + +static struct up_dev_s g_irdapriv = +{ + .xmit_fifo_size = UART_IRDA_XMIT_FIFO_SIZE, + .baud_base = BASE_BAUD, + .uartbase = UART_IRDA_BASE, + .baud = CONFIG_UART_IRDA_BAUD, + .parity = CONFIG_UART_IRDA_PARITY, + .bits = CONFIG_UART_IRDA_BITS, +#ifdef CONFIG_UART_IRDA_HWFLOWCONTROL + .flowcontrol = TRUE, +#endif + .stopbits2 = CONFIG_UART_IRDA_2STOP, +}; + +static uart_dev_t g_irdaport = +{ + .irq = C5471_IRQ_UART_IRDA, + .recv = + { + .size = CONFIG_UART_IRDA_RXBUFSIZE, + .buffer = g_irdarxbuffer, + }, + .xmit = + { + .size = CONFIG_UART_IRDA_TXBUFSIZE, + .buffer = g_irdatxbuffer, + }, + .ops = &g_uart_ops, + .priv = &g_irdapriv, +}; + +/* This describes the state of the C5471 serial Modem port. */ + +static struct up_dev_s g_modempriv = +{ + .xmit_fifo_size = UART_XMIT_FIFO_SIZE, + .baud_base = BASE_BAUD, + .uartbase = UART_MODEM_BASE, + .baud = CONFIG_UART_MODEM_BAUD, + .parity = CONFIG_UART_MODEM_PARITY, + .bits = CONFIG_UART_MODEM_BITS, +#ifdef CONFIG_UART_MODEM_HWFLOWCONTROL + .flowcontrol = TRUE, +#endif + .stopbits2 = CONFIG_UART_MODEM_2STOP, +}; + +static uart_dev_t g_modemport = +{ + .irq = C5471_IRQ_UART, + .recv = + { + .size = CONFIG_UART_MODEM_RXBUFSIZE, + .buffer = g_modemrxbuffer, + }, + .xmit = + { + .size = CONFIG_UART_MODEM_TXBUFSIZE, + .buffer = g_modemtxbuffer, + }, + .ops = &g_uart_ops, + .priv = &g_modempriv, +}; + +/* Now, which one with be tty0/console and which tty1? */ + +#ifdef CONFIG_SERIAL_IRDA_CONSOLE +# define CONSOLE_DEV g_irdaport +# define TTYS0_DEV g_irdaport +# define TTYS1_DEV g_modemport +#else +# define CONSOLE_DEV g_modemport +# define TTYS0_DEV g_modemport +# define TTYS1_DEV g_irdaport +#endif + +/************************************************************ + * Private Functions + ************************************************************/ + +/************************************************************ + * Name: up_inserial + ************************************************************/ + +static inline uint32 up_inserial(struct up_dev_s *priv, uint32 offset) +{ + return getreg32(priv->uartbase + offset); +} + +/************************************************************ + * Name: up_serialout + ************************************************************/ + +static inline void up_serialout(struct up_dev_s *priv, uint32 offset, uint32 value) +{ + putreg32(value, priv->uartbase + offset); +} + +/************************************************************ + * Name: up_disableuartint + ************************************************************/ + +static inline void up_disableuartint(struct up_dev_s *priv, uint16 *ier) +{ + if (ier) + { + *ier = priv->regs.ier & UART_IER_INTMASK; + } + priv->regs.ier &= ~UART_IER_INTMASK; + up_serialout(priv, UART_IER_OFFS, priv->regs.ier); +} + +/************************************************************ + * Name: up_restoreuartint + ************************************************************/ + +static inline void up_restoreuartint(struct up_dev_s *priv, uint16 ier) +{ + priv->regs.ier |= ier & (UART_IER_RECVINT|UART_IER_XMITINT); + up_serialout(priv, UART_IER_OFFS, priv->regs.ier); +} + +/************************************************************ + * Name: up_waittxfifonotfull + ************************************************************/ + +static inline void up_waittxfifonotfull(struct up_dev_s *priv) +{ + int tmp; + + for (tmp = 1000 ; tmp > 0 ; tmp--) + { + if ((up_inserial(priv, UART_SSR_OFFS) & UART_SSR_TXFULL) == 0) + { + break; + } + } +} +/************************************************************ + * Name: up_disablebreaks + ************************************************************/ + +static inline void up_disablebreaks(struct up_dev_s *priv) +{ + priv->regs.lcr &= ~UART_LCR_BOC; + up_serialout(priv, UART_LCR_OFFS, priv->regs.lcr); +} + +/************************************************************ + * Name: up_enablebreaks + ************************************************************/ + +static inline void up_enablebreaks(struct up_dev_s *priv) +{ + priv->regs.lcr |= UART_LCR_BOC; + up_serialout(priv, UART_LCR_OFFS, priv->regs.lcr); +} + +/************************************************************ + * Name: up_setrate + ************************************************************/ + +static inline void up_setrate(struct up_dev_s *priv, unsigned int rate) +{ + uint32 div_bit_rate; + + switch (rate) + { + case 115200: + div_bit_rate = BAUD_115200; + break; + case 57600: + div_bit_rate = BAUD_57600; + break; + case 38400: + div_bit_rate = BAUD_38400; + break; + case 19200: + div_bit_rate = BAUD_19200; + break; + case 4800: + div_bit_rate = BAUD_4800; + break; + case 2400: + div_bit_rate = BAUD_2400; + break; + case 1200: + div_bit_rate = BAUD_1200; + break; + case 9600: + default: + div_bit_rate = BAUD_9600; + break; + } + + up_serialout(priv, UART_DIV_BIT_RATE_OFFS, div_bit_rate); +} + +/************************************************************ + * Name: up_setup + * + * Description: + * Configure the UART baud, bits, parity, fifos, etc. This + * method is called the first time that the serial port is + * opened. + * + ************************************************************/ + +static int up_setup(struct uart_dev_s *dev) +{ +#ifdef CONFIG_SUPPRESS_UART_CONFIG + struct up_dev_s *priv = dev->priv; + unsigned int cval; + + if (priv->bits == 7) + { + cval = UART_LCR_7BITS; + } + else + { + cval = UART_LCR_8BITS; + } + + if (priv->stopbits2) + { + cval |= UART_LCR_2STOP; + } + + if (priv->parity == 1) /* Odd parity */ + { + cval |= (UART_LCR_PAREN|UART_LCR_PARODD); + } + else if (priv->parity == 2) /* Even parity */ + { + cval |= (UART_LCR_PAREN|UART_LCR_PAREVEN); + } + + /* Both the IrDA and MODEM UARTs support RESET and UART mode. */ + + up_serialout(priv, UART_MDR_OFFS, MDR_RESET_MODE); + up_delay(5); + up_serialout(priv, UART_MDR_OFFS, MDR_UART_MODE); + up_delay(5); + + priv->regs.ier = up_inserial(priv, UART_IER_OFFS); + priv->regs.lcr = up_inserial(priv, UART_LCR_OFFS); +#ifdef CONFIG_UART_HWFLOWCONTROL + if (priv->flowcontrol) + { + priv->regs.efr = up_inserial(priv, UART_EFR_OFFS); + priv->regs.tcr = up_inserial(priv, UART_TCR_OFFS); + } +#endif + + up_disableuartint(priv, NULL); + + up_serialout(priv, UART_EFR_OFFS, 0x0010); /* Enable fifo control */ + up_serialout(priv, UART_TFCR_OFFS, 0); /* Reset to 0 */ + up_serialout(priv, UART_RFCR_OFFS, UART_FCR_RX_CLR); /* Clear RX fifo */ + up_serialout(priv, UART_TFCR_OFFS, UART_FCR_TX_CLR); /* Clear TX fifo */ + up_serialout(priv, UART_TFCR_OFFS, UART_FCR_FIFO_EN); /* Enable RX/TX fifos */ + + up_disablebreaks(priv); + + priv->regs.fcr = (priv->regs.fcr & 0xffffffcf) | (val & 0x30); + up_serialout(priv, UART_RFCR_OFFS, priv->regs.fcr); + + priv->regs.fcr = (priv->regs.fcr & 0xffffff3f) | (val & 0xc0); + up_serialout(priv, UART_RFCR_OFFS, priv->regs.fcr); + + up_setrate(priv, priv->baud); + + priv->regs.lcr &= 0xffffffe0; /* clear original field, and... */ + priv->regs.lcr |= (uint32)mode; /* Set new bits in that field. */ + up_serialout(priv, UART_LCR_OFFS, priv->regs.lcr); + +#ifdef CONFIG_UART_HWFLOWCONTROL + if (priv->flowcontrol) + { + /* Set the FIFO level triggers for flow control + * Halt = 48 bytes, resume = 12 bytes + */ + + priv->regs.tcr = (priv->regs.tcr & 0xffffff00) | 0x0000003c; + up_serialout(priv, UART_TCR_OFFS, priv->regs.tcr); + + /* Enable RTS/CTS flow control */ + + priv->regs.efr |= 0x000000c0; + up_serialout(priv, UART_EFR_OFFS, priv->regs.efr); + } + else + { + /* Disable RTS/CTS flow control */ + + priv->regs.efr &= 0xffffff3f; + up_serialout(priv, UART_EFR_OFFS, priv->regs.efr); + } +#endif +#endif + return OK; +} + +/************************************************************ + * Name: up_shutdown + * + * Description: + * Disable the UART. This method is called when the serial port is closed + * + ************************************************************/ + +static void up_shutdown(struct uart_dev_s *dev) +{ + struct up_dev_s *priv = (struct up_dev_s*)CONSOLE_DEV.priv; + up_disableuartint(priv, NULL); +} + +/************************************************************ + * Name: up_interrupt + * + * Description: + * This is the UART interrupt handler. It will be invoked + * when an interrupt received on the 'irq' It should call + * uart_transmitchars or uart_receivechar to perform the + * appropriate data transfers. The interrupt handling logic\ + * must be able to map the 'irq' number into the approprite + * uart_dev_s structure in order to call these functions. + * + ************************************************************/ + +static int up_interrupt(int irq, void *context) +{ + struct uart_dev_s *dev = NULL; + struct up_dev_s *priv; + volatile uint32 cause; + + if (g_irdaport.irq == irq) + { + dev = &g_irdaport; + } + else if (g_modemport.irq == irq) + { + dev = &g_modemport; + } + else + { + PANIC(OSERR_INTERNAL); + } + priv = (struct up_dev_s*)dev->priv; + + cause = up_inserial(priv, UART_ISR_OFFS) & 0x0000003f; + + if ((cause & 0x0000000c) == 0x0000000c) + { + uint32 ier_val = 0; + + /* Is this an interrupt from the IrDA UART? */ + + if (irq == C5471_IRQ_UART_IRDA) + { + /* Save the currently enabled IrDA UART interrupts + * so that we can restore the IrDA interrupt state + * below. + */ + + ier_val = up_inserial(priv, UART_IER_OFFS); + + /* Then disable all IrDA UART interrupts */ + + up_serialout(priv, UART_IER_OFFS, 0); + } + + /* Receive characters from the RX fifo */ + + uart_recvchars(dev); + + /* read UART_RHR to clear int condition + * toss = up_inserialchar(priv,&status); + */ + + /* Is this an interrupt from the IrDA UART? */ + + if (irq == C5471_IRQ_UART_IRDA) + { + /* Restore the IrDA UART interrupt enables */ + + up_serialout(priv, UART_IER_OFFS, ier_val); + } + } + else if ((cause & 0x0000000c) == 0x00000004) + { + uart_recvchars(dev); + } + + if ((cause & 0x00000002) != 0) + { + uart_xmitchars(dev); + } + + return OK; +} + +/************************************************************ + * Name: up_ioctl + * + * Description: + * All ioctl calls will be routed through this method + * + ************************************************************/ + +static int up_ioctl(struct file *filep, int cmd, unsigned long arg) +{ + struct inode *inode = filep->f_inode; + struct uart_dev_s *dev = inode->i_private; + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + int ret = OK; + + switch (cmd) + { + case TIOCSERGSTRUCT: + { + struct up_dev_s *user = (struct up_dev_s*)arg; + if (!user) + { + *get_errno_ptr() = EINVAL; + ret = ERROR; + } + else + { + memcpy(user, dev, sizeof(struct up_dev_s)); + } + } + break; + + case TIOCSBRK: /* BSD compatibility: Turn break on, unconditionally */ + { + irqstate_t flags = irqsave(); + up_enablebreaks(priv); + irqrestore(flags); + } + break; + + case TIOCCBRK: /* BSD compatibility: Turn break off, unconditionally */ + { + irqstate_t flags; + flags = irqsave(); + up_disablebreaks(priv); + irqrestore(flags); + } + break; + + default: + *get_errno_ptr() = ENOTTY; + ret = ERROR; + break; + } + + return ret; +} + +/************************************************************ + * Name: up_receive + * + * Description: + * Called (usually) from the interrupt level to receive one character from + * the UART. Error bits associated with the receipt are provided in the + * the return 'status'. + * + ************************************************************/ + +static int up_receive(struct uart_dev_s *dev, unsigned int *status) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + uint32 rhr; + uint32 lsr; + + /* Construct a 16bit status word that uses the high byte to + * hold the status bits associated with framing,parity,break + * and a low byte that holds error bits of LSR for + * conditions such as overflow, etc. + */ + + rhr = up_inserial(priv, UART_RHR_OFFS); + lsr = up_inserial(priv, UART_LSR_OFFS); + + *status = (unsigned int)((rhr & 0x0000ff00) | (lsr & 0x000000ff)); + + return rhr & 0x000000ff; +} + +/************************************************************ + * Name: up_rxint + * + * Description: + * Call to enable or disable RX interrupts + * + ************************************************************/ + +static void up_rxint(struct uart_dev_s *dev, boolean enable) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + if (enable) + { +#ifndef CONFIG_SUPPRESS_SERIAL_INTS + priv->regs.ier |= UART_IER_RECVINT; + up_serialout(priv, UART_IER_OFFS, priv->regs.ier); +#endif + } + else + { + priv->regs.ier &= ~UART_IER_RECVINT; + up_serialout(priv, UART_IER_OFFS, priv->regs.ier); + } +} + +/************************************************************ + * Name: up_rxfifonotempty + * + * Description: + * Return TRUE if the receive fifo is not empty + * + ************************************************************/ + +static boolean up_rxfifonotempty(struct uart_dev_s *dev) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + return up_inserial(priv, UART_LSR_OFFS) & UART_RX_FIFO_NOEMPTY; +} + +/************************************************************ + * Name: up_send + * + * Description: + * This method will send one byte on the UART + * + ************************************************************/ + +static void up_send(struct uart_dev_s *dev, int ch) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + up_serialout(priv, UART_THR_OFFS, (ubyte)ch); +} + +/************************************************************ + * Name: up_txint + * + * Description: + * Call to enable or disable TX interrupts + * + ************************************************************/ + +static void up_txint(struct uart_dev_s *dev, boolean enable) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + if (enable) + { +#ifndef CONFIG_SUPPRESS_SERIAL_INTS + priv->regs.ier |= UART_IER_XMITINT; + up_serialout(priv, UART_IER_OFFS, priv->regs.ier); +#endif + } + else + { + priv->regs.ier &= ~UART_IER_XMITINT; + up_serialout(priv, UART_IER_OFFS, priv->regs.ier); + } +} + +/************************************************************ + * Name: up_txfifonotfull + * + * Description: + * Return TRUE if the tranmsit fifo is not full + * + ************************************************************/ + +static boolean up_txfifonotfull(struct uart_dev_s *dev) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + return (up_inserial(priv, UART_SSR_OFFS) & UART_SSR_TXFULL) == 0; +} + +/************************************************************ + * Name: up_txfifoempty + * + * Description: + * Return TRUE if the transmit fifo is empty + * + ************************************************************/ + +static boolean up_txfifoempty(struct uart_dev_s *dev) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + return (up_inserial(priv, UART_LSR_OFFS) & UART_LSR_TREF) != 0; +} + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_serialinit + * + * Description: + * Performs the low level UART initialization early in + * debug so that the serial console will be available + * during bootup. This must be called before up_serialinit. + * + ************************************************************/ + +void up_earlyserialinit(void) +{ + up_disableuartint(TTYS0_DEV.priv, NULL); + up_disableuartint(TTYS1_DEV.priv, NULL); + + CONSOLE_DEV.isconsole = TRUE; + up_setup(&CONSOLE_DEV); +} + +/************************************************************ + * Name: up_serialinit + * + * Description: + * Register serial console and serial ports. This assumes + * that up_earlyserialinit was called previously. + * + ************************************************************/ + +void up_serialinit(void) +{ + (void)uart_register("/dev/console", &CONSOLE_DEV); + (void)uart_register("/dev/ttyS0", &TTYS0_DEV); + (void)uart_register("/dev/ttyS1", &TTYS1_DEV); +} + +/************************************************************ + * Name: up_putc + * + * Description: + * Provide priority, low-level access to support OS debug + * writes + * + ************************************************************/ + +int up_putc(int ch) +{ + struct up_dev_s *priv = (struct up_dev_s*)CONSOLE_DEV.priv; + uint16 ier; + + up_disableuartint(priv, &ier); + up_waittxfifonotfull(priv); + up_serialout(priv, UART_THR_OFFS, (ubyte)ch); + + /* Check for LF */ + + if (ch == '\n') + { + /* Add CR */ + + up_waittxfifonotfull(priv); + up_serialout(priv, UART_THR_OFFS, '\r'); + } + + up_waittxfifonotfull(priv); + up_restoreuartint(priv, ier); + return ch; +} + diff --git a/nuttx/arch/arm/src/c5471/c5471_timerisr.c b/nuttx/arch/arm/src/c5471/c5471_timerisr.c new file mode 100644 index 000000000..e37043173 --- /dev/null +++ b/nuttx/arch/arm/src/c5471/c5471_timerisr.c @@ -0,0 +1,125 @@ +/************************************************************ + * c5471/c5471_timerisr.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <debug.h> +#include <nuttx/arch.h> +#include "clock_internal.h" +#include "up_internal.h" +#include "up_arch.h" + +/************************************************************ + * Definitions + ************************************************************/ + +/* We want the general purpose timer running at the rate + * MSEC_PER_TICK. The C5471 clock is 47.5MHz and we're using + * a timer PTV value of 3 (3 == divide incoming frequency by + * 16) which then yields a 16 bitCLKS_PER_INT value + * of 29687. + * + * 47500000 / 16 = 2968750 clocks/sec + * 2968750 / 100 = 29687 clocks/ 100Hz interrupt + * + */ + +#define CLKS_PER_INT 29687 +#define CLKS_PER_INT_SHIFT 5 +#define AR 0x00000010 +#define ST 0x00000008 +#define PTV 0x00000003 + +/************************************************************ + * Private Types + ************************************************************/ + +/************************************************************ + * Private Function Prototypes + ************************************************************/ + +/************************************************************ + * Global Functions + ************************************************************/ + +/************************************************************ + * Function: up_timerisr + * + * Description: + * The timer ISR will perform a variety of services for + * various portions of the systems. + * + ************************************************************/ + +int up_timerisr(int irq, uint32 *regs) +{ + /* Process timer interrupt */ + + sched_process_timer(); + return 0; +} + +/************************************************************ + * Function: up_timerinit + * + * Description: + * This function is called during start-up to initialize + * the timer interrupt. + * + ************************************************************/ + +void up_timerinit(void) +{ + uint32 val; + + up_disable_irq(C5471_IRQ_SYSTIMER); + + /* Start the general purpose timer running in auto-reload mode + * so that an interrupt is generated at the rate MSEC_PER_TICK. + */ + + val = ((CLKS_PER_INT-1) << CLKS_PER_INT_SHIFT) | AR | ST | PTV; + putreg32(val, C5471_TIMER2_CTRL); + + /* Attach and enable the timer interrupt */ + + irq_attach(C5471_IRQ_SYSTIMER, (xcpt_t)up_timerisr); + up_enable_irq(C5471_IRQ_SYSTIMER); +} + diff --git a/nuttx/arch/arm/src/c5471/c5471_vectors.S b/nuttx/arch/arm/src/c5471/c5471_vectors.S new file mode 100644 index 000000000..328485e17 --- /dev/null +++ b/nuttx/arch/arm/src/c5471/c5471_vectors.S @@ -0,0 +1,449 @@ +/******************************************************************** + * c5471/c5471_vectors.S + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <nuttx/irq.h> +#include "up_arch.h" + +/******************************************************************** + * Definitions + ********************************************************************/ + +/******************************************************************** + * Global Data + ********************************************************************/ + + .data +g_irqtmp: + .word 0 /* Saved lr */ + .word 0 /* Saved spsr */ +g_undeftmp: + .word 0 /* Saved lr */ + .word 0 /* Saved spsr */ +g_aborttmp: + .word 0 /* Saved lr */ + .word 0 /* Saved spsr */ + +/******************************************************************** + * Assembly Macros + ********************************************************************/ + +/******************************************************************** + * Private Functions + ********************************************************************/ + + .text + +/******************************************************************** + * Public Functions + ********************************************************************/ + + .text + +/******************************************************************** + * Name: up_vectorirq + * + * Description: + * Interrupt excetpion. Entered in IRQ mode with spsr = SVC + * CPSR, lr = SVC PC + ********************************************************************/ + + .globl up_vectorirq + .type up_vectorirq, %function +up_vectorirq: + /* On entry, we are in IRQ mode. We are free to use + * the IRQ mode r13 and r14. + * + */ + + ldr r13, .Lirqtmp + sub lr, lr, #4 + str lr, [r13] @ save lr_IRQ + mrs lr, spsr + str lr, [r13, #4] @ save spsr_IRQ + + /* Then switch back to SVC mode */ + + bic lr, lr, #MODE_MASK /* Keep F and T bits */ + orr lr, lr, #(SVC_MODE | PSR_I_BIT) + msr cpsr_c, lr /* Switch to SVC mode */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 + + /* Get the values for r15(pc) and CPSR in r3 and r4 */ + + ldr r0, .Lirqtmp /* Points to temp storage */ + ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Now decode the interrupt */ + +#if 0 + ldr lr, =SRC_IRQ_BIN_REG /* Fetch encoded IRQ */ + ldr r0, [lr] + and r0, r0, #0x0f /* Valid range is 0..15 */ + + /* Problems here... cannot read SRC_IRQ_BIN_REQ (and/or + * SRC_IRQ_REQ because this will clear edge triggered + * interrupts. Plus, no way to validate spurious + * interrupt. + */ +#else + ldr r6, =SRC_IRQ_REG + ldr r6, [r6] /* Get source IRQ reg */ + mov r0, #0 /* Assume IRQ0_IRQ set */ +.Lmorebits: + tst r6, #1 /* Is IRQ set? */ + bne .Lhaveirq /* Yes... we have the IRQ */ + add r0, r0, #1 /* Setup next IRQ */ + mov r6, r6, lsr #1 /* Shift right one */ + cmp r0, #16 /* Only 16 valid bits */ + bcc .Lmorebits /* Keep until we have looked + * at all bits */ + b .Lnoirqset /* If we get here, there is + * no pending interrupt */ +.Lhaveirq: +#endif + /* Then call the IRQ handler with interrupt disabled. */ + + mov fp, #0 /* Init frame pointer */ + mov r1, sp /* Get r1=xcp */ + bl c5471_doirq /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ +.Lnoirqset: + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr, r0 + ldmia sp, {r0-r15}^ /* Return */ + +.Lirqtmp: + .word g_irqtmp + + .align 5 + +/******************************************************************** + * Function: up_vectorswi + * + * Description: + * SWI interrupt. We enter the SWI in SVC mode + ********************************************************************/ + + .globl up_vectorswi + .type up_vectorswi, %function +up_vectorswi: + + /* The c547x rrload bootloader intemediates all + * interrupts. For the* case of the SWI, it mucked + * with the stack to create some temporary registers. + * We'll have to recover from this mucking here. + */ + + ldr r14, [sp,#-0x4] /* rrload workaround */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp), r14(lr), r15(pc) + * and CPSR in r1-r4 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 /* R14 is altered on return from SWI */ + mov r3, r14 /* Save r14 as the PC as well */ + mrs r4, spsr /* Get the saved CPSR */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the SWI handler with interrupt disabled. + * void up_syscall(struct xcptcontext *xcp) + */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_syscall /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ + + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr, r0 + ldmia sp, {r0-r15}^ /* Return */ + + .align 5 + +/******************************************************************** + * Name: up_vectordata + * + * Description: + * Data abort Exception dispatcher. Give control to data + * abort handler. This function is entered in ABORT mode + * with spsr = SVC CPSR, lr = SVC PC + * + ********************************************************************/ + + .globl up_vectordata + .type up_vectordata, %function +up_vectordata: + /* On entry we are free to use the ABORT mode registers + * r13 and r14 + */ + + ldr r13, .Ldaborttmp /* Points to temp storage */ + sub lr, lr, #8 /* Fixup return */ + str lr, [r13] /* Save in temp storage */ + mrs lr, spsr /* Get SPSR */ + str lr, [r13, #4] /* Save in temp storage */ + + /* Then switch back to SVC mode */ + + bic lr, lr, #MODE_MASK /* Keep F and T bits */ + orr lr, lr, #(SVC_MODE | PSR_I_BIT) + msr cpsr_c, lr /* Switch to SVC mode */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 + + /* Get the values for r15(pc) and CPSR in r3 and r4 */ + + ldr r0, .Ldaborttmp /* Points to temp storage */ + ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the data abort handler with interrupt disabled. + * void up_dataabort(struct xcptcontext *xcp) + */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_dataabort /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ + + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr_cxsf, r0 + ldmia sp, {r0-r15}^ /* Return */ + +.Ldaborttmp: + .word g_aborttmp + + .align 5 + +/******************************************************************** + * Name: up_vectorprefetch + * + * Description: + * Prefetch abort exception. Entered in ABT mode with + * spsr = SVC CPSR, lr = SVC PC + ********************************************************************/ + + .globl up_vectorprefetch + .type up_vectorprefetch, %function +up_vectorprefetch: + /* On entry we are free to use the ABORT mode registers + * r13 and r14 + */ + + ldr r13, .Lpaborttmp /* Points to temp storage */ + sub lr, lr, #4 /* Fixup return */ + str lr, [r13] /* Save in temp storage */ + mrs lr, spsr /* Get SPSR */ + str lr, [r13, #4] /* Save in temp storage */ + + /* Then switch back to SVC mode */ + + bic lr, lr, #MODE_MASK /* Keep F and T bits */ + orr lr, lr, #(SVC_MODE | PSR_I_BIT) + msr cpsr_c, lr /* Switch to SVC mode */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 + + /* Get the values for r15(pc) and CPSR in r3 and r4 */ + + ldr r0, .Lpaborttmp /* Points to temp storage */ + ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the prefetch abort handler with interrupt disabled. + * void up_prefetchabort(struct xcptcontext *xcp) + */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_prefetchabort /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ + + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr_cxsf, r0 + ldmia sp, {r0-r15}^ /* Return */ + +.Lpaborttmp: + .word g_aborttmp + + .align 5 + +/******************************************************************** + * Name: up_vectorundefinsn + * + * Description: + * Undefined instruction entry exception. Entered in + * UND mode, spsr = SVC CPSR, lr = SVC PC + * + ********************************************************************/ + + .globl up_vectorundefinsn + .type up_vectorundefinsn, %function +up_vectorundefinsn: + /* On entry we are free to use the UND mode registers + * r13 and r14 + */ + + ldr r13, .Lundeftmp /* Points to temp storage */ + str lr, [r13] /* Save in temp storage */ + mrs lr, spsr /* Get SPSR */ + str lr, [r13, #4] /* Save in temp storage */ + + /* Then switch back to SVC mode */ + + bic lr, lr, #MODE_MASK /* Keep F and T bits */ + orr lr, lr, #(SVC_MODE | PSR_I_BIT) + msr cpsr_c, lr /* Switch to SVC mode */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 + + /* Get the values for r15(pc) and CPSR in r3 and r4 */ + + ldr r0, .Lundeftmp /* Points to temp storage */ + ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the undef insn handler with interrupt disabled. + * void up_undefinedinsn(struct xcptcontext *xcp) + */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_undefinedinsn /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ + + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr_cxsf, r0 + ldmia sp, {r0-r15}^ /* Return */ + +.Lundeftmp: + .word g_undeftmp + + .align 5 + +/******************************************************************** + * Name: up_vectorfiq + * + * Description: + * Shouldn't happen + ********************************************************************/ + + .globl up_vectorfiq + .type up_vectorfiq, %function +up_vectorfiq: + subs pc, lr, #4 + +/******************************************************************** + * Name: up_vectoraddrexcption + * + * Description: + * Shouldn't happen + * + ********************************************************************/ + + .globl up_vectoraddrexcptn + .type up_vectoraddrexcptn, %function +up_vectoraddrexcptn: + b up_vectoraddrexcptn + .end diff --git a/nuttx/arch/arm/src/c5471/c5471_watchdog.c b/nuttx/arch/arm/src/c5471/c5471_watchdog.c new file mode 100644 index 000000000..04b390a5c --- /dev/null +++ b/nuttx/arch/arm/src/c5471/c5471_watchdog.c @@ -0,0 +1,392 @@ +/************************************************************************** + * c5471/c5471_watchdog.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <stdio.h> +#include <debug.h> +#include <errno.h> +#include <nuttx/fs.h> +#include <nuttx/irq.h> +#include <arch/watchdog.h> +#include "up_arch.h" + +/************************************************************************** + * Definitions + **************************************************************************/ + +#undef CONFIG_SOFTWARE_TEST +#undef CONFIG_SOFTWARE_REBOOT +#undef CONFIG_WATCHDOG_STRICT + +#define MAX_WDT_USEC 353200 +#define MAX_PRESCALER 256 +#define C5471_TIMER_STOP 0 + +#define C5471_TIMER_PRESCALER 0x07 /* Bits 0-2: Prescale value */ +#define C5471_TIMER_STARTBIT (1 << 3) /* Bit 3: Start timer bit */ +#define C5471_TIMER_AUTORELOAD (1 << 4) /* Bit 4: Auto-reload timer */ +#define C5471_TIMER_LOADTIM (0xffff << 5) /* Bits 20-5: Load timer value */ +#define C5471_TIMER_MODE (1 << 21) /* Bit 21: Timer mode */ +#define C5471_DISABLE_VALUE1 (0xf5 << 22) /* Bits 29-22: WD disable */ +#define C5471_DISABLE_VALUE2 (0xa0 << 22) + +#define CLOCK_KHZ 47500 +#define CLOCK_MHZx2 95 + +/* Macros to manage access to to watchdog timer macros */ + +#define c5471_wdt_cntl (*(volatile uint32*)C5471_TIMER0_CTRL) +#define c5471_wdt_count (*(volatile uint32*)C5471_TIMER0_CNT) + +/************************************************************************** + * Private Types + **************************************************************************/ + +/************************************************************************** + * Private Function Prototypes + **************************************************************************/ + +/* Local implementation of timer interface */ + +static inline unsigned int wdt_prescaletoptv(unsigned int prescale); + +static int wdt_setusec(uint32 usec); +static int wdt_interrupt(int irq, void *context); + +static int wdt_open(struct file *filep); +static int wdt_close(struct file *filep); +static ssize_t wdt_read(struct file *filep, char *buffer, size_t buflen); +static ssize_t wdt_write(struct file *filep, const char *buffer, size_t buflen); +static int wdt_ioctl(struct file *filep, int cmd, uint32 arg); + +/************************************************************************** + * Private Data + **************************************************************************/ + +static boolean g_wdtopen; + +struct file_operations g_wdtops = +{ + .open = wdt_open, + .close = wdt_close, + .read = wdt_read, + .write = wdt_write, + .ioctl = wdt_ioctl, +}; + +/************************************************************************** + * Private Functions + **************************************************************************/ + +/************************************************************************** + * Name: wdt_prescaletoptv + **************************************************************************/ + +static inline unsigned int wdt_prescaletoptv(unsigned int prescale) +{ + unsigned int ptv = 0; + + if (prescale > 255) + { + ptv = 7; + } + else + { + unsigned int value = prescale >> 1; + + /* 0: 0-2 + * 1: 3-4 + * 2: 5-8 + * 3: 9-16 + * 4: 17-32 + * 5: 33-64 + * 6: 65-128 + * 7: 129- + */ + + while (value > 1) + { + value >>= 1; + ptv++; + } + } + + dbg("prescale=%d -> ptv=%d\n", prescale, ptv); + return ptv; +} + +/************************************************************************** + * Name: wdt_setusec + **************************************************************************/ + +static int wdt_setusec(uint32 usec) +{ + /* prescaler: clock / prescaler = #clock ticks per counter in ptv + * divisor: #counts until the interrupt comes. + */ + + uint32 prescaler = MAX_PRESCALER; + uint32 divisor = 1; + uint32 mode; + + dbg("usec=%d\n", usec); + + /* Calculate a value of prescaler and divisor that will be able + * to count to the usec. It may not be exact or the best + * possible set, but it's a quick and simple algorithm. + * + * divisor max = 0x10000 + * prescaler max = MAX_PRESCALER + */ + + do + { + divisor = (CLOCK_MHZx2 * usec) / (prescaler * 2); + dbg("divisor=0x%x prescaler=0x%x\n", divisor, prescaler); + + if (divisor >= 0x10000) + { + if (prescaler == MAX_PRESCALER) + { + /* This is the max possible ~2.5 seconds. */ + + dbg("prescaler=0x%x too big!\n", prescaler); + return ERROR; + } + + prescaler <<= 1; + if (prescaler > MAX_PRESCALER) + { + prescaler = MAX_PRESCALER; + } + } + } + while (divisor >= 0x10000); + + dbg("prescaler=0x%x divisor=0x%x\n", prescaler, divisor); + + mode = wdt_prescaletoptv(prescaler); + mode &= ~C5471_TIMER_AUTORELOAD; /* One shot mode. */ + mode |= divisor << 5; + dbg("mode=0x%x\n", mode); + + c5471_wdt_cntl = mode; + + /* Now start the watchdog */ + + c5471_wdt_cntl |= C5471_TIMER_STARTBIT; + dbg("cntl_timer=0x%x\n", c5471_wdt_cntl); + + return 0; +} + +/************************************************************************** + * Private Functions + **************************************************************************/ + +/************************************************************************** + * Name: wdt_interrupt + **************************************************************************/ + +static int wdt_interrupt(int irq, void *context) +{ + dbg("expired\n"); + +#if defined(CONFIG_SOFTWARE_REBOOT) +# if defined(CONFIG_SOFTWARE_TEST) + dbg(" Test only\n"); +# else + dbg(" Re-booting\n"); +# warning "Add logic to reset CPU here" +# endif +#else + dbg(" No reboot\n"); +#endif + return OK; +} + +/************************************************************************** + * Name: wdt_read + **************************************************************************/ + +static ssize_t wdt_read(struct file *filep, char *buffer, size_t buflen) +{ + /* We are going to return "NNNNNNNN NNNNNNNN." The followig logic will + * not work if the user provides a buffer smaller than 18 bytes. + */ + + dbg("buflen=%d\n", buflen); + if (buflen >= 18) + { + sprintf(buffer, "#08x %08x\n", c5471_wdt_cntl, c5471_wdt_count); + return 18; + } + return 0; +} + +/************************************************************************** + * Name: wdt_write + **************************************************************************/ + +static ssize_t wdt_write(struct file *filep, const char *buffer, size_t buflen) +{ + dbg("buflen=%d\n", buflen); + if (buflen) + { + /* Reset the timer to the maximum delay */ + + wdt_setusec(MAX_WDT_USEC); + return 1; + } + + return 0; +} + +/************************************************************************** + * Name: wdt_ioctl + **************************************************************************/ + +static int wdt_ioctl(struct file *filep, int cmd, uint32 arg) +{ + dbg("ioctl Call: cmd=0x%x arg=0x%x", cmd, arg); + + /* Process the the IOCTL command (see arch/watchdog.h) */ + + switch(cmd) + { + case WDIOC_KEEPALIVE: + wdt_setusec(MAX_WDT_USEC); + break; + + default: + *get_errno_ptr() = ENOTTY; + return ERROR; + } + + return OK; +} + +/************************************************************************** + * Name: wdt_open + **************************************************************************/ + +static int wdt_open(struct file *filep) +{ + dbg(""); + + if (g_wdtopen) + { + *get_errno_ptr() = EBUSY; + } + + /* This will automatically load the timer with its max + * count and start it running. + */ + + c5471_wdt_cntl = C5471_DISABLE_VALUE1; + c5471_wdt_cntl = C5471_DISABLE_VALUE2; + + g_wdtopen = TRUE; + return OK; +} + +/************************************************************************** + * Name: wdt_close + **************************************************************************/ + +static int wdt_close(struct file *filep) +{ + dbg(""); + + /* The task controlling the watchdog has terminated. Take the timer + * the + * watchdog in interrupt mode -- we are going to reset unless the + * reopened again soon. + */ + +#ifndef CONFIG_WATCHDOG_STRICT + c5471_wdt_cntl = C5471_TIMER_MODE; +#endif + + g_wdtopen = FALSE; + return 0; +} + +/************************************************************************** + * Public Functions + **************************************************************************/ + +/************************************************************************** + * Name: up_wdtinit + **************************************************************************/ + +int up_wdtinit(void) +{ + int ret; + + dbg("C547x Watchdog Driver\n"); + + /* Register as /dev/wdt */ + + ret = register_inode("/dev/wdt", &g_wdtops, 0666, NULL); + if (ret) + { + return ERROR; + } + + /* Register for an interrupt level callback through wdt_interrupt */ + + dbg("Attach to IRQ=%d\n", C5471_IRQ_WATCHDOG); + + /* Make sure that the timer is stopped */ + + c5471_wdt_cntl = C5471_TIMER_STOP; + + /* Request the interrupt. */ + + ret = irq_attach(C5471_IRQ_WATCHDOG, wdt_interrupt); + if (ret) + { + unregister_inode("/dev/wdt"); + return ERROR; + } + + return OK; +} diff --git a/nuttx/arch/arm/src/c5471/chip.h b/nuttx/arch/arm/src/c5471/chip.h new file mode 100644 index 000000000..74ff3c509 --- /dev/null +++ b/nuttx/arch/arm/src/c5471/chip.h @@ -0,0 +1,323 @@ +/************************************************************ + * c5471/chip.h + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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. + * + ************************************************************/ + +#ifndef __C5471_CHIP_H +#define __C5471_CHIP_H + +/************************************************************ + * Included Files + ************************************************************/ + +/************************************************************ + * Definitions + ************************************************************/ + +/* UARTs ****************************************************/ + +#define UART_IRDA_BASE 0xffff0800 +#define UART_MODEM_BASE 0xffff1000 +#define UARTn_IO_RANGE 0x00000800 + +/* Common UART Registers. Expressed as offsets from the BASE address */ + +#define UART_RHR_OFFS 0x00000000 /* Rcv Holding Register */ +#define UART_THR_OFFS 0x00000004 /* Xmit Holding Register */ +#define UART_FCR_OFFS 0x00000008 /* FIFO Control Register */ +#define UART_RFCR_OFFS 0x00000008 /* Rcv FIFO Control Register */ +#define UART_TFCR_OFFS 0x00000008 /* Xmit FIFO Control Register */ +#define UART_SCR_OFFS 0x0000000c /* Status Control Register */ +#define UART_LCR_OFFS 0x00000010 /* Line Control Register */ +#define UART_LSR_OFFS 0x00000014 /* Line Status Register */ +#define UART_SSR_OFFS 0x00000018 /* Supplementary Status Register */ +#define UART_MCR_OFFS 0x0000001c /* Modem Control Register */ +#define UART_MSR_OFFS 0x00000020 /* Modem Status Register */ +#define UART_IER_OFFS 0x00000024 /* Interrupt Enable Register */ +#define UART_ISR_OFFS 0x00000028 /* Interrupt Status Register */ +#define UART_EFR_OFFS 0x0000002c /* Enhanced Feature Register */ +#define UART_XON1_OFFS 0x00000030 /* XON1 Character Register */ +#define UART_XON2_OFFS 0x00000034 /* XON2 Character Register */ +#define UART_XOFF1_OFFS 0x00000038 /* XOFF1 Character Register */ +#define UART_XOFF2_OFFS 0x0000003c /* XOFF2 Character Register */ +#define UART_SPR_OFFS 0x00000040 /* Scratch-pad Register */ +#define UART_DIV_115K_OFFS 0x00000044 /* Divisor for baud generation */ +#define UART_DIV_BIT_RATE_OFFS 0x00000048 /* For baud rate generation */ +#define UART_TCR_OFFS 0x0000004c /* Transmission Control Register */ +#define UART_TLR_OFFS 0x00000050 /* Trigger Level Register */ +#define UART_MDR_OFFS 0x00000054 /* Mode Definition Register */ + +/* Registers available only for the IrDA UART (absolute address). */ + +#define UART_IRDA_MDR1 0xffff0854 /* Mode Definition Register 1 */ +#define UART_IRDA_MDR2 0xffff0858 /* Mode Definition Register 2 */ +#define UART_IRDA_TXFLL 0xffff085c /* LS Xmit Frame Length Register */ +#define UART_IRDA_TXFLH 0xffff0860 /* MS Xmit Frame Length Register */ +#define UART_IRDA_RXFLL 0xffff0864 /* LS Rcvd Frame Length Register */ +#define UART_IRDA_RXFLH 0xffff0868 /* MS Rcvd Frame Length Register */ +#define UART_IRDA_SFLSR 0xffff086c /* Status FIFO Line Status Reg */ +#define UART_IRDA_SFREGL 0xffff0870 /* LS Status FIFO Register */ +#define UART_IRDA_SFREGH 0xffff0874 /* MS Status FIFO Register */ +#define UART_IRDA_BLR 0xffff0878 /* Begin of File Length Register */ +#define UART_IRDA_PULSE_WIDTH 0xffff087c /* Pulse Width Register */ +#define UART_IRDA_ACREG 0xffff0880 /* Auxiliary Control Register */ +#define UART_IRDA_PULSE_START 0xffff0884 /* Start time of pulse */ +#define UART_IRDA_RX_W_PTR 0xffff0888 /* RX FIFO write pointer */ +#define UART_IRDA_RX_R_PTR 0xffff088c /* RX FIFO read pointer */ +#define UART_IRDA_TX_W_PTR 0xffff0890 /* TX FIFO write pointer */ +#define UART_IRDA_TX_R_PTR 0xffff0894 /* TX FIFO read pointer */ +#define UART_IRDA_STATUS_W_PTR 0xffff0898 /* Write pointer of status FIFO */ +#define UART_IRDA_STATUS_R_PTR 0xffff089c /* Read pointer of status FIFO */ +#define UART_IRDA_RESUME 0xffff08a0 /* Resume register */ +#define UART_IRDA_MUX 0xffff08a4 /* Selects UART_IRDA output mux */ + +/* Registers available for the Modem UART (absolute addresses) */ + +#define UART_MODEM_MDR 0xffff1054 /* Mode Definition Register */ +#define UART_MODEM_UASR 0xffff1058 /* UART Auto-baud Status Register */ +#define UART_MODEM_RDPTR_URX 0xffff105c /* RX FIFO Read Pointer Register */ +#define UART_MODEM_WRPTR_URX 0xffff1060 /* RX FIFO Write Pointer Register */ +#define UART_MODEM_RDPTR_UTX 0xffff1064 /* TX FIFO Read Pointer Register */ +#define UART_MODEM_WRPTR_UTX 0xffff1068 /* TX FIFO Write Pointer Register */ + +/* UART Settings ********************************************/ + +/* Miscellaneous UART settings. */ + +#define UART_RX_FIFO_NOEMPTY 0x00000001 +#define UART_SSR_TXFULL 0x00000001 +#define UART_LSR_TREF 0x00000020 + +#define UART_XMIT_FIFO_SIZE 64 +#define UART_IRDA_XMIT_FIFO_SIZE 64 + +/* UART_LCR Register */ + /* Bits 31-7: Reserved */ +#define UART_LCR_BOC 0x00000040 /* Bit 6: Break Control */ + /* Bit 5: Parity Type 2 */ +#define UART_LCR_PAREVEN 0x00000010 /* Bit 4: Parity Type 1 */ +#define UART_LCR_PARODD 0x00000000 +#define UART_LCR_PAREN 0x00000008 /* Bit 3: Paity Enable */ +#define UART_LCR_PARDIS 0x00000000 +#define UART_LCR_2STOP 0x00000004 /* Bit 2: Number of stop bits */ +#define UART_LCR_1STOP 0x00000000 +#define UART_LCR_5BITS 0x00000000 /* Bits 0-1: Word-length */ +#define UART_LCR_6BITS 0x00000001 +#define UART_LCR_7BITS 0x00000002 +#define UART_LCR_8BITS 0x00000003 + +#define UART_FCR_FTL 0x00000000 +#define UART_FCR_FIFO_EN 0x00000001 +#define UART_FCR_TX_CLR 0x00000002 +#define UART_FCR_RX_CLR 0x00000004 + +#define UART_IER_RECVINT 0x00000001 +#define UART_IER_XMITINT 0x00000002 +#define UART_IER_LINESTSINT 0x00000004 +#define UART_IER_MODEMSTSINT 0x00000008 /* IrDA UART only */ +#define UART_IER_XOFFINT 0x00000020 +#define UART_IER_RTSINT 0x00000040 /* IrDA UART only */ +#define UART_IER_CTSINT 0x00000080 /* IrDA UART only */ +#define UART_IER_INTMASK 0x000000ff + +#define BAUD_115200 0x00000001 +#define BAUD_57600 0x00000002 +#define BAUD_38400 0x00000003 +#define BAUD_19200 0x00000006 +#define BAUD_9600 0x0000000C +#define BAUD_4800 0x00000018 +#define BAUD_2400 0x00000030 +#define BAUD_1200 0x00000060 + +#define MDR_UART_MODE 0x00000000 /* Both IrDA and Modem UARTs */ +#define MDR_SIR_MODE 0x00000001 /* IrDA UART only */ +#define MDR_AUTOBAUDING_MODE 0x00000002 /* Modem UART only */ +#define MDR_RESET_MODE 0x00000007 /* Both IrDA and Modem UARTs */ + +/* SPI ******************************************************/ + +#define MAX_SPI 3 + +#define SPI_REGISTER_BASE 0xffff2000 + +/* GIO ******************************************************/ + +#define MAX_GIO (35) + +#define GIO_REGISTER_BASE 0xffff2800 + +#define GPIO_IO 0xffff2800 /* Writeable when I/O is configured + * as an output; reads value on I/O + * pin when I/O is configured as an + * input */ +#define GPIO_CIO 0xffff2804 /* GPIO configuration register */ +#define GPIO_IRQA 0xffff2808 /* In conjunction with GPIO_IRQB + * determines the behavior when GPIO + * pins configured as input IRQ */ +#define GPIO_IRQB 0xffff280c /* Determines the behavior when GPIO + * pins configured as input IRQ */ +#define GPIO_DDIO 0xffff2810 /* Delta Detect Register + * (detects changes in the I/O pins) */ +#define GPIO_EN 0xffff2814 /* Selects register for muxed GPIOs */ + +#define KGIO_REGISTER_BASE 0xffff2900 + +#define KBGPIO_IO 0xffff2900 /* Keyboard I/O bits: Writeable + * when KBGPIO is configured as an + * output; reads value on I/O pin + * when KBGPIO is configured as an + * input */ +#define KBGPIO_CIO 0xffff2904 /* KBGPIO configuration register */ +#define KBGPIO_IRQA 0xffff2908 /* In conjunction with KBGPIO_IRQB + * determines the behavior when + * KBGPIO pins configured as input + * IRQ */ +#define KBGPIO_IRQB 0xffff290c /* In conjunction with KBGPIO_IRQA + * determines the behavior when + * KBGPIO pins configured as input + * IRQ */ +#define KBGPIO_DDIO 0xffff2910 /* Delta Detect Register (detects + * changes in the KBGPIO pins) */ +#define KBGPIO_EN 0xffff2914 /* Selects register for muxed + * KBGPIOs */ + +/* Timers ***************************************************/ + +#define C5471_TIMER0_CTRL 0xffff2a00 +#define C5471_TIMER0_CNT 0xffff2a04 +#define C5471_TIMER1_CTRL 0xffff2b00 +#define C5471_TIMER1_CNT 0xffff2b04 +#define C5471_TIMER2_CTRL 0xffff2c00 + +#define C5471_TIMER2_CNT 0xffff2c04 + +/* Interrupts */ + +#define HAVE_SRC_IRQ_BIN_REG 0 + +#define INT_FIRST_IO 0xffff2d00 +#define INT_IO_RANGE 0x5C + +#define IT_REG 0xffff2d00 +#define MASK_IT_REG 0xffff2d04 +#define SRC_IRQ_REG 0xffff2d08 +#define SRC_FIQ_REG 0xffff2d0c +#define SRC_IRQ_BIN_REG 0xffff2d10 +#define INT_CTRL_REG 0xffff2d18 + +#define ILR_IRQ0_REG 0xffff2d1C /* 0-Timer 0 */ +#define ILR_IRQ1_REG 0xffff2d20 /* 1-Timer 1 */ +#define ILR_IRQ2_REG 0xffff2d24 /* 2-Timer 2 */ +#define ILR_IRQ3_REG 0xffff2d28 /* 3-GPIO0 */ +#define ILR_IRQ4_REG 0xffff2d2c /* 4-Ethernet */ +#define ILR_IRQ5_REG 0xffff2d30 /* 5-KBGPIO[7:0] */ +#define ILR_IRQ6_REG 0xffff2d34 /* 6-Uart serial */ +#define ILR_IRQ7_REG 0xffff2d38 /* 7-Uart IRDA */ +#define ILR_IRQ8_REG 0xffff2d3c /* 8-KBGPIO[15:8] */ +#define ILR_IRQ9_REG 0xffff2d40 /* 9-GPIO3 */ +#define ILR_IRQ10_REG 0xffff2d44 /* 10-GPIO2 */ +#define ILR_IRQ11_REG 0xffff2d48 /* 11-I2C */ +#define ILR_IRQ12_REG 0xffff2d4c /* 12-GPIO1 */ +#define ILR_IRQ13_REG 0xffff2d50 /* 13-SPI */ +#define ILR_IRQ14_REG 0xffff2d54 /* 14-GPIO[19:4] */ +#define ILR_IRQ15_REG 0xffff2d58 /* 15-API */ + +/* I2C ******************************************************/ + +#define MAX_I2C 1 + +/* API ******************************************************/ + +#define DSPRAM_BASE 0xffe00000 /* DSPRAM base address */ +#define DSPRAM_END 0xffe03fff + +/* This is the API address range in the DSP address space. */ + +#define DSPMEM_DSP_START 0x2000 +#define DSPMEM_DSP_END 0x3fff + +/* This is the API address range in the ARM address space. */ + +#define DSPMEM_ARM_START DSPRAM_BASE /* Defined in hardware.h */ +#define DSPMEM_ARM_END DSPRAM_END + +/* DSPMEM_IN_RANGE is a generic macro to test is a value is within + * a range of values. + */ + +#define DSPMEM_IN_RANGE(addr, start, end) \ + ((((__u32)(addr)) >= (start)) && (((__u32)(addr)) <= (end))) + +/* DSPMEM_ADDR_ALIGNED verifies that a potential DSP address is + * properly word aligned. + */ + +#define DSPMEM_ADDR_ALIGNED(addr, cpu) ((((__u32)(addr)) & 1) == 0) + +/* DSPMEM_DSP_ADDR checks if a DSP address lies in within the + * DSP's API address range. + */ + +#define DSPMEM_DSP_ADDR(addr, cpu) \ + DSPMEM_IN_RANGE(addr, DSPMEM_DSP_START, DSPMEM_DSP_END) + +/* DSPMEM_ARM_ADDR checks if a ARM address lies in within the + * ARM's API address range. + */ + +#define DSPMEM_ARM_ADDR(addr) \ + DSPMEM_IN_RANGE(addr, DSPMEM_ARM_START, DSPMEM_ARM_END) + +/* DSPMEM_DSP_TO_ARM maps a DSP API address into an ARM API address */ + +#define DSPMEM_DSP_TO_ARM(addr, cpu) \ + ((((__u32)(addr) - DSPMEM_DSP_START) << 1) + DSPMEM_ARM_START) + +/* DSPMEM_ARM_TO_DSP maps an ARM API address into a DSP API address */ + +#define DSPMEM_ARM_TO_DSP(addr) \ + ((((__u32)(addr) - DSPMEM_ARM_START) >> 1) + DSPMEM_DSP_START) + +/************************************************************ + * Inline Functions + ************************************************************/ + +/************************************************************ + * Public Function Prototypes + ************************************************************/ + +#ifndef __ASSEMBLY__ +extern void c5471_doirq(int irq, uint32* regs); +#endif + +#endif /* __C5471_CHIP_H */ diff --git a/nuttx/arch/arm/src/common/arm.h b/nuttx/arch/arm/src/common/arm.h new file mode 100644 index 000000000..8f3597813 --- /dev/null +++ b/nuttx/arch/arm/src/common/arm.h @@ -0,0 +1,254 @@ +/************************************************************************************ + * common/arm.h + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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. + * + ************************************************************************************/ + +#ifndef __ARM_H +#define __ARM_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#ifndef __ASSEMBLY__ +# include <sys/types.h> +#endif + +/************************************************************************** + * Conditional Compilation + **************************************************************************/ + +#undef CONFIG_ALIGNMENT_TRAP +#undef CONFIG_DCACHE_WRITETHROUGH +#undef CONFIG_CACHE_ROUND_ROBIN +#undef CONFIG_DCACHE_DISABLE +#undef CONFIG_ICACHE_DISABLE + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* ARM9EJS **************************************************************************/ + +/* PSR bits */ + +#define MODE_MASK 0x0000001f +#define USR26_MODE 0x00000000 +#define FIQ26_MODE 0x00000001 +#define IRQ26_MODE 0x00000002 +#define SVC26_MODE 0x00000003 +#define USR_MODE 0x00000010 +#define FIQ_MODE 0x00000011 +#define IRQ_MODE 0x00000012 +#define SVC_MODE 0x00000013 +#define ABT_MODE 0x00000017 +#define UND_MODE 0x0000001b +#define MODE32_BIT 0x00000010 +#define SYSTEM_MODE 0x0000001f +#define PSR_T_BIT 0x00000020 +#define PSR_F_BIT 0x00000040 +#define PSR_I_BIT 0x00000080 +#define PSR_J_BIT 0x01000000 +#define PSR_Q_BIT 0x08000000 +#define PSR_V_BIT 0x10000000 +#define PSR_C_BIT 0x20000000 +#define PSR_Z_BIT 0x40000000 +#define PSR_N_BIT 0x80000000 + +/* CR1 bits (CP#15 CR1) */ + +#define CR_M 0x00000001 /* MMU enable */ +#define CR_A 0x00000002 /* Alignment abort enable */ +#define CR_C 0x00000004 /* Dcache enable */ +#define CR_W 0x00000008 /* Write buffer enable */ +#define CR_P 0x00000010 /* 32-bit exception handler */ +#define CR_D 0x00000020 /* 32-bit data address range */ +#define CR_L 0x00000040 /* Implementation defined */ +#define CR_B 0x00000080 /* Big endian */ +#define CR_S 0x00000100 /* System MMU protection */ +#define CR_R 0x00000200 /* ROM MMU protection */ +#define CR_F 0x00000400 /* Implementation defined */ +#define CR_Z 0x00000800 /* Implementation defined */ +#define CR_I 0x00001000 /* Icache enable */ +#define CR_V 0x00002000 /* Vectors relocated to 0xffff0000 */ +#define CR_RR 0x00004000 /* Round Robin cache replacement */ +#define CR_L4 0x00008000 /* LDR pc can set T bit */ +#define CR_DT 0x00010000 +#define CR_IT 0x00040000 +#define CR_ST 0x00080000 +#define CR_FI 0x00200000 /* Fast interrupt (lower latency mode) */ +#define CR_U 0x00400000 /* Unaligned access operation */ +#define CR_XP 0x00800000 /* Extended page tables */ +#define CR_VE 0x01000000 /* Vectored interrupts */ + +/* Hardware page table definitions. + * + * Level 1 Descriptor (PMD) + * + * Common definitions. + */ + +#define PMD_TYPE_MASK 0x00000003 /* Bits 1:0: Type of descriptor */ +#define PMD_TYPE_FAULT 0x00000000 +#define PMD_TYPE_COARSE 0x00000001 +#define PMD_TYPE_SECT 0x00000002 +#define PMD_TYPE_FINE 0x00000003 + /* Bits 3:2: Depends on descriptor */ +#define PMD_BIT4 0x00000010 /* Bit 4: Must be one */ +#define PMD_DOMAIN_MASK 0x000001e0 /* Bits 8:5: Domain control bits */ +#define PMD_DOMAIN(x) ((x) << 5) +#define PMD_PROTECTION 0x00000200 /* Bit 9: v5 only */ + /* Bits 31:10: Depend on descriptor */ + +/* Level 1 Section Descriptor. Section descriptors allow fast, single + * level mapping between 1Mb address regions. + */ + /* Bits 1:0: Type of mapping */ +#define PMD_SECT_BUFFERABLE 0x00000004 /* Bit 2: 1=bufferable */ +#define PMD_SECT_CACHEABLE 0x00000008 /* Bit 3: 1=cacheable */ + /* Bit 4: Common, must be one */ + /* Bits 8:5: Common domain control */ + /* Bit 9: Common protection */ +#define PMD_SECT_AP_MASK 0x00000c00 /* Bits 11:10: Access permission */ +#define PMD_SECT_AP_WRITE 0x00000400 +#define PMD_SECT_AP_READ 0x00000800 + /* Bits 19:20: Should be zero */ +#define PMD_SECT_TEX_MASK 0xfff00000 /* Bits 31:20: v5, Physical page */ +#define PMD_SECT_APX 0x00008000 /* Bit 15: v6 only */ +#define PMD_SECT_S 0x00010000 /* Bit 16: v6 only */ +#define PMD_SECT_nG 0x00020000 /* Bit 17: v6 only */ + +#define PMD_SECT_UNCACHED (0) +#define PMD_SECT_BUFFERED (PMD_SECT_BUFFERABLE) +#define PMD_SECT_WT (PMD_SECT_CACHEABLE) +#define PMD_SECT_WB (PMD_SECT_CACHEABLE|PMD_SECT_BUFFERABLE) +#define PMD_SECT_MINICACHE (PMD_SECT_TEX(1)|PMD_SECT_CACHEABLE) +#define PMD_SECT_WBWA (PMD_SECT_TEX(1)|PMD_SECT_CACHEABLE|PMD_SECT_BUFFERABLE) + +/* Level 1 Coarse Table Descriptor. Coarse Table Descriptors support + * two level mapping between 16Kb memory regions. + */ + /* Bits 1:0: Type of mapping */ + /* Bits 3:2: Should be zero */ + /* Bit 4: Common, must be one */ + /* Bits 8:5: Common domain control */ + /* Bits 9: Should be zero */ +#define PMD_COARSE_TEX_MASK 0xfffffc00 /* Bits 31:10: v5, Physical page */ + +/* Level 1 Fine Table Descriptor. Coarse Table Descriptors support + * two level mapping between 4Kb memory regions. + */ + + /* Bits 1:0: Type of mapping */ + /* Bits 3:2: Should be zero */ + /* Bit 4: Common, must be one */ + /* Bits 8:5: Common domain control */ + /* Bits 11:9: Should be zero */ +#define PMD_FINE_TEX_MASK 0xfffff000 /* Bits 31:12: v5, Physical page */ + +/* Level 2 Table Descriptor (PTE). -- All tables */ + +#define PTE_TYPE_MASK (3 << 0) /* Bits: 1:0: Type of mapping */ +#define PTE_TYPE_FAULT (0 << 0) /* None */ +#define PTE_TYPE_LARGE (1 << 0) /* 64Kb of memory */ +#define PTE_TYPE_SMALL (2 << 0) /* 4Kb of memory */ +#define PTE_TYPE_TINY (3 << 0) /* 1Kb of memory (v5)*/ +#define PTE_BUFFERABLE (1 << 2) /* Bit 2: 1=bufferable */ +#define PTE_CACHEABLE (1 << 3) /* Bit 3: 1=cacheable */ + /* Bits 31:4: Depend on type */ + +/* Large page -- 64Kb */ + /* Bits: 1:0: Type of mapping */ + /* Bits: 3:2: Bufferable/cacheable */ +#define PTE_LARGE_AP_MASK (0xff << 4) /* Bits 11:4 Access permissions */ +#define PTE_LARGE_AP_UNO_SRO (0x00 << 4) +#define PTE_LARGE_AP_UNO_SRW (0x55 << 4) +#define PTE_LARGE_AP_URO_SRW (0xaa << 4) +#define PTE_LARGE_AP_URW_SRW (0xff << 4) + /* Bits 15:12: Should be zero */ +#define PTE_LARGE_TEX_MASK 0xffff0000 /* Bits 31:16: v5, Physical page */ + +/* Small page -- 4Kb */ + + /* Bits: 1:0: Type of mapping */ + /* Bits: 3:2: Bufferable/cacheable */ +#define PTE_SMALL_AP_MASK (0xff << 4) /* Bits: 11:4: Access permissions */ +#define PTE_SMALL_AP_UNO_SRO (0x00 << 4) +#define PTE_SMALL_AP_UNO_SRW (0x55 << 4) +#define PTE_SMALL_AP_URO_SRW (0xaa << 4) +#define PTE_SMALL_AP_URW_SRW (0xff << 4) +#define PTE_SMALL_TEX_MASK 0xfffff000 /* Bits: 31:12: Physical page */ + +/* Tiny page -- 1Kb */ + + /* Bits: 1:0: Type of mapping */ + /* Bits: 3:2: Bufferable/cacheable */ +#define PTE_EXT_AP_MASK (3 << 4) /* Bits: 5:4: Access persions */ +#define PTE_EXT_AP_UNO_SRO (0 << 4) +#define PTE_EXT_AP_UNO_SRW (1 << 4) +#define PTE_EXT_AP_URO_SRW (2 << 4) +#define PTE_EXT_AP_URW_SRW (3 << 4) + /* Bits: 9:6: Should be zero */ +#define PTE_TINY_TEX_MASK 0xfffffc00 /* Bits: 31:10: Physical page */ + +/* Default MMU flags for memory and IO */ + +#define MMU_MEMFLAGS \ + (PMD_TYPE_SECT|PMD_SECT_WB|PMD_BIT4|PMD_SECT_AP_WRITE|PMD_SECT_AP_READ) + +#define MMU_IOFLAGS \ + (PMD_TYPE_SECT|PMD_BIT4|PMD_SECT_AP_WRITE|PMD_SECT_AP_READ) + +#define MMU_L1_VECTORFLAGS (PMD_TYPE_COARSE|PMD_BIT4) +#define MMU_L2_VECTORFLAGS (PTE_TYPE_SMALL|PTE_SMALL_AP_UNO_SRW) + +/* Mapped section size */ + +#define SECTION_SIZE (1 << 20) /* 1Mb */ + +/* We place the page tables 16K below the beginning of .text. The + * following value is assume to be the (virtual) start address of + * .text. + */ + +#define PGTABLE_SIZE 0x00004000 + +/************************************************************************************ + * Inline Functions + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#endif + +#endif /* __ARM_H */ diff --git a/nuttx/arch/arm/src/common/up_allocateheap.c b/nuttx/arch/arm/src/common/up_allocateheap.c new file mode 100644 index 000000000..8e8f6b169 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_allocateheap.c @@ -0,0 +1,79 @@ +/************************************************************ + * common/up_allocateheap.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <debug.h> +#include <nuttx/arch.h> +#include "up_arch.h" +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Functions + ************************************************************/ + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: up_allocate_heap + * + * Description: + * The heap may be statically allocated by + * defining CONFIG_HEAP_BASE and CONFIG_HEAP_SIZE. If these + * are not defined, then this function will be called to + * dynamically set aside the heap region. + * + ************************************************************/ + +void up_allocate_heap(FAR void **heap_start, size_t *heap_size) +{ + up_ledon(LED_HEAPALLOCATE); + *heap_start = (FAR void*)g_heapbase; + *heap_size = CONFIG_DRAM_END - g_heapbase; +} diff --git a/nuttx/arch/arm/src/common/up_arch.h b/nuttx/arch/arm/src/common/up_arch.h new file mode 100644 index 000000000..071e60d67 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_arch.h @@ -0,0 +1,92 @@ +/************************************************************************************ + * common/up_arch.h + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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. + * + ************************************************************************************/ + +#ifndef __UP_ARCH_H +#define __UP_ARCH_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> +#ifndef __ASSEMBLY__ +# include <sys/types.h> +#endif + +#include "arm.h" +#include <arch/board/board.h> +#include "chip.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Inline Functions + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +# define getreg8(a) (*(volatile ubyte *)(a)) +# define putreg8(v,a) (*(volatile ubyte *)(a) = (v)) +# define getreg32(a) (*(volatile uint32 *)(a)) +# define putreg32(v,a) (*(volatile uint32 *)(a) = (v)) + +/* Some compiler options will convert short loads and stores into byte loads + * and stores. We don't want this to happen for IO reads and writes! + */ + +/* # define getreg16(a) (*(volatile uint16 *)(a)) */ +static inline uint16 getreg16(unsigned int addr) +{ + uint16 retval; + __asm__ __volatile__("\tldrh %0, [%1]\n\t" : "=r"(retval) : "r"(addr)); + return retval; +} + +/* define putreg16(v,a) (*(volatile uint16 *)(a) = (v)) */ +static inline void putreg16(uint16 val, unsigned int addr) +{ + __asm__ __volatile__("\tstrh %0, [%1]\n\t": : "r"(val), "r"(addr)); +} + +/* Most DM320 registers are 16-bits wide */ + +#define getreg(a) getreg16(1) +#define putreg(v,a) putreg16(v,a) + +#endif + +#endif /* __UP_ARCH_H */ diff --git a/nuttx/arch/arm/src/common/up_assert.c b/nuttx/arch/arm/src/common/up_assert.c new file mode 100644 index 000000000..5c032c2f0 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_assert.c @@ -0,0 +1,224 @@ +/************************************************************ + * common/up_assert.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <stdlib.h> +#include <assert.h> +#include <debug.h> +#include <nuttx/irq.h> +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * 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: up_getsp + ************************************************************/ + +/* I don't know if the builtin to get SP is enabled */ + +static inline uint32 up_getsp(void) +{ + uint32 sp; + __asm__ + ( + "\tmov %0, sp\n\t" + : "=r"(sp) + ); + return sp; +} + +/************************************************************ + * Name: up_stackdump + ************************************************************/ + +#ifdef CONFIG_ARCH_STACKDUMP +static void up_stackdump(void) +{ + _TCB *rtcb = (_TCB*)g_readytorun.head; + uint32 sp = up_getsp(); + uint32 stack_base; + uint32 stack_size; + + if (rtcb->pid == 0) + { + stack_base = g_heapbase - 4; + stack_size = CONFIG_PROC_STACK_SIZE; + } + else + { + stack_base = (uint32)rtcb->adj_stack_ptr; + stack_size = (uint32)rtcb->adj_stack_size; + } + + lldbg("stack_base: %08x\n", stack_base); + lldbg("stack_size: %08x\n", stack_size); + lldbg("sp: %08x\n", sp); + + if (sp >= stack_base || sp < stack_base - stack_size) + { + lldbg("ERROR: Stack pointer is not within allocated stack\n"); + return; + } + else + { + uint32 stack = sp & ~0x1f; + + 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]); + } + } + + if (current_regs) + { + int regs; + + for (regs = REG_R0; regs <= REG_R15; regs += 8) + { + uint32 *ptr = (uint32*)¤t_regs[regs]; + lldbg("R%d: %08x %08x %08x %08x %08x %08x %08x %08x\n", + regs, ptr[0], ptr[1], ptr[2], ptr[3], + ptr[4], ptr[5], ptr[6], ptr[7]); + } + lldbg("CPSR: %08x\n", current_regs[REG_CPSR]); + } +} +#else +# define up_stackdump() +#endif + +/************************************************************ + * 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_delay(250); + up_ledoff(LED_PANIC); + up_delay(250); +#endif + } + } + else + { + exit(errorcode); + } +} + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_assert + ************************************************************/ + +void up_assert(const ubyte *filename, int lineno) +{ +#if CONFIG_TASK_NAME_SIZE > 0 + _TCB *rtcb = (_TCB*)g_readytorun.head; +#endif + + up_ledon(LED_ASSERTION); +#if CONFIG_TASK_NAME_SIZE > 0 + 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_stackdump(); + _up_assert(EXIT_FAILURE); +} + +/************************************************************ + * Name: up_assert_code + ************************************************************/ + +void up_assert_code(const ubyte *filename, int lineno, int errorcode) +{ +#if CONFIG_TASK_NAME_SIZE > 0 + _TCB *rtcb = (_TCB*)g_readytorun.head; +#endif + + up_ledon(LED_ASSERTION); +#if CONFIG_TASK_NAME_SIZE > 0 + 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_stackdump(); + _up_assert(errorcode); +} diff --git a/nuttx/arch/arm/src/common/up_blocktask.c b/nuttx/arch/arm/src/common/up_blocktask.c new file mode 100644 index 000000000..b72604429 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_blocktask.c @@ -0,0 +1,168 @@ +/************************************************************ + * common/up_blocktask.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <sched.h> +#include <debug.h> +#include <nuttx/arch.h> +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_block_task + * + * Description: + * The currently executing task at the head of + * the ready to run list must be stopped. Save its context + * and move it to the inactive list specified by task_state. + * + * Inputs: + * tcb: Refers to a task in the ready-to-run list (normally + * the task at the the head of the list). It most be + * stopped, its context saved and moved into one of the + * waiting task lists. It it was the task at the head + * of the ready-to-run list, then a context to the new + * ready to run task must be performed. + * task_state: Specifies which waiting task list should be + * hold the blocked task TCB. + * + ************************************************************/ + +void up_block_task(_TCB *tcb, tstate_t task_state) +{ + /* Verify that the context switch can be performed */ + + if ((tcb->task_state < FIRST_READY_TO_RUN_STATE) || + (tcb->task_state > LAST_READY_TO_RUN_STATE)) + { + PANIC(OSERR_BADBLOCKSTATE); + } + else + { + _TCB *rtcb = (_TCB*)g_readytorun.head; + boolean switch_needed; + + lldbg("Blocking TCB=%p\n", tcb); + + /* Remove the tcb task from the ready-to-run list. If we + * are blocking the task at the head of the task list (the + * most likely case), then a context switch to the next + * ready-to-run task is needed. In this case, it should + * also be true that rtcb == tcb. + */ + + switch_needed = sched_removereadytorun(tcb); + + /* Add the task to the specified blocked task list */ + + sched_addblocked(tcb, (tstate_t)task_state); + + /* If there are any pending tasks, then add them to the g_readytorun + * task list now + */ + + if (g_pendingtasks.head) + { + switch_needed |= sched_mergepending(); + } + + /* Now, perform the context switch if one is needed */ + + if (switch_needed) + { + /* Are we in an interrupt handler? */ + + if (current_regs) + { + /* Yes, then we have to do things differently. + * Just copy the current_regs into the OLD rtcb. + */ + + up_copystate(rtcb->xcp.regs, current_regs); + + /* Restore the exception context of the rtcb at the (new) head + * of the g_readytorun task list. + */ + + rtcb = (_TCB*)g_readytorun.head; + lldbg("New Active Task TCB=%p\n", rtcb); + + /* Then switch contexts */ + + up_copystate(current_regs, rtcb->xcp.regs); + } + + /* Copy the user C context into the TCB at the (old) head of the + * g_readytorun Task list. if up_saveusercontext returns a non-zero + * value, then this is really the previously running task restarting! + */ + + else if (!up_saveusercontext(rtcb->xcp.regs)) + { + /* Restore the exception context of the rtcb at the (new) head + * of the g_readytorun task list. + */ + + rtcb = (_TCB*)g_readytorun.head; + lldbg("New Active Task TCB=%p\n", rtcb); + + /* Then switch contexts */ + + up_fullcontextrestore(rtcb->xcp.regs); + } + } + } +} diff --git a/nuttx/arch/arm/src/common/up_cache.S b/nuttx/arch/arm/src/common/up_cache.S new file mode 100644 index 000000000..5e6f6610b --- /dev/null +++ b/nuttx/arch/arm/src/common/up_cache.S @@ -0,0 +1,74 @@ +/******************************************************************** + * common/up_cache.S + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include "up_internal.h" +#include "up_arch.h" + +/******************************************************************** + * Definitions + ********************************************************************/ + +#define CACHE_DLINESIZE 32 + +/******************************************************************** + * Assembly Macros + ********************************************************************/ + +/************************************************************************** + * Name: up_flushicache + **************************************************************************/ + +/* Esure coherency between the Icache and the Dcache in the region described + * by r0=start and r1=end. + */ + .globl up_flushicache + .type up_flushicache,%function +up_flushicache: + bic r0, r0, #CACHE_DLINESIZE - 1 +1: mcr p15, 0, r0, c7, c10, 1 /* Clean D entry */ + mcr p15, 0, r0, c7, c5, 1 /* Invalidate I entry */ + add r0, r0, #CACHE_DLINESIZE + cmp r0, r1 + blo 1b + mcr p15, 0, r0, c7, c10, 4 /* Drain WB */ + mov pc, lr + .size up_flushicache, .-up_flushicache + .end + diff --git a/nuttx/arch/arm/src/common/up_copystate.c b/nuttx/arch/arm/src/common/up_copystate.c new file mode 100644 index 000000000..9d21630e1 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_copystate.c @@ -0,0 +1,75 @@ +/************************************************************ + * common/up_copystate.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Functions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_undefinedinsn + ************************************************************/ + +/* A little faster than most memcpy's */ + +void up_copystate(uint32 *dest, uint32 *src) +{ + int i; + for (i = 0; i < XCPTCONTEXT_REGS; i++) + { + *dest++ = *src++; + } +} + diff --git a/nuttx/arch/arm/src/common/up_createstack.c b/nuttx/arch/arm/src/common/up_createstack.c new file mode 100644 index 000000000..a68e2edf3 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_createstack.c @@ -0,0 +1,128 @@ +/************************************************************ + * common/up_createstack.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <sched.h> +#include <debug.h> +#include <nuttx/kmalloc.h> +#include <nuttx/arch.h> +#include "up_arch.h" +#include "up_internal.h" + +/************************************************************ + * Private Types + ************************************************************/ + +/************************************************************ + * Private Function Prototypes + ************************************************************/ + +/************************************************************ + * Global Functions + ************************************************************/ + +/************************************************************ + * Name: up_create_stack + * + * Description: + * Allocate a stack for a new thread and setup + * up stack-related information in the TCB. + * + * The following TCB fields must be initialized: + * adj_stack_size: Stack size after adjustment for hardware, + * processor, etc. This value is retained only for debug + * purposes. + * stack_alloc_ptr: Pointer to allocated stack + * adj_stack_ptr: Adjusted stack_alloc_ptr for HW. The + * initial value of the stack pointer. + * + * Inputs: + * tcb: The TCB of new task + * stack_size: The requested stack size. At least this much + * must be allocated. + ************************************************************/ + +STATUS up_create_stack(_TCB *tcb, size_t stack_size) +{ + if (tcb->stack_alloc_ptr && + tcb->adj_stack_size != stack_size) + { + sched_free(tcb->stack_alloc_ptr); + tcb->stack_alloc_ptr = NULL; + } + + if (!tcb->stack_alloc_ptr) + { + tcb->stack_alloc_ptr = (uint32 *)kzmalloc(stack_size); + } + + if (tcb->stack_alloc_ptr) + { + size_t top_of_stack; + size_t size_of_stack; + + /* The Arm7Tdmi uses a push-down stack: the stack grows + * toward loweraddresses in memory. The stack pointer + * register, points to the lowest, valid work address + * (the "top" of the stack). Items on the stack are + * referenced as positive word offsets from sp. + */ + + top_of_stack = (uint32)tcb->stack_alloc_ptr + stack_size - 4; + + /* The Arm7Tdmi stack must be aligned at word (4 byte) + * boundaries. If necessary top_of_stack must be rounded + * down to the next boundary + */ + + top_of_stack &= ~3; + size_of_stack = top_of_stack - (uint32)tcb->stack_alloc_ptr + 4; + + /* Save the adjusted stack values in the _TCB */ + + tcb->adj_stack_ptr = (uint32*)top_of_stack; + tcb->adj_stack_size = size_of_stack; + + up_ledon(LED_STACKCREATED); + return OK; + } + + return ERROR; +} diff --git a/nuttx/arch/arm/src/common/up_dataabort.c b/nuttx/arch/arm/src/common/up_dataabort.c new file mode 100644 index 000000000..a7b48ed77 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_dataabort.c @@ -0,0 +1,81 @@ +/************************************************************ + * common/up_dataabort.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <debug.h> +#include <nuttx/irq.h> +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * 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 + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_dataabort + ************************************************************/ + +void up_dataabort(uint32 *regs) +{ + lldbg("Data abort at 0x%x\n", regs[REG_PC]); + current_regs = regs; + PANIC(OSERR_ERREXCEPTION); +} diff --git a/nuttx/arch/arm/src/common/up_delay.c b/nuttx/arch/arm/src/common/up_delay.c new file mode 100644 index 000000000..19e98484a --- /dev/null +++ b/nuttx/arch/arm/src/common/up_delay.c @@ -0,0 +1,100 @@ +/************************************************************ + * common/up_delay.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <unistd.h> +#include <semaphore.h> +#include <string.h> +#include <errno.h> +#include <debug.h> +#include <nuttx/irq.h> +#include <nuttx/arch.h> +#include <nuttx/fs.h> +#include <arch/serial.h> +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Definitions + ************************************************************/ + +/************************************************************ + * Private Types + ************************************************************/ + +/************************************************************ + * Private Function Prototypes + ************************************************************/ + +/************************************************************ + * Private Variables + ************************************************************/ + +/************************************************************ + * Private Functions + ************************************************************/ + + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_delay + * + * Description: + * Delay inline for the requested number of milliseconds. + * NOT multi-tasking friendly. + * + ************************************************************/ + +void up_delay(int milliseconds) +{ + volatile int i; + volatile int j; + + for (i = 0; i < milliseconds; i++) + { + for (j = 0; j < CONFIG_BOARD_LOOPSPERMSEC; j++) + { + } + } +} diff --git a/nuttx/arch/arm/src/common/up_doirq.c b/nuttx/arch/arm/src/common/up_doirq.c new file mode 100644 index 000000000..d49c3c4fa --- /dev/null +++ b/nuttx/arch/arm/src/common/up_doirq.c @@ -0,0 +1,104 @@ +/************************************************************ + * common/up_doirq.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <nuttx/irq.h> +#include <nuttx/arch.h> +#include <assert.h> +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Definitions + ************************************************************/ + +/************************************************************ + * Public Data + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Functions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +void up_doirq(int irq, uint32* regs) +{ + up_ledon(LED_INIRQ); +#ifdef CONFIG_SUPPRESS_INTERRUPTS + PANIC(OSERR_ERREXCEPTION); +#else + if ((unsigned)irq < NR_IRQS) + { + /* Current regs non-zero indicates that we are processing + * an interrupt; current_regs is also used to manage + * interrupt level context switches. + */ + + current_regs = regs; + + /* Mask and acknowledge the interrupt */ + + up_maskack_irq(irq); + + /* Deliver the IRQ */ + + irq_dispatch(irq, regs); + + /* Indicate that we are no long in an interrupt handler */ + + current_regs = NULL; + + /* Unmask the last interrupt (global interrupts are still + * disabled. + */ + + up_enable_irq(irq); + } + up_ledoff(LED_INIRQ); +#endif +} diff --git a/nuttx/arch/arm/src/common/up_exit.c b/nuttx/arch/arm/src/common/up_exit.c new file mode 100644 index 000000000..94a467006 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_exit.c @@ -0,0 +1,188 @@ +/************************************************************ + * common/up_exit.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <sched.h> +#include <debug.h> +#include <nuttx/arch.h> +#include "os_internal.h" +#include "up_internal.h" + +#ifdef CONFIG_DUMP_ON_EXIT +#include <nuttx/fs.h> +#endif + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Name: _up_dumponexit + * + * Description: + * Dump the state of all tasks whenever on task exits. This + * is debug instrumentation that was added to check file- + * related reference counting but could be useful again + * sometime in the future. + * + ************************************************************/ + +#if defined(CONFIG_DUMP_ON_EXIT) && defined(CONFIG_DEBUG) +static void _up_dumponexit(FAR _TCB *tcb, FAR void *arg) +{ + int i; + dbg(" TCB=%p name=%s\n", tcb, tcb->argv[0]); + if (tcb->filelist) + { + dbg(" filelist refcount=%d\n", + tcb->filelist->fl_crefs); + + for (i = 0; i < CONFIG_NFILE_DESCRIPTORS; i++) + { + struct inode *inode = tcb->filelist->fl_files[i].f_inode; + if (inode) + { + dbg(" fd=%d refcount=%d\n", + i, inode->i_crefs); + } + } + } + + if (tcb->streams) + { + dbg(" streamlist refcount=%d\n", + tcb->streams->sl_crefs); + + for (i = 0; i < CONFIG_NFILE_STREAMS; i++) + { + struct file_struct *filep = &tcb->streams->sl_streams[i]; + if (filep->fs_filedes >= 0) + { + dbg(" fd=%d nbytes=%d\n", + filep->fs_filedes, + filep->fs_bufpos - filep->fs_bufstart); + } + } + } +} +#endif + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: _exit + * + * Description: + * This function causes the currently executing task to cease + * to exist. This is a special case of task_delete(). + * + ************************************************************/ + +void _exit(int status) +{ + _TCB* tcb = (_TCB*)g_readytorun.head; + + /* Disable interrupts. They will be restored when the next + * task is started. + */ + + (void)irqsave(); + + lldbg("TCB=%p exitting\n", tcb); + +#if defined(CONFIG_DUMP_ON_EXIT) && defined(CONFIG_DEBUG) + dbg("Other tasks:\n"); + sched_foreach(_up_dumponexit, NULL); +#endif + + /* Remove the tcb task from the ready-to-run list. We can + * ignore the return value because we know that a context + * switch is needed. + */ + + (void)sched_removereadytorun(tcb); + + /* We are not in a bad stack-- the head of the ready to run task list + * does not correspond to the thread that is running. Disabling pre- + * emption on this TCB should be enough to keep things stable. + */ + + sched_lock(); + + /* Move the TCB to the specified blocked task list and delete it */ + + sched_addblocked(tcb, TSTATE_TASK_INACTIVE); + task_delete(tcb->pid); + + /* If there are any pending tasks, then add them to the g_readytorun + * task list now + */ + + if (g_pendingtasks.head) + { + (void)sched_mergepending(); + } + + /* Now calling sched_unlock() should have no effect */ + + sched_unlock(); + + /* Now, perform the context switch to the new ready-to-run task at the + * head of the list. + */ + + tcb = (_TCB*)g_readytorun.head; + lldbg("New Active Task TCB=%p\n", tcb); + + /* Then switch contexts */ + + up_fullcontextrestore(tcb->xcp.regs); +} + diff --git a/nuttx/arch/arm/src/common/up_fullcontextrestore.S b/nuttx/arch/arm/src/common/up_fullcontextrestore.S new file mode 100644 index 000000000..21529e6aa --- /dev/null +++ b/nuttx/arch/arm/src/common/up_fullcontextrestore.S @@ -0,0 +1,117 @@ +/************************************************************************** + * common/up_fullcontextrestore.S + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/irq.h> +#include "up_internal.h" + +/************************************************************************** + * Private Definitions + **************************************************************************/ + +/************************************************************************** + * Private Types + **************************************************************************/ + +/************************************************************************** + * Private Function Prototypes + **************************************************************************/ + +/************************************************************************** + * Global Variables + **************************************************************************/ + +/************************************************************************** + * Private Variables + **************************************************************************/ + +/************************************************************************** + * Private Functions + **************************************************************************/ + +/************************************************************************** + * Public Functions + **************************************************************************/ + +/************************************************************************** + * Name: up_fullcontextrestore + **************************************************************************/ + + .globl up_fullcontextrestore + .type up_fullcontextrestore, function +up_fullcontextrestore: + + /* On entry, a1 (r0) holds address of the register save area */ + + /* Recover all registers except for r0, r1, R15, and CPSR */ + + add r1, r0, #(4*REG_R2) /* Offset to REG_R2 storage */ + ldmia r1, {r2-r14} /* Recover registers */ + + /* Create a stack frame to hold the PC */ + + sub sp, sp, #(3*4) /* Frame for three registers */ + ldr r1, [r0, #(4*REG_R0)] /* Fetch the stored r0 value */ + str r1, [sp] /* Save it at the top of the stack */ + ldr r1, [r0, #(4*REG_R1)] /* Fetch the stored r1 value */ + str r1, [sp, #4] /* Save it in the stack */ + ldr r1, [r0, #(4*REG_PC)] /* Fetch the stored pc value */ + str r1, [sp, #8] /* Save it at the bottom of the frame */ + + /* Now we can restore the CPSR. We wait until we are completely + * finished with the context save data to do this. Restore the CPSR + * may re-enable and interrupts and we couldt be in a context + * where save structure is only protected by interrupts being disabled. + */ + + ldr r1, [r0, #(4*REG_CPSR)] /* Fetch the stored CPSR value */ + msr cpsr, r1 /* Set the CPSR */ + + /* Now recover r0 and r1 */ + + ldr r0, [sp] + ldr r1, [sp, #4] + add sp, sp, #(2*4) + + /* Then return to the address at the stop of the stack, + * destroying the stack frame + */ + + ldr pc, [sp], #4 + .size up_fullcontextrestore, . - up_fullcontextrestore + diff --git a/nuttx/arch/arm/src/common/up_head.S b/nuttx/arch/arm/src/common/up_head.S new file mode 100644 index 000000000..65403ee23 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_head.S @@ -0,0 +1,324 @@ +/******************************************************************** + * common/up_head.S + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include "up_internal.h" +#include "up_arch.h" + +/************************************************************************** + * Conditional Compilation + **************************************************************************/ + +#undef ALIGNMENT_TRAP +#undef CPU_DCACHE_WRITETHROUGH +#undef CPU_CACHE_ROUND_ROBIN +#undef CPU_DCACHE_DISABLE +#undef CPU_ICACHE_DISABLE + +/******************************************************************** + * Definitions + ********************************************************************/ + +/* The physical address of the beginning of SDRAM is provided by + * CONFIG_DRAM_START. The size of installed SDRAM is provided by + * CONFIG_DRAM_SIZE. The virtual address of SDRAM is provided by + * CONFIG_DRAM_VSTART. + */ + +#define NSDRAM_SECTIONS (CONFIG_DRAM_SIZE >> 20) + +/******************************************************************** + * Assembly Macros + ********************************************************************/ + +/* Since the page table is closely related to the NuttX base + * address, we can convert the page table base address to the + * base address of the section containing both. + */ + + .macro mksection, section, pgtable + bic \section, \pgtable, #0x000ff000 + .endm + +/* This macro will modify r0, r1, r2 and r14 */ + +#ifdef CONFIG_DEBUG + .macro showprogress, code + mov r0, #\code + bl up_lowputc + .endm +#else + .macro showprogress, code + .endm +#endif + +/******************************************************************** + * Name: __start + ********************************************************************/ + +/* We assume the bootloader has already initialized most of the h/w for + * us and that only leaves us having to do some os specific things + * below. + */ + .text + .global __start + .type __start, #function +__start: + /* Make sure that we are in SVC mode with all IRQs disabled */ + + mov r0, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT) + msr cpsr_c, r0 + + /* Clear the 16K level 1 page table */ + + ldr r4, .LCppgtable /* r4=phys. page table */ + mov r0, r4 + mov r1, #0 + add r2, r0, #PGTABLE_SIZE +.Lpgtableclear: + str r1, [r0], #4 + str r1, [r0], #4 + str r1, [r0], #4 + str r1, [r0], #4 + teq r0, r2 + bne .Lpgtableclear + + /* Create identity mapping for first MB section to support + * this startup logic executing out of the physical address + * space. This identity mapping will be removed by .Lvstart + * (see below). + */ + + mksection r0, r4 /* r0=phys. base section */ + ldr r1, .LCmmuflags /* FLGS=MMU_MEMFLAGS */ + add r3, r1, r0 /* r3=flags + base */ + str r3, [r4, r0, lsr #18] /* identity mapping */ + + /* Create a "normal" single section mapping for the first + * MB of memory. Now, we have the first 1MB mapping to + * both phyical and virtual addresses. The reset of the + * SDRAM mapping will be completed in .Lvstart once we have + * moved the physical mapping out of the way. + */ + + ldr r2, .LCvpgtable /* r2=virt. page table */ + mksection r0, r2 /* r0=virt. base section */ + str r3, [r4, r0, lsr #18] /* identity mapping */ + + /* The following logic will set up the ARM926 for normal operation */ + + mov r0, #0 + mcr p15, 0, r0, c7, c7 /* Invalidate I,D caches */ + mcr p15, 0, r0, c7, c10, 4 /* Drain write buffer */ + mcr p15, 0, r0, c8, c7 /* Invalidate I,D TLBs */ + mcr p15, 0, r4, c2, c0 /* Load page table pointer */ + +#ifdef CPU_DCACHE_WRITETHROUGH + mov r0, #4 /* Disable write-back on caches explicitly */ + mcr p15, 7, r0, c15, c0, 0 +#endif + + /* Enable the MMU and caches + * lr = Resume at .Lvstart with the MMU enabled + */ + + ldr lr, .LCvstart /* Abs. virtual address */ + + mov r0, #0x1f /* Domains 0, 1 = client */ + mcr p15, 0, r0, c3, c0 /* Load domain access register */ + mrc p15, 0, r0, c1, c0 /* Get control register */ + + /* Clear bits (see start.h) */ + + bic r0, r0, #(CR_R|CR_F|CR_Z) + bic r0, r0, #(CR_A|CR_C|CR_W) + bic r0, r0, #(CR_I) + + /* Set bits (see start.h) */ + + orr r0, r0, #(CR_M|CR_P|CR_D) + orr r0, r0, #(CR_S|CR_V) + +#ifdef CPU_CACHE_ROUND_ROBIN + orr r0, r0, #(CR_RR) +#endif +#ifndef CPU_DCACHE_DISABLE + orr r0, r0, #(CR_C) +#endif +#ifndef CPU_ICACHE_DISABLE + orr r0, r0, #(CR_I) +#endif +#ifdef ALIGNMENT_TRAP + orr r0, r0, #(CR_A) +#endif + mcr p15, 0, r0, c1, c0, 0 /* write control reg */ + + /* Get TMP=2 Processor ID register */ + + mrc p15, 0, r1, c0, c0, 0 /* read id reg */ + mov r1, r1 + mov r1, r1 + + mov pc, lr + +/************************************************************************** + * PC_Relative Data + **************************************************************************/ + + /* These addresses are all virtual address */ + + .type .LCvstart, %object +.LCvstart: + .long .Lvstart + .type .LCmmuflags, %object +.LCmmuflags: + .long MMU_MEMFLAGS + .type .LCppagetable, %object +.LCppgtable: + .long CONFIG_DRAM_START /* Physical start of DRAM */ + .type .LCvpagetable, %object +.LCvpgtable: + .long CONFIG_DRAM_VSTART /* Virtual start of DRAM */ + .size _start, .-_start + +/************************************************************************** + * Name: .Lvstart + **************************************************************************/ + +/* The following is executed after the MMU has been enabled. This uses + * absolute addresses; this is not position independent. + */ + .align 5 + .local .Lvstart + .type .Lvstart, %function +.Lvstart: + + /* Remove the temporary null mapping */ + + ldr r4, .LCvpgtable /* r4=virtual page table */ + ldr r1, .LCppgtable /* r1=phys. page table */ + mksection r3, r1 /* r2=phys. base addr */ + mov r0, #0 /* flags + base = 0 */ + str r0, [r4, r3, lsr #18] /* Undo identity mapping */ + + /* Now setup the pagetables for our normal SDRAM mappings + * mapped region. We round NUTTX_START_VADDR down to the + * nearest megabyte boundary. + */ + + ldr r1, .LCmmuflags /* FLGS=MMU_MEMFLAGS */ + add r3, r3, r1 /* r3=flags + base */ + + add r0, r4, #(NUTTX_START_VADDR & 0xff000000) >> 18 + bic r2, r3, #0x00f00000 + str r2, [r0] + + add r0, r0, #(NUTTX_START_VADDR & 0x00f00000) >> 18 + str r3, [r0], #4 + + /* Now map the remaining NSDRAM_SECTIONS-1 SDRAM sections */ + + .rept NSDRAM_SECTIONS-1 + add r3, r3, #SECTION_SIZE + str r3, [r0], #4 + .endr + + /* Zero BSS and set up the stack pointer */ + + adr r0, .Linitparms + ldmia r0, {r0, r1, sp} + + /* Clear the frame pointer and .bss */ + + mov fp, #0 + +.Lbssinit: + cmp r0, r1 /* Clear up to _bss_end_ */ + strcc fp, [r0],#4 + bcc .Lbssinit + + /* Perform early C-level initialization */ + + bl up_boot + + /* Set up the LEDs */ + +#ifdef CONFIG_ARCH_LEDS + bl up_ledinit +#endif + /* Perform early serial initialization */ + +#ifdef CONFIG_DEV_CONSOLE + bl up_earlyserialinit +#endif + + /* Finally branch to the OS entry point */ + + mov lr, #0 + b os_start + + /* Variables: + * _sbss is the start of the BSS region (see ld.script) + * _ebss is the end of the BSS regsion (see ld.script) + * The idle task stack starts at the end of BSS and is + * of size CONFIG_PROC_STACK_SIZE. The heap continues + * from there until the end of memory. See g_heapbase + * below. + */ + +.Linitparms: + .long _sbss + .long _ebss + .long _ebss+CONFIG_PROC_STACK_SIZE-4 + .size .Lvstart, .-.Lvstart + + /* This global variable is unsigned long g_heapbase and is + * exported from here only because of its coupling to .Linitparms + * above. + */ + + .data + .align 4 + .globl g_heapbase + .type g_heapbase, object +g_heapbase: + .long _ebss+CONFIG_PROC_STACK_SIZE + .size g_heapbase, .-g_heapbase + .end + diff --git a/nuttx/arch/arm/src/common/up_idle.c b/nuttx/arch/arm/src/common/up_idle.c new file mode 100644 index 000000000..d90b50aa8 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_idle.c @@ -0,0 +1,87 @@ +/************************************************************ + * common/up_idle.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <nuttx/arch.h> +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_idle + * + * Description: + * up_idle() is the logic that will be executed when their + * is no other ready-to-run task. This is processor idle + * time and will continue until some interrupt occurs to + * cause a context switch from the idle task. + * + * Processing in this state may be processor-specific. e.g., + * this is where power management operations might be + * performed. + * + ************************************************************/ + +void up_idle(void) +{ +#if defined(CONFIG_SUPPRESS_INTERRUPTS) || defined(CONFIG_SUPPRESS_TIMER_INTS) + /* If the system is idle and there are no timer interrupts, + * then process "fake" timer interrupts. Hopefully, something + * will wake up. + */ + + sched_process_timer(); +#endif +} + diff --git a/nuttx/arch/arm/src/common/up_initialize.c b/nuttx/arch/arm/src/common/up_initialize.c new file mode 100644 index 000000000..9fad207e5 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_initialize.c @@ -0,0 +1,104 @@ +/************************************************************ + * common/up_initialize.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <debug.h> +#include <nuttx/arch.h> +#include <nuttx/fs.h> +#include "up_arch.h" +#include "up_internal.h" + +/************************************************************ + * Private Types + ************************************************************/ + +/************************************************************ + * Private Function Prototypes + ************************************************************/ + +/************************************************************ + * Global Functions + ************************************************************/ + +/************************************************************ + * Name: up_initialize + * + * Description: + * up_initialize will be called once during OS + * initialization after the basic OS services have been + * initialized. The architecture specific details of + * initializing the OS will be handled here. Such things as + * setting up interrupt service routines, starting the + * clock, and registering device drivers are some of the + * things that are different for each processor and hardware + * platform. + * + * up_initialize is called after the OS initialized but + * before the init process has been started and before the + * libraries have been initialized. OS services and driver + * services are available. + * + ************************************************************/ + +void up_initialize(void) +{ + /* Initialize global variables */ + + current_regs = NULL; + + /* Initialize the interrupt subsystem */ + + up_irqinitialize(); + + /* Initialize the system timer interrupt */ + +#if !defined(CONFIG_SUPPRESS_INTERRUPTS) && !defined(CONFIG_SUPPRESS_TIMER_INTS) + up_timerinit(); +#endif + + /* Register devices */ + + devnull_register(); /* Standard /dev/null */ + + /* Initialize the serial device driver */ + + up_serialinit(); + up_ledon(LED_IRQSENABLED); +} diff --git a/nuttx/arch/arm/src/common/up_initialstate.c b/nuttx/arch/arm/src/common/up_initialstate.c new file mode 100644 index 000000000..64608d481 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_initialstate.c @@ -0,0 +1,91 @@ +/************************************************************ + * common/up_initialstate.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <string.h> +#include <nuttx/arch.h> +#include "up_internal.h" +#include "up_arch.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * 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_CPSR] = SVC_MODE | PSR_I_BIT | PSR_F_BIT; +#else + xcp->regs[REG_CPSR] = SVC_MODE | PSR_F_BIT; +#endif +} diff --git a/nuttx/arch/arm/src/common/up_internal.h b/nuttx/arch/arm/src/common/up_internal.h new file mode 100644 index 000000000..185b4e8ab --- /dev/null +++ b/nuttx/arch/arm/src/common/up_internal.h @@ -0,0 +1,161 @@ +/************************************************************ + * common/up_internal.h + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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. + * + ************************************************************/ + +#ifndef __UP_INTERNAL_H +#define __UP_INTERNAL_H + +/************************************************************ + * Included Files + ************************************************************/ + +/************************************************************ + * Definitions + ************************************************************/ + +/* Bring-up debug configurations. These are here (vs defconfig) + * because these should only be controlled during low level + * board bring-up and not part of normal platform configuration. + */ + +#undef CONFIG_SUPPRESS_INTERRUPTS /* Do not enable interrupts */ +#undef CONFIG_SUPPRESS_TIMER_INTS /* No timer */ +#undef CONFIG_SUPPRESS_SERIAL_INTS /* Console will poll */ +#undef CONFIG_SUPPRESS_UART_CONFIG /* Do not reconfig UART */ +#define CONFIG_DUMP_ON_EXIT 1 /* Dump task state on exit */ + +/************************************************************ + * Public Types + ************************************************************/ + +#ifndef __ASSEMBLY__ +typedef void (*up_vector_t)(void); +#endif + +/************************************************************ + * Public Variables + ************************************************************/ + +#ifndef __ASSEMBLY__ +/* This holds a references to the current interrupt level + * register storage structure. If is non-NULL only during + * interrupt processing. + */ + +extern uint32 *current_regs; + +/* This is the beginning of heap as provided from up_head.S. + * This is the first address in DRAM after the loaded + * program+bss+idle stack. The end of the heap is + * CONFIG_DRAM_END + */ + +extern uint32 g_heapbase; +#endif + +/************************************************************ + * Inline Functions + ************************************************************/ + + +/************************************************************ + * Public Functions + ************************************************************/ + +#ifndef __ASSEMBLY__ + +/* Defined in files with the same name as the function */ + +extern void up_boot(void); +extern void up_copystate(uint32 *dest, uint32 *src); +extern void up_dataabort(uint32 *regs); +extern void up_delay(int milliseconds); +extern void up_doirq(uint32 *regs); +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); + +#ifdef CONFIG_DEBUG +extern void up_lowputc(char ch); +#else +# define up_lowputc(ch) +#endif + +/* Defined in up_vectors.S */ + +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); + +/* Defined in up_serial.c */ + +extern void up_earlyserialinit(void); +extern void up_serialinit(void); + +/* Defined in up_watchdog.c */ + +extern void up_wdtinit(void); + +/* Defined in up_timerisr.c */ + +extern void up_timerinit(void); + +/* Defined in up_irq.c */ + +extern void up_maskack_irq(int irq); + +/* Defined in board/up_leds.c */ + +#ifdef CONFIG_ARCH_LEDS +extern void up_ledinit(void); +extern void up_ledon(int led); +extern void up_ledoff(int led); +#else +# define up_ledinit() +# define up_ledon(led) +# define up_ledoff(led) +#endif + +#endif /* __ASSEMBLY__ */ + +#endif /* __UP_INTERNAL_H */ diff --git a/nuttx/arch/arm/src/common/up_interruptcontext.c b/nuttx/arch/arm/src/common/up_interruptcontext.c new file mode 100644 index 000000000..a8c3210aa --- /dev/null +++ b/nuttx/arch/arm/src/common/up_interruptcontext.c @@ -0,0 +1,68 @@ +/************************************************************ + * common/up_interruptcontext.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <nuttx/arch.h> +#include <nuttx/irq.h> +#include "up_internal.h" + +/************************************************************ + * Private Types + ************************************************************/ + +/************************************************************ + * Private Function Prototypes + ************************************************************/ + +/************************************************************ + * Global Functions + ************************************************************/ + +/************************************************************ + * Name: up_interrupt_context + * + * Description: Return TRUE is we are currently executing in + * the interrupt handler context. + ************************************************************/ + +boolean up_interrupt_context(void) +{ + return current_regs != NULL; +} diff --git a/nuttx/arch/arm/src/common/up_nommuhead.S b/nuttx/arch/arm/src/common/up_nommuhead.S new file mode 100644 index 000000000..9d10449a7 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_nommuhead.S @@ -0,0 +1,162 @@ +/******************************************************************** + * common/up_nommuhead.S + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include "up_internal.h" +#include "up_arch.h" + +/******************************************************************** + * Macros + ********************************************************************/ + + /* This macro will modify r0, r1, r2 and r14 */ + +#ifdef CONFIG_DEBUG + .macro showprogress, code + mov r0, #\code + bl up_lowputc + .endm +#else + .macro showprogress, code + .endm +#endif + +/******************************************************************** + * OS Entry Point + ********************************************************************/ + +/* We assume the bootloader has already initialized most of the h/w for + * us and that only leaves us having to do some os specific things + * below. + */ + .text + .global __start + .type __start, #function +__start: + + /* First, setup initial processor mode */ + + mov r0, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT ) + msr cpsr, r0 + + showprogress 'A' + + /* Setup system stack (and get the BSS range) */ + + adr r0, LC0 + ldmia r0, {r4, r5, sp} + + /* Clear system BSS section */ + + mov r0, #0 +1: cmp r4, r5 + strcc r0, [r4], #4 + bcc 1b + + showprogress 'B' + + /* Copy system .data sections to new home in RAM. */ + +#ifdef CONFIG_BOOT_FROM_FLASH + + adr r3, LC2 + ldmia r3, {r0, r1, r2} + +1: ldmia r0!, {r3 - r10} + stmia r1!, {r3 - r10} + cmp r1, r2 + blt 1b + +#endif + /* Perform early serial initialization */ + + mov fp, #0 + bl up_earlyserialinit + +#ifdef CONFIG_DEBUG + mov r0, #'C' + bl up_putc + mov r0, #'\n' + bl up_putc +#endif + /* Initialize onboard LEDs */ + +#ifdef CONFIG_ARCH_LEDS + bl up_ledinit +#endif + + /* Then jump to OS entry */ + + b os_start + + /* Variables: + * _sbss is the start of the BSS region (see ld.script) + * _ebss is the end of the BSS regsion (see ld.script) + * The idle task stack starts at the end of BSS and is + * of size CONFIG_PROC_STACK_SIZE. The heap continues + * from there until the end of memory. See g_heapbase + * below. + */ + +LC0: .long _sbss + .long _ebss + .long _ebss+CONFIG_PROC_STACK_SIZE-4 + +#ifdef CONFIG_BOOT_FROM_FLASH +LC2: .long _eronly /* Where .data defaults are stored in FLASH */ + .long _sdata /* Where .data needs to reside in SDRAM */ + .long _edata +#endif + .size __start, .-__start + + /* This global variable is unsigned long g_heapbase and is + * exported from here only because of its coupling to LCO + * above. + */ + + .data + .align 4 + .globl g_heapbase + .type g_heapbase, object +g_heapbase: + .long _ebss+CONFIG_PROC_STACK_SIZE + .size g_heapbase, .-g_heapbase + + .end + diff --git a/nuttx/arch/arm/src/common/up_prefetchabort.c b/nuttx/arch/arm/src/common/up_prefetchabort.c new file mode 100644 index 000000000..343c02c87 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_prefetchabort.c @@ -0,0 +1,81 @@ +/************************************************************ + * common/up_prefetchabort.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <debug.h> +#include <nuttx/irq.h> +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * 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 + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_prefetchabort + ************************************************************/ + +void up_prefetchabort(uint32 *regs) +{ + lldbg("Prefetch abort at 0x%x\n", regs[REG_PC]); + current_regs = regs; + PANIC(OSERR_ERREXCEPTION); +} diff --git a/nuttx/arch/arm/src/common/up_releasepending.c b/nuttx/arch/arm/src/common/up_releasepending.c new file mode 100644 index 000000000..7f8016d53 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_releasepending.c @@ -0,0 +1,131 @@ +/************************************************************ + * common/up_releasepending.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <sched.h> +#include <debug.h> +#include <nuttx/arch.h> +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_release_pending + * + * Description: + * Release and ready-to-run tasks that have + * collected in the pending task list. This can call a + * context switch if a new task is placed at the head of + * the ready to run list. + * + ************************************************************/ + +void up_release_pending(void) +{ + _TCB *rtcb = (_TCB*)g_readytorun.head; + + lldbg("From TCB=%p\n", rtcb); + + /* Merge the g_pendingtasks list into the g_readytorun task list */ + + /* sched_lock(); */ + if (sched_mergepending()) + { + /* The currently active task has changed! We will need to + * switch contexts. First check if we are operating in + * interrupt context: + */ + + if (current_regs) + { + /* Yes, then we have to do things differently. + * Just copy the current_regs into the OLD rtcb. + */ + + up_copystate(rtcb->xcp.regs, current_regs); + + /* Restore the exception context of the rtcb at the (new) head + * of the g_readytorun task list. + */ + + rtcb = (_TCB*)g_readytorun.head; + lldbg("New Active Task TCB=%p\n", rtcb); + + /* Then switch contexts */ + + up_copystate(current_regs, rtcb->xcp.regs); + } + + /* Copy the exception context into the TCB of the task that + * was currently active. if up_saveusercontext returns a non-zero + * value, then this is really the previously running task + * restarting! + */ + + else if (!up_saveusercontext(rtcb->xcp.regs)) + { + /* Restore the exception context of the rtcb at the (new) head + * of the g_readytorun task list. + */ + + rtcb = (_TCB*)g_readytorun.head; + lldbg("New Active Task TCB=%p\n", rtcb); + + /* Then switch contexts */ + + up_fullcontextrestore(rtcb->xcp.regs); + } + } +} diff --git a/nuttx/arch/arm/src/common/up_releasestack.c b/nuttx/arch/arm/src/common/up_releasestack.c new file mode 100644 index 000000000..b00b9ab0c --- /dev/null +++ b/nuttx/arch/arm/src/common/up_releasestack.c @@ -0,0 +1,78 @@ +/************************************************************ + * common/up_releasestack.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <sched.h> +#include <debug.h> +#include <nuttx/arch.h> +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Private Types + ************************************************************/ + +/************************************************************ + * Private Function Prototypes + ************************************************************/ + +/************************************************************ + * Global Functions + ************************************************************/ + +/************************************************************ + * Name: up_release_stack + * + * Description: + * A task has been stopped. Free all stack + * related resources retained int the defunct TCB. + * + ************************************************************/ + +void up_release_stack(_TCB *dtcb) +{ + if (dtcb->stack_alloc_ptr) + { + sched_free(dtcb->stack_alloc_ptr); + dtcb->stack_alloc_ptr = NULL; + } + + dtcb->adj_stack_size = 0; +} diff --git a/nuttx/arch/arm/src/common/up_reprioritizertr.c b/nuttx/arch/arm/src/common/up_reprioritizertr.c new file mode 100644 index 000000000..1f589b73f --- /dev/null +++ b/nuttx/arch/arm/src/common/up_reprioritizertr.c @@ -0,0 +1,179 @@ +/************************************************************ + * common/up_reprioritizertr.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <sched.h> +#include <debug.h> +#include <nuttx/arch.h> +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_reprioritize_rtr + * + * Description: + * Called when the priority of a running or + * ready-to-run task changes and the reprioritization will + * cause a context switch. Two cases: + * + * 1) The priority of the currently running task drops and the next + * task in the ready to run list has priority. + * 2) An idle, ready to run task's priority has been raised above the + * the priority of the current, running task and it now has the + * priority. + * + * Inputs: + * tcb: The TCB of the task that has been reprioritized + * priority: The new task priority + * + ************************************************************/ + +void up_reprioritize_rtr(_TCB *tcb, ubyte priority) +{ + /* Verify that the caller is sane */ + + if (tcb->task_state < FIRST_READY_TO_RUN_STATE || + tcb->task_state > LAST_READY_TO_RUN_STATE || + priority < SCHED_PRIORITY_MIN || + priority > SCHED_PRIORITY_MAX) + { + PANIC(OSERR_BADREPRIORITIZESTATE); + } + else + { + _TCB *rtcb = (_TCB*)g_readytorun.head; + boolean switch_needed; + + lldbg("TCB=%p PRI=%d\n", tcb, priority); + + /* Remove the tcb task from the ready-to-run list. + * sched_removereadytorun will return TRUE if we just + * remove the head of the ready to run list. + */ + + switch_needed = sched_removereadytorun(tcb); + + /* Setup up the new task priority */ + + tcb->sched_priority = (ubyte)priority; + + /* Return the task to the specified blocked task list. + * sched_addreadytorun will return TRUE if the task was + * added to the new list. We will need to perform a context + * switch only if the EXCLUSIVE or of the two calls is non-zero + * (i.e., one and only one the calls changes the head of the + * ready-to-run list). + */ + + switch_needed ^= sched_addreadytorun(tcb); + + /* Now, perform the context switch if one is needed */ + + if (switch_needed) + { + /* If we are going to do a context switch, then now is the right + * time to add any pending tasks back into the ready-to-run list. + * task list now + */ + + if (g_pendingtasks.head) + { + sched_mergepending(); + } + + /* Are we in an interrupt handler? */ + + if (current_regs) + { + /* Yes, then we have to do things differently. + * Just copy the current_regs into the OLD rtcb. + */ + + up_copystate(rtcb->xcp.regs, current_regs); + + /* Restore the exception context of the rtcb at the (new) head + * of the g_readytorun task list. + */ + + rtcb = (_TCB*)g_readytorun.head; + lldbg("New Active Task TCB=%p\n", rtcb); + + /* Then switch contexts */ + + up_copystate(current_regs, rtcb->xcp.regs); + } + + /* Copy the exception context into the TCB at the (old) head of the + * g_readytorun Task list. if up_saveusercontext returns a non-zero + * value, then this is really the previously running task restarting! + */ + + else if (!up_saveusercontext(rtcb->xcp.regs)) + { + /* Restore the exception context of the rtcb at the (new) head + * of the g_readytorun task list. + */ + + rtcb = (_TCB*)g_readytorun.head; + lldbg("New Active Task TCB=%p\n", rtcb); + + /* Then switch contexts */ + + up_fullcontextrestore(rtcb->xcp.regs); + } + } + } +} diff --git a/nuttx/arch/arm/src/common/up_saveusercontext.S b/nuttx/arch/arm/src/common/up_saveusercontext.S new file mode 100644 index 000000000..cdaf7cc25 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_saveusercontext.S @@ -0,0 +1,119 @@ +/************************************************************************** + * common/up_saveusercontext.S + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/irq.h> +#include "up_internal.h" + +/************************************************************************** + * Private Definitions + **************************************************************************/ + +/************************************************************************** + * Private Types + **************************************************************************/ + +/************************************************************************** + * Private Function Prototypes + **************************************************************************/ + +/************************************************************************** + * Global Variables + **************************************************************************/ + +/************************************************************************** + * Private Variables + **************************************************************************/ + +/************************************************************************** + * Private Functions + **************************************************************************/ + +/************************************************************************** + * Public Functions + **************************************************************************/ + +/************************************************************************** + * Name: up_saveusercontext + **************************************************************************/ + + .text + .globl up_saveusercontext + .type up_saveusercontext, function +up_saveusercontext: + /* On entry, a1 (r0) holds address of struct xcptcontext. + * Offset to the user region. + */ + + /* Make sure that the return value will be non-zero (the + * value of the other volatile registers don't matter -- + * r1-r3, ip). This function is called throught the + * noraml C calling conventions and the values of these + * registers cannot be assumed at the point of setjmp + * return. + */ + + mov ip, #1 + str ip, [r0, #(4*REG_R0)] + + /* Save the volatile registers (plus r12 which really + * doesn't need to be saved) + */ + + add r1, r0, #(4*REG_R4) + stmia r1, {r4-r14} + + /* Save the current cpsr */ + + mrs r2, cpsr /* R3 = CPSR value */ + add r1, r0, #(4*REG_CPSR) + str r2, [r1] + + /* Finally save the return address as the PC so that we + * return to the exit from this function. + */ + + add r1, r0, #(4*REG_PC) + str lr, [r1] + + /* Return 0 */ + + mov r0, #0 /* Return value == 0 */ + mov pc, lr /* Return */ + .size up_saveusercontext, . - up_saveusercontext + diff --git a/nuttx/arch/arm/src/common/up_schedulesigaction.c b/nuttx/arch/arm/src/common/up_schedulesigaction.c new file mode 100644 index 000000000..6b56cc3b6 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_schedulesigaction.c @@ -0,0 +1,190 @@ +/************************************************************ + * common/up_schedulesigaction.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <sched.h> +#include <debug.h> +#include <nuttx/arch.h> +#include "os_internal.h" +#include "up_internal.h" +#include "up_arch.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_schedule_sigaction + * + * Description: + * This function is called by the OS when one or more + * signal handling actions have been queued for execution. + * The architecture specific code must configure things so + * that the 'igdeliver' callback is executed on the thread + * specified by 'tcb' as soon as possible. + * + * This function may be called from interrupt handling logic. + * + * This operation should not cause the task to be unblocked + * nor should it cause any immediate execution of sigdeliver. + * Typically, a few cases need to be considered: + * + * (1) This function may be called from an interrupt handler + * During interrupt processing, all xcptcontext structures + * should be valid for all tasks. That structure should + * be modified to invoke sigdeliver() either on return + * from (this) interrupt or on some subsequent context + * switch to the recipient task. + * (2) If not in an interrupt handler and the tcb is NOT + * the currently executing task, then again just modify + * the saved xcptcontext structure for the recipient + * task so it will invoke sigdeliver when that task is + * later resumed. + * (3) If not in an interrupt handler and the tcb IS the + * currently executing task -- just call the signal + * handler now. + * + ************************************************************/ + +void up_schedule_sigaction(_TCB *tcb, sig_deliver_t sigdeliver) +{ + /* Refuse to handle nested signal actions */ + + dbg("tcb=0x%p sigdeliver=0x%p\n", tcb, sigdeliver); + + if (!tcb->xcp.sigdeliver) + { + irqstate_t flags; + + /* Make sure that interrupts are disabled */ + + flags = irqsave(); + + /* First, handle some special cases when the signal is + * being delivered to the currently executing task. + */ + + dbg("rtcb=0x%p current_regs=0x%p\n", g_readytorun.head, current_regs); + + if (tcb == (_TCB*)g_readytorun.head) + { + /* CASE 1: We are not in an interrupt handler and + * a task is signalling itself for some reason. + */ + + if (!current_regs) + { + /* In this case just deliver the signal now. */ + + sigdeliver(tcb); + } + + /* CASE 2: We are in an interrupt handler AND the + * interrupted task is the same as the one that + * must receive the signal, then we will have to modify + * the return state as well as the state in the TCB. + */ + + else + { + /* Save the return lr and cpsr and one scratch register + * These will be restored by the signal trampoline after + * the signals have been delivered. + */ + + tcb->xcp.sigdeliver = sigdeliver; + tcb->xcp.saved_pc = current_regs[REG_PC]; + tcb->xcp.saved_cpsr = current_regs[REG_CPSR]; + + /* Then set up to vector to the trampoline with interrupts + * disabled + */ + + current_regs[REG_PC] = (uint32)up_sigdeliver; + current_regs[REG_CPSR] = SVC_MODE | PSR_I_BIT | PSR_F_BIT; + + /* And make sure that the saved context in the TCB + * is the same as the interrupt return context. + */ + + up_copystate(tcb->xcp.regs, current_regs); + } + } + + /* Otherwise, we are (1) signaling a task is not running + * from an interrupt handler or (2) we are not in an + * interrupt handler and the running task is signalling + * some non-running task. + */ + + else + { + /* Save the return lr and cpsr and one scratch register + * These will be restored by the signal trampoline after + * the signals have been delivered. + */ + + tcb->xcp.sigdeliver = sigdeliver; + tcb->xcp.saved_pc = tcb->xcp.regs[REG_PC]; + tcb->xcp.saved_cpsr = tcb->xcp.regs[REG_CPSR]; + + /* Then set up to vector to the trampoline with interrupts + * disabled + */ + + tcb->xcp.regs[REG_PC] = (uint32)up_sigdeliver; + tcb->xcp.regs[REG_CPSR] = SVC_MODE | PSR_I_BIT | PSR_F_BIT; + } + + irqrestore(flags); + } +} diff --git a/nuttx/arch/arm/src/common/up_sigdeliver.c b/nuttx/arch/arm/src/common/up_sigdeliver.c new file mode 100644 index 000000000..e6a0dfd89 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_sigdeliver.c @@ -0,0 +1,135 @@ +/************************************************************ + * common/up_sigdeliver.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <sched.h> +#include <debug.h> +#include <nuttx/irq.h> +#include <nuttx/arch.h> +#include "os_internal.h" +#include "up_internal.h" +#include "up_arch.h" + +/************************************************************ + * Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Functions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_sigdeliver + * + * Description: + * This is the a signal handling trampoline. When a + * signal action was posted. The task context was mucked + * with and forced to branch to this location with interrupts + * disabled. + * + ************************************************************/ + +void up_sigdeliver(void) +{ + _TCB *rtcb = (_TCB*)g_readytorun.head; + uint32 regs[XCPTCONTEXT_REGS]; + sig_deliver_t sigdeliver; + + /* Save the errno. This must be preserved throughout the + * signal handling so that the the user code final gets + * the correct errno value (probably EINTR). + */ + + int saved_errno = rtcb->errno; + + up_ledon(LED_SIGNAL); + + dbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p\n", + rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head); + ASSERT(rtcb->xcp.sigdeliver != NULL); + + /* Save the real return state on the stack. */ + + up_copystate(regs, rtcb->xcp.regs); + regs[REG_PC] = rtcb->xcp.saved_pc; + regs[REG_CPSR] = rtcb->xcp.saved_cpsr; + + /* Get a local copy of the sigdeliver function pointer. + * we do this so that we can nullify the sigdeliver + * function point in the TCB and accept more signal + * deliveries while processing the current pending + * signals. + */ + + sigdeliver = rtcb->xcp.sigdeliver; + rtcb->xcp.sigdeliver = NULL; + + /* Then restore the task interrupt statat. */ + + irqrestore(regs[REG_CPSR]); + + /* Deliver the signals */ + + sigdeliver(rtcb); + + /* Output any debug messaged BEFORE restoreing errno + * (becuase they may alter errno), then restore the + * original errno that is needed by the user logic + * (it is probably EINTR). + */ + + dbg("Resuming\n"); + rtcb->errno = saved_errno; + + /* Then restore the correct state for this thread of + * execution. + */ + + up_ledoff(LED_SIGNAL); + up_fullcontextrestore(regs); +} diff --git a/nuttx/arch/arm/src/common/up_syscall.c b/nuttx/arch/arm/src/common/up_syscall.c new file mode 100644 index 000000000..47a7150be --- /dev/null +++ b/nuttx/arch/arm/src/common/up_syscall.c @@ -0,0 +1,94 @@ +/************************************************************ + * common/up_syscall.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <debug.h> +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * 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 + ************************************************************/ + +/************************************************************ + * vectors + ************************************************************/ + +/************************************************************ + * Private Functions + ************************************************************/ + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: up_syscall + * + * Description: + * SWI interrupts will vection here with insn=the SWI + * instruction and xcp=the interrupt context + * + * The handler may get the SWI number be de-referencing + * the return address saved in the xcp and decoding + * the SWI instruction + * + ************************************************************/ + +void up_syscall(uint32 *regs) +{ + lldbg("Syscall from 0x%x\n", regs[REG_PC]); + current_regs = regs; + PANIC(OSERR_ERREXCEPTION); +} diff --git a/nuttx/arch/arm/src/common/up_unblocktask.c b/nuttx/arch/arm/src/common/up_unblocktask.c new file mode 100644 index 000000000..97c416182 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_unblocktask.c @@ -0,0 +1,161 @@ +/************************************************************ + * common/up_unblocktask.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <sched.h> +#include <debug.h> +#include <nuttx/arch.h> +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_unblock_task + * + * Description: + * A task is currently in an inactive task list + * but has been prepped to execute. Move the TCB to the + * ready-to-run list, restore its context, and start execution. + * + * Inputs: + * tcb: Refers to the tcb to be unblocked. This tcb is + * in one of the waiting tasks lists. It must be moved to + * the ready-to-run list and, if it is the highest priority + * ready to run taks, executed. + * + ************************************************************/ + +void up_unblock_task(_TCB *tcb) +{ + /* Verify that the context switch can be performed */ + + if ((tcb->task_state < FIRST_BLOCKED_STATE) || + (tcb->task_state > LAST_BLOCKED_STATE)) + { + PANIC(OSERR_BADUNBLOCKSTATE); + } + else + { + _TCB *rtcb = (_TCB*)g_readytorun.head; + + lldbg("Unblocking TCB=%p\n", tcb); + + /* Remove the task from the blocked task list */ + + sched_removeblocked(tcb); + + /* Reset its timeslice. This is only meaningful for round + * robin tasks but it doesn't here to do it for everything + */ + +#if CONFIG_RR_INTERVAL > 0 + tcb->timeslice = CONFIG_RR_INTERVAL; +#endif + + /* Add the task in the correct location in the prioritized + * g_readytorun task list + */ + + if (sched_addreadytorun(tcb)) + { + /* The currently active task has changed! We need to do + * a context switch to the new task. + * + * Are we in an interrupt handler? + */ + + if (current_regs) + { + /* Yes, then we have to do things differently. + * Just copy the current_regs into the OLD rtcb. + */ + + up_copystate(rtcb->xcp.regs, current_regs); + + /* Restore the exception context of the rtcb at the (new) head + * of the g_readytorun task list. + */ + + rtcb = (_TCB*)g_readytorun.head; + lldbg("New Active Task TCB=%p\n", rtcb); + + /* Then switch contexts */ + + up_copystate(current_regs, rtcb->xcp.regs); + } + + /* We are not in an interrupt handler. Copy the user C context + * into the TCB of the task that was previously active. if + * up_saveusercontext returns a non-zero value, then this is really the + * previously running task restarting! + */ + + else if (!up_saveusercontext(rtcb->xcp.regs)) + { + /* Restore the exception context of the new task that is ready to + * run (probably tcb). This is the new rtcb at the head of the + * g_readytorun task list. + */ + + rtcb = (_TCB*)g_readytorun.head; + lldbg("New Active Task TCB=%p\n", rtcb); + + /* Then switch contexts */ + + up_fullcontextrestore(rtcb->xcp.regs); + } + } + } +} diff --git a/nuttx/arch/arm/src/common/up_undefinedinsn.c b/nuttx/arch/arm/src/common/up_undefinedinsn.c new file mode 100644 index 000000000..261118ec7 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_undefinedinsn.c @@ -0,0 +1,79 @@ +/************************************************************ + * common/up_undefinedinsn.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * 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 + ************************************************************/ + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: up_undefinedinsn + ************************************************************/ + +void up_undefinedinsn(uint32 *regs) +{ + lldbg("Undefined instruction at 0x%x\n", regs[REG_PC]); + current_regs = regs; + PANIC(OSERR_UNDEFINEDINSN); +} diff --git a/nuttx/arch/arm/src/common/up_usestack.c b/nuttx/arch/arm/src/common/up_usestack.c new file mode 100644 index 000000000..2da044182 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_usestack.c @@ -0,0 +1,118 @@ +/************************************************************ + * common/up_usestack.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <sched.h> +#include <debug.h> +#include <nuttx/kmalloc.h> +#include <nuttx/arch.h> +#include "up_internal.h" + +/************************************************************ + * Private Types + ************************************************************/ + +/************************************************************ + * Private Function Prototypes + ************************************************************/ + +/************************************************************ + * Global Functions + ************************************************************/ + +/************************************************************ + * Name: up_use_stack + * + * Description: + * Setup up stack-related information in the TCB + * using pre-allocated stack memory + * + * The following TCB fields must be initialized: + * adj_stack_size: Stack size after adjustment for hardware, + * processor, etc. This value is retained only for debug + * purposes. + * stack_alloc_ptr: Pointer to allocated stack + * adj_stack_ptr: Adjusted stack_alloc_ptr for HW. The + * initial value of the stack pointer. + * + * Inputs: + * tcb: The TCB of new task + * stack_size: The allocated stack size. + * + ************************************************************/ + +STATUS up_use_stack(_TCB *tcb, void *stack, size_t stack_size) +{ + size_t top_of_stack; + size_t size_of_stack; + + if (tcb->stack_alloc_ptr) + { + sched_free(tcb->stack_alloc_ptr); + } + + /* Save the stack allocation */ + + tcb->stack_alloc_ptr = stack; + + /* The Arm7Tdmi uses a push-down stack: the stack grows + * toward loweraddresses in memory. The stack pointer + * register, points to the lowest, valid work address + * (the "top" of the stack). Items on the stack are + * referenced as positive word offsets from sp. + */ + + top_of_stack = (uint32)tcb->stack_alloc_ptr + stack_size - 4; + + /* The Arm7Tdmi stack must be aligned at word (4 byte) + * boundaries. If necessary top_of_stack must be rounded + * down to the next boundary + */ + + top_of_stack &= ~3; + size_of_stack = top_of_stack - (uint32)tcb->stack_alloc_ptr + 4; + + /* Save the adjusted stack values in the _TCB */ + + tcb->adj_stack_size = top_of_stack; + tcb->adj_stack_size = size_of_stack; + + return OK; +} diff --git a/nuttx/arch/arm/src/common/up_vectors.S b/nuttx/arch/arm/src/common/up_vectors.S new file mode 100644 index 000000000..648060218 --- /dev/null +++ b/nuttx/arch/arm/src/common/up_vectors.S @@ -0,0 +1,449 @@ +/******************************************************************** + * common/up_vectors.S + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <nuttx/irq.h> +#include "up_arch.h" + +/******************************************************************** + * Definitions + ********************************************************************/ + +/******************************************************************** + * Global Data + ********************************************************************/ + + .data +g_irqtmp: + .word 0 /* Saved lr */ + .word 0 /* Saved spsr */ +g_undeftmp: + .word 0 /* Saved lr */ + .word 0 /* Saved spsr */ +g_aborttmp: + .word 0 /* Saved lr */ + .word 0 /* Saved spsr */ + +/******************************************************************** + * Assembly Macros + ********************************************************************/ + +/******************************************************************** + * Private Functions + ********************************************************************/ + + .text + +/******************************************************************** + * Public Functions + ********************************************************************/ + + .text + +/******************************************************************** + * Name: up_vectorirq + * + * Description: + * Interrupt excetpion. Entered in IRQ mode with spsr = SVC + * CPSR, lr = SVC PC + ********************************************************************/ + + .globl up_vectorirq + .type up_vectorirq, %function +up_vectorirq: + /* On entry, we are in IRQ mode. We are free to use + * the IRQ mode r13 and r14. + * + */ + + ldr r13, .Lirqtmp + sub lr, lr, #4 + str lr, [r13] @ save lr_IRQ + mrs lr, spsr + str lr, [r13, #4] @ save spsr_IRQ + + /* Then switch back to SVC mode */ + + bic lr, lr, #MODE_MASK /* Keep F and T bits */ + orr lr, lr, #(SVC_MODE | PSR_I_BIT) + msr cpsr_c, lr /* Switch to SVC mode */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 + + /* Get the values for r15(pc) and CPSR in r3 and r4 */ + + ldr r0, .Lirqtmp /* Points to temp storage */ + ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the IRQ handler with interrupts disabled. */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_doirq /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ +.Lnoirqset: + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr, r0 + ldmia sp, {r0-r15}^ /* Return */ + +.Lirqtmp: + .word g_irqtmp + + .align 5 + +/******************************************************************** + * Function: up_vectorswi + * + * Description: + * SWI interrupt. We enter the SWI in SVC mode + ********************************************************************/ + + .globl up_vectorswi + .type up_vectorswi, %function +up_vectorswi: + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp), r14(lr), r15(pc) + * and CPSR in r1-r4 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 /* R14 is altered on return from SWI */ + mov r3, r14 /* Save r14 as the PC as well */ + mrs r4, spsr /* Get the saved CPSR */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the SWI handler with interrupts disabled. + * void up_syscall(struct xcptcontext *xcp) + */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_syscall /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ + + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr, r0 + ldmia sp, {r0-r15}^ /* Return */ + + .align 5 + +/******************************************************************** + * Name: up_vectordata + * + * Description: + * Data abort Exception dispatcher. Give control to data + * abort handler. This function is entered in ABORT mode + * with spsr = SVC CPSR, lr = SVC PC + * + ********************************************************************/ + + .globl up_vectordata + .type up_vectordata, %function +up_vectordata: + /* On entry we are free to use the ABORT mode registers + * r13 and r14 + */ + + ldr r13, .Ldaborttmp /* Points to temp storage */ + sub lr, lr, #8 /* Fixup return */ + str lr, [r13] /* Save in temp storage */ + mrs lr, spsr /* Get SPSR */ + str lr, [r13, #4] /* Save in temp storage */ + + /* Then switch back to SVC mode */ + + bic lr, lr, #MODE_MASK /* Keep F and T bits */ + orr lr, lr, #(SVC_MODE | PSR_I_BIT) + msr cpsr_c, lr /* Switch to SVC mode */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 + + /* Get the values for r15(pc) and CPSR in r3 and r4 */ + + ldr r0, .Ldaborttmp /* Points to temp storage */ + ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the data abort handler with interrupts disabled. + * void up_dataabort(struct xcptcontext *xcp) + */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_dataabort /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ + + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr_cxsf, r0 + ldmia sp, {r0-r15}^ /* Return */ + +.Ldaborttmp: + .word g_aborttmp + + .align 5 + +/******************************************************************** + * Name: up_vectorprefetch + * + * Description: + * Prefetch abort exception. Entered in ABT mode with + * spsr = SVC CPSR, lr = SVC PC + ********************************************************************/ + + .globl up_vectorprefetch + .type up_vectorprefetch, %function +up_vectorprefetch: + /* On entry we are free to use the ABORT mode registers + * r13 and r14 + */ + + ldr r13, .Lpaborttmp /* Points to temp storage */ + sub lr, lr, #4 /* Fixup return */ + str lr, [r13] /* Save in temp storage */ + mrs lr, spsr /* Get SPSR */ + str lr, [r13, #4] /* Save in temp storage */ + + /* Then switch back to SVC mode */ + + bic lr, lr, #MODE_MASK /* Keep F and T bits */ + orr lr, lr, #(SVC_MODE | PSR_I_BIT) + msr cpsr_c, lr /* Switch to SVC mode */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 + + /* Get the values for r15(pc) and CPSR in r3 and r4 */ + + ldr r0, .Lpaborttmp /* Points to temp storage */ + ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the prefetch abort handler with interrupts disabled. + * void up_prefetchabort(struct xcptcontext *xcp) + */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_prefetchabort /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ + + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr_cxsf, r0 + ldmia sp, {r0-r15}^ /* Return */ + +.Lpaborttmp: + .word g_aborttmp + + .align 5 + +/******************************************************************** + * Name: up_vectorundefinsn + * + * Description: + * Undefined instruction entry exception. Entered in + * UND mode, spsr = SVC CPSR, lr = SVC PC + * + ********************************************************************/ + + .globl up_vectorundefinsn + .type up_vectorundefinsn, %function +up_vectorundefinsn: + /* On entry we are free to use the UND mode registers + * r13 and r14 + */ + + ldr r13, .Lundeftmp /* Points to temp storage */ + str lr, [r13] /* Save in temp storage */ + mrs lr, spsr /* Get SPSR */ + str lr, [r13, #4] /* Save in temp storage */ + + /* Then switch back to SVC mode */ + + bic lr, lr, #MODE_MASK /* Keep F and T bits */ + orr lr, lr, #(SVC_MODE | PSR_I_BIT) + msr cpsr_c, lr /* Switch to SVC mode */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 + + /* Get the values for r15(pc) and CPSR in r3 and r4 */ + + ldr r0, .Lundeftmp /* Points to temp storage */ + ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the undef insn handler with interrupts disabled. + * void up_undefinedinsn(struct xcptcontext *xcp) + */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_undefinedinsn /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ + + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr_cxsf, r0 + ldmia sp, {r0-r15}^ /* Return */ + +.Lundeftmp: + .word g_undeftmp + + .align 5 + +/******************************************************************** + * Name: up_vectorfiq + * + * Description: + * Shouldn't happen + ********************************************************************/ + + .globl up_vectorfiq + .type up_vectorfiq, %function +up_vectorfiq: + subs pc, lr, #4 + +/******************************************************************** + * Name: up_vectoraddrexcption + * + * Description: + * Shouldn't happen + * + ********************************************************************/ + + .globl up_vectoraddrexcptn + .type up_vectoraddrexcptn, %function +up_vectoraddrexcptn: + b up_vectoraddrexcptn + +/************************************************************************** + * Vector initialization block. + **************************************************************************/ + +/* These will be relocated to VECTOR_BASE. */ + + .globl _vector_start +_vector_start: + ldr pc, .Lresethandler /* 0x00: Reset */ + ldr pc, .Lundefinedhandler /* 0x04: Undefined instruction */ + ldr pc, .Lswihandler /* 0x08: Software interrupt */ + ldr pc, .Lprefetchaborthandler /* 0x0c: Prefetch abort */ + ldr pc, .Ldataaborthandler /* 0x10: Data abort */ + ldr pc, .Laddrexcptnhandler /* 0x14: Address exception */ + ldr pc, .Lirqhandler /* 0x18: IRQ */ + ldr pc, .Lfiqhandler /* 0x1c: FIQ */ + +.Lresethandler: + .long __start +.Lundefinedhandler: + .long up_vectorundefinsn +.Lswihandler: + .long up_vectorswi +.Lprefetchaborthandler: + .long up_vectorprefetch +.Ldataaborthandler: + .long up_vectordata +.Laddrexcptnhandler: + .long up_vectoraddrexcptn +.Lirqhandler: + .long up_vectorirq +.Lfiqhandler: + .long up_vectorfiq + .globl _vector_end +_vector_end: + .end diff --git a/nuttx/arch/arm/src/dm320/Make.defs b/nuttx/arch/arm/src/dm320/Make.defs new file mode 100644 index 000000000..8868ee275 --- /dev/null +++ b/nuttx/arch/arm/src/dm320/Make.defs @@ -0,0 +1,50 @@ +############################################################################ +# dm320/Make.defs +# +# Copyright (C) 2007 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 Gregory Nutt 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. +# +############################################################################ + +HEAD_ASRC = up_head.S + +CMN_ASRCS = up_cache.S up_fullcontextrestore.S up_saveusercontext.S +CMN_CSRCS = up_assert.c up_blocktask.c up_copystate.c up_createstack.c \ + up_dataabort.c up_delay.c up_exit.c up_idle.c \ + up_initialize.c up_initialstate.c up_interruptcontext.c \ + up_prefetchabort.c up_releasepending.c up_releasestack.c \ + up_reprioritizertr.c up_schedulesigaction.c \ + up_sigdeliver.c up_syscall.c up_unblocktask.c \ + up_undefinedinsn.c up_usestack.c + +CHIP_ASRCS = dm320_lowputc.S dm320_restart.S dm320_vectors.S +CHIP_CSRCS = dm320_allocateheap.c dm320_boot.c dm320_doirq.c dm320_irq.c \ + dm320_serial.c dm320_timerisr.c + diff --git a/nuttx/arch/arm/src/dm320/chip.h b/nuttx/arch/arm/src/dm320/chip.h new file mode 100644 index 000000000..51a67e2fd --- /dev/null +++ b/nuttx/arch/arm/src/dm320/chip.h @@ -0,0 +1,57 @@ +/************************************************************************************ + * dm320/chip.h + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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. + * + ************************************************************************************/ + +#ifndef __DM320_CHIP_H +#define __DM320_CHIP_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include "dm320_memorymap.h" +#include "dm320_uart.h" +#include "dm320_timer.h" +#include "dm320_intc.h" +#include "dm320_gio.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Inline Functions + ************************************************************************************/ + +#endif /* __DM320_CHIP_H */ diff --git a/nuttx/arch/arm/src/dm320/dm320_allocateheap.c b/nuttx/arch/arm/src/dm320/dm320_allocateheap.c new file mode 100644 index 000000000..f97a55c7d --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_allocateheap.c @@ -0,0 +1,79 @@ +/************************************************************ + * dm320/dm320_allocateheap.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <debug.h> +#include <nuttx/arch.h> +#include "up_arch.h" +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Functions + ************************************************************/ + +/************************************************************ + * Public Functions + ************************************************************/ + +/************************************************************ + * Name: up_allocate_heap + * + * Description: + * The heap may be statically allocated by + * defining CONFIG_HEAP_BASE and CONFIG_HEAP_SIZE. If these + * are not defined, then this function will be called to + * dynamically set aside the heap region. + * + ************************************************************/ + +void up_allocate_heap(FAR void **heap_start, size_t *heap_size) +{ + up_ledon(LED_HEAPALLOCATE); + *heap_start = (FAR void*)g_heapbase; + *heap_size = (DM320_SDRAM_VADDR + CONFIG_DRAM_SIZE) - g_heapbase; +} diff --git a/nuttx/arch/arm/src/dm320/dm320_boot.c b/nuttx/arch/arm/src/dm320/dm320_boot.c new file mode 100644 index 000000000..46a39dfd2 --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_boot.c @@ -0,0 +1,227 @@ +/************************************************************************************ + * dm320/dm320_boot.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include "up_internal.h" +#include "up_arch.h" + +/************************************************************************************ + * Private Types + ************************************************************************************/ + +/************************************************************************************ + * Private Types + ************************************************************************************/ + +struct section_mapping_s +{ + uint32 physbase; /* Physical address of the region to be mapped */ + uint32 virtbase; /* Virtual address of the region to be mapped */ + uint32 mmuflags; /* MMU settings for the region (e.g., cache-able) */ + uint32 nsections; /* Number of mappings in the region */ +}; + +/************************************************************************************ + * Public Variables + ************************************************************************************/ + +extern uint32 _vector_start; /* Beginning of vector block */ +extern uint32 _vector_end; /* End+1 of vector block */ + +/************************************************************************************ + * Private Variables + ************************************************************************************/ + +static const struct section_mapping_s section_mapping[] = +{ + { DM320_PERIPHERALS_PSECTION, DM320_PERIPHERALS_VSECTION, + DM320_PERIPHERALS_MMUFLAGS, DM320_PERIPHERALS_NSECTIONS}, + { DM320_FLASH_PSECTION, DM320_FLASH_VSECTION, + DM320_FLASH_MMUFLAGS, DM320_FLASH_NSECTIONS}, + { DM320_CFI_PSECTION, DM320_CFI_VSECTION, + DM320_CFI_MMUFLAGS, DM320_CFI_NSECTIONS}, + { DM320_SSFDC_PSECTION, DM320_SSFDC_VSECTION, + DM320_SSFDC_MMUFLAGS, DM320_SSFDC_NSECTIONS}, + { DM320_CE1_PSECTION, DM320_CE1_VSECTION, + DM320_CE1_MMUFLAGS, DM320_CE1_NSECTIONS}, + { DM320_CE2_PSECTION, DM320_CE2_VSECTION, + DM320_CE2_MMUFLAGS, DM320_CE2_NSECTIONS}, + { DM320_VLYNQ_PSECTION, DM320_VLYNQ_VSECTION, + DM320_VLYNQ_MMUFLAGS, DM320_VLYNQ_NSECTIONS}, + { DM320_USBOTG_PSECTION, DM320_USBOTG_VSECTION, + DM320_USBOTG_MMUFLAGS, DM320_USBOTG_NSECTIONS} +}; +#define NMAPPINGS (sizeof(section_mapping) / sizeof(struct section_mapping_s)) + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: up_setlevel1entry + ************************************************************************************/ + +static inline void up_setlevel1entry(uint32 paddr, uint32 vaddr, uint32 mmuflags) +{ + uint32 *pgtable = (uint32*)PGTABLE_BASE_VADDR; + uint32 index = vaddr >> 20; + + /* Save the page table entry */ + + pgtable[index] = (paddr | mmuflags); +} + +/************************************************************************************ + * Name: up_setlevel2coarseentry + ************************************************************************************/ + +static inline void up_setlevel2coarseentry(uint32 ctabvaddr, uint32 paddr, + uint32 vaddr, uint32 mmuflags) +{ + uint32 *ctable = (uint32*)ctabvaddr; + uint32 index; + + /* The coarse table divides a 1Mb address space up into 256 entries, each + * corresponding to 4Kb of address space. The coarse page table index is + * related to the offset from the beginning of 1Mb region. + */ + + index = (vaddr & 0x000ff000) >> 12; + + /* Save the coarse table entry */ + + ctable[index] = (paddr | mmuflags); +} + +/************************************************************************************ + * Name: up_setupmappings + ************************************************************************************/ + +static void up_setupmappings(void) +{ + int i, j; + + for (i = 0; i < NMAPPINGS; i++) + { + uint32 sect_paddr = section_mapping[i].physbase; + uint32 sect_vaddr = section_mapping[i].virtbase; + uint32 mmuflags = section_mapping[i].mmuflags; + + for (j = 0; j < section_mapping[i].nsections; j++) + { + up_setlevel1entry(sect_paddr, sect_vaddr, mmuflags); + sect_paddr += SECTION_SIZE; + sect_vaddr += SECTION_SIZE; + } + } +} + +/************************************************************************************ + * Name: up_vectormapping + ************************************************************************************/ + +static void up_vectormapping(void) +{ + uint32 vector_paddr = DM320_IRAM_PADDR; + uint32 vector_vaddr = DM320_VECTOR_VADDR; + uint32 end_paddr = vector_paddr + DM320_IRAM_SIZE; + + /* We want to keep our interrupt vectors and interrupt-related logic in zero-wait + * state internal RAM (IRAM). The DM320 has 16Kb of IRAM positioned at physical + * address 0x0000:0000; we need to map this to 0xffff:0000. + */ + + while (vector_paddr < end_paddr) + { + up_setlevel2coarseentry(PGTABLE_COARSE_BASE_VADDR, + vector_paddr, + vector_vaddr, + MMU_L2_VECTORFLAGS); + vector_paddr += 4096; + vector_vaddr += 4096; + } + + /* Now set the level 1 descriptor to refer to the level 2 coarse page table. */ + + up_setlevel1entry(PGTABLE_COARSE_BASE_PADDR, + DM320_VECTOR_VCOARSE, + MMU_L1_VECTORFLAGS); +} + +/************************************************************************************ + * Name: up_copyvectorblock + ************************************************************************************/ + +static void up_copyvectorblock(void) +{ + uint32 *src = (uint32*)&_vector_start; + uint32 *end = (uint32*)&_vector_end; + uint32 *dest = (uint32*)VECTOR_BASE; + + while (src < end) + { + *dest++ = *src++; + } +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +void up_boot(void) +{ + /* __start provided the basic MMU mappings for SDRAM. Now provide mappings for all + * IO regions (Including the vector region). + */ + + up_setupmappings(); + + /* Provide a special mapping for the IRAM interrupt vector positioned in high + * memory. + */ + + up_vectormapping(); + + /* Setup up vector block. _vector_start and _vector_end are exported from + * up_vector.S + */ + + up_copyvectorblock(); +} diff --git a/nuttx/arch/arm/src/dm320/dm320_doirq.c b/nuttx/arch/arm/src/dm320/dm320_doirq.c new file mode 100644 index 000000000..66b55f75f --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_doirq.c @@ -0,0 +1,119 @@ +/******************************************************************************** + * dm320/dm320_doirq.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <nuttx/irq.h> +#include <nuttx/arch.h> +#include <assert.h> +#include <debug.h> +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" + +/******************************************************************************** + * Definitions + ********************************************************************************/ + +/******************************************************************************** + * Public Data + ********************************************************************************/ + +/******************************************************************************** + * Private Data + ********************************************************************************/ + +/******************************************************************************** + * Private Functions + ********************************************************************************/ + +/******************************************************************************** + * Public Funtions + ********************************************************************************/ + +void up_doirq(uint32* regs) +{ +#ifdef CONFIG_SUPPRESS_INTERRUPTS + lib_lowprintf("Unexpected IRQ\n"); + current_regs = regs; + PANIC(OSERR_ERREXCEPTION); +#else + /* Decode the interrupt. First, fetch the interrupt id register. */ + + uint16 irqentry = getreg16(DM320_INTC_IRQENTRY0); + + /* The irqentry value is an offset into a table. Zero means no interrupt. */ + + if (irqentry != 0) + { + /* If non-zero, then we can map the table offset into an IRQ number */ + + int irq = (irqentry >> 2) - 1; + + /* Verify that the resulting IRQ number is valie */ + + if ((unsigned)irq < NR_IRQS) + { + /* Mask and acknowledge the interrupt */ + + up_maskack_irq(irq); + + /* Current regs non-zero indicates that we are processing an interrupt; + * current_regs is also used to manage interrupt level context switches. + */ + + current_regs = regs; + + /* Deliver the IRQ */ + + irq_dispatch(irq, regs); + + /* Indicate that we are no long in an interrupt handler */ + + current_regs = NULL; + + /* Unmask the last interrupt (global interrupts are still + * disabled. + */ + + up_enable_irq(irq); + } + } +#endif +} diff --git a/nuttx/arch/arm/src/dm320/dm320_gio.h b/nuttx/arch/arm/src/dm320/dm320_gio.h new file mode 100644 index 000000000..9b9a206f3 --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_gio.h @@ -0,0 +1,175 @@ +/************************************************************************************ + * dm320/dm320_gio.h + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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. + * + ************************************************************************************/ + +#ifndef __DM320_DM320GIO_H +#define __DM320_DM320GIO_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#ifndef __ASSEMBLY__ +# include <sys/types.h> +#endif + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* General I/O Registers */ + +#define DM320_GIO_DIR0 (DM320_PERIPHERALS_VADDR + 0x0580) /* GIO Direction Register 0 */ +#define DM320_GIO_DIR1 (DM320_PERIPHERALS_VADDR + 0x0582) /* GIO Direction Register 1 */ +#define DM320_GIO_DIR2 (DM320_PERIPHERALS_VADDR + 0x0584) /* GIO Direction Register 2 */ +#define DM320_GIO_INV0 (DM320_PERIPHERALS_VADDR + 0x0586) /* GIO Inversion Register 0 */ +#define DM320_GIO_INV1 (DM320_PERIPHERALS_VADDR + 0x0588) /* GIO Inversion Register 1 */ +#define DM320_GIO_INV2 (DM320_PERIPHERALS_VADDR + 0x058A) /* GIO Inversion Register 2 */ +#define DM320_GIO_BITSET0 (DM320_PERIPHERALS_VADDR + 0x058C) /* GIO Bit Set Register 0 */ +#define DM320_GIO_BITSET1 (DM320_PERIPHERALS_VADDR + 0x058E) /* GIO Bit Set Register 1 */ +#define DM320_GIO_BITSET2 (DM320_PERIPHERALS_VADDR + 0x0590) /* GIO Bit Set Register 2 */ +#define DM320_GIO_BITCLR0 (DM320_PERIPHERALS_VADDR + 0x0592) /* GIO Bit Clear Register 0 */ +#define DM320_GIO_BITCLR1 (DM320_PERIPHERALS_VADDR + 0x0594) /* GIO Bit Clear Register 1 */ +#define DM320_GIO_BITCLR2 (DM320_PERIPHERALS_VADDR + 0x0596) /* GIO Bit Clear Register 2 */ +#define DM320_GIO_IRQPORT (DM320_PERIPHERALS_VADDR + 0x0598) /* GIO IRQ Port Setting Register */ +#define DM320_GIO_IRQEDGE (DM320_PERIPHERALS_VADDR + 0x059A) /* GIO IRQ Edge Setting Register */ +#define DM320_GIO_CHAT0 (DM320_PERIPHERALS_VADDR + 0x059C) /* GIO Chatter Setting Register 0 */ +#define DM320_GIO_CHAT1 (DM320_PERIPHERALS_VADDR + 0x059E) /* GIO Chatter Setting Register 1 */ +#define DM320_GIO_CHAT2 (DM320_PERIPHERALS_VADDR + 0x05A0) /* GIO Chatter Setting Register 2 */ +#define DM320_GIO_NCHAT (DM320_PERIPHERALS_VADDR + 0x05A2) /* GIO Chatter Value Register */ +#define DM320_GIO_FSEL0 (DM320_PERIPHERALS_VADDR + 0x05A4) /* GIO Function Select Register 0 */ +#define DM320_GIO_FSEL1 (DM320_PERIPHERALS_VADDR + 0x05A6) /* GIO Function Select Register 1 */ +#define DM320_GIO_FSEL2 (DM320_PERIPHERALS_VADDR + 0x05A8) /* GIO Function Select Register 2 */ +#define DM320_GIO_FSEL3 (DM320_PERIPHERALS_VADDR + 0x05AA) /* GIO Function Select Register 3 */ + +/* Macros for GIO access */ + +#define _GIO_READ_REG(pin, reg0, reg1, reg2, bval) \ + do { \ + register uint32 _reg; register int _pin; \ + if ((pin) < 16) { _reg = (reg0); _pin = (pin); } \ + else if ((pin) < 32) { _reg = (reg1); _pin = ((pin) - 16); } \ + else { _reg = (reg2); _pin = ((pin) - 32); } \ + bval = ((getreg16(_reg) & (1<<_pin)) != 0); \ + } + +#define _GIO_SET_REG(pin, reg0, reg1, reg2) \ + do { \ + register uint32 _reg; register int _pin; \ + if ((pin) < 16) { _reg = (reg0); _pin = (pin); } \ + else if ((pin) < 32) { _reg = (reg1); _pin = ((pin) - 16); } \ + else { _reg = (reg2); _pin = ((pin) - 32); } \ + putreg16((getreg16(_reg) | (1 << _pin)), _reg)); \ + } while (0) + +#define _GIO_CLEAR_REG(pin, reg0, reg1, reg2) \ + do { \ + register uint32 _reg; register int _pin; \ + if ((pin) < 16) { _reg = (reg0); _pin = (pin); } \ + else if ((pin) < 32) { _reg = (reg1); _pin = ((pin) - 16); } \ + else { _reg = (reg2); _pin = ((pin) - 32); } \ + putreg16((getreg16(_reg) & ~(1 << _pin)), _reg)); \ + } while (0) + +/* Select GIO input or output */ + +#define GIO_INPUT(pin) \ + _GIO_SET_REG((pin), DM320_GIO_DIR0, DM320_GIO_DIR1, DM320_GIO_DIR2) +#define GIO_OUTPUT(pin) \ + _GIO_CLEAR_REG((pin), DM320_GIO_DIR0, DM320_GIO_DIR1, DM320_GIO_DIR2) + +/* Select inverted or non-inverted GIO */ + +#define GIO_INVERTED(pin) \ + _GIO_SET_REG((pin), DM320_GIO_INV0, DM320_GIO_INV1, DM320_GIO_INV2) +#define GIO_NONINVERTED(pin) \ + _GIO_CLEAR_REG((pin), DM320_GIO_INV0, DM320_GIO_INV1, DM320_GIO_INV2) + +/* Set and clear outputs */ + +#define GIO_SET_OUTPUT(pin) \ + _GIO_SET_REG((pin), DM320_GIO_BITSET0, DM320_GIO_BITSET1, DM320_GIO_BITSET2) +#define GIO_CLEAR_OUTPUT(pin) \ + _GIO_SET_REG((pin), DM320_GIO_BITCLR0, DM320_GIO_BITCLR1, DM320_GIO_BITCLR2) + +/* Read input */ + +#define GIO_READ_INPUT(pin, bval) \ + _GIO_READ_REG((pin), DM320_GIO_BITSET0, DM320_GIO_BITSET1, DM320_GIO_BITSET2, (bval)) + +/* Configure GIO pins */ + +#define _GIO_SET_CONFIG(reg, sh, val) \ + putreg16(((getreg16(reg) & ~(3 << sh)) | (val << sh)), (reg)) + +#define GIO_CONFIGURE(pin, val) \ + do {\ + if ((pin) < 10) _GIO_SET_CONFIG(DM320_GIO_FSEL0, 0, (val)); \ + else if ((pin) < 17) _GIO_SET_CONFIG(DM320_GIO_FSEL0, 2*((pin)-9), (val)); \ + else if ((pin) < 25) _GIO_SET_CONFIG(DM320_GIO_FSEL1, 2*((pin)-17), (val)); \ + else if ((pin) < 33) _GIO_SET_CONFIG(DM320_GIO_FSEL2, 2*((pin)-25), (val)); \ + else _GIO_SET_CONFIG(DM320_GIO_FSEL3, 2*((pin)-33), (val)); \ + } + +/* Configure GIO interrupts (pins 1-15) */ + +#define GIO_INTERRUPT(pin) \ + if (pin < 16) putreg16((getreg16(DM320_GIO_IRQPORT) | (1<<(pin))), DM320_GIO_IRQPORT) +#define GIO_NONINTERRUPT(pin) \ + if (pin < 16) putreg16((getreg16(DM320_GIO_IRQPORT) & ~(1<<(pin))), DM320_GIO_IRQPORT) +#define GIO_FALLINGEDGE(pin) \ + if (pin < 16) { \ + putreg16((getreg16(DM320_GIO_IRQEDGE) & ~(1<<(pin))), DM320_GIO_IRQEDGE) \ + putreg16((getreg16(DM320_GIO_INV0) & ~(1<<(pin))), DM320_GIO_INV0); \ + } +#define GIO_RISINGEDGE(pin) \ + if (pin < 16) { \ + putreg16((getreg16(DM320_GIO_IRQEDGE) & ~(1<<(pin))), DM320_GIO_IRQEDGE); \ + putreg16((getreg16(DM320_GIO_INV0) | (1<<(pin))), DM320_GIO_INV0); \ + } +#define GIO_BOTHEDGES(pin) \ + if (pin < 16) { \ + putreg16((getreg16(DM320_GIO_IRQEDGE) | (1<<(pin))), DM320_GIO_IRQEDGE); \ + putreg16((getreg16(DM320_GIO_INV0) & ~(1<<(pin))), DM320_GIO_INV0); \ + } + +/************************************************************************************ + * Inline Functions + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#endif + +#endif /* __DM320_DM320_GIO_H */ diff --git a/nuttx/arch/arm/src/dm320/dm320_intc.h b/nuttx/arch/arm/src/dm320/dm320_intc.h new file mode 100644 index 000000000..9795718b2 --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_intc.h @@ -0,0 +1,105 @@ +/************************************************************************************ + * dm320/dm320_intc.h + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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. + * + ************************************************************************************/ + +#ifndef __DM320_DM320_INTC_H +#define __DM320_DM320_INTC_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#ifndef __ASSEMBLY__ +# include <sys/types.h> +#endif + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Interrupt Controller Registers */ + +#define DM320_INTC_FIQ0 (DM320_PERIPHERALS_VADDR + 0x0500) /* FIQ Interrupt Flag Register #0 */ +#define DM320_INTC_FIQ1 (DM320_PERIPHERALS_VADDR + 0x0502) /* FIQ Interrupt Flag Register #1 */ +#define DM320_INTC_FIQ2 (DM320_PERIPHERALS_VADDR + 0x0504) /* FIQ Interrupt Flag Register #2 */ +#define DM320_INTC_IRQ0 (DM320_PERIPHERALS_VADDR + 0x0508) /* IRQ Interrupt Flag Register #0 */ +#define DM320_INTC_IRQ1 (DM320_PERIPHERALS_VADDR + 0x050A) /* IRQ Interrupt Flag Register #1 */ +#define DM320_INTC_IRQ2 (DM320_PERIPHERALS_VADDR + 0x050C) /* IRQ Interrupt Flag Register #2 */ +#define DM320_INTC_FIQENTRY0 (DM320_PERIPHERALS_VADDR + 0x0510) /* FIQ Entry Address Register #0 */ +#define DM320_INTC_FIQENTRY1 (DM320_PERIPHERALS_VADDR + 0x0512) /* FIQ Entry Address Register #1 */ +#define DM320_INTC_FIQENTLCK0 (DM320_PERIPHERALS_VADDR + 0x0514) /* FIQ Lock Entry Address Register #1 */ +#define DM320_INTC_FIQENTLCK1 (DM320_PERIPHERALS_VADDR + 0x0516) /* FIQ Lock Entry Address Register #1 */ +#define DM320_INTC_IRQENTRY0 (DM320_PERIPHERALS_VADDR + 0x0518) /* IRQ Entry Address Register #0 */ +#define DM320_INTC_IRQENTRY1 (DM320_PERIPHERALS_VADDR + 0x051A) /* IRQ Entry Address Register #1 */ +#define DM320_INTC_IRQENTLCK0 (DM320_PERIPHERALS_VADDR + 0x051C) /* IRQ Lock Entry Address Register #1 */ +#define DM320_INTC_IRQENTLCK1 (DM320_PERIPHERALS_VADDR + 0x051E) /* Lock Entry Address Register #1 */ +#define DM320_INTC_FISEL0 (DM320_PERIPHERALS_VADDR + 0x0520) /* FIQ select register #0 */ +#define DM320_INTC_FISEL1 (DM320_PERIPHERALS_VADDR + 0x0522) /* FIQ select register #1 */ +#define DM320_INTC_FISEL2 (DM320_PERIPHERALS_VADDR + 0x0524) /* FIQ select register #2 */ +#define DM320_INTC_EINT0 (DM320_PERIPHERALS_VADDR + 0x0528) /* Interrupt Enable Register #0 */ +#define DM320_INTC_EINT1 (DM320_PERIPHERALS_VADDR + 0x052A) /* Interrupt Enable Register #1 */ +#define DM320_INTC_EINT2 (DM320_PERIPHERALS_VADDR + 0x052C) /* Interrupt Enable Register #2 */ +#define DM320_INTC_INTRAW (DM320_PERIPHERALS_VADDR + 0x0530) /* Interrupt Raw Register */ +#define DM320_INTC_EABASE0 (DM320_PERIPHERALS_VADDR + 0x0538) /* Entry Table Base Address Register #0 */ +#define DM320_INTC_EABASE1 (DM320_PERIPHERALS_VADDR + 0x053A) /* Entry Table Base Address Register #1 */ +#define DM320_INTC_INTPRI00 (DM320_PERIPHERALS_VADDR + 0x0540) /* Interrupt Priority Register #0 */ +#define DM320_INTC_INTPRI01 (DM320_PERIPHERALS_VADDR + 0x0542) /* Interrupt Priority Register #1 */ +#define DM320_INTC_INTPRI02 (DM320_PERIPHERALS_VADDR + 0x0544) /* Interrupt Priority Register #2 */ +#define DM320_INTC_INTPRI03 (DM320_PERIPHERALS_VADDR + 0x0546) /* Interrupt Priority Register #3 */ +#define DM320_INTC_INTPRI04 (DM320_PERIPHERALS_VADDR + 0x0548) /* Interrupt Priority Register #4 */ +#define DM320_INTC_INTPRI05 (DM320_PERIPHERALS_VADDR + 0x054A) /* Interrupt Priority Register #5 */ +#define DM320_INTC_INTPRI06 (DM320_PERIPHERALS_VADDR + 0x054C) /* Interrupt Priority Register #6 */ +#define DM320_INTC_INTPRI07 (DM320_PERIPHERALS_VADDR + 0x054E) /* Interrupt Priority Register #7 */ +#define DM320_INTC_INTPRI08 (DM320_PERIPHERALS_VADDR + 0x0550) /* Interrupt Priority Register #8 */ +#define DM320_INTC_INTPRI09 (DM320_PERIPHERALS_VADDR + 0x0552) /* Interrupt Priority Register #9 */ +#define DM320_INTC_INTPRI10 (DM320_PERIPHERALS_VADDR + 0x0554) /* Interrupt Priority Register #10 */ +#define DM320_INTC_INTPRI11 (DM320_PERIPHERALS_VADDR + 0x0556) /* Interrupt Priority Register #11 */ +#define DM320_INTC_INTPRI12 (DM320_PERIPHERALS_VADDR + 0x0558) /* Interrupt Priority Register #12 */ +#define DM320_INTC_INTPRI13 (DM320_PERIPHERALS_VADDR + 0x055A) /* Interrupt Priority Register #13 */ +#define DM320_INTC_INTPRI14 (DM320_PERIPHERALS_VADDR + 0x055C) /* Interrupt Priority Register #14 */ +#define DM320_INTC_INTPRI15 (DM320_PERIPHERALS_VADDR + 0x055E) /* Interrupt Priority Register #15 */ +#define DM320_INTC_INTPRI16 (DM320_PERIPHERALS_VADDR + 0x0560) /* Interrupt Priority Register #16 */ +#define DM320_INTC_INTPRI17 (DM320_PERIPHERALS_VADDR + 0x0562) /* Interrupt Priority Register #17 */ +#define DM320_INTC_INTPRI18 (DM320_PERIPHERALS_VADDR + 0x0564) /* Interrupt Priority Register #18 */ +#define DM320_INTC_INTPRI19 (DM320_PERIPHERALS_VADDR + 0x0566) /* Interrupt Priority Register #19 */ + +/************************************************************************************ + * Inline Functions + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#endif + +#endif /* __DM320_DM320_INTC_H */ diff --git a/nuttx/arch/arm/src/dm320/dm320_irq.c b/nuttx/arch/arm/src/dm320/dm320_irq.c new file mode 100644 index 000000000..692d06a98 --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_irq.c @@ -0,0 +1,245 @@ +/************************************************************ + * dm320/dm320_irq.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <nuttx/irq.h> +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Definitions + ************************************************************/ + +/************************************************************ + * Public Data + ************************************************************/ + +uint32 *current_regs; + +/************************************************************ + * Private Data + ************************************************************/ + +/* The value of _vflashstart is defined in ld.script. It + * could be hard-coded because we know that correct IRAM + * area is 0xffc00000. + */ + +extern int _svectors; /* Type does not matter */ + +/************************************************************ + * Private Functions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_irqinitialize + ************************************************************/ + +void up_irqinitialize(void) +{ + /* Clear, disable and configure all interrupts. */ + + putreg16(0, DM320_INTC_EINT0); /* Mask all IRQs/FIQs */ + putreg16(0, DM320_INTC_EINT1); + putreg16(0, DM320_INTC_EINT2); + + putreg16(0, DM320_INTC_INTRAW); /* No masked interrupts in status */ + + putreg16(0, DM320_INTC_FISEL0); /* No FIQs */ + putreg16(0, DM320_INTC_FISEL1); + putreg16(0, DM320_INTC_FISEL2); + + putreg16(0xffff, DM320_INTC_FIQ0); /* Clear all pending FIQs */ + putreg16(0xffff, DM320_INTC_FIQ1); + putreg16(0xffff, DM320_INTC_FIQ2); + + putreg16(0xffff, DM320_INTC_IRQ0); /* Clear all pending IRQs */ + putreg16(0xffff, DM320_INTC_IRQ1); + putreg16(0xffff, DM320_INTC_IRQ2); + + /* Make sure that the base addresses are zero and that + * the table increment is 4 bytes. + */ + + putreg16(0, DM320_INTC_EABASE0); + putreg16(0, DM320_INTC_EABASE1); + + /* currents_regs is non-NULL only while processing an interrupt */ + + current_regs = NULL; + + /* And finally, enable interrupts */ + +#ifndef CONFIG_SUPPRESS_INTERRUPTS + irqrestore(SVC_MODE | PSR_F_BIT); +#endif +} + +/************************************************************ + * Name: up_disable_irq + * + * Description: + * Disable the IRQ specified by 'irq' + * + ************************************************************/ + +void up_disable_irq(int irq) +{ + /* Disable the interrupt by clearing the corresponding bit in + * the IRQ enable register. + */ + + if (irq < 16) + { + /* IRQs0-15 are controlled by the IRQ0 enable register + * Clear the associated bit to disable the interrupt + */ + + putreg16((getreg16(DM320_INTC_EINT0) & ~(1 << irq)), DM320_INTC_EINT0); + } + else if (irq < 32) + { + /* IRQs16-31 are controlled by the IRQ1 enable register + * Clear the associated bit to disable the interrupt + */ + + putreg16((getreg16(DM320_INTC_EINT1) & ~(1 << (irq-16))), DM320_INTC_EINT1); + } + else + { + /* IRQs32- are controlled by the IRQ2 enable register + * Clear the associated bit to disable the interrupt + */ + + putreg16((getreg16(DM320_INTC_EINT2) & ~(1 << (irq-32))), DM320_INTC_EINT2); + } +} + +/************************************************************ + * Name: up_enable_irq + * + * Description: + * Enable the IRQ specified by 'irq' + * + ************************************************************/ + +void up_enable_irq(int irq) +{ + /* Enable the interrupt by setting the corresponding bit in + * the IRQ enable register. + */ + + if (irq < 16) + { + /* IRQs0-15 are controlled by the IRQ0 enable register + * Set the associated bit to enable the interrupt + */ + + putreg16((getreg16(DM320_INTC_EINT0) | (1 << irq)), DM320_INTC_EINT0); + } + else if (irq < 32) + { + /* IRQs16-31 are controlled by the IRQ1 enable register + * Set the associated bit to enable the interrupt + */ + + putreg16((getreg16(DM320_INTC_EINT1) | (1 << (irq-16))), DM320_INTC_EINT1); + } + else + { + /* IRQs32- are controlled by the IRQ2 enable register + * Set the associated bit to enable the interrupt + */ + + putreg16((getreg16(DM320_INTC_EINT2) | (1 << (irq-32))), DM320_INTC_EINT2); + } +} + +/************************************************************ + * Name: up_maskack_irq + * + * Description: + * Mask the IRQ and acknowledge it + * + ************************************************************/ + +void up_maskack_irq(int irq) +{ + /* Disable the interrupt by clearing the corresponding bit in + * the IRQ enable register. And acknowlege it by setting the + * corresponding bit in the IRQ status register. + */ + + if (irq < 16) + { + /* IRQs0-15 are controlled by the IRQ0 enable register + * Clear the associated enable bit to disable the interrupt + * Set the associated status bit to clear the interrupt + */ + + putreg16((getreg16(DM320_INTC_EINT0) & ~(1<< irq)), DM320_INTC_EINT0); + putreg16((1 << irq), DM320_INTC_IRQ0); + } + else if (irq < 32) + { + /* IRQs16-31 are controlled by the IRQ1 enable register + * Clear the associated enable bit to disable the interrupt + * Set the associated status bit to clear the interrupt + */ + + putreg16((getreg16(DM320_INTC_EINT1) & ~(1<< (irq-16))), DM320_INTC_EINT1); + putreg16((1 << (irq-16)), DM320_INTC_IRQ1); + } + else + { + /* IRQs32- are controlled by the IRQ2 enable register + * Clear the associated enable bit to disable the interrupt + * Set the associated status bit to clear the interrupt + */ + + putreg16((getreg16(DM320_INTC_EINT2) & ~(1<< (irq-32))), DM320_INTC_EINT2); + putreg16((1 << (irq-32)), DM320_INTC_IRQ2); + } +} diff --git a/nuttx/arch/arm/src/dm320/dm320_lowputc.S b/nuttx/arch/arm/src/dm320/dm320_lowputc.S new file mode 100644 index 000000000..c27918262 --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_lowputc.S @@ -0,0 +1,127 @@ +/************************************************************************** + * dm320/dm320_lowputc.S + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include "up_internal.h" +#include "up_arch.h" + +/************************************************************************** + * Private Definitions + **************************************************************************/ + +/************************************************************************** + * Private Types + **************************************************************************/ + +/************************************************************************** + * Private Function Prototypes + **************************************************************************/ + +/************************************************************************** + * Global Variables + **************************************************************************/ + +/************************************************************************** + * Private Variables + **************************************************************************/ + +/************************************************************************** + * Private Functions + **************************************************************************/ + +/************************************************************************** + * Public Functions + **************************************************************************/ + +/************************************************************************** + * Name: up_lowputc + **************************************************************************/ + +/* This assembly language version has the advantage that it can does not + * require a C stack and uses only r0-r1. Hence it can be used during + * early boot phases. + */ + + .text + .global up_lowputc + .type up_lowputc, function +up_lowputc: + /* On entry, r0 holds the character to be printed */ + +#ifdef CONFIG_UART0_SERIAL_CONSOLE + ldr r2, =DM320_UART0_REGISTER_BASE /* r2=UART0 base */ +#else + ldr r2, =DM320_UART1_REGISTER_BASE /* r2=UART1 base */ +#endif + + /* Poll the TX fifo trigger level bit of the UART_SSR + * register. When the bit is non-zero, the TX FIFO is no + * longer full + */ + +1: ldrh r1, [r2, #UART_SR] + tst r1, #UART_SR_TFTI + beq 1b + + /* Send the character by writing it into the UART_DTRR + * register. + */ + + strh r0, [r2, #UART_DTRR] + + /* Wait for the tranmsit regiser to be emptied. This is + * detemined when TX register empty bit of the SR is zero. + */ + +2: ldrh r1, [r2, #UART_SR] + tst r1, #UART_SR_TREF + bne 2b + + /* If the character that we just sent was a linefeed, + * then send a carriage return as well. + */ + + teq r0, #'\n' + moveq r0, #'\r' + beq 1b + + /* And return */ + + mov pc, lr + diff --git a/nuttx/arch/arm/src/dm320/dm320_memorymap.h b/nuttx/arch/arm/src/dm320/dm320_memorymap.h new file mode 100644 index 000000000..d53de2f96 --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_memorymap.h @@ -0,0 +1,238 @@ +/************************************************************************************ + * dm320/dm320_memorymap.h + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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. + * + ************************************************************************************/ + +#ifndef __DM320_MEMORYMAP_H +#define __DM320_MEMORYMAP_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#ifndef __ASSEMBLY__ +# include <sys/types.h> +#endif + +#include <nuttx/config.h> + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Mapped base of all registers *****************************************************/ + +/* DM320 Physical Memory Map, where: + * + * CW = cachable with write buffering + * -W = Write buffering only + * -- = Neither + * + * The DM320 only has a single control line for external peripherals. + * To support more than one peripheral, most hardware will use an + * external memory decode logic, so that physical memory regions is + * in the hardware specific files dm320-*.h + */ + +#if CONFIG_DRAM_START != 0x01000000 +# error "Invalid setting for CONFIG_DRAM_START +#endif + +/* Section/Region Name Phys Address Size TLB Enty CW */ +#define DM320_PERIPHERALS_PSECTION 0x00000000 /* 1Mb 1 section -- */ +#define DM320_IRAM_PADDR 0x00000000 /* 16Kb 1 large page CW */ +#define DM320_PERIPHERALS_PADDR 0x00030000 /* 4Kb 1 small pages -- */ +#define DM320_DSP_ONCHIP_RAM_PADDR 0x00040000 /* 128Kb 1 large page -- */ +#define DM320_AHB_PADDR 0x00060000 /* 4Kb 1 small page -- */ +#define DM320_COPRO_SUB_PADDR 0x00080000 /* 128Kb -- */ +#define DM320_FLASH_PSECTION 0x00100000 /* 16Mb many sections -- */ +#define DM320_EXT_MEM_PADDR 0x00100000 /* 16Mb flash -- */ +#define DM320_SDRAM_PSECTION 0x01000000 /* 496Mb many section -- */ +#define DM320_SDRAM_PADDR 0x01000000 /* 496Mb many sections CW */ +#define DM320_CFI_PSECTION 0x40000000 /* 16Mb 16 sections -- */ +#define DM320_CFI_PADDR 0x40000000 /* 16Mb 16 sections -- */ +#define DM320_SSFDC_PSECTION 0x48000000 /* 16Mb 16 sections -- */ +#define DM320_SSFDC_PADDR 0x48000000 /* 16Mb 16 sections -- */ +#define DM320_CE1_PSECTION 0x50000000 /* 16Mb 16 sections -- */ +#define DM320_CE1_PADDR 0x50000000 /* 16Mb 16 sections -- */ +#define DM320_CE2_PSECTION 0x60000000 /* 16Mb 16 sections -- */ +#define DM320_CE2_PADDR 0x60000000 /* 16Mb 16 sections -- */ +#define DM320_VLYNQ_PSECTION 0x70000000 /* 64MB 64 sections -- */ +#define DM320_VLYNQ_PADDR 0x70000000 /* 64MB 64 sections -- */ +#define DM320_USBOTG_PSECTION 0x80000000 /* 1Mb 1 section -- */ +#define DM320_USBOTG_PADDR 0x80000000 /* 1Kb 1 small page -- */ + +/* Sizes of sections/regions */ + +/* Section / Region Name Size */ +#define DM320_PERIPHERALS_NSECTIONS 1 /* 1Mb 1 section -- */ +#define DM320_IRAM_SIZE (16*1024) +#define DM320_PERIPHERALS_SIZE (4*1024) +#define DM320_DSP_ONCHIP_RAM_SIZE (128*1024) +#define DM320_AHB_SIZE (4*1024) +#define DM320_COPRO_SUB_SIZE (128*1024) +#define DM320_FLASH_NSECTIONS 16 /* 16Mb 16 sections -- */ +#define DM320_EXT_MEM_SIZE (16*1024*1024) +#define DM320_CFI_NSECTIONS 16 /* 16Mb 16 sections -- */ +#define DM320_CFI_SIZE (16*1024*1024) +#define DM320_SSFDC_NSECTIONS 16 /* 16Mb 16 sections -- */ +#define DM320_SSFDC_SIZE (16*1024*1024) +#define DM320_CE1_NSECTIONS 16 /* 16Mb 16 sections -- */ +#define DM320_CE1_SIZE (16*1024*1024) +#define DM320_CE2_NSECTIONS 16 /* 16Mb 16 sections -- */ +#define DM320_CE2_SIZE (16*1024*1024) +#define DM320_VLYNQ_NSECTIONS 64 /* 64MB 64 sections -- */ +#define DM320_VLYNQ_SIZE (64*1024*1024) +#define DM320_USBOTG_NSECTIONS 1 /* 1Mb 1 section -- */ +#define DM320_USBOTG_SIZE (1024) + +/* DM320 Virtual Memory Map */ + +#if CONFIG_DRAM_VSTART != 0x00000000 +# error "Invalid setting for CONFIG_DRAM_VSTART +#endif + +/* Section/Region Name Virt Address End Size CW */ +#define DM320_SDRAM_VSECTION 0x00000000 /* 0x1effffff 496Mb CW */ +#define DM320_SDRAM_VADDR 0x00000000 /* 0x1effffff 496Mb CW */ + /* 0x1f000000 0xdfffffff UNMAPPED */ +#define DM320_FLASH_VSECTION 0xc0000000 /* 0xc0ffffff 16Mb -- */ +#define DM320_EXT_MEM_VADDR 0xc0000000 /* 0xc0ffffff 16Mb -- */ +#define DM320_CFI_VSECTION 0xc4000000 /* 0xc4ffffff 16Mb -- */ +#define DM320_CFI_VADDR 0xc4000000 /* 0xc4ffffff 16Mb -- */ +#define DM320_SSFDC_VSECTION 0xc8000000 /* 0xc8ffffff 16Mb -- */ +#define DM320_SSFDC_VADDR 0xc8000000 /* 0xc8ffffff 16Mb -- */ +#define DM320_CE1_VSECTION 0xcc000000 /* 0xccffffff 16Mb -- */ +#define DM320_CE1_VADDR 0xcc000000 /* 0xccffffff 16Mb -- */ +#define DM320_CE2_VSECTION 0xd0000000 /* 0xd0ffffff 16Mb -- */ +#define DM320_CE2_VADDR 0xd0000000 /* 0xd0ffffff 16Mb -- */ +#define DM320_USBOTG_VSECTION 0xd4000000 /* 0xd40fffff 1Mb -- */ +#define DM320_USBOTG_VADDR 0xd4000000 /* 0xd40003ff 1Kb -- */ +#define DM320_VLYNQ_VSECTION 0xe0000000 /* 0xefffffff 64Mb -- */ +#define DM320_VLYNQ_VADDR 0xe0000000 /* 0xefffffff 64Mb -- */ +#define DM320_PERIPHERALS_VSECTION 0xf0000000 /* 0xf00fffff 1Mb -- */ +#define DM320_IRAM_VADDR 0xf0000000 /* 0xf0003fff 16Kb -- */ +#define DM320_PERIPHERALS_VADDR 0xf0030000 /* 0xf0030fff 4Kb -- */ +#define DM320_DSP_ONCHIP_RAM_VADDR 0xf0040000 /* 0xf005ffff 128Kb -- */ +#define DM320_AHB_VADDR 0xf0060000 /* 0xf0060fff 4Kb -- */ +#define DM320_COPRO_SUB_VADDR 0xf0080000 /* 0xf009ffff 128Kb -- */ + /* 0xf0100000 0xffefffff UNMAPPED */ +#define DM320_VECTOR_VCOARSE 0xfff00000 /* 0xffffffff 1Mb -- */ + /* 0xfff00000 0xfffeffff UNMAPPED */ +#define DM320_VECTOR_VADDR 0xffff0000 /* 0xffff3fff 16Kb -- */ + /* 0xffff4000 0xffffffff UNMAPPED */ + +/* The NuttX entry point starts at an offset from the virtual beginning of DRAM. + * This offset reserves space for the MMU page cache. + */ + +#define NUTTX_START_VADDR (DM320_SDRAM_VADDR+PGTABLE_SIZE) + +/* Section MMU Flags Flags CW */ +#define DM320_FLASH_MMUFLAGS MMU_IOFLAGS /* -- */ +#define DM320_CFI_MMUFLAGS MMU_IOFLAGS /* -- */ +#define DM320_SSFDC_MMUFLAGS MMU_IOFLAGS /* -- */ +#define DM320_CE1_MMUFLAGS MMU_IOFLAGS /* -- */ +#define DM320_CE2_MMUFLAGS MMU_IOFLAGS /* -- */ +#define DM320_VLYNQ_MMUFLAGS MMU_IOFLAGS /* -- */ +#define DM320_USBOTG_MMUFLAGS MMU_IOFLAGS /* -- */ +#define DM320_PERIPHERALS_MMUFLAGS MMU_IOFLAGS /* -- */ + +/* 16Kb of memory is reserved at the beginning of SDRAM to hold the + * page table for the virtual mappings. A portion of this table is + * not accessible in the virtual address space (for normal operation). + * We will reuse this memory for coarse page tables as follows: + */ + +#define PGTABLE_BASE_PADDR DM320_SDRAM_PADDR +#define PGTABLE_SDRAM_PADDR PGTABLE_BASE_PADDR +#define PGTABLE_COARSE_BASE_PADDR (PGTABLE_BASE_PADDR+0x00000800) +#define PGTABLE_COARSE_END_PADDR (PGTABLE_BASE_PADDR+0x00003000) +#define PTTABLE_PERIPHERALS_PADDR (PGTABLE_BASE_PADDR+0x00003000) +#define PGTABLE_END_PADDR (PGTABLE_BASE_PADDR+0x00004000) + +#define PGTABLE_BASE_VADDR DM320_SDRAM_VADDR +#define PGTABLE_SDRAM_VADDR PGTABLE_BASE_VADDR +#define PGTABLE_COARSE_BASE_VADDR (PGTABLE_BASE_VADDR+0x00000800) +#define PGTABLE_COARSE_END_VADDR (PGTABLE_BASE_VADDR+0x00003000) +#define PTTABLE_PERIPHERALS_VADDR (PGTABLE_BASE_VADDR+0x00003000) +#define PGTABLE_END_VADDR (PGTABLE_BASE_VADDR+0x00004000) + +#define PGTBALE_COARSE_TABLE_SIZE (4*256) +#define PGTABLE_COARSE_ALLOC (PGTABLE_COARSE_END_VADDR-PGTABLE_COARSE_BASE_VADDR) +#define PGTABLE_NCOARSE_TABLES (PGTABLE_COARSE_SIZE / PGTBALE_COARSE_TABLE_ALLOC) + +/* This is the base address of the interrupt vectors on the ARM926 */ + +#define VECTOR_BASE DM320_VECTOR_VADDR + +/* DM320 Peripheral Registers */ + +#define DM320_TIMER0_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0000) /* Timer 0 */ +#define DM320_TIMER1_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0080) /* Timer 1 */ +#define DM320_TIMER2_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0100) /* Timer 2 */ +#define DM320_TIMER3_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0180) /* Timer 3 */ +#define DM320_SERIAL0_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0200) /* Serial port 0 */ +#define DM320_SERIAL1_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0280) /* Serial port 1 */ +#define DM320_UART0_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0300) /* UART 0 */ +#define DM320_UART1_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0380) /* UART 1 */ +#define DM320_WDT_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0400) /* Watchdog timer */ +#define DM320_MMCSD_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0480) /* MMC/SD */ +#define DM320_INTC_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0500) /* Interrupt controller */ +#define DM320_GIO_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0580) /* GIO */ +#define DM320_DSPC_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0600) /* DSP controller */ +#define DM320_OSD_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0680) /* OSD */ +#define DM320_CCDC_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0700) /* CCD controller */ +#define DM320_VENC_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0800) /* Video encoder */ +#define DM320_CLKC_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0880) /* Clock controller */ +#define DM320_BUSC_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0900) /* Bus controller */ +#define DM320_SDRAMC_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0980) /* SDRAM controller */ +#define DM320_EMIF_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0A00) /* External memory interface */ +#define DM320_PREV_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0A80) /* Preview engine */ +#define DM320_AF_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0B80) /* Hardware 3A (AF/AE/AWB) */ +#define DM320_MSTICK_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0C80) /* Memory stick */ +#define DM320_I2C_REGISTER_BASE (DM320_PERIPHERALS_VADDR + 0x0D80) /* I2C */ +#define DM320_USB_REGISTER_BASE (DM320_USBOTG_VADDR + 0x0000) /* USB full speed OTG */ +#define DM320_USBDMA_REGISTER_BASE (DM320_USBOTG_VADDR + 0x0200) /* USB DMA */ +#define DM320_VLYNQ_REGISTER_BASE (DM320_AHB_VADDR + 0x0300) /* VLYNQ */ +#define DM320_AHBBUSC_REGISTER_BASE (DM320_AHB_VADDR + 0x0F00) /* AHBBUSC */ + +/************************************************************************************ + * Inline Functions + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#endif + +#endif /* __DM320_MEMORYMAP_H */ diff --git a/nuttx/arch/arm/src/dm320/dm320_restart.S b/nuttx/arch/arm/src/dm320/dm320_restart.S new file mode 100644 index 000000000..1ce9e373f --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_restart.S @@ -0,0 +1,136 @@ +/******************************************************************** + * dm320/dm320_restart.S + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include "up_internal.h" +#include "up_arch.h" + +/******************************************************************** + * Definitions + ********************************************************************/ + +/******************************************************************** + * Assembly Macros + ********************************************************************/ + +/* Since the page table is closely related to the NuttX base + * address, we can convert the page table base address to the + * base address of the section containing both. + */ + + .macro mksection, section, pgtable + bic \section, \pgtable, #0x000ff000 + .endm + +/************************************************************************** + * Name: up_restart + **************************************************************************/ + + .text + .globl up_restart + .type up_restart, %function +up_restart: + /* Make sure that we are in SVC mode with all IRQs disabled */ + + mov r0, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT) + msr cpsr_c, r0 + + /* Create identity mapping for first MB section to support + * this re-start logic executing out of the physical address + * space. + */ + + mksection r0, r4 /* r0=phys. base section */ + ldr r1, .LCmmuflags /* FLGS=MMU_MEMFLAGS */ + add r3, r1, r0 /* r3=flags + base */ + str r3, [r4, r0, lsr #18] /* identity mapping */ + + /* Jump into the physical address space */ + + ldr pc, .LCphysrestart + nop + nop + + /* We are now executing at our physical address, with the + * MMU disabled. + */ + +up_phyrestart: + + mov r0, #0 + mcr p15, 0, r0, c7, c7 /* Invalidate I,D caches */ + mcr p15, 0, r0, c7, c10, 4 /* Drain write buffer */ + mcr p15, 0, r0, c8, c7 /* Invalidate I,D TLBs */ + + /* Clear bits in control register (see start.h): Disable, + * MMU, Data cache, alignment traps, write buffer, Instruction + * cache, exceptions at 0xffff0000, round robin) + */ + + mrc p15, 0, r0, c1, c0 /* Get control register */ + bic r0, r0, #(CR_M|CR_C|CR_A|CR_W) + bic r0, r0, #(CR_S|CR_I|CR_V|CR_RR) + mcr p15, 0, r0, c1, c0, 0 /* Write control reg */ + + /* We know that the the bootloader entry point is at the + * beginning of flash. + */ +#if 1 + ldr pc, .LCbtldrentry /* Restart bootloader */ +#else + b __start /* Restart Nuttx */ +#endif + + .type .LCphysrestart, %object +.LCphysrestart: + .long (up_phyrestart - CONFIG_DRAM_VSTART - CONFIG_DRAM_START) +.LCbtldrentry: + .long DM320_EXT_MEM_PADDR + +/************************************************************************** + * PC_Relative Data + **************************************************************************/ + + .type .LCmmuflags, %object +.LCmmuflags: + .long MMU_MEMFLAGS + .size up_restart, .-up_restart + + .end + diff --git a/nuttx/arch/arm/src/dm320/dm320_serial.c b/nuttx/arch/arm/src/dm320/dm320_serial.c new file mode 100644 index 000000000..8051393cf --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_serial.c @@ -0,0 +1,725 @@ +/************************************************************ + * dm320/dm320_serial.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <unistd.h> +#include <semaphore.h> +#include <string.h> +#include <errno.h> +#include <debug.h> +#include <nuttx/irq.h> +#include <nuttx/arch.h> +#include <nuttx/serial.h> +#include <arch/serial.h> +#include "up_arch.h" +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Definitions + ************************************************************/ + +#define BASE_BAUD 115200 + +/************************************************************ + * Private Types + ************************************************************/ + +struct up_dev_s +{ + uint32 uartbase; /* Base address of UART + * registers */ + uint32 baud; /* Configured baud */ + uint16 msr; /* Saved MSR value */ + ubyte irq; /* IRQ associated with + * this UART */ + ubyte parity; /* 0=none, 1=odd, 2=even */ + ubyte bits; /* Number of bits (7 or 8) */ + boolean stopbits2; /* TRUE: Configure with 2 + * stop bits instead of 1 */ +}; + +/************************************************************ + * Private Function Prototypes + ************************************************************/ + +static int up_setup(struct uart_dev_s *dev); +static void up_shutdown(struct uart_dev_s *dev); +static int up_interrupt(int irq, void *context); +static int up_ioctl(struct file *filep, int cmd, unsigned long arg); +static int up_receive(struct uart_dev_s *dev, uint32 *status); +static void up_rxint(struct uart_dev_s *dev, boolean enable); +static boolean up_rxfifonotempty(struct uart_dev_s *dev); +static void up_send(struct uart_dev_s *dev, int ch); +static void up_txint(struct uart_dev_s *dev, boolean enable); +static boolean up_txfifonotfull(struct uart_dev_s *dev); +static boolean up_txfifoempty(struct uart_dev_s *dev); + +/************************************************************ + * Private Variables + ************************************************************/ + +struct uart_ops_s g_uart_ops = +{ + .setup = up_setup, + .shutdown = up_shutdown, + .handler = up_interrupt, + .ioctl = up_ioctl, + .receive = up_receive, + .rxint = up_rxint, + .rxfifonotempty = up_rxfifonotempty, + .send = up_send, + .txint = up_txint, + .txfifonotfull = up_txfifonotfull, + .txfifoempty = up_txfifoempty, +}; + +/* I/O buffers */ + +static char g_uart0rxbuffer[CONFIG_UART0_RXBUFSIZE]; +static char g_uart0txbuffer[CONFIG_UART0_TXBUFSIZE]; +static char g_uart1rxbuffer[CONFIG_UART1_RXBUFSIZE]; +static char g_uart1txbuffer[CONFIG_UART1_TXBUFSIZE]; + +/* This describes the state of the DM320 uart0 port. */ + +static struct up_dev_s g_uart0priv = +{ + .uartbase = DM320_UART0_REGISTER_BASE, + .baud = CONFIG_UART0_BAUD, + .parity = CONFIG_UART0_PARITY, + .bits = CONFIG_UART0_BITS, + .stopbits2 = CONFIG_UART0_2STOP, +}; + +static uart_dev_t g_uart0port = +{ + .irq = DM320_IRQ_UART0, + .recv = + { + .size = CONFIG_UART0_RXBUFSIZE, + .buffer = g_uart0rxbuffer, + }, + .xmit = + { + .size = CONFIG_UART0_TXBUFSIZE, + .buffer = g_uart0txbuffer, + }, + .ops = &g_uart_ops, + .priv = &g_uart0priv, +}; + +/* This describes the state of the DM320 uart1 port. */ + +static struct up_dev_s g_uart1priv = +{ + .uartbase = DM320_UART1_REGISTER_BASE, + .baud = CONFIG_UART1_BAUD, + .parity = CONFIG_UART1_PARITY, + .bits = CONFIG_UART1_BITS, + .stopbits2 = CONFIG_UART1_2STOP, +}; + +static uart_dev_t g_uart1port = +{ + .irq = DM320_IRQ_UART1, + .recv = + { + .size = CONFIG_UART1_RXBUFSIZE, + .buffer = g_uart1rxbuffer, + }, + .xmit = + { + .size = CONFIG_UART1_TXBUFSIZE, + .buffer = g_uart1txbuffer, + }, + .ops = &g_uart_ops, + .priv = &g_uart1priv, +}; + +/* Now, which one with be tty0/console and which tty1? */ + +#ifdef CONFIG_SERIAL_IRDA_CONSOLE +# define CONSOLE_DEV g_uart1port +# define TTYS0_DEV g_uart1port +# define TTYS1_DEV g_uart0port +#else +# define CONSOLE_DEV g_uart0port +# define TTYS0_DEV g_uart0port +# define TTYS1_DEV g_uart1port +#endif + +/************************************************************ + * Private Functions + ************************************************************/ + +/************************************************************ + * Name: up_serialin + ************************************************************/ + +static inline uint16 up_serialin(struct up_dev_s *priv, uint32 offset) +{ + return getreg16(priv->uartbase + offset); +} + +/************************************************************ + * Name: up_serialout + ************************************************************/ + +static inline void up_serialout(struct up_dev_s *priv, uint32 offset, uint16 value) +{ + putreg16(value, priv->uartbase + offset); +} + +/************************************************************ + * Name: up_disableuartint + ************************************************************/ + +static inline void up_disableuartint(struct up_dev_s *priv, uint16 *msr) +{ + if (msr) + { + *msr = priv->msr & UART_MSR_ALLIE; + } + + priv->msr &= ~UART_MSR_ALLIE; + up_serialout(priv, UART_MSR, priv->msr); +} + +/************************************************************ + * Name: up_restoreuartint + ************************************************************/ + +static inline void up_restoreuartint(struct up_dev_s *priv, uint16 msr) +{ + priv->msr |= msr & UART_MSR_ALLIE; + up_serialout(priv, UART_MSR, priv->msr); +} + +/************************************************************ + * Name: up_waittxfifonotfull + ************************************************************/ + +static inline void up_waittxfifonotfull(struct up_dev_s *priv) +{ + int tmp; + + for (tmp = 1000 ; tmp > 0 ; tmp--) + { + if ((up_serialin(priv, UART_SR) & UART_SR_TFTI) != 0) + { + break; + } + } +} + +/************************************************************ + * Name: up_enablebreaks + ************************************************************/ + +static inline void up_enablebreaks(struct up_dev_s *priv, boolean enable) +{ + uint16 lcr = up_serialin(priv, UART_LCR); + if (enable) + { + lcr |= UART_LCR_BOC; + } + else + { + lcr &= ~UART_LCR_BOC; + } + up_serialout(priv, UART_LCR, lcr); +} + +/************************************************************ + * Name: up_setup + * + * Description: + * Configure the UART baud, bits, parity, fifos, etc. This + * method is called the first time that the serial port is + * opened. + * + ************************************************************/ + +static int up_setup(struct uart_dev_s *dev) +{ +#ifdef CONFIG_SUPPRESS_UART_CONFIG + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + uint16 brsr; + + /* Clear fifos */ + + up_serialout(priv, UART_DM320_RFCR, 0x8000); + up_serialout(priv, UART_DM320_TFCR, 0x8000); + + /* Set rx and tx triggers */ + + up_serialout(priv, UART_DM320_RFCR, UART_RFCR_RTL_1); + up_serialout(priv, UART_DM320_TFCR, UART_TFCR_TTL_16); + + /* Set up the MSR */ + + priv->msr = up_serialin(priv, UART_MSR); + if (priv->bits == 7) + { + priv->msr |= UART_DATABIT_7; + } + else + { + priv->msr &= ~UART_MSR_CLS; + } + + if (priv->stopbits2) + { + priv->msr |= UART_STOPBIT_2; + } + else + { + priv->msr &= ~UART_MSR_SBLS; + } + + if (priv->parity == 1) + { + priv->msr |= UART_ODDPARITY; + } + else if (priv->parity == 2) + { + priv->msr |= UART_EVENPARITY; + } + else + { + priv->msr &= ~(UART_MSR_PSB|UART_MSR_PEB); + } + + /* Set up the BRSR */ + + switch (dev->baud) + { + case 2400: + brsr = UART_BAUD_2400; + break; + case 4800: + brsr = UART_BAUD_4800; + break; + default: + case 9600: + brsr = UART_BAUD_9600; + break; + case 14400: + brsr = UART_BAUD_14400; + break; + case 19200: + brsr = UART_BAUD_19200; + break; + case 28800: + brsr = UART_BAUD_28800; + break; + case 3840: + brsr = UART_BAUD_38400; + break; + case 57600: + brsr = UART_BAUD_57600; + break; + case 115200: + brsr = UART_BAUD_115200; + break; + case 230400: + brsr = UART_BAUD_230400; + break; + case 460800: + brsr = UART_BAUD_460800; + break; + case 921600: + brsr = UART_BAUD_921600; + break; + } + + /* Setup the new UART configuration */ + + up_serialout(priv,UART_MSR, priv->msr); + up_serialout(priv, UART_DM320_BRSR, brsr); + up_enablebreaks(priv, FALSE); +#endif + return OK; +} + +/************************************************************ + * Name: up_shutdown + * + * Description: + * Disable the UART. This method is called when the serial + * port is closed + * + ************************************************************/ + +static void up_shutdown(struct uart_dev_s *dev) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + up_disableuartint(priv, NULL); +} + +/************************************************************ + * Name: up_interrupt + * + * Description: + * This is the UART interrupt handler. It will be invoked + * when an interrupt received on the 'irq' It should call + * uart_transmitchars or uart_receivechar to perform the + * appropriate data transfers. The interrupt handling logic\ + * must be able to map the 'irq' number into the approprite + * uart_dev_s structure in order to call these functions. + * + ************************************************************/ + +static int up_interrupt(int irq, void *context) +{ + struct uart_dev_s *dev = NULL; + struct up_dev_s *priv; + uint16 status; + int passes = 0; + + if (g_uart1port.irq == irq) + { + dev = &g_uart1port; + } + else if (g_uart0port.irq == irq) + { + dev = &g_uart0port; + } + else + { + PANIC(OSERR_INTERNAL); + } + priv = (struct up_dev_s*)dev->priv; + + /* Loop until there are no characters to be transferred or, + * until we have been looping for a long time. + */ + + for(;;) + { + /* Get the current UART status and check for loop + * termination conditions + */ + + status = up_serialin(priv, UART_SR); + status &= (UART_SR_RFTI | UART_SR_TFTI); + + if (status == 0 || passes > 256) + { + return OK; + } + + /* Handline incoming, receive bytes */ + + if (status & UART_SR_RFTI) + { + uart_recvchars(dev); + } + + /* Handle outgoing, transmit bytes */ + + if (status & UART_SR_TFTI) + { + uart_xmitchars(dev); + } + + /* Keep track of how many times we do this in case there + * is some hardware failure condition. + */ + + passes++; + } +} + +/************************************************************ + * Name: up_ioctl + * + * Description: + * All ioctl calls will be routed through this method + * + ************************************************************/ + +static int up_ioctl(struct file *filep, int cmd, unsigned long arg) +{ + struct inode *inode = filep->f_inode; + struct uart_dev_s *dev = inode->i_private; + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + int ret = OK; + + switch (cmd) + { + case TIOCSERGSTRUCT: + { + struct up_dev_s *user = (struct up_dev_s*)arg; + if (!user) + { + *get_errno_ptr() = EINVAL; + ret = ERROR; + } + else + { + memcpy(user, dev, sizeof(struct up_dev_s)); + } + } + break; + + case TIOCSBRK: /* BSD compatibility: Turn break on, unconditionally */ + { + irqstate_t flags = irqsave(); + up_enablebreaks(priv, TRUE); + irqrestore(flags); + } + break; + + case TIOCCBRK: /* BSD compatibility: Turn break off, unconditionally */ + { + irqstate_t flags; + flags = irqsave(); + up_enablebreaks(priv, FALSE); + irqrestore(flags); + } + break; + + default: + *get_errno_ptr() = ENOTTY; + ret = ERROR; + break; + } + + return ret; +} + +/************************************************************ + * Name: up_receive + * + * Description: + * Called (usually) from the interrupt level to receive one + * character from the UART. Error bits associated with the + * receipt are provided in the the return 'status'. + * + ************************************************************/ + +static int up_receive(struct uart_dev_s *dev, uint32 *status) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + uint16 dtrr; + + dtrr = up_serialin(priv, UART_DTRR); + *status = dtrr; + return dtrr & UART_DTRR_DTR_MASK; +} + +/************************************************************ + * Name: up_rxint + * + * Description: + * Call to enable or disable RX interrupts + * + ************************************************************/ + +static void up_rxint(struct uart_dev_s *dev, boolean enable) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + if (enable) + { +#ifndef CONFIG_SUPPRESS_SERIAL_INTS + priv->msr |= UART_MSR_RFTIE; +#endif + } + else + { + priv->msr &= ~UART_MSR_RFTIE; + } + up_serialout(priv, UART_MSR, priv->msr); +} + +/************************************************************ + * Name: up_rxfifonotempty + * + * Description: + * Return TRUE if the receive fifo is not empty + * + ************************************************************/ + +static boolean up_rxfifonotempty(struct uart_dev_s *dev) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + return ((up_serialin(priv, UART_SR) & UART_SR_RFNEF) != 0); +} + +/************************************************************ + * Name: up_send + * + * Description: + * This method will send one byte on the UART + * + ************************************************************/ + +static void up_send(struct uart_dev_s *dev, int ch) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + up_serialout(priv, UART_DTRR, (uint16)ch); +} + +/************************************************************ + * Name: up_txint + * + * Description: + * Call to enable or disable TX interrupts + * + ************************************************************/ + +static void up_txint(struct uart_dev_s *dev, boolean enable) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + if (enable) + { +#ifndef CONFIG_SUPPRESS_SERIAL_INTS + priv->msr |= UART_MSR_TFTIE; +#endif + } + else + { + priv->msr &= ~UART_MSR_TFTIE; + } + up_serialout(priv, UART_MSR, priv->msr); +} + +/************************************************************ + * Name: up_txfifonotfull + * + * Description: + * Return TRUE if the tranmsit fifo is not full + * + ************************************************************/ + +static boolean up_txfifonotfull(struct uart_dev_s *dev) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + return ((up_serialin(priv, UART_SR) & UART_SR_TFTI) != 0); +} + +/************************************************************ + * Name: up_txfifoempty + * + * Description: + * Return TRUE if the transmit fifo is empty + * + ************************************************************/ + +static boolean up_txfifoempty(struct uart_dev_s *dev) +{ + struct up_dev_s *priv = (struct up_dev_s*)dev->priv; + return ((up_serialin(priv, UART_SR) & UART_SR_TREF) == 0); +} + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_serialinit + * + * Description: + * Performs the low level UART initialization early in + * debug so that the serial console will be available + * during bootup. This must be called before up_serialinit. + * + ************************************************************/ + +void up_earlyserialinit(void) +{ + up_disableuartint(TTYS0_DEV.priv, NULL); + up_disableuartint(TTYS1_DEV.priv, NULL); + + CONSOLE_DEV.isconsole = TRUE; + up_setup(&CONSOLE_DEV); +} + +/************************************************************ + * Name: up_serialinit + * + * Description: + * Register serial console and serial ports. This assumes + * that up_earlyserialinit was called previously. + * + ************************************************************/ + +void up_serialinit(void) +{ + (void)uart_register("/dev/console", &CONSOLE_DEV); + (void)uart_register("/dev/ttyS0", &TTYS0_DEV); + (void)uart_register("/dev/ttyS1", &TTYS1_DEV); +} + +/************************************************************ + * Name: up_putc + * + * Description: + * Provide priority, low-level access to support OS debug + * writes + * + ************************************************************/ + +int up_putc(int ch) +{ + struct up_dev_s *priv = (struct up_dev_s*)CONSOLE_DEV.priv; + uint16 ier; + + up_disableuartint(priv, &ier); + up_waittxfifonotfull(priv); + up_serialout(priv, UART_DTRR, (uint16)ch); + + /* Check for LF */ + + if (ch == '\n') + { + /* Add CR */ + + up_waittxfifonotfull(priv); + up_serialout(priv, UART_DTRR, '\r'); + } + + up_waittxfifonotfull(priv); + up_restoreuartint(priv, ier); + return ch; +} + diff --git a/nuttx/arch/arm/src/dm320/dm320_timer.h b/nuttx/arch/arm/src/dm320/dm320_timer.h new file mode 100644 index 000000000..6c76f4824 --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_timer.h @@ -0,0 +1,112 @@ +/************************************************************************************ + * dm320/dm320_timer.h + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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. + * + ************************************************************************************/ + +#ifndef __DM320_TIMER_H +#define __DM320_TIMER_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#ifndef __ASSEMBLY__ +# include <sys/types.h> +#endif + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Timer Registers */ + +#define DM320_TIMER0_TMMD (DM320_PERIPHERALS_VADDR + 0x0000) /* Timer 0 Mode */ +#define DM320_TIMER0_TMPRSCL (DM320_PERIPHERALS_VADDR + 0x0004) /* Timer 0 Prescalar */ +#define DM320_TIMER0_TMDIV (DM320_PERIPHERALS_VADDR + 0x0006) /* Timer 0 Divisor (count) */ +#define DM320_TIMER0_TMTRG (DM320_PERIPHERALS_VADDR + 0x0008) /* Timer 0 One-Shot Trigger */ +#define DM320_TIMER0_TMCNT (DM320_PERIPHERALS_VADDR + 0x000A) /* Timer 0 Count */ + +#define DM320_TIMER1_TMMD (DM320_PERIPHERALS_VADDR + 0x0080) /* Timer 1 Mode */ +#define DM320_TIMER1_TMPRSCL (DM320_PERIPHERALS_VADDR + 0x0084) /* Timer 1 Prescalar */ +#define DM320_TIMER1_TMDIV (DM320_PERIPHERALS_VADDR + 0x0086) /* Timer 1 Divisor (count) */ +#define DM320_TIMER1_TMTRG (DM320_PERIPHERALS_VADDR + 0x0088) /* Timer 1 One-Shot Trigger */ +#define DM320_TIMER1_TMCNT (DM320_PERIPHERALS_VADDR + 0x008A) /* Timer 1 Count */ + +#define DM320_TIMER2_TMMD (DM320_PERIPHERALS_VADDR + 0x0100) /* Timer 2 Mode */ +#define DM320_TIMER2_TMPRSCL (DM320_PERIPHERALS_VADDR + 0x0104) /* Timer 2 Prescalar */ +#define DM320_TIMER2_TMDIV (DM320_PERIPHERALS_VADDR + 0x0106) /* Timer 2 Divisor (count) */ +#define DM320_TIMER2_TMTRG (DM320_PERIPHERALS_VADDR + 0x0108) /* Timer 2 One-Shot Trigger */ +#define DM320_TIMER2_TMCNT (DM320_PERIPHERALS_VADDR + 0x010A) /* Timer 2 Count */ + +#define DM320_TIMER3_TMMD (DM320_PERIPHERALS_VADDR + 0x0180) /* Timer 2 Mode */ +#define DM320_TIMER3_TMPRSCL (DM320_PERIPHERALS_VADDR + 0x0184) /* Timer 2 Prescalar */ +#define DM320_TIMER3_TMDIV (DM320_PERIPHERALS_VADDR + 0x0186) /* Timer 2 Divisor (count) */ +#define DM320_TIMER3_TMTRG (DM320_PERIPHERALS_VADDR + 0x0188) /* Timer 2 One-Shot Trigger */ +#define DM320_TIMER3_TMCNT (DM320_PERIPHERALS_VADDR + 0x018A) /* Timer 2 Count */ + +/* Timer 0,1,2,3 Mode Register Bits: */ + +#define DM320_TMR_MODE_TEST_MASK 0x00fc /* Bits 7:2=Test */ +#define DM320_TMR_MODE_MODE_MASK 0x0003 /* Bits 1:0=timer mode */ + +# define DM320_TMR_MODE_STOP 0x0000 /* Stop Timer */ +# define DM320_TMR_MODE_ONESHOT 0x0001 /* Start one-shot timer */ +# define DM320_TMR_MODE_FREERUN 0x0002 /* Start free-running timer */ + +/* Timer 0,1,2,3 Clock Select Register Bits: */ + +#define DM320_TMR_PRSCL_MASK 0x03ff /* Bits 0:9=Timer prescale value */ + +/* Timer 0,1,2,3 Clock Divisor (Count) Register Bits: */ + +#define DM320_TMR_DIV_MASK 0xffff /* Bits 0:15=Timer divisor value */ + +/* Timer 0,1,2,3 Timer One-Short Trigger Register Bits: */ + +#define DM320_TMR_TMTRG_MASK 0x0001 /* Bit 0=One short trigger */ + +# define DM320_TMR_TMTRG_START 0x0001 /* 1 starts one shot timer */ + +/* Timer 0,1,2,3 Timer Counter Register Bits: */ + +#define DM320_TMR_COUNT_MASK 0xffff /* Bits 0:15=Current counter value */ + +/************************************************************************************ + * Inline Functions + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#endif + +#endif /* __DM320_TIMER_H */ diff --git a/nuttx/arch/arm/src/dm320/dm320_timerisr.c b/nuttx/arch/arm/src/dm320/dm320_timerisr.c new file mode 100644 index 000000000..603a66c91 --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_timerisr.c @@ -0,0 +1,153 @@ +/************************************************************ + * dm320/dm320_timerisr.c + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <sys/types.h> +#include <debug.h> +#include <nuttx/arch.h> +#include "clock_internal.h" +#include "up_internal.h" +#include "up_arch.h" + +/************************************************************ + * Definitions + ************************************************************/ + +/* DM320 Timers + * + * Each of the general-purpose timers can run in one of two modes: one- + * shot mode and free-run mode. In one-shot mode, an interrupt only + * occurs once and then the timer must be explicitly reset to begin the + * timing operation again. In free-run mode, when the timer generates an + * interrupt, the timer counter is automatically reloaded to start the count + * operation again. Use the bit field MODE in TMMDx to configure the + * timer for one-shot more or free-run mode. The bit field MODE in TMMDx + * also allows you to stop the timer. + * + * Either the ARM clock divided by 2 (CLK_ARM/2) or an external clock + * connected to the M27XI pin can be selected as the clock source of the + * timer. + * + * The actual clock frequency used in the timer count operation is the input + * clock divided by: 1 plus the value set in the bit field PRSCL of the + * register TMPRSCLx (10 bits). The timer expires when it reaches the + * value set in the bit field DIV of the register TMDIVx (16 bits) plus 1. + * PRSCL+1 is the source clock frequency divide factor and DIV+1 is the + * timer count value. The frequency of a timer interrupt is given by the + * following equation: + * + * Interrupt Frequency = (Source Clock Frequency) / (PRSCL+1) / (DIV+1) + */ + +/* System Timer + * + * Timer0 is dedicated as the system timer. The rate of system timer + * interrupts is assumed to to 10MS per tick / 100Hz. The following + * register settings are used for timer 0 + * + * System clock formula: + * Interrupt Frequency = (Source Clock Frequency) / (PRSCL+1) / (DIV+1) + * Source Clock Frequency = 27MHz (PLL clock) + * DIV = 26,999 (Yields 1Khz timer clock) + * PRSCL = 9 (Produces 100Hz interrupts) + */ + +#define DM320_TMR0_MODE DM320_TMR_MODE_FREERUN /* Free running */ +#define DM320_TMR0_DIV 26999 /* (see above) */ +#define DM320_TMR0_PRSCL 9 /* (see above) */ + +/************************************************************ + * Private Types + ************************************************************/ + +/************************************************************ + * Private Function Prototypes + ************************************************************/ + +/************************************************************ + * Global Functions + ************************************************************/ + +/************************************************************ + * Function: up_timerisr + * + * Description: + * The timer ISR will perform a variety of services for + * various portions of the systems. + * + ************************************************************/ + +int up_timerisr(int irq, uint32 *regs) +{ + /* Process timer interrupt */ + + sched_process_timer(); + return 0; +} + +/************************************************************ + * Function: up_timerinit + * + * Description: + * This function is called during start-up to initialize + * the timer interrupt. + * + ************************************************************/ + +void up_timerinit(void) +{ + up_disable_irq(DM320_IRQ_SYSTIMER); + + /* Start timer0 running so that an interrupt is generated at + * the rate MSEC_PER_TICK. + */ + + putreg16(DM320_TMR0_PRSCL, DM320_TIMER0_TMPRSCL); /* Timer 0 Prescalar */ + putreg16(DM320_TMR0_DIV, DM320_TIMER0_TMDIV); /* Timer 0 Divisor (count) */ + + /* Start the timer */ + + putreg16(DM320_TMR0_MODE, DM320_TIMER0_TMMD); /* Timer 0 Mode */ + + /* Attach and enable the timer interrupt */ + + irq_attach(DM320_IRQ_SYSTIMER, (xcpt_t)up_timerisr); + up_enable_irq(DM320_IRQ_SYSTIMER); +} + diff --git a/nuttx/arch/arm/src/dm320/dm320_uart.h b/nuttx/arch/arm/src/dm320/dm320_uart.h new file mode 100644 index 000000000..8938018a7 --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_uart.h @@ -0,0 +1,172 @@ +/************************************************************************************ + * dm320/dm320_uart.h + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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. + * + ************************************************************************************/ + +#ifndef __DM320_UART_H +#define __DM320_UART_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#ifndef __ASSEMBLY__ +# include <sys/types.h> +#endif + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* UART definitions *****************************************************************/ + +/* UART Registers (offsets from the register base) */ + +#define UART_DTRR 0 /* Data Transmission/Reception Register */ +#define UART_BRSR 2 /* Bit Rate Set Register */ +#define UART_MSR 4 /* Mode Set Register */ +#define UART_RFCR 6 /* Reception FIFO Control Register */ +#define UART_TFCR 8 /* Transmission FIFO Control Register */ +#define UART_LCR 10 /* Line Control Register */ +#define UART_SR 12 /* Status Register */ + +/* UART DTRR register bit definitions */ + +#define UART_DTRR_RVF 0x1000 /* Receive word valid flag */ +#define UART_DTRR_BF 0x0800 /* Break flag */ +#define UART_DTRR_FE 0x0400 /* Framing error */ +#define UART_DTRR_ORF 0x0200 /* Overrun flag */ +#define UART_DTRR_PEF 0x0100 /* Parity error */ +#define UART_DTRR_DTR_MASK 0x00ff /* Data transmit/receive */ + +/* UART BRSR register bit definitions */ +/* The UART clock is half of the ARM clock */ + +#define UART_CLK (DM320_ARM_CLOCK / 2) + +/* And baud rate = UART_CLK / 16 / (VALUE+1) */ + +#define UART_BAUD_2400 ((uint16)(((UART_CLK / 16) / 2400 ) - 1)) +#define UART_BAUD_4800 ((uint16)(((UART_CLK / 16) / 4800 ) - 1)) +#define UART_BAUD_9600 ((uint16)(((UART_CLK / 16) / 9600 ) - 1)) +#define UART_BAUD_14400 ((uint16)(((UART_CLK / 16) / 14400 ) - 1)) +#define UART_BAUD_19200 ((uint16)(((UART_CLK / 16) / 19200 ) - 1)) +#define UART_BAUD_28800 ((uint16)(((UART_CLK / 16) / 28800 ) - 1)) +#define UART_BAUD_38400 ((uint16)(((UART_CLK / 16) / 38400 ) - 1)) +#define UART_BAUD_57600 ((uint16)(((UART_CLK / 16) / 57600 ) - 1)) +#define UART_BAUD_115200 ((uint16)(((UART_CLK / 16) / 115200) - 1)) +#define UART_BAUD_230400 ((uint16)(((UART_CLK / 16) / 230400) - 1)) +#define UART_BAUD_460800 ((uint16)(((UART_CLK / 16) / 460800) - 1)) +#define UART_BAUD_921600 ((uint16)(((UART_CLK / 16) / 921600) - 1)) + +/* UART MSR register bit definitions */ + +#define UART_MSR_MODE_BITS 0x001f /* Aata length, stop, & parity */ +#define UART_MSR_CLS 0x0001 /* Char length (1=7bit, 0=8bit) */ +#define UART_DATABIT_7 0x0001 /* Data bit = 7bit */ +#define UART_DATABIT_8 0x0000 /* Data bit = 8bit */ +#define UART_MSR_SBLS 0x0004 /* Stop bit length selection */ +#define UART_STOPBIT_1 0x0000 /* Stop bit = 1bit */ +#define UART_STOPBIT_2 0x0004 /* Stop bit = 2bit */ +#define UART_MSR_PSB 0x0008 /* Parity selection bit */ +#define UART_MSR_PEB 0x0010 /* Parity enable bit */ +#define UART_NOPARITY 0x0000 /* No-parity */ +#define UART_ODDPARITY 0x0018 /* Odd parity */ +#define UART_EVENPARITY 0x0010 /* Even parity */ +#define UART_MSR_RTSC 0x0020 /* RTS receive FIFO control */ +#define UART_MSR_CSTC 0x0040 /* CTS send control */ +#define UART_MSR_TOIC_MASK 0x0c00 /* Timeout interrupt control */ +#define UART_MSR_TOIC_DIS 0x0000 /* Disabled */ +#define UART_MSR_TOIC_3 0x0400 /* 3 bytes */ +#define UART_MSR_TOIC_7 0x0800 /* 7 bytes */ +#define UART_MSR_TOIC_15 0x0c00 /* 15 bytes */ +#define UART_MSR_ALLIE 0xfc00 /* All interrupt bits */ +#define UART_MSR_LSIE 0x1000 /* Line status change int. enable */ +#define UART_MSR_REIE 0x2000 /* Receive error interrupt enable */ +#define UART_MSR_TFTIE 0x4000 /* Transmit FIFO trigger int. enable */ +#define UART_MSR_RFTIE 0x8000 /* Receive FIFO trigger int. enable */ + +#define UART_MSR_INIT (UART_NOPARITY | UART_STOPBIT_1 | UART_DATABIT_8) + +/* UART RFCR register bit definitions */ + +#define UART_RFCR_RWC_MASK 0x003f /* Receive byte count */ +#define UART_RFCR_RTL_MASK 0x0700 /* Receive trigger level */ +#define UART_RFCR_RTL_1 0x0000 /* 1 byte */ +#define UART_RFCR_RTL_4 0x0100 /* 4 bytes */ +#define UART_RFCR_RTL_8 0x0200 /* 8 bytes */ +#define UART_RFCR_RTL_16 0x0300 /* 16 bytes */ +#define UART_RFCR_RTL_24 0x0400 /* 24 bytes */ +#define UART_RFCR_RTL_32 0x0500 /* 32 bytes */ +#define UART_RFCR_RDEF 0x4000 /* Receive data error flag */ +#define UART_RFCR_RFCB 0x8000 /* Receive FIFO clear bit */ + +/* UART TFCR register bit definitions */ + +#define UART_TFCR_TWC_MASK 0x003f /* Transmit byte count */ +#define UART_TFCR_TTL_MASK 0x0700 /* Transmit trigger level */ +#define UART_TFCR_TTL_1 0x0000 /* 1 byte */ +#define UART_TFCR_TTL_4 0x0100 /* 4 bytes */ +#define UART_TFCR_TTL_8 0x0200 /* 8 bytes */ +#define UART_TFCR_TTL_16 0x0300 /* 16 bytes */ +#define UART_TFCR_TTL_24 0x0400 /* 24 bytes */ +#define UART_TFCR_TTL_32 0x0500 /* 32 bytes */ +#define UART_TFCR_TFCB 0x8000 /* Transmit FIFO clear bit */ + +/* UART LCR register bit definitions */ + +#define UART_LCR_RTS 0x0004 /* Current RTS value */ +#define UART_LCR_CTS 0x0010 /* Current CTS value */ +#define UART_LCR_DSR 0x0080 /* Current DSR value */ +#define UART_LCR_BOC 0x0100 /* Break output control */ +#define UART_LCR_UTST 0x4000 /* Test mode setting */ + +#define UART_LCR_INIT 0x0000 + +/* UART SR register bit definitions */ + +#define UART_SR_TREF 0x0001 /* Transmit register empty flag */ +#define UART_SR_TFEF 0x0002 /* Transmit FIFO empty flag */ +#define UART_SR_RFNEF 0x0004 /* Receive FIFO not empty flag */ +#define UART_SR_TOIF 0x0100 /* Timeout Interrupt flag */ +#define UART_SR_RFER 0x0200 /* Receive data error flag */ +#define UART_SR_TFTI 0x0400 /* Transmit FIFO trigger level */ +#define UART_SR_RFTI 0x0800 /* Receive FIFO trigger level */ +#define UART_SR_CTSS 0x1000 /* CTS status */ +#define UART_SR_DSRS 0x8000 /* DSR status */ + +/************************************************************************************ + * Inline Functions + ************************************************************************************/ + +#endif /* __DM320_UART_H */ diff --git a/nuttx/arch/arm/src/dm320/dm320_vectors.S b/nuttx/arch/arm/src/dm320/dm320_vectors.S new file mode 100644 index 000000000..2f22f2b42 --- /dev/null +++ b/nuttx/arch/arm/src/dm320/dm320_vectors.S @@ -0,0 +1,449 @@ +/******************************************************************** + * dm320/dm320_vectors.S + * + * Copyright (C) 2007 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 Gregory Nutt 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 <nuttx/config.h> +#include <nuttx/irq.h> +#include "up_arch.h" + +/******************************************************************** + * Definitions + ********************************************************************/ + +/******************************************************************** + * Global Data + ********************************************************************/ + + .data +g_irqtmp: + .word 0 /* Saved lr */ + .word 0 /* Saved spsr */ +g_undeftmp: + .word 0 /* Saved lr */ + .word 0 /* Saved spsr */ +g_aborttmp: + .word 0 /* Saved lr */ + .word 0 /* Saved spsr */ + +/******************************************************************** + * Assembly Macros + ********************************************************************/ + +/******************************************************************** + * Private Functions + ********************************************************************/ + + .text + +/******************************************************************** + * Public Functions + ********************************************************************/ + + .text + +/******************************************************************** + * Name: up_vectorirq + * + * Description: + * Interrupt excetpion. Entered in IRQ mode with spsr = SVC + * CPSR, lr = SVC PC + ********************************************************************/ + + .globl up_vectorirq + .type up_vectorirq, %function +up_vectorirq: + /* On entry, we are in IRQ mode. We are free to use + * the IRQ mode r13 and r14. + * + */ + + ldr r13, .Lirqtmp + sub lr, lr, #4 + str lr, [r13] @ save lr_IRQ + mrs lr, spsr + str lr, [r13, #4] @ save spsr_IRQ + + /* Then switch back to SVC mode */ + + bic lr, lr, #MODE_MASK /* Keep F and T bits */ + orr lr, lr, #(SVC_MODE | PSR_I_BIT) + msr cpsr_c, lr /* Switch to SVC mode */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 + + /* Get the values for r15(pc) and CPSR in r3 and r4 */ + + ldr r0, .Lirqtmp /* Points to temp storage */ + ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the IRQ handler with interrupts disabled. */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_doirq /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ +.Lnoirqset: + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr, r0 + ldmia sp, {r0-r15}^ /* Return */ + +.Lirqtmp: + .word g_irqtmp + + .align 5 + +/******************************************************************** + * Function: up_vectorswi + * + * Description: + * SWI interrupt. We enter the SWI in SVC mode + ********************************************************************/ + + .globl up_vectorswi + .type up_vectorswi, %function +up_vectorswi: + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp), r14(lr), r15(pc) + * and CPSR in r1-r4 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 /* R14 is altered on return from SWI */ + mov r3, r14 /* Save r14 as the PC as well */ + mrs r4, spsr /* Get the saved CPSR */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the SWI handler with interrupt disabled. + * void up_syscall(struct xcptcontext *xcp) + */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_syscall /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ + + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr, r0 + ldmia sp, {r0-r15}^ /* Return */ + + .align 5 + +/******************************************************************** + * Name: up_vectordata + * + * Description: + * Data abort Exception dispatcher. Give control to data + * abort handler. This function is entered in ABORT mode + * with spsr = SVC CPSR, lr = SVC PC + * + ********************************************************************/ + + .globl up_vectordata + .type up_vectordata, %function +up_vectordata: + /* On entry we are free to use the ABORT mode registers + * r13 and r14 + */ + + ldr r13, .Ldaborttmp /* Points to temp storage */ + sub lr, lr, #8 /* Fixup return */ + str lr, [r13] /* Save in temp storage */ + mrs lr, spsr /* Get SPSR */ + str lr, [r13, #4] /* Save in temp storage */ + + /* Then switch back to SVC mode */ + + bic lr, lr, #MODE_MASK /* Keep F and T bits */ + orr lr, lr, #(SVC_MODE | PSR_I_BIT) + msr cpsr_c, lr /* Switch to SVC mode */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 + + /* Get the values for r15(pc) and CPSR in r3 and r4 */ + + ldr r0, .Ldaborttmp /* Points to temp storage */ + ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the data abort handler with interrupt disabled. + * void up_dataabort(struct xcptcontext *xcp) + */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_dataabort /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ + + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr_cxsf, r0 + ldmia sp, {r0-r15}^ /* Return */ + +.Ldaborttmp: + .word g_aborttmp + + .align 5 + +/******************************************************************** + * Name: up_vectorprefetch + * + * Description: + * Prefetch abort exception. Entered in ABT mode with + * spsr = SVC CPSR, lr = SVC PC + ********************************************************************/ + + .globl up_vectorprefetch + .type up_vectorprefetch, %function +up_vectorprefetch: + /* On entry we are free to use the ABORT mode registers + * r13 and r14 + */ + + ldr r13, .Lpaborttmp /* Points to temp storage */ + sub lr, lr, #4 /* Fixup return */ + str lr, [r13] /* Save in temp storage */ + mrs lr, spsr /* Get SPSR */ + str lr, [r13, #4] /* Save in temp storage */ + + /* Then switch back to SVC mode */ + + bic lr, lr, #MODE_MASK /* Keep F and T bits */ + orr lr, lr, #(SVC_MODE | PSR_I_BIT) + msr cpsr_c, lr /* Switch to SVC mode */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 + + /* Get the values for r15(pc) and CPSR in r3 and r4 */ + + ldr r0, .Lpaborttmp /* Points to temp storage */ + ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the prefetch abort handler with interrupt disabled. + * void up_prefetchabort(struct xcptcontext *xcp) + */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_prefetchabort /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ + + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr_cxsf, r0 + ldmia sp, {r0-r15}^ /* Return */ + +.Lpaborttmp: + .word g_aborttmp + + .align 5 + +/******************************************************************** + * Name: up_vectorundefinsn + * + * Description: + * Undefined instruction entry exception. Entered in + * UND mode, spsr = SVC CPSR, lr = SVC PC + * + ********************************************************************/ + + .globl up_vectorundefinsn + .type up_vectorundefinsn, %function +up_vectorundefinsn: + /* On entry we are free to use the UND mode registers + * r13 and r14 + */ + + ldr r13, .Lundeftmp /* Points to temp storage */ + str lr, [r13] /* Save in temp storage */ + mrs lr, spsr /* Get SPSR */ + str lr, [r13, #4] /* Save in temp storage */ + + /* Then switch back to SVC mode */ + + bic lr, lr, #MODE_MASK /* Keep F and T bits */ + orr lr, lr, #(SVC_MODE | PSR_I_BIT) + msr cpsr_c, lr /* Switch to SVC mode */ + + /* Create a context structure. First set aside a stack frame + * and store r0-r12 into the frame. + */ + + sub sp, sp, #XCPTCONTEXT_SIZE + stmia sp, {r0-r12} /* Save the SVC mode regs */ + + /* Get the correct values of r13(sp) and r14(lr) in r1 and r2 */ + + add r1, sp, #XCPTCONTEXT_SIZE + mov r2, r14 + + /* Get the values for r15(pc) and CPSR in r3 and r4 */ + + ldr r0, .Lundeftmp /* Points to temp storage */ + ldmia r0, {r3, r4} /* Recover r1=lr_IRQ, r2=spsr_IRQ */ + + add r0, sp, #(4*REG_SP) /* Offset to pc, cpsr storage */ + stmia r0, {r1-r4} + + /* Then call the undef insn handler with interrupt disabled. + * void up_undefinedinsn(struct xcptcontext *xcp) + */ + + mov fp, #0 /* Init frame pointer */ + mov r0, sp /* Get r0=xcp */ + bl up_undefinedinsn /* Call the handler */ + + /* Restore the CPSR, SVC modr registers and return */ + + ldr r0, [sp, #(4*REG_CPSR)] /* Setup the SVC mode SPSR */ + msr spsr_cxsf, r0 + ldmia sp, {r0-r15}^ /* Return */ + +.Lundeftmp: + .word g_undeftmp + + .align 5 + +/******************************************************************** + * Name: up_vectorfiq + * + * Description: + * Shouldn't happen + ********************************************************************/ + + .globl up_vectorfiq + .type up_vectorfiq, %function +up_vectorfiq: + subs pc, lr, #4 + +/******************************************************************** + * Name: up_vectoraddrexcption + * + * Description: + * Shouldn't happen + * + ********************************************************************/ + + .globl up_vectoraddrexcptn + .type up_vectoraddrexcptn, %function +up_vectoraddrexcptn: + b up_vectoraddrexcptn + +/************************************************************************** + * Vector initialization block. + **************************************************************************/ + +/* These will be relocated to VECTOR_BASE. */ + + .globl _vector_start +_vector_start: + ldr pc, .Lresethandler /* 0x00: Reset */ + ldr pc, .Lundefinedhandler /* 0x04: Undefined instruction */ + ldr pc, .Lswihandler /* 0x08: Software interrupt */ + ldr pc, .Lprefetchaborthandler /* 0x0c: Prefetch abort */ + ldr pc, .Ldataaborthandler /* 0x10: Data abort */ + ldr pc, .Laddrexcptnhandler /* 0x14: Address exception */ + ldr pc, .Lirqhandler /* 0x18: IRQ */ + ldr pc, .Lfiqhandler /* 0x1c: FIQ */ + +.Lresethandler: + .long __start +.Lundefinedhandler: + .long up_vectorundefinsn +.Lswihandler: + .long up_vectorswi +.Lprefetchaborthandler: + .long up_vectorprefetch +.Ldataaborthandler: + .long up_vectordata +.Laddrexcptnhandler: + .long up_vectoraddrexcptn +.Lirqhandler: + .long up_vectorirq +.Lfiqhandler: + .long up_vectorfiq + .globl _vector_end +_vector_end: + .end |