1 files changed, 176 insertions, 21 deletions

diff --git a/drivers/hwmon/scmi-hwmon.c b/drivers/hwmon/scmi-hwmon.c
index 2e005edee0c9..eec223d174c0 100644
--- a/drivers/hwmon/scmi-hwmon.c
+++ b/drivers/hwmon/scmi-hwmon.c
@@ -2,7 +2,7 @@
 /*
  * System Control and Management Interface(SCMI) based hwmon sensor driver
  *
- * Copyright (C) 2018 ARM Ltd.
+ * Copyright (C) 2018-2021 ARM Ltd.
  * Sudeep Holla <sudeep.holla@arm.com>
  */
 
@@ -13,28 +13,91 @@
 #include <linux/sysfs.h>
 #include <linux/thermal.h>
 
+static const struct scmi_sensor_proto_ops *sensor_ops;
+
 struct scmi_sensors {
-	const struct scmi_handle *handle;
+	const struct scmi_protocol_handle *ph;
 	const struct scmi_sensor_info **info[hwmon_max];
 };
 
-static int scmi_hwmon_read(struct device *dev, enum hwmon_sensor_types type,
-			   u32 attr, int channel, long *val)
+struct scmi_thermal_sensor {
+	const struct scmi_protocol_handle *ph;
+	const struct scmi_sensor_info *info;
+};
+
+static inline u64 __pow10(u8 x)
+{
+	u64 r = 1;
+
+	while (x--)
+		r *= 10;
+
+	return r;
+}
+
+static int scmi_hwmon_scale(const struct scmi_sensor_info *sensor, u64 *value)
+{
+	int scale = sensor->scale;
+	u64 f;
+
+	switch (sensor->type) {
+	case TEMPERATURE_C:
+	case VOLTAGE:
+	case CURRENT:
+		scale += 3;
+		break;
+	case POWER:
+	case ENERGY:
+		scale += 6;
+		break;
+	default:
+		break;
+	}
+
+	if (scale == 0)
+		return 0;
+
+	if (abs(scale) > 19)
+		return -E2BIG;
+
+	f = __pow10(abs(scale));
+	if (scale > 0)
+		*value *= f;
+	else
+		*value = div64_u64(*value, f);
+
+	return 0;
+}
+
+static int scmi_hwmon_read_scaled_value(const struct scmi_protocol_handle *ph,
+					const struct scmi_sensor_info *sensor,
+					long *val)
 {
 	int ret;
 	u64 value;
-	const struct scmi_sensor_info *sensor;
-	struct scmi_sensors *scmi_sensors = dev_get_drvdata(dev);
-	const struct scmi_handle *h = scmi_sensors->handle;
 
-	sensor = *(scmi_sensors->info[type] + channel);
-	ret = h->sensor_ops->reading_get(h, sensor->id, false, &value);
+	ret = sensor_ops->reading_get(ph, sensor->id, &value);
+	if (ret)
+		return ret;
+
+	ret = scmi_hwmon_scale(sensor, &value);
 	if (!ret)
 		*val = value;
 
 	return ret;
 }
 
+static int scmi_hwmon_read(struct device *dev, enum hwmon_sensor_types type,
+			   u32 attr, int channel, long *val)
+{
+	const struct scmi_sensor_info *sensor;
+	struct scmi_sensors *scmi_sensors = dev_get_drvdata(dev);
+
+	sensor = *(scmi_sensors->info[type] + channel);
+
+	return scmi_hwmon_read_scaled_value(scmi_sensors->ph, sensor, val);
+}
+
 static int
 scmi_hwmon_read_string(struct device *dev, enum hwmon_sensor_types type,
 		       u32 attr, int channel, const char **str)
@@ -57,7 +120,7 @@ scmi_hwmon_is_visible(const void *drvdata, enum hwmon_sensor_types type,
 
 	sensor = *(scmi_sensors->info[type] + channel);
 	if (sensor)
-		return S_IRUGO;
+		return 0444;
 
 	return 0;
 }
