qualcomm/rmnet/rmnet_config.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /* Copyright (c) 2013-2018, The Linux Foundation. All rights reserved.
0003  *
0004  * RMNET configuration engine
0005  */
0006
0007 #include <net/sock.h>
0008 #include <linux/module.h>
0009 #include <linux/netlink.h>
0010 #include <linux/netdevice.h>
0011 #include "rmnet_config.h"
0012 #include "rmnet_handlers.h"
0013 #include "rmnet_vnd.h"
0014 #include "rmnet_private.h"
0015
0016 /* Local Definitions and Declarations */
0017
0018 static const struct nla_policy rmnet_policy[IFLA_RMNET_MAX + 1] = {
0019     [IFLA_RMNET_MUX_ID] = { .type = NLA_U16 },
0020     [IFLA_RMNET_FLAGS]  = { .len = sizeof(struct ifla_rmnet_flags) },
0021 };
0022
0023 static int rmnet_is_real_dev_registered(const struct net_device *real_dev)
0024 {
0025     return rcu_access_pointer(real_dev->rx_handler) == rmnet_rx_handler;
0026 }
0027
0028 /* Needs rtnl lock */
0029 struct rmnet_port*
0030 rmnet_get_port_rtnl(const struct net_device *real_dev)
0031 {
0032     return rtnl_dereference(real_dev->rx_handler_data);
0033 }
0034
0035 static int rmnet_unregister_real_device(struct net_device *real_dev)
0036 {
0037     struct rmnet_port *port = rmnet_get_port_rtnl(real_dev);
0038
0039     if (port->nr_rmnet_devs)
0040         return -EINVAL;
0041
0042     netdev_rx_handler_unregister(real_dev);
0043
0044     kfree(port);
0045
0046     netdev_dbg(real_dev, "Removed from rmnet\n");
0047     return 0;
0048 }
0049
0050 static int rmnet_register_real_device(struct net_device *real_dev,
0051                       struct netlink_ext_ack *extack)
0052 {
0053     struct rmnet_port *port;
0054     int rc, entry;
0055
0056     ASSERT_RTNL();
0057
0058     if (rmnet_is_real_dev_registered(real_dev)) {
0059         port = rmnet_get_port_rtnl(real_dev);
0060         if (port->rmnet_mode != RMNET_EPMODE_VND) {
0061             NL_SET_ERR_MSG_MOD(extack, "bridge device already exists");
0062             return -EINVAL;
0063         }
0064
0065         return 0;
0066     }
0067
0068     port = kzalloc(sizeof(*port), GFP_KERNEL);
0069     if (!port)
0070         return -ENOMEM;
0071
0072     port->dev = real_dev;
0073     rc = netdev_rx_handler_register(real_dev, rmnet_rx_handler, port);
0074     if (rc) {
0075         kfree(port);
0076         return -EBUSY;
0077     }
0078
0079     for (entry = 0; entry < RMNET_MAX_LOGICAL_EP; entry++)
0080         INIT_HLIST_HEAD(&port->muxed_ep[entry]);
0081
0082     netdev_dbg(real_dev, "registered with rmnet\n");
0083     return 0;
0084 }
0085
0086 static void rmnet_unregister_bridge(struct rmnet_port *port)
0087 {
0088     struct net_device *bridge_dev, *real_dev, *rmnet_dev;
0089     struct rmnet_port *real_port;
0090
0091     if (port->rmnet_mode != RMNET_EPMODE_BRIDGE)
0092         return;
0093
0094     rmnet_dev = port->rmnet_dev;
0095     if (!port->nr_rmnet_devs) {
0096         /* bridge device */
0097         real_dev = port->bridge_ep;
0098         bridge_dev = port->dev;
0099
0100         real_port = rmnet_get_port_rtnl(real_dev);
0101         real_port->bridge_ep = NULL;
0102         real_port->rmnet_mode = RMNET_EPMODE_VND;
0103     } else {
0104         /* real device */
0105         bridge_dev = port->bridge_ep;
0106
0107         port->bridge_ep = NULL;
0108         port->rmnet_mode = RMNET_EPMODE_VND;
0109     }
0110
0111     netdev_upper_dev_unlink(bridge_dev, rmnet_dev);
0112     rmnet_unregister_real_device(bridge_dev);
0113 }
0114
0115 static int rmnet_newlink(struct net *src_net, struct net_device *dev,
0116              struct nlattr *tb[], struct nlattr *data[],
0117              struct netlink_ext_ack *extack)
0118 {
0119     u32 data_format = RMNET_FLAGS_INGRESS_DEAGGREGATION;
0120     struct net_device *real_dev;
0121     int mode = RMNET_EPMODE_VND;
0122     struct rmnet_endpoint *ep;
0123     struct rmnet_port *port;
0124     int err = 0;
0125     u16 mux_id;
0126
0127     if (!tb[IFLA_LINK]) {
0128         NL_SET_ERR_MSG_MOD(extack, "link not specified");
0129         return -EINVAL;
0130     }
0131
0132     real_dev = __dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
0133     if (!real_dev) {
0134         NL_SET_ERR_MSG_MOD(extack, "link does not exist");
0135         return -ENODEV;
0136     }
0137
0138     ep = kzalloc(sizeof(*ep), GFP_KERNEL);
0139     if (!ep)
0140         return -ENOMEM;
0141
0142     mux_id = nla_get_u16(data[IFLA_RMNET_MUX_ID]);
0143
0144     err = rmnet_register_real_device(real_dev, extack);
0145     if (err)
0146         goto err0;
0147
0148     port = rmnet_get_port_rtnl(real_dev);
0149     err = rmnet_vnd_newlink(mux_id, dev, port, real_dev, ep, extack);
0150     if (err)
0151         goto err1;
0152
0153     err = netdev_upper_dev_link(real_dev, dev, extack);
0154     if (err < 0)
0155         goto err2;
0156
0157     port->rmnet_mode = mode;
0158     port->rmnet_dev = dev;
0159
0160     hlist_add_head_rcu(&ep->hlnode, &port->muxed_ep[mux_id]);
0161
0162     if (data[IFLA_RMNET_FLAGS]) {
0163         struct ifla_rmnet_flags *flags;
0164
0165         flags = nla_data(data[IFLA_RMNET_FLAGS]);
0166         data_format &= ~flags->mask;
0167         data_format |= flags->flags & flags->mask;
0168     }
0169
0170     netdev_dbg(dev, "data format [0x%08X]\n", data_format);
0171     port->data_format = data_format;
0172
0173     return 0;
0174
0175 err2:
0176     unregister_netdevice(dev);
0177     rmnet_vnd_dellink(mux_id, port, ep);
0178 err1:
0179     rmnet_unregister_real_device(real_dev);
0180 err0:
0181     kfree(ep);
0182     return err;
0183 }
0184
0185 static void rmnet_dellink(struct net_device *dev, struct list_head *head)
0186 {
0187     struct rmnet_priv *priv = netdev_priv(dev);
0188     struct net_device *real_dev, *bridge_dev;
0189     struct rmnet_port *real_port, *bridge_port;
0190     struct rmnet_endpoint *ep;
0191     u8 mux_id = priv->mux_id;
0192
0193     real_dev = priv->real_dev;
0194
0195     if (!rmnet_is_real_dev_registered(real_dev))
0196         return;
0197
0198     real_port = rmnet_get_port_rtnl(real_dev);
0199     bridge_dev = real_port->bridge_ep;
0200     if (bridge_dev) {
0201         bridge_port = rmnet_get_port_rtnl(bridge_dev);
0202         rmnet_unregister_bridge(bridge_port);
0203     }
0204
0205     ep = rmnet_get_endpoint(real_port, mux_id);
0206     if (ep) {
0207         hlist_del_init_rcu(&ep->hlnode);
0208         rmnet_vnd_dellink(mux_id, real_port, ep);
0209         kfree(ep);
0210     }
0211
0212     netdev_upper_dev_unlink(real_dev, dev);
0213     rmnet_unregister_real_device(real_dev);
0214     unregister_netdevice_queue(dev, head);
0215 }
0216
0217 static void rmnet_force_unassociate_device(struct net_device *real_dev)
0218 {
0219     struct hlist_node *tmp_ep;
0220     struct rmnet_endpoint *ep;
0221     struct rmnet_port *port;
0222     unsigned long bkt_ep;
0223     LIST_HEAD(list);
0224
0225     port = rmnet_get_port_rtnl(real_dev);
0226
0227     if (port->nr_rmnet_devs) {
0228         /* real device */
0229         rmnet_unregister_bridge(port);
0230         hash_for_each_safe(port->muxed_ep, bkt_ep, tmp_ep, ep, hlnode) {
0231             unregister_netdevice_queue(ep->egress_dev, &list);
0232             netdev_upper_dev_unlink(real_dev, ep->egress_dev);
0233             rmnet_vnd_dellink(ep->mux_id, port, ep);
0234             hlist_del_init_rcu(&ep->hlnode);
0235             kfree(ep);
0236         }
0237         rmnet_unregister_real_device(real_dev);
0238         unregister_netdevice_many(&list);
0239     } else {
0240         rmnet_unregister_bridge(port);
0241     }
0242 }
0243
0244 static int rmnet_config_notify_cb(struct notifier_block *nb,
0245                   unsigned long event, void *data)
0246 {
0247     struct net_device *real_dev = netdev_notifier_info_to_dev(data);
0248
0249     if (!rmnet_is_real_dev_registered(real_dev))
0250         return NOTIFY_DONE;
0251
0252     switch (event) {
0253     case NETDEV_UNREGISTER:
0254         netdev_dbg(real_dev, "Kernel unregister\n");
0255         rmnet_force_unassociate_device(real_dev);
0256         break;
0257     case NETDEV_CHANGEMTU:
0258         if (rmnet_vnd_validate_real_dev_mtu(real_dev))
0259             return NOTIFY_BAD;
0260         break;
0261     default:
0262         break;
0263     }
0264
0265     return NOTIFY_DONE;
0266 }
0267
0268 static struct notifier_block rmnet_dev_notifier __read_mostly = {
0269     .notifier_call = rmnet_config_notify_cb,
0270 };
0271
0272 static int rmnet_rtnl_validate(struct nlattr *tb[], struct nlattr *data[],
0273                    struct netlink_ext_ack *extack)
0274 {
0275     u16 mux_id;
0276
0277     if (!data || !data[IFLA_RMNET_MUX_ID]) {
0278         NL_SET_ERR_MSG_MOD(extack, "MUX ID not specified");
0279         return -EINVAL;
0280     }
0281
0282     mux_id = nla_get_u16(data[IFLA_RMNET_MUX_ID]);
0283     if (mux_id > (RMNET_MAX_LOGICAL_EP - 1)) {
0284         NL_SET_ERR_MSG_MOD(extack, "invalid MUX ID");
0285         return -ERANGE;
0286     }
0287
0288     return 0;
0289 }
0290
0291 static int rmnet_changelink(struct net_device *dev, struct nlattr *tb[],
0292                 struct nlattr *data[],
0293                 struct netlink_ext_ack *extack)
0294 {
0295     struct rmnet_priv *priv = netdev_priv(dev);
0296     struct net_device *real_dev;
0297     struct rmnet_port *port;
0298     u16 mux_id;
0299
0300     if (!dev)
0301         return -ENODEV;
0302
0303     real_dev = priv->real_dev;
0304     if (!rmnet_is_real_dev_registered(real_dev))
0305         return -ENODEV;
0306
0307     port = rmnet_get_port_rtnl(real_dev);
0308
0309     if (data[IFLA_RMNET_MUX_ID]) {
0310         mux_id = nla_get_u16(data[IFLA_RMNET_MUX_ID]);
0311
0312         if (mux_id != priv->mux_id) {
0313             struct rmnet_endpoint *ep;
0314
0315             ep = rmnet_get_endpoint(port, priv->mux_id);
0316             if (!ep)
0317                 return -ENODEV;
0318
0319             if (rmnet_get_endpoint(port, mux_id)) {
0320                 NL_SET_ERR_MSG_MOD(extack,
0321                            "MUX ID already exists");
0322                 return -EINVAL;
0323             }
0324
0325             hlist_del_init_rcu(&ep->hlnode);
0326             hlist_add_head_rcu(&ep->hlnode,
0327                        &port->muxed_ep[mux_id]);
0328
0329             ep->mux_id = mux_id;
0330             priv->mux_id = mux_id;
0331         }
0332     }
0333
0334     if (data[IFLA_RMNET_FLAGS]) {
0335         struct ifla_rmnet_flags *flags;
0336         u32 old_data_format;
0337
0338         old_data_format = port->data_format;
0339         flags = nla_data(data[IFLA_RMNET_FLAGS]);
0340         port->data_format &= ~flags->mask;
0341         port->data_format |= flags->flags & flags->mask;
0342
0343         if (rmnet_vnd_update_dev_mtu(port, real_dev)) {
0344             port->data_format = old_data_format;
0345             NL_SET_ERR_MSG_MOD(extack, "Invalid MTU on real dev");
0346             return -EINVAL;
0347         }
0348     }
0349
0350     return 0;
0351 }
0352
0353 static size_t rmnet_get_size(const struct net_device *dev)
0354 {
0355     return
0356         /* IFLA_RMNET_MUX_ID */
0357         nla_total_size(2) +
0358         /* IFLA_RMNET_FLAGS */
0359         nla_total_size(sizeof(struct ifla_rmnet_flags));
0360 }
0361
0362 static int rmnet_fill_info(struct sk_buff *skb, const struct net_device *dev)
0363 {
0364     struct rmnet_priv *priv = netdev_priv(dev);
0365     struct net_device *real_dev;
0366     struct ifla_rmnet_flags f;
0367     struct rmnet_port *port;
0368
0369     real_dev = priv->real_dev;
0370
0371     if (nla_put_u16(skb, IFLA_RMNET_MUX_ID, priv->mux_id))
0372         goto nla_put_failure;
0373
0374     if (rmnet_is_real_dev_registered(real_dev)) {
0375         port = rmnet_get_port_rtnl(real_dev);
0376         f.flags = port->data_format;
0377     } else {
0378         f.flags = 0;
0379     }
0380
0381     f.mask  = ~0;
0382
0383     if (nla_put(skb, IFLA_RMNET_FLAGS, sizeof(f), &f))
0384         goto nla_put_failure;
0385
0386     return 0;
0387
0388 nla_put_failure:
0389     return -EMSGSIZE;
0390 }
0391
0392 struct rtnl_link_ops rmnet_link_ops __read_mostly = {
0393     .kind       = "rmnet",
0394     .maxtype    = __IFLA_RMNET_MAX,
0395     .priv_size  = sizeof(struct rmnet_priv),
0396     .setup      = rmnet_vnd_setup,
0397     .validate   = rmnet_rtnl_validate,
0398     .newlink    = rmnet_newlink,
0399     .dellink    = rmnet_dellink,
0400     .get_size   = rmnet_get_size,
0401     .changelink     = rmnet_changelink,
0402     .policy     = rmnet_policy,
0403     .fill_info  = rmnet_fill_info,
0404 };
0405
0406 struct rmnet_port *rmnet_get_port_rcu(struct net_device *real_dev)
0407 {
0408     if (rmnet_is_real_dev_registered(real_dev))
0409         return rcu_dereference_bh(real_dev->rx_handler_data);
0410     else
0411         return NULL;
0412 }
0413
0414 struct rmnet_endpoint *rmnet_get_endpoint(struct rmnet_port *port, u8 mux_id)
0415 {
0416     struct rmnet_endpoint *ep;
0417
0418     hlist_for_each_entry_rcu(ep, &port->muxed_ep[mux_id], hlnode) {
0419         if (ep->mux_id == mux_id)
0420             return ep;
0421     }
0422
0423     return NULL;
0424 }
0425
0426 int rmnet_add_bridge(struct net_device *rmnet_dev,
0427              struct net_device *slave_dev,
0428              struct netlink_ext_ack *extack)
0429 {
0430     struct rmnet_priv *priv = netdev_priv(rmnet_dev);
0431     struct net_device *real_dev = priv->real_dev;
0432     struct rmnet_port *port, *slave_port;
0433     int err;
0434
0435     port = rmnet_get_port_rtnl(real_dev);
0436
0437     /* If there is more than one rmnet dev attached, its probably being
0438      * used for muxing. Skip the briding in that case
0439      */
0440     if (port->nr_rmnet_devs > 1) {
0441         NL_SET_ERR_MSG_MOD(extack, "more than one rmnet dev attached");
0442         return -EINVAL;
0443     }
0444
0445     if (port->rmnet_mode != RMNET_EPMODE_VND) {
0446         NL_SET_ERR_MSG_MOD(extack, "more than one bridge dev attached");
0447         return -EINVAL;
0448     }
0449
0450     if (rmnet_is_real_dev_registered(slave_dev)) {
0451         NL_SET_ERR_MSG_MOD(extack,
0452                    "slave cannot be another rmnet dev");
0453
0454         return -EBUSY;
0455     }
0456
0457     err = rmnet_register_real_device(slave_dev, extack);
0458     if (err)
0459         return -EBUSY;
0460
0461     err = netdev_master_upper_dev_link(slave_dev, rmnet_dev, NULL, NULL,
0462                        extack);
0463     if (err) {
0464         rmnet_unregister_real_device(slave_dev);
0465         return err;
0466     }
0467
0468     slave_port = rmnet_get_port_rtnl(slave_dev);
0469     slave_port->rmnet_mode = RMNET_EPMODE_BRIDGE;
0470     slave_port->bridge_ep = real_dev;
0471     slave_port->rmnet_dev = rmnet_dev;
0472
0473     port->rmnet_mode = RMNET_EPMODE_BRIDGE;
0474     port->bridge_ep = slave_dev;
0475
0476     netdev_dbg(slave_dev, "registered with rmnet as slave\n");
0477     return 0;
0478 }
0479
0480 int rmnet_del_bridge(struct net_device *rmnet_dev,
0481              struct net_device *slave_dev)
0482 {
0483     struct rmnet_port *port = rmnet_get_port_rtnl(slave_dev);
0484
0485     rmnet_unregister_bridge(port);
0486
0487     netdev_dbg(slave_dev, "removed from rmnet as slave\n");
0488     return 0;
0489 }
0490
0491 /* Startup/Shutdown */
0492
0493 static int __init rmnet_init(void)
0494 {
0495     int rc;
0496
0497     rc = register_netdevice_notifier(&rmnet_dev_notifier);
0498     if (rc != 0)
0499         return rc;
0500
0501     rc = rtnl_link_register(&rmnet_link_ops);
0502     if (rc != 0) {
0503         unregister_netdevice_notifier(&rmnet_dev_notifier);
0504         return rc;
0505     }
0506     return rc;
0507 }
0508
0509 static void __exit rmnet_exit(void)
0510 {
0511     rtnl_link_unregister(&rmnet_link_ops);
0512     unregister_netdevice_notifier(&rmnet_dev_notifier);
0513 }
0514
0515 module_init(rmnet_init)
0516 module_exit(rmnet_exit)
0517 MODULE_ALIAS_RTNL_LINK("rmnet");
0518 MODULE_LICENSE("GPL v2");