code:
/*	$OpenBSD: m88100_fp.c,v 1.3 2014/07/02 20:28:08 miod Exp $	*/

/*
 * Copyright (c) 2007, 2014, Miodrag Vallat.
 *
 * 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, this permission notice, and the disclaimer below
 * 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/types.h>
#include <sys/param.h>
#include <sys/proc.h>
#include <sys/signalvar.h>
#include <sys/systm.h>

#include <machine/fpu.h>
#include <machine/frame.h>
#include <machine/ieee.h>
#include <machine/ieeefp.h>
#include <machine/trap.h>
#include <machine/m88100.h>

#include <lib/libkern/softfloat.h>
#include <lib/libkern/milieu.h>
float32 normalizeRoundAndPackFloat32(int, int16, bits32);
float64 normalizeRoundAndPackFloat64(flag, int16, bits64);

#include <m88k/m88k/fpu.h>

int	m88100_fpu_emulate(struct trapframe *);
void	m88100_fpu_fetch(struct trapframe *, u_int, u_int, u_int, fparg *);
void	m88100_fpu_checksig(struct trapframe *, int, int);

/*
 * All 88100 precise floating-point exceptions are handled there.
 *
 * We ignore the exception cause register completely, except for the
 * `privilege violation' bit, and attempt to perform the computation in
 * software if needed.
 */

void
m88100_fpu_precise_exception(struct trapframe *frame)
{
	int fault_type;
	int sig;

	/* if FPECR_FUNIMP is set, all other bits are undefined, ignore them */
	if (ISSET(frame->tf_fpecr, FPECR_FUNIMP))
		frame->tf_fpecr = FPECR_FUNIMP;

	/* Reset the exception cause register */
	__asm__ volatile ("fstcr %r0, %fcr0");

	if (ISSET(frame->tf_fpecr, FPECR_FPRV)) {
		sig = SIGILL;
		fault_type = ILL_PRVREG;
	} else {
		sig = m88100_fpu_emulate(frame);
		fault_type = SI_NOINFO;
	}

	/*
	 * Update the floating point status register regardless of
	 * whether we'll deliver a signal or not.
	 */
	__asm__ volatile ("fstcr %0, %%fcr62" :: "r"(frame->tf_fpsr));

	m88100_fpu_checksig(frame, sig, fault_type);
}

/*
 * Convert a single floating-point argument with its exponent sign-extended
 * to 11 bits in an fphs/fpls pair to a correct 32-bit single precision
 * number.
 */
static inline uint32_t
m88100_fpu_parse_single(uint32_t hs, uint32_t ls)
{
	uint32_t result;

	result = hs << (DBL_EXPBITS - SNG_EXPBITS);
	result &= ~(1U << 31);		/* clear carry into sign bit */
	result |= ls >> (DBL_FRACBITS - SNG_FRACBITS);
	result |= hs & (1U << 31);	/* sign bit */

	return result;
}

/*
 * Load a floating-point argument into a fparg union, then convert it to
 * the required format if it is of larger precision.
 *
 * This assumes the final format (width) is not FTYPE_INT, and the original
 * format (orig_width) <= width.
 */
void
m88100_fpu_fetch(struct trapframe *frame, u_int operandno, u_int orig_width,
    u_int width, fparg *dest)
{
	u_int32_t tmp;

	switch (orig_width) {
	case FTYPE_INT:
		tmp = operandno == 1 ? frame->tf_fpls1 : frame->tf_fpls2;
		switch (width) {
		case FTYPE_SNG:
			dest->sng = int32_to_float32(tmp);
			break;
		case FTYPE_DBL:
			dest->dbl = int32_to_float64(tmp);
			break;
		}
		break;
	case FTYPE_SNG:
		tmp = operandno == 1 ?
		    m88100_fpu_parse_single(frame->tf_fphs1, frame->tf_fpls1) :
		    m88100_fpu_parse_single(frame->tf_fphs2, frame->tf_fpls2);
		switch (width) {
		case FTYPE_SNG:
			dest->sng = tmp;
			break;
		case FTYPE_DBL:
			dest->dbl = float32_to_float64(tmp);
			break;
		}
		break;
	case FTYPE_DBL:
		tmp = operandno == 1 ? frame->tf_fphs1 : frame->tf_fphs2;
		dest->dbl = ((float64)tmp) << 32;
		tmp = operandno == 1 ? frame->tf_fpls1 : frame->tf_fpls2;
		dest->dbl |= (float64)tmp;
		break;
	}
}

/*
 * Emulate an FPU instruction.  On return, the trapframe registers
 * will be modified to reflect the settings the hardware would have left.
 */
int
m88100_fpu_emulate(struct trapframe *frame)
{
	u_int rd, t1, t2, td, tmax, opcode;
	u_int32_t old_fpsr, old_fpcr;
	int rc;

	fparg arg1, arg2, dest;

	/*
	 * Crack the instruction.
	 */
	rd = frame->tf_fppt & 0x1f;
	opcode = (frame->tf_fppt >> 11) & 0x1f;
	t1 = (frame->tf_fppt >> 9) & 0x03;
	t2 = (frame->tf_fppt >> 7) & 0x03;
	td = (frame->tf_fppt >> 5) & 0x03;

	if (rd == 0)	/* r0 not allowed as destination */
		return (SIGILL);

	switch (opcode) {
	case 0x00:	/* fmul */
	case 0x05:	/* fadd */
	case 0x06:	/* fsub */
	case 0x0e:	/* fdiv */
		if ((t1 != FTYPE_SNG && t1 != FTYPE_DBL) ||
		    (t2 != FTYPE_SNG && t2 != FTYPE_DBL) ||
		    (td != FTYPE_SNG && td != FTYPE_DBL))
			return (SIGILL);
		break;
	case 0x04:	/* flt */
		if ((td != FTYPE_SNG && td != FTYPE_DBL) ||
		    t2 != 0x00 || t1 != 0x00)
			return (SIGILL);
		break;
	case 0x07:	/* fcmp */
		if ((t1 != FTYPE_SNG && t1 != FTYPE_DBL) ||
		    (t2 != FTYPE_SNG && t2 != FTYPE_DBL) ||
		    td != 0x00)
			return (SIGILL);
		break;
	case 0x09:	/* int */
	case 0x0a:	/* nint */
	case 0x0b:	/* trnc */
		if ((t2 != FTYPE_SNG && t2 != FTYPE_DBL) ||
		    t1 != 0x00 || td != 0x00)
			return (SIGILL);
		break;
	default:
		return (SIGILL);
	}

	/*
	 * Temporarily reset the status register, so that we can tell
	 * which exceptions are new after processing the opcode.
	 */
	old_fpsr = frame->tf_fpsr;
	frame->tf_fpsr = 0;

	/*
	 * Save fpcr as well, since we might need to change rounding mode
	 * temporarily.
	 */
	old_fpcr = frame->tf_fpcr;

	/*
	 * The logic for instruction emulation is:
	 *
	 * - the computation precision is the largest one of all the operands.
	 * - all source operands are converted to this precision if needed.
	 * - computation is performed.
	 * - the result is stored into the destination operand, converting it
	 *   to the destination precision if lower.
	 */

	switch (opcode) {
	case 0x00:	/* fmul */
		tmax = fpu_precision(t1, t2, td);
		m88100_fpu_fetch(frame, 1, t1, tmax, &arg1);
		m88100_fpu_fetch(frame, 2, t2, tmax, &arg2);
		switch (tmax) {
		case FTYPE_SNG:
			dest.sng = float32_mul(arg1.sng, arg2.sng);
			break;
		case FTYPE_DBL:
			dest.dbl = float64_mul(arg1.dbl, arg2.dbl);
			break;
		}
		fpu_store(frame, rd, tmax, td, &dest);
		break;

	case 0x04:	/* flt */
		m88100_fpu_fetch(frame, 2, FTYPE_INT, td, &dest);
		fpu_store(frame, rd, td, td, &dest);
		break;

	case 0x05:	/* fadd */
		tmax = fpu_precision(t1, t2, td);
		m88100_fpu_fetch(frame, 1, t1, tmax, &arg1);
		m88100_fpu_fetch(frame, 2, t2, tmax, &arg2);
		switch (tmax) {
		case FTYPE_SNG:
			dest.sng = float32_add(arg1.sng, arg2.sng);
			break;
		case FTYPE_DBL:
			dest.dbl = float64_add(arg1.dbl, arg2.dbl);
			break;
		}
		fpu_store(frame, rd, tmax, td, &dest);
		break;

	case 0x06:	/* fsub */
		tmax = fpu_precision(t1, t2, td);
		m88100_fpu_fetch(frame, 1, t1, tmax, &arg1);
		m88100_fpu_fetch(frame, 2, t2, tmax, &arg2);
		switch (tmax) {
		case FTYPE_SNG:
			dest.sng = float32_sub(arg1.sng, arg2.sng);
			break;
		case FTYPE_DBL:
			dest.dbl = float64_sub(arg1.dbl, arg2.dbl);
			break;
		}
		fpu_store(frame, rd, tmax, td, &dest);
		break;

	case 0x07:	/* fcmp */
		tmax = fpu_precision(t1, t2, IGNORE_PRECISION);
		m88100_fpu_fetch(frame, 1, t1, tmax, &arg1);
		m88100_fpu_fetch(frame, 2, t2, tmax, &arg2);
		fpu_compare(frame, &arg1, &arg2, tmax, rd, 0);
		break;

	case 0x09:	/* int */
do_int:
		m88100_fpu_fetch(frame, 2, t2, t2, &dest);
		fpu_store(frame, rd, t2, FTYPE_INT, &dest);
		break;

	case 0x0a:	/* nint */
		/* round to nearest */
		frame->tf_fpcr = (old_fpcr & ~(FPCR_RD_MASK << FPCR_RD_SHIFT)) |
		    (FP_RN << FPCR_RD_SHIFT);
		goto do_int;

	case 0x0b:	/* trnc */
		/* round towards zero */
		frame->tf_fpcr = (old_fpcr & ~(FPCR_RD_MASK << FPCR_RD_SHIFT)) |
		    (FP_RZ << FPCR_RD_SHIFT);
		goto do_int;

	case 0x0e:	/* fdiv */
		tmax = fpu_precision(t1, t2, td);
		m88100_fpu_fetch(frame, 1, t1, tmax, &arg1);
		m88100_fpu_fetch(frame, 2, t2, tmax, &arg2);
		switch (tmax) {
		case FTYPE_SNG:
			dest.sng = float32_div(arg1.sng, arg2.sng);
			break;
		case FTYPE_DBL:
			dest.dbl = float64_div(arg1.dbl, arg2.dbl);
			break;
		}
		fpu_store(frame, rd, tmax, td, &dest);
		break;
	}

	/*
	 * Mark new exceptions, if any, in the fpsr, and decide whether
	 * to send a signal or not.
	 */

	if (frame->tf_fpsr & old_fpcr)
		rc = SIGFPE;
	else
		rc = 0;
	frame->tf_fpsr |= old_fpsr;

	/*
	 * Restore fpcr as well.
	 */
	frame->tf_fpcr = old_fpcr;

	return (rc);
}

/*
 * All 88100 imprecise floating-point exceptions are handled there.
 *
 * We ignore the exception condition bits and simply round the intermediate
 * result according to the current rounding mode, raising whichever exception
 * conditions are necessary in the process.
 */

void
m88100_fpu_imprecise_exception(struct trapframe *frame)
{
	flag sign;
	int16 exp;
	bits32 mant32;
	bits64 mant64;
	fparg res;
	u_int fmt;

	/* Reset the exception cause register */
	__asm__ volatile ("fstcr %r0, %fcr0");

	/*
	 * Pick the inexact result, build a float32 or a float64 out of it, and
	 * normalize it to the destination width.
	 */

	if (frame->tf_fpit & FPIT_DBL)
		fmt = FTYPE_DBL;
	else
		fmt = FTYPE_SNG;

	sign = (frame->tf_fprh & FPRH_SIGN) != 0;
	exp = ((int32_t)frame->tf_fpit) >> 20; /* signed, unbiaised exponent */

	if (fmt == FTYPE_SNG) {
		exp += SNG_EXP_BIAS;
		mant32 = (frame->tf_fprh & FPRH_MANTH_MASK);
	       	mant32 <<= (SNG_FRACBITS - FPRH_MANTH_BITS + 1);
		mant32 |= frame->tf_fprl >> (DBL_FRACBITS - SNG_FRACBITS);

		/*
		 * If the mantissa has been incremented, revert this; but
		 * if doing so causes the mantissa hidden bit to clear,
		 * the exponent needs to be decremented and the hidden bit
		 * restored.
		 */
		if (frame->tf_fprh & FPRH_ADDONE) {
			mant32--;
			if ((mant32 & (1 << SNG_FRACBITS)) == 0) {
				exp--;
				mant32 |= 1 << SNG_FRACBITS;
			}
		}

		/* normalizeRoundAndPackFloat32() requirement */
		mant32 <<= (31 - SNG_FRACBITS - 1);
		res.sng = normalizeRoundAndPackFloat32(sign, exp, mant32);
	} else {
		exp += DBL_EXP_BIAS;
		mant64 = frame->tf_fprh & FPRH_MANTH_MASK;
		mant64 <<= (DBL_FRACBITS - FPRH_MANTH_BITS + 1);
		mant64 |= frame->tf_fprl;

		/*
		 * If the mantissa has been incremented, revert this; but
		 * if doing so causes the mantissa hidden bit to clear,
		 * the exponent needs to be decremented and the hidden bit
		 * restored.
		 */
		if (frame->tf_fprh & FPRH_ADDONE) {
			mant64--;
			if ((mant64 & (1LL << DBL_FRACBITS)) == 0) {
				exp--;
				mant64 |= 1LL << DBL_FRACBITS;
			}
		}

		/* normalizeRoundAndPackFloat64() requirement */
		mant64 <<= (63 - DBL_FRACBITS - 1);
		res.dbl = normalizeRoundAndPackFloat64(sign, exp, mant64);
	}

	fpu_store(frame, frame->tf_fpit & 0x1f, fmt, fmt, &res);

	/*
	 * Update the floating point status register regardless of
	 * whether we'll deliver a signal or not.
	 */
	__asm__ volatile ("fstcr %0, %%fcr62" :: "r"(frame->tf_fpsr));

	/* Check for a SIGFPE condition */
	if (frame->tf_fpsr & frame->tf_fpcr)
		m88100_fpu_checksig(frame, SIGFPE, 0 /* SI_NOINFO */);
}

/*
 * Check if a signal needs to be delivered, and send it.
 */
