1 files changed, 643 insertions, 527 deletions

diff --git a/rtemsbsd/sys/dev/atsam/if_atsam.c b/rtemsbsd/sys/dev/atsam/if_atsam.c
index ff8219f4..21a28fcd 100644
--- a/rtemsbsd/sys/dev/atsam/if_atsam.c
+++ b/rtemsbsd/sys/dev/atsam/if_atsam.c
@@ -42,6 +42,7 @@
 #include <sys/types.h>
 #include <sys/param.h>
 #include <sys/mbuf.h>
+#include <sys/sbuf.h>
 #include <sys/socket.h>
 #include <sys/sockio.h>
 #include <sys/kernel.h>
@@ -49,7 +50,9 @@
 #include <sys/bus.h>
 #include <sys/sysctl.h>
 
+#include <net/bpf.h>
 #include <net/if.h>
+#include <net/if_dl.h>
 #include <net/if_var.h>
 #include <net/if_types.h>
 #include <net/if_media.h>
@@ -66,6 +69,7 @@
 
 #include <rtems/bsd/local/miibus_if.h>
 #include <rtems/bsd/if_atsam.h>
+#include <rtems/bsd/bsd.h>
 
 /*
  * Number of interfaces supported by the driver
@@ -90,46 +94,40 @@
 /** The runtime pin configure list for GMAC */
 #define BOARD_GMAC_RUN_PINS			BOARD_GMAC_PINS
 
-/** Multicast Enable */
-#define GMAC_MC_ENABLE				(1u << 6)
-#define HASH_INDEX_AMOUNT			6
-#define HASH_ELEMENTS_PER_INDEX			8
-#define MAC_ADDR_MASK				0x0000FFFFFFFFFFFF
-#define MAC_IDX_MASK				(1u << 0)
-
-/** Promiscuous Mode Enable */
-#define GMAC_PROM_ENABLE			(1u << 4)
-
 /** RX Defines */
 #define GMAC_RX_BUFFER_SIZE			1536
 #define GMAC_RX_BUF_DESC_ADDR_MASK		0xFFFFFFFC
-#define GMAC_RX_SET_OFFSET			(1u << 15)
-#define GMAC_RX_SET_USED_WRAP			((1u << 1) | (1u << 0))
-#define GMAC_RX_SET_WRAP			(1u << 1)
-#define GMAC_RX_SET_USED			(1u << 0)
-/** TX Defines */
-#define GMAC_TX_SET_EOF				(1u << 15)
-#define GMAC_TX_SET_WRAP			(1u << 30)
-#define GMAC_TX_SET_USED			(1u << 31)
 
 #define GMAC_DESCRIPTOR_ALIGNMENT		8
 
 /** Events */
 #define ATSAMV7_ETH_RX_EVENT_INTERRUPT		RTEMS_EVENT_1
-#define ATSAMV7_ETH_TX_EVENT_INTERRUPT		RTEMS_EVENT_2
-#define ATSAMV7_ETH_START_TRANSMIT_EVENT	RTEMS_EVENT_3
-
-#define ATSAMV7_ETH_RX_DATA_OFFSET		2
-
-#define WATCHDOG_TIMEOUT			5
 
 /* FIXME: Make these configurable */
 #define MDIO_RETRIES 10
 #define MDIO_PHY MII_PHY_ANY
-#define RXBUF_COUNT 8
-#define TXBUF_COUNT 64
 #define IGNORE_RX_ERR false
 
+#define RX_INTERRUPTS (GMAC_ISR_RCOMP | GMAC_ISR_RXUBR | GMAC_ISR_ROVR)
+
+#define RX_DESC_LOG2 3
+#define RX_DESC_COUNT (1U << RX_DESC_LOG2)
+#define RX_DESC_WRAP(idx) \
+  ((((idx) + 1) & RX_DESC_COUNT) >> (RX_DESC_LOG2 - 1))
+RTEMS_STATIC_ASSERT(RX_DESC_WRAP(RX_DESC_COUNT - 1) ==
+    GMAC_RX_WRAP_BIT, rx_desc_wrap);
+RTEMS_STATIC_ASSERT(RX_DESC_WRAP(RX_DESC_COUNT - 2) ==
+    0, rx_desc_no_wrap);
+
+#define TX_DESC_LOG2 6
+#define TX_DESC_COUNT (1U << TX_DESC_LOG2)
+#define TX_DESC_WRAP(idx) \
+  ((((idx) + 1) & TX_DESC_COUNT) << (30 - TX_DESC_LOG2))
+RTEMS_STATIC_ASSERT(TX_DESC_WRAP(TX_DESC_COUNT - 1) ==
+    GMAC_TX_WRAP_BIT, tx_desc_wrap);
+RTEMS_STATIC_ASSERT(TX_DESC_WRAP(TX_DESC_COUNT - 2) ==
+    0, tx_desc_no_wrap);
+
 /** The PINs for GMAC */
 static const Pin gmacPins[] = { BOARD_GMAC_RUN_PINS };
 
@@ -139,11 +137,13 @@ typedef struct if_atsam_gmac {
 	uint32_t retries;
 } if_atsam_gmac;
 
-typedef struct ring_buffer {
-	unsigned tx_bd_used;
-	unsigned tx_bd_free;
-	size_t length;
-} ring_buffer;
+struct if_atsam_tx_bds {
+	volatile sGmacTxDescriptor bds[TX_DESC_COUNT];
+};
+
+struct if_atsam_rx_bds {
+	volatile sGmacRxDescriptor bds[RX_DESC_COUNT];
+};
 
 /*
  * Per-device data
@@ -156,17 +156,16 @@ typedef struct if_atsam_softc {
 	struct ifnet *ifp;
 	struct mtx mtx;
 	if_atsam_gmac Gmac_inst;
+	size_t tx_idx_head;
+	size_t tx_idx_tail;
+	struct if_atsam_tx_bds *tx;
+	struct mbuf *tx_mbufs[TX_DESC_COUNT];
+	size_t rx_idx_head;
+	struct if_atsam_rx_bds *rx;
+	struct mbuf *rx_mbufs[RX_DESC_COUNT];
 	uint8_t GMacAddress[6];
 	rtems_id rx_daemon_tid;
-	rtems_id tx_daemon_tid;
 	rtems_vector_number interrupt_number;
-	struct mbuf **rx_mbuf;
-	struct mbuf **tx_mbuf;
-	volatile sGmacTxDescriptor *tx_bd_base;
-	size_t rx_bd_fill_idx;
-	size_t amount_rx_buf;
-	size_t amount_tx_buf;
-	ring_buffer tx_ring;
 	struct callout tick_ch;
 
 	/*
@@ -190,13 +189,12 @@ typedef struct if_atsam_softc {
 	struct if_atsam_stats {
 		/* Software */
 		uint32_t rx_overrun_errors;
+		uint32_t rx_used_bit_reads;
 		uint32_t rx_interrupts;
-		uint32_t tx_complete_int;
 		uint32_t tx_tur_errors;
 		uint32_t tx_rlex_errors;
 		uint32_t tx_tfc_errors;
 		uint32_t tx_hresp_errors;
-		uint32_t tx_interrupts;
 
 		/* Hardware */
 		uint64_t octets_transm;
@@ -243,6 +241,8 @@ typedef struct if_atsam_softc {
 		uint32_t tcp_checksum_errors;
 		uint32_t udp_checksum_errors;
 	} stats;
+
+	int if_flags;
 } if_atsam_softc;
 
 static void if_atsam_poll_hw_stats(struct if_atsam_softc *sc);
@@ -251,27 +251,6 @@ static void if_atsam_poll_hw_stats(struct if_atsam_softc *sc);
 
 #define IF_ATSAM_UNLOCK(sc) mtx_unlock(&(sc)->mtx)
 
