// SPDX-License-Identifier: BSD-2-Clause
/*
 * Copyright 2023 ProvenRun SAS
 */

#include <drivers/versal_pm.h>
#include <kernel/pseudo_ta.h>
#include <malloc.h>
#include <mm/core_memprot.h>
#include <platform_config.h>
#include <pta_versal_loader.h>
#include <string.h>
#include <tee/cache.h>

#define PTA_NAME "versal-loader.pta"

static TEE_Result pta_versal_loader_subsys(uint32_t param_types,
					   TEE_Param params[TEE_NUM_PARAMS])
{
	TEE_Result ret = TEE_SUCCESS;
	uint8_t *buf = NULL;
	size_t bufsize = 0;
	uint32_t exp_param_types = TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INPUT,
						   TEE_PARAM_TYPE_NONE,
						   TEE_PARAM_TYPE_NONE,
						   TEE_PARAM_TYPE_NONE);

	if (param_types != exp_param_types)
		return TEE_ERROR_BAD_PARAMETERS;

	if (!params[0].memref.size)
		return TEE_ERROR_BAD_PARAMETERS;

	if (ROUNDUP_OVERFLOW(params[0].memref.size, CACHELINE_LEN, &bufsize))
		return TEE_ERROR_BAD_PARAMETERS;

	buf = memalign(CACHELINE_LEN, bufsize);
	if (!buf)
		return TEE_ERROR_OUT_OF_MEMORY;

	memset(buf, 0, bufsize);
	memcpy(buf, params[0].memref.buffer, params[0].memref.size);
	cache_operation(TEE_CACHEFLUSH, buf, bufsize);

	ret = versal_write_fpga(virt_to_phys(buf));

	free(buf);

	return ret;
}

static TEE_Result invoke_command(void *sess_ctx __unused,
				 uint32_t cmd_id,
				 uint32_t param_types,
				 TEE_Param params[TEE_NUM_PARAMS])
{
	switch (cmd_id) {
	case PTA_VERSAL_LOADER_SUBSYS:
		return pta_versal_loader_subsys(param_types, params);
	default:
		return TEE_ERROR_NOT_SUPPORTED;
	}
}

pseudo_ta_register(.uuid = PTA_VERSAL_LOADER_UUID, .name = PTA_NAME,
		   .flags = PTA_DEFAULT_FLAGS,
		   .invoke_command_entry_point = invoke_command);