[PATCH V2] hwmon: scmi-hwmon: implement change_mode

[Peng Fan (OSS)]

From: Peng Fan <peng.fan@xxxxxxx>

The sensor maybe disabled before kernel boot, so add change_mode
to support configuring the sensor to enabled state.

Signed-off-by: Peng Fan <peng.fan@xxxxxxx>
---

V2:
 Use SCMI_SENS_CFG_IS_ENABLED & clear BIT[31:9] before update config(Thanks Cristian)

 drivers/hwmon/scmi-hwmon.c | 32 ++++++++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/drivers/hwmon/scmi-hwmon.c b/drivers/hwmon/scmi-hwmon.c
index 364199b332c0..913acd1b137b 100644
--- a/drivers/hwmon/scmi-hwmon.c
+++ b/drivers/hwmon/scmi-hwmon.c
@@ -151,7 +151,39 @@ static int scmi_hwmon_thermal_get_temp(struct thermal_zone_device *tz,
 	return ret;
 }
 
+static int scmi_hwmon_thermal_change_mode(struct thermal_zone_device *tz,
+					  enum thermal_device_mode new_mode)
+{
+	int ret;
+	u32 config;
+	enum thermal_device_mode cur_mode = THERMAL_DEVICE_DISABLED;
+	struct scmi_thermal_sensor *th_sensor = thermal_zone_device_priv(tz);
+
+	ret = sensor_ops->config_get(th_sensor->ph, th_sensor->info->id,
+				     &config);
+	if (ret)
+		return ret;
+
+	if (SCMI_SENS_CFG_IS_ENABLED(config))
+		cur_mode = THERMAL_DEVICE_ENABLED;
+
+	if (cur_mode == new_mode)
+		return 0;
+
+	config &= ~(SCMI_SENS_CFG_UPDATE_SECS_MASK |
+		    SCMI_SENS_CFG_UPDATE_EXP_MASK |
+		    SCMI_SENS_CFG_ROUND_MASK);
+	if (new_mode == THERMAL_DEVICE_ENABLED)
+		config |= SCMI_SENS_CFG_SENSOR_ENABLED_MASK;
+	else
+		config &= ~SCMI_SENS_CFG_SENSOR_ENABLED_MASK;
+
+	return sensor_ops->config_set(th_sensor->ph, th_sensor->info->id,
+				      config);
+}
+
 static const struct thermal_zone_device_ops scmi_hwmon_thermal_ops = {
+	.change_mode = scmi_hwmon_thermal_change_mode,
 	.get_temp = scmi_hwmon_thermal_get_temp,
 };
 
-- 
2.37.1