intel_th: Make the switch allocate its subdevices

Instead of allocating devices for every possible output subdevice,
allow the switch to allocate only the ones that it knows about.

Signed-off-by: Alexander Shishkin <alexander.shishkin@linux.intel.com>

3 files changed, 230 insertions, 88 deletions

diff --git a/drivers/hwtracing/intel_th/core.c b/drivers/hwtracing/intel_th/core.c
index 323d3ac8d4f7..4f569593db01 100644
--- a/drivers/hwtracing/intel_th/core.c
+++ b/drivers/hwtracing/intel_th/core.c
@@ -101,17 +101,53 @@ out_pm:
         return ret;
 }
 
+static void intel_th_device_remove(struct intel_th_device *thdev);
+
 static int intel_th_remove(struct device *dev)
 {
         struct intel_th_driver *thdrv = to_intel_th_driver(dev->driver);
         struct intel_th_device *thdev = to_intel_th_device(dev);
-        struct intel_th_device *hub = to_intel_th_device(dev->parent);
+        struct intel_th_device *hub = to_intel_th_hub(thdev);
         int err;
 
         if (thdev->type == INTEL_TH_SWITCH) {
+                struct intel_th *th = to_intel_th(hub);
+                int i, lowest;
+
+                /* disconnect outputs */
                 err = device_for_each_child(dev, thdev, intel_th_child_remove);
                 if (err)
                         return err;
+
+                /*
+                 * Remove outputs, that is, hub's children: they are created
+                 * at hub's probe time by having the hub call
+                 * intel_th_output_enable() for each of them.
+                 */
+                for (i = 0, lowest = -1; i < th->num_thdevs; i++) {
+                        /*
+                         * Move the non-output devices from higher up the
+                         * th->thdev[] array to lower positions to maintain
+                         * a contiguous array.
+                         */
+                        if (th->thdev[i]->type != INTEL_TH_OUTPUT) {
+                                if (lowest >= 0) {
+                                        th->thdev[lowest] = th->thdev[i];
+                                        th->thdev[i] = NULL;
+                                        ++lowest;
+                                }
+
+                                continue;
+                        }
+
+                        if (lowest == -1)
+                                lowest = i;
+
+                        intel_th_device_remove(th->thdev[i]);
+                        th->thdev[i] = NULL;
+                }
+
+                th->num_thdevs = lowest;
         }
 
         if (thdrv->attr_group)
@@ -377,7 +413,7 @@ static const struct intel_th_subdevice {
         unsigned                otype;
         unsigned                scrpd;
         int                     id;
-} intel_th_subdevices[TH_SUBDEVICE_MAX] = {
+} intel_th_subdevices[] = {
         {
                 .nres   = 1,
                 .res    = {
@@ -511,98 +547,181 @@ static inline void intel_th_request_hub_module_flush(struct intel_th *th)
 }
 #endif /* CONFIG_MODULES */
 
-static int intel_th_populate(struct intel_th *th, struct resource *devres,
-                             unsigned int ndevres, int irq)
+static struct intel_th_device *
+intel_th_subdevice_alloc(struct intel_th *th,
+                         const struct intel_th_subdevice *subdev)
 {
+        struct intel_th_device *thdev;
         struct resource res[3];
         unsigned int req = 0;
-        int src, dst, err;
+        int r, err;
 
-        /* create devices for each intel_th_subdevice */
-        for (src = 0, dst = 0; src < ARRAY_SIZE(intel_th_subdevices); src++) {
-                const struct intel_th_subdevice *subdev =
-                        &intel_th_subdevices[src];
-                struct intel_th_device *thdev;
-                int r;
+        thdev = intel_th_device_alloc(th, subdev->type, subdev->name,
+                                      subdev->id);
+        if (!thdev)
+                return ERR_PTR(-ENOMEM);
 
-                /* only allow SOURCE and SWITCH devices in host mode */
-                if (host_mode && subdev->type == INTEL_TH_OUTPUT)
-                        continue;
 
-                thdev = intel_th_device_alloc(th, subdev->type, subdev->name,
-                                              subdev->id);
-                if (!thdev) {
-                        err = -ENOMEM;
-                        goto kill_subdevs;
+        memcpy(res, subdev->res,
+               sizeof(struct resource) * subdev->nres);
+
+        for (r = 0; r < subdev->nres; r++) {
+                struct resource *devres = th->resource;
+                int bar = TH_MMIO_CONFIG;
+
+                /*
+                 * Take .end == 0 to mean 'take the whole bar',
+                 * .start then tells us which bar it is. Default to
+                 * TH_MMIO_CONFIG.
+                 */
+                if (!res[r].end && res[r].flags == IORESOURCE_MEM) {
+                        bar = res[r].start;
+                        res[r].start = 0;
+                        res[r].end = resource_size(&devres[bar]) - 1;
                 }
 
-                memcpy(res, subdev->res,
-                       sizeof(struct resource) * subdev->nres);
+                if (res[r].flags & IORESOURCE_MEM) {
+                        res[r].start    += devres[bar].start;
+                        res[r].end      += devres[bar].start;
 
-                for (r = 0; r < subdev->nres; r++) {
-                        int bar = TH_MMIO_CONFIG;
+                        dev_dbg(th->dev, "%s:%d @ %pR\n",
+                                subdev->name, r, &res[r]);
+                } else if (res[r].flags & IORESOURCE_IRQ) {
+                        res[r].start    = th->irq;
+                }
+        }
 
-                        /*
-                         * Take .end == 0 to mean 'take the whole bar',
-                         * .start then tells us which bar it is. Default to
-                         * TH_MMIO_CONFIG.
-                         */
-                        if (!res[r].end && res[r].flags == IORESOURCE_MEM) {
-                                bar = res[r].start;
-                                res[r].start = 0;
-                                res[r].end = resource_size(&devres[bar]) - 1;
-                        }
+        err = intel_th_device_add_resources(thdev, res, subdev->nres);
+        if (err) {
+                put_device(&thdev->dev);
+                goto fail_put_device;
+        }
 
-                        if (res[r].flags & IORESOURCE_MEM) {
-                                res[r].start    += devres[bar].start;
-                                res[r].end      += devres[bar].start;
+        if (subdev->type == INTEL_TH_OUTPUT) {
+                thdev->dev.devt = MKDEV(th->major, th->num_thdevs);
+                thdev->output.type = subdev->otype;
+                thdev->output.port = -1;
+                thdev->output.scratchpad = subdev->scrpd;
+        } else if (subdev->type == INTEL_TH_SWITCH) {
+                thdev->host_mode = host_mode;
+                th->hub = thdev;
+        }
 
-                                dev_dbg(th->dev, "%s:%d @ %pR\n",
-                                        subdev->name, r, &res[r]);
-                        } else if (res[r].flags & IORESOURCE_IRQ) {
-                                res[r].start    = irq;
-                        }
-                }
+        err = device_add(&thdev->dev);
+        if (err) {
+                put_device(&thdev->dev);
+                goto fail_free_res;
+        }
 
-                err = intel_th_device_add_resources(thdev, res, subdev->nres);
-                if (err) {
-                        put_device(&thdev->dev);
-                        goto kill_subdevs;
-                }
+        /* need switch driver to be loaded to enumerate the rest */
+        if (subdev->type == INTEL_TH_SWITCH && !req) {
+                err = intel_th_request_hub_module(th);
+                if (!err)
+                        req++;
+        }
 
-                if (subdev->type == INTEL_TH_OUTPUT) {
-                        thdev->dev.devt = MKDEV(th->major, dst);
-                        thdev->output.type = subdev->otype;
-                        thdev->output.port = -1;
-                        thdev->output.scratchpad = subdev->scrpd;
-                } else if (subdev->type == INTEL_TH_SWITCH) {
-                        thdev->host_mode = host_mode;
-                }
+        return thdev;
+
+fail_free_res:
+        kfree(thdev->resource);
+
+fail_put_device:
+        put_device(&thdev->dev);
+
+        return ERR_PTR(err);
+}
 
-                err = device_add(&thdev->dev);
-                if (err) {
-                        put_device(&thdev->dev);
-                        goto kill_subdevs;
+/**
+ * intel_th_output_enable() - find and enable a device for a given output type
+ * @th:         Intel TH instance
+ * @otype:      output type
+ *
+ * Go through the unallocated output devices, find the first one whos type
+ * matches @otype and instantiate it. These devices are removed when the hub
+ * device is removed, see intel_th_remove().
+ */
+int intel_th_output_enable(struct intel_th *th, unsigned int otype)
+{
+        struct intel_th_device *thdev;
+        int src = 0, dst = 0;
+
+        for (src = 0, dst = 0; dst <= th->num_thdevs; src++, dst++) {
+                for (; src < ARRAY_SIZE(intel_th_subdevices); src++) {
+                        if (intel_th_subdevices[src].type != INTEL_TH_OUTPUT)
+                                continue;
+
+                        if (intel_th_subdevices[src].otype != otype)
+                                continue;
+
+                        break;
                 }
 
-                /* need switch driver to be loaded to enumerate the rest */
-                if (subdev->type == INTEL_TH_SWITCH && !req) {
-                        th->hub = thdev;
-                        err = intel_th_request_hub_module(th);
-                        if (!err)
-                                req++;
+                /* no unallocated matching subdevices */
+                if (src == ARRAY_SIZE(intel_th_subdevices))
+                        return -ENODEV;
+
+                for (; dst < th->num_thdevs; dst++) {
+                        if (th->thdev[dst]->type != INTEL_TH_OUTPUT)
+                                continue;
+
+                        if (th->thdev[dst]->output.type != otype)
+                                continue;
+
+                        break;
                 }
 
-                th->thdev[dst++] = thdev;
+                /*
+                 * intel_th_subdevices[src] matches our requirements and is
+                 * not matched in th::thdev[]
+                 */
+                if (dst == th->num_thdevs)
+                        goto found;
         }
 
+        return -ENODEV;
+
+found:
+        thdev = intel_th_subdevice_alloc(th, &intel_th_subdevices[src]);
+        if (IS_ERR(thdev))
+                return PTR_ERR(thdev);
+
+        th->thdev[th->num_thdevs++] = thdev;
+
         return 0;
+}
+EXPORT_SYMBOL_GPL(intel_th_output_enable);
 
-kill_subdevs:
-        for (; dst >= 0; dst--)
-                intel_th_device_remove(th->thdev[dst]);
+static int intel_th_populate(struct intel_th *th)
+{
+        int src;
 
-        return err;
+        /* create devices for each intel_th_subdevice */
+        for (src = 0; src < ARRAY_SIZE(intel_th_subdevices); src++) {
+                const struct intel_th_subdevice *subdev =
+                        &intel_th_subdevices[src];
+                struct intel_th_device *thdev;
+
+                /* only allow SOURCE and SWITCH devices in host mode */
+                if (host_mode && subdev->type == INTEL_TH_OUTPUT)
+                        continue;
+
+                /*
+                 * don't enable port OUTPUTs in this path; SWITCH enables them
+                 * via intel_th_output_enable()
+                 */
+                if (subdev->type == INTEL_TH_OUTPUT &&
+                    subdev->otype != GTH_NONE)
+                        continue;
+
+                thdev = intel_th_subdevice_alloc(th, subdev);
+                /* note: caller should free subdevices from th::thdev[] */
+                if (IS_ERR(thdev))
+                        return PTR_ERR(thdev);
+
+                th->thdev[th->num_thdevs++] = thdev;
+        }
+
+        return 0;
 }
 
 static int match_devt(struct device *dev, void *data)
@@ -679,24 +798,25 @@ intel_th_alloc(struct device *dev, struct resource *devres,
         }
         th->dev = dev;
 
+        th->resource = devres;
+        th->num_resources = ndevres;
+        th->irq = irq;
+
         dev_set_drvdata(dev, th);
 
         pm_runtime_no_callbacks(dev);
         pm_runtime_put(dev);
         pm_runtime_allow(dev);
 
-        err = intel_th_populate(th, devres, ndevres, irq);
-        if (err)
-                goto err_chrdev;
+        err = intel_th_populate(th);
+        if (err) {
+                /* free the subdevices and undo everything */
+                intel_th_free(th);
+                return ERR_PTR(err);
+        }
 
         return th;
 
-err_chrdev:
-        pm_runtime_forbid(dev);
-
-        __unregister_chrdev(th->major, 0, TH_POSSIBLE_OUTPUTS,
-                            "intel_th/output");
-
 err_ida:
         ida_simple_remove(&intel_th_ida, th->id);
 
@@ -712,11 +832,15 @@ void intel_th_free(struct intel_th *th)
         int i;
 
         intel_th_request_hub_module_flush(th);
-        for (i = 0; i < TH_SUBDEVICE_MAX; i++)
-                if (th->thdev[i] && th->thdev[i] != th->hub)
-                        intel_th_device_remove(th->thdev[i]);
 
         intel_th_device_remove(th->hub);
+        for (i = 0; i < th->num_thdevs; i++) {
+                if (th->thdev[i] != th->hub)
+                        intel_th_device_remove(th->thdev[i]);
+                th->thdev[i] = NULL;
+        }
+
+        th->num_thdevs = 0;
 
         pm_runtime_get_sync(th->dev);
         pm_runtime_forbid(th->dev);
diff --git a/drivers/hwtracing/intel_th/gth.c b/drivers/hwtracing/intel_th/gth.c
index dd32d0bad687..7d9d667fe017 100644
--- a/drivers/hwtracing/intel_th/gth.c
+++ b/drivers/hwtracing/intel_th/gth.c
@@ -639,6 +639,7 @@ intel_th_gth_set_output(struct intel_th_device *thdev, unsigned int master)
 static int intel_th_gth_probe(struct intel_th_device *thdev)
 {
         struct device *dev = &thdev->dev;
+        struct intel_th *th = dev_get_drvdata(dev->parent);
         struct gth_device *gth;
         struct resource *res;
         void __iomem *base;
@@ -660,6 +661,8 @@ static int intel_th_gth_probe(struct intel_th_device *thdev)
         gth->base = base;
         spin_lock_init(&gth->gth_lock);
 
+        dev_set_drvdata(dev, gth);
+
         /*
          * Host mode can be signalled via SW means or via SCRPD_DEBUGGER_IN_USE
          * bit. Either way, don't reset HW in this case, and don't export any
@@ -667,7 +670,7 @@ static int intel_th_gth_probe(struct intel_th_device *thdev)
          * drivers to ports, see intel_th_gth_assign().
          */
         if (thdev->host_mode)
-                goto done;
+                return 0;
 
         ret = intel_th_gth_reset(gth);
         if (ret) {
@@ -676,7 +679,7 @@ static int intel_th_gth_probe(struct intel_th_device *thdev)
 
                 thdev->host_mode = true;
 
-                goto done;
+                return 0;
         }
 
         for (i = 0; i < TH_CONFIGURABLE_MASTERS + 1; i++)
@@ -687,6 +690,13 @@ static int intel_th_gth_probe(struct intel_th_device *thdev)
                 gth->output[i].index = i;
                 gth->output[i].port_type =
                         gth_output_parm_get(gth, i, TH_OUTPUT_PARM(port));
+                if (gth->output[i].port_type == GTH_NONE)
+                        continue;
+
+                ret = intel_th_output_enable(th, gth->output[i].port_type);
+                /* -ENODEV is ok, we just won't have that device enumerated */
+                if (ret && ret != -ENODEV)
+                        return ret;
         }
 
         if (intel_th_output_attributes(gth) ||
@@ -698,9 +708,6 @@ static int intel_th_gth_probe(struct intel_th_device *thdev)
                 return -ENOMEM;
         }
 
-done:
-        dev_set_drvdata(dev, gth);
-
         return 0;
 }
 
diff --git a/drivers/hwtracing/intel_th/intel_th.h b/drivers/hwtracing/intel_th/intel_th.h
index 6243ac1b8bf1..d44da50be3b0 100644
--- a/drivers/hwtracing/intel_th/intel_th.h
+++ b/drivers/hwtracing/intel_th/intel_th.h
@@ -216,6 +216,7 @@ int intel_th_trace_enable(struct intel_th_device *thdev);
 int intel_th_trace_disable(struct intel_th_device *thdev);
 int intel_th_set_output(struct intel_th_device *thdev,
                         unsigned int master);
+int intel_th_output_enable(struct intel_th *th, unsigned int otype);
 
 enum {
         TH_MMIO_CONFIG = 0,
@@ -223,8 +224,9 @@ enum {
         TH_MMIO_END,
 };
 
-#define TH_SUBDEVICE_MAX        6
 #define TH_POSSIBLE_OUTPUTS     8
+/* Total number of possible subdevices: outputs + GTH + STH */
+#define TH_SUBDEVICE_MAX        (TH_POSSIBLE_OUTPUTS + 2)
 #define TH_CONFIGURABLE_MASTERS 256
 #define TH_MSC_MAX              2
 
@@ -233,6 +235,10 @@ enum {
  * @dev:        driver core's device
  * @thdev:      subdevices
  * @hub:        "switch" subdevice (GTH)
+ * @resource:   resources of the entire controller
+ * @num_thdevs: number of devices in the @thdev array
+ * @num_resources:      number or resources in the @resource array
+ * @irq:        irq number
  * @id:         this Intel TH controller's device ID in the system
  * @major:      device node major for output devices
  */
@@ -242,6 +248,11 @@ struct intel_th {
         struct intel_th_device  *thdev[TH_SUBDEVICE_MAX];
         struct intel_th_device  *hub;
 
+        struct resource         *resource;
+        unsigned int            num_thdevs;
+        unsigned int            num_resources;
+        int                     irq;
+
         int                     id;
         int                     major;
 #ifdef CONFIG_MODULES