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