code:
// SPDX-License-Identifier: GPL-2.0-only
/*
 *  linux/arch/arm/vfp/vfpmodule.c
 *
 *  Copyright (C) 2004 ARM Limited.
 *  Written by Deep Blue Solutions Limited.
 */
#include <linux/types.h>
#include <linux/cpu.h>
#include <linux/cpu_pm.h>
#include <linux/hardirq.h>
#include <linux/kernel.h>
#include <linux/notifier.h>
#include <linux/signal.h>
#include <linux/sched/signal.h>
#include <linux/smp.h>
#include <linux/init.h>
#include <linux/uaccess.h>
#include <linux/user.h>
#include <linux/export.h>

#include <asm/cp15.h>
#include <asm/cputype.h>
#include <asm/system_info.h>
#include <asm/thread_notify.h>
#include <asm/traps.h>
#include <asm/vfp.h>

#include "vfpinstr.h"
#include "vfp.h"

/*
 * Our undef handlers (in entry.S)
 */
asmlinkage void vfp_support_entry(void);
asmlinkage void vfp_null_entry(void);

asmlinkage void (*vfp_vector)(void) = vfp_null_entry;

/*
 * Dual-use variable.
 * Used in startup: set to non-zero if VFP checks fail
 * After startup, holds VFP architecture
 */
static unsigned int __initdata VFP_arch;

/*
 * The pointer to the vfpstate structure of the thread which currently
 * owns the context held in the VFP hardware, or NULL if the hardware
 * context is invalid.
 *
 * For UP, this is sufficient to tell which thread owns the VFP context.
 * However, for SMP, we also need to check the CPU number stored in the
 * saved state too to catch migrations.
 */
union vfp_state *vfp_current_hw_state[NR_CPUS];

/*
 * Is 'thread's most up to date state stored in this CPUs hardware?
 * Must be called from non-preemptible context.
 */
static bool vfp_state_in_hw(unsigned int cpu, struct thread_info *thread)
{
#ifdef CONFIG_SMP
	if (thread->vfpstate.hard.cpu != cpu)
		return false;
#endif
	return vfp_current_hw_state[cpu] == &thread->vfpstate;
}

/*
 * Force a reload of the VFP context from the thread structure.  We do
 * this by ensuring that access to the VFP hardware is disabled, and
 * clear vfp_current_hw_state.  Must be called from non-preemptible context.
 */
static void vfp_force_reload(unsigned int cpu, struct thread_info *thread)
{
	if (vfp_state_in_hw(cpu, thread)) {
		fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
		vfp_current_hw_state[cpu] = NULL;
	}
#ifdef CONFIG_SMP
	thread->vfpstate.hard.cpu = NR_CPUS;
#endif
}

/*
 * Per-thread VFP initialization.
 */
static void vfp_thread_flush(struct thread_info *thread)
{
	union vfp_state *vfp = &thread->vfpstate;
	unsigned int cpu;

	/*
	 * Disable VFP to ensure we initialize it first.  We must ensure
	 * that the modification of vfp_current_hw_state[] and hardware
	 * disable are done for the same CPU and without preemption.
	 *
	 * Do this first to ensure that preemption won't overwrite our
	 * state saving should access to the VFP be enabled at this point.
	 */
	cpu = get_cpu();
	if (vfp_current_hw_state[cpu] == vfp)
		vfp_current_hw_state[cpu] = NULL;
	fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
	put_cpu();

	memset(vfp, 0, sizeof(union vfp_state));

	vfp->hard.fpexc = FPEXC_EN;
	vfp->hard.fpscr = FPSCR_ROUND_NEAREST;
#ifdef CONFIG_SMP
	vfp->hard.cpu = NR_CPUS;
#endif
}

static void vfp_thread_exit(struct thread_info *thread)
{
	/* release case: Per-thread VFP cleanup. */
	union vfp_state *vfp = &thread->vfpstate;
	unsigned int cpu = get_cpu();

	if (vfp_current_hw_state[cpu] == vfp)
		vfp_current_hw_state[cpu] = NULL;
	put_cpu();
}

static void vfp_thread_copy(struct thread_info *thread)
{
	struct thread_info *parent = current_thread_info();

	vfp_sync_hwstate(parent);
	thread->vfpstate = parent->vfpstate;
#ifdef CONFIG_SMP
	thread->vfpstate.hard.cpu = NR_CPUS;
#endif
}

/*
 * When this function is called with the following 'cmd's, the following
 * is true while this function is being run:
 *  THREAD_NOFTIFY_SWTICH:
 *   - the previously running thread will not be scheduled onto another CPU.
 *   - the next thread to be run (v) will not be running on another CPU.
 *   - thread->cpu is the local CPU number
 *   - not preemptible as we're called in the middle of a thread switch
 *  THREAD_NOTIFY_FLUSH:
 *   - the thread (v) will be running on the local CPU, so
 *	v === current_thread_info()
 *   - thread->cpu is the local CPU number at the time it is accessed,
 *	but may change at any time.
 *   - we could be preempted if tree preempt rcu is enabled, so
 *	it is unsafe to use thread->cpu.
 *  THREAD_NOTIFY_EXIT
 *   - we could be preempted if tree preempt rcu is enabled, so
 *	it is unsafe to use thread->cpu.
 */
static int vfp_notifier(struct notifier_block *self, unsigned long cmd, void *v)
{
	struct thread_info *thread = v;
	u32 fpexc;
#ifdef CONFIG_SMP
	unsigned int cpu;
#endif

	switch (cmd) {
	case THREAD_NOTIFY_SWITCH:
		fpexc = fmrx(FPEXC);

#ifdef CONFIG_SMP
		cpu = thread->cpu;

		/*
		 * On SMP, if VFP is enabled, save the old state in
		 * case the thread migrates to a different CPU. The
		 * restoring is done lazily.
		 */
		if ((fpexc & FPEXC_EN) && vfp_current_hw_state[cpu])
			vfp_save_state(vfp_current_hw_state[cpu], fpexc);
#endif

		/*
		 * Always disable VFP so we can lazily save/restore the
		 * old state.
		 */
		fmxr(FPEXC, fpexc & ~FPEXC_EN);
		break;

	case THREAD_NOTIFY_FLUSH:
		vfp_thread_flush(thread);
		break;

	case THREAD_NOTIFY_EXIT:
		vfp_thread_exit(thread);
		break;

	case THREAD_NOTIFY_COPY:
		vfp_thread_copy(thread);
		break;
	}

	return NOTIFY_DONE;
}

static struct notifier_block vfp_notifier_block = {
	.notifier_call	= vfp_notifier,
};

/*
 * Raise a SIGFPE for the current process.
 * sicode describes the signal being raised.
 */
static void vfp_raise_sigfpe(unsigned int sicode, struct pt_regs *regs)
{
	/*
	 * This is the same as NWFPE, because it's not clear what
	 * this is used for
	 */
	current->thread.error_code = 0;
	current->thread.trap_no = 6;

	send_sig_fault(SIGFPE, sicode,
		       (void __user *)(instruction_pointer(regs) - 4),
		       current);
}

static void vfp_panic(char *reason, u32 inst)
{
	int i;

	pr_err("VFP: Error: %s\n", reason);
	pr_err("VFP: EXC 0x%08x SCR 0x%08x INST 0x%08x\n",
		fmrx(FPEXC), fmrx(FPSCR), inst);
	for (i = 0; i < 32; i += 2)
		pr_err("VFP: s%2u: 0x%08x s%2u: 0x%08x\n",
		       i, vfp_get_float(i), i+1, vfp_get_float(i+1));
}

/*
 * Process bitmask of exception conditions.
 */
static void vfp_raise_exceptions(u32 exceptions, u32 inst, u32 fpscr, struct pt_regs *regs)
{
	int si_code = 0;

	pr_debug("VFP: raising exceptions %08x\n", exceptions);

	if (exceptions == VFP_EXCEPTION_ERROR) {
		vfp_panic("unhandled bounce", inst);
		vfp_raise_sigfpe(FPE_FLTINV, regs);
		return;
	}

	/*
	 * If any of the status flags are set, update the FPSCR.
	 * Comparison instructions always return at least one of
	 * these flags set.
	 */
	if (exceptions & (FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V))
		fpscr &= ~(FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V);

	fpscr |= exceptions;

	fmxr(FPSCR, fpscr);

#define RAISE(stat,en,sig)				\
	if (exceptions & stat && fpscr & en)		\
		si_code = sig;

	/*
	 * These are arranged in priority order, least to highest.
	 */
	RAISE(FPSCR_DZC, FPSCR_DZE, FPE_FLTDIV);
	RAISE(FPSCR_IXC, FPSCR_IXE, FPE_FLTRES);
	RAISE(FPSCR_UFC, FPSCR_UFE, FPE_FLTUND);
	RAISE(FPSCR_OFC, FPSCR_OFE, FPE_FLTOVF);
	RAISE(FPSCR_IOC, FPSCR_IOE, FPE_FLTINV);

	if (si_code)
		vfp_raise_sigfpe(si_code, regs);
}

/*
 * Emulate a VFP instruction.
 */
static u32 vfp_emulate_instruction(u32 inst, u32 fpscr, struct pt_regs *regs)
{
	u32 exceptions = VFP_EXCEPTION_ERROR;

	pr_debug("VFP: emulate: INST=0x%08x SCR=0x%08x\n", inst, fpscr);

	if (INST_CPRTDO(inst)) {
		if (!INST_CPRT(inst)) {
			/*
			 * CPDO
			 */
			if (vfp_single(inst)) {
				exceptions = vfp_single_cpdo(inst, fpscr);
			} else {
				exceptions = vfp_double_cpdo(inst, fpscr);
			}
		} else {
			/*
			 * A CPRT instruction can not appear in FPINST2, nor
			 * can it cause an exception.  Therefore, we do not
			 * have to emulate it.
			 */
		}
	} else {
		/*
		 * A CPDT instruction can not appear in FPINST2, nor can
		 * it cause an exception.  Therefore, we do not have to
		 * emulate it.
		 */
	}
	return exceptions & ~VFP_NAN_FLAG;
}

/*
 * Package up a bounce condition.
 */
void VFP_bounce(u32 trigger, u32 fpexc, struct pt_regs *regs)
{
	u32 fpscr, orig_fpscr, fpsid, exceptions;

	pr_debug("VFP: bounce: trigger %08x fpexc %08x\n", trigger, fpexc);

	/*
	 * At this point, FPEXC can have the following configuration:
	 *
	 *  EX DEX IXE
	 *  0   1   x   - synchronous exception
	 *  1   x   0   - asynchronous exception
	 *  1   x   1   - sychronous on VFP subarch 1 and asynchronous on later
	 *  0   0   1   - synchronous on VFP9 (non-standard subarch 1
	 *                implementation), undefined otherwise
	 *
	 * Clear various bits and enable access to the VFP so we can
	 * handle the bounce.
	 */
	fmxr(FPEXC, fpexc & ~(FPEXC_EX|FPEXC_DEX|FPEXC_FP2V|FPEXC_VV|FPEXC_TRAP_MASK));

	fpsid = fmrx(FPSID);
	orig_fpscr = fpscr = fmrx(FPSCR);

	/*
	 * Check for the special VFP subarch 1 and FPSCR.IXE bit case
	 */
	if ((fpsid & FPSID_ARCH_MASK) == (1 << FPSID_ARCH_BIT)
	    && (fpscr & FPSCR_IXE)) {
		/*
		 * Synchronous exception, emulate the trigger instruction
		 */
		goto emulate;
	}

	if (fpexc & FPEXC_EX) {
#ifndef CONFIG_CPU_FEROCEON
		/*
		 * Asynchronous exception. The instruction is read from FPINST
		 * and the interrupted instruction has to be restarted.
		 */
		trigger = fmrx(FPINST);
		regs->ARM_pc -= 4;
#endif
	} else if (!(fpexc & FPEXC_DEX)) {
		/*
		 * Illegal combination of bits. It can be caused by an
		 * unallocated VFP instruction but with FPSCR.IXE set and not
		 * on VFP subarch 1.
		 */
		 vfp_raise_exceptions(VFP_EXCEPTION_ERROR, trigger, fpscr, regs);
		goto exit;
	}

	/*
	 * Modify fpscr to indicate the number of iterations remaining.
	 * If FPEXC.EX is 0, FPEXC.DEX is 1 and the FPEXC.VV bit indicates
	 * whether FPEXC.VECITR or FPSCR.LEN is used.
	 */
	if (fpexc & (FPEXC_EX | FPEXC_VV)) {
		u32 len;

		len = fpexc + (1 << FPEXC_LENGTH_BIT);

		fpscr &= ~FPSCR_LENGTH_MASK;
		fpscr |= (len & FPEXC_LENGTH_MASK) << (FPSCR_LENGTH_BIT - FPEXC_LENGTH_BIT);
	}

	/*
	 * Handle the first FP instruction.  We used to take note of the
	 * FPEXC bounce reason, but this appears to be unreliable.
	 * Emulate the bounced instruction instead.
	 */
	exceptions = vfp_emulate_instruction(trigger, fpscr, regs);
	if (exceptions)
		vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs);

	/*
	 * If there isn't a second FP instruction, exit now. Note that
	 * the FPEXC.FP2V bit is valid only if FPEXC.EX is 1.
	 */
	if ((fpexc & (FPEXC_EX | FPEXC_FP2V)) != (FPEXC_EX | FPEXC_FP2V))
		goto exit;

	/*
	 * The barrier() here prevents fpinst2 being read
	 * before the condition above.
	 */
	barrier();
	trigger = fmrx(FPINST2);

 emulate:
	exceptions = vfp_emulate_instruction(trigger, orig_fpscr, regs);
	if (exceptions)
		vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs);
 exit:
	preempt_enable();
}

