o Add BERI Virtio Networking Frontend (if_vtbe)

o Move similar block/networking methods to common file
o Follow r275640 and correct MMIO registers width
o Pass value to MMIO platform_note method.

Sponsored by:	DARPA, AFRL

11 files changed, 816 insertions, 98 deletions

diff --git a/sys/arm/altera/socfpga/files.socfpga b/sys/arm/altera/socfpga/files.socfpga
index 85c8f50..5487976 100644
--- a/sys/arm/altera/socfpga/files.socfpga
+++ b/sys/arm/altera/socfpga/files.socfpga
@@ -26,5 +26,6 @@ dev/mmc/host/dwmmc.c				optional dwmmc
 # BERI specific
 dev/beri/beri_ring.c				optional beri_ring
 dev/beri/beri_mem.c				optional beri_mem
-dev/beri/virtio/virtio.c			optional beri_vtblk
+dev/beri/virtio/virtio.c			optional beri_vtblk | vtbe
 dev/beri/virtio/virtio_block.c			optional beri_vtblk
+dev/beri/virtio/network/if_vtbe.c		optional vtbe
diff --git a/sys/arm/conf/SOCKIT-BERI b/sys/arm/conf/SOCKIT-BERI
index fece41c..76306de 100644
--- a/sys/arm/conf/SOCKIT-BERI
+++ b/sys/arm/conf/SOCKIT-BERI
@@ -123,6 +123,7 @@ device		spibus
 device		beri_ring
 device		beri_mem
 device		beri_vtblk
+device		vtbe
 device		altera_pio
 
 # Ethernet
diff --git a/sys/boot/fdt/dts/arm/socfpga-sockit-beri.dts b/sys/boot/fdt/dts/arm/socfpga-sockit-beri.dts
index 5016738..0ed81fc 100644
--- a/sys/boot/fdt/dts/arm/socfpga-sockit-beri.dts
+++ b/sys/boot/fdt/dts/arm/socfpga-sockit-beri.dts
@@ -39,7 +39,8 @@
 	compatible = "altr,socfpga-cyclone5", "altr,socfpga";
 
 	memreserve = < 0x00000000 0x1000 >, /* SMP trampoline */
-		     < 0x00001000 0x1000 >; /* virtio block */
+		     < 0x00001000 0x1000 >, /* virtio block */
+		     < 0x00002000 0x1000 >; /* virtio net */
 
 	memory {
 		device_type = "memory";
@@ -111,6 +112,15 @@
 			pio-recv = <&pio0>;
 			pio-send = <&pio1>;
 			beri-mem = <&beri_mem0>;
+			status = "disabled";
+		};
+
+		beri_vtnet: vtnet@00002000 {
+			compatible = "sri-cambridge,beri-vtnet";
+			reg = <0x00002000 0x1000>;
+			pio-recv = <&pio0>;
+			pio-send = <&pio1>;
+			beri-mem = <&beri_mem0>;
 			status = "okay";
 		};
 
