drivers/tee/optee/ffa_abi.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (c) 2021, 2023 Linaro Limited
   4  */
   5
   6 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
   7
   8 #include <linux/arm_ffa.h>
   9 #include <linux/errno.h>
  10 #include <linux/scatterlist.h>
  11 #include <linux/sched.h>
  12 #include <linux/slab.h>
  13 #include <linux/string.h>
  14 #include <linux/tee_drv.h>
  15 #include <linux/types.h>
  16 #include "optee_private.h"
  17 #include "optee_ffa.h"
  18 #include "optee_rpc_cmd.h"
  19
  20 /*
  21  * This file implement the FF-A ABI used when communicating with secure world
  22  * OP-TEE OS via FF-A.
  23  * This file is divided into the following sections:
  24  * 1. Maintain a hash table for lookup of a global FF-A memory handle
  25  * 2. Convert between struct tee_param and struct optee_msg_param
  26  * 3. Low level support functions to register shared memory in secure world
  27  * 4. Dynamic shared memory pool based on alloc_pages()
  28  * 5. Do a normal scheduled call into secure world
  29  * 6. Driver initialization.
  30  */
  31
  32 /*
  33  * 1. Maintain a hash table for lookup of a global FF-A memory handle
  34  *
  35  * FF-A assigns a global memory handle for each piece shared memory.
  36  * This handle is then used when communicating with secure world.
  37  *
  38  * Main functions are optee_shm_add_ffa_handle() and optee_shm_rem_ffa_handle()
  39  */
  40 struct shm_rhash {
  41         struct tee_shm *shm;
  42         u64 global_id;
  43         struct rhash_head linkage;
  44 };
  45
  46 static void rh_free_fn(void *ptr, void *arg)
  47 {
  48         kfree(ptr);
  49 }
  50
  51 static const struct rhashtable_params shm_rhash_params = {
  52         .head_offset = offsetof(struct shm_rhash, linkage),
  53         .key_len     = sizeof(u64),
  54         .key_offset  = offsetof(struct shm_rhash, global_id),
  55         .automatic_shrinking = true,
  56 };
  57
  58 static struct tee_shm *optee_shm_from_ffa_handle(struct optee *optee,
  59                                                  u64 global_id)
  60 {
  61         struct tee_shm *shm = NULL;
  62         struct shm_rhash *r;
  63
  64         mutex_lock(&optee->ffa.mutex);
  65         r = rhashtable_lookup_fast(&optee->ffa.global_ids, &global_id,
  66                                    shm_rhash_params);
  67         if (r)
  68                 shm = r->shm;
  69         mutex_unlock(&optee->ffa.mutex);
  70
  71         return shm;
  72 }
  73
  74 static int optee_shm_add_ffa_handle(struct optee *optee, struct tee_shm *shm,
  75                                     u64 global_id)
  76 {
  77         struct shm_rhash *r;
  78         int rc;
  79
  80         r = kmalloc(sizeof(*r), GFP_KERNEL);
  81         if (!r)
  82                 return -ENOMEM;
  83         r->shm = shm;
  84         r->global_id = global_id;
  85
  86         mutex_lock(&optee->ffa.mutex);
  87         rc = rhashtable_lookup_insert_fast(&optee->ffa.global_ids, &r->linkage,
  88                                            shm_rhash_params);
  89         mutex_unlock(&optee->ffa.mutex);
  90
  91         if (rc)
  92                 kfree(r);
  93
  94         return rc;
  95 }
  96
  97 static int optee_shm_rem_ffa_handle(struct optee *optee, u64 global_id)
  98 {
  99         struct shm_rhash *r;
 100         int rc = -ENOENT;
 101
 102         mutex_lock(&optee->ffa.mutex);
 103         r = rhashtable_lookup_fast(&optee->ffa.global_ids, &global_id,
 104                                    shm_rhash_params);
 105         if (r)
 106                 rc = rhashtable_remove_fast(&optee->ffa.global_ids,
 107                                             &r->linkage, shm_rhash_params);
 108         mutex_unlock(&optee->ffa.mutex);
 109
 110         if (!rc)
 111                 kfree(r);
 112
 113         return rc;
 114 }
 115
 116 /*
 117  * 2. Convert between struct tee_param and struct optee_msg_param
 118  *
 119  * optee_ffa_from_msg_param() and optee_ffa_to_msg_param() are the main
 120  * functions.
 121  */
 122
 123 static void from_msg_param_ffa_mem(struct optee *optee, struct tee_param *p,
 124                                    u32 attr, const struct optee_msg_param *mp)
 125 {
 126         struct tee_shm *shm = NULL;
 127         u64 offs_high = 0;
 128         u64 offs_low = 0;
 129
 130         p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT +
 131                   attr - OPTEE_MSG_ATTR_TYPE_FMEM_INPUT;
 132         p->u.memref.size = mp->u.fmem.size;
 133
 134         if (mp->u.fmem.global_id != OPTEE_MSG_FMEM_INVALID_GLOBAL_ID)
 135                 shm = optee_shm_from_ffa_handle(optee, mp->u.fmem.global_id);
 136         p->u.memref.shm = shm;
 137
 138         if (shm) {
 139                 offs_low = mp->u.fmem.offs_low;
 140                 offs_high = mp->u.fmem.offs_high;
 141         }
 142         p->u.memref.shm_offs = offs_low | offs_high << 32;
 143 }
 144
 145 /**
 146  * optee_ffa_from_msg_param() - convert from OPTEE_MSG parameters to
 147  *                              struct tee_param
 148  * @optee:      main service struct
 149  * @params:     subsystem internal parameter representation
 150  * @num_params: number of elements in the parameter arrays
 151  * @msg_params: OPTEE_MSG parameters
 152  *
 153  * Returns 0 on success or <0 on failure
 154  */
 155 static int optee_ffa_from_msg_param(struct optee *optee,
 156                                     struct tee_param *params, size_t num_params,
 157                                     const struct optee_msg_param *msg_params)
 158 {
 159         size_t n;
 160
 161         for (n = 0; n < num_params; n++) {
 162                 struct tee_param *p = params + n;
 163                 const struct optee_msg_param *mp = msg_params + n;
 164                 u32 attr = mp->attr & OPTEE_MSG_ATTR_TYPE_MASK;
 165
 166                 switch (attr) {
 167                 case OPTEE_MSG_ATTR_TYPE_NONE:
 168                         p->attr = TEE_IOCTL_PARAM_ATTR_TYPE_NONE;
 169                         memset(&p->u, 0, sizeof(p->u));
 170                         break;
 171                 case OPTEE_MSG_ATTR_TYPE_VALUE_INPUT:
 172                 case OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT:
 173                 case OPTEE_MSG_ATTR_TYPE_VALUE_INOUT:
 174                         optee_from_msg_param_value(p, attr, mp);
 175                         break;
 176                 case OPTEE_MSG_ATTR_TYPE_FMEM_INPUT:
 177                 case OPTEE_MSG_ATTR_TYPE_FMEM_OUTPUT:
 178                 case OPTEE_MSG_ATTR_TYPE_FMEM_INOUT:
 179                         from_msg_param_ffa_mem(optee, p, attr, mp);
 180                         break;
 181                 default:
 182                         return -EINVAL;
 183                 }
 184         }
 185
 186         return 0;
 187 }
 188
 189 static int to_msg_param_ffa_mem(struct optee_msg_param *mp,
 190                                 const struct tee_param *p)
 191 {
 192         struct tee_shm *shm = p->u.memref.shm;
 193
 194         mp->attr = OPTEE_MSG_ATTR_TYPE_FMEM_INPUT + p->attr -
 195                    TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT;
 196
 197         if (shm) {
 198                 u64 shm_offs = p->u.memref.shm_offs;
 199
 200                 mp->u.fmem.internal_offs = shm->offset;
 201
 202                 mp->u.fmem.offs_low = shm_offs;
 203                 mp->u.fmem.offs_high = shm_offs >> 32;
 204                 /* Check that the entire offset could be stored. */
 205                 if (mp->u.fmem.offs_high != shm_offs >> 32)
 206                         return -EINVAL;
 207
 208                 mp->u.fmem.global_id = shm->sec_world_id;
 209         } else {
 210                 memset(&mp->u, 0, sizeof(mp->u));
 211                 mp->u.fmem.global_id = OPTEE_MSG_FMEM_INVALID_GLOBAL_ID;
 212         }
 213         mp->u.fmem.size = p->u.memref.size;
 214
 215         return 0;
 216 }
 217
 218 /**
 219  * optee_ffa_to_msg_param() - convert from struct tee_params to OPTEE_MSG
 220  *                            parameters
 221  * @optee:      main service struct
 222  * @msg_params: OPTEE_MSG parameters
 223  * @num_params: number of elements in the parameter arrays
 224  * @params:     subsystem itnernal parameter representation
 225  * Returns 0 on success or <0 on failure
 226  */
 227 static int optee_ffa_to_msg_param(struct optee *optee,
 228                                   struct optee_msg_param *msg_params,
 229                                   size_t num_params,
 230                                   const struct tee_param *params)
 231 {
 232         size_t n;
 233
 234         for (n = 0; n < num_params; n++) {
 235                 const struct tee_param *p = params + n;
 236                 struct optee_msg_param *mp = msg_params + n;
 237
 238                 switch (p->attr) {
 239                 case TEE_IOCTL_PARAM_ATTR_TYPE_NONE:
 240                         mp->attr = TEE_IOCTL_PARAM_ATTR_TYPE_NONE;
 241                         memset(&mp->u, 0, sizeof(mp->u));
 242                         break;
 243                 case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT:
 244                 case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_OUTPUT:
 245                 case TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT:
 246                         optee_to_msg_param_value(mp, p);
 247                         break;
 248                 case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INPUT:
 249                 case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_OUTPUT:
 250                 case TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INOUT:
 251                         if (to_msg_param_ffa_mem(mp, p))
 252                                 return -EINVAL;
 253                         break;
 254                 default:
 255                         return -EINVAL;
 256                 }
 257         }
 258
 259         return 0;
 260 }
 261
 262 /*
 263  * 3. Low level support functions to register shared memory in secure world
 264  *
 265  * Functions to register and unregister shared memory both for normal
 266  * clients and for tee-supplicant.
 267  */
 268
 269 static int optee_ffa_shm_register(struct tee_context *ctx, struct tee_shm *shm,
 270                                   struct page **pages, size_t num_pages,
 271                                   unsigned long start)
 272 {
 273         struct optee *optee = tee_get_drvdata(ctx->teedev);
 274         struct ffa_device *ffa_dev = optee->ffa.ffa_dev;
 275         const struct ffa_mem_ops *mem_ops = ffa_dev->ops->mem_ops;
 276         struct ffa_mem_region_attributes mem_attr = {
 277                 .receiver = ffa_dev->vm_id,
 278                 .attrs = FFA_MEM_RW,
 279         };
 280         struct ffa_mem_ops_args args = {
 281                 .use_txbuf = true,
 282                 .attrs = &mem_attr,
 283                 .nattrs = 1,
 284         };
 285         struct sg_table sgt;
 286         int rc;
 287
 288         rc = optee_check_mem_type(start, num_pages);
 289         if (rc)
 290                 return rc;
 291
 292         rc = sg_alloc_table_from_pages(&sgt, pages, num_pages, 0,
 293                                        num_pages * PAGE_SIZE, GFP_KERNEL);
 294         if (rc)
 295                 return rc;
 296         args.sg = sgt.sgl;
 297         rc = mem_ops->memory_share(&args);
 298         sg_free_table(&sgt);
 299         if (rc)
 300                 return rc;
 301
 302         rc = optee_shm_add_ffa_handle(optee, shm, args.g_handle);
 303         if (rc) {
 304                 mem_ops->memory_reclaim(args.g_handle, 0);
 305                 return rc;
 306         }
 307
 308         shm->sec_world_id = args.g_handle;
 309
 310         return 0;
 311 }
 312
 313 static int optee_ffa_shm_unregister(struct tee_context *ctx,
 314                                     struct tee_shm *shm)
 315 {
 316         struct optee *optee = tee_get_drvdata(ctx->teedev);
 317         struct ffa_device *ffa_dev = optee->ffa.ffa_dev;
 318         const struct ffa_msg_ops *msg_ops = ffa_dev->ops->msg_ops;
 319         const struct ffa_mem_ops *mem_ops = ffa_dev->ops->mem_ops;
 320         u64 global_handle = shm->sec_world_id;
 321         struct ffa_send_direct_data data = {
 322                 .data0 = OPTEE_FFA_UNREGISTER_SHM,
 323                 .data1 = (u32)global_handle,
 324                 .data2 = (u32)(global_handle >> 32)
 325         };
 326         int rc;
 327
 328         optee_shm_rem_ffa_handle(optee, global_handle);
 329         shm->sec_world_id = 0;
 330
 331         rc = msg_ops->sync_send_receive(ffa_dev, &data);
 332         if (rc)
 333                 pr_err("Unregister SHM id 0x%llx rc %d\n", global_handle, rc);
 334
 335         rc = mem_ops->memory_reclaim(global_handle, 0);
 336         if (rc)
 337                 pr_err("mem_reclaim: 0x%llx %d", global_handle, rc);
 338
 339         return rc;
 340 }
 341
 342 static int optee_ffa_shm_unregister_supp(struct tee_context *ctx,
 343                                          struct tee_shm *shm)
 344 {
 345         struct optee *optee = tee_get_drvdata(ctx->teedev);
 346         const struct ffa_mem_ops *mem_ops;
 347         u64 global_handle = shm->sec_world_id;
 348         int rc;
 349
 350         /*
 351          * We're skipping the OPTEE_FFA_YIELDING_CALL_UNREGISTER_SHM call
 352          * since this is OP-TEE freeing via RPC so it has already retired
 353          * this ID.
 354          */
 355
 356         optee_shm_rem_ffa_handle(optee, global_handle);
 357         mem_ops = optee->ffa.ffa_dev->ops->mem_ops;
 358         rc = mem_ops->memory_reclaim(global_handle, 0);
 359         if (rc)
 360                 pr_err("mem_reclaim: 0x%llx %d", global_handle, rc);
 361
 362         shm->sec_world_id = 0;
 363
 364         return rc;
 365 }
 366
 367 /*
 368  * 4. Dynamic shared memory pool based on alloc_pages()
 369  *
 370  * Implements an OP-TEE specific shared memory pool.
 371  * The main function is optee_ffa_shm_pool_alloc_pages().
 372  */
 373
 374 static int pool_ffa_op_alloc(struct tee_shm_pool *pool,
 375                              struct tee_shm *shm, size_t size, size_t align)
 376 {
 377         return optee_pool_op_alloc_helper(pool, shm, size, align,
 378                                           optee_ffa_shm_register);
 379 }
 380
 381 static void pool_ffa_op_free(struct tee_shm_pool *pool,
 382                              struct tee_shm *shm)
 383 {
 384         optee_pool_op_free_helper(pool, shm, optee_ffa_shm_unregister);
 385 }
 386
 387 static void pool_ffa_op_destroy_pool(struct tee_shm_pool *pool)
 388 {
 389         kfree(pool);
 390 }
 391
 392 static const struct tee_shm_pool_ops pool_ffa_ops = {
 393         .alloc = pool_ffa_op_alloc,
 394         .free = pool_ffa_op_free,
 395         .destroy_pool = pool_ffa_op_destroy_pool,
 396 };
 397
 398 /**
 399  * optee_ffa_shm_pool_alloc_pages() - create page-based allocator pool
 400  *
 401  * This pool is used with OP-TEE over FF-A. In this case command buffers
 402  * and such are allocated from kernel's own memory.
 403  */
 404 static struct tee_shm_pool *optee_ffa_shm_pool_alloc_pages(void)
 405 {
 406         struct tee_shm_pool *pool = kzalloc(sizeof(*pool), GFP_KERNEL);
 407
 408         if (!pool)
 409                 return ERR_PTR(-ENOMEM);
 410
 411         pool->ops = &pool_ffa_ops;
 412
 413         return pool;
 414 }
 415
 416 /*
 417  * 5. Do a normal scheduled call into secure world
 418  *
 419  * The function optee_ffa_do_call_with_arg() performs a normal scheduled
 420  * call into secure world. During this call may normal world request help
 421  * from normal world using RPCs, Remote Procedure Calls. This includes
 422  * delivery of non-secure interrupts to for instance allow rescheduling of
 423  * the current task.
 424  */
 425
 426 static void handle_ffa_rpc_func_cmd_shm_alloc(struct tee_context *ctx,
 427                                               struct optee *optee,
 428                                               struct optee_msg_arg *arg)
 429 {
 430         struct tee_shm *shm;
 431
 432         if (arg->num_params != 1 ||
 433             arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT) {
 434                 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
 435                 return;
 436         }
 437
 438         switch (arg->params[0].u.value.a) {
 439         case OPTEE_RPC_SHM_TYPE_APPL:
 440                 shm = optee_rpc_cmd_alloc_suppl(ctx, arg->params[0].u.value.b);
 441                 break;
 442         case OPTEE_RPC_SHM_TYPE_KERNEL:
 443                 shm = tee_shm_alloc_priv_buf(optee->ctx,
 444                                              arg->params[0].u.value.b);
 445                 break;
 446         default:
 447                 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
 448                 return;
 449         }
 450
 451         if (IS_ERR(shm)) {
 452                 arg->ret = TEEC_ERROR_OUT_OF_MEMORY;
 453                 return;
 454         }
 455
 456         arg->params[0] = (struct optee_msg_param){
 457                 .attr = OPTEE_MSG_ATTR_TYPE_FMEM_OUTPUT,
 458                 .u.fmem.size = tee_shm_get_size(shm),
 459                 .u.fmem.global_id = shm->sec_world_id,
 460                 .u.fmem.internal_offs = shm->offset,
 461         };
 462
 463         arg->ret = TEEC_SUCCESS;
 464 }
 465
 466 static void handle_ffa_rpc_func_cmd_shm_free(struct tee_context *ctx,
 467                                              struct optee *optee,
 468                                              struct optee_msg_arg *arg)
 469 {
 470         struct tee_shm *shm;
 471
 472         if (arg->num_params != 1 ||
 473             arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT)
 474                 goto err_bad_param;
 475
 476         shm = optee_shm_from_ffa_handle(optee, arg->params[0].u.value.b);
 477         if (!shm)
 478                 goto err_bad_param;
 479         switch (arg->params[0].u.value.a) {
 480         case OPTEE_RPC_SHM_TYPE_APPL:
 481                 optee_rpc_cmd_free_suppl(ctx, shm);
 482                 break;
 483         case OPTEE_RPC_SHM_TYPE_KERNEL:
 484                 tee_shm_free(shm);
 485                 break;
 486         default:
 487                 goto err_bad_param;
 488         }
 489         arg->ret = TEEC_SUCCESS;
 490         return;
 491
 492 err_bad_param:
 493         arg->ret = TEEC_ERROR_BAD_PARAMETERS;
 494 }
 495
 496 static void handle_ffa_rpc_func_cmd(struct tee_context *ctx,
 497                                     struct optee *optee,
 498                                     struct optee_msg_arg *arg)
 499 {
 500         arg->ret_origin = TEEC_ORIGIN_COMMS;
 501         switch (arg->cmd) {
 502         case OPTEE_RPC_CMD_SHM_ALLOC:
 503                 handle_ffa_rpc_func_cmd_shm_alloc(ctx, optee, arg);
 504                 break;
 505         case OPTEE_RPC_CMD_SHM_FREE:
 506                 handle_ffa_rpc_func_cmd_shm_free(ctx, optee, arg);
 507                 break;
 508         default:
 509                 optee_rpc_cmd(ctx, optee, arg);
 510         }
 511 }
 512
 513 static void optee_handle_ffa_rpc(struct tee_context *ctx, struct optee *optee,
 514                                  u32 cmd, struct optee_msg_arg *arg)
 515 {
 516         switch (cmd) {
 517         case OPTEE_FFA_YIELDING_CALL_RETURN_RPC_CMD:
 518                 handle_ffa_rpc_func_cmd(ctx, optee, arg);
 519                 break;
 520         case OPTEE_FFA_YIELDING_CALL_RETURN_INTERRUPT:
 521                 /* Interrupt delivered by now */
 522                 break;
 523         default:
 524                 pr_warn("Unknown RPC func 0x%x\n", cmd);
 525                 break;
 526         }
 527 }
 528
 529 static int optee_ffa_yielding_call(struct tee_context *ctx,
 530                                    struct ffa_send_direct_data *data,
 531                                    struct optee_msg_arg *rpc_arg,
 532                                    bool system_thread)
 533 {
 534         struct optee *optee = tee_get_drvdata(ctx->teedev);
 535         struct ffa_device *ffa_dev = optee->ffa.ffa_dev;
 536         const struct ffa_msg_ops *msg_ops = ffa_dev->ops->msg_ops;
 537         struct optee_call_waiter w;
 538         u32 cmd = data->data0;
 539         u32 w4 = data->data1;
 540         u32 w5 = data->data2;
 541         u32 w6 = data->data3;
 542         int rc;
 543
 544         /* Initialize waiter */
 545         optee_cq_wait_init(&optee->call_queue, &w, system_thread);
 546         while (true) {
 547                 rc = msg_ops->sync_send_receive(ffa_dev, data);
 548                 if (rc)
 549                         goto done;
 550
 551                 switch ((int)data->data0) {
 552                 case TEEC_SUCCESS:
 553                         break;
 554                 case TEEC_ERROR_BUSY:
 555                         if (cmd == OPTEE_FFA_YIELDING_CALL_RESUME) {
 556                                 rc = -EIO;
 557                                 goto done;
 558                         }
 559
 560                         /*
 561                          * Out of threads in secure world, wait for a thread
 562                          * become available.
 563                          */
 564                         optee_cq_wait_for_completion(&optee->call_queue, &w);
 565                         data->data0 = cmd;
 566                         data->data1 = w4;
 567                         data->data2 = w5;
 568                         data->data3 = w6;
 569                         continue;
 570                 default:
 571                         rc = -EIO;
 572                         goto done;
 573                 }
 574
 575                 if (data->data1 == OPTEE_FFA_YIELDING_CALL_RETURN_DONE)
 576                         goto done;
 577
 578                 /*
 579                  * OP-TEE has returned with a RPC request.
 580                  *
 581                  * Note that data->data4 (passed in register w7) is already
 582                  * filled in by ffa_mem_ops->sync_send_receive() returning
 583                  * above.
 584                  */
 585                 cond_resched();
 586                 optee_handle_ffa_rpc(ctx, optee, data->data1, rpc_arg);
 587                 cmd = OPTEE_FFA_YIELDING_CALL_RESUME;
 588                 data->data0 = cmd;
 589                 data->data1 = 0;
 590                 data->data2 = 0;
 591                 data->data3 = 0;
 592         }
 593 done:
 594         /*
 595          * We're done with our thread in secure world, if there's any
 596          * thread waiters wake up one.
 597          */
 598         optee_cq_wait_final(&optee->call_queue, &w);
 599
 600         return rc;
 601 }
 602
 603 /**
 604  * optee_ffa_do_call_with_arg() - Do a FF-A call to enter OP-TEE in secure world
 605  * @ctx:        calling context
 606  * @shm:        shared memory holding the message to pass to secure world
 607  * @offs:       offset of the message in @shm
 608  * @system_thread: true if caller requests TEE system thread support
 609  *
 610  * Does a FF-A call to OP-TEE in secure world and handles eventual resulting
 611  * Remote Procedure Calls (RPC) from OP-TEE.
 612  *
 613  * Returns return code from FF-A, 0 is OK
 614  */
 615
 616 static int optee_ffa_do_call_with_arg(struct tee_context *ctx,
 617                                       struct tee_shm *shm, u_int offs,
 618                                       bool system_thread)
 619 {
 620         struct ffa_send_direct_data data = {
 621                 .data0 = OPTEE_FFA_YIELDING_CALL_WITH_ARG,
 622                 .data1 = (u32)shm->sec_world_id,
 623                 .data2 = (u32)(shm->sec_world_id >> 32),
 624                 .data3 = offs,
 625         };
 626         struct optee_msg_arg *arg;
 627         unsigned int rpc_arg_offs;
 628         struct optee_msg_arg *rpc_arg;
 629
 630         /*
 631          * The shared memory object has to start on a page when passed as
 632          * an argument struct. This is also what the shm pool allocator
 633          * returns, but check this before calling secure world to catch
 634          * eventual errors early in case something changes.
 635          */
 636         if (shm->offset)
 637                 return -EINVAL;
 638
 639         arg = tee_shm_get_va(shm, offs);
 640         if (IS_ERR(arg))
 641                 return PTR_ERR(arg);
 642
 643         rpc_arg_offs = OPTEE_MSG_GET_ARG_SIZE(arg->num_params);
 644         rpc_arg = tee_shm_get_va(shm, offs + rpc_arg_offs);
 645         if (IS_ERR(rpc_arg))
 646                 return PTR_ERR(rpc_arg);
 647
 648         return optee_ffa_yielding_call(ctx, &data, rpc_arg, system_thread);
 649 }
 650
 651 /*
 652  * 6. Driver initialization
 653  *
 654  * During driver inititialization is the OP-TEE Secure Partition is probed
 655  * to find out which features it supports so the driver can be initialized
 656  * with a matching configuration.
 657  */
 658
 659 static bool optee_ffa_api_is_compatbile(struct ffa_device *ffa_dev,
 660                                         const struct ffa_ops *ops)
 661 {
 662         const struct ffa_msg_ops *msg_ops = ops->msg_ops;
 663         struct ffa_send_direct_data data = { OPTEE_FFA_GET_API_VERSION };
 664         int rc;
 665
 666         msg_ops->mode_32bit_set(ffa_dev);
 667
 668         rc = msg_ops->sync_send_receive(ffa_dev, &data);
 669         if (rc) {
 670                 pr_err("Unexpected error %d\n", rc);
 671                 return false;
 672         }
 673         if (data.data0 != OPTEE_FFA_VERSION_MAJOR ||
 674             data.data1 < OPTEE_FFA_VERSION_MINOR) {
 675                 pr_err("Incompatible OP-TEE API version %lu.%lu",
 676                        data.data0, data.data1);
 677                 return false;
 678         }
 679
 680         data = (struct ffa_send_direct_data){ OPTEE_FFA_GET_OS_VERSION };
 681         rc = msg_ops->sync_send_receive(ffa_dev, &data);
 682         if (rc) {
 683                 pr_err("Unexpected error %d\n", rc);
 684                 return false;
 685         }
 686         if (data.data2)
 687                 pr_info("revision %lu.%lu (%08lx)",
 688                         data.data0, data.data1, data.data2);
 689         else
 690                 pr_info("revision %lu.%lu", data.data0, data.data1);
 691
 692         return true;
 693 }
 694
 695 static bool optee_ffa_exchange_caps(struct ffa_device *ffa_dev,
 696                                     const struct ffa_ops *ops,
 697                                     u32 *sec_caps,
 698                                     unsigned int *rpc_param_count,
 699                                     unsigned int *max_notif_value)
 700 {
 701         struct ffa_send_direct_data data = { OPTEE_FFA_EXCHANGE_CAPABILITIES };
 702         int rc;
 703
 704         rc = ops->msg_ops->sync_send_receive(ffa_dev, &data);
 705         if (rc) {
 706                 pr_err("Unexpected error %d", rc);
 707                 return false;
 708         }
 709         if (data.data0) {
 710                 pr_err("Unexpected exchange error %lu", data.data0);
 711                 return false;
 712         }
 713
 714         *rpc_param_count = (u8)data.data1;
 715         *sec_caps = data.data2;
 716         if (data.data3)
 717                 *max_notif_value = data.data3;
 718         else
 719                 *max_notif_value = OPTEE_DEFAULT_MAX_NOTIF_VALUE;
 720
 721         return true;
 722 }
 723
 724 static void notif_callback(int notify_id, void *cb_data)
 725 {
 726         struct optee *optee = cb_data;
 727
 728         if (notify_id == optee->ffa.bottom_half_value)
 729                 optee_do_bottom_half(optee->ctx);
 730         else
 731                 optee_notif_send(optee, notify_id);
 732 }
 733
 734 static int enable_async_notif(struct optee *optee)
 735 {
 736         struct ffa_device *ffa_dev = optee->ffa.ffa_dev;
 737         struct ffa_send_direct_data data = {
 738                 .data0 = OPTEE_FFA_ENABLE_ASYNC_NOTIF,
 739                 .data1 = optee->ffa.bottom_half_value,
 740         };
 741         int rc;
 742
 743         rc = ffa_dev->ops->msg_ops->sync_send_receive(ffa_dev, &data);
 744         if (rc)
 745                 return rc;
 746         return data.data0;
 747 }
 748
 749 static void optee_ffa_get_version(struct tee_device *teedev,
 750                                   struct tee_ioctl_version_data *vers)
 751 {
 752         struct tee_ioctl_version_data v = {
 753                 .impl_id = TEE_IMPL_ID_OPTEE,
 754                 .impl_caps = TEE_OPTEE_CAP_TZ,
 755                 .gen_caps = TEE_GEN_CAP_GP | TEE_GEN_CAP_REG_MEM |
 756                             TEE_GEN_CAP_MEMREF_NULL,
 757         };
 758
 759         *vers = v;
 760 }
 761
 762 static int optee_ffa_open(struct tee_context *ctx)
 763 {
 764         return optee_open(ctx, true);
 765 }
 766
 767 static const struct tee_driver_ops optee_ffa_clnt_ops = {
 768         .get_version = optee_ffa_get_version,
 769         .open = optee_ffa_open,
 770         .release = optee_release,
 771         .open_session = optee_open_session,
 772         .close_session = optee_close_session,
 773         .invoke_func = optee_invoke_func,
 774         .cancel_req = optee_cancel_req,
 775         .shm_register = optee_ffa_shm_register,
 776         .shm_unregister = optee_ffa_shm_unregister,
 777 };
 778
 779 static const struct tee_desc optee_ffa_clnt_desc = {
 780         .name = DRIVER_NAME "-ffa-clnt",
 781         .ops = &optee_ffa_clnt_ops,
 782         .owner = THIS_MODULE,
 783 };
 784
 785 static const struct tee_driver_ops optee_ffa_supp_ops = {
 786         .get_version = optee_ffa_get_version,
 787         .open = optee_ffa_open,
 788         .release = optee_release_supp,
 789         .supp_recv = optee_supp_recv,
 790         .supp_send = optee_supp_send,
 791         .shm_register = optee_ffa_shm_register, /* same as for clnt ops */
 792         .shm_unregister = optee_ffa_shm_unregister_supp,
 793 };
 794
 795 static const struct tee_desc optee_ffa_supp_desc = {
 796         .name = DRIVER_NAME "-ffa-supp",
 797         .ops = &optee_ffa_supp_ops,
 798         .owner = THIS_MODULE,
 799         .flags = TEE_DESC_PRIVILEGED,
 800 };
 801
 802 static const struct optee_ops optee_ffa_ops = {
 803         .do_call_with_arg = optee_ffa_do_call_with_arg,
 804         .to_msg_param = optee_ffa_to_msg_param,
 805         .from_msg_param = optee_ffa_from_msg_param,
 806 };
 807
 808 static void optee_ffa_remove(struct ffa_device *ffa_dev)
 809 {
 810         struct optee *optee = ffa_dev_get_drvdata(ffa_dev);
 811         u32 bottom_half_id = optee->ffa.bottom_half_value;
 812
 813         if (bottom_half_id != U32_MAX)
 814                 ffa_dev->ops->notifier_ops->notify_relinquish(ffa_dev,
 815                                                               bottom_half_id);
 816         optee_remove_common(optee);
 817
 818         mutex_destroy(&optee->ffa.mutex);
 819         rhashtable_free_and_destroy(&optee->ffa.global_ids, rh_free_fn, NULL);
 820
 821         kfree(optee);
 822 }
 823
 824 static int optee_ffa_async_notif_init(struct ffa_device *ffa_dev,
 825                                       struct optee *optee)
 826 {
 827         bool is_per_vcpu = false;
 828         u32 notif_id = 0;
 829         int rc;
 830
 831         while (true) {
 832                 rc = ffa_dev->ops->notifier_ops->notify_request(ffa_dev,
 833                                                                 is_per_vcpu,
 834                                                                 notif_callback,
 835                                                                 optee,
 836                                                                 notif_id);
 837                 if (!rc)
 838                         break;
 839                 /*
 840                  * -EACCES means that the notification ID was
 841                  * already bound, try the next one as long as we
 842                  * haven't reached the max. Any other error is a
 843                  * permanent error, so skip asynchronous
 844                  * notifications in that case.
 845                  */
 846                 if (rc != -EACCES)
 847                         return rc;
 848                 notif_id++;
 849                 if (notif_id >= OPTEE_FFA_MAX_ASYNC_NOTIF_VALUE)
 850                         return rc;
 851         }
 852         optee->ffa.bottom_half_value = notif_id;
 853
 854         rc = enable_async_notif(optee);
 855         if (rc < 0) {
 856                 ffa_dev->ops->notifier_ops->notify_relinquish(ffa_dev,
 857                                                               notif_id);
 858                 optee->ffa.bottom_half_value = U32_MAX;
 859         }
 860
 861         return rc;
 862 }
 863
 864 static int optee_ffa_probe(struct ffa_device *ffa_dev)
 865 {
 866         const struct ffa_notifier_ops *notif_ops;
 867         const struct ffa_ops *ffa_ops;
 868         unsigned int max_notif_value;
 869         unsigned int rpc_param_count;
 870         struct tee_shm_pool *pool;
 871         struct tee_device *teedev;
 872         struct tee_context *ctx;
 873         u32 arg_cache_flags = 0;
 874         struct optee *optee;
 875         u32 sec_caps;
 876         int rc;
 877
 878         ffa_ops = ffa_dev->ops;
 879         notif_ops = ffa_ops->notifier_ops;
 880
 881         if (!optee_ffa_api_is_compatbile(ffa_dev, ffa_ops))
 882                 return -EINVAL;
 883
 884         if (!optee_ffa_exchange_caps(ffa_dev, ffa_ops, &sec_caps,
 885                                      &rpc_param_count, &max_notif_value))
 886                 return -EINVAL;
 887         if (sec_caps & OPTEE_FFA_SEC_CAP_ARG_OFFSET)
 888                 arg_cache_flags |= OPTEE_SHM_ARG_SHARED;
 889
 890         optee = kzalloc(sizeof(*optee), GFP_KERNEL);
 891         if (!optee)
 892                 return -ENOMEM;
 893
 894         pool = optee_ffa_shm_pool_alloc_pages();
 895         if (IS_ERR(pool)) {
 896                 rc = PTR_ERR(pool);
 897                 goto err_free_optee;
 898         }
 899         optee->pool = pool;
 900
 901         optee->ops = &optee_ffa_ops;
 902         optee->ffa.ffa_dev = ffa_dev;
 903         optee->ffa.bottom_half_value = U32_MAX;
 904         optee->rpc_param_count = rpc_param_count;
 905
 906         teedev = tee_device_alloc(&optee_ffa_clnt_desc, NULL, optee->pool,
 907                                   optee);
 908         if (IS_ERR(teedev)) {
 909                 rc = PTR_ERR(teedev);
 910                 goto err_free_pool;
 911         }
 912         optee->teedev = teedev;
 913
 914         teedev = tee_device_alloc(&optee_ffa_supp_desc, NULL, optee->pool,
 915                                   optee);
 916         if (IS_ERR(teedev)) {
 917                 rc = PTR_ERR(teedev);
 918                 goto err_unreg_teedev;
 919         }
 920         optee->supp_teedev = teedev;
 921
 922         rc = tee_device_register(optee->teedev);
 923         if (rc)
 924                 goto err_unreg_supp_teedev;
 925
 926         rc = tee_device_register(optee->supp_teedev);
 927         if (rc)
 928                 goto err_unreg_supp_teedev;
 929
 930         rc = rhashtable_init(&optee->ffa.global_ids, &shm_rhash_params);
 931         if (rc)
 932                 goto err_unreg_supp_teedev;
 933         mutex_init(&optee->ffa.mutex);
 934         optee_cq_init(&optee->call_queue, 0);
 935         optee_supp_init(&optee->supp);
 936         optee_shm_arg_cache_init(optee, arg_cache_flags);
 937         ffa_dev_set_drvdata(ffa_dev, optee);
 938         ctx = teedev_open(optee->teedev);
 939         if (IS_ERR(ctx)) {
 940                 rc = PTR_ERR(ctx);
 941                 goto err_rhashtable_free;
 942         }
 943         optee->ctx = ctx;
 944         rc = optee_notif_init(optee, OPTEE_DEFAULT_MAX_NOTIF_VALUE);
 945         if (rc)
 946                 goto err_close_ctx;
 947         if (sec_caps & OPTEE_FFA_SEC_CAP_ASYNC_NOTIF) {
 948                 rc = optee_ffa_async_notif_init(ffa_dev, optee);
 949                 if (rc < 0)
 950                         pr_err("Failed to initialize async notifications: %d",
 951                                rc);
 952         }
 953
 954         rc = optee_enumerate_devices(PTA_CMD_GET_DEVICES);
 955         if (rc)
 956                 goto err_unregister_devices;
 957
 958         pr_info("initialized driver\n");
 959         return 0;
 960
 961 err_unregister_devices:
 962         optee_unregister_devices();
 963         if (optee->ffa.bottom_half_value != U32_MAX)
 964                 notif_ops->notify_relinquish(ffa_dev,
 965                                              optee->ffa.bottom_half_value);
 966         optee_notif_uninit(optee);
 967 err_close_ctx:
 968         teedev_close_context(ctx);
 969 err_rhashtable_free:
 970         rhashtable_free_and_destroy(&optee->ffa.global_ids, rh_free_fn, NULL);
 971         optee_supp_uninit(&optee->supp);
 972         mutex_destroy(&optee->call_queue.mutex);
 973         mutex_destroy(&optee->ffa.mutex);
 974 err_unreg_supp_teedev:
 975         tee_device_unregister(optee->supp_teedev);
 976 err_unreg_teedev:
 977         tee_device_unregister(optee->teedev);
 978 err_free_pool:
 979         tee_shm_pool_free(pool);
 980 err_free_optee:
 981         kfree(optee);
 982         return rc;
 983 }
 984
 985 static const struct ffa_device_id optee_ffa_device_id[] = {
 986         /* 486178e0-e7f8-11e3-bc5e0002a5d5c51b */
 987         { UUID_INIT(0x486178e0, 0xe7f8, 0x11e3,
 988                     0xbc, 0x5e, 0x00, 0x02, 0xa5, 0xd5, 0xc5, 0x1b) },
 989         {}
 990 };
 991
 992 static struct ffa_driver optee_ffa_driver = {
 993         .name = "optee",
 994         .probe = optee_ffa_probe,
 995         .remove = optee_ffa_remove,
 996         .id_table = optee_ffa_device_id,
 997 };
 998
 999 int optee_ffa_abi_register(void)
1000 {
1001         if (IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT))
1002                 return ffa_register(&optee_ffa_driver);
1003         else
1004                 return -EOPNOTSUPP;
1005 }
1006
1007 void optee_ffa_abi_unregister(void)
1008 {
1009         if (IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT))
1010                 ffa_unregister(&optee_ffa_driver);
1011 }