Hi Alexandre,
On Fri, Apr 12, 2024 at 4:23 PM Alexandre Bailon <abailon@xxxxxxxxxxxx> wrote:
>
> 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@xxxxxxxxxxxx>
> ---
> drivers/thermal/thermal_of.c | 139 +++++++++++++++++++++++++++++++++++
> 1 file changed, 139 insertions(+)
>
> diff --git a/drivers/thermal/thermal_of.c b/drivers/thermal/thermal_of.c
> index 1e0655b63259..3f36d8a3d8e8 100644
> --- a/drivers/thermal/thermal_of.c
> +++ b/drivers/thermal/thermal_of.c
> @@ -441,12 +441,146 @@ static void thermal_of_zone_unregister(struct thermal_zone_device *tz)
> struct thermal_trip *trips = tz->trips;
> struct thermal_zone_device_ops *ops = tz->ops;
>
> + thermal_multi_sensor_unregister(tz);
> thermal_zone_device_disable(tz);
> thermal_zone_device_unregister(tz);
> kfree(trips);
> kfree(ops);
> }
>
> +int thermal_of_get_sensor_id(struct device_node *tz_np,
> + struct device_node *sensor_np,
> + int phandle_index, u32 *id)
> +{
> + struct of_phandle_args sensor_specs;
> + int ret;
> +
> + ret = of_parse_phandle_with_args(tz_np,
> + "thermal-sensors",
> + "#thermal-sensor-cells",
> + phandle_index,
> + &sensor_specs);
> + if (ret)
> + return ret;
> +
> + if (sensor_specs.np != sensor_np) {
> + of_node_put(sensor_specs.np);
> + return -ENODEV;
> + }
> +
> + if (sensor_specs.args_count > 1)
> + pr_warn("%pOFn: too many cells in sensor specifier %d\n",
> + sensor_specs.np, sensor_specs.args_count);
> +
> + *id = sensor_specs.args_count ? sensor_specs.args[0] : 0;
> + of_node_put(sensor_specs.np);
> +
> + return 0;
> +}
> +
> +static int thermal_of_has_sensor_id(struct device_node *tz_np,
> + struct device_node *sensor_np,
> + u32 sensor_id)
> +{
> + int count;
> + int i;
> +
> + count = of_count_phandle_with_args(tz_np,
> + "thermal-sensors",
> + "#thermal-sensor-cells");
> + if (count <= 0)
> + return -ENODEV;
> +
> + for (i = 0; i < count; i++) {
> + int ret;
> + u32 id;
> +
> + ret = thermal_of_get_sensor_id(tz_np, sensor_np, i, &id);
> + if (ret)
> + return ret;
We should just `continue` here or handle -ENODEV differently. The
current implementation doesn't support a thermal zone references
sensors from different nodes.
> +
> + if (id == sensor_id)
> + return i;
> +
> + }
> +
> + return -ENODEV;
> +}
> +
> +static int thermal_of_register_mutli_sensor(struct device_node *sensor, int id,
> + struct thermal_zone_device *tz,
> + struct device_node *tz_np)
> +{
> + struct device_node *child;
> + u32 *coeff;
> + int ret;
> + int i;
> +
> + /*
> + * Go through all the thermal zone and check if the sensor is
> + * referenced. If so, find or create a multi sensor thermal zone
> + * and register the sensor to it.
> + */
> + for_each_available_child_of_node(of_get_parent(tz_np), child) {
> + int count;
> + int index;
> + int offset;
> +
> + /* Skip the tz that is currently registering */
> + if (child == tz_np)
> + continue;
of_thermal_zone_find() simply returns the first thermal zone
containing the sensor, so there's no guarantee that tz here is the
currently registering thermal zone. Maybe we should make
of_thermal_zone_find() only return thermal zones with only one sensor
attached?
> +
> + /* Test if the sensor is referenced by a tz*/
> + index = thermal_of_has_sensor_id(child, sensor, id);
> + if (index < 0)
> + continue;
> +
> + /*
> + * Get the coefficients and offset and assign them to the
> + * multi sensor thermal zone.
> + */
> + count = of_count_phandle_with_args(child,
> + "thermal-sensors",
> + "#thermal-sensor-cells");
> + coeff = kmalloc_array(count, sizeof(*coeff), GFP_KERNEL);
> + if (!coeff)
> + goto err;
> +
> + for (i = 0; i < count; i++) {
> + ret = of_property_read_u32_index(child,
> + "coefficients",
> + i, coeff + i);
> + if (ret)
> + coeff[i] = 1;
> + }
> +
> + ret = of_property_read_u32_index(child, "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);
> + if (ret)
> + goto err_free_coeff;
> +
> + ret = thermal_multi_sensor_register(child->name, tz,
> + coeff[index]);
The indentation of this line is wrong.
> + if (ret)
> + goto err_free_coeff;
> + kfree(coeff);
> + }
> +
> + return 0;
> +
> +err_free_coeff:
> + kfree(coeff);
> +err:
> + thermal_multi_sensor_unregister(tz);
> +
> + return ret;
> +}
> +
> /**
> * thermal_of_zone_register - Register a thermal zone with device node
> * sensor
> @@ -528,6 +662,11 @@ static struct thermal_zone_device *thermal_of_zone_register(struct device_node *
> return ERR_PTR(ret);
> }
>
> + /* Register the sensor to all other thermal zone referencing it */
> + ret = thermal_of_register_mutli_sensor(sensor, id, tz, np);
> + if (ret)
> + pr_warn("Failed to register a sensor to a multi sensor tz\n");
> +
> return tz;
>
> out_kfree_trips:
> --
> 2.41.0
>
Best regards,
Pin-yen
