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