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
18static 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
23static 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 */
29struct rmnet_port*
30rmnet_get_port_rtnl(const struct net_device *real_dev)
31{
32	return rtnl_dereference(real_dev->rx_handler_data);
33}
34
35static 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
50static 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
86static 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
115static 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
174err2:
175	unregister_netdevice(dev);
176	rmnet_vnd_dellink(mux_id, port, ep);
177err1:
178	rmnet_unregister_real_device(real_dev);
179err0:
180	kfree(ep);
181	return err;
182}
183
184static 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
216static 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
243static 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
267static struct notifier_block rmnet_dev_notifier __read_mostly = {
268	.notifier_call = rmnet_config_notify_cb,
269};
270
271static 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
290static 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
351static 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
360static 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
386nla_put_failure:
387	return -EMSGSIZE;
388}
389
390struct 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
404struct 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
412struct 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
424int 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
478int 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
491static 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
507static void __exit rmnet_exit(void)
508{
509	rtnl_link_unregister(&rmnet_link_ops);
510	unregister_netdevice_notifier(&rmnet_dev_notifier);
511}
512
513module_init(rmnet_init)
514module_exit(rmnet_exit)
515MODULE_ALIAS_RTNL_LINK("rmnet");
516MODULE_LICENSE("GPL v2");
517