code:
/*	$OpenBSD: bus_dma.c,v 1.2 2015/01/24 20:59:42 kettenis Exp $	*/
/*	$NetBSD: machdep.c,v 1.214 1996/11/10 03:16:17 thorpej Exp $	*/

/*-
 * Copyright (c) 1996, 1997 The NetBSD Foundation, Inc.
 * All rights reserved.
 *
 * This code is derived from software contributed to The NetBSD Foundation
 * by Jason R. Thorpe of the Numerical Aerospace Simulation Facility,
 * NASA Ames Research Center.
 *
 * 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.
 *
 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. 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 FOUNDATION OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <sys/param.h>
#include <sys/proc.h>
#include <sys/extent.h>
#include <sys/buf.h>
#include <sys/device.h>
#include <sys/systm.h>
#include <sys/conf.h>
#include <sys/file.h>
#include <sys/malloc.h>
#include <sys/mbuf.h>
#include <sys/mount.h>

#include <uvm/uvm_extern.h>

#include <machine/bus.h>

int _dmamap_load_buffer(bus_dma_tag_t, bus_dmamap_t, void *, bus_size_t,
    struct proc *, int, bus_addr_t *, int *, int);
/*
 * Common function for DMA map creation.  May be called by bus-specific
 * DMA map creation functions.
 */
int
_dmamap_create(bus_dma_tag_t t, bus_size_t size, int nsegments,
    bus_size_t maxsegsz, bus_size_t boundary, int flags, bus_dmamap_t *dmamp)
{
	struct powerpc_bus_dmamap *map;
	void *mapstore;
	size_t mapsize;

	/*
	 * Allocate and initialize the DMA map.  The end of the map
	 * is a variable-sized array of segments, so we allocate enough
	 * room for them in one shot.
	 *
	 * Note we don't preserve the WAITOK or NOWAIT flags.  Preservation
	 * of ALLOCNOW notifies others that we've reserved these resources,
	 * and they are not to be freed.
	 *
	 * The bus_dmamap_t includes one bus_dma_segment_t, hence
	 * the (nsegments - 1).
	 */
	mapsize = sizeof(struct powerpc_bus_dmamap) +
	    (sizeof(bus_dma_segment_t) * (nsegments - 1));
	if ((mapstore = malloc(mapsize, M_DEVBUF, (flags & BUS_DMA_NOWAIT) ?
	    (M_NOWAIT | M_ZERO) : (M_WAITOK | M_ZERO))) == NULL)
		return (ENOMEM);

	map = (struct powerpc_bus_dmamap *)mapstore;
	map->_dm_size = size;
	map->_dm_segcnt = nsegments;
	map->_dm_maxsegsz = maxsegsz;
	map->_dm_boundary = boundary;
	map->_dm_flags = flags & ~(BUS_DMA_WAITOK|BUS_DMA_NOWAIT);
	map->dm_nsegs = 0;		/* no valid mappings */
	map->dm_mapsize = 0;

	*dmamp = map;
	return (0);
}

/*
 * Common function for DMA map destruction.  May be called by bus-specific
 * DMA map destruction functions.
 */
void
_dmamap_destroy(bus_dma_tag_t t, bus_dmamap_t map)
{

	free(map, M_DEVBUF, 0);
}

int
_dmamap_load_buffer(bus_dma_tag_t t, bus_dmamap_t map, void *buf,
    bus_size_t buflen, struct proc *p, int flags, bus_addr_t *lastaddrp,
    int *segp, int first)
{
	bus_size_t sgsize;
	bus_addr_t curaddr, lastaddr, baddr, bmask;
	vaddr_t vaddr = (vaddr_t)buf;
	pmap_t pmap;
	int seg;

	lastaddr = *lastaddrp;
	bmask = ~(map->_dm_boundary - 1);

	if (p != NULL)
		pmap = p->p_vmspace->vm_map.pmap;
	else
		pmap = pmap_kernel();

	for (seg = *segp; buflen > 0; ) {
		/*
		 * Get the physical address for this segment.
		 */
		if (pmap_extract(pmap, vaddr, (paddr_t *)&curaddr) != TRUE) {
			panic("dmamap_load_buffer pmap %p vaddr %lx "
				"pmap_extract failed", pmap, vaddr);
		}

		/*
		 * Compute the segment size, and adjust counts.
		 */
		sgsize = PAGE_SIZE - ((u_long)vaddr & PGOFSET);
		if (buflen < sgsize)
			sgsize = buflen;

		/*
		 * Make sure we don't cross any boundaries.
		 */
		if (map->_dm_boundary > 0) {
			baddr = (curaddr + map->_dm_boundary) & bmask;
			if (sgsize > (baddr - curaddr))
				sgsize = (baddr - curaddr);
		}

		/*
		 * Insert chunk into a segment, coalescing with the
		 * previous segment if possible.
		 */
		if (first) {
			map->dm_segs[seg].ds_addr = curaddr;
			map->dm_segs[seg].ds_len = sgsize;
			first = 0;
		} else {
			if (curaddr == lastaddr &&
			    (map->dm_segs[seg].ds_len + sgsize) <=
			     map->_dm_maxsegsz &&
			    (map->_dm_boundary == 0 ||
			     (map->dm_segs[seg].ds_addr & bmask) ==
			     (curaddr & bmask)))
				map->dm_segs[seg].ds_len += sgsize;
			else {
				if (++seg >= map->_dm_segcnt)
					break;
				map->dm_segs[seg].ds_addr = curaddr;
				map->dm_segs[seg].ds_len = sgsize;
			}
		}

		lastaddr = curaddr + sgsize;
		vaddr += sgsize;
		buflen -= sgsize;
	}

	*segp = seg;
	*lastaddrp = lastaddr;

	/*
	 * Did we fit?
	 */
	if (buflen != 0)
		return (EFBIG);		/* XX better return value here? */

	return (0);
}

/*
 * Common function for loading a DMA map with a linear buffer.  May
 * be called by bus-specific DMA map load functions.
 */
int
_dmamap_load(bus_dma_tag_t t, bus_dmamap_t map, void *buf, bus_size_t buflen,
    struct proc *p, int flags)
{
	bus_addr_t lastaddr;
	int seg, error;

	/*
	 * Make sure that on error condition we return "no valid mappings".
	 */
	map->dm_mapsize = 0;
	map->dm_nsegs = 0;

	if (buflen > map->_dm_size)
		return (EINVAL);

	seg = 0;
	error = _dmamap_load_buffer(t, map, buf, buflen, p, flags,
	    &lastaddr, &seg, 1);
	if (error == 0) {
		map->dm_mapsize = buflen;
		map->dm_nsegs = seg + 1;
	}
	return (error);
}

/*
 * Like _bus_dmamap_load(), but for mbufs.
 */
int
_dmamap_load_mbuf(bus_dma_tag_t t, bus_dmamap_t map, struct mbuf *m0,
    int flags)
{
	bus_addr_t lastaddr;
	int seg, error, first;
	struct mbuf *m;

	/*
	 * Make sure that on error condition we return "no valid mappings".
	 */
	map->dm_mapsize = 0;
	map->dm_nsegs = 0;

#ifdef DIAGNOSTIC
	if ((m0->m_flags & M_PKTHDR) == 0)
		panic("_bus_dmamap_load_mbuf: no packet header");
#endif

	if (m0->m_pkthdr.len > map->_dm_size)
		return (EINVAL);

	first = 1;
	seg = 0;
	error = 0;
	for (m = m0; m != NULL && error == 0; m = m->m_next) {
		if (m->m_len == 0)
			continue;
		error = _dmamap_load_buffer(t, map, m->m_data, m->m_len,
		    NULL, flags, &lastaddr, &seg, first);
		first = 0;
	}
	if (error == 0) {
		map->dm_mapsize = m0->m_pkthdr.len;
		map->dm_nsegs = seg + 1;
	}
	return (error);
}

/*
 * Like _bus_dmamap_load(), but for uios.
 */
