sys/net/if_lagg.c

   1 /*      $OpenBSD: if_trunk.c,v 1.30 2007/01/31 06:20:19 reyk Exp $      */
   2
   3 /*
   4  * Copyright (c) 2005, 2006 Reyk Floeter <reyk@openbsd.org>
   5  * Copyright (c) 2007 Andrew Thompson <thompsa@FreeBSD.org>
   6  * Copyright (c) 2014, 2016 Marcelo Araujo <araujo@FreeBSD.org>
   7  *
   8  * Permission to use, copy, modify, and distribute this software for any
   9  * purpose with or without fee is hereby granted, provided that the above
  10  * copyright notice and this permission notice appear in all copies.
  11  *
  12  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
  13  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
  14  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
  15  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  16  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
  17  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
  18  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  19  */
  20
  21 #include <sys/cdefs.h>
  22 __FBSDID("$FreeBSD$");
  23
  24 #include "opt_inet.h"
  25 #include "opt_inet6.h"
  26 #include "opt_kern_tls.h"
  27 #include "opt_ratelimit.h"
  28
  29 #include <sys/param.h>
  30 #include <sys/kernel.h>
  31 #include <sys/malloc.h>
  32 #include <sys/mbuf.h>
  33 #include <sys/queue.h>
  34 #include <sys/socket.h>
  35 #include <sys/sockio.h>
  36 #include <sys/sysctl.h>
  37 #include <sys/module.h>
  38 #include <sys/priv.h>
  39 #include <sys/systm.h>
  40 #include <sys/proc.h>
  41 #include <sys/lock.h>
  42 #include <sys/rmlock.h>
  43 #include <sys/sx.h>
  44 #include <sys/taskqueue.h>
  45 #include <sys/eventhandler.h>
  46
  47 #include <net/ethernet.h>
  48 #include <net/if.h>
  49 #include <net/if_clone.h>
  50 #include <net/if_arp.h>
  51 #include <net/if_dl.h>
  52 #include <net/if_media.h>
  53 #include <net/if_types.h>
  54 #include <net/if_var.h>
  55 #include <net/if_private.h>
  56 #include <net/bpf.h>
  57 #include <net/route.h>
  58 #include <net/vnet.h>
  59 #include <net/infiniband.h>
  60
  61 #if defined(INET) || defined(INET6)
  62 #include <netinet/in.h>
  63 #include <netinet/ip.h>
  64 #endif
  65 #ifdef INET
  66 #include <netinet/in_systm.h>
  67 #include <netinet/if_ether.h>
  68 #endif
  69
  70 #ifdef INET6
  71 #include <netinet/ip6.h>
  72 #include <netinet6/in6_var.h>
  73 #include <netinet6/in6_ifattach.h>
  74 #endif
  75
  76 #include <net/if_vlan_var.h>
  77 #include <net/if_lagg.h>
  78 #include <net/ieee8023ad_lacp.h>
  79
  80 #ifdef DEV_NETMAP
  81 MODULE_DEPEND(if_lagg, netmap, 1, 1, 1);
  82 #endif
  83
  84 #define LAGG_SX_INIT(_sc)       sx_init(&(_sc)->sc_sx, "if_lagg sx")
  85 #define LAGG_SX_DESTROY(_sc)    sx_destroy(&(_sc)->sc_sx)
  86 #define LAGG_XLOCK(_sc)         sx_xlock(&(_sc)->sc_sx)
  87 #define LAGG_XUNLOCK(_sc)       sx_xunlock(&(_sc)->sc_sx)
  88 #define LAGG_SXLOCK_ASSERT(_sc) sx_assert(&(_sc)->sc_sx, SA_LOCKED)
  89 #define LAGG_XLOCK_ASSERT(_sc)  sx_assert(&(_sc)->sc_sx, SA_XLOCKED)
  90
  91 /* Special flags we should propagate to the lagg ports. */
  92 static struct {
  93         int flag;
  94         int (*func)(struct ifnet *, int);
  95 } lagg_pflags[] = {
  96         {IFF_PROMISC, ifpromisc},
  97         {IFF_ALLMULTI, if_allmulti},
  98         {0, NULL}
  99 };
 100
 101 struct lagg_snd_tag {
 102         struct m_snd_tag com;
 103         struct m_snd_tag *tag;
 104 };
 105
 106 VNET_DEFINE_STATIC(SLIST_HEAD(__trhead, lagg_softc), lagg_list); /* list of laggs */
 107 #define V_lagg_list     VNET(lagg_list)
 108 VNET_DEFINE_STATIC(struct mtx, lagg_list_mtx);
 109 #define V_lagg_list_mtx VNET(lagg_list_mtx)
 110 #define LAGG_LIST_LOCK_INIT(x)          mtx_init(&V_lagg_list_mtx, \
 111                                         "if_lagg list", NULL, MTX_DEF)
 112 #define LAGG_LIST_LOCK_DESTROY(x)       mtx_destroy(&V_lagg_list_mtx)
 113 #define LAGG_LIST_LOCK(x)               mtx_lock(&V_lagg_list_mtx)
 114 #define LAGG_LIST_UNLOCK(x)             mtx_unlock(&V_lagg_list_mtx)
 115 static eventhandler_tag lagg_detach_cookie = NULL;
 116
 117 static int      lagg_clone_create(struct if_clone *, char *, size_t,
 118                     struct ifc_data *, struct ifnet **);
 119 static int      lagg_clone_destroy(struct if_clone *, struct ifnet *, uint32_t);
 120 VNET_DEFINE_STATIC(struct if_clone *, lagg_cloner);
 121 #define V_lagg_cloner   VNET(lagg_cloner)
 122 static const char laggname[] = "lagg";
 123 static MALLOC_DEFINE(M_LAGG, laggname, "802.3AD Link Aggregation Interface");
 124
 125 static void     lagg_capabilities(struct lagg_softc *);
 126 static int      lagg_port_create(struct lagg_softc *, struct ifnet *);
 127 static int      lagg_port_destroy(struct lagg_port *, int);
 128 static struct mbuf *lagg_input_ethernet(struct ifnet *, struct mbuf *);
 129 static struct mbuf *lagg_input_infiniband(struct ifnet *, struct mbuf *);
 130 static void     lagg_linkstate(struct lagg_softc *);
 131 static void     lagg_port_state(struct ifnet *, int);
 132 static int      lagg_port_ioctl(struct ifnet *, u_long, caddr_t);
 133 static int      lagg_port_output(struct ifnet *, struct mbuf *,
 134                     const struct sockaddr *, struct route *);
 135 static void     lagg_port_ifdetach(void *arg __unused, struct ifnet *);
 136 #ifdef LAGG_PORT_STACKING
 137 static int      lagg_port_checkstacking(struct lagg_softc *);
 138 #endif
 139 static void     lagg_port2req(struct lagg_port *, struct lagg_reqport *);
 140 static void     lagg_init(void *);
 141 static void     lagg_stop(struct lagg_softc *);
 142 static int      lagg_ioctl(struct ifnet *, u_long, caddr_t);
 143 #if defined(KERN_TLS) || defined(RATELIMIT)
 144 static int      lagg_snd_tag_alloc(struct ifnet *,
 145                     union if_snd_tag_alloc_params *,
 146                     struct m_snd_tag **);
 147 static int      lagg_snd_tag_modify(struct m_snd_tag *,
 148                     union if_snd_tag_modify_params *);
 149 static int      lagg_snd_tag_query(struct m_snd_tag *,
 150                     union if_snd_tag_query_params *);
 151 static void     lagg_snd_tag_free(struct m_snd_tag *);
 152 static struct m_snd_tag *lagg_next_snd_tag(struct m_snd_tag *);
 153 static void     lagg_ratelimit_query(struct ifnet *,
 154                     struct if_ratelimit_query_results *);
 155 #endif
 156 static int      lagg_setmulti(struct lagg_port *);
 157 static int      lagg_clrmulti(struct lagg_port *);
 158 static void     lagg_setcaps(struct lagg_port *, int cap, int cap2);
 159 static int      lagg_setflag(struct lagg_port *, int, int,
 160                     int (*func)(struct ifnet *, int));
 161 static int      lagg_setflags(struct lagg_port *, int status);
 162 static uint64_t lagg_get_counter(struct ifnet *ifp, ift_counter cnt);
 163 static int      lagg_transmit_ethernet(struct ifnet *, struct mbuf *);
 164 static int      lagg_transmit_infiniband(struct ifnet *, struct mbuf *);
 165 static void     lagg_qflush(struct ifnet *);
 166 static int      lagg_media_change(struct ifnet *);
 167 static void     lagg_media_status(struct ifnet *, struct ifmediareq *);
 168 static struct lagg_port *lagg_link_active(struct lagg_softc *,
 169                     struct lagg_port *);
 170
 171 /* Simple round robin */
 172 static void     lagg_rr_attach(struct lagg_softc *);
 173 static int      lagg_rr_start(struct lagg_softc *, struct mbuf *);
 174
 175 /* Active failover */
 176 static int      lagg_fail_start(struct lagg_softc *, struct mbuf *);
 177 static struct mbuf *lagg_fail_input(struct lagg_softc *, struct lagg_port *,
 178                     struct mbuf *);
 179
 180 /* Loadbalancing */
 181 static void     lagg_lb_attach(struct lagg_softc *);
 182 static void     lagg_lb_detach(struct lagg_softc *);
 183 static int      lagg_lb_port_create(struct lagg_port *);
 184 static void     lagg_lb_port_destroy(struct lagg_port *);
 185 static int      lagg_lb_start(struct lagg_softc *, struct mbuf *);
 186 static int      lagg_lb_porttable(struct lagg_softc *, struct lagg_port *);
 187
 188 /* Broadcast */
 189 static int      lagg_bcast_start(struct lagg_softc *, struct mbuf *);
 190
 191 /* 802.3ad LACP */
 192 static void     lagg_lacp_attach(struct lagg_softc *);
 193 static void     lagg_lacp_detach(struct lagg_softc *);
 194 static int      lagg_lacp_start(struct lagg_softc *, struct mbuf *);
 195 static struct mbuf *lagg_lacp_input(struct lagg_softc *, struct lagg_port *,
 196                     struct mbuf *);
 197 static void     lagg_lacp_lladdr(struct lagg_softc *);
 198
 199 /* Default input */
 200 static struct mbuf *lagg_default_input(struct lagg_softc *, struct lagg_port *,
 201                     struct mbuf *);
 202
 203 /* lagg protocol table */
 204 static const struct lagg_proto {
 205         lagg_proto      pr_num;
 206         void            (*pr_attach)(struct lagg_softc *);
 207         void            (*pr_detach)(struct lagg_softc *);
 208         int             (*pr_start)(struct lagg_softc *, struct mbuf *);
 209         struct mbuf *   (*pr_input)(struct lagg_softc *, struct lagg_port *,
 210                             struct mbuf *);
 211         int             (*pr_addport)(struct lagg_port *);
 212         void            (*pr_delport)(struct lagg_port *);
 213         void            (*pr_linkstate)(struct lagg_port *);
 214         void            (*pr_init)(struct lagg_softc *);
 215         void            (*pr_stop)(struct lagg_softc *);
 216         void            (*pr_lladdr)(struct lagg_softc *);
 217         void            (*pr_request)(struct lagg_softc *, void *);
 218         void            (*pr_portreq)(struct lagg_port *, void *);
 219 } lagg_protos[] = {
 220     {
 221         .pr_num = LAGG_PROTO_NONE
 222     },
 223     {
 224         .pr_num = LAGG_PROTO_ROUNDROBIN,
 225         .pr_attach = lagg_rr_attach,
 226         .pr_start = lagg_rr_start,
 227         .pr_input = lagg_default_input,
 228     },
 229     {
 230         .pr_num = LAGG_PROTO_FAILOVER,
 231         .pr_start = lagg_fail_start,
 232         .pr_input = lagg_fail_input,
 233     },
 234     {
 235         .pr_num = LAGG_PROTO_LOADBALANCE,
 236         .pr_attach = lagg_lb_attach,
 237         .pr_detach = lagg_lb_detach,
 238         .pr_start = lagg_lb_start,
 239         .pr_input = lagg_default_input,
 240         .pr_addport = lagg_lb_port_create,
 241         .pr_delport = lagg_lb_port_destroy,
 242     },
 243     {
 244         .pr_num = LAGG_PROTO_LACP,
 245         .pr_attach = lagg_lacp_attach,
 246         .pr_detach = lagg_lacp_detach,
 247         .pr_start = lagg_lacp_start,
 248         .pr_input = lagg_lacp_input,
 249         .pr_addport = lacp_port_create,
 250         .pr_delport = lacp_port_destroy,
 251         .pr_linkstate = lacp_linkstate,
 252         .pr_init = lacp_init,
 253         .pr_stop = lacp_stop,
 254         .pr_lladdr = lagg_lacp_lladdr,
 255         .pr_request = lacp_req,
 256         .pr_portreq = lacp_portreq,
 257     },
 258     {
 259         .pr_num = LAGG_PROTO_BROADCAST,
 260         .pr_start = lagg_bcast_start,
 261         .pr_input = lagg_default_input,
 262     },
 263 };
 264
 265 SYSCTL_DECL(_net_link);
 266 SYSCTL_NODE(_net_link, OID_AUTO, lagg, CTLFLAG_RW | CTLFLAG_MPSAFE, 0,
 267     "Link Aggregation");
 268
 269 /* Allow input on any failover links */
 270 VNET_DEFINE_STATIC(int, lagg_failover_rx_all);
 271 #define V_lagg_failover_rx_all  VNET(lagg_failover_rx_all)
 272 SYSCTL_INT(_net_link_lagg, OID_AUTO, failover_rx_all, CTLFLAG_RW | CTLFLAG_VNET,
 273     &VNET_NAME(lagg_failover_rx_all), 0,
 274     "Accept input from any interface in a failover lagg");
 275
 276 /* Default value for using flowid */
 277 VNET_DEFINE_STATIC(int, def_use_flowid) = 0;
 278 #define V_def_use_flowid        VNET(def_use_flowid)
 279 SYSCTL_INT(_net_link_lagg, OID_AUTO, default_use_flowid,
 280     CTLFLAG_RWTUN | CTLFLAG_VNET, &VNET_NAME(def_use_flowid), 0,
 281     "Default setting for using flow id for load sharing");
 282
 283 /* Default value for using numa */
 284 VNET_DEFINE_STATIC(int, def_use_numa) = 1;
 285 #define V_def_use_numa  VNET(def_use_numa)
 286 SYSCTL_INT(_net_link_lagg, OID_AUTO, default_use_numa,
 287     CTLFLAG_RWTUN | CTLFLAG_VNET, &VNET_NAME(def_use_numa), 0,
 288     "Use numa to steer flows");
 289
 290 /* Default value for flowid shift */
 291 VNET_DEFINE_STATIC(int, def_flowid_shift) = 16;
 292 #define V_def_flowid_shift      VNET(def_flowid_shift)
 293 SYSCTL_INT(_net_link_lagg, OID_AUTO, default_flowid_shift,
 294     CTLFLAG_RWTUN | CTLFLAG_VNET, &VNET_NAME(def_flowid_shift), 0,
 295     "Default setting for flowid shift for load sharing");
 296
 297 static void
 298 vnet_lagg_init(const void *unused __unused)
 299 {
 300
 301         LAGG_LIST_LOCK_INIT();
 302         SLIST_INIT(&V_lagg_list);
 303         struct if_clone_addreq req = {
 304                 .create_f = lagg_clone_create,
 305                 .destroy_f = lagg_clone_destroy,
 306                 .flags = IFC_F_AUTOUNIT,
 307         };
 308         V_lagg_cloner = ifc_attach_cloner(laggname, &req);
 309 }
 310 VNET_SYSINIT(vnet_lagg_init, SI_SUB_PROTO_IFATTACHDOMAIN, SI_ORDER_ANY,
 311     vnet_lagg_init, NULL);
 312
 313 static void
 314 vnet_lagg_uninit(const void *unused __unused)
 315 {
 316
 317         ifc_detach_cloner(V_lagg_cloner);
 318         LAGG_LIST_LOCK_DESTROY();
 319 }
 320 VNET_SYSUNINIT(vnet_lagg_uninit, SI_SUB_INIT_IF, SI_ORDER_ANY,
 321     vnet_lagg_uninit, NULL);
 322
 323 static int
 324 lagg_modevent(module_t mod, int type, void *data)
 325 {
 326
 327         switch (type) {
 328         case MOD_LOAD:
 329                 lagg_input_ethernet_p = lagg_input_ethernet;
 330                 lagg_input_infiniband_p = lagg_input_infiniband;
 331                 lagg_linkstate_p = lagg_port_state;
 332                 lagg_detach_cookie = EVENTHANDLER_REGISTER(
 333                     ifnet_departure_event, lagg_port_ifdetach, NULL,
 334                     EVENTHANDLER_PRI_ANY);
 335                 break;
 336         case MOD_UNLOAD:
 337                 EVENTHANDLER_DEREGISTER(ifnet_departure_event,
 338                     lagg_detach_cookie);
 339                 lagg_input_ethernet_p = NULL;
 340                 lagg_input_infiniband_p = NULL;
 341                 lagg_linkstate_p = NULL;
 342                 break;
 343         default:
 344                 return (EOPNOTSUPP);
 345         }
 346         return (0);
 347 }
 348
 349 static moduledata_t lagg_mod = {
 350         "if_lagg",
 351         lagg_modevent,
 352         0
 353 };
 354
 355 DECLARE_MODULE(if_lagg, lagg_mod, SI_SUB_PSEUDO, SI_ORDER_ANY);
 356 MODULE_VERSION(if_lagg, 1);
 357 MODULE_DEPEND(if_lagg, if_infiniband, 1, 1, 1);
 358
 359 static void
 360 lagg_proto_attach(struct lagg_softc *sc, lagg_proto pr)
 361 {
 362
 363         LAGG_XLOCK_ASSERT(sc);
 364         KASSERT(sc->sc_proto == LAGG_PROTO_NONE, ("%s: sc %p has proto",
 365             __func__, sc));
 366
 367         if (sc->sc_ifflags & IFF_DEBUG)
 368                 if_printf(sc->sc_ifp, "using proto %u\n", pr);
 369
 370         if (lagg_protos[pr].pr_attach != NULL)
 371                 lagg_protos[pr].pr_attach(sc);
 372         sc->sc_proto = pr;
 373 }
 374
 375 static void
 376 lagg_proto_detach(struct lagg_softc *sc)
 377 {
 378         lagg_proto pr;
 379
 380         LAGG_XLOCK_ASSERT(sc);
 381         pr = sc->sc_proto;
 382         sc->sc_proto = LAGG_PROTO_NONE;
 383
 384         if (lagg_protos[pr].pr_detach != NULL)
 385                 lagg_protos[pr].pr_detach(sc);
 386 }
 387
 388 static inline int
 389 lagg_proto_start(struct lagg_softc *sc, struct mbuf *m)
 390 {
 391
 392         return (lagg_protos[sc->sc_proto].pr_start(sc, m));
 393 }
 394
 395 static inline struct mbuf *
 396 lagg_proto_input(struct lagg_softc *sc, struct lagg_port *lp, struct mbuf *m)
 397 {
 398
 399         return (lagg_protos[sc->sc_proto].pr_input(sc, lp, m));
 400 }
 401
 402 static int
 403 lagg_proto_addport(struct lagg_softc *sc, struct lagg_port *lp)
 404 {
 405
 406         if (lagg_protos[sc->sc_proto].pr_addport == NULL)
 407                 return (0);
 408         else
 409                 return (lagg_protos[sc->sc_proto].pr_addport(lp));
 410 }
 411
 412 static void
 413 lagg_proto_delport(struct lagg_softc *sc, struct lagg_port *lp)
 414 {
 415
 416         if (lagg_protos[sc->sc_proto].pr_delport != NULL)
 417                 lagg_protos[sc->sc_proto].pr_delport(lp);
 418 }
 419
 420 static void
 421 lagg_proto_linkstate(struct lagg_softc *sc, struct lagg_port *lp)
 422 {
 423
 424         if (lagg_protos[sc->sc_proto].pr_linkstate != NULL)
 425                 lagg_protos[sc->sc_proto].pr_linkstate(lp);
 426 }
 427
 428 static void
 429 lagg_proto_init(struct lagg_softc *sc)
 430 {
 431
 432         if (lagg_protos[sc->sc_proto].pr_init != NULL)
 433                 lagg_protos[sc->sc_proto].pr_init(sc);
 434 }
 435
 436 static void
 437 lagg_proto_stop(struct lagg_softc *sc)
 438 {
 439
 440         if (lagg_protos[sc->sc_proto].pr_stop != NULL)
 441                 lagg_protos[sc->sc_proto].pr_stop(sc);
 442 }
 443
 444 static void
 445 lagg_proto_lladdr(struct lagg_softc *sc)
 446 {
 447
 448         if (lagg_protos[sc->sc_proto].pr_lladdr != NULL)
 449                 lagg_protos[sc->sc_proto].pr_lladdr(sc);
 450 }
 451
 452 static void
 453 lagg_proto_request(struct lagg_softc *sc, void *v)
 454 {
 455
 456         if (lagg_protos[sc->sc_proto].pr_request != NULL)
 457                 lagg_protos[sc->sc_proto].pr_request(sc, v);
 458 }
 459
 460 static void
 461 lagg_proto_portreq(struct lagg_softc *sc, struct lagg_port *lp, void *v)
 462 {
 463
 464         if (lagg_protos[sc->sc_proto].pr_portreq != NULL)
 465                 lagg_protos[sc->sc_proto].pr_portreq(lp, v);
 466 }
 467
 468 /*
 469  * This routine is run via an vlan
 470  * config EVENT
 471  */
 472 static void
 473 lagg_register_vlan(void *arg, struct ifnet *ifp, u_int16_t vtag)
 474 {
 475         struct lagg_softc *sc = ifp->if_softc;
 476         struct lagg_port *lp;
 477
 478         if (ifp->if_softc != arg) /* Not our event */
 479                 return;
 480
 481         LAGG_XLOCK(sc);
 482         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries)
 483                 EVENTHANDLER_INVOKE(vlan_config, lp->lp_ifp, vtag);
 484         LAGG_XUNLOCK(sc);
 485 }
 486
 487 /*
 488  * This routine is run via an vlan
 489  * unconfig EVENT
 490  */
 491 static void
 492 lagg_unregister_vlan(void *arg, struct ifnet *ifp, u_int16_t vtag)
 493 {
 494         struct lagg_softc *sc = ifp->if_softc;
 495         struct lagg_port *lp;
 496
 497         if (ifp->if_softc != arg) /* Not our event */
 498                 return;
 499
 500         LAGG_XLOCK(sc);
 501         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries)
 502                 EVENTHANDLER_INVOKE(vlan_unconfig, lp->lp_ifp, vtag);
 503         LAGG_XUNLOCK(sc);
 504 }
 505
 506 static int
 507 lagg_clone_create(struct if_clone *ifc, char *name, size_t len,
 508     struct ifc_data *ifd, struct ifnet **ifpp)
 509 {
 510         struct iflaggparam iflp;
 511         struct lagg_softc *sc;
 512         struct ifnet *ifp;
 513         int if_type;
 514         int error;
 515         static const uint8_t eaddr[LAGG_ADDR_LEN];
 516
 517         if (ifd->params != NULL) {
 518                 error = ifc_copyin(ifd, &iflp, sizeof(iflp));
 519                 if (error)
 520                         return (error);
 521
 522                 switch (iflp.lagg_type) {
 523                 case LAGG_TYPE_ETHERNET:
 524                         if_type = IFT_ETHER;
 525                         break;
 526                 case LAGG_TYPE_INFINIBAND:
 527                         if_type = IFT_INFINIBAND;
 528                         break;
 529                 default:
 530                         return (EINVAL);
 531                 }
 532         } else {
 533                 if_type = IFT_ETHER;
 534         }
 535
 536         sc = malloc(sizeof(*sc), M_LAGG, M_WAITOK | M_ZERO);
 537         ifp = sc->sc_ifp = if_alloc(if_type);
 538         if (ifp == NULL) {
 539                 free(sc, M_LAGG);
 540                 return (ENOSPC);
 541         }
 542         LAGG_SX_INIT(sc);
 543
 544         mtx_init(&sc->sc_mtx, "lagg-mtx", NULL, MTX_DEF);
 545         callout_init_mtx(&sc->sc_watchdog, &sc->sc_mtx, 0);
 546
 547         LAGG_XLOCK(sc);
 548         if (V_def_use_flowid)
 549                 sc->sc_opts |= LAGG_OPT_USE_FLOWID;
 550         if (V_def_use_numa)
 551                 sc->sc_opts |= LAGG_OPT_USE_NUMA;
 552         sc->flowid_shift = V_def_flowid_shift;
 553
 554         /* Hash all layers by default */
 555         sc->sc_flags = MBUF_HASHFLAG_L2 | MBUF_HASHFLAG_L3 | MBUF_HASHFLAG_L4;
 556
 557         lagg_proto_attach(sc, LAGG_PROTO_DEFAULT);
 558
 559         CK_SLIST_INIT(&sc->sc_ports);
 560
 561         switch (if_type) {
 562         case IFT_ETHER:
 563                 /* Initialise pseudo media types */
 564                 ifmedia_init(&sc->sc_media, 0, lagg_media_change,
 565                     lagg_media_status);
 566                 ifmedia_add(&sc->sc_media, IFM_ETHER | IFM_AUTO, 0, NULL);
 567                 ifmedia_set(&sc->sc_media, IFM_ETHER | IFM_AUTO);
 568
 569                 if_initname(ifp, laggname, ifd->unit);
 570                 ifp->if_transmit = lagg_transmit_ethernet;
 571                 break;
 572         case IFT_INFINIBAND:
 573                 if_initname(ifp, laggname, ifd->unit);
 574                 ifp->if_transmit = lagg_transmit_infiniband;
 575                 break;
 576         default:
 577                 break;
 578         }
 579         ifp->if_softc = sc;
 580         ifp->if_qflush = lagg_qflush;
 581         ifp->if_init = lagg_init;
 582         ifp->if_ioctl = lagg_ioctl;
 583         ifp->if_get_counter = lagg_get_counter;
 584         ifp->if_flags = IFF_SIMPLEX | IFF_BROADCAST | IFF_MULTICAST;
 585 #if defined(KERN_TLS) || defined(RATELIMIT)
 586         ifp->if_snd_tag_alloc = lagg_snd_tag_alloc;
 587         ifp->if_ratelimit_query = lagg_ratelimit_query;
 588 #endif
 589         ifp->if_capenable = ifp->if_capabilities = IFCAP_HWSTATS;
 590
 591         /*
 592          * Attach as an ordinary ethernet device, children will be attached
 593          * as special device IFT_IEEE8023ADLAG or IFT_INFINIBANDLAG.
 594          */
 595         switch (if_type) {
 596         case IFT_ETHER:
 597                 ether_ifattach(ifp, eaddr);
 598                 break;
 599         case IFT_INFINIBAND:
 600                 infiniband_ifattach(ifp, eaddr, sc->sc_bcast_addr);
 601                 break;
 602         default:
 603                 break;
 604         }
 605
 606         sc->vlan_attach = EVENTHANDLER_REGISTER(vlan_config,
 607                 lagg_register_vlan, sc, EVENTHANDLER_PRI_FIRST);
 608         sc->vlan_detach = EVENTHANDLER_REGISTER(vlan_unconfig,
 609                 lagg_unregister_vlan, sc, EVENTHANDLER_PRI_FIRST);
 610
 611         /* Insert into the global list of laggs */
 612         LAGG_LIST_LOCK();
 613         SLIST_INSERT_HEAD(&V_lagg_list, sc, sc_entries);
 614         LAGG_LIST_UNLOCK();
 615         LAGG_XUNLOCK(sc);
 616         *ifpp = ifp;
 617
 618         return (0);
 619 }
 620
 621 static int
 622 lagg_clone_destroy(struct if_clone *ifc, struct ifnet *ifp, uint32_t flags)
 623 {
 624         struct lagg_softc *sc = (struct lagg_softc *)ifp->if_softc;
 625         struct lagg_port *lp;
 626
 627         LAGG_XLOCK(sc);
 628         sc->sc_destroying = 1;
 629         lagg_stop(sc);
 630         ifp->if_flags &= ~IFF_UP;
 631
 632         EVENTHANDLER_DEREGISTER(vlan_config, sc->vlan_attach);
 633         EVENTHANDLER_DEREGISTER(vlan_unconfig, sc->vlan_detach);
 634
 635         /* Shutdown and remove lagg ports */
 636         while ((lp = CK_SLIST_FIRST(&sc->sc_ports)) != NULL)
 637                 lagg_port_destroy(lp, 1);
 638
 639         /* Unhook the aggregation protocol */
 640         lagg_proto_detach(sc);
 641         LAGG_XUNLOCK(sc);
 642
 643         switch (ifp->if_type) {
 644         case IFT_ETHER:
 645                 ifmedia_removeall(&sc->sc_media);
 646                 ether_ifdetach(ifp);
 647                 break;
 648         case IFT_INFINIBAND:
 649                 infiniband_ifdetach(ifp);
 650                 break;
 651         default:
 652                 break;
 653         }
 654         if_free(ifp);
 655
 656         LAGG_LIST_LOCK();
 657         SLIST_REMOVE(&V_lagg_list, sc, lagg_softc, sc_entries);
 658         LAGG_LIST_UNLOCK();
 659
 660         mtx_destroy(&sc->sc_mtx);
 661         LAGG_SX_DESTROY(sc);
 662         free(sc, M_LAGG);
 663
 664         return (0);
 665 }
 666
 667 static void
 668 lagg_capabilities(struct lagg_softc *sc)
 669 {
 670         struct lagg_port *lp;
 671         int cap, cap2, ena, ena2, pena, pena2;
 672         uint64_t hwa;
 673         struct ifnet_hw_tsomax hw_tsomax;
 674
 675         LAGG_XLOCK_ASSERT(sc);
 676
 677         /* Get common enabled capabilities for the lagg ports */
 678         ena = ena2 = ~0;
 679         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
 680                 ena &= lp->lp_ifp->if_capenable;
 681                 ena2 &= lp->lp_ifp->if_capenable2;
 682         }
 683         if (CK_SLIST_FIRST(&sc->sc_ports) == NULL)
 684                 ena = ena2 = 0;
 685
 686         /*
 687          * Apply common enabled capabilities back to the lagg ports.
 688          * May require several iterations if they are dependent.
 689          */
 690         do {
 691                 pena = ena;
 692                 pena2 = ena2;
 693                 CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
 694                         lagg_setcaps(lp, ena, ena2);
 695                         ena &= lp->lp_ifp->if_capenable;
 696                         ena2 &= lp->lp_ifp->if_capenable2;
 697                 }
 698         } while (pena != ena || pena2 != ena2);
 699
 700         /* Get other capabilities from the lagg ports */
 701         cap = cap2 = ~0;
 702         hwa = ~(uint64_t)0;
 703         memset(&hw_tsomax, 0, sizeof(hw_tsomax));
 704         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
 705                 cap &= lp->lp_ifp->if_capabilities;
 706                 cap2 &= lp->lp_ifp->if_capabilities2;
 707                 hwa &= lp->lp_ifp->if_hwassist;
 708                 if_hw_tsomax_common(lp->lp_ifp, &hw_tsomax);
 709         }
 710         if (CK_SLIST_FIRST(&sc->sc_ports) == NULL)
 711                 cap = cap2 = hwa = 0;
 712
 713         if (sc->sc_ifp->if_capabilities != cap ||
 714             sc->sc_ifp->if_capenable != ena ||
 715             sc->sc_ifp->if_capenable2 != ena2 ||
 716             sc->sc_ifp->if_hwassist != hwa ||
 717             if_hw_tsomax_update(sc->sc_ifp, &hw_tsomax) != 0) {
 718                 sc->sc_ifp->if_capabilities = cap;
 719                 sc->sc_ifp->if_capabilities2 = cap2;
 720                 sc->sc_ifp->if_capenable = ena;
 721                 sc->sc_ifp->if_capenable2 = ena2;
 722                 sc->sc_ifp->if_hwassist = hwa;
 723                 getmicrotime(&sc->sc_ifp->if_lastchange);
 724
 725                 if (sc->sc_ifflags & IFF_DEBUG)
 726                         if_printf(sc->sc_ifp,
 727                             "capabilities 0x%08x enabled 0x%08x\n", cap, ena);
 728         }
 729 }
 730
 731 static int
 732 lagg_port_create(struct lagg_softc *sc, struct ifnet *ifp)
 733 {
 734         struct lagg_softc *sc_ptr;
 735         struct lagg_port *lp, *tlp;
 736         struct ifreq ifr;
 737         int error, i, oldmtu;
 738         int if_type;
 739         uint64_t *pval;
 740
 741         LAGG_XLOCK_ASSERT(sc);
 742
 743         if (sc->sc_ifp == ifp) {
 744                 if_printf(sc->sc_ifp,
 745                     "cannot add a lagg to itself as a port\n");
 746                 return (EINVAL);
 747         }
 748
 749         if (sc->sc_destroying == 1)
 750                 return (ENXIO);
 751
 752         /* Limit the maximal number of lagg ports */
 753         if (sc->sc_count >= LAGG_MAX_PORTS)
 754                 return (ENOSPC);
 755
 756         /* Check if port has already been associated to a lagg */
 757         if (ifp->if_lagg != NULL) {
 758                 /* Port is already in the current lagg? */
 759                 lp = (struct lagg_port *)ifp->if_lagg;
 760                 if (lp->lp_softc == sc)
 761                         return (EEXIST);
 762                 return (EBUSY);
 763         }
 764
 765         switch (sc->sc_ifp->if_type) {
 766         case IFT_ETHER:
 767                 /* XXX Disallow non-ethernet interfaces (this should be any of 802) */
 768                 if (ifp->if_type != IFT_ETHER && ifp->if_type != IFT_L2VLAN)
 769                         return (EPROTONOSUPPORT);
 770                 if_type = IFT_IEEE8023ADLAG;
 771                 break;
 772         case IFT_INFINIBAND:
 773                 /* XXX Disallow non-infiniband interfaces */
 774                 if (ifp->if_type != IFT_INFINIBAND)
 775                         return (EPROTONOSUPPORT);
 776                 if_type = IFT_INFINIBANDLAG;
 777                 break;
 778         default:
 779                 break;
 780         }
 781
 782         /* Allow the first Ethernet member to define the MTU */
 783         oldmtu = -1;
 784         if (CK_SLIST_EMPTY(&sc->sc_ports)) {
 785                 sc->sc_ifp->if_mtu = ifp->if_mtu;
 786         } else if (sc->sc_ifp->if_mtu != ifp->if_mtu) {
 787                 if (ifp->if_ioctl == NULL) {
 788                         if_printf(sc->sc_ifp, "cannot change MTU for %s\n",
 789                             ifp->if_xname);
 790                         return (EINVAL);
 791                 }
 792                 oldmtu = ifp->if_mtu;
 793                 strlcpy(ifr.ifr_name, ifp->if_xname, sizeof(ifr.ifr_name));
 794                 ifr.ifr_mtu = sc->sc_ifp->if_mtu;
 795                 error = (*ifp->if_ioctl)(ifp, SIOCSIFMTU, (caddr_t)&ifr);
 796                 if (error != 0) {
 797                         if_printf(sc->sc_ifp, "invalid MTU for %s\n",
 798                             ifp->if_xname);
 799                         return (error);
 800                 }
 801                 ifr.ifr_mtu = oldmtu;
 802         }
 803
 804         lp = malloc(sizeof(struct lagg_port), M_LAGG, M_WAITOK | M_ZERO);
 805         lp->lp_softc = sc;
 806
 807         /* Check if port is a stacked lagg */
 808         LAGG_LIST_LOCK();
 809         SLIST_FOREACH(sc_ptr, &V_lagg_list, sc_entries) {
 810                 if (ifp == sc_ptr->sc_ifp) {
 811                         LAGG_LIST_UNLOCK();
 812                         free(lp, M_LAGG);
 813                         if (oldmtu != -1)
 814                                 (*ifp->if_ioctl)(ifp, SIOCSIFMTU,
 815                                     (caddr_t)&ifr);
 816                         return (EINVAL);
 817                         /* XXX disable stacking for the moment, its untested */
 818 #ifdef LAGG_PORT_STACKING
 819                         lp->lp_flags |= LAGG_PORT_STACK;
 820                         if (lagg_port_checkstacking(sc_ptr) >=
 821                             LAGG_MAX_STACKING) {
 822                                 LAGG_LIST_UNLOCK();
 823                                 free(lp, M_LAGG);
 824                                 if (oldmtu != -1)
 825                                         (*ifp->if_ioctl)(ifp, SIOCSIFMTU,
 826                                             (caddr_t)&ifr);
 827                                 return (E2BIG);
 828                         }
 829 #endif
 830                 }
 831         }
 832         LAGG_LIST_UNLOCK();
 833
 834         if_ref(ifp);
 835         lp->lp_ifp = ifp;
 836
 837         bcopy(IF_LLADDR(ifp), lp->lp_lladdr, ifp->if_addrlen);
 838         lp->lp_ifcapenable = ifp->if_capenable;
 839         if (CK_SLIST_EMPTY(&sc->sc_ports)) {
 840                 bcopy(IF_LLADDR(ifp), IF_LLADDR(sc->sc_ifp), ifp->if_addrlen);
 841                 lagg_proto_lladdr(sc);
 842                 EVENTHANDLER_INVOKE(iflladdr_event, sc->sc_ifp);
 843         } else {
 844                 if_setlladdr(ifp, IF_LLADDR(sc->sc_ifp), ifp->if_addrlen);
 845         }
 846         lagg_setflags(lp, 1);
 847
 848         if (CK_SLIST_EMPTY(&sc->sc_ports))
 849                 sc->sc_primary = lp;
 850
 851         /* Change the interface type */
 852         lp->lp_iftype = ifp->if_type;
 853         ifp->if_type = if_type;
 854         ifp->if_lagg = lp;
 855         lp->lp_ioctl = ifp->if_ioctl;
 856         ifp->if_ioctl = lagg_port_ioctl;
 857         lp->lp_output = ifp->if_output;
 858         ifp->if_output = lagg_port_output;
 859
 860         /* Read port counters */
 861         pval = lp->port_counters.val;
 862         for (i = 0; i < IFCOUNTERS; i++, pval++)
 863                 *pval = ifp->if_get_counter(ifp, i);
 864
 865         /*
 866          * Insert into the list of ports.
 867          * Keep ports sorted by if_index. It is handy, when configuration
 868          * is predictable and `ifconfig laggN create ...` command
 869          * will lead to the same result each time.
 870          */
 871         CK_SLIST_FOREACH(tlp, &sc->sc_ports, lp_entries) {
 872                 if (tlp->lp_ifp->if_index < ifp->if_index && (
 873                     CK_SLIST_NEXT(tlp, lp_entries) == NULL ||
 874                     ((struct lagg_port*)CK_SLIST_NEXT(tlp, lp_entries))->lp_ifp->if_index >
 875                     ifp->if_index))
 876                         break;
 877         }
 878         if (tlp != NULL)
 879                 CK_SLIST_INSERT_AFTER(tlp, lp, lp_entries);
 880         else
 881                 CK_SLIST_INSERT_HEAD(&sc->sc_ports, lp, lp_entries);
 882         sc->sc_count++;
 883
 884         lagg_setmulti(lp);
 885
 886         if ((error = lagg_proto_addport(sc, lp)) != 0) {
 887                 /* Remove the port, without calling pr_delport. */
 888                 lagg_port_destroy(lp, 0);
 889                 if (oldmtu != -1)
 890                         (*ifp->if_ioctl)(ifp, SIOCSIFMTU, (caddr_t)&ifr);
 891                 return (error);
 892         }
 893
 894         /* Update lagg capabilities */
 895         lagg_capabilities(sc);
 896         lagg_linkstate(sc);
 897
 898         return (0);
 899 }
 900
 901 #ifdef LAGG_PORT_STACKING
 902 static int
 903 lagg_port_checkstacking(struct lagg_softc *sc)
 904 {
 905         struct lagg_softc *sc_ptr;
 906         struct lagg_port *lp;
 907         int m = 0;
 908
 909         LAGG_SXLOCK_ASSERT(sc);
 910         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
 911                 if (lp->lp_flags & LAGG_PORT_STACK) {
 912                         sc_ptr = (struct lagg_softc *)lp->lp_ifp->if_softc;
 913                         m = MAX(m, lagg_port_checkstacking(sc_ptr));
 914                 }
 915         }
 916
 917         return (m + 1);
 918 }
 919 #endif
 920
 921 static void
 922 lagg_port_destroy_cb(epoch_context_t ec)
 923 {
 924         struct lagg_port *lp;
 925         struct ifnet *ifp;
 926
 927         lp = __containerof(ec, struct lagg_port, lp_epoch_ctx);
 928         ifp = lp->lp_ifp;
 929
 930         if_rele(ifp);
 931         free(lp, M_LAGG);
 932 }
 933
 934 static int
 935 lagg_port_destroy(struct lagg_port *lp, int rundelport)
 936 {
 937         struct lagg_softc *sc = lp->lp_softc;
 938         struct lagg_port *lp_ptr, *lp0;
 939         struct ifnet *ifp = lp->lp_ifp;
 940         uint64_t *pval, vdiff;
 941         int i;
 942
 943         LAGG_XLOCK_ASSERT(sc);
 944
 945         if (rundelport)
 946                 lagg_proto_delport(sc, lp);
 947
 948         if (lp->lp_detaching == 0)
 949                 lagg_clrmulti(lp);
 950
 951         /* Restore interface */
 952         ifp->if_type = lp->lp_iftype;
 953         ifp->if_ioctl = lp->lp_ioctl;
 954         ifp->if_output = lp->lp_output;
 955         ifp->if_lagg = NULL;
 956
 957         /* Update detached port counters */
 958         pval = lp->port_counters.val;
 959         for (i = 0; i < IFCOUNTERS; i++, pval++) {
 960                 vdiff = ifp->if_get_counter(ifp, i) - *pval;
 961                 sc->detached_counters.val[i] += vdiff;
 962         }
 963
 964         /* Finally, remove the port from the lagg */
 965         CK_SLIST_REMOVE(&sc->sc_ports, lp, lagg_port, lp_entries);
 966         sc->sc_count--;
 967
 968         /* Update the primary interface */
 969         if (lp == sc->sc_primary) {
 970                 uint8_t lladdr[LAGG_ADDR_LEN];
 971
 972                 if ((lp0 = CK_SLIST_FIRST(&sc->sc_ports)) == NULL)
 973                         bzero(&lladdr, LAGG_ADDR_LEN);
 974                 else
 975                         bcopy(lp0->lp_lladdr, lladdr, LAGG_ADDR_LEN);
 976                 sc->sc_primary = lp0;
 977                 if (sc->sc_destroying == 0) {
 978                         bcopy(lladdr, IF_LLADDR(sc->sc_ifp), sc->sc_ifp->if_addrlen);
 979                         lagg_proto_lladdr(sc);
 980                         EVENTHANDLER_INVOKE(iflladdr_event, sc->sc_ifp);
 981
 982                         /*
 983                          * Update lladdr for each port (new primary needs update
 984                          * as well, to switch from old lladdr to its 'real' one).
 985                          * We can skip this if the lagg is being destroyed.
 986                          */
 987                         CK_SLIST_FOREACH(lp_ptr, &sc->sc_ports, lp_entries)
 988                                 if_setlladdr(lp_ptr->lp_ifp, lladdr,
 989                                     lp_ptr->lp_ifp->if_addrlen);
 990                 }
 991         }
 992
 993         if (lp->lp_ifflags)
 994                 if_printf(ifp, "%s: lp_ifflags unclean\n", __func__);
 995
 996         if (lp->lp_detaching == 0) {
 997                 lagg_setflags(lp, 0);
 998                 lagg_setcaps(lp, lp->lp_ifcapenable, lp->lp_ifcapenable2);
 999                 if_setlladdr(ifp, lp->lp_lladdr, ifp->if_addrlen);
1000         }
1001
1002         /*
1003          * free port and release it's ifnet reference after a grace period has
1004          * elapsed.
1005          */
1006         NET_EPOCH_CALL(lagg_port_destroy_cb, &lp->lp_epoch_ctx);
1007         /* Update lagg capabilities */
1008         lagg_capabilities(sc);
1009         lagg_linkstate(sc);
1010
1011         return (0);
1012 }
1013
1014 static int
1015 lagg_port_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
1016 {
1017         struct epoch_tracker et;
1018         struct lagg_reqport *rp = (struct lagg_reqport *)data;
1019         struct lagg_softc *sc;
1020         struct lagg_port *lp = NULL;
1021         int error = 0;
1022
1023         /* Should be checked by the caller */
1024         switch (ifp->if_type) {
1025         case IFT_IEEE8023ADLAG:
1026         case IFT_INFINIBANDLAG:
1027                 if ((lp = ifp->if_lagg) == NULL || (sc = lp->lp_softc) == NULL)
1028                         goto fallback;
1029                 break;
1030         default:
1031                 goto fallback;
1032         }
1033
1034         switch (cmd) {
1035         case SIOCGLAGGPORT:
1036                 if (rp->rp_portname[0] == '\0' ||
1037                     ifunit(rp->rp_portname) != ifp) {
1038                         error = EINVAL;
1039                         break;
1040                 }
1041
1042                 NET_EPOCH_ENTER(et);
1043                 if ((lp = ifp->if_lagg) == NULL || lp->lp_softc != sc) {
1044                         error = ENOENT;
1045                         NET_EPOCH_EXIT(et);
1046                         break;
1047                 }
1048
1049                 lagg_port2req(lp, rp);
1050                 NET_EPOCH_EXIT(et);
1051                 break;
1052
1053         case SIOCSIFCAP:
1054         case SIOCSIFCAPNV:
1055                 if (lp->lp_ioctl == NULL) {
1056                         error = EINVAL;
1057                         break;
1058                 }
1059                 error = (*lp->lp_ioctl)(ifp, cmd, data);
1060                 if (error)
1061                         break;
1062
1063                 /* Update lagg interface capabilities */
1064                 LAGG_XLOCK(sc);
1065                 lagg_capabilities(sc);
1066                 LAGG_XUNLOCK(sc);
1067                 VLAN_CAPABILITIES(sc->sc_ifp);
1068                 break;
1069
1070         case SIOCSIFMTU:
1071                 /* Do not allow the MTU to be changed once joined */
1072                 error = EINVAL;
1073                 break;
1074
1075         default:
1076                 goto fallback;
1077         }
1078
1079         return (error);
1080
1081 fallback:
1082         if (lp != NULL && lp->lp_ioctl != NULL)
1083                 return ((*lp->lp_ioctl)(ifp, cmd, data));
1084
1085         return (EINVAL);
1086 }
1087
1088 /*
1089  * Requests counter @cnt data.
1090  *
1091  * Counter value is calculated the following way:
1092  * 1) for each port, sum difference between current and "initial" measurements.
1093  * 2) add lagg logical interface counters.
1094  * 3) add data from detached_counters array.
1095  *
1096  * We also do the following things on ports attach/detach:
1097  * 1) On port attach we store all counters it has into port_counter array.
1098  * 2) On port detach we add the different between "initial" and
1099  *   current counters data to detached_counters array.
1100  */
1101 static uint64_t
1102 lagg_get_counter(struct ifnet *ifp, ift_counter cnt)
1103 {
1104         struct epoch_tracker et;
1105         struct lagg_softc *sc;
1106         struct lagg_port *lp;
1107         struct ifnet *lpifp;
1108         uint64_t newval, oldval, vsum;
1109
1110         /* Revise this when we've got non-generic counters. */
1111         KASSERT(cnt < IFCOUNTERS, ("%s: invalid cnt %d", __func__, cnt));
1112
1113         sc = (struct lagg_softc *)ifp->if_softc;
1114
1115         vsum = 0;
1116         NET_EPOCH_ENTER(et);
1117         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
1118                 /* Saved attached value */
1119                 oldval = lp->port_counters.val[cnt];
1120                 /* current value */
1121                 lpifp = lp->lp_ifp;
1122                 newval = lpifp->if_get_counter(lpifp, cnt);
1123                 /* Calculate diff and save new */
1124                 vsum += newval - oldval;
1125         }
1126         NET_EPOCH_EXIT(et);
1127
1128         /*
1129          * Add counter data which might be added by upper
1130          * layer protocols operating on logical interface.
1131          */
1132         vsum += if_get_counter_default(ifp, cnt);
1133
1134         /*
1135          * Add counter data from detached ports counters
1136          */
1137         vsum += sc->detached_counters.val[cnt];
1138
1139         return (vsum);
1140 }
1141
1142 /*
1143  * For direct output to child ports.
1144  */
1145 static int
1146 lagg_port_output(struct ifnet *ifp, struct mbuf *m,
1147         const struct sockaddr *dst, struct route *ro)
1148 {
1149         struct lagg_port *lp = ifp->if_lagg;
1150
1151         switch (dst->sa_family) {
1152                 case pseudo_AF_HDRCMPLT:
1153                 case AF_UNSPEC:
1154                         if (lp != NULL)
1155                                 return ((*lp->lp_output)(ifp, m, dst, ro));
1156         }
1157
1158         /* drop any other frames */
1159         m_freem(m);
1160         return (ENETDOWN);
1161 }
1162
1163 static void
1164 lagg_port_ifdetach(void *arg __unused, struct ifnet *ifp)
1165 {
1166         struct lagg_port *lp;
1167         struct lagg_softc *sc;
1168
1169         if ((lp = ifp->if_lagg) == NULL)
1170                 return;
1171         /* If the ifnet is just being renamed, don't do anything. */
1172         if (ifp->if_flags & IFF_RENAMING)
1173                 return;
1174
1175         sc = lp->lp_softc;
1176
1177         LAGG_XLOCK(sc);
1178         lp->lp_detaching = 1;
1179         lagg_port_destroy(lp, 1);
1180         LAGG_XUNLOCK(sc);
1181         VLAN_CAPABILITIES(sc->sc_ifp);
1182 }
1183
1184 static void
1185 lagg_port2req(struct lagg_port *lp, struct lagg_reqport *rp)
1186 {
1187         struct lagg_softc *sc = lp->lp_softc;
1188
1189         strlcpy(rp->rp_ifname, sc->sc_ifname, sizeof(rp->rp_ifname));
1190         strlcpy(rp->rp_portname, lp->lp_ifp->if_xname, sizeof(rp->rp_portname));
1191         rp->rp_prio = lp->lp_prio;
1192         rp->rp_flags = lp->lp_flags;
1193         lagg_proto_portreq(sc, lp, &rp->rp_psc);
1194
1195         /* Add protocol specific flags */
1196         switch (sc->sc_proto) {
1197                 case LAGG_PROTO_FAILOVER:
1198                         if (lp == sc->sc_primary)
1199                                 rp->rp_flags |= LAGG_PORT_MASTER;
1200                         if (lp == lagg_link_active(sc, sc->sc_primary))
1201                                 rp->rp_flags |= LAGG_PORT_ACTIVE;
1202                         break;
1203
1204                 case LAGG_PROTO_ROUNDROBIN:
1205                 case LAGG_PROTO_LOADBALANCE:
1206                 case LAGG_PROTO_BROADCAST:
1207                         if (LAGG_PORTACTIVE(lp))
1208                                 rp->rp_flags |= LAGG_PORT_ACTIVE;
1209                         break;
1210
1211                 case LAGG_PROTO_LACP:
1212                         /* LACP has a different definition of active */
1213                         if (lacp_isactive(lp))
1214                                 rp->rp_flags |= LAGG_PORT_ACTIVE;
1215                         if (lacp_iscollecting(lp))
1216                                 rp->rp_flags |= LAGG_PORT_COLLECTING;
1217                         if (lacp_isdistributing(lp))
1218                                 rp->rp_flags |= LAGG_PORT_DISTRIBUTING;
1219                         break;
1220         }
1221
1222 }
1223
1224 static void
1225 lagg_watchdog_infiniband(void *arg)
1226 {
1227         struct epoch_tracker et;
1228         struct lagg_softc *sc;
1229         struct lagg_port *lp;
1230         struct ifnet *ifp;
1231         struct ifnet *lp_ifp;
1232
1233         sc = arg;
1234
1235         /*
1236          * Because infiniband nodes have a fixed MAC address, which is
1237          * generated by the so-called GID, we need to regularly update
1238          * the link level address of the parent lagg<N> device when
1239          * the active port changes. Possibly we could piggy-back on
1240          * link up/down events aswell, but using a timer also provides
1241          * a guarantee against too frequent events. This operation
1242          * does not have to be atomic.
1243          */
1244         NET_EPOCH_ENTER(et);
1245         lp = lagg_link_active(sc, sc->sc_primary);
1246         if (lp != NULL) {
1247                 ifp = sc->sc_ifp;
1248                 lp_ifp = lp->lp_ifp;
1249
1250                 if (ifp != NULL && lp_ifp != NULL &&
1251                     (memcmp(IF_LLADDR(ifp), IF_LLADDR(lp_ifp), ifp->if_addrlen) != 0 ||
1252                      memcmp(sc->sc_bcast_addr, lp_ifp->if_broadcastaddr, ifp->if_addrlen) != 0)) {
1253                         memcpy(IF_LLADDR(ifp), IF_LLADDR(lp_ifp), ifp->if_addrlen);
1254                         memcpy(sc->sc_bcast_addr, lp_ifp->if_broadcastaddr, ifp->if_addrlen);
1255
1256                         CURVNET_SET(ifp->if_vnet);
1257                         EVENTHANDLER_INVOKE(iflladdr_event, ifp);
1258                         CURVNET_RESTORE();
1259                 }
1260         }
1261         NET_EPOCH_EXIT(et);
1262
1263         callout_reset(&sc->sc_watchdog, hz, &lagg_watchdog_infiniband, arg);
1264 }
1265
1266 static void
1267 lagg_init(void *xsc)
1268 {
1269         struct lagg_softc *sc = (struct lagg_softc *)xsc;
1270         struct ifnet *ifp = sc->sc_ifp;
1271         struct lagg_port *lp;
1272
1273         LAGG_XLOCK(sc);
1274         if (ifp->if_drv_flags & IFF_DRV_RUNNING) {
1275                 LAGG_XUNLOCK(sc);
1276                 return;
1277         }
1278
1279         ifp->if_drv_flags |= IFF_DRV_RUNNING;
1280
1281         /*
1282          * Update the port lladdrs if needed.
1283          * This might be if_setlladdr() notification
1284          * that lladdr has been changed.
1285          */
1286         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
1287                 if (memcmp(IF_LLADDR(ifp), IF_LLADDR(lp->lp_ifp),
1288                     ifp->if_addrlen) != 0)
1289                         if_setlladdr(lp->lp_ifp, IF_LLADDR(ifp), ifp->if_addrlen);
1290         }
1291
1292         lagg_proto_init(sc);
1293
1294         if (ifp->if_type == IFT_INFINIBAND) {
1295                 mtx_lock(&sc->sc_mtx);
1296                 lagg_watchdog_infiniband(sc);
1297                 mtx_unlock(&sc->sc_mtx);
1298         }
1299
1300         LAGG_XUNLOCK(sc);
1301 }
1302
1303 static void
1304 lagg_stop(struct lagg_softc *sc)
1305 {
1306         struct ifnet *ifp = sc->sc_ifp;
1307
1308         LAGG_XLOCK_ASSERT(sc);
1309
1310         if ((ifp->if_drv_flags & IFF_DRV_RUNNING) == 0)
1311                 return;
1312
1313         ifp->if_drv_flags &= ~IFF_DRV_RUNNING;
1314
1315         lagg_proto_stop(sc);
1316
1317         mtx_lock(&sc->sc_mtx);
1318         callout_stop(&sc->sc_watchdog);
1319         mtx_unlock(&sc->sc_mtx);
1320
1321         callout_drain(&sc->sc_watchdog);
1322 }
1323
1324 static int
1325 lagg_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
1326 {
1327         struct epoch_tracker et;
1328         struct lagg_softc *sc = (struct lagg_softc *)ifp->if_softc;
1329         struct lagg_reqall *ra = (struct lagg_reqall *)data;
1330         struct lagg_reqopts *ro = (struct lagg_reqopts *)data;
1331         struct lagg_reqport *rp = (struct lagg_reqport *)data, rpbuf;
1332         struct lagg_reqflags *rf = (struct lagg_reqflags *)data;
1333         struct ifreq *ifr = (struct ifreq *)data;
1334         struct lagg_port *lp;
1335         struct ifnet *tpif;
1336         struct thread *td = curthread;
1337         char *buf, *outbuf;
1338         int count, buflen, len, error = 0, oldmtu;
1339
1340         bzero(&rpbuf, sizeof(rpbuf));
1341
1342         /* XXX: This can race with lagg_clone_destroy. */
1343
1344         switch (cmd) {
1345         case SIOCGLAGG:
1346                 LAGG_XLOCK(sc);
1347                 buflen = sc->sc_count * sizeof(struct lagg_reqport);
1348                 outbuf = malloc(buflen, M_TEMP, M_WAITOK | M_ZERO);
1349                 ra->ra_proto = sc->sc_proto;
1350                 lagg_proto_request(sc, &ra->ra_psc);
1351                 count = 0;
1352                 buf = outbuf;
1353                 len = min(ra->ra_size, buflen);
1354                 CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
1355                         if (len < sizeof(rpbuf))
1356                                 break;
1357
1358                         lagg_port2req(lp, &rpbuf);
1359                         memcpy(buf, &rpbuf, sizeof(rpbuf));
1360                         count++;
1361                         buf += sizeof(rpbuf);
1362                         len -= sizeof(rpbuf);
1363                 }
1364                 LAGG_XUNLOCK(sc);
1365                 ra->ra_ports = count;
1366                 ra->ra_size = count * sizeof(rpbuf);
1367                 error = copyout(outbuf, ra->ra_port, ra->ra_size);
1368                 free(outbuf, M_TEMP);
1369                 break;
1370         case SIOCSLAGG:
1371                 error = priv_check(td, PRIV_NET_LAGG);
1372                 if (error)
1373                         break;
1374                 if (ra->ra_proto >= LAGG_PROTO_MAX) {
1375                         error = EPROTONOSUPPORT;
1376                         break;
1377                 }
1378                 /* Infiniband only supports the failover protocol. */
1379                 if (ra->ra_proto != LAGG_PROTO_FAILOVER &&
1380                     ifp->if_type == IFT_INFINIBAND) {
1381                         error = EPROTONOSUPPORT;
1382                         break;
1383                 }
1384                 LAGG_XLOCK(sc);
1385                 lagg_proto_detach(sc);
1386                 lagg_proto_attach(sc, ra->ra_proto);
1387                 LAGG_XUNLOCK(sc);
1388                 break;
1389         case SIOCGLAGGOPTS:
1390                 LAGG_XLOCK(sc);
1391                 ro->ro_opts = sc->sc_opts;
1392                 if (sc->sc_proto == LAGG_PROTO_LACP) {
1393                         struct lacp_softc *lsc;
1394
1395                         lsc = (struct lacp_softc *)sc->sc_psc;
1396                         if (lsc->lsc_debug.lsc_tx_test != 0)
1397                                 ro->ro_opts |= LAGG_OPT_LACP_TXTEST;
1398                         if (lsc->lsc_debug.lsc_rx_test != 0)
1399                                 ro->ro_opts |= LAGG_OPT_LACP_RXTEST;
1400                         if (lsc->lsc_strict_mode != 0)
1401                                 ro->ro_opts |= LAGG_OPT_LACP_STRICT;
1402                         if (lsc->lsc_fast_timeout != 0)
1403                                 ro->ro_opts |= LAGG_OPT_LACP_FAST_TIMO;
1404
1405                         ro->ro_active = sc->sc_active;
1406                 } else {
1407                         ro->ro_active = 0;
1408                         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries)
1409                                 ro->ro_active += LAGG_PORTACTIVE(lp);
1410                 }
1411                 ro->ro_bkt = sc->sc_stride;
1412                 ro->ro_flapping = sc->sc_flapping;
1413                 ro->ro_flowid_shift = sc->flowid_shift;
1414                 LAGG_XUNLOCK(sc);
1415                 break;
1416         case SIOCSLAGGOPTS:
1417                 error = priv_check(td, PRIV_NET_LAGG);
1418                 if (error)
1419                         break;
1420
1421                 /*
1422                  * The stride option was added without defining a corresponding
1423                  * LAGG_OPT flag, so handle a non-zero value before checking
1424                  * anything else to preserve compatibility.
1425                  */
1426                 LAGG_XLOCK(sc);
1427                 if (ro->ro_opts == 0 && ro->ro_bkt != 0) {
1428                         if (sc->sc_proto != LAGG_PROTO_ROUNDROBIN) {
1429                                 LAGG_XUNLOCK(sc);
1430                                 error = EINVAL;
1431                                 break;
1432                         }
1433                         sc->sc_stride = ro->ro_bkt;
1434                 }
1435                 if (ro->ro_opts == 0) {
1436                         LAGG_XUNLOCK(sc);
1437                         break;
1438                 }
1439
1440                 /*
1441                  * Set options.  LACP options are stored in sc->sc_psc,
1442                  * not in sc_opts.
1443                  */
1444                 int valid, lacp;
1445
1446                 switch (ro->ro_opts) {
1447                 case LAGG_OPT_USE_FLOWID:
1448                 case -LAGG_OPT_USE_FLOWID:
1449                 case LAGG_OPT_USE_NUMA:
1450                 case -LAGG_OPT_USE_NUMA:
1451                 case LAGG_OPT_FLOWIDSHIFT:
1452                 case LAGG_OPT_RR_LIMIT:
1453                         valid = 1;
1454                         lacp = 0;
1455                         break;
1456                 case LAGG_OPT_LACP_TXTEST:
1457                 case -LAGG_OPT_LACP_TXTEST:
1458                 case LAGG_OPT_LACP_RXTEST:
1459                 case -LAGG_OPT_LACP_RXTEST:
1460                 case LAGG_OPT_LACP_STRICT:
1461                 case -LAGG_OPT_LACP_STRICT:
1462                 case LAGG_OPT_LACP_FAST_TIMO:
1463                 case -LAGG_OPT_LACP_FAST_TIMO:
1464                         valid = lacp = 1;
1465                         break;
1466                 default:
1467                         valid = lacp = 0;
1468                         break;
1469                 }
1470
1471                 if (valid == 0 ||
1472                     (lacp == 1 && sc->sc_proto != LAGG_PROTO_LACP)) {
1473                         /* Invalid combination of options specified. */
1474                         error = EINVAL;
1475                         LAGG_XUNLOCK(sc);
1476                         break;  /* Return from SIOCSLAGGOPTS. */
1477                 }
1478
1479                 /*
1480                  * Store new options into sc->sc_opts except for
1481                  * FLOWIDSHIFT, RR and LACP options.
1482                  */
1483                 if (lacp == 0) {
1484                         if (ro->ro_opts == LAGG_OPT_FLOWIDSHIFT)
1485                                 sc->flowid_shift = ro->ro_flowid_shift;
1486                         else if (ro->ro_opts == LAGG_OPT_RR_LIMIT) {
1487                                 if (sc->sc_proto != LAGG_PROTO_ROUNDROBIN ||
1488                                     ro->ro_bkt == 0) {
1489                                         error = EINVAL;
1490                                         LAGG_XUNLOCK(sc);
1491                                         break;
1492                                 }
1493                                 sc->sc_stride = ro->ro_bkt;
1494                         } else if (ro->ro_opts > 0)
1495                                 sc->sc_opts |= ro->ro_opts;
1496                         else
1497                                 sc->sc_opts &= ~ro->ro_opts;
1498                 } else {
1499                         struct lacp_softc *lsc;
1500                         struct lacp_port *lp;
1501
1502                         lsc = (struct lacp_softc *)sc->sc_psc;
1503
1504                         switch (ro->ro_opts) {
1505                         case LAGG_OPT_LACP_TXTEST:
1506                                 lsc->lsc_debug.lsc_tx_test = 1;
1507                                 break;
1508                         case -LAGG_OPT_LACP_TXTEST:
1509                                 lsc->lsc_debug.lsc_tx_test = 0;
1510                                 break;
1511                         case LAGG_OPT_LACP_RXTEST:
1512                                 lsc->lsc_debug.lsc_rx_test = 1;
1513                                 break;
1514                         case -LAGG_OPT_LACP_RXTEST:
1515                                 lsc->lsc_debug.lsc_rx_test = 0;
1516                                 break;
1517                         case LAGG_OPT_LACP_STRICT:
1518                                 lsc->lsc_strict_mode = 1;
1519                                 break;
1520                         case -LAGG_OPT_LACP_STRICT:
1521                                 lsc->lsc_strict_mode = 0;
1522                                 break;
1523                         case LAGG_OPT_LACP_FAST_TIMO:
1524                                 LACP_LOCK(lsc);
1525                                 LIST_FOREACH(lp, &lsc->lsc_ports, lp_next)
1526                                         lp->lp_state |= LACP_STATE_TIMEOUT;
1527                                 LACP_UNLOCK(lsc);
1528                                 lsc->lsc_fast_timeout = 1;
1529                                 break;
1530                         case -LAGG_OPT_LACP_FAST_TIMO:
1531                                 LACP_LOCK(lsc);
1532                                 LIST_FOREACH(lp, &lsc->lsc_ports, lp_next)
1533                                         lp->lp_state &= ~LACP_STATE_TIMEOUT;
1534                                 LACP_UNLOCK(lsc);
1535                                 lsc->lsc_fast_timeout = 0;
1536                                 break;
1537                         }
1538                 }
1539                 LAGG_XUNLOCK(sc);
1540                 break;
1541         case SIOCGLAGGFLAGS:
1542                 rf->rf_flags = 0;
1543                 LAGG_XLOCK(sc);
1544                 if (sc->sc_flags & MBUF_HASHFLAG_L2)
1545                         rf->rf_flags |= LAGG_F_HASHL2;
1546                 if (sc->sc_flags & MBUF_HASHFLAG_L3)
1547                         rf->rf_flags |= LAGG_F_HASHL3;
1548                 if (sc->sc_flags & MBUF_HASHFLAG_L4)
1549                         rf->rf_flags |= LAGG_F_HASHL4;
1550                 LAGG_XUNLOCK(sc);
1551                 break;
1552         case SIOCSLAGGHASH:
1553                 error = priv_check(td, PRIV_NET_LAGG);
1554                 if (error)
1555                         break;
1556                 if ((rf->rf_flags & LAGG_F_HASHMASK) == 0) {
1557                         error = EINVAL;
1558                         break;
1559                 }
1560                 LAGG_XLOCK(sc);
1561                 sc->sc_flags = 0;
1562                 if (rf->rf_flags & LAGG_F_HASHL2)
1563                         sc->sc_flags |= MBUF_HASHFLAG_L2;
1564                 if (rf->rf_flags & LAGG_F_HASHL3)
1565                         sc->sc_flags |= MBUF_HASHFLAG_L3;
1566                 if (rf->rf_flags & LAGG_F_HASHL4)
1567                         sc->sc_flags |= MBUF_HASHFLAG_L4;
1568                 LAGG_XUNLOCK(sc);
1569                 break;
1570         case SIOCGLAGGPORT:
1571                 if (rp->rp_portname[0] == '\0' ||
1572                     (tpif = ifunit_ref(rp->rp_portname)) == NULL) {
1573                         error = EINVAL;
1574                         break;
1575                 }
1576
1577                 NET_EPOCH_ENTER(et);
1578                 if ((lp = (struct lagg_port *)tpif->if_lagg) == NULL ||
1579                     lp->lp_softc != sc) {
1580                         error = ENOENT;
1581                         NET_EPOCH_EXIT(et);
1582                         if_rele(tpif);
1583                         break;
1584                 }
1585
1586                 lagg_port2req(lp, rp);
1587                 NET_EPOCH_EXIT(et);
1588                 if_rele(tpif);
1589                 break;
1590         case SIOCSLAGGPORT:
1591                 error = priv_check(td, PRIV_NET_LAGG);
1592                 if (error)
1593                         break;
1594                 if (rp->rp_portname[0] == '\0' ||
1595                     (tpif = ifunit_ref(rp->rp_portname)) == NULL) {
1596                         error = EINVAL;
1597                         break;
1598                 }
1599 #ifdef INET6
1600                 /*
1601                  * A laggport interface should not have inet6 address
1602                  * because two interfaces with a valid link-local
1603                  * scope zone must not be merged in any form.  This
1604                  * restriction is needed to prevent violation of
1605                  * link-local scope zone.  Attempts to add a laggport
1606                  * interface which has inet6 addresses triggers
1607                  * removal of all inet6 addresses on the member
1608                  * interface.
1609                  */
1610                 if (in6ifa_llaonifp(tpif)) {
1611                         in6_ifdetach(tpif);
1612                                 if_printf(sc->sc_ifp,
1613                                     "IPv6 addresses on %s have been removed "
1614                                     "before adding it as a member to prevent "
1615                                     "IPv6 address scope violation.\n",
1616                                     tpif->if_xname);
1617                 }
1618 #endif
1619                 oldmtu = ifp->if_mtu;
1620                 LAGG_XLOCK(sc);
1621                 error = lagg_port_create(sc, tpif);
1622                 LAGG_XUNLOCK(sc);
1623                 if_rele(tpif);
1624
1625                 /*
1626                  * LAGG MTU may change during addition of the first port.
1627                  * If it did, do network layer specific procedure.
1628                  */
1629                 if (ifp->if_mtu != oldmtu)
1630                         if_notifymtu(ifp);
1631
1632                 VLAN_CAPABILITIES(ifp);
1633                 break;
1634         case SIOCSLAGGDELPORT:
1635                 error = priv_check(td, PRIV_NET_LAGG);
1636                 if (error)
1637                         break;
1638                 if (rp->rp_portname[0] == '\0' ||
1639                     (tpif = ifunit_ref(rp->rp_portname)) == NULL) {
1640                         error = EINVAL;
1641                         break;
1642                 }
1643
1644                 LAGG_XLOCK(sc);
1645                 if ((lp = (struct lagg_port *)tpif->if_lagg) == NULL ||
1646                     lp->lp_softc != sc) {
1647                         error = ENOENT;
1648                         LAGG_XUNLOCK(sc);
1649                         if_rele(tpif);
1650                         break;
1651                 }
1652
1653                 error = lagg_port_destroy(lp, 1);
1654                 LAGG_XUNLOCK(sc);
1655                 if_rele(tpif);
1656                 VLAN_CAPABILITIES(ifp);
1657                 break;
1658         case SIOCSIFFLAGS:
1659                 /* Set flags on ports too */
1660                 LAGG_XLOCK(sc);
1661                 CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
1662                         lagg_setflags(lp, 1);
1663                 }
1664
1665                 if (!(ifp->if_flags & IFF_UP) &&
1666                     (ifp->if_drv_flags & IFF_DRV_RUNNING)) {
1667                         /*
1668                          * If interface is marked down and it is running,
1669                          * then stop and disable it.
1670                          */
1671                         lagg_stop(sc);
1672                         LAGG_XUNLOCK(sc);
1673                 } else if ((ifp->if_flags & IFF_UP) &&
1674                     !(ifp->if_drv_flags & IFF_DRV_RUNNING)) {
1675                         /*
1676                          * If interface is marked up and it is stopped, then
1677                          * start it.
1678                          */
1679                         LAGG_XUNLOCK(sc);
1680                         (*ifp->if_init)(sc);
1681                 } else
1682                         LAGG_XUNLOCK(sc);
1683                 break;
1684         case SIOCADDMULTI:
1685         case SIOCDELMULTI:
1686                 LAGG_XLOCK(sc);
1687                 CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
1688                         lagg_clrmulti(lp);
1689                         lagg_setmulti(lp);
1690                 }
1691                 LAGG_XUNLOCK(sc);
1692                 error = 0;
1693                 break;
1694         case SIOCSIFMEDIA:
1695         case SIOCGIFMEDIA:
1696                 if (ifp->if_type == IFT_INFINIBAND)
1697                         error = EINVAL;
1698                 else
1699                         error = ifmedia_ioctl(ifp, ifr, &sc->sc_media, cmd);
1700                 break;
1701
1702         case SIOCSIFCAP:
1703         case SIOCSIFCAPNV:
1704                 LAGG_XLOCK(sc);
1705                 CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
1706                         if (lp->lp_ioctl != NULL)
1707                                 (*lp->lp_ioctl)(lp->lp_ifp, cmd, data);
1708                 }
1709                 lagg_capabilities(sc);
1710                 LAGG_XUNLOCK(sc);
1711                 VLAN_CAPABILITIES(ifp);
1712                 error = 0;
1713                 break;
1714
1715         case SIOCGIFCAPNV:
1716                 error = 0;
1717                 break;
1718
1719         case SIOCSIFMTU:
1720                 LAGG_XLOCK(sc);
1721                 CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
1722                         if (lp->lp_ioctl != NULL)
1723                                 error = (*lp->lp_ioctl)(lp->lp_ifp, cmd, data);
1724                         else
1725                                 error = EINVAL;
1726                         if (error != 0) {
1727                                 if_printf(ifp,
1728                                     "failed to change MTU to %d on port %s, "
1729                                     "reverting all ports to original MTU (%d)\n",
1730                                     ifr->ifr_mtu, lp->lp_ifp->if_xname, ifp->if_mtu);
1731                                 break;
1732                         }
1733                 }
1734                 if (error == 0) {
1735                         ifp->if_mtu = ifr->ifr_mtu;
1736                 } else {
1737                         /* set every port back to the original MTU */
1738                         ifr->ifr_mtu = ifp->if_mtu;
1739                         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
1740                                 if (lp->lp_ioctl != NULL)
1741                                         (*lp->lp_ioctl)(lp->lp_ifp, cmd, data);
1742                         }
1743                 }
1744                 lagg_capabilities(sc);
1745                 LAGG_XUNLOCK(sc);
1746                 VLAN_CAPABILITIES(ifp);
1747                 break;
1748
1749         default:
1750                 error = ether_ioctl(ifp, cmd, data);
1751                 break;
1752         }
1753         return (error);
1754 }
1755
1756 #if defined(KERN_TLS) || defined(RATELIMIT)
1757 #ifdef RATELIMIT
1758 static const struct if_snd_tag_sw lagg_snd_tag_ul_sw = {
1759         .snd_tag_modify = lagg_snd_tag_modify,
1760         .snd_tag_query = lagg_snd_tag_query,
1761         .snd_tag_free = lagg_snd_tag_free,
1762         .next_snd_tag = lagg_next_snd_tag,
1763         .type = IF_SND_TAG_TYPE_UNLIMITED
1764 };
1765
1766 static const struct if_snd_tag_sw lagg_snd_tag_rl_sw = {
1767         .snd_tag_modify = lagg_snd_tag_modify,
1768         .snd_tag_query = lagg_snd_tag_query,
1769         .snd_tag_free = lagg_snd_tag_free,
1770         .next_snd_tag = lagg_next_snd_tag,
1771         .type = IF_SND_TAG_TYPE_RATE_LIMIT
1772 };
1773 #endif
1774
1775 #ifdef KERN_TLS
1776 static const struct if_snd_tag_sw lagg_snd_tag_tls_sw = {
1777         .snd_tag_modify = lagg_snd_tag_modify,
1778         .snd_tag_query = lagg_snd_tag_query,
1779         .snd_tag_free = lagg_snd_tag_free,
1780         .next_snd_tag = lagg_next_snd_tag,
1781         .type = IF_SND_TAG_TYPE_TLS
1782 };
1783
1784 #ifdef RATELIMIT
1785 static const struct if_snd_tag_sw lagg_snd_tag_tls_rl_sw = {
1786         .snd_tag_modify = lagg_snd_tag_modify,
1787         .snd_tag_query = lagg_snd_tag_query,
1788         .snd_tag_free = lagg_snd_tag_free,
1789         .next_snd_tag = lagg_next_snd_tag,
1790         .type = IF_SND_TAG_TYPE_TLS_RATE_LIMIT
1791 };
1792 #endif
1793 #endif
1794
1795 static inline struct lagg_snd_tag *
1796 mst_to_lst(struct m_snd_tag *mst)
1797 {
1798
1799         return (__containerof(mst, struct lagg_snd_tag, com));
1800 }
1801
1802 /*
1803  * Look up the port used by a specific flow.  This only works for lagg
1804  * protocols with deterministic port mappings (e.g. not roundrobin).
1805  * In addition protocols which use a hash to map flows to ports must
1806  * be configured to use the mbuf flowid rather than hashing packet
1807  * contents.
1808  */
1809 static struct lagg_port *
1810 lookup_snd_tag_port(struct ifnet *ifp, uint32_t flowid, uint32_t flowtype,
1811     uint8_t numa_domain)
1812 {
1813         struct lagg_softc *sc;
1814         struct lagg_port *lp;
1815         struct lagg_lb *lb;
1816         uint32_t hash, p;
1817         int err;
1818
1819         sc = ifp->if_softc;
1820
1821         switch (sc->sc_proto) {
1822         case LAGG_PROTO_FAILOVER:
1823                 return (lagg_link_active(sc, sc->sc_primary));
1824         case LAGG_PROTO_LOADBALANCE:
1825                 if ((sc->sc_opts & LAGG_OPT_USE_FLOWID) == 0 ||
1826                     flowtype == M_HASHTYPE_NONE)
1827                         return (NULL);
1828                 p = flowid >> sc->flowid_shift;
1829                 p %= sc->sc_count;
1830                 lb = (struct lagg_lb *)sc->sc_psc;
1831                 lp = lb->lb_ports[p];
1832                 return (lagg_link_active(sc, lp));
1833         case LAGG_PROTO_LACP:
1834                 if ((sc->sc_opts & LAGG_OPT_USE_FLOWID) == 0 ||
1835                     flowtype == M_HASHTYPE_NONE)
1836                         return (NULL);
1837                 hash = flowid >> sc->flowid_shift;
1838                 return (lacp_select_tx_port_by_hash(sc, hash, numa_domain, &err));
1839         default:
1840                 return (NULL);
1841         }
1842 }
1843
1844 static int
1845 lagg_snd_tag_alloc(struct ifnet *ifp,
1846     union if_snd_tag_alloc_params *params,
1847     struct m_snd_tag **ppmt)
1848 {
1849         struct epoch_tracker et;
1850         const struct if_snd_tag_sw *sw;
1851         struct lagg_snd_tag *lst;
1852         struct lagg_port *lp;
1853         struct ifnet *lp_ifp;
1854         struct m_snd_tag *mst;
1855         int error;
1856
1857         switch (params->hdr.type) {
1858 #ifdef RATELIMIT
1859         case IF_SND_TAG_TYPE_UNLIMITED:
1860                 sw = &lagg_snd_tag_ul_sw;
1861                 break;
1862         case IF_SND_TAG_TYPE_RATE_LIMIT:
1863                 sw = &lagg_snd_tag_rl_sw;
1864                 break;
1865 #endif
1866 #ifdef KERN_TLS
1867         case IF_SND_TAG_TYPE_TLS:
1868                 sw = &lagg_snd_tag_tls_sw;
1869                 break;
1870         case IF_SND_TAG_TYPE_TLS_RX:
1871                 /* Return tag from port interface directly. */
1872                 sw = NULL;
1873                 break;
1874 #ifdef RATELIMIT
1875         case IF_SND_TAG_TYPE_TLS_RATE_LIMIT:
1876                 sw = &lagg_snd_tag_tls_rl_sw;
1877                 break;
1878 #endif
1879 #endif
1880         default:
1881                 return (EOPNOTSUPP);
1882         }
1883
1884         NET_EPOCH_ENTER(et);
1885         lp = lookup_snd_tag_port(ifp, params->hdr.flowid,
1886             params->hdr.flowtype, params->hdr.numa_domain);
1887         if (lp == NULL) {
1888                 NET_EPOCH_EXIT(et);
1889                 return (EOPNOTSUPP);
1890         }
1891         if (lp->lp_ifp == NULL) {
1892                 NET_EPOCH_EXIT(et);
1893                 return (EOPNOTSUPP);
1894         }
1895         lp_ifp = lp->lp_ifp;
1896         if_ref(lp_ifp);
1897         NET_EPOCH_EXIT(et);
1898
1899         if (sw != NULL) {
1900                 lst = malloc(sizeof(*lst), M_LAGG, M_NOWAIT);
1901                 if (lst == NULL) {
1902                         if_rele(lp_ifp);
1903                         return (ENOMEM);
1904                 }
1905         } else
1906                 lst = NULL;
1907
1908         error = m_snd_tag_alloc(lp_ifp, params, &mst);
1909         if_rele(lp_ifp);
1910         if (error) {
1911                 free(lst, M_LAGG);
1912                 return (error);
1913         }
1914
1915         if (sw != NULL) {
1916                 m_snd_tag_init(&lst->com, ifp, sw);
1917                 lst->tag = mst;
1918
1919                 *ppmt = &lst->com;
1920         } else
1921                 *ppmt = mst;
1922
1923         return (0);
1924 }
1925
1926 static struct m_snd_tag *
1927 lagg_next_snd_tag(struct m_snd_tag *mst)
1928 {
1929         struct lagg_snd_tag *lst;
1930
1931         lst = mst_to_lst(mst);
1932         return (lst->tag);
1933 }
1934
1935 static int
1936 lagg_snd_tag_modify(struct m_snd_tag *mst,
1937     union if_snd_tag_modify_params *params)
1938 {
1939         struct lagg_snd_tag *lst;
1940
1941         lst = mst_to_lst(mst);
1942         return (lst->tag->sw->snd_tag_modify(lst->tag, params));
1943 }
1944
1945 static int
1946 lagg_snd_tag_query(struct m_snd_tag *mst,
1947     union if_snd_tag_query_params *params)
1948 {
1949         struct lagg_snd_tag *lst;
1950
1951         lst = mst_to_lst(mst);
1952         return (lst->tag->sw->snd_tag_query(lst->tag, params));
1953 }
1954
1955 static void
1956 lagg_snd_tag_free(struct m_snd_tag *mst)
1957 {
1958         struct lagg_snd_tag *lst;
1959
1960         lst = mst_to_lst(mst);
1961         m_snd_tag_rele(lst->tag);
1962         free(lst, M_LAGG);
1963 }
1964
1965 static void
1966 lagg_ratelimit_query(struct ifnet *ifp __unused, struct if_ratelimit_query_results *q)
1967 {
1968         /*
1969          * For lagg, we have an indirect
1970          * interface. The caller needs to
1971          * get a ratelimit tag on the actual
1972          * interface the flow will go on.
1973          */
1974         q->rate_table = NULL;
1975         q->flags = RT_IS_INDIRECT;
1976         q->max_flows = 0;
1977         q->number_of_rates = 0;
1978 }
1979 #endif
1980
1981 static int
1982 lagg_setmulti(struct lagg_port *lp)
1983 {
1984         struct lagg_softc *sc = lp->lp_softc;
1985         struct ifnet *ifp = lp->lp_ifp;
1986         struct ifnet *scifp = sc->sc_ifp;
1987         struct lagg_mc *mc;
1988         struct ifmultiaddr *ifma;
1989         int error;
1990
1991         IF_ADDR_WLOCK(scifp);
1992         CK_STAILQ_FOREACH(ifma, &scifp->if_multiaddrs, ifma_link) {
1993                 if (ifma->ifma_addr->sa_family != AF_LINK)
1994                         continue;
1995                 mc = malloc(sizeof(struct lagg_mc), M_LAGG, M_NOWAIT);
1996                 if (mc == NULL) {
1997                         IF_ADDR_WUNLOCK(scifp);
1998                         return (ENOMEM);
1999                 }
2000                 bcopy(ifma->ifma_addr, &mc->mc_addr, ifma->ifma_addr->sa_len);
2001                 mc->mc_addr.sdl_index = ifp->if_index;
2002                 mc->mc_ifma = NULL;
2003                 SLIST_INSERT_HEAD(&lp->lp_mc_head, mc, mc_entries);
2004         }
2005         IF_ADDR_WUNLOCK(scifp);
2006         SLIST_FOREACH (mc, &lp->lp_mc_head, mc_entries) {
2007                 error = if_addmulti(ifp,
2008                     (struct sockaddr *)&mc->mc_addr, &mc->mc_ifma);
2009                 if (error)
2010                         return (error);
2011         }
2012         return (0);
2013 }
2014
2015 static int
2016 lagg_clrmulti(struct lagg_port *lp)
2017 {
2018         struct lagg_mc *mc;
2019
2020         LAGG_XLOCK_ASSERT(lp->lp_softc);
2021         while ((mc = SLIST_FIRST(&lp->lp_mc_head)) != NULL) {
2022                 SLIST_REMOVE(&lp->lp_mc_head, mc, lagg_mc, mc_entries);
2023                 if (mc->mc_ifma && lp->lp_detaching == 0)
2024                         if_delmulti_ifma(mc->mc_ifma);
2025                 free(mc, M_LAGG);
2026         }
2027         return (0);
2028 }
2029
2030 static void
2031 lagg_setcaps(struct lagg_port *lp, int cap, int cap2)
2032 {
2033         struct ifreq ifr;
2034         struct siocsifcapnv_driver_data drv_ioctl_data;
2035
2036         if (lp->lp_ifp->if_capenable == cap &&
2037             lp->lp_ifp->if_capenable2 == cap2)
2038                 return;
2039         if (lp->lp_ioctl == NULL)
2040                 return;
2041         /* XXX */
2042         if ((lp->lp_ifp->if_capabilities & IFCAP_NV) != 0) {
2043                 drv_ioctl_data.reqcap = cap;
2044                 drv_ioctl_data.reqcap2 = cap2;
2045                 drv_ioctl_data.nvcap = NULL;
2046                 (*lp->lp_ioctl)(lp->lp_ifp, SIOCSIFCAPNV,
2047                     (caddr_t)&drv_ioctl_data);
2048         } else {
2049                 ifr.ifr_reqcap = cap;
2050                 (*lp->lp_ioctl)(lp->lp_ifp, SIOCSIFCAP, (caddr_t)&ifr);
2051         }
2052 }
2053
2054 /* Handle a ref counted flag that should be set on the lagg port as well */
2055 static int
2056 lagg_setflag(struct lagg_port *lp, int flag, int status,
2057     int (*func)(struct ifnet *, int))
2058 {
2059         struct lagg_softc *sc = lp->lp_softc;
2060         struct ifnet *scifp = sc->sc_ifp;
2061         struct ifnet *ifp = lp->lp_ifp;
2062         int error;
2063
2064         LAGG_XLOCK_ASSERT(sc);
2065
2066         status = status ? (scifp->if_flags & flag) : 0;
2067         /* Now "status" contains the flag value or 0 */
2068
2069         /*
2070          * See if recorded ports status is different from what
2071          * we want it to be.  If it is, flip it.  We record ports
2072          * status in lp_ifflags so that we won't clear ports flag
2073          * we haven't set.  In fact, we don't clear or set ports
2074          * flags directly, but get or release references to them.
2075          * That's why we can be sure that recorded flags still are
2076          * in accord with actual ports flags.
2077          */
2078         if (status != (lp->lp_ifflags & flag)) {
2079                 error = (*func)(ifp, status);
2080                 if (error)
2081                         return (error);
2082                 lp->lp_ifflags &= ~flag;
2083                 lp->lp_ifflags |= status;
2084         }
2085         return (0);
2086 }
2087
2088 /*
2089  * Handle IFF_* flags that require certain changes on the lagg port
2090  * if "status" is true, update ports flags respective to the lagg
2091  * if "status" is false, forcedly clear the flags set on port.
2092  */
2093 static int
2094 lagg_setflags(struct lagg_port *lp, int status)
2095 {
2096         int error, i;
2097
2098         for (i = 0; lagg_pflags[i].flag; i++) {
2099                 error = lagg_setflag(lp, lagg_pflags[i].flag,
2100                     status, lagg_pflags[i].func);
2101                 if (error)
2102                         return (error);
2103         }
2104         return (0);
2105 }
2106
2107 static int
2108 lagg_transmit_ethernet(struct ifnet *ifp, struct mbuf *m)
2109 {
2110         struct lagg_softc *sc = (struct lagg_softc *)ifp->if_softc;
2111
2112         NET_EPOCH_ASSERT();
2113 #if defined(KERN_TLS) || defined(RATELIMIT)
2114         if (m->m_pkthdr.csum_flags & CSUM_SND_TAG)
2115                 MPASS(m->m_pkthdr.snd_tag->ifp == ifp);
2116 #endif
2117         /* We need a Tx algorithm and at least one port */
2118         if (sc->sc_proto == LAGG_PROTO_NONE || sc->sc_count == 0) {
2119                 m_freem(m);
2120                 if_inc_counter(ifp, IFCOUNTER_OERRORS, 1);
2121                 return (ENXIO);
2122         }
2123
2124         ETHER_BPF_MTAP(ifp, m);
2125
2126         return (lagg_proto_start(sc, m));
2127 }
2128
2129 static int
2130 lagg_transmit_infiniband(struct ifnet *ifp, struct mbuf *m)
2131 {
2132         struct lagg_softc *sc = (struct lagg_softc *)ifp->if_softc;
2133
2134         NET_EPOCH_ASSERT();
2135 #if defined(KERN_TLS) || defined(RATELIMIT)
2136         if (m->m_pkthdr.csum_flags & CSUM_SND_TAG)
2137                 MPASS(m->m_pkthdr.snd_tag->ifp == ifp);
2138 #endif
2139         /* We need a Tx algorithm and at least one port */
2140         if (sc->sc_proto == LAGG_PROTO_NONE || sc->sc_count == 0) {
2141                 m_freem(m);
2142                 if_inc_counter(ifp, IFCOUNTER_OERRORS, 1);
2143                 return (ENXIO);
2144         }
2145
2146         infiniband_bpf_mtap(ifp, m);
2147
2148         return (lagg_proto_start(sc, m));
2149 }
2150
2151 /*
2152  * The ifp->if_qflush entry point for lagg(4) is no-op.
2153  */
2154 static void
2155 lagg_qflush(struct ifnet *ifp __unused)
2156 {
2157 }
2158
2159 static struct mbuf *
2160 lagg_input_ethernet(struct ifnet *ifp, struct mbuf *m)
2161 {
2162         struct lagg_port *lp = ifp->if_lagg;
2163         struct lagg_softc *sc = lp->lp_softc;
2164         struct ifnet *scifp = sc->sc_ifp;
2165
2166         NET_EPOCH_ASSERT();
2167         if ((scifp->if_drv_flags & IFF_DRV_RUNNING) == 0 ||
2168             lp->lp_detaching != 0 ||
2169             sc->sc_proto == LAGG_PROTO_NONE) {
2170                 m_freem(m);
2171                 return (NULL);
2172         }
2173
2174         m = lagg_proto_input(sc, lp, m);
2175         if (m != NULL) {
2176                 ETHER_BPF_MTAP(scifp, m);
2177
2178                 if ((scifp->if_flags & IFF_MONITOR) != 0) {
2179                         m_freem(m);
2180                         m = NULL;
2181                 }
2182         }
2183
2184 #ifdef DEV_NETMAP
2185         if (m != NULL && scifp->if_capenable & IFCAP_NETMAP) {
2186                 scifp->if_input(scifp, m);
2187                 m = NULL;
2188         }
2189 #endif  /* DEV_NETMAP */
2190
2191         return (m);
2192 }
2193
2194 static struct mbuf *
2195 lagg_input_infiniband(struct ifnet *ifp, struct mbuf *m)
2196 {
2197         struct lagg_port *lp = ifp->if_lagg;
2198         struct lagg_softc *sc = lp->lp_softc;
2199         struct ifnet *scifp = sc->sc_ifp;
2200
2201         NET_EPOCH_ASSERT();
2202         if ((scifp->if_drv_flags & IFF_DRV_RUNNING) == 0 ||
2203             lp->lp_detaching != 0 ||
2204             sc->sc_proto == LAGG_PROTO_NONE) {
2205                 m_freem(m);
2206                 return (NULL);
2207         }
2208
2209         m = lagg_proto_input(sc, lp, m);
2210         if (m != NULL) {
2211                 infiniband_bpf_mtap(scifp, m);
2212
2213                 if ((scifp->if_flags & IFF_MONITOR) != 0) {
2214                         m_freem(m);
2215                         m = NULL;
2216                 }
2217         }
2218
2219         return (m);
2220 }
2221
2222 static int
2223 lagg_media_change(struct ifnet *ifp)
2224 {
2225         struct lagg_softc *sc = (struct lagg_softc *)ifp->if_softc;
2226
2227         if (sc->sc_ifflags & IFF_DEBUG)
2228                 printf("%s\n", __func__);
2229
2230         /* Ignore */
2231         return (0);
2232 }
2233
2234 static void
2235 lagg_media_status(struct ifnet *ifp, struct ifmediareq *imr)
2236 {
2237         struct epoch_tracker et;
2238         struct lagg_softc *sc = (struct lagg_softc *)ifp->if_softc;
2239         struct lagg_port *lp;
2240
2241         imr->ifm_status = IFM_AVALID;
2242         imr->ifm_active = IFM_ETHER | IFM_AUTO;
2243
2244         NET_EPOCH_ENTER(et);
2245         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
2246                 if (LAGG_PORTACTIVE(lp))
2247                         imr->ifm_status |= IFM_ACTIVE;
2248         }
2249         NET_EPOCH_EXIT(et);
2250 }
2251
2252 static void
2253 lagg_linkstate(struct lagg_softc *sc)
2254 {
2255         struct epoch_tracker et;
2256         struct lagg_port *lp;
2257         int new_link = LINK_STATE_DOWN;
2258         uint64_t speed;
2259
2260         LAGG_XLOCK_ASSERT(sc);
2261
2262         /* LACP handles link state itself */
2263         if (sc->sc_proto == LAGG_PROTO_LACP)
2264                 return;
2265
2266         /* Our link is considered up if at least one of our ports is active */
2267         NET_EPOCH_ENTER(et);
2268         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
2269                 if (lp->lp_ifp->if_link_state == LINK_STATE_UP) {
2270                         new_link = LINK_STATE_UP;
2271                         break;
2272                 }
2273         }
2274         NET_EPOCH_EXIT(et);
2275         if_link_state_change(sc->sc_ifp, new_link);
2276
2277         /* Update if_baudrate to reflect the max possible speed */
2278         switch (sc->sc_proto) {
2279                 case LAGG_PROTO_FAILOVER:
2280                         sc->sc_ifp->if_baudrate = sc->sc_primary != NULL ?
2281                             sc->sc_primary->lp_ifp->if_baudrate : 0;
2282                         break;
2283                 case LAGG_PROTO_ROUNDROBIN:
2284                 case LAGG_PROTO_LOADBALANCE:
2285                 case LAGG_PROTO_BROADCAST:
2286                         speed = 0;
2287                         NET_EPOCH_ENTER(et);
2288                         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries)
2289                                 speed += lp->lp_ifp->if_baudrate;
2290                         NET_EPOCH_EXIT(et);
2291                         sc->sc_ifp->if_baudrate = speed;
2292                         break;
2293                 case LAGG_PROTO_LACP:
2294                         /* LACP updates if_baudrate itself */
2295                         break;
2296         }
2297 }
2298
2299 static void
2300 lagg_port_state(struct ifnet *ifp, int state)
2301 {
2302         struct lagg_port *lp = (struct lagg_port *)ifp->if_lagg;
2303         struct lagg_softc *sc = NULL;
2304
2305         if (lp != NULL)
2306                 sc = lp->lp_softc;
2307         if (sc == NULL)
2308                 return;
2309
2310         LAGG_XLOCK(sc);
2311         lagg_linkstate(sc);
2312         lagg_proto_linkstate(sc, lp);
2313         LAGG_XUNLOCK(sc);
2314 }
2315
2316 struct lagg_port *
2317 lagg_link_active(struct lagg_softc *sc, struct lagg_port *lp)
2318 {
2319         struct lagg_port *lp_next, *rval = NULL;
2320
2321         /*
2322          * Search a port which reports an active link state.
2323          */
2324
2325 #ifdef INVARIANTS
2326         /*
2327          * This is called with either in the network epoch
2328          * or with LAGG_XLOCK(sc) held.
2329          */
2330         if (!in_epoch(net_epoch_preempt))
2331                 LAGG_XLOCK_ASSERT(sc);
2332 #endif
2333
2334         if (lp == NULL)
2335                 goto search;
2336         if (LAGG_PORTACTIVE(lp)) {
2337                 rval = lp;
2338                 goto found;
2339         }
2340         if ((lp_next = CK_SLIST_NEXT(lp, lp_entries)) != NULL &&
2341             LAGG_PORTACTIVE(lp_next)) {
2342                 rval = lp_next;
2343                 goto found;
2344         }
2345
2346 search:
2347         CK_SLIST_FOREACH(lp_next, &sc->sc_ports, lp_entries) {
2348                 if (LAGG_PORTACTIVE(lp_next)) {
2349                         return (lp_next);
2350                 }
2351         }
2352 found:
2353         return (rval);
2354 }
2355
2356 int
2357 lagg_enqueue(struct ifnet *ifp, struct mbuf *m)
2358 {
2359
2360 #if defined(KERN_TLS) || defined(RATELIMIT)
2361         if (m->m_pkthdr.csum_flags & CSUM_SND_TAG) {
2362                 struct lagg_snd_tag *lst;
2363                 struct m_snd_tag *mst;
2364
2365                 mst = m->m_pkthdr.snd_tag;
2366                 lst = mst_to_lst(mst);
2367                 if (lst->tag->ifp != ifp) {
2368                         m_freem(m);
2369                         return (EAGAIN);
2370                 }
2371                 m->m_pkthdr.snd_tag = m_snd_tag_ref(lst->tag);
2372                 m_snd_tag_rele(mst);
2373         }
2374 #endif
2375         return (ifp->if_transmit)(ifp, m);
2376 }
2377
2378 /*
2379  * Simple round robin aggregation
2380  */
2381 static void
2382 lagg_rr_attach(struct lagg_softc *sc)
2383 {
2384         sc->sc_seq = 0;
2385         sc->sc_stride = 1;
2386 }
2387
2388 static int
2389 lagg_rr_start(struct lagg_softc *sc, struct mbuf *m)
2390 {
2391         struct lagg_port *lp;
2392         uint32_t p;
2393
2394         p = atomic_fetchadd_32(&sc->sc_seq, 1);
2395         p /= sc->sc_stride;
2396         p %= sc->sc_count;
2397         lp = CK_SLIST_FIRST(&sc->sc_ports);
2398
2399         while (p--)
2400                 lp = CK_SLIST_NEXT(lp, lp_entries);
2401
2402         /*
2403          * Check the port's link state. This will return the next active
2404          * port if the link is down or the port is NULL.
2405          */
2406         if ((lp = lagg_link_active(sc, lp)) == NULL) {
2407                 if_inc_counter(sc->sc_ifp, IFCOUNTER_OERRORS, 1);
2408                 m_freem(m);
2409                 return (ENETDOWN);
2410         }
2411
2412         /* Send mbuf */
2413         return (lagg_enqueue(lp->lp_ifp, m));
2414 }
2415
2416 /*
2417  * Broadcast mode
2418  */
2419 static int
2420 lagg_bcast_start(struct lagg_softc *sc, struct mbuf *m)
2421 {
2422         int errors = 0;
2423         int ret;
2424         struct lagg_port *lp, *last = NULL;
2425         struct mbuf *m0;
2426
2427         NET_EPOCH_ASSERT();
2428         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries) {
2429                 if (!LAGG_PORTACTIVE(lp))
2430                         continue;
2431
2432                 if (last != NULL) {
2433                         m0 = m_copym(m, 0, M_COPYALL, M_NOWAIT);
2434                         if (m0 == NULL) {
2435                                 ret = ENOBUFS;
2436                                 errors++;
2437                                 break;
2438                         }
2439                         lagg_enqueue(last->lp_ifp, m0);
2440                 }
2441                 last = lp;
2442         }
2443
2444         if (last == NULL) {
2445                 if_inc_counter(sc->sc_ifp, IFCOUNTER_OERRORS, 1);
2446                 m_freem(m);
2447                 return (ENOENT);
2448         }
2449         if ((last = lagg_link_active(sc, last)) == NULL) {
2450                 errors++;
2451                 if_inc_counter(sc->sc_ifp, IFCOUNTER_OERRORS, errors);
2452                 m_freem(m);
2453                 return (ENETDOWN);
2454         }
2455
2456         ret = lagg_enqueue(last->lp_ifp, m);
2457         if (errors != 0)
2458                 if_inc_counter(sc->sc_ifp, IFCOUNTER_OERRORS, errors);
2459
2460         return (ret);
2461 }
2462
2463 /*
2464  * Active failover
2465  */
2466 static int
2467 lagg_fail_start(struct lagg_softc *sc, struct mbuf *m)
2468 {
2469         struct lagg_port *lp;
2470
2471         /* Use the master port if active or the next available port */
2472         if ((lp = lagg_link_active(sc, sc->sc_primary)) == NULL) {
2473                 if_inc_counter(sc->sc_ifp, IFCOUNTER_OERRORS, 1);
2474                 m_freem(m);
2475                 return (ENETDOWN);
2476         }
2477
2478         /* Send mbuf */
2479         return (lagg_enqueue(lp->lp_ifp, m));
2480 }
2481
2482 static struct mbuf *
2483 lagg_fail_input(struct lagg_softc *sc, struct lagg_port *lp, struct mbuf *m)
2484 {
2485         struct ifnet *ifp = sc->sc_ifp;
2486         struct lagg_port *tmp_tp;
2487
2488         if (lp == sc->sc_primary || V_lagg_failover_rx_all) {
2489                 m->m_pkthdr.rcvif = ifp;
2490                 return (m);
2491         }
2492
2493         if (!LAGG_PORTACTIVE(sc->sc_primary)) {
2494                 tmp_tp = lagg_link_active(sc, sc->sc_primary);
2495                 /*
2496                  * If tmp_tp is null, we've received a packet when all
2497                  * our links are down. Weird, but process it anyways.
2498                  */
2499                 if (tmp_tp == NULL || tmp_tp == lp) {
2500                         m->m_pkthdr.rcvif = ifp;
2501                         return (m);
2502                 }
2503         }
2504
2505         m_freem(m);
2506         return (NULL);
2507 }
2508
2509 /*
2510  * Loadbalancing
2511  */
2512 static void
2513 lagg_lb_attach(struct lagg_softc *sc)
2514 {
2515         struct lagg_port *lp;
2516         struct lagg_lb *lb;
2517
2518         LAGG_XLOCK_ASSERT(sc);
2519         lb = malloc(sizeof(struct lagg_lb), M_LAGG, M_WAITOK | M_ZERO);
2520         lb->lb_key = m_ether_tcpip_hash_init();
2521         sc->sc_psc = lb;
2522
2523         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries)
2524                 lagg_lb_port_create(lp);
2525 }
2526
2527 static void
2528 lagg_lb_detach(struct lagg_softc *sc)
2529 {
2530         struct lagg_lb *lb;
2531
2532         lb = (struct lagg_lb *)sc->sc_psc;
2533         if (lb != NULL)
2534                 free(lb, M_LAGG);
2535 }
2536
2537 static int
2538 lagg_lb_porttable(struct lagg_softc *sc, struct lagg_port *lp)
2539 {
2540         struct lagg_lb *lb = (struct lagg_lb *)sc->sc_psc;
2541         struct lagg_port *lp_next;
2542         int i = 0, rv;
2543
2544         rv = 0;
2545         bzero(&lb->lb_ports, sizeof(lb->lb_ports));
2546         LAGG_XLOCK_ASSERT(sc);
2547         CK_SLIST_FOREACH(lp_next, &sc->sc_ports, lp_entries) {
2548                 if (lp_next == lp)
2549                         continue;
2550                 if (i >= LAGG_MAX_PORTS) {
2551                         rv = EINVAL;
2552                         break;
2553                 }
2554                 if (sc->sc_ifflags & IFF_DEBUG)
2555                         printf("%s: port %s at index %d\n",
2556                             sc->sc_ifname, lp_next->lp_ifp->if_xname, i);
2557                 lb->lb_ports[i++] = lp_next;
2558         }
2559
2560         return (rv);
2561 }
2562
2563 static int
2564 lagg_lb_port_create(struct lagg_port *lp)
2565 {
2566         struct lagg_softc *sc = lp->lp_softc;
2567         return (lagg_lb_porttable(sc, NULL));
2568 }
2569
2570 static void
2571 lagg_lb_port_destroy(struct lagg_port *lp)
2572 {
2573         struct lagg_softc *sc = lp->lp_softc;
2574         lagg_lb_porttable(sc, lp);
2575 }
2576
2577 static int
2578 lagg_lb_start(struct lagg_softc *sc, struct mbuf *m)
2579 {
2580         struct lagg_lb *lb = (struct lagg_lb *)sc->sc_psc;
2581         struct lagg_port *lp = NULL;
2582         uint32_t p = 0;
2583
2584         if ((sc->sc_opts & LAGG_OPT_USE_FLOWID) &&
2585             M_HASHTYPE_GET(m) != M_HASHTYPE_NONE)
2586                 p = m->m_pkthdr.flowid >> sc->flowid_shift;
2587         else
2588                 p = m_ether_tcpip_hash(sc->sc_flags, m, lb->lb_key);
2589         p %= sc->sc_count;
2590         lp = lb->lb_ports[p];
2591
2592         /*
2593          * Check the port's link state. This will return the next active
2594          * port if the link is down or the port is NULL.
2595          */
2596         if ((lp = lagg_link_active(sc, lp)) == NULL) {
2597                 if_inc_counter(sc->sc_ifp, IFCOUNTER_OERRORS, 1);
2598                 m_freem(m);
2599                 return (ENETDOWN);
2600         }
2601
2602         /* Send mbuf */
2603         return (lagg_enqueue(lp->lp_ifp, m));
2604 }
2605
2606 /*
2607  * 802.3ad LACP
2608  */
2609 static void
2610 lagg_lacp_attach(struct lagg_softc *sc)
2611 {
2612         struct lagg_port *lp;
2613
2614         lacp_attach(sc);
2615         LAGG_XLOCK_ASSERT(sc);
2616         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries)
2617                 lacp_port_create(lp);
2618 }
2619
2620 static void
2621 lagg_lacp_detach(struct lagg_softc *sc)
2622 {
2623         struct lagg_port *lp;
2624         void *psc;
2625
2626         LAGG_XLOCK_ASSERT(sc);
2627         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries)
2628                 lacp_port_destroy(lp);
2629
2630         psc = sc->sc_psc;
2631         sc->sc_psc = NULL;
2632         lacp_detach(psc);
2633 }
2634
2635 static void
2636 lagg_lacp_lladdr(struct lagg_softc *sc)
2637 {
2638         struct lagg_port *lp;
2639
2640         LAGG_SXLOCK_ASSERT(sc);
2641
2642         /* purge all the lacp ports */
2643         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries)
2644                 lacp_port_destroy(lp);
2645
2646         /* add them back in */
2647         CK_SLIST_FOREACH(lp, &sc->sc_ports, lp_entries)
2648                 lacp_port_create(lp);
2649 }
2650
2651 static int
2652 lagg_lacp_start(struct lagg_softc *sc, struct mbuf *m)
2653 {
2654         struct lagg_port *lp;
2655         int err;
2656
2657         lp = lacp_select_tx_port(sc, m, &err);
2658         if (lp == NULL) {
2659                 if_inc_counter(sc->sc_ifp, IFCOUNTER_OERRORS, 1);
2660                 m_freem(m);
2661                 return (err);
2662         }
2663
2664         /* Send mbuf */
2665         return (lagg_enqueue(lp->lp_ifp, m));
2666 }
2667
2668 static struct mbuf *
2669 lagg_lacp_input(struct lagg_softc *sc, struct lagg_port *lp, struct mbuf *m)
2670 {
2671         struct ifnet *ifp = sc->sc_ifp;
2672         struct ether_header *eh;
2673         u_short etype;
2674
2675         eh = mtod(m, struct ether_header *);
2676         etype = ntohs(eh->ether_type);
2677
2678         /* Tap off LACP control messages */
2679         if ((m->m_flags & M_VLANTAG) == 0 && etype == ETHERTYPE_SLOW) {
2680                 m = lacp_input(lp, m);
2681                 if (m == NULL)
2682                         return (NULL);
2683         }
2684
2685         /*
2686          * If the port is not collecting or not in the active aggregator then
2687          * free and return.
2688          */
2689         if (!lacp_iscollecting(lp) || !lacp_isactive(lp)) {
2690                 m_freem(m);
2691                 return (NULL);
2692         }
2693
2694         m->m_pkthdr.rcvif = ifp;
2695         return (m);
2696 }
2697
2698 /* Default input */
2699 static struct mbuf *
2700 lagg_default_input(struct lagg_softc *sc, struct lagg_port *lp, struct mbuf *m)
2701 {
2702         struct ifnet *ifp = sc->sc_ifp;
2703
2704         /* Just pass in the packet to our lagg device */
2705         m->m_pkthdr.rcvif = ifp;
2706
2707         return (m);
2708 }