-static void if_atsam_event_send(rtems_id task, rtems_event_set event)
-{
-	rtems_event_send(task, event);
-}
-
-
-static void if_atsam_event_receive(if_atsam_softc *sc, rtems_event_set in)
-{
-	rtems_event_set out;
-
-	IF_ATSAM_UNLOCK(sc);
-	rtems_event_receive(
-	    in,
-	    RTEMS_EVENT_ANY | RTEMS_WAIT,
-	    RTEMS_NO_TIMEOUT,
-	    &out
-	);
-	IF_ATSAM_LOCK(sc);
-}
-
-
 static struct mbuf *if_atsam_new_mbuf(struct ifnet *ifp)
 {
 	struct mbuf *m;
@@ -356,11 +335,10 @@ if_atsam_miibus_readreg(device_t dev, int phy, int reg)
 static int
 if_atsam_miibus_writereg(device_t dev, int phy, int reg, int data)
 {
-	uint8_t err;
 	if_atsam_softc *sc = device_get_softc(dev);
 
 	IF_ATSAM_LOCK(sc);
-	err = if_atsam_write_phy(sc->Gmac_inst.gGmacd.pHw,
+	(void)if_atsam_write_phy(sc->Gmac_inst.gGmacd.pHw,
 	    (uint8_t)phy, (uint8_t)reg, data, sc->Gmac_inst.retries);
 	IF_ATSAM_UNLOCK(sc);
 
@@ -403,63 +381,53 @@ if_atsam_init_phy(if_atsam_gmac *gmac_inst, uint32_t mck,
 static void if_atsam_interrupt_handler(void *arg)
 {
 	if_atsam_softc *sc = (if_atsam_softc *)arg;
-	uint32_t irq_status_val;
-	rtems_event_set rx_event = 0;
-	rtems_event_set tx_event = 0;
 	Gmac *pHw = sc->Gmac_inst.gGmacd.pHw;
+	uint32_t is;
 
 	/* Get interrupt status */
-	irq_status_val = GMAC_GetItStatus(pHw, 0);
+	is = pHw->GMAC_ISR;
 
-	/* Check receive interrupts */
-	if ((irq_status_val & GMAC_IER_ROVR) != 0) {
-		++sc->stats.rx_overrun_errors;
-		rx_event = ATSAMV7_ETH_RX_EVENT_INTERRUPT;
-	}
-	if ((irq_status_val & GMAC_IER_RCOMP) != 0) {
-		rx_event = ATSAMV7_ETH_RX_EVENT_INTERRUPT;
+	if (__predict_false((is & GMAC_TX_ERR_BIT) != 0)) {
+		if ((is & GMAC_IER_TUR) != 0) {
+			++sc->stats.tx_tur_errors;
+		}
+		if ((is & GMAC_IER_RLEX) != 0) {
+			++sc->stats.tx_rlex_errors;
+		}
+		if ((is & GMAC_IER_TFC) != 0) {
+			++sc->stats.tx_tfc_errors;
+		}
+		if ((is & GMAC_IER_HRESP) != 0) {
+			++sc->stats.tx_hresp_errors;
+		}
 	}
-	/* Send events to receive task and switch off rx interrupts */
-	if (rx_event != 0) {
+
+	/* Check receive interrupts */
+	if (__predict_true((is & RX_INTERRUPTS) != 0)) {
+		if (__predict_false((is & GMAC_ISR_RXUBR) != 0)) {
+			++sc->stats.rx_used_bit_reads;
+		}
+
+		if (__predict_false((is & GMAC_ISR_ROVR) != 0)) {
+			++sc->stats.rx_overrun_errors;
+		}
+
 		++sc->stats.rx_interrupts;
-		/* Erase the interrupts for RX completion and errors */
-		GMAC_DisableIt(pHw, GMAC_IER_RCOMP | GMAC_IER_ROVR, 0);
-		(void)if_atsam_event_send(sc->rx_daemon_tid, rx_event);
-	}
-	if ((irq_status_val & GMAC_IER_TUR) != 0) {
-		++sc->stats.tx_tur_errors;
-		tx_event = ATSAMV7_ETH_TX_EVENT_INTERRUPT;
-	}
-	if ((irq_status_val & GMAC_IER_RLEX) != 0) {
-		++sc->stats.tx_rlex_errors;
-		tx_event = ATSAMV7_ETH_TX_EVENT_INTERRUPT;
-	}
-	if ((irq_status_val & GMAC_IER_TFC) != 0) {
-		++sc->stats.tx_tfc_errors;
-		tx_event = ATSAMV7_ETH_TX_EVENT_INTERRUPT;
-	}
-	if ((irq_status_val & GMAC_IER_HRESP) != 0) {
-		++sc->stats.tx_hresp_errors;
-		tx_event = ATSAMV7_ETH_TX_EVENT_INTERRUPT;
-	}
-	if ((irq_status_val & GMAC_IER_TCOMP) != 0) {
-		++sc->stats.tx_complete_int;
-		tx_event = ATSAMV7_ETH_TX_EVENT_INTERRUPT;
-	}
-	/* Send events to transmit task and switch off tx interrupts */
-	if (tx_event != 0) {
-		++sc->stats.tx_interrupts;
-		/* Erase the interrupts for TX completion and errors */
-		GMAC_DisableIt(pHw, GMAC_INT_TX_BITS, 0);
-		(void)if_atsam_event_send(sc->tx_daemon_tid, tx_event);
+
+		/* Disable RX interrupts */
+		pHw->GMAC_IDR = RX_INTERRUPTS;
+
+		(void)rtems_event_send(sc->rx_daemon_tid,
+		    ATSAMV7_ETH_RX_EVENT_INTERRUPT);
 	}
 }
 
-static void rx_update_mbuf(struct mbuf *m, sGmacRxDescriptor *buffer_desc)
+static void
+if_atsam_rx_update_mbuf(struct mbuf *m, uint32_t status)
 {
 	int frame_len;
 
-	frame_len = (int) (buffer_desc->status.bm.len);
+	frame_len = (int)(status & GMAC_LENGTH_FRAME);
 
 	m->m_data = mtod(m, char*)+ETHER_ALIGN;
 	m->m_len = frame_len;
@@ -467,7 +435,7 @@ static void rx_update_mbuf(struct mbuf *m, sGmacRxDescriptor *buffer_desc)
 
 	/* check checksum offload result */
 	m->m_pkthdr.csum_flags = 0;
-	switch (buffer_desc->status.bm.typeIDMatchOrCksumResult) {
+	switch ((status >> 22) & 0x3) {
 	case GMAC_RXDESC_ST_CKSUM_RESULT_IP_CHECKED:
 		m->m_pkthdr.csum_flags = CSUM_IP_CHECKED | CSUM_IP_VALID;
 		m->m_pkthdr.csum_data = 0xffff;
@@ -481,21 +449,17 @@ static void rx_update_mbuf(struct mbuf *m, sGmacRxDescriptor *buffer_desc)
 	}
 }
 
-/*
- * Receive daemon
- */
-static void if_atsam_rx_daemon(void *arg)
+static void
+if_atsam_rx_daemon(rtems_task_argument arg)
 {
 	if_atsam_softc *sc = (if_atsam_softc *)arg;
 	struct ifnet *ifp = sc->ifp;
-	rtems_event_set events = 0;
-	void *rx_bd_base;
-	struct mbuf *m;
-	struct mbuf *n;
-	volatile sGmacRxDescriptor *buffer_desc;
-	uint32_t tmp_rx_bd_address;
-	size_t i;
+	volatile sGmacRxDescriptor *base;
+	struct if_atsam_rx_bds *rx;
 	Gmac *pHw = sc->Gmac_inst.gGmacd.pHw;
+	size_t idx;
+	struct mbuf **mbufs;
+	struct mbuf *m;
 
 	IF_ATSAM_LOCK(sc);
 
@@ -506,47 +470,37 @@ static void if_atsam_rx_daemon(void *arg)
 	}
 
 	/* Allocate memory space for priority queue descriptor list */
-	rx_bd_base = rtems_cache_coherent_allocate(sizeof(sGmacRxDescriptor),
+	base = rtems_cache_coherent_allocate(sizeof(*base),
 		GMAC_DESCRIPTOR_ALIGNMENT, 0);
-	assert(rx_bd_base != NULL);
+	assert(base != NULL);
 
-	buffer_desc = (sGmacRxDescriptor *)rx_bd_base;
-	buffer_desc->addr.val = GMAC_RX_SET_USED_WRAP;
-	buffer_desc->status.val = 0;
+	base->addr.val = GMAC_RX_OWNERSHIP_BIT | GMAC_RX_WRAP_BIT;
+	base->status.val = 0;
 
-	GMAC_SetRxQueue(pHw, (uint32_t)buffer_desc, 1);
-	GMAC_SetRxQueue(pHw, (uint32_t)buffer_desc, 2);
+	GMAC_SetRxQueue(pHw, (uint32_t)base, 1);
+	GMAC_SetRxQueue(pHw, (uint32_t)base, 2);
 
 	/* Allocate memory space for buffer descriptor list */
-	rx_bd_base = rtems_cache_coherent_allocate(
-		sc->amount_rx_buf * sizeof(sGmacRxDescriptor),
+	rx = rtems_cache_coherent_allocate(sizeof(*rx),
 		GMAC_DESCRIPTOR_ALIGNMENT, 0);
-	assert(rx_bd_base != NULL);
-	buffer_desc = (sGmacRxDescriptor *)rx_bd_base;
+	assert(rx != NULL);
+	sc->rx = rx;
+	mbufs = &sc->rx_mbufs[0];
 
 	/* Create descriptor list and mark as empty */
-	for (sc->rx_bd_fill_idx = 0; sc->rx_bd_fill_idx < sc->amount_rx_buf;
-	    ++sc->rx_bd_fill_idx) {
+	for (idx = 0; idx < RX_DESC_COUNT; ++idx) {
 		m = if_atsam_new_mbuf(ifp);
 		assert(m != NULL);
-		sc->rx_mbuf[sc->rx_bd_fill_idx] = m;
-		buffer_desc->addr.val = ((uint32_t)m->m_data) &
-		    GMAC_RX_BUF_DESC_ADDR_MASK;
-		buffer_desc->status.val = 0;
-		if (sc->rx_bd_fill_idx == (sc->amount_rx_buf - 1)) {
-			buffer_desc->addr.bm.bWrap = 1;
-		} else {
-			buffer_desc++;
-		}
+		mbufs[idx] = m;
+		rx->bds[idx].addr.val = mtod(m, uint32_t) | RX_DESC_WRAP(idx);
 	}
-	buffer_desc = (sGmacRxDescriptor *)rx_bd_base;
 
 	/* Set 2 Byte Receive Buffer Offset */
-	pHw->GMAC_NCFGR |= GMAC_RX_SET_OFFSET;
+	pHw->GMAC_NCFGR |= GMAC_NCFGR_RXBUFO(2);
 
 	/* Write Buffer Queue Base Address Register */
 	GMAC_ReceiveEnable(pHw, 0);
-	GMAC_SetRxQueue(pHw, (uint32_t)buffer_desc, 0);
+	GMAC_SetRxQueue(pHw, (uint32_t)&rx->bds[0], 0);
 
 	/* Set address for address matching */
 	GMAC_SetAddress(pHw, 0, sc->GMacAddress);
@@ -554,306 +508,230 @@ static void if_atsam_rx_daemon(void *arg)
 	/* Enable Receiving of data */
 	GMAC_ReceiveEnable(pHw, 1);
 
-	/* Setup the interrupts for RX completion and errors */
-	GMAC_EnableIt(pHw, GMAC_IER_RCOMP | GMAC_IER_ROVR, 0);
+	IF_ATSAM_UNLOCK(sc);
 
-	sc->rx_bd_fill_idx = 0;
+	idx = 0;
 
 	while (true) {
-		/* Wait for events */
-		if_atsam_event_receive(sc, ATSAMV7_ETH_RX_EVENT_INTERRUPT);
+		rtems_event_set out;
 
-		/*
-		 * Check for all packets with a set ownership bit
-		 */
-		while (buffer_desc->addr.bm.bOwnership == 1) {
-			if (buffer_desc->status.bm.bEof == 1) {
-				m = sc->rx_mbuf[sc->rx_bd_fill_idx];
+		sc->rx_idx_head = idx;
 
-				/* New mbuf for desc */
-				n = if_atsam_new_mbuf(ifp);
-				if (n != NULL) {
-					rx_update_mbuf(m, buffer_desc);
+		/* Enable RX interrupts */
+		pHw->GMAC_IER = RX_INTERRUPTS;
 
-					IF_ATSAM_UNLOCK(sc);
-					sc->ifp->if_input(ifp, m);
-					IF_ATSAM_LOCK(sc);
-					m = n;
-				} else {
-					(void)if_atsam_event_send(
-					    sc->tx_daemon_tid, ATSAMV7_ETH_START_TRANSMIT_EVENT);
-				}
-				sc->rx_mbuf[sc->rx_bd_fill_idx] = m;
-				tmp_rx_bd_address = (uint32_t)m->m_data &
-				    GMAC_RX_BUF_DESC_ADDR_MASK;
-
-				/* Switch pointer to next buffer descriptor */
-				if (sc->rx_bd_fill_idx ==
-				    (sc->amount_rx_buf - 1)) {
-					tmp_rx_bd_address |= GMAC_RX_SET_WRAP;
-					sc->rx_bd_fill_idx = 0;
-				} else {
-					++sc->rx_bd_fill_idx;
-				}
+		(void) rtems_event_receive(ATSAMV7_ETH_RX_EVENT_INTERRUPT,
+		    RTEMS_EVENT_ALL | RTEMS_WAIT, RTEMS_NO_TIMEOUT, &out);
 
-				/*
-				 * Give ownership to GMAC for further processing
-				 */
-				tmp_rx_bd_address &= ~GMAC_RX_SET_USED;
-				_ARM_Data_synchronization_barrier();
-				buffer_desc->addr.val = tmp_rx_bd_address;
+		while (true) {
+			uint32_t addr;
+			uint32_t status;
 
-				buffer_desc = (sGmacRxDescriptor *)rx_bd_base
-				    + sc->rx_bd_fill_idx;
+			addr = rx->bds[idx].addr.val;
+			if ((addr & GMAC_RX_OWNERSHIP_BIT) == 0) {
+				break;
 			}
-		}
-		/* Setup the interrupts for RX completion and errors */
-		GMAC_EnableIt(pHw, GMAC_IER_RCOMP | GMAC_IER_ROVR, 0);
-	}
-}
 
-/*
- * Update of current transmit buffer position.
- */
-static void if_atsam_tx_bd_pos_update(size_t *pos, size_t amount_tx_buf)
-{
-	*pos = (*pos + 1) % amount_tx_buf;
-}
+			status = rx->bds[idx].status.val;
+			m = mbufs[idx];
 
-/*
- * Is RingBuffer empty
- */
-static bool if_atsam_ring_buffer_empty(ring_buffer *ring_buffer)
-{
-	return (ring_buffer->tx_bd_used == ring_buffer->tx_bd_free);
-}
+			if (__predict_true((status & GMAC_RX_EOF_BIT) != 0)) {
+				struct mbuf *n;
 
-/*
- * Is RingBuffer full
- */
-static bool if_atsam_ring_buffer_full(ring_buffer *ring_buffer)
-{
-	size_t tx_bd_used_next = ring_buffer->tx_bd_used;
+				n = if_atsam_new_mbuf(ifp);
+				if (n != NULL) {
+					if_atsam_rx_update_mbuf(m, status);
+					(*ifp->if_input)(ifp, m);
+					m = n;
+				}
+			} else {
+				if_inc_counter(ifp, IFCOUNTER_IERRORS, 1);
+			}
 
-	if_atsam_tx_bd_pos_update(&tx_bd_used_next, ring_buffer->length);
-	return (tx_bd_used_next == ring_buffer->tx_bd_free);
-}
+			mbufs[idx] = m;
+			rx->bds[idx].addr.val = mtod(m, uint32_t) |
+			    RX_DESC_WRAP(idx);
 
-/*
- * Cleanup transmit file descriptors by freeing mbufs which are not needed any
- * longer due to correct transmission.
- */
-static void if_atsam_tx_bd_cleanup(if_atsam_softc *sc)
-{
-	struct mbuf *m;
-	volatile sGmacTxDescriptor *cur;
-	bool eof_needed = false;
-
-	while (!if_atsam_ring_buffer_empty(&sc->tx_ring)){
-		cur = sc->tx_bd_base + sc->tx_ring.tx_bd_free;
-		if (((cur->status.bm.bUsed == 1) && !eof_needed) || eof_needed) {
-			eof_needed = true;
-			cur->status.val |= GMAC_TX_SET_USED;
-			m = sc->tx_mbuf[sc->tx_ring.tx_bd_free];
-			m_free(m);
-			sc->tx_mbuf[sc->tx_ring.tx_bd_free] = 0;
-			if_atsam_tx_bd_pos_update(&sc->tx_ring.tx_bd_free,
-			    sc->tx_ring.length);
-			if (cur->status.bm.bLastBuffer) {
-				eof_needed = false;
-			}
-		} else {
-			break;
+			idx = (idx + 1) % RX_DESC_COUNT;
 		}
 	}
 }
 
-/*
- * Prepare Ethernet frame to start transmission.
- */
-static bool if_atsam_send_packet(if_atsam_softc *sc, struct mbuf *m)
+static void
+if_atsam_tx_reclaim(struct if_atsam_softc *sc, struct ifnet *ifp)
 {
-	volatile sGmacTxDescriptor *cur;
-	volatile sGmacTxDescriptor *start_packet_tx_bd = 0;
-	int pos = 0;
-	uint32_t tmp_val = 0;
-	Gmac *pHw = sc->Gmac_inst.gGmacd.pHw;
-	bool success;
-	int csum_flags = m->m_pkthdr.csum_flags;
+	uint32_t head_idx;
+	uint32_t tail_idx;
+	volatile sGmacTxDescriptor *base;
 
-	if_atsam_tx_bd_cleanup(sc);
-	/* Wait for interrupt in case no buffer descriptors are available */
-	/* Wait for events */
-	while (true) {
-		if (if_atsam_ring_buffer_full(&sc->tx_ring)) {
-			/* Setup the interrupts for TX completion and errors */
-			GMAC_EnableIt(pHw, GMAC_INT_TX_BITS, 0);
-			success = false;
+	head_idx = sc->tx_idx_head;
+	tail_idx = sc->tx_idx_tail;
+	base = &sc->tx->bds[0];
+
+	while (head_idx != tail_idx) {
+		uint32_t status;
+		ift_counter cnt;
+		struct mbuf *m;
+
+		status = base[tail_idx].status.val;
+
+		if ((status & GMAC_TX_USED_BIT) == 0) {
 			break;
 		}
 
-		/*
-		 * Get current mbuf for data fill
-		 */
-		cur = &sc->tx_bd_base[sc->tx_ring.tx_bd_used];
-		/* Set the transfer data */
-		if (m->m_len) {
-			uintptr_t cache_adjustment = mtod(m, uintptr_t) % 32;
-
-			rtems_cache_flush_multiple_data_lines(
-			  mtod(m, const char *) - cache_adjustment,
-			  (size_t)(m->m_len + cache_adjustment));
-
-			cur->addr = mtod(m, uint32_t);
-			tmp_val = (uint32_t)m->m_len | GMAC_TX_SET_USED;
-			if (sc->tx_ring.tx_bd_used == (sc->tx_ring.length - 1)) {
-				tmp_val |= GMAC_TX_SET_WRAP;
-			}
-			if (pos == 0) {
-				start_packet_tx_bd = cur;
-			}
-			sc->tx_mbuf[sc->tx_ring.tx_bd_used] = m;
-			m = m->m_next;
-			if_atsam_tx_bd_pos_update(&sc->tx_ring.tx_bd_used,
-			    sc->tx_ring.length);
+		if (__predict_true((status & GMAC_TX_ERR_BITS) == 0)) {
+			cnt = IFCOUNTER_OPACKETS;
 		} else {
-			/* Discard empty mbufs */
-			m = m_free(m);
+			cnt = IFCOUNTER_OERRORS;
 		}
 
-		/*
-		 * Send out the buffer once the complete mbuf_chain has been
-		 * processed
-		 */
-		if (m == NULL) {
-			tmp_val |= GMAC_TX_SET_EOF;
-			tmp_val &= ~GMAC_TX_SET_USED;
-			if ((csum_flags & (CSUM_IP | CSUM_TCP | CSUM_UDP |
-			    CSUM_TCP_IPV6 | CSUM_UDP_IPV6)) != 0) {
-				start_packet_tx_bd->status.bm.bNoCRC = 0;
-			} else {
-				start_packet_tx_bd->status.bm.bNoCRC = 1;
-			}
-			_ARM_Data_synchronization_barrier();
-			cur->status.val = tmp_val;
-			start_packet_tx_bd->status.val &= ~GMAC_TX_SET_USED;
-			_ARM_Data_synchronization_barrier();
-			GMAC_TransmissionStart(pHw);
-			success = true;
-			break;
-		} else {
-			if (pos > 0) {
-				tmp_val &= ~GMAC_TX_SET_USED;
+		while ((m = sc->tx_mbufs[tail_idx]) == NULL ) {
+			base[tail_idx].status.val = status | GMAC_TX_USED_BIT;
+			tail_idx = (tail_idx + 1) % TX_DESC_COUNT;
+			status = base[tail_idx].status.val;
+
+			if (__predict_false((status & GMAC_TX_ERR_BITS) != 0)) {
+				cnt = IFCOUNTER_OERRORS;
 			}
-			pos++;
-			cur->status.val = tmp_val;
 		}
+
+		base[tail_idx].status.val = status | GMAC_TX_USED_BIT;
+		if_inc_counter(ifp, cnt, 1);
+		sc->tx_mbufs[tail_idx] = NULL;
+		m_freem(m);
+
+		tail_idx = (tail_idx + 1) % TX_DESC_COUNT;
 	}
-	return success;
-}
 
+	sc->tx_idx_tail = tail_idx;
+}
 
-/*
- * Transmit daemon
- */
-static void if_atsam_tx_daemon(void *arg)
+static void
+if_atsam_cache_flush(uintptr_t begin, uintptr_t size)
 {
-	if_atsam_softc *sc = (if_atsam_softc *)arg;
-	rtems_event_set events = 0;
-	sGmacTxDescriptor *buffer_desc;
-	int bd_number;
-	void *tx_bd_base;
-	struct mbuf *m;
-	bool success;
+	uintptr_t end;
+	uintptr_t mask;
+
+	/* Align begin and end of the data to a cache line */
+	end = begin + size;
+	mask = CPU_CACHE_LINE_BYTES - 1;
+	begin &= ~mask;
+	end = (end + mask) & ~mask;
+	rtems_cache_flush_multiple_data_lines((void *)begin, end - begin);
+}
 
-	IF_ATSAM_LOCK(sc);
+static int
+if_atsam_tx_enqueue(struct if_atsam_softc *sc, struct ifnet *ifp, struct mbuf *m)
+{
+	size_t head_idx;
+	size_t tail_idx;
+	size_t capacity;
+	size_t idx;
+	volatile sGmacTxDescriptor *base;
+	volatile sGmacTxDescriptor *desc;
+	size_t bufs;
+	uint32_t status;
+	struct mbuf *n;
 
-	Gmac *pHw = sc->Gmac_inst.gGmacd.pHw;
-	struct ifnet *ifp = sc->ifp;
+	head_idx = sc->tx_idx_head;
+	tail_idx = sc->tx_idx_tail;
+	capacity = (tail_idx - head_idx - 1) % TX_DESC_COUNT;
 
-	GMAC_TransmitEnable(pHw, 0);
+	idx = head_idx;
+	base = &sc->tx->bds[0];
+	bufs = 0;
+	n = m;
 
-	/* Allocate memory space for priority queue descriptor list */
-	tx_bd_base = rtems_cache_coherent_allocate(sizeof(sGmacTxDescriptor),
-		GMAC_DESCRIPTOR_ALIGNMENT, 0);
-	assert(tx_bd_base != NULL);
+	do {
+		uint32_t size;
 
-	buffer_desc = (sGmacTxDescriptor *)tx_bd_base;
-	buffer_desc->addr = 0;
-	buffer_desc->status.val = GMAC_TX_SET_USED | GMAC_TX_SET_WRAP;
+		desc = &base[idx];
 
-	GMAC_SetTxQueue(pHw, (uint32_t)buffer_desc, 1);
-	GMAC_SetTxQueue(pHw, (uint32_t)buffer_desc, 2);
+		size = (uint32_t)n->m_len;
+		if (__predict_true(size > 0)) {
+			uintptr_t begin;
 
-	/* Allocate memory space for buffer descriptor list */
-	tx_bd_base = rtems_cache_coherent_allocate(
-		sc->amount_tx_buf * sizeof(sGmacTxDescriptor),
-		GMAC_DESCRIPTOR_ALIGNMENT, 0);
-	assert(tx_bd_base != NULL);
-	buffer_desc = (sGmacTxDescriptor *)tx_bd_base;
+			++bufs;
+			if (__predict_false(bufs > capacity)) {
+				return (ENOBUFS);
+			}
 
-	/* Create descriptor list and mark as empty */
-	for (bd_number = 0; bd_number < sc->amount_tx_buf; bd_number++) {
-		buffer_desc->addr = 0;
-		buffer_desc->status.val = GMAC_TX_SET_USED;
-		if (bd_number == (sc->amount_tx_buf - 1)) {
-			buffer_desc->status.bm.bWrap = 1;
-		} else {
-			buffer_desc++;
+			begin = mtod(n, uintptr_t);
+			desc->addr = (uint32_t)begin;
+			status = GMAC_TX_USED_BIT | TX_DESC_WRAP(idx) | size;
+			desc->status.val = status;
+			if_atsam_cache_flush(begin, size);
+			idx = (idx + 1) % TX_DESC_COUNT;
 		}
+
+		n = n->m_next;
+	} while (n != NULL);
+
+	sc->tx_idx_head = idx;
+
+	idx = (idx - 1) % TX_DESC_COUNT;
+	desc = &base[idx];
+	sc->tx_mbufs[idx] = m;
+	status = GMAC_TX_LAST_BUFFER_BIT;
+
+	while (idx != head_idx) {
+		desc->status.val = (desc->status.val & ~GMAC_TX_USED_BIT) |
+		    status;
+		status = 0;
+
+		idx = (idx - 1) % TX_DESC_COUNT;
+		desc = &base[idx];
 	}
-	buffer_desc = (sGmacTxDescriptor *)tx_bd_base;
 
-	/* Write Buffer Queue Base Address Register */
-	GMAC_SetTxQueue(pHw, (uint32_t)buffer_desc, 0);
+	desc->status.val = (desc->status.val & ~GMAC_TX_USED_BIT) | status;
+	_ARM_Data_synchronization_barrier();
+	sc->Gmac_inst.gGmacd.pHw->GMAC_NCR |= GMAC_NCR_TSTART;
+	ETHER_BPF_MTAP(ifp, m);
+	return (0);
+}
 
-	/* Enable Transmission of data */
-	GMAC_TransmitEnable(pHw, 1);
+static int
+if_atsam_transmit(struct ifnet *ifp, struct mbuf *m)
+{
+	struct if_atsam_softc *sc;
+	int error;
 
-	/* Set variables in context */
-	sc->tx_bd_base = tx_bd_base;
+	if (__predict_false((m->m_flags & M_VLANTAG) != 0)) {
+		struct mbuf *n;
 
-	while (true) {
-		/* Wait for events */
-		if_atsam_event_receive(sc,
-		    ATSAMV7_ETH_START_TRANSMIT_EVENT |
-		    ATSAMV7_ETH_TX_EVENT_INTERRUPT);
-		//printf("TX Transmit Event received\n");
-
-		/*
-		 * Send packets till queue is empty
-		 */
-		while (true) {
-			/*
-			 * Get the mbuf chain to transmit
-			 */
-			if_atsam_tx_bd_cleanup(sc);
-			IF_DEQUEUE(&ifp->if_snd, m);
-			if (!m) {
-				ifp->if_drv_flags &= ~IFF_DRV_OACTIVE;
-				break;
-			}
-			success = if_atsam_send_packet(sc, m);
-			if (!success){
-				break;
-			}
+		n = ether_vlanencap(m, m->m_pkthdr.ether_vtag);
+		if (n == NULL) {
+			m_freem(m);
+			return (ENOBUFS);
 		}
+
+		m = n;
 	}
-}
 
+	sc = ifp->if_softc;
+	IF_ATSAM_LOCK(sc);
 
-/*
- * Send packet (caller provides header).
- */
-static void if_atsam_enet_start(struct ifnet *ifp)
-{
-	if_atsam_softc *sc = (if_atsam_softc *)ifp->if_softc;
+	error = if_atsam_tx_enqueue(sc, ifp, m);
+	if_atsam_tx_reclaim(sc, ifp);
 
-	ifp->if_drv_flags |= IFF_DRV_OACTIVE;
-	if_atsam_event_send(sc->tx_daemon_tid,
-	    ATSAMV7_ETH_START_TRANSMIT_EVENT);
-}
+	if (__predict_false(error != 0)) {
+		struct mbuf *n;
 
+		n = m_defrag(m, M_NOWAIT);
+		if (n != NULL) {
+			m = n;
+		}
+
+		error = if_atsam_tx_enqueue(sc, ifp, m);
+		if (error != 0) {
+			m_freem(m);
+			if_inc_counter(ifp, IFCOUNTER_OQDROPS, 1);
+		}
+	}
+
+	IF_ATSAM_UNLOCK(sc);
+	return (error);
+}
 
 static uint8_t if_atsam_get_gmac_linkspeed_from_media(uint32_t media_subtype)
 {
@@ -975,23 +853,55 @@ if_atsam_tick(void *context)
 	callout_reset(&sc->tick_ch, hz, if_atsam_tick, sc);
 }
 
+static void
+if_atsam_setup_tx(struct if_atsam_softc *sc)
+{
+	sGmacTxDescriptor *base;
+	struct if_atsam_tx_bds *tx;
+	size_t i;
+	Gmac *pHw;
 
-/*
- * Sets up the hardware and chooses the interface to be used
- */
-static void if_atsam_init(void *arg)
+	pHw = sc->Gmac_inst.gGmacd.pHw;
+	GMAC_TransmitEnable(pHw, 0);
+
+	/* Allocate memory space for priority queue descriptor list */
+	base = rtems_cache_coherent_allocate(sizeof(base),
+	    GMAC_DESCRIPTOR_ALIGNMENT, 0);
+	assert(base != NULL);
+
+	base->addr = 0;
+	base->status.val = GMAC_TX_USED_BIT | GMAC_TX_WRAP_BIT;
+
+	GMAC_SetTxQueue(pHw, (uint32_t)base, 1);
+	GMAC_SetTxQueue(pHw, (uint32_t)base, 2);
+
+	/* Allocate memory space for buffer descriptor list */
+	tx = rtems_cache_coherent_allocate(sizeof(*sc->tx),
+	    GMAC_DESCRIPTOR_ALIGNMENT, 0);
+	assert(tx != NULL);
+
+	/* Set variables in context */
+	sc->tx = tx;
+
+	/* Create descriptor list and mark as empty */
+	for (i = 0; i < TX_DESC_COUNT; ++i) {
+		tx->bds[i].addr = 0;
+		tx->bds[i].status.val = GMAC_TX_USED_BIT | TX_DESC_WRAP(i);
+	}
+
+	/* Write Buffer Queue Base Address Register */
+	GMAC_SetTxQueue(pHw, (uint32_t)&tx->bds[0], 0);
+
+	/* Enable Transmission of data */
+	GMAC_TransmitEnable(pHw, 1);
+}
+
+static void
+if_atsam_init(if_atsam_softc *sc)
 {
 	rtems_status_code status;
-
-	if_atsam_softc *sc = (if_atsam_softc *)arg;
-	struct ifnet *ifp = sc->ifp;
 	uint32_t dmac_cfg = 0;
-	uint32_t gmii_val = 0;
 
-	if (ifp->if_flags & IFF_DRV_RUNNING) {
-		return;
-	}
-	ifp->if_flags |= IFF_DRV_RUNNING;
 	sc->interrupt_number = GMAC_IRQn;
 
 	/* Enable Peripheral Clock */
@@ -1000,7 +910,6 @@ static void if_atsam_init(void *arg)
 	}
 	/* Setup interrupts */
 	NVIC_ClearPendingIRQ(GMAC_IRQn);
-	NVIC_EnableIRQ(GMAC_IRQn);
 
 	/* Configuration of DMAC */
 	dmac_cfg = (GMAC_DCFGR_DRBS(GMAC_RX_BUFFER_SIZE >> 6)) |
@@ -1011,20 +920,17 @@ static void if_atsam_init(void *arg)
 	/* Enable hardware checksum offload for receive */
 	sc->Gmac_inst.gGmacd.pHw->GMAC_NCFGR |= GMAC_NCFGR_RXCOEN;
 
+	/* Use Multicast Hash Filter */
+	sc->Gmac_inst.gGmacd.pHw->GMAC_NCFGR |= GMAC_NCFGR_MTIHEN;
+	sc->Gmac_inst.gGmacd.pHw->GMAC_HRB = 0;
+	sc->Gmac_inst.gGmacd.pHw->GMAC_HRT = 0;
+
 	/* Shut down Transmit and Receive */
 	GMAC_ReceiveEnable(sc->Gmac_inst.gGmacd.pHw, 0);
 	GMAC_TransmitEnable(sc->Gmac_inst.gGmacd.pHw, 0);
 
 	GMAC_StatisticsWriteEnable(sc->Gmac_inst.gGmacd.pHw, 1);
 
-	/*
-	 * Allocate mbuf pointers
-	 */
-	sc->rx_mbuf = malloc(sc->amount_rx_buf * sizeof *sc->rx_mbuf,
-		M_TEMP, M_NOWAIT);
-	sc->tx_mbuf = malloc(sc->amount_tx_buf * sizeof *sc->tx_mbuf,
-		M_TEMP, M_NOWAIT);
-
 	/* Install interrupt handler */
 	status = rtems_interrupt_handler_install(sc->interrupt_number,
 		"Ethernet",
@@ -1036,30 +942,143 @@ static void if_atsam_init(void *arg)
 	/*
 	 * Start driver tasks
 	 */
-	sc->rx_daemon_tid = rtems_bsdnet_newproc("SCrx", 4096,
-		if_atsam_rx_daemon, sc);
-	sc->tx_daemon_tid = rtems_bsdnet_newproc("SCtx", 4096,
-		if_atsam_tx_daemon, sc);
+
+	status = rtems_task_create(rtems_build_name('S', 'C', 'r', 'x'),
+	    rtems_bsd_get_task_priority(device_get_name(sc->dev)), 4096,
+	    RTEMS_DEFAULT_MODES, RTEMS_DEFAULT_MODES, &sc->rx_daemon_tid);
+	assert(status == RTEMS_SUCCESSFUL);
+
+	status = rtems_task_start(sc->rx_daemon_tid, if_atsam_rx_daemon,
+	    (rtems_task_argument)sc);
+	assert(status == RTEMS_SUCCESSFUL);
 
 	callout_reset(&sc->tick_ch, hz, if_atsam_tick, sc);
 
+	if_atsam_setup_tx(sc);
+}
+
+static int
+if_atsam_get_hash_index(const uint8_t *eaddr)
+{
+	uint64_t eaddr64;
+	int index;
+	int i;
+
+	eaddr64 = eaddr[5];
+
+	for (i = 4; i >= 0; --i) {
+		eaddr64 <<= 8;
+		eaddr64 |= eaddr[i];
+	}
+
+	index = 0;
+
+	for (i = 0; i < 6; ++i) {
+		uint64_t bits;
+		int j;
+		int hash;
+
+		bits = eaddr64 >> i;
+		hash = bits & 1;
+
+		for (j = 1; j < 8; ++j) {
+			bits >>= 6;
+			hash ^= bits & 1;
+		}
+
+		index |= hash << i;
+	}
+
+	return index;
+}
+
+static void
+if_atsam_setup_rxfilter(struct if_atsam_softc *sc)
+{
+	struct ifnet *ifp;
+	struct ifmultiaddr *ifma;
+	uint64_t mhash;
+	Gmac *pHw;
+
+	pHw = sc->Gmac_inst.gGmacd.pHw;
+
+	if ((sc->ifp->if_flags & IFF_PROMISC) != 0) {
+		pHw->GMAC_NCFGR |= GMAC_NCFGR_CAF;
+	} else {
+		pHw->GMAC_NCFGR &= ~GMAC_NCFGR_CAF;
+	}
+
+	ifp = sc->ifp;
+
+	if ((ifp->if_flags & IFF_ALLMULTI))
+		mhash = 0xffffffffffffffffLLU;
+	else {
+		mhash = 0;
+		if_maddr_rlock(ifp);
+		CK_STAILQ_FOREACH(ifma, &sc->ifp->if_multiaddrs, ifma_link) {
+			if (ifma->ifma_addr->sa_family != AF_LINK)
+				continue;
+			mhash |= 1LLU << if_atsam_get_hash_index(
+			    LLADDR((struct sockaddr_dl *) ifma->ifma_addr));
+		}
+		if_maddr_runlock(ifp);
+	}
+
+	pHw->GMAC_HRB = (uint32_t)mhash;
+	pHw->GMAC_HRT = (uint32_t)(mhash >> 32);
+}
+
+static void
+if_atsam_start_locked(struct if_atsam_softc *sc)
+{
+	struct ifnet *ifp = sc->ifp;
+	Gmac *pHw = sc->Gmac_inst.gGmacd.pHw;
+
+	if (ifp->if_drv_flags & IFF_DRV_RUNNING) {
+		return;
+	}
+
 	ifp->if_drv_flags |= IFF_DRV_RUNNING;
+
+	if_atsam_setup_rxfilter(sc);
+
+	/* Enable TX/RX */
+	pHw->GMAC_NCR |= GMAC_NCR_RXEN | GMAC_NCR_TXEN;
 }
 
+static void
+if_atsam_start(void *arg)
+{
+	struct if_atsam_softc *sc = arg;
 
-/*
- * Stop the device
- */
-static void if_atsam_stop(struct if_atsam_softc *sc)
+	IF_ATSAM_LOCK(sc);
+	if_atsam_start_locked(sc);
+	IF_ATSAM_UNLOCK(sc);
+}
+
+static void
+if_atsam_stop_locked(struct if_atsam_softc *sc)
 {
 	struct ifnet *ifp = sc->ifp;
 	Gmac *pHw = sc->Gmac_inst.gGmacd.pHw;
+	size_t i;
 
-	ifp->if_flags &= ~IFF_DRV_RUNNING;
+	ifp->if_drv_flags &= ~IFF_DRV_RUNNING;
 
-	/* Disable MDIO interface and TX/RX */
+	/* Disable TX/RX */
 	pHw->GMAC_NCR &= ~(GMAC_NCR_RXEN | GMAC_NCR_TXEN);
-	pHw->GMAC_NCR &= ~GMAC_NCR_MPE;
+
+	/* Reinitialize the TX descriptors */
+
+	sc->tx_idx_head = 0;
+	sc->tx_idx_tail = 0;
+
+	for (i = 0; i < TX_DESC_COUNT; ++i) {
+		sc->tx->bds[i].addr = 0;
+		sc->tx->bds[i].status.val = GMAC_TX_USED_BIT | TX_DESC_WRAP(i);
+		m_freem(sc->tx_mbufs[i]);
+		sc->tx_mbufs[i] = NULL;
+	}
 }
 
 
@@ -1070,7 +1089,7 @@ if_atsam_poll_hw_stats(struct if_atsam_softc *sc)
 	Gmac *pHw = sc->Gmac_inst.gGmacd.pHw;
 
 	octets = pHw->GMAC_OTLO;
-	octets |= pHw->GMAC_OTHI << 32;
+	octets |= (uint64_t)pHw->GMAC_OTHI << 32;
 	sc->stats.octets_transm += octets;
 	sc->stats.frames_transm += pHw->GMAC_FT;
 	sc->stats.broadcast_frames_transm += pHw->GMAC_BCFT;
@@ -1092,7 +1111,7 @@ if_atsam_poll_hw_stats(struct if_atsam_softc *sc)
 	sc->stats.carrier_sense_errors += pHw->GMAC_CSE;
 
 	octets = pHw->GMAC_ORLO;
-	octets |= pHw->GMAC_ORHI << 32;
+	octets |= (uint64_t)pHw->GMAC_ORHI << 32;
 	sc->stats.octets_rec += octets;
 	sc->stats.frames_rec += pHw->GMAC_FR;
 	sc->stats.broadcast_frames_rec += pHw->GMAC_BCFR;
@@ -1120,24 +1139,159 @@ if_atsam_poll_hw_stats(struct if_atsam_softc *sc)
 	sc->stats.udp_checksum_errors += pHw->GMAC_UCE;
 }
 
+static int
+if_atsam_stats_reset(SYSCTL_HANDLER_ARGS)
+{
+	struct if_atsam_softc *sc = arg1;
+	int value;
+	int error;
+
+	value = 0;
+	error = sysctl_handle_int(oidp, &value, 0, req);
+	if (error != 0 || req->newptr == NULL) {
+		return (error);
+	}
+
+	if (value != 0) {
+		IF_ATSAM_LOCK(sc);
+		if_atsam_poll_hw_stats(sc);
+		memset(&sc->stats, 0, sizeof(sc->stats));
+		IF_ATSAM_UNLOCK(sc);
+	}
+
+	return (0);
+}
+
+static int
+if_atsam_sysctl_reg(SYSCTL_HANDLER_ARGS)
+{
+	u_int value;
+
+	value = *(uint32_t *)arg1;
+	return (sysctl_handle_int(oidp, &value, 0, req));
+}
+
+static int
+if_atsam_sysctl_tx_desc(SYSCTL_HANDLER_ARGS)
+{
+	struct if_atsam_softc *sc = arg1;
+	Gmac *pHw = sc->Gmac_inst.gGmacd.pHw;
+	struct sbuf *sb;
+	int error;
+	size_t i;
+
+	error = sysctl_wire_old_buffer(req, 0);
+	if (error != 0) {
+		return (error);
+	}
+
+	sb = sbuf_new_for_sysctl(NULL, NULL, 1024, req);
+	if (sb == NULL) {
+		return (ENOMEM);
+	}
+
+	sbuf_printf(sb, "\n\tHead %u\n", sc->tx_idx_head);
+	sbuf_printf(sb, "\tTail %u\n", sc->tx_idx_tail);
+	sbuf_printf(sb, "\tDMA  %u\n",
+	    (pHw->GMAC_TBQB - (uintptr_t)&sc->tx->bds[0]) / 8);
+
+	for (i = 0; i < TX_DESC_COUNT; ++i) {
+		sbuf_printf(sb, "\t[%2u] %08x %08x\n", i,
+		    sc->tx->bds[i].status.val, sc->tx->bds[i].addr);
+	}
+
+	error = sbuf_finish(sb);
+	sbuf_delete(sb);
+	return (error);
+}
+
+static int
+if_atsam_sysctl_rx_desc(SYSCTL_HANDLER_ARGS)
+{
+	struct if_atsam_softc *sc = arg1;
+	Gmac *pHw = sc->Gmac_inst.gGmacd.pHw;
+	struct sbuf *sb;
+	int error;
+	size_t i;
+
+	error = sysctl_wire_old_buffer(req, 0);
+	if (error != 0) {
+		return (error);
+	}
+
+	sb = sbuf_new_for_sysctl(NULL, NULL, 1024, req);
+	if (sb == NULL) {
+		return (ENOMEM);
+	}
+
+	sbuf_printf(sb, "\n\tHead %u\n", sc->rx_idx_head);
+	sbuf_printf(sb, "\tDMA  %u\n",
+	    (pHw->GMAC_RBQB - (uintptr_t)&sc->rx->bds[0]) / 8);
+
+	for (i = 0; i < RX_DESC_COUNT; ++i) {
+		sbuf_printf(sb, "\t[%2u] %08x %08x\n", i,
+		    sc->rx->bds[i].status.val, sc->rx->bds[i].addr);
+	}
+
+	error = sbuf_finish(sb);
+	sbuf_delete(sb);
+	return (error);
+}
 
 static void
 if_atsam_add_sysctls(device_t dev)
 {
 	struct if_atsam_softc *sc = device_get_softc(dev);
+	Gmac *pHw = sc->Gmac_inst.gGmacd.pHw;
 	struct sysctl_ctx_list *ctx;
+	struct sysctl_oid_list *base;
 	struct sysctl_oid_list *statsnode;
 	struct sysctl_oid_list *hwstatsnode;
 	struct sysctl_oid_list *child;
 	struct sysctl_oid *tree;
 
 	ctx = device_get_sysctl_ctx(dev);
-	child = SYSCTL_CHILDREN(device_get_sysctl_tree(dev));
+	base = SYSCTL_CHILDREN(device_get_sysctl_tree(dev));
 
-	tree = SYSCTL_ADD_NODE(ctx, child, OID_AUTO, "stats", CTLFLAG_RD,
+	tree = SYSCTL_ADD_NODE(ctx, base, OID_AUTO, "regs", CTLFLAG_RD,
+			       NULL, "if_atsam registers");
+	child = SYSCTL_CHILDREN(tree);
+
+	SYSCTL_ADD_PROC(ctx, child, OID_AUTO, "txdesc", CTLTYPE_STRING |
+	    CTLFLAG_RD, sc, 0, if_atsam_sysctl_tx_desc, "A",
+	    "Transmit Descriptors");
+	SYSCTL_ADD_PROC(ctx, child, OID_AUTO, "rxdesc", CTLTYPE_STRING |
+	    CTLFLAG_RD, sc, 0, if_atsam_sysctl_rx_desc, "A",
+	    "Receive Descriptors");
+	SYSCTL_ADD_PROC(ctx, child, OID_AUTO, "imr", CTLTYPE_UINT |
+	    CTLFLAG_RD, __DEVOLATILE(uint32_t *, &pHw->GMAC_IMR), 0,
+	    if_atsam_sysctl_reg, "I", "IMR");
+	SYSCTL_ADD_PROC(ctx, child, OID_AUTO, "isr", CTLTYPE_UINT |
+	    CTLFLAG_RD, __DEVOLATILE(uint32_t *, &pHw->GMAC_ISR), 0,
+	    if_atsam_sysctl_reg, "I", "ISR");
+	SYSCTL_ADD_PROC(ctx, child, OID_AUTO, "rsr", CTLTYPE_UINT |
+	    CTLFLAG_RD, __DEVOLATILE(uint32_t *, &pHw->GMAC_RSR), 0,
+	    if_atsam_sysctl_reg, "I", "RSR");
+	SYSCTL_ADD_PROC(ctx, child, OID_AUTO, "tsr", CTLTYPE_UINT |
+	    CTLFLAG_RD, __DEVOLATILE(uint32_t *, &pHw->GMAC_TSR), 0,
+	    if_atsam_sysctl_reg, "I", "TSR");
+	SYSCTL_ADD_PROC(ctx, child, OID_AUTO, "nsr", CTLTYPE_UINT |
+	    CTLFLAG_RD, __DEVOLATILE(uint32_t *, &pHw->GMAC_NSR), 0,
+	    if_atsam_sysctl_reg, "I", "NSR");
+	SYSCTL_ADD_PROC(ctx, child, OID_AUTO, "ncfgr", CTLTYPE_UINT |
+	    CTLFLAG_RD, __DEVOLATILE(uint32_t *, &pHw->GMAC_NCFGR), 0,
+	    if_atsam_sysctl_reg, "I", "NCFGR");
+	SYSCTL_ADD_PROC(ctx, child, OID_AUTO, "ncr", CTLTYPE_UINT |
+	    CTLFLAG_RD, __DEVOLATILE(uint32_t *, &pHw->GMAC_NCR), 0,
+	    if_atsam_sysctl_reg, "I", "NCR");
+
+	tree = SYSCTL_ADD_NODE(ctx, base, OID_AUTO, "stats", CTLFLAG_RD,
 			       NULL, "if_atsam statistics");
 	statsnode = SYSCTL_CHILDREN(tree);
 
+	SYSCTL_ADD_PROC(ctx, statsnode, OID_AUTO, "reset", CTLTYPE_INT |
+	    CTLFLAG_WR, sc, 0, if_atsam_stats_reset, "I", "Reset");
+
 	tree = SYSCTL_ADD_NODE(ctx, statsnode, OID_AUTO, "sw", CTLFLAG_RD,
 			       NULL, "if_atsam software statistics");
 	child = SYSCTL_CHILDREN(tree);
@@ -1145,12 +1299,12 @@ if_atsam_add_sysctls(device_t dev)
 	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "rx_overrun_errors",
 	    CTLFLAG_RD, &sc->stats.rx_overrun_errors, 0,
 	    "RX overrun errors");
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "rx_used_bit_reads",
+	    CTLFLAG_RD, &sc->stats.rx_used_bit_reads, 0,
+	    "RX used bit reads");
 	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "rx_interrupts",
 	    CTLFLAG_RD, &sc->stats.rx_interrupts, 0,
 	    "Rx interrupts");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "tx_complete_int",
-	    CTLFLAG_RD, &sc->stats.tx_complete_int, 0,
-	    "Tx complete interrupts");
 	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "tx_tur_errors",
 	    CTLFLAG_RD, &sc->stats.tx_tur_errors, 0,
 	    "Error Tur Tx interrupts");
@@ -1163,9 +1317,6 @@ if_atsam_add_sysctls(device_t dev)
 	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "tx_hresp_errors",
 	    CTLFLAG_RD, &sc->stats.tx_hresp_errors, 0,
 	    "Error Hresp Tx interrupts");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "tx_interrupts",
-	    CTLFLAG_RD, &sc->stats.tx_interrupts, 0,
-	    "Tx interrupts");
 
 	tree = SYSCTL_ADD_NODE(ctx, statsnode, OID_AUTO, "hw", CTLFLAG_RD,
 			       NULL, "if_atsam hardware statistics");
@@ -1175,40 +1326,40 @@ if_atsam_add_sysctls(device_t dev)
 			       NULL, "if_atsam hardware transmit statistics");
 	child = SYSCTL_CHILDREN(tree);
 
-	SYSCTL_ADD_UQUAD(ctx, child, OID_AUTO, "octets_transm",
+	SYSCTL_ADD_UQUAD(ctx, child, OID_AUTO, "octets",
 	    CTLFLAG_RD, &sc->stats.octets_transm,
 	    "Octets Transmitted");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_transm",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames",
 	    CTLFLAG_RD, &sc->stats.frames_transm, 0,
 	    "Frames Transmitted");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "broadcast_frames_transm",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "broadcast_frames",
 	    CTLFLAG_RD, &sc->stats.broadcast_frames_transm, 0,
 	    "Broadcast Frames Transmitted");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "multicast_frames_transm",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "multicast_frames",
 	    CTLFLAG_RD, &sc->stats.multicast_frames_transm, 0,
 	    "Multicast Frames Transmitted");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "pause_frames_transm",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "pause_frames",
 	    CTLFLAG_RD, &sc->stats.pause_frames_transm, 0,
 	    "Pause Frames Transmitted");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_64_byte_transm",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_64_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_64_byte_transm, 0,
 	    "64 Byte Frames Transmitted");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_65_to_127_byte_transm",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_65_to_127_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_65_to_127_byte_transm, 0,
 	    "65 to 127 Byte Frames Transmitted");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_128_to_255_byte_transm",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_128_to_255_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_128_to_255_byte_transm, 0,
 	    "128 to 255 Byte Frames Transmitted");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_256_to_511_byte_transm",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_256_to_511_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_256_to_511_byte_transm, 0,
 	    "256 to 511 Byte Frames Transmitted");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_512_to_1023_byte_transm",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_512_to_1023_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_512_to_1023_byte_transm, 0,
 	    "512 to 1023 Byte Frames Transmitted");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_1024_to_1518_byte_transm",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_1024_to_1518_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_1024_to_1518_byte_transm, 0,
 	    "1024 to 1518 Byte Frames Transmitted");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_greater_1518_byte_transm",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_1519_to_maximum_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_greater_1518_byte_transm, 0,
 	    "Greater Than 1518 Byte Frames Transmitted");
 	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "transmit_underruns",
@@ -1237,49 +1388,49 @@ if_atsam_add_sysctls(device_t dev)
 			       NULL, "if_atsam hardware receive statistics");
 	child = SYSCTL_CHILDREN(tree);
 
