4 files changed, 62 insertions, 35 deletions

diff --git a/drivers/staging/greybus/control.c b/drivers/staging/greybus/control.c
index d79807c10b98..30779402b368 100644
--- a/drivers/staging/greybus/control.c
+++ b/drivers/staging/greybus/control.c
@@ -17,6 +17,43 @@
 #define GB_CONTROL_VERSION_MINOR	1
 
 
+int gb_control_get_version(struct gb_control *control)
+{
+	struct gb_interface *intf = control->connection->intf;
+	struct gb_control_version_request request;
+	struct gb_control_version_response response;
+	int ret;
+
+	request.major = GB_CONTROL_VERSION_MAJOR;
+	request.minor = GB_CONTROL_VERSION_MINOR;
+
+	ret = gb_operation_sync(control->connection,
+				GB_CONTROL_TYPE_VERSION,
+				&request, sizeof(request), &response,
+				sizeof(response));
+	if (ret) {
+		dev_err(&intf->dev,
+				"failed to get control-protocol version: %d\n",
+				ret);
+		return ret;
+	}
+
+	if (response.major > request.major) {
+		dev_err(&intf->dev,
+				"unsupported major control-protocol version (%u > %u)\n",
+				response.major, request.major);
+		return -ENOTSUPP;
+	}
+
+	control->protocol_major = response.major;
+	control->protocol_minor = response.minor;
+
+	dev_dbg(&intf->dev, "%s - %u.%u\n", __func__, response.major,
+			response.minor);
+
+	return 0;
+}
+
 /* Get Manifest's size from the interface */
 int gb_control_get_manifest_size_operation(struct gb_interface *intf)
 {
@@ -121,7 +158,7 @@ int gb_control_enable(struct gb_control *control)
 
 	dev_dbg(&control->connection->intf->dev, "%s\n", __func__);
 
-	ret = gb_connection_legacy_init(control->connection);
+	ret = gb_connection_enable_tx(control->connection);
 	if (ret) {
 		dev_err(&control->connection->intf->dev,
 				"failed to enable control connection: %d\n",
@@ -129,14 +166,23 @@ int gb_control_enable(struct gb_control *control)
 		return ret;
 	}
 
+	ret = gb_control_get_version(control);
+	if (ret)
+		goto err_disable_connection;
+
 	return 0;
+
+err_disable_connection:
+	gb_connection_disable(control->connection);
+
+	return ret;
 }
 
 void gb_control_disable(struct gb_control *control)
 {
 	dev_dbg(&control->connection->intf->dev, "%s\n", __func__);
 
-	gb_connection_legacy_exit(control->connection);
+	gb_connection_disable(control->connection);
 }
 
 void gb_control_destroy(struct gb_control *control)
@@ -144,25 +190,3 @@ void gb_control_destroy(struct gb_control *control)
 	gb_connection_destroy(control->connection);
 	kfree(control);
 }
-
-static int gb_control_connection_init(struct gb_connection *connection)
-{
-	dev_dbg(&connection->intf->dev, "%s\n", __func__);
-
-	return 0;
-}
-
-static void gb_control_connection_exit(struct gb_connection *connection)
-{
-	dev_dbg(&connection->intf->dev, "%s\n", __func__);
-}
-
-static struct gb_protocol control_protocol = {
-	.name			= "control",
-	.id			= GREYBUS_PROTOCOL_CONTROL,
-	.major			= GB_CONTROL_VERSION_MAJOR,
-	.minor			= GB_CONTROL_VERSION_MINOR,
-	.connection_init	= gb_control_connection_init,
-	.connection_exit	= gb_control_connection_exit,
-};
-gb_builtin_protocol_driver(control_protocol);
diff --git a/drivers/staging/greybus/control.h b/drivers/staging/greybus/control.h
index 7cb3dd2290d7..dd0a2d79da20 100644
--- a/drivers/staging/greybus/control.h
+++ b/drivers/staging/greybus/control.h
@@ -12,6 +12,9 @@
 
 struct gb_control {
 	struct gb_connection	*connection;
+
+	u8 protocol_major;
+	u8 protocol_minor;
 };
 
 struct gb_control *gb_control_create(struct gb_interface *intf);
@@ -26,6 +29,4 @@ int gb_control_get_manifest_operation(struct gb_interface *intf, void *manifest,
 				      size_t size);
 int gb_control_get_interface_version_operation(struct gb_interface *intf);
 
-int gb_control_protocol_init(void);
-void gb_control_protocol_exit(void);
 #endif /* __CONTROL_H */
diff --git a/drivers/staging/greybus/core.c b/drivers/staging/greybus/core.c
index 493f3920aaf0..6674b2728f98 100644
--- a/drivers/staging/greybus/core.c
+++ b/drivers/staging/greybus/core.c
@@ -234,12 +234,6 @@ static int __init gb_init(void)
 		goto error_operation;
 	}
 
-	retval = gb_control_protocol_init();
-	if (retval) {
-		pr_err("gb_control_protocol_init failed\n");
-		goto error_control;
-	}
-
 	retval = gb_svc_protocol_init();
 	if (retval) {
 		pr_err("gb_svc_protocol_init failed\n");
@@ -265,8 +259,6 @@ error_legacy:
 error_firmware:
 	gb_svc_protocol_exit();
 error_svc:
-	gb_control_protocol_exit();
-error_control:
 	gb_operation_exit();
 error_operation:
 	gb_hd_exit();
@@ -284,7 +276,6 @@ static void __exit gb_exit(void)
 	gb_legacy_exit();
 	gb_firmware_protocol_exit();
 	gb_svc_protocol_exit();
-	gb_control_protocol_exit();
 	gb_operation_exit();
 	gb_hd_exit();
 	bus_unregister(&greybus_bus_type);
diff --git a/drivers/staging/greybus/greybus_protocols.h b/drivers/staging/greybus/greybus_protocols.h
index c563e7454d9c..abbb214863c9 100644
--- a/drivers/staging/greybus/greybus_protocols.h
+++ b/drivers/staging/greybus/greybus_protocols.h
@@ -116,6 +116,7 @@ struct gb_protocol_version_response {
 /* Control Protocol */
 
 /* Greybus control request types */
+#define GB_CONTROL_TYPE_VERSION			0x01
 #define GB_CONTROL_TYPE_PROBE_AP		0x02
 #define GB_CONTROL_TYPE_GET_MANIFEST_SIZE	0x03
 #define GB_CONTROL_TYPE_GET_MANIFEST		0x04
@@ -123,6 +124,16 @@ struct gb_protocol_version_response {
 #define GB_CONTROL_TYPE_DISCONNECTED		0x06
 #define GB_CONTROL_TYPE_INTERFACE_VERSION	0x0a
 
+struct gb_control_version_request {
+	__u8	major;
+	__u8	minor;
+} __packed;
+
+struct gb_control_version_response {
+	__u8	major;
+	__u8	minor;
+} __packed;
+
 /* Control protocol manifest get size request has no payload*/
 struct gb_control_get_manifest_size_response {
 	__le16			size;