1 files changed, 34 insertions, 0 deletions

diff --git a/arch/s390/include/asm/system.h b/arch/s390/include/asm/system.h
index d73cc6b60000..2e0bb7f0f9b2 100644
--- a/arch/s390/include/asm/system.h
+++ b/arch/s390/include/asm/system.h
@@ -7,8 +7,10 @@
 #ifndef __ASM_SYSTEM_H
 #define __ASM_SYSTEM_H
 
+#include <linux/preempt.h>
 #include <linux/kernel.h>
 #include <linux/errno.h>
+#include <linux/string.h>
 #include <asm/types.h>
 #include <asm/ptrace.h>
 #include <asm/setup.h>
@@ -248,6 +250,38 @@ static inline int test_facility(unsigned long nr)
         return (*ptr & (0x80 >> (nr & 7))) != 0;
 }
 
+/**
+ * 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;
+
+        preempt_disable();
+        S390_lowcore.stfl_fac_list = 0;
+        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);
+        preempt_enable();
+}
+
 static inline unsigned short stap(void)
 {
         unsigned short cpu_address;