From 4468e738b480e22b70b068d53c25d23b51be55e4 Mon Sep 17 00:00:00 2001
From: Tang Jinyang <tangjinyang@wxiat.com>
Date: Wed, 24 Aug 2022 10:52:49 +0800
Subject: [PATCH] sw64: fix get_cpu_freq() bug

Sunway inclusion
category: bugfix
bugzilla: https://gitee.com/openeuler/kernel/issues/I56QDM

--------------------------------

The cpu_desc.frequency used to be unmodified in the presence of
CPU frequency scaling, and get_cpu_freq() always return initial
value. It has to maintain a constant correlation to the target
frequency.

Signed-off-by: Tang Jinyang <tangjinyang@wxiat.com>
Signed-off-by: Gu Zitao <guzitao@wxiat.com>
---
 arch/sw_64/include/asm/clock.h   | 2 +-
 arch/sw_64/include/asm/hw_init.h | 7 +++++++
 arch/sw_64/kernel/clock.c        | 6 +++---
 drivers/cpufreq/sw64_cpufreq.c   | 6 +++---
 4 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/arch/sw_64/include/asm/clock.h b/arch/sw_64/include/asm/clock.h
index 30983e8e7cc7..88714eb08507 100644
--- a/arch/sw_64/include/asm/clock.h
+++ b/arch/sw_64/include/asm/clock.h
@@ -44,7 +44,7 @@ struct clk {
 
 int clk_init(void);
 
-int sw64_set_rate(int index, unsigned long rate);
+void sw64_set_rate(unsigned long rate);
 
 struct clk *sw64_clk_get(struct device *dev, const char *id);
 
diff --git a/arch/sw_64/include/asm/hw_init.h b/arch/sw_64/include/asm/hw_init.h
index e0a5706710cd..8a28aac2e54f 100644
--- a/arch/sw_64/include/asm/hw_init.h
+++ b/arch/sw_64/include/asm/hw_init.h
@@ -82,6 +82,13 @@ static inline unsigned long get_cpu_freq(void)
 	return cpu_desc.frequency;
 }
 
+static inline void update_cpu_freq(unsigned long freq)
+{
+	freq = freq * 1000000;
+	if (cpu_desc.frequency != freq)
+		cpu_desc.frequency = freq;
+}
+
 #define EMUL_FLAG	(0x1UL << 63)
 #define MMSIZE_MASK	(EMUL_FLAG - 1)
 
diff --git a/arch/sw_64/kernel/clock.c b/arch/sw_64/kernel/clock.c
index aa22e9550e29..32f01d4b8255 100644
--- a/arch/sw_64/kernel/clock.c
+++ b/arch/sw_64/kernel/clock.c
@@ -131,15 +131,17 @@ void sw64_store_policy(struct cpufreq_policy *policy)
 }
 EXPORT_SYMBOL_GPL(sw64_store_policy);
 
-int sw64_set_rate(int index, unsigned long rate)
+void sw64_set_rate(unsigned long rate)
 {
 	unsigned int i, val;
+	int index = -1;
 
 	rate /= 1000000;
 
 	for (i = 0; i < sizeof(cpu_freq)/sizeof(int); i++) {
 		if (rate == cpu_freq[i]) {
 			index = i;
+			update_cpu_freq(cpu_freq[i]);
 			break;
 		}
 	}
@@ -185,7 +187,5 @@ int sw64_set_rate(int index, unsigned long rate)
 	/* LV1 select PLL0/PLL1 */
 	sw64_io_write(0, CLU_LV1_SEL, CLK_LV1_SEL_MUXB | CLK_LV1_SEL_PRT);
 	sw64_io_write(1, CLU_LV1_SEL, CLK_LV1_SEL_MUXB | CLK_LV1_SEL_PRT);
-
-	return index;
 }
 EXPORT_SYMBOL_GPL(sw64_set_rate);
diff --git a/drivers/cpufreq/sw64_cpufreq.c b/drivers/cpufreq/sw64_cpufreq.c
index 71f944de934b..819d8f1437e2 100644
--- a/drivers/cpufreq/sw64_cpufreq.c
+++ b/drivers/cpufreq/sw64_cpufreq.c
@@ -68,12 +68,12 @@ static int sw64_cpufreq_target(struct cpufreq_policy *policy,
 {
 	unsigned long freq;
 
-	freq = (get_cpu_freq() / 1000) * index / 48;
+	freq = 50000 * index;
 
 	sw64_store_policy(policy);
 
 	/* setting the cpu frequency */
-	sw64_set_rate(-1, freq * 1000);
+	sw64_set_rate(freq * 1000);
 
 	return 0;
 }
@@ -98,7 +98,7 @@ static int sw64_cpufreq_cpu_init(struct cpufreq_policy *policy)
 		if (sw64_clockmod_table[i].frequency == 0)
 			sw64_clockmod_table[i].frequency = (rate * i) / 48;
 
-	sw64_set_rate(-1, rate * 1000);
+	sw64_set_rate(rate * 1000);
 
 	policy->clk = cpuclk;
 
-- 
GitLab