[RFC PATCH 0/5] Add support for Mediatek SVS engine

[RFC PATCH 0/5] Add support for Mediatek SVS engine

[Pi-Cheng Chen]

This patchset tries to add support for Mediatek SVS engine.

The SVS (Smart Voltage Scaling) engine is a piece of hardware which is
used to caculate optimized voltage values of several power domains, e.g.
CPU clusters, according to chip process corner, temperatures, and other
factors. Then DVFS driver could apply those optimized voltage values to
reduce power consumption. This engine takes calibration data stored in
on-chip E-Fuse device as configuration input during initialization. Once
the initialization is done, SVS engine issues interrupts according to
temerature changes of power domains to notify DVFS driver to get
calculated voltage values.

The configuration registers of SVS engine are shared with Mediatek
thermal controller, and those registers are banked for different power
domains. In addition, the SVS engine also needs some information from
Mediatek thermal controller, e.g. the temperature of a specific power
domain, part of the thermal calibration data. Therefore the support for
SVS engine is integrated with Mediatek thermal controller driver.

Also, for platform specific requirement, to make SVS engine work
correctly, the initialization of SVS engine should be later then
Mediatek thermal controller, and prior to mt8173-cpufreq driver. Hence,
the platform device registration code of mt8173-cpufreq is removed here
after SVS initialization is done or skipped to ensure the platform
specific initialization flow.

The functionality of SVS engine is optional for Mediatek thermal
controller. If the required resources of SVS engine is absent or SVS
failed during initialization stage, the SVS control flow will be
skipped and the thermal controller will function normally.

This patchset relies heavily on the runtime voltage adjustment
mechanism of OPP introduced by Stephen Boyd's previous work[1]. And it
also depends on other MT8173 device drivers that are still under
reviewing and not yet merged, e.g. Mediatek thermal controller support
[2], Mediatek EFUSE driver[3].

CC: Stephen Boyd <sboyd@codeaurora.org>

[1] https://lkml.org/lkml/2015/9/18/833
[2] https://lkml.org/lkml/2015/11/30/239
[3] https://lkml.org/lkml/2015/11/19/169

Pi-Cheng Chen (5):
  thermal: MT8173: Replace mutex with spinlock
  cpufreq: mt8173: Remove platform device registration code
  dt-bindings: thermal: Add optional properties of Mediatek thermal
    controller
  PM / AVS: thermal: MT8173: Introduce support for SVS engine
  cpufreq: mt8173: Add notifier to handle OPP voltage adjustment

 .../bindings/thermal/mediatek-thermal.txt          |  23 +-
 drivers/cpufreq/mt8173-cpufreq.c                   |  68 +-
 drivers/thermal/mtk_thermal.c                      | 727 ++++++++++++++++++++-
 3 files changed, 782 insertions(+), 36 deletions(-)

-- 
1.9.1

[RFC PATCH 1/5] thermal: MT8173: Replace mutex with spinlock

[Pi-Cheng Chen]

Currently switching between the block of banked registers of Mediatek
thermal controller is synchronized by a mutex. Replace it with spinlock
since switching between register banks might happen in interrupt
context in SVS driver which will be introduced later.

Signed-off-by: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
---
 drivers/thermal/mtk_thermal.c | 35 ++++++++++++-----------------------
 1 file changed, 12 insertions(+), 23 deletions(-)

diff --git a/drivers/thermal/mtk_thermal.c b/drivers/thermal/mtk_thermal.c
index 3c233b8..f20e784 100644
--- a/drivers/thermal/mtk_thermal.c
+++ b/drivers/thermal/mtk_thermal.c
@@ -135,7 +135,7 @@ struct mtk_thermal {
 
 	struct mtk_thermal_bank banks[MT8173_NUM_ZONES];
 
-	struct mutex lock;
+	spinlock_t lock;
 
 	/* Calibration values */
 	s32 adc_ge;
@@ -222,19 +222,17 @@ static int raw_to_mcelsius(struct mtk_thermal *mt, int sensno, s32 raw)
 }
 
 /**
- * mtk_thermal_get_bank - get bank
+ * mtk_thermal_switch_bank - switch to bank
  * @bank:	The bank
  *
  * The bank registers are banked, we have to select a bank in the
  * PTPCORESEL register to access it.
  */
-static void mtk_thermal_get_bank(struct mtk_thermal_bank *bank)
+static void mtk_thermal_switch_bank(struct mtk_thermal_bank *bank)
 {
 	struct mtk_thermal *mt = bank->mt;
 	u32 val;
 
-	mutex_lock(&mt->lock);
-
 	val = readl(mt->thermal_base + PTPCORESEL);
 	val &= ~0xf;
 	val |= bank->id;
@@ -242,19 +240,6 @@ static void mtk_thermal_get_bank(struct mtk_thermal_bank *bank)
 }
 
 /**
- * mtk_thermal_put_bank - release bank
- * @bank:	The bank
- *
- * release a bank previously taken with mtk_thermal_get_bank,
- */
-static void mtk_thermal_put_bank(struct mtk_thermal_bank *bank)
-{
-	struct mtk_thermal *mt = bank->mt;
-
-	mutex_unlock(&mt->lock);
-}
-
-/**
  * mtk_thermal_bank_temperature - get the temperature of a bank
  * @bank:	The bank
  *
@@ -297,13 +282,15 @@ static int mtk_read_temp(void *data, int *temperature)
 
 	for (i = 0; i < MT8173_NUM_ZONES; i++) {
 		struct mtk_thermal_bank *bank = &mt->banks[i];
+		unsigned long flags;
 		int t;
 
-		mtk_thermal_get_bank(bank);
+		spin_lock_irqsave(&mt->lock, flags);
+		mtk_thermal_switch_bank(bank);
 
 		t = mtk_thermal_bank_temperature(bank);
 
-		mtk_thermal_put_bank(bank);
+		spin_unlock_irqrestore(&mt->lock, flags);
 
 		if (t > tempmax)
 			tempmax = t;
@@ -323,12 +310,14 @@ static void mtk_thermal_init_bank(struct mtk_thermal *mt, int num,
 {
 	struct mtk_thermal_bank *bank = &mt->banks[num];
 	const struct mtk_thermal_bank_cfg *cfg = &bank_data[num];
+	unsigned long flags;
 	int i;
 
 	bank->id = num;
 	bank->mt = mt;
 
-	mtk_thermal_get_bank(bank);
+	spin_lock_irqsave(&mt->lock, flags);
+	mtk_thermal_switch_bank(bank);
 
 	/* bus clock 66M counting unit is 12 * 15.15ns * 256 = 46.540us */
 	writel(TEMP_MONCTL1_PERIOD_UNIT(12), mt->thermal_base + TEMP_MONCTL1);
@@ -416,7 +405,7 @@ static void mtk_thermal_init_bank(struct mtk_thermal *mt, int num,
 	writel(TEMP_ADCWRITECTRL_ADC_PNP_WRITE | TEMP_ADCWRITECTRL_ADC_MUX_WRITE,
 			mt->thermal_base + TEMP_ADCWRITECTRL);
 
-	mtk_thermal_put_bank(bank);
+	spin_unlock_irqrestore(&mt->lock, flags);
 }
 
 static u64 of_get_phys_base(struct device_node *np)
@@ -513,7 +502,7 @@ static int mtk_thermal_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	mutex_init(&mt->lock);
+	spin_lock_init(&mt->lock);
 
 	mt->dev = &pdev->dev;
 
-- 
1.9.1

[RFC PATCH 2/5] cpufreq: mt8173: Remove platform device registration code

[Pi-Cheng Chen]

For MT8173 platform requirement, to make SVS (Smart Voltage Scaling)
work correctly, the initialization of SVS engine should be later then
thermal controller and prior to cpufreq driver. Remove the platform
device registration code of mt8173-cpufreq and it will be later added
in SVS driver code after SVS initialization done to ensure the platform
specific initialization flow.

Signed-off-by: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
---
 drivers/cpufreq/mt8173-cpufreq.c | 13 -------------
 1 file changed, 13 deletions(-)

diff --git a/drivers/cpufreq/mt8173-cpufreq.c b/drivers/cpufreq/mt8173-cpufreq.c
index 1efba34..1ede917 100644
--- a/drivers/cpufreq/mt8173-cpufreq.c
+++ b/drivers/cpufreq/mt8173-cpufreq.c
@@ -577,7 +577,6 @@ static struct platform_driver mt8173_cpufreq_platdrv = {
 
 static int mt8173_cpufreq_driver_init(void)
 {
-	struct platform_device *pdev;
 	int err;
 
 	if (!of_machine_is_compatible("mediatek,mt8173"))
@@ -587,18 +586,6 @@ static int mt8173_cpufreq_driver_init(void)
 	if (err)
 		return err;
 
-	/*
-	 * Since there's no place to hold device registration code and no
-	 * device tree based way to match cpufreq driver yet, both the driver
-	 * and the device registration codes are put here to handle defer
-	 * probing.
-	 */
-	pdev = platform_device_register_simple("mt8173-cpufreq", -1, NULL, 0);
-	if (IS_ERR(pdev)) {
-		pr_err("failed to register mtk-cpufreq platform device\n");
-		return PTR_ERR(pdev);
-	}
-
 	return 0;
 }
 device_initcall(mt8173_cpufreq_driver_init);
-- 
1.9.1

[RFC PATCH 3/5] dt-bindings: thermal: Add optional properties of Mediatek thermal controller

[Pi-Cheng Chen]

This adds optional properties of Mediatek thermal controller which are
required by SVS engine integrated with Mediatek thermal controller.

Signed-off-by: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
---
 .../bindings/thermal/mediatek-thermal.txt          | 23 ++++++++++++++++------
 1 file changed, 17 insertions(+), 6 deletions(-)

diff --git a/Documentation/devicetree/bindings/thermal/mediatek-thermal.txt b/Documentation/devicetree/bindings/thermal/mediatek-thermal.txt
index 81f9a51..acaacaa 100644
--- a/Documentation/devicetree/bindings/thermal/mediatek-thermal.txt
+++ b/Documentation/devicetree/bindings/thermal/mediatek-thermal.txt
@@ -7,6 +7,11 @@ this device needs phandles to the AUXADC. Also it controls a mux in the
 apmixedsys register space via AHB bus accesses, so a phandle to the APMIXEDSYS
 is also needed.
 
+There is another hardware engine, SVS (Smart Voltage Scaling) which shares the
+same block of banked registers with Mediatek thermal controller. Hence the
+driver of SVS is integrated with the driver of Mediatek thermal controller. The
+properties required by SVS engine are optional for Mediatek thermal controller.
+
 Required properties:
 - compatible: "mediatek,mt8173-thermal"
 - reg: Address range of the thermal controller
@@ -21,9 +26,15 @@ Required properties:
 - #thermal-sensor-cells : Should be 0. See ./thermal.txt for a description.
 
 Optional properties:
-- nvmem-cells: A phandle to the calibration data provided by a nvmem device. If
-               unspecified default values shall be used.
-- nvmem-cell-names: Should be "calibration-data"
+- clocks, clock-names: Clocks that are optional for the thermal controller.
+	       Specify to enable SVS engine.
+	       "svs_pll": The PLL clock should be switched to during
+			  initialization stage of SVS engine.
+	       "svs_mux": The MUX clock controls the clock input of SVS engine.
+- nvmem-cells: A list of phandles to the calibration data provided by a nvmem
+	       device. If unspecified default values shall be used. The SVS
+	       engine will be disabled if no SVS calibration data is specified.
+- nvmem-cell-names: Should be "calibration-data" and "svs-calibration-data"
 
 Example:
 
@@ -33,11 +44,11 @@ Example:
 		reg = <0 0x1100b000 0 0x1000>;
 		interrupts = <0 70 IRQ_TYPE_LEVEL_LOW>;
 		clocks = <&pericfg CLK_PERI_THERM>, <&pericfg CLK_PERI_AUXADC>;
-		clock-names = "therm", "auxadc";
+		clock-names = "therm", "auxadc", ;
 		resets = <&pericfg MT8173_PERI_THERM_SW_RST>;
 		reset-names = "therm";
 		mediatek,auxadc = <&auxadc>;
 		mediatek,apmixedsys = <&apmixedsys>;
-		nvmem-cells = <&thermal_calibration_data>;
-		nvmem-cell-names = "calibration-data";
+		nvmem-cells = <&thermal_calibration_data>, <&svs-calibration>;
+		nvmem-cell-names = "calibration-data", "svs-calibration-data";
 	};
-- 
1.9.1

Re: [RFC PATCH 3/5] dt-bindings: thermal: Add optional properties of Mediatek thermal controller

[Rob Herring]

On Fri, Jan 22, 2016 at 04:40:27PM +0800, Pi-Cheng Chen wrote:
> This adds optional properties of Mediatek thermal controller which are
> required by SVS engine integrated with Mediatek thermal controller.
> 
> Signed-off-by: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
> ---
>  .../bindings/thermal/mediatek-thermal.txt          | 23 ++++++++++++++++------
>  1 file changed, 17 insertions(+), 6 deletions(-)
> 
> diff --git a/Documentation/devicetree/bindings/thermal/mediatek-thermal.txt b/Documentation/devicetree/bindings/thermal/mediatek-thermal.txt
> index 81f9a51..acaacaa 100644
> --- a/Documentation/devicetree/bindings/thermal/mediatek-thermal.txt
> +++ b/Documentation/devicetree/bindings/thermal/mediatek-thermal.txt
> @@ -7,6 +7,11 @@ this device needs phandles to the AUXADC. Also it controls a mux in the
>  apmixedsys register space via AHB bus accesses, so a phandle to the APMIXEDSYS
>  is also needed.
>  
> +There is another hardware engine, SVS (Smart Voltage Scaling) which shares the
> +same block of banked registers with Mediatek thermal controller. Hence the
> +driver of SVS is integrated with the driver of Mediatek thermal controller. The
> +properties required by SVS engine are optional for Mediatek thermal controller.
> +
>  Required properties:
>  - compatible: "mediatek,mt8173-thermal"
>  - reg: Address range of the thermal controller
> @@ -21,9 +26,15 @@ Required properties:
>  - #thermal-sensor-cells : Should be 0. See ./thermal.txt for a description.
>  
>  Optional properties:
> -- nvmem-cells: A phandle to the calibration data provided by a nvmem device. If
> -               unspecified default values shall be used.
> -- nvmem-cell-names: Should be "calibration-data"
> +- clocks, clock-names: Clocks that are optional for the thermal controller.
> +	       Specify to enable SVS engine.
> +	       "svs_pll": The PLL clock should be switched to during
> +			  initialization stage of SVS engine.
> +	       "svs_mux": The MUX clock controls the clock input of SVS engine.
> +- nvmem-cells: A list of phandles to the calibration data provided by a nvmem
> +	       device. If unspecified default values shall be used. The SVS
> +	       engine will be disabled if no SVS calibration data is specified.
> +- nvmem-cell-names: Should be "calibration-data" and "svs-calibration-data"
>  
>  Example:
>  
> @@ -33,11 +44,11 @@ Example:
>  		reg = <0 0x1100b000 0 0x1000>;
>  		interrupts = <0 70 IRQ_TYPE_LEVEL_LOW>;
>  		clocks = <&pericfg CLK_PERI_THERM>, <&pericfg CLK_PERI_AUXADC>;
> -		clock-names = "therm", "auxadc";
> +		clock-names = "therm", "auxadc", ;

