code:
/*	$OpenBSD: fp_emulate.c,v 1.11 2015/05/05 21:24:58 jmatthew Exp $	*/

/*
 * Copyright (c) 2010 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.
 */

/*
 * Floating Point completion/emulation code (MI softfloat code control engine).
 *
 * Supports all MIPS IV COP1 and COP1X floating-point instructions.
 *
 * Floating-point load and store instructions, as well as branch instructions,
 * are only handled if the kernel is compiled with option FPUEMUL.
 */

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

#include <machine/cpu.h>
#include <mips64/mips_cpu.h>
#include <machine/fpu.h>
#include <machine/frame.h>
#include <machine/ieee.h>
#include <machine/ieeefp.h>
#include <machine/mips_opcode.h>
#include <machine/regnum.h>

#include <lib/libkern/softfloat.h>
#if defined(DEBUG) && defined(DDB)
#include <machine/db_machdep.h>
#endif

int	fpu_emulate(struct trap_frame *, uint32_t, union sigval *);
int	fpu_emulate_cop1(struct trap_frame *, uint32_t);
int	fpu_emulate_cop1x(struct trap_frame *, uint32_t);
uint64_t
	fpu_load(struct trap_frame *, uint, uint);
void	fpu_store(struct trap_frame *, uint, uint, uint64_t);
#ifdef FPUEMUL
int	nofpu_emulate_cop1(struct trap_frame *, uint32_t, union sigval *);
int	nofpu_emulate_cop1x(struct trap_frame *, uint32_t, union sigval *);
int	nofpu_emulate_loadstore(struct trap_frame *, uint32_t, union sigval *);
int	nofpu_emulate_movci(struct trap_frame *, uint32_t);
#endif

typedef	int (fpu_fn3)(struct trap_frame *, uint, uint, uint, uint);
typedef	int (fpu_fn4)(struct trap_frame *, uint, uint, uint, uint, uint);
fpu_fn3	fpu_abs;
fpu_fn3	fpu_add;
int	fpu_c(struct trap_frame *, uint, uint, uint, uint, uint);
fpu_fn3	fpu_ceil_l;
fpu_fn3	fpu_ceil_w;
fpu_fn3	fpu_cvt_d;
fpu_fn3	fpu_cvt_l;
fpu_fn3	fpu_cvt_s;
fpu_fn3	fpu_cvt_w;
fpu_fn3	fpu_div;
fpu_fn3	fpu_floor_l;
fpu_fn3	fpu_floor_w;
int	fpu_int_l(struct trap_frame *, uint, uint, uint, uint, uint);
int	fpu_int_w(struct trap_frame *, uint, uint, uint, uint, uint);
fpu_fn4	fpu_madd;
fpu_fn4	fpu_msub;
fpu_fn3	fpu_mov;
fpu_fn3	fpu_movcf;
fpu_fn3	fpu_movn;
fpu_fn3	fpu_movz;
fpu_fn3	fpu_mul;
fpu_fn3	fpu_neg;
fpu_fn4	fpu_nmadd;
fpu_fn4	fpu_nmsub;
fpu_fn3	fpu_recip;
fpu_fn3	fpu_round_l;
fpu_fn3	fpu_round_w;
fpu_fn3	fpu_rsqrt;
fpu_fn3	fpu_sqrt;
fpu_fn3	fpu_sub;
fpu_fn3	fpu_trunc_l;
fpu_fn3	fpu_trunc_w;

/*
 * Encoding of operand format within opcodes `fmt' and `fmt3' fields.
 */
#define	FMT_S	0x00
#define	FMT_D	0x01
#define	FMT_W	0x04
#define	FMT_L	0x05

/*
 * Inlines from softfloat-specialize.h which are not made public, needed
 * for fpu_abs.
 */
#define	float32_is_nan(a) \
	(0xff000000 < (a << 1))
#define	float32_is_signaling_nan(a) \
	((((a >> 22) & 0x1ff) == 0x1fe) && (a & 0x003fffff))

/*
 * Precomputed results of intXX_to_floatXX(1)
 */
#define	ONE_F32	(float32)(SNG_EXP_BIAS << SNG_FRACBITS)
#define	ONE_F64	(float64)((uint64_t)DBL_EXP_BIAS << DBL_FRACBITS)

/*
 * Handle a floating-point exception.
 */
void
MipsFPTrap(struct trap_frame *tf)
{
	struct cpu_info *ci = curcpu();
	struct proc *p = ci->ci_curproc;
	union sigval sv;
	vaddr_t pc;
	uint32_t fsr, excbits;
	uint32_t insn;
	InstFmt inst;
	int sig = 0;
	int fault_type = SI_NOINFO;
	int update_pcb = 0;
	int emulate = 0;
#ifdef FPUEMUL
	int skip_insn = 1;
#else
	register_t sr;
#endif

	KDASSERT(tf == p->p_md.md_regs);

#ifndef FPUEMUL
	/*
	 * Enable FPU, and read its status register.
	 */

	sr = getsr();
	setsr(sr | SR_COP_1_BIT);

	__asm__ volatile ("cfc1 %0, $31" : "=r" (fsr));
	__asm__ volatile ("cfc1 %0, $31" : "=r" (fsr));

	/*
	 * If this is not an unimplemented operation, but a genuine
	 * FPU exception, signal the process.
	 */

	if ((fsr & FPCSR_C_E) == 0) {
		sig = SIGFPE;
		goto deliver;
	}
#else
#ifdef CPU_OCTEON
	/*
	 * SR_FR_32 is hardwired to zero on Octeon; make sure it is
	 * set in the emulation view of the FPU state.
	 */
	tf->sr |= SR_FR_32;
#endif
#endif	/* FPUEMUL */

	/*
	 * Get the faulting instruction.  This should not fail, and
	 * if it does, it's probably not your lucky day.
	 */

	pc = (vaddr_t)tf->pc;
	if (tf->cause & CR_BR_DELAY)
		pc += 4;
	if (copyin((void *)pc, &insn, sizeof insn) != 0) {
		sig = SIGBUS;
		fault_type = BUS_OBJERR;
		goto deliver;
	}
	inst = *(InstFmt *)&insn;

	/*
	 * Emulate the instruction.
	 */

#ifdef DEBUG
#ifdef DDB
	printf("%s: unimplemented FPU completion, fsr 0x%08x\n0x%lx: ",
	    p->p_comm, fsr, pc);
	dbmd_print_insn(insn, pc, printf);
#else
	printf("%s: unimplemented FPU completion, insn 0x%08x fsr 0x%08x\n",
	    p->p_comm, insn, fsr);
#endif
#endif

	switch (inst.FRType.op) {
	default:
		/*
		 * Not a FPU instruction.
		 */
		break;
#ifdef FPUEMUL
	case OP_SPECIAL:
		switch (inst.FRType.func) {
		default:
			/*
			 * Not a FPU instruction.
			 */
			break;
		case OP_MOVCI:
			/*
			 * This instruction should not require emulation,
			 * unless there is no FPU.
			 */
			emulate = 1;
			break;
		}
		break;
	case OP_LDC1:
	case OP_LWC1:
	case OP_SDC1:
	case OP_SWC1:
		/*
		 * These instructions should not require emulation,
		 * unless there is no FPU.
		 */
		emulate = 1;
		break;
#endif
	case OP_COP1:
		switch (inst.RType.rs) {
		case OP_BC:
#ifdef FPUEMUL
			skip_insn = 0;
#endif
			/* FALLTHROUGH */
		case OP_MF:
		case OP_DMF:
		case OP_CF:
		case OP_MT:
		case OP_DMT:
		case OP_CT:
			/*
			 * These instructions should not require emulation,
			 * unless there is no FPU.
			 */
#ifdef FPUEMUL
			emulate = 1;
#endif
			break;
		default:
			emulate = 1;
			break;
		}
		break;
	case OP_COP1X:
		switch (inst.FQType.op4) {
		default:
			switch (inst.FRType.func) {
#ifdef FPUEMUL
			case OP_LDXC1:
			case OP_LWXC1:
			case OP_SDXC1:
			case OP_SWXC1:
			case OP_PREFX:
				/*
				 * These instructions should not require
				 * emulation, unless there is no FPU.
				 */
				emulate = 1;
				break;
#endif
			default:
				/*
				 * Not a valid instruction.
				 */
				break;
			}
			break;
		case OP_MADD:
		case OP_MSUB:
		case OP_NMADD:
		case OP_NMSUB:
			emulate = 1;
			break;
		}
		break;
	}

	if (emulate) {
#ifndef FPUEMUL
		KASSERT(p == ci->ci_fpuproc);
		save_fpu();
#endif
		update_pcb = 1;

		sig = fpu_emulate(tf, insn, &sv);
		/* reload fsr, possibly modified by softfloat code */
		fsr = tf->fsr;
		if (sig == 0) {
			/* raise SIGFPE if necessary */
			excbits = (fsr & FPCSR_C_MASK) >> FPCSR_C_SHIFT;
			excbits &= (fsr & FPCSR_E_MASK) >> FPCSR_E_SHIFT;
			if (excbits != 0)
				sig = SIGFPE;
		}
	} else {
		sig = SIGILL;
		fault_type = ILL_ILLOPC;
	}

deliver:
	switch (sig) {
	case SIGFPE:
		excbits = (fsr & FPCSR_C_MASK) >> FPCSR_C_SHIFT;
		excbits &= (fsr & FPCSR_E_MASK) >> FPCSR_E_SHIFT;
		if (excbits & FP_X_INV)
			fault_type = FPE_FLTINV;
		else if (excbits & FP_X_DZ)
			fault_type = FPE_INTDIV;
		else if (excbits & FP_X_OFL)
			fault_type = FPE_FLTUND;
		else if (excbits & FP_X_UFL)
			fault_type = FPE_FLTOVF;
		else /* if (excbits & FP_X_IMP) */
			fault_type = FPE_FLTRES;

		break;
#ifdef FPUEMUL
	case SIGBUS:
		fault_type = BUS_ADRALN;
		break;
	case SIGSEGV:
		fault_type = SEGV_MAPERR;
		break;
#endif
	}

	/*
	 * Skip the instruction, unless we are delivering SIGILL.
	 */
#ifdef FPUEMUL
	if (skip_insn) {
#endif
		if (sig != SIGILL) {
			if (tf->cause & CR_BR_DELAY) {
				/*
				 * Note that it doesn't matter, at this point,
				 * that we pass the updated FSR value, as it is
				 * only used to decide whether to branch or not
				 * if the faulting instruction was BC1[FT].
				 */
				tf->pc = MipsEmulateBranch(tf, tf->pc, fsr, 0);
			} else
				tf->pc += 4;
		}
#ifdef FPUEMUL
	}
#endif

	/*
	 * Update the FPU status register.
	 * We need to make sure that this will not cause an exception
	 * in kernel mode.
	 */

	/* propagate raised exceptions to the sticky bits */
	fsr &= ~FPCSR_C_E;
	excbits = (fsr & FPCSR_C_MASK) >> FPCSR_C_SHIFT;
	fsr |= excbits << FPCSR_F_SHIFT;
	/* clear all exception sources */
	fsr &= ~FPCSR_C_MASK;
	if (update_pcb)
		tf->fsr = fsr;
#ifndef FPUEMUL
	__asm__ volatile ("ctc1 %0, $31" :: "r" (fsr));
	/* disable fpu before returning to trap() */
	setsr(sr);
#endif

	if (sig != 0) {
#ifdef FPUEMUL
		if (sig != SIGBUS && sig != SIGSEGV)
#endif
			sv.sival_ptr = (void *)pc;
		KERNEL_LOCK();
		trapsignal(p, sig, 0, fault_type, sv);
		KERNEL_UNLOCK();
	}
}

/*
 * Emulate an FPU instruction.  The FPU register set has been saved in the
 * current PCB, and is pointed to by the trap frame.
 */
int
fpu_emulate(struct trap_frame *tf, uint32_t insn, union sigval *sv)
{
	InstFmt inst;

	tf->zero = 0;	/* not written by trap code */

	inst = *(InstFmt *)&insn;
	switch (inst.FRType.op) {
	default:
		break;
#ifdef FPUEMUL
	case OP_SPECIAL:
		return nofpu_emulate_movci(tf, insn);
	case OP_LDC1:
	case OP_LWC1:
	case OP_SDC1:
	case OP_SWC1:
		return nofpu_emulate_loadstore(tf, insn, sv);
#endif
	case OP_COP1:
		switch (inst.RType.rs) {
#ifdef FPUEMUL
		case OP_MF:
		case OP_DMF:
		case OP_CF:
		case OP_MT:
		case OP_DMT:
		case OP_CT:
		case OP_BC:
			return nofpu_emulate_cop1(tf, insn, sv);
#endif
		default:
			return fpu_emulate_cop1(tf, insn);
		}
		break;
	case OP_COP1X:
		switch (inst.FQType.op4) {
#ifdef FPUEMUL
		default:
			switch (inst.FRType.func) {
			case OP_LDXC1:
			case OP_LWXC1:
			case OP_SDXC1:
			case OP_SWXC1:
			case OP_PREFX:
				return nofpu_emulate_cop1x(tf, insn, sv);
			default:
				break;
			}
			break;
		case OP_MADD:
		case OP_MSUB:
		case OP_NMADD:
		case OP_NMSUB:
			return fpu_emulate_cop1x(tf, insn);
#else
		default:
			return fpu_emulate_cop1x(tf, insn);
#endif
		}
	}

	return SIGILL;
}

/*
 * Emulate a COP1 FPU instruction.
 */
int
fpu_emulate_cop1(struct trap_frame *tf, uint32_t insn)
{
	InstFmt inst;
	uint ft, fs, fd;
	fpu_fn3 *fpu_op;
	static fpu_fn3 *const fpu_ops1[1 << 6] = {
		fpu_add,		/* 0x00 */
		fpu_sub,
		fpu_mul,
		fpu_div,
		fpu_sqrt,
		fpu_abs,
		fpu_mov,
		fpu_neg,
		fpu_round_l,		/* 0x08 */
		fpu_trunc_l,
		fpu_ceil_l,
		fpu_floor_l,
		fpu_round_w,
		fpu_trunc_w,
		fpu_ceil_w,
		fpu_floor_w,
		NULL,			/* 0x10 */
		fpu_movcf,
		fpu_movz,
		fpu_movn,
		NULL,
		fpu_recip,
		fpu_rsqrt,
		NULL,
		NULL,			/* 0x18 */
		NULL,
		NULL,
		NULL,
		NULL,
		NULL,
		NULL,
		NULL,
		fpu_cvt_s,		/* 0x20 */
		fpu_cvt_d,
		NULL,
		NULL,
		fpu_cvt_w,
		fpu_cvt_l,
		NULL,
		NULL,
		NULL,			/* 0x28 */
		NULL,
		NULL,
		NULL,
		NULL,
		NULL,
		NULL,
		NULL,
		(fpu_fn3 *)fpu_c,	/* 0x30 */
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c,	/* 0x38 */
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c,
		(fpu_fn3 *)fpu_c
	};

	inst = *(InstFmt *)&insn;

	/*
	 * Check for valid function code.
	 */

	fpu_op = fpu_ops1[inst.FRType.func];
	if (fpu_op == NULL)
		return SIGILL;

	/*
	 * Check for valid format.  FRType assumes bit 25 is always set,
	 * so we need to check for it explicitely.
	 */

	if ((insn & (1 << 25)) == 0)
		return SIGILL;
	switch (inst.FRType.fmt) {
	default:
		return SIGILL;
	case FMT_S:
	case FMT_D:
	case FMT_W:
	case FMT_L:
		break;
	}

	/*
	 * Check for valid register values. Only even-numbered registers
	 * can be used if the FR bit is clear in coprocessor 0 status
	 * register.
	 *
	 * Note that c.cond does not specify a register number in the fd
	 * field, but the fd field must have zero in its low two bits, so
	 * the test will not reject valid c.cond instructions.
	 */

	ft = inst.FRType.ft;
	fs = inst.FRType.fs;
	fd = inst.FRType.fd;
	if ((tf->sr & SR_FR_32) == 0) {
		if ((ft | fs | fd) & 1)
			return SIGILL;
	}

	/*
	 * Finally dispatch to the proper routine.
	 */

	if (fpu_op == (fpu_fn3 *)&fpu_c)
		return fpu_c(tf, inst.FRType.fmt, ft, fs, fd, inst.FRType.func);
	else
		return (*fpu_op)(tf, inst.FRType.fmt, ft, fs, fd);
}