diff --git a/sys/dev/beri/virtio/network/if_vtbe.c b/sys/dev/beri/virtio/network/if_vtbe.c
new file mode 100644
index 0000000..a08a8a2
--- /dev/null
+++ b/sys/dev/beri/virtio/network/if_vtbe.c
@@ -0,0 +1,655 @@
+/*-
+ * Copyright (c) 2014 Ruslan Bukin <br@bsdpad.com>
+ * All rights reserved.
+ *
+ * This software was developed by SRI International and the University of
+ * Cambridge Computer Laboratory under DARPA/AFRL contract (FA8750-10-C-0237)
+ * ("CTSRD"), as part of the DARPA CRASH research programme.
+ *
+ * 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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.
+ */
+
+/*
+ * BERI Virtio Networking Frontend
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/bus.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/malloc.h>
+#include <sys/rman.h>
+#include <sys/timeet.h>
+#include <sys/timetc.h>
+#include <sys/endian.h>
+#include <sys/lock.h>
+#include <sys/mbuf.h>
+#include <sys/mutex.h>
+#include <sys/socket.h>
+#include <sys/sockio.h>
+#include <sys/sysctl.h>
+#include <sys/mdioctl.h>
+#include <sys/conf.h>
+#include <sys/stat.h>
+#include <sys/uio.h>
+
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
+#include <net/bpf.h>
+#include <net/if.h>
+#include <net/ethernet.h>
+#include <net/if_dl.h>
+#include <net/if_media.h>
+#include <net/if_types.h>
+#include <net/if_var.h>
+#include <net/if_vlan_var.h>
+
+#include <machine/bus.h>
+#include <machine/fdt.h>
+#include <machine/cpu.h>
+#include <machine/intr.h>
+
+#include <dev/beri/virtio/virtio.h>
+#include <dev/beri/virtio/virtio_mmio_platform.h>
+
+#include <dev/altera/pio/pio.h>
+
+#include <dev/virtio/mmio/virtio_mmio.h>
+#include <dev/virtio/network/virtio_net.h>
+#include <dev/virtio/virtio.h>
+#include <dev/virtio/virtio_ring.h>
+
+#include "pio_if.h"
+
+#define	DPRINTF(fmt, args...)	printf(fmt, ##args)
+
+#define	READ4(_sc, _reg) \
+	bus_read_4((_sc)->res[0], _reg)
+#define	WRITE4(_sc, _reg, _val) \
+	bus_write_4((_sc)->res[0], _reg, _val)
+
+#define	VTBE_LOCK(sc)			mtx_lock(&(sc)->mtx)
+#define	VTBE_UNLOCK(sc)			mtx_unlock(&(sc)->mtx)
+#define	VTBE_ASSERT_LOCKED(sc)		mtx_assert(&(sc)->mtx, MA_OWNED);
+#define	VTBE_ASSERT_UNLOCKED(sc)	mtx_assert(&(sc)->mtx, MA_NOTOWNED);
+
+/*
+ * Driver data and defines.
+ */
+#define	DESC_COUNT	256
+
+struct vtbe_softc {
+	struct resource		*res[2];
+	bus_space_tag_t		bst;
+	bus_space_handle_t	bsh;
+	device_t		dev;
+	struct ifnet		*ifp;
+	int			if_flags;
+	struct mtx		mtx;
+	boolean_t		is_attached;
+
+	int			beri_mem_offset;
+	device_t		pio_send;
+	device_t		pio_recv;
+	int			opened;
+
+	struct vqueue_info	vs_queues[2];
+	int			vs_curq;
+	int			hdrsize;
+};
+
+static struct resource_spec vtbe_spec[] = {
+	{ SYS_RES_MEMORY,	0,	RF_ACTIVE },
+	{ -1, 0 }
+};
+
+static void vtbe_txfinish_locked(struct vtbe_softc *sc);
+static void vtbe_rxfinish_locked(struct vtbe_softc *sc);
+static void vtbe_stop_locked(struct vtbe_softc *sc);
+static int pio_enable_irq(struct vtbe_softc *sc, int enable);
+
+static void
+vtbe_txstart_locked(struct vtbe_softc *sc)
+{
+	struct virtio_net_hdr_mrg_rxbuf *vnh;
+	struct iovec iov[DESC_COUNT];
+	struct vqueue_info *vq;
+	struct iovec *riov;
+	struct ifnet *ifp;
+	struct mbuf *m;
+	struct uio uio;
+	int enqueued;
+	int iolen;
+	int error;
+	int *addr;
+	int reg;
+	int len;
+	int n;
+
+	VTBE_ASSERT_LOCKED(sc);
+
+	/* RX queue */
+	vq = &sc->vs_queues[0];
+	if (!vq_has_descs(vq)) {
+		return;
+	}
+
+	ifp = sc->ifp;
+	if (ifp->if_drv_flags & IFF_DRV_OACTIVE) {
+		return;
+	}
+
+	enqueued = 0;
+
+	if (!vq_ring_ready(vq))
+		return;
+
+	vq->vq_save_used = be16toh(vq->vq_used->idx);
+
+	for (;;) {
+		if (!vq_has_descs(vq)) {
+			ifp->if_drv_flags |= IFF_DRV_OACTIVE;
+			break;
+		}
+
+		IFQ_DRV_DEQUEUE(&ifp->if_snd, m);
+		if (m == NULL) {
+			break;
+		}
+
+		n = vq_getchain(sc->beri_mem_offset, vq, iov,
+			DESC_COUNT, NULL);
+
+		KASSERT(n >= 1 && n <= DESC_COUNT,
+			("wrong descriptors num %d", n));
+
+		addr = iov[0].iov_base;
+		len = iov[0].iov_len;
+
+		vnh = iov[0].iov_base;
+		memset(vnh, 0, sc->hdrsize);
+		vnh->num_buffers = htobe16(1);
+
+		iov[0].iov_len -= sc->hdrsize;
+		iov[0].iov_base = (void *)((uintptr_t)iov[0].iov_base +
+					sc->hdrsize);
+		riov = &iov[0];
+
+		uio.uio_resid = iov[0].iov_len;
+		uio.uio_iov = riov;
+		uio.uio_segflg = UIO_SYSSPACE;
+		uio.uio_iovcnt = 1;
+		uio.uio_offset = 0;
+		uio.uio_rw = UIO_READ;
+
+		error = m_mbuftouio(&uio, m, 0);
+		if (error)
+			panic("m_mbuftouio failed\n");
+
+		iolen = (len - iov[0].iov_len - sc->hdrsize);
+		vq_relchain(vq, iov, 0, iolen + sc->hdrsize);
+		paddr_unmap((void *)addr, len);
+
+		if_inc_counter(ifp, IFCOUNTER_OPACKETS, 1);
+
+		BPF_MTAP(ifp, m);
+		m_freem(m);
+
+		++enqueued;
+	}
+
+	if (enqueued != 0) {
+		reg = htobe32(VIRTIO_MMIO_INT_VRING);
+		WRITE4(sc, VIRTIO_MMIO_INTERRUPT_STATUS, reg);
+
+		PIO_SET(sc->pio_send, Q_INTR, 1);
+	}
+}
+
+static void
+vtbe_txstart(struct ifnet *ifp)
+{
+	struct vtbe_softc *sc = ifp->if_softc;
+
+	VTBE_LOCK(sc);
+	vtbe_txstart_locked(sc);
+	VTBE_UNLOCK(sc);
+}
+
+static void
+vtbe_stop_locked(struct vtbe_softc *sc)
+{
+	struct ifnet *ifp;
+
+	VTBE_ASSERT_LOCKED(sc);
+
+	ifp = sc->ifp;
+	ifp->if_drv_flags &= ~(IFF_DRV_RUNNING | IFF_DRV_OACTIVE);
+}
+
+static void
+vtbe_init_locked(struct vtbe_softc *sc)
+{
+	struct ifnet *ifp = sc->ifp;
+
+	VTBE_ASSERT_LOCKED(sc);
+
+	if (ifp->if_drv_flags & IFF_DRV_RUNNING)
+		return;
+
+	ifp->if_drv_flags |= IFF_DRV_RUNNING;
+}
+
+static void
+vtbe_init(void *if_softc)
+{
+	struct vtbe_softc *sc = if_softc;
+
+	VTBE_LOCK(sc);
+	vtbe_init_locked(sc);
+	VTBE_UNLOCK(sc);
+}
+
+static int
+vtbe_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
+{
+	struct ifmediareq *ifmr;
+	struct vtbe_softc *sc;
+	struct ifreq *ifr;
+	int mask, error;
+
+	sc = ifp->if_softc;
+	ifr = (struct ifreq *)data;
+
+	error = 0;
+	switch (cmd) {
+	case SIOCSIFFLAGS:
+		VTBE_LOCK(sc);
+		if (ifp->if_flags & IFF_UP) {
+			pio_enable_irq(sc, 1);
+
+			if ((ifp->if_drv_flags & IFF_DRV_RUNNING) == 0) {
+				vtbe_init_locked(sc);
+			}
+		} else {
+			pio_enable_irq(sc, 0);
+
+			if (ifp->if_drv_flags & IFF_DRV_RUNNING) {
+				vtbe_stop_locked(sc);
+			}
+		}
+		sc->if_flags = ifp->if_flags;
+		VTBE_UNLOCK(sc);
+		break;
+	case SIOCADDMULTI:
+	case SIOCDELMULTI:
+		break;
+	case SIOCSIFMEDIA:
+	case SIOCGIFMEDIA:
+		ifmr = (struct ifmediareq *)data;
+		ifmr->ifm_count = 1;
+		ifmr->ifm_status = (IFM_AVALID | IFM_ACTIVE);
+		ifmr->ifm_active = (IFM_ETHER | IFM_10G_T | IFM_FDX);
+		ifmr->ifm_current = ifmr->ifm_active;
+		break;
+	case SIOCSIFCAP:
+		mask = ifp->if_capenable ^ ifr->ifr_reqcap;
+		if (mask & IFCAP_VLAN_MTU) {
+			ifp->if_capenable ^= IFCAP_VLAN_MTU;
+		}
+		break;
+
+	case SIOCSIFADDR:
+		pio_enable_irq(sc, 1);
+	default:
+		error = ether_ioctl(ifp, cmd, data);
+		break;
+	}
+
+	return (error);
+}
+
+static void
+vtbe_txfinish_locked(struct vtbe_softc *sc)
+{
+	struct ifnet *ifp;
+
+	VTBE_ASSERT_LOCKED(sc);
+
+	ifp = sc->ifp;
+}
+
+static int
+vq_init(struct vtbe_softc *sc)
+{
+	struct vqueue_info *vq;
+	uint8_t *base;
+	int size;
+	int reg;
+	int pfn;
+
+	vq = &sc->vs_queues[sc->vs_curq];
+	vq->vq_qsize = DESC_COUNT;
+
+	reg = READ4(sc, VIRTIO_MMIO_QUEUE_PFN);
+	pfn = be32toh(reg);
+	vq->vq_pfn = pfn;
+
+	size = vring_size(vq->vq_qsize, VRING_ALIGN);
+	base = paddr_map(sc->beri_mem_offset,
+		(pfn << PAGE_SHIFT), size);
+
+	/* First pages are descriptors */
+	vq->vq_desc = (struct vring_desc *)base;
+	base += vq->vq_qsize * sizeof(struct vring_desc);
+
+	/* Then avail ring */
+	vq->vq_avail = (struct vring_avail *)base;
+	base += (2 + vq->vq_qsize + 1) * sizeof(uint16_t);
+
+	/* Then it's rounded up to the next page */
+	base = (uint8_t *)roundup2((uintptr_t)base, VRING_ALIGN);
+
+	/* And the last pages are the used ring */
+	vq->vq_used = (struct vring_used *)base;
+
+	/* Mark queue as allocated, and start at 0 when we use it. */
+	vq->vq_flags = VQ_ALLOC;
+	vq->vq_last_avail = 0;
+
+	return (0);
+}
+
+static void
+vtbe_proc_rx(struct vtbe_softc *sc, struct vqueue_info *vq)
+{
+	struct iovec iov[DESC_COUNT];
+	struct ifnet *ifp;
+	struct uio uio;
+	struct mbuf *m;
+	int iolen;
+	int i;
+	int n;
+
+	ifp = sc->ifp;
+
+	n = vq_getchain(sc->beri_mem_offset, vq, iov,
+		DESC_COUNT, NULL);
+
+	KASSERT(n >= 1 && n <= DESC_COUNT,
+		("wrong n %d", n));
+
+	iolen = 0;
+	for (i = 1; i < n; i++) {
+		iolen += iov[i].iov_len;
+	}
+
+	uio.uio_resid = iolen;
+	uio.uio_iov = &iov[1];
+	uio.uio_segflg = UIO_SYSSPACE;
+	uio.uio_iovcnt = (n - 1);
+	uio.uio_rw = UIO_WRITE;
+
+	if ((m = m_uiotombuf(&uio, M_NOWAIT, 0, ETHER_ALIGN,
+	    M_PKTHDR)) == NULL) {
+		if_inc_counter(ifp, IFCOUNTER_IERRORS, 1);
+		goto done;
+	}
+
+	m->m_pkthdr.rcvif = ifp;
+
+	if_inc_counter(ifp, IFCOUNTER_IPACKETS, 1);
+
+	CURVNET_SET(ifp->if_vnet);
+	VTBE_UNLOCK(sc);
+	(*ifp->if_input)(ifp, m);
+	VTBE_LOCK(sc);
+	CURVNET_RESTORE();
+
+done:
+	vq_relchain(vq, iov, n, iolen + sc->hdrsize);
+}
+
+static void
+vtbe_rxfinish_locked(struct vtbe_softc *sc)
+{
+	struct vqueue_info *vq;
+	int reg;
+
+	/* TX queue */
+	vq = &sc->vs_queues[1];
+	if (!vq_ring_ready(vq))
+		return;
+
+	/* Process new descriptors */
+	vq->vq_save_used = be16toh(vq->vq_used->idx);
+
+	while (vq_has_descs(vq)) {
+		vtbe_proc_rx(sc, vq);
+	}
+
+	/* Interrupt the other side */
+	reg = htobe32(VIRTIO_MMIO_INT_VRING);
+	WRITE4(sc, VIRTIO_MMIO_INTERRUPT_STATUS, reg);
+
+	PIO_SET(sc->pio_send, Q_INTR, 1);
+}
+
+static void
+vtbe_intr(void *arg)
+{
+	struct vtbe_softc *sc;
+	int pending;
+	uint32_t reg;
+
+	sc = arg;
+
+	VTBE_LOCK(sc);
+
+	reg = PIO_READ(sc->pio_recv);
+
+	/* Ack */
+	PIO_SET(sc->pio_recv, reg, 0);
+
+	pending = htobe32(reg);
+	if (pending & Q_SEL) {
+		reg = READ4(sc, VIRTIO_MMIO_QUEUE_SEL);
+		sc->vs_curq = be32toh(reg);
+	}
+
+	if (pending & Q_PFN) {
+		vq_init(sc);
+	}
+
+	if (pending & Q_NOTIFY) {
+		/* beri rx / arm tx notify */
+		vtbe_txfinish_locked(sc);
+	}
+
+	if (pending & Q_NOTIFY1) {
+		vtbe_rxfinish_locked(sc);
+	}
+
+	VTBE_UNLOCK(sc);
+}
+
+static int
+vtbe_get_hwaddr(struct vtbe_softc *sc, uint8_t *hwaddr)
+{
+	int rnd;
+
+	/*
+	 * Generate MAC address, use 'bsd' + random 24 low-order bits.
+	 */
+
+	rnd = arc4random() & 0x00ffffff;
+
+	hwaddr[0] = 'b';
+	hwaddr[1] = 's';
+	hwaddr[2] = 'd';
+	hwaddr[3] = rnd >> 16;
+	hwaddr[4] = rnd >>  8;
+	hwaddr[5] = rnd >>  0;
+
+	return (0);
+}
+
+static int
+pio_enable_irq(struct vtbe_softc *sc, int enable)
+{
+
+	/*
+	 * IRQ lines should be disabled while reprogram FPGA core.
+	 */
+
+	if (enable) {
+		if (sc->opened == 0) {
+			sc->opened = 1;
+			PIO_SETUP_IRQ(sc->pio_recv, vtbe_intr, sc);
+		}
+	} else {
+		if (sc->opened == 1) {
+			PIO_TEARDOWN_IRQ(sc->pio_recv);
+			sc->opened = 0;
+		}
+	}
+
+	return (0);
+}
+
+static int
+vtbe_probe(device_t dev)
+{
+
+	if (!ofw_bus_status_okay(dev))
+		return (ENXIO);
+
+	if (!ofw_bus_is_compatible(dev, "sri-cambridge,beri-vtnet"))
+		return (ENXIO);
+
+	device_set_desc(dev, "Virtio BERI Ethernet Controller");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static int
+vtbe_attach(device_t dev)
+{
+	uint8_t macaddr[ETHER_ADDR_LEN];
+	struct vtbe_softc *sc;
+	struct ifnet *ifp;
+	int reg;
+
+	sc = device_get_softc(dev);
+	sc->dev = dev;
+
+	sc->hdrsize = sizeof(struct virtio_net_hdr_mrg_rxbuf);
+
+	if (bus_alloc_resources(dev, vtbe_spec, sc->res)) {
+		device_printf(dev, "could not allocate resources\n");
+		return (ENXIO);
+	}
+
+	/* Memory interface */
+	sc->bst = rman_get_bustag(sc->res[0]);
+	sc->bsh = rman_get_bushandle(sc->res[0]);
+
+	mtx_init(&sc->mtx, device_get_nameunit(sc->dev),
+	    MTX_NETWORK_LOCK, MTX_DEF);
+
+	if (setup_offset(dev, &sc->beri_mem_offset) != 0)
+		return (ENXIO);
+	if (setup_pio(dev, "pio-send", &sc->pio_send) != 0)
+		return (ENXIO);
+	if (setup_pio(dev, "pio-recv", &sc->pio_recv) != 0)
+		return (ENXIO);
+
+	/* Setup MMIO */
+
+	/* Specify that we provide network device */
+	reg = htobe32(VIRTIO_ID_NETWORK);
+	WRITE4(sc, VIRTIO_MMIO_DEVICE_ID, reg);
+
+	/* The number of desc we support */
+	reg = htobe32(DESC_COUNT);
+	WRITE4(sc, VIRTIO_MMIO_QUEUE_NUM_MAX, reg);
+
+	/* Our features */
+	reg = htobe32(VIRTIO_NET_F_MAC |
+			VIRTIO_NET_F_MRG_RXBUF |
+    			VIRTIO_F_NOTIFY_ON_EMPTY);
+	WRITE4(sc, VIRTIO_MMIO_HOST_FEATURES, reg);
+
+	/* Get MAC */
+	if (vtbe_get_hwaddr(sc, macaddr)) {
+		device_printf(sc->dev, "can't get mac\n");
+		return (ENXIO);
+	}
+
+	/* Set up the ethernet interface. */
+	sc->ifp = ifp = if_alloc(IFT_ETHER);
+	ifp->if_baudrate = IF_Gbps(10);
+	ifp->if_softc = sc;
+	if_initname(ifp, device_get_name(dev), device_get_unit(dev));
+	ifp->if_flags = (IFF_BROADCAST | IFF_SIMPLEX |
+			 IFF_MULTICAST | IFF_PROMISC);
+	ifp->if_capabilities = IFCAP_VLAN_MTU;
+	ifp->if_capenable = ifp->if_capabilities;
+	ifp->if_start = vtbe_txstart;
+	ifp->if_ioctl = vtbe_ioctl;
+	ifp->if_init = vtbe_init;
+	IFQ_SET_MAXLEN(&ifp->if_snd, DESC_COUNT - 1);
+	ifp->if_snd.ifq_drv_maxlen = DESC_COUNT - 1;
+	IFQ_SET_READY(&ifp->if_snd);
+	ifp->if_hdrlen = sizeof(struct ether_vlan_header);
+
+	/* All ready to run, attach the ethernet interface. */
+	ether_ifattach(ifp, macaddr);
+
+	sc->is_attached = true;
+
+	return (0);
+}
+
+static device_method_t vtbe_methods[] = {
+	DEVMETHOD(device_probe,		vtbe_probe),
+	DEVMETHOD(device_attach,	vtbe_attach),
+
+	{ 0, 0 }
+};
+
+static driver_t vtbe_driver = {
+	"vtbe",
+	vtbe_methods,
+	sizeof(struct vtbe_softc),
+};
+
+static devclass_t vtbe_devclass;
+
+DRIVER_MODULE(vtbe, simplebus, vtbe_driver, vtbe_devclass, 0, 0);
+MODULE_DEPEND(vtbe, ether, 1, 1, 1);
diff --git a/sys/dev/beri/virtio/virtio.c b/sys/dev/beri/virtio/virtio.c
index 5ad4ff9..4cc25c2 100644
--- a/sys/dev/beri/virtio/virtio.c
+++ b/sys/dev/beri/virtio/virtio.c
@@ -58,10 +58,18 @@ __FBSDID("$FreeBSD$");
 #include <machine/cpu.h>
 #include <machine/intr.h>
 
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/openfirm.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+
 #include <dev/beri/virtio/virtio.h>
 #include <dev/virtio/virtio.h>
 #include <dev/virtio/virtqueue.h>
 #include <dev/virtio/virtio_ring.h>
+#include <dev/altera/pio/pio.h>
+
+#include "pio_if.h"
 
 int
 vq_ring_ready(struct vqueue_info *vq)
@@ -167,12 +175,13 @@ vq_relchain(struct vqueue_info *vq, struct iovec *iov, int n, uint32_t iolen)
 	int i;
 
 	mask = vq->vq_qsize - 1;
+	vu = vq->vq_used;
 	head = be16toh(vq->vq_avail->ring[vq->vq_last_avail++ & mask]);
 
-	vu = vq->vq_used;
 	uidx = be16toh(vu->idx);
 	vue = &vu->ring[uidx++ & mask];
-	vue->id = htobe16(head);
+	vue->id = htobe32(head);
+
 	vue->len = htobe32(iolen);
 	vu->idx = htobe16(uidx);
 
@@ -181,3 +190,59 @@ vq_relchain(struct vqueue_info *vq, struct iovec *iov, int n, uint32_t iolen)
 		paddr_unmap((void *)iov[i].iov_base, iov[i].iov_len);
 	}
 }
