/*
 * Copyright (c) 2007 Jiri Slaby <jirislaby@gmail.com>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * Neither the name of the <organization> nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/mbuf.h>
#include <sys/device.h>
#include <sys/sockio.h>
#include <sys/socket.h>
#include <sys/queue.h>
#include <sys/callout.h>
#include <sys/lkm.h>

#include <net/bpf.h>
#include <net/if.h>
#include <net/if_dl.h>
#include <net/if_ether.h>
#include <net/if_media.h>

#define NTT		"nettest"
#define NTT_IFNAME	"ntt"
#define NTT_PORTS	2
#define NTT_PKTSIZE	1600
#define NTT_INIT_PKTS	100

struct ntt_packet {
	struct mbuf *mbuf;
	SIMPLEQ_ENTRY(ntt_packet) list;
};

SIMPLEQ_HEAD(packet_l, ntt_packet);

struct ntt_softc {
	struct ifmedia		sc_im[NTT_PORTS];
	struct ethercom		sc_ec[NTT_PORTS];

	struct packet_l pool, fpool;

	struct callout co_rx;
};

static int ntt_mediachange(struct ifnet *);
static void ntt_mediastat(struct ifnet *, struct ifmediareq *);

static void ntt_start(struct ifnet *);
static void ntt_stop(struct ifnet *, int);
static int ntt_init(struct ifnet *);
static int ntt_ioctl(struct ifnet *, u_long, caddr_t);

#if 0
static void
ntt_hexdump_packet(struct ntt_softc *sc, struct mbuf *m)
{
	size_t size;
	u_int8_t *data;
	unsigned int i, j = 0;

	if ((m->m_flags & M_PKTHDR) == 0) {
		printf("hexdump: expected packet header\n");
		return;
	}

	while (m != NULL) {
		size = m->m_len;
		data = mtod(m, u_int8_t *);

		for (i = 0; i < size; i++, j++) {
			printf("%02x ", data[i]);
			if (((j + 1) % 16) == 0)
				printf("\n");
		}

		m = m->m_next;
	}

	if ((j + 1) % 16)
		printf("\n");
}
#else
#define ntt_hexdump_packet(x,y) do { } while (0)
#endif

static unsigned int
ntt_alloc_pool(struct ntt_softc *sc, unsigned int count, int flags)
{
	struct ntt_packet *packet;
	unsigned int a;

	printf("allocating another %u\n", count);
	for (a = 0; a < count; a++) {
		packet = malloc(sizeof(*packet), M_TEMP, flags);
		if (packet == NULL)
			break;
		SIMPLEQ_INSERT_TAIL(&sc->fpool, packet, list);
	}

	return (a);
}

/*
 * Interface stuff
 */
static void
ntt_rx(void *param)
{
	static unsigned int b;
	struct ntt_softc *sc = param;
	struct ifnet *ifp;
	struct ntt_packet *packet;
	unsigned int a = 0;

	while (!SIMPLEQ_EMPTY(&sc->pool)) {
		packet = SIMPLEQ_FIRST(&sc->pool);
		SIMPLEQ_REMOVE_HEAD(&sc->pool, list);
		ifp = packet->mbuf->m_pkthdr.rcvif;

		if (!b++)
			printf("we're about to receive the first packet\n");

		if (ifp->if_bpf != NULL)
			bpf_mtap(ifp->if_bpf, packet->mbuf);

		ifp->if_input(ifp, packet->mbuf);
		ifp->if_ipackets++;
		SIMPLEQ_INSERT_TAIL(&sc->fpool, packet, list);
		a++;
	}
/*	printf("packets rxed: %u\n", a);*/
}