/*
 * Emulate a COP1X FPU instruction.
 */
int
fpu_emulate_cop1x(struct trap_frame *tf, uint32_t insn)
{
	InstFmt inst;
	uint fr, ft, fs, fd;
	fpu_fn4 *fpu_op;
	static fpu_fn4 *const fpu_ops1x[1 << 3] = {
		NULL,
		NULL,
		NULL,
		NULL,
		fpu_madd,
		fpu_msub,
		fpu_nmadd,
		fpu_nmsub
	};

	inst = *(InstFmt *)&insn;

	/*
	 * Check for valid function code.
	 */

	fpu_op = fpu_ops1x[inst.FQType.op4];
	if (fpu_op == NULL)
		return SIGILL;

	/*
	 * Check for valid format.
	 */

	switch (inst.FQType.fmt3) {
	default:
		return SIGILL;
	case FMT_S:
	case FMT_D:
	case FMT_W:
	case FMT_L:
		break;
	}

	/*
	 * Check for valid register values. Only even-numbered registers
	 * can be used if the FR bit is clear in coprocessor 0 status
	 * register.
	 */

	fr = inst.FQType.fr;
	ft = inst.FQType.ft;
	fs = inst.FQType.fs;
	fd = inst.FQType.fd;
	if ((tf->sr & SR_FR_32) == 0) {
		if ((fr | ft | fs | fd) & 1)
			return SIGILL;
	}

	/*
	 * Finally dispatch to the proper routine.
	 */

	return (*fpu_op)(tf, inst.FRType.fmt, fr, ft, fs, fd);
}

/*
 * Load a floating-point argument according to the specified format.
 */
uint64_t
fpu_load(struct trap_frame *tf, uint fmt, uint regno)
{
	register_t *regs = (register_t *)tf;
	uint64_t tmp, tmp2;

	tmp = (uint64_t)regs[FPBASE + regno];
	if (tf->sr & SR_FR_32) {
		switch (fmt) {
		case FMT_D:
		case FMT_L:
			break;
		case FMT_S:
		case FMT_W:
			tmp &= 0xffffffff;
			break;
		}
	} else {
		tmp &= 0xffffffff;
		switch (fmt) {
		case FMT_D:
		case FMT_L:
			/* caller has enforced regno is even */
			tmp2 = (uint64_t)regs[FPBASE + regno + 1];
			tmp |= tmp2 << 32;
			break;
		case FMT_S:
		case FMT_W:
			break;
		}
	}

	return tmp;
}

/*
 * Store a floating-point result according to the specified format.
 */
void
fpu_store(struct trap_frame *tf, uint fmt, uint regno, uint64_t rslt)
{
	register_t *regs = (register_t *)tf;

	if (tf->sr & SR_FR_32) {
		regs[FPBASE + regno] = rslt;
	} else {
		/* caller has enforced regno is even */
		regs[FPBASE + regno] = rslt & 0xffffffff;
		regs[FPBASE + regno + 1] = (rslt >> 32) & 0xffffffff;
	}
}

/*
 * Integer conversion
 */

int
fpu_int_l(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd, uint rm)
{
	uint64_t raw;
	uint32_t oldrm;

	if (ft != 0)
		return SIGILL;
	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw = fpu_load(tf, fmt, fs);

	/* round towards required mode */
	oldrm = tf->fsr & FPCSR_RM_MASK;
	tf->fsr = (tf->fsr & ~FPCSR_RM_MASK) | rm;
	if (fmt == FMT_S)
		raw = float32_to_int64((float32)raw);
	else
		raw = float64_to_int64((float64)raw);
	/* restore rounding mode */
	tf->fsr = (tf->fsr & ~FPCSR_RM_MASK) | oldrm;

	if ((tf->fsr & (FPCSR_C_V | FPCSR_E_V)) != (FPCSR_C_V | FPCSR_E_V))
		fpu_store(tf, fmt, fd, raw);

	return 0;
}

int
fpu_int_w(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd, uint rm)
{
	uint64_t raw;
	uint32_t oldrm;

	if (ft != 0)
		return SIGILL;
	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw = fpu_load(tf, fmt, fs);

	/* round towards required mode */
	oldrm = tf->fsr & FPCSR_RM_MASK;
	tf->fsr = (tf->fsr & ~FPCSR_RM_MASK) | rm;
	if (fmt == FMT_S)
		raw = float32_to_int32((float32)raw);
	else
		raw = float64_to_int32((float64)raw);
	/* restore rounding mode */
	tf->fsr = (tf->fsr & ~FPCSR_RM_MASK) | oldrm;

	if ((tf->fsr & (FPCSR_C_V | FPCSR_E_V)) != (FPCSR_C_V | FPCSR_E_V))
		fpu_store(tf, fmt, fd, raw);

	return 0;
}

/*
 * FPU Instruction emulation
 */

int
fpu_abs(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw;

	if (ft != 0)
		return SIGILL;
	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw = fpu_load(tf, fmt, fs);
	/* clear sign bit unless NaN */
	if (fmt == FMT_S) {
		float32 f32 = (float32)raw;
		if (float32_is_nan(f32)) {
			float_set_invalid();
		} else {
			f32 &= ~(1L << 31);
			raw = (uint64_t)f32;
		}
	} else {
		float64 f64 = (float64)raw;
		if (float64_is_nan(f64)) {
			float_set_invalid();
		} else {
			f64 &= ~(1L << 63);
			raw = (uint64_t)f64;
		}
	}
	fpu_store(tf, fmt, fd, raw);

	return 0;
}

int
fpu_add(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw1, raw2, rslt;

	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw1 = fpu_load(tf, fmt, fs);
	raw2 = fpu_load(tf, fmt, ft);
	if (fmt == FMT_S) {
		float32 f32 = float32_add((float32)raw1, (float32)raw2);
		rslt = (uint64_t)f32;
	} else {
		float64 f64 = float64_add((float64)raw1, (float64)raw2);
		rslt = (uint64_t)f64;
	}
	fpu_store(tf, fmt, fd, rslt);

	return 0;
}

int
fpu_c(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd, uint op)
{
	uint64_t raw1, raw2;
	uint cc, lt, eq, uo;

	if ((fd & 0x03) != 0)
		return SIGILL;
	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	lt = eq = uo = 0;
	cc = fd >> 2;

	raw1 = fpu_load(tf, fmt, fs);
	raw2 = fpu_load(tf, fmt, ft);

	if (fmt == FMT_S) {
		float32 f32a = (float32)raw1;
		float32 f32b = (float32)raw2;
		if (float32_is_nan(f32a)) {
			uo = 1 << 0;
			if (float32_is_signaling_nan(f32a))
				op |= 0x08;	/* force invalid exception */
		}
		if (float32_is_nan(f32b)) {
			uo = 1 << 0;
			if (float32_is_signaling_nan(f32b))
				op |= 0x08;	/* force invalid exception */
		}
		if (uo == 0) {
			if (float32_eq(f32a, f32b))
				eq = 1 << 1;
			else if (float32_lt(f32a, f32b))
				lt = 1 << 2;
		}
	} else {
		float64 f64a = (float64)raw1;
		float64 f64b = (float64)raw2;
		if (float64_is_nan(f64a)) {
			uo = 1 << 0;
			if (float64_is_signaling_nan(f64a))
				op |= 0x08;	/* force invalid exception */
		}
		if (float64_is_nan(f64b)) {
			uo = 1 << 0;
			if (float64_is_signaling_nan(f64b))
				op |= 0x08;	/* force invalid exception */
		}
		if (uo == 0) {
			if (float64_eq(f64a, f64b))
				eq = 1 << 1;
			else if (float64_lt(f64a, f64b))
				lt = 1 << 2;
		}
	}

	if (uo && (op & 0x08)) {
		float_set_invalid();
		if (tf->fsr & FPCSR_E_V) {
			/* comparison result intentionaly not written */
			goto skip;
		}
	} else {
		if ((uo | eq | lt) & op)
			tf->fsr |= FPCSR_CONDVAL(cc);
		else
			tf->fsr &= ~FPCSR_CONDVAL(cc);
	}
skip:

	return 0;
}

int
fpu_ceil_l(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	/* round towards positive infinity */
	return fpu_int_l(tf, fmt, ft, fs, fd, FP_RP);
}

int
fpu_ceil_w(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	/* round towards positive infinity */
	return fpu_int_w(tf, fmt, ft, fs, fd, FP_RP);
}

int
fpu_cvt_d(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw;

	if (ft != 0)
		return SIGILL;
	if (fmt == FMT_D)
		return SIGILL;

	raw = fpu_load(tf, fmt, fs);
	switch (fmt) {
	case FMT_L:
		raw = int64_to_float64((int64_t)raw);
		break;
	case FMT_S:
		raw = float32_to_float64((float32)raw);
		break;
	case FMT_W:
		raw = int32_to_float64((int32_t)raw);
		break;
	}
	fpu_store(tf, fmt, fd, raw);

	return 0;
}

int
fpu_cvt_l(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw;
	uint32_t rm;

	if (ft != 0)
		return SIGILL;
	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	rm = tf->fsr & FPCSR_RM_MASK;
	raw = fpu_load(tf, fmt, fs);
	if (fmt == FMT_D) {
		if (rm == FP_RZ)
			raw = float64_to_int64_round_to_zero((float64)raw);
		else
			raw = float64_to_int64((float64)raw);
	} else {
		if (rm == FP_RZ)
			raw = float32_to_int64_round_to_zero((float32)raw);
		else
			raw = float32_to_int64((float32)raw);
	}
	if ((tf->fsr & (FPCSR_C_V | FPCSR_E_V)) != (FPCSR_C_V | FPCSR_E_V))
		fpu_store(tf, fmt, fd, raw);

	return 0;
}

int
fpu_cvt_s(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw;

	if (ft != 0)
		return SIGILL;
	if (fmt == FMT_S)
		return SIGILL;

	raw = fpu_load(tf, fmt, fs);
	switch (fmt) {
	case FMT_D:
		raw = float64_to_float32((float64)raw);
		break;
	case FMT_L:
		raw = int64_to_float32((int64_t)raw);
		break;
	case FMT_W:
		raw = int32_to_float32((int32_t)raw);
		break;
	}
	fpu_store(tf, fmt, fd, raw);

	return 0;
}

int
fpu_cvt_w(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw;
	uint32_t rm;

	if (ft != 0)
		return SIGILL;
	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	rm = tf->fsr & FPCSR_RM_MASK;
	raw = fpu_load(tf, fmt, fs);
	if (fmt == FMT_D) {
		if (rm == FP_RZ)
			raw = float64_to_int32_round_to_zero((float64)raw);
		else
			raw = float64_to_int32((float64)raw);
	} else {
		if (rm == FP_RZ)
			raw = float32_to_int32_round_to_zero((float32)raw);
		else
			raw = float32_to_int32((float32)raw);
	}
	if ((tf->fsr & (FPCSR_C_V | FPCSR_E_V)) != (FPCSR_C_V | FPCSR_E_V))
		fpu_store(tf, fmt, fd, raw);

	return 0;
}

int
fpu_div(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw1, raw2, rslt;

	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw1 = fpu_load(tf, fmt, fs);
	raw2 = fpu_load(tf, fmt, ft);
	if (fmt == FMT_S) {
		float32 f32 = float32_div((float32)raw1, (float32)raw2);
		rslt = (uint64_t)f32;
	} else {
		float64 f64 = float64_div((float64)raw1, (float64)raw2);
		rslt = (uint64_t)f64;
	}
	fpu_store(tf, fmt, fd, rslt);

	return 0;
}

int
fpu_floor_l(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	/* round towards negative infinity */
	return fpu_int_l(tf, fmt, ft, fs, fd, FP_RM);
}

int
fpu_floor_w(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	/* round towards negative infinity */
	return fpu_int_w(tf, fmt, ft, fs, fd, FP_RM);
}

int
fpu_madd(struct trap_frame *tf, uint fmt, uint fr, uint ft, uint fs, uint fd)
{
	uint64_t raw1, raw2, raw3, rslt;

	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw1 = fpu_load(tf, fmt, fs);
	raw2 = fpu_load(tf, fmt, ft);
	raw3 = fpu_load(tf, fmt, fr);
	if (fmt == FMT_S) {
		float32 f32 = float32_add(
		    float32_mul((float32)raw1, (float32)raw2),
		    (float32)raw3);
		rslt = (uint64_t)f32;
	} else {
		float64 f64 = float64_add(
		    float64_mul((float64)raw1, (float64)raw2),
		    (float64)raw3);
		rslt = (uint64_t)f64;
	}
	fpu_store(tf, fmt, fd, rslt);

	return 0;
}

int
fpu_mov(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw;

	if (ft != 0)
		return SIGILL;
	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw = fpu_load(tf, fmt, fs);
	fpu_store(tf, fmt, fd, raw);

	return 0;
}

int
fpu_movcf(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw;
	uint cc, istf;
	int condition;

	if ((ft & 0x02) != 0)
		return SIGILL;
	cc = ft >> 2;
	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	condition = tf->fsr & FPCSR_CONDVAL(cc);
	istf = ft & COPz_BC_TF_MASK;
	if ((!condition && !istf) /*movf*/ || (condition && istf) /*movt*/) {
		raw = fpu_load(tf, fmt, fs);
		fpu_store(tf, fmt, fd, raw);
	}

	return 0;
}

int
fpu_movn(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	register_t *regs = (register_t *)tf;
	uint64_t raw;

	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	if (ft != ZERO && regs[ft] != 0) {
		raw = fpu_load(tf, fmt, fs);
		fpu_store(tf, fmt, fd, raw);
	}

	return 0;
}

int
fpu_movz(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	register_t *regs = (register_t *)tf;
	uint64_t raw;

	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	if (ft == ZERO || regs[ft] == 0) {
		raw = fpu_load(tf, fmt, fs);
		fpu_store(tf, fmt, fd, raw);
	}

	return 0;
}

