[PATCH v1 4/5] usb: typec: Implement alternate mode priority handling

[Andrei Kuchynski <akuchynski@chromium.org>]

This patch introduces APIs to manage the priority of USB Type-C alternate
modes. These APIs allow for setting and retrieving a priority number for
each mode. If a new priority value conflicts with an existing mode's
priority, the priorities of the conflicting mode and all subsequent modes
are automatically incremented to ensure uniqueness.

Signed-off-by: Andrei Kuchynski <akuchynski@chromium.org>
---
 drivers/usb/typec/Makefile         |   2 +-
 drivers/usb/typec/class.h          |   1 +
 drivers/usb/typec/mode_selection.c | 127 +++++++++++++++++++++++++++++
 drivers/usb/typec/mode_selection.h |   8 ++
 include/linux/usb/typec_altmode.h  |   9 ++
 5 files changed, 146 insertions(+), 1 deletion(-)
 create mode 100644 drivers/usb/typec/mode_selection.c
 create mode 100644 drivers/usb/typec/mode_selection.h

diff --git a/drivers/usb/typec/Makefile b/drivers/usb/typec/Makefile
index 7a368fea61bc..8a6a1c663eb6 100644
--- a/drivers/usb/typec/Makefile
+++ b/drivers/usb/typec/Makefile
@@ -1,6 +1,6 @@
 # SPDX-License-Identifier: GPL-2.0
 obj-$(CONFIG_TYPEC)		+= typec.o
-typec-y				:= class.o mux.o bus.o pd.o retimer.o
+typec-y				:= class.o mux.o bus.o pd.o retimer.o mode_selection.o
 typec-$(CONFIG_ACPI)		+= port-mapper.o
 obj-$(CONFIG_TYPEC)		+= altmodes/
 obj-$(CONFIG_TYPEC_TCPM)	+= tcpm/
diff --git a/drivers/usb/typec/class.h b/drivers/usb/typec/class.h
index f05d9201c233..c6467e576569 100644
--- a/drivers/usb/typec/class.h
+++ b/drivers/usb/typec/class.h
@@ -82,6 +82,7 @@ struct typec_port {
 	struct device			*usb3_dev;
 
 	bool				alt_mode_override;
+	struct list_head		mode_list;
 };
 
 #define to_typec_port(_dev_) container_of(_dev_, struct typec_port, dev)