int
_dmamap_load_uio(bus_dma_tag_t t, bus_dmamap_t map, struct uio *uio, int flags)
{
	bus_addr_t lastaddr;
	int seg, i, error, first;
	bus_size_t minlen, resid;
	struct proc *p = NULL;
	struct iovec *iov;
	caddr_t addr;

	/*
	 * Make sure that on error condition we return "no valid mappings".
	 */
	map->dm_mapsize = 0;
	map->dm_nsegs = 0;

	resid = uio->uio_resid;
	iov = uio->uio_iov;

	if (resid > map->_dm_size)
		return (EINVAL);

	if (uio->uio_segflg == UIO_USERSPACE) {
		p = uio->uio_procp;
#ifdef DIAGNOSTIC
		if (p == NULL)
			panic("_bus_dmamap_load_uio: USERSPACE but no proc");
#endif
	}

	first = 1;
	seg = 0;
	error = 0;
	for (i = 0; i < uio->uio_iovcnt && resid != 0 && error == 0; i++) {
		/*
		 * Now at the first iovec to load.  Load each iovec
		 * until we have exhausted the residual count.
		 */
		minlen = resid < iov[i].iov_len ? resid : iov[i].iov_len;
		addr = (caddr_t)iov[i].iov_base;

		error = _dmamap_load_buffer(t, map, addr, minlen,
		    p, flags, &lastaddr, &seg, first);
		first = 0;

		resid -= minlen;
	}
	if (error == 0) {
		map->dm_mapsize = uio->uio_resid;
		map->dm_nsegs = seg + 1;
	}
	return (error);
}

/*
 * Like _bus_dmamap_load(), but for raw memory allocated with
 * bus_dmamem_alloc().
 */
int
_dmamap_load_raw(bus_dma_tag_t t, bus_dmamap_t map, bus_dma_segment_t *segs,
    int nsegs, bus_size_t size, int flags)
{
	if (nsegs > map->_dm_segcnt || size > map->_dm_size)
		return (EINVAL);

	/*
	 * Make sure we don't cross any boundaries.
	 */
	if (map->_dm_boundary) {
		bus_addr_t bmask = ~(map->_dm_boundary - 1);
		int i;

		for (i = 0; i < nsegs; i++) {
			if (segs[i].ds_len > map->_dm_maxsegsz)
				return (EINVAL);
			if ((segs[i].ds_addr & bmask) !=
			    ((segs[i].ds_addr + segs[i].ds_len - 1) & bmask))
				return (EINVAL);
		}
	}

	bcopy(segs, map->dm_segs, nsegs * sizeof(*segs));
	map->dm_nsegs = nsegs;
	map->dm_mapsize = size;
	return (0);
}

/*
 * Common function for unloading a DMA map.  May be called by
 * bus-specific DMA map unload functions.
 */
void
_dmamap_unload(bus_dma_tag_t t, bus_dmamap_t map)
{

	/*
	 * No resources to free; just mark the mappings as
	 * invalid.
	 */
	map->dm_nsegs = 0;
	map->dm_mapsize = 0;
}

/*
 * Common function for DMA map synchronization.  May be called
 * by bus-specific DMA map synchronization functions.
 */
void
_dmamap_sync(bus_dma_tag_t t, bus_dmamap_t map, bus_addr_t offset,
bus_size_t len, int op)
{
	int i;
	bus_size_t minlen, wlen;
	bus_addr_t pa, addr;
	struct vm_page *pg;

	for (i = 0; i < map->dm_nsegs && len != 0; i++) {
		/* Find the beginning segment. */
		if (offset >= map->dm_segs[i].ds_len) {
			offset -= map->dm_segs[i].ds_len;
			continue;
		}

		minlen = len < map->dm_segs[i].ds_len - offset ?
		    len : map->dm_segs[i].ds_len - offset;

		addr = map->dm_segs[i].ds_addr + offset;

		switch (op) {
		case BUS_DMASYNC_POSTWRITE:
			for (pa = trunc_page(addr), wlen = 0;
			    pa < round_page(addr + minlen);
			    pa += PAGE_SIZE) {
				pg = PHYS_TO_VM_PAGE(pa);
				if (pg != NULL)
					atomic_clearbits_int(&pg->pg_flags,
					    PG_PMAP_EXE);
			}
		}

	}
}

/*
 * Common function for DMA-safe memory allocation.  May be called
 * by bus-specific DMA memory allocation functions.
 */
int
_dmamem_alloc(bus_dma_tag_t t, bus_size_t size, bus_size_t alignment,
    bus_size_t boundary, bus_dma_segment_t *segs, int nsegs, int *rsegs,
    int flags)
{
	return (_dmamem_alloc_range(t, size, alignment, boundary,
	    segs, nsegs, rsegs, flags, 0, -1));
}

/*
 * Common function for freeing DMA-safe memory.  May be called by
 * bus-specific DMA memory free functions.
 */
void
_dmamem_free(bus_dma_tag_t t, bus_dma_segment_t *segs, int nsegs)
{
	struct vm_page *m;
	bus_addr_t addr;
	struct pglist mlist;
	int curseg;

	/*
	 * Build a list of pages to free back to the VM system.
	 */
	TAILQ_INIT(&mlist);
	for (curseg = 0; curseg < nsegs; curseg++) {
		for (addr = segs[curseg].ds_addr;
		    addr < (segs[curseg].ds_addr + segs[curseg].ds_len);
		    addr += PAGE_SIZE) {
			m = PHYS_TO_VM_PAGE(addr);
			TAILQ_INSERT_TAIL(&mlist, m, pageq);
		}
	}

	uvm_pglistfree(&mlist);
}

/*
 * Common function for mapping DMA-safe memory.  May be called by
 * bus-specific DMA memory map functions.
 */
int
_dmamem_map(bus_dma_tag_t t, bus_dma_segment_t *segs, int nsegs, size_t size,
    caddr_t *kvap, int flags)
{
	vaddr_t va, sva;
	size_t ssize;
	bus_addr_t addr;
	int curseg, pmapflags = 0, error;
	const struct kmem_dyn_mode *kd;

	if (flags & BUS_DMA_NOCACHE)
		pmapflags |= PMAP_NOCACHE;

	size = round_page(size);
	kd = flags & BUS_DMA_NOWAIT ? &kd_trylock : &kd_waitok;
	va = (vaddr_t)km_alloc(size, &kv_any, &kp_none, kd);
	if (va == 0)
		return (ENOMEM);

	*kvap = (caddr_t)va;

	sva = va;
	ssize = size;
	for (curseg = 0; curseg < nsegs; curseg++) {
		for (addr = segs[curseg].ds_addr;
		    addr < (segs[curseg].ds_addr + segs[curseg].ds_len);
		    addr += PAGE_SIZE, va += PAGE_SIZE, size -= PAGE_SIZE) {
			if (size == 0)
				panic("_bus_dmamem_map: size botch");
			error = pmap_enter(pmap_kernel(), va, addr | pmapflags,
			    PROT_READ | PROT_WRITE,
			    PROT_READ | PROT_WRITE | PMAP_WIRED | PMAP_CANFAIL);
			if (error) {
				pmap_update(pmap_kernel());
				km_free((void *)sva, ssize, &kv_any, &kp_none);
				return (error);
			}
		}
	}
	pmap_update(pmap_kernel());

	return (0);
}

/*
 * Common function for unmapping DMA-safe memory.  May be called by
 * bus-specific DMA memory unmapping functions.
 */
void
_dmamem_unmap(bus_dma_tag_t t, caddr_t kva, size_t size)
{

#ifdef DIAGNOSTIC
	if ((u_long)kva & PGOFSET)
		panic("_bus_dmamem_unmap");
#endif

	km_free(kva, round_page(size), &kv_any, &kp_none);
}

/*
 * Common function for mmap(2)'ing DMA-safe memory.  May be called by
 * bus-specific DMA mmap(2)'ing functions.
 */
paddr_t
_dmamem_mmap(bus_dma_tag_t t, bus_dma_segment_t *segs, int nsegs, off_t off,
    int prot, int flags)
{
	int i, pmapflags = 0;

	if (flags & BUS_DMA_NOCACHE)
		pmapflags |= PMAP_NOCACHE;

	for (i = 0; i < nsegs; i++) {
#ifdef DIAGNOSTIC
		if (off & PGOFSET)
			panic("_bus_dmamem_mmap: offset unaligned");
		if (segs[i].ds_addr & PGOFSET)
			panic("_bus_dmamem_mmap: segment unaligned");
		if (segs[i].ds_len & PGOFSET)
			panic("_bus_dmamem_mmap: segment size not multiple"
			    " of page size");
#endif
		if (off >= segs[i].ds_len) {
			off -= segs[i].ds_len;
			continue;
		}

		return ((segs[i].ds_addr + off) | pmapflags);
	}

	/* Page not found. */
	return (-1);
}

