[mirror_ubuntu-artful-kernel.git] / net / hsr / hsr_netlink.c

/* Copyright 2011-2014 Autronica Fire and Security AS
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
 * Software Foundation; either version 2 of the License, or (at your option)
 * any later version.
 *
 * Author(s):
 *	2011-2014 Arvid Brodin, arvid.brodin@alten.se
 *
 * 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 },
	[IFLA_HSR_MULTICAST_SPEC]	= { .type = NLA_U8 },
	[IFLA_HSR_SUPERVISION_ADDR]	= { .type = NLA_BINARY, .len = ETH_ALEN },
	[IFLA_HSR_SEQ_NR]		= { .type = NLA_U16 },
};


/* 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];
	unsigned char multicast_spec;

	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;

	if (!data[IFLA_HSR_MULTICAST_SPEC])
		multicast_spec = 0;
	else
		multicast_spec = nla_get_u8(data[IFLA_HSR_MULTICAST_SPEC]);

	return hsr_dev_finalize(dev, link, multicast_spec);
}

static int hsr_fill_info(struct sk_buff *skb, const struct net_device *dev)
{
	struct hsr_priv *hsr;
	struct hsr_port *port;
	int res;

	hsr = netdev_priv(dev);

	res = 0;

	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->sup_multicast_addr) ||
	    nla_put_u16(skb, IFLA_HSR_SEQ_NR, hsr->sequence_nr))
		goto nla_put_failure;

	return 0;

nla_put_failure:
	return -EMSGSIZE;
}

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,
	.fill_info	= hsr_fill_info,
};


/* 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_NODE_ADDR_B] = { .type = NLA_BINARY, .len = ETH_ALEN },
	[HSR_A_IFINDEX] = { .type = NLA_U32 },
	[HSR_A_IF1_AGE] = { .type = NLA_U32 },
	[HSR_A_IF2_AGE] = { .type = NLA_U32 },
	[HSR_A_IF1_SEQ] = { .type = NLA_U16 },
	[HSR_A_IF2_SEQ] = { .type = NLA_U16 },
};

static struct genl_family hsr_genl_family = {
	.id = GENL_ID_GENERATE,
	.hdrsize = 0,
	.name = "HSR",
	.version = 1,
	.maxattr = HSR_A_MAX,
};

static const struct genl_multicast_group hsr_mcgrps[] = {
	{ .name = "hsr-network", },
};


/* 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(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;

	skb = genlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC);
	if (!skb)
		goto fail;

	msg_head = genlmsg_put(skb, 0, 0, &hsr_genl_family, 0, HSR_C_RING_ERROR);
	if (!msg_head)
		goto nla_put_failure;

	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, port->dev->ifindex);
	if (res < 0)
		goto nla_put_failure;

	genlmsg_end(skb, msg_head);
	genlmsg_multicast(&hsr_genl_family, skb, 0, 0, GFP_ATOMIC);

	return;

nla_put_failure:
	kfree_skb(skb);

fail:
	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, 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);
	if (!skb)
		goto fail;

	msg_head = genlmsg_put(skb, 0, 0, &hsr_genl_family, 0, HSR_C_NODE_DOWN);
	if (!msg_head)
		goto nla_put_failure;


	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(&hsr_genl_family, skb, 0, 0, GFP_ATOMIC);

	return;

nla_put_failure:
	kfree_skb(skb);

fail:
	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();
}


/* 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;
	struct net_device *hsr_dev;

	/* For sending */
	struct sk_buff *skb_out;
	void *msg_head;
	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;
	int hsr_node_if2_age;
	u16 hsr_node_if2_seq;
	int addr_b_ifindex;
	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 = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
	if (!skb_out) {
		res = -ENOMEM;
		goto fail;
	}

	msg_head = genlmsg_put(skb_out, NETLINK_CB(skb_in).portid,
				info->snd_seq, &hsr_genl_family, 0,
				HSR_C_SET_NODE_STATUS);
	if (!msg_head) {
		res = -ENOMEM;
		goto nla_put_failure;
	}

	res = nla_put_u32(skb_out, HSR_A_IFINDEX, hsr_dev->ifindex);
	if (res < 0)
		goto nla_put_failure;

	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,
			&hsr_node_if1_age,
			&hsr_node_if1_seq,
			&hsr_node_if2_age,
			&hsr_node_if2_seq);
	if (res < 0)
		goto nla_put_failure;

	res = nla_put(skb_out, HSR_A_NODE_ADDR, ETH_ALEN,
					nla_data(info->attrs[HSR_A_NODE_ADDR]));
	if (res < 0)
		goto nla_put_failure;

	if (addr_b_ifindex > -1) {
		res = nla_put(skb_out, HSR_A_NODE_ADDR_B, ETH_ALEN,
								hsr_node_addr_b);
		if (res < 0)
			goto nla_put_failure;

		res = nla_put_u32(skb_out, HSR_A_ADDR_B_IFINDEX, addr_b_ifindex);
		if (res < 0)
			goto nla_put_failure;
	}

	res = nla_put_u32(skb_out, HSR_A_IF1_AGE, hsr_node_if1_age);
	if (res < 0)
		goto nla_put_failure;
	res = nla_put_u16(skb_out, HSR_A_IF1_SEQ, hsr_node_if1_seq);
	if (res < 0)
		goto nla_put_failure;
	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,
				  port->dev->ifindex);
	rcu_read_unlock();
	if (res < 0)
		goto nla_put_failure;

	res = nla_put_u32(skb_out, HSR_A_IF2_AGE, hsr_node_if2_age);
	if (res < 0)
		goto nla_put_failure;
	res = nla_put_u16(skb_out, HSR_A_IF2_SEQ, hsr_node_if2_seq);
	if (res < 0)
		goto nla_put_failure;
	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,
				  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);

	return 0;

