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_msg_arg *arg)
 428 {
 429         struct tee_shm *shm;
 430
 431         if (arg->num_params != 1 ||
 432             arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT) {
 433                 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
 434                 return;
 435         }
 436
 437         switch (arg->params[0].u.value.a) {
 438         case OPTEE_RPC_SHM_TYPE_APPL:
 439                 shm = optee_rpc_cmd_alloc_suppl(ctx, arg->params[0].u.value.b);
 440                 break;
 441         case OPTEE_RPC_SHM_TYPE_KERNEL:
 442                 shm = tee_shm_alloc(ctx, arg->params[0].u.value.b,
 443                                     TEE_SHM_MAPPED | TEE_SHM_PRIV);
 444                 break;
 445         default:
 446                 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
 447                 return;
 448         }
 449
 450         if (IS_ERR(shm)) {
 451                 arg->ret = TEEC_ERROR_OUT_OF_MEMORY;
 452                 return;
 453         }
 454
 455         arg->params[0] = (struct optee_msg_param){
 456                 .attr = OPTEE_MSG_ATTR_TYPE_FMEM_OUTPUT,
 457                 .u.fmem.size = tee_shm_get_size(shm),
 458                 .u.fmem.global_id = shm->sec_world_id,
 459                 .u.fmem.internal_offs = shm->offset,
 460         };
 461
 462         arg->ret = TEEC_SUCCESS;
 463 }
 464
 465 static void handle_ffa_rpc_func_cmd_shm_free(struct tee_context *ctx,
 466                                              struct optee *optee,
 467                                              struct optee_msg_arg *arg)
 468 {
 469         struct tee_shm *shm;
 470
 471         if (arg->num_params != 1 ||
 472             arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT)
 473                 goto err_bad_param;
 474
 475         shm = optee_shm_from_ffa_handle(optee, arg->params[0].u.value.b);
 476         if (!shm)
 477                 goto err_bad_param;
 478         switch (arg->params[0].u.value.a) {
 479         case OPTEE_RPC_SHM_TYPE_APPL:
 480                 optee_rpc_cmd_free_suppl(ctx, shm);
 481                 break;
 482         case OPTEE_RPC_SHM_TYPE_KERNEL:
 483                 tee_shm_free(shm);
 484                 break;
 485         default:
 486                 goto err_bad_param;
 487         }
 488         arg->ret = TEEC_SUCCESS;
 489         return;
 490
 491 err_bad_param:
 492         arg->ret = TEEC_ERROR_BAD_PARAMETERS;
 493 }
 494
 495 static void handle_ffa_rpc_func_cmd(struct tee_context *ctx,
 496                                     struct optee_msg_arg *arg)
 497 {
 498         struct optee *optee = tee_get_drvdata(ctx->teedev);
 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, 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, u32 cmd,
 514                                  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, 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, 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 = tee_shm_get_va(shm, 0);
 623         unsigned int rpc_arg_offs = OPTEE_MSG_GET_ARG_SIZE(arg->num_params);
 624         struct optee_msg_arg *rpc_arg = tee_shm_get_va(shm, rpc_arg_offs);
 625
 626         return optee_ffa_yielding_call(ctx, &data, rpc_arg);
 627 }
 628
 629 /*
 630  * 6. Driver initialization
 631  *
 632  * During driver inititialization is the OP-TEE Secure Partition is probed
 633  * to find out which features it supports so the driver can be initialized
 634  * with a matching configuration.
 635  */
 636
 637 static bool optee_ffa_api_is_compatbile(struct ffa_device *ffa_dev,
 638                                         const struct ffa_dev_ops *ops)
 639 {
 640         struct ffa_send_direct_data data = { OPTEE_FFA_GET_API_VERSION };
 641         int rc;
 642
 643         ops->mode_32bit_set(ffa_dev);
 644
 645         rc = ops->sync_send_receive(ffa_dev, &data);
 646         if (rc) {
 647                 pr_err("Unexpected error %d\n", rc);
 648                 return false;
 649         }
 650         if (data.data0 != OPTEE_FFA_VERSION_MAJOR ||
 651             data.data1 < OPTEE_FFA_VERSION_MINOR) {
 652                 pr_err("Incompatible OP-TEE API version %lu.%lu",
 653                        data.data0, data.data1);
 654                 return false;
 655         }
 656
 657         data = (struct ffa_send_direct_data){ OPTEE_FFA_GET_OS_VERSION };
 658         rc = ops->sync_send_receive(ffa_dev, &data);
 659         if (rc) {
 660                 pr_err("Unexpected error %d\n", rc);
 661                 return false;
 662         }
 663         if (data.data2)
 664                 pr_info("revision %lu.%lu (%08lx)",
 665                         data.data0, data.data1, data.data2);
 666         else
 667                 pr_info("revision %lu.%lu", data.data0, data.data1);
 668
 669         return true;
 670 }
 671
 672 static bool optee_ffa_exchange_caps(struct ffa_device *ffa_dev,
 673                                     const struct ffa_dev_ops *ops,
 674                                     unsigned int *rpc_arg_count)
 675 {
 676         struct ffa_send_direct_data data = { OPTEE_FFA_EXCHANGE_CAPABILITIES };
 677         int rc;
 678
 679         rc = ops->sync_send_receive(ffa_dev, &data);
 680         if (rc) {
 681                 pr_err("Unexpected error %d", rc);
 682                 return false;
 683         }
 684         if (data.data0) {
 685                 pr_err("Unexpected exchange error %lu", data.data0);
 686                 return false;
 687         }
 688
 689         *rpc_arg_count = (u8)data.data1;
 690
 691         return true;
 692 }
 693
 694 static struct tee_shm_pool *optee_ffa_config_dyn_shm(void)
 695 {
 696         struct tee_shm_pool_mgr *priv_mgr;
 697         struct tee_shm_pool_mgr *dmabuf_mgr;
 698         void *rc;
 699
 700         rc = optee_ffa_shm_pool_alloc_pages();
 701         if (IS_ERR(rc))
 702                 return rc;
 703         priv_mgr = rc;
 704
 705         rc = optee_ffa_shm_pool_alloc_pages();
 706         if (IS_ERR(rc)) {
 707                 tee_shm_pool_mgr_destroy(priv_mgr);
 708                 return rc;
 709         }
 710         dmabuf_mgr = rc;
 711
 712         rc = tee_shm_pool_alloc(priv_mgr, dmabuf_mgr);
 713         if (IS_ERR(rc)) {
 714                 tee_shm_pool_mgr_destroy(priv_mgr);
 715                 tee_shm_pool_mgr_destroy(dmabuf_mgr);
 716         }
 717
 718         return rc;
 719 }
 720
 721 static void optee_ffa_get_version(struct tee_device *teedev,
 722                                   struct tee_ioctl_version_data *vers)
 723 {
 724         struct tee_ioctl_version_data v = {
 725                 .impl_id = TEE_IMPL_ID_OPTEE,
 726                 .impl_caps = TEE_OPTEE_CAP_TZ,
 727                 .gen_caps = TEE_GEN_CAP_GP | TEE_GEN_CAP_REG_MEM |
 728                             TEE_GEN_CAP_MEMREF_NULL,
 729         };
 730
 731         *vers = v;
 732 }
 733
 734 static int optee_ffa_open(struct tee_context *ctx)
 735 {
 736         return optee_open(ctx, true);
 737 }
 738
 739 static const struct tee_driver_ops optee_ffa_clnt_ops = {
 740         .get_version = optee_ffa_get_version,
 741         .open = optee_ffa_open,
 742         .release = optee_release,
 743         .open_session = optee_open_session,
 744         .close_session = optee_close_session,
 745         .invoke_func = optee_invoke_func,
 746         .cancel_req = optee_cancel_req,
 747         .shm_register = optee_ffa_shm_register,
 748         .shm_unregister = optee_ffa_shm_unregister,
 749 };
 750
 751 static const struct tee_desc optee_ffa_clnt_desc = {
 752         .name = DRIVER_NAME "-ffa-clnt",
 753         .ops = &optee_ffa_clnt_ops,
 754         .owner = THIS_MODULE,
 755 };
 756
 757 static const struct tee_driver_ops optee_ffa_supp_ops = {
 758         .get_version = optee_ffa_get_version,
 759         .open = optee_ffa_open,
 760         .release = optee_release_supp,
 761         .supp_recv = optee_supp_recv,
 762         .supp_send = optee_supp_send,
 763         .shm_register = optee_ffa_shm_register, /* same as for clnt ops */
 764         .shm_unregister = optee_ffa_shm_unregister_supp,
 765 };
 766
 767 static const struct tee_desc optee_ffa_supp_desc = {
 768         .name = DRIVER_NAME "-ffa-supp",
 769         .ops = &optee_ffa_supp_ops,
 770         .owner = THIS_MODULE,
 771         .flags = TEE_DESC_PRIVILEGED,
 772 };
 773
 774 static const struct optee_ops optee_ffa_ops = {
 775         .do_call_with_arg = optee_ffa_do_call_with_arg,
 776         .to_msg_param = optee_ffa_to_msg_param,
 777         .from_msg_param = optee_ffa_from_msg_param,
 778 };
 779
 780 static void optee_ffa_remove(struct ffa_device *ffa_dev)
 781 {
 782         struct optee *optee = ffa_dev->dev.driver_data;
 783
 784         optee_remove_common(optee);
 785
 786         mutex_destroy(&optee->ffa.mutex);
 787         rhashtable_free_and_destroy(&optee->ffa.global_ids, rh_free_fn, NULL);
 788
 789         kfree(optee);
 790 }
 791
 792 static int optee_ffa_probe(struct ffa_device *ffa_dev)
 793 {
 794         const struct ffa_dev_ops *ffa_ops;
 795         unsigned int rpc_arg_count;
 796         struct tee_device *teedev;
 797         struct optee *optee;
 798         int rc;
 799
 800         ffa_ops = ffa_dev_ops_get(ffa_dev);
 801         if (!ffa_ops) {
 802                 pr_warn("failed \"method\" init: ffa\n");
 803                 return -ENOENT;
 804         }
 805
 806         if (!optee_ffa_api_is_compatbile(ffa_dev, ffa_ops))
 807                 return -EINVAL;
 808
 809         if (!optee_ffa_exchange_caps(ffa_dev, ffa_ops, &rpc_arg_count))
 810                 return -EINVAL;
 811
 812         optee = kzalloc(sizeof(*optee), GFP_KERNEL);
 813         if (!optee)
 814                 return -ENOMEM;
 815
 816         optee->pool = optee_ffa_config_dyn_shm();
 817         if (IS_ERR(optee->pool)) {
 818                 rc = PTR_ERR(optee->pool);
 819                 optee->pool = NULL;
 820                 goto err;
 821         }
 822
 823         optee->ops = &optee_ffa_ops;
 824         optee->ffa.ffa_dev = ffa_dev;
 825         optee->ffa.ffa_ops = ffa_ops;
 826         optee->rpc_arg_count = rpc_arg_count;
 827
 828         teedev = tee_device_alloc(&optee_ffa_clnt_desc, NULL, optee->pool,
 829                                   optee);
 830         if (IS_ERR(teedev)) {
 831                 rc = PTR_ERR(teedev);
 832                 goto err;
 833         }
 834         optee->teedev = teedev;
 835
 836         teedev = tee_device_alloc(&optee_ffa_supp_desc, NULL, optee->pool,
 837                                   optee);
 838         if (IS_ERR(teedev)) {
 839                 rc = PTR_ERR(teedev);
 840                 goto err;
 841         }
 842         optee->supp_teedev = teedev;
 843
 844         rc = tee_device_register(optee->teedev);
 845         if (rc)
 846                 goto err;
 847
 848         rc = tee_device_register(optee->supp_teedev);
 849         if (rc)
 850                 goto err;
 851
 852         rc = rhashtable_init(&optee->ffa.global_ids, &shm_rhash_params);
 853         if (rc)
 854                 goto err;
 855         mutex_init(&optee->ffa.mutex);
 856         mutex_init(&optee->call_queue.mutex);
 857         INIT_LIST_HEAD(&optee->call_queue.waiters);
 858         optee_wait_queue_init(&optee->wait_queue);
 859         optee_supp_init(&optee->supp);
 860         ffa_dev_set_drvdata(ffa_dev, optee);
 861
 862         rc = optee_enumerate_devices(PTA_CMD_GET_DEVICES);
 863         if (rc) {
 864                 optee_ffa_remove(ffa_dev);
 865                 return rc;
 866         }
 867
 868         pr_info("initialized driver\n");
 869         return 0;
 870 err:
 871         /*
 872          * tee_device_unregister() is safe to call even if the
 873          * devices hasn't been registered with
 874          * tee_device_register() yet.
 875          */
 876         tee_device_unregister(optee->supp_teedev);
 877         tee_device_unregister(optee->teedev);
 878         if (optee->pool)
 879                 tee_shm_pool_free(optee->pool);
 880         kfree(optee);
 881         return rc;
 882 }
 883
 884 static const struct ffa_device_id optee_ffa_device_id[] = {
 885         /* 486178e0-e7f8-11e3-bc5e0002a5d5c51b */
 886         { UUID_INIT(0x486178e0, 0xe7f8, 0x11e3,
 887                     0xbc, 0x5e, 0x00, 0x02, 0xa5, 0xd5, 0xc5, 0x1b) },
 888         {}
 889 };
 890
 891 static struct ffa_driver optee_ffa_driver = {
 892         .name = "optee",
 893         .probe = optee_ffa_probe,
 894         .remove = optee_ffa_remove,
 895         .id_table = optee_ffa_device_id,
 896 };
 897
 898 int optee_ffa_abi_register(void)
 899 {
 900         if (IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT))
 901                 return ffa_register(&optee_ffa_driver);
 902         else
 903                 return -EOPNOTSUPP;
 904 }
 905
 906 void optee_ffa_abi_unregister(void)
 907 {
 908         if (IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT))
 909                 ffa_unregister(&optee_ffa_driver);
 910 }