#include <linux/smp.h>
#include <asm/system.h>
#include <linux/tasks.h>

#include "../include/rtl_conf.h"

#include "../include/rtl_sched.h"
#include <rtl_core.h>
#include <linux/cons.h>

#ifdef CONFIG_RTL_FP_SUPPORT
int is_set_TS(void)
{
	int current_TS;
	__asm__("movl %%cr0,%%eax;andl $8,%%eax;movl %%eax,%0" \
			:"=m" (current_TS)/*output*/ \
			: /* no inputs */ \
			:"ax");
	return current_TS;
}


void rtl_fpu_save (schedule_t *s, RTL_THREAD_STRUCT *current_t)
{
/* 	if (s->rtl_current == &(s->rtl_linux_task)) { */ /* not good */
	if (current_t == &(s->rtl_linux_task)) {
		s->sched_user[0] = is_set_TS();
		clts();
	}
	__asm__("fnsave %0" : "=m" (current_t->fpu_regs));
}


void rtl_fpu_restore (schedule_t *s,RTL_THREAD_STRUCT *current_t) {
	clts (); /* just in case */
	__asm__("frstor %0" : "=m" (current_t->fpu_regs));
	if ((current_t == &(s->rtl_linux_task)) && s->sched_user[0])
	{
		stts();
	}
}


/*
static int rt_is_fpu_used ()
{
	for (t = RTL_THREAD_STRUCTs; t; t = t->next) {
		if (t -> uses_fp) {
			return 1;
		}
	}
	return 0;
}
*/


void rtl_task_init_fpu (RTL_THREAD_STRUCT *t, RTL_THREAD_STRUCT *fpu_owner)
{
	int was_set_TS;
	/* we  must save/restore the fpu state*/
	if (fpu_owner) {
		if ((was_set_TS = is_set_TS())) {
			clts();
		}
		__asm__("fnsave %0" : "=m" (fpu_owner -> fpu_regs));
		__asm__("fninit");
		__asm__("fnsave %0" : "=m" (t->fpu_regs));
		__asm__("frstor %0" : "=m" (fpu_owner -> fpu_regs));
		if (was_set_TS) {
			stts();
		}
	} else {
		clts ();
		__asm__("fninit");
/* 		fpu_owner = t; */
	}
	return ;
}

#endif /* CONFIG_RTL_FP_SUPPORT */