invalid:
	netlink_ack(skb_in, nlmsg_hdr(skb_in), -EINVAL);
	return 0;

nla_put_failure:
	kfree_skb(skb_out);
	/* Fall through */

fail:
	return res;
}

/* Get a list of MacAddressA of all nodes known to this node (other than self).
 */
static int hsr_get_node_list(struct sk_buff *skb_in, struct genl_info *info)
{
	/* For receiving */
	struct nlattr *na;
	struct net_device *hsr_dev;

	/* For sending */
	struct sk_buff *skb_out;
	void *msg_head;
	struct hsr_priv *hsr;
	void *pos;
	unsigned char addr[ETH_ALEN];
	int res;

	if (!info)
		goto invalid;

	na = info->attrs[HSR_A_IFINDEX];
	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 = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
	if (!skb_out) {
		res = -ENOMEM;
		goto fail;
	}

	msg_head = genlmsg_put(skb_out, NETLINK_CB(skb_in).portid,
				info->snd_seq, &hsr_genl_family, 0,
				HSR_C_SET_NODE_LIST);
	if (!msg_head) {
		res = -ENOMEM;
		goto nla_put_failure;
	}

	res = nla_put_u32(skb_out, HSR_A_IFINDEX, hsr_dev->ifindex);
	if (res < 0)
		goto nla_put_failure;

	hsr = netdev_priv(hsr_dev);

	rcu_read_lock();
	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, pos, addr);
	}
	rcu_read_unlock();

	genlmsg_end(skb_out, msg_head);
	genlmsg_unicast(genl_info_net(info), skb_out, info->snd_portid);

	return 0;

invalid:
	netlink_ack(skb_in, nlmsg_hdr(skb_in), -EINVAL);
	return 0;

nla_put_failure:
	kfree_skb(skb_out);
	/* Fall through */

fail:
	return res;
}