static int
ntt_tx(struct ntt_softc *sc, struct mbuf *m, int unit)
{
	struct ifnet *ifp = &sc->sc_ec[unit].ec_if;
	struct ntt_packet *packet;
	unsigned int octets;
	struct mbuf *mp;

	if ((m->m_flags & M_PKTHDR) == 0)
		panic(NTT_IFNAME "%d: no header mbuf", unit);

	octets = m_length(m);
	if (octets < ETHER_MIN_LEN - ETHER_CRC_LEN) {
		printf(NTT_IFNAME "%d: too short, padding\n", unit);

		MGETHDR(mp, M_DONTWAIT, MT_DATA);
		if (m == NULL) {
			printf(NTT ": unable to allocate Tx mbuf\n");
			goto fail_0;
		}
		m_copydata(m, 0, octets, mtod(mp, caddr_t));
		memset(mtod(mp, u_char *) + octets, 0, ETHER_MIN_LEN -
				ETHER_CRC_LEN - octets);
		mp->m_pkthdr.len = mp->m_len = octets = ETHER_MIN_LEN -
				ETHER_CRC_LEN;
		m_freem(m);
		m = mp;
	}

	ntt_hexdump_packet(sc, m);

	if (ifp->if_bpf != NULL)
		bpf_mtap(ifp->if_bpf, m);

	if (SIMPLEQ_EMPTY(&sc->fpool))
		if (ntt_alloc_pool(sc, 30, M_NOWAIT) == 0)
			goto fail_0;

	packet = SIMPLEQ_FIRST(&sc->fpool);
	SIMPLEQ_REMOVE_HEAD(&sc->fpool, list);
	m->m_pkthdr.rcvif = &sc->sc_ec[!unit].ec_if;
	packet->mbuf = m;
	SIMPLEQ_INSERT_TAIL(&sc->pool, packet, list);
	if (!callout_pending(&sc->co_rx))
		callout_schedule(&sc->co_rx, 10);

	ifp->if_opackets++;

	return (0);
fail_0:
	ifp->if_oerrors++;
	m_freem(m);
	return ENOMEM;
}

static int
ntt_unit(struct ntt_softc *sc, struct ifnet *ifp)
{
	unsigned int i;

	for (i = 0; i < NTT_PORTS; i++)
		if (&sc->sc_ec[i].ec_if == ifp)
			return (i);

	return (-1);
}

static int
ntt_mediachange(struct ifnet *ifp)
{
	return (0);
}

static void
ntt_mediastat(struct ifnet *ifp, struct ifmediareq *imr)
{
	imr->ifm_active = IFM_ETHER | IFM_1000_T;
}

/*
 * This is the function where we SEND packets. We run under splnet().
 */
static void
ntt_start(struct ifnet *ifp)
{
	struct ntt_softc *sc = (struct ntt_softc *)ifp->if_softc;
	struct mbuf *m;
	unsigned int counter = 0;
	int unit;

	unit = ntt_unit(sc, ifp);
	if (unit == -1)
		panic(NTT ": invalid ifp unit");

	while (counter++ < 100) {
		IFQ_DEQUEUE(&ifp->if_snd, m);
		if (m == NULL)
			break;
		if (ntt_tx(sc, m, unit))
			break;
	}
}

/*
 * Note that both ifmedia_ioctl() and ether_ioctl() have to be
 * called under splnet().
 */
static int
ntt_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
{
	struct ntt_softc *sc = (struct ntt_softc *)ifp->if_softc;
	struct ifreq *ifr = (struct ifreq *)data;
	int s, unit, retval = 0;

	unit = ntt_unit(sc, ifp);
	if (unit == -1)
		return (ENXIO);

	s = splnet();

	switch (cmd) {
	case SIOCSIFMEDIA:
	case SIOCGIFMEDIA:
		retval = ifmedia_ioctl(ifp, ifr, &sc->sc_im[unit], cmd);
		break;
	case SIOCSIFMTU:
		if (ifr->ifr_mtu > NTT_PKTSIZE) {
			retval = EINVAL;
			break;
		}
		ifp->if_mtu = ifr->ifr_mtu;
		break;
	default:
		retval = ether_ioctl(ifp, cmd, data);
		if (retval == ENETRESET)
			retval = 0;
	}

	splx(s);

	return (retval);
}

/*
 * _init() would typically be called when an interface goes up,
 * meaning it should configure itself into the state in which it
 * can send packets.
 */
static int
ntt_init(struct ifnet *ifp)
{
	struct ntt_softc *sc = (struct ntt_softc *)ifp->if_softc;
	int unit, s;

	unit = ntt_unit(sc, ifp);
	if (unit == -1)
		panic(NTT ": invalid ifp unit");

	printf(NTT_IFNAME "%d: going up\n", unit);

	s = splnet();
	ifp->if_flags |= IFF_RUNNING;
	splx(s);

	return (0);
}

/*
 * _stop() is called when an interface goes down.  It is our
 * responsability to validate that state by clearing the
 * IFF_RUNNING flag.
 */
