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>

diff --git a/drivers/power/power_supply_core.c b/drivers/power/power_supply_core.c
index d843cc9..1c517c3 100644 (file)
--- 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 c1cbd5e..3828cef 100644 (file)
--- 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,