if_stmac: Add driver for STM32H7 ethernet module

Update #3910.

3 files changed, 1014 insertions, 0 deletions

diff --git a/libbsd.py b/libbsd.py
index a48616b4..1105328d 100644
--- a/libbsd.py
+++ b/libbsd.py
@@ -227,6 +227,7 @@ class rtems(builder.Module):
                 'sys/dev/ffec/if_ffec_mpc8xx.c',
                 'sys/dev/input/touchscreen/tsc_lpc32xx.c',
                 'sys/dev/smc/if_smc_nexus.c',
+                'sys/dev/stmac/if_stmac.c',
                 'sys/dev/tsec/if_tsec_nexus.c',
                 'sys/dev/usb/controller/ehci_mpc83xx.c',
                 'sys/dev/usb/controller/ohci_lpc32xx.c',
diff --git a/rtemsbsd/include/bsp/nexus-devices.h b/rtemsbsd/include/bsp/nexus-devices.h
index ad7ac146..9e4037ef 100644
--- a/rtemsbsd/include/bsp/nexus-devices.h
+++ b/rtemsbsd/include/bsp/nexus-devices.h
@@ -159,6 +159,11 @@ SYSINIT_DRIVER_REFERENCE(usbus, ohci);
 RTEMS_BSD_DRIVER_USB;
 RTEMS_BSD_DRIVER_USB_MASS;
 
+#elif defined(LIBBSP_ARM_STM32H7_BSP_H)
+
+RTEMS_BSD_DEFINE_NEXUS_DEVICE(stmac, 0, 0, NULL);
+SYSINIT_DRIVER_REFERENCE(ukphy, miibus);
+
 #elif defined(LIBBSP_I386_PC386_BSP_H)
 
 RTEMS_BSD_DRIVER_PC_LEGACY;
