[dragonfly.git] / tools / tools / netrate / pktgen / pktgen.c

/*
 * Copyright (c) 2008 The DragonFly Project.  All rights reserved.
 * 
 * This code is derived from software contributed to The DragonFly Project
 * by Sepherosa Ziehau <sepherosa@gmail.com>
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name of The DragonFly Project nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific, prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE
 * COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 * 
 * $DragonFly: src/tools/tools/netrate/pktgen/pktgen.c,v 1.4 2008/04/02 14:18:55 sephe Exp $
 */

#define _IP_VHL

#include <sys/param.h>
#include <sys/conf.h>
#include <sys/device.h>
#include <sys/in_cksum.h>
#include <sys/kernel.h>
#include <sys/malloc.h>
#include <sys/mbuf.h>
#include <sys/proc.h>
#include <sys/priv.h>
#include <sys/socket.h>
#include <sys/systm.h>
#include <sys/serialize.h>

#include <net/if.h>
#include <net/if_dl.h>
#include <net/if_var.h>
#include <net/ifq_var.h>
#include <net/ethernet.h>

#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/udp_var.h>

#include "pktgen.h"

#define CDEV_NAME	"pktg"
#define CDEV_MAJOR	151

struct pktgen {
	uint32_t		pktg_flags;	/* PKTG_F_ */
	int			pktg_refcnt;

	uint64_t		pktg_tx_cnt;
	uint64_t		pktg_err_cnt;
	struct timeval		pktg_start;
	struct timeval		pktg_end;

	struct callout		pktg_stop;
	int			pktg_duration;
	int			pktg_cpuid;
	void			(*pktg_thread)(void *);

	int			pktg_datalen;
	int			pktg_yield;
	struct ifnet		*pktg_ifp;

	in_addr_t		pktg_saddr;	/* host byte order */
	in_addr_t		pktg_daddr;	/* host byte order */
	u_short			pktg_sport;	/* host byte order */
	u_short			pktg_dport;	/* host byte order */

	int			pktg_nsaddr;
	int			pktg_ndaddr;
	int			pktg_nsport;
	int			pktg_ndport;

	uint8_t			pktg_dst_lladdr[ETHER_ADDR_LEN];
};

#define PKTG_F_CONFIG	0x1
#define PKTG_F_STOP	0x2
#define PKTG_F_RUNNING	0x4

static int 		pktgen_modevent(module_t, int, void *);
static int		pktgen_config(struct pktgen *,
				      const struct pktgen_conf *);
static int		pktgen_start(struct pktgen *, int);
static void		pktgen_thread_exit(struct pktgen *, uint64_t, uint64_t);
static void		pktgen_stop_cb(void *);
static void		pktgen_udp_thread(void *);
static void		pktgen_udp_thread1(void *);

static d_open_t		pktgen_open;
static d_close_t	pktgen_close;
static d_ioctl_t	pktgen_ioctl;

static struct dev_ops	pktgen_ops = {
	{ CDEV_NAME, CDEV_MAJOR, 0 },
	.d_open =	pktgen_open,
	.d_close =	pktgen_close,
	.d_ioctl =	pktgen_ioctl,
};

static int		pktgen_refcnt;
static struct lwkt_token pktgen_tok = LWKT_TOKEN_INITIALIZER(pktgen_token);

MALLOC_DECLARE(M_PKTGEN);
MALLOC_DEFINE(M_PKTGEN, CDEV_NAME, "Packet generator");

DEV_MODULE(pktgen, pktgen_modevent, NULL);

static int
pktgen_modevent(module_t mod, int type, void *data)
{
	int error = 0;

	switch (type) {
	case MOD_LOAD:
		make_dev(&pktgen_ops, 0, UID_ROOT, GID_WHEEL, 0600,
		    CDEV_NAME"%d", 0);
		break;

	case MOD_UNLOAD:
		if (pktgen_refcnt > 0)
			return EBUSY;
		dev_ops_remove_all(&pktgen_ops);
		break;

	default:
		error = EOPNOTSUPP;
		break;
	}
	return error;
}

