tools/pktgen: Setup link header length properly
[dragonfly.git] / 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                 m->m_pkthdr.csum_lhlen = sizeof(struct ether_header);
408
409                 ip = (struct ip *)ui;
410                 ip->ip_len = ip_len;
411                 ip->ip_ttl = 64;        /* XXX */
412                 ip->ip_tos = 0;         /* XXX */
413                 ip->ip_vhl = IP_VHL_BORING;
414                 ip->ip_off = 0;
415                 ip->ip_id = ip_newid();
416
417                 if (sw_csum & CSUM_DELAY_DATA)
418                         in_delayed_cksum(m);
419                 m->m_pkthdr.csum_flags = csum_flags;
420
421                 ip->ip_len = htons(ip->ip_len);
422                 ip->ip_sum = 0;
423                 if (sw_csum & CSUM_DELAY_IP)
424                         ip->ip_sum = in_cksum_hdr(ip);
425
426                 M_PREPEND(m, ETHER_HDR_LEN, MB_WAIT);
427                 eh = mtod(m, struct ether_header *);
428                 bcopy(pktg->pktg_dst_lladdr, eh->ether_dhost, ETHER_ADDR_LEN);
429                 bcopy(IF_LLADDR(ifp), eh->ether_shost, ETHER_ADDR_LEN);
430                 eh->ether_type = htons(ETHERTYPE_IP);
431
432                 ifnet_serialize_tx(ifp);
433                 error = ifq_handoff(ifp, m, NULL);
434                 ifnet_deserialize_tx(ifp);
435
436                 loop++;
437                 if (error) {
438                         err_cnt++;
439                         loop = 0;
440                         lwkt_yield();
441                 } else {
442                         cnt++;
443                         if (loop == pktg->pktg_yield) {
444                                 loop = 0;
445                                 lwkt_yield();
446                         }
447
448                         r++;
449                         saddr = pktg->pktg_saddr + (r % pktg->pktg_nsaddr);
450                         daddr = pktg->pktg_daddr + (r % pktg->pktg_ndaddr);
451                         sport = pktg->pktg_sport + (r % pktg->pktg_nsport);
452                         dport = pktg->pktg_dport + (r % pktg->pktg_ndport);
453                 }
454         }
455         microtime(&pktg->pktg_end);
456
457         pktgen_thread_exit(pktg, cnt, err_cnt);
458 }
459
460 static void
461 pktgen_udp_thread(void *arg)
462 {
463         struct pktgen *pktg = arg;
464         struct ifnet *ifp = pktg->pktg_ifp;
465         struct ip *ip;
466         struct udpiphdr *ui;
467         struct ether_header *eh;
468         struct mbuf *m;
469         u_short ulen, sum;
470         int len, ip_len;
471         int sw_csum, csum_flags;
472         int loop, error;
473         uint64_t err_cnt, cnt;
474         in_addr_t saddr, daddr;
475         u_short sport, dport;
476
477         callout_reset(&pktg->pktg_stop, pktg->pktg_duration * hz,
478                       pktgen_stop_cb, pktg);
479
480         cnt = err_cnt = 0;
481         loop = 0;
482
483         ip_len = pktg->pktg_datalen + sizeof(*ui);
484         len = ip_len + ETHER_HDR_LEN;
485
486         saddr = htonl(pktg->pktg_saddr);
487         daddr = htonl(pktg->pktg_daddr);
488         sport = htons(pktg->pktg_sport);
489         dport = htons(pktg->pktg_dport);
490
491         sum = in_pseudo(saddr, daddr,
492                 htons((u_short)pktg->pktg_datalen + sizeof(struct udphdr)
493                 + IPPROTO_UDP));
494         ulen = htons(pktg->pktg_datalen + sizeof(struct udphdr));
495
496         sw_csum = (CSUM_UDP | CSUM_IP) & ~ifp->if_hwassist;
497         csum_flags = (CSUM_UDP | CSUM_IP) & ifp->if_hwassist;
498
499         microtime(&pktg->pktg_start);
500         while ((pktg->pktg_flags & PKTG_F_STOP) == 0) {
501                 m = m_getl(len, MB_WAIT, MT_DATA, M_PKTHDR, NULL);
502                 m->m_len = m->m_pkthdr.len = len;
503
504                 m_adj(m, ETHER_HDR_LEN);
505
506                 ui = mtod(m, struct udpiphdr *);
507                 ui->ui_pr = IPPROTO_UDP;
508                 ui->ui_src.s_addr = saddr;
509                 ui->ui_dst.s_addr = daddr;
510                 ui->ui_sport = sport;
511                 ui->ui_dport = dport;
512                 ui->ui_ulen = ulen;
513                 ui->ui_sum = sum;
514                 m->m_pkthdr.csum_flags = (CSUM_IP | CSUM_UDP);
515                 m->m_pkthdr.csum_data = offsetof(struct udphdr, uh_sum);
516
517                 ip = (struct ip *)ui;
518                 ip->ip_len = ip_len;
519                 ip->ip_ttl = 64;        /* XXX */
520                 ip->ip_tos = 0;         /* XXX */
521                 ip->ip_vhl = IP_VHL_BORING;
522                 ip->ip_off = 0;
523                 ip->ip_id = ip_newid();
524
525                 if (sw_csum & CSUM_DELAY_DATA)
526                         in_delayed_cksum(m);
527                 m->m_pkthdr.csum_flags = csum_flags;
528
529                 ip->ip_len = htons(ip->ip_len);
530                 ip->ip_sum = 0;
531                 if (sw_csum & CSUM_DELAY_IP)
532                         ip->ip_sum = in_cksum_hdr(ip);
533
534                 M_PREPEND(m, ETHER_HDR_LEN, MB_WAIT);
535                 eh = mtod(m, struct ether_header *);
536                 bcopy(pktg->pktg_dst_lladdr, eh->ether_dhost, ETHER_ADDR_LEN);
537                 bcopy(IF_LLADDR(ifp), eh->ether_shost, ETHER_ADDR_LEN);
538                 eh->ether_type = htons(ETHERTYPE_IP);
539
540                 ifnet_serialize_tx(ifp);
541                 error = ifq_handoff(ifp, m, NULL);
542                 ifnet_deserialize_tx(ifp);
543
544                 loop++;
545                 if (error) {
546                         err_cnt++;
547                         loop = 0;
548                         lwkt_yield();
549                 } else {
550                         cnt++;
551                         if (loop == pktg->pktg_yield) {
552                                 loop = 0;
553                                 lwkt_yield();
554                         }
555                 }
556         }
557         microtime(&pktg->pktg_end);
558
559         pktgen_thread_exit(pktg, cnt, err_cnt);
560 }
561
562 static void
563 pktgen_thread_exit(struct pktgen *pktg, uint64_t tx_cnt, uint64_t err_cnt)
564 {
565         struct timeval end;
566
567         pktg->pktg_tx_cnt = tx_cnt;
568         pktg->pktg_err_cnt = err_cnt;
569
570         end = pktg->pktg_end;
571         timevalsub(&end, &pktg->pktg_start);
572         kprintf("cnt %ju, err %ju, time %ld.%06ld\n",
573                 (uintmax_t)pktg->pktg_tx_cnt,
574                 (uintmax_t)pktg->pktg_err_cnt, end.tv_sec, end.tv_usec);
575
576         pktg->pktg_flags &= ~(PKTG_F_STOP | PKTG_F_CONFIG | PKTG_F_RUNNING);
577
578         KKASSERT(pktg->pktg_refcnt > 0);
579         if (--pktg->pktg_refcnt == 0)
580                 kfree(pktg, M_PKTGEN);  /* XXX */
581
582         KKASSERT(pktgen_refcnt > 0);
583         pktgen_refcnt--;
584
585         lwkt_exit();
586 }