/*
 * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <arch.h>
#include <arch_helpers.h>
#include <pubsub.h>
#include <sve.h>

static void *disable_sve_hook(const void *arg)
{
	uint64_t features;

	features = read_id_aa64pfr0_el1() >> ID_AA64PFR0_SVE_SHIFT;
	if ((features & ID_AA64PFR0_SVE_MASK) == 1) {
		uint64_t cptr;

		/*
		 * Disable SVE, SIMD and FP access for the Secure world.
		 * As the SIMD/FP registers are part of the SVE Z-registers, any
		 * use of SIMD/FP functionality will corrupt the SVE registers.
		 * Therefore it is necessary to prevent use of SIMD/FP support
		 * in the Secure world as well as SVE functionality.
		 */
		cptr = read_cptr_el3();
		cptr = (cptr | TFP_BIT) & ~(CPTR_EZ_BIT);
		write_cptr_el3(cptr);

		/*
		 * No explicit ISB required here as ERET to switch to Secure
		 * world covers it
		 */
	}
	return 0;
}

static void *enable_sve_hook(const void *arg)
{
	uint64_t features;

	features = read_id_aa64pfr0_el1() >> ID_AA64PFR0_SVE_SHIFT;
	if ((features & ID_AA64PFR0_SVE_MASK) == 1) {
		uint64_t cptr;

		/*
		 * Enable SVE, SIMD and FP access for the Non-secure world.
		 */
		cptr = read_cptr_el3();
		cptr = (cptr | CPTR_EZ_BIT) & ~(TFP_BIT);
		write_cptr_el3(cptr);

		/*
		 * No explicit ISB required here as ERET to switch to Non-secure
		 * world covers it
		 */
	}
	return 0;
}

void sve_enable(int el2_unused)
{
	uint64_t features;

	features = read_id_aa64pfr0_el1() >> ID_AA64PFR0_SVE_SHIFT;
	if ((features & ID_AA64PFR0_SVE_MASK) == 1) {
		uint64_t cptr;
#if CTX_INCLUDE_FPREGS
		/*
		 * CTX_INCLUDE_FPREGS is not supported on SVE enabled systems.
		 */
		assert(0);
#endif
		/*
		 * Update CPTR_EL3 to enable access to SVE functionality for the
		 * Non-secure world.
		 * NOTE - assumed that CPTR_EL3.TFP is set to allow access to
		 * the SIMD, floating-point and SVE support.
		 *
		 * CPTR_EL3.EZ: Set to 1 to enable access to SVE  functionality
		 *  in the Non-secure world.
		 */
		cptr = read_cptr_el3();
		cptr |= CPTR_EZ_BIT;
		write_cptr_el3(cptr);

		/*
		 * Need explicit ISB here to guarantee that update to ZCR_ELx
		 * and CPTR_EL2.TZ do not result in trap to EL3.
		 */
		isb();

		/*
		 * Ensure lower ELs have access to full vector length.
		 */
		write_zcr_el3(ZCR_EL3_LEN_MASK);

		if (el2_unused) {
			/*
			 * Update CPTR_EL2 to enable access to SVE functionality
			 * for Non-secure world, EL2 and Non-secure EL1 and EL0.
			 * NOTE - assumed that CPTR_EL2.TFP is set to allow
			 * access to the SIMD, floating-point and SVE support.
			 *
			 * CPTR_EL2.TZ: Set to 0 to enable access to SVE support
			 *  for EL2 and Non-secure EL1 and EL0.
			 */
			cptr = read_cptr_el2();
			cptr &= ~(CPTR_EL2_TZ_BIT);
			write_cptr_el2(cptr);

			/*
			 * Ensure lower ELs have access to full vector length.
			 */
			write_zcr_el2(ZCR_EL2_LEN_MASK);
		}
		/*
		 * No explicit ISB required here as ERET to switch to
		 * Non-secure world covers it.
		 */
	}
}

SUBSCRIBE_TO_EVENT(cm_exited_normal_world, disable_sve_hook);
SUBSCRIBE_TO_EVENT(cm_entering_normal_world, enable_sve_hook);