@@ -73,6 +136,25 @@ static struct hwmon_chip_info scmi_chip_info = {
 	.info = NULL,
 };
 
+static int scmi_hwmon_thermal_get_temp(struct thermal_zone_device *tz,
+				       int *temp)
+{
+	int ret;
+	long value;
+	struct scmi_thermal_sensor *th_sensor = thermal_zone_device_priv(tz);
+
+	ret = scmi_hwmon_read_scaled_value(th_sensor->ph, th_sensor->info,
+					   &value);
+	if (!ret)
+		*temp = value;
+
+	return ret;
+}
+
+static const struct thermal_zone_device_ops scmi_hwmon_thermal_ops = {
+	.get_temp = scmi_hwmon_thermal_get_temp,
+};
+
 static int scmi_hwmon_add_chan_info(struct hwmon_channel_info *scmi_hwmon_chan,
 				    struct device *dev, int num,
 				    enum hwmon_sensor_types type, u32 config)
@@ -99,8 +181,7 @@ static enum hwmon_sensor_types scmi_types[] = {
 	[ENERGY] = hwmon_energy,
 };
 
-static u32 hwmon_attributes[] = {
-	[hwmon_chip] = HWMON_C_REGISTER_TZ,
+static u32 hwmon_attributes[hwmon_max] = {
 	[hwmon_temp] = HWMON_T_INPUT | HWMON_T_LABEL,
 	[hwmon_in] = HWMON_I_INPUT | HWMON_I_LABEL,
 	[hwmon_curr] = HWMON_C_INPUT | HWMON_C_LABEL,
@@ -108,6 +189,43 @@ static u32 hwmon_attributes[] = {
 	[hwmon_energy] = HWMON_E_INPUT | HWMON_E_LABEL,
 };
 
+static int scmi_thermal_sensor_register(struct device *dev,
+					const struct scmi_protocol_handle *ph,
+					const struct scmi_sensor_info *sensor)
+{
+	struct scmi_thermal_sensor *th_sensor;
+	struct thermal_zone_device *tzd;
+
+	th_sensor = devm_kzalloc(dev, sizeof(*th_sensor), GFP_KERNEL);
+	if (!th_sensor)
+		return -ENOMEM;
+
+	th_sensor->ph = ph;
+	th_sensor->info = sensor;
+
+	/*
+	 * Try to register a temperature sensor with the Thermal Framework:
+	 * skip sensors not defined as part of any thermal zone (-ENODEV) but
+	 * report any other errors related to misconfigured zones/sensors.
+	 */
+	tzd = devm_thermal_of_zone_register(dev, th_sensor->info->id, th_sensor,
+					    &scmi_hwmon_thermal_ops);
+	if (IS_ERR(tzd)) {
+		devm_kfree(dev, th_sensor);
+
+		if (PTR_ERR(tzd) != -ENODEV)
+			return PTR_ERR(tzd);
+
+		dev_dbg(dev, "Sensor '%s' not attached to any thermal zone.\n",
+			sensor->name);
+	} else {
+		dev_dbg(dev, "Sensor '%s' attached to thermal zone ID:%d\n",
+			sensor->name, thermal_zone_device_id(tzd));
+	}
+
+	return 0;
+}
+
 static int scmi_hwmon_probe(struct scmi_device *sdev)
 {
 	int i, idx;
@@ -115,17 +233,24 @@ static int scmi_hwmon_probe(struct scmi_device *sdev)
 	enum hwmon_sensor_types type;
 	struct scmi_sensors *scmi_sensors;
 	const struct scmi_sensor_info *sensor;
-	int nr_count[hwmon_max] = {0}, nr_types = 0;
+	int nr_count[hwmon_max] = {0}, nr_types = 0, nr_count_temp = 0;
 	const struct hwmon_chip_info *chip_info;
 	struct device *hwdev, *dev = &sdev->dev;
 	struct hwmon_channel_info *scmi_hwmon_chan;
 	const struct hwmon_channel_info **ptr_scmi_ci;
 	const struct scmi_handle *handle = sdev->handle;
+	struct scmi_protocol_handle *ph;
+	u32 sensor_config = FIELD_PREP(SCMI_SENS_CFG_SENSOR_ENABLED_MASK,
+				       SCMI_SENS_CFG_SENSOR_ENABLE);
 
-	if (!handle || !handle->sensor_ops)
+	if (!handle)
 		return -ENODEV;
 
-	nr_sensors = handle->sensor_ops->count_get(handle);
+	sensor_ops = handle->devm_protocol_get(sdev, SCMI_PROTOCOL_SENSOR, &ph);
+	if (IS_ERR(sensor_ops))
+		return PTR_ERR(sensor_ops);
+
+	nr_sensors = sensor_ops->count_get(ph);
 	if (!nr_sensors)
 		return -EIO;
 
@@ -133,10 +258,10 @@ static int scmi_hwmon_probe(struct scmi_device *sdev)
 	if (!scmi_sensors)
 		return -ENOMEM;
 
-	scmi_sensors->handle = handle;
+	scmi_sensors->ph = ph;
 
 	for (i = 0; i < nr_sensors; i++) {
-		sensor = handle->sensor_ops->info_get(handle, i);
+		sensor = sensor_ops->info_get(ph, i);
 		if (!sensor)
 			return -EINVAL;
 
@@ -155,7 +280,7 @@ static int scmi_hwmon_probe(struct scmi_device *sdev)
 	}
 
 	if (nr_count[hwmon_temp])
-		nr_count[hwmon_chip]++, nr_types++;
+		nr_count_temp = nr_count[hwmon_temp];
 
 	scmi_hwmon_chan = devm_kcalloc(dev, nr_types, sizeof(*scmi_hwmon_chan),
 				       GFP_KERNEL);
@@ -186,7 +311,7 @@ static int scmi_hwmon_probe(struct scmi_device *sdev)
 	}
 
 	for (i = nr_sensors - 1; i >= 0 ; i--) {
-		sensor = handle->sensor_ops->info_get(handle, i);
+		sensor = sensor_ops->info_get(ph, i);
 		if (!sensor)
 			continue;
 
@@ -206,12 +331,42 @@ static int scmi_hwmon_probe(struct scmi_device *sdev)
 	hwdev = devm_hwmon_device_register_with_info(dev, "scmi_sensors",
 						     scmi_sensors, chip_info,
 						     NULL);
+	if (IS_ERR(hwdev))
+		return PTR_ERR(hwdev);
+
+	for (i = 0; i < nr_count_temp; i++) {
+		int ret;
+
+		sensor = *(scmi_sensors->info[hwmon_temp] + i);
+		if (!sensor)
+			continue;
+
+		ret = sensor_ops->config_set(ph, i, sensor_config);
+		if (ret) {
+			dev_err(dev, "Error enabling sensor %s. err=%d\n",
+				sensor->name, ret);
+			continue;
+		}
 
-	return PTR_ERR_OR_ZERO(hwdev);
+		/*
+		 * Warn on any misconfiguration related to thermal zones but
+		 * bail out of probing only on memory errors.
+		 */
+		ret = scmi_thermal_sensor_register(dev, ph, sensor);
+		if (ret) {
+			if (ret == -ENOMEM)
+				return ret;
+			dev_warn(dev,
+				 "Thermal zone misconfigured for %s. err=%d\n",
+				 sensor->name, ret);
+		}
+	}
+
+	return 0;
 }
 
 static const struct scmi_device_id scmi_id_table[] = {
-	{ SCMI_PROTOCOL_SENSOR },
+	{ SCMI_PROTOCOL_SENSOR, "hwmon" },
 	{ },
 };
 MODULE_DEVICE_TABLE(scmi, scmi_id_table);