Let the thermal framework handle the trip points instead
of custom code inside the driver. This leads to remove a lot
of code and simplifies the driver.
Implement set_trips callback to set the low and high thresholds,
modify irq enable and disable to handle those thresholds.
Signed-off-by: Pascal Paillet <[email protected]>
---
drivers/thermal/st/stm_thermal.c | 283 +++++++++++--------------------
1 file changed, 95 insertions(+), 188 deletions(-)
diff --git a/drivers/thermal/st/stm_thermal.c b/drivers/thermal/st/stm_thermal.c
index 7577242dadb4..cb72252f2800 100644
--- a/drivers/thermal/st/stm_thermal.c
+++ b/drivers/thermal/st/stm_thermal.c
@@ -51,10 +51,17 @@
/* DTS_DR register mask definitions */
#define TS1_MFREQ_MASK GENMASK(15, 0)
+/* DTS_ITENR register mask definitions */
+#define ITENR_MASK (GENMASK(2, 0) | GENMASK(6, 4))
+
+/* DTS_ICIFR register mask definitions */
+#define ICIFR_MASK (GENMASK(2, 0) | GENMASK(6, 4))
+
/* Less significant bit position definitions */
#define TS1_T0_POS 16
#define TS1_SMP_TIME_POS 16
#define TS1_HITTHD_POS 16
+#define TS1_LITTHD_POS 0
#define HSREF_CLK_DIV_POS 24
/* DTS_CFGR1 bit definitions */
@@ -91,32 +98,28 @@ struct stm_thermal_sensor {
struct thermal_zone_device *th_dev;
enum thermal_device_mode mode;
struct clk *clk;
- int high_temp;
- int low_temp;
- int temp_critical;
- int temp_passive;
unsigned int low_temp_enabled;
- int num_trips;
+ unsigned int high_temp_enabled;
int irq;
void __iomem *base;
int t0, fmt0, ramp_coeff;
};
+static int stm_enable_irq(struct stm_thermal_sensor *sensor);
+
static irqreturn_t stm_thermal_alarm_irq_thread(int irq, void *sdata)
{
- u32 value;
struct stm_thermal_sensor *sensor = sdata;
- /* read IT reason in SR and clear flags */
- value = readl_relaxed(sensor->base + DTS_SR_OFFSET);
+ dev_dbg(sensor->dev, "sr:%d\n",
+ readl_relaxed(sensor->base + DTS_SR_OFFSET));
- if ((value & LOW_THRESHOLD) == LOW_THRESHOLD)
- writel_relaxed(LOW_THRESHOLD, sensor->base + DTS_ICIFR_OFFSET);
+ thermal_zone_device_update(sensor->th_dev, THERMAL_EVENT_UNSPECIFIED);
- if ((value & HIGH_THRESHOLD) == HIGH_THRESHOLD)
- writel_relaxed(HIGH_THRESHOLD, sensor->base + DTS_ICIFR_OFFSET);
+ stm_enable_irq(sensor);
- thermal_zone_device_update(sensor->th_dev, THERMAL_EVENT_UNSPECIFIED);
+ /* Acknoledge all DTS irqs */
+ writel_relaxed(ICIFR_MASK, sensor->base + DTS_ICIFR_OFFSET);
return IRQ_HANDLED;
}
@@ -149,6 +152,8 @@ static int stm_sensor_power_on(struct stm_thermal_sensor *sensor)
writel_relaxed(value, sensor->base +
DTS_CFGR1_OFFSET);
+ sensor->mode = THERMAL_DEVICE_ENABLED;
+
return 0;
}
@@ -156,6 +161,8 @@ static int stm_sensor_power_off(struct stm_thermal_sensor *sensor)
{
u32 value;
+ sensor->mode = THERMAL_DEVICE_DISABLED;
+
/* Stop measuring */
value = readl_relaxed(sensor->base + DTS_CFGR1_OFFSET);
value &= ~TS1_START;
@@ -277,50 +284,15 @@ static int stm_thermal_calculate_threshold(struct stm_thermal_sensor *sensor,
return 0;
}
-static int stm_thermal_set_threshold(struct stm_thermal_sensor *sensor)
-{
- u32 value, th;
- int ret;
-
- value = readl_relaxed(sensor->base + DTS_ITR1_OFFSET);
-
- /* Erase threshold content */
- value &= ~(TS1_LITTHD_MASK | TS1_HITTHD_MASK);
-
- /* Retrieve the sample threshold number th for a given temperature */
- ret = stm_thermal_calculate_threshold(sensor, sensor->high_temp, &th);
- if (ret)
- return ret;
-
- value |= th & TS1_LITTHD_MASK;
-
- if (sensor->low_temp_enabled) {
- /* Retrieve the sample threshold */
- ret = stm_thermal_calculate_threshold(sensor, sensor->low_temp,
- &th);
- if (ret)
- return ret;
-
- value |= (TS1_HITTHD_MASK & (th << TS1_HITTHD_POS));
- }
-
- /* Write value on the Low interrupt threshold */
- writel_relaxed(value, sensor->base + DTS_ITR1_OFFSET);
-
- return 0;
-}
-
/* Disable temperature interrupt */
static int stm_disable_irq(struct stm_thermal_sensor *sensor)
{
u32 value;
- /* Disable IT generation for low and high thresholds */
+ /* Disable IT generation */
value = readl_relaxed(sensor->base + DTS_ITENR_OFFSET);
- writel_relaxed(value & ~(LOW_THRESHOLD | HIGH_THRESHOLD),
- sensor->base + DTS_ITENR_OFFSET);
-
- dev_dbg(sensor->dev, "%s: IT disabled on sensor side", __func__);
+ value &= ~ITENR_MASK;
+ writel_relaxed(value, sensor->base + DTS_ITENR_OFFSET);
return 0;
}
@@ -330,62 +302,67 @@ static int stm_enable_irq(struct stm_thermal_sensor *sensor)
{
u32 value;
- /*
- * Code below enables High temperature threshold using a low threshold
- * sampling value
- */
-
- /* Make sure LOW_THRESHOLD IT is clear before enabling */
- writel_relaxed(LOW_THRESHOLD, sensor->base + DTS_ICIFR_OFFSET);
+ dev_dbg(sensor->dev, "low:%d high:%d\n", sensor->low_temp_enabled,
+ sensor->high_temp_enabled);
- /* Enable IT generation for low threshold */
+ /* Disable IT generation for low and high thresholds */
value = readl_relaxed(sensor->base + DTS_ITENR_OFFSET);
- value |= LOW_THRESHOLD;
-
- /* Enable the low temperature threshold if needed */
- if (sensor->low_temp_enabled) {
- /* Make sure HIGH_THRESHOLD IT is clear before enabling */
- writel_relaxed(HIGH_THRESHOLD, sensor->base + DTS_ICIFR_OFFSET);
+ value &= ~(LOW_THRESHOLD | HIGH_THRESHOLD);
- /* Enable IT generation for high threshold */
+ if (sensor->low_temp_enabled)
value |= HIGH_THRESHOLD;
- }
- /* Enable thresholds */
- writel_relaxed(value, sensor->base + DTS_ITENR_OFFSET);
+ if (sensor->high_temp_enabled)
+ value |= LOW_THRESHOLD;
- dev_dbg(sensor->dev, "%s: IT enabled on sensor side", __func__);
+ /* Enable interrupts */
+ writel_relaxed(value, sensor->base + DTS_ITENR_OFFSET);
return 0;
}
-static int stm_thermal_update_threshold(struct stm_thermal_sensor *sensor)
+static int stm_thermal_set_trips(void *data, int low, int high)
{
+ struct stm_thermal_sensor *sensor = data;
+ u32 itr1, th;
int ret;
- sensor->mode = THERMAL_DEVICE_DISABLED;
+ dev_dbg(sensor->dev, "set trips %d <--> %d\n", low, high);
- ret = stm_sensor_power_off(sensor);
- if (ret)
- return ret;
+ /* Erase threshold content */
+ itr1 = readl_relaxed(sensor->base + DTS_ITR1_OFFSET);
+ itr1 &= ~(TS1_LITTHD_MASK | TS1_HITTHD_MASK);
- ret = stm_disable_irq(sensor);
- if (ret)
- return ret;
+ /*
+ * Disable low-temp if "low" is too small. As per thermal framework
+ * API, we use -INT_MAX rather than INT_MIN.
+ */
- ret = stm_thermal_set_threshold(sensor);
- if (ret)
- return ret;
+ if (low > -INT_MAX) {
+ sensor->low_temp_enabled = 1;
+ ret = stm_thermal_calculate_threshold(sensor, low, &th);
+ if (ret)
+ return ret;
- ret = stm_enable_irq(sensor);
- if (ret)
- return ret;
+ itr1 |= (TS1_HITTHD_MASK & (th << TS1_HITTHD_POS));
+ } else {
+ sensor->low_temp_enabled = 0;
+ }
- ret = stm_sensor_power_on(sensor);
- if (ret)
- return ret;
+ /* Disable high-temp if "high" is too big. */
+ if (high < INT_MAX) {
+ sensor->high_temp_enabled = 1;
+ ret = stm_thermal_calculate_threshold(sensor, high, &th);
+ if (ret)
+ return ret;
- sensor->mode = THERMAL_DEVICE_ENABLED;
+ itr1 |= (TS1_LITTHD_MASK & (th << TS1_LITTHD_POS));
+ } else {
+ sensor->high_temp_enabled = 0;
+ }
+
+ /* Write new threshod values*/
+ writel_relaxed(itr1, sensor->base + DTS_ITR1_OFFSET);
return 0;
}
@@ -429,32 +406,6 @@ static int stm_thermal_get_temp(void *data, int *temp)
*temp = mcelsius(sensor->t0 + ((freqM - sensor->fmt0) /
sensor->ramp_coeff));
- dev_dbg(sensor->dev, "%s: temperature = %d millicelsius",
- __func__, *temp);
-
- /* Update thresholds */
- if (sensor->num_trips > 1) {
- /* Update alarm threshold value to next higher trip point */
- if (sensor->high_temp == sensor->temp_passive &&
- celsius(*temp) >= sensor->temp_passive) {
- sensor->high_temp = sensor->temp_critical;
- sensor->low_temp = sensor->temp_passive;
- sensor->low_temp_enabled = true;
- ret = stm_thermal_update_threshold(sensor);
- if (ret)
- return ret;
- }
-
- if (sensor->high_temp == sensor->temp_critical &&
- celsius(*temp) < sensor->temp_passive) {
- sensor->high_temp = sensor->temp_passive;
- sensor->low_temp_enabled = false;
- ret = stm_thermal_update_threshold(sensor);
- if (ret)
- return ret;
- }
- }
-
return 0;
}
@@ -491,6 +442,8 @@ static int stm_thermal_sensor_off(struct stm_thermal_sensor *sensor)
{
int ret;
+ stm_disable_irq(sensor);
+
ret = stm_sensor_power_off(sensor);
if (ret)
return ret;
@@ -503,7 +456,6 @@ static int stm_thermal_sensor_off(struct stm_thermal_sensor *sensor)
static int stm_thermal_prepare(struct stm_thermal_sensor *sensor)
{
int ret;
- struct device *dev = sensor->dev;
ret = clk_prepare_enable(sensor->clk);
if (ret)
@@ -517,26 +469,8 @@ static int stm_thermal_prepare(struct stm_thermal_sensor *sensor)
if (ret)
goto thermal_unprepare;
- /* Set threshold(s) for IRQ */
- ret = stm_thermal_set_threshold(sensor);
- if (ret)
- goto thermal_unprepare;
-
- ret = stm_enable_irq(sensor);
- if (ret)
- goto thermal_unprepare;
-
- ret = stm_sensor_power_on(sensor);
- if (ret) {
- dev_err(dev, "%s: failed to power on sensor\n", __func__);
- goto irq_disable;
- }
-
return 0;
-irq_disable:
- stm_disable_irq(sensor);
-
thermal_unprepare:
clk_disable_unprepare(sensor->clk);
@@ -553,8 +487,6 @@ static int stm_thermal_suspend(struct device *dev)
if (ret)
return ret;
- sensor->mode = THERMAL_DEVICE_DISABLED;
-
return 0;
}
@@ -567,7 +499,12 @@ static int stm_thermal_resume(struct device *dev)
if (ret)
return ret;
- sensor->mode = THERMAL_DEVICE_ENABLED;
+ ret = stm_sensor_power_on(sensor);
+ if (ret)
+ return ret;
+
+ thermal_zone_device_update(sensor->th_dev, THERMAL_EVENT_UNSPECIFIED);
+ stm_enable_irq(sensor);
return 0;
}
@@ -577,6 +514,7 @@ SIMPLE_DEV_PM_OPS(stm_thermal_pm_ops, stm_thermal_suspend, stm_thermal_resume);
static const struct thermal_zone_of_device_ops stm_tz_ops = {
.get_temp = stm_thermal_get_temp,
+ .set_trips = stm_thermal_set_trips,
};
static const struct of_device_id stm_thermal_of_match[] = {
@@ -589,9 +527,8 @@ static int stm_thermal_probe(struct platform_device *pdev)
{
struct stm_thermal_sensor *sensor;
struct resource *res;
- const struct thermal_trip *trip;
void __iomem *base;
- int ret, i;
+ int ret;
if (!pdev->dev.of_node) {
dev_err(&pdev->dev, "%s: device tree node not found\n",
@@ -622,10 +559,23 @@ static int stm_thermal_probe(struct platform_device *pdev)
return PTR_ERR(sensor->clk);
}
- /* Register IRQ into GIC */
- ret = stm_register_irq(sensor);
- if (ret)
+ stm_disable_irq(sensor);
+
+ /* Clear irq flags */
+ writel_relaxed(ICIFR_MASK, sensor->base + DTS_ICIFR_OFFSET);
+
+ /* Configure and enable HW sensor */
+ ret = stm_thermal_prepare(sensor);
+ if (ret) {
+ dev_err(&pdev->dev, "Error preprare sensor: %d\n", ret);
+ return ret;
+ }
+
+ ret = stm_sensor_power_on(sensor);
+ if (ret) {
+ dev_err(&pdev->dev, "Error power on sensor: %d\n", ret);
return ret;
+ }
sensor->th_dev = devm_thermal_zone_of_sensor_register(&pdev->dev, 0,
sensor,
@@ -638,53 +588,12 @@ static int stm_thermal_probe(struct platform_device *pdev)
return ret;
}
- if (!sensor->th_dev->ops->get_crit_temp) {
- /* Critical point must be provided */
- ret = -EINVAL;
- goto err_tz;
- }
-
- ret = sensor->th_dev->ops->get_crit_temp(sensor->th_dev,
- &sensor->temp_critical);
- if (ret) {
- dev_err(&pdev->dev,
- "Not able to read critical_temp: %d\n", ret);
+ /* Register IRQ into GIC */
+ ret = stm_register_irq(sensor);
+ if (ret)
goto err_tz;
- }
-
- sensor->temp_critical = celsius(sensor->temp_critical);
-
- /* Set thresholds for IRQ */
- sensor->high_temp = sensor->temp_critical;
-
- trip = of_thermal_get_trip_points(sensor->th_dev);
- sensor->num_trips = of_thermal_get_ntrips(sensor->th_dev);
-
- /* Find out passive temperature if it exists */
- for (i = (sensor->num_trips - 1); i >= 0; i--) {
- if (trip[i].type == THERMAL_TRIP_PASSIVE) {
- sensor->temp_passive = celsius(trip[i].temperature);
- /* Update high temperature threshold */
- sensor->high_temp = sensor->temp_passive;
- }
- }
-
- /*
- * Ensure low_temp_enabled flag is disabled.
- * By disabling low_temp_enabled, low threshold IT will not be
- * configured neither enabled because it is not needed as high
- * threshold is set on the lowest temperature trip point after
- * probe.
- */
- sensor->low_temp_enabled = false;
- /* Configure and enable HW sensor */
- ret = stm_thermal_prepare(sensor);
- if (ret) {
- dev_err(&pdev->dev,
- "Not able to enable sensor: %d\n", ret);
- goto err_tz;
- }
+ stm_enable_irq(sensor);
/*
* Thermal_zone doesn't enable hwmon as default,
@@ -695,8 +604,6 @@ static int stm_thermal_probe(struct platform_device *pdev)
if (ret)
goto err_tz;
- sensor->mode = THERMAL_DEVICE_ENABLED;
-
dev_info(&pdev->dev, "%s: Driver initialized successfully\n",
__func__);
--
2.17.1
Hi Pascal,
On 04/11/2019 14:30, Pascal Paillet wrote:
> Let the thermal framework handle the trip points instead
> of custom code inside the driver. This leads to remove a lot
> of code and simplifies the driver.
> Implement set_trips callback to set the low and high thresholds,
> modify irq enable and disable to handle those thresholds.
there are a lot of changes in this patch, please split it into smaller
pieces with a good description.
Thanks
> Signed-off-by: Pascal Paillet <[email protected]>
> ---
> drivers/thermal/st/stm_thermal.c | 283 +++++++++++--------------------
> 1 file changed, 95 insertions(+), 188 deletions(-)
>
> diff --git a/drivers/thermal/st/stm_thermal.c b/drivers/thermal/st/stm_thermal.c
> index 7577242dadb4..cb72252f2800 100644
> --- a/drivers/thermal/st/stm_thermal.c
> +++ b/drivers/thermal/st/stm_thermal.c
> @@ -51,10 +51,17 @@
> /* DTS_DR register mask definitions */
> #define TS1_MFREQ_MASK GENMASK(15, 0)
>
> +/* DTS_ITENR register mask definitions */
> +#define ITENR_MASK (GENMASK(2, 0) | GENMASK(6, 4))
> +
> +/* DTS_ICIFR register mask definitions */
> +#define ICIFR_MASK (GENMASK(2, 0) | GENMASK(6, 4))
> +
> /* Less significant bit position definitions */
> #define TS1_T0_POS 16
> #define TS1_SMP_TIME_POS 16
> #define TS1_HITTHD_POS 16
> +#define TS1_LITTHD_POS 0
> #define HSREF_CLK_DIV_POS 24
>
> /* DTS_CFGR1 bit definitions */
> @@ -91,32 +98,28 @@ struct stm_thermal_sensor {
> struct thermal_zone_device *th_dev;
> enum thermal_device_mode mode;
> struct clk *clk;
> - int high_temp;
> - int low_temp;
> - int temp_critical;
> - int temp_passive;
> unsigned int low_temp_enabled;
> - int num_trips;
> + unsigned int high_temp_enabled;
> int irq;
> void __iomem *base;
> int t0, fmt0, ramp_coeff;
> };
>
> +static int stm_enable_irq(struct stm_thermal_sensor *sensor);
> +
Move the function instead of adding a declaration.
> static irqreturn_t stm_thermal_alarm_irq_thread(int irq, void *sdata)
> {
> - u32 value;
> struct stm_thermal_sensor *sensor = sdata;
>
> - /* read IT reason in SR and clear flags */
> - value = readl_relaxed(sensor->base + DTS_SR_OFFSET);
> + dev_dbg(sensor->dev, "sr:%d\n",
> + readl_relaxed(sensor->base + DTS_SR_OFFSET));
>
> - if ((value & LOW_THRESHOLD) == LOW_THRESHOLD)
> - writel_relaxed(LOW_THRESHOLD, sensor->base + DTS_ICIFR_OFFSET);
> + thermal_zone_device_update(sensor->th_dev, THERMAL_EVENT_UNSPECIFIED);
>
> - if ((value & HIGH_THRESHOLD) == HIGH_THRESHOLD)
> - writel_relaxed(HIGH_THRESHOLD, sensor->base + DTS_ICIFR_OFFSET);
> + stm_enable_irq(sensor);
>
> - thermal_zone_device_update(sensor->th_dev, THERMAL_EVENT_UNSPECIFIED);
> + /* Acknoledge all DTS irqs */
> + writel_relaxed(ICIFR_MASK, sensor->base + DTS_ICIFR_OFFSET);
>
> return IRQ_HANDLED;
> }
> @@ -149,6 +152,8 @@ static int stm_sensor_power_on(struct stm_thermal_sensor *sensor)
> writel_relaxed(value, sensor->base +
> DTS_CFGR1_OFFSET);
>
> + sensor->mode = THERMAL_DEVICE_ENABLED;
> +
> return 0;
> }
>
> @@ -156,6 +161,8 @@ static int stm_sensor_power_off(struct stm_thermal_sensor *sensor)
> {
> u32 value;
>
> + sensor->mode = THERMAL_DEVICE_DISABLED;
> +
> /* Stop measuring */
> value = readl_relaxed(sensor->base + DTS_CFGR1_OFFSET);
> value &= ~TS1_START;
> @@ -277,50 +284,15 @@ static int stm_thermal_calculate_threshold(struct stm_thermal_sensor *sensor,
> return 0;
> }
>
> -static int stm_thermal_set_threshold(struct stm_thermal_sensor *sensor)
> -{
> - u32 value, th;
> - int ret;
> -
> - value = readl_relaxed(sensor->base + DTS_ITR1_OFFSET);
> -
> - /* Erase threshold content */
> - value &= ~(TS1_LITTHD_MASK | TS1_HITTHD_MASK);
> -
> - /* Retrieve the sample threshold number th for a given temperature */
> - ret = stm_thermal_calculate_threshold(sensor, sensor->high_temp, &th);
> - if (ret)
> - return ret;
> -
> - value |= th & TS1_LITTHD_MASK;
> -
> - if (sensor->low_temp_enabled) {
> - /* Retrieve the sample threshold */
> - ret = stm_thermal_calculate_threshold(sensor, sensor->low_temp,
> - &th);
> - if (ret)
> - return ret;
> -
> - value |= (TS1_HITTHD_MASK & (th << TS1_HITTHD_POS));
> - }
> -
> - /* Write value on the Low interrupt threshold */
> - writel_relaxed(value, sensor->base + DTS_ITR1_OFFSET);
> -
> - return 0;
> -}
> -
> /* Disable temperature interrupt */
> static int stm_disable_irq(struct stm_thermal_sensor *sensor)
> {
> u32 value;
>
> - /* Disable IT generation for low and high thresholds */
> + /* Disable IT generation */
> value = readl_relaxed(sensor->base + DTS_ITENR_OFFSET);
> - writel_relaxed(value & ~(LOW_THRESHOLD | HIGH_THRESHOLD),
> - sensor->base + DTS_ITENR_OFFSET);
> -
> - dev_dbg(sensor->dev, "%s: IT disabled on sensor side", __func__);
> + value &= ~ITENR_MASK;
> + writel_relaxed(value, sensor->base + DTS_ITENR_OFFSET);
>
> return 0;
> }
> @@ -330,62 +302,67 @@ static int stm_enable_irq(struct stm_thermal_sensor *sensor)
> {
> u32 value;
>
> - /*
> - * Code below enables High temperature threshold using a low threshold
> - * sampling value
> - */
> -
> - /* Make sure LOW_THRESHOLD IT is clear before enabling */
> - writel_relaxed(LOW_THRESHOLD, sensor->base + DTS_ICIFR_OFFSET);
> + dev_dbg(sensor->dev, "low:%d high:%d\n", sensor->low_temp_enabled,
> + sensor->high_temp_enabled);
>
> - /* Enable IT generation for low threshold */
> + /* Disable IT generation for low and high thresholds */
> value = readl_relaxed(sensor->base + DTS_ITENR_OFFSET);
> - value |= LOW_THRESHOLD;
> -
> - /* Enable the low temperature threshold if needed */
> - if (sensor->low_temp_enabled) {
> - /* Make sure HIGH_THRESHOLD IT is clear before enabling */
> - writel_relaxed(HIGH_THRESHOLD, sensor->base + DTS_ICIFR_OFFSET);
> + value &= ~(LOW_THRESHOLD | HIGH_THRESHOLD);
>
> - /* Enable IT generation for high threshold */
> + if (sensor->low_temp_enabled)
> value |= HIGH_THRESHOLD;
> - }
>
> - /* Enable thresholds */
> - writel_relaxed(value, sensor->base + DTS_ITENR_OFFSET);
> + if (sensor->high_temp_enabled)
> + value |= LOW_THRESHOLD;
>
> - dev_dbg(sensor->dev, "%s: IT enabled on sensor side", __func__);
> + /* Enable interrupts */
> + writel_relaxed(value, sensor->base + DTS_ITENR_OFFSET);
>
> return 0;
> }
>
> -static int stm_thermal_update_threshold(struct stm_thermal_sensor *sensor)
> +static int stm_thermal_set_trips(void *data, int low, int high)
> {
> + struct stm_thermal_sensor *sensor = data;
> + u32 itr1, th;
> int ret;
>
> - sensor->mode = THERMAL_DEVICE_DISABLED;
> + dev_dbg(sensor->dev, "set trips %d <--> %d\n", low, high);
>
> - ret = stm_sensor_power_off(sensor);
> - if (ret)
> - return ret;
> + /* Erase threshold content */
> + itr1 = readl_relaxed(sensor->base + DTS_ITR1_OFFSET);
> + itr1 &= ~(TS1_LITTHD_MASK | TS1_HITTHD_MASK);
>
> - ret = stm_disable_irq(sensor);
> - if (ret)
> - return ret;
> + /*
> + * Disable low-temp if "low" is too small. As per thermal framework
> + * API, we use -INT_MAX rather than INT_MIN.
> + */
>
> - ret = stm_thermal_set_threshold(sensor);
> - if (ret)
> - return ret;
> + if (low > -INT_MAX) {
> + sensor->low_temp_enabled = 1;
> + ret = stm_thermal_calculate_threshold(sensor, low, &th);
> + if (ret)
> + return ret;
>
> - ret = stm_enable_irq(sensor);
> - if (ret)
> - return ret;
> + itr1 |= (TS1_HITTHD_MASK & (th << TS1_HITTHD_POS));
> + } else {
> + sensor->low_temp_enabled = 0;
> + }
>
> - ret = stm_sensor_power_on(sensor);
> - if (ret)
> - return ret;
> + /* Disable high-temp if "high" is too big. */
> + if (high < INT_MAX) {
> + sensor->high_temp_enabled = 1;
> + ret = stm_thermal_calculate_threshold(sensor, high, &th);
> + if (ret)
> + return ret;
>
> - sensor->mode = THERMAL_DEVICE_ENABLED;
> + itr1 |= (TS1_LITTHD_MASK & (th << TS1_LITTHD_POS));
> + } else {
> + sensor->high_temp_enabled = 0;
> + }
> +
> + /* Write new threshod values*/
> + writel_relaxed(itr1, sensor->base + DTS_ITR1_OFFSET);
>
> return 0;
> }
> @@ -429,32 +406,6 @@ static int stm_thermal_get_temp(void *data, int *temp)
> *temp = mcelsius(sensor->t0 + ((freqM - sensor->fmt0) /
> sensor->ramp_coeff));
>
> - dev_dbg(sensor->dev, "%s: temperature = %d millicelsius",
> - __func__, *temp);
> -
> - /* Update thresholds */
> - if (sensor->num_trips > 1) {
> - /* Update alarm threshold value to next higher trip point */
> - if (sensor->high_temp == sensor->temp_passive &&
> - celsius(*temp) >= sensor->temp_passive) {
> - sensor->high_temp = sensor->temp_critical;
> - sensor->low_temp = sensor->temp_passive;
> - sensor->low_temp_enabled = true;
> - ret = stm_thermal_update_threshold(sensor);
> - if (ret)
> - return ret;
> - }
> -
> - if (sensor->high_temp == sensor->temp_critical &&
> - celsius(*temp) < sensor->temp_passive) {
> - sensor->high_temp = sensor->temp_passive;
> - sensor->low_temp_enabled = false;
> - ret = stm_thermal_update_threshold(sensor);
> - if (ret)
> - return ret;
> - }
> - }
> -
> return 0;
> }
>
> @@ -491,6 +442,8 @@ static int stm_thermal_sensor_off(struct stm_thermal_sensor *sensor)
> {
> int ret;
>
> + stm_disable_irq(sensor);
> +
> ret = stm_sensor_power_off(sensor);
> if (ret)
> return ret;
> @@ -503,7 +456,6 @@ static int stm_thermal_sensor_off(struct stm_thermal_sensor *sensor)
> static int stm_thermal_prepare(struct stm_thermal_sensor *sensor)
> {
> int ret;
> - struct device *dev = sensor->dev;
>
> ret = clk_prepare_enable(sensor->clk);
> if (ret)
> @@ -517,26 +469,8 @@ static int stm_thermal_prepare(struct stm_thermal_sensor *sensor)
> if (ret)
> goto thermal_unprepare;
>
> - /* Set threshold(s) for IRQ */
> - ret = stm_thermal_set_threshold(sensor);
> - if (ret)
> - goto thermal_unprepare;
> -
> - ret = stm_enable_irq(sensor);
> - if (ret)
> - goto thermal_unprepare;
> -
> - ret = stm_sensor_power_on(sensor);
> - if (ret) {
> - dev_err(dev, "%s: failed to power on sensor\n", __func__);
> - goto irq_disable;
> - }
> -
> return 0;
>
> -irq_disable:
> - stm_disable_irq(sensor);
> -
> thermal_unprepare:
> clk_disable_unprepare(sensor->clk);
>
> @@ -553,8 +487,6 @@ static int stm_thermal_suspend(struct device *dev)
> if (ret)
> return ret;
>
> - sensor->mode = THERMAL_DEVICE_DISABLED;
> -
> return 0;
> }
>
> @@ -567,7 +499,12 @@ static int stm_thermal_resume(struct device *dev)
> if (ret)
> return ret;
>
> - sensor->mode = THERMAL_DEVICE_ENABLED;
> + ret = stm_sensor_power_on(sensor);
> + if (ret)
> + return ret;
> +
> + thermal_zone_device_update(sensor->th_dev, THERMAL_EVENT_UNSPECIFIED);
> + stm_enable_irq(sensor);
>
> return 0;
> }
> @@ -577,6 +514,7 @@ SIMPLE_DEV_PM_OPS(stm_thermal_pm_ops, stm_thermal_suspend, stm_thermal_resume);
>
> static const struct thermal_zone_of_device_ops stm_tz_ops = {
> .get_temp = stm_thermal_get_temp,
> + .set_trips = stm_thermal_set_trips,
> };
>
> static const struct of_device_id stm_thermal_of_match[] = {
> @@ -589,9 +527,8 @@ static int stm_thermal_probe(struct platform_device *pdev)
> {
> struct stm_thermal_sensor *sensor;
> struct resource *res;
> - const struct thermal_trip *trip;
> void __iomem *base;
> - int ret, i;
> + int ret;
>
> if (!pdev->dev.of_node) {
> dev_err(&pdev->dev, "%s: device tree node not found\n",
> @@ -622,10 +559,23 @@ static int stm_thermal_probe(struct platform_device *pdev)
> return PTR_ERR(sensor->clk);
> }
>
> - /* Register IRQ into GIC */
> - ret = stm_register_irq(sensor);
> - if (ret)
> + stm_disable_irq(sensor);
> +
> + /* Clear irq flags */
> + writel_relaxed(ICIFR_MASK, sensor->base + DTS_ICIFR_OFFSET);
> +
> + /* Configure and enable HW sensor */
> + ret = stm_thermal_prepare(sensor);
> + if (ret) {
> + dev_err(&pdev->dev, "Error preprare sensor: %d\n", ret);
> + return ret;
> + }
> +
> + ret = stm_sensor_power_on(sensor);
> + if (ret) {
> + dev_err(&pdev->dev, "Error power on sensor: %d\n", ret);
> return ret;
> + }
>
> sensor->th_dev = devm_thermal_zone_of_sensor_register(&pdev->dev, 0,
> sensor,
> @@ -638,53 +588,12 @@ static int stm_thermal_probe(struct platform_device *pdev)
> return ret;
> }
>
> - if (!sensor->th_dev->ops->get_crit_temp) {
> - /* Critical point must be provided */
> - ret = -EINVAL;
> - goto err_tz;
> - }
> -
> - ret = sensor->th_dev->ops->get_crit_temp(sensor->th_dev,
> - &sensor->temp_critical);
> - if (ret) {
> - dev_err(&pdev->dev,
> - "Not able to read critical_temp: %d\n", ret);
> + /* Register IRQ into GIC */
> + ret = stm_register_irq(sensor);
> + if (ret)
> goto err_tz;
> - }
> -
> - sensor->temp_critical = celsius(sensor->temp_critical);
> -
> - /* Set thresholds for IRQ */
> - sensor->high_temp = sensor->temp_critical;
> -
> - trip = of_thermal_get_trip_points(sensor->th_dev);
> - sensor->num_trips = of_thermal_get_ntrips(sensor->th_dev);
> -
> - /* Find out passive temperature if it exists */
> - for (i = (sensor->num_trips - 1); i >= 0; i--) {
> - if (trip[i].type == THERMAL_TRIP_PASSIVE) {
> - sensor->temp_passive = celsius(trip[i].temperature);
> - /* Update high temperature threshold */
> - sensor->high_temp = sensor->temp_passive;
> - }
> - }
> -
> - /*
> - * Ensure low_temp_enabled flag is disabled.
> - * By disabling low_temp_enabled, low threshold IT will not be
> - * configured neither enabled because it is not needed as high
> - * threshold is set on the lowest temperature trip point after
> - * probe.
> - */
> - sensor->low_temp_enabled = false;
>
> - /* Configure and enable HW sensor */
> - ret = stm_thermal_prepare(sensor);
> - if (ret) {
> - dev_err(&pdev->dev,
> - "Not able to enable sensor: %d\n", ret);
> - goto err_tz;
> - }
> + stm_enable_irq(sensor);
>
> /*
> * Thermal_zone doesn't enable hwmon as default,
> @@ -695,8 +604,6 @@ static int stm_thermal_probe(struct platform_device *pdev)
> if (ret)
> goto err_tz;
>
> - sensor->mode = THERMAL_DEVICE_ENABLED;
> -
> dev_info(&pdev->dev, "%s: Driver initialized successfully\n",
> __func__);
>
>
--
<http://www.linaro.org/> Linaro.org │ Open source software for ARM SoCs
Follow Linaro: <http://www.facebook.com/pages/Linaro> Facebook |
<http://twitter.com/#!/linaroorg> Twitter |
<http://www.linaro.org/linaro-blog/> Blog