/*
 * Copyright (c) 2019,2020 Linaro Limited
 * Copyright (c) 2021 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr/kernel.h>
#include <cmsis_core.h>
#include <zephyr/arch/arm/cortex_m/fpu.h>

/**
 * @file @brief Helper functions for saving and restoring the FP context.
 *
 */

void z_arm_save_fp_context(struct fpu_ctx_full *buffer)
{
#if defined(CONFIG_FPU_SHARING)
	__ASSERT_NO_MSG(buffer != NULL);

	uint32_t CONTROL = __get_CONTROL();

	if (CONTROL & CONTROL_FPCA_Msk) {
		/* Store caller-saved and callee-saved FP registers. */
		__asm__ volatile(
			"vstmia %0, {s0-s15}\n"
			"vstmia %1, {s16-s31}\n"
			:: "r" (buffer->caller_saved), "r" (buffer->callee_saved) :
		);

		buffer->fpscr = __get_FPSCR();
		buffer->ctx_saved = true;

		/* Disable FPCA so no stacking of FP registers happens in TFM. */
		__set_CONTROL(CONTROL & ~CONTROL_FPCA_Msk);

		/* ISB is recommended after setting CONTROL. It's not needed
		 * here though, since FPCA should have no impact on instruction
		 * fetching.
		 */
	}
#endif
}

void z_arm_restore_fp_context(const struct fpu_ctx_full *buffer)
{
#if defined(CONFIG_FPU_SHARING)
	if (buffer->ctx_saved) {
		/* Set FPCA first so it is set even if an interrupt happens
		 * during restoration.
		 */
		__set_CONTROL(__get_CONTROL() | CONTROL_FPCA_Msk);

		/* Restore FP state. */
		__set_FPSCR(buffer->fpscr);

		__asm__ volatile(
			"vldmia %0, {s0-s15}\n"
			"vldmia %1, {s16-s31}\n"
			:: "r" (buffer->caller_saved), "r" (buffer->callee_saved) :
		);
	}
#endif
}