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