2 files changed, 16 insertions, 4 deletions

diff --git a/drivers/mfd/twl6040-core.c b/drivers/mfd/twl6040-core.c
index 7dc8c4715001..75987c8ca049 100644
--- a/drivers/mfd/twl6040-core.c
+++ b/drivers/mfd/twl6040-core.c
@@ -34,16 +34,24 @@
 #include <linux/mfd/core.h>
 #include <linux/mfd/twl6040.h>
 
+#define VIBRACTRL_MEMBER(reg) ((reg == TWL6040_REG_VIBCTLL) ? 0 : 1)
+
 int twl6040_reg_read(struct twl6040 *twl6040, unsigned int reg)
 {
         int ret;
         u8 val = 0;
 
         mutex_lock(&twl6040->io_mutex);
-        ret = twl_i2c_read_u8(TWL_MODULE_AUDIO_VOICE, &val, reg);
-        if (ret < 0) {
-                mutex_unlock(&twl6040->io_mutex);
-                return ret;
+        /* Vibra control registers from cache */
+        if (unlikely(reg == TWL6040_REG_VIBCTLL ||
+                     reg == TWL6040_REG_VIBCTLR)) {
+                val = twl6040->vibra_ctrl_cache[VIBRACTRL_MEMBER(reg)];
+        } else {
+                ret = twl_i2c_read_u8(TWL_MODULE_AUDIO_VOICE, &val, reg);
+                if (ret < 0) {
+                        mutex_unlock(&twl6040->io_mutex);
+                        return ret;
+                }
         }
         mutex_unlock(&twl6040->io_mutex);
 
@@ -57,6 +65,9 @@ int twl6040_reg_write(struct twl6040 *twl6040, unsigned int reg, u8 val)
 
         mutex_lock(&twl6040->io_mutex);
         ret = twl_i2c_write_u8(TWL_MODULE_AUDIO_VOICE, val, reg);
+        /* Cache the vibra control registers */
+        if (reg == TWL6040_REG_VIBCTLL || reg == TWL6040_REG_VIBCTLR)
+                twl6040->vibra_ctrl_cache[VIBRACTRL_MEMBER(reg)] = val;
         mutex_unlock(&twl6040->io_mutex);
 
         return ret;
diff --git a/include/linux/mfd/twl6040.h b/include/linux/mfd/twl6040.h
index e6c755db4560..2f8585a4c74b 100644
--- a/include/linux/mfd/twl6040.h
+++ b/include/linux/mfd/twl6040.h
@@ -184,6 +184,7 @@ struct twl6040 {
         int audpwron;
         int power_count;
         int rev;
+        u8 vibra_ctrl_cache[2];
 
         int pll;
         unsigned int sysclk;