Merge changes I9feae1fc,I5cbe7192,I1867ece3,I85c2434a,If8edeeec, ... into integration

* changes:
  plat: marvell: armada: mcbin: squash several IO windows into one
  plat: marvell: armada: fix BL32 extra parameters usage
  drivers: marvell: Fix the LLC SRAM driver
  plat: marvell: armada: a8k: change CCU LLC SRAM mapping
  plat: marvell: armada: adjust trusted DRAM size to match OP-TEE OS
  drivers: marvell: mg_conf_cm3: pass comphy lane number to AP FW
  plat: marvell: armada: move mg conf related code to appropriate driver
  marvell: comphy: start AP FW when comphy AP mode selected
  drivers: marvell: mg_conf_cm3: add basic driver
  tools: doimage: change the binary image alignment to 16
  tools: doimage: migrate to mbedtls v2.8 APIs

docs/plat/marvell/armada/build.rst

@@ -79,10 +79,12 @@ There are several build options:
 
 - LLC_SRAM
 
-        Flag defining the LLC (L3) cache SRAM support. The feature is
-        disabled by default (``LLC_ENABLE=0``).
-        When LLC SRAM is enabled, the secure payload (BL32) is loaded into this
-        SRAM area instead of the DRAM.
+        Flag enabling the LLC (L3) cache SRAM support. The LLC SRAM is activated and used
+        by Trusted OS (OP-TEE OS, BL32). The TF-A only prepares CCU address translation windows
+        for SRAM address range at BL31 execution stage with window target set to DRAM-0.
+        When Trusted OS activates LLC SRAM, the CCU window target is changed to SRAM.
+        There is no reason to enable this feature if OP-TEE OS built with CFG_WITH_PAGER=n.
+        Only set LLC_SRAM=1 if OP-TEE OS is built with CFG_WITH_PAGER=y.
 
 - MARVELL_SECURE_BOOT
 

drivers/marvell/cache_llc.c

@@ -111,28 +111,36 @@ void llc_runtime_enable(int ap_index)
 }
 
 #if LLC_SRAM
-void llc_sram_enable(int ap_index)
+int llc_sram_enable(int ap_index, int size)
 {
-	uint32_t tc, way;
+	uint32_t tc, way, ways_to_allocate;
 	uint32_t way_addr;
 
+	if ((size <= 0) || (size > LLC_SIZE) || (size % LLC_WAY_SIZE))
+		return -1;
+
+	llc_enable(ap_index, 1);
+	llc_inv_all(ap_index);
+
+	ways_to_allocate = size / LLC_WAY_SIZE;
+
 	/* Lockdown all available ways for all traffic classes */
 	for (tc = 0; tc < LLC_TC_NUM; tc++)
-		mmio_write_32(LLC_TCN_LOCK(ap_index, tc), LLC_WAY_MASK);
+		mmio_write_32(LLC_TCN_LOCK(ap_index, tc), LLC_ALL_WAYS_MASK);
 
 	/* Clear the high bits of SRAM address */
 	mmio_write_32(LLC_BANKED_MNT_AHR(ap_index), 0);
 
 	way_addr = PLAT_MARVELL_TRUSTED_RAM_BASE;
-	for (way = 0; way < LLC_WAYS; way++) {
+	for (way = 0; way < ways_to_allocate; way++) {
 		/* Trigger allocation block command */
 		mmio_write_32(LLC_BLK_ALOC(ap_index),
 			      LLC_BLK_ALOC_BASE_ADDR(way_addr) |
-			      LLC_BLK_ALOC_WAY_DATA_CLR |
+			      LLC_BLK_ALOC_WAY_DATA_SET |
 			      LLC_BLK_ALOC_WAY_ID(way));
 		way_addr += LLC_WAY_SIZE;
 	}
-	llc_enable(ap_index, 1);
+	return 0;
 }
 
 void llc_sram_disable(int ap_index)
@@ -146,4 +154,36 @@ void llc_sram_disable(int ap_index)
 	/* Invalidate all ways */
 	llc_inv_all(ap_index);
 }
