X-Git-Url: https://git.openpandora.org/cgi-bin/gitweb.cgi?a=blobdiff_plain;f=arch%2Farm%2Fmach-omap2%2Fvoltage.c;h=244c4a1b073cca0d544c9eed7675bca09d3e033f;hb=e2dc9d598b4945be932e176ff19b9f804c7ab1a4;hp=1f8fdf736e630976bc97fc781d58bfbf85704179;hpb=626e3677a2cb59f8afb9caef3f58c4685587986a;p=pandora-kernel.git

diff --git a/arch/arm/mach-omap2/voltage.c b/arch/arm/mach-omap2/voltage.c
index 1f8fdf736e63..244c4a1b073c 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;
 }