[Vivid][SRU][PATCH 2/4] HID: rmi: Add functions for writing to registers

[Phidias Chiang]

From: Andrew Duggan <aduggan at synaptics.com>

BugLink: http://bugs.launchpad.net/bugs/1515503

Writing to registers is needed for setting configuration parameters.

Signed-off-by: Andrew Duggan <aduggan at synaptics.com>
Tested-by: Gabriele Mazzotta <gabriele.mzt at gmail.com>
Signed-off-by: Jiri Kosina <jkosina at suse.cz>
(cherry picked from commit dd8df28459dcad4da5dec94d12801b149a895c36)

Signed-off-by: Phidias Chiang <phidias.chiang at canonical.com>
Reviewed-By: AceLan Kao <acelan.kao at canonical.com>
---
 drivers/hid/hid-rmi.c |   40 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/drivers/hid/hid-rmi.c b/drivers/hid/hid-rmi.c
index 5670c77..7cdf7e9 100644
--- a/drivers/hid/hid-rmi.c
+++ b/drivers/hid/hid-rmi.c
@@ -269,6 +269,46 @@ static inline int rmi_read(struct hid_device *hdev, u16 addr, void *buf)
 	return rmi_read_block(hdev, addr, buf, 1);
 }
 
+static int rmi_write_block(struct hid_device *hdev, u16 addr, void *buf,
+		const int len)
+{
+	struct rmi_data *data = hid_get_drvdata(hdev);
+	int ret;
+
+	mutex_lock(&data->page_mutex);
+
+	if (RMI_PAGE(addr) != data->page) {
+		ret = rmi_set_page(hdev, RMI_PAGE(addr));
+		if (ret < 0)
+			goto exit;
+	}
+
+	data->writeReport[0] = RMI_WRITE_REPORT_ID;
+	data->writeReport[1] = len;
+	data->writeReport[2] = addr & 0xFF;
+	data->writeReport[3] = (addr >> 8) & 0xFF;
+	memcpy(&data->writeReport[4], buf, len);
+
+	ret = rmi_write_report(hdev, data->writeReport,
+					data->output_report_size);
+	if (ret < 0) {
+		dev_err(&hdev->dev,
+			"failed to write request output report (%d)\n",
+			ret);
+		goto exit;
+	}
+	ret = 0;
+
+exit:
+	mutex_unlock(&data->page_mutex);
+	return ret;
+}
+
+static inline int rmi_write(struct hid_device *hdev, u16 addr, void *buf)
+{
+	return rmi_write_block(hdev, addr, buf, 1);
+}
+
 static void rmi_f11_process_touch(struct rmi_data *hdata, int slot,
 		u8 finger_state, u8 *touch_data)
 {
-- 
1.7.9.5