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