Merge tag 'mtk-soc-for-v6.20' of https://git.kernel.org/pub/scm/linux/kernel/git/mediatek/linux into soc/drivers

MediaTek soc driver updates

This adds:
 - A socinfo entry for the MT8371 Genio 520 SoC
 - Support for the Dynamic Voltage and Frequency Scaling
   Resource Controller (DVFSRC) version 4, found in the
   new MediaTek Kompanio Ultra (MT8196) SoC
 - Initial support for the CMDQ mailbox found in the MT8196.
 - A memory leak fix in the MediaTek SVS driver's debug ops.

* tag 'mtk-soc-for-v6.20' of https://git.kernel.org/pub/scm/linux/kernel/git/mediatek/linux:
  soc: mediatek: mtk-cmdq: Add mminfra_offset adjustment for DRAM addresses
  soc: mediatek: mtk-cmdq: Extend cmdq_pkt_write API for SoCs without subsys ID
  soc: mediatek: mtk-cmdq: Add pa_base parsing for hardware without subsys ID support
  soc: mediatek: mtk-cmdq: Add cmdq_get_mbox_priv() in cmdq_pkt_create()
  mailbox: mtk-cmdq: Add driver data to support for MT8196
  mailbox: mtk-cmdq: Add mminfra_offset configuration for DRAM transaction
  mailbox: mtk-cmdq: Add GCE hardware virtualization configuration
  mailbox: mtk-cmdq: Add cmdq private data to cmdq_pkt for generating instruction
  soc: mediatek: mtk-dvfsrc: Rework bandwidth calculations
  soc: mediatek: mtk-dvfsrc: Get and Enable DVFSRC clock
  soc: mediatek: mtk-dvfsrc: Add support for DVFSRCv4 and MT8196
  soc: mediatek: mtk-dvfsrc: Write bandwidth to EMI DDR if present
  soc: mediatek: mtk-dvfsrc: Add a new callback for calc_dram_bw
  soc: mediatek: mtk-dvfsrc: Add and propagate DVFSRC bandwidth type
  soc: mediatek: mtk-dvfsrc: Change error check for DVFSRCv4 START cmd
  dt-bindings: soc: mediatek: dvfsrc: Document clock
  soc: mediatek: mtk-socinfo: Add entry for MT8371AV/AZA Genio 520
  soc: mediatek: svs: Fix memory leak in svs_enable_debug_write()

Signed-off-by: Arnd Bergmann <arnd@arndb.de>

5 files changed, 480 insertions, 41 deletions

diff --git a/drivers/mailbox/mtk-cmdq-mailbox.c b/drivers/mailbox/mtk-cmdq-mailbox.c
index 5791f80f995a..1bf6984948ef 100644
--- a/drivers/mailbox/mtk-cmdq-mailbox.c
+++ b/drivers/mailbox/mtk-cmdq-mailbox.c
@@ -14,6 +14,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
+#include <linux/sizes.h>
 #include <linux/mailbox_controller.h>
 #include <linux/mailbox/mtk-cmdq-mailbox.h>
 #include <linux/of.h>
@@ -43,6 +44,13 @@
 #define GCE_CTRL_BY_SW				GENMASK(2, 0)
 #define GCE_DDR_EN				GENMASK(18, 16)
 
+#define GCE_VM_ID_MAP(n)		(0x5018 + (n) / 10 * 4)
+#define GCE_VM_ID_MAP_THR_FLD_SHIFT(n)		((n) % 10 * 3)
+#define GCE_VM_ID_MAP_HOST_VM			GENMASK(2, 0)
+#define GCE_VM_CPR_GSIZE		0x50c4
+#define GCE_VM_CPR_GSIZE_FLD_SHIFT(vm_id)	((vm_id) * 4)
+#define GCE_VM_CPR_GSIZE_MAX			GENMASK(3, 0)
+
 #define CMDQ_THR_ACTIVE_SLOT_CYCLES	0x3200
 #define CMDQ_THR_ENABLED		0x1
 #define CMDQ_THR_DISABLED		0x0
