--- /dev/null
+From b5d7f43e97dabfa04a4be5ff027ce7da119332be Mon Sep 17 00:00:00 2001
+From: Basavaraj Natikar <Basavaraj.Natikar@amd.com>
+Date: Mon, 9 May 2022 18:50:20 +0530
+Subject: HID: amd_sfh: Add support for sensor discovery
+
+From: Basavaraj Natikar <Basavaraj.Natikar@amd.com>
+
+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>
+---
+ drivers/hid/amd-sfh-hid/amd_sfh_client.c | 11 +++++++++++
+ drivers/hid/amd-sfh-hid/amd_sfh_pcie.c | 7 +++++++
+ drivers/hid/amd-sfh-hid/amd_sfh_pcie.h | 4 ++++
+ 3 files changed, 22 insertions(+)
+
+--- a/drivers/hid/amd-sfh-hid/amd_sfh_client.c
++++ b/drivers/hid/amd-sfh-hid/amd_sfh_client.c
+@@ -227,6 +227,17 @@ int amd_sfh_hid_client_init(struct amd_m
+ 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;
+
+--- a/drivers/hid/amd-sfh-hid/amd_sfh_pcie.c
++++ b/drivers/hid/amd-sfh-hid/amd_sfh_pcie.c
+@@ -130,6 +130,12 @@ static int amd_sfh_irq_init_v2(struct am
+ 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;
+@@ -245,6 +251,7 @@ static const struct amd_mp2_ops amd_sfh_
+ .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 = {
+--- a/drivers/hid/amd-sfh-hid/amd_sfh_pcie.h
++++ b/drivers/hid/amd-sfh-hid/amd_sfh_pcie.h
+@@ -39,6 +39,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;
+@@ -143,5 +146,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