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

[Pin-yen Lin <treapking@chromium.org>]

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