[mirror_ubuntu-artful-kernel.git] / drivers / cpufreq / exynos5440-cpufreq.c

/*
 * Copyright (c) 2013 Samsung Electronics Co., Ltd.
 *		http://www.samsung.com
 *
 * Amit Daniel Kachhap <amit.daniel@samsung.com>
 *
 * EXYNOS5440 - CPU frequency scaling support
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
*/

#include <linux/clk.h>
#include <linux/cpu.h>
#include <linux/cpufreq.h>
#include <linux/err.h>
#include <linux/interrupt.h>
#include <linux/io.h>
#include <linux/module.h>
#include <linux/of_address.h>
#include <linux/of_irq.h>
#include <linux/pm_opp.h>
#include <linux/platform_device.h>
#include <linux/slab.h>

/* Register definitions */
#define XMU_DVFS_CTRL		0x0060
#define XMU_PMU_P0_7		0x0064
#define XMU_C0_3_PSTATE		0x0090
#define XMU_P_LIMIT		0x00a0
#define XMU_P_STATUS		0x00a4
#define XMU_PMUEVTEN		0x00d0
#define XMU_PMUIRQEN		0x00d4
#define XMU_PMUIRQ		0x00d8

/* PMU mask and shift definations */
#define P_VALUE_MASK		0x7

#define XMU_DVFS_CTRL_EN_SHIFT	0

#define P0_7_CPUCLKDEV_SHIFT	21
#define P0_7_CPUCLKDEV_MASK	0x7
#define P0_7_ATBCLKDEV_SHIFT	18
#define P0_7_ATBCLKDEV_MASK	0x7
#define P0_7_CSCLKDEV_SHIFT	15
#define P0_7_CSCLKDEV_MASK	0x7
#define P0_7_CPUEMA_SHIFT	28
#define P0_7_CPUEMA_MASK	0xf
#define P0_7_L2EMA_SHIFT	24
#define P0_7_L2EMA_MASK		0xf
#define P0_7_VDD_SHIFT		8
#define P0_7_VDD_MASK		0x7f
#define P0_7_FREQ_SHIFT		0
#define P0_7_FREQ_MASK		0xff

#define C0_3_PSTATE_VALID_SHIFT	8
#define C0_3_PSTATE_CURR_SHIFT	4
#define C0_3_PSTATE_NEW_SHIFT	0

#define PSTATE_CHANGED_EVTEN_SHIFT	0

#define PSTATE_CHANGED_IRQEN_SHIFT	0

#define PSTATE_CHANGED_SHIFT		0

/* some constant values for clock divider calculation */
#define CPU_DIV_FREQ_MAX	500
#define CPU_DBG_FREQ_MAX	375
#define CPU_ATB_FREQ_MAX	500

#define PMIC_LOW_VOLT		0x30
#define PMIC_HIGH_VOLT		0x28

#define CPUEMA_HIGH		0x2
#define CPUEMA_MID		0x4
#define CPUEMA_LOW		0x7

#define L2EMA_HIGH		0x1
#define L2EMA_MID		0x3
#define L2EMA_LOW		0x4

#define DIV_TAB_MAX	2
/* frequency unit is 20MHZ */
#define FREQ_UNIT	20
#define MAX_VOLTAGE	1550000 /* In microvolt */
#define VOLTAGE_STEP	12500	/* In microvolt */

#define CPUFREQ_NAME		"exynos5440_dvfs"
#define DEF_TRANS_LATENCY	100000

enum cpufreq_level_index {
	L0, L1, L2, L3, L4,
	L5, L6, L7, L8, L9,
};
#define CPUFREQ_LEVEL_END	(L7 + 1)

struct exynos_dvfs_data {
	void __iomem *base;
	struct resource *mem;
	int irq;
	struct clk *cpu_clk;
	unsigned int latency;
	struct cpufreq_frequency_table *freq_table;
	unsigned int freq_count;
	struct device *dev;
	bool dvfs_enabled;
	struct work_struct irq_work;
};

static struct exynos_dvfs_data *dvfs_info;
static DEFINE_MUTEX(cpufreq_lock);
static struct cpufreq_freqs freqs;

