int do_spe_mathemu()

in math-emu/math_efp.c [176:720]


int do_spe_mathemu(struct pt_regs *regs)
{
	FP_DECL_EX;
	int IR, cmp;

	unsigned long type, func, fc, fa, fb, src, speinsn;
	union dw_union vc, va, vb;

	if (get_user(speinsn, (unsigned int __user *) regs->nip))
		return -EFAULT;
	if ((speinsn >> 26) != EFAPU)
		return -EINVAL;         /* not an spe instruction */

	type = insn_type(speinsn);
	if (type == NOTYPE)
		goto illegal;

	func = speinsn & 0x7ff;
	fc = (speinsn >> 21) & 0x1f;
	fa = (speinsn >> 16) & 0x1f;
	fb = (speinsn >> 11) & 0x1f;
	src = (speinsn >> 5) & 0x7;

	vc.wp[0] = current->thread.evr[fc];
	vc.wp[1] = regs->gpr[fc];
	va.wp[0] = current->thread.evr[fa];
	va.wp[1] = regs->gpr[fa];
	vb.wp[0] = current->thread.evr[fb];
	vb.wp[1] = regs->gpr[fb];

	__FPU_FPSCR = mfspr(SPRN_SPEFSCR);

	pr_debug("speinsn:%08lx spefscr:%08lx\n", speinsn, __FPU_FPSCR);
	pr_debug("vc: %08x  %08x\n", vc.wp[0], vc.wp[1]);
	pr_debug("va: %08x  %08x\n", va.wp[0], va.wp[1]);
	pr_debug("vb: %08x  %08x\n", vb.wp[0], vb.wp[1]);

	switch (src) {
	case SPFP: {
		FP_DECL_S(SA); FP_DECL_S(SB); FP_DECL_S(SR);

		switch (type) {
		case AB:
		case XCR:
			FP_UNPACK_SP(SA, va.wp + 1);
		case XB:
			FP_UNPACK_SP(SB, vb.wp + 1);
			break;
		case XA:
			FP_UNPACK_SP(SA, va.wp + 1);
			break;
		}

		pr_debug("SA: %ld %08lx %ld (%ld)\n", SA_s, SA_f, SA_e, SA_c);
		pr_debug("SB: %ld %08lx %ld (%ld)\n", SB_s, SB_f, SB_e, SB_c);

		switch (func) {
		case EFSABS:
			vc.wp[1] = va.wp[1] & ~SIGN_BIT_S;
			goto update_regs;

		case EFSNABS:
			vc.wp[1] = va.wp[1] | SIGN_BIT_S;
			goto update_regs;

		case EFSNEG:
			vc.wp[1] = va.wp[1] ^ SIGN_BIT_S;
			goto update_regs;

		case EFSADD:
			FP_ADD_S(SR, SA, SB);
			goto pack_s;

		case EFSSUB:
			FP_SUB_S(SR, SA, SB);
			goto pack_s;

		case EFSMUL:
			FP_MUL_S(SR, SA, SB);
			goto pack_s;

		case EFSDIV:
			FP_DIV_S(SR, SA, SB);
			goto pack_s;

		case EFSCMPEQ:
			cmp = 0;
			goto cmp_s;

		case EFSCMPGT:
			cmp = 1;
			goto cmp_s;

		case EFSCMPLT:
			cmp = -1;
			goto cmp_s;

		case EFSCTSF:
		case EFSCTUF:
			if (SB_c == FP_CLS_NAN) {
				vc.wp[1] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				SB_e += (func == EFSCTSF ? 31 : 32);
				FP_TO_INT_ROUND_S(vc.wp[1], SB, 32,
						(func == EFSCTSF));
			}
			goto update_regs;

		case EFSCFD: {
			FP_DECL_D(DB);
			FP_CLEAR_EXCEPTIONS;
			FP_UNPACK_DP(DB, vb.dp);

			pr_debug("DB: %ld %08lx %08lx %ld (%ld)\n",
					DB_s, DB_f1, DB_f0, DB_e, DB_c);

			FP_CONV(S, D, 1, 2, SR, DB);
			goto pack_s;
		}

		case EFSCTSI:
		case EFSCTUI:
			if (SB_c == FP_CLS_NAN) {
				vc.wp[1] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				FP_TO_INT_ROUND_S(vc.wp[1], SB, 32,
						((func & 0x3) != 0));
			}
			goto update_regs;

		case EFSCTSIZ:
		case EFSCTUIZ:
			if (SB_c == FP_CLS_NAN) {
				vc.wp[1] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				FP_TO_INT_S(vc.wp[1], SB, 32,
						((func & 0x3) != 0));
			}
			goto update_regs;

		default:
			goto illegal;
		}
		break;

pack_s:
		pr_debug("SR: %ld %08lx %ld (%ld)\n", SR_s, SR_f, SR_e, SR_c);

		FP_PACK_SP(vc.wp + 1, SR);
		goto update_regs;

cmp_s:
		FP_CMP_S(IR, SA, SB, 3);
		if (IR == 3 && (FP_ISSIGNAN_S(SA) || FP_ISSIGNAN_S(SB)))
			FP_SET_EXCEPTION(FP_EX_INVALID);
		if (IR == cmp) {
			IR = 0x4;
		} else {
			IR = 0;
		}
		goto update_ccr;
	}

	case DPFP: {
		FP_DECL_D(DA); FP_DECL_D(DB); FP_DECL_D(DR);

		switch (type) {
		case AB:
		case XCR:
			FP_UNPACK_DP(DA, va.dp);
		case XB:
			FP_UNPACK_DP(DB, vb.dp);
			break;
		case XA:
			FP_UNPACK_DP(DA, va.dp);
			break;
		}

		pr_debug("DA: %ld %08lx %08lx %ld (%ld)\n",
				DA_s, DA_f1, DA_f0, DA_e, DA_c);
		pr_debug("DB: %ld %08lx %08lx %ld (%ld)\n",
				DB_s, DB_f1, DB_f0, DB_e, DB_c);

		switch (func) {
		case EFDABS:
			vc.dp[0] = va.dp[0] & ~SIGN_BIT_D;
			goto update_regs;

		case EFDNABS:
			vc.dp[0] = va.dp[0] | SIGN_BIT_D;
			goto update_regs;

		case EFDNEG:
			vc.dp[0] = va.dp[0] ^ SIGN_BIT_D;
			goto update_regs;

		case EFDADD:
			FP_ADD_D(DR, DA, DB);
			goto pack_d;

		case EFDSUB:
			FP_SUB_D(DR, DA, DB);
			goto pack_d;

		case EFDMUL:
			FP_MUL_D(DR, DA, DB);
			goto pack_d;

		case EFDDIV:
			FP_DIV_D(DR, DA, DB);
			goto pack_d;

		case EFDCMPEQ:
			cmp = 0;
			goto cmp_d;

		case EFDCMPGT:
			cmp = 1;
			goto cmp_d;

		case EFDCMPLT:
			cmp = -1;
			goto cmp_d;

		case EFDCTSF:
		case EFDCTUF:
			if (DB_c == FP_CLS_NAN) {
				vc.wp[1] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				DB_e += (func == EFDCTSF ? 31 : 32);
				FP_TO_INT_ROUND_D(vc.wp[1], DB, 32,
						(func == EFDCTSF));
			}
			goto update_regs;

		case EFDCFS: {
			FP_DECL_S(SB);
			FP_CLEAR_EXCEPTIONS;
			FP_UNPACK_SP(SB, vb.wp + 1);

			pr_debug("SB: %ld %08lx %ld (%ld)\n",
					SB_s, SB_f, SB_e, SB_c);

			FP_CONV(D, S, 2, 1, DR, SB);
			goto pack_d;
		}

		case EFDCTUIDZ:
		case EFDCTSIDZ:
			if (DB_c == FP_CLS_NAN) {
				vc.dp[0] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				FP_TO_INT_D(vc.dp[0], DB, 64,
						((func & 0x1) == 0));
			}
			goto update_regs;

		case EFDCTUI:
		case EFDCTSI:
			if (DB_c == FP_CLS_NAN) {
				vc.wp[1] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				FP_TO_INT_ROUND_D(vc.wp[1], DB, 32,
						((func & 0x3) != 0));
			}
			goto update_regs;

		case EFDCTUIZ:
		case EFDCTSIZ:
			if (DB_c == FP_CLS_NAN) {
				vc.wp[1] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				FP_TO_INT_D(vc.wp[1], DB, 32,
						((func & 0x3) != 0));
			}
			goto update_regs;

		default:
			goto illegal;
		}
		break;

pack_d:
		pr_debug("DR: %ld %08lx %08lx %ld (%ld)\n",
				DR_s, DR_f1, DR_f0, DR_e, DR_c);

		FP_PACK_DP(vc.dp, DR);
		goto update_regs;

cmp_d:
		FP_CMP_D(IR, DA, DB, 3);
		if (IR == 3 && (FP_ISSIGNAN_D(DA) || FP_ISSIGNAN_D(DB)))
			FP_SET_EXCEPTION(FP_EX_INVALID);
		if (IR == cmp) {
			IR = 0x4;
		} else {
			IR = 0;
		}
		goto update_ccr;

	}

	case VCT: {
		FP_DECL_S(SA0); FP_DECL_S(SB0); FP_DECL_S(SR0);
		FP_DECL_S(SA1); FP_DECL_S(SB1); FP_DECL_S(SR1);
		int IR0, IR1;

		switch (type) {
		case AB:
		case XCR:
			FP_UNPACK_SP(SA0, va.wp);
			FP_UNPACK_SP(SA1, va.wp + 1);
		case XB:
			FP_UNPACK_SP(SB0, vb.wp);
			FP_UNPACK_SP(SB1, vb.wp + 1);
			break;
		case XA:
			FP_UNPACK_SP(SA0, va.wp);
			FP_UNPACK_SP(SA1, va.wp + 1);
			break;
		}

		pr_debug("SA0: %ld %08lx %ld (%ld)\n",
				SA0_s, SA0_f, SA0_e, SA0_c);
		pr_debug("SA1: %ld %08lx %ld (%ld)\n",
				SA1_s, SA1_f, SA1_e, SA1_c);
		pr_debug("SB0: %ld %08lx %ld (%ld)\n",
				SB0_s, SB0_f, SB0_e, SB0_c);
		pr_debug("SB1: %ld %08lx %ld (%ld)\n",
				SB1_s, SB1_f, SB1_e, SB1_c);

		switch (func) {
		case EVFSABS:
			vc.wp[0] = va.wp[0] & ~SIGN_BIT_S;
			vc.wp[1] = va.wp[1] & ~SIGN_BIT_S;
			goto update_regs;

		case EVFSNABS:
			vc.wp[0] = va.wp[0] | SIGN_BIT_S;
			vc.wp[1] = va.wp[1] | SIGN_BIT_S;
			goto update_regs;

		case EVFSNEG:
			vc.wp[0] = va.wp[0] ^ SIGN_BIT_S;
			vc.wp[1] = va.wp[1] ^ SIGN_BIT_S;
			goto update_regs;

		case EVFSADD:
			FP_ADD_S(SR0, SA0, SB0);
			FP_ADD_S(SR1, SA1, SB1);
			goto pack_vs;

		case EVFSSUB:
			FP_SUB_S(SR0, SA0, SB0);
			FP_SUB_S(SR1, SA1, SB1);
			goto pack_vs;

		case EVFSMUL:
			FP_MUL_S(SR0, SA0, SB0);
			FP_MUL_S(SR1, SA1, SB1);
			goto pack_vs;

		case EVFSDIV:
			FP_DIV_S(SR0, SA0, SB0);
			FP_DIV_S(SR1, SA1, SB1);
			goto pack_vs;

		case EVFSCMPEQ:
			cmp = 0;
			goto cmp_vs;

		case EVFSCMPGT:
			cmp = 1;
			goto cmp_vs;

		case EVFSCMPLT:
			cmp = -1;
			goto cmp_vs;

		case EVFSCTUF:
		case EVFSCTSF:
			if (SB0_c == FP_CLS_NAN) {
				vc.wp[0] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				SB0_e += (func == EVFSCTSF ? 31 : 32);
				FP_TO_INT_ROUND_S(vc.wp[0], SB0, 32,
						(func == EVFSCTSF));
			}
			if (SB1_c == FP_CLS_NAN) {
				vc.wp[1] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				SB1_e += (func == EVFSCTSF ? 31 : 32);
				FP_TO_INT_ROUND_S(vc.wp[1], SB1, 32,
						(func == EVFSCTSF));
			}
			goto update_regs;

		case EVFSCTUI:
		case EVFSCTSI:
			if (SB0_c == FP_CLS_NAN) {
				vc.wp[0] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				FP_TO_INT_ROUND_S(vc.wp[0], SB0, 32,
						((func & 0x3) != 0));
			}
			if (SB1_c == FP_CLS_NAN) {
				vc.wp[1] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				FP_TO_INT_ROUND_S(vc.wp[1], SB1, 32,
						((func & 0x3) != 0));
			}
			goto update_regs;

		case EVFSCTUIZ:
		case EVFSCTSIZ:
			if (SB0_c == FP_CLS_NAN) {
				vc.wp[0] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				FP_TO_INT_S(vc.wp[0], SB0, 32,
						((func & 0x3) != 0));
			}
			if (SB1_c == FP_CLS_NAN) {
				vc.wp[1] = 0;
				FP_SET_EXCEPTION(FP_EX_INVALID);
			} else {
				FP_TO_INT_S(vc.wp[1], SB1, 32,
						((func & 0x3) != 0));
			}
			goto update_regs;

		default:
			goto illegal;
		}
		break;

pack_vs:
		pr_debug("SR0: %ld %08lx %ld (%ld)\n",
				SR0_s, SR0_f, SR0_e, SR0_c);
		pr_debug("SR1: %ld %08lx %ld (%ld)\n",
				SR1_s, SR1_f, SR1_e, SR1_c);

		FP_PACK_SP(vc.wp, SR0);
		FP_PACK_SP(vc.wp + 1, SR1);
		goto update_regs;

cmp_vs:
		{
			int ch, cl;

			FP_CMP_S(IR0, SA0, SB0, 3);
			FP_CMP_S(IR1, SA1, SB1, 3);
			if (IR0 == 3 && (FP_ISSIGNAN_S(SA0) || FP_ISSIGNAN_S(SB0)))
				FP_SET_EXCEPTION(FP_EX_INVALID);
			if (IR1 == 3 && (FP_ISSIGNAN_S(SA1) || FP_ISSIGNAN_S(SB1)))
				FP_SET_EXCEPTION(FP_EX_INVALID);
			ch = (IR0 == cmp) ? 1 : 0;
			cl = (IR1 == cmp) ? 1 : 0;
			IR = (ch << 3) | (cl << 2) | ((ch | cl) << 1) |
				((ch & cl) << 0);
			goto update_ccr;
		}
	}
	default:
		return -EINVAL;
	}

update_ccr:
	regs->ccr &= ~(15 << ((7 - ((speinsn >> 23) & 0x7)) << 2));
	regs->ccr |= (IR << ((7 - ((speinsn >> 23) & 0x7)) << 2));

update_regs:
	/*
	 * If the "invalid" exception sticky bit was set by the
	 * processor for non-finite input, but was not set before the
	 * instruction being emulated, clear it.  Likewise for the
	 * "underflow" bit, which may have been set by the processor
	 * for exact underflow, not just inexact underflow when the
	 * flag should be set for IEEE 754 semantics.  Other sticky
	 * exceptions will only be set by the processor when they are
	 * correct according to IEEE 754 semantics, and we must not
	 * clear sticky bits that were already set before the emulated
	 * instruction as they represent the user-visible sticky
	 * exception status.  "inexact" traps to kernel are not
	 * required for IEEE semantics and are not enabled by default,
	 * so the "inexact" sticky bit may have been set by a previous
	 * instruction without the kernel being aware of it.
	 */
	__FPU_FPSCR
	  &= ~(FP_EX_INVALID | FP_EX_UNDERFLOW) | current->thread.spefscr_last;
	__FPU_FPSCR |= (FP_CUR_EXCEPTIONS & FP_EX_MASK);
	mtspr(SPRN_SPEFSCR, __FPU_FPSCR);
	current->thread.spefscr_last = __FPU_FPSCR;

	current->thread.evr[fc] = vc.wp[0];
	regs->gpr[fc] = vc.wp[1];

	pr_debug("ccr = %08lx\n", regs->ccr);
	pr_debug("cur exceptions = %08x spefscr = %08lx\n",
			FP_CUR_EXCEPTIONS, __FPU_FPSCR);
	pr_debug("vc: %08x  %08x\n", vc.wp[0], vc.wp[1]);
	pr_debug("va: %08x  %08x\n", va.wp[0], va.wp[1]);
	pr_debug("vb: %08x  %08x\n", vb.wp[0], vb.wp[1]);

	if (current->thread.fpexc_mode & PR_FP_EXC_SW_ENABLE) {
		if ((FP_CUR_EXCEPTIONS & FP_EX_DIVZERO)
		    && (current->thread.fpexc_mode & PR_FP_EXC_DIV))
			return 1;
		if ((FP_CUR_EXCEPTIONS & FP_EX_OVERFLOW)
		    && (current->thread.fpexc_mode & PR_FP_EXC_OVF))
			return 1;
		if ((FP_CUR_EXCEPTIONS & FP_EX_UNDERFLOW)
		    && (current->thread.fpexc_mode & PR_FP_EXC_UND))
			return 1;
		if ((FP_CUR_EXCEPTIONS & FP_EX_INEXACT)
		    && (current->thread.fpexc_mode & PR_FP_EXC_RES))
			return 1;
		if ((FP_CUR_EXCEPTIONS & FP_EX_INVALID)
		    && (current->thread.fpexc_mode & PR_FP_EXC_INV))
			return 1;
	}
	return 0;

illegal:
	if (have_e500_cpu_a005_erratum) {
		/* according to e500 cpu a005 erratum, reissue efp inst */
		regs_add_return_ip(regs, -4);
		pr_debug("re-issue efp inst: %08lx\n", speinsn);
		return 0;
	}

	printk(KERN_ERR "\nOoops! IEEE-754 compliance handler encountered un-supported instruction.\ninst code: %08lx\n", speinsn);
	return -ENOSYS;
}