4 files changed, 101 insertions, 2 deletions

diff --git a/arch/s390/boot/Makefile b/arch/s390/boot/Makefile
index 4cf0bddb7d92..e2c47d3a1c89 100644
--- a/arch/s390/boot/Makefile
+++ b/arch/s390/boot/Makefile
@@ -36,7 +36,7 @@ CFLAGS_sclp_early_core.o += -I$(srctree)/drivers/s390/char
 
 obj-y	:= head.o als.o startup.o mem_detect.o ipl_parm.o ipl_report.o
 obj-y	+= string.o ebcdic.o sclp_early_core.o mem.o ipl_vmparm.o cmdline.o
-obj-y	+= version.o ctype.o text_dma.o
+obj-y	+= version.o pgm_check_info.o ctype.o text_dma.o
 obj-$(CONFIG_PROTECTED_VIRTUALIZATION_GUEST)	+= uv.o
 obj-$(CONFIG_RELOCATABLE)	+= machine_kexec_reloc.o
 obj-$(CONFIG_RANDOMIZE_BASE)	+= kaslr.o
diff --git a/arch/s390/boot/boot.h b/arch/s390/boot/boot.h
index 082905d97309..1f7a8d4aff28 100644
--- a/arch/s390/boot/boot.h
+++ b/arch/s390/boot/boot.h
@@ -9,6 +9,7 @@ void setup_boot_command_line(void);
 void parse_boot_command_line(void);
 void setup_memory_end(void);
 void print_missing_facilities(void);
+void print_pgm_check_info(void);
 unsigned long get_random_base(unsigned long safe_addr);
 
 extern int kaslr_enabled;
diff --git a/arch/s390/boot/head.S b/arch/s390/boot/head.S
index 5b79ae7b44f3..4b86a8d3c121 100644
--- a/arch/s390/boot/head.S
+++ b/arch/s390/boot/head.S
@@ -365,12 +365,20 @@ ENTRY(startup_pgm_check_handler)
 	stctg	%c0,%c15,__LC_CREGS_SAVE_AREA-4095(%r1)
 	mvc	__LC_GPREGS_SAVE_AREA-4095(128,%r1),__LC_SAVE_AREA_SYNC
 	mvc	__LC_PSW_SAVE_AREA-4095(16,%r1),__LC_PGM_OLD_PSW
-	lg	%r1,__LC_SAVE_AREA_SYNC+8
 	mvc	__LC_RETURN_PSW(16),__LC_PGM_OLD_PSW
 	ni	__LC_RETURN_PSW,0xfc	# remove IO and EX bits
 	ni	__LC_RETURN_PSW+1,0xfb	# remove MCHK bit
 	oi	__LC_RETURN_PSW+1,0x2	# set wait state bit
+	larl	%r2,.Lold_psw_disabled_wait
+	stg	%r2,__LC_PGM_NEW_PSW+8
+	l	%r15,.Ldump_info_stack-.Lold_psw_disabled_wait(%r2)
+	brasl	%r14,print_pgm_check_info
+.Lold_psw_disabled_wait:
+	la	%r1,4095
+	lmg	%r0,%r15,__LC_GPREGS_SAVE_AREA-4095(%r1)
 	lpswe	__LC_RETURN_PSW		# disabled wait
+.Ldump_info_stack:
+	.long	0x5000 + PAGE_SIZE - STACK_FRAME_OVERHEAD
 ENDPROC(startup_pgm_check_handler)
 
 #
diff --git a/arch/s390/boot/pgm_check_info.c b/arch/s390/boot/pgm_check_info.c
new file mode 100644
index 000000000000..83b5b7915c32
--- /dev/null
+++ b/arch/s390/boot/pgm_check_info.c
@@ -0,0 +1,90 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <asm/lowcore.h>
+#include <asm/sclp.h>
+#include "boot.h"
+
+const char hex_asc[] = "0123456789abcdef";
+
+#define add_val_as_hex(dst, val)					       \
+	__add_val_as_hex(dst, (const unsigned char *)&val, sizeof(val))
+
+static char *__add_val_as_hex(char *dst, const unsigned char *src, size_t count)
+{
+	while (count--)
+		dst = hex_byte_pack(dst, *src++);
+	return dst;
+}
+
+static char *add_str(char *dst, char *src)
+{
+	strcpy(dst, src);
+	return dst + strlen(dst);
+}
+
+void print_pgm_check_info(void)
+{
+	struct psw_bits *psw = &psw_bits(S390_lowcore.psw_save_area);
+	unsigned short ilc = S390_lowcore.pgm_ilc >> 1;
+	char buf[256];
+	int row, col;
+	char *p;
+
+	add_str(buf, "Linux version ");
+	strlcat(buf, kernel_version, sizeof(buf));
+	sclp_early_printk(buf);
+
+	p = add_str(buf, "Kernel fault: interruption code ");
+	p = add_val_as_hex(buf + strlen(buf), S390_lowcore.pgm_code);
+	p = add_str(p, " ilc:");
+	*p++ = hex_asc_lo(ilc);
+	add_str(p, "\n");
+	sclp_early_printk(buf);
+
+	p = add_str(buf, "PSW : ");
+	p = add_val_as_hex(p, S390_lowcore.psw_save_area.mask);
+	p = add_str(p, " ");
+	p = add_val_as_hex(p, S390_lowcore.psw_save_area.addr);
+	add_str(p, "\n");
+	sclp_early_printk(buf);
+
+	p = add_str(buf, "      R:");
+	*p++ = hex_asc_lo(psw->per);
+	p = add_str(p, " T:");
+	*p++ = hex_asc_lo(psw->dat);
+	p = add_str(p, " IO:");
+	*p++ = hex_asc_lo(psw->io);
+	p = add_str(p, " EX:");
+	*p++ = hex_asc_lo(psw->ext);
+	p = add_str(p, " Key:");
+	*p++ = hex_asc_lo(psw->key);
+	p = add_str(p, " M:");
+	*p++ = hex_asc_lo(psw->mcheck);
+	p = add_str(p, " W:");
+	*p++ = hex_asc_lo(psw->wait);
+	p = add_str(p, " P:");
+	*p++ = hex_asc_lo(psw->pstate);
+	p = add_str(p, " AS:");
+	*p++ = hex_asc_lo(psw->as);
+	p = add_str(p, " CC:");
+	*p++ = hex_asc_lo(psw->cc);
+	p = add_str(p, " PM:");
+	*p++ = hex_asc_lo(psw->pm);
+	p = add_str(p, " RI:");
+	*p++ = hex_asc_lo(psw->ri);
+	p = add_str(p, " EA:");
+	*p++ = hex_asc_lo(psw->eaba);
+	add_str(p, "\n");
+	sclp_early_printk(buf);
+
+	for (row = 0; row < 4; row++) {
+		p = add_str(buf, row == 0 ? "GPRS:" : "     ");
+		for (col = 0; col < 4; col++) {
+			p = add_str(p, " ");
+			p = add_val_as_hex(p, S390_lowcore.gpregs_save_area[row * 4 + col]);
+		}
+		add_str(p, "\n");
+		sclp_early_printk(buf);
+	}
+}