From a79df348a54a7c959eeb8aed0fb93d6aad7faf1f Mon Sep 17 00:00:00 2001
From: Konstantin Porotchkin <kostap@marvell.com>
Date: Wed, 1 May 2019 17:08:18 +0300
Subject: [PATCH 01/11] tools: doimage: migrate to mbedtls v2.8 APIs

Replace deprecated mbedtls_sha256 with mbedtls_sha256_ret
The mbedtls_pk_parse_key does not work correctly anymore
with the DER buffer embedded in the secure image extentson
using the buffer size as the the key length.
Move to mbedtls_pk_parse_subpubkey API that handles such
case correctly.
The DER format already contains the key length, so there
is no particular reason to supply it to the key parser.
Update the doimage version to 3.3

Change-Id: I0ec5ee84b7d1505b43138e0b7a6bdba44a6702b6
Signed-off-by: Konstantin Porotchkin <kostap@marvell.com>
---
 tools/marvell/doimage/doimage.c | 20 ++++++++++++--------
 1 file changed, 12 insertions(+), 8 deletions(-)

diff --git a/tools/marvell/doimage/doimage.c b/tools/marvell/doimage/doimage.c
index 82fd375f1..deb0cbdb8 100644
--- a/tools/marvell/doimage/doimage.c
+++ b/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");

From 5985a1e4264ac50f0b3408315c43ac65f3f5c631 Mon Sep 17 00:00:00 2001
From: Konstantin Porotchkin <kostap@marvell.com>
Date: Thu, 2 May 2019 15:10:07 +0300
Subject: [PATCH 02/11] tools: doimage: change the binary image alignment to 16

Change the binary image alignment from 4 to 16.
The PKCS signature verification fails for unaligned images.

Change-Id: Ieb08dc3ea128790f542ad93e3c948117567a65af
Signed-off-by: Konstantin Porotchkin <kostap@marvell.com>
---
 tools/marvell/doimage/doimage.c | 20 +++++++++-----------
 1 file changed, 9 insertions(+), 11 deletions(-)

diff --git a/tools/marvell/doimage/doimage.c b/tools/marvell/doimage/doimage.c
index deb0cbdb8..e08b82072 100644
--- a/tools/marvell/doimage/doimage.c
+++ b/tools/marvell/doimage/doimage.c
@@ -1563,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;
@@ -1591,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;
@@ -1687,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;

From 9b88367323a3217c22f3114acad6c8cf1db539c8 Mon Sep 17 00:00:00 2001
From: Grzegorz Jaszczyk <jaz@semihalf.com>
Date: Fri, 12 Apr 2019 16:53:49 +0200
Subject: [PATCH 03/11] drivers: marvell: mg_conf_cm3: add basic driver

Implement function which will allow to start AP FW.

Change-Id: Ie0fc8ad138bf56b10809cdc92d1e5e96a2aaf33f
Signed-off-by: Grzegorz Jaszczyk <jaz@semihalf.com>
---
 drivers/marvell/mg_conf_cm3/mg_conf_cm3.c    | 33 ++++++++++++++++++++
 drivers/marvell/mg_conf_cm3/mg_conf_cm3.h    |  8 +++++
 plat/marvell/armada/a8k/common/a8k_common.mk |  3 +-
 3 files changed, 43 insertions(+), 1 deletion(-)
 create mode 100644 drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
 create mode 100644 drivers/marvell/mg_conf_cm3/mg_conf_cm3.h

diff --git a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
new file mode 100644
index 000000000..e249b5b38
--- /dev/null
+++ b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2019 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+#include <a8k_plat_def.h>
+#include <common/debug.h>
+#include <lib/mmio.h>
+#include <mvebu.h>
+
+/* CONFI REGISTERS */
+#define MG_CM3_CONFI_BASE(CP)		(MVEBU_CP_REGS_BASE(CP) + 0x100000)
+#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)
+
+void mg_start_ap_fw(int cp_nr)
+{
+	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 */
+	}
+
+	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);
+
+	/* TODO: add some routine for checking if ap process is running, if not
+	 * disable cm3.
+	 */
+}
diff --git a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
new file mode 100644
index 000000000..44c7b698a
--- /dev/null
+++ b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
@@ -0,0 +1,8 @@
+/*
+ * Copyright (C) 2019 Marvell International Ltd.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ * https://spdx.org/licenses
+ */
+
+void mg_start_ap_fw(int cp_nr);
diff --git a/plat/marvell/armada/a8k/common/a8k_common.mk b/plat/marvell/armada/a8k/common/a8k_common.mk
index dcbf9a66e..0446b8de7 100644
--- a/plat/marvell/armada/a8k/common/a8k_common.mk
+++ b/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
 