static void vfp_enable(void *unused)
{
	u32 access;

	BUG_ON(preemptible());
	access = get_copro_access();

	/*
	 * Enable full access to VFP (cp10 and cp11)
	 */
	set_copro_access(access | CPACC_FULL(10) | CPACC_FULL(11));
}

/* Called by platforms on which we want to disable VFP because it may not be
 * present on all CPUs within a SMP complex. Needs to be called prior to
 * vfp_init().
 */
void __init vfp_disable(void)
{
	if (VFP_arch) {
		pr_debug("%s: should be called prior to vfp_init\n", __func__);
		return;
	}
	VFP_arch = 1;
}

#ifdef CONFIG_CPU_PM
static int vfp_pm_suspend(void)
{
	struct thread_info *ti = current_thread_info();
	u32 fpexc = fmrx(FPEXC);

	/* if vfp is on, then save state for resumption */
	if (fpexc & FPEXC_EN) {
		pr_debug("%s: saving vfp state\n", __func__);
		vfp_save_state(&ti->vfpstate, fpexc);

		/* disable, just in case */
		fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
	} else if (vfp_current_hw_state[ti->cpu]) {
#ifndef CONFIG_SMP
		fmxr(FPEXC, fpexc | FPEXC_EN);
		vfp_save_state(vfp_current_hw_state[ti->cpu], fpexc);
		fmxr(FPEXC, fpexc);
#endif
	}

	/* clear any information we had about last context state */
	vfp_current_hw_state[ti->cpu] = NULL;

	return 0;
}

static void vfp_pm_resume(void)
{
	/* ensure we have access to the vfp */
	vfp_enable(NULL);

	/* and disable it to ensure the next usage restores the state */
	fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
}

static int vfp_cpu_pm_notifier(struct notifier_block *self, unsigned long cmd,
	void *v)
{
	switch (cmd) {
	case CPU_PM_ENTER:
		vfp_pm_suspend();
		break;
	case CPU_PM_ENTER_FAILED:
	case CPU_PM_EXIT:
		vfp_pm_resume();
		break;
	}
	return NOTIFY_OK;
}

static struct notifier_block vfp_cpu_pm_notifier_block = {
	.notifier_call = vfp_cpu_pm_notifier,
};

static void vfp_pm_init(void)
{
	cpu_pm_register_notifier(&vfp_cpu_pm_notifier_block);
}

#else
static inline void vfp_pm_init(void) { }
#endif /* CONFIG_CPU_PM */

/*
 * Ensure that the VFP state stored in 'thread->vfpstate' is up to date
 * with the hardware state.
 */
void vfp_sync_hwstate(struct thread_info *thread)
{
	unsigned int cpu = get_cpu();

	if (vfp_state_in_hw(cpu, thread)) {
		u32 fpexc = fmrx(FPEXC);

		/*
		 * Save the last VFP state on this CPU.
		 */
		fmxr(FPEXC, fpexc | FPEXC_EN);
		vfp_save_state(&thread->vfpstate, fpexc | FPEXC_EN);
		fmxr(FPEXC, fpexc);
	}

	put_cpu();
}

/* Ensure that the thread reloads the hardware VFP state on the next use. */
void vfp_flush_hwstate(struct thread_info *thread)
{
	unsigned int cpu = get_cpu();

	vfp_force_reload(cpu, thread);

	put_cpu();
}

/*
 * Save the current VFP state into the provided structures and prepare
 * for entry into a new function (signal handler).
 */
int vfp_preserve_user_clear_hwstate(struct user_vfp *ufp,
				    struct user_vfp_exc *ufp_exc)
{
	struct thread_info *thread = current_thread_info();
	struct vfp_hard_struct *hwstate = &thread->vfpstate.hard;

	/* Ensure that the saved hwstate is up-to-date. */
	vfp_sync_hwstate(thread);

	/*
	 * Copy the floating point registers. There can be unused
	 * registers see asm/hwcap.h for details.
	 */
	memcpy(&ufp->fpregs, &hwstate->fpregs, sizeof(hwstate->fpregs));

	/*
	 * Copy the status and control register.
	 */
	ufp->fpscr = hwstate->fpscr;

	/*
	 * Copy the exception registers.
	 */
	ufp_exc->fpexc = hwstate->fpexc;
	ufp_exc->fpinst = hwstate->fpinst;
	ufp_exc->fpinst2 = hwstate->fpinst2;

	/* Ensure that VFP is disabled. */
	vfp_flush_hwstate(thread);

	/*
	 * As per the PCS, clear the length and stride bits for function
	 * entry.
	 */
	hwstate->fpscr &= ~(FPSCR_LENGTH_MASK | FPSCR_STRIDE_MASK);
	return 0;
}

/* Sanitise and restore the current VFP state from the provided structures. */
int vfp_restore_user_hwstate(struct user_vfp *ufp, struct user_vfp_exc *ufp_exc)
{
	struct thread_info *thread = current_thread_info();
	struct vfp_hard_struct *hwstate = &thread->vfpstate.hard;
	unsigned long fpexc;

	/* Disable VFP to avoid corrupting the new thread state. */
	vfp_flush_hwstate(thread);

	/*
	 * Copy the floating point registers. There can be unused
	 * registers see asm/hwcap.h for details.
	 */
	memcpy(&hwstate->fpregs, &ufp->fpregs, sizeof(hwstate->fpregs));
	/*
	 * Copy the status and control register.
	 */
	hwstate->fpscr = ufp->fpscr;

	/*
	 * Sanitise and restore the exception registers.
	 */
	fpexc = ufp_exc->fpexc;

	/* Ensure the VFP is enabled. */
	fpexc |= FPEXC_EN;

	/* Ensure FPINST2 is invalid and the exception flag is cleared. */
	fpexc &= ~(FPEXC_EX | FPEXC_FP2V);
	hwstate->fpexc = fpexc;

	hwstate->fpinst = ufp_exc->fpinst;
	hwstate->fpinst2 = ufp_exc->fpinst2;

	return 0;
}

/*
 * VFP hardware can lose all context when a CPU goes offline.
 * As we will be running in SMP mode with CPU hotplug, we will save the
 * hardware state at every thread switch.  We clear our held state when
 * a CPU has been killed, indicating that the VFP hardware doesn't contain
 * a threads VFP state.  When a CPU starts up, we re-enable access to the
 * VFP hardware. The callbacks below are called on the CPU which
 * is being offlined/onlined.
 */
static int vfp_dying_cpu(unsigned int cpu)
{
	vfp_current_hw_state[cpu] = NULL;
	return 0;
}

static int vfp_starting_cpu(unsigned int unused)
{
	vfp_enable(NULL);
	return 0;
}

#ifdef CONFIG_KERNEL_MODE_NEON

static int vfp_kmode_exception(struct pt_regs *regs, unsigned int instr)
{
	/*
	 * If we reach this point, a floating point exception has been raised
	 * while running in kernel mode. If the NEON/VFP unit was enabled at the
	 * time, it means a VFP instruction has been issued that requires
	 * software assistance to complete, something which is not currently
	 * supported in kernel mode.
	 * If the NEON/VFP unit was disabled, and the location pointed to below
	 * is properly preceded by a call to kernel_neon_begin(), something has
	 * caused the task to be scheduled out and back in again. In this case,
	 * rebuilding and running with CONFIG_DEBUG_ATOMIC_SLEEP enabled should
	 * be helpful in localizing the problem.
	 */
	if (fmrx(FPEXC) & FPEXC_EN)
		pr_crit("BUG: unsupported FP instruction in kernel mode\n");
	else
		pr_crit("BUG: FP instruction issued in kernel mode with FP unit disabled\n");
	pr_crit("FPEXC == 0x%08x\n", fmrx(FPEXC));
	return 1;
}

static struct undef_hook vfp_kmode_exception_hook[] = {{
	.instr_mask	= 0xfe000000,
	.instr_val	= 0xf2000000,
	.cpsr_mask	= MODE_MASK | PSR_T_BIT,
	.cpsr_val	= SVC_MODE,
	.fn		= vfp_kmode_exception,
}, {
	.instr_mask	= 0xff100000,
	.instr_val	= 0xf4000000,
	.cpsr_mask	= MODE_MASK | PSR_T_BIT,
	.cpsr_val	= SVC_MODE,
	.fn		= vfp_kmode_exception,
}, {
	.instr_mask	= 0xef000000,
	.instr_val	= 0xef000000,
	.cpsr_mask	= MODE_MASK | PSR_T_BIT,
	.cpsr_val	= SVC_MODE | PSR_T_BIT,
	.fn		= vfp_kmode_exception,
}, {
	.instr_mask	= 0xff100000,
	.instr_val	= 0xf9000000,
	.cpsr_mask	= MODE_MASK | PSR_T_BIT,
	.cpsr_val	= SVC_MODE | PSR_T_BIT,
	.fn		= vfp_kmode_exception,
}, {
	.instr_mask	= 0x0c000e00,
	.instr_val	= 0x0c000a00,
	.cpsr_mask	= MODE_MASK,
	.cpsr_val	= SVC_MODE,
	.fn		= vfp_kmode_exception,
}};

static int __init vfp_kmode_exception_hook_init(void)
{
	int i;

	for (i = 0; i < ARRAY_SIZE(vfp_kmode_exception_hook); i++)
		register_undef_hook(&vfp_kmode_exception_hook[i]);
	return 0;
}
subsys_initcall(vfp_kmode_exception_hook_init);

/*
 * Kernel-side NEON support functions
 */
void kernel_neon_begin(void)
{
	struct thread_info *thread = current_thread_info();
	unsigned int cpu;
	u32 fpexc;

	/*
	 * Kernel mode NEON is only allowed outside of interrupt context
	 * with preemption disabled. This will make sure that the kernel
	 * mode NEON register contents never need to be preserved.
	 */
	BUG_ON(in_interrupt());
	cpu = get_cpu();

	fpexc = fmrx(FPEXC) | FPEXC_EN;
	fmxr(FPEXC, fpexc);

	/*
	 * Save the userland NEON/VFP state. Under UP,
	 * the owner could be a task other than 'current'
	 */
	if (vfp_state_in_hw(cpu, thread))
		vfp_save_state(&thread->vfpstate, fpexc);
#ifndef CONFIG_SMP
	else if (vfp_current_hw_state[cpu] != NULL)
		vfp_save_state(vfp_current_hw_state[cpu], fpexc);
#endif
	vfp_current_hw_state[cpu] = NULL;
}
EXPORT_SYMBOL(kernel_neon_begin);

void kernel_neon_end(void)
{
	/* Disable the NEON/VFP unit. */
	fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
	put_cpu();
}
EXPORT_SYMBOL(kernel_neon_end);

#endif /* CONFIG_KERNEL_MODE_NEON */

static int __init vfp_detect(struct pt_regs *regs, unsigned int instr)
{
	VFP_arch = UINT_MAX;	/* mark as not present */
	regs->ARM_pc += 4;
	return 0;
}

static struct undef_hook vfp_detect_hook __initdata = {
	.instr_mask	= 0x0c000e00,
	.instr_val	= 0x0c000a00,
	.cpsr_mask	= MODE_MASK,
	.cpsr_val	= SVC_MODE,
	.fn		= vfp_detect,
};

/*
 * VFP support code initialisation.
 */
