cpufreq: Add cpufreq driver for LoongArch

LoongArch inclusion
category: feature
bugzilla: https://gitee.com/openeuler/kernel/issues/I6BWFP

--------------------------------
Signed-off-by: Nzhangtianyang <zhangtianyang@loongson.cn>
Change-Id: Ic9390a1c78e1670422be1b03d35e3f0fdec1367f

arch/loongarch/Kconfig

@@ -514,4 +514,10 @@ source "drivers/acpi/Kconfig"
 
 endmenu
 
+menu "CPU Power Management"
+
+source "drivers/cpufreq/Kconfig"
+
+endmenu
+
 source "drivers/firmware/Kconfig"

arch/loongarch/configs/loongson3_defconfig

@@ -43,6 +43,10 @@ CONFIG_ACPI_SPCR_TABLE=y
 CONFIG_ACPI_DOCK=y
 CONFIG_ACPI_IPMI=m
 CONFIG_ACPI_PCI_SLOT=y
+CONFIG_CPU_FREQ=y
+CONFIG_CPU_FREQ_STAT=y
+CONFIG_CPU_FREQ_GOV_POWERSAVE=y
+CONFIG_LOONGSON3_ACPI_CPUFREQ=y
 CONFIG_EFI_CAPSULE_LOADER=m
 CONFIG_EFI_TEST=m
 CONFIG_MODULES=y

arch/loongarch/include/asm/fpu.h

@@ -45,6 +45,10 @@ static inline void disable_lasx(void);
 static inline void save_lasx(struct task_struct *t);
 static inline void restore_lasx(struct task_struct *t);
 
