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