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

/*
 * Copyright (c) 2012 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/queue.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 <net/netmsg2.h>
#include <net/netisr2.h>

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

#include "pktgen.h"

#define CDEV_NAME	"pktg"

#define PKTGEN_BUFSZ	2048

#ifndef PKTGEN_DEVCNT
#define PKTGEN_DEVCNT	4
#endif

struct pktgen;

struct netmsg_pktgen {
	struct netmsg_base	np_base;
	struct pktgen		*np_pktg;
	struct ifaltq_subque	*np_ifsq;
};

struct pktgen_buf {
	struct netmsg_base	pb_nmsg;	/* MUST BE THE FIRST */
	void			*pb_buf;
	volatile int		pb_done;
	int			pb_inuse;
	struct ifnet		*pb_ifp;
	struct ifaltq_subque	*pb_ifsq;
	int			pb_len;
	int			pb_cpuid;
	struct pktgen		*pb_pktg;
	LIST_ENTRY(pktgen_buf)	pb_link;
};

struct pktgen_pcpu {
	struct callout		pktg_stop;
	LIST_HEAD(, pktgen_buf)	pktg_buflist;
};

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

	int			pktg_duration;

	int			pktg_datalen;
	struct ifnet		*pktg_ifp;

	int			pktg_pktenq;

	struct sockaddr_in	pktg_src;
	int			pktg_ndst;
	struct sockaddr_in	*pktg_dst;
	uint8_t			pktg_dst_lladdr[ETHER_ADDR_LEN];

	struct pktgen_pcpu	pktg_pcpu[MAXCPU];
};

#define PKTG_F_CONFIG	0x1
#define PKTG_F_RUNNING	0x4

static int 		pktgen_modevent(module_t, int, void *);

static void		pktgen_buf_free(void *);
static void		pktgen_buf_ref(void *);
static void		pktgen_buf_send(netmsg_t);

static int		pktgen_config(struct pktgen *,
			    const struct pktgen_conf *);
static int		pktgen_start(struct pktgen *, int);
static void		pktgen_free(struct pktgen *);
static void		pktgen_ref(struct pktgen *);
static void		pktgen_pcpu_stop_cb(void *);
static void		pktgen_mbuf(struct pktgen_buf *, struct mbuf *);
static void		pktgen_start_ifsq(struct pktgen *,
			    struct ifaltq_subque *);
static void		pktgen_start_ifsq_handler(netmsg_t);

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, 0, D_MPSAFE },
	.d_open =	pktgen_open,
	.d_close =	pktgen_close,
	.d_ioctl =	pktgen_ioctl,
};

static volatile 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, i;

	switch (type) {
	case MOD_LOAD:
		for (i = 0; i < PKTGEN_DEVCNT; ++i) {
			make_dev(&pktgen_ops, 0, UID_ROOT, GID_WHEEL, 0600,
			    CDEV_NAME"%d", i);
		}
		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, i;

	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);
	for (i = 0; i < ncpus; ++i) {
		struct pktgen_pcpu *p = &pktg->pktg_pcpu[i];

		callout_init_mp(&p->pktg_stop);
		LIST_INIT(&p->pktg_buflist);
	}

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

	atomic_add_int(&pktgen_refcnt, 1);

	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);
	dev->si_drv1 = NULL;
	lwkt_reltoken(&pktgen_tok);

	pktgen_free(pktg);

	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, 0);
		break;

	case PKTGENMQSTART:
		error = pktgen_start(pktg, 1);
		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;
	struct sockaddr_in *dst = NULL;
	const struct sockaddr *sa;
	struct ifnet *ifp;
	size_t dst_size;
	int i, error, pktenq;

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

	if (conf->pc_datalen <= 0 ||
	    conf->pc_datalen > ETHERMTU - sizeof(struct udpiphdr))
		return EINVAL;
	if (conf->pc_duration <= 0)
		return EINVAL;

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

	if (conf->pc_ndst <= 0)
		return EINVAL;
	dst_size = conf->pc_ndst * sizeof(struct sockaddr_in);

	dst = kmalloc(dst_size, M_PKTGEN, M_WAITOK | M_NULLOK);
	if (dst == NULL)
		return ENOMEM;

	error = copyin(conf->pc_dst, dst, dst_size);
	if (error)
		goto failed;

	for (i = 0; i < conf->pc_ndst; ++i) {
		sin = &dst[i];
		if (sin->sin_family != AF_INET) {
			error = EPROTONOSUPPORT;
			goto failed;
		}
		if (sin->sin_port == 0) {
			error = EINVAL;
			goto failed;
		}
	}

	ifp = ifunit(conf->pc_ifname);
	if (ifp == NULL) {
		error = ENXIO;
		goto failed;
	}

	pktenq = conf->pc_pktenq;
	if (pktenq < 0 || pktenq > ifp->if_snd.altq_maxlen) {
		error = ENOBUFS;
		goto failed;
	} else if (pktenq == 0) {
		pktenq = (ifp->if_snd.altq_maxlen * 3) / 4;
	}

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

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

	pktg->pktg_duration = conf->pc_duration;
	pktg->pktg_datalen = conf->pc_datalen;
	pktg->pktg_pktenq = pktenq;
	pktg->pktg_ifp = ifp;
	pktg->pktg_src = conf->pc_src;
	pktg->pktg_ndst = conf->pc_ndst;
	KKASSERT(pktg->pktg_dst == NULL);
	pktg->pktg_dst = dst;
	bcopy(sa->sa_data, pktg->pktg_dst_lladdr, ETHER_ADDR_LEN);

	return 0;

