From f6e0b081fb300a4601b064346963cf6bb163f437 Mon Sep 17 00:00:00 2001 From: Rhyland Klein Date: Mon, 1 Apr 2013 17:45:55 -0400 Subject: 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 Signed-off-by: Anton Vorontsov --- drivers/power/power_supply_core.c | 140 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 140 insertions(+) (limited to 'drivers/power') 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 + +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; -- cgit v1.2.3