v4l: vsp1: sru: Make the intensity controllable during streaming

[platform/adaptation/renesas_rcar/renesas_kernel.git] / drivers / media / platform / vsp1 / vsp1_sru.c

diff --git a/drivers/media/platform/vsp1/vsp1_sru.c b/drivers/media/platform/vsp1/vsp1_sru.c
index 18e127a..b7d3c8b 100644 (file)
--- a/drivers/media/platform/vsp1/vsp1_sru.c
+++ b/drivers/media/platform/vsp1/vsp1_sru.c
@@ -42,39 +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,
-       .def = 1,
-       .step = 1,
-};
-
-/* -----------------------------------------------------------------------------
- * V4L2 Subdevice Core Operations
- */
-
 struct vsp1_sru_param {
        u32 ctrl0;
        u32 ctrl2;
@@ -111,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
@@ -134,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;
 }
@@ -359,7 +378,5 @@ struct vsp1_sru *vsp1_sru_create(struct vsp1_device *vsp1)
                return ERR_PTR(ret);
        }
 
-       v4l2_ctrl_handler_setup(&sru->ctrls);
-
        return sru;
 }