[mirror_ubuntu-hirsute-kernel.git] / drivers / net / ethernet / qualcomm / rmnet / rmnet_config.c

/* Copyright (c) 2013-2017, The Linux Foundation. All rights reserved.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 and
 * only version 2 as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * RMNET configuration engine
 *
 */

#include <net/sock.h>
#include <linux/module.h>
#include <linux/netlink.h>
#include <linux/netdevice.h>
#include "rmnet_config.h"
#include "rmnet_handlers.h"
#include "rmnet_vnd.h"
#include "rmnet_private.h"

/* Locking scheme -
 * The shared resource which needs to be protected is realdev->rx_handler_data.
 * For the writer path, this is using rtnl_lock(). The writer paths are
 * rmnet_newlink(), rmnet_dellink() and rmnet_force_unassociate_device(). These
 * paths are already called with rtnl_lock() acquired in. There is also an
 * ASSERT_RTNL() to ensure that we are calling with rtnl acquired. For
 * dereference here, we will need to use rtnl_dereference(). Dev list writing
 * needs to happen with rtnl_lock() acquired for netdev_master_upper_dev_link().
 * For the reader path, the real_dev->rx_handler_data is called in the TX / RX
 * path. We only need rcu_read_lock() for these scenarios. In these cases,
 * the rcu_read_lock() is held in __dev_queue_xmit() and
 * netif_receive_skb_internal(), so readers need to use rcu_dereference_rtnl()
 * to get the relevant information. For dev list reading, we again acquire
 * rcu_read_lock() in rmnet_dellink() for netdev_master_upper_dev_get_rcu().
 * We also use unregister_netdevice_many() to free all rmnet devices in
 * rmnet_force_unassociate_device() so we dont lose the rtnl_lock() and free in
 * same context.
 */

/* Local Definitions and Declarations */

struct rmnet_walk_data {
	struct net_device *real_dev;
	struct list_head *head;
	struct rmnet_port *port;
};

static int rmnet_is_real_dev_registered(const struct net_device *real_dev)
{
	return rcu_access_pointer(real_dev->rx_handler) == rmnet_rx_handler;
}

/* Needs rtnl lock */
static struct rmnet_port*
rmnet_get_port_rtnl(const struct net_device *real_dev)
{
	return rtnl_dereference(real_dev->rx_handler_data);
}

static int rmnet_unregister_real_device(struct net_device *real_dev,
					struct rmnet_port *port)
{
	if (port->nr_rmnet_devs)
		return -EINVAL;

	kfree(port);

	netdev_rx_handler_unregister(real_dev);

	/* release reference on real_dev */
	dev_put(real_dev);

	netdev_dbg(real_dev, "Removed from rmnet\n");
	return 0;
}

static int rmnet_register_real_device(struct net_device *real_dev)
{
	struct rmnet_port *port;
	int rc, entry;

	ASSERT_RTNL();

	if (rmnet_is_real_dev_registered(real_dev))
		return 0;

	port = kzalloc(sizeof(*port), GFP_ATOMIC);
	if (!port)
		return -ENOMEM;

	port->dev = real_dev;
	rc = netdev_rx_handler_register(real_dev, rmnet_rx_handler, port);
	if (rc) {
		kfree(port);
		return -EBUSY;
	}

	/* hold on to real dev for MAP data */
	dev_hold(real_dev);

	for (entry = 0; entry < RMNET_MAX_LOGICAL_EP; entry++)
		INIT_HLIST_HEAD(&port->muxed_ep[entry]);

	netdev_dbg(real_dev, "registered with rmnet\n");
	return 0;
}

static void rmnet_unregister_bridge(struct net_device *dev,
				    struct rmnet_port *port)
{
	struct net_device *rmnet_dev, *bridge_dev;
	struct rmnet_port *bridge_port;

	if (port->rmnet_mode != RMNET_EPMODE_BRIDGE)
		return;