failed:
	if (dst != NULL)
		kfree(dst, M_PKTGEN);
	return error;
}

static void
pktgen_start_ifsq(struct pktgen *pktg, struct ifaltq_subque *ifsq)
{
	struct netmsg_pktgen *np;

	np = kmalloc(sizeof(*np), M_LWKTMSG, M_WAITOK);
	netmsg_init(&np->np_base, NULL, &netisr_afree_rport, 0,
	    pktgen_start_ifsq_handler);
	np->np_pktg = pktg;
	np->np_ifsq = ifsq;

	lwkt_sendmsg(netisr_cpuport(ifsq_get_cpuid(ifsq)), &np->np_base.lmsg);
}

static int
pktgen_start(struct pktgen *pktg, int mq)
{
	struct ifaltq *ifq;

	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;

	ifq = &pktg->pktg_ifp->if_snd;
	if (!mq) {
		pktgen_ref(pktg);
		pktgen_start_ifsq(pktg, ifq_get_subq_default(ifq));
	} else {
		int i;

		for (i = 0; i < ifq->altq_subq_cnt; ++i)
			pktgen_ref(pktg);
		for (i = 0; i < ifq->altq_subq_cnt; ++i)
			pktgen_start_ifsq(pktg, ifq_get_subq(ifq, i));
	}
	return 0;
}

