summaryrefslogtreecommitdiff
path: root/nuttx/arch
diff options
context:
space:
mode:
authorpatacongo <patacongo@42af7a65-404d-4744-a932-0658087f49c3>2011-08-04 12:29:19 +0000
committerpatacongo <patacongo@42af7a65-404d-4744-a932-0658087f49c3>2011-08-04 12:29:19 +0000
commitc07e3aeb901dac03d1faff65b4c6eda1de25141c (patch)
tree67ad77464ffc9b98908f63a61f3d62a7529a9fc8 /nuttx/arch
parentf831ee4825edac31395b26876023e792cda23281 (diff)
downloadpx4-nuttx-c07e3aeb901dac03d1faff65b4c6eda1de25141c.tar.gz
px4-nuttx-c07e3aeb901dac03d1faff65b4c6eda1de25141c.tar.bz2
px4-nuttx-c07e3aeb901dac03d1faff65b4c6eda1de25141c.zip
Add Yu Qiang's patch for RGMP support on the ARM
git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@3843 42af7a65-404d-4744-a932-0658087f49c3
Diffstat (limited to 'nuttx/arch')
-rw-r--r--nuttx/arch/rgmp/include/arch.h16
-rw-r--r--nuttx/arch/rgmp/include/arm/arch/subarch/arch.h58
-rw-r--r--nuttx/arch/rgmp/include/irq.h8
-rw-r--r--nuttx/arch/rgmp/include/stdint.h22
-rw-r--r--nuttx/arch/rgmp/include/x86/arch/com.h (renamed from nuttx/arch/rgmp/include/com.h)1
-rw-r--r--nuttx/arch/rgmp/include/x86/arch/subarch/arch.h60
-rw-r--r--nuttx/arch/rgmp/src/Makefile7
-rw-r--r--nuttx/arch/rgmp/src/arm/Make.defs42
-rw-r--r--nuttx/arch/rgmp/src/arm/arch_nuttx.c91
-rw-r--r--nuttx/arch/rgmp/src/arm/sigentry.S49
-rw-r--r--nuttx/arch/rgmp/src/nuttx.c66
-rw-r--r--nuttx/arch/rgmp/src/rgmp.c28
-rw-r--r--nuttx/arch/rgmp/src/x86/Make.defs42
-rw-r--r--nuttx/arch/rgmp/src/x86/arch_nuttx.c106
-rw-r--r--nuttx/arch/rgmp/src/x86/com.c (renamed from nuttx/arch/rgmp/src/com.c)4
-rw-r--r--nuttx/arch/rgmp/src/x86/sigentry.S (renamed from nuttx/arch/rgmp/src/sigentry.S)2
16 files changed, 496 insertions, 106 deletions
diff --git a/nuttx/arch/rgmp/include/arch.h b/nuttx/arch/rgmp/include/arch.h
index 789c1d471..c93397baa 100644
--- a/nuttx/arch/rgmp/include/arch.h
+++ b/nuttx/arch/rgmp/include/arch.h
@@ -40,10 +40,12 @@
#ifndef __RGMP_ARCH_ARCH_H
#define __RGMP_ARCH_ARCH_H
+#include <arch/subarch/arch.h>
+
#ifndef __ASSEMBLY__
#include <nuttx/sched.h>
-#include <rgmp/hpet.h>
+
struct up_wait {
struct up_wait *next;
@@ -57,16 +59,6 @@ void up_sigentry(void);
int up_register_bridge(char *name, int size);
int up_unregister_bridge(char *name);
-static inline void up_mdelay(uint32_t msec)
-{
- hpet_ndelay(msec*1000000);
-}
-
-static inline void up_udelay(uint32_t usec)
-{
- hpet_udelay(usec*1000);
-}
-
-#endif
+#endif /* !__ASSEMBLY__ */
#endif
diff --git a/nuttx/arch/rgmp/include/arm/arch/subarch/arch.h b/nuttx/arch/rgmp/include/arm/arch/subarch/arch.h
new file mode 100644
index 000000000..8b92c6bfe
--- /dev/null
+++ b/nuttx/arch/rgmp/include/arm/arch/subarch/arch.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ * arch/rgmp/include/arm/arch/subarch/arch.h
+ *
+ * Copyright (C) 2011 Yu Qiang. All rights reserved.
+ * Author: Yu Qiang <yuq825@gmail.com>
+ *
+ * This file is a part of NuttX:
+ *
+ * Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *
+ * 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 __RGMP_ARCH_SUBARCH_ARCH_H
+#define __RGMP_ARCH_SUBARCH_ARCH_H
+
+#ifndef __ASSEMBLY__
+
+
+static inline void up_mdelay(uint32_t msec)
+{
+
+}
+
+static inline void up_udelay(uint32_t usec)
+{
+
+}
+
+#endif /* !__ASSEMBLY__ */
+
+#endif
diff --git a/nuttx/arch/rgmp/include/irq.h b/nuttx/arch/rgmp/include/irq.h
index 7f8a06dbb..00c977560 100644
--- a/nuttx/arch/rgmp/include/irq.h
+++ b/nuttx/arch/rgmp/include/irq.h
@@ -44,9 +44,9 @@
#ifndef __ASSEMBLY__
-#include <rgmp/trap.h>
#include <rgmp/spinlock.h>
#include <arch/types.h>
+#include <rgmp/trap.h>
struct xcptcontext {
struct Trapframe *tf;
@@ -56,6 +56,9 @@ struct xcptcontext {
void *sigdeliver;
};
+void push_xcptcontext(struct xcptcontext *xcp);
+void pop_xcptcontext(struct xcptcontext *xcp);
+
extern int nest_irq;
static inline irqstate_t irqsave(void)
@@ -68,5 +71,6 @@ static inline void irqrestore(irqstate_t flags)
popcli(flags);
}
-#endif
+#endif /* !__ASSEMBLY__ */
+
#endif
diff --git a/nuttx/arch/rgmp/include/stdint.h b/nuttx/arch/rgmp/include/stdint.h
index 59dff2c8d..7b244dd9f 100644
--- a/nuttx/arch/rgmp/include/stdint.h
+++ b/nuttx/arch/rgmp/include/stdint.h
@@ -207,29 +207,7 @@
* provided by their toolchain header files.
*/
-#ifdef CONFIG_ARCH_RGMP
#include <rgmp/types.h>
-#else
-typedef _int8_t int8_t;
-typedef _uint8_t uint8_t;
-
-typedef _int16_t int16_t;
-typedef _uint16_t uint16_t;
-
-#ifdef __INT24_DEFINED
-typedef _int24_t int24_t;
-typedef _uint24_t uint24_t;
-#endif
-
-typedef _int32_t int32_t;
-typedef _uint32_t uint32_t;
-
-#ifdef __INT64_DEFINED
-typedef _int64_t int64_t;
-typedef _uint64_t uint64_t;
-#endif
-
-#endif /* CONFIG_ARCH_RGMP */
/* Minimum-width integer types */
diff --git a/nuttx/arch/rgmp/include/com.h b/nuttx/arch/rgmp/include/x86/arch/com.h
index 918e3bd3f..89df901bb 100644
--- a/nuttx/arch/rgmp/include/com.h
+++ b/nuttx/arch/rgmp/include/x86/arch/com.h
@@ -54,6 +54,5 @@
#define COM_6_BITS 1
#define COM_5_BITS 0
-void up_serialinit(void);
#endif
diff --git a/nuttx/arch/rgmp/include/x86/arch/subarch/arch.h b/nuttx/arch/rgmp/include/x86/arch/subarch/arch.h
new file mode 100644
index 000000000..2f401c03b
--- /dev/null
+++ b/nuttx/arch/rgmp/include/x86/arch/subarch/arch.h
@@ -0,0 +1,60 @@
+/****************************************************************************
+ * arch/rgmp/include/x86/arch/subarch/arch.h
+ *
+ * Copyright (C) 2011 Yu Qiang. All rights reserved.
+ * Author: Yu Qiang <yuq825@gmail.com>
+ *
+ * This file is a part of NuttX:
+ *
+ * Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *
+ * 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 __RGMP_ARCH_SUBARCH_ARCH_H
+#define __RGMP_ARCH_SUBARCH_ARCH_H
+
+#ifndef __ASSEMBLY__
+
+#include <rgmp/arch/hpet.h>
+
+
+static inline void up_mdelay(uint32_t msec)
+{
+ hpet_ndelay(msec*1000000);
+}
+
+static inline void up_udelay(uint32_t usec)
+{
+ hpet_udelay(usec*1000);
+}
+
+#endif /* !__ASSEMBLY__ */
+
+#endif
diff --git a/nuttx/arch/rgmp/src/Makefile b/nuttx/arch/rgmp/src/Makefile
index ae2c68876..f973a28fa 100644
--- a/nuttx/arch/rgmp/src/Makefile
+++ b/nuttx/arch/rgmp/src/Makefile
@@ -34,13 +34,16 @@
############################################################################
-include $(TOPDIR)/Make.defs
+include $(CONFIG_RGMP_SUBARCH)/Make.defs
+RGMP_ARCH_ASRCS := $(addprefix $(CONFIG_RGMP_SUBARCH)/,$(RGMP_ARCH_ASRCS))
+RGMP_ARCH_CSRCS := $(addprefix $(CONFIG_RGMP_SUBARCH)/,$(RGMP_ARCH_CSRCS))
CFLAGS += -I$(TOPDIR)/sched -I$(TOPDIR)/fs
-ASRCS = sigentry.S
+ASRCS = $(RGMP_ARCH_ASRCS)
+CSRCS = nuttx.c rgmp.c bridge.c $(RGMP_ARCH_CSRCS)
AOBJS = $(ASRCS:.S=$(OBJEXT))
-CSRCS = nuttx.c rgmp.c bridge.c com.c
COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
diff --git a/nuttx/arch/rgmp/src/arm/Make.defs b/nuttx/arch/rgmp/src/arm/Make.defs
new file mode 100644
index 000000000..8185980c6
--- /dev/null
+++ b/nuttx/arch/rgmp/src/arm/Make.defs
@@ -0,0 +1,42 @@
+############################################################################
+# rgmp/arm/Make.defs
+#
+# Copyright (C) 2011 Yu Qiang. All rights reserved.
+# Author: Yu Qiang <yuq825@gmail.com>
+#
+# This file is a part of NuttX:
+#
+# Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#
+#
+# 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.
+#
+############################################################################
+
+RGMP_ARCH_ASRCS = sigentry.S
+RGMP_ARCH_CSRCS = arch_nuttx.c
diff --git a/nuttx/arch/rgmp/src/arm/arch_nuttx.c b/nuttx/arch/rgmp/src/arm/arch_nuttx.c
new file mode 100644
index 000000000..cb946bfd1
--- /dev/null
+++ b/nuttx/arch/rgmp/src/arm/arch_nuttx.c
@@ -0,0 +1,91 @@
+/****************************************************************************
+ * arch/rgmp/src/arm/arch_nuttx.c
+ *
+ * Copyright (C) 2011 Yu Qiang. All rights reserved.
+ * Author: Yu Qiang <yuq825@gmail.com>
+ *
+ * This file is a part of NuttX:
+ *
+ * Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#include <rgmp/mmu.h>
+#include <rgmp/string.h>
+
+#include <arch/arch.h>
+#include <nuttx/sched.h>
+#include <os_internal.h>
+
+
+void nuttx_arch_init(void)
+{
+
+}
+
+void nuttx_arch_exit(void)
+{
+
+}
+
+void up_initial_state(_TCB *tcb)
+{
+ struct Trapframe *tf;
+
+ if (tcb->pid != 0) {
+ tf = (struct Trapframe *)tcb->adj_stack_ptr-1;
+ memset(tf, 0, sizeof(struct Trapframe));
+ tf->tf_cpsr = SVC_MOD;
+ tf->tf_pc = (uint32_t)tcb->start;
+ tcb->xcp.tf = tf;
+ }
+}
+
+void push_xcptcontext(struct xcptcontext *xcp)
+{
+ xcp->save_eip = xcp->tf->tf_pc;
+ xcp->save_eflags = xcp->tf->tf_cpsr;
+
+ // set interrupts disabled
+ xcp->tf->tf_pc = (uint32_t)up_sigentry;
+ xcp->tf->tf_cpsr |= CPSR_IF;
+}
+
+void pop_xcptcontext(struct xcptcontext *xcp)
+{
+ xcp->tf->tf_pc = xcp->save_eip;
+ xcp->tf->tf_cpsr = xcp->save_eflags;
+}
+
+void raise(void)
+{
+
+}
+
diff --git a/nuttx/arch/rgmp/src/arm/sigentry.S b/nuttx/arch/rgmp/src/arm/sigentry.S
new file mode 100644
index 000000000..1e413450b
--- /dev/null
+++ b/nuttx/arch/rgmp/src/arm/sigentry.S
@@ -0,0 +1,49 @@
+/****************************************************************************
+ * arch/rgmp/src/arm/sigentry.S
+ *
+ * Copyright (C) 2011 Yu Qiang. All rights reserved.
+ * Author: Yu Qiang <yuq825@gmail.com>
+ *
+ * This file is a part of NuttX:
+ *
+ * Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+ .globl up_sigentry
+up_sigentry:
+ sub sp, sp, #68 @ 68 is the size of Trapframe
+ mov r0, sp
+ bl up_sigdeliver
+ add sp, sp, #4 @ skip current_task
+ pop {r0-r12, lr}
+ rfefd sp!
+
+ \ No newline at end of file
diff --git a/nuttx/arch/rgmp/src/nuttx.c b/nuttx/arch/rgmp/src/nuttx.c
index b81fc98dd..14b3e6d1d 100644
--- a/nuttx/arch/rgmp/src/nuttx.c
+++ b/nuttx/arch/rgmp/src/nuttx.c
@@ -1,5 +1,5 @@
/****************************************************************************
- * arch/rgmp/src/bridge.c
+ * arch/rgmp/src/nuttx.c
*
* Copyright (C) 2011 Yu Qiang. All rights reserved.
* Author: Yu Qiang <yuq825@gmail.com>
@@ -39,12 +39,13 @@
#include <rgmp/boot.h>
#include <rgmp/pmap.h>
-#include <rgmp/x86.h>
#include <rgmp/assert.h>
#include <rgmp/spinlock.h>
-#include <rgmp/console.h>
#include <rgmp/string.h>
-#include <rgmp/fpu.h>
+
+#include <rgmp/arch/arch.h>
+#include <rgmp/arch/console.h>
+
#include <nuttx/sched.h>
#include <nuttx/kmalloc.h>
#include <nuttx/arch.h>
@@ -52,11 +53,11 @@
#include <stdlib.h>
#include <arch/irq.h>
#include <arch/arch.h>
-#include <arch/com.h>
#include <os_internal.h>
_TCB *current_task = NULL;
+
/**
* This function is called in non-interrupt context
* to switch tasks.
@@ -75,9 +76,7 @@ static inline void up_switchcontext(_TCB *ctcb, _TCB *ntcb)
// start switch
current_task = ntcb;
- asm volatile ("int $0x40"
- ::"a"(ctcb?&ctcb->xcp.tf:NULL),
- "b"(ntcb->xcp.tf));
+ rgmp_context_switch(ctcb?&ctcb->xcp.tf:NULL, ntcb->xcp.tf);
}
void up_initialize(void)
@@ -85,7 +84,7 @@ void up_initialize(void)
extern pidhash_t g_pidhash[];
extern void up_register_bridges(void);
extern void vnet_initialize(void);
- extern void e1000_mod_init(void);
+ extern void nuttx_arch_init(void);
// intialize the current_task to g_idletcb
current_task = g_pidhash[PIDHASH(0)].tcb;
@@ -98,38 +97,15 @@ void up_initialize(void)
vnet_initialize();
#endif
- // setup COM device
- up_serialinit();
-
-#ifdef CONFIG_NET_E1000
- // setup e1000
- e1000_mod_init();
-#endif
+ nuttx_arch_init();
// enable interrupt
- sti();
+ local_irq_enable();
}
void up_idle(void)
{
- asm volatile("hlt");
-}
-
-void up_initial_state(_TCB *tcb)
-{
- struct Trapframe *tf;
-
- if (tcb->pid != 0) {
- tf = (struct Trapframe *)tcb->adj_stack_ptr-1;
- memset(tf, 0, sizeof(struct Trapframe));
- tf->tf_fpu = rgmp_fpu_init_regs;
- tf->tf_eflags = 0x00000202;
- tf->tf_cs = GD_KT;
- tf->tf_ds = GD_KD;
- tf->tf_es = GD_KD;
- tf->tf_eip = (uint32_t)tcb->start;
- tcb->xcp.tf = tf;
- }
+ arch_hlt();
}
void up_allocate_heap(void **heap_start, size_t *heap_size)
@@ -160,7 +136,7 @@ int up_create_stack(_TCB *tcb, size_t stack_size)
tcb->adj_stack_size = adj_stack_size;
tcb->stack_alloc_ptr = stack_alloc_ptr;
- tcb->adj_stack_ptr = adj_stack_ptr;
+ tcb->adj_stack_ptr = (void *)((unsigned int)adj_stack_ptr & ~7);
ret = OK;
}
return ret;
@@ -181,7 +157,7 @@ int up_use_stack(_TCB *tcb, void *stack, size_t stack_size)
tcb->adj_stack_size = adj_stack_size;
tcb->stack_alloc_ptr = stack;
- tcb->adj_stack_ptr = adj_stack_ptr;
+ tcb->adj_stack_ptr = (void *)((unsigned int)adj_stack_ptr & ~7);
return OK;
}
@@ -472,12 +448,7 @@ void up_schedule_sigaction(_TCB *tcb, sig_deliver_t sigdeliver)
// some non-running task.
else {
tcb->xcp.sigdeliver = sigdeliver;
- tcb->xcp.save_eip = tcb->xcp.tf->tf_eip;
- tcb->xcp.save_eflags = tcb->xcp.tf->tf_eflags;
-
- // Then set up to the trampoline with interrupts disabled
- tcb->xcp.tf->tf_eip = (uint32_t)up_sigentry;
- tcb->xcp.tf->tf_eflags = 0;
+ push_xcptcontext(&tcb->xcp);
}
popcli(flags);
@@ -516,14 +487,13 @@ int up_prioritize_irq(int irq, int priority)
void up_sigdeliver(struct Trapframe *tf)
{
sig_deliver_t sigdeliver;
-
- tf->tf_eip = current_task->xcp.save_eip;
- tf->tf_eflags = current_task->xcp.save_eflags;
+
+ pop_xcptcontext(&current_task->xcp);
sigdeliver = current_task->xcp.sigdeliver;
current_task->xcp.sigdeliver = NULL;
- sti();
+ local_irq_enable();
sigdeliver(current_task);
- cli();
+ local_irq_disable();
}
diff --git a/nuttx/arch/rgmp/src/rgmp.c b/nuttx/arch/rgmp/src/rgmp.c
index 6cc7aceb4..40fad1f87 100644
--- a/nuttx/arch/rgmp/src/rgmp.c
+++ b/nuttx/arch/rgmp/src/rgmp.c
@@ -37,8 +37,10 @@
*
****************************************************************************/
-#include <rgmp/x86.h>
#include <rgmp/trap.h>
+#include <rgmp/mmu.h>
+#include <rgmp/arch/arch.h>
+
#include <nuttx/config.h>
#include <nuttx/init.h>
#include <nuttx/arch.h>
@@ -66,7 +68,7 @@ void rtos_entry(void)
void *rtos_get_page(void)
{
- return memalign(4096, 4096);
+ return memalign(PTMEMSIZE, PTMEMSIZE);
}
void rtos_free_page(void *page)
@@ -86,19 +88,15 @@ void rtos_enter_interrupt(struct Trapframe *tf)
void rtos_exit_interrupt(struct Trapframe *tf)
{
- cli();
+ local_irq_disable();
nest_irq--;
if (!nest_irq) {
_TCB *rtcb = current_task;
_TCB *ntcb;
if (rtcb->xcp.sigdeliver) {
- rtcb->xcp.save_eip = tf->tf_eip;
- rtcb->xcp.save_eflags = tf->tf_eflags;
-
- // Then set up to the trampoline with interrupts disabled
- tf->tf_eip = (uint32_t)up_sigentry;
- tf->tf_eflags = 0;
+ rtcb->xcp.tf = tf;
+ push_xcptcontext(&rtcb->xcp);
}
ntcb = (_TCB*)g_readytorun.head;
// switch needed
@@ -156,16 +154,14 @@ int rtos_sem_down(void *sem)
void rtos_stop_running(void)
{
- extern void e1000_mod_exit(void);
+ extern void nuttx_arch_exit(void);
- cli();
-
-#ifdef CONFIG_NET_E1000
- e1000_mod_exit();
-#endif
+ local_irq_disable();
+
+ nuttx_arch_exit();
while(1) {
- asm volatile("hlt");
+ arch_hlt();
}
}
diff --git a/nuttx/arch/rgmp/src/x86/Make.defs b/nuttx/arch/rgmp/src/x86/Make.defs
new file mode 100644
index 000000000..5fb40006e
--- /dev/null
+++ b/nuttx/arch/rgmp/src/x86/Make.defs
@@ -0,0 +1,42 @@
+############################################################################
+# rgmp/x86/Make.defs
+#
+# Copyright (C) 2011 Yu Qiang. All rights reserved.
+# Author: Yu Qiang <yuq825@gmail.com>
+#
+# This file is a part of NuttX:
+#
+# Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#
+#
+# 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.
+#
+############################################################################
+
+RGMP_ARCH_ASRCS = sigentry.S
+RGMP_ARCH_CSRCS = com.c arch_nuttx.c
diff --git a/nuttx/arch/rgmp/src/x86/arch_nuttx.c b/nuttx/arch/rgmp/src/x86/arch_nuttx.c
new file mode 100644
index 000000000..c7b7577f3
--- /dev/null
+++ b/nuttx/arch/rgmp/src/x86/arch_nuttx.c
@@ -0,0 +1,106 @@
+/****************************************************************************
+ * arch/rgmp/src/x86/arch_nuttx.c
+ *
+ * Copyright (C) 2011 Yu Qiang. All rights reserved.
+ * Author: Yu Qiang <yuq825@gmail.com>
+ *
+ * This file is a part of NuttX:
+ *
+ * Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#include <rgmp/pmap.h>
+#include <rgmp/string.h>
+#include <rgmp/arch/fpu.h>
+
+#include <arch/arch.h>
+#include <nuttx/sched.h>
+#include <os_internal.h>
+
+
+void nuttx_arch_init(void)
+{
+ extern void e1000_mod_init(void);
+ extern void up_serialinit(void);
+
+ // setup COM device
+ up_serialinit();
+
+#ifdef CONFIG_NET_E1000
+ // setup e1000
+ e1000_mod_init();
+#endif
+
+}
+
+void nuttx_arch_exit(void)
+{
+ extern void e1000_mod_exit(void);
+
+#ifdef CONFIG_NET_E1000
+ e1000_mod_exit();
+#endif
+
+}
+
+void up_initial_state(_TCB *tcb)
+{
+ struct Trapframe *tf;
+
+ if (tcb->pid != 0) {
+ tf = (struct Trapframe *)tcb->adj_stack_ptr-1;
+ memset(tf, 0, sizeof(struct Trapframe));
+ tf->tf_fpu = rgmp_fpu_init_regs;
+ tf->tf_eflags = 0x00000202;
+ tf->tf_cs = GD_KT;
+ tf->tf_ds = GD_KD;
+ tf->tf_es = GD_KD;
+ tf->tf_eip = (uint32_t)tcb->start;
+ tcb->xcp.tf = tf;
+ }
+}
+
+void push_xcptcontext(struct xcptcontext *xcp)
+{
+ xcp->save_eip = xcp->tf->tf_eip;
+ xcp->save_eflags = xcp->tf->tf_eflags;
+
+ // set up signal entry with interrupts disabled
+ xcp->tf->tf_eip = (uint32_t)up_sigentry;
+ xcp->tf->tf_eflags = 0;
+}
+
+void pop_xcptcontext(struct xcptcontext *xcp)
+{
+ xcp->tf->tf_eip = xcp->save_eip;
+ xcp->tf->tf_eflags = xcp->save_eflags;
+}
+
diff --git a/nuttx/arch/rgmp/src/com.c b/nuttx/arch/rgmp/src/x86/com.c
index e1b8febb2..36ca15238 100644
--- a/nuttx/arch/rgmp/src/com.c
+++ b/nuttx/arch/rgmp/src/x86/com.c
@@ -1,5 +1,5 @@
/****************************************************************************
- * arch/rgmp/src/com.c
+ * arch/rgmp/src/x86/com.c
*
* Copyright (C) 2011 Yu Qiang. All rights reserved.
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
@@ -57,7 +57,7 @@
#include <arch/com.h>
#include <rgmp/trap.h>
-#include <rgmp/console.h>
+#include <rgmp/arch/console.h>
/****************************************************************************
* Pre-processor Definitions
diff --git a/nuttx/arch/rgmp/src/sigentry.S b/nuttx/arch/rgmp/src/x86/sigentry.S
index b2f9d6283..98891c026 100644
--- a/nuttx/arch/rgmp/src/sigentry.S
+++ b/nuttx/arch/rgmp/src/x86/sigentry.S
@@ -1,5 +1,5 @@
/****************************************************************************
- * arch/rgmp/src/sigentry.S
+ * arch/rgmp/src/x86/sigentry.S
*
* Copyright (C) 2011 Yu Qiang. All rights reserved.
* Author: Yu Qiang <yuq825@gmail.com>