core.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * Copyright (c) 2020-2023, Arm Limited
 */

#define DRIVER_NAME "Arm TSTEE"
#define pr_fmt(fmt) DRIVER_NAME ": " fmt

#include <linux/arm_ffa.h>
#include <linux/err.h>
#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/limits.h>
#include <linux/list.h>
#include <linux/mm.h>
#include <linux/module.h>
#include <linux/scatterlist.h>
#include <linux/slab.h>
#include <linux/stat.h>
#include <linux/tee_drv.h>
#include <linux/types.h>
#include <linux/uaccess.h>

#include "tstee_private.h"
#include "uapi/arm_tstee.h"

#define FFA_INVALID_MEM_HANDLE U64_MAX

/*
 * This module implements a TEE driver to enable user space applications to
 * invoke Trusted Services functionality deployed in FF-A Secure Partitions.
 */

static void arg_list_to_ffa_data(const u32 *args, struct ffa_send_direct_data *data)
{
	data->data0 = args[0];
	data->data1 = args[1];
	data->data2 = args[2];
	data->data3 = args[3];
	data->data4 = args[4];
}

static void arg_list_from_ffa_data(const struct ffa_send_direct_data *data, u32 *args)
{
	args[0] = lower_32_bits(data->data0);
	args[1] = lower_32_bits(data->data1);
	args[2] = lower_32_bits(data->data2);
	args[3] = lower_32_bits(data->data3);
	args[4] = lower_32_bits(data->data4);
}

static void tstee_get_version(struct tee_device *teedev, struct tee_ioctl_version_data *vers)
{
	struct tstee *tstee = tee_get_drvdata(teedev);
	struct tee_ioctl_version_data v = {
		.impl_id = TEE_IMPL_ID_TSTEE,
		.impl_caps = tstee->ffa_dev->vm_id,
		.gen_caps = 0,
	};

	*vers = v;
}

static int tstee_check_rpc_compatible(struct ffa_device *ffa_dev)
{
	struct ffa_send_direct_data ffa_data;
	u32 ffa_args[5] = {};
	int rc;

	ffa_args[TS_RPC_CTRL_REG] = TS_RPC_CTRL_PACK_IFACE_OPCODE(TS_RPC_MGMT_IFACE_ID,
								  TS_RPC_OP_GET_VERSION);

	arg_list_to_ffa_data(ffa_args, &ffa_data);
	rc = ffa_dev->ops->msg_ops->sync_send_receive(ffa_dev, &ffa_data);
	if (rc)
		return rc;

	arg_list_from_ffa_data(&ffa_data, ffa_args);

	if (ffa_args[TS_RPC_GET_VERSION_RESP] != TS_RPC_PROTOCOL_VERSION)
		return -EINVAL;

	return 0;
}

static int tstee_open(struct tee_context *ctx)
{
	struct tstee *tstee = tee_get_drvdata(ctx->teedev);
	struct ts_context_data *ctxdata;
	int rc;

	if (!tstee->rpc_compatible) {
		rc = tstee_check_rpc_compatible(tstee->ffa_dev);
		if (rc)
			return rc;
		else
			tstee->rpc_compatible = true;
	}

	ctxdata = kzalloc(sizeof(*ctxdata), GFP_KERNEL);
	if (!ctxdata)
		return -ENOMEM;

	mutex_init(&ctxdata->mutex);
	idr_init(&ctxdata->sess_ids);
	INIT_LIST_HEAD(&ctxdata->sess_list);

	ctx->data = ctxdata;

	return 0;
}

static void tstee_release(struct tee_context *ctx)
{
	struct ts_context_data *ctxdata = ctx->data;
	struct ts_session *sess, *sess_tmp;

	if (!ctxdata)
		return;

	list_for_each_entry_safe(sess, sess_tmp, &ctxdata->sess_list, list_node) {
		list_del(&sess->list_node);
		idr_remove(&ctxdata->sess_ids, sess->session_id);
		kfree(sess);
	}

	idr_destroy(&ctxdata->sess_ids);
	mutex_destroy(&ctxdata->mutex);

	kfree(ctxdata);
	ctx->data = NULL;
}

static struct ts_session *find_session(struct ts_context_data *ctxdata, u32 session_id)
{
	struct ts_session *sess;

	list_for_each_entry(sess, &ctxdata->sess_list, list_node)
		if (sess->session_id == session_id)
			return sess;

	return NULL;
}

