[linux-block.git] / drivers / cpufreq / s3c2416-cpufreq.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * S3C2416/2450 CPUfreq Support
 *
 * Copyright 2011 Heiko Stuebner <heiko@sntech.de>
 *
 * based on s3c64xx_cpufreq.c
 *
 * Copyright 2009 Wolfson Microelectronics plc
 */

#include <linux/kernel.h>
#include <linux/types.h>
#include <linux/init.h>
#include <linux/cpufreq.h>
#include <linux/clk.h>
#include <linux/err.h>
#include <linux/regulator/consumer.h>
#include <linux/reboot.h>
#include <linux/module.h>

static DEFINE_MUTEX(cpufreq_lock);

struct s3c2416_data {
	struct clk *armdiv;
	struct clk *armclk;
	struct clk *hclk;

	unsigned long regulator_latency;
#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	struct regulator *vddarm;
#endif

	struct cpufreq_frequency_table *freq_table;

	bool is_dvs;
	bool disable_dvs;
};

static struct s3c2416_data s3c2416_cpufreq;

struct s3c2416_dvfs {
	unsigned int vddarm_min;
	unsigned int vddarm_max;
};

/* pseudo-frequency for dvs mode */
#define FREQ_DVS	132333

/* frequency to sleep and reboot in
 * it's essential to leave dvs, as some boards do not reconfigure the
 * regulator on reboot
 */
#define FREQ_SLEEP	133333

/* Sources for the ARMCLK */
#define SOURCE_HCLK	0
#define SOURCE_ARMDIV	1

#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
/* S3C2416 only supports changing the voltage in the dvs-mode.
 * Voltages down to 1.0V seem to work, so we take what the regulator
 * can get us.
 */
static struct s3c2416_dvfs s3c2416_dvfs_table[] = {
	[SOURCE_HCLK] = {  950000, 1250000 },
	[SOURCE_ARMDIV] = { 1250000, 1350000 },
};
#endif

static struct cpufreq_frequency_table s3c2416_freq_table[] = {
	{ 0, SOURCE_HCLK, FREQ_DVS },
	{ 0, SOURCE_ARMDIV, 133333 },
	{ 0, SOURCE_ARMDIV, 266666 },
	{ 0, SOURCE_ARMDIV, 400000 },
	{ 0, 0, CPUFREQ_TABLE_END },
};

static struct cpufreq_frequency_table s3c2450_freq_table[] = {
	{ 0, SOURCE_HCLK, FREQ_DVS },
	{ 0, SOURCE_ARMDIV, 133500 },
	{ 0, SOURCE_ARMDIV, 267000 },
	{ 0, SOURCE_ARMDIV, 534000 },
	{ 0, 0, CPUFREQ_TABLE_END },
};

static unsigned int s3c2416_cpufreq_get_speed(unsigned int cpu)
{
	struct s3c2416_data *s3c_freq = &s3c2416_cpufreq;

	if (cpu != 0)
		return 0;

	/* return our pseudo-frequency when in dvs mode */
	if (s3c_freq->is_dvs)
		return FREQ_DVS;

	return clk_get_rate(s3c_freq->armclk) / 1000;
}

static int s3c2416_cpufreq_set_armdiv(struct s3c2416_data *s3c_freq,
				      unsigned int freq)
{
	int ret;

	if (clk_get_rate(s3c_freq->armdiv) / 1000 != freq) {
		ret = clk_set_rate(s3c_freq->armdiv, freq * 1000);
		if (ret < 0) {
			pr_err("cpufreq: Failed to set armdiv rate %dkHz: %d\n",
			       freq, ret);
			return ret;
		}
	}

	return 0;
}

static int s3c2416_cpufreq_enter_dvs(struct s3c2416_data *s3c_freq, int idx)
{
#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	struct s3c2416_dvfs *dvfs;
#endif
	int ret;

	if (s3c_freq->is_dvs) {
		pr_debug("cpufreq: already in dvs mode, nothing to do\n");
		return 0;
	}

	pr_debug("cpufreq: switching armclk to hclk (%lukHz)\n",
		 clk_get_rate(s3c_freq->hclk) / 1000);
	ret = clk_set_parent(s3c_freq->armclk, s3c_freq->hclk);
	if (ret < 0) {
		pr_err("cpufreq: Failed to switch armclk to hclk: %d\n", ret);
		return ret;
	}

#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	/* changing the core voltage is only allowed when in dvs mode */
	if (s3c_freq->vddarm) {
		dvfs = &s3c2416_dvfs_table[idx];

		pr_debug("cpufreq: setting regulator to %d-%d\n",
			 dvfs->vddarm_min, dvfs->vddarm_max);
		ret = regulator_set_voltage(s3c_freq->vddarm,
					    dvfs->vddarm_min,
					    dvfs->vddarm_max);

		/* when lowering the voltage failed, there is nothing to do */
		if (ret != 0)
			pr_err("cpufreq: Failed to set VDDARM: %d\n", ret);
	}
#endif

	s3c_freq->is_dvs = 1;

	return 0;
}

