| lists.openwall.net | lists / announce owl-users owl-dev john-users john-dev passwdqc-users yescrypt popa3d-users / oss-security kernel-hardening musl sabotage tlsify passwords / crypt-dev xvendor / Bugtraq Full-Disclosure linux-kernel linux-netdev linux-ext4 linux-hardening linux-cve-announce PHC | |
|
Open Source and information security mailing list archives
| ||
|
Date: Fri, 12 Oct 2012 17:11:12 +0000
From: Arvid Brodin <Arvid.Brodin@...n.com>
To: "netdev@...r.kernel.org" <netdev@...r.kernel.org>
CC: "joe@...ches.com" <joe@...ches.com>,
"shemminger@...tta.com" <shemminger@...tta.com>,
"jboticario@...il.com" <jboticario@...il.com>,
"balferreira@...glemail.com" <balferreira@...glemail.com>,
Arvid Brodin <Arvid.Brodin@...n.com>
Subject: [RFC v4 1/1] net/hsr: Support for the HSR protocol (IEC:2010 / HSR
v0)
This would add support for the High-availability Seamless Redundancy network
protocol, version 0 (2010).
This RFC is NOT meant for mainline inclusion at the moment since we're trying
to figure out how to handle a probable incompatibility with the newer
HSR v1 (2012) standard.
---
This RFC is mainly to let you know that I'm still working on this patch. This
is now a pretty complete implementation of the HSR v0 (2010) protocol. As
explained above, the main reason I'm still not sending this as a real patch
meant for mainline inclusion, is the fact that IEC recently released an updated
standard (HSR v1 / 2012) which as far as I can tell is not backwards compatible
with the 2010 standard implemented by this patch.
At the moment we're trying to figure out what to do about this.
Changes from RFC v3:
* Fixed style issues.
* More thorough device stats updates.
* Fixed bug where node that is downed and restarted within
NODE_FORGET_TIME is never seen by other nodes.
* In RFC v3, a slave that was unregistered (e.g. USB network device that
was unplugged) would crash the kernel. Works OK now.
* Slave envent handling: MTU change, MAC address change, unregister,
type change.
* Master event handling: MTU change.
* Removed NONSTANDARD_HSR config option.
* Guards for VLAN/HSR combo (allowed by the HSR standard, but not yet
implemented).
The patch has mostly been tested on 2.6.37, then modified to apply and compile
on 3.6.1.
Documentation/networking/hsr/hsr_genl.c | 221 ++++++++++++
include/linux/if_ether.h | 1 +
include/linux/if_link.h | 11 +
net/Kconfig | 1 +
net/Makefile | 1 +
net/hsr/Kconfig | 33 ++
net/hsr/Makefile | 7 +
net/hsr/hsr_device.c | 576 +++++++++++++++++++++++++++++++
net/hsr/hsr_device.h | 28 ++
net/hsr/hsr_framereg.c | 338 ++++++++++++++++++
net/hsr/hsr_framereg.h | 51 +++
net/hsr/hsr_main.c | 468 +++++++++++++++++++++++++
net/hsr/hsr_main.h | 157 +++++++++
net/hsr/hsr_netlink.c | 305 ++++++++++++++++
net/hsr/hsr_netlink.h | 64 ++++
15 files changed, 2262 insertions(+), 0 deletions(-)
create mode 100644 Documentation/networking/hsr/hsr_genl.c
create mode 100644 net/hsr/Kconfig
create mode 100644 net/hsr/Makefile
create mode 100644 net/hsr/hsr_device.c
create mode 100644 net/hsr/hsr_device.h
create mode 100644 net/hsr/hsr_framereg.c
create mode 100644 net/hsr/hsr_framereg.h
create mode 100644 net/hsr/hsr_main.c
create mode 100644 net/hsr/hsr_main.h
create mode 100644 net/hsr/hsr_netlink.c
create mode 100644 net/hsr/hsr_netlink.h
diff --git a/Documentation/networking/hsr/hsr_genl.c b/Documentation/networking/hsr/hsr_genl.c
new file mode 100644
index 0000000..cb384d5
--- /dev/null
+++ b/Documentation/networking/hsr/hsr_genl.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright 2011-2012 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-2012 Arvid Brodin, arvid.brodin@...n.com
+ *
+ * Userspace example of using Generic Netlink to get HSR
+ * ("High-availability Seamless Redundancy") link/network status.
+ *
+ * Needs userspace libnl-3. Current as of 2012-08-07.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <netlink/netlink.h>
+#include <netlink/socket.h>
+#include <netlink/attr.h>
+#include <netlink/genl/genl.h>
+#include <netlink/genl/ctrl.h>
+/**** insert path to hsr_netlink from your kernel below: ****/
+#include "<path>/net/hsr/hsr_netlink.h"
+
+
+static struct nla_policy hsr_genl_policy[HSR_A_MAX + 1] = {
+ [HSR_A_NODE_ADDR] = { .type = NLA_UNSPEC },
+ [HSR_A_IFINDEX] = { .type = NLA_U32 },
+ [HSR_A_IF1AGE] = { .type = NLA_U32 },
+ [HSR_A_IF2AGE] = { .type = NLA_U32 },
+};
+
+#define ETH_ALEN 6
+
+void print_mac(const unsigned char *addr)
+{
+ int i;
+
+ for (i = 0; i < ETH_ALEN; i++)
+ printf("%02x ", addr[i]);
+}
+
+
+int parse_genlmsg(struct nl_msg *msg, void *arg)
+{
+ struct nlattr *attrs[HSR_A_MAX + 1];
+ int rc;
+ struct genlmsghdr *hdr;
+ int i;
+
+ rc = genlmsg_parse(nlmsg_hdr(msg), 0, attrs, HSR_A_MAX, hsr_genl_policy);
+ if (rc < 0) {
+ printf("Error parsing genlmsg: %d\n", rc);
+ return rc;
+ }
+
+
+ /*
+ * Extract command ID from "message" -> "netlink header" ->
+ * "generic netlink header".
+ *
+ * These are the command enums used when creating a genl msg header
+ * in the kernel with genlmsg_put().
+ */
+ hdr = genlmsg_hdr(nlmsg_hdr(msg));
+
+ switch (hdr->cmd) {
+ case HSR_C_RING_ERROR:
+ printf("Ring error: \n");
+ break;
+ case HSR_C_NODE_DOWN:
+ printf("Node down: \n");
+ break;
+ case HSR_C_SET_NODE_STATUS:
+ printf("Node status: \n");
+ break;
+ default:
+ printf("Unknown genl message (%d)\n", hdr->cmd);
+ }
+
+
+ /*
+ * Extract the attached data (the "attributes").
+ */
+ for (i = 0; i < HSR_A_MAX + 1; i++)
+ if (attrs[i]) {
+ switch (attrs[i]->nla_type) {
+ case HSR_A_NODE_ADDR:
+ printf(" node address ");
+ print_mac(nla_data(attrs[i]));
+ printf("\n");
+ break;
+ case HSR_A_IFINDEX:
+ printf(" interface index %d\n",
+ nla_get_u32(attrs[i]));
+ break;
+ case HSR_A_IF1AGE:
+ printf(" last frame over slave #1 %d ms ago\n",
+ (int) nla_get_u32(attrs[i]));
+ break;
+ case HSR_A_IF2AGE:
+ printf(" last frame over slave #2 %d ms ago\n",
+ (int) nla_get_u32(attrs[i]));
+ break;
+ default:
+ printf(" unknown attribute type: %d\n",
+ attrs[i]->nla_type);
+ }
+ }
+
+ return 0;
+}
+
+/*
+ * Send a "simple" (header only) Generic Netlink message
+int query_link_status(int family)
+{
+ return (genl_send_simple(nlsk, family, HSR_C_GET_STATUS, 1, 0));
+}
+ */
+
+int query_node_status(struct nl_sock *nlsk, int family, int ifindex,
+ const unsigned char node_addr[ETH_ALEN])
+{
+ struct nl_msg *msg;
+ void *user_hdr;
+
+ msg = nlmsg_alloc();
+ if (!msg)
+ return -1;
+
+ user_hdr = genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, family,
+ 0, 0, HSR_C_GET_NODE_STATUS, 1);
+ if (!user_hdr)
+ goto nla_put_failure;
+
+/*
+ * Query by interface name could be implemented in the kernel if needed:
+ * NLA_PUT_STRING(msg, HSR_A_IFNAME, ifname);
+ */
+ NLA_PUT_U32(msg, HSR_A_IFINDEX, ifindex);
+ NLA_PUT(msg, HSR_A_NODE_ADDR, ETH_ALEN, node_addr);
+
+ printf("Querying if %d for status of node ", ifindex);
+ print_mac(node_addr);
+ printf("\n");
+
+ return nl_send_auto(nlsk, msg);
+
+nla_put_failure:
+ nlmsg_free(msg);
+ return -1;
+}
+
+
+int main()
+{
+ struct nl_sock *nlsk;
+ int hsr_mgroup;
+ int rc;
+
+ nlsk = nl_socket_alloc();
+ if (!nlsk) {
+ printf("nl_socket_alloc() failed\n");
+ return EXIT_FAILURE;
+ }
+ nl_socket_disable_seq_check(nlsk);
+ nl_socket_modify_cb(nlsk, NL_CB_VALID, NL_CB_CUSTOM, parse_genlmsg, NULL);
+ genl_connect(nlsk);
+
+ /*
+ * Sign up for HSR messages
+ */
+ hsr_mgroup = genl_ctrl_resolve_grp(nlsk, "HSR", "hsr-network");
+ if (hsr_mgroup < 0) {
+ printf("genl_ctrl_resolve_grp() failed: %d\n", hsr_mgroup);
+ rc = EXIT_FAILURE;
+ goto out;
+ }
+
+ printf("Registering for multicast group %d\n", hsr_mgroup);
+ rc = nl_socket_add_memberships(nlsk, hsr_mgroup, 0);
+ if (rc < 0) {
+ printf("nl_socket_add_memberships() failed: %d\n", rc);
+ goto out;
+ }
+
+ /*
+ * Send a query about the status of another node on the HSR network:
+ */
+ int hsr_family;
+ /* The hsr if we send the enquiry to (get it with e.g.
+ * 'cat /sys/class/net/hsr0/ifindex'): */
+ const int hsr_ifindex = 4;
+ /* The node to enquire about: */
+ const unsigned char node[ETH_ALEN] = {0x00, 0x24, 0x74, 0x00, 0x17, 0xAD};
+
+ hsr_family = genl_ctrl_resolve(nlsk, "HSR");
+ if (hsr_family < 0) {
+ printf("genl_ctrl_resolve() failed: %d\n", hsr_family);
+ goto receive;
+ }
+ rc = query_node_status(nlsk, hsr_family, hsr_ifindex, node);
+ printf("query_node_status() returned %d\n", rc);
+
+ /*
+ * Receive messages
+ */
+receive:
+ while (1)
+ nl_recvmsgs_default(nlsk);
+
+ rc = EXIT_SUCCESS;
+out:
+ nl_close(nlsk);
+ nl_socket_free(nlsk);
+ return rc;
+}
diff --git a/include/linux/if_ether.h b/include/linux/if_ether.h
index 167ce5b..a03334a 100644
--- a/include/linux/if_ether.h
+++ b/include/linux/if_ether.h
@@ -83,6 +83,7 @@
#define ETH_P_TIPC 0x88CA /* TIPC */
#define ETH_P_8021AH 0x88E7 /* 802.1ah Backbone Service Tag */
#define ETH_P_1588 0x88F7 /* IEEE 1588 Timesync */
+#define ETH_P_HSR 0x88FB /* IEC 62439-3 HSR/PRP */
#define ETH_P_FCOE 0x8906 /* Fibre Channel over Ethernet */
#define ETH_P_TDLS 0x890D /* TDLS */
#define ETH_P_FIP 0x8914 /* FCoE Initialization Protocol */
diff --git a/include/linux/if_link.h b/include/linux/if_link.h
index e4dad4d..24766c1 100644
--- a/include/linux/if_link.h
+++ b/include/linux/if_link.h
@@ -432,4 +432,15 @@ enum {
#define IFLA_IPOIB_MAX (__IFLA_IPOIB_MAX - 1)
+/* HSR section */
+
+enum {
+ IFLA_HSR_UNSPEC,
+ IFLA_HSR_SLAVE1,
+ IFLA_HSR_SLAVE2,
+ __IFLA_HSR_MAX,
+};
+
+#define IFLA_HSR_MAX (__IFLA_HSR_MAX - 1)
+
#endif /* _LINUX_IF_LINK_H */
diff --git a/net/Kconfig b/net/Kconfig
index 30b48f5..15e8ce1 100644
--- a/net/Kconfig
+++ b/net/Kconfig
@@ -218,6 +218,7 @@ source "net/dcb/Kconfig"
source "net/dns_resolver/Kconfig"
source "net/batman-adv/Kconfig"
source "net/openvswitch/Kconfig"
+source "net/hsr/Kconfig"
config RPS
boolean
diff --git a/net/Makefile b/net/Makefile
index 4f4ee08..adbf1de 100644
--- a/net/Makefile
+++ b/net/Makefile
@@ -70,3 +70,4 @@ obj-$(CONFIG_CEPH_LIB) += ceph/
obj-$(CONFIG_BATMAN_ADV) += batman-adv/
obj-$(CONFIG_NFC) += nfc/
obj-$(CONFIG_OPENVSWITCH) += openvswitch/
+obj-$(CONFIG_HSR) += hsr/
diff --git a/net/hsr/Kconfig b/net/hsr/Kconfig
new file mode 100644
index 0000000..a9ed304
--- /dev/null
+++ b/net/hsr/Kconfig
@@ -0,0 +1,33 @@
+#
+# IEC 62439-3 High-availability Seamless Redundancy
+#
+
+config HSR
+ tristate "High-availability Seamless Redundancy (HSR)"
+ ---help---
+ If you say Y here, then your Linux box will be able to act as a
+ DANH ("Doubly attached node implementing HSR"). For this to work,
+ your Linux box needs (at least) two physical Ethernet interfaces,
+ and you need to enslave these to a virtual hsr interface using the
+ appropriate user space tool, i.e.:
+
+ # ip link add name hsr0 type hsr dev1 dev2
+
+ Your Linux box must be connected as a node in a ring network
+ together with other HSR capable nodes.
+
+ All Ethernet frames sent over the hsr device will be sent in both
+ directions on the ring (over both slave ports), giving a redundant,
+ instant fail-over network.
+
+ Each HSR node in the ring acts like a bridge for HSR frames, but
+ filters frames that have been forwarded earlier.
+
+ This code is a "best effort" to comply with the HSR standard as
+ described in IEC 62439-3, but no compliancy tests have been made.
+ You need to perform any and all necessary tests yourself before
+ relying on this code in a safety critical system. In particular, the
+ standard is very diffuse on how to use the Ring ID field in the HSR
+ tag, and it's probable that this code does not do the right thing.
+
+ If unsure, say N.
diff --git a/net/hsr/Makefile b/net/hsr/Makefile
new file mode 100644
index 0000000..b68359f
--- /dev/null
+++ b/net/hsr/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for HSR
+#
+
+obj-$(CONFIG_HSR) += hsr.o
+
+hsr-y := hsr_main.o hsr_framereg.o hsr_device.o hsr_netlink.o
diff --git a/net/hsr/hsr_device.c b/net/hsr/hsr_device.c
new file mode 100644
index 0000000..cd1efa2
--- /dev/null
+++ b/net/hsr/hsr_device.c
@@ -0,0 +1,576 @@
+/*
+ * Copyright 2011-2012 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-2012 Arvid Brodin, arvid.brodin@...n.com
+ *
+ * This file contains device methods for creating, using and destroying
+ * virtual HSR devices.
+ */
+
+#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_framereg.h"
+#include "hsr_main.h"
+
+
+static bool is_admin_up(struct net_device *dev)
+{
+ return (dev && (dev->flags & IFF_UP));
+}
+
+static bool is_operstate_up(struct net_device *dev)
+{
+ return (dev && (dev->operstate == IF_OPER_UP));
+}
+
+static void __hsr_set_operstate(struct net_device *dev, int transition)
+{
+ if (dev->operstate != transition) {
+ write_lock_bh(&dev_base_lock);
+ dev->operstate = transition;
+ write_unlock_bh(&dev_base_lock);
+ netdev_state_change(dev);
+ }
+}
+
+void hsr_set_operstate(struct net_device *hsr_dev, struct net_device *slave1,
+ struct net_device *slave2)
+{
+ if (!is_admin_up(hsr_dev)) {
+ __hsr_set_operstate(hsr_dev, IF_OPER_DOWN);
+ return;
+ }
+
+ if (is_operstate_up(slave1) || is_operstate_up(slave2))
+ __hsr_set_operstate(hsr_dev, IF_OPER_UP);
+ else
+ __hsr_set_operstate(hsr_dev, IF_OPER_LOWERLAYERDOWN);
+}
+
+void hsr_set_carrier(struct net_device *hsr_dev, struct net_device *slave1,
+ struct net_device *slave2)
+{
+ if (is_operstate_up(slave1) || is_operstate_up(slave2))
+ netif_carrier_on(hsr_dev);
+ else
+ netif_carrier_off(hsr_dev);
+}
+
+
+void hsr_check_announce(struct net_device *hsr_dev, int old_operstate)
+{
+ struct hsr_priv *hsr_priv;
+
+ hsr_priv = 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 +
+ msecs_to_jiffies(HSR_ANNOUNCE_INTERVAL);
+ add_timer(&hsr_priv->announce_timer);
+ }
+
+ if ((hsr_dev->operstate != IF_OPER_UP) && (old_operstate == IF_OPER_UP))
+ /* Went down */
+ del_timer(&hsr_priv->announce_timer);
+}
+
+
+int hsr_get_max_mtu(struct hsr_priv *hsr_priv)
+{
+ int mtu_max;
+
+ 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;
+}
+
+static int hsr_dev_change_mtu(struct net_device *dev, int new_mtu)
+{
+ struct hsr_priv *hsr_priv;
+
+ hsr_priv = netdev_priv(dev);
+
+ 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);
+ return -EINVAL;
+ }
+
+ dev->mtu = new_mtu;
+
+ return 0;
+}
+
+static int hsr_dev_open(struct net_device *dev)
+{
+ struct hsr_priv *hsr_priv;
+
+ hsr_priv = netdev_priv(dev);
+
+ if (hsr_priv->slave[0])
+ dev_open(hsr_priv->slave[0]);
+ if (hsr_priv->slave[1])
+ dev_open(hsr_priv->slave[1]);
+
+ 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.
+ */
+ return 0;
+}
+
+
+static void hsr_fill_tag(struct hsr_ethhdr *hsr_ethhdr, struct hsr_priv *hsr_priv)
+{
+ unsigned long irqflags;
+
+ /*
+ * IEC 62439-1:2010, p 48, says the 4-bit "path" field can take values
+ * between 0001-1001 ("ring identifier", for regular HSR frames),
+ * or 1111 ("HSR management", supervision frames). Unfortunately, the
+ * spec writers forgot to explain what a "ring identifier" is, or
+ * how it is used. So we just set this to 0001 for regular frames,
+ * and 1111 for supervision frames.
+ */
+ set_hsr_tag_path(&hsr_ethhdr->hsr_tag, 0x1);
+
+ /*
+ * IEC 62439-1:2010, p 12: "The link service data unit in an Ethernet
+ * frame is the content of the frame located between the Length/Type
+ * field and the Frame Check Sequence."
+ *
+ * IEC 62439-3, p 48, specifies the "original LPDU" to include the
+ * original "LT" field (what "LT" means is not explained anywhere as
+ * far as I can see - perhaps "Length/Type"?). So LSDU_size might
+ * equal original length + 2.
+ * Also, the fact that this field is not used anywhere (might be used
+ * by a RedBox connecting HSR and PRP nets?) means I cannot test its
+ * correctness. Instead of guessing, I set this to 0 here, to make any
+ * problems immediately apparent. Anyone using this driver with PRP/HSR
+ * RedBoxes might need to fix this...
+ */
+ set_hsr_tag_LSDU_size(&hsr_ethhdr->hsr_tag, 0);
+
+ spin_lock_irqsave(&hsr_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_HSR);
+}
+
+static int slave_xmit(struct sk_buff *skb, struct net_device *dev,
+ struct net_device *hsr_dev)
+{
+ skb->dev = dev;
+ return dev_queue_xmit(skb);
+}
+
+
+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;
+
+ hsr_priv = netdev_priv(dev);
+ hsr_ethhdr = (struct hsr_ethhdr *) skb->data;
+
+ if ((ntohs(skb->protocol) != ETH_P_HSR) ||
+ (ntohs(hsr_ethhdr->ethhdr.h_proto) != ETH_P_HSR)) {
+
+ hsr_fill_tag(hsr_ethhdr, hsr_priv);
+ skb->protocol = htons(ETH_P_HSR);
+ }
+
+ skb2 = skb_clone(skb, GFP_ATOMIC);
+
+ res1 = NET_XMIT_DROP;
+ if (likely(hsr_priv->slave[0]))
+ res1 = slave_xmit(skb, hsr_priv->slave[0], dev);
+ res2 = NET_XMIT_DROP;
+ if (skb2 && likely(hsr_priv->slave[1])) {
+ /*
+ * Address substitution (IEC62439-3 pp 26, 50): replace mac
+ * address of outgoing frame with that of the outgoing slave's.
+ */
+ memcpy(hsr_ethhdr->ethhdr.h_source,
+ hsr_priv->slave[1]->dev_addr, ETH_ALEN);
+ res2 = slave_xmit(skb2, hsr_priv->slave[1], dev);
+ }
+
+ 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++;
+
+ 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,
+ .parse = eth_header_parse,
+};
+
+
+/*
+ * HSR:2010 supervision frames should be padded so that the whole frame,
+ * including headers and FCS, is 64 bytes (without VLAN).
+ */
+static int hsr_pad(int size)
+{
+ const int min_size = ETH_ZLEN - HSR_TAGLEN - ETH_HLEN;
+
+ if (size >= min_size)
+ return size;
+ return min_size;
+}
+
+static void send_hsr_supervision_frame(struct net_device *hsr_dev, 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;
+ 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->protocol = htons(ETH_P_HSR);
+ skb->priority = TC_PRIO_CONTROL;
+
+ if (dev_hard_header(skb, skb->dev, ETH_P_HSR, hsr_multicast_addr,
+ skb->dev->dev_addr, skb->len) < 0)
+ goto out;
+
+ skb_pull(skb, sizeof(struct ethhdr));
+ hsr_stag = (typeof(hsr_stag)) skb->data;
+
+ 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);
+
+ 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));
+ memcpy(hsr_sp->MacAddressA, hsr_dev->dev_addr, ETH_ALEN);
+
+ dev_queue_xmit(skb);
+ return;
+
+out:
+ kfree_skb(skb);
+}
+
+
+/*
+ * Announce (supervision frame) timer function
+ */
+static void hsr_announce(unsigned long data)
+{
+ struct hsr_priv *hsr_priv;
+
+ hsr_priv = (struct hsr_priv *) data;
+
+ if (hsr_priv->announce_count < 3) {
+ send_hsr_supervision_frame(hsr_priv->dev, HSR_TLV_ANNOUNCE);
+ hsr_priv->announce_count++;
+ } else
+ send_hsr_supervision_frame(hsr_priv->dev, HSR_TLV_LIFE_CHECK);
+
+ if (hsr_priv->announce_count < 3)
+ hsr_priv->announce_timer.expires = jiffies +
+ msecs_to_jiffies(HSR_ANNOUNCE_INTERVAL);
+ else
+ hsr_priv->announce_timer.expires = jiffies +
+ msecs_to_jiffies(HSR_LIFE_CHECK_INTERVAL);
+
+ if (is_admin_up(hsr_priv->dev))
+ add_timer(&hsr_priv->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 < 2; i++) {
+ if (!hsr_priv->slave[i])
+ continue;
+ res = dev_set_promiscuity(hsr_priv->slave[i], -1);
+ if (res)
+ netdev_info(hsr_dev, "HSR: 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);
+}
+
+/*
+ * According to comments in the declaration of struct net_device, this function
+ * is "Called from unregister, can be used to call free_netdev". Ok then...
+ */
+static void hsr_dev_destroy(struct net_device *hsr_dev)
+{
+ struct hsr_priv *hsr_priv;
+
+ hsr_priv = netdev_priv(hsr_dev);
+
+ 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 */
+}
+
+static const struct net_device_ops hsr_device_ops = {
+ .ndo_change_mtu = hsr_dev_change_mtu,
+ .ndo_open = hsr_dev_open,
+ .ndo_stop = hsr_dev_close,
+ .ndo_start_xmit = hsr_dev_xmit,
+};
+
+
+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->destructor = hsr_dev_destroy;
+}
+
+
+/*
+ * If dev is a HSR master, return 1; otherwise, return 0.
+ */
+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;
+}
+
+int hsr_dev_finalize(struct net_device *hsr_dev, struct net_device *slave[2])
+{
+ struct hsr_priv *hsr_priv;
+ int i;
+ 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 < 2; i++)
+ hsr_priv->slave[i] = slave[i];
+
+ spin_lock_init(&hsr_priv->seqnr_lock);
+ hsr_priv->sequence_nr = 0;
+
+ init_timer(&hsr_priv->announce_timer);
+ hsr_priv->announce_timer.function = hsr_announce;
+ hsr_priv->announce_timer.data = (unsigned long) hsr_priv;
+
+
+/*
+ * 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?
+ */
+
+ for (i = 0; i < 2; 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. */
+ hsr_dev->features |= NETIF_F_VLAN_CHALLENGED;
+
+ /* Set hsr_dev's MAC address to that of mac_slave1 */
+ memcpy(hsr_dev->dev_addr, hsr_priv->slave[0]->dev_addr, ETH_ALEN);
+
+ /* Set required header length */
+ for (i = 0; i < 2; 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 < 2; 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 < 2; i++) {
+ res = dev_set_promiscuity(slave[i], 1);
+ if (res) {
+ netdev_info(hsr_dev, "HSR: 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 = register_netdevice(hsr_dev);
+ if (res)
+ goto fail;
+
+ register_hsr_master(hsr_priv);
+
+ return 0;
+
+fail:
+ restore_slaves(hsr_dev);
+ return res;
+}
diff --git a/net/hsr/hsr_device.h b/net/hsr/hsr_device.h
new file mode 100644
index 0000000..419ee23
--- /dev/null
+++ b/net/hsr/hsr_device.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright 2011-2012 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-2012 Arvid Brodin, arvid.brodin@...n.com
+ */
+
+#ifndef __HSR_DEVICE_H
+#define __HSR_DEVICE_H
+
+#include <linux/netdevice.h>
+
+void hsr_dev_setup(struct net_device *dev);
+int hsr_dev_finalize(struct net_device *hsr_dev, struct net_device *slave[2]);
+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);
+bool is_hsr_master(struct net_device *dev);
+int hsr_get_max_mtu(struct hsr_priv *hsr_priv);
+
+#endif /* __HSR_DEVICE_H */
diff --git a/net/hsr/hsr_framereg.c b/net/hsr/hsr_framereg.c
new file mode 100644
index 0000000..09baedb
--- /dev/null
+++ b/net/hsr/hsr_framereg.c
@@ -0,0 +1,338 @@
+/*
+ * Copyright 2011-2012 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-2012 Arvid Brodin, arvid.brodin@...n.com
+ *
+ * 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
+ * sequence number. This code keeps track of senders and their sequence numbers
+ * to allow filtering of duplicate frames.
+ */
+
+#include <linux/if_ether.h>
+#include <linux/etherdevice.h>
+#include <linux/slab.h>
+#include <linux/rculist.h>
+#include "hsr_main.h"
+#include "hsr_framereg.h"
+#include "hsr_netlink.h"
+
+
+/*
+ TODO: use hash lists for mac addresses (linux/jhash.h)?
+*/
+
+struct node_entry {
+ struct list_head mac_list;
+ unsigned char MacAddressA[ETH_ALEN];
+ unsigned char MacAddressB[ETH_ALEN];
+ unsigned long time_in[HSR_MAX_SLAVE];
+ u16 seq_out[HSR_MAX_DEV];
+ struct rcu_head rcu_head;
+};
+
+
+/*
+ * Search for mac entry. Caller must hold rcu read lock.
+ */
+static struct node_entry *find_node_by_AddrA(struct list_head *node_db,
+ unsigned char addr[ETH_ALEN])
+{
+ struct node_entry *node;
+
+ list_for_each_entry_rcu(node, node_db, mac_list)
+ if (!compare_ether_addr(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,
+ unsigned char addr[ETH_ALEN])
+{
+ struct node_entry *node;
+
+ list_for_each_entry_rcu(node, node_db, mac_list)
+ if (!compare_ether_addr(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 (!compare_ether_addr(node->MacAddressA, ethhdr->h_source))
+ return node;
+ if (!compare_ether_addr(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;
+
+ memcpy(node->MacAddressA, addr_a, ETH_ALEN);
+ memcpy(node->MacAddressB, addr_b, ETH_ALEN);
+
+ 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;
+}
+
+static void node_entry_reclaim(struct rcu_head *rh)
+{
+ kfree(container_of(rh, struct node_entry, rcu_head));
+}
+
+
+/*
+ * 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.
+ *
+ * - (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).
+ */
+struct node_entry *hsr_merge_node(struct hsr_priv *hsr_priv,
+ struct node_entry *node,
+ struct sk_buff *skb)
+{
+ struct ethhdr *ethhdr;
+ struct hsr_sup_payload *hsr_sp;
+ int i;
+
+ ethhdr = (struct ethhdr *) skb_mac_header(skb);
+ hsr_sp = (struct hsr_sup_payload *) skb->data;
+
+ if (node && compare_ether_addr(node->MacAddressA, hsr_sp->MacAddressA)) {
+ /* node has changed its AddrA, frame was received from SlaveB */
+ list_del_rcu(&node->mac_list);
+ call_rcu(&node->rcu_head, node_entry_reclaim);
+ node = NULL;
+ }
+
+ if (node)
+ return node;
+
+ node = find_node_by_AddrA(&hsr_priv->node_db, hsr_sp->MacAddressA);
+ if (node) {
+ /* Node is PICS_SUBS capable; merge it */
+ memcpy(node->MacAddressB, ethhdr->h_source, ETH_ALEN);
+ return node;
+ }
+
+ node = kmalloc(sizeof(*node), GFP_ATOMIC);
+ if (!node)
+ return NULL;
+
+ memcpy(node->MacAddressA, hsr_sp->MacAddressA, ETH_ALEN);
+ memcpy(node->MacAddressB, ethhdr->h_source, ETH_ALEN);
+
+ for (i = 0; i < HSR_MAX_SLAVE; i++)
+ node->time_in[i] = 0;
+ for (i = 0; i < HSR_MAX_DEV; i++)
+ node->seq_out[i] = 0;
+
+ 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(struct hsr_priv *hsr_priv, struct sk_buff *skb)
+{
+ struct ethhdr *ethhdr;
+ struct node_entry *node;
+
+ ethhdr = (struct ethhdr *) skb_mac_header(skb);
+
+ rcu_read_lock();
+ node = find_node_by_AddrB(&hsr_priv->node_db, ethhdr->h_source);
+ if (node)
+ memcpy(ethhdr->h_source, node->MacAddressA, ETH_ALEN);
+ rcu_read_unlock();
+}
+
+
+
+/*
+ * above(a, b) - return 1 if a > b, 0 otherwise.
+ */
+static bool above(u16 a, u16 b)
+{
+ /* Remove inconsistency where above(a, b) == below(a, b) */
+ if ((int) b - a == 32768)
+ return 0;
+
+ return (((s16) (b - a)) < 0);
+}
+#define below(a, b) above((b), (a))
+#define above_or_eq(a, b) (!below((a), (b)))
+#define below_or_eq(a, b) (!above((a), (b)))
+
+
+void hsr_register_frame_in(struct node_entry *node, enum hsr_dev_idx dev_idx)
+{
+ if ((dev_idx < 0) || (dev_idx >= HSR_MAX_DEV)) {
+ WARN_ON(1);
+ return;
+ }
+ node->time_in[dev_idx] = jiffies;
+}
+
+/*
+ * 'skb' is a HSR Ethernet frame (with a HSR tag inserted), with a valid
+ * ethhdr->h_source address and skb->mac_header set.
+ *
+ * Return:
+ * 1 if frame can be shown to have been sent recently on this interface,
+ * 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)
+{
+ struct hsr_ethhdr *hsr_ethhdr;
+
+ if ((dev_idx < 0) || (dev_idx >= HSR_MAX_DEV)) {
+ WARN_ON(1);
+ return -EINVAL;
+ }
+ if (!skb_mac_header_was_set(skb)) {
+ WARN_ON(1);
+ return -EINVAL;
+ }
+ hsr_ethhdr = (struct hsr_ethhdr *) skb_mac_header(skb);
+
+ if (below_or_eq(hsr_ethhdr->hsr_tag.sequence_nr,
+ node->seq_out[dev_idx]))
+ return 1;
+
+ node->seq_out[dev_idx] = hsr_ethhdr->hsr_tag.sequence_nr;
+ return 0;
+}
+
+
+/*
+ * Remove stale sequence_nr records. Called by timer every
+ * HSR_LIFE_CHECK_INTERVAL (two seconds or so).
+ */
+void hsr_prune_nodes(struct list_head *node_db)
+{
+ struct node_entry *node;
+ unsigned long timestamp;
+
+ rcu_read_lock();
+ list_for_each_entry_rcu(node, node_db, mac_list) {
+
+ timestamp = max(node->time_in[HSR_DEV_SLAVE1],
+ node->time_in[HSR_DEV_SLAVE2]);
+
+ /* Warn only as long as we get frames at all */
+ if (time_is_after_jiffies(timestamp +
+ msecs_to_jiffies(1.5*MAX_SLAVE_DIFF))) {
+
+ /* Check for open ring */
+ if (time_after(node->time_in[HSR_DEV_SLAVE2],
+ node->time_in[HSR_DEV_SLAVE1] +
+ msecs_to_jiffies(MAX_SLAVE_DIFF)))
+ hsr_nl_ringerror(node->MacAddressA, HSR_DEV_SLAVE1);
+ else if (time_after(node->time_in[HSR_DEV_SLAVE1],
+ node->time_in[HSR_DEV_SLAVE2] +
+ msecs_to_jiffies(MAX_SLAVE_DIFF)))
+ hsr_nl_ringerror(node->MacAddressA, HSR_DEV_SLAVE2);
+ }
+
+ /* Prune old entries */
+ if (time_is_before_jiffies(timestamp +
+ msecs_to_jiffies(HSR_NODE_FORGET_TIME))) {
+ hsr_nl_nodedown(node->MacAddressA);
+ list_del_rcu(&node->mac_list);
+ call_rcu(&node->rcu_head, node_entry_reclaim);
+ }
+ }
+ rcu_read_unlock();
+}
+
+
+void hsr_get_node_times(struct hsr_priv *hsr_priv, unsigned char addr[ETH_ALEN],
+ unsigned long *time1, unsigned long *time2)
+{
+ struct node_entry *node;
+
+ rcu_read_lock();
+ node = find_node_by_AddrA(&hsr_priv->node_db, addr);
+ if (!node) {
+ *time1 = 0;
+ *time2 = 0;
+ } else {
+ *time1 = node->time_in[HSR_DEV_SLAVE1];
+ *time2 = node->time_in[HSR_DEV_SLAVE2];
+ }
+ rcu_read_unlock();
+}
diff --git a/net/hsr/hsr_framereg.h b/net/hsr/hsr_framereg.h
new file mode 100644
index 0000000..e579569
--- /dev/null
+++ b/net/hsr/hsr_framereg.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright 2011-2012 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-2012 Arvid Brodin, arvid.brodin@...n.com
+ */
+
+#ifndef _HSR_FRAMEREG_H
+#define _HSR_FRAMEREG_H
+
+#include "hsr_main.h"
+
+enum hsr_dev_idx {
+ HSR_DEV_SLAVE1 = 0,
+ HSR_DEV_SLAVE2,
+ HSR_DEV_MASTER,
+};
+
+struct node_entry;
+
+#define HSR_MAX_SLAVE (HSR_DEV_SLAVE2 + 1)
+#define HSR_MAX_DEV (HSR_DEV_MASTER + 1)
+
+struct node_entry *hsr_find_node(struct list_head *node_db, struct sk_buff *skb);
+
+struct node_entry *hsr_merge_node(struct hsr_priv *hsr_priv,
+ struct node_entry *node,
+ struct sk_buff *skb);
+
+void hsr_addr_subst(struct hsr_priv *hsr_priv, struct sk_buff *skb);
+
+void hsr_register_frame_in(struct node_entry *node, enum hsr_dev_idx);
+
+int hsr_register_frame_out(struct node_entry *node, enum hsr_dev_idx,
+ struct sk_buff *skb);
+
+void hsr_prune_nodes(struct list_head *node_db);
+
+void hsr_get_node_times(struct hsr_priv *hsr_priv, unsigned char addr[ETH_ALEN],
+ unsigned long *time1, unsigned long *time2);
+
+int hsr_create_self_node(struct list_head *self_node_db,
+ unsigned char addr_a[ETH_ALEN],
+ unsigned char addr_b[ETH_ALEN]);
+
+#endif /* _HSR_FRAMEREG_H */
diff --git a/net/hsr/hsr_main.c b/net/hsr/hsr_main.c
new file mode 100644
index 0000000..11747b3
--- /dev/null
+++ b/net/hsr/hsr_main.c
@@ -0,0 +1,468 @@
+/*
+ * Copyright 2011-2012 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-2012 Arvid Brodin, arvid.brodin@...n.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_HSR.
+ */
+
+#include <linux/netdevice.h>
+#include <linux/rculist.h>
+#include <linux/timer.h>
+#include <linux/etherdevice.h>
+#include "hsr_main.h"
+#include "hsr_device.h"
+#include "hsr_netlink.h"
+#include "hsr_framereg.h"
+
+
+/* Multicast address for HSR Supervision frames */
+const u8 hsr_multicast_addr[ETH_ALEN] = {0x01, 0x15, 0x4e, 0x00, 0x01, 0x00};
+
+
+/* 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;
+}
+
+
+static int hsr_netdev_notify(struct notifier_block *nb, unsigned long event,
+ void *ptr)
+{
+/*
+ * Slave NETDEV_GOING_DOWN - disallow for slaves? No use really, let's
+ * assume the admin user knows what he/she is doing...
+ */
+
+ struct net_device *slave, *other_slave;
+ struct hsr_priv *hsr_priv;
+ int old_operstate;
+ int mtu_max;
+ int res;
+
+ hsr_priv = get_hsr_master(ptr);
+ if (hsr_priv) { /* Is ptr a slave device? */
+ slave = ptr;
+ other_slave = get_other_slave(hsr_priv, slave);
+ } else {
+ if (!is_hsr_master(ptr))
+ return NOTIFY_DONE;
+ hsr_priv = netdev_priv(ptr);
+ slave = hsr_priv->slave[0];
+ other_slave = hsr_priv->slave[1];
+ }
+
+ 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);
+ break;
+ case NETDEV_CHANGEADDR:
+ /* This should not happen since there's no ndo_set_mac_address()
+ * for HSR devices - i.e. not supported. */
+ if (ptr == hsr_priv->dev)
+ break;
+ if (ptr == hsr_priv->slave[0])
+ memcpy(hsr_priv->dev->dev_addr,
+ hsr_priv->slave[0]->dev_addr, ETH_ALEN);
+
+ /* 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);
+ if (res)
+ netdev_warn(hsr_priv->dev,
+ "Could not update HSR node address.\n");
+
+ if (ptr == hsr_priv->slave[0])
+ call_netdevice_notifiers(NETDEV_CHANGEADDR, hsr_priv->dev);
+ break;
+ case NETDEV_CHANGEMTU:
+ if (ptr == hsr_priv->dev)
+ 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);
+ break;
+ case NETDEV_UNREGISTER:
+ if (ptr == hsr_priv->slave[0])
+ hsr_priv->slave[0] = NULL;
+ if (ptr == hsr_priv->slave[1])
+ hsr_priv->slave[1] = NULL;
+
+ /* There should really be a way to set a new slave device... */
+
+ break;
+ case NETDEV_PRE_TYPE_CHANGE:
+ /* HSR works only on Ethernet devices. Refuse slave to change
+ * its type. */
+ return NOTIFY_BAD;
+ }
+
+ return NOTIFY_DONE;
+}
+
+
+static struct timer_list prune_timer;
+
+static void prune_nodes_all(unsigned long data)
+{
+ struct hsr_priv *hsr_priv;
+
+ rcu_read_lock();
+ list_for_each_entry_rcu(hsr_priv, &hsr_list, hsr_list)
+ hsr_prune_nodes(&hsr_priv->node_db);
+ rcu_read_unlock();
+
+ prune_timer.expires = jiffies + msecs_to_jiffies(PRUNE_PERIOD);
+ add_timer(&prune_timer);
+}
+
+
+static struct sk_buff *strip_hsr_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);
+ 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 sk_buff *skb)
+{
+ struct hsr_sup_tag *hsr_stag;
+
+ if (compare_ether_addr(eth_hdr(skb)->h_dest, hsr_multicast_addr))
+ return 0;
+
+ hsr_stag = (struct hsr_sup_tag *) skb->data;
+ if (get_hsr_stag_path(hsr_stag) != 0x0f)
+ return 0;
+ if ((hsr_stag->HSR_TLV_Type != HSR_TLV_ANNOUNCE) &&
+ (hsr_stag->HSR_TLV_Type != HSR_TLV_LIFE_CHECK))
+ return 0;
+ if (hsr_stag->HSR_TLV_Length != 12)
+ return 0;
+
+ return 1;
+}
+
+
+/*
+ * 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;
+ int 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_SLAVE1;
+ dev_other_idx = HSR_DEV_SLAVE2;
+ } else {
+ dev_in_idx = HSR_DEV_SLAVE2;
+ dev_other_idx = HSR_DEV_SLAVE1;
+ }
+
+ 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 = 0;
+ if ((skb->pkt_type == PACKET_HOST) ||
+ (skb->pkt_type == PACKET_MULTICAST) ||
+ (skb->pkt_type == PACKET_BROADCAST))
+ deliver_to_self = 1;
+ else if (!compare_ether_addr(eth_hdr(skb)->h_dest,
+ hsr_priv->dev->dev_addr)) {
+ skb->pkt_type = PACKET_HOST;
+ deliver_to_self = 1;
+ }
+
+
+ rcu_read_lock(); /* node_db */
+ node = hsr_find_node(&hsr_priv->node_db, skb);
+
+ if (is_supervision_frame(skb)) {
+ skb_pull(skb, sizeof(struct hsr_sup_tag));
+ node = hsr_merge_node(hsr_priv, node, skb);
+ 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 = 0;
+ }
+
+ if (!node) {
+ /* Source node unknown; don't create a network loop */
+ rcu_read_unlock(); /* node_db */
+ netdev_info(dev, "HSR: Got HSR frame from unknown node %pM: "
+ "dropping it.\n",
+ eth_hdr(skb)->h_source);
+ kfree_skb(skb);
+ hsr_priv->dev->stats.rx_errors++;
+ 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 = 0;
+
+ 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) {
+#if !defined(CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS)
+ /* We have to memmove the whole payload below */
+ skb_deliver = skb_copy(skb, GFP_ATOMIC);
+#else
+ skb_deliver = skb_clone(skb, GFP_ATOMIC);
+#endif
+ if (!skb_deliver) {
+ deliver_to_self = 0;
+ hsr_priv->dev->stats.rx_dropped++;
+ }
+ }
+
+ if (deliver_to_self) {
+ bool multicast_frame;
+
+ skb_deliver = strip_hsr_tag(skb_deliver);
+ if (!skb_deliver) {
+ hsr_priv->dev->stats.rx_dropped++;
+ goto forward;
+ }
+#if !defined(CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS)
+ /*
+ * skb_deliver should be linear here, after the call to
+ * skb_copy() above. We need to memmove the whole payload to
+ * work around alignment problems caused by the 6-byte HSR tag.
+ */
+ memmove(skb_deliver->data - HSR_TAGLEN, skb_deliver->data,
+ skb_deliver->len);
+ skb_deliver->data -= HSR_TAGLEN;
+ skb_deliver->tail -= HSR_TAGLEN;
+#endif
+ skb_deliver->dev = hsr_priv->dev;
+ hsr_addr_subst(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_HSR),
+ .func = hsr_rcv,
+};
+
+static struct notifier_block hsr_nb = {
+ .notifier_call = hsr_netdev_notify, /* Slave event notifications */
+};
+
+
+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);
+
+ register_netdevice_notifier(&hsr_nb);
+
+ res = hsr_netlink_init();
+
+ return res;
+}
+
+static void __exit hsr_exit(void)
+{
+ unregister_netdevice_notifier(&hsr_nb);
+ del_timer(&prune_timer);
+ hsr_netlink_exit();
+ dev_remove_pack(&hsr_pt);
+}
+
+module_init(hsr_init);
+module_exit(hsr_exit);
+MODULE_LICENSE("GPL");
diff --git a/net/hsr/hsr_main.h b/net/hsr/hsr_main.h
new file mode 100644
index 0000000..e2a4c6d
--- /dev/null
+++ b/net/hsr/hsr_main.h
@@ -0,0 +1,157 @@
+/*
+ * Copyright 2011-2012 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-2012 Arvid Brodin, arvid.brodin@...n.com
+ */
+
+#ifndef _HSR_PRIVATE_H
+#define _HSR_PRIVATE_H
+
+#include <linux/netdevice.h>
+#include <linux/list.h>
+
+
+/*
+ * Time constants as specified in the HSR specification (IEC-62439-3 2010)
+ * Table 8.
+ * All values in milliseconds.
+ */
+#define HSR_LIFE_CHECK_INTERVAL 2000 /* ms */
+#define HSR_NODE_FORGET_TIME 60000 /* ms */
+#define HSR_ANNOUNCE_INTERVAL 100 /* ms */
+
+/*
+ * By how much may slave1 and slave2 timestamps of latest received frame from
+ * each node differ before we notify of communication problem?
+ */
+#define MAX_SLAVE_DIFF 3000 /* ms */
+
+/*
+ * How often shall we check for broken ring and remove node entries older than
+ * HSR_NODE_FORGET_TIME?
+ */
+#define PRUNE_PERIOD 3000 /* ms */
+
+
+#define HSR_TLV_ANNOUNCE 22
+#define HSR_TLV_LIFE_CHECK 23
+
+
+/*
+ * HSR Tag.
+ * As defined in IEC-62439-3:2010, the HSR tag is really { ethertype = 0x88FB,
+ * 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.
+ */
+#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;
+
+/*
+ * 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
+ * equivalently, the 4 most significant bits of HSR tag byte 14).
+ *
+ * This is unclear in the IEC specification; its definition of MAC addresses
+ * indicates the spec is written with the least significant bit first (to the
+ * left). This, however, would mean that the LSDU field would be split in two
+ * with the path field in-between, which seems strange. I'm guessing the MAC
+ * address definition is in error.
+ */
+static inline u16 get_hsr_tag_path(struct hsr_tag *ht)
+{
+ return ntohs(ht->path_and_LSDU_size) >> 12;
+}
+
+static inline u16 get_hsr_tag_LSDU_size(struct hsr_tag *ht)
+{
+ return ntohs(ht->path_and_LSDU_size) & 0x0FFF;
+}
+
+static inline void set_hsr_tag_path(struct hsr_tag *ht, u16 path)
+{
+ ht->path_and_LSDU_size = htons(
+ (ntohs(ht->path_and_LSDU_size) & 0x0FFF) | (path << 12));
+}
+
+static inline void set_hsr_tag_LSDU_size(struct hsr_tag *ht, u16 LSDU_size)
+{
+ ht->path_and_LSDU_size = htons(
+ (ntohs(ht->path_and_LSDU_size) & 0xF000) |
+ (LSDU_size & 0x0FFF));
+}
+
+struct hsr_ethhdr {
+ struct ethhdr ethhdr;
+ struct hsr_tag hsr_tag;
+} __packed;
+
+
+/*
+ * HSR Supervision Frame data types.
+ * Field names as defined in the IEC:2010 standard for HSR.
+ */
+struct hsr_sup_tag {
+ __be16 path_and_HSR_Ver;
+ __be16 sequence_nr;
+ __u8 HSR_TLV_Type;
+ __u8 HSR_TLV_Length;
+} __packed;
+
+struct hsr_sup_payload {
+ unsigned char MacAddressA[ETH_ALEN];
+} __packed;
+
+static inline u16 get_hsr_stag_path(struct hsr_sup_tag *hst)
+{
+ return get_hsr_tag_path((struct hsr_tag *) hst);
+}
+
+static inline u16 get_hsr_stag_HSR_ver(struct hsr_sup_tag *hst)
+{
+ return get_hsr_tag_LSDU_size((struct hsr_tag *) hst);
+}
+
+static inline void set_hsr_stag_path(struct hsr_sup_tag *hst, u16 path)
+{
+ set_hsr_tag_path((struct hsr_tag *) hst, path);
+}
+
+static inline void set_hsr_stag_HSR_Ver(struct hsr_sup_tag *hst, u16 HSR_Ver)
+{
+ set_hsr_tag_LSDU_size((struct hsr_tag *) hst, HSR_Ver);
+}
+
+
+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[2];
+ struct list_head node_db; /* Other HSR nodes */
+ struct list_head self_node_db; /* MACs of slaves */
+ struct timer_list announce_timer; /* Supervision frame dispatch */
+ int announce_count;
+ u16 sequence_nr;
+ spinlock_t seqnr_lock; /* locking for sequence_nr */
+};
+
+extern const u8 hsr_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);
+
+#endif /* _HSR_PRIVATE_H */
diff --git a/net/hsr/hsr_netlink.c b/net/hsr/hsr_netlink.c
new file mode 100644
index 0000000..877268b
--- /dev/null
+++ b/net/hsr/hsr_netlink.c
@@ -0,0 +1,305 @@
+/*
+ * Copyright 2011-2012 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-2012 Arvid Brodin, arvid.brodin@...n.com
+ *
+ * Routines for handling Netlink messages for HSR.
+ */
+
+#include "hsr_netlink.h"
+#include <linux/kernel.h>
+#include <net/rtnetlink.h>
+#include <net/genetlink.h>
+#include "hsr_main.h"
+#include "hsr_device.h"
+#include "hsr_framereg.h"
+
+static const struct nla_policy hsr_policy[IFLA_HSR_MAX + 1] = {
+ [IFLA_HSR_SLAVE1] = { .type = NLA_U32 },
+ [IFLA_HSR_SLAVE2] = { .type = NLA_U32 },
+};
+
+
+/*
+ * Here, it seems a netdevice has already been allocated for us, and the
+ * hsr_dev_setup routine has been executed. Nice!
+ */
+static int hsr_newlink(struct net *src_net, struct net_device *dev,
+ struct nlattr *tb[], struct nlattr *data[])
+{
+ struct net_device *link[2];
+
+ if (!data[IFLA_HSR_SLAVE1]) {
+ netdev_info(dev, "IFLA_HSR_SLAVE1 missing!\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");
+ return -EINVAL;
+ }
+ link[1] = __dev_get_by_index(src_net, nla_get_u32(data[IFLA_HSR_SLAVE2]));
+
+ if (!link[0] || !link[1])
+ return -ENODEV;
+ if (link[0] == link[1])
+ return -EINVAL;
+
+ return hsr_dev_finalize(dev, link);
+}
+
+static struct rtnl_link_ops hsr_link_ops __read_mostly = {
+ .kind = "hsr",
+ .maxtype = IFLA_HSR_MAX,
+ .policy = hsr_policy,
+ .priv_size = sizeof(struct hsr_priv),
+ .setup = hsr_dev_setup,
+ .newlink = hsr_newlink,
+};
+
+
+
+/* attribute policy */
+/* NLA_BINARY missing in libnl; use NLA_UNSPEC in userspace instead. */
+static const struct nla_policy hsr_genl_policy[HSR_A_MAX + 1] = {
+ [HSR_A_NODE_ADDR] = { .type = NLA_BINARY, .len = ETH_ALEN },
+ [HSR_A_IFINDEX] = { .type = NLA_U32 },
+ [HSR_A_IF1AGE] = { .type = NLA_U32 }, /* 32-bit int */
+ [HSR_A_IF2AGE] = { .type = NLA_U32 }, /* 32-bit int */
+};
+
+static struct genl_family hsr_genl_family = {
+ .id = GENL_ID_GENERATE,
+ .hdrsize = 0,
+ .name = "HSR",
+ .version = 1,
+ .maxattr = HSR_A_MAX,
+};
+
+static struct genl_multicast_group hsr_network_genl_mcgrp = {
+ .name = "hsr-network",
+};
+
+static int hsr_genl_seq = 0;
+
+
+
+static struct sk_buff *hsr_create_genl_msg(void **pmsg_head, unsigned gfp,
+ int cmd)
+{
+ struct sk_buff *skb;
+
+ skb = genlmsg_new(NLMSG_GOODSIZE, gfp);
+ if (!skb)
+ return NULL;
+
+ *pmsg_head = genlmsg_put(skb, 0, hsr_genl_seq++, &hsr_genl_family, 0,
+ cmd);
+ if (!pmsg_head) {
+ kfree_skb(skb);
+ return NULL;
+ }
+
+ return skb;
+}
+
+
+/*
+ * This is called if for some node with MAC address addr, we only get frames
+ * over one of the slave interfaces. This would indicate an open network ring
+ * (i.e. a link has failed somewhere).
+ */
+void hsr_nl_ringerror(unsigned char addr[ETH_ALEN], int dev_idx)
+{
+ struct sk_buff *skb;
+ void *msg_head;
+ int res;
+
+ skb = hsr_create_genl_msg(&msg_head, GFP_ATOMIC, HSR_C_RING_ERROR);
+ if (!skb)
+ return;
+
+ res = nla_put(skb, HSR_A_NODE_ADDR, ETH_ALEN, addr);
+ if (res < 0)
+ goto nla_put_failure;
+ res = nla_put_u32(skb, HSR_A_IFINDEX, dev_idx);
+ if (res < 0)
+ goto nla_put_failure;
+
+ genlmsg_end(skb, msg_head);
+ genlmsg_multicast(skb, 0, hsr_network_genl_mcgrp.id, GFP_ATOMIC);
+
+ return;
+
+nla_put_failure:
+ kfree_skb(skb);
+}
+
+/*
+ * 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(unsigned char addr[ETH_ALEN])
+{
+ struct sk_buff *skb;
+ void *msg_head;
+ int res;
+
+ skb = hsr_create_genl_msg(&msg_head, GFP_ATOMIC, HSR_C_NODE_DOWN);
+ if (!skb)
+ return;
+
+ res = nla_put(skb, HSR_A_NODE_ADDR, ETH_ALEN, addr);
+ if (res < 0)
+ goto nla_put_failure;
+
+ genlmsg_end(skb, msg_head);
+ genlmsg_multicast(skb, 0, hsr_network_genl_mcgrp.id, GFP_ATOMIC);
+
+ return;
+
+nla_put_failure:
+ kfree_skb(skb);
+}
+
+/*
+ * HSR_C_GET_NODE_STATUS lets userspace query the internal HSR node table
+ * about the status of a specific node in the network, defined by its MAC
+ * address.
+ *
+ * Input: hsr ifindex, node mac address
+ * Output: hsr ifindex, node mac address (copied from request),
+ * age of latest frame from node over slave 1, slave 2 [ms]
+ */
+static int hsr_get_node_status(struct sk_buff *skb_in, struct genl_info *info)
+{
+ /* For receiving */
+ struct nlattr *na;
+ char *node_addr;
+ struct net_device *hsr_dev;
+
+ /* For sending */
+ struct sk_buff *skb_out;
+ void *msg_head;
+ struct hsr_priv *hsr_priv;
+ unsigned long time1, time2;
+ int res;
+
+ if (!info)
+ goto invalid;
+
+ na = info->attrs[HSR_A_IFINDEX];
+ if (!na)
+ goto invalid;
+ na = info->attrs[HSR_A_NODE_ADDR];
+ if (!na)
+ goto invalid;
+
+ hsr_dev = __dev_get_by_index(genl_info_net(info),
+ nla_get_u32(info->attrs[HSR_A_IFINDEX]));
+ if (!hsr_dev)
+ goto invalid;
+ if (!is_hsr_master(hsr_dev))
+ goto invalid;
+
+
+ /* Send reply */
+
+ skb_out = hsr_create_genl_msg(&msg_head, GFP_ATOMIC,
+ HSR_C_SET_NODE_STATUS);
+ if (!skb_out)
+ return -ENOMEM;
+
+ res = nla_put_u32(skb_out, HSR_A_IFINDEX, hsr_dev->ifindex);
+ if (res < 0)
+ goto nla_put_failure;
+
+ node_addr = nla_data(info->attrs[HSR_A_NODE_ADDR]);
+ res = nla_put(skb_out, HSR_A_NODE_ADDR, ETH_ALEN, node_addr);
+ if (res < 0)
+ goto nla_put_failure;
+
+ hsr_priv = netdev_priv(hsr_dev);
+ hsr_get_node_times(hsr_priv, node_addr, &time1, &time2);
+
+ res = nla_put_u32(skb_out, HSR_A_IF1AGE, time1 ?
+ jiffies_to_msecs(jiffies - time1) : -1);
+ if (res < 0)
+ goto nla_put_failure;
+ res = nla_put_u32(skb_out, HSR_A_IF2AGE, time2 ?
+ jiffies_to_msecs(jiffies - time2) : -1);
+ if (res < 0)
+ goto nla_put_failure;
+
+ genlmsg_end(skb_out, msg_head);
+ genlmsg_unicast(genl_info_net(info), skb_out, info->snd_pid);
+
+ return 0;
+
+nla_put_failure:
+ kfree_skb(skb_out);
+
+ return -ENOMEM;
+
+invalid:
+ return -EINVAL;
+}
+
+static struct genl_ops hsr_ops_get_node_status = {
+ .cmd = HSR_C_GET_NODE_STATUS,
+ .flags = 0,
+ .policy = hsr_genl_policy,
+ .doit = hsr_get_node_status,
+ .dumpit = NULL,
+};
+
+
+int __init hsr_netlink_init(void)
+{
+ int rc;
+
+ rc = rtnl_link_register(&hsr_link_ops);
+ if (rc)
+ goto fail_rtnl_link_register;
+
+ rc = genl_register_family(&hsr_genl_family);
+ if (rc)
+ goto fail_genl_register_family;
+
+ rc = genl_register_ops(&hsr_genl_family, &hsr_ops_get_node_status);
+ if (rc)
+ goto fail_genl_register_ops;
+
+ rc = genl_register_mc_group(&hsr_genl_family, &hsr_network_genl_mcgrp);
+ if (rc)
+ goto fail_genl_register_mc_group;
+
+ return 0;
+
+fail_genl_register_mc_group:
+ genl_unregister_ops(&hsr_genl_family, &hsr_ops_get_node_status);
+fail_genl_register_ops:
+ genl_unregister_family(&hsr_genl_family);
+fail_genl_register_family:
+ rtnl_link_unregister(&hsr_link_ops);
+fail_rtnl_link_register:
+
+ return rc;
+}
+
+void __exit hsr_netlink_exit(void)
+{
+ genl_unregister_mc_group(&hsr_genl_family, &hsr_network_genl_mcgrp);
+ genl_unregister_ops(&hsr_genl_family, &hsr_ops_get_node_status);
+ genl_unregister_family(&hsr_genl_family);
+
+ rtnl_link_unregister(&hsr_link_ops);
+}
+
+MODULE_ALIAS_RTNL_LINK("hsr");
diff --git a/net/hsr/hsr_netlink.h b/net/hsr/hsr_netlink.h
new file mode 100644
index 0000000..4282d9f
--- /dev/null
+++ b/net/hsr/hsr_netlink.h
@@ -0,0 +1,64 @@
+/*
+ * Copyright 2011-2012 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-2012 Arvid Brodin, arvid.brodin@...n.com
+ */
+
+#ifndef __HSR_NETLINK_H
+#define __HSR_NETLINK_H
+
+/* attributes */
+enum {
+ HSR_A_UNSPEC,
+ HSR_A_NODE_ADDR,
+ HSR_A_IFINDEX,
+ HSR_A_IF1AGE,
+ HSR_A_IF2AGE,
+ __HSR_A_MAX,
+};
+#define HSR_A_MAX (__HSR_A_MAX - 1)
+
+
+#ifdef __KERNEL__
+
+#include <linux/if_ether.h>
+#include <linux/module.h>
+
+int __init hsr_netlink_init(void);
+void __exit hsr_netlink_exit(void);
+
+void hsr_nl_ringerror(unsigned char addr[ETH_ALEN], int dev_idx);
+void hsr_nl_nodedown(unsigned char addr[ETH_ALEN]);
+void hsr_nl_framedrop(int dropcount, int dev_idx);
+void hsr_nl_linkdown(int dev_idx);
+
+
+/*
+ * Generic Netlink HSR family definition
+ */
+
+
+#endif /* __KERNEL__ */
+
+
+
+/* commands */
+enum {
+ HSR_C_UNSPEC,
+ HSR_C_RING_ERROR,
+ HSR_C_NODE_DOWN,
+ HSR_C_GET_NODE_STATUS,
+ HSR_C_SET_NODE_STATUS,
+ __HSR_C_MAX,
+};
+#define HSR_C_MAX (__HSR_C_MAX - 1)
+
+
+
+#endif /* __HSR_NETLINK_H */
--
1.7.3.4
--
Arvid Brodin | Consultant (Linux)
XDIN AB | Knarrarnäsgatan 7 | SE-164 40 Kista | Sweden | xdin.com--
To unsubscribe from this list: send the line "unsubscribe netdev" in
the body of a message to majordomo@...r.kernel.org
More majordomo info at http://vger.kernel.org/majordomo-info.html
Powered by blists - more mailing lists