media: atomisp: Add atomisp_select_input() helper

When switching input/sensor the s_power() callback must be called
for old sensor drivers to power on the new sensor and power off
the previous sensor.

atomisp_s_input() already does this but atomisp_link_setup()
did not do this.

Add a new atomisp_select_input() helper which does this and use this
in both atomisp_s_input() and atomisp_link_setup() for consistent
behavior.

Also make atomisp_link_setup() turn the sensor back off when
a link gets disabled.

Reviewed-by: Andy Shevchenko <andy@kernel.org>
Signed-off-by: Hans de Goede <hdegoede@redhat.com>
Signed-off-by: Mauro Carvalho Chehab <mchehab@kernel.org>

4 files changed, 32 insertions, 18 deletions

diff --git a/drivers/staging/media/atomisp/pci/atomisp_cmd.c b/drivers/staging/media/atomisp/pci/atomisp_cmd.c
index 83a15a2d358e..6c93bab17955 100644
--- a/drivers/staging/media/atomisp/pci/atomisp_cmd.c
+++ b/drivers/staging/media/atomisp/pci/atomisp_cmd.c
@@ -3739,6 +3739,25 @@ int atomisp_s_sensor_power(struct atomisp_device *isp, unsigned int input, bool
 	return 0;
 }
 
+int atomisp_select_input(struct atomisp_device *isp, unsigned int input)
+{
+	unsigned int input_orig = isp->asd.input_curr;
+	int ret;
+
+	/* Power on new sensor */
+	ret = atomisp_s_sensor_power(isp, input, 1);
+	if (ret)
+		return ret;
+
+	isp->asd.input_curr = input;
+
+	/* Power off previous sensor */
+	if (input != input_orig)
+		atomisp_s_sensor_power(isp, input_orig, 0);
+
+	return 0;
+}
+
 static int atomisp_set_sensor_crop_and_fmt(struct atomisp_device *isp,
 					   struct v4l2_mbus_framefmt *ffmt,
 					   int which)
diff --git a/drivers/staging/media/atomisp/pci/atomisp_cmd.h b/drivers/staging/media/atomisp/pci/atomisp_cmd.h
index 2676236ee015..f302763b7b2f 100644
--- a/drivers/staging/media/atomisp/pci/atomisp_cmd.h
+++ b/drivers/staging/media/atomisp/pci/atomisp_cmd.h
@@ -244,6 +244,9 @@ void atomisp_get_padding(struct atomisp_device *isp, u32 width, u32 height,
 /* Set sensor power (no-op if already on/off) */
 int atomisp_s_sensor_power(struct atomisp_device *isp, unsigned int input, bool on);
 
+/* Select which sensor to use, must be called with a valid input */
+int atomisp_select_input(struct atomisp_device *isp, unsigned int input);
+
 /* This function looks up the closest available resolution. */
 int atomisp_try_fmt(struct atomisp_device *isp, struct v4l2_pix_format *f,
 		    const struct atomisp_format_bridge **fmt_ret,
diff --git a/drivers/staging/media/atomisp/pci/atomisp_ioctl.c b/drivers/staging/media/atomisp/pci/atomisp_ioctl.c
index be1f3f2ee63e..b3ad53449cb8 100644
--- a/drivers/staging/media/atomisp/pci/atomisp_ioctl.c
+++ b/drivers/staging/media/atomisp/pci/atomisp_ioctl.c
@@ -449,7 +449,6 @@ static int atomisp_s_input(struct file *file, void *fh, unsigned int input)
 	struct video_device *vdev = video_devdata(file);
 	struct atomisp_device *isp = video_get_drvdata(vdev);
 	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
-	struct atomisp_sub_device *asd = pipe->asd;
 	struct v4l2_subdev *camera = NULL;
 	int ret;
 
@@ -468,17 +467,7 @@ static int atomisp_s_input(struct file *file, void *fh, unsigned int input)
 		return -EINVAL;
 	}
 
-	/* power off the current owned sensor, as it is not used this time */
-	if (input != isp->asd.input_curr)
-		atomisp_s_sensor_power(isp, isp->asd.input_curr, 0);
-
-	/* powe on the new sensor */
-	ret = atomisp_s_sensor_power(isp, input, 1);
-	if (ret)
-		return ret;
-
-	asd->input_curr = input;
-	return 0;
+	return atomisp_select_input(isp, input);
 }
 
 /*
diff --git a/drivers/staging/media/atomisp/pci/atomisp_subdev.c b/drivers/staging/media/atomisp/pci/atomisp_subdev.c
index c36aae69d6f7..aabffd6a424d 100644
--- a/drivers/staging/media/atomisp/pci/atomisp_subdev.c
+++ b/drivers/staging/media/atomisp/pci/atomisp_subdev.c
@@ -663,9 +663,6 @@ static int atomisp_link_setup(struct media_entity *entity,
 		return -EINVAL;
 	}
 
-	/* Ignore disables, input_curr should only be updated on enables */
-	if (!(flags & MEDIA_LNK_FL_ENABLED))
-		return 0;
 
 	for (i = 0; i < isp->input_cnt; i++) {
 		if (isp->inputs[i].camera == isp->sensor_subdevs[csi_idx])
@@ -679,11 +676,17 @@ static int atomisp_link_setup(struct media_entity *entity,
 
 	mutex_lock(&isp->mutex);
 	ret = atomisp_pipe_check(&asd->video_out, true);
-	if (ret == 0)
-		asd->input_curr = i;
 	mutex_unlock(&isp->mutex);
+	if (ret)
+		return ret;
 
-	return ret;
+	/* Turn off the sensor on link disable */
+	if (!(flags & MEDIA_LNK_FL_ENABLED)) {
+		atomisp_s_sensor_power(isp, i, 0);
+		return 0;
+	}
+
+	return atomisp_select_input(isp, i);
 }
 
 static const struct media_entity_operations isp_subdev_media_ops = {