[PATCH master 2/2] ARM64: fall back to reading the counter directly in the PBL udelay()

[Ahmad Fatoum]

Since commit 67531b21addc ("ARM64: enable PBL_CLOCKSOURCE compatibility"),
udelay() in PBL expects a clocksource to have been registered. That
happens from a constructor in setup_c(), but i.MX93 already polls the
ELE mailbox from its unrelocated lowlevel init before that and now
panics with "No PBL clocksource has been initialized".

Let's override udelay(), get_time_ns() and is_timeout() in the ARM64
PBL: As long as no clocksource is registered, udelay() reads the counter
directly like it used to and time stands still, so timeouts never fire.
The check happens in assembly without touching the stack and
current_clock is moved to .data, so this is usable from image entry on,
before BSS is cleared.

Fixes: 67531b21addc ("ARM64: enable PBL_CLOCKSOURCE compatibility")
Assisted-by: Claude:fable-5.1
Signed-off-by: Ahmad Fatoum <a.fatoum at pengutronix.de>
---
 arch/arm/lib64/Makefile                 |  1 +
 arch/arm/lib64/clocksource-registered.S | 16 +++++++++
 arch/arm/lib64/delay.c                  | 47 +++++++++++++++++++++++++
 common/clock.c                          |  4 ++-
 include/clock.h                         |  5 +++
 5 files changed, 72 insertions(+), 1 deletion(-)
 create mode 100644 arch/arm/lib64/clocksource-registered.S
 create mode 100644 arch/arm/lib64/delay.c

diff --git a/arch/arm/lib64/Makefile b/arch/arm/lib64/Makefile
index 218de68a5468..a3b0b00f3024 100644
--- a/arch/arm/lib64/Makefile
+++ b/arch/arm/lib64/Makefile
@@ -11,4 +11,5 @@ obj-pbl-y   += setjmp.o
 obj-pbl-y   += reloc.o
 obj-y += io.o
 pbl-$(CONFIG_CLOCKSOURCE_ARM_ARCHITECTED_TIMER)	+= clocksource.o
+pbl-$(CONFIG_PBL_CLOCKSOURCE)	+= delay.o clocksource-registered.o
 pbl-y	+= div0.o
diff --git a/arch/arm/lib64/clocksource-registered.S b/arch/arm/lib64/clocksource-registered.S
new file mode 100644
index 000000000000..242643926388
--- /dev/null
+++ b/arch/arm/lib64/clocksource-registered.S
@@ -0,0 +1,16 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+
+/*
+ * Returns nonzero once init_clock() has set current_clock. Reads it
+ * PC-relatively and uses no stack, so it works before relocation,
+ * before BSS is cleared and before a stack is set up. Clobbers x0 only.
+ */
+.section .text.clocksource_registered, "ax"
+ENTRY(clocksource_registered)
+	ldr_l	x0, current_clock
+	cmp	x0, #0
+	cset	w0, ne
+	ret
+ENDPROC(clocksource_registered)
diff --git a/arch/arm/lib64/delay.c b/arch/arm/lib64/delay.c
new file mode 100644
index 000000000000..1ff770ca5450
--- /dev/null
+++ b/arch/arm/lib64/delay.c
@@ -0,0 +1,47 @@
+// SPDX-License-Identifier: GPL-2.0-only
+
+#include <asm/system.h>
+#include <clock.h>
+
+/* Uses only architected timer system registers, so usable before relocation */
+static __always_inline void arch_timer_udelay(unsigned long us)
+{
+	unsigned long cntfrq = get_cntfrq();
+	unsigned long ticks = (us * cntfrq) / 1000000;
+	unsigned long start = get_cntpct();
+
+	while ((long)(start + ticks - get_cntpct()) > 0)
+		;
+}
+
+/*
+ * Until a clocksource is registered, read the counter directly, which
+ * works as soon as CNTFRQ_EL0 is programmed, even before relocation.
+ */
+void __prereloc udelay(unsigned long us)
+{
+	if (clocksource_registered())
+		clocksource_current_udelay(us);
+	else
+		arch_timer_udelay(us);
+}
+
+/*
+ * Until a clocksource is registered, time stands still and timeouts
+ * never fire, so polls wait indefinitely instead of panicking.
+ */
+uint64_t __prereloc get_time_ns(void)
+{
+	if (clocksource_registered())
+		return clocksource_current_get_time_ns();
+
+	return 0;
+}
+
+int __prereloc is_timeout(uint64_t start_ns, uint64_t time_offset_ns)
+{
+	if (clocksource_registered())
+		return clocksource_current_is_timeout(start_ns, time_offset_ns);
+
+	return 0;
+}
diff --git a/common/clock.c b/common/clock.c
index 0f2f47ed5399..67c02f369a6c 100644
--- a/common/clock.c
+++ b/common/clock.c
@@ -35,7 +35,9 @@ static struct clocksource dummy_cs = {
 	.priority = -1,
 };
 
-static struct clocksource *current_clock = IN_PROPER ? &dummy_cs : NULL;
+/* in .data, so clocksource_registered() reads NULL even before BSS is cleared */
+struct clocksource *current_clock __section(.data) =
+	IN_PROPER ? &dummy_cs : NULL;
 
 static int dummy_csrc_warn(void)
 {
diff --git a/include/clock.h b/include/clock.h
index ea87bfd2020a..88de5deb47b9 100644
--- a/include/clock.h
+++ b/include/clock.h
@@ -53,6 +53,11 @@ static inline int is_timeout_interruptible(uint64_t start_ns,
 
 void arm_architected_timer_udelay(unsigned long us);
 
+extern struct clocksource *current_clock;
+
+/* implemented in assembly, so usable before relocation and stack setup */
+bool clocksource_registered(void);
+
 void ndelay(unsigned long nsecs);
 void udelay(unsigned long usecs);
 void mdelay(unsigned long msecs);
-- 
2.47.3