static int init_div_table(void)
{
	struct cpufreq_frequency_table *pos, *freq_tbl = dvfs_info->freq_table;
	unsigned int tmp, clk_div, ema_div, freq, volt_id;
	struct dev_pm_opp *opp;

	cpufreq_for_each_entry(pos, freq_tbl) {
		opp = dev_pm_opp_find_freq_exact(dvfs_info->dev,
					pos->frequency * 1000, true);
		if (IS_ERR(opp)) {
			dev_err(dvfs_info->dev,
				"failed to find valid OPP for %u KHZ\n",
				pos->frequency);
			return PTR_ERR(opp);
		}

		freq = pos->frequency / 1000; /* In MHZ */
		clk_div = ((freq / CPU_DIV_FREQ_MAX) & P0_7_CPUCLKDEV_MASK)
					<< P0_7_CPUCLKDEV_SHIFT;
		clk_div |= ((freq / CPU_ATB_FREQ_MAX) & P0_7_ATBCLKDEV_MASK)
					<< P0_7_ATBCLKDEV_SHIFT;
		clk_div |= ((freq / CPU_DBG_FREQ_MAX) & P0_7_CSCLKDEV_MASK)
					<< P0_7_CSCLKDEV_SHIFT;

		/* Calculate EMA */
		volt_id = dev_pm_opp_get_voltage(opp);

		volt_id = (MAX_VOLTAGE - volt_id) / VOLTAGE_STEP;
		if (volt_id < PMIC_HIGH_VOLT) {
			ema_div = (CPUEMA_HIGH << P0_7_CPUEMA_SHIFT) |
				(L2EMA_HIGH << P0_7_L2EMA_SHIFT);
		} else if (volt_id > PMIC_LOW_VOLT) {
			ema_div = (CPUEMA_LOW << P0_7_CPUEMA_SHIFT) |
				(L2EMA_LOW << P0_7_L2EMA_SHIFT);
		} else {
			ema_div = (CPUEMA_MID << P0_7_CPUEMA_SHIFT) |
				(L2EMA_MID << P0_7_L2EMA_SHIFT);
		}

		tmp = (clk_div | ema_div | (volt_id << P0_7_VDD_SHIFT)
			| ((freq / FREQ_UNIT) << P0_7_FREQ_SHIFT));

		__raw_writel(tmp, dvfs_info->base + XMU_PMU_P0_7 + 4 *
						(pos - freq_tbl));
		dev_pm_opp_put(opp);
	}

	return 0;
}

static void exynos_enable_dvfs(unsigned int cur_frequency)
{
	unsigned int tmp, cpu;
	struct cpufreq_frequency_table *freq_table = dvfs_info->freq_table;
	struct cpufreq_frequency_table *pos;
	/* Disable DVFS */
	__raw_writel(0,	dvfs_info->base + XMU_DVFS_CTRL);

	/* Enable PSTATE Change Event */
	tmp = __raw_readl(dvfs_info->base + XMU_PMUEVTEN);
	tmp |= (1 << PSTATE_CHANGED_EVTEN_SHIFT);
	__raw_writel(tmp, dvfs_info->base + XMU_PMUEVTEN);

	/* Enable PSTATE Change IRQ */
	tmp = __raw_readl(dvfs_info->base + XMU_PMUIRQEN);
	tmp |= (1 << PSTATE_CHANGED_IRQEN_SHIFT);
	__raw_writel(tmp, dvfs_info->base + XMU_PMUIRQEN);

	/* Set initial performance index */
	cpufreq_for_each_entry(pos, freq_table)
		if (pos->frequency == cur_frequency)
			break;

	if (pos->frequency == CPUFREQ_TABLE_END) {
		dev_crit(dvfs_info->dev, "Boot up frequency not supported\n");
		/* Assign the highest frequency */
		pos = freq_table;
		cur_frequency = pos->frequency;
	}

	dev_info(dvfs_info->dev, "Setting dvfs initial frequency = %uKHZ",
						cur_frequency);

	for (cpu = 0; cpu < CONFIG_NR_CPUS; cpu++) {
		tmp = __raw_readl(dvfs_info->base + XMU_C0_3_PSTATE + cpu * 4);
		tmp &= ~(P_VALUE_MASK << C0_3_PSTATE_NEW_SHIFT);
		tmp |= ((pos - freq_table) << C0_3_PSTATE_NEW_SHIFT);
		__raw_writel(tmp, dvfs_info->base + XMU_C0_3_PSTATE + cpu * 4);
	}

	/* Enable DVFS */
	__raw_writel(1 << XMU_DVFS_CTRL_EN_SHIFT,
				dvfs_info->base + XMU_DVFS_CTRL);
}

static int exynos_target(struct cpufreq_policy *policy, unsigned int index)
{
	unsigned int tmp;
	int i;
	struct cpufreq_frequency_table *freq_table = dvfs_info->freq_table;

	mutex_lock(&cpufreq_lock);

	freqs.old = policy->cur;
	freqs.new = freq_table[index].frequency;

	cpufreq_freq_transition_begin(policy, &freqs);

	/* Set the target frequency in all C0_3_PSTATE register */
	for_each_cpu(i, policy->cpus) {
		tmp = __raw_readl(dvfs_info->base + XMU_C0_3_PSTATE + i * 4);
		tmp &= ~(P_VALUE_MASK << C0_3_PSTATE_NEW_SHIFT);
		tmp |= (index << C0_3_PSTATE_NEW_SHIFT);

		__raw_writel(tmp, dvfs_info->base + XMU_C0_3_PSTATE + i * 4);
	}
	mutex_unlock(&cpufreq_lock);
	return 0;
}

