/*
 * Copyright (c) 2018, Arm Limited. All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <assert.h>
#include <errno.h>
#include <string.h>

#include <common/debug.h>
#include <lib/el3_runtime/context_mgmt.h>
#include <lib/smccc.h>
#include <lib/spinlock.h>
#include <lib/utils.h>
#include <services/spci_svc.h>
#include <services/sprt_svc.h>
#include <smccc_helpers.h>
#include <sprt_host.h>

#include "spm_private.h"

/*******************************************************************************
 * Macros to print UUIDs.
 ******************************************************************************/
#define PRINT_UUID_FORMAT	"%08x-%08x-%08x-%08x"
#define PRINT_UUID_ARGS(x)	x[0], x[1], x[2], x[3]

/*******************************************************************************
 * Array of structs that contains information about all handles of Secure
 * Services that are currently open.
 ******************************************************************************/
typedef enum spci_handle_status {
	HANDLE_STATUS_CLOSED = 0,
	HANDLE_STATUS_OPEN,
} spci_handle_status_t;

typedef struct spci_handle {
	/* 16-bit value used as reference in all SPCI calls */
	uint16_t handle;

	/* Client ID of the client that requested the handle */
	uint16_t client_id;

	/* Current status of the handle */
	spci_handle_status_t status;

	/*
	 * Context of the Secure Partition that provides the Secure Service
	 * referenced by this handle.
	 */
	sp_context_t *sp_ctx;

	/*
	 * The same handle might be used for multiple requests, keep a reference
	 * counter of them.
	 */
	unsigned int num_active_requests;
} spci_handle_t;

static spci_handle_t spci_handles[PLAT_SPCI_HANDLES_MAX_NUM];
static spinlock_t spci_handles_lock;

/*
 * Given a handle and a client ID, return the element of the spci_handles
 * array that contains the information of the handle. It can only return open
 * handles. It returns NULL if it couldn't find the element in the array.
 */
static spci_handle_t *spci_handle_info_get(uint16_t handle, uint16_t client_id)
{
	size_t i;

	for (i = 0; i < ARRAY_SIZE(spci_handles); i++) {
		spci_handle_t *h = &(spci_handles[i]);

		/* Only check for open handles */
		if (h->status == HANDLE_STATUS_CLOSED) {
			continue;
		}

		/* Check if either the handle or the client ID are different */
		if ((h->handle != handle) || (h->client_id != client_id)) {
			continue;
		}

		return h;
	}

	return NULL;
}

/*
 * Returns a unique value for a handle. This function must be called while
 * spci_handles_lock is locked. It returns 0 on success, -1 on error.
 */
static int spci_create_handle_value(uint16_t *handle)
{
	/*
	 * Trivial implementation that relies on the fact that any handle will
	 * be closed before 2^16 more handles have been opened.
	 */
	static uint16_t handle_count;

	*handle = handle_count;

	handle_count++;

	return 0;
}

/*******************************************************************************
 * Returns a unique token for a Secure Service request.
 ******************************************************************************/
static uint32_t spci_create_token_value(void)
{
	/*
	 * Trivial implementation that relies on the fact that any response will
	 * be read before 2^32 more service requests have been done.
	 */
	static uint32_t token_count;

	return token_count++;
}

/*******************************************************************************
 * This function looks for a Secure Partition that has a Secure Service
 * identified by the given UUID. It returns a handle that the client can use to
 * access the service, and an SPCI_*** error code.
 ******************************************************************************/
