70 lines
1.3 KiB
C
70 lines
1.3 KiB
C
#include "schedule.h"
|
|
#include "interrupts.h"
|
|
#include "memory.h"
|
|
#include "string.h"
|
|
|
|
extern struct task_t *current_task;
|
|
struct task_t *fpu_task;
|
|
|
|
unsigned char fpu_data[512] __attribute__((aligned(16)));
|
|
|
|
void fpu_set_cw(unsigned int cw) {
|
|
asm volatile("fldcw %0" :: "m"(cw));
|
|
}
|
|
|
|
void fpu_enable(void) {
|
|
asm volatile ("clts");
|
|
unsigned int t;
|
|
asm volatile ("mov %%cr4, %0" : "=r"(t));
|
|
t |= 3 << 9;
|
|
asm volatile ("mov %0, %%cr4" :: "r"(t));
|
|
}
|
|
|
|
void fpu_disable(void) {
|
|
unsigned int t;
|
|
asm volatile ("mov %%cr0, %0" : "=r"(t));
|
|
t |= 1 << 3;
|
|
asm volatile ("mov %0, %%cr0" :: "r"(t));
|
|
}
|
|
|
|
void fpu_restore(struct task_t * task) {
|
|
memcpy(fpu_data,task->fpu_data,512);
|
|
asm volatile ("fxrstor %0" : "=m"(fpu_data));
|
|
}
|
|
|
|
void fpu_save(struct task_t *task) {
|
|
asm volatile ("fxsave %0" : "=m"(fpu_data));
|
|
memcpy(task->fpu_data,(char *)fpu_data,512);
|
|
}
|
|
|
|
void fpu_init(void) {
|
|
asm volatile ("fninit");
|
|
fpu_set_cw(0x37F);
|
|
}
|
|
|
|
void fpu_isr(struct regs *r) {
|
|
fpu_enable();
|
|
if (fpu_task == current_task) {
|
|
return;
|
|
}
|
|
|
|
if (fpu_task != (void *)0) {
|
|
fpu_save(current_task);
|
|
}
|
|
|
|
fpu_task = current_task;
|
|
|
|
if (!fpu_task->fpu_enabled) {
|
|
fpu_init();
|
|
fpu_task->fpu_enabled = 1;
|
|
return;
|
|
}
|
|
fpu_restore(current_task);
|
|
}
|
|
|
|
void init_fpu() {
|
|
unsigned int t;
|
|
asm volatile ("mov %%cr0, %0" : "=r"(t));
|
|
t |= 1 << 5;
|
|
asm volatile ("mov %0, %%cr0" :: "r"(t));
|
|
} |