1 files changed, 72 insertions, 10 deletions

diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index 91c6028d7b74..16e4a25596e7 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -27,6 +27,7 @@
 #define KMSG_COMPONENT "ap"
 #define pr_fmt(fmt) KMSG_COMPONENT ": " fmt
 
+#include <linux/kernel_stat.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/delay.h>
@@ -154,14 +155,7 @@ static inline int ap_instructions_available(void)
  */
 static int ap_interrupts_available(void)
 {
-        unsigned long long facility_bits[2];
-
-        if (stfle(facility_bits, 2) <= 1)
-                return 0;
-        if (!(facility_bits[0] & (1ULL << 61)) ||
-            !(facility_bits[1] & (1ULL << 62)))
-                return 0;
-        return 1;
+        return test_facility(2) && test_facility(65);
 }
 
 /**
@@ -228,6 +222,69 @@ ap_queue_interruption_control(ap_qid_t qid, void *ind)
 }
 #endif
 
+static inline struct ap_queue_status __ap_4096_commands_available(ap_qid_t qid,
+                                                                  int *support)
+{
+        register unsigned long reg0 asm ("0") = 0UL | qid | (1UL << 23);
+        register struct ap_queue_status reg1 asm ("1");
+        register unsigned long reg2 asm ("2") = 0UL;
+
+        asm volatile(
+                ".long 0xb2af0000\n"
+                "0: la    %1,0\n"
+                "1:\n"
+                EX_TABLE(0b, 1b)
+                : "+d" (reg0), "=d" (reg1), "=d" (reg2)
+                :
+                : "cc");
+
+        if (reg2 & 0x6000000000000000ULL)
+                *support = 1;
+        else
+                *support = 0;
+
+        return reg1;
+}
+
+/**
+ * ap_4096_commands_availablen(): Check for availability of 4096 bit RSA
+ * support.
+ * @qid: The AP queue number
+ *
+ * Returns 1 if 4096 bit RSA keys are support fo the AP, returns 0 if not.
+ */
+int ap_4096_commands_available(ap_qid_t qid)
+{
+        struct ap_queue_status status;
+        int i, support = 0;
+        status = __ap_4096_commands_available(qid, &support);
+
+        for (i = 0; i < AP_MAX_RESET; i++) {
+                switch (status.response_code) {
+                case AP_RESPONSE_NORMAL:
+                        return support;
+                case AP_RESPONSE_RESET_IN_PROGRESS:
+                case AP_RESPONSE_BUSY:
+                        break;
+                case AP_RESPONSE_Q_NOT_AVAIL:
+                case AP_RESPONSE_DECONFIGURED:
+                case AP_RESPONSE_CHECKSTOPPED:
+                case AP_RESPONSE_INVALID_ADDRESS:
+                        return 0;
+                case AP_RESPONSE_OTHERWISE_CHANGED:
+                        break;
+                default:
+                        break;
+                }
+                if (i < AP_MAX_RESET - 1) {
+                        udelay(5);
+                        status = __ap_4096_commands_available(qid, &support);
+                }
+        }
+        return support;
+}
+EXPORT_SYMBOL(ap_4096_commands_available);
+
 /**
  * ap_queue_enable_interruption(): Enable interruption on an AP.
  * @qid: The AP queue number
@@ -1049,6 +1106,7 @@ out:
 
 static void ap_interrupt_handler(void *unused1, void *unused2)
 {
+        kstat_cpu(smp_processor_id()).irqs[IOINT_APB]++;
         tasklet_schedule(&ap_tasklet);
 }
 
@@ -1125,8 +1183,12 @@ static void ap_scan_bus(struct work_struct *unused)
                 INIT_LIST_HEAD(&ap_dev->list);
                 setup_timer(&ap_dev->timeout, ap_request_timeout,
                             (unsigned long) ap_dev);
-                if (device_type == 0)
-                        ap_probe_device_type(ap_dev);
+                if (device_type == 0) {
+                        if (ap_probe_device_type(ap_dev)) {
+                                kfree(ap_dev);
+                                continue;
+                        }
+                }
                 else
                         ap_dev->device_type = device_type;