Re: [PATCH v6 4/4] thermal: mediatek: add another get_temp ops for thermal sensors

Provide thermal zone to read thermal sensor in the SoC. We can read all the
thermal sensors value in the SoC by the node /sys/class/thermal/

In mtk_thermal_bank_temperature, return -EAGAIN instead of -EACCESS
on the first read of sensor that often are bogus values.
This can avoid following warning on boot:

  thermal thermal_zone6: failed to read out thermal zone (-13)

Signed-off-by: Michael Kao <redacted>
Signed-off-by: Hsin-Yi Wang <redacted>
Signed-off-by: Amjad Ouled-Ameur <redacted>
Reviewed-by: AngeloGioacchino Del Regno <angelogioacchino.delregno@collabora.com>
---
 drivers/thermal/mtk_thermal.c | 104 ++++++++++++++++++++++++++++++++----------
 1 file changed, 79 insertions(+), 25 deletions(-)

diff --git a/drivers/thermal/mtk_thermal.c b/drivers/thermal/mtk_thermal.c
index 3a5df1440822..311ad611fdab 100644
--- a/drivers/thermal/mtk_thermal.c
+++ b/drivers/thermal/mtk_thermal.c

@@ -259,6 +259,11 @@ enum mtk_thermal_version {

 struct mtk_thermal;

+struct mtk_thermal_zone {
+       struct mtk_thermal *mt;
+       int id;
+};
+
 struct thermal_bank_cfg {
        unsigned int num_sensors;
        const int *sensors;

@@ -307,6 +312,8 @@ struct mtk_thermal {

        const struct mtk_thermal_data *conf;
        struct mtk_thermal_bank banks[MAX_NUM_ZONES];
+
+       int (*raw_to_mcelsius)(struct mtk_thermal *mt, int sensno, s32 raw);
 };

 /* MT8183 thermal sensor data */

@@ -709,6 +716,29 @@ static void mtk_thermal_put_bank(struct mtk_thermal_bank *bank)
                mutex_unlock(&mt->lock);
 }

+static int _get_sensor_temp(struct mtk_thermal *mt, int id)
+{
+       u32 raw;
+       int temp;
+
+       const struct mtk_thermal_data *conf = mt->conf;
+
+       raw = readl(mt->thermal_base + conf->msr[id]);
+
+       temp = mt->raw_to_mcelsius(mt, id, raw);
+
+       /*
+        * The first read of a sensor often contains very high bogus
+        * temperature value. Filter these out so that the system does
+        * not immediately shut down.
+        */
+
+       if (temp > 200000)
+               return -EAGAIN;
+       else
+               return temp;
+}
+
 /**
  * mtk_thermal_bank_temperature - get the temperature of a bank
  * @bank:      The bank

@@ -721,26 +751,9 @@ static int mtk_thermal_bank_temperature(struct mtk_thermal_bank *bank)
        struct mtk_thermal *mt = bank->mt;
        const struct mtk_thermal_data *conf = mt->conf;
        int i, temp = INT_MIN, max = INT_MIN;
-       u32 raw;

        for (i = 0; i < conf->bank_data[bank->id].num_sensors; i++) {
-               raw = readl(mt->thermal_base + conf->msr[i]);
-
-               if (mt->conf->version == MTK_THERMAL_V1) {
-                       temp = raw_to_mcelsius_v1(
-                               mt, conf->bank_data[bank->id].sensors[i], raw);
-               } else {
-                       temp = raw_to_mcelsius_v2(
-                               mt, conf->bank_data[bank->id].sensors[i], raw);
-               }
-
-               /*
-                * The first read of a sensor often contains very high bogus
-                * temperature value. Filter these out so that the system does
-                * not immediately shut down.
-                */
-               if (temp > 200000)
-                       temp = 0;
+               temp = _get_sensor_temp(mt, i);

                if (temp > max)
                        max = temp;

@@ -749,9 +762,10 @@ static int mtk_thermal_bank_temperature(struct mtk_thermal_bank *bank)
        return max;
 }

