tools/tools/netrate/pktgen/pktgen.c

   1 /*
   2  * Copyright (c) 2008 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.3 2008/04/02 13:27:24 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/ioccom.h>
  43 #include <sys/in_cksum.h>
  44 #include <sys/kernel.h>
  45 #include <sys/malloc.h>
  46 #include <sys/mbuf.h>
  47 #include <sys/proc.h>
  48 #include <sys/socket.h>
  49 #include <sys/systm.h>
  50 #include <sys/serialize.h>
  51
  52 #include <net/if.h>
  53 #include <net/if_dl.h>
  54 #include <net/if_var.h>
  55 #include <net/ifq_var.h>
  56 #include <net/ethernet.h>
  57
  58 #include <netinet/in.h>
  59 #include <netinet/ip.h>
  60 #include <netinet/udp_var.h>
  61
  62 #include "pktgen.h"
  63
  64 #define CDEV_NAME       "pktg"
  65 #define CDEV_MAJOR      151
  66
  67 struct pktgen {
  68         uint32_t                pktg_flags;     /* PKTG_F_ */
  69         int                     pktg_refcnt;
  70
  71         struct callout          pktg_stop;
  72         int                     pktg_duration;
  73         int                     pktg_cpuid;
  74         void                    (*pktg_thread)(void *);
  75
  76         int                     pktg_datalen;
  77         int                     pktg_yield;
  78         struct ifnet            *pktg_ifp;
  79
  80         in_addr_t               pktg_saddr;     /* host byte order */
  81         in_addr_t               pktg_daddr;     /* host byte order */
  82         u_short                 pktg_sport;     /* host byte order */
  83         u_short                 pktg_dport;     /* host byte order */
  84
  85         int                     pktg_nsaddr;
  86         int                     pktg_ndaddr;
  87         int                     pktg_nsport;
  88         int                     pktg_ndport;
  89
  90         uint8_t                 pktg_dst_lladdr[ETHER_ADDR_LEN];
  91 };
  92
  93 #define PKTG_F_CONFIG   0x1
  94 #define PKTG_F_STOP     0x2
  95 #define PKTG_F_RUNNING  0x4
  96
  97 static int              pktgen_modevent(module_t, int, void *);
  98 static int              pktgen_config(struct pktgen *,
  99                                       const struct pktgen_conf *);
 100 static int              pktgen_start(struct pktgen *, int);
 101 static void             pktgen_stop_cb(void *);
 102 static void             pktgen_udp_thread(void *);
 103 static void             pktgen_udp_thread1(void *);
 104
 105 static d_open_t         pktgen_open;
 106 static d_close_t        pktgen_close;
 107 static d_ioctl_t        pktgen_ioctl;
 108
 109 static struct dev_ops   pktgen_ops = {
 110         { CDEV_NAME, CDEV_MAJOR, 0 },
 111         .d_open =       pktgen_open,
 112         .d_close =      pktgen_close,
 113         .d_ioctl =      pktgen_ioctl,
 114 };
 115
 116 static int              pktgen_refcnt;
 117
 118 MALLOC_DECLARE(M_PKTGEN);
 119 MALLOC_DEFINE(M_PKTGEN, CDEV_NAME, "Packet generator");
 120
 121 DEV_MODULE(pktgen, pktgen_modevent, NULL);
 122
 123 static int
 124 pktgen_modevent(module_t mod, int type, void *data)
 125 {
 126         int error = 0;
 127
 128         switch (type) {
 129         case MOD_LOAD:
 130                 dev_ops_add(&pktgen_ops, 0, 0);
 131                 break;
 132
 133         case MOD_UNLOAD:
 134                 if (pktgen_refcnt > 0)
 135                         return EBUSY;
 136                 dev_ops_remove(&pktgen_ops, 0, 0);
 137                 break;
 138
 139         default:
 140                 error = EOPNOTSUPP;
 141                 break;
 142         }
 143         return error;
 144 }
 145
 146 static int
 147 pktgen_open(struct dev_open_args *ap)
 148 {
 149         cdev_t dev = ap->a_head.a_dev;
 150         struct pktgen *pktg;
 151         int error;
 152
 153         error = suser_cred(ap->a_cred, 0);
 154         if (error)
 155                 return error;
 156
 157         get_mplock();
 158
 159         if (dev->si_drv1 != NULL) {
 160                 rel_mplock();
 161                 return EBUSY;
 162         }
 163
 164         pktg = kmalloc(sizeof(*pktg), M_PKTGEN, M_ZERO | M_WAITOK);
 165         callout_init(&pktg->pktg_stop);
 166
 167         dev = make_dev(&pktgen_ops, minor(dev), UID_ROOT, GID_WHEEL, 0600,
 168                        CDEV_NAME "%d", lminor(dev));
 169         dev->si_drv1 = pktg;
 170         pktg->pktg_refcnt = 1;
 171
 172         pktgen_refcnt++;
 173
 174         rel_mplock();
 175         return 0;
 176 }
 177
 178 static int
 179 pktgen_close(struct dev_close_args *ap)
 180 {
 181         cdev_t dev = ap->a_head.a_dev;
 182         struct pktgen *pktg = dev->si_drv1;
 183
 184         get_mplock();
 185
 186         KKASSERT(pktg->pktg_refcnt > 0);
 187         if (--pktg->pktg_refcnt == 0)
 188                 kfree(pktg, M_PKTGEN);
 189         dev->si_drv1 = NULL;
 190
 191         KKASSERT(pktgen_refcnt > 0);
 192         pktgen_refcnt--;
 193
 194         rel_mplock();
 195         return 0;
 196 }
 197
 198 static int
 199 pktgen_ioctl(struct dev_ioctl_args *ap __unused)
 200 {
 201         cdev_t dev = ap->a_head.a_dev;
 202         caddr_t data = ap->a_data;
 203         struct pktgen *pktg = dev->si_drv1;
 204         int error;
 205
 206         get_mplock();
 207
 208         switch (ap->a_cmd) {
 209         case PKTGENSTART:
 210                 error = pktgen_start(pktg, minor(dev));
 211                 break;
 212
 213         case PKTGENSCONF:
 214                 error = pktgen_config(pktg, (const struct pktgen_conf *)data);
 215                 break;
 216
 217         default:
 218                 error = EOPNOTSUPP;
 219                 break;
 220         }
 221
 222         rel_mplock();
 223         return error;
 224 }
 225
 226 static int
 227 pktgen_config(struct pktgen *pktg, const struct pktgen_conf *conf)
 228 {
 229         const struct sockaddr_in *sin;
 230         const struct sockaddr *sa;
 231         struct ifnet *ifp;
 232         int yield, nsaddr, ndaddr, nsport, ndport, thread1;
 233
 234         if (pktg->pktg_flags & PKTG_F_RUNNING)
 235                 return EBUSY;
 236
 237         if (conf->pc_cpuid < 0 || conf->pc_cpuid >= ncpus)
 238                 return EINVAL;
 239         if (conf->pc_datalen <= 0)
 240                 return EINVAL;
 241         if (conf->pc_duration <= 0)
 242                 return EINVAL;
 243
 244         yield = conf->pc_yield;
 245         if (yield <= 0)
 246                 yield = PKTGEN_YIELD_DEFAULT;
 247
 248         if (conf->pc_nsaddr <= 0 && conf->pc_ndaddr <= 0 &&
 249             conf->pc_nsport <= 0 && conf->pc_ndport <= 0)
 250                 thread1 = 0;
 251         else
 252                 thread1 = 1;
 253
 254         nsaddr = conf->pc_nsaddr;
 255         if (nsaddr <= 0)
 256                 nsaddr = 1;
 257         ndaddr = conf->pc_ndaddr;
 258         if (ndaddr <= 0)
 259                 ndaddr = 1;
 260
 261         nsport = conf->pc_nsport;
 262         if (nsport <= 0)
 263                 nsport = 1;
 264         ndport = conf->pc_ndport;
 265         if (ndport <= 0)
 266                 ndport = 1;
 267
 268         ifp = ifunit(conf->pc_ifname);
 269         if (ifp == NULL)
 270                 return ENXIO;
 271
 272         sa = &conf->pc_dst_lladdr;
 273         if (sa->sa_family != AF_LINK)
 274                 return EPROTONOSUPPORT;
 275         if (sa->sa_len != ETHER_ADDR_LEN)
 276                 return EPROTONOSUPPORT;
 277         if (ETHER_IS_MULTICAST(sa->sa_data) ||
 278             bcmp(sa->sa_data, ifp->if_broadcastaddr, ifp->if_addrlen) == 0)
 279                 return EADDRNOTAVAIL;
 280
 281         sin = &conf->pc_src;
 282         if (sin->sin_family != AF_INET)
 283                 return EPROTONOSUPPORT;
 284         if (sin->sin_port == 0)
 285                 return EINVAL;
 286
 287         sin = &conf->pc_dst;
 288         if (sin->sin_family != AF_INET)
 289                 return EPROTONOSUPPORT;
 290         if (sin->sin_port == 0)
 291                 return EINVAL;
 292
 293         /* Accept the config */
 294         pktg->pktg_flags |= PKTG_F_CONFIG;
 295         pktg->pktg_refcnt++;
 296         pktgen_refcnt++;
 297
 298         pktg->pktg_duration = conf->pc_duration;
 299         pktg->pktg_cpuid = conf->pc_cpuid;
 300         pktg->pktg_ifp = ifp;
 301         pktg->pktg_datalen = conf->pc_datalen;
 302         pktg->pktg_yield = yield;
 303         bcopy(sa->sa_data, pktg->pktg_dst_lladdr, ETHER_ADDR_LEN);
 304
 305         pktg->pktg_saddr = ntohl(conf->pc_src.sin_addr.s_addr);
 306         pktg->pktg_daddr = ntohl(conf->pc_dst.sin_addr.s_addr);
 307         pktg->pktg_nsaddr = nsaddr;
 308         pktg->pktg_ndaddr = ndaddr;
 309
 310         pktg->pktg_sport = ntohs(conf->pc_src.sin_port);
 311         pktg->pktg_dport = ntohs(conf->pc_dst.sin_port);
 312         pktg->pktg_nsport = nsport;
 313         pktg->pktg_ndport = ndport;
 314
 315         pktg->pktg_thread = thread1 ? pktgen_udp_thread1 : pktgen_udp_thread;
 316
 317         return 0;
 318 }
 319
 320 static int
 321 pktgen_start(struct pktgen *pktg, int m)
 322 {
 323         if ((pktg->pktg_flags & PKTG_F_CONFIG) == 0)
 324                 return EINVAL;
 325         if (pktg->pktg_flags & PKTG_F_RUNNING)
 326                 return EBUSY;
 327
 328         pktg->pktg_flags |= PKTG_F_RUNNING;
 329
 330         lwkt_create(pktg->pktg_thread, pktg, NULL, NULL, 0,
 331                     pktg->pktg_cpuid, "pktgen %d", m);
 332         return 0;
 333 }
 334
 335 static void
 336 pktgen_stop_cb(void *arg)
 337 {
 338         struct pktgen *pktg = arg;
 339
 340         pktg->pktg_flags |= PKTG_F_STOP;
 341 }
 342
 343 static void
 344 pktgen_udp_thread1(void *arg)
 345 {
 346         struct pktgen *pktg = arg;
 347         struct ifnet *ifp = pktg->pktg_ifp;
 348         struct ip *ip;
 349         struct udpiphdr *ui;
 350         struct ether_header *eh;
 351         struct mbuf *m;
 352         u_short ulen, psum;
 353         int len, ip_len;
 354         int sw_csum, csum_flags;
 355         int loop, r, error;
 356         uint64_t err_cnt, cnt;
 357         in_addr_t saddr, daddr;
 358         u_short sport, dport;
 359         struct timeval start, end;
 360
 361         rel_mplock();   /* Don't need MP lock */
 362
 363         callout_reset(&pktg->pktg_stop, pktg->pktg_duration * hz,
 364                       pktgen_stop_cb, pktg);
 365
 366         cnt = err_cnt = 0;
 367         r = loop = 0;
 368
 369         ip_len = pktg->pktg_datalen + sizeof(*ui);
 370         len = ip_len + ETHER_HDR_LEN;
 371
 372         psum = htons((u_short)pktg->pktg_datalen + sizeof(struct udphdr)
 373                      + IPPROTO_UDP);
 374         ulen = htons(pktg->pktg_datalen + sizeof(struct udphdr));
 375
 376         sw_csum = (CSUM_UDP | CSUM_IP) & ~ifp->if_hwassist;
 377         csum_flags = (CSUM_UDP | CSUM_IP) & ifp->if_hwassist;
 378
 379         saddr = pktg->pktg_saddr;
 380         daddr = pktg->pktg_daddr;
 381         sport = pktg->pktg_sport;
 382         dport = pktg->pktg_dport;
 383
 384         microtime(&start);
 385         while ((pktg->pktg_flags & PKTG_F_STOP) == 0) {
 386                 m = m_getl(len, MB_WAIT, MT_DATA, M_PKTHDR, NULL);
 387                 m->m_len = m->m_pkthdr.len = len;
 388
 389                 m_adj(m, ETHER_HDR_LEN);
 390
 391                 ui = mtod(m, struct udpiphdr *);
 392                 ui->ui_pr = IPPROTO_UDP;
 393                 ui->ui_src.s_addr = htonl(saddr);
 394                 ui->ui_dst.s_addr = htonl(daddr);
 395                 ui->ui_sport = htons(sport);
 396                 ui->ui_dport = htons(dport);
 397                 ui->ui_ulen = ulen;
 398                 ui->ui_sum = in_pseudo(ui->ui_src.s_addr,
 399                                        ui->ui_dst.s_addr, psum);
 400                 m->m_pkthdr.csum_flags = (CSUM_IP | CSUM_UDP);
 401                 m->m_pkthdr.csum_data = offsetof(struct udphdr, uh_sum);
 402
 403                 ip = (struct ip *)ui;
 404                 ip->ip_len = ip_len;
 405                 ip->ip_ttl = 64;        /* XXX */
 406                 ip->ip_tos = 0;         /* XXX */
 407                 ip->ip_vhl = IP_VHL_BORING;
 408                 ip->ip_off = 0;
 409                 ip->ip_id = ip_newid();
 410
 411                 if (sw_csum & CSUM_DELAY_DATA)
 412                         in_delayed_cksum(m);
 413                 m->m_pkthdr.csum_flags = csum_flags;
 414
 415                 ip->ip_len = htons(ip->ip_len);
 416                 ip->ip_sum = 0;
 417                 if (sw_csum & CSUM_DELAY_IP)
 418                         ip->ip_sum = in_cksum_hdr(ip);
 419
 420                 M_PREPEND(m, ETHER_HDR_LEN, MB_WAIT);
 421                 eh = mtod(m, struct ether_header *);
 422                 bcopy(pktg->pktg_dst_lladdr, eh->ether_dhost, ETHER_ADDR_LEN);
 423                 bcopy(IF_LLADDR(ifp), eh->ether_shost, ETHER_ADDR_LEN);
 424                 eh->ether_type = htons(ETHERTYPE_IP);
 425
 426                 lwkt_serialize_enter(ifp->if_serializer);
 427                 error = ifq_handoff(ifp, m, NULL);
 428                 lwkt_serialize_exit(ifp->if_serializer);
 429
 430                 loop++;
 431                 if (error) {
 432                         err_cnt++;
 433                         loop = 0;
 434                         lwkt_yield();
 435                 } else {
 436                         cnt++;
 437                         if (loop == pktg->pktg_yield) {
 438                                 loop = 0;
 439                                 lwkt_yield();
 440                         }
 441
 442                         r++;
 443                         saddr = pktg->pktg_saddr + (r % pktg->pktg_nsaddr);
 444                         daddr = pktg->pktg_daddr + (r % pktg->pktg_ndaddr);
 445                         sport = pktg->pktg_sport + (r % pktg->pktg_nsport);
 446                         dport = pktg->pktg_dport + (r % pktg->pktg_ndport);
 447                 }
 448         }
 449         microtime(&end);
 450
 451         timevalsub(&end, &start);
 452         kprintf("cnt %llu, err %llu, time %ld.%ld\n", cnt, err_cnt,
 453                 end.tv_sec, end.tv_usec);
 454
 455         pktg->pktg_flags &= ~(PKTG_F_STOP | PKTG_F_CONFIG | PKTG_F_RUNNING);
 456
 457         KKASSERT(pktg->pktg_refcnt > 0);
 458         if (--pktg->pktg_refcnt == 0)
 459                 kfree(pktg, M_PKTGEN);  /* XXX */
 460
 461         KKASSERT(pktgen_refcnt > 0);
 462         pktgen_refcnt--;
 463
 464         lwkt_exit();
 465 }
 466
 467 static void
 468 pktgen_udp_thread(void *arg)
 469 {
 470         struct pktgen *pktg = arg;
 471         struct ifnet *ifp = pktg->pktg_ifp;
 472         struct ip *ip;
 473         struct udpiphdr *ui;
 474         struct ether_header *eh;
 475         struct mbuf *m;
 476         u_short ulen, sum;
 477         int len, ip_len;
 478         int sw_csum, csum_flags;
 479         int loop, error;
 480         uint64_t err_cnt, cnt;
 481         in_addr_t saddr, daddr;
 482         u_short sport, dport;
 483         struct timeval start, end;
 484
 485         rel_mplock();   /* Don't need MP lock */
 486
 487         callout_reset(&pktg->pktg_stop, pktg->pktg_duration * hz,
 488                       pktgen_stop_cb, pktg);
 489
 490         cnt = err_cnt = 0;
 491         loop = 0;
 492
 493         ip_len = pktg->pktg_datalen + sizeof(*ui);
 494         len = ip_len + ETHER_HDR_LEN;
 495
 496         saddr = htonl(pktg->pktg_saddr);
 497         daddr = htonl(pktg->pktg_daddr);
 498         sport = htons(pktg->pktg_sport);
 499         dport = htons(pktg->pktg_dport);
 500
 501         sum = in_pseudo(saddr, daddr,
 502                 htons((u_short)pktg->pktg_datalen + sizeof(struct udphdr)
 503                 + IPPROTO_UDP));
 504         ulen = htons(pktg->pktg_datalen + sizeof(struct udphdr));
 505
 506         sw_csum = (CSUM_UDP | CSUM_IP) & ~ifp->if_hwassist;
 507         csum_flags = (CSUM_UDP | CSUM_IP) & ifp->if_hwassist;
 508
 509         microtime(&start);
 510         while ((pktg->pktg_flags & PKTG_F_STOP) == 0) {
 511                 m = m_getl(len, MB_WAIT, MT_DATA, M_PKTHDR, NULL);
 512                 m->m_len = m->m_pkthdr.len = len;
 513
 514                 m_adj(m, ETHER_HDR_LEN);
 515
 516                 ui = mtod(m, struct udpiphdr *);
 517                 ui->ui_pr = IPPROTO_UDP;
 518                 ui->ui_src.s_addr = saddr;
 519                 ui->ui_dst.s_addr = daddr;
 520                 ui->ui_sport = sport;
 521                 ui->ui_dport = dport;
 522                 ui->ui_ulen = ulen;
 523                 ui->ui_sum = sum;
 524                 m->m_pkthdr.csum_flags = (CSUM_IP | CSUM_UDP);
 525                 m->m_pkthdr.csum_data = offsetof(struct udphdr, uh_sum);
 526
 527                 ip = (struct ip *)ui;
 528                 ip->ip_len = ip_len;
 529                 ip->ip_ttl = 64;        /* XXX */
 530                 ip->ip_tos = 0;         /* XXX */
 531                 ip->ip_vhl = IP_VHL_BORING;
 532                 ip->ip_off = 0;
 533                 ip->ip_id = ip_newid();
 534
 535                 if (sw_csum & CSUM_DELAY_DATA)
 536                         in_delayed_cksum(m);
 537                 m->m_pkthdr.csum_flags = csum_flags;
 538
 539                 ip->ip_len = htons(ip->ip_len);
 540                 ip->ip_sum = 0;
 541                 if (sw_csum & CSUM_DELAY_IP)
 542                         ip->ip_sum = in_cksum_hdr(ip);
 543
 544                 M_PREPEND(m, ETHER_HDR_LEN, MB_WAIT);
 545                 eh = mtod(m, struct ether_header *);
 546                 bcopy(pktg->pktg_dst_lladdr, eh->ether_dhost, ETHER_ADDR_LEN);
 547                 bcopy(IF_LLADDR(ifp), eh->ether_shost, ETHER_ADDR_LEN);
 548                 eh->ether_type = htons(ETHERTYPE_IP);
 549
 550                 lwkt_serialize_enter(ifp->if_serializer);
 551                 error = ifq_handoff(ifp, m, NULL);
 552                 lwkt_serialize_exit(ifp->if_serializer);
 553
 554                 loop++;
 555                 if (error) {
 556                         err_cnt++;
 557                         loop = 0;
 558                         lwkt_yield();
 559                 } else {
 560                         cnt++;
 561                         if (loop == pktg->pktg_yield) {
 562                                 loop = 0;
 563                                 lwkt_yield();
 564                         }
 565                 }
 566         }
 567         microtime(&end);
 568
 569         timevalsub(&end, &start);
 570         kprintf("cnt %llu, err %llu, time %ld.%06ld\n", cnt, err_cnt,
 571                 end.tv_sec, end.tv_usec);
 572
 573         pktg->pktg_flags &= ~(PKTG_F_STOP | PKTG_F_CONFIG | PKTG_F_RUNNING);
 574
 575         KKASSERT(pktg->pktg_refcnt > 0);
 576         if (--pktg->pktg_refcnt == 0)
 577                 kfree(pktg, M_PKTGEN);  /* XXX */
 578
 579         KKASSERT(pktgen_refcnt > 0);
 580         pktgen_refcnt--;
 581
 582         lwkt_exit();
 583 }