	/* bridge slave handling */
	if (!port->nr_rmnet_devs) {
		rmnet_dev = netdev_master_upper_dev_get_rcu(dev);
		netdev_upper_dev_unlink(dev, rmnet_dev);

		bridge_dev = port->bridge_ep;

		bridge_port = rmnet_get_port_rtnl(bridge_dev);
		bridge_port->bridge_ep = NULL;
		bridge_port->rmnet_mode = RMNET_EPMODE_VND;
	} else {
		bridge_dev = port->bridge_ep;

		bridge_port = rmnet_get_port_rtnl(bridge_dev);
		rmnet_dev = netdev_master_upper_dev_get_rcu(bridge_dev);
		netdev_upper_dev_unlink(bridge_dev, rmnet_dev);

		rmnet_unregister_real_device(bridge_dev, bridge_port);
	}
}

static int rmnet_newlink(struct net *src_net, struct net_device *dev,
			 struct nlattr *tb[], struct nlattr *data[],
			 struct netlink_ext_ack *extack)
{
	int ingress_format = RMNET_INGRESS_FORMAT_DEMUXING |
			     RMNET_INGRESS_FORMAT_DEAGGREGATION |
			     RMNET_INGRESS_FORMAT_MAP;
	int egress_format = RMNET_EGRESS_FORMAT_MUXING |
			    RMNET_EGRESS_FORMAT_MAP;
	struct net_device *real_dev;
	int mode = RMNET_EPMODE_VND;
	struct rmnet_endpoint *ep;
	struct rmnet_port *port;
	int err = 0;
	u16 mux_id;

	real_dev = __dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
	if (!real_dev || !dev)
		return -ENODEV;

	if (!data[IFLA_VLAN_ID])
		return -EINVAL;

	ep = kzalloc(sizeof(*ep), GFP_ATOMIC);
	if (!ep)
		return -ENOMEM;

	mux_id = nla_get_u16(data[IFLA_VLAN_ID]);

	err = rmnet_register_real_device(real_dev);
	if (err)
		goto err0;

	port = rmnet_get_port_rtnl(real_dev);
	err = rmnet_vnd_newlink(mux_id, dev, port, real_dev, ep);
	if (err)
		goto err1;

	err = netdev_master_upper_dev_link(dev, real_dev, NULL, NULL, extack);
	if (err)
		goto err2;

	netdev_dbg(dev, "data format [ingress 0x%08X] [egress 0x%08X]\n",
		   ingress_format, egress_format);
	port->egress_data_format = egress_format;
	port->ingress_data_format = ingress_format;
	port->rmnet_mode = mode;

	hlist_add_head_rcu(&ep->hlnode, &port->muxed_ep[mux_id]);
	return 0;

err2:
	rmnet_vnd_dellink(mux_id, port, ep);
err1:
	rmnet_unregister_real_device(real_dev, port);
err0:
	kfree(ep);
	return err;
}

static void rmnet_dellink(struct net_device *dev, struct list_head *head)
{
	struct net_device *real_dev;
	struct rmnet_endpoint *ep;
	struct rmnet_port *port;
	u8 mux_id;

	rcu_read_lock();
	real_dev = netdev_master_upper_dev_get_rcu(dev);
	rcu_read_unlock();

	if (!real_dev || !rmnet_is_real_dev_registered(real_dev))
		return;

	port = rmnet_get_port_rtnl(real_dev);

	mux_id = rmnet_vnd_get_mux(dev);
	netdev_upper_dev_unlink(dev, real_dev);

	ep = rmnet_get_endpoint(port, mux_id);
	if (ep) {
		hlist_del_init_rcu(&ep->hlnode);
		rmnet_unregister_bridge(dev, port);
		rmnet_vnd_dellink(mux_id, port, ep);
		kfree(ep);
	}
	rmnet_unregister_real_device(real_dev, port);

	unregister_netdevice_queue(dev, head);
}

static int rmnet_dev_walk_unreg(struct net_device *rmnet_dev, void *data)
{
	struct rmnet_walk_data *d = data;
	struct rmnet_endpoint *ep;
	u8 mux_id;

	mux_id = rmnet_vnd_get_mux(rmnet_dev);
	ep = rmnet_get_endpoint(d->port, mux_id);
	if (ep) {
		hlist_del_init_rcu(&ep->hlnode);
		rmnet_vnd_dellink(mux_id, d->port, ep);
		kfree(ep);
	}
	netdev_upper_dev_unlink(rmnet_dev, d->real_dev);
	unregister_netdevice_queue(rmnet_dev, d->head);

	return 0;
}