static int
pktgen_open(struct dev_open_args *ap)
{
	cdev_t dev = ap->a_head.a_dev;
	struct pktgen *pktg;
	int error;

	error = priv_check_cred(ap->a_cred, PRIV_ROOT, 0);
	if (error)
		return error;

	lwkt_gettoken(&pktgen_tok);

	if (dev->si_drv1 != NULL) {
		lwkt_reltoken(&pktgen_tok);
		return EBUSY;
	}

	pktg = kmalloc(sizeof(*pktg), M_PKTGEN, M_ZERO | M_WAITOK);
	callout_init(&pktg->pktg_stop);

	dev->si_drv1 = pktg;
	pktg->pktg_refcnt = 1;

	pktgen_refcnt++;

	lwkt_reltoken(&pktgen_tok);
	return 0;
}

static int
pktgen_close(struct dev_close_args *ap)
{
	cdev_t dev = ap->a_head.a_dev;
	struct pktgen *pktg = dev->si_drv1;

	lwkt_gettoken(&pktgen_tok);

	KKASSERT(pktg->pktg_refcnt > 0);
	if (--pktg->pktg_refcnt == 0)
		kfree(pktg, M_PKTGEN);
	dev->si_drv1 = NULL;

	KKASSERT(pktgen_refcnt > 0);
	pktgen_refcnt--;

	lwkt_reltoken(&pktgen_tok);
	return 0;
}

static int
pktgen_ioctl(struct dev_ioctl_args *ap __unused)
{
	cdev_t dev = ap->a_head.a_dev;
	caddr_t data = ap->a_data;
	struct pktgen *pktg = dev->si_drv1;
	int error;

	lwkt_gettoken(&pktgen_tok);

	switch (ap->a_cmd) {
	case PKTGENSTART:
		error = pktgen_start(pktg, minor(dev));
		break;

	case PKTGENSCONF:
		error = pktgen_config(pktg, (const struct pktgen_conf *)data);
		break;

	default:
		error = EOPNOTSUPP;
		break;
	}

	lwkt_reltoken(&pktgen_tok);
	return error;
}

static int
pktgen_config(struct pktgen *pktg, const struct pktgen_conf *conf)
{
	const struct sockaddr_in *sin;
	const struct sockaddr *sa;
	struct ifnet *ifp;
	int yield, nsaddr, ndaddr, nsport, ndport, thread1;

	if (pktg->pktg_flags & PKTG_F_RUNNING)
		return EBUSY;

	if (conf->pc_cpuid < 0 || conf->pc_cpuid >= ncpus)
		return EINVAL;
	if (conf->pc_datalen <= 0)
		return EINVAL;
	if (conf->pc_duration <= 0)
		return EINVAL;

	yield = conf->pc_yield;
	if (yield <= 0)
		yield = PKTGEN_YIELD_DEFAULT;

	if (conf->pc_nsaddr <= 0 && conf->pc_ndaddr <= 0 &&
	    conf->pc_nsport <= 0 && conf->pc_ndport <= 0)
		thread1 = 0;
	else
		thread1 = 1;

	nsaddr = conf->pc_nsaddr;
	if (nsaddr <= 0)
		nsaddr = 1;
	ndaddr = conf->pc_ndaddr;
	if (ndaddr <= 0)
		ndaddr = 1;

	nsport = conf->pc_nsport;
	if (nsport <= 0)
		nsport = 1;
	ndport = conf->pc_ndport;
	if (ndport <= 0)
		ndport = 1;

	ifp = ifunit(conf->pc_ifname);
	if (ifp == NULL)
		return ENXIO;

	sa = &conf->pc_dst_lladdr;
	if (sa->sa_family != AF_LINK)
		return EPROTONOSUPPORT;
	if (sa->sa_len != ETHER_ADDR_LEN)
		return EPROTONOSUPPORT;
	if (ETHER_IS_MULTICAST(sa->sa_data) ||
	    bcmp(sa->sa_data, ifp->if_broadcastaddr, ifp->if_addrlen) == 0)
		return EADDRNOTAVAIL;

	sin = &conf->pc_src;
	if (sin->sin_family != AF_INET)
		return EPROTONOSUPPORT;
	if (sin->sin_port == 0)
		return EINVAL;

	sin = &conf->pc_dst;
	if (sin->sin_family != AF_INET)
		return EPROTONOSUPPORT;
	if (sin->sin_port == 0)
		return EINVAL;

	/* Accept the config */
	pktg->pktg_flags |= PKTG_F_CONFIG;
	pktg->pktg_refcnt++;
	pktgen_refcnt++;

	pktg->pktg_duration = conf->pc_duration;
	pktg->pktg_cpuid = conf->pc_cpuid;
	pktg->pktg_ifp = ifp;
	pktg->pktg_datalen = conf->pc_datalen;
	pktg->pktg_yield = yield;
	bcopy(sa->sa_data, pktg->pktg_dst_lladdr, ETHER_ADDR_LEN);

	pktg->pktg_saddr = ntohl(conf->pc_src.sin_addr.s_addr);
	pktg->pktg_daddr = ntohl(conf->pc_dst.sin_addr.s_addr);
	pktg->pktg_nsaddr = nsaddr;
	pktg->pktg_ndaddr = ndaddr;

	pktg->pktg_sport = ntohs(conf->pc_src.sin_port);
	pktg->pktg_dport = ntohs(conf->pc_dst.sin_port);
	pktg->pktg_nsport = nsport;
	pktg->pktg_ndport = ndport;

	pktg->pktg_thread = thread1 ? pktgen_udp_thread1 : pktgen_udp_thread;

	return 0;
}