void
m88100_fpu_checksig(struct trapframe *frame, int sig, int fault_type)
{
	struct proc *p = curproc;
	union sigval sv;

	if (sig != 0) {
		if (sig == SIGILL) {
			if (fault_type == SI_NOINFO)
				fault_type = ILL_ILLOPC;
		} else {
			if (frame->tf_fpecr & FPECR_FIOV)
				fault_type = FPE_FLTSUB;
			else if (frame->tf_fpecr & FPECR_FROP)
				fault_type = FPE_FLTINV;
			else if (frame->tf_fpecr & FPECR_FDVZ)
				fault_type = FPE_INTDIV;
			else if (frame->tf_fpecr & FPECR_FUNF) {
				if (frame->tf_fpsr & FPSR_EFUNF)
					fault_type = FPE_FLTUND;
				else if (frame->tf_fpsr & FPSR_EFINX)
					fault_type = FPE_FLTRES;
			} else if (frame->tf_fpecr & FPECR_FOVF) {
				if (frame->tf_fpsr & FPSR_EFOVF)
					fault_type = FPE_FLTOVF;
				else if (frame->tf_fpsr & FPSR_EFINX)
					fault_type = FPE_FLTRES;
			} else if (frame->tf_fpecr & FPECR_FINX)
				fault_type = FPE_FLTRES;
		}

		sv.sival_ptr = (void *)(frame->tf_sxip & XIP_ADDR);
		KERNEL_LOCK();
		trapsignal(p, sig, 0, fault_type, sv);
		KERNEL_UNLOCK();
	}
}


code:
/*	$OpenBSD: m88100_machdep.c,v 1.12 2014/05/31 11:19:06 miod Exp $	*/
/*
 * Mach Operating System
 * Copyright (c) 1993-1991 Carnegie Mellon University
 * Copyright (c) 1991 OMRON Corporation
 * All Rights Reserved.
 *
 * Permission to use, copy, modify and distribute this software and its
 * documentation is hereby granted, provided that both the copyright
 * notice and this permission notice appear in all copies of the
 * software, derivative works or modified versions, and any portions
 * thereof, and that both notices appear in supporting documentation.
 *
 * CARNEGIE MELLON AND OMRON ALLOW FREE USE OF THIS SOFTWARE IN ITS "AS IS"
 * CONDITION.  CARNEGIE MELLON AND OMRON DISCLAIM ANY LIABILITY OF ANY KIND
 * FOR ANY DAMAGES WHATSOEVER RESULTING FROM THE USE OF THIS SOFTWARE.
 *
 * Carnegie Mellon requests users of this software to return to
 *
 *  Software Distribution Coordinator  or  Software.Distribution@CS.CMU.EDU
 *  School of Computer Science
 *  Carnegie Mellon University
 *  Pittsburgh PA 15213-3890
 *
 * any improvements or extensions that they make and grant Carnegie the
 * rights to redistribute these changes.
 */

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

#include <machine/asm_macro.h>
#include <m88k/m88100.h>

#ifdef MULTIPROCESSOR
uint32_t m88100_mp_atomic_begin(__cpu_simple_lock_t *, uint *);
void	m88100_mp_atomic_end(uint32_t, __cpu_simple_lock_t *, uint);
#endif

/*
 *  Data Access Emulation for M88100 exceptions
 */

#define DMT_BYTE	1
#define DMT_HALF	2
#define DMT_WORD	4

const struct {
	unsigned char    offset;
	unsigned char    size;
} dmt_en_info[16] = {
	{0, 0},
	{3, DMT_BYTE},
	{2, DMT_BYTE},
	{2, DMT_HALF},
	{1, DMT_BYTE},
	{0, 0},
	{0, 0},
	{0, 0},
	{0, DMT_BYTE},
	{0, 0},
	{0, 0},
	{0, 0},
	{0, DMT_HALF},
	{0, 0},
	{0, 0},
	{0, DMT_WORD}
};

#ifdef DATA_DEBUG
int data_access_emulation_debug = 0;
#define DAE_DEBUG(stuff) \
	do { \
		if (data_access_emulation_debug != 0) { \
			stuff; \
		} \
	} while (0)
#else
#define DAE_DEBUG(stuff)
#endif

void	dae_print_one(u_int, u_int, u_int, u_int);
void	dae_process(struct trapframe *, u_int, u_int, u_int, u_int);

void
dae_print(u_int *f)
{
	struct trapframe *eframe = (void *)f;

	if (!ISSET(eframe->tf_dmt0, DMT_VALID))
		return;

	dae_print_one(0, eframe->tf_dma0, eframe->tf_dmd0, eframe->tf_dmt0);
	dae_print_one(1, eframe->tf_dma1, eframe->tf_dmd1, eframe->tf_dmt1);
	dae_print_one(2, eframe->tf_dma2, eframe->tf_dmd2, eframe->tf_dmt2);
}

void
dae_print_one(u_int x, u_int dmax, u_int dmdx, u_int dmtx)
{
	u_int enbits;
	const char *width, *usr, *xmem;

	if (!ISSET(dmtx, DMT_VALID))
		return;

	enbits = DMT_ENBITS(dmtx);
	dmax += dmt_en_info[enbits].offset;

	if (dmtx & DMT_DOUB1)
		width = ".d";
	else {
		switch (dmt_en_info[enbits].size) {
		case DMT_BYTE:
			if (dmtx & DMT_SIGNED)
				width = ".b";
			else
				width = ".bu";
			break;
		case DMT_HALF:
			if (dmtx & DMT_SIGNED)
				width = ".h";
			else
				width = ".hu";
			break;
		case DMT_WORD:
			width = "";
			break;
		default:
			width = ".???";
			break;
		}
	}
	if (dmtx & DMT_DAS)
		usr = "";
	else
		usr = ".usr";
	if (dmtx & DMT_LOCKBAR)
		xmem = "(xmem)";
	else
		xmem = "";

	if (ISSET(dmtx, DMT_WRITE))
		printf("[DMT%d=%x: %sst%s%s %08x to %08x]\n",
		    x, dmtx, xmem, width, usr, dmdx, dmax);
	else
		printf("[DMT%d=%x: %sld%s%s r%d <- %x]\n",
		    x, dmtx, xmem, width, usr, DMT_DREGBITS(dmtx), dmax);
}

void
data_access_emulation(u_int *f)
{
	struct trapframe *eframe = (void *)f;

	if (!ISSET(eframe->tf_dmt0, DMT_VALID))
		return;

	dae_process(eframe, 0,
	    eframe->tf_dma0, eframe->tf_dmd0, eframe->tf_dmt0);
	dae_process(eframe, 1,
	    eframe->tf_dma1, eframe->tf_dmd1, eframe->tf_dmt1);
	dae_process(eframe, 2,
	    eframe->tf_dma2, eframe->tf_dmd2, eframe->tf_dmt2);

	eframe->tf_dmt0 = 0;
}

void
dae_process(struct trapframe *eframe, u_int x,
    u_int dmax, u_int dmdx, u_int dmtx)
{
	u_int v, reg, enbits;

	if (!ISSET(dmtx, DMT_VALID))
		return;

	DAE_DEBUG(dae_print_one(x, dmax, dmdx, dmtx));

	enbits = DMT_ENBITS(dmtx);
	dmax += dmt_en_info[enbits].offset;
	reg = DMT_DREGBITS(dmtx);

	if (!ISSET(dmtx, DMT_LOCKBAR)) {
		/* the fault is not during an XMEM */

		if (x == 2 && ISSET(dmtx, DMT_DOUB1)) {
			/* pipeline 2 (earliest stage) for a double */

			if (ISSET(dmtx, DMT_WRITE)) {
				/*
				 * STORE DOUBLE WILL BE REINITIATED BY rte
				 */
			} else {
				/* EMULATE ld.d INSTRUCTION */
				v = do_load_word(dmax, dmtx & DMT_DAS);
				if (reg != 0)
					eframe->tf_r[reg] = v;
				v = do_load_word(dmax ^ 4, dmtx & DMT_DAS);
				if (reg != 31)
					eframe->tf_r[reg + 1] = v;
			}
		} else {
			/* not pipeline #2 with a double */
			if (dmtx & DMT_WRITE) {
				switch (dmt_en_info[enbits].size) {
				case DMT_BYTE:
				DAE_DEBUG(
					printf("[byte %x -> %08x(%c)]\n",
					    dmdx & 0xff, dmax,
					    ISSET(dmtx, DMT_DAS) ? 's' : 'u')
				);
					do_store_byte(dmax, dmdx,
					    dmtx & DMT_DAS);
					break;
				case DMT_HALF:
				DAE_DEBUG(
					printf("[half %x -> %08x(%c)]\n",
					    dmdx & 0xffff, dmax,
					    ISSET(dmtx, DMT_DAS) ? 's' : 'u')
				);
					do_store_half(dmax, dmdx,
					    dmtx & DMT_DAS);
					break;
				case DMT_WORD:
				DAE_DEBUG(
					printf("[word %x -> %08x(%c)]\n",
					    dmdx, dmax,
					    ISSET(dmtx, DMT_DAS) ? 's' : 'u')
				);
					do_store_word(dmax, dmdx,
					    dmtx & DMT_DAS);
					break;
				}
			} else {
				/* else it's a read */
				switch (dmt_en_info[enbits].size) {
				case DMT_BYTE:
					v = do_load_byte(dmax, dmtx & DMT_DAS);
					if (!ISSET(dmtx, DMT_SIGNED))
						v &= 0x000000ff;
					break;
				case DMT_HALF:
					v = do_load_half(dmax, dmtx & DMT_DAS);
					if (!ISSET(dmtx, DMT_SIGNED))
						v &= 0x0000ffff;
					break;
				case DMT_WORD:
					v = do_load_word(dmax, dmtx & DMT_DAS);
					break;
				}
				DAE_DEBUG(
					if (reg == 0)
						printf("[no write to r0 done]\n");
					else
						printf("[r%d <- %08x]\n", reg, v);
				);
				if (reg != 0)
					eframe->tf_r[reg] = v;
			}
		}
	} else {
		/* if lockbar is set... it's part of an XMEM */
		/*
		 * According to Motorola's "General Information",
		 * the DMT_DOUB1 bit is never set in this case, as it
		 * should be.
		 * If lockbar is set (as it is if we're here) and if
		 * the write is not set, then it's the same as if DOUB1
		 * was set...
		 */
		if (!ISSET(dmtx, DMT_WRITE)) {
			if (x < 2) {
				/* RERUN xmem WITH DMD(x+1) */
				dmdx =
				    x == 0 ? eframe->tf_dmd1 : eframe->tf_dmd2;
			} else {
				/* RERUN xmem WITH DMD2 */
			}

			if (dmt_en_info[enbits].size == DMT_WORD) {
				v = do_xmem_word(dmax, dmdx, dmtx & DMT_DAS);
			} else {
				v = do_xmem_byte(dmax, dmdx, dmtx & DMT_DAS);
			}
			DAE_DEBUG(
				if (reg == 0)
					printf("[no write to r0 done]\n");
				else
					printf("[r%d <- %08x]\n", reg, v);
			);
			if (reg != 0)
				eframe->tf_r[reg] = v;
		} else {
			if (x == 0) {
				if (reg != 0)
					eframe->tf_r[reg] = dmdx;
				m88100_rewind_insn(&(eframe->tf_regs));
				/* xmem RERUN ON rte */
				eframe->tf_dmt0 = 0;
				return;
			}
		}
	}
}

/*
 * Routines to patch the kernel code on 88100 systems not affected by
 * the xxx.usr bug.
 */

void
m88100_apply_patches()
{
#ifdef ERRATA__XXX_USR
	if (((get_cpu_pid() & PID_VN) >> VN_SHIFT) > 10) {
		/*
		 * Patch DAE helpers.
		 *	    before		    after
		 *	branch			branch
		 *	NOP			jmp.n r1
		 *	xxx.usr			xxx.usr
		 *	NOP; NOP; NOP
		 *	jmp r1
		 */
		((u_int32_t *)(do_load_word))[1] = 0xf400c401;
		((u_int32_t *)(do_load_half))[1] = 0xf400c401;
		((u_int32_t *)(do_load_byte))[1] = 0xf400c401;
		((u_int32_t *)(do_store_word))[1] = 0xf400c401;
		((u_int32_t *)(do_store_half))[1] = 0xf400c401;
		((u_int32_t *)(do_store_byte))[1] = 0xf400c401;
	}
#endif
}

#ifdef MULTIPROCESSOR
void
m88100_smp_setup(struct cpu_info *ci)
{
	/*
	 * Setup function pointers for mplock operation.
	 */

	ci->ci_mp_atomic_begin = m88100_mp_atomic_begin;
	ci->ci_mp_atomic_end = m88100_mp_atomic_end;
}

uint32_t
m88100_mp_atomic_begin(__cpu_simple_lock_t *lock, uint *csr)
{
	uint32_t psr;

	psr = get_psr();
	set_psr(psr | PSR_IND);
	__cpu_simple_lock(lock);

	return psr;
}

void
m88100_mp_atomic_end(uint32_t psr, __cpu_simple_lock_t *lock, uint csr)
{
	__cpu_simple_unlock(lock);
	set_psr(psr);
}
#endif

license: unknown


code:
/*	$OpenBSD: m88110_fp.c,v 1.10 2014/06/09 16:26:32 miod Exp $	*/

/*
 * Copyright (c) 2007, Miodrag Vallat.
 *
 * 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, this permission notice, and the disclaimer below
 * 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/types.h>
#include <sys/param.h>
#include <sys/proc.h>
#include <sys/signalvar.h>
#include <sys/systm.h>

#include <machine/fpu.h>
#include <machine/frame.h>
#include <machine/ieeefp.h>
#include <machine/trap.h>
#include <machine/m88110.h>

#include <lib/libkern/softfloat.h>

#include <m88k/m88k/fpu.h>

int	m88110_fpu_emulate(struct trapframe *, u_int32_t);
void	m88110_fpu_fetch(struct trapframe *, u_int, u_int, u_int, fparg *);

/*
 * All 88110 floating-point exceptions are handled there.
 *
 * We can unfortunately not trust the floating-point exception cause
 * register, as the 88110 will conveniently only set the ``unimplemented
 * instruction'' bit, more often than not.
 *
 * So we ignore it completely, and try to emulate the faulting instruction.
 * The instruction can be:
 *
 * - an invalid SFU1 opcode, in which case we'll send SIGILL to the process.
 *
 * - a genuinely unimplemented feature: fsqrt.
 *
 * - an opcode involving an odd-numbered register pair (as a double precision
 *   operand). Rather than issueing a correctly formed flavour in kernel mode,
 *   and having to handle a possible nested exception, we emulate it. This
 *   will of course be slower, but we have to draw the line somewhere.
 *   Gcc will however never produce such code, so we don't have to worry
 *   too much about this under OpenBSD.
 *
 * Note that, currently, opcodes involving the extended register file (XRF)
 * are handled as invalid opcodes. This will eventually change once the
 * toolchain can correctly assemble XRF instructions, and the XRF is saved
 * accross context switches (or not... lazy switching for XRF makes more
 * sense).
 */

