[deliverable/linux.git] / net / hsr / hsr_slave.c

/* Copyright 2011-2014 Autronica Fire and Security AS
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
 * Software Foundation; either version 2 of the License, or (at your option)
 * any later version.
 *
 * Author(s):
 *	2011-2014 Arvid Brodin, arvid.brodin@alten.se
 */

#include "hsr_slave.h"
#include <linux/etherdevice.h>
#include <linux/if_arp.h>
#include "hsr_main.h"
#include "hsr_device.h"
#include "hsr_framereg.h"


static int hsr_check_dev_ok(struct net_device *dev)
{
	/* Don't allow HSR on non-ethernet like devices */
	if ((dev->flags & IFF_LOOPBACK) || (dev->type != ARPHRD_ETHER) ||
	    (dev->addr_len != ETH_ALEN)) {
		netdev_info(dev, "Cannot use loopback or non-ethernet device as HSR slave.\n");
		return -EINVAL;
	}

	/* Don't allow enslaving hsr devices */
	if (is_hsr_master(dev)) {
		netdev_info(dev, "Cannot create trees of HSR devices.\n");
		return -EINVAL;
	}

	if (hsr_port_exists(dev)) {
		netdev_info(dev, "This device is already a HSR slave.\n");
		return -EINVAL;
	}

	if (dev->priv_flags & IFF_802_1Q_VLAN) {
		netdev_info(dev, "HSR on top of VLAN is not yet supported in this driver.\n");
		return -EINVAL;
	}

	/* HSR over bonded devices has not been tested, but I'm not sure it
	 * won't work...
	 */

	return 0;
}


static struct sk_buff *hsr_pull_tag(struct sk_buff *skb)
{
	struct hsr_tag *hsr_tag;
	struct sk_buff *skb2;

	skb2 = skb_share_check(skb, GFP_ATOMIC);
	if (unlikely(!skb2))
		goto err_free;
	skb = skb2;

	if (unlikely(!pskb_may_pull(skb, HSR_HLEN)))
		goto err_free;

	hsr_tag = (struct hsr_tag *) skb->data;
	skb->protocol = hsr_tag->encap_proto;
	skb_pull(skb, HSR_HLEN);

	return skb;

err_free:
	kfree_skb(skb);
	return NULL;
}


/* The uses I can see for these HSR supervision frames are:
 * 1) Use the frames that are sent after node initialization ("HSR_TLV.Type =
 *    22") to reset any sequence_nr counters belonging to that node. Useful if
 *    the other node's counter has been reset for some reason.
 *    --
 *    Or not - resetting the counter and bridging the frame would create a
 *    loop, unfortunately.
 *
 * 2) Use the LifeCheck frames to detect ring breaks. I.e. if no LifeCheck
 *    frame is received from a particular node, we know something is wrong.
 *    We just register these (as with normal frames) and throw them away.
 *
 * 3) Allow different MAC addresses for the two slave interfaces, using the
 *    MacAddressA field.
 */
static bool is_supervision_frame(struct hsr_priv *hsr, struct sk_buff *skb)
{
	struct hsr_sup_tag *hsr_stag;

	if (!ether_addr_equal(eth_hdr(skb)->h_dest,
			      hsr->sup_multicast_addr))
		return false;

	hsr_stag = (struct hsr_sup_tag *) skb->data;
	if (get_hsr_stag_path(hsr_stag) != 0x0f)
		return false;
	if ((hsr_stag->HSR_TLV_Type != HSR_TLV_ANNOUNCE) &&
	    (hsr_stag->HSR_TLV_Type != HSR_TLV_LIFE_CHECK))
		return false;
	if (hsr_stag->HSR_TLV_Length != 12)
		return false;

	return true;
}


/* Implementation somewhat according to IEC-62439-3, p. 43
 */
