From ae098fd5c47f6d805b356c892558a85d571bed67 Mon Sep 17 00:00:00 2001
From: Rui Miguel Silva <rui.silva@linaro.org>
Date: Tue, 18 Jul 2023 12:14:47 +0100
Subject: [PATCH] corstone1000: set kernel_addr based on boot_idx

We need to distinguish between boot banks and from which
partition to load the kernel+initramfs to memory.

For that, fetch the boot index, fetch the correspondent
partition, calculate the correct kernel address and
then set the env variable kernel_addr with that value.

Upstream-Status: Pending
Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
---
 board/armltd/corstone1000/corstone1000.c   | 58 +++++++++++++++++++++-
 board/armltd/corstone1000/corstone1000.env |  8 ---
 configs/corstone1000_defconfig             |  1 +
 3 files changed, 58 insertions(+), 9 deletions(-)

diff --git a/board/armltd/corstone1000/corstone1000.c b/board/armltd/corstone1000/corstone1000.c
index ba6d024b80..a045262ebb 100644
--- a/board/armltd/corstone1000/corstone1000.c
+++ b/board/armltd/corstone1000/corstone1000.c
@@ -5,15 +5,25 @@
  * Rui Miguel Silva <rui.silva@linaro.org>
  */
 
+#include <blk.h>
 #include <common.h>
 #include <cpu_func.h>
 #include <dm.h>
+#include <env.h>
 #include <fwu.h>
 #include <netdev.h>
+#include <nvmxip.h>
+#include <part.h>
 #include <dm/platform_data/serial_pl01x.h>
 #include <asm/armv8/mmu.h>
 #include <asm/global_data.h>
 
+#define CORSTONE1000_KERNEL_PARTS 2
+#define CORSTONE1000_KERNEL_PRIMARY "kernel_primary"
+#define CORSTONE1000_KERNEL_SECONDARY "kernel_secondary"
+
+static int corstone1000_boot_idx;
+
 static struct mm_region corstone1000_mem_map[] = {
 	{
 		/* CVM */
@@ -110,5 +120,51 @@ int dram_init_banksize(void)
 
 void fwu_plat_get_bootidx(uint *boot_idx)
 {
-	*boot_idx = 0;
+	*boot_idx = corstone1000_boot_idx;
+}
+
+int board_late_init(void)
+{
+	struct disk_partition part_info;
+	struct udevice *dev, *bdev;
+	struct nvmxip_plat *plat;
+	struct blk_desc *desc;
+	int ret;
+
+	ret = uclass_first_device_err(UCLASS_NVMXIP, &dev);
+	if (ret < 0) {
+		log_err("Cannot find kernel device\n");
+		return ret;
+	}
+
+	plat = dev_get_plat(dev);
+	device_find_first_child(dev, &bdev);
+	desc = dev_get_uclass_plat(bdev);
+	ret = fwu_get_active_index(&corstone1000_boot_idx);
+	if (ret < 0)
+		log_err("corstone1000: failed to read boot index\n");
+
+	if (!corstone1000_boot_idx)
+		ret = part_get_info_by_name(desc, CORSTONE1000_KERNEL_PRIMARY,
+					    &part_info);
+	else
+		ret = part_get_info_by_name(desc, CORSTONE1000_KERNEL_SECONDARY,
+					    &part_info);
+
+	if (ret < 0) {
+		log_err("failed to fetch kernel partition index: %d\n",
+			corstone1000_boot_idx);
+		return ret;
+	}
+
+	ret = 0;
+
+	ret |= env_set_hex("kernel_addr", plat->phys_base +
+			   (part_info.start * part_info.blksz));
+	ret |= env_set_hex("kernel_size", part_info.size * part_info.blksz);
+
+	if (ret < 0)
+		log_err("failed to setup kernel addr and size\n");
+
+	return ret;
 }
diff --git a/board/armltd/corstone1000/corstone1000.env b/board/armltd/corstone1000/corstone1000.env
index a6ee496221..ee318b1b1c 100644
--- a/board/armltd/corstone1000/corstone1000.env
+++ b/board/armltd/corstone1000/corstone1000.env
@@ -2,12 +2,4 @@
 
 usb_pgood_delay=250
 boot_bank_flag=0x08005006
-kernel_addr_bank_0=0x08280000
-kernel_addr_bank_1=0x0936E000
-retrieve_kernel_load_addr=
-	if itest.l *${boot_bank_flag} == 0; then
-		setenv kernel_addr $kernel_addr_bank_0;
-	else
-		setenv kernel_addr $kernel_addr_bank_1;
-	fi;
 kernel_addr_r=0x88200000
diff --git a/configs/corstone1000_defconfig b/configs/corstone1000_defconfig
index 4c75a01818..0232131a11 100644
--- a/configs/corstone1000_defconfig
+++ b/configs/corstone1000_defconfig
@@ -20,6 +20,7 @@ CONFIG_CONSOLE_RECORD=y
 CONFIG_LOGLEVEL=7
 # CONFIG_DISPLAY_CPUINFO is not set
 # CONFIG_DISPLAY_BOARDINFO is not set
+CONFIG_BOARD_LATE_INIT=y
 CONFIG_SYS_MAXARGS=64
 CONFIG_SYS_CBSIZE=512
 # CONFIG_CMD_CONSOLE is not set