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         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 {
 533         struct optee *optee = tee_get_drvdata(ctx->teedev);
 534         struct ffa_device *ffa_dev = optee->ffa.ffa_dev;
 535         const struct ffa_msg_ops *msg_ops = ffa_dev->ops->msg_ops;
 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 = msg_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_mem_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  * @offs:       offset of the message in @shm
 607  *
 608  * Does a FF-A call to OP-TEE in secure world and handles eventual resulting
 609  * Remote Procedure Calls (RPC) from OP-TEE.
 610  *
 611  * Returns return code from FF-A, 0 is OK
 612  */
 613
 614 static int optee_ffa_do_call_with_arg(struct tee_context *ctx,
 615                                       struct tee_shm *shm, u_int offs)
 616 {
 617         struct ffa_send_direct_data data = {
 618                 .data0 = OPTEE_FFA_YIELDING_CALL_WITH_ARG,
 619                 .data1 = (u32)shm->sec_world_id,
 620                 .data2 = (u32)(shm->sec_world_id >> 32),
 621                 .data3 = offs,
 622         };
 623         struct optee_msg_arg *arg;
 624         unsigned int rpc_arg_offs;
 625         struct optee_msg_arg *rpc_arg;
 626
 627         /*
 628          * The shared memory object has to start on a page when passed as
 629          * an argument struct. This is also what the shm pool allocator
 630          * returns, but check this before calling secure world to catch
 631          * eventual errors early in case something changes.
 632          */
 633         if (shm->offset)
 634                 return -EINVAL;
 635
 636         arg = tee_shm_get_va(shm, offs);
 637         if (IS_ERR(arg))
 638                 return PTR_ERR(arg);
 639
 640         rpc_arg_offs = OPTEE_MSG_GET_ARG_SIZE(arg->num_params);
 641         rpc_arg = tee_shm_get_va(shm, offs + rpc_arg_offs);
 642         if (IS_ERR(rpc_arg))
 643                 return PTR_ERR(rpc_arg);
 644
 645         return optee_ffa_yielding_call(ctx, &data, rpc_arg);
 646 }
 647
 648 /*
 649  * 6. Driver initialization
 650  *
 651  * During driver inititialization is the OP-TEE Secure Partition is probed
 652  * to find out which features it supports so the driver can be initialized
 653  * with a matching configuration.
 654  */
 655
 656 static bool optee_ffa_api_is_compatbile(struct ffa_device *ffa_dev,
 657                                         const struct ffa_ops *ops)
 658 {
 659         const struct ffa_msg_ops *msg_ops = ops->msg_ops;
 660         struct ffa_send_direct_data data = { OPTEE_FFA_GET_API_VERSION };
 661         int rc;
 662
 663         msg_ops->mode_32bit_set(ffa_dev);
 664
 665         rc = msg_ops->sync_send_receive(ffa_dev, &data);
 666         if (rc) {
 667                 pr_err("Unexpected error %d\n", rc);
 668                 return false;
 669         }
 670         if (data.data0 != OPTEE_FFA_VERSION_MAJOR ||
 671             data.data1 < OPTEE_FFA_VERSION_MINOR) {
 672                 pr_err("Incompatible OP-TEE API version %lu.%lu",
 673                        data.data0, data.data1);
 674                 return false;
 675         }
 676
 677         data = (struct ffa_send_direct_data){ OPTEE_FFA_GET_OS_VERSION };
 678         rc = msg_ops->sync_send_receive(ffa_dev, &data);
 679         if (rc) {
 680                 pr_err("Unexpected error %d\n", rc);
 681                 return false;
 682         }
 683         if (data.data2)
 684                 pr_info("revision %lu.%lu (%08lx)",
 685                         data.data0, data.data1, data.data2);
 686         else
 687                 pr_info("revision %lu.%lu", data.data0, data.data1);
 688
 689         return true;
 690 }
 691
 692 static bool optee_ffa_exchange_caps(struct ffa_device *ffa_dev,
 693                                     const struct ffa_ops *ops,
 694                                     u32 *sec_caps,
 695                                     unsigned int *rpc_param_count)
 696 {
 697         struct ffa_send_direct_data data = { OPTEE_FFA_EXCHANGE_CAPABILITIES };
 698         int rc;
 699
 700         rc = ops->msg_ops->sync_send_receive(ffa_dev, &data);
 701         if (rc) {
 702                 pr_err("Unexpected error %d", rc);
 703                 return false;
 704         }
 705         if (data.data0) {
 706                 pr_err("Unexpected exchange error %lu", data.data0);
 707                 return false;
 708         }
 709
 710         *rpc_param_count = (u8)data.data1;
 711         *sec_caps = data.data2;
 712
 713         return true;
 714 }
 715
 716 static void optee_ffa_get_version(struct tee_device *teedev,
 717                                   struct tee_ioctl_version_data *vers)
 718 {
 719         struct tee_ioctl_version_data v = {
 720                 .impl_id = TEE_IMPL_ID_OPTEE,
 721                 .impl_caps = TEE_OPTEE_CAP_TZ,
 722                 .gen_caps = TEE_GEN_CAP_GP | TEE_GEN_CAP_REG_MEM |
 723                             TEE_GEN_CAP_MEMREF_NULL,
 724         };
 725
 726         *vers = v;
 727 }
 728
 729 static int optee_ffa_open(struct tee_context *ctx)
 730 {
 731         return optee_open(ctx, true);
 732 }
 733
 734 static const struct tee_driver_ops optee_ffa_clnt_ops = {
 735         .get_version = optee_ffa_get_version,
 736         .open = optee_ffa_open,
 737         .release = optee_release,
 738         .open_session = optee_open_session,
 739         .close_session = optee_close_session,
 740         .invoke_func = optee_invoke_func,
 741         .cancel_req = optee_cancel_req,
 742         .shm_register = optee_ffa_shm_register,
 743         .shm_unregister = optee_ffa_shm_unregister,
 744 };
 745
 746 static const struct tee_desc optee_ffa_clnt_desc = {
 747         .name = DRIVER_NAME "-ffa-clnt",
 748         .ops = &optee_ffa_clnt_ops,
 749         .owner = THIS_MODULE,
 750 };
 751
 752 static const struct tee_driver_ops optee_ffa_supp_ops = {
 753         .get_version = optee_ffa_get_version,
 754         .open = optee_ffa_open,
 755         .release = optee_release_supp,
 756         .supp_recv = optee_supp_recv,
 757         .supp_send = optee_supp_send,
 758         .shm_register = optee_ffa_shm_register, /* same as for clnt ops */
 759         .shm_unregister = optee_ffa_shm_unregister_supp,
 760 };
 761
 762 static const struct tee_desc optee_ffa_supp_desc = {
 763         .name = DRIVER_NAME "-ffa-supp",
 764         .ops = &optee_ffa_supp_ops,
 765         .owner = THIS_MODULE,
 766         .flags = TEE_DESC_PRIVILEGED,
 767 };
 768
 769 static const struct optee_ops optee_ffa_ops = {
 770         .do_call_with_arg = optee_ffa_do_call_with_arg,
 771         .to_msg_param = optee_ffa_to_msg_param,
 772         .from_msg_param = optee_ffa_from_msg_param,
 773 };
 774
 775 static void optee_ffa_remove(struct ffa_device *ffa_dev)
 776 {
 777         struct optee *optee = ffa_dev_get_drvdata(ffa_dev);
 778
 779         optee_remove_common(optee);
 780
 781         mutex_destroy(&optee->ffa.mutex);
 782         rhashtable_free_and_destroy(&optee->ffa.global_ids, rh_free_fn, NULL);
 783
 784         kfree(optee);
 785 }
 786
 787 static int optee_ffa_probe(struct ffa_device *ffa_dev)
 788 {
 789         const struct ffa_ops *ffa_ops;
 790         unsigned int rpc_param_count;
 791         struct tee_shm_pool *pool;
 792         struct tee_device *teedev;
 793         struct tee_context *ctx;
 794         u32 arg_cache_flags = 0;
 795         struct optee *optee;
 796         u32 sec_caps;
 797         int rc;
 798
 799         ffa_ops = ffa_dev->ops;
 800
 801         if (!optee_ffa_api_is_compatbile(ffa_dev, ffa_ops))
 802                 return -EINVAL;
 803
 804         if (!optee_ffa_exchange_caps(ffa_dev, ffa_ops, &sec_caps,
 805                                      &rpc_param_count))
 806                 return -EINVAL;
 807         if (sec_caps & OPTEE_FFA_SEC_CAP_ARG_OFFSET)
 808                 arg_cache_flags |= OPTEE_SHM_ARG_SHARED;
 809
 810         optee = kzalloc(sizeof(*optee), GFP_KERNEL);
 811         if (!optee)
 812                 return -ENOMEM;
 813
 814         pool = optee_ffa_shm_pool_alloc_pages();
 815         if (IS_ERR(pool)) {
 816                 rc = PTR_ERR(pool);
 817                 goto err_free_optee;
 818         }
 819         optee->pool = pool;
 820
 821         optee->ops = &optee_ffa_ops;
 822         optee->ffa.ffa_dev = ffa_dev;
 823         optee->rpc_param_count = rpc_param_count;
 824
 825         teedev = tee_device_alloc(&optee_ffa_clnt_desc, NULL, optee->pool,
 826                                   optee);
 827         if (IS_ERR(teedev)) {
 828                 rc = PTR_ERR(teedev);
 829                 goto err_free_pool;
 830         }
 831         optee->teedev = teedev;
 832
 833         teedev = tee_device_alloc(&optee_ffa_supp_desc, NULL, optee->pool,
 834                                   optee);
 835         if (IS_ERR(teedev)) {
 836                 rc = PTR_ERR(teedev);
 837                 goto err_unreg_teedev;
 838         }
 839         optee->supp_teedev = teedev;
 840
 841         rc = tee_device_register(optee->teedev);
 842         if (rc)
 843                 goto err_unreg_supp_teedev;
 844
 845         rc = tee_device_register(optee->supp_teedev);
 846         if (rc)
 847                 goto err_unreg_supp_teedev;
 848
 849         rc = rhashtable_init(&optee->ffa.global_ids, &shm_rhash_params);
 850         if (rc)
 851                 goto err_unreg_supp_teedev;
 852         mutex_init(&optee->ffa.mutex);
 853         mutex_init(&optee->call_queue.mutex);
 854         INIT_LIST_HEAD(&optee->call_queue.waiters);
 855         optee_supp_init(&optee->supp);
 856         optee_shm_arg_cache_init(optee, arg_cache_flags);
 857         ffa_dev_set_drvdata(ffa_dev, optee);
 858         ctx = teedev_open(optee->teedev);
 859         if (IS_ERR(ctx)) {
 860                 rc = PTR_ERR(ctx);
 861                 goto err_rhashtable_free;
 862         }
 863         optee->ctx = ctx;
 864         rc = optee_notif_init(optee, OPTEE_DEFAULT_MAX_NOTIF_VALUE);
 865         if (rc)
 866                 goto err_close_ctx;
 867
 868         rc = optee_enumerate_devices(PTA_CMD_GET_DEVICES);
 869         if (rc)
 870                 goto err_unregister_devices;
 871
 872         pr_info("initialized driver\n");
 873         return 0;
 874
 875 err_unregister_devices:
 876         optee_unregister_devices();
 877         optee_notif_uninit(optee);
 878 err_close_ctx:
 879         teedev_close_context(ctx);
 880 err_rhashtable_free:
 881         rhashtable_free_and_destroy(&optee->ffa.global_ids, rh_free_fn, NULL);
 882         optee_supp_uninit(&optee->supp);
 883         mutex_destroy(&optee->call_queue.mutex);
 884         mutex_destroy(&optee->ffa.mutex);
 885 err_unreg_supp_teedev:
 886         tee_device_unregister(optee->supp_teedev);
 887 err_unreg_teedev:
 888         tee_device_unregister(optee->teedev);
 889 err_free_pool:
 890         tee_shm_pool_free(pool);
 891 err_free_optee:
 892         kfree(optee);
 893         return rc;
 894 }
 895
 896 static const struct ffa_device_id optee_ffa_device_id[] = {
 897         /* 486178e0-e7f8-11e3-bc5e0002a5d5c51b */
 898         { UUID_INIT(0x486178e0, 0xe7f8, 0x11e3,
 899                     0xbc, 0x5e, 0x00, 0x02, 0xa5, 0xd5, 0xc5, 0x1b) },
 900         {}
 901 };
 902
 903 static struct ffa_driver optee_ffa_driver = {
 904         .name = "optee",
 905         .probe = optee_ffa_probe,
 906         .remove = optee_ffa_remove,
 907         .id_table = optee_ffa_device_id,
 908 };
 909
 910 int optee_ffa_abi_register(void)
 911 {
 912         if (IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT))
 913                 return ffa_register(&optee_ffa_driver);
 914         else
 915                 return -EOPNOTSUPP;
 916 }
 917
 918 void optee_ffa_abi_unregister(void)
 919 {
 920         if (IS_REACHABLE(CONFIG_ARM_FFA_TRANSPORT))
 921                 ffa_unregister(&optee_ffa_driver);
 922 }