Reentrancy fixes

Before this patch: The normal world was only allowed to enter
secure world with one thread at a time.

After this patch: The normal world may try to enter secure world
with as many threads as it likes, secure world will return busy
when no more threads can be allowed. Secure world still only allows
one active thread at a time, but during RPC another thread may enter
and do some work. This is needed for cancellation to work.

* Adds a mutex that waits in normal world if busy
* Adds a new RPC service to wait in normal world
* Imports bitstring.h from FreeBSD to aid mutex implementation
* Adds a critical section in tee_ta_init_session
* Unmaps TA before RPC exit and maps it again on return to handle
  rescheduling of threads during RPC
* Doesn't clear a1-a3 when returning busy
* Bugfixes vector_std_smc_entry

This patch depends on the "Allow parallel entries to secure world"
patch in optee_linuxdriver.

2 files changed, 37 insertions, 4 deletions

diff --git a/core/arch/arm32/tee/arch_tee_fs.c b/core/arch/arm32/tee/arch_tee_fs.c
index 4ab94fc..8e5c785 100644
--- a/core/arch/arm32/tee/arch_tee_fs.c
+++ b/core/arch/arm32/tee/arch_tee_fs.c
@@ -34,12 +34,14 @@
 #include <mm/core_mmu.h>
 #include "tee_api_defines.h"
 #include <kernel/util.h>