static int __init vfp_init(void)
{
	unsigned int vfpsid;
	unsigned int cpu_arch = cpu_architecture();
	unsigned int isar6;

	/*
	 * Enable the access to the VFP on all online CPUs so the
	 * following test on FPSID will succeed.
	 */
	if (cpu_arch >= CPU_ARCH_ARMv6)
		on_each_cpu(vfp_enable, NULL, 1);

	/*
	 * First check that there is a VFP that we can use.
	 * The handler is already setup to just log calls, so
	 * we just need to read the VFPSID register.
	 */
	register_undef_hook(&vfp_detect_hook);
	barrier();
	vfpsid = fmrx(FPSID);
	barrier();
	unregister_undef_hook(&vfp_detect_hook);
	vfp_vector = vfp_null_entry;

	pr_info("VFP support v0.3: ");
	if (VFP_arch) {
		pr_cont("not present\n");
		return 0;
	/* Extract the architecture on CPUID scheme */
	} else if ((read_cpuid_id() & 0x000f0000) == 0x000f0000) {
		VFP_arch = vfpsid & FPSID_CPUID_ARCH_MASK;
		VFP_arch >>= FPSID_ARCH_BIT;
		/*
		 * Check for the presence of the Advanced SIMD
		 * load/store instructions, integer and single
		 * precision floating point operations. Only check
		 * for NEON if the hardware has the MVFR registers.
		 */
		if (IS_ENABLED(CONFIG_NEON) &&
		   (fmrx(MVFR1) & 0x000fff00) == 0x00011100)
			elf_hwcap |= HWCAP_NEON;

		if (IS_ENABLED(CONFIG_VFPv3)) {
			u32 mvfr0 = fmrx(MVFR0);
			if (((mvfr0 & MVFR0_DP_MASK) >> MVFR0_DP_BIT) == 0x2 ||
			    ((mvfr0 & MVFR0_SP_MASK) >> MVFR0_SP_BIT) == 0x2) {
				elf_hwcap |= HWCAP_VFPv3;
				/*
				 * Check for VFPv3 D16 and VFPv4 D16.  CPUs in
				 * this configuration only have 16 x 64bit
				 * registers.
				 */
				if ((mvfr0 & MVFR0_A_SIMD_MASK) == 1)
					/* also v4-D16 */
					elf_hwcap |= HWCAP_VFPv3D16;
				else
					elf_hwcap |= HWCAP_VFPD32;
			}

			if ((fmrx(MVFR1) & 0xf0000000) == 0x10000000)
				elf_hwcap |= HWCAP_VFPv4;
			if (((fmrx(MVFR1) & MVFR1_ASIMDHP_MASK) >> MVFR1_ASIMDHP_BIT) == 0x2)
				elf_hwcap |= HWCAP_ASIMDHP;
			if (((fmrx(MVFR1) & MVFR1_FPHP_MASK) >> MVFR1_FPHP_BIT) == 0x3)
				elf_hwcap |= HWCAP_FPHP;
		}

		/*
		 * Check for the presence of Advanced SIMD Dot Product
		 * instructions.
		 */
		isar6 = read_cpuid_ext(CPUID_EXT_ISAR6);
		if (cpuid_feature_extract_field(isar6, 4) == 0x1)
			elf_hwcap |= HWCAP_ASIMDDP;
		/*
		 * Check for the presence of Advanced SIMD Floating point
		 * half-precision multiplication instructions.
		 */
		if (cpuid_feature_extract_field(isar6, 8) == 0x1)
			elf_hwcap |= HWCAP_ASIMDFHM;
		/*
		 * Check for the presence of Advanced SIMD Bfloat16
		 * floating point instructions.
		 */
		if (cpuid_feature_extract_field(isar6, 20) == 0x1)
			elf_hwcap |= HWCAP_ASIMDBF16;
		/*
		 * Check for the presence of Advanced SIMD and floating point
		 * Int8 matrix multiplication instructions instructions.
		 */
		if (cpuid_feature_extract_field(isar6, 24) == 0x1)
			elf_hwcap |= HWCAP_I8MM;

	/* Extract the architecture version on pre-cpuid scheme */
	} else {
		if (vfpsid & FPSID_NODOUBLE) {
			pr_cont("no double precision support\n");
			return 0;
		}

		VFP_arch = (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT;
	}

	cpuhp_setup_state_nocalls(CPUHP_AP_ARM_VFP_STARTING,
				  "arm/vfp:starting", vfp_starting_cpu,
				  vfp_dying_cpu);

	vfp_vector = vfp_support_entry;

	thread_register_notifier(&vfp_notifier_block);
	vfp_pm_init();

	/*
	 * We detected VFP, and the support code is
	 * in place; report VFP support to userspace.
	 */
	elf_hwcap |= HWCAP_VFP;

	pr_cont("implementor %02x architecture %d part %02x variant %x rev %x\n",
		(vfpsid & FPSID_IMPLEMENTER_MASK) >> FPSID_IMPLEMENTER_BIT,
		VFP_arch,
		(vfpsid & FPSID_PART_MASK) >> FPSID_PART_BIT,
		(vfpsid & FPSID_VARIANT_MASK) >> FPSID_VARIANT_BIT,
		(vfpsid & FPSID_REV_MASK) >> FPSID_REV_BIT);

	return 0;
}

core_initcall(vfp_init);


code:
/*
 *  linux/arch/arm/vfp/vfpsingle.c
 *
 * This code is derived in part from John R. Housers softfloat library, which
 * carries the following notice:
 *
 * ===========================================================================
 * This C source file is part of the SoftFloat IEC/IEEE Floating-point
 * Arithmetic Package, Release 2.
 *
 * Written by John R. Hauser.  This work was made possible in part by the
 * International Computer Science Institute, located at Suite 600, 1947 Center
 * Street, Berkeley, California 94704.  Funding was partially provided by the
 * National Science Foundation under grant MIP-9311980.  The original version
 * of this code was written as part of a project to build a fixed-point vector
 * processor in collaboration with the University of California at Berkeley,
 * overseen by Profs. Nelson Morgan and John Wawrzynek.  More information
 * is available through the web page `http://HTTP.CS.Berkeley.EDU/~jhauser/
 * arithmetic/softfloat.html'.
 *
 * THIS SOFTWARE IS DISTRIBUTED AS IS, FOR FREE.  Although reasonable effort
 * has been made to avoid it, THIS SOFTWARE MAY CONTAIN FAULTS THAT WILL AT
 * TIMES RESULT IN INCORRECT BEHAVIOR.  USE OF THIS SOFTWARE IS RESTRICTED TO
 * PERSONS AND ORGANIZATIONS WHO CAN AND WILL TAKE FULL RESPONSIBILITY FOR ANY
 * AND ALL LOSSES, COSTS, OR OTHER PROBLEMS ARISING FROM ITS USE.
 *
 * Derivative works are acceptable, even for commercial purposes, so long as
 * (1) they include prominent notice that the work is derivative, and (2) they
 * include prominent notice akin to these three paragraphs for those parts of
 * this code that are retained.
 * ===========================================================================
 */
#include <linux/kernel.h>
#include <linux/bitops.h>

#include <asm/div64.h>
#include <asm/vfp.h>

#include "vfpinstr.h"
#include "vfp.h"

static struct vfp_single vfp_single_default_qnan = {
	.exponent	= 255,
	.sign		= 0,
	.significand	= VFP_SINGLE_SIGNIFICAND_QNAN,
};

static void vfp_single_dump(const char *str, struct vfp_single *s)
{
	pr_debug("VFP: %s: sign=%d exponent=%d significand=%08x\n",
		 str, s->sign != 0, s->exponent, s->significand);
}

static void vfp_single_normalise_denormal(struct vfp_single *vs)
{
	int bits = 31 - fls(vs->significand);

	vfp_single_dump("normalise_denormal: in", vs);

	if (bits) {
		vs->exponent -= bits - 1;
		vs->significand <<= bits;
	}

	vfp_single_dump("normalise_denormal: out", vs);
}

#ifndef DEBUG
#define vfp_single_normaliseround(sd,vsd,fpscr,except,func) __vfp_single_normaliseround(sd,vsd,fpscr,except)
u32 __vfp_single_normaliseround(int sd, struct vfp_single *vs, u32 fpscr, u32 exceptions)
#else
u32 vfp_single_normaliseround(int sd, struct vfp_single *vs, u32 fpscr, u32 exceptions, const char *func)
#endif
{
	u32 significand, incr, rmode;
	int exponent, shift, underflow;

	vfp_single_dump("pack: in", vs);

	/*
	 * Infinities and NaNs are a special case.
	 */
	if (vs->exponent == 255 && (vs->significand == 0 || exceptions))
		goto pack;

	/*
	 * Special-case zero.
	 */
	if (vs->significand == 0) {
		vs->exponent = 0;
		goto pack;
	}

	exponent = vs->exponent;
	significand = vs->significand;

	/*
	 * Normalise first.  Note that we shift the significand up to
	 * bit 31, so we have VFP_SINGLE_LOW_BITS + 1 below the least
	 * significant bit.
	 */
	shift = 32 - fls(significand);
	if (shift < 32 && shift) {
		exponent -= shift;
		significand <<= shift;
	}

#ifdef DEBUG
	vs->exponent = exponent;
	vs->significand = significand;
	vfp_single_dump("pack: normalised", vs);
#endif

	/*
	 * Tiny number?
	 */
	underflow = exponent < 0;
	if (underflow) {
		significand = vfp_shiftright32jamming(significand, -exponent);
		exponent = 0;
#ifdef DEBUG
		vs->exponent = exponent;
		vs->significand = significand;
		vfp_single_dump("pack: tiny number", vs);
#endif
		if (!(significand & ((1 << (VFP_SINGLE_LOW_BITS + 1)) - 1)))
			underflow = 0;
	}

	/*
	 * Select rounding increment.
	 */
	incr = 0;
	rmode = fpscr & FPSCR_RMODE_MASK;

	if (rmode == FPSCR_ROUND_NEAREST) {
		incr = 1 << VFP_SINGLE_LOW_BITS;
		if ((significand & (1 << (VFP_SINGLE_LOW_BITS + 1))) == 0)
			incr -= 1;
	} else if (rmode == FPSCR_ROUND_TOZERO) {
		incr = 0;
	} else if ((rmode == FPSCR_ROUND_PLUSINF) ^ (vs->sign != 0))
		incr = (1 << (VFP_SINGLE_LOW_BITS + 1)) - 1;

	pr_debug("VFP: rounding increment = 0x%08x\n", incr);

	/*
	 * Is our rounding going to overflow?
	 */
	if ((significand + incr) < significand) {
		exponent += 1;
		significand = (significand >> 1) | (significand & 1);
		incr >>= 1;
#ifdef DEBUG
		vs->exponent = exponent;
		vs->significand = significand;
		vfp_single_dump("pack: overflow", vs);
#endif
	}

	/*
	 * If any of the low bits (which will be shifted out of the
	 * number) are non-zero, the result is inexact.
	 */
	if (significand & ((1 << (VFP_SINGLE_LOW_BITS + 1)) - 1))
		exceptions |= FPSCR_IXC;

	/*
	 * Do our rounding.
	 */
	significand += incr;

	/*
	 * Infinity?
	 */
	if (exponent >= 254) {
		exceptions |= FPSCR_OFC | FPSCR_IXC;
		if (incr == 0) {
			vs->exponent = 253;
			vs->significand = 0x7fffffff;
		} else {
			vs->exponent = 255;		/* infinity */
			vs->significand = 0;
		}
	} else {
		if (significand >> (VFP_SINGLE_LOW_BITS + 1) == 0)
			exponent = 0;
		if (exponent || significand > 0x80000000)
			underflow = 0;
		if (underflow)
			exceptions |= FPSCR_UFC;
		vs->exponent = exponent;
		vs->significand = significand >> 1;
	}

 pack:
	vfp_single_dump("pack: final", vs);
	{
		s32 d = vfp_single_pack(vs);
#ifdef DEBUG
		pr_debug("VFP: %s: d(s%d)=%08x exceptions=%08x\n", func,
			 sd, d, exceptions);
#endif
		vfp_put_float(d, sd);
	}

	return exceptions;
}

/*
 * Propagate the NaN, setting exceptions if it is signalling.
 * 'n' is always a NaN.  'm' may be a number, NaN or infinity.
 */
static u32
vfp_propagate_nan(struct vfp_single *vsd, struct vfp_single *vsn,
		  struct vfp_single *vsm, u32 fpscr)
{
	struct vfp_single *nan;
	int tn, tm = 0;

	tn = vfp_single_type(vsn);

	if (vsm)
		tm = vfp_single_type(vsm);

	if (fpscr & FPSCR_DEFAULT_NAN)
		/*
		 * Default NaN mode - always returns a quiet NaN
		 */
		nan = &vfp_single_default_qnan;
	else {
		/*
		 * Contemporary mode - select the first signalling
		 * NAN, or if neither are signalling, the first
		 * quiet NAN.
		 */
		if (tn == VFP_SNAN || (tm != VFP_SNAN && tn == VFP_QNAN))
			nan = vsn;
		else
			nan = vsm;
		/*
		 * Make the NaN quiet.
		 */
		nan->significand |= VFP_SINGLE_SIGNIFICAND_QNAN;
	}

	*vsd = *nan;

	/*
	 * If one was a signalling NAN, raise invalid operation.
	 */
	return tn == VFP_SNAN || tm == VFP_SNAN ? FPSCR_IOC : VFP_NAN_FLAG;
}

/*
 * Extended operations
 */
static u32 vfp_single_fabs(int sd, int unused, s32 m, u32 fpscr)
{
	vfp_put_float(vfp_single_packed_abs(m), sd);
	return 0;
}

static u32 vfp_single_fcpy(int sd, int unused, s32 m, u32 fpscr)
{
	vfp_put_float(m, sd);
	return 0;
}

static u32 vfp_single_fneg(int sd, int unused, s32 m, u32 fpscr)
{
	vfp_put_float(vfp_single_packed_negate(m), sd);
	return 0;
}

static const u16 sqrt_oddadjust[] = {
	0x0004, 0x0022, 0x005d, 0x00b1, 0x011d, 0x019f, 0x0236, 0x02e0,
	0x039c, 0x0468, 0x0545, 0x0631, 0x072b, 0x0832, 0x0946, 0x0a67
};

static const u16 sqrt_evenadjust[] = {
	0x0a2d, 0x08af, 0x075a, 0x0629, 0x051a, 0x0429, 0x0356, 0x029e,
	0x0200, 0x0179, 0x0109, 0x00af, 0x0068, 0x0034, 0x0012, 0x0002
};

u32 vfp_estimate_sqrt_significand(u32 exponent, u32 significand)
{
	int index;
	u32 z, a;

	if ((significand & 0xc0000000) != 0x40000000) {
		pr_warn("VFP: estimate_sqrt: invalid significand\n");
	}

	a = significand << 1;
	index = (a >> 27) & 15;
	if (exponent & 1) {
		z = 0x4000 + (a >> 17) - sqrt_oddadjust[index];
		z = ((a / z) << 14) + (z << 15);
		a >>= 1;
	} else {
		z = 0x8000 + (a >> 17) - sqrt_evenadjust[index];
		z = a / z + z;
		z = (z >= 0x20000) ? 0xffff8000 : (z << 15);
		if (z <= a)
			return (s32)a >> 1;
	}
	{
		u64 v = (u64)a << 31;
		do_div(v, z);
		return v + (z >> 1);
	}
}

static u32 vfp_single_fsqrt(int sd, int unused, s32 m, u32 fpscr)
{
	struct vfp_single vsm, vsd;
	int ret, tm;

	vfp_single_unpack(&vsm, m);
	tm = vfp_single_type(&vsm);
	if (tm & (VFP_NAN|VFP_INFINITY)) {
		struct vfp_single *vsp = &vsd;

		if (tm & VFP_NAN)
			ret = vfp_propagate_nan(vsp, &vsm, NULL, fpscr);
		else if (vsm.sign == 0) {
 sqrt_copy:
			vsp = &vsm;
			ret = 0;
		} else {
 sqrt_invalid:
			vsp = &vfp_single_default_qnan;
			ret = FPSCR_IOC;
		}
		vfp_put_float(vfp_single_pack(vsp), sd);
		return ret;
	}

	/*
	 * sqrt(+/- 0) == +/- 0
	 */
	if (tm & VFP_ZERO)
		goto sqrt_copy;

	/*
	 * Normalise a denormalised number
	 */
	if (tm & VFP_DENORMAL)
		vfp_single_normalise_denormal(&vsm);

	/*
	 * sqrt(<0) = invalid
	 */
	if (vsm.sign)
		goto sqrt_invalid;

	vfp_single_dump("sqrt", &vsm);

	/*
	 * Estimate the square root.
	 */
	vsd.sign = 0;
	vsd.exponent = ((vsm.exponent - 127) >> 1) + 127;
	vsd.significand = vfp_estimate_sqrt_significand(vsm.exponent, vsm.significand) + 2;

	vfp_single_dump("sqrt estimate", &vsd);

	/*
	 * And now adjust.
	 */
	if ((vsd.significand & VFP_SINGLE_LOW_BITS_MASK) <= 5) {
		if (vsd.significand < 2) {
			vsd.significand = 0xffffffff;
		} else {
			u64 term;
			s64 rem;
			vsm.significand <<= !(vsm.exponent & 1);
			term = (u64)vsd.significand * vsd.significand;
			rem = ((u64)vsm.significand << 32) - term;

			pr_debug("VFP: term=%016llx rem=%016llx\n", term, rem);

			while (rem < 0) {
				vsd.significand -= 1;
				rem += ((u64)vsd.significand << 1) | 1;
			}
			vsd.significand |= rem != 0;
		}
	}
	vsd.significand = vfp_shiftright32jamming(vsd.significand, 1);

	return vfp_single_normaliseround(sd, &vsd, fpscr, 0, "fsqrt");
}

/*
 * Equal	:= ZC
 * Less than	:= N
 * Greater than	:= C
 * Unordered	:= CV
 */
static u32 vfp_compare(int sd, int signal_on_qnan, s32 m, u32 fpscr)
{
	s32 d;
	u32 ret = 0;

	d = vfp_get_float(sd);
	if (vfp_single_packed_exponent(m) == 255 && vfp_single_packed_mantissa(m)) {
		ret |= FPSCR_C | FPSCR_V;
		if (signal_on_qnan || !(vfp_single_packed_mantissa(m) & (1 << (VFP_SINGLE_MANTISSA_BITS - 1))))
			/*
			 * Signalling NaN, or signalling on quiet NaN
			 */
			ret |= FPSCR_IOC;
	}

	if (vfp_single_packed_exponent(d) == 255 && vfp_single_packed_mantissa(d)) {
		ret |= FPSCR_C | FPSCR_V;
		if (signal_on_qnan || !(vfp_single_packed_mantissa(d) & (1 << (VFP_SINGLE_MANTISSA_BITS - 1))))
			/*
			 * Signalling NaN, or signalling on quiet NaN
			 */
			ret |= FPSCR_IOC;
	}

	if (ret == 0) {
		if (d == m || vfp_single_packed_abs(d | m) == 0) {
			/*
			 * equal
			 */
			ret |= FPSCR_Z | FPSCR_C;
		} else if (vfp_single_packed_sign(d ^ m)) {
			/*
			 * different signs
			 */
			if (vfp_single_packed_sign(d))
				/*
				 * d is negative, so d < m
				 */
				ret |= FPSCR_N;
			else
				/*
				 * d is positive, so d > m
				 */
				ret |= FPSCR_C;
		} else if ((vfp_single_packed_sign(d) != 0) ^ (d < m)) {
			/*
			 * d < m
			 */
			ret |= FPSCR_N;
		} else if ((vfp_single_packed_sign(d) != 0) ^ (d > m)) {
			/*
			 * d > m
			 */
			ret |= FPSCR_C;
		}
	}
	return ret;
}

static u32 vfp_single_fcmp(int sd, int unused, s32 m, u32 fpscr)
{
	return vfp_compare(sd, 0, m, fpscr);
}

static u32 vfp_single_fcmpe(int sd, int unused, s32 m, u32 fpscr)
{
	return vfp_compare(sd, 1, m, fpscr);
}

static u32 vfp_single_fcmpz(int sd, int unused, s32 m, u32 fpscr)
{
	return vfp_compare(sd, 0, 0, fpscr);
}

static u32 vfp_single_fcmpez(int sd, int unused, s32 m, u32 fpscr)
{
	return vfp_compare(sd, 1, 0, fpscr);
}

static u32 vfp_single_fcvtd(int dd, int unused, s32 m, u32 fpscr)
{
	struct vfp_single vsm;
	struct vfp_double vdd;
	int tm;
	u32 exceptions = 0;

	vfp_single_unpack(&vsm, m);

	tm = vfp_single_type(&vsm);

	/*
	 * If we have a signalling NaN, signal invalid operation.
	 */
	if (tm == VFP_SNAN)
		exceptions = FPSCR_IOC;

	if (tm & VFP_DENORMAL)
		vfp_single_normalise_denormal(&vsm);

	vdd.sign = vsm.sign;
	vdd.significand = (u64)vsm.significand << 32;

	/*
	 * If we have an infinity or NaN, the exponent must be 2047.
	 */
	if (tm & (VFP_INFINITY|VFP_NAN)) {
		vdd.exponent = 2047;
		if (tm == VFP_QNAN)
			vdd.significand |= VFP_DOUBLE_SIGNIFICAND_QNAN;
		goto pack_nan;
	} else if (tm & VFP_ZERO)
		vdd.exponent = 0;
	else
		vdd.exponent = vsm.exponent + (1023 - 127);

	return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, "fcvtd");

 pack_nan:
	vfp_put_double(vfp_double_pack(&vdd), dd);
	return exceptions;
}

