[kvm-unit-tests PATCH v1 4/5] s390x: query and store STFL(E) facilitites

[David Hildenbrand <david@redhat.com>]

cpacf.h will require the test_facility() function, so let's properly
query and store the STFL(E) facilities.

STFLE requires double word alignment.

Signed-off-by: David Hildenbrand <david@redhat.com>
---
 lib/s390x/asm/facility.h | 48 ++++++++++++++++++++++++++++++++++++++++++++++++
 lib/s390x/io.c           |  3 +++
 2 files changed, 51 insertions(+)
 create mode 100644 lib/s390x/asm/facility.h

diff --git a/lib/s390x/asm/facility.h b/lib/s390x/asm/facility.h
new file mode 100644
index 0000000..5103dd4
--- /dev/null
+++ b/lib/s390x/asm/facility.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2017 Red Hat Inc
+ *
+ * Authors:
+ *  David Hildenbrand <david@redhat.com>
+ *
+ * This code is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU Library General Public License version 2.
+ */
+#ifndef _ASM_S390X_FACILITY_H_
+#define _ASM_S390X_FACILITY_H_
+
+#include <libcflat.h>
+#include <asm/facility.h>
+#include <asm/arch_def.h>
+
+#define NR_STFL_BYTES 256
+extern uint8_t stfl_bytes[];
+
+static inline bool test_facility(int nr)
+{
+	return stfl_bytes[nr / 8] & (0x80U >> (nr % 8));
+}
+
+static inline void stfl(void)
+{
+	asm volatile("	stfl	0(0)\n");
+}
+
+static inline void stfle(uint8_t *fac, unsigned int len)
+{
+	register unsigned long r0 asm("0") = len - 1;
+
+	asm volatile("	.insn	s,0xb2b00000,0(%1)\n"
+		     : "+d" (r0) : "a" (fac) : "memory", "cc");
+}
+
+static inline void setup_facilities(void)
+{
+	struct lowcore *lc = NULL;
+
+	stfl();
+	memcpy(stfl_bytes, &lc->stfl, sizeof(lc->stfl));
+	if (test_facility(7))
+		stfle(stfl_bytes, NR_STFL_BYTES);
+}
+
+#endif
diff --git a/lib/s390x/io.c b/lib/s390x/io.c
index 4ab5bd9..12f9d26 100644
--- a/lib/s390x/io.c
+++ b/lib/s390x/io.c
@@ -13,9 +13,11 @@
 #include <libcflat.h>
 #include <argv.h>
 #include <asm/spinlock.h>
+#include <asm/facility.h>
 #include "sclp.h"
 
 extern char ipl_args[];
+uint8_t stfl_bytes[NR_STFL_BYTES] __attribute__((aligned(8)));
 
 static struct spinlock lock;
 
@@ -39,6 +41,7 @@ static void sigp_stop()
 void setup()
 {
 	setup_args_progname(ipl_args);
+	setup_facilities();
 	sclp_setup();
 }
 
-- 
2.13.5