greybus: svc: destroy the route on module hot-unplug

We created two-way routes between the AP and module's interface on
hotplug, and forgot to remove them on hot-unplug. The same is also
required while handling errors in hotplug case.
Reviewed-by: Johan Hovold <johan@hovoldconsulting.com>
Signed-off-by: Viresh Kumar <viresh.kumar@linaro.org>
Signed-off-by: Greg Kroah-Hartman <gregkh@google.com>

drivers/staging/greybus/greybus_protocols.h

@@ -743,6 +743,7 @@ struct gb_spi_transfer_response {
 #define GB_SVC_TYPE_CONN_CREATE		0x07
 #define GB_SVC_TYPE_CONN_DESTROY	0x08
 #define GB_SVC_TYPE_ROUTE_CREATE	0x0b
+#define GB_SVC_TYPE_ROUTE_DESTROY	0x0c
 
 /* SVC version request/response have same payload as gb_protocol_version_response */
 
@@ -806,6 +807,12 @@ struct gb_svc_route_create_request {
 } __packed;
 /* route create response has no payload */
 
+struct gb_svc_route_destroy_request {
+	__u8	intf1_id;
+	__u8	intf2_id;
+} __packed;
+/* route destroy response has no payload */
+
 
 /* RAW */
 

drivers/staging/greybus/svc.c

@@ -163,6 +163,24 @@ static int gb_svc_route_create(struct gb_svc *svc, u8 intf1_id, u8 dev1_id,
 				 &request, sizeof(request), NULL, 0);
 }
 
+/* Destroys bi-directional routes between the devices */
+static void gb_svc_route_destroy(struct gb_svc *svc, u8 intf1_id, u8 intf2_id)
+{
+	struct gb_svc_route_destroy_request request;
+	int ret;
+
+	request.intf1_id = intf1_id;
+	request.intf2_id = intf2_id;
+
+	ret = gb_operation_sync(svc->connection, GB_SVC_TYPE_ROUTE_DESTROY,
+				&request, sizeof(request), NULL, 0);
+	if (ret) {
+		dev_err(&svc->connection->dev,
+			"failed to destroy route (%hhx %hhx) %d\n",
+			intf1_id, intf2_id, ret);
+	}
+}
+
 static int gb_svc_version_request(struct gb_operation *op)
 {
 	struct gb_connection *connection = op->connection;
@@ -303,18 +321,20 @@ static void svc_process_hotplug(struct work_struct *work)
 	if (ret) {
 		dev_err(dev, "%s: Route create operation failed, interface %hhu device_id %hhu (%d)\n",
 			__func__, intf_id, device_id, ret);
-		goto ida_put;
+		goto svc_id_free;
 	}
 
 	ret = gb_interface_init(intf, device_id);
 	if (ret) {
 		dev_err(dev, "%s: Failed to initialize interface, interface %hhu device_id %hhu (%d)\n",
 			__func__, intf_id, device_id, ret);
-		goto svc_id_free;
+		goto destroy_route;
 	}
 
 	goto free_svc_hotplug;
 
+destroy_route:
+	gb_svc_route_destroy(svc, hd->endo->ap_intf_id, intf_id);
 svc_id_free:
 	/*
 	 * XXX Should we tell SVC that this id doesn't belong to interface
@@ -369,6 +389,7 @@ static int gb_svc_intf_hot_unplug_recv(struct gb_operation *op)
 	struct gb_svc_intf_hot_unplug_request *hot_unplug = request->payload;
 	struct greybus_host_device *hd = op->connection->hd;
 	struct device *dev = &op->connection->dev;
+	struct gb_svc *svc = op->connection->private;
 	u8 device_id;
 	struct gb_interface *intf;
 	u8 intf_id;
@@ -391,6 +412,12 @@ static int gb_svc_intf_hot_unplug_recv(struct gb_operation *op)
 
 	device_id = intf->device_id;
 	gb_interface_remove(hd, intf_id);
+
+	/*
+	 * Destroy the two-way route between the AP and the interface.
+	 */
+	gb_svc_route_destroy(svc, hd->endo->ap_intf_id, intf_id);
+
 	ida_simple_remove(&greybus_svc_device_id_map, device_id);
 
 	return 0;