1 files changed, 165 insertions, 4 deletions

diff --git a/drivers/power/supply/power_supply_core.c b/drivers/power/supply/power_supply_core.c
index 2532e221b2e1..00d8bc98d588 100644
--- a/drivers/power/supply/power_supply_core.c
+++ b/drivers/power/supply/power_supply_core.c
@@ -477,6 +477,159 @@ struct power_supply *power_supply_get_by_name(const char *name)
 }
 EXPORT_SYMBOL_GPL(power_supply_get_by_name);
 
+static bool power_supply_is_system_battery(struct power_supply *psy)
+{
+	union power_supply_propval val;
+
+	if (psy->desc->type != POWER_SUPPLY_TYPE_BATTERY)
+		return false;
+
+	if (!power_supply_get_property_direct(psy, POWER_SUPPLY_PROP_SCOPE,
+					      &val))
+		if (val.intval == POWER_SUPPLY_SCOPE_DEVICE)
+			return false;
+
+	return true;
+}
+
+static int __power_supply_get_num_system_batteries(struct power_supply *epsy,
+						   void *data)
+{
+	int *count = data;
+
+	if (power_supply_is_system_battery(epsy))
+		(*count)++;
+
+	return 0;
+}
+
+static int power_supply_get_num_system_batteries(struct device *dev)
+{
+	int ret, count = 0;
+
+	ret = power_supply_for_each_psy(&count,
+					__power_supply_get_num_system_batteries);
+
+	dev_dbg(dev, "%s: count: %d ret %d\n", __func__, count, ret);
+
+	if (ret)
+		return ret;
+
+	return count;
+}
+
+struct psy_get_supplies_data {
+	int cnt;
+	int size;
+	struct power_supply **psys;
+};
+
+static int
+__power_supply_populate_system_batteries_array(struct power_supply *epsy,
+					       void *_data)
+{
+	struct psy_get_supplies_data *data = _data;
+
+	if (power_supply_is_system_battery(epsy)) {
+		if (data->size <= data->cnt)
+			return -EOVERFLOW;
+
+		get_device(&epsy->dev);
+		data->psys[data->cnt] = epsy;
+		atomic_inc(&epsy->use_cnt);
+		data->cnt++;
+	}
+
+	return 0;
+}
+
+static int
+power_supply_populate_system_batteries_array(struct device *dev, int size,
+					     struct power_supply **batteries)
+{
+	int ret;
+
+	struct psy_get_supplies_data data = {
+		.cnt = 0,
+		.size = size,
+		.psys = batteries,
+	};
+
+	ret = power_supply_for_each_psy(&data,
+					__power_supply_populate_system_batteries_array);
+
+	dev_dbg(dev, "%s Found %d batteries with array size %d ret %d\n",
+		__func__, data.cnt, data.size, ret);
+
+	if (ret < 0 || !data.cnt) {
+		power_supply_put_system_batteries(batteries, data.cnt);
+		return ret;
+	}
+
+	return data.cnt;
+}
+
+/**
+ * power_supply_get_system_batteries() - Fetches references to battery type
+ *                                       power supplies in the system.
+ * @dev: Pointer to device requesting the power supply refs.
+ * @psys: Pointer to an array of power supply refs.
+ *
+ * Helper function to get handles to battery type power supplies in the system.
+ * If acquiring a ref to a power supply fails, then the search for battery type
+ * power supplies will abort and the acquired power supply references will be
+ * released.
+ *
+ * Return: Indicates the number of battery type power supplies returned on
+ * success or a negative error code on failure.
+ *
+ * Call power_supply_put_system_batteries() after use to cleanup resources.
+ */
+int __must_check power_supply_get_system_batteries(struct device *dev,
+						   struct power_supply ***psys)
+{
+	int ret;
+
+	if (!psys)
+		return -EINVAL;
+
+	ret = power_supply_get_num_system_batteries(dev);
+	if (ret <= 0) {
+		*psys = NULL;
+		return ret;
+	}
+
+	*psys = kzalloc_objs(**psys, ret);
+	if (!*psys)
+		return -ENOMEM;
+
+	ret = power_supply_populate_system_batteries_array(dev, ret, *psys);
+	if (ret <= 0)
+		*psys = NULL;
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(power_supply_get_system_batteries);
+
+/**
+ * power_supply_put_system_batteries() - Cleanup resources allocated by
+ *                                       power_supply_get_system_batteries()
+ * @psys: Array of power supply references to release and free.
+ * @count: Number of elements in the array.
+ */
+void power_supply_put_system_batteries(struct power_supply **psys, int count)
+{
+	int i;
+
+	for (i = 0; i < count; i++) {
+		if (psys[i])
+			power_supply_put(psys[i]);
+	}
+
+	kfree(psys);
+}
+EXPORT_SYMBOL_GPL(power_supply_put_system_batteries);
+
 /**
  * power_supply_put() - Drop reference obtained with power_supply_get_by_name
  * @psy: Reference to put
@@ -1624,6 +1777,14 @@ __power_supply_register(struct device *parent,
 	init_rwsem(&psy->extensions_sem);
 	INIT_LIST_HEAD(&psy->extensions);
 
+	if (desc->init) {
+		rc = desc->init(psy);
+		if (WARN_ON_ONCE(rc > 0))
+			rc = -EINVAL;
+		if (rc)
+			goto check_supplies_failed;
+	}
+
 	rc = device_add(dev);
 	if (rc)
 		goto device_add_failed;
@@ -1636,11 +1797,11 @@ __power_supply_register(struct device *parent,
 	if (rc)
 		goto register_thermal_failed;
 
-	rc = power_supply_create_triggers(psy);
-	if (rc)
-		goto create_triggers_failed;
-
 	scoped_guard(rwsem_read, &psy->extensions_sem) {
+		rc = power_supply_create_triggers(psy);
+		if (rc)
+			goto create_triggers_failed;
+
 		rc = power_supply_add_hwmon_sysfs(psy);
 		if (rc)
 			goto add_hwmon_sysfs_failed;