rx_handler_result_t hsr_handle_frame(struct sk_buff **pskb)
{
	struct sk_buff *skb = *pskb;
	struct hsr_port *port, *other_port, *master;
	struct hsr_priv *hsr;
	struct hsr_node *node;
	bool deliver_to_self;
	struct sk_buff *skb_deliver;
	bool dup_out;
	int ret;

	if (eth_hdr(skb)->h_proto != htons(ETH_P_PRP))
		return RX_HANDLER_PASS;

	rcu_read_lock(); /* ports & node */

	port = hsr_port_get_rcu(skb->dev);
	hsr = port->hsr;
	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);

	node = hsr_find_node(&hsr->self_node_db, skb);
	if (node) {
		/* Always kill frames sent by ourselves */
		kfree_skb(skb);
		ret = RX_HANDLER_CONSUMED;
		goto finish;
	}

	/* Is this frame a candidate for local reception? */
	deliver_to_self = false;
	if ((skb->pkt_type == PACKET_HOST) ||
	    (skb->pkt_type == PACKET_MULTICAST) ||
	    (skb->pkt_type == PACKET_BROADCAST))
		deliver_to_self = true;
	else if (ether_addr_equal(eth_hdr(skb)->h_dest,
				  master->dev->dev_addr)) {
		skb->pkt_type = PACKET_HOST;
		deliver_to_self = true;
	}

	node = hsr_find_node(&hsr->node_db, skb);

	if (is_supervision_frame(hsr, skb)) {
		skb_pull(skb, sizeof(struct hsr_sup_tag));
		node = hsr_merge_node(node, skb, port);
		if (!node) {
			kfree_skb(skb);
			master->dev->stats.rx_dropped++;
			ret = RX_HANDLER_CONSUMED;
			goto finish;
		}
		skb_push(skb, sizeof(struct hsr_sup_tag));
		deliver_to_self = false;
	}

	if (!node) {
		/* Source node unknown; this might be a HSR frame from
		 * another net (different multicast address). Ignore it.
		 */
		kfree_skb(skb);
		ret = RX_HANDLER_CONSUMED;
		goto finish;
	}

	if (port->type == HSR_PT_SLAVE_A)
		other_port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_B);
	else
		other_port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_A);

	/* Register ALL incoming frames as outgoing through the other interface.
	 * This allows us to register frames as incoming only if they are valid
	 * for the receiving interface, without using a specific counter for
	 * incoming frames.
	 */
	if (other_port)
		dup_out = hsr_register_frame_out(node, other_port, skb);
	else
		dup_out = 0;
	if (!dup_out)
		hsr_register_frame_in(node, port);

	/* Forward this frame? */
	if (dup_out || (skb->pkt_type == PACKET_HOST))
		other_port = NULL;

	if (hsr_register_frame_out(node, master, skb))
		deliver_to_self = false;

	if (!deliver_to_self && !other_port) {
		kfree_skb(skb);
		/* Circulated frame; silently remove it. */
		ret = RX_HANDLER_CONSUMED;
		goto finish;
	}

	skb_deliver = skb;
	if (deliver_to_self && other_port) {
		/* skb_clone() is not enough since we will strip the hsr tag
		 * and do address substitution below
		 */
		skb_deliver = pskb_copy(skb, GFP_ATOMIC);
		if (!skb_deliver) {
			deliver_to_self = false;
			master->dev->stats.rx_dropped++;
		}
	}

	if (deliver_to_self) {
		bool multicast_frame;

		skb_deliver = hsr_pull_tag(skb_deliver);
		if (!skb_deliver) {
			master->dev->stats.rx_dropped++;
			goto forward;
		}
#if !defined(CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS)
		/* Move everything in the header that is after the HSR tag,
		 * to work around alignment problems caused by the 6-byte HSR
		 * tag. In practice, this removes/overwrites the HSR tag in
		 * the header and restores a "standard" packet.
		 */
		memmove(skb_deliver->data - HSR_HLEN, skb_deliver->data,
			skb_headlen(skb_deliver));

		/* Adjust skb members so they correspond with the move above.
		 * This cannot possibly underflow skb->data since hsr_pull_tag()
		 * above succeeded.
		 * At this point in the protocol stack, the transport and
		 * network headers have not been set yet, and we haven't touched
		 * the mac header nor the head. So we only need to adjust data
		 * and tail:
		 */
		skb_deliver->data -= HSR_HLEN;
		skb_deliver->tail -= HSR_HLEN;
#endif
		skb_deliver->dev = master->dev;
		hsr_addr_subst_source(hsr, skb_deliver);
		multicast_frame = (skb_deliver->pkt_type == PACKET_MULTICAST);
		ret = netif_rx(skb_deliver);
		if (ret == NET_RX_DROP) {
			master->dev->stats.rx_dropped++;
		} else {
			master->dev->stats.rx_packets++;
			master->dev->stats.rx_bytes += skb->len;
			if (multicast_frame)
				master->dev->stats.multicast++;
		}
	}

