msm: camera: isp: Enhance SFE driver

Enhance SFE irq handling, debug capabilities and support
line based config for SFE RDI0-2 WMs.

CRs-Fixed: 2841729
Change-Id: I2057310608f2833819f50f0d61b154421de3b0ad
Signed-off-by: Karthik Anantha Ram <[email protected]>

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

@@ -55,6 +55,17 @@ enum cam_sfe_bus_rd_irq_regs {
 	CAM_SFE_BUS_RD_IRQ_REGISTERS_MAX,
 };
 
+/*
+ * struct cam_sfe_debug_cfg_params:
+ *
+ * @sfe_debug_cfg : SFE debug cfg value
+ * @sfe_sensor_sel: SFE sensor sel for diag data
+ */
+struct cam_sfe_debug_cfg_params {
+	uint32_t sfe_debug_cfg;
+	uint32_t sfe_sensor_sel;
+};
+
 /*
  * struct cam_sfe_bw_control_args:
  *
@@ -171,6 +182,7 @@ struct cam_sfe_bus_wr_irq_evt_payload {
  *                           BUS related to SFE resources
  *
  * @list:                    list_head node for the payload
+ * @core_index:              Index of SFE HW that generated this IRQ event
  * @irq_reg_val              Bus irq register status
  * @constraint_violation     constraint violation
  * @error_type:              Identify different errors
@@ -179,6 +191,7 @@ struct cam_sfe_bus_wr_irq_evt_payload {
  */
 struct cam_sfe_bus_rd_irq_evt_payload {
 	struct list_head           list;
+	uint32_t                   core_index;
 	uint32_t                   irq_reg_val[
 		CAM_SFE_BUS_RD_IRQ_REGISTERS_MAX];
 	uint32_t                   constraint_violation;

drivers/cam_isp/isp_hw_mgr/isp_hw/sfe_hw/cam_sfe680.h

@@ -54,63 +54,53 @@ static struct cam_sfe_modules_common_reg_offset sfe680_modules_common_reg = {
 	.qcfa_hdrc_remo_out_mux_cfg    = 0x00005A74,
 };
 
+static struct cam_sfe_top_common_reg_data sfe_680_top_common_reg_data = {
+	.error_irq_mask                = 0x1C000,
+	.enable_diagnostic_hw          = 0x1,
+	.top_debug_cfg_en              = 0x3,
+	.sensor_sel_shift              = 0x1,
+};
+
 static struct cam_sfe_path_common_reg_data sfe_680_pix_reg_data = {
 	.sof_irq_mask                  = 0x4,
 	.eof_irq_mask                  = 0x8,
-	.error_irq_mask                = 0x1C000,
 	.subscribe_irq_mask            = 0xC,
-	.enable_diagnostic_hw          = 0x1,
-	.top_debug_cfg_en              = 0x3,
 };
 
 static struct cam_sfe_path_common_reg_data sfe_680_rdi0_reg_data = {
 	.sof_irq_mask                  = 0x10,
 	.eof_irq_mask                  = 0x20,
-	.error_irq_mask                = 0x1C000,
 	.subscribe_irq_mask            = 0x30,
-	.enable_diagnostic_hw          = 0x1,
-	.top_debug_cfg_en              = 0x3,
 };
 
 static struct cam_sfe_path_common_reg_data sfe_680_rdi1_reg_data = {
 	.sof_irq_mask                  = 0x40,
 	.eof_irq_mask                  = 0x80,
-	.error_irq_mask                = 0x1C000,
 	.subscribe_irq_mask            = 0xC0,
-	.enable_diagnostic_hw          = 0x1,
-	.top_debug_cfg_en              = 0x3,
 };
 
 static struct cam_sfe_path_common_reg_data sfe_680_rdi2_reg_data = {
 	.sof_irq_mask                  = 0x100,
 	.eof_irq_mask                  = 0x200,
-	.error_irq_mask                = 0x1C000,
 	.subscribe_irq_mask            = 0x300,
-	.enable_diagnostic_hw          = 0x1,
-	.top_debug_cfg_en              = 0x3,
 };
 
 static struct cam_sfe_path_common_reg_data sfe_680_rdi3_reg_data = {
 	.sof_irq_mask                  = 0x400,
 	.eof_irq_mask                  = 0x800,
-	.error_irq_mask                = 0x1C000,
 	.subscribe_irq_mask            = 0xC00,
-	.enable_diagnostic_hw          = 0x1,
-	.top_debug_cfg_en              = 0x3,
 };
 
 static struct cam_sfe_path_common_reg_data sfe_680_rdi4_reg_data = {
 	.sof_irq_mask                  = 0x1000,
 	.eof_irq_mask                  = 0x2000,
-	.error_irq_mask                = 0x1C000,
 	.subscribe_irq_mask            = 0x3000,
-	.enable_diagnostic_hw          = 0x1,
-	.top_debug_cfg_en              = 0x3,
 };
 
 static struct cam_sfe_top_hw_info sfe680_top_hw_info = {
 	.common_reg = &sfe680_top_commong_reg,
 	.modules_hw_info = &sfe680_modules_common_reg,
+	.common_reg_data = &sfe_680_top_common_reg_data,
 	.pix_reg_data = &sfe_680_pix_reg_data,
 	.rdi_reg_data[0] = &sfe_680_rdi0_reg_data,
 	.rdi_reg_data[1] = &sfe_680_rdi1_reg_data,
@@ -705,6 +695,7 @@ static struct cam_sfe_bus_wr_hw_info sfe680_bus_wr_hw_info = {
 	},
 	.num_comp_grp    = 10,
 	.comp_done_shift = 17,
+	.line_done_cfg   = 0x11,
 	.top_irq_shift   = 0x0,
 };
 

drivers/cam_isp/isp_hw_mgr/isp_hw/sfe_hw/cam_sfe_core.c

@@ -72,34 +72,16 @@ int cam_sfe_init_hw(void *hw_priv, void *init_hw_args, uint32_t arg_size)
 
 	CAM_DBG(CAM_SFE, "SFE SOC resource enabled");
 
-	/* Async Reset as part of power ON */
-	/* Sync Reset in CSID */
-
-	/* INIT SFE BUS WR */
-	rc = core_info->sfe_bus_wr->hw_ops.init(
-		core_info->sfe_bus_wr->bus_priv, NULL, 0);
-	if (rc) {
-		CAM_ERR(CAM_SFE,
-			"SFE bus wr init failed rc: %d", rc);
-		goto disable_soc;
-	}
-
-	/* INIT SFE BUS RD */
-	rc = core_info->sfe_bus_rd->hw_ops.init(
-		core_info->sfe_bus_rd->bus_priv, NULL, 0);
-	if (rc) {
-		CAM_ERR(CAM_SFE, "SFE bus rd init failed rc: %d", rc);
-		goto deinit_bus_wr;
-	}
+	/*
+	 * Async Reset as part of power ON
+	 * Any register write in bus_wr_init/bus_rd_init
+	 * will be cleared by sync reset in CSID.
+	 *
+	 */
 
 	sfe_hw->hw_state = CAM_HW_STATE_POWER_UP;
 	return rc;
 
-deinit_bus_wr:
-	core_info->sfe_bus_wr->hw_ops.deinit(
-		core_info->sfe_bus_wr->bus_priv, NULL, 0);
-disable_soc:
-	cam_sfe_disable_soc_resources(soc_info);
 decrement_open_cnt:
 	mutex_lock(&sfe_hw->hw_mutex);
 	sfe_hw->open_count--;
@@ -111,7 +93,6 @@ int cam_sfe_deinit_hw(void *hw_priv, void *deinit_hw_args, uint32_t arg_size)
 {
 	struct cam_hw_info                *sfe_hw = hw_priv;
 	struct cam_hw_soc_info            *soc_info = NULL;
-	struct cam_sfe_hw_core_info       *core_info = NULL;
 	int rc = 0;
 
 	if (!hw_priv) {
@@ -136,19 +117,6 @@ int cam_sfe_deinit_hw(void *hw_priv, void *deinit_hw_args, uint32_t arg_size)
 	mutex_unlock(&sfe_hw->hw_mutex);
 
 	soc_info = &sfe_hw->soc_info;
-	core_info = (struct cam_sfe_hw_core_info *)sfe_hw->core_info;
-
-	rc = core_info->sfe_bus_wr->hw_ops.deinit(
-		core_info->sfe_bus_wr->bus_priv, NULL, 0);
-	if (rc)
-		CAM_ERR(CAM_SFE, "SFE bus wr deinit failed rc: %d",
-			rc);
-
-	rc = core_info->sfe_bus_rd->hw_ops.deinit(
-		core_info->sfe_bus_rd->bus_priv, NULL, 0);
-	if (rc)
-		CAM_ERR(CAM_SFE, "SFE bus rd deinit failed rc: %d",
-			rc);
 
 	/* Turn OFF Regulators, Clocks and other SOC resources */
 	CAM_DBG(CAM_SFE, "Disable SFE SOC resource");
@@ -377,6 +345,22 @@ int cam_sfe_process_cmd(void *hw_priv, uint32_t cmd_type,
 	case  CAM_ISP_HW_CMD_UNMASK_BUS_WR_IRQ:
 		/* Needs to be handled based on hw_mgr change */
 		break;
+	case CAM_ISP_HW_CMD_SET_SFE_DEBUG_CFG:
+		/* propagate to SFE top */
+		core_info->sfe_top->hw_ops.process_cmd(
+			core_info->sfe_top->top_priv, cmd_type,
+			cmd_args, arg_size);
+
+		/* propagate to SFE bus wr */
+		core_info->sfe_bus_wr->hw_ops.process_cmd(
+			core_info->sfe_bus_wr->bus_priv, cmd_type,
+			cmd_args, arg_size);
+
+		/* propagate to SFE bus rd */
+		core_info->sfe_bus_rd->hw_ops.process_cmd(
+			core_info->sfe_bus_rd->bus_priv, cmd_type,
+			cmd_args, arg_size);
+		break;
 	default:
 		CAM_ERR(CAM_SFE, "Invalid cmd type: %d", cmd_type);
 		rc = -EINVAL;

drivers/cam_isp/isp_hw_mgr/isp_hw/sfe_hw/cam_sfe_core.h

@@ -37,6 +37,14 @@ struct cam_sfe_hw_core_info {
 	spinlock_t                          spin_lock;
 };
 
+/*
+ * Debug config to enable interrupts and debug features
+ */
+#define SFE_DEBUG_ENABLE_SOF_EOF_IRQ              BIT(0)
+#define SFE_DEBUG_ENABLE_SENSOR_DIAG_INFO         BIT(1)
+#define SFE_DEBUG_ENABLE_FRAME_COUNTER            BIT(2)
+#define SFE_DEBUG_ENABLE_RD_DONE_IRQ              BIT(3)
+
 int cam_sfe_get_hw_caps(void *device_priv,
 	void *get_hw_cap_args, uint32_t arg_size);
 int cam_sfe_reset(void *device_priv,

drivers/cam_isp/isp_hw_mgr/isp_hw/sfe_hw/cam_sfe_dev.c

@@ -228,7 +228,7 @@ int cam_sfe_hw_init(struct cam_hw_intf **sfe_hw, uint32_t hw_idx)
 
 static const struct of_device_id cam_sfe_dt_match[] = {
 	{
-		.compatible = "",
+		.compatible = "qcom,sfe680",
 		.data = &cam_sfe680_hw_info,
 	},
 	{}

drivers/cam_isp/isp_hw_mgr/isp_hw/sfe_hw/sfe_bus/cam_sfe_bus_rd.c

@@ -72,6 +72,8 @@ struct cam_sfe_bus_rd_common_data {
 	struct cam_sfe_bus_rd_irq_evt_payload       evt_payload[
 		CAM_SFE_BUS_RD_PAYLOAD_MAX];
 	cam_hw_mgr_event_cb_func                    event_cb;
+	bool                                        err_irq_subscribe;
+	uint32_t                                    sfe_debug_cfg;
 };
 
 struct cam_sfe_bus_rd_rm_resource_data {
@@ -97,6 +99,7 @@ struct cam_sfe_bus_rd_rm_resource_data {
 struct cam_sfe_bus_rd_data {
 	uint32_t                           bus_rd_type;
 	struct cam_sfe_bus_rd_common_data *common_data;
+	struct cam_sfe_bus_rd_priv        *bus_priv;
 
 	uint32_t                           num_rm;
 	struct cam_isp_resource_node      *rm_res[PLANE_MAX];
@@ -107,7 +110,6 @@ struct cam_sfe_bus_rd_data {
 	void                              *priv;
 	uint32_t                           secure_mode;
 	uint32_t                           fs_sync_enable;
-	uint32_t                           fs_line_sync_en;
 	bool                               is_offline;
 };
 
@@ -530,6 +532,82 @@ static int cam_sfe_bus_deinit_rm_resource(
 	return 0;
 }
 
+static int cam_sfe_bus_rd_out_done_top_half(
+	uint32_t evt_id,
+	struct cam_irq_th_payload *th_payload)
+{
+	int32_t                                 rc = 0, i;
+	struct cam_isp_resource_node           *sfe_rd = NULL;
+	struct cam_sfe_bus_rd_data             *rsrc_data = NULL;
+	struct cam_sfe_bus_rd_irq_evt_payload  *evt_payload;
+
+	sfe_rd = th_payload->handler_priv;
+	if (!sfe_rd) {
+		CAM_ERR_RATE_LIMIT(CAM_SFE, "No SFE RD resource");
+		return -ENODEV;
+	}
+
+	rsrc_data = sfe_rd->res_priv;
+
+	CAM_DBG(CAM_SFE, "SFE:%d Bus IRQ status_0: 0x%X",
+		rsrc_data->common_data->core_index,
+		th_payload->evt_status_arr[0]);
+
+	rc  = cam_sfe_bus_rd_get_evt_payload(rsrc_data->common_data,
+			&evt_payload);
+	if (rc) {
+		CAM_INFO_RATE_LIMIT(CAM_SFE,
+			"Failed to get payload for SFE:%d Bus IRQ status_0: 0x%X status_1: 0x%X",
+			rsrc_data->common_data->core_index,
+			th_payload->evt_status_arr[0]);
+		return rc;
+	}
+
+	cam_isp_hw_get_timestamp(&evt_payload->ts);
+
+	evt_payload->core_index = rsrc_data->common_data->core_index;
+	evt_payload->evt_id = evt_id;
+
+	for (i = 0; i < th_payload->num_registers; i++)
+		evt_payload->irq_reg_val[i] = th_payload->evt_status_arr[i];
+
+	th_payload->evt_payload_priv = evt_payload;
+	return rc;
+}
+
+static int cam_sfe_bus_rd_out_done_bottom_half(
+	void                *handler_priv,
+	void                *evt_payload_priv)
+{
+	int                                    rc = -EINVAL;
+	uint32_t                               status = 0;
+	struct cam_isp_resource_node          *sfe_rd = handler_priv;
+	struct cam_sfe_bus_rd_data            *rsrc_data = sfe_rd->res_priv;
+	struct cam_sfe_bus_rd_irq_evt_payload *evt_payload = evt_payload_priv;
+
+	status = evt_payload->irq_reg_val[CAM_SFE_IRQ_BUS_RD_REG_STATUS0];
+	cam_sfe_bus_rd_put_evt_payload(rsrc_data->common_data, &evt_payload);
+
+	if (status & 0x2)
+		CAM_DBG(CAM_SFE, "Received SFE:%d BUS RD RUP",
+			rsrc_data->common_data->core_index);
+
+	if (status & 0x4)
+		CAM_DBG(CAM_SFE, "Received SFE:%d BUS RD0 BUF DONE",
+			rsrc_data->common_data->core_index);
+
+	if (status & 0x8)
+		CAM_DBG(CAM_SFE, "Received SFE:%d BUS RD1 BUF DONE",
+			rsrc_data->common_data->core_index);
+
+	if (status & 0x10)
+		CAM_DBG(CAM_SFE, "Received SFE:%d BUS RD2 BUF DONE",
+			rsrc_data->common_data->core_index);
+
+	return rc;
+}
+
+
 static int cam_sfe_bus_rd_handle_irq_top_half(uint32_t evt_id,
 	struct cam_irq_th_payload *th_payload)
 {
@@ -589,22 +667,6 @@ static int cam_sfe_bus_rd_handle_irq_bottom_half(
 	constraint_violation = evt_payload->constraint_violation;
 	cam_sfe_bus_rd_put_evt_payload(common_data, &evt_payload);
 
-	if (status & 0x2)
-		CAM_DBG(CAM_SFE, "Received SFE:%d BUS RD RUP",
-			bus_priv->common_data.core_index);
-
-	if (status & 0x4)
-		CAM_DBG(CAM_SFE, "Received SFE:%d BUS RD0 BUF DONE",
-			bus_priv->common_data.core_index);
-
-	if (status & 0x8)
-		CAM_DBG(CAM_SFE, "Received SFE:%d BUS RD1 BUF DONE",
-			bus_priv->common_data.core_index);
-
-	if (status & 0x10)
-		CAM_DBG(CAM_SFE, "Received SFE:%d BUS RD2 BUF DONE",
-			bus_priv->common_data.core_index);
-
 	if (status & 0x1) {
 		CAM_ERR(CAM_SFE, "SFE:%d Constraint violation status:0x%x",
 			bus_priv->common_data.core_index,
@@ -623,6 +685,54 @@ static int cam_sfe_bus_rd_handle_irq_bottom_half(
 	return 0;
 }
 
+static int cam_sfe_bus_subscribe_error_irq(
+	struct cam_sfe_bus_rd_priv          *bus_priv)
+{
+	uint32_t sfe_top_irq_mask[CAM_SFE_IRQ_REGISTERS_MAX] = {0};
+
+	/* Subscribe top IRQ */
+	sfe_top_irq_mask[0] = (1 << bus_priv->top_irq_shift);
+
+	bus_priv->irq_handle = cam_irq_controller_subscribe_irq(
+		bus_priv->common_data.sfe_irq_controller,
+		CAM_IRQ_PRIORITY_0,
+		sfe_top_irq_mask,
+		bus_priv,
+		cam_sfe_bus_rd_handle_irq,
+		NULL,
+		NULL,
+		NULL);
+
+	if (bus_priv->irq_handle < 1) {
+		CAM_ERR(CAM_SFE,
+			"Failed to subscribe TOP IRQ for BUS RD");
+		bus_priv->irq_handle = 0;
+		return -EFAULT;
+	}
+
+	if (bus_priv->tasklet_info != NULL) {
+		bus_priv->error_irq_handle = cam_irq_controller_subscribe_irq(
+			bus_priv->common_data.bus_irq_controller,
+			CAM_IRQ_PRIORITY_0,
+			bus_rd_error_irq_mask,
+			bus_priv,
+			cam_sfe_bus_rd_handle_irq_top_half,
+			cam_sfe_bus_rd_handle_irq_bottom_half,
+			bus_priv->tasklet_info,
+			&tasklet_bh_api);
+
+		if (bus_priv->error_irq_handle < 1) {
+			CAM_ERR(CAM_SFE, "Failed to subscribe error IRQ");
+			bus_priv->error_irq_handle = 0;
+			return -EFAULT;
+		}
+	}
+
+	bus_priv->common_data.err_irq_subscribe = true;
+	CAM_DBG(CAM_SFE, "BUS RD error irq subscribed");
+	return 0;
+}
+
 static int cam_sfe_bus_rd_get_secure_mode(void *priv, void *cmd_args,
 	uint32_t arg_size)
 {
@@ -680,8 +790,8 @@ static int cam_sfe_bus_acquire_bus_rd(void *bus_priv, void *acquire_args,
 	rsrc_node->res_id = bus_rd_acquire_args->res_id;
 	rsrc_data = rsrc_node->res_priv;
 
-	CAM_DBG(CAM_SFE, "SFE:%d acquire RD type:0x%x",
-		rsrc_data->common_data->core_index, acq_args->rsrc_type);
+	CAM_DBG(CAM_SFE, "SFE:%d acquire RD type:0x%x res_id 0x%x",
+		rsrc_data->common_data->core_index, acq_args->rsrc_type, rsrc_node->res_id);
 
 	rsrc_data->num_rm = num_rm;
 	rsrc_node->tasklet_info = acq_args->tasklet;
@@ -691,6 +801,12 @@ static int cam_sfe_bus_acquire_bus_rd(void *bus_priv, void *acquire_args,
 	rsrc_data->common_data->event_cb = acq_args->event_cb;
 	rsrc_data->priv = acq_args->priv;
 	rsrc_data->is_offline = bus_rd_acquire_args->is_offline;
+	rsrc_data->bus_priv = bus_rd_priv;
+
+	/* Enable FS sync for sHDR & FS */
+	if (!rsrc_data->is_offline)
+		rsrc_data->fs_sync_enable = 1;
+
 	if (!rsrc_data->secure_mode) {
 		rsrc_data->secure_mode =
 			bus_rd_acquire_args->secure_mode;
@@ -774,11 +890,13 @@ static int cam_sfe_bus_release_bus_rd(void *bus_priv, void *release_args,
 static int cam_sfe_bus_start_bus_rd(
 	void *hw_priv, void *stop_hw_args, uint32_t arg_size)
 {
-	int rc = -ENODEV, i;
-	struct cam_isp_resource_node *sfe_bus_rd = NULL;
-	struct cam_sfe_bus_rd_data *rsrc_data = NULL;
+	int rc, i;
+	const uint32_t buf_done_shift = 2;
+	uint32_t bus_rd_done_irq_mask[1] = {0};
+	struct cam_isp_resource_node	  *sfe_bus_rd = NULL;
+	struct cam_sfe_bus_rd_data		  *rsrc_data = NULL;
+	struct cam_sfe_bus_rd_priv		  *bus_priv = NULL;
 	struct cam_sfe_bus_rd_common_data *common_data = NULL;
-	uint32_t val = 0;
 
 	if (!hw_priv) {
 		CAM_ERR(CAM_SFE, "Invalid input");
@@ -787,6 +905,7 @@ static int cam_sfe_bus_start_bus_rd(
 
 	sfe_bus_rd = (struct cam_isp_resource_node *)hw_priv;
 	rsrc_data = sfe_bus_rd->res_priv;
+	bus_priv = rsrc_data->bus_priv;
 	common_data = rsrc_data->common_data;
 
 	CAM_DBG(CAM_SFE, "SFE:%d start RD type:0x%x", sfe_bus_rd->res_id);
@@ -797,12 +916,18 @@ static int cam_sfe_bus_start_bus_rd(
 		return -EACCES;
 	}
 
-	if (!rsrc_data->is_offline) {
-		val = (rsrc_data->fs_sync_enable << 5);
-		cam_io_w_mb(val, common_data->mem_base +
-			common_data->common_reg->input_if_cmd);
+	/* subscribe error irqs */
+	if (!bus_priv->common_data.err_irq_subscribe) {
+		rc = cam_sfe_bus_subscribe_error_irq(bus_priv);
+		if (rc)
+			return rc;
 	}
 
+	if (!rsrc_data->is_offline)
+		cam_io_w_mb((rsrc_data->fs_sync_enable << 5),
+			common_data->mem_base +
+			common_data->common_reg->input_if_cmd);
+
 	if (rsrc_data->secure_mode == CAM_SECURE_MODE_SECURE)
 		cam_io_w_mb(1, common_data->mem_base +
 			common_data->common_reg->security_cfg);
@@ -810,10 +935,34 @@ static int cam_sfe_bus_start_bus_rd(
 	for (i = 0; i < rsrc_data->num_rm; i++)
 		rc = cam_sfe_bus_start_rm(rsrc_data->rm_res[i]);
 
-	/* TO DO Subscribe mask for buf_done */
-	if (!rc)
-		sfe_bus_rd->res_state = CAM_ISP_RESOURCE_STATE_STREAMING;
+	if (common_data->sfe_debug_cfg &
+		SFE_DEBUG_ENABLE_RD_DONE_IRQ) {
+
+		/* Subscribe RUP */
+		bus_rd_done_irq_mask[0] |= 0x2;
+
+		/* Subscribe buf done */
+		bus_rd_done_irq_mask[0] |=
+			(1 << (rsrc_data->bus_rd_type + buf_done_shift));
+		sfe_bus_rd->irq_handle = cam_irq_controller_subscribe_irq(
+			common_data->bus_irq_controller,
+			CAM_IRQ_PRIORITY_2,
+			bus_rd_done_irq_mask,
+			sfe_bus_rd,
+			cam_sfe_bus_rd_out_done_top_half,
+			cam_sfe_bus_rd_out_done_bottom_half,
+			sfe_bus_rd->tasklet_info,
+			&tasklet_bh_api);
+
+		if (sfe_bus_rd->irq_handle < 1) {
+			CAM_ERR(CAM_SFE,
+				"Failed to subscribe RUP/BUF done IRQ");
+			sfe_bus_rd->irq_handle = 0;
+			return -EFAULT;
+		}
+	}
 
+	sfe_bus_rd->res_state = CAM_ISP_RESOURCE_STATE_STREAMING;
 	return rc;
 }
 
@@ -823,6 +972,7 @@ static int cam_sfe_bus_stop_bus_rd(
 	int rc = 0, i;
 	struct cam_isp_resource_node *sfe_bus_rd = NULL;
 	struct cam_sfe_bus_rd_data   *rsrc_data = NULL;
+	struct cam_sfe_bus_rd_priv   *bus_priv = NULL;
 
 	if (!hw_priv) {
 		CAM_ERR(CAM_SFE, "Invalid input");
@@ -831,6 +981,7 @@ static int cam_sfe_bus_stop_bus_rd(
 
 	sfe_bus_rd = (struct cam_isp_resource_node *)hw_priv;
 	rsrc_data = sfe_bus_rd->res_priv;
+	bus_priv = rsrc_data->bus_priv;
 
 	if (sfe_bus_rd->res_state == CAM_ISP_RESOURCE_STATE_AVAILABLE ||
 		sfe_bus_rd->res_state == CAM_ISP_RESOURCE_STATE_RESERVED) {
@@ -844,7 +995,38 @@ static int cam_sfe_bus_stop_bus_rd(
 
 	sfe_bus_rd->res_state = CAM_ISP_RESOURCE_STATE_RESERVED;
 
-	/* Unsubscribe mask for this wm IRQ controller */
+	/* Unsubscribe RUP & Buf done */
+	if (sfe_bus_rd->irq_handle) {
+		rc = cam_irq_controller_unsubscribe_irq(
+				rsrc_data->common_data->bus_irq_controller,
+				sfe_bus_rd->irq_handle);
+		if (rc)
+			CAM_ERR(CAM_SFE, "Failed to unsubscribe rup/buf done irq");
+		sfe_bus_rd->irq_handle = 0;
+	}
+
+	/* Unsubscribe error irqs */
+	if (bus_priv->common_data.err_irq_subscribe) {
+		if (bus_priv->irq_handle) {
+			rc = cam_irq_controller_unsubscribe_irq(
+					bus_priv->common_data.sfe_irq_controller,
+					bus_priv->irq_handle);
+			if (rc)
+				CAM_ERR(CAM_SFE, "Failed to unsubscribe top irq");
+			bus_priv->irq_handle = 0;
+		}
+
+		if (bus_priv->error_irq_handle) {
+			rc = cam_irq_controller_unsubscribe_irq(
+					bus_priv->common_data.bus_irq_controller,
+					bus_priv->error_irq_handle);
+			if (rc)
+				CAM_ERR(CAM_SFE, "Failed to unsubscribe error irq");
+			bus_priv->error_irq_handle = 0;
+		}
+		bus_priv->common_data.err_irq_subscribe = false;
+	}
+
 	CAM_DBG(CAM_SFE, "SFE:%d stopped Bus RD:0x%x",
 		rsrc_data->common_data->core_index,
 		sfe_bus_rd->res_id);
@@ -1080,8 +1262,6 @@ static int cam_sfe_bus_rd_update_fs_cfg(void *priv, void *cmd_args,
 		rm_data->min_vbi = fe_cfg->min_vbi;
 		sfe_bus_rd_data->fs_sync_enable =
 			fe_cfg->fs_sync_enable;
-		sfe_bus_rd_data->fs_line_sync_en =
-			fe_cfg->fs_line_sync_en;
 
 		CAM_DBG(CAM_SFE,
 			"SFE:%d RM:%d format:0x%x unpacker_cfg:0x%x",
@@ -1101,91 +1281,27 @@ static int cam_sfe_bus_rd_update_fs_cfg(void *priv, void *cmd_args,
 static int cam_sfe_bus_init_hw(void *hw_priv,
 	void *init_hw_args, uint32_t arg_size)
 {
-	struct cam_sfe_bus_rd_priv    *bus_priv = hw_priv;
-	uint32_t                       offset = 0;
-	struct cam_sfe_bus_rd_reg_offset_common  *common_reg;
-	uint32_t sfe_top_irq_mask[CAM_SFE_IRQ_REGISTERS_MAX] = {0};
-
-	if (!bus_priv) {
-		CAM_ERR(CAM_SFE, "Invalid args");
-		return -EINVAL;
-	}
-	common_reg = bus_priv->common_data.common_reg;
-	sfe_top_irq_mask[0] = (1 << bus_priv->top_irq_shift);
-
-	/* Subscribe for top IRQ */
-	bus_priv->irq_handle = cam_irq_controller_subscribe_irq(
-		bus_priv->common_data.sfe_irq_controller,
-		CAM_IRQ_PRIORITY_2,
-		sfe_top_irq_mask,
-		bus_priv,
-		cam_sfe_bus_rd_handle_irq,
-		NULL,
-		NULL,
-		NULL);
-
-	if (bus_priv->irq_handle < 1) {
-		CAM_ERR(CAM_SFE,
-			"Failed to subscribe TOP IRQ for BUS RD");
-		bus_priv->irq_handle = 0;
-		return -EFAULT;
-	}
-
-	if (bus_priv->tasklet_info != NULL) {
-		bus_priv->error_irq_handle = cam_irq_controller_subscribe_irq(
-			bus_priv->common_data.bus_irq_controller,
-			CAM_IRQ_PRIORITY_1,
-			bus_rd_error_irq_mask,
-			bus_priv,
-			cam_sfe_bus_rd_handle_irq_top_half,
-			cam_sfe_bus_rd_handle_irq_bottom_half,
-			bus_priv->tasklet_info,
-			&tasklet_bh_api);
-
-		if (bus_priv->error_irq_handle < 1) {
-			CAM_ERR(CAM_SFE, "Failed to subscribe error IRQ");
-			bus_priv->error_irq_handle = 0;
-			return -EFAULT;
-		}
-	}
-	/* BUS_RD_TEST_BUS_CTRL disabling test bus */
-	offset = common_reg->test_bus_ctrl;
-	cam_io_w_mb(0x0, bus_priv->common_data.mem_base + offset);
-
 	return 0;
 }
 
 static int cam_sfe_bus_deinit_hw(void *hw_priv,
 	void *deinit_hw_args, uint32_t arg_size)
 {
-	struct cam_sfe_bus_rd_priv    *bus_priv = hw_priv;
-	int                            rc = 0;
-
-	if (!bus_priv) {
-		CAM_ERR(CAM_SFE, "Error: Invalid args");
-		return -EINVAL;
-	}
+	return 0;
+}
 
-	/* Unsubscribe irqs */
-	if (bus_priv->irq_handle) {
-		rc =   cam_irq_controller_unsubscribe_irq(
-				bus_priv->common_data.sfe_irq_controller,
-				bus_priv->irq_handle);
-		if (rc)
-			CAM_ERR(CAM_SFE, "Failed to unsubscribe top irq");
-		bus_priv->irq_handle = 0;
-	}
+static int cam_sfe_bus_rd_set_debug_cfg(
+	void *priv, void *cmd_args)
+{
+	struct cam_sfe_bus_rd_priv *bus_priv =
+		(struct cam_sfe_bus_rd_priv  *) priv;
+	struct cam_sfe_debug_cfg_params *debug_cfg;
 
-	if (bus_priv->error_irq_handle) {
-		rc =   cam_irq_controller_unsubscribe_irq(
-				bus_priv->common_data.bus_irq_controller,
-				bus_priv->error_irq_handle);
-		if (rc)
-			CAM_ERR(CAM_SFE, "Failed to unsubscribe error irq");
-		bus_priv->error_irq_handle = 0;
-	}
+	debug_cfg = (struct cam_sfe_debug_cfg_params *)cmd_args;
+	bus_priv->common_data.sfe_debug_cfg =
+		debug_cfg->sfe_debug_cfg;
 
-	return rc;
+	return 0;
 }
 
 static int cam_sfe_bus_rd_process_cmd(
@@ -1210,6 +1326,9 @@ static int cam_sfe_bus_rd_process_cmd(
 	case CAM_ISP_HW_CMD_FE_UPDATE_BUS_RD:
 		rc = cam_sfe_bus_rd_update_fs_cfg(priv, cmd_args, arg_size);
 		break;
+	case CAM_ISP_HW_CMD_SET_SFE_DEBUG_CFG:
+		rc = cam_sfe_bus_rd_set_debug_cfg(priv, cmd_args);
+		break;
 	default:
 		CAM_ERR_RATE_LIMIT(CAM_SFE,
 			"Invalid SFE BUS RD command type: %d",
@@ -1314,6 +1433,9 @@ int cam_sfe_bus_rd_init(
 
 	*sfe_bus = sfe_bus_local;
 
+	/* Remove after driver stabilizes */
+	bus_priv->common_data.sfe_debug_cfg |=
+		SFE_DEBUG_ENABLE_RD_DONE_IRQ;
 	return rc;
 
 deinit_sfe_bus_rd:

drivers/cam_isp/isp_hw_mgr/isp_hw/sfe_hw/sfe_bus/cam_sfe_bus_wr.c

@@ -62,6 +62,12 @@ enum cam_sfe_bus_wr_packer_format {
 	PACKER_FMT_MAX,
 };
 
+enum cam_sfe_bus_wr_wm_mode {
+	CAM_SFE_WM_LINE_BASED_MODE  = 1,
+	CAM_SFE_WM_FRAME_BASED_MODE,
+	CAM_SFE_WM_INDEX_BASED_MODE,
+};
+
 struct cam_sfe_bus_wr_common_data {
 	uint32_t                                    core_index;
 	uint32_t                                    hw_version;
@@ -82,7 +88,8 @@ struct cam_sfe_bus_wr_common_data {
 	uint32_t                                    num_sec_out;
 	uint32_t                                    addr_no_sync;
 	uint32_t                                    comp_done_shift;
-	bool                                        hw_init;
+	uint32_t                                    line_done_cfg;
+	bool                                        err_irq_subscribe;
 	cam_hw_mgr_event_cb_func                    event_cb;
 };
 
@@ -109,6 +116,7 @@ struct cam_sfe_bus_wr_wm_resource_data {
 	uint32_t             stride;
 	uint32_t             format;
 	enum cam_sfe_bus_wr_packer_format pack_fmt;
+	enum cam_sfe_bus_wr_wm_mode wm_mode;
 
 	uint32_t             packer_cfg;
 	uint32_t             h_init;
@@ -144,6 +152,7 @@ struct cam_sfe_bus_wr_out_data {
 	uint32_t                              out_type;
 	uint32_t                              source_group;
 	struct cam_sfe_bus_wr_common_data    *common_data;
+	struct cam_sfe_bus_wr_priv           *bus_priv;
 
 	uint32_t                           num_wm;
 	struct cam_isp_resource_node      *wm_res;
@@ -185,6 +194,9 @@ static int cam_sfe_bus_wr_process_cmd(
 	uint32_t cmd_type, void *cmd_args,
 	uint32_t arg_size);
 
+static int cam_sfe_bus_subscribe_error_irq(
+	struct cam_sfe_bus_wr_priv  *bus_priv);
+
 static bool cam_sfe_bus_can_be_secure(uint32_t out_type)
 {
 	switch (out_type) {
@@ -351,6 +363,145 @@ static enum cam_sfe_bus_wr_packer_format
 	}
 }
 
+static int cam_sfe_bus_config_rdi_wm(
+	struct cam_sfe_bus_wr_wm_resource_data  *rsrc_data)
+{
+	rsrc_data->pack_fmt = 0x0;
+	switch (rsrc_data->format) {
+	case CAM_FORMAT_MIPI_RAW_10:
+		if (rsrc_data->wm_mode == CAM_SFE_WM_LINE_BASED_MODE) {
+			rsrc_data->en_cfg = 0x1;
+			rsrc_data->width =
+				ALIGNUP((rsrc_data->width * 5) / 4, 16) / 16;
+		} else {
+			rsrc_data->width = CAM_SFE_RDI_BUS_DEFAULT_WIDTH;
+			rsrc_data->height = 0;
+			rsrc_data->stride = CAM_SFE_RDI_BUS_DEFAULT_STRIDE;
+			rsrc_data->en_cfg = (0x1 << 16) | 0x1;
+		}
+		break;
+	case CAM_FORMAT_MIPI_RAW_6:
+		if (rsrc_data->wm_mode == CAM_SFE_WM_LINE_BASED_MODE) {
+			rsrc_data->en_cfg = 0x1;
+			rsrc_data->width =
+				ALIGNUP((rsrc_data->width * 3) / 4, 16) / 16;
+		} else {
+			rsrc_data->width = CAM_SFE_RDI_BUS_DEFAULT_WIDTH;
+			rsrc_data->height = 0;
+			rsrc_data->stride = CAM_SFE_RDI_BUS_DEFAULT_STRIDE;
+			rsrc_data->en_cfg = (0x1 << 16) | 0x1;
+		}
+		break;
+	case CAM_FORMAT_MIPI_RAW_8:
+		if (rsrc_data->wm_mode == CAM_SFE_WM_LINE_BASED_MODE) {
+			rsrc_data->en_cfg = 0x1;
+			rsrc_data->width =
+				ALIGNUP(rsrc_data->width, 16) / 16;
+		} else {
+			rsrc_data->width = CAM_SFE_RDI_BUS_DEFAULT_WIDTH;
+			rsrc_data->height = 0;
+			rsrc_data->stride = CAM_SFE_RDI_BUS_DEFAULT_STRIDE;
+			rsrc_data->en_cfg = (0x1 << 16) | 0x1;
+		}
+		break;
+	case CAM_FORMAT_MIPI_RAW_12:
+		if (rsrc_data->wm_mode == CAM_SFE_WM_LINE_BASED_MODE) {
+			rsrc_data->en_cfg = 0x1;
+			rsrc_data->width =
+				ALIGNUP((rsrc_data->width * 3) / 2, 16) / 16;
+		} else {
+			rsrc_data->width = CAM_SFE_RDI_BUS_DEFAULT_WIDTH;
+			rsrc_data->height = 0;
+			rsrc_data->stride = CAM_SFE_RDI_BUS_DEFAULT_STRIDE;
+			rsrc_data->en_cfg = (0x1 << 16) | 0x1;
+		}
+		break;
+	case CAM_FORMAT_MIPI_RAW_14:
+		if (rsrc_data->wm_mode == CAM_SFE_WM_LINE_BASED_MODE) {
+			rsrc_data->en_cfg = 0x1;
+			rsrc_data->width =
+				ALIGNUP((rsrc_data->width * 7) / 2, 16) / 16;
+		} else {
+			rsrc_data->width = CAM_SFE_RDI_BUS_DEFAULT_WIDTH;
+			rsrc_data->height = 0;
+			rsrc_data->stride = CAM_SFE_RDI_BUS_DEFAULT_STRIDE;
+			rsrc_data->en_cfg = (0x1 << 16) | 0x1;
+		}
+		break;
+	case CAM_FORMAT_MIPI_RAW_16:
+		if (rsrc_data->wm_mode == CAM_SFE_WM_LINE_BASED_MODE) {
+			rsrc_data->en_cfg = 0x1;
+			rsrc_data->width =
+				ALIGNUP((rsrc_data->width * 2), 16) / 16;
+		} else {
+			rsrc_data->width = CAM_SFE_RDI_BUS_DEFAULT_WIDTH;
+			rsrc_data->height = 0;
+			rsrc_data->stride = CAM_SFE_RDI_BUS_DEFAULT_STRIDE;
+			rsrc_data->en_cfg = (0x1 << 16) | 0x1;
+		}
+		break;
+	case CAM_FORMAT_MIPI_RAW_20:
+		if (rsrc_data->wm_mode == CAM_SFE_WM_LINE_BASED_MODE) {
+			rsrc_data->en_cfg = 0x1;
+			rsrc_data->width =
+				ALIGNUP((rsrc_data->width * 5) / 2, 16) / 16;
+		} else {
+			rsrc_data->width = CAM_SFE_RDI_BUS_DEFAULT_WIDTH;
+			rsrc_data->height = 0;
+			rsrc_data->stride = CAM_SFE_RDI_BUS_DEFAULT_STRIDE;
+			rsrc_data->en_cfg = (0x1 << 16) | 0x1;
+		}
+		break;
+	case CAM_FORMAT_PLAIN128:
+		if (rsrc_data->wm_mode == CAM_SFE_WM_LINE_BASED_MODE) {
+			rsrc_data->en_cfg = 0x1;
+			rsrc_data->width =
+				ALIGNUP((rsrc_data->width * 16), 16) / 16;
+		} else {
+			rsrc_data->width = CAM_SFE_RDI_BUS_DEFAULT_WIDTH;
+			rsrc_data->height = 0;
+			rsrc_data->stride = CAM_SFE_RDI_BUS_DEFAULT_STRIDE;
+			rsrc_data->en_cfg = (0x1 << 16) | 0x1;
+		}
+		break;
+	case CAM_FORMAT_PLAIN32_20:
+		if (rsrc_data->wm_mode == CAM_SFE_WM_LINE_BASED_MODE) {
+			rsrc_data->en_cfg = 0x1;
+			rsrc_data->width =
+				ALIGNUP((rsrc_data->width * 4), 16) / 16;
+		} else {
+			rsrc_data->width = CAM_SFE_RDI_BUS_DEFAULT_WIDTH;
+			rsrc_data->height = 0;
+			rsrc_data->stride = CAM_SFE_RDI_BUS_DEFAULT_STRIDE;
+			rsrc_data->en_cfg = (0x1 << 16) | 0x1;
+		}
+		break;
+	case CAM_FORMAT_PLAIN8:
+		rsrc_data->en_cfg = 0x1;
+		rsrc_data->stride = rsrc_data->width * 2;
+		break;
+	case CAM_FORMAT_PLAIN16_10:
+	case CAM_FORMAT_PLAIN16_12:
+	case CAM_FORMAT_PLAIN16_14:
+	case CAM_FORMAT_PLAIN16_16:
+		rsrc_data->width =
+			ALIGNUP(rsrc_data->width * 2, 16) / 16;
+		rsrc_data->en_cfg = 0x1;
+		break;
+	case CAM_FORMAT_PLAIN64:
+		rsrc_data->width =
+			ALIGNUP(rsrc_data->width * 8, 16) / 16;
+		rsrc_data->en_cfg = 0x1;
+		break;
+	default:
+		CAM_ERR(CAM_SFE, "Unsupported RDI format %d",
+			rsrc_data->format);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int cam_sfe_bus_acquire_wm(
 	struct cam_sfe_bus_wr_priv            *bus_priv,
 	struct cam_isp_out_port_generic_info  *out_port_info,
@@ -362,7 +513,7 @@ static int cam_sfe_bus_acquire_wm(
 	uint32_t                               is_dual,
 	enum cam_sfe_bus_wr_comp_grp_type     *comp_grp_id)
 {
-	int32_t wm_idx = 0;
+	int32_t wm_idx = 0, rc;
 	struct cam_sfe_bus_wr_wm_resource_data  *rsrc_data = NULL;
 	char wm_mode[50] = {0};
 
@@ -384,6 +535,12 @@ static int cam_sfe_bus_acquire_wm(
 	rsrc_data->width = out_port_info->width;
 	rsrc_data->height = out_port_info->height;
 	rsrc_data->is_dual = is_dual;
+	/* RDI0-2 line based mode by default */
+	if (sfe_out_res_id == CAM_SFE_BUS_SFE_OUT_RDI0 ||
+		sfe_out_res_id == CAM_SFE_BUS_SFE_OUT_RDI1 ||
+		sfe_out_res_id == CAM_SFE_BUS_SFE_OUT_RDI2)
+		rsrc_data->wm_mode = CAM_SFE_WM_LINE_BASED_MODE;
+
 	/* Set WM offset value to default */
 	rsrc_data->offset  = 0;
 	CAM_DBG(CAM_SFE, "WM:%d width %d height %d", rsrc_data->index,
@@ -391,59 +548,20 @@ static int cam_sfe_bus_acquire_wm(
 
 	if ((sfe_out_res_id >= CAM_SFE_BUS_SFE_OUT_RDI0) &&
 		(sfe_out_res_id <= CAM_SFE_BUS_SFE_OUT_RDI4)) {
-
-		rsrc_data->pack_fmt = 0x0;
-		switch (rsrc_data->format) {
-		case CAM_FORMAT_MIPI_RAW_6:
-		case CAM_FORMAT_MIPI_RAW_8:
-		case CAM_FORMAT_MIPI_RAW_10:
-		case CAM_FORMAT_MIPI_RAW_12:
-		case CAM_FORMAT_MIPI_RAW_14:
-		case CAM_FORMAT_MIPI_RAW_16:
-		case CAM_FORMAT_MIPI_RAW_20:
-		case CAM_FORMAT_PLAIN128:
-		case CAM_FORMAT_PLAIN32_20:
-			rsrc_data->width = CAM_SFE_RDI_BUS_DEFAULT_WIDTH;
-			rsrc_data->height = 0;
-			rsrc_data->stride = CAM_SFE_RDI_BUS_DEFAULT_STRIDE;
-			rsrc_data->en_cfg = (0x1 << 16) | 0x1;
-			break;
-		case CAM_FORMAT_PLAIN8:
-			rsrc_data->en_cfg = 0x1;
-			rsrc_data->stride = rsrc_data->width * 2;
-			break;
-		case CAM_FORMAT_PLAIN16_10:
-		case CAM_FORMAT_PLAIN16_12:
-		case CAM_FORMAT_PLAIN16_14:
-		case CAM_FORMAT_PLAIN16_16:
-			rsrc_data->width =
-				ALIGNUP(rsrc_data->width * 2, 16) / 16;
-			rsrc_data->en_cfg = 0x1;
-			break;
-		case CAM_FORMAT_PLAIN64:
-			rsrc_data->width =
-				ALIGNUP(rsrc_data->width * 8, 16) / 16;
-			rsrc_data->en_cfg = 0x1;
-			break;
-		default:
-			CAM_ERR(CAM_SFE, "Unsupported RDI format %d",
-				rsrc_data->format);
-			return -EINVAL;
-		}
-
+		rc = cam_sfe_bus_config_rdi_wm(rsrc_data);
+		if (rc)
+			return rc;
 	} else if (sfe_out_res_id == CAM_SFE_BUS_SFE_OUT_RAW_DUMP) {
 		rsrc_data->stride = rsrc_data->width;
 		rsrc_data->en_cfg = 0x1;
 	} else if ((sfe_out_res_id >= CAM_SFE_BUS_SFE_OUT_BE_0) &&
 		(sfe_out_res_id <= CAM_SFE_BUS_SFE_OUT_BHIST_2)) {
-
 		rsrc_data->width = 0;
 		rsrc_data->height = 0;
 		rsrc_data->stride = 1;
 		rsrc_data->en_cfg = (0x1 << 16) | 0x1;
 
 	} else if (sfe_out_res_id == CAM_SFE_BUS_SFE_OUT_LCR) {
-
 		switch (rsrc_data->format) {
 		case CAM_FORMAT_PLAIN16_10:
 		case CAM_FORMAT_PLAIN16_12:
@@ -459,7 +577,6 @@ static int cam_sfe_bus_acquire_wm(
 				rsrc_data->format, sfe_out_res_id);
 			return -EINVAL;
 		}
-
 	} else {
 		CAM_ERR(CAM_SFE, "Invalid out_type:%d requested",
 			sfe_out_res_id);
@@ -535,6 +652,17 @@ static int cam_sfe_bus_start_wm(struct cam_isp_resource_node *wm_res)
 	cam_io_w(rsrc_data->pack_fmt,
 		common_data->mem_base + rsrc_data->hw_regs->packer_cfg);
 
+	/* configure line_done_cfg for RDI0-2 */
+	if ((rsrc_data->index >= 8) &&
+		(rsrc_data->index <= 10)) {
+		CAM_DBG(CAM_SFE, "configure line_done_cfg 0x%x for WM: %d",
+			rsrc_data->common_data->line_done_cfg,
+			rsrc_data->index);
+		cam_io_w_mb(rsrc_data->common_data->line_done_cfg,
+			common_data->mem_base +
+			rsrc_data->hw_regs->line_done_cfg);
+	}
+
 	/* Enable WM */
 	cam_io_w_mb(rsrc_data->en_cfg, common_data->mem_base +
 		rsrc_data->hw_regs->cfg);
@@ -821,7 +949,8 @@ static int cam_sfe_bus_start_comp_grp(
 	if (comp_grp->res_state == CAM_ISP_RESOURCE_STATE_STREAMING)
 		return 0;
 
-	bus_irq_reg_mask[CAM_SFE_IRQ_BUS_REG_STATUS0] =
+	/* CSID buf done register */
+	bus_irq_reg_mask[0] =
 		(0x1 << (rsrc_data->comp_grp_type +
 		rsrc_data->common_data->comp_done_shift));
 
@@ -867,7 +996,6 @@ static int cam_sfe_bus_wr_init_comp_grp(uint32_t index,
 
 	list_add_tail(&comp_grp->list, &bus_priv->free_comp_grp);
 
-	/* TO DO set top half */
 	comp_grp->top_half_handler = NULL;
 	comp_grp->hw_intf = bus_priv->common_data.hw_intf;
 
@@ -969,6 +1097,8 @@ static int cam_sfe_bus_acquire_sfe_out(void *priv, void *acquire_args,
 		acq_args->buf_done_controller;
 	rsrc_data->common_data->event_cb = acq_args->event_cb;
 	rsrc_data->priv = acq_args->priv;
+	rsrc_data->bus_priv = bus_priv;
+
 	secure_caps = cam_sfe_bus_can_be_secure(
 		rsrc_data->out_type);
 	mode = out_acquire_args->out_port_info->secure_mode;
@@ -996,7 +1126,7 @@ static int cam_sfe_bus_acquire_sfe_out(void *priv, void *acquire_args,
 	}
 	mutex_unlock(&rsrc_data->common_data->bus_mutex);
 
-	//bus_priv->tasklet_info = acq_args->tasklet;
+	bus_priv->tasklet_info = acq_args->tasklet;
 	rsrc_node->rdi_only_ctx = 0;
 	rsrc_node->res_id = out_acquire_args->out_port_info->res_type;
 	rsrc_node->tasklet_info = acq_args->tasklet;
@@ -1088,7 +1218,6 @@ static int cam_sfe_bus_release_sfe_out(void *bus_priv, void *release_args,
 
 	for (i = 0; i < rsrc_data->num_wm; i++)
 		cam_sfe_bus_release_wm(bus_priv, &rsrc_data->wm_res[i]);
-	rsrc_data->num_wm = 0;
 
 	if (rsrc_data->comp_grp)
 		cam_sfe_bus_release_comp_grp(bus_priv,
@@ -1139,7 +1268,7 @@ static int cam_sfe_bus_wr_get_evt_payload(
 
 	spin_lock(&common_data->spin_lock);
 
-	if (!common_data->hw_init) {
+	if (!common_data->err_irq_subscribe) {
 		CAM_ERR_RATE_LIMIT(CAM_SFE, "SFE:%d Bus uninitialized",
 			common_data->core_index);
 		*evt_payload = NULL;
@@ -1180,14 +1309,14 @@ static int cam_sfe_bus_wr_put_evt_payload(
 	}
 
 	spin_lock_irqsave(&common_data->spin_lock, flags);
-	if (common_data->hw_init)
+	if (common_data->err_irq_subscribe)
 		list_add_tail(&(*evt_payload)->list,
 			&common_data->free_payload_list);
 	spin_unlock_irqrestore(&common_data->spin_lock, flags);
 
 	*evt_payload = NULL;
 
-	CAM_DBG(CAM_SFE, "Done");
+	CAM_DBG(CAM_SFE, "Exit");
 	return 0;
 }
 
@@ -1196,8 +1325,9 @@ static int cam_sfe_bus_start_sfe_out(
 {
 	int rc = 0, i;
 	struct cam_sfe_bus_wr_out_data      *rsrc_data = NULL;
+	struct cam_sfe_bus_wr_priv          *bus_priv;
 	struct cam_sfe_bus_wr_common_data   *common_data = NULL;
-	uint32_t bus_irq_reg_mask[CAM_SFE_IRQ_REGISTERS_MAX];
+	uint32_t bus_irq_reg_mask[1];
 	uint32_t source_group = 0;
 
 	if (!sfe_out) {
@@ -1206,6 +1336,7 @@ static int cam_sfe_bus_start_sfe_out(
 	}
 
 	rsrc_data = sfe_out->res_priv;
+	bus_priv = rsrc_data->bus_priv;
 	common_data = rsrc_data->common_data;
 	source_group = rsrc_data->source_group;
 
@@ -1217,6 +1348,13 @@ static int cam_sfe_bus_start_sfe_out(
 		return -EACCES;
 	}
 
+	/* subscribe when first out rsrc is streamed on */
+	if (!bus_priv->common_data.err_irq_subscribe) {
+		rc = cam_sfe_bus_subscribe_error_irq(bus_priv);
+		if (rc)
+			return rc;
+	}
+
 	CAM_DBG(CAM_SFE, "Start SFE:%d out_type:0x%X",
 		rsrc_data->common_data->core_index, rsrc_data->out_type);
 
@@ -1255,6 +1393,7 @@ static int cam_sfe_bus_stop_sfe_out(
 {
 	int rc = 0, i;
 	struct cam_sfe_bus_wr_out_data      *rsrc_data = NULL;
+	struct cam_sfe_bus_wr_priv          *bus_priv;
 	struct cam_sfe_bus_wr_common_data   *common_data = NULL;
 
 	if (!sfe_out) {
@@ -1263,6 +1402,7 @@ static int cam_sfe_bus_stop_sfe_out(
 	}
 
 	rsrc_data = sfe_out->res_priv;
+	bus_priv = rsrc_data->bus_priv;
 	common_data = rsrc_data->common_data;
 
 	if (sfe_out->res_state == CAM_ISP_RESOURCE_STATE_AVAILABLE ||
@@ -1278,7 +1418,6 @@ static int cam_sfe_bus_stop_sfe_out(
 	for (i = 0; i < rsrc_data->num_wm; i++)
 		rc = cam_sfe_bus_stop_wm(&rsrc_data->wm_res[i]);
 
-	/* TO DO any IRQ handling */
 	if (sfe_out->irq_handle) {
 		cam_irq_controller_unsubscribe_irq(
 			common_data->buf_done_controller,
@@ -1286,6 +1425,32 @@ static int cam_sfe_bus_stop_sfe_out(
 		sfe_out->irq_handle = 0;
 	}
 
+	/*
+	 * Unsubscribe error irq when first
+	 * out rsrc is streamed off
+	 */
+	if (bus_priv->common_data.err_irq_subscribe) {
+		if (bus_priv->error_irq_handle) {
+			rc = cam_irq_controller_unsubscribe_irq(
+					bus_priv->common_data.bus_irq_controller,
+					bus_priv->error_irq_handle);
+			if (rc)
+				CAM_WARN(CAM_SFE, "failed to unsubscribe error irqs");
+			bus_priv->error_irq_handle = 0;
+		}
+
+		if (bus_priv->bus_irq_handle) {
+			rc = cam_irq_controller_unsubscribe_irq(
+					bus_priv->common_data.sfe_irq_controller,
+					bus_priv->bus_irq_handle);
+			if (rc)
+				CAM_WARN(CAM_SFE, "failed to unsubscribe top irq");
+			bus_priv->bus_irq_handle = 0;
+		}
+
+		bus_priv->common_data.err_irq_subscribe = false;
+	}
+
 	sfe_out->res_state = CAM_ISP_RESOURCE_STATE_RESERVED;
 	return rc;
 }
@@ -1311,19 +1476,17 @@ static int cam_sfe_bus_handle_sfe_out_done_top_half(
 	rsrc_data = sfe_out->res_priv;
 	resource_data = rsrc_data->comp_grp->res_priv;
 
-	CAM_DBG(CAM_SFE, "SFE:%d Bus IRQ status_0: 0x%X status_1: 0x%X",
+	CAM_DBG(CAM_SFE, "SFE:%d Bus IRQ status_0: 0x%X",
 		rsrc_data->common_data->core_index,
-		th_payload->evt_status_arr[0],
-		th_payload->evt_status_arr[1]);
+		th_payload->evt_status_arr[0]);
 
 	rc  = cam_sfe_bus_wr_get_evt_payload(rsrc_data->common_data,
 			&evt_payload);
 	if (rc) {
 		CAM_INFO_RATE_LIMIT(CAM_SFE,
-			"SFE:%d Bus IRQ status_0: 0x%X status_1: 0x%X",
+			"Failed to get payload for SFE:%d Bus IRQ status_0: 0x%X",
 			rsrc_data->common_data->core_index,
-			th_payload->evt_status_arr[0],
-			th_payload->evt_status_arr[1]);
+			th_payload->evt_status_arr[0]);
 		return rc;
 	}
 
@@ -1357,7 +1520,7 @@ static int cam_sfe_bus_handle_comp_done_bottom_half(
 	struct cam_isp_resource_node          *comp_grp = handler_priv;
 	struct cam_sfe_bus_wr_irq_evt_payload *evt_payload = evt_payload_priv;
 	struct cam_sfe_bus_wr_comp_grp_data   *rsrc_data = comp_grp->res_priv;
-	uint32_t                              *cam_ife_irq_regs;
+	uint32_t                              *cam_sfe_irq_regs;
 	uint32_t                               status_0;
 
 	if (!evt_payload)
@@ -1369,8 +1532,8 @@ static int cam_sfe_bus_handle_comp_done_bottom_half(
 		return rc;
 	}
 
-	cam_ife_irq_regs = evt_payload->irq_reg_val;
-	status_0 = cam_ife_irq_regs[CAM_SFE_IRQ_BUS_REG_STATUS0];
+	cam_sfe_irq_regs = evt_payload->irq_reg_val;
+	status_0 = cam_sfe_irq_regs[CAM_SFE_IRQ_BUS_REG_STATUS0];
 
 	if (status_0 & BIT(rsrc_data->comp_grp_type +
 		rsrc_data->common_data->comp_done_shift)) {
@@ -1387,17 +1550,46 @@ static int cam_sfe_bus_handle_comp_done_bottom_half(
 	return rc;
 }
 
+static uint32_t cam_sfe_bus_get_last_consumed_addr(
+	struct cam_sfe_bus_wr_priv *bus_priv,
+	uint32_t res_type,
+	uint32_t *val)
+{
+	struct cam_isp_resource_node             *rsrc_node = NULL;
+	struct cam_sfe_bus_wr_out_data           *rsrc_data = NULL;
+	struct cam_sfe_bus_wr_wm_resource_data   *wm_rsrc_data = NULL;
+	enum cam_sfe_bus_sfe_out_type             res_id;
+
+	res_id = cam_sfe_bus_wr_get_out_res_id(res_type);
+
+	if (res_id >= CAM_SFE_BUS_SFE_OUT_MAX) {
+		CAM_ERR(CAM_ISP, "invalid res id:%u", res_id);
+		return 0;
+	}
+
+	rsrc_node = &bus_priv->sfe_out[res_id];
+	rsrc_data = rsrc_node->res_priv;
+
+	/* All SFE out ports have single WM */
+	wm_rsrc_data = rsrc_data->wm_res->res_priv;
+	*val = cam_io_r_mb(
+		wm_rsrc_data->common_data->mem_base +
+		wm_rsrc_data->hw_regs->addr_status_0);
+
+	return 0;
+}
+
 static int cam_sfe_bus_handle_sfe_out_done_bottom_half(
 	void                *handler_priv,
 	void                *evt_payload_priv)
 {
-	int                                    rc = -EINVAL, num_out = 0, i = 0;
-	struct cam_isp_resource_node          *sfe_out = handler_priv;
-	struct cam_sfe_bus_wr_out_data        *rsrc_data = sfe_out->res_priv;
-	struct cam_sfe_bus_wr_irq_evt_payload *evt_payload = evt_payload_priv;
-	struct cam_isp_hw_event_info           evt_info;
-	void                                  *ctx = NULL;
-	uint32_t                               evt_id = 0, comp_mask = 0;
+	int rc = -EINVAL, num_out = 0, i;
+	uint32_t evt_id = 0, comp_mask = 0, val = 0;
+	struct cam_isp_resource_node           *sfe_out = handler_priv;
+	struct cam_sfe_bus_wr_out_data         *rsrc_data = sfe_out->res_priv;
+	struct cam_sfe_bus_wr_irq_evt_payload  *evt_payload = evt_payload_priv;
+	struct cam_isp_hw_event_info            evt_info;
+	void                                   *ctx = NULL;
 	uint32_t                       out_list[CAM_SFE_BUS_SFE_OUT_MAX];
 
 	rc = cam_sfe_bus_handle_comp_done_bottom_half(
@@ -1420,6 +1612,10 @@ static int cam_sfe_bus_handle_sfe_out_done_bottom_half(
 			comp_mask, out_list, &num_out);
 		for (i = 0; i < num_out; i++) {
 			evt_info.res_id = out_list[i];
+			 cam_sfe_bus_get_last_consumed_addr(
+				rsrc_data->bus_priv,
+				evt_info.res_id, &val);
+			evt_info.reg_val = val;
 			if (rsrc_data->common_data->event_cb)
 				rsrc_data->common_data->event_cb(ctx, evt_id,
 					(void *)&evt_info);
@@ -1681,6 +1877,53 @@ static int cam_sfe_bus_wr_irq_bottom_half(
 	return 0;
 }
 
+static int cam_sfe_bus_subscribe_error_irq(
+	struct cam_sfe_bus_wr_priv          *bus_priv)
+{
+	uint32_t top_irq_reg_mask[CAM_SFE_IRQ_REGISTERS_MAX] = {0};
+
+	/* Subscribe top IRQ */
+	top_irq_reg_mask[0] = (1 << bus_priv->top_irq_shift);
+
+	bus_priv->bus_irq_handle = cam_irq_controller_subscribe_irq(
+		bus_priv->common_data.sfe_irq_controller,
+		CAM_IRQ_PRIORITY_0,
+		top_irq_reg_mask,
+		bus_priv,
+		cam_sfe_bus_wr_handle_bus_irq,
+		NULL,
+		NULL,
+		NULL);
+
+	if (bus_priv->bus_irq_handle < 1) {
+		CAM_ERR(CAM_SFE, "Failed to subscribe BUS TOP IRQ");
+		bus_priv->bus_irq_handle = 0;
+		return -EFAULT;
+	}
+
+	if (bus_priv->tasklet_info != NULL) {
+		bus_priv->error_irq_handle = cam_irq_controller_subscribe_irq(
+			bus_priv->common_data.bus_irq_controller,
+			CAM_IRQ_PRIORITY_0,
+			bus_wr_error_irq_mask,
+			bus_priv,
+			cam_sfe_bus_wr_err_irq_top_half,
+			cam_sfe_bus_wr_irq_bottom_half,
+			bus_priv->tasklet_info,
+			&tasklet_bh_api);
+
+		if (bus_priv->error_irq_handle < 1) {
+			CAM_ERR(CAM_SFE, "Failed to subscribe BUS Error IRQ");
+			bus_priv->error_irq_handle = 0;
+			return -EFAULT;
+		}
+	}
+
+	bus_priv->common_data.err_irq_subscribe = true;
+	CAM_DBG(CAM_SFE, "BUS WR error irq subscribed");
+	return 0;
+}
+
 static int cam_sfe_bus_wr_update_wm(void *priv, void *cmd_args,
 	uint32_t arg_size)
 {
@@ -2053,80 +2296,13 @@ static int cam_sfe_bus_wr_stop_hw(void *hw_priv,
 static int cam_sfe_bus_wr_init_hw(void *hw_priv,
 	void *init_hw_args, uint32_t arg_size)
 {
-	struct cam_sfe_bus_wr_priv    *bus_priv = hw_priv;
-	uint32_t top_irq_reg_mask[CAM_SFE_IRQ_REGISTERS_MAX] = {0};
-
-	if (!bus_priv) {
-		CAM_ERR(CAM_SFE, "Invalid args");
-		return -EINVAL;
-	}
-
-	if (bus_priv->common_data.hw_init)
-		return 0;
-
-	/* Subscribe top IRQ */
-	top_irq_reg_mask[0] = (1 << bus_priv->top_irq_shift);
-
-	bus_priv->bus_irq_handle = cam_irq_controller_subscribe_irq(
-		bus_priv->common_data.sfe_irq_controller,
-		CAM_IRQ_PRIORITY_4,
-		top_irq_reg_mask,
-		bus_priv,
-		cam_sfe_bus_wr_handle_bus_irq,
-		NULL,
-		NULL,
-		NULL);
-
-	if (bus_priv->bus_irq_handle < 1) {
-		CAM_ERR(CAM_SFE, "Failed to subscribe BUS (buf_done) IRQ");
-		bus_priv->bus_irq_handle = 0;
-		return -EFAULT;
-	}
-
-	if (bus_priv->tasklet_info != NULL) {
-		bus_priv->error_irq_handle = cam_irq_controller_subscribe_irq(
-			bus_priv->common_data.bus_irq_controller,
-			CAM_IRQ_PRIORITY_0,
-			bus_wr_error_irq_mask,
-			bus_priv,
-			cam_sfe_bus_wr_err_irq_top_half,
-			cam_sfe_bus_wr_irq_bottom_half,
-			bus_priv->tasklet_info,
-			&tasklet_bh_api);
-
-		if (bus_priv->error_irq_handle < 1) {
-			CAM_ERR(CAM_SFE, "Failed to subscribe BUS Error IRQ");
-			bus_priv->error_irq_handle = 0;
-			return -EFAULT;
-		}
-	}
-
-	/* BUS_WR_TEST_BUS_CTRL */
-	cam_io_w_mb(0x0, bus_priv->common_data.mem_base +
-		bus_priv->common_data.common_reg->test_bus_ctrl);
-
-	bus_priv->common_data.hw_init = true;
-
 	return 0;
 }
 
 static int cam_sfe_bus_wr_deinit_hw(void *hw_priv,
 	void *deinit_hw_args, uint32_t arg_size)
 {
-	struct cam_sfe_bus_wr_priv    *bus_priv = hw_priv;
-	int                            rc = 0;
-
-	if (!bus_priv) {
-		CAM_ERR(CAM_SFE, "Error: Invalid args");
-		return -EINVAL;
-	}
-
-	if (!bus_priv->common_data.hw_init)
-		return 0;
-
-	/* To Do Unsubscribe IRQs */
-
-	return rc;
+	return 0;
 }
 
 static int __cam_sfe_bus_wr_process_cmd(
@@ -2243,7 +2419,8 @@ int cam_sfe_bus_wr_init(
 	bus_priv->common_data.hw_intf            = hw_intf;
 	bus_priv->common_data.common_reg         = &hw_info->common_reg;
 	bus_priv->common_data.comp_done_shift    = hw_info->comp_done_shift;
-	bus_priv->common_data.hw_init            = false;
+	bus_priv->common_data.line_done_cfg      = hw_info->line_done_cfg;
+	bus_priv->common_data.err_irq_subscribe  = false;
 	bus_priv->common_data.sfe_irq_controller = sfe_irq_controller;
 	rc = cam_cpas_get_cpas_hw_version(&bus_priv->common_data.hw_version);
 	if (rc) {
@@ -2321,7 +2498,7 @@ int cam_sfe_bus_wr_init(
 	sfe_bus_local->top_half_handler    = NULL;
 	sfe_bus_local->bottom_half_handler = NULL;
 	sfe_bus_local->hw_ops.process_cmd  = __cam_sfe_bus_wr_process_cmd;
-
+	bus_priv->bus_irq_handle = 0;
 	*sfe_bus = sfe_bus_local;
 
 	CAM_DBG(CAM_SFE, "Exit");
@@ -2378,7 +2555,7 @@ int cam_sfe_bus_wr_deinit(
 	INIT_LIST_HEAD(&bus_priv->common_data.free_payload_list);
 	for (i = 0; i < CAM_SFE_BUS_WR_PAYLOAD_MAX; i++)
 		INIT_LIST_HEAD(&bus_priv->common_data.evt_payload[i].list);
-	bus_priv->common_data.hw_init = false;
+	bus_priv->common_data.err_irq_subscribe = false;
 	spin_unlock_irqrestore(&bus_priv->common_data.spin_lock, flags);
 
 	for (i = 0; i < CAM_SFE_BUS_WR_COMP_GRP_MAX; i++) {

drivers/cam_isp/isp_hw_mgr/isp_hw/sfe_hw/sfe_bus/cam_sfe_bus_wr.h

@@ -131,6 +131,7 @@ struct cam_sfe_bus_sfe_out_hw_info {
  * @sfe_out_hw_info:  SFE output capability
  * @num_comp_grp:     Number of composite groups
  * @comp_done_shift:  Mask shift for comp done mask
+ * @line_done_cfg:    Line done cfg for wr/rd sync
  * @top_irq_shift:    Mask shift for top level BUS WR irq
  */
 struct cam_sfe_bus_wr_hw_info {
@@ -143,6 +144,7 @@ struct cam_sfe_bus_wr_hw_info {
 		sfe_out_hw_info[CAM_SFE_BUS_SFE_OUT_MAX];
 	uint32_t num_comp_grp;
 	uint32_t comp_done_shift;
+	uint32_t line_done_cfg;
 	uint32_t top_irq_shift;
 };
 

drivers/cam_isp/isp_hw_mgr/isp_hw/sfe_hw/sfe_top/cam_sfe_top.c

@@ -46,6 +46,7 @@ struct cam_sfe_top_priv {
 	struct cam_axi_vote             applied_axi_vote;
 	struct cam_sfe_core_cfg         core_cfg;
 	uint32_t                        sfe_debug_cfg;
+	uint32_t                        sensor_sel_diag_cfg;
 	spinlock_t                      spin_lock;
 };
 
@@ -55,6 +56,7 @@ struct cam_sfe_path_data {
 	struct cam_hw_intf                       *hw_intf;
 	struct cam_sfe_top_priv                  *top_priv;
 	struct cam_sfe_top_common_reg_offset     *common_reg;
+	struct cam_sfe_top_common_reg_data       *common_reg_data;
 	struct cam_sfe_modules_common_reg_offset *modules_reg;
 	struct cam_sfe_path_common_reg_data      *path_reg_data;
 	struct cam_hw_soc_info                   *soc_info;
@@ -64,6 +66,8 @@ struct cam_sfe_path_data {
 	cam_hw_mgr_event_cb_func                  event_cb;
 };
 
+static int start_stop_cnt;
+
 static const char *cam_sfe_top_res_id_to_string(
 	uint32_t res_id)
 {
@@ -544,7 +548,6 @@ static int cam_sfe_top_clock_update(
 			"SFE: %d not ready to set clocks yet :%d",
 			res->hw_intf->hw_idx,
 			hw_info->hw_state);
-		rc = -EAGAIN;
 	} else
 		rc = cam_sfe_top_set_hw_clk_rate(top_priv);
 
@@ -555,10 +558,11 @@ static int cam_sfe_set_top_debug(
 	struct cam_sfe_top_priv *top_priv,
 	void *cmd_args)
 {
-	uint32_t *sfe_debug;
+	struct cam_sfe_debug_cfg_params *debug_cfg;
 
-	sfe_debug = (uint32_t *)cmd_args;
-	top_priv->sfe_debug_cfg = *sfe_debug;
+	debug_cfg = (struct cam_sfe_debug_cfg_params *)cmd_args;
+	top_priv->sfe_debug_cfg = debug_cfg->sfe_debug_cfg;
+	top_priv->sensor_sel_diag_cfg = debug_cfg->sfe_sensor_sel;
 
 	return 0;
 }
@@ -802,7 +806,7 @@ static int cam_sfe_top_handle_err_irq_top_half(
 	irq_status = th_payload->evt_status_arr[0];
 
 	base = path_data->mem_base;
-	if (irq_status & path_data->path_reg_data->error_irq_mask) {
+	if (irq_status & path_data->common_reg_data->error_irq_mask) {
 		CAM_ERR(CAM_SFE,
 			"SFE Violation Detected irq_status: 0x%x",
 			irq_status);
@@ -861,10 +865,70 @@ static int cam_sfe_top_handle_irq_top_half(uint32_t evt_id,
 	return rc;
 }
 
+void cam_sfe_top_sel_frame_counter(
+	uint32_t res_id, uint32_t *val,
+	bool read_counter,
+	struct cam_sfe_path_data *path_data)
+{
+	const uint32_t frame_cnt_shift = 0x4;
+	uint32_t frame_cnt0 = 0, frame_cnt1 = 0;
+	struct cam_sfe_top_common_reg_offset *common_reg = NULL;
+
+	if (read_counter) {
+		common_reg = path_data->common_reg;
+		frame_cnt0 = cam_io_r(path_data->mem_base +
+			common_reg->diag_sensor_frame_cnt_status0);
+		frame_cnt1 = cam_io_r(path_data->mem_base +
+			common_reg->diag_sensor_frame_cnt_status1);
+	}
+
+	switch (res_id) {
+	case CAM_ISP_HW_SFE_IN_PIX:
+		*val |= (1 << frame_cnt_shift);
+		if (read_counter)
+			CAM_INFO(CAM_SFE, "IPP frame_cnt 0x%x",
+				frame_cnt0 & 0xFF);
+		break;
+	case CAM_ISP_HW_SFE_IN_RDI0:
+		*val |= (1 << (frame_cnt_shift + 1));
+		if (read_counter)
+			CAM_INFO(CAM_SFE, "RDI0 frame_cnt 0x%x",
+				(frame_cnt0 >> 16) & 0xFF);
+		break;
+	case CAM_ISP_HW_SFE_IN_RDI1:
+		*val |= (1 << (frame_cnt_shift + 2));
+		if (read_counter)
+			CAM_INFO(CAM_SFE, "RDI1 frame_cnt 0x%x",
+				(frame_cnt0 >> 24) & 0xFF);
+		break;
+	case CAM_ISP_HW_SFE_IN_RDI2:
+		*val |= (1 << (frame_cnt_shift + 3));
+		if (read_counter)
+			CAM_INFO(CAM_SFE, "RDI2 frame_cnt 0x%x",
+				frame_cnt1 & 0xFF);
+		break;
+	case CAM_ISP_HW_SFE_IN_RDI3:
+		*val |= (1 << (frame_cnt_shift + 4));
+		if (read_counter)
+			CAM_INFO(CAM_SFE, "RDI3 frame_cnt 0x%x",
+				(frame_cnt1 >> 16) & 0xFF);
+		break;
+	case CAM_ISP_HW_SFE_IN_RDI4:
+		*val |= (1 << (frame_cnt_shift + 5));
+		if (read_counter)
+			CAM_INFO(CAM_SFE, "RDI4 frame_cnt 0x%x",
+				(frame_cnt1 >> 24) & 0xFF);
+		break;
+	default:
+		break;
+	}
+}
+
 static int cam_sfe_top_handle_irq_bottom_half(
 	void *handler_priv, void *evt_payload_priv)
 {
 	int i;
+	uint32_t val0, val1, frame_cnt, offset0, offset1;
 	uint32_t irq_status[CAM_SFE_IRQ_REGISTERS_MAX] = {0};
 	enum cam_sfe_hw_irq_status          ret = CAM_SFE_IRQ_STATUS_MAX;
 	struct cam_isp_hw_event_info        evt_info;
@@ -882,7 +946,7 @@ static int cam_sfe_top_handle_irq_bottom_half(
 	evt_info.reg_val = 0;
 
 	if (irq_status[0] &
-		path_data->path_reg_data->error_irq_mask) {
+		path_data->common_reg_data->error_irq_mask) {
 		if (irq_status[0] & 0x4000)
 			CAM_ERR(CAM_SFE, "PP VIOLATION");
 
@@ -904,13 +968,33 @@ static int cam_sfe_top_handle_irq_bottom_half(
 		ret = CAM_SFE_IRQ_STATUS_VIOLATION;
 	}
 
-	/* To Do Debugfs entry to control the enablement */
 	if (irq_status[0] & path_data->path_reg_data->subscribe_irq_mask) {
 		if (irq_status[0] & path_data->path_reg_data->sof_irq_mask) {
 			CAM_INFO_RATE_LIMIT(CAM_SFE, "SFE:%d Received %s SOF",
 				evt_info.hw_idx,
 				cam_sfe_top_res_id_to_string(res->res_id));
-		}
+			offset0 = path_data->common_reg->diag_sensor_status_0;
+			offset1 = path_data->common_reg->diag_sensor_status_1;
+			/* check for any debug info at SOF */
+			if (top_priv->sfe_debug_cfg &
+				SFE_DEBUG_ENABLE_SENSOR_DIAG_INFO) {
+				val0 =  cam_io_r(path_data->mem_base +
+					offset0);
+				val1 = cam_io_r(path_data->mem_base +
+					offset1);
+				CAM_INFO(CAM_SFE,
+					"SFE:%d HBI: 0x%x VBI: 0x%x NEQ_HBI: %s HBI_MIN_ERR: %s",
+					evt_info.hw_idx, (val0 & 0x3FFF), val1,
+					(val0 & (0x4000) ? "TRUE" : "FALSE"),
+					(val0 & (0x8000) ? "TRUE" : "FALSE"));
+			}
+
+			if (top_priv->sfe_debug_cfg &
+				SFE_DEBUG_ENABLE_FRAME_COUNTER)
+				cam_sfe_top_sel_frame_counter(
+					res->res_id, &frame_cnt,
+					true, path_data);
+			}
 
 		if (irq_status[0] &
 			path_data->path_reg_data->eof_irq_mask) {
@@ -921,15 +1005,6 @@ static int cam_sfe_top_handle_irq_bottom_half(
 		ret = CAM_SFE_IRQ_STATUS_SUCCESS;
 	}
 
-	/* To DO frame counter under debugfs */
-	CAM_DBG(CAM_SFE,
-		"SFE:%d SFE_DIAG_SENSOR_STATUS0: 0x%x SFE_DIAG_SENSOR_STATUS1: 0x%x",
-		evt_info.hw_idx,
-		cam_io_r(path_data->mem_base +
-			path_data->common_reg->diag_sensor_status_0),
-		cam_io_r(path_data->mem_base +
-			path_data->common_reg->diag_sensor_status_1));
-
 	cam_sfe_top_put_evt_payload(top_priv, &payload);
 
 	return ret;
@@ -939,6 +1014,8 @@ int cam_sfe_top_start(
 	void *priv, void *start_args, uint32_t arg_size)
 {
 	int                                   rc = -EINVAL;
+	uint32_t                              val = 0, diag_cfg = 0;
+	bool                                  debug_cfg_enabled = false;
 	struct cam_sfe_top_priv              *top_priv;
 	struct cam_isp_resource_node         *sfe_res;
 	struct cam_hw_info                   *hw_info = NULL;
@@ -979,16 +1056,39 @@ int cam_sfe_top_start(
 			path_data->common_reg->core_cfg));
 
 	/* Enable debug cfg registers */
-	cam_io_w(path_data->path_reg_data->top_debug_cfg_en,
+	cam_io_w(path_data->common_reg_data->top_debug_cfg_en,
 		path_data->mem_base +
 		path_data->common_reg->top_debug_cfg);
 
 	/* Enable sensor diag info */
-	/* TO DO - debug fs to enable*/
-	cam_io_w(path_data->path_reg_data->enable_diagnostic_hw,
-		path_data->mem_base + path_data->common_reg->diag_config);
+	if (top_priv->sfe_debug_cfg &
+		SFE_DEBUG_ENABLE_SENSOR_DIAG_INFO) {
+		if ((top_priv->sensor_sel_diag_cfg) &&
+			(top_priv->sensor_sel_diag_cfg <
+			CAM_SFE_TOP_IN_PORT_MAX))
+			val |= top_priv->sensor_sel_diag_cfg <<
+				path_data->common_reg_data->sensor_sel_shift;
+		debug_cfg_enabled = true;
+	}
+
+	if (top_priv->sfe_debug_cfg & SFE_DEBUG_ENABLE_FRAME_COUNTER) {
+		cam_sfe_top_sel_frame_counter(sfe_res->res_id, &val,
+			false, path_data);
+		debug_cfg_enabled = true;
+	}
 
-	error_mask[0] = path_data->path_reg_data->error_irq_mask;
+	if (debug_cfg_enabled) {
+		diag_cfg = cam_io_r(path_data->mem_base +
+			path_data->common_reg->diag_config);
+		diag_cfg |= val;
+		diag_cfg |= path_data->common_reg_data->enable_diagnostic_hw;
+		CAM_DBG(CAM_SFE, "Diag config 0x%x", diag_cfg);
+		cam_io_w(diag_cfg,
+			path_data->mem_base +
+			path_data->common_reg->diag_config);
+	}
+
+	error_mask[0] = path_data->common_reg_data->error_irq_mask;
 	/* Enable error IRQ by default */
 	if (!path_data->error_irq_handle) {
 		path_data->error_irq_handle = cam_irq_controller_subscribe_irq(
@@ -1008,27 +1108,33 @@ int cam_sfe_top_start(
 		}
 	}
 
-	/* TO DO define the debugfs to enable/disable sof/eof irq */
-	if (!path_data->sof_eof_handle) {
-		sof_eof_mask[0] = path_data->path_reg_data->subscribe_irq_mask;
-		path_data->sof_eof_handle = cam_irq_controller_subscribe_irq(
-			top_priv->common_data.sfe_irq_controller,
-			CAM_IRQ_PRIORITY_1,
-			sof_eof_mask,
-			sfe_res,
-			cam_sfe_top_handle_irq_top_half,
-			cam_sfe_top_handle_irq_bottom_half,
-			sfe_res->tasklet_info,
-			&tasklet_bh_api);
-
-		if (path_data->sof_eof_handle < 1) {
-			CAM_ERR(CAM_SFE, "Failed to subscribe SOF/EOF IRQ");
-			path_data->sof_eof_handle = 0;
-			return -EFAULT;
+	if ((top_priv->sfe_debug_cfg & SFE_DEBUG_ENABLE_SOF_EOF_IRQ) ||
+		(debug_cfg_enabled)) {
+		if (!path_data->sof_eof_handle) {
+			sof_eof_mask[0] =
+				path_data->path_reg_data->subscribe_irq_mask;
+			path_data->sof_eof_handle =
+				cam_irq_controller_subscribe_irq(
+				top_priv->common_data.sfe_irq_controller,
+				CAM_IRQ_PRIORITY_1,
+				sof_eof_mask,
+				sfe_res,
+				cam_sfe_top_handle_irq_top_half,
+				cam_sfe_top_handle_irq_bottom_half,
+				sfe_res->tasklet_info,
+				&tasklet_bh_api);
+
+			if (path_data->sof_eof_handle < 1) {
+				CAM_ERR(CAM_SFE,
+					"Failed to subscribe SOF/EOF IRQ");
+				path_data->sof_eof_handle = 0;
+				return -EFAULT;
+			}
 		}
 	}
 
 	sfe_res->res_state = CAM_ISP_RESOURCE_STATE_STREAMING;
+	start_stop_cnt++;
 	return 0;
 }
 
@@ -1036,6 +1142,8 @@ int cam_sfe_top_stop(
 	void *priv, void *stop_args, uint32_t arg_size)
 {
 	int i;
+	bool debug_cfg_disable = false;
+	uint32_t val = 0, diag_cfg = 0;
 	struct cam_sfe_top_priv       *top_priv;
 	struct cam_isp_resource_node  *sfe_res;
 	struct cam_sfe_path_data      *path_data;
@@ -1070,12 +1178,41 @@ int cam_sfe_top_stop(
 		cam_irq_controller_unsubscribe_irq(
 			top_priv->common_data.sfe_irq_controller,
 			path_data->error_irq_handle);
+		path_data->error_irq_handle = 0;
 	}
 
 	if (path_data->sof_eof_handle > 0) {
 		cam_irq_controller_unsubscribe_irq(
 			top_priv->common_data.sfe_irq_controller,
 			path_data->sof_eof_handle);
+		path_data->sof_eof_handle = 0;
+	}
+
+	if (top_priv->sfe_debug_cfg & SFE_DEBUG_ENABLE_FRAME_COUNTER) {
+		cam_sfe_top_sel_frame_counter(sfe_res->res_id, &val,
+			false, path_data);
+		debug_cfg_disable = true;
+	}
+
+	if (start_stop_cnt)
+		start_stop_cnt--;
+
+	if (!start_stop_cnt &&
+		((top_priv->sfe_debug_cfg &
+		SFE_DEBUG_ENABLE_FRAME_COUNTER) ||
+		(top_priv->sfe_debug_cfg &
+		SFE_DEBUG_ENABLE_SENSOR_DIAG_INFO))) {
+		val |= path_data->common_reg_data->enable_diagnostic_hw;
+		debug_cfg_disable = true;
+	}
+
+	if (debug_cfg_disable) {
+		diag_cfg = cam_io_r(path_data->mem_base +
+			path_data->common_reg->diag_config);
+		diag_cfg &= ~val;
+		cam_io_w(diag_cfg,
+			path_data->mem_base +
+			path_data->common_reg->diag_config);
 	}
 
 	return 0;
@@ -1159,6 +1296,8 @@ int cam_sfe_top_init(
 			path_data->path_reg_data =
 				sfe_top_hw_info->pix_reg_data;
 			path_data->common_reg = sfe_top_hw_info->common_reg;
+			path_data->common_reg_data =
+				sfe_top_hw_info->common_reg_data;
 			path_data->modules_reg =
 				sfe_top_hw_info->modules_hw_info;
 			path_data->hw_intf = hw_intf;
@@ -1184,6 +1323,8 @@ int cam_sfe_top_init(
 				soc_info->reg_map[SFE_CORE_BASE_IDX].mem_base;
 			path_data->hw_intf = hw_intf;
 			path_data->common_reg = sfe_top_hw_info->common_reg;
+			path_data->common_reg_data =
+				sfe_top_hw_info->common_reg_data;
 			path_data->modules_reg =
 				sfe_top_hw_info->modules_hw_info;
 			path_data->soc_info = soc_info;
@@ -1199,6 +1340,10 @@ int cam_sfe_top_init(
 	top_priv->common_data.hw_intf = hw_intf;
 	top_priv->common_data.common_reg =
 		sfe_top_hw_info->common_reg;
+	top_priv->sfe_debug_cfg = 0;
+
+	/* Remove after driver stabilizes */
+	top_priv->sfe_debug_cfg |= SFE_DEBUG_ENABLE_SOF_EOF_IRQ;
 
 	sfe_top->hw_ops.process_cmd = cam_sfe_top_process_cmd;
 	sfe_top->hw_ops.start = cam_sfe_top_start;

drivers/cam_isp/isp_hw_mgr/isp_hw/sfe_hw/sfe_top/cam_sfe_top.h

@@ -69,18 +69,23 @@ struct cam_sfe_modules_common_reg_offset {
 	uint32_t qcfa_hdrc_remo_out_mux_cfg;
 };
 
+struct cam_sfe_top_common_reg_data {
+	uint32_t error_irq_mask;
+	uint32_t enable_diagnostic_hw;
+	uint32_t top_debug_cfg_en;
+	uint32_t sensor_sel_shift;
+};
+
 struct cam_sfe_path_common_reg_data {
 	uint32_t sof_irq_mask;
 	uint32_t eof_irq_mask;
-	uint32_t error_irq_mask;
 	uint32_t subscribe_irq_mask;
-	uint32_t enable_diagnostic_hw;
-	uint32_t top_debug_cfg_en;
 };
 
 struct cam_sfe_top_hw_info {
 	struct cam_sfe_top_common_reg_offset     *common_reg;
 	struct cam_sfe_modules_common_reg_offset *modules_hw_info;
+	struct cam_sfe_top_common_reg_data       *common_reg_data;
 	struct cam_sfe_path_common_reg_data      *pix_reg_data;
 	struct cam_sfe_path_common_reg_data      *rdi_reg_data[CAM_SFE_RDI_MAX];
 	uint32_t                                  num_inputs;