static uint64_t spci_service_handle_open_poll(void *handle, u_register_t x1,
			u_register_t x2, u_register_t x3, u_register_t x4,
			u_register_t x5, u_register_t x6, u_register_t x7)
{
	unsigned int i;
	sp_context_t *sp_ptr;
	uint16_t service_handle;

	/* Bits 31:16 of w7 are reserved (MBZ). */
	assert((x7 & 0xFFFF0000U) == 0);

	uint16_t client_id = x7 & 0x0000FFFFU;
	uint32_t uuid[4] = { x1, x2, x3, x4 };

	/* Get pointer to the Secure Partition that handles this service */
	sp_ptr = spm_sp_get_by_uuid(&uuid);
	if (sp_ptr == NULL) {
		WARN("SPCI: Service requested by client 0x%04x not found\n",
		     client_id);
		WARN("SPCI:   UUID: " PRINT_UUID_FORMAT "\n",
		     PRINT_UUID_ARGS(uuid));

		SMC_RET2(handle, SPCI_NOT_PRESENT, 0);
	}

	/* Get lock of the array of handles */
	spin_lock(&spci_handles_lock);

	/*
	 * We need to record the client ID and Secure Partition that correspond
	 * to this handle. Look for the first free entry in the array.
	 */
	for (i = 0; i < PLAT_SPCI_HANDLES_MAX_NUM; i++) {
		if (spci_handles[i].status == HANDLE_STATUS_CLOSED) {
			break;
		}
	}

	if (i == PLAT_SPCI_HANDLES_MAX_NUM) {
		spin_unlock(&spci_handles_lock);

		WARN("SPCI: Can't open more handles. Client 0x%04x\n",
		     client_id);
		WARN("SPCI:   UUID: " PRINT_UUID_FORMAT "\n",
		     PRINT_UUID_ARGS(uuid));

		SMC_RET2(handle, SPCI_NO_MEMORY, 0);
	}

	/* Create new handle value */
	if (spci_create_handle_value(&service_handle) != 0) {
		spin_unlock(&spci_handles_lock);

		WARN("SPCI: Can't create a new handle value. Client 0x%04x\n",
		     client_id);
		WARN("SPCI:   UUID: " PRINT_UUID_FORMAT "\n",
		     PRINT_UUID_ARGS(uuid));

		SMC_RET2(handle, SPCI_NO_MEMORY, 0);
	}

	/* Save all information about this handle */
	spci_handles[i].status = HANDLE_STATUS_OPEN;
	spci_handles[i].client_id = client_id;
	spci_handles[i].handle = service_handle;
	spci_handles[i].num_active_requests = 0U;
	spci_handles[i].sp_ctx = sp_ptr;

	/* Release lock of the array of handles */
	spin_unlock(&spci_handles_lock);

	VERBOSE("SPCI: Service handle request by client 0x%04x: 0x%04x\n",
		client_id, service_handle);
	VERBOSE("SPCI:   UUID: " PRINT_UUID_FORMAT "\n", PRINT_UUID_ARGS(uuid));

	/* The handle is returned in the top 16 bits of x1 */
	SMC_RET2(handle, SPCI_SUCCESS, ((uint32_t)service_handle) << 16);
}

/*******************************************************************************
 * This function closes a handle that a specific client uses to access a Secure
 * Service. It returns a SPCI_*** error code.
 ******************************************************************************/
static uint64_t spci_service_handle_close(void *handle, u_register_t x1)
{
	spci_handle_t *handle_info;
	uint16_t client_id = x1 & 0x0000FFFFU;
	uint16_t service_handle = (x1 >> 16) & 0x0000FFFFU;

	spin_lock(&spci_handles_lock);

	handle_info = spci_handle_info_get(service_handle, client_id);

	if (handle_info == NULL) {
		spin_unlock(&spci_handles_lock);

		WARN("SPCI: Tried to close invalid handle 0x%04x by client 0x%04x\n",
		     service_handle, client_id);

		SMC_RET1(handle, SPCI_INVALID_PARAMETER);
	}

	if (handle_info->status != HANDLE_STATUS_OPEN) {
		spin_unlock(&spci_handles_lock);

		WARN("SPCI: Tried to close handle 0x%04x by client 0x%04x in status %d\n",
			service_handle, client_id, handle_info->status);

		SMC_RET1(handle, SPCI_INVALID_PARAMETER);
	}

	if (handle_info->num_active_requests != 0U) {
		spin_unlock(&spci_handles_lock);

		/* A handle can't be closed if there are requests left */
		WARN("SPCI: Tried to close handle 0x%04x by client 0x%04x with %d requests left\n",
			service_handle, client_id,
			handle_info->num_active_requests);

		SMC_RET1(handle, SPCI_BUSY);
	}

	memset(handle_info, 0, sizeof(spci_handle_t));

	handle_info->status = HANDLE_STATUS_CLOSED;

	spin_unlock(&spci_handles_lock);

	VERBOSE("SPCI: Closed handle 0x%04x by client 0x%04x.\n",
		service_handle, client_id);

	SMC_RET1(handle, SPCI_SUCCESS);
}