static u32 vfp_single_fuito(int sd, int unused, s32 m, u32 fpscr)
{
	struct vfp_single vs;

	vs.sign = 0;
	vs.exponent = 127 + 31 - 1;
	vs.significand = (u32)m;

	return vfp_single_normaliseround(sd, &vs, fpscr, 0, "fuito");
}

static u32 vfp_single_fsito(int sd, int unused, s32 m, u32 fpscr)
{
	struct vfp_single vs;

	vs.sign = (m & 0x80000000) >> 16;
	vs.exponent = 127 + 31 - 1;
	vs.significand = vs.sign ? -m : m;

	return vfp_single_normaliseround(sd, &vs, fpscr, 0, "fsito");
}

static u32 vfp_single_ftoui(int sd, int unused, s32 m, u32 fpscr)
{
	struct vfp_single vsm;
	u32 d, exceptions = 0;
	int rmode = fpscr & FPSCR_RMODE_MASK;
	int tm;

	vfp_single_unpack(&vsm, m);
	vfp_single_dump("VSM", &vsm);

	/*
	 * Do we have a denormalised number?
	 */
	tm = vfp_single_type(&vsm);
	if (tm & VFP_DENORMAL)
		exceptions |= FPSCR_IDC;

	if (tm & VFP_NAN)
		vsm.sign = 0;

	if (vsm.exponent >= 127 + 32) {
		d = vsm.sign ? 0 : 0xffffffff;
		exceptions = FPSCR_IOC;
	} else if (vsm.exponent >= 127 - 1) {
		int shift = 127 + 31 - vsm.exponent;
		u32 rem, incr = 0;

		/*
		 * 2^0 <= m < 2^32-2^8
		 */
		d = (vsm.significand << 1) >> shift;
		rem = vsm.significand << (33 - shift);

		if (rmode == FPSCR_ROUND_NEAREST) {
			incr = 0x80000000;
			if ((d & 1) == 0)
				incr -= 1;
		} else if (rmode == FPSCR_ROUND_TOZERO) {
			incr = 0;
		} else if ((rmode == FPSCR_ROUND_PLUSINF) ^ (vsm.sign != 0)) {
			incr = ~0;
		}

		if ((rem + incr) < rem) {
			if (d < 0xffffffff)
				d += 1;
			else
				exceptions |= FPSCR_IOC;
		}

		if (d && vsm.sign) {
			d = 0;
			exceptions |= FPSCR_IOC;
		} else if (rem)
			exceptions |= FPSCR_IXC;
	} else {
		d = 0;
		if (vsm.exponent | vsm.significand) {
			exceptions |= FPSCR_IXC;
			if (rmode == FPSCR_ROUND_PLUSINF && vsm.sign == 0)
				d = 1;
			else if (rmode == FPSCR_ROUND_MINUSINF && vsm.sign) {
				d = 0;
				exceptions |= FPSCR_IOC;
			}
		}
	}

	pr_debug("VFP: ftoui: d(s%d)=%08x exceptions=%08x\n", sd, d, exceptions);

	vfp_put_float(d, sd);

	return exceptions;
}

static u32 vfp_single_ftouiz(int sd, int unused, s32 m, u32 fpscr)
{
	return vfp_single_ftoui(sd, unused, m, FPSCR_ROUND_TOZERO);
}

static u32 vfp_single_ftosi(int sd, int unused, s32 m, u32 fpscr)
{
	struct vfp_single vsm;
	u32 d, exceptions = 0;
	int rmode = fpscr & FPSCR_RMODE_MASK;
	int tm;

	vfp_single_unpack(&vsm, m);
	vfp_single_dump("VSM", &vsm);

	/*
	 * Do we have a denormalised number?
	 */
	tm = vfp_single_type(&vsm);
	if (vfp_single_type(&vsm) & VFP_DENORMAL)
		exceptions |= FPSCR_IDC;

	if (tm & VFP_NAN) {
		d = 0;
		exceptions |= FPSCR_IOC;
	} else if (vsm.exponent >= 127 + 32) {
		/*
		 * m >= 2^31-2^7: invalid
		 */
		d = 0x7fffffff;
		if (vsm.sign)
			d = ~d;
		exceptions |= FPSCR_IOC;
	} else if (vsm.exponent >= 127 - 1) {
		int shift = 127 + 31 - vsm.exponent;
		u32 rem, incr = 0;

		/* 2^0 <= m <= 2^31-2^7 */
		d = (vsm.significand << 1) >> shift;
		rem = vsm.significand << (33 - shift);

		if (rmode == FPSCR_ROUND_NEAREST) {
			incr = 0x80000000;
			if ((d & 1) == 0)
				incr -= 1;
		} else if (rmode == FPSCR_ROUND_TOZERO) {
			incr = 0;
		} else if ((rmode == FPSCR_ROUND_PLUSINF) ^ (vsm.sign != 0)) {
			incr = ~0;
		}

		if ((rem + incr) < rem && d < 0xffffffff)
			d += 1;
		if (d > 0x7fffffff + (vsm.sign != 0)) {
			d = 0x7fffffff + (vsm.sign != 0);
			exceptions |= FPSCR_IOC;
		} else if (rem)
			exceptions |= FPSCR_IXC;

		if (vsm.sign)
			d = -d;
	} else {
		d = 0;
		if (vsm.exponent | vsm.significand) {
			exceptions |= FPSCR_IXC;
			if (rmode == FPSCR_ROUND_PLUSINF && vsm.sign == 0)
				d = 1;
			else if (rmode == FPSCR_ROUND_MINUSINF && vsm.sign)
				d = -1;
		}
	}

	pr_debug("VFP: ftosi: d(s%d)=%08x exceptions=%08x\n", sd, d, exceptions);

	vfp_put_float((s32)d, sd);

	return exceptions;
}

static u32 vfp_single_ftosiz(int sd, int unused, s32 m, u32 fpscr)
{
	return vfp_single_ftosi(sd, unused, m, FPSCR_ROUND_TOZERO);
}

static struct op fops_ext[32] = {
	[FEXT_TO_IDX(FEXT_FCPY)]	= { vfp_single_fcpy,   0 },
	[FEXT_TO_IDX(FEXT_FABS)]	= { vfp_single_fabs,   0 },
	[FEXT_TO_IDX(FEXT_FNEG)]	= { vfp_single_fneg,   0 },
	[FEXT_TO_IDX(FEXT_FSQRT)]	= { vfp_single_fsqrt,  0 },
	[FEXT_TO_IDX(FEXT_FCMP)]	= { vfp_single_fcmp,   OP_SCALAR },
	[FEXT_TO_IDX(FEXT_FCMPE)]	= { vfp_single_fcmpe,  OP_SCALAR },
	[FEXT_TO_IDX(FEXT_FCMPZ)]	= { vfp_single_fcmpz,  OP_SCALAR },
	[FEXT_TO_IDX(FEXT_FCMPEZ)]	= { vfp_single_fcmpez, OP_SCALAR },
	[FEXT_TO_IDX(FEXT_FCVT)]	= { vfp_single_fcvtd,  OP_SCALAR|OP_DD },
	[FEXT_TO_IDX(FEXT_FUITO)]	= { vfp_single_fuito,  OP_SCALAR },
	[FEXT_TO_IDX(FEXT_FSITO)]	= { vfp_single_fsito,  OP_SCALAR },
	[FEXT_TO_IDX(FEXT_FTOUI)]	= { vfp_single_ftoui,  OP_SCALAR },
	[FEXT_TO_IDX(FEXT_FTOUIZ)]	= { vfp_single_ftouiz, OP_SCALAR },
	[FEXT_TO_IDX(FEXT_FTOSI)]	= { vfp_single_ftosi,  OP_SCALAR },
	[FEXT_TO_IDX(FEXT_FTOSIZ)]	= { vfp_single_ftosiz, OP_SCALAR },
};

