[kvm-unit-tests PATCH v4 3/7] lib: s390x: ap: Add proper ap setup code

[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 stfle 12 check to proper setup code that also
returns arrays for the aps and qns.

Signed-off-by: Janosch Frank <frankja@linux.ibm.com>
---
 lib/s390x/ap.c | 90 +++++++++++++++++++++++++++++++++++++++++++++++---
 lib/s390x/ap.h | 17 +++++++++-
 s390x/ap.c     |  4 ++-
 3 files changed, 105 insertions(+), 6 deletions(-)

diff --git a/lib/s390x/ap.c b/lib/s390x/ap.c
index 9560ff64..c85172a8 100644
--- a/lib/s390x/ap.c
+++ b/lib/s390x/ap.c
@@ -13,10 +13,18 @@
 
 #include <libcflat.h>
 #include <interrupt.h>
+#include <alloc.h>
+#include <bitops.h>
 #include <ap.h>
 #include <asm/time.h>
 #include <asm/facility.h>
 
+static uint8_t num_ap;
+static uint8_t num_queue;
+static uint8_t *array_ap;
+static uint8_t *array_queue;
+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)
 {
@@ -82,16 +90,90 @@ int ap_pqap_qci(struct ap_config_info *info)
 	return cc;
 }
 
-/* Will later be extended to a proper setup function */
-bool ap_setup(void)
+static int get_entry(uint8_t *ptr, int i, size_t len)
 {
+	/* Skip over the last entry */
+	if (i)
+		i++;
+
+	for (; i < 8 * len; i++) {
+		/* Do we even need to check the byte? */
+		if (!ptr[i / 8]) {
+			i += 7;
+			continue;
+		}
+
+		/* Check the bit in big-endian order */
+		if (ptr[i / 8] & BIT(7 - (i % 8)))
+			return i;
+	}
+	return -1;
+}
+
+static void setup_info(void)
+{
+	int i, entry = 0;
+
+	ap_pqap_qci(&qci);
+
+	for (i = 0; i < AP_ARRAY_MAX_NUM; i++) {
+		entry = get_entry((uint8_t *)qci.apm, entry, sizeof(qci.apm));
+		if (entry == -1)
+			break;
+
+		if (!num_ap) {
+			/*
+			 * We have at least one AP, time to
+			 * allocate the queue arrays
+			 */
+			array_ap = malloc(AP_ARRAY_MAX_NUM);
+			array_queue = malloc(AP_ARRAY_MAX_NUM);
+		}
+		array_ap[num_ap] = entry;
+		num_ap += 1;
+	}
+
+	/* Without an AP we don't even need to look at the queues */
+	if (!num_ap)
+		return;
+
+	entry = 0;
+	for (i = 0; i < AP_ARRAY_MAX_NUM; i++) {
+		entry = get_entry((uint8_t *)qci.aqm, entry, sizeof(qci.aqm));
+		if (entry == -1)
+			return;
+
+		array_queue[num_queue] = entry;
+		num_queue += 1;
+	}
+
+}
+
+int ap_setup(uint8_t **ap_array, uint8_t **qn_array, uint8_t *naps, uint8_t *nqns)
+{
+	assert(!num_ap);
+
 	/*
 	 * Base AP support has no STFLE or SCLP feature bit but the
 	 * PQAP QCI support is indicated via stfle bit 12. As this
 	 * library relies on QCI we bail out if it's not available.
 	 */
 	if (!test_facility(12))
-		return false;
+		return AP_SETUP_NOINSTR;
 
-	return true;
+	/* Setup ap and queue arrays */
+	setup_info();
+
+	if (!num_ap)
+		return AP_SETUP_NOAPQN;
+
+	/* Only provide the data if the caller actually needs it. */
+	if (ap_array && qn_array && naps && nqns) {
+		*ap_array = array_ap;
+		*qn_array = array_queue;
+		*naps = num_ap;
+		*nqns = num_queue;
+	}
+
+	return AP_SETUP_READY;
 }
diff --git a/lib/s390x/ap.h b/lib/s390x/ap.h
index b806513f..8feca43a 100644
--- a/lib/s390x/ap.h
+++ b/lib/s390x/ap.h
@@ -81,7 +81,22 @@ struct pqap_r2 {
 } __attribute__((packed))  __attribute__((aligned(8)));
 _Static_assert(sizeof(struct pqap_r2) == sizeof(uint64_t), "pqap_r2 size");
 
-bool ap_setup(void);
+/*
+ * Maximum number of APQNs that we keep track of.
+ *
+ * This value is already way larger than the number of APQNs a AP test
+ * is probably going to use.
+ */
+#define AP_ARRAY_MAX_NUM	16
+
+/* Return values of ap_setup() */
+enum {
+	AP_SETUP_NOINSTR,	/* AP instructions not available */
+	AP_SETUP_NOAPQN,	/* Instructions available but no APQNs */
+	AP_SETUP_READY		/* Full setup complete, at least one APQN */
+};
+
+int ap_setup(uint8_t **ap_array, uint8_t **qn_array, uint8_t *naps, uint8_t *nqns);
 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 b3cee37a..5c458e7e 100644
--- a/s390x/ap.c
+++ b/s390x/ap.c
@@ -292,8 +292,10 @@ static void test_priv(void)
 
 int main(void)
 {
+	int setup_rc = ap_setup(NULL, NULL, NULL, NULL);
+
 	report_prefix_push("ap");
-	if (!ap_check()) {
+	if (setup_rc == AP_SETUP_NOINSTR) {
 		report_skip("AP instructions not available");
 		goto done;
 	}
-- 
2.40.1