@@ -36,6 +36,7 @@
#include <asm/tee/tee.h>
#include <asm/tee/optee_msg.h>
#include <asm/tee/optee_smc.h>
+#include <asm/tee/optee_rpc_cmd.h>
/* Number of SMCs known to the mediator */
#define OPTEE_MEDIATOR_SMC_COUNT 11
@@ -47,6 +48,9 @@
*/
#define TEEC_ORIGIN_COMMS 0x00000002
+/* "Non-specific cause" as in GP TEE Client API Specification */
+#define TEEC_ERROR_GENERIC 0xFFFF0000
+
/*
* "Input parameters were invalid" as described
* in GP TEE Client API Specification.
@@ -89,6 +93,7 @@ struct optee_std_call {
paddr_t guest_arg_ipa;
int optee_thread_id;
int rpc_op;
+ uint64_t rpc_data_cookie;
bool in_flight;
register_t rpc_params[2];
};
@@ -97,6 +102,9 @@ struct optee_std_call {
struct shm_rpc {
struct list_head list;
struct page_info *guest_page;
+ struct page_info *xen_arg_pg;
+ struct optee_msg_arg *xen_arg;
+ gfn_t gfn;
uint64_t cookie;
};
@@ -350,10 +358,18 @@ static struct shm_rpc *allocate_and_pin_shm_rpc(struct optee_domain *ctx,
if ( !shm_rpc )
return ERR_PTR(-ENOMEM);
+ shm_rpc->xen_arg_pg = alloc_domheap_page(current->domain, 0);
+ if ( !shm_rpc->xen_arg_pg )
+ {
+ xfree(shm_rpc);
+ return ERR_PTR(-ENOMEM);
+ }
+
/* This page will be shared with OP-TEE, so we need to pin it. */
shm_rpc->guest_page = get_domain_ram_page(gfn);
if ( !shm_rpc->guest_page )
goto err;
+ shm_rpc->gfn = gfn;
shm_rpc->cookie = cookie;
@@ -376,6 +392,8 @@ static struct shm_rpc *allocate_and_pin_shm_rpc(struct optee_domain *ctx,
return shm_rpc;
err:
+ free_domheap_page(shm_rpc->xen_arg_pg);
+
if ( shm_rpc->guest_page )
put_page(shm_rpc->guest_page);
xfree(shm_rpc);
@@ -404,12 +422,32 @@ static void free_shm_rpc(struct optee_domain *ctx, uint64_t cookie)
if ( !found )
return;
+ free_domheap_page(shm_rpc->xen_arg_pg);
+
ASSERT(shm_rpc->guest_page);
put_page(shm_rpc->guest_page);
xfree(shm_rpc);
}
+static struct shm_rpc *find_shm_rpc(struct optee_domain *ctx, uint64_t cookie)
+{
+ struct shm_rpc *shm_rpc;
+
+ spin_lock(&ctx->lock);
+ list_for_each_entry( shm_rpc, &ctx->shm_rpc_list, list )
+ {
+ if ( shm_rpc->cookie == cookie )
+ {
+ spin_unlock(&ctx->lock);
+ return shm_rpc;
+ }
+ }
+ spin_unlock(&ctx->lock);
+
+ return NULL;
+}
+
static struct optee_shm_buf *allocate_optee_shm_buf(struct optee_domain *ctx,
uint64_t cookie,
unsigned int pages_cnt,
@@ -931,10 +969,13 @@ static void free_shm_buffers(struct optee_domain *ctx,
}
/* Handle RPC return from OP-TEE */
-static void handle_rpc_return(struct arm_smccc_res *res,
- struct cpu_user_regs *regs,
- struct optee_std_call *call)
+static int handle_rpc_return(struct optee_domain *ctx,
+ struct arm_smccc_res *res,
+ struct cpu_user_regs *regs,
+ struct optee_std_call *call)
{
+ int ret = 0;
+
call->rpc_op = OPTEE_SMC_RETURN_GET_RPC_FUNC(res->a0);
call->rpc_params[0] = res->a1;
call->rpc_params[1] = res->a2;
@@ -944,6 +985,51 @@ static void handle_rpc_return(struct arm_smccc_res *res,
set_user_reg(regs, 1, res->a1);
set_user_reg(regs, 2, res->a2);
set_user_reg(regs, 3, res->a3);
+
+ if ( call->rpc_op == OPTEE_SMC_RPC_FUNC_CMD )
+ {
+ /* Copy RPC request from shadowed buffer to guest */
+ uint64_t cookie = regpair_to_uint64(get_user_reg(regs, 1),
+ get_user_reg(regs, 2));
+ struct shm_rpc *shm_rpc = find_shm_rpc(ctx, cookie);
+
+ if ( !shm_rpc )
+ {
+ /*
+ * This is a very exceptional situation: OP-TEE used
+ * cookie for unknown shared buffer. Something is very
+ * wrong there. We can't even report error back to OP-TEE,
+ * because there is no buffer where we can write return
+ * code. Luckily, OP-TEE sets default error code into that
+ * buffer before the call, expecting that normal world
+ * will overwrite it with actual result. So we can just
+ * continue the call.
+ */
+ gprintk(XENLOG_ERR, "Can't find SHM-RPC with cookie %lx\n", cookie);
+
+ return -ERESTART;
+ }
+
+ shm_rpc->xen_arg = __map_domain_page(shm_rpc->xen_arg_pg);
+
+ if ( access_guest_memory_by_ipa(current->domain,
+ gfn_to_gaddr(shm_rpc->gfn),
+ shm_rpc->xen_arg,
+ OPTEE_MSG_GET_ARG_SIZE(shm_rpc->xen_arg->num_params),
+ true) )
+ {
+ /*
+ * We were unable to propagate request to guest, so let's return
+ * back to OP-TEE.
+ */
+ shm_rpc->xen_arg->ret = TEEC_ERROR_GENERIC;
+ ret = -ERESTART;
+ }
+
+ unmap_domain_page(shm_rpc->xen_arg);
+ }
+
+ return ret;
}
/*
@@ -956,6 +1042,9 @@ static void handle_rpc_return(struct arm_smccc_res *res,
* If call is complete - we need to return results with copy_std_request_back()
* and then we will destroy the call context as it is not needed anymore.
*
+ * In some rare cases we can't propagate RPC request back to guest, so we will
+ * restart the call, telling OP-TEE that request had failed.
+ *
* Shared buffers should be handled in a special way.
*/
static void do_call_with_arg(struct optee_domain *ctx,
@@ -971,7 +1060,16 @@ static void do_call_with_arg(struct optee_domain *ctx,
if ( OPTEE_SMC_RETURN_IS_RPC(res.a0) )
{
- handle_rpc_return(&res, regs, call);
+ while ( handle_rpc_return(ctx, &res, regs, call) == -ERESTART )
+ {
+ arm_smccc_smc(res.a0, res.a1, res.a2, res.a3, 0, 0, 0,
+ OPTEE_CLIENT_ID(current->domain), &res);
+
+ if ( !OPTEE_SMC_RETURN_IS_RPC(res.a0) )
+ break;
+
+ }
+
put_std_call(ctx, call);
return;
@@ -1097,6 +1195,124 @@ err:
* request from OP-TEE and wished to resume the interrupted standard
* call.
*/
+static void handle_rpc_cmd_alloc(struct optee_domain *ctx,
+ struct cpu_user_regs *regs,
+ struct optee_std_call *call,
+ struct shm_rpc *shm_rpc)
+{
+ if ( shm_rpc->xen_arg->ret || shm_rpc->xen_arg->num_params != 1 )
+ return;
+
+ if ( shm_rpc->xen_arg->params[0].attr != (OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT |
+ OPTEE_MSG_ATTR_NONCONTIG) )
+ {
+ gdprintk(XENLOG_WARNING, "Invalid attrs for shared mem buffer: %lx\n",
+ shm_rpc->xen_arg->params[0].attr);
+ return;
+ }
+
+ /* Free pg list for buffer */
+ if ( call->rpc_data_cookie )
+ free_optee_shm_buf_pg_list(ctx, call->rpc_data_cookie);
+
+ if ( !translate_noncontig(ctx, call, &shm_rpc->xen_arg->params[0]) )
+ {
+ call->rpc_data_cookie =
+ shm_rpc->xen_arg->params[0].u.tmem.shm_ref;
+ }
+ else
+ {
+ call->rpc_data_cookie = 0;
+ /*
+ * Okay, so there was problem with guest's buffer and we need
+ * to tell about this to OP-TEE.
+ */
+ shm_rpc->xen_arg->ret = TEEC_ERROR_GENERIC;
+ shm_rpc->xen_arg->num_params = 0;
+ /*
+ * TODO: With current implementation, OP-TEE will not issue
+ * RPC to free this buffer. Guest and OP-TEE will be out of
+ * sync: guest believes that it provided buffer to OP-TEE,
+ * while OP-TEE thinks of opposite. Ideally, we need to
+ * emulate RPC with OPTEE_MSG_RPC_CMD_SHM_FREE command.
+ */
+ gprintk(XENLOG_WARNING,
+ "translate_noncontig() failed, OP-TEE/guest state is out of sync.\n");
+ }
+}
+
+static void handle_rpc_cmd(struct optee_domain *ctx, struct cpu_user_regs *regs,
+ struct optee_std_call *call)
+{
+ struct shm_rpc *shm_rpc;
+ uint64_t cookie;
+ size_t arg_size;
+
+ cookie = regpair_to_uint64(get_user_reg(regs, 1),
+ get_user_reg(regs, 2));
+
+ shm_rpc = find_shm_rpc(ctx, cookie);
+
+ if ( !shm_rpc )
+ {
+ gdprintk(XENLOG_ERR, "Can't find SHM-RPC with cookie %lx\n", cookie);
+ return;
+ }
+
+ shm_rpc->xen_arg = __map_domain_page(shm_rpc->xen_arg_pg);
+
+ /* First, copy only header to read number of arguments */
+ if ( access_guest_memory_by_ipa(current->domain,
+ gfn_to_gaddr(shm_rpc->gfn),
+ shm_rpc->xen_arg,
+ sizeof(struct optee_msg_arg),
+ false) )
+ {
+ shm_rpc->xen_arg->ret = TEEC_ERROR_GENERIC;
+ goto out;
+ }
+
+ arg_size = OPTEE_MSG_GET_ARG_SIZE(shm_rpc->xen_arg->num_params);
+ if ( arg_size > OPTEE_MSG_NONCONTIG_PAGE_SIZE )
+ {
+ shm_rpc->xen_arg->ret = TEEC_ERROR_GENERIC;
+ goto out;
+ }
+
+ /* Read the whole command structure */
+ if ( access_guest_memory_by_ipa(current->domain, gfn_to_gaddr(shm_rpc->gfn),
+ shm_rpc->xen_arg, arg_size, false) )
+ {
+ shm_rpc->xen_arg->ret = TEEC_ERROR_GENERIC;
+ goto out;
+ }
+
+ switch (shm_rpc->xen_arg->cmd)
+ {
+ case OPTEE_RPC_CMD_GET_TIME:
+ case OPTEE_RPC_CMD_WAIT_QUEUE:
+ case OPTEE_RPC_CMD_SUSPEND:
+ break;
+ case OPTEE_RPC_CMD_SHM_ALLOC:
+ handle_rpc_cmd_alloc(ctx, regs, call, shm_rpc);
+ break;
+ case OPTEE_RPC_CMD_SHM_FREE:
+ free_optee_shm_buf(ctx, shm_rpc->xen_arg->params[0].u.value.b);
+ if ( call->rpc_data_cookie == shm_rpc->xen_arg->params[0].u.value.b )
+ call->rpc_data_cookie = 0;
+ break;
+ default:
+ break;
+ }
+
+out:
+ unmap_domain_page(shm_rpc->xen_arg);
+
+ do_call_with_arg(ctx, call, regs, OPTEE_SMC_CALL_RETURN_FROM_RPC, 0, 0,
+ get_user_reg(regs, 3), 0, 0);
+
+}
+
static void handle_rpc_func_alloc(struct optee_domain *ctx,
struct cpu_user_regs *regs,
struct optee_std_call *call)
@@ -1128,7 +1344,7 @@ static void handle_rpc_func_alloc(struct optee_domain *ctx,
ptr = 0;
}
else
- ptr = page_to_maddr(shm_rpc->guest_page);
+ ptr = page_to_maddr(shm_rpc->xen_arg_pg);
out:
uint64_to_regpair(&r1, &r2, ptr);
@@ -1174,8 +1390,8 @@ static void handle_rpc(struct optee_domain *ctx, struct cpu_user_regs *regs)
case OPTEE_SMC_RPC_FUNC_FOREIGN_INTR:
break;
case OPTEE_SMC_RPC_FUNC_CMD:
- /* TODO: Add handling */
- break;
+ handle_rpc_cmd(ctx, regs, call);
+ return;
}
do_call_with_arg(ctx, call, regs, OPTEE_SMC_CALL_RETURN_FROM_RPC,