#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));
}