regulator: When constraining modes fall back to higher power modes

If a mode requested by a consumer is not allowed by constraints
automatically fall back to a higher power mode if possible. This
ensures that consumers get at least the output they requested while
allowing machine drivers to transparently limit lower power modes
if required.

Signed-off-by: Mark Brown <broonie@opensource.wolfsonmicro.com>
Signed-off-by: Liam Girdwood <lrg@slimlogic.co.uk>

diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 0fae51c4845a3cf703f220c11d313be6a8ee099c..7104404a9fa7cfced79c8f656a56d92a3a02d068 100644 (file)
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -197,9 +197,9 @@ static int regulator_check_current_limit(struct regulator_dev *rdev,
 }
 
 /* operating mode constraint check */
-static int regulator_check_mode(struct regulator_dev *rdev, int mode)
+static int regulator_mode_constrain(struct regulator_dev *rdev, int *mode)
 {
-       switch (mode) {
+       switch (*mode) {
        case REGULATOR_MODE_FAST:
        case REGULATOR_MODE_NORMAL:
        case REGULATOR_MODE_IDLE:
@@ -217,11 +217,17 @@ static int regulator_check_mode(struct regulator_dev *rdev, int mode)
                rdev_err(rdev, "operation not allowed\n");
                return -EPERM;
        }
-       if (!(rdev->constraints->valid_modes_mask & mode)) {
-               rdev_err(rdev, "invalid mode %x\n", mode);
-               return -EINVAL;
+
+       /* The modes are bitmasks, the most power hungry modes having
+        * the lowest values. If the requested mode isn't supported
+        * try higher modes. */
+       while (*mode) {
+               if (rdev->constraints->valid_modes_mask & *mode)
+                       return 0;
+               *mode /= 2;
        }
-       return 0;
+
+       return -EINVAL;
 }
 
 /* dynamic regulator mode switching constraint check */
@@ -612,7 +618,7 @@ static void drms_uA_update(struct regulator_dev *rdev)
                                                  output_uV, current_uA);
 
        /* check the new mode is allowed */
-       err = regulator_check_mode(rdev, mode);
+       err = regulator_mode_constrain(rdev, &mode);
        if (err == 0)
                rdev->desc->ops->set_mode(rdev, mode);
 }
@@ -2005,7 +2011,7 @@ int regulator_set_mode(struct regulator *regulator, unsigned int mode)
        }
 
        /* constraints check */
-       ret = regulator_check_mode(rdev, mode);
+       ret = regulator_mode_constrain(rdev, mode);
        if (ret < 0)
                goto out;
 
@@ -2116,7 +2122,7 @@ int regulator_set_optimum_mode(struct regulator *regulator, int uA_load)
        mode = rdev->desc->ops->get_optimum_mode(rdev,
                                                 input_uV, output_uV,
                                                 total_uA_load);
-       ret = regulator_check_mode(rdev, mode);
+       ret = regulator_mode_constrain(rdev, &mode);
        if (ret < 0) {
                rdev_err(rdev, "failed to get optimum mode @ %d uA %d -> %d uV\n",
                         total_uA_load, input_uV, output_uV);