@@ -0,0 +1,72 @@
+/*
+ * Copyright 2011-2013 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-2013 Arvid Brodin, arvid.brodin@xdin.com
+ */
+
+#ifndef __HSR_NETLINK_H
+#define __HSR_NETLINK_H
+
+/* attributes */
+enum {
+ HSR_A_UNSPEC,
+ HSR_A_NODE_ADDR,
+ HSR_A_IFINDEX,
+ HSR_A_IF1_AGE,
+ HSR_A_IF2_AGE,
+ HSR_A_NODE_ADDR_B,
+ HSR_A_IF1_SEQ,
+ HSR_A_IF2_SEQ,
+ HSR_A_IF1_IFINDEX,
+ HSR_A_IF2_IFINDEX,
+ HSR_A_ADDR_B_IFINDEX,
+ __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_GET_NODE_LIST,
+ HSR_C_SET_NODE_LIST,
+ __HSR_C_MAX,
+};
+#define HSR_C_MAX (__HSR_C_MAX - 1)
+
+
+
+#endif /* __HSR_NETLINK_H */
@@ -0,0 +1,504 @@
+/*
+ * Copyright 2011-2013 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-2013 Arvid Brodin, arvid.brodin@xdin.com
+ *
+ * Userspace example of using Generic Netlink (through libnl-3) to get HSR
+ * ("High-availability Seamless Redundancy") link/network status.
+ */
+
+
+/*
+
+Manual static cross-build:
+
+$ PATH=[toolchain-path]/usr/bin/:${PATH} avr32-unknown-linux-uclibc-gcc -Wall -g -I[toolchain-path]/usr/include/libnl3 -static -L[toolchain-path]/usr/lib hsrinfo.c -o hsrinfo -lnl-genl-3 -lnl-3 -pthread -lm
+
+Native build:
+$ gcc -Wall -g -I /usr/include/libnl3/ -lnl-3 -lnl-genl-3 hsrinfo.c -o hsrinfo-x86
+
+*/
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <netlink/netlink.h>
+#include <netlink/socket.h>
+#include <netlink/attr.h>
+#include <netlink/genl/genl.h>
+#include <netlink/genl/ctrl.h>
+#include <net/if.h>
+#include <linux/if_ether.h>
+#include "hsr_netlink.h"
+
+struct node_item {
+ struct node_item *next;
+ unsigned char addr[ETH_ALEN];
+};
+
+static struct node_item *node_head = NULL;
+
+static int seq_nr = 10;
+
+
+static void nodelist_clear(struct node_item **ni)
+{
+ if (!*ni)
+ return;
+
+ nodelist_clear(&(*ni)->next);
+
+ free(*ni);
+ (*ni) = NULL;
+}
+
+static void nodelist_add(struct node_item **head, const char addr[ETH_ALEN])
+{
+ struct node_item **ni;
+
+ ni = head;
+ while (*ni)
+ ni = &(*ni)->next;
+
+ *ni = calloc(1, sizeof(struct node_item));
+ if (!*ni)
+ return; // No mem
+
+ memcpy((*ni)->addr, addr, ETH_ALEN);
+}
+
+
+static void print_mac(const unsigned char *addr)
+{
+ int i;
+
+ if (!addr) {
+ printf("(null) ");
+ return;
+ }
+
+ for (i = 0; i < ETH_ALEN - 1; i++)
+ printf("%02x:", addr[i]);
+ printf("%02x", addr[ETH_ALEN - 1]);
+}
+
+
+static void parse_ring_error(struct genlmsghdr *hdr)
+{
+ struct nlattr *attr;
+ unsigned char *AddrA;
+ int ifindex;
+ char ifname[IF_NAMESIZE];
+ char *nameptr;
+
+ printf("Ring error: ");
+
+ AddrA = NULL;
+ ifindex = -1;
+
+ attr = genlmsg_attrdata(hdr, 0);
+ int remaining = genlmsg_attrlen(hdr, 0);
+ while (nla_ok(attr, remaining)) {
+ switch (attr->nla_type) {
+ case HSR_A_NODE_ADDR:
+ AddrA = nla_data(attr);
+ break;
+ case HSR_A_IFINDEX:
+ ifindex = nla_get_u32(attr);
+ break;
+ default:
+ printf("unknown attribute type: %d\n", attr->nla_type);
+ }
+ attr = nla_next(attr, &remaining);
+ }
+
+ if (!AddrA) {
+ printf("Error: invalid HSR_C_RING_ERROR packet\n");
+ return;
+ }
+
+ nameptr = if_indextoname(ifindex, ifname);
+ if (!nameptr)
+ snprintf(ifname, IF_NAMESIZE, "if%d", ifindex);
+ printf("interface %s, node ", ifname);
+ print_mac(AddrA);
+ printf("\n");
+}
+
+static void parse_node_down(struct genlmsghdr *hdr)
+{
+ struct nlattr *attr;
+ unsigned char *AddrA;
+
+ printf("Node down: ");
+
+ AddrA = NULL;
+
+ attr = genlmsg_attrdata(hdr, 0);
+ int remaining = genlmsg_attrlen(hdr, 0);
+ while (nla_ok(attr, remaining)) {
+ switch (attr->nla_type) {
+ case HSR_A_NODE_ADDR:
+ AddrA = nla_data(attr);
+ break;
+ default:
+ printf("unknown attribute type: %d\n", attr->nla_type);
+ }
+ attr = nla_next(attr, &remaining);
+ }
+
+ if (!AddrA) {
+ printf("Error: invalid HSR_C_NODE_DOWN packet\n");
+ return;
+ }
+
+ print_mac(AddrA);
+ printf("\n");
+}
+
+static void parse_node_status(struct genlmsghdr *hdr)
+{
+ unsigned char *AddrA, *AddrB;
+ int if1_age, if2_age;
+ int if1_seq, if2_seq;
+ int if1_ifindex, if2_ifindex, addr_b_ifindex;
+ char if1_ifname[IF_NAMESIZE];
+ char if2_ifname[IF_NAMESIZE];
+ char addr_b_ifname[IF_NAMESIZE];
+ char *nameptr;
+ struct nlattr *attr;
+
+ AddrA = NULL;
+ AddrB = NULL;
+ if1_age = -1;
+ if2_age = -1;
+ if1_seq = -1;
+ if2_seq = -1;
+ if1_ifindex = -1;
+ if2_ifindex = -1;
+ addr_b_ifindex = -1;
+
+ attr = genlmsg_attrdata(hdr, 0);
+ int remaining = genlmsg_attrlen(hdr, 0);
+ while (nla_ok(attr, remaining)) {
+ switch (attr->nla_type) {
+ case HSR_A_NODE_ADDR:
+ if (AddrA)
+ printf("%s: Too many AddrA in message!\n", __func__);
+ AddrA = nla_data(attr);
+ break;
+ case HSR_A_NODE_ADDR_B:
+ if (AddrB)
+ printf("%s: Too many AddrB in message!\n", __func__);
+ AddrB = nla_data(attr);
+ break;
+ case HSR_A_IFINDEX:
+ break;
+ case HSR_A_IF1_AGE:
+ if1_age = (int) nla_get_u32(attr);
+ break;
+ case HSR_A_IF2_AGE:
+ if2_age = (int) nla_get_u32(attr);
+ break;
+ case HSR_A_IF1_SEQ:
+ if1_seq = nla_get_u16(attr);
+ break;
+ case HSR_A_IF2_SEQ:
+ if2_seq = nla_get_u16(attr);
+ break;
+ case HSR_A_IF1_IFINDEX:
+ if1_ifindex = nla_get_u32(attr);
+ break;
+ case HSR_A_IF2_IFINDEX:
+ if2_ifindex = nla_get_u32(attr);
+ break;
+ case HSR_A_ADDR_B_IFINDEX:
+ addr_b_ifindex = nla_get_u32(attr);
+ break;
+ default:
+ printf("%s: unknown attribute type: %d\n", __func__, attr->nla_type);
+ }
+ attr = nla_next(attr, &remaining);
+ }
+
+ if (!AddrA) {
+ printf("Error: invalid HSR_C_SET_NODE_STATUS packet\n");
+ return;
+ }
+
+ nameptr = if_indextoname(if1_ifindex, if1_ifname);
+ if (!nameptr)
+ snprintf(if1_ifname, IF_NAMESIZE, "if%d", if1_ifindex);
+ nameptr = if_indextoname(if2_ifindex, if2_ifname);
+ if (!nameptr)
+ snprintf(if2_ifname, IF_NAMESIZE, "if%d", if2_ifindex);
+ nameptr = if_indextoname(addr_b_ifindex, addr_b_ifname);
+ if (!nameptr)
+ snprintf(addr_b_ifname, IF_NAMESIZE, "if%d", addr_b_ifindex);
+
+ printf("Node: ");
+ print_mac(AddrA);
+ if (AddrB) {
+ printf(" AddrB: ");
+ print_mac(AddrB);
+ printf(" (over %s)", addr_b_ifname);
+ }
+ printf("\n Sequence nr (age): %s: %5d (%5d ms); %s: %5d (%5d ms)\n",
+ if1_ifname, if1_seq, if1_age,
+ if2_ifname, if2_seq, if2_age);
+}
+
+static void parse_node_list(struct genlmsghdr *hdr)
+{
+ struct nlattr *attr;
+
+ nodelist_clear(&node_head);
+
+ attr = genlmsg_attrdata(hdr, 0);
+ int remaining = genlmsg_attrlen(hdr, 0);
+ while (nla_ok(attr, remaining)) {
+ switch (attr->nla_type) {
+ case HSR_A_NODE_ADDR:
+ nodelist_add(&node_head, nla_data(attr));
+ break;
+ default:
+ printf("Unknown attribute type for HSR_C_SET_NODE_LIST: %d\n", attr->nla_type);
+ }
+ attr = nla_next(attr, &remaining);
+ }
+}
+
+
+static int parse_genlmsg(struct nl_msg *msg, void *arg)
+{
+ struct genlmsghdr *hdr;
+
+ /*
+ * 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));
+
+// printf("%d: ", nlmsg_hdr(msg)->nlmsg_seq);
+ switch (hdr->cmd) {
+ case HSR_C_RING_ERROR:
+ parse_ring_error(hdr);
+ break;
+ case HSR_C_NODE_DOWN:
+ parse_node_down(hdr);
+ break;
+ case HSR_C_SET_NODE_STATUS:
+ parse_node_status(hdr);
+ break;
+ case HSR_C_SET_NODE_LIST:
+ parse_node_list(hdr);
+ break;
+ default:
+ printf("Unknown genl message received (%d)\n", hdr->cmd);
+ }
+
+ return 0;
+}
+
+
+static int query_get_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, seq_nr++, family,
+ 0, NLM_F_REQUEST, HSR_C_GET_NODE_STATUS, 1);
+ if (!user_hdr)
+ goto nla_put_failure;
+
+ 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;
+}
+
+
+static int query_get_node_list(struct nl_sock *nlsk, int family, int ifindex)
+{
+ struct nl_msg *msg;
+ void *user_hdr;
+
+ msg = nlmsg_alloc();
+ if (!msg)
+ return -1;
+
+ user_hdr = genlmsg_put(msg, NL_AUTO_PORT, seq_nr++, family,
+ 0, NLM_F_REQUEST, HSR_C_GET_NODE_LIST, 1);
+ if (!user_hdr)
+ goto nla_put_failure;
+
+ NLA_PUT_U32(msg, HSR_A_IFINDEX, ifindex);
+
+// printf("Querying if %d for node list\n", ifindex);
+
+ return (nl_send_auto(nlsk, msg));
+
+nla_put_failure:
+ nlmsg_free(msg);
+ return -1;
+}
+
+
+
+static void print_usage(const char *name)
+{
+ printf(
+"Usage: %s [-q] interface [node mac address]\n"
+"Display ring error messages for a HSR network interface, or\n"
+"(-q) query the interface node database. The node address parameter is only\n"
+"valid with -q, and limits the output to data about a specific node.\n", name);
+}
+
+
+static const char optstring[] = "+q";
+
+int main(int argc, char **argv)
+{
+ struct nl_sock *nlsk;
+ int hsr_mgroup;
+ int query;
+ int opt, rc;
+ int hsr_ifindex;
+ struct node_item *ni;
+
+ query = 0;
+ opt = getopt(argc, argv, optstring);
+ while (opt != -1) {
+ switch (opt) {
+ case 'q':
+ query = 1;
+ break;
+ default:
+ print_usage(argv[0]);
+ return EXIT_FAILURE;
+ }
+ opt = getopt(argc, argv, optstring);
+ }
+
+
+ nlsk = nl_socket_alloc();
+ if (!nlsk) {
+ printf("nl_socket_alloc() failed\n");
+ return EXIT_FAILURE;
+ }
+ 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;
+ }
+
+ nl_socket_disable_seq_check(nlsk);
+
+ if (!query) {
+ if (argc - optind != 1) {
+ print_usage(argv[0]);
+ return EXIT_FAILURE;
+ }
+
+// 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;
+ }
+
+ while (1)
+ nl_recvmsgs_default(nlsk);
+
+ /* Not reached */
+ }
+
+
+ if (argc - optind < 1) {
+ print_usage(argv[0]);
+ return EXIT_FAILURE;
+ }
+
+ /* The hsr if we send the enquiry to (get it with e.g.
+ * 'cat /sys/class/net/hsr0/ifindex'): */
+ hsr_ifindex = if_nametoindex(argv[optind]);
+ if (hsr_ifindex == 0) {
+ printf("%s: %s\n", argv[optind], strerror(errno));
+ exit(EXIT_FAILURE);
+ }
+
+ /* Get node list */
+ int hsr_family;
+
+ hsr_family = genl_ctrl_resolve(nlsk, "HSR");
+ if (hsr_family < 0) {
+ printf("genl_ctrl_resolve() failed: %d\n", hsr_family);
+ goto out;
+ }
+
+ rc = query_get_node_list(nlsk, hsr_family, hsr_ifindex);
+// printf("query_get_node_list() returned %d\n", rc);
+
+ rc = nl_recvmsgs_default(nlsk);
+// printf("nl_recvmsgs_default() returned %d\n", rc);
+
+ ni = node_head;
+ while (ni) {
+ /*
+ * Send a query about the status of another node on the HSR network:
+ */
+ /* The node to enquire about: */
+ //const unsigned char node[ETH_ALEN] = {0x00, 0x24, 0x74, 0x00, 0x17, 0xAD};
+
+ rc = query_get_node_status(nlsk, hsr_family, hsr_ifindex, ni->addr);
+// printf("query_node_status() returned %d\n", rc);
+
+ ni = ni->next;
+ }
+
+ while (1) {
+ rc = nl_recvmsgs_default(nlsk);
+// printf("nl_recvmsgs_default() returned %d\n", rc);
+ }
+
+
+ rc = EXIT_SUCCESS;
+out:
+ nl_close(nlsk);
+ nl_socket_free(nlsk);
+ return rc;
+}