static void exynos_cpufreq_work(struct work_struct *work)
{
	unsigned int cur_pstate, index;
	struct cpufreq_policy *policy = cpufreq_cpu_get(0); /* boot CPU */
	struct cpufreq_frequency_table *freq_table = dvfs_info->freq_table;

	/* Ensure we can access cpufreq structures */
	if (unlikely(dvfs_info->dvfs_enabled == false))
		goto skip_work;

	mutex_lock(&cpufreq_lock);
	freqs.old = policy->cur;

	cur_pstate = __raw_readl(dvfs_info->base + XMU_P_STATUS);
	if (cur_pstate >> C0_3_PSTATE_VALID_SHIFT & 0x1)
		index = (cur_pstate >> C0_3_PSTATE_CURR_SHIFT) & P_VALUE_MASK;
	else
		index = (cur_pstate >> C0_3_PSTATE_NEW_SHIFT) & P_VALUE_MASK;

	if (likely(index < dvfs_info->freq_count)) {
		freqs.new = freq_table[index].frequency;
	} else {
		dev_crit(dvfs_info->dev, "New frequency out of range\n");
		freqs.new = freqs.old;
	}
	cpufreq_freq_transition_end(policy, &freqs, 0);

	cpufreq_cpu_put(policy);
	mutex_unlock(&cpufreq_lock);
skip_work:
	enable_irq(dvfs_info->irq);
}

static irqreturn_t exynos_cpufreq_irq(int irq, void *id)
{
	unsigned int tmp;

	tmp = __raw_readl(dvfs_info->base + XMU_PMUIRQ);
	if (tmp >> PSTATE_CHANGED_SHIFT & 0x1) {
		__raw_writel(tmp, dvfs_info->base + XMU_PMUIRQ);
		disable_irq_nosync(irq);
		schedule_work(&dvfs_info->irq_work);
	}
	return IRQ_HANDLED;
}

static void exynos_sort_descend_freq_table(void)
{
	struct cpufreq_frequency_table *freq_tbl = dvfs_info->freq_table;
	int i = 0, index;
	unsigned int tmp_freq;
	/*
	 * Exynos5440 clock controller state logic expects the cpufreq table to
	 * be in descending order. But the OPP library constructs the table in
	 * ascending order. So to make the table descending we just need to
	 * swap the i element with the N - i element.
	 */
	for (i = 0; i < dvfs_info->freq_count / 2; i++) {
		index = dvfs_info->freq_count - i - 1;
		tmp_freq = freq_tbl[i].frequency;
		freq_tbl[i].frequency = freq_tbl[index].frequency;
		freq_tbl[index].frequency = tmp_freq;
	}
}

static int exynos_cpufreq_cpu_init(struct cpufreq_policy *policy)
{
	policy->clk = dvfs_info->cpu_clk;
	return cpufreq_generic_init(policy, dvfs_info->freq_table,
			dvfs_info->latency);
}

static struct cpufreq_driver exynos_driver = {
	.flags		= CPUFREQ_STICKY | CPUFREQ_ASYNC_NOTIFICATION |
				CPUFREQ_NEED_INITIAL_FREQ_CHECK,
	.verify		= cpufreq_generic_frequency_table_verify,
	.target_index	= exynos_target,
	.get		= cpufreq_generic_get,
	.init		= exynos_cpufreq_cpu_init,
	.name		= CPUFREQ_NAME,
	.attr		= cpufreq_generic_attr,
};

static const struct of_device_id exynos_cpufreq_match[] = {
	{
		.compatible = "samsung,exynos5440-cpufreq",
	},
	{},
};
MODULE_DEVICE_TABLE(of, exynos_cpufreq_match);

static int exynos_cpufreq_probe(struct platform_device *pdev)
{
	int ret = -EINVAL;
	struct device_node *np;
	struct resource res;
	unsigned int cur_frequency;

	np = pdev->dev.of_node;
	if (!np)
		return -ENODEV;

	dvfs_info = devm_kzalloc(&pdev->dev, sizeof(*dvfs_info), GFP_KERNEL);
	if (!dvfs_info) {
		ret = -ENOMEM;
		goto err_put_node;
	}

	dvfs_info->dev = &pdev->dev;

	ret = of_address_to_resource(np, 0, &res);
	if (ret)
		goto err_put_node;

	dvfs_info->base = devm_ioremap_resource(dvfs_info->dev, &res);
	if (IS_ERR(dvfs_info->base)) {
		ret = PTR_ERR(dvfs_info->base);
		goto err_put_node;
	}

	dvfs_info->irq = irq_of_parse_and_map(np, 0);
	if (!dvfs_info->irq) {
		dev_err(dvfs_info->dev, "No cpufreq irq found\n");
		ret = -ENODEV;
		goto err_put_node;
	}

	ret = dev_pm_opp_of_add_table(dvfs_info->dev);
	if (ret) {
		dev_err(dvfs_info->dev, "failed to init OPP table: %d\n", ret);
		goto err_put_node;
	}

	ret = dev_pm_opp_init_cpufreq_table(dvfs_info->dev,
					    &dvfs_info->freq_table);
	if (ret) {
		dev_err(dvfs_info->dev,
			"failed to init cpufreq table: %d\n", ret);
		goto err_free_opp;
	}
	dvfs_info->freq_count = dev_pm_opp_get_opp_count(dvfs_info->dev);
	exynos_sort_descend_freq_table();

	if (of_property_read_u32(np, "clock-latency", &dvfs_info->latency))
		dvfs_info->latency = DEF_TRANS_LATENCY;

	dvfs_info->cpu_clk = devm_clk_get(dvfs_info->dev, "armclk");
	if (IS_ERR(dvfs_info->cpu_clk)) {
		dev_err(dvfs_info->dev, "Failed to get cpu clock\n");
		ret = PTR_ERR(dvfs_info->cpu_clk);
		goto err_free_table;
	}

	cur_frequency = clk_get_rate(dvfs_info->cpu_clk);
	if (!cur_frequency) {
		dev_err(dvfs_info->dev, "Failed to get clock rate\n");
		ret = -EINVAL;
		goto err_free_table;
	}
	cur_frequency /= 1000;

	INIT_WORK(&dvfs_info->irq_work, exynos_cpufreq_work);
	ret = devm_request_irq(dvfs_info->dev, dvfs_info->irq,
				exynos_cpufreq_irq, IRQF_TRIGGER_NONE,
				CPUFREQ_NAME, dvfs_info);
	if (ret) {
		dev_err(dvfs_info->dev, "Failed to register IRQ\n");
		goto err_free_table;
	}

	ret = init_div_table();
	if (ret) {
		dev_err(dvfs_info->dev, "Failed to initialise div table\n");
		goto err_free_table;
	}

	exynos_enable_dvfs(cur_frequency);
	ret = cpufreq_register_driver(&exynos_driver);
	if (ret) {
		dev_err(dvfs_info->dev,
			"%s: failed to register cpufreq driver\n", __func__);
		goto err_free_table;
	}

	of_node_put(np);
	dvfs_info->dvfs_enabled = true;
	return 0;

err_free_table:
	dev_pm_opp_free_cpufreq_table(dvfs_info->dev, &dvfs_info->freq_table);
err_free_opp:
	dev_pm_opp_of_remove_table(dvfs_info->dev);
err_put_node:
	of_node_put(np);
	dev_err(&pdev->dev, "%s: failed initialization\n", __func__);
	return ret;
}