Huh?

>  		resets = <&pericfg MT8173_PERI_THERM_SW_RST>;
>  		reset-names = "therm";
>  		mediatek,auxadc = <&auxadc>;
>  		mediatek,apmixedsys = <&apmixedsys>;
> -		nvmem-cells = <&thermal_calibration_data>;
> -		nvmem-cell-names = "calibration-data";
> +		nvmem-cells = <&thermal_calibration_data>, <&svs-calibration>;
> +		nvmem-cell-names = "calibration-data", "svs-calibration-data";
>  	};
> -- 
> 1.9.1
> 

Re: [RFC PATCH 3/5] dt-bindings: thermal: Add optional properties of Mediatek thermal controller

[Pi-Cheng Chen]

On Fri, Jan 22, 2016 at 04:31:36PM -0600, Rob Herring wrote:
> On Fri, Jan 22, 2016 at 04:40:27PM +0800, Pi-Cheng Chen wrote:
> > This adds optional properties of Mediatek thermal controller which are
> > required by SVS engine integrated with Mediatek thermal controller.
> > 
> > Signed-off-by: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
> > ---
> >  .../bindings/thermal/mediatek-thermal.txt          | 23 ++++++++++++++++------
> >  1 file changed, 17 insertions(+), 6 deletions(-)
> > 
> > diff --git a/Documentation/devicetree/bindings/thermal/mediatek-thermal.txt b/Documentation/devicetree/bindings/thermal/mediatek-thermal.txt
> > index 81f9a51..acaacaa 100644
> > --- a/Documentation/devicetree/bindings/thermal/mediatek-thermal.txt
> > +++ b/Documentation/devicetree/bindings/thermal/mediatek-thermal.txt
> > @@ -7,6 +7,11 @@ this device needs phandles to the AUXADC. Also it controls a mux in the
> >  apmixedsys register space via AHB bus accesses, so a phandle to the APMIXEDSYS
> >  is also needed.
> >  
> > +There is another hardware engine, SVS (Smart Voltage Scaling) which shares the
> > +same block of banked registers with Mediatek thermal controller. Hence the
> > +driver of SVS is integrated with the driver of Mediatek thermal controller. The
> > +properties required by SVS engine are optional for Mediatek thermal controller.
> > +
> >  Required properties:
> >  - compatible: "mediatek,mt8173-thermal"
> >  - reg: Address range of the thermal controller
> > @@ -21,9 +26,15 @@ Required properties:
> >  - #thermal-sensor-cells : Should be 0. See ./thermal.txt for a description.
> >  
> >  Optional properties:
> > -- nvmem-cells: A phandle to the calibration data provided by a nvmem device. If
> > -               unspecified default values shall be used.
> > -- nvmem-cell-names: Should be "calibration-data"
> > +- clocks, clock-names: Clocks that are optional for the thermal controller.
> > +	       Specify to enable SVS engine.
> > +	       "svs_pll": The PLL clock should be switched to during
> > +			  initialization stage of SVS engine.
> > +	       "svs_mux": The MUX clock controls the clock input of SVS engine.
> > +- nvmem-cells: A list of phandles to the calibration data provided by a nvmem
> > +	       device. If unspecified default values shall be used. The SVS
> > +	       engine will be disabled if no SVS calibration data is specified.
> > +- nvmem-cell-names: Should be "calibration-data" and "svs-calibration-data"
> >  
> >  Example:
> >  
> > @@ -33,11 +44,11 @@ Example:
> >  		reg = <0 0x1100b000 0 0x1000>;
> >  		interrupts = <0 70 IRQ_TYPE_LEVEL_LOW>;
> >  		clocks = <&pericfg CLK_PERI_THERM>, <&pericfg CLK_PERI_AUXADC>;
> > -		clock-names = "therm", "auxadc";
> > +		clock-names = "therm", "auxadc", ;
> 
> Huh?
> 

It should be:

clocks = <&pericfg CLK_PERI_THERM>, <&pericfg CLK_PERI_AUXADC>,
	 <&topckgen CLK_TOP_SYSPLL1_D2>, <&topckgen CLK_TOP_AXI_SEL>;
clock-names = "therm", "auxadc", "svs_pll", "svs_mux";

Sorry for my mistake.

> >  		resets = <&pericfg MT8173_PERI_THERM_SW_RST>;
> >  		reset-names = "therm";
> >  		mediatek,auxadc = <&auxadc>;
> >  		mediatek,apmixedsys = <&apmixedsys>;
> > -		nvmem-cells = <&thermal_calibration_data>;
> > -		nvmem-cell-names = "calibration-data";
> > +		nvmem-cells = <&thermal_calibration_data>, <&svs-calibration>;
> > +		nvmem-cell-names = "calibration-data", "svs-calibration-data";
> >  	};
> > -- 
> > 1.9.1
> > 

[RFC PATCH 4/5] PM / AVS: thermal: MT8173: Introduce support for SVS engine

[Pi-Cheng Chen]

The SVS (Smart Voltage Scaling) engine is a piece of hardware which is
used to caculate optimized voltage values of several power domains, e.g.
CPU clusters, according to chip process corner, temperatures, and other
factors. Then DVFS driver could apply those optimized voltage values to
reduce power consumption. This engine takes calibration data stored in
on-chip E-Fuse device as configuration input during initialization. Once
the initialization is done, SVS engine issues interrupts according to
temerature changes of power domains to notify DVFS driver to get
calculated voltage values.

The configuration registers of SVS engine are shared with Mediatek
thermal controller, and those registers are banked for different power
domains. In addition, the SVS engine also needs some information from
Mediatek thermal controller, e.g. the temperature of a specific power
domain, part of the thermal calibration data. Therefore the support for
SVS engine is integrated with Mediatek thermal controller driver.

Also, for platform specific requirement, to make SVS engine work
correctly, the initialization of SVS engine should be later then
Mediatek thermal controller, and prior to mt8173-cpufreq driver. Hence,
the platform device registration code of mt8173-cpufreq is removed here
after SVS initialization is done or skipped to ensure the platform
specific initialization flow.

The functionality of SVS engine is optional for Mediatek thermal
controller. If the required resources of SVS engine is absent or SVS
failed during initialization stage, the SVS control flow will be
skipped and the thermal controller will function normally.

CC: Stephen Boyd <sboyd@codeaurora.org>

Signed-off-by: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
---
This patch relies on the runtime voltage adjustment of OPP introduced in CPR
patchset done by Stephen Boyd[1].

[1] https://lkml.org/lkml/2015/9/18/833
---
 drivers/thermal/mtk_thermal.c | 704 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 704 insertions(+)

diff --git a/drivers/thermal/mtk_thermal.c b/drivers/thermal/mtk_thermal.c
index f20e784..d8ba9dd 100644
--- a/drivers/thermal/mtk_thermal.c
+++ b/drivers/thermal/mtk_thermal.c
@@ -13,6 +13,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>
@@ -21,12 +24,15 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/platform_device.h>
+#include <linux/pm_opp.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/nvmem-consumer.h>
+#include <linux/workqueue.h>
 
 /* AUXADC Registers */
 #define AUXADC_CON0_V		0x000
@@ -73,7 +79,32 @@
 
 #define TEMP_SPARE0		0x0f0
 
+#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)
 
@@ -106,6 +137,24 @@
 /* The number of sensing points per bank */
 #define MT8173_NUM_SENSORS_PER_ZONE	4
 
+/* The number of OPPs supported by SVS */
+#define MT8173_NUM_SVS_OPP		8
+
+/* Bit masks of SVS enable and IRQ configrations */
+#define PHASE_EN_MASK		0x7
+#define PHASE_0_EN		0x1
+#define PHASE_CON_EN		0x2
+#define PHASE_1_EN		0x4
+#define PHASE_01_EN		(PHASE_0_EN | PHASE_1_EN)
+#define PHASE_01_IRQ		0x1
+#define PHASE_CON_IRQ		(0xff << 16)
+
+/* The number of SVS banks implemented */
+#define MT8173_NUM_SVS_BANKS	2
+
+#define MT8173_SVS_BANK_CA53	0
+#define MT8173_SVS_BANK_CA72	1
+
 /* Layout of the fuses providing the calibration data */
 #define MT8173_CALIB_BUF0_VALID		(1 << 0)
 #define MT8173_CALIB_BUF1_ADC_GE(x)	(((x) >> 22 ) & 0x3ff)
@@ -116,6 +165,22 @@
 #define MT8173_CALIB_BUF2_VTS_TSABB(x)	(((x) >> 14 ) & 0x1ff)
 #define MT8173_CALIB_BUF0_DEGC_CALI(x)	(((x) >> 1 ) & 0x3f)
 #define MT8173_CALIB_BUF0_O_SLOPE(x)	(((x) >> 26 ) & 0x3f)
+#define MT8173_CALIB_BUF0_O_SLOPE_S(x)	(((x) >> 7) & 0x1)
+
+/* Helpers to calculate configuration values from SVS calibration data */
+#define SVS_CALIB_VALID	(1)
+#define BANK_SHIFT(bank) ((bank == 0) ? 8 : 0)
+#define SVS_CALIB_BANK_CONFIG0(buf, s)				\
+	(((((buf[33] >> (s)) & 0xff)) << 8) |			\
+	((buf[32] >> (s)) & 0xff))
+#define SVS_CALIB_BANK_CONFIG1(buf, s)				\
+	((((buf[34] >> (s)) & 0xff) << 8) | 0x100006)
+#define SVS_CALIB_BANK_CONFIG2L(base, s)			\
+	((buf[0] >> (s)) & 0xff)
+#define SVS_CALIB_BANK_CONFIG2H(base, s)			\
+	((buf[1] >> (s)) & 0xff)
+#define SVS_CALIB_BANK_CONFIG3(base, s)				\
+	(((buf[2] >> (s)) & 0xff) << 8)
 
 #define THERMAL_NAME    "mtk-thermal"
 
@@ -126,14 +191,55 @@ struct mtk_thermal_bank {
 	int id;
 };
 
+enum svs_state {
+	SVS_PHASE_0 = 0,
+	SVS_PHASE_1,
+	SVS_PHASE_CONTINUOUS,
+};
+
+struct mtk_svs_bank {
+	int bank_id;
+	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];
+	int volt_table[MT8173_NUM_SVS_OPP];
+	int updated_volt_table[MT8173_NUM_SVS_OPP];
+};
+
+struct mtk_svs_bank_cfg {
+	const int dev_id;
+	const int ts;
+	const int vmin;
+	const int vmax;
+	const int vboot;
+	const unsigned long base_freq;
+};
+
 struct mtk_thermal {
 	struct device *dev;
 	void __iomem *thermal_base;
 
 	struct clk *clk_peri_therm;
 	struct clk *clk_auxadc;
+	struct clk *svs_mux;
+	struct clk *svs_pll;
 
 	struct mtk_thermal_bank banks[MT8173_NUM_ZONES];
+	struct mtk_svs_bank svs_banks[MT8173_NUM_SVS_BANKS];
+
+	int svs_irq;
 
 	spinlock_t lock;
 
@@ -142,6 +248,9 @@ struct mtk_thermal {
 	s32 degc_cali;
 	s32 o_slope;
 	s32 vts[MT8173_NUM_SENSORS];
+	s32 x_roomt[MT8173_NUM_SENSORS];
+	s32 bts[MT8173_NUM_ZONES];
+	s32 mts;
 
 	struct thermal_zone_device *tzd;
 };
@@ -198,6 +307,25 @@ static const struct mtk_thermal_sense_point
 	},
 };
 
+static const struct mtk_svs_bank_cfg svs_bank_cfgs[MT8173_NUM_SVS_BANKS] = {
+	[MT8173_SVS_BANK_CA53] = {
+		.dev_id = 0,
+		.vmax = 1125,
+		.vmin = 800,
+		.vboot = 1000,
+		.base_freq = 1600000,
+		.ts = MT8173_TS3
+	},
+	[MT8173_SVS_BANK_CA72] = {
+		.dev_id = 2,
+		.vmax = 1125,
+		.vmin = 800,
+		.vboot = 1000,
+		.base_freq = 2000000,
+		.ts = MT8173_TS4
+	}
+};
+
 /**
  * raw_to_mcelsius - convert a raw ADC value to mcelsius
  * @mt:		The thermal controller
@@ -222,6 +350,34 @@ static int raw_to_mcelsius(struct mtk_thermal *mt, int sensno, s32 raw)
 }
 
 /**
+ * mvolt_to_config - convert a voltage value to SVS voltage config value
+ * @mvolt:	voltage value
+ */
+static inline u32 mvolt_to_config(int mvolt)
+{
+	return ((mvolt - 700) * 100 + 625 - 1) / 625;
+}
+
+/**
+ * mvolt_to_config - convert a SVS voltage config value to voltage value
+ * @val:	SVS voltage config value
+ */
+static inline int config_to_mv(u32 val)
+{
+	return (val * 625 / 100) + 700;
+}
+
+/**
+ * khz_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 int khz_to_config(unsigned long rate, unsigned long base_rate)
+{
+	return (rate * 100 + base_rate - 1) / base_rate;
+}
+
+/**
  * mtk_thermal_switch_bank - switch to bank
  * @bank:	The bank
  *
@@ -240,6 +396,46 @@ static void mtk_thermal_switch_bank(struct mtk_thermal_bank *bank)
 }
 
 /**
+ * mtk_svs_update_voltage_table - update the calculated voltage table
+ * @svs: The SVS bank
+ * @offset: The voltage offset
+ *
+ * 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, int offset)
+{
+	struct mtk_thermal *mt = svs->mt;
+	int vmin, vmax, *volt_table;
+	u32 reg;
+
+	vmin = svs_bank_cfgs[svs->bank_id].vmin;
+	vmax = svs_bank_cfgs[svs->bank_id].vmax;
+	volt_table = svs->updated_volt_table;
+
+	reg = readl(mt->thermal_base + SVS_BANK_VOP30);
+	volt_table[0] = clamp(config_to_mv((reg & 0xff) + offset),
+			      vmin, vmax);
+	volt_table[1] = clamp(config_to_mv(((reg >> 8) & 0xff) + offset),
+			      vmin, vmax);
+	volt_table[2] = clamp(config_to_mv(((reg >> 16) & 0xff) + offset),
+			      vmin, vmax);
+	volt_table[3] = clamp(config_to_mv(((reg >> 24) & 0xff) + offset),
+			      vmin, vmax);
+
+	reg = readl(mt->thermal_base + SVS_BANK_VOP74);
+	volt_table[4] = clamp(config_to_mv((reg & 0xff) + offset),
+			      vmin, vmax);
+	volt_table[5] = clamp(config_to_mv(((reg >> 8) & 0xff) + offset),
+			      vmin, vmax);
+	volt_table[6] = clamp(config_to_mv(((reg >> 16) & 0xff) + offset),
+			      vmin, vmax);
+	volt_table[7] = clamp(config_to_mv(((reg >> 24) & 0xff) + offset),
+			      vmin, vmax);
+}
+
+/**
  * mtk_thermal_bank_temperature - get the temperature of a bank
  * @bank:	The bank
  *
@@ -426,6 +622,7 @@ static int mtk_thermal_get_calibration_data(struct device *dev, struct mtk_therm
 	u32 *buf;
 	size_t len;
 	int i, ret = 0;
+	s32 o_slope_s, ge, oe, gain, x_roomt, temp1, temp2;
 
 	/* Start with default values */
 	mt->adc_ge = 512;