+
+int
+setup_pio(device_t dev, char *name, device_t *pio_dev)
+{
+	phandle_t pio_node;
+	struct fdt_ic *ic;
+	phandle_t xref;
+	phandle_t node;
+
+	if ((node = ofw_bus_get_node(dev)) == -1)
+		return (ENXIO);
+
+	if (OF_searchencprop(node, name, &xref,
+		sizeof(xref)) == -1) {
+		return (ENXIO);
+	}
+
+	pio_node = OF_node_from_xref(xref);
+	SLIST_FOREACH(ic, &fdt_ic_list_head, fdt_ics) {
+		if (ic->iph == pio_node) {
+			*pio_dev = ic->dev;
+			PIO_CONFIGURE(*pio_dev, PIO_OUT_ALL,
+					PIO_UNMASK_ALL);
+			return (0);
+		}
+	}
+
+	return (ENXIO);
+}
+
+int
+setup_offset(device_t dev, uint32_t *offset)
+{
+	pcell_t dts_value[2];
+	phandle_t mem_node;
+	phandle_t xref;
+	phandle_t node;
+	int len;
+
+	if ((node = ofw_bus_get_node(dev)) == -1)
+		return (ENXIO);
+
+	if (OF_searchencprop(node, "beri-mem", &xref,
+		sizeof(xref)) == -1) {
+		return (ENXIO);
+	}
+
+	mem_node = OF_node_from_xref(xref);
+	if ((len = OF_getproplen(mem_node, "reg")) <= 0)
+		return (ENXIO);
+	OF_getencprop(mem_node, "reg", dts_value, len);
+	*offset = dts_value[0];
+
+	return (0);
+}
+
diff --git a/sys/dev/beri/virtio/virtio.h b/sys/dev/beri/virtio/virtio.h
index a3461ad..51642d4 100644
--- a/sys/dev/beri/virtio/virtio.h
+++ b/sys/dev/beri/virtio/virtio.h
@@ -41,14 +41,10 @@
 
 #define	PAGE_SHIFT		12
 #define	VRING_ALIGN		4096
