msm: camera: isp: Recover missed SOF timestamp

In case SOF interrupt is not received by the camera driver, we can
recover the missing SOF timestamp for the frame at epoch using the
previous and current SOF timestamps from CSID.

CRs-Fixed: 3067696
Change-Id: I25c8596132b99829d04ea38dbd3d904fc2a302c2
Signed-off-by: Anand Ravi <[email protected]>

drivers/cam_isp/cam_isp_context.c

@@ -667,6 +667,19 @@ end:
 	return rc;
 }
 
+static inline void __cam_isp_ctx_update_sof_ts_util(
+	struct cam_isp_hw_sof_event_data *sof_event_data,
+	struct cam_isp_context *ctx_isp)
+{
+	/* Delayed update, skip if ts is already updated */
+	if (ctx_isp->sof_timestamp_val == sof_event_data->timestamp)
+		return;
+
+	ctx_isp->frame_id++;
+	ctx_isp->sof_timestamp_val = sof_event_data->timestamp;
+	ctx_isp->boot_timestamp = sof_event_data->boot_time;
+}
+
 static int cam_isp_ctx_dump_req(
 	struct cam_isp_ctx_req  *req_isp,
 	uintptr_t                cpu_addr,
@@ -1014,6 +1027,89 @@ static uint64_t __cam_isp_ctx_get_event_ts(uint32_t evt_id, void *evt_data)
 	return ts;
 }
 
+static int __cam_isp_ctx_get_hw_timestamp(struct cam_context *ctx, uint64_t *prev_ts,
+	uint64_t *curr_ts, uint64_t *boot_ts)
+{
+	struct cam_hw_cmd_args hw_cmd_args;
+	struct cam_isp_hw_cmd_args isp_hw_cmd_args;
+	int rc;
+
+	hw_cmd_args.ctxt_to_hw_map = ctx->ctxt_to_hw_map;
+	hw_cmd_args.cmd_type = CAM_HW_MGR_CMD_INTERNAL;
+	hw_cmd_args.u.internal_args = &isp_hw_cmd_args;
+
+	isp_hw_cmd_args.cmd_type = CAM_ISP_HW_MGR_GET_SOF_TS;
+	rc = ctx->hw_mgr_intf->hw_cmd(ctx->ctxt_to_hw_map, &hw_cmd_args);
+	if (rc)
+		return rc;
+
+	if (isp_hw_cmd_args.u.sof_ts.prev >= isp_hw_cmd_args.u.sof_ts.curr) {
+		CAM_ERR(CAM_ISP, "ctx:%u previous timestamp is greater than current timestamp",
+			ctx->ctx_id);
+		return -EINVAL;
+	}
+
+	*prev_ts = isp_hw_cmd_args.u.sof_ts.prev;
+	*curr_ts = isp_hw_cmd_args.u.sof_ts.curr;
+	*boot_ts = isp_hw_cmd_args.u.sof_ts.boot;
+
+	return 0;
+}
+
+static int __cam_isp_ctx_recover_sof_timestamp(struct cam_context *ctx)
+{
+	struct cam_isp_context *ctx_isp = ctx->ctx_priv;
+	uint64_t prev_ts, curr_ts, boot_ts;
+	uint64_t a, b, c;
+	int rc;
+
+	if (ctx_isp->frame_id < 1) {
+		CAM_ERR(CAM_ISP, "ctx:%u Timestamp recovery is not possible for the first frame",
+			ctx->ctx_id);
+		return -EPERM;
+	}
+
+	rc = __cam_isp_ctx_get_hw_timestamp(ctx, &prev_ts, &curr_ts, &boot_ts);
+	if (rc) {
+		CAM_ERR(CAM_ISP, "ctx:%u Failed to get timestamp from HW", ctx->ctx_id);
+		return rc;
+	}
+
+	/**
+	 * If the last received SOF was for frame A and we have missed the SOF for frame B,
+	 * then we need to find out if the hardware is at frame B or C.
+	 *   +-----+-----+-----+
+	 *   |  A  |  B  |  C  |
+	 *   +-----+-----+-----+
+	 */
+	a = ctx_isp->sof_timestamp_val;
+	if (a == prev_ts) {
+		/* Hardware is at frame B */
+		b = curr_ts;
+		CAM_DBG(CAM_ISP, "ctx:%u recovered timestamp (last:0x%llx, curr:0x%llx)",
+			ctx->ctx_id, a, b);
+	} else if (a < prev_ts) {
+		/* Hardware is at frame C */
+		b = prev_ts;
+		c = curr_ts;
+
+		CAM_DBG(CAM_ISP,
+			"ctx:%u recovered timestamp (last:0x%llx, prev:0x%llx, curr:0x%llx)",
+			ctx->ctx_id, a, b, c);
+	} else {
+		/* Hardware is at frame A (which we supposedly missed) */
+		CAM_ERR(CAM_ISP,
+			"ctx:%u erroneous call to SOF recovery (last:0x%llx, prev:0x%llx, curr:0x%llx)",
+			ctx->ctx_id, a, prev_ts, curr_ts);
+		return 0;
+	}
+
+	ctx_isp->boot_timestamp += (b - a);
+	ctx_isp->sof_timestamp_val = b;
+	ctx_isp->frame_id++;
+	return 0;
+}
+
 static void __cam_isp_ctx_send_sof_boot_timestamp(
 	struct cam_isp_context *ctx_isp, uint64_t request_id,
 	uint32_t sof_event_status)
