[v2,3/3] thermal: Add support of multi sensors to thermal_of

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 | 139 +++++++++++++++++++++++++++++++++++
 1 file changed, 139 insertions(+)

Hi Alexandre,

On Fri, Apr 12, 2024 at 4:23 PM Alexandre Bailon <abailon@baylibre.com> 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@baylibre.com>
> ---
>  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