power_supply: Populate supplied_from hierarchy from the device tree

With this patch the power_supply_core will try to populate supplied_from
hierarchy from the device tree.

Signed-off-by: Rhyland Klein <rklein@nvidia.com>
Signed-off-by: Anton Vorontsov <anton@enomsg.org>

2 files changed, 143 insertions, 0 deletions

diff --git a/drivers/power/power_supply_core.c b/drivers/power/power_supply_core.c
index d843cc9df030..1c517c34e4be 100644
--- a/drivers/power/power_supply_core.c
+++ b/drivers/power/power_supply_core.c
@@ -88,6 +88,139 @@ void power_supply_changed(struct power_supply *psy)
 }
 EXPORT_SYMBOL_GPL(power_supply_changed);
 
+#ifdef CONFIG_OF
+#include <linux/of.h>
+
+static int __power_supply_populate_supplied_from(struct device *dev,
+                                                 void *data)
+{
+        struct power_supply *psy = (struct power_supply *)data;
+        struct power_supply *epsy = dev_get_drvdata(dev);
+        struct device_node *np;
+        int i = 0;
+
+        do {
+                np = of_parse_phandle(psy->of_node, "power-supplies", i++);
+                if (!np)
+                        continue;
+
+                if (np == epsy->of_node) {
+                        dev_info(psy->dev, "%s: Found supply : %s\n",
+                                psy->name, epsy->name);
+                        psy->supplied_from[i-1] = (char *)epsy->name;
+                        psy->num_supplies++;
+                        break;
+                }
+        } while (np);
+
+        return 0;
+}
+
+static int power_supply_populate_supplied_from(struct power_supply *psy)
+{
+        int error;
+
+        error = class_for_each_device(power_supply_class, NULL, psy,
+                                      __power_supply_populate_supplied_from);
+
+        dev_dbg(psy->dev, "%s %d\n", __func__, error);
+
+        return error;
+}
+
+static int  __power_supply_find_supply_from_node(struct device *dev,
+                                                 void *data)
+{
+        struct device_node *np = (struct device_node *)data;
+        struct power_supply *epsy = dev_get_drvdata(dev);
+
+        /* return error breaks out of class_for_each_device loop */
+        if (epsy->of_node == np)
+                return -EINVAL;
+
+        return 0;
+}
+
+static int power_supply_find_supply_from_node(struct device_node *supply_node)
+{
+        int error;
+        struct device *dev;
+        struct class_dev_iter iter;
+
+        /*
+         * Use iterator to see if any other device is registered.
+         * This is required since class_for_each_device returns 0
+         * if there are no devices registered.
+         */
+        class_dev_iter_init(&iter, power_supply_class, NULL, NULL);
+        dev = class_dev_iter_next(&iter);
+
+        if (!dev)
+                return -EPROBE_DEFER;
+
+        /*
+         * We have to treat the return value as inverted, because if
+         * we return error on not found, then it won't continue looking.
+         * So we trick it by returning error on success to stop looking
+         * once the matching device is found.
+         */
+        error = class_for_each_device(power_supply_class, NULL, supply_node,
+                                       __power_supply_find_supply_from_node);
+
+        return error ? 0 : -EPROBE_DEFER;
+}
+
+static int power_supply_check_supplies(struct power_supply *psy)
+{
+        struct device_node *np;
+        int cnt = 0;
+
+        /* If there is already a list honor it */
+        if (psy->supplied_from && psy->num_supplies > 0)
+                return 0;
+
+        /* No device node found, nothing to do */
+        if (!psy->of_node)
+                return 0;
+
+        do {
+                int ret;
+
+                np = of_parse_phandle(psy->of_node, "power-supplies", cnt++);
+                if (!np)
+                        continue;
+
+                ret = power_supply_find_supply_from_node(np);
+                if (ret) {
+                        dev_dbg(psy->dev, "Failed to find supply, defer!\n");
+                        return -EPROBE_DEFER;
+                }
+        } while (np);
+
+        /* All supplies found, allocate char ** array for filling */
+        psy->supplied_from = devm_kzalloc(psy->dev, sizeof(psy->supplied_from),
+                                          GFP_KERNEL);
+        if (!psy->supplied_from) {
+                dev_err(psy->dev, "Couldn't allocate memory for supply list\n");
+                return -ENOMEM;
+        }
+
+        *psy->supplied_from = devm_kzalloc(psy->dev, sizeof(char *) * cnt,
+                                           GFP_KERNEL);
+        if (!*psy->supplied_from) {
+                dev_err(psy->dev, "Couldn't allocate memory for supply list\n");
+                return -ENOMEM;
+        }
+
+        return power_supply_populate_supplied_from(psy);
+}
+#else
+static inline int power_supply_check_supplies(struct power_supply *psy)
+{
+        return 0;
+}
+#endif
+
 static int __power_supply_am_i_supplied(struct device *dev, void *data)
 {
         union power_supply_propval ret = {0,};
@@ -357,6 +490,12 @@ int power_supply_register(struct device *parent, struct power_supply *psy)
 
         INIT_WORK(&psy->changed_work, power_supply_changed_work);
 
+        rc = power_supply_check_supplies(psy);
+        if (rc) {
+                dev_info(dev, "Not all required supplies found, defer probe\n");
+                goto check_supplies_failed;
+        }
+
         rc = kobject_set_name(&dev->kobj, "%s", psy->name);
         if (rc)
                 goto kobject_set_name_failed;
@@ -389,6 +528,7 @@ register_thermal_failed:
         device_del(dev);
 kobject_set_name_failed:
 device_add_failed:
+check_supplies_failed:
         put_device(dev);
 success:
         return rc;
diff --git a/include/linux/power_supply.h b/include/linux/power_supply.h
index c1cbd5e4e484..3828cefb4f65 100644
--- a/include/linux/power_supply.h
+++ b/include/linux/power_supply.h
@@ -173,6 +173,9 @@ struct power_supply {
 
         char **supplied_from;
         size_t num_supplies;
+#ifdef CONFIG_OF
+        struct device_node *of_node;
+#endif
 
         int (*get_property)(struct power_supply *psy,
                             enum power_supply_property psp,