-#define	NUM_QUEUES		1
 
 #define	VQ_ALLOC		0x01	/* set once we have a pfn */
 #define	VQ_MAX_DESCRIPTORS	512
 
-#define	VTBLK_BLK_ID_BYTES	20
-#define	VTBLK_MAXSEGS		256
-
 struct vqueue_info {
 	uint16_t vq_qsize;	/* size of this queue (a power of 2) */
 	uint16_t vq_num;
@@ -70,3 +66,5 @@ int vq_getchain(uint32_t beri_mem_offset, struct vqueue_info *vq,
 		struct iovec *iov, int n_iov, uint16_t *flags);
 void vq_relchain(struct vqueue_info *vq, struct iovec *iov, int n, uint32_t iolen);
 
+int setup_pio(device_t dev, char *name, device_t *pio_dev);
+int setup_offset(device_t dev, uint32_t *offset);
diff --git a/sys/dev/beri/virtio/virtio_block.c b/sys/dev/beri/virtio/virtio_block.c
index d078854..12b6e0e 100644
--- a/sys/dev/beri/virtio/virtio_block.c
+++ b/sys/dev/beri/virtio/virtio_block.c
@@ -74,6 +74,13 @@ __FBSDID("$FreeBSD$");
 
 #define DPRINTF(fmt, ...)
 
+/* We use indirect descriptors */
+#define	NUM_DESCS	1
+#define	NUM_QUEUES	1
+
+#define	VTBLK_BLK_ID_BYTES	20
+#define	VTBLK_MAXSEGS		256
+
 struct beri_vtblk_softc {
 	struct resource		*res[1];
 	bus_space_tag_t		bst;
@@ -286,8 +293,12 @@ vtblk_notify(struct beri_vtblk_softc *sc)
 	while (vq_has_descs(vq))
 		vtblk_proc(sc, vq);
 
-	/* Interrupt other side */
-	PIO_SET(sc->pio_send, Q_INTR, 1);
+	/* Interrupt the other side */
+	if ((be16toh(vq->vq_avail->flags) & VRING_AVAIL_F_NO_INTERRUPT) == 0) {
+		reg = htobe32(VIRTIO_MMIO_INT_VRING);
+		WRITE4(sc, VIRTIO_MMIO_INTERRUPT_STATUS, reg);
+		PIO_SET(sc->pio_send, Q_INTR, 1);
+	}
 
 	return (0);
 }
@@ -302,7 +313,7 @@ vq_init(struct beri_vtblk_softc *sc)
 	int pfn;
 
 	vq = &sc->vs_queues[0];
-	vq->vq_qsize = NUM_QUEUES;
+	vq->vq_qsize = NUM_DESCS;
 
 	reg = READ4(sc, VIRTIO_MMIO_QUEUE_PFN);
 	pfn = be32toh(reg);
@@ -353,61 +364,6 @@ vtblk_thread(void *arg)
 }
 
 static int