forward:
	if (other_port) {
		skb_push(skb, ETH_HLEN);
		skb->dev = other_port->dev;
		dev_queue_xmit(skb);
	}

	ret = RX_HANDLER_CONSUMED;

finish:
	rcu_read_unlock();
	return ret;
}

/* Setup device to be added to the HSR bridge. */
static int hsr_portdev_setup(struct net_device *dev, struct hsr_port *port)
{
	int res;

	dev_hold(dev);
	res = dev_set_promiscuity(dev, 1);
	if (res)
		goto fail_promiscuity;
	res = netdev_rx_handler_register(dev, hsr_handle_frame, port);
	if (res)
		goto fail_rx_handler;
	dev_disable_lro(dev);

	/* FIXME:
	 * What does net device "adjacency" mean? Should we do
	 * res = netdev_master_upper_dev_link(port->dev, port->hsr->dev); ?
	 */

	return 0;

fail_rx_handler:
	dev_set_promiscuity(dev, -1);
fail_promiscuity:
	dev_put(dev);

	return res;
}

int hsr_add_port(struct hsr_priv *hsr, struct net_device *dev,
		 enum hsr_port_type type)
{
	struct hsr_port *port, *master;
	int res;

	if (type != HSR_PT_MASTER) {
		res = hsr_check_dev_ok(dev);
		if (res)
			return res;
	}

	port = hsr_port_get_hsr(hsr, type);
	if (port != NULL)
		return -EBUSY;	/* This port already exists */

	port = kzalloc(sizeof(*port), GFP_KERNEL);
	if (port == NULL)
		return -ENOMEM;

	if (type != HSR_PT_MASTER) {
		res = hsr_portdev_setup(dev, port);
		if (res)
			goto fail_dev_setup;
	}

	port->hsr = hsr;
	port->dev = dev;
	port->type = type;

	list_add_tail_rcu(&port->port_list, &hsr->ports);
	synchronize_rcu();

	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);

	/* Set required header length */
	if (dev->hard_header_len + HSR_HLEN > master->dev->hard_header_len)
		master->dev->hard_header_len = dev->hard_header_len + HSR_HLEN;

	dev_set_mtu(master->dev, hsr_get_max_mtu(hsr));

	return 0;

fail_dev_setup:
	kfree(port);
	return res;
}

void hsr_del_port(struct hsr_port *port)
{
	struct hsr_priv *hsr;
	struct hsr_port *master;

	hsr = port->hsr;
	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
	list_del_rcu(&port->port_list);

	if (port != master) {
		dev_set_mtu(master->dev, hsr_get_max_mtu(hsr));
		netdev_rx_handler_unregister(port->dev);
		dev_set_promiscuity(port->dev, -1);
	}

	/* FIXME?
	 * netdev_upper_dev_unlink(port->dev, port->hsr->dev);
	 */

	synchronize_rcu();
	dev_put(port->dev);
}
Commit	Line	Data
81ba6afd AB	1	/* Copyright 2011-2014 Autronica Fire and Security AS
	2	*
	3	* This program is free software; you can redistribute it and/or modify it
	4	* under the terms of the GNU General Public License as published by the Free
	5	* Software Foundation; either version 2 of the License, or (at your option)
	6	* any later version.
	7	*
	8	* Author(s):
	9	* 2011-2014 Arvid Brodin, arvid.brodin@alten.se
	10	*/
	11
	12	#include "hsr_slave.h"
	13	#include <linux/etherdevice.h>