static void rmnet_force_unassociate_device(struct net_device *dev)
{
	struct net_device *real_dev = dev;
	struct rmnet_walk_data d;
	struct rmnet_port *port;
	LIST_HEAD(list);

	if (!rmnet_is_real_dev_registered(real_dev))
		return;

	ASSERT_RTNL();

	d.real_dev = real_dev;
	d.head = &list;

	port = rmnet_get_port_rtnl(dev);
	d.port = port;

	rcu_read_lock();
	rmnet_unregister_bridge(dev, port);

	netdev_walk_all_lower_dev_rcu(real_dev, rmnet_dev_walk_unreg, &d);
	rcu_read_unlock();
	unregister_netdevice_many(&list);

	rmnet_unregister_real_device(real_dev, port);
}

static int rmnet_config_notify_cb(struct notifier_block *nb,
				  unsigned long event, void *data)
{
	struct net_device *dev = netdev_notifier_info_to_dev(data);

	if (!dev)
		return NOTIFY_DONE;

	switch (event) {
	case NETDEV_UNREGISTER:
		netdev_dbg(dev, "Kernel unregister\n");
		rmnet_force_unassociate_device(dev);
		break;

	default:
		break;
	}

	return NOTIFY_DONE;
}

static struct notifier_block rmnet_dev_notifier __read_mostly = {
	.notifier_call = rmnet_config_notify_cb,
};

static int rmnet_rtnl_validate(struct nlattr *tb[], struct nlattr *data[],
			       struct netlink_ext_ack *extack)
{
	u16 mux_id;

	if (!data || !data[IFLA_VLAN_ID])
		return -EINVAL;

	mux_id = nla_get_u16(data[IFLA_VLAN_ID]);
	if (mux_id > (RMNET_MAX_LOGICAL_EP - 1))
		return -ERANGE;

	return 0;
}

static size_t rmnet_get_size(const struct net_device *dev)
{
	return nla_total_size(2); /* IFLA_VLAN_ID */
}

struct rtnl_link_ops rmnet_link_ops __read_mostly = {
	.kind		= "rmnet",
	.maxtype	= __IFLA_VLAN_MAX,
	.priv_size	= sizeof(struct rmnet_priv),
	.setup		= rmnet_vnd_setup,
	.validate	= rmnet_rtnl_validate,
	.newlink	= rmnet_newlink,
	.dellink	= rmnet_dellink,
	.get_size	= rmnet_get_size,
};

/* Needs either rcu_read_lock() or rtnl lock */
struct rmnet_port *rmnet_get_port(struct net_device *real_dev)
{
	if (rmnet_is_real_dev_registered(real_dev))
		return rcu_dereference_rtnl(real_dev->rx_handler_data);
	else
		return NULL;
}

struct rmnet_endpoint *rmnet_get_endpoint(struct rmnet_port *port, u8 mux_id)
{
	struct rmnet_endpoint *ep;

	hlist_for_each_entry_rcu(ep, &port->muxed_ep[mux_id], hlnode) {
		if (ep->mux_id == mux_id)
			return ep;
	}

	return NULL;
}

int rmnet_add_bridge(struct net_device *rmnet_dev,
		     struct net_device *slave_dev,
		     struct netlink_ext_ack *extack)
{
	struct rmnet_priv *priv = netdev_priv(rmnet_dev);
	struct net_device *real_dev = priv->real_dev;
	struct rmnet_port *port, *slave_port;
	int err;

	port = rmnet_get_port(real_dev);

	/* If there is more than one rmnet dev attached, its probably being
	 * used for muxing. Skip the briding in that case
	 */
	if (port->nr_rmnet_devs > 1)
		return -EINVAL;

	if (rmnet_is_real_dev_registered(slave_dev))
		return -EBUSY;

	err = rmnet_register_real_device(slave_dev);
	if (err)
		return -EBUSY;

	err = netdev_master_upper_dev_link(slave_dev, rmnet_dev, NULL, NULL,
					   extack);
	if (err)
		return -EINVAL;

