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