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