	slave_port = rmnet_get_port(slave_dev);
	slave_port->rmnet_mode = RMNET_EPMODE_BRIDGE;
	slave_port->bridge_ep = real_dev;

	port->rmnet_mode = RMNET_EPMODE_BRIDGE;
	port->bridge_ep = slave_dev;

	netdev_dbg(slave_dev, "registered with rmnet as slave\n");
	return 0;
}

int rmnet_del_bridge(struct net_device *rmnet_dev,
		     struct net_device *slave_dev)
{
	struct rmnet_priv *priv = netdev_priv(rmnet_dev);
	struct net_device *real_dev = priv->real_dev;
	struct rmnet_port *port, *slave_port;

	port = rmnet_get_port(real_dev);
	port->rmnet_mode = RMNET_EPMODE_VND;
	port->bridge_ep = NULL;

	netdev_upper_dev_unlink(slave_dev, rmnet_dev);
	slave_port = rmnet_get_port(slave_dev);
	rmnet_unregister_real_device(slave_dev, slave_port);

	netdev_dbg(slave_dev, "removed from rmnet as slave\n");
	return 0;
}

/* Startup/Shutdown */

static int __init rmnet_init(void)
{
	int rc;

	rc = register_netdevice_notifier(&rmnet_dev_notifier);
	if (rc != 0)
		return rc;

	rc = rtnl_link_register(&rmnet_link_ops);
	if (rc != 0) {
		unregister_netdevice_notifier(&rmnet_dev_notifier);
		return rc;
	}
	return rc;
}

static void __exit rmnet_exit(void)
{
	unregister_netdevice_notifier(&rmnet_dev_notifier);
	rtnl_link_unregister(&rmnet_link_ops);
}

module_init(rmnet_init)
module_exit(rmnet_exit)
MODULE_LICENSE("GPL v2");
Commit	Line	Data
ceed73a2 SAK	1	/* Copyright (c) 2013-2017, The Linux Foundation. All rights reserved.
	2	*
	3	* This program is free software; you can redistribute it and/or modify
	4	* it under the terms of the GNU General Public License version 2 and
	5	* only version 2 as published by the Free Software Foundation.
	6	*
	7	* This program is distributed in the hope that it will be useful,
	8	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	9	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	10	* GNU General Public License for more details.
	11	*
	12	* RMNET configuration engine
	13	*
	14	*/
	15
	16	#include <net/sock.h>
	17	#include <linux/module.h>
	18	#include <linux/netlink.h>
	19	#include <linux/netdevice.h>
	20	#include "rmnet_config.h"
	21	#include "rmnet_handlers.h"
	22	#include "rmnet_vnd.h"
	23	#include "rmnet_private.h"
	24
	25	/* Locking scheme -
	26	* The shared resource which needs to be protected is realdev->rx_handler_data.
	27	* For the writer path, this is using rtnl_lock(). The writer paths are
	28	* rmnet_newlink(), rmnet_dellink() and rmnet_force_unassociate_device(). These
	29	* paths are already called with rtnl_lock() acquired in. There is also an
	30	* ASSERT_RTNL() to ensure that we are calling with rtnl acquired. For
	31	* dereference here, we will need to use rtnl_dereference(). Dev list writing
	32	* needs to happen with rtnl_lock() acquired for netdev_master_upper_dev_link().
	33	* For the reader path, the real_dev->rx_handler_data is called in the TX / RX
	34	* path. We only need rcu_read_lock() for these scenarios. In these cases,
	35	* the rcu_read_lock() is held in __dev_queue_xmit() and
	36	* netif_receive_skb_internal(), so readers need to use rcu_dereference_rtnl()
	37	* to get the relevant information. For dev list reading, we again acquire
	38	* rcu_read_lock() in rmnet_dellink() for netdev_master_upper_dev_get_rcu().
	39	* We also use unregister_netdevice_many() to free all rmnet devices in
	40	* rmnet_force_unassociate_device() so we dont lose the rtnl_lock() and free in
	41	* same context.
	42	*/
	43
	44	/* Local Definitions and Declarations */