static void
pktgen_start_ifsq_handler(netmsg_t nmsg)
{
	struct netmsg_pktgen *np = (struct netmsg_pktgen *)nmsg;
	struct pktgen *pktg = np->np_pktg;
	struct ifaltq_subque *ifsq = np->np_ifsq;

	struct mbuf *m, *head = NULL, **next;
	struct ifnet *ifp;
	struct pktgen_pcpu *p;
	int cpuid, i, alloc_cnt, keep_cnt;

	u_short ulen, psum;
	int len, ip_len;

	/* Reply ASAP */
	lwkt_replymsg(&np->np_base.lmsg, 0);

	ifp = pktg->pktg_ifp;

	cpuid = ifsq_get_cpuid(ifsq);
	KKASSERT(cpuid == mycpuid);

	p = &pktg->pktg_pcpu[cpuid];

	keep_cnt = pktg->pktg_pktenq;
	alloc_cnt = keep_cnt * 2;

	/*
	 * Prefault enough mbuf into mbuf objcache
	 */
	next = &head;
	for (i = 0; i < alloc_cnt; ++i) {
		MGETHDR(m, MB_WAIT, MT_DATA);
		*next = m;
		next = &m->m_nextpkt;
	}

	for (i = 0; i < alloc_cnt - keep_cnt; ++i) {
		m = head;
		head = m->m_nextpkt;
		m->m_nextpkt = NULL;
		m_freem(m);
	}
	KKASSERT(head != NULL);

	/*
	 * Setup the packets' data
	 */
	ip_len = pktg->pktg_datalen + sizeof(struct udpiphdr);
	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));

	m = head;
	i = 0;
	while (m != NULL) {
		struct mbuf *nextm;
		const struct sockaddr_in *dst;
		struct pktgen_buf *pb;
		struct ip *ip;
		struct udpiphdr *ui;
		struct ether_header *eh;

		pktgen_ref(pktg);

		pb = kmalloc(sizeof(*pb), M_PKTGEN, M_WAITOK | M_ZERO);
		pb->pb_ifp = ifp;
		pb->pb_ifsq = ifsq;
		pb->pb_inuse = 1;
		pb->pb_buf = kmalloc(PKTGEN_BUFSZ, M_PKTGEN, M_WAITOK);
		pb->pb_len = len;
		pb->pb_cpuid = cpuid;
		pb->pb_pktg = pktg;
		netmsg_init(&pb->pb_nmsg, NULL, &netisr_adone_rport, 0,
		    pktgen_buf_send);
		LIST_INSERT_HEAD(&p->pktg_buflist, pb, pb_link);

		dst = &pktg->pktg_dst[i % pktg->pktg_ndst];
		++i;

		m->m_ext.ext_arg = pb;
		m->m_ext.ext_buf = pb->pb_buf;
		m->m_ext.ext_free = pktgen_buf_free;
		m->m_ext.ext_ref = pktgen_buf_ref;
		m->m_ext.ext_size = PKTGEN_BUFSZ;

		m->m_data = m->m_ext.ext_buf;
		m->m_flags |= M_EXT;
		m->m_len = m->m_pkthdr.len = len;

		m->m_data += ETHER_HDR_LEN;
		m->m_len -= ETHER_HDR_LEN;
		m->m_pkthdr.len -= ETHER_HDR_LEN;

		ui = mtod(m, struct udpiphdr *);
		ui->ui_pr = IPPROTO_UDP;
		ui->ui_src.s_addr = pktg->pktg_src.sin_addr.s_addr;
		ui->ui_dst.s_addr = dst->sin_addr.s_addr;
		ui->ui_sport = pktg->pktg_src.sin_port;
		ui->ui_dport = dst->sin_port;
		ui->ui_ulen = ulen;
		ui->ui_sum = in_pseudo(ui->ui_src.s_addr, ui->ui_dst.s_addr,
		    psum);
		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_sum = 0;
		ip->ip_id = ip_newid();

		in_delayed_cksum(m);

		ip->ip_len = htons(ip->ip_len);
		ip->ip_sum = in_cksum_hdr(ip);

		m->m_data -= ETHER_HDR_LEN;
		m->m_len += ETHER_HDR_LEN;
		m->m_pkthdr.len += ETHER_HDR_LEN;

		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);

		nextm = m->m_nextpkt;
		m->m_nextpkt = NULL;

		ifq_dispatch(ifp, m, NULL);

		m = nextm;
	}

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

	pktgen_free(pktg);
}

static void
pktgen_mbuf(struct pktgen_buf *pb, struct mbuf *m)
{
	m->m_ext.ext_arg = pb;
	m->m_ext.ext_buf = pb->pb_buf;
	m->m_ext.ext_free = pktgen_buf_free;
	m->m_ext.ext_ref = pktgen_buf_ref;
	m->m_ext.ext_size = PKTGEN_BUFSZ;

	m->m_data = m->m_ext.ext_buf;
	m->m_flags |= M_EXT;
	m->m_len = m->m_pkthdr.len = pb->pb_len;
}

static void
pktgen_buf_send(netmsg_t msg)
{
	struct pktgen_buf *pb = (struct pktgen_buf *)msg;
	struct mbuf *m;

	KKASSERT(&curthread->td_msgport == netisr_cpuport(pb->pb_cpuid));

	crit_enter();
	lwkt_replymsg(&pb->pb_nmsg.lmsg, 0);
	crit_exit();

	MGETHDR(m, MB_WAIT, MT_DATA);
	pktgen_mbuf(pb, m);
	ifq_dispatch(pb->pb_ifp, m, NULL);
}

