From e3940eb2080711edac189cca3f642ee89dc215f2 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sat, 17 Feb 2007 23:21:28 +0000 Subject: NuttX RTOS git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@3 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/sim/src/Makefile | 76 ++++++++++++++ nuttx/arch/sim/src/up_allocateheap.c | 81 ++++++++++++++ nuttx/arch/sim/src/up_blocktask.c | 157 +++++++++++++++++++++++++++ nuttx/arch/sim/src/up_createstack.c | 109 +++++++++++++++++++ nuttx/arch/sim/src/up_devconsole.c | 136 ++++++++++++++++++++++++ nuttx/arch/sim/src/up_exit.c | 123 ++++++++++++++++++++++ nuttx/arch/sim/src/up_head.c | 77 ++++++++++++++ nuttx/arch/sim/src/up_idle.c | 84 +++++++++++++++ nuttx/arch/sim/src/up_initialize.c | 88 ++++++++++++++++ nuttx/arch/sim/src/up_initialstate.c | 81 ++++++++++++++ nuttx/arch/sim/src/up_internal.h | 91 ++++++++++++++++ nuttx/arch/sim/src/up_interruptcontext.c | 74 +++++++++++++ nuttx/arch/sim/src/up_releasepending.c | 118 +++++++++++++++++++++ nuttx/arch/sim/src/up_releasestack.c | 81 ++++++++++++++ nuttx/arch/sim/src/up_reprioritizertr.c | 169 ++++++++++++++++++++++++++++++ nuttx/arch/sim/src/up_schedulesigaction.c | 109 +++++++++++++++++++ nuttx/arch/sim/src/up_setjmp.S | 129 +++++++++++++++++++++++ nuttx/arch/sim/src/up_unblocktask.c | 146 ++++++++++++++++++++++++++ nuttx/arch/sim/src/up_usestack.c | 100 ++++++++++++++++++ 19 files changed, 2029 insertions(+) create mode 100644 nuttx/arch/sim/src/Makefile create mode 100644 nuttx/arch/sim/src/up_allocateheap.c create mode 100644 nuttx/arch/sim/src/up_blocktask.c create mode 100644 nuttx/arch/sim/src/up_createstack.c create mode 100644 nuttx/arch/sim/src/up_devconsole.c create mode 100644 nuttx/arch/sim/src/up_exit.c create mode 100644 nuttx/arch/sim/src/up_head.c create mode 100644 nuttx/arch/sim/src/up_idle.c create mode 100644 nuttx/arch/sim/src/up_initialize.c create mode 100644 nuttx/arch/sim/src/up_initialstate.c create mode 100644 nuttx/arch/sim/src/up_internal.h create mode 100644 nuttx/arch/sim/src/up_interruptcontext.c create mode 100644 nuttx/arch/sim/src/up_releasepending.c create mode 100644 nuttx/arch/sim/src/up_releasestack.c create mode 100644 nuttx/arch/sim/src/up_reprioritizertr.c create mode 100644 nuttx/arch/sim/src/up_schedulesigaction.c create mode 100644 nuttx/arch/sim/src/up_setjmp.S create mode 100644 nuttx/arch/sim/src/up_unblocktask.c create mode 100644 nuttx/arch/sim/src/up_usestack.c (limited to 'nuttx/arch/sim/src') diff --git a/nuttx/arch/sim/src/Makefile b/nuttx/arch/sim/src/Makefile new file mode 100644 index 000000000..780cff596 --- /dev/null +++ b/nuttx/arch/sim/src/Makefile @@ -0,0 +1,76 @@ +############################################################ +# Makefile +# +# Copyright (C) 2007 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 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 + +MKDEP = $(TOPDIR)/tools/mkdeps.sh +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = up_setjmp.S +AOBJS = $(ASRCS:.S=.o) +CSRCS = up_initialize.c up_idle.c up_interruptcontext.c up_initialstate.c \ + up_createstack.c up_usestack.c up_releasestack.c \ + up_unblocktask.c up_blocktask.c up_releasepending.c up_reprioritizertr.c \ + up_exit.c up_schedulesigaction.c up_allocateheap.c up_devconsole.c +COBJS = $(CSRCS:.c=.o) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +all: up_head.o libarch.a + +$(AOBJS): %.o: %.S + $(CC) -c $(CFLAGS) -D__ASSEMBLY__ $< -o $@ + +$(COBJS) up_head.o: %.o: %.c + $(CC) -c $(CFLAGS) $< -o $@ + +libarch.a: $(OBJS) + $(AR) rcs $@ $(OBJS) + +.depend: Makefile $(SRCS) + $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + touch $@ + +depend: .depend + +clean: + rm -f libarch.a *.o *~ + +distclean: clean + rm -f Make.dep .depend + + +-include Make.dep diff --git a/nuttx/arch/sim/src/up_allocateheap.c b/nuttx/arch/sim/src/up_allocateheap.c new file mode 100644 index 000000000..b464719d4 --- /dev/null +++ b/nuttx/arch/sim/src/up_allocateheap.c @@ -0,0 +1,81 @@ +/************************************************************ + * up_allocateheap.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +static ubyte sim_heap[SIM_HEAP_SIZE]; + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * 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(void **heap_start, size_t *heap_size) +{ + *heap_start = sim_heap; + *heap_size = SIM_HEAP_SIZE; +} diff --git a/nuttx/arch/sim/src/up_blocktask.c b/nuttx/arch/sim/src/up_blocktask.c new file mode 100644 index 000000000..66cd89372 --- /dev/null +++ b/nuttx/arch/sim/src/up_blocktask.c @@ -0,0 +1,157 @@ +/************************************************************ + * up_blocktask.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include +#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; + + dbg("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) + { + /* Copy the exception context into the TCB at the (old) head of the + * g_readytorun Task list. if up_setjmp returns a non-zero + * value, then this is really the previously running task restarting! + */ + + if (!up_setjmp(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; + dbg("New Active Task TCB=%p\n", rtcb); + + /* The way that we handle signals in the simulation is kind of + * a kludge. This would be unsafe in a truly multi-threaded, interrupt + * driven environment. + */ + + if (rtcb->xcp.sigdeliver) + { + dbg("Delivering signals TCB=%p\n", rtcb); + ((sig_deliver_t)rtcb->xcp.sigdeliver)(rtcb); + rtcb->xcp.sigdeliver = NULL; + } + + /* Then switch contexts */ + + up_longjmp(rtcb->xcp.regs, 1); + } + } + } +} diff --git a/nuttx/arch/sim/src/up_createstack.c b/nuttx/arch/sim/src/up_createstack.c new file mode 100644 index 000000000..0e2c63e4d --- /dev/null +++ b/nuttx/arch/sim/src/up_createstack.c @@ -0,0 +1,109 @@ +/************************************************************ + * up_createstack.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * 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 StatckAllocPtr 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, uint32 stack_size) +{ + STATUS ret = ERROR; + + /* Move up to next even word boundary if necessary */ + + uint32 adj_stack_size = (stack_size + 3) & ~3; + uint32 adj_stack_words = adj_stack_size >> 2; + + /* Allocate the memory for the stack */ + + uint32 *stack_alloc_ptr = (uint32*)kmalloc(adj_stack_size); + if (stack_alloc_ptr) + { + /* This is the address of the last word in the allocation */ + + uint32 *adj_stack_ptr = &stack_alloc_ptr[adj_stack_words - 1]; + + /* Save the values in the TCB */ + tcb->adj_stack_size = adj_stack_size; + tcb->stack_alloc_ptr = stack_alloc_ptr; + tcb->adj_stack_ptr = adj_stack_ptr; + ret = OK; + } + return ret; +} diff --git a/nuttx/arch/sim/src/up_devconsole.c b/nuttx/arch/sim/src/up_devconsole.c new file mode 100644 index 000000000..b55df53df --- /dev/null +++ b/nuttx/arch/sim/src/up_devconsole.c @@ -0,0 +1,136 @@ +/************************************************************ + * up_devconsole.c + * + * Copyright (C) 2007 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 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 +#include + +#include +#include + +#include + +#include "up_internal.h" + +/************************************************************ + * Definitions + ************************************************************/ + +#define READ 3 +#define WRITE 4 + +/************************************************************ + * Private Function Prototypes + ************************************************************/ + +static ssize_t devconsole_read(struct file *, char *, size_t); +static ssize_t devconsole_write(struct file *, const char *, size_t); + +/************************************************************ + * Private Data + ************************************************************/ + +static struct file_operations devconsole_fops = +{ + .read = devconsole_read, + .write = devconsole_write, +}; + +/************************************************************ + * Private Functions + ************************************************************/ + +static inline int up_read(int fd, void* buf, size_t count) +{ + uint32 result; + + __asm__ volatile ("int $0x80" \ + : "=a" (result) \ + : "0" (READ), "b" ((uint32)(fd)), "c" ((uint32)(buf)), "d" ((uint32)(count)) \ + : "memory"); + + return (int)result; +} + +static inline int up_write(int fd, const void* buf, size_t count) +{ + uint32 result; + + __asm__ volatile ("int $0x80" \ + : "=a" (result) \ + : "0" (WRITE), "b" ((uint32)(fd)), "c" ((uint32)(buf)), "d" ((uint32)(count)) \ + : "memory"); + + return (int)result; +} + +static inline int up_check_result(int result) +{ + if (result >= (uint32)(-(128 + 1))) + { + *get_errno_ptr() = -result; + result = ERROR; + } + return result; +} + +static ssize_t devconsole_read(struct file *filp, char *buffer, size_t len) +{ + return up_check_result(up_read(1, buffer, len)); +} + +static ssize_t devconsole_write(struct file *filp, const char *buffer, size_t len) +{ + return up_check_result(up_write(1, buffer, len)); +} + +/************************************************************ + * Public Funtions + ************************************************************/ + +void up_devconsole(void) +{ + (void)register_inode("/dev/console", &devconsole_fops, 0666, NULL); +} + +int up_putc(int ch) +{ + char b = ch; + (void)up_write(1, &b, 1); + return ch; +} diff --git a/nuttx/arch/sim/src/up_exit.c b/nuttx/arch/sim/src/up_exit.c new file mode 100644 index 000000000..a0a3bdc9a --- /dev/null +++ b/nuttx/arch/sim/src/up_exit.c @@ -0,0 +1,123 @@ +/************************************************************ + * up_exit.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * 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; + + dbg("TCB=%p exitting\n", tcb); + + /* 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); + + /* 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, perform the context switch to the new ready-to-run task at the + * head of the list. + */ + + tcb = (_TCB*)g_readytorun.head; + dbg("New Active Task TCB=%p\n", tcb); + + /* The way that we handle signals in the simulation is kind of + * a kludge. This would be unsafe in a truly multi-threaded, interrupt + * driven environment. + */ + + if (tcb->xcp.sigdeliver) + { + dbg("Delivering signals TCB=%p\n", tcb); + ((sig_deliver_t)tcb->xcp.sigdeliver)(tcb); + tcb->xcp.sigdeliver = NULL; + } + + /* Then switch contexts */ + + up_longjmp(tcb->xcp.regs, 1); +} + diff --git a/nuttx/arch/sim/src/up_head.c b/nuttx/arch/sim/src/up_head.c new file mode 100644 index 000000000..9d1ccf153 --- /dev/null +++ b/nuttx/arch/sim/src/up_head.c @@ -0,0 +1,77 @@ +/************************************************************ + * up_head.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include +#include +#include + +/************************************************************ + * Private Data + ************************************************************/ + +static jmp_buf sim_abort; + +/************************************************************ + * Global Funtions + ************************************************************/ + +int main(int argc, char **argv, char **envp) +{ + if (setjmp(sim_abort) == 0) + { + os_start(); + } + return 0; +} + +void up_assert(const ubyte *filename, uint32 line) +{ + fprintf(stderr, "Assertion failed at file:%s line: %d\n", filename, line); + longjmp(sim_abort, 1); +} + +void up_assert_code(const ubyte *filename, uint32 line, uint16 code) +{ + fprintf(stderr, "Assertion failed at file:%s line: %d error code: %d\n", filename, line, code); + longjmp(sim_abort, 1); +} diff --git a/nuttx/arch/sim/src/up_idle.c b/nuttx/arch/sim/src/up_idle.c new file mode 100644 index 000000000..b40b2e3d8 --- /dev/null +++ b/nuttx/arch/sim/src/up_idle.c @@ -0,0 +1,84 @@ +/************************************************************ + * up_idle.c + * + * Copyright (C) 2007 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 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 +#include +#include +#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 the system, then process timer interrupts. Hopefully + * something will wake up. + */ + + sched_process_timer(); +} + diff --git a/nuttx/arch/sim/src/up_initialize.c b/nuttx/arch/sim/src/up_initialize.c new file mode 100644 index 000000000..90b0b49e4 --- /dev/null +++ b/nuttx/arch/sim/src/up_initialize.c @@ -0,0 +1,88 @@ +/************************************************************ + * up_initialize.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * 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) +{ + /* Register devices */ + + devnull_register(); /* Standard /dev/null */ + up_devconsole(); /* Our private /dev/console */ +} diff --git a/nuttx/arch/sim/src/up_initialstate.c b/nuttx/arch/sim/src/up_initialstate.c new file mode 100644 index 000000000..ec3e50991 --- /dev/null +++ b/nuttx/arch/sim/src/up_initialstate.c @@ -0,0 +1,81 @@ +/************************************************************ + * up_initialstate.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include "up_internal.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) +{ + memset(&tcb->xcp, 0, sizeof(struct xcptcontext)); + tcb->xcp.regs[JB_SP] = (uint32)tcb->adj_stack_ptr; + tcb->xcp.regs[JB_PC] = (uint32)tcb->start; +} diff --git a/nuttx/arch/sim/src/up_internal.h b/nuttx/arch/sim/src/up_internal.h new file mode 100644 index 000000000..29defe029 --- /dev/null +++ b/nuttx/arch/sim/src/up_internal.h @@ -0,0 +1,91 @@ +/************************************************************************** + * up_internal.h + * + * Copyright (C) 2007 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 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 __ARCH_UP_INTERNAL_H +#define __ARCH_UP_INTERNAL_H + +/************************************************************************** + * Included Files + **************************************************************************/ + +#include + +/************************************************************************** + * Public Definitions + **************************************************************************/ + +/* Storage order: %ebx, $esi, %edi, %ebp, sp, and return PC */ +#ifdef __ASSEMBLY__ +# define JB_EBX (0*4) +# define JB_ESI (1*4) +# define JB_EDI (2*4) +# define JB_EBP (3*4) +# define JB_SP (4*4) +# define JB_PC (5*4) +#else +# define JB_EBX (0) +# define JB_ESI (1) +# define JB_EDI (2) +# define JB_EBP (3) +# define JB_SP (4) +# define JB_PC (5) +#endif /* __ASSEMBLY__ */ + +#define SIM_HEAP_SIZE (4*1024*1024) + +/************************************************************************** + * Public Types + **************************************************************************/ + +/************************************************************************** + * Public Variables + **************************************************************************/ + +/************************************************************************** + * Public Function Prototypes + **************************************************************************/ + +#ifndef __ASSEMBLY__ +/* up_setjmp.S ************************************************************/ + +extern int up_setjmp(int *jb); +extern void up_longjmp(int *jb, int val) __attribute__ ((noreturn)); + +/* up_devconsole **********************************************************/ + +extern void up_devconsole(void); + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_UP_INTERNAL_H */ diff --git a/nuttx/arch/sim/src/up_interruptcontext.c b/nuttx/arch/sim/src/up_interruptcontext.c new file mode 100644 index 000000000..ad1eb74d7 --- /dev/null +++ b/nuttx/arch/sim/src/up_interruptcontext.c @@ -0,0 +1,74 @@ +/************************************************************ + * up_interruptcontext.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * Name: up_interrupt_context + * + * Description: + * Return TRUE is we are currently executing in + * the interrupt handler context. + ************************************************************/ + +boolean up_interrupt_context(void) +{ + /* The simulation is never in the interrupt state */ + return FALSE; +} + diff --git a/nuttx/arch/sim/src/up_releasepending.c b/nuttx/arch/sim/src/up_releasepending.c new file mode 100644 index 000000000..8a494a354 --- /dev/null +++ b/nuttx/arch/sim/src/up_releasepending.c @@ -0,0 +1,118 @@ +/************************************************************ + * up_releasepending.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include +#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; + + dbg("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! Copy the exception context + * into the TCB of the task that was currently active. if up_setjmp + * returns a non-zero value, then this is really the previously + * running task restarting! + */ + + if (!up_setjmp(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; + dbg("New Active Task TCB=%p\n", rtcb); + + /* The way that we handle signals in the simulation is kind of + * a kludge. This would be unsafe in a truly multi-threaded, interrupt + * driven environment. + */ + + if (rtcb->xcp.sigdeliver) + { + dbg("Delivering signals TCB=%p\n", rtcb); + ((sig_deliver_t)rtcb->xcp.sigdeliver)(rtcb); + rtcb->xcp.sigdeliver = NULL; + } + + /* Then switch contexts */ + + up_longjmp(rtcb->xcp.regs, 1); + } + } +} diff --git a/nuttx/arch/sim/src/up_releasestack.c b/nuttx/arch/sim/src/up_releasestack.c new file mode 100644 index 000000000..5fc950c6a --- /dev/null +++ b/nuttx/arch/sim/src/up_releasestack.c @@ -0,0 +1,81 @@ +/************************************************************ + * up_releasestack.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * 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) + { + free(dtcb->stack_alloc_ptr); + } + + dtcb->stack_alloc_ptr = NULL; + dtcb->adj_stack_size = 0; + dtcb->adj_stack_ptr = NULL; +} diff --git a/nuttx/arch/sim/src/up_reprioritizertr.c b/nuttx/arch/sim/src/up_reprioritizertr.c new file mode 100644 index 000000000..d4d91a88e --- /dev/null +++ b/nuttx/arch/sim/src/up_reprioritizertr.c @@ -0,0 +1,169 @@ +/************************************************************ + * up_reprioritizertr.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include +#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; + + dbg("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(); + } + + /* Copy the exception context into the TCB at the (old) head of the + * g_readytorun Task list. if up_setjmp returns a non-zero + * value, then this is really the previously running task restarting! + */ + + if (!up_setjmp(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; + dbg("New Active Task TCB=%p\n", rtcb); + + /* The way that we handle signals in the simulation is kind of + * a kludge. This would be unsafe in a truly multi-threaded, interrupt + * driven environment. + */ + + if (rtcb->xcp.sigdeliver) + { + dbg("Delivering signals TCB=%p\n", rtcb); + ((sig_deliver_t)rtcb->xcp.sigdeliver)(rtcb); + rtcb->xcp.sigdeliver = NULL; + } + + /* Then switch contexts */ + + up_longjmp(rtcb->xcp.regs, 1); + } + } + } +} diff --git a/nuttx/arch/sim/src/up_schedulesigaction.c b/nuttx/arch/sim/src/up_schedulesigaction.c new file mode 100644 index 000000000..6e6d13911 --- /dev/null +++ b/nuttx/arch/sim/src/up_schedulesigaction.c @@ -0,0 +1,109 @@ +/************************************************************ + * up_schedulesigaction.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include +#include "os_internal.h" +#include "up_internal.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) +{ + /* We don't have to anything complex of the simulated target */ + + if (tcb == (_TCB*)g_readytorun.head) + { + sigdeliver(tcb); + } + else + { + tcb->xcp.sigdeliver = sigdeliver; + } +} diff --git a/nuttx/arch/sim/src/up_setjmp.S b/nuttx/arch/sim/src/up_setjmp.S new file mode 100644 index 000000000..96b9bfee8 --- /dev/null +++ b/nuttx/arch/sim/src/up_setjmp.S @@ -0,0 +1,129 @@ +/************************************************************************** + * up_setjmp.S + * + * Copyright (C) 2007 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 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. + * + **************************************************************************/ + +/************************************************************************** + * Conditional Compilation Options + **************************************************************************/ + +/************************************************************************** + * Included Files + **************************************************************************/ + +#include "up_internal.h" + +/************************************************************************** + * Private Definitions + **************************************************************************/ + +/************************************************************************** + * Private Types + **************************************************************************/ + +/************************************************************************** + * Private Function Prototypes + **************************************************************************/ + +/************************************************************************** + * Global Variables + **************************************************************************/ + +/************************************************************************** + * Private Variables + **************************************************************************/ + +/************************************************************************** + * Private Functions + **************************************************************************/ + +/************************************************************************** + * Public Functions + **************************************************************************/ + + .text + .globl up_setjmp + .type up_setjmp, @function +up_setjmp: + + /* %ebx, %esi, %edi, and %ebp must be preserved. + * save %ebx, $esi, and %edi now... */ + + movl 4(%esp), %eax + movl %ebx, (JB_EBX)(%eax) + movl %esi, (JB_ESI)(%eax) + movl %edi, (JB_EDI)(%eax) + + /* Save the value of SP as will be after we return */ + + leal 4(%esp), %ecx + movl %ecx, (JB_SP)(%eax) + + /* Save the return PC */ + + movl 0(%esp), %ecx + movl %ecx, (JB_PC)(%eax) + + /* Save the framepointer */ + + movl %ebp, (JB_EBP)(%eax) + + /* And return 0 */ + + xorl %eax, %eax + ret + .size up_setjmp, . - up_setjmp + + .globl up_longjmp + .type up_longjmp, @function +up_longjmp: + movl 4(%esp), %ecx /* U_pthread_jmpbuf in %ecx. */ + movl 8(%esp), %eax /* Second argument is return value. */ + + /* Save the return address now. */ + + movl (JB_PC)(%ecx), %edx + + /* Restore registers. */ + + movl (JB_EBX)(%ecx), %ebx + movl (JB_ESI)(%ecx), %esi + movl (JB_EDI)(%ecx), %edi + movl (JB_EBP)(%ecx), %ebp + movl (JB_SP)(%ecx), %esp + + /* Jump to saved PC. */ + + jmp *%edx + .size up_longjmp, . - up_longjmp + diff --git a/nuttx/arch/sim/src/up_unblocktask.c b/nuttx/arch/sim/src/up_unblocktask.c new file mode 100644 index 000000000..bf3148506 --- /dev/null +++ b/nuttx/arch/sim/src/up_unblocktask.c @@ -0,0 +1,146 @@ +/************************************************************ + * up_unblocktask.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include +#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; + + dbg("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! Copy the exception context + * into the TCB of the task that was previously active. if + * up_setjmp returns a non-zero value, then this is really the + * previously running task restarting! + */ + + if (!up_setjmp(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; + dbg("New Active Task TCB=%p\n", rtcb); + + /* The way that we handle signals in the simulation is kind of + * a kludge. This would be unsafe in a truly multi-threaded, interrupt + * driven environment. + */ + + if (rtcb->xcp.sigdeliver) + { + dbg("Delivering signals TCB=%p\n", rtcb); + ((sig_deliver_t)rtcb->xcp.sigdeliver)(rtcb); + rtcb->xcp.sigdeliver = NULL; + } + + /* Then switch contexts */ + + up_longjmp(rtcb->xcp.regs, 1); + } + } + } +} diff --git a/nuttx/arch/sim/src/up_usestack.c b/nuttx/arch/sim/src/up_usestack.c new file mode 100644 index 000000000..26bf14b21 --- /dev/null +++ b/nuttx/arch/sim/src/up_usestack.c @@ -0,0 +1,100 @@ +/************************************************************ + * up_usestack.c + * + * Copyright (C) 2007 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 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 +#include +#include +#include +#include "os_internal.h" +#include "up_internal.h" + +/************************************************************ + * Private Definitions + ************************************************************/ + +/************************************************************ + * Private Data + ************************************************************/ + +/************************************************************ + * Private Funtions + ************************************************************/ + +/************************************************************ + * Public Funtions + ************************************************************/ + +/************************************************************ + * 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 StatckAllocPtr 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, uint32 *stack, uint32 stack_size) +{ + /* Move up to next even word boundary if necessary */ + + uint32 adj_stack_size = stack_size & ~3; + uint32 adj_stack_words = adj_stack_size >> 2; + + /* This is the address of the last word in the allocation */ + + uint32 *adj_stack_ptr = &stack[adj_stack_words - 1]; + + /* Save the values in the TCB */ + tcb->adj_stack_size = adj_stack_size; + tcb->stack_alloc_ptr = stack; + tcb->adj_stack_ptr = adj_stack_ptr; + return OK; +} -- cgit v1.2.3