1 files changed, 67 insertions, 40 deletions

diff --git a/drivers/media/platform/vsp1/vsp1_sru.c b/drivers/media/platform/vsp1/vsp1_sru.c
index aa0e04c56f3f..b7d3c8b9f189 100644
--- a/drivers/media/platform/vsp1/vsp1_sru.c
+++ b/drivers/media/platform/vsp1/vsp1_sru.c
@@ -42,38 +42,6 @@ static inline void vsp1_sru_write(struct vsp1_sru *sru, u32 reg, u32 data)
 
 #define V4L2_CID_VSP1_SRU_INTENSITY		(V4L2_CID_USER_BASE + 1)
 
-static int sru_s_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct vsp1_sru *sru =
-		container_of(ctrl->handler, struct vsp1_sru, ctrls);
-
-	switch (ctrl->id) {
-	case V4L2_CID_VSP1_SRU_INTENSITY:
-		sru->intensity = ctrl->val;
-		break;
-	}
-
-	return 0;
-}
-
-static const struct v4l2_ctrl_ops sru_ctrl_ops = {
-	.s_ctrl = sru_s_ctrl,
-};
-
-static const struct v4l2_ctrl_config sru_intensity_control = {
-	.ops = &sru_ctrl_ops,
-	.id = V4L2_CID_VSP1_SRU_INTENSITY,
-	.name = "Intensity",
-	.type = V4L2_CTRL_TYPE_INTEGER,
-	.min = 1,
-	.max = 6,
-	.step = 1,
-};
-
-/* -----------------------------------------------------------------------------
- * V4L2 Subdevice Core Operations
- */
-
 struct vsp1_sru_param {
 	u32 ctrl0;
 	u32 ctrl2;
@@ -110,22 +78,66 @@ static const struct vsp1_sru_param vsp1_sru_params[] = {
 	},
 };
 
+static int sru_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vsp1_sru *sru =
+		container_of(ctrl->handler, struct vsp1_sru, ctrls);
+	const struct vsp1_sru_param *param;
+	u32 value;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VSP1_SRU_INTENSITY:
+		param = &vsp1_sru_params[ctrl->val - 1];
+
+		value = vsp1_sru_read(sru, VI6_SRU_CTRL0);
+		value &= ~(VI6_SRU_CTRL0_PARAM0_MASK |
+			   VI6_SRU_CTRL0_PARAM1_MASK);
+		value |= param->ctrl0;
+		vsp1_sru_write(sru, VI6_SRU_CTRL0, value);
+
+		vsp1_sru_write(sru, VI6_SRU_CTRL2, param->ctrl2);
+		break;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops sru_ctrl_ops = {
+	.s_ctrl = sru_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config sru_intensity_control = {
+	.ops = &sru_ctrl_ops,
+	.id = V4L2_CID_VSP1_SRU_INTENSITY,
+	.name = "Intensity",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 1,
+	.max = 6,
+	.def = 1,
+	.step = 1,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Core Operations
+ */
+
 static int sru_s_stream(struct v4l2_subdev *subdev, int enable)
 {
 	struct vsp1_sru *sru = to_sru(subdev);
-	const struct vsp1_sru_param *param;
 	struct v4l2_mbus_framefmt *input;
 	struct v4l2_mbus_framefmt *output;
-	bool upscale;
 	u32 ctrl0;
+	int ret;
+
+	ret = vsp1_entity_set_streaming(&sru->entity, enable);
+	if (ret < 0)
+		return ret;
 
 	if (!enable)
 		return 0;
 
 	input = &sru->entity.formats[SRU_PAD_SINK];
 	output = &sru->entity.formats[SRU_PAD_SOURCE];
-	upscale = input->width != output->width;
-	param = &vsp1_sru_params[sru->intensity];
 
 	if (input->code == V4L2_MBUS_FMT_ARGB8888_1X32)
 		ctrl0 = VI6_SRU_CTRL0_PARAM2 | VI6_SRU_CTRL0_PARAM3
@@ -133,10 +145,18 @@ static int sru_s_stream(struct v4l2_subdev *subdev, int enable)
 	else
 		ctrl0 = VI6_SRU_CTRL0_PARAM3;
 
-	vsp1_sru_write(sru, VI6_SRU_CTRL0, param->ctrl0 | ctrl0 |
-		       (upscale ? VI6_SRU_CTRL0_MODE_UPSCALE : 0));
+	if (input->width != output->width)
+		ctrl0 |= VI6_SRU_CTRL0_MODE_UPSCALE;
+
+	/* Take the control handler lock to ensure that the CTRL0 value won't be
+	 * changed behind our back by a set control operation.
+	 */
+	mutex_lock(sru->ctrls.lock);
+	ctrl0 |= vsp1_sru_read(sru, VI6_SRU_CTRL0)
+	       & (VI6_SRU_CTRL0_PARAM0_MASK | VI6_SRU_CTRL0_PARAM1_MASK);
+	mutex_unlock(sru->ctrls.lock);
+
 	vsp1_sru_write(sru, VI6_SRU_CTRL1, VI6_SRU_CTRL1_PARAM5);
-	vsp1_sru_write(sru, VI6_SRU_CTRL2, param->ctrl2);
 
 	return 0;
 }
@@ -348,8 +368,15 @@ struct vsp1_sru *vsp1_sru_create(struct vsp1_device *vsp1)
 	/* Initialize the control handler. */
 	v4l2_ctrl_handler_init(&sru->ctrls, 1);
 	v4l2_ctrl_new_custom(&sru->ctrls, &sru_intensity_control, NULL);
-	v4l2_ctrl_handler_setup(&sru->ctrls);
+
 	sru->entity.subdev.ctrl_handler = &sru->ctrls;
 
+	if (sru->ctrls.error) {
+		dev_err(vsp1->dev, "sru: failed to initialize controls\n");
+		ret = sru->ctrls.error;
+		vsp1_entity_destroy(&sru->entity);
+		return ERR_PTR(ret);
+	}
+
 	return sru;
 }