static int
pktgen_start(struct pktgen *pktg, int m)
{
	if ((pktg->pktg_flags & PKTG_F_CONFIG) == 0)
		return EINVAL;
	if (pktg->pktg_flags & PKTG_F_RUNNING)
		return EBUSY;

	pktg->pktg_flags |= PKTG_F_RUNNING;

	lwkt_create(pktg->pktg_thread, pktg, NULL, NULL, 0,
		    pktg->pktg_cpuid, "pktgen %d", m);
	return 0;
}

static void
pktgen_stop_cb(void *arg)
{
	struct pktgen *pktg = arg;

	pktg->pktg_flags |= PKTG_F_STOP;
}

static void
pktgen_udp_thread1(void *arg)
{
	struct pktgen *pktg = arg;
	struct ifnet *ifp = pktg->pktg_ifp;
	struct ip *ip;
	struct udpiphdr *ui;
	struct ether_header *eh;
	struct mbuf *m;
	u_short ulen, psum;
	int len, ip_len;
	int sw_csum, csum_flags;
	int loop, r, error;
	uint64_t err_cnt, cnt;
	in_addr_t saddr, daddr;
	u_short sport, dport;

	callout_reset(&pktg->pktg_stop, pktg->pktg_duration * hz,
		      pktgen_stop_cb, pktg);

	cnt = err_cnt = 0;
	r = loop = 0;

	ip_len = pktg->pktg_datalen + sizeof(*ui);
	len = ip_len + ETHER_HDR_LEN;

	psum = htons((u_short)pktg->pktg_datalen + sizeof(struct udphdr)
		     + IPPROTO_UDP);
	ulen = htons(pktg->pktg_datalen + sizeof(struct udphdr));

	sw_csum = (CSUM_UDP | CSUM_IP) & ~ifp->if_hwassist;
	csum_flags = (CSUM_UDP | CSUM_IP) & ifp->if_hwassist;

	saddr = pktg->pktg_saddr;
	daddr = pktg->pktg_daddr;
	sport = pktg->pktg_sport;
	dport = pktg->pktg_dport;

	microtime(&pktg->pktg_start);
	while ((pktg->pktg_flags & PKTG_F_STOP) == 0) {
		m = m_getl(len, MB_WAIT, MT_DATA, M_PKTHDR, NULL);
		m->m_len = m->m_pkthdr.len = len;

		m_adj(m, ETHER_HDR_LEN);

		ui = mtod(m, struct udpiphdr *);
		ui->ui_pr = IPPROTO_UDP;
		ui->ui_src.s_addr = htonl(saddr);
		ui->ui_dst.s_addr = htonl(daddr);
		ui->ui_sport = htons(sport);
		ui->ui_dport = htons(dport);
		ui->ui_ulen = ulen;
		ui->ui_sum = in_pseudo(ui->ui_src.s_addr,
				       ui->ui_dst.s_addr, psum);
		m->m_pkthdr.csum_flags = (CSUM_IP | CSUM_UDP);
		m->m_pkthdr.csum_data = offsetof(struct udphdr, uh_sum);
		m->m_pkthdr.csum_iphlen = sizeof(struct ip);
		m->m_pkthdr.csum_thlen = sizeof(struct udphdr);
		m->m_pkthdr.csum_lhlen = sizeof(struct ether_header);

		ip = (struct ip *)ui;
		ip->ip_len = ip_len;
		ip->ip_ttl = 64;	/* XXX */
		ip->ip_tos = 0;		/* XXX */
		ip->ip_vhl = IP_VHL_BORING;
		ip->ip_off = 0;
		ip->ip_id = ip_newid();

		if (sw_csum & CSUM_DELAY_DATA)
			in_delayed_cksum(m);
		m->m_pkthdr.csum_flags = csum_flags;

		ip->ip_len = htons(ip->ip_len);
		ip->ip_sum = 0;
		if (sw_csum & CSUM_DELAY_IP)
			ip->ip_sum = in_cksum_hdr(ip);

		M_PREPEND(m, ETHER_HDR_LEN, MB_WAIT);
		eh = mtod(m, struct ether_header *);
		bcopy(pktg->pktg_dst_lladdr, eh->ether_dhost, ETHER_ADDR_LEN);
		bcopy(IF_LLADDR(ifp), eh->ether_shost, ETHER_ADDR_LEN);
		eh->ether_type = htons(ETHERTYPE_IP);

		ifnet_serialize_tx(ifp);
		error = ifq_handoff(ifp, m, NULL);
		ifnet_deserialize_tx(ifp);

		loop++;
		if (error) {
			err_cnt++;
			loop = 0;
			lwkt_yield();
		} else {
			cnt++;
			if (loop == pktg->pktg_yield) {
				loop = 0;
				lwkt_yield();
			}

			r++;
			saddr = pktg->pktg_saddr + (r % pktg->pktg_nsaddr);
			daddr = pktg->pktg_daddr + (r % pktg->pktg_ndaddr);
			sport = pktg->pktg_sport + (r % pktg->pktg_nsport);
			dport = pktg->pktg_dport + (r % pktg->pktg_ndport);
		}
	}
	microtime(&pktg->pktg_end);

	pktgen_thread_exit(pktg, cnt, err_cnt);
}