-	SYSCTL_ADD_UQUAD(ctx, child, OID_AUTO, "octets_rec",
+	SYSCTL_ADD_UQUAD(ctx, child, OID_AUTO, "octets",
 	    CTLFLAG_RD, &sc->stats.octets_rec,
 	    "Octets Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames",
 	    CTLFLAG_RD, &sc->stats.frames_rec, 0,
 	    "Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "broadcast_frames_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "broadcast_frames",
 	    CTLFLAG_RD, &sc->stats.broadcast_frames_rec, 0,
 	    "Broadcast Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "multicast_frames_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "multicast_frames",
 	    CTLFLAG_RD, &sc->stats.multicast_frames_rec, 0,
 	    "Multicast Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "pause_frames_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "pause_frames",
 	    CTLFLAG_RD, &sc->stats.pause_frames_rec, 0,
 	    "Pause Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_64_byte_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_64_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_64_byte_rec, 0,
 	    "64 Byte Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_65_to_127_byte_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_65_to_127_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_65_to_127_byte_rec, 0,
 	    "65 to 127 Byte Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_128_to_255_byte_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_128_to_255_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_128_to_255_byte_rec, 0,
 	    "128 to 255 Byte Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_256_to_511_byte_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_256_to_511_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_256_to_511_byte_rec, 0,
 	    "256 to 511 Byte Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_512_to_1023_byte_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_512_to_1023_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_512_to_1023_byte_rec, 0,
 	    "512 to 1023 Byte Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_1024_to_1518_byte_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_1024_to_1518_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_1024_to_1518_byte_rec, 0,
 	    "1024 to 1518 Byte Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_1519_to_maximum_byte_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frames_1519_to_maximum_bytes",
 	    CTLFLAG_RD, &sc->stats.frames_1519_to_maximum_byte_rec, 0,
 	    "1519 to Maximum Byte Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "undersize_frames_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "undersize_frames",
 	    CTLFLAG_RD, &sc->stats.undersize_frames_rec, 0,
 	    "Undersize Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "oversize_frames_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "oversize_frames",
 	    CTLFLAG_RD, &sc->stats.oversize_frames_rec, 0,
 	    "Oversize Frames Received");
-	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "jabbers_rec",
+	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "jabbers",
 	    CTLFLAG_RD, &sc->stats.jabbers_rec, 0,
 	    "Jabbers Received");
 	SYSCTL_ADD_UINT(ctx, child, OID_AUTO, "frame_check_sequence_errors",
@@ -1311,49 +1462,6 @@ if_atsam_add_sysctls(device_t dev)
 	    "UDP Checksum Errors");
 }
 