+#include <kernel/tee_ta_manager.h>
 #include <kernel/thread.h>
 #include <sm/teesmc.h>
 
 int tee_fs_send_cmd(struct tee_fs_rpc *bf_cmd, void *data, uint32_t len,
 		    uint32_t mode)
 {
+	struct tee_ta_session *sess = NULL;
 	struct teesmc32_arg *arg;
 	struct teesmc32_param *params;
 	const size_t num_params = 1;
@@ -49,6 +51,9 @@ int tee_fs_send_cmd(struct tee_fs_rpc *bf_cmd, void *data, uint32_t len,
 	struct tee_fs_rpc *bf;
 	int res = -1;
 
+	tee_ta_get_current_session(&sess);
+	tee_ta_set_current_session(NULL);
+
 	pharg = thread_rpc_alloc_arg(TEESMC32_GET_ARG_SIZE(num_params));
 	thread_st_rpc_alloc_payload(sizeof(struct tee_fs_rpc) + len,
 					        &phpayload, &cookie);
@@ -78,8 +83,11 @@ int tee_fs_send_cmd(struct tee_fs_rpc *bf_cmd, void *data, uint32_t len,
 	/* fill in parameters */
 	*bf = *bf_cmd;
 
-	if (mode & TEE_FS_MODE_IN)
+	if (mode & TEE_FS_MODE_IN) {
+		tee_ta_set_current_session(sess);
 		memcpy((void *)(bf + 1), data, len);
+		tee_ta_set_current_session(NULL);
+	}
 
 	thread_rpc_cmd(pharg);
 	/* update result */
@@ -90,7 +98,9 @@ int tee_fs_send_cmd(struct tee_fs_rpc *bf_cmd, void *data, uint32_t len,
 	if (mode & TEE_FS_MODE_OUT) {
 		uint32_t olen = MIN(len, bf->len);
 
+		tee_ta_set_current_session(sess);
 		memcpy(data, (void *)(bf + 1), olen);
+		tee_ta_set_current_session(NULL);
 	}
 
 	res = 0;
@@ -98,5 +108,6 @@ int tee_fs_send_cmd(struct tee_fs_rpc *bf_cmd, void *data, uint32_t len,
 exit:
 	thread_rpc_free_arg(pharg);
 	thread_st_rpc_free_payload(cookie);
+	tee_ta_set_current_session(sess);
 	return res;
 }
diff --git a/core/arch/arm32/tee/tee_rpmb.c b/core/arch/arm32/tee/tee_rpmb.c
index f2cd6c9..eb94687 100644
--- a/core/arch/arm32/tee/tee_rpmb.c
+++ b/core/arch/arm32/tee/tee_rpmb.c
@@ -34,6 +34,7 @@
 #include <kernel/tee_common_otp.h>
 #include <kernel/tee_rpc.h>
 #include <kernel/thread.h>
+#include <kernel/tee_ta_manager.h>
 #include <tee/tee_rpmb.h>
 #include <kernel/chip_services.h>
 #include <tee/tee_hash.h>
@@ -317,6 +318,7 @@ static void tee_rpmb_free(struct tee_rpmb_mem *mem)
 {
 	if (!mem)
 		return;
+
 	thread_rpc_free_arg(mem->pharg);
 	thread_rpc_free_payload(mem->phpayload);
 	mem->pharg = 0;
@@ -911,7 +913,8 @@ func_exit:
 TEE_Result tee_rpmb_write_key(uint16_t dev_id, bool commercial)
 {
 	TEE_Result res = TEE_ERROR_GENERIC;
-	struct tee_rpmb_mem mem;
+	struct tee_ta_session *sess = NULL;
+	struct tee_rpmb_mem mem = { 0 };
 	uint16_t msg_type;
 	struct rpmb_req *req = NULL;
 	struct rpmb_data_frame *resp = NULL;
@@ -919,9 +922,12 @@ TEE_Result tee_rpmb_write_key(uint16_t dev_id, bool commercial)
 	uint32_t req_size;
 	uint32_t resp_size;
 
+	tee_ta_get_current_session(&sess);
+	tee_ta_set_current_session(NULL);
+
 	res = tee_rpmb_init(dev_id, true, commercial);
 	if (res != TEE_SUCCESS)
-		return res;
+		goto func_exit;
 
 	req_size = sizeof(struct rpmb_req) + RPMB_DATA_FRAME_SIZE;
 	resp_size = RPMB_DATA_FRAME_SIZE;
@@ -957,14 +963,16 @@ TEE_Result tee_rpmb_write_key(uint16_t dev_id, bool commercial)
 
 func_exit:
 	tee_rpmb_free(&mem);
+	tee_ta_set_current_session(sess);
 	return res;
 }
 
 TEE_Result tee_rpmb_read(uint16_t dev_id,
 			 uint32_t addr, uint8_t *data, uint32_t len)
 {
+	struct tee_ta_session *sess;
 	TEE_Result res = TEE_ERROR_GENERIC;
-	struct tee_rpmb_mem mem;
+	struct tee_rpmb_mem mem = { 0 };
 	uint16_t msg_type;
 	uint8_t nonce[RPMB_NONCE_SIZE];
 	uint8_t hmac[RPMB_KEY_MAC_SIZE];
@@ -980,6 +988,9 @@ TEE_Result tee_rpmb_read(uint16_t dev_id,
 	if (data == NULL || len == 0)
 		return TEE_ERROR_BAD_PARAMETERS;
 
+	tee_ta_get_current_session(&sess);
+	tee_ta_set_current_session(NULL);
+
 	blk_idx = addr / RPMB_DATA_SIZE;
 	byte_offset = addr % RPMB_DATA_SIZE;
 
@@ -1035,6 +1046,7 @@ TEE_Result tee_rpmb_read(uint16_t dev_id,
 
 func_exit:
 	tee_rpmb_free(&mem);
+	tee_ta_set_current_session(sess);
 	return res;
 }
 
@@ -1158,11 +1170,15 @@ TEE_Result tee_rpmb_write(uint16_t dev_id,
 			  uint32_t addr, uint8_t *data, uint32_t len)
 {
 	TEE_Result res = TEE_ERROR_GENERIC;
+	struct tee_ta_session *sess;
 	uint8_t *data_tmp = NULL;
 	uint16_t blk_idx;
 	uint16_t blkcnt;
 	uint8_t byte_offset;
 
+	tee_ta_get_current_session(&sess);
+	tee_ta_set_current_session(NULL);
+
 	blk_idx = addr / RPMB_DATA_SIZE;
 	byte_offset = addr % RPMB_DATA_SIZE;
 
@@ -1196,16 +1212,21 @@ TEE_Result tee_rpmb_write(uint16_t dev_id,
 
 func_exit:
 	free(data_tmp);
+	tee_ta_set_current_session(sess);
 	return res;
 }
 
 TEE_Result tee_rpmb_get_write_counter(uint16_t dev_id, uint32_t *counter)
 {
 	TEE_Result res;
+	struct tee_ta_session *sess;
 
 	if (counter == NULL)
 		return TEE_ERROR_BAD_PARAMETERS;
 
+	tee_ta_get_current_session(&sess);
+	tee_ta_set_current_session(NULL);
+
 	if (rpmb_ctx == NULL || !rpmb_ctx->wr_cnt_synced) {
 		res = tee_rpmb_init(dev_id, false, true);
 		if (res != TEE_SUCCESS)
@@ -1214,5 +1235,6 @@ TEE_Result tee_rpmb_get_write_counter(uint16_t dev_id, uint32_t *counter)
 
 	*counter = rpmb_ctx->wr_cnt;
 
+	tee_ta_set_current_session(sess);
 	return TEE_SUCCESS;
 }