[PATCH V3] hwmon: scmi-hwmon: implement change_mode for thermal zones

["Peng Fan (OSS)" <peng.fan@oss.nxp.com>]

From: Peng Fan <peng.fan@nxp.com>

The thermal sensors maybe disabled before kernel boot, so add change_mode
for thermal zones to support configuring the thermal sensor to enabled
state. If reading the temperature when the sensor is disabled, there will
be error reported.

The cost is an extra config_get all to SCMI firmware to get the status
of the thermal sensor. No function level impact.

Reviewed-by: Cristian Marussi <cristian.marussi@arm.com>
Signed-off-by: Peng Fan <peng.fan@nxp.com>
---

V3:
 Update commit log to show it only applys to thermal
 Add comments in code
 Add R-b from Cristian

 Guenter, I Cced linux@roeck-us.net when sending V1/V2 
 Let me Cc Guenter Roeck <groeck7@gmail.com> in V3, hope you not mind

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

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

diff --git a/drivers/hwmon/scmi-hwmon.c b/drivers/hwmon/scmi-hwmon.c
index 364199b332c0..af2267fea5f0 100644
--- a/drivers/hwmon/scmi-hwmon.c
+++ b/drivers/hwmon/scmi-hwmon.c
@@ -151,7 +151,46 @@ 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;
+
+	/*
+	 * Per SENSOR_CONFIG_SET sensor_config description:
+	 * BIT[31:11] should be set to 0 if the sensor update interval does
+	 * not need to be updated, so clear them.
+	 * And SENSOR_CONFIG_GET does not return round up/down, so also clear
+	 * BIT[10:9] round up/down.
+	 */
+	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