@@ -433,6 +630,7 @@ static int mtk_thermal_get_calibration_data(struct device *dev, struct mtk_therm
 		mt->vts[i] = 260;
 	mt->degc_cali = 40;
 	mt->o_slope = 0;
+	o_slope_s = 0;
 
 	cell = nvmem_cell_get(dev, "calibration-data");
 	if (IS_ERR(cell)) {
@@ -463,23 +661,464 @@ static int mtk_thermal_get_calibration_data(struct device *dev, struct mtk_therm
 		mt->vts[MT8173_TSABB] = MT8173_CALIB_BUF2_VTS_TSABB(buf[2]);
 		mt->degc_cali = MT8173_CALIB_BUF0_DEGC_CALI(buf[0]);
 		mt->o_slope = MT8173_CALIB_BUF0_O_SLOPE(buf[0]);
+		o_slope_s = MT8173_CALIB_BUF0_O_SLOPE_S(buf[0]);
 	} else {
 		dev_info(dev, "Device not calibrated, using default calibration values\n");
 	}
 
+	oe = mt->adc_ge - 512;
+	ge = oe * 10000 / 4096;
+	gain = 10000 + ge;
+
+	/* calculating MTS */
+	mt->mts = (o_slope_s == 0) ?
+		  10000 * 100000 / gain * 15 / 18 / (165 + mt->o_slope) :
+		  10000 * 100000 / gain * 15 / 18 / (165 - mt->o_slope);
+
+	/* 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] + 3350 - oe * 10000 / 4096 * 10000 / gain;
+		temp1 = (10000 * 100000 / 4096 / gain) *
+			ge + x_roomt * 10 * 15 / 18;
+		temp2 = (o_slope_s == 0) ? temp1 * 10 / (165 + mt->o_slope) :
+			temp1 * 10 / (165 - mt->o_slope);
+		mt->bts[i] = (mt->degc_cali * 10 / 2 + temp2 - 250) * 4 / 10;
+	}
+
 out:
 	kfree(buf);
 
 	return ret;
 }
 
+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;
+
+	cell = nvmem_cell_get(dev, "svs-calibration-data");
+	if (IS_ERR(cell))
+		return PTR_ERR(cell);
+
+	buf = (u32 *)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;
+
+		mt->svs_banks[i].config0 =
+				SVS_CALIB_BANK_CONFIG0(buf, BANK_SHIFT(i));
+		mt->svs_banks[i].config1 =
+				SVS_CALIB_BANK_CONFIG1(buf, BANK_SHIFT(i));
+		mt->svs_banks[i].config3 =
+				SVS_CALIB_BANK_CONFIG3(buf, BANK_SHIFT(i));
+
+		temp = SVS_CALIB_BANK_CONFIG2H(buf, BANK_SHIFT(i));
+		if (temp < 128 && i == MT8173_SVS_BANK_CA72)
+			temp = (unsigned char)((temp - 256) / 2);
+		temp = ((temp & 0xff) << 8) |
+		       SVS_CALIB_BANK_CONFIG2L(buf, BANK_SHIFT(i));
+		mt->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;
+	int id = svs->bank_id;
+
+	freq_tbl = svs->freq_table;
+	base_freq = svs_bank_cfgs[id].base_freq;
+
+	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(0x555555, mt->thermal_base + SVS_BANK_CONFIG4);
+	writel(0x555555, mt->thermal_base + SVS_BANK_CONFIG5);
+	writel(0x80000000, mt->thermal_base + SVS_BANK_CONFIG10);
+
+	writel((khz_to_config(freq_tbl[0], base_freq) & 0xff) |
+	       ((khz_to_config(freq_tbl[1], base_freq) & 0xff) << 8) |
+	       ((khz_to_config(freq_tbl[2], base_freq) & 0xff) << 16) |
+	       ((khz_to_config(freq_tbl[3], base_freq) & 0xff) << 24),
+	       mt->thermal_base + SVS_BANK_FREQPCT30);
+
+	writel((khz_to_config(freq_tbl[4], base_freq) & 0xff) |
+	       ((khz_to_config(freq_tbl[5], base_freq) & 0xff) << 8) |
+	       ((khz_to_config(freq_tbl[6], base_freq) & 0xff) << 16) |
+	       ((khz_to_config(freq_tbl[7], base_freq) & 0xff) << 24),
+	       mt->thermal_base + SVS_BANK_FREQPCT74);
+
+	writel(((mvolt_to_config(svs_bank_cfgs[id].vmax) & 0xff) << 24) |
+	       ((mvolt_to_config(svs_bank_cfgs[id].vmin) & 0xff) << 16) | 0x1fe,
+	       mt->thermal_base + SVS_BANK_LIMITVALS);
+
+	writel(mvolt_to_config(svs_bank_cfgs[id].vboot),
+	       mt->thermal_base + SVS_BANK_CONFIG6);
+	writel(0xa28, mt->thermal_base + SVS_BANK_CONFIG7);
+	writel(0xffff, mt->thermal_base + SVS_BANK_CONFIG8);
+
+	/* clear all pending interrupt */
+	writel(0xffffffff, mt->thermal_base + SVS_BANK_INTST);
+
+	switch (phase) {
+	case SVS_PHASE_0:
+		writel(0x5f01, mt->thermal_base + SVS_BANK_CONTROL3);
+		writel(PHASE_0_EN, mt->thermal_base + SVS_BANK_EN);
+		break;
+	case SVS_PHASE_1:
+		writel(0x5f01, 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);
+		break;
+	case SVS_PHASE_CONTINUOUS:
+		writel(((mt->bts[id] & 0xfff) << 12) | (mt->mts & 0xfff),
+		       mt->thermal_base + SVS_BANK_CONFIG9);
+		writel(0xff0000, mt->thermal_base + SVS_BANK_CONTROL3);
+		writel(PHASE_CON_EN, mt->thermal_base + SVS_BANK_EN);
+	}
+}
+
+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] * 1000,
+					  svs->updated_volt_table[i] * 1000);
+	}
+}
+
+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;
+	unsigned long flags;
+	int temp, low_temp_offset = 0;
+
+	spin_lock_irqsave(&mt->lock, flags);
+	mtk_thermal_switch_bank(&mt->banks[svs->bank_id]);
+
+	temp = mtk_thermal_bank_temperature(&mt->banks[svs->bank_id]);
+
+	spin_unlock_irqrestore(&mt->lock, flags);
+
+	if (temp <= 33000)
+		low_temp_offset = 10;
+
+	mtk_svs_update_voltage_table(svs, low_temp_offset);
+	mtk_svs_adjust_voltage(svs);
+}
+
+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;
+	unsigned long flags;
+	u32 svs_intst, bank_en, bank_intst;
+	int i;
+
+	spin_lock_irqsave(&mt->lock, flags);
+
+	svs_intst = readl(mt->thermal_base + SVS_SVSINTST);
+	for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
+		struct mtk_svs_bank *svs = &mt->svs_banks[i];
+
+		if ((svs_intst >> i) & 0x1)
+			continue;
+
+		mtk_thermal_switch_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) {
+			/*
+			 * The cpufreq driver won't work during SVS
+			 * initialization stage, so it's safe to update OPP
+			 * table here since it won't trigger regulator
+			 * operation.
+			 */
+			mtk_svs_update_voltage_table(svs, 0);
+			mtk_svs_adjust_voltage(svs);
+
+			writel(0, mt->thermal_base + SVS_BANK_EN);
+			writel(PHASE_01_IRQ, mt->thermal_base + SVS_BANK_INTST);
+
+			complete(&svs->init_done);
+
+			mtk_svs_set_phase(svs, SVS_PHASE_CONTINUOUS);
+		} 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 {
+			mtk_svs_bank_disable(svs);
+			dev_err(mt->svs_banks[i].dev,
+				"SVS engine internal error. disabled.\n");
+
+			/*
+			 * Schedule a work to reset voltages of OPP table
+			 * entries.
+			 */
+			schedule_work(&svs->work);
+		}
+	}
+
+	spin_unlock_irqrestore(&mt->lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static int mtk_svs_bank_init(struct mtk_svs_bank *svs)
+{
+	struct dev_pm_opp *opp;
+	cpumask_t cpus;
+	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_bank_cfgs[svs->bank_id].dev_id);
+	if (!svs->dev) {
+		pr_err("failed to get cpu%d device\n",
+		       svs_bank_cfgs[svs->bank_id].dev_id);
+		return -ENODEV;
+	}
+
+	svs->reg = regulator_get_exclusive(svs->dev, "proc");
+	if (IS_ERR(svs->reg)) {
+		dev_err(svs->dev, "failed to get proc regulator\n");
+		return PTR_ERR(svs->reg);
+	}
+
+	ret = dev_pm_opp_of_get_sharing_cpus(svs->dev, &cpus);
+	if (ret) {
+		dev_err(svs->dev, "failed to get OPP-sharing information\n");
+		return ret;
+	}
+
+	ret = dev_pm_opp_of_cpumask_add_table(&cpus);
+	if (ret) {
+		dev_err(svs->dev, "no OPP table\n");
+		return ret;
+	}
+
+	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 / 1000;
+		svs->volt_table[i] = dev_pm_opp_get_voltage(opp) / 1000;
+	}
+
+out:
+	rcu_read_unlock();
+	if (ret) {
+		regulator_put(svs->reg);
+		dev_pm_opp_of_cpumask_remove_table(&cpus);
+	}
+
+	return ret;
+}
+
+static void mtk_svs_release(struct mtk_thermal *mt)
+{
+	int i, ret;
+	cpumask_t cpus;
+
+	if (!IS_ERR(mt->svs_pll))
+		clk_put(mt->svs_pll);
+
+	if (!IS_ERR(mt->svs_mux))
+		clk_put(mt->svs_mux);
+
+	if (mt->svs_irq != -1)
+		free_irq(mt->svs_irq, mt);
+
+	for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
+		struct mtk_svs_bank *svs = &mt->svs_banks[i];
+
+		ret = dev_pm_opp_of_get_sharing_cpus(svs->dev, &cpus);
+		if (!ret)
+			dev_pm_opp_of_cpumask_remove_table(&cpus);
+
+		if (svs->reg)
+			regulator_put(svs->reg);
+	}
+}
+
+static int mtk_svs_init(struct platform_device *pdev, struct mtk_thermal *mt)
+{
+	struct clk *parent;
+	int i, ret, vboot;
+	unsigned long flags, timeout;
+	struct mtk_svs_bank *svs;
+
+	parent = clk_get_parent(mt->svs_mux);
+	ret = clk_set_parent(mt->svs_mux, mt->svs_pll);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"failed to set svs_mux to svs_pll\n");
+		return ret;
+	}
+
+	for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
+		svs = &mt->svs_banks[i];
+
+		vboot = regulator_get_voltage(svs->reg) / 1000;
+		if (mvolt_to_config(vboot) !=
+		    mvolt_to_config(svs_bank_cfgs[i].vboot)) {
+			dev_err(svs->dev, "Vboot value mismatch!\n");
+			ret = -EINVAL;
+			break;
+		}
+
+		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;
+		}
+
+		spin_lock_irqsave(&mt->lock, flags);
+		mtk_thermal_switch_bank(&mt->banks[i]);
+
+		mtk_svs_set_phase(svs, SVS_PHASE_0);
+
+		spin_unlock_irqrestore(&mt->lock, flags);
+
+		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;
+		}
+
+		ret = regulator_set_mode(svs->reg, REGULATOR_MODE_NORMAL);
+		if (ret)
+			dev_err(svs->dev,
+				"Failed to set regulator in normal mode\n");
+
+		regulator_put(svs->reg);
+		svs->reg = NULL;
+	}
+
+err_bank_init:
+
+	if (ret)
+		for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
+			svs = &mt->svs_banks[i];
+
+			spin_lock_irqsave(&mt->lock, flags);
+			mtk_thermal_switch_bank(&mt->banks[i]);
+
+			mtk_svs_bank_disable(svs);
+
+			spin_unlock_irqrestore(&mt->lock, flags);
+
+			mtk_svs_adjust_voltage(svs);
+		}
+
+	ret = clk_set_parent(mt->svs_mux, parent);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"failed to set svs_mux to original parent\n");
+		return ret;
+	}
+
+	return ret;
+}
+
 static int mtk_thermal_probe(struct platform_device *pdev)
 {
 	int ret, i;
 	struct device_node *auxadc, *apmixedsys, *np = pdev->dev.of_node;
 	struct mtk_thermal *mt;
 	struct resource *res;
+	struct platform_device *pdev_ret;
 	u64 auxadc_phys_base, apmixed_phys_base;
+	bool skip_svs_init = false;
 
 	mt = devm_kzalloc(&pdev->dev, sizeof(*mt), GFP_KERNEL);
 	if (!mt)
@@ -493,6 +1132,23 @@ static int mtk_thermal_probe(struct platform_device *pdev)
 	if (IS_ERR(mt->clk_auxadc))
 		return PTR_ERR(mt->clk_auxadc);
 
+	/*
+	 * These two clks are optional for mtk-thermal. If present, SVS engine
+	 * will be activated.
+	 */
+	mt->svs_pll = devm_clk_get(&pdev->dev, "svs_pll");
+	mt->svs_mux = devm_clk_get(&pdev->dev, "svs_mux");
+	if (IS_ERR(mt->svs_pll) || IS_ERR(mt->svs_mux))
+		skip_svs_init = true;
+
+	mt->svs_irq = platform_get_irq(pdev, 1);
+	ret = devm_request_irq(&pdev->dev, mt->svs_irq, mtk_svs_interrupt,
+			       IRQF_TRIGGER_LOW, "mtk-svs", mt);
+	if (ret) {
+		mt->svs_irq = -1;
+		skip_svs_init = true;
+	}
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	mt->thermal_base = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(mt->thermal_base))
@@ -502,6 +1158,27 @@ static int mtk_thermal_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
+	ret = mtk_svs_get_calibration_data(&pdev->dev, mt);
+	if (ret == -EPROBE_DEFER)
+		return ret;
+	else if (ret)
+		skip_svs_init = true;
+
+	for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
+		mt->svs_banks[i].bank_id = i;
+		mt->svs_banks[i].mt = mt;
+
+		ret = mtk_svs_bank_init(&mt->svs_banks[i]);
+		if (ret) {
+			if (ret == -EPROBE_DEFER)
+				return ret;
+
+			dev_err(&pdev->dev,
+				"failed to initialize mtk svs bank%d\n", i);
+			skip_svs_init = true;
+		}
+	}
+
 	spin_lock_init(&mt->lock);
 
 	mt->dev = &pdev->dev;
@@ -562,6 +1239,33 @@ static int mtk_thermal_probe(struct platform_device *pdev)
 	if (IS_ERR(mt->tzd))
 		goto err_register;
 