/**********************************************************************
 * DMA utility functions
 **********************************************************************/

/*
 * Allocate physical memory from the given physical address range.
 * Called by DMA-safe memory allocation methods.
 */
int
_dmamem_alloc_range(bus_dma_tag_t t, bus_size_t size, bus_size_t alignment,
    bus_size_t boundary, bus_dma_segment_t *segs, int nsegs, int *rsegs,
    int flags, bus_addr_t low, bus_addr_t high)
{
	vaddr_t curaddr, lastaddr;
	struct vm_page *m;
	struct pglist mlist;
	int curseg, error, plaflag;

	/* Always round the size. */
	size = round_page(size);

	/*
	 * Allocate pages from the VM system.
	 */
	plaflag = flags & BUS_DMA_NOWAIT ? UVM_PLA_NOWAIT : UVM_PLA_WAITOK;
	if (flags & BUS_DMA_ZERO)
		plaflag |= UVM_PLA_ZERO;

	TAILQ_INIT(&mlist);
	error = uvm_pglistalloc(size, low, high,
	    alignment, boundary, &mlist, nsegs, plaflag);
	if (error)
		return (error);

	/*
	 * Compute the location, size, and number of segments actually
	 * returned by the VM code.
	 */
	m = TAILQ_FIRST(&mlist);
	curseg = 0;
	lastaddr = segs[curseg].ds_addr = VM_PAGE_TO_PHYS(m);
	segs[curseg].ds_len = PAGE_SIZE;
	m = TAILQ_NEXT(m, pageq);

	for (; m != NULL; m = TAILQ_NEXT(m, pageq)) {
		curaddr = VM_PAGE_TO_PHYS(m);
#ifdef DIAGNOSTIC
		if (curaddr < low || curaddr >= high) {
			printf("vm_page_alloc_memory returned non-sensical"
			    " address 0x%lx\n", curaddr);
			panic("dmamem_alloc_range");
		}
#endif
		if (curaddr == (lastaddr + PAGE_SIZE))
			segs[curseg].ds_len += PAGE_SIZE;
		else {
			curseg++;
			segs[curseg].ds_addr = curaddr;
			segs[curseg].ds_len = PAGE_SIZE;
		}
		lastaddr = curaddr;
	}

	*rsegs = curseg + 1;

	return (0);
}


code:
/*	$OpenBSD: bus_space.c,v 1.5 2015/02/09 13:35:44 deraadt Exp $	*/
/*	$NetBSD: machdep.c,v 1.4 1996/10/16 19:33:11 ws Exp $	*/

/*
 * Copyright (C) 1995, 1996 Wolfgang Solfrank.
 * Copyright (C) 1995, 1996 TooLs GmbH.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *	This product includes software developed by TooLs GmbH.
 * 4. The name of TooLs GmbH may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY TOOLS GMBH ``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 TOOLS GMBH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/malloc.h>
#include <sys/extent.h>

#include <uvm/uvm_extern.h>

#include <machine/bus.h>

extern int ppc_malloc_ok;
extern struct extent *devio_ex;

int bus_mem_add_mapping(bus_addr_t, bus_size_t, int, bus_space_handle_t *);
bus_addr_t bus_space_unmap_p(bus_space_tag_t, bus_space_handle_t,  bus_size_t);

/* BUS functions */
int
bus_space_map(bus_space_tag_t t, bus_addr_t bpa, bus_size_t size,
    int flags, bus_space_handle_t *bshp)
{
	int error;

	if  (POWERPC_BUS_TAG_BASE(t) == 0) {
		/* if bus has base of 0 fail. */
		return EINVAL;
	}

	/*
	 * The address may, or may not, be relative to the
	 * base address of this bus.
	 */
	if (bpa < POWERPC_BUS_TAG_BASE(t))
		bpa += POWERPC_BUS_TAG_BASE(t);

	if ((error = extent_alloc_region(devio_ex, bpa, size, EX_NOWAIT |
	    (ppc_malloc_ok ? EX_MALLOCOK : 0))))
		return error;

	if ((error = bus_mem_add_mapping(bpa, size, flags, bshp))) {
		if (extent_free(devio_ex, bpa, size, EX_NOWAIT |
			(ppc_malloc_ok ? EX_MALLOCOK : 0)))
		{
			printf("bus_space_map: pa 0x%lx, size 0x%lx\n",
				bpa, size);
			printf("bus_space_map: can't free region\n");
		}
	}
	return error;
}

bus_addr_t
bus_space_unmap_p(bus_space_tag_t t, bus_space_handle_t bsh, bus_size_t size)
{
	bus_addr_t paddr;

	pmap_extract(pmap_kernel(), bsh, &paddr);
	bus_space_unmap((t), (bsh), (size));
	return paddr ;
}

void
bus_space_unmap(bus_space_tag_t t, bus_space_handle_t bsh, bus_size_t size)
{
	bus_addr_t sva;
	bus_size_t off, len;
	bus_addr_t bpa;

	sva = trunc_page(bsh);
	off = bsh - sva;
	len = round_page(size+off);

	if (pmap_extract(pmap_kernel(), sva, &bpa) == TRUE) {
		if (extent_free(devio_ex, bpa | (bsh & PAGE_MASK), size,
		    EX_NOWAIT | (ppc_malloc_ok ? EX_MALLOCOK : 0)))
		{
			printf("bus_space_map: pa 0x%lx, size 0x%lx\n",
				bpa, size);
			printf("bus_space_map: can't free region\n");
		}
	}

	pmap_kremove(sva, len);
	pmap_update(pmap_kernel());

	/* do not free memory which was stolen from the vm system */
	if (ppc_malloc_ok &&
	    ((sva >= VM_MIN_KERNEL_ADDRESS) && (sva < VM_MAX_KERNEL_ADDRESS)))
		km_free((void *)sva, len, &kv_any, &kp_none);
}

paddr_t
bus_space_mmap(bus_space_tag_t t, bus_addr_t bpa, off_t off, int prot,
    int flags)
{
	int pmapflags = PMAP_NOCACHE;

	if (POWERPC_BUS_TAG_BASE(t) == 0)
		return (-1);

	/*
	 * The address may, or may not, be relative to the
	 * base address of this bus.
	 */
	if (bpa < POWERPC_BUS_TAG_BASE(t))
		bpa += POWERPC_BUS_TAG_BASE(t);

	if (flags & BUS_SPACE_MAP_CACHEABLE)
		pmapflags &= ~PMAP_NOCACHE;

	return ((bpa + off) | pmapflags);
}

vaddr_t ppc_kvm_stolen = VM_KERN_ADDRESS_SIZE;

int
bus_mem_add_mapping(bus_addr_t bpa, bus_size_t size, int flags,
    bus_space_handle_t *bshp)
{
	bus_addr_t vaddr;
	bus_addr_t spa, epa;
	bus_size_t off, len;
	int pmapflags;

	spa = trunc_page(bpa);
	epa = bpa + size;
	off = bpa - spa;
	len = round_page(size+off);

#ifdef DIAGNOSTIC
	if (epa <= spa && epa != 0)
		panic("bus_mem_add_mapping: overflow");
#endif

	if (ppc_malloc_ok == 0) {
		/* need to steal vm space before kernel vm is initialized */
		vaddr = VM_MIN_KERNEL_ADDRESS + ppc_kvm_stolen;
		ppc_kvm_stolen += len;
		if (ppc_kvm_stolen > PPC_SEGMENT_LENGTH) {
			panic("ppc_kvm_stolen, out of space");
		}
	} else {
		vaddr = (vaddr_t)km_alloc(len, &kv_any, &kp_none, &kd_nowait);
		if (vaddr == 0)
			return (ENOMEM);
	}
	*bshp = vaddr + off;

	if (flags & BUS_SPACE_MAP_CACHEABLE)
		pmapflags = PMAP_WT;
	else
		pmapflags = PMAP_NOCACHE;

	for (; len > 0; len -= PAGE_SIZE) {
		pmap_kenter_pa(vaddr, spa | pmapflags, PROT_READ | PROT_WRITE);
		spa += PAGE_SIZE;
		vaddr += PAGE_SIZE;
	}
	pmap_update(pmap_kernel());

	return (0);
}

