From cdbb7040cc2ba10d5c86fccc448660a401dad7b9 Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 11 Dec 2012 18:04:04 +0000 Subject: Add support for the Z180 MMU and generic hooks for processes git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@5428 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/z80/src/z180/z180_mmu.c | 405 +++++++++++++++++++++++++++++++++++++ 1 file changed, 405 insertions(+) create mode 100644 nuttx/arch/z80/src/z180/z180_mmu.c (limited to 'nuttx/arch/z80/src/z180/z180_mmu.c') diff --git a/nuttx/arch/z80/src/z180/z180_mmu.c b/nuttx/arch/z80/src/z180/z180_mmu.c new file mode 100644 index 000000000..c71f59755 --- /dev/null +++ b/nuttx/arch/z80/src/z180/z180_mmu.c @@ -0,0 +1,405 @@ +/**************************************************************************** + * arch/z80/src/z180/z180_mmu.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. + * + ****************************************************************************/ + +/* See arch/z80/src/z180/z180_mmu.txt for additional information */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include "z180_mmu.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ + +#ifndef CONFIG_ADDRENV +# warning "OS address environment support is required (CONFIG_ADDRENV)" +#endif + +#ifndef CONFIG_GRAN +# warning "This file requires the granual allocator (CONFIG_GRAN)" +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#ifndef CONFIG_GRAN_SINGLE +static GRAN_HANDLE g_physhandle; +#endif +static struct z180_cbr_s g_cbrs[CONFIG_MAX_TASKS]; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: z180_mmu_findcbr + * + * Description: + * Find an unused struture in g_cbrs (i.e., one with reference count == 0). + * If a structure is found, its reference count is set to one and a pointer + * to the structure is returned. + * + ****************************************************************************/ + +static FAR struct z180_cbr_s *z180_mmu_findcbr(void) +{ + int i; + + for (i = 0; i < CONFIG_MAX_TASKS; i++) + { + FAR struct z180_cbr_s *cbr = &g_cbrs[i]; + if (cbr->crefs == 0) + { + cbr->crefs = 1; + return cbr; + } + } + + return NULL; +} + +/**************************************************************************** + * Name: z180_mmu_freecbr + * + * Description: + * Free a struture in g_cbrs by setting its reference count to 0; + * + ****************************************************************************/ + +#define z180_mmu_freecbr(cbr) (cbr)->crefs = 0 + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: z180_mmu_lowinit + * + * Description: + * Low-level, power-up initialization of the z180 MMU. this must be + * called very early in the boot process to get the basic operating + * memory configuration correct. This function does *not* perform all + * necessray MMU initialization... only the basics needed at power-up. + * z180_mmu_init() must be called later to complete the entire MMU + * initialization. + * + ****************************************************************************/ + +void z180_mmu_lowinit(void) __naked +{ + __asm + ld c, #Z180_MMU_CBR ; port + ld a, #Z180_CBAR_VALUE ; value + out (c), a + + ld c, #Z180_MMU_BBR ; port + ld a, #Z180_BBR_VALUE ; value + out (c), a + __endasm; +} + +/**************************************************************************** + * Name: z180_mmu_init + * + * Description: + * Perform higher level initializatin of the MMU and physical memory + * memory management logic. + * + ****************************************************************************/ + +int z180_mmu_init(void) +{ + /* Here we use the granule allocator as a page allocator. We lie and + * say that 1 page is 1 byte. + */ + +#ifdef CONFIG_GRAN_SINGLE +return gran_initialize((FAR void *)Z180_PHYSHEAP_STARTPAGE, + Z180_PHYSHEAP_NPAGES, 0, 0); +#else +g_physhandle = gran_initialize((FAR void *)Z180_PHYSHEAP_STARTPAGE, + Z180_PHYSHEAP_NPAGES, 0, 0); +return g_physhandle ? OK : -ENOMEM; +#endif +} + +/**************************************************************************** + * Name: up_addrenv_create + * + * Description: + * This function is called from the binary loader logic when a new + * task is created in RAM in order to instantiate an address environment for + * the task. + * + * Input Parameters: + * tcb - The TCB of the task needing the address environment. + * envsize - The size (in bytes) of the address environment needed by the + * task. + * + * Returned Value: + * Zero (OK) on success; a negated errno value on failure. + * + ****************************************************************************/ + +int up_addrenv_create(FAR _TCB *tcb, size_t envsize) +{ + FAR struct z180_cbr_s *cbr; + irqstate_t flags; + uintptr_t alloc; + unsigned int npages; + int ret; + + /* Make sure that there is no address environment in place on this TCB */ + + DEBUGASSERT(tcb->xcp.cbr == NULL); + + /* Convert the size from bytes to numbers of pages */ + + npages = PHYS_ALIGNUP(envsize); + if (npages < 1) + { + /* No address environment... but I suppose that is not an error */ + + sdbg("ERROR: npages is zero\n"); + return OK; + } + + /* Allocate a structure in the common .bss to hold information about the + * task's address environment. NOTE that this is not a part of the TCB, + * but rather a break-away structure that can be shared by the task as + * well as other threads. That is necessary because the life of the + * address of environment might be longer than the life of the task. + */ + + flags = irqsave(); + cbr = z180_mmu_findcbr(); + if (!cbr) + { + sdbg("ERROR: No free CBR structures\n"); + ret = -ENOMEM; + goto errout_with_irq + } + + /* Now allocate the physical memory to back up the address environment */ + + +#ifdef CONFIG_GRAN_SINGLE + alloc = (uintptr_t)gran_alloc(npages); +#else + alloc = (uintptr_t)gran_alloc(g_physhandle, npages); +#endif + if (!alloc) + { + sdbg("ERROR: Failed to allocate %d pages\n", npages); + ret = -ENOMEM; + goto errout_with_cbr; + } + + /* Save the information in the CBR structure. Note that alloc is in + * 4KB pages, already in the right form for the CBR. + */ + + DEBUGASSERT(alloc <= 0xff); + + cbr->cbr = alloc; + cbr->pages = npages; + tcb->xcp.cbr = cbr; + + irqrestore(flags); + return OK; + +errout_with_cbr: + z180_mmu_freecbr(cbr); + +errout_with_irq: + irqrestore(flags); + return ret; +} + +/**************************************************************************** + * Name: up_addrenv_clone + * + * Description: + * This function is called from the core scheduler logic when a thread + * is created that needs to share the address ennvironment of its parent + * task. In this case, the parent's address environment needs to be + * "cloned" for the child. + * + * Input Parameters: + * ptcb - The TCB of the parent task that has the address environment. + * ctcb - The TCB of the child thread needing the address environment. + * + * Returned Value: + * Zero (OK) on success; a negated errno value on failure. + * + ****************************************************************************/ + +#ifdef CONFIG_ADDRENV +int up_addrenv_clone(FAR const _TCB *ptcb, FAR _TCB *ctcb) +{ + irqstate_t flags; + + /* Make sure that the child has no address environment. It is okay if + * if the parent does not have one. + */ + + DEBUGASSERT(ctcb->xcp.cbr == NULL); + + flags = irqsave(); + if (ptcb->xcp.cbr) + { + /* Clone the CBR by incrementing the reference counting and saving a + * copy in the child thread's TCB. + */ + + ptcb->xcp.cbr.crefs++; + ctcb->xcp.cbr = ptcb->xcp.cbr; + } + + irqrestore(flags); + return OK; +} +#endif + +/**************************************************************************** + * Name: up_addrenv_instantiate + * + * Description: + * After an address environment has been established for a task (via + * up_addrenv_create(). This function may be called to to instantiate + * that address environment in the virtual address space. this might be + * necessary, for example, to load the code for the task from a file or + * to access address environment private data. + * + * Input Parameters: + * tcb - The TCB of the task or thread whose the address environment will + * be instantiated. + * + * Returned Value: + * Zero (OK) on success; a negated errno value on failure. + * + ****************************************************************************/ + +#ifdef CONFIG_ADDRENV +int up_addrenv_instantiate(FAR _TCB *tcb) +{ + irqstate_t flags; + + /* Check if the task has an address environment. */ + + flags = irqsave(); + if (tcb->xcp.cbr) + { + /* Yes... write the CBR value into CBR register */ + + outp(Z180_MMU_CBR, tcb->xcp.cbr.cbr); + /* Clone the CBR by incrementing the reference counting and saving a + * copy in the child thread's TCB. + */ + + ptcb->xcp.cbr.crefs++; + ctcb->xcp.cbr = ptcb->xcp.cbr; + } + + irqrestore(flags); + return OK; +} +#endif + +/**************************************************************************** + * Name: up_addrenv_release + * + * Description: + * This function is called when a task or thread exits in order to release + * its reference to an address environment. When there are no further + * references to an address environment, that address environment should + * be destroyed. + * + * Input Parameters: + * tcb - The TCB of the task or thread whose the address environment will + * be released. + * + * Returned Value: + * Zero (OK) on success; a negated errno value on failure. + * + ****************************************************************************/ + +#ifdef CONFIG_ADDRENV +int up_addrenv_release(FAR _TCB *tcb) +{ + FAR struct z180_cbr_s *cbr; + irqstate_t flags; + + /* Check if the task has an address environment. */ + + flags = irqsave(); + cbr = tcb->xcp.cbr; + if (cbr) + { + /* Nullify the reference to the CBR structgure and decrement the number + * of references on the CBR. + */ + + tcb->xcp.cbr = NULL; + + /* If the reference count would decrement to zero, then free the CBR + * structure. + */ + + if (cbr->crefs <= 1) + { + z180_mmu_freecbr(cbr); + } + else + { + /* Otherwise, just decrement the reference count */ + + cbr->crefs--; + } + } + + irqrestore(flags); + return OK; +} +#endif + -- cgit v1.2.3