/*******************************************************************************
 * This function requests a Secure Service from a given handle and client ID.
 ******************************************************************************/
static uint64_t spci_service_request_blocking(void *handle,
			uint32_t smc_fid, u_register_t x1, u_register_t x2,
			u_register_t x3, u_register_t x4, u_register_t x5,
			u_register_t x6, u_register_t x7)
{
	spci_handle_t *handle_info;
	sp_context_t *sp_ctx;
	cpu_context_t *cpu_ctx;
	uint32_t rx0;
	u_register_t rx1, rx2, rx3;
	uint16_t request_handle, client_id;

	/* Get handle array lock */
	spin_lock(&spci_handles_lock);

	/* Get pointer to struct of this open handle and client ID. */
	request_handle = (x7 >> 16U) & 0x0000FFFFU;
	client_id = x7 & 0x0000FFFFU;

	handle_info = spci_handle_info_get(request_handle, client_id);
	if (handle_info == NULL) {
		spin_unlock(&spci_handles_lock);

		WARN("SPCI_SERVICE_TUN_REQUEST_BLOCKING: Not found.\n");
		WARN("  Handle 0x%04x. Client ID 0x%04x\n", request_handle,
		     client_id);

		SMC_RET1(handle, SPCI_BUSY);
	}

	/* Get pointer to the Secure Partition that handles the service */
	sp_ctx = handle_info->sp_ctx;
	assert(sp_ctx != NULL);
	cpu_ctx = &(sp_ctx->cpu_ctx);

	/* Blocking requests are only allowed if the queue is empty */
	if (handle_info->num_active_requests > 0) {
		spin_unlock(&spci_handles_lock);

		SMC_RET1(handle, SPCI_BUSY);
	}

	if (spm_sp_request_increase_if_zero(sp_ctx) == -1) {
		spin_unlock(&spci_handles_lock);

		SMC_RET1(handle, SPCI_BUSY);
	}

	/* Prevent this handle from being closed */
	handle_info->num_active_requests += 1;

	/* Release handle lock */
	spin_unlock(&spci_handles_lock);

	/* Save the Normal world context */
	cm_el1_sysregs_context_save(NON_SECURE);

	/* Wait until the Secure Partition is idle and set it to busy. */
	sp_state_wait_switch(sp_ctx, SP_STATE_IDLE, SP_STATE_BUSY);

	/* Pass arguments to the Secure Partition */
	struct sprt_queue_entry_message message = {
		.type = SPRT_MSG_TYPE_SERVICE_TUN_REQUEST,
		.client_id = client_id,
		.service_handle = request_handle,
		.session_id = x6,
		.token = 0, /* No token needed for blocking requests */
		.args = {smc_fid, x1, x2, x3, x4, x5}
	};

	spin_lock(&(sp_ctx->spm_sp_buffer_lock));
	int rc = sprt_push_message((void *)sp_ctx->spm_sp_buffer_base, &message,
				   SPRT_QUEUE_NUM_BLOCKING);
	spin_unlock(&(sp_ctx->spm_sp_buffer_lock));
	if (rc != 0) {
		/*
		 * This shouldn't happen, blocking requests can only be made if
		 * the request queue is empty.
		 */
		assert(rc == -ENOMEM);
		ERROR("SPCI_SERVICE_TUN_REQUEST_BLOCKING: Queue is full.\n");
		panic();
	}

	/* Jump to the Secure Partition. */
	rx0 = spm_sp_synchronous_entry(sp_ctx, 0);

	/* Verify returned value */
	if (rx0 != SPRT_PUT_RESPONSE_AARCH64) {
		ERROR("SPM: %s: Unexpected x0 value 0x%x\n", __func__, rx0);
		panic();
	}

	rx1 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X3);
	rx2 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X4);
	rx3 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X5);

	/* Flag Secure Partition as idle. */
	assert(sp_ctx->state == SP_STATE_BUSY);
	sp_state_set(sp_ctx, SP_STATE_IDLE);

	/* Decrease count of requests. */
	spin_lock(&spci_handles_lock);
	handle_info->num_active_requests -= 1;
	spin_unlock(&spci_handles_lock);
	spm_sp_request_decrease(sp_ctx);

	/* Restore non-secure state */
	cm_el1_sysregs_context_restore(NON_SECURE);
	cm_set_next_eret_context(NON_SECURE);

	SMC_RET4(handle, SPCI_SUCCESS, rx1, rx2, rx3);
}

