[U-Boot] [PATCH v2] arm: socfpga: fix issue with warm reset when CSEL is 0

[Dalon Westergreen <dwesterg@gmail.com>]

When CSEL=0x0 the socfpga bootrom does not touch the clock
configuration for the device.  This can lead to a boot failure
on warm resets.  To address this, the bootrom is configured to
run a bit of code in the last 4KB of onchip ram on a warm reset.
This code puts the PLLs in bypass, disables the bootrom configuration
to run the code snippet, and issues a warm reset to run the bootrom.

Signed-off-by: Dalon Westergreen <dwesterg@gmail.com>

--
Changes in V2:
 - Fix checkpatch issues predominently due to whitespace issues
---
 arch/arm/mach-socfpga/Makefile                     |  2 +-
 arch/arm/mach-socfpga/include/mach/clock_manager.h | 26 +++++++-
 arch/arm/mach-socfpga/include/mach/reset_manager.h |  4 ++
 .../arm/mach-socfpga/include/mach/system_manager.h |  7 ++-
 arch/arm/mach-socfpga/misc.c                       | 27 ++++++++
 arch/arm/mach-socfpga/reset_clock_manager.S        | 71 ++++++++++++++++++++++
 6 files changed, 134 insertions(+), 3 deletions(-)
 create mode 100644 arch/arm/mach-socfpga/reset_clock_manager.S

diff --git a/arch/arm/mach-socfpga/Makefile b/arch/arm/mach-socfpga/Makefile
index 809cd47..6876ccf 100644
--- a/arch/arm/mach-socfpga/Makefile
+++ b/arch/arm/mach-socfpga/Makefile
@@ -8,7 +8,7 @@
 #
 
 obj-y	+= misc.o timer.o reset_manager.o system_manager.o clock_manager.o \
-	   fpga_manager.o board.o
+	   fpga_manager.o board.o reset_clock_manager.o
 
 obj-$(CONFIG_SPL_BUILD) += spl.o freeze_controller.o
 
diff --git a/arch/arm/mach-socfpga/include/mach/clock_manager.h b/arch/arm/mach-socfpga/include/mach/clock_manager.h
index 803c926..78f63a4 100644
--- a/arch/arm/mach-socfpga/include/mach/clock_manager.h
+++ b/arch/arm/mach-socfpga/include/mach/clock_manager.h
@@ -19,9 +19,12 @@ const unsigned int cm_get_osc_clk_hz(const int osc);
 const unsigned int cm_get_f2s_per_ref_clk_hz(void);
 const unsigned int cm_get_f2s_sdr_ref_clk_hz(void);
 
+/* Onchip RAM functions for CSEL=0 */
+void reset_clock_manager(void);
+extern unsigned reset_clock_manager_size;
+
 /* Clock configuration accessors */
 const struct cm_config * const cm_get_default_config(void);
