ARM: implement ioremap_prot

[pandora-kernel.git] / arch / arm / mach-omap2 / voltage.c

diff --git a/arch/arm/mach-omap2/voltage.c b/arch/arm/mach-omap2/voltage.c
index 1f8fdf7..244c4a1 100644 (file)
--- a/arch/arm/mach-omap2/voltage.c
+++ b/arch/arm/mach-omap2/voltage.c
@@ -73,7 +73,8 @@ unsigned long voltdm_get_voltage(struct voltagedomain *voltdm)
 int voltdm_scale(struct voltagedomain *voltdm,
                 unsigned long target_volt)
 {
-       int ret;
+       int ret, i;
+       unsigned long volt = 0;
 
        if (!voltdm || IS_ERR(voltdm)) {
                pr_warning("%s: VDD specified does not exist!\n", __func__);
@@ -86,9 +87,23 @@ int voltdm_scale(struct voltagedomain *voltdm,
                return -ENODATA;
        }
 
-       ret = voltdm->scale(voltdm, target_volt);
+       /* Adjust voltage to the exact voltage from the OPP table */
+       for (i = 0; voltdm->volt_data[i].volt_nominal != 0; i++) {
+               if (voltdm->volt_data[i].volt_nominal >= target_volt) {
+                       volt = voltdm->volt_data[i].volt_nominal;
+                       break;
+               }
+       }
+
+       if (!volt) {
+               pr_warning("%s: not scaling. OPP voltage for %lu, not found.\n",
+                          __func__, target_volt);
+               return -EINVAL;
+       }
+
+       ret = voltdm->scale(voltdm, volt);
        if (!ret)
-               voltdm->nominal_volt = target_volt;
+               voltdm->nominal_volt = volt;
 
        return ret;
 }