core: add support for (de)registering remote shmem

This patch utilizes new shared memory subsystem. Now we can:
 - map commands buffers that were not allocated in shm carveout
 - map data buffers allocated thru RPC
 - register and map data buffers provided by clients

Signed-off-by: Volodymyr Babchuk <vlad.babchuk@gmail.com>

core/arch/arm/include/kernel/misc.h

@@ -37,6 +37,34 @@ size_t get_core_pos(void);
 uint32_t read_mode_sp(int cpu_mode);
 uint32_t read_mode_lr(int cpu_mode);
 
+/**
+ * extract_pages_from_params() - extract list of pages from
+ * OPTEE_MSG_ATTR_FRAGMENT parameters
+ *
+ * @params:	pointer to parameters array
+ * @pages:	output array of page addresses
+ * @num_params: number of parameters in array
+ *
+ * return:
+ *   page count on success
+ *   <0 on error
+ */
+int extract_pages_from_params(struct optee_msg_param *params, paddr_t *pages,
+			      uint32_t num_pages);
+
+/**
+ * map_params_buffer() - map parameters buffer into OP-TEE VA space
+ * @pa_params - physical pointer to parameters
+ * @num_params - number of parameters
+ * @map_offset - offset that should be deducted before mapping buffer
+ *
+ * return:
+ *  struct shmem_mapping of mapped buffer on success
+ *  NULL on error.
+ */
+struct shmem_mapping *map_params_buffer(paddr_t pa_params, uint32_t num_params,
+					paddr_t map_offset);
+
 static inline uint64_t reg_pair_to_64(uint32_t reg0, uint32_t reg1)
 {
 	return (uint64_t)reg0 << 32 | reg1;

core/arch/arm/include/mm/mobj.h

@@ -39,6 +39,7 @@
 struct mobj {
 	const struct mobj_ops *ops;
 	size_t size;
+	size_t granule;
 };
 
 struct mobj_ops {
@@ -108,12 +109,25 @@ static inline bool mobj_is_secure(struct mobj *mobj)
 	return mobj_matches(mobj, CORE_MEM_SEC);
 }
 
+static inline size_t mobj_get_granule(struct mobj *mobj)
+{
+	if (mobj->granule)
+		return mobj->granule;
+	return mobj->size;
+}
+
 struct mobj *mobj_mm_alloc(struct mobj *mobj_parent, size_t size,
 			   tee_mm_pool_t *pool);
 
 struct mobj *mobj_phys_alloc(paddr_t pa, size_t size, uint32_t cattr,
 			     enum buf_is_attr battr);
 
+struct mobj *mobj_reg_shm_alloc(paddr_t *pages,
+				size_t num_pages,
+				uint64_t cookie);
+
+struct mobj *mobj_reg_shm_find_by_cookie(uint64_t cookie);
+
 struct mobj *mobj_paged_alloc(size_t size);
 
 #ifdef CFG_PAGED_USER_TA

core/arch/arm/include/sm/optee_smc.h

@@ -290,6 +290,8 @@
 #define OPTEE_SMC_SEC_CAP_HAVE_RESERVED_SHM	(1 << 0)
 /* Secure world can communicate via previously unregistered shared memory */
 #define OPTEE_SMC_SEC_CAP_UNREGISTERED_SHM	(1 << 1)
+/* Secure world supporst commands "register/unregister shared memory" */
+#define OPTEE_SMC_SEC_CAP_REGISTER_SHM		(1 << 2)
 #define OPTEE_SMC_FUNCID_EXCHANGE_CAPABILITIES	9
 #define OPTEE_SMC_EXCHANGE_CAPABILITIES \
 	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_EXCHANGE_CAPABILITIES)

core/arch/arm/kernel/misc.c

