[kvm-unit-tests PATCH 3/5] lib: s390x: ap: Add ap_setup

[Janosch Frank <frankja@linux.ibm.com>]

For the next tests we need a valid queue which means we need to grab
the qci info and search the first set bit in the ap and aq masks.

Let's move from the ap_check function to a proper setup function that
also returns the first usable APQN. Later we can extend the setup to
build a list of all available APQNs but right now one APQN is plenty.

Signed-off-by: Janosch Frank <frankja@linux.ibm.com>
---
 lib/s390x/ap.c | 54 +++++++++++++++++++++++++++++++++++++++++++++++++-
 lib/s390x/ap.h |  5 ++++-
 s390x/ap.c     |  7 ++++++-
 3 files changed, 63 insertions(+), 3 deletions(-)

diff --git a/lib/s390x/ap.c b/lib/s390x/ap.c
index 374fa210..8d7f2992 100644
--- a/lib/s390x/ap.c
+++ b/lib/s390x/ap.c
@@ -13,9 +13,12 @@
 
 #include <libcflat.h>
 #include <interrupt.h>
+#include <bitops.h>
 #include <ap.h>
 #include <asm/time.h>
 
+static struct ap_config_info qci;
+
 int ap_pqap_tapq(uint8_t ap, uint8_t qn, struct ap_queue_status *apqsw,
 		 struct pqap_r2 *r2)
 {
@@ -77,7 +80,44 @@ int ap_pqap_qci(struct ap_config_info *info)
 	return cc;
 }
 
-bool ap_check(void)
+static int ap_get_apqn(uint8_t *ap, uint8_t *qn)
+{
+	unsigned long *ptr;
+	uint8_t bit;
+	int i;
+
+	ap_pqap_qci(&qci);
+	*ap = 0;
+	*qn = 0;
+
+	ptr = (unsigned long *)qci.apm;
+	for (i = 0; i < 4; i++) {
+		bit = fls(*ptr);
+		if (bit) {
+			*ap = i * 64 + 64 - bit;
+			break;
+		}
+	}
+	ptr = (unsigned long *)qci.aqm;
+	for (i = 0; i < 4; i++) {
+		bit = fls(*ptr);
+		if (bit) {
+			*qn = i * 64 + 64 - bit;
+			break;
+		}
+	}
+
+	if (!*ap || !*qn)
+		return -1;
+
+	/* fls returns 1 + bit number, so we need to remove 1 here */
+	*ap -= 1;
+	*qn -= 1;
+
+	return 0;
+}
+
+static bool ap_check(void)
 {
 	struct ap_queue_status r1 = {};
 	struct pqap_r2 r2 = {};
@@ -91,3 +131,15 @@ bool ap_check(void)
 
 	return true;
 }
+
+int ap_setup(uint8_t *ap, uint8_t *qn)
+{
+	if (!ap_check())
+		return AP_SETUP_NOINSTR;
+
+	/* Instructions available but no APQNs */
+	if (ap_get_apqn(ap, qn))
+		return AP_SETUP_NOAPQN;
+
+	return 0;
+}
diff --git a/lib/s390x/ap.h b/lib/s390x/ap.h
index 79fe6eb0..59595eba 100644
--- a/lib/s390x/ap.h
+++ b/lib/s390x/ap.h
@@ -79,7 +79,10 @@ struct pqap_r2 {
 } __attribute__((packed))  __attribute__((aligned(8)));
 _Static_assert(sizeof(struct pqap_r2) == sizeof(uint64_t), "pqap_r2 size");
 
-bool ap_check(void);
+#define AP_SETUP_NOINSTR	-1
+#define AP_SETUP_NOAPQN		1
+
+int ap_setup(uint8_t *ap, uint8_t *qn);
 int ap_pqap_tapq(uint8_t ap, uint8_t qn, struct ap_queue_status *apqsw,
 		 struct pqap_r2 *r2);
 int ap_pqap_qci(struct ap_config_info *info);
diff --git a/s390x/ap.c b/s390x/ap.c
index 82ddb6d2..20b4e76e 100644
--- a/s390x/ap.c
+++ b/s390x/ap.c
@@ -16,6 +16,9 @@
 #include <asm/time.h>
 #include <ap.h>
 
+static uint8_t apn;
+static uint8_t qn;
+
 /* For PQAP PGM checks where we need full control over the input */
 static void pqap(unsigned long grs[3])
 {
@@ -291,8 +294,10 @@ static void test_priv(void)
 
 int main(void)
 {
+	int setup_rc = ap_setup(&apn, &qn);
+
 	report_prefix_push("ap");
-	if (!ap_check()) {
+	if (setup_rc == AP_SETUP_NOINSTR) {
 		report_skip("AP instructions not available");
 		goto done;
 	}
-- 
2.34.1