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