static int tstee_open_session(struct tee_context *ctx, struct tee_ioctl_open_session_arg *arg,
			      struct tee_param *param __always_unused)
{
	struct tstee *tstee = tee_get_drvdata(ctx->teedev);
	struct ffa_device *ffa_dev = tstee->ffa_dev;
	struct ts_context_data *ctxdata = ctx->data;
	struct ffa_send_direct_data ffa_data;
	struct ts_session *sess = NULL;
	u32 ffa_args[5] = {};
	int sess_id;
	int rc;

	ffa_args[TS_RPC_CTRL_REG] = TS_RPC_CTRL_PACK_IFACE_OPCODE(TS_RPC_MGMT_IFACE_ID,
								  TS_RPC_OP_SERVICE_INFO);

	memcpy((u8 *)(ffa_args + TS_RPC_SERVICE_INFO_UUID0), arg->uuid, UUID_SIZE);

	arg_list_to_ffa_data(ffa_args, &ffa_data);
	rc = ffa_dev->ops->msg_ops->sync_send_receive(ffa_dev, &ffa_data);
	if (rc)
		return rc;

	arg_list_from_ffa_data(&ffa_data, ffa_args);

	if (ffa_args[TS_RPC_SERVICE_INFO_RPC_STATUS] != TS_RPC_OK)
		return -ENODEV;

	if (ffa_args[TS_RPC_SERVICE_INFO_IFACE] > U8_MAX)
		return -EINVAL;

	sess = kzalloc(sizeof(*sess), GFP_KERNEL);
	if (!sess)
		return -ENOMEM;

	sess_id = idr_alloc(&ctxdata->sess_ids, sess, 1, 0, GFP_KERNEL);
	if (sess_id < 0) {
		kfree(sess);
		return sess_id;
	}

	sess->session_id = sess_id;
	sess->iface_id = ffa_args[TS_RPC_SERVICE_INFO_IFACE];

	mutex_lock(&ctxdata->mutex);
	list_add(&sess->list_node, &ctxdata->sess_list);
	mutex_unlock(&ctxdata->mutex);

	arg->session = sess_id;
	arg->ret = 0;

	return 0;
}

static int tstee_close_session(struct tee_context *ctx, u32 session)
{
	struct ts_context_data *ctxdata = ctx->data;
	struct ts_session *sess;

	mutex_lock(&ctxdata->mutex);
	sess = find_session(ctxdata, session);
	if (sess)
		list_del(&sess->list_node);

	mutex_unlock(&ctxdata->mutex);

	if (!sess)
		return -EINVAL;

	idr_remove(&ctxdata->sess_ids, sess->session_id);
	kfree(sess);

	return 0;
}

static int tstee_invoke_func(struct tee_context *ctx, struct tee_ioctl_invoke_arg *arg,
			     struct tee_param *param)
{
	struct tstee *tstee = tee_get_drvdata(ctx->teedev);
	struct ffa_device *ffa_dev = tstee->ffa_dev;
	struct ts_context_data *ctxdata = ctx->data;
	struct ffa_send_direct_data ffa_data;
	struct tee_shm *shm = NULL;
	struct ts_session *sess;
	u32 req_len, ffa_args[5] = {};
	int shm_id, rc;
	u8 iface_id;
	u64 handle;
	u16 opcode;

	mutex_lock(&ctxdata->mutex);
	sess = find_session(ctxdata, arg->session);

	/* Do this while holding the mutex to make sure that the session wasn't closed meanwhile */
	if (sess)
		iface_id = sess->iface_id;

	mutex_unlock(&ctxdata->mutex);
	if (!sess)
		return -EINVAL;

	opcode = lower_16_bits(arg->func);
	shm_id = lower_32_bits(param[0].u.value.a);
	req_len = lower_32_bits(param[0].u.value.b);

	if (shm_id != 0) {
		shm = tee_shm_get_from_id(ctx, shm_id);
		if (IS_ERR(shm))
			return PTR_ERR(shm);

		if (shm->size < req_len) {
			pr_err("request doesn't fit into shared memory buffer\n");
			rc = -EINVAL;
			goto out;
		}

		handle = shm->sec_world_id;
	} else {
		handle = FFA_INVALID_MEM_HANDLE;
	}

	ffa_args[TS_RPC_CTRL_REG] = TS_RPC_CTRL_PACK_IFACE_OPCODE(iface_id, opcode);
	ffa_args[TS_RPC_SERVICE_MEM_HANDLE_LSW] = lower_32_bits(handle);
	ffa_args[TS_RPC_SERVICE_MEM_HANDLE_MSW] = upper_32_bits(handle);
	ffa_args[TS_RPC_SERVICE_REQ_LEN] = req_len;
	ffa_args[TS_RPC_SERVICE_CLIENT_ID] = 0;