From 5a9e46e69c154ee8f77dbec5e036774622b66ad3 Mon Sep 17 00:00:00 2001
From: Grzegorz Jaszczyk <jaz@semihalf.com>
Date: Fri, 12 Apr 2019 16:57:14 +0200
Subject: [PATCH 04/11] marvell: comphy: start AP FW when comphy AP mode
 selected

After configuring comphy to AP mode also start AP FW.

Change-Id: Ib28977d7ee643575a818ba17f69dea0b7e8e0df4
Signed-off-by: Grzegorz Jaszczyk <jaz@semihalf.com>
---
 drivers/marvell/comphy/phy-comphy-cp110.c | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/drivers/marvell/comphy/phy-comphy-cp110.c b/drivers/marvell/comphy/phy-comphy-cp110.c
index 2760f4603..7ca746070 100644
--- a/drivers/marvell/comphy/phy-comphy-cp110.c
+++ b/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);
+
 	return 0;
 }
 

From 0081cdd1c6d52b2d8b8ae15e16e096f428e2c499 Mon Sep 17 00:00:00 2001
From: Grzegorz Jaszczyk <jaz@semihalf.com>
Date: Wed, 17 Apr 2019 11:24:43 +0200
Subject: [PATCH 05/11] plat: marvell: armada: move mg conf related code to
 appropriate driver

Now when mg_conf_cm3 driver is present - move all relevant code there.

Change-Id: I444d9e877c450d6ee69ca3a49b547e4c3aeac0be
Signed-off-by: Grzegorz Jaszczyk <jaz@semihalf.com>
---
 drivers/marvell/mg_conf_cm3/mg_conf_cm3.c     | 28 ++++++++++++++++++-
 drivers/marvell/mg_conf_cm3/mg_conf_cm3.h     |  1 +
 .../armada/common/mss/mss_scp_bl2_format.h    |  1 -
 .../armada/common/mss/mss_scp_bootloader.c    | 28 ++-----------------
 4 files changed, 30 insertions(+), 28 deletions(-)

diff --git a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
index e249b5b38..0efa30e93 100644
--- a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
+++ b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
@@ -8,15 +8,41 @@
 #include <a8k_plat_def.h>
 #include <common/debug.h>
 #include <lib/mmio.h>