diff --git a/rtemsbsd/sys/dev/stmac/if_stmac.c b/rtemsbsd/sys/dev/stmac/if_stmac.c
new file mode 100644
index 00000000..614c51b9
--- /dev/null
+++ b/rtemsbsd/sys/dev/stmac/if_stmac.c
@@ -0,0 +1,1008 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+
+/*
+ * Copyright (C) 2020 embedded brains Gmb_h (http://www.embedded-brains.de)
+ *
+ * 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 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 OWNER 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.
+ */
+
+#include <machine/rtems-bsd-kernel-space.h>
+
+#include <bsp.h>
+
+#ifdef LIBBSP_ARM_STM32H7_BSP_H
+
+#include <sys/param.h>
+#include <sys/types.h>
+#include <sys/bus.h>
+#include <sys/mbuf.h>
+#include <sys/malloc.h>
+#include <sys/kernel.h>
+#include <sys/module.h>
+#include <sys/socket.h>
+#include <sys/sockio.h>
+
+#include <net/if.h>
+#include <net/ethernet.h>
+#include <net/if_arp.h>
+#include <net/if_dl.h>
+#include <net/if_media.h>
+#include <net/if_types.h>
+#include <net/if_var.h>
+
+#include <machine/bus.h>
+
+#include <dev/mii/mii.h>
+#include <dev/mii/miivar.h>
+
+#include <rtems/bsd/local/miibus_if.h>
+
+#include <stm32h7xx_hal.h>
+
+#include <rtems/bsd/bsd.h>
+#include <rtems/irq-extension.h>
+#include <rtems/score/armv7m.h>
+
+#define	RX_DESC_COUNT		64
+#define	TX_DESC_COUNT		256
+
+#define	STMAC_LOCK(sc) mtx_lock(&(sc)->mtx)
+#define	STMAC_UNLOCK(sc) mtx_unlock(&(sc)->mtx)
+
+#define	STMAC_TXLOCK(sc) mtx_lock(&(sc)->tx_mtx)
+#define	STMAC_TXUNLOCK(sc) mtx_unlock(&(sc)->tx_mtx)
+
+struct stmac_desc {
+	__IO uint32_t	DESC0;
+	__IO uint32_t	DESC1;
+	__IO uint32_t	DESC2;
+	__IO uint32_t	DESC3;
+	struct mbuf	*m;
+};
+
+struct stmac_softc {
+	ETH_HandleTypeDef	heth;
+	uint8_t			mac_addr[6];
+	struct ifnet		*ifp;
+	struct mtx		mtx;
+	device_t		miibus;
+	struct mtx		mii_mtx;
+	struct mii_data		*mii_softc;
+	u_int			mii_media_active;
+	u_int			mii_media_status;
+	struct callout		tick_callout;
+	int			if_flags;
+
+	/* RX */
+	struct stmac_desc	*rx_desc_ring;
+	uint32_t		rx_idx;
+	bool			rx_do_alloc;
+
+	/* TX */
+	struct mtx		tx_mtx;
+	struct stmac_desc	*tx_desc_ring;
+	uint32_t		tx_idx_head;
+	uint32_t		tx_idx_tail;
+};
+
+static void stmac_init_locked(struct stmac_softc *);
+static void stmac_stop_locked(struct stmac_softc *);
+
+struct mbuf *
+stmac_new_mbuf(struct ifnet *ifp)
+{
+	struct mbuf *m;
+
+	m = m_getcl(M_NOWAIT, MT_DATA, M_PKTHDR);
+	if (m != NULL) {
+		m->m_data = mtod(m, char *) + ETHER_ALIGN;
+		m->m_pkthdr.rcvif = ifp;
+		rtems_cache_invalidate_multiple_data_lines(m->m_data, m->m_len);
+	}
+
+	return m;
+}
+
+static void
+stmac_tick(void *arg)
+{
+	struct stmac_softc *sc;
+	struct ifnet *ifp;
+
+	sc = arg;
+	ifp = sc->ifp;
+
+	if ((ifp->if_drv_flags & IFF_DRV_RUNNING) == 0) {
+		return;
+	}
+
+	mii_tick(sc->mii_softc);
+	callout_reset(&sc->tick_callout, hz, stmac_tick, sc);
+}
+
+static uint32_t
+stmac_rx_desc3(uint32_t idx)
+{
+
+	if (idx % 16 != 0) {
+		return (ETH_DMARXNDESCRF_OWN | ETH_DMARXNDESCRF_BUF1V);
+	}
+
+	return (ETH_DMARXNDESCRF_OWN | ETH_DMARXNDESCRF_BUF1V |
+	    ETH_DMARXNDESCRF_IOC);
+}
+
+static void
+stmac_rx_setup_desc(struct stmac_softc *sc)
+{
+	uint32_t idx;
+	bool do_alloc;
+	ETH_TypeDef *regs;
+
+	do_alloc = sc->rx_do_alloc;
+	sc->rx_do_alloc = false;
+	sc->rx_idx = 0;
+
+	for (idx = 0; idx < RX_DESC_COUNT; ++idx) {
+		struct stmac_desc *desc;
+		struct mbuf *m;
+
+		desc = &sc->rx_desc_ring[idx];
+
+		if (do_alloc) {
+			m = stmac_new_mbuf(sc->ifp);
+			BSD_ASSERT(m != NULL);
+			desc->m = m;
+		} else {
+			m = desc->m;
+		}
+
+		WRITE_REG(desc->DESC0, mtod(m, uint32_t));
+		WRITE_REG(desc->DESC1, 0x0);
+		WRITE_REG(desc->DESC2, 0x0);
+		WRITE_REG(desc->DESC3, stmac_rx_desc3(idx));
+	}
+
+	regs = sc->heth.Instance;
+	WRITE_REG(regs->DMACRDRLR, RX_DESC_COUNT -1);
+	WRITE_REG(regs->DMACRDLAR, (uint32_t)&sc->rx_desc_ring[0]);
+	WRITE_REG(regs->DMACRDTPR,
+	    (uint32_t)&sc->rx_desc_ring[RX_DESC_COUNT - 1]);
+}
+
+static void
+stmac_tx_setup_desc(struct stmac_softc *sc)
+{
+	uint32_t idx;
+	ETH_TypeDef *regs;
+
+	sc->tx_idx_head = 0;
+	sc->tx_idx_tail = 0;
+
+	for (idx = 0; idx < TX_DESC_COUNT; ++idx) {
+		struct stmac_desc *desc;
+
+		desc = &sc->tx_desc_ring[idx];
+		WRITE_REG(desc->DESC0, 0x0);
+		WRITE_REG(desc->DESC1, 0x0);
+		WRITE_REG(desc->DESC2, 0x0);
+		WRITE_REG(desc->DESC3, 0x0);
+		desc->m = NULL;
+	}
+
+	regs = sc->heth.Instance;
+	WRITE_REG(regs->DMACTDRLR, TX_DESC_COUNT -1);
+	WRITE_REG(regs->DMACTDLAR, (uint32_t)&sc->tx_desc_ring[0]);
+	WRITE_REG(regs->DMACTDTPR, (uint32_t)&sc->tx_desc_ring[0]);
+}
+
+static void
+stmac_init_locked(struct stmac_softc *sc)
+{
+	struct ifnet *ifp;
+	ETH_TypeDef *regs;
+
+	ifp = sc->ifp;
+
+	if ((ifp->if_drv_flags & IFF_DRV_RUNNING) != 0) {
+		return;
+	}
+
+	ifp->if_drv_flags |= IFF_DRV_RUNNING;
+
+	stmac_rx_setup_desc(sc);
+	stmac_tx_setup_desc(sc);
+
+	HAL_ETH_Start(&sc->heth);
+	regs = sc->heth.Instance;
+
+	/* Enable interrupts */
+	SET_BIT(regs->DMACIER, ETH_DMACIER_NIE | ETH_DMACIER_RIE |
+	    ETH_DMACIER_FBEE | ETH_DMACIER_AIE);
+
+	mii_mediachg(sc->mii_softc);
+	callout_reset(&sc->tick_callout, hz, stmac_tick, sc);
+}
+
+static void
+stmac_tx_reclaim_all(struct stmac_softc *sc)
+{
+	uint32_t idx;
+
+	for (idx = 0; idx < TX_DESC_COUNT; ++idx) {
+		struct stmac_desc *desc;
+		struct mbuf *m;
+
+		desc = &sc->tx_desc_ring[idx];
+		m = desc->m;
+		desc->m = NULL;
+		m_freem(m);
+	}
+}
+
+static void
+stmac_stop_locked(struct stmac_softc *sc)
+{
+	struct ifnet *ifp;
+	ETH_TypeDef *regs;
+	rtems_interrupt_server_entry server_entry;
+	rtems_status_code status;
+	rtems_name name;
+	uint64_t t0;
+
+	ifp = sc->ifp;
+
+	if ((ifp->if_drv_flags & IFF_DRV_RUNNING) == 0) {
+		return;
+	}
+
+	ifp->if_drv_flags &= ~IFF_DRV_RUNNING;
+	regs = sc->heth.Instance;
+
+	/* Disable interrupts */
+	WRITE_REG(regs->DMACIER, 0);
+
+	/* Disable the DMA transmission and reception */
+	CLEAR_BIT(regs->DMACTCR, ETH_DMACTCR_ST);
+	CLEAR_BIT(regs->DMACRCR, ETH_DMACRCR_SR);
+
+	/* Wait for DMA done */
+	t0 = rtems_clock_get_uptime_nanoseconds();
+	while ((regs->MTLTQDR & (ETH_MTLTQDR_TXQSTS |
+	    ETH_MTLTQDR_TRCSTS)) != 0 ||
+	    (regs->MTLRQDR & (ETH_MTLRQDR_RXQSTS |
+	    ETH_MTLRQDR_PRXQ)) != 0) {
+		if (rtems_clock_get_uptime_nanoseconds() - t0 > 500000) {
+			break;
+		}
+	}
+
+	/* Disable the MAC reception and transmission */
+	CLEAR_BIT(regs->MACCR, ETH_MACCR_RE | ETH_MACCR_TE);
+
+	/* Make sure no receive interrupt is in progress */
+	status = rtems_object_get_classic_name(rtems_task_self(), &name);
+	BSD_ASSERT(status == RTEMS_SUCCESSFUL);
+	if (name != rtems_build_name('I', 'R', 'Q', 'S')) {
+		status = rtems_interrupt_server_entry_initialize(
+		    RTEMS_INTERRUPT_SERVER_DEFAULT, &server_entry);
+		BSD_ASSERT(status == RTEMS_SUCCESSFUL);
+		rtems_interrupt_server_entry_destroy(&server_entry);
+	}
+
+	stmac_tx_reclaim_all(sc);
+}
+
+static uint8_t
+stmac_bitreverse(uint8_t x)
+{
+	static const uint8_t nibbletab[] = {
+	    0, 8, 4, 12, 2, 10, 6, 14, 1, 9, 5, 13, 3, 11, 7, 15
+	};
+
+	return ((nibbletab[x & 15] << 4) | nibbletab[x >> 4]);
+}
+
+static uint64_t
+stmac_hash_bit(const uint8_t *eaddr)
+{
+	uint32_t crc;
+
+	/*
+	 * Thanks to the excellent documentation from STM it was easy to figure
+	 * this out.
+	 */
+	crc = 0;
+	crc = crc32_raw(eaddr, ETHER_ADDR_LEN, ~crc);
+	crc = stmac_bitreverse((uint8_t)~crc);
+	return ((uint64_t)1 << (crc >> 2));
+}
+
+static void
+stmac_rx_setup_filter(struct stmac_softc *sc)
+{
+	struct ifmultiaddr *ifma;
+	struct ifnet *ifp;
+	ETH_TypeDef *regs;
+	uint32_t macpfr;
+	uint64_t machtr;
+	const uint8_t *eaddr;
+
+	ifp = sc->ifp;
+
+	macpfr = ETH_MACPFR_HMC;
+	if ((ifp->if_flags & IFF_PROMISC) != 0) {
+		macpfr |= ETH_MACPFR_PR;
+	}
+
+	machtr = 0;
+	if ((ifp->if_flags & IFF_ALLMULTI) != 0) {
+		machtr = ~machtr;
+	} else {
+		if_maddr_rlock(ifp);
+		CK_STAILQ_FOREACH(ifma, &sc->ifp->if_multiaddrs, ifma_link) {
+			if (ifma->ifma_addr->sa_family != AF_LINK) {
+				continue;
+			}
+
+			eaddr = LLADDR((struct sockaddr_dl *)ifma->ifma_addr);
+			machtr |= stmac_hash_bit(eaddr);
+		}
+		if_maddr_runlock(ifp);
+	}
+
+	regs = sc->heth.Instance;
+	eaddr = IF_LLADDR(ifp);
+	regs->MACA0LR = eaddr[0] | (eaddr[1] << 8) | (eaddr[2] << 16) |
+	    (eaddr[3] << 24);
+	regs->MACA0HR = eaddr[4] | (eaddr[5] << 8);
+	regs->MACHT0R = (uint32_t)machtr;
+	regs->MACHT1R = (uint32_t)(machtr >> 32);
+	regs->MACPFR = macpfr;
+}
+
+static int
+stmac_ioctl(struct ifnet *ifp, ioctl_command_t cmd, caddr_t data)
+{
+	struct stmac_softc *sc;
+	int error;
+	struct mii_data *mii;
+
+	sc = ifp->if_softc;
+
+	error = 0;
+	switch (cmd) {
+	case SIOCSIFFLAGS:
+		STMAC_LOCK(sc);
+		if ((ifp->if_flags & IFF_UP) != 0) {
+			if ((ifp->if_drv_flags & IFF_DRV_RUNNING) != 0) {
+				if ((ifp->if_flags ^ sc->if_flags) &
+				    (IFF_PROMISC | IFF_ALLMULTI))
+					stmac_rx_setup_filter(sc);
+			} else {
+				stmac_init_locked(sc);
+			}
+		} else {
+			if ((ifp->if_drv_flags & IFF_DRV_RUNNING) != 0) {
+				STMAC_TXLOCK(sc);
+				stmac_stop_locked(sc);
+				STMAC_TXUNLOCK(sc);
+			}
+		}
+		sc->if_flags = ifp->if_flags;
+		STMAC_UNLOCK(sc);
+		break;
+	case SIOCADDMULTI:
+	case SIOCDELMULTI:
+		STMAC_LOCK(sc);
+		if ((ifp->if_drv_flags & IFF_DRV_RUNNING) != 0) {
+			stmac_rx_setup_filter(sc);
+		}
+		STMAC_UNLOCK(sc);
+		break;
+	case SIOCSIFMEDIA:
+	case SIOCGIFMEDIA:
+		mii = sc->mii_softc;
+		if (mii != NULL) {
+			error = ifmedia_ioctl(ifp, (struct ifreq *)data, &mii->mii_media, cmd);
+		} else {
+			error = ether_ioctl(ifp, cmd, data);
+		}
+		break;
+	default:
+		error = ether_ioctl(ifp, cmd, data);
+		break;
+	}
+
+	return (error);
+}
+
+static void
+stmac_init(void *arg)
+{
+	struct stmac_softc *sc;
+
+	sc = arg;
+	STMAC_LOCK(sc);
+	STMAC_TXLOCK(sc);
+	stmac_init_locked(sc);
+	STMAC_TXUNLOCK(sc);
+	STMAC_UNLOCK(sc);
+}
+
+static void
+stmac_media_status(struct ifnet *ifp, struct ifmediareq *ifmr_p)
+{
+	struct stmac_softc *sc;
+	struct mii_data *mii;
+
+	sc = ifp->if_softc;
+
+	STMAC_LOCK(sc);
+	mii = sc->mii_softc;
+	if (mii != NULL) {
+		mii_pollstat(mii);
+		ifmr_p->ifm_active = mii->mii_media_active;
+		ifmr_p->ifm_status = mii->mii_media_status;
+	}
+	STMAC_UNLOCK(sc);
+}
+
+static int
+stmac_media_change(struct ifnet *ifp)
+{
+	struct stmac_softc *sc;
+	int error;
+
+	sc = ifp->if_softc;
+
+	STMAC_LOCK(sc);
+	if (sc->mii_softc != NULL) {
+		error = mii_mediachg(sc->mii_softc);
+	} else {
+		error = ENXIO;
+	}
+	STMAC_UNLOCK(sc);
+	return (error);
+}
+
+static int
+stmac_probe(device_t dev)
+{
+
+	device_set_desc(dev, "STM MAC");
+	return (BUS_PROBE_DEFAULT);
+}
+
+static void
+stmac_rx_checksum(struct mbuf *m, uint32_t desc1)
+{
+
+	if ((desc1 & (ETH_DMARXNDESCWBF_IPCE | ETH_DMARXNDESCWBF_IPCB |
+	    ETH_DMARXNDESCWBF_IPHE)) == 0) {
+		uint32_t pt;
+
+		pt = desc1 & ETH_DMARXNDESCWBF_PT;
+		if (pt == ETH_DMARXNDESCWBF_PT_UDP ||
+		    pt == ETH_DMARXNDESCWBF_PT_TCP) {
+			m->m_pkthdr.csum_flags |=
+			    CSUM_IP_CHECKED |
+			    CSUM_IP_VALID |
+			    CSUM_DATA_VALID |
+			    CSUM_PSEUDO_HDR;
+			m->m_pkthdr.csum_data = 0xffff;
+		} else if ((desc1 & ETH_DMARXNDESCWBF_IPV4) != 0) {
+			m->m_pkthdr.csum_flags |=
+			    CSUM_IP_CHECKED |
+			    CSUM_IP_VALID;
+			m->m_pkthdr.csum_data = 0xffff;
+		}
+	}
+}
+
+static void
+stmac_rx_interrupt(struct stmac_softc *sc, ETH_TypeDef *regs)
+{
+	struct ifnet *ifp;
+	uint32_t idx;
+	struct stmac_desc *desc_ring;
+	struct stmac_desc *desc;
+
+	ifp = sc->ifp;
+	desc_ring = sc->rx_desc_ring;
+	idx = sc->rx_idx;
+	desc = &desc_ring[idx];
+
+	while (true) {
+		uint32_t desc3;
+		struct mbuf *m;
+
+		/*
+		 * Make sure the interrupt is cleared and no longer pending
+		 * before we read the descriptor status.
+		 */
+		regs->DMACSR = ETH_DMACSR_RI | ETH_DMACSR_NIS;
+		regs->DMACSR;
+		_ARMV7M_NVIC_Clear_pending(ETH_IRQn);
+
+		desc3 = desc->DESC3;
+		if ((desc3 & ETH_DMARXNDESCRF_OWN) != 0) {
+			break;
+		}
+
+		m = stmac_new_mbuf(ifp);
+		if (m != NULL) {
+			uint32_t mask;
+			uint32_t set;
+			uint32_t len;
+
+			mask = ETH_DMARXNDESCWBF_CTXT | ETH_DMARXNDESCWBF_FD |
+			    ETH_DMARXNDESCWBF_LD | ETH_DMARXNDESCWBF_ES;
+			set = ETH_DMARXNDESCWBF_FD | ETH_DMARXNDESCWBF_LD;
+			len = desc3 & ETH_DMARXNDESCWBF_PL;
+			if ((desc3 & mask) == set && len > ETHER_CRC_LEN) {
+				struct mbuf *rx;
+
+				rx = desc->m;
+				stmac_rx_checksum(rx, desc->DESC1);
+				rx->m_len = len;
+				rx->m_pkthdr.len = len;
+				(*ifp->if_input)(ifp, rx);
+			} else {
+				if_inc_counter(ifp, IFCOUNTER_IERRORS, 1);
+				m_freem(m);
+				m = desc->m;
+			}
+		} else {
+			if_inc_counter(ifp, IFCOUNTER_IQDROPS, 1);
+			m = desc->m;
+		}
+
+		WRITE_REG(desc->DESC0, mtod(m, uint32_t));
+		WRITE_REG(desc->DESC1, 0x0);
+		WRITE_REG(desc->DESC2, 0x0);
+		WRITE_REG(desc->DESC3, stmac_rx_desc3(idx));
+		desc->m = m;
+
+		_ARM_Data_synchronization_barrier();
+		WRITE_REG(regs->DMACRDTPR, (uint32_t)desc);
+
+		idx = (idx + 1) % RX_DESC_COUNT;
+		desc = &desc_ring[idx];
+	}
+
+	sc->rx_idx = idx;
+}
+
+static void
+stmac_reset(struct stmac_softc *sc)
+{
+	HAL_StatusTypeDef hal_status;
+	ETH_TypeDef *regs;
+
+	hal_status = HAL_ETH_Init(&sc->heth);
+	BSD_ASSERT(hal_status == HAL_OK);
+
+	regs = sc->heth.Instance;
+
+	/* Set descriptor skip length according to struct stmac_desc */
+	MODIFY_REG(regs->DMACCR, ETH_DMACCR_DSL, ETH_DMACCR_DSL_32BIT);
+
+	/*
+	 * FIXME: Just use a random value.  It is not clear which clock is used
+	 * here.
+	 */
+	regs->DMACRIWTR = 0x80;
+}
+
+static void
+stmac_interrupt(void *arg)
+{
+	struct stmac_softc *sc;
+	ETH_TypeDef *regs;
+	uint32_t dmacsr;
+
+	sc = arg;
+	regs = sc->heth.Instance;
+	dmacsr = regs->DMACSR;
+
+	/* This is almost always a receive interrupt */
+	stmac_rx_interrupt(sc, regs);
+
+	if (__predict_false((dmacsr & ETH_DMACSR_FBE) != 0)) {
+		regs->DMACSR = ETH_DMACSR_FBE | ETH_DMACSR_AIS;
+
+		STMAC_LOCK(sc);
+		STMAC_TXLOCK(sc);
+		stmac_stop_locked(sc);
+		stmac_reset(sc);
+		stmac_init_locked(sc);
+		STMAC_TXUNLOCK(sc);
+		STMAC_UNLOCK(sc);
+	}
+}
+
+static void
+stmac_tx_reclaim(struct stmac_softc *sc, struct ifnet *ifp)
+{
+	uint32_t head_idx;
+	uint32_t tail_idx;
+	struct stmac_desc *desc_ring;
+
+	head_idx = sc->tx_idx_head;
+	tail_idx = sc->tx_idx_tail;
+	desc_ring = sc->tx_desc_ring;
+
+	while (head_idx != tail_idx) {
+		struct stmac_desc *tail_desc;
+		uint32_t desc3;
+
+		tail_desc = &desc_ring[tail_idx];
+
+		desc3 = tail_desc->DESC3;
+		if ((desc3 & ETH_DMATXNDESCWBF_OWN) != 0) {
+			break;
+		}
+
+		if ((desc3 & ETH_DMATXNDESCWBF_LD) != 0) {
+			struct mbuf *m;
+			ift_counter cnt;
+
+			if ((desc3 & ETH_DMATXNDESCWBF_ES) == 0) {
+				cnt = IFCOUNTER_OPACKETS;
+			} else {
+				cnt = IFCOUNTER_OERRORS;
+			}
+
+			if_inc_counter(ifp, cnt, 1);
+
+			m = tail_desc->m;
+			tail_desc->m = NULL;
+			m_freem(m);
+		}
+
+		tail_idx = (tail_idx + 1) % TX_DESC_COUNT;
+	}
+
+	sc->tx_idx_tail = tail_idx;
+}
+
+static void
+stmac_cache_flush(uintptr_t begin, uintptr_t size)
+{
+	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);
+}
+
+static int
+stmac_tx_enqueue(struct stmac_softc *sc, struct ifnet *ifp, struct mbuf *m)
+{
+	uint32_t head_idx;
+	uint32_t tail_idx;
+	uint32_t capacity;
+	uint32_t new_head_idx;
+	uint32_t idx;
+	struct stmac_desc *desc_ring;
+	struct stmac_desc *desc;
+	ETH_TypeDef *regs;
+	uint32_t bufs;
+	uint32_t desc2;
+	uint32_t desc3;
+	struct mbuf *n;
+	int csum_flags;
+
+	head_idx = sc->tx_idx_head;
+	tail_idx = sc->tx_idx_tail;
+	capacity = 2 * ((tail_idx - head_idx - 1) % TX_DESC_COUNT);
+
+	idx = head_idx;
+	desc_ring = sc->tx_desc_ring;
+	desc2 = 0;
+	bufs = 0;
+	n = m;
+
+	do {
+		uintptr_t size;
+
+		desc = &desc_ring[idx];
+
+		size = (uintptr_t)n->m_len;
+		if (__predict_true(size > 0)) {
+			uintptr_t begin;
+
+			++bufs;
+			if (__predict_false(bufs > capacity)) {
+				return (ENOBUFS);
+			}
+
+			begin = mtod(n, uintptr_t);
+
+			if (bufs % 2 == 1) {
+				desc->DESC0 = begin;
+				desc2 = size;
+			} else {
+				desc->DESC1 = begin;
+				desc->DESC2 = (size << 16) | desc2;
+				idx = (idx + 1) % TX_DESC_COUNT;
+			}
+
+			stmac_cache_flush(begin, size);
+		}
+
+		n = n->m_next;
+	} while (n != NULL);
+
+	if (bufs % 2 == 1) {
+		desc->DESC1 = 0;
+		desc->DESC2 = desc2;
+		idx = (idx + 1) % TX_DESC_COUNT;
+	}
+
+	new_head_idx = idx;
+	sc->tx_idx_head = new_head_idx;
+
+	idx = (idx - 1) % TX_DESC_COUNT;
+	desc = &desc_ring[idx];
+	desc->m = m;
+
+	desc3 = ETH_DMATXNDESCRF_OWN | ETH_DMATXNDESCRF_LD | m->m_pkthdr.len;
+	csum_flags = m->m_pkthdr.csum_flags;
+	if ((csum_flags & (CSUM_TCP | CSUM_UDP | CSUM_TCP_IPV6 |
+	    CSUM_UDP_IPV6)) != 0) {
+		desc3 |= ETH_DMATXNDESCRF_CIC_IPHDR_PAYLOAD_INSERT_PHDR_CALC;
+	} else if ((csum_flags & CSUM_IP) != 0) {
+		desc3 |= ETH_DMATXNDESCRF_CIC_IPHDR_INSERT;
+	}
+
+	while (idx != head_idx) {
+		desc->DESC3 = desc3;
+		desc3 &= ~ETH_DMATXNDESCRF_LD;
+
+		idx = (idx - 1) % TX_DESC_COUNT;
+		desc = &desc_ring[idx];
+	}
+
+	desc->DESC3 = ETH_DMATXNDESCRF_FD | desc3;
+	_ARM_Data_synchronization_barrier();
+	regs = sc->heth.Instance;
+	WRITE_REG(regs->DMACTDTPR, (uint32_t)&desc_ring[new_head_idx]);
+	return (0);
+}
+
+static int
+stmac_transmit(struct ifnet *ifp, struct mbuf *m)
+{
+	struct stmac_softc *sc;
+	int error;
+
+	sc = ifp->if_softc;
+	STMAC_TXLOCK(sc);
+
+	error = stmac_tx_enqueue(sc, ifp, m);
+	stmac_tx_reclaim(sc, ifp);
+
+	if (__predict_false(error != 0)) {
+		error = stmac_tx_enqueue(sc, ifp, m);
+		if (error != 0) {
+			m_freem(m);
+			if_inc_counter(ifp, IFCOUNTER_OQDROPS, 1);
+		}
+	}
+
+	STMAC_TXUNLOCK(sc);
+	return (error);
+}
+
+static int
+stmac_transmit_no_link(struct ifnet *ifp, struct mbuf *m)
+{
+
+	(void)ifp;
+	(void)m;
+	return (ENETDOWN);
+}
+
+static void
+stmac_qflush(struct ifnet *ifp)
+{
+
+	(void)ifp;
+}
+
+static int
+stmac_attach(device_t dev)
+{
+	struct stmac_softc *sc;
+	struct ifnet *ifp;
+	int error;
+	struct stmac_desc *descs;
+	ETH_TypeDef *regs;
+	rtems_status_code status;
+
+	sc = device_get_softc(dev);
+
+	sc->rx_do_alloc = true;
+	mtx_init(&sc->mtx, device_get_nameunit(dev), MTX_NETWORK_LOCK, MTX_DEF);
+	mtx_init(&sc->tx_mtx, "stmac tx", MTX_NETWORK_LOCK, MTX_DEF);
+	callout_init_mtx(&sc->tick_callout, &sc->mtx, 0);
+
+	sc->ifp = ifp = if_alloc(IFT_ETHER);
+	ifp->if_softc = sc;
+
+	regs = ETH;
+	sc->heth.Instance = regs;
+
+	descs = rtems_cache_coherent_allocate(sizeof(*descs) *
+	    (RX_DESC_COUNT + TX_DESC_COUNT), 4, 0);
+	BSD_ASSERT(descs != NULL);
+
+	sc->tx_desc_ring = &descs[0];
+	sc->rx_desc_ring = &descs[TX_DESC_COUNT];
+
+	rtems_bsd_get_mac_address(device_get_name(dev), device_get_unit(dev),
+	    &sc->mac_addr[0]);
+
+	sc->heth.Init.MACAddr = &sc->mac_addr[0];
+	sc->heth.Init.MediaInterface = HAL_ETH_RMII_MODE;
+	sc->heth.Init.RxBuffLen = 1524;
+
+	stmac_reset(sc);
+
+	if_initname(ifp, "stm", device_get_unit(dev));
+	ifp->if_flags = IFF_SIMPLEX | IFF_MULTICAST | IFF_BROADCAST;
+	ifp->if_capabilities |= IFCAP_HWCSUM | IFCAP_HWCSUM_IPV6;
+	ifp->if_capenable = ifp->if_capabilities;
+	ifp->if_hwassist = CSUM_IP | CSUM_TCP | CSUM_UDP | CSUM_TCP_IPV6 | CSUM_UDP_IPV6;
+	ifp->if_transmit = stmac_transmit_no_link;
+	ifp->if_qflush = stmac_qflush;
+	ifp->if_ioctl = stmac_ioctl;
+	ifp->if_init = stmac_init;
+	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);
+
+	error = mii_attach(dev, &sc->miibus, ifp, stmac_media_change,
+	    stmac_media_status, BMSR_DEFCAPMASK, MII_PHY_ANY,
+	    MII_OFFSET_ANY, 0);
+	if (error == 0) {
+		sc->mii_softc = device_get_softc(sc->miibus);
+	}
+
+	ether_ifattach(ifp, &sc->heth.Init.MACAddr[0]);
+
+	status = rtems_interrupt_server_handler_install(
+	    RTEMS_INTERRUPT_SERVER_DEFAULT, ETH_IRQn, "stmac",
+	    RTEMS_INTERRUPT_SHARED, stmac_interrupt, sc);
+	BSD_ASSERT(status == RTEMS_SUCCESSFUL);
+	return (0);
+}
+
+static int
+stmac_miibus_read(device_t dev, int phy, int reg)
+{
+	struct stmac_softc *sc;
+	uint32_t val;
+	HAL_StatusTypeDef hal_status;
+
+	sc = device_get_softc(dev);
+	hal_status = HAL_ETH_ReadPHYRegister(&sc->heth, (uint32_t)phy,
+	    (uint32_t)reg,  &val);
+	if (hal_status != HAL_OK) {
+		return (-1);
+	}
+
+	return ((int)val);
+}
+
+static int
+stmac_miibus_write(device_t dev, int phy, int reg, int val)
+{
+	struct stmac_softc *sc;
+	HAL_StatusTypeDef hal_status;
+
+	sc = device_get_softc(dev);
+	hal_status = HAL_ETH_WritePHYRegister(&sc->heth, (uint32_t)phy,
+	    (uint32_t)reg, (uint32_t)val);
+	if (hal_status != HAL_OK) {
+		return (-1);
+	}
+
+	return (0);
+}
+
+static void
+stmac_miibus_statchg(device_t dev)
+{
+	struct stmac_softc *sc;
+	struct mii_data *mii;
+	u_int active;
+	u_int status;
+	ETH_TypeDef *regs;
+	uint32_t maccr;
+
+	sc = device_get_softc(dev);
+	mii = device_get_softc(sc->miibus);
+	active = mii->mii_media_active;
+	status = mii->mii_media_status;
+
+	if (sc->mii_media_active == active && sc->mii_media_status == status) {
+		return;
+	}
+
+	sc->mii_media_active = active;
+	sc->mii_media_status = status;
+
+	regs = sc->heth.Instance;
+	maccr = regs->MACCR;
+
+	if ((status & IFM_ACTIVE) != 0) {
+		if_settransmitfn(sc->ifp, stmac_transmit);
+
+		if ((IFM_OPTIONS(active) & IFM_FDX) != 0) {
+			maccr |= ETH_MACCR_DM;
+		} else {
+			maccr &= ~ETH_MACCR_DM;
+		}
+
+		if (IFM_SUBTYPE(active) != IFM_10_T) {
+			maccr |= ETH_MACCR_FES;
+		} else {
+			maccr &= ~ETH_MACCR_FES;
+		}
+	} else {
+		maccr |= ETH_MACCR_DM | ETH_MACCR_FES;
+	}
+
+	regs->MACCR = maccr;
+}
+
+static device_method_t stmac_methods[] = {
+	DEVMETHOD(device_probe, stmac_probe),
+	DEVMETHOD(device_attach, stmac_attach),
+	DEVMETHOD(miibus_readreg, stmac_miibus_read),
+	DEVMETHOD(miibus_writereg, stmac_miibus_write),
+	DEVMETHOD(miibus_statchg, stmac_miibus_statchg),
+	DEVMETHOD_END
+};
+
+driver_t stmac_driver = {
+	"stmac",
+	stmac_methods,
+	sizeof(struct stmac_softc)
+};
+
+static devclass_t stmac_devclass;
+
+DRIVER_MODULE(stmac, nexus, stmac_driver, stmac_devclass, 0, 0);
+DRIVER_MODULE(miibus, stmac, miibus_driver, miibus_devclass, 0, 0);
+
+MODULE_DEPEND(stmac, ether, 1, 1, 1);
+MODULE_DEPEND(stmac, miibus, 1, 1, 1);
+
+#endif /* LIBBSP_ARM_STM32H7_BSP_H */