static u32
vfp_single_fadd_nonnumber(struct vfp_single *vsd, struct vfp_single *vsn,
			  struct vfp_single *vsm, u32 fpscr)
{
	struct vfp_single *vsp;
	u32 exceptions = 0;
	int tn, tm;

	tn = vfp_single_type(vsn);
	tm = vfp_single_type(vsm);

	if (tn & tm & VFP_INFINITY) {
		/*
		 * Two infinities.  Are they different signs?
		 */
		if (vsn->sign ^ vsm->sign) {
			/*
			 * different signs -> invalid
			 */
			exceptions = FPSCR_IOC;
			vsp = &vfp_single_default_qnan;
		} else {
			/*
			 * same signs -> valid
			 */
			vsp = vsn;
		}
	} else if (tn & VFP_INFINITY && tm & VFP_NUMBER) {
		/*
		 * One infinity and one number -> infinity
		 */
		vsp = vsn;
	} else {
		/*
		 * 'n' is a NaN of some type
		 */
		return vfp_propagate_nan(vsd, vsn, vsm, fpscr);
	}
	*vsd = *vsp;
	return exceptions;
}

static u32
vfp_single_add(struct vfp_single *vsd, struct vfp_single *vsn,
	       struct vfp_single *vsm, u32 fpscr)
{
	u32 exp_diff, m_sig;

	if (vsn->significand & 0x80000000 ||
	    vsm->significand & 0x80000000) {
		pr_info("VFP: bad FP values in %s\n", __func__);
		vfp_single_dump("VSN", vsn);
		vfp_single_dump("VSM", vsm);
	}

	/*
	 * Ensure that 'n' is the largest magnitude number.  Note that
	 * if 'n' and 'm' have equal exponents, we do not swap them.
	 * This ensures that NaN propagation works correctly.
	 */
	if (vsn->exponent < vsm->exponent) {
		struct vfp_single *t = vsn;
		vsn = vsm;
		vsm = t;
	}

	/*
	 * Is 'n' an infinity or a NaN?  Note that 'm' may be a number,
	 * infinity or a NaN here.
	 */
	if (vsn->exponent == 255)
		return vfp_single_fadd_nonnumber(vsd, vsn, vsm, fpscr);

	/*
	 * We have two proper numbers, where 'vsn' is the larger magnitude.
	 *
	 * Copy 'n' to 'd' before doing the arithmetic.
	 */
	*vsd = *vsn;

	/*
	 * Align both numbers.
	 */
	exp_diff = vsn->exponent - vsm->exponent;
	m_sig = vfp_shiftright32jamming(vsm->significand, exp_diff);

	/*
	 * If the signs are different, we are really subtracting.
	 */
	if (vsn->sign ^ vsm->sign) {
		m_sig = vsn->significand - m_sig;
		if ((s32)m_sig < 0) {
			vsd->sign = vfp_sign_negate(vsd->sign);
			m_sig = -m_sig;
		} else if (m_sig == 0) {
			vsd->sign = (fpscr & FPSCR_RMODE_MASK) ==
				      FPSCR_ROUND_MINUSINF ? 0x8000 : 0;
		}
	} else {
		m_sig = vsn->significand + m_sig;
	}
	vsd->significand = m_sig;

	return 0;
}

static u32
vfp_single_multiply(struct vfp_single *vsd, struct vfp_single *vsn, struct vfp_single *vsm, u32 fpscr)
{
	vfp_single_dump("VSN", vsn);
	vfp_single_dump("VSM", vsm);

	/*
	 * Ensure that 'n' is the largest magnitude number.  Note that
	 * if 'n' and 'm' have equal exponents, we do not swap them.
	 * This ensures that NaN propagation works correctly.
	 */
	if (vsn->exponent < vsm->exponent) {
		struct vfp_single *t = vsn;
		vsn = vsm;
		vsm = t;
		pr_debug("VFP: swapping M <-> N\n");
	}

	vsd->sign = vsn->sign ^ vsm->sign;

	/*
	 * If 'n' is an infinity or NaN, handle it.  'm' may be anything.
	 */
	if (vsn->exponent == 255) {
		if (vsn->significand || (vsm->exponent == 255 && vsm->significand))
			return vfp_propagate_nan(vsd, vsn, vsm, fpscr);
		if ((vsm->exponent | vsm->significand) == 0) {
			*vsd = vfp_single_default_qnan;
			return FPSCR_IOC;
		}
		vsd->exponent = vsn->exponent;
		vsd->significand = 0;
		return 0;
	}

	/*
	 * If 'm' is zero, the result is always zero.  In this case,
	 * 'n' may be zero or a number, but it doesn't matter which.
	 */
	if ((vsm->exponent | vsm->significand) == 0) {
		vsd->exponent = 0;
		vsd->significand = 0;
		return 0;
	}

	/*
	 * We add 2 to the destination exponent for the same reason as
	 * the addition case - though this time we have +1 from each
	 * input operand.
	 */
	vsd->exponent = vsn->exponent + vsm->exponent - 127 + 2;
	vsd->significand = vfp_hi64to32jamming((u64)vsn->significand * vsm->significand);

	vfp_single_dump("VSD", vsd);
	return 0;
}

#define NEG_MULTIPLY	(1 << 0)
#define NEG_SUBTRACT	(1 << 1)

static u32
vfp_single_multiply_accumulate(int sd, int sn, s32 m, u32 fpscr, u32 negate, char *func)
{
	struct vfp_single vsd, vsp, vsn, vsm;
	u32 exceptions;
	s32 v;

	v = vfp_get_float(sn);
	pr_debug("VFP: s%u = %08x\n", sn, v);
	vfp_single_unpack(&vsn, v);
	if (vsn.exponent == 0 && vsn.significand)
		vfp_single_normalise_denormal(&vsn);

	vfp_single_unpack(&vsm, m);
	if (vsm.exponent == 0 && vsm.significand)
		vfp_single_normalise_denormal(&vsm);

	exceptions = vfp_single_multiply(&vsp, &vsn, &vsm, fpscr);
	if (negate & NEG_MULTIPLY)
		vsp.sign = vfp_sign_negate(vsp.sign);

	v = vfp_get_float(sd);
	pr_debug("VFP: s%u = %08x\n", sd, v);
	vfp_single_unpack(&vsn, v);
	if (vsn.exponent == 0 && vsn.significand)
		vfp_single_normalise_denormal(&vsn);
	if (negate & NEG_SUBTRACT)
		vsn.sign = vfp_sign_negate(vsn.sign);

	exceptions |= vfp_single_add(&vsd, &vsn, &vsp, fpscr);

	return vfp_single_normaliseround(sd, &vsd, fpscr, exceptions, func);
}

/*
 * Standard operations
 */

/*
 * sd = sd + (sn * sm)
 */
static u32 vfp_single_fmac(int sd, int sn, s32 m, u32 fpscr)
{
	return vfp_single_multiply_accumulate(sd, sn, m, fpscr, 0, "fmac");
}

/*
 * sd = sd - (sn * sm)
 */
static u32 vfp_single_fnmac(int sd, int sn, s32 m, u32 fpscr)
{
	return vfp_single_multiply_accumulate(sd, sn, m, fpscr, NEG_MULTIPLY, "fnmac");
}

/*
 * sd = -sd + (sn * sm)
 */
static u32 vfp_single_fmsc(int sd, int sn, s32 m, u32 fpscr)
{
	return vfp_single_multiply_accumulate(sd, sn, m, fpscr, NEG_SUBTRACT, "fmsc");
}

/*
 * sd = -sd - (sn * sm)
 */
static u32 vfp_single_fnmsc(int sd, int sn, s32 m, u32 fpscr)
{
	return vfp_single_multiply_accumulate(sd, sn, m, fpscr, NEG_SUBTRACT | NEG_MULTIPLY, "fnmsc");
}

/*
 * sd = sn * sm
 */
static u32 vfp_single_fmul(int sd, int sn, s32 m, u32 fpscr)
{
	struct vfp_single vsd, vsn, vsm;
	u32 exceptions;
	s32 n = vfp_get_float(sn);

	pr_debug("VFP: s%u = %08x\n", sn, n);

	vfp_single_unpack(&vsn, n);
	if (vsn.exponent == 0 && vsn.significand)
		vfp_single_normalise_denormal(&vsn);

	vfp_single_unpack(&vsm, m);
	if (vsm.exponent == 0 && vsm.significand)
		vfp_single_normalise_denormal(&vsm);

	exceptions = vfp_single_multiply(&vsd, &vsn, &vsm, fpscr);
	return vfp_single_normaliseround(sd, &vsd, fpscr, exceptions, "fmul");
}

/*
 * sd = -(sn * sm)
 */
static u32 vfp_single_fnmul(int sd, int sn, s32 m, u32 fpscr)
{
	struct vfp_single vsd, vsn, vsm;
	u32 exceptions;
	s32 n = vfp_get_float(sn);

	pr_debug("VFP: s%u = %08x\n", sn, n);

	vfp_single_unpack(&vsn, n);
	if (vsn.exponent == 0 && vsn.significand)
		vfp_single_normalise_denormal(&vsn);

	vfp_single_unpack(&vsm, m);
	if (vsm.exponent == 0 && vsm.significand)
		vfp_single_normalise_denormal(&vsm);

	exceptions = vfp_single_multiply(&vsd, &vsn, &vsm, fpscr);
	vsd.sign = vfp_sign_negate(vsd.sign);
	return vfp_single_normaliseround(sd, &vsd, fpscr, exceptions, "fnmul");
}

/*
 * sd = sn + sm
 */
static u32 vfp_single_fadd(int sd, int sn, s32 m, u32 fpscr)
{
	struct vfp_single vsd, vsn, vsm;
	u32 exceptions;
	s32 n = vfp_get_float(sn);

	pr_debug("VFP: s%u = %08x\n", sn, n);

	/*
	 * Unpack and normalise denormals.
	 */
	vfp_single_unpack(&vsn, n);
	if (vsn.exponent == 0 && vsn.significand)
		vfp_single_normalise_denormal(&vsn);

	vfp_single_unpack(&vsm, m);
	if (vsm.exponent == 0 && vsm.significand)
		vfp_single_normalise_denormal(&vsm);

	exceptions = vfp_single_add(&vsd, &vsn, &vsm, fpscr);

	return vfp_single_normaliseround(sd, &vsd, fpscr, exceptions, "fadd");
}

/*
 * sd = sn - sm
 */
static u32 vfp_single_fsub(int sd, int sn, s32 m, u32 fpscr)
{
	/*
	 * Subtraction is addition with one sign inverted.
	 */
	return vfp_single_fadd(sd, sn, vfp_single_packed_negate(m), fpscr);
}

/*
 * sd = sn / sm
 */
static u32 vfp_single_fdiv(int sd, int sn, s32 m, u32 fpscr)
{
	struct vfp_single vsd, vsn, vsm;
	u32 exceptions = 0;
	s32 n = vfp_get_float(sn);
	int tm, tn;

	pr_debug("VFP: s%u = %08x\n", sn, n);

	vfp_single_unpack(&vsn, n);
	vfp_single_unpack(&vsm, m);

	vsd.sign = vsn.sign ^ vsm.sign;

	tn = vfp_single_type(&vsn);
	tm = vfp_single_type(&vsm);

	/*
	 * Is n a NAN?
	 */
	if (tn & VFP_NAN)
		goto vsn_nan;

	/*
	 * Is m a NAN?
	 */
	if (tm & VFP_NAN)
		goto vsm_nan;

	/*
	 * If n and m are infinity, the result is invalid
	 * If n and m are zero, the result is invalid
	 */
	if (tm & tn & (VFP_INFINITY|VFP_ZERO))
		goto invalid;

	/*
	 * If n is infinity, the result is infinity
	 */
	if (tn & VFP_INFINITY)
		goto infinity;

	/*
	 * If m is zero, raise div0 exception
	 */
	if (tm & VFP_ZERO)
		goto divzero;

	/*
	 * If m is infinity, or n is zero, the result is zero
	 */
	if (tm & VFP_INFINITY || tn & VFP_ZERO)
		goto zero;

	if (tn & VFP_DENORMAL)
		vfp_single_normalise_denormal(&vsn);
	if (tm & VFP_DENORMAL)
		vfp_single_normalise_denormal(&vsm);

	/*
	 * Ok, we have two numbers, we can perform division.
	 */
	vsd.exponent = vsn.exponent - vsm.exponent + 127 - 1;
	vsm.significand <<= 1;
	if (vsm.significand <= (2 * vsn.significand)) {
		vsn.significand >>= 1;
		vsd.exponent++;
	}
	{
		u64 significand = (u64)vsn.significand << 32;
		do_div(significand, vsm.significand);
		vsd.significand = significand;
	}
	if ((vsd.significand & 0x3f) == 0)
		vsd.significand |= ((u64)vsm.significand * vsd.significand != (u64)vsn.significand << 32);

	return vfp_single_normaliseround(sd, &vsd, fpscr, 0, "fdiv");

 vsn_nan:
	exceptions = vfp_propagate_nan(&vsd, &vsn, &vsm, fpscr);
 pack:
	vfp_put_float(vfp_single_pack(&vsd), sd);
	return exceptions;

 vsm_nan:
	exceptions = vfp_propagate_nan(&vsd, &vsm, &vsn, fpscr);
	goto pack;

 zero:
	vsd.exponent = 0;
	vsd.significand = 0;
	goto pack;

 divzero:
	exceptions = FPSCR_DZC;
 infinity:
	vsd.exponent = 255;
	vsd.significand = 0;
	goto pack;

 invalid:
	vfp_put_float(vfp_single_pack(&vfp_single_default_qnan), sd);
	return FPSCR_IOC;
}

