/*
 * Copyright (C) 2001,2002,2003,2004 Philippe Gerum <rpm@xenomai.org>.
 *
 * 64-bit PowerPC adoption
 *   copyright (C) 2005 Taneli Vähäkangas and Heikki Lindholm
 *
 * Xenomai is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * Xenomai is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Xenomai; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
 * 02111-1307, USA.
 */

#include <linux/sched.h>
#include <linux/ipipe.h>
#include <linux/mm.h>
#include <asm/mmu_context.h>
#include <cobalt/kernel/thread.h>

asmlinkage struct task_struct *
_switch(struct thread_struct *prev, struct thread_struct *next);

void xnarch_switch_to(struct xnthread *out, struct xnthread *in)
{
	struct xnarchtcb *out_tcb = &out->tcb, *in_tcb = &in->tcb;
	struct mm_struct *prev_mm, *next_mm;
	struct task_struct *next;

	next = in_tcb->core.host_task;
	prev_mm = out_tcb->core.active_mm;

	next_mm = in_tcb->core.mm;
	if (next_mm == NULL) {
		in_tcb->core.active_mm = prev_mm;
		enter_lazy_tlb(prev_mm, next);
	} else {
		ipipe_switch_mm_head(prev_mm, next_mm, next);
		/*
		 * We might be switching back to the root thread,
		 * which we preempted earlier, shortly after "current"
		 * dropped its mm context in the do_exit() path
		 * (next->mm == NULL). In that particular case, the
		 * kernel expects a lazy TLB state for leaving the mm.
		 */
		if (next->mm == NULL)
			enter_lazy_tlb(prev_mm, next);
	}

	hard_local_irq_disable();
	_switch(out_tcb->core.tsp, in_tcb->core.tsp);
}

#ifdef CONFIG_XENO_ARCH_FPU

asmlinkage void __asm_init_fpu(struct thread_struct *ts);

asmlinkage void __asm_save_fpu(struct thread_struct *ts);

asmlinkage void __asm_restore_fpu(struct thread_struct *ts);

asmlinkage void __asm_disable_fpu(void);

asmlinkage void __asm_enable_fpu(void);

#if !defined(CONFIG_SMP) && LINUX_VERSION_CODE < KERNEL_VERSION(4,4,0)
#define get_fpu_owner(cur) last_task_used_math
#else /* CONFIG_SMP */
#define get_fpu_owner(cur) ({					\
    struct task_struct * _cur = (cur);                          \
    ((_cur->thread.regs && (_cur->thread.regs->msr & MSR_FP))   \
     ? _cur : NULL);                                            \
})
#endif /* CONFIG_SMP */

static void xnarch_enable_fpu(struct xnthread *thread)
{
	struct xnarchtcb *tcb = xnthread_archtcb(thread);
	struct task_struct *task = tcb->core.host_task;

	if (task && task != tcb->core.user_fpu_owner)
		__asm_disable_fpu();
	else
		__asm_enable_fpu();
}

static void do_save_fpu(struct xnthread *thread)
{
	struct xnarchtcb *tcb = xnthread_archtcb(thread);

	if (tcb->fpup) {
		__asm_save_fpu(tcb->fpup);

		if (tcb->core.user_fpu_owner &&
		    tcb->core.user_fpu_owner->thread.regs)
			tcb->core.user_fpu_owner->thread.regs->msr &= ~(MSR_FP|MSR_FE0|MSR_FE1);
	}
}

static void xnarch_restore_fpu(struct xnthread *thread)
{
	struct xnarchtcb *tcb = xnthread_archtcb(thread);
	struct thread_struct *ts;
	struct pt_regs *regs;

	if (tcb->fpup) {
		__asm_restore_fpu(tcb->fpup);
		/*
		 * Note: Only enable FP in MSR, if it was enabled when
		 * we saved the fpu state.
		 */
		if (tcb->core.user_fpu_owner) {
			ts = &tcb->core.user_fpu_owner->thread;
			regs = ts->regs;
			if (regs) {
				regs->msr &= ~(MSR_FE0|MSR_FE1);
				regs->msr |= (MSR_FP|ts->fpexc_mode);
			}
		}
	}
	/*
	 * FIXME: We restore FPU "as it was" when Xenomai preempted Linux,
	 * whereas we could be much lazier.
	 */
	if (tcb->core.host_task &&
	    tcb->core.host_task != tcb->core.user_fpu_owner)
		__asm_disable_fpu();
}

void xnarch_switch_fpu(struct xnthread *from, struct xnthread *to)
{
	if (from == to || 
		xnarch_fpu_ptr(xnthread_archtcb(from)) == 
		xnarch_fpu_ptr(xnthread_archtcb(to))) {
		xnarch_enable_fpu(to);
		return;
	}
	
	if (from)
		do_save_fpu(from);
	
	xnarch_restore_fpu(to);
}

void xnarch_leave_root(struct xnthread *root)
{
	struct xnarchtcb *rootcb = xnthread_archtcb(root);
	rootcb->core.user_fpu_owner = get_fpu_owner(rootcb->core.host_task);
	/* So that do_save_fpu() operates on the right FPU area. */
	rootcb->fpup = rootcb->core.user_fpu_owner ?
		&rootcb->core.user_fpu_owner->thread : NULL;
}

void xnarch_init_root_tcb(struct xnthread *thread)
{
	struct xnarchtcb *tcb = &thread->tcb;
	tcb->fpup = NULL;
}

void xnarch_init_shadow_tcb(struct xnthread *thread)
{
	struct xnarchtcb *tcb = &thread->tcb;
	tcb->fpup = &tcb->core.host_task->thread;
}

#endif /* CONFIG_XENO_ARCH_FPU */