mfd:rk616: vif:add sync mode

	       core:add write_dev_bits interface

1 files changed, 36 insertions, 0 deletions

diff --git a/drivers/mfd/rk616-core.c b/drivers/mfd/rk616-core.c
index 97d1b662e8f5..376ecb07b990 100755
--- a/drivers/mfd/rk616-core.c
+++ b/drivers/mfd/rk616-core.c
@@ -94,6 +94,40 @@ static int rk616_i2c_write_reg(struct mfd_rk616 *rk616, u16 reg,u32 *pval)
 }
 
 
+static int rk616_i2c_write_bits(struct mfd_rk616 *rk616, u16 reg,u32 mask,u32 *pval)
+{
+	
+	struct i2c_client *client = rk616->client;
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_msg msg;
+	int ret;
+	u32 reg_val;
+	char *tx_buf = NULL;
+	mutex_lock(&rk616->reg_lock);
+	tx_buf = (char *)kmalloc(6, GFP_KERNEL);
+	if(!tx_buf)
+		return -ENOMEM;
+	
+	rk616->read_dev(rk616,reg,&reg_val);
+	reg_val &= ~mask;
+	reg_val |= *pval;
+	*pval = reg_val;
+	memcpy(tx_buf, &reg, 2); 
+	memcpy(tx_buf+2, (char *)pval, 4); 
+
+	msg.addr = client->addr;
+	msg.flags = client->flags;
+	msg.len = 6;
+	msg.buf = (char *)tx_buf;
+	msg.scl_rate = rk616->pdata->scl_rate;
+	msg.udelay = client->udelay;
+
+	ret = i2c_transfer(adap, &msg, 1);
+	kfree(tx_buf);
+	mutex_unlock(&rk616->reg_lock);
+	
+	return (ret == 1) ? 4 : ret;
+}
 #if defined(CONFIG_DEBUG_FS)
 static int rk616_reg_show(struct seq_file *s, void *v)
 {
@@ -444,12 +478,14 @@ static int rk616_i2c_probe(struct i2c_client *client,const struct i2c_device_id
 		//clk_put(iis_clk);
 	}
 
+	mutex_init(&rk616->reg_lock);
 	
 	if(rk616->pdata->power_init)
 		rk616->pdata->power_init();
 	
 	rk616->read_dev = rk616_i2c_read_reg;
 	rk616->write_dev = rk616_i2c_write_reg;
+	rk616->write_dev_bits = rk616_i2c_write_bits;
 	
 #if defined(CONFIG_DEBUG_FS)
 	rk616->debugfs_dir = debugfs_create_dir("rk616", NULL);