int
fpu_msub(struct trap_frame *tf, uint fmt, uint fr, uint ft, uint fs, uint fd)
{
	uint64_t raw1, raw2, raw3, rslt;

	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw1 = fpu_load(tf, fmt, fs);
	raw2 = fpu_load(tf, fmt, ft);
	raw3 = fpu_load(tf, fmt, fr);
	if (fmt == FMT_S) {
		float32 f32 = float32_sub(
		    float32_mul((float32)raw1, (float32)raw2),
		    (float32)raw3);
		rslt = (uint64_t)f32;
	} else {
		float64 f64 = float64_sub(
		    float64_mul((float64)raw1, (float64)raw2),
		    (float64)raw3);
		rslt = (uint64_t)f64;
	}
	fpu_store(tf, fmt, fd, rslt);

	return 0;
}

int
fpu_mul(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw1, raw2, rslt;

	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw1 = fpu_load(tf, fmt, fs);
	raw2 = fpu_load(tf, fmt, ft);
	if (fmt == FMT_S) {
		float32 f32 = float32_mul((float32)raw1, (float32)raw2);
		rslt = (uint64_t)f32;
	} else {
		float64 f64 = float64_mul((float64)raw1, (float64)raw2);
		rslt = (uint64_t)f64;
	}
	fpu_store(tf, fmt, fd, rslt);

	return 0;
}

int
fpu_neg(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw;

	if (ft != 0)
		return SIGILL;
	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw = fpu_load(tf, fmt, fs);
	/* flip sign bit unless NaN */
	if (fmt == FMT_S) {
		float32 f32 = (float32)raw;
		if (float32_is_nan(f32)) {
			float_set_invalid();
		} else {
			f32 ^= 1L << 31;
			raw = (uint64_t)f32;
		}
	} else {
		float64 f64 = (float64)raw;
		if (float64_is_nan(f64)) {
			float_set_invalid();
		} else {
			f64 ^= 1L << 63;
			raw = (uint64_t)f64;
		}
	}
	fpu_store(tf, fmt, fd, raw);

	return 0;
}

int
fpu_nmadd(struct trap_frame *tf, uint fmt, uint fr, uint ft, uint fs, uint fd)
{
	uint64_t raw1, raw2, raw3, rslt;

	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw1 = fpu_load(tf, fmt, fs);
	raw2 = fpu_load(tf, fmt, ft);
	raw3 = fpu_load(tf, fmt, fr);
	if (fmt == FMT_S) {
		float32 f32 = float32_add(
		    float32_mul((float32)raw1, (float32)raw2),
		    (float32)raw3);
		if (float32_is_nan(f32))
			float_set_invalid();
		else
			f32 ^= 1L << 31;
		rslt = (uint64_t)f32;
	} else {
		float64 f64 = float64_add(
		    float64_mul((float64)raw1, (float64)raw2),
		    (float64)raw3);
		if (float64_is_nan(f64))
			float_set_invalid();
		else
			f64 ^= 1L << 63;
		rslt = (uint64_t)f64;
	}
	fpu_store(tf, fmt, fd, rslt);

	return 0;
}

int
fpu_nmsub(struct trap_frame *tf, uint fmt, uint fr, uint ft, uint fs, uint fd)
{
	uint64_t raw1, raw2, raw3, rslt;

	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw1 = fpu_load(tf, fmt, fs);
	raw2 = fpu_load(tf, fmt, ft);
	raw3 = fpu_load(tf, fmt, fr);
	if (fmt == FMT_S) {
		float32 f32 = float32_sub(
		    float32_mul((float32)raw1, (float32)raw2),
		    (float32)raw3);
		if (float32_is_nan(f32))
			float_set_invalid();
		else
			f32 ^= 1L << 31;
		rslt = (uint64_t)f32;
	} else {
		float64 f64 = float64_sub(
		    float64_mul((float64)raw1, (float64)raw2),
		    (float64)raw3);
		if (float64_is_nan(f64))
			float_set_invalid();
		else
			f64 ^= 1L << 63;
		rslt = (uint64_t)f64;
	}
	fpu_store(tf, fmt, fd, rslt);

	return 0;
}

int
fpu_recip(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw;

	if (ft != 0)
		return SIGILL;
	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw = fpu_load(tf, fmt, fs);
	if (fmt == FMT_S) {
		float32 f32 = float32_div(ONE_F32, (float32)raw);
		raw = (uint64_t)f32;
	} else {
		float64 f64 = float64_div(ONE_F64, (float64)raw);
		raw = (uint64_t)f64;
	}
	fpu_store(tf, fmt, fd, raw);

	return 0;
}

int
fpu_round_l(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	/* round towards nearest */
	return fpu_int_l(tf, fmt, ft, fs, fd, FP_RN);
}

int
fpu_round_w(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	/* round towards nearest */
	return fpu_int_w(tf, fmt, ft, fs, fd, FP_RN);
}

int
fpu_rsqrt(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw;

	if (ft != 0)
		return SIGILL;
	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw = fpu_load(tf, fmt, fs);
	if (fmt == FMT_S) {
		float32 f32 = float32_sqrt((float32)raw);
		if ((tf->fsr & (FPCSR_C_V | FPCSR_E_V)) !=
		    (FPCSR_C_V | FPCSR_E_V))
			f32 = float32_div(ONE_F32, f32);
		raw = (uint64_t)f32;
	} else {
		float64 f64 = float64_sqrt((float64)raw);
		if ((tf->fsr & (FPCSR_C_V | FPCSR_E_V)) !=
		    (FPCSR_C_V | FPCSR_E_V))
			f64 = float64_div(ONE_F64, f64);
		raw = (uint64_t)f64;
	}
	fpu_store(tf, fmt, fd, raw);

	return 0;
}

int
fpu_sqrt(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw;

	if (ft != 0)
		return SIGILL;
	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw = fpu_load(tf, fmt, fs);
	if (fmt == FMT_S) {
		float32 f32 = float32_sqrt((float32)raw);
		raw = (uint64_t)f32;
	} else {
		float64 f64 = float64_sqrt((float64)raw);
		raw = (uint64_t)f64;
	}
	fpu_store(tf, fmt, fd, raw);

	return 0;
}

int
fpu_sub(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	uint64_t raw1, raw2, rslt;

	if (fmt != FMT_S && fmt != FMT_D)
		return SIGILL;

	raw1 = fpu_load(tf, fmt, fs);
	raw2 = fpu_load(tf, fmt, ft);
	if (fmt == FMT_S) {
		float32 f32 = float32_sub((float32)raw1, (float32)raw2);
		rslt = (uint64_t)f32;
	} else {
		float64 f64 = float64_sub((float64)raw1, (float64)raw2);
		rslt = (uint64_t)f64;
	}
	fpu_store(tf, fmt, fd, rslt);

	return 0;
}

int
fpu_trunc_l(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	/* round towards zero */
	return fpu_int_l(tf, fmt, ft, fs, fd, FP_RZ);
}

int
fpu_trunc_w(struct trap_frame *tf, uint fmt, uint ft, uint fs, uint fd)
{
	/* round towards zero */
	return fpu_int_w(tf, fmt, ft, fs, fd, FP_RZ);
}

#ifdef FPUEMUL

/*
 * Emulate a COP1 non-FPU instruction.
 */
int
nofpu_emulate_cop1(struct trap_frame *tf, uint32_t insn, union sigval *sv)
{
	register_t *regs = (register_t *)tf;
	InstFmt inst;
	int32_t cval;

	inst = *(InstFmt *)&insn;

	switch (inst.RType.rs) {
	case OP_MF:
		if (inst.FRType.fd != 0 || inst.FRType.func != 0)
			return SIGILL;
		if (inst.FRType.ft != ZERO)
			regs[inst.FRType.ft] =
			    (int32_t)regs[FPBASE + inst.FRType.fs];
		break;
	case OP_DMF:
		if (inst.FRType.fd != 0 || inst.FRType.func != 0)
			return SIGILL;
		if ((tf->sr & SR_FR_32) != 0 || (inst.FRType.fs & 1) == 0) {
			if (inst.FRType.ft != ZERO)
				regs[inst.FRType.ft] =
				    fpu_load(tf, FMT_L, inst.FRType.fs);
		}
		break;
	case OP_CF:
		if (inst.FRType.fd != 0 || inst.FRType.func != 0)
			return SIGILL;
		if (inst.FRType.ft != ZERO) {
			switch (inst.FRType.fs) {
			case 0:	/* FPC_ID */
				cval = MIPS_SOFT << 8;
				break;
			case 31: /* FPC_CSR */
				cval = (int32_t)tf->fsr;
				break;
			default:
				cval = 0;
				break;
			}
			regs[inst.FRType.ft] = (int64_t)cval;
		}
		break;
	case OP_MT:
		if (inst.FRType.fd != 0 || inst.FRType.func != 0)
			return SIGILL;
		regs[FPBASE + inst.FRType.fs] = (int32_t)regs[inst.FRType.ft];
		break;
	case OP_DMT:
		if (inst.FRType.fd != 0 || inst.FRType.func != 0)
			return SIGILL;
		if ((tf->sr & SR_FR_32) != 0 || (inst.FRType.fs & 1) == 0) {
			fpu_store(tf, FMT_L, inst.FRType.fs,
			    regs[inst.FRType.ft]);
		}
		break;
	case OP_CT:
		if (inst.FRType.fd != 0 || inst.FRType.func != 0)
			return SIGILL;
		cval = (int32_t)regs[inst.FRType.ft];
		switch (inst.FRType.fs) {
		case 31: /* FPC_CSR */
			cval &= ~FPCSR_C_E;
			tf->fsr = cval;
			break;
		case 0:	/* FPC_ID */
		default:
			break;
		}
		break;
	case OP_BC:
	   {
		uint cc, nd, istf;
		int condition;
		vaddr_t dest;
		uint32_t dinsn;

		cc = (inst.RType.rt & COPz_BC_CC_MASK) >> COPz_BC_CC_SHIFT;
		nd = inst.RType.rt & COPz_BCL_TF_MASK;
		istf = inst.RType.rt & COPz_BC_TF_MASK;
		condition = tf->fsr & FPCSR_CONDVAL(cc);
		if ((!condition && !istf) /*bc1f*/ ||
		    (condition && istf) /*bc1t*/) {
			/*
			 * Branch taken: if the delay slot is not a nop,
			 * copy the delay slot instruction to the dedicated
			 * relocation page, in order to be able to have the
			 * cpu process it and give control back to the
			 * kernel, for us to redirect to the branch
			 * destination.
			 */
			/* inline MipsEmulateBranch(tf, tf->pc, tf->fsr, insn)*/
			dest = tf->pc + 4 + ((short)inst.IType.imm << 2);
			if (copyin((const void *)(tf->pc + 4), &dinsn,
			    sizeof dinsn)) {
				sv->sival_ptr = (void *)(tf->pc + 4);
				return SIGSEGV;
			}
			if (dinsn == 0x00000000 /* nop */ ||
			    dinsn == 0x00000040 /* ssnop */) {
				tf->pc = dest;
			} else {
				if (fpe_branch_emulate(curproc, tf, dinsn,
				    dest) != 0)
					return SIGILL;
			}
		} else {
			/*
			 * Branch not taken: skip the instruction, and
			 * skip the delay slot if it was a `branch likely'
			 * instruction.
			 */
			tf->pc += 4;
			if (nd)
				tf->pc += 4;
		}
	    }
		break;
	}

	return 0;
}

/*
 * Emulate a COP1X non-FPU instruction.
 */
int
nofpu_emulate_cop1x(struct trap_frame *tf, uint32_t insn, union sigval *sv)
{
	register_t *regs = (register_t *)tf;
	InstFmt inst;
	vaddr_t va;
	uint64_t ddata;
	uint32_t wdata;

	inst = *(InstFmt *)&insn;
	switch (inst.FRType.func) {
	case OP_LDXC1:
		if (inst.FQType.fs != 0)
			return SIGILL;
		va = (vaddr_t)regs[inst.FQType.fr] +
		    (vaddr_t)regs[inst.FQType.ft];
		if ((va & 0x07) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGBUS;
		}
		if (copyin((const void *)va, &ddata, sizeof ddata) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGSEGV;
		}
		if ((tf->sr & SR_FR_32) != 0 || (inst.FQType.fd & 1) == 0)
			fpu_store(tf, FMT_L, inst.FQType.fd, ddata);
		break;
	case OP_LWXC1:
		if (inst.FQType.fs != 0)
			return SIGILL;
		va = (vaddr_t)regs[inst.FQType.fr] +
		    (vaddr_t)regs[inst.FQType.ft];
		if ((va & 0x03) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGBUS;
		}
		if (copyin((const void *)va, &wdata, sizeof wdata) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGSEGV;
		}
		regs[FPBASE + inst.FQType.fd] = wdata;
		break;
	case OP_SDXC1:
		if (inst.FQType.fd != 0)
			return SIGILL;
		va = (vaddr_t)regs[inst.FQType.fr] +
		    (vaddr_t)regs[inst.FQType.ft];
		if ((va & 0x07) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGBUS;
		}
		if ((tf->sr & SR_FR_32) != 0 || (inst.FQType.fs & 1) == 0)
			ddata = fpu_load(tf, FMT_L, inst.FQType.fs);
		else {
			/* undefined behaviour, don't expose stack content */
			ddata = 0;
		}
		if (copyout(&ddata, (void *)va, sizeof ddata) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGSEGV;
		}
		break;
	case OP_SWXC1:
		if (inst.FQType.fd != 0)
			return SIGILL;
		va = (vaddr_t)regs[inst.FQType.fr] +
		    (vaddr_t)regs[inst.FQType.ft];
		if ((va & 0x03) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGBUS;
		}
		wdata = regs[FPBASE + inst.FQType.fs];
		if (copyout(&wdata, (void *)va, sizeof wdata) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGSEGV;
		}
		break;
	case OP_PREFX:
		/* nothing to do */
		break;
	}

	return 0;
}

/*
 * Emulate a load/store instruction on FPU registers.
 */
int
nofpu_emulate_loadstore(struct trap_frame *tf, uint32_t insn, union sigval *sv)
{
	register_t *regs = (register_t *)tf;
	InstFmt inst;
	vaddr_t va;
	uint64_t ddata;
	uint32_t wdata;

	inst = *(InstFmt *)&insn;
	switch (inst.IType.op) {
	case OP_LDC1:
		va = (vaddr_t)regs[inst.IType.rs] + (int16_t)inst.IType.imm;
		if ((va & 0x07) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGBUS;
		}
		if (copyin((const void *)va, &ddata, sizeof ddata) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGSEGV;
		}
		if ((tf->sr & SR_FR_32) != 0 || (inst.IType.rt & 1) == 0)
			fpu_store(tf, FMT_L, inst.IType.rt, ddata);
		break;
	case OP_LWC1:
		va = (vaddr_t)regs[inst.IType.rs] + (int16_t)inst.IType.imm;
		if ((va & 0x03) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGBUS;
		}
		if (copyin((const void *)va, &wdata, sizeof wdata) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGSEGV;
		}
		regs[FPBASE + inst.IType.rt] = wdata;
		break;
	case OP_SDC1:
		va = (vaddr_t)regs[inst.IType.rs] + (int16_t)inst.IType.imm;
		if ((va & 0x07) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGBUS;
		}
		if ((tf->sr & SR_FR_32) != 0 || (inst.IType.rt & 1) == 0)
			ddata = fpu_load(tf, FMT_L, inst.IType.rt);
		else {
			/* undefined behaviour, don't expose stack content */
			ddata = 0;
		}
		if (copyout(&ddata, (void *)va, sizeof ddata) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGSEGV;
		}
		break;
	case OP_SWC1:
		va = (vaddr_t)regs[inst.IType.rs] + (int16_t)inst.IType.imm;
		if ((va & 0x03) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGBUS;
		}
		wdata = regs[FPBASE + inst.IType.rt];
		if (copyout(&wdata, (void *)va, sizeof wdata) != 0) {
			sv->sival_ptr = (void *)va;
			return SIGSEGV;
		}
		break;
	}

	return 0;
}

