1 files changed, 64 insertions, 20 deletions

diff --git a/drivers/i2c/mc9sdz60.c b/drivers/i2c/mc9sdz60.c
index 4b1068d17e..3580af8852 100644
--- a/drivers/i2c/mc9sdz60.c
+++ b/drivers/i2c/mc9sdz60.c
@@ -26,45 +26,88 @@
 #include <errno.h>
 
 #include <i2c/i2c.h>
-
-#include <asm/byteorder.h>
+#include <i2c/mc9sdz60.h>
 
 #define DRIVERNAME		"mc9sdz60"
 
-struct mc_priv {
-	struct cdev		cdev;
-	struct i2c_client	*client;
-};
-
-#define to_mc_priv(a)		container_of(a, struct mc_priv, cdev)
+#define to_mc9sdz60(a)		container_of(a, struct mc9sdz60, cdev)
 
-static struct mc_priv *mc_dev;
+static struct mc9sdz60 *mc_dev;
 
-struct i2c_client *mc9sdz60_get_client(void)
+struct mc9sdz60 *mc9sdz60_get(void)
 {
 	if (!mc_dev)
 		return NULL;
 
-	return mc_dev->client;
+	return mc_dev;
 }
+EXPORT_SYMBOL(mc9sdz60_get);
 
-static u32 mc_read_reg(struct mc_priv *mc, int reg)
+int mc9sdz60_reg_read(struct mc9sdz60 *mc9sdz60, enum mc9sdz60_reg reg, u8 *val)
 {
-	u8 buf;
+	int ret;
 
-	i2c_read_reg(mc->client, reg, &buf, sizeof(buf));
+	ret = i2c_read_reg(mc9sdz60->client, reg, val, 1);
 
-	return buf;
+	return ret == 1 ? 0 : ret;
 }
+EXPORT_SYMBOL(mc9sdz60_reg_read)
+
+int mc9sdz60_reg_write(struct mc9sdz60 *mc9sdz60, enum mc9sdz60_reg reg, u8 val)
+{
+	int ret;
+
+	ret = i2c_write_reg(mc9sdz60->client, reg, &val, 1);
+
+	return ret == 1 ? 0 : ret;
+}
+EXPORT_SYMBOL(mc9sdz60_reg_write)
+
+int mc9sdz60_set_bits(struct mc9sdz60 *mc9sdz60, enum mc9sdz60_reg reg, u8 mask, u8 val)
+{
+	u8 tmp;
+	int err;
+
+	err = mc9sdz60_reg_read(mc9sdz60, reg, &tmp);
+	tmp = (tmp & ~mask) | val;
+
+	if (!err)
+		err = mc9sdz60_reg_write(mc9sdz60, reg, tmp);
+
+	return err;
+}
+EXPORT_SYMBOL(mc9sdz60_set_bits);
 
 static ssize_t mc_read(struct cdev *cdev, void *_buf, size_t count, ulong offset, ulong flags)
 {
-	struct mc_priv *priv = to_mc_priv(cdev);
-	int i = count;
+	struct mc9sdz60 *mc9sdz60 = to_mc9sdz60(cdev);
 	u8 *buf = _buf;
+	size_t i = count;
+	int err;
+
+	while (i) {
+		err = mc9sdz60_reg_read(mc9sdz60, offset, buf);
+		if (err)
+			return (ssize_t)err;
+		buf++;
+		i--;
+		offset++;
+	}
+
+	return count;
+}
+
+static ssize_t mc_write(struct cdev *cdev, const void *_buf, size_t count, ulong offset, ulong flags)
+{
+	struct mc9sdz60 *mc9sdz60 = to_mc9sdz60(cdev);
+	const u8 *buf = _buf;
+	size_t i = count;
+	int err;
 
 	while (i) {
-		*buf = mc_read_reg(priv, offset);
+		err = mc9sdz60_reg_write(mc9sdz60, offset, *buf);
+		if (err)
+			return (ssize_t)err;
 		buf++;
 		i--;
 		offset++;
@@ -76,6 +119,7 @@ static ssize_t mc_read(struct cdev *cdev, void *_buf, size_t count, ulong offset
 static struct file_operations mc_fops = {
 	.lseek	= dev_lseek_default,
 	.read	= mc_read,
+	.write	= mc_write,
 };
 
 static int mc_probe(struct device_d *dev)
@@ -83,10 +127,10 @@ static int mc_probe(struct device_d *dev)
 	if (mc_dev)
 		return -EBUSY;
 
-	mc_dev = xzalloc(sizeof(struct mc_priv));
+	mc_dev = xzalloc(sizeof(struct mc9sdz60));
 	mc_dev->cdev.name = DRIVERNAME;
 	mc_dev->client = to_i2c_client(dev);
-	mc_dev->cdev.size = 256;
+	mc_dev->cdev.size = 64;		/* 35 known registers */
 	mc_dev->cdev.dev = dev;
 	mc_dev->cdev.ops = &mc_fops;