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