summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorpatacongo <patacongo@42af7a65-404d-4744-a932-0658087f49c3>2011-03-05 19:28:59 +0000
committerpatacongo <patacongo@42af7a65-404d-4744-a932-0658087f49c3>2011-03-05 19:28:59 +0000
commit6218c1707eba6130302dfb8cfe253df1fafdde68 (patch)
tree85eae9be3711104ae9aa5bfc27df4e09c9ce0844
parent8a818deb60c19ee90d47b99cf12ed079f36d51b3 (diff)
downloadpx4-nuttx-6218c1707eba6130302dfb8cfe253df1fafdde68.tar.gz
px4-nuttx-6218c1707eba6130302dfb8cfe253df1fafdde68.tar.bz2
px4-nuttx-6218c1707eba6130302dfb8cfe253df1fafdde68.zip
Completes port of interrpt handling logic
git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@3340 42af7a65-404d-4744-a932-0658087f49c3
-rwxr-xr-xnuttx/arch/x86/include/i486/arch.h33
-rwxr-xr-xnuttx/arch/x86/include/i486/irq.h51
-rw-r--r--nuttx/arch/x86/src/common/up_internal.h1
-rw-r--r--nuttx/arch/x86/src/i486/i486_utils.S166
-rw-r--r--nuttx/arch/x86/src/i486/up_assert.c17
-rw-r--r--nuttx/arch/x86/src/i486/up_initialstate.c4
-rw-r--r--nuttx/arch/x86/src/i486/up_schedulesigaction.c16
-rw-r--r--nuttx/arch/x86/src/i486/up_sigdeliver.c6
-rwxr-xr-xnuttx/arch/x86/src/qemu/Make.defs6
-rw-r--r--nuttx/arch/x86/src/qemu/qemu_fullcontextrestore.S7
-rw-r--r--nuttx/arch/x86/src/qemu/qemu_handlers.c207
-rwxr-xr-xnuttx/arch/x86/src/qemu/qemu_head.S49
-rwxr-xr-xnuttx/arch/x86/src/qemu/qemu_internal.h59
-rwxr-xr-xnuttx/arch/x86/src/qemu/qemu_irq.c177
-rw-r--r--nuttx/arch/x86/src/qemu/qemu_lowsetup.c14
-rw-r--r--nuttx/arch/x86/src/qemu/qemu_saveusercontext.S5
-rwxr-xr-xnuttx/arch/x86/src/qemu/qemu_timerisr.c82
-rwxr-xr-xnuttx/arch/x86/src/qemu/qemu_vectors.S171
-rwxr-xr-xnuttx/configs/qemu-i486/include/board.h4
19 files changed, 939 insertions, 136 deletions
diff --git a/nuttx/arch/x86/include/i486/arch.h b/nuttx/arch/x86/include/i486/arch.h
index f2c270067..e2dde5555 100755
--- a/nuttx/arch/x86/include/i486/arch.h
+++ b/nuttx/arch/x86/include/i486/arch.h
@@ -85,7 +85,7 @@
* Public Types
****************************************************************************/
-/* GDT data structures
+/* GDT data structures ******************************************************
*
* The Global Descriptor Table or GDT is a data structure used by Intel x86-
* family processors starting with the 80286 in order to define the
@@ -114,7 +114,33 @@ struct gdt_entry_s
struct gdt_ptr_s
{
uint16_t limit; /* The upper 16 bits of all selector limits */
- uint32_t base; /* The address of the first gdt_entry_t struct */
+ uint32_t base; /* The address of the first GDT entry */
+} __attribute__((packed));
+
+/* IDT data structures ******************************************************
+ *
+ * The Interrupt Descriptor Table (IDT) is a data structure used by the x86
+ * architecture to implement an interrupt vector table. The IDT is used by the
+ * processor to determine the correct response to interrupts and exceptions.
+ */
+
+struct idt_entry_s
+{
+ uint16_t lobase; /* Lower 16-bits of vector address for interrupt */
+ uint16_t sel; /* Kernel segment selector */
+ uint8_t zero; /* This must always be zero */
+ uint8_t flags; /* (See documentation) */
+ uint16_t hibase; /* Upper 16-bits of vector address for interrupt */
+} __attribute__((packed));
+
+/* A struct describing a pointer to an array of interrupt handlers. This is
+ * in a format suitable for giving to 'lidt'.
+ */
+
+struct idt_ptr_s
+{
+ uint16_t limit;
+ uint32_t base; /* The address of the first GDT entry */
} __attribute__((packed));
/****************************************************************************
@@ -156,6 +182,9 @@ extern "C" {
#define EXTERN extern
#endif
+EXTERN void gdt_flush(uint32_t gdt_addr);
+EXTERN void idt_flush(uint32_t idt_addr);
+
#undef EXTERN
#ifdef __cplusplus
}
diff --git a/nuttx/arch/x86/include/i486/irq.h b/nuttx/arch/x86/include/i486/irq.h
index 0ff572df4..c82c92859 100755
--- a/nuttx/arch/x86/include/i486/irq.h
+++ b/nuttx/arch/x86/include/i486/irq.h
@@ -54,28 +54,29 @@
* Definitions
****************************************************************************/
-/* Storage order: %ebx, $esi, %edi, %ebp, sp, and return PC */
-
-#ifdef __ASSEMBLY__
-# define REG_EBX (0*4)
-# define REG_ESI (1*4)
-# define REG_EDI (2*4)
-# define REG_EBP (3*4)
-# define REG_SP (4*4)
-# define REG_PC (5*4)
-# define REG_FLAGS (6*4)
-#else
-# define REG_EBX (0)
-# define REG_ESI (1)
-# define REG_EDI (2)
-# define REG_EBP (3)
-# define REG_SP (4)
-# define REG_PC (5)
-# define REG_FLAGS (6)
-#endif /* __ASSEMBLY__ */
+/* Common register save structgure created by up_saveusercontext() and by
+ * ISR/IRQ interrupt processing.
+ */
-#define XCPTCONTEXT_REGS (7)
-#define XCPTCONTEXT_SIZE (4 * XCPTCONTEXT_REGS)
+#define REG_DS (0) /* Data segment selector */
+#define REG_EDI (1) /* Saved by pusha */
+#define REG_ESI (2) /* " " "" " " */
+#define REG_EBP (3) /* " " "" " " */
+#define REG_ESP (4) /* " " "" " " */
+#define REG_EBX (5) /* " " "" " " */
+#define REG_EDX (6) /* " " "" " " */
+#define REG_ECX (7) /* " " "" " " */
+#define REG_EAX (8) /* " " "" " " */
+#define REG_IRQNO (9) /* Interrupt number */
+#define REG_ERRCODE (10) /* Error code */
+#define REG_EIP (11) /* Pushed by process on interrupt processing */
+#define REG_CS (12) /* " " "" " " "" " " " " */
+#define REG_EFLAGS (13) /* " " "" " " "" " " " " */
+#define REG_SP (14) /* " " "" " " "" " " " " */
+#define REG_SS (15) /* " " "" " " "" " " " " */
+
+#define XCPTCONTEXT_REGS (16)
+#define XCPTCONTEXT_SIZE (4 * XCPTCONTEXT_REGS)
/****************************************************************************
* Public Types
@@ -93,10 +94,12 @@ struct xcptcontext
#ifndef CONFIG_DISABLE_SIGNALS
void *sigdeliver; /* Actual type is sig_deliver_t */
- /* These are saved copies of LR and CPSR used during signal processing. */
+ /* These are saved copies of instruction pointer and EFLAGS used during
+ * signal processing.
+ */
- uint32_t saved_pc;
- uint32_t saved_flags;
+ uint32_t saved_eip;
+ uint32_t saved_eflags;
#endif
/* Register save area */
diff --git a/nuttx/arch/x86/src/common/up_internal.h b/nuttx/arch/x86/src/common/up_internal.h
index 9730c3372..35b4e7b5c 100644
--- a/nuttx/arch/x86/src/common/up_internal.h
+++ b/nuttx/arch/x86/src/common/up_internal.h
@@ -159,7 +159,6 @@ extern int up_saveusercontext(uint32_t *saveregs);
extern void up_fullcontextrestore(uint32_t *restoreregs) __attribute__ ((noreturn));
extern void up_switchcontext(uint32_t *saveregs, uint32_t *restoreregs);
extern void up_sigdeliver(void);
-extern int up_timerisr(int irq, uint32_t *regs);
extern void up_lowputc(char ch);
extern void up_puts(const char *str);
extern void up_lowputs(const char *str);
diff --git a/nuttx/arch/x86/src/i486/i486_utils.S b/nuttx/arch/x86/src/i486/i486_utils.S
new file mode 100644
index 000000000..12a420d14
--- /dev/null
+++ b/nuttx/arch/x86/src/i486/i486_utils.S
@@ -0,0 +1,166 @@
+/****************************************************************************
+ * arch/x86/src/i486/i486_utils.S
+ *
+ * Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Based on Bran's kernel development tutorials. Rewritten for JamesM's
+ * kernel development tutorials.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#ifdef __CYGWIN__
+# define SYMBOL(s) _##s
+#else
+# define SYMBOL(s) s
+#endif
+
+/****************************************************************************
+ * Nasm
+ ****************************************************************************/
+
+#ifdef CONFIG_X86_NASM
+
+/****************************************************************************
+ * Nasm externals
+ ****************************************************************************/
+
+global SYMBOL(gdt_flush)
+global SYMBOL(idt_flush)
+
+/****************************************************************************
+ * Nasm macros
+ ****************************************************************************/
+
+/****************************************************************************
+ * Nasm .text
+ ****************************************************************************/
+
+SECTION .text
+BITS 32
+
+/****************************************************************************
+ * Name: gdt_flush
+ ****************************************************************************/
+
+SYMBOL(gdt_flush):
+ mov eax, [esp+4] /* Get the pointer to the GDT, passed as a parameter */
+ lgdt [eax] /* Load the new GDT pointer */
+
+ mov ax, 0x10 /* 0x10 is the offset in the GDT to our data segment */
+ mov ds, ax /* Load all data segment selectors */
+ mov es, ax
+ mov fs, ax
+ mov gs, ax
+ mov ss, ax
+ jmp 0x08:.gflush /* 0x08 is the offset to our code segment: Far jump! */
+.gflush:
+ ret
+
+/****************************************************************************
+ * Name: idt_flush
+ ****************************************************************************/
+
+SYMBOL(idt_flush):
+ mov eax, [esp+4] /* Get the pointer to the IDT, passed as a parameter */
+ lidt [eax] /* Load the IDT pointer */
+ ret
+
+#else /* !CONFIG_X86_NASM (GAS) */
+
+/****************************************************************************
+ * GAS
+ ****************************************************************************/
+
+ .file "i486_utils.S"
+
+/****************************************************************************
+ * GAS Globals
+ ****************************************************************************/
+
+ .globl SYMBOL(gdt_flush)
+ .globl SYMBOL(idt_flush)
+
+/****************************************************************************
+ * GAS .text
+ ****************************************************************************/
+
+ .text
+
+/****************************************************************************
+ * Name: gdt_flush
+ ****************************************************************************/
+
+#ifndef __CYGWIN__
+ .type SYMBOL(gdt_flush), @function
+#endif
+SYMBOL(gdt_flush):
+ movl %eax, 4(%esp) /* Get the pointer to the GDT, passed as a parameter */
+ lgdt (%eax) /* Load the new GDT pointer */
+
+ mov %ax, 0x10 /* 0x10 is the offset in the GDT to our data segment */
+ mov %ds, %ax /* Load all data segment selectors */
+ mov %es, %ax
+ mov %fs, %ax
+ mov %gs, %ax
+ mov %ss, %ax
+ ljmp *.Lgflush /* 0x08 is the offset to our code segment: Far jump! */
+.Lgflush:
+ ret
+#ifndef __CYGWIN__
+ .size SYMBOL(gdt_flush), . - SYMBOL(gdt_flush)
+#endif
+
+/****************************************************************************
+ * Name: idt_flush
+ ****************************************************************************/
+
+#ifndef __CYGWIN__
+ .type SYMBOL(idt_flush), @function
+#endif
+SYMBOL(idt_flush):
+ movl %eax, 4(%esp) /* Get the pointer to the IDT, passed as a parameter */
+ lidt (%eax) /* Load the IDT pointer */
+ ret
+#ifndef __CYGWIN__
+ .size SYMBOL(idt_flush), . - SYMBOL(idt_flush)
+#endif
+ .end
+#endif /* CONFIG_X86_NASM */
diff --git a/nuttx/arch/x86/src/i486/up_assert.c b/nuttx/arch/x86/src/i486/up_assert.c
index e9be9bcfc..69a6fe923 100644
--- a/nuttx/arch/x86/src/i486/up_assert.c
+++ b/nuttx/arch/x86/src/i486/up_assert.c
@@ -119,10 +119,19 @@ static inline void up_registerdump(void)
if (current_regs)
{
- lldbg("ebx:%08x esi:%08x edi:%08x ebp:%08x\n",
- current_regs[REG_EBX], current_regs[REG_ESI], current_regs[REG_EDI], current_regs[REG_EBP]);
- lldbg(" sp:%08x pc:%08x\n",
- current_regs[REG_SP], current_regs[REG_PC]);
+ lldbg(" ds:%08x irq:%08x err:%08x\n",
+ current_regs[REG_DS], current_regs[REG_IRQNO],
+ current_regs[REG_ERRCODE]);
+ lldbg("edi:%08x esi:%08x ebp:%08x esp:%08x\n",
+ current_regs[REG_EDI], current_regs[REG_ESI],
+ current_regs[REG_EBP], current_regs[REG_ESP]);
+ lldbg("ebx:%08x edx:%08x ecx:%08x eax:%08x\n",
+ current_regs[REG_EBX], current_regs[REG_EDX],
+ current_regs[REG_ECX], current_regs[REG_EAX]);
+ lldbg("eip:%08x cs:%08x flg:%08x sp:%08x ss:%08x\n",
+ current_regs[REG_EIP], current_regs[REG_CS],
+ current_regs[REG_EFLAGS], current_regs[REG_SP],
+ current_regs[REG_SS]);
}
}
#else
diff --git a/nuttx/arch/x86/src/i486/up_initialstate.c b/nuttx/arch/x86/src/i486/up_initialstate.c
index e458d35b8..7bcc222d0 100644
--- a/nuttx/arch/x86/src/i486/up_initialstate.c
+++ b/nuttx/arch/x86/src/i486/up_initialstate.c
@@ -91,14 +91,14 @@ void up_initial_state(_TCB *tcb)
/* Save the task entry point */
- xcp->regs[REG_PC] = (uint32_t)tcb->start;
+ xcp->regs[REG_EIP] = (uint32_t)tcb->start;
/* Enable or disable interrupts, based on user configuration. If the IF
* bit is set, maskable interrupts will be enabled.
*/
#ifndef CONFIG_SUPPRESS_INTERRUPTS
- xcp->regs[REG_FLAGS] = X86_FLAGS_IF;
+ xcp->regs[REG_EFLAGS] = X86_FLAGS_IF;
#endif
}
diff --git a/nuttx/arch/x86/src/i486/up_schedulesigaction.c b/nuttx/arch/x86/src/i486/up_schedulesigaction.c
index 6bcc3c656..7d91bb3e3 100644
--- a/nuttx/arch/x86/src/i486/up_schedulesigaction.c
+++ b/nuttx/arch/x86/src/i486/up_schedulesigaction.c
@@ -148,15 +148,15 @@ void up_schedule_sigaction(_TCB *tcb, sig_deliver_t sigdeliver)
*/
tcb->xcp.sigdeliver = sigdeliver;
- tcb->xcp.saved_pc = current_regs[REG_PC];
- tcb->xcp.saved_flags = current_regs[REG_FLAGS];
+ tcb->xcp.saved_eip = current_regs[REG_EIP];
+ tcb->xcp.saved_eflags = current_regs[REG_EFLAGS];
/* Then set up to vector to the trampoline with interrupts
* disabled
*/
- current_regs[REG_PC] = (uint32_t)up_sigdeliver;
- current_regs[REG_FLAGS] = 0;
+ current_regs[REG_EIP] = (uint32_t)up_sigdeliver;
+ current_regs[REG_EFLAGS] = 0;
/* And make sure that the saved context in the TCB
* is the same as the interrupt return context.
@@ -180,15 +180,15 @@ void up_schedule_sigaction(_TCB *tcb, sig_deliver_t sigdeliver)
*/
tcb->xcp.sigdeliver = sigdeliver;
- tcb->xcp.saved_pc = tcb->xcp.regs[REG_PC];
- tcb->xcp.saved_flags = tcb->xcp.regs[REG_FLAGS];
+ tcb->xcp.saved_eip = tcb->xcp.regs[REG_EIP];
+ tcb->xcp.saved_eflags = tcb->xcp.regs[REG_EFLAGS];
/* Then set up to vector to the trampoline with interrupts
* disabled
*/
- tcb->xcp.regs[REG_PC] = (uint32_t)up_sigdeliver;
- tcb->xcp.regs[REG_FLAGS] = 0;
+ tcb->xcp.regs[REG_EIP] = (uint32_t)up_sigdeliver;
+ tcb->xcp.regs[REG_EFLAGS] = 0;
}
irqrestore(flags);
diff --git a/nuttx/arch/x86/src/i486/up_sigdeliver.c b/nuttx/arch/x86/src/i486/up_sigdeliver.c
index 839943bcc..d39b2c6e1 100644
--- a/nuttx/arch/x86/src/i486/up_sigdeliver.c
+++ b/nuttx/arch/x86/src/i486/up_sigdeliver.c
@@ -101,8 +101,8 @@ void up_sigdeliver(void)
/* Save the real return state on the stack. */
up_copystate(regs, rtcb->xcp.regs);
- regs[REG_PC] = rtcb->xcp.saved_pc;
- regs[REG_FLAGS] = rtcb->xcp.saved_flags;
+ regs[REG_EIP] = rtcb->xcp.saved_eip;
+ regs[REG_EFLAGS] = rtcb->xcp.saved_eflags;
/* Get a local copy of the sigdeliver function pointer. we do this so that
* we can nullify the sigdeliver function pointer in the TCB and accept
@@ -114,7 +114,7 @@ void up_sigdeliver(void)
/* Then restore the task interrupt state */
- irqrestore(regs[REG_FLAGS]);
+ irqrestore(regs[REG_EFLAGS]);
/* Deliver the signals */
diff --git a/nuttx/arch/x86/src/qemu/Make.defs b/nuttx/arch/x86/src/qemu/Make.defs
index 0010f3f3e..e44028699 100755
--- a/nuttx/arch/x86/src/qemu/Make.defs
+++ b/nuttx/arch/x86/src/qemu/Make.defs
@@ -39,7 +39,7 @@ HEAD_ASRC = qemu_head.S
# Common x86 and i486 files
-CMN_ASRCS =
+CMN_ASRCS = i486_utils.S
CMN_CSRCS = up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \
up_createstack.c up_mdelay.c up_udelay.c up_exit.c \
up_initialize.c up_initialstate.c up_interruptcontext.c \
@@ -51,7 +51,7 @@ CMN_CSRCS = up_allocateheap.c up_assert.c up_blocktask.c up_copystate.c \
# Required QEMU files
CHIP_ASRCS = qemu_saveusercontext.S qemu_fullcontextrestore.S qemu_vectors.S
-CHIP_CSRCS = qemu_idle.c qemu_irq.c qemu_lowputc.c qemu_lowsetup.c \
- qemu_timerisr.c
+CHIP_CSRCS = qemu_handlers.c qemu_idle.c qemu_irq.c qemu_lowputc.c \
+ qemu_lowsetup.c qemu_timerisr.c
# Configuration-dependent QEMU files
diff --git a/nuttx/arch/x86/src/qemu/qemu_fullcontextrestore.S b/nuttx/arch/x86/src/qemu/qemu_fullcontextrestore.S
index 07023e5a0..8083c3b8f 100644
--- a/nuttx/arch/x86/src/qemu/qemu_fullcontextrestore.S
+++ b/nuttx/arch/x86/src/qemu/qemu_fullcontextrestore.S
@@ -46,7 +46,7 @@
#include "up_internal.h"
/**************************************************************************
- * Private Definitions
+ * Pre-processor Definitions
**************************************************************************/
#ifdef __CYGWIN__
@@ -90,6 +90,7 @@
#ifdef CONFIG_X86_NASM
# warning "No Nasm support"
#else
+ .file "qemu_fullcontextrestore.S"
.text
.globl SYMBOL(up_fullcontextrestore)
#ifndef __CYGWIN__
@@ -104,7 +105,7 @@ SYMBOL(up_fullcontextrestore):
/* Save the return address. */
- movl (REG_PC)(%ecx), %edx
+ movl (REG_EIP)(%ecx), %edx
/* Restore registers. */
@@ -116,7 +117,7 @@ SYMBOL(up_fullcontextrestore):
/* Conditionally restore interrupts */
- testl $512, (REG_FLAGS)(%ecx)
+ testl $512, (REG_EFLAGS)(%ecx)
je .Ldisabled
sti
.Ldisabled:
diff --git a/nuttx/arch/x86/src/qemu/qemu_handlers.c b/nuttx/arch/x86/src/qemu/qemu_handlers.c
new file mode 100644
index 000000000..221bfbf0f
--- /dev/null
+++ b/nuttx/arch/x86/src/qemu/qemu_handlers.c
@@ -0,0 +1,207 @@
+/****************************************************************************
+ * arch/x86/src/qemu/qemu_handlers.c
+ *
+ * Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <nuttx/arch.h>
+#include <arch/io.h>
+
+#include "up_internal.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static void idt_outb(uint8_t val, uint16_t addr) __attribute__((noinline));
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name idt_outb
+ *
+ * Description:
+ * A slightly slower version of outb
+ *
+ ****************************************************************************/
+
+static void idt_outb(uint8_t val, uint16_t addr)
+{
+ outb(val, addr);
+}
+
+/****************************************************************************
+ * Name: common_handler
+ *
+ * Description:
+ * Common logic for the ISR/IRQ handlers
+ *
+ ****************************************************************************/
+
+#ifndef CONFIG_SUPPRESS_INTERRUPTS
+static uint32_t *common_handler(int irq, uint32_t *regs)
+{
+ up_ledon(LED_INIRQ);
+
+ /* Nested interrupts are not supported in this implementation. If you want
+ * implemented nested interrupts, you would have to (1) change the way that
+ * current regs is handled and (2) the design associated with
+ * CONFIG_ARCH_INTERRUPTSTACK.
+ */
+
+ /* Current regs non-zero indicates that we are processing an interrupt;
+ * current_regs is also used to manage interrupt level context switches.
+ */
+
+ DEBUGASSERT(current_regs == NULL);
+ current_regs = regs;
+
+ /* Mask and acknowledge the interrupt */
+
+ up_maskack_irq(irq);
+
+ /* Deliver the IRQ */
+
+ irq_dispatch(irq, regs);
+
+ /* If a context switch occurred while processing the interrupt then
+ * current_regs may have change value. If we return any value different
+ * from the input regs, then the lower level will know that a context
+ * switch occurred during interrupt processing.
+ */
+
+ regs = current_regs;
+
+ /* Indicate that we are no long in an interrupt handler */
+
+ current_regs = NULL;
+
+ /* Unmask the last interrupt (global interrupts are still disabled) */
+
+ up_enable_irq(irq);
+ return regs;
+}
+#endif
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: isr_handler
+ *
+ * Description:
+ * This gets called from ISR vector handling logic in qemu_vectors.S
+ *
+ ****************************************************************************/
+
+uint32_t *isr_handler(uint32_t *regs)
+{
+#ifdef CONFIG_SUPPRESS_INTERRUPTS
+ up_ledon(LED_INIRQ);
+ PANIC(OSERR_ERREXCEPTION); /* Doesn't return */
+ return regs; /* To keep the compiler happy */
+#else
+ uint32_t *ret;
+
+ /* Dispatch the interrupt */
+
+ up_ledon(LED_INIRQ);
+ ret = common_handler((int)regs[REG_IRQNO], regs);
+ up_ledoff(LED_INIRQ);
+ return ret;
+#endif
+}
+
+/****************************************************************************
+ * Name: isr_handler
+ *
+ * Description:
+ * This gets called from IRQ vector handling logic in qemu_vectors.S
+ *
+ ****************************************************************************/
+
+uint32_t *irq_handler(uint32_t *regs)
+{
+#ifdef CONFIG_SUPPRESS_INTERRUPTS
+ up_ledon(LED_INIRQ);
+ PANIC(OSERR_ERREXCEPTION); /* Doesn't return */
+ return regs; /* To keep the compiler happy */
+#else
+ uint32_t *ret;
+ int irq;
+
+ up_ledon(LED_INIRQ);
+
+ /* Get the IRQ number */
+
+ irq = (int)regs[REG_IRQNO];
+
+ /* Send an EOI (end of interrupt) signal to the PICs if this interrupt
+ * involved the slave.
+ */
+
+ if (irq >= 40)
+ {
+ /* Send reset signal to slave */
+
+ idt_outb(0x20, 0xa0);
+ }
+
+ /* Send reset signal to master */
+
+ idt_outb(0x20, 0x20);
+
+ /* Dispatch the interrupt */
+
+ ret = common_handler(irq, regs);
+ up_ledoff(LED_INIRQ);
+ return ret;
+#endif
+}
+
diff --git a/nuttx/arch/x86/src/qemu/qemu_head.S b/nuttx/arch/x86/src/qemu/qemu_head.S
index cf754408f..bef18cd60 100755
--- a/nuttx/arch/x86/src/qemu/qemu_head.S
+++ b/nuttx/arch/x86/src/qemu/qemu_head.S
@@ -43,6 +43,12 @@
* Pre-processor definitions
****************************************************************************/
+#ifdef __CYGWIN__
+# define SYMBOL(s) _##s
+#else
+# define SYMBOL(s) s
+#endif
+
/* Memory Map: _sbss is the start of the BSS region (see ld.script) _ebss is
* the end of the BSS regsion (see ld.script). The idle task stack starts at
* the end of BSS and is of size CONFIG_IDLETHREAD_STACKSIZE. The IDLE thread
@@ -61,9 +67,9 @@
#ifdef CONFIG_X86_NASM
global __start /* Making entry point visible to linker */
-global _g_heapbase /* The start of the heap */
-extern _os_start /* os_start is defined elsewhere */
-extern _up_lowsetup /* up_lowsetup is defined elsewhere */
+global SYMBOL(g_heapbase) /* The start of the heap */
+extern SYMBOL(os_start) /* os_start is defined elsewhere */
+extern SYMBOL(up_lowsetup) /* up_lowsetup is defined elsewhere */
/* Setting up the Multiboot header - see GRUB docs for details */
@@ -92,8 +98,8 @@ __start:
/* Initialize and start NuttX */
- call _up_lowsetup /* Low-level, pre-OS initialization */
- call _os_start /* Start NuttX */
+ call SYMBOL(up_lowsetup) /* Low-level, pre-OS initialization */
+ call SYMBOL(os_start) /* Start NuttX */
/* NuttX will not return */
@@ -129,7 +135,7 @@ section .rodata
*/
align 4
-g_heapbase:
+SYMBOL(g_heapbase):
dd _ebss
#else /* !CONFIG_X86_NASM (GAS) */
@@ -138,10 +144,11 @@ g_heapbase:
* GAS .text
****************************************************************************/
+ .file "qemu_head.S"
.global __start /* Making entry point visible to linker */
- .global _os_start /* os_start is defined elsewhere */
- .global _up_lowsetup /* up_lowsetup is defined elsewhere */
- .global _g_heapbase /* The start of the heap */
+ .global SYMBOL(os_start) /* os_start is defined elsewhere */
+ .global SYMBOL(up_lowsetup) /* up_lowsetup is defined elsewhere */
+ .global SYMBOL(g_heapbase) /* The start of the heap */
/* Setting up the Multiboot header - see GRUB docs for details */
@@ -157,6 +164,9 @@ g_heapbase:
.long FLAGS
.long CHECKSUM
+#ifndef __CYGWIN__
+ .type __start, @function
+#endif
__start:
/* Set up the stack */
@@ -169,8 +179,8 @@ __start:
/* Initialize and start NuttX */
- call _up_lowsetup /* Low-level, pre-OS initialization */
- call _os_start /* Start NuttX */
+ call SYMBOL(up_lowsetup) /* Low-level, pre-OS initialization */
+ call SYMBOL(os_start) /* Start NuttX */
/* NuttX will not return */
@@ -178,6 +188,9 @@ __start:
hang:
hlt /* Halt machine should NuttX return */
jmp hang
+#ifndef __CYGWIN__
+ .size __start, . - __start
+#endif
/****************************************************************************
* .bss
@@ -189,7 +202,13 @@ hang:
* do in the system (see up_idle()).
*/
+#ifndef __CYGWIN__
+ .type idle_stack, @object
+#endif
.comm idle_stack, CONFIG_IDLETHREAD_STACKSIZE, 32
+#ifndef __CYGWIN__
+ .size idle_stack, . - idle_stack
+#endif
/****************************************************************************
* .rodata
@@ -202,7 +221,13 @@ hang:
* until the end of memory.
*/
-_g_heapbase:
+#ifndef __CYGWIN__
+ .type SYMBOL(g_heapbase), @object
+#endif
+SYMBOL(g_heapbase):
.word _ebss
+#ifndef __CYGWIN__
+ .size SYMBOL(g_heapbase), . - SYMBOL(g_heapbase)
+#endif
.end
#endif /* CONFIG_X86_NASM */ \ No newline at end of file
diff --git a/nuttx/arch/x86/src/qemu/qemu_internal.h b/nuttx/arch/x86/src/qemu/qemu_internal.h
index 601f84b77..c8bd5cd95 100755
--- a/nuttx/arch/x86/src/qemu/qemu_internal.h
+++ b/nuttx/arch/x86/src/qemu/qemu_internal.h
@@ -380,6 +380,65 @@ EXTERN void i486_dmadump(DMA_HANDLE handle, const struct i486_dmaregs_s *regs,
#endif
#endif
+/****************************************************************************
+ * Name: vector_*
+ *
+ * Description:
+ * These are the various ISR/IRQ vector address exported from
+ * qemu_vectors.S. These addresses need to have global scope so that they
+ * can be known to the interrupt intialization logic in qemu_irq.c.
+ *
+ ****************************************************************************/
+
+EXTERN void vector_isr0(void);
+EXTERN void vector_isr1(void);
+EXTERN void vector_isr2(void);
+EXTERN void vector_isr3(void);
+EXTERN void vector_isr4(void);
+EXTERN void vector_isr5(void);
+EXTERN void vector_isr6(void);
+EXTERN void vector_isr7(void);
+EXTERN void vector_isr8(void);
+EXTERN void vector_isr9(void);
+EXTERN void vector_isr10(void);
+EXTERN void vector_isr11(void);
+EXTERN void vector_isr12(void);
+EXTERN void vector_isr13(void);
+EXTERN void vector_isr14(void);
+EXTERN void vector_isr15(void);
+EXTERN void vector_isr16(void);
+EXTERN void vector_isr17(void);
+EXTERN void vector_isr18(void);
+EXTERN void vector_isr19(void);
+EXTERN void vector_isr20(void);
+EXTERN void vector_isr21(void);
+EXTERN void vector_isr22(void);
+EXTERN void vector_isr23(void);
+EXTERN void vector_isr24(void);
+EXTERN void vector_isr25(void);
+EXTERN void vector_isr26(void);
+EXTERN void vector_isr27(void);
+EXTERN void vector_isr28(void);
+EXTERN void vector_isr29(void);
+EXTERN void vector_isr30(void);
+EXTERN void vector_isr31(void);
+EXTERN void vector_irq0(void);
+EXTERN void vector_irq1(void);
+EXTERN void vector_irq2(void);
+EXTERN void vector_irq3(void);
+EXTERN void vector_irq4(void);
+EXTERN void vector_irq5(void);
+EXTERN void vector_irq6(void);
+EXTERN void vector_irq7(void);
+EXTERN void vector_irq8(void);
+EXTERN void vector_irq9(void);
+EXTERN void vector_irq10(void);
+EXTERN void vector_irq11(void);
+EXTERN void vector_irq12(void);
+EXTERN void vector_irq13(void);
+EXTERN void vector_irq14(void);
+EXTERN void vector_irq15(void);
+
#undef EXTERN
#if defined(__cplusplus)
}
diff --git a/nuttx/arch/x86/src/qemu/qemu_irq.c b/nuttx/arch/x86/src/qemu/qemu_irq.c
index 6d16cfc92..be6c4a40e 100755
--- a/nuttx/arch/x86/src/qemu/qemu_irq.c
+++ b/nuttx/arch/x86/src/qemu/qemu_irq.c
@@ -41,11 +41,13 @@
#include <nuttx/config.h>
#include <stdint.h>
+#include <string.h>
#include <debug.h>
#include <nuttx/irq.h>
#include <nuttx/arch.h>
#include <arch/irq.h>
+#include <arch/io.h>
#include "up_arch.h"
#include "os_internal.h"
@@ -57,6 +59,16 @@
****************************************************************************/
/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static void idt_outb(uint8_t val, uint16_t addr) __attribute__((noinline));
+static void up_remappic(void);
+static void up_idtentry(struct idt_entry_s *entry, uint32_t base,
+ uint16_t sel, uint8_t flags);
+static inline void up_idtinit(void);
+
+/****************************************************************************
* Public Data
****************************************************************************/
@@ -71,6 +83,159 @@ uint32_t *current_regs;
****************************************************************************/
/****************************************************************************
+ * Name idt_outb
+ *
+ * Description:
+ * A slightly slower version of outb
+ *
+ ****************************************************************************/
+
+static void idt_outb(uint8_t val, uint16_t addr)
+{
+ outb(val, addr);
+}
+
+/****************************************************************************
+ * Name up_remappic
+ *
+ * Description:
+ * Remap the PIC. The Programmable Interrupt Controller (PIC) is used to
+ * combine several sources of interrupt onto one or more CPU lines, while
+ * allowing priority levels to be assigned to its interrupt outputs. When
+ * the device has multiple interrupt outputs to assert, it will assert them
+ * in the order of their relative priority.
+ *
+ ****************************************************************************/
+
+static void up_remappic(void)
+{
+ /* Mask interrupts from PIC */
+
+ idt_outb(0xff, 0x21);
+ idt_outb(0xff, 0xA1);
+
+ /* Remap the irq table for primary */
+
+ idt_outb(0x11, 0x20);
+ idt_outb(0x20, 0x21);
+ idt_outb(0x04, 0x21);
+ idt_outb(0x01, 0x21);
+
+ /* Remap irq for slave */
+
+ idt_outb(0x11, 0xA0);
+ idt_outb(0x28, 0xA1);
+ idt_outb(0x02, 0xA1);
+ idt_outb(0x01, 0xA1);
+
+ /* Enable IRQ0 on the master with the mask */
+
+ idt_outb( 0xff, 0xA1);
+ idt_outb( 0xfe, 0x21);
+}
+
+/****************************************************************************
+ * Name up_idtentry
+ *
+ * Description:
+ * Initialize one IDT entry.
+ *
+ ****************************************************************************/
+
+static void up_idtentry(struct idt_entry_s *entry, uint32_t base,
+ uint16_t sel, uint8_t flags)
+{
+ entry->lobase = base & 0xFFFF;
+ entry->hibase = (base >> 16) & 0xFFFF;
+
+ entry->sel = sel;
+ entry->zero = 0;
+
+ /* We must uncomment the OR below when we get to using user-mode. It sets the
+ * interrupt gate's privilege level to 3.
+ */
+
+ entry->flags = flags /* | 0x60 */;
+}
+
+/****************************************************************************
+ * Name up_idtinit
+ *
+ * Description:
+ * Initialize the IDT. The Interrupt Descriptor Table (IDT) is a data
+ * structure used by the x86 architecture to implement an interrupt vector
+ * table. The IDT is used by the processor to determine the correct
+ * response to interrupts and exceptions.
+ *
+ ****************************************************************************/
+
+static inline void up_idtinit(void)
+{
+ /* This uses a ton of stack! */
+
+ struct idt_entry_s idt_entries[256];
+ struct idt_ptr_s idt_ptr;
+
+ idt_ptr.limit = sizeof(struct idt_entry_s) * 256 - 1;
+ idt_ptr.base = (uint32_t)&idt_entries;
+
+ memset(&idt_entries, 0, sizeof(struct idt_entry_s)*256);
+
+ up_remappic();
+
+ up_idtentry(&idt_entries[0], (uint32_t)vector_isr0 , 0x08, 0x8e);
+ up_idtentry(&idt_entries[1], (uint32_t)vector_isr1 , 0x08, 0x8e);
+ up_idtentry(&idt_entries[2], (uint32_t)vector_isr2 , 0x08, 0x8e);
+ up_idtentry(&idt_entries[3], (uint32_t)vector_isr3 , 0x08, 0x8e);
+ up_idtentry(&idt_entries[4], (uint32_t)vector_isr4 , 0x08, 0x8e);
+ up_idtentry(&idt_entries[5], (uint32_t)vector_isr5 , 0x08, 0x8e);
+ up_idtentry(&idt_entries[6], (uint32_t)vector_isr6 , 0x08, 0x8e);
+ up_idtentry(&idt_entries[7], (uint32_t)vector_isr7 , 0x08, 0x8e);
+ up_idtentry(&idt_entries[8], (uint32_t)vector_isr8 , 0x08, 0x8e);
+ up_idtentry(&idt_entries[9], (uint32_t)vector_isr9 , 0x08, 0x8e);
+ up_idtentry(&idt_entries[10], (uint32_t)vector_isr10, 0x08, 0x8e);
+ up_idtentry(&idt_entries[11], (uint32_t)vector_isr11, 0x08, 0x8e);
+ up_idtentry(&idt_entries[12], (uint32_t)vector_isr12, 0x08, 0x8e);
+ up_idtentry(&idt_entries[13], (uint32_t)vector_isr13, 0x08, 0x8e);
+ up_idtentry(&idt_entries[14], (uint32_t)vector_isr14, 0x08, 0x8e);
+ up_idtentry(&idt_entries[15], (uint32_t)vector_isr15, 0x08, 0x8e);
+ up_idtentry(&idt_entries[16], (uint32_t)vector_isr16, 0x08, 0x8e);
+ up_idtentry(&idt_entries[17], (uint32_t)vector_isr17, 0x08, 0x8e);
+ up_idtentry(&idt_entries[18], (uint32_t)vector_isr18, 0x08, 0x8e);
+ up_idtentry(&idt_entries[19], (uint32_t)vector_isr19, 0x08, 0x8e);
+ up_idtentry(&idt_entries[20], (uint32_t)vector_isr20, 0x08, 0x8e);
+ up_idtentry(&idt_entries[21], (uint32_t)vector_isr21, 0x08, 0x8e);
+ up_idtentry(&idt_entries[22], (uint32_t)vector_isr22, 0x08, 0x8e);
+ up_idtentry(&idt_entries[23], (uint32_t)vector_isr23, 0x08, 0x8e);
+ up_idtentry(&idt_entries[24], (uint32_t)vector_isr24, 0x08, 0x8e);
+ up_idtentry(&idt_entries[25], (uint32_t)vector_isr25, 0x08, 0x8e);
+ up_idtentry(&idt_entries[26], (uint32_t)vector_isr26, 0x08, 0x8e);
+ up_idtentry(&idt_entries[27], (uint32_t)vector_isr27, 0x08, 0x8e);
+ up_idtentry(&idt_entries[28], (uint32_t)vector_isr28, 0x08, 0x8e);
+ up_idtentry(&idt_entries[29], (uint32_t)vector_isr29, 0x08, 0x8e);
+ up_idtentry(&idt_entries[30], (uint32_t)vector_isr30, 0x08, 0x8e);
+ up_idtentry(&idt_entries[31], (uint32_t)vector_isr31, 0x08, 0x8e);
+ up_idtentry(&idt_entries[32], (uint32_t)vector_irq0, 0x08, 0x8e);
+ up_idtentry(&idt_entries[33], (uint32_t)vector_irq1, 0x08, 0x8e);
+ up_idtentry(&idt_entries[34], (uint32_t)vector_irq2, 0x08, 0x8e);
+ up_idtentry(&idt_entries[35], (uint32_t)vector_irq3, 0x08, 0x8e);
+ up_idtentry(&idt_entries[36], (uint32_t)vector_irq4, 0x08, 0x8e);
+ up_idtentry(&idt_entries[37], (uint32_t)vector_irq5, 0x08, 0x8e);
+ up_idtentry(&idt_entries[38], (uint32_t)vector_irq6, 0x08, 0x8e);
+ up_idtentry(&idt_entries[39], (uint32_t)vector_irq7, 0x08, 0x8e);
+ up_idtentry(&idt_entries[40], (uint32_t)vector_irq8, 0x08, 0x8e);
+ up_idtentry(&idt_entries[41], (uint32_t)vector_irq9, 0x08, 0x8e);
+ up_idtentry(&idt_entries[42], (uint32_t)vector_irq10, 0x08, 0x8e);
+ up_idtentry(&idt_entries[43], (uint32_t)vector_irq11, 0x08, 0x8e);
+ up_idtentry(&idt_entries[44], (uint32_t)vector_irq12, 0x08, 0x8e);
+ up_idtentry(&idt_entries[45], (uint32_t)vector_irq13, 0x08, 0x8e);
+ up_idtentry(&idt_entries[46], (uint32_t)vector_irq14, 0x08, 0x8e);
+ up_idtentry(&idt_entries[47], (uint32_t)vector_irq15, 0x08, 0x8e);
+
+ idt_flush((uint32_t)&idt_ptr);
+}
+
+/****************************************************************************
* Public Functions
****************************************************************************/
@@ -80,19 +245,13 @@ uint32_t *current_regs;
void up_irqinitialize(void)
{
- /* Disable all interrupts */
-
/* currents_regs is non-NULL only while processing an interrupt */
current_regs = NULL;
- /* Initialize logic to support a second level of interrupt decoding for
- * GPIO pins.
- */
-
-#ifdef CONFIG_GPIO_IRQ
- lpc17_gpioirqinitialize();
-#endif
+ /* Initialize the IDT */
+
+ up_idtinit();
/* And finally, enable interrupts */
diff --git a/nuttx/arch/x86/src/qemu/qemu_lowsetup.c b/nuttx/arch/x86/src/qemu/qemu_lowsetup.c
index aac19042d..bbc7f1229 100644
--- a/nuttx/arch/x86/src/qemu/qemu_lowsetup.c
+++ b/nuttx/arch/x86/src/qemu/qemu_lowsetup.c
@@ -96,11 +96,11 @@ static void up_gdtinit(void)
struct gdt_entry_s gdt_entries[5];
struct gdt_ptr_s gdt_ptr;
- up_gdtentry(0, 0, 0, 0, 0); /* Null segment */
- up_gdtentry(1, 0, 0xffffffff, 0x9a, 0xcf); /* Code segment */
- up_gdtentry(2, 0, 0xffffffff, 0x92, 0xcf); /* Data segment */
- up_gdtentry(3, 0, 0xffffffff, 0xfa, 0xcf); /* User mode code segment */
- up_gdtentry(4, 0, 0xffffffff, 0xf2, 0xcf); /* User mode data segment */
+ up_gdtentry(&gdt_entries[0], 0, 0, 0, 0); /* Null segment */
+ up_gdtentry(&gdt_entries[1], 0, 0xffffffff, 0x9a, 0xcf); /* Code segment */
+ up_gdtentry(&gdt_entries[2], 0, 0xffffffff, 0x92, 0xcf); /* Data segment */
+ up_gdtentry(&gdt_entries[3], 0, 0xffffffff, 0xfa, 0xcf); /* User mode code segment */
+ up_gdtentry(&gdt_entries[4], 0, 0xffffffff, 0xf2, 0xcf); /* User mode data segment */
gdt_ptr.limit = (sizeof(struct gdt_entry_s) * 5) - 1;
gdt_ptr.base = (uint32_t)gdt_entries;
@@ -122,7 +122,9 @@ static void up_gdtinit(void)
void up_lowsetup(void)
{
- /* Perform chip-specific initializations */
+ /* Initialize the Global descriptor table */
+
+ up_gdtinit();
/* Now perform board-specific initializations */
diff --git a/nuttx/arch/x86/src/qemu/qemu_saveusercontext.S b/nuttx/arch/x86/src/qemu/qemu_saveusercontext.S
index cc4641835..dc352c181 100644
--- a/nuttx/arch/x86/src/qemu/qemu_saveusercontext.S
+++ b/nuttx/arch/x86/src/qemu/qemu_saveusercontext.S
@@ -90,6 +90,7 @@
#ifdef CONFIG_X86_NASM
# warning "No Nasm support"
#else
+ .file "qemu_saveusercontext.S"
.text
.globl SYMBOL(up_saveusercontext)
#ifndef __CYGWIN__
@@ -113,7 +114,7 @@ SYMBOL(up_saveusercontext):
/* Save the return PC */
movl 0(%esp), %ecx
- movl %ecx, (REG_PC)(%eax)
+ movl %ecx, (REG_EIP)(%eax)
/* Save the framepointer */
@@ -123,7 +124,7 @@ SYMBOL(up_saveusercontext):
pushf
pop %ecx
- movl %ecx, (REG_FLAGS)(%eax)
+ movl %ecx, (REG_EFLAGS)(%eax)
/* And return 0 */
diff --git a/nuttx/arch/x86/src/qemu/qemu_timerisr.c b/nuttx/arch/x86/src/qemu/qemu_timerisr.c
index 072a54aa9..ddc4a5e74 100755
--- a/nuttx/arch/x86/src/qemu/qemu_timerisr.c
+++ b/nuttx/arch/x86/src/qemu/qemu_timerisr.c
@@ -4,6 +4,9 @@
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
+ * Based on Bran's kernel development tutorials. Rewritten for JamesM's
+ * kernel development tutorials.
+ *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
@@ -44,6 +47,7 @@
#include <debug.h>
#include <nuttx/arch.h>
+#include <arch/io.h>
#include <arch/board/board.h>
#include "clock_internal.h"
@@ -54,9 +58,30 @@
#include "qemu_internal.h"
/****************************************************************************
- * Definitions
+ * Pre-processor Definitions
****************************************************************************/
+/* Programmable interval timer (PIT)
+ *
+ * Fpit = Fin / divisor
+ * divisor = Fin / divisor
+ *
+ * Where:
+ *
+ * Fpit = The desired interrupt frequency.
+ * Fin = PIT input frequency (PIT_CLOCK provided in board.h)
+ *
+ * The desired timer interrupt frequency is provided by the definition CLK_TCK
+ * (see include/time.h). CLK_TCK defines the desired number of system clock
+ * ticks per second. That value is a user configurable setting that defaults
+ * to 100 (100 ticks per second = 10 MS interval).
+ */
+
+#define PIT_DIVISOR ((uint32_t)PIT_CLOCK/(uint32_t)CLK_TCK)
+
+#define PIT_MODE 0x43
+#define PIT_CH0 0x40
+
/****************************************************************************
* Private Types
****************************************************************************/
@@ -65,6 +90,43 @@
* Private Function Prototypes
****************************************************************************/
+static void pit_outb(uint8_t val, uint16_t addr) __attribute__((noinline));
+static int up_timerisr(int irq, uint32_t *regs);
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name pit_outb
+ *
+ * Description:
+ * A slightly slower version of outb
+ *
+ ****************************************************************************/
+
+static void pit_outb(uint8_t val, uint16_t addr)
+{
+ outb(val, addr);
+}
+
+/****************************************************************************
+ * Function: up_timerisr
+ *
+ * Description:
+ * The timer ISR will perform a variety of services for various portions
+ * of the systems.
+ *
+ ****************************************************************************/
+
+static int up_timerisr(int irq, uint32_t *regs)
+{
+ /* Process timer interrupt */
+
+ sched_process_timer();
+ return 0;
+}
+
/****************************************************************************
* Global Functions
****************************************************************************/
@@ -80,5 +142,21 @@
void up_timerinit(void)
{
-#warning "Not implemented"
+ /* uint32_t to avoid compile time overflow errors */
+
+ uint32_t divisor = PIT_DIVISOR;
+ DEBUGASSERT(divisor <= 0xffff);
+
+ /* Attach to the timer interrupt handler */
+
+ (void)irq_attach(IRQ0, (xcpt_t)up_timerisr);
+
+ /* Send the command byte */
+
+ pit_outb(0x36, PIT_MODE);
+
+ /* Set the PIT input frequency divisor */
+
+ pit_outb((uint8_t)(divisor & 0xff), PIT_CH0);
+ pit_outb((uint8_t)((divisor >> 8) & 0xff), PIT_CH0);
}
diff --git a/nuttx/arch/x86/src/qemu/qemu_vectors.S b/nuttx/arch/x86/src/qemu/qemu_vectors.S
index 93cac4b3d..2d66ca01c 100755
--- a/nuttx/arch/x86/src/qemu/qemu_vectors.S
+++ b/nuttx/arch/x86/src/qemu/qemu_vectors.S
@@ -47,6 +47,12 @@
* Pre-processor Definitions
****************************************************************************/
+#ifdef __CYGWIN__
+# define SYMBOL(s) _##s
+#else
+# define SYMBOL(s) s
+#endif
+
#define KSEG 0x10
/****************************************************************************
@@ -54,8 +60,17 @@
****************************************************************************/
#ifdef CONFIG_X86_NASM
-extern _irq_handler
-extern _isr_handler
+
+/****************************************************************************
+ * Nasm externals
+ ****************************************************************************/
+
+extern SYMBOL(irq_handler)
+extern SYMBOL(isr_handler)
+
+/****************************************************************************
+ * Nasm macros
+ ****************************************************************************/
/* Trace macros, use like trace 'i' to print char to serial port. */
@@ -74,8 +89,8 @@ extern _isr_handler
*/
%macro ISR_NOERRCODE 1
- global vector_isr%1
- vector_isr%1:
+ global SYMBOL(vector_isr%1)
+ SYMBOL(vector_isr%1):
cli /* Disable interrupts firstly. */
push byte 0 /* Push a dummy error code. */
push byte %1 /* Push the interrupt number. */
@@ -87,8 +102,8 @@ extern _isr_handler
*/
%macro ISR_ERRCODE 1
- global vector_isr%1
- vector_isr%1:
+ global SYMBOL(vector_isr%1)
+ SYMBOL(vector_isr%1):
cli /* Disable interrupts. */
push byte %1 /* Push the interrupt number */
jmp isr_common
@@ -99,14 +114,18 @@ extern _isr_handler
*/
%macro IRQ 2
- global vector_irq%1
- vector_irq%1:
+ global SYMBOL(vector_irq%1)
+ SYMBOL(vector_irq%1):
cli
push byte 0
push byte %2
jmp irq_common
%endmacro
+/****************************************************************************
+ * Nasm vectors
+ ****************************************************************************/
+
/* The following will be the vector address programmed into the IDT */
ISR_NOERRCODE ISR0
@@ -158,10 +177,15 @@ IRQ 13, IRQ13
IRQ 14, IRQ14
IRQ 15, IRQ15
-/* This is our common ISR stub. It saves the processor state, sets up for
- * kernel mode segments, calls the C-level fault handler, and finally restores
- * the stack frame.
- */
+/****************************************************************************
+ * Name: isr_common
+ *
+ * Description:
+ * This is the common ISR logic. It saves the processor state, sets up for
+ * kernel mode segments, calls the C-level fault handler, and finally
+ * restores the stack frame.
+ *
+ ****************************************************************************/
isr_common:
/* trace 'S' */
@@ -176,7 +200,7 @@ isr_common:
mov fs, ax
mov gs, ax
- call _isr_handler
+ call SYMBOL(isr_handler)
pop ebx /* Reload the original data segment descriptor */
mov ds, bx
@@ -189,10 +213,15 @@ isr_common:
sti
iret /* Pops 5 things at once: CS, EIP, EFLAGS, SS, and ESP */
-/* This is our common IRQ stub. It saves the processor state, sets up for
- * kernel mode segments, calls the C-level fault handler, and finally restores
- * the stack frame.
- */
+/****************************************************************************
+ * Name: irq_common
+ *
+ * Description:
+ * This is the common IRQ logic. It saves the processor state, sets up for
+ * kernel mode segments, calls the C-level fault handler, and finally
+ * restores the stack frame.
+ *
+ ****************************************************************************/
irq_common:
/* trace 'R' */
@@ -207,7 +236,7 @@ irq_common:
mov fs, ax
mov gs, ax
- call _irq_handler
+ call SYMBOL(irq_handler)
pop ebx /* Reload the original data segment descriptor */
mov ds, bx
@@ -226,8 +255,18 @@ irq_common:
* GAS .text
****************************************************************************/
- .globl _irq_handler
- .globl _isr_handler
+ .file "qemu_vectors.S"
+
+/****************************************************************************
+ * GAS globals
+ ****************************************************************************/
+
+ .globl SYMBOL(irq_handler)
+ .globl SYMBOL(isr_handler)
+
+/****************************************************************************
+ * GAS macros
+ ****************************************************************************/
/* Trace macros, use like trace 'i' to print char to serial port. */
@@ -246,8 +285,8 @@ irq_common:
*/
.macro ISR_NOERRCODE, intno
- .globl vector_isr\intno
-vector_isr\intno:
+ .globl SYMBOL(vector_isr\intno)
+SYMBOL(vector_isr\intno):
cli /* Disable interrupts firstly. */
push $0 /* Push a dummy error code. */
push $\intno /* Push the interrupt number. */
@@ -259,8 +298,8 @@ vector_isr\intno:
*/
.macro ISR_ERRCODE, intno
- .globl vector_isr\intno
-vector_isr\intno:
+ .globl SYMBOL(vector_isr\intno)
+SYMBOL(vector_isr\intno):
cli /* Disable interrupts firstly. */
push $\intno /* Push the interrupt number. */
jmp isr_common /* Go to the common handler code. */
@@ -271,8 +310,8 @@ vector_isr\intno:
*/
.macro IRQ, irqno, intno
- .globl vector_irq\irqno
-vector_irq\irqno:
+ .globl SYMBOL(vector_irq\irqno)
+SYMBOL(vector_irq\irqno):
cli /* Disable interrupts firstly. */
push $0 /* Push a dummy error code. */
push $\intno /* Push the interrupt number. */
@@ -330,66 +369,88 @@ vector_irq\irqno:
IRQ 14, IRQ14
IRQ 15, IRQ15
-/* This is our common ISR stub. It saves the processor state, sets up for
- * kernel mode segments, calls the C-level fault handler, and finally restores
- * the stack frame.
- */
+/****************************************************************************
+ * Name: isr_common
+ *
+ * Description:
+ * This is the common ISR logic. It saves the processor state, sets up for
+ * kernel mode segments, calls the C-level fault handler, and finally
+ * restores the stack frame.
+ *
+ ****************************************************************************/
+#ifndef __CYGWIN__
+ .type isr_common, @function
+#endif
isr_common:
/* trace 'S' */
pusha /* Pushes edi,esi,ebp,esp,ebx,edx,ecx,eax */
- mov %ax, ds /* Lower 16-bits of eax = ds. */
+ mov %ax, %ds /* Lower 16-bits of eax = ds. */
pushl %eax /* Save the data segment descriptor */
mov %ax, KSEG /* Load the kernel data segment descriptor */
- mov ds, %ax
- mov es, %ax
- mov fs, %ax
- mov gs, %ax
+ mov %ds, %ax
+ mov %es, %ax
+ mov %fs, %ax
+ mov %gs, %ax
- call _isr_handler
+ call SYMBOL(isr_handler)
- pop ebx /* Reload the original data segment descriptor */
- mov ds, %bx
- mov es, %bx
- mov fs, %bx
- mov gs, %bx
+ pop %ebx /* Reload the original data segment descriptor */
+ mov %ds, %bx
+ mov %es, %bx
+ mov %fs, %bx
+ mov %gs, %bx
popa /* Pops edi,esi,ebp... */
add %esp, 8 /* Cleans up the pushed error code and pushed ISR number */
sti
+#ifndef __CYGWIN__
+ .size isr_common, . - isr_common
+#endif
iret /* Pops 5 things at once: CS, EIP, EFLAGS, SS, and ESP */
-/* This is our common IRQ stub. It saves the processor state, sets up for
- * kernel mode segments, calls the C-level fault handler, and finally restores
- * the stack frame.
- */
+/****************************************************************************
+ * Name: irq_common
+ *
+ * Description:
+ * This is the common IRQ logic. It saves the processor state, sets up for
+ * kernel mode segments, calls the C-level fault handler, and finally
+ * restores the stack frame.
+ *
+ ****************************************************************************/
+#ifndef __CYGWIN__
+ .type irq_common, @function
+#endif
irq_common:
/* trace 'R' */
pusha /* Pushes edi,esi,ebp,esp,ebx,edx,ecx,eax */
- mov %ax, ds /* Lower 16-bits of eax = ds. */
+ mov %ax, %ds /* Lower 16-bits of eax = ds. */
push %eax /* Save the data segment descriptor */
mov %ax, KSEG /* Load the kernel data segment descriptor */
- mov ds, %ax
- mov es, %ax
- mov fs, %ax
- mov gs, %ax
+ mov %ds, %ax
+ mov %es, %ax
+ mov %fs, %ax
+ mov %gs, %ax
- call _irq_handler
+ call SYMBOL(irq_handler)
pop %ebx /* Reload the original data segment descriptor */
- mov ds, %bx
- mov es, %bx
- mov fs, %bx
- mov gs, %bx
+ mov %ds, %bx
+ mov %es, %bx
+ mov %fs, %bx
+ mov %gs, %bx
popa /* Pops edi,esi,ebp... */
add %esp, 8 /* Cleans up the pushed error code and pushed ISR number */
sti
iret /* Pops 5 things at once: CS, EIP, EFLAGS, SS, and ESP */
+#ifndef __CYGWIN__
+ .size irq_common, . - irq_common
+#endif
.end
#endif /* CONFIG_X86_NASM */
diff --git a/nuttx/configs/qemu-i486/include/board.h b/nuttx/configs/qemu-i486/include/board.h
index 35f98987c..31ae72dc4 100755
--- a/nuttx/configs/qemu-i486/include/board.h
+++ b/nuttx/configs/qemu-i486/include/board.h
@@ -49,6 +49,10 @@
/* Clocking *************************************************************************/
+/* Programmable interval timer (PIT) */
+
+#define PIT_CLOCK 1041816 /* PIT input clock */
+
/* LED definitions ******************************************************************/
/* Button definitions ***************************************************************/