static int s3c2416_cpufreq_leave_dvs(struct s3c2416_data *s3c_freq, int idx)
{
#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	struct s3c2416_dvfs *dvfs;
#endif
	int ret;

	if (!s3c_freq->is_dvs) {
		pr_debug("cpufreq: not in dvs mode, so can't leave\n");
		return 0;
	}

#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	if (s3c_freq->vddarm) {
		dvfs = &s3c2416_dvfs_table[idx];

		pr_debug("cpufreq: setting regulator to %d-%d\n",
			 dvfs->vddarm_min, dvfs->vddarm_max);
		ret = regulator_set_voltage(s3c_freq->vddarm,
					    dvfs->vddarm_min,
					    dvfs->vddarm_max);
		if (ret != 0) {
			pr_err("cpufreq: Failed to set VDDARM: %d\n", ret);
			return ret;
		}
	}
#endif

	/* force armdiv to hclk frequency for transition from dvs*/
	if (clk_get_rate(s3c_freq->armdiv) > clk_get_rate(s3c_freq->hclk)) {
		pr_debug("cpufreq: force armdiv to hclk frequency (%lukHz)\n",
			 clk_get_rate(s3c_freq->hclk) / 1000);
		ret = s3c2416_cpufreq_set_armdiv(s3c_freq,
					clk_get_rate(s3c_freq->hclk) / 1000);
		if (ret < 0) {
			pr_err("cpufreq: Failed to set the armdiv to %lukHz: %d\n",
			       clk_get_rate(s3c_freq->hclk) / 1000, ret);
			return ret;
		}
	}

	pr_debug("cpufreq: switching armclk parent to armdiv (%lukHz)\n",
			clk_get_rate(s3c_freq->armdiv) / 1000);

	ret = clk_set_parent(s3c_freq->armclk, s3c_freq->armdiv);
	if (ret < 0) {
		pr_err("cpufreq: Failed to switch armclk clock parent to armdiv: %d\n",
		       ret);
		return ret;
	}

	s3c_freq->is_dvs = 0;

	return 0;
}

static int s3c2416_cpufreq_set_target(struct cpufreq_policy *policy,
				      unsigned int index)
{
	struct s3c2416_data *s3c_freq = &s3c2416_cpufreq;
	unsigned int new_freq;
	int idx, ret, to_dvs = 0;

	mutex_lock(&cpufreq_lock);

	idx = s3c_freq->freq_table[index].driver_data;

	if (idx == SOURCE_HCLK)
		to_dvs = 1;

	/* switching to dvs when it's not allowed */
	if (to_dvs && s3c_freq->disable_dvs) {
		pr_debug("cpufreq: entering dvs mode not allowed\n");
		ret = -EINVAL;
		goto out;
	}

	/* When leavin dvs mode, always switch the armdiv to the hclk rate
	 * The S3C2416 has stability issues when switching directly to
	 * higher frequencies.
	 */
	new_freq = (s3c_freq->is_dvs && !to_dvs)
				? clk_get_rate(s3c_freq->hclk) / 1000
				: s3c_freq->freq_table[index].frequency;

	if (to_dvs) {
		pr_debug("cpufreq: enter dvs\n");
		ret = s3c2416_cpufreq_enter_dvs(s3c_freq, idx);
	} else if (s3c_freq->is_dvs) {
		pr_debug("cpufreq: leave dvs\n");
		ret = s3c2416_cpufreq_leave_dvs(s3c_freq, idx);
	} else {
		pr_debug("cpufreq: change armdiv to %dkHz\n", new_freq);
		ret = s3c2416_cpufreq_set_armdiv(s3c_freq, new_freq);
	}

out:
	mutex_unlock(&cpufreq_lock);

	return ret;
}

#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
static void s3c2416_cpufreq_cfg_regulator(struct s3c2416_data *s3c_freq)
{
	int count, v, i, found;
	struct cpufreq_frequency_table *pos;
	struct s3c2416_dvfs *dvfs;

	count = regulator_count_voltages(s3c_freq->vddarm);
	if (count < 0) {
		pr_err("cpufreq: Unable to check supported voltages\n");
		return;
	}

	if (!count)
		goto out;

	cpufreq_for_each_valid_entry(pos, s3c_freq->freq_table) {
		dvfs = &s3c2416_dvfs_table[pos->driver_data];
		found = 0;

		/* Check only the min-voltage, more is always ok on S3C2416 */
		for (i = 0; i < count; i++) {
			v = regulator_list_voltage(s3c_freq->vddarm, i);
			if (v >= dvfs->vddarm_min)
				found = 1;
		}

		if (!found) {
			pr_debug("cpufreq: %dkHz unsupported by regulator\n",
				 pos->frequency);
			pos->frequency = CPUFREQ_ENTRY_INVALID;
		}
	}

out:
	/* Guessed */
	s3c_freq->regulator_latency = 1 * 1000 * 1000;
}
#endif