@@ -0,0 +1,151 @@
+/*
+ * Copyright (c) 2016, EPAM Systems
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <mm/shmem.h>
+#include <optee_msg.h>
+#include <sm/optee_smc.h>
+#include <sm/sm.h>
+#include <trace.h>
+#include <util.h>
+
+/**
+ * extract_pages_from_params() - extract list of pages from
+ * OPTEE_MSG_ATTR_FRAGMENT parameters
+ *
+ * @params:	pointer to parameters array
+ * @pages:	output array of page addresses
+ * @num_params: number of parameters in array
+ *
+ * return:
+ *   page count on success
+ *   <0 on error
+ */
+int extract_pages_from_params(struct optee_msg_param *params, paddr_t *pages,
+			      uint32_t num_params)
+{
+	uint32_t pages_cnt = 0;
+
+	if (params[num_params-1].attr & OPTEE_MSG_ATTR_FRAGMENT)
+		return -1;
+
+	for (uint32_t i = 0; i < num_params; i++) {
+		uint32_t attr = params[i].attr & OPTEE_MSG_ATTR_TYPE_MASK;
+
+		switch (attr) {
+		case OPTEE_MSG_ATTR_TYPE_NEXT_FRAGMENT:
+			continue;
+		case OPTEE_MSG_ATTR_TYPE_TMEM_INPUT:
+		case OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT:
+			if (pages_cnt >= num_params)
+				return -1;
+			if (!(params[i].attr & OPTEE_MSG_ATTR_FRAGMENT) &&
+			    (i != num_params - 1))
+				return -1;
+			pages[pages_cnt++] = params[i].u.tmem.buf_ptr
+				& ~SMALL_PAGE_MASK;
+			break;
+		default:
+			return -1;
+		}
+	}
+	return pages_cnt;
+}
+
+/**
+ * map_params_buffer() - map parameters buffer into OP-TEE VA space
+ * @pa_params - physical pointer to parameters
+ * @num_params - number of parameters
+ * @map_offset - offset that should be deducted before mapping buffer
+ *
+ * return:
+ *  struct shmem_mapping of mapped buffer on success
+ *  NULL on error.
+ */
+struct shmem_mapping *map_params_buffer(paddr_t pa_params, uint32_t num_params,
+					paddr_t map_offset)
+{
+	struct shmem_mapping *shm_mapping = NULL;
+	struct optee_msg_param *params;
+	paddr_t *pages = NULL;
+	uint32_t max_pages, pages_cnt;
+	size_t args_size;
+
+	if (shmem_map_page(pa_params, &shm_mapping))
+		return NULL;
+
+	args_size = OPTEE_MSG_GET_ARG_SIZE(num_params);
+	max_pages = args_size/SMALL_PAGE_SIZE + 1;
+	pages = calloc(max_pages, sizeof(paddr_t));
+	if (!pages)
+		goto err;
+
+	params = (struct optee_msg_param *) shmem_pa2va(pa_params);
+	pages[0] = pa_params & ~SMALL_PAGE_MASK;
+	pages_cnt = 1;
+	for (uint32_t i = 0; i < num_params; i++) {
+		if ((params->attr & OPTEE_MSG_ATTR_TYPE_MASK) ==
+		    OPTEE_MSG_ATTR_TYPE_NEXT_FRAGMENT) {
+			if (pages_cnt >= max_pages)
+				goto err;
+			pages[pages_cnt] = params->u.tmem.buf_ptr;
+			shmem_unmap_buffer(shm_mapping);
+			shm_mapping = NULL;
+
+			if (shmem_map_page(pages[pages_cnt], &shm_mapping))
+				goto err;
+
+			pages_cnt++;
+			params = (struct optee_msg_param *)
+				shmem_get_va(shm_mapping);
+		} else {
+			params++;
+			if (((vaddr_t)params & SMALL_PAGE_MASK) == 0)
+				goto err;
+		}
+	}
+
+	shmem_unmap_buffer(shm_mapping);
+	shm_mapping = NULL;
+
+	if (shmem_map_buffer(pa_params - map_offset, args_size,
+			     pages, pages_cnt, &shm_mapping))
+		goto err;
+
+	goto out;
+err:
+	if (shm_mapping)
+		shmem_unmap_buffer(shm_mapping);
+	shm_mapping = NULL;
+out:
+	if (pages)
+		free(pages);
+	return shm_mapping;
+
+}

