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