static int exynos_cpufreq_remove(struct platform_device *pdev)
{
	cpufreq_unregister_driver(&exynos_driver);
	dev_pm_opp_free_cpufreq_table(dvfs_info->dev, &dvfs_info->freq_table);
	dev_pm_opp_of_remove_table(dvfs_info->dev);
	return 0;
}

static struct platform_driver exynos_cpufreq_platdrv = {
	.driver = {
		.name	= "exynos5440-cpufreq",
		.of_match_table = exynos_cpufreq_match,
	},
	.probe		= exynos_cpufreq_probe,
	.remove		= exynos_cpufreq_remove,
};
module_platform_driver(exynos_cpufreq_platdrv);

MODULE_AUTHOR("Amit Daniel Kachhap <amit.daniel@samsung.com>");
MODULE_DESCRIPTION("Exynos5440 cpufreq driver");
MODULE_LICENSE("GPL");
Commit	Line	Data
49d7b5bf ADK	1	/*
	2	* Copyright (c) 2013 Samsung Electronics Co., Ltd.
	3	* http://www.samsung.com
	4	*
	5	* Amit Daniel Kachhap <amit.daniel@samsung.com>
	6	*
	7	* EXYNOS5440 - CPU frequency scaling support
	8	*
	9	* This program is free software; you can redistribute it and/or modify
	10	* it under the terms of the GNU General Public License version 2 as
	11	* published by the Free Software Foundation.
	12	*/
	13
	14	#include <linux/clk.h>
	15	#include <linux/cpu.h>
	16	#include <linux/cpufreq.h>
	17	#include <linux/err.h>
	18	#include <linux/interrupt.h>
	19	#include <linux/io.h>
	20	#include <linux/module.h>
	21	#include <linux/of_address.h>
	22	#include <linux/of_irq.h>
