techpack: camera: Correct the offset for Gyro calibrationandroid-s-preview-1_r0.5android-msm-redbull-4.19-s-preview-1

Bug: 175363337
Test: build pass, local verified
Change-Id: If098a6649c3eb4da397e437ab16de1740803d0ea
Signed-off-by: horngchuang <horngchuang@google.com>

5 files changed, 110 insertions, 1 deletions

diff --git a/drivers/cam_sensor_module/cam_fw_update/fw_update.c b/drivers/cam_sensor_module/cam_fw_update/fw_update.c
index f68bbc7..b0ddd89 100644
--- a/drivers/cam_sensor_module/cam_fw_update/fw_update.c
+++ b/drivers/cam_sensor_module/cam_fw_update/fw_update.c
@@ -375,5 +375,88 @@ int getFWVersion(struct cam_sensor_ctrl_t *s_ctrl)
 }
 EXPORT_SYMBOL_GPL(getFWVersion);
 
+int GyroOffsetCorrect(struct camera_io_master *io_master_info, uint32_t gyro_correct_index)
+{
+	int rc = 0;
+	uint32_t AddrGyroZ = 0;
+	uint32_t AddrAccelX = 0;
+	uint32_t AddrAccelY = 0;
+	uint32_t AddrAccelZ = 0;
+	uint32_t UlReadVal = 0;
+
+	g_io_master_info = io_master_info;
+	if (g_io_master_info == NULL)
+		return -EINVAL;
+
+	CAM_INFO(CAM_SENSOR,
+		"[OISFW]:%s gyro_correct_index = %d\n",
+			__func__, gyro_correct_index);
+
+	if (gyro_correct_index > 0) {
+		// Program Memory to read
+		if (gyro_correct_index == 1) {
+			AddrGyroZ = 0x0008469C;
+			AddrAccelX = 0x00084698;
+			AddrAccelY = 0x0008469A;
+			AddrAccelZ = 0x0008469C;
+		} else if (gyro_correct_index == 2)  {
+			AddrGyroZ = 0x00084634;
+			AddrAccelX = 0x00084630;
+			AddrAccelY = 0x00084632;
+			AddrAccelZ = 0x00084634;
+		} else {
+			CAM_INFO(CAM_SENSOR,
+				"[OISFW]:%s index is not supported : %d\n",
+					__func__, gyro_correct_index);
+			return -EINVAL;
+		}
+	} else {
+		CAM_INFO(CAM_SENSOR,
+			"[OISFW]:%s index is out of range : %d\n",
+				__func__, gyro_correct_index);
+		return -EINVAL;
+	}
+
+	rc = checkHighLevelCommand(20);
+	if (rc != 0) {
+		CAM_ERR(CAM_SENSOR,
+			"[OISFW]:%s checkHighLevelCommand failed = %d\n",
+			__func__, rc);
+		return -EINVAL;
+	} else {
+		// Gyro Offset Z
+		RamWrite32A(0x3000, AddrGyroZ);
+		RamRead32A(0x4000, &UlReadVal);
+		UlReadVal = UlReadVal & 0xFFFF;
+		UlReadVal = ((UlReadVal << 8) & 0xff00) | ((UlReadVal >> 8) & 0x00ff);
+		RamWrite32A(0x03A8, UlReadVal);
+
+		// Accel Offset X
+		RamWrite32A(0x3000, AddrAccelX);
+		RamRead32A(0x4000, &UlReadVal);
+		UlReadVal = (UlReadVal & 0xFFFF0000) >> 16;
+		UlReadVal = ((UlReadVal << 8) & 0xff00) | ((UlReadVal >> 8) & 0x00ff);
+		RamWrite32A(0x0454, UlReadVal);
+
+		// Accel Offset Y
+		RamWrite32A(0x3000, AddrAccelY);
+		RamRead32A(0x4000, &UlReadVal);
+		UlReadVal = (UlReadVal & 0xFFFF0000) >> 16;
+		UlReadVal = ((UlReadVal << 8) & 0xff00) | ((UlReadVal >> 8) & 0x00ff);
+		RamWrite32A(0x0480, UlReadVal);
+
+		// Accel Offset Z
+		RamWrite32A(0x3000, AddrAccelZ);
+		RamRead32A(0x4000, &UlReadVal);
+		UlReadVal = (UlReadVal & 0xFFFF0000) >> 16;
+		UlReadVal = ((UlReadVal << 8) & 0xff00) | ((UlReadVal >> 8) & 0x00ff);
+		RamWrite32A(0x04AC, UlReadVal);
+	}
+
+	return rc;
+}
+EXPORT_SYMBOL_GPL(GyroOffsetCorrect);
+
+
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("Fw Update");
diff --git a/drivers/cam_sensor_module/cam_fw_update/fw_update.h b/drivers/cam_sensor_module/cam_fw_update/fw_update.h
index c8a1248..0872ef4 100644
--- a/drivers/cam_sensor_module/cam_fw_update/fw_update.h
+++ b/drivers/cam_sensor_module/cam_fw_update/fw_update.h
@@ -16,4 +16,6 @@ int GyroReCalibrate(struct camera_io_master *io_master_info,
 	stReCalib *cal_result);
 int WrGyroOffsetData(struct camera_io_master *io_master_info,
 	stReCalib *cal_result);
