[PATCH 5/6] arm: socfpga: mailbox_s10: add write_qspi_refclk helper

[Michael Tretter]

Add a helper function that hides the actually used register, field, and
format. This makes the code easier to read and allows to move it to a
better location.

Signed-off-by: Michael Tretter <m.tretter at pengutronix.de>
---
 arch/arm/mach-socfpga/mailbox_s10.c         |  9 +++------
 include/mach/socfpga/soc64-system-manager.h | 21 +++++++++++++++++++++
 2 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/arch/arm/mach-socfpga/mailbox_s10.c b/arch/arm/mach-socfpga/mailbox_s10.c
index c6ea081165d4..417816673c3d 100644
--- a/arch/arm/mach-socfpga/mailbox_s10.c
+++ b/arch/arm/mach-socfpga/mailbox_s10.c
@@ -299,7 +299,6 @@ int socfpga_mailbox_s10_qspi_open(void)
 	int ret;
 	u32 resp_buf[1] = {};
 	u32 resp_buf_len = ARRAY_SIZE(resp_buf);
-	u32 reg;
 	u32 clk_rate;
 	int try = 0;
 
@@ -340,11 +339,9 @@ int socfpga_mailbox_s10_qspi_open(void)
 
 	pr_info("QSPI: reference clock at %d kHz\n", clk_rate / 1000);
 
-	reg = (readl(SOCFPGA_SYSMGR_ADDRESS + SYSMGR_SOC64_BOOT_SCRATCH_COLD0)) &
-			~(SYSMGR_SCRATCH_REG_0_QSPI_REFCLK_MASK);
-
-	writel(((clk_rate / 1000) & SYSMGR_SCRATCH_REG_0_QSPI_REFCLK_MASK) | reg,
-	       SOCFPGA_SYSMGR_ADDRESS + SYSMGR_SOC64_BOOT_SCRATCH_COLD0);
+	ret = socfpga_agilex5_write_qspi_refclk(clk_rate);
+	if (ret)
+		return ret;
 
 	return 0;
 
diff --git a/include/mach/socfpga/soc64-system-manager.h b/include/mach/socfpga/soc64-system-manager.h
index 862e974b1902..a5df69855a28 100644
--- a/include/mach/socfpga/soc64-system-manager.h
+++ b/include/mach/socfpga/soc64-system-manager.h
@@ -8,6 +8,8 @@
 
 #include <linux/bitops.h>
 
+#include <mach/socfpga/soc64-regs.h>
+
 #define SYSMGR_SOC64_SILICONID_1		0x00
 #define SYSMGR_SOC64_SILICONID_2		0x04
 #define SYSMGR_SOC64_WDDBG			0x08
@@ -173,4 +175,23 @@ void agilex5_security_interleaving_off(void);
 void agilex5_initialize_security_policies(void);
 void agilex5_sysmgr_pinmux_init(void);
 
+static inline int socfpga_agilex5_write_qspi_refclk(unsigned long clkrate)
+{
+	unsigned long clkrate_khz;
+	u32 reg;
+
+	/* Follow U-Boot and store clock rate in kHz */
+	clkrate_khz = clkrate / 1000;
+	if (clkrate_khz & ~SYSMGR_SCRATCH_REG_0_QSPI_REFCLK_MASK)
+		return -EINVAL;
+
+	reg = readl(SOCFPGA_SYSMGR_ADDRESS + SYSMGR_SOC64_BOOT_SCRATCH_COLD0);
+	reg &= ~SYSMGR_SCRATCH_REG_0_QSPI_REFCLK_MASK;
+	reg |= (SYSMGR_SCRATCH_REG_0_QSPI_REFCLK_MASK & clkrate_khz);
+
+	writel(reg, SOCFPGA_SYSMGR_ADDRESS + SYSMGR_SOC64_BOOT_SCRATCH_COLD0);
+
+	return 0;
+}
+
 #endif /* _SOC64_SYSTEM_MANAGER_H_ */

-- 
2.47.3