drivers/tee/optee/ffa_abi.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (c) 2021, 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         const struct ffa_dev_ops *ffa_ops = optee->ffa.ffa_ops;
 275         struct ffa_device *ffa_dev = optee->ffa.ffa_dev;
 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 = ffa_ops->memory_share(ffa_dev, &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                 ffa_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         const struct ffa_dev_ops *ffa_ops = optee->ffa.ffa_ops;
 318         struct ffa_device *ffa_dev = optee->ffa.ffa_dev;
 319         u64 global_handle = shm->sec_world_id;
 320         struct ffa_send_direct_data data = {
 321                 .data0 = OPTEE_FFA_UNREGISTER_SHM,
 322                 .data1 = (u32)global_handle,
 323                 .data2 = (u32)(global_handle >> 32)
 324         };
 325         int rc;
 326
 327         optee_shm_rem_ffa_handle(optee, global_handle);
 328         shm->sec_world_id = 0;
 329
 330         rc = ffa_ops->sync_send_receive(ffa_dev, &data);
 331         if (rc)
 332                 pr_err("Unregister SHM id 0x%llx rc %d\n", global_handle, rc);
 333
 334         rc = ffa_ops->memory_reclaim(global_handle, 0);
 335         if (rc)
 336                 pr_err("mem_reclaim: 0x%llx %d", global_handle, rc);
 337
 338         return rc;
 339 }
 340
 341 static int optee_ffa_shm_unregister_supp(struct tee_context *ctx,
 342                                          struct tee_shm *shm)
 343 {
 344         struct optee *optee = tee_get_drvdata(ctx->teedev);
 345         const struct ffa_dev_ops *ffa_ops = optee->ffa.ffa_ops;
 346         u64 global_handle = shm->sec_world_id;
 347         int rc;
 348
 349         /*
 350          * We're skipping the OPTEE_FFA_YIELDING_CALL_UNREGISTER_SHM call
 351          * since this is OP-TEE freeing via RPC so it has already retired
 352          * this ID.
 353          */
 354
 355         optee_shm_rem_ffa_handle(optee, global_handle);
 356         rc = ffa_ops->memory_reclaim(global_handle, 0);
 357         if (rc)
 358                 pr_err("mem_reclaim: 0x%llx %d", global_handle, rc);
 359
 360         shm->sec_world_id = 0;
 361
 362         return rc;
 363 }
 364
 365 /*
 366  * 4. Dynamic shared memory pool based on alloc_pages()
 367  *
 368  * Implements an OP-TEE specific shared memory pool.
 369  * The main function is optee_ffa_shm_pool_alloc_pages().
 370  */
 371
 372 static int pool_ffa_op_alloc(struct tee_shm_pool_mgr *poolm,
 373                              struct tee_shm *shm, size_t size)
 374 {
 375         return optee_pool_op_alloc_helper(poolm, shm, size,
 376                                           optee_ffa_shm_register);
 377 }
 378
 379 static void pool_ffa_op_free(struct tee_shm_pool_mgr *poolm,
 380                              struct tee_shm *shm)
 381 {
 382         optee_ffa_shm_unregister(shm->ctx, shm);
 383         free_pages((unsigned long)shm->kaddr, get_order(shm->size));
 384         shm->kaddr = NULL;
 385 }
 386
 387 static void pool_ffa_op_destroy_poolmgr(struct tee_shm_pool_mgr *poolm)
 388 {
 389         kfree(poolm);
 390 }
 391
 392 static const struct tee_shm_pool_mgr_ops pool_ffa_ops = {
 393         .alloc = pool_ffa_op_alloc,
 394         .free = pool_ffa_op_free,
 395         .destroy_poolmgr = pool_ffa_op_destroy_poolmgr,
 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_mgr *optee_ffa_shm_pool_alloc_pages(void)
 405 {
 406         struct tee_shm_pool_mgr *mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
 407
 408         if (!mgr)
 409                 return ERR_PTR(-ENOMEM);
 410
 411         mgr->ops = &pool_ffa_ops;
 412
 413         return mgr;
 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(optee->ctx, arg->params[0].u.value.b,
 444                                     TEE_SHM_MAPPED | TEE_SHM_PRIV);
 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 {
 533         struct optee *optee = tee_get_drvdata(ctx->teedev);
 534         const struct ffa_dev_ops *ffa_ops = optee->ffa.ffa_ops;
 535         struct ffa_device *ffa_dev = optee->ffa.ffa_dev;
 536         struct optee_call_waiter w;
 537         u32 cmd = data->data0;
 538         u32 w4 = data->data1;
 539         u32 w5 = data->data2;
 540         u32 w6 = data->data3;
 541         int rc;
 542
 543         /* Initialize waiter */
 544         optee_cq_wait_init(&optee->call_queue, &w);
 545         while (true) {
 546                 rc = ffa_ops->sync_send_receive(ffa_dev, data);
 547                 if (rc)
 548                         goto done;
 549
 550                 switch ((int)data->data0) {
 551                 case TEEC_SUCCESS:
 552                         break;
 553                 case TEEC_ERROR_BUSY:
 554                         if (cmd == OPTEE_FFA_YIELDING_CALL_RESUME) {
 555                                 rc = -EIO;
 556                                 goto done;
 557                         }
 558
 559                         /*
 560                          * Out of threads in secure world, wait for a thread
 561                          * become available.
 562                          */
 563                         optee_cq_wait_for_completion(&optee->call_queue, &w);
 564                         data->data0 = cmd;
 565                         data->data1 = w4;
 566                         data->data2 = w5;
 567                         data->data3 = w6;
 568                         continue;
 569                 default:
 570                         rc = -EIO;
 571                         goto done;
 572                 }
 573
 574                 if (data->data1 == OPTEE_FFA_YIELDING_CALL_RETURN_DONE)
 575                         goto done;
 576
 577                 /*
 578                  * OP-TEE has returned with a RPC request.
 579                  *
 580                  * Note that data->data4 (passed in register w7) is already
 581                  * filled in by ffa_ops->sync_send_receive() returning
 582                  * above.
 583                  */
 584                 cond_resched();
 585                 optee_handle_ffa_rpc(ctx, optee, data->data1, rpc_arg);
 586                 cmd = OPTEE_FFA_YIELDING_CALL_RESUME;
 587                 data->data0 = cmd;
 588                 data->data1 = 0;
 589                 data->data2 = 0;
 590                 data->data3 = 0;
 591         }
 592 done:
 593         /*
 594          * We're done with our thread in secure world, if there's any
 595          * thread waiters wake up one.
 596          */
 597         optee_cq_wait_final(&optee->call_queue, &w);
 598
 599         return rc;
 600 }
 601
 602 /**
 603  * optee_ffa_do_call_with_arg() - Do a FF-A call to enter OP-TEE in secure world
 604  * @ctx:        calling context
 605  * @shm:        shared memory holding the message to pass to secure world
 606  *
 607  * Does a FF-A call to OP-TEE in secure world and handles eventual resulting
 608  * Remote Procedure Calls (RPC) from OP-TEE.
 609  *
 610  * Returns return code from FF-A, 0 is OK
 611  */
 612
 613 static int optee_ffa_do_call_with_arg(struct tee_context *ctx,
 614                                       struct tee_shm *shm)
 615 {
 616         struct ffa_send_direct_data data = {
 617                 .data0 = OPTEE_FFA_YIELDING_CALL_WITH_ARG,
 618                 .data1 = (u32)shm->sec_world_id,
 619                 .data2 = (u32)(shm->sec_world_id >> 32),
 620                 .data3 = shm->offset,
 621         };
 622         struct optee_msg_arg *arg;
 623         unsigned int rpc_arg_offs;
 624         struct optee_msg_arg *rpc_arg;
 625
 626         arg = tee_shm_get_va(shm, 0);
 627         if (IS_ERR(arg))
 628                 return PTR_ERR(arg);
 629
 630         rpc_arg_offs = OPTEE_MSG_GET_ARG_SIZE(arg->num_params);
 631         rpc_arg = tee_shm_get_va(shm, rpc_arg_offs);
 632         if (IS_ERR(rpc_arg))
 633                 return PTR_ERR(rpc_arg);
 634
 635         return optee_ffa_yielding_call(ctx, &data, rpc_arg);
 636 }
 637
 638 /*
 639  * 6. Driver initialization
 640  *
 641  * During driver inititialization is the OP-TEE Secure Partition is probed
 642  * to find out which features it supports so the driver can be initialized
 643  * with a matching configuration.
 644  */
 645
 646 static bool optee_ffa_api_is_compatbile(struct ffa_device *ffa_dev,
 647                                         const struct ffa_dev_ops *ops)
 648 {
 649         struct ffa_send_direct_data data = { OPTEE_FFA_GET_API_VERSION };
 650         int rc;
 651
 652         ops->mode_32bit_set(ffa_dev);
 653
 654         rc = ops->sync_send_receive(ffa_dev, &data);
 655         if (rc) {
 656                 pr_err("Unexpected error %d\n", rc);
 657                 return false;
 658         }
 659         if (data.data0 != OPTEE_FFA_VERSION_MAJOR ||
 660             data.data1 < OPTEE_FFA_VERSION_MINOR) {
 661                 pr_err("Incompatible OP-TEE API version %lu.%lu",
 662                        data.data0, data.data1);
 663                 return false;
 664         }
 665
 666         data = (struct ffa_send_direct_data){ OPTEE_FFA_GET_OS_VERSION };
 667         rc = ops->sync_send_receive(ffa_dev, &data);
 668         if (rc) {
 669                 pr_err("Unexpected error %d\n", rc);
 670                 return false;
 671         }
 672         if (data.data2)
 673                 pr_info("revision %lu.%lu (%08lx)",
 674                         data.data0, data.data1, data.data2);
 675         else
 676                 pr_info("revision %lu.%lu", data.data0, data.data1);
 677
 678         return true;
 679 }
 680
 681 static bool optee_ffa_exchange_caps(struct ffa_device *ffa_dev,
 682                                     const struct ffa_dev_ops *ops,
 683                                     unsigned int *rpc_arg_count)
 684 {
 685         struct ffa_send_direct_data data = { OPTEE_FFA_EXCHANGE_CAPABILITIES };
 686         int rc;
 687
 688         rc = ops->sync_send_receive(ffa_dev, &data);
 689         if (rc) {
 690                 pr_err("Unexpected error %d", rc);
 691                 return false;
 692         }
 693         if (data.data0) {
 694                 pr_err("Unexpected exchange error %lu", data.data0);
 695                 return false;
 696         }
 697
 698         *rpc_arg_count = (u8)data.data1;
 699
 700         return true;
 701 }
 702
 703 static struct tee_shm_pool *optee_ffa_config_dyn_shm(void)
 704 {
 705         struct tee_shm_pool_mgr *priv_mgr;
 706         struct tee_shm_pool_mgr *dmabuf_mgr;
 707         void *rc;
 708
 709         rc = optee_ffa_shm_pool_alloc_pages();
 710         if (IS_ERR(rc))
 711                 return rc;
 712         priv_mgr = rc;
 713
 714         rc = optee_ffa_shm_pool_alloc_pages();
 715         if (IS_ERR(rc)) {
 716                 tee_shm_pool_mgr_destroy(priv_mgr);
 717                 return rc;
 718         }
 719         dmabuf_mgr = rc;
 720
 721         rc = tee_shm_pool_alloc(priv_mgr, dmabuf_mgr);
 722         if (IS_ERR(rc)) {
 723                 tee_shm_pool_mgr_destroy(priv_mgr);
 724                 tee_shm_pool_mgr_destroy(dmabuf_mgr);
 725         }
 726
 727         return rc;
 728 }
 729
 730 static void optee_ffa_get_version(struct tee_device *teedev,
 731                                   struct tee_ioctl_version_data *vers)
 732 {
 733         struct tee_ioctl_version_data v = {
 734                 .impl_id = TEE_IMPL_ID_OPTEE,
 735                 .impl_caps = TEE_OPTEE_CAP_TZ,
 736                 .gen_caps = TEE_GEN_CAP_GP | TEE_GEN_CAP_REG_MEM |
 737                             TEE_GEN_CAP_MEMREF_NULL,
 738         };
 739
 740         *vers = v;
 741 }
 742
 743 static int optee_ffa_open(struct tee_context *ctx)
 744 {
 745         return optee_open(ctx, true);
 746 }
 747
 748 static const struct tee_driver_ops optee_ffa_clnt_ops = {
 749         .get_version = optee_ffa_get_version,
 750         .open = optee_ffa_open,
 751         .release = optee_release,
 752         .open_session = optee_open_session,
 753         .close_session = optee_close_session,
 754         .invoke_func = optee_invoke_func,
 755         .cancel_req = optee_cancel_req,
 756         .shm_register = optee_ffa_shm_register,
 757         .shm_unregister = optee_ffa_shm_unregister,
 758 };
 759
 760 static const struct tee_desc optee_ffa_clnt_desc = {
 761         .name = DRIVER_NAME "-ffa-clnt",
 762         .ops = &optee_ffa_clnt_ops,
 763         .owner = THIS_MODULE,
 764 };
 765
 766 static const struct tee_driver_ops optee_ffa_supp_ops = {
 767         .get_version = optee_ffa_get_version,
 768         .open = optee_ffa_open,
 769         .release = optee_release_supp,
 770         .supp_recv = optee_supp_recv,
 771         .supp_send = optee_supp_send,
 772         .shm_register = optee_ffa_shm_register, /* same as for clnt ops */
 773         .shm_unregister = optee_ffa_shm_unregister_supp,
 774 };
 775
 776 static const struct tee_desc optee_ffa_supp_desc = {
 777         .name = DRIVER_NAME "-ffa-supp",
 778         .ops = &optee_ffa_supp_ops,
 779         .owner = THIS_MODULE,
 780         .flags = TEE_DESC_PRIVILEGED,
 781 };
 782
 783 static const struct optee_ops optee_ffa_ops = {
 784         .do_call_with_arg = optee_ffa_do_call_with_arg,
 785         .to_msg_param = optee_ffa_to_msg_param,
 786         .from_msg_param = optee_ffa_from_msg_param,
 787 };
 788
 789 static void optee_ffa_remove(struct ffa_device *ffa_dev)
 790 {
 791         struct optee *optee = ffa_dev->dev.driver_data;
 792
 793         optee_remove_common(optee);
 794
 795         mutex_destroy(&optee->ffa.mutex);
 796         rhashtable_free_and_destroy(&optee->ffa.global_ids, rh_free_fn, NULL);
 797
 798         kfree(optee);
 799 }
 800
 801 static int optee_ffa_probe(struct ffa_device *ffa_dev)
 802 {
 803         const struct ffa_dev_ops *ffa_ops;
 804         unsigned int rpc_arg_count;
 805         struct tee_shm_pool *pool;
 806         struct tee_device *teedev;
 807         struct tee_context *ctx;
 808         struct optee *optee;
 809         int rc;
 810
 811         ffa_ops = ffa_dev_ops_get(ffa_dev);
 812         if (!ffa_ops) {
 813                 pr_warn("failed \"method\" init: ffa\n");
 814                 return -ENOENT;
 815         }
 816
 817         if (!optee_ffa_api_is_compatbile(ffa_dev, ffa_ops))
 818                 return -EINVAL;
 819
 820         if (!optee_ffa_exchange_caps(ffa_dev, ffa_ops, &rpc_arg_count))
 821                 return -EINVAL;
 822
 823         optee = kzalloc(sizeof(*optee), GFP_KERNEL);
 824         if (!optee)
 825                 return -ENOMEM;
 826
 827         pool = optee_ffa_config_dyn_shm();
 828         if (IS_ERR(pool)) {
 829                 rc = PTR_ERR(pool);
 830                 goto err_free_optee;
 831         }
 832         optee->pool = pool;
 833
 834         optee->ops = &optee_ffa_ops;
 835         optee->ffa.ffa_dev = ffa_dev;
 836         optee->ffa.ffa_ops = ffa_ops;
 837         optee->rpc_arg_count = rpc_arg_count;
 838
 839         teedev = tee_device_alloc(&optee_ffa_clnt_desc, NULL, optee->pool,
 840                                   optee);
 841         if (IS_ERR(teedev)) {
 842                 rc = PTR_ERR(teedev);
 843                 goto err_free_pool;
 844         }
 845         optee->teedev = teedev;
 846
 847         teedev = tee_device_alloc(&optee_ffa_supp_desc, NULL, optee->pool,
 848                                   optee);
 849         if (IS_ERR(teedev)) {
 850                 rc = PTR_ERR(teedev);
 851                 goto err_unreg_teedev;
 852         }
 853         optee->supp_teedev = teedev;
 854
 855         rc = tee_device_register(optee->teedev);
 856         if (rc)
 857                 goto err_unreg_supp_teedev;
 858
 859         rc = tee_device_register(optee->supp_teedev);
 860         if (rc)
 861                 goto err_unreg_supp_teedev;
 862
 863         rc = rhashtable_init(&optee->ffa.global_ids, &shm_rhash_params);
 864         if (rc)
 865                 goto err_unreg_supp_teedev;
 866         mutex_init(&optee->ffa.mutex);
 867         mutex_init(&optee->call_queue.mutex);
 868         INIT_LIST_HEAD(&optee->call_queue.waiters);
 869         optee_supp_init(&optee->supp);
 870         ffa_dev_set_drvdata(ffa_dev, optee);
 871         ctx = teedev_open(optee->teedev);
 872         if (IS_ERR(ctx))
 873                 goto err_rhashtable_free;
 874         optee->ctx = ctx;
 875         rc = optee_notif_init(optee, OPTEE_DEFAULT_MAX_NOTIF_VALUE);
 876         if (rc)
 877                 goto err_close_ctx;
 878
 879         rc = optee_enumerate_devices(PTA_CMD_GET_DEVICES);
 880         if (rc)
 881                 goto err_unregister_devices;
 882
 883         pr_info("initialized driver\n");
 884         return 0;
 885
 886 err_unregister_devices:
 887         optee_unregister_devices();
 888         optee_notif_uninit(optee);
 889 err_close_ctx:
 890         teedev_close_context(ctx);
 891 err_rhashtable_free:
 892         rhashtable_free_and_destroy(&optee->ffa.global_ids, rh_free_fn, NULL);
 893         optee_supp_uninit(&optee->supp);
 894         mutex_destroy(&optee->call_queue.mutex);
 895 err_unreg_supp_teedev:
 896         tee_device_unregister(optee->supp_teedev);
 897 err_unreg_teedev:
 898         tee_device_unregister(optee->teedev);
 899 err_free_pool:
 900         tee_shm_pool_free(pool);
 901 err_free_optee:
 902         kfree(optee);
 903         return rc;
 904 }
 905
 906 static const struct ffa_device_id optee_ffa_device_id[] = {
 907         /* 486178e0-e7f8-11e3-bc5e0002a5d5c51b */
 908         { UUID_INIT(0x486178e0, 0xe7f8, 0x11e3,
 909                     0xbc, 0x5e, 0x00, 0x02, 0xa5, 0xd5, 0xc5, 0x1b) },
 910         {}
 911 };
 912
 913 static struct ffa_driver optee_ffa_driver = {
 914         .name = "optee",
 915         .probe = optee_ffa_probe,
 916         .remove = optee_ffa_remove,
 917         .id_table = optee_ffa_device_id,
 918 };
 919
 920 int optee_ffa_abi_register(void)
 921 {
 922         if (IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT))
 923                 return ffa_register(&optee_ffa_driver);
 924         else
 925                 return -EOPNOTSUPP;
 926 }
 927
 928 void optee_ffa_abi_unregister(void)
 929 {
 930         if (IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT))
 931                 ffa_unregister(&optee_ffa_driver);
 932 }