-#include <mvebu.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_SRAM_SIZE	0x20000 /* 128KB */
+
+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)
 {
 	if (mmio_read_32(MG_CM3_CONFI_GLOB_CFG_REG(cp_nr)) & CM3_CPU_EN_BIT) {
diff --git a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
index 44c7b698a..8dfaa32db 100644
--- a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
+++ b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
@@ -6,3 +6,4 @@
  */
 
 void mg_start_ap_fw(int cp_nr);
+int mg_image_load(uintptr_t src_addr, uint32_t size, int cp_index);
diff --git a/plat/marvell/armada/common/mss/mss_scp_bl2_format.h b/plat/marvell/armada/common/mss/mss_scp_bl2_format.h
index 7150f0a06..74dddc645 100644
--- a/plat/marvell/armada/common/mss/mss_scp_bl2_format.h
+++ b/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 {
diff --git a/plat/marvell/armada/common/mss/mss_scp_bootloader.c b/plat/marvell/armada/common/mss/mss_scp_bootloader.c
index 4473d81e1..adf570ea9 100644
--- a/plat/marvell/armada/common/mss/mss_scp_bootloader.c
+++ b/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;

From 2cae4a8586e253c00b3040152fd58b52168d89cd Mon Sep 17 00:00:00 2001
From: Grzegorz Jaszczyk <jaz@semihalf.com>
Date: Tue, 18 Jun 2019 14:43:02 +0200
Subject: [PATCH 06/11] drivers: marvell: mg_conf_cm3: pass comphy lane number
 to AP FW

Since the AP process can be enabled on different setups, the information
about used comphy lane should be passed to AP FW. For instance:
- A8K development board uses comphy lane 2 for eth 0
- cn913x development board uses comphy lane 4 for eth 0

Change-Id: Icf001fb3eea4d9c24c09384e49844ecaf8655ad2
Signed-off-by: Grzegorz Jaszczyk <jaz@semihalf.com>
---
 drivers/marvell/comphy/phy-comphy-cp110.c |  2 +-
 drivers/marvell/mg_conf_cm3/mg_conf_cm3.c | 46 +++++++++++++++++++++--
 drivers/marvell/mg_conf_cm3/mg_conf_cm3.h |  2 +-
 3 files changed, 44 insertions(+), 6 deletions(-)

diff --git a/drivers/marvell/comphy/phy-comphy-cp110.c b/drivers/marvell/comphy/phy-comphy-cp110.c
index 7ca746070..1d5b6f564 100644
--- a/drivers/marvell/comphy/phy-comphy-cp110.c
+++ b/drivers/marvell/comphy/phy-comphy-cp110.c
@@ -2251,7 +2251,7 @@ static int mvebu_cp110_comphy_ap_power_on(uint64_t comphy_base,
 
 	/* Start AP Firmware */
 	mvebu_cp110_get_ap_and_cp_nr(&ap_nr, &cp_nr, comphy_base);
-	mg_start_ap_fw(cp_nr);
+	mg_start_ap_fw(cp_nr, comphy_index);
 
 	return 0;
 }
diff --git a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
index 0efa30e93..98e189687 100644
--- a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
+++ b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.c
@@ -6,6 +6,7 @@
  */
 
 #include <a8k_plat_def.h>
+#include <arch_helpers.h>
 #include <common/debug.h>
 #include <lib/mmio.h>
 #include <mss_scp_bl2_format.h>
@@ -18,8 +19,25 @@
 #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);
@@ -43,17 +61,37 @@ int mg_image_load(uintptr_t src_addr, uint32_t size, int cp_index)
 	return 0;
 }
 
-void mg_start_ap_fw(int cp_nr)
+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);
 
-	/* TODO: add some routine for checking if ap process is running, if not
-	 * disable cm3.
-	 */
+	/* 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);
+	}
 }
diff --git a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
index 8dfaa32db..e2756de9c 100644
--- a/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
+++ b/drivers/marvell/mg_conf_cm3/mg_conf_cm3.h
@@ -5,5 +5,5 @@
  * https://spdx.org/licenses
  */
 
-void mg_start_ap_fw(int cp_nr);
+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);

From 0eb3d1fc75e3929d2a29a934bdbe858177b26e07 Mon Sep 17 00:00:00 2001
From: Konstantin Porotchkin <kostap@marvell.com>
Date: Mon, 15 Apr 2019 16:25:59 +0300
Subject: [PATCH 07/11] plat: marvell: armada: adjust trusted DRAM size to
 match OP-TEE OS

Area used as trusted DRAM is 12MB in Marvell OP-TEE OS module.
It is followed by 4MB of shared memory.

Change-Id: If8edeeec5861b529408baca25f78c06a0a440d8c
Signed-off-by: Konstantin Porotchkin <kostap@marvell.com>
---
 plat/marvell/armada/a3k/common/include/platform_def.h | 6 ++++--
 plat/marvell/armada/a8k/common/include/platform_def.h | 6 ++++--
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/plat/marvell/armada/a3k/common/include/platform_def.h b/plat/marvell/armada/a3k/common/include/platform_def.h
index 7f8f79a45..61c7dfe70 100644
--- a/plat/marvell/armada/a3k/common/include/platform_def.h
+++ b/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
diff --git a/plat/marvell/armada/a8k/common/include/platform_def.h b/plat/marvell/armada/a8k/common/include/platform_def.h
index b26e3ea1e..cbef3a173 100644
--- a/plat/marvell/armada/a8k/common/include/platform_def.h
+++ b/plat/marvell/armada/a8k/common/include/platform_def.h
@@ -96,9 +96,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 RAM */
+/* 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 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_SIZE		0x00100000	/* 1 MB SRAM */

From 0a977b9b8bfc0356830d88e3602e2e5ef4a1ac06 Mon Sep 17 00:00:00 2001
From: Konstantin Porotchkin <kostap@marvell.com>
Date: Mon, 15 Apr 2019 16:32:59 +0300
Subject: [PATCH 08/11] plat: marvell: armada: a8k: change CCU LLC SRAM mapping
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

