| @@ -0,0 +1,480 @@ |
| 1 | +// SPDX-License-Identifier: GPL-2.0-only |
| 2 | +/* |
| 3 | + * Copyright (c) 2023, Arm Limited |
| 4 | + */ |
| 5 | + |
| 6 | +#include <linux/arm_ffa.h> |
| 7 | +#include <linux/err.h> |
| 8 | +#include <linux/errno.h> |
| 9 | +#include <linux/kernel.h> |
| 10 | +#include <linux/limits.h> |
| 11 | +#include <linux/mm.h> |
| 12 | +#include <linux/module.h> |
| 13 | +#include <linux/scatterlist.h> |
| 14 | +#include <linux/slab.h> |
| 15 | +#include <linux/tee_core.h> |
| 16 | +#include <linux/types.h> |
| 17 | +#include <linux/uuid.h> |
| 18 | +#include <linux/xarray.h> |
| 19 | +#include "tstee_private.h" |
| 20 | + |
| 21 | +#define FFA_DIRECT_REQ_ARG_NUM 5 |
| 22 | +#define FFA_INVALID_MEM_HANDLE U64_MAX |
| 23 | + |
| 24 | +static void arg_list_to_ffa_data(const u32 *args, |
| 25 | + struct ffa_send_direct_data *data) |
| 26 | +{ |
| 27 | + data->data0 = args[0]; |
| 28 | + data->data1 = args[1]; |
| 29 | + data->data2 = args[2]; |
| 30 | + data->data3 = args[3]; |
| 31 | + data->data4 = args[4]; |
| 32 | +} |
| 33 | + |
| 34 | +static void arg_list_from_ffa_data(const struct ffa_send_direct_data *data, |
| 35 | + u32 *args) |
| 36 | +{ |
| 37 | + args[0] = lower_32_bits(data->data0); |
| 38 | + args[1] = lower_32_bits(data->data1); |
| 39 | + args[2] = lower_32_bits(data->data2); |
| 40 | + args[3] = lower_32_bits(data->data3); |
| 41 | + args[4] = lower_32_bits(data->data4); |
| 42 | +} |
| 43 | + |
| 44 | +static void tstee_get_version(struct tee_device *teedev, |
| 45 | + struct tee_ioctl_version_data *vers) |
| 46 | +{ |
| 47 | + struct tstee *tstee = tee_get_drvdata(teedev); |
| 48 | + struct tee_ioctl_version_data v = { |
| 49 | + .impl_id = TEE_IMPL_ID_TSTEE, |
| 50 | + /* FF-A endpoint ID only uses the lower 16 bits */ |
| 51 | + .impl_caps = lower_16_bits(tstee->ffa_dev->vm_id), |
| 52 | + .gen_caps = 0, |
| 53 | + }; |
| 54 | + |
| 55 | + *vers = v; |
| 56 | +} |
| 57 | + |
| 58 | +static int tstee_open(struct tee_context *ctx) |
| 59 | +{ |
| 60 | + struct ts_context_data *ctxdata; |
| 61 | + |
| 62 | + ctxdata = kzalloc(sizeof(*ctxdata), GFP_KERNEL); |
| 63 | + if (!ctxdata) |
| 64 | + return -ENOMEM; |
| 65 | + |
| 66 | + xa_init_flags(&ctxdata->sess_list, XA_FLAGS_ALLOC); |
| 67 | + |
| 68 | + ctx->data = ctxdata; |
| 69 | + |
| 70 | + return 0; |
| 71 | +} |
| 72 | + |
| 73 | +static void tstee_release(struct tee_context *ctx) |
| 74 | +{ |
| 75 | + struct ts_context_data *ctxdata = ctx->data; |
| 76 | + struct ts_session *sess; |
| 77 | + unsigned long idx; |
| 78 | + |
| 79 | + if (!ctxdata) |
| 80 | + return; |
| 81 | + |
| 82 | + xa_for_each(&ctxdata->sess_list, idx, sess) { |
| 83 | + xa_erase(&ctxdata->sess_list, idx); |
| 84 | + kfree(sess); |
| 85 | + } |
| 86 | + |
| 87 | + xa_destroy(&ctxdata->sess_list); |
| 88 | + |
| 89 | + kfree(ctxdata); |
| 90 | + ctx->data = NULL; |
| 91 | +} |
| 92 | + |
| 93 | +static int tstee_open_session(struct tee_context *ctx, |
| 94 | + struct tee_ioctl_open_session_arg *arg, |
| 95 | + struct tee_param *param __always_unused) |
| 96 | +{ |
| 97 | + struct tstee *tstee = tee_get_drvdata(ctx->teedev); |
| 98 | + struct ffa_device *ffa_dev = tstee->ffa_dev; |
| 99 | + struct ts_context_data *ctxdata = ctx->data; |
| 100 | + struct ffa_send_direct_data ffa_data; |
| 101 | + struct ts_session *sess = NULL; |
| 102 | + u32 ffa_args[FFA_DIRECT_REQ_ARG_NUM] = {}; |
| 103 | + u32 sess_id; |
| 104 | + int rc; |
| 105 | + |
| 106 | + ffa_args[TS_RPC_CTRL_REG] = |
| 107 | + TS_RPC_CTRL_PACK_IFACE_OPCODE(TS_RPC_MGMT_IFACE_ID, |
| 108 | + TS_RPC_OP_SERVICE_INFO); |
| 109 | + |
| 110 | + memcpy(ffa_args + TS_RPC_SERVICE_INFO_UUID0, arg->uuid, UUID_SIZE); |
| 111 | + |
| 112 | + arg_list_to_ffa_data(ffa_args, &ffa_data); |
| 113 | + rc = ffa_dev->ops->msg_ops->sync_send_receive(ffa_dev, &ffa_data); |
| 114 | + if (rc) |
| 115 | + return rc; |
| 116 | + |
| 117 | + arg_list_from_ffa_data(&ffa_data, ffa_args); |
| 118 | + |
| 119 | + if (ffa_args[TS_RPC_SERVICE_INFO_RPC_STATUS] != TS_RPC_OK) |
| 120 | + return -ENODEV; |
| 121 | + |
| 122 | + if (ffa_args[TS_RPC_SERVICE_INFO_IFACE] > U8_MAX) |
| 123 | + return -EINVAL; |
| 124 | + |
| 125 | + sess = kzalloc(sizeof(*sess), GFP_KERNEL); |
| 126 | + if (!sess) |
| 127 | + return -ENOMEM; |
| 128 | + |
| 129 | + sess->iface_id = ffa_args[TS_RPC_SERVICE_INFO_IFACE]; |
| 130 | + |
| 131 | + rc = xa_alloc(&ctxdata->sess_list, &sess_id, sess, xa_limit_32b, |
| 132 | + GFP_KERNEL); |
| 133 | + if (rc) { |
| 134 | + kfree(sess); |
| 135 | + return rc; |
| 136 | + } |
| 137 | + |
| 138 | + arg->session = sess_id; |
| 139 | + arg->ret = 0; |
| 140 | + |
| 141 | + return 0; |
| 142 | +} |
| 143 | + |
| 144 | +static int tstee_close_session(struct tee_context *ctx, u32 session) |
| 145 | +{ |
| 146 | + struct ts_context_data *ctxdata = ctx->data; |
| 147 | + struct ts_session *sess; |
| 148 | + |
| 149 | + /* Calls xa_lock() internally */ |
| 150 | + sess = xa_erase(&ctxdata->sess_list, session); |
| 151 | + if (!sess) |
| 152 | + return -EINVAL; |
| 153 | + |
| 154 | + kfree(sess); |
| 155 | + |
| 156 | + return 0; |
| 157 | +} |
| 158 | + |
| 159 | +static int tstee_invoke_func(struct tee_context *ctx, |
| 160 | + struct tee_ioctl_invoke_arg *arg, |
| 161 | + struct tee_param *param) |
| 162 | +{ |
| 163 | + struct tstee *tstee = tee_get_drvdata(ctx->teedev); |
| 164 | + struct ffa_device *ffa_dev = tstee->ffa_dev; |
| 165 | + struct ts_context_data *ctxdata = ctx->data; |
| 166 | + struct ffa_send_direct_data ffa_data; |
| 167 | + struct tee_shm *shm = NULL; |
| 168 | + struct ts_session *sess; |
| 169 | + u32 req_len, ffa_args[FFA_DIRECT_REQ_ARG_NUM] = {}; |
| 170 | + int shm_id, rc; |
| 171 | + u8 iface_id; |
| 172 | + u64 handle; |
| 173 | + u16 opcode; |
| 174 | + |
| 175 | + xa_lock(&ctxdata->sess_list); |
| 176 | + sess = xa_load(&ctxdata->sess_list, arg->session); |
| 177 | + |
| 178 | + /* |
| 179 | + * Do this while holding the lock to make sure that the session wasn't |
| 180 | + * closed meanwhile |
| 181 | + */ |
| 182 | + if (sess) |
| 183 | + iface_id = sess->iface_id; |
| 184 | + |
| 185 | + xa_unlock(&ctxdata->sess_list); |
| 186 | + if (!sess) |
| 187 | + return -EINVAL; |
| 188 | + |
| 189 | + opcode = lower_16_bits(arg->func); |
| 190 | + shm_id = lower_32_bits(param[0].u.value.a); |
| 191 | + req_len = lower_32_bits(param[0].u.value.b); |
| 192 | + |
| 193 | + if (shm_id != 0) { |
| 194 | + shm = tee_shm_get_from_id(ctx, shm_id); |
| 195 | + if (IS_ERR(shm)) |
| 196 | + return PTR_ERR(shm); |
| 197 | + |
| 198 | + if (shm->size < req_len) { |
| 199 | + dev_err(&ffa_dev->dev, |
| 200 | + "request doesn't fit into shared memory buffer\n"); |
| 201 | + rc = -EINVAL; |
| 202 | + goto out; |
| 203 | + } |
| 204 | + |
| 205 | + handle = shm->sec_world_id; |
| 206 | + } else { |
| 207 | + handle = FFA_INVALID_MEM_HANDLE; |
| 208 | + } |
| 209 | + |
| 210 | + ffa_args[TS_RPC_CTRL_REG] = TS_RPC_CTRL_PACK_IFACE_OPCODE(iface_id, |
| 211 | + opcode); |
| 212 | + ffa_args[TS_RPC_SERVICE_MEM_HANDLE_LSW] = lower_32_bits(handle); |
| 213 | + ffa_args[TS_RPC_SERVICE_MEM_HANDLE_MSW] = upper_32_bits(handle); |
| 214 | + ffa_args[TS_RPC_SERVICE_REQ_LEN] = req_len; |
| 215 | + ffa_args[TS_RPC_SERVICE_CLIENT_ID] = 0; |
| 216 | + |
| 217 | + arg_list_to_ffa_data(ffa_args, &ffa_data); |
| 218 | + rc = ffa_dev->ops->msg_ops->sync_send_receive(ffa_dev, &ffa_data); |
| 219 | + if (rc) |
| 220 | + goto out; |
| 221 | + |
| 222 | + arg_list_from_ffa_data(&ffa_data, ffa_args); |
| 223 | + |
| 224 | + if (ffa_args[TS_RPC_SERVICE_RPC_STATUS] != TS_RPC_OK) { |
| 225 | + dev_err(&ffa_dev->dev, "invoke_func rpc status: %d\n", |
| 226 | + ffa_args[TS_RPC_SERVICE_RPC_STATUS]); |
| 227 | + rc = -EINVAL; |
| 228 | + goto out; |
| 229 | + } |
| 230 | + |
| 231 | + arg->ret = ffa_args[TS_RPC_SERVICE_STATUS]; |
| 232 | + if (shm && shm->size >= ffa_args[TS_RPC_SERVICE_RESP_LEN]) |
| 233 | + param[0].u.value.a = ffa_args[TS_RPC_SERVICE_RESP_LEN]; |
| 234 | + |
| 235 | +out: |
| 236 | + if (shm) |
| 237 | + tee_shm_put(shm); |
| 238 | + |
| 239 | + return rc; |
| 240 | +} |
| 241 | + |
| 242 | +static int tstee_shm_register(struct tee_context *ctx, struct tee_shm *shm, |
| 243 | + struct page **pages, size_t num_pages, |
| 244 | + unsigned long start __always_unused) |
| 245 | +{ |
| 246 | + struct tstee *tstee = tee_get_drvdata(ctx->teedev); |
| 247 | + struct ffa_device *ffa_dev = tstee->ffa_dev; |
| 248 | + struct ffa_mem_region_attributes mem_attr = { |
| 249 | + .receiver = tstee->ffa_dev->vm_id, |
| 250 | + .attrs = FFA_MEM_RW, |
| 251 | + .flag = 0, |
| 252 | + }; |
| 253 | + struct ffa_mem_ops_args mem_args = { |
| 254 | + .attrs = &mem_attr, |
| 255 | + .use_txbuf = true, |
| 256 | + .nattrs = 1, |
| 257 | + .flags = 0, |
| 258 | + }; |
| 259 | + struct ffa_send_direct_data ffa_data; |
| 260 | + struct sg_table sgt; |
| 261 | + u32 ffa_args[FFA_DIRECT_REQ_ARG_NUM] = {}; |
| 262 | + int rc; |
| 263 | + |
| 264 | + rc = sg_alloc_table_from_pages(&sgt, pages, num_pages, 0, |
| 265 | + num_pages * PAGE_SIZE, GFP_KERNEL); |
| 266 | + if (rc) |
| 267 | + return rc; |
| 268 | + |
| 269 | + mem_args.sg = sgt.sgl; |
| 270 | + rc = ffa_dev->ops->mem_ops->memory_share(&mem_args); |
| 271 | + sg_free_table(&sgt); |
| 272 | + if (rc) |
| 273 | + return rc; |
| 274 | + |
| 275 | + shm->sec_world_id = mem_args.g_handle; |
| 276 | + |
| 277 | + ffa_args[TS_RPC_CTRL_REG] = |
| 278 | + TS_RPC_CTRL_PACK_IFACE_OPCODE(TS_RPC_MGMT_IFACE_ID, |
| 279 | + TS_RPC_OP_RETRIEVE_MEM); |
| 280 | + ffa_args[TS_RPC_RETRIEVE_MEM_HANDLE_LSW] = |
| 281 | + lower_32_bits(shm->sec_world_id); |
| 282 | + ffa_args[TS_RPC_RETRIEVE_MEM_HANDLE_MSW] = |
| 283 | + upper_32_bits(shm->sec_world_id); |
| 284 | + ffa_args[TS_RPC_RETRIEVE_MEM_TAG_LSW] = 0; |
| 285 | + ffa_args[TS_RPC_RETRIEVE_MEM_TAG_MSW] = 0; |
| 286 | + |
| 287 | + arg_list_to_ffa_data(ffa_args, &ffa_data); |
| 288 | + rc = ffa_dev->ops->msg_ops->sync_send_receive(ffa_dev, &ffa_data); |
| 289 | + if (rc) { |
| 290 | + (void)ffa_dev->ops->mem_ops->memory_reclaim(shm->sec_world_id, |
| 291 | + 0); |
| 292 | + return rc; |
| 293 | + } |
| 294 | + |
| 295 | + arg_list_from_ffa_data(&ffa_data, ffa_args); |
| 296 | + |
| 297 | + if (ffa_args[TS_RPC_RETRIEVE_MEM_RPC_STATUS] != TS_RPC_OK) { |
| 298 | + dev_err(&ffa_dev->dev, "shm_register rpc status: %d\n", |
| 299 | + ffa_args[TS_RPC_RETRIEVE_MEM_RPC_STATUS]); |
| 300 | + ffa_dev->ops->mem_ops->memory_reclaim(shm->sec_world_id, 0); |
| 301 | + return -EINVAL; |
| 302 | + } |
| 303 | + |
| 304 | + return 0; |
| 305 | +} |
| 306 | + |
| 307 | +static int tstee_shm_unregister(struct tee_context *ctx, struct tee_shm *shm) |
| 308 | +{ |
| 309 | + struct tstee *tstee = tee_get_drvdata(ctx->teedev); |
| 310 | + struct ffa_device *ffa_dev = tstee->ffa_dev; |
| 311 | + struct ffa_send_direct_data ffa_data; |
| 312 | + u32 ffa_args[FFA_DIRECT_REQ_ARG_NUM] = {}; |
| 313 | + int rc; |
| 314 | + |
| 315 | + ffa_args[TS_RPC_CTRL_REG] = |
| 316 | + TS_RPC_CTRL_PACK_IFACE_OPCODE(TS_RPC_MGMT_IFACE_ID, |
| 317 | + TS_RPC_OP_RELINQ_MEM); |
| 318 | + ffa_args[TS_RPC_RELINQ_MEM_HANDLE_LSW] = |
| 319 | + lower_32_bits(shm->sec_world_id); |
| 320 | + ffa_args[TS_RPC_RELINQ_MEM_HANDLE_MSW] = |
| 321 | + upper_32_bits(shm->sec_world_id); |
| 322 | + |
| 323 | + arg_list_to_ffa_data(ffa_args, &ffa_data); |
| 324 | + rc = ffa_dev->ops->msg_ops->sync_send_receive(ffa_dev, &ffa_data); |
| 325 | + if (rc) |
| 326 | + return rc; |
| 327 | + arg_list_from_ffa_data(&ffa_data, ffa_args); |
| 328 | + |
| 329 | + if (ffa_args[TS_RPC_RELINQ_MEM_RPC_STATUS] != TS_RPC_OK) { |
| 330 | + dev_err(&ffa_dev->dev, "shm_unregister rpc status: %d\n", |
| 331 | + ffa_args[TS_RPC_RELINQ_MEM_RPC_STATUS]); |
| 332 | + return -EINVAL; |
| 333 | + } |
| 334 | + |
| 335 | + rc = ffa_dev->ops->mem_ops->memory_reclaim(shm->sec_world_id, 0); |
| 336 | + |
| 337 | + return rc; |
| 338 | +} |
| 339 | + |
| 340 | +static const struct tee_driver_ops tstee_ops = { |
| 341 | + .get_version = tstee_get_version, |
| 342 | + .open = tstee_open, |
| 343 | + .release = tstee_release, |
| 344 | + .open_session = tstee_open_session, |
| 345 | + .close_session = tstee_close_session, |
| 346 | + .invoke_func = tstee_invoke_func, |
| 347 | +}; |
| 348 | + |
| 349 | +static const struct tee_desc tstee_desc = { |
| 350 | + .name = "tstee-clnt", |
| 351 | + .ops = &tstee_ops, |
| 352 | + .owner = THIS_MODULE, |
| 353 | +}; |
| 354 | + |
| 355 | +static int pool_op_alloc(struct tee_shm_pool *pool, struct tee_shm *shm, |
| 356 | + size_t size, size_t align) |
| 357 | +{ |
| 358 | + return tee_dyn_shm_alloc_helper(shm, size, align, tstee_shm_register); |
| 359 | +} |
| 360 | + |
| 361 | +static void pool_op_free(struct tee_shm_pool *pool, struct tee_shm *shm) |
| 362 | +{ |
| 363 | + tee_dyn_shm_free_helper(shm, tstee_shm_unregister); |
| 364 | +} |
| 365 | + |
| 366 | +static void pool_op_destroy_pool(struct tee_shm_pool *pool) |
| 367 | +{ |
| 368 | + kfree(pool); |
| 369 | +} |
| 370 | + |
| 371 | +static const struct tee_shm_pool_ops pool_ops = { |
| 372 | + .alloc = pool_op_alloc, |
| 373 | + .free = pool_op_free, |
| 374 | + .destroy_pool = pool_op_destroy_pool, |
| 375 | +}; |
| 376 | + |
| 377 | +static struct tee_shm_pool *tstee_create_shm_pool(void) |
| 378 | +{ |
| 379 | + struct tee_shm_pool *pool = kzalloc(sizeof(*pool), GFP_KERNEL); |
| 380 | + |
| 381 | + if (!pool) |
| 382 | + return ERR_PTR(-ENOMEM); |
| 383 | + |
| 384 | + pool->ops = &pool_ops; |
| 385 | + |
| 386 | + return pool; |
| 387 | +} |
| 388 | + |
| 389 | +static bool tstee_check_rpc_compatible(struct ffa_device *ffa_dev) |
| 390 | +{ |
| 391 | + struct ffa_send_direct_data ffa_data; |
| 392 | + u32 ffa_args[FFA_DIRECT_REQ_ARG_NUM] = {}; |
| 393 | + |
| 394 | + ffa_args[TS_RPC_CTRL_REG] = |
| 395 | + TS_RPC_CTRL_PACK_IFACE_OPCODE(TS_RPC_MGMT_IFACE_ID, |
| 396 | + TS_RPC_OP_GET_VERSION); |
| 397 | + |
| 398 | + arg_list_to_ffa_data(ffa_args, &ffa_data); |
| 399 | + if (ffa_dev->ops->msg_ops->sync_send_receive(ffa_dev, &ffa_data)) |
| 400 | + return false; |
| 401 | + |
| 402 | + arg_list_from_ffa_data(&ffa_data, ffa_args); |
| 403 | + |
| 404 | + return ffa_args[TS_RPC_GET_VERSION_RESP] == TS_RPC_PROTOCOL_VERSION; |
| 405 | +} |
| 406 | + |
| 407 | +static int tstee_probe(struct ffa_device *ffa_dev) |
| 408 | +{ |
| 409 | + struct tstee *tstee; |
| 410 | + int rc; |
| 411 | + |
| 412 | + ffa_dev->ops->msg_ops->mode_32bit_set(ffa_dev); |
| 413 | + |
| 414 | + if (!tstee_check_rpc_compatible(ffa_dev)) |
| 415 | + return -EINVAL; |
| 416 | + |
| 417 | + tstee = kzalloc(sizeof(*tstee), GFP_KERNEL); |
| 418 | + if (!tstee) |
| 419 | + return -ENOMEM; |
| 420 | + |
| 421 | + tstee->ffa_dev = ffa_dev; |
| 422 | + |
| 423 | + tstee->pool = tstee_create_shm_pool(); |
| 424 | + if (IS_ERR(tstee->pool)) { |
| 425 | + rc = PTR_ERR(tstee->pool); |
| 426 | + tstee->pool = NULL; |
| 427 | + goto err_free_tstee; |
| 428 | + } |
| 429 | + |
| 430 | + tstee->teedev = tee_device_alloc(&tstee_desc, NULL, tstee->pool, tstee); |
| 431 | + if (IS_ERR(tstee->teedev)) { |
| 432 | + rc = PTR_ERR(tstee->teedev); |
| 433 | + tstee->teedev = NULL; |
| 434 | + goto err_free_pool; |
| 435 | + } |
| 436 | + |
| 437 | + rc = tee_device_register(tstee->teedev); |
| 438 | + if (rc) |
| 439 | + goto err_unreg_teedev; |
| 440 | + |
| 441 | + ffa_dev_set_drvdata(ffa_dev, tstee); |
| 442 | + |
| 443 | + return 0; |
| 444 | + |
| 445 | +err_unreg_teedev: |
| 446 | + tee_device_unregister(tstee->teedev); |
| 447 | +err_free_pool: |
| 448 | + tee_shm_pool_free(tstee->pool); |
| 449 | +err_free_tstee: |
| 450 | + kfree(tstee); |
| 451 | + return rc; |
| 452 | +} |
| 453 | + |
| 454 | +static void tstee_remove(struct ffa_device *ffa_dev) |
| 455 | +{ |
| 456 | + struct tstee *tstee = ffa_dev->dev.driver_data; |
| 457 | + |
| 458 | + tee_device_unregister(tstee->teedev); |
| 459 | + tee_shm_pool_free(tstee->pool); |
| 460 | + kfree(tstee); |
| 461 | +} |
| 462 | + |
| 463 | +static const struct ffa_device_id tstee_device_ids[] = { |
| 464 | + /* TS RPC protocol UUID: bdcd76d7-825e-4751-963b-86d4f84943ac */ |
| 465 | + { TS_RPC_UUID }, |
| 466 | + {} |
| 467 | +}; |
| 468 | + |
| 469 | +static struct ffa_driver tstee_driver = { |
| 470 | + .name = "arm_tstee", |
| 471 | + .probe = tstee_probe, |
| 472 | + .remove = tstee_remove, |
| 473 | + .id_table = tstee_device_ids, |
| 474 | +}; |
| 475 | + |
| 476 | +module_ffa_driver(tstee_driver); |
| 477 | + |
| 478 | +MODULE_AUTHOR("Balint Dobszay <balint.dobszay@arm.com>"); |
| 479 | +MODULE_DESCRIPTION("Arm Trusted Services TEE driver"); |
| 480 | +MODULE_LICENSE("GPL"); |