e4db1c74	23	#include <linux/pm_opp.h>
49d7b5bf ADK	24	#include <linux/platform_device.h>
	25	#include <linux/slab.h>
	26
	27	/* Register definitions */
	28	#define XMU_DVFS_CTRL 0x0060
	29	#define XMU_PMU_P0_7 0x0064
	30	#define XMU_C0_3_PSTATE 0x0090
	31	#define XMU_P_LIMIT 0x00a0
	32	#define XMU_P_STATUS 0x00a4
	33	#define XMU_PMUEVTEN 0x00d0
	34	#define XMU_PMUIRQEN 0x00d4
	35	#define XMU_PMUIRQ 0x00d8
	36
	37	/* PMU mask and shift definations */
	38	#define P_VALUE_MASK 0x7
	39
	40	#define XMU_DVFS_CTRL_EN_SHIFT 0
	41
	42	#define P0_7_CPUCLKDEV_SHIFT 21
	43	#define P0_7_CPUCLKDEV_MASK 0x7
	44	#define P0_7_ATBCLKDEV_SHIFT 18
	45	#define P0_7_ATBCLKDEV_MASK 0x7
	46	#define P0_7_CSCLKDEV_SHIFT 15
	47	#define P0_7_CSCLKDEV_MASK 0x7
	48	#define P0_7_CPUEMA_SHIFT 28
	49	#define P0_7_CPUEMA_MASK 0xf
	50	#define P0_7_L2EMA_SHIFT 24
	51	#define P0_7_L2EMA_MASK 0xf
	52	#define P0_7_VDD_SHIFT 8
	53	#define P0_7_VDD_MASK 0x7f
	54	#define P0_7_FREQ_SHIFT 0
	55	#define P0_7_FREQ_MASK 0xff
	56
	57	#define C0_3_PSTATE_VALID_SHIFT 8
	58	#define C0_3_PSTATE_CURR_SHIFT 4
	59	#define C0_3_PSTATE_NEW_SHIFT 0
	60
	61	#define PSTATE_CHANGED_EVTEN_SHIFT 0
	62
	63	#define PSTATE_CHANGED_IRQEN_SHIFT 0
	64
	65	#define PSTATE_CHANGED_SHIFT 0
	66
	67	/* some constant values for clock divider calculation */
	68	#define CPU_DIV_FREQ_MAX 500
	69	#define CPU_DBG_FREQ_MAX 375
	70	#define CPU_ATB_FREQ_MAX 500
	71
	72	#define PMIC_LOW_VOLT 0x30
	73	#define PMIC_HIGH_VOLT 0x28
	74
	75	#define CPUEMA_HIGH 0x2
	76	#define CPUEMA_MID 0x4
	77	#define CPUEMA_LOW 0x7
	78
	79	#define L2EMA_HIGH 0x1
	80	#define L2EMA_MID 0x3
	81	#define L2EMA_LOW 0x4
	82
	83	#define DIV_TAB_MAX 2
	84	/* frequency unit is 20MHZ */
	85	#define FREQ_UNIT 20
	86	#define MAX_VOLTAGE 1550000 /* In microvolt */
	87	#define VOLTAGE_STEP 12500 /* In microvolt */
88
89	#define CPUFREQ_NAME "exynos5440_dvfs"
90	#define DEF_TRANS_LATENCY 100000
91
92	enum cpufreq_level_index {
93	L0, L1, L2, L3, L4,
94	L5, L6, L7, L8, L9,
95	};
96	#define CPUFREQ_LEVEL_END (L7 + 1)
97
98	struct exynos_dvfs_data {
99	void __iomem *base;
100	struct resource *mem;
101	int irq;
102	struct clk *cpu_clk;
49d7b5bf ADK	103	unsigned int latency;
	104	struct cpufreq_frequency_table *freq_table;
	105	unsigned int freq_count;
	106	struct device *dev;
	107	bool dvfs_enabled;
	108	struct work_struct irq_work;
	109	};
	110
	111	static struct exynos_dvfs_data *dvfs_info;
	112	static DEFINE_MUTEX(cpufreq_lock);
	113	static struct cpufreq_freqs freqs;
	114
	115	static int init_div_table(void)
	116	{
041526f9	117	struct cpufreq_frequency_table pos, freq_tbl = dvfs_info->freq_table;
49d7b5bf	118	unsigned int tmp, clk_div, ema_div, freq, volt_id;
47d43ba7	119	struct dev_pm_opp *opp;
49d7b5bf	120
041526f9	121	cpufreq_for_each_entry(pos, freq_tbl) {
5d4879cd	122	opp = dev_pm_opp_find_freq_exact(dvfs_info->dev,
041526f9	123	pos->frequency * 1000, true);
49d7b5bf ADK	124	if (IS_ERR(opp)) {
	125	dev_err(dvfs_info->dev,
	126	"failed to find valid OPP for %u KHZ\n",
041526f9	127	pos->frequency);
49d7b5bf ADK	128	return PTR_ERR(opp);
	129	}
	130
041526f9	131	freq = pos->frequency / 1000; /* In MHZ */
49d7b5bf ADK	132	clk_div = ((freq / CPU_DIV_FREQ_MAX) & P0_7_CPUCLKDEV_MASK)
	133	<< P0_7_CPUCLKDEV_SHIFT;
	134	clk_div \|= ((freq / CPU_ATB_FREQ_MAX) & P0_7_ATBCLKDEV_MASK)
	135	<< P0_7_ATBCLKDEV_SHIFT;
	136	clk_div \|= ((freq / CPU_DBG_FREQ_MAX) & P0_7_CSCLKDEV_MASK)
	137	<< P0_7_CSCLKDEV_SHIFT;
	138
	139	/* Calculate EMA */
5d4879cd	140	volt_id = dev_pm_opp_get_voltage(opp);
8a31d9d9	141
49d7b5bf ADK	142	volt_id = (MAX_VOLTAGE - volt_id) / VOLTAGE_STEP;
	143	if (volt_id < PMIC_HIGH_VOLT) {
	144	ema_div = (CPUEMA_HIGH << P0_7_CPUEMA_SHIFT) \|
	145	(L2EMA_HIGH << P0_7_L2EMA_SHIFT);
	146	} else if (volt_id > PMIC_LOW_VOLT) {
	147	ema_div = (CPUEMA_LOW << P0_7_CPUEMA_SHIFT) \|
	148	(L2EMA_LOW << P0_7_L2EMA_SHIFT);
	149	} else {
	150	ema_div = (CPUEMA_MID << P0_7_CPUEMA_SHIFT) \|
	151	(L2EMA_MID << P0_7_L2EMA_SHIFT);
	152	}
	153
	154	tmp = (clk_div \| ema_div \| (volt_id << P0_7_VDD_SHIFT)
	155	\| ((freq / FREQ_UNIT) << P0_7_FREQ_SHIFT));
	156
041526f9 SK	157	__raw_writel(tmp, dvfs_info->base + XMU_PMU_P0_7 + 4 *
041526f9 SK	158	(pos - freq_tbl));
8a31d9d9	159	dev_pm_opp_put(opp);
49d7b5bf ADK	160	}
	161
	162	return 0;
	163	}
	164
