target/linux/ipq806x/patches-5.10/0063-4-ip806x-tsense-rework-driver.patch

   1 --- a/drivers/thermal/qcom/tsens-ipq8064.c
   2 +++ b/drivers/thermal/qcom/tsens-ipq8064.c
   3 @@ -13,10 +13,12 @@
   4   */
   5
   6  #include <linux/platform_device.h>
   7 +#include <linux/err.h>
   8  #include <linux/delay.h>
   9  #include <linux/bitops.h>
  10  #include <linux/regmap.h>
  11  #include <linux/thermal.h>
  12 +#include <linux/slab.h>
  13  #include <linux/nvmem-consumer.h>
  14  #include <linux/of_platform.h>
  15  #include <linux/io.h>
  16 @@ -211,9 +213,8 @@ static void tsens_scheduler_fn(struct wo
  17         struct tsens_priv *priv = container_of(work, struct tsens_priv,
  18                                         tsens_work);
  19         unsigned int threshold, threshold_low, code, reg, sensor, mask;
  20 -       unsigned int sensor_addr;
  21         bool upper_th_x, lower_th_x;
  22 -       int adc_code, ret;
  23 +       int ret;
  24
  25         ret = regmap_read(priv->tm_map, STATUS_CNTL_8064, &reg);
  26         if (ret)
  27 @@ -262,9 +263,8 @@ static void tsens_scheduler_fn(struct wo
  28                 if (upper_th_x || lower_th_x) {
  29                         /* Notify user space */
  30                         schedule_work(&priv->sensor[0].notify_work);
  31 -                       regmap_read(priv->tm_map, sensor_addr, &adc_code);
  32                         pr_debug("Trigger (%d degrees) for sensor %d\n",
  33 -                               code_to_degC(adc_code, &priv->sensor[0]), 0);
  34 +                               code_to_degC(code, &priv->sensor[0]), 0);
  35                 }
  36         }
  37         regmap_write(priv->tm_map, STATUS_CNTL_8064, reg & mask);
  38 @@ -404,40 +404,55 @@ err_put_device:
  39  static int calibrate_ipq8064(struct tsens_priv *priv)
  40  {
  41         int i;
  42 -       char *data, *data_backup;
  43 -
  44 +       int ret = 0;
  45 +       u8 *data, *data_backup;
  46 +       struct device *dev = priv->dev;
  47         ssize_t num_read = priv->num_sensors;
  48         struct tsens_sensor *s = priv->sensor;
  49
  50 -       data = qfprom_read(priv->dev, "calib");
  51 +       data = qfprom_read(dev, "calib");
  52         if (IS_ERR(data)) {
  53 -               pr_err("Calibration not found.\n");
  54 -               return PTR_ERR(data);
  55 +               ret = PTR_ERR(data);
  56 +               if (ret != -EPROBE_DEFER)
  57 +                       dev_err(dev, "Calibration not found.");
  58 +               goto exit;
  59         }
  60
  61 -       data_backup = qfprom_read(priv->dev, "calib_backup");
  62 +       data_backup = qfprom_read(dev, "calib_backup");
  63         if (IS_ERR(data_backup)) {
  64 -               pr_err("Backup calibration not found.\n");
  65 -               return PTR_ERR(data_backup);
  66 +               ret = PTR_ERR(data_backup);
  67 +               if (ret != -EPROBE_DEFER)
  68 +                       dev_err(dev, "Backup Calibration not found.");
  69 +               goto free_data;
  70         }
  71
  72         for (i = 0; i < num_read; i++) {
  73                 s[i].calib_data = readb_relaxed(data + i);
  74 -               s[i].calib_data_backup = readb_relaxed(data_backup + i);
  75 +
  76 +               if (!s[i].calib_data) {
  77 +                       s[i].calib_data_backup = readb_relaxed(data_backup + i);
  78 +
  79 +                       if (!s[i].calib_data_backup) {
  80 +                               dev_err(dev, "QFPROM TSENS calibration data not present");
  81 +                               ret = -ENODEV;
  82 +                               goto free_backup;
  83 +                       }
  84
  85 -               if (s[i].calib_data_backup)
  86                         s[i].calib_data = s[i].calib_data_backup;
  87 -               if (!s[i].calib_data) {
  88 -                       pr_err("QFPROM TSENS calibration data not present\n");
  89 -                       return -ENODEV;
  90                 }
  91 +
  92                 s[i].slope = tsens_8064_slope[i];
  93                 s[i].offset = CAL_MDEGC - (s[i].calib_data * s[i].slope);
  94         }
  95
  96         hw_init(priv);
  97
  98 -       return 0;
  99 +free_backup:
 100 +       kfree(data_backup);
 101 +free_data:
 102 +       kfree(data);
 103 +exit:
 104 +       return ret;
 105  }
 106
 107  static int get_temp_ipq8064(struct tsens_priv *priv, int id, int *temp)