ieee8023ad_lacp.c revision 168793
1/*	$NetBSD: ieee8023ad_lacp.c,v 1.3 2005/12/11 12:24:54 christos Exp $	*/
2
3/*-
4 * Copyright (c)2005 YAMAMOTO Takashi,
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 * 1. Redistributions of source code must retain the above copyright
11 *    notice, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright
13 *    notice, this list of conditions and the following disclaimer in the
14 *    documentation and/or other materials provided with the distribution.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
26 * SUCH DAMAGE.
27 */
28
29#include <sys/cdefs.h>
30__FBSDID("$FreeBSD: head/sys/net/ieee8023ad_lacp.c 168793 2007-04-17 00:35:11Z thompsa $");
31
32#include <sys/param.h>
33#include <sys/callout.h>
34#include <sys/mbuf.h>
35#include <sys/systm.h>
36#include <sys/malloc.h>
37#include <sys/kernel.h> /* hz */
38#include <sys/socket.h> /* for net/if.h */
39#include <sys/sockio.h>
40#include <machine/stdarg.h>
41
42#include <net/if.h>
43#include <net/if_dl.h>
44#include <net/ethernet.h>
45#include <net/if_media.h>
46#include <net/if_types.h>
47
48#include <net/if_lagg.h>
49#include <net/ieee8023ad_lacp.h>
50
51/*
52 * actor system priority and port priority.
53 * XXX should be configurable.
54 */
55
56#define	LACP_SYSTEM_PRIO	0x8000
57#define	LACP_PORT_PRIO		0x8000
58
59const uint8_t ethermulticastaddr_slowprotocols[ETHER_ADDR_LEN] =
60    { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x02 };
61
62static const struct tlv_template lacp_info_tlv_template[] = {
63	{ LACP_TYPE_ACTORINFO,
64	    sizeof(struct tlvhdr) + sizeof(struct lacp_peerinfo) },
65	{ LACP_TYPE_PARTNERINFO,
66	    sizeof(struct tlvhdr) + sizeof(struct lacp_peerinfo) },
67	{ LACP_TYPE_COLLECTORINFO,
68	    sizeof(struct tlvhdr) + sizeof(struct lacp_collectorinfo) },
69	{ 0, 0 },
70};
71
72typedef void (*lacp_timer_func_t)(struct lacp_port *);
73
74static const struct tlv_template marker_info_tlv_template[] = {
75	{ MARKER_TYPE_INFO, 16 },
76	{ 0, 0 },
77};
78
79static const struct tlv_template marker_response_tlv_template[] = {
80	{ MARKER_TYPE_RESPONSE, 16 },
81	{ 0, 0 },
82};
83
84static void	lacp_fill_actorinfo(struct lacp_port *, struct lacp_peerinfo *);
85
86static uint64_t	lacp_aggregator_bandwidth(struct lacp_aggregator *);
87static void	lacp_suppress_distributing(struct lacp_softc *,
88		    struct lacp_aggregator *);
89static void	lacp_transit_expire(void *);
90static void	lacp_select_active_aggregator(struct lacp_softc *);
91static uint16_t	lacp_compose_key(struct lacp_port *);
92static int	tlv_check(const void *, size_t, const struct tlvhdr *,
93		    const struct tlv_template *, boolean_t);
94static void	lacp_tick(void *);
95
96static void	lacp_fill_aggregator_id(struct lacp_aggregator *,
97		    const struct lacp_port *);
98static void	lacp_fill_aggregator_id_peer(struct lacp_peerinfo *,
99		    const struct lacp_peerinfo *);
100static int	lacp_aggregator_is_compatible(const struct lacp_aggregator *,
101		    const struct lacp_port *);
102static int	lacp_peerinfo_is_compatible(const struct lacp_peerinfo *,
103		    const struct lacp_peerinfo *);
104
105static struct lacp_aggregator *lacp_aggregator_get(struct lacp_softc *,
106		    struct lacp_port *);
107static void	lacp_aggregator_addref(struct lacp_softc *,
108		    struct lacp_aggregator *);
109static void	lacp_aggregator_delref(struct lacp_softc *,
110		    struct lacp_aggregator *);
111
112/* receive machine */
113
114static void	lacp_sm_rx(struct lacp_port *, const struct lacpdu *);
115static void	lacp_sm_rx_timer(struct lacp_port *);
116static void	lacp_sm_rx_set_expired(struct lacp_port *);
117static void	lacp_sm_rx_update_ntt(struct lacp_port *,
118		    const struct lacpdu *);
119static void	lacp_sm_rx_record_pdu(struct lacp_port *,
120		    const struct lacpdu *);
121static void	lacp_sm_rx_update_selected(struct lacp_port *,
122		    const struct lacpdu *);
123static void	lacp_sm_rx_record_default(struct lacp_port *);
124static void	lacp_sm_rx_update_default_selected(struct lacp_port *);
125static void	lacp_sm_rx_update_selected_from_peerinfo(struct lacp_port *,
126		    const struct lacp_peerinfo *);
127
128/* mux machine */
129
130static void	lacp_sm_mux(struct lacp_port *);
131static void	lacp_set_mux(struct lacp_port *, enum lacp_mux_state);
132static void	lacp_sm_mux_timer(struct lacp_port *);
133
134/* periodic transmit machine */
135
136static void	lacp_sm_ptx_update_timeout(struct lacp_port *, uint8_t);
137static void	lacp_sm_ptx_tx_schedule(struct lacp_port *);
138static void	lacp_sm_ptx_timer(struct lacp_port *);
139
140/* transmit machine */
141
142static void	lacp_sm_tx(struct lacp_port *);
143static void	lacp_sm_assert_ntt(struct lacp_port *);
144
145static void	lacp_run_timers(struct lacp_port *);
146static int	lacp_compare_peerinfo(const struct lacp_peerinfo *,
147		    const struct lacp_peerinfo *);
148static int	lacp_compare_systemid(const struct lacp_systemid *,
149		    const struct lacp_systemid *);
150static void	lacp_port_enable(struct lacp_port *);
151static void	lacp_port_disable(struct lacp_port *);
152static void	lacp_select(struct lacp_port *);
153static void	lacp_unselect(struct lacp_port *);
154static void	lacp_disable_collecting(struct lacp_port *);
155static void	lacp_enable_collecting(struct lacp_port *);
156static void	lacp_disable_distributing(struct lacp_port *);
157static void	lacp_enable_distributing(struct lacp_port *);
158static int	lacp_xmit_lacpdu(struct lacp_port *);
159
160#if defined(LACP_DEBUG)
161static void	lacp_dump_lacpdu(const struct lacpdu *);
162static const char *lacp_format_partner(const struct lacp_peerinfo *, char *,
163		    size_t);
164static const char *lacp_format_lagid(const struct lacp_peerinfo *,
165		    const struct lacp_peerinfo *, char *, size_t);
166static const char *lacp_format_lagid_aggregator(const struct lacp_aggregator *,
167		    char *, size_t);
168static const char *lacp_format_state(uint8_t, char *, size_t);
169static const char *lacp_format_mac(const uint8_t *, char *, size_t);
170static const char *lacp_format_systemid(const struct lacp_systemid *, char *,
171		    size_t);
172static const char *lacp_format_portid(const struct lacp_portid *, char *,
173		    size_t);
174static void	lacp_dprintf(const struct lacp_port *, const char *, ...)
175		    __attribute__((__format__(__printf__, 2, 3)));
176#define	LACP_DPRINTF(a)	lacp_dprintf a
177#else
178#define LACP_DPRINTF(a) /* nothing */
179#endif
180
181/*
182 * partner administration variables.
183 * XXX should be configurable.
184 */
185
186static const struct lacp_peerinfo lacp_partner_admin = {
187	.lip_systemid = { .lsi_prio = 0xffff },
188	.lip_portid = { .lpi_prio = 0xffff },
189#if 1
190	/* optimistic */
191	.lip_state = LACP_STATE_SYNC | LACP_STATE_AGGREGATION |
192	    LACP_STATE_COLLECTING | LACP_STATE_DISTRIBUTING,
193#else
194	/* pessimistic */
195	.lip_state = 0,
196#endif
197};
198
199static const lacp_timer_func_t lacp_timer_funcs[LACP_NTIMER] = {
200	[LACP_TIMER_CURRENT_WHILE] = lacp_sm_rx_timer,
201	[LACP_TIMER_PERIODIC] = lacp_sm_ptx_timer,
202	[LACP_TIMER_WAIT_WHILE] = lacp_sm_mux_timer,
203};
204
205/*
206 * lacp_input: process lacpdu
207 */
208int
209lacp_input(struct lagg_port *lgp, struct mbuf *m)
210{
211	struct lacp_port *lp = LACP_PORT(lgp);
212	struct lacpdu *du;
213	int error = 0;
214
215	LAGG_LOCK_ASSERT(lgp->lp_lagg);
216
217	if (__predict_false(lp->lp_flags & LACP_PORT_DETACHING)) {
218		goto bad;
219	}
220
221	if (m->m_pkthdr.len != sizeof(*du)) {
222		goto bad;
223	}
224
225	if ((m->m_flags & M_MCAST) == 0) {
226		goto bad;
227	}
228
229	if (m->m_len < sizeof(*du)) {
230		m = m_pullup(m, sizeof(*du));
231		if (m == NULL) {
232			return (ENOMEM);
233		}
234	}
235
236	du = mtod(m, struct lacpdu *);
237
238	if (memcmp(&du->ldu_eh.ether_dhost,
239	    &ethermulticastaddr_slowprotocols, ETHER_ADDR_LEN)) {
240		goto bad;
241	}
242
243	/* XXX
244	KASSERT(du->ldu_sph.sph_subtype == SLOWPROTOCOLS_SUBTYPE_LACP,
245	    ("a very bad kassert!"));
246	*/
247
248	/*
249	 * ignore the version for compatibility with
250	 * the future protocol revisions.
251	 */
252
253#if 0
254	if (du->ldu_sph.sph_version != 1) {
255		goto bad;
256	}
257#endif
258
259	/*
260	 * ignore tlv types for compatibility with
261	 * the future protocol revisions.
262	 */
263
264	if (tlv_check(du, sizeof(*du), &du->ldu_tlv_actor,
265	    lacp_info_tlv_template, FALSE)) {
266		goto bad;
267	}
268
269#if defined(LACP_DEBUG)
270	LACP_DPRINTF((lp, "lacpdu receive\n"));
271	lacp_dump_lacpdu(du);
272#endif /* defined(LACP_DEBUG) */
273	lacp_sm_rx(lp, du);
274
275	m_freem(m);
276
277	return (error);
278
279bad:
280	m_freem(m);
281	return (EINVAL);
282}
283
284static void
285lacp_fill_actorinfo(struct lacp_port *lp, struct lacp_peerinfo *info)
286{
287	struct lagg_port *lgp = lp->lp_lagg;
288	struct lagg_softc *lgs = lgp->lp_lagg;
289
290	info->lip_systemid.lsi_prio = htons(LACP_SYSTEM_PRIO);
291	memcpy(&info->lip_systemid.lsi_mac,
292	    IF_LLADDR(lgs->sc_ifp), ETHER_ADDR_LEN);
293	info->lip_portid.lpi_prio = htons(LACP_PORT_PRIO);
294	info->lip_portid.lpi_portno = htons(lp->lp_ifp->if_index);
295	info->lip_state = lp->lp_state;
296}
297
298static int
299lacp_xmit_lacpdu(struct lacp_port *lp)
300{
301	struct lagg_port *lgp = lp->lp_lagg;
302	struct mbuf *m;
303	struct lacpdu *du;
304	int error;
305
306	LAGG_LOCK_ASSERT(lgp->lp_lagg);
307
308	m = m_gethdr(M_DONTWAIT, MT_DATA);
309	if (m == NULL) {
310		return (ENOMEM);
311	}
312	m->m_len = m->m_pkthdr.len = sizeof(*du);
313
314	du = mtod(m, struct lacpdu *);
315	memset(du, 0, sizeof(*du));
316
317	memcpy(&du->ldu_eh.ether_dhost, ethermulticastaddr_slowprotocols,
318	    ETHER_ADDR_LEN);
319	memcpy(&du->ldu_eh.ether_shost, lgp->lp_lladdr, ETHER_ADDR_LEN);
320	du->ldu_eh.ether_type = htons(ETHERTYPE_SLOW);
321
322	du->ldu_sph.sph_subtype = SLOWPROTOCOLS_SUBTYPE_LACP;
323	du->ldu_sph.sph_version = 1;
324
325	TLV_SET(&du->ldu_tlv_actor, LACP_TYPE_ACTORINFO, sizeof(du->ldu_actor));
326	du->ldu_actor = lp->lp_actor;
327
328	TLV_SET(&du->ldu_tlv_partner, LACP_TYPE_PARTNERINFO,
329	    sizeof(du->ldu_partner));
330	du->ldu_partner = lp->lp_partner;
331
332	TLV_SET(&du->ldu_tlv_collector, LACP_TYPE_COLLECTORINFO,
333	    sizeof(du->ldu_collector));
334	du->ldu_collector.lci_maxdelay = 0;
335
336#if defined(LACP_DEBUG)
337	LACP_DPRINTF((lp, "lacpdu transmit\n"));
338	lacp_dump_lacpdu(du);
339#endif /* defined(LACP_DEBUG) */
340
341	m->m_flags |= M_MCAST;
342
343	/*
344	 * XXX should use higher priority queue.
345	 * otherwise network congestion can break aggregation.
346	 */
347
348	error = lagg_enqueue(lp->lp_ifp, m);
349	return (error);
350}
351
352void
353lacp_linkstate(struct lagg_port *lgp)
354{
355	struct lacp_port *lp = LACP_PORT(lgp);
356	struct ifnet *ifp = lgp->lp_ifp;
357	struct ifmediareq ifmr;
358	int error = 0;
359	u_int media;
360	uint8_t old_state;
361	uint16_t old_key;
362
363	LAGG_LOCK_ASSERT(lgp->lp_lagg);
364
365	bzero((char *)&ifmr, sizeof(ifmr));
366	error = (*ifp->if_ioctl)(ifp, SIOCGIFMEDIA, (caddr_t)&ifmr);
367	if (error != 0)
368		return;
369
370	media = ifmr.ifm_active;
371	LACP_DPRINTF((lp, "media changed 0x%x -> 0x%x\n", lp->lp_media, media));
372	old_state = lp->lp_state;
373	old_key = lp->lp_key;
374
375	lp->lp_media = media;
376	if ((media & IFM_HDX) != 0 || ifp->if_link_state == LINK_STATE_DOWN) {
377		lacp_port_disable(lp);
378	} else {
379		lacp_port_enable(lp);
380	}
381	lp->lp_key = lacp_compose_key(lp);
382
383	if (old_state != lp->lp_state || old_key != lp->lp_key) {
384		LACP_DPRINTF((lp, "-> UNSELECTED\n"));
385		lp->lp_selected = LACP_UNSELECTED;
386	}
387}
388
389static void
390lacp_tick(void *arg)
391{
392	struct lacp_softc *lsc = arg;
393	struct lacp_port *lp;
394
395	LIST_FOREACH(lp, &lsc->lsc_ports, lp_next) {
396		if ((lp->lp_state & LACP_STATE_AGGREGATION) == 0)
397			continue;
398
399		lacp_run_timers(lp);
400
401		lacp_select(lp);
402		lacp_sm_mux(lp);
403		lacp_sm_tx(lp);
404		lacp_sm_ptx_tx_schedule(lp);
405	}
406	callout_reset(&lsc->lsc_callout, hz, lacp_tick, lsc);
407}
408
409int
410lacp_port_create(struct lagg_port *lgp)
411{
412	struct lagg_softc *lgs = lgp->lp_lagg;
413	struct lacp_softc *lsc = LACP_SOFTC(lgs);
414	struct lacp_port *lp;
415	struct ifnet *ifp = lgp->lp_ifp;
416	struct sockaddr_dl sdl;
417	struct ifmultiaddr *rifma = NULL;
418	int error;
419
420	boolean_t active = TRUE; /* XXX should be configurable */
421	boolean_t fast = FALSE; /* XXX should be configurable */
422
423	LAGG_LOCK_ASSERT(lgs);
424
425	bzero((char *)&sdl, sizeof(sdl));
426	sdl.sdl_len = sizeof(sdl);
427	sdl.sdl_family = AF_LINK;
428	sdl.sdl_index = ifp->if_index;
429	sdl.sdl_type = IFT_ETHER;
430	sdl.sdl_alen = ETHER_ADDR_LEN;
431
432	bcopy(&ethermulticastaddr_slowprotocols,
433	    LLADDR(&sdl), ETHER_ADDR_LEN);
434	error = if_addmulti(ifp, (struct sockaddr *)&sdl, &rifma);
435	if (error) {
436		printf("%s: ADDMULTI failed on %s\n", __func__, lgp->lp_ifname);
437		return (error);
438	}
439
440	lp = malloc(sizeof(struct lacp_port),
441	    M_DEVBUF, M_NOWAIT|M_ZERO);
442	if (lp == NULL)
443		return (ENOMEM);
444
445	lgp->lp_psc = (caddr_t)lp;
446	lp->lp_ifp = ifp;
447	lp->lp_lagg = lgp;
448	lp->lp_lsc = lsc;
449
450	LIST_INSERT_HEAD(&lsc->lsc_ports, lp, lp_next);
451
452	lacp_fill_actorinfo(lp, &lp->lp_actor);
453	lp->lp_state =
454	    (active ? LACP_STATE_ACTIVITY : 0) |
455	    (fast ? LACP_STATE_TIMEOUT : 0);
456	lp->lp_aggregator = NULL;
457	lacp_linkstate(lgp);
458	lacp_sm_rx_set_expired(lp);
459
460	return (0);
461}
462
463void
464lacp_port_destroy(struct lagg_port *lgp)
465{
466	struct lacp_port *lp = LACP_PORT(lgp);
467	struct ifnet *ifp = lgp->lp_ifp;
468	struct sockaddr_dl sdl;
469	int i, error;
470
471	LAGG_LOCK_ASSERT(lgp->lp_lagg);
472
473	for (i = 0; i < LACP_NTIMER; i++) {
474		LACP_TIMER_DISARM(lp, i);
475	}
476
477	lacp_disable_collecting(lp);
478	lacp_disable_distributing(lp);
479	lacp_unselect(lp);
480
481	bzero((char *)&sdl, sizeof(sdl));
482	sdl.sdl_len = sizeof(sdl);
483	sdl.sdl_family = AF_LINK;
484	sdl.sdl_index = ifp->if_index;
485	sdl.sdl_type = IFT_ETHER;
486	sdl.sdl_alen = ETHER_ADDR_LEN;
487
488	bcopy(&ethermulticastaddr_slowprotocols,
489	    LLADDR(&sdl), ETHER_ADDR_LEN);
490	error = if_delmulti(ifp, (struct sockaddr *)&sdl);
491	if (error)
492		printf("%s: DELMULTI failed on %s\n", __func__, lgp->lp_ifname);
493
494	LIST_REMOVE(lp, lp_next);
495	free(lp, M_DEVBUF);
496}
497
498int
499lacp_port_isactive(struct lagg_port *lgp)
500{
501	struct lacp_port *lp = LACP_PORT(lgp);
502	struct lacp_softc *lsc = lp->lp_lsc;
503	struct lacp_aggregator *la = lp->lp_aggregator;
504
505	/* This port is joined to the active aggregator */
506	if (la != NULL && la == lsc->lsc_active_aggregator)
507		return (1);
508
509	return (0);
510}
511
512static void
513lacp_disable_collecting(struct lacp_port *lp)
514{
515	struct lagg_port *lgp = lp->lp_lagg;
516
517	LACP_DPRINTF((lp, "collecting disabled\n"));
518
519	lp->lp_state &= ~LACP_STATE_COLLECTING;
520	lgp->lp_flags &= ~LAGG_PORT_COLLECTING;
521}
522
523static void
524lacp_enable_collecting(struct lacp_port *lp)
525{
526	struct lagg_port *lgp = lp->lp_lagg;
527
528	LACP_DPRINTF((lp, "collecting enabled\n"));
529
530	lp->lp_state |= LACP_STATE_COLLECTING;
531	lgp->lp_flags |= LAGG_PORT_COLLECTING;
532}
533
534static void
535lacp_disable_distributing(struct lacp_port *lp)
536{
537	struct lacp_aggregator *la = lp->lp_aggregator;
538	struct lacp_softc *lsc = lp->lp_lsc;
539	struct lagg_port *lgp = lp->lp_lagg;
540#if defined(LACP_DEBUG)
541	char buf[LACP_LAGIDSTR_MAX+1];
542#endif /* defined(LACP_DEBUG) */
543
544	LAGG_LOCK_ASSERT(lgp->lp_lagg);
545
546	if (la == NULL || (lp->lp_state & LACP_STATE_DISTRIBUTING) == 0) {
547		return;
548	}
549
550	KASSERT(!TAILQ_EMPTY(&la->la_ports), ("no aggregator ports"));
551	KASSERT(la->la_nports > 0, ("nports invalid (%d)", la->la_nports));
552	KASSERT(la->la_refcnt >= la->la_nports, ("aggregator refcnt invalid"));
553
554	LACP_DPRINTF((lp, "disable distributing on aggregator %s, "
555	    "nports %d -> %d\n",
556	    lacp_format_lagid_aggregator(la, buf, sizeof(buf)),
557	    la->la_nports, la->la_nports - 1));
558
559	TAILQ_REMOVE(&la->la_ports, lp, lp_dist_q);
560	la->la_nports--;
561
562	lacp_suppress_distributing(lsc, la);
563
564	lp->lp_state &= ~LACP_STATE_DISTRIBUTING;
565	lgp->lp_flags &= ~LAGG_PORT_DISTRIBUTING;
566
567	if (lsc->lsc_active_aggregator == la) {
568		lacp_select_active_aggregator(lsc);
569	}
570}
571
572static void
573lacp_enable_distributing(struct lacp_port *lp)
574{
575	struct lacp_aggregator *la = lp->lp_aggregator;
576	struct lacp_softc *lsc = lp->lp_lsc;
577	struct lagg_port *lgp = lp->lp_lagg;
578#if defined(LACP_DEBUG)
579	char buf[LACP_LAGIDSTR_MAX+1];
580#endif /* defined(LACP_DEBUG) */
581
582	LAGG_LOCK_ASSERT(lgp->lp_lagg);
583
584	if ((lp->lp_state & LACP_STATE_DISTRIBUTING) != 0) {
585		return;
586	}
587
588	LACP_DPRINTF((lp, "enable distributing on aggregator %s, "
589	    "nports %d -> %d\n",
590	    lacp_format_lagid_aggregator(la, buf, sizeof(buf)),
591	    la->la_nports, la->la_nports + 1));
592
593	KASSERT(la->la_refcnt > la->la_nports, ("aggregator refcnt invalid"));
594	TAILQ_INSERT_HEAD(&la->la_ports, lp, lp_dist_q);
595	la->la_nports++;
596
597	lacp_suppress_distributing(lsc, la);
598
599	lp->lp_state |= LACP_STATE_DISTRIBUTING;
600	lgp->lp_flags |= LAGG_PORT_DISTRIBUTING;
601
602	if (lsc->lsc_active_aggregator != la) {
603		lacp_select_active_aggregator(lsc);
604	}
605}
606
607static void
608lacp_transit_expire(void *vp)
609{
610	struct lacp_softc *lsc = vp;
611
612	LACP_DPRINTF((NULL, "%s\n", __func__));
613	lsc->lsc_suppress_distributing = FALSE;
614}
615
616int
617lacp_attach(struct lagg_softc *lgs)
618{
619	struct lacp_softc *lsc;
620
621	LAGG_LOCK_ASSERT(lgs);
622
623	lsc = malloc(sizeof(struct lacp_softc),
624	    M_DEVBUF, M_NOWAIT|M_ZERO);
625	if (lsc == NULL)
626		return (ENOMEM);
627
628	lgs->sc_psc = (caddr_t)lsc;
629	lsc->lsc_lagg = lgs;
630
631	lsc->lsc_hashkey = arc4random();
632	lsc->lsc_active_aggregator = NULL;
633	TAILQ_INIT(&lsc->lsc_aggregators);
634	LIST_INIT(&lsc->lsc_ports);
635
636	callout_init_mtx(&lsc->lsc_transit_callout, &lgs->sc_mtx, 0);
637	callout_init_mtx(&lsc->lsc_callout, &lgs->sc_mtx, 0);
638
639	/* if the lagg is already up then do the same */
640	if (lgs->sc_ifp->if_drv_flags & IFF_DRV_RUNNING)
641		lacp_init(lgs);
642
643	return (0);
644}
645
646int
647lacp_detach(struct lagg_softc *lgs)
648{
649	struct lacp_softc *lsc = LACP_SOFTC(lgs);
650
651	KASSERT(TAILQ_EMPTY(&lsc->lsc_aggregators),
652	    ("aggregators still active"));
653	KASSERT(lsc->lsc_active_aggregator == NULL,
654	    ("aggregator still attached"));
655
656	lgs->sc_psc = NULL;
657	callout_drain(&lsc->lsc_transit_callout);
658	callout_drain(&lsc->lsc_callout);
659
660	free(lsc, M_DEVBUF);
661	return (0);
662}
663
664void
665lacp_init(struct lagg_softc *lgs)
666{
667	struct lacp_softc *lsc = LACP_SOFTC(lgs);
668
669	callout_reset(&lsc->lsc_callout, hz, lacp_tick, lsc);
670}
671
672void
673lacp_stop(struct lagg_softc *lgs)
674{
675	struct lacp_softc *lsc = LACP_SOFTC(lgs);
676
677	callout_stop(&lsc->lsc_transit_callout);
678	callout_stop(&lsc->lsc_callout);
679}
680
681struct lagg_port *
682lacp_select_tx_port(struct lagg_softc *lgs, struct mbuf *m)
683{
684	struct lacp_softc *lsc = LACP_SOFTC(lgs);
685	struct lacp_aggregator *la;
686	struct lacp_port *lp;
687	uint32_t hash;
688	int nports;
689
690	LAGG_LOCK_ASSERT(lgs);
691
692	if (__predict_false(lsc->lsc_suppress_distributing)) {
693		LACP_DPRINTF((NULL, "%s: waiting transit\n", __func__));
694		return (NULL);
695	}
696
697	la = lsc->lsc_active_aggregator;
698	if (__predict_false(la == NULL)) {
699		LACP_DPRINTF((NULL, "%s: no active aggregator\n", __func__));
700		return (NULL);
701	}
702
703	nports = la->la_nports;
704	KASSERT(nports > 0, ("no ports available"));
705
706	hash = lagg_hashmbuf(m, lsc->lsc_hashkey);
707	hash %= nports;
708	lp = TAILQ_FIRST(&la->la_ports);
709	while (hash--) {
710		lp = TAILQ_NEXT(lp, lp_dist_q);
711	}
712
713	KASSERT((lp->lp_state & LACP_STATE_DISTRIBUTING) != 0,
714	    ("aggregated port is not distributing"));
715
716	return (lp->lp_lagg);
717}
718/*
719 * lacp_suppress_distributing: drop transmit packets for a while
720 * to preserve packet ordering.
721 */
722
723static void
724lacp_suppress_distributing(struct lacp_softc *lsc, struct lacp_aggregator *la)
725{
726	if (lsc->lsc_active_aggregator != la) {
727		return;
728	}
729
730	LACP_DPRINTF((NULL, "%s\n", __func__));
731	lsc->lsc_suppress_distributing = TRUE;
732	/* XXX should consider collector max delay */
733	callout_reset(&lsc->lsc_transit_callout,
734	    LACP_TRANSIT_DELAY * hz / 1000, lacp_transit_expire, lsc);
735}
736
737static int
738lacp_compare_peerinfo(const struct lacp_peerinfo *a,
739    const struct lacp_peerinfo *b)
740{
741	return (memcmp(a, b, offsetof(struct lacp_peerinfo, lip_state)));
742}
743
744static int
745lacp_compare_systemid(const struct lacp_systemid *a,
746    const struct lacp_systemid *b)
747{
748	return (memcmp(a, b, sizeof(*a)));
749}
750
751#if 0	/* unused */
752static int
753lacp_compare_portid(const struct lacp_portid *a,
754    const struct lacp_portid *b)
755{
756	return (memcmp(a, b, sizeof(*a)));
757}
758#endif
759
760static uint64_t
761lacp_aggregator_bandwidth(struct lacp_aggregator *la)
762{
763	struct lacp_port *lp;
764	uint64_t speed;
765
766	lp = TAILQ_FIRST(&la->la_ports);
767	if (lp == NULL) {
768		return (0);
769	}
770
771	speed = ifmedia_baudrate(lp->lp_media);
772	speed *= la->la_nports;
773	if (speed == 0) {
774		LACP_DPRINTF((lp, "speed 0? media=0x%x nports=%d\n",
775		    lp->lp_media, la->la_nports));
776	}
777
778	return (speed);
779}
780
781/*
782 * lacp_select_active_aggregator: select an aggregator to be used to transmit
783 * packets from lagg(4) interface.
784 */
785
786static void
787lacp_select_active_aggregator(struct lacp_softc *lsc)
788{
789	struct lacp_aggregator *la;
790	struct lacp_aggregator *best_la = NULL;
791	uint64_t best_speed = 0;
792#if defined(LACP_DEBUG)
793	char buf[LACP_LAGIDSTR_MAX+1];
794#endif /* defined(LACP_DEBUG) */
795
796	LACP_DPRINTF((NULL, "%s:\n", __func__));
797
798	TAILQ_FOREACH(la, &lsc->lsc_aggregators, la_q) {
799		uint64_t speed;
800
801		if (la->la_nports == 0) {
802			continue;
803		}
804
805		speed = lacp_aggregator_bandwidth(la);
806		LACP_DPRINTF((NULL, "%s, speed=%jd, nports=%d\n",
807		    lacp_format_lagid_aggregator(la, buf, sizeof(buf)),
808		    speed, la->la_nports));
809		if (speed > best_speed ||
810		    (speed == best_speed &&
811		    la == lsc->lsc_active_aggregator)) {
812			best_la = la;
813			best_speed = speed;
814		}
815	}
816
817	KASSERT(best_la == NULL || best_la->la_nports > 0,
818	    ("invalid aggregator refcnt"));
819	KASSERT(best_la == NULL || !TAILQ_EMPTY(&best_la->la_ports),
820	    ("invalid aggregator list"));
821
822#if defined(LACP_DEBUG)
823	if (lsc->lsc_active_aggregator != best_la) {
824		LACP_DPRINTF((NULL, "active aggregator changed\n"));
825		LACP_DPRINTF((NULL, "old %s\n",
826		    lacp_format_lagid_aggregator(lsc->lsc_active_aggregator,
827		    buf, sizeof(buf))));
828	} else {
829		LACP_DPRINTF((NULL, "active aggregator not changed\n"));
830	}
831	LACP_DPRINTF((NULL, "new %s\n",
832	    lacp_format_lagid_aggregator(best_la, buf, sizeof(buf))));
833#endif /* defined(LACP_DEBUG) */
834
835	if (lsc->lsc_active_aggregator != best_la) {
836		lsc->lsc_active_aggregator = best_la;
837		if (best_la) {
838			lacp_suppress_distributing(lsc, best_la);
839		}
840	}
841}
842
843static uint16_t
844lacp_compose_key(struct lacp_port *lp)
845{
846	struct lagg_port *lgp = lp->lp_lagg;
847	struct lagg_softc *lgs = lgp->lp_lagg;
848	u_int media = lp->lp_media;
849	uint16_t key;
850
851	KASSERT(IFM_TYPE(media) == IFM_ETHER, ("invalid interface type"));
852
853	if ((lp->lp_state & LACP_STATE_AGGREGATION) == 0) {
854
855		/*
856		 * non-aggregatable links should have unique keys.
857		 *
858		 * XXX this isn't really unique as if_index is 16 bit.
859		 */
860
861		/* bit 0..14:	(some bits of) if_index of this port */
862		key = lp->lp_ifp->if_index;
863		/* bit 15:	1 */
864		key |= 0x8000;
865	} else {
866		u_int subtype = IFM_SUBTYPE(media);
867
868		KASSERT((media & IFM_HDX) == 0, ("aggregating HDX interface"));
869
870		/* bit 0..4:	IFM_SUBTYPE */
871		key = subtype;
872		/* bit 5..14:	(some bits of) if_index of lagg device */
873		key |= 0x7fe0 & ((lgs->sc_ifp->if_index) << 5);
874		/* bit 15:	0 */
875	}
876	return (htons(key));
877}
878
879static void
880lacp_aggregator_addref(struct lacp_softc *lsc, struct lacp_aggregator *la)
881{
882#if defined(LACP_DEBUG)
883	char buf[LACP_LAGIDSTR_MAX+1];
884#endif
885
886	LACP_DPRINTF((NULL, "%s: lagid=%s, refcnt %d -> %d\n",
887	    __func__,
888	    lacp_format_lagid(&la->la_actor, &la->la_partner,
889	    buf, sizeof(buf)),
890	    la->la_refcnt, la->la_refcnt + 1));
891
892	KASSERT(la->la_refcnt > 0, ("refcount <= 0"));
893	la->la_refcnt++;
894	KASSERT(la->la_refcnt > la->la_nports, ("invalid refcount"));
895}
896
897static void
898lacp_aggregator_delref(struct lacp_softc *lsc, struct lacp_aggregator *la)
899{
900#if defined(LACP_DEBUG)
901	char buf[LACP_LAGIDSTR_MAX+1];
902#endif
903
904	LACP_DPRINTF((NULL, "%s: lagid=%s, refcnt %d -> %d\n",
905	    __func__,
906	    lacp_format_lagid(&la->la_actor, &la->la_partner,
907	    buf, sizeof(buf)),
908	    la->la_refcnt, la->la_refcnt - 1));
909
910	KASSERT(la->la_refcnt > la->la_nports, ("invalid refcnt"));
911	la->la_refcnt--;
912	if (la->la_refcnt > 0) {
913		return;
914	}
915
916	KASSERT(la->la_refcnt == 0, ("refcount not zero"));
917	KASSERT(lsc->lsc_active_aggregator != la, ("aggregator active"));
918
919	TAILQ_REMOVE(&lsc->lsc_aggregators, la, la_q);
920
921	free(la, M_DEVBUF);
922}
923
924/*
925 * lacp_aggregator_get: allocate an aggregator.
926 */
927
928static struct lacp_aggregator *
929lacp_aggregator_get(struct lacp_softc *lsc, struct lacp_port *lp)
930{
931	struct lacp_aggregator *la;
932
933	la = malloc(sizeof(*la), M_DEVBUF, M_NOWAIT);
934	if (la) {
935		la->la_refcnt = 1;
936		la->la_nports = 0;
937		TAILQ_INIT(&la->la_ports);
938		la->la_pending = 0;
939		TAILQ_INSERT_TAIL(&lsc->lsc_aggregators, la, la_q);
940	}
941
942	return (la);
943}
944
945/*
946 * lacp_fill_aggregator_id: setup a newly allocated aggregator from a port.
947 */
948
949static void
950lacp_fill_aggregator_id(struct lacp_aggregator *la, const struct lacp_port *lp)
951{
952	lacp_fill_aggregator_id_peer(&la->la_partner, &lp->lp_partner);
953	lacp_fill_aggregator_id_peer(&la->la_actor, &lp->lp_actor);
954
955	la->la_actor.lip_state = lp->lp_state & LACP_STATE_AGGREGATION;
956}
957
958static void
959lacp_fill_aggregator_id_peer(struct lacp_peerinfo *lpi_aggr,
960    const struct lacp_peerinfo *lpi_port)
961{
962	memset(lpi_aggr, 0, sizeof(*lpi_aggr));
963	lpi_aggr->lip_systemid = lpi_port->lip_systemid;
964	lpi_aggr->lip_key = lpi_port->lip_key;
965}
966
967/*
968 * lacp_aggregator_is_compatible: check if a port can join to an aggregator.
969 */
970
971static int
972lacp_aggregator_is_compatible(const struct lacp_aggregator *la,
973    const struct lacp_port *lp)
974{
975	if (!(lp->lp_state & LACP_STATE_AGGREGATION) ||
976	    !(lp->lp_partner.lip_state & LACP_STATE_AGGREGATION)) {
977		return (0);
978	}
979
980	if (!(la->la_actor.lip_state & LACP_STATE_AGGREGATION)) {
981		return (0);
982	}
983
984	if (!lacp_peerinfo_is_compatible(&la->la_partner, &lp->lp_partner)) {
985		return (0);
986	}
987
988	if (!lacp_peerinfo_is_compatible(&la->la_actor, &lp->lp_actor)) {
989		return (0);
990	}
991
992	return (1);
993}
994
995static int
996lacp_peerinfo_is_compatible(const struct lacp_peerinfo *a,
997    const struct lacp_peerinfo *b)
998{
999	if (memcmp(&a->lip_systemid, &b->lip_systemid,
1000	    sizeof(a->lip_systemid))) {
1001		return (0);
1002	}
1003
1004	if (memcmp(&a->lip_key, &b->lip_key, sizeof(a->lip_key))) {
1005		return (0);
1006	}
1007
1008	return (1);
1009}
1010
1011static void
1012lacp_port_enable(struct lacp_port *lp)
1013{
1014	lp->lp_state |= LACP_STATE_AGGREGATION;
1015}
1016
1017static void
1018lacp_port_disable(struct lacp_port *lp)
1019{
1020	lacp_set_mux(lp, LACP_MUX_DETACHED);
1021
1022	lp->lp_state &= ~LACP_STATE_AGGREGATION;
1023	lp->lp_selected = LACP_UNSELECTED;
1024	lacp_sm_rx_record_default(lp);
1025	lp->lp_partner.lip_state &= ~LACP_STATE_AGGREGATION;
1026	lp->lp_state &= ~LACP_STATE_EXPIRED;
1027}
1028
1029/*
1030 * lacp_select: select an aggregator.  create one if necessary.
1031 */
1032static void
1033lacp_select(struct lacp_port *lp)
1034{
1035	struct lacp_softc *lsc = lp->lp_lsc;
1036	struct lacp_aggregator *la;
1037#if defined(LACP_DEBUG)
1038	char buf[LACP_LAGIDSTR_MAX+1];
1039#endif
1040
1041	if (lp->lp_aggregator) {
1042		return;
1043	}
1044
1045	KASSERT(!LACP_TIMER_ISARMED(lp, LACP_TIMER_WAIT_WHILE),
1046	    ("timer_wait_while still active"));
1047
1048	LACP_DPRINTF((lp, "port lagid=%s\n",
1049	    lacp_format_lagid(&lp->lp_actor, &lp->lp_partner,
1050	    buf, sizeof(buf))));
1051
1052	TAILQ_FOREACH(la, &lsc->lsc_aggregators, la_q) {
1053		if (lacp_aggregator_is_compatible(la, lp)) {
1054			break;
1055		}
1056	}
1057
1058	if (la == NULL) {
1059		la = lacp_aggregator_get(lsc, lp);
1060		if (la == NULL) {
1061			LACP_DPRINTF((lp, "aggregator creation failed\n"));
1062
1063			/*
1064			 * will retry on the next tick.
1065			 */
1066
1067			return;
1068		}
1069		lacp_fill_aggregator_id(la, lp);
1070		LACP_DPRINTF((lp, "aggregator created\n"));
1071	} else {
1072		LACP_DPRINTF((lp, "compatible aggregator found\n"));
1073		lacp_aggregator_addref(lsc, la);
1074	}
1075
1076	LACP_DPRINTF((lp, "aggregator lagid=%s\n",
1077	    lacp_format_lagid(&la->la_actor, &la->la_partner,
1078	    buf, sizeof(buf))));
1079
1080	lp->lp_aggregator = la;
1081	lp->lp_selected = LACP_SELECTED;
1082}
1083
1084/*
1085 * lacp_unselect: finish unselect/detach process.
1086 */
1087
1088static void
1089lacp_unselect(struct lacp_port *lp)
1090{
1091	struct lacp_softc *lsc = lp->lp_lsc;
1092	struct lacp_aggregator *la = lp->lp_aggregator;
1093
1094	KASSERT(!LACP_TIMER_ISARMED(lp, LACP_TIMER_WAIT_WHILE),
1095	    ("timer_wait_while still active"));
1096
1097	if (la == NULL) {
1098		return;
1099	}
1100
1101	lp->lp_aggregator = NULL;
1102	lacp_aggregator_delref(lsc, la);
1103}
1104
1105/* mux machine */
1106
1107static void
1108lacp_sm_mux(struct lacp_port *lp)
1109{
1110	enum lacp_mux_state new_state;
1111	boolean_t p_sync =
1112		    (lp->lp_partner.lip_state & LACP_STATE_SYNC) != 0;
1113	boolean_t p_collecting =
1114	    (lp->lp_partner.lip_state & LACP_STATE_COLLECTING) != 0;
1115	enum lacp_selected selected = lp->lp_selected;
1116	struct lacp_aggregator *la;
1117
1118	/* LACP_DPRINTF((lp, "%s: state %d\n", __func__, lp->lp_mux_state)); */
1119
1120re_eval:
1121	la = lp->lp_aggregator;
1122	KASSERT(lp->lp_mux_state == LACP_MUX_DETACHED || la != NULL,
1123	    ("MUX not detached"));
1124	new_state = lp->lp_mux_state;
1125	switch (lp->lp_mux_state) {
1126	case LACP_MUX_DETACHED:
1127		if (selected != LACP_UNSELECTED) {
1128			new_state = LACP_MUX_WAITING;
1129		}
1130		break;
1131	case LACP_MUX_WAITING:
1132		KASSERT(la->la_pending > 0 ||
1133		    !LACP_TIMER_ISARMED(lp, LACP_TIMER_WAIT_WHILE),
1134		    ("timer_wait_while still active"));
1135		if (selected == LACP_SELECTED && la->la_pending == 0) {
1136			new_state = LACP_MUX_ATTACHED;
1137		} else if (selected == LACP_UNSELECTED) {
1138			new_state = LACP_MUX_DETACHED;
1139		}
1140		break;
1141	case LACP_MUX_ATTACHED:
1142		if (selected == LACP_SELECTED && p_sync) {
1143			new_state = LACP_MUX_COLLECTING;
1144		} else if (selected != LACP_SELECTED) {
1145			new_state = LACP_MUX_DETACHED;
1146		}
1147		break;
1148	case LACP_MUX_COLLECTING:
1149		if (selected == LACP_SELECTED && p_sync && p_collecting) {
1150			new_state = LACP_MUX_DISTRIBUTING;
1151		} else if (selected != LACP_SELECTED || !p_sync) {
1152			new_state = LACP_MUX_ATTACHED;
1153		}
1154		break;
1155	case LACP_MUX_DISTRIBUTING:
1156		if (selected != LACP_SELECTED || !p_sync || !p_collecting) {
1157			new_state = LACP_MUX_COLLECTING;
1158		}
1159		break;
1160	default:
1161		panic("%s: unknown state", __func__);
1162	}
1163
1164	if (lp->lp_mux_state == new_state) {
1165		return;
1166	}
1167
1168	lacp_set_mux(lp, new_state);
1169	goto re_eval;
1170}
1171
1172static void
1173lacp_set_mux(struct lacp_port *lp, enum lacp_mux_state new_state)
1174{
1175	struct lacp_aggregator *la = lp->lp_aggregator;
1176
1177	if (lp->lp_mux_state == new_state) {
1178		return;
1179	}
1180
1181	switch (new_state) {
1182	case LACP_MUX_DETACHED:
1183		lp->lp_state &= ~LACP_STATE_SYNC;
1184		lacp_disable_distributing(lp);
1185		lacp_disable_collecting(lp);
1186		lacp_sm_assert_ntt(lp);
1187		/* cancel timer */
1188		if (LACP_TIMER_ISARMED(lp, LACP_TIMER_WAIT_WHILE)) {
1189			KASSERT(la->la_pending > 0,
1190			    ("timer_wait_while not active"));
1191			la->la_pending--;
1192		}
1193		LACP_TIMER_DISARM(lp, LACP_TIMER_WAIT_WHILE);
1194		lacp_unselect(lp);
1195		break;
1196	case LACP_MUX_WAITING:
1197		LACP_TIMER_ARM(lp, LACP_TIMER_WAIT_WHILE,
1198		    LACP_AGGREGATE_WAIT_TIME);
1199		la->la_pending++;
1200		break;
1201	case LACP_MUX_ATTACHED:
1202		lp->lp_state |= LACP_STATE_SYNC;
1203		lacp_disable_collecting(lp);
1204		lacp_sm_assert_ntt(lp);
1205		break;
1206	case LACP_MUX_COLLECTING:
1207		lacp_enable_collecting(lp);
1208		lacp_disable_distributing(lp);
1209		lacp_sm_assert_ntt(lp);
1210		break;
1211	case LACP_MUX_DISTRIBUTING:
1212		lacp_enable_distributing(lp);
1213		break;
1214	default:
1215		panic("%s: unknown state", __func__);
1216	}
1217
1218	LACP_DPRINTF((lp, "mux_state %d -> %d\n", lp->lp_mux_state, new_state));
1219
1220	lp->lp_mux_state = new_state;
1221}
1222
1223static void
1224lacp_sm_mux_timer(struct lacp_port *lp)
1225{
1226	struct lacp_aggregator *la = lp->lp_aggregator;
1227#if defined(LACP_DEBUG)
1228	char buf[LACP_LAGIDSTR_MAX+1];
1229#endif
1230
1231	KASSERT(la->la_pending > 0, ("no pending event"));
1232
1233	LACP_DPRINTF((lp, "%s: aggregator %s, pending %d -> %d\n", __func__,
1234	    lacp_format_lagid(&la->la_actor, &la->la_partner,
1235	    buf, sizeof(buf)),
1236	    la->la_pending, la->la_pending - 1));
1237
1238	la->la_pending--;
1239}
1240
1241/* periodic transmit machine */
1242
1243static void
1244lacp_sm_ptx_update_timeout(struct lacp_port *lp, uint8_t oldpstate)
1245{
1246	if (LACP_STATE_EQ(oldpstate, lp->lp_partner.lip_state,
1247	    LACP_STATE_TIMEOUT)) {
1248		return;
1249	}
1250
1251	LACP_DPRINTF((lp, "partner timeout changed\n"));
1252
1253	/*
1254	 * FAST_PERIODIC -> SLOW_PERIODIC
1255	 * or
1256	 * SLOW_PERIODIC (-> PERIODIC_TX) -> FAST_PERIODIC
1257	 *
1258	 * let lacp_sm_ptx_tx_schedule to update timeout.
1259	 */
1260
1261	LACP_TIMER_DISARM(lp, LACP_TIMER_PERIODIC);
1262
1263	/*
1264	 * if timeout has been shortened, assert NTT.
1265	 */
1266
1267	if ((lp->lp_partner.lip_state & LACP_STATE_TIMEOUT)) {
1268		lacp_sm_assert_ntt(lp);
1269	}
1270}
1271
1272static void
1273lacp_sm_ptx_tx_schedule(struct lacp_port *lp)
1274{
1275	int timeout;
1276
1277	if (!(lp->lp_state & LACP_STATE_ACTIVITY) &&
1278	    !(lp->lp_partner.lip_state & LACP_STATE_ACTIVITY)) {
1279
1280		/*
1281		 * NO_PERIODIC
1282		 */
1283
1284		LACP_TIMER_DISARM(lp, LACP_TIMER_PERIODIC);
1285		return;
1286	}
1287
1288	if (LACP_TIMER_ISARMED(lp, LACP_TIMER_PERIODIC)) {
1289		return;
1290	}
1291
1292	timeout = (lp->lp_partner.lip_state & LACP_STATE_TIMEOUT) ?
1293	    LACP_FAST_PERIODIC_TIME : LACP_SLOW_PERIODIC_TIME;
1294
1295	LACP_TIMER_ARM(lp, LACP_TIMER_PERIODIC, timeout);
1296}
1297
1298static void
1299lacp_sm_ptx_timer(struct lacp_port *lp)
1300{
1301	lacp_sm_assert_ntt(lp);
1302}
1303
1304static void
1305lacp_sm_rx(struct lacp_port *lp, const struct lacpdu *du)
1306{
1307	int timeout;
1308
1309	/*
1310	 * check LACP_DISABLED first
1311	 */
1312
1313	if (!(lp->lp_state & LACP_STATE_AGGREGATION)) {
1314		return;
1315	}
1316
1317	/*
1318	 * check loopback condition.
1319	 */
1320
1321	if (!lacp_compare_systemid(&du->ldu_actor.lip_systemid,
1322	    &lp->lp_actor.lip_systemid)) {
1323		return;
1324	}
1325
1326	/*
1327	 * EXPIRED, DEFAULTED, CURRENT -> CURRENT
1328	 */
1329
1330	lacp_sm_rx_update_selected(lp, du);
1331	lacp_sm_rx_update_ntt(lp, du);
1332	lacp_sm_rx_record_pdu(lp, du);
1333
1334	timeout = (lp->lp_state & LACP_STATE_TIMEOUT) ?
1335	    LACP_SHORT_TIMEOUT_TIME : LACP_LONG_TIMEOUT_TIME;
1336	LACP_TIMER_ARM(lp, LACP_TIMER_CURRENT_WHILE, timeout);
1337
1338	lp->lp_state &= ~LACP_STATE_EXPIRED;
1339
1340	/*
1341	 * kick transmit machine without waiting the next tick.
1342	 */
1343
1344	lacp_sm_tx(lp);
1345}
1346
1347static void
1348lacp_sm_rx_set_expired(struct lacp_port *lp)
1349{
1350	lp->lp_partner.lip_state &= ~LACP_STATE_SYNC;
1351	lp->lp_partner.lip_state |= LACP_STATE_TIMEOUT;
1352	LACP_TIMER_ARM(lp, LACP_TIMER_CURRENT_WHILE, LACP_SHORT_TIMEOUT_TIME);
1353	lp->lp_state |= LACP_STATE_EXPIRED;
1354}
1355
1356static void
1357lacp_sm_rx_timer(struct lacp_port *lp)
1358{
1359	if ((lp->lp_state & LACP_STATE_EXPIRED) == 0) {
1360		/* CURRENT -> EXPIRED */
1361		LACP_DPRINTF((lp, "%s: CURRENT -> EXPIRED\n", __func__));
1362		lacp_sm_rx_set_expired(lp);
1363	} else {
1364		/* EXPIRED -> DEFAULTED */
1365		LACP_DPRINTF((lp, "%s: EXPIRED -> DEFAULTED\n", __func__));
1366		lacp_sm_rx_update_default_selected(lp);
1367		lacp_sm_rx_record_default(lp);
1368		lp->lp_state &= ~LACP_STATE_EXPIRED;
1369	}
1370}
1371
1372static void
1373lacp_sm_rx_record_pdu(struct lacp_port *lp, const struct lacpdu *du)
1374{
1375	boolean_t active;
1376	uint8_t oldpstate;
1377#if defined(LACP_DEBUG)
1378	char buf[LACP_STATESTR_MAX+1];
1379#endif
1380
1381	/* LACP_DPRINTF((lp, "%s\n", __func__)); */
1382
1383	oldpstate = lp->lp_partner.lip_state;
1384
1385	active = (du->ldu_actor.lip_state & LACP_STATE_ACTIVITY)
1386	    || ((lp->lp_state & LACP_STATE_ACTIVITY) &&
1387	    (du->ldu_partner.lip_state & LACP_STATE_ACTIVITY));
1388
1389	lp->lp_partner = du->ldu_actor;
1390	if (active &&
1391	    ((LACP_STATE_EQ(lp->lp_state, du->ldu_partner.lip_state,
1392	    LACP_STATE_AGGREGATION) &&
1393	    !lacp_compare_peerinfo(&lp->lp_actor, &du->ldu_partner))
1394	    || (du->ldu_partner.lip_state & LACP_STATE_AGGREGATION) == 0)) {
1395		/* XXX nothing? */
1396	} else {
1397		lp->lp_partner.lip_state &= ~LACP_STATE_SYNC;
1398	}
1399
1400	lp->lp_state &= ~LACP_STATE_DEFAULTED;
1401
1402	if (oldpstate != lp->lp_partner.lip_state) {
1403		LACP_DPRINTF((lp, "old pstate %s\n",
1404		    lacp_format_state(oldpstate, buf, sizeof(buf))));
1405		LACP_DPRINTF((lp, "new pstate %s\n",
1406		    lacp_format_state(lp->lp_partner.lip_state, buf,
1407		    sizeof(buf))));
1408	}
1409
1410	lacp_sm_ptx_update_timeout(lp, oldpstate);
1411}
1412
1413static void
1414lacp_sm_rx_update_ntt(struct lacp_port *lp, const struct lacpdu *du)
1415{
1416	/* LACP_DPRINTF((lp, "%s\n", __func__)); */
1417
1418	if (lacp_compare_peerinfo(&lp->lp_actor, &du->ldu_partner) ||
1419	    !LACP_STATE_EQ(lp->lp_state, du->ldu_partner.lip_state,
1420	    LACP_STATE_ACTIVITY | LACP_STATE_SYNC | LACP_STATE_AGGREGATION)) {
1421		LACP_DPRINTF((lp, "%s: assert ntt\n", __func__));
1422		lacp_sm_assert_ntt(lp);
1423	}
1424}
1425
1426static void
1427lacp_sm_rx_record_default(struct lacp_port *lp)
1428{
1429	uint8_t oldpstate;
1430
1431	/* LACP_DPRINTF((lp, "%s\n", __func__)); */
1432
1433	oldpstate = lp->lp_partner.lip_state;
1434	lp->lp_partner = lacp_partner_admin;
1435	lp->lp_state |= LACP_STATE_DEFAULTED;
1436	lacp_sm_ptx_update_timeout(lp, oldpstate);
1437}
1438
1439static void
1440lacp_sm_rx_update_selected_from_peerinfo(struct lacp_port *lp,
1441    const struct lacp_peerinfo *info)
1442{
1443	/* LACP_DPRINTF((lp, "%s\n", __func__)); */
1444
1445	if (lacp_compare_peerinfo(&lp->lp_partner, info) ||
1446	    !LACP_STATE_EQ(lp->lp_partner.lip_state, info->lip_state,
1447	    LACP_STATE_AGGREGATION)) {
1448		lp->lp_selected = LACP_UNSELECTED;
1449		/* mux machine will clean up lp->lp_aggregator */
1450	}
1451}
1452
1453static void
1454lacp_sm_rx_update_selected(struct lacp_port *lp, const struct lacpdu *du)
1455{
1456	/* LACP_DPRINTF((lp, "%s\n", __func__)); */
1457
1458	lacp_sm_rx_update_selected_from_peerinfo(lp, &du->ldu_actor);
1459}
1460
1461static void
1462lacp_sm_rx_update_default_selected(struct lacp_port *lp)
1463{
1464	/* LACP_DPRINTF((lp, "%s\n", __func__)); */
1465
1466	lacp_sm_rx_update_selected_from_peerinfo(lp, &lacp_partner_admin);
1467}
1468
1469/* transmit machine */
1470
1471static void
1472lacp_sm_tx(struct lacp_port *lp)
1473{
1474	int error;
1475
1476	if (!(lp->lp_state & LACP_STATE_AGGREGATION)
1477#if 1
1478	    || (!(lp->lp_state & LACP_STATE_ACTIVITY)
1479	    && !(lp->lp_partner.lip_state & LACP_STATE_ACTIVITY))
1480#endif
1481	    ) {
1482		lp->lp_flags &= ~LACP_PORT_NTT;
1483	}
1484
1485	if (!(lp->lp_flags & LACP_PORT_NTT)) {
1486		return;
1487	}
1488
1489	/* Rate limit to 3 PDUs per LACP_FAST_PERIODIC_TIME */
1490	if (ppsratecheck(&lp->lp_last_lacpdu, &lp->lp_lacpdu_sent,
1491		    (3 / LACP_FAST_PERIODIC_TIME)) == 0) {
1492		LACP_DPRINTF((lp, "rate limited pdu\n"));
1493		return;
1494	}
1495
1496	error = lacp_xmit_lacpdu(lp);
1497
1498	if (error == 0) {
1499		lp->lp_flags &= ~LACP_PORT_NTT;
1500	} else {
1501		LACP_DPRINTF((lp, "lacpdu transmit failure, error %d\n",
1502		    error));
1503	}
1504}
1505
1506static void
1507lacp_sm_assert_ntt(struct lacp_port *lp)
1508{
1509
1510	lp->lp_flags |= LACP_PORT_NTT;
1511}
1512
1513static void
1514lacp_run_timers(struct lacp_port *lp)
1515{
1516	int i;
1517
1518	for (i = 0; i < LACP_NTIMER; i++) {
1519		KASSERT(lp->lp_timer[i] >= 0,
1520		    ("invalid timer value %d", lp->lp_timer[i]));
1521		if (lp->lp_timer[i] == 0) {
1522			continue;
1523		} else if (--lp->lp_timer[i] <= 0) {
1524			if (lacp_timer_funcs[i]) {
1525				(*lacp_timer_funcs[i])(lp);
1526			}
1527		}
1528	}
1529}
1530
1531int
1532lacp_marker_input(struct lagg_port *lgp, struct mbuf *m)
1533{
1534	struct lacp_port *lp = LACP_PORT(lgp);
1535	struct markerdu *mdu;
1536	int error = 0;
1537
1538	LAGG_LOCK_ASSERT(lgp->lp_lagg);
1539
1540	if (__predict_false(lp->lp_flags & LACP_PORT_DETACHING)) {
1541		goto bad;
1542	}
1543
1544	if (m->m_pkthdr.len != sizeof(*mdu)) {
1545		goto bad;
1546	}
1547
1548	if ((m->m_flags & M_MCAST) == 0) {
1549		goto bad;
1550	}
1551
1552	if (m->m_len < sizeof(*mdu)) {
1553		m = m_pullup(m, sizeof(*mdu));
1554		if (m == NULL) {
1555			return (ENOMEM);
1556		}
1557	}
1558
1559	mdu = mtod(m, struct markerdu *);
1560
1561	if (memcmp(&mdu->mdu_eh.ether_dhost,
1562	    &ethermulticastaddr_slowprotocols, ETHER_ADDR_LEN)) {
1563		goto bad;
1564	}
1565
1566	/* XXX
1567	KASSERT(mdu->mdu_sph.sph_subtype == SLOWPROTOCOLS_SUBTYPE_MARKER,
1568	    ("a very bad kassert!"));
1569	*/
1570
1571	if (mdu->mdu_sph.sph_version != 1) {
1572		goto bad;
1573	}
1574
1575	switch (mdu->mdu_tlv.tlv_type) {
1576	case MARKER_TYPE_INFO:
1577		if (tlv_check(mdu, sizeof(*mdu), &mdu->mdu_tlv,
1578		    marker_info_tlv_template, TRUE)) {
1579			goto bad;
1580		}
1581		mdu->mdu_tlv.tlv_type = MARKER_TYPE_RESPONSE;
1582		memcpy(&mdu->mdu_eh.ether_dhost,
1583		    &ethermulticastaddr_slowprotocols, ETHER_ADDR_LEN);
1584		memcpy(&mdu->mdu_eh.ether_shost,
1585		    lgp->lp_lladdr, ETHER_ADDR_LEN);
1586		error = lagg_enqueue(lp->lp_ifp, m);
1587		break;
1588
1589	case MARKER_TYPE_RESPONSE:
1590		if (tlv_check(mdu, sizeof(*mdu), &mdu->mdu_tlv,
1591		    marker_response_tlv_template, TRUE)) {
1592			goto bad;
1593		}
1594		/*
1595		 * we are not interested in responses as
1596		 * we don't have a marker sender.
1597		 */
1598		/* FALLTHROUGH */
1599	default:
1600		goto bad;
1601	}
1602
1603	return (error);
1604
1605bad:
1606	m_freem(m);
1607	return (EINVAL);
1608}
1609
1610static int
1611tlv_check(const void *p, size_t size, const struct tlvhdr *tlv,
1612    const struct tlv_template *tmpl, boolean_t check_type)
1613{
1614	while (/* CONSTCOND */ 1) {
1615		if ((const char *)tlv - (const char *)p + sizeof(*tlv) > size) {
1616			return (EINVAL);
1617		}
1618		if ((check_type && tlv->tlv_type != tmpl->tmpl_type) ||
1619		    tlv->tlv_length != tmpl->tmpl_length) {
1620			return (EINVAL);
1621		}
1622		if (tmpl->tmpl_type == 0) {
1623			break;
1624		}
1625		tlv = (const struct tlvhdr *)
1626		    ((const char *)tlv + tlv->tlv_length);
1627		tmpl++;
1628	}
1629
1630	return (0);
1631}
1632
1633#if defined(LACP_DEBUG)
1634const char *
1635lacp_format_mac(const uint8_t *mac, char *buf, size_t buflen)
1636{
1637	snprintf(buf, buflen, "%02X-%02X-%02X-%02X-%02X-%02X",
1638	    (int)mac[0],
1639	    (int)mac[1],
1640	    (int)mac[2],
1641	    (int)mac[3],
1642	    (int)mac[4],
1643	    (int)mac[5]);
1644
1645	return (buf);
1646}
1647
1648const char *
1649lacp_format_systemid(const struct lacp_systemid *sysid,
1650    char *buf, size_t buflen)
1651{
1652	char macbuf[LACP_MACSTR_MAX+1];
1653
1654	snprintf(buf, buflen, "%04X,%s",
1655	    ntohs(sysid->lsi_prio),
1656	    lacp_format_mac(sysid->lsi_mac, macbuf, sizeof(macbuf)));
1657
1658	return (buf);
1659}
1660
1661const char *
1662lacp_format_portid(const struct lacp_portid *portid, char *buf, size_t buflen)
1663{
1664	snprintf(buf, buflen, "%04X,%04X",
1665	    ntohs(portid->lpi_prio),
1666	    ntohs(portid->lpi_portno));
1667
1668	return (buf);
1669}
1670
1671const char *
1672lacp_format_partner(const struct lacp_peerinfo *peer, char *buf, size_t buflen)
1673{
1674	char sysid[LACP_SYSTEMIDSTR_MAX+1];
1675	char portid[LACP_PORTIDSTR_MAX+1];
1676
1677	snprintf(buf, buflen, "(%s,%04X,%s)",
1678	    lacp_format_systemid(&peer->lip_systemid, sysid, sizeof(sysid)),
1679	    ntohs(peer->lip_key),
1680	    lacp_format_portid(&peer->lip_portid, portid, sizeof(portid)));
1681
1682	return (buf);
1683}
1684
1685const char *
1686lacp_format_lagid(const struct lacp_peerinfo *a,
1687    const struct lacp_peerinfo *b, char *buf, size_t buflen)
1688{
1689	char astr[LACP_PARTNERSTR_MAX+1];
1690	char bstr[LACP_PARTNERSTR_MAX+1];
1691
1692#if 0
1693	/*
1694	 * there's a convention to display small numbered peer
1695	 * in the left.
1696	 */
1697
1698	if (lacp_compare_peerinfo(a, b) > 0) {
1699		const struct lacp_peerinfo *t;
1700
1701		t = a;
1702		a = b;
1703		b = t;
1704	}
1705#endif
1706
1707	snprintf(buf, buflen, "[%s,%s]",
1708	    lacp_format_partner(a, astr, sizeof(astr)),
1709	    lacp_format_partner(b, bstr, sizeof(bstr)));
1710
1711	return (buf);
1712}
1713
1714const char *
1715lacp_format_lagid_aggregator(const struct lacp_aggregator *la,
1716    char *buf, size_t buflen)
1717{
1718	if (la == NULL) {
1719		return ("(none)");
1720	}
1721
1722	return (lacp_format_lagid(&la->la_actor, &la->la_partner, buf, buflen));
1723}
1724
1725const char *
1726lacp_format_state(uint8_t state, char *buf, size_t buflen)
1727{
1728	snprintf(buf, buflen, "%b", state, LACP_STATE_BITS);
1729	return (buf);
1730}
1731
1732static void
1733lacp_dump_lacpdu(const struct lacpdu *du)
1734{
1735	char buf[LACP_PARTNERSTR_MAX+1];
1736	char buf2[LACP_STATESTR_MAX+1];
1737
1738	printf("actor=%s\n",
1739	    lacp_format_partner(&du->ldu_actor, buf, sizeof(buf)));
1740	printf("actor.state=%s\n",
1741	    lacp_format_state(du->ldu_actor.lip_state, buf2, sizeof(buf2)));
1742	printf("partner=%s\n",
1743	    lacp_format_partner(&du->ldu_partner, buf, sizeof(buf)));
1744	printf("partner.state=%s\n",
1745	    lacp_format_state(du->ldu_partner.lip_state, buf2, sizeof(buf2)));
1746
1747	printf("maxdelay=%d\n", ntohs(du->ldu_collector.lci_maxdelay));
1748}
1749
1750static void
1751lacp_dprintf(const struct lacp_port *lp, const char *fmt, ...)
1752{
1753	va_list va;
1754
1755	if (lp) {
1756		printf("%s: ", lp->lp_ifp->if_xname);
1757	}
1758
1759	va_start(va, fmt);
1760	vprintf(fmt, va);
1761	va_end(va);
1762}
1763#endif
1764