void
m88110_fpu_exception(struct trapframe *frame)
{
	struct proc *p = curproc;
	int fault_type;
	vaddr_t fault_addr;
	union sigval sv;
	u_int32_t insn;
	int sig;

	fault_addr = frame->tf_exip & XIP_ADDR;

	/*
	 * Skip the instruction now. Signals will blame the correct
	 * address, and this has to be done before trapsignal() is
	 * invoked, or we won't run the first instruction of the signal
	 * handler...
	 */
	m88110_skip_insn(frame);

	/*
	 * The low-level exception code did not save the floating point
	 * exception registers. Do it now, and reset the exception
	 * cause register.
	 */
	__asm__ volatile ("fldcr %0, %%fcr0" : "=r"(frame->tf_fpecr));
	__asm__ volatile ("fldcr %0, %%fcr62" : "=r"(frame->tf_fpsr));
	__asm__ volatile ("fldcr %0, %%fcr63" : "=r"(frame->tf_fpcr));
	__asm__ volatile ("fstcr %r0, %fcr0");

	/*
	 * Fetch the faulting instruction. This should not fail, if it
	 * does, it's probably not your lucky day.
	 */
	if (copyin((void *)fault_addr, &insn, sizeof insn) != 0) {
		sig = SIGBUS;
		fault_type = BUS_OBJERR;
		goto deliver;
	}

	switch (insn >> 26) {
	case 0x20:
		/*
		 * f{ld,st,x}cr instruction. If it caused a fault in
		 * user mode, this is a privilege violation.
		 */
		sig = SIGILL;
		fault_type = ILL_PRVREG;
		goto deliver;
	case 0x21:
		/*
		 * ``real'' FPU instruction. We'll try to emulate it,
		 * unless FPU is disabled.
		 */
		if (frame->tf_epsr & PSR_SFD1) {	/* don't bother */
			sig = SIGFPE;
			fault_type = FPE_FLTINV;
			goto deliver;
		}
		sig = m88110_fpu_emulate(frame, insn);
		fault_type = SI_NOINFO;
		/*
		 * Update the floating point status register regardless of
		 * whether we'll deliver a signal or not.
		 */
		__asm__ volatile ("fstcr %0, %%fcr62" :: "r"(frame->tf_fpsr));
		break;
	default:
		/*
		 * Not a FPU instruction. Should not have raised this
		 * exception, so bail out.
		 */
		sig = SIGILL;
		fault_type = ILL_ILLOPC;
		goto deliver;
	}

	if (sig != 0) {
		if (sig == SIGILL)
			fault_type = ILL_ILLOPC;
		else {
			if (frame->tf_fpecr & FPECR_FIOV)
				fault_type = FPE_FLTSUB;
			else if (frame->tf_fpecr & FPECR_FROP)
				fault_type = FPE_FLTINV;
			else if (frame->tf_fpecr & FPECR_FDVZ)
				fault_type = FPE_INTDIV;
			else if (frame->tf_fpecr & FPECR_FUNF) {
				if (frame->tf_fpsr & FPSR_EFUNF)
					fault_type = FPE_FLTUND;
				else if (frame->tf_fpsr & FPSR_EFINX)
					fault_type = FPE_FLTRES;
			} else if (frame->tf_fpecr & FPECR_FOVF) {
				if (frame->tf_fpsr & FPSR_EFOVF)
					fault_type = FPE_FLTOVF;
				else if (frame->tf_fpsr & FPSR_EFINX)
					fault_type = FPE_FLTRES;
			} else if (frame->tf_fpecr & FPECR_FINX)
				fault_type = FPE_FLTRES;
		}

deliver:
		sv.sival_ptr = (void *)fault_addr;
		KERNEL_LOCK();
		trapsignal(p, sig, 0, fault_type, sv);
		KERNEL_UNLOCK();
	}
}

/*
 * Load a floating-point argument into a fparg union, then convert it to
 * the required format if it is of larger precision.
 *
 * This assumes the final format (width) is not FTYPE_INT, and the original
 * format (orig_width) <= width.
 */
void
m88110_fpu_fetch(struct trapframe *frame, u_int regno, u_int orig_width,
    u_int width, fparg *dest)
{
	u_int32_t tmp;

	switch (orig_width) {
	case FTYPE_INT:
		tmp = regno == 0 ? 0 : frame->tf_r[regno];
		switch (width) {
		case FTYPE_SNG:
			dest->sng = int32_to_float32(tmp);
			break;
		case FTYPE_DBL:
			dest->dbl = int32_to_float64(tmp);
			break;
		}
		break;
	case FTYPE_SNG:
		tmp = regno == 0 ? 0 : frame->tf_r[regno];
		switch (width) {
		case FTYPE_SNG:
			dest->sng = tmp;
			break;
		case FTYPE_DBL:
			dest->dbl = float32_to_float64(tmp);
			break;
		}
		break;
	case FTYPE_DBL:
		tmp = regno == 0 ? 0 : frame->tf_r[regno];
		dest->dbl = ((float64)tmp) << 32;
		tmp = regno == 31 ? 0 : frame->tf_r[regno + 1];
		dest->dbl |= (float64)tmp;
		break;
	}
}

/*
 * Emulate an FPU instruction.  On return, the trapframe registers
 * will be modified to reflect the settings the hardware would have left.
 */
int
m88110_fpu_emulate(struct trapframe *frame, u_int32_t insn)
{
	u_int rf, rd, rs1, rs2, t1, t2, td, tmax, opcode;
	u_int32_t old_fpsr, old_fpcr;
	int rc;

	fparg arg1, arg2, dest;

	/*
	 * Crack the instruction.
	 */
	rd = (insn >> 21) & 0x1f;
	rs1 = (insn >> 16) & 0x1f;
	rs2 = insn & 0x1f;
	rf = (insn >> 15) & 0x01;
	opcode = (insn >> 11) & 0x0f;
	t1 = (insn >> 9) & 0x03;
	t2 = (insn >> 7) & 0x03;
	td = (insn >> 5) & 0x03;

	/*
	 * Discard invalid opcodes, as well as instructions involving XRF,
	 * since we do not support them yet.
	 */
	if (rf != 0)
		return (SIGILL);

	switch (opcode) {
	case 0x00:	/* fmul */
	case 0x05:	/* fadd */
	case 0x06:	/* fsub */
	case 0x0e:	/* fdiv */
		if ((t1 != FTYPE_SNG && t1 != FTYPE_DBL) ||
		    (t2 != FTYPE_SNG && t2 != FTYPE_DBL) ||
		    (td != FTYPE_SNG && td != FTYPE_DBL))
			return (SIGILL);
		break;
	case 0x04:	/* flt */
		if (t1 != 0x00)	/* flt on XRF */
			return (SIGILL);
		if ((td != FTYPE_SNG && td != FTYPE_DBL) ||
		    t2 != 0x00 || rs1 != 0)
			return (SIGILL);
		break;
	case 0x07:	/* fcmp, fcmpu */
		if ((t1 != FTYPE_SNG && t1 != FTYPE_DBL) ||
		    (t2 != FTYPE_SNG && t2 != FTYPE_DBL))
			return (SIGILL);
		if (td != 0x00 /* fcmp */ && td != 0x01 /* fcmpu */)
			return (SIGILL);
		break;
	case 0x09:	/* int */
	case 0x0a:	/* nint */
	case 0x0b:	/* trnc */
		if ((t2 != FTYPE_SNG && t2 != FTYPE_DBL) ||
		    t1 != 0x00 || td != 0x00 || rs1 != 0)
			return (SIGILL);
		break;
	case 0x01:	/* fcvt */
		if (t2 == td)
			return (SIGILL);
		/* FALLTHROUGH */
	case 0x0f:	/* fsqrt */
		if ((t2 != FTYPE_SNG && t2 != FTYPE_DBL) ||
		    (td != FTYPE_SNG && td != FTYPE_DBL) ||
		    t1 != 0x00 || rs1 != 0)
			return (SIGILL);
		break;
	default:
	case 0x08:	/* mov */
		return (SIGILL);
	}

	/*
	 * Temporarily reset the status register, so that we can tell
	 * which exceptions are new after processing the opcode.
	 */
	old_fpsr = frame->tf_fpsr;
	frame->tf_fpsr = 0;

	/*
	 * Save fpcr as well, since we might need to change rounding mode
	 * temporarily.
	 */
	old_fpcr = frame->tf_fpcr;

	/*
	 * The logic for instruction emulation is:
	 *
	 * - the computation precision is the largest one of all the operands.
	 * - all source operands are converted to this precision if needed.
	 * - computation is performed.
	 * - the result is stored into the destination operand, converting it
	 *   to the destination precision if lower.
	 */

	switch (opcode) {
	case 0x00:	/* fmul */
		tmax = fpu_precision(t1, t2, td);
		m88110_fpu_fetch(frame, rs1, t1, tmax, &arg1);
		m88110_fpu_fetch(frame, rs2, t2, tmax, &arg2);
		switch (tmax) {
		case FTYPE_SNG:
			dest.sng = float32_mul(arg1.sng, arg2.sng);
			break;
		case FTYPE_DBL:
			dest.dbl = float64_mul(arg1.dbl, arg2.dbl);
			break;
		}
		fpu_store(frame, rd, tmax, td, &dest);
		break;

	case 0x01:	/* fcvt */
		tmax = fpu_precision(IGNORE_PRECISION, t2, td);
		m88110_fpu_fetch(frame, rs2, t2, tmax, &dest);
		fpu_store(frame, rd, tmax, td, &dest);
		break;

	case 0x04:	/* flt */
		m88110_fpu_fetch(frame, rs2, FTYPE_INT, td, &dest);
		fpu_store(frame, rd, td, td, &dest);
		break;

	case 0x05:	/* fadd */
		tmax = fpu_precision(t1, t2, td);
		m88110_fpu_fetch(frame, rs1, t1, tmax, &arg1);
		m88110_fpu_fetch(frame, rs2, t2, tmax, &arg2);
		switch (tmax) {
		case FTYPE_SNG:
			dest.sng = float32_add(arg1.sng, arg2.sng);
			break;
		case FTYPE_DBL:
			dest.dbl = float64_add(arg1.dbl, arg2.dbl);
			break;
		}
		fpu_store(frame, rd, tmax, td, &dest);
		break;

	case 0x06:	/* fsub */
		tmax = fpu_precision(t1, t2, td);
		m88110_fpu_fetch(frame, rs1, t1, tmax, &arg1);
		m88110_fpu_fetch(frame, rs2, t2, tmax, &arg2);
		switch (tmax) {
		case FTYPE_SNG:
			dest.sng = float32_sub(arg1.sng, arg2.sng);
			break;
		case FTYPE_DBL:
			dest.dbl = float64_sub(arg1.dbl, arg2.dbl);
			break;
		}
		fpu_store(frame, rd, tmax, td, &dest);
		break;

	case 0x07:	/* fcmp, fcmpu */
		tmax = fpu_precision(t1, t2, IGNORE_PRECISION);
		m88110_fpu_fetch(frame, rs1, t1, tmax, &arg1);
		m88110_fpu_fetch(frame, rs2, t2, tmax, &arg2);
		fpu_compare(frame, &arg1, &arg2, tmax, rd, td /* fcmpu */);
		break;

	case 0x09:	/* int */
do_int:
		m88110_fpu_fetch(frame, rs2, t2, t2, &dest);
		fpu_store(frame, rd, t2, FTYPE_INT, &dest);
		break;

	case 0x0a:	/* nint */
		/* round to nearest */
		frame->tf_fpcr = (old_fpcr & ~(FPCR_RD_MASK << FPCR_RD_SHIFT)) |
		    (FP_RN << FPCR_RD_SHIFT);
		goto do_int;

	case 0x0b:	/* trnc */
		/* round towards zero */
		frame->tf_fpcr = (old_fpcr & ~(FPCR_RD_MASK << FPCR_RD_SHIFT)) |
		    (FP_RZ << FPCR_RD_SHIFT);
		goto do_int;

	case 0x0e:	/* fdiv */
		tmax = fpu_precision(t1, t2, td);
		m88110_fpu_fetch(frame, rs1, t1, tmax, &arg1);
		m88110_fpu_fetch(frame, rs2, t2, tmax, &arg2);
		switch (tmax) {
		case FTYPE_SNG:
			dest.sng = float32_div(arg1.sng, arg2.sng);
			break;
		case FTYPE_DBL:
			dest.dbl = float64_div(arg1.dbl, arg2.dbl);
			break;
		}
		fpu_store(frame, rd, tmax, td, &dest);
		break;

	case 0x0f:	/* sqrt */
		tmax = fpu_precision(IGNORE_PRECISION, t2, td);
		m88110_fpu_fetch(frame, rs2, t2, tmax, &arg1);
		switch (tmax) {
		case FTYPE_SNG:
			dest.sng = float32_sqrt(arg1.sng);
			break;
		case FTYPE_DBL:
			dest.dbl = float64_sqrt(arg1.dbl);
			break;
		}
		fpu_store(frame, rd, tmax, td, &dest);
		break;
	}

	/*
	 * Mark new exceptions, if any, in the fpsr, and decide whether
	 * to send a signal or not.
	 */

	if (frame->tf_fpsr & old_fpcr)
		rc = SIGFPE;
	else
		rc = 0;
	frame->tf_fpsr |= old_fpsr;

	/*
	 * Restore fpcr as well.
	 */
	frame->tf_fpcr = old_fpcr;

	return (rc);
}


code:
#	$OpenBSD: m88110_mmu.S,v 1.7 2013/01/05 11:20:56 miod Exp $
/*
 * Copyright (c) 2000 Steve Murphree, Jr.
 * 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 Steve Murphree, Jr.
 * 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 <machine/asm.h>

/*
 * 88110 errata #20 (4.2) or #4 (5.1.1):
 * ``If a ldcr, stcr, fldcr, or fstcr instruction is located in the last
 *   word of an instruction page, and the fetch of the next page results
 *   in an mmu exception (invalid descriptor, tablewalk bus error,
 *   software atc miss), the exip and enip value may be incorrectly
 *   incremented to the second word of the page.
 *   Suggested fix: do not allow these instructions to occupy the last
 *   word of a page if this can occur.''
 *
 * This is why the following function are not inlines in asm.h, and are
 * aligned to 32 byte boundaries.
 *
 * Note that fldcr/fstcr are used in several places in userland (libc,
 * libpthread, afs lwp) which will need alignment hints as well to
 * avoid this. Sigh -- miod
 */

	.text

/* set routines */
	.align 5
ENTRY(set_icmd)
	FLUSH_PIPELINE
	stcr	%r2, ICMD
	jmp	%r1

	.align 5
ENTRY(set_ictl)
	FLUSH_PIPELINE
	stcr	%r2, ICTL
	NOP
	FLUSH_PIPELINE
	jmp	%r1

	.align 5
ENTRY(set_isar)
	stcr	%r2, ISAR
	jmp	%r1

	.align 5
ENTRY(set_isap)
	FLUSH_PIPELINE
	NOP
	stcr	%r2, ISAP
	jmp	%r1

	.align 5
ENTRY(set_iuap)
	FLUSH_PIPELINE
	NOP
 	stcr	%r2, IUAP
	jmp	%r1

	.align 5
ENTRY(set_iir)
	stcr	%r2, IIR
	jmp	%r1

	.align 5
ENTRY(set_ibp)
	stcr	%r2, IBP
	jmp	%r1

	.align 5
ENTRY(set_ippu)
	stcr	%r2, IPPU
	jmp	%r1

	.align 5
ENTRY(set_ippl)
	stcr	%r2, IPPL
	jmp	%r1

	.align 5
ENTRY(set_isr)
	stcr	%r2, ISR
	jmp	%r1

	.align 5
