ENGR00233366-3 CPUFREQ:add cpufreq restore back if set fail

Restore back cpu freq and regulator if set fail.

Signed-off-by: Robin Gong <b38343@freescale.com>

diff --git a/arch/arm/plat-mxc/cpufreq.c b/arch/arm/plat-mxc/cpufreq.c
index 12334d5b01f1c9eda703b959344a1fa3cfbd8b1d..33f85225a21b764286eea47bddbe1a1368803e88 100755 (executable)
--- 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)