int
bus_space_alloc(bus_space_tag_t tag, bus_addr_t rstart, bus_addr_t rend,
    bus_size_t size, bus_size_t alignment, bus_size_t boundary, int flags,
    bus_addr_t *addrp, bus_space_handle_t *handlep)
{

	panic("bus_space_alloc: unimplemented");
}

void
bus_space_free(bus_space_tag_t tag, bus_space_handle_t handle, bus_size_t size)
{

	panic("bus_space_free: unimplemented");
}

void *
mapiodev(paddr_t pa, psize_t len)
{
	paddr_t spa;
	vaddr_t vaddr, va;
	int off;
	int size;

	spa = trunc_page(pa);
	off = pa - spa;
	size = round_page(off+len);
	if (ppc_malloc_ok == 0) {
		/* need to steal vm space before kernel vm is initialized */
		va = VM_MIN_KERNEL_ADDRESS + ppc_kvm_stolen;
		ppc_kvm_stolen += size;
		if (ppc_kvm_stolen > PPC_SEGMENT_LENGTH) {
			panic("ppc_kvm_stolen, out of space");
		}
	} else {
		va = (vaddr_t)km_alloc(size, &kv_any, &kp_none, &kd_nowait);
		if (va == 0)
			return (NULL);
	}

	for (vaddr = va; size > 0; size -= PAGE_SIZE) {
		pmap_kenter_pa(vaddr, spa, PROT_READ | PROT_WRITE);
		spa += PAGE_SIZE;
		vaddr += PAGE_SIZE;
	}
	pmap_update(pmap_kernel());

	return (void *) (va+off);
}

void
unmapiodev(void *kva, psize_t p_size)
{
	vaddr_t vaddr;
	int size;

	size = round_page(p_size);

	vaddr = trunc_page((vaddr_t)kva);

	pmap_kremove(vaddr, size);
	pmap_update(pmap_kernel());

	km_free((void *)vaddr, size, &kv_any, &kp_none);
}

/*
 * probably should be ppc_space_copy
 */

#define _CONCAT(A,B) A ## B
#define __C(A,B)	_CONCAT(A,B)

#define BUS_SPACE_COPY_N(BYTES,TYPE)					\
void									\
__C(bus_space_copy_,BYTES)(void *v, bus_space_handle_t h1,		\
    bus_size_t o1, bus_space_handle_t h2, bus_size_t o2,		\
    bus_size_t c)							\
{									\
	TYPE *src, *dst;						\
	int i;								\
									\
	src = (TYPE *) (h1+o1);						\
	dst = (TYPE *) (h2+o2);						\
									\
	if (h1 == h2 && o2 > o1)					\
		for (i = c-1; i >= 0; i--)				\
			dst[i] = src[i];				\
	else								\
		for (i = 0; i < c; i++)					\
			dst[i] = src[i];				\
}
BUS_SPACE_COPY_N(1,u_int8_t)
BUS_SPACE_COPY_N(2,u_int16_t)
BUS_SPACE_COPY_N(4,u_int32_t)

void
bus_space_set_region_1(bus_space_tag_t t, bus_space_handle_t h, bus_size_t o,
    u_int8_t val, bus_size_t c)
{
	u_int8_t *dst;
	int i;

	dst = (u_int8_t *) (h+o);

	for (i = 0; i < c; i++)
		dst[i] = val;
}

void
bus_space_set_region_2(bus_space_tag_t t, bus_space_handle_t h, bus_size_t o,
    u_int16_t val, bus_size_t c)
{
	u_int16_t *dst;
	int i;

	dst = (u_int16_t *) (h+o);
	val = swap16(val);

	for (i = 0; i < c; i++)
		dst[i] = val;
}
void
bus_space_set_region_4(bus_space_tag_t t, bus_space_handle_t h, bus_size_t o,
    u_int32_t val, bus_size_t c)
{
	u_int32_t *dst;
	int i;

	dst = (u_int32_t *) (h+o);
	val = swap32(val);

	for (i = 0; i < c; i++)
		dst[i] = val;
}

#define BUS_SPACE_READ_RAW_MULTI_N(BYTES,SHIFT,TYPE)			\
void									\
__C(bus_space_read_raw_multi_,BYTES)(bus_space_tag_t bst,		\
    bus_space_handle_t h, bus_addr_t o, u_int8_t *dst, bus_size_t size)	\
{									\
	TYPE *src;							\
	TYPE *rdst = (TYPE *)dst;					\
	int i;								\
	int count = size >> SHIFT;					\
									\
	src = (TYPE *)(h+o);						\
	for (i = 0; i < count; i++) {					\
		rdst[i] = *src;						\
		__asm__("eieio");					\
	}								\
}
BUS_SPACE_READ_RAW_MULTI_N(2,1,u_int16_t)
BUS_SPACE_READ_RAW_MULTI_N(4,2,u_int32_t)

#define BUS_SPACE_WRITE_RAW_MULTI_N(BYTES,SHIFT,TYPE)			\
void									\
__C(bus_space_write_raw_multi_,BYTES)( bus_space_tag_t bst,		\
    bus_space_handle_t h, bus_addr_t o, const u_int8_t *src,		\
    bus_size_t size)							\
{									\
	int i;								\
	TYPE *dst;							\
	TYPE *rsrc = (TYPE *)src;					\
	int count = size >> SHIFT;					\
									\
	dst = (TYPE *)(h+o);						\
	for (i = 0; i < count; i++) {					\
		*dst = rsrc[i];						\
		__asm__("eieio");					\
	}								\
}

BUS_SPACE_WRITE_RAW_MULTI_N(2,1,u_int16_t)
BUS_SPACE_WRITE_RAW_MULTI_N(4,2,u_int32_t)

int
bus_space_subregion(bus_space_tag_t t, bus_space_handle_t bsh,
    bus_size_t offset, bus_size_t size, bus_space_handle_t *nbshp)
{
	*nbshp = bsh + offset;
	return (0);
}

license: unknown


code:
/*	$OpenBSD: copystr.c,v 1.8 2003/10/15 02:43:09 drahn Exp $	*/

/*-
 * Copyright (C) 1995 Wolfgang Solfrank.
 * Copyright (C) 1995 TooLs GmbH.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *	This product includes software developed by TooLs GmbH.
 * 4. The name of TooLs GmbH may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY TOOLS GMBH ``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 TOOLS GMBH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#include <sys/param.h>
#include <sys/errno.h>
#include <sys/systm.h>

/*
 * Emulate copystr.
 */
int
copystr(const void *kfaddr, void *kdaddr, size_t len, size_t *done)
{
	u_char *kfp = (u_char *)kfaddr;
	u_char *kdp = kdaddr;
	size_t l;

	for (l = 0; len-- > 0; l++) {
		if (!(*kdp++ = *kfp++)) {
			if (done)
				*done = l + 1;
			return (0);
		}
	}
	if (done)
		*done = l;
	return (ENAMETOOLONG);
}


code:
/*	$OpenBSD: cpu_subr.c,v 1.8 2015/04/27 07:20:57 mpi Exp $	*/

/*
 * Copyright (c) 2013 Martin Pieuchot
 * Copyright (c) 2005 Mark Kettenis
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include <sys/param.h>

#include <powerpc/cpu.h>

int		ppc_cpuidle;		/* Support DOZE, NAP or DEEP NAP? */
int		ppc_altivec;		/* CPU has altivec support. */
int		ppc_proc_is_64b;	/* CPU is 64bit */
int		ppc_nobat;		/* Do not use BAT registers. */

struct patch {
	uint32_t *s;
	uint32_t *e;
};
extern struct patch	rfi_start, nop32_start, nopbat_start;
extern uint32_t		rfid_inst, nop_inst;