static void
pktgen_buf_free(void *arg)
{
	struct pktgen_buf *pb = arg;
	struct mbuf *m;

	KKASSERT(pb->pb_inuse > 0);
	if (pb->pb_done) {
		if (atomic_fetchadd_int(&pb->pb_inuse, -1) == 1) {
			struct pktgen *pktg;

			pktg = pb->pb_pktg;
			crit_enter();
			LIST_REMOVE(pb, pb_link);
			crit_exit();
			kfree(pb->pb_buf, M_PKTGEN);
			kfree(pb, M_PKTGEN);

			pktgen_free(pktg);
		}
		return;
	}

	if (&curthread->td_msgport != netisr_cpuport(pb->pb_cpuid)) {
		KKASSERT(pb->pb_cpuid == mycpuid);
		crit_enter();
		KKASSERT(pb->pb_nmsg.lmsg.ms_flags & MSGF_DONE);
		lwkt_sendmsg(netisr_cpuport(pb->pb_cpuid), &pb->pb_nmsg.lmsg);
		crit_exit();
		return;
	}

	MGETHDR(m, MB_WAIT, MT_DATA);
	pktgen_mbuf(pb, m);
	ifsq_enqueue(pb->pb_ifsq, m, NULL);
}

static void
pktgen_buf_ref(void *arg)
{
	struct pktgen_buf *pb = arg;

	panic("%s should never be called\n", __func__);

	KKASSERT(pb->pb_inuse > 0);
	atomic_add_int(&pb->pb_inuse, 1);
}

static void
pktgen_free(struct pktgen *pktg)
{
	KKASSERT(pktg->pktg_refcnt > 0);
	if (atomic_fetchadd_int(&pktg->pktg_refcnt, -1) == 1) {
		int i;

		if (pktg->pktg_dst != NULL)
			kfree(pktg->pktg_dst, M_PKTGEN);

		for (i = 0; i < ncpus; ++i)
			KKASSERT(LIST_EMPTY(&pktg->pktg_pcpu[i].pktg_buflist));
		kfree(pktg, M_PKTGEN);
	}

	KKASSERT(pktgen_refcnt > 0);
	atomic_subtract_int(&pktgen_refcnt, 1);
}

static void
pktgen_pcpu_stop_cb(void *arg)
{
	struct pktgen_pcpu *p = arg;
	struct pktgen_buf *pb;

	crit_enter();
	LIST_FOREACH(pb, &p->pktg_buflist, pb_link)
		pb->pb_done = 1;
	crit_exit();
}

static void
pktgen_ref(struct pktgen *pktg)
{
	atomic_add_int(&pktg->pktg_refcnt, 1);
	atomic_add_int(&pktgen_refcnt, 1);
}
Commit	Line	Data
4fa8e46e	1	/*
391741ec	2	* Copyright (c) 2012 The DragonFly Project. All rights reserved.
4fa8e46e SZ	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>
391741ec	48	#include <sys/queue.h>
4fa8e46e SZ	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>
391741ec	58	#include <net/netmsg2.h>
5337421c	59	#include <net/netisr2.h>
4fa8e46e SZ	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"
4fa8e46e	68
391741ec SZ	69	#define PKTGEN_BUFSZ 2048
391741ec SZ	70
390d8a05 SZ	71	#ifndef PKTGEN_DEVCNT
	72	#define PKTGEN_DEVCNT 4
	73	#endif
	74
391741ec SZ	75	struct pktgen;
391741ec SZ	76
b0cd2b4b SZ	77	struct netmsg_pktgen {
	78	struct netmsg_base np_base;
	79	struct pktgen *np_pktg;
	80	struct ifaltq_subque *np_ifsq;
	81	};
	82
391741ec SZ	83	struct pktgen_buf {
	84	struct netmsg_base pb_nmsg; /* MUST BE THE FIRST */
	85	void *pb_buf;
	86	volatile int pb_done;
4c9906cb	87	int pb_inuse;
391741ec	88	struct ifnet *pb_ifp;
2fb36fab	89	struct ifaltq_subque *pb_ifsq;
391741ec SZ	90	int pb_len;
	91	int pb_cpuid;
	92	struct pktgen *pb_pktg;
	93	LIST_ENTRY(pktgen_buf) pb_link;
	94	};
	95