/*******************************************************************************
 * This function requests a Secure Service from a given handle and client ID.
 ******************************************************************************/
static uint64_t spci_service_request_start(void *handle,
			uint32_t smc_fid, u_register_t x1, u_register_t x2,
			u_register_t x3, u_register_t x4, u_register_t x5,
			u_register_t x6, u_register_t x7)
{
	spci_handle_t *handle_info;
	sp_context_t *sp_ctx;
	cpu_context_t *cpu_ctx;
	uint16_t request_handle, client_id;
	uint32_t token;

	/* Get handle array lock */
	spin_lock(&spci_handles_lock);

	/* Get pointer to struct of this open handle and client ID. */
	request_handle = (x7 >> 16U) & 0x0000FFFFU;
	client_id = x7 & 0x0000FFFFU;

	handle_info = spci_handle_info_get(request_handle, client_id);
	if (handle_info == NULL) {
		spin_unlock(&spci_handles_lock);

		WARN("SPCI_SERVICE_TUN_REQUEST_START: Not found.\n"
		     "  Handle 0x%04x. Client ID 0x%04x\n", request_handle,
		     client_id);

		SMC_RET1(handle, SPCI_INVALID_PARAMETER);
	}

	/* Get pointer to the Secure Partition that handles the service */
	sp_ctx = handle_info->sp_ctx;
	assert(sp_ctx != NULL);
	cpu_ctx = &(sp_ctx->cpu_ctx);

	/* Prevent this handle from being closed */
	handle_info->num_active_requests += 1;

	spm_sp_request_increase(sp_ctx);

	/* Create new token for this request */
	token = spci_create_token_value();

	/* Release handle lock */
	spin_unlock(&spci_handles_lock);

	/* Pass arguments to the Secure Partition */
	struct sprt_queue_entry_message message = {
		.type = SPRT_MSG_TYPE_SERVICE_TUN_REQUEST,
		.client_id = client_id,
		.service_handle = request_handle,
		.session_id = x6,
		.token = token,
		.args = {smc_fid, x1, x2, x3, x4, x5}
	};

	spin_lock(&(sp_ctx->spm_sp_buffer_lock));
	int rc = sprt_push_message((void *)sp_ctx->spm_sp_buffer_base, &message,
				   SPRT_QUEUE_NUM_NON_BLOCKING);
	spin_unlock(&(sp_ctx->spm_sp_buffer_lock));
	if (rc != 0) {
		WARN("SPCI_SERVICE_TUN_REQUEST_START: SPRT queue full.\n"
		     "  Handle 0x%04x. Client ID 0x%04x\n", request_handle,
		     client_id);
		SMC_RET1(handle, SPCI_NO_MEMORY);
	}

	/* Try to enter the partition. If it's not possible, simply return. */
	if (sp_state_try_switch(sp_ctx, SP_STATE_IDLE, SP_STATE_BUSY) != 0) {
		SMC_RET2(handle, SPCI_SUCCESS, token);
	}

	/* Save the Normal world context */
	cm_el1_sysregs_context_save(NON_SECURE);

	/*
	 * This request is non-blocking and needs to be interruptible by
	 * non-secure interrupts. Enable their routing to EL3 during the
	 * processing of the Secure Partition's service on this core.
	 */

	/* Jump to the Secure Partition. */
	uint64_t ret = spm_sp_synchronous_entry(sp_ctx, 1);

	/* Verify returned values */
	if (ret == SPRT_PUT_RESPONSE_AARCH64) {
		uint32_t token;
		uint64_t rx1, rx2, rx3, x6;

		token = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X1);
		rx1 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X3);
		rx2 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X4);
		rx3 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X5);
		x6 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X6);

		uint16_t client_id = x6 & 0xFFFFU;
		uint16_t service_handle = x6 >> 16;

		int rc = spm_response_add(client_id, service_handle, token,
					  rx1, rx2, rx3);
		if (rc != 0) {
			/*
			 * This is error fatal because we can't return to the SP
			 * from this SMC. The SP has crashed.
			 */
			panic();
		}
	} else if ((ret != SPRT_YIELD_AARCH64) &&
		   (ret != SPM_SECURE_PARTITION_PREEMPTED)) {
		ERROR("SPM: %s: Unexpected x0 value 0x%llx\n", __func__, ret);
		panic();
	}

	/* Flag Secure Partition as idle. */
	assert(sp_ctx->state == SP_STATE_BUSY);
	sp_state_set(sp_ctx, SP_STATE_IDLE);

	/* Restore non-secure state */
	cm_el1_sysregs_context_restore(NON_SECURE);
	cm_set_next_eret_context(NON_SECURE);

	SMC_RET2(handle, SPCI_SUCCESS, token);
}