void
cpu_bootstrap(void)
{
	uint32_t cpu;
	uint32_t *inst;
	struct patch *p;

	cpu = ppc_mfpvr() >> 16;

	switch (cpu) {
	case PPC_CPU_IBM970:
	case PPC_CPU_IBM970FX:
	case PPC_CPU_IBM970MP:
		ppc_nobat = 1;
		ppc_proc_is_64b = 1;

		for (p = &rfi_start; p->s; p++) {
			for (inst = p->s; inst < p->e; inst++)
				*inst = rfid_inst;
			syncicache(p->s, (p->e - p->s) * sizeof(*p->e));
		}
		break;
	case PPC_CPU_MPC83xx:
		ppc_mtibat4u(0);
		ppc_mtibat5u(0);
		ppc_mtibat6u(0);
		ppc_mtibat7u(0);
		ppc_mtdbat4u(0);
		ppc_mtdbat5u(0);
		ppc_mtdbat6u(0);
		ppc_mtdbat7u(0);
		/* FALLTHROUGH */
	default:
		ppc_mtibat0u(0);
		ppc_mtibat1u(0);
		ppc_mtibat2u(0);
		ppc_mtibat3u(0);
		ppc_mtdbat0u(0);
		ppc_mtdbat1u(0);
		ppc_mtdbat2u(0);
		ppc_mtdbat3u(0);

		for (p = &nop32_start; p->s; p++) {
			for (inst = p->s; inst < p->e; inst++)
				*inst = nop_inst;
			syncicache(p->s, (p->e - p->s) * sizeof(*p->e));
		}
	}

	if (ppc_nobat) {
		for (p = &nopbat_start; p->s; p++) {
			for (inst = p->s; inst < p->e; inst++)
				*inst = nop_inst;
			syncicache(p->s, (p->e - p->s) * sizeof(*p->e));
		}
	}
}

void
ppc_mtscomc(u_int32_t val)
{
	int s;

	s = ppc_intr_disable();
	__asm volatile ("mtspr 276,%0; isync" :: "r" (val));
	ppc_intr_enable(s);
}

void
ppc_mtscomd(u_int32_t val)
{
	int s;

	s = ppc_intr_disable();
	__asm volatile ("mtspr 277,%0; isync" :: "r" (val));
	ppc_intr_enable(s);
}

u_int64_t
ppc64_mfscomc(void)
{
	u_int64_t ret;
	int s;

	s = ppc_intr_disable();
	__asm volatile ("mfspr %0,276;"
	    " mr %0+1, %0; srdi %0,%0,32" : "=r" (ret));
	ppc_intr_enable(s);
	return ret;
}

void
ppc64_mtscomc(u_int64_t val)
{
	int s;

	s = ppc_intr_disable();
	__asm volatile ("sldi %0,%0,32; or %0,%0,%0+1;"
	    " mtspr 276,%0; isync" :: "r" (val));
	ppc_intr_enable(s);
}

u_int64_t
ppc64_mfscomd(void)
{
	u_int64_t ret;
	int s;

	s = ppc_intr_disable();
	__asm volatile ("mfspr %0,277;"
            " mr %0+1, %0; srdi %0,%0,32" : "=r" (ret));
	ppc_intr_enable(s);
	return ret;
}

static __inline u_int32_t
ppc64_mfhid0(u_int32_t *lo)
{
	u_int32_t hid0_hi, hid0_lo;

	__asm volatile ("mfspr %0,1008;"
	    " mr %1, %0; srdi %0,%0,32;" : "=r" (hid0_hi), "=r" (hid0_lo));
	if (lo != NULL)
		*lo = hid0_lo;
	return hid0_hi;
}

static __inline void
ppc64_mthid0(u_int32_t hid0_hi, u_int32_t hid0_lo)
{
	/*
	 * No! It's not a joke (:
	 *
	 * Note 1 of the Table 2-3 from the 970MP User manual.
	 */
	__asm volatile ("sldi %0,%0,32; or %0,%0,%1;"
	    "sync; mtspr 1008,%0;"
	    "mfspr %0,1008; mfspr %0,1008; mfspr %0,1008;"
	    "mfspr %0,1008; mfspr %0,1008; mfspr %0,1008;"
	    "isync" :: "r" (hid0_hi), "r"(hid0_lo));
}

u_int32_t
ppc_mfhid0(void)
{
	u_int32_t ret;

	/* Since the lower 32 bits are reserved, do not expose them. */
	if (ppc_proc_is_64b)
		return ppc64_mfhid0(NULL);

	__asm volatile ("mfspr %0,1008" : "=r" (ret));
	return ret;
}

void
ppc_mthid0(u_int32_t val)
{
	if (ppc_proc_is_64b) {
		u_int32_t lo;

		/* Don't write any garbage in the lower 32 bits. */
		(void)ppc64_mfhid0(&lo);
		return ppc64_mthid0(val, lo);
	}

	__asm volatile ("mtspr 1008,%0; isync" :: "r" (val));
}

u_int64_t
ppc64_mfhid1(void)
{
	u_int64_t ret;

	__asm volatile ("mfspr %0,1009;"
            " mr %0+1, %0; srdi %0,%0,32" : "=r" (ret));
	return ret;
}

void
ppc64_mthid1(u_int64_t val)
{
	__asm volatile ("sldi %0,%0,32; or %0,%0,%0+1;"
	    "mtspr 1009,%0; mtspr 1009,%0; isync;" :: "r" (val));
}

u_int64_t
ppc64_mfhid4(void)
{
	u_int64_t ret;

	__asm volatile ("mfspr %0,1012;"
            " mr %0+1, %0; srdi %0,%0,32" : "=r" (ret));
	return ret;
}

void
ppc64_mthid4(u_int64_t val)
{
	__asm volatile ("sldi %0,%0,32; or %0,%0,%0+1;"
	    "sync; mtspr 1012,%0; isync;" :: "r" (val));
}

u_int64_t
ppc64_mfhid5(void)
{
	u_int64_t ret;

	__asm volatile ("mfspr %0,1014;"
            " mr %0+1, %0; srdi %0,%0,32" : "=r" (ret));
	return ret;
}

void
ppc64_mthid5(u_int64_t val)
{
	__asm volatile ("sldi %0,%0,32; or %0,%0,%0+1;"
	    "sync; mtspr 1014,%0; isync;" :: "r" (val));
}


code:
/*	$OpenBSD: fpu.c,v 1.14 2014/07/09 08:34:49 deraadt Exp $	*/
/*	$NetBSD: fpu.c,v 1.1 1996/09/30 16:34:44 ws Exp $	*/

