1 files changed, 18 insertions, 5 deletions

diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index a68b2279e8df..dd2577b796a4 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -431,17 +431,24 @@ static int mxt_read_reg(struct i2c_client *client, u16 reg, u8 *val)
         return __mxt_read_reg(client, reg, 1, val);
 }
 
-static int mxt_write_reg(struct i2c_client *client, u16 reg, u8 val)
+static int __mxt_write_reg(struct i2c_client *client, u16 reg, u16 len,
+                           const void *val)
 {
-        u8 buf[3];
+        u8 *buf;
+        size_t count;
         int ret;
 
+        count = len + 2;
+        buf = kmalloc(count, GFP_KERNEL);
+        if (!buf)
+                return -ENOMEM;
+
         buf[0] = reg & 0xff;
         buf[1] = (reg >> 8) & 0xff;
-        buf[2] = val;
+        memcpy(&buf[2], val, len);
 
-        ret = i2c_master_send(client, buf, 3);
-        if (ret == 3) {
+        ret = i2c_master_send(client, buf, count);
+        if (ret == count) {
                 ret = 0;
         } else {
                 if (ret >= 0)
@@ -450,9 +457,15 @@ static int mxt_write_reg(struct i2c_client *client, u16 reg, u8 val)
                         __func__, ret);
         }
 
+        kfree(buf);
         return ret;
 }
 
+static int mxt_write_reg(struct i2c_client *client, u16 reg, u8 val)
+{
+        return __mxt_write_reg(client, reg, 1, &val);
+}
+
 static int mxt_read_object_table(struct i2c_client *client,
                                       u16 reg, u8 *object_buf)
 {