+	/*
+	 * SVS engine needs to be initialized after mtk-thermal initialization
+	 * is done and before the initialization of cpufreq driver.
+	 */
+	if (!skip_svs_init) {
+		ret = mtk_svs_init(pdev, mt);
+		if (ret) {
+			/* Keep running system with SVS engine disabled. */
+			dev_err(&pdev->dev,
+				"Failed to initialze MTK SVS engine\n");
+			skip_svs_init = true;
+		}
+	}
+
+	/*
+	 * Release all resources needed by SVS if not all required resources
+	 * are present or SVS initialization failed.
+	 */
+	if (skip_svs_init)
+		mtk_svs_release(mt);
+
+	pdev_ret = platform_device_register_simple("mt8173-cpufreq", -1,
+						   NULL, 0);
+	if (IS_ERR(pdev_ret))
+		dev_err(&pdev->dev,
+			"failed to register mt8173-cpufreq device\n");
+
 	return 0;
 
 err_register:
-- 
1.9.1

Re: [RFC PATCH 4/5] PM / AVS: thermal: MT8173: Introduce support for SVS engine

[Daniel Kurtz]

Hi Pi-Cheng,

Thanks for the patch!

Some review comments inline...

On Fri, Jan 22, 2016 at 12:40 AM, Pi-Cheng Chen
<pi-cheng.chen@linaro.org> wrote:
> The SVS (Smart Voltage Scaling) engine is a piece of hardware which is
> used to caculate optimized voltage values of several power domains, e.g.

calculate

> CPU clusters, according to chip process corner, temperatures, and other
> factors. Then DVFS driver could apply those optimized voltage values to
> reduce power consumption. This engine takes calibration data stored in
> on-chip E-Fuse device as configuration input during initialization. Once
> the initialization is done, SVS engine issues interrupts according to
> temerature changes of power domains to notify DVFS driver to get

temperature

> calculated voltage values.
>
> The configuration registers of SVS engine are shared with Mediatek
> thermal controller, and those registers are banked for different power
> domains. In addition, the SVS engine also needs some information from
> Mediatek thermal controller, e.g. the temperature of a specific power
> domain, part of the thermal calibration data. Therefore the support for
> SVS engine is integrated with Mediatek thermal controller driver.
>
> Also, for platform specific requirement, to make SVS engine work
> correctly, the initialization of SVS engine should be later then
> Mediatek thermal controller, and prior to mt8173-cpufreq driver. Hence,
> the platform device registration code of mt8173-cpufreq is removed here
> after SVS initialization is done or skipped to ensure the platform
> specific initialization flow.
>
> The functionality of SVS engine is optional for Mediatek thermal
> controller. If the required resources of SVS engine is absent or SVS
> failed during initialization stage, the SVS control flow will be
> skipped and the thermal controller will function normally.
>
> CC: Stephen Boyd <sboyd@codeaurora.org>
>
> Signed-off-by: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
> ---
> This patch relies on the runtime voltage adjustment of OPP introduced in CPR
> patchset done by Stephen Boyd[1].
>
> [1] https://lkml.org/lkml/2015/9/18/833
> ---
>  drivers/thermal/mtk_thermal.c | 704 ++++++++++++++++++++++++++++++++++++++++++
>  1 file changed, 704 insertions(+)
>
> diff --git a/drivers/thermal/mtk_thermal.c b/drivers/thermal/mtk_thermal.c
> index f20e784..d8ba9dd 100644
> --- a/drivers/thermal/mtk_thermal.c
> +++ b/drivers/thermal/mtk_thermal.c
> @@ -13,6 +13,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>
> @@ -21,12 +24,15 @@
>  #include <linux/of.h>
>  #include <linux/of_address.h>
>  #include <linux/platform_device.h>
> +#include <linux/pm_opp.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/nvmem-consumer.h>
> +#include <linux/workqueue.h>
>
>  /* AUXADC Registers */
>  #define AUXADC_CON0_V          0x000
> @@ -73,7 +79,32 @@
>
>  #define TEMP_SPARE0            0x0f0
>
> +#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)
>
> @@ -106,6 +137,24 @@
>  /* The number of sensing points per bank */
>  #define MT8173_NUM_SENSORS_PER_ZONE    4
>
> +/* The number of OPPs supported by SVS */
> +#define MT8173_NUM_SVS_OPP             8
> +
> +/* Bit masks of SVS enable and IRQ configrations */
> +#define PHASE_EN_MASK          0x7
> +#define PHASE_0_EN             0x1
> +#define PHASE_CON_EN           0x2
> +#define PHASE_1_EN             0x4
> +#define PHASE_01_EN            (PHASE_0_EN | PHASE_1_EN)
> +#define PHASE_01_IRQ           0x1
> +#define PHASE_CON_IRQ          (0xff << 16)
> +
> +/* The number of SVS banks implemented */
> +#define MT8173_NUM_SVS_BANKS   2
> +
> +#define MT8173_SVS_BANK_CA53   0
> +#define MT8173_SVS_BANK_CA72   1
> +
>  /* Layout of the fuses providing the calibration data */
>  #define MT8173_CALIB_BUF0_VALID                (1 << 0)
>  #define MT8173_CALIB_BUF1_ADC_GE(x)    (((x) >> 22 ) & 0x3ff)
> @@ -116,6 +165,22 @@
>  #define MT8173_CALIB_BUF2_VTS_TSABB(x) (((x) >> 14 ) & 0x1ff)
>  #define MT8173_CALIB_BUF0_DEGC_CALI(x) (((x) >> 1 ) & 0x3f)
>  #define MT8173_CALIB_BUF0_O_SLOPE(x)   (((x) >> 26 ) & 0x3f)
> +#define MT8173_CALIB_BUF0_O_SLOPE_S(x) (((x) >> 7) & 0x1)
> +
> +/* Helpers to calculate configuration values from SVS calibration data */
> +#define SVS_CALIB_VALID        (1)
> +#define BANK_SHIFT(bank) ((bank == 0) ? 8 : 0)
> +#define SVS_CALIB_BANK_CONFIG0(buf, s)                         \
> +       (((((buf[33] >> (s)) & 0xff)) << 8) |                   \
> +       ((buf[32] >> (s)) & 0xff))

Align opening parentheses.

> +#define SVS_CALIB_BANK_CONFIG1(buf, s)                         \
> +       ((((buf[34] >> (s)) & 0xff) << 8) | 0x100006)
> +#define SVS_CALIB_BANK_CONFIG2L(base, s)                       \
> +       ((buf[0] >> (s)) & 0xff)
> +#define SVS_CALIB_BANK_CONFIG2H(base, s)                       \
> +       ((buf[1] >> (s)) & 0xff)
> +#define SVS_CALIB_BANK_CONFIG3(base, s)                                \
> +       (((buf[2] >> (s)) & 0xff) << 8)
>
>  #define THERMAL_NAME    "mtk-thermal"
>
> @@ -126,14 +191,55 @@ struct mtk_thermal_bank {
>         int id;
>  };
>
> +enum svs_state {
> +       SVS_PHASE_0 = 0,
> +       SVS_PHASE_1,
> +       SVS_PHASE_CONTINUOUS,
> +};
> +
> +struct mtk_svs_bank {
> +       int bank_id;
> +       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];
> +       int volt_table[MT8173_NUM_SVS_OPP];
> +       int updated_volt_table[MT8173_NUM_SVS_OPP];
> +};
> +
> +struct mtk_svs_bank_cfg {
> +       const int dev_id;
> +       const int ts;
> +       const int vmin;
> +       const int vmax;
> +       const int vboot;
> +       const unsigned long base_freq;
> +};
> +
>  struct mtk_thermal {
>         struct device *dev;
>         void __iomem *thermal_base;
>
>         struct clk *clk_peri_therm;
>         struct clk *clk_auxadc;
> +       struct clk *svs_mux;
> +       struct clk *svs_pll;
>
>         struct mtk_thermal_bank banks[MT8173_NUM_ZONES];
> +       struct mtk_svs_bank svs_banks[MT8173_NUM_SVS_BANKS];
> +
> +       int svs_irq;
>
>         spinlock_t lock;
>
> @@ -142,6 +248,9 @@ struct mtk_thermal {
>         s32 degc_cali;
>         s32 o_slope;
>         s32 vts[MT8173_NUM_SENSORS];
> +       s32 x_roomt[MT8173_NUM_SENSORS];

AFAICT, this x_roomt isn't used.

> +       s32 bts[MT8173_NUM_ZONES];
> +       s32 mts;

What are "bts" and "mts".
Please use more descriptive names, or at least a comment.

>
>         struct thermal_zone_device *tzd;
>  };
> @@ -198,6 +307,25 @@ static const struct mtk_thermal_sense_point
>         },
>  };
>
> +static const struct mtk_svs_bank_cfg svs_bank_cfgs[MT8173_NUM_SVS_BANKS] = {
> +       [MT8173_SVS_BANK_CA53] = {
> +               .dev_id = 0,
> +               .vmax = 1125,
> +               .vmin = 800,
> +               .vboot = 1000,
> +               .base_freq = 1600000,
> +               .ts = MT8173_TS3
> +       },
> +       [MT8173_SVS_BANK_CA72] = {
> +               .dev_id = 2,
> +               .vmax = 1125,
> +               .vmin = 800,
> +               .vboot = 1000,
> +               .base_freq = 2000000,
> +               .ts = MT8173_TS4
> +       }
> +};
> +
>  /**
>   * raw_to_mcelsius - convert a raw ADC value to mcelsius
>   * @mt:                The thermal controller
> @@ -222,6 +350,34 @@ static int raw_to_mcelsius(struct mtk_thermal *mt, int sensno, s32 raw)
>  }
>
>  /**
> + * mvolt_to_config - convert a voltage value to SVS voltage config value
> + * @mvolt:     voltage value
> + */
> +static inline u32 mvolt_to_config(int mvolt)
> +{
> +       return ((mvolt - 700) * 100 + 625 - 1) / 625;
> +}
> +
> +/**
> + * mvolt_to_config - convert a SVS voltage config value to voltage value
> + * @val:       SVS voltage config value
> + */
> +static inline int config_to_mv(u32 val)
> +{
> +       return (val * 625 / 100) + 700;
> +}
> +
> +/**
> + * khz_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 int khz_to_config(unsigned long rate, unsigned long base_rate)
> +{
> +       return (rate * 100 + base_rate - 1) / base_rate;
> +}
> +
> +/**
>   * mtk_thermal_switch_bank - switch to bank
>   * @bank:      The bank
>   *
> @@ -240,6 +396,46 @@ static void mtk_thermal_switch_bank(struct mtk_thermal_bank *bank)
>  }
>
>  /**
> + * mtk_svs_update_voltage_table - update the calculated voltage table
> + * @svs: The SVS bank
> + * @offset: The voltage offset
> + *
> + * 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, int offset)
> +{
> +       struct mtk_thermal *mt = svs->mt;
> +       int vmin, vmax, *volt_table;
> +       u32 reg;
> +
> +       vmin = svs_bank_cfgs[svs->bank_id].vmin;
> +       vmax = svs_bank_cfgs[svs->bank_id].vmax;
> +       volt_table = svs->updated_volt_table;
> +
> +       reg = readl(mt->thermal_base + SVS_BANK_VOP30);

Please add a comment explaining what are these two registers.

> +       volt_table[0] = clamp(config_to_mv((reg & 0xff) + offset),
> +                             vmin, vmax);
> +       volt_table[1] = clamp(config_to_mv(((reg >> 8) & 0xff) + offset),
> +                             vmin, vmax);
> +       volt_table[2] = clamp(config_to_mv(((reg >> 16) & 0xff) + offset),
> +                             vmin, vmax);
> +       volt_table[3] = clamp(config_to_mv(((reg >> 24) & 0xff) + offset),
> +                             vmin, vmax);
> +
> +       reg = readl(mt->thermal_base + SVS_BANK_VOP74);
> +       volt_table[4] = clamp(config_to_mv((reg & 0xff) + offset),
> +                             vmin, vmax);
> +       volt_table[5] = clamp(config_to_mv(((reg >> 8) & 0xff) + offset),
> +                             vmin, vmax);
> +       volt_table[6] = clamp(config_to_mv(((reg >> 16) & 0xff) + offset),
> +                             vmin, vmax);
> +       volt_table[7] = clamp(config_to_mv(((reg >> 24) & 0xff) + offset),
> +                             vmin, vmax);
> +}
> +
> +/**
>   * mtk_thermal_bank_temperature - get the temperature of a bank
>   * @bank:      The bank
>   *
> @@ -426,6 +622,7 @@ static int mtk_thermal_get_calibration_data(struct device *dev, struct mtk_therm
>         u32 *buf;
>         size_t len;
>         int i, ret = 0;
> +       s32 o_slope_s, ge, oe, gain, x_roomt, temp1, temp2;
>
>         /* Start with default values */
>         mt->adc_ge = 512;
> @@ -433,6 +630,7 @@ static int mtk_thermal_get_calibration_data(struct device *dev, struct mtk_therm
>                 mt->vts[i] = 260;
>         mt->degc_cali = 40;
>         mt->o_slope = 0;
> +       o_slope_s = 0;
>
>         cell = nvmem_cell_get(dev, "calibration-data");
>         if (IS_ERR(cell)) {
> @@ -463,23 +661,464 @@ static int mtk_thermal_get_calibration_data(struct device *dev, struct mtk_therm
>                 mt->vts[MT8173_TSABB] = MT8173_CALIB_BUF2_VTS_TSABB(buf[2]);
>                 mt->degc_cali = MT8173_CALIB_BUF0_DEGC_CALI(buf[0]);
>                 mt->o_slope = MT8173_CALIB_BUF0_O_SLOPE(buf[0]);
> +               o_slope_s = MT8173_CALIB_BUF0_O_SLOPE_S(buf[0]);

Perhaps just do this once, here:
  o_slope_s = MT8173_CALIB_BUF0_O_SLOPE_S(buf[0]);
  mt->o_slope = (o_slope ? -1 : 1) * MT8173_CALIB_BUF0_O_SLOPE(buf[0]);

Don't we need to consider the slope's sign in raw_to_mcelsius(), too?

>         } else {
>                 dev_info(dev, "Device not calibrated, using default calibration values\n");
>         }
>
> +       oe = mt->adc_ge - 512;
> +       ge = oe * 10000 / 4096;
> +       gain = 10000 + ge;
> +
> +       /* calculating MTS */
> +       mt->mts = (o_slope_s == 0) ?
> +                 10000 * 100000 / gain * 15 / 18 / (165 + mt->o_slope) :
> +                 10000 * 100000 / gain * 15 / 18 / (165 - mt->o_slope);

What are all of these magic numbers?
Please define some named constants.

> +
> +       /* 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] + 3350 - oe * 10000 / 4096 * 10000 / gain;
> +               temp1 = (10000 * 100000 / 4096 / gain) *
> +                       ge + x_roomt * 10 * 15 / 18;
> +               temp2 = (o_slope_s == 0) ? temp1 * 10 / (165 + mt->o_slope) :
> +                       temp1 * 10 / (165 - mt->o_slope);
> +               mt->bts[i] = (mt->degc_cali * 10 / 2 + temp2 - 250) * 4 / 10;

This math would be more clear if the temporary variables and constants
had meaningful names.
Also, if you really must do integer division, you probably want to use
DIV_ROUND_UP().

> +       }
> +
>  out:
>         kfree(buf);
>
>         return ret;
>  }
>
> +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;
> +
> +       cell = nvmem_cell_get(dev, "svs-calibration-data");
> +       if (IS_ERR(cell))
> +               return PTR_ERR(cell);
> +
> +       buf = (u32 *)nvmem_cell_read(cell, &len);

No need to explicitly cast from (void *) to (u32 *).

> +       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;

Move the BANK_SHIFT(i) into SVS_CALIB_BANK_*() macros and the
following will be more readable.

> +
> +               mt->svs_banks[i].config0 =
> +                               SVS_CALIB_BANK_CONFIG0(buf, BANK_SHIFT(i));
> +               mt->svs_banks[i].config1 =
> +                               SVS_CALIB_BANK_CONFIG1(buf, BANK_SHIFT(i));
> +               mt->svs_banks[i].config3 =
> +                               SVS_CALIB_BANK_CONFIG3(buf, BANK_SHIFT(i));
> +

Add a comment here to explain what you are doing here for config2.

> +               temp = SVS_CALIB_BANK_CONFIG2H(buf, BANK_SHIFT(i));
> +               if (temp < 128 && i == MT8173_SVS_BANK_CA72)
> +                       temp = (unsigned char)((temp - 256) / 2);
> +               temp = ((temp & 0xff) << 8) |
> +                      SVS_CALIB_BANK_CONFIG2L(buf, BANK_SHIFT(i));
> +               mt->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;
> +       int id = svs->bank_id;
> +
> +       freq_tbl = svs->freq_table;
> +       base_freq = svs_bank_cfgs[id].base_freq;
> +
> +       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(0x555555, mt->thermal_base + SVS_BANK_CONFIG4);
> +       writel(0x555555, mt->thermal_base + SVS_BANK_CONFIG5);
> +       writel(0x80000000, mt->thermal_base + SVS_BANK_CONFIG10);

Define some named constants (macros) for these magic numbers.

> +
> +       writel((khz_to_config(freq_tbl[0], base_freq) & 0xff) |

Just have khz_to_config return a u8 and remove the "& 0xff" here.

> +              ((khz_to_config(freq_tbl[1], base_freq) & 0xff) << 8) |
> +              ((khz_to_config(freq_tbl[2], base_freq) & 0xff) << 16) |
> +              ((khz_to_config(freq_tbl[3], base_freq) & 0xff) << 24),
> +              mt->thermal_base + SVS_BANK_FREQPCT30);

Please add a comment here describing what these two registers are for.

> +
> +       writel((khz_to_config(freq_tbl[4], base_freq) & 0xff) |
> +              ((khz_to_config(freq_tbl[5], base_freq) & 0xff) << 8) |
> +              ((khz_to_config(freq_tbl[6], base_freq) & 0xff) << 16) |
> +              ((khz_to_config(freq_tbl[7], base_freq) & 0xff) << 24),
> +              mt->thermal_base + SVS_BANK_FREQPCT74);
> +

Same thing here for mvolt_to_config().

> +       writel(((mvolt_to_config(svs_bank_cfgs[id].vmax) & 0xff) << 24) |
> +              ((mvolt_to_config(svs_bank_cfgs[id].vmin) & 0xff) << 16) | 0x1fe,

Named constants here for 0x1fe and below...

> +              mt->thermal_base + SVS_BANK_LIMITVALS);
> +
> +       writel(mvolt_to_config(svs_bank_cfgs[id].vboot),
> +              mt->thermal_base + SVS_BANK_CONFIG6);
> +       writel(0xa28, mt->thermal_base + SVS_BANK_CONFIG7);
> +       writel(0xffff, mt->thermal_base + SVS_BANK_CONFIG8);
> +
> +       /* clear all pending interrupt */
> +       writel(0xffffffff, mt->thermal_base + SVS_BANK_INTST);

Why is this necessary?

> +
> +       switch (phase) {
> +       case SVS_PHASE_0:
> +               writel(0x5f01, mt->thermal_base + SVS_BANK_CONTROL3);
> +               writel(PHASE_0_EN, mt->thermal_base + SVS_BANK_EN);
> +               break;
> +       case SVS_PHASE_1:
> +               writel(0x5f01, 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);
> +               break;
> +       case SVS_PHASE_CONTINUOUS:
> +               writel(((mt->bts[id] & 0xfff) << 12) | (mt->mts & 0xfff),
> +                      mt->thermal_base + SVS_BANK_CONFIG9);
> +               writel(0xff0000, mt->thermal_base + SVS_BANK_CONTROL3);
> +               writel(PHASE_CON_EN, mt->thermal_base + SVS_BANK_EN);
> +       }
> +}
> +
> +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;
> +

Don't we need to grab some "whole table" lock before manipulating the
OPP table entries?
Otherwise, someone who is iterating over the entire table might see
something unexpected.

> +               dev_pm_opp_adjust_voltage(svs->dev, svs->freq_table[i] * 1000,
> +                                         svs->updated_volt_table[i] * 1000);
> +       }
> +}
> +
> +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;
> +       unsigned long flags;
> +       int temp, low_temp_offset = 0;
> +
> +       spin_lock_irqsave(&mt->lock, flags);
> +       mtk_thermal_switch_bank(&mt->banks[svs->bank_id]);
> +
> +       temp = mtk_thermal_bank_temperature(&mt->banks[svs->bank_id]);
> +
> +       spin_unlock_irqrestore(&mt->lock, flags);
> +
> +       if (temp <= 33000)
> +               low_temp_offset = 10;

