greybus: camera: Factorize link power mode configuration code into a function

Avoid duplicating the same code block multiple times.

Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
Tested-by: Jacopo Mondi <jacopo.mondi@linaro.org>
Signed-off-by: Greg Kroah-Hartman <gregkh@google.com>

1 files changed, 52 insertions, 64 deletions

diff --git a/drivers/staging/greybus/camera.c b/drivers/staging/greybus/camera.c
index 3f31a2fd7e5e..8383770910c5 100644
--- a/drivers/staging/greybus/camera.c
+++ b/drivers/staging/greybus/camera.c
@@ -107,6 +107,54 @@ static const struct gb_camera_fmt_map mbus_to_gbus_format[] = {
  * Camera Protocol Operations
  */
 
+static int gb_camera_set_intf_power_mode(struct gb_camera *gcam, u8 intf_id,
+					 bool hs)
+{
+	struct gb_svc *svc = gcam->connection->hd->svc;
+	int ret;
+
+	if (hs)
+		ret = gb_svc_intf_set_power_mode(svc, intf_id,
+						 GB_SVC_UNIPRO_HS_SERIES_A,
+						 GB_SVC_UNIPRO_FAST_MODE, 2, 2,
+						 GB_SVC_UNIPRO_FAST_MODE, 2, 2,
+						 GB_SVC_PWRM_RXTERMINATION |
+						 GB_SVC_PWRM_TXTERMINATION, 0);
+	else
+		ret = gb_svc_intf_set_power_mode(svc, intf_id,
+						 GB_SVC_UNIPRO_HS_SERIES_A,
+						 GB_SVC_UNIPRO_SLOW_AUTO_MODE,
+						 1, 2,
+						 GB_SVC_UNIPRO_SLOW_AUTO_MODE,
+						 1, 2,
+						 0, 0);
+
+	return ret;
+}
+
+static int gb_camera_set_power_mode(struct gb_camera *gcam, bool hs)
+{
+	struct gb_interface *intf = gcam->connection->intf;
+	struct gb_svc *svc = gcam->connection->hd->svc;
+	int ret;
+
+	ret = gb_camera_set_intf_power_mode(gcam, intf->interface_id, hs);
+	if (ret < 0) {
+		gcam_err(gcam, "failed to set module interface to %s (%d)\n",
+			 hs ? "HS" : "PWM", ret);
+		return ret;
+	}
+
+	ret = gb_camera_set_intf_power_mode(gcam, svc->ap_intf_id, hs);
+	if (ret < 0) {
+		gcam_err(gcam, "failed to set AP interface to %s (%d)\n",
+			 hs ? "HS" : "PWM", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
 struct ap_csi_config_request {
 	__u8 csi_id;
 	__u8 clock_mode;
@@ -120,8 +168,6 @@ static int gb_camera_configure_streams(struct gb_camera *gcam,
 				       unsigned int *flags,
 				       struct gb_camera_stream_config *streams)
 {
-	struct gb_interface *intf = gcam->connection->intf;
-	struct gb_svc *svc = gcam->connection->hd->svc;
 	struct gb_camera_configure_streams_request *req;
 	struct gb_camera_configure_streams_response *resp;
 	struct ap_csi_config_request csi_cfg;
@@ -151,49 +197,13 @@ static int gb_camera_configure_streams(struct gb_camera *gcam,
 	 * before CSI interfaces gets configured
 	 */
 	if (nstreams && !(*flags & GB_CAMERA_CONFIGURE_STREAMS_TEST_ONLY)) {
-		ret = gb_svc_intf_set_power_mode(svc, intf->interface_id,
-						 GB_SVC_UNIPRO_HS_SERIES_A,
-						 GB_SVC_UNIPRO_FAST_MODE, 2, 2,
-						 GB_SVC_UNIPRO_FAST_MODE, 2, 2,
-						 GB_SVC_PWRM_RXTERMINATION |
-						 GB_SVC_PWRM_TXTERMINATION, 0);
-		if (ret < 0)
-			goto done;
-
-		ret = gb_svc_intf_set_power_mode(svc, svc->ap_intf_id,
-						 GB_SVC_UNIPRO_HS_SERIES_A,
-						 GB_SVC_UNIPRO_FAST_MODE, 2, 2,
-						 GB_SVC_UNIPRO_FAST_MODE, 2, 2,
-						 GB_SVC_PWRM_RXTERMINATION |
-						 GB_SVC_PWRM_TXTERMINATION, 0);
+		ret = gb_camera_set_power_mode(gcam, true);
 		if (ret < 0)
 			goto done;
 	} else if (nstreams == 0) {
-		ret = gb_svc_intf_set_power_mode(svc, intf->interface_id,
-						 GB_SVC_UNIPRO_HS_SERIES_A,
-						 GB_SVC_UNIPRO_SLOW_AUTO_MODE,
-						 1, 2,
-						 GB_SVC_UNIPRO_SLOW_AUTO_MODE,
-						 1, 2,
-						 0, 0);
-		if (ret < 0) {
-			gcam_err(gcam, "can't take camera link to PWM-G1 auto: %d\n",
-				 ret);
-			goto done;
-		}
-
-		ret = gb_svc_intf_set_power_mode(svc, svc->ap_intf_id,
-						 GB_SVC_UNIPRO_HS_SERIES_A,
-						 GB_SVC_UNIPRO_SLOW_AUTO_MODE,
-						 1, 2,
-						 GB_SVC_UNIPRO_SLOW_AUTO_MODE,
-						 1, 2,
-						 0, 0);
-		if (ret < 0) {
-			gcam_err(gcam, "can't take AP link to PWM-G1 auto: %d\n",
-				 ret);
+		ret = gb_camera_set_power_mode(gcam, false);
+		if (ret < 0)
 			goto done;
-		}
 	}
 
 	req->num_streams = nstreams;
@@ -283,29 +293,7 @@ static int gb_camera_configure_streams(struct gb_camera *gcam,
 	return ret;
 
 set_unipro_slow_mode:
-	ret = gb_svc_intf_set_power_mode(svc, intf->interface_id,
-					 GB_SVC_UNIPRO_HS_SERIES_A,
-					 GB_SVC_UNIPRO_SLOW_AUTO_MODE,
-					 1, 2,
-					 GB_SVC_UNIPRO_SLOW_AUTO_MODE,
-					 1, 2,
-					 0, 0);
-	if (ret < 0) {
-		gcam_err(gcam, "can't take camera link to PWM-G1 auto: %d\n",
-			 ret);
-		goto done;
-	}
-
-	ret = gb_svc_intf_set_power_mode(svc, svc->ap_intf_id,
-				   GB_SVC_UNIPRO_HS_SERIES_A,
-				   GB_SVC_UNIPRO_SLOW_AUTO_MODE,
-				   1, 2,
-				   GB_SVC_UNIPRO_SLOW_AUTO_MODE,
-				   1, 2,
-				   0, 0);
-	if (ret < 0)
-		gcam_err(gcam, "can't take AP link to PWM-G1 auto: %d\n",
-			 ret);
+	ret = gb_camera_set_power_mode(gcam, false);
 
 done:
 	kfree(req);