/*******************************************************************************
 * This function returns the response of a Secure Service given a handle, a
 * client ID and a token. If not available, it will schedule a Secure Partition
 * and give it CPU time.
 ******************************************************************************/
static uint64_t spci_service_request_resume(void *handle, u_register_t x1,
					    u_register_t x7)
{
	int rc;
	u_register_t rx1 = 0, rx2 = 0, rx3 = 0;
	spci_handle_t *handle_info;
	sp_context_t *sp_ctx;
	cpu_context_t *cpu_ctx;
	uint32_t token = (uint32_t) x1;
	uint16_t client_id = x7 & 0x0000FFFF;
	uint16_t service_handle = (x7 >> 16) & 0x0000FFFF;

	/* Get pointer to struct of this open handle and client ID. */
	spin_lock(&spci_handles_lock);

	handle_info = spci_handle_info_get(service_handle, client_id);
	if (handle_info == NULL) {
		spin_unlock(&spci_handles_lock);
		WARN("SPCI_SERVICE_REQUEST_RESUME: Not found.\n"
		     "Handle 0x%04x. Client ID 0x%04x, Token 0x%08x.\n",
		     client_id, service_handle, token);

		SMC_RET1(handle, SPCI_INVALID_PARAMETER);
	}

	/* Get pointer to the Secure Partition that handles the service */
	sp_ctx = handle_info->sp_ctx;
	assert(sp_ctx != NULL);
	cpu_ctx = &(sp_ctx->cpu_ctx);

	spin_unlock(&spci_handles_lock);

	/* Look for a valid response in the global queue */
	rc = spm_response_get(client_id, service_handle, token,
			      &rx1, &rx2, &rx3);
	if (rc == 0) {
		/* Decrease request count */
		spin_lock(&spci_handles_lock);
		handle_info->num_active_requests -= 1;
		spin_unlock(&spci_handles_lock);
		spm_sp_request_decrease(sp_ctx);

		SMC_RET4(handle, SPCI_SUCCESS, rx1, rx2, rx3);
	}

	/* Try to enter the partition. If it's not possible, simply return. */
	if (sp_state_try_switch(sp_ctx, SP_STATE_IDLE, SP_STATE_BUSY) != 0) {
		SMC_RET1(handle, SPCI_QUEUED);
	}

	/* Save the Normal world context */
	cm_el1_sysregs_context_save(NON_SECURE);

	/*
	 * This request is non-blocking and needs to be interruptible by
	 * non-secure interrupts. Enable their routing to EL3 during the
	 * processing of the Secure Partition's service on this core.
	 */

	/* Jump to the Secure Partition. */
	uint64_t ret = spm_sp_synchronous_entry(sp_ctx, 1);

	/* Verify returned values */
	if (ret == SPRT_PUT_RESPONSE_AARCH64) {
		uint32_t token;
		uint64_t rx1, rx2, rx3, x6;

		token = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X1);
		rx1 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X3);
		rx2 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X4);
		rx3 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X5);
		x6 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X6);

		uint16_t client_id = x6 & 0xFFFFU;
		uint16_t service_handle = x6 >> 16;

		int rc = spm_response_add(client_id, service_handle, token,
					  rx1, rx2, rx3);
		if (rc != 0) {
			/*
			 * This is error fatal because we can't return to the SP
			 * from this SMC. The SP has crashed.
			 */
			panic();
		}
	} else if ((ret != SPRT_YIELD_AARCH64) &&
		   (ret != SPM_SECURE_PARTITION_PREEMPTED)) {
		ERROR("SPM: %s: Unexpected x0 value 0x%llx\n", __func__, ret);
		panic();
	}

	/* Flag Secure Partition as idle. */
	assert(sp_ctx->state == SP_STATE_BUSY);
	sp_state_set(sp_ctx, SP_STATE_IDLE);

	/* Restore non-secure state */
	cm_el1_sysregs_context_restore(NON_SECURE);
	cm_set_next_eret_context(NON_SECURE);

	/* Look for a valid response in the global queue */
	rc = spm_response_get(client_id, service_handle, token,
			      &rx1, &rx2, &rx3);
	if (rc != 0) {
		SMC_RET1(handle, SPCI_QUEUED);
	}

	/* Decrease request count */
	spin_lock(&spci_handles_lock);
	handle_info->num_active_requests -= 1;
	spin_unlock(&spci_handles_lock);
	spm_sp_request_decrease(sp_ctx);

	/* Return response */
	SMC_RET4(handle, SPCI_SUCCESS, rx1, rx2, rx3);
}

