net/hsr: Better frame dispatch

This patch removes the separate paths for frames coming from the outside, and
frames sent from the HSR device, and instead makes all frames go through
hsr_forward_skb() in hsr_forward.c. This greatly improves code readability and
also opens up the possibility for future support of the HSR Interlink device
that is the basis for HSR RedBoxes and HSR QuadBoxes, as well as VLAN
compatibility.

Other improvements:
* A reduction in the number of times an skb is copied on machines without
  HAVE_EFFICIENT_UNALIGNED_ACCESS, which improves throughput somewhat.
* Headers are now created using the standard eth_header(), and using the
  standard hard_header_len.
* Each HSR slave now gets its own private skb, so slave-specific fields can be
  correctly set.

Signed-off-by: Arvid Brodin <arvid.brodin@alten.se>
Signed-off-by: David S. Miller <davem@davemloft.net>

11 files changed, 656 insertions, 586 deletions

diff --git a/net/hsr/Makefile b/net/hsr/Makefile
index b533b243a48b..9ae972a820f4 100644
--- a/net/hsr/Makefile
+++ b/net/hsr/Makefile
@@ -4,5 +4,5 @@
 
 obj-$(CONFIG_HSR)	+= hsr.o
 
-hsr-y			:= hsr_main.o hsr_framereg.o hsr_device.o hsr_netlink.o \
-			   hsr_slave.o
+hsr-y			:= hsr_main.o hsr_framereg.o hsr_device.o \
+			   hsr_netlink.o hsr_slave.o hsr_forward.o
diff --git a/net/hsr/hsr_device.c b/net/hsr/hsr_device.c
index c2ae54924a0f..a138d75751df 100644
--- a/net/hsr/hsr_device.c
+++ b/net/hsr/hsr_device.c
@@ -21,6 +21,7 @@
 #include "hsr_slave.h"
 #include "hsr_framereg.h"
 #include "hsr_main.h"
+#include "hsr_forward.h"
 
 
 static bool is_admin_up(struct net_device *dev)
@@ -231,141 +232,21 @@ static netdev_features_t hsr_fix_features(struct net_device *dev,
 }
 
 
-static void hsr_fill_tag(struct hsr_ethhdr *hsr_ethhdr, struct hsr_priv *hsr)
-{
-	unsigned long irqflags;
-
-	/* IEC 62439-1:2010, p 48, says the 4-bit "path" field can take values
-	 * between 0001-1001 ("ring identifier", for regular HSR frames),
-	 * or 1111 ("HSR management", supervision frames). Unfortunately, the
-	 * spec writers forgot to explain what a "ring identifier" is, or
-	 * how it is used. So we just set this to 0001 for regular frames,
-	 * and 1111 for supervision frames.
-	 */
-	set_hsr_tag_path(&hsr_ethhdr->hsr_tag, 0x1);
-
-	/* IEC 62439-1:2010, p 12: "The link service data unit in an Ethernet
-	 * frame is the content of the frame located between the Length/Type
-	 * field and the Frame Check Sequence."
-	 *
-	 * IEC 62439-3, p 48, specifies the "original LPDU" to include the
-	 * original "LT" field (what "LT" means is not explained anywhere as
-	 * far as I can see - perhaps "Length/Type"?). So LSDU_size might
-	 * equal original length + 2.
-	 *   Also, the fact that this field is not used anywhere (might be used
-	 * by a RedBox connecting HSR and PRP nets?) means I cannot test its
-	 * correctness. Instead of guessing, I set this to 0 here, to make any
-	 * problems immediately apparent. Anyone using this driver with PRP/HSR
-	 * RedBoxes might need to fix this...
-	 */
-	set_hsr_tag_LSDU_size(&hsr_ethhdr->hsr_tag, 0);
-
-	spin_lock_irqsave(&hsr->seqnr_lock, irqflags);
-	hsr_ethhdr->hsr_tag.sequence_nr = htons(hsr->sequence_nr);
-	hsr->sequence_nr++;
-	spin_unlock_irqrestore(&hsr->seqnr_lock, irqflags);
-
-	hsr_ethhdr->hsr_tag.encap_proto = hsr_ethhdr->ethhdr.h_proto;
-
-	hsr_ethhdr->ethhdr.h_proto = htons(ETH_P_PRP);
-}
-
-static int slave_xmit(struct hsr_priv *hsr, struct sk_buff *skb,
-		      enum hsr_port_type type)
-{
-	struct hsr_port *port;
-	struct hsr_ethhdr *hsr_ethhdr;
-
-	hsr_ethhdr = (struct hsr_ethhdr *) skb->data;
-
-	rcu_read_lock();
-	port = hsr_port_get_hsr(hsr, type);
-	if (!port) {
-		rcu_read_unlock();
-		return NET_XMIT_DROP;
-	}
-	skb->dev = port->dev;
-
-	hsr_addr_subst_dest(port->hsr, &hsr_ethhdr->ethhdr, port);
-	rcu_read_unlock();
-
-	/* Address substitution (IEC62439-3 pp 26, 50): replace mac
-	 * address of outgoing frame with that of the outgoing slave's.
-	 */
-	ether_addr_copy(hsr_ethhdr->ethhdr.h_source, skb->dev->dev_addr);
-
-	return dev_queue_xmit(skb);
-}
-
 static int hsr_dev_xmit(struct sk_buff *skb, struct net_device *dev)
 {
-	struct hsr_priv *hsr;
+	struct hsr_priv *hsr = netdev_priv(dev);
 	struct hsr_port *master;
-	struct hsr_ethhdr *hsr_ethhdr;
-	struct sk_buff *skb2;
-	int res1, res2;
-
-	hsr = netdev_priv(dev);
-	hsr_ethhdr = (struct hsr_ethhdr *) skb->data;
-
-	if ((skb->protocol != htons(ETH_P_PRP)) ||
-	    (hsr_ethhdr->ethhdr.h_proto != htons(ETH_P_PRP))) {
-		hsr_fill_tag(hsr_ethhdr, hsr);
-		skb->protocol = htons(ETH_P_PRP);
-	}
 
-	skb2 = pskb_copy(skb, GFP_ATOMIC);
-
-	res1 = slave_xmit(hsr, skb, HSR_PT_SLAVE_A);
-	if (skb2)
-		res2 = slave_xmit(hsr, skb2, HSR_PT_SLAVE_B);
-	else
-		res2 = NET_XMIT_DROP;
-
-	rcu_read_lock();
 	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
-	if (likely(res1 == NET_XMIT_SUCCESS || res1 == NET_XMIT_CN ||
-		   res2 == NET_XMIT_SUCCESS || res2 == NET_XMIT_CN)) {
-		master->dev->stats.tx_packets++;
-		master->dev->stats.tx_bytes += skb->len;
-	} else {
-		master->dev->stats.tx_dropped++;
-	}
-	rcu_read_unlock();
+	skb->dev = master->dev;
+	hsr_forward_skb(skb, master);
 
 	return NETDEV_TX_OK;
 }
 
 