@@ -87,22 +95,33 @@ struct cmdq {
 struct gce_plat {
 	u32 thread_nr;
 	u8 shift;
+	dma_addr_t mminfra_offset;
 	bool control_by_sw;
 	bool sw_ddr_en;
+	bool gce_vm;
 	u32 gce_num;
 };
 
 static inline u32 cmdq_convert_gce_addr(dma_addr_t addr, const struct gce_plat *pdata)
 {
 	/* Convert DMA addr (PA or IOVA) to GCE readable addr */
-	return addr >> pdata->shift;
+	return (addr + pdata->mminfra_offset) >> pdata->shift;
 }
 
 static inline dma_addr_t cmdq_revert_gce_addr(u32 addr, const struct gce_plat *pdata)
 {
 	/* Revert GCE readable addr to DMA addr (PA or IOVA) */
-	return (dma_addr_t)addr << pdata->shift;
+	return ((dma_addr_t)addr << pdata->shift) - pdata->mminfra_offset;
+}
+
+void cmdq_get_mbox_priv(struct mbox_chan *chan, struct cmdq_mbox_priv *priv)
+{
+	struct cmdq *cmdq = container_of(chan->mbox, struct cmdq, mbox);
+
+	priv->shift_pa = cmdq->pdata->shift;
+	priv->mminfra_offset = cmdq->pdata->mminfra_offset;
 }
+EXPORT_SYMBOL(cmdq_get_mbox_priv);
 
 u8 cmdq_get_shift_pa(struct mbox_chan *chan)
 {
@@ -112,6 +131,45 @@ u8 cmdq_get_shift_pa(struct mbox_chan *chan)
 }
 EXPORT_SYMBOL(cmdq_get_shift_pa);
 
+static void cmdq_vm_init(struct cmdq *cmdq)
+{
+	int i;
+	u32 vm_cpr_gsize = 0, vm_id_map = 0;
+	u32 *vm_map = NULL;
+
+	if (!cmdq->pdata->gce_vm)
+		return;
+
+	vm_map = kcalloc(cmdq->pdata->thread_nr, sizeof(*vm_map), GFP_KERNEL);
+	if (!vm_map)
+		return;
+
+	/* only configure the max CPR SRAM size to host vm (vm_id = 0) currently */
+	vm_cpr_gsize = GCE_VM_CPR_GSIZE_MAX << GCE_VM_CPR_GSIZE_FLD_SHIFT(0);
+
+	/* set all thread mapping to host vm currently */
+	for (i = 0; i < cmdq->pdata->thread_nr; i++)
+		vm_map[i] = GCE_VM_ID_MAP_HOST_VM << GCE_VM_ID_MAP_THR_FLD_SHIFT(i);
+
+	/* set the amount of CPR SRAM to allocate to each VM */
+	writel(vm_cpr_gsize, cmdq->base + GCE_VM_CPR_GSIZE);
+
+	/* config CPR_GSIZE before setting VM_ID_MAP to avoid data leakage */
+	for (i = 0; i < cmdq->pdata->thread_nr; i++) {
+		vm_id_map |= vm_map[i];
+		/* config every 10 threads, e.g., thread id=0~9, 10~19, ..., into one register */
+		if ((i + 1) % 10 == 0) {
+			writel(vm_id_map, cmdq->base + GCE_VM_ID_MAP(i));
+			vm_id_map = 0;
+		}
+	}
+	/* config remaining threads settings */
+	if (cmdq->pdata->thread_nr % 10 != 0)
+		writel(vm_id_map, cmdq->base + GCE_VM_ID_MAP(cmdq->pdata->thread_nr - 1));
+
+	kfree(vm_map);
+}
+
 static void cmdq_gctl_value_toggle(struct cmdq *cmdq, bool ddr_enable)
 {
 	u32 val = cmdq->pdata->control_by_sw ? GCE_CTRL_BY_SW : 0;
@@ -156,6 +214,7 @@ static void cmdq_init(struct cmdq *cmdq)
 
 	WARN_ON(clk_bulk_enable(cmdq->pdata->gce_num, cmdq->clocks));
 
+	cmdq_vm_init(cmdq);
 	cmdq_gctl_value_toggle(cmdq, true);
 
 	writel(CMDQ_THR_ACTIVE_SLOT_CYCLES, cmdq->base + CMDQ_THR_SLOT_CYCLES);
@@ -782,6 +841,16 @@ static const struct gce_plat gce_plat_mt8195 = {
 	.gce_num = 2
 };
 
+static const struct gce_plat gce_plat_mt8196 = {
+	.thread_nr = 32,
+	.shift = 3,
+	.mminfra_offset = SZ_2G,
+	.control_by_sw = true,
+	.sw_ddr_en = true,
+	.gce_vm = true,
+	.gce_num = 2
+};
+
 static const struct of_device_id cmdq_of_ids[] = {
 	{.compatible = "mediatek,mt6779-gce", .data = (void *)&gce_plat_mt6779},
 	{.compatible = "mediatek,mt8173-gce", .data = (void *)&gce_plat_mt8173},
@@ -790,6 +859,7 @@ static const struct of_device_id cmdq_of_ids[] = {
 	{.compatible = "mediatek,mt8188-gce", .data = (void *)&gce_plat_mt8188},
 	{.compatible = "mediatek,mt8192-gce", .data = (void *)&gce_plat_mt8192},
 	{.compatible = "mediatek,mt8195-gce", .data = (void *)&gce_plat_mt8195},
+	{.compatible = "mediatek,mt8196-gce", .data = (void *)&gce_plat_mt8196},
 	{}
 };
 MODULE_DEVICE_TABLE(of, cmdq_of_ids);
diff --git a/drivers/soc/mediatek/mtk-cmdq-helper.c b/drivers/soc/mediatek/mtk-cmdq-helper.c
index 455221e8de24..67e5879374ac 100644
--- a/drivers/soc/mediatek/mtk-cmdq-helper.c
+++ b/drivers/soc/mediatek/mtk-cmdq-helper.c
@@ -8,6 +8,7 @@
 #include <linux/module.h>
 #include <linux/mailbox_controller.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/soc/mediatek/mtk-cmdq.h>
 
 #define CMDQ_WRITE_ENABLE_MASK	BIT(0)
@@ -60,20 +61,41 @@ int cmdq_dev_get_client_reg(struct device *dev,
 			    struct cmdq_client_reg *client_reg, int idx)
 {
 	struct of_phandle_args spec;
+	struct resource res;
 	int err;
 
 	if (!client_reg)
 		return -ENOENT;
 
+	err = of_address_to_resource(dev->of_node, 0, &res);
+	if (err) {
+		dev_err(dev, "Missing reg in %s node\n", dev->of_node->full_name);
+		return -EINVAL;
+	}
+	client_reg->pa_base = res.start;
+
 	err = of_parse_phandle_with_fixed_args(dev->of_node,
 					       "mediatek,gce-client-reg",
 					       3, idx, &spec);
 	if (err < 0) {
-		dev_warn(dev,
+		dev_dbg(dev,
 			"error %d can't parse gce-client-reg property (%d)",
 			err, idx);
 
-		return err;
+		/* make subsys invalid */
+		client_reg->subsys = CMDQ_SUBSYS_INVALID;
+
+		/*
+		 * All GCEs support writing register PA with mask without subsys,
+		 * but this requires extra GCE instructions to convert the PA into
+		 * a format that GCE can handle, which is less performance than
+		 * directly using subsys. Therefore, when subsys is available,
+		 * we prefer to use subsys for writing register PA.
+		 */
+		client_reg->pkt_write = cmdq_pkt_write_pa;
+		client_reg->pkt_write_mask = cmdq_pkt_write_mask_pa;
+
+		return 0;
 	}
 
 	client_reg->subsys = (u8)spec.args[0];
@@ -81,6 +103,9 @@ int cmdq_dev_get_client_reg(struct device *dev,
 	client_reg->size = (u16)spec.args[2];
 	of_node_put(spec.np);
 
+	client_reg->pkt_write = cmdq_pkt_write_subsys;
+	client_reg->pkt_write_mask = cmdq_pkt_write_mask_subsys;
+
 	return 0;
 }
 EXPORT_SYMBOL(cmdq_dev_get_client_reg);
@@ -140,6 +165,7 @@ int cmdq_pkt_create(struct cmdq_client *client, struct cmdq_pkt *pkt, size_t siz
 	}
 
 	pkt->pa_base = dma_addr;
+	cmdq_get_mbox_priv(client->chan, &pkt->priv);
 
 	return 0;
 }
@@ -201,6 +227,26 @@ int cmdq_pkt_write(struct cmdq_pkt *pkt, u8 subsys, u16 offset, u32 value)
 }
 EXPORT_SYMBOL(cmdq_pkt_write);
 
+int cmdq_pkt_write_pa(struct cmdq_pkt *pkt, u8 subsys /*unused*/, u32 pa_base,
+		      u16 offset, u32 value)
+{
+	int err;
+
+	err = cmdq_pkt_assign(pkt, CMDQ_THR_SPR_IDX0, CMDQ_ADDR_HIGH(pa_base));
+	if (err < 0)
+		return err;
+
+	return cmdq_pkt_write_s_value(pkt, CMDQ_THR_SPR_IDX0, CMDQ_ADDR_LOW(offset), value);
+}
+EXPORT_SYMBOL(cmdq_pkt_write_pa);
+
+int cmdq_pkt_write_subsys(struct cmdq_pkt *pkt, u8 subsys, u32 pa_base /*unused*/,
+			  u16 offset, u32 value)
+{
+	return cmdq_pkt_write(pkt, subsys, offset, value);
+}
+EXPORT_SYMBOL(cmdq_pkt_write_subsys);
+
 int cmdq_pkt_write_mask(struct cmdq_pkt *pkt, u8 subsys,
 			u16 offset, u32 value, u32 mask)
 {
@@ -218,6 +264,27 @@ int cmdq_pkt_write_mask(struct cmdq_pkt *pkt, u8 subsys,
 }
 EXPORT_SYMBOL(cmdq_pkt_write_mask);
 
+int cmdq_pkt_write_mask_pa(struct cmdq_pkt *pkt, u8 subsys /*unused*/, u32 pa_base,
+			   u16 offset, u32 value, u32 mask)
+{
+	int err;
+
+	err = cmdq_pkt_assign(pkt, CMDQ_THR_SPR_IDX0, CMDQ_ADDR_HIGH(pa_base));
+	if (err < 0)
+		return err;
+
+	return cmdq_pkt_write_s_mask_value(pkt, CMDQ_THR_SPR_IDX0,
+					   CMDQ_ADDR_LOW(offset), value, mask);
+}
+EXPORT_SYMBOL(cmdq_pkt_write_mask_pa);
+
+int cmdq_pkt_write_mask_subsys(struct cmdq_pkt *pkt, u8 subsys, u32 pa_base /*unused*/,
+			       u16 offset, u32 value, u32 mask)
+{
+	return cmdq_pkt_write_mask(pkt, subsys, offset, value, mask);
+}
+EXPORT_SYMBOL(cmdq_pkt_write_mask_subsys);
+
 int cmdq_pkt_read_s(struct cmdq_pkt *pkt, u16 high_addr_reg_idx, u16 addr_low,
 		    u16 reg_idx)
 {
@@ -305,6 +372,7 @@ int cmdq_pkt_mem_move(struct cmdq_pkt *pkt, dma_addr_t src_addr, dma_addr_t dst_
 	int ret;
 
 	/* read the value of src_addr into high_addr_reg_idx */
+	src_addr += pkt->priv.mminfra_offset;
 	ret = cmdq_pkt_assign(pkt, high_addr_reg_idx, CMDQ_ADDR_HIGH(src_addr));
 	if (ret < 0)
 		return ret;
@@ -313,6 +381,7 @@ int cmdq_pkt_mem_move(struct cmdq_pkt *pkt, dma_addr_t src_addr, dma_addr_t dst_
 		return ret;
 
 	/* write the value of value_reg_idx into dst_addr */
+	dst_addr += pkt->priv.mminfra_offset;
 	ret = cmdq_pkt_assign(pkt, high_addr_reg_idx, CMDQ_ADDR_HIGH(dst_addr));
 	if (ret < 0)
 		return ret;
@@ -438,7 +507,7 @@ int cmdq_pkt_poll_addr(struct cmdq_pkt *pkt, dma_addr_t addr, u32 value, u32 mas
 	inst.op = CMDQ_CODE_MASK;
 	inst.dst_t = CMDQ_REG_TYPE;
 	inst.sop = CMDQ_POLL_ADDR_GPR;
-	inst.value = addr;
+	inst.value = addr + pkt->priv.mminfra_offset;
 	ret = cmdq_pkt_append_command(pkt, inst);
 	if (ret < 0)
 		return ret;
@@ -498,7 +567,7 @@ int cmdq_pkt_jump_abs(struct cmdq_pkt *pkt, dma_addr_t addr, u8 shift_pa)
 	struct cmdq_instruction inst = {
 		.op = CMDQ_CODE_JUMP,
 		.offset = CMDQ_JUMP_ABSOLUTE,
-		.value = addr >> shift_pa
+		.value = (addr +  pkt->priv.mminfra_offset) >> pkt->priv.shift_pa
 	};
 	return cmdq_pkt_append_command(pkt, inst);
 }
diff --git a/drivers/soc/mediatek/mtk-dvfsrc.c b/drivers/soc/mediatek/mtk-dvfsrc.c
index 41add5636b03..548a28f50242 100644
--- a/drivers/soc/mediatek/mtk-dvfsrc.c
+++ b/drivers/soc/mediatek/mtk-dvfsrc.c
@@ -7,6 +7,7 @@
 
 #include <linux/arm-smccc.h>
 #include <linux/bitfield.h>
+#include <linux/clk.h>
 #include <linux/iopoll.h>
 #include <linux/module.h>
 #include <linux/of.h>
@@ -15,11 +16,17 @@
 #include <linux/soc/mediatek/dvfsrc.h>
 #include <linux/soc/mediatek/mtk_sip_svc.h>
 
+/* DVFSRC_BASIC_CONTROL */
+#define DVFSRC_V4_BASIC_CTRL_OPP_COUNT	GENMASK(26, 20)
+
 /* DVFSRC_LEVEL */
 #define DVFSRC_V1_LEVEL_TARGET_LEVEL	GENMASK(15, 0)
 #define DVFSRC_TGT_LEVEL_IDLE		0x00
 #define DVFSRC_V1_LEVEL_CURRENT_LEVEL	GENMASK(31, 16)
 
+#define DVFSRC_V4_LEVEL_TARGET_LEVEL	GENMASK(15, 8)
+#define DVFSRC_V4_LEVEL_TARGET_PRESENT	BIT(16)
+
 /* DVFSRC_SW_REQ, DVFSRC_SW_REQ2 */
 #define DVFSRC_V1_SW_REQ2_DRAM_LEVEL	GENMASK(1, 0)
 #define DVFSRC_V1_SW_REQ2_VCORE_LEVEL	GENMASK(3, 2)
@@ -27,24 +34,40 @@
 #define DVFSRC_V2_SW_REQ_DRAM_LEVEL	GENMASK(3, 0)
 #define DVFSRC_V2_SW_REQ_VCORE_LEVEL	GENMASK(6, 4)
 
+#define DVFSRC_V4_SW_REQ_EMI_LEVEL	GENMASK(3, 0)
+#define DVFSRC_V4_SW_REQ_DRAM_LEVEL	GENMASK(15, 12)
+
 /* DVFSRC_VCORE */
 #define DVFSRC_V2_VCORE_REQ_VSCP_LEVEL	GENMASK(14, 12)
 
+/* DVFSRC_TARGET_GEAR */
+#define DVFSRC_V4_GEAR_TARGET_DRAM	GENMASK(7, 0)
+#define DVFSRC_V4_GEAR_TARGET_VCORE	GENMASK(15, 8)
+
+/* DVFSRC_GEAR_INFO */
+#define DVFSRC_V4_GEAR_INFO_REG_WIDTH	0x4
+#define DVFSRC_V4_GEAR_INFO_REG_LEVELS	64
+#define DVFSRC_V4_GEAR_INFO_VCORE	GENMASK(3, 0)
+#define DVFSRC_V4_GEAR_INFO_EMI		GENMASK(7, 4)
+#define DVFSRC_V4_GEAR_INFO_DRAM	GENMASK(15, 12)
+
 #define DVFSRC_POLL_TIMEOUT_US		1000
 #define STARTUP_TIME_US			1
 
 #define MTK_SIP_DVFSRC_INIT		0x0
 #define MTK_SIP_DVFSRC_START		0x1
 
-struct dvfsrc_bw_constraints {
-	u16 max_dram_nom_bw;
-	u16 max_dram_peak_bw;
-	u16 max_dram_hrt_bw;
+enum mtk_dvfsrc_bw_type {
+	DVFSRC_BW_AVG,
+	DVFSRC_BW_PEAK,
+	DVFSRC_BW_HRT,
+	DVFSRC_BW_MAX,
 };
 
 struct dvfsrc_opp {
 	u32 vcore_opp;
 	u32 dram_opp;
+	u32 emi_opp;
 };
 
 struct dvfsrc_opp_desc {
@@ -55,6 +78,7 @@ struct dvfsrc_opp_desc {
 struct dvfsrc_soc_data;
 struct mtk_dvfsrc {
 	struct device *dev;
+	struct clk *clk;
 	struct platform_device *icc;
 	struct platform_device *regulator;
 	const struct dvfsrc_soc_data *dvd;
@@ -65,11 +89,16 @@ struct mtk_dvfsrc {
 
 struct dvfsrc_soc_data {
 	const int *regs;
+	const u8 *bw_units;
+	const bool has_emi_ddr;
 	const struct dvfsrc_opp_desc *opps_desc;
+	u32 (*calc_dram_bw)(struct mtk_dvfsrc *dvfsrc, enum mtk_dvfsrc_bw_type type, u64 bw);
 	u32 (*get_target_level)(struct mtk_dvfsrc *dvfsrc);
 	u32 (*get_current_level)(struct mtk_dvfsrc *dvfsrc);
 	u32 (*get_vcore_level)(struct mtk_dvfsrc *dvfsrc);
 	u32 (*get_vscp_level)(struct mtk_dvfsrc *dvfsrc);
+	u32 (*get_opp_count)(struct mtk_dvfsrc *dvfsrc);
+	int (*get_hw_opps)(struct mtk_dvfsrc *dvfsrc);
 	void (*set_dram_bw)(struct mtk_dvfsrc *dvfsrc, u64 bw);
 	void (*set_dram_peak_bw)(struct mtk_dvfsrc *dvfsrc, u64 bw);
 	void (*set_dram_hrt_bw)(struct mtk_dvfsrc *dvfsrc, u64 bw);
@@ -78,7 +107,22 @@ struct dvfsrc_soc_data {
 	void (*set_vscp_level)(struct mtk_dvfsrc *dvfsrc, u32 level);
 	int (*wait_for_opp_level)(struct mtk_dvfsrc *dvfsrc, u32 level);
 	int (*wait_for_vcore_level)(struct mtk_dvfsrc *dvfsrc, u32 level);
-	const struct dvfsrc_bw_constraints *bw_constraints;
+
+	/**
+	 * @bw_max_constraints - array of maximum bandwidth for this hardware
+	 *
+	 * indexed by &enum mtk_dvfsrc_bw_type, storing the maximum permissible
+	 * hardware value for each bandwidth type.
+	 */
+	const u32 *const bw_max_constraints;
+
+	/**
+	 * @bw_min_constraints - array of minimum bandwidth for this hardware
+	 *
+	 * indexed by &enum mtk_dvfsrc_bw_type, storing the minimum permissible
+	 * hardware value for each bandwidth type.
+	 */
+	const u32 *const bw_min_constraints;
 };
 
 static u32 dvfsrc_readl(struct mtk_dvfsrc *dvfs, u32 offset)
@@ -92,6 +136,7 @@ static void dvfsrc_writel(struct mtk_dvfsrc *dvfs, u32 offset, u32 val)
 }
 
 enum dvfsrc_regs {
+	DVFSRC_BASIC_CONTROL,
 	DVFSRC_SW_REQ,
 	DVFSRC_SW_REQ2,
 	DVFSRC_LEVEL,
@@ -99,7 +144,11 @@ enum dvfsrc_regs {
 	DVFSRC_SW_BW,
 	DVFSRC_SW_PEAK_BW,
 	DVFSRC_SW_HRT_BW,
+	DVFSRC_SW_EMI_BW,
 	DVFSRC_VCORE,
+	DVFSRC_TARGET_GEAR,
+	DVFSRC_GEAR_INFO_L,
+	DVFSRC_GEAR_INFO_H,
 	DVFSRC_REGS_MAX,
 };
 
@@ -120,6 +169,22 @@ static const int dvfsrc_mt8195_regs[] = {
 	[DVFSRC_TARGET_LEVEL] = 0xd48,
 };
 
+static const int dvfsrc_mt8196_regs[] = {
+	[DVFSRC_BASIC_CONTROL] = 0x0,
+	[DVFSRC_SW_REQ] = 0x18,
+	[DVFSRC_VCORE] = 0x80,
+	[DVFSRC_GEAR_INFO_L] = 0xfc,
+	[DVFSRC_SW_BW] = 0x1e8,
+	[DVFSRC_SW_PEAK_BW] = 0x1f4,
+	[DVFSRC_SW_HRT_BW] = 0x20c,
+	[DVFSRC_LEVEL] = 0x5f0,
+	[DVFSRC_TARGET_LEVEL] = 0x5f0,
+	[DVFSRC_SW_REQ2] = 0x604,
+	[DVFSRC_SW_EMI_BW] = 0x60c,
+	[DVFSRC_TARGET_GEAR] = 0x6ac,
+	[DVFSRC_GEAR_INFO_H] = 0x6b0,
+};
+
 static const struct dvfsrc_opp *dvfsrc_get_current_opp(struct mtk_dvfsrc *dvfsrc)
 {
 	u32 level = dvfsrc->dvd->get_current_level(dvfsrc);
@@ -127,6 +192,20 @@ static const struct dvfsrc_opp *dvfsrc_get_current_opp(struct mtk_dvfsrc *dvfsrc
 	return &dvfsrc->curr_opps->opps[level];
 }
 
+static u32 dvfsrc_get_current_target_vcore_gear(struct mtk_dvfsrc *dvfsrc)
+{
+	u32 val = dvfsrc_readl(dvfsrc, DVFSRC_TARGET_GEAR);
+
+	return FIELD_GET(DVFSRC_V4_GEAR_TARGET_VCORE, val);
+}
+
+static u32 dvfsrc_get_current_target_dram_gear(struct mtk_dvfsrc *dvfsrc)
+{
+	u32 val = dvfsrc_readl(dvfsrc, DVFSRC_TARGET_GEAR);
+
+	return FIELD_GET(DVFSRC_V4_GEAR_TARGET_DRAM, val);
+}
+
 static bool dvfsrc_is_idle(struct mtk_dvfsrc *dvfsrc)
 {
 	if (!dvfsrc->dvd->get_target_level)
@@ -183,6 +262,24 @@ static int dvfsrc_wait_for_opp_level_v2(struct mtk_dvfsrc *dvfsrc, u32 level)
 	return 0;
 }
 
+static int dvfsrc_wait_for_vcore_level_v4(struct mtk_dvfsrc *dvfsrc, u32 level)
+{
+	u32 val;
+
+	return readx_poll_timeout_atomic(dvfsrc_get_current_target_vcore_gear,
+					 dvfsrc, val, val >= level,
+					 STARTUP_TIME_US, DVFSRC_POLL_TIMEOUT_US);
+}
+
+static int dvfsrc_wait_for_opp_level_v4(struct mtk_dvfsrc *dvfsrc, u32 level)
+{
+	u32 val;
+
+	return readx_poll_timeout_atomic(dvfsrc_get_current_target_dram_gear,
+					 dvfsrc, val, val >= level,
+					 STARTUP_TIME_US, DVFSRC_POLL_TIMEOUT_US);
+}
+
 static u32 dvfsrc_get_target_level_v1(struct mtk_dvfsrc *dvfsrc)
 {
 	u32 val = dvfsrc_readl(dvfsrc, DVFSRC_LEVEL);
@@ -216,6 +313,27 @@ static u32 dvfsrc_get_current_level_v2(struct mtk_dvfsrc *dvfsrc)
 	return 0;
 }
 
+static u32 dvfsrc_get_target_level_v4(struct mtk_dvfsrc *dvfsrc)
+{
+	u32 val = dvfsrc_readl(dvfsrc, DVFSRC_TARGET_LEVEL);
+
+	if (val & DVFSRC_V4_LEVEL_TARGET_PRESENT)
+		return FIELD_GET(DVFSRC_V4_LEVEL_TARGET_LEVEL, val) + 1;
+	return 0;
+}
+
+static u32 dvfsrc_get_current_level_v4(struct mtk_dvfsrc *dvfsrc)
+{
+	u32 level = dvfsrc_readl(dvfsrc, DVFSRC_LEVEL) + 1;
+
+	/* Valid levels */
+	if (level < dvfsrc->curr_opps->num_opp)
+		return dvfsrc->curr_opps->num_opp - level;
+
+	/* Zero for level 0 or invalid level */
+	return 0;
+}
+
 static u32 dvfsrc_get_vcore_level_v1(struct mtk_dvfsrc *dvfsrc)
 {
 	u32 val = dvfsrc_readl(dvfsrc, DVFSRC_SW_REQ2);
@@ -267,39 +385,69 @@ static void dvfsrc_set_vscp_level_v2(struct mtk_dvfsrc *dvfsrc, u32 level)
 	dvfsrc_writel(dvfsrc, DVFSRC_VCORE, val);
 }
 
+static u32 dvfsrc_get_opp_count_v4(struct mtk_dvfsrc *dvfsrc)
+{
+	u32 val = dvfsrc_readl(dvfsrc, DVFSRC_BASIC_CONTROL);
+
+	return FIELD_GET(DVFSRC_V4_BASIC_CTRL_OPP_COUNT, val) + 1;
+}
+
+static u32
+dvfsrc_calc_dram_bw_v1(struct mtk_dvfsrc *dvfsrc, enum mtk_dvfsrc_bw_type type, u64 bw)
+{
+	return clamp_val(div_u64(bw, 100 * 1000), dvfsrc->dvd->bw_min_constraints[type],
+			 dvfsrc->dvd->bw_max_constraints[type]);
+}
+
+/**
+ * dvfsrc_calc_dram_bw_v4 - convert kbps to hardware register bandwidth value
+ * @dvfsrc: pointer to the &struct mtk_dvfsrc of this driver instance
+ * @type: one of %DVFSRC_BW_AVG, %DVFSRC_BW_PEAK, or %DVFSRC_BW_HRT
+ * @bw: the bandwidth in kilobits per second
+ *
+ * Returns the hardware register value appropriate for expressing @bw, clamped
+ * to hardware limits.
+ */
+static u32
+dvfsrc_calc_dram_bw_v4(struct mtk_dvfsrc *dvfsrc, enum mtk_dvfsrc_bw_type type, u64 bw)
+{
+	u8 bw_unit = dvfsrc->dvd->bw_units[type];
+	u64 bw_mbps;
+	u32 bw_hw;
+
+	if (type < DVFSRC_BW_AVG || type >= DVFSRC_BW_MAX)
+		return 0;
+
+	bw_mbps = div_u64(bw, 1000);
+	bw_hw = div_u64((bw_mbps + bw_unit - 1), bw_unit);
+	return clamp_val(bw_hw, dvfsrc->dvd->bw_min_constraints[type],
+			 dvfsrc->dvd->bw_max_constraints[type]);
+}
+
 static void __dvfsrc_set_dram_bw_v1(struct mtk_dvfsrc *dvfsrc, u32 reg,
-				    u16 max_bw, u16 min_bw, u64 bw)
+				    enum mtk_dvfsrc_bw_type type, u64 bw)
 {
-	u32 new_bw = (u32)div_u64(bw, 100 * 1000);
+	u32 bw_hw = dvfsrc->dvd->calc_dram_bw(dvfsrc, type, bw);
 
-	/* If bw constraints (in mbps) are defined make sure to respect them */
-	if (max_bw)
-		new_bw = min(new_bw, max_bw);
-	if (min_bw && new_bw > 0)
-		new_bw = max(new_bw, min_bw);
+	dvfsrc_writel(dvfsrc, reg, bw_hw);
 
-	dvfsrc_writel(dvfsrc, reg, new_bw);
+	if (type == DVFSRC_BW_AVG && dvfsrc->dvd->has_emi_ddr)
+		dvfsrc_writel(dvfsrc, DVFSRC_SW_EMI_BW, bw_hw);
 }
 
 static void dvfsrc_set_dram_bw_v1(struct mtk_dvfsrc *dvfsrc, u64 bw)
 {
-	u64 max_bw = dvfsrc->dvd->bw_constraints->max_dram_nom_bw;
-
-	__dvfsrc_set_dram_bw_v1(dvfsrc, DVFSRC_SW_BW, max_bw, 0, bw);
+	__dvfsrc_set_dram_bw_v1(dvfsrc, DVFSRC_SW_BW, DVFSRC_BW_AVG, bw);
 };
 
 static void dvfsrc_set_dram_peak_bw_v1(struct mtk_dvfsrc *dvfsrc, u64 bw)
 {
-	u64 max_bw = dvfsrc->dvd->bw_constraints->max_dram_peak_bw;
-
-	__dvfsrc_set_dram_bw_v1(dvfsrc, DVFSRC_SW_PEAK_BW, max_bw, 0, bw);
+	__dvfsrc_set_dram_bw_v1(dvfsrc, DVFSRC_SW_PEAK_BW, DVFSRC_BW_PEAK, bw);
 }
 
 static void dvfsrc_set_dram_hrt_bw_v1(struct mtk_dvfsrc *dvfsrc, u64 bw)
 {
-	u64 max_bw = dvfsrc->dvd->bw_constraints->max_dram_hrt_bw;
-
-	__dvfsrc_set_dram_bw_v1(dvfsrc, DVFSRC_SW_HRT_BW, max_bw, 0, bw);
+	__dvfsrc_set_dram_bw_v1(dvfsrc, DVFSRC_SW_HRT_BW, DVFSRC_BW_HRT, bw);
 }
 
 static void dvfsrc_set_opp_level_v1(struct mtk_dvfsrc *dvfsrc, u32 level)
@@ -315,6 +463,100 @@ static void dvfsrc_set_opp_level_v1(struct mtk_dvfsrc *dvfsrc, u32 level)
 	dvfsrc_writel(dvfsrc, DVFSRC_SW_REQ, val);
 }
 
+static u32 dvfsrc_get_opp_gear(struct mtk_dvfsrc *dvfsrc, u8 level)
+{
+	u32 reg_ofst, val;
+	u8 idx;
+
+	/* Calculate register offset and index for requested gear */
+	if (level < DVFSRC_V4_GEAR_INFO_REG_LEVELS) {
+		reg_ofst = dvfsrc->dvd->regs[DVFSRC_GEAR_INFO_L];
+		idx = level;
+	} else {
+		reg_ofst = dvfsrc->dvd->regs[DVFSRC_GEAR_INFO_H];
+		idx = level - DVFSRC_V4_GEAR_INFO_REG_LEVELS;
+	}
+	reg_ofst += DVFSRC_V4_GEAR_INFO_REG_WIDTH * (level / 2);
+
+	/* Read the corresponding gear register */
+	val = readl(dvfsrc->regs + reg_ofst);
+
+	/* Each register contains two sets of data, 16 bits per gear */
+	val >>= 16 * (idx % 2);
+
+	return val;
+}
+
+static int dvfsrc_get_hw_opps_v4(struct mtk_dvfsrc *dvfsrc)
+{
+	struct dvfsrc_opp *dvfsrc_opps;
+	struct dvfsrc_opp_desc *desc;
+	u32 num_opps, gear_info;
+	u8 num_vcore, num_dram;
+	u8 num_emi;
+	int i;
+
+	num_opps = dvfsrc_get_opp_count_v4(dvfsrc);
+	if (num_opps == 0) {
+		dev_err(dvfsrc->dev, "No OPPs programmed in DVFSRC MCU.\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * The first 16 bits set in the gear info table says how many OPPs
+	 * and how many vcore, dram and emi table entries are available.
+	 */
+	gear_info = dvfsrc_readl(dvfsrc, DVFSRC_GEAR_INFO_L);
+	if (gear_info == 0) {
+		dev_err(dvfsrc->dev, "No gear info in DVFSRC MCU.\n");
+		return -EINVAL;
+	}
+
+	num_vcore = FIELD_GET(DVFSRC_V4_GEAR_INFO_VCORE, gear_info) + 1;
+	num_dram = FIELD_GET(DVFSRC_V4_GEAR_INFO_DRAM, gear_info) + 1;
+	num_emi = FIELD_GET(DVFSRC_V4_GEAR_INFO_EMI, gear_info) + 1;
+	dev_info(dvfsrc->dev,
+		 "Discovered %u gears and %u vcore, %u dram, %u emi table entries.\n",
+		 num_opps, num_vcore, num_dram, num_emi);
+
+	/* Allocate everything now as anything else after that cannot fail */
+	desc = devm_kzalloc(dvfsrc->dev, sizeof(*desc), GFP_KERNEL);
+	if (!desc)
+		return -ENOMEM;
+
+	dvfsrc_opps = devm_kcalloc(dvfsrc->dev, num_opps + 1,
+				   sizeof(*dvfsrc_opps), GFP_KERNEL);
+	if (!dvfsrc_opps)
+		return -ENOMEM;
+
+	/* Read the OPP table gear indices */
+	for (i = 0; i <= num_opps; i++) {
+		gear_info = dvfsrc_get_opp_gear(dvfsrc, num_opps - i);
+		dvfsrc_opps[i].vcore_opp = FIELD_GET(DVFSRC_V4_GEAR_INFO_VCORE, gear_info);
+		dvfsrc_opps[i].dram_opp = FIELD_GET(DVFSRC_V4_GEAR_INFO_DRAM, gear_info);
+		dvfsrc_opps[i].emi_opp = FIELD_GET(DVFSRC_V4_GEAR_INFO_EMI, gear_info);
+	};
+	desc->num_opp = num_opps + 1;
+	desc->opps = dvfsrc_opps;
+
+	/* Assign to main structure now that everything is done! */
+	dvfsrc->curr_opps = desc;
+
+	return 0;
+}
+
+static void dvfsrc_set_dram_level_v4(struct mtk_dvfsrc *dvfsrc, u32 level)
+{
+	u32 val = dvfsrc_readl(dvfsrc, DVFSRC_SW_REQ);
+
+	val &= ~DVFSRC_V4_SW_REQ_DRAM_LEVEL;
+	val |= FIELD_PREP(DVFSRC_V4_SW_REQ_DRAM_LEVEL, level);
+
+	dev_dbg(dvfsrc->dev, "%s level=%u\n", __func__, level);
+
+	dvfsrc_writel(dvfsrc, DVFSRC_SW_REQ, val);
+}
+
 int mtk_dvfsrc_send_request(const struct device *dev, u32 cmd, u64 data)
 {
 	struct mtk_dvfsrc *dvfsrc = dev_get_drvdata(dev);
@@ -422,6 +664,11 @@ static int mtk_dvfsrc_probe(struct platform_device *pdev)
 	if (IS_ERR(dvfsrc->regs))
 		return PTR_ERR(dvfsrc->regs);
 
+	dvfsrc->clk = devm_clk_get_enabled(&pdev->dev, NULL);
+	if (IS_ERR(dvfsrc->clk))
+		return dev_err_probe(&pdev->dev, PTR_ERR(dvfsrc->clk),
+				     "Couldn't get and enable DVFSRC clock\n");
+
 	arm_smccc_smc(MTK_SIP_DVFSRC_VCOREFS_CONTROL, MTK_SIP_DVFSRC_INIT,
 		      0, 0, 0, 0, 0, 0, &ares);
 	if (ares.a0)
@@ -430,7 +677,14 @@ static int mtk_dvfsrc_probe(struct platform_device *pdev)
 	dvfsrc->dram_type = ares.a1;
 	dev_dbg(&pdev->dev, "DRAM Type: %d\n", dvfsrc->dram_type);
 
-	dvfsrc->curr_opps = &dvfsrc->dvd->opps_desc[dvfsrc->dram_type];
+	/* Newer versions of the DVFSRC MCU have pre-programmed gear tables */
+	if (dvfsrc->dvd->get_hw_opps) {
+		ret = dvfsrc->dvd->get_hw_opps(dvfsrc);
+		if (ret)
+			return ret;
+	} else {
+		dvfsrc->curr_opps = &dvfsrc->dvd->opps_desc[dvfsrc->dram_type];
+	}
 	platform_set_drvdata(pdev, dvfsrc);
 
 	ret = devm_of_platform_populate(&pdev->dev);
@@ -440,17 +694,28 @@ static int mtk_dvfsrc_probe(struct platform_device *pdev)
 	/* Everything is set up - make it run! */
 	arm_smccc_smc(MTK_SIP_DVFSRC_VCOREFS_CONTROL, MTK_SIP_DVFSRC_START,
 		      0, 0, 0, 0, 0, 0, &ares);
-	if (ares.a0)
+	if (ares.a0 & BIT(0))
 		return dev_err_probe(&pdev->dev, -EINVAL, "Cannot start DVFSRC: %lu\n", ares.a0);
 
 	return 0;
 }
 
-static const struct dvfsrc_bw_constraints dvfsrc_bw_constr_v1 = { 0, 0, 0 };
-static const struct dvfsrc_bw_constraints dvfsrc_bw_constr_v2 = {
-	.max_dram_nom_bw = 255,
-	.max_dram_peak_bw = 255,
-	.max_dram_hrt_bw = 1023,
+static const u32 dvfsrc_bw_min_constr_none[DVFSRC_BW_MAX] = {
+	[DVFSRC_BW_AVG] = 0,
+	[DVFSRC_BW_PEAK] = 0,
+	[DVFSRC_BW_HRT] = 0,
+};
+
+static const u32 dvfsrc_bw_max_constr_v1[DVFSRC_BW_MAX] = {
+	[DVFSRC_BW_AVG] = U32_MAX,
+	[DVFSRC_BW_PEAK] = U32_MAX,
+	[DVFSRC_BW_HRT] = U32_MAX,
+};
+
+static const u32 dvfsrc_bw_max_constr_v2[DVFSRC_BW_MAX] = {
+	[DVFSRC_BW_AVG] = 65535,
+	[DVFSRC_BW_PEAK] = 65535,
+	[DVFSRC_BW_HRT] = 1023,
 };
 
 static const struct dvfsrc_opp dvfsrc_opp_mt6893_lp4[] = {
@@ -483,7 +748,8 @@ static const struct dvfsrc_soc_data mt6893_data = {
 	.set_vscp_level = dvfsrc_set_vscp_level_v2,
 	.wait_for_opp_level = dvfsrc_wait_for_opp_level_v2,
 	.wait_for_vcore_level = dvfsrc_wait_for_vcore_level_v1,
-	.bw_constraints = &dvfsrc_bw_constr_v2,
+	.bw_max_constraints = dvfsrc_bw_max_constr_v2,
+	.bw_min_constraints = dvfsrc_bw_min_constr_none,
 };
 
 static const struct dvfsrc_opp dvfsrc_opp_mt8183_lp4[] = {
@@ -512,6 +778,7 @@ static const struct dvfsrc_opp_desc dvfsrc_opp_mt8183_desc[] = {
 static const struct dvfsrc_soc_data mt8183_data = {
 	.opps_desc = dvfsrc_opp_mt8183_desc,
 	.regs = dvfsrc_mt8183_regs,
+	.calc_dram_bw = dvfsrc_calc_dram_bw_v1,
 	.get_target_level = dvfsrc_get_target_level_v1,
 	.get_current_level = dvfsrc_get_current_level_v1,
 	.get_vcore_level = dvfsrc_get_vcore_level_v1,
@@ -520,7 +787,8 @@ static const struct dvfsrc_soc_data mt8183_data = {
 	.set_vcore_level = dvfsrc_set_vcore_level_v1,
 	.wait_for_opp_level = dvfsrc_wait_for_opp_level_v1,
 	.wait_for_vcore_level = dvfsrc_wait_for_vcore_level_v1,
-	.bw_constraints = &dvfsrc_bw_constr_v1,
+	.bw_max_constraints = dvfsrc_bw_max_constr_v1,
+	.bw_min_constraints = dvfsrc_bw_min_constr_none,
 };
 
 static const struct dvfsrc_opp dvfsrc_opp_mt8195_lp4[] = {
@@ -542,6 +810,7 @@ static const struct dvfsrc_opp_desc dvfsrc_opp_mt8195_desc[] = {
 static const struct dvfsrc_soc_data mt8195_data = {
 	.opps_desc = dvfsrc_opp_mt8195_desc,
 	.regs = dvfsrc_mt8195_regs,
+	.calc_dram_bw = dvfsrc_calc_dram_bw_v1,
 	.get_target_level = dvfsrc_get_target_level_v2,
 	.get_current_level = dvfsrc_get_current_level_v2,
 	.get_vcore_level = dvfsrc_get_vcore_level_v2,
@@ -553,13 +822,44 @@ static const struct dvfsrc_soc_data mt8195_data = {
 	.set_vscp_level = dvfsrc_set_vscp_level_v2,
 	.wait_for_opp_level = dvfsrc_wait_for_opp_level_v2,
 	.wait_for_vcore_level = dvfsrc_wait_for_vcore_level_v1,
-	.bw_constraints = &dvfsrc_bw_constr_v2,
+	.bw_max_constraints = dvfsrc_bw_max_constr_v2,
+	.bw_min_constraints = dvfsrc_bw_min_constr_none,
+};
+
+static const u8 mt8196_bw_units[] = {
+	[DVFSRC_BW_AVG] = 64,
+	[DVFSRC_BW_PEAK] = 64,
+	[DVFSRC_BW_HRT] = 30,
+};
+
+static const struct dvfsrc_soc_data mt8196_data = {
+	.regs = dvfsrc_mt8196_regs,
+	.bw_units = mt8196_bw_units,
+	.has_emi_ddr = true,
+	.get_target_level = dvfsrc_get_target_level_v4,
+	.get_current_level = dvfsrc_get_current_level_v4,
+	.get_vcore_level = dvfsrc_get_vcore_level_v2,
+	.get_vscp_level = dvfsrc_get_vscp_level_v2,
+	.get_opp_count = dvfsrc_get_opp_count_v4,
+	.get_hw_opps = dvfsrc_get_hw_opps_v4,
+	.calc_dram_bw = dvfsrc_calc_dram_bw_v4,
+	.set_dram_bw = dvfsrc_set_dram_bw_v1,
+	.set_dram_peak_bw = dvfsrc_set_dram_peak_bw_v1,
+	.set_dram_hrt_bw = dvfsrc_set_dram_hrt_bw_v1,
+	.set_opp_level = dvfsrc_set_dram_level_v4,
+	.set_vcore_level = dvfsrc_set_vcore_level_v2,
+	.set_vscp_level = dvfsrc_set_vscp_level_v2,
+	.wait_for_opp_level = dvfsrc_wait_for_opp_level_v4,
+	.wait_for_vcore_level = dvfsrc_wait_for_vcore_level_v4,
+	.bw_max_constraints = dvfsrc_bw_max_constr_v2,
+	.bw_min_constraints = dvfsrc_bw_min_constr_none,
 };
 
 static const struct of_device_id mtk_dvfsrc_of_match[] = {
 	{ .compatible = "mediatek,mt6893-dvfsrc", .data = &mt6893_data },
 	{ .compatible = "mediatek,mt8183-dvfsrc", .data = &mt8183_data },
 	{ .compatible = "mediatek,mt8195-dvfsrc", .data = &mt8195_data },
+	{ .compatible = "mediatek,mt8196-dvfsrc", .data = &mt8196_data },
 	{ /* sentinel */ }
 };
 
diff --git a/drivers/soc/mediatek/mtk-socinfo.c b/drivers/soc/mediatek/mtk-socinfo.c
index 978c43e9115a..424a1eb82c20 100644
--- a/drivers/soc/mediatek/mtk-socinfo.c
+++ b/drivers/soc/mediatek/mtk-socinfo.c
@@ -59,6 +59,7 @@ static struct socinfo_data socinfo_data_table[] = {
 	MTK_SOCINFO_ENTRY("MT8195", "MT8195TV/EZA", "Kompanio 1380", 0x81950400, CELL_NOT_USED),
 	MTK_SOCINFO_ENTRY("MT8195", "MT8195TV/EHZA", "Kompanio 1380", 0x81950404, CELL_NOT_USED),
 	MTK_SOCINFO_ENTRY("MT8370", "MT8370AV/AZA", "Genio 510", 0x83700000, 0x00000081),
+	MTK_SOCINFO_ENTRY("MT8371", "MT8371AV/AZA", "Genio 520", 0x83710000, 0x00000081),
 	MTK_SOCINFO_ENTRY("MT8390", "MT8390AV/AZA", "Genio 700", 0x83900000, 0x00000080),
 	MTK_SOCINFO_ENTRY("MT8391", "MT8391AV/AZA", "Genio 720", 0x83910000, 0x00000080),
 	MTK_SOCINFO_ENTRY("MT8395", "MT8395AV/ZA", "Genio 1200", 0x83950100, CELL_NOT_USED),
diff --git a/drivers/soc/mediatek/mtk-svs.c b/drivers/soc/mediatek/mtk-svs.c
index f45537546553..99edecb204f2 100644
--- a/drivers/soc/mediatek/mtk-svs.c
+++ b/drivers/soc/mediatek/mtk-svs.c
@@ -9,6 +9,7 @@
 #include <linux/bits.h>
 #include <linux/clk.h>
 #include <linux/completion.h>
+#include <linux/cleanup.h>
 #include <linux/cpu.h>
 #include <linux/cpuidle.h>
 #include <linux/debugfs.h>
@@ -789,7 +790,7 @@ static ssize_t svs_enable_debug_write(struct file *filp,
 	struct svs_bank *svsb = file_inode(filp)->i_private;
 	struct svs_platform *svsp = dev_get_drvdata(svsb->dev);
 	int enabled, ret;
-	char *buf = NULL;
+	char *buf __free(kfree) = NULL;
 
 	if (count >= PAGE_SIZE)
 		return -EINVAL;
@@ -807,8 +808,6 @@ static ssize_t svs_enable_debug_write(struct file *filp,
 		svsb->mode_support = SVSB_MODE_ALL_DISABLE;
 	}
 
-	kfree(buf);
-
 	return count;
 }