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(dev: real_dev); |
46 | |
47 | kfree(objp: 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(size: sizeof(*port), GFP_KERNEL); |
72 | if (!port) |
73 | return -ENOMEM; |
74 | |
75 | port->dev = real_dev; |
76 | rc = netdev_rx_handler_register(dev: real_dev, rx_handler: rmnet_rx_handler, rx_handler_data: port); |
77 | if (rc) { |
78 | kfree(objp: 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(dev: bridge_dev, upper_dev: rmnet_dev); |
117 | rmnet_unregister_real_device(real_dev: 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(net: src_net, ifindex: nla_get_u32(nla: 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(size: sizeof(*ep), GFP_KERNEL); |
144 | if (!ep) |
145 | return -ENOMEM; |
146 | |
147 | mux_id = nla_get_u16(nla: 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(id: mux_id, rmnet_dev: dev, port, real_dev, ep, extack); |
155 | if (err) |
156 | goto err1; |
157 | |
158 | err = netdev_upper_dev_link(dev: real_dev, upper_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(n: &ep->hlnode, h: &port->muxed_ep[mux_id]); |
166 | |
167 | if (data[IFLA_RMNET_FLAGS]) { |
168 | struct ifla_rmnet_flags *flags; |
169 | |
170 | flags = nla_data(nla: 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(id: mux_id, port, ep); |
183 | err1: |
184 | rmnet_unregister_real_device(real_dev); |
185 | err0: |
186 | kfree(objp: 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(real_dev: bridge_dev); |
207 | rmnet_unregister_bridge(port: bridge_port); |
208 | } |
209 | |
210 | ep = rmnet_get_endpoint(port: real_port, mux_id); |
211 | if (ep) { |
212 | hlist_del_init_rcu(n: &ep->hlnode); |
213 | rmnet_vnd_dellink(id: mux_id, port: real_port, ep); |
214 | kfree(objp: ep); |
215 | } |
216 | |
217 | netdev_upper_dev_unlink(dev: real_dev, upper_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(dev: ep->egress_dev, head: &list); |
237 | netdev_upper_dev_unlink(dev: real_dev, upper_dev: ep->egress_dev); |
238 | rmnet_vnd_dellink(id: ep->mux_id, port, ep); |
239 | hlist_del_init_rcu(n: &ep->hlnode); |
240 | kfree(objp: ep); |
241 | } |
242 | rmnet_unregister_real_device(real_dev); |
243 | unregister_netdevice_many(head: &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(info: 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(nla: 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(nla: 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, mux_id: 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(n: &ep->hlnode); |
331 | hlist_add_head_rcu(n: &ep->hlnode, |
332 | h: &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(nla: 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(payload: 2) + |
363 | /* IFLA_RMNET_FLAGS */ |
364 | nla_total_size(payload: 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, attrtype: IFLA_RMNET_MUX_ID, value: 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, attrtype: IFLA_RMNET_FLAGS, attrlen: sizeof(f), data: &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(dev: 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(real_dev: 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(real_dev: slave_dev, extack); |
463 | if (err) |
464 | return -EBUSY; |
465 | |
466 | err = netdev_master_upper_dev_link(dev: slave_dev, upper_dev: rmnet_dev, NULL, NULL, |
467 | extack); |
468 | if (err) { |
469 | rmnet_unregister_real_device(real_dev: slave_dev); |
470 | return err; |
471 | } |
472 | |
473 | slave_port = rmnet_get_port_rtnl(real_dev: 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(real_dev: 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(nb: &rmnet_dev_notifier); |
503 | if (rc != 0) |
504 | return rc; |
505 | |
506 | rc = rtnl_link_register(ops: &rmnet_link_ops); |
507 | if (rc != 0) { |
508 | unregister_netdevice_notifier(nb: &rmnet_dev_notifier); |
509 | return rc; |
510 | } |
511 | return rc; |
512 | } |
513 | |
514 | static void __exit rmnet_exit(void) |
515 | { |
516 | rtnl_link_unregister(ops: &rmnet_link_ops); |
517 | unregister_netdevice_notifier(nb: &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 | |