-static int hsr_header_create(struct sk_buff *skb, struct net_device *dev,
-			     unsigned short type, const void *daddr,
-			     const void *saddr, unsigned int len)
-{
-	int res;
-
-	/* Make room for the HSR tag now. We will fill it in later (in
-	 * hsr_dev_xmit)
-	 */
-	if (skb_headroom(skb) < HSR_HLEN + ETH_HLEN)
-		return -ENOBUFS;
-	skb_push(skb, HSR_HLEN);
-
-	/* To allow VLAN/HSR combos we should probably use
-	 * res = dev_hard_header(skb, dev, type, daddr, saddr, len + HSR_HLEN);
-	 * here instead. It would require other changes too, though - e.g.
-	 * separate headers for each slave etc...
-	 */
-	res = eth_header(skb, dev, type, daddr, saddr, len + HSR_HLEN);
-	if (res <= 0)
-		return res;
-	skb_reset_mac_header(skb);
-
-	return res + HSR_HLEN;
-}
-
-
 static const struct header_ops hsr_header_ops = {
-	.create	 = hsr_header_create,
+	.create	 = eth_header,
 	.parse	 = eth_header_parse,
 };
 
@@ -382,19 +263,13 @@ static int hsr_pad(int size)
 	return min_size;
 }
 
-static void send_hsr_supervision_frame(struct net_device *hsr_dev, u8 type)
+static void send_hsr_supervision_frame(struct hsr_port *master, u8 type)
 {
-	struct hsr_priv *hsr;
-	struct hsr_port *master;
 	struct sk_buff *skb;
 	int hlen, tlen;
 	struct hsr_sup_tag *hsr_stag;
 	struct hsr_sup_payload *hsr_sp;
 	unsigned long irqflags;
-	int res;
-
-	hsr = netdev_priv(hsr_dev);
-	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
 
 	hlen = LL_RESERVED_SPACE(master->dev);
 	tlen = master->dev->needed_tailroom;
@@ -410,33 +285,30 @@ static void send_hsr_supervision_frame(struct net_device *hsr_dev, u8 type)
 	skb->protocol = htons(ETH_P_PRP);
 	skb->priority = TC_PRIO_CONTROL;
 
-	res = dev_hard_header(skb, skb->dev, ETH_P_PRP,
-			      hsr->sup_multicast_addr,
-			      skb->dev->dev_addr, skb->len);
-	if (res <= 0)
+	if (dev_hard_header(skb, skb->dev, ETH_P_PRP,
+			    master->hsr->sup_multicast_addr,
+			    skb->dev->dev_addr, skb->len) <= 0)
 		goto out;
+	skb_reset_mac_header(skb);
 
-	skb_pull(skb, sizeof(struct ethhdr));
-	hsr_stag = (typeof(hsr_stag)) skb->data;
+	hsr_stag = (typeof(hsr_stag)) skb_put(skb, sizeof(*hsr_stag));
 
 	set_hsr_stag_path(hsr_stag, 0xf);
 	set_hsr_stag_HSR_Ver(hsr_stag, 0);
 
-	spin_lock_irqsave(&hsr->seqnr_lock, irqflags);
-	hsr_stag->sequence_nr = htons(hsr->sequence_nr);
-	hsr->sequence_nr++;
-	spin_unlock_irqrestore(&hsr->seqnr_lock, irqflags);
+	spin_lock_irqsave(&master->hsr->seqnr_lock, irqflags);
+	hsr_stag->sequence_nr = htons(master->hsr->sequence_nr);
+	master->hsr->sequence_nr++;
+	spin_unlock_irqrestore(&master->hsr->seqnr_lock, irqflags);
 
 	hsr_stag->HSR_TLV_Type = type;
 	hsr_stag->HSR_TLV_Length = 12;
 
-	skb_push(skb, sizeof(struct ethhdr));
-
 	/* Payload: MacAddressA */
 	hsr_sp = (typeof(hsr_sp)) skb_put(skb, sizeof(*hsr_sp));
 	ether_addr_copy(hsr_sp->MacAddressA, master->dev->dev_addr);
 
-	dev_queue_xmit(skb);
+	hsr_forward_skb(skb, master);
 	return;
 
 out:
@@ -453,13 +325,15 @@ static void hsr_announce(unsigned long data)
 	struct hsr_port *master;
 
 	hsr = (struct hsr_priv *) data;
+
+	rcu_read_lock();
 	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
 
 	if (hsr->announce_count < 3) {
-		send_hsr_supervision_frame(master->dev, HSR_TLV_ANNOUNCE);
+		send_hsr_supervision_frame(master, HSR_TLV_ANNOUNCE);
 		hsr->announce_count++;
 	} else {
-		send_hsr_supervision_frame(master->dev, HSR_TLV_LIFE_CHECK);
+		send_hsr_supervision_frame(master, HSR_TLV_LIFE_CHECK);
 	}
 
 	if (hsr->announce_count < 3)
@@ -471,6 +345,8 @@ static void hsr_announce(unsigned long data)
 
 	if (is_admin_up(master->dev))
 		add_timer(&hsr->announce_timer);
+
+	rcu_read_unlock();
 }
 
 