-static int mtk_read_temp(struct thermal_zone_device *tz, int *temperature)
+static int mtk_read_temp(struct thermal_zone_device *tzdev, int *temperature)
 {
-       struct mtk_thermal *mt = tz->devdata;
+       struct mtk_thermal_zone *tz = tzdev->devdata;
+       struct mtk_thermal *mt = tz->mt;
        int i;
        int tempmax = INT_MIN;

@@ -770,10 +784,28 @@ static int mtk_read_temp(struct thermal_zone_device *tz, int *temperature)
        return 0;
 }

+static int mtk_read_sensor_temp(struct thermal_zone_device *tzdev, int *temperature)
+{
+       struct mtk_thermal_zone *tz = tzdev->devdata;
+       struct mtk_thermal *mt = tz->mt;
+       int id = tz->id - 1;
+
+       if (id < 0)
+               return -EACCES;
+
+       *temperature = _get_sensor_temp(mt, id);
+
+       return 0;
+}
+
 static const struct thermal_zone_device_ops mtk_thermal_ops = {
        .get_temp = mtk_read_temp,
 };

+static const struct thermal_zone_device_ops mtk_thermal_sensor_ops = {
+       .get_temp = mtk_read_sensor_temp,
+};
+
 static void mtk_thermal_init_bank(struct mtk_thermal *mt, int num,
                                  u32 apmixed_phys_base, u32 auxadc_phys_base,
                                  int ctrl_id)

@@ -1072,6 +1104,7 @@ static int mtk_thermal_probe(struct platform_device *pdev)
        u64 auxadc_phys_base, apmixed_phys_base;
        struct thermal_zone_device *tzdev;
        void __iomem *apmixed_base, *auxadc_base;
+       struct mtk_thermal_zone *tz;

        mt = devm_kzalloc(&pdev->dev, sizeof(*mt), GFP_KERNEL);
        if (!mt)

@@ -1150,6 +1183,9 @@ static int mtk_thermal_probe(struct platform_device *pdev)

        mtk_thermal_turn_on_buffer(mt, apmixed_base);

+       mt->raw_to_mcelsius = (mt->conf->version == MTK_THERMAL_V1) ?
+                               raw_to_mcelsius_v1 : raw_to_mcelsius_v2;
+
        if (mt->conf->version == MTK_THERMAL_V2) {
                mtk_thermal_release_periodic_ts(mt, auxadc_base);
        }

@@ -1161,11 +1197,29 @@ static int mtk_thermal_probe(struct platform_device *pdev)

        platform_set_drvdata(pdev, mt);

-       tzdev = devm_thermal_of_zone_register(&pdev->dev, 0, mt,
-                                             &mtk_thermal_ops);
-       if (IS_ERR(tzdev)) {
-               ret = PTR_ERR(tzdev);
-               goto err_disable_clk_peri_therm;
+       for (i = 0; i < mt->conf->num_sensors + 1; i++) {
+               tz = devm_kmalloc(&pdev->dev, sizeof(*tz), GFP_KERNEL);
+               if (!tz)
+                       return -ENOMEM;
+
+               tz->mt = mt;
+               tz->id = i;
+
+               tzdev = devm_thermal_of_zone_register(&pdev->dev, i, tz, (i == 0) ?
+                                                            &mtk_thermal_ops :
+                                                            &mtk_thermal_sensor_ops);
+
+               if (IS_ERR(tzdev)) {
+                       if (PTR_ERR(tzdev) == -ENODEV) {
+                               dev_warn(&pdev->dev,
+                                        "sensor %d not registered in thermal zone in dt\n", i);
+                               continue;
+                       }
+                       if (PTR_ERR(tzdev) == -EACCES) {
+                               ret = PTR_ERR(tzdev);
+                               goto err_disable_clk_peri_therm;
+                       }
+               }
        }

        ret = devm_thermal_add_hwmon_sysfs(tzdev);