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