Subject: [PATCH] [FEAT RAS1401] zipl: Add vector register support
From: Michael Holzheu <holzheu@linux.vnet.ibm.com>

Summary:     zipl: Add vector register support
Description: Save vector registers to "vector_save_area_addr" address
             that is provided by the Linux kernel.
Upstream-ID: -
Problem-ID:  RAS1401

Signed-off-by: Michael Holzheu <holzheu@linux.vnet.ibm.com>
---
 zdump/df_elf.h         |   14 +++++
 zdump/dfi.c            |   59 +++++++++++++++++++++++++
 zdump/dfi.h            |   37 +++++++++++----
 zdump/dfi_elf.c        |   28 +++++++++++
 zdump/dfo_elf.c        |   24 +++++++++-
 zdump/dfo_s390.c       |   27 +++++++++++
 zipl/boot/libc.h       |    3 -
 zipl/boot/s390.h       |  115 +++++++++++++++++++++++++++++++++++++++++++++++--
 zipl/boot/stage2dump.c |   36 +++++++++++++--
 9 files changed, 322 insertions(+), 21 deletions(-)

--- a/zdump/df_elf.h
+++ b/zdump/df_elf.h
@@ -51,6 +51,20 @@
 #endif
 
 /*
+ * S390 vector registers 0-15 upper half note (16 * u64)
+ */
+#ifndef NT_S390_VXRS_LOW
+#define NT_S390_VXRS_LOW 0x309
+#endif
+
+/*
+ * S390 vector registers 16-31 note (16 * u128)
+ */
+#ifndef NT_S390_VXRS_HIGH
+#define NT_S390_VXRS_HIGH 0x30a
+#endif
+
+/*
  * prstatus ELF Note
  */
 struct nt_prstatus_64 {
--- a/zdump/dfi.c
+++ b/zdump/dfi.c
@@ -39,6 +39,7 @@ u64 dfi_live_dump_magic = 0x4c4956454455
 struct cpus {
 	struct util_list	list;
 	enum dfi_cpu_content	content;
+	int			fac;
 	unsigned int		cnt;
 };
 
@@ -472,6 +473,22 @@ enum dfi_cpu_content dfi_cpu_content(voi
 }
 
 /*
+ * Add CPU facility
+ */
+void dfi_cpu_content_fac_add(int flags)
+{
+	l.cpus.fac |= flags;
+}
+
+/*
+ * Check CPU facility
+ */
+int dfi_cpu_content_fac_check(int flags)
+{
+	return l.cpus.fac & flags;
+}
+
+/*
  * Set DFI architecture
  */
 void dfi_arch_set(enum dfi_arch arch)
@@ -742,6 +759,9 @@ void dfi_cpu_64_to_32(struct dfi_cpu_32
  */
 static void lc2cpu_64(struct dfi_cpu *cpu, struct dfi_lowcore_64 *lc)
 {
+	char vx_sa[DFI_VX_SA_SIZE];
+	int i;
+
 	memcpy(&cpu->gprs, lc->gpregs_save_area, sizeof(cpu->gprs));
 	memcpy(&cpu->ctrs, lc->cregs_save_area, sizeof(cpu->ctrs));
 	memcpy(&cpu->acrs, lc->access_regs_save_area, sizeof(cpu->acrs));
@@ -752,6 +772,14 @@ static void lc2cpu_64(struct dfi_cpu *cp
 	memcpy(&cpu->timer, lc->timer_save_area, sizeof(cpu->timer));
 	memcpy(&cpu->todpreg, &lc->tod_progreg_save_area, sizeof(cpu->todpreg));
 	memcpy(&cpu->todcmp, lc->clock_comp_save_area, sizeof(cpu->todcmp));
+	/* Add VX registers if available */
+	if (!dfi_cpu_lc_has_vx_sa(lc))
+		return;
+	dfi_mem_read(lc->vector_save_area_addr, &vx_sa, sizeof(vx_sa));
+	memcpy(cpu->vxrs_high, &vx_sa[16 * 16], sizeof(cpu->vxrs_high));
+	for (i = 0; i < 16; i++)
+		memcpy(&cpu->vxrs_low[i], &vx_sa[16 * i + 8], sizeof(u64));
+	dfi_cpu_content_fac_add(DFI_CPU_CONTENT_FAC_VX);
 }
 
 /*
@@ -803,6 +831,37 @@ void dfi_cpu_add_from_lc(u32 lc_addr)
 }
 
 /*
+ * Check if lowcore has VX registers
+ */
+int dfi_cpu_lc_has_vx_sa(void *_lc)
+{
+	struct dfi_lowcore_64 *lc = _lc;
+
+	if (l.arch == DFI_ARCH_32)
+		return 0;
+	if (lc->vector_save_area_addr == 0)
+		return 0;
+	if (lc->vector_save_area_addr % 1024 != 0)
+		return 0;
+	return 1;
+}
+
+/*
+ * Copy VX registers out of save areas
+ */
+void dfi_cpu_vx_copy(void *buf, struct dfi_cpu *cpu)
+{
+	char *_buf = buf;
+	int i;
+
+	for (i = 0; i < 16; i++) {
+		memcpy(&_buf[i * 16], &cpu->fprs[i], 8);
+		memcpy(&_buf[i * 16 + 8], &cpu->vxrs_low[i], 8);
+	}
+	memcpy(&_buf[16 * 16], &cpu->vxrs_high[0], 16 * 16);
+}
+
+/*
  * Return kdump base
  */
 unsigned long dfi_kdump_base(void)
--- a/zdump/dfi.h
+++ b/zdump/dfi.h
@@ -48,7 +48,9 @@ struct dfi_lowcore_32 {
 struct dfi_lowcore_64 {
 	u8	pad_0x0000[0x0084 - 0x0000];	/* 0x0000 */
 	u16	cpu_addr;			/* 0x0084 */
-	u8	pad_0x0086[0x1200 - 0x0086];	/* 0x0086 */
+	u8	pad_0x0086[0x11b0 - 0x0086];	/* 0x0086 */
+	u64	vector_save_area_addr;		/* 0x11b0 */
+	u8	pad_0x11b8[0x1200 - 0x11b8];	/* 0x11b8 */
 	u64	floating_pt_save_area[16];	/* 0x1200 */
 	u64	gpregs_save_area[16];		/* 0x1280 */
 	u32	st_status_fixed_logout[4];	/* 0x1300 */
@@ -73,6 +75,11 @@ static inline u64 dfi_lc_size(enum dfi_a
 		return 0x1000;
 }
 
+struct dfi_vxrs {
+	u64 low;
+	u64 high;
+};
+
 struct dfi_cpu {
 	struct util_list_node	list;
 	u64		gprs[16];
@@ -85,17 +92,21 @@ struct dfi_cpu {
 	u64		timer;
 	u64		todcmp;
 	u32		todpreg;
+	u64		vxrs_low[16];
+	struct dfi_vxrs	vxrs_high[16];
 };
 
 struct dfi_cpu_32 {
-	u32	gprs[16];
-	u32	ctrs[16];
-	u32	acrs[16];
-	u64	fprs[4];
-	u32	psw[2];
-	u32	prefix;
-	u64	timer;
-	u64	todcmp;
+	u32		gprs[16];
+	u32		ctrs[16];
+	u32		acrs[16];
+	u64		fprs[4];
+	u32		psw[2];
+	u32		prefix;
+	u64		timer;
+	u64		todcmp;
+	u64		vxrs_low[16];
+	struct dfi_vxrs	vxrs_high[16];
 };
 
 extern void dfi_cpu_64_to_32(struct dfi_cpu_32 *cpu_32, struct dfi_cpu *cpu_64);
@@ -110,6 +121,10 @@ enum dfi_cpu_content {
 	DFI_CPU_CONTENT_ALL,	/* Complete register information available */
 };
 
+#define DFI_CPU_CONTENT_FAC_VX	0x00000001
+extern int dfi_cpu_content_fac_check(int falgs);
+extern void dfi_cpu_content_fac_add(int flags);
+
 #define dfi_cpu_iterate(cpu) \
 	util_list_iterate(dfi_cpu_list(), cpu)
 
@@ -122,6 +137,10 @@ extern unsigned int dfi_cpu_cnt(void);
 extern enum dfi_cpu_content dfi_cpu_content(void);
 extern void dfi_cpu_add_from_lc(u32 lc_addr);
 
+#define DFI_VX_SA_SIZE		(32 * 16)
+extern int dfi_cpu_lc_has_vx_sa(void *lc);
+extern void dfi_cpu_vx_copy(void *buf, struct dfi_cpu *cpu);
+
 /*
  * Mem chunk functions and definitions
  */
--- a/zdump/dfi_elf.c
+++ b/zdump/dfi_elf.c
@@ -173,6 +173,24 @@ static int nt_s390_prefix_read(struct df
 }
 
 /*
+ * Read s390 vxrs_low note
+ */
+static int nt_s390_vxrs_low_read(struct dfi_cpu *cpu, Elf64_Nhdr *note)
+{
+	check_cpu(cpu, "S390_VXRS_LOW");
+	return nt_read(note, &cpu->vxrs_low);
+}
+
+/*
+ * Read s390 vxrs_high note
+ */
+static int nt_s390_vxrs_high_read(struct dfi_cpu *cpu, Elf64_Nhdr *note)
+{
+	check_cpu(cpu, "S390_VXRS_HIGH");
+	return nt_read(note, &cpu->vxrs_high);
+}
+
+/*
  * Add all notes for notes phdr
  */
 static int pt_notes_add(Elf64_Phdr *phdr)
@@ -219,6 +237,16 @@ static int pt_notes_add(Elf64_Phdr *phdr
 			if (nt_s390_prefix_read(cpu_current, &note))
 				return -EINVAL;
 			break;
+		case NT_S390_VXRS_LOW:
+			if (nt_s390_vxrs_low_read(cpu_current, &note))
+				return -EINVAL;
+			dfi_cpu_content_fac_add(DFI_CPU_CONTENT_FAC_VX);
+			break;
+		case NT_S390_VXRS_HIGH:
+			if (nt_s390_vxrs_high_read(cpu_current, &note))
+				return -EINVAL;
+			dfi_cpu_content_fac_add(DFI_CPU_CONTENT_FAC_VX);
+			break;
 		default:
 			if (nt_skip(&note))
 				return -EINVAL;
--- a/zdump/dfo_elf.c
+++ b/zdump/dfo_elf.c
@@ -15,7 +15,7 @@
 #include <assert.h>
 #include "zgetdump.h"
 
-#define HDR_PER_CPU_SIZE	0x300
+#define HDR_PER_CPU_SIZE	0x4a0
 #define HDR_PER_MEMC_SIZE	0x100
 #define HDR_BASE_SIZE		0x2000
 
@@ -183,6 +183,24 @@ static void *nt_s390_prefix(void *ptr, s
 }
 
 /*
+ * Initialize vxrs_low register note
+ */
+static void *nt_s390_vxrs_low(void *ptr, struct dfi_cpu *cpu)
+{
+	return nt_init(ptr, NT_S390_VXRS_LOW, &cpu->vxrs_low,
+			 sizeof(cpu->vxrs_low), "LINUX");
+}
+
+/*
+ * Initialize vxrs_high register note
+ */
+static void *nt_s390_vxrs_high(void *ptr, struct dfi_cpu *cpu)
+{
+	return nt_init(ptr, NT_S390_VXRS_HIGH, &cpu->vxrs_high,
+			 sizeof(cpu->vxrs_high), "LINUX");
+}
+
+/*
  * Initialize prpsinfo note
  */
 static void *nt_prpsinfo(void *ptr)
@@ -231,6 +249,10 @@ static void *notes_init(Elf64_Phdr *phdr
 		ptr = nt_s390_tod_preg(ptr, cpu);
 		ptr = nt_s390_ctrs(ptr, cpu);
 		ptr = nt_s390_prefix(ptr, cpu);
+		if (dfi_cpu_content_fac_check(DFI_CPU_CONTENT_FAC_VX)) {
+			ptr = nt_s390_vxrs_low(ptr, cpu);
+			ptr = nt_s390_vxrs_high(ptr, cpu);
+		}
 	}
 out:
 	ptr = nt_vmcoreinfo(ptr);
--- a/zdump/dfo_s390.c
+++ b/zdump/dfo_s390.c
@@ -104,17 +104,44 @@ static void dfo_s390_dump_chunk_lc_fn(st
 }
 
 /*
+ * Copy register set to prefix page
+ */
+static void dfo_s390_dump_chunk_vx_fn(struct dfo_chunk *dump_chunk,
+				      u64 off, void *buf, u64 cnt)
+{
+	char *vx_regs = dump_chunk->data;
+
+	memcpy(buf, &vx_regs[off], cnt);
+}
+
+/*
  * Add register set to dump layout. We copy the register sets to the
  * lowcore pages.
  */
 static void add_cpu_to_dfo(struct dfi_cpu *cpu)
 {
+	struct dfi_lowcore_64 lc;
+	void *vx_regs;
+
 	if (dfi_cpu_content() != DFI_CPU_CONTENT_ALL)
 		return;
 
+	/* Add lowcore to memory */
 	dfo_chunk_add(cpu->prefix + DF_S390_HDR_SIZE,
 		      dfi_lc_size(dfi_arch()), cpu,
 		      dfo_s390_dump_chunk_lc_fn);
+	/* Add VX save area to memory */
+	if (dfi_arch() != DFI_ARCH_64)
+		return;
+	if (!dfi_cpu_content_fac_check(DFI_CPU_CONTENT_FAC_VX))
+		return;
+	dfi_mem_read(cpu->prefix, &lc, sizeof(lc));
+	if (!dfi_cpu_lc_has_vx_sa(&lc))
+		return;
+	vx_regs = zg_alloc(DFI_VX_SA_SIZE);
+	dfi_cpu_vx_copy(vx_regs, cpu);
+	dfo_chunk_add(lc.vector_save_area_addr + DF_S390_HDR_SIZE,
+		      DFI_VX_SA_SIZE, vx_regs, dfo_s390_dump_chunk_vx_fn);
 }
 
 /*
--- a/zipl/boot/libc.h
+++ b/zipl/boot/libc.h
@@ -45,8 +45,6 @@ typedef unsigned int uint32_t;
 typedef unsigned short uint16_t;
 typedef unsigned char uint8_t;
 
-#include "s390.h"
-
 void printf(const char *, ...);
 void sprintf(char *, const char *, ...);
 void *memcpy(void *, const void *, unsigned long);
@@ -63,7 +61,6 @@ void initialize(void);
 void libc_stop(unsigned long) __attribute__((noreturn));
 void start(void);
 void pgm_check_handler(void);
-void load_wait_psw(uint64_t, struct psw_t *);
 void pgm_check_handler_fn(void);
 
 static inline int isdigit(int c)
--- a/zipl/boot/s390.h
+++ b/zipl/boot/s390.h
@@ -35,6 +35,8 @@ struct psw_t {
 	uint64_t addr;
 } __aligned(8);
 
+void load_wait_psw(uint64_t, struct psw_t *);
+
 struct _lowcore {
 	uint8_t	pad_0x0000[0x0014-0x0000];	/* 0x0000 */
 	uint32_t	ipl_parmblock_ptr;		/* 0x0014 */
@@ -161,8 +163,8 @@ struct _lowcore {
 
 	/* Extended facility list */
 	uint64_t	stfle_fac_list[32];		/* 0x0f00 */
-	uint8_t	pad_0x1000[0x11b8-0x1000];	/* 0x1000 */
-
+	uint8_t	pad_0x1000[0x11b0-0x1000];	/* 0x1000 */
+	uint64_t	vector_save_area_addr;		/* 0x11b0 */
 	/* 64 bit extparam used for pfault/diag 250: defined by architecture */
 	uint64_t	ext_params2;			/* 0x11B8 */
 	uint8_t	pad_0x11c0[0x1200-0x11C0];	/* 0x11C0 */
@@ -218,7 +220,7 @@ static inline int tprot(unsigned long ad
 		"0:     ipm     %0\n"
 		"       srl     %0,28\n"
 		"1:\n"
-		EX_TABLE(0b,1b)
+		EX_TABLE(0b, 1b)
 		: "+d" (rc) : "a" (addr) : "cc");
 	return rc;
 }
@@ -259,6 +261,15 @@ static inline uint32_t csum_partial(cons
 		  "i" (low), "i" (high));			\
 })
 
+static inline void __ctl_set_bit(unsigned int cr, unsigned int bit)
+{
+	unsigned long reg;
+
+	__ctl_store(reg, cr, cr);
+	reg |= 1UL << bit;
+	__ctl_load(reg, cr, cr);
+}
+
 /*
  * DIAG 308 support
  */
@@ -288,6 +299,7 @@ static inline int diag308(unsigned long
  */
 #define SIGP_STOP_AND_STORE_STATUS	9
 #define SIGP_SET_MULTI_THREADING	22
+#define SIGP_STORE_ASTATUS_AT_ADDRESS	23
 
 #define SIGP_CC_ORDER_CODE_ACCEPTED	0
 #define SIGP_CC_BUSY			2
@@ -308,6 +320,17 @@ static inline int sigp(uint16_t addr, ui
 	return cc;
 }
 
+static inline int sigp_busy(uint16_t addr, uint8_t order, uint32_t parm,
+			    uint32_t *status)
+{
+	int cc;
+
+	do {
+		cc = sigp(addr, order, parm, status);
+	} while (cc == SIGP_CC_BUSY);
+	return cc;
+}
+
 /*
  * Store CPU address
  */
@@ -372,4 +395,90 @@ static inline int is_zvm(void)
 	return cpuid.version == 0xff;
 }
 
+/*
+ * Vector register definition
+ */
+typedef struct {
+	uint32_t u[4];
+} __vector128;
+
+/*
+ * Save vector registers
+ */
+static inline void save_vx_regs(__vector128 *vxrs)
+{
+	typedef struct { __vector128 _[32]; } addrtype;
+
+	asm volatile(
+		"	la	1,%0\n"
+		"	.word	0xe70f,0x1000,0x003e\n"	/* vstm 0,15,0(1) */
+		"	.word	0xe70f,0x1100,0x0c3e\n"	/* vstm 16,31,256(1) */
+		: "=Q" (*(addrtype *) vxrs) : : "1");
+}
+
+/*
+ * Save vector registers safe
+ */
+static inline void save_vx_regs_safe(__vector128 *vxrs)
+{
+	unsigned long cr0;
+
+	__ctl_store(cr0, 0, 0);
+	__ctl_set_bit(0, 17);
+	__ctl_set_bit(0, 18); /* AFP-register-control */
+	save_vx_regs(vxrs);
+	__ctl_load(cr0, 0, 0);
+}
+
+#define MAX_FACILITY_BIT (256*8)	/* stfle_fac_list has 256 bytes */
+
+static inline int __test_facility(unsigned long nr, void *facilities)
+{
+	unsigned char *ptr;
+
+	if (nr >= MAX_FACILITY_BIT)
+		return 0;
+	ptr = (unsigned char *) facilities + (nr >> 3);
+	return (*ptr & (0x80 >> (nr & 7))) != 0;
+}
+
+/*
+ * The test_facility function uses the bit odering where the MSB is bit 0.
+ * That makes it easier to query facility bits with the bit number as
+ * documented in the Principles of Operation.
+ */
+static inline int test_facility(unsigned long nr)
+{
+	return __test_facility(nr, &S390_lowcore.stfle_fac_list);
+}
+
+/**
+ * stfle - Store facility list extended
+ * @stfle_fac_list: array where facility list can be stored
+ * @size: size of passed in array in double words
+ */
+static inline void stfle(u64 *stfle_fac_list, int size)
+{
+	unsigned long nr;
+
+	asm volatile(
+		"	.insn s,0xb2b10000,0(0)\n" /* stfl */
+		"0:\n"
+		EX_TABLE(0b, 0b)
+		: "+m" (S390_lowcore.stfl_fac_list));
+	nr = 4; /* bytes stored by stfl */
+	memcpy(stfle_fac_list, &S390_lowcore.stfl_fac_list, 4);
+	if (S390_lowcore.stfl_fac_list & 0x01000000) {
+		/* More facility bits available with stfle */
+		register unsigned long reg0 asm("0") = size - 1;
+
+		asm volatile(".insn s,0xb2b00000,0(%1)" /* stfle */
+			     : "+d" (reg0)
+			     : "a" (stfle_fac_list)
+			     : "memory", "cc");
+		nr = (reg0 + 1) * 8; /* # bytes stored by stfle */
+	}
+	memset((char *) stfle_fac_list + nr, 0, size * 8 - nr);
+}
+
 #endif /* S390_H */
--- a/zipl/boot/stage2dump.c
+++ b/zipl/boot/stage2dump.c
@@ -13,10 +13,12 @@
 #include "sclp.h"
 
 #define CPU_ADDRESS_MAX	1000
+#define MACHINE_HAS_VX	machine_has_vx
 
 /*
  * Static globals
  */
+static uint8_t machine_has_vx;
 static unsigned long progress_next_addr;
 static unsigned long progress_inc = 0x20000000; /* Issue message each 512M */
 
@@ -52,6 +54,8 @@ static void init_early(void)
 	/* Set clock comparator and CPU timer to future */
 	set_clock_comparator(0xffffffffffffffffULL);
 	set_cpu_timer(0x7fffffffffffffffULL);
+	if (test_facility(129))
+		machine_has_vx = 1;
 }
 
 /*
@@ -223,11 +227,29 @@ static void copy_lowcore_64(void)
 }
 
 /*
+ * Has machine VX and has kernel prepared save area?
+ */
+static int should_dump_vx(struct _lowcore *lc)
+{
+	unsigned long vx_sa;
+
+	if (!MACHINE_HAS_VX)
+		return 0;
+	vx_sa = lc->vector_save_area_addr;
+	if (vx_sa == 0 || (vx_sa % 1024) != 0)
+		return 0;
+	if (!page_is_valid(vx_sa))
+		return 0;
+	return 1;
+}
+
+/*
  * Do store status
  */
 static void store_status(void)
 {
 	unsigned short current_cpu;
+	struct _lowcore *lc;
 	unsigned long page;
 	int addr, cc;
 
@@ -237,18 +259,22 @@ static void store_status(void)
 
 	current_cpu = stap();
 
+	lc = (void *)(unsigned long) S390_lowcore.prefixreg_save_area;
+	if (should_dump_vx(lc))
+		save_vx_regs_safe((__vector128 *) lc->vector_save_area_addr);
 	copy_lowcore_64();
-
 	for (addr = 0; addr < CPU_ADDRESS_MAX; addr++) {
 		if (addr == current_cpu)
 			continue;
-		do {
-			cc = sigp(addr, SIGP_STOP_AND_STORE_STATUS, 0, NULL);
-		} while (cc == SIGP_CC_BUSY);
-
+		cc = sigp_busy(addr, SIGP_STOP_AND_STORE_STATUS, 0, NULL);
 		if (cc != SIGP_CC_ORDER_CODE_ACCEPTED)
 			continue;
+		lc = (void *)(unsigned long) S390_lowcore.prefixreg_save_area;
 		copy_lowcore_64();
+		if (!should_dump_vx(lc))
+			continue;
+		sigp_busy(addr, SIGP_STORE_ASTATUS_AT_ADDRESS,
+			  lc->vector_save_area_addr, NULL);
 	}
 	/* Restore absolute zero lowcore */
 	memcpy((void *) 0x1000, (void *) page, PAGE_SIZE);