static int s3c2416_cpufreq_reboot_notifier_evt(struct notifier_block *this,
					       unsigned long event, void *ptr)
{
	struct s3c2416_data *s3c_freq = &s3c2416_cpufreq;
	int ret;
	struct cpufreq_policy *policy;

	mutex_lock(&cpufreq_lock);

	/* disable further changes */
	s3c_freq->disable_dvs = 1;

	mutex_unlock(&cpufreq_lock);

	/* some boards don't reconfigure the regulator on reboot, which
	 * could lead to undervolting the cpu when the clock is reset.
	 * Therefore we always leave the DVS mode on reboot.
	 */
	if (s3c_freq->is_dvs) {
		pr_debug("cpufreq: leave dvs on reboot\n");

		policy = cpufreq_cpu_get(0);
		if (!policy) {
			pr_debug("cpufreq: get no policy for cpu0\n");
			return NOTIFY_BAD;
		}

		ret = cpufreq_driver_target(policy, FREQ_SLEEP, 0);
		cpufreq_cpu_put(policy);

		if (ret < 0)
			return NOTIFY_BAD;
	}

	return NOTIFY_DONE;
}

static struct notifier_block s3c2416_cpufreq_reboot_notifier = {
	.notifier_call = s3c2416_cpufreq_reboot_notifier_evt,
};

static int s3c2416_cpufreq_driver_init(struct cpufreq_policy *policy)
{
	struct s3c2416_data *s3c_freq = &s3c2416_cpufreq;
	struct cpufreq_frequency_table *pos;
	struct clk *msysclk;
	unsigned long rate;
	int ret;

	if (policy->cpu != 0)
		return -EINVAL;

	msysclk = clk_get(NULL, "msysclk");
	if (IS_ERR(msysclk)) {
		ret = PTR_ERR(msysclk);
		pr_err("cpufreq: Unable to obtain msysclk: %d\n", ret);
		return ret;
	}

	/*
	 * S3C2416 and S3C2450 share the same processor-ID and also provide no
	 * other means to distinguish them other than through the rate of
	 * msysclk. On S3C2416 msysclk runs at 800MHz and on S3C2450 at 533MHz.
	 */
	rate = clk_get_rate(msysclk);
	if (rate == 800 * 1000 * 1000) {
		pr_info("cpufreq: msysclk running at %lukHz, using S3C2416 frequency table\n",
			rate / 1000);
		s3c_freq->freq_table = s3c2416_freq_table;
		policy->cpuinfo.max_freq = 400000;
	} else if (rate / 1000 == 534000) {
		pr_info("cpufreq: msysclk running at %lukHz, using S3C2450 frequency table\n",
			rate / 1000);
		s3c_freq->freq_table = s3c2450_freq_table;
		policy->cpuinfo.max_freq = 534000;
	}

	/* not needed anymore */
	clk_put(msysclk);

	if (s3c_freq->freq_table == NULL) {
		pr_err("cpufreq: No frequency information for this CPU, msysclk at %lukHz\n",
		       rate / 1000);
		return -ENODEV;
	}

	s3c_freq->is_dvs = 0;

	s3c_freq->armdiv = clk_get(NULL, "armdiv");
	if (IS_ERR(s3c_freq->armdiv)) {
		ret = PTR_ERR(s3c_freq->armdiv);
		pr_err("cpufreq: Unable to obtain ARMDIV: %d\n", ret);
		return ret;
	}

	s3c_freq->hclk = clk_get(NULL, "hclk");
	if (IS_ERR(s3c_freq->hclk)) {
		ret = PTR_ERR(s3c_freq->hclk);
		pr_err("cpufreq: Unable to obtain HCLK: %d\n", ret);
		goto err_hclk;
	}

	/* chech hclk rate, we only support the common 133MHz for now
	 * hclk could also run at 66MHz, but this not often used
	 */
	rate = clk_get_rate(s3c_freq->hclk);
	if (rate < 133 * 1000 * 1000) {
		pr_err("cpufreq: HCLK not at 133MHz\n");
		ret = -EINVAL;
		goto err_armclk;
	}

	s3c_freq->armclk = clk_get(NULL, "armclk");
	if (IS_ERR(s3c_freq->armclk)) {
		ret = PTR_ERR(s3c_freq->armclk);
		pr_err("cpufreq: Unable to obtain ARMCLK: %d\n", ret);
		goto err_armclk;
	}

#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	s3c_freq->vddarm = regulator_get(NULL, "vddarm");
	if (IS_ERR(s3c_freq->vddarm)) {
		ret = PTR_ERR(s3c_freq->vddarm);
		pr_err("cpufreq: Failed to obtain VDDARM: %d\n", ret);
		goto err_vddarm;
	}

	s3c2416_cpufreq_cfg_regulator(s3c_freq);
#else
	s3c_freq->regulator_latency = 0;
#endif

	cpufreq_for_each_entry(pos, s3c_freq->freq_table) {
		/* special handling for dvs mode */
		if (pos->driver_data == 0) {
			if (!s3c_freq->hclk) {
				pr_debug("cpufreq: %dkHz unsupported as it would need unavailable dvs mode\n",
					 pos->frequency);
				pos->frequency = CPUFREQ_ENTRY_INVALID;
			} else {
				continue;
			}
		}

		/* Check for frequencies we can generate */
		rate = clk_round_rate(s3c_freq->armdiv,
				      pos->frequency * 1000);
		rate /= 1000;
		if (rate != pos->frequency) {
			pr_debug("cpufreq: %dkHz unsupported by clock (clk_round_rate return %lu)\n",
				pos->frequency, rate);
			pos->frequency = CPUFREQ_ENTRY_INVALID;
		}
	}

	/* Datasheet says PLL stabalisation time must be at least 300us,
	 * so but add some fudge. (reference in LOCKCON0 register description)
	 */
	cpufreq_generic_init(policy, s3c_freq->freq_table,
			(500 * 1000) + s3c_freq->regulator_latency);
	register_reboot_notifier(&s3c2416_cpufreq_reboot_notifier);

	return 0;

#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
err_vddarm:
	clk_put(s3c_freq->armclk);
#endif
err_armclk:
	clk_put(s3c_freq->hclk);
err_hclk:
	clk_put(s3c_freq->armdiv);

	return ret;
}