1bbaa2ad SZ	96	struct pktgen_pcpu {
	97	struct callout pktg_stop;
	98	LIST_HEAD(, pktgen_buf) pktg_buflist;
	99	};
	100
4fa8e46e SZ	101	struct pktgen {
	102	uint32_t pktg_flags; /* PKTG_F_ */
	103	int pktg_refcnt;
	104
4fa8e46e	105	int pktg_duration;
4fa8e46e SZ	106
4fa8e46e SZ	107	int pktg_datalen;
4fa8e46e	108	struct ifnet *pktg_ifp;
c95ebcd6	109
793f802f SZ	110	int pktg_pktenq;
793f802f SZ	111
391741ec SZ	112	struct sockaddr_in pktg_src;
	113	int pktg_ndst;
	114	struct sockaddr_in *pktg_dst;
4fa8e46e	115	uint8_t pktg_dst_lladdr[ETHER_ADDR_LEN];
391741ec	116
1bbaa2ad	117	struct pktgen_pcpu pktg_pcpu[MAXCPU];
4fa8e46e SZ	118	};
	119
	120	#define PKTG_F_CONFIG 0x1
4fa8e46e SZ	121	#define PKTG_F_RUNNING 0x4
	122
	123	static int pktgen_modevent(module_t, int, void *);
391741ec SZ	124
	125	static void pktgen_buf_free(void *);
	126	static void pktgen_buf_ref(void *);
	127	static void pktgen_buf_send(netmsg_t);
	128
4fa8e46e	129	static int pktgen_config(struct pktgen *,
391741ec	130	const struct pktgen_conf *);
b0cd2b4b	131	static int pktgen_start(struct pktgen *, int);
391741ec	132	static void pktgen_free(struct pktgen *);
c6830078	133	static void pktgen_ref(struct pktgen *);
1bbaa2ad	134	static void pktgen_pcpu_stop_cb(void *);
391741ec	135	static void pktgen_mbuf(struct pktgen_buf , struct mbuf );
b0cd2b4b SZ	136	static void pktgen_start_ifsq(struct pktgen *,
	137	struct ifaltq_subque *);
	138	static void pktgen_start_ifsq_handler(netmsg_t);
4fa8e46e SZ	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 = {
624409c2	145	{ CDEV_NAME, 0, D_MPSAFE },
4fa8e46e SZ	146	.d_open = pktgen_open,
	147	.d_close = pktgen_close,
	148	.d_ioctl = pktgen_ioctl,
	149	};
	150
4c9906cb	151	static volatile int pktgen_refcnt;
e3d0d8d7	152	static struct lwkt_token pktgen_tok = LWKT_TOKEN_INITIALIZER(pktgen_token);
4fa8e46e SZ	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	{
390d8a05	162	int error = 0, i;
4fa8e46e SZ	163
	164	switch (type) {
	165	case MOD_LOAD:
390d8a05 SZ	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	}
4fa8e46e SZ	170	break;
	171
	172	case MOD_UNLOAD:
	173	if (pktgen_refcnt > 0)
	174	return EBUSY;
4b3b3198	175	dev_ops_remove_all(&pktgen_ops);
4fa8e46e SZ	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;
1bbaa2ad	190	int error, i;
4fa8e46e	191
9eaf083f	192	error = priv_check_cred(ap->a_cred, PRIV_ROOT, 0);
4fa8e46e SZ	193	if (error)
	194	return error;
	195
e3d0d8d7	196	lwkt_gettoken(&pktgen_tok);
4fa8e46e SZ	197
4fa8e46e SZ	198	if (dev->si_drv1 != NULL) {
e3d0d8d7	199	lwkt_reltoken(&pktgen_tok);
4fa8e46e SZ	200	return EBUSY;
	201	}
	202
	203	pktg = kmalloc(sizeof(*pktg), M_PKTGEN, M_ZERO \| M_WAITOK);
1bbaa2ad SZ	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	}
4fa8e46e	210
4fa8e46e SZ	211	dev->si_drv1 = pktg;
	212	pktg->pktg_refcnt = 1;
	213