core/arch/arm/kernel/sub.mk

@@ -2,6 +2,7 @@ srcs-$(CFG_WITH_USER_TA) += user_ta.c
 srcs-y += static_ta.c
 srcs-y += elf_load.c
 srcs-y += tee_time.c
+srcs-y += misc.c
 
 srcs-$(CFG_SECURE_TIME_SOURCE_CNTPCT) += tee_time_arm_cntpct.c
 srcs-$(CFG_SECURE_TIME_SOURCE_REE) += tee_time_ree.c

core/arch/arm/kernel/thread.c

@@ -41,6 +41,7 @@
 #include <mm/tee_mm.h>
 #include <mm/tee_mmu.h>
 #include <mm/tee_pager.h>
+#include <mm/shmem.h>
 #include <optee_msg.h>
 #include <sm/optee_smc.h>
 #include <sm/sm.h>
@@ -614,6 +615,7 @@ void __thread_std_smc_entry(struct thread_smc_args *args)
 	tee_fs_rpc_cache_clear(&thr->tsd);
 	if (!thread_prealloc_rpc_cache) {
 		thread_rpc_free_arg(thr->rpc_carg);
+		shmem_unmap_buffer(shmem_get_by_cookie(thr->rpc_carg));
 		thr->rpc_carg = 0;
 		thr->rpc_arg = 0;
 	}
@@ -1250,22 +1252,40 @@ void thread_rpc_free_arg(uint64_t cookie)
 
 void thread_rpc_alloc_arg(size_t size, paddr_t *arg, uint64_t *cookie)
 {
-	paddr_t pa;
+	paddr_t pa, page;
 	uint64_t co;
 	uint32_t rpc_args[THREAD_RPC_NUM_ARGS] = {
 		OPTEE_SMC_RETURN_RPC_ALLOC, size
 	};
+	struct shmem_mapping *mapping = NULL;
 
 	thread_rpc(rpc_args);
 
 	pa = reg_pair_to_64(rpc_args[1], rpc_args[2]);
 	co = reg_pair_to_64(rpc_args[4], rpc_args[5]);
-	if (!check_alloced_shm(pa, size, sizeof(uint64_t))) {
-		thread_rpc_free_arg(co);
-		pa = 0;
-		co = 0;
+
+	/* Check if this region is in static shared space */
+	if (!core_pbuf_is(CORE_MEM_NSEC_SHM, pa, size)) {
+		/* If not - then map it into dymanic shared space */
+		page = pa & ~SMALL_PAGE_MASK;
+		if (shmem_map_buffer(pa, size, &page,
+				     1, &mapping))
+			goto err;
+		shmem_set_cookie(mapping, co);
 	}
 
+	if (!check_alloced_shm(pa, size, sizeof(uint64_t)))
+		goto err;
+
+	*arg = pa;
+	*cookie = co;
+	return;
+err:
+	thread_rpc_free_arg(co);
+	pa = 0;
+	co = 0;
+	if (mapping)
+		shmem_unmap_buffer(mapping);
 	*arg = pa;
 	*cookie = co;
 }
@@ -1283,6 +1303,7 @@ static void thread_rpc_free(unsigned int bt, uint64_t cookie)
 	struct optee_msg_arg *arg = thr->rpc_arg;
 	uint64_t carg = thr->rpc_carg;
 	struct optee_msg_param *params = OPTEE_MSG_GET_PARAMS(arg);
+	struct shmem_mapping *mapping;
 
 	memset(arg, 0, OPTEE_MSG_GET_ARG_SIZE(1));
 	arg->cmd = OPTEE_MSG_RPC_CMD_SHM_FREE;
@@ -1295,6 +1316,8 @@ static void thread_rpc_free(unsigned int bt, uint64_t cookie)
 	params[0].u.value.c = 0;
 
 	reg_pair_from_64(carg, rpc_args + 1, rpc_args + 2);
+	mapping = shmem_get_by_cookie(cookie);
+	shmem_unmap_buffer(mapping);
 	thread_rpc(rpc_args);
 }
 
