openwrt-xburst/target/linux/xburst/patches-3.0/0028-Add-cpufreq-support.patch

From b0b7794018b75ce33be133664fb58ab3e6efc2e8 Mon Sep 17 00:00:00 2001
From: Xiangfu Liu <xiangfu@macbook.openmobilefree.net>
Date: Wed, 14 Sep 2011 14:29:52 +0800
Subject: [PATCH 28/32] Add cpufreq support

---
 drivers/cpufreq/cpufreq_stats.c |  161 ++++++++++++++++++++-------------------
 drivers/mmc/host/jz4740_mmc.c   |   69 ++++++++++++++++-
 2 files changed, 150 insertions(+), 80 deletions(-)

diff --git a/drivers/cpufreq/cpufreq_stats.c b/drivers/cpufreq/cpufreq_stats.c
index faf7c52..86d032c 100644
--- a/drivers/cpufreq/cpufreq_stats.c
+++ b/drivers/cpufreq/cpufreq_stats.c
@@ -20,6 +20,7 @@
 #include <linux/kobject.h>
 #include <linux/spinlock.h>
 #include <linux/notifier.h>
+#include <linux/string.h>
 #include <asm/cputime.h>
 
 static spinlock_t cpufreq_stats_lock;
@@ -36,7 +37,7 @@ struct cpufreq_stats {
 	unsigned long long  last_time;
 	unsigned int max_state;
 	unsigned int state_num;
-	unsigned int last_index;
+	int last_index;
 	cputime64_t *time_in_state;
 	unsigned int *freq_table;
 #ifdef CONFIG_CPU_FREQ_STAT_DETAILS
@@ -59,7 +60,7 @@ static int cpufreq_stats_update(unsigned int cpu)
 	cur_time = get_jiffies_64();
 	spin_lock(&cpufreq_stats_lock);
 	stat = per_cpu(cpufreq_stats_table, cpu);
-	if (stat->time_in_state)
+	if (stat->time_in_state && stat->last_index != -1)
 		stat->time_in_state[stat->last_index] =
 			cputime64_add(stat->time_in_state[stat->last_index],
 				      cputime_sub(cur_time, stat->last_time));
@@ -82,7 +83,7 @@ static ssize_t show_time_in_state(struct cpufreq_policy *policy, char *buf)
 	ssize_t len = 0;
 	int i;
 	struct cpufreq_stats *stat = per_cpu(cpufreq_stats_table, policy->cpu);
-	if (!stat)
+	if (!stat || !stat->time_in_state)
 		return 0;
 	cpufreq_stats_update(stat->cpu);
 	for (i = 0; i < stat->state_num; i++) {
@@ -100,7 +101,7 @@ static ssize_t show_trans_table(struct cpufreq_policy *policy, char *buf)
 	int i, j;
 
 	struct cpufreq_stats *stat = per_cpu(cpufreq_stats_table, policy->cpu);
-	if (!stat)
+	if (!stat || !stat->trans_table)
 		return 0;
 	cpufreq_stats_update(stat->cpu);
 	len += snprintf(buf + len, PAGE_SIZE - len, "   From  :    To\n");
@@ -159,63 +160,35 @@ static struct attribute_group stats_attr_group = {
 static int freq_table_get_index(struct cpufreq_stats *stat, unsigned int freq)
 {
 	int index;
-	for (index = 0; index < stat->max_state; index++)
-		if (stat->freq_table[index] == freq)
-			return index;
+	if (stat->freq_table)
+		for (index = 0; index < stat->max_state; index++)
+			if (stat->freq_table[index] == freq)
+				return index;
 	return -1;
 }
 
-/* should be called late in the CPU removal sequence so that the stats
- * memory is still available in case someone tries to use it.
- */
 static void cpufreq_stats_free_table(unsigned int cpu)
 {
 	struct cpufreq_stats *stat = per_cpu(cpufreq_stats_table, cpu);
+	struct cpufreq_policy *policy = cpufreq_cpu_get(cpu);
+	if (policy && policy->cpu == cpu)
+		sysfs_remove_group(&policy->kobj, &stats_attr_group);
 	if (stat) {
 		kfree(stat->time_in_state);
 		kfree(stat);
 	}
 	per_cpu(cpufreq_stats_table, cpu) = NULL;
-}
-
-/* must be called early in the CPU removal sequence (before
- * cpufreq_remove_dev) so that policy is still valid.
- */
-static void cpufreq_stats_free_sysfs(unsigned int cpu)
-{
-	struct cpufreq_policy *policy = cpufreq_cpu_get(cpu);
-	if (policy && policy->cpu == cpu)
-		sysfs_remove_group(&policy->kobj, &stats_attr_group);
 	if (policy)
 		cpufreq_cpu_put(policy);
 }
 
-static int cpufreq_stats_create_table(struct cpufreq_policy *policy,
+static int cpufreq_stats_update_table(struct cpufreq_policy *policy,
 		struct cpufreq_frequency_table *table)
 {
-	unsigned int i, j, count = 0, ret = 0;
-	struct cpufreq_stats *stat;
-	struct cpufreq_policy *data;
+	unsigned int i, j, count = 0;
 	unsigned int alloc_size;
 	unsigned int cpu = policy->cpu;
-	if (per_cpu(cpufreq_stats_table, cpu))
-		return -EBUSY;
-	stat = kzalloc(sizeof(struct cpufreq_stats), GFP_KERNEL);
-	if ((stat) == NULL)
-		return -ENOMEM;
-
-	data = cpufreq_cpu_get(cpu);
-	if (data == NULL) {
-		ret = -EINVAL;
-		goto error_get_fail;
-	}
-
-	ret = sysfs_create_group(&data->kobj, &stats_attr_group);
-	if (ret)
-		goto error_out;
-
-	stat->cpu = cpu;
-	per_cpu(cpufreq_stats_table, cpu) = stat;
+	struct cpufreq_stats *stat = per_cpu(cpufreq_stats_table, cpu);
 
 	for (i = 0; table[i].frequency != CPUFREQ_TABLE_END; i++) {
 		unsigned int freq = table[i].frequency;
@@ -224,40 +197,73 @@ static int cpufreq_stats_create_table(struct cpufreq_policy *policy,
 		count++;
 	}
 
+	if (stat->max_state != count) {
+		stat->max_state = count;
+		kfree(stat->time_in_state);
+		stat->time_in_state = NULL;
+	}
 	alloc_size = count * sizeof(int) + count * sizeof(cputime64_t);
-
 #ifdef CONFIG_CPU_FREQ_STAT_DETAILS
 	alloc_size += count * count * sizeof(int);
 #endif
-	stat->max_state = count;
-	stat->time_in_state = kzalloc(alloc_size, GFP_KERNEL);
-	if (!stat->time_in_state) {
-		ret = -ENOMEM;
-		goto error_out;
-	}
-	stat->freq_table = (unsigned int *)(stat->time_in_state + count);
-
+	if (stat->time_in_state) {
+		memset(stat->time_in_state, 0, alloc_size);
+	} else {
+		stat->time_in_state = kzalloc(alloc_size, GFP_KERNEL);
+		if (!stat->time_in_state)
+			return -ENOMEM;
+		stat->freq_table = (unsigned int *)(
+				stat->time_in_state + count);
 #ifdef CONFIG_CPU_FREQ_STAT_DETAILS
-	stat->trans_table = stat->freq_table + count;
+		stat->trans_table = stat->freq_table + count;
 #endif
+	}
+
 	j = 0;
-	for (i = 0; table[i].frequency != CPUFREQ_TABLE_END; i++) {
-		unsigned int freq = table[i].frequency;
-		if (freq == CPUFREQ_ENTRY_INVALID)
-			continue;
-		if (freq_table_get_index(stat, freq) == -1)
-			stat->freq_table[j++] = freq;
+	if (stat->freq_table) {
+		for (i = 0; table[i].frequency != CPUFREQ_TABLE_END; i++) {
+			unsigned int freq = table[i].frequency;
+			if (freq == CPUFREQ_ENTRY_INVALID)
+				continue;
+			if (freq_table_get_index(stat, freq) == -1)
+				stat->freq_table[j++] = freq;
+		}
 	}
 	stat->state_num = j;
 	spin_lock(&cpufreq_stats_lock);
 	stat->last_time = get_jiffies_64();
 	stat->last_index = freq_table_get_index(stat, policy->cur);
 	spin_unlock(&cpufreq_stats_lock);
+	return 0;
+}
+
+static int cpufreq_stats_create_table(struct cpufreq_policy *policy,
+		struct cpufreq_frequency_table *table)
+{
+	unsigned int ret = 0;
+	struct cpufreq_stats *stat;
+	struct cpufreq_policy *data;
+	unsigned int cpu = policy->cpu;
+
+	stat = kzalloc(sizeof(struct cpufreq_stats), GFP_KERNEL);
+	if ((stat) == NULL)
+		return -ENOMEM;
+
+	data = cpufreq_cpu_get(cpu);
+	if (data == NULL) {
+		ret = -EINVAL;
+		goto error_out;
+	}
+	ret = sysfs_create_group(&data->kobj, &stats_attr_group);
 	cpufreq_cpu_put(data);
+	if (ret)
+		goto error_out;
+
+	stat->cpu = cpu;
+	per_cpu(cpufreq_stats_table, cpu) = stat;
+
 	return 0;
 error_out:
-	cpufreq_cpu_put(data);
-error_get_fail:
 	kfree(stat);
 	per_cpu(cpufreq_stats_table, cpu) = NULL;
 	return ret;
@@ -275,10 +281,12 @@ static int cpufreq_stat_notifier_policy(struct notifier_block *nb,
 	table = cpufreq_frequency_get_table(cpu);
 	if (!table)
 		return 0;
-	ret = cpufreq_stats_create_table(policy, table);
-	if (ret)
-		return ret;
-	return 0;
+	if (!per_cpu(cpufreq_stats_table, cpu)) {
+		ret = cpufreq_stats_create_table(policy, table);
+		if (ret)
+			return ret;
+	}
+	return cpufreq_stats_update_table(policy, table);
 }
 
 static int cpufreq_stat_notifier_trans(struct notifier_block *nb,
@@ -298,21 +306,23 @@ static int cpufreq_stat_notifier_trans(struct notifier_block *nb,
 	old_index = stat->last_index;
 	new_index = freq_table_get_index(stat, freq->new);
 
-	/* We can't do stat->time_in_state[-1]= .. */
-	if (old_index == -1 || new_index == -1)
-		return 0;
-
 	cpufreq_stats_update(freq->cpu);
-
 	if (old_index == new_index)
 		return 0;
 
+	if (new_index == -1)
+		return 0;
+
 	spin_lock(&cpufreq_stats_lock);
 	stat->last_index = new_index;
+	if (old_index != -1) {
 #ifdef CONFIG_CPU_FREQ_STAT_DETAILS
-	stat->trans_table[old_index * stat->max_state + new_index]++;
+		if (stat->trans_table)
+			stat->trans_table[old_index * stat->max_state +
+					  new_index]++;
 #endif
-	stat->total_trans++;
+		stat->total_trans++;
+	}
 	spin_unlock(&cpufreq_stats_lock);
 	return 0;
 }
@@ -328,9 +338,6 @@ static int __cpuinit cpufreq_stat_cpu_callback(struct notifier_block *nfb,
 	case CPU_ONLINE_FROZEN:
 		cpufreq_update_policy(cpu);
 		break;
-	case CPU_DOWN_PREPARE:
-		cpufreq_stats_free_sysfs(cpu);
-		break;
 	case CPU_DEAD:
 	case CPU_DEAD_FROZEN:
 		cpufreq_stats_free_table(cpu);
@@ -339,10 +346,9 @@ static int __cpuinit cpufreq_stat_cpu_callback(struct notifier_block *nfb,
 	return NOTIFY_OK;
 }
 
-/* priority=1 so this will get called before cpufreq_remove_dev */
-static struct notifier_block cpufreq_stat_cpu_notifier __refdata = {
+static struct notifier_block cpufreq_stat_cpu_notifier __refdata =
+{
 	.notifier_call = cpufreq_stat_cpu_callback,
-	.priority = 1,
 };
 
 static struct notifier_block notifier_policy_block = {
@@ -389,7 +395,6 @@ static void __exit cpufreq_stats_exit(void)
 	unregister_hotcpu_notifier(&cpufreq_stat_cpu_notifier);
 	for_each_online_cpu(cpu) {
 		cpufreq_stats_free_table(cpu);
-		cpufreq_stats_free_sysfs(cpu);
 	}
 }
 
diff --git a/drivers/mmc/host/jz4740_mmc.c b/drivers/mmc/host/jz4740_mmc.c
index 74218ad..6e40f1b 100644
--- a/drivers/mmc/host/jz4740_mmc.c
+++ b/drivers/mmc/host/jz4740_mmc.c
@@ -23,6 +23,7 @@
 #include <linux/delay.h>
 #include <linux/scatterlist.h>
 #include <linux/clk.h>
+#include <linux/cpufreq.h>
 
 #include <linux/bitops.h>
 #include <linux/gpio.h>
@@ -685,6 +686,60 @@ static void jz4740_mmc_enable_sdio_irq(struct mmc_host *mmc, int enable)
 	jz4740_mmc_set_irq_enabled(host, JZ_MMC_IRQ_SDIO, enable);
 }
 
+#ifdef CONFIG_CPU_FREQ
+
+static struct jz4740_mmc_host *cpufreq_host;
+
+static int jz4740_mmc_cpufreq_transition(struct notifier_block *nb,
+					 unsigned long val, void *data)
+{
+	/* TODO: We only have to take action when the PLL freq changes:
+	         the main dividers have no influence on the MSC device clock. */
+
+	if (val == CPUFREQ_PRECHANGE) {
+		mmc_claim_host(cpufreq_host->mmc);
+		clk_disable(cpufreq_host->clk);
+	} else if (val == CPUFREQ_POSTCHANGE) {
+		struct mmc_ios *ios = &cpufreq_host->mmc->ios;
+		if (ios->clock)
+			jz4740_mmc_set_clock_rate(cpufreq_host, ios->clock);
+		if (ios->power_mode != MMC_POWER_OFF)
+			clk_enable(cpufreq_host->clk);
+		mmc_release_host(cpufreq_host->mmc);
+	}
+	return 0;
+}
+
+static struct notifier_block jz4740_mmc_cpufreq_nb = {
+	.notifier_call = jz4740_mmc_cpufreq_transition,
+};
+
+static inline int jz4740_mmc_cpufreq_register(struct jz4740_mmc_host *host)
+{
+	cpufreq_host = host;
+	return cpufreq_register_notifier(&jz4740_mmc_cpufreq_nb,
+					 CPUFREQ_TRANSITION_NOTIFIER);
+}
+
+static inline void jz4740_mmc_cpufreq_unregister(void)
+{
+	cpufreq_unregister_notifier(&jz4740_mmc_cpufreq_nb,
+				    CPUFREQ_TRANSITION_NOTIFIER);
+}
+
+#else
+
+static inline int jz4740_mmc_cpufreq_register(struct jz4740_mmc_host *host)
+{
+	return 0;
+}
+
+static inline void jz4740_mmc_cpufreq_unregister(void)
+{
+}
+
+#endif
+
 static const struct mmc_host_ops jz4740_mmc_ops = {
 	.request	= jz4740_mmc_request,
 	.set_ios	= jz4740_mmc_set_ios,
@@ -834,11 +889,18 @@ static int __devinit jz4740_mmc_probe(struct platform_device* pdev)
 		goto err_free_host;
 	}
 
+	ret = jz4740_mmc_cpufreq_register(host);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Failed to register cpufreq transition notifier\n");
+		goto err_clk_put;
+	}
+
 	host->mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!host->mem) {
 		ret = -ENOENT;
 		dev_err(&pdev->dev, "Failed to get base platform memory\n");
-		goto err_clk_put;
+		goto err_cpufreq_unreg;
 	}
 
 	host->mem = request_mem_region(host->mem->start,
@@ -846,7 +908,7 @@ static int __devinit jz4740_mmc_probe(struct platform_device* pdev)
 	if (!host->mem) {
 		ret = -EBUSY;
 		dev_err(&pdev->dev, "Failed to request base memory region\n");
-		goto err_clk_put;
+		goto err_cpufreq_unreg;
 	}
 
 	host->base = ioremap_nocache(host->mem->start, resource_size(host->mem));
@@ -929,6 +991,8 @@ err_iounmap:
 	iounmap(host->base);
 err_release_mem_region:
 	release_mem_region(host->mem->start, resource_size(host->mem));
+err_cpufreq_unreg:
+	jz4740_mmc_cpufreq_unregister();
 err_clk_put:
 	clk_put(host->clk);
 err_free_host:
@@ -958,6 +1022,7 @@ static int __devexit jz4740_mmc_remove(struct platform_device *pdev)
 	iounmap(host->base);
 	release_mem_region(host->mem->start, resource_size(host->mem));
 
+	jz4740_mmc_cpufreq_unregister();
 	clk_put(host->clk);
 
 	platform_set_drvdata(pdev, NULL);
-- 
1.7.4.1