+
+int llc_sram_test(int ap_index, int size, char *msg)
+{
+	uintptr_t addr, end_addr;
+	uint32_t data = 0;
+
+	if ((size <= 0) || (size > LLC_SIZE))
+		return -1;
+
+	INFO("=== LLC SRAM WRITE test %s\n", msg);
+	for (addr = PLAT_MARVELL_TRUSTED_RAM_BASE,
+	     end_addr = PLAT_MARVELL_TRUSTED_RAM_BASE + size;
+	     addr < end_addr; addr += 4) {
+		mmio_write_32(addr, addr);
+	}
+	INFO("=== LLC SRAM WRITE test %s PASSED\n", msg);
+	INFO("=== LLC SRAM READ test %s\n", msg);
+	for (addr = PLAT_MARVELL_TRUSTED_RAM_BASE,
+	     end_addr = PLAT_MARVELL_TRUSTED_RAM_BASE + size;
+	     addr < end_addr; addr += 4) {
+		data = mmio_read_32(addr);
+		if (data != addr) {
+			INFO("=== LLC SRAM READ test %s FAILED @ 0x%08lx)\n",
+			     msg, addr);
+			return -1;
+		}
+	}
+	INFO("=== LLC SRAM READ test %s PASSED (last read = 0x%08x)\n",
+	     msg, data);
+	return 0;
+}
+
 #endif /* LLC_SRAM */

drivers/marvell/comphy/phy-comphy-cp110.c

@@ -11,6 +11,7 @@
 
 #include <common/debug.h>
 #include <drivers/delay_timer.h>
+#include <mg_conf_cm3/mg_conf_cm3.h>
 #include <lib/mmio.h>
 #include <lib/spinlock.h>
 
@@ -2231,6 +2232,7 @@ static int mvebu_cp110_comphy_ap_power_on(uint64_t comphy_base,
 					  uint32_t comphy_mode)
 {
 	uint32_t mask, data;
+	uint8_t ap_nr, cp_nr;
 	uintptr_t comphy_addr = comphy_addr =
 				COMPHY_ADDR(comphy_base, comphy_index);
 
@@ -2247,6 +2249,10 @@ static int mvebu_cp110_comphy_ap_power_on(uint64_t comphy_base,
 	reg_set(comphy_addr + COMMON_PHY_CFG1_REG, data, mask);
 	debug_exit();
 
+	/* Start AP Firmware */
+	mvebu_cp110_get_ap_and_cp_nr(&ap_nr, &cp_nr, comphy_base);
+	mg_start_ap_fw(cp_nr, comphy_index);
+
 	return 0;
 }
 

drivers/marvell/mg_conf_cm3/mg_conf_cm3.c

+/*
+ * Copyright (C) 2019 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <a8k_plat_def.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <mss_scp_bl2_format.h>
+
+/* CONFI REGISTERS */
+#define MG_CM3_CONFI_BASE(CP)		(MVEBU_CP_REGS_BASE(CP) + 0x100000)
+#define MG_CM3_SRAM_BASE(CP)		MG_CM3_CONFI_BASE(CP)
+#define MG_CM3_CONFI_GLOB_CFG_REG(CP)	(MG_CM3_CONFI_BASE(CP) + 0x2B500)
+#define CM3_CPU_EN_BIT			BIT(28)
+#define MG_CM3_MG_INT_MFX_REG(CP)	(MG_CM3_CONFI_BASE(CP) + 0x2B054)
+#define CM3_SYS_RESET_BIT		BIT(0)
+
+#define MG_CM3_SHARED_MEM_BASE(CP)	(MG_CM3_SRAM_BASE(CP) + 0x1FC00ULL)
+
+#define MG_SRAM_SIZE	0x20000 /* 128KB */
+
+#define MG_ACK_TIMEOUT 10
+
+/**
+ * struct ap_sharedmem_ctrl - used to pass information between the HOST and CM3
+ * @init_done:	Set by CM3 when ap_proces initialzied. Host check if CM3 set
+ *		this flag to confirm that the process is running
+ * @lane_nr:	Set by Host to mark which comphy lane should be configure. E.g.:
+ *		- A8K development board uses comphy lane 2 for eth0
+ *		- CN913x development board uses comphy lane 4 for eth0
+ */
+struct ap_sharedmem_ctrl {
+	uint32_t init_done;
+	uint32_t lane_nr;
+};
+
+int mg_image_load(uintptr_t src_addr, uint32_t size, int cp_index)
+{
+	uintptr_t mg_regs = MG_CM3_SRAM_BASE(cp_index);
+
+	if (size > MG_SRAM_SIZE) {
+		ERROR("image is too big to fit into MG CM3 memory\n");
+		return 1;
+	}
+
+	NOTICE("Loading MG image from address 0x%lx Size 0x%x to MG at 0x%lx\n",
+	       src_addr, size, mg_regs);
+
+	/* Copy image to MG CM3 SRAM */
+	memcpy((void *)mg_regs, (void *)src_addr, size);
+
+	/* Don't release MG CM3 from reset - it will be done by next step
+	 * bootloader (e.g. U-Boot), when appriopriate device-tree setup (which
+	 * has enabeld 802.3. auto-neg) will be choosen.
+	 */
+
+	return 0;
+}
+
+void mg_start_ap_fw(int cp_nr, uint8_t comphy_index)
+{
+	volatile struct ap_sharedmem_ctrl *ap_shared_ctrl =
+					(void *)MG_CM3_SHARED_MEM_BASE(cp_nr);
+	int timeout = MG_ACK_TIMEOUT;
+
+	if (mmio_read_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr)) & CM3_CPU_EN_BIT) {
+		VERBOSE("cm3 already running\n");
+		return;  /* cm3 already running */
+	}
+
+	/*
+	 * Mark which comphy lane should be used - it will be read via shared
+	 * mem by ap process
+	 */
+	ap_shared_ctrl->lane_nr = comphy_index;
+	/* Make sure it took place before enabling cm3 */
+	dmbst();
+
+	mmio_setbits_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr), CM3_CPU_EN_BIT);
+	mmio_setbits_32(MG_CM3_MG_INT_MFX_REG(cp_nr), CM3_SYS_RESET_BIT);
+
+	/* Check for ap process initialization by fw */
+	while (ap_shared_ctrl->init_done != 1 && timeout--)
+		VERBOSE("Waiting for ap process ack, timeout %d\n", timeout);
+
+	if (timeout == 0) {
+		ERROR("AP process failed, disabling cm3\n");
+		mmio_clrbits_32(MG_CM3_MG_INT_MFX_REG(cp_nr),
+				CM3_SYS_RESET_BIT);
+		mmio_clrbits_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr),
+				CM3_CPU_EN_BIT);
+	}
+}