/*
 * Emulate MOVF and MOVT.
 */
int
nofpu_emulate_movci(struct trap_frame *tf, uint32_t insn)
{
	register_t *regs = (register_t *)tf;
	InstFmt inst;
	uint cc, istf;
	int condition;

	inst = *(InstFmt *)&insn;
	if ((inst.RType.rt & 0x02) != 0 || inst.RType.shamt != 0)
		return SIGILL;

	cc = inst.RType.rt >> 2;
	istf = inst.RType.rt & COPz_BC_TF_MASK;
	condition = tf->fsr & FPCSR_CONDVAL(cc);
	if ((!condition && !istf) /*movf*/ || (condition && istf) /*movt*/) {
		if (inst.RType.rd != ZERO)
			regs[inst.RType.rd] = regs[inst.RType.rs];
	}

	return 0;
}

#endif	/* FPUEMUL */


code:
#	$OpenBSD: genassym.cf,v 1.12 2014/07/09 08:49:20 deraadt Exp $
#
# Copyright (c) 1997 Per Fogelstrom / Opsycon AB
#
# 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/buf.h>
include <sys/proc.h>
include <sys/mbuf.h>
include <sys/user.h>
include <uvm/uvm_extern.h>
include <sys/siginfo.h>

include <machine/cpu.h>

export	SONPROC

struct	proc
member	p_addr
member	p_cpu
member	p_stat
member	p_vmspace
member	P_ASTPENDING	p_md.md_astpending
member	P_PC_CTRL	p_md.md_pc_ctrl
member	P_PC_COUNT	p_md.md_pc_count
member	P_WATCH_1	p_md.md_watch_1
member	P_WATCH_2	p_md.md_watch_2
member	P_WATCH_M	p_md.md_watch_m

struct	pcb
member	pcb_regs
member	PCB_FPREGS	pcb_regs.f0
member	pcb_context
member	pcb_onfault
member	pcb_segtab
member	pcb_nwired

struct	cpu_info
member	ci_cpuid
member	ci_curproc
member	ci_curprocpaddr
member	ci_fpuproc
member	ci_ipl

export	CKSEG0_BASE
export	CKSEG1_BASE
export	VM_MIN_KERNEL_ADDRESS

struct	vmspace
member	VMSPACE_PMAP	vm_map.pmap

struct	pmap
member	PM_ASID pm_asid[0].pma_asid
export	NPTEPG
export	PMAP_SEGTABSIZE
export	SEGSHIFT

export	CCA_CACHED
export	CCA_COHERENT_EXCLWRITE
export	CCA_NC

license: unknown


code:
/*	$OpenBSD: interrupt.c,v 1.64 2014/09/30 06:51:58 jmatthew Exp $ */

/*
 * Copyright (c) 2001-2004 Opsycon AB  (www.opsycon.se / www.opsycon.com)
 *
 * 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 <sys/kernel.h>
#include <sys/proc.h>
#include <sys/user.h>
#include <sys/atomic.h>

#include <uvm/uvm_extern.h>

#include <machine/cpu.h>
#include <mips64/mips_cpu.h>
#include <machine/intr.h>
#include <machine/frame.h>

#include <mips64/rm7000.h>

#ifdef DDB
#include <mips64/db_machdep.h>
#include <ddb/db_sym.h>
#endif

void	dummy_splx(int);
void	interrupt(struct trap_frame *);

static struct evcount soft_count;
static int soft_irq = 0;

uint32_t idle_mask;
int	last_low_int;

struct {
	uint32_t int_mask;
	uint32_t (*int_hand)(uint32_t, struct trap_frame *);
} cpu_int_tab[NLOWINT];

int_f	*splx_hand = &dummy_splx;

/*
 *  Modern versions of MIPS processors have extended interrupt
 *  capabilities. How these are handled differs from implementation
 *  to implementation. This code tries to hide away some of these
 *  in "higher level" interrupt code.
 *
 *  Basically there are <n> interrupt inputs to the processor and
 *  typically the HW designer ties these interrupts to various
 *  sources in the HW. The low level code does not deal with interrupts
 *  in more than it dispatches handling to the code that has registered
 *  an interrupt handler for that particular interrupt. More than one
 *  handler can register to an interrupt input and one handler may register
 *  for more than one interrupt input. A handler is only called once even
 *  if it registers for more than one interrupt input.
 *
 *  The interrupt mechanism in this port uses a delayed masking model
 *  where interrupts are not really masked when doing an spl(). Instead
 *  a masked interrupt will be taken and validated in the various
 *  handlers. If the handler finds that an interrupt is masked it will
 *  register this interrupt as pending and return a new mask to this
 *  code that will turn off the interrupt hardware wise. Later when
 *  the pending interrupt is unmasked it will be processed as usual
 *  and the regular hardware mask will be restored.
 */

/*
 * Handle an interrupt. Both kernel and user mode are handled here.
 *
 * The interrupt handler is called with the CR_INT bits set that
 * were given when the handler was registered.
 * The handler should return a similar word with a mask indicating
 * which CR_INT bits have been handled.
 */

void
interrupt(struct trap_frame *trapframe)
{
	struct cpu_info *ci = curcpu();
	u_int32_t pending;
	int i, s;

	/*
	 *  Paranoic? Perhaps. But if we got here with the enable
	 *  bit reset a mtc0 COP_0_STATUS_REG may have been interrupted.
	 *  If this was a disable and the pipeline had advanced long
	 *  enough... i don't know but better safe than sorry...
	 *  The main effect is not the interrupts but the spl mechanism.
	 */
	if (!(trapframe->sr & SR_INT_ENAB))
		return;

	ci->ci_intrdepth++;

#ifdef DEBUG_INTERRUPT
	trapdebug_enter(ci, trapframe, T_INT);
#endif
	atomic_inc_int(&uvmexp.intrs);

	/* Mask out interrupts from cause that are unmasked */
	pending = trapframe->cause & CR_INT_MASK & trapframe->sr;

	if (pending & SOFT_INT_MASK_0) {
		clearsoftintr0();
		atomic_inc_long((unsigned long *)&soft_count.ec_count);
	}

#ifdef RM7K_PERFCNTR
	if (pending & CR_INT_PERF)
		rm7k_perfintr(trapframe);
#endif

	for (i = 0; i <= last_low_int; i++) {
		uint32_t active;
		active = cpu_int_tab[i].int_mask & pending;
		if (active != 0)
			(*cpu_int_tab[i].int_hand)(active, trapframe);
	}

	/*
	 * Dispatch soft interrupts if current ipl allows them.
	 */
	if (ci->ci_ipl < IPL_SOFTINT && ci->ci_softpending != 0) {
		s = splsoft();
		dosoftint();
		__asm__ (".set noreorder\n");
		ci->ci_ipl = s;	/* no-overhead splx */
		mips_sync();
		__asm__ (".set reorder\n");
	}

	ci->ci_intrdepth--;
}

/*
 * Set up handler for external interrupt events.
 * Use CR_INT_<n> to select the proper interrupt condition to dispatch on.
 * We also enable the software ints here since they are always on.
 */
void
set_intr(int pri, uint32_t mask,
    uint32_t (*int_hand)(uint32_t, struct trap_frame *))
{
	if ((idle_mask & SOFT_INT_MASK) == 0) {
		evcount_attach(&soft_count, "soft", &soft_irq);
		idle_mask |= SOFT_INT_MASK;
	}
	if (pri < 0 || pri >= NLOWINT)
		panic("set_intr: too high priority (%d), increase NLOWINT",
		    pri);

	if (pri > last_low_int)
		last_low_int = pri;

	if ((mask & ~CR_INT_MASK) != 0)
		panic("set_intr: invalid mask 0x%x", mask);

	if (cpu_int_tab[pri].int_mask != 0 &&
	   (cpu_int_tab[pri].int_mask != mask ||
	    cpu_int_tab[pri].int_hand != int_hand))
		panic("set_intr: int already set at pri %d", pri);

	cpu_int_tab[pri].int_hand = int_hand;
	cpu_int_tab[pri].int_mask = mask;
	idle_mask |= mask;
}

void
dummy_splx(int newcpl)
{
	/* Dummy handler */
}

/*
 *  splinit() is special in that sense that it require us to update
 *  the interrupt mask in the CPU since it may be the first time we arm
 *  the interrupt system. This function is called right after
 *  autoconfiguration has completed in autoconf.c.
 *  We enable everything in idle_mask.
 */
void
splinit()
{
	struct proc *p = curproc;
	struct pcb *pcb = &p->p_addr->u_pcb;

	/*
	 * Update proc0 pcb to contain proper values.
	 */
#ifdef RM7000_ICR
	pcb->pcb_context.val[12] = (idle_mask << 8) & IC_INT_MASK;
#endif
	pcb->pcb_context.val[11] = (pcb->pcb_regs.sr & ~SR_INT_MASK) |
	    (idle_mask & SR_INT_MASK);

	spl0();
	(void)updateimask(0);
}

int
splraise(int newipl)
{
	struct cpu_info *ci = curcpu();
        int oldipl;

	__asm__ (".set noreorder\n");
	oldipl = ci->ci_ipl;
	if (oldipl < newipl) {
		/* XXX to kill warning about dla being used in a delay slot */
		__asm__("nop");
		ci->ci_ipl = newipl;
	}
	mips_sync();
	__asm__ (".set reorder\n");
	return oldipl;
}

void
splx(int newipl)
{
	(*splx_hand)(newipl);
}

int
spllower(int newipl)
{
	struct cpu_info *ci = curcpu();
	int oldipl;

	oldipl = ci->ci_ipl;
	splx(newipl);
	return oldipl;
}


code:
/* $OpenBSD: ipifuncs.c,v 1.11 2015/07/27 16:33:40 visa Exp $ */
/* $NetBSD: ipifuncs.c,v 1.40 2008/04/28 20:23:10 martin Exp $ */

/*-
 * Copyright (c) 1998, 1999, 2000, 2001 The NetBSD Foundation, Inc.
 * All rights reserved.
 *
 * This code is derived from software contributed to The NetBSD Foundation
 * by Jason R. Thorpe of the Numerical Aerospace Simulation Facility,
 * NASA Ames Research Center.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

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

#include <uvm/uvm_extern.h>

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

int	mips64_ipi_intr(void *);
void	mips64_ipi_nop(void);
void	smp_rendezvous_action(void);
void	mips64_ipi_ddb(void);
void	mips64_multicast_ipi(unsigned int, unsigned int);
unsigned int ipi_mailbox[MAXCPUS];

/* Variables needed for SMP rendezvous. */
struct mutex smp_ipi_mtx;
volatile unsigned long smp_rv_map;
void (*volatile smp_rv_action_func)(void *arg);
void * volatile smp_rv_func_arg;
volatile unsigned int smp_rv_waiters[2];

/*
 * NOTE: This table must be kept in order with the bit definitions
 * in <machine/intr.h>.
 */
typedef void (*ipifunc_t)(void);

ipifunc_t ipifuncs[MIPS64_NIPIS] = {
	mips64_ipi_nop,
	smp_rendezvous_action,
	mips64_ipi_ddb
};

/*
 * Initialize IPI state for a CPU.
 */
void
mips64_ipi_init(void)
{
	cpuid_t cpuid = cpu_number();
	int error;

	if (!cpuid)
		mtx_init(&smp_ipi_mtx, IPL_IPI);

	hw_ipi_intr_clear(cpuid);

	error = hw_ipi_intr_establish(mips64_ipi_intr, cpuid);
	if (error)
		panic("hw_ipi_intr_establish failed:%d", error);
}

/*
 * Process IPIs for a CPU.
 */
int
mips64_ipi_intr(void *arg)
{
	unsigned int pending_ipis, bit;
	unsigned int cpuid = (unsigned int)(unsigned long)arg;

	KASSERT (cpuid == cpu_number());

	/* clear ipi interrupt */
	hw_ipi_intr_clear(cpuid);
	/* get and clear pending ipis */
	pending_ipis = atomic_swap_uint(&ipi_mailbox[cpuid], 0);

	if (pending_ipis > 0) {
		for (bit = 0; bit < MIPS64_NIPIS; bit++)
			if (pending_ipis & (1UL << bit))
				(*ipifuncs[bit])();
	}

	return 1;
}

/*
 * Send an interprocessor interrupt.
 */
void
mips64_send_ipi(unsigned int cpuid, unsigned int ipimask)
{
#ifdef DEBUG
	if (cpuid >= CPU_MAXID || get_cpu_info(cpuid) == NULL)
		panic("mips_send_ipi: bogus cpu_id");
	if (!cpuset_isset(&cpus_running, get_cpu_info(cpuid)))
	        panic("mips_send_ipi: CPU %ld not running", cpuid);
#endif

	atomic_setbits_int(&ipi_mailbox[cpuid], ipimask);

	hw_ipi_intr_set(cpuid);
}

/*
 * Send an IPI to all in the list but ourselves.
 */
void
mips64_multicast_ipi(unsigned int cpumask, unsigned int ipimask)
{
	struct cpu_info *ci;
	CPU_INFO_ITERATOR cii;

	cpumask &= ~(1 << cpu_number());

	CPU_INFO_FOREACH(cii, ci) {
		if (!(cpumask & (1UL << ci->ci_cpuid)) ||
		    !cpuset_isset(&cpus_running, ci))
			continue;
		mips64_send_ipi(ci->ci_cpuid, ipimask);
	}
}

void
mips64_ipi_nop(void)
{
#ifdef DEBUG
	printf("mips64_ipi_nop on cpu%d\n", cpu_number());
#endif
}

/*
 * All-CPU rendezvous.  CPUs are signalled, all execute the setup function
 * (if specified), rendezvous, execute the action function (if specified),
 * rendezvous again, execute the teardown function (if specified), and then
 * resume.
 *
 * Note that the supplied external functions _must_ be reentrant and aware
 * that they are running in parallel and in an unknown lock context.
 */

void
smp_rendezvous_action(void)
{
	void* local_func_arg = smp_rv_func_arg;
	void (*local_action_func)(void*) = smp_rv_action_func;
	unsigned int cpumask = 1 << cpu_number();

	/* Ensure we have up-to-date values. */
	atomic_setbits_int(&smp_rv_waiters[0], cpumask);
	while (smp_rv_waiters[0] != smp_rv_map)
		;

	/* action function */
	if (local_action_func != NULL)
		local_action_func(local_func_arg);

	/* spin on exit rendezvous */
	atomic_setbits_int(&smp_rv_waiters[1], cpumask);
}

