From fbb1d181782f990c0ac5f39d4aa9eda5c39cb442 Mon Sep 17 00:00:00 2001
From: Sam Shih <sam.shih@mediatek.com>
Date: Tue, 4 Mar 2025 19:28:14 +0800
Subject: [PATCH 1/2] cpufreq: add support to adjust cpu volt by efuse
 calibration data

---
 drivers/cpufreq/mediatek-cpufreq.c | 81 ++++++++++++++++++++++++++++--
 1 file changed, 76 insertions(+), 5 deletions(-)

--- a/drivers/cpufreq/mediatek-cpufreq.c
+++ b/drivers/cpufreq/mediatek-cpufreq.c
@@ -15,14 +15,26 @@
 #include <linux/platform_device.h>
 #include <linux/pm_opp.h>
 #include <linux/regulator/consumer.h>
+#include <linux/nvmem-consumer.h>
+
+struct mtk_cpufreq_corr_data {
+	unsigned int freq;
+	unsigned int vbase;
+	unsigned int vscale;
+	unsigned int vmax;
+};
 
 struct mtk_cpufreq_platform_data {
+	/* cpufreq correction data specification */
+	const struct mtk_cpufreq_corr_data *corr_data;
 	int min_volt_shift;
 	int max_volt_shift;
 	int proc_max_volt;
 	int sram_min_volt;
 	int sram_max_volt;
 	bool ccifreq_supported;
+	/* whether voltage correction via nvmem is supported */
+	bool nvmem_volt_corr;
 };
 
 /*
@@ -197,6 +209,50 @@ static bool is_ccifreq_ready(struct mtk_
 	return true;
 }
 
+static int mtk_cpufreq_nvmem_volt_corr(struct mtk_cpu_dvfs_info *info,
+				      struct cpufreq_policy *policy)
+{
+	const struct mtk_cpufreq_corr_data *corr_data;
+	unsigned int target_voltage;
+	struct nvmem_cell *cell;
+	unsigned int cal_data;
+	const u8 *buf;
+	size_t len;
+	int i;
+
+	cell = nvmem_cell_get(info->cpu_dev, "calibration-data");
+	if (IS_ERR(cell))
+		return PTR_ERR(cell);
+
+	buf = nvmem_cell_read(cell, &len);
+	nvmem_cell_put(cell);
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
+
+	cal_data = buf[0] & 0x1f;
+	pr_debug("%s: read vbinning value: %d\n", __func__, cal_data);
+	kfree(buf);
+	if (!info->soc_data->corr_data) {
+		pr_err("voltage correction data not found\n");
+		return -EINVAL;
+	}
+
+	corr_data = &info->soc_data->corr_data[0];
+	for (i = 0 ; i < corr_data->freq ; i++) {
+		target_voltage =  corr_data->vbase + cal_data * corr_data->vscale;
+		if (target_voltage > corr_data->vmax) {
+			pr_warn("freq %u exceeds max voltage\n", corr_data->freq);
+			pr_warn("force update voltage to %u\n", corr_data->vmax);
+			target_voltage = corr_data->vmax;
+		}
+		dev_pm_opp_remove(info->cpu_dev, corr_data->freq);
+		dev_pm_opp_add(info->cpu_dev, corr_data->freq, target_voltage);
+		corr_data = &info->soc_data->corr_data[i + 1];
+	}
+
+	return 0;
+}
+
 static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
 				  unsigned int index)
 {
@@ -584,6 +640,15 @@ static int mtk_cpufreq_init(struct cpufr
 		return -EINVAL;
 	}
 
+	if (info->soc_data->nvmem_volt_corr) {
+		ret = mtk_cpufreq_nvmem_volt_corr(info, policy);
+		if (ret) {
+			pr_err("failed to correction voltage for cpu%d: %d\n",
+			       policy->cpu, ret);
+			return ret;
+		}
+	}
+
 	ret = dev_pm_opp_init_cpufreq_table(info->cpu_dev, &freq_table);
 	if (ret) {
 		dev_err(info->cpu_dev,
