#include <cpus.h>
#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/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/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;
#if NCPUS == 1
volatile thread_act_t fp_act = THR_ACT_NULL;
volatile thread_act_t fp_intr_act = THR_ACT_NULL;
#define clear_fpu() \
{ \
set_ts(); \
fp_act = THR_ACT_NULL; \
}
#else
#define clear_fpu() \
{ \
set_ts(); \
}
#endif
extern void fpinit(void);
extern void fp_save(
thread_act_t thr_act);
extern void fp_load(
thread_act_t thr_act);
void
init_fpu(void)
{
unsigned short status, control;
set_cr0(get_cr0() & ~(CR0_EM|CR0_TS));
fninit();
status = fnstsw();
fnstcw(&control);
if ((status & 0xff) == 0 &&
(control & 0x103f) == 0x3f)
{
#if 0
volatile double fp_infinity, fp_one, fp_zero;
fp_one = 1.0;
fp_zero = 0.0;
fp_infinity = fp_one / fp_zero;
if (fp_infinity == -fp_infinity) {
fp_kind = FP_287;
__asm__ volatile(".byte 0xdb; .byte 0xe4");
}
else
#endif
{
fp_kind = FP_387;
}
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
fp_free(fps)
struct i386_fpsave_state *fps;
{
ASSERT_IPL(SPL0);
#if NCPUS == 1
if ((fp_act != THR_ACT_NULL) && (fp_act->mact.pcb->ims.ifps == fps)) {
fwait();
clear_fpu();
}
#endif
zfree(ifps_zone, (vm_offset_t) fps);
}
kern_return_t
fpu_set_state(
thread_act_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 != THR_ACT_NULL);
pcb = thr_act->mact.pcb;
#if NCPUS == 1
if (fp_act == thr_act) {
fwait();
clear_fpu();
}
#endif
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, (vm_offset_t) 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);
goto Retry;
}
ifps = new_ifps;
new_ifps = 0;
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;
simple_unlock(&pcb->lock);
if (new_ifps != 0)
zfree(ifps_zone, (vm_offset_t) ifps);
}
return KERN_SUCCESS;
}
kern_return_t
fpu_get_state(
thread_act_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 != THR_ACT_NULL);
pcb = thr_act->mact.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 NCPUS == 1
if (thr_act == fp_act)
#else
if (thr_act == current_act())
#endif
{
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();
#if NCPUS == 1
if (fp_act == current_act())
return;
fwait();
if (fp_act != THR_ACT_NULL) {
fp_save(fp_act);
}
fp_act = current_act();
#endif
fp_load(current_act());
}
void
fpextovrflt(void)
{
register thread_act_t thr_act = current_act();
register pcb_t pcb;
register struct i386_fpsave_state *ifps;
#if NCPUS == 1
if (fp_act != thr_act) {
panic("fpextovrflt");
}
#endif
pcb = thr_act->mact.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, (vm_offset_t) ifps);
i386_exception(EXC_BAD_ACCESS, VM_PROT_READ|VM_PROT_EXECUTE, 0);
}
void
fpexterrflt(void)
{
register thread_act_t thr_act = current_act();
ASSERT_IPL(SPL0);
#if NCPUS == 1
if (fp_act != THR_ACT_NULL) {
panic("fpexterrflt");
return;
}
if (fp_intr_act != thr_act) {
if (fp_intr_act == THR_ACT_NULL) {
panic("fpexterrflt: fp_intr_act == THR_ACT_NULL");
return;
}
fp_intr_act->mact.pcb->ims.ifps->fp_valid = 2;
fp_intr_act = THR_ACT_NULL;
return;
}
fp_intr_act = THR_ACT_NULL;
#else
fp_save(thr_act);
#endif
i386_exception(EXC_ARITHMETIC,
EXC_I386_EXTERR,
thr_act->mact.pcb->ims.ifps->fp_save_state.fp_status);
}
void
fp_save(
thread_act_t thr_act)
{
register pcb_t pcb = thr_act->mact.pcb;
register struct i386_fpsave_state *ifps = pcb->ims.ifps;
if (ifps != 0 && !ifps->fp_valid) {
ifps->fp_valid = TRUE;
fnsave(&ifps->fp_save_state);
}
}
void
fp_load(
thread_act_t thr_act)
{
register pcb_t pcb = thr_act->mact.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);
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->mact.pcb->ims.ifps->fp_save_state.fp_status);
#endif
} else {
frstor(ifps->fp_save_state);
}
ifps->fp_valid = FALSE;
}
void
fp_state_alloc(void)
{
pcb_t pcb = current_act()->mact.pcb;
struct i386_fpsave_state *ifps;
ifps = (struct i386_fpsave_state *)zalloc(ifps_zone);
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;
}
void
fpflush(thread_act_t thr_act)
{
#if NCPUS == 1
if (fp_act && thr_act == fp_act) {
clear_ts();
fwait();
clear_fpu();
}
#else
#endif
}
void
fpintr(void)
{
spl_t s;
thread_act_t thr_act = current_act();
ASSERT_IPL(SPL1);
outb(0xf0, 0);
#if NCPUS == 1
if (fp_act == THR_ACT_NULL) {
printf("fpintr: FPU not belonging to anyone!\n");
clear_ts();
fninit();
clear_fpu();
return;
}
if (fp_act != thr_act) {
clear_ts();
fp_save(fp_act);
fp_act->mact.pcb->ims.ifps->fp_valid = 2;
fninit();
clear_fpu();
return;
}
if (fp_intr_act != THR_ACT_NULL)
panic("fp_intr: already caught intr");
fp_intr_act = thr_act;
#endif
clear_ts();
fp_save(thr_act);
fninit();
clear_fpu();
s = splsched();
mp_disable_preemption();
ast_on(AST_I386_FP);
mp_enable_preemption();
splx(s);
}