From a19382521c583b3dde89df14678b011960097f6c Mon Sep 17 00:00:00 2001
From: Jassi Brar <jaswinder.singh@linaro.org>
Date: Mon, 23 May 2022 13:16:01 -0500
Subject: [PATCH] feat(synquacer): add FWU Multi Bank Update support

Add FWU Multi Bank Update support. This reads the platform metadata
and update the FIP base address so that BL2 can load correct BL3X
based on the boot index.

Cc: Sumit Garg <sumit.garg@linaro.org>
Cc: Masahisa Kojima <masahisa.kojima@linaro.org>
Cc: Manish V Badarkhe <manish.badarkhe@arm.com>
Cc: Leonardo Sandoval <leonardo.sandoval@linaro.org>
Change-Id: I5d96972bc4b3b9a12a8157117e53a05da5ce89f6
Signed-off-by: Masami Hiramatsu <masami.hiramatsu@linaro.org>
Signed-off-by: Jassi Brar <jaswinder.singh@linaro.org>
---
 .../synquacer/include/platform_def.h          |  5 +++-
 plat/socionext/synquacer/sq_io_storage.c      | 28 +++++++++++++++++++
 2 files changed, 32 insertions(+), 1 deletion(-)

diff --git a/plat/socionext/synquacer/include/platform_def.h b/plat/socionext/synquacer/include/platform_def.h
index 493809165..a84660b6d 100644
--- a/plat/socionext/synquacer/include/platform_def.h
+++ b/plat/socionext/synquacer/include/platform_def.h
@@ -54,6 +54,9 @@
 #define BL2_MAILBOX_BASE		0x0403f000
 #define BL2_MAILBOX_SIZE		0x1000
 
+#define PLAT_SQ_BOOTIDX_BASE		0x08510000
+#define PLAT_SQ_MAX_BOOT_INDEX		2
+
 #define MAX_IO_HANDLES			2
 #define MAX_IO_DEVICES			2
 #define MAX_IO_BLOCK_DEVICES	U(1)
@@ -81,7 +84,7 @@
 #define PLAT_SQ_BL33_BASE		0xe0000000
 #define PLAT_SQ_BL33_SIZE		0x00100000
 
-/* FIP IO base */
+/* FWU FIP IO base */
 #define PLAT_SQ_FIP_IOBASE		0x08600000
 #define PLAT_SQ_FIP_MAXSIZE		0x00400000
 
diff --git a/plat/socionext/synquacer/sq_io_storage.c b/plat/socionext/synquacer/sq_io_storage.c
index 3991c9e26..ea83dadfd 100644
--- a/plat/socionext/synquacer/sq_io_storage.c
+++ b/plat/socionext/synquacer/sq_io_storage.c
@@ -156,10 +156,38 @@ static const struct sq_io_policy sq_io_policies[] = {
 #endif
 };
 
+static int sq_update_fip_spec(void)
+{
+	uint32_t boot_index;
+	int ret;
+
+	ret = mmap_add_dynamic_region(PLAT_SQ_BOOTIDX_BASE, PLAT_SQ_BOOTIDX_BASE,
+				      PAGE_SIZE, MT_RO_DATA | MT_SECURE);
+	if (ret) {
+		return ret;
+	}
+
+	boot_index = mmio_read_32(PLAT_SQ_BOOTIDX_BASE);
+	if (boot_index < PLAT_SQ_MAX_BOOT_INDEX) {
+		sq_fip_spec.offset += PLAT_SQ_FIP_MAXSIZE * boot_index;
+		INFO("FWU Enabled: boot_index %d\n", boot_index);
+	} else {
+		WARN("FWU Disabled: wrong boot_index value. Fallback to index 0.\n");
+	}
+
+	mmap_remove_dynamic_region(PLAT_SQ_BOOTIDX_BASE, PAGE_SIZE);
+	return 0;
+}
+
 static int sq_io_memmap_setup(void)
 {
 	int ret;
 
+	ret = sq_update_fip_spec();
+	if (ret) {
+		return ret;
+	}
+
 	ret = mmap_add_dynamic_region(sq_fip_spec.offset, sq_fip_spec.offset,
 				      sq_fip_spec.length, MT_RO_DATA | MT_SECURE);
 	if (ret) {