652ed95d	165	static void exynos_enable_dvfs(unsigned int cur_frequency)
49d7b5bf	166	{
041526f9	167	unsigned int tmp, cpu;
49d7b5bf	168	struct cpufreq_frequency_table *freq_table = dvfs_info->freq_table;
041526f9	169	struct cpufreq_frequency_table *pos;
49d7b5bf ADK	170	/* Disable DVFS */
	171	__raw_writel(0, dvfs_info->base + XMU_DVFS_CTRL);
	172
	173	/* Enable PSTATE Change Event */
	174	tmp = __raw_readl(dvfs_info->base + XMU_PMUEVTEN);
	175	tmp \|= (1 << PSTATE_CHANGED_EVTEN_SHIFT);
e773f5c7	176	__raw_writel(tmp, dvfs_info->base + XMU_PMUEVTEN);
49d7b5bf ADK	177
	178	/* Enable PSTATE Change IRQ */
	179	tmp = __raw_readl(dvfs_info->base + XMU_PMUIRQEN);
	180	tmp \|= (1 << PSTATE_CHANGED_IRQEN_SHIFT);
e773f5c7	181	__raw_writel(tmp, dvfs_info->base + XMU_PMUIRQEN);
49d7b5bf ADK	182
49d7b5bf ADK	183	/* Set initial performance index */
041526f9 SK	184	cpufreq_for_each_entry(pos, freq_table)
041526f9 SK	185	if (pos->frequency == cur_frequency)
49d7b5bf ADK	186	break;
49d7b5bf ADK	187
041526f9	188	if (pos->frequency == CPUFREQ_TABLE_END) {
49d7b5bf ADK	189	dev_crit(dvfs_info->dev, "Boot up frequency not supported\n");
49d7b5bf ADK	190	/* Assign the highest frequency */
041526f9 SK	191	pos = freq_table;
041526f9 SK	192	cur_frequency = pos->frequency;
49d7b5bf ADK	193	}
	194
	195	dev_info(dvfs_info->dev, "Setting dvfs initial frequency = %uKHZ",
652ed95d	196	cur_frequency);
49d7b5bf ADK	197
	198	for (cpu = 0; cpu < CONFIG_NR_CPUS; cpu++) {
	199	tmp = __raw_readl(dvfs_info->base + XMU_C0_3_PSTATE + cpu * 4);
	200	tmp &= ~(P_VALUE_MASK << C0_3_PSTATE_NEW_SHIFT);
041526f9	201	tmp \|= ((pos - freq_table) << C0_3_PSTATE_NEW_SHIFT);
49d7b5bf ADK	202	__raw_writel(tmp, dvfs_info->base + XMU_C0_3_PSTATE + cpu * 4);
	203	}
	204
	205	/* Enable DVFS */
	206	__raw_writel(1 << XMU_DVFS_CTRL_EN_SHIFT,
	207	dvfs_info->base + XMU_DVFS_CTRL);
	208	}
	209
