HID: amd_sfh: Add command response to check command status

Sometimes sensor enable/disable may take time, without checking the
actual status bits from MP2 FW can lead the amd-sfh to misbehave.

Hence add a status check of enable/disable command
by waiting on the command response before sending the next
command to FW.

Reviewed-by: Shyam Sundar S K <Shyam-sundar.S-k@amd.com>
Signed-off-by: Basavaraj Natikar <Basavaraj.Natikar@amd.com>
Signed-off-by: Jiri Kosina <jkosina@suse.cz>

1 files changed, 30 insertions, 10 deletions

diff --git a/drivers/hid/amd-sfh-hid/amd_sfh_client.c b/drivers/hid/amd-sfh-hid/amd_sfh_client.c
index 4710b9aa24a5..b7b66a1eb971 100644
--- a/drivers/hid/amd-sfh-hid/amd_sfh_client.c
+++ b/drivers/hid/amd-sfh-hid/amd_sfh_client.c
@@ -123,14 +123,24 @@ static void amd_sfh_work_buffer(struct work_struct *work)
 	int i;
 
 	for (i = 0; i < cli_data->num_hid_devices; i++) {
-		report_size = get_input_report(i, cli_data->sensor_idx[i], cli_data->report_id[i],
-					       in_data);
-		hid_input_report(cli_data->hid_sensor_hubs[i], HID_INPUT_REPORT,
-				 in_data->input_report[i], report_size, 0);
+		if (cli_data->sensor_sts[i] == SENSOR_ENABLED) {
+			report_size = get_input_report
+				(i, cli_data->sensor_idx[i], cli_data->report_id[i], in_data);
+			hid_input_report(cli_data->hid_sensor_hubs[i], HID_INPUT_REPORT,
+					 in_data->input_report[i], report_size, 0);
+		}
 	}
 	schedule_delayed_work(&cli_data->work_buffer, msecs_to_jiffies(AMD_SFH_IDLE_LOOP));
 }
 
+u32 amd_sfh_wait_for_response(struct amd_mp2_dev *mp2, u8 sid, u32 sensor_sts)
+{
+	if (mp2->mp2_ops->response)
+		sensor_sts = mp2->mp2_ops->response(mp2, sid, sensor_sts);
+
+	return sensor_sts;
+}
+
 int amd_sfh_hid_client_init(struct amd_mp2_dev *privdata)
 {
 	struct amd_input_data *in_data = &privdata->in_data;
@@ -139,8 +149,8 @@ int amd_sfh_hid_client_init(struct amd_mp2_dev *privdata)
 	struct device *dev;
 	u32 feature_report_size;
 	u32 input_report_size;
+	int rc, i, status;
 	u8 cl_idx;
-	int rc, i;
 
 	dev = &privdata->pdev->dev;
 
@@ -155,7 +165,7 @@ int amd_sfh_hid_client_init(struct amd_mp2_dev *privdata)
 		in_data->sensor_virt_addr[i] = dma_alloc_coherent(dev, sizeof(int) * 8,
 								  &cl_data->sensor_dma_addr[i],
 								  GFP_KERNEL);
-		cl_data->sensor_sts[i] = 0;
+		cl_data->sensor_sts[i] = SENSOR_DISABLED;
 		cl_data->sensor_requested_cnt[i] = 0;
 		cl_data->cur_hid_dev = i;
 		cl_idx = cl_data->sensor_idx[i];
@@ -201,7 +211,10 @@ int amd_sfh_hid_client_init(struct amd_mp2_dev *privdata)
 		if (rc)
 			return rc;
 		privdata->mp2_ops->start(privdata, info);
-		cl_data->sensor_sts[i] = 1;
+		status = amd_sfh_wait_for_response
+				(privdata, cl_data->sensor_idx[i], SENSOR_ENABLED);
+		if (status == SENSOR_ENABLED)
+			cl_data->sensor_sts[i] = SENSOR_ENABLED;
 	}
 	schedule_delayed_work(&cl_data->work_buffer, msecs_to_jiffies(AMD_SFH_IDLE_LOOP));
 	return 0;
@@ -224,10 +237,17 @@ int amd_sfh_hid_client_deinit(struct amd_mp2_dev *privdata)
 {
 	struct amdtp_cl_data *cl_data = privdata->cl_data;
 	struct amd_input_data *in_data = cl_data->in_data;
-	int i;
+	int i, status;
 
-	for (i = 0; i < cl_data->num_hid_devices; i++)
-		privdata->mp2_ops->stop(privdata, i);
+	for (i = 0; i < cl_data->num_hid_devices; i++) {
+		if (cl_data->sensor_sts[i] == SENSOR_ENABLED) {
+			privdata->mp2_ops->stop(privdata, cl_data->sensor_idx[i]);
+			status = amd_sfh_wait_for_response
+					(privdata, cl_data->sensor_idx[i], SENSOR_DISABLED);
+			if (status != SENSOR_ENABLED)
+				cl_data->sensor_sts[i] = SENSOR_DISABLED;
+		}
+	}
 
 	cancel_delayed_work_sync(&cl_data->work);
 	cancel_delayed_work_sync(&cl_data->work_buffer);