static void
pktgen_udp_thread(void *arg)
{
	struct pktgen *pktg = arg;
	struct ifnet *ifp = pktg->pktg_ifp;
	struct ip *ip;
	struct udpiphdr *ui;
	struct ether_header *eh;
	struct mbuf *m;
	u_short ulen, sum;
	int len, ip_len;
	int sw_csum, csum_flags;
	int loop, error;
	uint64_t err_cnt, cnt;
	in_addr_t saddr, daddr;
	u_short sport, dport;

	callout_reset(&pktg->pktg_stop, pktg->pktg_duration * hz,
		      pktgen_stop_cb, pktg);

	cnt = err_cnt = 0;
	loop = 0;

	ip_len = pktg->pktg_datalen + sizeof(*ui);
	len = ip_len + ETHER_HDR_LEN;

	saddr = htonl(pktg->pktg_saddr);
	daddr = htonl(pktg->pktg_daddr);
	sport = htons(pktg->pktg_sport);
	dport = htons(pktg->pktg_dport);

	sum = in_pseudo(saddr, daddr,
		htons((u_short)pktg->pktg_datalen + sizeof(struct udphdr)
		+ IPPROTO_UDP));
	ulen = htons(pktg->pktg_datalen + sizeof(struct udphdr));

	sw_csum = (CSUM_UDP | CSUM_IP) & ~ifp->if_hwassist;
	csum_flags = (CSUM_UDP | CSUM_IP) & ifp->if_hwassist;

	microtime(&pktg->pktg_start);
	while ((pktg->pktg_flags & PKTG_F_STOP) == 0) {
		m = m_getl(len, MB_WAIT, MT_DATA, M_PKTHDR, NULL);
		m->m_len = m->m_pkthdr.len = len;

		m_adj(m, ETHER_HDR_LEN);

		ui = mtod(m, struct udpiphdr *);
		ui->ui_pr = IPPROTO_UDP;
		ui->ui_src.s_addr = saddr;
		ui->ui_dst.s_addr = daddr;
		ui->ui_sport = sport;
		ui->ui_dport = dport;
		ui->ui_ulen = ulen;
		ui->ui_sum = sum;
		m->m_pkthdr.csum_flags = (CSUM_IP | CSUM_UDP);
		m->m_pkthdr.csum_data = offsetof(struct udphdr, uh_sum);

		ip = (struct ip *)ui;
		ip->ip_len = ip_len;
		ip->ip_ttl = 64;	/* XXX */
		ip->ip_tos = 0;		/* XXX */
		ip->ip_vhl = IP_VHL_BORING;
		ip->ip_off = 0;
		ip->ip_id = ip_newid();

		if (sw_csum & CSUM_DELAY_DATA)
			in_delayed_cksum(m);
		m->m_pkthdr.csum_flags = csum_flags;

		ip->ip_len = htons(ip->ip_len);
		ip->ip_sum = 0;
		if (sw_csum & CSUM_DELAY_IP)
			ip->ip_sum = in_cksum_hdr(ip);

		M_PREPEND(m, ETHER_HDR_LEN, MB_WAIT);
		eh = mtod(m, struct ether_header *);
		bcopy(pktg->pktg_dst_lladdr, eh->ether_dhost, ETHER_ADDR_LEN);
		bcopy(IF_LLADDR(ifp), eh->ether_shost, ETHER_ADDR_LEN);
		eh->ether_type = htons(ETHERTYPE_IP);

		ifnet_serialize_tx(ifp);
		error = ifq_handoff(ifp, m, NULL);
		ifnet_deserialize_tx(ifp);

		loop++;
		if (error) {
			err_cnt++;
			loop = 0;
			lwkt_yield();
		} else {
			cnt++;
			if (loop == pktg->pktg_yield) {
				loop = 0;
				lwkt_yield();
			}
		}
	}
	microtime(&pktg->pktg_end);

	pktgen_thread_exit(pktg, cnt, err_cnt);
}

