diff --git a/net/hsr/Makefile b/net/hsr/Makefile
index b68359f181cc..9ae972a820f4 100644
--- a/net/hsr/Makefile
+++ b/net/hsr/Makefile
@@ -4,4 +4,5 @@
 
 obj-$(CONFIG_HSR)	+= hsr.o
 
-hsr-y			:= hsr_main.o hsr_framereg.o hsr_device.o hsr_netlink.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 e5302b7f7ca9..a138d75751df 100644
--- a/net/hsr/hsr_device.c
+++ b/net/hsr/hsr_device.c
@@ -1,4 +1,4 @@
-/* Copyright 2011-2013 Autronica Fire and Security AS
+/* 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
@@ -6,7 +6,7 @@
  * any later version.
  *
  * Author(s):
- *	2011-2013 Arvid Brodin, arvid.brodin@xdin.com
+ *	2011-2014 Arvid Brodin, arvid.brodin@alten.se
  *
  * This file contains device methods for creating, using and destroying
  * virtual HSR devices.
@@ -15,12 +15,13 @@
 #include <linux/netdevice.h>
 #include <linux/skbuff.h>
 #include <linux/etherdevice.h>
-#include <linux/if_arp.h>
 #include <linux/rtnetlink.h>
 #include <linux/pkt_sched.h>
 #include "hsr_device.h"
+#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)
@@ -45,75 +46,108 @@ static void __hsr_set_operstate(struct net_device *dev, int transition)
 	}
 }
 
-void hsr_set_operstate(struct net_device *hsr_dev, struct net_device *slave1,
-		       struct net_device *slave2)
+static void hsr_set_operstate(struct hsr_port *master, bool has_carrier)
 {
-	if (!is_admin_up(hsr_dev)) {
-		__hsr_set_operstate(hsr_dev, IF_OPER_DOWN);
+	if (!is_admin_up(master->dev)) {
+		__hsr_set_operstate(master->dev, IF_OPER_DOWN);
 		return;
 	}
 
-	if (is_slave_up(slave1) || is_slave_up(slave2))
-		__hsr_set_operstate(hsr_dev, IF_OPER_UP);
+	if (has_carrier)
+		__hsr_set_operstate(master->dev, IF_OPER_UP);
 	else
-		__hsr_set_operstate(hsr_dev, IF_OPER_LOWERLAYERDOWN);
+		__hsr_set_operstate(master->dev, IF_OPER_LOWERLAYERDOWN);
 }
 
-void hsr_set_carrier(struct net_device *hsr_dev, struct net_device *slave1,
-		     struct net_device *slave2)
+static bool hsr_check_carrier(struct hsr_port *master)
 {
-	if (is_slave_up(slave1) || is_slave_up(slave2))
-		netif_carrier_on(hsr_dev);
+	struct hsr_port *port;
+	bool has_carrier;
+
+	has_carrier = false;
+
+	rcu_read_lock();
+	hsr_for_each_port(master->hsr, port)
+		if ((port->type != HSR_PT_MASTER) && is_slave_up(port->dev)) {
+			has_carrier = true;
+			break;
+		}
+	rcu_read_unlock();
+
+	if (has_carrier)
+		netif_carrier_on(master->dev);
 	else
-		netif_carrier_off(hsr_dev);
+		netif_carrier_off(master->dev);
+
+	return has_carrier;
 }
 
 
-void hsr_check_announce(struct net_device *hsr_dev, int old_operstate)
+static void hsr_check_announce(struct net_device *hsr_dev,
+			       unsigned char old_operstate)
 {
-	struct hsr_priv *hsr_priv;
+	struct hsr_priv *hsr;
 
-	hsr_priv = netdev_priv(hsr_dev);
+	hsr = netdev_priv(hsr_dev);
 
 	if ((hsr_dev->operstate == IF_OPER_UP) && (old_operstate != IF_OPER_UP)) {
 		/* Went up */
-		hsr_priv->announce_count = 0;
-		hsr_priv->announce_timer.expires = jiffies +
+		hsr->announce_count = 0;
+		hsr->announce_timer.expires = jiffies +
 				msecs_to_jiffies(HSR_ANNOUNCE_INTERVAL);
-		add_timer(&hsr_priv->announce_timer);
+		add_timer(&hsr->announce_timer);
 	}
 
 	if ((hsr_dev->operstate != IF_OPER_UP) && (old_operstate == IF_OPER_UP))
 		/* Went down */
-		del_timer(&hsr_priv->announce_timer);
+		del_timer(&hsr->announce_timer);
 }
 
-
-int hsr_get_max_mtu(struct hsr_priv *hsr_priv)
+void hsr_check_carrier_and_operstate(struct hsr_priv *hsr)
 {
-	int mtu_max;
+	struct hsr_port *master;
+	unsigned char old_operstate;
+	bool has_carrier;
 
-	if (hsr_priv->slave[0] && hsr_priv->slave[1])
-		mtu_max = min(hsr_priv->slave[0]->mtu, hsr_priv->slave[1]->mtu);
-	else if (hsr_priv->slave[0])
-		mtu_max = hsr_priv->slave[0]->mtu;
-	else if (hsr_priv->slave[1])
-		mtu_max = hsr_priv->slave[1]->mtu;
-	else
-		mtu_max = HSR_TAGLEN;
-
-	return mtu_max - HSR_TAGLEN;
+	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
+	/* netif_stacked_transfer_operstate() cannot be used here since
+	 * it doesn't set IF_OPER_LOWERLAYERDOWN (?)
+	 */
+	old_operstate = master->dev->operstate;
+	has_carrier = hsr_check_carrier(master);
+	hsr_set_operstate(master, has_carrier);
+	hsr_check_announce(master->dev, old_operstate);
 }
 