ceed73a2 SAK	45
	46	struct rmnet_walk_data {
	47	struct net_device *real_dev;
	48	struct list_head *head;
b665f4f8	49	struct rmnet_port *port;
ceed73a2 SAK	50	};
	51
	52	static int rmnet_is_real_dev_registered(const struct net_device *real_dev)
	53	{
5c346525	54	return rcu_access_pointer(real_dev->rx_handler) == rmnet_rx_handler;
ceed73a2 SAK	55	}
ceed73a2 SAK	56
ceed73a2	57	/* Needs rtnl lock */
b665f4f8 SAK	58	static struct rmnet_port*
b665f4f8 SAK	59	rmnet_get_port_rtnl(const struct net_device *real_dev)
ceed73a2 SAK	60	{
	61	return rtnl_dereference(real_dev->rx_handler_data);
	62	}
	63
ceed73a2	64	static int rmnet_unregister_real_device(struct net_device *real_dev,
b665f4f8	65	struct rmnet_port *port)
ceed73a2	66	{
b665f4f8	67	if (port->nr_rmnet_devs)
ceed73a2 SAK	68	return -EINVAL;
ceed73a2 SAK	69
b665f4f8	70	kfree(port);
ceed73a2 SAK	71
	72	netdev_rx_handler_unregister(real_dev);
	73
	74	/* release reference on real_dev */
	75	dev_put(real_dev);
	76
	77	netdev_dbg(real_dev, "Removed from rmnet\n");
	78	return 0;
	79	}
	80
	81	static int rmnet_register_real_device(struct net_device *real_dev)
	82	{
b665f4f8	83	struct rmnet_port *port;
3352e6c4	84	int rc, entry;
ceed73a2 SAK	85
	86	ASSERT_RTNL();
	87
	88	if (rmnet_is_real_dev_registered(real_dev))
	89	return 0;
	90
b665f4f8 SAK	91	port = kzalloc(sizeof(*port), GFP_ATOMIC);
b665f4f8 SAK	92	if (!port)
ceed73a2 SAK	93	return -ENOMEM;
ceed73a2 SAK	94
b665f4f8 SAK	95	port->dev = real_dev;
b665f4f8 SAK	96	rc = netdev_rx_handler_register(real_dev, rmnet_rx_handler, port);
ceed73a2	97	if (rc) {
b665f4f8	98	kfree(port);
ceed73a2 SAK	99	return -EBUSY;
	100	}
	101
	102	/* hold on to real dev for MAP data */
	103	dev_hold(real_dev);
	104
3352e6c4 SAK	105	for (entry = 0; entry < RMNET_MAX_LOGICAL_EP; entry++)
	106	INIT_HLIST_HEAD(&port->muxed_ep[entry]);
	107
ceed73a2 SAK	108	netdev_dbg(real_dev, "registered with rmnet\n");
	109	return 0;
	110	}
	111
60d58f97 SAK	112	static void rmnet_unregister_bridge(struct net_device *dev,
	113	struct rmnet_port *port)
	114	{
	115	struct net_device rmnet_dev, bridge_dev;
	116	struct rmnet_port *bridge_port;
	117
	118	if (port->rmnet_mode != RMNET_EPMODE_BRIDGE)
	119	return;
	120
	121	/* bridge slave handling */
	122	if (!port->nr_rmnet_devs) {
	123	rmnet_dev = netdev_master_upper_dev_get_rcu(dev);
	124	netdev_upper_dev_unlink(dev, rmnet_dev);
	125
	126	bridge_dev = port->bridge_ep;
	127
	128	bridge_port = rmnet_get_port_rtnl(bridge_dev);
	129	bridge_port->bridge_ep = NULL;
	130	bridge_port->rmnet_mode = RMNET_EPMODE_VND;
	131	} else {
	132	bridge_dev = port->bridge_ep;
	133
	134	bridge_port = rmnet_get_port_rtnl(bridge_dev);
	135	rmnet_dev = netdev_master_upper_dev_get_rcu(bridge_dev);
	136	netdev_upper_dev_unlink(bridge_dev, rmnet_dev);
	137
	138	rmnet_unregister_real_device(bridge_dev, bridge_port);
	139	}
	140	}
	141
