From d2551b1f7c70bd43f9d19ab790dd5ab52a744dc8 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sun, 3 Oct 2010 18:01:33 +0000 Subject: Add framework for AVR git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@2961 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/avr/src/common/up_allocateheap.c | 83 ++++++++ nuttx/arch/avr/src/common/up_arch.h | 90 +++++++++ nuttx/arch/avr/src/common/up_createstack.c | 138 +++++++++++++ nuttx/arch/avr/src/common/up_exit.c | 174 +++++++++++++++++ nuttx/arch/avr/src/common/up_idle.c | 87 +++++++++ nuttx/arch/avr/src/common/up_initialize.c | 170 ++++++++++++++++ nuttx/arch/avr/src/common/up_internal.h | 245 ++++++++++++++++++++++++ nuttx/arch/avr/src/common/up_interruptcontext.c | 70 +++++++ nuttx/arch/avr/src/common/up_lowputs.c | 75 ++++++++ nuttx/arch/avr/src/common/up_mdelay.c | 91 +++++++++ nuttx/arch/avr/src/common/up_modifyreg16.c | 86 +++++++++ nuttx/arch/avr/src/common/up_modifyreg32.c | 86 +++++++++ nuttx/arch/avr/src/common/up_modifyreg8.c | 86 +++++++++ nuttx/arch/avr/src/common/up_puts.c | 76 ++++++++ nuttx/arch/avr/src/common/up_releasestack.c | 80 ++++++++ nuttx/arch/avr/src/common/up_udelay.c | 130 +++++++++++++ nuttx/arch/avr/src/common/up_usestack.c | 123 ++++++++++++ 17 files changed, 1890 insertions(+) create mode 100644 nuttx/arch/avr/src/common/up_allocateheap.c create mode 100644 nuttx/arch/avr/src/common/up_arch.h create mode 100644 nuttx/arch/avr/src/common/up_createstack.c create mode 100644 nuttx/arch/avr/src/common/up_exit.c create mode 100644 nuttx/arch/avr/src/common/up_idle.c create mode 100644 nuttx/arch/avr/src/common/up_initialize.c create mode 100644 nuttx/arch/avr/src/common/up_internal.h create mode 100644 nuttx/arch/avr/src/common/up_interruptcontext.c create mode 100644 nuttx/arch/avr/src/common/up_lowputs.c create mode 100644 nuttx/arch/avr/src/common/up_mdelay.c create mode 100644 nuttx/arch/avr/src/common/up_modifyreg16.c create mode 100644 nuttx/arch/avr/src/common/up_modifyreg32.c create mode 100644 nuttx/arch/avr/src/common/up_modifyreg8.c create mode 100644 nuttx/arch/avr/src/common/up_puts.c create mode 100644 nuttx/arch/avr/src/common/up_releasestack.c create mode 100644 nuttx/arch/avr/src/common/up_udelay.c create mode 100644 nuttx/arch/avr/src/common/up_usestack.c (limited to 'nuttx/arch/avr/src/common') diff --git a/nuttx/arch/avr/src/common/up_allocateheap.c b/nuttx/arch/avr/src/common/up_allocateheap.c new file mode 100644 index 000000000..62be64338 --- /dev/null +++ b/nuttx/arch/avr/src/common/up_allocateheap.c @@ -0,0 +1,83 @@ +/**************************************************************************** + * arch/avr/src/common/up_allocateheap.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "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/avr/src/common/up_arch.h b/nuttx/arch/avr/src/common/up_arch.h new file mode 100644 index 000000000..16a303c5a --- /dev/null +++ b/nuttx/arch/avr/src/common/up_arch.h @@ -0,0 +1,90 @@ +/**************************************************************************** + * arch/avr/src/common/up_arch.h + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef __ARCH_AVR_SRC_COMMON_UP_ARCH_H +#define __ARCH_AVR_SRC_COMMON_UP_ARCH_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Inline Functions + ****************************************************************************/ + +#ifndef __ASSEMBLY__ + +# define getreg8(a) (*(volatile uint8_t *)(a)) +# define putreg8(v,a) (*(volatile uint8_t *)(a) = (v)) +# define getreg16(a) (*(volatile uint16_t *)(a)) +# define putreg16(v,a) (*(volatile uint16_t *)(a) = (v)) +# define getreg32(a) (*(volatile uint32_t *)(a)) +# define putreg32(v,a) (*(volatile uint32_t *)(a) = (v)) + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/* Atomic modification of registers */ + +EXTERN void modifyreg8(unsigned int addr, uint8_t clearbits, uint8_t setbits); +EXTERN void modifyreg16(unsigned int addr, uint16_t clearbits, uint16_t setbits); +EXTERN void modifyreg32(unsigned int addr, uint32_t clearbits, uint32_t setbits); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_AVR_SRC_COMMON_UP_ARCH_H */ + diff --git a/nuttx/arch/avr/src/common/up_createstack.c b/nuttx/arch/avr/src/common/up_createstack.c new file mode 100644 index 000000000..7b83357b6 --- /dev/null +++ b/nuttx/arch/avr/src/common/up_createstack.c @@ -0,0 +1,138 @@ +/**************************************************************************** + * arch/avr/src/common/up_createstack.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#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. + ****************************************************************************/ + +int 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) + { +#ifdef CONFIG_DEBUG + tcb->stack_alloc_ptr = (uint32_t*)zalloc(stack_size); +#else + tcb->stack_alloc_ptr = (uint32_t*)malloc(stack_size); +#endif + } + + if (tcb->stack_alloc_ptr) + { + size_t top_of_stack; + size_t size_of_stack; + + /* The AVR32 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_t)tcb->stack_alloc_ptr + stack_size - 4; + + /* The AVR32 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_t)tcb->stack_alloc_ptr + 4; + + /* Save the adjusted stack values in the _TCB */ + + tcb->adj_stack_ptr = (uint32_t*)top_of_stack; + tcb->adj_stack_size = size_of_stack; + + up_ledon(LED_STACKCREATED); + return OK; + } + + return ERROR; +} diff --git a/nuttx/arch/avr/src/common/up_exit.c b/nuttx/arch/avr/src/common/up_exit.c new file mode 100644 index 000000000..d7104c5da --- /dev/null +++ b/nuttx/arch/avr/src/common/up_exit.c @@ -0,0 +1,174 @@ +/**************************************************************************** + * arch/avr/src/common/up_exit.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "os_internal.h" +#include "up_internal.h" + +#ifdef CONFIG_DUMP_ON_EXIT +#include +#endif + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * 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) +{ +#if CONFIG_NFILE_DESCRIPTORS > 0 || CONFIG_NFILE_STREAMS > 0 + int i; +#endif + + sdbg(" TCB=%p name=%s pid=%d\n", tcb, tcb->argv[0], tcb->pid); + sdbg(" priority=%d state=%d\n", tcb->sched_priority, tcb->task_state); + +#if CONFIG_NFILE_DESCRIPTORS > 0 + if (tcb->filelist) + { + sdbg(" 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) + { + sdbg(" fd=%d refcount=%d\n", + i, inode->i_crefs); + } + } + } +#endif + +#if CONFIG_NFILE_STREAMS > 0 + if (tcb->streams) + { + sdbg(" 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) + { +#if CONFIG_STDIO_BUFFER_SIZE > 0 + sdbg(" fd=%d nbytes=%d\n", + filep->fs_filedes, + filep->fs_bufpos - filep->fs_bufstart); +#else + sdbg(" fd=%d\n", filep->fs_filedes); +#endif + } + } + } +#endif +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: _exit + * + * Description: + * This function causes the currently executing task to cease + * to exist. This is a special case of task_delete() where the task to + * be deleted is the currently executing task. It is more complex because + * a context switch must be perform to the next ready to run task. + * + ****************************************************************************/ + +void _exit(int status) +{ + _TCB* tcb; + + /* Disable interrupts. They will be restored when the next + * task is started. + */ + + (void)irqsave(); + + slldbg("TCB=%p exitting\n", g_readytorun.head); + +#if defined(CONFIG_DUMP_ON_EXIT) && defined(CONFIG_DEBUG) + slldbg("Other tasks:\n"); + sched_foreach(_up_dumponexit, NULL); +#endif + + /* Destroy the task at the head of the ready to run list. */ + + (void)task_deletecurrent(); + + /* Now, perform the context switch to the new ready-to-run task at the + * head of the list. + */ + + tcb = (_TCB*)g_readytorun.head; + + /* Then switch contexts */ + + up_fullcontextrestore(tcb->xcp.regs); +} + diff --git a/nuttx/arch/avr/src/common/up_idle.c b/nuttx/arch/avr/src/common/up_idle.c new file mode 100644 index 000000000..2854956cd --- /dev/null +++ b/nuttx/arch/avr/src/common/up_idle.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * arch/avr/src/common/up_idle.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include "up_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * 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/avr/src/common/up_initialize.c b/nuttx/arch/avr/src/common/up_initialize.c new file mode 100644 index 000000000..fde474ec6 --- /dev/null +++ b/nuttx/arch/avr/src/common/up_initialize.c @@ -0,0 +1,170 @@ +/**************************************************************************** + * arch/avr/src/common/up_initialize.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include +#include +#include + +#include "up_arch.h" +#include "up_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_calibratedelay + * + * Description: + * Delay loops are provided for short timing loops. This function, if + * enabled, will just wait for 100 seconds. Using a stopwatch, you can + * can then determine if the timing loops are properly calibrated. + * + ****************************************************************************/ + +#if defined(CONFIG_ARCH_CALIBRATION) && defined(CONFIG_DEBUG) +static void up_calibratedelay(void) +{ + int i; + lldbg("Beginning 100s delay\n"); + for (i = 0; i < 100; i++) + { + up_mdelay(1000); + } + lldbg("End 100s delay\n"); +} +#else +# define up_calibratedelay() +#endif + +/**************************************************************************** + * Public 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 user + * initialization logic 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; + + /* Calibrate the timing loop */ + + up_calibratedelay(); + + /* Add any extra memory fragments to the memory manager */ + + up_addregion(); + + /* Initialize the interrupt subsystem */ + + up_irqinitialize(); + + /* Initialize the DMA subsystem if the weak function stm32_dmainitialize has been + * brought into the build + */ + +#ifdef CONFIG_ARCH_DMA +#ifdef CONFIG_HAVE_WEAKFUNCTIONS + if (up_dmainitialize) +#endif + { + up_dmainitialize(); + } +#endif + + /* Initialize the system timer interrupt */ + +#if !defined(CONFIG_SUPPRESS_INTERRUPTS) && !defined(CONFIG_SUPPRESS_TIMER_INTS) + up_timerinit(); +#endif + + /* Register devices */ + +#if CONFIG_NFILE_DESCRIPTORS > 0 + devnull_register(); /* Standard /dev/null */ +#endif + + /* Initialize the serial device driver */ + +#ifdef CONFIG_USE_SERIALDRIVER + up_serialinit(); +#elif defined(CONFIG_DEV_LOWCONSOLE) + lowconsole_init(); +#endif + + /* Initialize the netwok */ + + up_netinitialize(); + + /* Initialize USB */ + + up_usbinitialize(); + + up_ledon(LED_IRQSENABLED); +} + diff --git a/nuttx/arch/avr/src/common/up_internal.h b/nuttx/arch/avr/src/common/up_internal.h new file mode 100644 index 000000000..2468326ee --- /dev/null +++ b/nuttx/arch/avr/src/common/up_internal.h @@ -0,0 +1,245 @@ +/**************************************************************************** + * arch/avr/src/common/up_internal.h + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef __UP_INTERNAL_H +#define __UP_INTERNAL_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#ifndef __ASSEMBLY__ +# include +#endif + +/**************************************************************************** + * Pre-processor 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 /* DEFINED: Do not enable interrupts */ +#undef CONFIG_SUPPRESS_TIMER_INTS /* DEFINED: No timer */ +#undef CONFIG_SUPPRESS_SERIAL_INTS /* DEFINED: Console will poll */ +#undef CONFIG_SUPPRESS_UART_CONFIG /* DEFINED: Do not reconfig UART */ +#undef CONFIG_DUMP_ON_EXIT /* DEFINED: Dump task state on exit */ + +/* Determine which (if any) console driver to use */ + +#if CONFIG_NFILE_DESCRIPTORS == 0 || defined(CONFIG_DEV_LOWCONSOLE) +# undef CONFIG_USE_SERIALDRIVER +# undef CONFIG_USE_EARLYSERIALINIT +#elif defined(CONFIG_DEV_CONSOLE) && CONFIG_NFILE_DESCRIPTORS > 0 +# define CONFIG_USE_SERIALDRIVER 1 +# define CONFIG_USE_EARLYSERIALINIT 1 +#endif + +/* Check if an interrupt stack size is configured */ + +#ifndef CONFIG_ARCH_INTERRUPTSTACK +# define CONFIG_ARCH_INTERRUPTSTACK 0 +#endif + +/* Macros to handle saving and restore interrupt state. The state is copied + * from the stack to the TCB, but only a referenced is passed to get the + * state from the TCB. + */ + +#define up_savestate(regs) up_copystate(regs, current_regs) +#define up_restorestate(regs) (current_regs = regs) + +/**************************************************************************** + * 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_t *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_t g_heapbase; + +/* Address of the saved user stack pointer */ + +#if CONFIG_ARCH_INTERRUPTSTACK > 3 +extern void g_intstackbase; +#endif + +/* These 'addresses' of these values are setup by the linker script. They are + * not actual uint32_t storage locations! They are only used meaningfully in the + * following way: + * + * - The linker script defines, for example, the symbol_sdata. + * - The declareion extern uint32_t _sdata; makes C happy. C will believe + * that the value _sdata is the address of a uint32_t variable _data (it is + * not!). + * - We can recoved the linker value then by simply taking the address of + * of _data. like: uint32_t *pdata = &_sdata; + */ + +extern uint32_t _stext; /* Start of .text */ +extern uint32_t _etext; /* End_1 of .text + .rodata */ +extern const uint32_t _eronly; /* End+1 of read only section (.text + .rodata) */ +extern uint32_t _sdata; /* Start of .data */ +extern uint32_t _edata; /* End+1 of .data */ +extern uint32_t _sbss; /* Start of .bss */ +extern uint32_t _ebss; /* End+1 of .bss */ +#endif + +/**************************************************************************** + * 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_t *dest, uint32_t *src); +extern void up_decodeirq(uint32_t *regs); +extern void up_irqinitialize(void); +#ifdef CONFIG_ARCH_DMA +extern void weak_function up_dmainitialize(void); +#endif +extern int up_saveusercontext(uint32_t *saveregs); +extern void up_fullcontextrestore(uint32_t *restoreregs) __attribute__ ((noreturn)); +extern void up_switchcontext(uint32_t *saveregs, uint32_t *restoreregs); +extern void up_sigdeliver(void); +extern int up_timerisr(int irq, uint32_t *regs); +extern void up_lowputc(char ch); +extern void up_puts(const char *str); +extern void up_lowputs(const char *str); +extern uint32_t *up_doirq(int irq, uint32_t *regs); +extern int up_svcall(int irq, FAR void *context); +extern int up_hardfault(int irq, FAR void *context); +#ifdef CONFIG_PAGING +extern void up_pginitialize(void); +extern uint32_t *up_va2pte(uintptr_t vaddr); +#else /* CONFIG_PAGING */ +# define up_pginitialize() +#endif /* CONFIG_PAGING */ + +/* Defined in up_allocateheap.c */ + +#if CONFIG_MM_REGIONS > 1 +void up_addregion(void); +#else +# define up_addregion() +#endif + +/* Defined in up_serial.c */ + +#if CONFIG_NFILE_DESCRIPTORS > 0 +extern void up_earlyserialinit(void); +extern void up_serialinit(void); +#else +# define up_earlyserialinit() +# define up_serialinit() +#endif + +/* Defined in drivers/lowconsole.c */ + +#ifdef CONFIG_DEV_LOWCONSOLE +extern void lowconsole_init(void); +#else +# define lowconsole_init() +#endif + +/* 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 + +/* Defined in board/up_network.c */ + +#ifdef CONFIG_NET +extern void up_netinitialize(void); +#else +# define up_netinitialize() +#endif + +/* Defined in board/up_ethernet.c */ + +#ifdef CONFIG_USBDEV +extern void up_usbinitialize(void); +extern void up_usbuninitialize(void); +#else +# define up_usbinitialize() +# define up_usbuninitialize() +#endif + +#endif /* __ASSEMBLY__ */ + +#endif /* __UP_INTERNAL_H */ + diff --git a/nuttx/arch/avr/src/common/up_interruptcontext.c b/nuttx/arch/avr/src/common/up_interruptcontext.c new file mode 100644 index 000000000..c1bfb0e4d --- /dev/null +++ b/nuttx/arch/avr/src/common/up_interruptcontext.c @@ -0,0 +1,70 @@ +/**************************************************************************** + * arch/avr/src/common/up_interruptcontext.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "up_internal.h" + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Global Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_interrupt_context + * + * Description: Return true is we are currently executing in + * the interrupt handler context. + ****************************************************************************/ + +bool up_interrupt_context(void) +{ + return current_regs != NULL; +} diff --git a/nuttx/arch/avr/src/common/up_lowputs.c b/nuttx/arch/avr/src/common/up_lowputs.c new file mode 100644 index 000000000..5695b806a --- /dev/null +++ b/nuttx/arch/avr/src/common/up_lowputs.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * arch/avr/src/common/up_lowputs.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include "up_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_lowputs + * + * Description: + * This is a low-level helper function used to support debug. + * + ****************************************************************************/ + +void up_lowputs(const char *str) +{ + while(*str) + { + up_lowputc(*str++); + } +} + diff --git a/nuttx/arch/avr/src/common/up_mdelay.c b/nuttx/arch/avr/src/common/up_mdelay.c new file mode 100644 index 000000000..3fb76a058 --- /dev/null +++ b/nuttx/arch/avr/src/common/up_mdelay.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * arch/avr/src/common/up_mdelay.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_mdelay + * + * Description: + * Delay inline for the requested number of milliseconds. + * *** NOT multi-tasking friendly *** + * + * ASSUMPTIONS: + * The setting CONFIG_BOARD_LOOPSPERMSEC has been calibrated + * + ****************************************************************************/ + +void up_mdelay(unsigned 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/avr/src/common/up_modifyreg16.c b/nuttx/arch/avr/src/common/up_modifyreg16.c new file mode 100644 index 000000000..e06f91f96 --- /dev/null +++ b/nuttx/arch/avr/src/common/up_modifyreg16.c @@ -0,0 +1,86 @@ +/**************************************************************************** + * arch/avr/src/common/up_modifyreg16.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "up_arch.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: modifyreg16 + * + * Description: + * Atomically modify the specified bits in a memory mapped register + * + ****************************************************************************/ + +void modifyreg16(unsigned int addr, uint16_t clearbits, uint16_t setbits) +{ + irqstate_t flags; + uint16_t regval; + + flags = irqsave(); + regval = getreg16(addr); + regval &= ~clearbits; + regval |= setbits; + putreg16(regval, addr); + irqrestore(flags); +} + diff --git a/nuttx/arch/avr/src/common/up_modifyreg32.c b/nuttx/arch/avr/src/common/up_modifyreg32.c new file mode 100644 index 000000000..3369abb56 --- /dev/null +++ b/nuttx/arch/avr/src/common/up_modifyreg32.c @@ -0,0 +1,86 @@ +/**************************************************************************** + * arch/avr/src/common/up_modifyreg32.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "up_arch.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: modifyreg32 + * + * Description: + * Atomically modify the specified bits in a memory mapped register + * + ****************************************************************************/ + +void modifyreg32(unsigned int addr, uint32_t clearbits, uint32_t setbits) +{ + irqstate_t flags; + uint32_t regval; + + flags = irqsave(); + regval = getreg32(addr); + regval &= ~clearbits; + regval |= setbits; + putreg32(regval, addr); + irqrestore(flags); +} + diff --git a/nuttx/arch/avr/src/common/up_modifyreg8.c b/nuttx/arch/avr/src/common/up_modifyreg8.c new file mode 100644 index 000000000..35537a4ab --- /dev/null +++ b/nuttx/arch/avr/src/common/up_modifyreg8.c @@ -0,0 +1,86 @@ +/**************************************************************************** + * arch/avr/src/common/up_modifyreg8.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "up_arch.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: modifyreg8 + * + * Description: + * Atomically modify the specified bits in a memory mapped register + * + ****************************************************************************/ + +void modifyreg8(unsigned int addr, uint8_t clearbits, uint8_t setbits) +{ + irqstate_t flags; + uint8_t regval; + + flags = irqsave(); + regval = getreg8(addr); + regval &= ~clearbits; + regval |= setbits; + putreg8(regval, addr); + irqrestore(flags); +} + diff --git a/nuttx/arch/avr/src/common/up_puts.c b/nuttx/arch/avr/src/common/up_puts.c new file mode 100644 index 000000000..aa651e294 --- /dev/null +++ b/nuttx/arch/avr/src/common/up_puts.c @@ -0,0 +1,76 @@ +/**************************************************************************** + * arch/avr/src/common/up_puts.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include "up_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_puts + * + * Description: + * This is a low-level helper function used to support debug. + * + ****************************************************************************/ + +void up_puts(const char *str) +{ + while(*str) + { + up_putc(*str++); + } +} + diff --git a/nuttx/arch/avr/src/common/up_releasestack.c b/nuttx/arch/avr/src/common/up_releasestack.c new file mode 100644 index 000000000..4c453c590 --- /dev/null +++ b/nuttx/arch/avr/src/common/up_releasestack.c @@ -0,0 +1,80 @@ +/**************************************************************************** + * arch/avr/src/common/up_releasestack.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "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/avr/src/common/up_udelay.c b/nuttx/arch/avr/src/common/up_udelay.c new file mode 100644 index 000000000..5bbeed907 --- /dev/null +++ b/nuttx/arch/avr/src/common/up_udelay.c @@ -0,0 +1,130 @@ +/**************************************************************************** + * arch/avr/src/common/up_udelay.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include + +/**************************************************************************** + * Definitions + ****************************************************************************/ + +#define CONFIG_BOARD_LOOPSPER100USEC ((CONFIG_BOARD_LOOPSPERMSEC+5)/10) +#define CONFIG_BOARD_LOOPSPER10USEC ((CONFIG_BOARD_LOOPSPERMSEC+50)/100) +#define CONFIG_BOARD_LOOPSPERUSEC ((CONFIG_BOARD_LOOPSPERMSEC+500)/1000) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_udelay + * + * Description: + * Delay inline for the requested number of microseconds. NOTE: Because + * of all of the setup, several microseconds will be lost before the actual + * timing looop begins. Thus, the delay will always be a few microseconds + * longer than requested. + * + * *** NOT multi-tasking friendly *** + * + * ASSUMPTIONS: + * The setting CONFIG_BOARD_LOOPSPERMSEC has been calibrated + * + ****************************************************************************/ + +void up_udelay(useconds_t microseconds) +{ + volatile int i; + + /* We'll do this a little at a time because we expect that the + * CONFIG_BOARD_LOOPSPERUSEC is very inaccurate during to truncation in + * the divisions of its calculation. We'll use the largest values that + * we can in order to prevent significant error buildup in the loops. + */ + + while (microseconds > 1000) + { + for (i = 0; i < CONFIG_BOARD_LOOPSPERMSEC; i++) + { + } + microseconds -= 1000; + } + + while (microseconds > 100) + { + for (i = 0; i < CONFIG_BOARD_LOOPSPER100USEC; i++) + { + } + microseconds -= 100; + } + + while (microseconds > 10) + { + for (i = 0; i < CONFIG_BOARD_LOOPSPER10USEC; i++) + { + } + microseconds -= 10; + } + + while (microseconds > 0) + { + for (i = 0; i < CONFIG_BOARD_LOOPSPERUSEC; i++) + { + } + microseconds--; + } +} + diff --git a/nuttx/arch/avr/src/common/up_usestack.c b/nuttx/arch/avr/src/common/up_usestack.c new file mode 100644 index 000000000..b2da00cf2 --- /dev/null +++ b/nuttx/arch/avr/src/common/up_usestack.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * arch/avr/src/common/up_usestack.c + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "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. + * + ****************************************************************************/ + +int 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 AVR32 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_t)tcb->stack_alloc_ptr + stack_size - 4; + + /* The AVR32 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_t)tcb->stack_alloc_ptr + 4; + + /* Save the adjusted stack values in the _TCB */ + + tcb->adj_stack_ptr = (uint32_t*)top_of_stack; + tcb->adj_stack_size = size_of_stack; + + return OK; +} + -- cgit v1.2.3