@@ -570,7 +446,7 @@ int hsr_dev_finalize(struct net_device *hsr_dev, struct net_device *slave[2],
 
 	spin_lock_init(&hsr->seqnr_lock);
 	/* Overflow soon to find bugs easier: */
-	hsr->sequence_nr = USHRT_MAX - 1024;
+	hsr->sequence_nr = HSR_SEQNR_START;
 
 	init_timer(&hsr->announce_timer);
 	hsr->announce_timer.function = hsr_announce;
diff --git a/net/hsr/hsr_forward.c b/net/hsr/hsr_forward.c
new file mode 100644
index 000000000000..7871ed6d3825
--- /dev/null
+++ b/net/hsr/hsr_forward.c
@@ -0,0 +1,368 @@
+/* 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_forward.h"
+#include <linux/types.h>
+#include <linux/skbuff.h>
+#include <linux/etherdevice.h>
+#include <linux/if_vlan.h>
+#include "hsr_main.h"
+#include "hsr_framereg.h"
+
+
+struct hsr_node;
+
+struct hsr_frame_info {
+	struct sk_buff *skb_std;
+	struct sk_buff *skb_hsr;
+	struct hsr_port *port_rcv;
+	struct hsr_node *node_src;
+	u16 sequence_nr;
+	bool is_supervision;
+	bool is_vlan;
+	bool is_local_dest;
+	bool is_local_exclusive;
+};
+
+
+/* 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_ethhdr_sp *hdr;
+
+	WARN_ON_ONCE(!skb_mac_header_was_set(skb));
+	hdr = (struct hsr_ethhdr_sp *) skb_mac_header(skb);
+
+	if (!ether_addr_equal(hdr->ethhdr.h_dest,
+			      hsr->sup_multicast_addr))
+		return false;
+
+	if (get_hsr_stag_path(&hdr->hsr_sup) != 0x0f)
+		return false;
+	if ((hdr->hsr_sup.HSR_TLV_Type != HSR_TLV_ANNOUNCE) &&
+	    (hdr->hsr_sup.HSR_TLV_Type != HSR_TLV_LIFE_CHECK))
+		return false;
+	if (hdr->hsr_sup.HSR_TLV_Length != 12)
+		return false;
+
+	return true;
+}
+
+
+static struct sk_buff *create_stripped_skb(struct sk_buff *skb_in,
+					   struct hsr_frame_info *frame)
+{
+	struct sk_buff *skb;
+	int copylen;
+	unsigned char *dst, *src;
+
+	skb_pull(skb_in, HSR_HLEN);
+	skb = __pskb_copy(skb_in, skb_headroom(skb_in) - HSR_HLEN, GFP_ATOMIC);
+	skb_push(skb_in, HSR_HLEN);
+	if (skb == NULL)
+		return NULL;
+
+	skb_reset_mac_header(skb);
+
+	if (skb->ip_summed == CHECKSUM_PARTIAL)
+		skb->csum_start -= HSR_HLEN;
+
+	copylen = 2*ETH_ALEN;
+	if (frame->is_vlan)
+		copylen += VLAN_HLEN;
+	src = skb_mac_header(skb_in);
+	dst = skb_mac_header(skb);
+	memcpy(dst, src, copylen);
+
+	skb->protocol = eth_hdr(skb)->h_proto;
+	return skb;
+}
+
+static struct sk_buff *frame_get_stripped_skb(struct hsr_frame_info *frame,
+					      struct hsr_port *port)
+{
+	if (!frame->skb_std)
+		frame->skb_std = create_stripped_skb(frame->skb_hsr, frame);
+	return skb_clone(frame->skb_std, GFP_ATOMIC);
+}
+
+
+static void hsr_fill_tag(struct sk_buff *skb, struct hsr_frame_info *frame,
+			 struct hsr_port *port)
+{
+	struct hsr_ethhdr *hsr_ethhdr;
+	int lane_id;
+	int lsdu_size;
+
+	if (port->type == HSR_PT_SLAVE_A)
+		lane_id = 0;
+	else
+		lane_id = 1;
+
+	lsdu_size = skb->len - 14;
+	if (frame->is_vlan)
+		lsdu_size -= 4;
+
+	hsr_ethhdr = (struct hsr_ethhdr *) skb_mac_header(skb);
+
+	set_hsr_tag_path(&hsr_ethhdr->hsr_tag, lane_id);
+	set_hsr_tag_LSDU_size(&hsr_ethhdr->hsr_tag, lsdu_size);
+	hsr_ethhdr->hsr_tag.sequence_nr = htons(frame->sequence_nr);
+	hsr_ethhdr->hsr_tag.encap_proto = hsr_ethhdr->ethhdr.h_proto;
+	hsr_ethhdr->ethhdr.h_proto = htons(ETH_P_PRP);
+}
+
+static struct sk_buff *create_tagged_skb(struct sk_buff *skb_o,
+					 struct hsr_frame_info *frame,
+					 struct hsr_port *port)
+{
+	int movelen;
+	unsigned char *dst, *src;
+	struct sk_buff *skb;
+
+	/* Create the new skb with enough headroom to fit the HSR tag */
+	skb = __pskb_copy(skb_o, skb_headroom(skb_o) + HSR_HLEN, GFP_ATOMIC);
+	if (skb == NULL)
+		return NULL;
+	skb_reset_mac_header(skb);
+
+	if (skb->ip_summed == CHECKSUM_PARTIAL)
+		skb->csum_start += HSR_HLEN;
+
+	movelen = ETH_HLEN;
+	if (frame->is_vlan)
+		movelen += VLAN_HLEN;
+
+	src = skb_mac_header(skb);
+	dst = skb_push(skb, HSR_HLEN);
+	memmove(dst, src, movelen);
+	skb_reset_mac_header(skb);
+
+	hsr_fill_tag(skb, frame, port);
+
+	return skb;
+}
+
+/* If the original frame was an HSR tagged frame, just clone it to be sent
+ * unchanged. Otherwise, create a private frame especially tagged for 'port'.
+ */
+static struct sk_buff *frame_get_tagged_skb(struct hsr_frame_info *frame,
+					    struct hsr_port *port)
+{
+	if (frame->skb_hsr)
+		return skb_clone(frame->skb_hsr, GFP_ATOMIC);
+
+	if ((port->type != HSR_PT_SLAVE_A) && (port->type != HSR_PT_SLAVE_B)) {
+		WARN_ONCE(1, "HSR: Bug: trying to create a tagged frame for a non-ring port");
+		return NULL;
+	}
+
+	return create_tagged_skb(frame->skb_std, frame, port);
+}
+
+
+static void hsr_deliver_master(struct sk_buff *skb, struct net_device *dev,
+			       struct hsr_node *node_src)
+{
+	bool was_multicast_frame;
+	int res;
+
+	was_multicast_frame = (skb->pkt_type == PACKET_MULTICAST);
+	hsr_addr_subst_source(node_src, skb);
+	skb_pull(skb, ETH_HLEN);
+	res = netif_rx(skb);
+	if (res == NET_RX_DROP) {
+		dev->stats.rx_dropped++;
+	} else {
+		dev->stats.rx_packets++;
+		dev->stats.rx_bytes += skb->len;
+		if (was_multicast_frame)
+			dev->stats.multicast++;
+	}
+}
+
+static int hsr_xmit(struct sk_buff *skb, struct hsr_port *port,
+		    struct hsr_frame_info *frame)
+{
+	if (frame->port_rcv->type == HSR_PT_MASTER) {
+		hsr_addr_subst_dest(frame->node_src, skb, port);
+
+		/* Address substitution (IEC62439-3 pp 26, 50): replace mac
+		 * address of outgoing frame with that of the outgoing slave's.
+		 */
+		ether_addr_copy(eth_hdr(skb)->h_source, port->dev->dev_addr);
+	}
+	return dev_queue_xmit(skb);
+}
+
+
+/* Forward the frame through all devices except:
+ * - Back through the receiving device
+ * - If it's a HSR frame: through a device where it has passed before
+ * - To the local HSR master only if the frame is directly addressed to it, or
+ *   a non-supervision multicast or broadcast frame.
+ *
+ * HSR slave devices should insert a HSR tag into the frame, or forward the
+ * frame unchanged if it's already tagged. Interlink devices should strip HSR
+ * tags if they're of the non-HSR type (but only after duplicate discard). The
+ * master device always strips HSR tags.
+ */
+static void hsr_forward_do(struct hsr_frame_info *frame)
+{
+	struct hsr_port *port;
+	struct sk_buff *skb;
+
+	hsr_for_each_port(frame->port_rcv->hsr, port) {
+		/* Don't send frame back the way it came */
+		if (port == frame->port_rcv)
+			continue;
+
+		/* Don't deliver locally unless we should */
+		if ((port->type == HSR_PT_MASTER) && !frame->is_local_dest)
+			continue;
+
+		/* Deliver frames directly addressed to us to master only */
+		if ((port->type != HSR_PT_MASTER) && frame->is_local_exclusive)
+			continue;
+
+		/* Don't send frame over port where it has been sent before */
+		if (hsr_register_frame_out(port, frame->node_src,
+					   frame->sequence_nr))
+			continue;
+
+		if (frame->is_supervision && (port->type == HSR_PT_MASTER)) {
+			hsr_handle_sup_frame(frame->skb_hsr,
+					     frame->node_src,
+					     frame->port_rcv);
+			continue;
+		}
+
+		if (port->type != HSR_PT_MASTER)
+			skb = frame_get_tagged_skb(frame, port);
+		else
+			skb = frame_get_stripped_skb(frame, port);
+		if (skb == NULL) {
+			/* FIXME: Record the dropped frame? */
+			continue;
+		}
+
+		skb->dev = port->dev;
+		if (port->type == HSR_PT_MASTER)
+			hsr_deliver_master(skb, port->dev, frame->node_src);
+		else
+			hsr_xmit(skb, port, frame);
+	}
+}
+
+
+static void check_local_dest(struct hsr_priv *hsr, struct sk_buff *skb,
+			     struct hsr_frame_info *frame)
+{
+	struct net_device *master_dev;
+
+	master_dev = hsr_port_get_hsr(hsr, HSR_PT_MASTER)->dev;
+
+	if (hsr_addr_is_self(hsr, eth_hdr(skb)->h_dest)) {
+		frame->is_local_exclusive = true;
+		skb->pkt_type = PACKET_HOST;
+	} else {
+		frame->is_local_exclusive = false;
+	}
+
+	if ((skb->pkt_type == PACKET_HOST) ||
+	    (skb->pkt_type == PACKET_MULTICAST) ||
+	    (skb->pkt_type == PACKET_BROADCAST)) {
+		frame->is_local_dest = true;
+	} else {
+		frame->is_local_dest = false;
+	}
+}
+
+
+static int hsr_fill_frame_info(struct hsr_frame_info *frame,
+			       struct sk_buff *skb, struct hsr_port *port)
+{
+	struct ethhdr *ethhdr;
+	unsigned long irqflags;
+
+	frame->is_supervision = is_supervision_frame(port->hsr, skb);
+	frame->node_src = hsr_get_node(&port->hsr->node_db, skb,
+				       frame->is_supervision);
+	if (frame->node_src == NULL)
+		return -1; /* Unknown node and !is_supervision, or no mem */
+
+	ethhdr = (struct ethhdr *) skb_mac_header(skb);
+	frame->is_vlan = false;
+	if (ethhdr->h_proto == htons(ETH_P_8021Q)) {
+		frame->is_vlan = true;
+		/* FIXME: */
+		WARN_ONCE(1, "HSR: VLAN not yet supported");
+	}
+	if (ethhdr->h_proto == htons(ETH_P_PRP)) {
+		frame->skb_std = NULL;
+		frame->skb_hsr = skb;
+		frame->sequence_nr = hsr_get_skb_sequence_nr(skb);
+	} else {
+		frame->skb_std = skb;
+		frame->skb_hsr = NULL;
+		/* Sequence nr for the master node */
+		spin_lock_irqsave(&port->hsr->seqnr_lock, irqflags);
+		frame->sequence_nr = port->hsr->sequence_nr;
+		port->hsr->sequence_nr++;
+		spin_unlock_irqrestore(&port->hsr->seqnr_lock, irqflags);
+	}
+
+	frame->port_rcv = port;
+	check_local_dest(port->hsr, skb, frame);
+
+	return 0;
+}
+
+/* Must be called holding rcu read lock (because of the port parameter) */
+void hsr_forward_skb(struct sk_buff *skb, struct hsr_port *port)
+{
+	struct hsr_frame_info frame;
+
+	if (skb_mac_header(skb) != skb->data) {
+		WARN_ONCE(1, "%s:%d: Malformed frame (port_src %s)\n",
+			  __FILE__, __LINE__, port->dev->name);
+		goto out_drop;
+	}
+
+	if (hsr_fill_frame_info(&frame, skb, port) < 0)
+		goto out_drop;
+	hsr_register_frame_in(frame.node_src, port, frame.sequence_nr);
+	hsr_forward_do(&frame);
+
+	if (frame.skb_hsr != NULL)
+		kfree_skb(frame.skb_hsr);
+	if (frame.skb_std != NULL)
+		kfree_skb(frame.skb_std);
+	return;
+
+out_drop:
+	port->dev->stats.tx_dropped++;
+	kfree_skb(skb);
+}
diff --git a/net/hsr/hsr_forward.h b/net/hsr/hsr_forward.h
new file mode 100644
index 000000000000..5c5bc4b6b75f
--- /dev/null
+++ b/net/hsr/hsr_forward.h
@@ -0,0 +1,20 @@
+/* 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
+ */
+
+#ifndef __HSR_FORWARD_H
+#define __HSR_FORWARD_H
+
+#include <linux/netdevice.h>
+#include "hsr_main.h"
+
+void hsr_forward_skb(struct sk_buff *skb, struct hsr_port *port);
+
+#endif /* __HSR_FORWARD_H */
diff --git a/net/hsr/hsr_framereg.c b/net/hsr/hsr_framereg.c
index c9b78c52c75b..c709c13842cb 100644
--- a/net/hsr/hsr_framereg.c
+++ b/net/hsr/hsr_framereg.c
@@ -35,58 +35,56 @@ struct hsr_node {
 	struct rcu_head		rcu_head;
 };
 