51f3c605	14	#include <linux/if_arp.h>
81ba6afd	15	#include "hsr_main.h"
51f3c605	16	#include "hsr_device.h"
81ba6afd AB	17	#include "hsr_framereg.h"
	18
	19
c5a75911	20	static int hsr_check_dev_ok(struct net_device *dev)
51f3c605 AB	21	{
	22	/* Don't allow HSR on non-ethernet like devices */
	23	if ((dev->flags & IFF_LOOPBACK) \|\| (dev->type != ARPHRD_ETHER) \|\|
	24	(dev->addr_len != ETH_ALEN)) {
	25	netdev_info(dev, "Cannot use loopback or non-ethernet device as HSR slave.\n");
	26	return -EINVAL;
	27	}
	28
	29	/* Don't allow enslaving hsr devices */
	30	if (is_hsr_master(dev)) {
	31	netdev_info(dev, "Cannot create trees of HSR devices.\n");
	32	return -EINVAL;
	33	}
	34
c5a75911	35	if (hsr_port_exists(dev)) {
51f3c605 AB	36	netdev_info(dev, "This device is already a HSR slave.\n");
	37	return -EINVAL;
	38	}
	39
	40	if (dev->priv_flags & IFF_802_1Q_VLAN) {
	41	netdev_info(dev, "HSR on top of VLAN is not yet supported in this driver.\n");
	42	return -EINVAL;
	43	}
	44
	45	/* HSR over bonded devices has not been tested, but I'm not sure it
	46	* won't work...
	47	*/
	48
	49	return 0;
	50	}
	51
	52
81ba6afd AB	53	static struct sk_buff hsr_pull_tag(struct sk_buff skb)
	54	{
	55	struct hsr_tag *hsr_tag;
	56	struct sk_buff *skb2;
	57
	58	skb2 = skb_share_check(skb, GFP_ATOMIC);
	59	if (unlikely(!skb2))
	60	goto err_free;
	61	skb = skb2;
	62
	63	if (unlikely(!pskb_may_pull(skb, HSR_HLEN)))
	64	goto err_free;
	65
	66	hsr_tag = (struct hsr_tag *) skb->data;
	67	skb->protocol = hsr_tag->encap_proto;
	68	skb_pull(skb, HSR_HLEN);
	69
	70	return skb;
	71
	72	err_free:
	73	kfree_skb(skb);
	74	return NULL;
	75	}
	76
	77
	78	/* The uses I can see for these HSR supervision frames are:
	79	* 1) Use the frames that are sent after node initialization ("HSR_TLV.Type =
	80	* 22") to reset any sequence_nr counters belonging to that node. Useful if
	81	* the other node's counter has been reset for some reason.
	82	* --
	83	* Or not - resetting the counter and bridging the frame would create a
	84	* loop, unfortunately.
	85	*
	86	* 2) Use the LifeCheck frames to detect ring breaks. I.e. if no LifeCheck
	87	* frame is received from a particular node, we know something is wrong.
	88	* We just register these (as with normal frames) and throw them away.
	89	*
	90	* 3) Allow different MAC addresses for the two slave interfaces, using the
	91	* MacAddressA field.
	92	*/
	93	static bool is_supervision_frame(struct hsr_priv hsr, struct sk_buff skb)
	94	{
	95	struct hsr_sup_tag *hsr_stag;
	96
	97	if (!ether_addr_equal(eth_hdr(skb)->h_dest,
	98	hsr->sup_multicast_addr))
	99	return false;
	100
	101	hsr_stag = (struct hsr_sup_tag *) skb->data;
	102	if (get_hsr_stag_path(hsr_stag) != 0x0f)
	103	return false;
	104	if ((hsr_stag->HSR_TLV_Type != HSR_TLV_ANNOUNCE) &&
	105	(hsr_stag->HSR_TLV_Type != HSR_TLV_LIFE_CHECK))
	106	return false;
	107	if (hsr_stag->HSR_TLV_Length != 12)
	108	return false;
	109
	110	return true;
	111	}
	112
	113
	114	/* Implementation somewhat according to IEC-62439-3, p. 43
	115	*/
	116	rx_handler_result_t hsr_handle_frame(struct sk_buff **pskb)