static struct cpufreq_driver s3c2416_cpufreq_driver = {
	.flags		= CPUFREQ_NEED_INITIAL_FREQ_CHECK,
	.verify		= cpufreq_generic_frequency_table_verify,
	.target_index	= s3c2416_cpufreq_set_target,
	.get		= s3c2416_cpufreq_get_speed,
	.init		= s3c2416_cpufreq_driver_init,
	.name		= "s3c2416",
	.attr		= cpufreq_generic_attr,
};

static int __init s3c2416_cpufreq_init(void)
{
	return cpufreq_register_driver(&s3c2416_cpufreq_driver);
}
module_init(s3c2416_cpufreq_init);
Commit	Line	Data
d2912cb1	1	// SPDX-License-Identifier: GPL-2.0-only
34ee5507 HS	2	/*
	3	* S3C2416/2450 CPUfreq Support
	4	*
	5	* Copyright 2011 Heiko Stuebner <heiko@sntech.de>
	6	*
	7	* based on s3c64xx_cpufreq.c
	8	*
	9	* Copyright 2009 Wolfson Microelectronics plc
34ee5507 HS	10	*/
	11
	12	#include <linux/kernel.h>
	13	#include <linux/types.h>
	14	#include <linux/init.h>
	15	#include <linux/cpufreq.h>
	16	#include <linux/clk.h>
	17	#include <linux/err.h>
	18	#include <linux/regulator/consumer.h>
	19	#include <linux/reboot.h>
	20	#include <linux/module.h>
	21
	22	static DEFINE_MUTEX(cpufreq_lock);
	23
	24	struct s3c2416_data {
	25	struct clk *armdiv;
	26	struct clk *armclk;
	27	struct clk *hclk;
	28
	29	unsigned long regulator_latency;
	30	#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	31	struct regulator *vddarm;
	32	#endif
	33
	34	struct cpufreq_frequency_table *freq_table;
	35
	36	bool is_dvs;
	37	bool disable_dvs;
	38	};
	39
	40	static struct s3c2416_data s3c2416_cpufreq;
	41
	42	struct s3c2416_dvfs {
	43	unsigned int vddarm_min;
	44	unsigned int vddarm_max;
	45	};
	46
	47	/* pseudo-frequency for dvs mode */
	48	#define FREQ_DVS 132333
	49
	50	/* frequency to sleep and reboot in
	51	* it's essential to leave dvs, as some boards do not reconfigure the
	52	* regulator on reboot
	53	*/
	54	#define FREQ_SLEEP 133333
	55
	56	/* Sources for the ARMCLK */
	57	#define SOURCE_HCLK 0
	58	#define SOURCE_ARMDIV 1
	59
	60	#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	61	/* S3C2416 only supports changing the voltage in the dvs-mode.
	62	* Voltages down to 1.0V seem to work, so we take what the regulator
	63	* can get us.
	64	*/
	65	static struct s3c2416_dvfs s3c2416_dvfs_table[] = {
	66	[SOURCE_HCLK] = { 950000, 1250000 },
	67	[SOURCE_ARMDIV] = { 1250000, 1350000 },
	68	};
	69	#endif
	70
	71	static struct cpufreq_frequency_table s3c2416_freq_table[] = {
7f4b0461 VK	72	{ 0, SOURCE_HCLK, FREQ_DVS },
	73	{ 0, SOURCE_ARMDIV, 133333 },
	74	{ 0, SOURCE_ARMDIV, 266666 },
	75	{ 0, SOURCE_ARMDIV, 400000 },
	76	{ 0, 0, CPUFREQ_TABLE_END },
34ee5507 HS	77	};
	78
	79	static struct cpufreq_frequency_table s3c2450_freq_table[] = {
7f4b0461 VK	80	{ 0, SOURCE_HCLK, FREQ_DVS },
	81	{ 0, SOURCE_ARMDIV, 133500 },
	82	{ 0, SOURCE_ARMDIV, 267000 },
	83	{ 0, SOURCE_ARMDIV, 534000 },
	84	{ 0, 0, CPUFREQ_TABLE_END },
34ee5507 HS	85	};
34ee5507 HS	86
34ee5507 HS	87	static unsigned int s3c2416_cpufreq_get_speed(unsigned int cpu)
	88	{
	89	struct s3c2416_data *s3c_freq = &s3c2416_cpufreq;
	90
	91	if (cpu != 0)
	92	return 0;
	93
	94	/* return our pseudo-frequency when in dvs mode */
	95	if (s3c_freq->is_dvs)
	96	return FREQ_DVS;
	97
	98	return clk_get_rate(s3c_freq->armclk) / 1000;
	99	}
	100
	101	static int s3c2416_cpufreq_set_armdiv(struct s3c2416_data *s3c_freq,
	102	unsigned int freq)
	103	{
	104	int ret;
	105
	106	if (clk_get_rate(s3c_freq->armdiv) / 1000 != freq) {
	107	ret = clk_set_rate(s3c_freq->armdiv, freq * 1000);
	108	if (ret < 0) {
	109	pr_err("cpufreq: Failed to set armdiv rate %dkHz: %d\n",
	110	freq, ret);
	111	return ret;
	112	}
	113	}
	114
	115	return 0;
	116	}
	117
	118	static int s3c2416_cpufreq_enter_dvs(struct s3c2416_data *s3c_freq, int idx)
	119	{
	120	#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	121	struct s3c2416_dvfs *dvfs;
	122	#endif
	123	int ret;
	124
	125	if (s3c_freq->is_dvs) {
	126	pr_debug("cpufreq: already in dvs mode, nothing to do\n");
	127	return 0;
	128	}
	129
	130	pr_debug("cpufreq: switching armclk to hclk (%lukHz)\n",
	131	clk_get_rate(s3c_freq->hclk) / 1000);
	132	ret = clk_set_parent(s3c_freq->armclk, s3c_freq->hclk);
	133	if (ret < 0) {
	134	pr_err("cpufreq: Failed to switch armclk to hclk: %d\n", ret);
	135	return ret;
	136	}
	137
	138	#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	139	/* changing the core voltage is only allowed when in dvs mode */
	140	if (s3c_freq->vddarm) {
	141	dvfs = &s3c2416_dvfs_table[idx];
	142
c03c3013	143	pr_debug("cpufreq: setting regulator to %d-%d\n",
34ee5507 HS	144	dvfs->vddarm_min, dvfs->vddarm_max);
	145	ret = regulator_set_voltage(s3c_freq->vddarm,
	146	dvfs->vddarm_min,
	147	dvfs->vddarm_max);
	148
	149	/* when lowering the voltage failed, there is nothing to do */
	150	if (ret != 0)
	151	pr_err("cpufreq: Failed to set VDDARM: %d\n", ret);
	152	}
	153	#endif
	154
	155	s3c_freq->is_dvs = 1;
	156
	157	return 0;
	158	}
	159
	160	static int s3c2416_cpufreq_leave_dvs(struct s3c2416_data *s3c_freq, int idx)
	161	{
	162	#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	163	struct s3c2416_dvfs *dvfs;
	164	#endif
	165	int ret;
	166
	167	if (!s3c_freq->is_dvs) {
	168	pr_debug("cpufreq: not in dvs mode, so can't leave\n");
	169	return 0;
	170	}
	171
	172	#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	173	if (s3c_freq->vddarm) {
	174	dvfs = &s3c2416_dvfs_table[idx];
	175
c03c3013	176	pr_debug("cpufreq: setting regulator to %d-%d\n",
34ee5507 HS	177	dvfs->vddarm_min, dvfs->vddarm_max);
	178	ret = regulator_set_voltage(s3c_freq->vddarm,
	179	dvfs->vddarm_min,
	180	dvfs->vddarm_max);
	181	if (ret != 0) {
	182	pr_err("cpufreq: Failed to set VDDARM: %d\n", ret);
	183	return ret;
	184	}
	185	}
	186	#endif
	187
	188	/* force armdiv to hclk frequency for transition from dvs*/
	189	if (clk_get_rate(s3c_freq->armdiv) > clk_get_rate(s3c_freq->hclk)) {
	190	pr_debug("cpufreq: force armdiv to hclk frequency (%lukHz)\n",
	191	clk_get_rate(s3c_freq->hclk) / 1000);
	192	ret = s3c2416_cpufreq_set_armdiv(s3c_freq,
	193	clk_get_rate(s3c_freq->hclk) / 1000);
	194	if (ret < 0) {
278cee05	195	pr_err("cpufreq: Failed to set the armdiv to %lukHz: %d\n",
34ee5507 HS	196	clk_get_rate(s3c_freq->hclk) / 1000, ret);
	197	return ret;
	198	}
	199	}
	200
	201	pr_debug("cpufreq: switching armclk parent to armdiv (%lukHz)\n",
	202	clk_get_rate(s3c_freq->armdiv) / 1000);
	203
	204	ret = clk_set_parent(s3c_freq->armclk, s3c_freq->armdiv);
	205	if (ret < 0) {
	206	pr_err("cpufreq: Failed to switch armclk clock parent to armdiv: %d\n",
	207	ret);
	208	return ret;
	209	}
	210
	211	s3c_freq->is_dvs = 0;
	212
	213	return 0;
	214	}
	215
	216	static int s3c2416_cpufreq_set_target(struct cpufreq_policy *policy,
9c0ebcf7	217	unsigned int index)
34ee5507 HS	218	{
34ee5507 HS	219	struct s3c2416_data *s3c_freq = &s3c2416_cpufreq;
d4019f0a	220	unsigned int new_freq;
34ee5507	221	int idx, ret, to_dvs = 0;
34ee5507 HS	222
	223	mutex_lock(&cpufreq_lock);
	224
9c0ebcf7	225	idx = s3c_freq->freq_table[index].driver_data;
34ee5507 HS	226
	227	if (idx == SOURCE_HCLK)
	228	to_dvs = 1;
	229
	230	/* switching to dvs when it's not allowed */
	231	if (to_dvs && s3c_freq->disable_dvs) {
	232	pr_debug("cpufreq: entering dvs mode not allowed\n");
	233	ret = -EINVAL;
	234	goto out;
	235	}
	236
34ee5507 HS	237	/* When leavin dvs mode, always switch the armdiv to the hclk rate
	238	* The S3C2416 has stability issues when switching directly to
	239	* higher frequencies.
	240	*/
d4019f0a	241	new_freq = (s3c_freq->is_dvs && !to_dvs)
34ee5507	242	? clk_get_rate(s3c_freq->hclk) / 1000
9c0ebcf7	243	: s3c_freq->freq_table[index].frequency;
34ee5507	244
34ee5507 HS	245	if (to_dvs) {
	246	pr_debug("cpufreq: enter dvs\n");
	247	ret = s3c2416_cpufreq_enter_dvs(s3c_freq, idx);
	248	} else if (s3c_freq->is_dvs) {
	249	pr_debug("cpufreq: leave dvs\n");
	250	ret = s3c2416_cpufreq_leave_dvs(s3c_freq, idx);
	251	} else {
d4019f0a VK	252	pr_debug("cpufreq: change armdiv to %dkHz\n", new_freq);
d4019f0a VK	253	ret = s3c2416_cpufreq_set_armdiv(s3c_freq, new_freq);
34ee5507 HS	254	}
34ee5507 HS	255
34ee5507 HS	256	out:
	257	mutex_unlock(&cpufreq_lock);
	258
	259	return ret;
	260	}
	261
	262	#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
