[PATCH v10 13/13] powercap: arm_scmi: Synthetic zone enable/disable

[Philip Radford <philip.radford@arm.com>]

The synthetic instance root contols the same set of top-level SCMI domains
already handled by the control-type enable/disable helpers previously
introduced.

Add synthetic zone enabled attribute to the existing per-instance helpers
instead of duplicating the enable-state tracking and rollback logic.

Signed-off-by: Philip Radford <philip.radford@arm.com>
---
V9->V10
- add synthetic-root enable helper with per-zone state snapshot and roolback
- abort synthetic-root enables writes if any root child zone is not configurable
- restore each changed child zone to its previous state on failure
---
 drivers/powercap/arm_scmi_powercap.c | 104 +++++++++++++++++++++++++++
 1 file changed, 104 insertions(+)

diff --git a/drivers/powercap/arm_scmi_powercap.c b/drivers/powercap/arm_scmi_powercap.c
index d0255fd42007..0e86ad605189 100644
--- a/drivers/powercap/arm_scmi_powercap.c
+++ b/drivers/powercap/arm_scmi_powercap.c
@@ -489,10 +489,114 @@ static int instance_root_get_constraint(struct powercap_zone *pz, int cid, u64 *
 	return -EOPNOTSUPP;
 }
 
+static int scmi_powercap_set_root_children_enable_state(struct scmi_powercap_root *pr,
+							bool mode)
+{
+	struct scmi_powercap_zone_state *states;
+	struct scmi_powercap_zone *spz;
+	int num_zones = 0, nr_states = 0;
+	int i, ret = 0;
+
+	for (i = 0; i < pr->num_zones; i++) {
+		spz = &pr->spzones[i];
+
+		if (scmi_powercap_is_control_type_child(spz))
+			num_zones++;
+	}
+
+	states = kcalloc(num_zones, sizeof(*states), GFP_KERNEL);
+	if (!states)
+		return -ENOMEM;
+
+	for (i = 0; i < pr->num_zones; i++) {
+		spz = &pr->spzones[i];
+
+		if (!scmi_powercap_is_control_type_child(spz))
+			continue;
+
+		if (!spz->info->cpli[0].cap_config) {
+			ret = -EOPNOTSUPP;
+			goto out_free;
+		}
+
+		states[nr_states].spz = spz;
+
+		ret = powercap_ops->cap_enable_get(spz->ph, spz->info->id,
+			&states[nr_states].prev_enabled);
+
+		if (ret)
+			goto out_free;
+
+		nr_states++;
+	}
+
+	for (i = 0; i < nr_states; i++) {
+		spz = states[i].spz;
+
+		if (states[i].prev_enabled == mode)
+			continue;
+
+		ret = powercap_ops->cap_enable_set(spz->ph, spz->info->id, mode);
+		if (ret)
+			goto revert;
+
+		states[i].changed = true;
+	}
+
+	pr->enabled = mode;
+	goto out_free;
+
+revert:
+	while (i--) {
+		spz = states[i].spz;
+
+		if (!states[i].changed)
+			continue;
+
+		powercap_ops->cap_enable_set(spz->ph, spz->info->id,
+			states[i].prev_enabled);
+	}
+
+out_free:
+	kfree(states);
+	return ret;
+}
+
+static int instance_root_set_enable(struct powercap_zone *pz, bool mode)
+{
+	struct scmi_powercap_root *pr = to_scmi_powercap_root(pz);
+	int ret;
+
+	mutex_lock(&scmi_powercap_roots_lock);
+	ret = scmi_powercap_set_root_children_enable_state(pr, mode);
+	mutex_unlock(&scmi_powercap_roots_lock);
+
+	return ret;
+}
+
+static int instance_root_get_enable(struct powercap_zone *pz, bool *mode)
+{
+	struct scmi_powercap_root *pr = to_scmi_powercap_root(pz);
+	int ret;
+
+	if (!mode)
+		return -EINVAL;
+
+	mutex_lock(&scmi_powercap_roots_lock);
+	ret = scmi_powercap_read_root_children_enable_state(pr, mode);
+	if (!ret)
+		pr->enabled = *mode;
+	mutex_unlock(&scmi_powercap_roots_lock);
+
+	return ret;
+}
+
 static const struct powercap_zone_ops instance_root_ops = {
 	.get_max_power_range_uw = scmi_powercap_get_max_power_range_uw,
 	.get_power_uw = instance_root_get_power_uw,
 	.release = instance_root_release,
+	.set_enable = instance_root_set_enable,
+	.get_enable = instance_root_get_enable,
 };
 
 static const struct powercap_zone_constraint_ops instance_root_const_ops = {
-- 
2.34.1