ARM: OMAP2+: voltage: ensure voltage used is exact voltage from OPP table

When using the SMPS regulators to scale voltages, the regulator
framework may pass a minimum voltage that is not an exact OPP voltage.
For the VC/VP controlled voltage domains, we must ensure that the
voltage requested is the exact voltage from the OPP table.  This is
especially critical when using SR.

To fix, voltdm_scale() uses the target voltage passed to walk through
the OPP voltages until it finds a voltage that is >= one of the OPP
voltages.

Cc: Tero Kristo <t-kristo@ti.com>
Cc: Nishanth Menon <nm@ti.com>
Signed-off-by: Kevin Hilman <khilman@ti.com>

1 files changed, 18 insertions, 3 deletions

diff --git a/arch/arm/mach-omap2/voltage.c b/arch/arm/mach-omap2/voltage.c
index 8a36342e60d2..4dc60e83e00d 100644
--- 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;
 }