/*
 * Copyright (C) 2001,2002,2003,2004 Philippe Gerum <rpm@xenomai.org>.
 *
 * ARM port
 *   Copyright (C) 2005 Stelian Pop
 *
 * 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 <linux/jump_label.h>
#include <asm/mmu_context.h>
#include <cobalt/kernel/thread.h>

struct static_key __xeno_vfp_key = STATIC_KEY_INIT_TRUE;

asmlinkage void __asm_thread_switch(struct thread_info *out,
				    struct thread_info *in);

asmlinkage void __asm_thread_trampoline(void);

#if defined(CONFIG_XENO_ARCH_FPU) && defined(CONFIG_VFP)

static unsigned int vfp_checked;
static DEFINE_MUTEX(vfp_check_lock);

asmlinkage void __asm_vfp_save(union vfp_state *vfp, unsigned int fpexc);

asmlinkage void __asm_vfp_load(union vfp_state *vfp, unsigned int cpu);

#define do_vfp_fmrx(_vfp_)						\
	({								\
		u32 __v;						\
		asm volatile("mrc p10, 7, %0, " __stringify(_vfp_)	\
			     ", cr0, 0 @ fmrx %0, " #_vfp_:		\
			     "=r" (__v));				\
		__v;							\
	})

#define do_vfp_fmxr(_vfp_,_var_)				\
	asm volatile("mcr p10, 7, %0, " __stringify(_vfp_)	\
		     ", cr0, 0 @ fmxr " #_vfp_ ", %0":		\
		     /* */ : "r" (_var_))

extern union vfp_state *vfp_current_hw_state[NR_CPUS];

static inline union vfp_state *get_fpu_owner(void)
{
	union vfp_state *vfp_owner;
	unsigned int cpu;
#ifdef CONFIG_SMP
	unsigned int fpexc;
#endif

#if __LINUX_ARM_ARCH__ <= 6
	if (!static_key_true(&__xeno_vfp_key))
		return NULL;
#endif

#ifdef CONFIG_SMP
	fpexc = do_vfp_fmrx(FPEXC);
	if (!(fpexc & FPEXC_EN))
		return NULL;
#endif

	cpu = raw_smp_processor_id();
	vfp_owner = vfp_current_hw_state[cpu];
	if (!vfp_owner)
		return NULL;

#ifdef CONFIG_SMP
	if (vfp_owner->hard.cpu != cpu)
		return NULL;
#endif /* SMP */

	return vfp_owner;
}

#define do_disable_vfp(fpexc)					\
	do_vfp_fmxr(FPEXC, fpexc & ~FPEXC_EN)

#define XNARCH_VFP_ANY_EXC						\
	(FPEXC_EX|FPEXC_DEX|FPEXC_FP2V|FPEXC_VV|FPEXC_TRAP_MASK)

#define do_enable_vfp()							\
	({								\
		unsigned _fpexc = do_vfp_fmrx(FPEXC) | FPEXC_EN;	\
		do_vfp_fmxr(FPEXC, _fpexc & ~XNARCH_VFP_ANY_EXC);	\
		_fpexc;							\
	})

int xnarch_fault_fpu_p(struct ipipe_trap_data *d)
{
	/* This function does the same thing to decode the faulting instruct as
	   "call_fpe" in arch/arm/entry-armv.S */
	static unsigned copro_to_exc[16] = {
		IPIPE_TRAP_UNDEFINSTR,
		/* FPE */
		IPIPE_TRAP_FPU, IPIPE_TRAP_FPU,
		IPIPE_TRAP_UNDEFINSTR,
#ifdef CONFIG_CRUNCH
		IPIPE_TRAP_FPU, IPIPE_TRAP_FPU, IPIPE_TRAP_FPU,
#else /* !CONFIG_CRUNCH */
		IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
#endif /* !CONFIG_CRUNCH */
		IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
#ifdef CONFIG_VFP
		IPIPE_TRAP_VFP, IPIPE_TRAP_VFP,
#else /* !CONFIG_VFP */
		IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
#endif /* !CONFIG_VFP */
		IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
		IPIPE_TRAP_UNDEFINSTR, IPIPE_TRAP_UNDEFINSTR,
	};
	unsigned instr, exc, cp;
	char *pc;

	if (d->exception == IPIPE_TRAP_FPU)
		return 1;

	if (d->exception == IPIPE_TRAP_VFP)
		goto trap_vfp;

	if (d->exception != IPIPE_TRAP_UNDEFINSTR)
		return 0;

	pc = (char *) xnarch_fault_pc(d);
	if (unlikely(thumb_mode(d->regs))) {
		unsigned short thumbh, thumbl;

#if defined(CONFIG_ARM_THUMB) && __LINUX_ARM_ARCH__ >= 6 && defined(CONFIG_CPU_V7)
#if __LINUX_ARM_ARCH__ < 7
		if (cpu_architecture() < CPU_ARCH_ARMv7)
#else
		if (0)
#endif /* arch < 7 */
#endif /* thumb && arch >= 6 && cpu_v7 */
			return 0;

		thumbh = *(unsigned short *) pc;
		thumbl = *((unsigned short *) pc + 1);

		if ((thumbh & 0x0000f800) < 0x0000e800)
			return 0;
		instr = (thumbh << 16) | thumbl;

#ifdef CONFIG_NEON
		if ((instr & 0xef000000) == 0xef000000
		    || (instr & 0xff100000) == 0xf9000000)
			goto trap_vfp;
#endif
	} else {
		instr = *(unsigned *) pc;

#ifdef CONFIG_NEON
		if ((instr & 0xfe000000) == 0xf2000000
		    || (instr & 0xff100000) == 0xf4000000)
			goto trap_vfp;
#endif
	}

	if ((instr & 0x0c000000) != 0x0c000000)
		return 0;

	cp = (instr & 0x00000f00) >> 8;
#ifdef CONFIG_IWMMXT
	/* We need something equivalent to _TIF_USING_IWMMXT for Xenomai kernel
	   threads */
	if (cp <= 1) {
		d->exception = IPIPE_TRAP_FPU;
		return 1;
	}
#endif

	exc = copro_to_exc[cp];
	if (exc == IPIPE_TRAP_VFP) {
	  trap_vfp:
		/* If an exception is pending, the VFP fault is not really an
		   "FPU unavailable" fault, so we return undefinstr in that
		   case, the nucleus will let linux handle the fault. */
		exc = do_vfp_fmrx(FPEXC);
		if (exc & (FPEXC_EX|FPEXC_DEX)
		    || ((exc & FPEXC_EN) && do_vfp_fmrx(FPSCR) & FPSCR_IXE))
			exc = IPIPE_TRAP_UNDEFINSTR;
		else
			exc = IPIPE_TRAP_VFP;
	}

	d->exception = exc;
	return exc != IPIPE_TRAP_UNDEFINSTR;
}

