#include <platforms.h>
#include <mach/exception_types.h>
#include <mach/i386/thread_status.h>
#include <mach/i386/fp_reg.h>
#include <kern/mach_param.h>
#include <kern/processor.h>
#include <kern/thread.h>
#include <kern/zalloc.h>
#include <kern/misc_protos.h>
#include <kern/spl.h>
#include <kern/assert.h>
#include <i386/thread.h>
#include <i386/fpu.h>
#include <i386/trap.h>
#include <i386/pio.h>
#include <i386/cpuid.h>
#include <i386/misc_protos.h>
#if 0
#include <i386/ipl.h>
extern int curr_ipl;
#define ASSERT_IPL(L) \
{ \
if (curr_ipl != L) { \
printf("IPL is %d, expected %d\n", curr_ipl, L); \
panic("fpu: wrong ipl"); \
} \
}
#else
#define ASSERT_IPL(L)
#endif
int fp_kind = FP_387;
zone_t ifps_zone;
#define clear_fpu() \
{ \
set_ts(); \
}
#define ALIGNED(addr,size) (((unsigned)(addr)&((size)-1))==0)
extern void fpinit(void);
extern void fp_save(
thread_t thr_act);
extern void fp_load(
thread_t thr_act);
void
init_fpu(void)
{
unsigned short status, control;
set_cr0((get_cr0() & ~(CR0_EM|CR0_TS)) | CR0_NE);
fninit();
status = fnstsw();
fnstcw(&control);
if ((status & 0xff) == 0 &&
(control & 0x103f) == 0x3f)
{
fp_kind = FP_387;
if (cpuid_features() & CPUID_FEATURE_FXSR) {
fp_kind = FP_FXSR;
set_cr4(get_cr4() | CR4_FXS);
printf("Enabling XMM register save/restore");
if (cpuid_features() & CPUID_FEATURE_SSE) {
printf(" and SSE/SSE2");
set_cr4(get_cr4() | CR4_XMM);
}
printf(" opcodes\n");
}
set_cr0(get_cr0() | CR0_TS | CR0_MP);
}
else
{
fp_kind = FP_NO;
set_cr0(get_cr0() | CR0_EM);
}
}
void
fpu_module_init(void)
{
ifps_zone = zinit(sizeof(struct i386_fpsave_state),
THREAD_MAX * sizeof(struct i386_fpsave_state),
THREAD_CHUNK * sizeof(struct i386_fpsave_state),
"i386 fpsave state");
}
void
fpu_free(fps)
struct i386_fpsave_state *fps;
{
ASSERT_IPL(SPL0);
zfree(ifps_zone, fps);
}
kern_return_t
fpu_set_fxstate(
thread_t thr_act,
struct i386_float_state *state)
{
register pcb_t pcb;
register struct i386_fpsave_state *ifps;
register struct i386_fpsave_state *new_ifps;
ASSERT_IPL(SPL0);
if (fp_kind == FP_NO)
return KERN_FAILURE;
if (state->fpkind != FP_FXSR) {
return fpu_set_state(thr_act, state);
}
assert(thr_act != THREAD_NULL);
pcb = thr_act->machine.pcb;
if (state->initialized == 0) {
simple_lock(&pcb->lock);
ifps = pcb->ims.ifps;
pcb->ims.ifps = 0;
simple_unlock(&pcb->lock);
if (ifps != 0) {
zfree(ifps_zone, ifps);
}
}
else {
new_ifps = 0;
Retry:
simple_lock(&pcb->lock);
ifps = pcb->ims.ifps;
if (ifps == 0) {
if (new_ifps == 0) {
simple_unlock(&pcb->lock);
new_ifps = (struct i386_fpsave_state *) zalloc(ifps_zone);
assert(ALIGNED(new_ifps,16));
goto Retry;
}
ifps = new_ifps;
new_ifps = 0;
bzero((char *)ifps, sizeof *ifps);
pcb->ims.ifps = ifps;
}
bcopy((char *)&state->hw_state[0], (char *)&ifps->fx_save_state, sizeof(struct i386_fx_save));
ifps->fp_save_flavor = FP_FXSR;
simple_unlock(&pcb->lock);
if (new_ifps != 0)
zfree(ifps_zone, ifps);
}
return KERN_SUCCESS;
}
kern_return_t
fpu_get_fxstate(
thread_t thr_act,
register struct i386_float_state *state)
{
register pcb_t pcb;
register struct i386_fpsave_state *ifps;
ASSERT_IPL(SPL0);
if (fp_kind == FP_NO) {
return KERN_FAILURE;
} else if (fp_kind == FP_387) {
return fpu_get_state(thr_act, state);
}
assert(thr_act != THREAD_NULL);
pcb = thr_act->machine.pcb;
simple_lock(&pcb->lock);
ifps = pcb->ims.ifps;
if (ifps == 0) {
simple_unlock(&pcb->lock);
bzero((char *)state, sizeof(struct i386_float_state));
return KERN_SUCCESS;
}
if (thr_act == current_thread())
{
clear_ts();
fp_save(thr_act);
clear_fpu();
}
state->fpkind = fp_kind;
state->exc_status = 0;
state->initialized = ifps->fp_valid;
bcopy( (char *)&ifps->fx_save_state, (char *)&state->hw_state[0], sizeof(struct i386_fx_save));
simple_unlock(&pcb->lock);
return KERN_SUCCESS;
}
kern_return_t
fpu_set_state(
thread_t thr_act,
struct i386_float_state *state)
{
register pcb_t pcb;
register struct i386_fpsave_state *ifps;
register struct i386_fpsave_state *new_ifps;
ASSERT_IPL(SPL0);
if (fp_kind == FP_NO)
return KERN_FAILURE;
assert(thr_act != THREAD_NULL);
pcb = thr_act->machine.pcb;
if (state->initialized == 0) {
simple_lock(&pcb->lock);
ifps = pcb->ims.ifps;
pcb->ims.ifps = 0;
simple_unlock(&pcb->lock);
if (ifps != 0) {
zfree(ifps_zone, ifps);
}
}
else {
register struct i386_fp_save *user_fp_state;
register struct i386_fp_regs *user_fp_regs;
user_fp_state = (struct i386_fp_save *) &state->hw_state[0];
user_fp_regs = (struct i386_fp_regs *)
&state->hw_state[sizeof(struct i386_fp_save)];
new_ifps = 0;
Retry:
simple_lock(&pcb->lock);
ifps = pcb->ims.ifps;
if (ifps == 0) {
if (new_ifps == 0) {
simple_unlock(&pcb->lock);
new_ifps = (struct i386_fpsave_state *) zalloc(ifps_zone);
assert(ALIGNED(new_ifps,16));
goto Retry;
}
ifps = new_ifps;
new_ifps = 0;
bzero((char *)ifps, sizeof *ifps); pcb->ims.ifps = ifps;
}
bzero((char *)&ifps->fp_save_state, sizeof(struct i386_fp_save));
ifps->fp_save_state.fp_control = user_fp_state->fp_control;
ifps->fp_save_state.fp_status = user_fp_state->fp_status;
ifps->fp_save_state.fp_tag = user_fp_state->fp_tag;
ifps->fp_save_state.fp_eip = user_fp_state->fp_eip;
ifps->fp_save_state.fp_cs = user_fp_state->fp_cs;
ifps->fp_save_state.fp_opcode = user_fp_state->fp_opcode;
ifps->fp_save_state.fp_dp = user_fp_state->fp_dp;
ifps->fp_save_state.fp_ds = user_fp_state->fp_ds;
ifps->fp_regs = *user_fp_regs;
ifps->fp_save_flavor = FP_387;
simple_unlock(&pcb->lock);
if (new_ifps != 0)
zfree(ifps_zone, ifps);
}
return KERN_SUCCESS;
}
kern_return_t
fpu_get_state(
thread_t thr_act,
register struct i386_float_state *state)
{
register pcb_t pcb;
register struct i386_fpsave_state *ifps;
ASSERT_IPL(SPL0);
if (fp_kind == FP_NO)
return KERN_FAILURE;
assert(thr_act != THREAD_NULL);
pcb = thr_act->machine.pcb;
simple_lock(&pcb->lock);
ifps = pcb->ims.ifps;
if (ifps == 0) {
simple_unlock(&pcb->lock);
bzero((char *)state, sizeof(struct i386_float_state));
return KERN_SUCCESS;
}
if (thr_act == current_thread())
{
clear_ts();
fp_save(thr_act);
clear_fpu();
}
state->fpkind = fp_kind;
state->exc_status = 0;
{
register struct i386_fp_save *user_fp_state;
register struct i386_fp_regs *user_fp_regs;
state->initialized = ifps->fp_valid;
user_fp_state = (struct i386_fp_save *) &state->hw_state[0];
user_fp_regs = (struct i386_fp_regs *)
&state->hw_state[sizeof(struct i386_fp_save)];
bzero((char *)user_fp_state, sizeof(struct i386_fp_save));
user_fp_state->fp_control = ifps->fp_save_state.fp_control;
user_fp_state->fp_status = ifps->fp_save_state.fp_status;
user_fp_state->fp_tag = ifps->fp_save_state.fp_tag;
user_fp_state->fp_eip = ifps->fp_save_state.fp_eip;
user_fp_state->fp_cs = ifps->fp_save_state.fp_cs;
user_fp_state->fp_opcode = ifps->fp_save_state.fp_opcode;
user_fp_state->fp_dp = ifps->fp_save_state.fp_dp;
user_fp_state->fp_ds = ifps->fp_save_state.fp_ds;
*user_fp_regs = ifps->fp_regs;
}
simple_unlock(&pcb->lock);
return KERN_SUCCESS;
}
void
fpinit(void)
{
unsigned short control;
ASSERT_IPL(SPL0);
clear_ts();
fninit();
fnstcw(&control);
control &= ~(FPC_PC|FPC_RC);
control |= (FPC_PC_53 |
FPC_RC_RN |
FPC_ZE |
FPC_OE |
FPC_UE |
FPC_IE |
FPC_DE |
FPC_PE);
fldcw(control);
}
void
fpnoextflt(void)
{
ASSERT_IPL(SPL0);
clear_ts();
fp_load(current_thread());
}
void
fpextovrflt(void)
{
register thread_t thr_act = current_thread();
register pcb_t pcb;
register struct i386_fpsave_state *ifps;
pcb = thr_act->machine.pcb;
simple_lock(&pcb->lock);
ifps = pcb->ims.ifps;
pcb->ims.ifps = 0;
simple_unlock(&pcb->lock);
clear_ts();
fninit();
clear_fpu();
if (ifps)
zfree(ifps_zone, ifps);
i386_exception(EXC_BAD_ACCESS, VM_PROT_READ|VM_PROT_EXECUTE, 0);
}
void
fpexterrflt(void)
{
register thread_t thr_act = current_thread();
ASSERT_IPL(SPL0);
fp_save(thr_act);
i386_exception(EXC_ARITHMETIC,
EXC_I386_EXTERR,
thr_act->machine.pcb->ims.ifps->fp_save_state.fp_status);
}
void
fp_save(
thread_t thr_act)
{
register pcb_t pcb = thr_act->machine.pcb;
register struct i386_fpsave_state *ifps = pcb->ims.ifps;
if (ifps != 0 && !ifps->fp_valid) {
ifps->fp_valid = TRUE;
ifps->fp_save_flavor = FP_387;
if (FXSAFE()) {
fxsave(&ifps->fx_save_state); ifps->fp_save_flavor = FP_FXSR;
}
fnsave(&ifps->fp_save_state); }
}
void
fp_load(
thread_t thr_act)
{
register pcb_t pcb = thr_act->machine.pcb;
register struct i386_fpsave_state *ifps;
ASSERT_IPL(SPL0);
ifps = pcb->ims.ifps;
if (ifps == 0) {
ifps = (struct i386_fpsave_state *) zalloc(ifps_zone);
assert(ALIGNED(ifps,16));
bzero((char *)ifps, sizeof *ifps);
pcb->ims.ifps = ifps;
fpinit();
#if 1
} else if (ifps->fp_valid == 2) {
ifps->fp_valid = TRUE;
clear_fpu();
i386_exception(EXC_ARITHMETIC,
EXC_I386_EXTERR,
thr_act->machine.pcb->ims.ifps->fp_save_state.fp_status);
#endif
} else {
if (ifps->fp_save_flavor == FP_FXSR) fxrstor(&ifps->fx_save_state);
else frstor(ifps->fp_save_state);
}
ifps->fp_valid = FALSE;
}
void
fp_state_alloc(void)
{
pcb_t pcb = current_thread()->machine.pcb;
struct i386_fpsave_state *ifps;
ifps = (struct i386_fpsave_state *)zalloc(ifps_zone);
assert(ALIGNED(ifps,16));
bzero((char *)ifps, sizeof *ifps);
pcb->ims.ifps = ifps;
ifps->fp_valid = TRUE;
ifps->fp_save_state.fp_control = (0x037f
& ~(FPC_IM|FPC_ZM|FPC_OM|FPC_PC))
| (FPC_PC_53|FPC_IC_AFF);
ifps->fp_save_state.fp_status = 0;
ifps->fp_save_state.fp_tag = 0xffff;
ifps->fx_save_state.fx_control = ifps->fp_save_state.fp_control;
ifps->fx_save_state.fx_status = ifps->fp_save_state.fp_status;
ifps->fx_save_state.fx_tag = 0x00;
ifps->fx_save_state.fx_MXCSR = 0x1f80;
}
void
fpflush(__unused thread_t thr_act)
{
}
void
fpintr(void)
{
spl_t s;
thread_t thr_act = current_thread();
ASSERT_IPL(SPL1);
outb(0xf0, 0);
clear_ts();
fp_save(thr_act);
fninit();
clear_fpu();
s = splsched();
mp_disable_preemption();
ast_on(AST_I386_FP);
mp_enable_preemption();
splx(s);
}