tools/tools/netrate/pktgen/pktgen.c

   1 /*
   2  * Copyright (c) 2012 The DragonFly Project.  All rights reserved.
   3  *
   4  * This code is derived from software contributed to The DragonFly Project
   5  * by Sepherosa Ziehau <sepherosa@gmail.com>
   6  *
   7  * Redistribution and use in source and binary forms, with or without
   8  * modification, are permitted provided that the following conditions
   9  * are met:
  10  *
  11  * 1. Redistributions of source code must retain the above copyright
  12  *    notice, this list of conditions and the following disclaimer.
  13  * 2. Redistributions in binary form must reproduce the above copyright
  14  *    notice, this list of conditions and the following disclaimer in
  15  *    the documentation and/or other materials provided with the
  16  *    distribution.
  17  * 3. Neither the name of The DragonFly Project nor the names of its
  18  *    contributors may be used to endorse or promote products derived
  19  *    from this software without specific, prior written permission.
  20  *
  21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE
  25  * COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  26  * INCIDENTAL, SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING,
  27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
  29  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  30  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
  31  * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  32  * SUCH DAMAGE.
  33  *
  34  * $DragonFly: src/tools/tools/netrate/pktgen/pktgen.c,v 1.4 2008/04/02 14:18:55 sephe Exp $
  35  */
  36
  37 #define _IP_VHL
  38
  39 #include <sys/param.h>
  40 #include <sys/conf.h>
  41 #include <sys/device.h>
  42 #include <sys/in_cksum.h>
  43 #include <sys/kernel.h>
  44 #include <sys/malloc.h>
  45 #include <sys/mbuf.h>
  46 #include <sys/proc.h>
  47 #include <sys/priv.h>
  48 #include <sys/queue.h>
  49 #include <sys/socket.h>
  50 #include <sys/systm.h>
  51 #include <sys/serialize.h>
  52
  53 #include <net/if.h>
  54 #include <net/if_dl.h>
  55 #include <net/if_var.h>
  56 #include <net/ifq_var.h>
  57 #include <net/ethernet.h>
  58 #include <net/netmsg2.h>
  59
  60 #include <netinet/in.h>
  61 #include <netinet/ip.h>
  62 #include <netinet/udp_var.h>
  63
  64 #include "pktgen.h"
  65
  66 #define CDEV_NAME       "pktg"
  67 #define CDEV_MAJOR      151
  68
  69 #define PKTGEN_BUFSZ    2048
  70
  71 struct pktgen;
  72
  73 struct pktgen_buf {
  74         struct netmsg_base      pb_nmsg;        /* MUST BE THE FIRST */
  75         void                    *pb_buf;
  76         volatile int            pb_done;
  77         volatile int            pb_inuse;
  78         struct ifnet            *pb_ifp;
  79         int                     pb_len;
  80         int                     pb_cpuid;
  81         struct pktgen           *pb_pktg;
  82         LIST_ENTRY(pktgen_buf)  pb_link;
  83 };
  84
  85 struct pktgen {
  86         uint32_t                pktg_flags;     /* PKTG_F_ */
  87         int                     pktg_refcnt;
  88
  89         struct callout          pktg_stop;
  90         int                     pktg_duration;
  91
  92         int                     pktg_datalen;
  93         struct ifnet            *pktg_ifp;
  94
  95         struct sockaddr_in      pktg_src;
  96         int                     pktg_ndst;
  97         struct sockaddr_in      *pktg_dst;
  98         uint8_t                 pktg_dst_lladdr[ETHER_ADDR_LEN];
  99
 100         LIST_HEAD(, pktgen_buf) pktg_buflist;
 101 };
 102
 103 #define PKTG_F_CONFIG   0x1
 104 #define PKTG_F_RUNNING  0x4
 105
 106 static int              pktgen_modevent(module_t, int, void *);
 107
 108 static void             pktgen_buf_free(void *);
 109 static void             pktgen_buf_ref(void *);
 110 static void             pktgen_buf_send(netmsg_t);
 111
 112 static int              pktgen_config(struct pktgen *,
 113                             const struct pktgen_conf *);
 114 static int              pktgen_start(struct pktgen *);
 115 static void             pktgen_free(struct pktgen *);
 116 static void             pktgen_stop_cb(void *);
 117 static void             pktgen_mbuf(struct pktgen_buf *, struct mbuf *);
 118
 119 static d_open_t         pktgen_open;
 120 static d_close_t        pktgen_close;
 121 static d_ioctl_t        pktgen_ioctl;
 122
 123 static struct dev_ops   pktgen_ops = {
 124         { CDEV_NAME, CDEV_MAJOR, 0 },
 125         .d_open =       pktgen_open,
 126         .d_close =      pktgen_close,
 127         .d_ioctl =      pktgen_ioctl,
 128 };
 129
 130 static int              pktgen_refcnt;
 131 static struct lwkt_token pktgen_tok = LWKT_TOKEN_INITIALIZER(pktgen_token);
 132
 133 MALLOC_DECLARE(M_PKTGEN);
 134 MALLOC_DEFINE(M_PKTGEN, CDEV_NAME, "Packet generator");
 135
 136 DEV_MODULE(pktgen, pktgen_modevent, NULL);
 137
 138 static int
 139 pktgen_modevent(module_t mod, int type, void *data)
 140 {
 141         int error = 0;
 142
 143         switch (type) {
 144         case MOD_LOAD:
 145                 make_dev(&pktgen_ops, 0, UID_ROOT, GID_WHEEL, 0600,
 146                     CDEV_NAME"%d", 0);
 147                 break;
 148
 149         case MOD_UNLOAD:
 150                 if (pktgen_refcnt > 0)
 151                         return EBUSY;
 152                 dev_ops_remove_all(&pktgen_ops);
 153                 break;
 154
 155         default:
 156                 error = EOPNOTSUPP;
 157                 break;
 158         }
 159         return error;
 160 }
 161
 162 static int
 163 pktgen_open(struct dev_open_args *ap)
 164 {
 165         cdev_t dev = ap->a_head.a_dev;
 166         struct pktgen *pktg;
 167         int error;
 168
 169         error = priv_check_cred(ap->a_cred, PRIV_ROOT, 0);
 170         if (error)
 171                 return error;
 172
 173         lwkt_gettoken(&pktgen_tok);
 174
 175         if (dev->si_drv1 != NULL) {
 176                 lwkt_reltoken(&pktgen_tok);
 177                 return EBUSY;
 178         }
 179
 180         pktg = kmalloc(sizeof(*pktg), M_PKTGEN, M_ZERO | M_WAITOK);
 181         callout_init_mp(&pktg->pktg_stop);
 182         LIST_INIT(&pktg->pktg_buflist);
 183
 184         dev->si_drv1 = pktg;
 185         pktg->pktg_refcnt = 1;
 186
 187         pktgen_refcnt++;
 188
 189         lwkt_reltoken(&pktgen_tok);
 190         return 0;
 191 }
 192
 193 static int
 194 pktgen_close(struct dev_close_args *ap)
 195 {
 196         cdev_t dev = ap->a_head.a_dev;
 197         struct pktgen *pktg = dev->si_drv1;
 198
 199         lwkt_gettoken(&pktgen_tok);
 200
 201         dev->si_drv1 = NULL;
 202         pktgen_free(pktg);
 203
 204         lwkt_reltoken(&pktgen_tok);
 205
 206         return 0;
 207 }
 208
 209 static int
 210 pktgen_ioctl(struct dev_ioctl_args *ap __unused)
 211 {
 212         cdev_t dev = ap->a_head.a_dev;
 213         caddr_t data = ap->a_data;
 214         struct pktgen *pktg = dev->si_drv1;
 215         int error;
 216
 217         lwkt_gettoken(&pktgen_tok);
 218
 219         switch (ap->a_cmd) {
 220         case PKTGENSTART:
 221                 error = pktgen_start(pktg);
 222                 break;
 223
 224         case PKTGENSCONF:
 225                 error = pktgen_config(pktg, (const struct pktgen_conf *)data);
 226                 break;
 227
 228         default:
 229                 error = EOPNOTSUPP;
 230                 break;
 231         }
 232
 233         lwkt_reltoken(&pktgen_tok);
 234         return error;
 235 }
 236
 237 static int
 238 pktgen_config(struct pktgen *pktg, const struct pktgen_conf *conf)
 239 {
 240         const struct sockaddr_in *sin;
 241         struct sockaddr_in *dst = NULL;
 242         const struct sockaddr *sa;
 243         struct ifnet *ifp;
 244         size_t dst_size;
 245         int i, error;
 246
 247         if (pktg->pktg_flags & (PKTG_F_RUNNING | PKTG_F_CONFIG))
 248                 return EBUSY;
 249
 250         if (conf->pc_datalen <= 0 ||
 251             conf->pc_datalen > ETHERMTU - sizeof(struct udpiphdr))
 252                 return EINVAL;
 253         if (conf->pc_duration <= 0)
 254                 return EINVAL;
 255
 256         sin = &conf->pc_src;
 257         if (sin->sin_family != AF_INET)
 258                 return EPROTONOSUPPORT;
 259         if (sin->sin_port == 0)
 260                 return EINVAL;
 261
 262         if (conf->pc_ndst <= 0)
 263                 return EINVAL;
 264         dst_size = conf->pc_ndst * sizeof(struct sockaddr_in);
 265
 266         dst = kmalloc(dst_size, M_PKTGEN, M_WAITOK | M_NULLOK);
 267         if (dst == NULL)
 268                 return ENOMEM;
 269
 270         error = copyin(conf->pc_dst, dst, dst_size);
 271         if (error)
 272                 goto failed;
 273
 274         for (i = 0; i < conf->pc_ndst; ++i) {
 275                 sin = &dst[i];
 276                 if (sin->sin_family != AF_INET) {
 277                         error = EPROTONOSUPPORT;
 278                         goto failed;
 279                 }
 280                 if (sin->sin_port == 0) {
 281                         error = EINVAL;
 282                         goto failed;
 283                 }
 284         }
 285
 286         ifp = ifunit(conf->pc_ifname);
 287         if (ifp == NULL) {
 288                 error = ENXIO;
 289                 goto failed;
 290         }
 291
 292         sa = &conf->pc_dst_lladdr;
 293         if (sa->sa_family != AF_LINK) {
 294                 error = EPROTONOSUPPORT;
 295                 goto failed;
 296         }
 297         if (sa->sa_len != ETHER_ADDR_LEN) {
 298                 error = EPROTONOSUPPORT;
 299                 goto failed;
 300         }
 301         if (ETHER_IS_MULTICAST(sa->sa_data) ||
 302             bcmp(sa->sa_data, ifp->if_broadcastaddr, ifp->if_addrlen) == 0) {
 303                 error = EADDRNOTAVAIL;
 304                 goto failed;
 305         }
 306
 307         /*
 308          * Accept the config
 309          */
 310         pktg->pktg_flags |= PKTG_F_CONFIG;
 311
 312         pktg->pktg_duration = conf->pc_duration;
 313         pktg->pktg_datalen = conf->pc_datalen;
 314         pktg->pktg_ifp = ifp;
 315         pktg->pktg_src = conf->pc_src;
 316         pktg->pktg_ndst = conf->pc_ndst;
 317         KKASSERT(pktg->pktg_dst == NULL);
 318         pktg->pktg_dst = dst;
 319         bcopy(sa->sa_data, pktg->pktg_dst_lladdr, ETHER_ADDR_LEN);
 320
 321         return 0;
 322
 323 failed:
 324         if (dst != NULL)
 325                 kfree(dst, M_PKTGEN);
 326         return error;
 327 }
 328
 329 static int
 330 pktgen_start(struct pktgen *pktg)
 331 {
 332         struct mbuf *m, *head = NULL, **next;
 333         struct ifnet *ifp;
 334         int cpuid, orig_cpuid, i, alloc_cnt, keep_cnt;
 335
 336         u_short ulen, psum;
 337         int len, ip_len;
 338
 339         if ((pktg->pktg_flags & PKTG_F_CONFIG) == 0)
 340                 return EINVAL;
 341         if (pktg->pktg_flags & PKTG_F_RUNNING)
 342                 return EBUSY;
 343         pktg->pktg_flags |= PKTG_F_RUNNING;
 344
 345         ifp = pktg->pktg_ifp;
 346
 347         orig_cpuid = mycpuid;
 348         cpuid = ifp->if_start_cpuid(ifp);
 349         if (cpuid != orig_cpuid)
 350                 lwkt_migratecpu(cpuid);
 351
 352         alloc_cnt = ifp->if_snd.ifq_maxlen * 2;
 353         keep_cnt = (ifp->if_snd.ifq_maxlen * 7) / 8;
 354
 355         /*
 356          * Prefault enough mbuf into mbuf objcache
 357          */
 358         next = &head;
 359         for (i = 0; i < alloc_cnt; ++i) {
 360                 MGETHDR(m, MB_WAIT, MT_DATA);
 361                 *next = m;
 362                 next = &m->m_nextpkt;
 363         }
 364
 365         for (i = 0; i < alloc_cnt - keep_cnt; ++i) {
 366                 m = head;
 367                 head = m->m_nextpkt;
 368                 m->m_nextpkt = NULL;
 369                 m_freem(m);
 370         }
 371         KKASSERT(head != NULL);
 372
 373         /*
 374          * Setup the packets' data
 375          */
 376         ip_len = pktg->pktg_datalen + sizeof(struct udpiphdr);
 377         len = ip_len + ETHER_HDR_LEN;
 378
 379         psum = htons((u_short)pktg->pktg_datalen + sizeof(struct udphdr) +
 380             IPPROTO_UDP);
 381         ulen = htons(pktg->pktg_datalen + sizeof(struct udphdr));
 382
 383         m = head;
 384         i = 0;
 385         while (m != NULL) {
 386                 struct mbuf *nextm;
 387                 const struct sockaddr_in *dst;
 388                 struct pktgen_buf *pb;
 389                 struct ip *ip;
 390                 struct udpiphdr *ui;
 391                 struct ether_header *eh;
 392
 393                 pktg->pktg_refcnt++;
 394                 pktgen_refcnt++;
 395
 396                 pb = kmalloc(sizeof(*pb), M_PKTGEN, M_WAITOK | M_ZERO);
 397                 pb->pb_ifp = ifp;
 398                 pb->pb_inuse = 1;
 399                 pb->pb_buf = kmalloc(PKTGEN_BUFSZ, M_PKTGEN, M_WAITOK);
 400                 pb->pb_len = len;
 401                 pb->pb_cpuid = cpuid;
 402                 pb->pb_pktg = pktg;
 403                 netmsg_init(&pb->pb_nmsg, NULL, &netisr_adone_rport, 0,
 404                     pktgen_buf_send);
 405                 LIST_INSERT_HEAD(&pktg->pktg_buflist, pb, pb_link);
 406
 407                 dst = &pktg->pktg_dst[i % pktg->pktg_ndst];
 408                 ++i;
 409
 410                 m->m_ext.ext_arg = pb;
 411                 m->m_ext.ext_buf = pb->pb_buf;
 412                 m->m_ext.ext_free = pktgen_buf_free;
 413                 m->m_ext.ext_ref = pktgen_buf_ref;
 414                 m->m_ext.ext_size = PKTGEN_BUFSZ;
 415
 416                 m->m_data = m->m_ext.ext_buf;
 417                 m->m_flags |= M_EXT;
 418                 m->m_len = m->m_pkthdr.len = len;
 419
 420                 m->m_data += ETHER_HDR_LEN;
 421                 m->m_len -= ETHER_HDR_LEN;
 422                 m->m_pkthdr.len -= ETHER_HDR_LEN;
 423
 424                 ui = mtod(m, struct udpiphdr *);
 425                 ui->ui_pr = IPPROTO_UDP;
 426                 ui->ui_src.s_addr = pktg->pktg_src.sin_addr.s_addr;
 427                 ui->ui_dst.s_addr = dst->sin_addr.s_addr;
 428                 ui->ui_sport = pktg->pktg_src.sin_port;
 429                 ui->ui_dport = dst->sin_port;
 430                 ui->ui_ulen = ulen;
 431                 ui->ui_sum = in_pseudo(ui->ui_src.s_addr, ui->ui_dst.s_addr,
 432                     psum);
 433                 m->m_pkthdr.csum_data = offsetof(struct udphdr, uh_sum);
 434
 435                 ip = (struct ip *)ui;
 436                 ip->ip_len = ip_len;
 437                 ip->ip_ttl = 64;        /* XXX */
 438                 ip->ip_tos = 0;         /* XXX */
 439                 ip->ip_vhl = IP_VHL_BORING;
 440                 ip->ip_off = 0;
 441                 ip->ip_id = ip_newid();
 442
 443                 in_delayed_cksum(m);
 444
 445                 ip->ip_len = htons(ip->ip_len);
 446                 ip->ip_sum = in_cksum_hdr(ip);
 447
 448                 m->m_data -= ETHER_HDR_LEN;
 449                 m->m_len += ETHER_HDR_LEN;
 450                 m->m_pkthdr.len += ETHER_HDR_LEN;
 451
 452                 eh = mtod(m, struct ether_header *);
 453                 bcopy(pktg->pktg_dst_lladdr, eh->ether_dhost, ETHER_ADDR_LEN);
 454                 bcopy(IF_LLADDR(ifp), eh->ether_shost, ETHER_ADDR_LEN);
 455                 eh->ether_type = htons(ETHERTYPE_IP);
 456
 457                 nextm = m->m_nextpkt;
 458                 m->m_nextpkt = NULL;
 459
 460                 ifq_dispatch(ifp, m, NULL);
 461
 462                 m = nextm;
 463         }
 464
 465         callout_reset(&pktg->pktg_stop, pktg->pktg_duration * hz,
 466             pktgen_stop_cb, pktg);
 467
 468         if (cpuid != orig_cpuid)
 469                 lwkt_migratecpu(orig_cpuid);
 470
 471         return 0;
 472 }
 473
 474 static void
 475 pktgen_mbuf(struct pktgen_buf *pb, struct mbuf *m)
 476 {
 477         m->m_ext.ext_arg = pb;
 478         m->m_ext.ext_buf = pb->pb_buf;
 479         m->m_ext.ext_free = pktgen_buf_free;
 480         m->m_ext.ext_ref = pktgen_buf_ref;
 481         m->m_ext.ext_size = PKTGEN_BUFSZ;
 482
 483         m->m_data = m->m_ext.ext_buf;
 484         m->m_flags |= M_EXT;
 485         m->m_len = m->m_pkthdr.len = pb->pb_len;
 486 }
 487
 488 static void
 489 pktgen_buf_send(netmsg_t msg)
 490 {
 491         struct pktgen_buf *pb = (struct pktgen_buf *)msg;
 492         struct mbuf *m;
 493
 494         KKASSERT(&curthread->td_msgport == netisr_portfn(pb->pb_cpuid));
 495
 496         crit_enter();
 497         lwkt_replymsg(&pb->pb_nmsg.lmsg, 0);
 498         crit_exit();
 499
 500         MGETHDR(m, MB_WAIT, MT_DATA);
 501         pktgen_mbuf(pb, m);
 502         ifq_dispatch(pb->pb_ifp, m, NULL);
 503 }
 504
 505 static void
 506 pktgen_buf_free(void *arg)
 507 {
 508         struct pktgen_buf *pb = arg;
 509         struct mbuf *m;
 510
 511         KKASSERT(pb->pb_inuse > 0);
 512         if (pb->pb_done) {
 513                 if (atomic_fetchadd_int(&pb->pb_inuse, -1) == 1) {
 514                         struct pktgen *pktg;
 515
 516                         lwkt_gettoken(&pktgen_tok);
 517
 518                         pktg = pb->pb_pktg;
 519                         LIST_REMOVE(pb, pb_link);
 520                         kfree(pb->pb_buf, M_PKTGEN);
 521                         kfree(pb, M_PKTGEN);
 522
 523                         pktgen_free(pktg);
 524
 525                         lwkt_reltoken(&pktgen_tok);
 526                 }
 527                 return;
 528         }
 529
 530         if (&curthread->td_msgport != netisr_portfn(pb->pb_cpuid)) {
 531                 KKASSERT(pb->pb_cpuid == mycpuid);
 532                 crit_enter();
 533                 KKASSERT(pb->pb_nmsg.lmsg.ms_flags & MSGF_DONE);
 534                 lwkt_sendmsg(netisr_portfn(pb->pb_cpuid), &pb->pb_nmsg.lmsg);
 535                 crit_exit();
 536                 return;
 537         }
 538
 539         MGETHDR(m, MB_WAIT, MT_DATA);
 540         pktgen_mbuf(pb, m);
 541         ifq_enqueue(&pb->pb_ifp->if_snd, m, NULL);
 542 }
 543
 544 static void
 545 pktgen_buf_ref(void *arg)
 546 {
 547         struct pktgen_buf *pb = arg;
 548
 549         panic("%s should never be called\n", __func__);
 550
 551         KKASSERT(pb->pb_inuse > 0);
 552         atomic_add_int(&pb->pb_inuse, 1);
 553 }
 554
 555 static void
 556 pktgen_free(struct pktgen *pktg)
 557 {
 558         KKASSERT(pktg->pktg_refcnt > 0);
 559         if (--pktg->pktg_refcnt == 0) {
 560                 if (pktg->pktg_dst != NULL)
 561                         kfree(pktg->pktg_dst, M_PKTGEN);
 562                 KKASSERT(LIST_EMPTY(&pktg->pktg_buflist));
 563                 kfree(pktg, M_PKTGEN);
 564         }
 565
 566         KKASSERT(pktgen_refcnt > 0);
 567         pktgen_refcnt--;
 568 }
 569
 570 static void
 571 pktgen_stop_cb(void *arg)
 572 {
 573         struct pktgen *pktg = arg;
 574         struct pktgen_buf *pb;
 575
 576         lwkt_gettoken(&pktgen_tok);
 577         LIST_FOREACH(pb, &pktg->pktg_buflist, pb_link)
 578                 pb->pb_done = 1;
 579         lwkt_reltoken(&pktgen_tok);
 580 }