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

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