+int hsr_get_max_mtu(struct hsr_priv *hsr)
+{
+	unsigned int mtu_max;
+	struct hsr_port *port;
+
+	mtu_max = ETH_DATA_LEN;
+	rcu_read_lock();
+	hsr_for_each_port(hsr, port)
+		if (port->type != HSR_PT_MASTER)
+			mtu_max = min(port->dev->mtu, mtu_max);
+	rcu_read_unlock();
+
+	if (mtu_max < HSR_HLEN)
+		return 0;
+	return mtu_max - HSR_HLEN;
+}
+
+
 static int hsr_dev_change_mtu(struct net_device *dev, int new_mtu)
 {
-	struct hsr_priv *hsr_priv;
+	struct hsr_priv *hsr;
+	struct hsr_port *master;
 
-	hsr_priv = netdev_priv(dev);
+	hsr = netdev_priv(dev);
+	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
 
-	if (new_mtu > hsr_get_max_mtu(hsr_priv)) {
-		netdev_info(hsr_priv->dev, "A HSR master's MTU cannot be greater than the smallest MTU of its slaves minus the HSR Tag length (%d octets).\n",
-			    HSR_TAGLEN);
+	if (new_mtu > hsr_get_max_mtu(hsr)) {
+		netdev_info(master->dev, "A HSR master's MTU cannot be greater than the smallest MTU of its slaves minus the HSR Tag length (%d octets).\n",
+			    HSR_HLEN);
 		return -EINVAL;
 	}
 
@@ -124,164 +158,95 @@ static int hsr_dev_change_mtu(struct net_device *dev, int new_mtu)
 
 static int hsr_dev_open(struct net_device *dev)
 {
-	struct hsr_priv *hsr_priv;
-	int i;
-	char *slave_name;
+	struct hsr_priv *hsr;
+	struct hsr_port *port;
+	char designation;
 
-	hsr_priv = netdev_priv(dev);
+	hsr = netdev_priv(dev);
+	designation = '\0';
 
-	for (i = 0; i < HSR_MAX_SLAVE; i++) {
-		if (hsr_priv->slave[i])
-			slave_name = hsr_priv->slave[i]->name;
-		else
-			slave_name = "null";
-
-		if (!is_slave_up(hsr_priv->slave[i]))
-			netdev_warn(dev, "Slave %c (%s) is not up; please bring it up to get a working HSR network\n",
-				    'A' + i, slave_name);
+	rcu_read_lock();
+	hsr_for_each_port(hsr, port) {
+		if (port->type == HSR_PT_MASTER)
+			continue;
+		switch (port->type) {
+		case HSR_PT_SLAVE_A:
+			designation = 'A';
+			break;
+		case HSR_PT_SLAVE_B:
+			designation = 'B';
+			break;
+		default:
+			designation = '?';
+		}
+		if (!is_slave_up(port->dev))
+			netdev_warn(dev, "Slave %c (%s) is not up; please bring it up to get a fully working HSR network\n",
+				    designation, port->dev->name);
 	}
+	rcu_read_unlock();
+
+	if (designation == '\0')
+		netdev_warn(dev, "No slave devices configured\n");
 
 	return 0;
 }
 
+
 static int hsr_dev_close(struct net_device *dev)
 {
-	/* Nothing to do here. We could try to restore the state of the slaves
-	 * to what they were before being changed by the hsr master dev's state,
-	 * but they might have been changed manually in the mean time too, so
-	 * taking them up or down here might be confusing and is probably not a
-	 * good idea.
-	 */
+	/* Nothing to do here. */
 	return 0;
 }
 
 
-static void hsr_fill_tag(struct hsr_ethhdr *hsr_ethhdr, struct hsr_priv *hsr_priv)
+static netdev_features_t hsr_features_recompute(struct hsr_priv *hsr,
+						netdev_features_t features)
 {
-	unsigned long irqflags;
+	netdev_features_t mask;
+	struct hsr_port *port;
 
-	/* 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);
+	mask = features;
 
-	/* 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."
+	/* Mask out all features that, if supported by one device, should be
+	 * enabled for all devices (see NETIF_F_ONE_FOR_ALL).
 	 *
-	 * 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...
+	 * Anything that's off in mask will not be enabled - so only things
+	 * that were in features originally, and also is in NETIF_F_ONE_FOR_ALL,
+	 * may become enabled.
 	 */
-	set_hsr_tag_LSDU_size(&hsr_ethhdr->hsr_tag, 0);
+	features &= ~NETIF_F_ONE_FOR_ALL;
+	hsr_for_each_port(hsr, port)
+		features = netdev_increment_features(features,
+						     port->dev->features,
+						     mask);
 
-	spin_lock_irqsave(&hsr_priv->seqnr_lock, irqflags);
-	hsr_ethhdr->hsr_tag.sequence_nr = htons(hsr_priv->sequence_nr);
-	hsr_priv->sequence_nr++;
-	spin_unlock_irqrestore(&hsr_priv->seqnr_lock, irqflags);
-
-	hsr_ethhdr->hsr_tag.encap_proto = hsr_ethhdr->ethhdr.h_proto;
-
-	hsr_ethhdr->ethhdr.h_proto = htons(ETH_P_PRP);
+	return features;
 }
 
-static int slave_xmit(struct sk_buff *skb, struct hsr_priv *hsr_priv,
-		      enum hsr_dev_idx dev_idx)
+static netdev_features_t hsr_fix_features(struct net_device *dev,
+					  netdev_features_t features)
 {
-	struct hsr_ethhdr *hsr_ethhdr;
+	struct hsr_priv *hsr = netdev_priv(dev);
 
-	hsr_ethhdr = (struct hsr_ethhdr *) skb->data;
-
-	skb->dev = hsr_priv->slave[dev_idx];
-
-	hsr_addr_subst_dest(hsr_priv, &hsr_ethhdr->ethhdr, dev_idx);
-
-	/* 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);
+	return hsr_features_recompute(hsr, features);
 }
 
 
 static int hsr_dev_xmit(struct sk_buff *skb, struct net_device *dev)
 {
-	struct hsr_priv *hsr_priv;
-	struct hsr_ethhdr *hsr_ethhdr;
-	struct sk_buff *skb2;
-	int res1, res2;
+	struct hsr_priv *hsr = netdev_priv(dev);
+	struct hsr_port *master;
 
-	hsr_priv = 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_priv);
-		skb->protocol = htons(ETH_P_PRP);
-	}
-
-	skb2 = pskb_copy(skb, GFP_ATOMIC);
-
-	res1 = NET_XMIT_DROP;
-	if (likely(hsr_priv->slave[HSR_DEV_SLAVE_A]))
-		res1 = slave_xmit(skb, hsr_priv, HSR_DEV_SLAVE_A);
-
-	res2 = NET_XMIT_DROP;
-	if (likely(skb2 && hsr_priv->slave[HSR_DEV_SLAVE_B]))
-		res2 = slave_xmit(skb2, hsr_priv, HSR_DEV_SLAVE_B);
-
-	if (likely(res1 == NET_XMIT_SUCCESS || res1 == NET_XMIT_CN ||
-		   res2 == NET_XMIT_SUCCESS || res2 == NET_XMIT_CN)) {
-		hsr_priv->dev->stats.tx_packets++;
-		hsr_priv->dev->stats.tx_bytes += skb->len;
-	} else {
-		hsr_priv->dev->stats.tx_dropped++;
-	}
+	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
+	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_TAGLEN + ETH_HLEN)
-		return -ENOBUFS;
-	skb_push(skb, HSR_TAGLEN);
-
-	/* To allow VLAN/HSR combos we should probably use
-	 * res = dev_hard_header(skb, dev, type, daddr, saddr, len + HSR_TAGLEN);
-	 * 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_TAGLEN);
-	if (res <= 0)
-		return res;
-	skb_reset_mac_header(skb);
-
-	return res + HSR_TAGLEN;
-}
-
-
 static const struct header_ops hsr_header_ops = {
-	.create	 = hsr_header_create,
+	.create	 = eth_header,
 	.parse	 = eth_header_parse,
 };
 
@@ -291,67 +256,63 @@ static const struct header_ops hsr_header_ops = {
  */
 static int hsr_pad(int size)
 {
-	const int min_size = ETH_ZLEN - HSR_TAGLEN - ETH_HLEN;
+	const int min_size = ETH_ZLEN - HSR_HLEN - ETH_HLEN;
 
 	if (size >= min_size)
 		return 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_priv;
 	struct sk_buff *skb;
 	int hlen, tlen;
 	struct hsr_sup_tag *hsr_stag;
 	struct hsr_sup_payload *hsr_sp;
 	unsigned long irqflags;
 
-	hlen = LL_RESERVED_SPACE(hsr_dev);
-	tlen = hsr_dev->needed_tailroom;
+	hlen = LL_RESERVED_SPACE(master->dev);
+	tlen = master->dev->needed_tailroom;
 	skb = alloc_skb(hsr_pad(sizeof(struct hsr_sup_payload)) + hlen + tlen,
 			GFP_ATOMIC);
 
 	if (skb == NULL)
 		return;
 
-	hsr_priv = netdev_priv(hsr_dev);
-
 	skb_reserve(skb, hlen);
 
-	skb->dev = hsr_dev;
+	skb->dev = master->dev;
 	skb->protocol = htons(ETH_P_PRP);
 	skb->priority = TC_PRIO_CONTROL;
 
 	if (dev_hard_header(skb, skb->dev, ETH_P_PRP,
-			    hsr_priv->sup_multicast_addr,
-			    skb->dev->dev_addr, skb->len) < 0)
+			    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_priv->seqnr_lock, irqflags);
-	hsr_stag->sequence_nr = htons(hsr_priv->sequence_nr);
-	hsr_priv->sequence_nr++;
-	spin_unlock_irqrestore(&hsr_priv->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, hsr_dev->dev_addr);
+	ether_addr_copy(hsr_sp->MacAddressA, master->dev->dev_addr);
 
-	dev_queue_xmit(skb);
+	hsr_forward_skb(skb, master);
 	return;
 
 out:
+	WARN_ON_ONCE("HSR: Could not send supervision frame\n");
 	kfree_skb(skb);
 }
 
@@ -360,59 +321,32 @@ out:
  */
 static void hsr_announce(unsigned long data)
 {
-	struct hsr_priv *hsr_priv;
+	struct hsr_priv *hsr;
+	struct hsr_port *master;
 
-	hsr_priv = (struct hsr_priv *) data;
+	hsr = (struct hsr_priv *) data;
 
-	if (hsr_priv->announce_count < 3) {
-		send_hsr_supervision_frame(hsr_priv->dev, HSR_TLV_ANNOUNCE);
-		hsr_priv->announce_count++;
+	rcu_read_lock();
+	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
+
+	if (hsr->announce_count < 3) {
+		send_hsr_supervision_frame(master, HSR_TLV_ANNOUNCE);
+		hsr->announce_count++;
 	} else {
-		send_hsr_supervision_frame(hsr_priv->dev, HSR_TLV_LIFE_CHECK);
+		send_hsr_supervision_frame(master, HSR_TLV_LIFE_CHECK);
 	}
 
-	if (hsr_priv->announce_count < 3)
-		hsr_priv->announce_timer.expires = jiffies +
+	if (hsr->announce_count < 3)
+		hsr->announce_timer.expires = jiffies +
 				msecs_to_jiffies(HSR_ANNOUNCE_INTERVAL);
 	else
-		hsr_priv->announce_timer.expires = jiffies +
+		hsr->announce_timer.expires = jiffies +
 				msecs_to_jiffies(HSR_LIFE_CHECK_INTERVAL);
 
-	if (is_admin_up(hsr_priv->dev))
-		add_timer(&hsr_priv->announce_timer);
-}
+	if (is_admin_up(master->dev))
+		add_timer(&hsr->announce_timer);
 
-
-static void restore_slaves(struct net_device *hsr_dev)
-{
-	struct hsr_priv *hsr_priv;
-	int i;
-	int res;
-
-	hsr_priv = netdev_priv(hsr_dev);
-
-	rtnl_lock();
-
-	/* Restore promiscuity */
-	for (i = 0; i < HSR_MAX_SLAVE; i++) {
-		if (!hsr_priv->slave[i])
-			continue;
-		res = dev_set_promiscuity(hsr_priv->slave[i], -1);
-		if (res)
-			netdev_info(hsr_dev,
-				    "Cannot restore slave promiscuity (%s, %d)\n",
-				    hsr_priv->slave[i]->name, res);
-	}
-
-	rtnl_unlock();
-}
-
-static void reclaim_hsr_dev(struct rcu_head *rh)
-{
-	struct hsr_priv *hsr_priv;
-
-	hsr_priv = container_of(rh, struct hsr_priv, rcu_head);
-	free_netdev(hsr_priv->dev);
+	rcu_read_unlock();
 }
 
 
@@ -421,14 +355,18 @@ static void reclaim_hsr_dev(struct rcu_head *rh)
  */
 static void hsr_dev_destroy(struct net_device *hsr_dev)
 {
-	struct hsr_priv *hsr_priv;
+	struct hsr_priv *hsr;
+	struct hsr_port *port;
 
-	hsr_priv = netdev_priv(hsr_dev);
+	hsr = netdev_priv(hsr_dev);
+	hsr_for_each_port(hsr, port)
+		hsr_del_port(port);
 
-	del_timer(&hsr_priv->announce_timer);
-	unregister_hsr_master(hsr_priv);    /* calls list_del_rcu on hsr_priv */
-	restore_slaves(hsr_dev);
-	call_rcu(&hsr_priv->rcu_head, reclaim_hsr_dev);   /* reclaim hsr_priv */
+	del_timer_sync(&hsr->prune_timer);
+	del_timer_sync(&hsr->announce_timer);
+
+	synchronize_rcu();
+	free_netdev(hsr_dev);
 }
 
 static const struct net_device_ops hsr_device_ops = {
@@ -436,62 +374,51 @@ static const struct net_device_ops hsr_device_ops = {
 	.ndo_open = hsr_dev_open,
 	.ndo_stop = hsr_dev_close,
 	.ndo_start_xmit = hsr_dev_xmit,
+	.ndo_fix_features = hsr_fix_features,
 };
 
+static struct device_type hsr_type = {
+	.name = "hsr",
+};
 
 void hsr_dev_setup(struct net_device *dev)
 {
 	random_ether_addr(dev->dev_addr);
 
 	ether_setup(dev);
-	dev->header_ops		 = &hsr_header_ops;
-	dev->netdev_ops		 = &hsr_device_ops;
-	dev->tx_queue_len	 = 0;
+	dev->header_ops = &hsr_header_ops;
+	dev->netdev_ops = &hsr_device_ops;
+	SET_NETDEV_DEVTYPE(dev, &hsr_type);
+	dev->tx_queue_len = 0;
 
 	dev->destructor = hsr_dev_destroy;
+
+	dev->hw_features = NETIF_F_SG | NETIF_F_FRAGLIST | NETIF_F_HIGHDMA |
+			   NETIF_F_GSO_MASK | NETIF_F_HW_CSUM |
+			   NETIF_F_HW_VLAN_CTAG_TX;
+
+	dev->features = dev->hw_features;
+
+	/* Prevent recursive tx locking */
+	dev->features |= NETIF_F_LLTX;
+	/* VLAN on top of HSR needs testing and probably some work on
+	 * hsr_header_create() etc.
+	 */
+	dev->features |= NETIF_F_VLAN_CHALLENGED;
+	/* Not sure about this. Taken from bridge code. netdev_features.h says
+	 * it means "Does not change network namespaces".
+	 */
+	dev->features |= NETIF_F_NETNS_LOCAL;
 }
 
 
 /* Return true if dev is a HSR master; return false otherwise.
  */
-bool is_hsr_master(struct net_device *dev)
+inline bool is_hsr_master(struct net_device *dev)
 {
 	return (dev->netdev_ops->ndo_start_xmit == hsr_dev_xmit);
 }
 
-static int check_slave_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 (is_hsr_slave(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;
-}
-
-
 /* Default multicast address for HSR Supervision frames */
 static const unsigned char def_multicast_addr[ETH_ALEN] __aligned(2) = {
 	0x01, 0x15, 0x4e, 0x00, 0x01, 0x00
@@ -500,97 +427,74 @@ static const unsigned char def_multicast_addr[ETH_ALEN] __aligned(2) = {
 int hsr_dev_finalize(struct net_device *hsr_dev, struct net_device *slave[2],
 		     unsigned char multicast_spec)
 {
-	struct hsr_priv *hsr_priv;
-	int i;
+	struct hsr_priv *hsr;
+	struct hsr_port *port;
 	int res;
 
-	hsr_priv = netdev_priv(hsr_dev);
-	hsr_priv->dev = hsr_dev;
-	INIT_LIST_HEAD(&hsr_priv->node_db);
-	INIT_LIST_HEAD(&hsr_priv->self_node_db);
-	for (i = 0; i < HSR_MAX_SLAVE; i++)
-		hsr_priv->slave[i] = slave[i];
+	hsr = netdev_priv(hsr_dev);
+	INIT_LIST_HEAD(&hsr->ports);
+	INIT_LIST_HEAD(&hsr->node_db);
+	INIT_LIST_HEAD(&hsr->self_node_db);
 
-	spin_lock_init(&hsr_priv->seqnr_lock);
+	ether_addr_copy(hsr_dev->dev_addr, slave[0]->dev_addr);
+
+	/* Make sure we recognize frames from ourselves in hsr_rcv() */
+	res = hsr_create_self_node(&hsr->self_node_db, hsr_dev->dev_addr,
+				   slave[1]->dev_addr);
+	if (res < 0)
+		return res;
+
+	spin_lock_init(&hsr->seqnr_lock);
 	/* Overflow soon to find bugs easier: */
-	hsr_priv->sequence_nr = USHRT_MAX - 1024;
+	hsr->sequence_nr = HSR_SEQNR_START;
 
-	init_timer(&hsr_priv->announce_timer);
-	hsr_priv->announce_timer.function = hsr_announce;
-	hsr_priv->announce_timer.data = (unsigned long) hsr_priv;
+	init_timer(&hsr->announce_timer);
+	hsr->announce_timer.function = hsr_announce;
+	hsr->announce_timer.data = (unsigned long) hsr;
 
-	ether_addr_copy(hsr_priv->sup_multicast_addr, def_multicast_addr);
-	hsr_priv->sup_multicast_addr[ETH_ALEN - 1] = multicast_spec;
+	init_timer(&hsr->prune_timer);
+	hsr->prune_timer.function = hsr_prune_nodes;
+	hsr->prune_timer.data = (unsigned long) hsr;
 
-/* FIXME: should I modify the value of these?
- *
- * - hsr_dev->flags - i.e.
- *			IFF_MASTER/SLAVE?
- * - hsr_dev->priv_flags - i.e.
- *			IFF_EBRIDGE?
- *			IFF_TX_SKB_SHARING?
- *			IFF_HSR_MASTER/SLAVE?
- */
+	ether_addr_copy(hsr->sup_multicast_addr, def_multicast_addr);
+	hsr->sup_multicast_addr[ETH_ALEN - 1] = multicast_spec;
 
-	for (i = 0; i < HSR_MAX_SLAVE; i++) {
-		res = check_slave_ok(slave[i]);
-		if (res)
-			return res;
-	}
-
-	hsr_dev->features = slave[0]->features & slave[1]->features;
-	/* Prevent recursive tx locking */
-	hsr_dev->features |= NETIF_F_LLTX;
-	/* VLAN on top of HSR needs testing and probably some work on
-	 * hsr_header_create() etc.
+	/* FIXME: should I modify the value of these?
+	 *
+	 * - hsr_dev->flags - i.e.
+	 *			IFF_MASTER/SLAVE?
+	 * - hsr_dev->priv_flags - i.e.
+	 *			IFF_EBRIDGE?
+	 *			IFF_TX_SKB_SHARING?
+	 *			IFF_HSR_MASTER/SLAVE?
 	 */
-	hsr_dev->features |= NETIF_F_VLAN_CHALLENGED;
-
-	/* Set hsr_dev's MAC address to that of mac_slave1 */
-	ether_addr_copy(hsr_dev->dev_addr, hsr_priv->slave[0]->dev_addr);
-
-	/* Set required header length */
-	for (i = 0; i < HSR_MAX_SLAVE; i++) {
-		if (slave[i]->hard_header_len + HSR_TAGLEN >
-						hsr_dev->hard_header_len)
-			hsr_dev->hard_header_len =
-					slave[i]->hard_header_len + HSR_TAGLEN;
-	}
-
-	/* MTU */
-	for (i = 0; i < HSR_MAX_SLAVE; i++)
-		if (slave[i]->mtu - HSR_TAGLEN < hsr_dev->mtu)
-			hsr_dev->mtu = slave[i]->mtu - HSR_TAGLEN;
 
 	/* Make sure the 1st call to netif_carrier_on() gets through */
 	netif_carrier_off(hsr_dev);
 
-	/* Promiscuity */
-	for (i = 0; i < HSR_MAX_SLAVE; i++) {
-		res = dev_set_promiscuity(slave[i], 1);
-		if (res) {
-			netdev_info(hsr_dev, "Cannot set slave promiscuity (%s, %d)\n",
-				    slave[i]->name, res);
-			goto fail;
-		}
-	}
-
-	/* Make sure we recognize frames from ourselves in hsr_rcv() */
-	res = hsr_create_self_node(&hsr_priv->self_node_db,
-					hsr_dev->dev_addr,
-					hsr_priv->slave[1]->dev_addr);
-	if (res < 0)
-		goto fail;
+	res = hsr_add_port(hsr, hsr_dev, HSR_PT_MASTER);
+	if (res)
+		return res;
 
 	res = register_netdevice(hsr_dev);
 	if (res)
 		goto fail;
 
-	register_hsr_master(hsr_priv);
+	res = hsr_add_port(hsr, slave[0], HSR_PT_SLAVE_A);
+	if (res)
+		goto fail;
+	res = hsr_add_port(hsr, slave[1], HSR_PT_SLAVE_B);
+	if (res)
+		goto fail;
+
+	hsr->prune_timer.expires = jiffies + msecs_to_jiffies(PRUNE_PERIOD);
+	add_timer(&hsr->prune_timer);
 
 	return 0;
 
 fail:
-	restore_slaves(hsr_dev);
+	hsr_for_each_port(hsr, port)
+		hsr_del_port(port);
+
 	return res;
 }
diff --git a/net/hsr/hsr_device.h b/net/hsr/hsr_device.h
index 2c7148e73914..108a5d59d2a6 100644
--- a/net/hsr/hsr_device.h
+++ b/net/hsr/hsr_device.h
@@ -1,4 +1,4 @@
-/* Copyright 2011-2013 Autronica Fire and Security AS
+/* 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
@@ -6,7 +6,7 @@
  * any later version.
  *
  * Author(s):
- *	2011-2013 Arvid Brodin, arvid.brodin@xdin.com
+ *	2011-2014 Arvid Brodin, arvid.brodin@alten.se
  */
 
 #ifndef __HSR_DEVICE_H
@@ -18,12 +18,8 @@
 void hsr_dev_setup(struct net_device *dev);
 int hsr_dev_finalize(struct net_device *hsr_dev, struct net_device *slave[2],
 		     unsigned char multicast_spec);
-void hsr_set_operstate(struct net_device *hsr_dev, struct net_device *slave1,
-		       struct net_device *slave2);
-void hsr_set_carrier(struct net_device *hsr_dev, struct net_device *slave1,
-		     struct net_device *slave2);
-void hsr_check_announce(struct net_device *hsr_dev, int old_operstate);
+void hsr_check_carrier_and_operstate(struct hsr_priv *hsr);
 bool is_hsr_master(struct net_device *dev);
-int hsr_get_max_mtu(struct hsr_priv *hsr_priv);
+int hsr_get_max_mtu(struct hsr_priv *hsr);
 
 #endif /* __HSR_DEVICE_H */
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 83e58449366a..c709c13842cb 100644
--- a/net/hsr/hsr_framereg.c
+++ b/net/hsr/hsr_framereg.c
@@ -1,4 +1,4 @@
-/* Copyright 2011-2013 Autronica Fire and Security AS
+/* 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
@@ -6,7 +6,7 @@
  * any later version.
  *
  * Author(s):
- *	2011-2013 Arvid Brodin, arvid.brodin@xdin.com
+ *	2011-2014 Arvid Brodin, arvid.brodin@alten.se
  *
  * The HSR spec says never to forward the same frame twice on the same
  * interface. A frame is identified by its source MAC address and its HSR
@@ -23,260 +23,22 @@
 #include "hsr_netlink.h"
 
 
-struct node_entry {
-	struct list_head mac_list;
-	unsigned char	MacAddressA[ETH_ALEN];
-	unsigned char	MacAddressB[ETH_ALEN];
-	enum hsr_dev_idx   AddrB_if;	/* The local slave through which AddrB
-					 * frames are received from this node
-					 */
-	unsigned long	time_in[HSR_MAX_SLAVE];
-	bool		time_in_stale[HSR_MAX_SLAVE];
-	u16		seq_out[HSR_MAX_DEV];
-	struct rcu_head rcu_head;
+struct hsr_node {
+	struct list_head	mac_list;
+	unsigned char		MacAddressA[ETH_ALEN];
+	unsigned char		MacAddressB[ETH_ALEN];
+	/* Local slave through which AddrB frames are received from this node */
+	enum hsr_port_type	AddrB_port;
+	unsigned long		time_in[HSR_PT_PORTS];
+	bool			time_in_stale[HSR_PT_PORTS];
+	u16			seq_out[HSR_PT_PORTS];
+	struct rcu_head		rcu_head;
 };
 
+
 /*	TODO: use hash lists for mac addresses (linux/jhash.h)?    */
 
 
-
-/* Search for mac entry. Caller must hold rcu read lock.
- */
-static struct node_entry *find_node_by_AddrA(struct list_head *node_db,
-					     const unsigned char addr[ETH_ALEN])
-{
-	struct node_entry *node;
-
-	list_for_each_entry_rcu(node, node_db, mac_list) {
-		if (ether_addr_equal(node->MacAddressA, addr))
-			return node;
-	}
-
-	return NULL;
-}
-
-
-/* Search for mac entry. Caller must hold rcu read lock.
- */
-static struct node_entry *find_node_by_AddrB(struct list_head *node_db,
-					     const unsigned char addr[ETH_ALEN])
-{
-	struct node_entry *node;
-
-	list_for_each_entry_rcu(node, node_db, mac_list) {
-		if (ether_addr_equal(node->MacAddressB, addr))
-			return node;
-	}
-
-	return NULL;
-}
-
-
-/* Search for mac entry. Caller must hold rcu read lock.
- */
-struct node_entry *hsr_find_node(struct list_head *node_db, struct sk_buff *skb)
-{
-	struct node_entry *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))
-			return node;
-	}
-
-	return NULL;
-}
-
-
-/* Helper for device init; the self_node_db is used in hsr_rcv() to recognize
- * frames from self that's been looped over the HSR ring.
- */
-int hsr_create_self_node(struct list_head *self_node_db,
-			 unsigned char addr_a[ETH_ALEN],
-			 unsigned char addr_b[ETH_ALEN])
-{
-	struct node_entry *node, *oldnode;
-
-	node = kmalloc(sizeof(*node), GFP_KERNEL);
-	if (!node)
-		return -ENOMEM;
-
-	ether_addr_copy(node->MacAddressA, addr_a);
-	ether_addr_copy(node->MacAddressB, addr_b);
-
-	rcu_read_lock();
-	oldnode = list_first_or_null_rcu(self_node_db,
-						struct node_entry, mac_list);
-	if (oldnode) {
-		list_replace_rcu(&oldnode->mac_list, &node->mac_list);
-		rcu_read_unlock();
-		synchronize_rcu();
-		kfree(oldnode);
-	} else {
-		rcu_read_unlock();
-		list_add_tail_rcu(&node->mac_list, self_node_db);
-	}
-
-	return 0;
-}
-
-
-/* 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.
- */
-struct node_entry *hsr_merge_node(struct hsr_priv *hsr_priv,
-				  struct node_entry *node,
-				  struct sk_buff *skb,
-				  enum hsr_dev_idx dev_idx)
-{
-	struct hsr_sup_payload *hsr_sp;
-	struct hsr_ethhdr_sp *hsr_ethsup;
-	int i;
-	unsigned long now;
-
-	hsr_ethsup = (struct hsr_ethhdr_sp *) skb_mac_header(skb);
-	hsr_sp = (struct hsr_sup_payload *) skb->data;
-
-	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 && (dev_idx == node->AddrB_if) &&
-	    !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 && (dev_idx != node->AddrB_if) &&
-	    (node->AddrB_if != HSR_DEV_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_priv->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_if = dev_idx;
-		return node;
-	}
-
-	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_if = dev_idx;
-	else
-		node->AddrB_if = HSR_DEV_NONE;
-
-	/* We are only interested in time diffs here, so use current jiffies
-	 * as initialization. (0 could trigger an spurious ring error warning).
-	 */
-	now = jiffies;
-	for (i = 0; i < HSR_MAX_SLAVE; i++)
-		node->time_in[i] = now;
-	for (i = 0; i < HSR_MAX_DEV; i++)
-		node->seq_out[i] = ntohs(hsr_ethsup->hsr_sup.sequence_nr) - 1;
-
-	list_add_tail_rcu(&node->mac_list, &hsr_priv->node_db);
-
-	return node;
-}
-
-
-/* '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
- * 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_priv, struct sk_buff *skb)
-{
-	struct ethhdr *ethhdr;
-	struct node_entry *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_priv->node_db, ethhdr->h_source);
-	if (node)
-		ether_addr_copy(ethhdr->h_source, node->MacAddressA);
-	rcu_read_unlock();
-}
-
-
-/* 'skb' is a frame meant for another host.
- * 'hsr_dev_idx' is the HSR index of the outgoing device
- *
- * Substitute the target (dest) MAC address if necessary, so the it matches the
- * recipient interface MAC address, regardless of whether that is the
- * recipient's A or B interface.
- * 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_priv, struct ethhdr *ethhdr,
-			 enum hsr_dev_idx dev_idx)
-{
-	struct node_entry *node;
-
-	rcu_read_lock();
-	node = find_node_by_AddrA(&hsr_priv->node_db, ethhdr->h_dest);
-	if (node && (node->AddrB_if == dev_idx))
-		ether_addr_copy(ethhdr->h_dest, node->MacAddressB);
-	rcu_read_unlock();
-}
-
-
 /* seq_nr_after(a, b) - return true if a is after (higher in sequence than) b,
  * false otherwise.
  */
@@ -295,16 +57,261 @@ static bool seq_nr_after(u16 a, u16 b)
 #define seq_nr_before_or_eq(a, b)	(!seq_nr_after((a), (b)))
 
 
-void hsr_register_frame_in(struct node_entry *node, enum hsr_dev_idx dev_idx)
+bool hsr_addr_is_self(struct hsr_priv *hsr, unsigned char *addr)
 {
-	if ((dev_idx < 0) || (dev_idx >= HSR_MAX_SLAVE)) {
-		WARN_ONCE(1, "%s: Invalid dev_idx (%d)\n", __func__, dev_idx);
-		return;
+	struct hsr_node *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;
 	}
-	node->time_in[dev_idx] = jiffies;
-	node->time_in_stale[dev_idx] = false;
+
+	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.
+ */
+static struct hsr_node *find_node_by_AddrA(struct list_head *node_db,
+					   const unsigned char addr[ETH_ALEN])
+{
+	struct hsr_node *node;
+
+	list_for_each_entry_rcu(node, node_db, mac_list) {
+		if (ether_addr_equal(node->MacAddressA, addr))
+			return node;
+	}
+
+	return NULL;
+}
+
+
+/* Helper for device init; the self_node_db is used in hsr_rcv() to recognize
+ * frames from self that's been looped over the HSR ring.
+ */
+int hsr_create_self_node(struct list_head *self_node_db,
+			 unsigned char addr_a[ETH_ALEN],
+			 unsigned char addr_b[ETH_ALEN])
+{
+	struct hsr_node *node, *oldnode;
+
+	node = kmalloc(sizeof(*node), GFP_KERNEL);
+	if (!node)
+		return -ENOMEM;
+
+	ether_addr_copy(node->MacAddressA, addr_a);
+	ether_addr_copy(node->MacAddressB, addr_b);
+
+	rcu_read_lock();
+	oldnode = list_first_or_null_rcu(self_node_db,
+						struct hsr_node, mac_list);
+	if (oldnode) {
+		list_replace_rcu(&oldnode->mac_list, &node->mac_list);
+		rcu_read_unlock();
+		synchronize_rcu();
+		kfree(oldnode);
+	} else {
+		rcu_read_unlock();
+		list_add_tail_rcu(&node->mac_list, self_node_db);
+	}
+
+	return 0;
+}
+
+
+/* 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_add_node(struct list_head *node_db, unsigned char addr[],
+			      u16 seq_out)
+{
+	struct hsr_node *node;
+	unsigned long now;
+	int i;
+
+	node = kzalloc(sizeof(*node), GFP_ATOMIC);
+	if (!node)
+		return NULL;
+
+	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).
+	 */
+	now = jiffies;
+	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] = seq_out;
+
+	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 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_node *node, struct sk_buff *skb)
+{
+	if (!skb_mac_header_was_set(skb)) {
+		WARN_ONCE(1, "%s: Mac header not set\n", __func__);
+		return;
+	}
+
+	memcpy(&eth_hdr(skb)->h_source, node->MacAddressA, ETH_ALEN);
+}
+
+/* 'skb' is a frame meant for another host.
+ * '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
+ * recipient's A or B interface.
+ * 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_node *node_src, struct sk_buff *skb,
+			 struct hsr_port *port)
+{
+	struct hsr_node *node_dst;
+
+	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;
+
+	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;
+	}
+
+	ether_addr_copy(eth_hdr(skb)->h_dest, node_dst->MacAddressB);
+}
+
+
+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.
@@ -314,102 +321,87 @@ void hsr_register_frame_in(struct node_entry *node, enum hsr_dev_idx dev_idx)
  *	 0 otherwise, or
  *	 negative error code on error
  */
-int hsr_register_frame_out(struct node_entry *node, enum hsr_dev_idx dev_idx,
-			   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 ((dev_idx < 0) || (dev_idx >= HSR_MAX_DEV)) {
-		WARN_ONCE(1, "%s: Invalid dev_idx (%d)\n", __func__, dev_idx);
-		return -EINVAL;
-	}
-	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[dev_idx]))
+	if (seq_nr_before_or_eq(sequence_nr, node->seq_out[port->type]))
 		return 1;
 
-	node->seq_out[dev_idx] = sequence_nr;
+	node->seq_out[port->type] = sequence_nr;
 	return 0;
 }
 
 
-
-static bool is_late(struct node_entry *node, enum hsr_dev_idx dev_idx)
+static struct hsr_port *get_late_port(struct hsr_priv *hsr,
+				      struct hsr_node *node)
 {
-	enum hsr_dev_idx other;
+	if (node->time_in_stale[HSR_PT_SLAVE_A])
+		return hsr_port_get_hsr(hsr, HSR_PT_SLAVE_A);
+	if (node->time_in_stale[HSR_PT_SLAVE_B])
+		return hsr_port_get_hsr(hsr, HSR_PT_SLAVE_B);
 
-	if (node->time_in_stale[dev_idx])
-		return true;
+	if (time_after(node->time_in[HSR_PT_SLAVE_B],
+		       node->time_in[HSR_PT_SLAVE_A] +
+					msecs_to_jiffies(MAX_SLAVE_DIFF)))
+		return hsr_port_get_hsr(hsr, HSR_PT_SLAVE_A);
+	if (time_after(node->time_in[HSR_PT_SLAVE_A],
+		       node->time_in[HSR_PT_SLAVE_B] +
+					msecs_to_jiffies(MAX_SLAVE_DIFF)))
+		return hsr_port_get_hsr(hsr, HSR_PT_SLAVE_B);
 
-	if (dev_idx == HSR_DEV_SLAVE_A)
-		other = HSR_DEV_SLAVE_B;
-	else
-		other = HSR_DEV_SLAVE_A;
-
-	if (node->time_in_stale[other])
-		return false;
-
-	if (time_after(node->time_in[other], node->time_in[dev_idx] +
-		       msecs_to_jiffies(MAX_SLAVE_DIFF)))
-		return true;
-
-	return false;
+	return NULL;
 }
 
 
 /* Remove stale sequence_nr records. Called by timer every
  * HSR_LIFE_CHECK_INTERVAL (two seconds or so).
  */
-void hsr_prune_nodes(struct hsr_priv *hsr_priv)
+void hsr_prune_nodes(unsigned long data)
 {
-	struct node_entry *node;
+	struct hsr_priv *hsr;
+	struct hsr_node *node;
+	struct hsr_port *port;
 	unsigned long timestamp;
 	unsigned long time_a, time_b;
 
+	hsr = (struct hsr_priv *) data;
+
 	rcu_read_lock();
-	list_for_each_entry_rcu(node, &hsr_priv->node_db, mac_list) {
+	list_for_each_entry_rcu(node, &hsr->node_db, mac_list) {
 		/* Shorthand */
-		time_a = node->time_in[HSR_DEV_SLAVE_A];
-		time_b = node->time_in[HSR_DEV_SLAVE_B];
+		time_a = node->time_in[HSR_PT_SLAVE_A];
+		time_b = node->time_in[HSR_PT_SLAVE_B];
 
 		/* Check for timestamps old enough to risk wrap-around */
 		if (time_after(jiffies, time_a + MAX_JIFFY_OFFSET/2))
-			node->time_in_stale[HSR_DEV_SLAVE_A] = true;
+			node->time_in_stale[HSR_PT_SLAVE_A] = true;
 		if (time_after(jiffies, time_b + MAX_JIFFY_OFFSET/2))
-			node->time_in_stale[HSR_DEV_SLAVE_B] = true;
+			node->time_in_stale[HSR_PT_SLAVE_B] = true;
 
 		/* Get age of newest frame from node.
 		 * At least one time_in is OK here; nodes get pruned long
 		 * before both time_ins can get stale
 		 */
 		timestamp = time_a;
-		if (node->time_in_stale[HSR_DEV_SLAVE_A] ||
-		    (!node->time_in_stale[HSR_DEV_SLAVE_B] &&
+		if (node->time_in_stale[HSR_PT_SLAVE_A] ||
+		    (!node->time_in_stale[HSR_PT_SLAVE_B] &&
 		    time_after(time_b, time_a)))
 			timestamp = time_b;
 
 		/* Warn of ring error only as long as we get frames at all */
 		if (time_is_after_jiffies(timestamp +
 					msecs_to_jiffies(1.5*MAX_SLAVE_DIFF))) {
-
-			if (is_late(node, HSR_DEV_SLAVE_A))
-				hsr_nl_ringerror(hsr_priv, node->MacAddressA,
-						 HSR_DEV_SLAVE_A);
-			else if (is_late(node, HSR_DEV_SLAVE_B))
-				hsr_nl_ringerror(hsr_priv, node->MacAddressA,
-						 HSR_DEV_SLAVE_B);
+			rcu_read_lock();
+			port = get_late_port(hsr, node);
+			if (port != NULL)
+				hsr_nl_ringerror(hsr, node->MacAddressA, port);
+			rcu_read_unlock();
 		}
 
 		/* Prune old entries */
 		if (time_is_before_jiffies(timestamp +
 					msecs_to_jiffies(HSR_NODE_FORGET_TIME))) {
-			hsr_nl_nodedown(hsr_priv, node->MacAddressA);
+			hsr_nl_nodedown(hsr, node->MacAddressA);
 			list_del_rcu(&node->mac_list);
 			/* Note that we need to free this entry later: */
 			kfree_rcu(node, rcu_head);
@@ -419,21 +411,21 @@ void hsr_prune_nodes(struct hsr_priv *hsr_priv)
 }
 
 
-void *hsr_get_next_node(struct hsr_priv *hsr_priv, void *_pos,
+void *hsr_get_next_node(struct hsr_priv *hsr, void *_pos,
 			unsigned char addr[ETH_ALEN])
 {
-	struct node_entry *node;
+	struct hsr_node *node;
 
 	if (!_pos) {
-		node = list_first_or_null_rcu(&hsr_priv->node_db,
-						struct node_entry, mac_list);
+		node = list_first_or_null_rcu(&hsr->node_db,
+					      struct hsr_node, mac_list);
 		if (node)
 			ether_addr_copy(addr, node->MacAddressA);
 		return node;
 	}
 
 	node = _pos;
-	list_for_each_entry_continue_rcu(node, &hsr_priv->node_db, mac_list) {
+	list_for_each_entry_continue_rcu(node, &hsr->node_db, mac_list) {
 		ether_addr_copy(addr, node->MacAddressA);
 		return node;
 	}
@@ -442,7 +434,7 @@ void *hsr_get_next_node(struct hsr_priv *hsr_priv, void *_pos,
 }
 
 
-int hsr_get_node_data(struct hsr_priv *hsr_priv,
+int hsr_get_node_data(struct hsr_priv *hsr,
 		      const unsigned char *addr,
 		      unsigned char addr_b[ETH_ALEN],
 		      unsigned int *addr_b_ifindex,
@@ -451,12 +443,13 @@ int hsr_get_node_data(struct hsr_priv *hsr_priv,
 		      int *if2_age,
 		      u16 *if2_seq)
 {
-	struct node_entry *node;
+	struct hsr_node *node;
+	struct hsr_port *port;
 	unsigned long tdiff;
 
 
 	rcu_read_lock();
-	node = find_node_by_AddrA(&hsr_priv->node_db, addr);
+	node = find_node_by_AddrA(&hsr->node_db, addr);
 	if (!node) {
 		rcu_read_unlock();
 		return -ENOENT;	/* No such entry */
@@ -464,8 +457,8 @@ int hsr_get_node_data(struct hsr_priv *hsr_priv,
 
 	ether_addr_copy(addr_b, node->MacAddressB);
 
-	tdiff = jiffies - node->time_in[HSR_DEV_SLAVE_A];
-	if (node->time_in_stale[HSR_DEV_SLAVE_A])
+	tdiff = jiffies - node->time_in[HSR_PT_SLAVE_A];
+	if (node->time_in_stale[HSR_PT_SLAVE_A])
 		*if1_age = INT_MAX;
 #if HZ <= MSEC_PER_SEC
 	else if (tdiff > msecs_to_jiffies(INT_MAX))
@@ -474,8 +467,8 @@ int hsr_get_node_data(struct hsr_priv *hsr_priv,
 	else
 		*if1_age = jiffies_to_msecs(tdiff);
 
-	tdiff = jiffies - node->time_in[HSR_DEV_SLAVE_B];
-	if (node->time_in_stale[HSR_DEV_SLAVE_B])
+	tdiff = jiffies - node->time_in[HSR_PT_SLAVE_B];
+	if (node->time_in_stale[HSR_PT_SLAVE_B])
 		*if2_age = INT_MAX;
 #if HZ <= MSEC_PER_SEC
 	else if (tdiff > msecs_to_jiffies(INT_MAX))
@@ -485,13 +478,15 @@ int hsr_get_node_data(struct hsr_priv *hsr_priv,
 		*if2_age = jiffies_to_msecs(tdiff);
 
 	/* Present sequence numbers as if they were incoming on interface */
-	*if1_seq = node->seq_out[HSR_DEV_SLAVE_B];
-	*if2_seq = node->seq_out[HSR_DEV_SLAVE_A];
+	*if1_seq = node->seq_out[HSR_PT_SLAVE_B];
+	*if2_seq = node->seq_out[HSR_PT_SLAVE_A];
 
-	if ((node->AddrB_if != HSR_DEV_NONE) && hsr_priv->slave[node->AddrB_if])
-		*addr_b_ifindex = hsr_priv->slave[node->AddrB_if]->ifindex;
-	else
+	if (node->AddrB_port != HSR_PT_NONE) {
+		port = hsr_port_get_hsr(hsr, node->AddrB_port);
+		*addr_b_ifindex = port->dev->ifindex;
+	} else {
 		*addr_b_ifindex = -1;
+	}
 
 	rcu_read_unlock();
 
diff --git a/net/hsr/hsr_framereg.h b/net/hsr/hsr_framereg.h
index e6c4022030ad..438b40f98f5a 100644
--- a/net/hsr/hsr_framereg.h
+++ b/net/hsr/hsr_framereg.h
@@ -1,4 +1,4 @@
-/* Copyright 2011-2013 Autronica Fire and Security AS
+/* 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
@@ -6,42 +6,43 @@
  * any later version.
  *
  * Author(s):
- *	2011-2013 Arvid Brodin, arvid.brodin@xdin.com
+ *	2011-2014 Arvid Brodin, arvid.brodin@alten.se
  */
 
-#ifndef _HSR_FRAMEREG_H
-#define _HSR_FRAMEREG_H
+#ifndef __HSR_FRAMEREG_H
+#define __HSR_FRAMEREG_H
 
 #include "hsr_main.h"
 
-struct node_entry;
+struct hsr_node;
 
-struct node_entry *hsr_find_node(struct list_head *node_db, struct sk_buff *skb);
+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);
 
-struct node_entry *hsr_merge_node(struct hsr_priv *hsr_priv,
-				  struct node_entry *node,
-				  struct sk_buff *skb,
-				  enum hsr_dev_idx dev_idx);
+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_addr_subst_source(struct hsr_priv *hsr_priv, struct sk_buff *skb);
-void hsr_addr_subst_dest(struct hsr_priv *hsr_priv, struct ethhdr *ethhdr,
-			 enum hsr_dev_idx dev_idx);
+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_register_frame_in(struct node_entry *node, enum hsr_dev_idx dev_idx);
-
-int hsr_register_frame_out(struct node_entry *node, enum hsr_dev_idx dev_idx,
-			   struct sk_buff *skb);
-
-void hsr_prune_nodes(struct hsr_priv *hsr_priv);
+void hsr_prune_nodes(unsigned long data);
 
 int hsr_create_self_node(struct list_head *self_node_db,
 			 unsigned char addr_a[ETH_ALEN],
 			 unsigned char addr_b[ETH_ALEN]);
 
-void *hsr_get_next_node(struct hsr_priv *hsr_priv, void *_pos,
+void *hsr_get_next_node(struct hsr_priv *hsr, void *_pos,
 			unsigned char addr[ETH_ALEN]);
 
-int hsr_get_node_data(struct hsr_priv *hsr_priv,
+int hsr_get_node_data(struct hsr_priv *hsr,
 		      const unsigned char *addr,
 		      unsigned char addr_b[ETH_ALEN],
 		      unsigned int *addr_b_ifindex,
@@ -50,4 +51,4 @@ int hsr_get_node_data(struct hsr_priv *hsr_priv,
 		      int *if2_age,
 		      u16 *if2_seq);
 
-#endif /* _HSR_FRAMEREG_H */
+#endif /* __HSR_FRAMEREG_H */
diff --git a/net/hsr/hsr_main.c b/net/hsr/hsr_main.c
index 3fee5218a691..779d28b65417 100644
--- a/net/hsr/hsr_main.c
+++ b/net/hsr/hsr_main.c
@@ -1,4 +1,4 @@
-/* Copyright 2011-2013 Autronica Fire and Security AS
+/* 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
@@ -6,11 +6,7 @@
  * any later version.
  *
  * Author(s):
- *	2011-2013 Arvid Brodin, arvid.brodin@xdin.com
- *
- * In addition to routines for registering and unregistering HSR support, this
- * file also contains the receive routine that handles all incoming frames with
- * Ethertype (protocol) ETH_P_PRP (HSRv0), and network device event handling.
+ *	2011-2014 Arvid Brodin, arvid.brodin@alten.se
  */
 
 #include <linux/netdevice.h>
@@ -21,154 +17,71 @@
 #include "hsr_device.h"
 #include "hsr_netlink.h"
 #include "hsr_framereg.h"
-
-
-/* List of all registered virtual HSR devices */
-static LIST_HEAD(hsr_list);
-
-void register_hsr_master(struct hsr_priv *hsr_priv)
-{
-	list_add_tail_rcu(&hsr_priv->hsr_list, &hsr_list);
-}
-
-void unregister_hsr_master(struct hsr_priv *hsr_priv)
-{
-	struct hsr_priv *hsr_priv_it;
-
-	list_for_each_entry(hsr_priv_it, &hsr_list, hsr_list)
-		if (hsr_priv_it == hsr_priv) {
-			list_del_rcu(&hsr_priv_it->hsr_list);
-			return;
-		}
-}
-
-bool is_hsr_slave(struct net_device *dev)
-{
-	struct hsr_priv *hsr_priv_it;
-
-	list_for_each_entry_rcu(hsr_priv_it, &hsr_list, hsr_list) {
-		if (dev == hsr_priv_it->slave[0])
-			return true;
-		if (dev == hsr_priv_it->slave[1])
-			return true;
-	}
-
-	return false;
-}
-
-
-/* If dev is a HSR slave device, return the virtual master device. Return NULL
- * otherwise.
- */
-static struct hsr_priv *get_hsr_master(struct net_device *dev)
-{
-	struct hsr_priv *hsr_priv;
-
-	rcu_read_lock();
-	list_for_each_entry_rcu(hsr_priv, &hsr_list, hsr_list)
-		if ((dev == hsr_priv->slave[0]) ||
-		    (dev == hsr_priv->slave[1])) {
-			rcu_read_unlock();
-			return hsr_priv;
-		}
-
-	rcu_read_unlock();
-	return NULL;
-}
-
-
-/* If dev is a HSR slave device, return the other slave device. Return NULL
- * otherwise.
- */
-static struct net_device *get_other_slave(struct hsr_priv *hsr_priv,
-					  struct net_device *dev)
-{
-	if (dev == hsr_priv->slave[0])
-		return hsr_priv->slave[1];
-	if (dev == hsr_priv->slave[1])
-		return hsr_priv->slave[0];
-
-	return NULL;
-}
+#include "hsr_slave.h"
 
 
 static int hsr_netdev_notify(struct notifier_block *nb, unsigned long event,
 			     void *ptr)
 {
-	struct net_device *slave, *other_slave;
-	struct hsr_priv *hsr_priv;
-	int old_operstate;
+	struct net_device *dev;
+	struct hsr_port *port, *master;
+	struct hsr_priv *hsr;
 	int mtu_max;
 	int res;
-	struct net_device *dev;
 
 	dev = netdev_notifier_info_to_dev(ptr);
-
-	hsr_priv = get_hsr_master(dev);
-	if (hsr_priv) {
-		/* dev is a slave device */
-		slave = dev;
-		other_slave = get_other_slave(hsr_priv, slave);
-	} else {
+	port = hsr_port_get_rtnl(dev);
+	if (port == NULL) {
 		if (!is_hsr_master(dev))
-			return NOTIFY_DONE;
-		hsr_priv = netdev_priv(dev);
-		slave = hsr_priv->slave[0];
-		other_slave = hsr_priv->slave[1];
+			return NOTIFY_DONE;	/* Not an HSR device */
+		hsr = netdev_priv(dev);
+		port = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
+	} else {
+		hsr = port->hsr;
 	}
 
 	switch (event) {
 	case NETDEV_UP:		/* Administrative state DOWN */
 	case NETDEV_DOWN:	/* Administrative state UP */
 	case NETDEV_CHANGE:	/* Link (carrier) state changes */
-		old_operstate = hsr_priv->dev->operstate;
-		hsr_set_carrier(hsr_priv->dev, slave, other_slave);
-		/* netif_stacked_transfer_operstate() cannot be used here since
-		 * it doesn't set IF_OPER_LOWERLAYERDOWN (?)
-		 */
-		hsr_set_operstate(hsr_priv->dev, slave, other_slave);
-		hsr_check_announce(hsr_priv->dev, old_operstate);
+		hsr_check_carrier_and_operstate(hsr);
 		break;
 	case NETDEV_CHANGEADDR:
-
-		/* This should not happen since there's no ndo_set_mac_address()
-		 * for HSR devices - i.e. not supported.
-		 */
-		if (dev == hsr_priv->dev)
+		if (port->type == HSR_PT_MASTER) {
+			/* This should not happen since there's no
+			 * ndo_set_mac_address() for HSR devices - i.e. not
+			 * supported.
+			 */
 			break;
+		}
 
-		if (dev == hsr_priv->slave[0])
-			ether_addr_copy(hsr_priv->dev->dev_addr,
-					hsr_priv->slave[0]->dev_addr);
+		master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
+
+		if (port->type == HSR_PT_SLAVE_A) {
+			ether_addr_copy(master->dev->dev_addr, dev->dev_addr);
+			call_netdevice_notifiers(NETDEV_CHANGEADDR, master->dev);
+		}
 
 		/* Make sure we recognize frames from ourselves in hsr_rcv() */
-		res = hsr_create_self_node(&hsr_priv->self_node_db,
-					   hsr_priv->dev->dev_addr,
-					   hsr_priv->slave[1] ?
-						hsr_priv->slave[1]->dev_addr :
-						hsr_priv->dev->dev_addr);
+		port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_B);
+		res = hsr_create_self_node(&hsr->self_node_db,
+					   master->dev->dev_addr,
+					   port ?
+						port->dev->dev_addr :
+						master->dev->dev_addr);
 		if (res)
-			netdev_warn(hsr_priv->dev,
+			netdev_warn(master->dev,
 				    "Could not update HSR node address.\n");
-
-		if (dev == hsr_priv->slave[0])
-			call_netdevice_notifiers(NETDEV_CHANGEADDR, hsr_priv->dev);
 		break;
 	case NETDEV_CHANGEMTU:
-		if (dev == hsr_priv->dev)
+		if (port->type == HSR_PT_MASTER)
 			break; /* Handled in ndo_change_mtu() */
-		mtu_max = hsr_get_max_mtu(hsr_priv);
-		if (hsr_priv->dev->mtu > mtu_max)
-			dev_set_mtu(hsr_priv->dev, mtu_max);
+		mtu_max = hsr_get_max_mtu(port->hsr);
+		master = hsr_port_get_hsr(port->hsr, HSR_PT_MASTER);
+		master->dev->mtu = mtu_max;
 		break;
 	case NETDEV_UNREGISTER:
-		if (dev == hsr_priv->slave[0])
-			hsr_priv->slave[0] = NULL;
-		if (dev == hsr_priv->slave[1])
-			hsr_priv->slave[1] = NULL;
-
-		/* There should really be a way to set a new slave device... */
-
+		hsr_del_port(port);
 		break;
 	case NETDEV_PRE_TYPE_CHANGE:
 		/* HSR works only on Ethernet devices. Refuse slave to change
@@ -181,255 +94,16 @@ static int hsr_netdev_notify(struct notifier_block *nb, unsigned long event,
 }
 
 
-static struct timer_list prune_timer;
-
-static void prune_nodes_all(unsigned long data)
+struct hsr_port *hsr_port_get_hsr(struct hsr_priv *hsr, enum hsr_port_type pt)
 {
-	struct hsr_priv *hsr_priv;
+	struct hsr_port *port;
 
-	rcu_read_lock();
-	list_for_each_entry_rcu(hsr_priv, &hsr_list, hsr_list)
-		hsr_prune_nodes(hsr_priv);
-	rcu_read_unlock();
-
-	prune_timer.expires = jiffies + msecs_to_jiffies(PRUNE_PERIOD);
-	add_timer(&prune_timer);
-}
-
-
-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_TAGLEN)))
-		goto err_free;
-
-	hsr_tag = (struct hsr_tag *) skb->data;
-	skb->protocol = hsr_tag->encap_proto;
-	skb_pull(skb, HSR_TAGLEN);
-
-	return skb;
-
-err_free:
-	kfree_skb(skb);
+	hsr_for_each_port(hsr, port)
+		if (port->type == pt)
+			return port;
 	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_priv, struct sk_buff *skb)
-{
-	struct hsr_sup_tag *hsr_stag;
-
-	if (!ether_addr_equal(eth_hdr(skb)->h_dest,
-			      hsr_priv->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
- */
-static int hsr_rcv(struct sk_buff *skb, struct net_device *dev,
-		   struct packet_type *pt, struct net_device *orig_dev)
-{
-	struct hsr_priv *hsr_priv;
-	struct net_device *other_slave;
-	struct node_entry *node;
-	bool deliver_to_self;
-	struct sk_buff *skb_deliver;
-	enum hsr_dev_idx dev_in_idx, dev_other_idx;
-	bool dup_out;
-	int ret;
-
-	hsr_priv = get_hsr_master(dev);
-
-	if (!hsr_priv) {
-		/* Non-HSR-slave device 'dev' is connected to a HSR network */
-		kfree_skb(skb);
-		dev->stats.rx_errors++;
-		return NET_RX_SUCCESS;
-	}
-
-	if (dev == hsr_priv->slave[0]) {
-		dev_in_idx = HSR_DEV_SLAVE_A;
-		dev_other_idx = HSR_DEV_SLAVE_B;
-	} else {
-		dev_in_idx = HSR_DEV_SLAVE_B;
-		dev_other_idx = HSR_DEV_SLAVE_A;
-	}
-
-	node = hsr_find_node(&hsr_priv->self_node_db, skb);
-	if (node) {
-		/* Always kill frames sent by ourselves */
-		kfree_skb(skb);
-		return NET_RX_SUCCESS;
-	}
-
-	/* 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,
-				     hsr_priv->dev->dev_addr)) {
-		skb->pkt_type = PACKET_HOST;
-		deliver_to_self = true;
-	}
-
-
-	rcu_read_lock(); /* node_db */
-	node = hsr_find_node(&hsr_priv->node_db, skb);
-
-	if (is_supervision_frame(hsr_priv, skb)) {
-		skb_pull(skb, sizeof(struct hsr_sup_tag));
-		node = hsr_merge_node(hsr_priv, node, skb, dev_in_idx);
-		if (!node) {
-			rcu_read_unlock(); /* node_db */
-			kfree_skb(skb);
-			hsr_priv->dev->stats.rx_dropped++;
-			return NET_RX_DROP;
-		}
-		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.
-		 */
-		rcu_read_unlock(); /* node_db */
-		kfree_skb(skb);
-		return NET_RX_SUCCESS;
-	}
-
-	/* 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.
-	 */
-	dup_out = hsr_register_frame_out(node, dev_other_idx, skb);
-	if (!dup_out)
-		hsr_register_frame_in(node, dev_in_idx);
-
-	/* Forward this frame? */
-	if (!dup_out && (skb->pkt_type != PACKET_HOST))
-		other_slave = get_other_slave(hsr_priv, dev);
-	else
-		other_slave = NULL;
-
-	if (hsr_register_frame_out(node, HSR_DEV_MASTER, skb))
-		deliver_to_self = false;
-
-	rcu_read_unlock(); /* node_db */
-
-	if (!deliver_to_self && !other_slave) {
-		kfree_skb(skb);
-		/* Circulated frame; silently remove it. */
-		return NET_RX_SUCCESS;
-	}
-
-	skb_deliver = skb;
-	if (deliver_to_self && other_slave) {
-		/* 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;
-			hsr_priv->dev->stats.rx_dropped++;
-		}
-	}
-
-	if (deliver_to_self) {
-		bool multicast_frame;
-
-		skb_deliver = hsr_pull_tag(skb_deliver);
-		if (!skb_deliver) {
-			hsr_priv->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_TAGLEN, 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_TAGLEN;
-		skb_deliver->tail -= HSR_TAGLEN;
-#endif
-		skb_deliver->dev = hsr_priv->dev;
-		hsr_addr_subst_source(hsr_priv, skb_deliver);
-		multicast_frame = (skb_deliver->pkt_type == PACKET_MULTICAST);
-		ret = netif_rx(skb_deliver);
-		if (ret == NET_RX_DROP) {
-			hsr_priv->dev->stats.rx_dropped++;
-		} else {
-			hsr_priv->dev->stats.rx_packets++;
-			hsr_priv->dev->stats.rx_bytes += skb->len;
-			if (multicast_frame)
-				hsr_priv->dev->stats.multicast++;
-		}
-	}
-
-forward:
-	if (other_slave) {
-		skb_push(skb, ETH_HLEN);
-		skb->dev = other_slave;
-		dev_queue_xmit(skb);
-	}
-
-	return NET_RX_SUCCESS;
-}
-
-
-static struct packet_type hsr_pt __read_mostly = {
-	.type = htons(ETH_P_PRP),
-	.func = hsr_rcv,
-};
-
 static struct notifier_block hsr_nb = {
 	.notifier_call = hsr_netdev_notify,	/* Slave event notifications */
 };
@@ -439,18 +113,9 @@ static int __init hsr_init(void)
 {
 	int res;
 
-	BUILD_BUG_ON(sizeof(struct hsr_tag) != HSR_TAGLEN);
-
-	dev_add_pack(&hsr_pt);
-
-	init_timer(&prune_timer);
-	prune_timer.function = prune_nodes_all;
-	prune_timer.data = 0;
-	prune_timer.expires = jiffies + msecs_to_jiffies(PRUNE_PERIOD);
-	add_timer(&prune_timer);
+	BUILD_BUG_ON(sizeof(struct hsr_tag) != HSR_HLEN);
 
 	register_netdevice_notifier(&hsr_nb);
-
 	res = hsr_netlink_init();
 
 	return res;
@@ -459,9 +124,7 @@ static int __init hsr_init(void)
 static void __exit hsr_exit(void)
 {
 	unregister_netdevice_notifier(&hsr_nb);
-	del_timer_sync(&prune_timer);
 	hsr_netlink_exit();
-	dev_remove_pack(&hsr_pt);
 }
 
 module_init(hsr_init);
diff --git a/net/hsr/hsr_main.h b/net/hsr/hsr_main.h
index 56fe060c0ab1..5a9c69962ded 100644
--- a/net/hsr/hsr_main.h
+++ b/net/hsr/hsr_main.h
@@ -1,4 +1,4 @@
-/* Copyright 2011-2013 Autronica Fire and Security AS
+/* 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
@@ -6,11 +6,11 @@
  * any later version.
  *
  * Author(s):
- *	2011-2013 Arvid Brodin, arvid.brodin@xdin.com
+ *	2011-2014 Arvid Brodin, arvid.brodin@alten.se
  */
 
-#ifndef _HSR_PRIVATE_H
-#define _HSR_PRIVATE_H
+#ifndef __HSR_PRIVATE_H
+#define __HSR_PRIVATE_H
 
 #include <linux/netdevice.h>
 #include <linux/list.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
@@ -46,16 +47,16 @@
  * path, LSDU_size, sequence Nr }. But we let eth_header() create { h_dest,
  * h_source, h_proto = 0x88FB }, and add { path, LSDU_size, sequence Nr,
  * encapsulated protocol } instead.
+ *
+ * Field names as defined in the IEC:2010 standard for HSR.
  */
-#define HSR_TAGLEN	6
-
-/* Field names below as defined in the IEC:2010 standard for HSR. */
 struct hsr_tag {
 	__be16		path_and_LSDU_size;
 	__be16		sequence_nr;
 	__be16		encap_proto;
 } __packed;
 
+#define HSR_HLEN	6
 
 /* The helper functions below assumes that 'path' occupies the 4 most
  * significant bits of the 16-bit field shared by 'path' and 'LSDU_size' (or
@@ -136,31 +137,47 @@ struct hsr_ethhdr_sp {
 } __packed;
 
 
-enum hsr_dev_idx {
-	HSR_DEV_NONE = -1,
-	HSR_DEV_SLAVE_A = 0,
-	HSR_DEV_SLAVE_B,
-	HSR_DEV_MASTER,
+enum hsr_port_type {
+	HSR_PT_NONE = 0,	/* Must be 0, used by framereg */
+	HSR_PT_SLAVE_A,
+	HSR_PT_SLAVE_B,
+	HSR_PT_INTERLINK,
+	HSR_PT_MASTER,
+	HSR_PT_PORTS,	/* This must be the last item in the enum */
+};
+
+struct hsr_port {
+	struct list_head	port_list;
+	struct net_device	*dev;
+	struct hsr_priv		*hsr;
+	enum hsr_port_type	type;
 };
-#define HSR_MAX_SLAVE	(HSR_DEV_SLAVE_B + 1)
-#define HSR_MAX_DEV	(HSR_DEV_MASTER + 1)
 
 struct hsr_priv {
-	struct list_head	hsr_list;	/* List of hsr devices */
 	struct rcu_head		rcu_head;
-	struct net_device	*dev;
-	struct net_device	*slave[HSR_MAX_SLAVE];
-	struct list_head	node_db;	/* Other HSR nodes */
+	struct list_head	ports;
+	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;
 	int announce_count;
 	u16 sequence_nr;
 	spinlock_t seqnr_lock;			/* locking for sequence_nr */
 	unsigned char		sup_multicast_addr[ETH_ALEN];
 };
 
-void register_hsr_master(struct hsr_priv *hsr_priv);
-void unregister_hsr_master(struct hsr_priv *hsr_priv);
-bool is_hsr_slave(struct net_device *dev);
+#define hsr_for_each_port(hsr, port) \
+	list_for_each_entry_rcu((port), &(hsr)->ports, port_list)
 
-#endif /*  _HSR_PRIVATE_H */
+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 01a5261ac7a5..a2c7e4c0ac1e 100644
--- a/net/hsr/hsr_netlink.c
+++ b/net/hsr/hsr_netlink.c
@@ -1,4 +1,4 @@
-/* Copyright 2011-2013 Autronica Fire and Security AS
+/* 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
@@ -6,7 +6,7 @@
  * any later version.
  *
  * Author(s):
- *	2011-2013 Arvid Brodin, arvid.brodin@xdin.com
+ *	2011-2014 Arvid Brodin, arvid.brodin@alten.se
  *
  * Routines for handling Netlink messages for HSR.
  */
@@ -37,13 +37,17 @@ static int hsr_newlink(struct net *src_net, struct net_device *dev,
 	struct net_device *link[2];
 	unsigned char multicast_spec;
 
+	if (!data) {
+		netdev_info(dev, "HSR: No slave devices specified\n");
+		return -EINVAL;
+	}
 	if (!data[IFLA_HSR_SLAVE1]) {
-		netdev_info(dev, "IFLA_HSR_SLAVE1 missing!\n");
+		netdev_info(dev, "HSR: Slave1 device not specified\n");
 		return -EINVAL;
 	}
 	link[0] = __dev_get_by_index(src_net, nla_get_u32(data[IFLA_HSR_SLAVE1]));
 	if (!data[IFLA_HSR_SLAVE2]) {
-		netdev_info(dev, "IFLA_HSR_SLAVE2 missing!\n");
+		netdev_info(dev, "HSR: Slave2 device not specified\n");
 		return -EINVAL;
 	}
 	link[1] = __dev_get_by_index(src_net, nla_get_u32(data[IFLA_HSR_SLAVE2]));
@@ -63,21 +67,33 @@ static int hsr_newlink(struct net *src_net, struct net_device *dev,
 
 static int hsr_fill_info(struct sk_buff *skb, const struct net_device *dev)
 {
-	struct hsr_priv *hsr_priv;
+	struct hsr_priv *hsr;
+	struct hsr_port *port;
+	int res;
 
-	hsr_priv = netdev_priv(dev);
+	hsr = netdev_priv(dev);
 
-	if (hsr_priv->slave[0])
-		if (nla_put_u32(skb, IFLA_HSR_SLAVE1, hsr_priv->slave[0]->ifindex))
-			goto nla_put_failure;
+	res = 0;
 
-	if (hsr_priv->slave[1])
-		if (nla_put_u32(skb, IFLA_HSR_SLAVE2, hsr_priv->slave[1]->ifindex))
-			goto nla_put_failure;
+	rcu_read_lock();
+	port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_A);
+	if (port)
+		res = nla_put_u32(skb, IFLA_HSR_SLAVE1, port->dev->ifindex);
+	rcu_read_unlock();
+	if (res)
+		goto nla_put_failure;
+
+	rcu_read_lock();
+	port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_B);
+	if (port)
+		res = nla_put_u32(skb, IFLA_HSR_SLAVE2, port->dev->ifindex);
+	rcu_read_unlock();
+	if (res)
+		goto nla_put_failure;
 
 	if (nla_put(skb, IFLA_HSR_SUPERVISION_ADDR, ETH_ALEN,
-		    hsr_priv->sup_multicast_addr) ||
-	    nla_put_u16(skb, IFLA_HSR_SEQ_NR, hsr_priv->sequence_nr))
+		    hsr->sup_multicast_addr) ||
+	    nla_put_u16(skb, IFLA_HSR_SEQ_NR, hsr->sequence_nr))
 		goto nla_put_failure;
 
 	return 0;
@@ -128,13 +144,13 @@ static const struct genl_multicast_group hsr_mcgrps[] = {
  * over one of the slave interfaces. This would indicate an open network ring
  * (i.e. a link has failed somewhere).
  */
-void hsr_nl_ringerror(struct hsr_priv *hsr_priv, unsigned char addr[ETH_ALEN],
-		      enum hsr_dev_idx dev_idx)
+void hsr_nl_ringerror(struct hsr_priv *hsr, unsigned char addr[ETH_ALEN],
+		      struct hsr_port *port)
 {
 	struct sk_buff *skb;
 	void *msg_head;
+	struct hsr_port *master;
 	int res;
-	int ifindex;
 
 	skb = genlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC);
 	if (!skb)
@@ -148,11 +164,7 @@ void hsr_nl_ringerror(struct hsr_priv *hsr_priv, unsigned char addr[ETH_ALEN],
 	if (res < 0)
 		goto nla_put_failure;
 
-	if (hsr_priv->slave[dev_idx])
-		ifindex = hsr_priv->slave[dev_idx]->ifindex;
-	else
-		ifindex = -1;
-	res = nla_put_u32(skb, HSR_A_IFINDEX, ifindex);
+	res = nla_put_u32(skb, HSR_A_IFINDEX, port->dev->ifindex);
 	if (res < 0)
 		goto nla_put_failure;
 
@@ -165,16 +177,20 @@ nla_put_failure:
 	kfree_skb(skb);
 
 fail:
-	netdev_warn(hsr_priv->dev, "Could not send HSR ring error message\n");
+	rcu_read_lock();
+	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
+	netdev_warn(master->dev, "Could not send HSR ring error message\n");
+	rcu_read_unlock();
 }
 
 /* This is called when we haven't heard from the node with MAC address addr for
  * some time (just before the node is removed from the node table/list).
  */
-void hsr_nl_nodedown(struct hsr_priv *hsr_priv, unsigned char addr[ETH_ALEN])
+void hsr_nl_nodedown(struct hsr_priv *hsr, unsigned char addr[ETH_ALEN])
 {
 	struct sk_buff *skb;
 	void *msg_head;
+	struct hsr_port *master;
 	int res;
 
 	skb = genlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC);
@@ -199,7 +215,10 @@ nla_put_failure:
 	kfree_skb(skb);
 
 fail:
-	netdev_warn(hsr_priv->dev, "Could not send HSR node down\n");
+	rcu_read_lock();
+	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
+	netdev_warn(master->dev, "Could not send HSR node down\n");
+	rcu_read_unlock();
 }
 
 
@@ -220,7 +239,8 @@ static int hsr_get_node_status(struct sk_buff *skb_in, struct genl_info *info)
 	/* For sending */
 	struct sk_buff *skb_out;
 	void *msg_head;
-	struct hsr_priv *hsr_priv;
+	struct hsr_priv *hsr;
+	struct hsr_port *port;
 	unsigned char hsr_node_addr_b[ETH_ALEN];
 	int hsr_node_if1_age;
 	u16 hsr_node_if1_seq;
@@ -267,8 +287,8 @@ static int hsr_get_node_status(struct sk_buff *skb_in, struct genl_info *info)
 	if (res < 0)
 		goto nla_put_failure;
 
-	hsr_priv = netdev_priv(hsr_dev);
-	res = hsr_get_node_data(hsr_priv,
+	hsr = netdev_priv(hsr_dev);
+	res = hsr_get_node_data(hsr,
 			(unsigned char *) nla_data(info->attrs[HSR_A_NODE_ADDR]),
 			hsr_node_addr_b,
 			&addr_b_ifindex,
@@ -301,9 +321,12 @@ static int hsr_get_node_status(struct sk_buff *skb_in, struct genl_info *info)
 	res = nla_put_u16(skb_out, HSR_A_IF1_SEQ, hsr_node_if1_seq);
 	if (res < 0)
 		goto nla_put_failure;
-	if (hsr_priv->slave[0])
+	rcu_read_lock();
+	port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_A);
+	if (port)
 		res = nla_put_u32(skb_out, HSR_A_IF1_IFINDEX,
-						hsr_priv->slave[0]->ifindex);
+				  port->dev->ifindex);
+	rcu_read_unlock();
 	if (res < 0)
 		goto nla_put_failure;
 
@@ -313,9 +336,14 @@ static int hsr_get_node_status(struct sk_buff *skb_in, struct genl_info *info)
 	res = nla_put_u16(skb_out, HSR_A_IF2_SEQ, hsr_node_if2_seq);
 	if (res < 0)
 		goto nla_put_failure;
-	if (hsr_priv->slave[1])
+	rcu_read_lock();
+	port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_B);
+	if (port)
 		res = nla_put_u32(skb_out, HSR_A_IF2_IFINDEX,
-						hsr_priv->slave[1]->ifindex);
+				  port->dev->ifindex);
+	rcu_read_unlock();
+	if (res < 0)
+		goto nla_put_failure;
 
 	genlmsg_end(skb_out, msg_head);
 	genlmsg_unicast(genl_info_net(info), skb_out, info->snd_portid);
@@ -334,7 +362,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)
 {
@@ -345,7 +373,7 @@ static int hsr_get_node_list(struct sk_buff *skb_in, struct genl_info *info)
 	/* For sending */
 	struct sk_buff *skb_out;
 	void *msg_head;
-	struct hsr_priv *hsr_priv;
+	struct hsr_priv *hsr;
 	void *pos;
 	unsigned char addr[ETH_ALEN];
 	int res;
@@ -385,17 +413,17 @@ static int hsr_get_node_list(struct sk_buff *skb_in, struct genl_info *info)
 	if (res < 0)
 		goto nla_put_failure;
 
-	hsr_priv = netdev_priv(hsr_dev);
+	hsr = netdev_priv(hsr_dev);
 
 	rcu_read_lock();
-	pos = hsr_get_next_node(hsr_priv, NULL, addr);
+	pos = hsr_get_next_node(hsr, NULL, addr);
 	while (pos) {
 		res = nla_put(skb_out, HSR_A_NODE_ADDR, ETH_ALEN, addr);
 		if (res < 0) {
 			rcu_read_unlock();
 			goto nla_put_failure;
 		}
-		pos = hsr_get_next_node(hsr_priv, pos, addr);
+		pos = hsr_get_next_node(hsr, pos, addr);
 	}
 	rcu_read_unlock();
 
diff --git a/net/hsr/hsr_netlink.h b/net/hsr/hsr_netlink.h
index d4579dcc3c7d..3f6b95b5b6b8 100644
--- a/net/hsr/hsr_netlink.h
+++ b/net/hsr/hsr_netlink.h
@@ -1,4 +1,4 @@
-/* Copyright 2011-2013 Autronica Fire and Security AS
+/* 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
@@ -6,7 +6,7 @@
  * any later version.
  *
  * Author(s):
- *	2011-2013 Arvid Brodin, arvid.brodin@xdin.com
+ *	2011-2014 Arvid Brodin, arvid.brodin@alten.se
  */
 
 #ifndef __HSR_NETLINK_H
@@ -17,13 +17,14 @@
 #include <uapi/linux/hsr_netlink.h>
 
 struct hsr_priv;
+struct hsr_port;
 
 int __init hsr_netlink_init(void);
 void __exit hsr_netlink_exit(void);
 
-void hsr_nl_ringerror(struct hsr_priv *hsr_priv, unsigned char addr[ETH_ALEN],
-		      int dev_idx);
-void hsr_nl_nodedown(struct hsr_priv *hsr_priv, unsigned char addr[ETH_ALEN]);
+void hsr_nl_ringerror(struct hsr_priv *hsr, unsigned char addr[ETH_ALEN],
+		      struct hsr_port *port);
+void hsr_nl_nodedown(struct hsr_priv *hsr, unsigned char addr[ETH_ALEN]);
 void hsr_nl_framedrop(int dropcount, int dev_idx);
 void hsr_nl_linkdown(int dev_idx);
 
diff --git a/net/hsr/hsr_slave.c b/net/hsr/hsr_slave.c
new file mode 100644
index 000000000000..a348dcbcd683
--- /dev/null
+++ b/net/hsr/hsr_slave.c
@@ -0,0 +1,196 @@
+/* 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_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 */
+	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;
+	}
+
+	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...
+	 */
+
+	return 0;
+}
+
+
+/* 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;
+
+	/* 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:
+	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);
+	netdev_update_features(master->dev);
+	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) {
+		netdev_update_features(master->dev);
+		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);
+}
diff --git a/net/hsr/hsr_slave.h b/net/hsr/hsr_slave.h
new file mode 100644
index 000000000000..3ccfbf71c92e
--- /dev/null
+++ b/net/hsr/hsr_slave.h
@@ -0,0 +1,38 @@
+/* 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_SLAVE_H
+#define __HSR_SLAVE_H
+
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/rtnetlink.h>
+#include "hsr_main.h"
+
+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);
+bool hsr_port_exists(const struct net_device *dev);
+
+static inline struct hsr_port *hsr_port_get_rtnl(const struct net_device *dev)
+{
+	ASSERT_RTNL();
+	return hsr_port_exists(dev) ?
+				rtnl_dereference(dev->rx_handler_data) : NULL;
+}
+
+static inline struct hsr_port *hsr_port_get_rcu(const struct net_device *dev)
+{
+	return hsr_port_exists(dev) ?
+				rcu_dereference(dev->rx_handler_data) : NULL;
+}
+
+#endif /* __HSR_SLAVE_H */