-/*	TODO: use hash lists for mac addresses (linux/jhash.h)?    */
 
+/*	TODO: use hash lists for mac addresses (linux/jhash.h)?    */
 
 
-/* Search for mac entry. Caller must hold rcu read lock.
+/* seq_nr_after(a, b) - return true if a is after (higher in sequence than) b,
+ * false otherwise.
  */
-static struct hsr_node *find_node_by_AddrA(struct list_head *node_db,
-					   const unsigned char addr[ETH_ALEN])
+static bool seq_nr_after(u16 a, u16 b)
 {
-	struct hsr_node *node;
-
-	list_for_each_entry_rcu(node, node_db, mac_list) {
-		if (ether_addr_equal(node->MacAddressA, addr))
-			return node;
-	}
+	/* Remove inconsistency where
+	 * seq_nr_after(a, b) == seq_nr_before(a, b)
+	 */
+	if ((int) b - a == 32768)
+		return false;
 
-	return NULL;
+	return (((s16) (b - a)) < 0);
 }
+#define seq_nr_before(a, b)		seq_nr_after((b), (a))
+#define seq_nr_after_or_eq(a, b)	(!seq_nr_before((a), (b)))
+#define seq_nr_before_or_eq(a, b)	(!seq_nr_after((a), (b)))
 
 
-/* Search for mac entry. Caller must hold rcu read lock.
- */
-static struct hsr_node *find_node_by_AddrB(struct list_head *node_db,
-					   const unsigned char addr[ETH_ALEN])
+bool hsr_addr_is_self(struct hsr_priv *hsr, unsigned char *addr)
 {
 	struct hsr_node *node;
 
-	list_for_each_entry_rcu(node, node_db, mac_list) {
-		if (ether_addr_equal(node->MacAddressB, addr))
-			return node;
+	node = list_first_or_null_rcu(&hsr->self_node_db, struct hsr_node,
+				      mac_list);
+	if (!node) {
+		WARN_ONCE(1, "HSR: No self node\n");
+		return false;
 	}
 
-	return NULL;
-}
+	if (ether_addr_equal(addr, node->MacAddressA))
+		return true;
+	if (ether_addr_equal(addr, node->MacAddressB))
+		return true;
 
+	return false;
+}
 
 /* Search for mac entry. Caller must hold rcu read lock.
  */
