greybus: svc: add power mode call for link hibernation

Due to when using set_power_mode to hibernate a link, it won't trigger a
POWERMODEIND event, hence the hard-coded GB_SVC_SETPWRM_PWR_OK would be
returned and it should also be considered as successful result code for
link hibernation. Therefore, adding this set_power_mode_hibernate
function to separate the two calls in order to check with the correct
result code.

Testing Done:
 - Suspend an Interface and observe no set power mode error.

Signed-off-by: David Lin <dtwlin@google.com>
Reviewed-by: Johan Hovold <johan@hovoldconsulting.com>
Signed-off-by: Alex Elder <elder@linaro.org>

2 files changed, 36 insertions, 0 deletions

diff --git a/drivers/staging/greybus/svc.c b/drivers/staging/greybus/svc.c
index a46d7fb0139b..14bada965ddd 100644
--- a/drivers/staging/greybus/svc.c
+++ b/drivers/staging/greybus/svc.c
@@ -687,6 +687,41 @@ int gb_svc_intf_set_power_mode(struct gb_svc *svc, u8 intf_id, u8 hs_series,
 }
 EXPORT_SYMBOL_GPL(gb_svc_intf_set_power_mode);
 
+int gb_svc_intf_set_power_mode_hibernate(struct gb_svc *svc, u8 intf_id)
+{
+	struct gb_svc_intf_set_pwrm_request request;
+	struct gb_svc_intf_set_pwrm_response response;
+	int ret;
+	u16 result_code;
+
+	memset(&request, 0, sizeof(request));
+
+	request.intf_id = intf_id;
+	request.hs_series = GB_SVC_UNIPRO_HS_SERIES_A;
+	request.tx_mode = GB_SVC_UNIPRO_HIBERNATE_MODE;
+	request.rx_mode = GB_SVC_UNIPRO_HIBERNATE_MODE;
+
+	ret = gb_operation_sync(svc->connection, GB_SVC_TYPE_INTF_SET_PWRM,
+				&request, sizeof(request),
+				&response, sizeof(response));
+	if (ret < 0) {
+		dev_err(&svc->dev,
+			"failed to send set power mode operation to interface %u: %d\n",
+			intf_id, ret);
+		return ret;
+	}
+
+	result_code = response.result_code;
+	if (result_code != GB_SVC_SETPWRM_PWR_OK) {
+		dev_err(&svc->dev,
+			"failed to hibernate the link for interface %u: %u\n",
+			intf_id, result_code);
+		return -EIO;
+	}
+
+	return 0;
+}
+
 int gb_svc_ping(struct gb_svc *svc)
 {
 	return gb_operation_sync_timeout(svc->connection, GB_SVC_TYPE_PING,
diff --git a/drivers/staging/greybus/svc.h b/drivers/staging/greybus/svc.h
index 750eaff7d0cd..4ab066b90a5b 100644
--- a/drivers/staging/greybus/svc.h
+++ b/drivers/staging/greybus/svc.h
@@ -84,6 +84,7 @@ int gb_svc_intf_set_power_mode(struct gb_svc *svc, u8 intf_id, u8 hs_series,
 			       u8 flags, u32 quirks,
 			       struct gb_svc_l2_timer_cfg *local,
 			       struct gb_svc_l2_timer_cfg *remote);
+int gb_svc_intf_set_power_mode_hibernate(struct gb_svc *svc, u8 intf_id);
 int gb_svc_ping(struct gb_svc *svc);
 int gb_svc_watchdog_create(struct gb_svc *svc);
 void gb_svc_watchdog_destroy(struct gb_svc *svc);