ENTRY(set_dcmd)
	FLUSH_PIPELINE
	stcr	%r2, DCMD
	jmp	%r1

	.align 5
ENTRY(set_dctl)
	FLUSH_PIPELINE
	stcr	%r2, DCTL
	NOP
	FLUSH_PIPELINE
	jmp	%r1

	.align 5
ENTRY(set_dsar)
	stcr	%r2, DSAR
	NOP
	jmp	%r1

	.align 5
ENTRY(set_dsap)
	FLUSH_PIPELINE
	NOP
	stcr	%r2, DSAP
	FLUSH_PIPELINE
	NOP
	jmp	%r1

	.align 5
ENTRY(set_duap)
	FLUSH_PIPELINE
	NOP
	stcr	%r2, DUAP
	FLUSH_PIPELINE
	NOP
	jmp	%r1

	.align 5
ENTRY(set_dir)
	stcr	%r2, DIR
	jmp	%r1

	.align 5
ENTRY(set_dbp)
	stcr	%r2, DBP
	jmp	%r1

	.align 5
ENTRY(set_dppu)
	stcr	%r2, DPPU
	jmp	%r1

	.align 5
ENTRY(set_dppl)
	stcr	%r2, DPPL
	jmp	%r1

	.align 5
ENTRY(set_dsr)
	stcr	%r2, DSR
	jmp	%r1

/* get routines */
	.align 5
ENTRY(get_ictl)
	ldcr	%r2, ICTL
	jmp	%r1

	.align 5
ENTRY(get_dctl)
	ldcr	%r2, DCTL
	jmp	%r1

	.align 5
ENTRY(get_dsr)
	ldcr	%r2, DSR
	jmp	%r1

	.align 5
ENTRY(get_isr)
	ldcr	%r2, ISR
	jmp	%r1


code:
/*	$OpenBSD: m8820x_machdep.c,v 1.61 2013/11/16 18:45:20 miod Exp $	*/
/*
 * Copyright (c) 2004, 2007, 2010, 2011, 2013, Miodrag Vallat.
 *
 * 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.
 */
/*
 * Copyright (c) 2001 Steve Murphree, Jr.
 * Copyright (c) 1996 Nivas Madhur
 * 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 Nivas Madhur.
 * 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.
 *
 */
/*
 * Mach Operating System
 * Copyright (c) 1993-1991 Carnegie Mellon University
 * Copyright (c) 1991 OMRON Corporation
 * All Rights Reserved.
 *
 * Permission to use, copy, modify and distribute this software and its
 * documentation is hereby granted, provided that both the copyright
 * notice and this permission notice appear in all copies of the
 * software, derivative works or modified versions, and any portions
 * thereof, and that both notices appear in supporting documentation.
 *
 * CARNEGIE MELLON AND OMRON ALLOW FREE USE OF THIS SOFTWARE IN ITS "AS IS"
 * CONDITION.  CARNEGIE MELLON AND OMRON DISCLAIM ANY LIABILITY OF ANY KIND
 * FOR ANY DAMAGES WHATSOEVER RESULTING FROM THE USE OF THIS SOFTWARE.
 *
 * Carnegie Mellon requests users of this software to return to
 *
 *  Software Distribution Coordinator  or  Software.Distribution@CS.CMU.EDU
 *  School of Computer Science
 *  Carnegie Mellon University
 *  Pittsburgh PA 15213-3890
 *
 * any improvements or extensions that they make and grant Carnegie the
 * rights to redistribute these changes.
 */

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

#include <uvm/uvm_extern.h>

#include <machine/asm_macro.h>
#include <machine/cmmu.h>
#include <machine/cpu.h>
#include <machine/lock.h>
#include <machine/m8820x.h>
#include <machine/psl.h>

extern	void m8820x_zeropage(vaddr_t);
extern	void m8820x_copypage(vaddr_t, vaddr_t);

cpuid_t	m8820x_init(void);
void	m8820x_batc_setup(cpuid_t, apr_t);
void	m8820x_cpu_configuration_print(int);
void	m8820x_shutdown(void);
apr_t	m8820x_apr_cmode(void);
apr_t	m8820x_pte_cmode(void);
void	m8820x_set_sapr(apr_t);
void	m8820x_set_uapr(apr_t);
void	m8820x_tlbis(cpuid_t, vaddr_t, pt_entry_t);
void	m8820x_tlbiu(cpuid_t, vaddr_t, pt_entry_t);
void	m8820x_tlbia(cpuid_t);
void	m8820x_cache_wbinv(cpuid_t, paddr_t, psize_t);
void	m8820x_dcache_wb(cpuid_t, paddr_t, psize_t);
void	m8820x_icache_inv(cpuid_t, paddr_t, psize_t);
void	m8820x_dma_cachectl(paddr_t, psize_t, int);
void	m8820x_dma_cachectl_local(paddr_t, psize_t, int);
void	m8820x_initialize_cpu(cpuid_t);

const struct cmmu_p cmmu8820x = {
	m8820x_init,
	m8820x_batc_setup,
	m8820x_setup_board_config,
	m8820x_cpu_configuration_print,
	m8820x_shutdown,
	m8820x_cpu_number,
	m8820x_apr_cmode,
	m8820x_pte_cmode,
	m8820x_set_sapr,
	m8820x_set_uapr,
	m8820x_tlbis,
	m8820x_tlbiu,
	m8820x_tlbia,
	m8820x_cache_wbinv,
	m8820x_dcache_wb,
	m8820x_icache_inv,
	m8820x_dma_cachectl,
#ifdef MULTIPROCESSOR
	m8820x_dma_cachectl_local,
	m8820x_initialize_cpu,
#endif
};

/*
 * Systems with more than 2 CMMUs per CPU use split schemes, which sometimes
 * are programmable (well, no more than having a few hardwired choices).
 *
 * The following schemes are available on MVME188 boards:
 * - split on A12 address bit (A14 for 88204)
 * - split on supervisor/user access
 * - split on SRAM/non-SRAM addresses, with either supervisor-only or all
 *   access to SRAM.
 *
 * MVME188 configuration 6, with 4 CMMUs par CPU, also forces a split on
 * A14 address bit (A16 for 88204).
 *
 * Under OpenBSD, we will only split on A12 and A14 address bits, since we
 * do not want to waste CMMU resources on the SRAM, and user/supervisor
 * splits seem less efficient.
 *
 * The really nasty part of this choice is in the exception handling code,
 * when it needs to get error information from up to 4 CMMUs. See eh.S for
 * the gory details.
 */

struct m8820x_cmmu m8820x_cmmu[MAX_CMMUS]
    __attribute__ ((__section__(".rodata")));
u_int max_cmmus
    __attribute__ ((__section__(".rodata")));
u_int cmmu_shift
    __attribute__ ((__section__(".rodata")));

/* local prototypes */
void	m8820x_cmmu_configuration_print(int, int);
void	m8820x_cmmu_set_reg(int, u_int, int);
void	m8820x_cmmu_set_reg_if_mode(int, u_int, int, int);
void	m8820x_cmmu_set_cmd(u_int, int, vaddr_t);
void	m8820x_cmmu_set_cmd_if_addr(u_int, int, vaddr_t);
void	m8820x_cmmu_set_cmd_if_mode(u_int, int, vaddr_t, int);
void	m8820x_cmmu_wait(int);
void	m8820x_cmmu_wb_locked(int, paddr_t, psize_t);
void	m8820x_cmmu_wbinv_locked(int, paddr_t, psize_t);
void	m8820x_cmmu_inv_locked(int, paddr_t, psize_t);
#if defined(__luna88k__) && !defined(MULTIPROCESSOR)
void	m8820x_enable_other_cmmu_cache(void);
#endif

static inline
void	m8820x_dbatc_set(cpuid_t, uint, batc_t);
static inline
void	m8820x_ibatc_set(cpuid_t, uint, batc_t);

/* Flags passed to m8820x_cmmu_set_*() */
#define MODE_VAL		0x01
#define ADDR_VAL		0x02

/*
 * Helper functions to poke values into the appropriate CMMU registers.
 */

void
m8820x_cmmu_set_reg(int reg, u_int val, int cpu)
{
	struct m8820x_cmmu *cmmu;
	int mmu, cnt;

	mmu = cpu << cmmu_shift;
	cmmu = m8820x_cmmu + mmu;

	/*
	 * We scan all CMMUs to find the matching ones and store the
	 * values there.
	 */
	for (cnt = 1 << cmmu_shift; cnt != 0; cnt--, mmu++, cmmu++) {
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
		if (cmmu->cmmu_regs == NULL)
			continue;
#endif
		cmmu->cmmu_regs[reg] = val;
	}
}

void
m8820x_cmmu_set_reg_if_mode(int reg, u_int val, int cpu, int mode)
{
	struct m8820x_cmmu *cmmu;
	int mmu, cnt;

	mmu = cpu << cmmu_shift;
	cmmu = m8820x_cmmu + mmu;

	/*
	 * We scan all CMMUs to find the matching ones and store the
	 * values there.
	 */
	for (cnt = 1 << cmmu_shift; cnt != 0; cnt--, mmu++, cmmu++) {
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
		if (cmmu->cmmu_regs == NULL)
			continue;
#endif
		if (CMMU_MODE(mmu) != mode)
			continue;
		cmmu->cmmu_regs[reg] = val;
	}
}

void
m8820x_cmmu_set_cmd(u_int cmd, int cpu, vaddr_t addr)
{
	struct m8820x_cmmu *cmmu;
	int mmu, cnt;

	mmu = cpu << cmmu_shift;
	cmmu = m8820x_cmmu + mmu;

	/*
	 * We scan all CMMUs to find the matching ones and store the
	 * values there.
	 */
	for (cnt = 1 << cmmu_shift; cnt != 0; cnt--, mmu++, cmmu++) {
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
		if (cmmu->cmmu_regs == NULL)
			continue;
#endif
		cmmu->cmmu_regs[CMMU_SAR] = addr;
		cmmu->cmmu_regs[CMMU_SCR] = cmd;
	}
}

void
m8820x_cmmu_set_cmd_if_mode(u_int cmd, int cpu, vaddr_t addr, int mode)
{
	struct m8820x_cmmu *cmmu;
	int mmu, cnt;

	mmu = cpu << cmmu_shift;
	cmmu = m8820x_cmmu + mmu;

	/*
	 * We scan all CMMUs to find the matching ones and store the
	 * values there.
	 */
	for (cnt = 1 << cmmu_shift; cnt != 0; cnt--, mmu++, cmmu++) {
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
		if (cmmu->cmmu_regs == NULL)
			continue;
#endif
		if (CMMU_MODE(mmu) != mode)
			continue;
		cmmu->cmmu_regs[CMMU_SAR] = addr;
		cmmu->cmmu_regs[CMMU_SCR] = cmd;
	}
}

#ifdef M88200_HAS_SPLIT_ADDRESS
void
m8820x_cmmu_set_cmd_if_addr(u_int cmd, int cpu, vaddr_t addr)
{
	struct m8820x_cmmu *cmmu;
	int mmu, cnt;

	mmu = cpu << cmmu_shift;
	cmmu = m8820x_cmmu + mmu;

	/*
	 * We scan all CMMUs to find the matching ones and store the
	 * values there.
	 */
	for (cnt = 1 << cmmu_shift; cnt != 0; cnt--, mmu++, cmmu++) {
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
		if (cmmu->cmmu_regs == NULL)
			continue;
#endif
		if (cmmu->cmmu_addr_mask != 0) {
			if ((addr & cmmu->cmmu_addr_mask) != cmmu->cmmu_addr)
				continue;
		}
		cmmu->cmmu_regs[CMMU_SAR] = addr;
		cmmu->cmmu_regs[CMMU_SCR] = cmd;
	}
}
#else
#define	m8820x_cmmu_set_cmd_if_addr	m8820x_cmmu_set_cmd
#endif

/*
 * Force a read from the CMMU status register, thereby forcing execution to
 * stop until all pending CMMU operations are finished.
 * This is used by the various cache invalidation functions.
 */
void
m8820x_cmmu_wait(int cpu)
{
	struct m8820x_cmmu *cmmu;
	int mmu, cnt;

	mmu = cpu << cmmu_shift;
	cmmu = m8820x_cmmu + mmu;

	/*
	 * We scan all related CMMUs and read their status register.
	 */
	for (cnt = 1 << cmmu_shift; cnt != 0; cnt--, mmu++, cmmu++) {
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
		if (cmmu->cmmu_regs == NULL)
			continue;
#endif
#ifdef DEBUG
		if (cmmu->cmmu_regs[CMMU_SSR] & CMMU_SSR_BE) {
			panic("cache flush failed!");
		}
#else
		(void)cmmu->cmmu_regs[CMMU_SSR];
#endif
	}
}

/*
 * BATC routines
 */

static inline
void
m8820x_dbatc_set(cpuid_t cpu, uint batcno, batc_t batc)
{
	m8820x_cmmu_set_reg_if_mode(CMMU_BWP(batcno), batc, cpu, DATA_CMMU);
}

static inline
void
m8820x_ibatc_set(cpuid_t cpu, uint batcno, batc_t batc)
{
	m8820x_cmmu_set_reg_if_mode(CMMU_BWP(batcno), batc, cpu, INST_CMMU);
}

void
m8820x_batc_setup(cpuid_t cpu, apr_t cmode)
{
	paddr_t s_text, e_text, s_data, e_data,	e_rodata;
	uint batcno;
	batc_t batc, proto;
	extern caddr_t kernelstart;
	extern caddr_t etext;
	extern caddr_t erodata;
	extern caddr_t end;

	proto = BATC_SO | BATC_V;
	if (cmode & CACHE_WT)
		proto |= BATC_WT;
	if (cmode & CACHE_INH)
		proto |= BATC_INH;

	s_text = round_batc((paddr_t)&kernelstart);
	s_data = e_text = round_batc((paddr_t)&etext);
	e_rodata = round_batc((paddr_t)&erodata);
#if 0 /* not until pmap makes sure kvm starts on a BATC boundary */
	e_data = round_batc((paddr_t)&end);
#else
	e_data = trunc_batc((paddr_t)&end);
#endif

	/* map s_text..e_text with IBATC */
	batcno = 0;
	while (s_text != e_text) {
		batc = (s_text >> BATC_BLKSHIFT) << BATC_VSHIFT;
		batc |= (s_text >> BATC_BLKSHIFT) << BATC_PSHIFT;
		batc |= proto;
#ifdef DEBUG
		printf("cpu%d ibat%d %p(%08x)\n", cpu, batcno, s_text, batc);
#endif
		global_ibatc[batcno] = batc;
		s_text += BATC_BLKBYTES;
		if (++batcno == BATC_MAX)
			break;
	}

	/* map e_text..e_data with DBATC */
	if (cmode & CACHE_GLOBAL)
		proto |= BATC_GLOBAL;
	batcno = 0;
	while (s_data != e_data) {
		batc = (s_data >> BATC_BLKSHIFT) << BATC_VSHIFT;
		batc |= (s_data >> BATC_BLKSHIFT) << BATC_PSHIFT;
		batc |= proto;
		if (s_data < e_rodata)
			batc |= BATC_PROT;
#if defined(MULTIPROCESSOR)	/* XXX */
		else
			break;
#endif
#ifdef DEBUG
		printf("cpu%d dbat%d %p(%08x)\n", cpu, batcno, s_data, batc);
#endif
		global_dbatc[batcno] = batc;
		s_data += BATC_BLKBYTES;
		if (++batcno == BATC_MAX)
			break;
	}

	for (batcno = 0; batcno < BATC_MAX; batcno++) {
		m8820x_dbatc_set(cpu, batcno, global_dbatc[batcno]);
		m8820x_ibatc_set(cpu, batcno, global_ibatc[batcno]);
	}
}

