sw64: cpufreq: optimize the granularity of cpufreq

Optimize the granularity of cpufreq according to the actual clock domain
to improve system energy efficiency.

Signed-off-by: Jing Li <[email protected]>
Reviewed-by: He Sheng <[email protected]>
Signed-off-by: Gu Zitao <[email protected]>

Author: Jing Li

arch/sw_64/include/asm/cpu.h

@@ -7,6 +7,9 @@
 #include <linux/cache.h>
 #include <linux/cacheinfo.h>
 
+#define KHZ (1000UL)
+#define MHZ (1000UL * 1000UL)
+
 #define current_cpu_data cpu_data[smp_processor_id()]
 
 enum hmcall_cpuid_cmd {
@@ -48,10 +51,23 @@ extern struct cpuinfo_sw64 cpu_data[NR_CPUS];
 extern cpumask_t cpu_offline;
 
 extern void store_cpu_data(int cpu);
-extern unsigned long get_cpu_freq(void);
-extern void update_cpu_freq(unsigned long khz);
 
 extern unsigned int get_cpu_cache_size(int cpu, int level, enum cache_type type);
 extern unsigned int get_cpu_cacheline_size(int cpu, int level, enum cache_type type);
 
+/* Hz */
+static inline unsigned long sunway_max_cpu_freq(void)
+{
+	return cpuid(GET_CPU_FREQ, 0) * MHZ;
+}
+
+#ifdef CONFIG_SW64_CPUFREQ
+extern unsigned long get_cpu_freq(unsigned int cpu);
+#else
+static inline unsigned long get_cpu_freq(unsigned int cpu)
+{
+	return sunway_max_cpu_freq();
+}
+#endif
+
 #endif /* _ASM_SW64_CPU_H */

arch/sw_64/kernel/cpu.c

@@ -2,6 +2,7 @@
 
 #include <linux/cacheinfo.h>
 #include <linux/cpu.h>
+#include <linux/cpufreq.h>
 #include <linux/cpumask.h>
 #include <linux/delay.h>
 #include <linux/seq_file.h>
@@ -40,25 +41,11 @@ EXPORT_SYMBOL(cpu_data);
 
 cpumask_t cpu_offline = CPU_MASK_NONE;
 
-static unsigned long cpu_freq;
 static unsigned long cpu_info;
 static __u16 family;
 static char vendor_id[64];
 static char model_id[64];
 
-unsigned long get_cpu_freq(void)
-{
-	if (likely(cpu_freq))
-		return cpu_freq;
-
-	return cpuid(GET_CPU_FREQ, 0) * 1000UL * 1000UL;
-}
-
-void update_cpu_freq(unsigned long khz)
-{
-	cpu_freq = khz * 1000;
-}
-
 /* Move global data into per-processor storage */
 void store_cpu_data(int cpu)
 {
@@ -240,7 +227,7 @@ static int show_cpuinfo(struct seq_file *f, void *slot)
 	unsigned int l3_cache_size, l3_cachline_size;
 	unsigned long freq;
 
-	freq = cpuid(GET_CPU_FREQ, 0);
+	freq = sunway_max_cpu_freq() / MHZ;
 
 	for_each_online_cpu(i) {
 		l3_cache_size = get_cpu_cache_size(i, 3, CACHE_TYPE_UNIFIED);
@@ -267,7 +254,7 @@ static int show_cpuinfo(struct seq_file *f, void *slot)
 				"cache size\t: %u KB\n"
 				"physical id\t: %d\n"
 				"bogomips\t: %lu.%02lu\n",
-				get_cpu_freq() / 1000 / 1000, l3_cache_size >> 10,
+				get_cpu_freq(i) / 1000 / 1000, l3_cache_size >> 10,
 				cpu_topology[i].package_id,
 				loops_per_jiffy / (500000/HZ),
 				(loops_per_jiffy / (5000/HZ)) % 100);
@@ -313,3 +300,16 @@ bool arch_match_cpu_phys_id(int cpu, u64 phys_id)
 	return phys_id == cpu_physical_id(cpu);
 }
 
+#ifdef CONFIG_SW64_CPUFREQ
+
+unsigned long get_cpu_freq(unsigned int cpu)
+{
+	unsigned long freq = cpufreq_quick_get(cpu);
+
+	if (likely(freq))
+		return freq * KHZ;
+
+	return sunway_max_cpu_freq();
+}
+
+#endif

arch/sw_64/kernel/setup.c

@@ -133,7 +133,7 @@ EXPORT_SYMBOL(screen_info);
 #ifdef CONFIG_HARDLOCKUP_DETECTOR_PERF
 u64 hw_nmi_get_sample_period(int watchdog_thresh)
 {
-	return get_cpu_freq() * watchdog_thresh;
+	return sunway_max_cpu_freq() * watchdog_thresh;
 }
 #endif
 

arch/sw_64/kernel/time.c

@@ -31,7 +31,7 @@ time_init(void)
 {
 	unsigned long cycle_freq;
 
-	cycle_freq = get_cpu_freq();
+	cycle_freq = sunway_max_cpu_freq();
 
 	pr_info("CPU Cycle frequency = %ld Hz\n", cycle_freq);
 

arch/sw_64/lib/udelay.c

@@ -32,7 +32,7 @@ EXPORT_SYMBOL(__delay);
 
 void udelay(unsigned long usecs)
 {
-	unsigned long loops = usecs * get_cpu_freq() / 1000000;
+	unsigned long loops = usecs * get_cpu_freq(smp_processor_id()) / 1000000;
 	unsigned long tmp;
 
 	__asm__ __volatile__(
@@ -47,7 +47,7 @@ EXPORT_SYMBOL(udelay);
 
 void ndelay(unsigned long nsecs)
 {
-	unsigned long loops = nsecs * get_cpu_freq() / 1000000000;
+	unsigned long loops = nsecs * get_cpu_freq(smp_processor_id()) / 1000000000;
 	unsigned long tmp;
 
 	__asm__ __volatile__(

drivers/clocksource/timer-sw64.c

@@ -174,7 +174,7 @@ void __init sw64_setup_clocksource(void)
 	else
 		clocksource_register_khz(&clocksource_vtime, mclk_khz);
 #else
-	clocksource_register_hz(&clocksource_tc, get_cpu_freq());
+	clocksource_register_hz(&clocksource_tc, sunway_max_cpu_freq());
 	pr_info("Setup clocksource TC, mult = %d\n", clocksource_tc.mult);
 #endif
 }
@@ -208,23 +208,13 @@ void __init setup_sched_clock(void)
 
 	sc_shift = 7;
 	step = 1UL << sc_shift;
-	sc_multi = step * NSEC_PER_SEC / get_cpu_freq();
+	sc_multi = step * NSEC_PER_SEC / sunway_max_cpu_freq();
 	calibrate_sched_clock();
 
 	pr_info("sched_clock: sc_multi=%llu, sc_shift=%llu\n", sc_multi, sc_shift);
 }
 
-#ifdef CONFIG_GENERIC_SCHED_CLOCK
-static u64 notrace read_sched_clock(void)
-{
-	return (rdtc() - sc_start) >> sc_shift;
-}
-
-void __init sw64_sched_clock_init(void)
-{
-	sched_clock_register(sched_clock_read, BITS_PER_LONG, get_cpu_freq() >> sc_shift);
-}
-#else /* !CONFIG_GENERIC_SCHED_CLOCK */
+#ifndef CONFIG_GENERIC_SCHED_CLOCK
 /*
  * scheduler clock - returns current time in nanoseconds.
  */
@@ -428,7 +418,7 @@ void sw64_setup_timer(void)
 	struct clock_event_device *swevt = &per_cpu(timer_events, cpu);
 
 	/* min_delta ticks => 100ns */
-	min_delta = get_cpu_freq()/1000/1000/10;
+	min_delta = sunway_max_cpu_freq()/1000/1000/10;
 
 	if (is_in_guest()) {
 		memcpy(swevt, &vtimer_clockevent, sizeof(*swevt));
@@ -443,7 +433,7 @@ void sw64_setup_timer(void)
 	}
 	swevt->cpumask = cpumask_of(cpu);
 	swevt->set_state_shutdown(swevt);
-	clockevents_config_and_register(swevt, get_cpu_freq(), min_delta, ULONG_MAX);
+	clockevents_config_and_register(swevt, sunway_max_cpu_freq(), min_delta, ULONG_MAX);
 }
 
 void sw64_timer_interrupt(void)

drivers/cpufreq/sunway-cpufreq.c

@@ -77,9 +77,6 @@ struct cpufreq_frequency_table freq_table[] = {
 	FV(2850, 0),
 	{0, 0, CPUFREQ_TABLE_END},
 };
-static void __init fill_freq_table(struct cpufreq_frequency_table *ft)
-{
-}
 #endif
 
 #ifdef CONFIG_PLATFORM_XUELANG
@@ -135,60 +132,65 @@ static void __init fill_freq_table(struct cpufreq_frequency_table *ft)
 
 static unsigned int sunway_get_rate(struct cpufreq_policy *policy)
 {
-	int i;
+	int i, node;
 	u64 val;
-	void __iomem *spbu_base = misc_platform_get_spbu_base(0);
+	void __iomem *spbu_base;
 	struct cpufreq_frequency_table *ft = policy->freq_table;
 
+	node = per_cpu(hard_node_id, policy->cpu);
+	spbu_base = misc_platform_get_spbu_base(node);
+
 	/* PLL2 provides working frequency for core */
 	val = readq(spbu_base + OFFSET_CLK_CTL) >> CORE_PLL2_CFG_SHIFT;
 	val &= CORE_PLL2_CFG_MASK;
 
 	for (i = 0; ft[i].frequency != CPUFREQ_TABLE_END; i++) {
 		if (val == i) {
 			if (ft[i].frequency == CPUFREQ_ENTRY_INVALID)
-				return cpuid(GET_CPU_FREQ, 0) * 1000UL;
+				return sunway_max_cpu_freq() / KHZ;
 			return ft[i].frequency;
 		}
 	}
 
 	return 0;
 }
 
-static int sunway_set_rate(unsigned int index)
+static int sunway_set_rate(struct cpufreq_policy *policy, unsigned int index)
 {
-	int i, retry, cpu_num;
+	int retry, node;
 	void __iomem *spbu_base;
 
-	cpu_num = sw64_chip->get_cpu_num();
-	for (i = 0; i < cpu_num; i++) {
-		spbu_base = misc_platform_get_spbu_base(i);
-
-		/* select PLL0/PLL1 */
-		writeq(CLK_LV1_SEL_PROTECT, spbu_base + OFFSET_CLU_LV1_SEL);
-		/* reset PLL2 */
-		writeq(CLK2_PROTECT | CORE_CLK2_RESET | CORE_CLK2_VALID, spbu_base + OFFSET_CLK_CTL);
-		/* configure PLL2_CFG */
-		writeq(CLK2_PROTECT | CORE_CLK2_VALID | (unsigned long)index << CORE_PLL2_CFG_SHIFT,
-				spbu_base + OFFSET_CLK_CTL);
-		udelay(1);
-		/* reset over */
-		writeq(CORE_CLK2_VALID, spbu_base + OFFSET_CLK_CTL);
-		retry = 0;
-		while (retry < MAX_RETRY) {
-			if (readq(spbu_base + OFFSET_CLK_CTL) & CORE_CLK2_LOCK)
-				break;
-			retry++;
-			udelay(100);
-		}
-		if (retry == MAX_RETRY)
-			return -ETIME;
-		/* configure over */
-		writeq(0, spbu_base + OFFSET_CLK_CTL);
-		/* select PLL2/PLL2 */
-		writeq(CLK_LV1_SEL_MUXA | CLK_LV1_SEL_MUXB | CLK_LV1_SEL_PROTECT,
-				spbu_base + OFFSET_CLU_LV1_SEL);
+	node = per_cpu(hard_node_id, policy->cpu);
+	spbu_base = misc_platform_get_spbu_base(node);
+
+	/* select PLL0/PLL1 */
+	writeq(CLK_LV1_SEL_PROTECT, spbu_base + OFFSET_CLU_LV1_SEL);
+	/* reset PLL2 */
+	writeq(CLK2_PROTECT | CORE_CLK2_RESET | CORE_CLK2_VALID, spbu_base + OFFSET_CLK_CTL);
+	/* configure PLL2_CFG */
+	writeq(CLK2_PROTECT | CORE_CLK2_VALID | (unsigned long)index << CORE_PLL2_CFG_SHIFT,
+			spbu_base + OFFSET_CLK_CTL);
+
+	udelay(1);
+	/* reset over */
+	writeq(CORE_CLK2_VALID, spbu_base + OFFSET_CLK_CTL);
+	retry = 0;
+	while (retry < MAX_RETRY) {
+		if (readq(spbu_base + OFFSET_CLK_CTL) & CORE_CLK2_LOCK)
+			break;
+		retry++;
+		udelay(100);
 	}
+
+	if (retry == MAX_RETRY)
+		return -ETIME;
+
+	/* configure over */
+	writeq(0, spbu_base + OFFSET_CLK_CTL);
+	/* select PLL2/PLL2 */
+	writeq(CLK_LV1_SEL_MUXA | CLK_LV1_SEL_MUXB | CLK_LV1_SEL_PROTECT,
+			spbu_base + OFFSET_CLU_LV1_SEL);
+
 	return 0;
 }
 
@@ -205,9 +207,6 @@ static unsigned int sunway_cpufreq_get(unsigned int cpu)
 	return sunway_get_rate(policy);
 }
 
-/*
- * Here we notify other drivers of the proposed change and the final change.
- */
 static int sunway_cpufreq_target(struct cpufreq_policy *policy,
 				     unsigned int index)
 {
@@ -218,24 +217,27 @@ static int sunway_cpufreq_target(struct cpufreq_policy *policy,
 		return -ENODEV;
 
 	/* setting the cpu frequency */
-	ret = sunway_set_rate(index);
+	ret = sunway_set_rate(policy, index);
 	if (ret)
 		return ret;
-	update_cpu_freq(freq_table[index].frequency);
 
 	return 0;
 }
 
 static int sunway_cpufreq_init(struct cpufreq_policy *policy)
 {
-	cpufreq_generic_init(policy, freq_table, 0);
+	int cpu, node;
 
-	return 0;
-}
+	node = per_cpu(hard_node_id, policy->cpu);
 
-static int sunway_cpufreq_verify(struct cpufreq_policy_data *policy)
-{
-	return cpufreq_frequency_table_verify(policy, freq_table);
+	for_each_possible_cpu(cpu) {
+		if (per_cpu(hard_node_id, cpu) == node)
+			cpumask_set_cpu(cpu, policy->cpus);
+	}
+
+	policy->freq_table = freq_table;
+
+	return 0;
 }
 
 static int sunway_cpufreq_exit(struct cpufreq_policy *policy)
@@ -251,7 +253,7 @@ static struct cpufreq_driver sunway_cpufreq_driver = {
 	.name = "sunway-cpufreq",
 	.flags = CPUFREQ_NEED_INITIAL_FREQ_CHECK,
 	.init = sunway_cpufreq_init,
-	.verify = sunway_cpufreq_verify,
+	.verify = cpufreq_generic_frequency_table_verify,
 	.target_index = sunway_cpufreq_target,
 	.get = sunway_cpufreq_get,
 	.exit = sunway_cpufreq_exit,
@@ -261,14 +263,17 @@ static struct cpufreq_driver sunway_cpufreq_driver = {
 static int __init cpufreq_init(void)
 {
 	int i, ret;
-	unsigned long max_rate = get_cpu_freq() / 1000;
+	unsigned long max_rate = sunway_max_cpu_freq() / KHZ; /* KHz */
 
 	if (!is_in_host()) {
 		pr_warn("cpufreq driver of Sunway platforms is only supported in host mode\n");
 		return -ENODEV;
 	}
 
+#ifdef CONFIG_PLATFORM_XUELANG
 	fill_freq_table(freq_table);
+#endif
+
 	for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++) {
 		if (max_rate == freq_table[i].frequency)
 			freq_table[i+1].frequency = CPUFREQ_TABLE_END;