/*
 * Copyright (C) 1996 Wolfgang Solfrank.
 * Copyright (C) 1996 TooLs GmbH.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *	This product includes software developed by TooLs GmbH.
 * 4. The name of TooLs GmbH may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY TOOLS GMBH ``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 TOOLS GMBH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#include <sys/param.h>
#include <sys/systm.h>
#include <sys/proc.h>
#include <sys/user.h>

#include <machine/fpu.h>
#include <machine/psl.h>

void
enable_fpu(struct proc *p)
{
	struct cpu_info *ci = curcpu();
	struct pcb *pcb = &p->p_addr->u_pcb;
	struct trapframe *tf = trapframe(p);
	int msr;

	if (!(pcb->pcb_flags & PCB_FPU)) {
		bzero(&pcb->pcb_fpu, sizeof pcb->pcb_fpu);
		pcb->pcb_flags |= PCB_FPU;
	}

	if (pcb->pcb_fpcpu != NULL || ci->ci_fpuproc != NULL) {
		printf("attempting to restore fpu state when in use pcb %p"
		    " fpproc %p\n", pcb->pcb_fpcpu, ci->ci_fpuproc);
	}
	msr = ppc_mfmsr();
	ppc_mtmsr((msr  & ~PSL_EE) | PSL_FP);
	__asm volatile("isync");

	asm volatile ("lfd 0,0(%0); mtfsf 0xff,0" :: "b"(&pcb->pcb_fpu.fpcsr));
	asm ("lfd 0,0(%0);"
	     "lfd 1,8(%0);"
	     "lfd 2,16(%0);"
	     "lfd 3,24(%0);"
	     "lfd 4,32(%0);"
	     "lfd 5,40(%0);"
	     "lfd 6,48(%0);"
	     "lfd 7,56(%0);"
	     "lfd 8,64(%0);"
	     "lfd 9,72(%0);"
	     "lfd 10,80(%0);"
	     "lfd 11,88(%0);"
	     "lfd 12,96(%0);"
	     "lfd 13,104(%0);"
	     "lfd 14,112(%0);"
	     "lfd 15,120(%0);"
	     "lfd 16,128(%0);"
	     "lfd 17,136(%0);"
	     "lfd 18,144(%0);"
	     "lfd 19,152(%0);"
	     "lfd 20,160(%0);"
	     "lfd 21,168(%0);"
	     "lfd 22,176(%0);"
	     "lfd 23,184(%0);"
	     "lfd 24,192(%0);"
	     "lfd 25,200(%0);"
	     "lfd 26,208(%0);"
	     "lfd 27,216(%0);"
	     "lfd 28,224(%0);"
	     "lfd 29,232(%0);"
	     "lfd 30,240(%0);"
	     "lfd 31,248(%0)" :: "b"(&pcb->pcb_fpu.fpr[0]));
	ci->ci_fpuproc = p;
	pcb->pcb_fpcpu = ci;
	tf->srr1 |= PSL_FP;
	ppc_mtmsr(msr);
	__asm volatile("isync");
}

void
save_fpu(void)
{
	struct cpu_info *ci = curcpu();
	struct pcb *pcb;
	struct proc *p;
	struct trapframe *tf;
	int msr;

	msr = ppc_mfmsr();
	ppc_mtmsr((msr  & ~PSL_EE) | PSL_FP);

	p = ci->ci_fpuproc;

	if (p == NULL) {
		ppc_mtmsr(msr);
		return;
	}

	pcb = &p->p_addr->u_pcb;

	__asm volatile("isync");

	asm ("stfd 0,0(%0);"
	     "stfd 1,8(%0);"
	     "stfd 2,16(%0);"
	     "stfd 3,24(%0);"
	     "stfd 4,32(%0);"
	     "stfd 5,40(%0);"
	     "stfd 6,48(%0);"
	     "stfd 7,56(%0);"
	     "stfd 8,64(%0);"
	     "stfd 9,72(%0);"
	     "stfd 10,80(%0);"
	     "stfd 11,88(%0);"
	     "stfd 12,96(%0);"
	     "stfd 13,104(%0);"
	     "stfd 14,112(%0);"
	     "stfd 15,120(%0);"
	     "stfd 16,128(%0);"
	     "stfd 17,136(%0);"
	     "stfd 18,144(%0);"
	     "stfd 19,152(%0);"
	     "stfd 20,160(%0);"
	     "stfd 21,168(%0);"
	     "stfd 22,176(%0);"
	     "stfd 23,184(%0);"
	     "stfd 24,192(%0);"
	     "stfd 25,200(%0);"
	     "stfd 26,208(%0);"
	     "stfd 27,216(%0);"
	     "stfd 28,224(%0);"
	     "stfd 29,232(%0);"
	     "stfd 30,240(%0);"
	     "stfd 31,248(%0)" :: "b"(&pcb->pcb_fpu.fpr[0]));
	asm volatile ("mffs 0; stfd 0,0(%0)" :: "b"(&pcb->pcb_fpu.fpcsr));
	asm ("lfd 0,0(%0);" :: "b"(&pcb->pcb_fpu.fpr[0]));

	tf = trapframe(ci->ci_fpuproc);
	tf->srr1 &= ~PSL_FP;
	ci->ci_fpuproc = NULL;
	pcb->pcb_fpcpu = NULL;

	ppc_mtmsr(msr);
	__asm volatile("isync");
}


code: #	$OpenBSD: genassym.cf,v 1.4 2010/04/26 22:11:38 deraadt Exp $


code:
/*	$OpenBSD: in_cksum.c,v 1.10 2014/07/22 10:35:35 mpi Exp $	*/
/*	$NetBSD: in_cksum.c,v 1.7 2003/07/15 02:54:48 lukem Exp $	*/

/*
 * Copyright 2001 Wasabi Systems, Inc.
 * All rights reserved.
 *
 * Written by Simon Burge and Eduardo Horvath for Wasabi Systems, Inc.
 *
 * 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. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *      This product includes software developed for the NetBSD Project by
 *      Wasabi Systems, Inc.
 * 4. The name of Wasabi Systems, Inc. may not be used to endorse
 *    or promote products derived from this software without specific prior
 *    written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY WASABI SYSTEMS, INC. ``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 WASABI SYSTEMS, INC
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/mbuf.h>
#include <sys/socketvar.h>

#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/ip_var.h>

/*
 * Checksum routine for Internet Protocol family headers.
 *
 * This routine is very heavily used in the network
 * code and should be modified for each CPU to be as fast as possible.
 *
 * PowerPC version.
 */

#define	REDUCE1		sum = (sum & 0xffff) + (sum >> 16)
/* Two REDUCE1s is faster than REDUCE1; if (sum > 65535) sum -= 65536; */
#define	REDUCE		{ REDUCE1; REDUCE1; }

static __inline__ int
in_cksum_internal(struct mbuf *m, int off, int len, u_int sum)
{
	uint8_t *w;
	int mlen = 0;
	int byte_swapped = 0;
	int n;

	union {
		uint8_t  c[2];
		uint16_t s;
	} s_util;

	for (;m && len; m = m->m_next) {
		if (m->m_len == 0)
			continue;
		w = mtod(m, uint8_t *) + off;

		/*
		 * 'off' can only be non-zero on the first pass of this
		 * loop when mlen != -1, so we don't need to worry about
		 * 'off' in the if clause below.
		 */
		if (mlen == -1) {
			/*
			 * The first byte of this mbuf is the continuation
			 * of a word spanning between this mbuf and the
			 * last mbuf.
			 *
			 * s_util.c[0] is already saved when scanning previous
			 * mbuf.
			 */
			s_util.c[1] = *w++;
			sum += s_util.s;
			mlen = m->m_len - 1;
			len--;
		} else {
			mlen = m->m_len - off;
			off = 0;
		}
		if (len < mlen)
			mlen = len;
		len -= mlen;

		/*
		 * Force to a word boundary.
		 */
		if ((3 & (long) w) && (mlen > 0)) {
			if ((1 & (long) w)) {
				REDUCE;
				sum <<= 8;
				s_util.c[0] = *w++;
				mlen--;
				byte_swapped = 1;
			}
			if ((2 & (long) w) && (mlen > 1)) {
				/*
				 * Since the `sum' may contain full 32 bit
				 * value, we can't simply add any value.
				 */
				__asm volatile(
				    "lhz 7,0(%1);"	/* load current data
							   half word */
				    "addc %0,%0,7;"	/* add to sum */
				    "addze %0,%0;"	/* add carry bit */
				    : "+r"(sum)
				    : "b"(w)
				    : "7");		/* clobber r7 */
				w += 2;
				mlen -= 2;
			}
		}

		if (mlen >= 64) {
			n = mlen >> 6;
			__asm volatile(
			    "addic 0,0,0;"		/* clear carry */
			    "mtctr %1;"			/* load loop count */
			    "1:"
			    "lwz 7,4(%2);"		/* load current data
							   word */
			    "lwz 8,8(%2);"
			    "lwz 9,12(%2);"
			    "lwz 10,16(%2);"
			    "adde %0,%0,7;"		/* add to sum */
			    "adde %0,%0,8;"
			    "adde %0,%0,9;"
			    "adde %0,%0,10;"
			    "lwz 7,20(%2);"
			    "lwz 8,24(%2);"
			    "lwz 9,28(%2);"
			    "lwz 10,32(%2);"
			    "adde %0,%0,7;"
			    "adde %0,%0,8;"
			    "adde %0,%0,9;"
			    "adde %0,%0,10;"
			    "lwz 7,36(%2);"
			    "lwz 8,40(%2);"
			    "lwz 9,44(%2);"
			    "lwz 10,48(%2);"
			    "adde %0,%0,7;"
			    "adde %0,%0,8;"
			    "adde %0,%0,9;"
			    "adde %0,%0,10;"
			    "lwz 7,52(%2);"
			    "lwz 8,56(%2);"
			    "lwz 9,60(%2);"
			    "lwzu 10,64(%2);"
			    "adde %0,%0,7;"
			    "adde %0,%0,8;"
			    "adde %0,%0,9;"
			    "adde %0,%0,10;"
			    "bdnz 1b;"			/* loop */
			    "addze %0,%0;"		/* add carry bit */
			    : "+r"(sum)
			    : "r"(n), "b"(w - 4)
			    : "7", "8", "9", "10");	/* clobber r7, r8, r9,
							   r10 */
			w += n * 64;
			mlen -= n * 64;
		}

		if (mlen >= 8) {
			n = mlen >> 3;
			__asm volatile(
			    "addic 0,0,0;"		/* clear carry */
			    "mtctr %1;"			/* load loop count */
			    "1:"
			    "lwz 7,4(%2);"		/* load current data
							   word */
			    "lwzu 8,8(%2);"
			    "adde %0,%0,7;"		/* add to sum */
			    "adde %0,%0,8;"
			    "bdnz 1b;"			/* loop */
			    "addze %0,%0;"		/* add carry bit */
			    : "+r"(sum)
			    : "r"(n), "b"(w - 4)
			    : "7", "8");		/* clobber r7, r8 */
			w += n * 8;
			mlen -= n * 8;
		}

		if (mlen == 0 && byte_swapped == 0)
			continue;
		REDUCE;

		while ((mlen -= 2) >= 0) {
			sum += *(uint16_t *)w;
			w += 2;
		}

		if (byte_swapped) {
			REDUCE;
			sum <<= 8;
			byte_swapped = 0;
			if (mlen == -1) {
				s_util.c[1] = *w;
				sum += s_util.s;
				mlen = 0;
			} else
				mlen = -1;
		} else if (mlen == -1)
			s_util.c[0] = *w;
	}
	if (len)
		printf("cksum: out of data\n");
	if (mlen == -1) {
		/* The last mbuf has odd # of bytes. Follow the
		   standard (the odd byte may be shifted left by 8 bits
		   or not as determined by endian-ness of the machine) */
		s_util.c[1] = 0;
		sum += s_util.s;
	}
	REDUCE;
	return (~sum & 0xffff);
}