117	{
118	struct sk_buff skb = pskb;
c5a75911	119	struct hsr_port port, other_port, *master;
81ba6afd	120	struct hsr_priv *hsr;
81ba6afd AB	121	struct hsr_node *node;
	122	bool deliver_to_self;
	123	struct sk_buff *skb_deliver;
81ba6afd AB	124	bool dup_out;
	125	int ret;
	126
	127	if (eth_hdr(skb)->h_proto != htons(ETH_P_PRP))
	128	return RX_HANDLER_PASS;
	129
c5a75911	130	rcu_read_lock(); /* ports & node */
81ba6afd	131
c5a75911 AB	132	port = hsr_port_get_rcu(skb->dev);
	133	hsr = port->hsr;
	134	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
81ba6afd AB	135
	136	node = hsr_find_node(&hsr->self_node_db, skb);
	137	if (node) {
	138	/* Always kill frames sent by ourselves */
	139	kfree_skb(skb);
c5a75911 AB	140	ret = RX_HANDLER_CONSUMED;
c5a75911 AB	141	goto finish;
81ba6afd AB	142	}
	143
	144	/* Is this frame a candidate for local reception? */
	145	deliver_to_self = false;
	146	if ((skb->pkt_type == PACKET_HOST) \|\|
	147	(skb->pkt_type == PACKET_MULTICAST) \|\|
	148	(skb->pkt_type == PACKET_BROADCAST))
	149	deliver_to_self = true;
c5a75911 AB	150	else if (ether_addr_equal(eth_hdr(skb)->h_dest,
c5a75911 AB	151	master->dev->dev_addr)) {
81ba6afd AB	152	skb->pkt_type = PACKET_HOST;
	153	deliver_to_self = true;
	154	}
	155
81ba6afd AB	156	node = hsr_find_node(&hsr->node_db, skb);
	157
	158	if (is_supervision_frame(hsr, skb)) {
	159	skb_pull(skb, sizeof(struct hsr_sup_tag));
c5a75911	160	node = hsr_merge_node(node, skb, port);
81ba6afd	161	if (!node) {
81ba6afd	162	kfree_skb(skb);
c5a75911 AB	163	master->dev->stats.rx_dropped++;
	164	ret = RX_HANDLER_CONSUMED;
	165	goto finish;
81ba6afd AB	166	}
	167	skb_push(skb, sizeof(struct hsr_sup_tag));
	168	deliver_to_self = false;
	169	}
	170
	171	if (!node) {
	172	/* Source node unknown; this might be a HSR frame from
	173	* another net (different multicast address). Ignore it.
	174	*/
81ba6afd	175	kfree_skb(skb);
c5a75911 AB	176	ret = RX_HANDLER_CONSUMED;
c5a75911 AB	177	goto finish;
81ba6afd AB	178	}
81ba6afd AB	179
c5a75911 AB	180	if (port->type == HSR_PT_SLAVE_A)
	181	other_port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_B);
	182	else
	183	other_port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_A);
	184
81ba6afd AB	185	/* Register ALL incoming frames as outgoing through the other interface.
	186	* This allows us to register frames as incoming only if they are valid
	187	* for the receiving interface, without using a specific counter for
	188	* incoming frames.
	189	*/
