1 files changed, 40 insertions, 11 deletions

diff --git a/board/xilinx/zynqmp/zynqmp.c b/board/xilinx/zynqmp/zynqmp.c
index 2cf47125d4..44d347ed3b 100644
--- a/board/xilinx/zynqmp/zynqmp.c
+++ b/board/xilinx/zynqmp/zynqmp.c
@@ -9,6 +9,7 @@
 #include <netdev.h>
 #include <ahci.h>
 #include <scsi.h>
+#include <asm/arch/clk.h>
 #include <asm/arch/hardware.h>
 #include <asm/arch/sys_proto.h>
 #include <asm/io.h>
@@ -28,10 +29,18 @@ int board_early_init_r(void)
 {
 	u32 val;
 
-	val = readl(&crlapb_base->timestamp_ref_ctrl);
-	val |= ZYNQMP_CRL_APB_TIMESTAMP_REF_CTRL_CLKACT;
-	writel(val, &crlapb_base->timestamp_ref_ctrl);
-
+	if (current_el() == 3) {
+		val = readl(&crlapb_base->timestamp_ref_ctrl);
+		val |= ZYNQMP_CRL_APB_TIMESTAMP_REF_CTRL_CLKACT;
+		writel(val, &crlapb_base->timestamp_ref_ctrl);
+
+		/* Program freq register in System counter */
+		writel(zynqmp_get_system_timer_freq(),
+		       &iou_scntr_secure->base_frequency_id_register);
+		/* And enable system counter */
+		writel(ZYNQMP_IOU_SCNTR_COUNTER_CONTROL_REGISTER_EN,
+		       &iou_scntr_secure->counter_control_register);
+	}
 	/* Program freq register in System counter and enable system counter */
 	writel(gd->cpu_clk, &iou_scntr->base_frequency_id_register);
 	writel(ZYNQMP_IOU_SCNTR_COUNTER_CONTROL_REGISTER_HDBG |
@@ -48,11 +57,6 @@ int dram_init(void)
 	return 0;
 }
 
-int timer_init(void)
-{
-	return 0;
-}
-
 void reset_cpu(ulong addr)
 {
 }
@@ -73,11 +77,36 @@ int board_late_init(void)
 	reg = readl(&crlapb_base->boot_mode);
 	bootmode = reg & BOOT_MODES_MASK;
 
+	puts("Bootmode: ");
 	switch (bootmode) {
-	case SD_MODE:
+	case JTAG_MODE:
+		puts("JTAG_MODE\n");
+		setenv("modeboot", "jtagboot");
+		break;
+	case QSPI_MODE_24BIT:
+	case QSPI_MODE_32BIT:
+		setenv("modeboot", "qspiboot");
+		puts("QSPI_MODE\n");
+		break;
 	case EMMC_MODE:
+		puts("EMMC_MODE\n");
+		setenv("modeboot", "sdboot");
+		break;
+	case SD_MODE:
+		puts("SD_MODE\n");
 		setenv("modeboot", "sdboot");
 		break;
+	case SD_MODE1:
+		puts("SD_MODE1\n");
+#if defined(CONFIG_ZYNQ_SDHCI0) && defined(CONFIG_ZYNQ_SDHCI1)
+		setenv("sdbootdev", "1");
+#endif
+		setenv("modeboot", "sdboot");
+		break;
+	case NAND_MODE:
+		puts("NAND_MODE\n");
+		setenv("modeboot", "nandboot");
+		break;
 	default:
 		printf("Invalid Boot Mode:0x%x\n", bootmode);
 		break;
@@ -88,7 +117,7 @@ int board_late_init(void)
 
 int checkboard(void)
 {
-	puts("Board:\tXilinx ZynqMP\n");
+	puts("Board: Xilinx ZynqMP\n");
 	return 0;
 }