drivers/net/ethernet/qualcomm/rmnet/rmnet_config.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /* Copyright (c) 2013-2018, The Linux Foundation. All rights reserved.
   3  *
   4  * RMNET configuration engine
   5  */
   6
   7 #include <net/sock.h>
   8 #include <linux/module.h>
   9 #include <linux/netlink.h>
  10 #include <linux/netdevice.h>
  11 #include "rmnet_config.h"
  12 #include "rmnet_handlers.h"
  13 #include "rmnet_vnd.h"
  14 #include "rmnet_private.h"
  15
  16 /* Local Definitions and Declarations */
  17
  18 static const struct nla_policy rmnet_policy[IFLA_RMNET_MAX + 1] = {
  19         [IFLA_RMNET_MUX_ID]     = { .type = NLA_U16 },
  20         [IFLA_RMNET_FLAGS]      = { .len = sizeof(struct ifla_rmnet_flags) },
  21 };
  22
  23 static int rmnet_is_real_dev_registered(const struct net_device *real_dev)
  24 {
  25         return rcu_access_pointer(real_dev->rx_handler) == rmnet_rx_handler;
  26 }
  27
  28 /* Needs rtnl lock */
  29 static struct rmnet_port*
  30 rmnet_get_port_rtnl(const struct net_device *real_dev)
  31 {
  32         return rtnl_dereference(real_dev->rx_handler_data);
  33 }
  34
  35 static int rmnet_unregister_real_device(struct net_device *real_dev)
  36 {
  37         struct rmnet_port *port = rmnet_get_port_rtnl(real_dev);
  38
  39         if (port->nr_rmnet_devs)
  40                 return -EINVAL;
  41
  42         netdev_rx_handler_unregister(real_dev);
  43
  44         kfree(port);
  45
  46         netdev_dbg(real_dev, "Removed from rmnet\n");
  47         return 0;
  48 }
  49
  50 static int rmnet_register_real_device(struct net_device *real_dev)
  51 {
  52         struct rmnet_port *port;
  53         int rc, entry;
  54
  55         ASSERT_RTNL();
  56
  57         if (rmnet_is_real_dev_registered(real_dev))
  58                 return 0;
  59
  60         port = kzalloc(sizeof(*port), GFP_ATOMIC);
  61         if (!port)
  62                 return -ENOMEM;
  63
  64         port->dev = real_dev;
  65         rc = netdev_rx_handler_register(real_dev, rmnet_rx_handler, port);
  66         if (rc) {
  67                 kfree(port);
  68                 return -EBUSY;
  69         }
  70
  71         for (entry = 0; entry < RMNET_MAX_LOGICAL_EP; entry++)
  72                 INIT_HLIST_HEAD(&port->muxed_ep[entry]);
  73
  74         netdev_dbg(real_dev, "registered with rmnet\n");
  75         return 0;
  76 }
  77
  78 static void rmnet_unregister_bridge(struct rmnet_port *port)
  79 {
  80         struct net_device *bridge_dev, *real_dev, *rmnet_dev;
  81         struct rmnet_port *real_port;
  82
  83         if (port->rmnet_mode != RMNET_EPMODE_BRIDGE)
  84                 return;
  85
  86         rmnet_dev = port->rmnet_dev;
  87         if (!port->nr_rmnet_devs) {
  88                 /* bridge device */
  89                 real_dev = port->bridge_ep;
  90                 bridge_dev = port->dev;
  91
  92                 real_port = rmnet_get_port_rtnl(real_dev);
  93                 real_port->bridge_ep = NULL;
  94                 real_port->rmnet_mode = RMNET_EPMODE_VND;
  95         } else {
  96                 /* real device */
  97                 bridge_dev = port->bridge_ep;
  98
  99                 port->bridge_ep = NULL;
 100                 port->rmnet_mode = RMNET_EPMODE_VND;
 101         }
 102
 103         netdev_upper_dev_unlink(bridge_dev, rmnet_dev);
 104         rmnet_unregister_real_device(bridge_dev);
 105 }
 106
 107 static int rmnet_newlink(struct net *src_net, struct net_device *dev,
 108                          struct nlattr *tb[], struct nlattr *data[],
 109                          struct netlink_ext_ack *extack)
 110 {
 111         u32 data_format = RMNET_FLAGS_INGRESS_DEAGGREGATION;
 112         struct net_device *real_dev;
 113         int mode = RMNET_EPMODE_VND;
 114         struct rmnet_endpoint *ep;
 115         struct rmnet_port *port;
 116         int err = 0;
 117         u16 mux_id;
 118
 119         if (!tb[IFLA_LINK]) {
 120                 NL_SET_ERR_MSG_MOD(extack, "link not specified");
 121                 return -EINVAL;
 122         }
 123
 124         real_dev = __dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
 125         if (!real_dev || !dev)
 126                 return -ENODEV;
 127
 128         if (!data[IFLA_RMNET_MUX_ID])
 129                 return -EINVAL;
 130
 131         ep = kzalloc(sizeof(*ep), GFP_ATOMIC);
 132         if (!ep)
 133                 return -ENOMEM;
 134
 135         mux_id = nla_get_u16(data[IFLA_RMNET_MUX_ID]);
 136
 137         err = rmnet_register_real_device(real_dev);
 138         if (err)
 139                 goto err0;
 140
 141         port = rmnet_get_port_rtnl(real_dev);
 142         err = rmnet_vnd_newlink(mux_id, dev, port, real_dev, ep);
 143         if (err)
 144                 goto err1;
 145
 146         err = netdev_upper_dev_link(real_dev, dev, extack);
 147         if (err < 0)
 148                 goto err2;
 149
 150         port->rmnet_mode = mode;
 151         port->rmnet_dev = dev;
 152
 153         hlist_add_head_rcu(&ep->hlnode, &port->muxed_ep[mux_id]);
 154
 155         if (data[IFLA_RMNET_FLAGS]) {
 156                 struct ifla_rmnet_flags *flags;
 157
 158                 flags = nla_data(data[IFLA_RMNET_FLAGS]);
 159                 data_format = flags->flags & flags->mask;
 160         }
 161
 162         netdev_dbg(dev, "data format [0x%08X]\n", data_format);
 163         port->data_format = data_format;
 164
 165         return 0;
 166
 167 err2:
 168         unregister_netdevice(dev);
 169         rmnet_vnd_dellink(mux_id, port, ep);
 170 err1:
 171         rmnet_unregister_real_device(real_dev);
 172 err0:
 173         kfree(ep);
 174         return err;
 175 }
 176
 177 static void rmnet_dellink(struct net_device *dev, struct list_head *head)
 178 {
 179         struct rmnet_priv *priv = netdev_priv(dev);
 180         struct net_device *real_dev, *bridge_dev;
 181         struct rmnet_port *real_port, *bridge_port;
 182         struct rmnet_endpoint *ep;
 183         u8 mux_id = priv->mux_id;
 184
 185         real_dev = priv->real_dev;
 186
 187         if (!rmnet_is_real_dev_registered(real_dev))
 188                 return;
 189
 190         real_port = rmnet_get_port_rtnl(real_dev);
 191         bridge_dev = real_port->bridge_ep;
 192         if (bridge_dev) {
 193                 bridge_port = rmnet_get_port_rtnl(bridge_dev);
 194                 rmnet_unregister_bridge(bridge_port);
 195         }
 196
 197         ep = rmnet_get_endpoint(real_port, mux_id);
 198         if (ep) {
 199                 hlist_del_init_rcu(&ep->hlnode);
 200                 rmnet_vnd_dellink(mux_id, real_port, ep);
 201                 kfree(ep);
 202         }
 203
 204         netdev_upper_dev_unlink(real_dev, dev);
 205         rmnet_unregister_real_device(real_dev);
 206         unregister_netdevice_queue(dev, head);
 207 }
 208
 209 static void rmnet_force_unassociate_device(struct net_device *real_dev)
 210 {
 211         struct hlist_node *tmp_ep;
 212         struct rmnet_endpoint *ep;
 213         struct rmnet_port *port;
 214         unsigned long bkt_ep;
 215         LIST_HEAD(list);
 216
 217         port = rmnet_get_port_rtnl(real_dev);
 218
 219         if (port->nr_rmnet_devs) {
 220                 /* real device */
 221                 rmnet_unregister_bridge(port);
 222                 hash_for_each_safe(port->muxed_ep, bkt_ep, tmp_ep, ep, hlnode) {
 223                         unregister_netdevice_queue(ep->egress_dev, &list);
 224                         netdev_upper_dev_unlink(real_dev, ep->egress_dev);
 225                         rmnet_vnd_dellink(ep->mux_id, port, ep);
 226                         hlist_del_init_rcu(&ep->hlnode);
 227                         kfree(ep);
 228                 }
 229                 rmnet_unregister_real_device(real_dev);
 230                 unregister_netdevice_many(&list);
 231         } else {
 232                 rmnet_unregister_bridge(port);
 233         }
 234 }
 235
 236 static int rmnet_config_notify_cb(struct notifier_block *nb,
 237                                   unsigned long event, void *data)
 238 {
 239         struct net_device *real_dev = netdev_notifier_info_to_dev(data);
 240
 241         if (!rmnet_is_real_dev_registered(real_dev))
 242                 return NOTIFY_DONE;
 243
 244         switch (event) {
 245         case NETDEV_UNREGISTER:
 246                 netdev_dbg(real_dev, "Kernel unregister\n");
 247                 rmnet_force_unassociate_device(real_dev);
 248                 break;
 249
 250         default:
 251                 break;
 252         }
 253
 254         return NOTIFY_DONE;
 255 }
 256
 257 static struct notifier_block rmnet_dev_notifier __read_mostly = {
 258         .notifier_call = rmnet_config_notify_cb,
 259 };
 260
 261 static int rmnet_rtnl_validate(struct nlattr *tb[], struct nlattr *data[],
 262                                struct netlink_ext_ack *extack)
 263 {
 264         u16 mux_id;
 265
 266         if (!data || !data[IFLA_RMNET_MUX_ID])
 267                 return -EINVAL;
 268
 269         mux_id = nla_get_u16(data[IFLA_RMNET_MUX_ID]);
 270         if (mux_id > (RMNET_MAX_LOGICAL_EP - 1))
 271                 return -ERANGE;
 272
 273         return 0;
 274 }
 275
 276 static int rmnet_changelink(struct net_device *dev, struct nlattr *tb[],
 277                             struct nlattr *data[],
 278                             struct netlink_ext_ack *extack)
 279 {
 280         struct rmnet_priv *priv = netdev_priv(dev);
 281         struct net_device *real_dev;
 282         struct rmnet_endpoint *ep;
 283         struct rmnet_port *port;
 284         u16 mux_id;
 285
 286         if (!dev)
 287                 return -ENODEV;
 288
 289         real_dev = priv->real_dev;
 290         if (!rmnet_is_real_dev_registered(real_dev))
 291                 return -ENODEV;
 292
 293         port = rmnet_get_port_rtnl(real_dev);
 294
 295         if (data[IFLA_RMNET_MUX_ID]) {
 296                 mux_id = nla_get_u16(data[IFLA_RMNET_MUX_ID]);
 297                 if (rmnet_get_endpoint(port, mux_id)) {
 298                         NL_SET_ERR_MSG_MOD(extack, "MUX ID already exists");
 299                         return -EINVAL;
 300                 }
 301                 ep = rmnet_get_endpoint(port, priv->mux_id);
 302                 if (!ep)
 303                         return -ENODEV;
 304
 305                 hlist_del_init_rcu(&ep->hlnode);
 306                 hlist_add_head_rcu(&ep->hlnode, &port->muxed_ep[mux_id]);
 307
 308                 ep->mux_id = mux_id;
 309                 priv->mux_id = mux_id;
 310         }
 311
 312         if (data[IFLA_RMNET_FLAGS]) {
 313                 struct ifla_rmnet_flags *flags;
 314
 315                 flags = nla_data(data[IFLA_RMNET_FLAGS]);
 316                 port->data_format = flags->flags & flags->mask;
 317         }
 318
 319         return 0;
 320 }
 321
 322 static size_t rmnet_get_size(const struct net_device *dev)
 323 {
 324         return
 325                 /* IFLA_RMNET_MUX_ID */
 326                 nla_total_size(2) +
 327                 /* IFLA_RMNET_FLAGS */
 328                 nla_total_size(sizeof(struct ifla_rmnet_flags));
 329 }
 330
 331 static int rmnet_fill_info(struct sk_buff *skb, const struct net_device *dev)
 332 {
 333         struct rmnet_priv *priv = netdev_priv(dev);
 334         struct net_device *real_dev;
 335         struct ifla_rmnet_flags f;
 336         struct rmnet_port *port;
 337
 338         real_dev = priv->real_dev;
 339
 340         if (nla_put_u16(skb, IFLA_RMNET_MUX_ID, priv->mux_id))
 341                 goto nla_put_failure;
 342
 343         if (rmnet_is_real_dev_registered(real_dev)) {
 344                 port = rmnet_get_port_rtnl(real_dev);
 345                 f.flags = port->data_format;
 346         } else {
 347                 f.flags = 0;
 348         }
 349
 350         f.mask  = ~0;
 351
 352         if (nla_put(skb, IFLA_RMNET_FLAGS, sizeof(f), &f))
 353                 goto nla_put_failure;
 354
 355         return 0;
 356
 357 nla_put_failure:
 358         return -EMSGSIZE;
 359 }
 360
 361 struct rtnl_link_ops rmnet_link_ops __read_mostly = {
 362         .kind           = "rmnet",
 363         .maxtype        = __IFLA_RMNET_MAX,
 364         .priv_size      = sizeof(struct rmnet_priv),
 365         .setup          = rmnet_vnd_setup,
 366         .validate       = rmnet_rtnl_validate,
 367         .newlink        = rmnet_newlink,
 368         .dellink        = rmnet_dellink,
 369         .get_size       = rmnet_get_size,
 370         .changelink     = rmnet_changelink,
 371         .policy         = rmnet_policy,
 372         .fill_info      = rmnet_fill_info,
 373 };
 374
 375 struct rmnet_port *rmnet_get_port_rcu(struct net_device *real_dev)
 376 {
 377         if (rmnet_is_real_dev_registered(real_dev))
 378                 return rcu_dereference_bh(real_dev->rx_handler_data);
 379         else
 380                 return NULL;
 381 }
 382
 383 struct rmnet_endpoint *rmnet_get_endpoint(struct rmnet_port *port, u8 mux_id)
 384 {
 385         struct rmnet_endpoint *ep;
 386
 387         hlist_for_each_entry_rcu(ep, &port->muxed_ep[mux_id], hlnode) {
 388                 if (ep->mux_id == mux_id)
 389                         return ep;
 390         }
 391
 392         return NULL;
 393 }
 394
 395 int rmnet_add_bridge(struct net_device *rmnet_dev,
 396                      struct net_device *slave_dev,
 397                      struct netlink_ext_ack *extack)
 398 {
 399         struct rmnet_priv *priv = netdev_priv(rmnet_dev);
 400         struct net_device *real_dev = priv->real_dev;
 401         struct rmnet_port *port, *slave_port;
 402         int err;
 403
 404         port = rmnet_get_port_rtnl(real_dev);
 405
 406         /* If there is more than one rmnet dev attached, its probably being
 407          * used for muxing. Skip the briding in that case
 408          */
 409         if (port->nr_rmnet_devs > 1)
 410                 return -EINVAL;
 411
 412         if (port->rmnet_mode != RMNET_EPMODE_VND)
 413                 return -EINVAL;
 414
 415         if (rmnet_is_real_dev_registered(slave_dev))
 416                 return -EBUSY;
 417
 418         err = rmnet_register_real_device(slave_dev);
 419         if (err)
 420                 return -EBUSY;
 421
 422         err = netdev_master_upper_dev_link(slave_dev, rmnet_dev, NULL, NULL,
 423                                            extack);
 424         if (err) {
 425                 rmnet_unregister_real_device(slave_dev);
 426                 return err;
 427         }
 428
 429         slave_port = rmnet_get_port_rtnl(slave_dev);
 430         slave_port->rmnet_mode = RMNET_EPMODE_BRIDGE;
 431         slave_port->bridge_ep = real_dev;
 432         slave_port->rmnet_dev = rmnet_dev;
 433
 434         port->rmnet_mode = RMNET_EPMODE_BRIDGE;
 435         port->bridge_ep = slave_dev;
 436
 437         netdev_dbg(slave_dev, "registered with rmnet as slave\n");
 438         return 0;
 439 }
 440
 441 int rmnet_del_bridge(struct net_device *rmnet_dev,
 442                      struct net_device *slave_dev)
 443 {
 444         struct rmnet_port *port = rmnet_get_port_rtnl(slave_dev);
 445
 446         rmnet_unregister_bridge(port);
 447
 448         netdev_dbg(slave_dev, "removed from rmnet as slave\n");
 449         return 0;
 450 }
 451
 452 /* Startup/Shutdown */
 453
 454 static int __init rmnet_init(void)
 455 {
 456         int rc;
 457
 458         rc = register_netdevice_notifier(&rmnet_dev_notifier);
 459         if (rc != 0)
 460                 return rc;
 461
 462         rc = rtnl_link_register(&rmnet_link_ops);
 463         if (rc != 0) {
 464                 unregister_netdevice_notifier(&rmnet_dev_notifier);
 465                 return rc;
 466         }
 467         return rc;
 468 }
 469
 470 static void __exit rmnet_exit(void)
 471 {
 472         rtnl_link_unregister(&rmnet_link_ops);
 473         unregister_netdevice_notifier(&rmnet_dev_notifier);
 474 }
 475
 476 module_init(rmnet_init)
 477 module_exit(rmnet_exit)
 478 MODULE_LICENSE("GPL v2");