Merge tag 'optee-fixes-for-v5.17' of git://git.linaro.org/people/jens.wiklander/linux...

[uclinux-h8/linux.git] / drivers / tee / optee / ffa_abi.c

diff --git a/drivers/tee/optee/ffa_abi.c b/drivers/tee/optee/ffa_abi.c
index 0775759..f2bf6c6 100644 (file)
--- a/drivers/tee/optee/ffa_abi.c
+++ b/drivers/tee/optee/ffa_abi.c
@@ -424,6 +424,7 @@ static struct tee_shm_pool_mgr *optee_ffa_shm_pool_alloc_pages(void)
  */
 
 static void handle_ffa_rpc_func_cmd_shm_alloc(struct tee_context *ctx,
+                                             struct optee *optee,
                                              struct optee_msg_arg *arg)
 {
        struct tee_shm *shm;
@@ -439,7 +440,7 @@ static void handle_ffa_rpc_func_cmd_shm_alloc(struct tee_context *ctx,
                shm = optee_rpc_cmd_alloc_suppl(ctx, arg->params[0].u.value.b);
                break;
        case OPTEE_RPC_SHM_TYPE_KERNEL:
-               shm = tee_shm_alloc(ctx, arg->params[0].u.value.b,
+               shm = tee_shm_alloc(optee->ctx, arg->params[0].u.value.b,
                                    TEE_SHM_MAPPED | TEE_SHM_PRIV);
                break;
        default:
@@ -493,14 +494,13 @@ err_bad_param:
 }
 
 static void handle_ffa_rpc_func_cmd(struct tee_context *ctx,
+                                   struct optee *optee,
                                    struct optee_msg_arg *arg)
 {
-       struct optee *optee = tee_get_drvdata(ctx->teedev);
-
        arg->ret_origin = TEEC_ORIGIN_COMMS;
        switch (arg->cmd) {
        case OPTEE_RPC_CMD_SHM_ALLOC:
-               handle_ffa_rpc_func_cmd_shm_alloc(ctx, arg);
+               handle_ffa_rpc_func_cmd_shm_alloc(ctx, optee, arg);
                break;
        case OPTEE_RPC_CMD_SHM_FREE:
                handle_ffa_rpc_func_cmd_shm_free(ctx, optee, arg);
@@ -510,12 +510,12 @@ static void handle_ffa_rpc_func_cmd(struct tee_context *ctx,
        }
 }
 
-static void optee_handle_ffa_rpc(struct tee_context *ctx, u32 cmd,
-                                struct optee_msg_arg *arg)
+static void optee_handle_ffa_rpc(struct tee_context *ctx, struct optee *optee,
+                                u32 cmd, struct optee_msg_arg *arg)
 {
        switch (cmd) {
        case OPTEE_FFA_YIELDING_CALL_RETURN_RPC_CMD:
-               handle_ffa_rpc_func_cmd(ctx, arg);
+               handle_ffa_rpc_func_cmd(ctx, optee, arg);
                break;
        case OPTEE_FFA_YIELDING_CALL_RETURN_INTERRUPT:
                /* Interrupt delivered by now */
@@ -582,7 +582,7 @@ static int optee_ffa_yielding_call(struct tee_context *ctx,
                 * above.
                 */
                cond_resched();
-               optee_handle_ffa_rpc(ctx, data->data1, rpc_arg);
+               optee_handle_ffa_rpc(ctx, optee, data->data1, rpc_arg);
                cmd = OPTEE_FFA_YIELDING_CALL_RESUME;
                data->data0 = cmd;
                data->data1 = 0;
@@ -802,7 +802,9 @@ static int optee_ffa_probe(struct ffa_device *ffa_dev)
 {
        const struct ffa_dev_ops *ffa_ops;
        unsigned int rpc_arg_count;
+       struct tee_shm_pool *pool;
        struct tee_device *teedev;
+       struct tee_context *ctx;
        struct optee *optee;
        int rc;
 
@@ -822,12 +824,12 @@ static int optee_ffa_probe(struct ffa_device *ffa_dev)
        if (!optee)
                return -ENOMEM;
 
-       optee->pool = optee_ffa_config_dyn_shm();
-       if (IS_ERR(optee->pool)) {
-               rc = PTR_ERR(optee->pool);
-               optee->pool = NULL;
-               goto err;
+       pool = optee_ffa_config_dyn_shm();
+       if (IS_ERR(pool)) {
+               rc = PTR_ERR(pool);
+               goto err_free_optee;
        }
+       optee->pool = pool;
 
        optee->ops = &optee_ffa_ops;
        optee->ffa.ffa_dev = ffa_dev;
@@ -838,7 +840,7 @@ static int optee_ffa_probe(struct ffa_device *ffa_dev)
                                  optee);
        if (IS_ERR(teedev)) {
                rc = PTR_ERR(teedev);
-               goto err;
+               goto err_free_pool;
        }
        optee->teedev = teedev;
 
@@ -846,50 +848,57 @@ static int optee_ffa_probe(struct ffa_device *ffa_dev)
                                  optee);
        if (IS_ERR(teedev)) {
                rc = PTR_ERR(teedev);
-               goto err;
+               goto err_unreg_teedev;
        }
        optee->supp_teedev = teedev;
 
        rc = tee_device_register(optee->teedev);
        if (rc)
-               goto err;
+               goto err_unreg_supp_teedev;
 
        rc = tee_device_register(optee->supp_teedev);
        if (rc)
-               goto err;
+               goto err_unreg_supp_teedev;
 
        rc = rhashtable_init(&optee->ffa.global_ids, &shm_rhash_params);
        if (rc)
-               goto err;
+               goto err_unreg_supp_teedev;
        mutex_init(&optee->ffa.mutex);
        mutex_init(&optee->call_queue.mutex);
        INIT_LIST_HEAD(&optee->call_queue.waiters);
        optee_supp_init(&optee->supp);
        ffa_dev_set_drvdata(ffa_dev, optee);
+       ctx = teedev_open(optee->teedev);
+       if (IS_ERR(ctx))
+               goto err_rhashtable_free;
+       optee->ctx = ctx;
        rc = optee_notif_init(optee, OPTEE_DEFAULT_MAX_NOTIF_VALUE);
-       if (rc) {
-               optee_ffa_remove(ffa_dev);
-               return rc;
-       }
+       if (rc)
+               goto err_close_ctx;
 
        rc = optee_enumerate_devices(PTA_CMD_GET_DEVICES);
-       if (rc) {
-               optee_ffa_remove(ffa_dev);
-               return rc;
-       }
+       if (rc)
+               goto err_unregister_devices;
 
        pr_info("initialized driver\n");
        return 0;
-err:
-       /*
-        * tee_device_unregister() is safe to call even if the
-        * devices hasn't been registered with
-        * tee_device_register() yet.
-        */
+
+err_unregister_devices:
+       optee_unregister_devices();
+       optee_notif_uninit(optee);
+err_close_ctx:
+       teedev_close_context(ctx);
+err_rhashtable_free:
+       rhashtable_free_and_destroy(&optee->ffa.global_ids, rh_free_fn, NULL);
+       optee_supp_uninit(&optee->supp);
+       mutex_destroy(&optee->call_queue.mutex);
+err_unreg_supp_teedev:
        tee_device_unregister(optee->supp_teedev);
+err_unreg_teedev:
        tee_device_unregister(optee->teedev);
-       if (optee->pool)
-               tee_shm_pool_free(optee->pool);
+err_free_pool:
+       tee_shm_pool_free(pool);
+err_free_optee:
        kfree(optee);
        return rc;
 }