From 9e89c85b7cd40dc69f9a3e1f51dd1a6ba1e5dfbe Mon Sep 17 00:00:00 2001
From: Benjamin Schaaf <ben.schaaf@gmail.com>
Date: Mon, 22 Nov 2021 23:38:26 +1100
Subject: [PATCH 061/417] media: ov5640: Fix focus commands blocking until
 complete

Previously setting the focus controls would block until the sensor
completed the focus routine.
---
 drivers/media/i2c/ov5640.c | 72 +++++++++++---------------------------
 1 file changed, 21 insertions(+), 51 deletions(-)

diff --git a/drivers/media/i2c/ov5640.c b/drivers/media/i2c/ov5640.c
index a7b93310c..1a1c006a7 100644
--- a/drivers/media/i2c/ov5640.c
+++ b/drivers/media/i2c/ov5640.c
@@ -52,7 +52,7 @@
 #define OV5640_REG_FW_CMD_PARA1		0x3025
 #define OV5640_REG_FW_CMD_PARA2		0x3026
 #define OV5640_REG_FW_CMD_PARA3		0x3027
-#define OV5640_REG_FW_CMD_PARA4		0x3028
+#define OV5640_REG_FW_CMD_STATUS	0x3028
 #define OV5640_REG_FW_STATUS		0x3029
 #define OV5640_REG_SYSTEM_CONTROL1	0x302e
 #define OV5640_REG_SC_PLL_CTRL0		0x3034
@@ -120,6 +120,7 @@
 
 #define OV5640_FW_CMD_TRIGGER_FOCUS	0x03
 #define OV5640_FW_CMD_CONTINUOUS_FOCUS	0x04
+#define OV5640_FW_CMD_PAUSE_AUTO_FOCUS	0x06
 #define OV5640_FW_CMD_GET_FOCUS_RESULT	0x07
 #define OV5640_FW_CMD_RELEASE_FOCUS	0x08
 #define OV5640_FW_CMD_ZONE_CONFIG	0x12
@@ -2035,7 +2036,7 @@ static int ov5640_copy_fw_to_device(struct ov5640_dev *sensor,
 	ov5640_write_reg(sensor, OV5640_REG_FW_CMD_PARA1, 0x00);
 	ov5640_write_reg(sensor, OV5640_REG_FW_CMD_PARA2, 0x00);
 	ov5640_write_reg(sensor, OV5640_REG_FW_CMD_PARA3, 0x00);
-	ov5640_write_reg(sensor, OV5640_REG_FW_CMD_PARA4, 0x00);
+	ov5640_write_reg(sensor, OV5640_REG_FW_CMD_STATUS, 0x00);
 	ov5640_write_reg(sensor, OV5640_REG_FW_STATUS, 0x7f);
 
 	// Start AF MCU
@@ -2063,6 +2064,19 @@ static int ov5640_copy_fw_to_device(struct ov5640_dev *sensor,
 	return -ETIMEDOUT;
 }
 
+static int ov5640_fw_command(struct ov5640_dev *sensor, int command)
+{
+	int ret;
+
+	ret = ov5640_write_reg(sensor, OV5640_REG_FW_CMD_MAIN, command);
+	if(ret)
+		return ret;
+
+	msleep(5);
+
+	return 0;
+}
+
 static int ov5640_af_init(struct ov5640_dev *sensor)
 {
 	struct i2c_client *client = sensor->i2c_client;
@@ -2100,6 +2114,11 @@ static int ov5640_af_init(struct ov5640_dev *sensor)
 
 	// Set lens focus driver on
 	ret = ov5640_write_reg(sensor, OV5640_REG_VCM_CONTROL4, 0x3f);
+	if (ret)
+		return ret;
+
+	// Set the default focus zone
+	ret = ov5640_fw_command(sensor, OV5640_FW_CMD_ZONE_CONFIG);
 	if (ret)
 		return ret;
 	return ret;
@@ -2611,35 +2630,6 @@ static int ov5640_set_framefmt(struct ov5640_dev *sensor,
 			      is_jpeg ? (BIT(5) | BIT(3)) : 0);
 }
 
-static int ov5640_fw_command(struct ov5640_dev *sensor, int command)
-{
-	u8 fw_ack;
-	int i;
-	int ret;
-
-	ret = ov5640_write_reg(sensor, OV5640_REG_FW_CMD_ACK, 0x01);
-	if(ret)
-		return ret;
-	
-	ret = ov5640_write_reg(sensor, OV5640_REG_FW_CMD_MAIN, command);
-	if(ret)
-		return ret;
-
-	for (i = 0; i < 100; i++) {
-		ret = ov5640_read_reg(sensor, OV5640_REG_FW_CMD_ACK, &fw_ack);
-		if (ret)
-			return ret;
-
-		if (fw_ack == 0){
-			return ret;
-		}
-
-		msleep(50);
-	}
-	return -ETIMEDOUT;
-}
-
-
 /*
  * Sensor Controls.
  */
@@ -2775,26 +2765,6 @@ static int ov5640_set_ctrl_focus(struct ov5640_dev *sensor, int command)
 		return 0;
 	}
 
-	if (command == OV5640_FW_CMD_RELEASE_FOCUS) {
-		dev_dbg(&client->dev, "%s: Releasing autofocus\n",
-			__func__);
-		return ov5640_fw_command(sensor, OV5640_FW_CMD_RELEASE_FOCUS);
-	}
-	
-	// Restart zone config
-	ret = ov5640_fw_command(sensor, OV5640_FW_CMD_ZONE_CONFIG);
-	if (ret)
-		return ret;
-
-	// Set default focus zones
-	ret = ov5640_fw_command(sensor, OV5640_FW_CMD_DEFAULT_ZONES);
-	if (ret)
-		return ret;
-
-	dev_dbg(&client->dev, "%s: Triggering autofocus\n",
-		__func__);
-
-	// Start focussing
 	return ov5640_fw_command(sensor, command);
 }
 
-- 
2.35.3