thermal/drivers/qoriq: Only enable supported sensors

There are MAX 16 sensors, but not all of them supported. Such as
i.MX8MQ, there are only 3 sensors. Enabling all 16 sensors will
touch reserved bits from i.MX8MQ reference mannual, and TMU will stuck,
temperature will not update anymore.

Fixes: 45038e03d633 ("thermal: qoriq: Enable all sensors before registering them")
Signed-off-by: Peng Fan <peng.fan@nxp.com>
Signed-off-by: Daniel Lezcano <daniel.lezcano@linaro.org>
Link: https://lore.kernel.org/r/20230516083746.63436-3-peng.fan@oss.nxp.com

diff --git a/drivers/thermal/qoriq_thermal.c b/drivers/thermal/qoriq_thermal.c
index b806a09..53748c4 100644 (file)
--- a/drivers/thermal/qoriq_thermal.c
+++ b/drivers/thermal/qoriq_thermal.c
@@ -31,7 +31,6 @@
 #define TMR_DISABLE    0x0
 #define TMR_ME         0x80000000
 #define TMR_ALPF       0x0c000000
-#define TMR_MSITE_ALL  GENMASK(15, 0)
 
 #define REGS_TMTMIR    0x008   /* Temperature measurement interval Register */
 #define TMTMIR_DEFAULT 0x0000000f
@@ -105,6 +104,11 @@ static int tmu_get_temp(struct thermal_zone_device *tz, int *temp)
         * within sensor range. TEMP is an 9 bit value representing
         * temperature in KelVin.
         */
+
+       regmap_read(qdata->regmap, REGS_TMR, &val);
+       if (!(val & TMR_ME))
+               return -EAGAIN;
+
        if (regmap_read_poll_timeout(qdata->regmap,
                                     REGS_TRITSR(qsensor->id),
                                     val,
@@ -128,15 +132,7 @@ static const struct thermal_zone_device_ops tmu_tz_ops = {
 static int qoriq_tmu_register_tmu_zone(struct device *dev,
                                       struct qoriq_tmu_data *qdata)
 {
-       int id;
-
-       if (qdata->ver == TMU_VER1) {
-               regmap_write(qdata->regmap, REGS_TMR,
-                            TMR_MSITE_ALL | TMR_ME | TMR_ALPF);
-       } else {
-               regmap_write(qdata->regmap, REGS_V2_TMSR, TMR_MSITE_ALL);
-               regmap_write(qdata->regmap, REGS_TMR, TMR_ME | TMR_ALPF_V2);
-       }
+       int id, sites = 0;
 
        for (id = 0; id < SITES_MAX; id++) {
                struct thermal_zone_device *tzd;
@@ -153,14 +149,26 @@ static int qoriq_tmu_register_tmu_zone(struct device *dev,
                        if (ret == -ENODEV)
                                continue;
 
-                       regmap_write(qdata->regmap, REGS_TMR, TMR_DISABLE);
                        return ret;
                }
 
+               if (qdata->ver == TMU_VER1)
+                       sites |= 0x1 << (15 - id);
+               else
+                       sites |= 0x1 << id;
+
                if (devm_thermal_add_hwmon_sysfs(dev, tzd))
                        dev_warn(dev,
                                 "Failed to add hwmon sysfs attributes\n");
+       }
 
+       if (sites) {
+               if (qdata->ver == TMU_VER1) {
+                       regmap_write(qdata->regmap, REGS_TMR, TMR_ME | TMR_ALPF | sites);
+               } else {
+                       regmap_write(qdata->regmap, REGS_V2_TMSR, sites);
+                       regmap_write(qdata->regmap, REGS_TMR, TMR_ME | TMR_ALPF_V2);
+               }
        }
 
        return 0;