4c9906cb	214	atomic_add_int(&pktgen_refcnt, 1);
4fa8e46e	215
e3d0d8d7	216	lwkt_reltoken(&pktgen_tok);
4fa8e46e SZ	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
e3d0d8d7	226	lwkt_gettoken(&pktgen_tok);
4fa8e46e	227	dev->si_drv1 = NULL;
e3d0d8d7	228	lwkt_reltoken(&pktgen_tok);
391741ec	229
4c9906cb SZ	230	pktgen_free(pktg);
4c9906cb SZ	231
4fa8e46e SZ	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
e3d0d8d7	243	lwkt_gettoken(&pktgen_tok);
4fa8e46e SZ	244
	245	switch (ap->a_cmd) {
	246	case PKTGENSTART:
b0cd2b4b SZ	247	error = pktgen_start(pktg, 0);
	248	break;
	249
	250	case PKTGENMQSTART:
	251	error = pktgen_start(pktg, 1);
4fa8e46e SZ	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
e3d0d8d7	263	lwkt_reltoken(&pktgen_tok);
4fa8e46e SZ	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;
391741ec	271	struct sockaddr_in *dst = NULL;
4fa8e46e SZ	272	const struct sockaddr *sa;
4fa8e46e SZ	273	struct ifnet *ifp;
391741ec	274	size_t dst_size;
793f802f	275	int i, error, pktenq;
4fa8e46e	276
391741ec	277	if (pktg->pktg_flags & (PKTG_F_RUNNING \| PKTG_F_CONFIG))
4fa8e46e SZ	278	return EBUSY;
4fa8e46e SZ	279
391741ec SZ	280	if (conf->pc_datalen <= 0 \|\|
391741ec SZ	281	conf->pc_datalen > ETHERMTU - sizeof(struct udpiphdr))
4fa8e46e SZ	282	return EINVAL;
	283	if (conf->pc_duration <= 0)
	284	return EINVAL;
	285
4fa8e46e SZ	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
391741ec	292	if (conf->pc_ndst <= 0)
4fa8e46e	293	return EINVAL;
391741ec SZ	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;
4fa8e46e	299
391741ec SZ	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
793f802f	322	pktenq = conf->pc_pktenq;
2fb36fab	323	if (pktenq < 0 \|\| pktenq > ifp->if_snd.altq_maxlen) {
793f802f SZ	324	error = ENOBUFS;
	325	goto failed;
	326	} else if (pktenq == 0) {
2fb36fab	327	pktenq = (ifp->if_snd.altq_maxlen * 3) / 4;
793f802f SZ	328	}
793f802f SZ	329
391741ec SZ	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	*/
4fa8e46e	348	pktg->pktg_flags \|= PKTG_F_CONFIG;
4fa8e46e SZ	349
4fa8e46e SZ	350	pktg->pktg_duration = conf->pc_duration;
4fa8e46e	351	pktg->pktg_datalen = conf->pc_datalen;
793f802f	352	pktg->pktg_pktenq = pktenq;
391741ec SZ	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;
4fa8e46e	358	bcopy(sa->sa_data, pktg->pktg_dst_lladdr, ETHER_ADDR_LEN);
c95ebcd6	359
4fa8e46e	360	return 0;
391741ec SZ	361
	362	failed:
	363	if (dst != NULL)
	364	kfree(dst, M_PKTGEN);
	365	return error;
4fa8e46e SZ	366	}
4fa8e46e SZ	367
b0cd2b4b SZ	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
ec7f7fc8	379	lwkt_sendmsg(netisr_cpuport(ifsq_get_cpuid(ifsq)), &np->np_base.lmsg);
b0cd2b4b SZ	380	}
b0cd2b4b SZ	381
4fa8e46e	382	static int
b0cd2b4b	383	pktgen_start(struct pktgen *pktg, int mq)
4fa8e46e	384	{
b0cd2b4b SZ	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
391741ec SZ	415	struct mbuf m, head = NULL, **next;
391741ec SZ	416	struct ifnet *ifp;
1bbaa2ad	417	struct pktgen_pcpu *p;
b0cd2b4b	418	int cpuid, i, alloc_cnt, keep_cnt;
391741ec SZ	419
	420	u_short ulen, psum;
	421	int len, ip_len;
	422
b0cd2b4b SZ	423	/* Reply ASAP */
b0cd2b4b SZ	424	lwkt_replymsg(&np->np_base.lmsg, 0);
4fa8e46e	425
391741ec	426	ifp = pktg->pktg_ifp;
4fa8e46e	427
2fb36fab	428	cpuid = ifsq_get_cpuid(ifsq);
b0cd2b4b	429	KKASSERT(cpuid == mycpuid);
4fa8e46e	430
1bbaa2ad SZ	431	p = &pktg->pktg_pcpu[cpuid];
1bbaa2ad SZ	432
793f802f SZ	433	keep_cnt = pktg->pktg_pktenq;
793f802f SZ	434	alloc_cnt = keep_cnt * 2;
4fa8e46e	435
391741ec SZ	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	}
4fa8e46e	445
391741ec SZ	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);
4fa8e46e	453
391741ec SZ	454	/*
	455	* Setup the packets' data
	456	*/
	457	ip_len = pktg->pktg_datalen + sizeof(struct udpiphdr);