The LLC SRAM will be enabled in OP-TEE OS for usage as secure storage.
The CCU have to prepare SRAM window, but point to the DRAM-0 target
until the SRAM is actually enabled.
This patch changes CCU SRAM window target to DRAM-0
Remove dependence between LLC_SRAM and LLC_ENABLE and update the
build documentation.
The SRAМ base moved to follow the OP-TEE SHMEM area (0x05400000)

Change-Id: I85c2434a3d515ec37da5ae8eb729e3280f91c456
Signed-off-by: Konstantin Porotchkin <kostap@marvell.com>
---
 docs/plat/marvell/armada/build.rst                     | 10 ++++++----
 .../armada/a8k/a70x0/board/marvell_plat_config.c       |  5 ++++-
 .../armada/a8k/a70x0_amc/board/marvell_plat_config.c   |  5 ++++-
 .../armada/a8k/a80x0/board/marvell_plat_config.c       |  5 ++++-
 .../armada/a8k/a80x0_mcbin/board/marvell_plat_config.c |  5 ++++-
 plat/marvell/armada/a8k/common/include/platform_def.h  |  6 +++---
 plat/marvell/armada/common/marvell_common.mk           |  8 --------
 7 files changed, 25 insertions(+), 19 deletions(-)

diff --git a/docs/plat/marvell/armada/build.rst b/docs/plat/marvell/armada/build.rst
index bec0bcbd4..da4ba565a 100644
--- a/docs/plat/marvell/armada/build.rst
+++ b/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
 
diff --git a/plat/marvell/armada/a8k/a70x0/board/marvell_plat_config.c b/plat/marvell/armada/a8k/a70x0/board/marvell_plat_config.c
index 7d30ebe5a..a40926102 100644
--- a/plat/marvell/armada/a8k/a70x0/board/marvell_plat_config.c
+++ b/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 */
diff --git a/plat/marvell/armada/a8k/a70x0_amc/board/marvell_plat_config.c b/plat/marvell/armada/a8k/a70x0_amc/board/marvell_plat_config.c
index 7fc33f1f9..3b68e91ba 100644
--- a/plat/marvell/armada/a8k/a70x0_amc/board/marvell_plat_config.c
+++ b/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 */
diff --git a/plat/marvell/armada/a8k/a80x0/board/marvell_plat_config.c b/plat/marvell/armada/a8k/a80x0/board/marvell_plat_config.c
index 856c07a6e..4ccda14e9 100644
--- a/plat/marvell/armada/a8k/a80x0/board/marvell_plat_config.c
+++ b/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 */
diff --git a/plat/marvell/armada/a8k/a80x0_mcbin/board/marvell_plat_config.c b/plat/marvell/armada/a8k/a80x0_mcbin/board/marvell_plat_config.c
index 0edc97745..b9329675f 100644
--- a/plat/marvell/armada/a8k/a80x0_mcbin/board/marvell_plat_config.c
+++ b/plat/marvell/armada/a8k/a80x0_mcbin/board/marvell_plat_config.c
@@ -166,7 +166,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 */
diff --git a/plat/marvell/armada/a8k/common/include/platform_def.h b/plat/marvell/armada/a8k/common/include/platform_def.h
index cbef3a173..944a1517b 100644
--- a/plat/marvell/armada/a8k/common/include/platform_def.h
+++ b/plat/marvell/armada/a8k/common/include/platform_def.h
@@ -96,13 +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 12M for SCP (Secure PayLoad) Trusted RAM
- * OP-TEE SHMEM follows this region
+/* 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		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 */
 
 /*
diff --git a/plat/marvell/armada/common/marvell_common.mk b/plat/marvell/armada/common/marvell_common.mk
index fcc97acd3..5c8c7db2d 100644
--- a/plat/marvell/armada/common/marvell_common.mk
+++ b/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

From 506ff4c0c1d850c3fc18959a44c8280c46788734 Mon Sep 17 00:00:00 2001
From: Konstantin Porotchkin <kostap@marvell.com>
Date: Thu, 4 Apr 2019 10:02:20 +0300
Subject: [PATCH 09/11] drivers: marvell: Fix the LLC SRAM driver

- Fix the line address macro
- LLC invalidate and enable before ways lock for allocation
- Add support for limited SRAM size allocation
- Add SRAM RW test function

Change-Id: I1867ece3047566ddd7931bd7472e1f47fb42c8d4
Signed-off-by: Konstantin Porotchkin <kostap@marvell.com>
---
 drivers/marvell/cache_llc.c         | 52 +++++++++++++++++++++++++----
 include/drivers/marvell/cache_llc.h |  5 +--
 2 files changed, 49 insertions(+), 8 deletions(-)

diff --git a/drivers/marvell/cache_llc.c b/drivers/marvell/cache_llc.c
index 836aae7b8..4b06b4752 100644
--- a/drivers/marvell/cache_llc.c
+++ b/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 */
diff --git a/include/drivers/marvell/cache_llc.h b/include/drivers/marvell/cache_llc.h
index b326474ee..d6dd65382 100644
--- a/include/drivers/marvell/cache_llc.h
+++ b/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__ */
 

