optee: refactor RPC memory allocation function

Allow the logic used to handle allocating shared memory during RPCs to be
reusable for other purposes. Specifically, the code in question is
necessary to handle shared memory allocation requests for OCALLs.

This change refactors that code into a shared function.

Signed-off-by: Hernan Gatta <[email protected]>

Author: Hernan Gatta

drivers/tee/optee/rpc.c

@@ -192,14 +192,61 @@ static struct tee_shm *cmd_alloc_suppl(struct tee_context *ctx, size_t sz)
 	return shm;
 }
 
+int optee_rpc_process_shm_alloc(struct tee_shm *shm,
+				struct optee_msg_param *msg_param, void **list)
+{
+	phys_addr_t pa;
+	size_t sz;
+
+	struct page **pages;
+	size_t page_num;
+	void *pages_list;
+
+	if (tee_shm_get_pa(shm, 0, &pa))
+		return -EINVAL;
+
+	sz = tee_shm_get_size(shm);
+
+	if (tee_shm_is_registered(shm)) {
+		pages = tee_shm_get_pages(shm, &page_num);
+		if (!pages || !page_num)
+			return -EINVAL;
+
+		pages_list = optee_allocate_pages_list(page_num);
+		if (!pages_list)
+			return -ENOMEM;
+
+		msg_param->attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT |
+				  OPTEE_MSG_ATTR_NONCONTIG;
+		msg_param->u.tmem.buf_ptr =
+			virt_to_phys(pages_list) |
+			(tee_shm_get_page_offset(shm) &
+			 (OPTEE_MSG_NONCONTIG_PAGE_SIZE - 1));
+		msg_param->u.tmem.size = tee_shm_get_size(shm);
+		msg_param->u.tmem.shm_ref = (unsigned long)shm;
+		optee_fill_pages_list(pages_list, pages, page_num,
+				      tee_shm_get_page_offset(shm));
+		*list = pages_list;
+	} else {
+		msg_param->attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
+		msg_param->u.tmem.buf_ptr = pa;
+		msg_param->u.tmem.size = sz;
+		msg_param->u.tmem.shm_ref = (unsigned long)shm;
+		*list = NULL;
+	}
+
+	return 0;
+}
+
 static void handle_rpc_func_cmd_shm_alloc(struct tee_context *ctx,
 					  struct optee_msg_arg *arg,
 					  struct optee_call_ctx *call_ctx)
 {
-	phys_addr_t pa;
 	struct tee_shm *shm;
+	void *pages_list;
 	size_t sz;
 	size_t n;
+	int rc;
 
 	arg->ret_origin = TEEC_ORIGIN_COMMS;
 
@@ -237,52 +284,16 @@ static void handle_rpc_func_cmd_shm_alloc(struct tee_context *ctx,
 		return;
 	}
 
-	if (tee_shm_get_pa(shm, 0, &pa)) {
-		arg->ret = TEEC_ERROR_BAD_PARAMETERS;
+	rc = optee_rpc_process_shm_alloc(shm, &arg->params[0], &pages_list);
+	if (rc) {
+		arg->ret = rc == -ENOMEM
+			? TEEC_ERROR_OUT_OF_MEMORY : TEEC_ERROR_BAD_PARAMETERS;
 		goto bad;
 	}
 
-	sz = tee_shm_get_size(shm);
-
 	if (tee_shm_is_registered(shm)) {
-		struct page **pages;
-		u64 *pages_list;
-		size_t page_num;
-
-		pages = tee_shm_get_pages(shm, &page_num);
-		if (!pages || !page_num) {
-			arg->ret = TEEC_ERROR_OUT_OF_MEMORY;
-			goto bad;
-		}
-
-		pages_list = optee_allocate_pages_list(page_num);
-		if (!pages_list) {
-			arg->ret = TEEC_ERROR_OUT_OF_MEMORY;
-			goto bad;
-		}
-
 		call_ctx->pages_list = pages_list;
-		call_ctx->num_entries = page_num;
-
-		arg->params[0].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT |
-				      OPTEE_MSG_ATTR_NONCONTIG;
-		/*
-		 * In the least bits of u.tmem.buf_ptr we store buffer offset
-		 * from 4k page, as described in OP-TEE ABI.
-		 */
-		arg->params[0].u.tmem.buf_ptr = virt_to_phys(pages_list) |
-			(tee_shm_get_page_offset(shm) &
-			 (OPTEE_MSG_NONCONTIG_PAGE_SIZE - 1));
-		arg->params[0].u.tmem.size = tee_shm_get_size(shm);
-		arg->params[0].u.tmem.shm_ref = (unsigned long)shm;
-
-		optee_fill_pages_list(pages_list, pages, page_num,
-				      tee_shm_get_page_offset(shm));
-	} else {
-		arg->params[0].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
-		arg->params[0].u.tmem.buf_ptr = pa;
-		arg->params[0].u.tmem.size = sz;
-		arg->params[0].u.tmem.shm_ref = (unsigned long)shm;
+		call_ctx->num_entries = shm->num_pages;
 	}
 
 	arg->ret = TEEC_SUCCESS;