void
smp_rendezvous_cpus(unsigned long map,
	void (* action_func)(void *),
	void *arg)
{
	unsigned int cpumask = 1 << cpu_number();

	if (ncpus == 1) {
		if (action_func != NULL)
			action_func(arg);
		return;
	}

	/* obtain rendezvous lock */
        mtx_enter(&smp_ipi_mtx);

	/* set static function pointers */
	smp_rv_map = map;
	smp_rv_action_func = action_func;
	smp_rv_func_arg = arg;
	smp_rv_waiters[0] = 0;
	smp_rv_waiters[1] = 0;

	/* signal other processors, which will enter the IPI with interrupts off */
	mips64_multicast_ipi(map & ~cpumask, MIPS64_IPI_RENDEZVOUS);

	/* Check if the current CPU is in the map */
	if (map & cpumask)
		smp_rendezvous_action();

	while (smp_rv_waiters[1] != smp_rv_map)
		;
	/* release lock */
	mtx_leave(&smp_ipi_mtx);
}

void
mips64_ipi_ddb(void)
{
#ifdef DDB
	Debugger();
#endif
}


code:
/*	$OpenBSD: lcore_access.S,v 1.24 2012/10/03 11:18:23 miod Exp $ */

/*
 * Copyright (c) 2001-2003 Opsycon AB  (www.opsycon.se / www.opsycon.com)
 *
 * 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/errno.h>
#include <sys/syscall.h>

#include <machine/param.h>
#include <machine/asm.h>
#include <machine/cpu.h>
#include <mips64/mips_cpu.h>
#include <machine/regnum.h>
#include <machine/cpustate.h>
#include <machine/trap.h>

#include "assym.h"

	.set	mips3

	.set	noreorder		# Noreorder is default style!

/*
 * Primitives
 */

/*
 * This table is indexed by u.u_pcb.pcb_onfault in trap().
 * The reason for using this table rather than storing an address in
 * u.u_pcb.pcb_onfault is simply to make the code faster.
 * This table must match with definitions in trap.h.
 */
	.globl	onfault_table
	.data
	.align	3
onfault_table:
	PTR_VAL	0		# invalid index number
	PTR_VAL	_copyerr
	PTR_VAL	_kcopyerr
#if defined(DDB) || defined(DEBUG)
	PTR_VAL	kt_ddberr
#else
	PTR_VAL	0
#endif
	PTR_VAL	_guarderr
	.text

/*
 * This code is copied the user's stack for returning from signal handlers
 * (see sendsig() and sigreturn()). We have to compute the address
 * of the sigcontext struct for the sigreturn call.
 */
	.globl	sigcode
sigcode:
	PTR_ADDU a0, sp, 4*REGSZ		# address of sigcontext
	LI	v0, SYS_sigreturn	# sigreturn(scp)
	syscall
	break	0			# just in case sigreturn fails
	.globl	esigcode
esigcode:

/*
 * Copy a null terminated string within the kernel address space.
 * Maxlength may be null if count not wanted.
 *	copystr(fromaddr, toaddr, maxlength, &lencopied)
 *		caddr_t fromaddr;
 *		caddr_t toaddr;
 *		u_int maxlength;
 *		u_long *lencopied;
 */
LEAF(copystr, 0)
	move	t2, a2			# Save the number of bytes
	move	v0, zero
1:
	lbu	t0, 0(a0)
	PTR_SUBU a2, a2, 1
	sb	t0, 0(a1)
	PTR_ADDU a0, a0, 1
	beq	t0, zero, 2f
	 PTR_ADDU a1, a1, 1
	bne	a2, zero, 1b
	 NOP
	LI	v0, ENAMETOOLONG	# String is longer than maxlength
2:
	beq	a3, zero, 3f
	 PTR_SUBU a2, t2, a2		# Compute length copied
	REG_S	a2, 0(a3)
3:
	j	ra
	 NOP
END(copystr)

/*
 * Optimized memory zero code.
 * mem_zero_page(addr);
 */
LEAF(mem_zero_page, 0)
	LI	v0, PAGE_SIZE
1:
	PTR_SUBU v0, 8
	sd	zero, 0(a0)
	bne	zero, v0, 1b
	 PTR_ADDU a0, 8
	jr	ra
	 NOP
END(mem_zero_page)

/*
 * Copy a null terminated string from the user address space into
 * the kernel address space.
 *
 *	copyinstr(fromaddr, toaddr, maxlength, &lencopied)
 *		caddr_t fromaddr;
 *		caddr_t toaddr;
 *		u_int maxlength;
 *		u_int *lencopied;
 */
NON_LEAF(copyinstr, FRAMESZ(CF_SZ), ra)
	PTR_SUBU sp, sp, FRAMESZ(CF_SZ)
	.mask	0x80000000, (CF_RA_OFFS - FRAMESZ(CF_SZ))
	PTR_S	ra, CF_RA_OFFS(sp)
	blt	a0, zero, _copyerr	# make sure address is in user space
	 LI	v0, KT_COPYERR
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)
	jal	copystr
	 sw	v0, PCB_ONFAULT(t3)

	PTR_L	ra, CF_RA_OFFS(sp)
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)
	sw	zero, PCB_ONFAULT(t3)
	PTR_ADDU sp, sp, FRAMESZ(CF_SZ)
	j	ra
	 NOP
END(copyinstr)

/*
 * Copy a null terminated string from the kernel address space into
 * the user address space.
 *
 *	copyoutstr(fromaddr, toaddr, maxlength, &lencopied)
 *		caddr_t fromaddr;
 *		caddr_t toaddr;
 *		u_int maxlength;
 *		u_int *lencopied;
 */
NON_LEAF(copyoutstr, FRAMESZ(CF_SZ), ra)
	PTR_SUBU sp, sp, FRAMESZ(CF_SZ)
	.mask	0x80000000, (CF_RA_OFFS - FRAMESZ(CF_SZ))
	PTR_S	ra, CF_RA_OFFS(sp)
	blt	a1, zero, _copyerr	# make sure address is in user space
	 LI	v0, KT_COPYERR
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)
	jal	copystr
	 sw	v0, PCB_ONFAULT(t3)

	PTR_L	ra, CF_RA_OFFS(sp)
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)
	sw	zero, PCB_ONFAULT(t3)
	PTR_ADDU sp, sp, FRAMESZ(CF_SZ)
	j	ra
	 NOP
END(copyoutstr)

/*
 * Copy specified amount of data from user space into the kernel
 *	copyin(from, to, len)
 *		caddr_t *from;	(user source address)
 *		caddr_t *to;	(kernel destination address)
 *		unsigned len;
 */
NON_LEAF(copyin, FRAMESZ(CF_SZ), ra)
	PTR_SUBU sp, sp, FRAMESZ(CF_SZ)
	.mask	0x80000000, (CF_RA_OFFS - FRAMESZ(CF_SZ))
	PTR_S	ra, CF_RA_OFFS(sp)
	blt	a0, zero, _copyerr	# make sure address is in user space
	 li	v0, KT_COPYERR
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)
	jal	bcopy
	 sw	v0, PCB_ONFAULT(t3)

	PTR_L	ra, CF_RA_OFFS(sp)
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)
	sw	zero, PCB_ONFAULT(t3)
	PTR_ADDU sp, sp, FRAMESZ(CF_SZ)
	j	ra
	 move	v0, zero
END(copyin)

/*
 * Copy specified amount of data from kernel to the user space
 *	copyout(from, to, len)
 *		caddr_t *from;	(kernel source address)
 *		caddr_t *to;	(user destination address)
 *		unsigned len;
 */
NON_LEAF(copyout, FRAMESZ(CF_SZ), ra)
	PTR_SUBU sp, sp, FRAMESZ(CF_SZ)
	.mask	0x80000000, (CF_RA_OFFS - FRAMESZ(CF_SZ))
	PTR_S	ra, CF_RA_OFFS(sp)
	blt	a1, zero, _copyerr	# make sure address is in user space
	 li	v0, KT_COPYERR
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)
	jal	bcopy
	 sw	v0, PCB_ONFAULT(t3)

	PTR_L	ra, CF_RA_OFFS(sp)
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)
	sw	zero, PCB_ONFAULT(t3)
	PTR_ADDU sp, sp, FRAMESZ(CF_SZ)
	j	ra
	 move	v0, zero
END(copyout)

_copyerr:
	PTR_L	ra, CF_RA_OFFS(sp)
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)
	sw	zero, PCB_ONFAULT(t3)
	PTR_ADDU sp, sp, FRAMESZ(CF_SZ)
	j	ra
	 li	v0, EFAULT		# return error

/*
 *  kcopy is a wrapper around bcopy that catches bad memory references.
 */
NON_LEAF(kcopy, FRAMESZ(CF_SZ + REGSZ), ra)
	PTR_SUBU sp, sp, FRAMESZ(CF_SZ + REGSZ)
	.mask	0x80000000, (CF_RA_OFFS - FRAMESZ(CF_SZ + REGSZ))
	PTR_S	ra, CF_RA_OFFS + REGSZ(sp)
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)
	lw	v1, PCB_ONFAULT(t3)
	li	v0, KT_KCOPYERR
	PTR_S	v1, CF_ARGSZ(sp)	# save old pcb_onfault
	jal	bcopy
	 sw	v0, PCB_ONFAULT(t3)

	PTR_L	v0, CF_ARGSZ(sp)
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)
	PTR_L	ra, CF_RA_OFFS + REGSZ(sp)
	sw	v0, PCB_ONFAULT(t3)
	PTR_ADDU sp, sp, FRAMESZ(CF_SZ + REGSZ)
	j	ra
	 move	v0, zero
END(kcopy)

_kcopyerr:
	PTR_L	v0, CF_ARGSZ(sp)
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)
	PTR_L	ra, CF_RA_OFFS + REGSZ(sp)
	sw	v0, PCB_ONFAULT(t3)
	PTR_ADDU sp, sp, FRAMESZ(CF_SZ + REGSZ)
	j	ra
	 li	v0, EFAULT		# return error

/*
 * Guarded ``memory'' access routines
 * int guarded_read_1(paddr_t address, uint8_t *dest);
 * int guarded_read_2(paddr_t address, uint16_t *dest);
 * int guarded_read_4(paddr_t address, uint32_t *dest);
 * int guarded_write_4(paddr_t address, uint32_t src);
 */

LEAF(guarded_read_1, 0)
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)

#ifndef CPU_R8000
	li	a3, SR_BOOT_EXC_VEC
	MFC0	a2, COP_0_STATUS_REG
	or	a4, a2, a3
	xor	a4, a4, a3
#endif

	li	v0, KT_GUARDERR
	lw	v1, PCB_ONFAULT(t3)
	sw	v0, PCB_ONFAULT(t3)

#ifndef CPU_R8000
	MTC0	a4, COP_0_STATUS_REG
	MTC0_HAZARD
#endif

	lb	v0, 0(a0)
	sb	v0, 0(a1)

#ifndef CPU_R8000
	MTC0	a2, COP_0_STATUS_REG
	MTC0_HAZARD
#endif

	sw	v1, PCB_ONFAULT(t3)
	j	ra
	 move	v0, zero
END(guarded_read_1)

LEAF(guarded_read_2, 0)
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)

#ifndef CPU_R8000
	li	a3, SR_BOOT_EXC_VEC
	MFC0	a2, COP_0_STATUS_REG
	or	a4, a2, a3
	xor	a4, a4, a3
#endif

	li	v0, KT_GUARDERR
	lw	v1, PCB_ONFAULT(t3)
	sw	v0, PCB_ONFAULT(t3)

#ifndef CPU_R8000
	MTC0	a4, COP_0_STATUS_REG
	MTC0_HAZARD
#endif

	lh	v0, 0(a0)
	sh	v0, 0(a1)

#ifndef CPU_R8000
	MTC0	a2, COP_0_STATUS_REG
	MTC0_HAZARD
#endif

	sw	v1, PCB_ONFAULT(t3)
	j	ra
	move	v0, zero
END(guarded_read_2)

LEAF(guarded_read_4, 0)
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)

#ifndef CPU_R8000
	li	a3, SR_BOOT_EXC_VEC
	MFC0	a2, COP_0_STATUS_REG
	or	a4, a2, a3
	xor	a4, a4, a3
#endif

	li	v0, KT_GUARDERR
	lw	v1, PCB_ONFAULT(t3)
	sw	v0, PCB_ONFAULT(t3)

#ifndef CPU_R8000
	MTC0	a4, COP_0_STATUS_REG
	MTC0_HAZARD
#endif

	lw	v0, 0(a0)
	sw	v0, 0(a1)

#ifndef CPU_R8000
	MTC0	a2, COP_0_STATUS_REG
	MTC0_HAZARD
#endif

	sw	v1, PCB_ONFAULT(t3)
	j	ra
	move	v0, zero
END(guarded_read_4)

LEAF(guarded_write_4, 0)
	GET_CPU_INFO(t1, t0)
	PTR_L	t3, CI_CURPROCPADDR(t1)

#ifndef CPU_R8000
	li	a3, SR_BOOT_EXC_VEC
	MFC0	a2, COP_0_STATUS_REG
	or	a4, a2, a3
	xor	a4, a4, a3
#endif

	li	v0, KT_GUARDERR
	lw	v1, PCB_ONFAULT(t3)
	sw	v0, PCB_ONFAULT(t3)

#ifndef CPU_R8000
	MTC0	a4, COP_0_STATUS_REG
	MTC0_HAZARD
#endif

	sw	a1, 0(a0)

#ifndef CPU_R8000
	MTC0	a2, COP_0_STATUS_REG
	MTC0_HAZARD
#endif

	sw	v1, PCB_ONFAULT(t3)
	j	ra
	move	v0, zero
END(guarded_write_4)

_guarderr:
#ifndef CPU_R8000
	MTC0	a2, COP_0_STATUS_REG
	MTC0_HAZARD
#endif

	sw	v1, PCB_ONFAULT(t3)
	j	ra
	li	v0, EFAULT		# return error


code:
/*	$OpenBSD: lcore_ddb.S,v 1.13 2012/10/03 11:18:23 miod Exp $ */

