sw64: fix CPUFreq bug

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

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

Dynamic frequency scaling cannot change the current cpu frequency
when userspace policy is used, and cpuinfo_cur_freq always displays
the default value. This patch fixes it.
Signed-off-by: NTang Jinyang <tangjinyang@wxiat.com>
Signed-off-by: NGu Zitao <guzitao@wxiat.com>

arch/sw_64/include/asm/clock.h

@@ -48,9 +48,9 @@ int sw64_set_rate(int index, unsigned long rate);
 
 struct clk *sw64_clk_get(struct device *dev, const char *id);
 
-unsigned long sw64_clk_get_rate(struct clk *clk);
-
 void sw64_update_clockevents(unsigned long cpu, u32 freq);
 
 void sw64_store_policy(struct cpufreq_policy *policy);
+
+unsigned int __sw64_cpufreq_get(struct cpufreq_policy *policy);
 #endif				/* _ASM_SW64_CLOCK_H */

arch/sw_64/kernel/clock.c

@@ -109,14 +109,21 @@ struct clk *sw64_clk_get(struct device *dev, const char *id)
 }
 EXPORT_SYMBOL(sw64_clk_get);
 
-unsigned long sw64_clk_get_rate(struct clk *clk)
+unsigned int __sw64_cpufreq_get(struct cpufreq_policy *policy)
 {
-	if (!clk)
-		return 0;
+	int i;
+	u64 val;
+
+	val = sw64_io_read(0, CLK_CTL);
+	val = val >> CORE_PLL2_CFG_SHIFT;
 
-	return (unsigned long)clk->rate;
+	for (i = 0; i < sizeof(cpu_freq)/sizeof(int); i++) {
+		if (cpu_freq[val] == cpu_freq[i])
+			return cpu_freq[i];
+	}
+	return 0;
 }
-EXPORT_SYMBOL(sw64_clk_get_rate);
+EXPORT_SYMBOL(__sw64_cpufreq_get);
 
 void sw64_store_policy(struct cpufreq_policy *policy)
 {

drivers/cpufreq/sw64_cpufreq.c

@@ -59,7 +59,7 @@ static unsigned int sw64_cpufreq_get(unsigned int cpu)
 		return 0;
 	}
 
-	return sw64_clk_get_rate(policy->clk);
+	return __sw64_cpufreq_get(policy) * 1000;
 }
 
 /*