3 files changed, 66 insertions, 24 deletions

diff --git a/drivers/staging/greybus/control.c b/drivers/staging/greybus/control.c
index 2cc1917adda0..9c282e40142b 100644
--- a/drivers/staging/greybus/control.c
+++ b/drivers/staging/greybus/control.c
@@ -59,31 +59,70 @@ int gb_control_disconnected_operation(struct gb_control *control, u16 cport_id)
 				 sizeof(request), NULL, 0);
 }
 
-static int gb_control_connection_init(struct gb_connection *connection)
+struct gb_control *gb_control_create(struct gb_interface *intf)
 {
 	struct gb_control *control;
 
 	control = kzalloc(sizeof(*control), GFP_KERNEL);
 	if (!control)
-		return -ENOMEM;
+		return NULL;
+
+	control->connection = gb_connection_create_dynamic(intf, NULL,
+						GB_CONTROL_CPORT_ID,
+						GREYBUS_PROTOCOL_CONTROL);
+	if (!control->connection) {
+		dev_err(&intf->dev, "failed to create control connection\n");
+		kfree(control);
+		return NULL;
+	}
 
-	control->connection = connection;
-	connection->private = control;
+	control->connection->private = control;
+
+	return control;
+}
+
+int gb_control_enable(struct gb_control *control)
+{
+	int ret;
 
-	/* Set interface's control connection */
-	connection->intf->control = control;
+	dev_dbg(&control->connection->intf->dev, "%s\n", __func__);
+
+	ret = gb_connection_init(control->connection);
+	if (ret) {
+		dev_err(&control->connection->intf->dev,
+				"failed to enable control connection: %d\n",
+				ret);
+		return ret;
+	}
 
 	return 0;
 }
 
-static void gb_control_connection_exit(struct gb_connection *connection)
+void gb_control_disable(struct gb_control *control)
 {
-	struct gb_control *control = connection->private;
+	dev_dbg(&control->connection->intf->dev, "%s\n", __func__);
 
-	connection->intf->control = NULL;
+	gb_connection_exit(control->connection);
+}
+
+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,
diff --git a/drivers/staging/greybus/control.h b/drivers/staging/greybus/control.h
index 3248d965e593..da0fa6652dca 100644
--- a/drivers/staging/greybus/control.h
+++ b/drivers/staging/greybus/control.h
@@ -14,6 +14,11 @@ struct gb_control {
 	struct gb_connection	*connection;
 };
 
+struct gb_control *gb_control_create(struct gb_interface *intf);
+int gb_control_enable(struct gb_control *control);
+void gb_control_disable(struct gb_control *control);
+void gb_control_destroy(struct gb_control *control);
+
 int gb_control_connected_operation(struct gb_control *control, u16 cport_id);
 int gb_control_disconnected_operation(struct gb_control *control, u16 cport_id);
 int gb_control_get_manifest_size_operation(struct gb_interface *intf);
diff --git a/drivers/staging/greybus/interface.c b/drivers/staging/greybus/interface.c
index 85fcae76928c..aa30dc2a9551 100644
--- a/drivers/staging/greybus/interface.c
+++ b/drivers/staging/greybus/interface.c
@@ -61,6 +61,9 @@ static void gb_interface_release(struct device *dev)
 	kfree(intf->product_string);
 	kfree(intf->vendor_string);
 
+	if (intf->control)
+		gb_control_destroy(intf->control);
+
 	kfree(intf);
 }
 
@@ -106,6 +109,12 @@ struct gb_interface *gb_interface_create(struct gb_host_device *hd,
 	device_initialize(&intf->dev);
 	dev_set_name(&intf->dev, "%d-%d", hd->bus_id, interface_id);
 
+	intf->control = gb_control_create(intf);
+	if (!intf->control) {
+		put_device(&intf->dev);
+		return NULL;
+	}
+
 	spin_lock_irq(&gb_interfaces_lock);
 	list_add(&intf->links, &hd->interfaces);
 	spin_unlock_irq(&gb_interfaces_lock);
@@ -127,8 +136,7 @@ void gb_interface_remove(struct gb_interface *intf)
 	if (device_is_registered(&intf->dev))
 		device_del(&intf->dev);
 
-	if (intf->control)
-		gb_connection_destroy(intf->control->connection);
+	gb_control_disable(intf->control);
 
 	spin_lock_irq(&gb_interfaces_lock);
 	list_del(&intf->links);
@@ -161,20 +169,10 @@ int gb_interface_init(struct gb_interface *intf, u8 device_id)
 
 	intf->device_id = device_id;
 
-	/* Establish control CPort connection */
-	connection = gb_connection_create_dynamic(intf, NULL,
-						GB_CONTROL_CPORT_ID,
-						GREYBUS_PROTOCOL_CONTROL);
-	if (!connection) {
-		dev_err(&intf->dev, "failed to create control connection\n");
-		return -ENOMEM;
-	}
-
-	ret = gb_connection_init(connection);
-	if (ret) {
-		gb_connection_destroy(connection);
+	/* Establish control connection */
+	ret = gb_control_enable(intf->control);
+	if (ret)
 		return ret;
-	}
 
 	/* Get manifest size using control protocol on CPort */
 	size = gb_control_get_manifest_size_operation(intf);