/*
 * Copyright (c) 2001-2003 Opsycon AB  (www.opsycon.se / www.opsycon.com)
 *
 * 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/errno.h>
#include <sys/syscall.h>

#include <machine/param.h>
#include <machine/asm.h>
#include <machine/cpu.h>
#include <mips64/mips_cpu.h>
#include <machine/regnum.h>
#include <machine/cpustate.h>
#include <machine/trap.h>

#include "assym.h"

	.set	mips3
	.set	noreorder		# Noreorder is default style!

LEAF(kdbpeekd, 0)
	GET_CPU_INFO(t1, t0)
	PTR_L	t0, CI_CURPROCPADDR(t1)
	li	v0, KT_DDBERR
	and	v1, a0, 7			# unaligned ?
	bne	v1, zero, 1f
	 sw	v0, PCB_ONFAULT(t0)

	ld	v0, (a0)
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)

1:
	LDHI	v0, 0(a0)
	LDLO	v0, 7(a0)
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)
END(kdbpeekd)

LEAF(kdbpeek, 0)
	GET_CPU_INFO(t1, t0)
	PTR_L	t0, CI_CURPROCPADDR(t1)
	li	v0, KT_DDBERR
	and	v1, a0, 3			# unaligned ?
	bne	v1, zero, 1f
	 sw	v0, PCB_ONFAULT(t0)

	lwu	v0, (a0)
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)

1:
	LWHI	v0, 0(a0)
	LWLO	v0, 3(a0)
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)
END(kdbpeek)

LEAF(kdbpeekw, 0)
	GET_CPU_INFO(t1, t0)
	PTR_L	t0, CI_CURPROCPADDR(t1)
	li	v0, KT_DDBERR
	and	v1, a0, 1			# unaligned ?
	bne	v1, zero, 1f
	 sw	v0, PCB_ONFAULT(t0)

	lh	v0, (a0)
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)

1:
	li	v0, -1				# error!
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)
END(kdbpeekw)

LEAF(kdbpeekb, 0)
	GET_CPU_INFO(t1, t0)
	PTR_L	t0, CI_CURPROCPADDR(t1)
	li	v0, KT_DDBERR
	sw	v0, PCB_ONFAULT(t0)
	lb	v0, 0(a0)
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)
END(kdbpeekb)

	.globl	kt_ddberr
kt_ddberr:
	jr	ra
	 li	v0, -1

LEAF(kdbpoked, 0)
	GET_CPU_INFO(t1, t0)
	PTR_L	t0, CI_CURPROCPADDR(t1)
	li	v0, KT_DDBERR
	and	v1, a0, 7			# unaligned ?
	bne	v1, zero, 1f
	 sw	v0, PCB_ONFAULT(t0)

	sd	a1, (a0)
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)

1:
	SDHI	a1, 0(a0)
	SDLO	a1, 7(a0)
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)
END(kdbpoked)

LEAF(kdbpoke, 0)
	GET_CPU_INFO(t1, t0)
	PTR_L	t0, CI_CURPROCPADDR(t1)
	li	v0, KT_DDBERR
	and	v1, a0, 3			# unaligned ?
	bne	v1, zero, 1f
	 sw	v0, PCB_ONFAULT(t0)

	sw	a1, (a0)
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)

1:
	SWHI	a1, 0(a0)
	SWLO	a1, 3(a0)
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)
END(kdbpoke)

LEAF(kdbpokew, 0)
	GET_CPU_INFO(t1, t0)
	PTR_L	t0, CI_CURPROCPADDR(t1)
	li	v0, KT_DDBERR
	and	v1, a0, 1			# unaligned ?
	bne	v1, zero, 1f
	 sw	v0, PCB_ONFAULT(t0)

	sh	a1, (a0)
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)

1:
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)
END(kdbpokew)

LEAF(kdbpokeb, 0)
	GET_CPU_INFO(t1, t0)
	PTR_L	t0, CI_CURPROCPADDR(t1)
	li	v0, KT_DDBERR
	sw	v0, PCB_ONFAULT(t0)
	sb	a1, 0(a0)
	jr	ra
	 sw	zero, PCB_ONFAULT(t0)
END(kdbpokeb)

LEAF(Debugger, 0)
	break   BREAK_SOVER_VAL
	jr	ra
	 NOP
END(Debugger)

LEAF(setjmp, 0)
	MFC0	v0, COP_0_STATUS_REG
	MFC0_HAZARD
	REG_S	s0, REGSZ * 0(a0)
	REG_S	s1, REGSZ * 1(a0)
	REG_S	s2, REGSZ * 2(a0)
	REG_S	s3, REGSZ * 3(a0)
	REG_S	s4, REGSZ * 4(a0)
	REG_S	s5, REGSZ * 5(a0)
	REG_S	s6, REGSZ * 6(a0)
	REG_S	s7, REGSZ * 7(a0)
	REG_S	s8, REGSZ * 8(a0)
	REG_S	sp, REGSZ * 9(a0)
	REG_S	ra, REGSZ * 10(a0)
	REG_S	v0, REGSZ * 11(a0)
	jr	ra
	 li	v0, 0			# setjmp return
END(setjmp)

LEAF(longjmp, 0)
	REG_L	v0, REGSZ * 11(a0)
	REG_L	ra, REGSZ * 10(a0)
	REG_L	s0, REGSZ * 0(a0)
	REG_L	s1, REGSZ * 1(a0)
	REG_L	s2, REGSZ * 2(a0)
	REG_L	s3, REGSZ * 3(a0)
	REG_L	s4, REGSZ * 4(a0)
	REG_L	s5, REGSZ * 5(a0)
	REG_L	s6, REGSZ * 6(a0)
	REG_L	s7, REGSZ * 7(a0)
	REG_L	s8, REGSZ * 8(a0)
	REG_L	sp, REGSZ * 9(a0)
	MTC0	v0, COP_0_STATUS_REG
	MTC0_SR_IE_HAZARD
	jr	ra
	 li	v0, 1			# longjmp return
END(longjmp)


code:
/*	$OpenBSD: lcore_float.S,v 1.22 2012/10/03 11:18:23 miod Exp $ */

/*
 * Copyright (c) 2001-2003 Opsycon AB  (www.opsycon.se / www.opsycon.com)
 *
 * 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/errno.h>
#include <sys/syscall.h>

#include <machine/param.h>
#include <machine/asm.h>
#include <machine/cpu.h>
#include <mips64/mips_cpu.h>
#include <machine/regnum.h>
#include <machine/cpustate.h>

#include "assym.h"

	.set	mips3
	.set	noreorder		# Noreorder is default style!

/*----------------------------------------------------------------------------
 *
 * MipsSwitchFPState --
 *
 *	Save the current state into 'from' and restore it from 'to'.
 *
 *	MipsSwitchFPState(from, to)
 *		struct proc *from;
 *		struct user *to;
 *
 * Results:
 *	None.
 *
 * Side effects:
 *	None.
 *
 *----------------------------------------------------------------------------
 */
LEAF(MipsSwitchFPState, 0)
	MFC0	t1, COP_0_STATUS_REG	# Save old SR
	MFC0_HAZARD
	or	t0, t1, SR_COP_1_BIT|SR_FR_32	# enable the coprocessor
	MTC0	t0, COP_0_STATUS_REG
	MTC0_SR_CU_HAZARD

	beq	a0, zero, 1f		# skip save if NULL pointer
	 NOP
/*
 * First read out the status register to make sure that all FP operations
 * have completed.
 */
	PTR_L	a0, P_ADDR(a0)		# get pointer to pcb for proc
	cfc1	t0, FPC_CSR		# stall til FP done
	cfc1	t0, FPC_CSR		# now get status
	LI	t3, ~SR_COP_1_BIT
	REG_L	t2, PCB_REGS+(SR * REGSZ)(a0)	# get CPU status register
	REG_S	t0, PCB_FPREGS+(32 * REGSZ)(a0)	# save FP status
	and	t2, t2, t3			# clear COP_1 enable bit
	REG_S	t2, PCB_REGS+(SR * REGSZ)(a0)	# save new status register
/*
 * Save the floating point registers.
 */
	sdc1	$f0, PCB_FPREGS+(0 * REGSZ)(a0)
	sdc1	$f1, PCB_FPREGS+(1 * REGSZ)(a0)
	sdc1	$f2, PCB_FPREGS+(2 * REGSZ)(a0)
	sdc1	$f3, PCB_FPREGS+(3 * REGSZ)(a0)
	sdc1	$f4, PCB_FPREGS+(4 * REGSZ)(a0)
	sdc1	$f5, PCB_FPREGS+(5 * REGSZ)(a0)
	sdc1	$f6, PCB_FPREGS+(6 * REGSZ)(a0)
	sdc1	$f7, PCB_FPREGS+(7 * REGSZ)(a0)
	sdc1	$f8, PCB_FPREGS+(8 * REGSZ)(a0)
	sdc1	$f9, PCB_FPREGS+(9 * REGSZ)(a0)
	sdc1	$f10, PCB_FPREGS+(10 * REGSZ)(a0)
	sdc1	$f11, PCB_FPREGS+(11 * REGSZ)(a0)
	sdc1	$f12, PCB_FPREGS+(12 * REGSZ)(a0)
	sdc1	$f13, PCB_FPREGS+(13 * REGSZ)(a0)
	sdc1	$f14, PCB_FPREGS+(14 * REGSZ)(a0)
	sdc1	$f15, PCB_FPREGS+(15 * REGSZ)(a0)
	sdc1	$f16, PCB_FPREGS+(16 * REGSZ)(a0)
	sdc1	$f17, PCB_FPREGS+(17 * REGSZ)(a0)
	sdc1	$f18, PCB_FPREGS+(18 * REGSZ)(a0)
	sdc1	$f19, PCB_FPREGS+(19 * REGSZ)(a0)
	sdc1	$f20, PCB_FPREGS+(20 * REGSZ)(a0)
	sdc1	$f21, PCB_FPREGS+(21 * REGSZ)(a0)
	sdc1	$f22, PCB_FPREGS+(22 * REGSZ)(a0)
	sdc1	$f23, PCB_FPREGS+(23 * REGSZ)(a0)
	sdc1	$f24, PCB_FPREGS+(24 * REGSZ)(a0)
	sdc1	$f25, PCB_FPREGS+(25 * REGSZ)(a0)
	sdc1	$f26, PCB_FPREGS+(26 * REGSZ)(a0)
	sdc1	$f27, PCB_FPREGS+(27 * REGSZ)(a0)
	sdc1	$f28, PCB_FPREGS+(28 * REGSZ)(a0)
	sdc1	$f29, PCB_FPREGS+(29 * REGSZ)(a0)
	sdc1	$f30, PCB_FPREGS+(30 * REGSZ)(a0)
	sdc1	$f31, PCB_FPREGS+(31 * REGSZ)(a0)

1:
/*
 *  Restore the floating point registers.
 */
	REG_L	t0, PCB_FPREGS+(32 * REGSZ)(a1)	# get status register
	ldc1	$f0, PCB_FPREGS+(0 * REGSZ)(a1)
	ldc1	$f1, PCB_FPREGS+(1 * REGSZ)(a1)
	ldc1	$f2, PCB_FPREGS+(2 * REGSZ)(a1)
	ldc1	$f3, PCB_FPREGS+(3 * REGSZ)(a1)
	ldc1	$f4, PCB_FPREGS+(4 * REGSZ)(a1)
	ldc1	$f5, PCB_FPREGS+(5 * REGSZ)(a1)
	ldc1	$f6, PCB_FPREGS+(6 * REGSZ)(a1)
	ldc1	$f7, PCB_FPREGS+(7 * REGSZ)(a1)
	ldc1	$f8, PCB_FPREGS+(8 * REGSZ)(a1)
	ldc1	$f9, PCB_FPREGS+(9 * REGSZ)(a1)
	ldc1	$f10, PCB_FPREGS+(10 * REGSZ)(a1)
	ldc1	$f11, PCB_FPREGS+(11 * REGSZ)(a1)
	ldc1	$f12, PCB_FPREGS+(12 * REGSZ)(a1)
	ldc1	$f13, PCB_FPREGS+(13 * REGSZ)(a1)
	ldc1	$f14, PCB_FPREGS+(14 * REGSZ)(a1)
	ldc1	$f15, PCB_FPREGS+(15 * REGSZ)(a1)
	ldc1	$f16, PCB_FPREGS+(16 * REGSZ)(a1)
	ldc1	$f17, PCB_FPREGS+(17 * REGSZ)(a1)
	ldc1	$f18, PCB_FPREGS+(18 * REGSZ)(a1)
	ldc1	$f19, PCB_FPREGS+(19 * REGSZ)(a1)
	ldc1	$f20, PCB_FPREGS+(20 * REGSZ)(a1)
	ldc1	$f21, PCB_FPREGS+(21 * REGSZ)(a1)
	ldc1	$f22, PCB_FPREGS+(22 * REGSZ)(a1)
	ldc1	$f23, PCB_FPREGS+(23 * REGSZ)(a1)
	ldc1	$f24, PCB_FPREGS+(24 * REGSZ)(a1)
	ldc1	$f25, PCB_FPREGS+(25 * REGSZ)(a1)
	ldc1	$f26, PCB_FPREGS+(26 * REGSZ)(a1)
	ldc1	$f27, PCB_FPREGS+(27 * REGSZ)(a1)
	ldc1	$f28, PCB_FPREGS+(28 * REGSZ)(a1)
	ldc1	$f29, PCB_FPREGS+(29 * REGSZ)(a1)
	ldc1	$f30, PCB_FPREGS+(30 * REGSZ)(a1)
	ldc1	$f31, PCB_FPREGS+(31 * REGSZ)(a1)

	ctc1	t0, FPC_CSR
	NOP

	MTC0	t1, COP_0_STATUS_REG	# Restore the status register.
	MTC0_SR_CU_HAZARD
	j	ra
	 NOP
END(MipsSwitchFPState)

LEAF(MipsSwitchFPState16, 0)
	MFC0	t1, COP_0_STATUS_REG	# Save old SR
	MFC0_HAZARD
	or	t0, t1, SR_COP_1_BIT	# enable the coprocessor
	MTC0	t0, COP_0_STATUS_REG
	MTC0_SR_CU_HAZARD

	beq	a0, zero, 1f		# skip save if NULL pointer
	 NOP
/*
 * First read out the status register to make sure that all FP operations
 * have completed.
 */
	PTR_L	a0, P_ADDR(a0)		# get pointer to pcb for proc
	cfc1	t0, FPC_CSR		# stall til FP done
	cfc1	t0, FPC_CSR		# now get status
	LI	t3, ~SR_COP_1_BIT
	REG_L	t2, PCB_REGS+(SR * REGSZ)(a0)	# get CPU status register
	REG_S	t0, PCB_FPREGS+(32 * REGSZ)(a0)	# save FP status
	and	t2, t2, t3			# clear COP_1 enable bit
	REG_S	t2, PCB_REGS+(SR * REGSZ)(a0)	# save new status register
/*
 * Save the floating point registers.
 */
	swc1	$f0, PCB_FPREGS+(0 * REGSZ)(a0)
	swc1	$f1, PCB_FPREGS+(1 * REGSZ)(a0)
	swc1	$f2, PCB_FPREGS+(2 * REGSZ)(a0)
	swc1	$f3, PCB_FPREGS+(3 * REGSZ)(a0)
	swc1	$f4, PCB_FPREGS+(4 * REGSZ)(a0)
	swc1	$f5, PCB_FPREGS+(5 * REGSZ)(a0)
	swc1	$f6, PCB_FPREGS+(6 * REGSZ)(a0)
	swc1	$f7, PCB_FPREGS+(7 * REGSZ)(a0)
	swc1	$f8, PCB_FPREGS+(8 * REGSZ)(a0)
	swc1	$f9, PCB_FPREGS+(9 * REGSZ)(a0)
	swc1	$f10, PCB_FPREGS+(10 * REGSZ)(a0)
	swc1	$f11, PCB_FPREGS+(11 * REGSZ)(a0)
	swc1	$f12, PCB_FPREGS+(12 * REGSZ)(a0)
	swc1	$f13, PCB_FPREGS+(13 * REGSZ)(a0)
	swc1	$f14, PCB_FPREGS+(14 * REGSZ)(a0)
	swc1	$f15, PCB_FPREGS+(15 * REGSZ)(a0)
	swc1	$f16, PCB_FPREGS+(16 * REGSZ)(a0)
	swc1	$f17, PCB_FPREGS+(17 * REGSZ)(a0)
	swc1	$f18, PCB_FPREGS+(18 * REGSZ)(a0)
	swc1	$f19, PCB_FPREGS+(19 * REGSZ)(a0)
	swc1	$f20, PCB_FPREGS+(20 * REGSZ)(a0)
	swc1	$f21, PCB_FPREGS+(21 * REGSZ)(a0)
	swc1	$f22, PCB_FPREGS+(22 * REGSZ)(a0)
	swc1	$f23, PCB_FPREGS+(23 * REGSZ)(a0)
	swc1	$f24, PCB_FPREGS+(24 * REGSZ)(a0)
	swc1	$f25, PCB_FPREGS+(25 * REGSZ)(a0)
	swc1	$f26, PCB_FPREGS+(26 * REGSZ)(a0)
	swc1	$f27, PCB_FPREGS+(27 * REGSZ)(a0)
	swc1	$f28, PCB_FPREGS+(28 * REGSZ)(a0)
	swc1	$f29, PCB_FPREGS+(29 * REGSZ)(a0)
	swc1	$f30, PCB_FPREGS+(30 * REGSZ)(a0)
	swc1	$f31, PCB_FPREGS+(31 * REGSZ)(a0)