@@ -1316,6 +1339,8 @@ static void thread_rpc_alloc(size_t size, size_t align, unsigned int bt,
 	struct optee_msg_arg *arg = thr->rpc_arg;
 	uint64_t carg = thr->rpc_carg;
 	struct optee_msg_param *params = OPTEE_MSG_GET_PARAMS(arg);
+	struct shmem_mapping *mapping;
+	paddr_t *pages = NULL;
 
 	memset(arg, 0, OPTEE_MSG_GET_ARG_SIZE(1));
 	arg->cmd = OPTEE_MSG_RPC_CMD_SHM_ALLOC;
@@ -1335,17 +1360,58 @@ static void thread_rpc_alloc(size_t size, size_t align, unsigned int bt,
 	if (arg->num_params != 1)
 		goto fail;
 
-	if (params[0].attr != OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT)
-		goto fail;
+	if (!core_pbuf_is(CORE_MEM_NSEC_SHM,
+			  params[0].u.tmem.buf_ptr,
+			  params[0].u.tmem.size) &&
+	    (params[0].attr ==
+	     (OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT | OPTEE_MSG_ATTR_NESTED))) {
+
+		int num_pages = (params[0].u.tmem.size - 1)
+			/ SMALL_PAGE_SIZE + 1;
+		int num_params = num_pages +
+			((num_pages * sizeof(struct optee_msg_param)) /
+			 SMALL_PAGE_SIZE);
+
+		pages = malloc(num_pages * sizeof(paddr_t));
 
-	if (!check_alloced_shm(params[0].u.tmem.buf_ptr, size, align)) {
-		thread_rpc_free(bt, params[0].u.tmem.shm_ref);
+		if (!pages)
+			goto free_first;
+
+		mapping = map_params_buffer(params[0].u.tmem.buf_ptr,
+					    num_params, 0);
+
+		if (!mapping)
+			goto free_first;
+
+		if (extract_pages_from_params(shmem_get_va(mapping),
+					      pages,
+					      num_params) != num_pages)
+			goto free_first;
+		shmem_unmap_buffer(mapping);
+		mapping = NULL;
+
+		if (shmem_map_buffer(params[0].u.tmem.buf_ptr,
+				     params[0].u.tmem.size,
+				     pages, num_pages, &mapping))
+			goto free_first;
+
+		shmem_set_cookie(mapping, params[0].u.tmem.shm_ref);
+		free(pages);
+
+	} else if (!(params[0].attr & OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT))
 		goto fail;
-	}
 
 	*payload = params[0].u.tmem.buf_ptr;
 	*cookie = params[0].u.tmem.shm_ref;
+	/* Check if this region is in static shared space */
+	if (!core_pbuf_is(CORE_MEM_NSEC_SHM, *payload, size))
+		panic("This should not happen");
+
 	return;
+free_first:
+	if (pages)
+		free(pages);
+	thread_rpc_free(bt, params[0].u.tmem.shm_ref);
 fail:
 	*payload = 0;
 	*cookie = 0;

core/arch/arm/mm/core_mmu.c

@@ -825,6 +825,8 @@ static void set_pg_region(struct core_mmu_table_info *dir_info,
 
 		r.size = MIN(CORE_MMU_PGDIR_SIZE - (r.va - pg_info->va_base),
 			     end - r.va);
+		r.size = MIN(r.size, mobj_get_granule(region->mobj));
+
 		if (!mobj_is_paged(region->mobj)) {
 			size_t granule = BIT(pg_info->shift);
 			size_t offset = r.va - region->va + region->offset;