fpu_entry.c

freebsd v4.4内核源码

/*	  status_word &= ~SW_Summary; ****/

			/* We need to simulate the action of the kernel to FPU
			 * interrupts here. Currently, the "real FPU" part of
			 * the kernel (0.99.10) clears the exception flags,
			 * sets the registers to empty, and passes information
			 * back to the interrupted process via the cs selector
			 * and operand selector, so we do the same. */
	do_the_FPU_interrupt:
			cs_selector &= 0xffff0000;
			cs_selector |= (status_word & ~SW_Top) | ((top & 7) << SW_Top_Shift);
			operand_selector = tag_word();
			status_word = 0;
			top = 0;
			{
				int     r;
				for (r = 0; r < 8; r++) {
					regs[r].tag = TW_Empty;
				}
			}
			REENTRANT_CHECK(OFF);
			math_abort(SIGFPE);
		}
	}
	FPU_entry_eip = FPU_ORIG_EIP = FPU_EIP;

	if ((code & 0xff) == 0x66) {	/* size prefix */
		FPU_EIP++;
		REENTRANT_CHECK(OFF);
		code = fuword((u_int *) FPU_EIP);
		REENTRANT_CHECK(ON);
	}
	FPU_EIP += 2;

	FPU_modrm = code >> 8;
	FPU_rm = FPU_modrm & 7;

	if (FPU_modrm < 0300) {
		/* All of these instructions use the mod/rm byte to get a data
		 * address */
		get_address(FPU_modrm);
		if (!(code & 1)) {
			unsigned short status1 = status_word;
			FPU_st0_ptr = &st(0);
			FPU_st0_tag = FPU_st0_ptr->tag;

			/* Stack underflow has priority */
			if (NOT_EMPTY_0) {
				switch ((code >> 1) & 3) {
				case 0:
					reg_load_single();
					break;
				case 1:
					reg_load_int32();
					break;
				case 2:
					reg_load_double();
					break;
				case 3:
					reg_load_int16();
					break;
				}

				/* No more access to user memory, it is safe
				 * to use static data now */
				FPU_st0_ptr = &st(0);
				FPU_st0_tag = FPU_st0_ptr->tag;

				/* NaN operands have the next priority. */
				/* We have to delay looking at st(0) until
				 * after loading the data, because that data
				 * might contain an SNaN */
				if ((FPU_st0_tag == TW_NaN) ||
				    (FPU_loaded_data.tag == TW_NaN)) {
					/* Restore the status word; we might
					 * have loaded a denormal. */
					status_word = status1;
					if ((FPU_modrm & 0x30) == 0x10) {
						/* fcom or fcomp */
						EXCEPTION(EX_Invalid);
						setcc(SW_C3 | SW_C2 | SW_C0);
						if (FPU_modrm & 0x08)
							pop();	/* fcomp, so we pop. */
					} else
						real_2op_NaN(FPU_st0_ptr, &FPU_loaded_data, FPU_st0_ptr);
					goto reg_mem_instr_done;
				}
				switch ((FPU_modrm >> 3) & 7) {
				case 0:	/* fadd */
					reg_add(FPU_st0_ptr, &FPU_loaded_data, FPU_st0_ptr, control_word);
					break;
				case 1:	/* fmul */
					reg_mul(FPU_st0_ptr, &FPU_loaded_data, FPU_st0_ptr, control_word);
					break;
				case 2:	/* fcom */
					compare_st_data();
					break;
				case 3:	/* fcomp */
					compare_st_data();
					pop();
					break;
				case 4:	/* fsub */
					reg_sub(FPU_st0_ptr, &FPU_loaded_data, FPU_st0_ptr, control_word);
					break;
				case 5:	/* fsubr */
					reg_sub(&FPU_loaded_data, FPU_st0_ptr, FPU_st0_ptr, control_word);
					break;
				case 6:	/* fdiv */
					reg_div(FPU_st0_ptr, &FPU_loaded_data, FPU_st0_ptr, control_word);
					break;
				case 7:	/* fdivr */
					if (FPU_st0_tag == TW_Zero)
						status_word = status1;	/* Undo any denorm tag,
									 * zero-divide has
									 * priority. */
					reg_div(&FPU_loaded_data, FPU_st0_ptr, FPU_st0_ptr, control_word);
					break;
				}
			} else {
				if ((FPU_modrm & 0x30) == 0x10) {
					/* The instruction is fcom or fcomp */
					EXCEPTION(EX_StackUnder);
					setcc(SW_C3 | SW_C2 | SW_C0);
					if (FPU_modrm & 0x08)
						pop();	/* fcomp, Empty or not,
							 * we pop. */
				} else
					stack_underflow();
			}
		} else {
			load_store_instr(((FPU_modrm & 0x38) | (code & 6)) >> 1);
		}

reg_mem_instr_done:

		data_operand_offset = (unsigned long) FPU_data_address;
	} else {
		/* None of these instructions access user memory */
		unsigned char instr_index = (FPU_modrm & 0x38) | (code & 7);

		FPU_st0_ptr = &st(0);
		FPU_st0_tag = FPU_st0_ptr->tag;
		switch (type_table[(int) instr_index]) {
		case _NONE_:	/* also _REGIc: _REGIn */
			break;
		case _REG0_:
			if (!NOT_EMPTY_0) {
				stack_underflow();
				goto FPU_instruction_done;
			}
			break;
		case _REGIi:
			if (!NOT_EMPTY_0 || !NOT_EMPTY(FPU_rm)) {
				stack_underflow_i(FPU_rm);
				goto FPU_instruction_done;
			}
			break;
		case _REGIp:
			if (!NOT_EMPTY_0 || !NOT_EMPTY(FPU_rm)) {
				stack_underflow_i(FPU_rm);
				pop();
				goto FPU_instruction_done;
			}
			break;
		case _REGI_:
			if (!NOT_EMPTY_0 || !NOT_EMPTY(FPU_rm)) {
				stack_underflow();
				goto FPU_instruction_done;
			}
			break;
		case _PUSH_:	/* Only used by the fld st(i) instruction */
			break;
		case _null_:
			Un_impl();
			goto FPU_instruction_done;
		default:
			EXCEPTION(EX_INTERNAL | 0x111);
			goto FPU_instruction_done;
		}
		(*st_instr_table[(int) instr_index]) ();
	}

