dea4d46a0c038bc7ea1735705115e5fc0d852e914abf1f20badfb555.patch

From 08e510116c93f6bd8e00462005785bf3329d5dfc Mon Sep 17 00:00:00 2001
From: Louis Yu <louis.yu@mediatek.com>
Date: Tue, 2 Jul 2019 17:16:25 +0800
Subject: [PATCH] FROMLIST: thermal: mediatek: add suspend/resume callback

Add suspend/resume callback to disable/enable Mediatek thermal sensor
respectively. Since thermal power domain is off in suspend, thermal driver
needs re-initialization during resume.

Signed-off-by: Louis Yu <louis.yu@mediatek.com>
Signed-off-by: Michael Kao <michael.kao@mediatek.com>

(am from https://patchwork.kernel.org/patch/11027491/)

BUG=b:131870692
TEST=boot to shell

Change-Id: I43f9af5c2d2b682f985eb570d6ed984243772e91
Signed-off-by: Eddie Huang <eddie.huang@mediatek.com>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/third_party/kernel/+/1628417
Tested-by: Nicolas Boichat <drinkcat@chromium.org>
Reviewed-by: Nicolas Boichat <drinkcat@chromium.org>
Commit-Queue: Nicolas Boichat <drinkcat@chromium.org>
[rebase510(groeck): Context conflicts]
Signed-off-by: Guenter Roeck <groeck@chromium.org>
Kcr-patch: 2ff11a92ef99ee3ad8128e7130885d57fb1854cb3d64c14b5f74934f.patch
---
 drivers/thermal/mediatek/auxadc_thermal.c | 1003 ++++++++++++++++++++-
 1 file changed, 988 insertions(+), 15 deletions(-)

diff --git a/drivers/thermal/mediatek/auxadc_thermal.c b/drivers/thermal/mediatek/auxadc_thermal.c
index 470b87b538ae2a33122746cadb34eb7486787795..df62df069dfd6244b262b8f05c65afc9f326483d 100644
--- a/drivers/thermal/mediatek/auxadc_thermal.c
+++ b/drivers/thermal/mediatek/auxadc_thermal.c
@@ -8,6 +8,9 @@
  */
 
 #include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/cpu.h>
+#include <linux/cpufreq.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
@@ -16,11 +19,16 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/platform_device.h>
+#include <linux/pm_opp.h>
+#include <linux/pm_qos.h>
+#include <linux/regulator/consumer.h>
 #include <linux/slab.h>
 #include <linux/io.h>
 #include <linux/thermal.h>
 #include <linux/reset.h>
 #include <linux/types.h>
+#include <linux/workqueue.h>
+#include <linux/iopoll.h>
 
 #include "../thermal_hwmon.h"
 
@@ -33,6 +41,8 @@
 #define APMIXED_SYS_TS_CON0	0x600
 #define APMIXED_SYS_TS_CON1	0x604
 
+#define APMIXED_SYS_TS_CON1_BUFFER_OFF 0x30
+
 /* Thermal Controller Registers */
 #define TEMP_MONCTL0		0x000
 #define TEMP_MONCTL1		0x004
@@ -75,7 +85,32 @@
 #define TEMP_ADCPNP3_1          0x1b4
 #define TEMP_MSR3_1             0x1B8
 
+#define SVS_BANK_CONFIG0	0x200
+#define SVS_BANK_CONFIG1	0x204
+#define SVS_BANK_CONFIG2	0x208
+#define SVS_BANK_CONFIG3	0x20c
+#define SVS_BANK_CONFIG4	0x210
+#define SVS_BANK_CONFIG5	0x214
+#define SVS_BANK_FREQPCT30	0x218
+#define SVS_BANK_FREQPCT74	0x21c
+#define SVS_BANK_LIMITVALS	0x220
+#define SVS_BANK_CONFIG6	0x224
+#define SVS_BANK_CONFIG7	0x228
+#define SVS_BANK_CONFIG8	0x22c
+#define SVS_BANK_CONFIG9	0x230
+#define SVS_BANK_CONFIG10	0x234
+#define SVS_BANK_EN		0x238
+#define SVS_BANK_CONTROL0	0x23c
+#define SVS_BANK_CONTROL1	0x240
+#define SVS_BANK_CONTROL2	0x244
+#define SVS_BANK_VOP30		0x248
+#define SVS_BANK_VOP74		0x24c
+#define SVS_BANK_INTST		0x254
+#define SVS_BANK_CONTROL3	0x25c
+#define SVS_BANK_CONTROL4	0x264
+
 #define PTPCORESEL		0x400
+#define SVS_SVSINTST		0x408
 
 #define TEMP_MONCTL1_PERIOD_UNIT(x)	((x) & 0x3ff)
 
@@ -90,6 +125,9 @@
 #define TEMP_ADCVALIDMASK_VALID_HIGH		BIT(5)
 #define TEMP_ADCVALIDMASK_VALID_POS(bit)	(bit)
 
+#define TEMP_MSRCTL1_BUS_STA   (BIT(0) | BIT(7))
+#define TEMP_MSRCTL1_SENSING_POINTS_PAUSE      0x10E
+
 /* MT8173 thermal sensors */
 #define MT8173_TS1	0
 #define MT8173_TS2	1
@@ -115,6 +153,30 @@
 /* The calibration coefficient of sensor  */
 #define MT8173_CALIBRATION	165
 
+/* The number of OPPs supported by SVS */
+#define MT8173_NUM_SVS_OPP		8
+
+/* Bit masks of SVS enable and IRQ configrations */
+#define PHASE_0_EN		BIT(0)
+#define PHASE_CON_EN		BIT(1)
+#define PHASE_1_EN		BIT(2)
+#define PHASE_EN_MASK		(PHASE_0_EN | PHASE_CON_EN | PHASE_1_EN)
+#define PHASE_01_EN		(PHASE_0_EN | PHASE_1_EN)
+#define PHASE_01_IRQ		BIT(0)
+#define PHASE_CON_IRQ		(0xff << 16)
+
+/* Bit mask of SVS bank flags*/
+#define SVS_NEED_OVERFLOW_FIX	BIT(0)
+
+/* SVS bank status */
+#define SVS_STATUS_ERROR	BIT(0)
+
+/* The number of SVS banks implemented */
+#define MT8173_NUM_SVS_BANKS	2
+
+#define MT8173_SVS_BANK_CA53	0
+#define MT8173_SVS_BANK_CA72	1
+
 /* Valid temperatures range */
 #define MT8173_TEMP_MIN		-20000
 #define MT8173_TEMP_MAX		150000
@@ -168,6 +230,48 @@
 #define CALIB_BUF1_O_SLOPE_SIGN_V3(x)	(((x) >> 19) & 0x1)
 #define CALIB_BUF1_ID_V3(x)		(((x) >> 20) & 0x1)
 
+/* SVS configuration register constants */
+#define SVS_LIMITVALS_CONST	0x1fe
+#define SVS_CONFIG1_CONST	0x100006
+#define SVS_CONFIG4_CONST	0x555555
+#define SVS_CONFIG5_CONST	0x555555
+#define SVS_CONFIG7_CONST	0xa28
+#define SVS_CONFIG8_CONST	0xffff
+#define SVS_CONFIG10_CONST	0x80000000
+#define SVS_CONTROL3_P01	0x5f01
+#define SVS_CONTROL3_CON	0xff0000
+
+#define SVS_CONFIG9_VAL(b, m)	((((b) & 0xfff) << 12) | ((m) & 0xfff))
+#define SVS_CONTROL4_OVFIX(v)	(((v) & ~0xf) | 0x7)
+
+#define SVS_LOW_TEMP		33000
+#define SVS_LOW_TEMP_OFFSET	10
+
+/* Constants for calibration data calculation */
+#define GE_ZERO_BASE	512		/* 0 of 10-bit sign integer */
+#define SLOPE_OFFSET	165		/* 0.00165 * 100000  */
+#define TS_GAIN		18		/* 1.8 * 10 */
+#define ADC_FS		15		/* 1.5 * 10 */
+#define TEMP_OFFSET	(25 * 10)
+#define VTS_OFFSET	3350
+#define ADC_RESOLUTION	(1 << 12)	/* 12-bit ADC full code */
+#define BTS_PRESCALE	4
+
+/* Helpers to calculate configuration values from SVS calibration data */
+#define SVS_CALIB_VALID	BIT(0)
+#define BANK_SHIFT(bank) (((bank) == 0) ? 8 : 0)
+#define SVS_CALIB_BANK_CONFIG0(buf, b)				\
+	(((((buf[33] >> BANK_SHIFT(b)) & 0xff)) << 8) |		\
+	((buf[32] >> BANK_SHIFT(b)) & 0xff))
+#define SVS_CALIB_BANK_CONFIG1(buf, b)				\
+	((((buf[34] >> BANK_SHIFT(b)) & 0xff) << 8) | SVS_CONFIG1_CONST)
+#define SVS_CALIB_BANK_CONFIG2L(base, b)			\
+	((buf[0] >> BANK_SHIFT(b)) & 0xff)
+#define SVS_CALIB_BANK_CONFIG2H(base, b)			\
+	((buf[1] >> BANK_SHIFT(b)) & 0xff)
+#define SVS_CALIB_BANK_CONFIG3(base, b)				\
+	(((buf[2] >> BANK_SHIFT(b)) & 0xff) << 8)
+
 enum {
 	VTS1,
 	VTS2,
@@ -330,17 +434,73 @@ struct mtk_thermal_data {
 	u32 apmixed_buffer_ctl_reg;
 	u32 apmixed_buffer_ctl_mask;
 	u32 apmixed_buffer_ctl_set;
+	bool use_svs;
+};
+
+enum mtk_svs_state {
+	SVS_INIT,
+	SVS_PHASE_0,
+	SVS_PHASE_1,
+	SVS_PHASE_CONTINUOUS,
+};
+
+struct mtk_svs_bank {
+	int bank_id;
+	int cpu_dev_id;
+
+	u32 flags;
+
+	u32 status;
+
+	enum mtk_svs_state state;
+	/* Use this to limit bank frequency */
+	unsigned long max_freq_khz;
+	unsigned long min_freq_khz;
+
+	struct mtk_thermal *mt;
+	struct completion init_done;
+	struct work_struct work;
+
+	struct device *dev;
+	struct regulator *reg;
+
+	/* SVS per-bank calibration values */
+	u32 ctrl0;
+	u32 config0;
+	u32 config1;
+	u32 config2;
+	u32 config3;
+
+	unsigned long freq_table[MT8173_NUM_SVS_OPP];	/* in KHz*/
+	int volt_table[MT8173_NUM_SVS_OPP];		/* in uVolt */
+	int updated_volt_table[MT8173_NUM_SVS_OPP];	/* in uVolt */
+};
+
+struct mtk_svs_bank_cfg {
+	int ts;
+	int vmin_uV;
+	int vmax_uV;
+	int vboot_uV;
+	unsigned long base_freq_hz;
 };
 
 struct mtk_thermal {
 	struct device *dev;
 	void __iomem *thermal_base;
+        void __iomem *apmixed_base;
+        void __iomem *auxadc_base;
+        u64 apmixed_phys_base;
+        u64 auxadc_phys_base;
 
 	struct clk *clk_peri_therm;
 	struct clk *clk_auxadc;
+	struct clk *svs_mux;
+	struct clk *svs_pll;
 	/* lock: for getting and putting banks */
 	struct mutex lock;
 
+	int svs_irq;
+
 	/* Calibration values */
 	s32 adc_ge;
 	s32 adc_oe;
@@ -349,6 +509,13 @@ struct mtk_thermal {
 	s32 o_slope_sign;
 	s32 vts[MAX_NUM_VTS];
 
+	/*
+	 * MTS and BTS are factors used by SVS to get per-bank temperature:
+	 * Bank Temperature = [ADC Value] * MTS + BTS[Bank]
+	 */
+	s32 bts[MT8173_NUM_ZONES];
+	s32 mts;
+
 	const struct mtk_thermal_data *conf;
 	struct mtk_thermal_bank banks[MAX_NUM_ZONES];
 
@@ -399,6 +566,25 @@ static const int mt8173_vts_index[MT8173_NUM_SENSORS] = {
 	VTS1, VTS2, VTS3, VTS4, VTSABB
 };
 
+static const struct mtk_svs_bank_cfg svs_bank_cfgs[MT8173_NUM_SVS_BANKS] = {
+	[MT8173_SVS_BANK_CA53] = {
+		.vmax_uV = 1125000,
+		.vmin_uV = 800000,
+		.vboot_uV = 1000000,
+		.base_freq_hz = 1600000000,
+		.ts = MT8173_TS3
+	},
+	[MT8173_SVS_BANK_CA72] = {
+		.vmax_uV = 1125000,
+		.vmin_uV = 800000,
+		.vboot_uV = 1000000,
+		.base_freq_hz = 2000000000,
+		.ts = MT8173_TS4
+	}
+};
+
+static struct mtk_svs_bank svs_banks[MT8173_NUM_SVS_BANKS] = {{0}};
+
 /* MT2701 thermal sensor data */
 static const int mt2701_bank_data[MT2701_NUM_SENSORS] = {
 	MT2701_TS1, MT2701_TS2, MT2701_TSABB
@@ -514,6 +700,7 @@ static const struct mtk_thermal_data mt8173_thermal_data = {
 	.adcpnp = mt8173_adcpnp,
 	.sensor_mux_values = mt8173_mux_values,
 	.version = MTK_THERMAL_V1,
+	.use_svs = true,
 };
 
 /*
@@ -756,6 +943,34 @@ static int raw_to_mcelsius_v2(struct mtk_thermal *mt, int sensno, s32 raw)
 	return (format_2 - tmp) * 100;
 }
 
+/**
+ * uvolt_to_config - convert a voltage value to SVS voltage config value
+ * @uvolt:	voltage value
+ */
+static inline u8 uvolt_to_config(int uvolt)
+{
+	return ((uvolt / 1000 - 700) * 100 + 625 - 1) / 625;
+}
+
+/**
+ * config_to_uvolt - convert a SVS voltage config value to voltage value
+ * @val:	SVS voltage config value
+ */
+static inline int config_to_uvolt(u32 val)
+{
+	return ((val * 625 / 100) + 700) * 1000;
+}
+
+/**
+ * hz_to_config - convert a frequency value to SVS frequency config value
+ * @rate:	frequency value
+ * @base_rate:	rate to be used to calculate frequency percentage
+ */
+static inline u8 hz_to_config(unsigned long rate, unsigned long base_rate)
+{
+	return (rate * 100 + base_rate - 1) / base_rate;
+}
+
 static int raw_to_mcelsius_v3(struct mtk_thermal *mt, int sensno, s32 raw)
 {
 	s32 tmp;
@@ -849,7 +1064,8 @@ static int mtk_thermal_bank_temperature(struct mtk_thermal_bank *bank)
 
 static int mtk_read_temp(struct thermal_zone_device *tz, int *temperature)
 {
-	struct mtk_thermal *mt = thermal_zone_device_priv(tz);
+	struct mtk_thermal_zone *mtz = tz->devdata;
+	struct mtk_thermal *mt = mtz->mt;
 	int i;
 	int tempmax = INT_MIN;
 
@@ -870,7 +1086,8 @@ static int mtk_read_temp(struct thermal_zone_device *tz, int *temperature)
 
 static int mtk_read_sensor_temp(struct thermal_zone_device *tz, int *temperature)
 {
-	struct mtk_thermal *mt = tz->devdata;
+	struct mtk_thermal_zone *mtz = tz->devdata;
+	struct mtk_thermal *mt = mtz->mt;
 	const struct mtk_thermal_data *conf = mt->conf;
 	int id = tz->id - 1;
 	int temp = INT_MIN;
@@ -1013,6 +1230,42 @@ static void mtk_thermal_init_bank(struct mtk_thermal *mt, int num,
 	mtk_thermal_put_bank(bank);
 }
 
+static int mtk_thermal_disable_sensing(struct mtk_thermal *mt, int num)
+{
+       struct mtk_thermal_bank *bank = &mt->banks[num];
+       u32 val;
+       unsigned long timeout;
+       void __iomem *addr;
+       int ret = 0;
+
+       bank->id = num;
+       bank->mt = mt;
+
+       mtk_thermal_get_bank(bank);
+
+       val = readl(mt->thermal_base + TEMP_MSRCTL1);
+       /* pause periodic temperature measurement for sensing points */
+       writel(val | TEMP_MSRCTL1_SENSING_POINTS_PAUSE,
+              mt->thermal_base + TEMP_MSRCTL1);
+
+       /* wait until temperature measurement bus idle */
+       timeout = jiffies + HZ;
+       addr = mt->thermal_base + TEMP_MSRCTL1;
+
+       ret = readl_poll_timeout(addr, val, (val & TEMP_MSRCTL1_BUS_STA) == 0x0,
+                                0, timeout);
+       if (ret < 0)
+               goto out;
+
+       /* disable periodic temperature meausrement on sensing points */
+       writel(0x0, mt->thermal_base + TEMP_MONCTL0);
+
+out:
+       mtk_thermal_put_bank(bank);
+
+       return ret;
+}
+
 static u64 of_get_phys_base(struct device_node *np)
 {
 	struct resource res;
@@ -1166,6 +1419,654 @@ static int mtk_thermal_get_calibration_data(struct device *dev,
 	return ret;
 }
 
+/* This should only be run after mtk_thermal_get_calibration_data */
+static void mtk_thermal_get_calibration_data_for_svs(struct device *dev,
+						     struct mtk_thermal *mt)
+{
+	int i;
+	s32 ge, oe, gain, x_roomt, ts_intercept, ts_degc, ts_factor;
+
+	/*
+	 * The constants 10, 10000, 100000 below are pre-scalers to avoid
+	 * calculation underflow, and will be divided in the final results.
+	 */
+	oe = mt->adc_ge - GE_ZERO_BASE;
+	ge = oe * 10000 / ADC_RESOLUTION;
+	gain = 10000 + ge;
+
+	/* calculating MTS */
+	mt->mts = 100000 * 10000 / gain * ADC_FS / TS_GAIN / mt->o_slope;
+
+	ts_degc = mt->degc_cali * 10 / 2;
+	ts_factor = 100000 * 10000 / ADC_RESOLUTION / gain * ge;
+
+	/* calculating per-bank BTS */
+	for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
+		int ts = svs_bank_cfgs[i].ts;
+
+		x_roomt = mt->vts[ts] + VTS_OFFSET - oe * 10000 /
+			ADC_RESOLUTION * 10000 / gain;
+		ts_intercept = (ts_factor + x_roomt * 10 * ADC_FS / TS_GAIN) *
+			10 / mt->o_slope;
+		ts_intercept += ts_degc - TEMP_OFFSET;
+
+		mt->bts[i] = ts_intercept * BTS_PRESCALE / 10;
+	}
+}
+
+static int mtk_svs_get_calibration_data(struct device *dev,
+					struct mtk_thermal *mt)
+{
+	struct nvmem_cell *cell;
+	u32 *buf;
+	size_t len;
+	int i, ret = 0;
+
+	mtk_thermal_get_calibration_data_for_svs(dev, mt);
+
+	cell = nvmem_cell_get(dev, "svs-calibration-data");
+	if (IS_ERR(cell))
+		return PTR_ERR(cell);
+
+	buf = nvmem_cell_read(cell, &len);
+	nvmem_cell_put(cell);
+
+	if (IS_ERR(buf)) {
+		dev_err(dev, "failed to get svs calibration data: %ld\n",
+			PTR_ERR(buf));
+		return PTR_ERR(buf);
+	}
+
+	if (len < 0x8c || !(buf[29] & SVS_CALIB_VALID)) {
+		dev_err(dev, "Invalid SVS calibration data\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
+		u32 temp;
+
+		svs_banks[i].config0 =
+				SVS_CALIB_BANK_CONFIG0(buf, i);
+		svs_banks[i].config1 =
+				SVS_CALIB_BANK_CONFIG1(buf, i);
+		svs_banks[i].config3 =
+				SVS_CALIB_BANK_CONFIG3(buf, i);
+
+		temp = SVS_CALIB_BANK_CONFIG2H(buf, i);
+		if (temp < 128 && i == MT8173_SVS_BANK_CA72) {
+			temp = (unsigned char)((temp - 256) / 2);
+			svs_banks[i].flags |= SVS_NEED_OVERFLOW_FIX;
+		}
+		temp = ((temp & 0xff) << 8) |
+		       SVS_CALIB_BANK_CONFIG2L(buf, i);
+		svs_banks[i].config2 = temp;
+	}
+
+out:
+	kfree(buf);
+
+	return ret;
+}
+
+/* Caller must call this function with mt->lock held */
+static void mtk_svs_set_phase(struct mtk_svs_bank *svs, int phase)
+{
+	struct mtk_thermal *mt = svs->mt;
+	unsigned long *freq_tbl, base_freq_hz;
+	int id = svs->bank_id;
+
+	freq_tbl = svs->freq_table;
+	base_freq_hz = svs_bank_cfgs[id].base_freq_hz;
+
+	writel(svs->config0, mt->thermal_base + SVS_BANK_CONFIG0);
+	writel(svs->config1, mt->thermal_base + SVS_BANK_CONFIG1);
+	writel(svs->config2, mt->thermal_base + SVS_BANK_CONFIG2);
+	writel(svs->config3, mt->thermal_base + SVS_BANK_CONFIG3);
+	writel(SVS_CONFIG4_CONST, mt->thermal_base + SVS_BANK_CONFIG4);
+	writel(SVS_CONFIG5_CONST, mt->thermal_base + SVS_BANK_CONFIG5);
+	writel(SVS_CONFIG10_CONST, mt->thermal_base + SVS_BANK_CONFIG10);
+
+	/*
+	 * SVS_BANK_FREQPCT30 and SVS_BANK_FREQPCT74 inform SVS the frequencies
+	 * of OPP table. The frequency values are set in the form:
+	 * frequency = (config / 100) * [base frequency of this bank]
+	 */
+	writel(hz_to_config(freq_tbl[0], base_freq_hz) |
+	       (hz_to_config(freq_tbl[1], base_freq_hz) << 8) |
+	       (hz_to_config(freq_tbl[2], base_freq_hz) << 16) |
+	       (hz_to_config(freq_tbl[3], base_freq_hz) << 24),
+	       mt->thermal_base + SVS_BANK_FREQPCT30);
+
+	writel(hz_to_config(freq_tbl[4], base_freq_hz) |
+	       (hz_to_config(freq_tbl[5], base_freq_hz) << 8) |
+	       (hz_to_config(freq_tbl[6], base_freq_hz) << 16) |
+	       (hz_to_config(freq_tbl[7], base_freq_hz) << 24),
+	       mt->thermal_base + SVS_BANK_FREQPCT74);
+
+	writel((uvolt_to_config(svs_bank_cfgs[id].vmax_uV) << 24) |
+	       (uvolt_to_config(svs_bank_cfgs[id].vmin_uV) << 16) |
+	       SVS_LIMITVALS_CONST, mt->thermal_base + SVS_BANK_LIMITVALS);
+
+	writel(uvolt_to_config(svs_bank_cfgs[id].vboot_uV),
+	       mt->thermal_base + SVS_BANK_CONFIG6);
+	writel(SVS_CONFIG7_CONST, mt->thermal_base + SVS_BANK_CONFIG7);
+	writel(SVS_CONFIG8_CONST, mt->thermal_base + SVS_BANK_CONFIG8);
+
+	/* clear all pending interrupt */
+	writel(0xffffffff, mt->thermal_base + SVS_BANK_INTST);
+
+	/* Workaround for calibration data overflow on CA72 bank */
+	if (svs->flags & SVS_NEED_OVERFLOW_FIX) {
+		u32 reg;
+
+		reg = readl(mt->thermal_base + SVS_BANK_CONTROL4);
+		writel(SVS_CONTROL4_OVFIX(reg),
+		       mt->thermal_base + MT8173_SVS_BANK_CA72);
+	}
+
+	switch (phase) {
+	case SVS_PHASE_0:
+		writel(SVS_CONTROL3_P01, mt->thermal_base + SVS_BANK_CONTROL3);
+		writel(PHASE_0_EN, mt->thermal_base + SVS_BANK_EN);
+		svs->state = SVS_PHASE_0;
+		break;
+	case SVS_PHASE_1:
+		writel(SVS_CONTROL3_P01, mt->thermal_base + SVS_BANK_CONTROL3);
+		writel(svs->ctrl0, mt->thermal_base + SVS_BANK_CONTROL0);
+		writel(PHASE_0_EN | PHASE_1_EN,
+		       mt->thermal_base + SVS_BANK_EN);
+		svs->state = SVS_PHASE_1;
+		break;
+	case SVS_PHASE_CONTINUOUS:
+		writel(SVS_CONFIG9_VAL(mt->bts[id], mt->mts),
+		       mt->thermal_base + SVS_BANK_CONFIG9);
+		writel(SVS_CONTROL3_CON, mt->thermal_base + SVS_BANK_CONTROL3);
+		writel(PHASE_CON_EN, mt->thermal_base + SVS_BANK_EN);
+		svs->state = SVS_PHASE_CONTINUOUS;
+	}
+}
+
+static void mtk_svs_adjust_voltage(struct mtk_svs_bank *svs)
+{
+	int i;
+
+	for (i = 0; i < MT8173_NUM_SVS_OPP; i++) {
+		if (!svs->freq_table[i])
+			continue;
+
+		dev_pm_opp_adjust_voltage(svs->dev, svs->freq_table[i],
+					  svs->updated_volt_table[i],
+					  svs_bank_cfgs[svs->bank_id].vmin_uV,
+					  svs_bank_cfgs[svs->bank_id].vmax_uV);
+	}
+}
+
+/**
+ * mtk_svs_update_voltage_table - update the calculated voltage table
+ * @svs: The SVS bank
+ *
+ * Read the calculated voltage values from registers and update the SVS bank
+ * voltage table which will be write to OPP table entries later. Caller should
+ * select the bank and hold mt->lock before calling it.
+ */
+static void mtk_svs_update_voltage_table(struct mtk_svs_bank *svs)
+{
+	struct mtk_thermal *mt = svs->mt;
+	int vmin_uV, vmax_uV, *volt_table;
+	u32 reg;
+	int temp, offset = 0;
+
+	temp = mtk_thermal_bank_temperature(&mt->banks[svs->bank_id]);
+	if (temp <= SVS_LOW_TEMP)
+		offset = SVS_LOW_TEMP_OFFSET;
+
+	vmin_uV = svs_bank_cfgs[svs->bank_id].vmin_uV;
+	vmax_uV = svs_bank_cfgs[svs->bank_id].vmax_uV;
+	volt_table = svs->updated_volt_table;
+
+	/*
+	 * The optimized voltage values calculated by SVS are put in the two
+	 * registers, SVS_BANK_VOP30 and SVS_BANK_VOP74 which stores values
+	 * corresponding to OPP[4-7] and OPP[4-7].
+	 */
+	reg = readl(mt->thermal_base + SVS_BANK_VOP30);
+	volt_table[0] = clamp(config_to_uvolt((reg & 0xff) + offset),
+			      vmin_uV, vmax_uV);
+	volt_table[1] = clamp(config_to_uvolt(((reg >> 8) & 0xff) + offset),
+			      vmin_uV, vmax_uV);
+	volt_table[2] = clamp(config_to_uvolt(((reg >> 16) & 0xff) + offset),
+			      vmin_uV, vmax_uV);
+	volt_table[3] = clamp(config_to_uvolt(((reg >> 24) & 0xff) + offset),
+			      vmin_uV, vmax_uV);
+
+	reg = readl(mt->thermal_base + SVS_BANK_VOP74);
+	volt_table[4] = clamp(config_to_uvolt((reg & 0xff) + offset),
+			      vmin_uV, vmax_uV);
+	volt_table[5] = clamp(config_to_uvolt(((reg >> 8) & 0xff) + offset),
+			      vmin_uV, vmax_uV);
+	volt_table[6] = clamp(config_to_uvolt(((reg >> 16) & 0xff) + offset),
+			      vmin_uV, vmax_uV);
+	volt_table[7] = clamp(config_to_uvolt(((reg >> 24) & 0xff) + offset),
+			      vmin_uV, vmax_uV);
+}
+
+static void adjust_voltage_work(struct work_struct *work)
+{
+	struct mtk_svs_bank *svs = container_of(work, struct mtk_svs_bank,
+						work);
+	struct mtk_thermal *mt = svs->mt;
+
+	if (svs->status & SVS_STATUS_ERROR || svs->state == SVS_INIT)
+		goto out_only_adjust_voltage;
+
+	mtk_thermal_get_bank(&mt->banks[svs->bank_id]);
+
+	mtk_svs_update_voltage_table(svs);
+
+	if (!completion_done(&svs->init_done)) {
+		complete(&svs->init_done);
+		mtk_svs_set_phase(svs, SVS_PHASE_CONTINUOUS);
+	}
+
+	mtk_thermal_put_bank(&mt->banks[svs->bank_id]);
+
+out_only_adjust_voltage:
+	mtk_svs_adjust_voltage(svs);
+	if (svs->state == SVS_INIT)
+		complete(&svs->init_done);
+}
+
+static void mtk_svs_bank_disable(struct mtk_svs_bank *svs)
+{
+	struct mtk_thermal *mt = svs->mt;
+	int i;
+
+	writel(0, mt->thermal_base + SVS_BANK_EN);
+	writel(0xffffff, mt->thermal_base + SVS_BANK_INTST);
+
+	for (i = 0; i < MT8173_NUM_SVS_OPP; i++) {
+		if (!svs->freq_table[i])
+			continue;
+
+		svs->updated_volt_table[i] = svs->volt_table[i];
+	}
+}
+
+static irqreturn_t mtk_svs_interrupt(int irqno, void *dev_id)
+{
+	struct mtk_thermal *mt = dev_id;
+	u32 svs_intst, bank_en, bank_intst;
+	int i;
+
+
+	svs_intst = readl(mt->thermal_base + SVS_SVSINTST);
+	for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
+		struct mtk_svs_bank *svs = &svs_banks[i];
+
+		if (svs_intst & BIT(i))
+			continue;
+
+		mtk_thermal_get_bank(&mt->banks[i]);
+
+		bank_intst = readl(mt->thermal_base + SVS_BANK_INTST);
+		bank_en = readl(mt->thermal_base + SVS_BANK_EN);
+
+		if (bank_intst == PHASE_01_IRQ && /* phase 0 */
+		    (bank_en & PHASE_EN_MASK) == PHASE_0_EN) {
+			u32 reg;
+
+			reg = readl(mt->thermal_base + SVS_BANK_CONTROL1);
+			svs->ctrl0 |= (~(reg & 0xffff) + 1) & 0xffff;
+			reg =  readl(mt->thermal_base + SVS_BANK_CONTROL2);
+			svs->ctrl0 |= (reg & 0xffff) << 16;
+
+			writel(0, mt->thermal_base + SVS_BANK_EN);
+			writel(PHASE_01_IRQ, mt->thermal_base + SVS_BANK_INTST);
+
+			mtk_svs_set_phase(svs, SVS_PHASE_1);
+		} else if (bank_intst == PHASE_01_IRQ && /* phase 1 */
+			   (bank_en & PHASE_EN_MASK) == PHASE_01_EN) {
+			/*
+			 * Schedule a work to update voltages of OPP table
+			 * entries.
+			 */
+			schedule_work(&svs->work);
+
+			writel(0, mt->thermal_base + SVS_BANK_EN);
+			writel(PHASE_01_IRQ, mt->thermal_base + SVS_BANK_INTST);
+		} else if (bank_intst & PHASE_CON_IRQ) { /* phase continuous*/
+			/*
+			 * Schedule a work to update voltages of OPP table
+			 * entries.
+			 */
+			schedule_work(&svs->work);
+
+			writel(PHASE_CON_IRQ,
+			       mt->thermal_base + SVS_BANK_INTST);
+		} else {
+			svs->status |= SVS_STATUS_ERROR;
+
+			mtk_svs_bank_disable(svs);
+			dev_err(svs->dev,
+				"SVS engine internal error. disabled.\n");
+
+			/*
+			 * Schedule a work to reset voltages of OPP table
+			 * entries.
+			 */
+			schedule_work(&svs->work);
+		}
+
+		mtk_thermal_put_bank(&mt->banks[i]);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int mtk_svs_bank_init(struct mtk_svs_bank *svs)
+{
+	struct dev_pm_opp *opp;
+	int ret = 0, count, i;
+	unsigned long rate;
+
+	init_completion(&svs->init_done);
+
+	INIT_WORK(&svs->work, adjust_voltage_work);
+
+	svs->dev = get_cpu_device(svs->cpu_dev_id);
+	if (!svs->dev) {
+		pr_err("failed to get cpu%d device\n", svs->cpu_dev_id);
+		return -ENODEV;
+	}
+
+	/* Assume CPU DVFS OPP table is already initialized by cpufreq driver*/
+	rcu_read_lock();
+	count = dev_pm_opp_get_opp_count(svs->dev);
+	if (count > MT8173_NUM_SVS_OPP)
+		dev_warn(svs->dev, "%d OPP entries found.\n"
+			 "But only %d OPP entry supported.\n", count,
+			 MT8173_NUM_SVS_OPP);
+
+	for (i = 0, rate = (unsigned long)-1; i < MT8173_NUM_SVS_OPP &&
+	     i < count; i++, rate--) {
+		opp = dev_pm_opp_find_freq_floor(svs->dev, &rate);
+		if (IS_ERR(opp)) {
+			dev_err(svs->dev, "error opp entry!!\n");
+			rcu_read_unlock();
+			ret = PTR_ERR(opp);
+			goto out;
+		}
+
+		svs->freq_table[i] = rate;
+		svs->volt_table[i] = dev_pm_opp_get_voltage(opp);
+	}
+
+out:
+	rcu_read_unlock();
+
+	return ret;
+}
+
+static int mtk_svs_hw_init(struct mtk_thermal *mt)
+{
+	struct clk *parent;
+	unsigned long timeout;
+	struct mtk_svs_bank *svs;
+	struct cpufreq_policy policy;
+	struct pm_qos_request qos_request = {{0}};
+	int i, j, ret, vboot_uV;
+
+	parent = clk_get_parent(mt->svs_mux);
+	ret = clk_set_parent(mt->svs_mux, mt->svs_pll);
+	if (ret) {
+		dev_err(mt->dev,
+			"failed to set svs_mux to svs_pll\n");
+		return ret;
+	}
+
+	/*
+	 * When doing SVS init, we have to make sure all CPUs are on and
+	 * working at 1.0 volt. Add a pm_qos request to prevent CPUs from
+	 * entering CPU off idle state.
+	 */
+	cpu_latency_qos_add_request(&qos_request, 1);
+
+	for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
+		svs = &svs_banks[i];
+
+		/* Backup current cpufreq policy */
+		ret = cpufreq_get_policy(&policy, svs->cpu_dev_id);
+		if (ret) {
+			dev_err(svs->dev, "cpufreq is not ready.\n");
+			cpu_latency_qos_remove_request(&qos_request);
+			clk_set_parent(mt->svs_mux, parent);
+			return ret;
+		}
+
+		/* Force CPUFreq switch to OPP with 1.0 volt */
+		for (j = 0; j < MT8173_NUM_SVS_OPP; j++) {
+			svs->updated_volt_table[j] = svs->volt_table[j];
+			if (svs->volt_table[j] <= svs_bank_cfgs[i].vboot_uV &&
+			    (!svs->max_freq_khz || !svs->min_freq_khz)) {
+				svs->updated_volt_table[j] =
+						svs_bank_cfgs[i].vboot_uV;
+				svs->max_freq_khz = svs->freq_table[j] / 1000;
+				svs->min_freq_khz = svs->freq_table[j] / 1000;
+			}
+		}
+
+		schedule_work(&svs->work);
+		timeout = wait_for_completion_timeout(&svs->init_done, HZ);
+		if (timeout == 0) {
+			dev_err(svs->dev, "SVS vboot init timeout.\n");
+			ret = -EINVAL;
+			goto err_bank_init;
+		}
+
+		reinit_completion(&svs->init_done);
+
+		cpufreq_update_policy(svs->cpu_dev_id);
+
+		/* Check if the voltage is successfully set as 1.0 volt */
+		vboot_uV = regulator_get_voltage(svs->reg);
+		if (uvolt_to_config(vboot_uV) !=
+		    uvolt_to_config(svs_bank_cfgs[i].vboot_uV)) {
+			dev_err(svs->dev, "Vboot value mismatch!\n");
+			ret = -EINVAL;
+			break;
+		}
+
+		/* Configure regulator to PWM mode */
+		ret = regulator_set_mode(svs->reg, REGULATOR_MODE_FAST);
+		if (ret) {
+			dev_err(svs->dev,
+				"Failed to set regulator in PWM mode\n");
+			ret = -EINVAL;
+			break;
+		}
+
+		mtk_thermal_get_bank(&mt->banks[i]);
+
+		mtk_svs_set_phase(svs, SVS_PHASE_0);
+
+		mtk_thermal_put_bank(&mt->banks[i]);
+
+		timeout = wait_for_completion_timeout(&svs->init_done, HZ);
+		if (timeout == 0) {
+			dev_err(svs->dev, "SVS initialization timeout.\n");
+			ret = -EINVAL;
+			goto err_bank_init;
+		}
+
+		/* Unlimit CPUFreq OPP range */
+		svs->max_freq_khz = policy.max;
+		svs->min_freq_khz = policy.min;
+		cpufreq_update_policy(svs->cpu_dev_id);
+
+		/* Configure regulator to normal mode */
+		ret = regulator_set_mode(svs->reg, REGULATOR_MODE_NORMAL);
+		if (ret)
+			dev_err(svs->dev,
+				"Failed to set regulator in normal mode\n");
+	}
+
+err_bank_init:
+
+	if (ret)
+		for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
+			svs = &svs_banks[i];
+
+			mtk_thermal_get_bank(&mt->banks[i]);
+
+			mtk_svs_bank_disable(svs);
+			svs->status |= SVS_STATUS_ERROR;
+
+			mtk_thermal_put_bank(&mt->banks[i]);
+
+			schedule_work(&svs->work);
+		}
+
+	cpu_latency_qos_remove_request(&qos_request);
+
+	ret = clk_set_parent(mt->svs_mux, parent);
+	if (ret) {
+		dev_err(mt->dev,
+			"failed to set svs_mux to original parent\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static bool allow_svs_late_init;
+
+/*
+ * When doing SVS init, we have to make sure all CPUs are on and working at
+ * 1.0 volt. Currently we relies on cpufreq driver doing this by changing
+ * OPP voltage and limit OPP during SVS init. To make sure cpufreq is already
+ * working, put SVS hardware part init in late_initcall().
+ */
+static int mtk_svs_late_init(void)
+{
+	int ret, i;
+
+	if (!allow_svs_late_init)
+		return -EINVAL;
+
+	for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
+		svs_banks[i].bank_id = i;
+
+		ret = mtk_svs_bank_init(&svs_banks[i]);
+		if (ret) {
+			pr_err("failed to initialize mtk svs bank%d\n", i);
+			return ret;
+		}
+	}
+
+	ret = mtk_svs_hw_init(svs_banks[0].mt);
+	if (ret)
+		pr_err("Failed to initialize MTK SVS engine\n");
+
+	return ret;
+}
+late_initcall(mtk_svs_late_init);
+
+static int mtk_svs_get_cpu_id(struct platform_device *pdev)
+{
+	int ret;
+	struct device_node *np = pdev->dev.of_node;
+
+	ret = of_property_read_u32(np, "mediatek,svs-little-core-id",
+				  &svs_banks[MT8173_SVS_BANK_CA53].cpu_dev_id);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Cannot find property mediatek,svs-little-core-id\n");
+		return ret;
+	}
+
+	ret = of_property_read_u32(np, "mediatek,svs-big-core-id",
+				  &svs_banks[MT8173_SVS_BANK_CA72].cpu_dev_id);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Cannot find property mediatek,svs-big-core-id\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static int mtk_svs_probe(struct platform_device *pdev)
+{
+	struct mtk_thermal *mt = platform_get_drvdata(pdev);
+	char supply[8];
+	int i, ret;
+
+	if (!mt->conf->use_svs)
+		return 0;
+
+	ret = mtk_svs_get_cpu_id(pdev);
+	if (ret)
+		return ret;
+
+	mt->svs_pll = devm_clk_get(&pdev->dev, "svs_pll");
+	if (IS_ERR(mt->svs_pll)) {
+		if (PTR_ERR(mt->svs_pll) == -EPROBE_DEFER)
+			return PTR_ERR(mt->svs_pll);
+
+		pr_err("Failed to get SVS PLL clock\n");
+		return ret;
+	}
+
+	mt->svs_mux = devm_clk_get(&pdev->dev, "svs_mux");
+	if (IS_ERR(mt->svs_mux)) {
+		if (PTR_ERR(mt->svs_mux) == -EPROBE_DEFER)
+			return PTR_ERR(mt->svs_mux);
+
+		pr_err("Failed to get SVS MUX clock\n");
+		return ret;
+	}
+
+	for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
+		struct regulator *reg;
+
+		snprintf(supply, sizeof(supply), "bank%d", i);
+		reg = devm_regulator_get_optional(&pdev->dev, supply);
+		if (IS_ERR(reg)) {
+			if (PTR_ERR(reg) == -EPROBE_DEFER)
+				return PTR_ERR(reg);
+
+			pr_err("Failed to get %s regulator\n", supply);
+			return ret;
+		}
+
+		svs_banks[i].reg = reg;
+		svs_banks[i].mt = mt;
+	}
+
+	ret = mtk_svs_get_calibration_data(mt->dev, mt);
+	if (ret) {
+		if (ret != -EPROBE_DEFER)
+			pr_err("Failed to get SVS calibration data\n");
+		return ret;
+	}
+
+	mt->svs_irq = platform_get_irq(pdev, 1);
+	ret = devm_request_threaded_irq(&pdev->dev, mt->svs_irq, NULL,
+					mtk_svs_interrupt,
+					IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					"mtk-svs", mt);
+	if (ret) {
+		pr_err("Failed to get SVS IRQ\n");
+		return ret;
+	}
+
+	/* SVS has successfully probed, allow SVS late init */
+	allow_svs_late_init = true;
+
+	return 0;
+}
+
 static const struct of_device_id mtk_thermal_of_match[] = {
 	{
 		.compatible = "mediatek,mt8173-thermal",
@@ -1230,9 +2131,7 @@ static int mtk_thermal_probe(struct platform_device *pdev)
 	int ret, i, ctrl_id;
 	struct device_node *auxadc, *apmixedsys, *np = pdev->dev.of_node;
 	struct mtk_thermal *mt;
-	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);
@@ -1259,12 +2158,12 @@ static int mtk_thermal_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	auxadc_base = of_iomap(auxadc, 0);
-	auxadc_phys_base = of_get_phys_base(auxadc);
+        mt->auxadc_base = of_iomap(auxadc, 0);
+        mt->auxadc_phys_base = of_get_phys_base(auxadc);
 
 	of_node_put(auxadc);
 
-	if (auxadc_phys_base == OF_BAD_ADDR) {
+	if (mt->auxadc_phys_base == OF_BAD_ADDR) {
 		dev_err(&pdev->dev, "Can't get auxadc phys address\n");
 		return -EINVAL;
 	}
@@ -1275,12 +2174,12 @@ static int mtk_thermal_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	apmixed_base = of_iomap(apmixedsys, 0);
-	apmixed_phys_base = of_get_phys_base(apmixedsys);
+	mt->apmixed_base = of_iomap(apmixedsys, 0);
+	mt->apmixed_phys_base = of_get_phys_base(apmixedsys);
 
 	of_node_put(apmixedsys);
 
-	if (apmixed_phys_base == OF_BAD_ADDR) {
+	if (mt->apmixed_phys_base == OF_BAD_ADDR) {
 		dev_err(&pdev->dev, "Can't get auxadc phys address\n");
 		return -EINVAL;
 	}
@@ -1303,10 +2202,10 @@ static int mtk_thermal_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	mtk_thermal_turn_on_buffer(mt, apmixed_base);
+	mtk_thermal_turn_on_buffer(mt, mt->apmixed_base);
 
-	if (mt->conf->version != MTK_THERMAL_V1)
-		mtk_thermal_release_periodic_ts(mt, auxadc_base);
+	if (mt->conf->version != MTK_THERMAL_V2)
+		mtk_thermal_release_periodic_ts(mt, mt->auxadc_base);
 
 	if (mt->conf->version == MTK_THERMAL_V1)
 		mt->raw_to_mcelsius = raw_to_mcelsius_v1;
@@ -1317,8 +2216,8 @@ static int mtk_thermal_probe(struct platform_device *pdev)
 
 	for (ctrl_id = 0; ctrl_id < mt->conf->num_controller ; ctrl_id++)
 		for (i = 0; i < mt->conf->num_banks; i++)
-			mtk_thermal_init_bank(mt, i, apmixed_phys_base,
-					      auxadc_phys_base, ctrl_id);
+			mtk_thermal_init_bank(mt, i, mt->apmixed_phys_base,
+					      mt->auxadc_phys_base, ctrl_id);
 
 	tzdev = devm_thermal_of_zone_register(&pdev->dev, 0, mt,
 					      &mtk_thermal_ops);
@@ -1353,13 +2252,87 @@ static int mtk_thermal_probe(struct platform_device *pdev)
 			dev_warn(&pdev->dev, "error in thermal_add_hwmon_sysfs: %d\n", ret);
 	}
 
+	ret = mtk_svs_probe(pdev);
+	if (ret == -EPROBE_DEFER)
+		return ret;
+
 	return 0;
 }
 
+static int __maybe_unused mtk_thermal_suspend(struct device *dev)
+{
+       struct platform_device *pdev = to_platform_device(dev);
+       struct mtk_thermal *mt = platform_get_drvdata(pdev);
+       int i, ret;
+
+       for (i = 0; i < mt->conf->num_banks; i++) {
+               ret = mtk_thermal_disable_sensing(mt, i);
+               if (ret)
+                       goto out;
+       }
+
+       /* disable buffer */
+       writel(readl(mt->apmixed_base + APMIXED_SYS_TS_CON1) |
+              APMIXED_SYS_TS_CON1_BUFFER_OFF,
+              mt->apmixed_base + APMIXED_SYS_TS_CON1);
+
+       clk_disable_unprepare(mt->clk_peri_therm);
+       clk_disable_unprepare(mt->clk_auxadc);
+
+       return 0;
+
+out:
+       dev_err(&pdev->dev, "Failed to wait until bus idle\n");
+
+       return ret;
+}
+
+static int __maybe_unused mtk_thermal_resume(struct device *dev)
+{
+       struct platform_device *pdev = to_platform_device(dev);
+       struct mtk_thermal *mt = platform_get_drvdata(pdev);
+       int i, ret, ctrl_id;
+
+       ret = device_reset(&pdev->dev);
+       if (ret)
+               return ret;
+
+       ret = clk_prepare_enable(mt->clk_auxadc);
+       if (ret) {
+               dev_err(&pdev->dev, "Can't enable auxadc clk: %d\n", ret);
+               goto err_disable_clk_auxadc;
+       }
+
+       ret = clk_prepare_enable(mt->clk_peri_therm);
+       if (ret) {
+               dev_err(&pdev->dev, "Can't enable peri clk: %d\n", ret);
+               goto err_disable_clk_peri_therm;
+       }
+
+       for (ctrl_id = 0; ctrl_id < mt->conf->num_controller ; ctrl_id++)
+               for (i = 0; i < mt->conf->num_banks; i++)
+                       mtk_thermal_init_bank(mt, i, mt->apmixed_phys_base,
+                                             mt->auxadc_phys_base, ctrl_id);
+
+       return 0;
+
+err_disable_clk_peri_therm:
+       clk_disable_unprepare(mt->clk_peri_therm);
+err_disable_clk_auxadc:
+       clk_disable_unprepare(mt->clk_auxadc);
+
+       return ret;
+}
+
+static SIMPLE_DEV_PM_OPS(mtk_thermal_pm_ops,
+                        mtk_thermal_suspend, mtk_thermal_resume);
+
+
 static struct platform_driver mtk_thermal_driver = {
 	.probe = mtk_thermal_probe,
 	.driver = {
 		.name = "mtk-thermal",
+                .pm = &mtk_thermal_pm_ops,
 		.of_match_table = mtk_thermal_of_match,
 	},
 };
-- 
2.43.0.rc2.451.g8631bc7472-goog