int
in_cksum(struct mbuf *m, int len)
{

	return (in_cksum_internal(m, 0, len, 0));
}

int
in4_cksum(struct mbuf *m, uint8_t nxt, int off, int len)
{
	uint16_t *w;
	u_int sum = 0;
	union {
		struct ipovly ipov;
		u_int16_t w[10];
	} u;

	if (nxt != 0) {
		/* pseudo header */
		memset(&u.ipov, 0, sizeof(u.ipov));
		u.ipov.ih_len = htons(len);
		u.ipov.ih_pr = nxt;
		u.ipov.ih_src = mtod(m, struct ip *)->ip_src;
		u.ipov.ih_dst = mtod(m, struct ip *)->ip_dst;
		w = u.w;
		/* assumes sizeof(ipov) == 20 */
		sum += w[0]; sum += w[1]; sum += w[2]; sum += w[3]; sum += w[4];
		sum += w[5]; sum += w[6]; sum += w[7]; sum += w[8]; sum += w[9];
	}

	/* skip unnecessary part */
	while (m && off > 0) {
		if (m->m_len > off)
			break;
		off -= m->m_len;
		m = m->m_next;
	}

	return (in_cksum_internal(m, off, len, sum));
}


code:
/*	$OpenBSD: intr.c,v 1.8 2015/01/04 13:01:42 mpi Exp $	*/

/*
 * Copyright (c) 1997 Per Fogelstrom, Opsycon AB and RTMX Inc, USA.
 *
 * 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. All advertising materials mentioning features or use of this software
 *    must display the following acknowledgement:
 *	This product includes software developed under OpenBSD by
 *	Per Fogelstrom, Opsycon AB, Sweden for RTMX Inc, North Carolina USA.
 * 4. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 */
#include <sys/param.h>

#include <machine/cpu.h>
#include <machine/intr.h>

int ppc_dflt_splraise(int);
int ppc_dflt_spllower(int);
void ppc_dflt_splx(int);

/* provide a function for asm code to call */
#undef splraise
#undef spllower
#undef splx

int ppc_smask[IPL_NUM];

void
ppc_smask_init()
{
        int i;

        for (i = IPL_NONE; i <= IPL_HIGH; i++)  {
                ppc_smask[i] = 0;
                if (i < IPL_SOFTCLOCK)
                        ppc_smask[i] |= SI_TO_IRQBIT(SI_SOFTCLOCK);
                if (i < IPL_SOFTNET)
                        ppc_smask[i] |= SI_TO_IRQBIT(SI_SOFTNET);
                if (i < IPL_SOFTTTY)
                        ppc_smask[i] |= SI_TO_IRQBIT(SI_SOFTTTY);
        }
}

int
splraise(int newcpl)
{
	return ppc_intr_func.raise(newcpl);
}

int
spllower(int newcpl)
{
	return ppc_intr_func.lower(newcpl);
}

void
splx(int newcpl)
{
	ppc_intr_func.x(newcpl);
}

/*
 * functions with 'default' behavior to use before the real
 * interrupt controller attaches
 */

int
ppc_dflt_splraise(int newcpl)
{
        struct cpu_info *ci = curcpu();
        int oldcpl;

        oldcpl = ci->ci_cpl;
        if (newcpl < oldcpl)
                newcpl = oldcpl;
        ci->ci_cpl = newcpl;

        return (oldcpl);
}

int
ppc_dflt_spllower(int newcpl)
{
        struct cpu_info *ci = curcpu();
        int oldcpl;

        oldcpl = ci->ci_cpl;

        splx(newcpl);

        return (oldcpl);
}

void
ppc_dflt_splx(int newcpl)
{
        struct cpu_info *ci = curcpu();

        ci->ci_cpl = newcpl;

        if (ci->ci_ipending & ppc_smask[newcpl])
		dosoftint(newcpl);
}

struct ppc_intr_func ppc_intr_func =
{
        ppc_dflt_splraise,
	ppc_dflt_spllower,
	ppc_dflt_splx
};

char *
ppc_intr_typename(int type)
{
	switch (type) {
	case IST_NONE :
		return ("none");
	case IST_PULSE:
		return ("pulsed");
	case IST_EDGE:
		return ("edge-triggered");
	case IST_LEVEL:
		return ("level-triggered");
	default:
		return ("unknown");
	}
}

#ifdef DIAGNOSTIC
void
splassert_check(int wantipl, const char *func)
{
	struct cpu_info *ci = curcpu();

	if (ci->ci_cpl < wantipl)
		splassert_fail(wantipl, ci->ci_cpl, func);

	if (wantipl == IPL_NONE && ci->ci_intrdepth != -1)
		splassert_fail(-1, ci->ci_intrdepth, func);
}
#endif


code:
/*	$OpenBSD: lock_machdep.c,v 1.2 2015/06/26 12:46:13 dlg Exp $	*/

/*
 * Copyright (c) 2007 Artur Grabowski <art@openbsd.org>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/atomic.h>

#include <machine/lock.h>
#include <machine/psl.h>

#include <ddb/db_output.h>

void
__mp_lock_init(struct __mp_lock *lock)
{
	lock->mpl_cpu = NULL;
	lock->mpl_count = 0;
}

#if defined(MP_LOCKDEBUG)
#ifndef DDB
#error "MP_LOCKDEBUG requires DDB"
#endif

/* CPU-dependent timing, needs this to be settable from ddb. */
extern int __mp_lock_spinout;
#endif

#define SPINLOCK_SPIN_HOOK	/**/

static __inline void
__mp_lock_spin(struct __mp_lock *mpl)
{
#ifndef MP_LOCKDEBUG
	while (mpl->mpl_count != 0)
		SPINLOCK_SPIN_HOOK;
#else
	int ticks = __mp_lock_spinout;

	while (mpl->mpl_count != 0 && --ticks > 0)
		SPINLOCK_SPIN_HOOK;

	if (ticks == 0) {
		db_printf("__mp_lock(%p): lock spun out", mpl);
		Debugger();
	}
#endif
}

void
__mp_lock(struct __mp_lock *mpl)
{
	/*
	 * Please notice that mpl_count gets incremented twice for the
	 * first lock. This is on purpose. The way we release the lock
	 * in mp_unlock is to decrement the mpl_count and then check if
	 * the lock should be released. Since mpl_count is what we're
	 * spinning on, decrementing it in mpl_unlock to 0 means that
	 * we can't clear mpl_cpu, because we're no longer holding the
	 * lock. In theory mpl_cpu doesn't need to be cleared, but it's
	 * safer to clear it and besides, setting mpl_count to 2 on the
	 * first lock makes most of this code much simpler.
	 */

	while (1) {
		int s;

		s = ppc_intr_disable();
		if (atomic_cas_ulong(&mpl->mpl_count, 0, 1) == 0) {
			membar_enter();
			mpl->mpl_cpu = curcpu();
		}

		if (mpl->mpl_cpu == curcpu()) {
			mpl->mpl_count++;
			ppc_intr_enable(s);
			break;
		}
		ppc_intr_enable(s);

		__mp_lock_spin(mpl);
	}
}

