drivers/usb/typec/port-mapper.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * USB Type-C Connector Class Port Mapping Utility
   4  *
   5  * Copyright (C) 2021, Intel Corporation
   6  * Author: Heikki Krogerus <heikki.krogerus@linux.intel.com>
   7  */
   8
   9 #include <linux/acpi.h>
  10 #include <linux/usb.h>
  11 #include <linux/usb/typec.h>
  12
  13 #include "class.h"
  14
  15 struct port_node {
  16         struct list_head list;
  17         struct device *dev;
  18         void *pld;
  19 };
  20
  21 static int acpi_pld_match(const struct acpi_pld_info *pld1,
  22                           const struct acpi_pld_info *pld2)
  23 {
  24         if (!pld1 || !pld2)
  25                 return 0;
  26
  27         /*
  28          * To speed things up, first checking only the group_position. It seems
  29          * to often have the first unique value in the _PLD.
  30          */
  31         if (pld1->group_position == pld2->group_position)
  32                 return !memcmp(pld1, pld2, sizeof(struct acpi_pld_info));
  33
  34         return 0;
  35 }
  36
  37 static void *get_pld(struct device *dev)
  38 {
  39 #ifdef CONFIG_ACPI
  40         struct acpi_pld_info *pld;
  41         acpi_status status;
  42
  43         if (!has_acpi_companion(dev))
  44                 return NULL;
  45
  46         status = acpi_get_physical_device_location(ACPI_HANDLE(dev), &pld);
  47         if (ACPI_FAILURE(status))
  48                 return NULL;
  49
  50         return pld;
  51 #else
  52         return NULL;
  53 #endif
  54 }
  55
  56 static void free_pld(void *pld)
  57 {
  58 #ifdef CONFIG_ACPI
  59         ACPI_FREE(pld);
  60 #endif
  61 }
  62
  63 static int __link_port(struct typec_port *con, struct port_node *node)
  64 {
  65         int ret;
  66
  67         ret = sysfs_create_link(&node->dev->kobj, &con->dev.kobj, "connector");
  68         if (ret)
  69                 return ret;
  70
  71         ret = sysfs_create_link(&con->dev.kobj, &node->dev->kobj,
  72                                 dev_name(node->dev));
  73         if (ret) {
  74                 sysfs_remove_link(&node->dev->kobj, "connector");
  75                 return ret;
  76         }
  77
  78         list_add_tail(&node->list, &con->port_list);
  79
  80         return 0;
  81 }
  82
  83 static int link_port(struct typec_port *con, struct port_node *node)
  84 {
  85         int ret;
  86
  87         mutex_lock(&con->port_list_lock);
  88         ret = __link_port(con, node);
  89         mutex_unlock(&con->port_list_lock);
  90
  91         return ret;
  92 }
  93
  94 static void __unlink_port(struct typec_port *con, struct port_node *node)
  95 {
  96         sysfs_remove_link(&con->dev.kobj, dev_name(node->dev));
  97         sysfs_remove_link(&node->dev->kobj, "connector");
  98         list_del(&node->list);
  99 }
 100
 101 static void unlink_port(struct typec_port *con, struct port_node *node)
 102 {
 103         mutex_lock(&con->port_list_lock);
 104         __unlink_port(con, node);
 105         mutex_unlock(&con->port_list_lock);
 106 }
 107
 108 static struct port_node *create_port_node(struct device *port)
 109 {
 110         struct port_node *node;
 111
 112         node = kzalloc(sizeof(*node), GFP_KERNEL);
 113         if (!node)
 114                 return ERR_PTR(-ENOMEM);
 115
 116         node->dev = get_device(port);
 117         node->pld = get_pld(port);
 118
 119         return node;
 120 }
 121
 122 static void remove_port_node(struct port_node *node)
 123 {
 124         put_device(node->dev);
 125         free_pld(node->pld);
 126         kfree(node);
 127 }
 128
 129 static int connector_match(struct device *dev, const void *data)
 130 {
 131         const struct port_node *node = data;
 132
 133         if (!is_typec_port(dev))
 134                 return 0;
 135
 136         return acpi_pld_match(to_typec_port(dev)->pld, node->pld);
 137 }
 138
 139 static struct device *find_connector(struct port_node *node)
 140 {
 141         if (!node->pld)
 142                 return NULL;
 143
 144         return class_find_device(&typec_class, NULL, node, connector_match);
 145 }
 146
 147 /**
 148  * typec_link_port - Link a port to its connector
 149  * @port: The port device
 150  *
 151  * Find the connector of @port and create symlink named "connector" for it.
 152  * Returns 0 on success, or errno in case of a failure.
 153  *
 154  * NOTE. The function increments the reference count of @port on success.
 155  */
 156 int typec_link_port(struct device *port)
 157 {
 158         struct device *connector;
 159         struct port_node *node;
 160         int ret;
 161
 162         node = create_port_node(port);
 163         if (IS_ERR(node))
 164                 return PTR_ERR(node);
 165
 166         connector = find_connector(node);
 167         if (!connector) {
 168                 ret = 0;
 169                 goto remove_node;
 170         }
 171
 172         ret = link_port(to_typec_port(connector), node);
 173         if (ret)
 174                 goto put_connector;
 175
 176         return 0;
 177
 178 put_connector:
 179         put_device(connector);
 180 remove_node:
 181         remove_port_node(node);
 182
 183         return ret;
 184 }
 185 EXPORT_SYMBOL_GPL(typec_link_port);
 186
 187 static int port_match_and_unlink(struct device *connector, void *port)
 188 {
 189         struct port_node *node;
 190         struct port_node *tmp;
 191         int ret = 0;
 192
 193         if (!is_typec_port(connector))
 194                 return 0;
 195
 196         mutex_lock(&to_typec_port(connector)->port_list_lock);
 197         list_for_each_entry_safe(node, tmp, &to_typec_port(connector)->port_list, list) {
 198                 ret = node->dev == port;
 199                 if (ret) {
 200                         unlink_port(to_typec_port(connector), node);
 201                         remove_port_node(node);
 202                         put_device(connector);
 203                         break;
 204                 }
 205         }
 206         mutex_unlock(&to_typec_port(connector)->port_list_lock);
 207
 208         return ret;
 209 }
 210
 211 /**
 212  * typec_unlink_port - Unlink port from its connector
 213  * @port: The port device
 214  *
 215  * Removes the symlink "connector" and decrements the reference count of @port.
 216  */
 217 void typec_unlink_port(struct device *port)
 218 {
 219         class_for_each_device(&typec_class, NULL, port, port_match_and_unlink);
 220 }
 221 EXPORT_SYMBOL_GPL(typec_unlink_port);
 222
 223 static int each_port(struct device *port, void *connector)
 224 {
 225         struct port_node *node;
 226         int ret;
 227
 228         node = create_port_node(port);
 229         if (IS_ERR(node))
 230                 return PTR_ERR(node);
 231
 232         if (!connector_match(connector, node)) {
 233                 remove_port_node(node);
 234                 return 0;
 235         }
 236
 237         ret = link_port(to_typec_port(connector), node);
 238         if (ret) {
 239                 remove_port_node(node->pld);
 240                 return ret;
 241         }
 242
 243         get_device(connector);
 244
 245         return 0;
 246 }
 247
 248 int typec_link_ports(struct typec_port *con)
 249 {
 250         int ret = 0;
 251
 252         con->pld = get_pld(&con->dev);
 253         if (!con->pld)
 254                 return 0;
 255
 256         ret = usb_for_each_port(&con->dev, each_port);
 257         if (ret)
 258                 typec_unlink_ports(con);
 259
 260         return ret;
 261 }
 262
 263 void typec_unlink_ports(struct typec_port *con)
 264 {
 265         struct port_node *node;
 266         struct port_node *tmp;
 267
 268         mutex_lock(&con->port_list_lock);
 269
 270         list_for_each_entry_safe(node, tmp, &con->port_list, list) {
 271                 __unlink_port(con, node);
 272                 remove_port_node(node);
 273                 put_device(&con->dev);
 274         }
 275
 276         mutex_unlock(&con->port_list_lock);
 277
 278         free_pld(con->pld);
 279 }