1:
/*
 *  Restore the floating point registers.
 */
	REG_L	t0, PCB_FPREGS+(32 * REGSZ)(a1)	# get status register
	lwc1	$f0, PCB_FPREGS+(0 * REGSZ)(a1)
	lwc1	$f1, PCB_FPREGS+(1 * REGSZ)(a1)
	lwc1	$f2, PCB_FPREGS+(2 * REGSZ)(a1)
	lwc1	$f3, PCB_FPREGS+(3 * REGSZ)(a1)
	lwc1	$f4, PCB_FPREGS+(4 * REGSZ)(a1)
	lwc1	$f5, PCB_FPREGS+(5 * REGSZ)(a1)
	lwc1	$f6, PCB_FPREGS+(6 * REGSZ)(a1)
	lwc1	$f7, PCB_FPREGS+(7 * REGSZ)(a1)
	lwc1	$f8, PCB_FPREGS+(8 * REGSZ)(a1)
	lwc1	$f9, PCB_FPREGS+(9 * REGSZ)(a1)
	lwc1	$f10, PCB_FPREGS+(10 * REGSZ)(a1)
	lwc1	$f11, PCB_FPREGS+(11 * REGSZ)(a1)
	lwc1	$f12, PCB_FPREGS+(12 * REGSZ)(a1)
	lwc1	$f13, PCB_FPREGS+(13 * REGSZ)(a1)
	lwc1	$f14, PCB_FPREGS+(14 * REGSZ)(a1)
	lwc1	$f15, PCB_FPREGS+(15 * REGSZ)(a1)
	lwc1	$f16, PCB_FPREGS+(16 * REGSZ)(a1)
	lwc1	$f17, PCB_FPREGS+(17 * REGSZ)(a1)
	lwc1	$f18, PCB_FPREGS+(18 * REGSZ)(a1)
	lwc1	$f19, PCB_FPREGS+(19 * REGSZ)(a1)
	lwc1	$f20, PCB_FPREGS+(20 * REGSZ)(a1)
	lwc1	$f21, PCB_FPREGS+(21 * REGSZ)(a1)
	lwc1	$f22, PCB_FPREGS+(22 * REGSZ)(a1)
	lwc1	$f23, PCB_FPREGS+(23 * REGSZ)(a1)
	lwc1	$f24, PCB_FPREGS+(24 * REGSZ)(a1)
	lwc1	$f25, PCB_FPREGS+(25 * REGSZ)(a1)
	lwc1	$f26, PCB_FPREGS+(26 * REGSZ)(a1)
	lwc1	$f27, PCB_FPREGS+(27 * REGSZ)(a1)
	lwc1	$f28, PCB_FPREGS+(28 * REGSZ)(a1)
	lwc1	$f29, PCB_FPREGS+(29 * REGSZ)(a1)
	lwc1	$f30, PCB_FPREGS+(30 * REGSZ)(a1)
	lwc1	$f31, PCB_FPREGS+(31 * REGSZ)(a1)

	ctc1	t0, FPC_CSR
	NOP

	MTC0	t1, COP_0_STATUS_REG	# Restore the status register.
	MTC0_SR_CU_HAZARD
	j	ra
	 NOP
END(MipsSwitchFPState16)

/*----------------------------------------------------------------------------
 *
 * MipsSaveCurFPState --
 *
 *	Save the current floating point coprocessor state.
 *
 *	MipsSaveCurFPState(p)
 *		struct proc *p;
 *
 * Results:
 *	None.
 *
 * Side effects:
 *	curcpu()->ci_fpuproc is cleared.
 *
 *----------------------------------------------------------------------------
 */
LEAF(MipsSaveCurFPState, 0)
	PTR_L	a0, P_ADDR(a0)			# get pointer to pcb for proc
	MFC0	t1, COP_0_STATUS_REG	# Disable interrupts and
	MFC0_HAZARD
	or	t0, t1, SR_COP_1_BIT|SR_FR_32	# enable the coprocessor
	MTC0	t0, COP_0_STATUS_REG
	MTC0_SR_IE_HAZARD
	GET_CPU_INFO(t2, t3)
	PTR_S	zero, CI_FPUPROC(t2)		# indicate state has been saved
/*
 * First read out the status register to make sure that all FP operations
 * have completed.
 */
	REG_L	t2, PCB_REGS+(SR * REGSZ)(a0)	# get CPU status register
	LI	t3, ~SR_COP_1_BIT
	and	t2, t2, t3			# clear COP_1 enable bit
	cfc1	t0, FPC_CSR		# stall til FP done
	cfc1	t0, FPC_CSR		# now get status
	REG_S	t2, PCB_REGS+(SR * REGSZ)(a0)	# save new status register
	REG_S	t0, PCB_FPREGS+(32 * REGSZ)(a0)	# save FP status
/*
 * Save the floating point registers.
 */
	sdc1	$f0, PCB_FPREGS+(0 * REGSZ)(a0)
	sdc1	$f1, PCB_FPREGS+(1 * REGSZ)(a0)
	sdc1	$f2, PCB_FPREGS+(2 * REGSZ)(a0)
	sdc1	$f3, PCB_FPREGS+(3 * REGSZ)(a0)
	sdc1	$f4, PCB_FPREGS+(4 * REGSZ)(a0)
	sdc1	$f5, PCB_FPREGS+(5 * REGSZ)(a0)
	sdc1	$f6, PCB_FPREGS+(6 * REGSZ)(a0)
	sdc1	$f7, PCB_FPREGS+(7 * REGSZ)(a0)
	sdc1	$f8, PCB_FPREGS+(8 * REGSZ)(a0)
	sdc1	$f9, PCB_FPREGS+(9 * REGSZ)(a0)
	sdc1	$f10, PCB_FPREGS+(10 * REGSZ)(a0)
	sdc1	$f11, PCB_FPREGS+(11 * REGSZ)(a0)
	sdc1	$f12, PCB_FPREGS+(12 * REGSZ)(a0)
	sdc1	$f13, PCB_FPREGS+(13 * REGSZ)(a0)
	sdc1	$f14, PCB_FPREGS+(14 * REGSZ)(a0)
	sdc1	$f15, PCB_FPREGS+(15 * REGSZ)(a0)
	sdc1	$f16, PCB_FPREGS+(16 * REGSZ)(a0)
	sdc1	$f17, PCB_FPREGS+(17 * REGSZ)(a0)
	sdc1	$f18, PCB_FPREGS+(18 * REGSZ)(a0)
	sdc1	$f19, PCB_FPREGS+(19 * REGSZ)(a0)
	sdc1	$f20, PCB_FPREGS+(20 * REGSZ)(a0)
	sdc1	$f21, PCB_FPREGS+(21 * REGSZ)(a0)
	sdc1	$f22, PCB_FPREGS+(22 * REGSZ)(a0)
	sdc1	$f23, PCB_FPREGS+(23 * REGSZ)(a0)
	sdc1	$f24, PCB_FPREGS+(24 * REGSZ)(a0)
	sdc1	$f25, PCB_FPREGS+(25 * REGSZ)(a0)
	sdc1	$f26, PCB_FPREGS+(26 * REGSZ)(a0)
	sdc1	$f27, PCB_FPREGS+(27 * REGSZ)(a0)
	sdc1	$f28, PCB_FPREGS+(28 * REGSZ)(a0)
	sdc1	$f29, PCB_FPREGS+(29 * REGSZ)(a0)
	sdc1	$f30, PCB_FPREGS+(30 * REGSZ)(a0)
	sdc1	$f31, PCB_FPREGS+(31 * REGSZ)(a0)

	MTC0	t1, COP_0_STATUS_REG	# Restore the status register.
	MTC0_SR_IE_HAZARD
	j	ra
	 NOP
END(MipsSaveCurFPState)

LEAF(MipsSaveCurFPState16, 0)
	PTR_L	a0, P_ADDR(a0)			# get pointer to pcb for proc
	MFC0	t1, COP_0_STATUS_REG	# Disable interrupts and
	MFC0_HAZARD
	or	t0, t1, SR_COP_1_BIT	# enable the coprocessor
	MTC0	t0, COP_0_STATUS_REG
	MTC0_SR_IE_HAZARD
	GET_CPU_INFO(t2, t3)
	PTR_S	zero, CI_FPUPROC(t2)		# indicate state has been saved
/*
 * First read out the status register to make sure that all FP operations
 * have completed.
 */
	REG_L	t2, PCB_REGS+(SR * REGSZ)(a0)	# get CPU status register
	LI	t3, ~SR_COP_1_BIT
	and	t2, t2, t3			# clear COP_1 enable bit
	cfc1	t0, FPC_CSR		# stall til FP done
	cfc1	t0, FPC_CSR		# now get status
	REG_S	t2, PCB_REGS+(SR * REGSZ)(a0)	# save new status register
	REG_S	t0, PCB_FPREGS+(32 * REGSZ)(a0)	# save FP status
/*
 * Save the floating point registers.
 */
	swc1	$f0, PCB_FPREGS+(0 * REGSZ)(a0)
	swc1	$f1, PCB_FPREGS+(1 * REGSZ)(a0)
	swc1	$f2, PCB_FPREGS+(2 * REGSZ)(a0)
	swc1	$f3, PCB_FPREGS+(3 * REGSZ)(a0)
	swc1	$f4, PCB_FPREGS+(4 * REGSZ)(a0)
	swc1	$f5, PCB_FPREGS+(5 * REGSZ)(a0)
	swc1	$f6, PCB_FPREGS+(6 * REGSZ)(a0)
	swc1	$f7, PCB_FPREGS+(7 * REGSZ)(a0)
	swc1	$f8, PCB_FPREGS+(8 * REGSZ)(a0)
	swc1	$f9, PCB_FPREGS+(9 * REGSZ)(a0)
	swc1	$f10, PCB_FPREGS+(10 * REGSZ)(a0)
	swc1	$f11, PCB_FPREGS+(11 * REGSZ)(a0)
	swc1	$f12, PCB_FPREGS+(12 * REGSZ)(a0)
	swc1	$f13, PCB_FPREGS+(13 * REGSZ)(a0)
	swc1	$f14, PCB_FPREGS+(14 * REGSZ)(a0)
	swc1	$f15, PCB_FPREGS+(15 * REGSZ)(a0)
	swc1	$f16, PCB_FPREGS+(16 * REGSZ)(a0)
	swc1	$f17, PCB_FPREGS+(17 * REGSZ)(a0)
	swc1	$f18, PCB_FPREGS+(18 * REGSZ)(a0)
	swc1	$f19, PCB_FPREGS+(19 * REGSZ)(a0)
	swc1	$f20, PCB_FPREGS+(20 * REGSZ)(a0)
	swc1	$f21, PCB_FPREGS+(21 * REGSZ)(a0)
	swc1	$f22, PCB_FPREGS+(22 * REGSZ)(a0)
	swc1	$f23, PCB_FPREGS+(23 * REGSZ)(a0)
	swc1	$f24, PCB_FPREGS+(24 * REGSZ)(a0)
	swc1	$f25, PCB_FPREGS+(25 * REGSZ)(a0)
	swc1	$f26, PCB_FPREGS+(26 * REGSZ)(a0)
	swc1	$f27, PCB_FPREGS+(27 * REGSZ)(a0)
	swc1	$f28, PCB_FPREGS+(28 * REGSZ)(a0)
	swc1	$f29, PCB_FPREGS+(29 * REGSZ)(a0)
	swc1	$f30, PCB_FPREGS+(30 * REGSZ)(a0)
	swc1	$f31, PCB_FPREGS+(31 * REGSZ)(a0)

	MTC0	t1, COP_0_STATUS_REG	# Restore the status register.
	MTC0_SR_IE_HAZARD
	j	ra
	 NOP
END(MipsSaveCurFPState16)

/*----------------------------------------------------------------------------
 *
 * cp1_get_prid
 *
 *	Get the floating point co-processor id.
 *
 *	cp1_get_prid(void)
 *
 * Results:
 *	FPC_ID
 *
 * Side effects:
 *	None.
 *
 *----------------------------------------------------------------------------
 */
LEAF(cp1_get_prid, 0)
	MFC0	v1, COP_0_STATUS_REG
	MFC0_HAZARD
	li	a0, SR_COP_1_BIT
	or	v1, a0
	MTC0	v1, COP_0_STATUS_REG
	MTC0_SR_CU_HAZARD
	cfc1	v0, FPC_ID
	xor	v1, a0
	MTC0	v1, COP_0_STATUS_REG
	MTC0_SR_CU_HAZARD
	jr	ra
	 NOP
END(cp1_get_prid)


code:
/*	$OpenBSD: mem.c,v 1.20 2015/02/10 22:44:35 miod Exp $	*/
/*	$NetBSD: mem.c,v 1.6 1995/04/10 11:55:03 mycroft 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 and Ralph Campbell.
 *
 * 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 the University of
 *	California, Berkeley and its contributors.
 * 4. 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/conf.h>
#include <sys/buf.h>
#include <sys/proc.h>
#include <sys/msgbuf.h>
#include <sys/systm.h>
#include <sys/uio.h>
#include <sys/malloc.h>

#include <machine/autoconf.h>
#include <machine/cpu.h>
#include <machine/memconf.h>

#include <uvm/uvm_extern.h>

boolean_t is_memory_range(paddr_t, psize_t, psize_t);

caddr_t zeropage;

#define mmread  mmrw
#define mmwrite mmrw
cdev_decl(mm);

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

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

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

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

	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;
			c = iov->iov_len;
			if (v + c < v || v + c > ptoa((psize_t)physmem))
				return (EFAULT);
			v = (vaddr_t)PHYS_TO_XKPHYS(v, CCA_NONCOHERENT);
			error = uiomove((caddr_t)v, c, uio);
			continue;

/* minor device 1 is kernel memory */
		case 1:
			v = uio->uio_offset;
			c = ulmin(iov->iov_len, MAXPHYS);

			/* Allow access to RAM through XKPHYS... */
			if (IS_XKPHYS(v))
				allowed = is_memory_range(XKPHYS_TO_PHYS(v),
				    (psize_t)c, 0);
			/* ...or through CKSEG0... */
			else if (v >= CKSEG0_BASE &&
			    v < CKSEG0_BASE + CKSEG_SIZE)
				allowed = is_memory_range(CKSEG0_TO_PHYS(v),
				    (psize_t)c, CKSEG_SIZE);
			/* ...or through CKSEG1... */
			else if (v >= CKSEG1_BASE &&
			    v < CKSEG1_BASE + CKSEG_SIZE)
				allowed = is_memory_range(CKSEG1_TO_PHYS(v),
				    (psize_t)c, CKSEG_SIZE);
			/* ...otherwise, check it's within kernel kvm limits. */
			else
				allowed = uvm_kernacc((caddr_t)v, c,
				    uio->uio_rw == UIO_READ ? B_READ : B_WRITE);

			if (allowed) {
				error = uiomove((caddr_t)v, c, uio);
				continue;
			} else {
				return (EFAULT);
			}

