HID: i2c-hid: fix GET/SET_REPORT for unnumbered reports

[platform/kernel/linux-starfive.git] / drivers / hid / i2c-hid / i2c-hid-core.c

diff --git a/drivers/hid/i2c-hid/i2c-hid-core.c b/drivers/hid/i2c-hid/i2c-hid-core.c
index 899441b..748619e 100644 (file)
--- a/drivers/hid/i2c-hid/i2c-hid-core.c
+++ b/drivers/hid/i2c-hid/i2c-hid-core.c
@@ -614,6 +614,17 @@ static int i2c_hid_get_raw_report(struct hid_device *hid,
        if (report_type == HID_OUTPUT_REPORT)
                return -EINVAL;
 
+       /*
+        * In case of unnumbered reports the response from the device will
+        * not have the report ID that the upper layers expect, so we need
+        * to stash it the buffer ourselves and adjust the data size.
+        */
+       if (!report_number) {
+               buf[0] = 0;
+               buf++;
+               count--;
+       }
+
        /* +2 bytes to include the size of the reply in the query buffer */
        ask_count = min(count + 2, (size_t)ihid->bufsize);
 
@@ -635,6 +646,9 @@ static int i2c_hid_get_raw_report(struct hid_device *hid,
        count = min(count, ret_count - 2);
        memcpy(buf, ihid->rawbuf + 2, count);
 
+       if (!report_number)
+               count++;
+
        return count;
 }
 
@@ -651,17 +665,19 @@ static int i2c_hid_output_raw_report(struct hid_device *hid, __u8 *buf,
 
        mutex_lock(&ihid->reset_lock);
 
-       if (report_id) {
-               buf++;
-               count--;
-       }
-
+       /*
+        * Note that both numbered and unnumbered reports passed here
+        * are supposed to have report ID stored in the 1st byte of the
+        * buffer, so we strip it off unconditionally before passing payload
+        * to i2c_hid_set_or_send_report which takes care of encoding
+        * everything properly.
+        */
        ret = i2c_hid_set_or_send_report(client,
                                report_type == HID_FEATURE_REPORT ? 0x03 : 0x02,
-                               report_id, buf, count, use_data);
+                               report_id, buf + 1, count - 1, use_data);
 
-       if (report_id && ret >= 0)
-               ret++; /* add report_id to the number of transfered bytes */
+       if (ret >= 0)
+               ret++; /* add report_id to the number of transferred bytes */
 
        mutex_unlock(&ihid->reset_lock);