void xnarch_leave_root(struct xnthread *root)
{
	struct xnarchtcb *rootcb = xnthread_archtcb(root);
	rootcb->fpup = get_fpu_owner();
}

void xnarch_switch_fpu(struct xnthread *from, struct xnthread *to)
{
	union vfp_state *const from_fpup = from ? from->tcb.fpup : NULL;
	unsigned cpu = raw_smp_processor_id();

	if (xnthread_test_state(to, XNROOT) == 0) {
		union vfp_state *const to_fpup = to->tcb.fpup;
		unsigned fpexc = do_enable_vfp();

		if (from_fpup == to_fpup)
			return;

		if (from_fpup)
			__asm_vfp_save(from_fpup, fpexc);

		__asm_vfp_load(to_fpup, cpu);
       } else {
		/*
		 * We are restoring the Linux current thread. The FPU
		 * can be disabled, so that a fault will occur if the
		 * newly switched thread uses the FPU, to allow the
		 * kernel handler to pick the correct FPU context, and
		 * save in the same move the last used RT context.
		 */
		vfp_current_hw_state[cpu] = from_fpup;
#ifdef CONFIG_SMP
		/*
		 * On SMP, since returning to FPU disabled mode means
		 * that we have to save fpu, avoid doing it if
		 * current FPU context belongs to the task we are
		 * switching to.
		 */
		if (from_fpup) {
			union vfp_state *const current_task_fpup =
				&to->tcb.core.tip->vfpstate;
			const unsigned fpdis = do_vfp_fmrx(FPEXC);
			const unsigned fpen = fpdis | FPEXC_EN;

			do_vfp_fmxr(FPEXC, fpen & ~XNARCH_VFP_ANY_EXC);
			if (from_fpup == current_task_fpup)
				return;

			__asm_vfp_save(from_fpup, fpen);
			do_vfp_fmxr(FPEXC, fpdis);
		}
#endif
	}
}

int xnarch_handle_fpu_fault(struct xnthread *from,
			struct xnthread *to, struct ipipe_trap_data *d)
{
	if (xnthread_test_state(to, XNFPU))
		/* FPU is already enabled, probably an exception */
               return 0;

#if __LINUX_ARM_ARCH__ <= 6
	if (!static_key_true(&__xeno_vfp_key))
		/* VFP instruction emitted, on a cpu without VFP, this
		   is an error */
		return 0;
#endif

	xnlock_get(&nklock);
	xnthread_set_state(to, XNFPU);
	xnlock_put(&nklock);

	xnarch_switch_fpu(from, to);

	/* Retry faulting instruction */
	d->regs->ARM_pc = xnarch_fault_pc(d);
	return 1;
}

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

	tcb->fpup = &task_thread_info(tcb->core.host_task)->vfpstate;

	if (vfp_checked == 0) {
		mutex_lock(&vfp_check_lock);
		if (vfp_checked == 0) {
			if ((elf_hwcap & HWCAP_VFP) == 0)
				static_key_slow_dec(&__xeno_vfp_key);
			vfp_checked = 1;
		}
		mutex_unlock(&vfp_check_lock);
	}

	/* XNFPU is set upon first FPU fault */
	xnthread_clear_state(thread, XNFPU);
}

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

#endif /* CONFIG_XENO_ARCH_FPU && CONFIG_VFP*/

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

	__asm_thread_switch(out_tcb->core.tip, in_tcb->core.tip);
}