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_KERNEL);
  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) {
 126                 NL_SET_ERR_MSG_MOD(extack, "link does not exist");
 127                 return -ENODEV;
 128         }
 129
 130         ep = kzalloc(sizeof(*ep), GFP_KERNEL);
 131         if (!ep)
 132                 return -ENOMEM;
 133
 134         mux_id = nla_get_u16(data[IFLA_RMNET_MUX_ID]);
 135
 136         err = rmnet_register_real_device(real_dev);
 137         if (err)
 138                 goto err0;
 139
 140         port = rmnet_get_port_rtnl(real_dev);
 141         err = rmnet_vnd_newlink(mux_id, dev, port, real_dev, ep, extack);
 142         if (err)
 143                 goto err1;
 144
 145         err = netdev_upper_dev_link(real_dev, dev, extack);
 146         if (err < 0)
 147                 goto err2;
 148
 149         port->rmnet_mode = mode;
 150         port->rmnet_dev = dev;
 151
 152         hlist_add_head_rcu(&ep->hlnode, &port->muxed_ep[mux_id]);
 153
 154         if (data[IFLA_RMNET_FLAGS]) {
 155                 struct ifla_rmnet_flags *flags;
 156
 157                 flags = nla_data(data[IFLA_RMNET_FLAGS]);
 158                 data_format = flags->flags & flags->mask;
 159         }
 160
 161         netdev_dbg(dev, "data format [0x%08X]\n", data_format);
 162         port->data_format = data_format;
 163
 164         return 0;
 165
 166 err2:
 167         unregister_netdevice(dev);
 168         rmnet_vnd_dellink(mux_id, port, ep);
 169 err1:
 170         rmnet_unregister_real_device(real_dev);
 171 err0:
 172         kfree(ep);
 173         return err;
 174 }
 175
 176 static void rmnet_dellink(struct net_device *dev, struct list_head *head)
 177 {
 178         struct rmnet_priv *priv = netdev_priv(dev);
 179         struct net_device *real_dev, *bridge_dev;
 180         struct rmnet_port *real_port, *bridge_port;
 181         struct rmnet_endpoint *ep;
 182         u8 mux_id = priv->mux_id;
 183
 184         real_dev = priv->real_dev;
 185
 186         if (!rmnet_is_real_dev_registered(real_dev))
 187                 return;
 188
 189         real_port = rmnet_get_port_rtnl(real_dev);
 190         bridge_dev = real_port->bridge_ep;
 191         if (bridge_dev) {
 192                 bridge_port = rmnet_get_port_rtnl(bridge_dev);
 193                 rmnet_unregister_bridge(bridge_port);
 194         }
 195
 196         ep = rmnet_get_endpoint(real_port, mux_id);
 197         if (ep) {
 198                 hlist_del_init_rcu(&ep->hlnode);
 199                 rmnet_vnd_dellink(mux_id, real_port, ep);
 200                 kfree(ep);
 201         }
 202
 203         netdev_upper_dev_unlink(real_dev, dev);
 204         rmnet_unregister_real_device(real_dev);
 205         unregister_netdevice_queue(dev, head);
 206 }
 207
 208 static void rmnet_force_unassociate_device(struct net_device *real_dev)
 209 {
 210         struct hlist_node *tmp_ep;
 211         struct rmnet_endpoint *ep;
 212         struct rmnet_port *port;
 213         unsigned long bkt_ep;
 214         LIST_HEAD(list);
 215
 216         port = rmnet_get_port_rtnl(real_dev);
 217
 218         if (port->nr_rmnet_devs) {
 219                 /* real device */
 220                 rmnet_unregister_bridge(port);
 221                 hash_for_each_safe(port->muxed_ep, bkt_ep, tmp_ep, ep, hlnode) {
 222                         unregister_netdevice_queue(ep->egress_dev, &list);
 223                         netdev_upper_dev_unlink(real_dev, ep->egress_dev);
 224                         rmnet_vnd_dellink(ep->mux_id, port, ep);
 225                         hlist_del_init_rcu(&ep->hlnode);
 226                         kfree(ep);
 227                 }
 228                 rmnet_unregister_real_device(real_dev);
 229                 unregister_netdevice_many(&list);
 230         } else {
 231                 rmnet_unregister_bridge(port);
 232         }
 233 }
 234
 235 static int rmnet_config_notify_cb(struct notifier_block *nb,
 236                                   unsigned long event, void *data)
 237 {
 238         struct net_device *real_dev = netdev_notifier_info_to_dev(data);
 239
 240         if (!rmnet_is_real_dev_registered(real_dev))
 241                 return NOTIFY_DONE;
 242
 243         switch (event) {
 244         case NETDEV_UNREGISTER:
 245                 netdev_dbg(real_dev, "Kernel unregister\n");
 246                 rmnet_force_unassociate_device(real_dev);
 247                 break;
 248
 249         default:
 250                 break;
 251         }
 252
 253         return NOTIFY_DONE;
 254 }
 255
 256 static struct notifier_block rmnet_dev_notifier __read_mostly = {
 257         .notifier_call = rmnet_config_notify_cb,
 258 };
 259
 260 static int rmnet_rtnl_validate(struct nlattr *tb[], struct nlattr *data[],
 261                                struct netlink_ext_ack *extack)
 262 {
 263         u16 mux_id;
 264
 265         if (!data || !data[IFLA_RMNET_MUX_ID]) {
 266                 NL_SET_ERR_MSG_MOD(extack, "MUX ID not specified");
 267                 return -EINVAL;
 268         }
 269
 270         mux_id = nla_get_u16(data[IFLA_RMNET_MUX_ID]);
 271         if (mux_id > (RMNET_MAX_LOGICAL_EP - 1)) {
 272                 NL_SET_ERR_MSG_MOD(extack, "invalid MUX ID");
 273                 return -ERANGE;
 274         }
 275
 276         return 0;
 277 }
 278
 279 static int rmnet_changelink(struct net_device *dev, struct nlattr *tb[],
 280                             struct nlattr *data[],
 281                             struct netlink_ext_ack *extack)
 282 {
 283         struct rmnet_priv *priv = netdev_priv(dev);
 284         struct net_device *real_dev;
 285         struct rmnet_endpoint *ep;
 286         struct rmnet_port *port;
 287         u16 mux_id;
 288
 289         if (!dev)
 290                 return -ENODEV;
 291
 292         real_dev = priv->real_dev;
 293         if (!rmnet_is_real_dev_registered(real_dev))
 294                 return -ENODEV;
 295
 296         port = rmnet_get_port_rtnl(real_dev);
 297
 298         if (data[IFLA_RMNET_MUX_ID]) {
 299                 mux_id = nla_get_u16(data[IFLA_RMNET_MUX_ID]);
 300                 if (rmnet_get_endpoint(port, mux_id)) {
 301                         NL_SET_ERR_MSG_MOD(extack, "MUX ID already exists");
 302                         return -EINVAL;
 303                 }
 304                 ep = rmnet_get_endpoint(port, priv->mux_id);
 305                 if (!ep)
 306                         return -ENODEV;
 307
 308                 hlist_del_init_rcu(&ep->hlnode);
 309                 hlist_add_head_rcu(&ep->hlnode, &port->muxed_ep[mux_id]);
 310
 311                 ep->mux_id = mux_id;
 312                 priv->mux_id = mux_id;
 313         }
 314
 315         if (data[IFLA_RMNET_FLAGS]) {
 316                 struct ifla_rmnet_flags *flags;
 317
 318                 flags = nla_data(data[IFLA_RMNET_FLAGS]);
 319                 port->data_format = flags->flags & flags->mask;
 320         }
 321
 322         return 0;
 323 }
 324
 325 static size_t rmnet_get_size(const struct net_device *dev)
 326 {
 327         return
 328                 /* IFLA_RMNET_MUX_ID */
 329                 nla_total_size(2) +
 330                 /* IFLA_RMNET_FLAGS */
 331                 nla_total_size(sizeof(struct ifla_rmnet_flags));
 332 }
 333
 334 static int rmnet_fill_info(struct sk_buff *skb, const struct net_device *dev)
 335 {
 336         struct rmnet_priv *priv = netdev_priv(dev);
 337         struct net_device *real_dev;
 338         struct ifla_rmnet_flags f;
 339         struct rmnet_port *port;
 340
 341         real_dev = priv->real_dev;
 342
 343         if (nla_put_u16(skb, IFLA_RMNET_MUX_ID, priv->mux_id))
 344                 goto nla_put_failure;
 345
 346         if (rmnet_is_real_dev_registered(real_dev)) {
 347                 port = rmnet_get_port_rtnl(real_dev);
 348                 f.flags = port->data_format;
 349         } else {
 350                 f.flags = 0;
 351         }
 352
 353         f.mask  = ~0;
 354
 355         if (nla_put(skb, IFLA_RMNET_FLAGS, sizeof(f), &f))
 356                 goto nla_put_failure;
 357
 358         return 0;
 359
 360 nla_put_failure:
 361         return -EMSGSIZE;
 362 }
 363
 364 struct rtnl_link_ops rmnet_link_ops __read_mostly = {
 365         .kind           = "rmnet",
 366         .maxtype        = __IFLA_RMNET_MAX,
 367         .priv_size      = sizeof(struct rmnet_priv),
 368         .setup          = rmnet_vnd_setup,
 369         .validate       = rmnet_rtnl_validate,
 370         .newlink        = rmnet_newlink,
 371         .dellink        = rmnet_dellink,
 372         .get_size       = rmnet_get_size,
 373         .changelink     = rmnet_changelink,
 374         .policy         = rmnet_policy,
 375         .fill_info      = rmnet_fill_info,
 376 };
 377
 378 struct rmnet_port *rmnet_get_port_rcu(struct net_device *real_dev)
 379 {
 380         if (rmnet_is_real_dev_registered(real_dev))
 381                 return rcu_dereference_bh(real_dev->rx_handler_data);
 382         else
 383                 return NULL;
 384 }
 385
 386 struct rmnet_endpoint *rmnet_get_endpoint(struct rmnet_port *port, u8 mux_id)
 387 {
 388         struct rmnet_endpoint *ep;
 389
 390         hlist_for_each_entry_rcu(ep, &port->muxed_ep[mux_id], hlnode) {
 391                 if (ep->mux_id == mux_id)
 392                         return ep;
 393         }
 394
 395         return NULL;
 396 }
 397
 398 int rmnet_add_bridge(struct net_device *rmnet_dev,
 399                      struct net_device *slave_dev,
 400                      struct netlink_ext_ack *extack)
 401 {
 402         struct rmnet_priv *priv = netdev_priv(rmnet_dev);
 403         struct net_device *real_dev = priv->real_dev;
 404         struct rmnet_port *port, *slave_port;
 405         int err;
 406
 407         port = rmnet_get_port_rtnl(real_dev);
 408
 409         /* If there is more than one rmnet dev attached, its probably being
 410          * used for muxing. Skip the briding in that case
 411          */
 412         if (port->nr_rmnet_devs > 1) {
 413                 NL_SET_ERR_MSG_MOD(extack, "more than one rmnet dev attached");
 414                 return -EINVAL;
 415         }
 416
 417         if (port->rmnet_mode != RMNET_EPMODE_VND) {
 418                 NL_SET_ERR_MSG_MOD(extack, "bridge device already exists");
 419                 return -EINVAL;
 420         }
 421
 422         if (rmnet_is_real_dev_registered(slave_dev)) {
 423                 NL_SET_ERR_MSG_MOD(extack,
 424                                    "slave cannot be another rmnet dev");
 425
 426                 return -EBUSY;
 427         }
 428
 429         err = rmnet_register_real_device(slave_dev);
 430         if (err)
 431                 return -EBUSY;
 432
 433         err = netdev_master_upper_dev_link(slave_dev, rmnet_dev, NULL, NULL,
 434                                            extack);
 435         if (err) {
 436                 rmnet_unregister_real_device(slave_dev);
 437                 return err;
 438         }
 439
 440         slave_port = rmnet_get_port_rtnl(slave_dev);
 441         slave_port->rmnet_mode = RMNET_EPMODE_BRIDGE;
 442         slave_port->bridge_ep = real_dev;
 443         slave_port->rmnet_dev = rmnet_dev;
 444
 445         port->rmnet_mode = RMNET_EPMODE_BRIDGE;
 446         port->bridge_ep = slave_dev;
 447
 448         netdev_dbg(slave_dev, "registered with rmnet as slave\n");
 449         return 0;
 450 }
 451
 452 int rmnet_del_bridge(struct net_device *rmnet_dev,
 453                      struct net_device *slave_dev)
 454 {
 455         struct rmnet_port *port = rmnet_get_port_rtnl(slave_dev);
 456
 457         rmnet_unregister_bridge(port);
 458
 459         netdev_dbg(slave_dev, "removed from rmnet as slave\n");
 460         return 0;
 461 }
 462
 463 /* Startup/Shutdown */
 464
 465 static int __init rmnet_init(void)
 466 {
 467         int rc;
 468
 469         rc = register_netdevice_notifier(&rmnet_dev_notifier);
 470         if (rc != 0)
 471                 return rc;
 472
 473         rc = rtnl_link_register(&rmnet_link_ops);
 474         if (rc != 0) {
 475                 unregister_netdevice_notifier(&rmnet_dev_notifier);
 476                 return rc;
 477         }
 478         return rc;
 479 }
 480
 481 static void __exit rmnet_exit(void)
 482 {
 483         rtnl_link_unregister(&rmnet_link_ops);
 484         unregister_netdevice_notifier(&rmnet_dev_notifier);
 485 }
 486
 487 module_init(rmnet_init)
 488 module_exit(rmnet_exit)
 489 MODULE_ALIAS_RTNL_LINK("rmnet");
 490 MODULE_LICENSE("GPL v2");