drivers/marvell/mg_conf_cm3/mg_conf_cm3.h

+/*
+ * Copyright (C) 2019 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+void mg_start_ap_fw(int cp_nr, uint8_t comphy_index);
+int mg_image_load(uintptr_t src_addr, uint32_t size, int cp_index);

include/drivers/marvell/cache_llc.h

@@ -41,7 +41,7 @@
 #define LLC_BLK_ALOC_WAY_DATA_DSBL	(0x0 << 6)
 #define LLC_BLK_ALOC_WAY_DATA_CLR	(0x1 << 6)
 #define LLC_BLK_ALOC_WAY_DATA_SET	(0x3 << 6)
-#define LLC_BLK_ALOC_BASE_ADDR(addr)	((addr) & (LLC_WAY_SIZE - 1))
+#define LLC_BLK_ALOC_BASE_ADDR(addr)	((addr) & ~(LLC_WAY_SIZE - 1))
 
 #ifndef __ASSEMBLER__
 void llc_cache_sync(int ap_index);
@@ -53,8 +53,9 @@ void llc_enable(int ap_index, int excl_mode);
 int llc_is_exclusive(int ap_index);
 void llc_runtime_enable(int ap_index);
 #if LLC_SRAM
-void llc_sram_enable(int ap_index);
+int llc_sram_enable(int ap_index, int size);
 void llc_sram_disable(int ap_index);
+int llc_sram_test(int ap_index, int size, char *msg);
 #endif /* LLC_SRAM */
 #endif /* __ASSEMBLY__ */
 

plat/marvell/armada/a3k/common/include/platform_def.h

@@ -83,9 +83,11 @@
 #define PLAT_MARVELL_TRUSTED_ROM_BASE		PLAT_MARVELL_ATF_LOAD_ADDR
 /* 4 MB for FIP image */
 #define PLAT_MARVELL_TRUSTED_ROM_SIZE		0x00400000
-/* Reserve 16M for SCP (Secure PayLoad) Trusted DRAM */
+/* Reserve 12M for SCP (Secure PayLoad) Trusted RAM
+ * OP-TEE SHMEM follows this region
+ */
 #define PLAT_MARVELL_TRUSTED_RAM_BASE		0x04400000
-#define PLAT_MARVELL_TRUSTED_RAM_SIZE		0x01000000	/* 16 MB */
+#define PLAT_MARVELL_TRUSTED_RAM_SIZE		0x00C00000	/* 12 MB DRAM */
 
 /*
  * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size

plat/marvell/armada/a8k/a70x0/board/marvell_plat_config.c

@@ -103,7 +103,10 @@ struct addr_map_win ccu_memory_map[] = {	/* IO window */
 	{0x00000000f2000000,	0x4000000,	IO_0_TID}, /* IO window */
 #else
 #if LLC_SRAM