/*
 * Should only be called after the calling cpus knows its cpu
 * number and main/secondary status. Should be called first
 * by the main processor, before the others are started.
*/
void
m8820x_cpu_configuration_print(int main)
{
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
	struct m8820x_cmmu *cmmu;
#endif
	int pid = get_cpu_pid();
	int proctype = (pid & PID_ARN) >> ARN_SHIFT;
	int procvers = (pid & PID_VN) >> VN_SHIFT;
	int kind, nmmu, mmuno, cnt, cpu = cpu_number();

	printf("cpu%d: ", cpu);
	switch (proctype) {
	default:
		printf("unknown model arch 0x%x rev 0x%x\n",
		    proctype, procvers);
		break;
	case ARN_88100:
		printf("M88100 rev 0x%x", procvers);
#ifdef MULTIPROCESSOR
		if (main == 0)
			printf(", secondary");
#endif
		nmmu = 1 << cmmu_shift;
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
		cmmu = m8820x_cmmu + (cpu << cmmu_shift);
		for (cnt = 1 << cmmu_shift; cnt != 0; cnt--, cmmu++)
			if (cmmu->cmmu_regs == NULL)
				nmmu--;
#endif
		printf(", %d CMMU\n", nmmu);

		for (kind = INST_CMMU; kind <= DATA_CMMU; kind++) {
			mmuno = (cpu << cmmu_shift) + kind;
			for (cnt = 1 << cmmu_shift; cnt != 0;
			    cnt -= 2, mmuno += 2)
				m8820x_cmmu_configuration_print(cpu, mmuno);
		}
		break;
	}

#ifndef ERRATA__XXX_USR
	{
		static int errata_warn = 0;

		if (proctype == ARN_88100 && procvers <= 10) {
			if (!errata_warn++)
				printf("WARNING: M88100 bug workaround code "
				    "not enabled.\nPlease recompile the kernel "
				    "with option ERRATA__XXX_USR !\n");
		}
	}
#endif
}

void
m8820x_cmmu_configuration_print(int cpu, int mmuno)
{
	struct m8820x_cmmu *cmmu;
	int mmuid, cssp;
	u_int line;
	uint32_t linestatus;
#ifdef M88200_HAS_SPLIT_ADDRESS
	int aline, abit, amask;
#endif

	cmmu = m8820x_cmmu + mmuno;
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
	if (cmmu->cmmu_regs == NULL)
		return;
#endif

	mmuid = CMMU_TYPE(cmmu->cmmu_idr);

	printf("cpu%d: ", cpu);
	switch (mmuid) {
	case M88200_ID:
		printf("M88200 (16K)");
		break;
	case M88204_ID:
		printf("M88204 (64K)");
		break;
	default:
		printf("unknown CMMU id 0x%x", mmuid);
		break;
	}
	printf(" rev 0x%x,", CMMU_VERSION(cmmu->cmmu_idr));
#ifdef M88200_HAS_SPLIT_ADDRESS
	/*
	 * Print address lines
	 */
	amask = cmmu->cmmu_addr_mask;
	if (amask != 0) {
		aline = 0;
		while (amask != 0) {
			abit = ff1(amask);
			if ((cmmu->cmmu_addr & (1 << abit)) != 0)
				printf("%cA%02d",
				    aline != 0 ? '/' : ' ', abit);
			else
				printf("%cA%02d*",
				    aline != 0 ? '/' : ' ', abit);
			amask ^= 1 << abit;
		}
	} else if (cmmu_shift != 1) {
		/* unknown split scheme */
		printf(" split");
	} else
#endif
		printf(" full");
	printf(" %ccache\n", CMMU_MODE(mmuno) == INST_CMMU ? 'I' : 'D');

	/*
	 * Report disabled cache lines.
	 */
	for (cssp = mmuid == M88204_ID ? 3 : 0; cssp >= 0; cssp--)
		for (line = 0; line <= 255; line++) {
			cmmu->cmmu_regs[CMMU_SAR] =
			    line << MC88200_CACHE_SHIFT;
			linestatus = cmmu->cmmu_regs[CMMU_CSSP(cssp)];
			if (linestatus & (CMMU_CSSP_D3 | CMMU_CSSP_D2 |
			     CMMU_CSSP_D1 | CMMU_CSSP_D0)) {
				printf("cpu%d: cache line 0x%03x disabled\n",
				    cpu, (cssp << 8) | line);
				}
			}
}

/*
 * CMMU initialization routine
 */
cpuid_t
m8820x_init()
{
	cpuid_t cpu;

	cpu = m8820x_cpu_number();
	m8820x_initialize_cpu(cpu);
#if defined(__luna88k__) && !defined(MULTIPROCESSOR)
	m8820x_enable_other_cmmu_cache();
#endif
	return (cpu);
}

/*
 * Initialize the set of CMMUs tied to a particular CPU.
 */
void
m8820x_initialize_cpu(cpuid_t cpu)
{
	struct cpu_info *ci;
	struct m8820x_cmmu *cmmu;
	int mmuid, cssp;
	u_int line, cnt;
	uint32_t linestatus;
	apr_t apr;

	apr = ((0x00000 << PG_BITS) | CACHE_GLOBAL | CACHE_INH) & ~APR_V;

	cmmu = m8820x_cmmu + (cpu << cmmu_shift);

	/*
	 * Setup CMMU pointers for faster exception processing.
	 * This relies on the board-dependent code putting instruction
	 * CMMUs and data CMMUs interleaved with instruction CMMUs first.
	 */
	ci = &m88k_cpus[cpu];
	switch (cmmu_shift) {
	default:
		/* exception code may not use ci_pfsr fields, compute anyway */
		/* FALLTHROUGH */
	case 2:
		ci->ci_pfsr_d1 = (u_int)cmmu[3].cmmu_regs + CMMU_PFSR * 4;
		ci->ci_pfsr_i1 = (u_int)cmmu[2].cmmu_regs + CMMU_PFSR * 4;
		/* FALLTHROUGH */
	case 1:
		ci->ci_pfsr_d0 = (u_int)cmmu[1].cmmu_regs + CMMU_PFSR * 4;
		ci->ci_pfsr_i0 = (u_int)cmmu[0].cmmu_regs + CMMU_PFSR * 4;
		break;
	}

	for (cnt = 1 << cmmu_shift; cnt != 0; cnt--, cmmu++) {
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
		if (cmmu->cmmu_regs == NULL)
			continue;
#endif
		cmmu->cmmu_idr = cmmu->cmmu_regs[CMMU_IDR];
		mmuid = CMMU_TYPE(cmmu->cmmu_idr);

		/*
		 * Reset cache, but keep disabled lines disabled.
		 *
		 * Note that early Luna88k PROM apparently forget to
		 * initialize the last line (#255) correctly, and the
		 * CMMU initializes with whatever its state upon powerup
		 * happens to be.
		 *
		 * It is then unlikely that these particular cache lines
		 * have been exercized by the self-tests; better disable
		 * the whole line.
		 */
		for (cssp = mmuid == M88204_ID ? 3 : 0; cssp >= 0; cssp--)
			for (line = 0; line <= 255; line++) {
				cmmu->cmmu_regs[CMMU_SAR] =
				    line << MC88200_CACHE_SHIFT;
				linestatus = cmmu->cmmu_regs[CMMU_CSSP(cssp)];
				if (linestatus & (CMMU_CSSP_D3 | CMMU_CSSP_D2 |
				     CMMU_CSSP_D1 | CMMU_CSSP_D0))
					linestatus =
					    CMMU_CSSP_D3 | CMMU_CSSP_D2 |
					    CMMU_CSSP_D1 | CMMU_CSSP_D0;
				else
					linestatus = 0;
				cmmu->cmmu_regs[CMMU_CSSP(cssp)] = linestatus |
				    CMMU_CSSP_L5 | CMMU_CSSP_L4 |
				    CMMU_CSSP_L3 | CMMU_CSSP_L2 |
				    CMMU_CSSP_L1 | CMMU_CSSP_L0 |
				    CMMU_CSSP_VV(3, CMMU_VV_INVALID) |
				    CMMU_CSSP_VV(2, CMMU_VV_INVALID) |
				    CMMU_CSSP_VV(1, CMMU_VV_INVALID) |
				    CMMU_CSSP_VV(0, CMMU_VV_INVALID);
			}

		/*
		 * Set the SCTR, SAPR, and UAPR to some known state.
		 * Snooping is always enabled, so that we do not need to
		 * writeback userland code pages when they first get filled
		 * as data pages.
		 */
		cmmu->cmmu_regs[CMMU_SCTR] = CMMU_SCTR_SE;

		cmmu->cmmu_regs[CMMU_SAPR] = cmmu->cmmu_regs[CMMU_UAPR] = apr;

		cmmu->cmmu_regs[CMMU_BWP0] = cmmu->cmmu_regs[CMMU_BWP1] =
		cmmu->cmmu_regs[CMMU_BWP2] = cmmu->cmmu_regs[CMMU_BWP3] =
		cmmu->cmmu_regs[CMMU_BWP4] = cmmu->cmmu_regs[CMMU_BWP5] =
		cmmu->cmmu_regs[CMMU_BWP6] = cmmu->cmmu_regs[CMMU_BWP7] = 0;
		cmmu->cmmu_regs[CMMU_SCR] = CMMU_FLUSH_CACHE_INV_ALL;
		(void)cmmu->cmmu_regs[CMMU_SSR];
		cmmu->cmmu_regs[CMMU_SCR] = CMMU_FLUSH_SUPER_ALL;
		cmmu->cmmu_regs[CMMU_SCR] = CMMU_FLUSH_USER_ALL;
	}

	/*
	 * Enable instruction cache.
	 */
	apr &= ~CACHE_INH;
	m8820x_cmmu_set_reg_if_mode(CMMU_SAPR, apr, cpu, INST_CMMU);

	/*
	 * Data cache will be enabled at pmap_bootstrap_cpu() time,
	 * because the PROM won't likely expect its work area in memory
	 * to be cached. On at least aviion, starting secondary processors
	 * returns an error code although the processor has correctly spun
	 * up, if the PROM work area is cached.
	 */
#ifdef dont_do_this_at_home
	apr |= CACHE_WT;
	m8820x_cmmu_set_reg_if_mode(CMMU_SAPR, apr, cpu, DATA_CMMU);
#endif

	ci->ci_zeropage = m8820x_zeropage;
	ci->ci_copypage = m8820x_copypage;
}

/*
 * Just before poweroff or reset....
 */
void
m8820x_shutdown()
{
	u_int cmmu_num;
	struct m8820x_cmmu *cmmu;

	CMMU_LOCK;

	cmmu = m8820x_cmmu;
	for (cmmu_num = 0; cmmu_num < max_cmmus; cmmu_num++, cmmu++) {
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
		if (cmmu->cmmu_regs == NULL)
			continue;
#endif
		cmmu->cmmu_regs[CMMU_SAPR] = cmmu->cmmu_regs[CMMU_UAPR] =
		    ((0x00000 << PG_BITS) | CACHE_INH) &
		    ~(CACHE_WT | CACHE_GLOBAL | APR_V);
	}

	CMMU_UNLOCK;
}

/*
 * Older systems do not xmem correctly on writeback cache lines, causing
 * the remainder of the cache line to be corrupted.
 * This behaviour has been observed on a system with 88100 rev 8 and
 * 88200 rev 5; it is unknown whether the culprit is the 88100 or the 88200;
 * however we can rely upon 88100 rev 10 onwards and 88200 rev 7 onwards
 * (as well as all 88204 revs) to be safe.
 */
apr_t
m8820x_apr_cmode()
{
	u_int cmmu_num;
	struct m8820x_cmmu *cmmu;

	cmmu = m8820x_cmmu;
	for (cmmu_num = max_cmmus; cmmu_num != 0; cmmu_num--, cmmu++) {
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
		if (cmmu->cmmu_regs == NULL)
			continue;
#endif
		/*
		 * XXX 88200 v6 could not be tested. Do 88200 ever have
		 * XXX even version numbers anyway?
		 */
		if (CMMU_TYPE(cmmu->cmmu_idr) == M88200_ID &&
		    CMMU_VERSION(cmmu->cmmu_idr) <= 6)
			return CACHE_WT;
	}
	/*
	 * XXX 88100 v9 could not be tested. Might be unaffected, but
	 * XXX better be safe than sorry.
	 */
	if (((get_cpu_pid() & PID_VN) >> VN_SHIFT) <= 9)
		return CACHE_WT;

	return CACHE_DFL;
}

/*
 * Older systems require page tables to be cache inhibited (write-through
 * won't even cut it).
 * We can rely upon 88200 rev 9 onwards to be safe (as well as all 88204
 * revs).
 */
apr_t
m8820x_pte_cmode()
{
	u_int cmmu_num;
	struct m8820x_cmmu *cmmu;

	cmmu = m8820x_cmmu;
	for (cmmu_num = max_cmmus; cmmu_num != 0; cmmu_num--, cmmu++) {
#ifdef M88200_HAS_ASYMMETRICAL_ASSOCIATION
		if (cmmu->cmmu_regs == NULL)
			continue;
#endif
		/*
		 * XXX 88200 v8 could not be tested. Do 88200 ever have
		 * XXX even version numbers anyway?
		 */
		if (CMMU_TYPE(cmmu->cmmu_idr) == M88200_ID &&
		    CMMU_VERSION(cmmu->cmmu_idr) <= 8)
			return CACHE_INH;
	}

	return CACHE_WT;
}

void
m8820x_set_sapr(apr_t ap)
{
	int cpu = cpu_number();

	CMMU_LOCK;

	m8820x_cmmu_set_reg(CMMU_SAPR, ap, cpu);

	CMMU_UNLOCK;
}

void
m8820x_set_uapr(apr_t ap)
{
	u_int32_t psr;
	int cpu = cpu_number();

	psr = get_psr();
	set_psr(psr | PSR_IND);
	CMMU_LOCK;

	m8820x_cmmu_set_reg(CMMU_UAPR, ap, cpu);

	CMMU_UNLOCK;
	set_psr(psr);
}

/*
 * Functions that invalidate TLB entries.
 */

void
m8820x_tlbis(cpuid_t cpu, vaddr_t va, pt_entry_t pte)
{
	u_int32_t psr;

	psr = get_psr();
	set_psr(psr | PSR_IND);
	CMMU_LOCK;
	m8820x_cmmu_set_cmd_if_addr(CMMU_FLUSH_SUPER_PAGE, cpu, va);
	CMMU_UNLOCK;
	set_psr(psr);
}