/* 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 (zeropage == NULL)
				zeropage = malloc(PAGE_SIZE, M_TEMP,
				    M_WAITOK | M_ZERO);
			c = ulmin(iov->iov_len, PAGE_SIZE);
			error = uiomove(zeropage, c, uio);
			continue;

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

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

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


code:
/*	$OpenBSD: mips64_machdep.c,v 1.19 2014/12/15 02:24:22 guenther Exp $ */

/*
 * Copyright (c) 2009, 2010, 2012 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-2004 Opsycon AB  (www.opsycon.se / www.opsycon.com)
 *
 * 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 <sys/kernel.h>
#include <sys/proc.h>
#include <sys/exec.h>
#include <sys/sysctl.h>
#include <sys/timetc.h>

#include <machine/autoconf.h>
#include <machine/cpu.h>
#include <mips64/mips_cpu.h>
#include <mips64/mips_opcode.h>

#include <uvm/uvm_extern.h>

#include <mips64/dev/clockvar.h>

/*
 * Build a tlb trampoline
 */
void
build_trampoline(vaddr_t addr, vaddr_t dest)
{
	const uint32_t insns[] = {
		0x3c1a0000,	/* lui k0, imm16 */
		0x675a0000,	/* daddiu k0, k0, imm16 */
		0x001ad438,	/* dsll k0, k0, 0x10 */
		0x675a0000,	/* daddiu k0, k0, imm16 */
		0x001ad438,	/* dsll k0, k0, 0x10 */
		0x675a0000,	/* daddiu k0, k0, imm16 */
		0x03400008,	/* jr k0 */
		0x00000000	/* nop */
	};
	uint32_t *dst = (uint32_t *)addr;
	const uint32_t *src = insns;
	uint32_t a, b, c, d;

	/*
	 * Decompose the handler address in the four components which,
	 * added with sign extension, will produce the correct address.
	 */
	d = dest & 0xffff;
	dest >>= 16;
	if (d & 0x8000)
		dest++;
	c = dest & 0xffff;
	dest >>= 16;
	if (c & 0x8000)
		dest++;
	b = dest & 0xffff;
	dest >>= 16;
	if (b & 0x8000)
		dest++;
	a = dest & 0xffff;

	/*
	 * Build the trampoline, skipping noop computations.
	 */
	*dst++ = *src++ | a;
	if (b != 0)
		*dst++ = *src++ | b;
	else
		src++;
	*dst++ = *src++;
	if (c != 0)
		*dst++ = *src++ | c;
	else
		src++;
	*dst++ = *src++;
	if (d != 0)
		*dst++ = *src++ | d;
	else
		src++;
	*dst++ = *src++;
	*dst++ = *src++;

	/*
	 * Note that we keep the delay slot instruction a nop, instead
	 * of branching to the second instruction of the handler and
	 * having its first instruction in the delay slot, so that the
	 * tlb handler is free to use k0 immediately.
	 */
}

/*
 * Prototype status registers value for userland processes.
 */
register_t protosr = SR_FR_32 | SR_XX | SR_UX | SR_KSU_USER | SR_EXL |
#ifdef CPU_R8000
    SR_SERIALIZE_FPU |
#else
    SR_KX |
#endif
    SR_INT_ENAB;

/*
 * Set registers on exec for native exec format. For o64/64.
 */
void
setregs(p, pack, stack, retval)
	struct proc *p;
	struct exec_package *pack;
	u_long stack;
	register_t *retval;
{
	struct cpu_info *ci = curcpu();

	bzero((caddr_t)p->p_md.md_regs, sizeof(struct trap_frame));
	p->p_md.md_regs->sp = stack;
	p->p_md.md_regs->pc = pack->ep_entry & ~3;
	p->p_md.md_regs->t9 = pack->ep_entry & ~3; /* abicall req */
	p->p_md.md_regs->sr = protosr | (idle_mask & SR_INT_MASK);
	p->p_md.md_regs->ic = (idle_mask << 8) & IC_INT_MASK;
#ifndef FPUEMUL
	p->p_md.md_flags &= ~MDP_FPUSED;
#endif
	if (ci->ci_fpuproc == p)
		ci->ci_fpuproc = NULL;
	p->p_md.md_ss_addr = 0;
	p->p_md.md_pc_ctrl = 0;
	p->p_md.md_watch_1 = 0;
	p->p_md.md_watch_2 = 0;

	retval[1] = 0;
}

int
exec_md_map(struct proc *p, struct exec_package *pack)
{
#ifdef FPUEMUL
	int rc;
	vaddr_t va;

	/*
	 * If we are running with FPU instruction emulation, we need
	 * to allocate a special page in the process' address space,
	 * in order to be able to emulate delay slot instructions of
	 * successful conditional branches.
	 */

	va = 0;
	rc = uvm_map(&p->p_vmspace->vm_map, &va, PAGE_SIZE, NULL,
	    UVM_UNKNOWN_OFFSET, 0,
	    UVM_MAPFLAG(PROT_NONE, PROT_MASK, MAP_INHERIT_COPY,
	      MADV_NORMAL, UVM_FLAG_COPYONW));
	if (rc != 0)
		return rc;
#ifdef DEBUG
	printf("%s: p %p fppgva %p\n", __func__, p, va);
#endif
	p->p_md.md_fppgva = va;
#endif

	return 0;
}

/*
 * Initial TLB setup for the current processor.
 */
void
tlb_init(unsigned int tlbsize)
{
#ifdef CPU_R8000
	register_t sr;

	sr = getsr();
	sr &= ~(((uint64_t)SR_PGSZ_MASK << SR_KPGSZ_SHIFT) |
	        ((uint64_t)SR_PGSZ_MASK << SR_UPGSZ_SHIFT));
	sr |= ((uint64_t)SR_PGSZ_16K << SR_KPGSZ_SHIFT) |
	    ((uint64_t)SR_PGSZ_16K << SR_UPGSZ_SHIFT);
	protosr |= ((uint64_t)SR_PGSZ_16K << SR_KPGSZ_SHIFT) |
	    ((uint64_t)SR_PGSZ_16K << SR_UPGSZ_SHIFT);
	setsr(sr);
#else
	tlb_set_page_mask(TLB_PAGE_MASK);
#endif
	tlb_set_wired(0);
	tlb_flush(tlbsize);
#if UPAGES > 1
	tlb_set_wired(UPAGES / 2);
#endif
}

/*
 * Handle an ASID wrap.
 */
void
tlb_asid_wrap(struct cpu_info *ci)
{
	tlb_flush(ci->ci_hw.tlbsize);
#ifdef CPU_R8000
	Mips_InvalidateICache(ci, 0, ci->ci_l1inst.size);
#endif
}

/*
 *	Mips machine independent clock routines.
 */

struct tod_desc sys_tod;
void (*md_startclock)(struct cpu_info *);

/*
 * Wait "n" microseconds.
 */
void
delay(int n)
{
	int dly;
	int p, c;
	struct cpu_info *ci = curcpu();
	uint32_t delayconst;

	delayconst = ci->ci_delayconst;
	if (delayconst == 0)
		delayconst = bootcpu_hwinfo.clock / CP0_CYCLE_DIVIDER;
	p = cp0_get_count();
	dly = (delayconst / 1000000) * n;
	while (dly > 0) {
		c = cp0_get_count();
		dly -= c - p;
		p = c;
	}
}

#ifndef MULTIPROCESSOR
u_int cp0_get_timecount(struct timecounter *);

struct timecounter cp0_timecounter = {
	cp0_get_timecount,	/* get_timecount */
	0,			/* no poll_pps */
	0xffffffff,		/* counter_mask */
	0,			/* frequency */
	"CP0",			/* name */
	0			/* quality */
};

u_int
cp0_get_timecount(struct timecounter *tc)
{
	return (cp0_get_count());
}
#endif

/*
 * Calibrate cpu internal counter against the TOD clock if available.
 */
void
cp0_calibrate(struct cpu_info *ci)
{
	struct tod_desc *cd = &sys_tod;
	struct tod_time ct;
	u_int first_cp0, second_cp0, cycles_per_sec;
	int first_sec;

	if (cd->tod_get == NULL)
		return;

	(*cd->tod_get)(cd->tod_cookie, 0, &ct);
	first_sec = ct.sec;

	/* Let the clock tick one second. */
	do {
		first_cp0 = cp0_get_count();
		(*cd->tod_get)(cd->tod_cookie, 0, &ct);
	} while (ct.sec == first_sec);
	first_sec = ct.sec;
	/* Let the clock tick one more second. */
	do {
		second_cp0 = cp0_get_count();
		(*cd->tod_get)(cd->tod_cookie, 0, &ct);
	} while (ct.sec == first_sec);

	cycles_per_sec = second_cp0 - first_cp0;
	ci->ci_hw.clock = cycles_per_sec * CP0_CYCLE_DIVIDER;
	ci->ci_delayconst = cycles_per_sec;
}

/*
 * Start the real-time and statistics clocks.
 */
void
cpu_initclocks()
{
	struct cpu_info *ci = curcpu();

	profhz = hz;

	tick = 1000000 / hz;	/* number of micro-seconds between interrupts */
	tickadj = 240000 / (60 * hz);		/* can adjust 240ms in 60s */

	cp0_calibrate(ci);

#ifndef MULTIPROCESSOR
	if (cpu_setperf == NULL) {
		cp0_timecounter.tc_frequency =
		    (uint64_t)ci->ci_hw.clock / CP0_CYCLE_DIVIDER;
		tc_init(&cp0_timecounter);
	}
#endif

#ifdef DIAGNOSTIC
	if (md_startclock == NULL)
		panic("no clock");
#endif
	(*md_startclock)(ci);
}

/*
 * We assume newhz is either stathz or profhz, and that neither will
 * change after being set up above.  Could recalculate intervals here
 * but that would be a drag.
 */
void
setstatclockrate(int newhz)
{
}

/* XXX switch to kern/clock_subr.c routines */
/*
 * This code is defunct after 2099. Will Unix still be here then??
 */
static short dayyr[12] = {
	0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334
};

#define	SECMIN	(60)		/* seconds per minute */
#define	SECHOUR	(60*SECMIN)	/* seconds per hour */

#define	YEARDAYS(year)	(((((year) + 1900) % 4) == 0 && \
			 ((((year) + 1900) % 100) != 0 || \
			  (((year) + 1900) % 400) == 0)) ? 366 : 365)

/*
 * Initialize the time of day register, based on the time base which
 * is, e.g. from a filesystem.
 */
void
inittodr(time_t base)
{
	struct timespec ts;
	struct tod_time c;
	struct tod_desc *cd = &sys_tod;
	int days, yr;

	ts.tv_nsec = 0;

	if (base < 35 * SECYR) {
		printf("WARNING: preposterous time in file system");
		/* read the system clock anyway */
		base = 40 * SECYR;	/* 2010 */
	}

	/*
	 * Read RTC chip registers NOTE: Read routines are responsible
	 * for sanity checking clock. Dates after 19991231 should be
	 * returned as year >= 100.
	 */
	if (cd->tod_get) {
		(*cd->tod_get)(cd->tod_cookie, base, &c);
	} else {
		printf("WARNING: No TOD clock, believing file system.\n");
		goto bad;
	}

	days = 0;
	for (yr = 70; yr < c.year; yr++) {
		days += YEARDAYS(yr);
	}

	days += dayyr[c.mon - 1] + c.day - 1;
	if (YEARDAYS(c.year) == 366 && c.mon > 2) {
		days++;
	}

	/* now have days since Jan 1, 1970; the rest is easy... */
	ts.tv_sec = days * SECDAY + c.hour * 3600 + c.min * 60 + c.sec;

	/*
	 * See if we gained/lost time.
	 */
	if (base < ts.tv_sec - 5*SECYR) {
		printf("WARNING: file system time much less than clock time\n");
	} else if (base > ts.tv_sec + 5*SECYR) {
		printf("WARNING: clock time much less than file system time\n");
		printf("WARNING: using file system time\n");
	} else {
		tc_setclock(&ts);
		cd->tod_valid = 1;
		return;
	}

bad:
	ts.tv_sec = base;
	tc_setclock(&ts);
	cd->tod_valid = 1;
	printf("WARNING: CHECK AND RESET THE DATE!\n");
}

/*
 * Reset the TOD clock. This is done when the system is halted or
 * when the time is reset by the stime system call.
 */
void
resettodr()
{
	struct tod_time c;
	struct tod_desc *cd = &sys_tod;
	int t, t2;

	/*
	 *  Don't reset TOD if time has not been set!
	 */
	if (!cd->tod_valid)
		return;

	/* compute the day of week. 1 is Sunday*/
	t2 = time_second / SECDAY;
	c.dow = (t2 + 5) % 7 + 1;	/* 1/1/1970 was thursday */

	/* compute the year */
	t = 0;
	t2 = time_second / SECDAY;
	c.year = 69;
	while (t2 >= 0) {	/* whittle off years */
		t = t2;
		c.year++;
		t2 -= YEARDAYS(c.year);
	}

	/* t = month + day; separate */
	t2 = YEARDAYS(c.year);
	for (c.mon = 1; c.mon < 12; c.mon++) {
		if (t < dayyr[c.mon] + (t2 == 366 && c.mon > 1))
			break;
	}

	c.day = t - dayyr[c.mon - 1] + 1;
	if (t2 == 366 && c.mon > 2) {
		c.day--;
	}

	t = time_second % SECDAY;
	c.hour = t / 3600;
	t %= 3600;
	c.min = t / 60;
	c.sec = t % 60;

	if (cd->tod_set)
		(*cd->tod_set)(cd->tod_cookie, &c);
}

/*
 * Decode instruction and figure out type.
 */
int
classify_insn(uint32_t insn)
{
	InstFmt	inst;

	inst.word = insn;
	switch (inst.JType.op) {
	case OP_SPECIAL:
		switch (inst.RType.func) {
		case OP_JR:
			return INSNCLASS_BRANCH;
		case OP_JALR:
			return INSNCLASS_CALL;
		}
		break;

	case OP_BCOND:
		switch (inst.IType.rt) {
		case OP_BLTZ:
		case OP_BLTZL:
		case OP_BGEZ:
		case OP_BGEZL:
			return INSNCLASS_BRANCH;
		case OP_BLTZAL:
		case OP_BLTZALL:
		case OP_BGEZAL:
		case OP_BGEZALL:
			return INSNCLASS_CALL;
		}
		break;

	case OP_JAL:
		return INSNCLASS_CALL;

	case OP_J:
	case OP_BEQ:
	case OP_BEQL:
	case OP_BNE:
	case OP_BNEL:
	case OP_BLEZ:
	case OP_BLEZL:
	case OP_BGTZ:
	case OP_BGTZL:
		return INSNCLASS_BRANCH;

	case OP_COP1:
		switch (inst.RType.rs) {
		case OP_BC:
			return INSNCLASS_BRANCH;
		}
		break;
	}

	return INSNCLASS_NEUTRAL;
}