static const struct genl_ops hsr_ops[] = {
	{
		.cmd = HSR_C_GET_NODE_STATUS,
		.flags = 0,
		.policy = hsr_genl_policy,
		.doit = hsr_get_node_status,
		.dumpit = NULL,
	},
	{
		.cmd = HSR_C_GET_NODE_LIST,
		.flags = 0,
		.policy = hsr_genl_policy,
		.doit = hsr_get_node_list,
		.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_with_ops_groups(&hsr_genl_family, hsr_ops,
						  hsr_mcgrps);
	if (rc)
		goto fail_genl_register_family;

	return 0;

fail_genl_register_family:
	rtnl_link_unregister(&hsr_link_ops);
fail_rtnl_link_register:

	return rc;
}

void __exit hsr_netlink_exit(void)
{
	genl_unregister_family(&hsr_genl_family);
	rtnl_link_unregister(&hsr_link_ops);
}

MODULE_ALIAS_RTNL_LINK("hsr");
Commit	Line	Data
70ebe4a4	1	/* Copyright 2011-2014 Autronica Fire and Security AS
f421436a AB	2	*
	3	* This program is free software; you can redistribute it and/or modify it
	4	* under the terms of the GNU General Public License as published by the Free
	5	* Software Foundation; either version 2 of the License, or (at your option)
	6	* any later version.
	7	*
	8	* Author(s):
70ebe4a4	9	* 2011-2014 Arvid Brodin, arvid.brodin@alten.se
f421436a AB	10	*
	11	* Routines for handling Netlink messages for HSR.
	12	*/
	13
	14	#include "hsr_netlink.h"
	15	#include <linux/kernel.h>
	16	#include <net/rtnetlink.h>
	17	#include <net/genetlink.h>
	18	#include "hsr_main.h"
	19	#include "hsr_device.h"
	20	#include "hsr_framereg.h"
	21
	22	static const struct nla_policy hsr_policy[IFLA_HSR_MAX + 1] = {
	23	[IFLA_HSR_SLAVE1] = { .type = NLA_U32 },
	24	[IFLA_HSR_SLAVE2] = { .type = NLA_U32 },
	25	[IFLA_HSR_MULTICAST_SPEC] = { .type = NLA_U8 },
98bf8362 AB	26	[IFLA_HSR_SUPERVISION_ADDR] = { .type = NLA_BINARY, .len = ETH_ALEN },
98bf8362 AB	27	[IFLA_HSR_SEQ_NR] = { .type = NLA_U16 },
f421436a AB	28	};
	29
	30
	31	/* Here, it seems a netdevice has already been allocated for us, and the
	32	* hsr_dev_setup routine has been executed. Nice!
	33	*/
	34	static int hsr_newlink(struct net src_net, struct net_device dev,
	35	struct nlattr tb[], struct nlattr data[])
	36	{
	37	struct net_device *link[2];
	38	unsigned char multicast_spec;
	39
	40	if (!data[IFLA_HSR_SLAVE1]) {
	41	netdev_info(dev, "IFLA_HSR_SLAVE1 missing!\n");
	42	return -EINVAL;
	43	}
	44	link[0] = __dev_get_by_index(src_net, nla_get_u32(data[IFLA_HSR_SLAVE1]));
	45	if (!data[IFLA_HSR_SLAVE2]) {
	46	netdev_info(dev, "IFLA_HSR_SLAVE2 missing!\n");
	47	return -EINVAL;
	48	}
	49	link[1] = __dev_get_by_index(src_net, nla_get_u32(data[IFLA_HSR_SLAVE2]));
	50
	51	if (!link[0] \|\| !link[1])
	52	return -ENODEV;
	53	if (link[0] == link[1])
	54	return -EINVAL;
	55
	56	if (!data[IFLA_HSR_MULTICAST_SPEC])
	57	multicast_spec = 0;
	58	else
	59	multicast_spec = nla_get_u8(data[IFLA_HSR_MULTICAST_SPEC]);
	60
	61	return hsr_dev_finalize(dev, link, multicast_spec);
	62	}
	63
98bf8362 AB	64	static int hsr_fill_info(struct sk_buff skb, const struct net_device dev)
98bf8362 AB	65	{
70ebe4a4	66	struct hsr_priv *hsr;
c5a75911	67	struct hsr_port *port;
51f3c605	68	int res;
98bf8362	69
70ebe4a4	70	hsr = netdev_priv(dev);
98bf8362	71
51f3c605	72	res = 0;
98bf8362	73
51f3c605	74	rcu_read_lock();
c5a75911 AB	75	port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_A);
	76	if (port)
	77	res = nla_put_u32(skb, IFLA_HSR_SLAVE1, port->dev->ifindex);
51f3c605 AB	78	rcu_read_unlock();
	79	if (res)
	80	goto nla_put_failure;
	81
	82	rcu_read_lock();
c5a75911 AB	83	port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_B);
	84	if (port)
	85	res = nla_put_u32(skb, IFLA_HSR_SLAVE2, port->dev->ifindex);