Please define some named constants for this threshold temperature and
the offset voltage.

> +
> +       mtk_svs_update_voltage_table(svs, low_temp_offset);
> +       mtk_svs_adjust_voltage(svs);
> +}
> +
> +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;
> +       unsigned long flags;
> +       u32 svs_intst, bank_en, bank_intst;
> +       int i;
> +
> +       spin_lock_irqsave(&mt->lock, flags);
> +
> +       svs_intst = readl(mt->thermal_base + SVS_SVSINTST);

Do you need to acknowledge/clear these interrupt status bits?

> +       for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
> +               struct mtk_svs_bank *svs = &mt->svs_banks[i];
> +
> +               if ((svs_intst >> i) & 0x1)
> +                       continue;
> +
> +               mtk_thermal_switch_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) {
> +                       /*
> +                        * The cpufreq driver won't work during SVS
> +                        * initialization stage, so it's safe to update OPP
> +                        * table here since it won't trigger regulator
> +                        * operation.
> +                        */
> +                       mtk_svs_update_voltage_table(svs, 0);

Shouldn't this pass in the current temperature to apply the "low
temperature offset"?

> +                       mtk_svs_adjust_voltage(svs);
> +
> +                       writel(0, mt->thermal_base + SVS_BANK_EN);
> +                       writel(PHASE_01_IRQ, mt->thermal_base + SVS_BANK_INTST);
> +
> +                       complete(&svs->init_done);
> +
> +                       mtk_svs_set_phase(svs, SVS_PHASE_CONTINUOUS);
> +               } else if (bank_intst && PHASE_CON_IRQ) { /* phase continuous*/

What triggers PHASE_CON_IRQ interrupts?

> +                       /*
> +                        * 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 {
> +                       mtk_svs_bank_disable(svs);
> +                       dev_err(mt->svs_banks[i].dev,
> +                               "SVS engine internal error. disabled.\n");
> +
> +                       /*
> +                        * Schedule a work to reset voltages of OPP table
> +                        * entries.
> +                        */
> +                       schedule_work(&svs->work);

You'll want to return IRQ_NONE in this case to notify the spurious irq detector.

> +               }
> +       }
> +
> +       spin_unlock_irqrestore(&mt->lock, flags);
> +
> +       return IRQ_HANDLED;
> +}

Nothing in this interrupt handler looks very time critical.
Can we use a threaded interrupt instead, and continue to use a mutex
for mt->lock.
We could also then just call adjust_voltage_work() directly rather
than indirectly through the work.

> +
> +static int mtk_svs_bank_init(struct mtk_svs_bank *svs)
> +{
> +       struct dev_pm_opp *opp;
> +       cpumask_t cpus;
> +       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_bank_cfgs[svs->bank_id].dev_id);
> +       if (!svs->dev) {
> +               pr_err("failed to get cpu%d device\n",
> +                      svs_bank_cfgs[svs->bank_id].dev_id);
> +               return -ENODEV;
> +       }
> +
> +       svs->reg = regulator_get_exclusive(svs->dev, "proc");
> +       if (IS_ERR(svs->reg)) {
> +               dev_err(svs->dev, "failed to get proc regulator\n");
> +               return PTR_ERR(svs->reg);
> +       }
> +
> +       ret = dev_pm_opp_of_get_sharing_cpus(svs->dev, &cpus);
> +       if (ret) {
> +               dev_err(svs->dev, "failed to get OPP-sharing information\n");
> +               return ret;
> +       }
> +
> +       ret = dev_pm_opp_of_cpumask_add_table(&cpus);
> +       if (ret) {
> +               dev_err(svs->dev, "no OPP table\n");
> +               return ret;
> +       }
> +
> +       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 / 1000;
> +               svs->volt_table[i] = dev_pm_opp_get_voltage(opp) / 1000;

Why / 1000?
Just save the original values.  This will keep us from having to * 1000 later.

> +       }
> +
> +out:
> +       rcu_read_unlock();
> +       if (ret) {
> +               regulator_put(svs->reg);
> +               dev_pm_opp_of_cpumask_remove_table(&cpus);
> +       }
> +
> +       return ret;
> +}
> +
> +static void mtk_svs_release(struct mtk_thermal *mt)
> +{
> +       int i, ret;
> +       cpumask_t cpus;
> +
> +       if (!IS_ERR(mt->svs_pll))
> +               clk_put(mt->svs_pll);

devm_clk_put(), devm_free_irq() & devm_regulator_put in this function.

> +
> +       if (!IS_ERR(mt->svs_mux))
> +               clk_put(mt->svs_mux);
> +
> +       if (mt->svs_irq != -1)
> +               free_irq(mt->svs_irq, mt);
> +
> +       for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
> +               struct mtk_svs_bank *svs = &mt->svs_banks[i];
> +
> +               ret = dev_pm_opp_of_get_sharing_cpus(svs->dev, &cpus);
> +               if (!ret)
> +                       dev_pm_opp_of_cpumask_remove_table(&cpus);
> +
> +               if (svs->reg)
> +                       regulator_put(svs->reg);
> +       }
> +}
> +
> +static int mtk_svs_init(struct platform_device *pdev, struct mtk_thermal *mt)
> +{
> +       struct clk *parent;
> +       int i, ret, vboot;
> +       unsigned long flags, timeout;
> +       struct mtk_svs_bank *svs;
> +
> +       parent = clk_get_parent(mt->svs_mux);
> +       ret = clk_set_parent(mt->svs_mux, mt->svs_pll);
> +       if (ret) {
> +               dev_err(&pdev->dev,
> +                       "failed to set svs_mux to svs_pll\n");
> +               return ret;
> +       }
> +
> +       for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
> +               svs = &mt->svs_banks[i];
> +

Why do you do this boot voltage check?
Doesn't this depend on the specific OPP used by firmware, which is
outside the control of the kernel?

> +               vboot = regulator_get_voltage(svs->reg) / 1000;
> +               if (mvolt_to_config(vboot) !=
> +                   mvolt_to_config(svs_bank_cfgs[i].vboot)) {
> +                       dev_err(svs->dev, "Vboot value mismatch!\n");
> +                       ret = -EINVAL;
> +                       break;
> +               }
> +
> +               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;
> +               }

Shouldn't this be best effort?
If we can't set the regulator to fast mode, can we continue anyway?

> +
> +               spin_lock_irqsave(&mt->lock, flags);
> +               mtk_thermal_switch_bank(&mt->banks[i]);
> +
> +               mtk_svs_set_phase(svs, SVS_PHASE_0);
> +
> +               spin_unlock_irqrestore(&mt->lock, flags);
> +
> +               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;
> +               }
> +
> +               ret = regulator_set_mode(svs->reg, REGULATOR_MODE_NORMAL);
> +               if (ret)
> +                       dev_err(svs->dev,
> +                               "Failed to set regulator in normal mode\n");
> +
> +               regulator_put(svs->reg);
> +               svs->reg = NULL;
> +       }
> +
> +err_bank_init:
> +
> +       if (ret)
> +               for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
> +                       svs = &mt->svs_banks[i];
> +
> +                       spin_lock_irqsave(&mt->lock, flags);
> +                       mtk_thermal_switch_bank(&mt->banks[i]);
> +
> +                       mtk_svs_bank_disable(svs);
> +
> +                       spin_unlock_irqrestore(&mt->lock, flags);
> +
> +                       mtk_svs_adjust_voltage(svs);
> +               }
> +
> +       ret = clk_set_parent(mt->svs_mux, parent);
> +       if (ret) {
> +               dev_err(&pdev->dev,
> +                       "failed to set svs_mux to original parent\n");
> +               return ret;
> +       }
> +
> +       return ret;
> +}
> +
>  static int mtk_thermal_probe(struct platform_device *pdev)
>  {
>         int ret, i;
>         struct device_node *auxadc, *apmixedsys, *np = pdev->dev.of_node;
>         struct mtk_thermal *mt;
>         struct resource *res;
> +       struct platform_device *pdev_ret;
>         u64 auxadc_phys_base, apmixed_phys_base;
> +       bool skip_svs_init = false;
>
>         mt = devm_kzalloc(&pdev->dev, sizeof(*mt), GFP_KERNEL);
>         if (!mt)
> @@ -493,6 +1132,23 @@ static int mtk_thermal_probe(struct platform_device *pdev)
>         if (IS_ERR(mt->clk_auxadc))
>                 return PTR_ERR(mt->clk_auxadc);
>
> +       /*
> +        * These two clks are optional for mtk-thermal. If present, SVS engine
> +        * will be activated.
> +        */
> +       mt->svs_pll = devm_clk_get(&pdev->dev, "svs_pll");
> +       mt->svs_mux = devm_clk_get(&pdev->dev, "svs_mux");
> +       if (IS_ERR(mt->svs_pll) || IS_ERR(mt->svs_mux))
> +               skip_svs_init = true;
> +
> +       mt->svs_irq = platform_get_irq(pdev, 1);
> +       ret = devm_request_irq(&pdev->dev, mt->svs_irq, mtk_svs_interrupt,
> +                              IRQF_TRIGGER_LOW, "mtk-svs", mt);
> +       if (ret) {
> +               mt->svs_irq = -1;
> +               skip_svs_init = true;
> +       }
> +
>         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
>         mt->thermal_base = devm_ioremap_resource(&pdev->dev, res);
>         if (IS_ERR(mt->thermal_base))
> @@ -502,6 +1158,27 @@ static int mtk_thermal_probe(struct platform_device *pdev)
>         if (ret)
>                 return ret;
>
> +       ret = mtk_svs_get_calibration_data(&pdev->dev, mt);
> +       if (ret == -EPROBE_DEFER)
> +               return ret;
> +       else if (ret)
> +               skip_svs_init = true;
> +
> +       for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
> +               mt->svs_banks[i].bank_id = i;
> +               mt->svs_banks[i].mt = mt;
> +
> +               ret = mtk_svs_bank_init(&mt->svs_banks[i]);
> +               if (ret) {
> +                       if (ret == -EPROBE_DEFER)
> +                               return ret;
> +
> +                       dev_err(&pdev->dev,
> +                               "failed to initialize mtk svs bank%d\n", i);
> +                       skip_svs_init = true;
> +               }
> +       }
> +
>         spin_lock_init(&mt->lock);
>
>         mt->dev = &pdev->dev;
> @@ -562,6 +1239,33 @@ static int mtk_thermal_probe(struct platform_device *pdev)
>         if (IS_ERR(mt->tzd))
>                 goto err_register;
>
> +       /*
> +        * SVS engine needs to be initialized after mtk-thermal initialization
> +        * is done and before the initialization of cpufreq driver.
> +        */
> +       if (!skip_svs_init) {
> +               ret = mtk_svs_init(pdev, mt);
> +               if (ret) {
> +                       /* Keep running system with SVS engine disabled. */
> +                       dev_err(&pdev->dev,
> +                               "Failed to initialze MTK SVS engine\n");
> +                       skip_svs_init = true;
> +               }
> +       }
> +
> +       /*
> +        * Release all resources needed by SVS if not all required resources
> +        * are present or SVS initialization failed.
> +        */
> +       if (skip_svs_init)
> +               mtk_svs_release(mt);
> +
> +       pdev_ret = platform_device_register_simple("mt8173-cpufreq", -1,
> +                                                  NULL, 0);
> +       if (IS_ERR(pdev_ret))
> +               dev_err(&pdev->dev,
> +                       "failed to register mt8173-cpufreq device\n");
> +
>         return 0;
>
>  err_register:
> --
> 1.9.1
>
>
> _______________________________________________
> Linux-mediatek mailing list
> Linux-mediatek@lists.infradead.org
> http://lists.infradead.org/mailman/listinfo/linux-mediatek

