From e2517977596b7faff87f410b1b254676a0941383 Mon Sep 17 00:00:00 2001
From: Grazvydas Ignotas <notasas@gmail.com>
Date: Mon, 16 Feb 2015 21:46:47 +0200
Subject: [PATCH] mfd: twl-core: add atomic read-modify-write function

---
 drivers/mfd/twl-core.c  | 82 +++++++++++++++++++++++++++++++++++++++++
 include/linux/i2c/twl.h |  1 +
 2 files changed, 83 insertions(+)

diff --git a/drivers/mfd/twl-core.c b/drivers/mfd/twl-core.c
index eb714fd498ed..5182f6d8e761 100644
--- a/drivers/mfd/twl-core.c
+++ b/drivers/mfd/twl-core.c
@@ -458,6 +458,88 @@ int twl_i2c_read(u8 mod_no, u8 *value, u8 reg, unsigned num_bytes)
 }
 EXPORT_SYMBOL(twl_i2c_read);
 
+/**
+ * twl_i2c_rmw_u8 - Reads a 8 bit register, modifies it and writes back
+ * @mod_no: module number
+ * @bits_to_clear
+ * @bits_to_set
+ * @reg: register address (just offset will do)
+ *
+ * Returns result of operation - 0 is success else failure.
+ */
+int twl_i2c_rmw_u8(u8 mod_no, u8 bits_to_clear, u8 bits_to_set, u8 reg)
+{
+	u8 value_w[2] = { 0 };
+	u8 value = 0;
+	int ret;
+	u8 val;
+	int sid;
+	struct twl_client *twl;
+	struct i2c_msg *msg;
+
+	if (unlikely(mod_no > TWL_MODULE_LAST)) {
+		pr_err("%s: invalid module number %d\n", DRIVER_NAME, mod_no);
+		return -EPERM;
+	}
+	if (unlikely(!inuse)) {
+		pr_err("%s: not initialized\n", DRIVER_NAME);
+		return -EPERM;
+	}
+	sid = twl_map[mod_no].sid;
+	twl = &twl_modules[sid];
+
+	mutex_lock(&twl->xfer_lock);
+	/* [MSG1] fill the register address data */
+	msg = &twl->xfer_msg[0];
+	msg->addr = twl->address;
+	msg->len = 1;
+	msg->flags = 0;	/* Read the register value */
+	val = twl_map[mod_no].base + reg;
+	msg->buf = &val;
+	/* [MSG2] fill the data rx buffer */
+	msg = &twl->xfer_msg[1];
+	msg->addr = twl->address;
+	msg->flags = I2C_M_RD;	/* Read the register value */
+	msg->len = 1;
+	msg->buf = &value;
+	ret = i2c_transfer(twl->client->adapter, twl->xfer_msg, 2);
+	/* i2c_transfer returns number of messages transferred */
+	if (ret != 2) {
+		pr_err("%s: i2c_read failed to transfer all messages\n",
+			DRIVER_NAME);
+		if (ret >= 0)
+			ret = -EIO;
+		goto out;
+	}
+
+	value &= ~bits_to_clear;
+	value |= bits_to_set;
+
+	value_w[0] = twl_map[mod_no].base + reg;
+	value_w[1] = value;
+
+	msg = &twl->xfer_msg[0];
+	msg->addr = twl->address;
+	msg->len = 2;
+	msg->flags = 0;
+	msg->buf = value_w;
+	ret = i2c_transfer(twl->client->adapter, twl->xfer_msg, 1);
+	/* i2c_transfer returns number of messages transferred */
+	if (ret != 1) {
+		pr_err("%s: i2c_write failed to transfer all messages\n",
+			DRIVER_NAME);
+		if (ret >= 0)
+			ret = -EIO;
+		goto out;
+	}
+
+	ret = 0;
+out:
+	mutex_unlock(&twl->xfer_lock);
+	return ret;
+}
+EXPORT_SYMBOL(twl_i2c_rmw_u8);
+
 /**
  * twl_i2c_write_u8 - Writes a 8 bit register in TWL4030/TWL5030/TWL60X0
  * @mod_no: module number
diff --git a/include/linux/i2c/twl.h b/include/linux/i2c/twl.h
index 99bc7bf3b491..ca0aeec4ccbf 100644
--- a/include/linux/i2c/twl.h
+++ b/include/linux/i2c/twl.h
@@ -178,6 +178,7 @@ TWL_CLASS_IS(6030, TWL6030_CLASS_ID)
  */
 int twl_i2c_write_u8(u8 mod_no, u8 val, u8 reg);
 int twl_i2c_read_u8(u8 mod_no, u8 *val, u8 reg);
+int twl_i2c_rmw_u8(u8 mod_no, u8 bits_to_clear, u8 bits_to_set, u8 reg);
 
 /*
  * Read and write several 8-bit registers at once.
-- 
2.39.2