diff mbox series

[v15,3/3] cpufreq: mediatek-hw: Add support for CPUFREQ HW

Message ID 1630658364-6192-4-git-send-email-hector.yuan@mediatek.com (mailing list archive)
State New, archived
Delegated to: viresh kumar
Headers show
Series [v15,1/3] dt-bindings: cpufreq: add bindings for MediaTek cpufreq HW | expand

Commit Message

Hector Yuan Sept. 3, 2021, 8:39 a.m. UTC 
From: "Hector.Yuan" <hector.yuan@mediatek.com>

Introduce cpufreq HW driver which can support
CPU frequency adjust in MT6779 platform.

Signed-off-by: Hector.Yuan <hector.yuan@mediatek.com>
---
 drivers/cpufreq/Kconfig.arm           |   12 ++
 drivers/cpufreq/Makefile              |    1 +
 drivers/cpufreq/mediatek-cpufreq-hw.c |  340 +++++++++++++++++++++++++++++++++
 3 files changed, 353 insertions(+)
 create mode 100644 drivers/cpufreq/mediatek-cpufreq-hw.c

Comments

kernel test robot Sept. 3, 2021, 3:54 p.m. UTC | #1 
Hi Hector,

Thank you for the patch! Yet something to improve:

[auto build test ERROR on soc/for-next]
[also build test ERROR on robh/for-next v5.14]
[cannot apply to pm/linux-next next-20210903]
[If your patch is applied to the wrong git tree, kindly drop us a note.
And when submitting patch, we suggest to use '--base' as documented in
https://git-scm.com/docs/git-format-patch]

url:    https://github.com/0day-ci/linux/commits/Hector-Yuan/dt-bindings-cpufreq-add-bindings-for-MediaTek-cpufreq-HW/20210903-174004
base:   https://git.kernel.org/pub/scm/linux/kernel/git/soc/soc.git for-next
config: arm-randconfig-c002-20210903 (attached as .config)
compiler: clang version 14.0.0 (https://github.com/llvm/llvm-project 1104e3258b5064e7110cc297e2cec60ac9acfc0a)
reproduce (this is a W=1 build):
        wget https://raw.githubusercontent.com/intel/lkp-tests/master/sbin/make.cross -O ~/bin/make.cross
        chmod +x ~/bin/make.cross
        # install arm cross compiling tool for clang build
        # apt-get install binutils-arm-linux-gnueabi
        # https://github.com/0day-ci/linux/commit/a7148c526ed4198bbe953fb17e52257f0d50d8c4
        git remote add linux-review https://github.com/0day-ci/linux
        git fetch --no-tags linux-review Hector-Yuan/dt-bindings-cpufreq-add-bindings-for-MediaTek-cpufreq-HW/20210903-174004
        git checkout a7148c526ed4198bbe953fb17e52257f0d50d8c4
        # save the attached .config to linux build tree
        mkdir build_dir
        COMPILER_INSTALL_PATH=$HOME/0day COMPILER=clang make.cross O=build_dir ARCH=arm SHELL=/bin/bash drivers/cpufreq/

If you fix the issue, kindly add following tag as appropriate
Reported-by: kernel test robot <lkp@intel.com>

All errors (new ones prefixed by >>):

>> drivers/cpufreq/mediatek-cpufreq-hw.c:280:3: error: field designator 'register_em' does not refer to any field in type 'struct cpufreq_driver'
           .register_em    = mtk_cpufreq_register_em,
            ^
   1 error generated.


vim +280 drivers/cpufreq/mediatek-cpufreq-hw.c

   270	
   271	static struct cpufreq_driver cpufreq_mtk_hw_driver = {
   272		.flags		= CPUFREQ_NEED_INITIAL_FREQ_CHECK |
   273				  CPUFREQ_HAVE_GOVERNOR_PER_POLICY |
   274				  CPUFREQ_IS_COOLING_DEV,
   275		.verify		= cpufreq_generic_frequency_table_verify,
   276		.target_index	= mtk_cpufreq_hw_target_index,
   277		.get		= mtk_cpufreq_hw_get,
   278		.init		= mtk_cpufreq_hw_cpu_init,
   279		.exit		= mtk_cpufreq_hw_cpu_exit,
 > 280		.register_em	= mtk_cpufreq_register_em,
   281		.fast_switch	= mtk_cpufreq_hw_fast_switch,
   282		.name		= "mtk-cpufreq-hw",
   283		.attr		= cpufreq_generic_attr,
   284	};
   285	

---
0-DAY CI Kernel Test Service, Intel Corporation
https://lists.01.org/hyperkitty/list/kbuild-all@lists.01.org
kernel test robot Sept. 3, 2021, 4:34 p.m. UTC | #2 
Hi Hector,

Thank you for the patch! Yet something to improve:

[auto build test ERROR on soc/for-next]
[also build test ERROR on robh/for-next v5.14]
[cannot apply to pm/linux-next next-20210903]
[If your patch is applied to the wrong git tree, kindly drop us a note.
And when submitting patch, we suggest to use '--base' as documented in
https://git-scm.com/docs/git-format-patch]

url:    https://github.com/0day-ci/linux/commits/Hector-Yuan/dt-bindings-cpufreq-add-bindings-for-MediaTek-cpufreq-HW/20210903-174004
base:   https://git.kernel.org/pub/scm/linux/kernel/git/soc/soc.git for-next
config: arm-defconfig (attached as .config)
compiler: arm-linux-gnueabi-gcc (GCC) 11.2.0
reproduce (this is a W=1 build):
        wget https://raw.githubusercontent.com/intel/lkp-tests/master/sbin/make.cross -O ~/bin/make.cross
        chmod +x ~/bin/make.cross
        # https://github.com/0day-ci/linux/commit/a7148c526ed4198bbe953fb17e52257f0d50d8c4
        git remote add linux-review https://github.com/0day-ci/linux
        git fetch --no-tags linux-review Hector-Yuan/dt-bindings-cpufreq-add-bindings-for-MediaTek-cpufreq-HW/20210903-174004
        git checkout a7148c526ed4198bbe953fb17e52257f0d50d8c4
        # save the attached .config to linux build tree
        mkdir build_dir
        COMPILER_INSTALL_PATH=$HOME/0day COMPILER=gcc-11.2.0 make.cross O=build_dir ARCH=arm SHELL=/bin/bash drivers/cpufreq/

If you fix the issue, kindly add following tag as appropriate
Reported-by: kernel test robot <lkp@intel.com>

All errors (new ones prefixed by >>):

>> drivers/cpufreq/mediatek-cpufreq-hw.c:280:10: error: 'struct cpufreq_driver' has no member named 'register_em'
     280 |         .register_em    = mtk_cpufreq_register_em,
         |          ^~~~~~~~~~~
>> drivers/cpufreq/mediatek-cpufreq-hw.c:280:27: error: initialization of 'int (*)(struct cpufreq_policy *)' from incompatible pointer type 'void (*)(struct cpufreq_policy *)' [-Werror=incompatible-pointer-types]
     280 |         .register_em    = mtk_cpufreq_register_em,
         |                           ^~~~~~~~~~~~~~~~~~~~~~~
   drivers/cpufreq/mediatek-cpufreq-hw.c:280:27: note: (near initialization for 'cpufreq_mtk_hw_driver.suspend')
   cc1: some warnings being treated as errors


vim +280 drivers/cpufreq/mediatek-cpufreq-hw.c

   270	
   271	static struct cpufreq_driver cpufreq_mtk_hw_driver = {
   272		.flags		= CPUFREQ_NEED_INITIAL_FREQ_CHECK |
   273				  CPUFREQ_HAVE_GOVERNOR_PER_POLICY |
   274				  CPUFREQ_IS_COOLING_DEV,
   275		.verify		= cpufreq_generic_frequency_table_verify,
   276		.target_index	= mtk_cpufreq_hw_target_index,
   277		.get		= mtk_cpufreq_hw_get,
   278		.init		= mtk_cpufreq_hw_cpu_init,
   279		.exit		= mtk_cpufreq_hw_cpu_exit,
 > 280		.register_em	= mtk_cpufreq_register_em,
   281		.fast_switch	= mtk_cpufreq_hw_fast_switch,
   282		.name		= "mtk-cpufreq-hw",
   283		.attr		= cpufreq_generic_attr,
   284	};
   285	

---
0-DAY CI Kernel Test Service, Intel Corporation
https://lists.01.org/hyperkitty/list/kbuild-all@lists.01.org
Viresh Kumar Sept. 6, 2021, 9:44 a.m. UTC | #3 
On 03-09-21, 16:39, Hector Yuan wrote:
> From: "Hector.Yuan" <hector.yuan@mediatek.com>
> 
> Introduce cpufreq HW driver which can support
> CPU frequency adjust in MT6779 platform.
> 
> Signed-off-by: Hector.Yuan <hector.yuan@mediatek.com>
> ---
>  drivers/cpufreq/Kconfig.arm           |   12 ++
>  drivers/cpufreq/Makefile              |    1 +
>  drivers/cpufreq/mediatek-cpufreq-hw.c |  340 +++++++++++++++++++++++++++++++++
>  3 files changed, 353 insertions(+)
>  create mode 100644 drivers/cpufreq/mediatek-cpufreq-hw.c

Here as well, I have added below diff to the original patch. Lemme
know if you don't like something.

diff --git a/drivers/cpufreq/mediatek-cpufreq-hw.c b/drivers/cpufreq/mediatek-cpufreq-hw.c
index 9c6df1b00f3e..0cf18dd46b92 100644
--- a/drivers/cpufreq/mediatek-cpufreq-hw.c
+++ b/drivers/cpufreq/mediatek-cpufreq-hw.c
@@ -33,12 +33,6 @@ enum {
        REG_ARRAY_SIZE,
 };

-struct mtk_cpufreq_drv;
-
-struct mtk_cpufreq_drv {
-       const u16 *offsets;
-};
-
 struct mtk_cpufreq_data {
        struct cpufreq_frequency_table *table;
        void __iomem *reg_bases[REG_ARRAY_SIZE];
@@ -126,8 +120,8 @@ static int mtk_cpu_create_freq_table(struct platform_device *pdev,
                                     struct mtk_cpufreq_data *data)
 {
        struct device *dev = &pdev->dev;
-       void __iomem *base_table;
        u32 temp, i, freq, prev_freq = 0;
+       void __iomem *base_table;

        data->table = devm_kcalloc(dev, LUT_MAX_ENTRIES + 1,
                                   sizeof(*data->table), GFP_KERNEL);
@@ -198,15 +192,13 @@ static int mtk_cpu_resources_init(struct platform_device *pdev,
 static int mtk_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
 {
        struct platform_device *pdev = cpufreq_get_driver_data();
-       struct mtk_cpufreq_drv *drv = platform_get_drvdata(pdev);
        int sig, pwr_hw = CPUFREQ_HW_STATUS | SVS_HW_STATUS;
        struct mtk_cpufreq_data *data;
-       struct device *cpu_dev;
        unsigned int latency;
        int ret;

        /* Get the bases of cpufreq for domains */
-       ret = mtk_cpu_resources_init(pdev, policy, drv->offsets);
+       ret = mtk_cpu_resources_init(pdev, policy, platform_get_drvdata(pdev));
        if (ret) {
                dev_info(&pdev->dev, "CPUFreq resource init failed\n");
                return ret;
@@ -218,9 +210,7 @@ static int mtk_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
        if (!latency)
                latency = CPUFREQ_ETERNAL;

-       /* us convert to ns */
        policy->cpuinfo.transition_latency = latency;
-
        policy->fast_switch_possible = true;

        /* HW should be in enabled state to proceed now */
@@ -237,12 +227,6 @@ static int mtk_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
                pr_info("SVS of CPU%d is not enabled\n", policy->cpu);
        }

-       cpu_dev = get_cpu_device(policy->cpu);
-       if (!cpu_dev) {
-               pr_info("failed to get cpu%d device\n", policy->cpu);
-               return -ENODEV;
-       }
-
        return 0;
 }

@@ -261,9 +245,6 @@ static void mtk_cpufreq_register_em(struct cpufreq_policy *policy)
        struct em_data_callback em_cb = EM_DATA_CB(mtk_cpufreq_get_cpu_power);
        struct mtk_cpufreq_data *data = policy->driver_data;

-       if (!data->nr_opp)
-               return;
-
        em_dev_register_perf_domain(get_cpu_device(policy->cpu), data->nr_opp,
                                    &em_cb, policy->cpus, true);
 }
@@ -285,22 +266,14 @@ static struct cpufreq_driver cpufreq_mtk_hw_driver = {

 static int mtk_cpufreq_hw_driver_probe(struct platform_device *pdev)
 {
-       struct mtk_cpufreq_drv *drv;
-       const u16 *offsets;
+       const void *data;
        int ret;

-       offsets = of_device_get_match_data(&pdev->dev);
-       if (!offsets)
+       data = of_device_get_match_data(&pdev->dev);
+       if (!data)
                return -EINVAL;

-       drv = kzalloc(sizeof(*drv), GFP_KERNEL);
-       if (!drv)
-               return -ENOMEM;
-
-       drv->offsets = offsets;
-
-       platform_set_drvdata(pdev, drv);
-
+       platform_set_drvdata(pdev, (void *) data);
        cpufreq_mtk_hw_driver.driver_data = pdev;

        ret = cpufreq_register_driver(&cpufreq_mtk_hw_driver);
@@ -312,11 +285,6 @@ static int mtk_cpufreq_hw_driver_probe(struct platform_device *pdev)

 static int mtk_cpufreq_hw_driver_remove(struct platform_device *pdev)
 {
-       struct mtk_cpufreq_drv *drv = platform_get_drvdata(pdev);
-
-       kfree(drv->offsets);
-       kfree(drv);
-
        return cpufreq_unregister_driver(&cpufreq_mtk_hw_driver);
 }
diff mbox series

Patch

diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
index a5c5f70..954749a 100644
--- a/drivers/cpufreq/Kconfig.arm
+++ b/drivers/cpufreq/Kconfig.arm
@@ -133,6 +133,18 @@  config ARM_MEDIATEK_CPUFREQ
 	help
 	  This adds the CPUFreq driver support for MediaTek SoCs.
 
+config ARM_MEDIATEK_CPUFREQ_HW
+	tristate "MediaTek CPUFreq HW driver"
+	depends on ARCH_MEDIATEK || COMPILE_TEST
+	default m
+	help
+	  Support for the CPUFreq HW driver.
+	  Some MediaTek chipsets have a HW engine to offload the steps
+	  necessary for changing the frequency of the CPUs. Firmware loaded
+	  in this engine exposes a programming interface to the OS.
+	  The driver implements the cpufreq interface for this HW engine.
+	  Say Y if you want to support CPUFreq HW.
+
 config ARM_OMAP2PLUS_CPUFREQ
 	bool "TI OMAP2+"
 	depends on ARCH_OMAP2PLUS
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index 27d3bd7..48ee585 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -56,6 +56,7 @@  obj-$(CONFIG_ARM_IMX6Q_CPUFREQ)		+= imx6q-cpufreq.o
 obj-$(CONFIG_ARM_IMX_CPUFREQ_DT)	+= imx-cpufreq-dt.o
 obj-$(CONFIG_ARM_KIRKWOOD_CPUFREQ)	+= kirkwood-cpufreq.o
 obj-$(CONFIG_ARM_MEDIATEK_CPUFREQ)	+= mediatek-cpufreq.o
+obj-$(CONFIG_ARM_MEDIATEK_CPUFREQ_HW)	+= mediatek-cpufreq-hw.o
 obj-$(CONFIG_MACH_MVEBU_V7)		+= mvebu-cpufreq.o
 obj-$(CONFIG_ARM_OMAP2PLUS_CPUFREQ)	+= omap-cpufreq.o
 obj-$(CONFIG_ARM_PXA2xx_CPUFREQ)	+= pxa2xx-cpufreq.o
diff --git a/drivers/cpufreq/mediatek-cpufreq-hw.c b/drivers/cpufreq/mediatek-cpufreq-hw.c
new file mode 100644
index 0000000..9c6df1b
--- /dev/null
+++ b/drivers/cpufreq/mediatek-cpufreq-hw.c
@@ -0,0 +1,340 @@ 
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+
+#include <linux/bitfield.h>
+#include <linux/cpufreq.h>
+#include <linux/energy_model.h>
+#include <linux/init.h>
+#include <linux/iopoll.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/slab.h>
+
+#define LUT_MAX_ENTRIES			32U
+#define LUT_FREQ			GENMASK(11, 0)
+#define LUT_ROW_SIZE			0x4
+#define CPUFREQ_HW_STATUS		BIT(0)
+#define SVS_HW_STATUS			BIT(1)
+#define POLL_USEC			1000
+#define TIMEOUT_USEC			300000
+
+enum {
+	REG_FREQ_LUT_TABLE,
+	REG_FREQ_ENABLE,
+	REG_FREQ_PERF_STATE,
+	REG_FREQ_HW_STATE,
+	REG_EM_POWER_TBL,
+	REG_FREQ_LATENCY,
+
+	REG_ARRAY_SIZE,
+};
+
+struct mtk_cpufreq_drv;
+
+struct mtk_cpufreq_drv {
+	const u16 *offsets;
+};
+
+struct mtk_cpufreq_data {
+	struct cpufreq_frequency_table *table;
+	void __iomem *reg_bases[REG_ARRAY_SIZE];
+	int nr_opp;
+};
+
+static const u16 cpufreq_mtk_offsets[REG_ARRAY_SIZE] = {
+	[REG_FREQ_LUT_TABLE]	= 0x0,
+	[REG_FREQ_ENABLE]	= 0x84,
+	[REG_FREQ_PERF_STATE]	= 0x88,
+	[REG_FREQ_HW_STATE]	= 0x8c,
+	[REG_EM_POWER_TBL]	= 0x90,
+	[REG_FREQ_LATENCY]	= 0x110,
+};
+
+static int __maybe_unused
+mtk_cpufreq_get_cpu_power(unsigned long *mW,
+			  unsigned long *KHz, struct device *cpu_dev)
+{
+	struct mtk_cpufreq_data *data;
+	struct cpufreq_policy *policy;
+	int i;
+
+	policy = cpufreq_cpu_get_raw(cpu_dev->id);
+	if (!policy)
+		return 0;
+
+	data = policy->driver_data;
+
+	for (i = 0; i < data->nr_opp; i++) {
+		if (data->table[i].frequency < *KHz)
+			break;
+	}
+	i--;
+
+	*KHz = data->table[i].frequency;
+	*mW = readl_relaxed(data->reg_bases[REG_EM_POWER_TBL] +
+			    i * LUT_ROW_SIZE) / 1000;
+
+	return 0;
+}
+
+static int mtk_cpufreq_hw_target_index(struct cpufreq_policy *policy,
+				       unsigned int index)
+{
+	struct mtk_cpufreq_data *data = policy->driver_data;
+
+	writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]);
+
+	return 0;
+}
+
+static unsigned int mtk_cpufreq_hw_get(unsigned int cpu)
+{
+	struct mtk_cpufreq_data *data;
+	struct cpufreq_policy *policy;
+	unsigned int index;
+
+	policy = cpufreq_cpu_get_raw(cpu);
+	if (!policy)
+		return 0;
+
+	data = policy->driver_data;
+
+	index = readl_relaxed(data->reg_bases[REG_FREQ_PERF_STATE]);
+	index = min(index, LUT_MAX_ENTRIES - 1);
+
+	return data->table[index].frequency;
+}
+
+static unsigned int mtk_cpufreq_hw_fast_switch(struct cpufreq_policy *policy,
+					       unsigned int target_freq)
+{
+	struct mtk_cpufreq_data *data = policy->driver_data;
+	unsigned int index;
+
+	index = cpufreq_table_find_index_dl(policy, target_freq);
+
+	writel_relaxed(index, data->reg_bases[REG_FREQ_PERF_STATE]);
+
+	return policy->freq_table[index].frequency;
+}
+
+static int mtk_cpu_create_freq_table(struct platform_device *pdev,
+				     struct mtk_cpufreq_data *data)
+{
+	struct device *dev = &pdev->dev;
+	void __iomem *base_table;
+	u32 temp, i, freq, prev_freq = 0;
+
+	data->table = devm_kcalloc(dev, LUT_MAX_ENTRIES + 1,
+				   sizeof(*data->table), GFP_KERNEL);
+	if (!data->table)
+		return -ENOMEM;
+
+	base_table = data->reg_bases[REG_FREQ_LUT_TABLE];
+
+	for (i = 0; i < LUT_MAX_ENTRIES; i++) {
+		temp = readl_relaxed(base_table + (i * LUT_ROW_SIZE));
+		freq = FIELD_GET(LUT_FREQ, temp) * 1000;
+
+		if (freq == prev_freq)
+			break;
+
+		data->table[i].frequency = freq;
+
+		dev_dbg(dev, "index=%d freq=%d\n", i, data->table[i].frequency);
+
+		prev_freq = freq;
+	}
+
+	data->table[i].frequency = CPUFREQ_TABLE_END;
+	data->nr_opp = i;
+
+	return 0;
+}
+
+static int mtk_cpu_resources_init(struct platform_device *pdev,
+				  struct cpufreq_policy *policy,
+				  const u16 *offsets)
+{
+	struct mtk_cpufreq_data *data;
+	struct device *dev = &pdev->dev;
+	void __iomem *base;
+	int ret, i;
+	int index;
+
+	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	index = of_perf_domain_get_sharing_cpumask(policy->cpu, "performance-domains",
+						   "#performance-domain-cells",
+						   policy->cpus);
+	if (index < 0)
+		return index;
+
+	base = devm_platform_ioremap_resource(pdev, index);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	for (i = REG_FREQ_LUT_TABLE; i < REG_ARRAY_SIZE; i++)
+		data->reg_bases[i] = base + offsets[i];
+
+	ret = mtk_cpu_create_freq_table(pdev, data);
+	if (ret) {
+		dev_info(dev, "Domain-%d failed to create freq table\n", index);
+		return ret;
+	}
+
+	policy->freq_table = data->table;
+	policy->driver_data = data;
+
+	return 0;
+}
+
+static int mtk_cpufreq_hw_cpu_init(struct cpufreq_policy *policy)
+{
+	struct platform_device *pdev = cpufreq_get_driver_data();
+	struct mtk_cpufreq_drv *drv = platform_get_drvdata(pdev);
+	int sig, pwr_hw = CPUFREQ_HW_STATUS | SVS_HW_STATUS;
+	struct mtk_cpufreq_data *data;
+	struct device *cpu_dev;
+	unsigned int latency;
+	int ret;
+
+	/* Get the bases of cpufreq for domains */
+	ret = mtk_cpu_resources_init(pdev, policy, drv->offsets);
+	if (ret) {
+		dev_info(&pdev->dev, "CPUFreq resource init failed\n");
+		return ret;
+	}
+
+	data = policy->driver_data;
+
+	latency = readl_relaxed(data->reg_bases[REG_FREQ_LATENCY]) * 1000;
+	if (!latency)
+		latency = CPUFREQ_ETERNAL;
+
+	/* us convert to ns */
+	policy->cpuinfo.transition_latency = latency;
+
+	policy->fast_switch_possible = true;
+
+	/* HW should be in enabled state to proceed now */
+	writel_relaxed(0x1, data->reg_bases[REG_FREQ_ENABLE]);
+	if (readl_poll_timeout(data->reg_bases[REG_FREQ_HW_STATE], sig,
+			       (sig & pwr_hw) == pwr_hw, POLL_USEC,
+			       TIMEOUT_USEC)) {
+		if (!(sig & CPUFREQ_HW_STATUS)) {
+			pr_info("cpufreq hardware of CPU%d is not enabled\n",
+				policy->cpu);
+			return -ENODEV;
+		}
+
+		pr_info("SVS of CPU%d is not enabled\n", policy->cpu);
+	}
+
+	cpu_dev = get_cpu_device(policy->cpu);
+	if (!cpu_dev) {
+		pr_info("failed to get cpu%d device\n", policy->cpu);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int mtk_cpufreq_hw_cpu_exit(struct cpufreq_policy *policy)
+{
+	struct mtk_cpufreq_data *data = policy->driver_data;
+
+	/* HW should be in paused state now */
+	writel_relaxed(0x0, data->reg_bases[REG_FREQ_ENABLE]);
+
+	return 0;
+}
+
+static void mtk_cpufreq_register_em(struct cpufreq_policy *policy)
+{
+	struct em_data_callback em_cb = EM_DATA_CB(mtk_cpufreq_get_cpu_power);
+	struct mtk_cpufreq_data *data = policy->driver_data;
+
+	if (!data->nr_opp)
+		return;
+
+	em_dev_register_perf_domain(get_cpu_device(policy->cpu), data->nr_opp,
+				    &em_cb, policy->cpus, true);
+}
+
+static struct cpufreq_driver cpufreq_mtk_hw_driver = {
+	.flags		= CPUFREQ_NEED_INITIAL_FREQ_CHECK |
+			  CPUFREQ_HAVE_GOVERNOR_PER_POLICY |
+			  CPUFREQ_IS_COOLING_DEV,
+	.verify		= cpufreq_generic_frequency_table_verify,
+	.target_index	= mtk_cpufreq_hw_target_index,
+	.get		= mtk_cpufreq_hw_get,
+	.init		= mtk_cpufreq_hw_cpu_init,
+	.exit		= mtk_cpufreq_hw_cpu_exit,
+	.register_em	= mtk_cpufreq_register_em,
+	.fast_switch	= mtk_cpufreq_hw_fast_switch,
+	.name		= "mtk-cpufreq-hw",
+	.attr		= cpufreq_generic_attr,
+};
+
+static int mtk_cpufreq_hw_driver_probe(struct platform_device *pdev)
+{
+	struct mtk_cpufreq_drv *drv;
+	const u16 *offsets;
+	int ret;
+
+	offsets = of_device_get_match_data(&pdev->dev);
+	if (!offsets)
+		return -EINVAL;
+
+	drv = kzalloc(sizeof(*drv), GFP_KERNEL);
+	if (!drv)
+		return -ENOMEM;
+
+	drv->offsets = offsets;
+
+	platform_set_drvdata(pdev, drv);
+
+	cpufreq_mtk_hw_driver.driver_data = pdev;
+
+	ret = cpufreq_register_driver(&cpufreq_mtk_hw_driver);
+	if (ret)
+		dev_err(&pdev->dev, "CPUFreq HW driver failed to register\n");
+
+	return ret;
+}
+
+static int mtk_cpufreq_hw_driver_remove(struct platform_device *pdev)
+{
+	struct mtk_cpufreq_drv *drv = platform_get_drvdata(pdev);
+
+	kfree(drv->offsets);
+	kfree(drv);
+
+	return cpufreq_unregister_driver(&cpufreq_mtk_hw_driver);
+}
+
+static const struct of_device_id mtk_cpufreq_hw_match[] = {
+	{ .compatible = "mediatek,cpufreq-hw", .data = &cpufreq_mtk_offsets },
+	{}
+};
+
+static struct platform_driver mtk_cpufreq_hw_driver = {
+	.probe = mtk_cpufreq_hw_driver_probe,
+	.remove = mtk_cpufreq_hw_driver_remove,
+	.driver = {
+		.name = "mtk-cpufreq-hw",
+		.of_match_table = mtk_cpufreq_hw_match,
+	},
+};
+module_platform_driver(mtk_cpufreq_hw_driver);
+
+MODULE_AUTHOR("Hector Yuan <hector.yuan@mediatek.com>");
+MODULE_DESCRIPTION("Mediatek cpufreq-hw driver");
+MODULE_LICENSE("GPL v2");