x86/kprobes: Fix crash when probing CS CALL instructionsgrafted

When using eBPF to probe CS CALL instructions within a function,
a crash can be triggered.

The eBPF tool probes offset 257 of the __hrtimer_run_queues()
function:

<__hrtimer_run_queues+249>:  nopl   0x0(%rax,%rax,1)
<__hrtimer_run_queues+254>:  mov    %r14,%rdi
<__hrtimer_run_queues+257>:  cs call <__x86_indirect_thunk_r12>
<__hrtimer_run_queues+263>:  mov    %eax,%r12d
<__hrtimer_run_queues+266>:  xchg   %ax,%ax
<__hrtimer_run_queues+268>:  mov    %r13,%rdi

Which triggers this crash:

  BUG: unable to handle page fault for address: 00000000000f41c9
  #PF: supervisor write access in kernel mode
  #PF: error_code(0x0002) - not-present page
  PGD 0 P4D 0
  Oops: 0002 [#1] SMP NOPTI
  CPU: 1 PID: 0 Comm: swapper/1 Kdump: loaded Tainted: P
  RIP: 0010:__hrtimer_run_queues+0x106/0x230

Note that __hrtimer_run_queues+0x106 is __hrtimer_run_queues+262, which is
at the 6th byte of the above CS CALL instruction. Since the CS CALL
instruction occupies 6 bytes, the exception occurred in the middle of that
call instruction.

The root cause is that when using eBPF tools to probe in the middle of a
function, a kprobe with INT3 is used as the underlying implementation.

During single-step emulation of the original CALL instruction,
int3_emulate_call() assumes that the probed CALL instruction is 5 bytes
long. However, the actual CS-prefixed CALL instruction occupies 6 bytes,
so it constructs an incorrect exception return address. When the CPU
returns from the kprobe handler, the next instruction to be executed is at
the address of the last byte of that CS CALL instruction. Coincidentally,
starting from that address, the CPU fetches and decodes a completely
different instruction, which ultimately triggers a kernel crash.

Fix the issue by using the actual instruction length obtained from
the instruction decoder when constructing the exception return
address, rather than relying on the hardcoded CALL_INSN_SIZE macro.

[ mingo: Refined the changelog ]

Fixes: 6256e668b7af ("x86/kprobes: Use int3 instead of debug trap for single-step")
Suggested-by: Masami Hiramatsu (Google) <mhiramat@kernel.org>
Signed-off-by: Jinke Han <jinkehan@didiglobal.com>
Signed-off-by: Ingo Molnar <mingo@kernel.org>
Reviewed-by: Masami Hiramatsu (Google) <mhiramat@kernel.org>
Acked-by: Yafang Shao <laoar.shao@gmail.com>
Acked-by: Borislav Petkov <bp@alien8.de>
Cc: Peter Zijlstra <peterz@infradead.org>
Link: https://patch.msgid.link/20260908073742.GA10517@didi-ThinkCentre-M920t-N000

6 files changed, 1165 insertions, 0 deletions

diff --git a/drivers/thermal/st/Kconfig b/drivers/thermal/st/Kconfig
new file mode 100644
index 000000000..ecbdf4ef0
--- /dev/null
+++ b/drivers/thermal/st/Kconfig
@@ -0,0 +1,23 @@
+# SPDX-License-Identifier: GPL-2.0-only
+#
+# STMicroelectronics thermal drivers configuration
+#
+
+config ST_THERMAL
+	tristate "Thermal sensors on STMicroelectronics STi series of SoCs"
+	help
+	  Support for thermal sensors on STMicroelectronics STi series of SoCs.
+
+config ST_THERMAL_MEMMAP
+	select ST_THERMAL
+	tristate "STi series memory mapped access based thermal sensors"
+
+config STM32_THERMAL
+	tristate "Thermal framework support on STMicroelectronics STM32 series of SoCs"
+	depends on MACH_STM32MP157
+	default y
+	help
+	  Support for thermal framework on STMicroelectronics STM32 series of
+	  SoCs. This thermal driver allows to access to general thermal framework
+	  functionalities and to access to SoC sensor functionalities. This
+	  configuration is fully dependent of MACH_STM32MP157.
diff --git a/drivers/thermal/st/Makefile b/drivers/thermal/st/Makefile
new file mode 100644
index 000000000..9bb0342b7
--- /dev/null
+++ b/drivers/thermal/st/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0-only
+obj-$(CONFIG_ST_THERMAL)		:= st_thermal.o
+obj-$(CONFIG_ST_THERMAL_MEMMAP)		+= st_thermal_memmap.o
+obj-$(CONFIG_STM32_THERMAL)		+= stm_thermal.o
diff --git a/drivers/thermal/st/st_thermal.c b/drivers/thermal/st/st_thermal.c
new file mode 100644
index 000000000..1470ca519
--- /dev/null
+++ b/drivers/thermal/st/st_thermal.c
@@ -0,0 +1,267 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * ST Thermal Sensor Driver core routines
+ * Author: Ajit Pal Singh <ajitpal.singh@st.com>
+ *
+ * Copyright (C) 2003-2014 STMicroelectronics (R&D) Limited
+ */
+
+#include <linux/clk.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+
+#include "st_thermal.h"
+#include "../thermal_hwmon.h"
+
+/* The Thermal Framework expects millidegrees */
+#define mcelsius(temp)			((temp) * 1000)
+
+/*
+ * Function to allocate regfields which are common
+ * between syscfg and memory mapped based sensors
+ */
+static int st_thermal_alloc_regfields(struct st_thermal_sensor *sensor)
+{
+	struct device *dev = sensor->dev;
+	struct regmap *regmap = sensor->regmap;
+	const struct reg_field *reg_fields = sensor->cdata->reg_fields;
+
+	sensor->dcorrect = devm_regmap_field_alloc(dev, regmap,
+						   reg_fields[DCORRECT]);
+
+	sensor->overflow = devm_regmap_field_alloc(dev, regmap,
+						   reg_fields[OVERFLOW]);
+
+	sensor->temp_data = devm_regmap_field_alloc(dev, regmap,
+						    reg_fields[DATA]);
+
+	if (IS_ERR(sensor->dcorrect) ||
+	    IS_ERR(sensor->overflow) ||
+	    IS_ERR(sensor->temp_data)) {
+		dev_err(dev, "failed to allocate common regfields\n");
+		return -EINVAL;
+	}
+
+	return sensor->ops->alloc_regfields(sensor);
+}
+
+static int st_thermal_sensor_on(struct st_thermal_sensor *sensor)
+{
+	int ret;
+	struct device *dev = sensor->dev;
+
+	ret = clk_prepare_enable(sensor->clk);
+	if (ret) {
+		dev_err(dev, "failed to enable clk\n");
+		return ret;
+	}
+
+	ret = sensor->ops->power_ctrl(sensor, POWER_ON);
+	if (ret) {
+		dev_err(dev, "failed to power on sensor\n");
+		clk_disable_unprepare(sensor->clk);
+	}
+
+	return ret;
+}
+
+static int st_thermal_sensor_off(struct st_thermal_sensor *sensor)
+{
+	int ret;
+
+	ret = sensor->ops->power_ctrl(sensor, POWER_OFF);
+	if (ret)
+		return ret;
+
+	clk_disable_unprepare(sensor->clk);
+
+	return 0;
+}
+
+static int st_thermal_calibration(struct st_thermal_sensor *sensor)
+{
+	int ret;
+	unsigned int val;
+	struct device *dev = sensor->dev;
+
+	/* Check if sensor calibration data is already written */
+	ret = regmap_field_read(sensor->dcorrect, &val);
+	if (ret) {
+		dev_err(dev, "failed to read calibration data\n");
+		return ret;
+	}
+
+	if (!val) {
+		/*
+		 * Sensor calibration value not set by bootloader,
+		 * default calibration data to be used
+		 */
+		ret = regmap_field_write(sensor->dcorrect,
+					 sensor->cdata->calibration_val);
+		if (ret)
+			dev_err(dev, "failed to set calibration data\n");
+	}
+
+	return ret;
+}
+
+/* Callback to get temperature from HW*/
+static int st_thermal_get_temp(struct thermal_zone_device *th, int *temperature)
+{
+	struct st_thermal_sensor *sensor = thermal_zone_device_priv(th);
+	unsigned int temp;
+	unsigned int overflow;
+	int ret;
+
+	ret = regmap_field_read(sensor->overflow, &overflow);
+	if (ret)
+		return ret;
+	if (overflow)
+		return -EIO;
+
+	ret = regmap_field_read(sensor->temp_data, &temp);
+	if (ret)
+		return ret;
+
+	temp += sensor->cdata->temp_adjust_val;
+	temp = mcelsius(temp);
+
+	*temperature = temp;
+
+	return 0;
+}
+
+static const struct thermal_zone_device_ops st_tz_ops = {
+	.get_temp	= st_thermal_get_temp,
+};
+
+int st_thermal_register(struct platform_device *pdev,
+			const struct of_device_id *st_thermal_of_match)
+{
+	struct st_thermal_sensor *sensor;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	const struct of_device_id *match;
+
+	int ret;
+
+	if (!np) {
+		dev_err(dev, "device tree node not found\n");
+		return -EINVAL;
+	}
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	sensor->dev = dev;
+
+	match = of_match_device(st_thermal_of_match, dev);
+	if (!(match && match->data))
+		return -EINVAL;
+
+	sensor->cdata = match->data;
+	if (!sensor->cdata->ops)
+		return -EINVAL;
+
+	sensor->ops = sensor->cdata->ops;
+
+	ret = (sensor->ops->regmap_init)(sensor);
+	if (ret)
+		return ret;
+
+	ret = st_thermal_alloc_regfields(sensor);
+	if (ret)
+		return ret;
+
+	sensor->clk = devm_clk_get(dev, "thermal");
+	if (IS_ERR(sensor->clk)) {
+		dev_err(dev, "failed to fetch clock\n");
+		return PTR_ERR(sensor->clk);
+	}
+
+	if (sensor->ops->register_enable_irq) {
+		ret = sensor->ops->register_enable_irq(sensor);
+		if (ret)
+			return ret;
+	}
+
+	ret = st_thermal_sensor_on(sensor);
+	if (ret)
+		return ret;
+
+	ret = st_thermal_calibration(sensor);
+	if (ret)
+		goto sensor_off;
+
+	sensor->thermal_dev =
+		devm_thermal_of_zone_register(dev, 0, sensor, &st_tz_ops);
+	if (IS_ERR(sensor->thermal_dev)) {
+		dev_err(dev, "failed to register thermal of zone\n");
+		ret = PTR_ERR(sensor->thermal_dev);
+		goto sensor_off;
+	}
+
+	platform_set_drvdata(pdev, sensor);
+
+	/*
+	 * devm_thermal_of_zone_register() doesn't enable hwmon by default
+	 * Enable it here
+	 */
+	devm_thermal_add_hwmon_sysfs(dev, sensor->thermal_dev);
+
+	return 0;
+
+sensor_off:
+	st_thermal_sensor_off(sensor);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(st_thermal_register);
+
+void st_thermal_unregister(struct platform_device *pdev)
+{
+	struct st_thermal_sensor *sensor = platform_get_drvdata(pdev);
+
+	st_thermal_sensor_off(sensor);
+	thermal_remove_hwmon_sysfs(sensor->thermal_dev);
+	devm_thermal_of_zone_unregister(sensor->dev, sensor->thermal_dev);
+}
+EXPORT_SYMBOL_GPL(st_thermal_unregister);
+
+static int st_thermal_suspend(struct device *dev)
+{
+	struct st_thermal_sensor *sensor = dev_get_drvdata(dev);
+
+	return st_thermal_sensor_off(sensor);
+}
+
+static int st_thermal_resume(struct device *dev)
+{
+	int ret;
+	struct st_thermal_sensor *sensor = dev_get_drvdata(dev);
+
+	ret = st_thermal_sensor_on(sensor);
+	if (ret)
+		return ret;
+
+	ret = st_thermal_calibration(sensor);
+	if (ret)
+		return ret;
+
+	if (sensor->ops->enable_irq) {
+		ret = sensor->ops->enable_irq(sensor);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+DEFINE_SIMPLE_DEV_PM_OPS(st_thermal_pm_ops, st_thermal_suspend, st_thermal_resume);
+EXPORT_SYMBOL_GPL(st_thermal_pm_ops);
+
+MODULE_AUTHOR("STMicroelectronics (R&D) Limited <ajitpal.singh@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics STi SoC Thermal Sensor Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/thermal/st/st_thermal.h b/drivers/thermal/st/st_thermal.h
new file mode 100644
index 000000000..8639d9165
--- /dev/null
+++ b/drivers/thermal/st/st_thermal.h
@@ -0,0 +1,100 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * ST Thermal Sensor Driver for STi series of SoCs
+ * Author: Ajit Pal Singh <ajitpal.singh@st.com>
+ *
+ * Copyright (C) 2003-2014 STMicroelectronics (R&D) Limited
+ */
+
+#ifndef __STI_THERMAL_SYSCFG_H
+#define __STI_THERMAL_SYSCFG_H
+
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/thermal.h>
+
+enum st_thermal_regfield_ids {
+	INT_THRESH_HI = 0, /* Top two regfield IDs are mutually exclusive */
+	TEMP_PWR = 0,
+	DCORRECT,
+	OVERFLOW,
+	DATA,
+	INT_ENABLE,
+
+	MAX_REGFIELDS
+};
+
+/* Thermal sensor power states */
+enum st_thermal_power_state {
+	POWER_OFF = 0,
+	POWER_ON
+};
+
+struct st_thermal_sensor;
+
+/**
+ * Description of private thermal sensor ops.
+ *
+ * @power_ctrl:		Function for powering on/off a sensor. Clock to the
+ *			sensor is also controlled from this function.
+ * @alloc_regfields:	Allocate regmap register fields, specific to a sensor.
+ * @do_memmap_regmap:	Memory map the thermal register space and init regmap
+ *			instance or find regmap instance.
+ * @register_irq:	Register an interrupt handler for a sensor.
+ */
+struct st_thermal_sensor_ops {
+	int (*power_ctrl)(struct st_thermal_sensor *, enum st_thermal_power_state);
+	int (*alloc_regfields)(struct st_thermal_sensor *);
+	int (*regmap_init)(struct st_thermal_sensor *);
+	int (*register_enable_irq)(struct st_thermal_sensor *);
+	int (*enable_irq)(struct st_thermal_sensor *);
+};
+
+/**
+ * Description of thermal driver compatible data.
+ *
+ * @reg_fields:		Pointer to the regfields array for a sensor.
+ * @sys_compat:		Pointer to the syscon node compatible string.
+ * @ops:		Pointer to private thermal ops for a sensor.
+ * @calibration_val:	Default calibration value to be written to the DCORRECT
+ *			register field for a sensor.
+ * @temp_adjust_val:	Value to be added/subtracted from the data read from
+ *			the sensor. If value needs to be added please provide a
+ *			positive value and if it is to be subtracted please
+ *			provide a negative value.
+ * @crit_temp:		The temperature beyond which the SoC should be shutdown
+ *			to prevent damage.
+ */
+struct st_thermal_compat_data {
+	char *sys_compat;
+	const struct reg_field *reg_fields;
+	const struct st_thermal_sensor_ops *ops;
+	unsigned int calibration_val;
+	int temp_adjust_val;
+	int crit_temp;
+};
+
+struct st_thermal_sensor {
+	struct device *dev;
+	struct thermal_zone_device *thermal_dev;
+	const struct st_thermal_sensor_ops *ops;
+	const struct st_thermal_compat_data *cdata;
+	struct clk *clk;
+	struct regmap *regmap;
+	struct regmap_field *pwr;
+	struct regmap_field *dcorrect;
+	struct regmap_field *overflow;
+	struct regmap_field *temp_data;
+	struct regmap_field *int_thresh_hi;
+	struct regmap_field *int_enable;
+	int irq;
+	void __iomem *mmio_base;
+};
+
+extern int st_thermal_register(struct platform_device *pdev,
+			       const struct of_device_id *st_thermal_of_match);
+extern void st_thermal_unregister(struct platform_device *pdev);
+extern const struct dev_pm_ops st_thermal_pm_ops;
+
+#endif /* __STI_RESET_SYSCFG_H */
diff --git a/drivers/thermal/st/st_thermal_memmap.c b/drivers/thermal/st/st_thermal_memmap.c
new file mode 100644
index 000000000..e3dbe4df8
--- /dev/null
+++ b/drivers/thermal/st/st_thermal_memmap.c
@@ -0,0 +1,182 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * ST Thermal Sensor Driver for memory mapped sensors.
+ * Author: Ajit Pal Singh <ajitpal.singh@st.com>
+ *
+ * Copyright (C) 2003-2014 STMicroelectronics (R&D) Limited
+ */
+
+#include <linux/of.h>
+#include <linux/module.h>
+
+#include "st_thermal.h"
+
+#define STIH416_MPE_CONF			0x0
+#define STIH416_MPE_STATUS			0x4
+#define STIH416_MPE_INT_THRESH			0x8
+#define STIH416_MPE_INT_EN			0xC
+
+/* Power control bits for the memory mapped thermal sensor */
+#define THERMAL_PDN				BIT(4)
+#define THERMAL_SRSTN				BIT(10)
+
+static const struct reg_field st_mmap_thermal_regfields[MAX_REGFIELDS] = {
+	/*
+	 * According to the STIH416 MPE temp sensor data sheet -
+	 * the PDN (Power Down Bit) and SRSTN (Soft Reset Bit) need to be
+	 * written simultaneously for powering on and off the temperature
+	 * sensor. regmap_update_bits() will be used to update the register.
+	 */
+	[INT_THRESH_HI]	= REG_FIELD(STIH416_MPE_INT_THRESH,	0,  7),
+	[DCORRECT]	= REG_FIELD(STIH416_MPE_CONF,		5,  9),
+	[OVERFLOW]	= REG_FIELD(STIH416_MPE_STATUS,		9,  9),
+	[DATA]		= REG_FIELD(STIH416_MPE_STATUS,		11, 18),
+	[INT_ENABLE]	= REG_FIELD(STIH416_MPE_INT_EN,		0,  0),
+};
+
+static irqreturn_t st_mmap_thermal_trip_handler(int irq, void *sdata)
+{
+	struct st_thermal_sensor *sensor = sdata;
+
+	thermal_zone_device_update(sensor->thermal_dev,
+				   THERMAL_EVENT_UNSPECIFIED);
+
+	return IRQ_HANDLED;
+}
+
+/* Private ops for the Memory Mapped based thermal sensors */
+static int st_mmap_power_ctrl(struct st_thermal_sensor *sensor,
+			      enum st_thermal_power_state power_state)
+{
+	const unsigned int mask = (THERMAL_PDN | THERMAL_SRSTN);
+	const unsigned int val = power_state ? mask : 0;
+
+	return regmap_update_bits(sensor->regmap, STIH416_MPE_CONF, mask, val);
+}
+
+static int st_mmap_alloc_regfields(struct st_thermal_sensor *sensor)
+{
+	struct device *dev = sensor->dev;
+	struct regmap *regmap = sensor->regmap;
+	const struct reg_field *reg_fields = sensor->cdata->reg_fields;
+
+	sensor->int_thresh_hi = devm_regmap_field_alloc(dev, regmap,
+						reg_fields[INT_THRESH_HI]);
+	sensor->int_enable = devm_regmap_field_alloc(dev, regmap,
+						reg_fields[INT_ENABLE]);
+
+	if (IS_ERR(sensor->int_thresh_hi) || IS_ERR(sensor->int_enable)) {
+		dev_err(dev, "failed to alloc mmap regfields\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int st_mmap_enable_irq(struct st_thermal_sensor *sensor)
+{
+	int ret;
+
+	/* Set upper critical threshold */
+	ret = regmap_field_write(sensor->int_thresh_hi,
+				 sensor->cdata->crit_temp -
+				 sensor->cdata->temp_adjust_val);
+	if (ret)
+		return ret;
+
+	return regmap_field_write(sensor->int_enable, 1);
+}
+
+static int st_mmap_register_enable_irq(struct st_thermal_sensor *sensor)
+{
+	struct device *dev = sensor->dev;
+	struct platform_device *pdev = to_platform_device(dev);
+	int ret;
+
+	sensor->irq = platform_get_irq(pdev, 0);
+	if (sensor->irq < 0)
+		return sensor->irq;
+
+	ret = devm_request_threaded_irq(dev, sensor->irq,
+					NULL, st_mmap_thermal_trip_handler,
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					dev->driver->name, sensor);
+	if (ret)
+		return ret;
+
+	return st_mmap_enable_irq(sensor);
+}
+
+static const struct regmap_config st_416mpe_regmap_config = {
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_stride = 4,
+};
+
+static int st_mmap_regmap_init(struct st_thermal_sensor *sensor)
+{
+	struct device *dev = sensor->dev;
+	struct platform_device *pdev = to_platform_device(dev);
+
+	sensor->mmio_base = devm_platform_get_and_ioremap_resource(pdev, 0, NULL);
+	if (IS_ERR(sensor->mmio_base))
+		return PTR_ERR(sensor->mmio_base);
+
+	sensor->regmap = devm_regmap_init_mmio(dev, sensor->mmio_base,
+				&st_416mpe_regmap_config);
+	if (IS_ERR(sensor->regmap)) {
+		dev_err(dev, "failed to initialise regmap\n");
+		return PTR_ERR(sensor->regmap);
+	}
+
+	return 0;
+}
+
+static const struct st_thermal_sensor_ops st_mmap_sensor_ops = {
+	.power_ctrl		= st_mmap_power_ctrl,
+	.alloc_regfields	= st_mmap_alloc_regfields,
+	.regmap_init		= st_mmap_regmap_init,
+	.register_enable_irq	= st_mmap_register_enable_irq,
+	.enable_irq		= st_mmap_enable_irq,
+};
+
+/* Compatible device data stih407 thermal sensor */
+static const struct st_thermal_compat_data st_407_cdata = {
+	.reg_fields		= st_mmap_thermal_regfields,
+	.ops			= &st_mmap_sensor_ops,
+	.calibration_val	= 16,
+	.temp_adjust_val	= -95,
+	.crit_temp		= 120,
+};
+
+static const struct of_device_id st_mmap_thermal_of_match[] = {
+	{ .compatible = "st,stih407-thermal",     .data = &st_407_cdata },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, st_mmap_thermal_of_match);
+
+static int st_mmap_probe(struct platform_device *pdev)
+{
+	return st_thermal_register(pdev,  st_mmap_thermal_of_match);
+}
+
+static void st_mmap_remove(struct platform_device *pdev)
+{
+	st_thermal_unregister(pdev);
+}
+
+static struct platform_driver st_mmap_thermal_driver = {
+	.driver = {
+		.name	= "st_thermal_mmap",
+		.pm     = pm_sleep_ptr(&st_thermal_pm_ops),
+		.of_match_table = st_mmap_thermal_of_match,
+	},
+	.probe		= st_mmap_probe,
+	.remove		= st_mmap_remove,
+};
+
+module_platform_driver(st_mmap_thermal_driver);
+
+MODULE_AUTHOR("STMicroelectronics (R&D) Limited <ajitpal.singh@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics STi SoC Thermal Sensor Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/thermal/st/stm_thermal.c b/drivers/thermal/st/stm_thermal.c
new file mode 100644
index 000000000..3290da7ab
--- /dev/null
+++ b/drivers/thermal/st/stm_thermal.c
@@ -0,0 +1,589 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) STMicroelectronics 2018 - All Rights Reserved
+ * Author: David Hernandez Sanchez <david.hernandezsanchez@st.com> for
+ * STMicroelectronics.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/thermal.h>
+#include <linux/units.h>
+
+#include "../thermal_hwmon.h"
+
+/* DTS register offsets */
+#define DTS_CFGR1_OFFSET	0x0
+#define DTS_T0VALR1_OFFSET	0x8
+#define DTS_RAMPVALR_OFFSET	0X10
+#define DTS_ITR1_OFFSET		0x14
+#define DTS_DR_OFFSET		0x1C
+#define DTS_SR_OFFSET		0x20
+#define DTS_ITENR_OFFSET	0x24
+#define DTS_ICIFR_OFFSET	0x28
+
+/* DTS_CFGR1 register mask definitions */
+#define HSREF_CLK_DIV_MASK	GENMASK(30, 24)
+#define TS1_SMP_TIME_MASK	GENMASK(19, 16)
+#define TS1_INTRIG_SEL_MASK	GENMASK(11, 8)
+
+/* DTS_T0VALR1 register mask definitions */
+#define TS1_T0_MASK		GENMASK(17, 16)
+#define TS1_FMT0_MASK		GENMASK(15, 0)
+
+/* DTS_RAMPVALR register mask definitions */
+#define TS1_RAMP_COEFF_MASK	GENMASK(15, 0)
+
+/* DTS_ITR1 register mask definitions */
+#define TS1_HITTHD_MASK		GENMASK(31, 16)
+#define TS1_LITTHD_MASK		GENMASK(15, 0)
+
+/* 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_HITTHD_POS		16
+#define TS1_LITTHD_POS		0
+#define HSREF_CLK_DIV_POS	24
+
+/* DTS_CFGR1 bit definitions */
+#define TS1_EN			BIT(0)
+#define TS1_START		BIT(4)
+#define REFCLK_SEL		BIT(20)
+#define REFCLK_LSE		REFCLK_SEL
+#define Q_MEAS_OPT		BIT(21)
+#define CALIBRATION_CONTROL	Q_MEAS_OPT
+
+/* DTS_SR bit definitions */
+#define TS_RDY			BIT(15)
+/* Bit definitions below are common for DTS_SR, DTS_ITENR and DTS_CIFR */
+#define HIGH_THRESHOLD		BIT(2)
+#define LOW_THRESHOLD		BIT(1)
+
+/* Constants */
+#define ADJUST			100
+#define POLL_TIMEOUT		5000
+#define STARTUP_TIME		40
+#define TS1_T0_VAL0		30000  /* 30 celsius */
+#define TS1_T0_VAL1		130000 /* 130 celsius */
+#define NO_HW_TRIG		0
+#define SAMPLING_TIME		15
+
+struct stm_thermal_sensor {
+	struct device *dev;
+	struct thermal_zone_device *th_dev;
+	enum thermal_device_mode mode;
+	struct clk *clk;
+	unsigned int low_temp_enabled;
+	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)
+{
+	u32 value;
+
+	dev_dbg(sensor->dev, "low:%d high:%d\n", sensor->low_temp_enabled,
+		sensor->high_temp_enabled);
+
+	/* Disable IT generation for low and high thresholds */
+	value = readl_relaxed(sensor->base + DTS_ITENR_OFFSET);
+	value &= ~(LOW_THRESHOLD | HIGH_THRESHOLD);
+
+	if (sensor->low_temp_enabled)
+		value |= HIGH_THRESHOLD;
+
+	if (sensor->high_temp_enabled)
+		value |= LOW_THRESHOLD;
+
+	/* Enable interrupts */
+	writel_relaxed(value, sensor->base + DTS_ITENR_OFFSET);
+
+	return 0;
+}
+
+static irqreturn_t stm_thermal_irq_handler(int irq, void *sdata)
+{
+	struct stm_thermal_sensor *sensor = sdata;
+
+	dev_dbg(sensor->dev, "sr:%d\n",
+		readl_relaxed(sensor->base + DTS_SR_OFFSET));
+
+	thermal_zone_device_update(sensor->th_dev, THERMAL_EVENT_UNSPECIFIED);
+
+	stm_enable_irq(sensor);
+
+	/* Acknoledge all DTS irqs */
+	writel_relaxed(ICIFR_MASK, sensor->base + DTS_ICIFR_OFFSET);
+
+	return IRQ_HANDLED;
+}
+
+static int stm_sensor_power_on(struct stm_thermal_sensor *sensor)
+{
+	int ret;
+	u32 value;
+
+	/* Enable sensor */
+	value = readl_relaxed(sensor->base + DTS_CFGR1_OFFSET);
+	value |= TS1_EN;
+	writel_relaxed(value, sensor->base + DTS_CFGR1_OFFSET);
+
+	/*
+	 * The DTS block can be enabled by setting TSx_EN bit in
+	 * DTS_CFGRx register. It requires a startup time of
+	 * 40μs. Use 5 ms as arbitrary timeout.
+	 */
+	ret = readl_poll_timeout(sensor->base + DTS_SR_OFFSET,
+				 value, (value & TS_RDY),
+				 STARTUP_TIME, POLL_TIMEOUT);
+	if (ret)
+		return ret;
+
+	/* Start continuous measuring */
+	value = readl_relaxed(sensor->base +
+			      DTS_CFGR1_OFFSET);
+	value |= TS1_START;
+	writel_relaxed(value, sensor->base +
+		       DTS_CFGR1_OFFSET);
+
+	sensor->mode = THERMAL_DEVICE_ENABLED;
+
+	return 0;
+}
+
+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;
+	writel_relaxed(value, sensor->base + DTS_CFGR1_OFFSET);
+
+	/* Ensure stop is taken into account */
+	usleep_range(STARTUP_TIME, POLL_TIMEOUT);
+
+	/* Disable sensor */
+	value = readl_relaxed(sensor->base + DTS_CFGR1_OFFSET);
+	value &= ~TS1_EN;
+	writel_relaxed(value, sensor->base + DTS_CFGR1_OFFSET);
+
+	/* Ensure disable is taken into account */
+	return readl_poll_timeout(sensor->base + DTS_SR_OFFSET, value,
+				  !(value & TS_RDY),
+				  STARTUP_TIME, POLL_TIMEOUT);
+}
+
+static int stm_thermal_calibration(struct stm_thermal_sensor *sensor)
+{
+	u32 value, clk_freq;
+	u32 prescaler;
+
+	/* Figure out prescaler value for PCLK during calibration */
+	clk_freq = clk_get_rate(sensor->clk);
+	if (!clk_freq)
+		return -EINVAL;
+
+	prescaler = 0;
+	clk_freq /= HZ_PER_MHZ;
+	if (clk_freq) {
+		while (prescaler <= clk_freq)
+			prescaler++;
+	}
+
+	value = readl_relaxed(sensor->base + DTS_CFGR1_OFFSET);
+
+	/* Clear prescaler */
+	value &= ~HSREF_CLK_DIV_MASK;
+
+	/* Set prescaler. pclk_freq/prescaler < 1MHz */
+	value |= (prescaler << HSREF_CLK_DIV_POS);
+
+	/* Select PCLK as reference clock */
+	value &= ~REFCLK_SEL;
+
+	/* Set maximal sampling time for better precision */
+	value |= TS1_SMP_TIME_MASK;
+
+	/* Measure with calibration */
+	value &= ~CALIBRATION_CONTROL;
+
+	/* select trigger */
+	value &= ~TS1_INTRIG_SEL_MASK;
+	value |= NO_HW_TRIG;
+
+	writel_relaxed(value, sensor->base + DTS_CFGR1_OFFSET);
+
+	return 0;
+}
+
+/* Fill in DTS structure with factory sensor values */
+static int stm_thermal_read_factory_settings(struct stm_thermal_sensor *sensor)
+{
+	/* Retrieve engineering calibration temperature */
+	sensor->t0 = readl_relaxed(sensor->base + DTS_T0VALR1_OFFSET) &
+					TS1_T0_MASK;
+	if (!sensor->t0)
+		sensor->t0 = TS1_T0_VAL0;
+	else
+		sensor->t0 = TS1_T0_VAL1;
+
+	/* Retrieve fmt0 and put it on Hz */
+	sensor->fmt0 = ADJUST * (readl_relaxed(sensor->base +
+				 DTS_T0VALR1_OFFSET) & TS1_FMT0_MASK);
+
+	/* Retrieve ramp coefficient */
+	sensor->ramp_coeff = readl_relaxed(sensor->base + DTS_RAMPVALR_OFFSET) &
+					   TS1_RAMP_COEFF_MASK;
+
+	if (!sensor->fmt0 || !sensor->ramp_coeff) {
+		dev_err(sensor->dev, "%s: wrong setting\n", __func__);
+		return -EINVAL;
+	}
+
+	dev_dbg(sensor->dev, "%s: T0 = %doC, FMT0 = %dHz, RAMP_COEFF = %dHz/oC",
+		__func__, sensor->t0, sensor->fmt0, sensor->ramp_coeff);
+
+	return 0;
+}
+
+static int stm_thermal_calculate_threshold(struct stm_thermal_sensor *sensor,
+					   int temp, u32 *th)
+{
+	int freqM;
+
+	/* Figure out the CLK_PTAT frequency for a given temperature */
+	freqM = ((temp - sensor->t0) * sensor->ramp_coeff) / 1000 +
+		sensor->fmt0;
+
+	/* Figure out the threshold sample number */
+	*th = clk_get_rate(sensor->clk) * SAMPLING_TIME / freqM;
+	if (!*th)
+		return -EINVAL;
+
+	dev_dbg(sensor->dev, "freqM=%d Hz, threshold=0x%x", freqM, *th);
+
+	return 0;
+}
+
+/* Disable temperature interrupt */
+static int stm_disable_irq(struct stm_thermal_sensor *sensor)
+{
+	u32 value;
+
+	/* Disable IT generation */
+	value = readl_relaxed(sensor->base + DTS_ITENR_OFFSET);
+	value &= ~ITENR_MASK;
+	writel_relaxed(value, sensor->base + DTS_ITENR_OFFSET);
+
+	return 0;
+}
+
+static int stm_thermal_set_trips(struct thermal_zone_device *tz, int low, int high)
+{
+	struct stm_thermal_sensor *sensor = thermal_zone_device_priv(tz);
+	u32 itr1, th;
+	int ret;
+
+	dev_dbg(sensor->dev, "set trips %d <--> %d\n", low, high);
+
+	/* Erase threshold content */
+	itr1 = readl_relaxed(sensor->base + DTS_ITR1_OFFSET);
+	itr1 &= ~(TS1_LITTHD_MASK | TS1_HITTHD_MASK);
+
+	/*
+	 * Disable low-temp if "low" is too small. As per thermal framework
+	 * API, we use -INT_MAX rather than INT_MIN.
+	 */
+
+	if (low > -INT_MAX) {
+		sensor->low_temp_enabled = 1;
+		/* add 0.5 of hysteresis due to measurement error */
+		ret = stm_thermal_calculate_threshold(sensor, low - 500, &th);
+		if (ret)
+			return ret;
+
+		itr1 |= (TS1_HITTHD_MASK  & (th << TS1_HITTHD_POS));
+	} else {
+		sensor->low_temp_enabled = 0;
+	}
+
+	/* 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;
+
+		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;
+}
+
+/* Callback to get temperature from HW */
+static int stm_thermal_get_temp(struct thermal_zone_device *tz, int *temp)
+{
+	struct stm_thermal_sensor *sensor = thermal_zone_device_priv(tz);
+	u32 periods;
+	int freqM, ret;
+
+	if (sensor->mode != THERMAL_DEVICE_ENABLED)
+		return -EAGAIN;
+
+	/* Retrieve the number of periods sampled */
+	ret = readl_relaxed_poll_timeout(sensor->base + DTS_DR_OFFSET, periods,
+					 (periods & TS1_MFREQ_MASK),
+					 STARTUP_TIME, POLL_TIMEOUT);
+	if (ret)
+		return ret;
+
+	/* Figure out the CLK_PTAT frequency */
+	freqM = (clk_get_rate(sensor->clk) * SAMPLING_TIME) / periods;
+	if (!freqM)
+		return -EINVAL;
+
+	/* Figure out the temperature in mili celsius */
+	*temp = (freqM - sensor->fmt0) * 1000 / sensor->ramp_coeff + sensor->t0;
+
+	return 0;
+}
+
+/* Registers DTS irq to be visible by GIC */
+static int stm_register_irq(struct stm_thermal_sensor *sensor)
+{
+	struct device *dev = sensor->dev;
+	struct platform_device *pdev = to_platform_device(dev);
+	int ret;
+
+	sensor->irq = platform_get_irq(pdev, 0);
+	if (sensor->irq < 0)
+		return sensor->irq;
+
+	ret = devm_request_threaded_irq(dev, sensor->irq,
+					NULL,
+					stm_thermal_irq_handler,
+					IRQF_ONESHOT,
+					dev->driver->name, sensor);
+	if (ret)
+		return ret;
+
+	dev_dbg(dev, "%s: thermal IRQ registered", __func__);
+
+	return 0;
+}
+
+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;
+
+	clk_disable_unprepare(sensor->clk);
+
+	return 0;
+}
+
+static int stm_thermal_prepare(struct stm_thermal_sensor *sensor)
+{
+	int ret;
+
+	ret = clk_prepare_enable(sensor->clk);
+	if (ret)
+		return ret;
+
+	ret = stm_thermal_read_factory_settings(sensor);
+	if (ret)
+		goto thermal_unprepare;
+
+	ret = stm_thermal_calibration(sensor);
+	if (ret)
+		goto thermal_unprepare;
+
+	return 0;
+
+thermal_unprepare:
+	clk_disable_unprepare(sensor->clk);
+
+	return ret;
+}
+
+static int stm_thermal_suspend(struct device *dev)
+{
+	struct stm_thermal_sensor *sensor = dev_get_drvdata(dev);
+
+	return stm_thermal_sensor_off(sensor);
+}
+
+static int stm_thermal_resume(struct device *dev)
+{
+	int ret;
+	struct stm_thermal_sensor *sensor = dev_get_drvdata(dev);
+
+	ret = stm_thermal_prepare(sensor);
+	if (ret)
+		return ret;
+
+	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;
+}
+
+static DEFINE_SIMPLE_DEV_PM_OPS(stm_thermal_pm_ops,
+				stm_thermal_suspend, stm_thermal_resume);
+
+static const struct thermal_zone_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[] = {
+		{ .compatible = "st,stm32-thermal"},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, stm_thermal_of_match);
+
+static int stm_thermal_probe(struct platform_device *pdev)
+{
+	struct stm_thermal_sensor *sensor;
+	void __iomem *base;
+	int ret;
+
+	if (!pdev->dev.of_node) {
+		dev_err(&pdev->dev, "%s: device tree node not found\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	sensor = devm_kzalloc(&pdev->dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, sensor);
+
+	sensor->dev = &pdev->dev;
+
+	base = devm_platform_get_and_ioremap_resource(pdev, 0, NULL);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	/* Populate sensor */
+	sensor->base = base;
+
+	sensor->clk = devm_clk_get(&pdev->dev, "pclk");
+	if (IS_ERR(sensor->clk)) {
+		dev_err(&pdev->dev, "%s: failed to fetch PCLK clock\n",
+			__func__);
+		return PTR_ERR(sensor->clk);
+	}
+
+	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 prepare 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_of_zone_register(&pdev->dev, 0,
+						       sensor,
+						       &stm_tz_ops);
+
+	if (IS_ERR(sensor->th_dev)) {
+		dev_err(&pdev->dev, "%s: thermal zone sensor registering KO\n",
+			__func__);
+		ret = PTR_ERR(sensor->th_dev);
+		return ret;
+	}
+
+	/* Register IRQ into GIC */
+	ret = stm_register_irq(sensor);
+	if (ret)
+		goto err_tz;
+
+	stm_enable_irq(sensor);
+
+	/*
+	 * Thermal_zone doesn't enable hwmon as default,
+	 * enable it here
+	 */
+	ret = thermal_add_hwmon_sysfs(sensor->th_dev);
+	if (ret)
+		goto err_tz;
+
+	dev_info(&pdev->dev, "%s: Driver initialized successfully\n",
+		 __func__);
+
+	return 0;
+
+err_tz:
+	return ret;
+}
+
+static void stm_thermal_remove(struct platform_device *pdev)
+{
+	struct stm_thermal_sensor *sensor = platform_get_drvdata(pdev);
+
+	stm_thermal_sensor_off(sensor);
+	thermal_remove_hwmon_sysfs(sensor->th_dev);
+}
+
+static struct platform_driver stm_thermal_driver = {
+	.driver = {
+		.name	= "stm_thermal",
+		.pm     = pm_sleep_ptr(&stm_thermal_pm_ops),
+		.of_match_table = stm_thermal_of_match,
+	},
+	.probe		= stm_thermal_probe,
+	.remove		= stm_thermal_remove,
+};
+module_platform_driver(stm_thermal_driver);
+
+MODULE_DESCRIPTION("STMicroelectronics STM32 Thermal Sensor Driver");
+MODULE_AUTHOR("David Hernandez Sanchez <david.hernandezsanchez@st.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:stm_thermal");