drivers/cpuidle/cpuidle-ambarella.c - nest-hello/linux-3.10 - Git at Google

 /*
  * Copyright 2015 ambarella, Inc.
  *
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
  * version 2, as published by the Free Software Foundation.
  *
  * This program is distributed in the hope 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, see <http://www.gnu.org/licenses/>.
  *
  * Maintainer: Jorney Tu <qtu@ambarella.com>
  */

 #include <linux/cpuidle.h>
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/of.h>
 #include <linux/time.h>
 #include <linux/delay.h>
 #include <linux/suspend.h>
 #include <asm/cpuidle.h>
 #include <asm/proc-fns.h>
 #include <asm/smp_scu.h>
 #include <linux/interrupt.h>
 #include <linux/percpu.h>
 #include <linux/cpu.h>
 #include <linux/clk.h>
 #include <linux/cpufreq.h>
 #include "../../kernel/time/tick-internal.h"

 #define DEFAULT_FREQENCY_INDEX  1

 #define MIN(x,y)   ( (x) < (y) ? (x) : (y) )

 #define CPUIDLE_DBG  0

 #if CPUIDLE_DBG
 #define dbg_print(fmt, args...)   printk(fmt, ##args)
 #else
 #define dbg_print(fmt, args...)   do{}while(0)
 #endif


 struct ambarella_cpuidle_info {
 	unsigned int core_old_freqency ;
 	unsigned int cortex_old_freqency ;

 	unsigned int freq_tbl_cnt;
 	struct cpufreq_frequency_table *freq_tbl;

 	struct clk *core_clk ;   // pll_out_core !!!
 	struct clk *cortex_clk ;
 };

 static struct ambarella_cpuidle_info *info = NULL;

 static int ambarella_cpufreq_probe(void)
 {
 	const struct property *prop;
 	struct cpufreq_frequency_table *freqs;
 	struct device_node *np;
 	int i, count;
 	int ret = 0;
 	const __be32 *value;

 	info = kmalloc(sizeof(struct ambarella_cpuidle_info), GFP_KERNEL);
 	if (!info) {
 		pr_err("kmalloc ambarella_cpuidle_info failed !\n");
 		return -ENOMEM;
 	}

 	np = of_find_node_by_path("/cpus");
 	if (!np) {
 		pr_err("No cpu node found");
 		ret = -EFAULT;
 		goto info_mem;
 	}

 	prop = of_find_property(np, "cpufreq_tbl", NULL);
 	if (!prop || !prop->value) {
 		pr_err("Invalid cpufreq_tbl");
 		ret = -ENODEV;
 		goto node_put;
 	}

 	count = prop->length / sizeof(u32);
 	value = prop->value;

 	freqs = kmalloc(sizeof(struct cpufreq_frequency_table) * count,GFP_KERNEL);
 	if (!freqs) {
 		ret = -ENOMEM;
 		goto node_put;
 	}


 	for (i = 0; i < count; i++) {
 		freqs[i].index = i;
 		freqs[i].frequency = be32_to_cpup(value++);
 	}

 	info->freq_tbl_cnt = count;
 	info->freq_tbl = freqs;

 	info->core_clk = clk_get(NULL, "pll_out_core");
 	if (IS_ERR(info->core_clk)){
 		pr_err("unable get core clk\n");
 		ret = -EFAULT;
 		goto freq_mem;
 	}

 	info->cortex_clk = clk_get(NULL, "gclk_cortex");
 	if (IS_ERR(info->cortex_clk)){
 		pr_err("unable get cortex clk\n");
 		ret = -EFAULT;
 		goto freq_mem;
 	}

 	of_node_put(np);
 	return ret;

 freq_mem:
 	kfree(freqs);
 node_put:
 	of_node_put(np);
 info_mem:
 	kfree(info);
 	return ret;
 }

 static unsigned int ambarella_cur_freq(struct clk *clk)
 {
 	return clk_get_rate(clk) / 1000;
 }

 static int ambarella_cpufreq_switch(unsigned int index)
 {
 	int ret ;
 	unsigned int core_new_freqency = 0;
 	unsigned int cortex_new_freqency = 0;

 	if(index < 0 || 2 * index > info->freq_tbl_cnt)
 		index = 0;

 	info->core_old_freqency = ambarella_cur_freq(info->core_clk) / 2;
 	info->cortex_old_freqency = ambarella_cur_freq(info->cortex_clk);

 	core_new_freqency = info->freq_tbl[index * 2].frequency;
 	cortex_new_freqency = info->freq_tbl[index * 2  + 1].frequency;

 	ret = clk_set_rate(info->core_clk, core_new_freqency * 1000 * 2);
 	if (ret){
 		pr_err("Core clock set failed\n");
 		return -1;
 	}

 	ret = clk_set_rate(info->cortex_clk, cortex_new_freqency * 1000);
 	if (ret){
 		pr_err("Cortex clock set failed\n");
 		return -1;
 	}

 	dbg_print("Switch Core frequency from %d to %d\n", info->core_old_freqency, core_new_freqency);
 	dbg_print("Switch Cortex frequency from %d to %d\n", info->cortex_old_freqency, cortex_new_freqency);
 	return 0;
 }

 static int ambarella_cpufreq_recover(void)
 {
 	int ret ;
 	unsigned int core_new_freqency = 0;
 	unsigned int cortex_new_freqency = 0;

 	core_new_freqency = info->core_old_freqency;
 	cortex_new_freqency = info->cortex_old_freqency;

 	info->core_old_freqency = ambarella_cur_freq(info->core_clk) / 2;
 	info->cortex_old_freqency = ambarella_cur_freq(info->cortex_clk);

 	ret = clk_set_rate(info->core_clk, core_new_freqency * 1000 * 2);
 	if (ret){
 		pr_err("Core clock set failed\n");
 		return -1;
 	}

 	ret = clk_set_rate(info->cortex_clk, cortex_new_freqency * 1000);
 	if (ret){
 		pr_err("Cortex clock set failed\n");
 		return -1;
 	}

 	dbg_print("Recover Core frequency from %d to %d\n", info->core_old_freqency, core_new_freqency);
 	dbg_print("Recover Cortex frequency from %d to %d\n", info->cortex_old_freqency, cortex_new_freqency);
 	return 0;
 }

 static int disable_percpu_timer(void)
 {

 	struct clock_event_device  *dev;
 	struct tick_device *td;
 	int cpu;

 	cpu = smp_processor_id();
 	td = &per_cpu(tick_cpu_device, cpu);
 	dev = td->evtdev;
 	clockevents_set_mode(dev, CLOCK_EVT_MODE_SHUTDOWN);

 	return 0;
 }

 static int enable_percpu_timer(void)
 {

 	struct clock_event_device  *dev;
 	struct tick_device *td;
 	int cpu;

 	cpu = smp_processor_id();
 	td = &per_cpu(tick_cpu_device, cpu);
 	dev = td->evtdev;
 	clockevents_set_mode(dev, CLOCK_EVT_MODE_ONESHOT);

 	return 0;
 }

 static void ambarella_idle_exec(unsigned int freq_index)
 {

 	ambarella_cpufreq_switch(freq_index);

 	disable_percpu_timer();

 	cpu_do_idle();

 	enable_percpu_timer();

 	ambarella_cpufreq_recover();
 }

 static int ambarella_pwrdown_idle(struct cpuidle_device *dev,
 				struct cpuidle_driver *drv,
 				int index)
 {
 	ambarella_idle_exec(DEFAULT_FREQENCY_INDEX);

 	return index;
 }

 static struct cpuidle_driver ambarella_idle_driver = {
 	.name = "ambarella_idle",
 	.states = {
 		ARM_CPUIDLE_WFI_STATE,
 		{
 			.name = "FG",
 			.desc = "FREQ Gate",
 			.flags = CPUIDLE_FLAG_TIMER_STOP,
 			.exit_latency = 30,
 			.power_usage = 50,
 			.target_residency = 400000,      // for judging cpu idle time
 			.enter = ambarella_pwrdown_idle,
 		},
 	},
 	.state_count = 2,
 };

 static int __init ambarella_cpuidle_init(void)
 {

 	if (ambarella_cpufreq_probe())
 		return -EFAULT;

 	return cpuidle_register(&ambarella_idle_driver, NULL);
 }
 module_init(ambarella_cpuidle_init);
	/*
	* Copyright 2015 ambarella, Inc.
	*
	*
	* This program is free software; you can redistribute it and/or modify it
	* under the terms and conditions of the GNU General Public License,
	* version 2, as published by the Free Software Foundation.
	*
	* This program is distributed in the hope 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, see <http://www.gnu.org/licenses/>.
	*
	* Maintainer: Jorney Tu <qtu@ambarella.com>
	*/

	#include <linux/cpuidle.h>
	#include <linux/init.h>
	#include <linux/io.h>
	#include <linux/of.h>
	#include <linux/time.h>
	#include <linux/delay.h>
	#include <linux/suspend.h>
	#include <asm/cpuidle.h>
	#include <asm/proc-fns.h>
	#include <asm/smp_scu.h>
	#include <linux/interrupt.h>
	#include <linux/percpu.h>
	#include <linux/cpu.h>
	#include <linux/clk.h>
	#include <linux/cpufreq.h>
	#include "../../kernel/time/tick-internal.h"

	#define DEFAULT_FREQENCY_INDEX 1

	#define MIN(x,y) ( (x) < (y) ? (x) : (y) )

	#define CPUIDLE_DBG 0

	#if CPUIDLE_DBG
	#define dbg_print(fmt, args...) printk(fmt, ##args)
	#else
	#define dbg_print(fmt, args...) do{}while(0)
	#endif


	struct ambarella_cpuidle_info {
	unsigned int core_old_freqency ;
	unsigned int cortex_old_freqency ;

	unsigned int freq_tbl_cnt;
	struct cpufreq_frequency_table *freq_tbl;

	struct clk *core_clk ; // pll_out_core !!!
	struct clk *cortex_clk ;
	};

	static struct ambarella_cpuidle_info *info = NULL;

	static int ambarella_cpufreq_probe(void)
	{
	const struct property *prop;
	struct cpufreq_frequency_table *freqs;
	struct device_node *np;
	int i, count;
	int ret = 0;
	const __be32 *value;

	info = kmalloc(sizeof(struct ambarella_cpuidle_info), GFP_KERNEL);
	if (!info) {
	pr_err("kmalloc ambarella_cpuidle_info failed !\n");
	return -ENOMEM;
	}

	np = of_find_node_by_path("/cpus");
	if (!np) {
	pr_err("No cpu node found");
	ret = -EFAULT;
	goto info_mem;
	}

	prop = of_find_property(np, "cpufreq_tbl", NULL);
	if (!prop \|\| !prop->value) {
	pr_err("Invalid cpufreq_tbl");
	ret = -ENODEV;
	goto node_put;
	}

	count = prop->length / sizeof(u32);
	value = prop->value;

	freqs = kmalloc(sizeof(struct cpufreq_frequency_table) * count,GFP_KERNEL);
	if (!freqs) {
	ret = -ENOMEM;
	goto node_put;
	}


	for (i = 0; i < count; i++) {
	freqs[i].index = i;
	freqs[i].frequency = be32_to_cpup(value++);
	}

	info->freq_tbl_cnt = count;
	info->freq_tbl = freqs;

	info->core_clk = clk_get(NULL, "pll_out_core");
	if (IS_ERR(info->core_clk)){
	pr_err("unable get core clk\n");
	ret = -EFAULT;
	goto freq_mem;
	}

	info->cortex_clk = clk_get(NULL, "gclk_cortex");
	if (IS_ERR(info->cortex_clk)){
	pr_err("unable get cortex clk\n");
	ret = -EFAULT;
	goto freq_mem;
	}

	of_node_put(np);
	return ret;

	freq_mem:
	kfree(freqs);
	node_put:
	of_node_put(np);
	info_mem:
	kfree(info);
	return ret;
	}

	static unsigned int ambarella_cur_freq(struct clk *clk)
	{
	return clk_get_rate(clk) / 1000;
	}

	static int ambarella_cpufreq_switch(unsigned int index)
	{
	int ret ;
	unsigned int core_new_freqency = 0;
	unsigned int cortex_new_freqency = 0;

	if(index < 0 \|\| 2 * index > info->freq_tbl_cnt)
	index = 0;

	info->core_old_freqency = ambarella_cur_freq(info->core_clk) / 2;
	info->cortex_old_freqency = ambarella_cur_freq(info->cortex_clk);

	core_new_freqency = info->freq_tbl[index * 2].frequency;
	cortex_new_freqency = info->freq_tbl[index * 2 + 1].frequency;

	ret = clk_set_rate(info->core_clk, core_new_freqency * 1000 * 2);
	if (ret){
	pr_err("Core clock set failed\n");
	return -1;
	}

	ret = clk_set_rate(info->cortex_clk, cortex_new_freqency * 1000);
	if (ret){
	pr_err("Cortex clock set failed\n");
	return -1;
	}

	dbg_print("Switch Core frequency from %d to %d\n", info->core_old_freqency, core_new_freqency);
	dbg_print("Switch Cortex frequency from %d to %d\n", info->cortex_old_freqency, cortex_new_freqency);
	return 0;
	}

	static int ambarella_cpufreq_recover(void)
	{
	int ret ;
	unsigned int core_new_freqency = 0;
	unsigned int cortex_new_freqency = 0;

	core_new_freqency = info->core_old_freqency;
	cortex_new_freqency = info->cortex_old_freqency;

	info->core_old_freqency = ambarella_cur_freq(info->core_clk) / 2;
	info->cortex_old_freqency = ambarella_cur_freq(info->cortex_clk);

	ret = clk_set_rate(info->core_clk, core_new_freqency * 1000 * 2);
	if (ret){
	pr_err("Core clock set failed\n");
	return -1;
	}

	ret = clk_set_rate(info->cortex_clk, cortex_new_freqency * 1000);
	if (ret){
	pr_err("Cortex clock set failed\n");
	return -1;
	}

	dbg_print("Recover Core frequency from %d to %d\n", info->core_old_freqency, core_new_freqency);
	dbg_print("Recover Cortex frequency from %d to %d\n", info->cortex_old_freqency, cortex_new_freqency);
	return 0;
	}

	static int disable_percpu_timer(void)
	{

	struct clock_event_device *dev;
	struct tick_device *td;
	int cpu;

	cpu = smp_processor_id();
	td = &per_cpu(tick_cpu_device, cpu);
	dev = td->evtdev;
	clockevents_set_mode(dev, CLOCK_EVT_MODE_SHUTDOWN);

	return 0;
	}

	static int enable_percpu_timer(void)
	{

	struct clock_event_device *dev;
	struct tick_device *td;
	int cpu;

	cpu = smp_processor_id();
	td = &per_cpu(tick_cpu_device, cpu);
	dev = td->evtdev;
	clockevents_set_mode(dev, CLOCK_EVT_MODE_ONESHOT);

	return 0;
	}

	static void ambarella_idle_exec(unsigned int freq_index)
	{

	ambarella_cpufreq_switch(freq_index);

	disable_percpu_timer();

	cpu_do_idle();

	enable_percpu_timer();

	ambarella_cpufreq_recover();
	}

	static int ambarella_pwrdown_idle(struct cpuidle_device *dev,
	struct cpuidle_driver *drv,
	int index)
	{
	ambarella_idle_exec(DEFAULT_FREQENCY_INDEX);

	return index;
	}

	static struct cpuidle_driver ambarella_idle_driver = {
	.name = "ambarella_idle",
	.states = {
	ARM_CPUIDLE_WFI_STATE,
	{
	.name = "FG",
	.desc = "FREQ Gate",
	.flags = CPUIDLE_FLAG_TIMER_STOP,
	.exit_latency = 30,
	.power_usage = 50,
	.target_residency = 400000, // for judging cpu idle time
	.enter = ambarella_pwrdown_idle,
	},
	},
	.state_count = 2,
	};

	static int __init ambarella_cpuidle_init(void)
	{

	if (ambarella_cpufreq_probe())
	return -EFAULT;

	return cpuidle_register(&ambarella_idle_driver, NULL);
	}
	module_init(ambarella_cpuidle_init);