greybus: add bg_hd_connection_find()

Add a function that looks up a connection given the host device
pointer an the host cport id.  This will be used to determine which
connection an incoming message is associated with.

Replace the list tracking host device connections with a red-black
tree so lookup can scale and be done quickly.

Signed-off-by: Alex Elder <elder@linaro.org>
Signed-off-by: Greg Kroah-Hartman <greg@kroah.com>

1 files changed, 52 insertions, 2 deletions

diff --git a/drivers/staging/greybus/connection.c b/drivers/staging/greybus/connection.c
index 53086126f961..449ae34bf531 100644
--- a/drivers/staging/greybus/connection.c
+++ b/drivers/staging/greybus/connection.c
@@ -13,6 +13,56 @@
 
 static DEFINE_SPINLOCK(gb_connections_lock);
 
+static void _gb_hd_connection_insert(struct greybus_host_device *hd,
+					struct gb_connection *connection)
+{
+	struct rb_root *root = &hd->connections;
+	struct rb_node *node = &connection->hd_node;
+	struct rb_node **link = &root->rb_node;
+	struct rb_node *above = NULL;
+	u16 cport_id = connection->hd_cport_id;
+
+	while (*link) {
+		struct gb_connection *connection;
+
+		above = *link;
+		connection = rb_entry(above, struct gb_connection, hd_node);
+		if (connection->hd_cport_id > cport_id)
+			link = &above->rb_left;
+		else if (connection->hd_cport_id < cport_id)
+			link = &above->rb_right;
+	}
+	rb_link_node(node, above, link);
+	rb_insert_color(node, root);
+}
+
+static void _gb_hd_connection_remove(struct gb_connection *connection)
+{
+	rb_erase(&connection->hd_node, &connection->hd->connections);
+}
+
+struct gb_connection *gb_hd_connection_find(struct greybus_host_device *hd,
+						u16 cport_id)
+{
+	struct gb_connection *connection = NULL;
+	struct rb_node *node;
+
+	spin_lock_irq(&gb_connections_lock);
+	node = hd->connections.rb_node;
+	while (node) {
+		connection = rb_entry(node, struct gb_connection, hd_node);
+		if (connection->hd_cport_id > cport_id)
+			node = node->rb_left;
+		else if (connection->hd_cport_id < cport_id)
+			node = node->rb_right;
+		else
+			break;
+	}
+	spin_unlock_irq(&gb_connections_lock);
+
+	return connection;
+}
+
 /*
  * Allocate an available CPort Id for use for the host side of the
  * given connection.  The lowest-available id is returned, so the
@@ -80,7 +130,7 @@ struct gb_connection *gb_connection_create(struct gb_interface *interface,
 	connection->protocol = protocol;
 
 	spin_lock_irq(&gb_connections_lock);
-	list_add_tail(&connection->hd_links, &hd->connections);
+	_gb_hd_connection_insert(hd, connection);
 	list_add_tail(&connection->interface_links, &interface->connections);
 	spin_unlock_irq(&gb_connections_lock);
 
@@ -102,8 +152,8 @@ void gb_connection_destroy(struct gb_connection *connection)
 	WARN_ON(!list_empty(&connection->operations));
 
 	spin_lock_irq(&gb_connections_lock);
-	list_del(&connection->hd_links);
 	list_del(&connection->interface_links);
+	_gb_hd_connection_remove(connection);
 	spin_unlock_irq(&gb_connections_lock);
 
 	gb_connection_hd_cport_id_free(connection);