+#ifdef CONFIG_LOONGSON3_ACPI_CPUFREQ
+DECLARE_PER_CPU(unsigned long, msa_count);
+DECLARE_PER_CPU(unsigned long, lasx_count);
+#endif
 /*
  * Mask the FCSR Cause bits according to the Enable bits, observing
  * that Unimplemented is always enabled.
@@ -198,6 +202,9 @@ static inline void enable_lsx(void)
 {
 	if (cpu_has_lsx)
 		csr_xchg32(CSR_EUEN_LSXEN, CSR_EUEN_LSXEN, LOONGARCH_CSR_EUEN);
+#ifdef CONFIG_LOONGSON3_ACPI_CPUFREQ
+		per_cpu(msa_count, raw_smp_processor_id())++;
+#endif
 }
 
 static inline void disable_lsx(void)
@@ -251,8 +258,12 @@ static inline void restore_lsx_upper(struct task_struct *t) {}
 static inline void enable_lasx(void)
 {
 
-	if (cpu_has_lasx)
+	if (cpu_has_lasx) {
 		csr_xchg32(CSR_EUEN_LASXEN, CSR_EUEN_LASXEN, LOONGARCH_CSR_EUEN);
+#ifdef CONFIG_LOONGSON3_ACPI_CPUFREQ
+		per_cpu(lasx_count, raw_smp_processor_id())++;
+#endif
+	}
 }
 
 static inline void disable_lasx(void)

drivers/cpufreq/Kconfig

@@ -287,6 +287,17 @@ config LOONGSON1_CPUFREQ
 	  If in doubt, say N.
 endif
 
+if LOONGARCH
+config LOONGSON3_ACPI_CPUFREQ
+	bool "Loongson3 ACPI cpufreq driver"
+	depends on ACPI_PROCESSOR
+	help
+	  This driver adds a CPUFreq driver which utilizes the ACPI
+	  Processor Performance States.
+	  This driver supports Loongson 3A5000 compatible CPUs.
+	  If in doubt, say N.
+endif
+
 if SPARC64
 config SPARC_US3_CPUFREQ
 	tristate "UltraSPARC-III CPU Frequency driver"

drivers/cpufreq/Makefile

@@ -106,6 +106,7 @@ obj-$(CONFIG_BMIPS_CPUFREQ)		+= bmips-cpufreq.o
 obj-$(CONFIG_IA64_ACPI_CPUFREQ)		+= ia64-acpi-cpufreq.o
 obj-$(CONFIG_LOONGSON2_CPUFREQ)		+= loongson2_cpufreq.o
 obj-$(CONFIG_LOONGSON1_CPUFREQ)		+= loongson1-cpufreq.o
+obj-$(CONFIG_LOONGSON3_ACPI_CPUFREQ)	+= loongson3-acpi-cpufreq.o
 obj-$(CONFIG_SH_CPU_FREQ)		+= sh-cpufreq.o
 obj-$(CONFIG_SPARC_US2E_CPUFREQ)	+= sparc-us2e-cpufreq.o
 obj-$(CONFIG_SPARC_US3_CPUFREQ)		+= sparc-us3-cpufreq.o

drivers/cpufreq/loongson3-acpi-cpufreq.c

+/*
+ * loongson3-acpi-cpufreq.c - Loongson ACPI Processor P-States Driver
+ *
+ *  Copyright (C) 2020  lvjianmin <lvjianmin@loongson.cn>
+ *			Yijun <yijun@loongson.cn>
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or (at
+ *  your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful, but
+ *  WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, write to the Free Software Foundation, Inc.,
+ *
+ * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/smp.h>
+#include <linux/sched.h>
+#include <linux/cpufreq.h>
+#include <linux/compiler.h>
+#include <linux/sched/cpufreq.h>
+#include <linux/dmi.h>
+#include <linux/slab.h>
+#include <linux/kthread.h>
+#include <linux/acpi.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+#include <uapi/linux/sched/types.h>
+#include <acpi/processor.h>
+
+#include <asm/processor.h>
+#include <asm/cpufeature.h>
+#include <asm/loongson.h>
+#include "cpufreq_governor.h"
+
+#include <asm/time.h>
+#define CPU_ID_FIELD    0xf
+
+#define COMPLETE_STATUS 0x80000000
+#define VOLTAGE_COMMAND 0x21
+
+#define DVFS_INFO	0x22
+#define DVFS_INFO_BOOST_LEVEL	0x23
+#define DVFS_INFO_MIN_FREQ	0xf
+#define DVFS_INFO_MAX_FREQ	0xf0
+#define DVFS_INFO_BOOST_CORE_FREQ	0xff00
+#define DVFS_INFO_NORMAL_CORE_UPPER_LIMIT	0xf0000
+#define DVFS_INFO_BOOST_CORES	0xf00000
+
+#define BOOST_MODE	0x80000
+#define NORMAL_MODE	0x40000
+
+MODULE_DESCRIPTION("Loongson 3A5000 ACPI Processor P-States Driver");
+
+MODULE_LICENSE("GPL");
+
+#define CPUFREQ_SAMPLING_INTERVAL	(2 * TICK_NSEC / NSEC_PER_USEC)
+#define LOONGSON_CONTROL_MASK		(0xFF)
+#define FACTOR				(0xeac0c6e8)
+#define BOOST_THRESHOLD			(900)
+#define MAX_CORES_PER_PACKAGE		64
+#define CPU_ID_FIELD			0xf
+#define VOLTAGE_COMMAND			0x21
+#define MAX_READY_TIMEOUT		300000000
+#define RESERVED_FREQ			3
+
+#define LOONGSON_BOOST_FREQ_MASK		(0x7 << 8)
+#define FREQ_STEP		(25)
+
+static struct mutex boost_mutex[MAX_PACKAGES];
+static bool cpufreq_has_boost_freq;
+static int max_boost_cores;
+static int boost_gears;
+static int boost_freqs[NR_CPUS + 1];
+struct package_data;
+struct core_data;
+static struct acpi_processor_performance __percpu *acpi_perf_data;
+static struct cpufreq_driver loongson3_cpufreq_driver;
+static struct freq_attr *loongson3_cpufreq_attr[];
+DECLARE_PER_CPU(struct clock_event_device, stable_clockevent_device);
+static inline struct core_data *get_core_data(int cpu);
+extern struct clk *cpu_clk_get(int cpu);
+
+static int min_freq_level;
+static int max_freq_level;
+static int max_upper_index;
+static int max_boost_freq;
+
+/* threshold of core's get into msa */
+static int msa_count_threshold = 200;
+/* threshold of core's get into lasx */
+static int lasx_count_threshold = 200;
+/* other cores' upper load threshold when 1 core get into boost mode and enable msa/lasx */
+static int load_threshold = 60;
+
+DEFINE_PER_CPU(unsigned long, msa_count);
+EXPORT_PER_CPU_SYMBOL(msa_count);
+
+#if defined(CONFIG_CPU_HAS_LASX)
+DEFINE_PER_CPU(unsigned long, lasx_count);
+EXPORT_PER_CPU_SYMBOL(lasx_count);
+#endif
+
+struct ce_update_data {
+	struct clock_event_device *cd;
+	unsigned int new_freq;
+};
+
+static struct kthread_worker	cpufreq_worker;
+static struct task_struct	*cpufreq_thread;
+/**
+ * struct core_data -	Store core related information
+ * @in_boost:		the core is boosting to boost_freq
+ * @cpu:		logical cpu of the core
+ * @update_util		The update_util_data pointer of @cpu, is passed to the callback
+ * 			function, which will be called by cpufreq_update_util()
+ * @package		The package_data structure the core belonged to
+ * @work_in_progress	@work is busy
+ * @irq_work		to enqueue callback handling on irq workqueue
+ * @work		to enqueue work from irq workqueue on system workqueue
+ * @perf		store frequency table related information from ACPI table
+ * @max_freq		max normal freq of cpu
+ * @boost_freq		max boost freq of cpu
+ * @clock_scale		clock scale to calculate cpu_data[cpu].udelay_val in boost mode
+ * @package_id		package id of core
+ * @shift		clock shift to calculate cpu_data[cpu].udelay_val in boost mode
+ * @update_util_set	if callback has been set for cpufreq_update_util()
+ * @load		current load of the core
+ * @last_freq_update_time	last freq update time
+ * @freq_update_delay_ns	min interval of freq update, which is
+ * 			transition_latency configured in ACPI table
+ *
+ * following elements are used to calculate load of the core
+ * @prev_update_time
+ * @prev_cpu_idle
+ * @prev_load
+ * @sampling_rate
+ *
+ */
+struct core_data {
+	bool in_boost;
+	int cpu;
+	struct update_util_data update_util;
+	struct package_data *package;
+	bool work_in_progress;
+	struct	irq_work irq_work;
+	struct kthread_work work;
+	struct acpi_processor_performance *perf;
+	unsigned int normal_max_freq;
+	unsigned int *boost_freq;
+	unsigned int *clock_scale;
+	unsigned int package_id;
+	unsigned int *shift;
+	bool update_util_set;
+	unsigned long long load;
+
+	u64 last_freq_update_time;
+	s64 freq_update_delay_ns;
+	u64 prev_update_time;
+	u64 prev_cpu_idle;
+	u32 prev_load;
+	u32 sampling_rate;
+};
+
+struct package_data {
+	int boost_cores;
+	int max_boost_cores;
+	int nr_cores;
+	char in_boost;
+	int nr_full_load_cores;
+	struct core_data core[MAX_CORES_PER_PACKAGE];
+} all_package_data[MAX_PACKAGES];
+
+static bool boost_supported(void)
+{
+	return loongson3_cpufreq_driver.set_boost;
+}
+
+/*
+ * Check if target_freq is a boost freq
+ *
+ * target_freq must be a freq in freq table when
+ * calling the function.
+ * */
+static int boost_level(struct acpi_processor_performance *perf, unsigned int target_freq)
+{
+	int i;
+
+	for (i = 0; i < perf->state_count; i++) {
+		if (target_freq == (perf->states[i].core_frequency * 1000)) {
+			return (perf->states[i].control & LOONGSON_BOOST_FREQ_MASK) >> 8;
+		}
+	}
+	return 0;
+}
+
+#ifdef CONFIG_SMP
+static int loongson3_cpu_freq_notifier(struct notifier_block *nb,
+		unsigned long val, void *data)
+{
+	struct cpufreq_freqs *freqs;
+	struct clock_event_device __maybe_unused *cd;
+	struct core_data *core;
+	unsigned int __maybe_unused new_freq;
+	unsigned long cpu;
+	struct ce_update_data __maybe_unused ce_data;
+	int cur_boost_level;
+
+	if (val == CPUFREQ_POSTCHANGE) {
+		freqs = (struct cpufreq_freqs *)data;
+		cpu = freqs->policy->cpu;
+		core = get_core_data(cpu);
+		cur_boost_level = boost_level(core->perf, freqs->new);
+		if (cur_boost_level != 0) {
+			lpj_fine = (unsigned int) (((int64_t)core->clock_scale[cur_boost_level] *
+						cpufreq_scale(loops_per_jiffy, boost_freqs[cur_boost_level] * 1000,
+							freqs->new)) / core->shift[cur_boost_level]);
+		} else {
+			lpj_fine =
+				cpufreq_scale(loops_per_jiffy, core->normal_max_freq * 1000, freqs->new);
+		}
+	}
+
+	return 0;
+}
+#else
+static int loongson3_cpu_freq_notifier(struct notifier_block *nb,
+		unsigned long val, void *data)
+{
+	struct cpufreq_freqs *freqs;
+	struct clock_event_device __maybe_unused *cd;
+	struct core_data *core;
+	unsigned int __maybe_unused new_freq;
+	unsigned long cpu;
+	int cur_boost_level;
+
+	if (val == CPUFREQ_POSTCHANGE) {
+
+		freqs = (struct cpufreq_freqs *)data;
+		cpu = freqs->cpu;
+		core = get_core_data(cpu);
+		cur_boost_level = boost_level(core->perf, target_freq);
+
+		if (cur_boost_level != 0) {
+			lpj_fine = (unsigned int) (((int64_t)core->clock_scale[cur_boost_level] *
+						loops_per_jiffy) / core->shift[cur_boost_level]);
+		} else {
+			lpj_fine = loops_per_jiffy;
+		}
+	}
+
+	return 0;
+}
+#endif
+static struct notifier_block loongson3_cpufreq_notifier_block = {
+	.notifier_call = loongson3_cpu_freq_notifier
+};
+
+static int cpufreq_perf_find_level(struct acpi_processor_performance *perf,
+		unsigned int target_freq,
+		unsigned int boost_level)
+{
+	int i;
+	for (i = 0; i < perf->state_count; i++) {
+		if (boost_level) {
+			if (perf->states[i].control & LOONGSON_BOOST_FREQ_MASK) {
+				if (target_freq == (perf->states[i].core_frequency * 1000))
+					return perf->states[i].control & LOONGSON_CONTROL_MASK;
+			}
+		} else {
+			if (!(perf->states[i].control & LOONGSON_BOOST_FREQ_MASK))
+				if (target_freq == (perf->states[i].core_frequency * 1000))
+					return perf->states[i].control;
+		}
+	}
+	return 0;
+}
+
+static int cpufreq_perf_find_freq(struct acpi_processor_performance *perf,
+		unsigned int target_index,
+		unsigned int boost_level)
+{
+	int i;
+	for (i = 0; i < perf->state_count; i++) {
+		if (boost_level) {
+			if (perf->states[i].control & LOONGSON_BOOST_FREQ_MASK)
+				if (target_index == (perf->states[i].control & LOONGSON_CONTROL_MASK))
+					return perf->states[i].core_frequency;
+		} else {
+			if (!(perf->states[i].control & LOONGSON_BOOST_FREQ_MASK))
+				if (target_index == perf->states[i].control)
+					return perf->states[i].core_frequency;
+		}
+	}
+	return 0;
+}
+
+
+static inline struct core_data *get_core_data(int cpu)
+{
+	int package_id = cpu_data[cpu].package;
+	struct package_data *package = &all_package_data[package_id];
+	int core_id = cpu_logical_map(cpu) % package->nr_cores;
+	return &package->core[core_id];
+}
+
+static bool package_boost(struct package_data *package)
+{
+	int i;
+	int cur_full_load = 0;
+
+#if defined(CONFIG_CPU_HAS_LASX)
+	int lasx_enable_count = 0;
+	unsigned long lasx_num;
+	bool clear_lasx = false;
+#endif
+
+	int msa_enable_count = 0;
+	unsigned long msa_num;
+	bool clear_msa = false;
+
+	for (i = 0; i < package->nr_cores; i++) {
+
+#if defined(CONFIG_CPU_HAS_LASX)
+		lasx_num = per_cpu(lasx_count, package->core[i].cpu);
+
+		if (lasx_num) {
+			lasx_enable_count++;
+		}
+
+		if (lasx_num >= lasx_count_threshold) {
+			clear_lasx = true;
+		}
+
+		pr_debug("file %s, line %d, lasx enabled, i %d, cpu %d, lasx_num %lu\n",
+				__FILE__, __LINE__, i, package->core[i].cpu, lasx_num);
+#endif
+		msa_num = per_cpu(msa_count, package->core[i].cpu);
+
+		if (msa_num) {
+			msa_enable_count++;
+		}
+
+		if (msa_num >= msa_count_threshold) {
+			clear_msa = true;
+		}
+
+		pr_debug("file %s, line %d, msa enabled, i %d, cpu %d, msa_num %lu\n",
+				__FILE__, __LINE__, i, package->core[i].cpu, msa_num);
+
+		if (package->core[i].prev_load >= load_threshold) {
+			cur_full_load++;
+		}
+	}
+
+#if defined(CONFIG_CPU_HAS_LASX)
+	if (clear_lasx) {
+		for (i = 0; i < package->nr_cores; i++) {
+			per_cpu(lasx_count, package->core[i].cpu) = 0;
+		}
+	}
+#endif
+
+	if (clear_msa) {
+		for (i = 0; i < package->nr_cores; i++) {
+			per_cpu(msa_count, package->core[i].cpu) = 0;
+		}
+	}
+
+#if defined(CONFIG_CPU_HAS_LASX)
+	if (lasx_enable_count > 1
+		|| (lasx_enable_count && package->nr_full_load_cores > 1)
+		|| (lasx_enable_count && cur_full_load > 1)) {
+		return false;
+	}
+#endif
+
+	if (msa_enable_count > 1
+	|| (msa_enable_count && package->nr_full_load_cores > 1)
+	|| (msa_enable_count && cur_full_load > 1)) {
+		return false;
+	}
+
+	if (package->nr_full_load_cores &&
+			package->nr_full_load_cores <= package->max_boost_cores)
+		return true;
+
+	return false;
+}
+
+/*
+ * check if the cpu can be boosted.
+ *
+ * call the function after load of cpu updated.
+ * */
+static bool cpu_can_boost(int cpu)
+{
+	struct core_data *core = get_core_data(cpu);
+	struct package_data *package = core->package;
+	if (package->boost_cores >= package->max_boost_cores)
+		return false;
+	if (core->load > BOOST_THRESHOLD) {
+		return true;
+	}
+	return false;
+}
+
+static void do_set_freq_level(int cpu, int freq_level)
+{
+	uint32_t message;
+	uint32_t val;
+
+	message = (0 << 31) | (VOLTAGE_COMMAND << 24)
+		| ((uint32_t)freq_level << 4)
+		| (cpu & CPU_ID_FIELD);
+	iocsr_write32(message, 0x51c);
+	val = iocsr_read32(0x420);
+
+	val |= 1 << 10;
+	iocsr_write32(val, 0x420);
+}
+
+static int wait_for_ready_timeout(int64_t timeout)
+{
+	int ret;
+	struct timespec64 prev_ts;
+	struct timespec64 curr_ts;
+	ktime_t delay = ktime_set(0, 100);
+
+	ktime_get_ts64(&prev_ts);
+	ktime_get_ts64(&curr_ts);
+
+	ret = -EPERM;
+	while (((curr_ts.tv_sec - prev_ts.tv_sec) * 1000000000 + (curr_ts.tv_nsec - prev_ts.tv_nsec)) < timeout) {
+		ktime_get_ts64(&curr_ts);
+
+		if (iocsr_read32(0x51c) & COMPLETE_STATUS) {
+			ret = 0;
+			break;
+		}
+
+		__set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_hrtimeout(&delay, HRTIMER_MODE_REL);
+	}
+	return ret;
+}
+
+/* Find closest freq to target in a table in ascending order */
+static int cpufreq_table_find_freq_ac(struct cpufreq_policy *policy,
+		unsigned int target_freq,
+		int boost_level)
+{
+	struct cpufreq_frequency_table *table = policy->freq_table;
+	struct cpufreq_frequency_table *pos;
+	unsigned int freq;
+	unsigned int best_freq = 0;
+	int idx, best = -1;
+	cpufreq_for_each_valid_entry_idx(pos, table, idx) {
+		freq = pos->frequency;
+
+		if (pos->driver_data != boost_level)
+			continue;
+		if (freq > policy->max || freq < policy->min)
+			continue;
+		if (freq == target_freq)
+			return freq;
+
+		if (freq < target_freq) {
+			best = idx;
+			best_freq = freq;
+			continue;
+		}
+
+		/* No freq found below target_freq, return freq above target_freq */
+		if (best == -1)
+			return freq;
+
+		/* Choose the closest freq */
+		if (target_freq - table[best].frequency > freq - target_freq)
+			return freq;
+
+		return best_freq;
+	}
+
+	return best_freq;
+}
+
+/* Find closest freq to target in a table in descending order */
+static int cpufreq_table_find_freq_dc(struct cpufreq_policy *policy,
+					unsigned int target_freq,
+					int boost_level)
+{
+	struct cpufreq_frequency_table *table = policy->freq_table;
+	struct cpufreq_frequency_table *pos;
+	unsigned int freq;
+	unsigned int best_freq = 0;
+	int idx, best = -1;
+
+	cpufreq_for_each_valid_entry_idx(pos, table, idx) {
+		freq = pos->frequency;
+
+		if (pos->driver_data != boost_level)
+			continue;
+		if (freq > policy->max || freq < policy->min)
+			continue;
+
+		if (freq == target_freq) {
+
+			return freq;
+		}
+		if (freq > target_freq) {
+			best = idx;
+			best_freq = freq;
+			continue;
+		}
+
+		/* No freq found above target_freq, return freq below target_freq */
+		if (best == -1) {
+			return freq;
+		}
+		/* Choose the closest freq */
+		if (table[best].frequency - target_freq > target_freq - freq) {
+
+			return freq;
+		}
+		return best_freq;
+	}
+
+	return best_freq;
+}
+
+/* Works only on sorted freq-tables */
+static int cpufreq_table_find_freq(struct cpufreq_policy *policy,
+					unsigned int target_freq,
+					int boost_level)
+{
+	target_freq = clamp_val(target_freq, policy->min, policy->max);
+	if (policy->freq_table_sorted == CPUFREQ_TABLE_SORTED_ASCENDING)
+		return cpufreq_table_find_freq_ac(policy, target_freq, boost_level);
+	else
+		return cpufreq_table_find_freq_dc(policy, target_freq, boost_level);
+}
+
+static void transition_end(struct cpufreq_policy *policy,
+		struct cpufreq_freqs *freqs, bool failed)
+{
+	if (unlikely(!policy->transition_ongoing)) {
+		return;
+	}
+	cpufreq_freq_transition_end(policy, freqs, failed);
+}
+static void transition_begin(struct cpufreq_policy *policy,
+		struct cpufreq_freqs *freqs)
+{
+	if (unlikely(policy->transition_ongoing)) {
+		cpufreq_freq_transition_end(policy, freqs, true);
+	}
+	cpufreq_freq_transition_begin(policy, freqs);
+}
+
+static void update_core_boost_info(struct core_data *core, bool boost_set)
+{
+	core->in_boost = boost_set;
+	if (boost_set)
+		core->package->boost_cores++;
+	else
+		core->package->boost_cores--;
+}
+
+static unsigned int cores_freq_trans_notify(struct package_data *package,
+						bool before_trans,
+						bool trans_failed,
+						int find_level,
+						int find_freq,
+						unsigned int skip_cpumask)
+{
+	int i;
+	struct cpufreq_policy *policy;
+	struct cpufreq_freqs freqs;
+	unsigned int cores_level = 0;
+	unsigned int core_level;
+
+	for (i = 0; i < package->nr_cores; i++) {
+		struct core_data *core = &package->core[i];
+		policy = cpufreq_cpu_get_raw(core->cpu);
+		if (((1 << i) & skip_cpumask) || !policy) {
+			continue;
+		}
+		freqs.old = policy->cur;
+		freqs.flags = 0;
+
+		/* find level from normal levels */
+		core_level = cpufreq_perf_find_level(core->perf, policy->cur, find_level);
+		if (!core_level) {
+			pr_debug("cpu%d policy->cur=%d find_level=%d freq=%d skip_cpumask=%x \n",
+					policy->cpu, policy->cur, find_level, find_freq, skip_cpumask);
+		}
+		freqs.new = cpufreq_perf_find_freq(core->perf, core_level, find_freq) * 1000;
+		if (!freqs.new) {
+			pr_debug("file %s, line %d, find freq error\n", __FILE__, __LINE__);
+		}
+
+		pr_debug("file %s, line %d, cpu %d, old freq %d, new freq %d, find_level %d, find_freq %d\n",
+				__FILE__, __LINE__, policy->cpu, freqs.old, freqs.new, find_level, find_freq);
+		cores_level |= (core_level << (i << 2));
+
+		if (before_trans)
+			transition_begin(policy, &freqs);
+		else {
+			transition_end(policy, &freqs, trans_failed);
+		}
+	}
+	return cores_level;
+}
+static int loongson3_set_freq(struct core_data *core, unsigned long freq, int boost_level)
+{
+	int ret = 0;
+	int freq_level;
+	int phy_cpu;
+	int target_freq;
+	struct cpufreq_freqs freqs;
+	struct cpufreq_policy *policy = cpufreq_cpu_get_raw(core->cpu);
+
+	if (!policy)
+		return -EINVAL;
+
+	ret = wait_for_ready_timeout(MAX_READY_TIMEOUT);
+	if (ret)
+		return ret;
+
+	phy_cpu = cpu_logical_map(core->cpu);
+	target_freq = cpufreq_table_find_freq(policy, freq, boost_level);
+	if (!target_freq)
+		return -1;
+	if (target_freq == policy->cur)
+		return -1;
+
+	freqs.flags = 0;
+	freqs.old = policy->cur;
+	freqs.new = target_freq;
+	freq_level = cpufreq_perf_find_level(core->perf, target_freq, boost_level);
+	if (!freq_level) {
+		pr_debug("loongson3_set_freq cpu%d freq=%lu targetfreq=%d boost_level=%d find level error\n",
+				core->cpu, freq, target_freq, boost_level);
+	}
+
+	transition_begin(policy, &freqs);
+	do_set_freq_level(phy_cpu, freq_level);
+	ret = wait_for_ready_timeout(MAX_READY_TIMEOUT);
+	transition_end(policy, &freqs, !!ret);
+
+	return ret;
+}
+
+int loongson3_set_mode(int mode, int freq_level)
+{
+	uint32_t val;
+	int ret = 0;
+	uint32_t message;
+
+	ret = wait_for_ready_timeout(MAX_READY_TIMEOUT);
+	if (ret)
+		return ret;
+
+	message = mode | (VOLTAGE_COMMAND << 24) | freq_level;
+	iocsr_write32(message, 0x51c);
+	val = iocsr_read32(0x420);
+	val |= 1 << 10;
+	iocsr_write32(val, 0x420);
+	return wait_for_ready_timeout(MAX_READY_TIMEOUT);
+}
+
+enum freq_adjust_action{
+	FAA_NORMAL,
+	FAA_N2B,
+	FAA_B2N,
+	FAA_BOOST,
+};
+
+static int faa_normal(struct cpufreq_policy *policy, int load)
+{
+	int ret;
+	unsigned int freq_next, min_f, max_f;
+	struct core_data *core = get_core_data(policy->cpu);
+	if (!core)
+		return -1;
+
+	pr_debug("file %s, line %d, func %s\n", __FILE__, __LINE__, __func__);
+
+	min_f = policy->min;
+	max_f = policy->max;
+	freq_next = min_f + load * (max_f - min_f) / 100;
+	ret = loongson3_set_freq(core, freq_next, 0);
+	return ret;
+}
+
+static void handle_boost_cores(struct core_data *core, struct package_data *package,
+		unsigned long target_freq, bool skip_update_and_notify, bool update_core, bool inc_boost)
+{
+	int boost_level;
+	int find_level;
+	int find_freq;
+	int ret;
+	int inc_core = inc_boost ? 1 : -1;
+
+	if (boost_gears == 1) {
+		find_level = 0;
+		boost_level = boost_gears;
+	} else {
+		find_level = package->boost_cores;
+		if (update_core)
+			boost_level = package->boost_cores + inc_core;
+		else
+			boost_level = package->boost_cores;
+	}
+	find_freq = boost_level;
+	ret = loongson3_set_freq(core, target_freq, boost_level);
+	if (ret)
+		return;
+
+	if (skip_update_and_notify) {
+		if (update_core)
+			update_core_boost_info(core, inc_boost);
+		return;
+	}
+
+	if (boost_gears != 1) {
+		cores_freq_trans_notify(package, true, false,
+				find_level, find_freq, 1 << core->cpu);
+		cores_freq_trans_notify(package, false, false,
+				find_level, find_freq, 1 << core->cpu);
+	}
+	if (update_core)
+		update_core_boost_info(core, inc_boost);
+}
+
+static void faa_boost(struct cpufreq_policy *policy, int load)
+{
+	unsigned int min_f, max_f;
+	struct core_data *core = get_core_data(policy->cpu);
+	struct package_data *package = core->package;
+	unsigned long target_freq;
+
+	pr_debug("file %s, line %d, func %s\n", __FILE__, __LINE__, __func__);
+
+	/* boost cores form n to n + 1 */
+	if (core->load > BOOST_THRESHOLD) {
+		if (package->boost_cores < package->max_boost_cores
+				&& !core->in_boost) {
+			if (boost_gears == 1) {
+				target_freq = policy->max;
+			} else {
+				target_freq = cpufreq_table_find_freq(policy, policy->max, package->boost_cores + 1);
+				if (!target_freq) {
+					pr_debug("file %s, line %d, find freq error ,boost_level %d, cur freq %d\n",
+							__FILE__, __LINE__, package->boost_cores, policy->max);
+				}
+			}
+			handle_boost_cores(core, package, target_freq, false, true, true);
+		}
+	} else {
+		/* 1. core not in boost, level up but not change  pll
+		 * 2. core in boost, boost cores from n to n - 1 */
+		min_f = policy->min;
+		max_f = policy->max;
+		target_freq = min_f + load * (max_f - min_f) / 100;
+		handle_boost_cores(core, package, target_freq, !core->in_boost, core->in_boost, false);
+	}
+
+
+}
+
+static void get_boost_cores(struct package_data *package, int *boost_cores, int *boost_count)
+{
+	struct core_data *core;
+	struct cpufreq_policy *policy;
+	int i;
+
+	/* count boost cores */
+	for (i = 0; i < package->nr_cores; i++) {
+		core = &package->core[i];
+		policy = cpufreq_cpu_get_raw(core->cpu);
+		if (!policy)
+			continue;
+
+		if (cpu_can_boost(core->cpu)) {
+			if (boost_cores)
+				*boost_cores |= (1 << i);
+
+			(*boost_count)++;
+		}
+	}
+}
+
+static void faa_n2b(struct package_data *package, struct core_data *core)
+{
+	int boost_cores = 0;
+	int boost_count = 0;
+	int freq_level;
+
+	pr_debug("file %s, line %d func %s\n", __FILE__, __LINE__, __func__);
+
+	get_boost_cores(package, &boost_cores, &boost_count);
+
+	if (boost_gears == 1) {
+		boost_count = 1;
+	}
+
+	freq_level = cores_freq_trans_notify(package, true, false,
+			0, boost_count, 0);
+	if (!loongson3_set_mode(BOOST_MODE, freq_level)) {
+		int i;
+		cores_freq_trans_notify(package, false, false,
+				0, boost_count, 0);
+		package->in_boost = true;
+		for (i = 0; i < package->nr_cores; i++) {
+			if (boost_cores & (1 << i))
+				update_core_boost_info(&package->core[i], true);
+		}
+	} else
+		cores_freq_trans_notify(package, false, true,
+				0, boost_count, 0);
+}
+
+static void faa_b2n(struct package_data *package)
+{
+	int i;
+	int boost_count = package->boost_cores;
+
+	if (boost_gears == 1) {
+		boost_count = 1;
+	}
+
+	pr_debug("file %s, line %d, func %s\n", __FILE__, __LINE__, __func__);
+
+	cores_freq_trans_notify(package, true, false,
+			boost_count, 0, 0);
+	if (!loongson3_set_mode(NORMAL_MODE, 0)) {
+		cores_freq_trans_notify(package, false, false,
+				boost_count, 0, 0);
+		for (i = 0; i < package->nr_cores; i++) {
+			if (package->core[i].in_boost)
+				update_core_boost_info(&package->core[i], false);
+		}
+		package->in_boost = false;
+	} else
+		cores_freq_trans_notify(package, false, true,
+				boost_count, 0, 0);
+}
+
+
+unsigned int load_update(struct core_data *core)
+{
+	int i;
+	u64 update_time, cur_idle_time;
+	unsigned int idle_time, time_elapsed;
+	unsigned int load = 0;
+	struct package_data *package = core->package;
+
+	cur_idle_time = get_cpu_idle_time(core->cpu, &update_time, true);
+
+	time_elapsed = update_time - core->prev_update_time;
+	core->prev_update_time = update_time;
+
+	idle_time = cur_idle_time - core->prev_cpu_idle;
+	core->prev_cpu_idle = cur_idle_time;
+
+	if (unlikely(!time_elapsed)) {
+		/*
+		 * That can only happen when this function is called
+		 * twice in a row with a very short interval between the
+		 * calls, so the previous load value can be used then.
+		 */
+		load = core->prev_load;
+	} else if (unlikely((int)idle_time > 2 * core->sampling_rate &&
+				core->prev_load)) {
+
+		load = core->prev_load;
+		core->prev_load = 0;
+	} else {
+		if (time_elapsed >= idle_time) {
+			load = 100 * (time_elapsed - idle_time) / time_elapsed;
+		} else {
+			load = (int)idle_time < 0 ? 100 : 0;
+		}
+		core->prev_load = load;
+	}
+
+	package->nr_full_load_cores = 0;
+	for (i = 0; i < package->nr_cores; i++) {
+		if (package->core[i].load > BOOST_THRESHOLD) {
+			package->nr_full_load_cores++;
+		}
+	}
+
+	return load;
+}
+
+static bool cpufreq_should_update_freq(struct core_data *core, u64 time)
+{
+	s64 delta_ns;
+	delta_ns = time - core->last_freq_update_time;
+	return delta_ns >= core->freq_update_delay_ns;
+}
+
+static void cpufreq_update(struct cpufreq_policy *policy)
+{
+	int action;
+	struct core_data *core;
+	struct package_data *package;
+	unsigned long int load;
+	bool should_be_boost = 0;
+
+	core = get_core_data(policy->cpu);
+	package = core->package;
+
+	mutex_lock(&boost_mutex[core->package_id]);
+
+	if (!core->update_util_set) {
+		mutex_unlock(&boost_mutex[core->package_id]);
+		return;
+	}
+
+	load = load_update(core);
+	core->load = (u64)load + ((core->load * FACTOR) >> 32);
+
+	if (cpufreq_boost_enabled()) {
+		should_be_boost = package_boost(package);
+	} else {
+		if (package->in_boost)
+			should_be_boost = false;
+	}
+
+	action = (package->in_boost << 1) | should_be_boost;
+	switch (action) {
+	case FAA_NORMAL:
+		faa_normal(policy, load);
+		break;
+	case FAA_B2N:
+		faa_b2n(package);
+		break;
+	case FAA_N2B:
+		faa_n2b(package, core);
+		break;
+	case FAA_BOOST:
+		faa_boost(policy, load);
+		break;
+	}
+	mutex_unlock(&boost_mutex[core->package_id]);
+}
+
+static void set_max_within_limits(struct cpufreq_policy *policy)
+{
+	struct core_data *core = get_core_data(policy->cpu);
+	/*
+	 * policy->max <= cpu->pstate.max_freq indecates that
+	 * the boost is disabled, so max freq is in normal range
+	 *
+	 * Skip performance policy with boost enabled!!!
+	 *
+	 * */
+	if (policy->max <= (core->normal_max_freq * 1000)) {
+		mutex_lock(&boost_mutex[core->package_id]);
+		if (!loongson3_set_freq(core, policy->max, 0))
+			pr_debug("Set cpu %d to performance mode under normal range.\n", policy->cpu);
+		mutex_unlock(&boost_mutex[core->package_id]);
+	}
+}
+
+static void clear_update_util_hook(unsigned int cpu)
+{
+	struct core_data *core = get_core_data(cpu);
+
+	if (!core->update_util_set)
+		return;
+
+	cpufreq_remove_update_util_hook(cpu);
+	core->update_util_set = false;
+	synchronize_rcu();
+}
+
+static void update_util_handler(struct update_util_data *data, u64 time,
+		unsigned int flags)
+{
+	struct core_data *core = container_of(data, struct core_data, update_util);
+
+	if (!cpufreq_should_update_freq(core, time))
+		return;
+	if (!core->work_in_progress) {
+		core->last_freq_update_time = time;
+		core->work_in_progress = true;
+		irq_work_queue(&core->irq_work);
+	}
+}
+static void set_update_util_hook(unsigned int cpu)
+{
+	struct core_data *core = get_core_data(cpu);
+	if (core->update_util_set)
+		return;
+
+	cpufreq_add_update_util_hook(cpu, &core->update_util,
+			update_util_handler);
+	core->update_util_set = true;
+}
+static int loongson3_cpufreq_set_policy(struct cpufreq_policy *policy)
+{
+	if (!policy->cpuinfo.max_freq)
+		return -ENODEV;
+
+	if (policy->policy == CPUFREQ_POLICY_PERFORMANCE) {
+		clear_update_util_hook(policy->cpu);
+		set_max_within_limits(policy);
+	} else {
+		set_update_util_hook(policy->cpu);
+	}
+
+	return 0;
+}
+
+static int loongson3_cpufreq_verify_policy(struct cpufreq_policy_data *policy)
+{
+	cpufreq_verify_within_limits(policy, policy->cpuinfo.min_freq, policy->cpuinfo.max_freq);
+
+	return 0;
+}
+
+static void set_boost_freq(bool has)
+{
+	cpufreq_has_boost_freq = has;
+}
+
+static bool has_boost_freq(void)
+{
+	return cpufreq_has_boost_freq;
+}
+
+static int compute_scale(int *shift, int dividor, int dividee)
+{
+	int i;
+	int result = 0;
+	int remainder = 0;
+	int scale_resolution = 8;
+
+	result = dividor / dividee;
+	remainder = (dividor % dividee) * 10;
+
+	for (i = 0; i < scale_resolution; i++) {
+		result =  result * 10 +  remainder  / dividee;
+		remainder = (remainder % dividee) * 10;
+		*shift *= 10;
+	}
+
+	return result;
+}
+
+static void cpufreq_work_handler(struct kthread_work *work)
+{
+	struct core_data *core;
+	struct cpufreq_policy *policy;
+
+	core = container_of(work, struct core_data, work);
+	policy = cpufreq_cpu_get_raw(core->cpu);
+
+	if (policy) {
+		cpufreq_update(policy);
+		core->work_in_progress = false;
+	}
+}
+
+static void cpufreq_irq_work(struct irq_work *irq_work)
+{
+	struct core_data *core = container_of(irq_work, struct core_data, irq_work);
+	kthread_queue_work(&cpufreq_worker, &core->work);
+}
+
+static void cpufreq_kthread_stop(void)
+{
+	kthread_flush_worker(&cpufreq_worker);
+	kthread_stop(cpufreq_thread);
+}
+static int cpufreq_kthread_create(void)
+{
+	struct sched_attr attr = {
+		.size		= sizeof(struct sched_attr),
+		.sched_policy	= SCHED_DEADLINE,
+		.sched_flags	= 0x10000000,
+		.sched_nice	= 0,
+		.sched_priority	= 0,
+		.sched_runtime	=  1000000,
+		.sched_deadline = 10000000,
+		.sched_period	= 10000000,
+	};
+	int ret;
+
+	kthread_init_worker(&cpufreq_worker);
+	cpufreq_thread = kthread_create(kthread_worker_fn, &cpufreq_worker, "lsfrq:%d", 0);
+	if (IS_ERR(cpufreq_thread)) {
+		return PTR_ERR(cpufreq_thread);
+	}
+
+	ret = sched_setattr_nocheck(cpufreq_thread, &attr);
+	if (ret) {
+		kthread_stop(cpufreq_thread);
+		pr_warn("%s: failed to set SCHED_DEADLINE\n", __func__);
+		return ret;
+	}
+
+	wake_up_process(cpufreq_thread);
+
+	return 0;
+}
+
+static int init_acpi(struct acpi_processor_performance *perf)
+{
+	int result = 0;
+	int i;
+
+	perf->shared_type = 0;
+	perf->state_count = (max_freq_level - min_freq_level + 1) * (boost_gears + 1);
+
+	perf->states =
+		kmalloc_array(perf->state_count,
+				sizeof(struct acpi_processor_px),
+				GFP_KERNEL);
+
+	if (!perf->states) {
+		result = -ENOMEM;
+		return result;
+	}
+
+	for (i = 0; i < perf->state_count; i++) {
+		perf->states[i].power = 0x3A98;
+		perf->states[i].transition_latency = 10000;
+		perf->states[i].bus_master_latency = 10000;
+		perf->states[i].status = (RESERVED_FREQ + i / (boost_gears + 1));
+		perf->states[i].control = (RESERVED_FREQ + i / (boost_gears + 1));
+
+		switch (i % (boost_gears + 1)) {
+		case 0:
+			perf->states[i].core_frequency = (cpu_clock_freq / 1000000) * (8 - i / (boost_gears + 1)) / 8;
+			break;
+		case 1:
+		case 2:
+		case 3:
+		case 4:
+			perf->states[i].core_frequency =
+				boost_freqs[i % (boost_gears + 1)] * (8 - i / (boost_gears + 1)) / 8;
+			perf->states[i].control |= ((i % (boost_gears + 1)) << 8);
+			break;
+		default:
+			pr_info("file %s, line %d, i %d freq table error\n", __FILE__, __LINE__, i);
+		}
+	}
+
+	return result;
+}
+
+static int loongson3_cpufreq_cpu_init(struct cpufreq_policy *policy)
+{
+	unsigned int i;
+	struct acpi_processor_performance *perf;
+	struct cpufreq_frequency_table *freq_table;
+	struct core_data *core;
+	int package_id;
+	unsigned int cpu = policy->cpu;
+	unsigned int result = 0;
+
+	perf = per_cpu_ptr(acpi_perf_data, cpu);
+	package_id = cpu_data[cpu].package;
+	core = get_core_data(cpu);
+	all_package_data[package_id].nr_cores = loongson_sysconf.cores_per_package;
+	all_package_data[package_id].max_boost_cores = max_boost_cores;
+	core->normal_max_freq = 0;
+	all_package_data[package_id].nr_full_load_cores = 0;
+	core->cpu = cpu;
+	core->work_in_progress = false;
+	core->last_freq_update_time = 0;
+	core->perf = perf;
+	core->package_id = package_id;
+	core->package = &all_package_data[package_id];
+
+	core->boost_freq = kmalloc_array(boost_gears + 1, sizeof(typeof(core->boost_freq)), GFP_KERNEL);
+	core->clock_scale = kmalloc_array(boost_gears + 1, sizeof(typeof(core->clock_scale)), GFP_KERNEL);
+	core->shift = kmalloc_array(boost_gears + 1, sizeof(typeof(core->shift)), GFP_KERNEL);
+
+	for (i = 0; i < boost_gears + 1; i++) {
+		core->boost_freq[i] = boost_freqs[i];
+		core->shift[i] = 1;
+	}
+
+	if (!acpi_disabled)
+		result = acpi_processor_register_performance(perf, cpu);
+	else {
+		result = init_acpi(perf);
+		policy->shared_type = perf->shared_type;
+	}
+
+	if (result) {
+		pr_info("CPU%d acpi_processor_register_performance failed.\n", cpu);
+		return result;
+	}
+
+	for (i = 0; i < MAX_PACKAGES; i++) {
+		mutex_init(&boost_mutex[i]);
+	}
+
+	/* capability check */
+	if (perf->state_count <= 1) {
+		pr_debug("No P-States\n");
+		result = -ENODEV;
+		goto err_unreg;
+	}
+
+	freq_table = kcalloc(perf->state_count + 1, sizeof(*freq_table),
+			GFP_KERNEL);
+	if (!freq_table) {
+		result = -ENOMEM;
+		goto err_unreg;
+	}
+
+	/* detect transition latency */
+	policy->cpuinfo.transition_latency = 0;
+	for (i = 0; i < perf->state_count; i++) {
+		if ((perf->states[i].transition_latency * 1000) >
+				policy->cpuinfo.transition_latency)
+			policy->cpuinfo.transition_latency =
+				perf->states[i].transition_latency * 1000;
+		if (perf->states[i].control & LOONGSON_BOOST_FREQ_MASK) {
+			set_boost_freq(true);
+		} else {
+			if (perf->states[i].core_frequency > core->normal_max_freq)
+				core->normal_max_freq = perf->states[i].core_frequency;
+		}
+	}
+
+	core->freq_update_delay_ns = policy->cpuinfo.transition_latency;
+
+	for (i = 0; i < boost_gears + 1; i++) {
+		core->clock_scale[i] = compute_scale(&core->shift[i], boost_freqs[i], core->normal_max_freq);
+		pr_debug("file %s, line %d, boost_freqs[%d] %d, normal_max_freq %d, scale %d, shift %d\n",
+				__FILE__, __LINE__, i, boost_freqs[i], core->normal_max_freq, core->clock_scale[i], core->shift[i]);
+	}
+
+	/* table init */
+	for (i = 0; i < perf->state_count; i++) {
+		freq_table[i].driver_data = (perf->states[i].control & LOONGSON_BOOST_FREQ_MASK) >> 8;
+		if (freq_table[i].driver_data)
+			freq_table[i].flags |= CPUFREQ_BOOST_FREQ;
+		freq_table[i].frequency =
+			perf->states[i].core_frequency * 1000;
+	}
+	freq_table[i].frequency = CPUFREQ_TABLE_END;
+	policy->freq_table = freq_table;
+	perf->state = 0;
+
+	/* add boost-attr if supported. */
+	if (has_boost_freq() && boost_supported())
+		loongson3_cpufreq_attr[1] = &cpufreq_freq_attr_scaling_boost_freqs;
+
+	pr_info("CPU%u - ACPI performance management activated.\n", cpu);
+	for (i = 0; i < perf->state_count; i++)
+		pr_debug("     %cP%d: %d MHz, %d mW, %d uS %d level\n",
+				(i == perf->state ? '*' : ' '), i,
+				(u32) perf->states[i].core_frequency,
+				(u32) perf->states[i].power,
+				(u32) perf->states[i].transition_latency,
+				(u32) perf->states[i].control);
+
+	/*
+	 * the first call to ->target() should result in us actually
+	 * writing something to the appropriate registers.
+	 */
+	policy->fast_switch_possible = false;
+
+	init_irq_work(&core->irq_work, cpufreq_irq_work);
+	kthread_init_work(&core->work, cpufreq_work_handler);
+	core->sampling_rate = max_t(unsigned int,
+			CPUFREQ_SAMPLING_INTERVAL,
+			cpufreq_policy_transition_delay_us(policy));
+	return result;
+
+err_unreg:
+	if (!acpi_disabled)
+		acpi_processor_unregister_performance(cpu);
+
+	return result;
+}
+
+static int loongson3_cpufreq_cpu_exit(struct cpufreq_policy *policy)
+{
+	struct core_data *core = get_core_data(policy->cpu);
+	clear_update_util_hook(policy->cpu);
+	irq_work_sync(&core->irq_work);
+	kthread_cancel_work_sync(&core->work);
+	core->work_in_progress = false;
+	policy->fast_switch_possible = false;
+	if (!acpi_disabled)
+		acpi_processor_unregister_performance(policy->cpu);
+	kfree(policy->freq_table);
+	kfree(core->boost_freq);
+	kfree(core->clock_scale);
+	kfree(core->shift);
+	return 0;
+}
+
+static struct freq_attr *loongson3_cpufreq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,   /* Extra space for boost-attr if supported */
+	NULL,
+};
+
+static struct cpufreq_driver loongson3_cpufreq_driver = {
+	.verify		= loongson3_cpufreq_verify_policy,
+	.setpolicy	= loongson3_cpufreq_set_policy,
+	.init		= loongson3_cpufreq_cpu_init,
+	.exit		= loongson3_cpufreq_cpu_exit,
+	.name		= "acpi-cpufreq",
+	.attr		= loongson3_cpufreq_attr,
+};
+
+static void free_acpi_perf_data(void)
+{
+	unsigned int i;
+
+	/* Freeing a NULL pointer is OK, and alloc_percpu zeroes. */
+	for_each_possible_cpu(i)
+		free_cpumask_var(per_cpu_ptr(acpi_perf_data, i)
+				->shared_cpu_map);
+	free_percpu(acpi_perf_data);
+}
+
+static int __init loongson3_cpufreq_early_init(void)
+{
+	unsigned int i;
+	pr_debug("acpi_cpufreq_early_init\n");
+
+	acpi_perf_data = alloc_percpu(struct acpi_processor_performance);
+	if (!acpi_perf_data) {
+		return -ENOMEM;
+	}
+	for_each_possible_cpu(i) {
+		if (!zalloc_cpumask_var_node(
+					&per_cpu_ptr(acpi_perf_data, i)->shared_cpu_map,
+					GFP_KERNEL, cpu_to_node(i))) {
+			free_acpi_perf_data();
+			return -ENOMEM;
+		}
+	}
+	return 0;
+}
+
+static bool support_boost(void)
+{
+	int message;
+	int val;
+	int i;
+
+	if (wait_for_ready_timeout(MAX_READY_TIMEOUT))
+		return false;
+	message = DVFS_INFO << 24;
+	iocsr_write32(message, 0x51c);
+	val = iocsr_read32(0x420);
+
+	val |= 1 << 10;
+	iocsr_write32(val, 0x420);
+	if (wait_for_ready_timeout(MAX_READY_TIMEOUT)) {
+		pr_info("file %s, line %d, not support boost\n", __FILE__, __LINE__);
+		return false;
+	}
+
+	val = iocsr_read32(0x51c);
+
+	min_freq_level = val & DVFS_INFO_MIN_FREQ;
+	max_freq_level = (val & DVFS_INFO_MAX_FREQ) >> 4;
+
+	if ((val & DVFS_INFO_BOOST_CORE_FREQ) && ((val & DVFS_INFO_BOOST_CORES) >> 20)) {
+		max_boost_cores = (val & DVFS_INFO_BOOST_CORES) >> 20;
+		max_boost_freq = ((val & DVFS_INFO_BOOST_CORE_FREQ) >> 8) * 25;
+		max_upper_index = (val & DVFS_INFO_NORMAL_CORE_UPPER_LIMIT) >> 16;
+	} else {
+		boost_gears = 0;
+		return false;
+	}
+
+	/* Read boost levels */
+	if (wait_for_ready_timeout(MAX_READY_TIMEOUT))
+		return false;
+
+	/* for version 1, single boost freq boost */
+	message = DVFS_INFO_BOOST_LEVEL << 24;
+	iocsr_write32(message, 0x51c);
+	val = iocsr_read32(0x420);
+
+	val |= 1 << 10;
+	iocsr_write32(val, 0x420);
+
+	if (wait_for_ready_timeout(MAX_READY_TIMEOUT)) {
+		pr_info("file %s, line %d, single boost mode\n", __FILE__, __LINE__);
+		boost_gears = 1;
+		boost_freqs[0] = calc_const_freq() / 1000000;
+		for (i = 1; i < boost_gears + 1; i++) {
+			boost_freqs[i] = max_boost_freq;
+		}
+
+		/* set 0x51c complete */
+		iocsr_write32(COMPLETE_STATUS, 0x51c);
+	} else {
+		pr_info("file %s, line %d, multi boost mode\n", __FILE__, __LINE__);
+		boost_gears = max_boost_cores;
+		val = iocsr_read32(0x51c);
+
+		boost_freqs[0] = calc_const_freq() / 1000000;
+		boost_freqs[1] = max_boost_freq;
+
+		if (boost_gears > 1) {
+			for (i = 2; i < boost_gears + 1; i++) {
+				boost_freqs[i] = max_boost_freq - (((val >> ((i-2) * 4)) & 0xf) * FREQ_STEP);
+			}
+		}
+	}
+
+	pr_info("file %s, line %d, min_freq_level %d, max_freq_level %d, max_boost_cores %d, boost_gears %d\n",
+			__FILE__, __LINE__, min_freq_level, max_freq_level, max_boost_cores, boost_gears);
+
+	return true;
+}
+
+static int cpufreq_table_cpuinfo(struct cpufreq_policy *policy,
+				struct cpufreq_frequency_table *table,
+				bool boost)
+{
+	struct cpufreq_frequency_table *pos;
+	unsigned int min_freq = ~0;
+	unsigned int max_freq = 0;
+	unsigned int freq;
+
+	cpufreq_for_each_valid_entry(pos, table) {
+		freq = pos->frequency;
+
+		if (!boost) {
+			if (pos->driver_data)
+				continue;
+		}
+		if (freq < min_freq)
+			min_freq = freq;
+		if (freq > max_freq)
+			max_freq = freq;
+	}
+
+	policy->min = policy->cpuinfo.min_freq = min_freq;
+	policy->max = policy->cpuinfo.max_freq = max_freq;
+	if (policy->min == ~0)
+		return -EINVAL;
+	else
+		return 0;
+}
+
+static int set_boost(struct cpufreq_policy *policy, int state)
+{
+	if (!has_boost_freq())
+		return -EINVAL;
+
+	if (!policy)
+		return -EINVAL;
+
+	if (!state) {
+		if (policy->policy == CPUFREQ_POLICY_POWERSAVE) {
+			cpufreq_update(policy);
+		}
+	}
+	if (!policy->freq_table)
+		return -EINVAL;
+
+	cpufreq_table_cpuinfo(policy, policy->freq_table, state);
+	down_write(&policy->rwsem);
+	up_write(&policy->rwsem);
+
+	if (!state) {
+		set_max_within_limits(policy);
+	}
+
+
+	return 0;
+}
+
+static void __init loongson3_cpufreq_boost_init(void)
+{
+	if (!support_boost()) {
+		pr_info("Boost capabilities not present in the processor\n");
+		return;
+	}
+
+	loongson3_cpufreq_driver.set_boost = set_boost;
+}
+
+static int cpufreq_supported_detect(void)
+{
+	return wait_for_ready_timeout(MAX_READY_TIMEOUT);
+}
+
+static int __init loongson3_cpufreq_init(void)
+{
+	int ret;
+	if (!cpu_has_csr || !cpu_has_scalefreq)
+			return -ENODEV;
+
+	/* don't keep reloading if cpufreq_driver exists */
+	if (cpufreq_get_current_driver())
+		return -EEXIST;
+
+	pr_debug("loongson3_cpufreq_init\n");
+	if (cpufreq_supported_detect()) {
+		pr_info("loongson3_cpufreq_init failed!\n");
+		return -ENODEV;
+	}
+
+	ret = loongson3_cpufreq_early_init();
+	if (ret)
+		return ret;
+	loongson3_cpufreq_boost_init();
+
+	cpufreq_register_notifier(&loongson3_cpufreq_notifier_block,
+			CPUFREQ_TRANSITION_NOTIFIER);
+	ret = cpufreq_register_driver(&loongson3_cpufreq_driver);
+	cpufreq_kthread_create();
+	if (ret) {
+		free_acpi_perf_data();
+	}
+	return ret;
+}
+
+static void __exit loongson3_cpufreq_exit(void)
+{
+	pr_debug("loongson3_cpufreq_exit\n");
+
+	cpufreq_unregister_driver(&loongson3_cpufreq_driver);
+	free_acpi_perf_data();
+	cpufreq_kthread_stop();
+}
+
+late_initcall(loongson3_cpufreq_init);
+module_exit(loongson3_cpufreq_exit);
+
+static const struct acpi_device_id processor_device_ids[] = {
+	{ACPI_PROCESSOR_OBJECT_HID, },
+	{ACPI_PROCESSOR_DEVICE_HID, },
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, processor_device_ids);
+
+MODULE_ALIAS("acpi");