void
m8820x_tlbiu(cpuid_t cpu, vaddr_t va, pt_entry_t pte)
{
	u_int32_t psr;

	psr = get_psr();
	set_psr(psr | PSR_IND);
	CMMU_LOCK;
	m8820x_cmmu_set_cmd_if_addr(CMMU_FLUSH_USER_PAGE, cpu, va);
	CMMU_UNLOCK;
	set_psr(psr);
}

void
m8820x_tlbia(cpuid_t cpu)
{
	u_int32_t psr;

	psr = get_psr();
	set_psr(psr | PSR_IND);
	CMMU_LOCK;
	m8820x_cmmu_set_reg(CMMU_SCR, CMMU_FLUSH_USER_ALL, cpu);
	CMMU_UNLOCK;
	set_psr(psr);
}

/*
 * Functions that invalidate caches.
 *
 * Cache operations require physical addresses.
 *
 * We don't writeback instruction caches prior to invalidate because they
 * are never modified.
 *
 * Note that on systems with more than two CMMUs per CPU, we can not benefit
 * from the address split - the split is done on virtual (not translated yet)
 * addresses, but caches are physically indexed.
 */

#define	trunc_cache_line(a)	((a) & ~(MC88200_CACHE_LINE - 1))
#define	round_cache_line(a)	trunc_cache_line((a) + MC88200_CACHE_LINE - 1)

/*
 * invalidate I$, writeback and invalidate D$
 */
void
m8820x_cache_wbinv(cpuid_t cpu, paddr_t pa, psize_t size)
{
	u_int32_t psr;
	psize_t count;

	size = round_cache_line(pa + size) - trunc_cache_line(pa);
	pa = trunc_cache_line(pa);

	psr = get_psr();
	set_psr(psr | PSR_IND);
	CMMU_LOCK;

	while (size != 0) {
		if ((pa & PAGE_MASK) == 0 && size >= PAGE_SIZE) {
			m8820x_cmmu_set_cmd(CMMU_FLUSH_CACHE_CBI_PAGE, cpu, pa);
			count = PAGE_SIZE;
		} else {
			m8820x_cmmu_set_cmd(CMMU_FLUSH_CACHE_CBI_LINE, cpu, pa);
			count = MC88200_CACHE_LINE;
		}
		pa += count;
		size -= count;
		m8820x_cmmu_wait(cpu);
	}

	CMMU_UNLOCK;
	set_psr(psr);
}

/*
 * writeback D$
 */
void
m8820x_dcache_wb(cpuid_t cpu, paddr_t pa, psize_t size)
{
	u_int32_t psr;
	psize_t count;

	size = round_cache_line(pa + size) - trunc_cache_line(pa);
	pa = trunc_cache_line(pa);

	psr = get_psr();
	set_psr(psr | PSR_IND);
	CMMU_LOCK;

	while (size != 0) {
		if ((pa & PAGE_MASK) == 0 && size >= PAGE_SIZE) {
			m8820x_cmmu_set_cmd_if_mode(CMMU_FLUSH_CACHE_CB_PAGE,
			    cpu, pa, DATA_CMMU);
			count = PAGE_SIZE;
		} else {
			m8820x_cmmu_set_cmd_if_mode(CMMU_FLUSH_CACHE_CB_LINE,
			    cpu, pa, DATA_CMMU);
			count = MC88200_CACHE_LINE;
		}
		pa += count;
		size -= count;
		m8820x_cmmu_wait(cpu);
	}

	CMMU_UNLOCK;
	set_psr(psr);
}

/*
 * invalidate I$
 */
void
m8820x_icache_inv(cpuid_t cpu, paddr_t pa, psize_t size)
{
	u_int32_t psr;
	psize_t count;

	size = round_cache_line(pa + size) - trunc_cache_line(pa);
	pa = trunc_cache_line(pa);

	psr = get_psr();
	set_psr(psr | PSR_IND);
	CMMU_LOCK;

	while (size != 0) {
		if ((pa & PAGE_MASK) == 0 && size >= PAGE_SIZE) {
			m8820x_cmmu_set_cmd_if_mode(CMMU_FLUSH_CACHE_INV_PAGE,
			    cpu, pa, INST_CMMU);
			count = PAGE_SIZE;
		} else {
			m8820x_cmmu_set_cmd_if_mode(CMMU_FLUSH_CACHE_INV_LINE,
			    cpu, pa, INST_CMMU);
			count = MC88200_CACHE_LINE;
		}
		pa += count;
		size -= count;
		m8820x_cmmu_wait(cpu);
	}

	CMMU_UNLOCK;
	set_psr(psr);
}

/*
 * writeback D$
 */
void
m8820x_cmmu_wb_locked(int cpu, paddr_t pa, psize_t size)
{
	if (size <= MC88200_CACHE_LINE) {
		m8820x_cmmu_set_cmd_if_mode(CMMU_FLUSH_CACHE_CB_LINE,
		    cpu, pa, DATA_CMMU);
	} else {
		m8820x_cmmu_set_cmd_if_mode(CMMU_FLUSH_CACHE_CB_PAGE,
		    cpu, pa, DATA_CMMU);
	}
}

/*
 * invalidate I$, writeback and invalidate D$
 */
void
m8820x_cmmu_wbinv_locked(int cpu, paddr_t pa, psize_t size)
{
	if (size <= MC88200_CACHE_LINE)
		m8820x_cmmu_set_cmd(CMMU_FLUSH_CACHE_CBI_LINE, cpu, pa);
	else
		m8820x_cmmu_set_cmd(CMMU_FLUSH_CACHE_CBI_PAGE, cpu, pa);
}

/*
 * invalidate I$ and D$
 */
void
m8820x_cmmu_inv_locked(int cpu, paddr_t pa, psize_t size)
{
	if (size <= MC88200_CACHE_LINE)
		m8820x_cmmu_set_cmd(CMMU_FLUSH_CACHE_INV_LINE, cpu, pa);
	else
		m8820x_cmmu_set_cmd(CMMU_FLUSH_CACHE_INV_PAGE, cpu, pa);
}

/*
 * High level cache handling functions (used by bus_dma).
 *
 * On multiprocessor systems, since the CMMUs snoop each other, they
 * all have a coherent view of the data. Thus, we only need to writeback
 * on a single CMMU. However, invalidations need to be done on all CMMUs.
 */

void
m8820x_dma_cachectl(paddr_t _pa, psize_t _size, int op)
{
	u_int32_t psr;
	int cpu;
#ifdef MULTIPROCESSOR
	struct cpu_info *ci = curcpu();
#endif
	paddr_t pa;
	psize_t size, count;
	void (*flusher)(int, paddr_t, psize_t);
	uint8_t lines[2 * MC88200_CACHE_LINE];
	paddr_t pa1, pa2;
	psize_t sz1, sz2;

	pa = trunc_cache_line(_pa);
	size = round_cache_line(_pa + _size) - pa;
	sz1 = sz2 = 0;

	switch (op) {
	case DMA_CACHE_SYNC:
		flusher = m8820x_cmmu_wb_locked;
		break;
	case DMA_CACHE_SYNC_INVAL:
		flusher = m8820x_cmmu_wbinv_locked;
		break;
	default:
	case DMA_CACHE_INV:
		pa1 = pa;
		sz1 = _pa - pa1;
		pa2 = _pa + _size;
		sz2 = pa + size - pa2;
		flusher = m8820x_cmmu_inv_locked;
		break;
	}

#ifndef MULTIPROCESSOR
	cpu = cpu_number();
#endif

	psr = get_psr();
	set_psr(psr | PSR_IND);
	CMMU_LOCK;

	/*
	 * Preserve the data from incomplete cache lines about to be
	 * invalidated, if necessary.
	 */
	if (sz1 != 0)
		bcopy((void *)pa1, lines, sz1);
	if (sz2 != 0)
		bcopy((void *)pa2, lines + MC88200_CACHE_LINE, sz2);

	while (size != 0) {
		count = (pa & PAGE_MASK) == 0 && size >= PAGE_SIZE ?
		    PAGE_SIZE : MC88200_CACHE_LINE;

#ifdef MULTIPROCESSOR
		/*
		 * Theoretically, it should be possible to issue the writeback
		 * operation only on the CMMU which has the affected cache
		 * lines in its memory; snooping would force the other CMMUs
		 * to invalidate their own copy of the line, if any.
		 *
		 * Unfortunately, there is no cheap way to figure out
		 * which CMMU has the lines (and has them as dirty).
		 */
		for (cpu = 0; cpu < MAX_CPUS; cpu++) {
			if (!ISSET(m88k_cpus[cpu].ci_flags, CIF_ALIVE))
				continue;
			(*flusher)(cpu, pa, count);
		}
		for (cpu = 0; cpu < MAX_CPUS; cpu++) {
			if (!ISSET(m88k_cpus[cpu].ci_flags, CIF_ALIVE))
				continue;
			m8820x_cmmu_wait(cpu);
		}
#else	/* MULTIPROCESSOR */
		(*flusher)(cpu, pa, count);
		m8820x_cmmu_wait(cpu);
#endif	/* MULTIPROCESSOR */

		pa += count;
		size -= count;
	}

	/*
	 * Restore data from incomplete cache lines having been invalidated,
	 * if necessary, write them back, and invalidate them again.
	 * (Note that these lines have been invalidated from all processors
	 *  in the loop above, so there is no need to remote invalidate them
	 *  again.)
	 */
	if (sz1 != 0)
		bcopy(lines, (void *)pa1, sz1);
	if (sz2 != 0)
		bcopy(lines + MC88200_CACHE_LINE, (void *)pa2, sz2);
	if (sz1 != 0) {
#ifdef MULTIPROCESSOR
		m8820x_cmmu_wbinv_locked(ci->ci_cpuid, pa1, MC88200_CACHE_LINE);
		m8820x_cmmu_wait(ci->ci_cpuid);
#else
		m8820x_cmmu_wbinv_locked(cpu, pa1, MC88200_CACHE_LINE);
		m8820x_cmmu_wait(cpu);
#endif
	}
	if (sz2 != 0) {
		pa2 = trunc_cache_line(pa2);
#ifdef MULTIPROCESSOR
		m8820x_cmmu_wbinv_locked(ci->ci_cpuid, pa2, MC88200_CACHE_LINE);
		m8820x_cmmu_wait(ci->ci_cpuid);
#else
		m8820x_cmmu_wbinv_locked(cpu, pa2, MC88200_CACHE_LINE);
		m8820x_cmmu_wait(cpu);
#endif
	}

	CMMU_UNLOCK;
	set_psr(psr);
}

#ifdef MULTIPROCESSOR
void
m8820x_dma_cachectl_local(paddr_t pa, psize_t size, int op)
{
	/* This function is not used on 88100 systems */
}
#endif

#if defined(__luna88k__) && !defined(MULTIPROCESSOR)
/*
 * On luna88k, secondary processors are not disabled while the kernel
 * is initializing.  They are running an infinite loop in
 * locore.S:secondary_init on non-MULTIPROCESSOR kernel.  Then, after
 * initializing the CMMUs tied to the currently-running processor, we
 * turn on the instruction cache of other processors to make them
 * happier.
 */
void
m8820x_enable_other_cmmu_cache()
{
	int cpu, master_cpu = cpu_number();

	for (cpu = 0; cpu < ncpusfound; cpu++) {
		if (cpu == master_cpu)
			continue;
		/* Enable other processor's instruction cache */
		m8820x_cmmu_set_reg_if_mode(CMMU_SAPR, CACHE_GLOBAL,
			cpu, INST_CMMU);
	}
}
#endif


code:
/*	$OpenBSD: m88k_machdep.c,v 1.64 2015/02/11 07:05:39 dlg Exp $	*/
/*
 * Copyright (c) 1998, 1999, 2000, 2001 Steve Murphree, Jr.
 * Copyright (c) 1996 Nivas Madhur
 * 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 Nivas Madhur.
 * 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.
 *
 */
/*
 * Mach Operating System
 * Copyright (c) 1993-1991 Carnegie Mellon University
 * Copyright (c) 1991 OMRON Corporation
 * All Rights Reserved.
 *
 * Permission to use, copy, modify and distribute this software and its
 * documentation is hereby granted, provided that both the copyright
 * notice and this permission notice appear in all copies of the
 * software, derivative works or modified versions, and any portions
 * thereof, and that both notices appear in supporting documentation.
 *
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/proc.h>
#include <sys/user.h>
#include <sys/msgbuf.h>
#include <sys/exec.h>
#include <sys/errno.h>
#include <sys/malloc.h>
#ifdef MULTIPROCESSOR
#include <sys/mplock.h>
#endif

#include <machine/asm.h>
#include <machine/asm_macro.h>
#include <machine/atomic.h>
#include <machine/lock.h>
#include <machine/cmmu.h>
#include <machine/cpu.h>
#include <machine/reg.h>
#ifdef M88100
#include <machine/m88100.h>
#endif

#include <uvm/uvm_extern.h>

#ifdef DDB
#include <machine/db_machdep.h>
#include <ddb/db_extern.h>
#include <ddb/db_interface.h>
#endif /* DDB */

typedef struct {
	u_int32_t word_one, word_two;
} m88k_exception_vector_area;

void	dumpconf(void);
void	dumpsys(void);
void	regdump(struct trapframe *f);
void	*vector_init(m88k_exception_vector_area *, u_int32_t *, int);
void	atomic_init(void);

/*
 * CMMU and CPU variables
 */

#ifdef MULTIPROCESSOR
cpuid_t	master_cpu;
__cpu_simple_lock_t cmmu_cpu_lock = __SIMPLELOCK_UNLOCKED;
#endif

struct cpu_info m88k_cpus[MAX_CPUS] = {
#ifndef MULTIPROCESSOR
	{ .ci_flags = CIF_ALIVE | CIF_PRIMARY }
#endif
};
const struct cmmu_p *cmmu;

/*
 * safepri is a safe priority for sleep to set for a spin-wait
 * during autoconfiguration or after a panic.
 */
int   safepri = IPL_NONE;

/*
 * Set registers on exec.
 * Clear all except sp and pc.
 */