/*******************************************************************************
 * This function returns the response of a Secure Service given a handle, a
 * client ID and a token.
 ******************************************************************************/
static uint64_t spci_service_get_response(void *handle, u_register_t x1,
					    u_register_t x7)

{
	int rc;
	u_register_t rx1 = 0, rx2 = 0, rx3 = 0;
	spci_handle_t *handle_info;
	uint32_t token = (uint32_t) x1;
	uint16_t client_id = x7 & 0x0000FFFF;
	uint16_t service_handle = (x7 >> 16) & 0x0000FFFF;

	/* Get pointer to struct of this open handle and client ID. */

	spin_lock(&spci_handles_lock);

	handle_info = spci_handle_info_get(service_handle, client_id);
	if (handle_info == NULL) {
		spin_unlock(&spci_handles_lock);
		WARN("SPCI_SERVICE_GET_RESPONSE: Not found.\n"
		     "Handle 0x%04x. Client ID 0x%04x, Token 0x%08x.\n",
		     client_id, service_handle, token);

		SMC_RET1(handle, SPCI_INVALID_PARAMETER);
	}

	spin_unlock(&spci_handles_lock);

	/* Look for a valid response in the global queue */
	rc = spm_response_get(client_id, service_handle, token,
			      &rx1, &rx2, &rx3);

	if (rc != 0) {
		SMC_RET1(handle, SPCI_QUEUED);
	}

	/* Decrease request count */
	spin_lock(&spci_handles_lock);
	handle_info->num_active_requests -= 1;
	sp_context_t *sp_ctx;
	sp_ctx = handle_info->sp_ctx;
	spin_unlock(&spci_handles_lock);
	spm_sp_request_decrease(sp_ctx);

	/* Return response */
	SMC_RET4(handle, SPCI_SUCCESS, rx1, rx2, rx3);
}