ceed73a2 SAK	142	static int rmnet_newlink(struct net src_net, struct net_device dev,
	143	struct nlattr tb[], struct nlattr data[],
	144	struct netlink_ext_ack *extack)
	145	{
	146	int ingress_format = RMNET_INGRESS_FORMAT_DEMUXING \|
	147	RMNET_INGRESS_FORMAT_DEAGGREGATION \|
	148	RMNET_INGRESS_FORMAT_MAP;
	149	int egress_format = RMNET_EGRESS_FORMAT_MUXING \|
	150	RMNET_EGRESS_FORMAT_MAP;
ceed73a2 SAK	151	struct net_device *real_dev;
ceed73a2 SAK	152	int mode = RMNET_EPMODE_VND;
3352e6c4	153	struct rmnet_endpoint *ep;
b665f4f8	154	struct rmnet_port *port;
ceed73a2 SAK	155	int err = 0;
	156	u16 mux_id;
	157
	158	real_dev = __dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
	159	if (!real_dev \|\| !dev)
	160	return -ENODEV;
	161
	162	if (!data[IFLA_VLAN_ID])
	163	return -EINVAL;
	164
3352e6c4 SAK	165	ep = kzalloc(sizeof(*ep), GFP_ATOMIC);
	166	if (!ep)
	167	return -ENOMEM;
	168
ceed73a2 SAK	169	mux_id = nla_get_u16(data[IFLA_VLAN_ID]);
	170
	171	err = rmnet_register_real_device(real_dev);
	172	if (err)
	173	goto err0;
	174
b665f4f8	175	port = rmnet_get_port_rtnl(real_dev);
3352e6c4	176	err = rmnet_vnd_newlink(mux_id, dev, port, real_dev, ep);
ceed73a2 SAK	177	if (err)
	178	goto err1;
	179
42ab19ee	180	err = netdev_master_upper_dev_link(dev, real_dev, NULL, NULL, extack);
ceed73a2 SAK	181	if (err)
	182	goto err2;
	183
032ee468 SAK	184	netdev_dbg(dev, "data format [ingress 0x%08X] [egress 0x%08X]\n",
032ee468 SAK	185	ingress_format, egress_format);
b665f4f8 SAK	186	port->egress_data_format = egress_format;
b665f4f8 SAK	187	port->ingress_data_format = ingress_format;
91489632	188	port->rmnet_mode = mode;
032ee468	189
3352e6c4	190	hlist_add_head_rcu(&ep->hlnode, &port->muxed_ep[mux_id]);
ceed73a2 SAK	191	return 0;
	192
	193	err2:
3352e6c4	194	rmnet_vnd_dellink(mux_id, port, ep);
ceed73a2	195	err1:
b665f4f8	196	rmnet_unregister_real_device(real_dev, port);
ceed73a2	197	err0:
6296928f	198	kfree(ep);
ceed73a2 SAK	199	return err;
	200	}
	201
	202	static void rmnet_dellink(struct net_device dev, struct list_head head)
	203	{
ceed73a2	204	struct net_device *real_dev;
3352e6c4	205	struct rmnet_endpoint *ep;
b665f4f8	206	struct rmnet_port *port;
ceed73a2 SAK	207	u8 mux_id;
	208
	209	rcu_read_lock();
	210	real_dev = netdev_master_upper_dev_get_rcu(dev);
	211	rcu_read_unlock();
	212
	213	if (!real_dev \|\| !rmnet_is_real_dev_registered(real_dev))
	214	return;
	215
b665f4f8	216	port = rmnet_get_port_rtnl(real_dev);
ceed73a2 SAK	217
ceed73a2 SAK	218	mux_id = rmnet_vnd_get_mux(dev);
ceed73a2	219	netdev_upper_dev_unlink(dev, real_dev);
3352e6c4 SAK	220
	221	ep = rmnet_get_endpoint(port, mux_id);
	222	if (ep) {
	223	hlist_del_init_rcu(&ep->hlnode);
60d58f97	224	rmnet_unregister_bridge(dev, port);
3352e6c4 SAK	225	rmnet_vnd_dellink(mux_id, port, ep);
	226	kfree(ep);
	227	}
b665f4f8	228	rmnet_unregister_real_device(real_dev, port);
ceed73a2 SAK	229
	230	unregister_netdevice_queue(dev, head);
	231	}
	232
	233	static int rmnet_dev_walk_unreg(struct net_device rmnet_dev, void data)
	234	{
	235	struct rmnet_walk_data *d = data;
3352e6c4	236	struct rmnet_endpoint *ep;
ceed73a2 SAK	237	u8 mux_id;
	238
	239	mux_id = rmnet_vnd_get_mux(rmnet_dev);
3352e6c4 SAK	240	ep = rmnet_get_endpoint(d->port, mux_id);
	241	if (ep) {
	242	hlist_del_init_rcu(&ep->hlnode);
	243	rmnet_vnd_dellink(mux_id, d->port, ep);
	244	kfree(ep);
	245	}
ceed73a2 SAK	246	netdev_upper_dev_unlink(rmnet_dev, d->real_dev);
	247	unregister_netdevice_queue(rmnet_dev, d->head);
	248
	249	return 0;
	250	}
	251
	252	static void rmnet_force_unassociate_device(struct net_device *dev)
	253	{
	254	struct net_device *real_dev = dev;
ceed73a2	255	struct rmnet_walk_data d;
b665f4f8	256	struct rmnet_port *port;
ceed73a2 SAK	257	LIST_HEAD(list);
	258
	259	if (!rmnet_is_real_dev_registered(real_dev))
	260	return;
	261
	262	ASSERT_RTNL();
	263
	264	d.real_dev = real_dev;
	265	d.head = &list;
	266
b665f4f8 SAK	267	port = rmnet_get_port_rtnl(dev);
b665f4f8 SAK	268	d.port = port;
ceed73a2 SAK	269
ceed73a2 SAK	270	rcu_read_lock();
60d58f97 SAK	271	rmnet_unregister_bridge(dev, port);
60d58f97 SAK	272
ceed73a2 SAK	273	netdev_walk_all_lower_dev_rcu(real_dev, rmnet_dev_walk_unreg, &d);
	274	rcu_read_unlock();
	275	unregister_netdevice_many(&list);
	276
b665f4f8	277	rmnet_unregister_real_device(real_dev, port);
ceed73a2 SAK	278	}
	279
	280	static int rmnet_config_notify_cb(struct notifier_block *nb,
	281	unsigned long event, void *data)
	282	{
	283	struct net_device *dev = netdev_notifier_info_to_dev(data);
	284
	285	if (!dev)
	286	return NOTIFY_DONE;
	287
	288	switch (event) {
	289	case NETDEV_UNREGISTER:
	290	netdev_dbg(dev, "Kernel unregister\n");
	291	rmnet_force_unassociate_device(dev);
	292	break;
	293
	294	default:
	295	break;
	296	}
	297
	298	return NOTIFY_DONE;
	299	}
	300
	301	static struct notifier_block rmnet_dev_notifier __read_mostly = {
	302	.notifier_call = rmnet_config_notify_cb,
	303	};
	304
	305	static int rmnet_rtnl_validate(struct nlattr tb[], struct nlattr data[],
	306	struct netlink_ext_ack *extack)
	307	{
	308	u16 mux_id;
	309
	310	if (!data \|\| !data[IFLA_VLAN_ID])
	311	return -EINVAL;
	312
	313	mux_id = nla_get_u16(data[IFLA_VLAN_ID]);
	314	if (mux_id > (RMNET_MAX_LOGICAL_EP - 1))
	315	return -ERANGE;
	316
	317	return 0;
	318	}
	319
	320	static size_t rmnet_get_size(const struct net_device *dev)
	321	{
	322	return nla_total_size(2); /* IFLA_VLAN_ID */
	323	}
	324
	325	struct rtnl_link_ops rmnet_link_ops __read_mostly = {
	326	.kind = "rmnet",
	327	.maxtype = __IFLA_VLAN_MAX,
	328	.priv_size = sizeof(struct rmnet_priv),
	329	.setup = rmnet_vnd_setup,
	330	.validate = rmnet_rtnl_validate,
	331	.newlink = rmnet_newlink,
	332	.dellink = rmnet_dellink,
	333	.get_size = rmnet_get_size,
	334	};
	335
