s390x/css: factor out some generic code from virtio_ccw_device_realize()

A lot of what virtio_ccw_device_realize() does isn't specific to
virtio; it would apply to emulated CCW as well. Factor it out to make
it easier to implement emulated CCW devices later on.

Signed-off-by: Sascha Silbe <silbe@linux.vnet.ibm.com>
Reviewed-by: Dong Jia Shi <bjsdjshi@linux.vnet.ibm.com>
Reviewed-by: Halil Pasic <pasic@linux.vnet.ibm.com>
Signed-off-by: Cornelia Huck <cornelia.huck@de.ibm.com>

4 files changed, 175 insertions, 110 deletions

diff --git a/hw/s390x/css.c b/hw/s390x/css.c
index 76668814da..54991f5d6f 100644
--- a/hw/s390x/css.c
+++ b/hw/s390x/css.c
@@ -1340,6 +1340,116 @@ SubchDev *css_find_subch(uint8_t m, uint8_t cssid, uint8_t ssid, uint16_t schid)
     return channel_subsys.css[real_cssid]->sch_set[ssid]->sch[schid];
 }
 
+/**
+ * Return free device number in subchannel set.
+ *
+ * Return index of the first free device number in the subchannel set
+ * identified by @p cssid and @p ssid, beginning the search at @p
+ * start and wrapping around at MAX_DEVNO. Return a value exceeding
+ * MAX_SCHID if there are no free device numbers in the subchannel
+ * set.
+ */
+static uint32_t css_find_free_devno(uint8_t cssid, uint8_t ssid,
+                                    uint16_t start)
+{
+    uint32_t round;
+
+    for (round = 0; round <= MAX_DEVNO; round++) {
+        uint16_t devno = (start + round) % MAX_DEVNO;
+
+        if (!css_devno_used(cssid, ssid, devno)) {
+            return devno;
+        }
+    }
+    return MAX_DEVNO + 1;
+}
+
+/**
+ * Return first free subchannel (id) in subchannel set.
+ *
+ * Return index of the first free subchannel in the subchannel set
+ * identified by @p cssid and @p ssid, if there is any. Return a value
+ * exceeding MAX_SCHID if there are no free subchannels in the
+ * subchannel set.
+ */
+static uint32_t css_find_free_subch(uint8_t cssid, uint8_t ssid)
+{
+    uint32_t schid;
+
+    for (schid = 0; schid <= MAX_SCHID; schid++) {
+        if (!css_find_subch(1, cssid, ssid, schid)) {
+            return schid;
+        }
+    }
+    return MAX_SCHID + 1;
+}
+
+/**
+ * Return first free subchannel (id) in subchannel set for a device number
+ *
+ * Verify the device number @p devno is not used yet in the subchannel
+ * set identified by @p cssid and @p ssid. Set @p schid to the index
+ * of the first free subchannel in the subchannel set, if there is
+ * any. Return true if everything succeeded and false otherwise.
+ */
+static bool css_find_free_subch_for_devno(uint8_t cssid, uint8_t ssid,
+                                          uint16_t devno, uint16_t *schid,
+                                          Error **errp)
+{
+    uint32_t free_schid;
+
+    assert(schid);
+    if (css_devno_used(cssid, ssid, devno)) {
+        error_setg(errp, "Device %x.%x.%04x already exists",
+                   cssid, ssid, devno);
+        return false;
+    }
+    free_schid = css_find_free_subch(cssid, ssid);
+    if (free_schid > MAX_SCHID) {
+        error_setg(errp, "No free subchannel found for %x.%x.%04x",
+                   cssid, ssid, devno);
+        return false;
+    }
+    *schid = free_schid;
+    return true;
+}
+
+/**
+ * Return first free subchannel (id) and device number
+ *
+ * Locate the first free subchannel and first free device number in
+ * any of the subchannel sets of the channel subsystem identified by
+ * @p cssid. Return false if no free subchannel / device number could
+ * be found. Otherwise set @p ssid, @p devno and @p schid to identify
+ * the available subchannel and device number and return true.
+ *
+ * May modify @p ssid, @p devno and / or @p schid even if no free
+ * subchannel / device number could be found.
+ */
+static bool css_find_free_subch_and_devno(uint8_t cssid, uint8_t *ssid,
+                                          uint16_t *devno, uint16_t *schid,
+                                          Error **errp)
+{
+    uint32_t free_schid, free_devno;
+
+    assert(ssid && devno && schid);
+    for (*ssid = 0; *ssid <= MAX_SSID; (*ssid)++) {
+        free_schid = css_find_free_subch(cssid, *ssid);
+        if (free_schid > MAX_SCHID) {
+            continue;
+        }
+        free_devno = css_find_free_devno(cssid, *ssid, free_schid);
+        if (free_devno > MAX_DEVNO) {
+            continue;
+        }
+        *schid = free_schid;
+        *devno = free_devno;
+        return true;
+    }
+    error_setg(errp, "Virtual channel subsystem is full!");
+    return false;
+}
+
 bool css_subch_visible(SubchDev *sch)
 {
     if (sch->ssid > channel_subsys.max_ssid) {
@@ -1762,3 +1872,36 @@ PropertyInfo css_devid_propinfo = {
     .get = get_css_devid,
     .set = set_css_devid,
 };
+
+SubchDev *css_create_virtual_sch(CssDevId bus_id, Error **errp)
+{
+    uint16_t schid = 0;
+    SubchDev *sch;
+
+    if (bus_id.valid) {
+        /* Enforce use of virtual cssid. */
+        if (bus_id.cssid != VIRTUAL_CSSID) {
+            error_setg(errp, "cssid %hhx not valid for virtual devices",
+                       bus_id.cssid);
+            return NULL;
+        }
+        if (!css_find_free_subch_for_devno(bus_id.cssid, bus_id.ssid,
+                                           bus_id.devid, &schid, errp)) {
+            return NULL;
+        }
+    } else {
+        bus_id.cssid = VIRTUAL_CSSID;
+        if (!css_find_free_subch_and_devno(bus_id.cssid, &bus_id.ssid,
+                                           &bus_id.devid, &schid, errp)) {
+            return NULL;
+        }
+    }
+
+    sch = g_malloc0(sizeof(*sch));
+    sch->cssid = bus_id.cssid;
+    sch->ssid = bus_id.ssid;
+    sch->devno = bus_id.devid;
+    sch->schid = schid;
+    css_subch_assign(sch->cssid, sch->ssid, schid, sch->devno, sch);
+    return sch;
+}
diff --git a/hw/s390x/virtio-ccw.c b/hw/s390x/virtio-ccw.c
index 8b709e362e..67d7867c56 100644
--- a/hw/s390x/virtio-ccw.c
+++ b/hw/s390x/virtio-ccw.c
@@ -703,116 +703,27 @@ static void virtio_sch_disable_cb(SubchDev *sch)
 
 static void virtio_ccw_device_realize(VirtioCcwDevice *dev, Error **errp)
 {
-    unsigned int schid;
-    bool found = false;
-    SubchDev *sch;
-    Error *err = NULL;
     VirtIOCCWDeviceClass *k = VIRTIO_CCW_DEVICE_GET_CLASS(dev);
+    SubchDev *sch = css_create_virtual_sch(dev->bus_id, errp);
+    Error *err = NULL;
 
-    sch = g_malloc0(sizeof(SubchDev));
-
-    sch->driver_data = dev;
-    dev->sch = sch;
-
-    dev->indicators = NULL;
-
-    /* Initialize subchannel structure. */
-    sch->channel_prog = 0x0;
-    sch->last_cmd_valid = false;
-    sch->thinint_active = false;
-    /*
-     * Use a device number if provided. Otherwise, fall back to subchannel
-     * number.
-     */
-    if (dev->bus_id.valid) {
-        /* Enforce use of virtual cssid. */
-        if (dev->bus_id.cssid != VIRTUAL_CSSID) {
-            error_setg(errp, "cssid %x not valid for virtio devices",
-                       dev->bus_id.cssid);
-            goto out_err;
-        }
-        if (css_devno_used(dev->bus_id.cssid, dev->bus_id.ssid,
-                           dev->bus_id.devid)) {
-                error_setg(errp, "Device %x.%x.%04x already exists",
-                           dev->bus_id.cssid, dev->bus_id.ssid,
-                           dev->bus_id.devid);
-                goto out_err;
-        }
-        sch->cssid = dev->bus_id.cssid;
-        sch->ssid = dev->bus_id.ssid;
-        sch->devno = dev->bus_id.devid;
-
-        /* Find the next free id. */
-        for (schid = 0; schid <= MAX_SCHID; schid++) {
-            if (!css_find_subch(1, sch->cssid, sch->ssid, schid)) {
-                sch->schid = schid;
-                css_subch_assign(sch->cssid, sch->ssid, sch->schid,
-                                 sch->devno, sch);
-                found = true;
-                break;
-            }
-        }
-        if (!found) {
-            error_setg(errp, "No free subchannel found for %x.%x.%04x",
-                       sch->cssid, sch->ssid, sch->devno);
-            goto out_err;
-        }
-        trace_virtio_ccw_new_device(sch->cssid, sch->ssid, sch->schid,
-                                    sch->devno, "user-configured");
-    } else {
-        unsigned int cssid = VIRTUAL_CSSID, ssid, devno;
-
-        for (ssid = 0; ssid <= MAX_SSID; ssid++) {
-            for (schid = 0; schid <= MAX_SCHID; schid++) {
-                if (!css_find_subch(1, cssid, ssid, schid)) {
-                    sch->cssid = cssid;
-                    sch->ssid = ssid;
-                    sch->schid = schid;
-                    devno = schid;
-                    /*
-                     * If the devno is already taken, look further in this
-                     * subchannel set.
-                     */
-                    while (css_devno_used(cssid, ssid, devno)) {
-                        if (devno == MAX_SCHID) {
-                            devno = 0;
-                        } else if (devno == schid - 1) {
-                            error_setg(errp, "No free devno found");
-                            goto out_err;
-                        } else {
-                            devno++;
-                        }
-                    }
-                    sch->devno = devno;
-                    css_subch_assign(cssid, ssid, schid, devno, sch);
-                    found = true;
-                    break;
-                }
-            }
-            if (found) {
-                break;
-            }
-        }
-        if (!found) {
-            error_setg(errp, "Virtual channel subsystem is full!");
-            goto out_err;
-        }
-        trace_virtio_ccw_new_device(cssid, ssid, schid, devno,
-                                    "auto-configured");
+    if (!sch) {
+        return;
     }
 
-    /* Build initial schib. */
-    css_sch_build_virtual_schib(sch, 0, VIRTIO_CCW_CHPID_TYPE);
-
+    sch->driver_data = dev;
     sch->ccw_cb = virtio_ccw_cb;
     sch->disable_cb = virtio_sch_disable_cb;
-
-    /* Build senseid data. */
-    memset(&sch->id, 0, sizeof(SenseId));
     sch->id.reserved = 0xff;
     sch->id.cu_type = VIRTIO_CCW_CU_TYPE;
-
+    dev->sch = sch;
+    dev->indicators = NULL;
     dev->revision = -1;
+    css_sch_build_virtual_schib(sch, 0, VIRTIO_CCW_CHPID_TYPE);
+
+    trace_virtio_ccw_new_device(
+        sch->cssid, sch->ssid, sch->schid, sch->devno,
+        dev->bus_id.valid ? "user-configured" : "auto-configured");
 
     if (k->realize) {
         k->realize(dev, &err);
@@ -820,14 +731,9 @@ static void virtio_ccw_device_realize(VirtioCcwDevice *dev, Error **errp)
     if (err) {
         error_propagate(errp, err);
         css_subch_assign(sch->cssid, sch->ssid, sch->schid, sch->devno, NULL);
-        goto out_err;
+        dev->sch = NULL;
+        g_free(sch);
     }
-
-    return;
-
-out_err:
-    dev->sch = NULL;
-    g_free(sch);
 }
 
 static int virtio_ccw_exit(VirtioCcwDevice *dev)
diff --git a/hw/s390x/virtio-ccw.h b/hw/s390x/virtio-ccw.h
index 0bfb5d93c6..7243fb08be 100644
--- a/hw/s390x/virtio-ccw.h
+++ b/hw/s390x/virtio-ccw.h
@@ -27,8 +27,6 @@
 #include <hw/s390x/s390_flic.h>
 #include <hw/s390x/css.h>
 
-#define VIRTUAL_CSSID 0xfe
-
 #define VIRTIO_CCW_CU_TYPE 0x3832
 #define VIRTIO_CCW_CHPID_TYPE 0x32
 
diff --git a/include/hw/s390x/css.h b/include/hw/s390x/css.h
index 38f4d77779..1da63e361d 100644
--- a/include/hw/s390x/css.h
+++ b/include/hw/s390x/css.h
@@ -17,6 +17,7 @@
 #include "hw/s390x/ioinst.h"
 
 /* Channel subsystem constants. */
+#define MAX_DEVNO 65535
 #define MAX_SCHID 65535
 #define MAX_SSID 3
 #define MAX_CSSID 254 /* 255 is reserved */
@@ -24,6 +25,8 @@
 
 #define MAX_CIWS 62
 
+#define VIRTUAL_CSSID 0xfe
+
 typedef struct CIW {
     uint8_t type;
     uint8_t command;
@@ -169,4 +172,19 @@ extern PropertyInfo css_devid_propinfo;
 #define DEFINE_PROP_CSS_DEV_ID(_n, _s, _f) \
     DEFINE_PROP(_n, _s, _f, css_devid_propinfo, CssDevId)
 
+/**
+ * Create a subchannel for the given bus id.
+ *
+ * If @p bus_id is valid, verify that it uses the virtual channel
+ * subsystem id and is not already in use, and find a free subchannel
+ * id for it. If @p bus_id is not valid, find a free subchannel id and
+ * device number across all subchannel sets. If either of the former
+ * actions succeed, allocate a subchannel structure, initialise it
+ * with the bus id, subchannel id and device number, register it with
+ * the CSS and return it. Otherwise return NULL.
+ *
+ * The caller becomes owner of the returned subchannel structure and
+ * is responsible for unregistering and freeing it.
+ */
+SubchDev *css_create_virtual_sch(CssDevId bus_id, Error **errp);
 #endif