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
19static 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
24static 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 */
30struct rmnet_port*
31rmnet_get_port_rtnl(const struct net_device *real_dev)
32{
33	return rtnl_dereference(real_dev->rx_handler_data);
34}
35
36static 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
53static 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
91static 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
120static 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
180err2:
181	unregister_netdevice(dev);
182	rmnet_vnd_dellink(mux_id, port, ep);
183err1:
184	rmnet_unregister_real_device(real_dev);
185err0:
186	kfree(ep);
187	return err;
188}
189
190static 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
222static 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
249static 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
273static struct notifier_block rmnet_dev_notifier __read_mostly = {
274	.notifier_call = rmnet_config_notify_cb,
275};
276
277static 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
296static 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
358static 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
367static 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
393nla_put_failure:
394	return -EMSGSIZE;
395}
396
397struct 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
411struct 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
419struct 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
431int 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
485int 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
498static 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
514static void __exit rmnet_exit(void)
515{
516	rtnl_link_unregister(&rmnet_link_ops);
517	unregister_netdevice_notifier(&rmnet_dev_notifier);
518}
519
520module_init(rmnet_init)
521module_exit(rmnet_exit)
522MODULE_ALIAS_RTNL_LINK("rmnet");
523MODULE_DESCRIPTION("Qualcomm RmNet MAP driver");
524MODULE_LICENSE("GPL v2");
525