bsd/net/ndrv.c

   1 /*
   2  * Copyright (c) 2000 Apple Computer, Inc. All rights reserved.
   3  *
   4  * @APPLE_LICENSE_HEADER_START@
   5  *
   6  * The contents of this file constitute Original Code as defined in and
   7  * are subject to the Apple Public Source License Version 1.1 (the
   8  * "License").  You may not use this file except in compliance with the
   9  * License.  Please obtain a copy of the License at
  10  * http://www.apple.com/publicsource and read it before using this file.
  11  *
  12  * This Original Code and all software distributed under the License are
  13  * distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY KIND, EITHER
  14  * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
  15  * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
  16  * FITNESS FOR A PARTICULAR PURPOSE OR NON-INFRINGEMENT.  Please see the
  17  * License for the specific language governing rights and limitations
  18  * under the License.
  19  *
  20  * @APPLE_LICENSE_HEADER_END@
  21  */
  22 /* Copyright (c) 1997, 1998 Apple Computer, Inc. All Rights Reserved */
  23 /*
  24  *      @(#)ndrv.c      1.1 (MacOSX) 6/10/43
  25  * Justin Walker, 970604
  26  *   AF_NDRV support
  27  * 980130 - Cleanup, reorg, performance improvemements
  28  * 000816 - Removal of Y adapter cruft
  29  */
  30
  31 /*
  32  * PF_NDRV allows raw access to a specified network device, directly
  33  *  with a socket.  Expected use involves a socket option to request
  34  *  protocol packets.  This lets ndrv_output() call dlil_output(), and
  35  *  lets DLIL find the proper recipient for incoming packets.
  36  *  The purpose here is for user-mode protocol implementation.
  37  * Note that "pure raw access" will still be accomplished with BPF.
  38  *
  39  * In addition to the former use, when combined with socket NKEs,
  40  * PF_NDRV permits a fairly flexible mechanism for implementing
  41  * strange protocol support.  One of the main ones will be the
  42  * BlueBox/Classic Shared IP Address support.
  43  */
  44 #include <mach/mach_types.h>
  45
  46 #include <sys/param.h>
  47 #include <sys/systm.h>
  48 #include <sys/kernel.h>
  49 #include <sys/malloc.h>
  50 #include <sys/mbuf.h>
  51 #include <sys/protosw.h>
  52 #include <sys/domain.h>
  53 #include <sys/socket.h>
  54 #include <sys/socketvar.h>
  55 #include <sys/ioctl.h>
  56 #include <sys/errno.h>
  57 #include <sys/syslog.h>
  58 #include <sys/proc.h>
  59
  60 #include <kern/queue.h>
  61
  62 #include <net/ndrv.h>
  63 #include <net/netisr.h>
  64 #include <net/route.h>
  65 #include <net/if_llc.h>
  66 #include <net/if_dl.h>
  67 #include <net/if_types.h>
  68 #include <net/ndrv_var.h>
  69
  70 #if INET
  71 #include <netinet/in.h>
  72 #include <netinet/in_var.h>
  73 #endif
  74 #include <netinet/if_ether.h>
  75
  76 #include <machine/spl.h>
  77
  78 int ndrv_do_detach(struct ndrv_cb *);
  79 int ndrv_do_disconnect(struct ndrv_cb *);
  80 struct ndrv_cb *ndrv_find_tag(unsigned int);
  81 void ndrv_read_event(struct socket* inSo, caddr_t ref, int waitf);
  82 int ndrv_setspec(struct ndrv_cb *np, struct sockopt *sopt);
  83 int ndrv_delspec(struct ndrv_cb *);
  84 int ndrv_to_dlil_demux(struct ndrv_demux_desc* ndrv, struct dlil_demux_desc* dlil);
  85 void ndrv_handle_ifp_detach(u_long family, short unit);
  86 static int ndrv_do_add_multicast(struct ndrv_cb *np, struct sockopt *sopt);
  87 static int ndrv_do_remove_multicast(struct ndrv_cb *np, struct sockopt *sopt);
  88 static struct ndrv_multiaddr* ndrv_have_multicast(struct ndrv_cb *np, struct sockaddr* addr);
  89 static void ndrv_remove_all_multicast(struct ndrv_cb *np);
  90
  91 unsigned long  ndrv_sendspace = NDRVSNDQ;
  92 unsigned long  ndrv_recvspace = NDRVRCVQ;
  93 struct ndrv_cb ndrvl;           /* Head of controlblock list */
  94
  95 struct domain ndrvdomain;
  96 struct protosw ndrvsw;
  97 static struct socket* ndrv_so;
  98
  99
 100 /*
 101  * Protocol init function for NDRV protocol
 102  * Init the control block list.
 103  */
 104 void
 105 ndrv_init()
 106 {
 107     int retval;
 108     struct kev_request kev_request;
 109
 110         ndrvl.nd_next = ndrvl.nd_prev = &ndrvl;
 111
 112     /* Create a PF_SYSTEM socket so we can listen for events */
 113     retval = socreate(PF_SYSTEM, &ndrv_so, SOCK_RAW, SYSPROTO_EVENT);
 114     if (retval != 0 || ndrv_so == NULL)
 115         retval = KERN_FAILURE;
 116
 117     /* Install a callback function for the socket */
 118     ndrv_so->so_rcv.sb_flags |= SB_NOTIFY|SB_UPCALL;
 119     ndrv_so->so_upcall = ndrv_read_event;
 120     ndrv_so->so_upcallarg = NULL;
 121
 122     /* Configure the socket to receive the events we're interested in */
 123     kev_request.vendor_code = KEV_VENDOR_APPLE;
 124     kev_request.kev_class = KEV_NETWORK_CLASS;
 125     kev_request.kev_subclass = KEV_DL_SUBCLASS;
 126     retval = ndrv_so->so_proto->pr_usrreqs->pru_control(ndrv_so, SIOCSKEVFILT, (caddr_t)&kev_request, 0, 0);
 127     if (retval != 0)
 128     {
 129         /*
 130          * We will not get attaching or detaching events in this case.
 131          * We should probably prevent any sockets from binding so we won't
 132          * panic later if the interface goes away.
 133          */
 134         log(LOG_WARNING, "PF_NDRV: ndrv_init - failed to set event filter (%d)",
 135             retval);
 136     }
 137 }
 138
 139 /*
 140  * Protocol output - Called to output a raw network packet directly
 141  *  to the driver.
 142  */
 143 int
 144 ndrv_output(register struct mbuf *m, register struct socket *so)
 145 {
 146     register struct ndrv_cb *np = sotondrvcb(so);
 147         register struct ifnet *ifp = np->nd_if;
 148         extern void kprintf(const char *, ...);
 149     int result = 0;
 150
 151 #if NDRV_DEBUG
 152         kprintf("NDRV output: %x, %x, %x\n", m, so, np);
 153 #endif
 154
 155         /*
 156          * No header is a format error
 157          */
 158         if ((m->m_flags&M_PKTHDR) == 0)
 159                 return(EINVAL);
 160
 161         /*
 162      * Call DLIL if we can. DLIL is much safer than calling the
 163      * ifp directly.
 164      */
 165     if (np->nd_tag != 0)
 166         result = dlil_output(np->nd_tag, m, (caddr_t)NULL,
 167                             (struct sockaddr*)NULL, 1);
 168     else if (np->nd_send_tag != 0)
 169         result = dlil_output(np->nd_send_tag, m, (caddr_t)NULL,
 170                             (struct sockaddr*)NULL, 1);
 171     else
 172         result = ENXIO;
 173         return (result);
 174 }
 175
 176 /* Our input routine called from DLIL */
 177 int
 178 ndrv_input(struct mbuf *m,
 179            char *frame_header,
 180            struct ifnet *ifp,
 181            u_long  dl_tag,
 182            int sync_ok)
 183 {
 184         struct socket *so;
 185         struct sockaddr_dl ndrvsrc = {sizeof (struct sockaddr_dl), AF_NDRV};
 186         register struct ndrv_cb *np;
 187
 188
 189     /* move packet from if queue to socket */
 190         /* Should be media-independent */
 191     ndrvsrc.sdl_type = IFT_ETHER;
 192     ndrvsrc.sdl_nlen = 0;
 193     ndrvsrc.sdl_alen = 6;
 194     ndrvsrc.sdl_slen = 0;
 195     bcopy(frame_header, &ndrvsrc.sdl_data, 6);
 196
 197         np = ndrv_find_tag(dl_tag);
 198         if (np == NULL)
 199         {
 200                 return(ENOENT);
 201         }
 202         so = np->nd_socket;
 203     /* prepend the frame header */
 204     m = m_prepend(m, ifp->if_data.ifi_hdrlen, M_NOWAIT);
 205     if (m == NULL)
 206         return EJUSTRETURN;
 207     bcopy(frame_header, m->m_data, ifp->if_data.ifi_hdrlen);
 208         if (sbappendaddr(&(so->so_rcv), (struct sockaddr *)&ndrvsrc,
 209                          m, (struct mbuf *)0) == 0)
 210         {
 211         /* yes, sbappendaddr returns zero if the sockbuff is full... */
 212         /* caller will free m */
 213                 return(ENOMEM);
 214         } else
 215                 sorwakeup(so);
 216         return(0);
 217 }
 218
 219 int
 220 ndrv_control(struct socket *so, u_long cmd, caddr_t data,
 221                   struct ifnet *ifp, struct proc *p)
 222 {
 223         return (0);
 224 }
 225
 226 /*
 227  * Allocate an ndrv control block and some buffer space for the socket
 228  */
 229 int
 230 ndrv_attach(struct socket *so, int proto, struct proc *p)
 231 {
 232     int error;
 233         register struct ndrv_cb *np = sotondrvcb(so);
 234
 235         if ((so->so_state & SS_PRIV) == 0)
 236                 return(EPERM);
 237
 238 #if NDRV_DEBUG
 239         kprintf("NDRV attach: %x, %x, %x\n", so, proto, np);
 240 #endif
 241
 242         if ((error = soreserve(so, ndrv_sendspace, ndrv_recvspace)))
 243                 return(error);
 244
 245         MALLOC(np, struct ndrv_cb *, sizeof(*np), M_PCB, M_WAITOK);
 246         if (np == NULL)
 247                 return (ENOMEM);
 248     so->so_pcb = (caddr_t)np;
 249     bzero(np, sizeof(*np));
 250 #if NDRV_DEBUG
 251         kprintf("NDRV attach: %x, %x, %x\n", so, proto, np);
 252 #endif
 253         TAILQ_INIT(&np->nd_dlist);
 254         np->nd_signature = NDRV_SIGNATURE;
 255         np->nd_socket = so;
 256         np->nd_proto.sp_family = so->so_proto->pr_domain->dom_family;
 257         np->nd_proto.sp_protocol = proto;
 258     np->nd_if = NULL;
 259     np->nd_tag = 0;
 260     np->nd_family = 0;
 261     np->nd_unit = 0;
 262         insque((queue_t)np, (queue_t)&ndrvl);
 263         return(0);
 264 }
 265
 266 /*
 267  * Destroy state just before socket deallocation.
 268  * Flush data or not depending on the options.
 269  */
 270
 271 int
 272 ndrv_detach(struct socket *so)
 273 {
 274         register struct ndrv_cb *np = sotondrvcb(so);
 275
 276         if (np == 0)
 277                 return EINVAL;
 278         return ndrv_do_detach(np);
 279 }
 280
 281
 282 /*
 283  * If a socket isn't bound to a single address,
 284  * the ndrv input routine will hand it anything
 285  * within that protocol family (assuming there's
 286  * nothing else around it should go to).
 287  *
 288  * Don't expect this to be used.
 289  */
 290
 291 int ndrv_connect(struct socket *so, struct sockaddr *nam, struct proc *p)
 292 {
 293         register struct ndrv_cb *np = sotondrvcb(so);
 294     int result = 0;
 295
 296         if (np == 0)
 297                 return EINVAL;
 298
 299         if (np->nd_faddr)
 300                 return EISCONN;
 301
 302     /* Allocate memory to store the remote address */
 303     MALLOC(np->nd_faddr, struct sockaddr_ndrv*,
 304                 nam->sa_len, M_IFADDR, M_WAITOK);
 305     if (result != 0)
 306         return result;
 307     if (np->nd_faddr == NULL)
 308         return ENOMEM;
 309
 310         bcopy((caddr_t) nam, (caddr_t) np->nd_faddr, nam->sa_len);
 311         soisconnected(so);
 312         return 0;
 313 }
 314
 315 /*
 316  * This is the "driver open" hook - we 'bind' to the
 317  *  named driver.
 318  * Here's where we latch onto the driver.
 319  */
 320 int
 321 ndrv_bind(struct socket *so, struct sockaddr *nam, struct proc *p)
 322 {
 323     register struct sockaddr_ndrv *sa = (struct sockaddr_ndrv *) nam;
 324         register char *dname;
 325         register struct ndrv_cb *np;
 326         register struct ifnet *ifp;
 327         extern int name_cmp(struct ifnet *, char *);
 328     int result;
 329
 330         if TAILQ_EMPTY(&ifnet)
 331                 return(EADDRNOTAVAIL); /* Quick sanity check */
 332         np = sotondrvcb(so);
 333         if (np == 0)
 334                 return EINVAL;
 335
 336         if (np->nd_laddr)
 337                 return EINVAL;                  /* XXX */
 338
 339         /* I think we just latch onto a copy here; the caller frees */
 340         np->nd_laddr = _MALLOC(sizeof(struct sockaddr_ndrv), M_IFADDR, M_WAITOK);
 341         if (np->nd_laddr == NULL)
 342                 return(ENOMEM);
 343         bcopy((caddr_t) sa, (caddr_t) np->nd_laddr, sizeof(struct sockaddr_ndrv));
 344         dname = sa->snd_name;
 345         if (*dname == '\0')
 346                 return(EINVAL);
 347 #if NDRV_DEBUG
 348         kprintf("NDRV bind: %x, %x, %s\n", so, np, dname);
 349 #endif
 350         /* Track down the driver and its ifnet structure.
 351          * There's no internal call for this so we have to dup the code
 352          *  in if.c/ifconf()
 353          */
 354         TAILQ_FOREACH(ifp, &ifnet, if_link) {
 355                 if (name_cmp(ifp, dname) == 0)
 356                         break;
 357         }
 358
 359         if (ifp == NULL)
 360                 return(EADDRNOTAVAIL);
 361
 362     /*
 363      * Loopback demuxing doesn't work with PF_NDRV.
 364      * The first 4 bytes of the packet must be the
 365      * protocol ptr. Can't get that from userland.
 366      */
 367     if (ifp->if_family == APPLE_IF_FAM_LOOPBACK)
 368         return (ENOTSUP);
 369
 370     if ((dlil_find_dltag(ifp->if_family, ifp->if_unit,
 371                          PF_NDRV, &np->nd_send_tag) != 0) &&
 372         (ifp->if_family != APPLE_IF_FAM_PPP)) {
 373         /* NDRV isn't registered on this interface, lets change that */
 374         struct dlil_proto_reg_str       ndrv_proto;
 375         int     result = 0;
 376         bzero(&ndrv_proto, sizeof(ndrv_proto));
 377         TAILQ_INIT(&ndrv_proto.demux_desc_head);
 378
 379         ndrv_proto.interface_family = ifp->if_family;
 380         ndrv_proto.protocol_family = PF_NDRV;
 381         ndrv_proto.unit_number = ifp->if_unit;
 382
 383         result = dlil_attach_protocol(&ndrv_proto, &np->nd_send_tag);
 384
 385         /*
 386          * If the interface does not allow PF_NDRV to attach, we will
 387          * respect it's wishes. Sending will be disabled. No error is
 388          * returned because the client may later attach a real protocol
 389          * that the interface may accept.
 390          */
 391         if (result != 0)
 392             np->nd_send_tag = 0;
 393     }
 394
 395         np->nd_if = ifp;
 396     np->nd_family = ifp->if_family;
 397     np->nd_unit = ifp->if_unit;
 398
 399         return(0);
 400 }
 401
 402 int
 403 ndrv_disconnect(struct socket *so)
 404 {
 405         register struct ndrv_cb *np = sotondrvcb(so);
 406
 407         if (np == 0)
 408                 return EINVAL;
 409
 410         if (np->nd_faddr == 0)
 411                 return ENOTCONN;
 412
 413         ndrv_do_disconnect(np);
 414         return 0;
 415 }
 416
 417 /*
 418  * Accessor function
 419  */
 420 struct ifnet*
 421 ndrv_get_ifp(caddr_t ndrv_pcb)
 422 {
 423     struct ndrv_cb*     np = (struct ndrv_cb*)ndrv_pcb;
 424
 425 #if DEBUG
 426     {
 427         struct ndrv_cb* temp = ndrvl.nd_next;
 428         /* Verify existence of pcb */
 429         for (temp = ndrvl.nd_next; temp != &ndrvl; temp = temp->nd_next)
 430         {
 431             if (temp == np)
 432                 break;
 433         }
 434
 435         if (temp != np)
 436         {
 437             log(LOG_WARNING, "PF_NDRV: ndrv_get_ifp called with invalid ndrv_cb!");
 438             return NULL;
 439         }
 440     }
 441 #endif
 442
 443     return np->nd_if;
 444 }
 445
 446 /*
 447  * Mark the connection as being incapable of further input.
 448  */
 449 int
 450 ndrv_shutdown(struct socket *so)
 451 {
 452         socantsendmore(so);
 453         return 0;
 454 }
 455
 456 /*
 457  * Ship a packet out.  The ndrv output will pass it
 458  *  to the appropriate driver.  The really tricky part
 459  *  is the destination address...
 460  */
 461 int
 462 ndrv_send(struct socket *so, int flags, struct mbuf *m,
 463           struct sockaddr *addr, struct mbuf *control,
 464           struct proc *p)
 465 {
 466         int error;
 467
 468         if (control)
 469                 return EOPNOTSUPP;
 470
 471         error = ndrv_output(m, so);
 472         m = NULL;
 473         return error;
 474 }
 475
 476
 477 int
 478 ndrv_abort(struct socket *so)
 479 {
 480         register struct ndrv_cb *np = sotondrvcb(so);
 481
 482         if (np == 0)
 483                 return EINVAL;
 484
 485         ndrv_do_disconnect(np);
 486         return 0;
 487 }
 488
 489 int
 490 ndrv_sense(struct socket *so, struct stat *sb)
 491 {
 492         /*
 493          * stat: don't bother with a blocksize.
 494          */
 495         return (0);
 496 }
 497
 498 int
 499 ndrv_sockaddr(struct socket *so, struct sockaddr **nam)
 500 {
 501         register struct ndrv_cb *np = sotondrvcb(so);
 502         int len;
 503
 504         if (np == 0)
 505                 return EINVAL;
 506
 507         if (np->nd_laddr == 0)
 508                 return EINVAL;
 509
 510         len = np->nd_laddr->snd_len;
 511         bcopy((caddr_t)np->nd_laddr, *nam,
 512               (unsigned)len);
 513         return 0;
 514 }
 515
 516
 517 int
 518 ndrv_peeraddr(struct socket *so, struct sockaddr **nam)
 519 {
 520         register struct ndrv_cb *np = sotondrvcb(so);
 521         int len;
 522
 523         if (np == 0)
 524                 return EINVAL;
 525
 526         if (np->nd_faddr == 0)
 527                 return ENOTCONN;
 528
 529         len = np->nd_faddr->snd_len;
 530         bcopy((caddr_t)np->nd_faddr, *nam,
 531               (unsigned)len);
 532         return 0;
 533 }
 534
 535
 536 /* Control input */
 537
 538 void
 539 ndrv_ctlinput(int dummy1, struct sockaddr *dummy2, void *dummy3)
 540 {
 541 }
 542
 543 /* Control output */
 544
 545 int
 546 ndrv_ctloutput(struct socket *so, struct sockopt *sopt)
 547 {
 548     register struct ndrv_cb *np = sotondrvcb(so);
 549         int error = 0;
 550
 551     switch(sopt->sopt_name)
 552     {
 553         case NDRV_DELDMXSPEC: /* Delete current spec */
 554             /* Verify no parameter was passed */
 555             if (sopt->sopt_val != NULL || sopt->sopt_valsize != 0) {
 556                 /*
 557                  * We don't support deleting a specific demux, it's
 558                  * all or nothing.
 559                  */
 560                 return EINVAL;
 561             }
 562             error = ndrv_delspec(np);
 563             break;
 564         case NDRV_SETDMXSPEC: /* Set protocol spec */
 565             error = ndrv_setspec(np, sopt);
 566             break;
 567         case NDRV_ADDMULTICAST:
 568             error = ndrv_do_add_multicast(np, sopt);
 569             break;
 570         case NDRV_DELMULTICAST:
 571             error = ndrv_do_remove_multicast(np, sopt);
 572             break;
 573         default:
 574             error = ENOTSUP;
 575     }
 576 #ifdef NDRV_DEBUG
 577         log(LOG_WARNING, "NDRV CTLOUT: %x returns %d\n", sopt->sopt_name,
 578             error);
 579 #endif
 580         return(error);
 581 }
 582
 583 /* Drain the queues */
 584 void
 585 ndrv_drain()
 586 {
 587 }
 588
 589 /* Sysctl hook for NDRV */
 590 int
 591 ndrv_sysctl()
 592 {
 593         return(0);
 594 }
 595
 596 int
 597 ndrv_do_detach(register struct ndrv_cb *np)
 598 {
 599     struct ndrv_cb*     cur_np = NULL;
 600     struct socket *so = np->nd_socket;
 601     struct ndrv_multicast*      next;
 602     int error = 0;
 603
 604 #if NDRV_DEBUG
 605         kprintf("NDRV detach: %x, %x\n", so, np);
 606 #endif
 607     ndrv_remove_all_multicast(np);
 608
 609     if (np->nd_tag != 0)
 610     {
 611         error = dlil_detach_protocol(np->nd_tag);
 612         if (error)
 613         {
 614             log(LOG_WARNING, "NDRV ndrv_do_detach: error %d removing dl_tag %d",
 615                 error, np->nd_tag);
 616             return error;
 617         }
 618     }
 619
 620     /* Remove from the linked list of control blocks */
 621         remque((queue_t)np);
 622
 623     if (np->nd_send_tag != 0)
 624     {
 625         /* Check if this is the last socket attached to this interface */
 626         for (cur_np = ndrvl.nd_next; cur_np != &ndrvl; cur_np = cur_np->nd_next)
 627         {
 628             if (cur_np->nd_family == np->nd_family &&
 629                 cur_np->nd_unit == np->nd_unit)
 630             {
 631                 break;
 632             }
 633         }
 634
 635         /* If there are no other interfaces, detach PF_NDRV from the interface */
 636         if (cur_np == &ndrvl)
 637         {
 638             dlil_detach_protocol(np->nd_send_tag);
 639         }
 640     }
 641
 642         FREE((caddr_t)np, M_PCB);
 643         so->so_pcb = 0;
 644         sofree(so);
 645         return error;
 646 }
 647
 648 int
 649 ndrv_do_disconnect(register struct ndrv_cb *np)
 650 {
 651 #if NDRV_DEBUG
 652         kprintf("NDRV disconnect: %x\n", np);
 653 #endif
 654         if (np->nd_faddr)
 655         {
 656         FREE(np->nd_faddr, M_IFADDR);
 657                 np->nd_faddr = 0;
 658         }
 659         if (np->nd_socket->so_state & SS_NOFDREF)
 660                 ndrv_do_detach(np);
 661         soisdisconnected(np->nd_socket);
 662         return(0);
 663 }
 664
 665 /*
 666  * Try to compare a device name (q) with one of the funky ifnet
 667  *  device names (ifp).
 668  */
 669 int name_cmp(register struct ifnet *ifp, register char *q)
 670 {       register char *r;
 671         register int len;
 672         char buf[IFNAMSIZ];
 673         static char *sprint_d();
 674
 675         r = buf;
 676         len = strlen(ifp->if_name);
 677         strncpy(r, ifp->if_name, IFNAMSIZ);
 678         r += len;
 679         (void)sprint_d(ifp->if_unit, r, IFNAMSIZ-(r-buf));
 680 #if NDRV_DEBUG
 681         kprintf("Comparing %s, %s\n", buf, q);
 682 #endif
 683         return(strncmp(buf, q, IFNAMSIZ));
 684 }
 685
 686 /* Hackery - return a string version of a decimal number */
 687 static char *
 688 sprint_d(n, buf, buflen)
 689         u_int n;
 690         char *buf;
 691         int buflen;
 692 {       char dbuf[IFNAMSIZ];
 693         register char *cp = dbuf+IFNAMSIZ-1;
 694
 695         *cp = 0;
 696         do {    buflen--;
 697                 cp--;
 698                 *cp = "0123456789"[n % 10];
 699                 n /= 10;
 700         } while (n != 0 && buflen > 0);
 701         strncpy(buf, cp, IFNAMSIZ-buflen);
 702         return (cp);
 703 }
 704
 705 /*
 706  * When closing, dump any enqueued mbufs.
 707  */
 708 void
 709 ndrv_flushq(register struct ifqueue *q)
 710 {
 711     register struct mbuf *m;
 712         for (;;)
 713         {
 714                 IF_DEQUEUE(q, m);
 715                 if (m == NULL)
 716                         break;
 717                 IF_DROP(q);
 718                 if (m)
 719                         m_freem(m);
 720         }
 721 }
 722
 723 int
 724 ndrv_setspec(struct ndrv_cb *np, struct sockopt *sopt)
 725 {
 726     struct dlil_proto_reg_str   dlilSpec;
 727     struct ndrv_protocol_desc   ndrvSpec;
 728     struct dlil_demux_desc*             dlilDemux = NULL;
 729     struct ndrv_demux_desc*             ndrvDemux = NULL;
 730     int                                                 error = 0;
 731
 732     /* Sanity checking */
 733     if (np->nd_tag)
 734         return EBUSY;
 735     if (np->nd_if == NULL)
 736         return EINVAL;
 737     if (sopt->sopt_valsize != sizeof(struct ndrv_protocol_desc))
 738         return EINVAL;
 739
 740     /* Copy the ndrvSpec */
 741     error = sooptcopyin(sopt, &ndrvSpec, sizeof(struct ndrv_protocol_desc),
 742                         sizeof(struct ndrv_protocol_desc));
 743     if (error != 0)
 744         return error;
 745
 746     /* Verify the parameter */
 747     if (ndrvSpec.version > NDRV_PROTOCOL_DESC_VERS)
 748         return ENOTSUP; // version is too new!
 749     else if (ndrvSpec.version < 1)
 750         return EINVAL; // version is not valid
 751
 752     /* Allocate storage for demux array */
 753     MALLOC(ndrvDemux, struct ndrv_demux_desc*,
 754             ndrvSpec.demux_count * sizeof(struct ndrv_demux_desc), M_TEMP, M_WAITOK);
 755     if (ndrvDemux == NULL)
 756         return ENOMEM;
 757
 758     /* Allocate enough dlil_demux_descs */
 759     MALLOC(dlilDemux, struct dlil_demux_desc*,
 760             sizeof(*dlilDemux) * ndrvSpec.demux_count, M_TEMP, M_WAITOK);
 761     if (dlilDemux == NULL)
 762         error = ENOMEM;
 763
 764     if (error == 0)
 765     {
 766         /* Copy the ndrv demux array from userland */
 767         error = copyin(ndrvSpec.demux_list, ndrvDemux,
 768                     ndrvSpec.demux_count * sizeof(struct ndrv_demux_desc));
 769         ndrvSpec.demux_list = ndrvDemux;
 770     }
 771
 772     if (error == 0)
 773     {
 774         /* At this point, we've at least got enough bytes to start looking around */
 775         u_long  demuxOn = 0;
 776
 777         bzero(&dlilSpec, sizeof(dlilSpec));
 778         TAILQ_INIT(&dlilSpec.demux_desc_head);
 779         dlilSpec.interface_family = np->nd_family;
 780         dlilSpec.unit_number = np->nd_unit;
 781         dlilSpec.input = ndrv_input;
 782         dlilSpec.protocol_family = ndrvSpec.protocol_family;
 783
 784         for (demuxOn = 0; demuxOn < ndrvSpec.demux_count; demuxOn++)
 785         {
 786             /* Convert an ndrv_demux_desc to a dlil_demux_desc */
 787             error = ndrv_to_dlil_demux(&ndrvSpec.demux_list[demuxOn], &dlilDemux[demuxOn]);
 788             if (error)
 789                 break;
 790
 791             /* Add the dlil_demux_desc to the list */
 792             TAILQ_INSERT_TAIL(&dlilSpec.demux_desc_head, &dlilDemux[demuxOn], next);
 793         }
 794     }
 795
 796     if (error == 0)
 797     {
 798         /* We've got all our ducks lined up...lets attach! */
 799         error = dlil_attach_protocol(&dlilSpec, &np->nd_tag);
 800     }
 801
 802     /* Free any memory we've allocated */
 803     if (dlilDemux)
 804         FREE(dlilDemux, M_TEMP);
 805     if (ndrvDemux)
 806         FREE(ndrvDemux, M_TEMP);
 807
 808     return error;
 809 }
 810
 811
 812 int
 813 ndrv_to_dlil_demux(struct ndrv_demux_desc* ndrv, struct dlil_demux_desc* dlil)
 814 {
 815     bzero(dlil, sizeof(*dlil));
 816
 817     if (ndrv->type < DLIL_DESC_ETYPE2)
 818     {
 819         /* using old "type", not supported */
 820         return ENOTSUP;
 821     }
 822
 823     if (ndrv->length > 28)
 824     {
 825         return EINVAL;
 826     }
 827
 828     dlil->type = ndrv->type;
 829     dlil->native_type = ndrv->data.other;
 830     dlil->variants.native_type_length = ndrv->length;
 831
 832     return 0;
 833 }
 834
 835 int
 836 ndrv_delspec(struct ndrv_cb *np)
 837 {
 838     int result = 0;
 839
 840     if (np->nd_tag == 0)
 841         return EINVAL;
 842
 843     /* Detach the protocol */
 844     result = dlil_detach_protocol(np->nd_tag);
 845     if (result == 0)
 846     {
 847         np->nd_tag = 0;
 848     }
 849
 850         return result;
 851 }
 852
 853 struct ndrv_cb *
 854 ndrv_find_tag(unsigned int tag)
 855 {
 856     struct ndrv_cb* np;
 857         int i;
 858
 859     if (tag == 0)
 860         return NULL;
 861
 862     for (np = ndrvl.nd_next; np != NULL; np = np->nd_next)
 863     {
 864         if (np->nd_tag == tag)
 865         {
 866             return np;
 867         }
 868     }
 869
 870         return NULL;
 871 }
 872
 873 void ndrv_dominit()
 874 {
 875         static int ndrv_dominited = 0;
 876
 877         if (ndrv_dominited == 0 &&
 878             net_add_proto(&ndrvsw, &ndrvdomain) == 0)
 879                 ndrv_dominited = 1;
 880 }
 881
 882 void
 883 ndrv_read_event(struct socket* so, caddr_t ref, int waitf)
 884 {
 885     // Read an event
 886     struct mbuf *m = NULL;
 887     struct kern_event_msg *msg;
 888     struct uio auio = {0};
 889     int result = 0;
 890     int flags = 0;
 891
 892     // Get the data
 893     auio.uio_resid = 1000000; // large number to get all of the data
 894     flags = MSG_DONTWAIT;
 895     result = soreceive(so, (struct sockaddr**)NULL, &auio, &m,
 896         (struct mbuf**)NULL, &flags);
 897     if (result != 0 || m == NULL)
 898         return;
 899
 900     // cast the mbuf to a kern_event_msg
 901     // this is dangerous, doesn't handle linked mbufs
 902     msg = mtod(m, struct kern_event_msg*);
 903
 904     // check for detaches, assume even filtering is working
 905     if (msg->event_code == KEV_DL_IF_DETACHING ||
 906         msg->event_code == KEV_DL_IF_DETACHED)
 907     {
 908         struct net_event_data *ev_data;
 909         ev_data = (struct net_event_data*)msg->event_data;
 910         ndrv_handle_ifp_detach(ev_data->if_family, ev_data->if_unit);
 911     }
 912
 913     m_free(m);
 914 }
 915
 916 void
 917 ndrv_handle_ifp_detach(u_long family, short unit)
 918 {
 919     struct ndrv_cb* np;
 920     u_long                      dl_tag;
 921
 922     /* Find all sockets using this interface. */
 923     for (np = ndrvl.nd_next; np != &ndrvl; np = np->nd_next)
 924     {
 925         if (np->nd_family == family &&
 926             np->nd_unit == unit)
 927         {
 928             /* This cb is using the detaching interface, but not for long. */
 929             /* Let the protocol go */
 930             if (np->nd_tag != 0)
 931                 ndrv_delspec(np);
 932
 933             /* Delete the multicasts first */
 934             ndrv_remove_all_multicast(np);
 935
 936             /* Disavow all knowledge of the ifp */
 937             np->nd_if = NULL;
 938             np->nd_unit = 0;
 939             np->nd_family = 0;
 940             np->nd_send_tag = 0;
 941
 942             /* Make sure sending returns an error */
 943             /* Is this safe? Will we drop the funnel? */
 944             socantsendmore(np->nd_socket);
 945             socantrcvmore(np->nd_socket);
 946         }
 947     }
 948
 949     /* Unregister our protocol */
 950     if (dlil_find_dltag(family, unit, PF_NDRV, &dl_tag) == 0) {
 951         dlil_detach_protocol(dl_tag);
 952     }
 953 }
 954
 955 static int
 956 ndrv_do_add_multicast(struct ndrv_cb *np, struct sockopt *sopt)
 957 {
 958     struct ndrv_multiaddr*      ndrv_multi;
 959     int                                         result;
 960
 961     if (sopt->sopt_val == NULL || sopt->sopt_valsize < 2 ||
 962         sopt->sopt_level != SOL_NDRVPROTO)
 963         return EINVAL;
 964     if (np->nd_if == NULL)
 965         return ENXIO;
 966
 967     // Allocate storage
 968     MALLOC(ndrv_multi, struct ndrv_multiaddr*, sizeof(struct ndrv_multiaddr) -
 969         sizeof(struct sockaddr) + sopt->sopt_valsize, M_IFADDR, M_WAITOK);
 970     if (ndrv_multi == NULL)
 971         return ENOMEM;
 972
 973     // Copy in the address
 974     result = copyin(sopt->sopt_val, &ndrv_multi->addr, sopt->sopt_valsize);
 975
 976     // Validate the sockaddr
 977     if (result == 0 && sopt->sopt_valsize != ndrv_multi->addr.sa_len)
 978         result = EINVAL;
 979
 980     if (result == 0 && ndrv_have_multicast(np, &ndrv_multi->addr))
 981         result = EEXIST;
 982
 983     if (result == 0)
 984     {
 985         // Try adding the multicast
 986         result = if_addmulti(np->nd_if, &ndrv_multi->addr, NULL);
 987     }
 988
 989     if (result == 0)
 990     {
 991         // Add to our linked list
 992         ndrv_multi->next = np->nd_multiaddrs;
 993         np->nd_multiaddrs = ndrv_multi;
 994     }
 995     else
 996     {
 997         // Free up the memory, something went wrong
 998         FREE(ndrv_multi, M_IFADDR);
 999     }
1000
1001     return result;
1002 }
1003
1004 static int
1005 ndrv_do_remove_multicast(struct ndrv_cb *np, struct sockopt *sopt)
1006 {
1007     struct sockaddr*            multi_addr;
1008     struct ndrv_multiaddr*      ndrv_entry = NULL;
1009     int                                 result;
1010
1011     if (sopt->sopt_val == NULL || sopt->sopt_valsize < 2 ||
1012         sopt->sopt_level != SOL_NDRVPROTO)
1013         return EINVAL;
1014     if (np->nd_if == NULL)
1015         return ENXIO;
1016
1017     // Allocate storage
1018     MALLOC(multi_addr, struct sockaddr*, sopt->sopt_valsize,
1019             M_TEMP, M_WAITOK);
1020     if (multi_addr == NULL)
1021         return ENOMEM;
1022
1023     // Copy in the address
1024     result = copyin(sopt->sopt_val, multi_addr, sopt->sopt_valsize);
1025
1026     // Validate the sockaddr
1027     if (result == 0 && sopt->sopt_valsize != multi_addr->sa_len)
1028         result = EINVAL;
1029
1030     if (result == 0)
1031     {
1032         /* Find the old entry */
1033         ndrv_entry = ndrv_have_multicast(np, multi_addr);
1034
1035         if (ndrv_entry == NULL)
1036             result = ENOENT;
1037     }
1038
1039     if (result == 0)
1040     {
1041         // Try deleting the multicast
1042         result = if_delmulti(np->nd_if, &ndrv_entry->addr);
1043     }
1044
1045     if (result == 0)
1046     {
1047         // Remove from our linked list
1048         struct ndrv_multiaddr*  cur = np->nd_multiaddrs;
1049
1050         if (cur == ndrv_entry)
1051         {
1052             np->nd_multiaddrs = cur->next;
1053         }
1054         else
1055         {
1056             for (cur = cur->next; cur != NULL; cur = cur->next)
1057             {
1058                 if (cur->next == ndrv_entry)
1059                 {
1060                     cur->next = cur->next->next;
1061                     break;
1062                 }
1063             }
1064         }
1065
1066         // Free the memory
1067         FREE(ndrv_entry, M_IFADDR);
1068     }
1069     FREE(multi_addr, M_TEMP);
1070
1071     return result;
1072 }
1073
1074 static struct ndrv_multiaddr*
1075 ndrv_have_multicast(struct ndrv_cb *np, struct sockaddr* inAddr)
1076 {
1077     struct ndrv_multiaddr*      cur;
1078     for (cur = np->nd_multiaddrs; cur != NULL; cur = cur->next)
1079     {
1080
1081         if ((inAddr->sa_len == cur->addr.sa_len) &&
1082             (bcmp(&cur->addr, inAddr, inAddr->sa_len) == 0))
1083         {
1084             // Found a match
1085             return cur;
1086         }
1087     }
1088
1089     return NULL;
1090 }
1091
1092 static void
1093 ndrv_remove_all_multicast(struct ndrv_cb* np)
1094 {
1095     struct ndrv_multiaddr*      cur;
1096
1097     if (np->nd_if != NULL)
1098     {
1099         while (np->nd_multiaddrs != NULL)
1100         {
1101             cur = np->nd_multiaddrs;
1102             np->nd_multiaddrs = cur->next;
1103
1104             if_delmulti(np->nd_if, &cur->addr);
1105             FREE(cur, M_IFADDR);
1106         }
1107     }
1108 }
1109
1110 struct pr_usrreqs ndrv_usrreqs = {
1111         ndrv_abort, pru_accept_notsupp, ndrv_attach, ndrv_bind,
1112         ndrv_connect, pru_connect2_notsupp, ndrv_control, ndrv_detach,
1113         ndrv_disconnect, pru_listen_notsupp, ndrv_peeraddr, pru_rcvd_notsupp,
1114         pru_rcvoob_notsupp, ndrv_send, ndrv_sense, ndrv_shutdown,
1115         ndrv_sockaddr, sosend, soreceive, sopoll
1116 };
1117
1118 struct protosw ndrvsw =
1119 {       SOCK_RAW, &ndrvdomain, NDRVPROTO_NDRV, PR_ATOMIC|PR_ADDR,
1120     0, ndrv_output, ndrv_ctlinput, ndrv_ctloutput,
1121     0, ndrv_init, 0, 0,
1122     ndrv_drain, ndrv_sysctl, &ndrv_usrreqs
1123 };
1124
1125 struct domain ndrvdomain =
1126 {       AF_NDRV, "NetDriver", ndrv_dominit, NULL, NULL,
1127         NULL,
1128         NULL, NULL, 0, 0, 0, 0
1129 };