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 #include <net/netisr2.h> 60 61 #include <netinet/in.h> 62 #include <netinet/ip.h> 63 #include <netinet/udp_var.h> 64 65 #include "pktgen.h" 66 67 #define CDEV_NAME "pktg" 68 69 #define PKTGEN_BUFSZ 2048 70 71 #ifndef PKTGEN_DEVCNT 72 #define PKTGEN_DEVCNT 4 73 #endif 74 75 struct pktgen; 76 77 struct netmsg_pktgen { 78 struct netmsg_base np_base; 79 struct pktgen *np_pktg; 80 struct ifaltq_subque *np_ifsq; 81 }; 82 83 struct pktgen_buf { 84 struct netmsg_base pb_nmsg; /* MUST BE THE FIRST */ 85 void *pb_buf; 86 volatile int pb_done; 87 int pb_inuse; 88 struct ifnet *pb_ifp; 89 struct ifaltq_subque *pb_ifsq; 90 int pb_len; 91 int pb_cpuid; 92 struct pktgen *pb_pktg; 93 LIST_ENTRY(pktgen_buf) pb_link; 94 }; 95 96 struct pktgen_pcpu { 97 struct callout pktg_stop; 98 LIST_HEAD(, pktgen_buf) pktg_buflist; 99 }; 100 101 struct pktgen { 102 uint32_t pktg_flags; /* PKTG_F_ */ 103 int pktg_refcnt; 104 105 int pktg_duration; 106 107 int pktg_datalen; 108 struct ifnet *pktg_ifp; 109 110 int pktg_pktenq; 111 112 struct sockaddr_in pktg_src; 113 int pktg_ndst; 114 struct sockaddr_in *pktg_dst; 115 uint8_t pktg_dst_lladdr[ETHER_ADDR_LEN]; 116 117 struct pktgen_pcpu pktg_pcpu[MAXCPU]; 118 }; 119 120 #define PKTG_F_CONFIG 0x1 121 #define PKTG_F_RUNNING 0x4 122 123 static int pktgen_modevent(module_t, int, void *); 124 125 static void pktgen_buf_free(void *); 126 static void pktgen_buf_ref(void *); 127 static void pktgen_buf_send(netmsg_t); 128 129 static int pktgen_config(struct pktgen *, 130 const struct pktgen_conf *); 131 static int pktgen_start(struct pktgen *, int); 132 static void pktgen_free(struct pktgen *); 133 static void pktgen_ref(struct pktgen *); 134 static void pktgen_pcpu_stop_cb(void *); 135 static void pktgen_mbuf(struct pktgen_buf *, struct mbuf *); 136 static void pktgen_start_ifsq(struct pktgen *, 137 struct ifaltq_subque *); 138 static void pktgen_start_ifsq_handler(netmsg_t); 139 140 static d_open_t pktgen_open; 141 static d_close_t pktgen_close; 142 static d_ioctl_t pktgen_ioctl; 143 144 static struct dev_ops pktgen_ops = { 145 { CDEV_NAME, 0, D_MPSAFE }, 146 .d_open = pktgen_open, 147 .d_close = pktgen_close, 148 .d_ioctl = pktgen_ioctl, 149 }; 150 151 static volatile int pktgen_refcnt; 152 static struct lwkt_token pktgen_tok = LWKT_TOKEN_INITIALIZER(pktgen_token); 153 154 MALLOC_DECLARE(M_PKTGEN); 155 MALLOC_DEFINE(M_PKTGEN, CDEV_NAME, "Packet generator"); 156 157 DEV_MODULE(pktgen, pktgen_modevent, NULL); 158 159 static int 160 pktgen_modevent(module_t mod, int type, void *data) 161 { 162 int error = 0, i; 163 164 switch (type) { 165 case MOD_LOAD: 166 for (i = 0; i < PKTGEN_DEVCNT; ++i) { 167 make_dev(&pktgen_ops, 0, UID_ROOT, GID_WHEEL, 0600, 168 CDEV_NAME"%d", i); 169 } 170 break; 171 172 case MOD_UNLOAD: 173 if (pktgen_refcnt > 0) 174 return EBUSY; 175 dev_ops_remove_all(&pktgen_ops); 176 break; 177 178 default: 179 error = EOPNOTSUPP; 180 break; 181 } 182 return error; 183 } 184 185 static int 186 pktgen_open(struct dev_open_args *ap) 187 { 188 cdev_t dev = ap->a_head.a_dev; 189 struct pktgen *pktg; 190 int error, i; 191 192 error = priv_check_cred(ap->a_cred, PRIV_ROOT, 0); 193 if (error) 194 return error; 195 196 lwkt_gettoken(&pktgen_tok); 197 198 if (dev->si_drv1 != NULL) { 199 lwkt_reltoken(&pktgen_tok); 200 return EBUSY; 201 } 202 203 pktg = kmalloc(sizeof(*pktg), M_PKTGEN, M_ZERO | M_WAITOK); 204 for (i = 0; i < ncpus; ++i) { 205 struct pktgen_pcpu *p = &pktg->pktg_pcpu[i]; 206 207 callout_init_mp(&p->pktg_stop); 208 LIST_INIT(&p->pktg_buflist); 209 } 210 211 dev->si_drv1 = pktg; 212 pktg->pktg_refcnt = 1; 213 214 atomic_add_int(&pktgen_refcnt, 1); 215 216 lwkt_reltoken(&pktgen_tok); 217 return 0; 218 } 219 220 static int 221 pktgen_close(struct dev_close_args *ap) 222 { 223 cdev_t dev = ap->a_head.a_dev; 224 struct pktgen *pktg = dev->si_drv1; 225 226 lwkt_gettoken(&pktgen_tok); 227 dev->si_drv1 = NULL; 228 lwkt_reltoken(&pktgen_tok); 229 230 pktgen_free(pktg); 231 232 return 0; 233 } 234 235 static int 236 pktgen_ioctl(struct dev_ioctl_args *ap __unused) 237 { 238 cdev_t dev = ap->a_head.a_dev; 239 caddr_t data = ap->a_data; 240 struct pktgen *pktg = dev->si_drv1; 241 int error; 242 243 lwkt_gettoken(&pktgen_tok); 244 245 switch (ap->a_cmd) { 246 case PKTGENSTART: 247 error = pktgen_start(pktg, 0); 248 break; 249 250 case PKTGENMQSTART: 251 error = pktgen_start(pktg, 1); 252 break; 253 254 case PKTGENSCONF: 255 error = pktgen_config(pktg, (const struct pktgen_conf *)data); 256 break; 257 258 default: 259 error = EOPNOTSUPP; 260 break; 261 } 262 263 lwkt_reltoken(&pktgen_tok); 264 return error; 265 } 266 267 static int 268 pktgen_config(struct pktgen *pktg, const struct pktgen_conf *conf) 269 { 270 const struct sockaddr_in *sin; 271 struct sockaddr_in *dst = NULL; 272 const struct sockaddr *sa; 273 struct ifnet *ifp; 274 size_t dst_size; 275 int i, error, pktenq; 276 277 if (pktg->pktg_flags & (PKTG_F_RUNNING | PKTG_F_CONFIG)) 278 return EBUSY; 279 280 if (conf->pc_datalen <= 0 || 281 conf->pc_datalen > ETHERMTU - sizeof(struct udpiphdr)) 282 return EINVAL; 283 if (conf->pc_duration <= 0) 284 return EINVAL; 285 286 sin = &conf->pc_src; 287 if (sin->sin_family != AF_INET) 288 return EPROTONOSUPPORT; 289 if (sin->sin_port == 0) 290 return EINVAL; 291 292 if (conf->pc_ndst <= 0) 293 return EINVAL; 294 dst_size = conf->pc_ndst * sizeof(struct sockaddr_in); 295 296 dst = kmalloc(dst_size, M_PKTGEN, M_WAITOK | M_NULLOK); 297 if (dst == NULL) 298 return ENOMEM; 299 300 error = copyin(conf->pc_dst, dst, dst_size); 301 if (error) 302 goto failed; 303 304 for (i = 0; i < conf->pc_ndst; ++i) { 305 sin = &dst[i]; 306 if (sin->sin_family != AF_INET) { 307 error = EPROTONOSUPPORT; 308 goto failed; 309 } 310 if (sin->sin_port == 0) { 311 error = EINVAL; 312 goto failed; 313 } 314 } 315 316 ifp = ifunit(conf->pc_ifname); 317 if (ifp == NULL) { 318 error = ENXIO; 319 goto failed; 320 } 321 322 pktenq = conf->pc_pktenq; 323 if (pktenq < 0 || pktenq > ifp->if_snd.altq_maxlen) { 324 error = ENOBUFS; 325 goto failed; 326 } else if (pktenq == 0) { 327 pktenq = (ifp->if_snd.altq_maxlen * 3) / 4; 328 } 329 330 sa = &conf->pc_dst_lladdr; 331 if (sa->sa_family != AF_LINK) { 332 error = EPROTONOSUPPORT; 333 goto failed; 334 } 335 if (sa->sa_len != ETHER_ADDR_LEN) { 336 error = EPROTONOSUPPORT; 337 goto failed; 338 } 339 if (ETHER_IS_MULTICAST(sa->sa_data) || 340 bcmp(sa->sa_data, ifp->if_broadcastaddr, ifp->if_addrlen) == 0) { 341 error = EADDRNOTAVAIL; 342 goto failed; 343 } 344 345 /* 346 * Accept the config 347 */ 348 pktg->pktg_flags |= PKTG_F_CONFIG; 349 350 pktg->pktg_duration = conf->pc_duration; 351 pktg->pktg_datalen = conf->pc_datalen; 352 pktg->pktg_pktenq = pktenq; 353 pktg->pktg_ifp = ifp; 354 pktg->pktg_src = conf->pc_src; 355 pktg->pktg_ndst = conf->pc_ndst; 356 KKASSERT(pktg->pktg_dst == NULL); 357 pktg->pktg_dst = dst; 358 bcopy(sa->sa_data, pktg->pktg_dst_lladdr, ETHER_ADDR_LEN); 359 360 return 0; 361 362 failed: 363 if (dst != NULL) 364 kfree(dst, M_PKTGEN); 365 return error; 366 } 367 368 static void 369 pktgen_start_ifsq(struct pktgen *pktg, struct ifaltq_subque *ifsq) 370 { 371 struct netmsg_pktgen *np; 372 373 np = kmalloc(sizeof(*np), M_LWKTMSG, M_WAITOK); 374 netmsg_init(&np->np_base, NULL, &netisr_afree_rport, 0, 375 pktgen_start_ifsq_handler); 376 np->np_pktg = pktg; 377 np->np_ifsq = ifsq; 378 379 lwkt_sendmsg(netisr_cpuport(ifsq_get_cpuid(ifsq)), &np->np_base.lmsg); 380 } 381 382 static int 383 pktgen_start(struct pktgen *pktg, int mq) 384 { 385 struct ifaltq *ifq; 386 387 if ((pktg->pktg_flags & PKTG_F_CONFIG) == 0) 388 return EINVAL; 389 if (pktg->pktg_flags & PKTG_F_RUNNING) 390 return EBUSY; 391 pktg->pktg_flags |= PKTG_F_RUNNING; 392 393 ifq = &pktg->pktg_ifp->if_snd; 394 if (!mq) { 395 pktgen_ref(pktg); 396 pktgen_start_ifsq(pktg, ifq_get_subq_default(ifq)); 397 } else { 398 int i; 399 400 for (i = 0; i < ifq->altq_subq_cnt; ++i) 401 pktgen_ref(pktg); 402 for (i = 0; i < ifq->altq_subq_cnt; ++i) 403 pktgen_start_ifsq(pktg, ifq_get_subq(ifq, i)); 404 } 405 return 0; 406 } 407 408 static void 409 pktgen_start_ifsq_handler(netmsg_t nmsg) 410 { 411 struct netmsg_pktgen *np = (struct netmsg_pktgen *)nmsg; 412 struct pktgen *pktg = np->np_pktg; 413 struct ifaltq_subque *ifsq = np->np_ifsq; 414 415 struct mbuf *m, *head = NULL, **next; 416 struct ifnet *ifp; 417 struct pktgen_pcpu *p; 418 int cpuid, i, alloc_cnt, keep_cnt; 419 420 u_short ulen, psum; 421 int len, ip_len; 422 423 /* Reply ASAP */ 424 lwkt_replymsg(&np->np_base.lmsg, 0); 425 426 ifp = pktg->pktg_ifp; 427 428 cpuid = ifsq_get_cpuid(ifsq); 429 KKASSERT(cpuid == mycpuid); 430 431 p = &pktg->pktg_pcpu[cpuid]; 432 433 keep_cnt = pktg->pktg_pktenq; 434 alloc_cnt = keep_cnt * 2; 435 436 /* 437 * Prefault enough mbuf into mbuf objcache 438 */ 439 next = &head; 440 for (i = 0; i < alloc_cnt; ++i) { 441 MGETHDR(m, MB_WAIT, MT_DATA); 442 *next = m; 443 next = &m->m_nextpkt; 444 } 445 446 for (i = 0; i < alloc_cnt - keep_cnt; ++i) { 447 m = head; 448 head = m->m_nextpkt; 449 m->m_nextpkt = NULL; 450 m_freem(m); 451 } 452 KKASSERT(head != NULL); 453 454 /* 455 * Setup the packets' data 456 */ 457 ip_len = pktg->pktg_datalen + sizeof(struct udpiphdr); 458 len = ip_len + ETHER_HDR_LEN; 459 460 psum = htons((u_short)pktg->pktg_datalen + sizeof(struct udphdr) + 461 IPPROTO_UDP); 462 ulen = htons(pktg->pktg_datalen + sizeof(struct udphdr)); 463 464 m = head; 465 i = 0; 466 while (m != NULL) { 467 struct mbuf *nextm; 468 const struct sockaddr_in *dst; 469 struct pktgen_buf *pb; 470 struct ip *ip; 471 struct udpiphdr *ui; 472 struct ether_header *eh; 473 474 pktgen_ref(pktg); 475 476 pb = kmalloc(sizeof(*pb), M_PKTGEN, M_WAITOK | M_ZERO); 477 pb->pb_ifp = ifp; 478 pb->pb_ifsq = ifsq; 479 pb->pb_inuse = 1; 480 pb->pb_buf = kmalloc(PKTGEN_BUFSZ, M_PKTGEN, M_WAITOK); 481 pb->pb_len = len; 482 pb->pb_cpuid = cpuid; 483 pb->pb_pktg = pktg; 484 netmsg_init(&pb->pb_nmsg, NULL, &netisr_adone_rport, 0, 485 pktgen_buf_send); 486 LIST_INSERT_HEAD(&p->pktg_buflist, pb, pb_link); 487 488 dst = &pktg->pktg_dst[i % pktg->pktg_ndst]; 489 ++i; 490 491 m->m_ext.ext_arg = pb; 492 m->m_ext.ext_buf = pb->pb_buf; 493 m->m_ext.ext_free = pktgen_buf_free; 494 m->m_ext.ext_ref = pktgen_buf_ref; 495 m->m_ext.ext_size = PKTGEN_BUFSZ; 496 497 m->m_data = m->m_ext.ext_buf; 498 m->m_flags |= M_EXT; 499 m->m_len = m->m_pkthdr.len = len; 500 501 m->m_data += ETHER_HDR_LEN; 502 m->m_len -= ETHER_HDR_LEN; 503 m->m_pkthdr.len -= ETHER_HDR_LEN; 504 505 ui = mtod(m, struct udpiphdr *); 506 ui->ui_pr = IPPROTO_UDP; 507 ui->ui_src.s_addr = pktg->pktg_src.sin_addr.s_addr; 508 ui->ui_dst.s_addr = dst->sin_addr.s_addr; 509 ui->ui_sport = pktg->pktg_src.sin_port; 510 ui->ui_dport = dst->sin_port; 511 ui->ui_ulen = ulen; 512 ui->ui_sum = in_pseudo(ui->ui_src.s_addr, ui->ui_dst.s_addr, 513 psum); 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_sum = 0; 523 ip->ip_id = ip_newid(); 524 525 in_delayed_cksum(m); 526 527 ip->ip_len = htons(ip->ip_len); 528 ip->ip_sum = in_cksum_hdr(ip); 529 530 m->m_data -= ETHER_HDR_LEN; 531 m->m_len += ETHER_HDR_LEN; 532 m->m_pkthdr.len += ETHER_HDR_LEN; 533 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 nextm = m->m_nextpkt; 540 m->m_nextpkt = NULL; 541 542 ifq_dispatch(ifp, m, NULL); 543 544 m = nextm; 545 } 546 547 callout_reset(&p->pktg_stop, pktg->pktg_duration * hz, 548 pktgen_pcpu_stop_cb, p); 549 550 pktgen_free(pktg); 551 } 552 553 static void 554 pktgen_mbuf(struct pktgen_buf *pb, struct mbuf *m) 555 { 556 m->m_ext.ext_arg = pb; 557 m->m_ext.ext_buf = pb->pb_buf; 558 m->m_ext.ext_free = pktgen_buf_free; 559 m->m_ext.ext_ref = pktgen_buf_ref; 560 m->m_ext.ext_size = PKTGEN_BUFSZ; 561 562 m->m_data = m->m_ext.ext_buf; 563 m->m_flags |= M_EXT; 564 m->m_len = m->m_pkthdr.len = pb->pb_len; 565 } 566 567 static void 568 pktgen_buf_send(netmsg_t msg) 569 { 570 struct pktgen_buf *pb = (struct pktgen_buf *)msg; 571 struct mbuf *m; 572 573 KKASSERT(&curthread->td_msgport == netisr_cpuport(pb->pb_cpuid)); 574 575 crit_enter(); 576 lwkt_replymsg(&pb->pb_nmsg.lmsg, 0); 577 crit_exit(); 578 579 MGETHDR(m, MB_WAIT, MT_DATA); 580 pktgen_mbuf(pb, m); 581 ifq_dispatch(pb->pb_ifp, m, NULL); 582 } 583 584 static void 585 pktgen_buf_free(void *arg) 586 { 587 struct pktgen_buf *pb = arg; 588 struct mbuf *m; 589 590 KKASSERT(pb->pb_inuse > 0); 591 if (pb->pb_done) { 592 if (atomic_fetchadd_int(&pb->pb_inuse, -1) == 1) { 593 struct pktgen *pktg; 594 595 pktg = pb->pb_pktg; 596 crit_enter(); 597 LIST_REMOVE(pb, pb_link); 598 crit_exit(); 599 kfree(pb->pb_buf, M_PKTGEN); 600 kfree(pb, M_PKTGEN); 601 602 pktgen_free(pktg); 603 } 604 return; 605 } 606 607 if (&curthread->td_msgport != netisr_cpuport(pb->pb_cpuid)) { 608 KKASSERT(pb->pb_cpuid == mycpuid); 609 crit_enter(); 610 KKASSERT(pb->pb_nmsg.lmsg.ms_flags & MSGF_DONE); 611 lwkt_sendmsg(netisr_cpuport(pb->pb_cpuid), &pb->pb_nmsg.lmsg); 612 crit_exit(); 613 return; 614 } 615 616 MGETHDR(m, MB_WAIT, MT_DATA); 617 pktgen_mbuf(pb, m); 618 ifsq_enqueue(pb->pb_ifsq, m, NULL); 619 } 620 621 static void 622 pktgen_buf_ref(void *arg) 623 { 624 struct pktgen_buf *pb = arg; 625 626 panic("%s should never be called\n", __func__); 627 628 KKASSERT(pb->pb_inuse > 0); 629 atomic_add_int(&pb->pb_inuse, 1); 630 } 631 632 static void 633 pktgen_free(struct pktgen *pktg) 634 { 635 KKASSERT(pktg->pktg_refcnt > 0); 636 if (atomic_fetchadd_int(&pktg->pktg_refcnt, -1) == 1) { 637 int i; 638 639 if (pktg->pktg_dst != NULL) 640 kfree(pktg->pktg_dst, M_PKTGEN); 641 642 for (i = 0; i < ncpus; ++i) 643 KKASSERT(LIST_EMPTY(&pktg->pktg_pcpu[i].pktg_buflist)); 644 kfree(pktg, M_PKTGEN); 645 } 646 647 KKASSERT(pktgen_refcnt > 0); 648 atomic_subtract_int(&pktgen_refcnt, 1); 649 } 650 651 static void 652 pktgen_pcpu_stop_cb(void *arg) 653 { 654 struct pktgen_pcpu *p = arg; 655 struct pktgen_buf *pb; 656 657 crit_enter(); 658 LIST_FOREACH(pb, &p->pktg_buflist, pb_link) 659 pb->pb_done = 1; 660 crit_exit(); 661 } 662 663 static void 664 pktgen_ref(struct pktgen *pktg) 665 { 666 atomic_add_int(&pktg->pktg_refcnt, 1); 667 atomic_add_int(&pktgen_refcnt, 1); 668 } 669