static struct op fops[16] = {
	[FOP_TO_IDX(FOP_FMAC)]	= { vfp_single_fmac,  0 },
	[FOP_TO_IDX(FOP_FNMAC)]	= { vfp_single_fnmac, 0 },
	[FOP_TO_IDX(FOP_FMSC)]	= { vfp_single_fmsc,  0 },
	[FOP_TO_IDX(FOP_FNMSC)]	= { vfp_single_fnmsc, 0 },
	[FOP_TO_IDX(FOP_FMUL)]	= { vfp_single_fmul,  0 },
	[FOP_TO_IDX(FOP_FNMUL)]	= { vfp_single_fnmul, 0 },
	[FOP_TO_IDX(FOP_FADD)]	= { vfp_single_fadd,  0 },
	[FOP_TO_IDX(FOP_FSUB)]	= { vfp_single_fsub,  0 },
	[FOP_TO_IDX(FOP_FDIV)]	= { vfp_single_fdiv,  0 },
};

#define FREG_BANK(x)	((x) & 0x18)
#define FREG_IDX(x)	((x) & 7)

u32 vfp_single_cpdo(u32 inst, u32 fpscr)
{
	u32 op = inst & FOP_MASK;
	u32 exceptions = 0;
	unsigned int dest;
	unsigned int sn = vfp_get_sn(inst);
	unsigned int sm = vfp_get_sm(inst);
	unsigned int vecitr, veclen, vecstride;
	struct op *fop;

	vecstride = 1 + ((fpscr & FPSCR_STRIDE_MASK) == FPSCR_STRIDE_MASK);

	fop = (op == FOP_EXT) ? &fops_ext[FEXT_TO_IDX(inst)] : &fops[FOP_TO_IDX(op)];

	/*
	 * fcvtsd takes a dN register number as destination, not sN.
	 * Technically, if bit 0 of dd is set, this is an invalid
	 * instruction.  However, we ignore this for efficiency.
	 * It also only operates on scalars.
	 */
	if (fop->flags & OP_DD)
		dest = vfp_get_dd(inst);
	else
		dest = vfp_get_sd(inst);

	/*
	 * If destination bank is zero, vector length is always '1'.
	 * ARM DDI0100F C5.1.3, C5.3.2.
	 */
	if ((fop->flags & OP_SCALAR) || FREG_BANK(dest) == 0)
		veclen = 0;
	else
		veclen = fpscr & FPSCR_LENGTH_MASK;

	pr_debug("VFP: vecstride=%u veclen=%u\n", vecstride,
		 (veclen >> FPSCR_LENGTH_BIT) + 1);

	if (!fop->fn)
		goto invalid;

	for (vecitr = 0; vecitr <= veclen; vecitr += 1 << FPSCR_LENGTH_BIT) {
		s32 m = vfp_get_float(sm);
		u32 except;
		char type;

		type = fop->flags & OP_DD ? 'd' : 's';
		if (op == FOP_EXT)
			pr_debug("VFP: itr%d (%c%u) = op[%u] (s%u=%08x)\n",
				 vecitr >> FPSCR_LENGTH_BIT, type, dest, sn,
				 sm, m);
		else
			pr_debug("VFP: itr%d (%c%u) = (s%u) op[%u] (s%u=%08x)\n",
				 vecitr >> FPSCR_LENGTH_BIT, type, dest, sn,
				 FOP_TO_IDX(op), sm, m);

		except = fop->fn(dest, sn, m, fpscr);
		pr_debug("VFP: itr%d: exceptions=%08x\n",
			 vecitr >> FPSCR_LENGTH_BIT, except);

		exceptions |= except;

		/*
		 * CHECK: It appears to be undefined whether we stop when
		 * we encounter an exception.  We continue.
		 */
		dest = FREG_BANK(dest) + ((FREG_IDX(dest) + vecstride) & 7);
		sn = FREG_BANK(sn) + ((FREG_IDX(sn) + vecstride) & 7);
		if (FREG_BANK(sm) != 0)
			sm = FREG_BANK(sm) + ((FREG_IDX(sm) + vecstride) & 7);
	}
	return exceptions;

 invalid:
	return (u32)-1;
}

repo_name: angery/linux


code:
# SPDX-License-Identifier: GPL-2.0-only
obj-y		:= enlighten.o hypercall.o grant-table.o p2m.o mm.o

path: arch/arm/xen/Makefile


code:
// SPDX-License-Identifier: GPL-2.0-only
#include <xen/xen.h>
#include <xen/events.h>
#include <xen/grant_table.h>
#include <xen/hvm.h>
#include <xen/interface/vcpu.h>
#include <xen/interface/xen.h>
#include <xen/interface/memory.h>
#include <xen/interface/hvm/params.h>
#include <xen/features.h>
#include <xen/platform_pci.h>
#include <xen/xenbus.h>
#include <xen/page.h>
#include <xen/interface/sched.h>
#include <xen/xen-ops.h>
#include <asm/xen/hypervisor.h>
#include <asm/xen/hypercall.h>
#include <asm/system_misc.h>
#include <asm/efi.h>
#include <linux/interrupt.h>
#include <linux/irqreturn.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/of_fdt.h>
#include <linux/of_irq.h>
#include <linux/of_address.h>
#include <linux/cpuidle.h>
#include <linux/cpufreq.h>
#include <linux/cpu.h>
#include <linux/console.h>
#include <linux/pvclock_gtod.h>
#include <linux/reboot.h>
#include <linux/time64.h>
#include <linux/timekeeping.h>
#include <linux/timekeeper_internal.h>
#include <linux/acpi.h>
#include <linux/virtio_anchor.h>

#include <linux/mm.h>

static struct start_info _xen_start_info;
struct start_info *xen_start_info = &_xen_start_info;
EXPORT_SYMBOL(xen_start_info);

enum xen_domain_type xen_domain_type = XEN_NATIVE;
EXPORT_SYMBOL(xen_domain_type);

struct shared_info xen_dummy_shared_info;
struct shared_info *HYPERVISOR_shared_info = (void *)&xen_dummy_shared_info;

DEFINE_PER_CPU(struct vcpu_info *, xen_vcpu);
static struct vcpu_info __percpu *xen_vcpu_info;

/* Linux <-> Xen vCPU id mapping */
DEFINE_PER_CPU(uint32_t, xen_vcpu_id);
EXPORT_PER_CPU_SYMBOL(xen_vcpu_id);

/* These are unused until we support booting "pre-ballooned" */
unsigned long xen_released_pages;
struct xen_memory_region xen_extra_mem[XEN_EXTRA_MEM_MAX_REGIONS] __initdata;

static __read_mostly unsigned int xen_events_irq;
static __read_mostly phys_addr_t xen_grant_frames;

#define GRANT_TABLE_INDEX   0
#define EXT_REGION_INDEX    1

uint32_t xen_start_flags;
EXPORT_SYMBOL(xen_start_flags);

int xen_unmap_domain_gfn_range(struct vm_area_struct *vma,
			       int nr, struct page **pages)
{
	return xen_xlate_unmap_gfn_range(vma, nr, pages);
}
EXPORT_SYMBOL_GPL(xen_unmap_domain_gfn_range);

static void xen_read_wallclock(struct timespec64 *ts)
{
	u32 version;
	struct timespec64 now, ts_monotonic;
	struct shared_info *s = HYPERVISOR_shared_info;
	struct pvclock_wall_clock *wall_clock = &(s->wc);

	/* get wallclock at system boot */
	do {
		version = wall_clock->version;
		rmb();		/* fetch version before time */
		now.tv_sec  = ((uint64_t)wall_clock->sec_hi << 32) | wall_clock->sec;
		now.tv_nsec = wall_clock->nsec;
		rmb();		/* fetch time before checking version */
	} while ((wall_clock->version & 1) || (version != wall_clock->version));

	/* time since system boot */
	ktime_get_ts64(&ts_monotonic);
	*ts = timespec64_add(now, ts_monotonic);
}

static int xen_pvclock_gtod_notify(struct notifier_block *nb,
				   unsigned long was_set, void *priv)
{
	/* Protected by the calling core code serialization */
	static struct timespec64 next_sync;

	struct xen_platform_op op;
	struct timespec64 now, system_time;
	struct timekeeper *tk = priv;

	now.tv_sec = tk->xtime_sec;
	now.tv_nsec = (long)(tk->tkr_mono.xtime_nsec >> tk->tkr_mono.shift);
	system_time = timespec64_add(now, tk->wall_to_monotonic);

	/*
	 * We only take the expensive HV call when the clock was set
	 * or when the 11 minutes RTC synchronization time elapsed.
	 */
	if (!was_set && timespec64_compare(&now, &next_sync) < 0)
		return NOTIFY_OK;

	op.cmd = XENPF_settime64;
	op.u.settime64.mbz = 0;
	op.u.settime64.secs = now.tv_sec;
	op.u.settime64.nsecs = now.tv_nsec;
	op.u.settime64.system_time = timespec64_to_ns(&system_time);
	(void)HYPERVISOR_platform_op(&op);

	/*
	 * Move the next drift compensation time 11 minutes
	 * ahead. That's emulating the sync_cmos_clock() update for
	 * the hardware RTC.
	 */
	next_sync = now;
	next_sync.tv_sec += 11 * 60;

	return NOTIFY_OK;
}

static struct notifier_block xen_pvclock_gtod_notifier = {
	.notifier_call = xen_pvclock_gtod_notify,
};

static int xen_starting_cpu(unsigned int cpu)
{
	struct vcpu_register_vcpu_info info;
	struct vcpu_info *vcpup;
	int err;

	/*
	 * VCPUOP_register_vcpu_info cannot be called twice for the same
	 * vcpu, so if vcpu_info is already registered, just get out. This
	 * can happen with cpu-hotplug.
	 */
	if (per_cpu(xen_vcpu, cpu) != NULL)
		goto after_register_vcpu_info;

	pr_info("Xen: initializing cpu%d\n", cpu);
	vcpup = per_cpu_ptr(xen_vcpu_info, cpu);

	info.mfn = percpu_to_gfn(vcpup);
	info.offset = xen_offset_in_page(vcpup);

	err = HYPERVISOR_vcpu_op(VCPUOP_register_vcpu_info, xen_vcpu_nr(cpu),
				 &info);
	BUG_ON(err);
	per_cpu(xen_vcpu, cpu) = vcpup;

	if (!xen_kernel_unmapped_at_usr())
		xen_setup_runstate_info(cpu);

after_register_vcpu_info:
	enable_percpu_irq(xen_events_irq, 0);
	return 0;
}

static int xen_dying_cpu(unsigned int cpu)
{
	disable_percpu_irq(xen_events_irq);
	return 0;
}

void xen_reboot(int reason)
{
	struct sched_shutdown r = { .reason = reason };
	int rc;

	rc = HYPERVISOR_sched_op(SCHEDOP_shutdown, &r);
	BUG_ON(rc);
}

static int xen_restart(struct notifier_block *nb, unsigned long action,
		       void *data)
{
	xen_reboot(SHUTDOWN_reboot);

	return NOTIFY_DONE;
}

static struct notifier_block xen_restart_nb = {
	.notifier_call = xen_restart,
	.priority = 192,
};

static void xen_power_off(void)
{
	xen_reboot(SHUTDOWN_poweroff);
}

static irqreturn_t xen_arm_callback(int irq, void *arg)
{
	xen_hvm_evtchn_do_upcall();
	return IRQ_HANDLED;
}

static __initdata struct {
	const char *compat;
	const char *prefix;
	const char *version;
	bool found;
} hyper_node = {"xen,xen", "xen,xen-", NULL, false};

static int __init fdt_find_hyper_node(unsigned long node, const char *uname,
				      int depth, void *data)
{
	const void *s = NULL;
	int len;

	if (depth != 1 || strcmp(uname, "hypervisor") != 0)
		return 0;

	if (of_flat_dt_is_compatible(node, hyper_node.compat))
		hyper_node.found = true;

	s = of_get_flat_dt_prop(node, "compatible", &len);
	if (strlen(hyper_node.prefix) + 3  < len &&
	    !strncmp(hyper_node.prefix, s, strlen(hyper_node.prefix)))
		hyper_node.version = s + strlen(hyper_node.prefix);

	/*
	 * Check if Xen supports EFI by checking whether there is the
	 * "/hypervisor/uefi" node in DT. If so, runtime services are available
	 * through proxy functions (e.g. in case of Xen dom0 EFI implementation
	 * they call special hypercall which executes relevant EFI functions)
	 * and that is why they are always enabled.
	 */
	if (IS_ENABLED(CONFIG_XEN_EFI)) {
		if ((of_get_flat_dt_subnode_by_name(node, "uefi") > 0) &&
		    !efi_runtime_disabled())
			set_bit(EFI_RUNTIME_SERVICES, &efi.flags);
	}

	return 0;
}

/*
 * see Documentation/devicetree/bindings/arm/xen.txt for the
 * documentation of the Xen Device Tree format.
 */
