/*
 * Copyright (C) 2001,2002,2003,2004 Philippe Gerum <rpm@xenomai.org>.
 *
 * ARM port
 *   Copyright (C) 2005 Stelian Pop
 * 
 * ARM64 port
 *   Copyright (C) 2015 Dmitriy Cherkasov <dmitriy@mperpetuo.com>
 *   Copyright (C) 2015 Gilles Chanteperdrix <gch@xenomai.org>
 *
 * 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>
#include <asm/fpsimd.h>
#include <asm/processor.h>
#include <asm/hw_breakpoint.h>

#ifdef ARM64_XENO_OLD_SWITCH

#include <asm/fpsimd.h>

#define FPSIMD_EN (0x3 << 20)

static inline unsigned long get_cpacr(void)
{
	unsigned long result;
	__asm__ ("mrs %0, cpacr_el1": "=r"(result));
	return result;
}

static inline void set_cpacr(long val)
{
	__asm__ __volatile__ (
		"msr cpacr_el1, %0\n\t"
		"isb"
		: /* */ : "r"(val));
}

static inline void enable_fpsimd(void)
{
	set_cpacr(get_cpacr() | FPSIMD_EN);
}

static inline struct fpsimd_state *get_fpu_owner(struct xnarchtcb *rootcb)
{
	struct task_struct *curr = rootcb->core.host_task;

	if (test_ti_thread_flag(task_thread_info(curr), TIF_FOREIGN_FPSTATE))
		/* Foreign fpu state, use auxiliary backup area */
		return &rootcb->xnfpsimd_state;

	return &curr->thread.fpsimd_state;
}

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

void xnarch_switch_fpu(struct xnthread *from, struct xnthread *to)
{
	struct fpsimd_state *const from_fpup = from ? from->tcb.fpup : NULL;
	struct fpsimd_state *const to_fpup = to->tcb.fpup;

	enable_fpsimd();

	if (from_fpup == to_fpup)
		return;

	fpsimd_save_state(from_fpup);

	fpsimd_load_state(to_fpup);
	to_fpup->cpu = raw_smp_processor_id();
}

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

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

#endif /* ARM64_XENO_OLD_SWITCH */

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

	next = in_tcb->core.host_task;
	prev = out_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);
	}

#if LINUX_VERSION_CODE < KERNEL_VERSION(4,9,0)
	ipipe_switch_to(prev, next);
	(void)last;
#else
	switch_to(prev, next, last);
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4,19,0)
	fpsimd_restore_current_state();
#endif
#endif
}

int xnarch_escalate(void)
{
	if (ipipe_root_p) {
		ipipe_raise_irq(cobalt_pipeline.escalate_virq);
		return 1;
	}

	return 0;
}