PROTO: map UART, flash and shared page for StMMfwu_update

2 files changed, 66 insertions, 3 deletions

diff --git a/core/arch/arm/include/mm/pgt_cache.h b/core/arch/arm/include/mm/pgt_cache.h
index d19414e4..182b1654 100644
--- a/core/arch/arm/include/mm/pgt_cache.h
+++ b/core/arch/arm/include/mm/pgt_cache.h
@@ -43,11 +43,11 @@ struct pgt {
  * on how large the system might be.
  */
 #if CFG_NUM_THREADS < 2
-#define PGT_CACHE_SIZE	4
+#define PGT_CACHE_SIZE	4 + 8
 #elif (CFG_NUM_THREADS == 2 && !defined(CFG_WITH_LPAE))
-#define PGT_CACHE_SIZE	8
+#define PGT_CACHE_SIZE	8 + 8
 #else
-#define PGT_CACHE_SIZE	ROUNDUP(CFG_NUM_THREADS * 2, PGT_NUM_PGT_PER_PAGE)
+#define PGT_CACHE_SIZE	ROUNDUP(CFG_NUM_THREADS * 2, PGT_NUM_PGT_PER_PAGE) + 8
 #endif
 
 SLIST_HEAD(pgt_cache, pgt);
diff --git a/core/arch/arm/kernel/stmm_sp.c b/core/arch/arm/kernel/stmm_sp.c
index 895d7df2..f701c1a6 100644
--- a/core/arch/arm/kernel/stmm_sp.c
+++ b/core/arch/arm/kernel/stmm_sp.c
@@ -75,6 +75,67 @@ extern unsigned char stmm_image[];
 extern const unsigned int stmm_image_size;
 extern const unsigned int stmm_image_uncompressed_size;
 
+static TEE_Result alloc_SPD_io(struct stmm_ctx *spc, paddr_t pa,
+				   size_t sz, uint32_t prot, vaddr_t *va, \
+				   size_t pad_begin, size_t pad_end)
+{
+	struct mobj *mobj;
+	TEE_Result res = TEE_SUCCESS;
+	*va = 0x0;
+
+	sz = ROUNDUP(sz, SMALL_PAGE_SIZE);
+	mobj = mobj_phys_alloc(pa, sz, 0, CORE_MEM_SDP_MEM);
+
+	if (!mobj)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = vm_map_pad(&spc->uctx, va, sz,
+		      prot, 0, mobj,
+		      0, pad_begin, pad_end, 0);
+
+	if (res != TEE_SUCCESS) {
+		EMSG("failed to set S1 mapping PA %#lX <-> VA %#lX \n",pa, *va);
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result hack_map_StMM_devices(struct stmm_ctx *spc)
+{
+ 	TEE_Result res;
+ 	vaddr_t uart_va = 0;
+	res = alloc_SPD_io(spc, 0x09040000, 0x00001000,
+ 			       TEE_MATTR_URW | TEE_MATTR_PRW,
+ 			       &uart_va, 0, 0);
+ 	if (res) {
+ 		EMSG("failed to alloc_and_map uart");
+ 		return res;
+ 	}
+	EMSG("------------------------------uart va=%#"PRIxVA, uart_va);
+
+	// Secure variable storage
+	// base address at from [0x0, 0xA00000[
+	res = alloc_SPD_io(spc, 0x000000, 0xA00000,
+ 			       TEE_MATTR_URW | TEE_MATTR_PRW,
+ 			       &uart_va, 0, 0);
+ 	if (res) {
+ 		EMSG("failed to alloc_and_map secure flash");
+ 		return res;
+ 	}
+	EMSG("------------------------------secure variable storage base va=%#"PRIxVA, uart_va);
+
+	res = alloc_SPD_io(spc, 0xe05f000, 0x1000,
+ 			       TEE_MATTR_URW | TEE_MATTR_PRW,
+ 			       &uart_va, 0, 0);
+ 	if (res) {
+ 		EMSG("failed to alloc_and_map secure flash");
+ 		return res;
+ 	}
+	EMSG("------------------------------secure FWU page (SRAM) va=%#"PRIxVA, uart_va);
+
+	return res;
+}
 static struct stmm_ctx *stmm_alloc_ctx(const TEE_UUID *uuid)
 {
 	TEE_Result res = TEE_SUCCESS;
@@ -278,6 +339,8 @@ static TEE_Result load_stmm(struct stmm_ctx *spc)
 
 	DMSG("stmm load address %#"PRIxVA, image_addr);
 
+	hack_map_StMM_devices(spc);
+
 	boot_info = (struct stmm_boot_info *)sec_buf_addr;
 	mp_info = (struct stmm_mp_info *)(boot_info + 1);
 	*boot_info = (struct stmm_boot_info){