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>
diff --git a/drivers/staging/greybus/camera.c b/drivers/staging/greybus/camera.c
index 3f31a2f..8383770 100644
--- a/drivers/staging/greybus/camera.c
+++ b/drivers/staging/greybus/camera.c
@@ -107,6 +107,54 @@
* 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 @@
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 @@
* 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);
+ ret = gb_camera_set_power_mode(gcam, false);
+ 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_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);
- goto done;
- }
}
req->num_streams = nstreams;
@@ -283,29 +293,7 @@
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);