arch: arm: rsu: Multiboot selection Uboot SSBL for RSU

Use different Uboot in QSPI for each RSU bitstream based on the SPL.
Selection is based on the naming of SSBL partition, e.g: P1 SPL will select
SSBL.P1 for loading the Uboot SSBL.

Signed-off-by: Kah Jing Lee <kah.jing.lee@intel.com>
Signed-off-by: Radu Bacrau <radu.bacrau@intel.com>

---
v2:
- Updated on review changes
- Add socfpga_rsu_s10_spt_slot to store SSBL address & length
- Add Kconfig for RSU multiboot feature
- Add CONFIG to build for AGILEX, N5X, S10
---
v3:
- Rework based on review
- Replace spi_probe with spi_flash_probe_bus_cs
- Change error return with linux errno
---
v4:
- Rework based on review
- Update strcpy to strncpy, rename function name
---
v5:
- Rework based on review
- Use strstr to compare the SSBL. prefix instead of the need to
strcpy the uboot name before compare
- Create get_ssbl_slot function to get both uboot address & size
---
v6:
- Minor rework, cleanup unused variable and return type
---
v7:
- Minor rework, cleanup return value and CONFIG for build
---

6 files changed, 224 insertions, 1 deletions

diff --git a/arch/arm/mach-socfpga/Kconfig b/arch/arm/mach-socfpga/Kconfig
index dc8197f88c..2cb2536432 100644
--- a/arch/arm/mach-socfpga/Kconfig
+++ b/arch/arm/mach-socfpga/Kconfig
@@ -6,6 +6,15 @@ config ERR_PTR_OFFSET
 config NR_DRAM_BANKS
 	default 1
 
+config SOCFPGA_RSU_MULTIBOOT
+	bool "Enable RSU Multiboot Selection Feature"
+	depends on TARGET_SOCFPGA_SOC64 && SPI_FLASH
+	default y
+	help
+	 Multiboot u-boot proper image (SSBL) selection feature for RSU.
+	 SPL will select the respective SSBL based on the partition it resides
+	 inside RSU QSPI flash layout.
+
 config SOCFPGA_SECURE_VAB_AUTH
 	bool "Enable boot image authentication with Secure Device Manager"
 	depends on TARGET_SOCFPGA_AGILEX || TARGET_SOCFPGA_N5X
diff --git a/arch/arm/mach-socfpga/Makefile b/arch/arm/mach-socfpga/Makefile
index c02de6a9f1..a43de5a3b1 100644
--- a/arch/arm/mach-socfpga/Makefile
+++ b/arch/arm/mach-socfpga/Makefile
@@ -134,6 +134,10 @@ obj-$(CONFIG_SPL_ATF) += secure_reg_helper.o
 obj-$(CONFIG_SPL_ATF) += smc_api.o
 endif
 
+ifdef CONFIG_SOCFPGA_RSU_MULTIBOOT
+obj-y	+= rsu_spl.o
+endif
+
 ifdef CONFIG_TARGET_SOCFPGA_GEN5
 # QTS-generated config file wrappers
 CFLAGS_wrap_iocsr_config.o	+= -I$(srctree)/board/$(BOARDDIR)
diff --git a/arch/arm/mach-socfpga/include/mach/rsu_s10.h b/arch/arm/mach-socfpga/include/mach/rsu_s10.h
index 9f076a25cd..2ff3d8c765 100644
--- a/arch/arm/mach-socfpga/include/mach/rsu_s10.h
+++ b/arch/arm/mach-socfpga/include/mach/rsu_s10.h
@@ -14,6 +14,8 @@ extern u32 smc_rsu_update_address;
 #define SPT0_INDEX	1
 #define SPT1_INDEX	3
 