Re: [RFC PATCH 4/5] PM / AVS: thermal: MT8173: Introduce support for SVS engine

[Pi-Cheng Chen]

Hi Daniel,

Thanks for review.


On Fri, Jan 22, 2016 at 03:38:06PM -0800, Daniel Kurtz wrote:
> Hi Pi-Cheng,
> 
> Thanks for the patch!
> 
> Some review comments inline...
> 
> On Fri, Jan 22, 2016 at 12:40 AM, Pi-Cheng Chen
> <pi-cheng.chen@linaro.org> wrote:
> > The SVS (Smart Voltage Scaling) engine is a piece of hardware which is
> > used to caculate optimized voltage values of several power domains, e.g.
> 
> calculate
> 

Will fix it.

> > CPU clusters, according to chip process corner, temperatures, and other
> > factors. Then DVFS driver could apply those optimized voltage values to
> > reduce power consumption. This engine takes calibration data stored in
> > on-chip E-Fuse device as configuration input during initialization. Once
> > the initialization is done, SVS engine issues interrupts according to
> > temerature changes of power domains to notify DVFS driver to get
> 
> temperature
> 

Will fix it.

> > calculated voltage values.
> >
> > The configuration registers of SVS engine are shared with Mediatek
> > thermal controller, and those registers are banked for different power
> > domains. In addition, the SVS engine also needs some information from
> > Mediatek thermal controller, e.g. the temperature of a specific power
> > domain, part of the thermal calibration data. Therefore the support for
> > SVS engine is integrated with Mediatek thermal controller driver.
> >
> > Also, for platform specific requirement, to make SVS engine work
> > correctly, the initialization of SVS engine should be later then
> > Mediatek thermal controller, and prior to mt8173-cpufreq driver. Hence,
> > the platform device registration code of mt8173-cpufreq is removed here
> > after SVS initialization is done or skipped to ensure the platform
> > specific initialization flow.
> >
> > The functionality of SVS engine is optional for Mediatek thermal
> > controller. If the required resources of SVS engine is absent or SVS
> > failed during initialization stage, the SVS control flow will be
> > skipped and the thermal controller will function normally.
> >
> > CC: Stephen Boyd <sboyd@codeaurora.org>
> >
> > Signed-off-by: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
> > ---
> > This patch relies on the runtime voltage adjustment of OPP introduced in CPR
> > patchset done by Stephen Boyd[1].
> >
> > [1] https://lkml.org/lkml/2015/9/18/833
> > ---
> >  drivers/thermal/mtk_thermal.c | 704 ++++++++++++++++++++++++++++++++++++++++++
> >  1 file changed, 704 insertions(+)
> >
> > diff --git a/drivers/thermal/mtk_thermal.c b/drivers/thermal/mtk_thermal.c
> > index f20e784..d8ba9dd 100644
> > --- a/drivers/thermal/mtk_thermal.c
> > +++ b/drivers/thermal/mtk_thermal.c
> > @@ -13,6 +13,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>
> > @@ -21,12 +24,15 @@
> >  #include <linux/of.h>
> >  #include <linux/of_address.h>
> >  #include <linux/platform_device.h>
> > +#include <linux/pm_opp.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/nvmem-consumer.h>
> > +#include <linux/workqueue.h>
> >
> >  /* AUXADC Registers */
> >  #define AUXADC_CON0_V          0x000
> > @@ -73,7 +79,32 @@
> >
> >  #define TEMP_SPARE0            0x0f0
> >
> > +#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)
> >
> > @@ -106,6 +137,24 @@
> >  /* The number of sensing points per bank */
> >  #define MT8173_NUM_SENSORS_PER_ZONE    4
> >
> > +/* The number of OPPs supported by SVS */
> > +#define MT8173_NUM_SVS_OPP             8
> > +
> > +/* Bit masks of SVS enable and IRQ configrations */
> > +#define PHASE_EN_MASK          0x7
> > +#define PHASE_0_EN             0x1
> > +#define PHASE_CON_EN           0x2
> > +#define PHASE_1_EN             0x4
> > +#define PHASE_01_EN            (PHASE_0_EN | PHASE_1_EN)
> > +#define PHASE_01_IRQ           0x1
> > +#define PHASE_CON_IRQ          (0xff << 16)
> > +
> > +/* The number of SVS banks implemented */
> > +#define MT8173_NUM_SVS_BANKS   2
> > +
> > +#define MT8173_SVS_BANK_CA53   0
> > +#define MT8173_SVS_BANK_CA72   1
> > +
> >  /* Layout of the fuses providing the calibration data */
> >  #define MT8173_CALIB_BUF0_VALID                (1 << 0)
> >  #define MT8173_CALIB_BUF1_ADC_GE(x)    (((x) >> 22 ) & 0x3ff)
> > @@ -116,6 +165,22 @@
> >  #define MT8173_CALIB_BUF2_VTS_TSABB(x) (((x) >> 14 ) & 0x1ff)
> >  #define MT8173_CALIB_BUF0_DEGC_CALI(x) (((x) >> 1 ) & 0x3f)
> >  #define MT8173_CALIB_BUF0_O_SLOPE(x)   (((x) >> 26 ) & 0x3f)
> > +#define MT8173_CALIB_BUF0_O_SLOPE_S(x) (((x) >> 7) & 0x1)
> > +
> > +/* Helpers to calculate configuration values from SVS calibration data */
> > +#define SVS_CALIB_VALID        (1)
> > +#define BANK_SHIFT(bank) ((bank == 0) ? 8 : 0)
> > +#define SVS_CALIB_BANK_CONFIG0(buf, s)                         \
> > +       (((((buf[33] >> (s)) & 0xff)) << 8) |                   \
> > +       ((buf[32] >> (s)) & 0xff))
> 
> Align opening parentheses.
> 

Will fix it.