void
__mp_unlock(struct __mp_lock *mpl)
{
	int s;

#ifdef MP_LOCKDEBUG
	if (mpl->mpl_cpu != curcpu()) {
		db_printf("__mp_unlock(%p): not held lock\n", mpl);
		Debugger();
	}
#endif

	s = ppc_intr_disable();
	if (--mpl->mpl_count == 1) {
		mpl->mpl_cpu = NULL;
		membar_exit();
		mpl->mpl_count = 0;
	}
	ppc_intr_enable(s);
}

int
__mp_release_all(struct __mp_lock *mpl)
{
	int rv = mpl->mpl_count - 1;
	int s;

#ifdef MP_LOCKDEBUG
	if (mpl->mpl_cpu != curcpu()) {
		db_printf("__mp_release_all(%p): not held lock\n", mpl);
		Debugger();
	}
#endif

	s = ppc_intr_disable();
	mpl->mpl_cpu = NULL;
	membar_exit();
	mpl->mpl_count = 0;
	ppc_intr_enable(s);

	return (rv);
}

int
__mp_release_all_but_one(struct __mp_lock *mpl)
{
	int rv = mpl->mpl_count - 2;

#ifdef MP_LOCKDEBUG
	if (mpl->mpl_cpu != curcpu()) {
		db_printf("__mp_release_all_but_one(%p): not held lock\n", mpl);
		Debugger();
	}
#endif

	mpl->mpl_count = 2;

	return (rv);
}

void
__mp_acquire_count(struct __mp_lock *mpl, int count)
{
	while (count--)
		__mp_lock(mpl);
}

int
__mp_lock_held(struct __mp_lock *mpl)
{
	return mpl->mpl_cpu == curcpu();
}


code:
/*	$OpenBSD: mutex.S,v 1.16 2014/06/18 18:42:29 kettenis Exp $	*/

/*
 * Copyright (c) 2007 Dale Rahn
 * Copyright (c) 2007 Mark Kettenis
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include "assym.h"

#include <machine/asm.h>

/* XXX */
#define GET_CPUINFO(r)	mfsprg r,0

ENTRY(__mtx_init)
	li	%r5,0
	stw	%r4,MTX_WANTIPL(%r3)
	stw	%r5,MTX_OLDCPL(%r3)
	stw	%r5,MTX_OWNER(%r3)
	blr

ENTRY(mtx_enter)
	stwu	%r1,-16(%r1)			# reserve stack
	mflr	%r0
	stw	%r0,20(%r1)			# save return address
.L_retry:
	stw	%r3, 12(%r1)
	lwz	%r3,MTX_WANTIPL(%r3)		# load new ipl
	bl	_C_LABEL(splraise)
	mr	%r7, %r3
	GET_CPUINFO(%r4)
	lwz	%r3,12(%r1)
	li	%r5,MTX_OWNER			# load offset constant
	lwarx	%r6,%r5,%r3			# load reserve owner
	cmpwi	0,%r6,0				# test owner == 0
	beq+	0,.L_mutex_free			# if owner == 0 branch free
.L_mutex_locked:
#ifdef DIAGNOSTIC
	cmpl	0,%r4,%r6
	beq-	.L_mutex_selflocked
#endif
	stw	%r3,12(%r1)			# save mtx during lcsplx
	la	%r4,12(%r1)
	stwcx.	%r3,0,%r4			# unreserve owner
	mr	%r3,%r7				# move old cpl to arg0
	bl	_C_LABEL(lcsplx)		# call splx on old cpl
	lwz	%r3,12(%r1)
	b	.L_retry

.L_mutex_free:
	stwcx.	%r4,%r5,%r3			# old owner was 0 cond store
	bne-	.L_mutex_locked			# branch if reserve cancelled
	isync					# memory barrier
#ifdef DIAGNOSTIC
	lwz	%r6,CI_MUTEX_LEVEL(%r4)
	addi	%r6,%r6,1			# curpcu->ci_mutex_level++
	stw	%r6,CI_MUTEX_LEVEL(%r4)
#endif
	stw	%r7,MTX_OLDCPL(%r3)		# save old ipl
	lwz	%r0,20(%r1)			# load return address
	mtlr	%r0
	addi	%r1,%r1,16			# restore stack
	blr

#ifdef DIAGNOSTIC
.L_mutex_selflocked:
	mr	%r5, %r3
	lis	%r3,.L_paniclocked@ha
	la	%r3,.L_paniclocked@l(%r3)
	bl 	panic
.L_paniclocked:
	.string "mtx_enter: recursed %x %x\n"
#endif

ENTRY(mtx_enter_try)
	stwu	%r1,-16(%r1)			# reserve stack
	mflr	%r0
	stw	%r0,20(%r1)			# save return address
	stw	%r3, 12(%r1)
	lwz	%r3,MTX_WANTIPL(%r3)		# load new ipl
	bl	_C_LABEL(splraise)
	mr	%r7, %r3
	GET_CPUINFO(%r4)
	lwz	%r3,12(%r1)
	li	%r5,MTX_OWNER			# load offset constant
	lwarx	%r6,%r5,%r3			# load reserve owner
	cmpwi	0,%r6,0				# test owner == 0
	beq+	0,.L_mutex_try_free		# if owner == 0 branch free
.L_mutex_try_locked:
#ifdef DIAGNOSTIC
	cmpl	0,%r4,%r6
	beq-	.L_mutex_try_selflocked
#endif
	stw	%r3,12(%r1)			# save mtx during lcsplx
	la	%r4,12(%r1)
	stwcx.	%r3,0,%r4			# unreserve owner
	mr	%r3,%r7				# move old cpl to arg0
	bl	_C_LABEL(lcsplx)		# call splx on old cpl

	lwz	%r0,20(%r1)			# load return address
	mtlr	%r0
	addi	%r1,%r1,16			# restore stack
	li	%r3,0				# return zero
	blr

.L_mutex_try_free:
	stwcx.	%r4,%r5,%r3			# old owner was 0 cond store
	bne-	.L_mutex_try_locked		# branch if reserve cancelled
	isync					# memory barrier
#ifdef DIAGNOSTIC
	lwz	%r6,CI_MUTEX_LEVEL(%r4)
	addi	%r6,%r6,1			# curpcu->ci_mutex_level++
	stw	%r6,CI_MUTEX_LEVEL(%r4)
#endif
	stw	%r7,MTX_OLDCPL(%r3)		# save old ipl
	lwz	%r0,20(%r1)			# load return address
	mtlr	%r0
	addi	%r1,%r1,16			# restore stack
	li	%r3,1				# return nonzero
	blr

#ifdef DIAGNOSTIC
.L_mutex_try_selflocked:
	mr	%r5, %r3
	lis	%r3,.L_panictrylocked@ha
	la	%r3,.L_panictrylocked@l(%r3)
	bl 	panic
.L_panictrylocked:
	.string "mtx_enter_try: recursed %x %x\n"
#endif

ENTRY(mtx_leave)
#ifdef DIAGNOSTIC
	lwz	%r6,MTX_OWNER(%r3)
	cmpwi   0,%r6,0				# test owner == 0

	beq-	.L_mutex_notlocked
#endif
	li	%r4,0
	lwz	%r5,MTX_OLDCPL(%r3)
	stw	%r4,MTX_OLDCPL(%r3)
	sync					# memory barrier
	stw	%r4,MTX_OWNER(%r3)
	GET_CPUINFO(%r4)
#ifdef DIAGNOSTIC
	lwz	%r6,CI_MUTEX_LEVEL(%r4)
	addi	%r6,%r6,-1			# curpcu->ci_mutex_level--
	stw	%r6,CI_MUTEX_LEVEL(%r4)
#endif
	mr	%r3,%r5
	lwz	%r5,CI_CPL(%r4)
	cmpl	0,%r3,%r5
	beq	1f
	b	_C_LABEL(lcsplx)
1:
	blr

#ifdef DIAGNOSTIC
.L_mutex_notlocked:
	GET_CPUINFO(%r4)
	mr	%r5, %r3
	lis	%r3,.L_panicnotlocked@ha
	la	%r3,.L_panicnotlocked@l(%r3)
	bl 	panic
.L_panicnotlocked:
	.string "mtx_leave: not locked %x %x\n"
#endif