4fa8e46e SZ	458	len = ip_len + ETHER_HDR_LEN;
4fa8e46e SZ	459
391741ec SZ	460	psum = htons((u_short)pktg->pktg_datalen + sizeof(struct udphdr) +
391741ec SZ	461	IPPROTO_UDP);
4fa8e46e SZ	462	ulen = htons(pktg->pktg_datalen + sizeof(struct udphdr));
4fa8e46e SZ	463
391741ec SZ	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
c6830078	474	pktgen_ref(pktg);
391741ec SZ	475
	476	pb = kmalloc(sizeof(*pb), M_PKTGEN, M_WAITOK \| M_ZERO);
	477	pb->pb_ifp = ifp;
2fb36fab	478	pb->pb_ifsq = ifsq;
391741ec SZ	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);
1bbaa2ad	486	LIST_INSERT_HEAD(&p->pktg_buflist, pb, pb_link);
391741ec SZ	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;
4fa8e46e SZ	499	m->m_len = m->m_pkthdr.len = len;
4fa8e46e SZ	500
391741ec SZ	501	m->m_data += ETHER_HDR_LEN;
	502	m->m_len -= ETHER_HDR_LEN;
	503	m->m_pkthdr.len -= ETHER_HDR_LEN;
4fa8e46e SZ	504
	505	ui = mtod(m, struct udpiphdr *);
	506	ui->ui_pr = IPPROTO_UDP;
391741ec SZ	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;
4fa8e46e	511	ui->ui_ulen = ulen;
391741ec SZ	512	ui->ui_sum = in_pseudo(ui->ui_src.s_addr, ui->ui_dst.s_addr,
391741ec SZ	513	psum);
64c94a5f	514	m->m_pkthdr.csum_data = offsetof(struct udphdr, uh_sum);
4fa8e46e SZ	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;
2640b9e5	522	ip->ip_sum = 0;
4fa8e46e SZ	523	ip->ip_id = ip_newid();
4fa8e46e SZ	524
391741ec	525	in_delayed_cksum(m);
4fa8e46e SZ	526
4fa8e46e SZ	527	ip->ip_len = htons(ip->ip_len);
391741ec SZ	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;
c95ebcd6	533
c95ebcd6 SZ	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
391741ec SZ	539	nextm = m->m_nextpkt;
391741ec SZ	540	m->m_nextpkt = NULL;
c95ebcd6	541
391741ec	542	ifq_dispatch(ifp, m, NULL);
c95ebcd6	543
391741ec SZ	544	m = nextm;
391741ec SZ	545	}
c95ebcd6	546
1bbaa2ad SZ	547	callout_reset(&p->pktg_stop, pktg->pktg_duration * hz,
1bbaa2ad SZ	548	pktgen_pcpu_stop_cb, p);
c95ebcd6	549
b0cd2b4b	550	pktgen_free(pktg);
391741ec	551	}
c95ebcd6	552
391741ec SZ	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	}
c95ebcd6	566
391741ec SZ	567	static void
	568	pktgen_buf_send(netmsg_t msg)
	569	{
	570	struct pktgen_buf pb = (struct pktgen_buf )msg;
	571	struct mbuf *m;
c95ebcd6	572
ec7f7fc8	573	KKASSERT(&curthread->td_msgport == netisr_cpuport(pb->pb_cpuid));
c95ebcd6	574
391741ec SZ	575	crit_enter();
	576	lwkt_replymsg(&pb->pb_nmsg.lmsg, 0);
	577	crit_exit();
c95ebcd6	578
391741ec SZ	579	MGETHDR(m, MB_WAIT, MT_DATA);
	580	pktgen_mbuf(pb, m);
	581	ifq_dispatch(pb->pb_ifp, m, NULL);
	582	}