> > +#define SVS_CALIB_BANK_CONFIG1(buf, s)                         \
> > +       ((((buf[34] >> (s)) & 0xff) << 8) | 0x100006)
> > +#define SVS_CALIB_BANK_CONFIG2L(base, s)                       \
> > +       ((buf[0] >> (s)) & 0xff)
> > +#define SVS_CALIB_BANK_CONFIG2H(base, s)                       \
> > +       ((buf[1] >> (s)) & 0xff)
> > +#define SVS_CALIB_BANK_CONFIG3(base, s)                                \
> > +       (((buf[2] >> (s)) & 0xff) << 8)
> >
> >  #define THERMAL_NAME    "mtk-thermal"
> >
> > @@ -126,14 +191,55 @@ struct mtk_thermal_bank {
> >         int id;
> >  };
> >
> > +enum svs_state {
> > +       SVS_PHASE_0 = 0,
> > +       SVS_PHASE_1,
> > +       SVS_PHASE_CONTINUOUS,
> > +};
> > +
> > +struct mtk_svs_bank {
> > +       int bank_id;
> > +       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];
> > +       int volt_table[MT8173_NUM_SVS_OPP];
> > +       int updated_volt_table[MT8173_NUM_SVS_OPP];
> > +};
> > +
> > +struct mtk_svs_bank_cfg {
> > +       const int dev_id;
> > +       const int ts;
> > +       const int vmin;
> > +       const int vmax;
> > +       const int vboot;
> > +       const unsigned long base_freq;
> > +};
> > +
> >  struct mtk_thermal {
> >         struct device *dev;
> >         void __iomem *thermal_base;
> >
> >         struct clk *clk_peri_therm;
> >         struct clk *clk_auxadc;
> > +       struct clk *svs_mux;
> > +       struct clk *svs_pll;
> >
> >         struct mtk_thermal_bank banks[MT8173_NUM_ZONES];
> > +       struct mtk_svs_bank svs_banks[MT8173_NUM_SVS_BANKS];
> > +
> > +       int svs_irq;
> >
> >         spinlock_t lock;
> >
> > @@ -142,6 +248,9 @@ struct mtk_thermal {
> >         s32 degc_cali;
> >         s32 o_slope;
> >         s32 vts[MT8173_NUM_SENSORS];
> > +       s32 x_roomt[MT8173_NUM_SENSORS];
> 
> AFAICT, this x_roomt isn't used.
> 

Will remove it.

> > +       s32 bts[MT8173_NUM_ZONES];
> > +       s32 mts;
> 
> What are "bts" and "mts".
> Please use more descriptive names, or at least a comment.
> 

Both these values are used to calculate temperature of blocks inside SoC by SVS
hardware:

Block Temperature = MTS * Juntion ADC value + BTS

I will add comments for them.

> >
> >         struct thermal_zone_device *tzd;
> >  };
> > @@ -198,6 +307,25 @@ static const struct mtk_thermal_sense_point
> >         },
> >  };
> >
> > +static const struct mtk_svs_bank_cfg svs_bank_cfgs[MT8173_NUM_SVS_BANKS] = {
> > +       [MT8173_SVS_BANK_CA53] = {
> > +               .dev_id = 0,
> > +               .vmax = 1125,
> > +               .vmin = 800,
> > +               .vboot = 1000,
> > +               .base_freq = 1600000,
> > +               .ts = MT8173_TS3
> > +       },
> > +       [MT8173_SVS_BANK_CA72] = {
> > +               .dev_id = 2,
> > +               .vmax = 1125,
> > +               .vmin = 800,
> > +               .vboot = 1000,
> > +               .base_freq = 2000000,
> > +               .ts = MT8173_TS4
> > +       }
> > +};
> > +
> >  /**
> >   * raw_to_mcelsius - convert a raw ADC value to mcelsius
> >   * @mt:                The thermal controller
> > @@ -222,6 +350,34 @@ static int raw_to_mcelsius(struct mtk_thermal *mt, int sensno, s32 raw)
> >  }
> >
> >  /**
> > + * mvolt_to_config - convert a voltage value to SVS voltage config value
> > + * @mvolt:     voltage value
> > + */
> > +static inline u32 mvolt_to_config(int mvolt)
> > +{
> > +       return ((mvolt - 700) * 100 + 625 - 1) / 625;
> > +}
> > +
> > +/**
> > + * mvolt_to_config - convert a SVS voltage config value to voltage value
> > + * @val:       SVS voltage config value
> > + */
> > +static inline int config_to_mv(u32 val)
> > +{
> > +       return (val * 625 / 100) + 700;
> > +}
> > +
> > +/**
> > + * khz_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 int khz_to_config(unsigned long rate, unsigned long base_rate)
> > +{
> > +       return (rate * 100 + base_rate - 1) / base_rate;
> > +}
> > +
> > +/**
> >   * mtk_thermal_switch_bank - switch to bank
> >   * @bank:      The bank
> >   *
> > @@ -240,6 +396,46 @@ static void mtk_thermal_switch_bank(struct mtk_thermal_bank *bank)
> >  }
> >
> >  /**
> > + * mtk_svs_update_voltage_table - update the calculated voltage table
> > + * @svs: The SVS bank
> > + * @offset: The voltage offset
> > + *
> > + * 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, int offset)
> > +{
> > +       struct mtk_thermal *mt = svs->mt;
> > +       int vmin, vmax, *volt_table;
> > +       u32 reg;
> > +
> > +       vmin = svs_bank_cfgs[svs->bank_id].vmin;
> > +       vmax = svs_bank_cfgs[svs->bank_id].vmax;
> > +       volt_table = svs->updated_volt_table;
> > +
> > +       reg = readl(mt->thermal_base + SVS_BANK_VOP30);
> 
> Please add a comment explaining what are these two registers.
> 

These two registers report the updated voltage values caculated by SVS hardware.
SVS_BANK_VOP30 reports updated values for OPP[0:3] while SVS_BANK_VOP74 for
OPP[4:7]
I will add comments.

> > +       volt_table[0] = clamp(config_to_mv((reg & 0xff) + offset),
> > +                             vmin, vmax);
> > +       volt_table[1] = clamp(config_to_mv(((reg >> 8) & 0xff) + offset),
> > +                             vmin, vmax);
> > +       volt_table[2] = clamp(config_to_mv(((reg >> 16) & 0xff) + offset),
> > +                             vmin, vmax);
> > +       volt_table[3] = clamp(config_to_mv(((reg >> 24) & 0xff) + offset),
> > +                             vmin, vmax);
> > +
> > +       reg = readl(mt->thermal_base + SVS_BANK_VOP74);
> > +       volt_table[4] = clamp(config_to_mv((reg & 0xff) + offset),
> > +                             vmin, vmax);
> > +       volt_table[5] = clamp(config_to_mv(((reg >> 8) & 0xff) + offset),
> > +                             vmin, vmax);
> > +       volt_table[6] = clamp(config_to_mv(((reg >> 16) & 0xff) + offset),
> > +                             vmin, vmax);
> > +       volt_table[7] = clamp(config_to_mv(((reg >> 24) & 0xff) + offset),
> > +                             vmin, vmax);
> > +}
> > +
> > +/**
> >   * mtk_thermal_bank_temperature - get the temperature of a bank
> >   * @bank:      The bank
> >   *
> > @@ -426,6 +622,7 @@ static int mtk_thermal_get_calibration_data(struct device *dev, struct mtk_therm
> >         u32 *buf;
> >         size_t len;
> >         int i, ret = 0;
> > +       s32 o_slope_s, ge, oe, gain, x_roomt, temp1, temp2;
> >
> >         /* Start with default values */
> >         mt->adc_ge = 512;
> > @@ -433,6 +630,7 @@ static int mtk_thermal_get_calibration_data(struct device *dev, struct mtk_therm
> >                 mt->vts[i] = 260;
> >         mt->degc_cali = 40;
> >         mt->o_slope = 0;
> > +       o_slope_s = 0;
> >
> >         cell = nvmem_cell_get(dev, "calibration-data");
> >         if (IS_ERR(cell)) {
> > @@ -463,23 +661,464 @@ static int mtk_thermal_get_calibration_data(struct device *dev, struct mtk_therm
> >                 mt->vts[MT8173_TSABB] = MT8173_CALIB_BUF2_VTS_TSABB(buf[2]);
> >                 mt->degc_cali = MT8173_CALIB_BUF0_DEGC_CALI(buf[0]);
> >                 mt->o_slope = MT8173_CALIB_BUF0_O_SLOPE(buf[0]);
> > +               o_slope_s = MT8173_CALIB_BUF0_O_SLOPE_S(buf[0]);
> 
> Perhaps just do this once, here:
>   o_slope_s = MT8173_CALIB_BUF0_O_SLOPE_S(buf[0]);
>   mt->o_slope = (o_slope ? -1 : 1) * MT8173_CALIB_BUF0_O_SLOPE(buf[0]);
> 
> Don't we need to consider the slope's sign in raw_to_mcelsius(), too?
> 

Not sure about it.

@Dawei,
Would you please comment it?

> >         } else {
> >                 dev_info(dev, "Device not calibrated, using default calibration values\n");
> >         }
> >
> > +       oe = mt->adc_ge - 512;
> > +       ge = oe * 10000 / 4096;
> > +       gain = 10000 + ge;
> > +
> > +       /* calculating MTS */
> > +       mt->mts = (o_slope_s == 0) ?
> > +                 10000 * 100000 / gain * 15 / 18 / (165 + mt->o_slope) :
> > +                 10000 * 100000 / gain * 15 / 18 / (165 - mt->o_slope);
> 
> What are all of these magic numbers?
> Please define some named constants.
> 

Will do it.

> > +
> > +       /* 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] + 3350 - oe * 10000 / 4096 * 10000 / gain;
> > +               temp1 = (10000 * 100000 / 4096 / gain) *
> > +                       ge + x_roomt * 10 * 15 / 18;
> > +               temp2 = (o_slope_s == 0) ? temp1 * 10 / (165 + mt->o_slope) :
> > +                       temp1 * 10 / (165 - mt->o_slope);
> > +               mt->bts[i] = (mt->degc_cali * 10 / 2 + temp2 - 250) * 4 / 10;
> 
> This math would be more clear if the temporary variables and constants
> had meaningful names.
> Also, if you really must do integer division, you probably want to use
> DIV_ROUND_UP().
> 

Will try to make this block more readable.

> > +       }
> > +
> >  out:
> >         kfree(buf);
> >
> >         return ret;
> >  }
> >
> > +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;
> > +
> > +       cell = nvmem_cell_get(dev, "svs-calibration-data");
> > +       if (IS_ERR(cell))
> > +               return PTR_ERR(cell);
> > +
> > +       buf = (u32 *)nvmem_cell_read(cell, &len);
> 
> No need to explicitly cast from (void *) to (u32 *).
> 

Will remove the cast.

> > +       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;
> 
> Move the BANK_SHIFT(i) into SVS_CALIB_BANK_*() macros and the
> following will be more readable.
> 

Will do it.

> > +
> > +               mt->svs_banks[i].config0 =
> > +                               SVS_CALIB_BANK_CONFIG0(buf, BANK_SHIFT(i));
> > +               mt->svs_banks[i].config1 =
> > +                               SVS_CALIB_BANK_CONFIG1(buf, BANK_SHIFT(i));
> > +               mt->svs_banks[i].config3 =
> > +                               SVS_CALIB_BANK_CONFIG3(buf, BANK_SHIFT(i));
> > +
> 
> Add a comment here to explain what you are doing here for config2.
> 

This is a workaround to handle calibration value overflow. Another piece of
code to handle this should be in mtk_svs_set_phase() is missed. I will add it.

> > +               temp = SVS_CALIB_BANK_CONFIG2H(buf, BANK_SHIFT(i));
> > +               if (temp < 128 && i == MT8173_SVS_BANK_CA72)
> > +                       temp = (unsigned char)((temp - 256) / 2);
> > +               temp = ((temp & 0xff) << 8) |
> > +                      SVS_CALIB_BANK_CONFIG2L(buf, BANK_SHIFT(i));
> > +               mt->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;
> > +       int id = svs->bank_id;
> > +
> > +       freq_tbl = svs->freq_table;
> > +       base_freq = svs_bank_cfgs[id].base_freq;
> > +
> > +       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(0x555555, mt->thermal_base + SVS_BANK_CONFIG4);
> > +       writel(0x555555, mt->thermal_base + SVS_BANK_CONFIG5);
> > +       writel(0x80000000, mt->thermal_base + SVS_BANK_CONFIG10);
> 
> Define some named constants (macros) for these magic numbers.
> 

Will do it.

> > +
> > +       writel((khz_to_config(freq_tbl[0], base_freq) & 0xff) |
> 
> Just have khz_to_config return a u8 and remove the "& 0xff" here.
> 

Will do it.

> > +              ((khz_to_config(freq_tbl[1], base_freq) & 0xff) << 8) |
> > +              ((khz_to_config(freq_tbl[2], base_freq) & 0xff) << 16) |
> > +              ((khz_to_config(freq_tbl[3], base_freq) & 0xff) << 24),
> > +              mt->thermal_base + SVS_BANK_FREQPCT30);
> 
> Please add a comment here describing what these two registers are for.
> 

These two registers store the frequency percentage value of each OPP for SVS.

Frequency Percentage = Frequency / Max Frequency of this bank

Will add comments.

> > +
> > +       writel((khz_to_config(freq_tbl[4], base_freq) & 0xff) |
> > +              ((khz_to_config(freq_tbl[5], base_freq) & 0xff) << 8) |
> > +              ((khz_to_config(freq_tbl[6], base_freq) & 0xff) << 16) |
> > +              ((khz_to_config(freq_tbl[7], base_freq) & 0xff) << 24),
> > +              mt->thermal_base + SVS_BANK_FREQPCT74);
> > +
> 
> Same thing here for mvolt_to_config().
> 

Will do it.

> > +       writel(((mvolt_to_config(svs_bank_cfgs[id].vmax) & 0xff) << 24) |
> > +              ((mvolt_to_config(svs_bank_cfgs[id].vmin) & 0xff) << 16) | 0x1fe,
> 
> Named constants here for 0x1fe and below...
> 

Will do it.

> > +              mt->thermal_base + SVS_BANK_LIMITVALS);
> > +
> > +       writel(mvolt_to_config(svs_bank_cfgs[id].vboot),
> > +              mt->thermal_base + SVS_BANK_CONFIG6);
> > +       writel(0xa28, mt->thermal_base + SVS_BANK_CONFIG7);
> > +       writel(0xffff, mt->thermal_base + SVS_BANK_CONFIG8);
> > +
> > +       /* clear all pending interrupt */
> > +       writel(0xffffffff, mt->thermal_base + SVS_BANK_INTST);
> 
> Why is this necessary?
> 

It's the program flow provided in house solution code.
I will check if we really need it or not.

> > +
> > +       switch (phase) {
> > +       case SVS_PHASE_0:
> > +               writel(0x5f01, mt->thermal_base + SVS_BANK_CONTROL3);
> > +               writel(PHASE_0_EN, mt->thermal_base + SVS_BANK_EN);
> > +               break;
> > +       case SVS_PHASE_1:
> > +               writel(0x5f01, 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);
> > +               break;
> > +       case SVS_PHASE_CONTINUOUS:
> > +               writel(((mt->bts[id] & 0xfff) << 12) | (mt->mts & 0xfff),
> > +                      mt->thermal_base + SVS_BANK_CONFIG9);
> > +               writel(0xff0000, mt->thermal_base + SVS_BANK_CONTROL3);
> > +               writel(PHASE_CON_EN, mt->thermal_base + SVS_BANK_EN);
> > +       }
> > +}
> > +
> > +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;
> > +
> 
> Don't we need to grab some "whole table" lock before manipulating the
> OPP table entries?
> Otherwise, someone who is iterating over the entire table might see
> something unexpected.
> 

I think ne one else will access this table during this stage. It should be safe
to read it without locking.

> > +               dev_pm_opp_adjust_voltage(svs->dev, svs->freq_table[i] * 1000,
> > +                                         svs->updated_volt_table[i] * 1000);
> > +       }
> > +}
> > +
> > +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;
> > +       unsigned long flags;
> > +       int temp, low_temp_offset = 0;
> > +
> > +       spin_lock_irqsave(&mt->lock, flags);
> > +       mtk_thermal_switch_bank(&mt->banks[svs->bank_id]);
> > +
> > +       temp = mtk_thermal_bank_temperature(&mt->banks[svs->bank_id]);
> > +
> > +       spin_unlock_irqrestore(&mt->lock, flags);
> > +
> > +       if (temp <= 33000)
> > +               low_temp_offset = 10;
> 
> Please define some named constants for this threshold temperature and
> the offset voltage.
> 

Will do it.

> > +
> > +       mtk_svs_update_voltage_table(svs, low_temp_offset);
> > +       mtk_svs_adjust_voltage(svs);
> > +}
> > +
> > +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;
> > +       unsigned long flags;
> > +       u32 svs_intst, bank_en, bank_intst;
> > +       int i;
> > +
> > +       spin_lock_irqsave(&mt->lock, flags);
> > +
> > +       svs_intst = readl(mt->thermal_base + SVS_SVSINTST);
> 
> Do you need to acknowledge/clear these interrupt status bits?
> 

It should be cleared after per-bank status are all cleared.

> > +       for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
> > +               struct mtk_svs_bank *svs = &mt->svs_banks[i];
> > +
> > +               if ((svs_intst >> i) & 0x1)
> > +                       continue;
> > +
> > +               mtk_thermal_switch_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) {
> > +                       /*
> > +                        * The cpufreq driver won't work during SVS
> > +                        * initialization stage, so it's safe to update OPP
> > +                        * table here since it won't trigger regulator
> > +                        * operation.
> > +                        */
> > +                       mtk_svs_update_voltage_table(svs, 0);
> 
> Shouldn't this pass in the current temperature to apply the "low
> temperature offset"?
> 

The temperature factor is not taken into consideration in this stage and is
only considered in "continuous phase". So no need to apply the low temperature
offset here.

> > +                       mtk_svs_adjust_voltage(svs);
> > +
> > +                       writel(0, mt->thermal_base + SVS_BANK_EN);
> > +                       writel(PHASE_01_IRQ, mt->thermal_base + SVS_BANK_INTST);
> > +
> > +                       complete(&svs->init_done);
> > +
> > +                       mtk_svs_set_phase(svs, SVS_PHASE_CONTINUOUS);
> > +               } else if (bank_intst && PHASE_CON_IRQ) { /* phase continuous*/
> 
> What triggers PHASE_CON_IRQ interrupts?
> 

After we do mtk_svs_set_phase(svs, SVS_PHASE_CONTINUOUS) above.

> > +                       /*
> > +                        * 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 {
> > +                       mtk_svs_bank_disable(svs);
> > +                       dev_err(mt->svs_banks[i].dev,
> > +                               "SVS engine internal error. disabled.\n");
> > +
> > +                       /*
> > +                        * Schedule a work to reset voltages of OPP table
> > +                        * entries.
> > +                        */
> > +                       schedule_work(&svs->work);
> 
> You'll want to return IRQ_NONE in this case to notify the spurious irq detector.
> 

Will do it.

> > +               }
> > +       }
> > +
> > +       spin_unlock_irqrestore(&mt->lock, flags);
> > +
> > +       return IRQ_HANDLED;
> > +}
> 
> Nothing in this interrupt handler looks very time critical.
> Can we use a threaded interrupt instead, and continue to use a mutex
> for mt->lock.
> We could also then just call adjust_voltage_work() directly rather
> than indirectly through the work.
> 

Yes. I will try it.

> > +
> > +static int mtk_svs_bank_init(struct mtk_svs_bank *svs)
> > +{
> > +       struct dev_pm_opp *opp;
> > +       cpumask_t cpus;
> > +       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_bank_cfgs[svs->bank_id].dev_id);
> > +       if (!svs->dev) {
> > +               pr_err("failed to get cpu%d device\n",
> > +                      svs_bank_cfgs[svs->bank_id].dev_id);
> > +               return -ENODEV;
> > +       }
> > +
> > +       svs->reg = regulator_get_exclusive(svs->dev, "proc");
> > +       if (IS_ERR(svs->reg)) {
> > +               dev_err(svs->dev, "failed to get proc regulator\n");
> > +               return PTR_ERR(svs->reg);
> > +       }
> > +
> > +       ret = dev_pm_opp_of_get_sharing_cpus(svs->dev, &cpus);
> > +       if (ret) {
> > +               dev_err(svs->dev, "failed to get OPP-sharing information\n");
> > +               return ret;
> > +       }
> > +
> > +       ret = dev_pm_opp_of_cpumask_add_table(&cpus);
> > +       if (ret) {
> > +               dev_err(svs->dev, "no OPP table\n");
> > +               return ret;
> > +       }
> > +
> > +       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 / 1000;
> > +               svs->volt_table[i] = dev_pm_opp_get_voltage(opp) / 1000;
> 
> Why / 1000?
> Just save the original values.  This will keep us from having to * 1000 later.
> 

Will do it.

> > +       }
> > +
> > +out:
> > +       rcu_read_unlock();
> > +       if (ret) {
> > +               regulator_put(svs->reg);
> > +               dev_pm_opp_of_cpumask_remove_table(&cpus);
> > +       }
> > +
> > +       return ret;
> > +}
> > +
> > +static void mtk_svs_release(struct mtk_thermal *mt)
> > +{
> > +       int i, ret;
> > +       cpumask_t cpus;
> > +
> > +       if (!IS_ERR(mt->svs_pll))
> > +               clk_put(mt->svs_pll);
> 
> devm_clk_put(), devm_free_irq() & devm_regulator_put in this function.
> 

Will do it.

> > +
> > +       if (!IS_ERR(mt->svs_mux))
> > +               clk_put(mt->svs_mux);
> > +
> > +       if (mt->svs_irq != -1)
> > +               free_irq(mt->svs_irq, mt);
> > +
> > +       for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
> > +               struct mtk_svs_bank *svs = &mt->svs_banks[i];
> > +
> > +               ret = dev_pm_opp_of_get_sharing_cpus(svs->dev, &cpus);
> > +               if (!ret)
> > +                       dev_pm_opp_of_cpumask_remove_table(&cpus);
> > +
> > +               if (svs->reg)
> > +                       regulator_put(svs->reg);
> > +       }
> > +}
> > +
> > +static int mtk_svs_init(struct platform_device *pdev, struct mtk_thermal *mt)
> > +{
> > +       struct clk *parent;
> > +       int i, ret, vboot;
> > +       unsigned long flags, timeout;
> > +       struct mtk_svs_bank *svs;
> > +
> > +       parent = clk_get_parent(mt->svs_mux);
> > +       ret = clk_set_parent(mt->svs_mux, mt->svs_pll);
> > +       if (ret) {
> > +               dev_err(&pdev->dev,
> > +                       "failed to set svs_mux to svs_pll\n");
> > +               return ret;
> > +       }
> > +
> > +       for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
> > +               svs = &mt->svs_banks[i];
> > +
> 
> Why do you do this boot voltage check?
> Doesn't this depend on the specific OPP used by firmware, which is
> outside the control of the kernel?
> 

If the boot voltages are not matched, the calibration environment of SVS also
differs. That will lead to wrong voltage values output by SVS. That also imply
the firmware needs to setup the correct OPP to make SVS work normally. Or we
need some mechanism to pause cpufreq governor and set to specific OPP.

> > +               vboot = regulator_get_voltage(svs->reg) / 1000;
> > +               if (mvolt_to_config(vboot) !=
> > +                   mvolt_to_config(svs_bank_cfgs[i].vboot)) {
> > +                       dev_err(svs->dev, "Vboot value mismatch!\n");
> > +                       ret = -EINVAL;
> > +                       break;
> > +               }
> > +
> > +               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;
> > +               }
> 
> Shouldn't this be best effort?
> If we can't set the regulator to fast mode, can we continue anyway?
> 

