mfd: Make use of the ab8500 firmware read-modify-write service

This patch updates the AB8500 driver to make use of the I2C
read-modify-write service in the PRCMU firmware.

Signed-off-by: Mattias Nilsson <mattias.i.nilsson@stericsson.com>
Reviewed-by: Mattias Wallin <mattias.wallin@stericsson.com>
Signed-off-by: Linus Walleij <linus.walleij@linaro.org>
Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>

1 files changed, 23 insertions, 14 deletions

diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index c637c8d2e7de..1f08704f7ae8 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -201,29 +201,38 @@ static int mask_and_set_register_interruptible(struct ab8500 *ab8500, u8 bank,
         u8 reg, u8 bitmask, u8 bitvalues)
 {
         int ret;
-        u8 data;
         /* put the u8 bank and u8 reg together into a an u16.
          * bank on higher 8 bits and reg in lower */
         u16 addr = ((u16)bank) << 8 | reg;
 
         mutex_lock(&ab8500->lock);
 
-        ret = ab8500->read(ab8500, addr);
-        if (ret < 0) {
-                dev_err(ab8500->dev, "failed to read reg %#x: %d\n",
-                        addr, ret);
-                goto out;
-        }
+        if (ab8500->write_masked == NULL) {
+                u8 data;
 
-        data = (u8)ret;
-        data = (~bitmask & data) | (bitmask & bitvalues);
+                ret = ab8500->read(ab8500, addr);
+                if (ret < 0) {
+                        dev_err(ab8500->dev, "failed to read reg %#x: %d\n",
+                                addr, ret);
+                        goto out;
+                }
 
-        ret = ab8500->write(ab8500, addr, data);
-        if (ret < 0)
-                dev_err(ab8500->dev, "failed to write reg %#x: %d\n",
-                        addr, ret);
+                data = (u8)ret;
+                data = (~bitmask & data) | (bitmask & bitvalues);
+
+                ret = ab8500->write(ab8500, addr, data);
+                if (ret < 0)
+                        dev_err(ab8500->dev, "failed to write reg %#x: %d\n",
+                                addr, ret);
 
-        dev_vdbg(ab8500->dev, "mask: addr %#x => data %#x\n", addr, data);
+                dev_vdbg(ab8500->dev, "mask: addr %#x => data %#x\n", addr,
+                        data);
+                goto out;
+        }
+        ret = ab8500->write_masked(ab8500, addr, bitmask, bitvalues);
+        if (ret < 0)
+                dev_err(ab8500->dev, "failed to modify reg %#x: %d\n", addr,
+                        ret);
 out:
         mutex_unlock(&ab8500->lock);
         return ret;