static void
pktgen_thread_exit(struct pktgen *pktg, uint64_t tx_cnt, uint64_t err_cnt)
{
	struct timeval end;

	pktg->pktg_tx_cnt = tx_cnt;
	pktg->pktg_err_cnt = err_cnt;

	end = pktg->pktg_end;
	timevalsub(&end, &pktg->pktg_start);
	kprintf("cnt %ju, err %ju, time %ld.%06ld\n",
		(uintmax_t)pktg->pktg_tx_cnt,
		(uintmax_t)pktg->pktg_err_cnt, end.tv_sec, end.tv_usec);

	pktg->pktg_flags &= ~(PKTG_F_STOP | PKTG_F_CONFIG | PKTG_F_RUNNING);

	KKASSERT(pktg->pktg_refcnt > 0);
	if (--pktg->pktg_refcnt == 0)
		kfree(pktg, M_PKTGEN);	/* XXX */

	KKASSERT(pktgen_refcnt > 0);
	pktgen_refcnt--;

	lwkt_exit();
}
Commit	Line	Data
4fa8e46e SZ	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	*
db161a19	34	* $DragonFly: src/tools/tools/netrate/pktgen/pktgen.c,v 1.4 2008/04/02 14:18:55 sephe Exp $
4fa8e46e SZ	35	*/
	36
	37	#define _IP_VHL
	38
	39	#include <sys/param.h>
	40	#include <sys/conf.h>
	41	#include <sys/device.h>
4fa8e46e SZ	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>
9eaf083f	47	#include <sys/priv.h>
4fa8e46e SZ	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
db161a19 SZ	71	uint64_t pktg_tx_cnt;
	72	uint64_t pktg_err_cnt;
	73	struct timeval pktg_start;
	74	struct timeval pktg_end;
	75
4fa8e46e SZ	76	struct callout pktg_stop;
	77	int pktg_duration;
	78	int pktg_cpuid;
c95ebcd6	79	void (pktg_thread)(void );
4fa8e46e SZ	80
	81	int pktg_datalen;
	82	int pktg_yield;
	83	struct ifnet *pktg_ifp;
c95ebcd6 SZ	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
4fa8e46e SZ	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);
db161a19	106	static void pktgen_thread_exit(struct pktgen *, uint64_t, uint64_t);
4fa8e46e	107	static void pktgen_stop_cb(void *);
c95ebcd6 SZ	108	static void pktgen_udp_thread(void *);
c95ebcd6 SZ	109	static void pktgen_udp_thread1(void *);
4fa8e46e SZ	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;
e3d0d8d7	123	static struct lwkt_token pktgen_tok = LWKT_TOKEN_INITIALIZER(pktgen_token);
4fa8e46e SZ	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:
4b3b3198 AH	137	make_dev(&pktgen_ops, 0, UID_ROOT, GID_WHEEL, 0600,
4b3b3198 AH	138	CDEV_NAME"%d", 0);
4fa8e46e SZ	139	break;
	140
	141	case MOD_UNLOAD:
	142	if (pktgen_refcnt > 0)
	143	return EBUSY;
