[PATCH 06/18] arm64: versal: Move board_early_init_r clock setup to mach code

[Michal Simek]

board_early_init_r() programmed the IOU switch clock and the system
timestamp counter directly with readl()/writel() in board code. This is
SoC register setup rather than board policy, and the same block is
duplicated across the Xilinx SoCs.

Move it into versal_timer_setup() in arch/arm/mach-versal so the board
hook only keeps the EL3 guard and calls the helper.

Signed-off-by: Michal Simek <michal.simek at amd.com>
---

 arch/arm/mach-versal/cpu.c                    | 43 +++++++++++++++++++
 arch/arm/mach-versal/include/mach/sys_proto.h |  2 +
 board/xilinx/versal/board.c                   | 38 +---------------
 3 files changed, 46 insertions(+), 37 deletions(-)

diff --git a/arch/arm/mach-versal/cpu.c b/arch/arm/mach-versal/cpu.c
index 2a2b683b1fd4..76cf3181e28c 100644
--- a/arch/arm/mach-versal/cpu.c
+++ b/arch/arm/mach-versal/cpu.c
@@ -7,6 +7,8 @@
  */
 
 #include <init.h>
+#include <log.h>
+#include <time.h>
 #include <asm/armv8/mmu.h>
 #include <asm/cache.h>
 #include <asm/global_data.h>
@@ -120,6 +122,47 @@ u32 __weak versal_pmc_multi_boot(void)
 	return versal_multi_boot_reg();
 }
 
+void versal_timer_setup(void)
+{
+	u32 val;
+
+	debug("iou_switch ctrl div0 %x\n",
+	      readl(&crlapb_base->iou_switch_ctrl));
+
+	writel(IOU_SWITCH_CTRL_CLKACT_BIT |
+	       (CONFIG_IOU_SWITCH_DIVISOR0 << IOU_SWITCH_CTRL_DIVISOR0_SHIFT),
+	       &crlapb_base->iou_switch_ctrl);
+
+	/* Global timer init - Program time stamp reference clk */
+	val = readl(&crlapb_base->timestamp_ref_ctrl);
+	val |= CRL_APB_TIMESTAMP_REF_CTRL_CLKACT_BIT;
+	writel(val, &crlapb_base->timestamp_ref_ctrl);
+
+	debug("ref ctrl 0x%x\n",
+	      readl(&crlapb_base->timestamp_ref_ctrl));
+
+	/* Clear reset of timestamp reg */
+	writel(0, &crlapb_base->rst_timestamp);
+
+	/*
+	 * Program freq register in System counter and
+	 * enable system counter.
+	 */
+	writel(CONFIG_COUNTER_FREQUENCY,
+	       &iou_scntr_secure->base_frequency_id_register);
+
+	debug("counter val 0x%x\n",
+	      readl(&iou_scntr_secure->base_frequency_id_register));
+
+	writel(IOU_SCNTRS_CONTROL_EN,
+	       &iou_scntr_secure->counter_control_register);
+
+	debug("scntrs control 0x%x\n",
+	      readl(&iou_scntr_secure->counter_control_register));
+	debug("timer 0x%llx\n", get_ticks());
+	debug("timer 0x%llx\n", get_ticks());
+}
+
 u32 versal_bootmode_reg(void)
 {
 	return readl(&crp_base->boot_mode_usr);
diff --git a/arch/arm/mach-versal/include/mach/sys_proto.h b/arch/arm/mach-versal/include/mach/sys_proto.h
index f8836db5ef05..cb373e6fad91 100644
--- a/arch/arm/mach-versal/include/mach/sys_proto.h
+++ b/arch/arm/mach-versal/include/mach/sys_proto.h
@@ -25,5 +25,7 @@ u32 versal_multi_boot_reg(void);
 u8 versal_get_bootmode(void);
 /* Direct MMIO read of the bootmode register (EL3 / no-firmware path) */
 u32 versal_bootmode_reg(void);
+/* EL3 clock/timer register setup, called from board_early_init_r() */
+void versal_timer_setup(void);
 
 #endif /* _ASM_ARCH_SYS_PROTO_H */
diff --git a/board/xilinx/versal/board.c b/board/xilinx/versal/board.c
index d0c15fed1e93..0537517b1b2e 100644
--- a/board/xilinx/versal/board.c
+++ b/board/xilinx/versal/board.c
@@ -68,46 +68,10 @@ int board_init(void)
 
 int board_early_init_r(void)
 {
-	u32 val;
-
 	if (current_el() != 3)
 		return 0;
 
-	debug("iou_switch ctrl div0 %x\n",
-	      readl(&crlapb_base->iou_switch_ctrl));
-
-	writel(IOU_SWITCH_CTRL_CLKACT_BIT |
-	       (CONFIG_IOU_SWITCH_DIVISOR0 << IOU_SWITCH_CTRL_DIVISOR0_SHIFT),
-	       &crlapb_base->iou_switch_ctrl);
-
-	/* Global timer init - Program time stamp reference clk */
-	val = readl(&crlapb_base->timestamp_ref_ctrl);
-	val |= CRL_APB_TIMESTAMP_REF_CTRL_CLKACT_BIT;
-	writel(val, &crlapb_base->timestamp_ref_ctrl);
-
-	debug("ref ctrl 0x%x\n",
-	      readl(&crlapb_base->timestamp_ref_ctrl));
-
-	/* Clear reset of timestamp reg */
-	writel(0, &crlapb_base->rst_timestamp);
-
-	/*
-	 * Program freq register in System counter and
-	 * enable system counter.
-	 */
-	writel(CONFIG_COUNTER_FREQUENCY,
-	       &iou_scntr_secure->base_frequency_id_register);
-
-	debug("counter val 0x%x\n",
-	      readl(&iou_scntr_secure->base_frequency_id_register));
-
-	writel(IOU_SCNTRS_CONTROL_EN,
-	       &iou_scntr_secure->counter_control_register);
-
-	debug("scntrs control 0x%x\n",
-	      readl(&iou_scntr_secure->counter_control_register));
-	debug("timer 0x%llx\n", get_ticks());
-	debug("timer 0x%llx\n", get_ticks());
+	versal_timer_setup();
 
 	return 0;
 }
-- 
2.43.0