From 2141d250f510fb33d6b42ff35bfa8f3fbbc343d0 Mon Sep 17 00:00:00 2001
From: Pankit Garg <pankit.garg@nxp.com>
Date: Mon, 5 Nov 2018 18:01:52 +0000
Subject: [PATCH] armv8: fsl-layerscape: bootcmd identification for TFABOOT

Adds bootcmd identificaton on basis on boot source, valid
in TFABOOT configuration.

Signed-off-by: Rajesh Bhagat <rajesh.bhagat@nxp.com>
Signed-off-by: Pankit Garg <pankit.garg@nxp.com>
[YS: remove unnecessary braces]
Reviewed-by: York Sun <york.sun@nxp.com>
---
 arch/arm/cpu/armv8/fsl-layerscape/cpu.c | 14 +++++
 arch/arm/cpu/armv8/fsl-layerscape/soc.c | 77 +++++++++++++++++++++++++
 2 files changed, 91 insertions(+)

diff --git a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
index 0b6f1e1b47..e25515ef9b 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/cpu.c
@@ -33,6 +33,9 @@
 
 #ifdef CONFIG_TFABOOT
 #include <environment.h>
+#ifdef CONFIG_CHAIN_OF_TRUST
+#include <fsl_validate.h>
+#endif
 #endif
 
 DECLARE_GLOBAL_DATA_PTR;
@@ -741,6 +744,14 @@ enum env_location env_get_location(enum env_operation op, int prio)
 	if (prio)
 		return ENVL_UNKNOWN;
 
+#ifdef CONFIG_CHAIN_OF_TRUST
+	/* Check Boot Mode
+	 * If Boot Mode is Secure, return ENVL_NOWHERE
+	 */
+	if (fsl_check_boot_mode_secure() == 1)
+		goto done;
+#endif
+
 	switch (src) {
 	case BOOT_SOURCE_IFC_NOR:
 		env_loc = ENVL_FLASH;
@@ -768,6 +779,9 @@ enum env_location env_get_location(enum env_operation op, int prio)
 		break;
 	}
 
+#ifdef CONFIG_CHAIN_OF_TRUST
+done:
+#endif
 	return env_loc;
 }
 #endif	/* CONFIG_TFABOOT */
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/soc.c b/arch/arm/cpu/armv8/fsl-layerscape/soc.c
index 54fb0745f9..c0a8e301a5 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/soc.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/soc.c
@@ -24,6 +24,10 @@
 #include <fsl_validate.h>
 #endif
 #include <fsl_immap.h>
+#ifdef CONFIG_TFABOOT
+#include <environment.h>
+DECLARE_GLOBAL_DATA_PTR;
+#endif
 
 bool soc_has_dp_ddr(void)
 {
@@ -679,12 +683,85 @@ int qspi_ahb_init(void)
 }
 #endif
 
+#ifdef CONFIG_TFABOOT
+#define MAX_BOOTCMD_SIZE	256
+
+int fsl_setenv_bootcmd(void)
+{
+	int ret;
+	enum boot_src src = get_boot_src();
+	char bootcmd_str[MAX_BOOTCMD_SIZE];
+
+	switch (src) {
+#ifdef IFC_NOR_BOOTCOMMAND
+	case BOOT_SOURCE_IFC_NOR:
+		sprintf(bootcmd_str, IFC_NOR_BOOTCOMMAND);
+		break;
+#endif
+#ifdef QSPI_NOR_BOOTCOMMAND
+	case BOOT_SOURCE_QSPI_NOR:
+		sprintf(bootcmd_str, QSPI_NOR_BOOTCOMMAND);
+		break;
+#endif
+#ifdef XSPI_NOR_BOOTCOMMAND
+	case BOOT_SOURCE_XSPI_NOR:
+		sprintf(bootcmd_str, XSPI_NOR_BOOTCOMMAND);
+		break;
+#endif
+#ifdef IFC_NAND_BOOTCOMMAND
+	case BOOT_SOURCE_IFC_NAND:
+		sprintf(bootcmd_str, IFC_NAND_BOOTCOMMAND);
+		break;
+#endif
+#ifdef QSPI_NAND_BOOTCOMMAND
+	case BOOT_SOURCE_QSPI_NAND:
+		sprintf(bootcmd_str, QSPI_NAND_BOOTCOMMAND);
+		break;
+#endif
+#ifdef XSPI_NAND_BOOTCOMMAND
+	case BOOT_SOURCE_XSPI_NAND:
+		sprintf(bootcmd_str, XSPI_NAND_BOOTCOMMAND);
+		break;
+#endif
+#ifdef SD_BOOTCOMMAND
+	case BOOT_SOURCE_SD_MMC:
+		sprintf(bootcmd_str, SD_BOOTCOMMAND);
+		break;
+#endif
+#ifdef SD2_BOOTCOMMAND
+	case BOOT_SOURCE_SD_MMC2:
+		sprintf(bootcmd_str, SD2_BOOTCOMMAND);
+		break;
+#endif
+	default:
+#ifdef QSPI_NOR_BOOTCOMMAND
+		sprintf(bootcmd_str, QSPI_NOR_BOOTCOMMAND);
+#endif
+		break;
+	}
+
+	ret = env_set("bootcmd", bootcmd_str);
+	if (ret) {
+		printf("Failed to set bootcmd: ret = %d\n", ret);
+		return ret;
+	}
+	return 0;
+}
+#endif
+
 #ifdef CONFIG_BOARD_LATE_INIT
 int board_late_init(void)
 {
 #ifdef CONFIG_CHAIN_OF_TRUST
 	fsl_setenv_chain_of_trust();
 #endif
+#ifdef CONFIG_TFABOOT
+	/*
+	 * check if gd->env_addr is default_environment; then setenv bootcmd
+	 */
+	if (gd->env_addr + gd->reloc_off == (ulong)&default_environment[0])
+		fsl_setenv_bootcmd();
+#endif
 #ifdef CONFIG_QSPI_AHB_INIT
 	qspi_ahb_init();
 #endif
-- 
2.25.1