+#define MAX_PART_NAME_LENGTH 16
+
 /* CMF pointer block */
 struct socfpga_rsu_s10_cpb {
 	u32 magic_number;
@@ -28,7 +30,7 @@ struct socfpga_rsu_s10_cpb {
 
 /* sub partition slot */
 struct socfpga_rsu_s10_spt_slot {
-	char name[16];
+	char name[MAX_PART_NAME_LENGTH];
 	u32 offset[2];
 	u32 length;
 	u32 flag;
diff --git a/arch/arm/mach-socfpga/include/mach/rsu_spl.h b/arch/arm/mach-socfpga/include/mach/rsu_spl.h
new file mode 100644
index 0000000000..f9cc49034e
--- /dev/null
+++ b/arch/arm/mach-socfpga/include/mach/rsu_spl.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier:    GPL-2.0 */
+/*
+ * Copyright (C) 2022 Intel Corporation <www.intel.com>
+ *
+ */
+
+#ifndef _RSU_S10_SPL_H_
+#define _RSU_S10_SPL_H_
+#include <asm/arch/rsu_s10.h>
+
+u32 rsu_spl_ssbl_address(void);
+u32 rsu_spl_ssbl_size(void);
+
+#endif /* _RSU_S10_SPL__H_ */
diff --git a/arch/arm/mach-socfpga/rsu_spl.c b/arch/arm/mach-socfpga/rsu_spl.c
new file mode 100644
index 0000000000..92d12682c3
--- /dev/null
+++ b/arch/arm/mach-socfpga/rsu_spl.c
@@ -0,0 +1,184 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2022 Intel Corporation <www.intel.com>
+ *
+ */
+#include <common.h>
+#include <linux/errno.h>
+#include <spi.h>
+#include <spi_flash.h>
+#include <asm/arch/mailbox_s10.h>
+#include <asm/arch/rsu.h>
+#include <asm/arch/rsu_s10.h>
+#include <asm/arch/rsu_spl.h>
+
+#define SSBL_PART_PREFIX	"SSBL."
+#define RSU_ADDR_MASK	0xFFFFFFFF
+#define RSU_ADDR_SHIFT	32
+
+static int get_ssbl_slot(struct socfpga_rsu_s10_spt_slot *rsu_ssbl_slot)
+{
+	struct socfpga_rsu_s10_spt rsu_spt = {0};
+	u32 rsu_spt0_offset = 0, rsu_spt1_offset = 0;
+	u32 spt_offset[4];
+	struct rsu_status_info rsu_status;
+	int crt_spt_index = -EINVAL;
+	char *result;
+	struct spi_flash *flash;
+	int i;
+
+	rsu_ssbl_slot->offset[0] = -EINVAL;
+
+	/* get rsu status */
+	if (mbox_rsu_status((u32 *)&rsu_status, sizeof(rsu_status) / 4)) {
+		puts("RSU: Error - mbox_rsu_status failed!\n");
+		return -EOPNOTSUPP;
+	}
+
+	/* get spt offsets */
+	if (mbox_rsu_get_spt_offset(spt_offset, 4)) {
+		puts("RSU: Error - mbox_rsu_get_spt_offset failed!\n");
+		return -EINVAL;
+	}
+
+	rsu_spt0_offset = spt_offset[SPT0_INDEX];
+	rsu_spt1_offset = spt_offset[SPT1_INDEX];
+
+	/* initialize flash */
+	flash = spi_flash_probe(CONFIG_SF_DEFAULT_BUS,
+				CONFIG_SF_DEFAULT_CS,
+				CONFIG_SF_DEFAULT_SPEED,
+				CONFIG_SF_DEFAULT_MODE);
+	if (!flash) {
+		puts("RSU: Error - spi_flash_probe failed!\n");
+		return -EINVAL;
+	}
+
+	/* read spt0 */
+	if (spi_flash_read(flash, rsu_spt0_offset, sizeof(rsu_spt), &rsu_spt)) {
+		puts("RSU: Error - spi_flash_read failed!\n");
+		return -EINVAL;
+	}
+
+	/* if spt0 does not have the correct magic number */
+	if (rsu_spt.magic_number != RSU_S10_SPT_MAGIC_NUMBER) {
+		/* read spt1 */
+		if (spi_flash_read(flash, rsu_spt1_offset, sizeof(rsu_spt), &rsu_spt)) {
+			printf("RSU: Error - spi_flash_read failed!\n");
+			return -EINVAL;
+		}
+
+		/* bail out if spt1 does not have the correct magic number */
+		if (rsu_spt.magic_number != RSU_S10_SPT_MAGIC_NUMBER) {
+			printf("RSU: Error: spt table magic number not match 0x%08x!\n",
+			       rsu_spt.magic_number);
+			return -EINVAL;
+		}
+	}
+
+	/* display status */
+	debug("RSU current image:  0x%08x\n", (u32)rsu_status.current_image);
+	debug("RSU state:          0x%08x\n", rsu_status.state);
+	debug("RSU error location: 0x%08x\n", rsu_status.error_location);
+	debug("RSU error details:  0x%08x\n", rsu_status.error_details);
+
+	/* display partitions */
+	for (i = 0; i < rsu_spt.entries; i++) {
+		debug("RSU: Partition '%s' start=0x%08x length=0x%08x\n",
+		      rsu_spt.spt_slot[i].name, rsu_spt.spt_slot[i].offset[0],
+		      rsu_spt.spt_slot[i].length);
+	}
+
+	/* locate the SPT entry for currently loaded image */
+	for (i = 0; i < rsu_spt.entries; i++) {
+		if (((rsu_status.current_image & RSU_ADDR_MASK) ==
+			rsu_spt.spt_slot[i].offset[0]) &&
+		   ((rsu_status.current_image >> RSU_ADDR_SHIFT) ==
+			rsu_spt.spt_slot[i].offset[1])) {
+			crt_spt_index = i;
+			break;
+		}
+	}
+
+	if (crt_spt_index == -EINVAL) {
+		puts("RSU: Error - could not locate partition in the SPT table!\n");
+		return -EINVAL;
+	}
+
+	/* locate the u-boot proper(SSBL) partition and return its address */
+	for (i = 0; i < rsu_spt.entries; i++) {
+		/* get the substring ptr to the first occurrence of SSBL. prefix */
+		result = strstr(rsu_spt.spt_slot[i].name, SSBL_PART_PREFIX);
+
+		/* skip if not found the SSBL prefix */
+		if (!result)
+			continue;
+
+		/* check if the prefix is located at the first */
+		if (result == rsu_spt.spt_slot[i].name) {
+			/* move to the substring after SSBL. prefix */
+			result += strlen(SSBL_PART_PREFIX);
+
+			/* compare SPL's spt name after the prefix */
+			if (!strncmp(result, rsu_spt.spt_slot[crt_spt_index].name,
+				     MAX_PART_NAME_LENGTH - strlen(SSBL_PART_PREFIX))) {
+				printf("RSU: found SSBL partition %s at address 0x%08x.\n",
+				       result, (int)rsu_spt.spt_slot[i].offset[0]);
+				memcpy(rsu_ssbl_slot, &rsu_spt.spt_slot[i],
+				       sizeof(struct socfpga_rsu_s10_spt_slot));
+
+				return 0;
+			}
+		}
+	}
+
+	/* fail to find u-boot proper(SSBL) */
+	printf("RSU: Error - could not find u-boot proper partition SSBL.%s!\n",
+	       rsu_spt.spt_slot[crt_spt_index].name);
+
+	return -EINVAL;
+}
+
+u32 rsu_spl_ssbl_address(void)
+{
+	int ret;
+	struct socfpga_rsu_s10_spt_slot rsu_ssbl_slot = {0};
+
+	ret = get_ssbl_slot(&rsu_ssbl_slot);
+	if (ret) {
+		if (ret == -EOPNOTSUPP) {
+			puts("RSU: Error - mbox_rsu_status failed! Check for RSU image.\n");
+			return CONFIG_SYS_SPI_U_BOOT_OFFS;
+		}
+
+		/* should throw error if cannot find u-boot proper(SSBL) address */
+		panic("ERROR: could not find u-boot proper(SSBL) address!");
+	}
+
+	printf("RSU: Success found SSBL at offset: %08x.\n", rsu_ssbl_slot.offset[0]);
+	return rsu_ssbl_slot.offset[0];
+}
+
+u32 rsu_spl_ssbl_size(void)
+{
+	struct socfpga_rsu_s10_spt_slot rsu_ssbl_slot = {0};
+
+	/* check for valid u-boot proper(SSBL) address for the size */
+	if (get_ssbl_slot(&rsu_ssbl_slot) == -EOPNOTSUPP) {
+		printf("ERROR: Invalid address, could not retrieve SSBL size!");
+		return 0;
+	}
+
+	if (!rsu_ssbl_slot.length) {
+		/* throw error if cannot find u-boot proper(SSBL) size */
+		panic("ERROR: could not retrieve u-boot proper(SSBL) size!");
+	}
+
+	printf("RSU: Success found SSBL with length: %08x.\n", rsu_ssbl_slot.length);
+	return rsu_ssbl_slot.length;
+}
+
+unsigned int spl_spi_get_uboot_offs(struct spi_flash *flash)
+{
+	return rsu_spl_ssbl_address();
+}
diff --git a/env/sf.c b/env/sf.c
index a425ecc11c..820cbe1da0 100644
--- a/env/sf.c
+++ b/env/sf.c
@@ -22,6 +22,9 @@
 #include <asm/global_data.h>
 #include <dm/device-internal.h>
 #include <u-boot/crc.h>
+#if (CONFIG_IS_ENABLED(SOCFPGA_RSU_MULTIBOOT))
+#include <asm/arch/rsu_spl.h>
+#endif
 
 #define	OFFSET_INVALID		(~(u32)0)
 
@@ -37,6 +40,13 @@ static ulong env_new_offset	= CONFIG_ENV_OFFSET_REDUND;
 
 #endif /* CONFIG_ENV_OFFSET_REDUND */
 
+#if (CONFIG_IS_ENABLED(SOCFPGA_RSU_MULTIBOOT))
+#undef CONFIG_ENV_OFFSET
+#define CONFIG_ENV_OFFSET (rsu_spl_ssbl_address() +\
+					       rsu_spl_ssbl_size() -\
+					       CONFIG_ENV_SIZE)
+#endif
+
 DECLARE_GLOBAL_DATA_PTR;
 
 static int setup_flash_device(struct spi_flash **env_flash)