@@ -714,38 +714,13 @@ static int csi2_link_setup(struct media_entity *entity,
 	return ret;
 }
 
-static struct v4l2_mbus_framefmt *
-__csi2_get_fmt(struct dw_mipi_csi2_dev *csi2, struct v4l2_subdev_state *sd_state,
-	       unsigned int pad, enum v4l2_subdev_format_whence which)
-{
-	if (which == V4L2_SUBDEV_FORMAT_TRY)
-		return v4l2_subdev_state_get_format(sd_state, pad);
-	else
-		return &csi2->format_mbus;
-}
-
-static int csi2_get_fmt(struct v4l2_subdev *sd,
-			struct v4l2_subdev_state *sd_state,
-			struct v4l2_subdev_format *sdformat)
-{
-	struct dw_mipi_csi2_dev *csi2 = sd_to_dev(sd);
-	struct v4l2_mbus_framefmt *fmt;
-
-	guard(mutex)(&csi2->lock);
-
-	fmt = __csi2_get_fmt(csi2, sd_state, sdformat->pad, sdformat->which);
-
-	sdformat->format = *fmt;
-
-	return 0;
-}
-
 static int csi2_set_fmt(struct v4l2_subdev *sd,
 			struct v4l2_subdev_state *sd_state,
 			struct v4l2_subdev_format *sdformat)
 {
 	struct dw_mipi_csi2_dev *csi2 = sd_to_dev(sd);
 	struct v4l2_mbus_framefmt *fmt;
+	int i;
 	u32 align;
 	u32 bpp;
 

@@ -756,7 +731,7 @@ static int csi2_set_fmt(struct v4l2_subdev *sd,
 	 * The CSIS can't transcode in any way, the source format can't be
 	 * modified.
 	 */
-	if (sdformat->pad < csi2->num_src_pads)
+	if (csi2->pad[sdformat->pad].flags == MEDIA_PAD_FL_SOURCE)
 		return v4l2_subdev_get_fmt(sd, sd_state, sdformat);
 
 	guard(mutex)(&csi2->lock);

@@ -771,13 +746,17 @@ static int csi2_set_fmt(struct v4l2_subdev *sd,
 	v4l_bound_align_image(&sdformat->format.width, 1, U16_MAX, align,
 			      &sdformat->format.height, 1, U16_MAX, 0, 0);
 
-	/* Output pads mirror active input pad, no limits on input pads */
-	if (!(csi2->config->sink_pad_mask & BIT(sdformat->pad)))
-		sdformat->format = csi2->format_mbus;
+	fmt = v4l2_subdev_state_get_format(sd_state, sdformat->pad);
+	*fmt = sdformat->format;
 
-	fmt = __csi2_get_fmt(csi2, sd_state, sdformat->pad, sdformat->which);
+	/* Output pads mirror active input pad, no limits on input pads */
+	for (i = 0; i < csi2->config->num_pads; i++)
+		if (csi2->pad[i].flags == MEDIA_PAD_FL_SOURCE) {
+			fmt = v4l2_subdev_state_get_format(sd_state, i);
+			*fmt = sdformat->format;
+		}
 
-	*fmt = sdformat->format;
+	csi2->format_mbus = *fmt;
 
 	return 0;
 }

@@ -818,7 +797,7 @@ static const struct media_entity_operations csi2_entity_ops = {
 };
 
 static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
-	.get_fmt = csi2_get_fmt,
+	.get_fmt = v4l2_subdev_get_fmt,
 	.set_fmt = csi2_set_fmt,
 	.enable_streams = dw_csi2_enable_streams,
 	.disable_streams = dw_csi2_disable_streams,

@@ -1071,6 +1050,10 @@ int dw_mipi_csi2_init(struct platform_device *pdev, struct dw_mipi_csi2_dev *csi
 	if (ret)
 		return ret;
 
+	ret = v4l2_subdev_init_finalize(&csi2->sd);
+	if (ret)
+		return ret;
+
 	ret = devm_add_action_or_reset(&pdev->dev, csi2_nf_cleanup, csi2);
 	if (ret)
 		return ret;