FPU_instruction_done:

	ip_offset = FPU_entry_eip;
	bswapw(code);
	*(1 + (unsigned short *) &cs_selector) = code & 0x7ff;

#ifdef DEBUG
	REENTRANT_CHECK(OFF);
	emu_printall();
	REENTRANT_CHECK(ON);
#endif				/* DEBUG */
#ifdef LOOKAHEAD_LIMIT
if (--lookahead_limit)
#endif
	if (FPU_lookahead) {
		unsigned char next;

		/* (This test should generate no machine code) */
		while (1) {
			REENTRANT_CHECK(OFF);
			next = fubyte((u_char *) FPU_EIP);
			REENTRANT_CHECK(ON);
			if (((next & 0xf8) == 0xd8) || (next == 0x9b)) {	/* fwait */
				goto do_another_FPU_instruction;
			} else
				if (next == 0x66) {	/* size prefix */
					REENTRANT_CHECK(OFF);
					next = fubyte((u_char *) (FPU_EIP + 1));
					REENTRANT_CHECK(ON);
					if ((next & 0xf8) == 0xd8) {
						FPU_EIP++;
						goto do_another_FPU_instruction;
					}
				}
			break;
		}
	}
	REENTRANT_CHECK(OFF);
	return (0);		/* --pink-- */
}

#ifdef LKM
MOD_MISC(gnufpu);
static int
gnufpu_load(struct lkm_table *lkmtp, int cmd)
{
	if (pmath_emulate) {
		printf("Math emulator already present\n");
		return EBUSY;
	}
	pmath_emulate = math_emulate;
	return 0;
}

static int
gnufpu_unload(struct lkm_table *lkmtp, int cmd)
{
	if (pmath_emulate != math_emulate) {
		printf("Cannot unload another math emulator\n");
		return EACCES;
	}
	pmath_emulate = 0;
	return 0;
}

int
gnufpu(struct lkm_table *lkmtp, int cmd, int ver)
{
	DISPATCH(lkmtp, cmd, ver, gnufpu_load, gnufpu_unload, lkm_nullcmd);
}
#else /* !LKM */

static void
gnufpu_init(void *unused)
{
	if (pmath_emulate)
		printf("Another Math emulator already present\n");
	else
		pmath_emulate = math_emulate;
}

SYSINIT(gnufpu, SI_SUB_CPU, SI_ORDER_ANY, gnufpu_init, NULL);

#endif /* LKM */