/*******************************************************************************
 * This function handles all SMCs in the range reserved for SPCI.
 ******************************************************************************/
uint64_t spci_smc_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2,
			  uint64_t x3, uint64_t x4, void *cookie, void *handle,
			  uint64_t flags)
{
	uint32_t spci_fid;

	/* SPCI only supported from the Non-secure world for now */
	if (is_caller_non_secure(flags) == SMC_FROM_SECURE) {
		SMC_RET1(handle, SMC_UNK);
	}

	if ((smc_fid & SPCI_FID_TUN_FLAG) == 0) {

		/* Miscellaneous calls */

		spci_fid = (smc_fid >> SPCI_FID_MISC_SHIFT) & SPCI_FID_MISC_MASK;

		switch (spci_fid) {

		case SPCI_FID_VERSION:
			SMC_RET1(handle, SPCI_VERSION_COMPILED);

		case SPCI_FID_SERVICE_HANDLE_OPEN:
		{
			if ((smc_fid & SPCI_SERVICE_HANDLE_OPEN_NOTIFY_BIT) != 0) {
				/* Not supported for now */
				WARN("SPCI_SERVICE_HANDLE_OPEN_NOTIFY not supported.\n");
				SMC_RET1(handle, SPCI_INVALID_PARAMETER);
			}

			uint64_t x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
			uint64_t x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
			uint64_t x7 = SMC_GET_GP(handle, CTX_GPREG_X7);

			return spci_service_handle_open_poll(handle, x1, x2, x3,
							     x4, x5, x6, x7);
		}
		case SPCI_FID_SERVICE_HANDLE_CLOSE:
			return spci_service_handle_close(handle, x1);

		case SPCI_FID_SERVICE_REQUEST_BLOCKING:
		{
			uint64_t x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
			uint64_t x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
			uint64_t x7 = SMC_GET_GP(handle, CTX_GPREG_X7);

			return spci_service_request_blocking(handle,
					smc_fid, x1, x2, x3, x4, x5, x6, x7);
		}

		case SPCI_FID_SERVICE_REQUEST_START:
		{
			uint64_t x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
			uint64_t x6 = SMC_GET_GP(handle, CTX_GPREG_X6);
			uint64_t x7 = SMC_GET_GP(handle, CTX_GPREG_X7);

			return spci_service_request_start(handle,
					smc_fid, x1, x2, x3, x4, x5, x6, x7);
		}

		case SPCI_FID_SERVICE_GET_RESPONSE:
		{
			uint64_t x7 = SMC_GET_GP(handle, CTX_GPREG_X7);

			return spci_service_get_response(handle, x1, x7);
		}

		default:
			break;
		}

	} else {

		/* Tunneled calls */

		spci_fid = (smc_fid >> SPCI_FID_TUN_SHIFT) & SPCI_FID_TUN_MASK;

		switch (spci_fid) {

		case SPCI_FID_SERVICE_REQUEST_RESUME:
		{
			uint64_t x7 = SMC_GET_GP(handle, CTX_GPREG_X7);

			return spci_service_request_resume(handle, x1, x7);
		}

		default:
			break;
		}
	}

	WARN("SPCI: Unsupported call 0x%08x\n", smc_fid);
	SMC_RET1(handle, SPCI_NOT_SUPPORTED);
}