9c0ebcf7	210	static int exynos_target(struct cpufreq_policy *policy, unsigned int index)
49d7b5bf	211	{
9c0ebcf7 VK	212	unsigned int tmp;
9c0ebcf7 VK	213	int i;
49d7b5bf ADK	214	struct cpufreq_frequency_table *freq_table = dvfs_info->freq_table;
	215
	216	mutex_lock(&cpufreq_lock);
	217
652ed95d	218	freqs.old = policy->cur;
49d7b5bf ADK	219	freqs.new = freq_table[index].frequency;
49d7b5bf ADK	220
8fec051e	221	cpufreq_freq_transition_begin(policy, &freqs);
49d7b5bf ADK	222
	223	/* Set the target frequency in all C0_3_PSTATE register */
	224	for_each_cpu(i, policy->cpus) {
	225	tmp = __raw_readl(dvfs_info->base + XMU_C0_3_PSTATE + i * 4);
	226	tmp &= ~(P_VALUE_MASK << C0_3_PSTATE_NEW_SHIFT);
	227	tmp \|= (index << C0_3_PSTATE_NEW_SHIFT);
	228
	229	__raw_writel(tmp, dvfs_info->base + XMU_C0_3_PSTATE + i * 4);
	230	}
49d7b5bf	231	mutex_unlock(&cpufreq_lock);
9c0ebcf7	232	return 0;
49d7b5bf ADK	233	}
	234
	235	static void exynos_cpufreq_work(struct work_struct *work)
	236	{
	237	unsigned int cur_pstate, index;
	238	struct cpufreq_policy policy = cpufreq_cpu_get(0); / boot CPU */
	239	struct cpufreq_frequency_table *freq_table = dvfs_info->freq_table;
	240
	241	/* Ensure we can access cpufreq structures */
	242	if (unlikely(dvfs_info->dvfs_enabled == false))
	243	goto skip_work;
	244
	245	mutex_lock(&cpufreq_lock);
652ed95d	246	freqs.old = policy->cur;
49d7b5bf ADK	247
	248	cur_pstate = __raw_readl(dvfs_info->base + XMU_P_STATUS);
	249	if (cur_pstate >> C0_3_PSTATE_VALID_SHIFT & 0x1)
	250	index = (cur_pstate >> C0_3_PSTATE_CURR_SHIFT) & P_VALUE_MASK;
	251	else
	252	index = (cur_pstate >> C0_3_PSTATE_NEW_SHIFT) & P_VALUE_MASK;
	253
	254	if (likely(index < dvfs_info->freq_count)) {
	255	freqs.new = freq_table[index].frequency;
49d7b5bf ADK	256	} else {
49d7b5bf ADK	257	dev_crit(dvfs_info->dev, "New frequency out of range\n");
652ed95d	258	freqs.new = freqs.old;
49d7b5bf	259	}
8fec051e	260	cpufreq_freq_transition_end(policy, &freqs, 0);
49d7b5bf ADK	261
	262	cpufreq_cpu_put(policy);
	263	mutex_unlock(&cpufreq_lock);
	264	skip_work:
	265	enable_irq(dvfs_info->irq);
	266	}
	267
	268	static irqreturn_t exynos_cpufreq_irq(int irq, void *id)
	269	{
	270	unsigned int tmp;
	271
	272	tmp = __raw_readl(dvfs_info->base + XMU_PMUIRQ);
	273	if (tmp >> PSTATE_CHANGED_SHIFT & 0x1) {
	274	__raw_writel(tmp, dvfs_info->base + XMU_PMUIRQ);
	275	disable_irq_nosync(irq);
	276	schedule_work(&dvfs_info->irq_work);
	277	}
	278	return IRQ_HANDLED;
	279	}
	280
	281	static void exynos_sort_descend_freq_table(void)
	282	{
	283	struct cpufreq_frequency_table *freq_tbl = dvfs_info->freq_table;
	284	int i = 0, index;
	285	unsigned int tmp_freq;
	286	/*
	287	* Exynos5440 clock controller state logic expects the cpufreq table to
	288	* be in descending order. But the OPP library constructs the table in
	289	* ascending order. So to make the table descending we just need to
	290	* swap the i element with the N - i element.
	291	*/
	292	for (i = 0; i < dvfs_info->freq_count / 2; i++) {
	293	index = dvfs_info->freq_count - i - 1;
	294	tmp_freq = freq_tbl[i].frequency;
	295	freq_tbl[i].frequency = freq_tbl[index].frequency;
	296	freq_tbl[index].frequency = tmp_freq;
	297	}
	298	}
	299
	300	static int exynos_cpufreq_cpu_init(struct cpufreq_policy *policy)
	301	{
652ed95d	302	policy->clk = dvfs_info->cpu_clk;
b249abae VK	303	return cpufreq_generic_init(policy, dvfs_info->freq_table,
b249abae VK	304	dvfs_info->latency);
49d7b5bf ADK	305	}
	306
	307	static struct cpufreq_driver exynos_driver = {
ae6b4271 VK	308	.flags = CPUFREQ_STICKY \| CPUFREQ_ASYNC_NOTIFICATION \|
ae6b4271 VK	309	CPUFREQ_NEED_INITIAL_FREQ_CHECK,
eea6181e	310	.verify = cpufreq_generic_frequency_table_verify,
9c0ebcf7	311	.target_index = exynos_target,
652ed95d	312	.get = cpufreq_generic_get,
49d7b5bf ADK	313	.init = exynos_cpufreq_cpu_init,
49d7b5bf ADK	314	.name = CPUFREQ_NAME,
5d7e690d	315	.attr = cpufreq_generic_attr,
49d7b5bf ADK	316	};
	317
	318	static const struct of_device_id exynos_cpufreq_match[] = {
	319	{
	320	.compatible = "samsung,exynos5440-cpufreq",
	321	},
	322	{},
	323	};
	324	MODULE_DEVICE_TABLE(of, exynos_cpufreq_match);
	325
	326	static int exynos_cpufreq_probe(struct platform_device *pdev)
	327	{
	328	int ret = -EINVAL;
	329	struct device_node *np;
	330	struct resource res;
652ed95d	331	unsigned int cur_frequency;
49d7b5bf	332
e773f5c7	333	np = pdev->dev.of_node;
49d7b5bf ADK	334	if (!np)
	335	return -ENODEV;
	336
	337	dvfs_info = devm_kzalloc(&pdev->dev, sizeof(*dvfs_info), GFP_KERNEL);
	338	if (!dvfs_info) {
	339	ret = -ENOMEM;
	340	goto err_put_node;
	341	}
	342
	343	dvfs_info->dev = &pdev->dev;
	344
	345	ret = of_address_to_resource(np, 0, &res);
	346	if (ret)
	347	goto err_put_node;
	348
	349	dvfs_info->base = devm_ioremap_resource(dvfs_info->dev, &res);
	350	if (IS_ERR(dvfs_info->base)) {
	351	ret = PTR_ERR(dvfs_info->base);
	352	goto err_put_node;
	353	}
	354
	355	dvfs_info->irq = irq_of_parse_and_map(np, 0);
	356	if (!dvfs_info->irq) {
	357	dev_err(dvfs_info->dev, "No cpufreq irq found\n");
	358	ret = -ENODEV;
	359	goto err_put_node;
	360	}
	361
8f8d37b2	362	ret = dev_pm_opp_of_add_table(dvfs_info->dev);
49d7b5bf ADK	363	if (ret) {
	364	dev_err(dvfs_info->dev, "failed to init OPP table: %d\n", ret);
	365	goto err_put_node;
	366	}
	367
5d4879cd NM	368	ret = dev_pm_opp_init_cpufreq_table(dvfs_info->dev,
5d4879cd NM	369	&dvfs_info->freq_table);
49d7b5bf ADK	370	if (ret) {
	371	dev_err(dvfs_info->dev,
	372	"failed to init cpufreq table: %d\n", ret);
c6104fdb	373	goto err_free_opp;
49d7b5bf	374	}
5d4879cd	375	dvfs_info->freq_count = dev_pm_opp_get_opp_count(dvfs_info->dev);
49d7b5bf ADK	376	exynos_sort_descend_freq_table();
	377
	378	if (of_property_read_u32(np, "clock-latency", &dvfs_info->latency))
	379	dvfs_info->latency = DEF_TRANS_LATENCY;
	380
	381	dvfs_info->cpu_clk = devm_clk_get(dvfs_info->dev, "armclk");
	382	if (IS_ERR(dvfs_info->cpu_clk)) {
	383	dev_err(dvfs_info->dev, "Failed to get cpu clock\n");
	384	ret = PTR_ERR(dvfs_info->cpu_clk);
	385	goto err_free_table;
	386	}
	387
652ed95d VK	388	cur_frequency = clk_get_rate(dvfs_info->cpu_clk);
652ed95d VK	389	if (!cur_frequency) {
49d7b5bf ADK	390	dev_err(dvfs_info->dev, "Failed to get clock rate\n");
	391	ret = -EINVAL;
	392	goto err_free_table;
	393	}
652ed95d	394	cur_frequency /= 1000;
49d7b5bf ADK	395
	396	INIT_WORK(&dvfs_info->irq_work, exynos_cpufreq_work);
	397	ret = devm_request_irq(dvfs_info->dev, dvfs_info->irq,
	398	exynos_cpufreq_irq, IRQF_TRIGGER_NONE,
	399	CPUFREQ_NAME, dvfs_info);
	400	if (ret) {
	401	dev_err(dvfs_info->dev, "Failed to register IRQ\n");
	402	goto err_free_table;
	403	}
	404
	405	ret = init_div_table();
	406	if (ret) {
	407	dev_err(dvfs_info->dev, "Failed to initialise div table\n");
	408	goto err_free_table;
	409	}
	410
652ed95d	411	exynos_enable_dvfs(cur_frequency);
49d7b5bf ADK	412	ret = cpufreq_register_driver(&exynos_driver);
	413	if (ret) {
	414	dev_err(dvfs_info->dev,
	415	"%s: failed to register cpufreq driver\n", __func__);
	416	goto err_free_table;
	417	}
	418
	419	of_node_put(np);
	420	dvfs_info->dvfs_enabled = true;
	421	return 0;
	422
	423	err_free_table:
5d4879cd	424	dev_pm_opp_free_cpufreq_table(dvfs_info->dev, &dvfs_info->freq_table);
c6104fdb	425	err_free_opp:
8f8d37b2	426	dev_pm_opp_of_remove_table(dvfs_info->dev);
49d7b5bf ADK	427	err_put_node:
49d7b5bf ADK	428	of_node_put(np);
116decb7	429	dev_err(&pdev->dev, "%s: failed initialization\n", __func__);
49d7b5bf ADK	430	return ret;
	431	}
	432
	433	static int exynos_cpufreq_remove(struct platform_device *pdev)
	434	{
	435	cpufreq_unregister_driver(&exynos_driver);
5d4879cd	436	dev_pm_opp_free_cpufreq_table(dvfs_info->dev, &dvfs_info->freq_table);
8f8d37b2	437	dev_pm_opp_of_remove_table(dvfs_info->dev);
49d7b5bf ADK	438	return 0;
	439	}
	440
	441	static struct platform_driver exynos_cpufreq_platdrv = {
	442	.driver = {
	443	.name = "exynos5440-cpufreq",
49d7b5bf ADK	444	.of_match_table = exynos_cpufreq_match,
	445	},
	446	.probe = exynos_cpufreq_probe,
	447	.remove = exynos_cpufreq_remove,
	448	};
	449	module_platform_driver(exynos_cpufreq_platdrv);
	450
	451	MODULE_AUTHOR("Amit Daniel Kachhap <amit.daniel@samsung.com>");
	452	MODULE_DESCRIPTION("Exynos5440 cpufreq driver");
	453	MODULE_LICENSE("GPL");