c5a75911 AB	190	if (other_port)
	191	dup_out = hsr_register_frame_out(node, other_port, skb);
	192	else
	193	dup_out = 0;
81ba6afd	194	if (!dup_out)
c5a75911	195	hsr_register_frame_in(node, port);
81ba6afd AB	196
81ba6afd AB	197	/* Forward this frame? */
c5a75911 AB	198	if (dup_out \|\| (skb->pkt_type == PACKET_HOST))
c5a75911 AB	199	other_port = NULL;
81ba6afd	200
c5a75911	201	if (hsr_register_frame_out(node, master, skb))
81ba6afd AB	202	deliver_to_self = false;
81ba6afd AB	203
c5a75911	204	if (!deliver_to_self && !other_port) {
81ba6afd AB	205	kfree_skb(skb);
81ba6afd AB	206	/* Circulated frame; silently remove it. */
c5a75911 AB	207	ret = RX_HANDLER_CONSUMED;
c5a75911 AB	208	goto finish;
81ba6afd AB	209	}
	210
	211	skb_deliver = skb;
c5a75911	212	if (deliver_to_self && other_port) {
81ba6afd AB	213	/* skb_clone() is not enough since we will strip the hsr tag
	214	* and do address substitution below
	215	*/
	216	skb_deliver = pskb_copy(skb, GFP_ATOMIC);
	217	if (!skb_deliver) {
	218	deliver_to_self = false;
c5a75911	219	master->dev->stats.rx_dropped++;
81ba6afd AB	220	}
	221	}
	222
	223	if (deliver_to_self) {
	224	bool multicast_frame;
	225
	226	skb_deliver = hsr_pull_tag(skb_deliver);
	227	if (!skb_deliver) {
c5a75911	228	master->dev->stats.rx_dropped++;
81ba6afd AB	229	goto forward;
	230	}
	231	#if !defined(CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS)
	232	/* Move everything in the header that is after the HSR tag,
	233	* to work around alignment problems caused by the 6-byte HSR
	234	* tag. In practice, this removes/overwrites the HSR tag in
	235	* the header and restores a "standard" packet.
	236	*/
	237	memmove(skb_deliver->data - HSR_HLEN, skb_deliver->data,
	238	skb_headlen(skb_deliver));
	239
	240	/* Adjust skb members so they correspond with the move above.
	241	* This cannot possibly underflow skb->data since hsr_pull_tag()
	242	* above succeeded.
	243	* At this point in the protocol stack, the transport and
	244	* network headers have not been set yet, and we haven't touched
	245	* the mac header nor the head. So we only need to adjust data
	246	* and tail:
	247	*/
	248	skb_deliver->data -= HSR_HLEN;
	249	skb_deliver->tail -= HSR_HLEN;
	250	#endif
c5a75911	251	skb_deliver->dev = master->dev;
81ba6afd AB	252	hsr_addr_subst_source(hsr, skb_deliver);
	253	multicast_frame = (skb_deliver->pkt_type == PACKET_MULTICAST);
	254	ret = netif_rx(skb_deliver);
	255	if (ret == NET_RX_DROP) {
c5a75911	256	master->dev->stats.rx_dropped++;
81ba6afd	257	} else {
c5a75911 AB	258	master->dev->stats.rx_packets++;
c5a75911 AB	259	master->dev->stats.rx_bytes += skb->len;
81ba6afd	260	if (multicast_frame)
c5a75911	261	master->dev->stats.multicast++;
81ba6afd AB	262	}
	263	}
	264
	265	forward:
c5a75911	266	if (other_port) {
81ba6afd	267	skb_push(skb, ETH_HLEN);
c5a75911	268	skb->dev = other_port->dev;
81ba6afd AB	269	dev_queue_xmit(skb);
	270	}
	271
