[PATCH v3 3/6] thermal: Add support of multi sensors to thermal_of

[Alexandre Bailon <abailon@baylibre.com>]

This updates thermal_of to support more than one sensor.
If during the registration we find another thermal zone referencing
this sensors and some other, then we create the multi sensor thermal
zone (if it doesn't exist) and register the sensor to it.

Signed-off-by: Alexandre Bailon <abailon@baylibre.com>
---
 drivers/thermal/thermal_of.c | 250 +++++++++++++++++++++++++++++++++--
 1 file changed, 241 insertions(+), 9 deletions(-)

diff --git a/drivers/thermal/thermal_of.c b/drivers/thermal/thermal_of.c
index aa34b6e82e26..75e3cfb8488a 100644
--- a/drivers/thermal/thermal_of.c
+++ b/drivers/thermal/thermal_of.c
@@ -18,6 +18,8 @@
 
 #include "thermal_core.h"
 
+#define STRLEN_ID (8)
+
 /***   functions parsing device tree nodes   ***/
 
 static int of_find_trip_id(struct device_node *np, struct device_node *trip)
@@ -222,6 +224,77 @@ static struct device_node *of_thermal_zone_find(struct device_node *sensor, int
 	return tz;
 }
 
+static int thermal_of_multi_sensor_get_name(struct device_node *sensor, int id,
+					    struct device_node *tz, char *name)
+{
+	struct of_phandle_args sensor_specs;
+	int count, i;
+
+	tz = of_thermal_zone_find(sensor, id);
+	if (!tz) {
+		pr_debug("No thermal zones description\n");
+		return -ENODEV;
+	}
+
+	count = of_count_phandle_with_args(tz, "thermal-sensors",
+						"#thermal-sensor-cells");
+	if (count <= 0)
+		return count;
+
+	for (i = 0; i < count; i++) {
+
+		int ret;
+
+		ret = of_parse_phandle_with_args(tz, "thermal-sensors",
+							"#thermal-sensor-cells",
+							i, &sensor_specs);
+		if (ret < 0) {
+			pr_err("%pOFn: Failed to read thermal-sensors cells: %d\n", tz, ret);
+			return ret;
+		}
+
+		if ((sensor == sensor_specs.np) && id == (sensor_specs.args_count ?
+								sensor_specs.args[0] : 0)) {
+			snprintf(name, THERMAL_NAME_LENGTH, "%s%d", tz->name, id);
+			return 0;
+		}
+	}
+
+	return -ENODEV;
+}
+
+static int thermal_of_multi_sensor_get_id(struct device_node *sensor,
+					 struct device_node *tz, int id)
+{
+	struct of_phandle_args sensor_specs;
+	int count, i;
+
+	count = of_count_phandle_with_args(tz, "thermal-sensors",
+						"#thermal-sensor-cells");
+	if (count <= 0)
+		return 0;
+
+	for (i = 0; i < count; i++) {
+
+		int ret;
+
+		ret = of_parse_phandle_with_args(tz, "thermal-sensors",
+							"#thermal-sensor-cells",
+							i, &sensor_specs);
+		if (ret < 0) {
+			pr_err("%pOFn: Failed to read thermal-sensors cells: %d\n", tz, ret);
+			return 0;
+		}
+
+		if ((sensor == sensor_specs.np) && id == (sensor_specs.args_count ?
+								sensor_specs.args[0] : 0)) {
+			return i;
+		}
+	}
+
+	return -ENODEV;
+}
+
 static int thermal_of_monitor_init(struct device_node *np, int *delay, int *pdelay)
 {
 	int ret;
@@ -281,6 +354,17 @@ static struct device_node *thermal_of_zone_get_by_name(struct thermal_zone_devic
 		return ERR_PTR(-ENODEV);
 
 	tz_np = of_get_child_by_name(np, tz->type);
+	if (!tz_np) {
+		char tmp[THERMAL_NAME_LENGTH];
+		char *ptr;
+
+		ptr = strrchr(tz->type, '.');
+		if (!ptr)
+			return ERR_PTR(-ENODEV);
+
+		strscpy(tmp, tz->type, (ptr - tz->type) + 1);
+		tz_np = of_get_child_by_name(np, tmp);
+	}
 
 	of_node_put(np);
 
@@ -444,10 +528,140 @@ static int thermal_of_unbind(struct thermal_zone_device *tz,
  */
 static void thermal_of_zone_unregister(struct thermal_zone_device *tz)
 {
+	thermal_multi_sensor_unregister(tz);
 	thermal_zone_device_disable(tz);
 	thermal_zone_device_unregister(tz);
 }
 
+static int thermal_of_multi_sensor_validate_coeff(struct device_node *sensor, int id,
+						  struct device_node *tz_np)
+{
+	u32 *coeff;
+	int ret;
+	int i;
+
+	int count;
+	int index;
+	int offset;
+
+	index = thermal_of_multi_sensor_get_id(sensor, tz_np, id);
+	if (index < 0)
+		return -ENODEV;
+
+
+	count = of_count_phandle_with_args(tz_np,
+					   "thermal-sensors",
+					   "#thermal-sensor-cells");
+	if (count < 0)
+		return count;
+
+	coeff = kmalloc_array(count, sizeof(*coeff), GFP_KERNEL);
+	if (!coeff)
+		return -ENOMEM;
+
+	for (i = 0; i < count; i++) {
+		ret = of_property_read_u32_index(tz_np,
+						 "coefficients",
+						 i, coeff + i);
+		if (ret)
+			coeff[i] = 1;
+	}
+
+	ret = of_property_read_u32_index(tz_np, "coefficients",
+					 count, &offset);
+	if (ret)
+		offset = 0;
+
+	/* Make sure the coeff and offset won't cause an overflow */
+	ret = thermal_multi_sensor_validate_coeff(coeff, count, offset);
+
+	kfree(coeff);
+
+	return ret;
+}
+
+static int thermal_of_mutli_sensor_coeff(struct device_node *sensor, int id,
+					 struct device_node *tz_np,
+					 u32 *coeff)
+{
+	int index;
+	int ret;
+
+	index = thermal_of_multi_sensor_get_id(sensor, tz_np, id);
+	if (index < 0)
+		return index;
+
+	ret = of_property_read_u32_index(tz_np, "coefficients", index, coeff);
+	if (ret)
+		*coeff = 1;
+
+	return 0;
+}
+
+static struct thermal_zone_device *
+thermal_of_register_multi_tz(struct device_node *sensor, int id, struct device_node *np,
+			     const char *type, struct thermal_trip *trips, int num_trips,
+			     void *devdata, struct thermal_zone_device_ops *ops,
+			     const struct thermal_zone_params *tzp, int passive_delay,
+			     int polling_delay)
+{
+	struct thermal_zone_device *multi_tz, *tz;
+	char name[THERMAL_NAME_LENGTH];
+	u32 coeff;
+	int ret;
+
+	multi_tz = thermal_multi_sensor_find_tz(type);
+	if (!multi_tz) {
+		struct thermal_zone_device_ops *multi_ops;
+
+		ret = thermal_of_multi_sensor_validate_coeff(sensor, id, np);
+		if (ret)
+			return ERR_PTR(ret);
+
+		multi_ops = thermal_multi_sensor_alloc_ops(THERMAL_AGGR_AVG);
+		if (IS_ERR_OR_NULL(multi_ops))
+			return ERR_PTR(PTR_ERR(multi_ops));
+		multi_ops->bind = thermal_of_bind;
+		multi_ops->unbind = thermal_of_unbind;
+
+		multi_tz = thermal_multi_sensor_tz_alloc(type, trips, num_trips,
+							 multi_ops,
+							 passive_delay, polling_delay);
+		if (IS_ERR_OR_NULL(multi_tz)) {
+			kfree(multi_ops);
+			return multi_tz;
+		}
+	}
+
+	ret = thermal_of_multi_sensor_get_name(sensor, id, np, name);
+	if (ret)
+		goto out_release_multi_tz;
+
+	tz = thermal_tripless_zone_device_register(name, devdata, ops, tzp);
+	if (IS_ERR_OR_NULL(tz)) {
+		ret = PTR_ERR(tz);
+		goto out_release_multi_tz;
+	}
+
+	ret = thermal_of_mutli_sensor_coeff(sensor, id, np, &coeff);
+	if (ret)
+		goto out_release_tz;
+
+	ret = thermal_multi_sensor_register(multi_tz, tz, coeff);
+	if (ret)
+		goto out_release_tz;
+
+	return tz;
+
+out_release_tz:
+	thermal_zone_device_unregister(tz);
+out_release_multi_tz:
+	thermal_multi_sensor_tz_free(multi_tz);
+
+	return ERR_PTR(ret);
+}
+
+
 /**
  * thermal_of_zone_register - Register a thermal zone with device node
  * sensor
@@ -479,6 +693,7 @@ static struct thermal_zone_device *thermal_of_zone_register(struct device_node *
 	const char *action;
 	int delay, pdelay;
 	int ntrips;
+	int count;
 	int ret;
 
 	np = of_thermal_zone_find(sensor, id);
@@ -488,10 +703,19 @@ static struct thermal_zone_device *thermal_of_zone_register(struct device_node *
 		return ERR_CAST(np);
 	}
 
-	trips = thermal_of_trips_init(np, &ntrips);
-	if (IS_ERR(trips)) {
-		pr_err("Failed to find trip points for %pOFn id=%d\n", sensor, id);
-		return ERR_CAST(trips);
+	count = of_count_phandle_with_args(np, "thermal-sensors",
+						"#thermal-sensor-cells");
+	if (count <= 0)
+		return ERR_PTR(count);
+
+	/* Only allocate trips if the thermal zone doesn't exist yet */
+	if (!thermal_multi_sensor_find_tz(np->name)) {
+		trips = thermal_of_trips_init(np, &ntrips);
+		if (IS_ERR(trips)) {
+			pr_err("Failed to find trip points for %pOFn id=%d\n", sensor, id);
+			ret = PTR_ERR(trips);
+			goto out_kfree_trips;
+		}
 	}
 
 	ret = thermal_of_monitor_init(np, &delay, &pdelay);
@@ -502,17 +726,25 @@ static struct thermal_zone_device *thermal_of_zone_register(struct device_node *
 
 	thermal_of_parameters_init(np, &tzp);
 
-	of_ops.bind = thermal_of_bind;
-	of_ops.unbind = thermal_of_unbind;
+	if (count == 1) {
+		of_ops.bind = thermal_of_bind;
+		of_ops.unbind = thermal_of_unbind;
+	}
 
 	ret = of_property_read_string(np, "critical-action", &action);
 	if (!ret)
 		if (!of_ops.critical && !strcasecmp(action, "reboot"))
 			of_ops.critical = thermal_zone_device_critical_reboot;
 
-	tz = thermal_zone_device_register_with_trips(np->name, trips, ntrips,
-						     data, &of_ops, &tzp,
-						     pdelay, delay);
+	if (count == 1) {
+		tz = thermal_zone_device_register_with_trips(np->name, trips, ntrips,
+							     data, &of_ops, &tzp,
+							     pdelay, delay);
+	} else {
+		tz = thermal_of_register_multi_tz(sensor, id, np, np->name, trips,
+						  ntrips, data, &of_ops, &tzp,
+						  pdelay, delay);
+	}
 	if (IS_ERR(tz)) {
 		ret = PTR_ERR(tz);
 		pr_err("Failed to register thermal zone %pOFn: %d\n", np, ret);
-- 
2.44.1