	arg_list_to_ffa_data(ffa_args, &ffa_data);
	rc = ffa_dev->ops->msg_ops->sync_send_receive(ffa_dev, &ffa_data);
	if (rc)
		goto out;

	arg_list_from_ffa_data(&ffa_data, ffa_args);

	if (ffa_args[TS_RPC_SERVICE_RPC_STATUS] != TS_RPC_OK) {
		pr_err("invoke_func rpc status: %d\n", ffa_args[TS_RPC_SERVICE_RPC_STATUS]);
		rc = -EINVAL;
		goto out;
	}

	arg->ret = ffa_args[TS_RPC_SERVICE_STATUS];
	if (shm && shm->size >= ffa_args[TS_RPC_SERVICE_RESP_LEN])
		param[0].u.value.a = ffa_args[TS_RPC_SERVICE_RESP_LEN];

out:
	if (shm)
		tee_shm_put(shm);

	return rc;
}

static int tstee_cancel_req(struct tee_context *ctx __always_unused, u32 cancel_id __always_unused,
			    u32 session __always_unused)
{
	return -EINVAL;
}

int tstee_shm_register(struct tee_context *ctx, struct tee_shm *shm, struct page **pages,
		       size_t num_pages, unsigned long start __always_unused)
{
	struct tstee *tstee = tee_get_drvdata(ctx->teedev);
	struct ffa_device *ffa_dev = tstee->ffa_dev;
	struct ffa_mem_region_attributes mem_attr = {
		.receiver = tstee->ffa_dev->vm_id,
		.attrs = FFA_MEM_RW,
		.flag = 0,
	};
	struct ffa_mem_ops_args mem_args = {
		.attrs = &mem_attr,
		.use_txbuf = true,
		.nattrs = 1,
		.flags = 0,
	};
	struct ffa_send_direct_data ffa_data;
	struct sg_table sgt;
	u32 ffa_args[5] = {};
	int rc;

	rc = sg_alloc_table_from_pages(&sgt, pages, num_pages, 0, num_pages * PAGE_SIZE,
				       GFP_KERNEL);
	if (rc)
		return rc;

	mem_args.sg = sgt.sgl;
	rc = ffa_dev->ops->mem_ops->memory_share(&mem_args);
	sg_free_table(&sgt);
	if (rc)
		return rc;

	shm->sec_world_id = mem_args.g_handle;

	ffa_args[TS_RPC_CTRL_REG] = TS_RPC_CTRL_PACK_IFACE_OPCODE(TS_RPC_MGMT_IFACE_ID,
								  TS_RPC_OP_RETRIEVE_MEM);
	ffa_args[TS_RPC_RETRIEVE_MEM_HANDLE_LSW] = lower_32_bits(shm->sec_world_id);
	ffa_args[TS_RPC_RETRIEVE_MEM_HANDLE_MSW] = upper_32_bits(shm->sec_world_id);
	ffa_args[TS_RPC_RETRIEVE_MEM_TAG_LSW] = 0;
	ffa_args[TS_RPC_RETRIEVE_MEM_TAG_MSW] = 0;

	arg_list_to_ffa_data(ffa_args, &ffa_data);
	rc = ffa_dev->ops->msg_ops->sync_send_receive(ffa_dev, &ffa_data);
	if (rc) {
		(void)ffa_dev->ops->mem_ops->memory_reclaim(shm->sec_world_id, 0);
		return rc;
	}

	arg_list_from_ffa_data(&ffa_data, ffa_args);

	if (ffa_args[TS_RPC_RETRIEVE_MEM_RPC_STATUS] != TS_RPC_OK) {
		pr_err("shm_register rpc status: %d\n", ffa_args[TS_RPC_RETRIEVE_MEM_RPC_STATUS]);
		ffa_dev->ops->mem_ops->memory_reclaim(shm->sec_world_id, 0);
		return -EINVAL;
	}

	return 0;
}