-	{PLAT_MARVELL_LLC_SRAM_BASE, PLAT_MARVELL_LLC_SRAM_SIZE, SRAM_TID},
+	/* This entry is prepared for OP-TEE OS that enables the LLC SRAM
+	 * and changes the window target to SRAM_TID.
+	 */
+	{PLAT_MARVELL_LLC_SRAM_BASE, PLAT_MARVELL_LLC_SRAM_SIZE, DRAM_0_TID},
 #endif
 	{0x00000000f2000000,	0xe000000,	IO_0_TID},
 	{0x00000000c0000000,	0x30000000,	IO_0_TID}, /* IO window */

plat/marvell/armada/a8k/a70x0_amc/board/marvell_plat_config.c

@@ -94,7 +94,10 @@ struct addr_map_win ccu_memory_map[] = {
 	{0x00000000f2000000,	0x4000000,	IO_0_TID}, /* IO window */
 #else
 #if LLC_SRAM
-	{PLAT_MARVELL_LLC_SRAM_BASE, PLAT_MARVELL_LLC_SRAM_SIZE, SRAM_TID},
+	/* This entry is prepared for OP-TEE OS that enables the LLC SRAM
+	 * and changes the window target to SRAM_TID.
+	 */
+	{PLAT_MARVELL_LLC_SRAM_BASE, PLAT_MARVELL_LLC_SRAM_SIZE, DRAM_0_TID},
 #endif
 	{0x00000000f2000000,	0xe000000,	IO_0_TID},
 	{0x00000000c0000000,    0x30000000,	IO_0_TID}, /* IO window */

plat/marvell/armada/a8k/a80x0/board/marvell_plat_config.c

@@ -132,7 +132,10 @@ struct addr_map_win ccu_memory_map[] = {
 	{0x00000000f2000000,	0x4000000,  IO_0_TID}, /* IO window */
 #else
 #if LLC_SRAM
-	{PLAT_MARVELL_LLC_SRAM_BASE, PLAT_MARVELL_LLC_SRAM_SIZE, SRAM_TID},
+	/* This entry is prepared for OP-TEE OS that enables the LLC SRAM
+	 * and changes the window target to SRAM_TID.
+	 */
+	{PLAT_MARVELL_LLC_SRAM_BASE, PLAT_MARVELL_LLC_SRAM_SIZE, DRAM_0_TID},
 #endif
 	{0x00000000f2000000,	0xe000000,  IO_0_TID}, /* IO window */
 	{0x00000000c0000000,	0x30000000,  IO_0_TID}, /* IO window */

plat/marvell/armada/a8k/a80x0_mcbin/board/marvell_plat_config.c

@@ -78,12 +78,8 @@ struct addr_map_win io_win_memory_map[] = {
 	/* CP1 (MCI0) internal regs */
 	{0x00000000f4000000,		0x2000000,  MCI_0_TID},
 #ifndef IMAGE_BLE
-	/* PCIe0 and SPI1_CS0 (RUNIT) on CP1*/
-	{0x00000000f9000000,		0x2000000,  MCI_0_TID},
-	/* PCIe1 on CP1*/
-	{0x00000000fb000000,		0x1000000,  MCI_0_TID},
-	/* PCIe2 on CP1*/
-	{0x00000000fc000000,		0x1000000,  MCI_0_TID},
+	/* PCIe0-2 and SPI1_CS0 (RUNIT) on CP1*/
+	{0x00000000f9000000,		0x4000000,  MCI_0_TID},
 	/* MCI 0 indirect window */
 	{MVEBU_MCI_REG_BASE_REMAP(0),	0x100000,   MCI_0_TID},
 	/* MCI 1 indirect window */
@@ -166,7 +162,10 @@ struct addr_map_win ccu_memory_map[] = {
 	{0x00000000f2000000,	0x4000000,  IO_0_TID}, /* IO window */
 #else
 #if LLC_SRAM
-	{PLAT_MARVELL_LLC_SRAM_BASE, PLAT_MARVELL_LLC_SRAM_SIZE, SRAM_TID},
+	/* This entry is prepared for OP-TEE OS that enables the LLC SRAM
+	 * and changes the window target to SRAM_TID.
+	 */
+	{PLAT_MARVELL_LLC_SRAM_BASE, PLAT_MARVELL_LLC_SRAM_SIZE, DRAM_0_TID},
 #endif
 	{0x00000000f2000000,	0xe000000,  IO_0_TID}, /* IO window */
 	{0x00000000c0000000,	0x30000000,  IO_0_TID}, /* IO window */

plat/marvell/armada/a8k/common/a8k_common.mk

@@ -85,7 +85,8 @@ MARVELL_DRV		:= 	$(MARVELL_DRV_BASE)/io_win.c	\
 				$(MARVELL_DRV_BASE)/ccu.c	\
 				$(MARVELL_DRV_BASE)/cache_llc.c	\
 				$(MARVELL_DRV_BASE)/comphy/phy-comphy-cp110.c \
-				$(MARVELL_DRV_BASE)/mc_trustzone/mc_trustzone.c
+				$(MARVELL_DRV_BASE)/mc_trustzone/mc_trustzone.c \
+				$(MARVELL_DRV_BASE)/mg_conf_cm3/mg_conf_cm3.c
 
 BL31_PORTING_SOURCES	:=	$(BOARD_DIR)/board/marvell_plat_config.c
 

plat/marvell/armada/a8k/common/include/platform_def.h

@@ -96,11 +96,13 @@
 #define PLAT_MARVELL_TRUSTED_ROM_BASE		PLAT_MARVELL_ATF_LOAD_ADDR
 /* 4 MB for FIP image */
 #define PLAT_MARVELL_TRUSTED_ROM_SIZE		0x00400000
-/* Reserve 16M for SCP (Secure PayLoad) Trusted RAM */
+/* Reserve 12MB for SCP (Secure PayLoad) Trusted RAM
+ * OP-TEE 4MB SHMEM follows this region
+ */
 #define PLAT_MARVELL_TRUSTED_RAM_BASE		0x04400000
-#define PLAT_MARVELL_TRUSTED_RAM_SIZE		0x01000000	/* 16 MB DRAM */
+#define PLAT_MARVELL_TRUSTED_RAM_SIZE		0x00C00000	/* 12 MB DRAM */
 
-#define PLAT_MARVELL_LLC_SRAM_BASE		PLAT_MARVELL_TRUSTED_RAM_BASE
+#define PLAT_MARVELL_LLC_SRAM_BASE		0x05400000
 #define PLAT_MARVELL_LLC_SRAM_SIZE		0x00100000	/* 1 MB SRAM */
 
 /*

plat/marvell/armada/common/aarch64/marvell_bl2_mem_params_desc.c

@@ -100,6 +100,45 @@ static bl_mem_params_node_t bl2_mem_params_descs[] = {
 
 	    .next_handoff_image_id = BL33_IMAGE_ID,
     },
+
+	/*
+	 * Fill BL32 external 1 related information.
+	 * A typical use for extra1 image is with OP-TEE
+	 * where it is the pager image.
+	 */
+	{
+	    .image_id = BL32_EXTRA1_IMAGE_ID,
+
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+	    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+	    .image_info.image_base = BL32_BASE,
+	    .image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+
+	    .next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+	/*
+	 * Fill BL32 external 2 related information.
+	 * A typical use for extra2 image is with OP-TEE,
+	 * where it is the paged image.
+	 */
+	{
+	    .image_id = BL32_EXTRA2_IMAGE_ID,
+
+	    SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+		    VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+
+	    SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+		    VERSION_2, image_info_t, IMAGE_ATTRIB_SKIP_LOADING),
+#ifdef SPD_opteed
+	    .image_info.image_base = MARVELL_OPTEE_PAGEABLE_LOAD_BASE,
+	    .image_info.image_max_size = MARVELL_OPTEE_PAGEABLE_LOAD_SIZE,
+#endif
+	    .next_handoff_image_id = INVALID_IMAGE_ID,
+	},
 # endif /* BL32_BASE */
 
 	/* Fill BL33 related information */

plat/marvell/armada/common/marvell_bl2_setup.c

@@ -17,6 +17,9 @@
 #include <drivers/console.h>
 #include <lib/utils.h>
 
+#ifdef SPD_opteed
+#include <optee_utils.h>
+#endif
 #include <marvell_def.h>
 #include <plat_marvell.h>
 
@@ -97,9 +100,29 @@ int marvell_bl2_handle_post_image_load(unsigned int image_id)
 	int err = 0;
 	bl_mem_params_node_t *bl_mem_params = get_bl_mem_params_node(image_id);
 
+#ifdef SPD_opteed
+	bl_mem_params_node_t *pager_mem_params = NULL;
+	bl_mem_params_node_t *paged_mem_params = NULL;
+#endif /* SPD_opteed */
 	assert(bl_mem_params);
 
 	switch (image_id) {
+	case BL32_IMAGE_ID:
+#ifdef SPD_opteed
+		pager_mem_params = get_bl_mem_params_node(BL32_EXTRA1_IMAGE_ID);
+		assert(pager_mem_params);
+
+		paged_mem_params = get_bl_mem_params_node(BL32_EXTRA2_IMAGE_ID);
+		assert(paged_mem_params);
+
+		err = parse_optee_header(&bl_mem_params->ep_info,
+					 &pager_mem_params->image_info,
+					 &paged_mem_params->image_info);
+		if (err != 0)
+			WARN("OPTEE header parse error.\n");
+#endif /* SPD_opteed */
+		bl_mem_params->ep_info.spsr = marvell_get_spsr_for_bl32_entry();
+		break;
 
 	case BL33_IMAGE_ID:
 		/* BL33 expects to receive the primary CPU MPID (through r0) */

plat/marvell/armada/common/marvell_common.mk

@@ -22,15 +22,7 @@ LLC_SRAM			:= 0
 $(eval $(call add_define,LLC_SRAM))
 
 # Enable/Disable LLC
-ifeq (${LLC_SRAM}, 0)
 LLC_ENABLE			:= 1
-else
-# When LLC_SRAM=1, the entire LLC converted to SRAM and enabled at BL1.
-# All existing cases activating LLC at BL31 stage should be disabled.
-# The below assignment does not allow changing the LLC_ENABLE
-# value in the command line.
-LLC_ENABLE			= 0
-endif
 $(eval $(call add_define,LLC_ENABLE))
 
 include lib/xlat_tables_v2/xlat_tables.mk
@@ -66,6 +58,10 @@ BL2_SOURCES		+=	drivers/io/io_fip.c					\
 				$(MARVELL_PLAT_BASE)/common/aarch64/marvell_bl2_mem_params_desc.c	\
 				$(MARVELL_PLAT_BASE)/common/marvell_image_load.c
 
+ifeq (${SPD},opteed)
+PLAT_INCLUDES		+=	-Iinclude/lib
+BL2_SOURCES		+=	lib/optee/optee_utils.c
+endif
 
 BL31_SOURCES		+=	$(MARVELL_PLAT_BASE)/common/marvell_bl31_setup.c	\
 				$(MARVELL_PLAT_BASE)/common/marvell_pm.c		\
@@ -77,6 +73,15 @@ BL31_SOURCES		+=	$(MARVELL_PLAT_BASE)/common/marvell_bl31_setup.c	\
 # PSCI functionality
 $(eval $(call add_define,CONFIG_ARM64))
 
+# Add the build options to pack Trusted OS Extra1 and Trusted OS Extra2 images
+# in the FIP if the platform requires.
+ifneq ($(BL32_EXTRA1),)
+$(eval $(call TOOL_ADD_IMG,bl32_extra1,--tos-fw-extra1))
+endif
+ifneq ($(BL32_EXTRA2),)
+$(eval $(call TOOL_ADD_IMG,bl32_extra2,--tos-fw-extra2))
+endif
+
 # MSS (SCP) build
 ifeq (${MSS_SUPPORT}, 1)
 include $(MARVELL_PLAT_BASE)/common/mss/mss_common.mk

plat/marvell/armada/common/marvell_io_storage.c

@@ -43,6 +43,15 @@ static const io_uuid_spec_t bl31_uuid_spec = {
 static const io_uuid_spec_t bl32_uuid_spec = {
 	.uuid = UUID_SECURE_PAYLOAD_BL32,
 };
+
+static const io_uuid_spec_t bl32_extra1_uuid_spec = {
+	.uuid = UUID_SECURE_PAYLOAD_BL32_EXTRA1,
+};
+
+static const io_uuid_spec_t bl32_extra2_uuid_spec = {
+	.uuid = UUID_SECURE_PAYLOAD_BL32_EXTRA2,
+};
+
 static const io_uuid_spec_t bl33_uuid_spec = {
 	.uuid = UUID_NON_TRUSTED_FIRMWARE_BL33,
 };
@@ -83,6 +92,16 @@ static const struct plat_io_policy policies[] = {
 		(uintptr_t)&bl32_uuid_spec,
 		open_fip
 	},
+	[BL32_EXTRA1_IMAGE_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl32_extra1_uuid_spec,
+		open_fip
+	},
+	[BL32_EXTRA2_IMAGE_ID] = {
+		&fip_dev_handle,
+		(uintptr_t)&bl32_extra2_uuid_spec,
+		open_fip
+	},
 	[BL33_IMAGE_ID] = {
 		&fip_dev_handle,
 		(uintptr_t)&bl33_uuid_spec,

plat/marvell/armada/common/mss/mss_scp_bl2_format.h

@@ -13,7 +13,6 @@
 #define HEADER_VERSION	0x1
 
 #define MSS_IDRAM_SIZE	0x10000 /* 64KB */
-#define MG_SRAM_SIZE	0x20000 /* 128KB */
 
 /* Types definitions */
 typedef struct file_header {

plat/marvell/armada/common/mss/mss_scp_bootloader.c

@@ -12,6 +12,7 @@
 #include <arch_helpers.h>
 #include <common/debug.h>
 #include <drivers/delay_timer.h>
+#include <mg_conf_cm3/mg_conf_cm3.h>
 #include <lib/mmio.h>
 
 #include <plat_pm_trace.h>
@@ -42,8 +43,6 @@
 
 #define MSS_HANDSHAKE_TIMEOUT		50
 
-#define MG_CM3_SRAM_BASE(CP)		(MVEBU_CP_REGS_BASE(CP) + 0x100000)
-
 static int mss_check_image_ready(volatile struct mss_pm_ctrl_block *mss_pm_crtl)
 {
 	int timeout = MSS_HANDSHAKE_TIMEOUT;
@@ -61,28 +60,6 @@ static int mss_check_image_ready(volatile struct mss_pm_ctrl_block *mss_pm_crtl)
 	return 0;
 }
 
-static int mg_image_load(uintptr_t src_addr, uint32_t size, uintptr_t mg_regs)
-{
-	if (size > MG_SRAM_SIZE) {
-		ERROR("image is too big to fit into MG CM3 memory\n");
-		return 1;
-	}
-
-	NOTICE("Loading MG image from address 0x%lx Size 0x%x to MG at 0x%lx\n",
-	       src_addr, size, mg_regs);
-
-	/* Copy image to MG CM3 SRAM */
-	memcpy((void *)mg_regs, (void *)src_addr, size);
-
-	/*
-	 * Don't release MG CM3 from reset - it will be done by next step
-	 * bootloader (e.g. U-Boot), when appriopriate device-tree setup (which
-	 * has enabeld 802.3. auto-neg) will be choosen.
-	 */
-
-	return 0;
-}
-
 static int mss_image_load(uint32_t src_addr, uint32_t size, uintptr_t mss_regs)
 {
 	uint32_t i, loop_num, timeout;
@@ -258,8 +235,7 @@ static int load_img_to_cm3(enum cm3_t cm3_type,
 			break;
 		}
 		NOTICE("Load image to CP%d MG\n", cp_index);
-		ret = mg_image_load(single_img, image_size,
-				    MG_CM3_SRAM_BASE(cp_index));
+		ret = mg_image_load(single_img, image_size, cp_index);
 		if (ret != 0) {
 			ERROR("SCP Image load failed\n");
 			return -1;

tools/marvell/doimage/doimage.c

@@ -51,7 +51,7 @@
 /* Number of address pairs in control array */
 #define CP_CTRL_EL_ARRAY_SZ	32
 
-#define VERSION_STRING		"Marvell(C) doimage utility version 3.2"
+#define VERSION_STRING		"Marvell(C) doimage utility version 3.3"
 
 /* A8K definitions */
 
@@ -303,7 +303,7 @@ int create_rsa_signature(mbedtls_pk_context	*pk_ctx,
 				MBEDTLS_RSA_PKCS_V21, MBEDTLS_MD_SHA256);
 
 	/* First compute the SHA256 hash for the input blob */
-	mbedtls_sha256(input, ilen, hash, 0);
+	mbedtls_sha256_ret(input, ilen, hash, 0);
 
 	/* Then calculate the hash signature */
 	rval = mbedtls_rsa_rsassa_pss_sign(mbedtls_pk_rsa(*pk_ctx),
@@ -354,6 +354,7 @@ int verify_rsa_signature(const unsigned char	*pub_key,
 	mbedtls_pk_context		pk_ctx;
 	unsigned char			hash[32];
 	int				rval;
+	unsigned char			*pkey = (unsigned char *)pub_key;
 
 	/* Not sure this is required,
 	 * but it's safer to start with empty buffer
@@ -373,8 +374,7 @@ int verify_rsa_signature(const unsigned char	*pub_key,
 	}
 
 	/* Check ability to read the public key */
-	rval = mbedtls_pk_parse_public_key(&pk_ctx, pub_key,
-					   MAX_RSA_DER_BYTE_LEN);
+	rval = mbedtls_pk_parse_subpubkey(&pkey, pub_key + klen, &pk_ctx);
 	if (rval != 0) {
 		fprintf(stderr, " Failed in pk_parse_public_key (%#x)!\n",
 			rval);
@@ -387,7 +387,7 @@ int verify_rsa_signature(const unsigned char	*pub_key,
 				MBEDTLS_MD_SHA256);
 
 	/* Compute the SHA256 hash for the input buffer */
-	mbedtls_sha256(input, ilen, hash, 0);
+	mbedtls_sha256_ret(input, ilen, hash, 0);
 
 	rval = mbedtls_rsa_rsassa_pss_verify(mbedtls_pk_rsa(pk_ctx),
 					     mbedtls_ctr_drbg_random,
@@ -458,7 +458,7 @@ int image_encrypt(uint8_t *buf, uint32_t blen)
 	/* compute SHA-256 digest of the results
 	 * and use it as the init vector (IV)
 	 */
-	mbedtls_sha256(IV, AES_BLOCK_SZ, digest, 0);
+	mbedtls_sha256_ret(IV, AES_BLOCK_SZ, digest, 0);
 	memcpy(IV, digest, AES_BLOCK_SZ);
 	mbedtls_aes_setkey_enc(&aes_ctx, opts.sec_opts->aes_key,
 			       AES_KEY_BIT_LEN);
@@ -880,11 +880,13 @@ int format_sec_ext(char *filename, FILE *out_fd)
 				fname);
 			return 1;
 		}
+
 		/* Data in the output buffer is aligned to the buffer end */
 		der_buf_start = output_buf + sizeof(output_buf) - output_len;
 		/* In the header DER data is aligned
 		 * to the start of appropriate field
 		 */
+		bzero(out_der_key, MAX_RSA_DER_BYTE_LEN);
 		memcpy(out_der_key, der_buf_start, output_len);
 
 	} /* for every private key file */
@@ -899,8 +901,10 @@ int format_sec_ext(char *filename, FILE *out_fd)
 		fprintf(stderr, "Failed to sign CSK keys block!\n");
 		return 1;
 	}
+
 	/* Check that everything is correct */
-	if (verify_rsa_signature(sec_ext.kak_key, MAX_RSA_DER_BYTE_LEN,
+	if (verify_rsa_signature(sec_ext.kak_key,
+				 MAX_RSA_DER_BYTE_LEN,
 				 &sec_ext.csk_keys[0][0],
 				 sizeof(sec_ext.csk_keys),
 				 opts.sec_opts->kak_key_file,
@@ -1333,7 +1337,7 @@ int parse_image(uint8_t *buf, int size)
 					goto error;
 				}
 
-				mbedtls_sha256(sec_entry->kak_key,
+				mbedtls_sha256_ret(sec_entry->kak_key,
 					       MAX_RSA_DER_BYTE_LEN, hash, 0);
 				fprintf(stdout,
 					">>>>>>>>>> KAK KEY HASH >>>>>>>>>>\n");
@@ -1559,13 +1563,9 @@ error:
 
 int write_boot_image(uint8_t *buf, uint32_t image_size, FILE *out_fd)
 {
-	int aligned_size;
 	int written;
 
-	/* Image size must be aligned to 4 bytes */
-	aligned_size = (image_size + 3) & (~0x3);
-
-	written = fwrite(buf, aligned_size, 1, out_fd);
+	written = fwrite(buf, image_size, 1, out_fd);
 	if (written != 1) {
 		fprintf(stderr, "Error: Failed to write boot image\n");
 		goto error;
@@ -1587,7 +1587,7 @@ int main(int argc, char *argv[])
 	int ext_cnt = 0;
 	int opt;
 	int ret = 0;
-	int image_size;
+	int image_size, file_size;
 	uint8_t *image_buf = NULL;
 	int read;
 	size_t len;
@@ -1683,16 +1683,18 @@ int main(int argc, char *argv[])
 		goto main_exit;
 	}
 
-	/* Read the input file to buffer */
-	image_size = get_file_size(in_file);
-	image_buf = calloc((image_size + AES_BLOCK_SZ - 1) &
-			   ~(AES_BLOCK_SZ - 1), 1);
+	/* Read the input file to buffer
+	 * Always align the image to 16 byte boundary
+	 */
+	file_size  = get_file_size(in_file);
+	image_size = (file_size + AES_BLOCK_SZ - 1) & ~(AES_BLOCK_SZ - 1);
+	image_buf  = calloc(image_size, 1);
 	if (image_buf == NULL) {
 		fprintf(stderr, "Error: failed allocating input buffer\n");
 		return 1;
 	}
 
-	read = fread(image_buf, image_size, 1, in_fd);
+	read = fread(image_buf, file_size, 1, in_fd);
 	if (read != 1) {
 		fprintf(stderr, "Error: failed to read input file\n");
 		goto main_exit;