1 files changed, 99 insertions, 0 deletions

diff --git a/drivers/hid/hid-rmi.c b/drivers/hid/hid-rmi.c
index 49d4fe4f5987..28579d783155 100644
--- a/drivers/hid/hid-rmi.c
+++ b/drivers/hid/hid-rmi.c
@@ -104,6 +104,7 @@ struct rmi_data {
 
         unsigned long flags;
 
+        struct rmi_function f01;
         struct rmi_function f11;
         struct rmi_function f30;
 
@@ -124,6 +125,7 @@ struct rmi_data {
         struct hid_device *hdev;
 
         unsigned long device_flags;
+        unsigned long firmware_id;
 };
 
 #define RMI_PAGE(addr) (((addr) >> 8) & 0xff)
@@ -532,6 +534,9 @@ static void rmi_register_function(struct rmi_data *data,
         u16 page_base = page << 8;
 
         switch (pdt_entry->function_number) {
+        case 0x01:
+                f = &data->f01;
+                break;
         case 0x11:
                 f = &data->f11;
                 break;
@@ -604,6 +609,92 @@ error_exit:
         return retval;
 }
 
+#define RMI_DEVICE_F01_BASIC_QUERY_LEN  11
+
+static int rmi_populate_f01(struct hid_device *hdev)
+{
+        struct rmi_data *data = hid_get_drvdata(hdev);
+        u8 basic_queries[RMI_DEVICE_F01_BASIC_QUERY_LEN];
+        u8 info[3];
+        int ret;
+        bool has_query42;
+        bool has_lts;
+        bool has_sensor_id;
+        bool has_ds4_queries = false;
+        bool has_build_id_query = false;
+        bool has_package_id_query = false;
+        u16 query_offset = data->f01.query_base_addr;
+        u16 prod_info_addr;
+        u8 ds4_query_len;
+
+        ret = rmi_read_block(hdev, query_offset, basic_queries,
+                                RMI_DEVICE_F01_BASIC_QUERY_LEN);
+        if (ret) {
+                hid_err(hdev, "Can not read basic queries from Function 0x1.\n");
+                return ret;
+        }
+
+        has_lts = !!(basic_queries[0] & BIT(2));
+        has_sensor_id = !!(basic_queries[1] & BIT(3));
+        has_query42 = !!(basic_queries[1] & BIT(7));
+
+        query_offset += 11;
+        prod_info_addr = query_offset + 6;
+        query_offset += 10;
+
+        if (has_lts)
+                query_offset += 20;
+
+        if (has_sensor_id)
+                query_offset++;
+
+        if (has_query42) {
+                ret = rmi_read(hdev, query_offset, info);
+                if (ret) {
+                        hid_err(hdev, "Can not read query42.\n");
+                        return ret;
+                }
+                has_ds4_queries = !!(info[0] & BIT(0));
+                query_offset++;
+        }
+
+        if (has_ds4_queries) {
+                ret = rmi_read(hdev, query_offset, &ds4_query_len);
+                if (ret) {
+                        hid_err(hdev, "Can not read DS4 Query length.\n");
+                        return ret;
+                }
+                query_offset++;
+
+                if (ds4_query_len > 0) {
+                        ret = rmi_read(hdev, query_offset, info);
+                        if (ret) {
+                                hid_err(hdev, "Can not read DS4 query.\n");
+                                return ret;
+                        }
+
+                        has_package_id_query = !!(info[0] & BIT(0));
+                        has_build_id_query = !!(info[0] & BIT(1));
+                }
+        }
+
+        if (has_package_id_query)
+                prod_info_addr++;
+
+        if (has_build_id_query) {
+                ret = rmi_read_block(hdev, prod_info_addr, info, 3);
+                if (ret) {
+                        hid_err(hdev, "Can not read product info.\n");
+                        return ret;
+                }
+
+                data->firmware_id = info[1] << 8 | info[0];
+                data->firmware_id += info[2] * 65536;
+        }
+
+        return 0;
+}
+
 static int rmi_populate_f11(struct hid_device *hdev)
 {
         struct rmi_data *data = hid_get_drvdata(hdev);
@@ -858,6 +949,12 @@ static int rmi_populate(struct hid_device *hdev)
                 return ret;
         }
 
+        ret = rmi_populate_f01(hdev);
+        if (ret) {
+                hid_err(hdev, "Error while initializing F01 (%d).\n", ret);
+                return ret;
+        }
+
         ret = rmi_populate_f11(hdev);
         if (ret) {
                 hid_err(hdev, "Error while initializing F11 (%d).\n", ret);
@@ -907,6 +1004,8 @@ static void rmi_input_configured(struct hid_device *hdev, struct hid_input *hi)
         if (ret)
                 goto exit;
 
+        hid_info(hdev, "firmware id: %ld\n", data->firmware_id);
+
         __set_bit(EV_ABS, input->evbit);
         input_set_abs_params(input, ABS_MT_POSITION_X, 1, data->max_x, 0, 0);
         input_set_abs_params(input, ABS_MT_POSITION_Y, 1, data->max_y, 0, 0);