HID: amd_sfh: Add support for sensor discovery

commit b5d7f43e97dabfa04a4be5ff027ce7da119332be upstream.

Sensor discovery status fails in case of broken sensors or
platform not supported. Hence disable driver on failure
of sensor discovery.

Signed-off-by: Mario Limonciello <mario.limonciello@amd.com>
Signed-off-by: Basavaraj Natikar <Basavaraj.Natikar@amd.com>
Signed-off-by: Jiri Kosina <jkosina@suse.cz>
Cc: Mario Limonciello <Mario.Limonciello@amd.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/hid/amd-sfh-hid/amd_sfh_client.c b/drivers/hid/amd-sfh-hid/amd_sfh_client.c
index 840fd075c56f11b2fe103222b4f1cd6e402b382a..6284db50ec9bf708fb20ea94b4b8967ed09e2f27 100644 (file)
--- a/drivers/hid/amd-sfh-hid/amd_sfh_client.c
+++ b/drivers/hid/amd-sfh-hid/amd_sfh_client.c
@@ -226,6 +226,17 @@ int amd_sfh_hid_client_init(struct amd_mp2_dev *privdata)
                dev_dbg(dev, "sid 0x%x status 0x%x\n",
                        cl_data->sensor_idx[i], cl_data->sensor_sts[i]);
        }
+       if (privdata->mp2_ops->discovery_status &&
+           privdata->mp2_ops->discovery_status(privdata) == 0) {
+               amd_sfh_hid_client_deinit(privdata);
+               for (i = 0; i < cl_data->num_hid_devices; i++) {
+                       devm_kfree(dev, cl_data->feature_report[i]);
+                       devm_kfree(dev, in_data->input_report[i]);
+                       devm_kfree(dev, cl_data->report_descr[i]);
+               }
+               dev_warn(dev, "Failed to discover, sensors not enabled\n");
+               return -EOPNOTSUPP;
+       }
        schedule_delayed_work(&cl_data->work_buffer, msecs_to_jiffies(AMD_SFH_IDLE_LOOP));
        return 0;
 

diff --git a/drivers/hid/amd-sfh-hid/amd_sfh_pcie.c b/drivers/hid/amd-sfh-hid/amd_sfh_pcie.c
index 561bb27f42b1019a860a8521f499342ee36cd2f4..ae8f1f2536e9492637b61db7d8a4457a0c683204 100644 (file)
--- a/drivers/hid/amd-sfh-hid/amd_sfh_pcie.c
+++ b/drivers/hid/amd-sfh-hid/amd_sfh_pcie.c
@@ -126,6 +126,12 @@ static int amd_sfh_irq_init_v2(struct amd_mp2_dev *privdata)
        return 0;
 }
 
+static int amd_sfh_dis_sts_v2(struct amd_mp2_dev *privdata)
+{
+       return (readl(privdata->mmio + AMD_P2C_MSG(1)) &
+                     SENSOR_DISCOVERY_STATUS_MASK) >> SENSOR_DISCOVERY_STATUS_SHIFT;
+}
+
 void amd_start_sensor(struct amd_mp2_dev *privdata, struct amd_mp2_sensor_info info)
 {
        union sfh_cmd_param cmd_param;
@@ -241,6 +247,7 @@ static const struct amd_mp2_ops amd_sfh_ops_v2 = {
        .response = amd_sfh_wait_response_v2,
        .clear_intr = amd_sfh_clear_intr_v2,
        .init_intr = amd_sfh_irq_init_v2,
+       .discovery_status = amd_sfh_dis_sts_v2,
 };
 
 static const struct amd_mp2_ops amd_sfh_ops = {

diff --git a/drivers/hid/amd-sfh-hid/amd_sfh_pcie.h b/drivers/hid/amd-sfh-hid/amd_sfh_pcie.h
index 00fc083dc12391a3e3fcd893193ae909747ac1ba..2d3203d3daeb33b7829bcb923ae2ea064d41d870 100644 (file)
--- a/drivers/hid/amd-sfh-hid/amd_sfh_pcie.h
+++ b/drivers/hid/amd-sfh-hid/amd_sfh_pcie.h
@@ -38,6 +38,9 @@
 
 #define AMD_SFH_IDLE_LOOP      200
 
+#define SENSOR_DISCOVERY_STATUS_MASK           GENMASK(5, 3)
+#define SENSOR_DISCOVERY_STATUS_SHIFT          3
+
 /* SFH Command register */
 union sfh_cmd_base {
        u32 ul;
@@ -142,5 +145,6 @@ struct amd_mp2_ops {
         int (*response)(struct amd_mp2_dev *mp2, u8 sid, u32 sensor_sts);
         void (*clear_intr)(struct amd_mp2_dev *privdata);
         int (*init_intr)(struct amd_mp2_dev *privdata);
+        int (*discovery_status)(struct amd_mp2_dev *privdata);
 };
 #endif