61882b63	263	static void s3c2416_cpufreq_cfg_regulator(struct s3c2416_data *s3c_freq)
34ee5507 HS	264	{
34ee5507 HS	265	int count, v, i, found;
041526f9	266	struct cpufreq_frequency_table *pos;
34ee5507 HS	267	struct s3c2416_dvfs *dvfs;
	268
	269	count = regulator_count_voltages(s3c_freq->vddarm);
	270	if (count < 0) {
	271	pr_err("cpufreq: Unable to check supported voltages\n");
	272	return;
	273	}
	274
041526f9 SK	275	if (!count)
041526f9 SK	276	goto out;
34ee5507	277
041526f9 SK	278	cpufreq_for_each_valid_entry(pos, s3c_freq->freq_table) {
041526f9 SK	279	dvfs = &s3c2416_dvfs_table[pos->driver_data];
34ee5507 HS	280	found = 0;
	281
	282	/* Check only the min-voltage, more is always ok on S3C2416 */
	283	for (i = 0; i < count; i++) {
	284	v = regulator_list_voltage(s3c_freq->vddarm, i);
	285	if (v >= dvfs->vddarm_min)
	286	found = 1;
	287	}
	288
	289	if (!found) {
	290	pr_debug("cpufreq: %dkHz unsupported by regulator\n",
041526f9 SK	291	pos->frequency);
041526f9 SK	292	pos->frequency = CPUFREQ_ENTRY_INVALID;
34ee5507	293	}
34ee5507 HS	294	}
34ee5507 HS	295
041526f9	296	out:
34ee5507 HS	297	/* Guessed */
	298	s3c_freq->regulator_latency = 1 * 1000 * 1000;
	299	}
	300	#endif
	301
	302	static int s3c2416_cpufreq_reboot_notifier_evt(struct notifier_block *this,
	303	unsigned long event, void *ptr)
	304	{
	305	struct s3c2416_data *s3c_freq = &s3c2416_cpufreq;
	306	int ret;
8ead819f	307	struct cpufreq_policy *policy;
34ee5507 HS	308
	309	mutex_lock(&cpufreq_lock);
	310
	311	/* disable further changes */
	312	s3c_freq->disable_dvs = 1;
	313
	314	mutex_unlock(&cpufreq_lock);
	315
	316	/* some boards don't reconfigure the regulator on reboot, which
	317	* could lead to undervolting the cpu when the clock is reset.
	318	* Therefore we always leave the DVS mode on reboot.
	319	*/
	320	if (s3c_freq->is_dvs) {
	321	pr_debug("cpufreq: leave dvs on reboot\n");
8ead819f	322
	323	policy = cpufreq_cpu_get(0);
	324	if (!policy) {
	325	pr_debug("cpufreq: get no policy for cpu0\n");
	326	return NOTIFY_BAD;
	327	}
	328
	329	ret = cpufreq_driver_target(policy, FREQ_SLEEP, 0);
	330	cpufreq_cpu_put(policy);
	331
34ee5507 HS	332	if (ret < 0)
	333	return NOTIFY_BAD;
	334	}
	335
	336	return NOTIFY_DONE;
	337	}
	338
	339	static struct notifier_block s3c2416_cpufreq_reboot_notifier = {
	340	.notifier_call = s3c2416_cpufreq_reboot_notifier_evt,
	341	};
	342