void
setregs(p, pack, stack, retval)
	struct proc *p;
	struct exec_package *pack;
	u_long stack;
	register_t retval[2];
{
	struct trapframe *tf = (struct trapframe *)USER_REGS(p);

	/*
	 * Setup proper floating-point settings. This is necessary because
	 * we will return through the exception path, which only saves the
	 * integer registers, and not through cpu_switchto() (which saves
	 * fcr62 and fcr63 in the pcb).  This is safe to do here since the
	 * FPU is enabled and the kernel doesn't use it.
	 */
	__asm__ volatile ("fstcr %r0, %fcr0");
	__asm__ volatile ("fstcr %r0, %fcr62");
	__asm__ volatile ("fstcr %r0, %fcr63");

	bzero((caddr_t)tf, sizeof *tf);

#ifdef M88110
	if (CPU_IS88110) {
		/*
		 * user mode, interrupts enabled,
		 * graphics unit, fp enabled
		 */
		tf->tf_epsr = PSR_SFD;
	}
#endif
#ifdef M88100
	if (CPU_IS88100) {
		/*
		 * user mode, interrupts enabled,
		 * no graphics unit, fp enabled
		 */
		tf->tf_epsr = PSR_SFD | PSR_SFD2;
	}
#endif

	/*
	 * We want to start executing at pack->ep_entry. The way to
	 * do this is force the processor to fetch from ep_entry.
	 *
	 * However, since we will return through m{88100,88110}_syscall(),
	 * we need to setup registers so that the success return, when
	 * ``incrementing'' the instruction pointers, will cause the
	 * binary to start at the expected address.
	 *
	 * This relies on the fact that binaries start with
	 *
	 *	br.n	1f
	 *	 or	r2, r0, r30
	 * 1:
	 *
	 * So the first two instructions can be skipped.
	 */
#ifdef M88110
	if (CPU_IS88110) {
		/*
		 * m88110_syscall() will resume at exip + 8... which
		 * really is the first instruction we want to run.
		 */
		tf->tf_exip = pack->ep_entry & XIP_ADDR;
	}
#endif
#ifdef M88100
	if (CPU_IS88100) {
		/*
		 * m88100_syscall() will resume at sfip / sfip + 4...
		 */
		tf->tf_sfip = ((pack->ep_entry + 8) & FIP_ADDR) | FIP_V;

		/*
		 * ... unless we are starting init, in which case we
		 * won't be returning through the regular path, and
		 * need to explicitely set up nip and fip (note that
		 * 88110 do not need such a test).
		 */
		if (p->p_pid == 1) {
			tf->tf_snip = tf->tf_sfip;
			tf->tf_sfip += 4;
		}
	}
#endif
	tf->tf_r[2] = retval[0] = stack;
	tf->tf_r[31] = stack;
	retval[1] = 0;
}

int
copystr(fromaddr, toaddr, maxlength, lencopied)
	const void *fromaddr;
	void *toaddr;
	size_t maxlength;
	size_t *lencopied;
{
	u_int tally;

	tally = 0;

	while (maxlength--) {
		*(u_char *)toaddr = *(u_char *)fromaddr++;
		tally++;
		if (*(u_char *)toaddr++ == 0) {
			if (lencopied) *lencopied = tally;
			return (0);
		}
	}

	if (lencopied)
		*lencopied = tally;

	return (ENAMETOOLONG);
}

#ifdef DDB
int longformat = 1;
void
regdump(struct trapframe *f)
{
#define R(i) f->tf_r[i]
	printf("R00-05: 0x%08lx  0x%08lx  0x%08lx  0x%08lx  0x%08lx  0x%08lx\n",
	       R(0),R(1),R(2),R(3),R(4),R(5));
	printf("R06-11: 0x%08lx  0x%08lx  0x%08lx  0x%08lx  0x%08lx  0x%08lx\n",
	       R(6),R(7),R(8),R(9),R(10),R(11));
	printf("R12-17: 0x%08lx  0x%08lx  0x%08lx  0x%08lx  0x%08lx  0x%08lx\n",
	       R(12),R(13),R(14),R(15),R(16),R(17));
	printf("R18-23: 0x%08lx  0x%08lx  0x%08lx  0x%08lx  0x%08lx  0x%08lx\n",
	       R(18),R(19),R(20),R(21),R(22),R(23));
	printf("R24-29: 0x%08lx  0x%08lx  0x%08lx  0x%08lx  0x%08lx  0x%08lx\n",
	       R(24),R(25),R(26),R(27),R(28),R(29));
	printf("R30-31: 0x%08lx  0x%08lx\n",R(30),R(31));
#ifdef M88110
	if (CPU_IS88110) {
		printf("exip %lx enip %lx\n", f->tf_exip, f->tf_enip);
	}
#endif
#ifdef M88100
	if (CPU_IS88100) {
		printf("sxip %lx snip %lx sfip %lx\n",
		    f->tf_sxip, f->tf_snip, f->tf_sfip);
	}
	if (CPU_IS88100 && ISSET(f->tf_dmt0, DMT_VALID)) {
		/* print dmt stuff for data access fault */
		printf("fault type %ld\n", (f->tf_dpfsr >> 16) & 0x7);
		dae_print((u_int *)f);
	}
	if (CPU_IS88100 && longformat != 0) {
		printf("fpsr %lx fpcr %lx epsr %lx ssbr %lx\n",
		    f->tf_fpsr, f->tf_fpcr, f->tf_epsr, f->tf_ssbr);
		printf("fpecr %lx fphs1 %lx fpls1 %lx fphs2 %lx fpls2 %lx\n",
		    f->tf_fpecr, f->tf_fphs1, f->tf_fpls1,
		    f->tf_fphs2, f->tf_fpls2);
		printf("fppt %lx fprh %lx fprl %lx fpit %lx\n",
		    f->tf_fppt, f->tf_fprh, f->tf_fprl, f->tf_fpit);
		printf("vector %ld mask %lx flags %lx scratch1 %lx cpu %p\n",
		    f->tf_vector, f->tf_mask, f->tf_flags,
		    f->tf_scratch1, f->tf_cpu);
	}
#endif
#ifdef M88110
	if (CPU_IS88110 && longformat != 0) {
		printf("fpsr %lx fpcr %lx fpecr %lx epsr %lx\n",
		    f->tf_fpsr, f->tf_fpcr, f->tf_fpecr, f->tf_epsr);
		printf("dsap %lx duap %lx dsr %lx dlar %lx dpar %lx\n",
		    f->tf_dsap, f->tf_duap, f->tf_dsr, f->tf_dlar, f->tf_dpar);
		printf("isap %lx iuap %lx isr %lx ilar %lx ipar %lx\n",
		    f->tf_isap, f->tf_iuap, f->tf_isr, f->tf_ilar, f->tf_ipar);
		printf("vector %ld mask %lx flags %lx scratch1 %lx cpu %p\n",
		    f->tf_vector, f->tf_mask, f->tf_flags,
		    f->tf_scratch1, f->tf_cpu);
	}
#endif
}
#endif	/* DDB */

/*
 * Set up the cpu_info pointer and the cpu number for the current processor.
 */
void
set_cpu_number(cpuid_t number)
{
	struct cpu_info *ci;

#ifdef MULTIPROCESSOR
	ci = &m88k_cpus[number];
#else
	ci = &m88k_cpus[0];
#endif
	ci->ci_cpuid = number;

	__asm__ volatile ("stcr %0, %%cr17" :: "r" (ci));
	flush_pipeline();
}

/*
 * Notify the current process (p) that it has a signal pending,
 * process as soon as possible.
 */
void
signotify(struct proc *p)
{
	aston(p);
	cpu_unidle(p->p_cpu);
}

#ifdef MULTIPROCESSOR
void
cpu_unidle(struct cpu_info *ci)
{
	if (ci != curcpu())
		m88k_send_ipi(CI_IPI_NOTIFY, ci->ci_cpuid);
}
#endif

/*
 * Preempt the current process if in interrupt from user mode,
 * or after the current trap/syscall if in system mode.
 */
void
need_resched(struct cpu_info *ci)
{
	ci->ci_want_resched = 1;

	/* There's a risk we'll be called before the idle threads start */
	if (ci->ci_curproc != NULL) {
		aston(ci->ci_curproc);
		if (ci != curcpu())
			cpu_unidle(ci);
	}
}

/*
 * Generic soft interrupt interface
 */

void	dosoftint(int);
int	softpending;

void
dosoftint(int sir)
{
	int q, mask;

#ifdef MULTIPROCESSOR
	__mp_lock(&kernel_lock);
#endif

	for (q = SI_NQUEUES - 1, mask = 1 << (SI_NQUEUES - 1); mask != 0;
	    q--, mask >>= 1)
		if (mask & sir)
			softintr_dispatch(q);

#ifdef MULTIPROCESSOR
	__mp_unlock(&kernel_lock);
#endif
}

int
spl0()
{
	int sir;
	int s;

	/*
	 * Try to avoid potentially expensive setipl calls if nothing
	 * seems to be pending.
	 */
	if ((sir = atomic_clear_int(&softpending)) != 0) {
		s = setipl(IPL_SOFTINT);
		dosoftint(sir);
		setipl(IPL_NONE);
	} else
		s = setipl(IPL_NONE);

	return (s);
}

#define EMPTY_BR	0xc0000000	/* empty "br" instruction */
#define NO_OP 		0xf4005800	/* "or r0, r0, r0" */

#define BRANCH(FROM, TO) \
	(EMPTY_BR | ((((vaddr_t)(TO) - (vaddr_t)(FROM)) >> 2) & 0x03ffffff))

#define SET_VECTOR_88100(NUM, VALUE) \
	do { \
		vbr[NUM].word_one = NO_OP; \
		vbr[NUM].word_two = BRANCH(&vbr[NUM].word_two, VALUE); \
	} while (0)

#define SET_VECTOR_88110(NUM, VALUE) \
	do { \
		vbr[NUM].word_one = BRANCH(&vbr[NUM].word_one, VALUE); \
		vbr[NUM].word_two = NO_OP; \
	} while (0)

/*
 * vector_init(vector, vector_init_list, bootstrap)
 *
 * This routine sets up the m88k vector table for the running processor,
 * as well as the atomic operation routines for multiprocessor kernels.
 * This is the first C code to run, before anything is initialized.
 *
 * I would add an extra four bytes to the exception vectors page pointed
 * to by the vbr, since the 88100 may execute the first instruction of the
 * next trap handler, as documented in its Errata. Processing trap #511
 * would then fall into the next page, unless the address computation wraps,
 * or software traps can not trigger the issue - the Errata does not provide
 * more detail. And since the MVME BUG does not add an extra NOP after its
 * VBR page, I'll assume this is safe for now -- miod
 */
void *
vector_init(m88k_exception_vector_area *vbr, u_int32_t *vector_init_list,
    int bootstrap)
{
	u_int num;
	u_int32_t vec;

	switch (cputyp) {
	default:
#ifdef M88110
	case CPU_88110:
	    {
		extern void m88110_sigsys(void);
		extern void m88110_syscall_handler(void);
		extern void m88110_cache_flush_handler(void);
		extern void m88110_stepbpt(void);
		extern void m88110_userbpt(void);

		for (num = 0; (vec = vector_init_list[num]) != 0; num++)
			SET_VECTOR_88110(num, vec);

		if (bootstrap)
			SET_VECTOR_88110(0x03, vector_init_list[num + 1]);

		for (; num < 512; num++)
			SET_VECTOR_88110(num, m88110_sigsys);

		SET_VECTOR_88110(450, m88110_syscall_handler);
		SET_VECTOR_88110(451, m88110_cache_flush_handler);
		/*
		 * GCC will by default produce explicit trap 503
		 * for division by zero
		 */
		SET_VECTOR_88110(503, vector_init_list[8]);
		SET_VECTOR_88110(504, m88110_stepbpt);
		SET_VECTOR_88110(511, m88110_userbpt);
	    }
		break;
#endif
#ifdef M88100
	case CPU_88100:
	    {
		extern void sigsys(void);
		extern void syscall_handler(void);
		extern void cache_flush_handler(void);
		extern void stepbpt(void);
		extern void userbpt(void);

		for (num = 0; (vec = vector_init_list[num]) != 0; num++)
			SET_VECTOR_88100(num, vec);

		if (bootstrap)
			SET_VECTOR_88100(0x03, vector_init_list[num + 1]);

		for (; num < 512; num++)
			SET_VECTOR_88100(num, sigsys);

		SET_VECTOR_88100(450, syscall_handler);
		SET_VECTOR_88100(451, cache_flush_handler);
		/*
		 * GCC will by default produce explicit trap 503
		 * for division by zero
		 */
		SET_VECTOR_88100(503, vector_init_list[8]);
		SET_VECTOR_88100(504, stepbpt);
		SET_VECTOR_88100(511, userbpt);
	    }
		break;
#endif
	}

	return vbr;
}

#ifdef MULTIPROCESSOR
/*
 * void atomic_init(void);
 *
 * This routine sets up proper atomic operation code for SMP kernels
 * with both 88100 and 88110 support compiled-in. This is crucial enough
 * to have to be done as early as possible.
 * This is among the first C code to run, before anything is initialized.
 */
void
atomic_init()
{
#if defined(M88100) && defined(M88110)
	if (cputyp == CPU_88100) {
		extern uint32_t __atomic_lock[];
		extern uint32_t __atomic_lock_88100[], __atomic_lock_88100_end[];
		extern uint32_t __atomic_unlock[];
		extern uint32_t __atomic_unlock_88100[], __atomic_unlock_88100_end[];

		uint32_t *s, *e, *d;

		d = __atomic_lock;
		s = __atomic_lock_88100;
		e = __atomic_lock_88100_end;
		while (s != e)
				*d++ = *s++;

		d = __atomic_unlock;
		s = __atomic_unlock_88100;
		e = __atomic_unlock_88100_end;
		while (s != e)
				*d++ = *s++;
	}
#endif	/* M88100 && M88110 */
}
#endif	/* MULTIPROCESSOR */

#ifdef MULTIPROCESSOR

/*
 * This function is invoked when it turns out one secondary processor is
 * not usable.
 * Be sure to put the process currently running on it in the run queues,
 * so that another processor can take care of it.
 */
__dead void
cpu_emergency_disable()
{
	struct cpu_info *ci = curcpu();
	struct schedstate_percpu *spc = &ci->ci_schedstate;
	struct proc *p = curproc;
	int s;
	extern void savectx(struct pcb *);

	if (p != NULL && p != spc->spc_idleproc) {
		savectx(curpcb);

		/*
		 * The following is an inline yield(), without the call
		 * to mi_switch().
		 */
		SCHED_LOCK(s);
		p->p_priority = p->p_usrpri;
		p->p_stat = SRUN;
		setrunqueue(p);
		p->p_ru.ru_nvcsw++;
		SCHED_UNLOCK(s);
	}

	CLR(ci->ci_flags, CIF_ALIVE);
	set_psr(get_psr() | PSR_IND);
	splhigh();

	for (;;) ;
	/* NOTREACHED */
}

#endif	/* MULTIPROCESSOR */


code:
/*	$OpenBSD: mem.c,v 1.3 2015/02/10 22:44:35 miod Exp $ */

/*
 * Copyright (c) 1988 University of Utah.
 * Copyright (c) 1982, 1986, 1990, 1993
 *	The Regents of the University of California.  All rights reserved.
 *
 * This code is derived from software contributed to Berkeley by
 * the Systems Programming Group of the University of Utah Computer
 * Science Department.
 *
 * 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 of the University 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 REGENTS 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 REGENTS 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.
 *
 *	@(#)mem.c	8.3 (Berkeley) 1/12/94
 */

/*
 * Memory special file
 */

#include <sys/param.h>
#include <sys/buf.h>
#include <sys/systm.h>
#include <sys/uio.h>
#include <sys/malloc.h>

#include <machine/conf.h>

#include <uvm/uvm_extern.h>

caddr_t zpage;
extern vaddr_t first_addr, last_addr;
extern caddr_t kernelstart;
extern void *etext;

/*ARGSUSED*/
int
mmopen(dev, flag, mode, p)
	dev_t dev;
	int flag, mode;
	struct proc *p;
{