@@ -1110,6 +1206,12 @@ static void __cam_isp_ctx_send_sof_timestamp(
 {
 	struct cam_req_mgr_message   req_msg;
 
+	if (ctx_isp->reported_frame_id == ctx_isp->frame_id) {
+		if (__cam_isp_ctx_recover_sof_timestamp(ctx_isp->base))
+			CAM_WARN(CAM_ISP, "Missed SOF. Unable to recover SOF timestamp.");
+	}
+	ctx_isp->reported_frame_id = ctx_isp->frame_id;
+
 	if ((ctx_isp->v4l2_event_sub_ids & (1 << V4L_EVENT_CAM_REQ_MGR_SOF_UNIFIED_TS))
 		&& !ctx_isp->use_frame_header_ts) {
 		__cam_isp_ctx_send_unified_timestamp(ctx_isp,request_id);
@@ -2401,9 +2503,7 @@ static int __cam_isp_ctx_sof_in_activated_state(
 		return -EINVAL;
 	}
 
-	ctx_isp->frame_id++;
-	ctx_isp->sof_timestamp_val = sof_event_data->timestamp;
-	ctx_isp->boot_timestamp = sof_event_data->boot_time;
+	__cam_isp_ctx_update_sof_ts_util(sof_event_data, ctx_isp);
 
 	__cam_isp_ctx_update_state_monitor_array(ctx_isp,
 		CAM_ISP_STATE_CHANGE_TRIGGER_SOF, request_id);
@@ -2603,9 +2703,7 @@ static int __cam_isp_ctx_sof_in_epoch(struct cam_isp_context *ctx_isp,
 	if (atomic_read(&ctx_isp->apply_in_progress))
 		CAM_INFO(CAM_ISP, "Apply is in progress at the time of SOF");
 
-	ctx_isp->frame_id++;
-	ctx_isp->sof_timestamp_val = sof_event_data->timestamp;
-	ctx_isp->boot_timestamp = sof_event_data->boot_time;
+	__cam_isp_ctx_update_sof_ts_util(sof_event_data, ctx_isp);
 
 	if (list_empty(&ctx->active_req_list))
 		ctx_isp->substate_activated = CAM_ISP_CTX_ACTIVATED_SOF;
@@ -3028,9 +3126,7 @@ static int __cam_isp_ctx_fs2_sof_in_sof_state(
 		return -EINVAL;
 	}
 
-	ctx_isp->frame_id++;
-	ctx_isp->sof_timestamp_val = sof_event_data->timestamp;
-	ctx_isp->boot_timestamp = sof_event_data->boot_time;
+	__cam_isp_ctx_update_sof_ts_util(sof_event_data, ctx_isp);
 
 	CAM_DBG(CAM_ISP, "frame id: %lld time stamp:0x%llx",
 		ctx_isp->frame_id, ctx_isp->sof_timestamp_val);
@@ -4384,9 +4480,7 @@ static int __cam_isp_ctx_rdi_only_sof_in_top_state(
 		return -EINVAL;
 	}
 
-	ctx_isp->frame_id++;
-	ctx_isp->sof_timestamp_val = sof_event_data->timestamp;
-	ctx_isp->boot_timestamp = sof_event_data->boot_time;
+	__cam_isp_ctx_update_sof_ts_util(sof_event_data, ctx_isp);
 
 	CAM_DBG(CAM_ISP, "frame id: %lld time stamp:0x%llx",
 		ctx_isp->frame_id, ctx_isp->sof_timestamp_val);
@@ -4442,9 +4536,8 @@ static int __cam_isp_ctx_rdi_only_sof_in_applied_state(
 		return -EINVAL;
 	}
 
-	ctx_isp->frame_id++;
-	ctx_isp->sof_timestamp_val = sof_event_data->timestamp;
-	ctx_isp->boot_timestamp = sof_event_data->boot_time;
+	__cam_isp_ctx_update_sof_ts_util(sof_event_data, ctx_isp);
+
 	CAM_DBG(CAM_ISP, "frame id: %lld time stamp:0x%llx",
 		ctx_isp->frame_id, ctx_isp->sof_timestamp_val);
 
@@ -4476,9 +4569,8 @@ static int __cam_isp_ctx_rdi_only_sof_in_bubble_applied(
 	__cam_isp_ctx_send_sof_timestamp(ctx_isp, request_id,
 		CAM_REQ_MGR_SOF_EVENT_SUCCESS);
 
-	ctx_isp->frame_id++;
-	ctx_isp->sof_timestamp_val = sof_event_data->timestamp;
-	ctx_isp->boot_timestamp = sof_event_data->boot_time;
+	__cam_isp_ctx_update_sof_ts_util(sof_event_data, ctx_isp);
+
 	CAM_DBG(CAM_ISP, "frame id: %lld time stamp:0x%llx",
 		ctx_isp->frame_id, ctx_isp->sof_timestamp_val);
 
@@ -4565,9 +4657,7 @@ static int __cam_isp_ctx_rdi_only_sof_in_bubble_state(
 		return -EINVAL;
 	}
 
-	ctx_isp->frame_id++;
-	ctx_isp->sof_timestamp_val = sof_event_data->timestamp;
-	ctx_isp->boot_timestamp = sof_event_data->boot_time;
+	__cam_isp_ctx_update_sof_ts_util(sof_event_data, ctx_isp);
 	CAM_DBG(CAM_ISP, "frame id: %lld time stamp:0x%llx",
 		ctx_isp->frame_id, ctx_isp->sof_timestamp_val);
 
@@ -5004,6 +5094,7 @@ static int __cam_isp_ctx_release_hw_in_top_state(struct cam_context *ctx,
 	ctx_isp->frame_id = 0;
 	ctx_isp->active_req_cnt = 0;
 	ctx_isp->reported_req_id = 0;
+	ctx_isp->reported_frame_id = 0;
 	ctx_isp->hw_acquired = false;
 	ctx_isp->init_received = false;
 	ctx_isp->support_consumed_addr = false;
@@ -5073,6 +5164,7 @@ static int __cam_isp_ctx_release_dev_in_top_state(struct cam_context *ctx,
 	ctx_isp->frame_id = 0;
 	ctx_isp->active_req_cnt = 0;
 	ctx_isp->reported_req_id = 0;
+	ctx_isp->reported_frame_id = 0;
 	ctx_isp->hw_acquired = false;
 	ctx_isp->init_received = false;
 	ctx_isp->offline_context = false;
@@ -6088,6 +6180,7 @@ static inline void __cam_isp_context_reset_ctx_params(
 	ctx_isp->boot_timestamp = 0;
 	ctx_isp->active_req_cnt = 0;
 	ctx_isp->reported_req_id = 0;
+	ctx_isp->reported_frame_id = 0;
 	ctx_isp->bubble_frame_cnt = 0;
 	ctx_isp->recovery_req_id = 0;
 }
@@ -6324,6 +6417,7 @@ static int __cam_isp_ctx_stop_dev_in_activated_unlock(
 	ctx_isp->frame_id = 0;
 	ctx_isp->active_req_cnt = 0;
 	ctx_isp->reported_req_id = 0;
+	ctx_isp->reported_frame_id = 0;
 	ctx_isp->last_applied_req_id = 0;
 	ctx_isp->req_info.last_bufdone_req_id = 0;
 	ctx_isp->bubble_frame_cnt = 0;

drivers/cam_isp/cam_isp_context.h

@@ -302,6 +302,7 @@ struct cam_isp_context {
 	uint64_t                         boot_timestamp;
 	int32_t                          active_req_cnt;
 	int64_t                          reported_req_id;
+	uint64_t                         reported_frame_id;
 	uint32_t                         subscribe_event;
 	int64_t                          last_applied_req_id;
 	uint64_t                         recovery_req_id;

drivers/cam_isp/isp_hw_mgr/cam_ife_hw_mgr.c

@@ -77,6 +77,9 @@ static int cam_ife_hw_mgr_event_handler(
 static int cam_ife_mgr_prog_default_settings(
 	bool need_rup_aup, struct cam_ife_hw_mgr_ctx *ctx);
 
+static int cam_ife_mgr_cmd_get_sof_timestamp(struct cam_ife_hw_mgr_ctx *ife_ctx,
+	uint64_t *time_stamp, uint64_t *boot_time_stamp, uint64_t *prev_time_stamp);
+
 static int cam_ife_mgr_finish_clk_bw_update(
 	struct cam_ife_hw_mgr_ctx *ctx,
 	uint64_t request_id, bool skip_clk_data_rst)
@@ -11162,6 +11165,12 @@ static int cam_ife_mgr_cmd(void *hw_mgr_priv, void *cmd_args)
 		case CAM_ISP_HW_MGR_CMD_PROG_DEFAULT_CFG:
 			rc = cam_ife_mgr_prog_default_settings(true, ctx);
 			break;
+		case CAM_ISP_HW_MGR_GET_SOF_TS:
+			rc = cam_ife_mgr_cmd_get_sof_timestamp(ctx,
+				&isp_hw_cmd_args->u.sof_ts.curr,
+				&isp_hw_cmd_args->u.sof_ts.boot,
+				&isp_hw_cmd_args->u.sof_ts.prev);
+			break;
 		default:
 			CAM_ERR(CAM_ISP, "Invalid HW mgr command:0x%x",
 				hw_cmd_args->cmd_type);
@@ -11383,7 +11392,8 @@ static inline void cam_ife_hw_mgr_get_offline_sof_timestamp(
 static int cam_ife_mgr_cmd_get_sof_timestamp(
 	struct cam_ife_hw_mgr_ctx            *ife_ctx,
 	uint64_t                             *time_stamp,
-	uint64_t                             *boot_time_stamp)
+	uint64_t                             *boot_time_stamp,
+	uint64_t                             *prev_time_stamp)
 {
 	int                                   rc = -EINVAL;
 	uint32_t                              i;
@@ -11416,6 +11426,7 @@ static int cam_ife_mgr_cmd_get_sof_timestamp(
 
 			csid_get_time.node_res =
 				hw_mgr_res->hw_res[i];
+			csid_get_time.get_prev_timestamp = (prev_time_stamp != NULL);
 			rc = hw_intf->hw_ops.process_cmd(
 				hw_intf->hw_priv,
 				CAM_IFE_CSID_CMD_GET_TIME_STAMP,
@@ -11427,6 +11438,9 @@ static int cam_ife_mgr_cmd_get_sof_timestamp(
 					csid_get_time.time_stamp_val;
 				*boot_time_stamp =
 					csid_get_time.boot_timestamp;
+				if (prev_time_stamp)
+					*prev_time_stamp =
+						csid_get_time.prev_time_stamp_val;
 			}
 		}
 	}
@@ -12171,7 +12185,7 @@ static int cam_ife_hw_mgr_handle_hw_sof(
 				cam_ife_mgr_cmd_get_sof_timestamp(
 				ife_hw_mgr_ctx,
 				&sof_done_event_data.timestamp,
-				&sof_done_event_data.boot_time);
+				&sof_done_event_data.boot_time, NULL);
 		}
 
 		if (atomic_read(&ife_hw_mgr_ctx->overflow_pending))
@@ -12190,7 +12204,7 @@ static int cam_ife_hw_mgr_handle_hw_sof(
 			break;
 		cam_ife_mgr_cmd_get_sof_timestamp(ife_hw_mgr_ctx,
 			&sof_done_event_data.timestamp,
-			&sof_done_event_data.boot_time);
+			&sof_done_event_data.boot_time, NULL);
 		if (atomic_read(&ife_hw_mgr_ctx->overflow_pending))
 			break;
 		ife_hw_irq_sof_cb(ife_hw_mgr_ctx->common.cb_priv,

drivers/cam_isp/isp_hw_mgr/include/cam_isp_hw_mgr_intf.h

@@ -326,6 +326,7 @@ enum cam_isp_hw_mgr_command {
 	CAM_ISP_HW_MGR_GET_PACKET_OPCODE,
 	CAM_ISP_HW_MGR_GET_LAST_CDM_DONE,
 	CAM_ISP_HW_MGR_CMD_PROG_DEFAULT_CFG,
+	CAM_ISP_HW_MGR_GET_SOF_TS,
 	CAM_ISP_HW_MGR_CMD_MAX,
 };
 
@@ -345,6 +346,7 @@ enum cam_isp_ctx_type {
  * @ctx_type:              RDI_ONLY, PIX and RDI, or FS2
  * @packet_op_code:        Packet opcode
  * @last_cdm_done:         Last cdm done request
+ * @sof_ts:                SOF timestamps (current, boot and previous)
  */
 struct cam_isp_hw_cmd_args {
 	uint32_t                          cmd_type;
@@ -354,6 +356,11 @@ struct cam_isp_hw_cmd_args {
 		uint32_t                      ctx_type;
 		uint32_t                      packet_op_code;
 		uint64_t                      last_cdm_done;
+		struct {
+			uint64_t                      curr;
+			uint64_t                      prev;
+			uint64_t                      boot;
+		} sof_ts;
 	} u;
 };
 

drivers/cam_isp/isp_hw_mgr/isp_hw/ife_csid_hw/cam_ife_csid_hw_ver2.c

@@ -4696,18 +4696,30 @@ static int cam_ife_csid_ver2_program_offline_go_cmd(
 	return 0;
 }
 
+static uint64_t __cam_ife_csid_ver2_get_time_stamp(void __iomem *mem_base, uint32_t timestamp0_addr,
+	uint32_t timestamp1_addr)
+{
+	uint64_t timestamp_val, time_hi, time_lo;
+
+	time_hi = cam_io_r_mb(mem_base + timestamp1_addr);
+	time_lo = cam_io_r_mb(mem_base + timestamp0_addr);
+	timestamp_val = (time_hi << 32) | time_lo;
+
+	return mul_u64_u32_div(timestamp_val,
+		CAM_IFE_CSID_QTIMER_MUL_FACTOR,
+		CAM_IFE_CSID_QTIMER_DIV_FACTOR);
+}
+
 static int cam_ife_csid_ver2_get_time_stamp(
 	struct cam_ife_csid_ver2_hw  *csid_hw, void *cmd_args)
 {
 	const struct cam_ife_csid_ver2_path_reg_info *path_reg;
 	struct cam_isp_resource_node         *res = NULL;
-	uint64_t time_lo, time_hi;
 	struct cam_hw_soc_info              *soc_info;
 	struct cam_csid_get_time_stamp_args *timestamp_args;
 	struct cam_ife_csid_ver2_reg_info *csid_reg;
 	uint64_t  time_delta;
 	struct timespec64 ts;
-	uint32_t curr_0_sof_addr, curr_1_sof_addr;
 
 	timestamp_args = (struct cam_csid_get_time_stamp_args *)cmd_args;
 	res = timestamp_args->node_res;
@@ -4738,19 +4750,17 @@ static int cam_ife_csid_ver2_get_time_stamp(
 		return -EINVAL;
 	}
 
-	curr_0_sof_addr = path_reg->timestamp_curr0_sof_addr;
-	curr_1_sof_addr = path_reg->timestamp_curr1_sof_addr;
-
-	time_hi = cam_io_r_mb(soc_info->reg_map[0].mem_base +
-			curr_1_sof_addr);
-	time_lo = cam_io_r_mb(soc_info->reg_map[0].mem_base +
-			curr_0_sof_addr);
-	timestamp_args->time_stamp_val = (time_hi << 32) | time_lo;
+	if (timestamp_args->get_prev_timestamp) {
+		timestamp_args->prev_time_stamp_val = __cam_ife_csid_ver2_get_time_stamp(
+			soc_info->reg_map[0].mem_base,
+			path_reg->timestamp_perv0_sof_addr,
+			path_reg->timestamp_perv1_sof_addr);
+	}
 
-	timestamp_args->time_stamp_val = mul_u64_u32_div(
-		timestamp_args->time_stamp_val,
-		CAM_IFE_CSID_QTIMER_MUL_FACTOR,
-		CAM_IFE_CSID_QTIMER_DIV_FACTOR);
+	timestamp_args->time_stamp_val = __cam_ife_csid_ver2_get_time_stamp(
+		soc_info->reg_map[0].mem_base,
+		path_reg->timestamp_curr0_sof_addr,
+		path_reg->timestamp_curr1_sof_addr);
 
 	time_delta = timestamp_args->time_stamp_val -
 		csid_hw->timestamp.prev_sof_ts;

drivers/cam_isp/isp_hw_mgr/isp_hw/include/cam_ife_csid_hw_intf.h

@@ -309,14 +309,18 @@ struct cam_csid_reset_cfg_args {
 
 /**
  * struct cam_csid_get_time_stamp_args-  time stamp capture arguments
- * @node_res         : resource to get the time stamp
- * @time_stamp_val   : captured time stamp
- * @boot_timestamp   : boot time stamp
+ * @node_res            : resource to get the time stamp
+ * @time_stamp_val      : captured time stamp
+ * @boot_timestamp      : boot time stamp
+ * @get_prev_timestamp  : flag to fetch previous captured time stamp from hardware
+ * @prev_time_stamp_val : previous captured time stamp
  */
 struct cam_csid_get_time_stamp_args {
 	struct cam_isp_resource_node      *node_res;
 	uint64_t                           time_stamp_val;
 	uint64_t                           boot_timestamp;
+	bool                               get_prev_timestamp;
+	uint64_t                           prev_time_stamp_val;
 };
 
 /**