-setup_pio(struct beri_vtblk_softc *sc, char *name, device_t *dev)
-{
-	phandle_t pio_node;
-	struct fdt_ic *ic;
-	phandle_t xref;
-	phandle_t node;
-
-	if ((node = ofw_bus_get_node(sc->dev)) == -1)
-		return (ENXIO);
-
-	if (OF_searchencprop(node, name, &xref,
-		sizeof(xref)) == -1) {
-		return (ENXIO);
-	}
-
-	pio_node = OF_node_from_xref(xref);
-	SLIST_FOREACH(ic, &fdt_ic_list_head, fdt_ics) {
-		if (ic->iph == pio_node) {
-			*dev = ic->dev;
-			PIO_CONFIGURE(*dev, PIO_OUT_ALL,
-					PIO_UNMASK_ALL);
-			return (0);
-		}
-	}
-
-	return (ENXIO);
-}
-
-static int
-setup_offset(struct beri_vtblk_softc *sc)
-{
-	pcell_t dts_value[2];
-	phandle_t mem_node;
-	phandle_t xref;
-	phandle_t node;
-	int len;
-
-	if ((node = ofw_bus_get_node(sc->dev)) == -1)
-		return (ENXIO);
-
-	if (OF_searchencprop(node, "beri-mem", &xref,
-		sizeof(xref)) == -1) {
-		return (ENXIO);
-	}
-
-	mem_node = OF_node_from_xref(xref);
-	if ((len = OF_getproplen(mem_node, "reg")) <= 0)
-		return (ENXIO);
-	OF_getencprop(mem_node, "reg", dts_value, len);
-	sc->beri_mem_offset = dts_value[0];
-
-	return (0);
-}
-
-static int
 backend_info(struct beri_vtblk_softc *sc)
 {
 	struct virtio_blk_config *cfg;
@@ -419,9 +375,9 @@ backend_info(struct beri_vtblk_softc *sc)
 	reg = htobe32(VIRTIO_ID_BLOCK);
 	WRITE4(sc, VIRTIO_MMIO_DEVICE_ID, reg);
 
-	/* The number of queues we support */
-	reg = htobe16(NUM_QUEUES);
-	WRITE2(sc, VIRTIO_MMIO_QUEUE_NUM, reg);
+	/* Queue size */
+	reg = htobe32(NUM_DESCS);
+	WRITE4(sc, VIRTIO_MMIO_QUEUE_NUM_MAX, reg);
 
 	/* Our features */
 	reg = htobe32(VIRTIO_RING_F_INDIRECT_DESC
@@ -566,11 +522,11 @@ beri_vtblk_attach(device_t dev)
 		return (ENXIO);
 	}
 
-	if (setup_offset(sc) != 0)
+	if (setup_offset(dev, &sc->beri_mem_offset) != 0)
 		return (ENXIO);
-	if (setup_pio(sc, "pio-send", &sc->pio_send) != 0)
+	if (setup_pio(dev, "pio-send", &sc->pio_send) != 0)
 		return (ENXIO);
-	if (setup_pio(sc, "pio-recv", &sc->pio_recv) != 0)
+	if (setup_pio(dev, "pio-recv", &sc->pio_recv) != 0)
 		return (ENXIO);
 
 	sc->cdev = make_dev(&beri_cdevsw, 0, UID_ROOT, GID_WHEEL,
diff --git a/sys/dev/beri/virtio/virtio_mmio_platform.c b/sys/dev/beri/virtio/virtio_mmio_platform.c
index 41c50f8..0c58747d 100644
--- a/sys/dev/beri/virtio/virtio_mmio_platform.c
+++ b/sys/dev/beri/virtio/virtio_mmio_platform.c
@@ -149,20 +149,49 @@ virtio_mmio_platform_attach(device_t dev)
 }
 
 static int
-platform_note(device_t dev, size_t offset)
+platform_note(device_t dev, size_t offset, int val)
 {
 	struct virtio_mmio_platform_softc *sc;
+	int note;
+	int i;
 
 	sc = device_get_softc(dev);
 
-	if (offset == VIRTIO_MMIO_QUEUE_NOTIFY) {
-		mips_dcache_wbinv_all();
-		PIO_SET(sc->pio_send, Q_NOTIFY, 1);
+	switch (offset) {
+		case (VIRTIO_MMIO_QUEUE_NOTIFY):
+			if (val == 0)
+				note = Q_NOTIFY;
+			else if (val == 1)
+				note = Q_NOTIFY1;
+			break;
+		case (VIRTIO_MMIO_QUEUE_PFN):
+			note = Q_PFN;
+			break;
+		case (VIRTIO_MMIO_QUEUE_SEL):
+			note = Q_SEL;
+			break;
+		default:
+			note = 0;
 	}
 
-	if (offset == VIRTIO_MMIO_QUEUE_PFN) {
+	if (note) {
 		mips_dcache_wbinv_all();
-		PIO_SET(sc->pio_send, Q_PFN, 1);
+
+		PIO_SET(sc->pio_send, note, 1);
+
+		/* 
+		 * Wait until host ack the request.
+		 * Usually done within few cycles.
+		 * TODO: bad
+		 */
+
+		for (i = 100; i > 0; i--) {
+			if (PIO_READ(sc->pio_send) == 0)
+				break;
+		}
+
+		if (i == 0)
+			device_printf(sc->dev, "Warning: host busy\n");
 	}
 
 	return (0);
diff --git a/sys/dev/beri/virtio/virtio_mmio_platform.h b/sys/dev/beri/virtio/virtio_mmio_platform.h
index d996449e..c420d4f 100644
--- a/sys/dev/beri/virtio/virtio_mmio_platform.h
+++ b/sys/dev/beri/virtio/virtio_mmio_platform.h
@@ -30,6 +30,8 @@
  * $FreeBSD$
  */
 
-#define	Q_NOTIFY	0x1
-#define	Q_PFN		0x2
-#define	Q_INTR		0x4
+#define	Q_NOTIFY	0x01
+#define	Q_PFN		0x02
+#define	Q_INTR		0x04
+#define	Q_SEL		0x08
+#define	Q_NOTIFY1	0x10
diff --git a/sys/dev/virtio/mmio/virtio_mmio.c b/sys/dev/virtio/mmio/virtio_mmio.c
index f7902c7..5c6a0e1 100644
--- a/sys/dev/virtio/mmio/virtio_mmio.c
+++ b/sys/dev/virtio/mmio/virtio_mmio.c
@@ -135,23 +135,23 @@ static void	vtmmio_vq_intr(void *);
 /*
  * I/O port read/write wrappers.
  */
-#define vtmmio_write_config_1(sc, o, v)			\
-do {							\
-	bus_write_1((sc)->res[0], (o), (v)); 		\
-	if (sc->platform != NULL)			\
-		VIRTIO_MMIO_NOTE(sc->platform, (o));	\
+#define vtmmio_write_config_1(sc, o, v)				\
+do {								\
+	bus_write_1((sc)->res[0], (o), (v)); 			\
+	if (sc->platform != NULL)				\
+		VIRTIO_MMIO_NOTE(sc->platform, (o), (v));	\
 } while (0)
-#define vtmmio_write_config_2(sc, o, v)			\
-do {							\
-	bus_write_2((sc)->res[0], (o), (v));		\
-	if (sc->platform != NULL)			\
-		VIRTIO_MMIO_NOTE(sc->platform, (o));	\
+#define vtmmio_write_config_2(sc, o, v)				\
+do {								\
+	bus_write_2((sc)->res[0], (o), (v));			\
+	if (sc->platform != NULL)				\
+		VIRTIO_MMIO_NOTE(sc->platform, (o), (v));	\
 } while (0)
-#define vtmmio_write_config_4(sc, o, v)			\
-do {							\
-	bus_write_4((sc)->res[0], (o), (v));		\
-	if (sc->platform != NULL)			\
-		VIRTIO_MMIO_NOTE(sc->platform, (o));	\
+#define vtmmio_write_config_4(sc, o, v)				\
+do {								\
+	bus_write_4((sc)->res[0], (o), (v));			\
+	if (sc->platform != NULL)				\
+		VIRTIO_MMIO_NOTE(sc->platform, (o), (v));	\
 } while (0)
 
 #define vtmmio_read_config_1(sc, o) \
diff --git a/sys/dev/virtio/mmio/virtio_mmio_if.m b/sys/dev/virtio/mmio/virtio_mmio_if.m
index 51511fa..8514c30 100644
--- a/sys/dev/virtio/mmio/virtio_mmio_if.m
+++ b/sys/dev/virtio/mmio/virtio_mmio_if.m
@@ -42,7 +42,7 @@ INTERFACE virtio_mmio;
 
 CODE {
 	static int
-	virtio_mmio_write(device_t dev, size_t offset)
+	virtio_mmio_note(device_t dev, size_t offset, int val)
 	{
 
 		return (1);
@@ -63,7 +63,8 @@ CODE {
 METHOD int note {
 	device_t	dev;
 	size_t		offset;
-} DEFAULT virtio_mmio_write;
+	int		val;
+} DEFAULT virtio_mmio_note;
 
 #
 # Setup backend-specific interrupts.