From 8f19276b46da8868faad0050fcd3666c24c6c6f0 Mon Sep 17 00:00:00 2001 From: Robin Gong Date: Tue, 13 Nov 2012 13:02:57 +0800 Subject: [PATCH] ENGR00233366-3 CPUFREQ:add cpufreq restore back if set fail Restore back cpu freq and regulator if set fail. Signed-off-by: Robin Gong --- arch/arm/plat-mxc/cpufreq.c | 86 +++++++++++++++++++++++++++++-------- 1 file changed, 69 insertions(+), 17 deletions(-) diff --git a/arch/arm/plat-mxc/cpufreq.c b/arch/arm/plat-mxc/cpufreq.c index 12334d5b01f1..33f85225a21b 100755 --- a/arch/arm/plat-mxc/cpufreq.c +++ b/arch/arm/plat-mxc/cpufreq.c @@ -59,9 +59,9 @@ int set_cpu_freq(int freq) { int i, ret = 0; int org_cpu_rate; - int gp_volt = 0; - int soc_volt = 0; - int pu_volt = 0; + int gp_volt = 0, org_gp_volt = 0; + int soc_volt = 0, org_soc_volt = 0; + int pu_volt = 0, org_pu_volt = 0; org_cpu_rate = clk_get_rate(cpu_clk); if (org_cpu_rate == freq) @@ -73,6 +73,11 @@ int set_cpu_freq(int freq) soc_volt = cpu_op_tbl[i].soc_voltage; pu_volt = cpu_op_tbl[i].pu_voltage; } + if (org_cpu_rate == cpu_op_tbl[i].cpu_rate) { + org_gp_volt = cpu_op_tbl[i].cpu_voltage; + org_soc_volt = cpu_op_tbl[i].soc_voltage; + org_pu_volt = cpu_op_tbl[i].pu_voltage; + } } if (gp_volt == 0) @@ -86,9 +91,9 @@ int set_cpu_freq(int freq) ret = regulator_set_voltage(soc_regulator, soc_volt, soc_volt); if (ret < 0) { - printk(KERN_DEBUG + printk(KERN_ERR "COULD NOT SET SOC VOLTAGE!!!!\n"); - return ret; + goto err1; } } /*if pu_regulator is enabled, it will be tracked with VDDARM*/ @@ -97,39 +102,39 @@ int set_cpu_freq(int freq) ret = regulator_set_voltage(pu_regulator, pu_volt, pu_volt); if (ret < 0) { - printk(KERN_DEBUG + printk(KERN_ERR "COULD NOT SET PU VOLTAGE!!!!\n"); - return ret; + goto err2; } } ret = regulator_set_voltage(cpu_regulator, gp_volt, gp_volt); if (ret < 0) { - printk(KERN_DEBUG "COULD NOT SET GP VOLTAGE!!!!\n"); - return ret; + printk(KERN_ERR "COULD NOT SET GP VOLTAGE!!!!\n"); + goto err3; } udelay(50); } ret = clk_set_rate(cpu_clk, freq); if (ret != 0) { - printk(KERN_DEBUG "cannot set CPU clock rate\n"); - return ret; + printk(KERN_ERR "cannot set CPU clock rate\n"); + goto err4; } if (freq < org_cpu_rate) { ret = regulator_set_voltage(cpu_regulator, gp_volt, gp_volt); if (ret < 0) { - printk(KERN_DEBUG "COULD NOT SET GP VOLTAGE!!!!\n"); - return ret; + printk(KERN_ERR "COULD NOT SET GP VOLTAGE!!!!\n"); + goto err5; } if (!IS_ERR(soc_regulator)) { ret = regulator_set_voltage(soc_regulator, soc_volt, soc_volt); if (ret < 0) { - printk(KERN_DEBUG + printk(KERN_ERR "COULD NOT SET SOC VOLTAGE BACK!!!!\n"); - return ret; + goto err6; } } /*if pu_regulator is enabled, it will be tracked with VDDARM*/ @@ -138,9 +143,9 @@ int set_cpu_freq(int freq) ret = regulator_set_voltage(pu_regulator, pu_volt, pu_volt); if (ret < 0) { - printk(KERN_DEBUG + printk(KERN_ERR "COULD NOT SET PU VOLTAGE!!!!\n"); - return ret; + goto err7; } } /* Check if the bus freq can be decreased.*/ @@ -148,6 +153,53 @@ int set_cpu_freq(int freq) } return ret; + /*Restore back if any regulator or clock rate set fail.*/ +err7: + ret = regulator_set_voltage(soc_regulator, org_soc_volt, + org_soc_volt); + if (ret < 0) { + printk(KERN_ERR "COULD NOT RESTORE SOC VOLTAGE!!!!\n"); + goto err7; + } + +err6: + ret = regulator_set_voltage(cpu_regulator, org_gp_volt, org_gp_volt); + if (ret < 0) { + printk(KERN_ERR "COULD NOT RESTORE GP VOLTAGE!!!!\n"); + goto err6; + } + +err5: + ret = clk_set_rate(cpu_clk, org_cpu_rate); + if (ret != 0) { + printk(KERN_ERR "cannot restore CPU clock rate\n"); + goto err5; + } + return -1; + +err4: + ret = regulator_set_voltage(cpu_regulator, org_gp_volt, org_gp_volt); + if (ret < 0) { + printk(KERN_ERR "COULD NOT RESTORE GP VOLTAGE!!!!\n"); + goto err4; + } + +err3: + ret = regulator_set_voltage(pu_regulator, org_pu_volt, org_pu_volt); + if (ret < 0) { + printk(KERN_ERR "COULD NOT RESTORE PU VOLTAGE!!!!\n"); + goto err3; + } +err2: + ret = regulator_set_voltage(soc_regulator, org_soc_volt, + org_soc_volt); + if (ret < 0) { + printk(KERN_ERR "COULD NOT RESTORE SOC VOLTAGE!!!!\n"); + goto err2; + } +err1: + return -1; + } static int mxc_verify_speed(struct cpufreq_policy *policy) -- 2.39.5