c95ebcd6	583
391741ec SZ	584	static void
	585	pktgen_buf_free(void *arg)
	586	{
	587	struct pktgen_buf *pb = arg;
	588	struct mbuf *m;
c95ebcd6	589
391741ec SZ	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;
c95ebcd6	594
391741ec	595	pktg = pb->pb_pktg;
4c9906cb	596	crit_enter();
391741ec	597	LIST_REMOVE(pb, pb_link);
4c9906cb	598	crit_exit();
391741ec SZ	599	kfree(pb->pb_buf, M_PKTGEN);
391741ec SZ	600	kfree(pb, M_PKTGEN);
4fa8e46e	601
391741ec	602	pktgen_free(pktg);
4fa8e46e	603	}
391741ec	604	return;
4fa8e46e	605	}
db161a19	606
ec7f7fc8	607	if (&curthread->td_msgport != netisr_cpuport(pb->pb_cpuid)) {
391741ec SZ	608	KKASSERT(pb->pb_cpuid == mycpuid);
	609	crit_enter();
	610	KKASSERT(pb->pb_nmsg.lmsg.ms_flags & MSGF_DONE);
ec7f7fc8	611	lwkt_sendmsg(netisr_cpuport(pb->pb_cpuid), &pb->pb_nmsg.lmsg);
391741ec SZ	612	crit_exit();
	613	return;
	614	}
	615
	616	MGETHDR(m, MB_WAIT, MT_DATA);
	617	pktgen_mbuf(pb, m);
2fb36fab	618	ifsq_enqueue(pb->pb_ifsq, m, NULL);
db161a19 SZ	619	}
	620
	621	static void
391741ec	622	pktgen_buf_ref(void *arg)
db161a19	623	{
391741ec	624	struct pktgen_buf *pb = arg;
db161a19	625
391741ec	626	panic("%s should never be called\n", __func__);
4fa8e46e	627
391741ec SZ	628	KKASSERT(pb->pb_inuse > 0);
	629	atomic_add_int(&pb->pb_inuse, 1);
	630	}
4fa8e46e	631
391741ec SZ	632	static void
	633	pktgen_free(struct pktgen *pktg)
	634	{
4fa8e46e	635	KKASSERT(pktg->pktg_refcnt > 0);
4c9906cb	636	if (atomic_fetchadd_int(&pktg->pktg_refcnt, -1) == 1) {
1bbaa2ad SZ	637	int i;
1bbaa2ad SZ	638
391741ec SZ	639	if (pktg->pktg_dst != NULL)
391741ec SZ	640	kfree(pktg->pktg_dst, M_PKTGEN);
1bbaa2ad SZ	641
	642	for (i = 0; i < ncpus; ++i)
	643	KKASSERT(LIST_EMPTY(&pktg->pktg_pcpu[i].pktg_buflist));
391741ec SZ	644	kfree(pktg, M_PKTGEN);
391741ec SZ	645	}
4fa8e46e SZ	646
4fa8e46e SZ	647	KKASSERT(pktgen_refcnt > 0);
4c9906cb	648	atomic_subtract_int(&pktgen_refcnt, 1);
391741ec SZ	649	}
	650
	651	static void
1bbaa2ad	652	pktgen_pcpu_stop_cb(void *arg)
391741ec	653	{
1bbaa2ad	654	struct pktgen_pcpu *p = arg;
391741ec	655	struct pktgen_buf *pb;
4fa8e46e	656
4c9906cb	657	crit_enter();
1bbaa2ad	658	LIST_FOREACH(pb, &p->pktg_buflist, pb_link)
391741ec	659	pb->pb_done = 1;
4c9906cb	660	crit_exit();
4fa8e46e	661	}
c6830078 SZ	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	}