-int getFWVersion(struct cam_sensor_ctrl_t *s_ctrl);
\ No newline at end of file
+int getFWVersion(struct cam_sensor_ctrl_t *s_ctrl);
+int GyroOffsetCorrect(struct camera_io_master *io_master_info,
+	uint32_t gyro_correct_index);
diff --git a/drivers/cam_sensor_module/cam_ois/cam_ois_core.c b/drivers/cam_sensor_module/cam_ois/cam_ois_core.c
index d286ea9..c7d141f 100644
--- a/drivers/cam_sensor_module/cam_ois/cam_ois_core.c
+++ b/drivers/cam_sensor_module/cam_ois/cam_ois_core.c
@@ -952,6 +952,10 @@ static int cam_ois_pkt_parse(struct cam_ois_ctrl_t *o_ctrl, void *arg)
 			}
 		}
 
+		// To correct the gyro offset data
+		if (o_ctrl->gyro_correct_enable)
+			GyroOffsetCorrect(&o_ctrl->io_master_info, o_ctrl->gyro_correct_index);
+
 		rc = cam_ois_apply_settings(o_ctrl, &o_ctrl->i2c_init_data);
 		if ((rc == -EAGAIN) &&
 			(o_ctrl->io_master_info.master_type == CCI_MASTER)) {
diff --git a/drivers/cam_sensor_module/cam_ois/cam_ois_dev.h b/drivers/cam_sensor_module/cam_ois/cam_ois_dev.h
index e46ee03..858d991 100644
--- a/drivers/cam_sensor_module/cam_ois/cam_ois_dev.h
+++ b/drivers/cam_sensor_module/cam_ois/cam_ois_dev.h
@@ -173,6 +173,8 @@ struct cam_ois_ctrl_t {
 	struct mutex ois_shift_mutex;
 	struct kthread_worker worker;
 	struct task_struct *worker_thread;
+	uint32_t gyro_correct_enable;
+	uint32_t gyro_correct_index;
 };
 
 #endif /*_CAM_OIS_DEV_H_ */
diff --git a/drivers/cam_sensor_module/cam_ois/cam_ois_soc.c b/drivers/cam_sensor_module/cam_ois/cam_ois_soc.c
index 9c14d87..8d6f074 100644
--- a/drivers/cam_sensor_module/cam_ois/cam_ois_soc.c
+++ b/drivers/cam_sensor_module/cam_ois/cam_ois_soc.c
@@ -41,6 +41,24 @@ static int cam_ois_get_dt_data(struct cam_ois_ctrl_t *o_ctrl)
 		return rc;
 	}
 
+	if (of_property_read_u32(of_node, "gyro-correct-enable",
+		&o_ctrl->gyro_correct_enable) < 0) {
+		/* Set default gyro-correct-enable */
+		CAM_INFO(CAM_OIS,
+			"failed to parse gyro-correct-enable, set to default");
+		o_ctrl->gyro_correct_enable = 0;
+	}
+	CAM_INFO(CAM_OIS, "gyro-correct-enable %d", o_ctrl->gyro_correct_enable);
+
+	if (of_property_read_u32(of_node, "gyro-correct-index",
+		&o_ctrl->gyro_correct_index) < 0) {
+		/* Set default gyro-correct-index */
+		CAM_INFO(CAM_OIS,
+			"failed to parse gyro-correct-index, set to default");
+		o_ctrl->gyro_correct_index = 0;
+	}
+	CAM_INFO(CAM_OIS, "gyro-correct-index %d", o_ctrl->gyro_correct_index);
+
 	if (!soc_info->gpio_data) {
 		CAM_INFO(CAM_OIS, "No GPIO found");
 		return 0;