int tstee_shm_unregister(struct tee_context *ctx, struct tee_shm *shm)
{
	struct tstee *tstee = tee_get_drvdata(ctx->teedev);
	struct ffa_device *ffa_dev = tstee->ffa_dev;
	struct ffa_send_direct_data ffa_data;
	u32 ffa_args[5] = {};
	int rc;

	ffa_args[TS_RPC_CTRL_REG] = TS_RPC_CTRL_PACK_IFACE_OPCODE(TS_RPC_MGMT_IFACE_ID,
								  TS_RPC_OP_RELINQ_MEM);
	ffa_args[TS_RPC_RELINQ_MEM_HANDLE_LSW] = lower_32_bits(shm->sec_world_id);
	ffa_args[TS_RPC_RELINQ_MEM_HANDLE_MSW] = upper_32_bits(shm->sec_world_id);

	arg_list_to_ffa_data(ffa_args, &ffa_data);
	rc = ffa_dev->ops->msg_ops->sync_send_receive(ffa_dev, &ffa_data);
	if (rc)
		return rc;
	arg_list_from_ffa_data(&ffa_data, ffa_args);

	if (ffa_args[TS_RPC_RELINQ_MEM_RPC_STATUS] != TS_RPC_OK) {
		pr_err("shm_unregister rpc status: %d\n", ffa_args[TS_RPC_RELINQ_MEM_RPC_STATUS]);
		return -EINVAL;
	}

	rc = ffa_dev->ops->mem_ops->memory_reclaim(shm->sec_world_id, 0);

	return rc;
}

static const struct tee_driver_ops tstee_ops = {
	.get_version = tstee_get_version,
	.open = tstee_open,
	.release = tstee_release,
	.open_session = tstee_open_session,
	.close_session = tstee_close_session,
	.invoke_func = tstee_invoke_func,
	.cancel_req = tstee_cancel_req,
	.shm_register = tstee_shm_register,
	.shm_unregister = tstee_shm_unregister,
};

static const struct tee_desc tstee_desc = {
	.name = "tstee-clnt",
	.ops = &tstee_ops,
	.owner = THIS_MODULE,
};

static void tstee_deinit_common(struct tstee *tstee)
{
	tee_device_unregister(tstee->teedev);
	if (tstee->pool)
		tee_shm_pool_free(tstee->pool);

	kfree(tstee);
}

static int tstee_probe(struct ffa_device *ffa_dev)
{
	struct tstee *tstee;
	int rc;

	ffa_dev->ops->msg_ops->mode_32bit_set(ffa_dev);

	tstee = kzalloc(sizeof(*tstee), GFP_KERNEL);
	if (!tstee)
		return -ENOMEM;

	tstee->ffa_dev = ffa_dev;

	tstee->pool = tstee_create_shm_pool();
	if (IS_ERR(tstee->pool)) {
		rc = PTR_ERR(tstee->pool);
		tstee->pool = NULL;
		goto err;
	}

	tstee->teedev = tee_device_alloc(&tstee_desc, NULL, tstee->pool, tstee);
	if (IS_ERR(tstee->teedev)) {
		rc = PTR_ERR(tstee->teedev);
		tstee->teedev = NULL;
		goto err;
	}

	/* Don't fail if the check doesn't succeed now, maybe the SP is busy */
	tstee->rpc_compatible = (tstee_check_rpc_compatible(ffa_dev) == 0);

	rc = tee_device_register(tstee->teedev);
	if (rc)
		goto err;

	ffa_dev_set_drvdata(ffa_dev, tstee);

	pr_info("driver initialized for endpoint 0x%x\n", ffa_dev->vm_id);

	return 0;

err:
	tstee_deinit_common(tstee);
	return rc;
}

static void tstee_remove(struct ffa_device *ffa_dev)
{
	tstee_deinit_common(ffa_dev->dev.driver_data);
}

static const struct ffa_device_id tstee_device_ids[] = {
	/* TS RPC protocol UUID: bdcd76d7-825e-4751-963b-86d4f84943ac */
	{ TS_RPC_UUID },
	{}
};

static struct ffa_driver tstee_driver = {
	.name = "arm_tstee",
	.probe = tstee_probe,
	.remove = tstee_remove,
	.id_table = tstee_device_ids,
};

static int __init mod_init(void)
{
	if (!IS_REACHABLE(CONFIG_TEE) || !IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT))
		return -EOPNOTSUPP;

	return ffa_register(&tstee_driver);
}
module_init(mod_init)

static void __exit mod_exit(void)
{
	if (IS_REACHABLE(CONFIG_TEE) && IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT))
		ffa_unregister(&tstee_driver);
}
module_exit(mod_exit)

MODULE_ALIAS("arm-tstee");
MODULE_AUTHOR("Balint Dobszay <balint.dobszay@arm.com>");
MODULE_DESCRIPTION("Arm Trusted Services TEE driver");
MODULE_LICENSE("GPL v2");
MODULE_VERSION("2.0.0");