-
-/*
- * Calculates the index that is to be sent into the hash registers
- */
-static void if_atsam_get_hash_index(uint64_t addr, uint32_t *val)
-{
-	uint64_t tmp_val;
-	uint8_t i, j;
-	uint64_t idx;
-	int offset = 0;
-
-	addr &= MAC_ADDR_MASK;
-
-	for (i = 0; i < HASH_INDEX_AMOUNT; ++i) {
-		tmp_val = 0;
-		offset = 0;
-		for (j = 0; j < HASH_ELEMENTS_PER_INDEX; j++) {
-			idx = (addr >> (offset + i)) & MAC_IDX_MASK;
-			tmp_val ^= idx;
-			offset += HASH_INDEX_AMOUNT;
-		}
-		if (tmp_val > 0) {
-			*val |= (1u << i);
-		}
-	}
-}
-
-
-/*
- * Dis/Enable promiscuous Mode
- */
-static void if_atsam_promiscuous_mode(if_atsam_softc *sc, bool enable)
-{
-	Gmac *pHw = sc->Gmac_inst.gGmacd.pHw;
-
-	if (enable) {
-		pHw->GMAC_NCFGR |= GMAC_PROM_ENABLE;
-	} else {
-		pHw->GMAC_NCFGR &= ~GMAC_PROM_ENABLE;
-	}
-}
-
-
 static int
 if_atsam_mediaioctl(if_atsam_softc *sc, struct ifreq *ifr, u_long command)
 {
@@ -1380,8 +1488,6 @@ if_atsam_ioctl(struct ifnet *ifp, ioctl_command_t command, caddr_t data)
 	if_atsam_softc *sc = (if_atsam_softc *)ifp->if_softc;
 	struct ifreq *ifr = (struct ifreq *)data;
 	int rv = 0;
-	bool prom_enable;
-	struct mii_data *mii;
 
 	switch (command) {
 	case SIOCGIFMEDIA:
@@ -1389,17 +1495,31 @@ if_atsam_ioctl(struct ifnet *ifp, ioctl_command_t command, caddr_t data)
 		rv = if_atsam_mediaioctl(sc, ifr, command);
 		break;
 	case SIOCSIFFLAGS:
+		IF_ATSAM_LOCK(sc);
 		if (ifp->if_flags & IFF_UP) {
-			if (!(ifp->if_drv_flags & IFF_DRV_RUNNING)) {
-				if_atsam_init(sc);
+			if (ifp->if_drv_flags & IFF_DRV_RUNNING) {
+				if ((ifp->if_flags ^ sc->if_flags) &
+				    (IFF_PROMISC | IFF_ALLMULTI)) {
+					if_atsam_setup_rxfilter(sc);
+				}
+			} else {
+				if_atsam_start_locked(sc);
 			}
-			prom_enable = ((ifp->if_flags & IFF_PROMISC) != 0);
-			if_atsam_promiscuous_mode(sc, prom_enable);
 		} else {
 			if (ifp->if_drv_flags & IFF_DRV_RUNNING) {
-				if_atsam_stop(sc);
+				if_atsam_stop_locked(sc);
 			}
 		}
+		sc->if_flags = ifp->if_flags;
+		IF_ATSAM_UNLOCK(sc);
+		break;
+	case SIOCADDMULTI:
+	case SIOCDELMULTI:
+		if (ifp->if_drv_flags & IFF_DRV_RUNNING) {
+			IF_ATSAM_LOCK(sc);
+			if_atsam_setup_rxfilter(sc);
+			IF_ATSAM_UNLOCK(sc);
+		}
 		break;
 	default:
 		rv = ether_ioctl(ifp, command, data);
@@ -1416,7 +1536,6 @@ static int if_atsam_driver_attach(device_t dev)
 	if_atsam_softc *sc;
 	struct ifnet *ifp;
 	int unit;
-	char *unitName;
 	uint8_t eaddr[ETHER_ADDR_LEN];
 
 	sc = device_get_softc(dev);
@@ -1437,13 +1556,6 @@ static int if_atsam_driver_attach(device_t dev)
 
 	memcpy(sc->GMacAddress, eaddr, ETHER_ADDR_LEN);
 
-	sc->amount_rx_buf = RXBUF_COUNT;
-	sc->amount_tx_buf = TXBUF_COUNT;
-
-	sc->tx_ring.tx_bd_used = 0;
-	sc->tx_ring.tx_bd_free = 0;
-	sc->tx_ring.length = sc->amount_tx_buf;
-
 	/* Set Initial Link Speed */
 	sc->link_speed = GMAC_SPEED_100M;
 	sc->link_duplex = GMAC_DUPLEX_FULL;
@@ -1486,17 +1598,20 @@ static int if_atsam_driver_attach(device_t dev)
 	 */
 	ifp->if_softc = sc;
 	if_initname(ifp, device_get_name(dev), device_get_unit(dev));
-	ifp->if_init = if_atsam_init;
+	ifp->if_init = if_atsam_start;
 	ifp->if_ioctl = if_atsam_ioctl;
-	ifp->if_start = if_atsam_enet_start;
-	ifp->if_flags = IFF_BROADCAST | IFF_SIMPLEX;
+	ifp->if_transmit = if_atsam_transmit;
+	ifp->if_qflush = if_qflush;
+	ifp->if_flags = IFF_BROADCAST | IFF_MULTICAST | IFF_SIMPLEX;
 	ifp->if_capabilities |= IFCAP_HWCSUM | IFCAP_HWCSUM_IPV6 |
-	    IFCAP_VLAN_HWCSUM;
+	    IFCAP_VLAN_HWCSUM | IFCAP_VLAN_HWTAGGING;
+	ifp->if_capenable = ifp->if_capabilities;
 	ifp->if_hwassist = CSUM_IP | CSUM_IP_UDP | CSUM_IP_TCP |
 	    CSUM_IP6_UDP | CSUM_IP6_TCP;
-	IFQ_SET_MAXLEN(&ifp->if_snd, TXBUF_COUNT - 1);
-	ifp->if_snd.ifq_drv_maxlen = TXBUF_COUNT - 1;
+	IFQ_SET_MAXLEN(&ifp->if_snd, TX_DESC_COUNT - 1);
+	ifp->if_snd.ifq_drv_maxlen = TX_DESC_COUNT - 1;
 	IFQ_SET_READY(&ifp->if_snd);
+	ifp->if_hdrlen = sizeof(struct ether_vlan_header);
 
 	/*
 	 * Attach the interface
@@ -1504,6 +1619,7 @@ static int if_atsam_driver_attach(device_t dev)
 	ether_ifattach(ifp, eaddr);
 
 	if_atsam_add_sysctls(dev);
+	if_atsam_init(sc);
 
 	return (0);
 }