msm: camera: isp: Add support to configure offset dynamically

Add support to configure image address offset dynamically for
SFE RMs and WMs. For SFE RM use existing WM blob to get this info.

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

drivers/cam_isp/isp_hw_mgr/cam_ife_hw_mgr.c

@@ -518,6 +518,29 @@ static int cam_ife_hw_mgr_is_rdi_res(uint32_t res_id)
 	return rc;
 }
 
+static inline bool cam_ife_hw_mgr_is_ife_out_port(uint32_t res_id)
+{
+	bool is_ife_out = false;
+
+	if ((res_id >= CAM_ISP_IFE_OUT_RES_BASE) &&
+		(res_id <= (CAM_ISP_IFE_OUT_RES_BASE +
+		max_ife_out_res)))
+		is_ife_out = true;
+
+	return is_ife_out;
+}
+
+static inline bool cam_ife_hw_mgr_is_sfe_out_port(uint32_t res_id)
+{
+	bool is_sfe_out = false;
+
+	if ((res_id >= CAM_ISP_SFE_OUT_RES_BASE) &&
+		(res_id < CAM_ISP_SFE_OUT_RES_MAX))
+		is_sfe_out = true;
+
+	return is_sfe_out;
+}
+
 static int cam_ife_hw_mgr_notify_overflow(
 	struct cam_isp_hw_event_info    *evt,
 	void                            *ctx)
