rmnet_config.c

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