void __init xen_early_init(void)
{
	of_scan_flat_dt(fdt_find_hyper_node, NULL);
	if (!hyper_node.found) {
		pr_debug("No Xen support\n");
		return;
	}

	if (hyper_node.version == NULL) {
		pr_debug("Xen version not found\n");
		return;
	}

	pr_info("Xen %s support found\n", hyper_node.version);

	xen_domain_type = XEN_HVM_DOMAIN;

	xen_setup_features();

	if (xen_feature(XENFEAT_dom0))
		xen_start_flags |= SIF_INITDOMAIN|SIF_PRIVILEGED;

	if (!console_set_on_cmdline && !xen_initial_domain())
		add_preferred_console("hvc", 0, NULL);
}

static void __init xen_acpi_guest_init(void)
{
#ifdef CONFIG_ACPI
	struct xen_hvm_param a;
	int interrupt, trigger, polarity;

	a.domid = DOMID_SELF;
	a.index = HVM_PARAM_CALLBACK_IRQ;

	if (HYPERVISOR_hvm_op(HVMOP_get_param, &a)
	    || (a.value >> 56) != HVM_PARAM_CALLBACK_TYPE_PPI) {
		xen_events_irq = 0;
		return;
	}

	interrupt = a.value & 0xff;
	trigger = ((a.value >> 8) & 0x1) ? ACPI_EDGE_SENSITIVE
					 : ACPI_LEVEL_SENSITIVE;
	polarity = ((a.value >> 8) & 0x2) ? ACPI_ACTIVE_LOW
					  : ACPI_ACTIVE_HIGH;
	xen_events_irq = acpi_register_gsi(NULL, interrupt, trigger, polarity);
#endif
}

#ifdef CONFIG_XEN_UNPOPULATED_ALLOC
/*
 * A type-less specific Xen resource which contains extended regions
 * (unused regions of guest physical address space provided by the hypervisor).
 */
static struct resource xen_resource = {
	.name = "Xen unused space",
};

int __init arch_xen_unpopulated_init(struct resource **res)
{
	struct device_node *np;
	struct resource *regs, *tmp_res;
	uint64_t min_gpaddr = -1, max_gpaddr = 0;
	unsigned int i, nr_reg = 0;
	int rc;

	if (!xen_domain())
		return -ENODEV;

	if (!acpi_disabled)
		return -ENODEV;

	np = of_find_compatible_node(NULL, NULL, "xen,xen");
	if (WARN_ON(!np))
		return -ENODEV;

	/* Skip region 0 which is reserved for grant table space */
	while (of_get_address(np, nr_reg + EXT_REGION_INDEX, NULL, NULL))
		nr_reg++;

	if (!nr_reg) {
		pr_err("No extended regions are found\n");
		of_node_put(np);
		return -EINVAL;
	}

	regs = kcalloc(nr_reg, sizeof(*regs), GFP_KERNEL);
	if (!regs) {
		of_node_put(np);
		return -ENOMEM;
	}

	/*
	 * Create resource from extended regions provided by the hypervisor to be
	 * used as unused address space for Xen scratch pages.
	 */
	for (i = 0; i < nr_reg; i++) {
		rc = of_address_to_resource(np, i + EXT_REGION_INDEX, &regs[i]);
		if (rc)
			goto err;

		if (max_gpaddr < regs[i].end)
			max_gpaddr = regs[i].end;
		if (min_gpaddr > regs[i].start)
			min_gpaddr = regs[i].start;
	}

	xen_resource.start = min_gpaddr;
	xen_resource.end = max_gpaddr;

	/*
	 * Mark holes between extended regions as unavailable. The rest of that
	 * address space will be available for the allocation.
	 */
	for (i = 1; i < nr_reg; i++) {
		resource_size_t start, end;

		/* There is an overlap between regions */
		if (regs[i - 1].end + 1 > regs[i].start) {
			rc = -EINVAL;
			goto err;
		}

		/* There is no hole between regions */
		if (regs[i - 1].end + 1 == regs[i].start)
			continue;

		start = regs[i - 1].end + 1;
		end = regs[i].start - 1;

		tmp_res = kzalloc(sizeof(*tmp_res), GFP_KERNEL);
		if (!tmp_res) {
			rc = -ENOMEM;
			goto err;
		}

		tmp_res->name = "Unavailable space";
		tmp_res->start = start;
		tmp_res->end = end;

		rc = insert_resource(&xen_resource, tmp_res);
		if (rc) {
			pr_err("Cannot insert resource %pR (%d)\n", tmp_res, rc);
			kfree(tmp_res);
			goto err;
		}
	}

	*res = &xen_resource;

err:
	of_node_put(np);
	kfree(regs);
	return rc;
}
#endif

static void __init xen_dt_guest_init(void)
{
	struct device_node *xen_node;
	struct resource res;

	xen_node = of_find_compatible_node(NULL, NULL, "xen,xen");
	if (!xen_node) {
		pr_err("Xen support was detected before, but it has disappeared\n");
		return;
	}

	xen_events_irq = irq_of_parse_and_map(xen_node, 0);

	if (of_address_to_resource(xen_node, GRANT_TABLE_INDEX, &res)) {
		pr_err("Xen grant table region is not found\n");
		of_node_put(xen_node);
		return;
	}
	of_node_put(xen_node);
	xen_grant_frames = res.start;
}

static int __init xen_guest_init(void)
{
	struct xen_add_to_physmap xatp;
	struct shared_info *shared_info_page = NULL;
	int rc, cpu;

	if (!xen_domain())
		return 0;

	if (IS_ENABLED(CONFIG_XEN_VIRTIO))
		virtio_set_mem_acc_cb(xen_virtio_restricted_mem_acc);

	if (!acpi_disabled)
		xen_acpi_guest_init();
	else
		xen_dt_guest_init();

	if (!xen_events_irq) {
		pr_err("Xen event channel interrupt not found\n");
		return -ENODEV;
	}

	/*
	 * The fdt parsing codes have set EFI_RUNTIME_SERVICES if Xen EFI
	 * parameters are found. Force enable runtime services.
	 */
	if (efi_enabled(EFI_RUNTIME_SERVICES))
		xen_efi_runtime_setup();

	shared_info_page = (struct shared_info *)get_zeroed_page(GFP_KERNEL);

	if (!shared_info_page) {
		pr_err("not enough memory\n");
		return -ENOMEM;
	}
	xatp.domid = DOMID_SELF;
	xatp.idx = 0;
	xatp.space = XENMAPSPACE_shared_info;
	xatp.gpfn = virt_to_gfn(shared_info_page);
	if (HYPERVISOR_memory_op(XENMEM_add_to_physmap, &xatp))
		BUG();

	HYPERVISOR_shared_info = (struct shared_info *)shared_info_page;

	/* xen_vcpu is a pointer to the vcpu_info struct in the shared_info
	 * page, we use it in the event channel upcall and in some pvclock
	 * related functions.
	 * The shared info contains exactly 1 CPU (the boot CPU). The guest
	 * is required to use VCPUOP_register_vcpu_info to place vcpu info
	 * for secondary CPUs as they are brought up.
	 * For uniformity we use VCPUOP_register_vcpu_info even on cpu0.
	 */
	xen_vcpu_info = alloc_percpu(struct vcpu_info);
	if (xen_vcpu_info == NULL)
		return -ENOMEM;

	/* Direct vCPU id mapping for ARM guests. */
	for_each_possible_cpu(cpu)
		per_cpu(xen_vcpu_id, cpu) = cpu;

	if (!xen_grant_frames) {
		xen_auto_xlat_grant_frames.count = gnttab_max_grant_frames();
		rc = xen_xlate_map_ballooned_pages(&xen_auto_xlat_grant_frames.pfn,
										   &xen_auto_xlat_grant_frames.vaddr,
										   xen_auto_xlat_grant_frames.count);
	} else
		rc = gnttab_setup_auto_xlat_frames(xen_grant_frames);
	if (rc) {
		free_percpu(xen_vcpu_info);
		return rc;
	}
	gnttab_init();

	/*
	 * Making sure board specific code will not set up ops for
	 * cpu idle and cpu freq.
	 */
	disable_cpuidle();
	disable_cpufreq();

	xen_init_IRQ();

	if (request_percpu_irq(xen_events_irq, xen_arm_callback,
			       "events", &xen_vcpu)) {
		pr_err("Error request IRQ %d\n", xen_events_irq);
		return -EINVAL;
	}

	if (!xen_kernel_unmapped_at_usr())
		xen_time_setup_guest();

	if (xen_initial_domain())
		pvclock_gtod_register_notifier(&xen_pvclock_gtod_notifier);

	return cpuhp_setup_state(CPUHP_AP_ARM_XEN_STARTING,
				 "arm/xen:starting", xen_starting_cpu,
				 xen_dying_cpu);
}
early_initcall(xen_guest_init);

static int __init xen_pm_init(void)
{
	if (!xen_domain())
		return -ENODEV;

	pm_power_off = xen_power_off;
	register_restart_handler(&xen_restart_nb);
	if (!xen_initial_domain()) {
		struct timespec64 ts;
		xen_read_wallclock(&ts);
		do_settimeofday64(&ts);
	}

	return 0;
}
late_initcall(xen_pm_init);

/* empty stubs */
void xen_arch_pre_suspend(void) { }
void xen_arch_post_suspend(int suspend_cancelled) { }
void xen_timer_resume(void) { }
void xen_arch_resume(void) { }
void xen_arch_suspend(void) { }

/* In the hypercall.S file. */
EXPORT_SYMBOL_GPL(HYPERVISOR_event_channel_op);
EXPORT_SYMBOL_GPL(HYPERVISOR_grant_table_op);
EXPORT_SYMBOL_GPL(HYPERVISOR_xen_version);
EXPORT_SYMBOL_GPL(HYPERVISOR_console_io);
EXPORT_SYMBOL_GPL(HYPERVISOR_sched_op);
EXPORT_SYMBOL_GPL(HYPERVISOR_hvm_op);
EXPORT_SYMBOL_GPL(HYPERVISOR_memory_op);
EXPORT_SYMBOL_GPL(HYPERVISOR_physdev_op);
EXPORT_SYMBOL_GPL(HYPERVISOR_vcpu_op);
EXPORT_SYMBOL_GPL(HYPERVISOR_platform_op_raw);
EXPORT_SYMBOL_GPL(HYPERVISOR_multicall);
EXPORT_SYMBOL_GPL(HYPERVISOR_vm_assist);
EXPORT_SYMBOL_GPL(HYPERVISOR_dm_op);
EXPORT_SYMBOL_GPL(privcmd_call);


code:
/******************************************************************************
 * grant_table.c
 * ARM specific part
 *
 * Granting foreign access to our memory reservation.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License version 2
 * as published by the Free Software Foundation; or, when distributed
 * separately from the Linux kernel or incorporated into other
 * software packages, subject to the following license:
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this source file (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use, copy, modify,
 * merge, publish, distribute, sublicense, and/or sell copies of the Software,
 * and to permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */

#include <xen/interface/xen.h>
#include <xen/page.h>
#include <xen/grant_table.h>

int arch_gnttab_map_shared(xen_pfn_t *frames, unsigned long nr_gframes,
			   unsigned long max_nr_gframes,
			   void **__shared)
{
	return -ENOSYS;
}

void arch_gnttab_unmap(void *shared, unsigned long nr_gframes)
{
	return;
}

int arch_gnttab_map_status(uint64_t *frames, unsigned long nr_gframes,
			   unsigned long max_nr_gframes,
			   grant_status_t **__shared)
{
	return -ENOSYS;
}

int arch_gnttab_init(unsigned long nr_shared, unsigned long nr_status)
{
	return 0;
}


code:
/******************************************************************************
 * hypercall.S
 *
 * Xen hypercall wrappers
 *
 * Stefano Stabellini <stefano.stabellini@eu.citrix.com>, Citrix, 2012
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License version 2
 * as published by the Free Software Foundation; or, when distributed
 * separately from the Linux kernel or incorporated into other
 * software packages, subject to the following license:
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this source file (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use, copy, modify,
 * merge, publish, distribute, sublicense, and/or sell copies of the Software,
 * and to permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */

/*
 * The Xen hypercall calling convention is very similar to the ARM
 * procedure calling convention: the first paramter is passed in r0, the
 * second in r1, the third in r2 and the fourth in r3. Considering that
 * Xen hypercalls have 5 arguments at most, the fifth paramter is passed
 * in r4, differently from the procedure calling convention of using the
 * stack for that case.
 *
 * The hypercall number is passed in r12.
 *
 * The return value is in r0.
 *
 * The hvc ISS is required to be 0xEA1, that is the Xen specific ARM
 * hypercall tag.
 */

#include <linux/linkage.h>
#include <asm/assembler.h>
#include <asm/opcodes-virt.h>
#include <xen/interface/xen.h>

#define XEN_IMM 0xEA1