@@ -1818,10 +1841,7 @@ static int cam_ife_hw_mgr_acquire_res_ife_out_pixel(
 	for (i = 0; i < in_port->num_out_res; i++) {
 		out_port = &in_port->data[i];
 		/* Skip output ports for SFE */
-		if ((out_port->res_type < CAM_ISP_IFE_OUT_RES_BASE) ||
-			(out_port->res_type >
-			(CAM_ISP_IFE_OUT_RES_BASE +
-			max_ife_out_res)))
+		if (!cam_ife_hw_mgr_is_ife_out_port(out_port->res_type))
 			continue;
 
 		if (cam_ife_hw_mgr_is_rdi_res(out_port->res_type))
@@ -2013,8 +2033,7 @@ static int cam_ife_hw_mgr_acquire_res_sfe_out_pix(
 		out_port = &in_port->data[i];
 
 		/* Skip IFE ports */
-		if (out_port->res_type < CAM_ISP_SFE_OUT_RES_BASE ||
-			out_port->res_type >= CAM_ISP_SFE_OUT_RES_MAX)
+		if (!cam_ife_hw_mgr_is_sfe_out_port(out_port->res_type))
 			continue;
 
 		if (cam_ife_hw_mgr_is_sfe_rdi_res(out_port->res_type))
@@ -7091,6 +7110,7 @@ static int cam_isp_blob_sfe_scratch_buf_update(
 		port_info->height = buffer_info->height;
 		port_info->stride = buffer_info->stride;
 		port_info->slice_height = buffer_info->slice_height;
+		port_info->offset = 0;
 		CAM_DBG(CAM_ISP,
 			"res_id: 0x%x w: 0x%x h: 0x%x s: 0x%x sh: 0x%x addr: %pK",
 			port_info->res_id, port_info->width,
@@ -7904,6 +7924,86 @@ static int cam_isp_blob_sfe_clock_update(
 	return rc;
 }
 
+static int cam_isp_blob_sfe_rd_update(
+	uint32_t                               blob_type,
+	uint32_t                               kmd_buf_remain_size,
+	uint32_t                              *cmd_buf_addr,
+	uint32_t                              *total_used_bytes,
+	struct cam_ife_hw_mgr_ctx             *ctx,
+	struct cam_isp_generic_blob_info      *blob_info,
+	struct cam_isp_vfe_wm_config          *wm_config)
+{
+	int                                   rc;
+	uint32_t                              bytes_used = 0;
+	bool                                  found = false;
+	struct cam_isp_hw_mgr_res            *sfe_rd_res;
+
+	list_for_each_entry(sfe_rd_res, &ctx->res_list_ife_in_rd,
+		list) {
+		if (sfe_rd_res->res_id == wm_config->port_type) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		CAM_ERR(CAM_ISP,
+			"Failed to find SFE rd resource: %u, check if rsrc is acquired",
+			wm_config->port_type);
+		return -EINVAL;
+	}
+
+	CAM_DBG(CAM_ISP, "SFE RM config for port: 0x%x",
+		wm_config->port_type);
+
+	rc = cam_isp_add_cmd_buf_update(
+		sfe_rd_res, blob_type,
+		CAM_ISP_HW_CMD_FE_UPDATE_BUS_RD,
+		blob_info->base_info->idx,
+		(void *)cmd_buf_addr,
+		kmd_buf_remain_size,
+		(void *)wm_config,
+		&bytes_used);
+	if (rc < 0) {
+		CAM_ERR(CAM_ISP,
+			"Failed to update SFE RM config out_type:0x%X base_idx:%d bytes_used:%u rc:%d",
+			wm_config->port_type, blob_info->base_info->idx,
+			bytes_used, rc);
+		return rc;
+	}
+
+	*total_used_bytes += bytes_used;
+	return rc;
+}
+
+static int cam_ife_hw_mgr_update_scratch_offset(
+	struct cam_ife_hw_mgr_ctx             *ctx,
+	struct cam_isp_vfe_wm_config          *wm_config)
+{
+	uint32_t res_id;
+	struct cam_sfe_scratch_buf_info       *port_info;
+
+	if (!ctx->sfe_info.scratch_config->config_done) {
+		CAM_ERR(CAM_ISP,
+			"Scratch buffers not configured on ctx: %u",
+			ctx->ctx_index);
+		return -EINVAL;
+	}
+
+	if ((wm_config->port_type - CAM_ISP_SFE_OUT_RES_RDI_0) >=
+		ctx->sfe_info.num_fetches)
+		return 0;
+
+	res_id = wm_config->port_type & 0xFF;
+	port_info = &ctx->sfe_info.scratch_config->buf_info[res_id];
+	port_info->offset = wm_config->offset;
+
+	CAM_DBG(CAM_ISP, "Scratch offset: %u updated for: %s",
+		port_info->offset, wm_config->port_type);
+
+	return 0;
+}
+
 static int cam_isp_blob_vfe_out_update(
 	uint32_t                               blob_type,
 	struct cam_isp_generic_blob_info      *blob_info,
@@ -7916,12 +8016,14 @@ static int cam_isp_blob_vfe_out_update(
 	struct cam_kmd_buf_info               *kmd_buf_info;
 	struct cam_ife_hw_mgr_ctx             *ctx = NULL;
 	struct cam_isp_hw_mgr_res             *isp_out_res;
+	enum cam_isp_hw_sfe_in                 rd_path = CAM_ISP_HW_SFE_IN_MAX;
 	uint32_t                               res_id_out, i;
 	uint32_t                               total_used_bytes = 0;
 	uint32_t                               kmd_buf_remain_size;
 	uint32_t                              *cmd_buf_addr;
 	uint32_t                               bytes_used = 0;
 	int                                    rc = 0;
+	bool                                   rm_config = false;
 
 	ctx = prepare->ctxt_to_hw_map;
 
@@ -7935,16 +8037,18 @@ static int cam_isp_blob_vfe_out_update(
 	kmd_buf_info = blob_info->kmd_buf_info;
 	for (i = 0; i < vfe_out_config->num_ports; i++) {
 		wm_config = &vfe_out_config->wm_config[i];
-		res_id_out = wm_config->port_type & 0xFF;
+		if ((hw_type == CAM_ISP_HW_TYPE_VFE) &&
+			(!cam_ife_hw_mgr_is_ife_out_port(wm_config->port_type)))
+			continue;
 
-		CAM_DBG(CAM_ISP, "%s out config idx: %d port: 0x%x",
-			(hw_type == CAM_ISP_HW_TYPE_SFE ? "SFE" : "VFE"),
-			i, wm_config->port_type);
+		if (hw_type == CAM_ISP_HW_TYPE_SFE) {
+			rd_path = cam_ife_hw_mgr_get_sfe_rd_res_id(wm_config->port_type);
+			if ((!cam_ife_hw_mgr_is_sfe_out_port(wm_config->port_type)) &&
+				(rd_path == CAM_ISP_HW_SFE_IN_MAX))
+				continue;
 
-		if (res_id_out >= size_isp_out) {
-			CAM_ERR(CAM_ISP, "Invalid out port:0x%x",
-				wm_config->port_type);
-			return -EINVAL;
+			if (rd_path != CAM_ISP_HW_SFE_IN_MAX)
+				rm_config = true;
 		}
 
 		if ((kmd_buf_info->used_bytes
@@ -7963,10 +8067,43 @@ static int cam_isp_blob_vfe_out_update(
 		cmd_buf_addr = kmd_buf_info->cpu_addr +
 			(kmd_buf_info->used_bytes / 4) +
 			(total_used_bytes / 4);
-		if (hw_type == CAM_ISP_HW_TYPE_SFE)
+
+		if (rm_config) {
+			rc = cam_isp_blob_sfe_rd_update(blob_type,
+				kmd_buf_remain_size, cmd_buf_addr,
+				&total_used_bytes, ctx, blob_info, wm_config);
+			if (rc)
+				return rc;
+
+			rm_config = false;
+			continue;
+		}
+
+		res_id_out = wm_config->port_type & 0xFF;
+
+		CAM_DBG(CAM_ISP, "%s out config idx: %d port: 0x%x",
+			(hw_type == CAM_ISP_HW_TYPE_SFE ? "SFE" : "VFE"),
+			i, wm_config->port_type);
+
+		if (res_id_out >= size_isp_out) {
+			CAM_ERR(CAM_ISP, "Invalid out port:0x%x",
+				wm_config->port_type);
+			return -EINVAL;
+		}
+
+		if (hw_type == CAM_ISP_HW_TYPE_SFE) {
+			/* Update offset for scratch to compare for buf done */
+			if ((ctx->flags.is_sfe_shdr) &&
+				(cam_ife_hw_mgr_is_sfe_rdi_for_fetch(wm_config->port_type))) {
+				rc = cam_ife_hw_mgr_update_scratch_offset(ctx, wm_config);
+				if (rc)
+					return rc;
+			}
+
 			isp_out_res = &ctx->res_list_sfe_out[res_id_out];
-		else
+		} else {
 			isp_out_res = &ctx->res_list_ife_out[res_id_out];
+		}
 
 		rc = cam_isp_add_cmd_buf_update(
 			isp_out_res, blob_type,
@@ -11505,6 +11642,7 @@ static int cam_ife_hw_mgr_check_rdi_scratch_buf_done(
 {
 	int rc = 0;
 	struct cam_sfe_scratch_buf_info *buf_info;
+	dma_addr_t final_addr;
 	uint32_t cmp_addr = 0;
 
 	if (!scratch_cfg->config_done) {
@@ -11516,9 +11654,10 @@ static int cam_ife_hw_mgr_check_rdi_scratch_buf_done(
 	case CAM_ISP_SFE_OUT_RES_RDI_0:
 	case CAM_ISP_SFE_OUT_RES_RDI_1:
 	case CAM_ISP_SFE_OUT_RES_RDI_2:
-		buf_info = &scratch_cfg->buf_info[res_id - CAM_ISP_SFE_OUT_RES_BASE];
+		buf_info = &scratch_cfg->buf_info[res_id - CAM_ISP_SFE_OUT_RES_RDI_0];
+		final_addr = buf_info->io_addr + buf_info->offset;
 		cmp_addr = cam_smmu_is_expanded_memory() ?
-			CAM_36BIT_INTF_GET_IOVA_BASE(buf_info->io_addr) : buf_info->io_addr;
+			CAM_36BIT_INTF_GET_IOVA_BASE(final_addr) : final_addr;
 		if (cmp_addr == last_consumed_addr) {
 			CAM_DBG(CAM_ISP, "SFE RDI%u buf done for scratch - skip ctx notify",
 				(res_id - CAM_ISP_SFE_OUT_RES_BASE));

drivers/cam_isp/isp_hw_mgr/cam_ife_hw_mgr.h

@@ -78,6 +78,7 @@ struct cam_ife_hw_mgr_debug {
  * @slice_height: Height in lines
  * @io_addr: Buffer address
  * @res_id: Resource type
+ * @offset: Buffer offset
  * @is_secure: secure scratch buffer
  */
 struct cam_sfe_scratch_buf_info {
@@ -87,6 +88,7 @@ struct cam_sfe_scratch_buf_info {
 	uint32_t   slice_height;
 	dma_addr_t io_addr;
 	uint32_t   res_id;
+	uint32_t   offset;
 	bool       is_secure;
 };
 

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

@@ -94,11 +94,11 @@ struct cam_sfe_bus_rd_rm_resource_data {
 	uint32_t             unpacker_cfg;
 	uint32_t             burst_len;
 	uint32_t             en_cfg;
-	dma_addr_t           img_addr;
 	uint32_t             input_if_cmd;
 	bool                 enable_caching;
 	uint32_t             cache_cfg;
 	uint32_t             current_scid;
+	uint32_t             offset;
 };
 
 struct cam_sfe_bus_rd_data {
@@ -404,6 +404,7 @@ static int cam_sfe_bus_acquire_rm(
 	rsrc_data->latency_buf_allocation =
 		BUS_RD_DEFAULT_LATENCY_BUF_ALLOC;
 	rsrc_data->enable_caching =  false;
+	rsrc_data->offset = 0;
 	/* Default register value */
 	rsrc_data->cache_cfg = 0x20;
 
@@ -438,13 +439,17 @@ static int cam_sfe_bus_release_rm(void          *bus_priv,
 
 static int cam_sfe_bus_start_rm(struct cam_isp_resource_node *rm_res)
 {
+	uint32_t width_in_bytes = 0;
 	struct cam_sfe_bus_rd_rm_resource_data  *rm_data;
 	struct cam_sfe_bus_rd_common_data       *common_data;
 
 	rm_data = rm_res->res_priv;
 	common_data = rm_data->common_data;
 
-	cam_io_w_mb(rm_data->width, common_data->mem_base +
+	cam_sfe_bus_rd_pxls_to_bytes(rm_data->width,
+		rm_data->unpacker_cfg, &width_in_bytes);
+
+	cam_io_w_mb(width_in_bytes, common_data->mem_base +
 		rm_data->hw_regs->buf_width);
 	cam_io_w_mb(rm_data->height, common_data->mem_base +
 		rm_data->hw_regs->buf_height);
@@ -459,10 +464,10 @@ static int cam_sfe_bus_start_rm(struct cam_isp_resource_node *rm_res)
 	rm_res->res_state = CAM_ISP_RESOURCE_STATE_STREAMING;
 
 	CAM_DBG(CAM_SFE,
-		"Start SFE:%d RM:%d offset:0x%X width:%d height:%d",
+		"Start SFE:%d RM:%d offset:0x%X width:%d[in bytes: %u] height:%d",
 		rm_data->common_data->core_index, rm_data->index,
 		(uint32_t) rm_data->hw_regs->cfg,
-		rm_data->width, rm_data->height);
+		rm_data->width, width_in_bytes, rm_data->height);
 	CAM_DBG(CAM_SFE, "RM:%d pk_fmt:%d stride:%d", rm_data->index,
 		rm_data->unpacker_cfg, rm_data->stride);
 
@@ -481,6 +486,7 @@ static int cam_sfe_bus_stop_rm(struct cam_isp_resource_node *rm_res)
 
 	rm_res->res_state = CAM_ISP_RESOURCE_STATE_RESERVED;
 	rsrc_data->enable_caching =  false;
+	rsrc_data->offset = 0;
 
 	CAM_DBG(CAM_SFE, "SFE:%d RM:%d stopped",
 		rsrc_data->common_data->core_index, rsrc_data->index);
@@ -1143,7 +1149,7 @@ static int cam_sfe_bus_rd_config_rm(void *priv, void *cmd_args,
 	struct cam_sfe_bus_rd_data             *sfe_bus_rd_data = NULL;
 	struct cam_sfe_bus_rd_rm_resource_data *rm_data = NULL;
 	struct cam_sfe_bus_cache_dbg_cfg       *cache_dbg_cfg = NULL;
-	uint32_t width = 0, height = 0, stride = 0;
+	uint32_t width = 0, height = 0, stride = 0, width_in_bytes = 0;
 	uint32_t  i;
 
 	bus_priv = (struct cam_sfe_bus_rd_priv  *) priv;
@@ -1173,13 +1179,19 @@ static int cam_sfe_bus_rd_config_rm(void *priv, void *cmd_args,
 		rm_data = sfe_bus_rd_data->rm_res[i]->res_priv;
 
 		stride = update_buf->rm_update->stride;
-		width = update_buf->rm_update->width;
-		height = update_buf->rm_update->height;
+		if (rm_data->width && rm_data->height) {
+			width =  rm_data->width;
+			height = rm_data->height;
+		} else {
+			width = update_buf->rm_update->width;
+			height = update_buf->rm_update->height;
+		}
 
 		/* update size register */
 		cam_sfe_bus_rd_pxls_to_bytes(width,
-			rm_data->unpacker_cfg, &rm_data->width);
+			rm_data->unpacker_cfg, &width_in_bytes);
 		rm_data->height = height;
+		rm_data->width = width;
 
 		rm_data->cache_cfg = 0x20;
 		if ((!cache_dbg_cfg->disable_for_scratch) &&
@@ -1200,17 +1212,16 @@ static int cam_sfe_bus_rd_config_rm(void *priv, void *cmd_args,
 			rm_data->common_data->core_index,
 			rm_data->index, rm_data->cache_cfg);
 
-		cam_io_w_mb(rm_data->width,
+		cam_io_w_mb(width_in_bytes,
 			rm_data->common_data->mem_base +
 			rm_data->hw_regs->buf_width);
-		cam_io_w_mb(rm_data->height,
+		cam_io_w_mb(height,
 			rm_data->common_data->mem_base +
 			rm_data->hw_regs->buf_height);
 
-		CAM_DBG(CAM_SFE, "SFE:%d RM:%d width:0x%X height:0x%X",
+		CAM_DBG(CAM_SFE, "SFE:%d RM:%d width:0x%X[in bytes: 0x%x] height:0x%X",
 			rm_data->common_data->core_index,
-			rm_data->index, rm_data->width,
-			rm_data->height);
+			rm_data->index, width, width_in_bytes, height);
 
 		rm_data->stride = stride;
 		cam_io_w_mb(stride,
@@ -1220,15 +1231,16 @@ static int cam_sfe_bus_rd_config_rm(void *priv, void *cmd_args,
 			rm_data->common_data->core_index,
 			rm_data->index, stride);
 
-		cam_io_w_mb(update_buf->rm_update->image_buf[i],
+		cam_io_w_mb(
+			(update_buf->rm_update->image_buf[i] + rm_data->offset),
 			rm_data->common_data->mem_base +
 			rm_data->hw_regs->image_addr);
-		CAM_DBG(CAM_SFE, "SFE:%d RM:%d image_address:0x%llx",
+
+		CAM_DBG(CAM_SFE, "SFE:%d RM:%d image_address:0x%X offset: 0x%x",
 			rm_data->common_data->core_index,
 			rm_data->index,
-			(update_buf->rm_update->image_buf[i]));
-		rm_data->img_addr =
-			update_buf->rm_update->image_buf[i];
+			update_buf->rm_update->image_buf[i],
+			rm_data->offset);
 	}
 
 	return 0;
@@ -1246,7 +1258,7 @@ static int cam_sfe_bus_rd_update_rm(void *priv, void *cmd_args,
 	struct cam_sfe_bus_cache_dbg_cfg       *cache_dbg_cfg = NULL;
 	uint32_t *reg_val_pair;
 	uint32_t num_regval_pairs = 0;
-	uint32_t width = 0, height = 0, stride = 0;
+	uint32_t width = 0, height = 0, stride = 0, width_in_bytes = 0;
 	uint32_t  i, j, size = 0;
 
 	bus_priv = (struct cam_sfe_bus_rd_priv  *) priv;
@@ -1295,10 +1307,17 @@ static int cam_sfe_bus_rd_update_rm(void *priv, void *cmd_args,
 			height = io_cfg->planes[i].height;
 		}
 
+		/* If width & height updated in blob, use that */
+		if (rm_data->width && rm_data->height) {
+			width =  rm_data->width;
+			height = rm_data->height;
+		}
+
 		/* update size register */
 		cam_sfe_bus_rd_pxls_to_bytes(width,
-			rm_data->unpacker_cfg, &rm_data->width);
+			rm_data->unpacker_cfg, &width_in_bytes);
 		rm_data->height = height;
+		rm_data->width = width;
 
 		rm_data->cache_cfg = 0x20;
 		if (rm_data->enable_caching) {
@@ -1337,13 +1356,13 @@ skip_cache_cfg:
 			rm_data->index, reg_val_pair[j-1]);
 
 		CAM_SFE_ADD_REG_VAL_PAIR(reg_val_pair, j,
-			rm_data->hw_regs->buf_width, rm_data->width);
+			rm_data->hw_regs->buf_width, width_in_bytes);
 		CAM_SFE_ADD_REG_VAL_PAIR(reg_val_pair, j,
-			rm_data->hw_regs->buf_height, rm_data->height);
-		CAM_DBG(CAM_SFE, "SFE:%d RM:%d width:0x%X height:0x%X",
+			rm_data->hw_regs->buf_height, height);
+		CAM_DBG(CAM_SFE, "SFE:%d RM:%d width:0x%X [in bytes: 0x%x] height:0x%X",
 			rm_data->common_data->core_index,
 			rm_data->index, rm_data->width,
-			rm_data->height);
+			width_in_bytes, rm_data->height);
 
 		rm_data->stride = stride;
 		CAM_SFE_ADD_REG_VAL_PAIR(reg_val_pair, j,
@@ -1354,11 +1373,10 @@ skip_cache_cfg:
 
 		CAM_SFE_ADD_REG_VAL_PAIR(reg_val_pair, j,
 			rm_data->hw_regs->image_addr,
-			update_buf->rm_update->image_buf[i]);
+			(update_buf->rm_update->image_buf[i] + rm_data->offset));
 		CAM_DBG(CAM_SFE, "SFE:%d RM:%d image_address:0x%X",
 			rm_data->common_data->core_index,
 			rm_data->index, reg_val_pair[j-1]);
-		rm_data->img_addr = reg_val_pair[j-1];
 	}
 
 	num_regval_pairs = j / 2;
@@ -1480,6 +1498,44 @@ static int cam_sfe_bus_deinit_hw(void *hw_priv,
 	return 0;
 }
 
+static int cam_sfe_bus_rd_config_update(
+	void *priv, void *cmd_args, uint32_t arg_size)
+{
+	int i;
+	struct cam_sfe_bus_rd_priv             *bus_priv;
+	struct cam_isp_hw_get_cmd_update       *rm_config_update;
+	struct cam_isp_vfe_wm_config           *rm_config = NULL;
+	struct cam_sfe_bus_rd_data             *sfe_bus_rd_data = NULL;
+	struct cam_sfe_bus_rd_rm_resource_data *rm_data = NULL;
+
+	bus_priv = (struct cam_sfe_bus_rd_priv  *) priv;
+	rm_config_update =  (struct cam_isp_hw_get_cmd_update *) cmd_args;
+	rm_config = (struct cam_isp_vfe_wm_config  *)
+		rm_config_update->data;
+
+	sfe_bus_rd_data = (struct cam_sfe_bus_rd_data *)
+		rm_config_update->res->res_priv;
+
+	if (!sfe_bus_rd_data) {
+		CAM_ERR(CAM_SFE, "Failed! Invalid data");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < sfe_bus_rd_data->num_rm; i++) {
+		rm_data = sfe_bus_rd_data->rm_res[i]->res_priv;
+
+		rm_data->offset = rm_config->offset;
+		rm_data->width = rm_config->width;
+		rm_data->height = rm_config->height;
+		CAM_DBG(CAM_SFE,
+			"SFE: %u RM: %d offset: %u width: %u height: %u",
+			rm_data->index, rm_data->index, rm_data->offset,
+			rm_data->width, rm_data->height);
+	}
+
+	return 0;
+}
+
 static int cam_sfe_bus_rd_cache_config(void *priv, void *cmd_args,
 	uint32_t arg_size)
 {
@@ -1563,8 +1619,7 @@ static int cam_sfe_bus_rd_process_cmd(
 		rc = cam_sfe_bus_rd_get_secure_mode(priv, cmd_args, arg_size);
 		break;
 	case CAM_ISP_HW_CMD_FE_UPDATE_BUS_RD:
-		/* Currently no need of blob cfg for SFE RD */
-		rc = 0;
+		rc = cam_sfe_bus_rd_config_update(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);

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

@@ -393,6 +393,7 @@ static void cam_sfe_bus_wr_print_constraint_errors(
 }
 
 static void cam_sfe_bus_wr_get_constraint_errors(
+	bool                       *skip_error_notify,
 	struct cam_sfe_bus_wr_priv *bus_priv)
 {
 	uint32_t i, j, constraint_errors;
@@ -420,6 +421,19 @@ static void cam_sfe_bus_wr_get_constraint_errors(
 				if (!constraint_errors)
 					continue;
 
+				/*
+				 * Due to a HW bug in constraint checker skip addr unalign
+				 * for RDI clients
+				 */
+				if ((out_rsrc_data->out_type >= CAM_SFE_BUS_SFE_OUT_RDI0) &&
+					(out_rsrc_data->out_type <= CAM_SFE_BUS_SFE_OUT_RDI4) &&
+					(constraint_errors >> 21)) {
+					*skip_error_notify = true;
+					CAM_DBG(CAM_SFE, "WM: %s constraint_error: 0x%x",
+						wm_name, constraint_errors);
+					continue;
+				}
+
 				cam_sfe_bus_wr_print_constraint_errors(
 					bus_priv, wm_name, constraint_errors);
 			}
@@ -599,6 +613,7 @@ static int cam_sfe_bus_acquire_wm(
 	rsrc_data->acquired_height = out_port_info->height;
 	rsrc_data->is_dual = is_dual;
 	rsrc_data->enable_caching =  false;
+	rsrc_data->offset = 0;
 
 	/* RDI0-4 line based mode by default */
 	if (sfe_out_res_id == CAM_SFE_BUS_SFE_OUT_RDI0 ||
@@ -794,6 +809,7 @@ static int cam_sfe_bus_stop_wm(struct cam_isp_resource_node *wm_res)
 	rsrc_data->init_cfg_done = false;
 	rsrc_data->hfr_cfg_done = false;
 	rsrc_data->enable_caching =  false;
+	rsrc_data->offset = 0;
 
 	return 0;
 }
@@ -1987,12 +2003,12 @@ static int cam_sfe_bus_wr_irq_bottom_half(
 	void *handler_priv, void *evt_payload_priv)
 {
 	int i;
-	uint32_t status = 0;
+	uint32_t status = 0, cons_violation = 0;
+	bool skip_err_notify = false;
 	struct cam_sfe_bus_wr_priv            *bus_priv = handler_priv;
 	struct cam_sfe_bus_wr_common_data     *common_data;
 	struct cam_isp_hw_event_info           evt_info;
 	struct cam_sfe_bus_wr_irq_evt_payload *evt_payload = evt_payload_priv;
-	uint32_t cons_violation = 0;
 
 	if (!handler_priv || !evt_payload_priv)
 		return -EINVAL;
@@ -2014,33 +2030,35 @@ static int cam_sfe_bus_wr_irq_bottom_half(
 			bus_priv);
 
 	if (cons_violation)
-		cam_sfe_bus_wr_get_constraint_errors(bus_priv);
+		cam_sfe_bus_wr_get_constraint_errors(&skip_err_notify, bus_priv);
 
 	cam_sfe_bus_wr_put_evt_payload(common_data, &evt_payload);
 
-	evt_info.hw_idx = common_data->core_index;
-	evt_info.hw_type = CAM_ISP_HW_TYPE_SFE;
-	evt_info.res_type = CAM_ISP_RESOURCE_SFE_OUT;
-	evt_info.res_id = CAM_SFE_BUS_SFE_OUT_MAX;
-	evt_info.err_type = CAM_SFE_IRQ_STATUS_VIOLATION;
+	if (!skip_err_notify) {
+		evt_info.hw_idx = common_data->core_index;
+		evt_info.hw_type = CAM_ISP_HW_TYPE_SFE;
+		evt_info.res_type = CAM_ISP_RESOURCE_SFE_OUT;
+		evt_info.res_id = CAM_SFE_BUS_SFE_OUT_MAX;
+		evt_info.err_type = CAM_SFE_IRQ_STATUS_VIOLATION;
 
-	if (common_data->event_cb) {
-		struct cam_isp_resource_node      *out_rsrc_node = NULL;
-		struct cam_sfe_bus_wr_out_data    *out_rsrc_data = NULL;
+		if (common_data->event_cb) {
+			struct cam_isp_resource_node      *out_rsrc_node = NULL;
+			struct cam_sfe_bus_wr_out_data    *out_rsrc_data = NULL;
 
-		for (i = 0; i < bus_priv->num_out; i++) {
-			out_rsrc_node = &bus_priv->sfe_out[i];
+			for (i = 0; i < bus_priv->num_out; i++) {
+				out_rsrc_node = &bus_priv->sfe_out[i];
 
-			if (!out_rsrc_node || !out_rsrc_node->res_priv)
-				continue;
+				if (!out_rsrc_node || !out_rsrc_node->res_priv)
+					continue;
 
-			if (out_rsrc_node->res_state != CAM_ISP_RESOURCE_STATE_STREAMING)
-				continue;
+				if (out_rsrc_node->res_state != CAM_ISP_RESOURCE_STATE_STREAMING)
+					continue;
 
-			out_rsrc_data = out_rsrc_node->res_priv;
-			common_data->event_cb(out_rsrc_data->priv,
-				CAM_ISP_HW_EVENT_ERROR, (void *)&evt_info);
-			break;
+				out_rsrc_data = out_rsrc_node->res_priv;
+				common_data->event_cb(out_rsrc_data->priv,
+					CAM_ISP_HW_EVENT_ERROR, (void *)&evt_info);
+				break;
+			}
 		}
 	}
 
@@ -2230,19 +2248,15 @@ skip_cache_cfg:
 
 		/* WM Image address */
 		for (k = 0; k < loop_size; k++) {
-			if (wm_data->en_cfg & (0x3 << 16))
-				CAM_SFE_ADD_REG_VAL_PAIR(reg_val_pair, j,
-					wm_data->hw_regs->image_addr,
-					(update_buf->wm_update->image_buf[i] +
-					wm_data->offset + k * frame_inc));
-			else
-				CAM_SFE_ADD_REG_VAL_PAIR(reg_val_pair, j,
-					wm_data->hw_regs->image_addr,
-					(update_buf->wm_update->image_buf[i] +
-					k * frame_inc));
-
-			CAM_DBG(CAM_SFE, "WM:%d image address 0x%X",
-				wm_data->index, reg_val_pair[j-1]);
+			CAM_SFE_ADD_REG_VAL_PAIR(reg_val_pair, j,
+				wm_data->hw_regs->image_addr,
+				(update_buf->wm_update->image_buf[i] +
+				wm_data->offset + k * frame_inc));
+			update_buf->wm_update->image_buf_offset[i] =
+				wm_data->offset;
+
+			CAM_DBG(CAM_SFE, "WM:%d image address 0x%X offset: 0x%x",
+				wm_data->index, reg_val_pair[j-1], wm_data->offset);
 		}
 
 		CAM_SFE_ADD_REG_VAL_PAIR(reg_val_pair, j,
@@ -2374,16 +2388,13 @@ static int cam_sfe_bus_wr_config_wm(void *priv, void *cmd_args,
 
 		/* WM Image address */
 		for (k = 0; k < loop_size; k++) {
-			if (wm_data->en_cfg & (0x3 << 16))
-				cam_io_w_mb((update_buf->wm_update->image_buf[i] +
-					wm_data->offset + k * frame_inc),
-					wm_data->common_data->mem_base +
-					wm_data->hw_regs->image_addr);
-			else
-				cam_io_w_mb((update_buf->wm_update->image_buf[i] +
-					k * frame_inc),
-					wm_data->common_data->mem_base +
-					wm_data->hw_regs->image_addr);
+			cam_io_w_mb((update_buf->wm_update->image_buf[i] +
+				wm_data->offset + k * frame_inc),
+				wm_data->common_data->mem_base +
+				wm_data->hw_regs->image_addr);
+			CAM_DBG(CAM_SFE, "WM:%d image address: 0x%x offset: 0x%x",
+				wm_data->index, update_buf->wm_update->image_buf[i],
+				wm_data->offset);
 		}
 
 		cam_io_w_mb(frame_inc,
@@ -2631,16 +2642,23 @@ static int cam_sfe_bus_wr_update_wm_config(
 
 		wm_data->en_cfg = (wm_config->wm_mode << 16) | 0x1;
 		wm_data->width  = wm_config->width;
+		if ((sfe_out_data->out_type >= CAM_SFE_BUS_SFE_OUT_RDI0) &&
+			(sfe_out_data->out_type <= CAM_SFE_BUS_SFE_OUT_RDI4)) {
+			/* WM mode enum starts at 1, userland to send based on HW desc */
+			wm_data->wm_mode = wm_config->wm_mode + 1;
+			cam_sfe_bus_config_rdi_wm(wm_data);
+		}
 
 		if (i == PLANE_C)
 			wm_data->height = wm_config->height / 2;
 		else
 			wm_data->height = wm_config->height;
 
+		wm_data->offset = wm_config->offset;
 		CAM_DBG(CAM_SFE,
-			"WM:%d en_cfg:0x%X height:%d width:%d",
+			"WM:%d en_cfg:0x%X height:%d width:%d offset:%u packer_fmt: 0x%x",
 			wm_data->index, wm_data->en_cfg, wm_data->height,
-			wm_data->width);
+			wm_data->width, wm_data->offset, wm_data->pack_fmt);
 	}
 
 	return 0;