From 70309dc77699dd598ade67ca7fca5a17ac3275c1 Mon Sep 17 00:00:00 2001
From: Vasily Gorbik <gor@linux.ibm.com>
Date: Fri, 29 Nov 2024 12:56:48 +0100
Subject: [PATCH] s390/boot: Add timestamps to early boot messages

When CONFIG_PRINTK_TIME is enabled, add timestamps to boot messages in
the same format as regular printk. Timestamps appear only with earlyprintk
and are stored in the boot messages ring buffer, but are not propagated
to main kernel messages (if earlyprintk is not enabled). This prevents
double timestamps in the output.

Signed-off-by: Vasily Gorbik <gor@linux.ibm.com>
Acked-by: Heiko Carstens <hca@linux.ibm.com>
Signed-off-by: Alexander Gordeev <agordeev@linux.ibm.com>
---
 arch/s390/boot/printk.c           | 27 ++++++++++++++++++++++-----
 arch/s390/include/asm/boot_data.h | 11 +++++++++++
 arch/s390/kernel/setup.c          |  2 +-
 3 files changed, 34 insertions(+), 6 deletions(-)

diff --git a/arch/s390/boot/printk.c b/arch/s390/boot/printk.c
index abfa05cf9562..b4c66fa667d5 100644
--- a/arch/s390/boot/printk.c
+++ b/arch/s390/boot/printk.c
@@ -190,12 +190,29 @@ static void boot_console_earlyprintk(const char *buf)
 		return;
 	buf = printk_skip_level(buf);
 	/* print debug messages only when bootdebug is enabled */
-	if (level == LOGLEVEL_DEBUG && (!bootdebug || !bootdebug_filter_match(buf)))
+	if (level == LOGLEVEL_DEBUG && (!bootdebug || !bootdebug_filter_match(skip_timestamp(buf))))
 		return;
 	if (boot_ignore_loglevel || level < boot_console_loglevel)
 		sclp_early_printk(buf);
 }
 
+static char *add_timestamp(char *buf)
+{
+#ifdef CONFIG_PRINTK_TIME
+	union tod_clock *boot_clock = (union tod_clock *)&get_lowcore()->boot_clock;
+	unsigned long ns = tod_to_ns(get_tod_clock() - boot_clock->tod);
+	char ts[MAX_NUMLEN];
+
+	*buf++ = '[';
+	buf += strpad(buf, MAX_NUMLEN, as_dec(ts, ns / NSEC_PER_SEC, 0), 5, 0, 0);
+	*buf++ = '.';
+	buf += strpad(buf, MAX_NUMLEN, as_dec(ts, (ns % NSEC_PER_SEC) / NSEC_PER_USEC, 0), 6, 1, 0);
+	*buf++ = ']';
+	*buf++ = ' ';
+#endif
+	return buf;
+}
+
 #define va_arg_len_type(args, lenmod, typemod)				\
 	((lenmod == 'l') ? va_arg(args, typemod long) :			\
 	 (lenmod == 'h') ? (typemod short)va_arg(args, typemod int) :	\
@@ -215,10 +232,10 @@ int boot_printk(const char *fmt, ...)
 	ssize_t len;
 	int pad;
 
-	if (!printk_get_level(fmt)) {
-		*p++ = KERN_SOH_ASCII;
-		*p++ = '0' + MESSAGE_LOGLEVEL_DEFAULT;
-	}
+	*p++ = KERN_SOH_ASCII;
+	*p++ = printk_get_level(fmt) ?: '0' + MESSAGE_LOGLEVEL_DEFAULT;
+	p = add_timestamp(p);
+	fmt = printk_skip_level(fmt);
 
 	va_start(args, fmt);
 	for (; p < end && *fmt; fmt++) {
diff --git a/arch/s390/include/asm/boot_data.h b/arch/s390/include/asm/boot_data.h
index 73c441f5e99a..f55f8227058e 100644
--- a/arch/s390/include/asm/boot_data.h
+++ b/arch/s390/include/asm/boot_data.h
@@ -55,4 +55,15 @@ static inline bool bootdebug_filter_match(const char *buf)
 	return false;
 }
 
+static inline const char *skip_timestamp(const char *buf)
+{
+#ifdef CONFIG_PRINTK_TIME
+	const char *p = memchr(buf, ']', strlen(buf));
+
+	if (p && p[1] == ' ')
+		return p + 2;
+#endif
+	return buf;
+}
+
 #endif /* _ASM_S390_BOOT_DATA_H */
diff --git a/arch/s390/kernel/setup.c b/arch/s390/kernel/setup.c
index 74e2ee75b189..20fc68bdefa1 100644
--- a/arch/s390/kernel/setup.c
+++ b/arch/s390/kernel/setup.c
@@ -892,7 +892,7 @@ static void __init print_rb_entry(const char *buf)
 	char fmt[] = KERN_SOH "0boot: %s";
 	int level = printk_get_level(buf);
 
-	buf = printk_skip_level(buf);
+	buf = skip_timestamp(printk_skip_level(buf));
 	if (level == KERN_DEBUG[1] && (!bootdebug || !bootdebug_filter_match(buf)))
 		return;