51f3c605 AB	86	rcu_read_unlock();
	87	if (res)
	88	goto nla_put_failure;
98bf8362 AB	89
98bf8362 AB	90	if (nla_put(skb, IFLA_HSR_SUPERVISION_ADDR, ETH_ALEN,
70ebe4a4 AB	91	hsr->sup_multicast_addr) \|\|
70ebe4a4 AB	92	nla_put_u16(skb, IFLA_HSR_SEQ_NR, hsr->sequence_nr))
98bf8362 AB	93	goto nla_put_failure;
	94
	95	return 0;
	96
	97	nla_put_failure:
	98	return -EMSGSIZE;
	99	}
	100
f421436a AB	101	static struct rtnl_link_ops hsr_link_ops __read_mostly = {
	102	.kind = "hsr",
	103	.maxtype = IFLA_HSR_MAX,
	104	.policy = hsr_policy,
	105	.priv_size = sizeof(struct hsr_priv),
	106	.setup = hsr_dev_setup,
	107	.newlink = hsr_newlink,
98bf8362	108	.fill_info = hsr_fill_info,
f421436a AB	109	};
	110
	111
	112
	113	/* attribute policy */
	114	/* NLA_BINARY missing in libnl; use NLA_UNSPEC in userspace instead. */
	115	static const struct nla_policy hsr_genl_policy[HSR_A_MAX + 1] = {
	116	[HSR_A_NODE_ADDR] = { .type = NLA_BINARY, .len = ETH_ALEN },
	117	[HSR_A_NODE_ADDR_B] = { .type = NLA_BINARY, .len = ETH_ALEN },
	118	[HSR_A_IFINDEX] = { .type = NLA_U32 },
	119	[HSR_A_IF1_AGE] = { .type = NLA_U32 },
	120	[HSR_A_IF2_AGE] = { .type = NLA_U32 },
	121	[HSR_A_IF1_SEQ] = { .type = NLA_U16 },
	122	[HSR_A_IF2_SEQ] = { .type = NLA_U16 },
	123	};
	124
	125	static struct genl_family hsr_genl_family = {
	126	.id = GENL_ID_GENERATE,
	127	.hdrsize = 0,
	128	.name = "HSR",
	129	.version = 1,
	130	.maxattr = HSR_A_MAX,
	131	};
	132
2a94fe48 JB	133	static const struct genl_multicast_group hsr_mcgrps[] = {
2a94fe48 JB	134	{ .name = "hsr-network", },
f421436a AB	135	};
	136
	137
	138
	139	/* This is called if for some node with MAC address addr, we only get frames
	140	* over one of the slave interfaces. This would indicate an open network ring
	141	* (i.e. a link has failed somewhere).
	142	*/
70ebe4a4	143	void hsr_nl_ringerror(struct hsr_priv *hsr, unsigned char addr[ETH_ALEN],
c5a75911	144	struct hsr_port *port)
f421436a AB	145	{
	146	struct sk_buff *skb;
	147	void *msg_head;
c5a75911	148	struct hsr_port *master;
f421436a	149	int res;
f421436a AB	150
	151	skb = genlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC);
	152	if (!skb)
	153	goto fail;
	154
	155	msg_head = genlmsg_put(skb, 0, 0, &hsr_genl_family, 0, HSR_C_RING_ERROR);
	156	if (!msg_head)
	157	goto nla_put_failure;
	158
	159	res = nla_put(skb, HSR_A_NODE_ADDR, ETH_ALEN, addr);
	160	if (res < 0)
	161	goto nla_put_failure;
	162
c5a75911	163	res = nla_put_u32(skb, HSR_A_IFINDEX, port->dev->ifindex);
f421436a AB	164	if (res < 0)
	165	goto nla_put_failure;
	166
	167	genlmsg_end(skb, msg_head);
