From 001fe13573dd64ca77421b95cb4bc66c21c1aa98 Mon Sep 17 00:00:00 2001
From: wangdp11 <wangdp11@motorola.com>
Date: Mon, 24 Jul 2023 10:19:48 +0800
Subject: [PATCH] ctwo: Solve TOF phio ioctl timeout

Detail:
Wait 4*timing_budget when first frame happen to
avoid timeout.

Change-Id: Ie3fc9762778aa8e4cb808411f828a5f8345def57
Signed-off-by: wangdp11 <wangdp11@motorola.com>
Reviewed-on: https://gerrit.mot.com/2685802
SLTApproved: Slta Waiver
SME-Granted: SME Approvals Granted
Tested-by: Jira Key
Reviewed-by: Wang Liu <liuwang2@lenovo.com>
Reviewed-by: <liuzy55@motorola.com>
Reviewed-by: Zhichao Chen <chenzc2@motorola.com>
Reviewed-by: Zhenxin Xi <xizx@motorola.com>
Submit-Approved: Jira Key
---
 drivers/input/misc/vl53L1_14_1_2/Android.mk   |  4 ++++
 drivers/input/misc/vl53L1_14_1_2/Kbuild       |  5 ++++-
 drivers/input/misc/vl53L1_14_1_2/stmvl53l1.h  |  5 +++++
 .../misc/vl53L1_14_1_2/stmvl53l1_module.c     | 19 ++++++++++++++++++-
 4 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/drivers/input/misc/vl53L1_14_1_2/Android.mk b/drivers/input/misc/vl53L1_14_1_2/Android.mk
index 65c0d1d456b9..d2c85992bf76 100644
--- a/drivers/input/misc/vl53L1_14_1_2/Android.mk
+++ b/drivers/input/misc/vl53L1_14_1_2/Android.mk
@@ -13,6 +13,10 @@ ifeq ($(TOF_KERNEL_COMPILE_FIX),true)
 	KBUILD_OPTIONS += CONFIG_TOF_COMPILE_FIX=y
 endif
 
+ifeq ($(TOF_PHIO_TIMEOUT_FIX),true)
+	KBUILD_OPTIONS += CONFIG_PHIO_TIMEOUT_FIX=y
+endif
+
 include $(CLEAR_VARS)
 LOCAL_MODULE := stmvl53l3.ko
 LOCAL_MODULE_TAGS := optional
diff --git a/drivers/input/misc/vl53L1_14_1_2/Kbuild b/drivers/input/misc/vl53L1_14_1_2/Kbuild
index a29c5dd146b4..36d80f0664c1 100644
--- a/drivers/input/misc/vl53L1_14_1_2/Kbuild
+++ b/drivers/input/misc/vl53L1_14_1_2/Kbuild
@@ -45,11 +45,14 @@ ifneq ($(filter m y,$(CONFIG_INPUT_MISC_TOF_USE_VL53L3)),)
 	EXTRA_CFLAGS += -DTOF_USE_VL53L3_EN
 endif
 
-
 ifneq ($(filter m y,$(CONFIG_INPUT_MISC_PMIC_WL2864C)),)
 	EXTRA_CFLAGS += -DTOF_USE_PMIC_WL2864C
 endif
 
 ifneq ($(filter m y,$(CONFIG_TOF_COMPILE_FIX)),)
 	EXTRA_CFLAGS += -Wno-unused-but-set-variable
+endif
+
+ifneq ($(filter m y,$(CONFIG_PHIO_TIMEOUT_FIX)),)
+	EXTRA_CFLAGS += -DPHIO_TIMEOUT_FIX
 endif
\ No newline at end of file
diff --git a/drivers/input/misc/vl53L1_14_1_2/stmvl53l1.h b/drivers/input/misc/vl53L1_14_1_2/stmvl53l1.h
index f46079cbaf59..058089c70a29 100644
--- a/drivers/input/misc/vl53L1_14_1_2/stmvl53l1.h
+++ b/drivers/input/misc/vl53L1_14_1_2/stmvl53l1.h
@@ -154,6 +154,11 @@ struct stmvl53l1_data {
 	/* set when first start has be done */
 	int is_first_start_done;
 
+#ifdef PHIO_TIMEOUT_FIX
+	/* set when first frame happen */
+	bool not_first_frame;
+#endif
+
 	/* control data */
 	int poll_mode;	/*!< use poll even if interrupt line present*/
 	int poll_delay_ms;	/*!< rescheduled time use in poll mode  */
diff --git a/drivers/input/misc/vl53L1_14_1_2/stmvl53l1_module.c b/drivers/input/misc/vl53L1_14_1_2/stmvl53l1_module.c
index ebd707b7056f..6c13ab7e10ae 100644
--- a/drivers/input/misc/vl53L1_14_1_2/stmvl53l1_module.c
+++ b/drivers/input/misc/vl53L1_14_1_2/stmvl53l1_module.c
@@ -784,6 +784,9 @@ static int stmvl53l1_start(struct stmvl53l1_data *data)
 	data->is_first_irq = true;
 	data->is_data_valid = false;
 	data->is_xtalk_value_changed = false;
+#ifdef PHIO_TIMEOUT_FIX
+	data->not_first_frame = false;
+#endif
 
 	rc = reset_release(data);
 	if (rc)
@@ -2492,9 +2495,20 @@ static int sleep_for_data_timeout(struct stmvl53l1_data *data, pid_t pid,
 	int rc;
 
 	mutex_unlock(&data->work_mutex);
+#ifdef PHIO_TIMEOUT_FIX
+	if(!data->not_first_frame)
+		rc = wait_event_interruptible_timeout(data->waiter_for_data,
+					sleep_for_data_condition(data, pid, head),
+					usecs_to_jiffies(4 * data->timing_budget));
+	else
+		rc = wait_event_interruptible_timeout(data->waiter_for_data,
+				sleep_for_data_condition(data, pid, head),
+				usecs_to_jiffies(2 * data->timing_budget));
+#else
 	rc = wait_event_interruptible_timeout(data->waiter_for_data,
 				sleep_for_data_condition(data, pid, head),
 				usecs_to_jiffies(2 * data->timing_budget));
+#endif
 	if (rc == 0) //condition evaluated to false after timeout elapsed
 		rc = -EAGAIN;
 	else
@@ -2605,7 +2619,10 @@ static int ctrl_mz_data_blocking_common(struct stmvl53l1_data *data,
 			sizeof(VL53L1_MultiRangingData_t));
 		goto done;
 	}
-
+#ifdef PHIO_TIMEOUT_FIX
+	if (data->meas.multi_range_data.StreamCount == 0)
+		data->not_first_frame = true;
+#endif
 	/* unless we got interrupted we return data to user and note read */
 	rc = copy_to_user(&d->data, &data->meas.multi_range_data,
 		sizeof(VL53L1_MultiRangingData_t));