-#endif
 
 struct cm_config {
 	/* main group */
@@ -127,6 +130,19 @@ struct socfpga_clock_manager {
 	struct socfpga_clock_manager_altera altera;
 	u32	_pad_0xe8_0x200[70];
 };
+#endif
+
+#define CLKMGR_CTRL_ADDRESS 0x0
+#define CLKMGR_BYPASS_ADDRESS 0x4
+#define CLKMGR_INTER_ADDRESS 0x8
+#define CLKMGR_INTREN_ADDRESS 0xc
+#define CLKMGR_DBCTRL_ADDRESS 0x10
+#define CLKMGR_STAT_ADDRESS 0x14
+#define CLKMGR_MAINPLLGRP_MAINQSPICLK_ADDRESS 0x54
+#define CLKMGR_MAINPLLGRP_MAINNANDSDMMCCLK_ADDRESS 0x58
+#define CLKMGR_PERPLLGRP_PERQSPICLK_ADDRESS 0x90
+#define CLKMGR_PERPLLGRP_PERNANDSDMMCCLK_ADDRESS 0x94
+
 
 #define CLKMGR_CTRL_SAFEMODE				(1 << 0)
 #define CLKMGR_CTRL_SAFEMODE_OFFSET			0
@@ -314,4 +330,12 @@ struct socfpga_clock_manager {
 #define CLKMGR_SDRPLLGRP_S2FUSER2CLK_PHASE_OFFSET	9
 #define CLKMGR_SDRPLLGRP_S2FUSER2CLK_PHASE_MASK		0x00000e00
 
+/* Bypass Main and Per PLL, bypass source per input mux */
+#define CLKMGR_BYPASS_MAIN_PER_PLL_MASK         0x19
+                                                                                
+#define CLKMGR_MAINQSPICLK_RESET_VALUE          0x3
+#define CLKMGR_MAINNANDSDMMCCLK_RESET_VALUE     0x3
+#define CLKMGR_PERQSPICLK_RESET_VALUE           0x1
+#define CLKMGR_PERNANDSDMMCCLK_RESET_VALUE      0x1
+
 #endif /* _CLOCK_MANAGER_H_ */
diff --git a/arch/arm/mach-socfpga/include/mach/reset_manager.h b/arch/arm/mach-socfpga/include/mach/reset_manager.h
index 2f070f2..58d77fb 100644
--- a/arch/arm/mach-socfpga/include/mach/reset_manager.h
+++ b/arch/arm/mach-socfpga/include/mach/reset_manager.h
@@ -7,6 +7,7 @@
 #ifndef	_RESET_MANAGER_H_
 #define	_RESET_MANAGER_H_
 
+#ifndef __ASSEMBLY__
 void reset_cpu(ulong addr);
 void reset_deassert_peripherals_handoff(void);
 
@@ -28,6 +29,8 @@ struct socfpga_reset_manager {
 	u32	padding2[12];
 	u32	tstscratch;
 };
+#endif
+
 
 #if defined(CONFIG_SOCFPGA_VIRTUAL_TARGET)
 #define RSTMGR_CTRL_SWWARMRSTREQ_LSB 2
@@ -40,6 +43,7 @@ struct socfpga_reset_manager {
  * and reset ID can be extracted using the subsequent macros
  * RSTMGR_RESET() and RSTMGR_BANK().
  */
+#define RSTMGR_CTRL_OFFSET	4
 #define RSTMGR_BANK_OFFSET	8
 #define RSTMGR_BANK_MASK	0x7
 #define RSTMGR_RESET_OFFSET	0
diff --git a/arch/arm/mach-socfpga/include/mach/system_manager.h b/arch/arm/mach-socfpga/include/mach/system_manager.h
index c45edea..b89f269 100644
--- a/arch/arm/mach-socfpga/include/mach/system_manager.h
+++ b/arch/arm/mach-socfpga/include/mach/system_manager.h
@@ -13,7 +13,6 @@ void sysmgr_pinmux_init(void);
 void sysmgr_config_warmrstcfgio(int enable);
 
 void sysmgr_get_pinmux_table(const u8 **table, unsigned int *table_len);
-#endif
 
 struct socfpga_system_manager {
 	/* System Manager Module */
@@ -115,6 +114,12 @@ struct socfpga_system_manager {
 	u32	_pad_0x734;
 	u32	spim0usefpga;			/* 0x738 */
 };
+#endif
+
+#define CONFIG_SYSMGR_WARMRAMGRP_ENABLE		(SOCFPGA_SYSMGR_ADDRESS + 0xe0)
+
+#define SYSMGR_BOOTINFO_CSEL_MASK	0x18
+#define SYSMGR_BOOTINFO_CSEL_LSB	3
 
 #define SYSMGR_ROMCODEGRP_CTRL_WARMRSTCFGPINMUX	(1 << 0)
 #define SYSMGR_ROMCODEGRP_CTRL_WARMRSTCFGIO	(1 << 1)
diff --git a/arch/arm/mach-socfpga/misc.c b/arch/arm/mach-socfpga/misc.c
index dd6b53b..57e3334 100644
--- a/arch/arm/mach-socfpga/misc.c
+++ b/arch/arm/mach-socfpga/misc.c
@@ -16,6 +16,7 @@
 #include <asm/arch/reset_manager.h>
 #include <asm/arch/scan_manager.h>
 #include <asm/arch/system_manager.h>
+#include <asm/arch/clock_manager.h>
 #include <asm/arch/nic301.h>
 #include <asm/arch/scu.h>
 #include <asm/pl310.h>
@@ -356,6 +357,32 @@ static uint32_t iswgrp_handoff[8];
 int arch_early_init_r(void)
 {
 	int i;
+	unsigned csel, ramboot_addr;
+
+	/* Check the CSEL value */
+	csel = (readl(&sysmgr_regs->bootinfo) & SYSMGR_BOOTINFO_CSEL_MASK) >>
+		SYSMGR_BOOTINFO_CSEL_LSB;
+
+	/*
+	 * For CSEL = 0 the bootrom does not configure the clocks which can
+	 * result in a boot failure on warm resets.  To remedy this a small
+	 * bit of code is placed at the end of the onchip ram and run on
+	 * a warm reset.  It puts the PLLs in bypass and issues another warm
+	 * reset to get back to the bootrom.
+	 */
+	if (!csel) {
+		/* Put the code snippet in the last 4KB of the onchip ram */
+		ramboot_addr = CONFIG_SYS_INIT_RAM_ADDR +
+			CONFIG_SYS_INIT_RAM_SIZE - 0x1000;
+
+		/* Copy the code to the onchip ramlocation */
+		memcpy((void *)ramboot_addr, reset_clock_manager,
+		      reset_clock_manager_size);
+
+		/* Set the bootrom to run the code snippet on reset */
+		writel(ramboot_addr,
+		      &sysmgr_regs->romcodegrp_warmramgrp_execution);
+	}
 
 	/*
 	 * Write magic value into magic register to unlock support for
diff --git a/arch/arm/mach-socfpga/reset_clock_manager.S b/arch/arm/mach-socfpga/reset_clock_manager.S
new file mode 100644
index 0000000..1818b2d
--- /dev/null
+++ b/arch/arm/mach-socfpga/reset_clock_manager.S
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2017, Intel Corporation
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+#include <asm/arch/system_manager.h>
+#include <asm/arch/reset_manager.h>
+#include <asm/arch/clock_manager.h>
+
+/*
+ */
+ENTRY(reset_clock_manager)
+	/* Put Main PLL and Peripheral PLL in bypass */
+	ldr	r0, SOCFPGA_CLKMGR
+	mov	r1, #CLKMGR_BYPASS_ADDRESS
+	mov	r2, #CLKMGR_BYPASS_MAIN_PER_PLL_MASK
+	add	r3, r0, r1
+	ldr	r4, [r3]
+	orr	r5, r4, r2
+	str	r5, [r3]
+	dsb
+	isb
+	mov	r1, #CLKMGR_MAINPLLGRP_MAINQSPICLK_ADDRESS
+	mov	r2, #CLKMGR_MAINQSPICLK_RESET_VALUE
+	add	r3, r0, r1
+	str	r2, [r3]
+	mov	r1, #CLKMGR_MAINPLLGRP_MAINNANDSDMMCCLK_ADDRESS
+	mov	r2, #CLKMGR_MAINNANDSDMMCCLK_RESET_VALUE
+	add	r3, r0, r1
+	str	r2, [r3]
+	mov	r1, #CLKMGR_PERPLLGRP_PERQSPICLK_ADDRESS
+	mov	r2, #CLKMGR_PERQSPICLK_RESET_VALUE
+	add	r3, r0, r1
+	str	r2, [r3]
+	mov	r1, #CLKMGR_PERPLLGRP_PERNANDSDMMCCLK_ADDRESS
+	mov	r2, #CLKMGR_PERNANDSDMMCCLK_RESET_VALUE
+	add	r3, r0, r1
+	str	r2, [r3]
+
+	/* Disable the RAM boot */
+	ldr	r0, SOCFPGA_RSTMGR
+	ldr	r1, SYSMGR_WARMRAMGRP_ENABLE
+	mov	r2, #0
+	str	r2, [r1]
+
+	/* Trigger warm reset to continue boot normally */
+	mov	r1, #RSTMGR_CTRL_OFFSET
+	add	r2, r0, r1
+	mov	r3, #1
+	mov	r3, r3, LSL #RSTMGR_CTRL_SWWARMRSTREQ_LSB
+	ldr	r4, [r2]
+	orr	r4, r3, r4
+	str	r4, [r2]
+
+reset_clock_manager_loop:
+	dsb
+	isb
+	b	reset_clock_manager_loop
+ENDPROC(reset_clock_manager)
+
+SOCFPGA_CLKMGR:			.word	SOCFPGA_CLKMGR_ADDRESS
+SOCFPGA_RSTMGR:			.word	SOCFPGA_RSTMGR_ADDRESS
+SYSMGR_WARMRAMGRP_ENABLE:	.word	CONFIG_SYSMGR_WARMRAMGRP_ENABLE
+
+.globl reset_clock_manager_size
+reset_clock_manager_size:
+	.word	. - reset_clock_manager
+
-- 
2.7.4