-struct hsr_node *hsr_find_node(struct list_head *node_db, struct sk_buff *skb)
+static struct hsr_node *find_node_by_AddrA(struct list_head *node_db,
+					   const unsigned char addr[ETH_ALEN])
 {
 	struct hsr_node *node;
-	struct ethhdr *ethhdr;
-
-	if (!skb_mac_header_was_set(skb))
-		return NULL;
-
-	ethhdr = (struct ethhdr *) skb_mac_header(skb);
 
 	list_for_each_entry_rcu(node, node_db, mac_list) {
-		if (ether_addr_equal(node->MacAddressA, ethhdr->h_source))
-			return node;
-		if (ether_addr_equal(node->MacAddressB, ethhdr->h_source))
+		if (ether_addr_equal(node->MacAddressA, addr))
 			return node;
 	}
 
@@ -127,90 +125,22 @@ int hsr_create_self_node(struct list_head *self_node_db,
 }
 
 
-/* Add/merge node to the database of nodes. 'skb' must contain an HSR
- * supervision frame.
- * - If the supervision header's MacAddressA field is not yet in the database,
- * this frame is from an hitherto unknown node - add it to the database.
- * - If the sender's MAC address is not the same as its MacAddressA address,
- * the node is using PICS_SUBS (address substitution). Record the sender's
- * address as the node's MacAddressB.
- *
- * This function needs to work even if the sender node has changed one of its
- * slaves' MAC addresses. In this case, there are four different cases described
- * by (Addr-changed, received-from) pairs as follows. Note that changing the
- * SlaveA address is equal to changing the node's own address:
- *
- * - (AddrB, SlaveB): The new AddrB will be recorded by PICS_SUBS code since
- *		      node == NULL.
- * - (AddrB, SlaveA): Will work as usual (the AddrB change won't be detected
- *		      from this frame).
- *
- * - (AddrA, SlaveB): The old node will be found. We need to detect this and
- *		      remove the node.
- * - (AddrA, SlaveA): A new node will be registered (non-PICS_SUBS at first).
- *		      The old one will be pruned after HSR_NODE_FORGET_TIME.
- *
- * We also need to detect if the sender's SlaveA and SlaveB cables have been
- * swapped.
+/* Allocate an hsr_node and add it to node_db. 'addr' is the node's AddressA;
+ * seq_out is used to initialize filtering of outgoing duplicate frames
+ * originating from the newly added node.
  */
-struct hsr_node *hsr_merge_node(struct hsr_node *node, struct sk_buff *skb,
-				struct hsr_port *port)
+struct hsr_node *hsr_add_node(struct list_head *node_db, unsigned char addr[],
+			      u16 seq_out)
 {
-	struct hsr_priv *hsr;
-	struct hsr_sup_payload *hsr_sp;
-	struct hsr_ethhdr_sp *hsr_ethsup;
-	int i;
+	struct hsr_node *node;
 	unsigned long now;
-
-	hsr_ethsup = (struct hsr_ethhdr_sp *) skb_mac_header(skb);
-	hsr_sp = (struct hsr_sup_payload *) skb->data;
-	hsr = port->hsr;
-
-	if (node && !ether_addr_equal(node->MacAddressA, hsr_sp->MacAddressA)) {
-		/* Node has changed its AddrA, frame was received from SlaveB */
-		list_del_rcu(&node->mac_list);
-		kfree_rcu(node, rcu_head);
-		node = NULL;
-	}
-
-	if (node && (port->type == node->AddrB_port) &&
-	    !ether_addr_equal(node->MacAddressB, hsr_ethsup->ethhdr.h_source)) {
-		/* Cables have been swapped */
-		list_del_rcu(&node->mac_list);
-		kfree_rcu(node, rcu_head);
-		node = NULL;
-	}
-
-	if (node && (port->type != node->AddrB_port) &&
-	    (node->AddrB_port != HSR_PT_NONE) &&
-	    !ether_addr_equal(node->MacAddressA, hsr_ethsup->ethhdr.h_source)) {
-		/* Cables have been swapped */
-		list_del_rcu(&node->mac_list);
-		kfree_rcu(node, rcu_head);
-		node = NULL;
-	}
-
-	if (node)
-		return node;
-
-	node = find_node_by_AddrA(&hsr->node_db, hsr_sp->MacAddressA);
-	if (node) {
-		/* Node is known, but frame was received from an unknown
-		 * address. Node is PICS_SUBS capable; merge its AddrB.
-		 */
-		ether_addr_copy(node->MacAddressB, hsr_ethsup->ethhdr.h_source);
-		node->AddrB_port = port->type;
-		return node;
-	}
+	int i;
 
 	node = kzalloc(sizeof(*node), GFP_ATOMIC);
 	if (!node)
 		return NULL;
 
-	ether_addr_copy(node->MacAddressA, hsr_sp->MacAddressA);
-	ether_addr_copy(node->MacAddressB, hsr_ethsup->ethhdr.h_source);
-	if (!ether_addr_equal(hsr_sp->MacAddressA, hsr_ethsup->ethhdr.h_source))
-		node->AddrB_port = port->type;
+	ether_addr_copy(node->MacAddressA, addr);
 
 	/* We are only interested in time diffs here, so use current jiffies
 	 * as initialization. (0 could trigger an spurious ring error warning).
@@ -219,41 +149,120 @@ struct hsr_node *hsr_merge_node(struct hsr_node *node, struct sk_buff *skb,
 	for (i = 0; i < HSR_PT_PORTS; i++)
 		node->time_in[i] = now;
 	for (i = 0; i < HSR_PT_PORTS; i++)
-		node->seq_out[i] = ntohs(hsr_ethsup->hsr_sup.sequence_nr) - 1;
+		node->seq_out[i] = seq_out;
 
-	list_add_tail_rcu(&node->mac_list, &hsr->node_db);
+	list_add_tail_rcu(&node->mac_list, node_db);
 
 	return node;
 }
 
+/* Get the hsr_node from which 'skb' was sent.
+ */
+struct hsr_node *hsr_get_node(struct list_head *node_db, struct sk_buff *skb,
+			      bool is_sup)
+{
+	struct hsr_node *node;
+	struct ethhdr *ethhdr;
+	u16 seq_out;
+
+	if (!skb_mac_header_was_set(skb))
+		return NULL;
+
+	ethhdr = (struct ethhdr *) skb_mac_header(skb);
+
+	list_for_each_entry_rcu(node, node_db, mac_list) {
+		if (ether_addr_equal(node->MacAddressA, ethhdr->h_source))
+			return node;
+		if (ether_addr_equal(node->MacAddressB, ethhdr->h_source))
+			return node;
+	}
+
+	if (!is_sup)
+		return NULL; /* Only supervision frame may create node entry */
+
+	if (ethhdr->h_proto == htons(ETH_P_PRP)) {
+		/* Use the existing sequence_nr from the tag as starting point
+		 * for filtering duplicate frames.
+		 */
+		seq_out = hsr_get_skb_sequence_nr(skb) - 1;
+	} else {
+		WARN_ONCE(1, "%s: Non-HSR frame\n", __func__);
+		seq_out = 0;
+	}
+
+	return hsr_add_node(node_db, ethhdr->h_source, seq_out);
+}
+
+/* Use the Supervision frame's info about an eventual MacAddressB for merging
+ * nodes that has previously had their MacAddressB registered as a separate
+ * node.
+ */
+void hsr_handle_sup_frame(struct sk_buff *skb, struct hsr_node *node_curr,
+			  struct hsr_port *port_rcv)
+{
+	struct hsr_node *node_real;
+	struct hsr_sup_payload *hsr_sp;
+	struct list_head *node_db;
+	int i;
+
+	skb_pull(skb, sizeof(struct hsr_ethhdr_sp));
+	hsr_sp = (struct hsr_sup_payload *) skb->data;
+
+	if (ether_addr_equal(eth_hdr(skb)->h_source, hsr_sp->MacAddressA))
+		/* Not sent from MacAddressB of a PICS_SUBS capable node */
+		goto done;
+
+	/* Merge node_curr (registered on MacAddressB) into node_real */
+	node_db = &port_rcv->hsr->node_db;
+	node_real = find_node_by_AddrA(node_db, hsr_sp->MacAddressA);
+	if (!node_real)
+		/* No frame received from AddrA of this node yet */
+		node_real = hsr_add_node(node_db, hsr_sp->MacAddressA,
+					 HSR_SEQNR_START - 1);
+	if (!node_real)
+		goto done; /* No mem */
+	if (node_real == node_curr)
+		/* Node has already been merged */
+		goto done;
+
+	ether_addr_copy(node_real->MacAddressB, eth_hdr(skb)->h_source);
+	for (i = 0; i < HSR_PT_PORTS; i++) {
+		if (!node_curr->time_in_stale[i] &&
+		    time_after(node_curr->time_in[i], node_real->time_in[i])) {
+			node_real->time_in[i] = node_curr->time_in[i];
+			node_real->time_in_stale[i] = node_curr->time_in_stale[i];
+		}
+		if (seq_nr_after(node_curr->seq_out[i], node_real->seq_out[i]))
+			node_real->seq_out[i] = node_curr->seq_out[i];
+	}
+	node_real->AddrB_port = port_rcv->type;
+
+	list_del_rcu(&node_curr->mac_list);
+	kfree_rcu(node_curr, rcu_head);
+
+done:
+	skb_push(skb, sizeof(struct hsr_ethhdr_sp));
+}
+
 
 /* 'skb' is a frame meant for this host, that is to be passed to upper layers.
  *
- * If the frame was sent by a node's B interface, replace the sender
+ * If the frame was sent by a node's B interface, replace the source
  * address with that node's "official" address (MacAddressA) so that upper
  * layers recognize where it came from.
  */
-void hsr_addr_subst_source(struct hsr_priv *hsr, struct sk_buff *skb)
+void hsr_addr_subst_source(struct hsr_node *node, struct sk_buff *skb)
 {
-	struct ethhdr *ethhdr;
-	struct hsr_node *node;
-
 	if (!skb_mac_header_was_set(skb)) {
 		WARN_ONCE(1, "%s: Mac header not set\n", __func__);
 		return;
 	}
-	ethhdr = (struct ethhdr *) skb_mac_header(skb);
 
-	rcu_read_lock();
-	node = find_node_by_AddrB(&hsr->node_db, ethhdr->h_source);
-	if (node)
-		ether_addr_copy(ethhdr->h_source, node->MacAddressA);
-	rcu_read_unlock();
+	memcpy(&eth_hdr(skb)->h_source, node->MacAddressA, ETH_ALEN);
 }
 
-
 /* 'skb' is a frame meant for another host.
- * 'hsr_dev_idx' is the HSR index of the outgoing device
+ * 'port' is the outgoing interface
  *
  * Substitute the target (dest) MAC address if necessary, so the it matches the
  * recipient interface MAC address, regardless of whether that is the
@@ -261,44 +270,49 @@ void hsr_addr_subst_source(struct hsr_priv *hsr, struct sk_buff *skb)
  * This is needed to keep the packets flowing through switches that learn on
  * which "side" the different interfaces are.
  */
-void hsr_addr_subst_dest(struct hsr_priv *hsr, struct ethhdr *ethhdr,
+void hsr_addr_subst_dest(struct hsr_node *node_src, struct sk_buff *skb,
 			 struct hsr_port *port)
 {
-	struct hsr_node *node;
+	struct hsr_node *node_dst;
 
-	rcu_read_lock();
-	node = find_node_by_AddrA(&hsr->node_db, ethhdr->h_dest);
-	if (node && (node->AddrB_port == port->type))
-		ether_addr_copy(ethhdr->h_dest, node->MacAddressB);
-	rcu_read_unlock();
-}
+	if (!skb_mac_header_was_set(skb)) {
+		WARN_ONCE(1, "%s: Mac header not set\n", __func__);
+		return;
+	}
 
+	if (!is_unicast_ether_addr(eth_hdr(skb)->h_dest))
+		return;
 
-/* seq_nr_after(a, b) - return true if a is after (higher in sequence than) b,
- * false otherwise.
- */
-static bool seq_nr_after(u16 a, u16 b)
-{
-	/* Remove inconsistency where
-	 * seq_nr_after(a, b) == seq_nr_before(a, b)
-	 */
-	if ((int) b - a == 32768)
-		return false;
+	node_dst = find_node_by_AddrA(&port->hsr->node_db, eth_hdr(skb)->h_dest);
+	if (!node_dst) {
+		WARN_ONCE(1, "%s: Unknown node\n", __func__);
+		return;
+	}
+	if (port->type != node_dst->AddrB_port)
+		return;
+	if (!node_dst->MacAddressB) {
+		WARN_ONCE(1, "%s: No MacAddressB\n", __func__);
+		return;
+	}
 
-	return (((s16) (b - a)) < 0);
+	ether_addr_copy(eth_hdr(skb)->h_dest, node_dst->MacAddressB);
 }
-#define seq_nr_before(a, b)		seq_nr_after((b), (a))
-#define seq_nr_after_or_eq(a, b)	(!seq_nr_before((a), (b)))
-#define seq_nr_before_or_eq(a, b)	(!seq_nr_after((a), (b)))
 
 
-void hsr_register_frame_in(struct hsr_node *node, struct hsr_port *port)
+void hsr_register_frame_in(struct hsr_node *node, struct hsr_port *port,
+			   u16 sequence_nr)
 {
+	/* Don't register incoming frames without a valid sequence number. This
+	 * ensures entries of restarted nodes gets pruned so that they can
+	 * re-register and resume communications.
+	 */
+	if (seq_nr_before(sequence_nr, node->seq_out[port->type]))
+		return;
+
 	node->time_in[port->type] = jiffies;
 	node->time_in_stale[port->type] = false;
 }
 
-
 /* 'skb' is a HSR Ethernet frame (with a HSR tag inserted), with a valid
  * ethhdr->h_source address and skb->mac_header set.
  *
@@ -307,19 +321,9 @@ void hsr_register_frame_in(struct hsr_node *node, struct hsr_port *port)
  *	 0 otherwise, or
  *	 negative error code on error
  */
-int hsr_register_frame_out(struct hsr_node *node, struct hsr_port *port,
-			   struct sk_buff *skb)
+int hsr_register_frame_out(struct hsr_port *port, struct hsr_node *node,
+			   u16 sequence_nr)
 {
-	struct hsr_ethhdr *hsr_ethhdr;
-	u16 sequence_nr;
-
-	if (!skb_mac_header_was_set(skb)) {
-		WARN_ONCE(1, "%s: Mac header not set\n", __func__);
-		return -EINVAL;
-	}
-	hsr_ethhdr = (struct hsr_ethhdr *) skb_mac_header(skb);
-
-	sequence_nr = ntohs(hsr_ethhdr->hsr_tag.sequence_nr);
 	if (seq_nr_before_or_eq(sequence_nr, node->seq_out[port->type]))
 		return 1;
 
diff --git a/net/hsr/hsr_framereg.h b/net/hsr/hsr_framereg.h
index c87f36fc154c..438b40f98f5a 100644
--- a/net/hsr/hsr_framereg.h
+++ b/net/hsr/hsr_framereg.h
@@ -16,19 +16,22 @@
 
 struct hsr_node;
 
-struct hsr_node *hsr_find_node(struct list_head *node_db, struct sk_buff *skb);
-
-struct hsr_node *hsr_merge_node(struct hsr_node *node, struct sk_buff *skb,
-				struct hsr_port *port);
-
-void hsr_addr_subst_source(struct hsr_priv *hsr, struct sk_buff *skb);
-void hsr_addr_subst_dest(struct hsr_priv *hsr, struct ethhdr *ethhdr,
+struct hsr_node *hsr_add_node(struct list_head *node_db, unsigned char addr[],
+			      u16 seq_out);
+struct hsr_node *hsr_get_node(struct list_head *node_db, struct sk_buff *skb,
+			      bool is_sup);
+void hsr_handle_sup_frame(struct sk_buff *skb, struct hsr_node *node_curr,
+			  struct hsr_port *port);
+bool hsr_addr_is_self(struct hsr_priv *hsr, unsigned char *addr);
+
+void hsr_addr_subst_source(struct hsr_node *node, struct sk_buff *skb);
+void hsr_addr_subst_dest(struct hsr_node *node_src, struct sk_buff *skb,
 			 struct hsr_port *port);
 
-void hsr_register_frame_in(struct hsr_node *node, struct hsr_port *port);
-
-int hsr_register_frame_out(struct hsr_node *node, struct hsr_port *port,
-			   struct sk_buff *skb);
+void hsr_register_frame_in(struct hsr_node *node, struct hsr_port *port,
+			   u16 sequence_nr);
+int hsr_register_frame_out(struct hsr_port *port, struct hsr_node *node,
+			   u16 sequence_nr);
 
 void hsr_prune_nodes(unsigned long data);
 
diff --git a/net/hsr/hsr_main.c b/net/hsr/hsr_main.c
index a06cab57ab68..779d28b65417 100644
--- a/net/hsr/hsr_main.c
+++ b/net/hsr/hsr_main.c
@@ -20,26 +20,6 @@
 #include "hsr_slave.h"
 
 
-/* List of all registered virtual HSR devices */
-static LIST_HEAD(hsr_list);
-
-void register_hsr_master(struct hsr_priv *hsr)
-{
-	list_add_tail_rcu(&hsr->hsr_list, &hsr_list);
-}
-
-void unregister_hsr_master(struct hsr_priv *hsr)
-{
-	struct hsr_priv *hsr_it;
-
-	list_for_each_entry(hsr_it, &hsr_list, hsr_list)
-		if (hsr_it == hsr) {
-			list_del_rcu(&hsr_it->hsr_list);
-			return;
-		}
-}
-
-
 static int hsr_netdev_notify(struct notifier_block *nb, unsigned long event,
 			     void *ptr)
 {
diff --git a/net/hsr/hsr_main.h b/net/hsr/hsr_main.h
index e31c3069fdf8..5a9c69962ded 100644
--- a/net/hsr/hsr_main.h
+++ b/net/hsr/hsr_main.h
@@ -29,6 +29,7 @@
  * each node differ before we notify of communication problem?
  */
 #define MAX_SLAVE_DIFF			 3000 /* ms */
+#define HSR_SEQNR_START			(USHRT_MAX - 1024)
 
 
 /* How often shall we check for broken ring and remove node entries older than
@@ -153,10 +154,9 @@ struct hsr_port {
 };
 
 struct hsr_priv {
-	struct list_head	hsr_list;	/* List of hsr devices */
 	struct rcu_head		rcu_head;
 	struct list_head	ports;
-	struct list_head	node_db;	/* Other HSR nodes */
+	struct list_head	node_db;	/* Known HSR nodes */
 	struct list_head	self_node_db;	/* MACs of slaves */
 	struct timer_list	announce_timer;	/* Supervision frame dispatch */
 	struct timer_list	prune_timer;
@@ -166,6 +166,18 @@ struct hsr_priv {
 	unsigned char		sup_multicast_addr[ETH_ALEN];
 };
 
+#define hsr_for_each_port(hsr, port) \
+	list_for_each_entry_rcu((port), &(hsr)->ports, port_list)
+
 struct hsr_port *hsr_port_get_hsr(struct hsr_priv *hsr, enum hsr_port_type pt);
 
+/* Caller must ensure skb is a valid HSR frame */
+static inline u16 hsr_get_skb_sequence_nr(struct sk_buff *skb)
+{
+	struct hsr_ethhdr *hsr_ethhdr;
+
+	hsr_ethhdr = (struct hsr_ethhdr *) skb_mac_header(skb);
+	return ntohs(hsr_ethhdr->hsr_tag.sequence_nr);
+}
+
 #endif /*  __HSR_PRIVATE_H */
diff --git a/net/hsr/hsr_netlink.c b/net/hsr/hsr_netlink.c
index 67082453928c..fbdf53f1d874 100644
--- a/net/hsr/hsr_netlink.c
+++ b/net/hsr/hsr_netlink.c
@@ -358,7 +358,7 @@ fail:
 	return res;
 }
 
-/* Get a list of MacAddressA of all nodes known to this node (other than self).
+/* Get a list of MacAddressA of all nodes known to this node (including self).
  */
 static int hsr_get_node_list(struct sk_buff *skb_in, struct genl_info *info)
 {
diff --git a/net/hsr/hsr_slave.c b/net/hsr/hsr_slave.c
index 23817d0b765b..a348dcbcd683 100644
--- a/net/hsr/hsr_slave.c
+++ b/net/hsr/hsr_slave.c
@@ -14,9 +14,51 @@
 #include <linux/if_arp.h>
 #include "hsr_main.h"
 #include "hsr_device.h"
+#include "hsr_forward.h"
 #include "hsr_framereg.h"
 
 
+static rx_handler_result_t hsr_handle_frame(struct sk_buff **pskb)
+{
+	struct sk_buff *skb = *pskb;
+	struct hsr_port *port;
+
+	if (!skb_mac_header_was_set(skb)) {
+		WARN_ONCE(1, "%s: skb invalid", __func__);
+		return RX_HANDLER_PASS;
+	}
+
+	rcu_read_lock(); /* hsr->node_db, hsr->ports */
+	port = hsr_port_get_rcu(skb->dev);
+
+	if (hsr_addr_is_self(port->hsr, eth_hdr(skb)->h_source)) {
+		/* Directly kill frames sent by ourselves */
+		kfree_skb(skb);
+		goto finish_consume;
+	}
+
+	if (eth_hdr(skb)->h_proto != htons(ETH_P_PRP))
+		goto finish_pass;
+
+	skb_push(skb, ETH_HLEN);
+
+	hsr_forward_skb(skb, port);
+
+finish_consume:
+	rcu_read_unlock(); /* hsr->node_db, hsr->ports */
+	return RX_HANDLER_CONSUMED;
+
+finish_pass:
+	rcu_read_unlock(); /* hsr->node_db, hsr->ports */
+	return RX_HANDLER_PASS;
+}
+
+bool hsr_port_exists(const struct net_device *dev)
+{
+	return rcu_access_pointer(dev->rx_handler) == hsr_handle_frame;
+}
+
+
 static int hsr_check_dev_ok(struct net_device *dev)
 {
 	/* Don't allow HSR on non-ethernet like devices */
@@ -42,6 +84,11 @@ static int hsr_check_dev_ok(struct net_device *dev)
 		return -EINVAL;
 	}
 
+	if (dev->priv_flags & IFF_DONT_BRIDGE) {
+		netdev_info(dev, "This device does not support bridging.\n");
+		return -EOPNOTSUPP;
+	}
+
 	/* HSR over bonded devices has not been tested, but I'm not sure it
 	 * won't work...
 	 */
@@ -50,232 +97,6 @@ static int hsr_check_dev_ok(struct net_device *dev)
 }
 
 
-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)
 {
@@ -285,16 +106,17 @@ static int hsr_portdev_setup(struct net_device *dev, struct hsr_port *port)
 	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); ?
 	 */
 
+	res = netdev_rx_handler_register(dev, hsr_handle_frame, port);
+	if (res)
+		goto fail_rx_handler;
+	dev_disable_lro(dev);
+
 	return 0;
 
 fail_rx_handler:
@@ -339,11 +161,6 @@ int hsr_add_port(struct hsr_priv *hsr, struct net_device *dev,
 	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;
-
 	netdev_update_features(master->dev);
 	dev_set_mtu(master->dev, hsr_get_max_mtu(hsr));
 
diff --git a/net/hsr/hsr_slave.h b/net/hsr/hsr_slave.h
index 3055022eddb3..3ccfbf71c92e 100644
--- a/net/hsr/hsr_slave.h
+++ b/net/hsr/hsr_slave.h
@@ -20,17 +20,7 @@
 int hsr_add_port(struct hsr_priv *hsr, struct net_device *dev,
 		 enum hsr_port_type pt);
 void hsr_del_port(struct hsr_port *port);
-rx_handler_result_t hsr_handle_frame(struct sk_buff **pskb);
-
-
-#define hsr_for_each_port(hsr, port) \
-	list_for_each_entry_rcu((port), &(hsr)->ports, port_list)
-
-
-static inline bool hsr_port_exists(const struct net_device *dev)
-{
-	return dev->rx_handler == hsr_handle_frame;
-}
+bool hsr_port_exists(const struct net_device *dev);
 
 static inline struct hsr_port *hsr_port_get_rtnl(const struct net_device *dev)
 {