diff --git a/drivers/usb/typec/class.c b/drivers/usb/typec/class.c index 1644ca08e91c..4270bf72fc88 100644 --- a/drivers/usb/typec/class.c +++ b/drivers/usb/typec/class.c @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -1531,6 +1532,60 @@ typec_port_register_altmode(struct typec_port *port, } EXPORT_SYMBOL_GPL(typec_port_register_altmode); +void typec_port_register_altmodes(struct typec_port *port, + const struct typec_altmode_ops *ops, void *drvdata, + struct typec_altmode **altmodes, size_t n) +{ + struct fwnode_handle *altmodes_node, *child; + struct typec_altmode_desc desc; + struct typec_altmode *alt; + size_t index = 0; + u32 svid, vdo; + int ret; + + altmodes_node = device_get_named_child_node(&port->dev, "altmodes"); + if (!altmodes_node) + return; /* No altmodes specified */ + + fwnode_for_each_child_node(altmodes_node, child) { + ret = fwnode_property_read_u32(child, "svid", &svid); + if (ret) { + dev_err(&port->dev, "Error reading svid for altmode %s\n", + kbasename(of_node_full_name(to_of_node(child)))); + continue; + } + + ret = fwnode_property_read_u32(child, "vdo", &vdo); + if (ret) { + dev_err(&port->dev, "Error reading vdo for altmode %s\n", + kbasename(of_node_full_name(to_of_node(child)))); + continue; + } + + if (index >= n) { + dev_err(&port->dev, "Error not enough space for altmode %s\n", + kbasename(of_node_full_name(to_of_node(child)))); + continue; + } + + desc.svid = svid; + desc.vdo = vdo; + desc.mode = index + 1; + alt = typec_port_register_altmode(port, &desc); + if (IS_ERR(alt)) { + dev_err(&port->dev, "Error registering altmode %s\n", + kbasename(of_node_full_name(to_of_node(child)))); + continue; + } + + alt->ops = ops; + typec_altmode_set_drvdata(alt, drvdata); + altmodes[index] = alt; + index++; + } +} +EXPORT_SYMBOL_GPL(typec_port_register_altmodes); + /** * typec_register_port - Register a USB Type-C Port * @parent: Parent device diff --git a/include/linux/usb/typec.h b/include/linux/usb/typec.h index 27a6fc7c35ea..56780e541baa 100644 --- a/include/linux/usb/typec.h +++ b/include/linux/usb/typec.h @@ -14,6 +14,7 @@ struct typec_partner; struct typec_cable; struct typec_plug; struct typec_port; +struct typec_altmode_ops; struct fwnode_handle; struct device; @@ -118,6 +119,11 @@ struct typec_altmode struct typec_altmode *typec_port_register_altmode(struct typec_port *port, const struct typec_altmode_desc *desc); + +void typec_port_register_altmodes(struct typec_port *port, + const struct typec_altmode_ops *ops, void *drvdata, + struct typec_altmode **altmodes, size_t n); + void typec_unregister_altmode(struct typec_altmode *altmode); struct typec_port *typec_altmode2port(struct typec_altmode *alt);