89 lines
1.8 KiB
C
89 lines
1.8 KiB
C
/* vim: tabstop=4 shiftwidth=4 noexpandtab
|
|
*
|
|
* Tiny FPU enable module.
|
|
*
|
|
* Part of the ToAruOS Kernel
|
|
* (C) 2011 Kevin Lange ...
|
|
* To whatever possible level this short of a code chunk
|
|
* can be considered copyrightable in your jurisdiction.
|
|
*/
|
|
#include <system.h>
|
|
#include <logging.h>
|
|
|
|
process_t * fpu_thread = NULL;
|
|
|
|
/**
|
|
* Set the FPU control word
|
|
*
|
|
* @param cw What to set the control word to.
|
|
*/
|
|
void
|
|
set_fpu_cw(const uint16_t cw) {
|
|
asm volatile("fldcw %0" :: "m"(cw));
|
|
}
|
|
|
|
/**
|
|
* Enable the FPU
|
|
*
|
|
* We are assuming that we have one to begin with, but since we
|
|
* only really operate on 686 machines, we do, so we're not
|
|
* going to bother checking.
|
|
*/
|
|
void enable_fpu() {
|
|
asm volatile ("clts");
|
|
size_t t;
|
|
asm volatile ("mov %%cr4, %0" : "=r"(t));
|
|
t |= 3 << 9;
|
|
asm volatile ("mov %0, %%cr4" :: "r"(t));
|
|
}
|
|
|
|
void disable_fpu() {
|
|
size_t t;
|
|
asm volatile ("mov %%cr0, %0" : "=r"(t));
|
|
t |= 1 << 3;
|
|
asm volatile ("mov %0, %%cr0" :: "r"(t));
|
|
}
|
|
|
|
uint8_t saves[512] __attribute__((aligned(16)));
|
|
|
|
void restore_fpu(process_t * proc) {
|
|
memcpy(&saves,(uint8_t *)&proc->thread.fp_regs,512);
|
|
asm volatile ("fxrstor %0" : "=m"(saves));
|
|
}
|
|
|
|
void save_fpu(process_t * proc) {
|
|
asm volatile ("fxsave %0" : "=m"(saves));
|
|
memcpy((uint8_t *)&proc->thread.fp_regs,&saves,512);
|
|
}
|
|
|
|
void init_fpu() {
|
|
asm volatile ("fninit");
|
|
set_fpu_cw(0x37F);
|
|
}
|
|
|
|
void invalid_op(struct regs * r) {
|
|
enable_fpu();
|
|
if (fpu_thread == current_process) {
|
|
return;
|
|
}
|
|
if (fpu_thread) {
|
|
save_fpu(fpu_thread);
|
|
}
|
|
fpu_thread = (process_t *)current_process;
|
|
if (!fpu_thread->thread.fpu_enabled) {
|
|
init_fpu();
|
|
fpu_thread->thread.fpu_enabled = 1;
|
|
return;
|
|
}
|
|
restore_fpu(fpu_thread);
|
|
}
|
|
|
|
void switch_fpu() {
|
|
disable_fpu();
|
|
}
|
|
|
|
void auto_fpu() {
|
|
isrs_install_handler(6, &invalid_op);
|
|
isrs_install_handler(7, &invalid_op);
|
|
}
|