61882b63	343	static int s3c2416_cpufreq_driver_init(struct cpufreq_policy *policy)
34ee5507 HS	344	{
34ee5507 HS	345	struct s3c2416_data *s3c_freq = &s3c2416_cpufreq;
041526f9	346	struct cpufreq_frequency_table *pos;
34ee5507 HS	347	struct clk *msysclk;
	348	unsigned long rate;
	349	int ret;
	350
	351	if (policy->cpu != 0)
	352	return -EINVAL;
	353
	354	msysclk = clk_get(NULL, "msysclk");
	355	if (IS_ERR(msysclk)) {
	356	ret = PTR_ERR(msysclk);
	357	pr_err("cpufreq: Unable to obtain msysclk: %d\n", ret);
	358	return ret;
	359	}
	360
	361	/*
	362	* S3C2416 and S3C2450 share the same processor-ID and also provide no
	363	* other means to distinguish them other than through the rate of
	364	* msysclk. On S3C2416 msysclk runs at 800MHz and on S3C2450 at 533MHz.
	365	*/
	366	rate = clk_get_rate(msysclk);
	367	if (rate == 800 * 1000 * 1000) {
	368	pr_info("cpufreq: msysclk running at %lukHz, using S3C2416 frequency table\n",
	369	rate / 1000);
	370	s3c_freq->freq_table = s3c2416_freq_table;
	371	policy->cpuinfo.max_freq = 400000;
	372	} else if (rate / 1000 == 534000) {
	373	pr_info("cpufreq: msysclk running at %lukHz, using S3C2450 frequency table\n",
	374	rate / 1000);
	375	s3c_freq->freq_table = s3c2450_freq_table;
	376	policy->cpuinfo.max_freq = 534000;
	377	}
	378
	379	/* not needed anymore */
	380	clk_put(msysclk);
	381
	382	if (s3c_freq->freq_table == NULL) {
	383	pr_err("cpufreq: No frequency information for this CPU, msysclk at %lukHz\n",
	384	rate / 1000);
	385	return -ENODEV;
	386	}
	387
	388	s3c_freq->is_dvs = 0;
	389
	390	s3c_freq->armdiv = clk_get(NULL, "armdiv");
	391	if (IS_ERR(s3c_freq->armdiv)) {
	392	ret = PTR_ERR(s3c_freq->armdiv);
	393	pr_err("cpufreq: Unable to obtain ARMDIV: %d\n", ret);
	394	return ret;
	395	}
	396
	397	s3c_freq->hclk = clk_get(NULL, "hclk");
	398	if (IS_ERR(s3c_freq->hclk)) {
	399	ret = PTR_ERR(s3c_freq->hclk);
	400	pr_err("cpufreq: Unable to obtain HCLK: %d\n", ret);
	401	goto err_hclk;
	402	}
	403
	404	/* chech hclk rate, we only support the common 133MHz for now
	405	* hclk could also run at 66MHz, but this not often used
	406	*/
	407	rate = clk_get_rate(s3c_freq->hclk);
	408	if (rate < 133 * 1000 * 1000) {
	409	pr_err("cpufreq: HCLK not at 133MHz\n");
34ee5507 HS	410	ret = -EINVAL;
	411	goto err_armclk;
	412	}
	413
	414	s3c_freq->armclk = clk_get(NULL, "armclk");
	415	if (IS_ERR(s3c_freq->armclk)) {
	416	ret = PTR_ERR(s3c_freq->armclk);
	417	pr_err("cpufreq: Unable to obtain ARMCLK: %d\n", ret);
	418	goto err_armclk;
	419	}
	420
	421	#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
	422	s3c_freq->vddarm = regulator_get(NULL, "vddarm");
	423	if (IS_ERR(s3c_freq->vddarm)) {
	424	ret = PTR_ERR(s3c_freq->vddarm);
	425	pr_err("cpufreq: Failed to obtain VDDARM: %d\n", ret);
	426	goto err_vddarm;
	427	}
	428
	429	s3c2416_cpufreq_cfg_regulator(s3c_freq);
	430	#else
	431	s3c_freq->regulator_latency = 0;
	432	#endif
	433
041526f9	434	cpufreq_for_each_entry(pos, s3c_freq->freq_table) {
34ee5507	435	/* special handling for dvs mode */
041526f9	436	if (pos->driver_data == 0) {
34ee5507 HS	437	if (!s3c_freq->hclk) {
34ee5507 HS	438	pr_debug("cpufreq: %dkHz unsupported as it would need unavailable dvs mode\n",
041526f9 SK	439	pos->frequency);
041526f9 SK	440	pos->frequency = CPUFREQ_ENTRY_INVALID;
34ee5507	441	} else {
34ee5507 HS	442	continue;
	443	}
	444	}
	445
	446	/* Check for frequencies we can generate */
	447	rate = clk_round_rate(s3c_freq->armdiv,
041526f9	448	pos->frequency * 1000);
34ee5507	449	rate /= 1000;
041526f9	450	if (rate != pos->frequency) {
34ee5507	451	pr_debug("cpufreq: %dkHz unsupported by clock (clk_round_rate return %lu)\n",
041526f9 SK	452	pos->frequency, rate);
041526f9 SK	453	pos->frequency = CPUFREQ_ENTRY_INVALID;
34ee5507	454	}
34ee5507 HS	455	}
34ee5507 HS	456
34ee5507 HS	457	/* Datasheet says PLL stabalisation time must be at least 300us,
	458	* so but add some fudge. (reference in LOCKCON0 register description)
	459	*/
c4dcc8a1	460	cpufreq_generic_init(policy, s3c_freq->freq_table,
a307a1e6	461	(500 * 1000) + s3c_freq->regulator_latency);
34ee5507 HS	462	register_reboot_notifier(&s3c2416_cpufreq_reboot_notifier);
	463
	464	return 0;
	465
34ee5507	466	#ifdef CONFIG_ARM_S3C2416_CPUFREQ_VCORESCALE
34ee5507	467	err_vddarm:
34ee5507	468	clk_put(s3c_freq->armclk);
c4dcc8a1	469	#endif
34ee5507 HS	470	err_armclk:
	471	clk_put(s3c_freq->hclk);
	472	err_hclk:
	473	clk_put(s3c_freq->armdiv);
	474
	475	return ret;
	476	}
	477
34ee5507	478	static struct cpufreq_driver s3c2416_cpufreq_driver = {
ae6b4271	479	.flags = CPUFREQ_NEED_INITIAL_FREQ_CHECK,
e96a4105	480	.verify = cpufreq_generic_frequency_table_verify,
9c0ebcf7	481	.target_index = s3c2416_cpufreq_set_target,
34ee5507 HS	482	.get = s3c2416_cpufreq_get_speed,
	483	.init = s3c2416_cpufreq_driver_init,
	484	.name = "s3c2416",
e96a4105	485	.attr = cpufreq_generic_attr,
34ee5507 HS	486	};
	487
	488	static int __init s3c2416_cpufreq_init(void)
	489	{
	490	return cpufreq_register_driver(&s3c2416_cpufreq_driver);
	491	}
	492	module_init(s3c2416_cpufreq_init);