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