From 57cbb33b4853e6a5cc4b035eecc6c691c48eaa7e Mon Sep 17 00:00:00 2001 From: patacongo Date: Mon, 10 Dec 2012 18:40:01 +0000 Subject: Add source files for z180 git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@5426 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/z80/src/z180/Kconfig | 55 ++++ nuttx/arch/z80/src/z180/Make.defs | 55 ++++ nuttx/arch/z80/src/z180/README.txt | 49 ++++ nuttx/arch/z80/src/z180/Toolchain.defs | 75 +++++ nuttx/arch/z80/src/z180/chip.h | 54 ++++ nuttx/arch/z80/src/z180/switch.h | 166 +++++++++++ nuttx/arch/z80/src/z180/up_mem.h | 75 +++++ nuttx/arch/z80/src/z180/z180_copystate.c | 78 ++++++ nuttx/arch/z80/src/z180/z180_head.asm | 304 +++++++++++++++++++++ nuttx/arch/z80/src/z180/z180_initialstate.c | 91 ++++++ nuttx/arch/z80/src/z180/z180_io.c | 94 +++++++ nuttx/arch/z80/src/z180/z180_irq.c | 113 ++++++++ nuttx/arch/z80/src/z180/z180_registerdump.c | 91 ++++++ .../arch/z80/src/z180/z180_restoreusercontext.asm | 104 +++++++ nuttx/arch/z80/src/z180/z180_rom.asm | 297 ++++++++++++++++++++ nuttx/arch/z80/src/z180/z180_saveusercontext.asm | 143 ++++++++++ nuttx/arch/z80/src/z180/z180_schedulesigaction.c | 192 +++++++++++++ nuttx/arch/z80/src/z180/z180_sigdeliver.c | 139 ++++++++++ 18 files changed, 2175 insertions(+) create mode 100644 nuttx/arch/z80/src/z180/Kconfig create mode 100644 nuttx/arch/z80/src/z180/Make.defs create mode 100644 nuttx/arch/z80/src/z180/README.txt create mode 100644 nuttx/arch/z80/src/z180/Toolchain.defs create mode 100644 nuttx/arch/z80/src/z180/chip.h create mode 100644 nuttx/arch/z80/src/z180/switch.h create mode 100644 nuttx/arch/z80/src/z180/up_mem.h create mode 100644 nuttx/arch/z80/src/z180/z180_copystate.c create mode 100644 nuttx/arch/z80/src/z180/z180_head.asm create mode 100644 nuttx/arch/z80/src/z180/z180_initialstate.c create mode 100644 nuttx/arch/z80/src/z180/z180_io.c create mode 100644 nuttx/arch/z80/src/z180/z180_irq.c create mode 100644 nuttx/arch/z80/src/z180/z180_registerdump.c create mode 100644 nuttx/arch/z80/src/z180/z180_restoreusercontext.asm create mode 100644 nuttx/arch/z80/src/z180/z180_rom.asm create mode 100644 nuttx/arch/z80/src/z180/z180_saveusercontext.asm create mode 100644 nuttx/arch/z80/src/z180/z180_schedulesigaction.c create mode 100644 nuttx/arch/z80/src/z180/z180_sigdeliver.c (limited to 'nuttx/arch/z80/src/z180') diff --git a/nuttx/arch/z80/src/z180/Kconfig b/nuttx/arch/z80/src/z180/Kconfig new file mode 100644 index 000000000..4eafa8da8 --- /dev/null +++ b/nuttx/arch/z80/src/z180/Kconfig @@ -0,0 +1,55 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# + +if ARCH_CHIP_Z180 + +choice + prompt "Toolchain Selection" + default Z180_TOOLCHAIN_SDCCW if HOST_WINDOWS + default Z180_TOOLCHAIN_SDCCL if !HOST_WINDOWS + +config Z180_TOOLCHAIN_SDCCL + bool "SDCC for Linux, MAC OSX, or Cygwin" + depends on !WINDOWS_NATIVE + +config Z180_TOOLCHAIN_SDCCW + bool "SDCC for Windows" + depends on HOST_WINDOWS + +endchoice + +config LINKER_HOME_AREA + hex "Start of _HOME area" + default 0x0000 + ---help--- + Start of the linker HOME area. Default: 0x0000 + +config LINKER_CODE_AREA + hex "Start of _CODE area" + default 0x0200 + ---help--- + Start of the linker _CODE area. Default: 0x0200 + +config LINKER_DATA_AREA + hex "Start of _DATA area" + default 0x8000 + ---help--- + Start of the linker _DATA area. Default: 0x8000 + +config LINKER_ROM_AT_0000 + bool "ROM at 0x0000" + default n + ---help--- + Some architectures may have ROM located at address zero. In this + case, a special version of the "head" file must be used. + +config ARCH_HAVEHEAD + bool "Board-specific Head File" + default n + ---help--- + Use a board-specific version of the "head" file in the + configs//src directory + +endif diff --git a/nuttx/arch/z80/src/z180/Make.defs b/nuttx/arch/z80/src/z180/Make.defs new file mode 100644 index 000000000..d371a5d22 --- /dev/null +++ b/nuttx/arch/z80/src/z180/Make.defs @@ -0,0 +1,55 @@ +############################################################################ +# arch/z80/src/z180/Make.defs +# +# Copyright (C) 2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +ifeq ($(CONFIG_ARCH_HAVEHEAD),) +ifeq ($(CONFIG_LINKER_ROM_AT_0000),y) +HEAD_ASRC = z180_rom.asm +else +HEAD_ASRC = z180_head.asm +endif +endif + +CMN_ASRCS = +CMN_CSRCS = up_initialize.c up_allocateheap.c up_createstack.c \ + up_releasestack.c up_interruptcontext.c up_blocktask.c \ + up_unblocktask.c up_exit.c up_releasepending.c \ + up_reprioritizertr.c up_idle.c up_assert.c up_doirq.c \ + up_mdelay.c up_udelay.c up_usestack.c + +CHIP_ASRCS = z180_saveusercontext.asm z180_restoreusercontext.asm +CHIP_CSRCS = z180_initialstate.c z180_io.c z180_irq.c z180_copystate.c \ + z180_schedulesigaction.c z180_sigdeliver.c \ + z180_registerdump.c + diff --git a/nuttx/arch/z80/src/z180/README.txt b/nuttx/arch/z80/src/z180/README.txt new file mode 100644 index 000000000..c06164cdd --- /dev/null +++ b/nuttx/arch/z80/src/z180/README.txt @@ -0,0 +1,49 @@ +arch/z80/src/z180 +^^^^^^^^^^^^^^^^^ + +The arch/z80 directories contain files to support a variety of 8-bit architectures +from ZiLOG (and spin-architectures such as the Rabbit2000). The arch/z80/src/z180 +sub-directory contains logic unique to the classic Z180 family of chips. + +Files in this directory include: + +z180_head.asm + This is the main entry point into the Z180 program. This includes the + handler for the RESET, power-up interrupt vector and address zero and all + RST interrupts. + +z180_rom.asm + Some architectures may have ROM located at address zero. In this case, a + special version of the "head" logic must be used. This special "head" + file is probably board-specific and, hence, belongs in the board-specific + configs//src directory. This file may, however, be used as + a model for such a board-specific file. + + z180_rom.S is enabled by specifying CONFIG_LINKER_ROM_AT_0000 in the + configuration file. + + A board specific version in the configs//src directory can be + used by: + + 1. Define CONFIG_ARCH_HAVEHEAD + 2. Add the board-specific head file, say .asm, to + configs//src + 3. Add a file called Make.defs in the configs//src directory + containing the line: HEAD_ASRC = .asm + +Make.defs + This is the standard makefile fragment that must be provided in all + chip directories. This fragment identifies the chip-specific file to + be used in building libarch. + +chip.h + This is the standard header file that must be provided in all chip + directories. + +z180_initialstate.c, z180_copystate.c, z180_restoreusercontext.asm, and +z180_saveusercontext.asm, switch + These files implement the Z180 context switching logic + +z180_schedulesigaction.c and z180_sigdeliver.c + These files implement Z180 signal handling. + diff --git a/nuttx/arch/z80/src/z180/Toolchain.defs b/nuttx/arch/z80/src/z180/Toolchain.defs new file mode 100644 index 000000000..3979f3f0e --- /dev/null +++ b/nuttx/arch/z80/src/z180/Toolchain.defs @@ -0,0 +1,75 @@ +############################################################################ +# arch/z80/src/z180/Toolchain.defs +# +# Copyright (C) 2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# Setup for the selected toolchain + +# +# SDCC is currently the only z180 toolchain supported. See +# http://sdcc.sourceforge.net/. Source and pre-built SDCC binaries can be +# downloaded from the SDCC SourceForge site: +# http://sourceforge.net/projects/sdcc/files/. Pre-built binaries are +# available for Linux, MAC OSX, and for Win32. In addition, SDCC can be +# built to run on Windows as a POSIX toolchain. The various SDCC options are +# selected in the NuttX configuration with: +# +# CONFIG_Z180_TOOLCHAIN_SDCCL=y : SDCC for Linux, MAC OSX or Cygwin +# CONFIG_Z180_TOOLCHAIN_SDCCW=y : SDCC for Win32 +# + +ifeq ($(filter y, \ + $(CONFIG_Z180_TOOLCHAIN_SDCCL) \ + ),y) + CONFIG_Z180_TOOLCHAIN ?= SDCCPOSIX +endif +ifeq ($(filter y, \ + $(CONFIG_Z180_TOOLCHAIN_SDCCW) \ + ),y) + CONFIG_Z180_TOOLCHAIN ?= SDCCWIN32 +endif + +# SDCC toolchain under Linux, MAC OSX or Cygwin + +ifeq ($(CONFIG_Z180_TOOLCHAIN),POSIX) +endif + +# SDCC toolchain under Windows/Cygwin + +ifeq ($(CONFIG_AVR_TOOLCHAIN),SDCCWIN32) + ifneq ($(CONFIG_WINDOWS_NATIVE),y) + WINTOOL = y + endif +endif + + diff --git a/nuttx/arch/z80/src/z180/chip.h b/nuttx/arch/z80/src/z180/chip.h new file mode 100644 index 000000000..b766654f6 --- /dev/null +++ b/nuttx/arch/z80/src/z180/chip.h @@ -0,0 +1,54 @@ +/************************************************************************************ + * arch/z80/src/z180/chip.h + * arch/z80/src/chip/chip.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +#ifndef __ARCH_Z80_SRC_Z180_CHIP_H +#define __ARCH_Z80_SRC_Z180_CHIP_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ + +#endif /* __ARCH_Z80_SRC_Z180_CHIP_H */ diff --git a/nuttx/arch/z80/src/z180/switch.h b/nuttx/arch/z80/src/z180/switch.h new file mode 100644 index 000000000..38054e4b8 --- /dev/null +++ b/nuttx/arch/z80/src/z180/switch.h @@ -0,0 +1,166 @@ +/************************************************************************************ + * arch/z80/src/z180/switch.h + * arch/z80/src/chip/switch.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +#ifndef __ARCH_Z80_SRC_Z180_SWITCH_H +#define __ARCH_Z80_SRC_Z180_SWITCH_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include + +#include "up_internal.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Macros for portability *********************************************************** + * + * Common logic in arch/z80/src/common is customized for the z180 context switching + * logic via the following macros. + */ + +/* Initialize the IRQ state */ + +#define INIT_IRQCONTEXT() current_regs = NULL + +/* IN_INTERRUPT returns true if the system is currently operating in the interrupt + * context. IN_INTERRUPT is the inline equivalent of up_interrupt_context(). + */ + +#define IN_INTERRUPT() (current_regs != NULL) + +/* The following macro is used when the system enters interrupt handling logic */ + +#define DECL_SAVESTATE() \ + FAR chipreg_t *savestate + +#define IRQ_ENTER(irq, regs) \ + do { \ + savestate = (FAR chipreg_t *)current_regs; \ + current_regs = (regs); \ + } while (0) + +/* The following macro is used when the system exits interrupt handling logic */ + +#define IRQ_LEAVE(irq) current_regs = savestate + +/* The following macro is used to sample the interrupt state (as a opaque handle) */ + +#define IRQ_STATE() (current_regs) + +/* Save the current IRQ context in the specified TCB */ + +#define SAVE_IRQCONTEXT(tcb) z180_copystate((tcb)->xcp.regs, (FAR chipreg_t*)current_regs) + +/* Set the current IRQ context to the state specified in the TCB */ + +#define SET_IRQCONTEXT(tcb) z180_copystate((FAR chipreg_t*)current_regs, (tcb)->xcp.regs) + +/* Save the user context in the specified TCB. User context saves can be simpler + * because only those registers normally saved in a C called need be stored. + */ + +#define SAVE_USERCONTEXT(tcb) z180_saveusercontext((tcb)->xcp.regs) + +/* Restore the full context -- either a simple user state save or the full, + * IRQ state save. + */ + +#define RESTORE_USERCONTEXT(tcb) z180_restoreusercontext((tcb)->xcp.regs) + +/* Dump the current machine registers */ + +#define _REGISTER_DUMP() z180_registerdump() + +/************************************************************************************ + * Public Types + ************************************************************************************/ + +/************************************************************************************ + * Public Variables + ************************************************************************************/ + +#ifndef __ASSEMBLY__ +/* This holds a references to the current interrupt level register storage structure. + * If is non-NULL only during interrupt processing. + */ + +extern volatile chipreg_t *current_regs; +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ + +#ifndef __ASSEMBLY__ +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/* Defined in z180_copystate.c */ + +EXTERN void z180_copystate(FAR chipreg_t *dest, FAR const chipreg_t *src); + +/* Defined in z180_saveusercontext.asm */ + +EXTERN int z180_saveusercontext(FAR chipreg_t *regs); + +/* Defined in z180_restoreusercontext.asm */ + +EXTERN void z180_restoreusercontext(FAR chipreg_t *regs); + +/* Defined in z180_sigsetup.c */ + +EXTERN void z180_sigsetup(FAR _TCB *tcb, sig_deliver_t sigdeliver, FAR chipreg_t *regs); + +/* Defined in z180_registerdump.c */ + +EXTERN void z180_registerdump(void); + +#undef EXTERN +#ifdef __cplusplus +} +#endif +#endif + +#endif /* __ARCH_Z80_SRC_Z180_SWITCH_H */ diff --git a/nuttx/arch/z80/src/z180/up_mem.h b/nuttx/arch/z80/src/z180/up_mem.h new file mode 100644 index 000000000..e414c547c --- /dev/null +++ b/nuttx/arch/z80/src/z180/up_mem.h @@ -0,0 +1,75 @@ +/************************************************************************************ + * common/sdcc.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ + +#ifndef __ARCH_Z180_SRC_COMMON_UP_MEM_H +#define __ARCH_Z180_SRC_COMMON_UP_MEM_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Locate the IDLE thread stack at the end of RAM. */ + +#define CONFIG_STACK_END CONFIG_DRAM_SIZE +#define CONFIG_STACK_BASE (CONFIG_STACK_END - CONFIG_IDLETHREAD_STACKSIZE) + +/* The heap then extends from the linker determined beginning of the heap (s__HEAP). + * to the bottom of the IDLE thread stack. NOTE: The symbol s__HEAP is not + * accessible from C because it does not begin with the _ character. g_heapbase + * is defined in z180_head.asm to provide that value to the C code. + */ + +#define CONFIG_HEAP1_END CONFIG_STACK_BASE +#define CONFIG_HEAP1_BASE g_heapbase + +/************************************************************************************ + * Public variables + ************************************************************************************/ + +/* This is the bottom of the heap as provided by the linker symbol s__HEAP. NOTE: + * The symbol s__HEAP is not accessible from C because it does not begin with the _ + * character. g_heapbase is defined in z180_head.asm to provide that value to the C + * code. + */ + +extern const uint16_t g_heapbase; + +#endif /* __ARCH_Z180_SRC_COMMON_UP_MEM_H */ diff --git a/nuttx/arch/z80/src/z180/z180_copystate.c b/nuttx/arch/z80/src/z180/z180_copystate.c new file mode 100644 index 000000000..a088f8d40 --- /dev/null +++ b/nuttx/arch/z80/src/z180/z180_copystate.c @@ -0,0 +1,78 @@ +/**************************************************************************** + * arch/z80/src/z180/z180_copystate.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include "chip/switch.h" +#include "os_internal.h" +#include "up_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: z180_copystate + ****************************************************************************/ + +/* Maybe a little faster than most memcpy's */ + +void z180_copystate(FAR chipreg_t *dest, FAR const chipreg_t *src) +{ + int i; + for (i = 0; i < XCPTCONTEXT_REGS; i++) + { + *dest++ = *src++; + } +} + diff --git a/nuttx/arch/z80/src/z180/z180_head.asm b/nuttx/arch/z80/src/z180/z180_head.asm new file mode 100644 index 000000000..ca064160a --- /dev/null +++ b/nuttx/arch/z80/src/z180/z180_head.asm @@ -0,0 +1,304 @@ +;************************************************************************** +; arch/z80/src/z180/z180_head.asm +; +; Copyright (C) 2012 Gregory Nutt. All rights reserved. +; Author: Gregory Nutt +; +; Redistribution and use in source and binary forms, with or without +; modification, are permitted provided that the following conditions +; are met: +; +; 1. Redistributions of source code must retain the above copyright +; notice, this list of conditions and the following disclaimer. +; 2. Redistributions in binary form must reproduce the above copyright +; notice, this list of conditions and the following disclaimer in +; the documentation and/or other materials provided with the +; distribution. +; 3. Neither the name NuttX nor the names of its contributors may be +; used to endorse or promote products derived from this software +; without specific prior written permission. +; +; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +; "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +; LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +; FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +; COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +; INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +; BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +; OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +; AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +; LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +; ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +; POSSIBILITY OF SUCH DAMAGE. +; +;************************************************************************** + + .title NuttX for the Z180 + .module z180_head + +;************************************************************************** +; Constants +;************************************************************************** + + ; Register save area layout + + XCPT_I == 0 ; Offset 0: Saved I w/interrupt state in carry + XCPT_BC == 2 ; Offset 1: Saved BC register + XCPT_DE == 4 ; Offset 2: Saved DE register + XCPT_IX == 6 ; Offset 3: Saved IX register + XCPT_IY == 8 ; Offset 4: Saved IY register + XCPT_SP == 10 ; Offset 5: Offset to SP at time of interrupt + XCPT_HL == 12 ; Offset 6: Saved HL register + XCPT_AF == 14 ; Offset 7: Saved AF register + XCPT_PC == 16 ; Offset 8: Offset to PC at time of interrupt + + ; Default stack base (needs to be fixed) + + .include "asm_mem.h" + +;************************************************************************** +; Global symbols used +;************************************************************************** + + .globl _os_start ; OS entry point + .globl _up_doirq ; Interrupt decoding logic + +;************************************************************************** +; Reset entry point +;************************************************************************** + + .area _HEADER (ABS) + .org 0x0000 + + di ; Disable interrupts + im 1 ; Set interrupt mode 1 + jr _up_reset ; And boot the system + +;************************************************************************** +; Other reset handlers +; +; Interrupt mode 1 behavior: +; +; 1. M1 cycle: 7 ticks +; Acknowledge interrupt and decrements SP +; 2. M2 cycle: 3 ticks +; Writes the MS byte of the PC onto the stack and decrements SP +; 3. M3 cycle: 3 ticks +; Writes the LS byte of the PC onto the stack and sets the PC to 0x0038. +; +;************************************************************************** + + .org 0x0008 ; RST 1 + + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #1 ; 1 = Z180_RST1 + jr _up_rstcommon ; Remaining RST handling is common + + .org 0x0010 ; RST 2 + + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #2 ; 2 = Z180_RST2 + jr _up_rstcommon ; Remaining RST handling is common + + .org 0x0018 ; RST 3 + + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #3 ; 1 = Z180_RST3 + jr _up_rstcommon ; Remaining RST handling is common + + .org 0x0020 ; RST 4 + + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #4 ; 1 = Z180_RST4 + jr _up_rstcommon ; Remaining RST handling is common + + .org 0x0028 ; RST 5 + + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #5 ; 1 = Z180_RST5 + jr _up_rstcommon ; Remaining RST handling is common + + .org 0x0030 ; RST 6 + + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #6 ; 1 = Z180_RST6 + jr _up_rstcommon ; Remaining RST handling is common + + .org 0x0038 ; Int mode 1 / RST 7 + + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #7 ; 7 = Z180_RST7 + jr _up_rstcommon ; Remaining RST handling is common + +;************************************************************************** +; NMI interrupt handler +;************************************************************************** + + .org 0x0066 + retn + +;************************************************************************** +; System start logic +;************************************************************************** + +_up_reset: + ; Set up the stack pointer at the location determined the Makefile + ; and stored in asm_mem.h + + ld SP, #CONFIG_STACK_END ; Set stack pointer + + ; Performed initialization unique to the SDCC toolchain + + call gsinit ; Initialize the data section + + ; Then start NuttX + + call _os_start ; jump to the OS entry point + + ; NuttX will never return, but just in case... + +_up_halt:: + halt ; We should never get here + jp _up_halt + +;************************************************************************** +; Common Interrupt handler +;************************************************************************** + +_up_rstcommon:: + ; Create a register frame. SP points to top of frame + 4, pushes + ; decrement the stack pointer. Already have + ; + ; Offset 8: Return PC is already on the stack + ; Offset 7: AF (retaining flags) + ; + ; IRQ number is in A + + push hl ; Offset 6: HL + ld hl, #(3*2) ; HL is the value of the stack pointer before + add hl, sp ; the interrupt occurred + push hl ; Offset 5: Stack pointer + push iy ; Offset 4: IY + push ix ; Offset 3: IX + push de ; Offset 2: DE + push bc ; Offset 1: BC + + ld b, a ; Save the reset number in B + ld a, i ; Parity bit holds interrupt state + push af ; Offset 0: I with interrupt state in parity + di + + ; Call the interrupt decode logic. SP points to the beginning of the reg structure + + ld hl, #0 ; Argument #2 is the beginning of the reg structure + add hl, sp ; + push hl ; Place argument #2 at the top of stack + push bc ; Argument #1 is the Reset number + inc sp ; (make byte sized) + call _up_doirq ; Decode the IRQ + + ; On return, HL points to the beginning of the reg structure to restore + ; Note that (1) the arguments pushed on the stack are not popped, and (2) the + ; original stack pointer is lost. In the normal case (no context switch), + ; HL will contain the value of the SP before the arguments were pushed. + + ld sp, hl ; Use the new stack pointer + + ; Restore registers. HL points to the beginning of the reg structure to restore + + ex af, af' ; Select alternate AF + pop af ; Offset 0: AF' = I with interrupt state in carry + ex af, af' ; Restore original AF + pop bc ; Offset 1: BC + pop de ; Offset 2: DE + pop ix ; Offset 3: IX + pop iy ; Offset 4: IY + exx ; Use alternate BC/DE/HL + ld hl, #-2 ; Offset of SP to account for ret addr on stack + pop de ; Offset 5: HL' = Stack pointer after return + add hl, de ; HL = Stack pointer value before return + exx ; Restore original BC/DE/HL + pop hl ; Offset 6: HL + pop af ; Offset 7: AF + + ; Restore the stack pointer + + exx ; Use alternate BC/DE/HL + ld sp, hl ; Set SP = saved stack pointer value before return + exx ; Restore original BC/DE/HL + + ; Restore interrupt state + + ex af, af' ; Recover interrupt state + jp po, nointenable ; Odd parity, IFF2=0, means disabled + ex af, af' ; Restore AF (before enabling interrupts) + ei ; yes + reti +nointenable:: + ex af, af' ; Restore AF + reti + +;************************************************************************** +; Ordering of segments for the linker (SDCC only) +;************************************************************************** + + .area _HOME + .area _CODE + .area _INITIALIZER + .area _GSINIT + .area _GSFINAL + + .area _DATA + .area _INITIALIZED + .area _BSEG + .area _BSS + .area _HEAP + +;************************************************************************** +; Global data initialization logic (SDCC only) +;************************************************************************** + + .area _GSINIT +gsinit:: + ld bc, #l__INITIALIZER + ld a, b + or a, c + jr Z, gsinit_next + ld de, #s__INITIALIZED + ld hl, #s__INITIALIZER + ldir +gsinit_next: + + .area _GSFINAL + ret + +;************************************************************************** +; The start of the heap (SDCC only). Note that is actually resides in +; the _CODE area (which may be FLASH or ROM) +;************************************************************************** + + .area _CODE +_g_heapbase:: + .dw #s__HEAP + diff --git a/nuttx/arch/z80/src/z180/z180_initialstate.c b/nuttx/arch/z80/src/z180/z180_initialstate.c new file mode 100644 index 000000000..8d0211261 --- /dev/null +++ b/nuttx/arch/z80/src/z180/z180_initialstate.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * arch/z80/src/z180/z180_initialstate.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include "chip/chip.h" +#include "up_internal.h" +#include "up_arch.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * 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 initial architecture registers + * and/or stack so that execution will begin at tcb->start + * on the next context switch. + * + ****************************************************************************/ + +void up_initial_state(_TCB *tcb) +{ + struct xcptcontext *xcp = &tcb->xcp; + + /* Initialize the initial exception register context structure */ + + memset(xcp, 0, sizeof(struct xcptcontext)); +#ifndef CONFIG_SUPPRESS_INTERRUPTS + xcp->regs[XCPT_I] = Z180_PV_FLAG; /* Parity flag will enable interrupts */ +#endif + xcp->regs[XCPT_SP] = (chipreg_t)tcb->adj_stack_ptr; + xcp->regs[XCPT_PC] = (chipreg_t)tcb->start; +} diff --git a/nuttx/arch/z80/src/z180/z180_io.c b/nuttx/arch/z80/src/z180/z180_io.c new file mode 100644 index 000000000..7e738c953 --- /dev/null +++ b/nuttx/arch/z80/src/z180/z180_io.c @@ -0,0 +1,94 @@ +/**************************************************************************** + * arch/z80/src/z180/z180_io.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +/* #include */ + +#include "up_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: outb + * + * Description: + * Output byte c on port p + * + ****************************************************************************/ + +void outp(char p, char c) +{ + __asm + ld c, 4(ix) ; port + ld a, 5(ix) ; value + out (c), a + __endasm; +} + + +/**************************************************************************** + * Name: inpb + * + * Description: + * Input byte from port p + * + ****************************************************************************/ + +char inp(char p) __naked +{ + __asm + ld c, 4(ix) ;port + in l, (c) + __endasm; +} diff --git a/nuttx/arch/z80/src/z180/z180_irq.c b/nuttx/arch/z80/src/z180/z180_irq.c new file mode 100644 index 000000000..929f38d6a --- /dev/null +++ b/nuttx/arch/z80/src/z180/z180_irq.c @@ -0,0 +1,113 @@ +/**************************************************************************** + * arch/z80/src/z180/z180_irq.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include + +#include "chip/switch.h" +#include "up_internal.h" + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* This holds a references to the current interrupt level register storage + * structure. If is non-NULL only during interrupt processing. + */ + +volatile chipreg_t *current_regs; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: irqsave + * + * Description: + * Disable all interrupts; return previous interrupt state + * + ****************************************************************************/ + +irqstate_t irqsave(void) __naked +{ + __asm + ld a, i ; AF Parity bit holds interrupt state + di ; Interrupts are disabled + push af ; Return AF in HL + pop hl ; + ret ; + __endasm; +} + +/**************************************************************************** + * Name: irqrestore + * + * Description: + * Restore previous interrupt state + * + ****************************************************************************/ + +void irqrestore(irqstate_t flags) __naked +{ + __asm + di ; Assume disabled + pop hl ; HL = return address + pop af ; AF Parity bit holds interrupt state + jp po, statedisable + ei +statedisable: + push af ; Restore stack + push hl ; + ret ; and return + __endasm; +} diff --git a/nuttx/arch/z80/src/z180/z180_registerdump.c b/nuttx/arch/z80/src/z180/z180_registerdump.c new file mode 100644 index 000000000..2ccfc2f99 --- /dev/null +++ b/nuttx/arch/z80/src/z180/z180_registerdump.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * arch/z80/src/z180/z180_registerdump.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include +#include + +#include "chip/switch.h" +#include "os_internal.h" +#include "up_internal.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Output debug info if stack dump is selected -- even if + * debug is not selected. + */ + +#ifdef CONFIG_ARCH_STACKDUMP +# undef lldbg +# define lldbg lib_lowprintf +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: z180_registerdump + ****************************************************************************/ + +#ifdef CONFIG_ARCH_STACKDUMP +static void z180_registerdump(void) +{ + if (current_regs) + { + lldbg("AF: %04x I: %04x\n", + current_regs[XCPT_AF], current_regs[XCPT_I]); + lldbg("BC: %04x DE: %04x HL: %04x\n", + current_regs[XCPT_BC], current_regs[XCPT_DE], current_regs[XCPT_HL]); + lldbg("IX: %04x IY: %04x\n", + current_regs[XCPT_IX], current_regs[XCPT_IY]); + lldbg("SP: %04x PC: %04x\n" + current_regs[XCPT_SP], current_regs[XCPT_PC]); + } +} +#endif diff --git a/nuttx/arch/z80/src/z180/z180_restoreusercontext.asm b/nuttx/arch/z80/src/z180/z180_restoreusercontext.asm new file mode 100644 index 000000000..fd2a70c1f --- /dev/null +++ b/nuttx/arch/z80/src/z180/z180_restoreusercontext.asm @@ -0,0 +1,104 @@ +;************************************************************************** +; arch/z80/src/z180/z180_restoreusercontext.asm +; +; Copyright (C) 2012 Gregory Nutt. All rights reserved. +; Author: Gregory Nutt +; +; Redistribution and use in source and binary forms, with or without +; modification, are permitted provided that the following conditions +; are met: +; +; 1. Redistributions of source code must retain the above copyright +; notice, this list of conditions and the following disclaimer. +; 2. Redistributions in binary form must reproduce the above copyright +; notice, this list of conditions and the following disclaimer in +; the documentation and/or other materials provided with the +; distribution. +; 3. Neither the name NuttX nor the names of its contributors may be +; used to endorse or promote products derived from this software +; without specific prior written permission. +; +; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +; "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +; LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +; FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +; COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +; INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +; BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +; OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +; AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +; LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +; ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +; POSSIBILITY OF SUCH DAMAGE. +; +;************************************************************************** + + ; Register save area layout + + .globl XCPT_I ; Offset 0: Saved I w/interrupt state in carry + .globl XCPT_BC ; Offset 1: Saved BC register + .globl XCPT_DE ; Offset 2: Saved DE register + .globl XCPT_IX ; Offset 3: Saved IX register + .globl XCPT_IY ; Offset 4: Saved IY register + .globl XCPT_SP ; Offset 5: Offset to SP at time of interrupt + .globl XCPT_HL ; Offset 6: Saved HL register + .globl XCPT_AF ; Offset 7: Saved AF register + .globl XCPT_PC ; Offset 8: Offset to PC at time of interrupt + +;************************************************************************** +; z180_restoreusercontext +;************************************************************************** + + .area _CODE +_z180_restoreusercontext: + ; On entry, stack contains return address (not used), then address + ; of the register save structure + + ; Discard the return address, we won't be returning + + pop hl + + ; Get the address of the beginning of the state save area. Each + ; pop will increment to the next element of the structure + + pop hl ; BC = Address of save structure + ld sp, hl ; SP points to top of storage area + + ; Disable interrupts while we muck with the alternative registers. The + ; Correct interrupt state will be restore below + + di + + ; Restore registers. HL points to the beginning of the reg structure to restore + + ex af, af' ; Select alternate AF + pop af ; Offset 0: AF' = I with interrupt state in parity + ex af, af' ; Restore original AF + pop bc ; Offset 1: BC + pop de ; Offset 2: DE + pop ix ; Offset 3: IX + pop iy ; Offset 4: IY + exx ; Use alternate BC/DE/HL + pop hl ; Offset 5: HL' = Stack pointer after return + exx ; Restore original BC/DE/HL + pop hl ; Offset 6: HL + pop af ; Offset 7: AF + + ; Restore the stack pointer + + exx ; Use alternate BC/DE/HL + pop de ; DE' = return address + ld sp, hl ; Set SP = saved stack pointer value before return + push de ; Save return address for ret instruction + exx ; Restore original BC/DE/HL + + ; Restore interrupt state + + ex af, af' ; Recover interrupt state + jp po, noinrestore ; Odd parity, IFF2=0, means disabled + ex af, af' ; Restore AF (before enabling interrupts) + ei ; yes.. Enable interrupts + ret ; and return +noinrestore: + ex af, af' ; Restore AF + ret ; Return with interrupts disabled diff --git a/nuttx/arch/z80/src/z180/z180_rom.asm b/nuttx/arch/z80/src/z180/z180_rom.asm new file mode 100644 index 000000000..c27574f4e --- /dev/null +++ b/nuttx/arch/z80/src/z180/z180_rom.asm @@ -0,0 +1,297 @@ +;************************************************************************** +; arch/z80/src/z180/z180_rom.asm +; +; Copyright (C) 2012 Gregory Nutt. All rights reserved. +; Author: Gregory Nutt +; +; Redistribution and use in source and binary forms, with or without +; modification, are permitted provided that the following conditions +; are met: +; +; 1. Redistributions of source code must retain the above copyright +; notice, this list of conditions and the following disclaimer. +; 2. Redistributions in binary form must reproduce the above copyright +; notice, this list of conditions and the following disclaimer in +; the documentation and/or other materials provided with the +; distribution. +; 3. Neither the name NuttX nor the names of its contributors may be +; used to endorse or promote products derived from this software +; without specific prior written permission. +; +; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +; "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +; LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +; FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +; COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +; INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +; BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +; OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +; AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +; LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +; ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +; POSSIBILITY OF SUCH DAMAGE. +; +;************************************************************************** + + .title NuttX for the Z180 + .module z180_head + +;************************************************************************** +; Constants +;************************************************************************** + + ; Register save area layout + + XCPT_I == 0 ; Offset 0: Saved I w/interrupt state in carry + XCPT_BC == 2 ; Offset 1: Saved BC register + XCPT_DE == 4 ; Offset 2: Saved DE register + XCPT_IX == 6 ; Offset 3: Saved IX register + XCPT_IY == 8 ; Offset 4: Saved IY register + XCPT_SP == 10 ; Offset 5: Offset to SP at time of interrupt + XCPT_HL == 12 ; Offset 6: Saved HL register + XCPT_AF == 14 ; Offset 7: Saved AF register + XCPT_PC == 16 ; Offset 8: Offset to PC at time of interrupt + + ; Default stack base (needs to be fixed) + + .include "asm_mem.h" + +;************************************************************************** +; Global symbols used +;************************************************************************** + + .globl _os_start ; OS entry point + .globl _up_doirq ; Interrupt decoding logic + +;************************************************************************** +; System start logic +;************************************************************************** + +_up_reset: + ; Set up the stack pointer at the location determined the Makefile + ; and stored in asm_mem.h + + ld SP, #CONFIG_STACK_END ; Set stack pointer + + ; Performed initialization unique to the SDCC toolchain + + call gsinit ; Initialize the data section + + ; Copy the reset vectors + + ld hl, #_up_rstvectors ; code for RAM + ld de, #0x4000 ; move it here + ld bc, #3*7 ; 7 vectors / 3 bytes each + ldir + + ; Then start NuttX + + call _os_start ; jump to the OS entry point + + ; NuttX will never return, but just in case... + +_up_halt:: + halt ; We should never get here + jp _up_halt + + ; Data to copy to address 0x4000 + +_up_rstvectors: + jp _up_rst1 ; 0x4000 : RST 1 + jp _up_rst2 ; 0x4003 : RST 2 + jp _up_rst3 ; 0x4006 : RST 3 + jp _up_rst4 ; 0x4009 : RST 4 + jp _up_rst5 ; 0x400c : RST 5 + jp _up_rst6 ; 0x400f : RST 6 + jp _up_rst7 ; 0x4012 : RST 7 + +;************************************************************************** +; Other reset handlers +; +; Interrupt mode 1 behavior: +; +; 1. M1 cycle: 7 ticks +; Acknowledge interrupt and decrements SP +; 2. M2 cycle: 3 ticks +; Writes the MS byte of the PC onto the stack and decrements SP +; 3. M3 cycle: 3 ticks +; Writes the LS byte of the PC onto the stack and sets the PC to 0x0038. +; +;************************************************************************** + +_up_rst1: ; RST 1 + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #1 ; 1 = Z180_RST1 + jr _up_rstcommon ; Remaining RST handling is common + +_up_rst2: ; RST 2 + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #2 ; 2 = Z180_RST2 + jr _up_rstcommon ; Remaining RST handling is common + +_up_rst3: ; RST 3 + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #3 ; 1 = Z180_RST3 + jr _up_rstcommon ; Remaining RST handling is common + +_up_rst4: ; RST 4 + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #4 ; 1 = Z180_RST4 + jr _up_rstcommon ; Remaining RST handling is common + +_up_rst5: ; RST 5 + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #5 ; 1 = Z180_RST5 + jr _up_rstcommon ; Remaining RST handling is common + +_up_rst6: ; RST 6 + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #6 ; 1 = Z180_RST6 + jr _up_rstcommon ; Remaining RST handling is common + +_up_rst7: ; RST 7 + ; Save AF on the stack, set the interrupt number and jump to the + ; common reset handling logic. + ; Offset 8: Return PC is already on the stack + push af ; Offset 7: AF (retaining flags) + ld a, #7 ; 7 = Z180_RST7 + jr _up_rstcommon ; Remaining RST handling is common + +;************************************************************************** +; Common Interrupt handler +;************************************************************************** + +_up_rstcommon: + ; Create a register frame. SP points to top of frame + 4, pushes + ; decrement the stack pointer. Already have + ; + ; Offset 8: Return PC is already on the stack + ; Offset 7: AF (retaining flags) + ; + ; IRQ number is in A + + push hl ; Offset 6: HL + ld hl, #(3*2) ; HL is the value of the stack pointer before + add hl, sp ; the interrupt occurred + push hl ; Offset 5: Stack pointer + push iy ; Offset 4: IY + push ix ; Offset 3: IX + push de ; Offset 2: DE + push bc ; Offset 1: BC + + ld b, a ; Save the reset number in B + ld a, i ; Parity bit holds interrupt state + push af ; Offset 0: I with interrupt state in parity + di + + ; Call the interrupt decode logic. SP points to the beginning of the reg structure + + ld hl, #0 ; Argument #2 is the beginning of the reg structure + add hl, sp ; + push hl ; Place argument #2 at the top of stack + push bc ; Argument #1 is the Reset number + inc sp ; (make byte sized) + call _up_doirq ; Decode the IRQ + + ; On return, HL points to the beginning of the reg structure to restore + ; Note that (1) the arguments pushed on the stack are not popped, and (2) the + ; original stack pointer is lost. In the normal case (no context switch), + ; HL will contain the value of the SP before the arguments were pushed. + + ld sp, hl ; Use the new stack pointer + + ; Restore registers. HL points to the beginning of the reg structure to restore + + ex af, af' ; Select alternate AF + pop af ; Offset 0: AF' = I with interrupt state in carry + ex af, af' ; Restore original AF + pop bc ; Offset 1: BC + pop de ; Offset 2: DE + pop ix ; Offset 3: IX + pop iy ; Offset 4: IY + exx ; Use alternate BC/DE/HL + ld hl, #-2 ; Offset of SP to account for ret addr on stack + pop de ; Offset 5: HL' = Stack pointer after return + add hl, de ; HL = Stack pointer value before return + exx ; Restore original BC/DE/HL + pop hl ; Offset 6: HL + pop af ; Offset 7: AF + + ; Restore the stack pointer + + exx ; Use alternate BC/DE/HL + ld sp, hl ; Set SP = saved stack pointer value before return + exx ; Restore original BC/DE/HL + + ; Restore interrupt state + + ex af, af' ; Recover interrupt state + jp po, nointenable ; Odd parity, IFF2=0, means disabled + ex af, af' ; Restore AF (before enabling interrupts) + ei ; yes + reti +nointenable:: + ex af, af' ; Restore AF + reti + +;************************************************************************** +; Ordering of segments for the linker (SDCC only) +;************************************************************************** + + .area _HOME + .area _CODE + .area _INITIALIZER + .area _GSINIT + .area _GSFINAL + + .area _DATA + .area _INITIALIZED + .area _BSEG + .area _BSS + .area _HEAP + +;************************************************************************** +; Global data initialization logic (SDCC only) +;************************************************************************** + + .area _GSINIT +gsinit:: + ld bc, #l__INITIALIZER + ld a, b + or a, c + jr Z, gsinit_next + ld de, #s__INITIALIZED + ld hl, #s__INITIALIZER + ldir +gsinit_next: + + .area _GSFINAL + ret + +;************************************************************************** +; The start of the heap (SDCC only). Note that is actually resides in +; the _CODE area (which may be FLASH or ROM) +;************************************************************************** + + .area _CODE +_g_heapbase:: + .dw #s__HEAP + diff --git a/nuttx/arch/z80/src/z180/z180_saveusercontext.asm b/nuttx/arch/z80/src/z180/z180_saveusercontext.asm new file mode 100644 index 000000000..31c9ef8ce --- /dev/null +++ b/nuttx/arch/z80/src/z180/z180_saveusercontext.asm @@ -0,0 +1,143 @@ +;************************************************************************* +; arch/z80/src/z180/z180_saveusercontext.asm +; +; Copyright (C) 2012 Gregory Nutt. All rights reserved. +; Author: Gregory Nutt +; +; Redistribution and use in source and binary forms, with or without +; modification, are permitted provided that the following conditions +; are met: +; +; 1. Redistributions of source code must retain the above copyright +; notice, this list of conditions and the following disclaimer. +; 2. Redistributions in binary form must reproduce the above copyright +; notice, this list of conditions and the following disclaimer in +; the documentation and/or other materials provided with the +; distribution. +; 3. Neither the name NuttX nor the names of its contributors may be +; used to endorse or promote products derived from this software +; without specific prior written permission. +; +; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +; "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +; LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +; FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +; COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +; INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +; BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +; OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +; AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +; LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +; ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +; POSSIBILITY OF SUCH DAMAGE. +; +;************************************************************************* + +;************************************************************************* +; Constants +;************************************************************************* + + ; Register save area layout + + .globl XCPT_I ; Offset 0: Saved I w/interrupt state in parity + .globl XCPT_BC ; Offset 1: Saved BC register + .globl XCPT_DE ; Offset 2: Saved DE register + .globl XCPT_IX ; Offset 3: Saved IX register + .globl XCPT_IY ; Offset 4: Saved IY register + .globl XCPT_SP ; Offset 5: Offset to SP at time of interrupt + .globl XCPT_HL ; Offset 6: Saved HL register + .globl XCPT_AF ; Offset 7: Saved AF register + .globl XCPT_PC ; Offset 8: Offset to PC at time of interrupt + + ; Stack frame + + FRAME_IY == 0 ; Location of IY on the stack + FRAME_IX == 2 ; Location of IX on the stack + FRAME_RET == 4 ; Location of return address on the stack + FRAME_REGS == 6 ; Location of reg save area on stack + + SP_OFFSET == 6 + +;************************************************************************* +; Name: z180_saveusercontext +;************************************************************************* + + .area _CODE +_z180_saveusercontext: + ; Set up a stack frame + + push ix ; Save IX and IY + push iy + ld ix, #0 + add ix, sp ; IX = stack frame + + ; Fetch the address of the save area + + ld e, FRAME_REGS(ix) ; HL = save area address + ld d, FRAME_REGS+1(ix) ; + ld iy, #0 + add iy, de ; IY = save area address + + ; Then save the registers + + ; Save the current interrupt state at offset 0 + + ld a, i ; Get interrupt state + push af + pop hl + ld XCPT_I(iy), l ; Offset 0: I w/interrupt state in parity + ld XCPT_I+1(iy), h + + ; Save BC at offset 1 + + ld XCPT_BC(iy), c ; Offset 1: BC + ld XCPT_BC+1(iy), b + + ; DE is not preserved (offset 2) + + ; Save IX at offset 3 + + ld l, FRAME_IX(ix) ; HL = Saved alue of IX + ld h, FRAME_IX+1(ix) ; + ld XCPT_IX(iy), l ; Offset 3: IX + ld XCPT_IX+1(iy), h ; + + ; Save IY at offset 4 + + ld l, FRAME_IY(ix) ; HL = Saved value of IY + ld h, FRAME_IY+1(ix) ; + ld XCPT_IY(iy), l ; Offset 4: IY + ld XCPT_IY+1(iy), h + + ; Save that stack pointer as it would be upon return in offset 5 + + ld hl, #SP_OFFSET ; Value of stack pointer on return + add hl, sp + ld XCPT_SP(iy), l ; Offset 5 SP + ld XCPT_SP+1(iy), h + + ; HL is saved as the value 1 at offset 6 + + xor a ; A = 0 + ld XCPT_HL+1(iy), a ; Offset 2: HL on return (=1) + inc a ; A = 1 + ld XCPT_HL(iy), a ; + + ; AF is not preserved (offset 7) + + ; Save the return address in offset 8 + + ld l, FRAME_RET(ix) ; HL = Saved return address + ld h, FRAME_RET+1(ix) ; + ld XCPT_PC(iy), l ; Offset 8: PC + ld XCPT_PC+1(iy), h + + ; Return the value 0 + + xor a ; HL = return value of zero + ld l, a + ld h, a + + pop iy + pop ix + ret diff --git a/nuttx/arch/z80/src/z180/z180_schedulesigaction.c b/nuttx/arch/z80/src/z180/z180_schedulesigaction.c new file mode 100644 index 000000000..3d9bbe55b --- /dev/null +++ b/nuttx/arch/z80/src/z180/z180_schedulesigaction.c @@ -0,0 +1,192 @@ +/**************************************************************************** + * arch/z80/src/z180/z180_schedulesigaction.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include "chip/switch.h" +#include "os_internal.h" +#include "up_internal.h" + +#ifndef CONFIG_DISABLE_SIGNALS + +/**************************************************************************** + * Private Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: z180_sigsetup + ****************************************************************************/ + +static void z180_sigsetup(FAR _TCB *tcb, sig_deliver_t sigdeliver, FAR chipreg_t *regs) +{ + /* Save the return address and interrupt state. These will be restored by + * the signal trampoline after the signals have been delivered. + */ + + tcb->xcp.sigdeliver = sigdeliver; + tcb->xcp.saved_pc = regs[XCPT_PC]; + tcb->xcp.saved_i = regs[XCPT_I]; + + /* Then set up to vector to the trampoline with interrupts disabled */ + + regs[XCPT_PC] = (chipreg_t)up_sigdeliver; + regs[XCPT_I] = 0; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * 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(FAR _TCB *tcb, sig_deliver_t sigdeliver) +{ + dbg("tcb=0x%p sigdeliver=0x%04x\n", tcb, (uint16_t)sigdeliver); + + /* Refuse to handle nested signal actions */ + + if (tcb->xcp.sigdeliver == NULL) + { + irqstate_t flags; + + /* Make sure that interrupts are disabled */ + + flags = irqsave(); + + /* First, handle some special cases when the signal is being delivered + * to the currently executing task. + */ + + if (tcb == (FAR _TCB*)g_readytorun.head) + { + /* CASE 1: We are not in an interrupt handler and a task is + * signalling itself for some reason. + */ + + if (!IN_INTERRUPT()) + { + /* In this case just deliver the signal now. */ + + sigdeliver(tcb); + } + + /* CASE 2: We are in an interrupt handler AND the interrupted task + * is the same as the one that must receive the signal, then we + * will have to modify the return state as well as the state in + * the TCB. + */ + + else + { + /* Set up to vector to the trampoline with interrupts disabled. */ + + z180_sigsetup(tcb, sigdeliver, IRQ_STATE()); + + /* And make sure that the saved context in the TCB + * is the same as the interrupt return context. + */ + + SAVE_IRQCONTEXT(tcb); + } + } + + /* Otherwise, we are (1) signaling a task is not running from an interrupt + * handler or (2) we are not in an interrupt handler and the running task + * is signalling some non-running task. + */ + + else + { + /* Set up to vector to the trampoline with interrupts disabled. */ + + z180_sigsetup(tcb, sigdeliver, tcb->xcp.regs); + } + + irqrestore(flags); + } +} + +#endif /* CONFIG_DISABLE_SIGNALS */ + diff --git a/nuttx/arch/z80/src/z180/z180_sigdeliver.c b/nuttx/arch/z80/src/z180/z180_sigdeliver.c new file mode 100644 index 000000000..281cfcf6a --- /dev/null +++ b/nuttx/arch/z80/src/z180/z180_sigdeliver.c @@ -0,0 +1,139 @@ +/**************************************************************************** + * arch/z80/src/z180/z180_sigdeliver.c + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip/switch.h" +#include "os_internal.h" +#include "up_internal.h" + +#ifndef CONFIG_DISABLE_SIGNALS + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: up_sigdeliver + * + * Description: + * This is the a signal handling trampoline. When a signal action was + * posted. The task context was mucked with and forced to branch to this + * location with interrupts disabled. + * + ****************************************************************************/ + +void up_sigdeliver(void) +{ +#ifndef CONFIG_DISABLE_SIGNALS + FAR _TCB *rtcb = (_TCB*)g_readytorun.head; + chipreg_t regs[XCPTCONTEXT_REGS]; + sig_deliver_t sigdeliver; + + /* Save the errno. This must be preserved throughout the signal handling + * so that the user code final gets the correct errno value (probably + * EINTR). + */ + + int saved_errno = rtcb->pterrno; + + up_ledon(LED_SIGNAL); + + sdbg("rtcb=%p sigdeliver=%p sigpendactionq.head=%p\n", + rtcb, rtcb->xcp.sigdeliver, rtcb->sigpendactionq.head); + ASSERT(rtcb->xcp.sigdeliver != NULL); + + /* Save the real return state on the stack. */ + + z180_copystate(regs, rtcb->xcp.regs); + regs[XCPT_PC] = rtcb->xcp.saved_pc; + regs[XCPT_I] = rtcb->xcp.saved_i; + + /* 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 more signal deliveries while processing the current pending + * signals. + */ + + sigdeliver = rtcb->xcp.sigdeliver; + rtcb->xcp.sigdeliver = NULL; + + /* Then restore the task interrupt state. */ + + irqrestore(regs[XCPT_I]); + + /* Deliver the signals */ + + sigdeliver(rtcb); + + /* Output any debug messages BEFORE restoring errno (because they may + * alter errno), then disable interrupts again and restore the original + * errno that is needed by the user logic (it is probably EINTR). + */ + + sdbg("Resuming\n"); + (void)irqsave(); + rtcb->pterrno = saved_errno; + + /* Then restore the correct state for this thread of execution. */ + + up_ledoff(LED_SIGNAL); + z180_restoreusercontext(regs); +#endif +} + +#endif /* CONFIG_DISABLE_SIGNALS */ -- cgit v1.2.3