static void
ntt_stop(struct ifnet *ifp, int disable)
{
	struct ntt_softc *sc = (struct ntt_softc *)ifp->if_softc;
	struct ntt_packet *packet;
	unsigned int a = 0;
	int unit, s;

	unit = ntt_unit(sc, ifp);
	if (unit == -1)
		panic(NTT ": invalid ifp unit");

	printf(NTT_IFNAME "%d: going down\n", unit);

	s = splnet();
	ifp->if_flags &= ~IFF_RUNNING;
	callout_stop(&sc->co_rx);
	SIMPLEQ_FOREACH(packet, &sc->fpool, list)
		a++;
	while (!SIMPLEQ_EMPTY(&sc->pool)) {
		packet = SIMPLEQ_FIRST(&sc->pool);
		SIMPLEQ_REMOVE_HEAD(&sc->pool, list);
		m_freem(packet->mbuf);
		if (a++ < NTT_INIT_PKTS)
			SIMPLEQ_INSERT_TAIL(&sc->fpool, packet, list);
		else
			free(packet, M_TEMP);
	}
	splx(s);
}

static struct ntt_softc sc_glob;

static int
nettest_init(struct lkm_table *lkmtp, int cmd)
{
	struct ntt_softc *sc = &sc_glob;
	struct ifnet *ifp;
	u_int8_t enaddr[ETHER_ADDR_LEN] = "hello";
	unsigned int j;

	if (lkmexists(lkmtp))
		return (EEXIST);

	SIMPLEQ_INIT(&sc->pool);
	SIMPLEQ_INIT(&sc->fpool);

	ntt_alloc_pool(sc, NTT_INIT_PKTS, M_WAIT);
	
	callout_init(&sc->co_rx);
	callout_setfunc(&sc->co_rx, ntt_rx, sc);

	for (j = 0; j < NTT_PORTS; j++) {
		enaddr[5] = (u_int8_t) j;

		printf(NTT_IFNAME "%d: Ethernet address %s\n", j,
			ether_sprintf(enaddr));

		ifmedia_init(&sc->sc_im[j], 0, ntt_mediachange, ntt_mediastat);
		ifmedia_add(&sc->sc_im[j], IFM_ETHER|IFM_1000_T, 0, NULL);
		ifmedia_set(&sc->sc_im[j], IFM_ETHER|IFM_1000_T);

		/*
		 * One should note that an interface must do multicast in
		 * order to support IPv6.
		 */
		ifp = &sc->sc_ec[j].ec_if;
		sprintf(ifp->if_xname, NTT_IFNAME "%d", j);
		ifp->if_softc	= sc;
		ifp->if_flags	= IFF_BROADCAST | IFF_SIMPLEX | IFF_MULTICAST |
			IFF_PROMISC;
		ifp->if_ioctl	= ntt_ioctl;
		ifp->if_start	= ntt_start;
		ifp->if_stop	= ntt_stop;
		ifp->if_init	= ntt_init;
		IFQ_SET_READY(&ifp->if_snd);

		sc->sc_ec[j].ec_capabilities = ETHERCAP_VLAN_MTU;

		if_attach(ifp);
		ether_ifattach(ifp, enaddr);
	}

	return (0);
}

static int
nettest_exit(struct lkm_table *lkmtp, int cmd)
{
	struct ntt_softc *sc = &sc_glob;
	struct ntt_packet *packet;
	unsigned int i;

	for (i = 0; i < NTT_PORTS; i++)
		if (sc->sc_ec[i].ec_if.if_flags & IFF_RUNNING)
			break;
	if (i != NTT_PORTS)
		return (EBUSY);

	callout_stop(&sc->co_rx);

	for (i = 0; i < NTT_PORTS; i++) {
		ether_ifdetach(&sc->sc_ec[i].ec_if);
		if_detach(&sc->sc_ec[i].ec_if);
		ifmedia_delete_instance(&sc->sc_im[i], IFM_INST_ANY);
	}

	while (!SIMPLEQ_EMPTY(&sc->pool)) {
		packet = SIMPLEQ_FIRST(&sc->pool);
		SIMPLEQ_REMOVE_HEAD(&sc->pool, list);
		m_freem(packet->mbuf);
		free(packet, M_TEMP);
	}

	while (!SIMPLEQ_EMPTY(&sc->fpool)) {
		packet = SIMPLEQ_FIRST(&sc->fpool);
		SIMPLEQ_REMOVE_HEAD(&sc->fpool, list);
		free(packet, M_TEMP);
	}

	return (0);
}

MOD_MISC("nettest");

int
nettest_lkmentry(struct lkm_table *lkmtp, int cmd, int ver)
{
	LKM_DISPATCH(lkmtp, cmd, ver, nettest_init, nettest_exit, lkm_nofunc);
}