diff --git a/drivers/usb/typec/mode_selection.c b/drivers/usb/typec/mode_selection.c
new file mode 100644
index 000000000000..8a54639b86bf
--- /dev/null
+++ b/drivers/usb/typec/mode_selection.c
@@ -0,0 +1,127 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright 2025 Google LLC.
+ */
+
+#include <linux/usb/typec_altmode.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include "mode_selection.h"
+#include "class.h"
+
+static const char * const mode_names[TYPEC_ALTMODE_MAX] = {
+	[TYPEC_ALTMODE_DP] = "DisplayPort",
+	[TYPEC_ALTMODE_TBT] = "Thunderbolt3",
+	[TYPEC_ALTMODE_USB4] = "USB4",
+};
+
+static const int default_priorities[TYPEC_ALTMODE_MAX] = {
+	[TYPEC_ALTMODE_DP] = 2,
+	[TYPEC_ALTMODE_TBT] = 1,
+	[TYPEC_ALTMODE_USB4] = 0,
+};
+
+static inline enum typec_mode_type typec_svid_to_altmode(const u16 svid)
+{
+	switch (svid) {
+	case USB_TYPEC_DP_SID:
+		return TYPEC_ALTMODE_DP;
+	case USB_TYPEC_TBT_SID:
+		return TYPEC_ALTMODE_TBT;
+	case USB_TYPEC_USB4_SID:
+		return TYPEC_ALTMODE_USB4;
+	}
+	return TYPEC_ALTMODE_MAX;
+}
+
+/**
+ * struct mode_selection_state - State tracking for a specific Type-C mode
+ * @mode: The type of mode this instance represents
+ * @priority: The mode priority. Lower values indicate a more preferred mode.
+ * @list: List head to link this mode state into a prioritized list.
+ */
+struct mode_selection_state {
+	enum typec_mode_type mode;
+	int priority;
+	struct list_head list;
+};
+
+/* -------------------------------------------------------------------------- */
+/* port 'mode_priorities' attribute */
+
+int typec_mode_set_priority(struct typec_altmode *adev, const int priority)
+{
+	struct typec_port *port = to_typec_port(adev->dev.parent);
+	const enum typec_mode_type mode = typec_svid_to_altmode(adev->svid);
+	struct mode_selection_state *ms_target = NULL;
+	struct mode_selection_state *ms, *tmp;
+
+	if (mode >= TYPEC_ALTMODE_MAX || !mode_names[mode])
+		return -EOPNOTSUPP;
+
+	list_for_each_entry_safe(ms, tmp, &port->mode_list, list) {
+		if (ms->mode == mode) {
+			ms_target = ms;
+			list_del(&ms->list);
+			break;
+		}
+	}
+
+	if (!ms_target) {
+		ms_target = kzalloc(sizeof(struct mode_selection_state), GFP_KERNEL);
+		if (!ms_target)
+			return -ENOMEM;
+		ms_target->mode = mode;
+		INIT_LIST_HEAD(&ms_target->list);
+	}
+
+	if (priority >= 0)
+		ms_target->priority = priority;
+	else
+		ms_target->priority = default_priorities[mode];
+
+	while (ms_target) {
+		struct mode_selection_state *ms_peer = NULL;
+
+		list_for_each_entry(ms, &port->mode_list, list)
+			if (ms->priority >= ms_target->priority) {
+				if (ms->priority == ms_target->priority)
+					ms_peer = ms;
+				break;
+			}
+
+		list_add_tail(&ms_target->list, &ms->list);
+		ms_target = ms_peer;
+		if (ms_target) {
+			ms_target->priority++;
+			list_del(&ms_target->list);
+		}
+	}
+
+	return 0;
+}
+
+int typec_mode_get_priority(struct typec_altmode *adev, int *priority)
+{
+	struct typec_port *port = to_typec_port(adev->dev.parent);
+	const enum typec_mode_type mode = typec_svid_to_altmode(adev->svid);
+	struct mode_selection_state *ms;
+
+	list_for_each_entry(ms, &port->mode_list, list)
+		if (ms->mode == mode) {
+			*priority = ms->priority;
+			return 0;
+		}
+
+	return -EOPNOTSUPP;
+}
+
+void typec_mode_selection_destroy(struct typec_port *port)
+{
+	struct mode_selection_state *ms, *tmp;
+
+	list_for_each_entry_safe(ms, tmp, &port->mode_list, list) {
+		list_del(&ms->list);
+		kfree(ms);
+	}
+}
diff --git a/drivers/usb/typec/mode_selection.h b/drivers/usb/typec/mode_selection.h
new file mode 100644
index 000000000000..69adfcf39d7c
--- /dev/null
+++ b/drivers/usb/typec/mode_selection.h
@@ -0,0 +1,8 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+
+#include <linux/usb/typec_dp.h>
+#include <linux/usb/typec_tbt.h>
+
+int typec_mode_set_priority(struct typec_altmode *adev, const int priority);
+int typec_mode_get_priority(struct typec_altmode *adev, int *priority);
+void typec_mode_selection_destroy(struct typec_port *port);
diff --git a/include/linux/usb/typec_altmode.h b/include/linux/usb/typec_altmode.h
index b3c0866ea70f..318858fc7bec 100644
--- a/include/linux/usb/typec_altmode.h
+++ b/include/linux/usb/typec_altmode.h
@@ -145,6 +145,15 @@ enum {
 
 #define TYPEC_MODAL_STATE(_state_)	((_state_) + TYPEC_STATE_MODAL)
 
+#define USB_TYPEC_USB4_SID	0xff00
+
+enum typec_mode_type {
+	TYPEC_ALTMODE_DP = 0,
+	TYPEC_ALTMODE_TBT,
+	TYPEC_ALTMODE_USB4,
+	TYPEC_ALTMODE_MAX,
+};
+
 struct typec_altmode *typec_altmode_get_plug(struct typec_altmode *altmode,
 					     enum typec_plug_index index);
 void typec_altmode_put_plug(struct typec_altmode *plug);
-- 
2.51.0.rc0.215.g125493bb4a-goog