	switch (minor(dev)) {
		case 0:
		case 1:
		case 2:
		case 12:
			return (0);
		default:
			return (ENXIO);
	}
}

/*ARGSUSED*/
int
mmclose(dev, flag, mode, p)
	dev_t dev;
	int flag, mode;
	struct proc *p;
{

	return (0);
}

/*ARGSUSED*/
int
mmrw(dev, uio, flags)
	dev_t dev;
	struct uio *uio;
	int flags;
{
	vaddr_t v;
	size_t c;
	struct iovec *iov;
	int error = 0;

	while (uio->uio_resid > 0 && error == 0) {
		iov = uio->uio_iov;
		if (iov->iov_len == 0) {
			uio->uio_iov++;
			uio->uio_iovcnt--;
			if (uio->uio_iovcnt < 0)
				panic("mmrw");
			continue;
		}
		switch (minor(dev)) {

/* minor device 0 is physical memory */
		case 0:
			v = uio->uio_offset;
			error = uiomove((caddr_t)v, uio->uio_resid, uio);
			continue;

/* minor device 1 is kernel memory */
		case 1:
			v = uio->uio_offset;
			c = ulmin(iov->iov_len, MAXPHYS);
			if (v >= (vaddr_t)&kernelstart &&
			    v < (vaddr_t)first_addr) {
				if (v < (vaddr_t)etext &&
				    uio->uio_rw == UIO_WRITE)
					return (EFAULT);
			} else if (!uvm_kernacc((caddr_t)v, c,
			    uio->uio_rw == UIO_READ ? B_READ : B_WRITE))
				return (EFAULT);
			error = uiomove((caddr_t)v, c, uio);
			continue;

/* minor device 2 is EOF/RATHOLE */
		case 2:
			if (uio->uio_rw == UIO_WRITE)
				uio->uio_resid = 0;
			return (0);

/* minor device 12 (/dev/zero) is source of nulls on read, rathole on write */
		case 12:
			if (uio->uio_rw == UIO_WRITE) {
				c = iov->iov_len;
				break;
			}
			if (zpage == NULL)
				zpage = malloc(PAGE_SIZE, M_TEMP,
				    M_WAITOK | M_ZERO);
			c = ulmin(iov->iov_len, PAGE_SIZE);
			error = uiomove(zpage, c, uio);
			continue;

		default:
			return (ENXIO);
		}
		if (error)
			break;
		iov->iov_base += c;
		iov->iov_len -= c;
		uio->uio_offset += c;
		uio->uio_resid -= c;
	}

	return (error);
}

paddr_t
mmmmap(dev, off, prot)
        dev_t dev;
        off_t off;
	int prot;
{
	return (-1);
}

/*ARGSUSED*/
int
mmioctl(dev, cmd, data, flags, p)
	dev_t dev;
	u_long cmd;
	caddr_t data;
	int flags;
	struct proc *p;
{
	return (EOPNOTSUPP);
}


code:
/*	$OpenBSD: mplock.c,v 1.3 2015/02/11 07:05:39 dlg Exp $	*/

/*
 * Copyright (c) 2004 Niklas Hallqvist.  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.
 *
 * 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 <sys/systm.h>

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

#include <ddb/db_output.h>

#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	do { /* nothing */ } while (0)

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(0x%x): lock spun out", mpl);
		Debugger();
	}
#endif
}

void
__mp_lock(struct __mp_lock *mpl)
{
	struct cpu_info *ci = curcpu();
	uint32_t psr;
	uint gcsr;

	/*
	 * 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.
	 */

	for (;;) {
		psr = (*ci->ci_mp_atomic_begin)(&mpl->mpl_lock, &gcsr);

		if (mpl->mpl_count == 0) {
			mpl->mpl_count = 1;
			mpl->mpl_cpu = ci;
		}
		if (mpl->mpl_cpu == ci) {
			mpl->mpl_count++;
			(*ci->ci_mp_atomic_end)(psr, &mpl->mpl_lock, gcsr);
			break;
		}
		(*ci->ci_mp_atomic_end)(psr, &mpl->mpl_lock, gcsr);

		__mp_lock_spin(mpl);
	}
}

void
__mp_unlock(struct __mp_lock *mpl)
{
	struct cpu_info *ci = curcpu();
	u_int32_t psr;
	uint gcsr;

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

	psr = (*ci->ci_mp_atomic_begin)(&mpl->mpl_lock, &gcsr);
	if (--mpl->mpl_count == 1) {
		mpl->mpl_cpu = NULL;
		mpl->mpl_count = 0;
	}
	(*ci->ci_mp_atomic_end)(psr, &mpl->mpl_lock, gcsr);
}

int
__mp_release_all(struct __mp_lock *mpl)
{
	struct cpu_info *ci = curcpu();
	u_int32_t psr;
	uint gcsr;
	int rv;

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

	psr = (*ci->ci_mp_atomic_begin)(&mpl->mpl_lock, &gcsr);
	rv = mpl->mpl_count - 1;
	mpl->mpl_cpu = NULL;
	mpl->mpl_count = 0;
	(*ci->ci_mp_atomic_end)(psr, &mpl->mpl_lock, gcsr);

	return (rv);
}

int
__mp_release_all_but_one(struct __mp_lock *mpl)
{
	struct cpu_info *ci = curcpu();
	u_int32_t psr;
	uint gcsr;
	int rv;

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

	psr = (*ci->ci_mp_atomic_begin)(&mpl->mpl_lock, &gcsr);
	rv = mpl->mpl_count - 2;
	mpl->mpl_count = 2;
	(*ci->ci_mp_atomic_end)(psr, &mpl->mpl_lock, gcsr);

	return (rv);
}


code:
/*	$OpenBSD: mutex.S,v 1.14 2015/07/03 15:12:49 miod Exp $	*/

/*
 * Copyright (c) 2005, Miodrag Vallat.
 *
 * 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 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 "assym.h"
#if MTX_LOCK != 0
#error	Lack of Humppa in mutex code
#endif

#include <machine/asm.h>

/*
 * void mtx_init(struct mutex *mtx, int ipl)
 */
ENTRY(__mtx_init)
	st	%r0, %r2, MTX_LOCK	/* mtx->mtx_lock = 0 */
	st	%r0, %r2, MTX_OLDIPL	/* mtx->mtx_oldipl = IPL_NONE */
	st	%r3, %r2, MTX_WANTIPL	/* mtx->mtx_wantipl = ipl */
	jmp.n	%r1
	 st	%r0, %r2, MTX_OWNER	/* mtx->mtx_owner = NULL */

/*
 * void mtx_enter(struct mutex *mtx)
 */
ENTRY(mtx_enter)
	subu	%r31, %r31, 8
	st	%r1,  %r31, 4		/* save return address */

#ifdef MULTIPROCESSOR

	st	%r2,  %r31, 0		/* save mtx */
enter_again:
	ld	%r2,  %r2,  MTX_WANTIPL
	bcnd	eq0,  %r2,  1f
	bsr	_C_LABEL(raiseipl)	/* raiseipl(mtx->mtx_wantipl) */
1:
	ld	%r4,  %r31, 0
	or	%r3,  %r0,  1
	xmem	%r3,  %r4,  %r0		/* attempt to claim the lock, old */
	bcnd	ne0,  %r3,  enter_failed /* mtx->mtx_lock is 0 if successful */

	ldcr	%r3,  CPU
	st	%r2,  %r4,  MTX_OLDIPL	/* save into mtx_oldipl */
	st	%r3,  %r4,  MTX_OWNER	/* mtx->mtx_owner = curcpu() */
#ifdef DIAGNOSTIC
	ld	%r2,  %r3,  CI_MUTEX_LEVEL
	addu	%r2,  %r2,  1		/* curcpu()->ci_mutex_level++ */
	st	%r2,  %r3,  CI_MUTEX_LEVEL
#endif

	ld	%r1,  %r31, 4
	jmp.n	%r1
	 addu	%r31, %r31, 8

enter_failed:	/* the lock is not ours... */
	ld	%r3,  %r4,  MTX_WANTIPL
	bcnd	eq0,  %r3,  2f
	bcnd	ne0,  %r2,  1f		/* splx(oldipl) */
	bsr.n	_C_LABEL(spl0)
	 addu	%r1,  %r1,  2f - . - 4
1:
	bsr	_C_LABEL(setipl)
2:
	ld	%r2,  %r31, 0		/* restore mtx */
enter_spin:
#ifdef DIAGNOSTIC
	ldcr	%r3,  CPU
	ld	%r4,  %r2,  MTX_OWNER
	cmp	%r5,  %r3,  %r4
	bcnd	eq0,  %r5,  enter_panic
#endif
	ld	%r3,  %r2,  MTX_LOCK
	bcnd	eq0,  %r3,  enter_again
	br	enter_spin

#ifdef DIAGNOSTIC
enter_panic:
	or.u	%r2,  %r0,  %hi16(9f)
	bsr.n	_C_LABEL(panic)
	 or	%r2,  %r2,  %lo16(9f)

	.data
9:
	.string	"mtx_enter: humpaan itsekseni"
#endif

#else	/* MULTIPROCESSOR */

	st	%r2,  %r31, 0		/* save mtx */
	ld	%r2,  %r2,  MTX_WANTIPL
	bcnd	eq0,  %r2,  1f
	bsr	_C_LABEL(raiseipl)	/* raiseipl(mtx->mtx_wantipl) */
1:
	ld	%r4,  %r31, 0
	ldcr	%r3,  CPU
	st	%r3,  %r4,  MTX_LOCK	/* locked! */

	st	%r2,  %r4,  MTX_OLDIPL	/* save into mtx_oldipl */

#ifdef DIAGNOSTIC
	/* necessary for MUTEX_ASSERT_LOCKED */
	st	%r3,  %r4,  MTX_OWNER	/* mtx->mtx_owner = curcpu() */

	ld	%r2,  %r3,  CI_MUTEX_LEVEL
	addu	%r2,  %r2,  1		/* curcpu()->ci_mutex_level++ */
	st	%r2,  %r3,  CI_MUTEX_LEVEL
#endif

	ld	%r1,  %r31, 4
	jmp.n	%r1
	 addu	%r31, %r31, 8

#endif	/* MULTIPROCESSOR */

/*
 * int mtx_enter_try(struct mutex *mtx)
 */
ENTRY(mtx_enter_try)
	subu	%r31, %r31, 8
	st	%r1,  %r31, 4		/* save return address */

#ifdef MULTIPROCESSOR

	st	%r2,  %r31, 0		/* save mtx */
enter_try_again:
	ld	%r2,  %r2,  MTX_WANTIPL
	bcnd	eq0,  %r2,  1f
	bsr	_C_LABEL(raiseipl)	/* raiseipl(mtx->mtx_wantipl) */
1:
	ld	%r4,  %r31, 0
	or	%r3,  %r0,  1
	xmem	%r3,  %r4,  %r0		/* attempt to claim the lock, old */
	bcnd	ne0,  %r3,  enter_try_failed /* mtx->mtx_lock is 0 if successful*/

	ldcr	%r3,  CPU
	st	%r2,  %r4,  MTX_OLDIPL	/* save into mtx_oldipl */
	st	%r3,  %r4,  MTX_OWNER	/* mtx->mtx_owner = curcpu() */
#ifdef DIAGNOSTIC
	ld	%r2,  %r3,  CI_MUTEX_LEVEL
	addu	%r2,  %r2,  1		/* curcpu()->ci_mutex_level++ */
	st	%r2,  %r3,  CI_MUTEX_LEVEL
#endif

	ld	%r1,  %r31, 4
	or	%r2,  %r0,  1		/* return nonzero */
	jmp.n	%r1
	 addu	%r31, %r31, 8

enter_try_failed:	/* the lock is not ours... */
	ld	%r3,  %r4,  MTX_WANTIPL
	bcnd	eq0,  %r3,  2f
	bcnd	ne0,  %r2,  1f		/* splx(oldipl) */
	bsr.n	_C_LABEL(spl0)
	 addu	%r1,  %r1,  2f - . - 4
1:
	bsr	_C_LABEL(setipl)
2:
#ifdef DIAGNOSTIC
	ld	%r2,  %r31, 0		/* restore mtx */
	ldcr	%r3,  CPU
	ld	%r4,  %r2,  MTX_OWNER
	cmp	%r5,  %r3,  %r4
	bcnd	eq0,  %r5,  enter_try_panic
#endif
	or	%r2,  %r0,  %r0		/* return zero */
	jmp.n	%r1
	 addu	%r31, %r31, 8

#ifdef DIAGNOSTIC
enter_try_panic:
	or.u	%r2,  %r0,  %hi16(9f)
	bsr.n	_C_LABEL(panic)
	 or	%r2,  %r2,  %lo16(9f)

	.data
9:
	.string	"mtx_enter_try: humpaan itsekseni"
#endif

#else	/* MULTIPROCESSOR */

	st	%r2,  %r31, 0		/* save mtx */
	ld	%r2,  %r2,  MTX_WANTIPL
	bcnd	eq0,  %r2,  1f
	bsr	_C_LABEL(raiseipl)	/* raiseipl(mtx->mtx_wantipl) */
1:
	ld	%r4,  %r31, 0
	ldcr	%r3,  CPU
	st	%r3,  %r4,  MTX_LOCK	/* locked! */

	st	%r2,  %r4,  MTX_OLDIPL	/* save into mtx_oldipl */

#ifdef DIAGNOSTIC
	/* necessary for MUTEX_ASSERT_LOCKED */
	st	%r3,  %r4,  MTX_OWNER	/* mtx->mtx_owner = curcpu() */

	ld	%r2,  %r3,  CI_MUTEX_LEVEL
	addu	%r2,  %r2,  1		/* curcpu()->ci_mutex_level++ */
	st	%r2,  %r3,  CI_MUTEX_LEVEL
#endif

	ld	%r1,  %r31, 4
	or	%r2,  %r0,  1		/* return nonzero */
	jmp.n	%r1
	 addu	%r31, %r31, 8

#endif	/* MULTIPROCESSOR */

/*
 * void mtx_leave(struct mutex *mtx)
 */
ENTRY(mtx_leave)
	ld	%r3,  %r2,  MTX_OLDIPL
	ld	%r4,  %r2,  MTX_WANTIPL
#ifdef DIAGNOSTIC
	ld	%r5,  %r2,  MTX_OWNER
	ld	%r6,  %r5,  CI_MUTEX_LEVEL
	subu	%r6,  %r6,  1		/* curcpu()->ci_mutex_level++ */
	st	%r6,  %r5,  CI_MUTEX_LEVEL
#endif
	st	%r0,  %r2,  MTX_OWNER		/* mtx->mtx_owner = NULL */
	bcnd.n	eq0,  %r4,  2f
	 st	%r0,  %r2,  MTX_LOCK		/* mtx->mtx_lock = 0 */

	bcnd	ne0, %r3,  1f			/* splx(mtx->mtx_oldipl) */
	br	_C_LABEL(spl0)
1:
	br.n	_C_LABEL(setipl)
	 or	%r2,  %r3,  %r0
2:
	jmp	%r1