1 files changed, 279 insertions, 0 deletions
diff --git a/drivers/usb/typec/port-mapper.c b/drivers/usb/typec/port-mapper.c
new file mode 100644
index 000000000000..9b0991bdf391
--- /dev/null
+++ b/drivers/usb/typec/port-mapper.c
@@ -0,0 +1,279 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * USB Type-C Connector Class Port Mapping Utility
+ *
+ * Copyright (C) 2021, Intel Corporation
+ * Author: Heikki Krogerus <heikki.krogerus@linux.intel.com>
+ */
+
+#include <linux/acpi.h>
+#include <linux/usb.h>
+#include <linux/usb/typec.h>
+
+#include "class.h"
+
+struct port_node {
+	struct list_head list;
+	struct device *dev;
+	void *pld;
+};
+
+static int acpi_pld_match(const struct acpi_pld_info *pld1,
+			  const struct acpi_pld_info *pld2)
+{
+	if (!pld1 || !pld2)
+		return 0;
+
+	/*
+	 * To speed things up, first checking only the group_position. It seems
+	 * to often have the first unique value in the _PLD.
+	 */
+	if (pld1->group_position == pld2->group_position)
+		return !memcmp(pld1, pld2, sizeof(struct acpi_pld_info));
+
+	return 0;
+}
+
+static void *get_pld(struct device *dev)
+{
+#ifdef CONFIG_ACPI
+	struct acpi_pld_info *pld;
+	acpi_status status;
+
+	if (!has_acpi_companion(dev))
+		return NULL;
+
+	status = acpi_get_physical_device_location(ACPI_HANDLE(dev), &pld);
+	if (ACPI_FAILURE(status))
+		return NULL;
+
+	return pld;
+#else
+	return NULL;
+#endif
+}
+
+static void free_pld(void *pld)
+{
+#ifdef CONFIG_ACPI
+	ACPI_FREE(pld);
+#endif
+}
+
+static int __link_port(struct typec_port *con, struct port_node *node)
+{
+	int ret;
+
+	ret = sysfs_create_link(&node->dev->kobj, &con->dev.kobj, "connector");
+	if (ret)
+		return ret;
+
+	ret = sysfs_create_link(&con->dev.kobj, &node->dev->kobj,
+				dev_name(node->dev));
+	if (ret) {
+		sysfs_remove_link(&node->dev->kobj, "connector");
+		return ret;
+	}
+
+	list_add_tail(&node->list, &con->port_list);
+
+	return 0;
+}
+
+static int link_port(struct typec_port *con, struct port_node *node)
+{
+	int ret;
+
+	mutex_lock(&con->port_list_lock);
+	ret = __link_port(con, node);
+	mutex_unlock(&con->port_list_lock);
+
+	return ret;
+}
+
+static void __unlink_port(struct typec_port *con, struct port_node *node)
+{
+	sysfs_remove_link(&con->dev.kobj, dev_name(node->dev));
+	sysfs_remove_link(&node->dev->kobj, "connector");
+	list_del(&node->list);
+}
+
+static void unlink_port(struct typec_port *con, struct port_node *node)
+{
+	mutex_lock(&con->port_list_lock);
+	__unlink_port(con, node);
+	mutex_unlock(&con->port_list_lock);
+}
+
+static struct port_node *create_port_node(struct device *port)
+{
+	struct port_node *node;
+
+	node = kzalloc(sizeof(*node), GFP_KERNEL);
+	if (!node)
+		return ERR_PTR(-ENOMEM);
+
+	node->dev = get_device(port);
+	node->pld = get_pld(port);
+
+	return node;
+}
+
+static void remove_port_node(struct port_node *node)
+{
+	put_device(node->dev);
+	free_pld(node->pld);
+	kfree(node);
+}
+
+static int connector_match(struct device *dev, const void *data)
+{
+	const struct port_node *node = data;
+
+	if (!is_typec_port(dev))
+		return 0;
+
+	return acpi_pld_match(to_typec_port(dev)->pld, node->pld);
+}
+
+static struct device *find_connector(struct port_node *node)
+{
+	if (!node->pld)
+		return NULL;
+
+	return class_find_device(&typec_class, NULL, node, connector_match);
+}
+
+/**
+ * typec_link_port - Link a port to its connector
+ * @port: The port device
+ *
+ * Find the connector of @port and create symlink named "connector" for it.
+ * Returns 0 on success, or errno in case of a failure.
+ *
+ * NOTE. The function increments the reference count of @port on success.
+ */
+int typec_link_port(struct device *port)
+{
+	struct device *connector;
+	struct port_node *node;
+	int ret;
+
+	node = create_port_node(port);
+	if (IS_ERR(node))
+		return PTR_ERR(node);
+
+	connector = find_connector(node);
+	if (!connector) {
+		ret = 0;
+		goto remove_node;
+	}
+
+	ret = link_port(to_typec_port(connector), node);
+	if (ret)
+		goto put_connector;
+
+	return 0;
+
+put_connector:
+	put_device(connector);
+remove_node:
+	remove_port_node(node);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(typec_link_port);
+
+static int port_match_and_unlink(struct device *connector, void *port)
+{
+	struct port_node *node;
+	struct port_node *tmp;
+	int ret = 0;
+
+	if (!is_typec_port(connector))
+		return 0;
+
+	mutex_lock(&to_typec_port(connector)->port_list_lock);
+	list_for_each_entry_safe(node, tmp, &to_typec_port(connector)->port_list, list) {
+		ret = node->dev == port;
+		if (ret) {
+			unlink_port(to_typec_port(connector), node);
+			remove_port_node(node);
+			put_device(connector);
+			break;
+		}
+	}
+	mutex_unlock(&to_typec_port(connector)->port_list_lock);
+
+	return ret;
+}
+
+/**
+ * typec_unlink_port - Unlink port from its connector
+ * @port: The port device
+ *
+ * Removes the symlink "connector" and decrements the reference count of @port.
+ */
+void typec_unlink_port(struct device *port)
+{
+	class_for_each_device(&typec_class, NULL, port, port_match_and_unlink);
+}
+EXPORT_SYMBOL_GPL(typec_unlink_port);
+
+static int each_port(struct device *port, void *connector)
+{
+	struct port_node *node;
+	int ret;
+
+	node = create_port_node(port);
+	if (IS_ERR(node))
+		return PTR_ERR(node);
+
+	if (!connector_match(connector, node)) {
+		remove_port_node(node);
+		return 0;
+	}
+
+	ret = link_port(to_typec_port(connector), node);
+	if (ret) {
+		remove_port_node(node->pld);
+		return ret;
+	}
+
+	get_device(connector);
+
+	return 0;
+}
+
+int typec_link_ports(struct typec_port *con)
+{
+	int ret = 0;
+
+	con->pld = get_pld(&con->dev);
+	if (!con->pld)
+		return 0;
+
+	ret = usb_for_each_port(&con->dev, each_port);
+	if (ret)
+		typec_unlink_ports(con);
+
+	return ret;
+}
+
+void typec_unlink_ports(struct typec_port *con)
+{
+	struct port_node *node;
+	struct port_node *tmp;
+
+	mutex_lock(&con->port_list_lock);
+
+	list_for_each_entry_safe(node, tmp, &con->port_list, list) {
+		__unlink_port(con, node);
+		remove_port_node(node);
+		put_device(&con->dev);
+	}
+
+	mutex_unlock(&con->port_list_lock);
+
+	free_pld(con->pld);
+}