#define HYPERCALL_SIMPLE(hypercall)		\
ENTRY(HYPERVISOR_##hypercall)			\
	mov r12, #__HYPERVISOR_##hypercall;	\
	__HVC(XEN_IMM);						\
	ret lr;					\
ENDPROC(HYPERVISOR_##hypercall)

#define HYPERCALL0 HYPERCALL_SIMPLE
#define HYPERCALL1 HYPERCALL_SIMPLE
#define HYPERCALL2 HYPERCALL_SIMPLE
#define HYPERCALL3 HYPERCALL_SIMPLE
#define HYPERCALL4 HYPERCALL_SIMPLE

#define HYPERCALL5(hypercall)			\
ENTRY(HYPERVISOR_##hypercall)			\
	stmdb sp!, {r4}						\
	ldr r4, [sp, #4]					\
	mov r12, #__HYPERVISOR_##hypercall;	\
	__HVC(XEN_IMM);						\
	ldm sp!, {r4}						\
	ret lr					\
ENDPROC(HYPERVISOR_##hypercall)

                .text

HYPERCALL2(xen_version);
HYPERCALL3(console_io);
HYPERCALL3(grant_table_op);
HYPERCALL2(sched_op);
HYPERCALL2(event_channel_op);
HYPERCALL2(hvm_op);
HYPERCALL2(memory_op);
HYPERCALL2(physdev_op);
HYPERCALL3(vcpu_op);
HYPERCALL1(platform_op_raw);
HYPERCALL2(multicall);
HYPERCALL2(vm_assist);
HYPERCALL3(dm_op);

ENTRY(privcmd_call)
	stmdb sp!, {r4}
	mov r12, r0
	mov r0, r1
	mov r1, r2
	mov r2, r3
	ldr r3, [sp, #8]
	/*
	 * Privcmd calls are issued by the userspace. We need to allow the
	 * kernel to access the userspace memory before issuing the hypercall.
	 */
	uaccess_enable r4

	/* r4 is loaded now as we use it as scratch register before */
	ldr r4, [sp, #4]
	__HVC(XEN_IMM)

	/*
	 * Disable userspace access from kernel. This is fine to do it
	 * unconditionally as no set_fs(KERNEL_DS) is called before.
	 */
	uaccess_disable r4

	ldm sp!, {r4}
	ret lr
ENDPROC(privcmd_call);


code:
// SPDX-License-Identifier: GPL-2.0-only
#include <linux/cpu.h>
#include <linux/dma-direct.h>
#include <linux/dma-map-ops.h>
#include <linux/gfp.h>
#include <linux/highmem.h>
#include <linux/export.h>
#include <linux/memblock.h>
#include <linux/of_address.h>
#include <linux/slab.h>
#include <linux/types.h>
#include <linux/vmalloc.h>
#include <linux/swiotlb.h>

#include <xen/xen.h>
#include <xen/interface/grant_table.h>
#include <xen/interface/memory.h>
#include <xen/page.h>
#include <xen/xen-ops.h>
#include <xen/swiotlb-xen.h>

#include <asm/cacheflush.h>
#include <asm/xen/hypercall.h>
#include <asm/xen/interface.h>

static gfp_t xen_swiotlb_gfp(void)
{
	phys_addr_t base;
	u64 i;

	for_each_mem_range(i, &base, NULL) {
		if (base < (phys_addr_t)0xffffffff) {
			if (IS_ENABLED(CONFIG_ZONE_DMA32))
				return __GFP_DMA32;
			return __GFP_DMA;
		}
	}

	return GFP_KERNEL;
}

static bool hypercall_cflush = false;

/* buffers in highmem or foreign pages cannot cross page boundaries */
static void dma_cache_maint(struct device *dev, dma_addr_t handle,
			    size_t size, u32 op)
{
	struct gnttab_cache_flush cflush;

	cflush.offset = xen_offset_in_page(handle);
	cflush.op = op;
	handle &= XEN_PAGE_MASK;

	do {
		cflush.a.dev_bus_addr = dma_to_phys(dev, handle);

		if (size + cflush.offset > XEN_PAGE_SIZE)
			cflush.length = XEN_PAGE_SIZE - cflush.offset;
		else
			cflush.length = size;

		HYPERVISOR_grant_table_op(GNTTABOP_cache_flush, &cflush, 1);

		cflush.offset = 0;
		handle += cflush.length;
		size -= cflush.length;
	} while (size);
}

/*
 * Dom0 is mapped 1:1, and while the Linux page can span across multiple Xen
 * pages, it is not possible for it to contain a mix of local and foreign Xen
 * pages.  Calling pfn_valid on a foreign mfn will always return false, so if
 * pfn_valid returns true the pages is local and we can use the native
 * dma-direct functions, otherwise we call the Xen specific version.
 */
void xen_dma_sync_for_cpu(struct device *dev, dma_addr_t handle,
			  size_t size, enum dma_data_direction dir)
{
	if (dir != DMA_TO_DEVICE)
		dma_cache_maint(dev, handle, size, GNTTAB_CACHE_INVAL);
}

void xen_dma_sync_for_device(struct device *dev, dma_addr_t handle,
			     size_t size, enum dma_data_direction dir)
{
	if (dir == DMA_FROM_DEVICE)
		dma_cache_maint(dev, handle, size, GNTTAB_CACHE_INVAL);
	else
		dma_cache_maint(dev, handle, size, GNTTAB_CACHE_CLEAN);
}

bool xen_arch_need_swiotlb(struct device *dev,
			   phys_addr_t phys,
			   dma_addr_t dev_addr)
{
	unsigned int xen_pfn = XEN_PFN_DOWN(phys);
	unsigned int bfn = XEN_PFN_DOWN(dma_to_phys(dev, dev_addr));

	/*
	 * The swiotlb buffer should be used if
	 *	- Xen doesn't have the cache flush hypercall
	 *	- The Linux page refers to foreign memory
	 *	- The device doesn't support coherent DMA request
	 *
	 * The Linux page may be spanned acrros multiple Xen page, although
	 * it's not possible to have a mix of local and foreign Xen page.
	 * Furthermore, range_straddles_page_boundary is already checking
	 * if buffer is physically contiguous in the host RAM.
	 *
	 * Therefore we only need to check the first Xen page to know if we
	 * require a bounce buffer because the device doesn't support coherent
	 * memory and we are not able to flush the cache.
	 */
	return (!hypercall_cflush && (xen_pfn != bfn) &&
		!dev_is_dma_coherent(dev));
}

static int __init xen_mm_init(void)
{
	struct gnttab_cache_flush cflush;
	int rc;

	if (!xen_swiotlb_detect())
		return 0;

	/* we can work with the default swiotlb */
	if (!io_tlb_default_mem.nslabs) {
		rc = swiotlb_init_late(swiotlb_size_or_default(),
				       xen_swiotlb_gfp(), NULL);
		if (rc < 0)
			return rc;
	}

	cflush.op = 0;
	cflush.a.dev_bus_addr = 0;
	cflush.offset = 0;
	cflush.length = 0;
	if (HYPERVISOR_grant_table_op(GNTTABOP_cache_flush, &cflush, 1) != -ENOSYS)
		hypercall_cflush = true;
	return 0;
}
arch_initcall(xen_mm_init);


code:
// SPDX-License-Identifier: GPL-2.0-only
#include <linux/memblock.h>
#include <linux/gfp.h>
#include <linux/export.h>
#include <linux/spinlock.h>
#include <linux/slab.h>
#include <linux/types.h>
#include <linux/dma-mapping.h>
#include <linux/vmalloc.h>
#include <linux/swiotlb.h>

#include <xen/xen.h>
#include <xen/interface/memory.h>
#include <xen/grant_table.h>
#include <xen/page.h>
#include <xen/swiotlb-xen.h>

#include <asm/cacheflush.h>
#include <asm/xen/hypercall.h>
#include <asm/xen/interface.h>

struct xen_p2m_entry {
	unsigned long pfn;
	unsigned long mfn;
	unsigned long nr_pages;
	struct rb_node rbnode_phys;
};

static rwlock_t p2m_lock;
struct rb_root phys_to_mach = RB_ROOT;
EXPORT_SYMBOL_GPL(phys_to_mach);

static int xen_add_phys_to_mach_entry(struct xen_p2m_entry *new)
{
	struct rb_node **link = &phys_to_mach.rb_node;
	struct rb_node *parent = NULL;
	struct xen_p2m_entry *entry;
	int rc = 0;

	while (*link) {
		parent = *link;
		entry = rb_entry(parent, struct xen_p2m_entry, rbnode_phys);

		if (new->pfn == entry->pfn)
			goto err_out;

		if (new->pfn < entry->pfn)
			link = &(*link)->rb_left;
		else
			link = &(*link)->rb_right;
	}
	rb_link_node(&new->rbnode_phys, parent, link);
	rb_insert_color(&new->rbnode_phys, &phys_to_mach);
	goto out;

err_out:
	rc = -EINVAL;
	pr_warn("%s: cannot add pfn=%pa -> mfn=%pa: pfn=%pa -> mfn=%pa already exists\n",
			__func__, &new->pfn, &new->mfn, &entry->pfn, &entry->mfn);
out:
	return rc;
}

unsigned long __pfn_to_mfn(unsigned long pfn)
{
	struct rb_node *n;
	struct xen_p2m_entry *entry;
	unsigned long irqflags;

	read_lock_irqsave(&p2m_lock, irqflags);
	n = phys_to_mach.rb_node;
	while (n) {
		entry = rb_entry(n, struct xen_p2m_entry, rbnode_phys);
		if (entry->pfn <= pfn &&
				entry->pfn + entry->nr_pages > pfn) {
			unsigned long mfn = entry->mfn + (pfn - entry->pfn);
			read_unlock_irqrestore(&p2m_lock, irqflags);
			return mfn;
		}
		if (pfn < entry->pfn)
			n = n->rb_left;
		else
			n = n->rb_right;
	}
	read_unlock_irqrestore(&p2m_lock, irqflags);

	return INVALID_P2M_ENTRY;
}
EXPORT_SYMBOL_GPL(__pfn_to_mfn);

int set_foreign_p2m_mapping(struct gnttab_map_grant_ref *map_ops,
			    struct gnttab_map_grant_ref *kmap_ops,
			    struct page **pages, unsigned int count)
{
	int i;

	for (i = 0; i < count; i++) {
		struct gnttab_unmap_grant_ref unmap;
		int rc;

		if (map_ops[i].status)
			continue;
		if (likely(set_phys_to_machine(map_ops[i].host_addr >> XEN_PAGE_SHIFT,
				    map_ops[i].dev_bus_addr >> XEN_PAGE_SHIFT)))
			continue;

		/*
		 * Signal an error for this slot. This in turn requires
		 * immediate unmapping.
		 */
		map_ops[i].status = GNTST_general_error;
		unmap.host_addr = map_ops[i].host_addr,
		unmap.handle = map_ops[i].handle;
		map_ops[i].handle = INVALID_GRANT_HANDLE;
		if (map_ops[i].flags & GNTMAP_device_map)
			unmap.dev_bus_addr = map_ops[i].dev_bus_addr;
		else
			unmap.dev_bus_addr = 0;

		/*
		 * Pre-populate the status field, to be recognizable in
		 * the log message below.
		 */
		unmap.status = 1;

		rc = HYPERVISOR_grant_table_op(GNTTABOP_unmap_grant_ref,
					       &unmap, 1);
		if (rc || unmap.status != GNTST_okay)
			pr_err_once("gnttab unmap failed: rc=%d st=%d\n",
				    rc, unmap.status);
	}

	return 0;
}

int clear_foreign_p2m_mapping(struct gnttab_unmap_grant_ref *unmap_ops,
			      struct gnttab_unmap_grant_ref *kunmap_ops,
			      struct page **pages, unsigned int count)
{
	int i;

	for (i = 0; i < count; i++) {
		set_phys_to_machine(unmap_ops[i].host_addr >> XEN_PAGE_SHIFT,
				    INVALID_P2M_ENTRY);
	}

	return 0;
}

bool __set_phys_to_machine_multi(unsigned long pfn,
		unsigned long mfn, unsigned long nr_pages)
{
	int rc;
	unsigned long irqflags;
	struct xen_p2m_entry *p2m_entry;
	struct rb_node *n;

	if (mfn == INVALID_P2M_ENTRY) {
		write_lock_irqsave(&p2m_lock, irqflags);
		n = phys_to_mach.rb_node;
		while (n) {
			p2m_entry = rb_entry(n, struct xen_p2m_entry, rbnode_phys);
			if (p2m_entry->pfn <= pfn &&
					p2m_entry->pfn + p2m_entry->nr_pages > pfn) {
				rb_erase(&p2m_entry->rbnode_phys, &phys_to_mach);
				write_unlock_irqrestore(&p2m_lock, irqflags);
				kfree(p2m_entry);
				return true;
			}
			if (pfn < p2m_entry->pfn)
				n = n->rb_left;
			else
				n = n->rb_right;
		}
		write_unlock_irqrestore(&p2m_lock, irqflags);
		return true;
	}

	p2m_entry = kzalloc(sizeof(*p2m_entry), GFP_NOWAIT);
	if (!p2m_entry)
		return false;

	p2m_entry->pfn = pfn;
	p2m_entry->nr_pages = nr_pages;
	p2m_entry->mfn = mfn;

	write_lock_irqsave(&p2m_lock, irqflags);
	rc = xen_add_phys_to_mach_entry(p2m_entry);
	if (rc < 0) {
		write_unlock_irqrestore(&p2m_lock, irqflags);
		kfree(p2m_entry);
		return false;
	}
	write_unlock_irqrestore(&p2m_lock, irqflags);
	return true;
}
EXPORT_SYMBOL_GPL(__set_phys_to_machine_multi);

bool __set_phys_to_machine(unsigned long pfn, unsigned long mfn)
{
	return __set_phys_to_machine_multi(pfn, mfn, 1);
}
EXPORT_SYMBOL_GPL(__set_phys_to_machine);

static int p2m_init(void)
{
	rwlock_init(&p2m_lock);
	return 0;
}
arch_initcall(p2m_init);


code:
# SPDX-License-Identifier: GPL-2.0-only
obj-y			+= kernel/ mm/ net/
obj-$(CONFIG_KVM)	+= kvm/
obj-$(CONFIG_XEN)	+= xen/
obj-$(subst m,y,$(CONFIG_HYPERV))	+= hyperv/
obj-$(CONFIG_CRYPTO)	+= crypto/

# for cleaning
subdir- += boot