From 41e8c6fcd1afcbbd13ed7af9ac9239ad0349dde7 Mon Sep 17 00:00:00 2001
From: Marcin Wojtas <mw@semihalf.com>
Date: Wed, 13 Nov 2019 13:31:48 +0100
Subject: [PATCH 10/11] plat: marvell: armada: fix BL32 extra parameters usage

Update missing code releated to the BL32 payload.

Change-Id: I5cbe71921467c53c45be5510f950cefdacc110e1
Signed-off-by: Marcin Wojtas <mw@semihalf.com>
---
 .../aarch64/marvell_bl2_mem_params_desc.c     | 39 +++++++++++++++++++
 .../marvell/armada/common/marvell_bl2_setup.c | 23 +++++++++++
 plat/marvell/armada/common/marvell_common.mk  | 13 +++++++
 .../armada/common/marvell_io_storage.c        | 19 +++++++++
 4 files changed, 94 insertions(+)

diff --git a/plat/marvell/armada/common/aarch64/marvell_bl2_mem_params_desc.c b/plat/marvell/armada/common/aarch64/marvell_bl2_mem_params_desc.c
index 6a8e11c90..8d909dc59 100644
--- a/plat/marvell/armada/common/aarch64/marvell_bl2_mem_params_desc.c
+++ b/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 */
diff --git a/plat/marvell/armada/common/marvell_bl2_setup.c b/plat/marvell/armada/common/marvell_bl2_setup.c
index 3c1c39112..3dfa82e0e 100644
--- a/plat/marvell/armada/common/marvell_bl2_setup.c
+++ b/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) */
diff --git a/plat/marvell/armada/common/marvell_common.mk b/plat/marvell/armada/common/marvell_common.mk
index 5c8c7db2d..2e96e2f84 100644
--- a/plat/marvell/armada/common/marvell_common.mk
+++ b/plat/marvell/armada/common/marvell_common.mk
@@ -58,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		\
@@ -69,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
diff --git a/plat/marvell/armada/common/marvell_io_storage.c b/plat/marvell/armada/common/marvell_io_storage.c
index 065f95688..2627ba4ef 100644
--- a/plat/marvell/armada/common/marvell_io_storage.c
+++ b/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,

From a5de4319ac4957ed42f9e7e774c6eb965ac819ed Mon Sep 17 00:00:00 2001
From: Grzegorz Jaszczyk <jaz@semihalf.com>
Date: Mon, 10 Jun 2019 17:01:05 +0200
Subject: [PATCH 11/11] plat: marvell: armada: mcbin: squash several IO windows
 into one

There is no need to open tree different IO window when there is
possibility of having one covering required range.

Change-Id: I9feae1fc583df1f7d97d28161cf7601f43513856
Signed-off-by: Grzegorz Jaszczyk <jaz@semihalf.com>
---
 .../armada/a8k/a80x0_mcbin/board/marvell_plat_config.c    | 8 ++------
 1 file changed, 2 insertions(+), 6 deletions(-)

diff --git a/plat/marvell/armada/a8k/a80x0_mcbin/board/marvell_plat_config.c b/plat/marvell/armada/a8k/a80x0_mcbin/board/marvell_plat_config.c
index b9329675f..75a1b0c30 100644
--- a/plat/marvell/armada/a8k/a80x0_mcbin/board/marvell_plat_config.c
+++ b/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 */