032ee468	336	/* Needs either rcu_read_lock() or rtnl lock */
b665f4f8	337	struct rmnet_port rmnet_get_port(struct net_device real_dev)
ceed73a2	338	{
032ee468 SAK	339	if (rmnet_is_real_dev_registered(real_dev))
	340	return rcu_dereference_rtnl(real_dev->rx_handler_data);
	341	else
	342	return NULL;
ceed73a2 SAK	343	}
ceed73a2 SAK	344
3352e6c4 SAK	345	struct rmnet_endpoint rmnet_get_endpoint(struct rmnet_port port, u8 mux_id)
	346	{
	347	struct rmnet_endpoint *ep;
	348
	349	hlist_for_each_entry_rcu(ep, &port->muxed_ep[mux_id], hlnode) {
	350	if (ep->mux_id == mux_id)
	351	return ep;
	352	}
	353
	354	return NULL;
	355	}
	356
60d58f97 SAK	357	int rmnet_add_bridge(struct net_device *rmnet_dev,
	358	struct net_device *slave_dev,
	359	struct netlink_ext_ack *extack)
	360	{
	361	struct rmnet_priv *priv = netdev_priv(rmnet_dev);
	362	struct net_device *real_dev = priv->real_dev;
	363	struct rmnet_port port, slave_port;
	364	int err;
	365
	366	port = rmnet_get_port(real_dev);
	367
	368	/* If there is more than one rmnet dev attached, its probably being
	369	* used for muxing. Skip the briding in that case
	370	*/
	371	if (port->nr_rmnet_devs > 1)
	372	return -EINVAL;
	373
	374	if (rmnet_is_real_dev_registered(slave_dev))
	375	return -EBUSY;
	376
	377	err = rmnet_register_real_device(slave_dev);
	378	if (err)
	379	return -EBUSY;
	380
	381	err = netdev_master_upper_dev_link(slave_dev, rmnet_dev, NULL, NULL,
	382	extack);
	383	if (err)
	384	return -EINVAL;
	385
	386	slave_port = rmnet_get_port(slave_dev);
	387	slave_port->rmnet_mode = RMNET_EPMODE_BRIDGE;
	388	slave_port->bridge_ep = real_dev;
	389
	390	port->rmnet_mode = RMNET_EPMODE_BRIDGE;
	391	port->bridge_ep = slave_dev;
	392
	393	netdev_dbg(slave_dev, "registered with rmnet as slave\n");
	394	return 0;
	395	}
	396
	397	int rmnet_del_bridge(struct net_device *rmnet_dev,
	398	struct net_device *slave_dev)
	399	{
	400	struct rmnet_priv *priv = netdev_priv(rmnet_dev);
	401	struct net_device *real_dev = priv->real_dev;
	402	struct rmnet_port port, slave_port;
	403
	404	port = rmnet_get_port(real_dev);
	405	port->rmnet_mode = RMNET_EPMODE_VND;
	406	port->bridge_ep = NULL;
	407
	408	netdev_upper_dev_unlink(slave_dev, rmnet_dev);
	409	slave_port = rmnet_get_port(slave_dev);
	410	rmnet_unregister_real_device(slave_dev, slave_port);
	411
	412	netdev_dbg(slave_dev, "removed from rmnet as slave\n");
	413	return 0;
	414	}
	415
ceed73a2 SAK	416	/* Startup/Shutdown */
	417
	418	static int __init rmnet_init(void)
	419	{
	420	int rc;
	421
	422	rc = register_netdevice_notifier(&rmnet_dev_notifier);
	423	if (rc != 0)
	424	return rc;
	425
	426	rc = rtnl_link_register(&rmnet_link_ops);
	427	if (rc != 0) {
	428	unregister_netdevice_notifier(&rmnet_dev_notifier);
	429	return rc;
	430	}
	431	return rc;
	432	}
	433
	434	static void __exit rmnet_exit(void)
	435	{
	436	unregister_netdevice_notifier(&rmnet_dev_notifier);
	437	rtnl_link_unregister(&rmnet_link_ops);
	438	}
	439
	440	module_init(rmnet_init)
	441	module_exit(rmnet_exit)
	442	MODULE_LICENSE("GPL v2");