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