2a94fe48	168	genlmsg_multicast(&hsr_genl_family, skb, 0, 0, GFP_ATOMIC);
f421436a AB	169
	170	return;
	171
	172	nla_put_failure:
	173	kfree_skb(skb);
	174
	175	fail:
c5a75911 AB	176	rcu_read_lock();
	177	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
	178	netdev_warn(master->dev, "Could not send HSR ring error message\n");
	179	rcu_read_unlock();
f421436a AB	180	}
	181
	182	/* This is called when we haven't heard from the node with MAC address addr for
	183	* some time (just before the node is removed from the node table/list).
	184	*/
70ebe4a4	185	void hsr_nl_nodedown(struct hsr_priv *hsr, unsigned char addr[ETH_ALEN])
f421436a AB	186	{
	187	struct sk_buff *skb;
	188	void *msg_head;
c5a75911	189	struct hsr_port *master;
f421436a AB	190	int res;
	191
	192	skb = genlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC);
	193	if (!skb)
	194	goto fail;
	195
	196	msg_head = genlmsg_put(skb, 0, 0, &hsr_genl_family, 0, HSR_C_NODE_DOWN);
	197	if (!msg_head)
	198	goto nla_put_failure;
	199
	200
	201	res = nla_put(skb, HSR_A_NODE_ADDR, ETH_ALEN, addr);
	202	if (res < 0)
	203	goto nla_put_failure;
	204
	205	genlmsg_end(skb, msg_head);
2a94fe48	206	genlmsg_multicast(&hsr_genl_family, skb, 0, 0, GFP_ATOMIC);
f421436a AB	207
	208	return;
	209
	210	nla_put_failure:
	211	kfree_skb(skb);
	212
	213	fail:
c5a75911 AB	214	rcu_read_lock();
	215	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
	216	netdev_warn(master->dev, "Could not send HSR node down\n");
	217	rcu_read_unlock();
