1 files changed, 148 insertions, 25 deletions

diff --git a/drivers/firmware/arm_scmi/powercap.c b/drivers/firmware/arm_scmi/powercap.c
index 83b90bde755c..244929cb4f3e 100644
--- a/drivers/firmware/arm_scmi/powercap.c
+++ b/drivers/firmware/arm_scmi/powercap.c
@@ -108,6 +108,8 @@ struct scmi_powercap_meas_changed_notify_payld {
 };
 
 struct scmi_powercap_state {
+	bool enabled;
+	u32 last_pcap;
 	bool meas_notif_enabled;
 	u64 thresholds;
 #define THRESH_LOW(p, id)				\
@@ -313,24 +315,33 @@ static int scmi_powercap_xfer_cap_get(const struct scmi_protocol_handle *ph,
 	return ret;
 }
 
-static int scmi_powercap_cap_get(const struct scmi_protocol_handle *ph,
-				 u32 domain_id, u32 *power_cap)
+static int __scmi_powercap_cap_get(const struct scmi_protocol_handle *ph,
+				   const struct scmi_powercap_info *dom,
+				   u32 *power_cap)
 {
-	struct scmi_powercap_info *dom;
-	struct powercap_info *pi = ph->get_priv(ph);
-
-	if (!power_cap || domain_id >= pi->num_domains)
-		return -EINVAL;
-
-	dom = pi->powercaps + domain_id;
 	if (dom->fc_info && dom->fc_info[POWERCAP_FC_CAP].get_addr) {
 		*power_cap = ioread32(dom->fc_info[POWERCAP_FC_CAP].get_addr);
 		trace_scmi_fc_call(SCMI_PROTOCOL_POWERCAP, POWERCAP_CAP_GET,
-				   domain_id, *power_cap, 0);
+				   dom->id, *power_cap, 0);
 		return 0;
 	}
 
-	return scmi_powercap_xfer_cap_get(ph, domain_id, power_cap);
+	return scmi_powercap_xfer_cap_get(ph, dom->id, power_cap);
+}
+
+static int scmi_powercap_cap_get(const struct scmi_protocol_handle *ph,
+				 u32 domain_id, u32 *power_cap)
+{
+	const struct scmi_powercap_info *dom;
+
+	if (!power_cap)
+		return -EINVAL;
+
+	dom = scmi_powercap_dom_info_get(ph, domain_id);
+	if (!dom)
+		return -EINVAL;
+
+	return __scmi_powercap_cap_get(ph, dom, power_cap);
 }
 
 static int scmi_powercap_xfer_cap_set(const struct scmi_protocol_handle *ph,
@@ -375,17 +386,20 @@ static int scmi_powercap_xfer_cap_set(const struct scmi_protocol_handle *ph,
 	return ret;
 }
 
-static int scmi_powercap_cap_set(const struct scmi_protocol_handle *ph,
-				 u32 domain_id, u32 power_cap,
-				 bool ignore_dresp)
+static int __scmi_powercap_cap_set(const struct scmi_protocol_handle *ph,
+				   struct powercap_info *pi, u32 domain_id,
+				   u32 power_cap, bool ignore_dresp)
 {
+	int ret = -EINVAL;
 	const struct scmi_powercap_info *pc;
 
 	pc = scmi_powercap_dom_info_get(ph, domain_id);
-	if (!pc || !pc->powercap_cap_config || !power_cap ||
-	    power_cap < pc->min_power_cap ||
-	    power_cap > pc->max_power_cap)
-		return -EINVAL;
+	if (!pc || !pc->powercap_cap_config)
+		return ret;
+
+	if (power_cap &&
+	    (power_cap < pc->min_power_cap || power_cap > pc->max_power_cap))
+		return ret;
 
 	if (pc->fc_info && pc->fc_info[POWERCAP_FC_CAP].set_addr) {
 		struct scmi_fc_info *fci = &pc->fc_info[POWERCAP_FC_CAP];
@@ -394,10 +408,41 @@ static int scmi_powercap_cap_set(const struct scmi_protocol_handle *ph,
 		ph->hops->fastchannel_db_ring(fci->set_db);
 		trace_scmi_fc_call(SCMI_PROTOCOL_POWERCAP, POWERCAP_CAP_SET,
 				   domain_id, power_cap, 0);
+		ret = 0;
+	} else {
+		ret = scmi_powercap_xfer_cap_set(ph, pc, power_cap,
+						 ignore_dresp);
+	}
+
+	/* Save the last explicitly set non-zero powercap value */
+	if (PROTOCOL_REV_MAJOR(pi->version) >= 0x2 && !ret && power_cap)
+		pi->states[domain_id].last_pcap = power_cap;
+
+	return ret;
+}
+
+static int scmi_powercap_cap_set(const struct scmi_protocol_handle *ph,
+				 u32 domain_id, u32 power_cap,
+				 bool ignore_dresp)
+{
+	struct powercap_info *pi = ph->get_priv(ph);
+
+	/*
+	 * Disallow zero as a possible explicitly requested powercap:
+	 * there are enable/disable operations for this.
+	 */
+	if (!power_cap)
+		return -EINVAL;
+
+	/* Just log the last set request if acting on a disabled domain */
+	if (PROTOCOL_REV_MAJOR(pi->version) >= 0x2 &&
+	    !pi->states[domain_id].enabled) {
+		pi->states[domain_id].last_pcap = power_cap;
 		return 0;
 	}
 
-	return scmi_powercap_xfer_cap_set(ph, pc, power_cap, ignore_dresp);
+	return __scmi_powercap_cap_set(ph, pi, domain_id,
+				       power_cap, ignore_dresp);
 }
 
 static int scmi_powercap_xfer_pai_get(const struct scmi_protocol_handle *ph,
@@ -564,11 +609,78 @@ scmi_powercap_measurements_threshold_set(const struct scmi_protocol_handle *ph,
 	return ret;
 }
 
+static int scmi_powercap_cap_enable_set(const struct scmi_protocol_handle *ph,
+					u32 domain_id, bool enable)
+{
+	int ret;
+	u32 power_cap;
+	struct powercap_info *pi = ph->get_priv(ph);
+
+	if (PROTOCOL_REV_MAJOR(pi->version) < 0x2)
+		return -EINVAL;
+
+	if (enable == pi->states[domain_id].enabled)
+		return 0;
+
+	if (enable) {
+		/* Cannot enable with a zero powercap. */
+		if (!pi->states[domain_id].last_pcap)
+			return -EINVAL;
+
+		ret = __scmi_powercap_cap_set(ph, pi, domain_id,
+					      pi->states[domain_id].last_pcap,
+					      true);
+	} else {
+		ret = __scmi_powercap_cap_set(ph, pi, domain_id, 0, true);
+	}
+
+	if (ret)
+		return ret;
+
+	/*
+	 * Update our internal state to reflect final platform state: the SCMI
+	 * server could have ignored a disable request and kept enforcing some
+	 * powercap limit requested by other agents.
+	 */
+	ret = scmi_powercap_cap_get(ph, domain_id, &power_cap);
+	if (!ret)
+		pi->states[domain_id].enabled = !!power_cap;
+
+	return ret;
+}
+
+static int scmi_powercap_cap_enable_get(const struct scmi_protocol_handle *ph,
+					u32 domain_id, bool *enable)
+{
+	int ret;
+	u32 power_cap;
+	struct powercap_info *pi = ph->get_priv(ph);
+
+	*enable = true;
+	if (PROTOCOL_REV_MAJOR(pi->version) < 0x2)
+		return 0;
+
+	/*
+	 * Report always real platform state; platform could have ignored
+	 * a previous disable request. Default true on any error.
+	 */
+	ret = scmi_powercap_cap_get(ph, domain_id, &power_cap);
+	if (!ret)
+		*enable = !!power_cap;
+
+	/* Update internal state with current real platform state */
+	pi->states[domain_id].enabled = *enable;
+
+	return 0;
+}
+
 static const struct scmi_powercap_proto_ops powercap_proto_ops = {
 	.num_domains_get = scmi_powercap_num_domains_get,
 	.info_get = scmi_powercap_dom_info_get,
 	.cap_get = scmi_powercap_cap_get,
 	.cap_set = scmi_powercap_cap_set,
+	.cap_enable_set = scmi_powercap_cap_enable_set,
+	.cap_enable_get = scmi_powercap_cap_enable_get,
 	.pai_get = scmi_powercap_pai_get,
 	.pai_set = scmi_powercap_pai_set,
 	.measurements_get = scmi_powercap_measurements_get,
@@ -829,6 +941,11 @@ scmi_powercap_protocol_init(const struct scmi_protocol_handle *ph)
 	if (!pinfo->powercaps)
 		return -ENOMEM;
 
+	pinfo->states = devm_kcalloc(ph->dev, pinfo->num_domains,
+				     sizeof(*pinfo->states), GFP_KERNEL);
+	if (!pinfo->states)
+		return -ENOMEM;
+
 	/*
 	 * Note that any failure in retrieving any domain attribute leads to
 	 * the whole Powercap protocol initialization failure: this way the
@@ -843,15 +960,21 @@ scmi_powercap_protocol_init(const struct scmi_protocol_handle *ph)
 		if (pinfo->powercaps[domain].fastchannels)
 			scmi_powercap_domain_init_fc(ph, domain,
 						     &pinfo->powercaps[domain].fc_info);
-	}
 
-	pinfo->states = devm_kcalloc(ph->dev, pinfo->num_domains,
-				     sizeof(*pinfo->states), GFP_KERNEL);
-	if (!pinfo->states)
-		return -ENOMEM;
+		/* Grab initial state when disable is supported. */
+		if (PROTOCOL_REV_MAJOR(version) >= 0x2) {
+			ret = __scmi_powercap_cap_get(ph,
+						      &pinfo->powercaps[domain],
+						      &pinfo->states[domain].last_pcap);
+			if (ret)
+				return ret;
 
-	pinfo->version = version;
+			pinfo->states[domain].enabled =
+				!!pinfo->states[domain].last_pcap;
+		}
+	}
 
+	pinfo->version = version;
 	return ph->set_priv(ph, pinfo);
 }