162306a36Sopenharmony_ci// SPDX-License-Identifier: GPL-2.0-only
262306a36Sopenharmony_ci/*
362306a36Sopenharmony_ci *	IEEE 802.1D Generic Attribute Registration Protocol (GARP)
462306a36Sopenharmony_ci *
562306a36Sopenharmony_ci *	Copyright (c) 2008 Patrick McHardy <kaber@trash.net>
662306a36Sopenharmony_ci */
762306a36Sopenharmony_ci#include <linux/kernel.h>
862306a36Sopenharmony_ci#include <linux/timer.h>
962306a36Sopenharmony_ci#include <linux/skbuff.h>
1062306a36Sopenharmony_ci#include <linux/netdevice.h>
1162306a36Sopenharmony_ci#include <linux/etherdevice.h>
1262306a36Sopenharmony_ci#include <linux/rtnetlink.h>
1362306a36Sopenharmony_ci#include <linux/llc.h>
1462306a36Sopenharmony_ci#include <linux/slab.h>
1562306a36Sopenharmony_ci#include <linux/module.h>
1662306a36Sopenharmony_ci#include <net/llc.h>
1762306a36Sopenharmony_ci#include <net/llc_pdu.h>
1862306a36Sopenharmony_ci#include <net/garp.h>
1962306a36Sopenharmony_ci#include <asm/unaligned.h>
2062306a36Sopenharmony_ci
2162306a36Sopenharmony_cistatic unsigned int garp_join_time __read_mostly = 200;
2262306a36Sopenharmony_cimodule_param(garp_join_time, uint, 0644);
2362306a36Sopenharmony_ciMODULE_PARM_DESC(garp_join_time, "Join time in ms (default 200ms)");
2462306a36Sopenharmony_ciMODULE_LICENSE("GPL");
2562306a36Sopenharmony_ci
2662306a36Sopenharmony_cistatic const struct garp_state_trans {
2762306a36Sopenharmony_ci	u8	state;
2862306a36Sopenharmony_ci	u8	action;
2962306a36Sopenharmony_ci} garp_applicant_state_table[GARP_APPLICANT_MAX + 1][GARP_EVENT_MAX + 1] = {
3062306a36Sopenharmony_ci	[GARP_APPLICANT_VA] = {
3162306a36Sopenharmony_ci		[GARP_EVENT_TRANSMIT_PDU]	= { .state = GARP_APPLICANT_AA,
3262306a36Sopenharmony_ci						    .action = GARP_ACTION_S_JOIN_IN },
3362306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_IN]		= { .state = GARP_APPLICANT_AA },
3462306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_EMPTY]	= { .state = GARP_APPLICANT_VA },
3562306a36Sopenharmony_ci		[GARP_EVENT_R_EMPTY]		= { .state = GARP_APPLICANT_VA },
3662306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_IN]		= { .state = GARP_APPLICANT_VA },
3762306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_EMPTY]	= { .state = GARP_APPLICANT_VP },
3862306a36Sopenharmony_ci		[GARP_EVENT_REQ_JOIN]		= { .state = GARP_APPLICANT_INVALID },
3962306a36Sopenharmony_ci		[GARP_EVENT_REQ_LEAVE]		= { .state = GARP_APPLICANT_LA },
4062306a36Sopenharmony_ci	},
4162306a36Sopenharmony_ci	[GARP_APPLICANT_AA] = {
4262306a36Sopenharmony_ci		[GARP_EVENT_TRANSMIT_PDU]	= { .state = GARP_APPLICANT_QA,
4362306a36Sopenharmony_ci						    .action = GARP_ACTION_S_JOIN_IN },
4462306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_IN]		= { .state = GARP_APPLICANT_QA },
4562306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_EMPTY]	= { .state = GARP_APPLICANT_VA },
4662306a36Sopenharmony_ci		[GARP_EVENT_R_EMPTY]		= { .state = GARP_APPLICANT_VA },
4762306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_IN]		= { .state = GARP_APPLICANT_VA },
4862306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_EMPTY]	= { .state = GARP_APPLICANT_VP },
4962306a36Sopenharmony_ci		[GARP_EVENT_REQ_JOIN]		= { .state = GARP_APPLICANT_INVALID },
5062306a36Sopenharmony_ci		[GARP_EVENT_REQ_LEAVE]		= { .state = GARP_APPLICANT_LA },
5162306a36Sopenharmony_ci	},
5262306a36Sopenharmony_ci	[GARP_APPLICANT_QA] = {
5362306a36Sopenharmony_ci		[GARP_EVENT_TRANSMIT_PDU]	= { .state = GARP_APPLICANT_INVALID },
5462306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_IN]		= { .state = GARP_APPLICANT_QA },
5562306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_EMPTY]	= { .state = GARP_APPLICANT_VA },
5662306a36Sopenharmony_ci		[GARP_EVENT_R_EMPTY]		= { .state = GARP_APPLICANT_VA },
5762306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_IN]		= { .state = GARP_APPLICANT_VP },
5862306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_EMPTY]	= { .state = GARP_APPLICANT_VP },
5962306a36Sopenharmony_ci		[GARP_EVENT_REQ_JOIN]		= { .state = GARP_APPLICANT_INVALID },
6062306a36Sopenharmony_ci		[GARP_EVENT_REQ_LEAVE]		= { .state = GARP_APPLICANT_LA },
6162306a36Sopenharmony_ci	},
6262306a36Sopenharmony_ci	[GARP_APPLICANT_LA] = {
6362306a36Sopenharmony_ci		[GARP_EVENT_TRANSMIT_PDU]	= { .state = GARP_APPLICANT_VO,
6462306a36Sopenharmony_ci						    .action = GARP_ACTION_S_LEAVE_EMPTY },
6562306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_IN]		= { .state = GARP_APPLICANT_LA },
6662306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_EMPTY]	= { .state = GARP_APPLICANT_VO },
6762306a36Sopenharmony_ci		[GARP_EVENT_R_EMPTY]		= { .state = GARP_APPLICANT_LA },
6862306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_IN]		= { .state = GARP_APPLICANT_LA },
6962306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_EMPTY]	= { .state = GARP_APPLICANT_VO },
7062306a36Sopenharmony_ci		[GARP_EVENT_REQ_JOIN]		= { .state = GARP_APPLICANT_VA },
7162306a36Sopenharmony_ci		[GARP_EVENT_REQ_LEAVE]		= { .state = GARP_APPLICANT_INVALID },
7262306a36Sopenharmony_ci	},
7362306a36Sopenharmony_ci	[GARP_APPLICANT_VP] = {
7462306a36Sopenharmony_ci		[GARP_EVENT_TRANSMIT_PDU]	= { .state = GARP_APPLICANT_AA,
7562306a36Sopenharmony_ci						    .action = GARP_ACTION_S_JOIN_IN },
7662306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_IN]		= { .state = GARP_APPLICANT_AP },
7762306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_EMPTY]	= { .state = GARP_APPLICANT_VP },
7862306a36Sopenharmony_ci		[GARP_EVENT_R_EMPTY]		= { .state = GARP_APPLICANT_VP },
7962306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_IN]		= { .state = GARP_APPLICANT_VP },
8062306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_EMPTY]	= { .state = GARP_APPLICANT_VP },
8162306a36Sopenharmony_ci		[GARP_EVENT_REQ_JOIN]		= { .state = GARP_APPLICANT_INVALID },
8262306a36Sopenharmony_ci		[GARP_EVENT_REQ_LEAVE]		= { .state = GARP_APPLICANT_VO },
8362306a36Sopenharmony_ci	},
8462306a36Sopenharmony_ci	[GARP_APPLICANT_AP] = {
8562306a36Sopenharmony_ci		[GARP_EVENT_TRANSMIT_PDU]	= { .state = GARP_APPLICANT_QA,
8662306a36Sopenharmony_ci						    .action = GARP_ACTION_S_JOIN_IN },
8762306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_IN]		= { .state = GARP_APPLICANT_QP },
8862306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_EMPTY]	= { .state = GARP_APPLICANT_VP },
8962306a36Sopenharmony_ci		[GARP_EVENT_R_EMPTY]		= { .state = GARP_APPLICANT_VP },
9062306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_IN]		= { .state = GARP_APPLICANT_VP },
9162306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_EMPTY]	= { .state = GARP_APPLICANT_VP },
9262306a36Sopenharmony_ci		[GARP_EVENT_REQ_JOIN]		= { .state = GARP_APPLICANT_INVALID },
9362306a36Sopenharmony_ci		[GARP_EVENT_REQ_LEAVE]		= { .state = GARP_APPLICANT_AO },
9462306a36Sopenharmony_ci	},
9562306a36Sopenharmony_ci	[GARP_APPLICANT_QP] = {
9662306a36Sopenharmony_ci		[GARP_EVENT_TRANSMIT_PDU]	= { .state = GARP_APPLICANT_INVALID },
9762306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_IN]		= { .state = GARP_APPLICANT_QP },
9862306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_EMPTY]	= { .state = GARP_APPLICANT_VP },
9962306a36Sopenharmony_ci		[GARP_EVENT_R_EMPTY]		= { .state = GARP_APPLICANT_VP },
10062306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_IN]		= { .state = GARP_APPLICANT_VP },
10162306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_EMPTY]	= { .state = GARP_APPLICANT_VP },
10262306a36Sopenharmony_ci		[GARP_EVENT_REQ_JOIN]		= { .state = GARP_APPLICANT_INVALID },
10362306a36Sopenharmony_ci		[GARP_EVENT_REQ_LEAVE]		= { .state = GARP_APPLICANT_QO },
10462306a36Sopenharmony_ci	},
10562306a36Sopenharmony_ci	[GARP_APPLICANT_VO] = {
10662306a36Sopenharmony_ci		[GARP_EVENT_TRANSMIT_PDU]	= { .state = GARP_APPLICANT_INVALID },
10762306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_IN]		= { .state = GARP_APPLICANT_AO },
10862306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_EMPTY]	= { .state = GARP_APPLICANT_VO },
10962306a36Sopenharmony_ci		[GARP_EVENT_R_EMPTY]		= { .state = GARP_APPLICANT_VO },
11062306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_IN]		= { .state = GARP_APPLICANT_VO },
11162306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_EMPTY]	= { .state = GARP_APPLICANT_VO },
11262306a36Sopenharmony_ci		[GARP_EVENT_REQ_JOIN]		= { .state = GARP_APPLICANT_VP },
11362306a36Sopenharmony_ci		[GARP_EVENT_REQ_LEAVE]		= { .state = GARP_APPLICANT_INVALID },
11462306a36Sopenharmony_ci	},
11562306a36Sopenharmony_ci	[GARP_APPLICANT_AO] = {
11662306a36Sopenharmony_ci		[GARP_EVENT_TRANSMIT_PDU]	= { .state = GARP_APPLICANT_INVALID },
11762306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_IN]		= { .state = GARP_APPLICANT_QO },
11862306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_EMPTY]	= { .state = GARP_APPLICANT_VO },
11962306a36Sopenharmony_ci		[GARP_EVENT_R_EMPTY]		= { .state = GARP_APPLICANT_VO },
12062306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_IN]		= { .state = GARP_APPLICANT_VO },
12162306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_EMPTY]	= { .state = GARP_APPLICANT_VO },
12262306a36Sopenharmony_ci		[GARP_EVENT_REQ_JOIN]		= { .state = GARP_APPLICANT_AP },
12362306a36Sopenharmony_ci		[GARP_EVENT_REQ_LEAVE]		= { .state = GARP_APPLICANT_INVALID },
12462306a36Sopenharmony_ci	},
12562306a36Sopenharmony_ci	[GARP_APPLICANT_QO] = {
12662306a36Sopenharmony_ci		[GARP_EVENT_TRANSMIT_PDU]	= { .state = GARP_APPLICANT_INVALID },
12762306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_IN]		= { .state = GARP_APPLICANT_QO },
12862306a36Sopenharmony_ci		[GARP_EVENT_R_JOIN_EMPTY]	= { .state = GARP_APPLICANT_VO },
12962306a36Sopenharmony_ci		[GARP_EVENT_R_EMPTY]		= { .state = GARP_APPLICANT_VO },
13062306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_IN]		= { .state = GARP_APPLICANT_VO },
13162306a36Sopenharmony_ci		[GARP_EVENT_R_LEAVE_EMPTY]	= { .state = GARP_APPLICANT_VO },
13262306a36Sopenharmony_ci		[GARP_EVENT_REQ_JOIN]		= { .state = GARP_APPLICANT_QP },
13362306a36Sopenharmony_ci		[GARP_EVENT_REQ_LEAVE]		= { .state = GARP_APPLICANT_INVALID },
13462306a36Sopenharmony_ci	},
13562306a36Sopenharmony_ci};
13662306a36Sopenharmony_ci
13762306a36Sopenharmony_cistatic int garp_attr_cmp(const struct garp_attr *attr,
13862306a36Sopenharmony_ci			 const void *data, u8 len, u8 type)
13962306a36Sopenharmony_ci{
14062306a36Sopenharmony_ci	if (attr->type != type)
14162306a36Sopenharmony_ci		return attr->type - type;
14262306a36Sopenharmony_ci	if (attr->dlen != len)
14362306a36Sopenharmony_ci		return attr->dlen - len;
14462306a36Sopenharmony_ci	return memcmp(attr->data, data, len);
14562306a36Sopenharmony_ci}
14662306a36Sopenharmony_ci
14762306a36Sopenharmony_cistatic struct garp_attr *garp_attr_lookup(const struct garp_applicant *app,
14862306a36Sopenharmony_ci					  const void *data, u8 len, u8 type)
14962306a36Sopenharmony_ci{
15062306a36Sopenharmony_ci	struct rb_node *parent = app->gid.rb_node;
15162306a36Sopenharmony_ci	struct garp_attr *attr;
15262306a36Sopenharmony_ci	int d;
15362306a36Sopenharmony_ci
15462306a36Sopenharmony_ci	while (parent) {
15562306a36Sopenharmony_ci		attr = rb_entry(parent, struct garp_attr, node);
15662306a36Sopenharmony_ci		d = garp_attr_cmp(attr, data, len, type);
15762306a36Sopenharmony_ci		if (d > 0)
15862306a36Sopenharmony_ci			parent = parent->rb_left;
15962306a36Sopenharmony_ci		else if (d < 0)
16062306a36Sopenharmony_ci			parent = parent->rb_right;
16162306a36Sopenharmony_ci		else
16262306a36Sopenharmony_ci			return attr;
16362306a36Sopenharmony_ci	}
16462306a36Sopenharmony_ci	return NULL;
16562306a36Sopenharmony_ci}
16662306a36Sopenharmony_ci
16762306a36Sopenharmony_cistatic struct garp_attr *garp_attr_create(struct garp_applicant *app,
16862306a36Sopenharmony_ci					  const void *data, u8 len, u8 type)
16962306a36Sopenharmony_ci{
17062306a36Sopenharmony_ci	struct rb_node *parent = NULL, **p = &app->gid.rb_node;
17162306a36Sopenharmony_ci	struct garp_attr *attr;
17262306a36Sopenharmony_ci	int d;
17362306a36Sopenharmony_ci
17462306a36Sopenharmony_ci	while (*p) {
17562306a36Sopenharmony_ci		parent = *p;
17662306a36Sopenharmony_ci		attr = rb_entry(parent, struct garp_attr, node);
17762306a36Sopenharmony_ci		d = garp_attr_cmp(attr, data, len, type);
17862306a36Sopenharmony_ci		if (d > 0)
17962306a36Sopenharmony_ci			p = &parent->rb_left;
18062306a36Sopenharmony_ci		else if (d < 0)
18162306a36Sopenharmony_ci			p = &parent->rb_right;
18262306a36Sopenharmony_ci		else {
18362306a36Sopenharmony_ci			/* The attribute already exists; re-use it. */
18462306a36Sopenharmony_ci			return attr;
18562306a36Sopenharmony_ci		}
18662306a36Sopenharmony_ci	}
18762306a36Sopenharmony_ci	attr = kmalloc(sizeof(*attr) + len, GFP_ATOMIC);
18862306a36Sopenharmony_ci	if (!attr)
18962306a36Sopenharmony_ci		return attr;
19062306a36Sopenharmony_ci	attr->state = GARP_APPLICANT_VO;
19162306a36Sopenharmony_ci	attr->type  = type;
19262306a36Sopenharmony_ci	attr->dlen  = len;
19362306a36Sopenharmony_ci	memcpy(attr->data, data, len);
19462306a36Sopenharmony_ci
19562306a36Sopenharmony_ci	rb_link_node(&attr->node, parent, p);
19662306a36Sopenharmony_ci	rb_insert_color(&attr->node, &app->gid);
19762306a36Sopenharmony_ci	return attr;
19862306a36Sopenharmony_ci}
19962306a36Sopenharmony_ci
20062306a36Sopenharmony_cistatic void garp_attr_destroy(struct garp_applicant *app, struct garp_attr *attr)
20162306a36Sopenharmony_ci{
20262306a36Sopenharmony_ci	rb_erase(&attr->node, &app->gid);
20362306a36Sopenharmony_ci	kfree(attr);
20462306a36Sopenharmony_ci}
20562306a36Sopenharmony_ci
20662306a36Sopenharmony_cistatic void garp_attr_destroy_all(struct garp_applicant *app)
20762306a36Sopenharmony_ci{
20862306a36Sopenharmony_ci	struct rb_node *node, *next;
20962306a36Sopenharmony_ci	struct garp_attr *attr;
21062306a36Sopenharmony_ci
21162306a36Sopenharmony_ci	for (node = rb_first(&app->gid);
21262306a36Sopenharmony_ci	     next = node ? rb_next(node) : NULL, node != NULL;
21362306a36Sopenharmony_ci	     node = next) {
21462306a36Sopenharmony_ci		attr = rb_entry(node, struct garp_attr, node);
21562306a36Sopenharmony_ci		garp_attr_destroy(app, attr);
21662306a36Sopenharmony_ci	}
21762306a36Sopenharmony_ci}
21862306a36Sopenharmony_ci
21962306a36Sopenharmony_cistatic int garp_pdu_init(struct garp_applicant *app)
22062306a36Sopenharmony_ci{
22162306a36Sopenharmony_ci	struct sk_buff *skb;
22262306a36Sopenharmony_ci	struct garp_pdu_hdr *gp;
22362306a36Sopenharmony_ci
22462306a36Sopenharmony_ci#define LLC_RESERVE	sizeof(struct llc_pdu_un)
22562306a36Sopenharmony_ci	skb = alloc_skb(app->dev->mtu + LL_RESERVED_SPACE(app->dev),
22662306a36Sopenharmony_ci			GFP_ATOMIC);
22762306a36Sopenharmony_ci	if (!skb)
22862306a36Sopenharmony_ci		return -ENOMEM;
22962306a36Sopenharmony_ci
23062306a36Sopenharmony_ci	skb->dev = app->dev;
23162306a36Sopenharmony_ci	skb->protocol = htons(ETH_P_802_2);
23262306a36Sopenharmony_ci	skb_reserve(skb, LL_RESERVED_SPACE(app->dev) + LLC_RESERVE);
23362306a36Sopenharmony_ci
23462306a36Sopenharmony_ci	gp = __skb_put(skb, sizeof(*gp));
23562306a36Sopenharmony_ci	put_unaligned(htons(GARP_PROTOCOL_ID), &gp->protocol);
23662306a36Sopenharmony_ci
23762306a36Sopenharmony_ci	app->pdu = skb;
23862306a36Sopenharmony_ci	return 0;
23962306a36Sopenharmony_ci}
24062306a36Sopenharmony_ci
24162306a36Sopenharmony_cistatic int garp_pdu_append_end_mark(struct garp_applicant *app)
24262306a36Sopenharmony_ci{
24362306a36Sopenharmony_ci	if (skb_tailroom(app->pdu) < sizeof(u8))
24462306a36Sopenharmony_ci		return -1;
24562306a36Sopenharmony_ci	__skb_put_u8(app->pdu, GARP_END_MARK);
24662306a36Sopenharmony_ci	return 0;
24762306a36Sopenharmony_ci}
24862306a36Sopenharmony_ci
24962306a36Sopenharmony_cistatic void garp_pdu_queue(struct garp_applicant *app)
25062306a36Sopenharmony_ci{
25162306a36Sopenharmony_ci	if (!app->pdu)
25262306a36Sopenharmony_ci		return;
25362306a36Sopenharmony_ci
25462306a36Sopenharmony_ci	garp_pdu_append_end_mark(app);
25562306a36Sopenharmony_ci	garp_pdu_append_end_mark(app);
25662306a36Sopenharmony_ci
25762306a36Sopenharmony_ci	llc_pdu_header_init(app->pdu, LLC_PDU_TYPE_U, LLC_SAP_BSPAN,
25862306a36Sopenharmony_ci			    LLC_SAP_BSPAN, LLC_PDU_CMD);
25962306a36Sopenharmony_ci	llc_pdu_init_as_ui_cmd(app->pdu);
26062306a36Sopenharmony_ci	llc_mac_hdr_init(app->pdu, app->dev->dev_addr,
26162306a36Sopenharmony_ci			 app->app->proto.group_address);
26262306a36Sopenharmony_ci
26362306a36Sopenharmony_ci	skb_queue_tail(&app->queue, app->pdu);
26462306a36Sopenharmony_ci	app->pdu = NULL;
26562306a36Sopenharmony_ci}
26662306a36Sopenharmony_ci
26762306a36Sopenharmony_cistatic void garp_queue_xmit(struct garp_applicant *app)
26862306a36Sopenharmony_ci{
26962306a36Sopenharmony_ci	struct sk_buff *skb;
27062306a36Sopenharmony_ci
27162306a36Sopenharmony_ci	while ((skb = skb_dequeue(&app->queue)))
27262306a36Sopenharmony_ci		dev_queue_xmit(skb);
27362306a36Sopenharmony_ci}
27462306a36Sopenharmony_ci
27562306a36Sopenharmony_cistatic int garp_pdu_append_msg(struct garp_applicant *app, u8 attrtype)
27662306a36Sopenharmony_ci{
27762306a36Sopenharmony_ci	struct garp_msg_hdr *gm;
27862306a36Sopenharmony_ci
27962306a36Sopenharmony_ci	if (skb_tailroom(app->pdu) < sizeof(*gm))
28062306a36Sopenharmony_ci		return -1;
28162306a36Sopenharmony_ci	gm = __skb_put(app->pdu, sizeof(*gm));
28262306a36Sopenharmony_ci	gm->attrtype = attrtype;
28362306a36Sopenharmony_ci	garp_cb(app->pdu)->cur_type = attrtype;
28462306a36Sopenharmony_ci	return 0;
28562306a36Sopenharmony_ci}
28662306a36Sopenharmony_ci
28762306a36Sopenharmony_cistatic int garp_pdu_append_attr(struct garp_applicant *app,
28862306a36Sopenharmony_ci				const struct garp_attr *attr,
28962306a36Sopenharmony_ci				enum garp_attr_event event)
29062306a36Sopenharmony_ci{
29162306a36Sopenharmony_ci	struct garp_attr_hdr *ga;
29262306a36Sopenharmony_ci	unsigned int len;
29362306a36Sopenharmony_ci	int err;
29462306a36Sopenharmony_ciagain:
29562306a36Sopenharmony_ci	if (!app->pdu) {
29662306a36Sopenharmony_ci		err = garp_pdu_init(app);
29762306a36Sopenharmony_ci		if (err < 0)
29862306a36Sopenharmony_ci			return err;
29962306a36Sopenharmony_ci	}
30062306a36Sopenharmony_ci
30162306a36Sopenharmony_ci	if (garp_cb(app->pdu)->cur_type != attr->type) {
30262306a36Sopenharmony_ci		if (garp_cb(app->pdu)->cur_type &&
30362306a36Sopenharmony_ci		    garp_pdu_append_end_mark(app) < 0)
30462306a36Sopenharmony_ci			goto queue;
30562306a36Sopenharmony_ci		if (garp_pdu_append_msg(app, attr->type) < 0)
30662306a36Sopenharmony_ci			goto queue;
30762306a36Sopenharmony_ci	}
30862306a36Sopenharmony_ci
30962306a36Sopenharmony_ci	len = sizeof(*ga) + attr->dlen;
31062306a36Sopenharmony_ci	if (skb_tailroom(app->pdu) < len)
31162306a36Sopenharmony_ci		goto queue;
31262306a36Sopenharmony_ci	ga = __skb_put(app->pdu, len);
31362306a36Sopenharmony_ci	ga->len   = len;
31462306a36Sopenharmony_ci	ga->event = event;
31562306a36Sopenharmony_ci	memcpy(ga->data, attr->data, attr->dlen);
31662306a36Sopenharmony_ci	return 0;
31762306a36Sopenharmony_ci
31862306a36Sopenharmony_ciqueue:
31962306a36Sopenharmony_ci	garp_pdu_queue(app);
32062306a36Sopenharmony_ci	goto again;
32162306a36Sopenharmony_ci}
32262306a36Sopenharmony_ci
32362306a36Sopenharmony_cistatic void garp_attr_event(struct garp_applicant *app,
32462306a36Sopenharmony_ci			    struct garp_attr *attr, enum garp_event event)
32562306a36Sopenharmony_ci{
32662306a36Sopenharmony_ci	enum garp_applicant_state state;
32762306a36Sopenharmony_ci
32862306a36Sopenharmony_ci	state = garp_applicant_state_table[attr->state][event].state;
32962306a36Sopenharmony_ci	if (state == GARP_APPLICANT_INVALID)
33062306a36Sopenharmony_ci		return;
33162306a36Sopenharmony_ci
33262306a36Sopenharmony_ci	switch (garp_applicant_state_table[attr->state][event].action) {
33362306a36Sopenharmony_ci	case GARP_ACTION_NONE:
33462306a36Sopenharmony_ci		break;
33562306a36Sopenharmony_ci	case GARP_ACTION_S_JOIN_IN:
33662306a36Sopenharmony_ci		/* When appending the attribute fails, don't update state in
33762306a36Sopenharmony_ci		 * order to retry on next TRANSMIT_PDU event. */
33862306a36Sopenharmony_ci		if (garp_pdu_append_attr(app, attr, GARP_JOIN_IN) < 0)
33962306a36Sopenharmony_ci			return;
34062306a36Sopenharmony_ci		break;
34162306a36Sopenharmony_ci	case GARP_ACTION_S_LEAVE_EMPTY:
34262306a36Sopenharmony_ci		garp_pdu_append_attr(app, attr, GARP_LEAVE_EMPTY);
34362306a36Sopenharmony_ci		/* As a pure applicant, sending a leave message implies that
34462306a36Sopenharmony_ci		 * the attribute was unregistered and can be destroyed. */
34562306a36Sopenharmony_ci		garp_attr_destroy(app, attr);
34662306a36Sopenharmony_ci		return;
34762306a36Sopenharmony_ci	default:
34862306a36Sopenharmony_ci		WARN_ON(1);
34962306a36Sopenharmony_ci	}
35062306a36Sopenharmony_ci
35162306a36Sopenharmony_ci	attr->state = state;
35262306a36Sopenharmony_ci}
35362306a36Sopenharmony_ci
35462306a36Sopenharmony_ciint garp_request_join(const struct net_device *dev,
35562306a36Sopenharmony_ci		      const struct garp_application *appl,
35662306a36Sopenharmony_ci		      const void *data, u8 len, u8 type)
35762306a36Sopenharmony_ci{
35862306a36Sopenharmony_ci	struct garp_port *port = rtnl_dereference(dev->garp_port);
35962306a36Sopenharmony_ci	struct garp_applicant *app = rtnl_dereference(port->applicants[appl->type]);
36062306a36Sopenharmony_ci	struct garp_attr *attr;
36162306a36Sopenharmony_ci
36262306a36Sopenharmony_ci	spin_lock_bh(&app->lock);
36362306a36Sopenharmony_ci	attr = garp_attr_create(app, data, len, type);
36462306a36Sopenharmony_ci	if (!attr) {
36562306a36Sopenharmony_ci		spin_unlock_bh(&app->lock);
36662306a36Sopenharmony_ci		return -ENOMEM;
36762306a36Sopenharmony_ci	}
36862306a36Sopenharmony_ci	garp_attr_event(app, attr, GARP_EVENT_REQ_JOIN);
36962306a36Sopenharmony_ci	spin_unlock_bh(&app->lock);
37062306a36Sopenharmony_ci	return 0;
37162306a36Sopenharmony_ci}
37262306a36Sopenharmony_ciEXPORT_SYMBOL_GPL(garp_request_join);
37362306a36Sopenharmony_ci
37462306a36Sopenharmony_civoid garp_request_leave(const struct net_device *dev,
37562306a36Sopenharmony_ci			const struct garp_application *appl,
37662306a36Sopenharmony_ci			const void *data, u8 len, u8 type)
37762306a36Sopenharmony_ci{
37862306a36Sopenharmony_ci	struct garp_port *port = rtnl_dereference(dev->garp_port);
37962306a36Sopenharmony_ci	struct garp_applicant *app = rtnl_dereference(port->applicants[appl->type]);
38062306a36Sopenharmony_ci	struct garp_attr *attr;
38162306a36Sopenharmony_ci
38262306a36Sopenharmony_ci	spin_lock_bh(&app->lock);
38362306a36Sopenharmony_ci	attr = garp_attr_lookup(app, data, len, type);
38462306a36Sopenharmony_ci	if (!attr) {
38562306a36Sopenharmony_ci		spin_unlock_bh(&app->lock);
38662306a36Sopenharmony_ci		return;
38762306a36Sopenharmony_ci	}
38862306a36Sopenharmony_ci	garp_attr_event(app, attr, GARP_EVENT_REQ_LEAVE);
38962306a36Sopenharmony_ci	spin_unlock_bh(&app->lock);
39062306a36Sopenharmony_ci}
39162306a36Sopenharmony_ciEXPORT_SYMBOL_GPL(garp_request_leave);
39262306a36Sopenharmony_ci
39362306a36Sopenharmony_cistatic void garp_gid_event(struct garp_applicant *app, enum garp_event event)
39462306a36Sopenharmony_ci{
39562306a36Sopenharmony_ci	struct rb_node *node, *next;
39662306a36Sopenharmony_ci	struct garp_attr *attr;
39762306a36Sopenharmony_ci
39862306a36Sopenharmony_ci	for (node = rb_first(&app->gid);
39962306a36Sopenharmony_ci	     next = node ? rb_next(node) : NULL, node != NULL;
40062306a36Sopenharmony_ci	     node = next) {
40162306a36Sopenharmony_ci		attr = rb_entry(node, struct garp_attr, node);
40262306a36Sopenharmony_ci		garp_attr_event(app, attr, event);
40362306a36Sopenharmony_ci	}
40462306a36Sopenharmony_ci}
40562306a36Sopenharmony_ci
40662306a36Sopenharmony_cistatic void garp_join_timer_arm(struct garp_applicant *app)
40762306a36Sopenharmony_ci{
40862306a36Sopenharmony_ci	unsigned long delay;
40962306a36Sopenharmony_ci
41062306a36Sopenharmony_ci	delay = get_random_u32_below(msecs_to_jiffies(garp_join_time));
41162306a36Sopenharmony_ci	mod_timer(&app->join_timer, jiffies + delay);
41262306a36Sopenharmony_ci}
41362306a36Sopenharmony_ci
41462306a36Sopenharmony_cistatic void garp_join_timer(struct timer_list *t)
41562306a36Sopenharmony_ci{
41662306a36Sopenharmony_ci	struct garp_applicant *app = from_timer(app, t, join_timer);
41762306a36Sopenharmony_ci
41862306a36Sopenharmony_ci	spin_lock(&app->lock);
41962306a36Sopenharmony_ci	garp_gid_event(app, GARP_EVENT_TRANSMIT_PDU);
42062306a36Sopenharmony_ci	garp_pdu_queue(app);
42162306a36Sopenharmony_ci	spin_unlock(&app->lock);
42262306a36Sopenharmony_ci
42362306a36Sopenharmony_ci	garp_queue_xmit(app);
42462306a36Sopenharmony_ci	garp_join_timer_arm(app);
42562306a36Sopenharmony_ci}
42662306a36Sopenharmony_ci
42762306a36Sopenharmony_cistatic int garp_pdu_parse_end_mark(struct sk_buff *skb)
42862306a36Sopenharmony_ci{
42962306a36Sopenharmony_ci	if (!pskb_may_pull(skb, sizeof(u8)))
43062306a36Sopenharmony_ci		return -1;
43162306a36Sopenharmony_ci	if (*skb->data == GARP_END_MARK) {
43262306a36Sopenharmony_ci		skb_pull(skb, sizeof(u8));
43362306a36Sopenharmony_ci		return -1;
43462306a36Sopenharmony_ci	}
43562306a36Sopenharmony_ci	return 0;
43662306a36Sopenharmony_ci}
43762306a36Sopenharmony_ci
43862306a36Sopenharmony_cistatic int garp_pdu_parse_attr(struct garp_applicant *app, struct sk_buff *skb,
43962306a36Sopenharmony_ci			       u8 attrtype)
44062306a36Sopenharmony_ci{
44162306a36Sopenharmony_ci	const struct garp_attr_hdr *ga;
44262306a36Sopenharmony_ci	struct garp_attr *attr;
44362306a36Sopenharmony_ci	enum garp_event event;
44462306a36Sopenharmony_ci	unsigned int dlen;
44562306a36Sopenharmony_ci
44662306a36Sopenharmony_ci	if (!pskb_may_pull(skb, sizeof(*ga)))
44762306a36Sopenharmony_ci		return -1;
44862306a36Sopenharmony_ci	ga = (struct garp_attr_hdr *)skb->data;
44962306a36Sopenharmony_ci	if (ga->len < sizeof(*ga))
45062306a36Sopenharmony_ci		return -1;
45162306a36Sopenharmony_ci
45262306a36Sopenharmony_ci	if (!pskb_may_pull(skb, ga->len))
45362306a36Sopenharmony_ci		return -1;
45462306a36Sopenharmony_ci	skb_pull(skb, ga->len);
45562306a36Sopenharmony_ci	dlen = sizeof(*ga) - ga->len;
45662306a36Sopenharmony_ci
45762306a36Sopenharmony_ci	if (attrtype > app->app->maxattr)
45862306a36Sopenharmony_ci		return 0;
45962306a36Sopenharmony_ci
46062306a36Sopenharmony_ci	switch (ga->event) {
46162306a36Sopenharmony_ci	case GARP_LEAVE_ALL:
46262306a36Sopenharmony_ci		if (dlen != 0)
46362306a36Sopenharmony_ci			return -1;
46462306a36Sopenharmony_ci		garp_gid_event(app, GARP_EVENT_R_LEAVE_EMPTY);
46562306a36Sopenharmony_ci		return 0;
46662306a36Sopenharmony_ci	case GARP_JOIN_EMPTY:
46762306a36Sopenharmony_ci		event = GARP_EVENT_R_JOIN_EMPTY;
46862306a36Sopenharmony_ci		break;
46962306a36Sopenharmony_ci	case GARP_JOIN_IN:
47062306a36Sopenharmony_ci		event = GARP_EVENT_R_JOIN_IN;
47162306a36Sopenharmony_ci		break;
47262306a36Sopenharmony_ci	case GARP_LEAVE_EMPTY:
47362306a36Sopenharmony_ci		event = GARP_EVENT_R_LEAVE_EMPTY;
47462306a36Sopenharmony_ci		break;
47562306a36Sopenharmony_ci	case GARP_EMPTY:
47662306a36Sopenharmony_ci		event = GARP_EVENT_R_EMPTY;
47762306a36Sopenharmony_ci		break;
47862306a36Sopenharmony_ci	default:
47962306a36Sopenharmony_ci		return 0;
48062306a36Sopenharmony_ci	}
48162306a36Sopenharmony_ci
48262306a36Sopenharmony_ci	if (dlen == 0)
48362306a36Sopenharmony_ci		return -1;
48462306a36Sopenharmony_ci	attr = garp_attr_lookup(app, ga->data, dlen, attrtype);
48562306a36Sopenharmony_ci	if (attr == NULL)
48662306a36Sopenharmony_ci		return 0;
48762306a36Sopenharmony_ci	garp_attr_event(app, attr, event);
48862306a36Sopenharmony_ci	return 0;
48962306a36Sopenharmony_ci}
49062306a36Sopenharmony_ci
49162306a36Sopenharmony_cistatic int garp_pdu_parse_msg(struct garp_applicant *app, struct sk_buff *skb)
49262306a36Sopenharmony_ci{
49362306a36Sopenharmony_ci	const struct garp_msg_hdr *gm;
49462306a36Sopenharmony_ci
49562306a36Sopenharmony_ci	if (!pskb_may_pull(skb, sizeof(*gm)))
49662306a36Sopenharmony_ci		return -1;
49762306a36Sopenharmony_ci	gm = (struct garp_msg_hdr *)skb->data;
49862306a36Sopenharmony_ci	if (gm->attrtype == 0)
49962306a36Sopenharmony_ci		return -1;
50062306a36Sopenharmony_ci	skb_pull(skb, sizeof(*gm));
50162306a36Sopenharmony_ci
50262306a36Sopenharmony_ci	while (skb->len > 0) {
50362306a36Sopenharmony_ci		if (garp_pdu_parse_attr(app, skb, gm->attrtype) < 0)
50462306a36Sopenharmony_ci			return -1;
50562306a36Sopenharmony_ci		if (garp_pdu_parse_end_mark(skb) < 0)
50662306a36Sopenharmony_ci			break;
50762306a36Sopenharmony_ci	}
50862306a36Sopenharmony_ci	return 0;
50962306a36Sopenharmony_ci}
51062306a36Sopenharmony_ci
51162306a36Sopenharmony_cistatic void garp_pdu_rcv(const struct stp_proto *proto, struct sk_buff *skb,
51262306a36Sopenharmony_ci			 struct net_device *dev)
51362306a36Sopenharmony_ci{
51462306a36Sopenharmony_ci	struct garp_application *appl = proto->data;
51562306a36Sopenharmony_ci	struct garp_port *port;
51662306a36Sopenharmony_ci	struct garp_applicant *app;
51762306a36Sopenharmony_ci	const struct garp_pdu_hdr *gp;
51862306a36Sopenharmony_ci
51962306a36Sopenharmony_ci	port = rcu_dereference(dev->garp_port);
52062306a36Sopenharmony_ci	if (!port)
52162306a36Sopenharmony_ci		goto err;
52262306a36Sopenharmony_ci	app = rcu_dereference(port->applicants[appl->type]);
52362306a36Sopenharmony_ci	if (!app)
52462306a36Sopenharmony_ci		goto err;
52562306a36Sopenharmony_ci
52662306a36Sopenharmony_ci	if (!pskb_may_pull(skb, sizeof(*gp)))
52762306a36Sopenharmony_ci		goto err;
52862306a36Sopenharmony_ci	gp = (struct garp_pdu_hdr *)skb->data;
52962306a36Sopenharmony_ci	if (get_unaligned(&gp->protocol) != htons(GARP_PROTOCOL_ID))
53062306a36Sopenharmony_ci		goto err;
53162306a36Sopenharmony_ci	skb_pull(skb, sizeof(*gp));
53262306a36Sopenharmony_ci
53362306a36Sopenharmony_ci	spin_lock(&app->lock);
53462306a36Sopenharmony_ci	while (skb->len > 0) {
53562306a36Sopenharmony_ci		if (garp_pdu_parse_msg(app, skb) < 0)
53662306a36Sopenharmony_ci			break;
53762306a36Sopenharmony_ci		if (garp_pdu_parse_end_mark(skb) < 0)
53862306a36Sopenharmony_ci			break;
53962306a36Sopenharmony_ci	}
54062306a36Sopenharmony_ci	spin_unlock(&app->lock);
54162306a36Sopenharmony_cierr:
54262306a36Sopenharmony_ci	kfree_skb(skb);
54362306a36Sopenharmony_ci}
54462306a36Sopenharmony_ci
54562306a36Sopenharmony_cistatic int garp_init_port(struct net_device *dev)
54662306a36Sopenharmony_ci{
54762306a36Sopenharmony_ci	struct garp_port *port;
54862306a36Sopenharmony_ci
54962306a36Sopenharmony_ci	port = kzalloc(sizeof(*port), GFP_KERNEL);
55062306a36Sopenharmony_ci	if (!port)
55162306a36Sopenharmony_ci		return -ENOMEM;
55262306a36Sopenharmony_ci	rcu_assign_pointer(dev->garp_port, port);
55362306a36Sopenharmony_ci	return 0;
55462306a36Sopenharmony_ci}
55562306a36Sopenharmony_ci
55662306a36Sopenharmony_cistatic void garp_release_port(struct net_device *dev)
55762306a36Sopenharmony_ci{
55862306a36Sopenharmony_ci	struct garp_port *port = rtnl_dereference(dev->garp_port);
55962306a36Sopenharmony_ci	unsigned int i;
56062306a36Sopenharmony_ci
56162306a36Sopenharmony_ci	for (i = 0; i <= GARP_APPLICATION_MAX; i++) {
56262306a36Sopenharmony_ci		if (rtnl_dereference(port->applicants[i]))
56362306a36Sopenharmony_ci			return;
56462306a36Sopenharmony_ci	}
56562306a36Sopenharmony_ci	RCU_INIT_POINTER(dev->garp_port, NULL);
56662306a36Sopenharmony_ci	kfree_rcu(port, rcu);
56762306a36Sopenharmony_ci}
56862306a36Sopenharmony_ci
56962306a36Sopenharmony_ciint garp_init_applicant(struct net_device *dev, struct garp_application *appl)
57062306a36Sopenharmony_ci{
57162306a36Sopenharmony_ci	struct garp_applicant *app;
57262306a36Sopenharmony_ci	int err;
57362306a36Sopenharmony_ci
57462306a36Sopenharmony_ci	ASSERT_RTNL();
57562306a36Sopenharmony_ci
57662306a36Sopenharmony_ci	if (!rtnl_dereference(dev->garp_port)) {
57762306a36Sopenharmony_ci		err = garp_init_port(dev);
57862306a36Sopenharmony_ci		if (err < 0)
57962306a36Sopenharmony_ci			goto err1;
58062306a36Sopenharmony_ci	}
58162306a36Sopenharmony_ci
58262306a36Sopenharmony_ci	err = -ENOMEM;
58362306a36Sopenharmony_ci	app = kzalloc(sizeof(*app), GFP_KERNEL);
58462306a36Sopenharmony_ci	if (!app)
58562306a36Sopenharmony_ci		goto err2;
58662306a36Sopenharmony_ci
58762306a36Sopenharmony_ci	err = dev_mc_add(dev, appl->proto.group_address);
58862306a36Sopenharmony_ci	if (err < 0)
58962306a36Sopenharmony_ci		goto err3;
59062306a36Sopenharmony_ci
59162306a36Sopenharmony_ci	app->dev = dev;
59262306a36Sopenharmony_ci	app->app = appl;
59362306a36Sopenharmony_ci	app->gid = RB_ROOT;
59462306a36Sopenharmony_ci	spin_lock_init(&app->lock);
59562306a36Sopenharmony_ci	skb_queue_head_init(&app->queue);
59662306a36Sopenharmony_ci	rcu_assign_pointer(dev->garp_port->applicants[appl->type], app);
59762306a36Sopenharmony_ci	timer_setup(&app->join_timer, garp_join_timer, 0);
59862306a36Sopenharmony_ci	garp_join_timer_arm(app);
59962306a36Sopenharmony_ci	return 0;
60062306a36Sopenharmony_ci
60162306a36Sopenharmony_cierr3:
60262306a36Sopenharmony_ci	kfree(app);
60362306a36Sopenharmony_cierr2:
60462306a36Sopenharmony_ci	garp_release_port(dev);
60562306a36Sopenharmony_cierr1:
60662306a36Sopenharmony_ci	return err;
60762306a36Sopenharmony_ci}
60862306a36Sopenharmony_ciEXPORT_SYMBOL_GPL(garp_init_applicant);
60962306a36Sopenharmony_ci
61062306a36Sopenharmony_civoid garp_uninit_applicant(struct net_device *dev, struct garp_application *appl)
61162306a36Sopenharmony_ci{
61262306a36Sopenharmony_ci	struct garp_port *port = rtnl_dereference(dev->garp_port);
61362306a36Sopenharmony_ci	struct garp_applicant *app = rtnl_dereference(port->applicants[appl->type]);
61462306a36Sopenharmony_ci
61562306a36Sopenharmony_ci	ASSERT_RTNL();
61662306a36Sopenharmony_ci
61762306a36Sopenharmony_ci	RCU_INIT_POINTER(port->applicants[appl->type], NULL);
61862306a36Sopenharmony_ci
61962306a36Sopenharmony_ci	/* Delete timer and generate a final TRANSMIT_PDU event to flush out
62062306a36Sopenharmony_ci	 * all pending messages before the applicant is gone. */
62162306a36Sopenharmony_ci	timer_shutdown_sync(&app->join_timer);
62262306a36Sopenharmony_ci
62362306a36Sopenharmony_ci	spin_lock_bh(&app->lock);
62462306a36Sopenharmony_ci	garp_gid_event(app, GARP_EVENT_TRANSMIT_PDU);
62562306a36Sopenharmony_ci	garp_attr_destroy_all(app);
62662306a36Sopenharmony_ci	garp_pdu_queue(app);
62762306a36Sopenharmony_ci	spin_unlock_bh(&app->lock);
62862306a36Sopenharmony_ci
62962306a36Sopenharmony_ci	garp_queue_xmit(app);
63062306a36Sopenharmony_ci
63162306a36Sopenharmony_ci	dev_mc_del(dev, appl->proto.group_address);
63262306a36Sopenharmony_ci	kfree_rcu(app, rcu);
63362306a36Sopenharmony_ci	garp_release_port(dev);
63462306a36Sopenharmony_ci}
63562306a36Sopenharmony_ciEXPORT_SYMBOL_GPL(garp_uninit_applicant);
63662306a36Sopenharmony_ci
63762306a36Sopenharmony_ciint garp_register_application(struct garp_application *appl)
63862306a36Sopenharmony_ci{
63962306a36Sopenharmony_ci	appl->proto.rcv = garp_pdu_rcv;
64062306a36Sopenharmony_ci	appl->proto.data = appl;
64162306a36Sopenharmony_ci	return stp_proto_register(&appl->proto);
64262306a36Sopenharmony_ci}
64362306a36Sopenharmony_ciEXPORT_SYMBOL_GPL(garp_register_application);
64462306a36Sopenharmony_ci
64562306a36Sopenharmony_civoid garp_unregister_application(struct garp_application *appl)
64662306a36Sopenharmony_ci{
64762306a36Sopenharmony_ci	stp_proto_unregister(&appl->proto);
64862306a36Sopenharmony_ci}
64962306a36Sopenharmony_ciEXPORT_SYMBOL_GPL(garp_unregister_application);
650