f421436a AB	218	}
	219
	220
	221	/* HSR_C_GET_NODE_STATUS lets userspace query the internal HSR node table
	222	* about the status of a specific node in the network, defined by its MAC
	223	* address.
	224	*
	225	* Input: hsr ifindex, node mac address
	226	* Output: hsr ifindex, node mac address (copied from request),
	227	* age of latest frame from node over slave 1, slave 2 [ms]
	228	*/
	229	static int hsr_get_node_status(struct sk_buff skb_in, struct genl_info info)
	230	{
	231	/* For receiving */
	232	struct nlattr *na;
c5a75911	233	struct net_device *hsr_dev;
f421436a AB	234
	235	/* For sending */
	236	struct sk_buff *skb_out;
	237	void *msg_head;
70ebe4a4	238	struct hsr_priv *hsr;
c5a75911	239	struct hsr_port *port;
f421436a AB	240	unsigned char hsr_node_addr_b[ETH_ALEN];
	241	int hsr_node_if1_age;
	242	u16 hsr_node_if1_seq;
	243	int hsr_node_if2_age;
	244	u16 hsr_node_if2_seq;
	245	int addr_b_ifindex;
	246	int res;
	247
	248	if (!info)
	249	goto invalid;
	250
	251	na = info->attrs[HSR_A_IFINDEX];
	252	if (!na)
	253	goto invalid;
	254	na = info->attrs[HSR_A_NODE_ADDR];
	255	if (!na)
	256	goto invalid;
	257
	258	hsr_dev = __dev_get_by_index(genl_info_net(info),
	259	nla_get_u32(info->attrs[HSR_A_IFINDEX]));
	260	if (!hsr_dev)
	261	goto invalid;
	262	if (!is_hsr_master(hsr_dev))
	263	goto invalid;
	264
	265
	266	/* Send reply */
	267
	268	skb_out = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
	269	if (!skb_out) {
	270	res = -ENOMEM;
	271	goto fail;
	272	}
	273
	274	msg_head = genlmsg_put(skb_out, NETLINK_CB(skb_in).portid,
	275	info->snd_seq, &hsr_genl_family, 0,
	276	HSR_C_SET_NODE_STATUS);
	277	if (!msg_head) {
	278	res = -ENOMEM;
	279	goto nla_put_failure;
	280	}
	281
	282	res = nla_put_u32(skb_out, HSR_A_IFINDEX, hsr_dev->ifindex);
	283	if (res < 0)
	284	goto nla_put_failure;
	285
70ebe4a4 AB	286	hsr = netdev_priv(hsr_dev);
70ebe4a4 AB	287	res = hsr_get_node_data(hsr,
f421436a AB	288	(unsigned char *) nla_data(info->attrs[HSR_A_NODE_ADDR]),
	289	hsr_node_addr_b,
	290	&addr_b_ifindex,
	291	&hsr_node_if1_age,
	292	&hsr_node_if1_seq,
	293	&hsr_node_if2_age,
	294	&hsr_node_if2_seq);
	295	if (res < 0)
84a035f6	296	goto nla_put_failure;
f421436a AB	297
	298	res = nla_put(skb_out, HSR_A_NODE_ADDR, ETH_ALEN,
	299	nla_data(info->attrs[HSR_A_NODE_ADDR]));
	300	if (res < 0)
	301	goto nla_put_failure;
	302
	303	if (addr_b_ifindex > -1) {
	304	res = nla_put(skb_out, HSR_A_NODE_ADDR_B, ETH_ALEN,
	305	hsr_node_addr_b);
	306	if (res < 0)
	307	goto nla_put_failure;
	308
	309	res = nla_put_u32(skb_out, HSR_A_ADDR_B_IFINDEX, addr_b_ifindex);
	310	if (res < 0)
	311	goto nla_put_failure;
	312	}
	313
	314	res = nla_put_u32(skb_out, HSR_A_IF1_AGE, hsr_node_if1_age);
	315	if (res < 0)
	316	goto nla_put_failure;
	317	res = nla_put_u16(skb_out, HSR_A_IF1_SEQ, hsr_node_if1_seq);
	318	if (res < 0)
	319	goto nla_put_failure;
51f3c605	320	rcu_read_lock();
c5a75911 AB	321	port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_A);
	322	if (port)
	323	res = nla_put_u32(skb_out, HSR_A_IF1_IFINDEX,
	324	port->dev->ifindex);
51f3c605	325	rcu_read_unlock();
f421436a AB	326	if (res < 0)
	327	goto nla_put_failure;
	328
	329	res = nla_put_u32(skb_out, HSR_A_IF2_AGE, hsr_node_if2_age);
	330	if (res < 0)
	331	goto nla_put_failure;
	332	res = nla_put_u16(skb_out, HSR_A_IF2_SEQ, hsr_node_if2_seq);
	333	if (res < 0)
	334	goto nla_put_failure;
51f3c605	335	rcu_read_lock();
c5a75911 AB	336	port = hsr_port_get_hsr(hsr, HSR_PT_SLAVE_B);
	337	if (port)
	338	res = nla_put_u32(skb_out, HSR_A_IF2_IFINDEX,
	339	port->dev->ifindex);
51f3c605 AB	340	rcu_read_unlock();
	341	if (res < 0)
	342	goto nla_put_failure;
f421436a AB	343
	344	genlmsg_end(skb_out, msg_head);
	345	genlmsg_unicast(genl_info_net(info), skb_out, info->snd_portid);
	346
	347	return 0;
	348
	349	invalid:
	350	netlink_ack(skb_in, nlmsg_hdr(skb_in), -EINVAL);
	351	return 0;
	352
	353	nla_put_failure:
	354	kfree_skb(skb_out);
	355	/* Fall through */
	356
	357	fail:
	358	return res;
	359	}
	360
