[media] v4l: vsp1: sru: Make the intensity controllable during streaming

The control value is currently stored in the SRU structure by the
control set handler and written to the hardware at stream on time,
making control set during streaming ineffective. Fix it by writing to
the registers from within the control set handler.

Signed-off-by: Laurent Pinchart <[e-mail]>
Signed-off-by: Mauro Carvalho Chehab <[e-mail]>

drivers/media/platform/vsp1/vsp1_regs.h

···
  */
 
 #define VI6_SRU_CTRL0			0x2200
+#define VI6_SRU_CTRL0_PARAM0_MASK	(0x1ff << 16)
 #define VI6_SRU_CTRL0_PARAM0_SHIFT	16
+#define VI6_SRU_CTRL0_PARAM1_MASK	(0x1f << 8)
 #define VI6_SRU_CTRL0_PARAM1_SHIFT	8
 #define VI6_SRU_CTRL0_MODE_UPSCALE	(4 << 4)
 #define VI6_SRU_CTRL0_PARAM2		(1 << 3)

drivers/media/platform/vsp1/vsp1_sru.c

···
 
 #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,
-	.def = 1,
-	.step = 1,
-};
-
-/* -----------------------------------------------------------------------------
- * V4L2 Subdevice Core Operations
- */
-
 struct vsp1_sru_param {
 	u32 ctrl0;
 	u32 ctrl2;
···
 	},
 };
 
+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
···
 	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;
 }
···
 		vsp1_entity_destroy(&sru->entity);
 		return ERR_PTR(ret);
 	}
-
-	v4l2_ctrl_handler_setup(&sru->ctrls);
 
 	return sru;
 }

drivers/media/platform/vsp1/vsp1_sru.h

···
 	struct vsp1_entity entity;
 
 	struct v4l2_ctrl_handler ctrls;
-	unsigned int intensity;
 };
 
 static inline struct vsp1_sru *to_sru(struct v4l2_subdev *subdev)