[PATCH v2 2/2] HID: amd_sfh: Cleanup all sensors when init fails

[Mario Limonciello <superm1@kernel.org>]

From: Mario Limonciello <mario.limonciello@amd.com>

If any sensors have been started when a failure occurs during client
init they are left enabled by the firmware.

As part of the cleanup routine check all sensors and if they're enabled
stop them.

Suggested-by: Basavaraj Natikar <Basavaraj.Natikar@amd.com>
Signed-off-by: Mario Limonciello <mario.limonciello@amd.com>
---
v2:
 * New patch
---
 drivers/hid/amd-sfh-hid/sfh1_1/amd_sfh_init.c | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/drivers/hid/amd-sfh-hid/sfh1_1/amd_sfh_init.c b/drivers/hid/amd-sfh-hid/sfh1_1/amd_sfh_init.c
index c1bdf1e0d44af..c61cc42fa7a33 100644
--- a/drivers/hid/amd-sfh-hid/sfh1_1/amd_sfh_init.c
+++ b/drivers/hid/amd-sfh-hid/sfh1_1/amd_sfh_init.c
@@ -235,12 +235,20 @@ static int amd_sfh1_1_hid_client_init(struct amd_mp2_dev *privdata)
 	return 0;
 
 cleanup:
-	amd_sfh_hid_client_deinit(privdata);
 	for (i = 0; i < cl_data->num_hid_devices; i++) {
+		if (cl_data->sensor_sts[i] == SENSOR_ENABLED) {
+			mp2_ops->stop(privdata, cl_data->sensor_idx[i]);
+			status = amd_sfh_wait_for_response
+					(privdata, cl_data->sensor_idx[i], DISABLE_SENSOR);
+			cl_data->sensor_sts[i] = (status == 0) ? SENSOR_ENABLED : SENSOR_DISABLED;
+		}
+		if (cl_data->sensor_idx[i] == SRA_IDX)
+			continue;
 		devm_kfree(dev, cl_data->feature_report[i]);
 		devm_kfree(dev, in_data->input_report[i]);
 		devm_kfree(dev, cl_data->report_descr[i]);
 	}
+	amd_sfh_hid_client_deinit(privdata);
 	return rc;
 }
 
-- 
2.43.0