f421436a AB	361	/* Get a list of MacAddressA of all nodes known to this node (other than self).
	362	*/
	363	static int hsr_get_node_list(struct sk_buff skb_in, struct genl_info info)
	364	{
	365	/* For receiving */
	366	struct nlattr *na;
	367	struct net_device *hsr_dev;
	368
	369	/* For sending */
	370	struct sk_buff *skb_out;
	371	void *msg_head;
70ebe4a4	372	struct hsr_priv *hsr;
f421436a AB	373	void *pos;
	374	unsigned char addr[ETH_ALEN];
	375	int res;
	376
	377	if (!info)
	378	goto invalid;
	379
	380	na = info->attrs[HSR_A_IFINDEX];
	381	if (!na)
	382	goto invalid;
	383
	384	hsr_dev = __dev_get_by_index(genl_info_net(info),
	385	nla_get_u32(info->attrs[HSR_A_IFINDEX]));
	386	if (!hsr_dev)
	387	goto invalid;
	388	if (!is_hsr_master(hsr_dev))
	389	goto invalid;
	390
	391
	392	/* Send reply */
	393
	394	skb_out = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
	395	if (!skb_out) {
	396	res = -ENOMEM;
	397	goto fail;
	398	}
	399
	400	msg_head = genlmsg_put(skb_out, NETLINK_CB(skb_in).portid,
	401	info->snd_seq, &hsr_genl_family, 0,
	402	HSR_C_SET_NODE_LIST);
	403	if (!msg_head) {
	404	res = -ENOMEM;
	405	goto nla_put_failure;
	406	}
	407
	408	res = nla_put_u32(skb_out, HSR_A_IFINDEX, hsr_dev->ifindex);
	409	if (res < 0)
	410	goto nla_put_failure;
	411
70ebe4a4	412	hsr = netdev_priv(hsr_dev);
f421436a AB	413
f421436a AB	414	rcu_read_lock();
70ebe4a4	415	pos = hsr_get_next_node(hsr, NULL, addr);
f421436a AB	416	while (pos) {
	417	res = nla_put(skb_out, HSR_A_NODE_ADDR, ETH_ALEN, addr);
	418	if (res < 0) {
	419	rcu_read_unlock();
	420	goto nla_put_failure;
	421	}
70ebe4a4	422	pos = hsr_get_next_node(hsr, pos, addr);
f421436a AB	423	}
	424	rcu_read_unlock();
	425
	426	genlmsg_end(skb_out, msg_head);
	427	genlmsg_unicast(genl_info_net(info), skb_out, info->snd_portid);
	428
	429	return 0;
	430
	431	invalid:
	432	netlink_ack(skb_in, nlmsg_hdr(skb_in), -EINVAL);
	433	return 0;
	434
	435	nla_put_failure:
	436	kfree_skb(skb_out);
	437	/* Fall through */
	438
	439	fail:
	440	return res;
	441	}
	442
	443
4534de83	444	static const struct genl_ops hsr_ops[] = {
9504b3ee JB	445	{
	446	.cmd = HSR_C_GET_NODE_STATUS,
	447	.flags = 0,
	448	.policy = hsr_genl_policy,
	449	.doit = hsr_get_node_status,
	450	.dumpit = NULL,
	451	},
	452	{
	453	.cmd = HSR_C_GET_NODE_LIST,
	454	.flags = 0,
	455	.policy = hsr_genl_policy,
	456	.doit = hsr_get_node_list,
	457	.dumpit = NULL,
	458	},
f421436a AB	459	};
	460
	461	int __init hsr_netlink_init(void)
	462	{
	463	int rc;
	464
	465	rc = rtnl_link_register(&hsr_link_ops);
	466	if (rc)
	467	goto fail_rtnl_link_register;
	468
2a94fe48 JB	469	rc = genl_register_family_with_ops_groups(&hsr_genl_family, hsr_ops,
2a94fe48 JB	470	hsr_mcgrps);
f421436a AB	471	if (rc)
	472	goto fail_genl_register_family;
	473
f421436a AB	474	return 0;
f421436a AB	475
f421436a AB	476	fail_genl_register_family:
	477	rtnl_link_unregister(&hsr_link_ops);
	478	fail_rtnl_link_register:
	479
	480	return rc;
	481	}
	482
	483	void __exit hsr_netlink_exit(void)
	484	{
f421436a	485	genl_unregister_family(&hsr_genl_family);
f421436a AB	486	rtnl_link_unregister(&hsr_link_ops);
	487	}
	488
	489	MODULE_ALIAS_RTNL_LINK("hsr");