c5a75911 AB	272	ret = RX_HANDLER_CONSUMED;
	273
	274	finish:
	275	rcu_read_unlock();
	276	return ret;
81ba6afd	277	}
51f3c605	278
c5a75911 AB	279	/* Setup device to be added to the HSR bridge. */
c5a75911 AB	280	static int hsr_portdev_setup(struct net_device dev, struct hsr_port port)
51f3c605 AB	281	{
	282	int res;
	283
	284	dev_hold(dev);
51f3c605 AB	285	res = dev_set_promiscuity(dev, 1);
51f3c605 AB	286	if (res)
c5a75911 AB	287	goto fail_promiscuity;
c5a75911 AB	288	res = netdev_rx_handler_register(dev, hsr_handle_frame, port);
51f3c605 AB	289	if (res)
51f3c605 AB	290	goto fail_rx_handler;
c5a75911	291	dev_disable_lro(dev);
51f3c605	292
c5a75911 AB	293	/* FIXME:
	294	* What does net device "adjacency" mean? Should we do
	295	* res = netdev_master_upper_dev_link(port->dev, port->hsr->dev); ?
	296	*/
51f3c605 AB	297
	298	return 0;
	299
	300	fail_rx_handler:
	301	dev_set_promiscuity(dev, -1);
c5a75911	302	fail_promiscuity:
51f3c605	303	dev_put(dev);
c5a75911	304
51f3c605 AB	305	return res;
	306	}
	307
c5a75911 AB	308	int hsr_add_port(struct hsr_priv hsr, struct net_device dev,
c5a75911 AB	309	enum hsr_port_type type)
51f3c605	310	{
c5a75911 AB	311	struct hsr_port port, master;
c5a75911 AB	312	int res;
51f3c605	313
c5a75911 AB	314	if (type != HSR_PT_MASTER) {
	315	res = hsr_check_dev_ok(dev);
	316	if (res)
	317	return res;
	318	}
	319
	320	port = hsr_port_get_hsr(hsr, type);
	321	if (port != NULL)
	322	return -EBUSY; /* This port already exists */
	323
	324	port = kzalloc(sizeof(*port), GFP_KERNEL);
	325	if (port == NULL)
	326	return -ENOMEM;
	327
	328	if (type != HSR_PT_MASTER) {
	329	res = hsr_portdev_setup(dev, port);
	330	if (res)
	331	goto fail_dev_setup;
	332	}
	333
	334	port->hsr = hsr;
	335	port->dev = dev;
	336	port->type = type;
	337
	338	list_add_tail_rcu(&port->port_list, &hsr->ports);
	339	synchronize_rcu();
	340
	341	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
	342
	343	/* Set required header length */
	344	if (dev->hard_header_len + HSR_HLEN > master->dev->hard_header_len)
	345	master->dev->hard_header_len = dev->hard_header_len + HSR_HLEN;
51f3c605	346
c5a75911	347	dev_set_mtu(master->dev, hsr_get_max_mtu(hsr));
51f3c605	348
c5a75911 AB	349	return 0;
	350
	351	fail_dev_setup:
	352	kfree(port);
	353	return res;
	354	}
	355
	356	void hsr_del_port(struct hsr_port *port)
	357	{
	358	struct hsr_priv *hsr;
	359	struct hsr_port *master;
	360
	361	hsr = port->hsr;
	362	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
	363	list_del_rcu(&port->port_list);
	364
	365	if (port != master) {
	366	dev_set_mtu(master->dev, hsr_get_max_mtu(hsr));
	367	netdev_rx_handler_unregister(port->dev);
	368	dev_set_promiscuity(port->dev, -1);
51f3c605 AB	369	}
51f3c605 AB	370
c5a75911 AB	371	/* FIXME?
	372	* netdev_upper_dev_unlink(port->dev, port->hsr->dev);
	373	*/
	374
51f3c605	375	synchronize_rcu();
c5a75911	376	dev_put(port->dev);
51f3c605	377	}