| @@ -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"); |