Index: sys/arm64/conf/QUARTZ64 =================================================================== --- sys/arm64/conf/QUARTZ64 +++ sys/arm64/conf/QUARTZ64 @@ -74,6 +74,7 @@ device miibus device iflib device em # Intel Gig ethernet +device eqos # DesignWare Ethernet Quality Of Service # NVM Express (NVMe) support device nvme # base NVMe driver Index: sys/conf/files =================================================================== --- sys/conf/files +++ sys/conf/files @@ -2430,6 +2430,7 @@ dev/mii/ip1000phy.c optional miibus | ip1000phy dev/mii/jmphy.c optional miibus | jmphy dev/mii/lxtphy.c optional miibus | lxtphy +dev/mii/mcommphy.c optional miibus | mcommphy dev/mii/micphy.c optional miibus fdt | micphy fdt dev/mii/mii.c optional miibus | mii dev/mii/mii_bitbang.c optional miibus | mii_bitbang Index: sys/conf/files.arm64 =================================================================== --- sys/conf/files.arm64 +++ sys/conf/files.arm64 @@ -189,6 +189,10 @@ dev/enetc/enetc_mdio.c optional enetc soc_nxp_ls dev/enetc/if_enetc.c optional enetc iflib pci fdt soc_nxp_ls +dev/eqos/eqos.c optional eqos +dev/eqos/eqos_if.m optional eqos +dev/eqos/eqos_fdt.c optional eqos fdt + dev/etherswitch/felix/felix.c optional enetc etherswitch fdt felix pci soc_nxp_ls dev/gpio/pl061.c optional pl061 gpio Index: sys/dev/eqos/eqos.c =================================================================== --- sys/dev/eqos/eqos.c +++ sys/dev/eqos/eqos.c @@ -0,0 +1,1258 @@ +/*- + * SPDX-License-Identifier: BSD-2-Clause-FreeBSD + * + * Copyright (c) 2022 Soren Schmidt + * Copyright (c) 2022 Jared McNeill + * All rights reserved. + * + * 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 ``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 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. + * + * $Id$ + */ + +/* + * DesignWare Ethernet Quality-of-Service controller + */ + +#include "opt_platform.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "miibus_if.h" +#include "eqos_if.h" + +#ifdef FDT +#include +#include +#include +#include +#endif + +#include +#include + +#define DESC_BOUNDARY (1ULL << 32) +#define DESC_ALIGN sizeof(struct eqos_dma_desc) +#define DESC_OFFSET(n) ((n) * sizeof(struct eqos_dma_desc)) + +#define TX_DESC_COUNT EQOS_DMA_DESC_COUNT +#define TX_DESC_SIZE (TX_DESC_COUNT * DESC_ALIGN) +#define TX_MAX_SEGS (TX_DESC_COUNT / 2) +#define TX_NEXT(n) (((n) + 1 ) % TX_DESC_COUNT) +#define TX_QUEUED(h, t) ((((h) - (t)) + TX_DESC_COUNT) % TX_DESC_COUNT) + +#define RX_DESC_COUNT EQOS_DMA_DESC_COUNT +#define RX_DESC_SIZE (RX_DESC_COUNT * DESC_ALIGN) +#define RX_NEXT(n) (((n) + 1) % RX_DESC_COUNT) + +#define MII_BUSY_RETRY 1000 +#define WATCHDOG_TIMEOUT_SECS 3 + +#define EQOS_LOCK(sc) mtx_lock(&(sc)->lock) +#define EQOS_UNLOCK(sc) mtx_unlock(&(sc)->lock) +#define EQOS_ASSERT_LOCKED(sc) mtx_assert(&(sc)->lock, MA_OWNED) + +#define RD4(sc, o) bus_read_4(sc->res[EQOS_RES_MEM], (o)) +#define WR4(sc, o, v) bus_write_4(sc->res[EQOS_RES_MEM], (o), (v)) + + +static struct resource_spec eqos_spec[] = { + { SYS_RES_MEMORY, 0, RF_ACTIVE }, + { SYS_RES_IRQ, 0, RF_ACTIVE }, + { -1, 0 } +}; + +static void eqos_tick(void *softc); + + +static int +eqos_miibus_readreg(device_t dev, int phy, int reg) +{ + struct eqos_softc *sc = device_get_softc(dev); + uint32_t addr; + int retry, val; + + addr = sc->csr_clock_range | + (phy << GMAC_MAC_MDIO_ADDRESS_PA_SHIFT) | + (reg << GMAC_MAC_MDIO_ADDRESS_RDA_SHIFT) | + GMAC_MAC_MDIO_ADDRESS_GOC_READ | GMAC_MAC_MDIO_ADDRESS_GB; + WR4(sc, GMAC_MAC_MDIO_ADDRESS, addr); + + DELAY(100); + + for (retry = MII_BUSY_RETRY; retry > 0; retry--) { + addr = RD4(sc, GMAC_MAC_MDIO_ADDRESS); + if (!(addr & GMAC_MAC_MDIO_ADDRESS_GB)) { + val = RD4(sc, GMAC_MAC_MDIO_DATA) & 0xFFFF; + break; + } + DELAY(10); + } + if (!retry) { + device_printf(dev, "phy read timeout, phy=%d reg=%d\n", phy, reg); + return ETIMEDOUT; + } + return val; +} + +static int +eqos_miibus_writereg(device_t dev, int phy, int reg, int val) +{ + struct eqos_softc *sc = device_get_softc(dev); + uint32_t addr; + int retry; + + WR4(sc, GMAC_MAC_MDIO_DATA, val); + + addr = sc->csr_clock_range | + (phy << GMAC_MAC_MDIO_ADDRESS_PA_SHIFT) | + (reg << GMAC_MAC_MDIO_ADDRESS_RDA_SHIFT) | + GMAC_MAC_MDIO_ADDRESS_GOC_WRITE | GMAC_MAC_MDIO_ADDRESS_GB; + WR4(sc, GMAC_MAC_MDIO_ADDRESS, addr); + + DELAY(100); + + for (retry = MII_BUSY_RETRY; retry > 0; retry--) { + addr = RD4(sc, GMAC_MAC_MDIO_ADDRESS); + if (!(addr & GMAC_MAC_MDIO_ADDRESS_GB)) + break; + DELAY(10); + } + if (!retry) { + device_printf(dev, "phy write timeout, phy=%d reg=%d\n", phy, reg); + return ETIMEDOUT; + } + return 0; +} + +static void +eqos_miibus_statchg(device_t dev) +{ + struct eqos_softc *sc = device_get_softc(dev); + struct mii_data *mii = device_get_softc(sc->miibus); + uint32_t reg; + + EQOS_ASSERT_LOCKED(sc); + + if (mii->mii_media_status & IFM_ACTIVE) + sc->link_up = true; + else + sc->link_up = false; + + reg = RD4(sc, GMAC_MAC_CONFIGURATION); + + switch (IFM_SUBTYPE(mii->mii_media_active)) { + case IFM_10_T: + reg |= GMAC_MAC_CONFIGURATION_PS; + reg &= ~GMAC_MAC_CONFIGURATION_FES; + break; + case IFM_100_TX: + reg |= GMAC_MAC_CONFIGURATION_PS; + reg |= GMAC_MAC_CONFIGURATION_FES; + break; + case IFM_1000_T: + case IFM_1000_SX: + reg &= ~GMAC_MAC_CONFIGURATION_PS; + reg &= ~GMAC_MAC_CONFIGURATION_FES; + break; + case IFM_2500_T: + case IFM_2500_SX: + reg &= ~GMAC_MAC_CONFIGURATION_PS; + reg |= GMAC_MAC_CONFIGURATION_FES; + break; + default: + sc->link_up = false; + return; + } + + if ((IFM_OPTIONS(mii->mii_media_active) & IFM_FDX)) + reg |= GMAC_MAC_CONFIGURATION_DM; + else + reg &= ~GMAC_MAC_CONFIGURATION_DM; + + WR4(sc, GMAC_MAC_CONFIGURATION, reg); + + EQOS_SET_SPEED(dev, IFM_SUBTYPE(mii->mii_media_active)); + + WR4(sc, GMAC_MAC_1US_TIC_COUNTER, (sc->csr_clock / 1000000) - 1); +} + +static void +eqos_media_status(struct ifnet *ifp, struct ifmediareq *ifmr) +{ + struct eqos_softc *sc = ifp->if_softc; + struct mii_data *mii = device_get_softc(sc->miibus); + + EQOS_LOCK(sc); + mii_pollstat(mii); + ifmr->ifm_active = mii->mii_media_active; + ifmr->ifm_status = mii->mii_media_status; + EQOS_UNLOCK(sc); +} + +static int +eqos_media_change(struct ifnet *ifp) +{ + struct eqos_softc *sc = ifp->if_softc; + int error; + + EQOS_LOCK(sc); + error = mii_mediachg(device_get_softc(sc->miibus)); + EQOS_UNLOCK(sc); + return (error); +} + +static void +eqos_setup_txdesc(struct eqos_softc *sc, int index, int flags, + bus_addr_t paddr, u_int len, u_int total_len) +{ + uint32_t tdes2, tdes3; + + if (!paddr || !len) { + tdes2 = 0; + tdes3 = flags; + } else { + tdes2 = (flags & EQOS_TDES3_LD) ? EQOS_TDES2_IOC : 0; + tdes3 = flags; + } + bus_dmamap_sync(sc->tx.desc_tag, sc->tx.desc_map, BUS_DMASYNC_PREWRITE); + sc->tx.desc_ring[index].tdes0 = htole32((uint32_t)paddr); + sc->tx.desc_ring[index].tdes1 = htole32((uint32_t)(paddr >> 32)); + sc->tx.desc_ring[index].tdes2 = htole32(tdes2 | len); + sc->tx.desc_ring[index].tdes3 = htole32(tdes3 | total_len); +} + +static int +eqos_setup_txbuf(struct eqos_softc *sc, struct mbuf *m) +{ + bus_dma_segment_t segs[TX_MAX_SEGS]; + int first = sc->tx.head; + int error, nsegs, idx; + uint32_t flags; + + error = bus_dmamap_load_mbuf_sg(sc->tx.buf_tag, sc->tx.buf_map[first].map, m, segs, &nsegs, 0); + if (error == EFBIG) { + struct mbuf *mb; + + device_printf(sc->dev, "TX packet too big trying defrag\n"); + bus_dmamap_unload(sc->tx.buf_tag, sc->tx.buf_map[first].map); + if (!(mb = m_defrag(m, M_NOWAIT))) + return ENOMEM; + m = mb; + error = bus_dmamap_load_mbuf_sg(sc->tx.buf_tag, sc->tx.buf_map[first].map, m, segs, +&nsegs, 0); + } + if (error) + return ENOMEM; + + if (TX_QUEUED(sc->tx.head, sc->tx.tail) + nsegs > TX_DESC_COUNT) { + bus_dmamap_unload(sc->tx.buf_tag, sc->tx.buf_map[first].map); + device_printf(sc->dev, "TX packet no more queue space\n"); + return ENOMEM; + } + + bus_dmamap_sync(sc->tx.buf_tag, sc->tx.buf_map[first].map, BUS_DMASYNC_PREWRITE); + + sc->tx.buf_map[first].mbuf = m; + + for (flags = EQOS_TDES3_FD, idx = 0; idx < nsegs; idx++) { + if (idx == (nsegs - 1)) + flags |= EQOS_TDES3_LD; + eqos_setup_txdesc(sc, sc->tx.head, flags, segs[idx].ds_addr, segs[idx].ds_len, m->m_pkthdr.len); + flags &= ~EQOS_TDES3_FD; + flags |= EQOS_TDES3_OWN; + sc->tx.head = TX_NEXT(sc->tx.head); + } + + /* Defer setting OWN bit on the first descriptor until all descriptors have been updated */ + bus_dmamap_sync(sc->tx.desc_tag, sc->tx.desc_map, BUS_DMASYNC_PREWRITE); + sc->tx.desc_ring[first].tdes3 |= htole32(EQOS_TDES3_OWN); + + return 0; +} + +static void +eqos_setup_rxdesc(struct eqos_softc *sc, int index, bus_addr_t paddr) +{ + sc->rx.desc_ring[index].tdes0 = htole32((uint32_t)paddr); + sc->rx.desc_ring[index].tdes1 = htole32((uint32_t)(paddr >> 32)); + sc->rx.desc_ring[index].tdes2 = htole32(0); + bus_dmamap_sync(sc->rx.desc_tag, sc->rx.desc_map, BUS_DMASYNC_PREWRITE); + sc->rx.desc_ring[index].tdes3 = htole32(EQOS_RDES3_OWN | EQOS_RDES3_IOC | EQOS_RDES3_BUF1V); +} + +static int +eqos_setup_rxbuf(struct eqos_softc *sc, int index, struct mbuf *m) +{ + struct bus_dma_segment seg; + int error, nsegs; + + m_adj(m, ETHER_ALIGN); + + error = bus_dmamap_load_mbuf_sg(sc->rx.buf_tag, sc->rx.buf_map[index].map, m, &seg, &nsegs, 0); + if (error) + return error; + + bus_dmamap_sync(sc->rx.buf_tag, sc->rx.buf_map[index].map, BUS_DMASYNC_PREREAD); + + sc->rx.buf_map[index].mbuf = m; + eqos_setup_rxdesc(sc, index, seg.ds_addr); + + return 0; +} + +static struct mbuf * +eqos_alloc_mbufcl(struct eqos_softc *sc) +{ + struct mbuf *m; + + if ((m = m_getcl(M_NOWAIT, MT_DATA, M_PKTHDR))) + m->m_pkthdr.len = m->m_len = m->m_ext.ext_size; + return m; +} + +static void +eqos_enable_intr(struct eqos_softc *sc) +{ + WR4(sc, GMAC_DMA_CHAN0_INTR_ENABLE, + GMAC_DMA_CHAN0_INTR_ENABLE_NIE | GMAC_DMA_CHAN0_INTR_ENABLE_AIE | + GMAC_DMA_CHAN0_INTR_ENABLE_FBE | GMAC_DMA_CHAN0_INTR_ENABLE_RIE | + GMAC_DMA_CHAN0_INTR_ENABLE_TIE); +} + +static void +eqos_disable_intr(struct eqos_softc *sc) +{ + WR4(sc, GMAC_DMA_CHAN0_INTR_ENABLE, 0); +} + +static uint32_t +eqos_bitrev32(uint32_t x) +{ + x = (((x & 0xaaaaaaaa) >> 1) | ((x & 0x55555555) << 1)); + x = (((x & 0xcccccccc) >> 2) | ((x & 0x33333333) << 2)); + x = (((x & 0xf0f0f0f0) >> 4) | ((x & 0x0f0f0f0f) << 4)); + x = (((x & 0xff00ff00) >> 8) | ((x & 0x00ff00ff) << 8)); + return (x >> 16) | (x << 16); +} + +static u_int +eqos_hash_maddr(void *arg, struct sockaddr_dl *sdl, u_int cnt) +{ + uint32_t crc, *hash = arg; + + crc = ether_crc32_le(LLADDR(sdl), ETHER_ADDR_LEN); + crc &= 0x7f; + crc = eqos_bitrev32(~crc) >> 26; + hash[crc >> 5] |= 1 << (crc & 0x1f); + return 1; +} + +static void +eqos_setup_rxfilter(struct eqos_softc *sc) +{ + struct ifnet *ifp = sc->ifp; + uint32_t pfil, hash[2]; + const uint8_t *eaddr; + uint32_t val; + + EQOS_ASSERT_LOCKED(sc); + + pfil = RD4(sc, GMAC_MAC_PACKET_FILTER); + pfil &= ~(GMAC_MAC_PACKET_FILTER_PR | + GMAC_MAC_PACKET_FILTER_PM | + GMAC_MAC_PACKET_FILTER_HMC | + GMAC_MAC_PACKET_FILTER_PCF_MASK); + hash[0] = hash[1] = 0xffffffff; + + if ((ifp->if_flags & IFF_PROMISC)) { + pfil |= GMAC_MAC_PACKET_FILTER_PR | + GMAC_MAC_PACKET_FILTER_PCF_ALL; + } else if ((ifp->if_flags & IFF_ALLMULTI)) { + pfil |= GMAC_MAC_PACKET_FILTER_PM; + } else { + hash[0] = hash[1] = 0; + pfil |= GMAC_MAC_PACKET_FILTER_HMC; + if_foreach_llmaddr(ifp, eqos_hash_maddr, hash); + } + + /* Write our unicast address */ + eaddr = IF_LLADDR(ifp); + val = eaddr[4] | (eaddr[5] << 8); + WR4(sc, GMAC_MAC_ADDRESS0_HIGH, val); + val = eaddr[0] | (eaddr[1] << 8) | (eaddr[2] << 16) | + (eaddr[3] << 24); + WR4(sc, GMAC_MAC_ADDRESS0_LOW, val); + + /* Multicast hash filters */ + WR4(sc, GMAC_MAC_HASH_TABLE_REG0, hash[1]); + WR4(sc, GMAC_MAC_HASH_TABLE_REG1, hash[0]); + + /* Packet filter config */ + WR4(sc, GMAC_MAC_PACKET_FILTER, pfil); +} + +static int +eqos_reset(struct eqos_softc *sc) +{ + uint32_t val; + int retry; + + WR4(sc, GMAC_DMA_MODE, GMAC_DMA_MODE_SWR); + for (retry = 2000; retry > 0; retry--) { + DELAY(1000); + val = RD4(sc, GMAC_DMA_MODE); + if (!(val & GMAC_DMA_MODE_SWR)) + return 0; + } + return ETIMEDOUT; +} + +static void +eqos_init_rings(struct eqos_softc *sc) +{ + WR4(sc, GMAC_DMA_CHAN0_TX_BASE_ADDR_HI, + (uint32_t)(sc->tx.desc_ring_paddr >> 32)); + WR4(sc, GMAC_DMA_CHAN0_TX_BASE_ADDR, + (uint32_t)sc->tx.desc_ring_paddr); + WR4(sc, GMAC_DMA_CHAN0_TX_RING_LEN, TX_DESC_COUNT - 1); + + WR4(sc, GMAC_DMA_CHAN0_RX_BASE_ADDR_HI, + (uint32_t)(sc->rx.desc_ring_paddr >> 32)); + WR4(sc, GMAC_DMA_CHAN0_RX_BASE_ADDR, + (uint32_t)sc->rx.desc_ring_paddr); + WR4(sc, GMAC_DMA_CHAN0_RX_RING_LEN, RX_DESC_COUNT - 1); + + WR4(sc, GMAC_DMA_CHAN0_RX_END_ADDR, + (uint32_t)sc->rx.desc_ring_paddr + DESC_OFFSET(RX_DESC_COUNT)); +} + +static void +eqos_init(void *if_softc) +{ + struct eqos_softc *sc = if_softc; + struct ifnet *ifp = sc->ifp; + struct mii_data *mii = device_get_softc(sc->miibus); + uint32_t val; + + if (if_getdrvflags(ifp) & IFF_DRV_RUNNING) + return; + + EQOS_LOCK(sc); + + eqos_init_rings(sc); + + eqos_setup_rxfilter(sc); + + WR4(sc, GMAC_MAC_1US_TIC_COUNTER, (sc->csr_clock / 1000000) - 1); + + /* Enable transmit and receive DMA */ + val = RD4(sc, GMAC_DMA_CHAN0_CONTROL); + val &= ~GMAC_DMA_CHAN0_CONTROL_DSL_MASK; + val |= ((DESC_ALIGN - 16) / 8) << GMAC_DMA_CHAN0_CONTROL_DSL_SHIFT; + val |= GMAC_DMA_CHAN0_CONTROL_PBLX8; + WR4(sc, GMAC_DMA_CHAN0_CONTROL, val); + val = RD4(sc, GMAC_DMA_CHAN0_TX_CONTROL); + val |= GMAC_DMA_CHAN0_TX_CONTROL_OSP; + val |= GMAC_DMA_CHAN0_TX_CONTROL_START; + WR4(sc, GMAC_DMA_CHAN0_TX_CONTROL, val); + val = RD4(sc, GMAC_DMA_CHAN0_RX_CONTROL); + val &= ~GMAC_DMA_CHAN0_RX_CONTROL_RBSZ_MASK; + val |= (MCLBYTES << GMAC_DMA_CHAN0_RX_CONTROL_RBSZ_SHIFT); + val |= GMAC_DMA_CHAN0_RX_CONTROL_START; + WR4(sc, GMAC_DMA_CHAN0_RX_CONTROL, val); + + /* Disable counters */ + WR4(sc, GMAC_MMC_CONTROL, + GMAC_MMC_CONTROL_CNTFREEZ | + GMAC_MMC_CONTROL_CNTPRST | + GMAC_MMC_CONTROL_CNTPRSTLVL); + + /* Configure operation modes */ + WR4(sc, GMAC_MTL_TXQ0_OPERATION_MODE, + GMAC_MTL_TXQ0_OPERATION_MODE_TSF | + GMAC_MTL_TXQ0_OPERATION_MODE_TXQEN_EN); + WR4(sc, GMAC_MTL_RXQ0_OPERATION_MODE, + GMAC_MTL_RXQ0_OPERATION_MODE_RSF | + GMAC_MTL_RXQ0_OPERATION_MODE_FEP | + GMAC_MTL_RXQ0_OPERATION_MODE_FUP); + + /* Enable flow control */ + val = RD4(sc, GMAC_MAC_Q0_TX_FLOW_CTRL); + val |= 0xFFFFU << GMAC_MAC_Q0_TX_FLOW_CTRL_PT_SHIFT; + val |= GMAC_MAC_Q0_TX_FLOW_CTRL_TFE; + WR4(sc, GMAC_MAC_Q0_TX_FLOW_CTRL, val); + val = RD4(sc, GMAC_MAC_RX_FLOW_CTRL); + val |= GMAC_MAC_RX_FLOW_CTRL_RFE; + WR4(sc, GMAC_MAC_RX_FLOW_CTRL, val); + + /* Enable transmitter and receiver */ + val = RD4(sc, GMAC_MAC_CONFIGURATION); + val |= GMAC_MAC_CONFIGURATION_BE; + val |= GMAC_MAC_CONFIGURATION_JD; + val |= GMAC_MAC_CONFIGURATION_JE; + val |= GMAC_MAC_CONFIGURATION_DCRS; + val |= GMAC_MAC_CONFIGURATION_TE; + val |= GMAC_MAC_CONFIGURATION_RE; + WR4(sc, GMAC_MAC_CONFIGURATION, val); + + eqos_enable_intr(sc); + + if_setdrvflagbits(ifp, IFF_DRV_RUNNING, IFF_DRV_OACTIVE); + + mii_mediachg(mii); + callout_reset(&sc->callout, hz, eqos_tick, sc); + + EQOS_UNLOCK(sc); +} + +static void +eqos_start_locked(struct ifnet *ifp) +{ + struct eqos_softc *sc = ifp->if_softc; + struct mbuf *m; + int pending = 0; + + if (!sc->link_up) + return; + + if ((if_getdrvflags(ifp) & (IFF_DRV_RUNNING|IFF_DRV_OACTIVE)) != IFF_DRV_RUNNING) + return; + + while (true) { + if (TX_QUEUED(sc->tx.head, sc->tx.tail) >= TX_DESC_COUNT - TX_MAX_SEGS) { + if_setdrvflagbits(ifp, IFF_DRV_OACTIVE, 0); + break; + } + + if (!(m = if_dequeue(ifp))) + break; + + if (eqos_setup_txbuf(sc, m)) { + if_sendq_prepend(ifp, m); + if_setdrvflagbits(ifp, IFF_DRV_OACTIVE, 0); + break; + } + if_bpfmtap(ifp, m); + pending++; + } + + if (pending) { + bus_dmamap_sync(sc->tx.desc_tag, sc->tx.desc_map, BUS_DMASYNC_PREREAD | BUS_DMASYNC_PREWRITE); + + /* Start and run TX DMA */ + WR4(sc, GMAC_DMA_CHAN0_TX_END_ADDR, + (uint32_t)sc->tx.desc_ring_paddr + DESC_OFFSET(sc->tx.head)); + sc->tx_watchdog = WATCHDOG_TIMEOUT_SECS; + } +} + +static void +eqos_start(struct ifnet *ifp) +{ + struct eqos_softc *sc = ifp->if_softc; + + EQOS_LOCK(sc); + eqos_start_locked(ifp); + EQOS_UNLOCK(sc); +} + +static void +eqos_stop(struct eqos_softc *sc) +{ + struct ifnet *ifp = sc->ifp; + uint32_t val; + int retry; + + EQOS_LOCK(sc); + + if_setdrvflagbits(ifp, 0, IFF_DRV_RUNNING | IFF_DRV_OACTIVE); + + callout_stop(&sc->callout); + + /* Disable receiver */ + val = RD4(sc, GMAC_MAC_CONFIGURATION); + val &= ~GMAC_MAC_CONFIGURATION_RE; + WR4(sc, GMAC_MAC_CONFIGURATION, val); + + /* Stop receive DMA */ + val = RD4(sc, GMAC_DMA_CHAN0_RX_CONTROL); + val &= ~GMAC_DMA_CHAN0_RX_CONTROL_START; + WR4(sc, GMAC_DMA_CHAN0_RX_CONTROL, val); + + /* Stop transmit DMA */ + val = RD4(sc, GMAC_DMA_CHAN0_TX_CONTROL); + val &= ~GMAC_DMA_CHAN0_TX_CONTROL_START; + WR4(sc, GMAC_DMA_CHAN0_TX_CONTROL, val); + + /* Flush data in the TX FIFO */ + val = RD4(sc, GMAC_MTL_TXQ0_OPERATION_MODE); + val |= GMAC_MTL_TXQ0_OPERATION_MODE_FTQ; + WR4(sc, GMAC_MTL_TXQ0_OPERATION_MODE, val); + for (retry = 10000; retry > 0; retry--) { + val = RD4(sc, GMAC_MTL_TXQ0_OPERATION_MODE); + if (!(val & GMAC_MTL_TXQ0_OPERATION_MODE_FTQ)) + break; + DELAY(10); + } + if (!retry) + device_printf(sc->dev, "timeout flushing TX queue\n"); + + /* Disable transmitter */ + val = RD4(sc, GMAC_MAC_CONFIGURATION); + val &= ~GMAC_MAC_CONFIGURATION_TE; + WR4(sc, GMAC_MAC_CONFIGURATION, val); + + eqos_disable_intr(sc); + + EQOS_UNLOCK(sc); +} + +static void +eqos_rxintr(struct eqos_softc *sc) +{ + struct ifnet *ifp = sc->ifp; + struct mbuf *m; + uint32_t tdes3; + int error, length; + + while (true) { + tdes3 = le32toh(sc->rx.desc_ring[sc->rx.head].tdes3); + if ((tdes3 & EQOS_RDES3_OWN)) + break; + + if (tdes3 & (EQOS_RDES3_OE | EQOS_RDES3_RE)) + printf("Receive errer rdes3=%08x\n", tdes3); + + bus_dmamap_sync(sc->rx.buf_tag, sc->rx.buf_map[sc->rx.head].map, BUS_DMASYNC_POSTREAD); + bus_dmamap_unload(sc->rx.buf_tag, sc->rx.buf_map[sc->rx.head].map); + + length = tdes3 & EQOS_RDES3_LENGTH_MASK; + if (length) { + m = sc->rx.buf_map[sc->rx.head].mbuf; + m->m_pkthdr.rcvif = ifp; + m->m_pkthdr.len = length; + m->m_len = length; + m->m_nextpkt = NULL; + + /* Remove trailing FCS */ + m_adj(m, -ETHER_CRC_LEN); + + EQOS_UNLOCK(sc); + (*ifp->if_input)(ifp, m); + EQOS_LOCK(sc); + } + + if ((m = eqos_alloc_mbufcl(sc))) { + if ((error = eqos_setup_rxbuf(sc, sc->rx.head, m))) + printf("ERROR: Hole in RX ring!!\n"); + } + else + if_inc_counter(ifp, IFCOUNTER_IERRORS, 1); + + if_inc_counter(ifp, IFCOUNTER_IPACKETS, 1); + + WR4(sc, GMAC_DMA_CHAN0_RX_END_ADDR, + (uint32_t)sc->rx.desc_ring_paddr + DESC_OFFSET(sc->rx.head)); + + sc->rx.head = RX_NEXT(sc->rx.head); + } +} + +static void +eqos_txintr(struct eqos_softc *sc) +{ + struct ifnet *ifp = sc->ifp; + struct eqos_bufmap *bmap; + uint32_t tdes3; + + EQOS_ASSERT_LOCKED(sc); + + while (sc->tx.tail != sc->tx.head) { + tdes3 = le32toh(sc->tx.desc_ring[sc->tx.tail].tdes3); + if ((tdes3 & EQOS_TDES3_OWN)) + break; + + bmap = &sc->tx.buf_map[sc->tx.tail]; + if (bmap->mbuf) { + bus_dmamap_sync(sc->tx.buf_tag, bmap->map, BUS_DMASYNC_POSTWRITE); + bus_dmamap_unload(sc->tx.buf_tag, bmap->map); + m_freem(bmap->mbuf); + bmap->mbuf = NULL; + } + + eqos_setup_txdesc(sc, sc->tx.tail, 0, 0, 0, 0); + + if_setdrvflagbits(ifp, 0, IFF_DRV_OACTIVE); + + /* Last descriptor in a packet contains DMA status */ + if ((tdes3 & EQOS_TDES3_LD)) { + if ((tdes3 & EQOS_TDES3_DE)) { + if_inc_counter(ifp, IFCOUNTER_OERRORS, 1); + } else if ((tdes3 & EQOS_TDES3_ES)) { + if_inc_counter(ifp, IFCOUNTER_OERRORS, 1); + } else { + if_inc_counter(ifp, IFCOUNTER_OPACKETS, 1); + } + } + sc->tx.tail = TX_NEXT(sc->tx.tail); + } + if (sc->tx.tail == sc->tx.head) + sc->tx_watchdog = 0; + eqos_start_locked(sc->ifp); +} + +static void +eqos_intr_mtl(struct eqos_softc *sc, uint32_t mtl_status) +{ + uint32_t mtl_istat = 0; + + if ((mtl_status & GMAC_MTL_INTERRUPT_STATUS_Q0IS)) { + uint32_t mtl_clear = 0; + + mtl_istat = RD4(sc, GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS); + if ((mtl_istat & GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS_RXOVFIS)) { + mtl_clear |= GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS_RXOVFIS; + } + if ((mtl_istat & GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS_TXUNFIS)) { + mtl_clear |= GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS_TXUNFIS; + } + if (mtl_clear) { + mtl_clear |= (mtl_istat & + (GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS_RXOIE | + GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS_TXUIE)); + WR4(sc, GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS, mtl_clear); + } + } + + device_printf(sc->dev, + "GMAC_MTL_INTERRUPT_STATUS = 0x%08X, " + "GMAC_MTL_INTERRUPT_STATUS_Q0IS = 0x%08X\n", + mtl_status, mtl_istat); +} + +static void +eqos_tick(void *softc) +{ + struct eqos_softc *sc = softc; + struct mii_data *mii = device_get_softc(sc->miibus); + bool link_status; + + EQOS_ASSERT_LOCKED(sc); + + if (sc->tx_watchdog > 0) + if (!--sc->tx_watchdog) { + device_printf(sc->dev, "watchdog timeout\n"); + eqos_txintr(sc); + } + + link_status = sc->link_up; + mii_tick(mii); + if (sc->link_up && !link_status) + eqos_start_locked(sc->ifp); + + callout_reset(&sc->callout, hz, eqos_tick, sc); +} + +static void +eqos_intr(void *arg) +{ + struct eqos_softc *sc = arg; + uint32_t mac_status, mtl_status, dma_status, rx_tx_status; + + mac_status = RD4(sc, GMAC_MAC_INTERRUPT_STATUS); + mac_status &= RD4(sc, GMAC_MAC_INTERRUPT_ENABLE); + + if (mac_status) + device_printf(sc->dev, "MAC interrupt\n"); + + if ((mtl_status = RD4(sc, GMAC_MTL_INTERRUPT_STATUS))) + eqos_intr_mtl(sc, mtl_status); + + dma_status = RD4(sc, GMAC_DMA_CHAN0_STATUS); + dma_status &= RD4(sc, GMAC_DMA_CHAN0_INTR_ENABLE); + + if (dma_status) + WR4(sc, GMAC_DMA_CHAN0_STATUS, dma_status); + + EQOS_LOCK(sc); + + if (dma_status & GMAC_DMA_CHAN0_STATUS_RI) + eqos_rxintr(sc); + + if (dma_status & GMAC_DMA_CHAN0_STATUS_TI) + eqos_txintr(sc); + + EQOS_UNLOCK(sc); + + if (!(mac_status | mtl_status | dma_status)) { + device_printf(sc->dev, "spurious interrupt mac=%08x mtl=%08x dma=%08x\n", + RD4(sc, GMAC_MAC_INTERRUPT_STATUS), + RD4(sc, GMAC_MTL_INTERRUPT_STATUS), + RD4(sc, GMAC_DMA_CHAN0_STATUS)); + } + if ((rx_tx_status = RD4(sc, GMAC_MAC_RX_TX_STATUS))) + device_printf(sc->dev, "RX/TX status interrupt\n"); +} + +static int +eqos_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data) +{ + struct eqos_softc *sc = ifp->if_softc; + struct ifreq *ifr = (struct ifreq *)data; + struct mii_data *mii; + int flags, mask; + int error = 0; + + switch (cmd) { + case SIOCSIFFLAGS: + if (if_getflags(ifp) & IFF_UP) { + if (if_getdrvflags(ifp) & IFF_DRV_RUNNING) { + flags = if_getflags(ifp); + if ((flags & (IFF_PROMISC|IFF_ALLMULTI))) { + EQOS_LOCK(sc); + eqos_setup_rxfilter(sc); + EQOS_UNLOCK(sc); + } + } + else { + eqos_init(sc); + } + } + else { + if (if_getdrvflags(ifp) & IFF_DRV_RUNNING) + eqos_stop(sc); + } + break; + + case SIOCADDMULTI: + case SIOCDELMULTI: + if (if_getdrvflags(ifp) & IFF_DRV_RUNNING) { + EQOS_LOCK(sc); + eqos_setup_rxfilter(sc); + EQOS_UNLOCK(sc); + } + break; + + case SIOCSIFMEDIA: + case SIOCGIFMEDIA: + mii = device_get_softc(sc->miibus); + error = ifmedia_ioctl(ifp, ifr, &mii->mii_media, cmd); + break; + + case SIOCSIFCAP: + mask = ifr->ifr_reqcap ^ if_getcapenable(ifp); + if (mask & IFCAP_VLAN_MTU) + if_togglecapenable(ifp, IFCAP_VLAN_MTU); + if (mask & IFCAP_RXCSUM) + if_togglecapenable(ifp, IFCAP_RXCSUM); + if (mask & IFCAP_TXCSUM) + if_togglecapenable(ifp, IFCAP_TXCSUM); + if ((if_getcapenable(ifp) & IFCAP_TXCSUM)) + if_sethwassistbits(ifp, CSUM_IP | CSUM_UDP | CSUM_TCP, 0); + else + if_sethwassistbits(ifp, 0, CSUM_IP | CSUM_UDP | CSUM_TCP); + break; + + default: + error = ether_ioctl(ifp, cmd, data); + break; + } + + return error; +} + +static void +eqos_get_eaddr(struct eqos_softc *sc, uint8_t *eaddr) +{ + uint32_t maclo, machi, rnd; + + maclo = htobe32(RD4(sc, GMAC_MAC_ADDRESS0_LOW)); + machi = htobe16(RD4(sc, GMAC_MAC_ADDRESS0_HIGH) & 0xFFFF); + + if (maclo == 0xffffffff && machi == 0xffff) { + rnd = arc4random() & 0x00ffffff; + eaddr[0] = 'b'; + eaddr[1] = 's'; + eaddr[2] = 'd'; + eaddr[3] = rnd >> 16; + eaddr[4] = rnd >> 8; + eaddr[5] = rnd >> 0; + } + else { + eaddr[0] = maclo & 0xff; + eaddr[1] = (maclo >> 8) & 0xff; + eaddr[2] = (maclo >> 16) & 0xff; + eaddr[3] = (maclo >> 24) & 0xff; + eaddr[4] = machi & 0xff; + eaddr[5] = (machi >> 8) & 0xff; + } +} + +static void +eqos_axi_configure(struct eqos_softc *sc) +{ + uint32_t val; + + val = RD4(sc, GMAC_DMA_SYSBUS_MODE); + + /* Max Write Outstanding Req Limit */ + val &= ~GMAC_DMA_SYSBUS_MODE_WR_OSR_LMT_MASK; + val |= 0x03 << GMAC_DMA_SYSBUS_MODE_WR_OSR_LMT_SHIFT; + + /* Max Read Outstanding Req Limit */ + val &= ~GMAC_DMA_SYSBUS_MODE_RD_OSR_LMT_MASK; + val |= 0x07 << GMAC_DMA_SYSBUS_MODE_RD_OSR_LMT_SHIFT; + + /* Allowed Burst Length's */ + val |= GMAC_DMA_SYSBUS_MODE_BLEN16; + val |= GMAC_DMA_SYSBUS_MODE_BLEN8; + val |= GMAC_DMA_SYSBUS_MODE_BLEN4; + + /* Fixed Burst Length */ + val |= GMAC_DMA_SYSBUS_MODE_MB; + + WR4(sc, GMAC_DMA_SYSBUS_MODE, val); +} + +static void +eqos_get1paddr(void *arg, bus_dma_segment_t *segs, int nsegs, int error) +{ + if (!error) + *(bus_addr_t *)arg = segs[0].ds_addr; +} + +static int +eqos_setup_dma(struct eqos_softc *sc) +{ + struct mbuf *m; + int error, i; + + /* Set up TX descriptor ring, descriptors, and dma maps */ + if ((error = bus_dma_tag_create(bus_get_dma_tag(sc->dev), + DESC_ALIGN, DESC_BOUNDARY, + BUS_SPACE_MAXADDR_32BIT, + BUS_SPACE_MAXADDR, NULL, NULL, + TX_DESC_SIZE, 1, TX_DESC_SIZE, 0, + NULL, NULL, &sc->tx.desc_tag))) { + device_printf(sc->dev, "could not create TX ring DMA tag\n"); + return error; + } + + if ((error = bus_dmamem_alloc(sc->tx.desc_tag, (void**)&sc->tx.desc_ring, + BUS_DMA_COHERENT | BUS_DMA_WAITOK | BUS_DMA_ZERO, + &sc->tx.desc_map))) { + device_printf(sc->dev, "could not allocate TX descriptor ring.\n"); + return error; + } + + if ((error = bus_dmamap_load(sc->tx.desc_tag, sc->tx.desc_map, sc->tx.desc_ring, + TX_DESC_SIZE, eqos_get1paddr, &sc->tx.desc_ring_paddr, 0))) { + device_printf(sc->dev, "could not load TX descriptor ring map.\n"); + return error; + } + + if ((error = bus_dma_tag_create(bus_get_dma_tag(sc->dev), 1, 0, + BUS_SPACE_MAXADDR_32BIT, + BUS_SPACE_MAXADDR, NULL, NULL, + MCLBYTES*TX_MAX_SEGS, TX_MAX_SEGS, + MCLBYTES, 0, NULL, NULL, + &sc->tx.buf_tag))) { + device_printf(sc->dev, "could not create TX buffer DMA tag.\n"); + return error; + } + + for (i = 0; i < TX_DESC_COUNT; i++) { + if ((error = bus_dmamap_create(sc->tx.buf_tag, BUS_DMA_COHERENT, + &sc->tx.buf_map[i].map))) { + device_printf(sc->dev, "cannot create TX buffer map\n"); + return error; + } + eqos_setup_txdesc(sc, i, EQOS_TDES3_OWN, 0, 0, 0); + } + + /* Set up RX descriptor ring, descriptors, dma maps, and mbufs */ + if ((error = bus_dma_tag_create(bus_get_dma_tag(sc->dev), + DESC_ALIGN, DESC_BOUNDARY, + BUS_SPACE_MAXADDR_32BIT, + BUS_SPACE_MAXADDR, NULL, NULL, + RX_DESC_SIZE, 1, RX_DESC_SIZE, 0, + NULL, NULL, &sc->rx.desc_tag))) { + device_printf(sc->dev, "could not create RX ring DMA tag.\n"); + return error; + } + + if ((error = bus_dmamem_alloc(sc->rx.desc_tag, (void **)&sc->rx.desc_ring, + BUS_DMA_COHERENT | BUS_DMA_WAITOK | BUS_DMA_ZERO, + &sc->rx.desc_map))) { + device_printf(sc->dev, "could not allocate RX descriptor ring.\n"); + return error; + } + + if ((error = bus_dmamap_load(sc->rx.desc_tag, sc->rx.desc_map, sc->rx.desc_ring, + RX_DESC_SIZE, eqos_get1paddr, + &sc->rx.desc_ring_paddr, 0))) { + device_printf(sc->dev, "could not load RX descriptor ring map.\n"); + return error; + } + + if ((error = bus_dma_tag_create(bus_get_dma_tag(sc->dev), 1, 0, + BUS_SPACE_MAXADDR_32BIT, + BUS_SPACE_MAXADDR, NULL, NULL, + MCLBYTES, 1, + MCLBYTES, 0, NULL, NULL, + &sc->rx.buf_tag))) { + device_printf(sc->dev, "could not create RX buf DMA tag.\n"); + return error; + } + + for (i = 0; i < RX_DESC_COUNT; i++) { + if ((error = bus_dmamap_create(sc->rx.buf_tag, BUS_DMA_COHERENT, + &sc->rx.buf_map[i].map))) { + device_printf(sc->dev, "cannot create RX buffer map\n"); + return error; + } + if (!(m = eqos_alloc_mbufcl(sc))) { + device_printf(sc->dev, "cannot allocate RX mbuf\n"); + return ENOMEM; + } + if ((error = eqos_setup_rxbuf(sc, i, m))) { + device_printf(sc->dev, "cannot create RX buffer\n"); + return error; + } + } + + if (bootverbose) + device_printf(sc->dev, "TX ring @ 0x%lx, RX ring @ 0x%lx\n", + sc->tx.desc_ring_paddr, sc->rx.desc_ring_paddr); + return 0; +} + +static int +eqos_attach(device_t dev) +{ + struct eqos_softc *sc = device_get_softc(dev); + struct ifnet *ifp; + uint32_t ver; + uint8_t eaddr[ETHER_ADDR_LEN]; + u_int userver, snpsver; + int error; + int n; + + /* setup resources */ + if (bus_alloc_resources(dev, eqos_spec, sc->res)) { + device_printf(dev, "Could not allocate resources\n"); + bus_release_resources(dev, eqos_spec, sc->res); + return ENXIO; + } + + sc->dev = dev; + ver = RD4(sc, GMAC_MAC_VERSION); + userver = (ver & GMAC_MAC_VERSION_USERVER_MASK) >> + GMAC_MAC_VERSION_USERVER_SHIFT; + snpsver = ver & GMAC_MAC_VERSION_SNPSVER_MASK; + + if (snpsver != 0x51) { + device_printf(dev, "EQOS version 0x%02xx not supported\n", + snpsver); + return ENXIO; + } + + for (n = 0; n < 4; n++) + sc->hw_feature[n] = RD4(sc, GMAC_MAC_HW_FEATURE(n)); + + if (bootverbose) { + device_printf(dev, "DesignWare EQOS ver 0x%02x (0x%02x)\n", + snpsver, userver); + device_printf(dev, "hw features %08x %08x %08x %08x\n", + sc->hw_feature[0], sc->hw_feature[1], + sc->hw_feature[2], sc->hw_feature[3]); + } + + + if ((error = EQOS_INIT(dev))) + return error; + + mtx_init(&sc->lock, "eqos lock", MTX_NETWORK_LOCK, MTX_DEF); + callout_init_mtx(&sc->callout, &sc->lock, 0); + + eqos_get_eaddr(sc, eaddr); + if (bootverbose) + device_printf(sc->dev, "Ethernet address %6D\n", eaddr, ":"); + + /* Soft reset EMAC core */ + if ((error = eqos_reset(sc))) { + device_printf(sc->dev, "reset timeout!\n"); + return error; + } + + /* Configure AXI Bus mode parameters */ + eqos_axi_configure(sc); + + /* Setup DMA descriptors */ + if (eqos_setup_dma(sc)) { + device_printf(sc->dev, "failed to setup DMA descriptors\n"); + return EINVAL; + } + + /* setup interrupt delivery */ + if ((bus_setup_intr(dev, sc->res[EQOS_RES_IRQ0], EQOS_INTR_FLAGS, + NULL, eqos_intr, sc, &sc->irq_handle))) { + device_printf(dev, "unable to setup 1st interrupt\n"); + bus_release_resources(dev, eqos_spec, sc->res); + return ENXIO; + } + + /* Setup ethernet interface */ + ifp = sc->ifp = if_alloc(IFT_ETHER); + ifp->if_softc = sc; + if_initname(ifp, device_get_name(sc->dev), device_get_unit(sc->dev)); + if_setflags(sc->ifp, IFF_BROADCAST | IFF_SIMPLEX | IFF_MULTICAST); + if_setstartfn(ifp, eqos_start); + if_setioctlfn(ifp, eqos_ioctl); + if_setinitfn(ifp, eqos_init); + if_setsendqlen(ifp, TX_DESC_COUNT - 1); + if_setsendqready(ifp); + if_setcapabilities(ifp, IFCAP_VLAN_MTU /*| IFCAP_HWCSUM*/); + if_setcapenable(ifp, if_getcapabilities(ifp)); + + /* Attach MII driver */ + if ((error = mii_attach(sc->dev, &sc->miibus, ifp, eqos_media_change, + eqos_media_status, BMSR_DEFCAPMASK, MII_PHY_ANY, + MII_OFFSET_ANY, 0))) { + device_printf(sc->dev, "PHY attach failed\n"); + return (ENXIO); + } + + /* Attach ethernet interface */ + ether_ifattach(ifp, eaddr); + + return 0; +} + +static int +eqos_detach(device_t dev) +{ + struct eqos_softc *sc = device_get_softc(dev); + int i; + + if (device_is_attached(dev)) { + EQOS_LOCK(sc); + eqos_stop(sc); + EQOS_UNLOCK(sc); + sc->ifp->if_flags &= ~IFF_UP; + ether_ifdetach(sc->ifp); + } + + if (sc->miibus) + device_delete_child(dev, sc->miibus); + bus_generic_detach(dev); + + if (sc->irq_handle) + bus_teardown_intr(dev, sc->res[EQOS_RES_IRQ0], sc->irq_handle); + + if (sc->ifp) + if_free(sc->ifp); + + bus_release_resources(dev, eqos_spec, sc->res); + + if (sc->tx.desc_tag) { + if (sc->tx.desc_map) { + bus_dmamap_unload(sc->tx.desc_tag, sc->tx.desc_map); + bus_dmamem_free(sc->tx.desc_tag, sc->tx.desc_ring, sc->tx.desc_map); + } + bus_dma_tag_destroy(sc->tx.desc_tag); + } + if (sc->tx.buf_tag) { + for (i = 0; i < TX_DESC_COUNT; i++) { + m_free(sc->tx.buf_map[i].mbuf); + bus_dmamap_destroy(sc->tx.buf_tag, sc->tx.buf_map[i].map); + } + bus_dma_tag_destroy(sc->tx.buf_tag); + } + + if (sc->rx.desc_tag) { + if (sc->rx.desc_map) { + bus_dmamap_unload(sc->rx.desc_tag, sc->rx.desc_map); + bus_dmamem_free(sc->rx.desc_tag, sc->rx.desc_ring, sc->rx.desc_map); + } + bus_dma_tag_destroy(sc->rx.desc_tag); + } + if (sc->rx.buf_tag) { + for (i = 0; i < RX_DESC_COUNT; i++) { + m_free(sc->rx.buf_map[i].mbuf); + bus_dmamap_destroy(sc->rx.buf_tag, sc->rx.buf_map[i].map); + } + bus_dma_tag_destroy(sc->rx.buf_tag); + } + + mtx_destroy(&sc->lock); + + return 0; +} + + +static device_method_t eqos_methods[] = { + /* Device Interface */ + DEVMETHOD(device_attach, eqos_attach), + DEVMETHOD(device_detach, eqos_detach), + + /* MII Interface */ + DEVMETHOD(miibus_readreg, eqos_miibus_readreg), + DEVMETHOD(miibus_writereg, eqos_miibus_writereg), + DEVMETHOD(miibus_statchg, eqos_miibus_statchg), + + DEVMETHOD_END +}; + +driver_t eqos_driver = { + "eqos", + eqos_methods, + sizeof(struct eqos_softc), +}; + +devclass_t eqos_devclass; + +DRIVER_MODULE(miibus, eqos, miibus_driver, 0, 0); Index: sys/dev/eqos/eqos_fdt.c =================================================================== --- sys/dev/eqos/eqos_fdt.c +++ sys/dev/eqos/eqos_fdt.c @@ -0,0 +1,281 @@ +/*- + * SPDX-License-Identifier: BSD-2-Clause-FreeBSD + * + * Copyright (c) 2022 Soren Schmidt + * All rights reserved. + * + * 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. + * + * $Id$ + */ + +#include "opt_platform.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include + +#include "eqos_if.h" +#include "syscon_if.h" +#include "gpio_if.h" + + +#define EQOS_GRF_GMAC0_CON0 0x0380 +#define EQOS_GRF_GMAC0_CON1 0x0384 +#define EQOS_GRF_GMAC1_CON0 0x0388 +#define EQOS_GRF_GMAC1_CON1 0x038c + +#define EQOS_GMAC_PHY_INTF_SEL_RGMII 0x00f80010 +#define EQOS_GMAC_PHY_INTF_SEL_RMII 0x00f80040 +#define EQOS_GMAC_RXCLK_DLY_ENABLE 0x00020002 +#define EQOS_GMAC_RXCLK_DLY_DISABLE 0x00020000 +#define EQOS_GMAC_TXCLK_DLY_ENABLE 0x00010001 +#define EQOS_GMAC_TXCLK_DLY_DISABLE 0x00010000 + +#define EQOS_GMAC_CLK_RX_DL_CFG(val) (0x7f000000 | val << 8) +#define EQOS_GMAC_CLK_TX_DL_CFG(val) (0x007f0000 | val) + +static const struct ofw_compat_data compat_data[] = { + {"snps,dwmac-4.20a", 1}, + { NULL, 0 } +}; + + +static int +eqos_phy_reset(device_t dev) +{ + pcell_t gpio_prop[4]; + pcell_t delay_prop[3]; + phandle_t node, gpio_node; + device_t gpio; + uint32_t pin, flags; + uint32_t pin_value; + + node = ofw_bus_get_node(dev); + if (OF_getencprop(node, "snps,reset-gpio", + gpio_prop, sizeof(gpio_prop)) <= 0) + return 0; + + if (OF_getencprop(node, "snps,reset-delays-us", + delay_prop, sizeof(delay_prop)) <= 0) { + device_printf(dev, + "Wrong property for snps,reset-delays-us"); + return ENXIO; + } + + gpio_node = OF_node_from_xref(gpio_prop[0]); + if ((gpio = OF_device_from_xref(gpio_prop[0])) == NULL) { + device_printf(dev, + "Can't find gpio controller for phy reset\n"); + return ENXIO; + } + + if (GPIO_MAP_GPIOS(gpio, node, gpio_node, + nitems(gpio_prop) - 1, + gpio_prop + 1, &pin, &flags) != 0) { + device_printf(dev, "Can't map gpio for phy reset\n"); + return ENXIO; + } + + pin_value = GPIO_PIN_LOW; + if (OF_hasprop(node, "snps,reset-active-low")) + pin_value = GPIO_PIN_HIGH; + + GPIO_PIN_SETFLAGS(gpio, pin, GPIO_PIN_OUTPUT); + GPIO_PIN_SET(gpio, pin, pin_value); + DELAY(delay_prop[0]); + GPIO_PIN_SET(gpio, pin, !pin_value); + DELAY(delay_prop[1]); + GPIO_PIN_SET(gpio, pin, pin_value); + DELAY(delay_prop[2]); + + return 0; +} + +static int +eqos_fdt_set_speed(device_t dev, int speed) +{ + struct eqos_softc *sc = device_get_softc(dev); + uint64_t freq = 0; + + switch (speed) { + case IFM_10_T: + freq = 2500000; + break; + case IFM_100_TX: + freq = 25000000; + break; + case IFM_1000_T: + case IFM_1000_SX: + freq = 125000000; + break; + default: + sc->link_up = false; + return ENXIO; + } + if (!clk_set_freq(sc->clk_mac_speed, freq, 0)) { + sc->csr_clock = freq; + if (sc->csr_clock > 35000000) { + sc->csr_clock_range = GMAC_MAC_MDIO_ADDRESS_CR_100_150; + SYSCON_WRITE_4(sc->grf, EQOS_GRF_GMAC1_CON1, + EQOS_GMAC_PHY_INTF_SEL_RGMII | + EQOS_GMAC_RXCLK_DLY_ENABLE | + EQOS_GMAC_TXCLK_DLY_ENABLE); + } + else { + sc->csr_clock_range = GMAC_MAC_MDIO_ADDRESS_CR_20_35; + SYSCON_WRITE_4(sc->grf, EQOS_GRF_GMAC1_CON1, + EQOS_GMAC_PHY_INTF_SEL_RGMII | + EQOS_GMAC_RXCLK_DLY_ENABLE | + EQOS_GMAC_TXCLK_DLY_ENABLE | 0x00040004); + } + if (bootverbose) + device_printf(dev, "setting interface speed to %d\n", + sc->csr_clock); + } + else + device_printf(dev, "setting interface speed failed\n"); + return 0; +} + +static int +eqos_fdt_init(device_t dev) +{ + struct eqos_softc *sc = device_get_softc(dev); + phandle_t node = ofw_bus_get_node(dev); + hwreset_t eqos_reset; + regulator_t eqos_supply; + uint32_t rx_delay, tx_delay; + int error; + + if (OF_hasprop(node, "rockchip,grf") && + syscon_get_by_ofw_property(dev, node, "rockchip,grf", &sc->grf)) { + device_printf(dev, "cannot get grf driver handle\n"); + return ENXIO; + } + + if (hwreset_get_by_ofw_idx(dev, node, 0, &eqos_reset)) { + device_printf(dev, "cannot get reset\n"); + return ENXIO; + } + else + hwreset_assert(eqos_reset); + + if (!clk_get_by_ofw_name(dev, 0, "clk_mac_speed", &sc->clk_mac_speed)) { + if ((error = clk_enable(sc->clk_mac_speed))) { + device_printf(dev, "cannot enable clk_mac_speed\n"); + return error; + } + if (bootverbose) { + uint64_t freq; + + clk_get_freq(sc->clk_mac_speed, &freq); + device_printf(dev, "MAC frequency=%ld\n", freq); + } + } + else { + device_printf(dev, "could not find clock clk_mac_speed\n"); + return ENXIO; + } + + if (OF_getencprop(node, "tx_delay", &tx_delay, sizeof(tx_delay)) <= 0) + tx_delay = 0x30; + if (OF_getencprop(node, "rx_delay", &rx_delay, sizeof(rx_delay)) <= 0) + rx_delay = 0x10; + SYSCON_WRITE_4(sc->grf, EQOS_GRF_GMAC1_CON0, + EQOS_GMAC_CLK_RX_DL_CFG(rx_delay) | + EQOS_GMAC_CLK_TX_DL_CFG(tx_delay)); + + if (!regulator_get_by_ofw_property(dev, 0, "phy-supply", &eqos_supply)){ + if (regulator_enable(eqos_supply)) + device_printf(dev, "cannot enable 'phy' regulator\n"); + } + else + device_printf(dev, "no phy-supply property\n"); + + if (eqos_phy_reset(dev)) + return ENXIO; + + eqos_fdt_set_speed(dev, IFM_1000_T); + + if (eqos_reset) + hwreset_deassert(eqos_reset); + + /* maybe find a way to set the MAC address */ + + return 0; +} + +static int +eqos_fdt_probe(device_t dev) +{ + if (!ofw_bus_status_okay(dev)) + return ENXIO; + if (ofw_bus_search_compatible(dev, compat_data)->ocd_data == 0) + return ENXIO; + + device_set_desc(dev, "DesignWare EQOS Gigabit ethernet"); + + return BUS_PROBE_DEFAULT; +} + + +static device_method_t eqos_fdt_methods[] = { + /* Device interface */ + DEVMETHOD(device_probe, eqos_fdt_probe), + + /* EQOS interface */ + DEVMETHOD(eqos_init, eqos_fdt_init), + DEVMETHOD(eqos_set_speed, eqos_fdt_set_speed), + + DEVMETHOD_END +}; + +DEFINE_CLASS_1(eqos, eqos_fdt_driver, eqos_fdt_methods, + sizeof(struct eqos_softc), eqos_driver); + +extern devclass_t eqos_devclass; + +DRIVER_MODULE(eqos, simplebus, eqos_fdt_driver, eqos_devclass, 0, 0); +MODULE_DEPEND(eqos, ether, 1, 1, 1); +MODULE_DEPEND(eqos, miibus, 1, 1, 1); Index: sys/dev/eqos/eqos_if.m =================================================================== --- sys/dev/eqos/eqos_if.m +++ sys/dev/eqos/eqos_if.m @@ -0,0 +1,63 @@ +#- +# SPDX-License-Identifier: BSD-2-Clause-FreeBSD +# +# Copyright (c) 2022 Soren Schmidt +# All rights reserved. +# +# 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. +# +# $Id$ +# + +INTERFACE eqos; + +CODE { + static int + eqos_default_init(device_t dev) + { + return 0; + } + + static int + eqos_default_set_speed(device_t dev, int speed) + { + return 0; + } +}: + +HEADER { +}; + +# +# Initialize SoC specific registers. +# +METHOD int init { + device_t dev; +} DEFAULT eqos_default_init; + +# +# Signal media change to a specific hardware +# +METHOD int set_speed { + device_t dev; + int speed; +} DEFAULT eqos_default_set_speed; Index: sys/dev/eqos/eqos_reg.h =================================================================== --- sys/dev/eqos/eqos_reg.h +++ sys/dev/eqos/eqos_reg.h @@ -0,0 +1,295 @@ +/*- + * SPDX-License-Identifier: BSD-2-Clause-FreeBSD + * + * Copyright (c) 2022 Jared McNeill + * All rights reserved. + * + * 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 ``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 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. + * + * $Id$ + */ + +/* + * DesignWare Ethernet Quality-of-Service controller + */ + +#ifndef _EQOS_REG_H +#define _EQOS_REG_H + +#define GMAC_MAC_CONFIGURATION 0x0000 +#define GMAC_MAC_CONFIGURATION_CST (1U << 21) +#define GMAC_MAC_CONFIGURATION_ACS (1U << 20) +#define GMAC_MAC_CONFIGURATION_BE (1U << 18) +#define GMAC_MAC_CONFIGURATION_JD (1U << 17) +#define GMAC_MAC_CONFIGURATION_JE (1U << 16) +#define GMAC_MAC_CONFIGURATION_PS (1U << 15) +#define GMAC_MAC_CONFIGURATION_FES (1U << 14) +#define GMAC_MAC_CONFIGURATION_DM (1U << 13) +#define GMAC_MAC_CONFIGURATION_DCRS (1U << 9) +#define GMAC_MAC_CONFIGURATION_TE (1U << 1) +#define GMAC_MAC_CONFIGURATION_RE (1U << 0) +#define GMAC_MAC_EXT_CONFIGURATION 0x0004 +#define GMAC_MAC_PACKET_FILTER 0x0008 +#define GMAC_MAC_PACKET_FILTER_HPF (1U << 10) +#define GMAC_MAC_PACKET_FILTER_PCF_MASK (3U << 6) +#define GMAC_MAC_PACKET_FILTER_PCF_ALL (2U << 6) +#define GMAC_MAC_PACKET_FILTER_DBF (1U << 5) +#define GMAC_MAC_PACKET_FILTER_PM (1U << 4) +#define GMAC_MAC_PACKET_FILTER_HMC (1U << 2) +#define GMAC_MAC_PACKET_FILTER_HUC (1U << 1) +#define GMAC_MAC_PACKET_FILTER_PR (1U << 0) +#define GMAC_MAC_WATCHDOG_TIMEOUT 0x000C +#define GMAC_MAC_HASH_TABLE_REG0 0x0010 +#define GMAC_MAC_HASH_TABLE_REG1 0x0014 +#define GMAC_MAC_VLAN_TAG 0x0050 +#define GMAC_MAC_Q0_TX_FLOW_CTRL 0x0070 +#define GMAC_MAC_Q0_TX_FLOW_CTRL_PT_SHIFT 16 +#define GMAC_MAC_Q0_TX_FLOW_CTRL_TFE (1U << 1) +#define GMAC_MAC_RX_FLOW_CTRL 0x0090 +#define GMAC_MAC_RX_FLOW_CTRL_RFE (1U << 0) +#define GMAC_RXQ_CTRL0 0x00A0 +#define GMAC_RXQ_CTRL0_EN_MASK 0x3 +#define GMAC_RXQ_CTRL0_EN_DCB 0x2 +#define GMAC_RXQ_CTRL1 0x00A4 +#define GMAC_MAC_INTERRUPT_STATUS 0x00B0 +#define GMAC_MAC_INTERRUPT_ENABLE 0x00B4 +#define GMAC_MAC_RX_TX_STATUS 0x00B8 +#define GMAC_MAC_RX_TX_STATUS_RWT (1U << 8) +#define GMAC_MAC_RX_TX_STATUS_EXCOL (1U << 5) +#define GMAC_MAC_RX_TX_STATUS_LCOL (1U << 4) +#define GMAC_MAC_RX_TX_STATUS_EXDEF (1U << 3) +#define GMAC_MAC_RX_TX_STATUS_LCARR (1U << 2) +#define GMAC_MAC_RX_TX_STATUS_NCARR (1U << 1) +#define GMAC_MAC_RX_TX_STATUS_TJT (1U << 0) +#define GMAC_MAC_PMT_CONTROL_STATUS 0x00C0 +#define GMAC_MAC_RWK_PACKET_FILTER 0x00C4 +#define GMAC_MAC_LPI_CONTROL_STATUS 0x00D0 +#define GMAC_MAC_LPI_TIMERS_CONTROL 0x00D4 +#define GMAC_MAC_LPI_ENTRY_TIMER 0x00D8 +#define GMAC_MAC_1US_TIC_COUNTER 0x00DC +#define GMAC_MAC_PHYIF_CONTROL_STATUS 0x00F8 +#define GMAC_MAC_VERSION 0x0110 +#define GMAC_MAC_VERSION_USERVER_SHIFT 8 +#define GMAC_MAC_VERSION_USERVER_MASK (0xFFU << GMAC_MAC_VERSION_USERVER_SHIFT) +#define GMAC_MAC_VERSION_SNPSVER_MASK 0xFFU +#define GMAC_MAC_DEBUG 0x0114 +#define GMAC_MAC_HW_FEATURE(n) (0x011C + 0x4 * (n)) +#define GMAC_MAC_HW_FEATURE1_ADDR64_SHIFT 14 +#define GMAC_MAC_HW_FEATURE1_ADDR64_MASK (0x3U << GMAC_MAC_HW_FEATURE1_ADDR64_SHIFT) +#define GMAC_MAC_HW_FEATURE1_ADDR64_32BIT (0x0U << GMAC_MAC_HW_FEATURE1_ADDR64_SHIFT) +#define GMAC_MAC_MDIO_ADDRESS 0x0200 +#define GMAC_MAC_MDIO_ADDRESS_PA_SHIFT 21 +#define GMAC_MAC_MDIO_ADDRESS_RDA_SHIFT 16 +#define GMAC_MAC_MDIO_ADDRESS_CR_SHIFT 8 +#define GMAC_MAC_MDIO_ADDRESS_CR_MASK (0x7U << GMAC_MAC_MDIO_ADDRESS_CR_SHIFT) +#define GMAC_MAC_MDIO_ADDRESS_CR_60_100 (0U << GMAC_MAC_MDIO_ADDRESS_CR_SHIFT) +#define GMAC_MAC_MDIO_ADDRESS_CR_100_150 (1U << GMAC_MAC_MDIO_ADDRESS_CR_SHIFT) +#define GMAC_MAC_MDIO_ADDRESS_CR_20_35 (2U << GMAC_MAC_MDIO_ADDRESS_CR_SHIFT) +#define GMAC_MAC_MDIO_ADDRESS_CR_35_60 (3U << GMAC_MAC_MDIO_ADDRESS_CR_SHIFT) +#define GMAC_MAC_MDIO_ADDRESS_CR_150_250 (4U << GMAC_MAC_MDIO_ADDRESS_CR_SHIFT) +#define GMAC_MAC_MDIO_ADDRESS_CR_250_300 (5U << GMAC_MAC_MDIO_ADDRESS_CR_SHIFT) +#define GMAC_MAC_MDIO_ADDRESS_CR_300_500 (6U << GMAC_MAC_MDIO_ADDRESS_CR_SHIFT) +#define GMAC_MAC_MDIO_ADDRESS_CR_500_800 (7U << GMAC_MAC_MDIO_ADDRESS_CR_SHIFT) +#define GMAC_MAC_MDIO_ADDRESS_SKAP (1U << 4) +#define GMAC_MAC_MDIO_ADDRESS_GOC_SHIFT 2 +#define GMAC_MAC_MDIO_ADDRESS_GOC_READ (3U << GMAC_MAC_MDIO_ADDRESS_GOC_SHIFT) +#define GMAC_MAC_MDIO_ADDRESS_GOC_WRITE (1U << GMAC_MAC_MDIO_ADDRESS_GOC_SHIFT) +#define GMAC_MAC_MDIO_ADDRESS_C45E (1U << 1) +#define GMAC_MAC_MDIO_ADDRESS_GB (1U << 0) +#define GMAC_MAC_MDIO_DATA 0x0204 +#define GMAC_MAC_CSR_SW_CTRL 0x0230 +#define GMAC_MAC_ADDRESS0_HIGH 0x0300 +#define GMAC_MAC_ADDRESS0_LOW 0x0304 +#define GMAC_MMC_CONTROL 0x0700 +#define GMAC_MMC_CONTROL_UCDBC (1U << 8) +#define GMAC_MMC_CONTROL_CNTPRSTLVL (1U << 5) +#define GMAC_MMC_CONTROL_CNTPRST (1U << 4) +#define GMAC_MMC_CONTROL_CNTFREEZ (1U << 3) +#define GMAC_MMC_CONTROL_RSTONRD (1U << 2) +#define GMAC_MMC_CONTROL_CNTSTOPRO (1U << 1) +#define GMAC_MMC_CONTROL_CNTRST (1U << 0) +#define GMAC_MMC_RX_INTERRUPT 0x0704 +#define GMAC_MMC_TX_INTERRUPT 0x0708 +#define GMAC_MMC_RX_INTERRUPT_MASK 0x070C +#define GMAC_MMC_TX_INTERRUPT_MASK 0x0710 +#define GMAC_TX_OCTET_COUNT_GOOD_BAD 0x0714 +#define GMAC_TX_PACKET_COUNT_GOOD_BAD 0x0718 +#define GMAC_TX_UNDERFLOW_ERROR_PACKETS 0x0748 +#define GMAC_TX_CARRIER_ERROR_PACKETS 0x0760 +#define GMAC_TX_OCTET_COUNT_GOOD 0x0764 +#define GMAC_TX_PACKET_COUNT_GOOD 0x0768 +#define GMAC_RX_PACKETS_COUNT_GOOD_BAD 0x0780 +#define GMAC_RX_OCTET_COUNT_GOOD_BAD 0x0784 +#define GMAC_RX_OCTET_COUNT_GOOD 0x0788 +#define GMAC_RX_MULTICAST_PACKETS_GOOD 0x0790 +#define GMAC_RX_CRC_ERROR_PACKETS 0x0794 +#define GMAC_RX_LENGTH_ERROR_PACKETS 0x07C8 +#define GMAC_RX_FIFO_OVERFLOW_PACKETS 0x07D4 +#define GMAC_MMC_IPC_RX_INTERRUPT_MASK 0x0800 +#define GMAC_MMC_IPC_RX_INTERRUPT 0x0808 +#define GMAC_RXIPV4_GOOD_PACKETS 0x0810 +#define GMAC_RXIPV4_HEADER_ERROR_PACKETS 0x0814 +#define GMAC_RXIPV6_GOOD_PACKETS 0x0824 +#define GMAC_RXIPV6_HEADER_ERROR_PACKETS 0x0828 +#define GMAC_RXUDP_ERROR_PACKETS 0x0834 +#define GMAC_RXTCP_ERROR_PACKETS 0x083C +#define GMAC_RXICMP_ERROR_PACKETS 0x0844 +#define GMAC_RXIPV4_HEADER_ERROR_OCTETS 0x0854 +#define GMAC_RXIPV6_HEADER_ERROR_OCTETS 0x0868 +#define GMAC_RXUDP_ERROR_OCTETS 0x0874 +#define GMAC_RXTCP_ERROR_OCTETS 0x087C +#define GMAC_RXICMP_ERROR_OCTETS 0x0884 +#define GMAC_MAC_TIMESTAMP_CONTROL 0x0B00 +#define GMAC_MAC_SUB_SECOND_INCREMENT 0x0B04 +#define GMAC_MAC_SYSTEM_TIME_SECS 0x0B08 +#define GMAC_MAC_SYSTEM_TIME_NS 0x0B0C +#define GMAC_MAC_SYS_TIME_SECS_UPDATE 0x0B10 +#define GMAC_MAC_SYS_TIME_NS_UPDATE 0x0B14 +#define GMAC_MAC_TIMESTAMP_ADDEND 0x0B18 +#define GMAC_MAC_TIMESTAMP_STATUS 0x0B20 +#define GMAC_MAC_TX_TS_STATUS_NS 0x0B30 +#define GMAC_MAC_TX_TS_STATUS_SECS 0x0B34 +#define GMAC_MAC_AUXILIARY_CONTROL 0x0B40 +#define GMAC_MAC_AUXILIARY_TS_NS 0x0B48 +#define GMAC_MAC_AUXILIARY_TS_SECS 0x0B4C +#define GMAC_MAC_TS_INGRESS_CORR_NS 0x0B58 +#define GMAC_MAC_TS_EGRESS_CORR_NS 0x0B5C +#define GMAC_MAC_TS_INGRESS_LATENCY 0x0B68 +#define GMAC_MAC_TS_EGRESS_LATENCY 0x0B6C +#define GMAC_MAC_PPS_CONTROL 0x0B70 +#define GMAC_MTL_DBG_CTL 0x0C08 +#define GMAC_MTL_DBG_STS 0x0C0C +#define GMAC_MTL_FIFO_DEBUG_DATA 0x0C10 +#define GMAC_MTL_INTERRUPT_STATUS 0x0C20 +#define GMAC_MTL_INTERRUPT_STATUS_DBGIS (1U << 17) +#define GMAC_MTL_INTERRUPT_STATUS_Q0IS (1U << 0) +#define GMAC_MTL_TXQ0_OPERATION_MODE 0x0D00 +#define GMAC_MTL_TXQ0_OPERATION_MODE_TXQEN_SHIFT 2 +#define GMAC_MTL_TXQ0_OPERATION_MODE_TXQEN_MASK (0x3U << GMAC_MTL_TXQ0_OPERATION_MODE_TXQEN_SHIFT) +#define GMAC_MTL_TXQ0_OPERATION_MODE_TXQEN_EN (2U << GMAC_MTL_TXQ0_OPERATION_MODE_TXQEN_SHIFT) +#define GMAC_MTL_TXQ0_OPERATION_MODE_TSF (1U << 1) +#define GMAC_MTL_TXQ0_OPERATION_MODE_FTQ (1U << 0) +#define GMAC_MTL_TXQ0_UNDERFLOW 0x0D04 +#define GMAC_MTL_TXQ0_DEBUG 0x0D08 +#define GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS 0x0D2C +#define GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS_RXOIE (1U << 24) +#define GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS_RXOVFIS (1U << 16) +#define GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS_TXUIE (1U << 8) +#define GMAC_MTL_Q0_INTERRUPT_CTRL_STATUS_TXUNFIS (1U << 0) +#define GMAC_MTL_RXQ0_OPERATION_MODE 0x0D30 +#define GMAC_MTL_RXQ0_OPERATION_MODE_RSF (1U << 5) +#define GMAC_MTL_RXQ0_OPERATION_MODE_FEP (1U << 4) +#define GMAC_MTL_RXQ0_OPERATION_MODE_FUP (1U << 3) +#define GMAC_MTL_RXQ0_MISS_PKT_OVF_CNT 0x0D34 +#define GMAC_MTL_RXQ0_DEBUG 0x0D38 +#define GMAC_DMA_MODE 0x1000 +#define GMAC_DMA_MODE_SWR (1U << 0) +#define GMAC_DMA_SYSBUS_MODE 0x1004 +#define GMAC_DMA_SYSBUS_MODE_WR_OSR_LMT_SHIFT 24 +#define GMAC_DMA_SYSBUS_MODE_WR_OSR_LMT_MASK (0x3U << GMAC_DMA_SYSBUS_MODE_WR_OSR_LMT_SHIFT) +#define GMAC_DMA_SYSBUS_MODE_RD_OSR_LMT_SHIFT 16 +#define GMAC_DMA_SYSBUS_MODE_RD_OSR_LMT_MASK (0x7U << GMAC_DMA_SYSBUS_MODE_RD_OSR_LMT_SHIFT) +#define GMAC_DMA_SYSBUS_MODE_MB (1U << 14) +#define GMAC_DMA_SYSBUS_MODE_EAME (1U << 11) +#define GMAC_DMA_SYSBUS_MODE_BLEN16 (1U << 3) +#define GMAC_DMA_SYSBUS_MODE_BLEN8 (1U << 2) +#define GMAC_DMA_SYSBUS_MODE_BLEN4 (1U << 1) +#define GMAC_DMA_SYSBUS_MODE_FB (1U << 0) +#define GMAC_DMA_INTERRUPT_STATUS 0x1008 +#define GMAC_DMA_DEBUG_STATUS0 0x100C +#define GMAC_AXI_LPI_ENTRY_INTERVAL 0x1040 +#define GMAC_RWK_FILTERn_BYTE_MASK(n) (0x10C0 + 0x4 * (n)) +#define GMAC_RWK_FILTER01_CRC 0x10D0 +#define GMAC_RWK_FILTER23_CRC 0x10D4 +#define GMAC_RWK_FILTER_OFFSET 0x10D8 +#define GMAC_RWK_FILTER_COMMAND 0x10DC +#define GMAC_DMA_CHAN0_CONTROL 0x1100 +#define GMAC_DMA_CHAN0_CONTROL_DSL_SHIFT 18 +#define GMAC_DMA_CHAN0_CONTROL_DSL_MASK (0x7U << GMAC_DMA_CHAN0_CONTROL_DSL_SHIFT) +#define GMAC_DMA_CHAN0_CONTROL_PBLX8 (1U << 16) +#define GMAC_DMA_CHAN0_TX_CONTROL 0x1104 +#define GMAC_DMA_CHAN0_TX_CONTROL_OSP (1U << 4) +#define GMAC_DMA_CHAN0_TX_CONTROL_START (1U << 0) +#define GMAC_DMA_CHAN0_RX_CONTROL 0x1108 +#define GMAC_DMA_CHAN0_RX_CONTROL_RBSZ_SHIFT 1 +#define GMAC_DMA_CHAN0_RX_CONTROL_RBSZ_MASK (0x3FFFU << GMAC_DMA_CHAN0_RX_CONTROL_RBSZ_SHIFT) +#define GMAC_DMA_CHAN0_RX_CONTROL_START (1U << 0) +#define GMAC_DMA_CHAN0_TX_BASE_ADDR_HI 0x1110 +#define GMAC_DMA_CHAN0_TX_BASE_ADDR 0x1114 +#define GMAC_DMA_CHAN0_RX_BASE_ADDR_HI 0x1118 +#define GMAC_DMA_CHAN0_RX_BASE_ADDR 0x111C +#define GMAC_DMA_CHAN0_TX_END_ADDR 0x1120 +#define GMAC_DMA_CHAN0_RX_END_ADDR 0x1128 +#define GMAC_DMA_CHAN0_TX_RING_LEN 0x112C +#define GMAC_DMA_CHAN0_RX_RING_LEN 0x1130 +#define GMAC_DMA_CHAN0_INTR_ENABLE 0x1134 +#define GMAC_DMA_CHAN0_INTR_ENABLE_NIE (1U << 15) +#define GMAC_DMA_CHAN0_INTR_ENABLE_AIE (1U << 14) +#define GMAC_DMA_CHAN0_INTR_ENABLE_CDE (1U << 13) +#define GMAC_DMA_CHAN0_INTR_ENABLE_FBE (1U << 12) +#define GMAC_DMA_CHAN0_INTR_ENABLE_ERI (1U << 11) +#define GMAC_DMA_CHAN0_INTR_ENABLE_ETI (1U << 10) +#define GMAC_DMA_CHAN0_INTR_ENABLE_RWT (1U << 9) +#define GMAC_DMA_CHAN0_INTR_ENABLE_RPS (1U << 8) +#define GMAC_DMA_CHAN0_INTR_ENABLE_RBU (1U << 7) +#define GMAC_DMA_CHAN0_INTR_ENABLE_RIE (1U << 6) +#define GMAC_DMA_CHAN0_INTR_ENABLE_TPU (1U << 2) +#define GMAC_DMA_CHAN0_INTR_ENABLE_TPS (1U << 1) +#define GMAC_DMA_CHAN0_INTR_ENABLE_TIE (1U << 0) +#define GMAC_DMA_CHAN0_RX_WATCHDOG 0x1138 +#define GMAC_DMA_CHAN0_SLOT_CTRL_STATUS 0x113C +#define GMAC_DMA_CHAN0_CUR_TX_DESC 0x1144 +#define GMAC_DMA_CHAN0_CUR_RX_DESC 0x114C +#define GMAC_DMA_CHAN0_CUR_TX_BUF_ADDR 0x1154 +#define GMAC_DMA_CHAN0_CUR_RX_BUF_ADDR 0x115C +#define GMAC_DMA_CHAN0_STATUS 0x1160 +#define GMAC_DMA_CHAN0_STATUS_NIS (1U << 15) +#define GMAC_DMA_CHAN0_STATUS_AIS (1U << 14) +#define GMAC_DMA_CHAN0_STATUS_CDE (1U << 13) +#define GMAC_DMA_CHAN0_STATUS_FB (1U << 12) +#define GMAC_DMA_CHAN0_STATUS_ERI (1U << 11) +#define GMAC_DMA_CHAN0_STATUS_ETI (1U << 10) +#define GMAC_DMA_CHAN0_STATUS_RWT (1U << 9) +#define GMAC_DMA_CHAN0_STATUS_RPS (1U << 8) +#define GMAC_DMA_CHAN0_STATUS_RBU (1U << 7) +#define GMAC_DMA_CHAN0_STATUS_RI (1U << 6) +#define GMAC_DMA_CHAN0_STATUS_TPU (1U << 2) +#define GMAC_DMA_CHAN0_STATUS_TPS (1U << 1) +#define GMAC_DMA_CHAN0_STATUS_TI (1U << 0) + +#define EQOS_TDES2_IOC (1U << 31) +#define EQOS_TDES3_OWN (1U << 31) +#define EQOS_TDES3_FD (1U << 29) +#define EQOS_TDES3_LD (1U << 28) +#define EQOS_TDES3_DE (1U << 23) +#define EQOS_TDES3_OE (1U << 21) +#define EQOS_TDES3_ES (1U << 15) + +#define EQOS_RDES3_OWN (1U << 31) +#define EQOS_RDES3_IOC (1U << 30) +#define EQOS_RDES3_BUF1V (1U << 24) +#define EQOS_RDES3_GP (1U << 23) +#define EQOS_RDES3_OE (1U << 21) +#define EQOS_RDES3_RE (1U << 20) +#define EQOS_RDES3_LENGTH_MASK 0x7FFFU + +#endif Index: sys/dev/eqos/eqos_var.h =================================================================== --- sys/dev/eqos/eqos_var.h +++ sys/dev/eqos/eqos_var.h @@ -0,0 +1,99 @@ +/*- + * SPDX-License-Identifier: BSD-2-Clause-FreeBSD + * + * Copyright (c) 2022 Soren Schmidt + * Copyright (c) 2022 Jared McNeill + * All rights reserved. + * + * 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 ``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 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. + * + * $Id$ + */ + +/* + * DesignWare Ethernet Quality-of-Service controller + */ + +#ifndef _EQOS_VAR_H +#define _EQOS_VAR_H + +#include + +#define EQOS_DMA_DESC_COUNT 256 + +#define EQOS_RES_MEM 0 +#define EQOS_RES_IRQ0 1 +#define EQOS_RES_COUNT 2 + +#define EQOS_INTR_FLAGS (INTR_TYPE_NET | INTR_MPSAFE) + +struct eqos_dma_desc { + uint32_t tdes0; + uint32_t tdes1; + uint32_t tdes2; + uint32_t tdes3; +} __packed; + +struct eqos_bufmap { + bus_dmamap_t map; + struct mbuf *mbuf; +}; + +struct eqos_ring { + bus_dma_tag_t desc_tag; + bus_dmamap_t desc_map; + struct eqos_dma_desc *desc_ring; + bus_addr_t desc_ring_paddr; + + bus_dma_tag_t buf_tag; + struct eqos_bufmap buf_map[EQOS_DMA_DESC_COUNT]; + + u_int head; + u_int tail; +}; + +struct eqos_softc { + device_t dev; + struct resource *res[EQOS_RES_COUNT]; + void *irq_handle; +#ifdef FDT + struct syscon *grf; + clk_t clk_mac_speed; +#endif + uint32_t csr_clock; + uint32_t csr_clock_range; + uint32_t hw_feature[4]; + bool link_up; + int tx_watchdog; + + struct ifnet *ifp; + device_t miibus; + struct mtx lock; + struct callout callout; + + struct eqos_ring tx; + struct eqos_ring rx; +}; + +DECLARE_CLASS(eqos_driver); + +#endif Index: sys/dev/mii/mcommphy.c =================================================================== --- sys/dev/mii/mcommphy.c +++ sys/dev/mii/mcommphy.c @@ -0,0 +1,183 @@ +/* + * Copyright (c) 2022 Jared McNeill + * Copyright (c) 2022 Soren Schmidt + * All rights reserved. + * + * 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 NETBSD FOUNDATION, INC. 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 FOUNDATION 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. + */ + +/* + * Motorcomm YT8511C / YT8511H Integrated 10/100/1000 Gigabit Ethernet phy + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "miibus_if.h" + +#define MCOMMPHY_OUI 0x000000 +#define MCOMMPHY_MODEL 0x10 +#define MCOMMPHY_REV 0x0a + +#define EXT_REG_ADDR 0x1e +#define EXT_REG_DATA 0x1f + +/* Extended registers */ +#define PHY_CLOCK_GATING_REG 0x0c +#define RX_CLK_DELAY_EN 0x0001 +#define CLK_25M_SEL 0x0006 +#define TX_CLK_DELAY_SEL 0x00f0 +#define PHY_SLEEP_CONTROL1_REG 0x27 +#define PLLON_IN_SLP 0x4000 + + +static int +mcommphy_service(struct mii_softc *sc, struct mii_data *mii, int cmd) +{ + switch (cmd) { + case MII_POLLSTAT: + break; + + case MII_MEDIACHG: + mii_phy_setmedia(sc); + break; + + case MII_TICK: + if (mii_phy_tick(sc) == EJUSTRETURN) + return 0; + break; + } + + /* Update the media status. */ + PHY_STATUS(sc); + + /* Callback if something changed. */ + mii_phy_update(sc, cmd); + + return (0); +} + +static const struct mii_phy_funcs mcommphy_funcs = { + mcommphy_service, + ukphy_status, + mii_phy_reset +}; + +static int +mcommphy_probe(device_t dev) +{ + struct mii_attach_args *ma = device_get_ivars(dev); + + /* + * The YT8511C reports an OUI of 0. Best we can do here is to match + * exactly the contents of the PHY identification registers. + */ + if (MII_OUI(ma->mii_id1, ma->mii_id2) == MCOMMPHY_OUI && + MII_MODEL(ma->mii_id2) == MCOMMPHY_MODEL && + MII_REV(ma->mii_id2) == MCOMMPHY_REV) { + device_set_desc(dev, "Motorcomm YT8511 media interface"); + return BUS_PROBE_DEFAULT; + } + return ENXIO; +} + +static int +mcommphy_attach(device_t dev) +{ + struct mii_softc *sc = device_get_softc(dev); + uint16_t oldaddr, data; + + mii_phy_dev_attach(dev, MIIF_NOMANPAUSE, &mcommphy_funcs, 0); + + PHY_RESET(sc); + + /* begin chip stuff */ + oldaddr = PHY_READ(sc, EXT_REG_ADDR); + + PHY_WRITE(sc, EXT_REG_ADDR, PHY_CLOCK_GATING_REG); + data = PHY_READ(sc, EXT_REG_DATA); + data &= ~CLK_25M_SEL; + data |= 0x06; + if (sc->mii_flags & MIIF_RX_DELAY) { + data |= RX_CLK_DELAY_EN; + } else { + data &= ~RX_CLK_DELAY_EN; + } + data &= ~TX_CLK_DELAY_SEL; + if (sc->mii_flags & MIIF_TX_DELAY) { + data |= 0xf0; + } else { + data |= 0x20; + } + PHY_WRITE(sc, EXT_REG_DATA, data); + + PHY_WRITE(sc, EXT_REG_ADDR, PHY_SLEEP_CONTROL1_REG); + data = PHY_READ(sc, EXT_REG_DATA); + data |= PLLON_IN_SLP; + PHY_WRITE(sc, EXT_REG_DATA, data); + + PHY_WRITE(sc, EXT_REG_ADDR, oldaddr); + /* end chip stuff */ + + sc->mii_capabilities = PHY_READ(sc, MII_BMSR) & sc->mii_capmask; + if (sc->mii_capabilities & BMSR_EXTSTAT) + sc->mii_extcapabilities = PHY_READ(sc, MII_EXTSR); + device_printf(dev, " "); + mii_phy_add_media(sc); + printf("\n"); + + MIIBUS_MEDIAINIT(sc->mii_dev); + + return (0); +} + + +static device_method_t mcommphy_methods[] = { + /* device interface */ + DEVMETHOD(device_probe, mcommphy_probe), + DEVMETHOD(device_attach, mcommphy_attach), + DEVMETHOD(device_detach, mii_phy_detach), + DEVMETHOD(device_shutdown, bus_generic_shutdown), + DEVMETHOD_END +}; + +static devclass_t mcommphy_devclass; + +static driver_t mcommphy_driver = { + "mcommphy", + mcommphy_methods, + sizeof(struct mii_softc) +}; + +DRIVER_MODULE(mcommphy, miibus, mcommphy_driver, mcommphy_devclass, 0, 0); Index: sys/dev/mii/miidevs =================================================================== --- sys/dev/mii/miidevs +++ sys/dev/mii/miidevs @@ -63,6 +63,7 @@ oui LEVEL1 0x00207b Level 1 oui MARVELL 0x005043 Marvell Semiconductor oui MICREL 0x0010a1 Micrel +oui MOTORCOMM 0x000000 Motorcomm oui MYSON 0x00c0b4 Myson Technology oui NATSEMI 0x080017 National Semiconductor oui PMCSIERRA 0x00e004 PMC-Sierra @@ -290,6 +291,9 @@ model MICREL KSZ9021 0x0021 Micrel KSZ9021 10/100/1000 PHY model MICREL KSZ9031 0x0022 Micrel KSZ9031 10/100/1000 PHY +/* Motorcomm PHYs */ +model MOTORCOMM YT8511 0x010a Motorcomm YT8511 10/100/1000 PHY + /* Myson Technology PHYs */ model xxMYSON MTD972 0x0000 MTD972 10/100 media interface model MYSON MTD803 0x0000 MTD803 3-in-1 media interface