AFAIK, we need to set the regulator to fast mode to get stable voltage as
possible. I am sure about the details.

@Henry,
Would you please comment it?

Thanks,
Pi-Cheng

> > +
> > +               spin_lock_irqsave(&mt->lock, flags);
> > +               mtk_thermal_switch_bank(&mt->banks[i]);
> > +
> > +               mtk_svs_set_phase(svs, SVS_PHASE_0);
> > +
> > +               spin_unlock_irqrestore(&mt->lock, flags);
> > +
> > +               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;
> > +               }
> > +
> > +               ret = regulator_set_mode(svs->reg, REGULATOR_MODE_NORMAL);
> > +               if (ret)
> > +                       dev_err(svs->dev,
> > +                               "Failed to set regulator in normal mode\n");
> > +
> > +               regulator_put(svs->reg);
> > +               svs->reg = NULL;
> > +       }
> > +
> > +err_bank_init:
> > +
> > +       if (ret)
> > +               for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
> > +                       svs = &mt->svs_banks[i];
> > +
> > +                       spin_lock_irqsave(&mt->lock, flags);
> > +                       mtk_thermal_switch_bank(&mt->banks[i]);
> > +
> > +                       mtk_svs_bank_disable(svs);
> > +
> > +                       spin_unlock_irqrestore(&mt->lock, flags);
> > +
> > +                       mtk_svs_adjust_voltage(svs);
> > +               }
> > +
> > +       ret = clk_set_parent(mt->svs_mux, parent);
> > +       if (ret) {
> > +               dev_err(&pdev->dev,
> > +                       "failed to set svs_mux to original parent\n");
> > +               return ret;
> > +       }
> > +
> > +       return ret;
> > +}
> > +
> >  static int mtk_thermal_probe(struct platform_device *pdev)
> >  {
> >         int ret, i;
> >         struct device_node *auxadc, *apmixedsys, *np = pdev->dev.of_node;
> >         struct mtk_thermal *mt;
> >         struct resource *res;
> > +       struct platform_device *pdev_ret;
> >         u64 auxadc_phys_base, apmixed_phys_base;
> > +       bool skip_svs_init = false;
> >
> >         mt = devm_kzalloc(&pdev->dev, sizeof(*mt), GFP_KERNEL);
> >         if (!mt)
> > @@ -493,6 +1132,23 @@ static int mtk_thermal_probe(struct platform_device *pdev)
> >         if (IS_ERR(mt->clk_auxadc))
> >                 return PTR_ERR(mt->clk_auxadc);
> >
> > +       /*
> > +        * These two clks are optional for mtk-thermal. If present, SVS engine
> > +        * will be activated.
> > +        */
> > +       mt->svs_pll = devm_clk_get(&pdev->dev, "svs_pll");
> > +       mt->svs_mux = devm_clk_get(&pdev->dev, "svs_mux");
> > +       if (IS_ERR(mt->svs_pll) || IS_ERR(mt->svs_mux))
> > +               skip_svs_init = true;
> > +
> > +       mt->svs_irq = platform_get_irq(pdev, 1);
> > +       ret = devm_request_irq(&pdev->dev, mt->svs_irq, mtk_svs_interrupt,
> > +                              IRQF_TRIGGER_LOW, "mtk-svs", mt);
> > +       if (ret) {
> > +               mt->svs_irq = -1;
> > +               skip_svs_init = true;
> > +       }
> > +
> >         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
> >         mt->thermal_base = devm_ioremap_resource(&pdev->dev, res);
> >         if (IS_ERR(mt->thermal_base))
> > @@ -502,6 +1158,27 @@ static int mtk_thermal_probe(struct platform_device *pdev)
> >         if (ret)
> >                 return ret;
> >
> > +       ret = mtk_svs_get_calibration_data(&pdev->dev, mt);
> > +       if (ret == -EPROBE_DEFER)
> > +               return ret;
> > +       else if (ret)
> > +               skip_svs_init = true;
> > +
> > +       for (i = 0; i < MT8173_NUM_SVS_BANKS; i++) {
> > +               mt->svs_banks[i].bank_id = i;
> > +               mt->svs_banks[i].mt = mt;
> > +
> > +               ret = mtk_svs_bank_init(&mt->svs_banks[i]);
> > +               if (ret) {
> > +                       if (ret == -EPROBE_DEFER)
> > +                               return ret;
> > +
> > +                       dev_err(&pdev->dev,
> > +                               "failed to initialize mtk svs bank%d\n", i);
> > +                       skip_svs_init = true;
> > +               }
> > +       }
> > +
> >         spin_lock_init(&mt->lock);
> >
> >         mt->dev = &pdev->dev;
> > @@ -562,6 +1239,33 @@ static int mtk_thermal_probe(struct platform_device *pdev)
> >         if (IS_ERR(mt->tzd))
> >                 goto err_register;
> >
> > +       /*
> > +        * SVS engine needs to be initialized after mtk-thermal initialization
> > +        * is done and before the initialization of cpufreq driver.
> > +        */
> > +       if (!skip_svs_init) {
> > +               ret = mtk_svs_init(pdev, mt);
> > +               if (ret) {
> > +                       /* Keep running system with SVS engine disabled. */
> > +                       dev_err(&pdev->dev,
> > +                               "Failed to initialze MTK SVS engine\n");
> > +                       skip_svs_init = true;
> > +               }
> > +       }
> > +
> > +       /*
> > +        * Release all resources needed by SVS if not all required resources
> > +        * are present or SVS initialization failed.
> > +        */
> > +       if (skip_svs_init)
> > +               mtk_svs_release(mt);
> > +
> > +       pdev_ret = platform_device_register_simple("mt8173-cpufreq", -1,
> > +                                                  NULL, 0);
> > +       if (IS_ERR(pdev_ret))
> > +               dev_err(&pdev->dev,
> > +                       "failed to register mt8173-cpufreq device\n");
> > +
> >         return 0;
> >
> >  err_register:
> > --
> > 1.9.1
> >
> >
> > _______________________________________________
> > Linux-mediatek mailing list
> > Linux-mediatek@lists.infradead.org
> > http://lists.infradead.org/mailman/listinfo/linux-mediatek

Re: [RFC PATCH 4/5] PM / AVS: thermal: MT8173: Introduce support for SVS engine

[Henry Chen]

On Thu, 2016-02-18 at 11:00 +0800, Pi-Cheng Chen wrote:

> > > +               vboot = regulator_get_voltage(svs->reg) / 1000;
> > > +               if (mvolt_to_config(vboot) !=
> > > +                   mvolt_to_config(svs_bank_cfgs[i].vboot)) {
> > > +                       dev_err(svs->dev, "Vboot value mismatch!\n");
> > > +                       ret = -EINVAL;
> > > +                       break;
> > > +               }
> > > +
> > > +               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;
> > > +               }
> > 
> > Shouldn't this be best effort?
> > If we can't set the regulator to fast mode, can we continue anyway?
> > 
> 
> AFAIK, we need to set the regulator to fast mode to get stable voltage as
> possible. I am sure about the details.
> 
> @Henry,
> Would you please comment it?

Hi,

Yes, as you said, PTPOD need the more precisely voltage as possible when
driver initialize, there has two mode in mt6397: pwm/auto mode, the pwm
mode which can get the more stable Vout than auto mode.

Thanks,
Henry

Re: [RFC PATCH 4/5] PM / AVS: thermal: MT8173: Introduce support for SVS engine

[Henry Chen]

On Fri, 2016-02-19 at 13:08 +0800, Henry Chen wrote:
> On Thu, 2016-02-18 at 11:00 +0800, Pi-Cheng Chen wrote:
> 
> > > > +               vboot = regulator_get_voltage(svs->reg) / 1000;
> > > > +               if (mvolt_to_config(vboot) !=
> > > > +                   mvolt_to_config(svs_bank_cfgs[i].vboot)) {
> > > > +                       dev_err(svs->dev, "Vboot value mismatch!\n");
> > > > +                       ret = -EINVAL;
> > > > +                       break;
> > > > +               }
> > > > +
> > > > +               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;
> > > > +               }
> > > 
> > > Shouldn't this be best effort?
> > > If we can't set the regulator to fast mode, can we continue anyway?
> > > 
> > 
> > AFAIK, we need to set the regulator to fast mode to get stable voltage as
> > possible. I am sure about the details.
> > 
> > @Henry,
> > Would you please comment it?
> 

Hi,

Correct typos.Sorry for the wrong module name. 

Yes, as you said, SVS need the more precisely voltage as possible when
driver initialize, there has two mode in mt6397: pwm/auto mode, the pwm
mode which can get the more stable Vout than auto mode.

Thanks,
Henry

--
To unsubscribe from this list: send the line "unsubscribe devicetree" in
the body of a message to majordomo-u79uwXL29TY76Z2rM5mHXA@public.gmane.org
More majordomo info at  http://vger.kernel.org/majordomo-info.html

[PATCH 5/5] cpufreq: mt8173: Add notifier to handle OPP voltage adjustment

[Pi-Cheng Chen]

Add a notifier to receive OPP voltage adjustment event which is
triggered by voltage change of OPP entries done by Mediatek SVS
(Smart Voltage Scaling) engine.

CC: Stephen Boyd <sboyd@codeaurora.org>

Signed-off-by: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
---
This patch is modify from the patch which adds[1] a notifier in
cpufreq-dt to handle OPP voltage adjust event. This patch relies on the
on the runtime voltage adjustment mechanism of OPP introduced in the
same patchset.

[1] https://lkml.org/lkml/2015/9/18/833
---
 drivers/cpufreq/mt8173-cpufreq.c | 55 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 55 insertions(+)

diff --git a/drivers/cpufreq/mt8173-cpufreq.c b/drivers/cpufreq/mt8173-cpufreq.c
index 1ede917..248f6e8 100644
--- a/drivers/cpufreq/mt8173-cpufreq.c
+++ b/drivers/cpufreq/mt8173-cpufreq.c
@@ -48,8 +48,11 @@ struct mtk_cpu_dvfs_info {
 	struct clk *cpu_clk;
 	struct clk *inter_clk;
 	struct thermal_cooling_device *cdev;
+	struct mutex lock;
+	struct notifier_block opp_nb;
 	struct list_head list_head;
 	int intermediate_voltage;
+	unsigned long opp_freq;
 	bool need_voltage_tracking;
 };
 
@@ -211,6 +214,33 @@ static int mtk_cpufreq_set_voltage(struct mtk_cpu_dvfs_info *info, int vproc)
 					     vproc + VOLT_TOL);
 }
 
+static int mtk_cpufreq_opp_notifier(struct notifier_block *nb,
+				    unsigned long event, void *data)
+{
+	struct dev_pm_opp *opp = data;
+	struct mtk_cpu_dvfs_info *info = container_of(nb,
+						      struct mtk_cpu_dvfs_info,
+						      opp_nb);
+	unsigned long freq, volt;
+	int ret = 0;
+
+	if (event == OPP_EVENT_ADJUST_VOLTAGE) {
+		freq = dev_pm_opp_get_freq(opp);
+
+		if (info->opp_freq == freq) {
+			volt = dev_pm_opp_get_voltage(opp);
+			mutex_lock(&info->lock);
+			ret = mtk_cpufreq_set_voltage(info, volt);
+			mutex_unlock(&info->lock);
+			if (ret)
+				dev_err(info->cpu_dev,
+					"failed to scale voltage: %d\n", ret);
+		}
+	}
+
+	return notifier_from_errno(ret);
+}
+
 static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
 				  unsigned int index)
 {
@@ -245,6 +275,8 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
 	vproc = dev_pm_opp_get_voltage(opp);
 	rcu_read_unlock();
 
+	mutex_lock(&info->lock);
+
 	/*
 	 * If the new voltage or the intermediate voltage is higher than the
 	 * current voltage, scale up voltage first.
@@ -256,6 +288,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
 			pr_err("cpu%d: failed to scale up voltage!\n",
 			       policy->cpu);
 			mtk_cpufreq_set_voltage(info, old_vproc);
+			mutex_unlock(&info->lock);
 			return ret;
 		}
 	}
@@ -267,6 +300,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
 		       policy->cpu);
 		mtk_cpufreq_set_voltage(info, old_vproc);
 		WARN_ON(1);
+		mutex_unlock(&info->lock);
 		return ret;
 	}
 
@@ -277,6 +311,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
 		       policy->cpu);
 		clk_set_parent(cpu_clk, armpll);
 		mtk_cpufreq_set_voltage(info, old_vproc);
+		mutex_unlock(&info->lock);
 		return ret;
 	}
 
@@ -287,6 +322,7 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
 		       policy->cpu);
 		mtk_cpufreq_set_voltage(info, inter_vproc);
 		WARN_ON(1);
+		mutex_unlock(&info->lock);
 		return ret;
 	}
 
@@ -302,10 +338,14 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
 			clk_set_parent(cpu_clk, info->inter_clk);
 			clk_set_rate(armpll, old_freq_hz);
 			clk_set_parent(cpu_clk, armpll);
+			mutex_unlock(&info->lock);
 			return ret;
 		}
 	}
 
+	info->opp_freq = freq_hz;
+	mutex_unlock(&info->lock);
+
 	return 0;
 }
 
@@ -343,6 +383,7 @@ static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
 	struct dev_pm_opp *opp;
 	unsigned long rate;
 	int ret;
+	struct srcu_notifier_head *opp_srcu_head;
 
 	cpu_dev = get_cpu_device(cpu);
 	if (!cpu_dev) {
@@ -417,11 +458,25 @@ static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
 	info->intermediate_voltage = dev_pm_opp_get_voltage(opp);
 	rcu_read_unlock();
 
+	opp_srcu_head = dev_pm_opp_get_notifier(cpu_dev);
+	if (IS_ERR(opp_srcu_head)) {
+		ret = PTR_ERR(opp_srcu_head);
+		goto out_free_opp_table;
+	}
+
+	info->opp_nb.notifier_call = mtk_cpufreq_opp_notifier;
+	ret = srcu_notifier_chain_register(opp_srcu_head, &info->opp_nb);
+	if (ret)
+		goto out_free_opp_table;
+
 	info->cpu_dev = cpu_dev;
 	info->proc_reg = proc_reg;
 	info->sram_reg = IS_ERR(sram_reg) ? NULL : sram_reg;
 	info->cpu_clk = cpu_clk;
 	info->inter_clk = inter_clk;
+	info->opp_freq = clk_get_rate(cpu_clk);
+
+	mutex_init(&info->lock);
 
 	/*
 	 * If SRAM regulator is present, software "voltage tracking" is needed
-- 
1.9.1