4b3b3198	144	dev_ops_remove_all(&pktgen_ops);
4fa8e46e SZ	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
9eaf083f	161	error = priv_check_cred(ap->a_cred, PRIV_ROOT, 0);
4fa8e46e SZ	162	if (error)
	163	return error;
	164
e3d0d8d7	165	lwkt_gettoken(&pktgen_tok);
4fa8e46e SZ	166
4fa8e46e SZ	167	if (dev->si_drv1 != NULL) {
e3d0d8d7	168	lwkt_reltoken(&pktgen_tok);
4fa8e46e SZ	169	return EBUSY;
	170	}
	171
	172	pktg = kmalloc(sizeof(*pktg), M_PKTGEN, M_ZERO \| M_WAITOK);
	173	callout_init(&pktg->pktg_stop);
	174
4fa8e46e SZ	175	dev->si_drv1 = pktg;
	176	pktg->pktg_refcnt = 1;
	177
	178	pktgen_refcnt++;
	179
e3d0d8d7	180	lwkt_reltoken(&pktgen_tok);
4fa8e46e SZ	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
e3d0d8d7	190	lwkt_gettoken(&pktgen_tok);
4fa8e46e SZ	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
e3d0d8d7	200	lwkt_reltoken(&pktgen_tok);
4fa8e46e SZ	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
e3d0d8d7	212	lwkt_gettoken(&pktgen_tok);
4fa8e46e SZ	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
e3d0d8d7	228	lwkt_reltoken(&pktgen_tok);
4fa8e46e SZ	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;
c95ebcd6	238	int yield, nsaddr, ndaddr, nsport, ndport, thread1;
4fa8e46e SZ	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
c95ebcd6 SZ	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
4fa8e46e SZ	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);
c95ebcd6 SZ	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;
4fa8e46e SZ	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
c95ebcd6	336	lwkt_create(pktg->pktg_thread, pktg, NULL, NULL, 0,
4fa8e46e SZ	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
c95ebcd6	350	pktgen_udp_thread1(void *arg)
4fa8e46e SZ	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;
c95ebcd6	361	int loop, r, error;
4fa8e46e	362	uint64_t err_cnt, cnt;
c95ebcd6 SZ	363	in_addr_t saddr, daddr;
c95ebcd6 SZ	364	u_short sport, dport;
4fa8e46e	365
4fa8e46e SZ	366	callout_reset(&pktg->pktg_stop, pktg->pktg_duration * hz,
	367	pktgen_stop_cb, pktg);
	368
	369	cnt = err_cnt = 0;
c95ebcd6	370	r = loop = 0;
4fa8e46e SZ	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
c95ebcd6 SZ	382	saddr = pktg->pktg_saddr;
	383	daddr = pktg->pktg_daddr;
	384	sport = pktg->pktg_sport;
	385	dport = pktg->pktg_dport;
	386
db161a19	387	microtime(&pktg->pktg_start);
4fa8e46e SZ	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;
c95ebcd6 SZ	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);
4fa8e46e SZ	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);
a8d48371 SZ	405	m->m_pkthdr.csum_iphlen = sizeof(struct ip);
a8d48371 SZ	406	m->m_pkthdr.csum_thlen = sizeof(struct udphdr);
bde2fa86	407	m->m_pkthdr.csum_lhlen = sizeof(struct ether_header);
4fa8e46e SZ	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);
c95ebcd6 SZ	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
39d72cc5	432	ifnet_serialize_tx(ifp);
c95ebcd6	433	error = ifq_handoff(ifp, m, NULL);
39d72cc5	434	ifnet_deserialize_tx(ifp);
c95ebcd6 SZ	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	}
db161a19	455	microtime(&pktg->pktg_end);
c95ebcd6	456
db161a19	457	pktgen_thread_exit(pktg, cnt, err_cnt);
c95ebcd6 SZ	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;
c95ebcd6	476
c95ebcd6 SZ	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
db161a19	499	microtime(&pktg->pktg_start);
c95ebcd6 SZ	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);
4fa8e46e SZ	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
39d72cc5	540	ifnet_serialize_tx(ifp);
4fa8e46e	541	error = ifq_handoff(ifp, m, NULL);
39d72cc5	542	ifnet_deserialize_tx(ifp);
4fa8e46e SZ	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	}
db161a19 SZ	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;
4fa8e46e	569
db161a19 SZ	570	end = pktg->pktg_end;
db161a19 SZ	571	timevalsub(&end, &pktg->pktg_start);
e3d0d8d7 SZ	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);
4fa8e46e SZ	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	}