Date: Thu, 25 Sep 2014 18:03:14 +0000 (UTC) From: Ruslan Bukin <br@FreeBSD.org> To: src-committers@freebsd.org, svn-src-all@freebsd.org, svn-src-head@freebsd.org Subject: svn commit: r272120 - in head/sys: arm/altera/socfpga arm/conf boot/fdt/dts/arm dev/dwc Message-ID: <201409251803.s8PI3E5D010096@svn.freebsd.org>
next in thread | raw e-mail | index | archive | help
Author: br Date: Thu Sep 25 18:03:14 2014 New Revision: 272120 URL: http://svnweb.freebsd.org/changeset/base/272120 Log: Add driver for Synopsys DesignWare 3504-0 Universal 10/100/1000 Ethernet MAC. Sponsored by: DARPA, AFRL Added: head/sys/dev/dwc/ head/sys/dev/dwc/if_dwc.c (contents, props changed) head/sys/dev/dwc/if_dwc.h (contents, props changed) Modified: head/sys/arm/altera/socfpga/files.socfpga head/sys/arm/conf/SOCKIT head/sys/boot/fdt/dts/arm/socfpga-sockit.dts head/sys/boot/fdt/dts/arm/socfpga.dtsi Modified: head/sys/arm/altera/socfpga/files.socfpga ============================================================================== --- head/sys/arm/altera/socfpga/files.socfpga Thu Sep 25 17:59:00 2014 (r272119) +++ head/sys/arm/altera/socfpga/files.socfpga Thu Sep 25 18:03:14 2014 (r272120) @@ -17,3 +17,5 @@ arm/altera/socfpga/socfpga_common.c sta arm/altera/socfpga/socfpga_machdep.c standard arm/altera/socfpga/socfpga_manager.c standard arm/altera/socfpga/socfpga_rstmgr.c standard + +dev/dwc/if_dwc.c optional dwc Modified: head/sys/arm/conf/SOCKIT ============================================================================== --- head/sys/arm/conf/SOCKIT Thu Sep 25 17:59:00 2014 (r272119) +++ head/sys/arm/conf/SOCKIT Thu Sep 25 18:03:14 2014 (r272120) @@ -124,6 +124,7 @@ device ether device mii device smsc device smscphy +device dwc # USB ethernet support, requires miibus device miibus Modified: head/sys/boot/fdt/dts/arm/socfpga-sockit.dts ============================================================================== --- head/sys/boot/fdt/dts/arm/socfpga-sockit.dts Thu Sep 25 17:59:00 2014 (r272119) +++ head/sys/boot/fdt/dts/arm/socfpga-sockit.dts Thu Sep 25 18:03:14 2014 (r272120) @@ -51,6 +51,10 @@ usb1: usb@ffb40000 { status = "okay"; }; + + gmac1: ethernet@ff702000 { + status = "okay"; + }; }; chosen { Modified: head/sys/boot/fdt/dts/arm/socfpga.dtsi ============================================================================== --- head/sys/boot/fdt/dts/arm/socfpga.dtsi Thu Sep 25 17:59:00 2014 (r272119) +++ head/sys/boot/fdt/dts/arm/socfpga.dtsi Thu Sep 25 18:03:14 2014 (r272120) @@ -71,6 +71,11 @@ interrupt-parent = < &GIC >; }; + sysmgr: sysmgr@ffd08000 { + compatible = "altr,sys-mgr"; + reg = <0xffd08000 0x1000>; + }; + rstmgr: rstmgr@ffd05000 { compatible = "altr,rst-mgr"; reg = <0xffd05000 0x1000>; @@ -127,5 +132,25 @@ dr_mode = "host"; status = "disabled"; }; + + gmac0: ethernet@ff700000 { + compatible = "altr,socfpga-stmmac", + "snps,dwmac-3.70a", "snps,dwmac"; + reg = <0xff700000 0x2000>; + interrupts = <147>; + interrupt-parent = <&GIC>; + phy-mode = "rgmii"; + status = "disabled"; + }; + + gmac1: ethernet@ff702000 { + compatible = "altr,socfpga-stmmac", + "snps,dwmac-3.70a", "snps,dwmac"; + reg = <0xff702000 0x2000>; + interrupts = <152>; + interrupt-parent = <&GIC>; + phy-mode = "rgmii"; + status = "disabled"; + }; }; }; Added: head/sys/dev/dwc/if_dwc.c ============================================================================== --- /dev/null 00:00:00 1970 (empty, because file is newly added) +++ head/sys/dev/dwc/if_dwc.c Thu Sep 25 18:03:14 2014 (r272120) @@ -0,0 +1,1324 @@ +/*- + * Copyright (c) 2014 Ruslan Bukin <br@bsdpad.com> + * All rights reserved. + * + * This software was developed by SRI International and the University of + * Cambridge Computer Laboratory under DARPA/AFRL contract (FA8750-10-C-0237) + * ("CTSRD"), as part of the DARPA CRASH research programme. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + */ + +/* + * Ethernet media access controller (EMAC) + * Chapter 17, Altera Cyclone V Device Handbook (CV-5V2 2014.07.22) + * + * EMAC is an instance of the Synopsys DesignWare 3504-0 + * Universal 10/100/1000 Ethernet MAC (DWC_gmac). + */ + +#include <sys/cdefs.h> +__FBSDID("$FreeBSD$"); + +#include <sys/param.h> +#include <sys/systm.h> +#include <sys/bus.h> +#include <sys/kernel.h> +#include <sys/module.h> +#include <sys/malloc.h> +#include <sys/rman.h> +#include <sys/timeet.h> +#include <sys/timetc.h> +#include <sys/endian.h> +#include <sys/lock.h> +#include <sys/mbuf.h> +#include <sys/mutex.h> +#include <sys/socket.h> +#include <sys/sockio.h> +#include <sys/sysctl.h> + +#include <dev/fdt/fdt_common.h> +#include <dev/ofw/openfirm.h> +#include <dev/ofw/ofw_bus.h> +#include <dev/ofw/ofw_bus_subr.h> + +#include <net/bpf.h> +#include <net/if.h> +#include <net/ethernet.h> +#include <net/if_dl.h> +#include <net/if_media.h> +#include <net/if_types.h> +#include <net/if_var.h> +#include <net/if_vlan_var.h> + +#include <machine/bus.h> +#include <machine/fdt.h> +#include <machine/cpu.h> +#include <machine/intr.h> + +#include <dev/mii/mii.h> +#include <dev/mii/miivar.h> +#include "miibus_if.h" + +#define READ4(_sc, _reg) \ + bus_read_4((_sc)->res[0], _reg) +#define WRITE4(_sc, _reg, _val) \ + bus_write_4((_sc)->res[0], _reg, _val) + +#define WATCHDOG_TIMEOUT_SECS 5 +#define STATS_HARVEST_INTERVAL 2 +#define MII_CLK_VAL 2 + +#include <dev/dwc/if_dwc.h> + +#define DWC_LOCK(sc) mtx_lock(&(sc)->mtx) +#define DWC_UNLOCK(sc) mtx_unlock(&(sc)->mtx) +#define DWC_ASSERT_LOCKED(sc) mtx_assert(&(sc)->mtx, MA_OWNED); +#define DWC_ASSERT_UNLOCKED(sc) mtx_assert(&(sc)->mtx, MA_NOTOWNED); + +#define DDESC_TDES0_OWN (1 << 31) +#define DDESC_TDES0_TXINT (1 << 30) +#define DDESC_TDES0_TXLAST (1 << 29) +#define DDESC_TDES0_TXFIRST (1 << 28) +#define DDESC_TDES0_TXCRCDIS (1 << 27) +#define DDESC_TDES0_TXRINGEND (1 << 21) +#define DDESC_TDES0_TXCHAIN (1 << 20) + +#define DDESC_RDES0_OWN (1 << 31) +#define DDESC_RDES0_FL_MASK 0x3fff +#define DDESC_RDES0_FL_SHIFT 16 /* Frame Length */ +#define DDESC_RDES1_CHAINED (1 << 14) + +struct dwc_bufmap { + bus_dmamap_t map; + struct mbuf *mbuf; +}; + +/* + * A hardware buffer descriptor. Rx and Tx buffers have the same descriptor + * layout, but the bits in the flags field have different meanings. + */ +struct dwc_hwdesc +{ + uint32_t tdes0; + uint32_t tdes1; + uint32_t addr; /* pointer to buffer data */ + uint32_t addr_next; /* link to next descriptor */ +}; + +/* + * Driver data and defines. + */ +#define RX_DESC_COUNT 1024 +#define RX_DESC_SIZE (sizeof(struct dwc_hwdesc) * RX_DESC_COUNT) +#define TX_DESC_COUNT 1024 +#define TX_DESC_SIZE (sizeof(struct dwc_hwdesc) * TX_DESC_COUNT) + +/* + * The hardware imposes alignment restrictions on various objects involved in + * DMA transfers. These values are expressed in bytes (not bits). + */ +#define DWC_DESC_RING_ALIGN 2048 + +struct dwc_softc { + struct resource *res[2]; + bus_space_tag_t bst; + bus_space_handle_t bsh; + device_t dev; + int mii_clk; + device_t miibus; + struct mii_data * mii_softc; + struct ifnet *ifp; + int if_flags; + struct mtx mtx; + void * intr_cookie; + struct callout dwc_callout; + uint8_t phy_conn_type; + uint8_t mactype; + boolean_t link_is_up; + boolean_t is_attached; + boolean_t is_detaching; + int tx_watchdog_count; + int stats_harvest_count; + + /* RX */ + bus_dma_tag_t rxdesc_tag; + bus_dmamap_t rxdesc_map; + struct dwc_hwdesc *rxdesc_ring; + bus_addr_t rxdesc_ring_paddr; + bus_dma_tag_t rxbuf_tag; + struct dwc_bufmap rxbuf_map[RX_DESC_COUNT]; + uint32_t rx_idx; + + /* TX */ + bus_dma_tag_t txdesc_tag; + bus_dmamap_t txdesc_map; + struct dwc_hwdesc *txdesc_ring; + bus_addr_t txdesc_ring_paddr; + bus_dma_tag_t txbuf_tag; + struct dwc_bufmap txbuf_map[RX_DESC_COUNT]; + uint32_t tx_idx_head; + uint32_t tx_idx_tail; + int txcount; +}; + +static struct resource_spec dwc_spec[] = { + { SYS_RES_MEMORY, 0, RF_ACTIVE }, + { SYS_RES_IRQ, 0, RF_ACTIVE }, + { -1, 0 } +}; + +static void dwc_txfinish_locked(struct dwc_softc *sc); +static void dwc_rxfinish_locked(struct dwc_softc *sc); +static void dwc_stop_locked(struct dwc_softc *sc); +static void dwc_setup_rxfilter(struct dwc_softc *sc); + +static inline uint32_t +next_rxidx(struct dwc_softc *sc, uint32_t curidx) +{ + + return ((curidx + 1) % RX_DESC_COUNT); +} + +static inline uint32_t +next_txidx(struct dwc_softc *sc, uint32_t curidx) +{ + + return ((curidx + 1) % TX_DESC_COUNT); +} + +static void +dwc_get1paddr(void *arg, bus_dma_segment_t *segs, int nsegs, int error) +{ + + if (error != 0) + return; + *(bus_addr_t *)arg = segs[0].ds_addr; +} + +inline static uint32_t +dwc_setup_txdesc(struct dwc_softc *sc, int idx, bus_addr_t paddr, + uint32_t len) +{ + uint32_t flags; + uint32_t nidx; + + nidx = next_txidx(sc, idx); + + /* Addr/len 0 means we're clearing the descriptor after xmit done. */ + if (paddr == 0 || len == 0) { + flags = 0; + --sc->txcount; + } else { + flags = DDESC_TDES0_TXCHAIN | DDESC_TDES0_TXFIRST + | DDESC_TDES0_TXLAST | DDESC_TDES0_TXINT; + ++sc->txcount; + } + + sc->txdesc_ring[idx].addr = (uint32_t)(paddr); + sc->txdesc_ring[idx].tdes0 = flags; + sc->txdesc_ring[idx].tdes1 = len; + + if (paddr && len) { + wmb(); + sc->txdesc_ring[idx].tdes0 |= DDESC_TDES0_OWN; + wmb(); + } + + return (nidx); +} + +static int +dwc_setup_txbuf(struct dwc_softc *sc, int idx, struct mbuf **mp) +{ + struct bus_dma_segment seg; + int error, nsegs; + struct mbuf * m; + + if ((m = m_defrag(*mp, M_NOWAIT)) == NULL) + return (ENOMEM); + *mp = m; + + error = bus_dmamap_load_mbuf_sg(sc->txbuf_tag, sc->txbuf_map[idx].map, + m, &seg, &nsegs, 0); + if (error != 0) { + return (ENOMEM); + } + + KASSERT(nsegs == 1, ("%s: %d segments returned!", __func__, nsegs)); + + bus_dmamap_sync(sc->txbuf_tag, sc->txbuf_map[idx].map, + BUS_DMASYNC_PREWRITE); + + sc->txbuf_map[idx].mbuf = m; + + dwc_setup_txdesc(sc, idx, seg.ds_addr, seg.ds_len); + + return (0); +} + +static void +dwc_txstart_locked(struct dwc_softc *sc) +{ + struct ifnet *ifp; + struct mbuf *m; + int enqueued; + + DWC_ASSERT_LOCKED(sc); + + if (!sc->link_is_up) + return; + + ifp = sc->ifp; + + if (ifp->if_drv_flags & IFF_DRV_OACTIVE) { + return; + } + + enqueued = 0; + + for (;;) { + if (sc->txcount == (TX_DESC_COUNT-1)) { + ifp->if_drv_flags |= IFF_DRV_OACTIVE; + break; + } + + IFQ_DRV_DEQUEUE(&ifp->if_snd, m); + if (m == NULL) + break; + if (dwc_setup_txbuf(sc, sc->tx_idx_head, &m) != 0) { + IFQ_DRV_PREPEND(&ifp->if_snd, m); + break; + } + BPF_MTAP(ifp, m); + sc->tx_idx_head = next_txidx(sc, sc->tx_idx_head); + ++enqueued; + } + + if (enqueued != 0) { + WRITE4(sc, TRANSMIT_POLL_DEMAND, 0x1); + sc->tx_watchdog_count = WATCHDOG_TIMEOUT_SECS; + } +} + +static void +dwc_txstart(struct ifnet *ifp) +{ + struct dwc_softc *sc = ifp->if_softc; + + DWC_LOCK(sc); + dwc_txstart_locked(sc); + DWC_UNLOCK(sc); +} + +static void +dwc_stop_locked(struct dwc_softc *sc) +{ + struct ifnet *ifp; + int reg; + + DWC_ASSERT_LOCKED(sc); + + ifp = sc->ifp; + ifp->if_drv_flags &= ~(IFF_DRV_RUNNING | IFF_DRV_OACTIVE); + sc->tx_watchdog_count = 0; + sc->stats_harvest_count = 0; + + callout_stop(&sc->dwc_callout); + + /* Stop DMA TX */ + reg = READ4(sc, OPERATION_MODE); + reg &= ~(MODE_ST); + WRITE4(sc, OPERATION_MODE, reg); + + /* Flush TX */ + reg = READ4(sc, OPERATION_MODE); + reg |= (MODE_FTF); + WRITE4(sc, OPERATION_MODE, reg); + + /* Stop transmitters */ + reg = READ4(sc, MAC_CONFIGURATION); + reg &= ~(CONF_TE | CONF_RE); + WRITE4(sc, MAC_CONFIGURATION, reg); + + /* Stop DMA RX */ + reg = READ4(sc, OPERATION_MODE); + reg &= ~(MODE_SR); + WRITE4(sc, OPERATION_MODE, reg); +} + +static void dwc_clear_stats(struct dwc_softc *sc) +{ + int reg; + + reg = READ4(sc, MMC_CONTROL); + reg |= (MMC_CONTROL_CNTRST); + WRITE4(sc, MMC_CONTROL, reg); +} + +static void +dwc_harvest_stats(struct dwc_softc *sc) +{ + struct ifnet *ifp; + + /* We don't need to harvest too often. */ + if (++sc->stats_harvest_count < STATS_HARVEST_INTERVAL) + return; + + sc->stats_harvest_count = 0; + ifp = sc->ifp; + + if_inc_counter(ifp, IFCOUNTER_IPACKETS, READ4(sc, RXFRAMECOUNT_GB)); + if_inc_counter(ifp, IFCOUNTER_IMCASTS, READ4(sc, RXMULTICASTFRAMES_G)); + if_inc_counter(ifp, IFCOUNTER_IERRORS, + READ4(sc, RXOVERSIZE_G) + READ4(sc, RXUNDERSIZE_G) + + READ4(sc, RXCRCERROR) + READ4(sc, RXALIGNMENTERROR) + + READ4(sc, RXRUNTERROR) + READ4(sc, RXJABBERERROR) + + READ4(sc, RXLENGTHERROR)); + + if_inc_counter(ifp, IFCOUNTER_OPACKETS, READ4(sc, TXFRAMECOUNT_G)); + if_inc_counter(ifp, IFCOUNTER_OMCASTS, READ4(sc, TXMULTICASTFRAMES_G)); + if_inc_counter(ifp, IFCOUNTER_OERRORS, + READ4(sc, TXOVERSIZE_G) + READ4(sc, TXEXCESSDEF) + + READ4(sc, TXCARRIERERR) + READ4(sc, TXUNDERFLOWERROR)); + + if_inc_counter(ifp, IFCOUNTER_COLLISIONS, + READ4(sc, TXEXESSCOL) + READ4(sc, TXLATECOL)); + + dwc_clear_stats(sc); +} + +static void +dwc_tick(void *arg) +{ + struct dwc_softc *sc; + struct ifnet *ifp; + int link_was_up; + + sc = arg; + + DWC_ASSERT_LOCKED(sc); + + ifp = sc->ifp; + + if (!(ifp->if_drv_flags & IFF_DRV_RUNNING)) + return; + + /* + * Typical tx watchdog. If this fires it indicates that we enqueued + * packets for output and never got a txdone interrupt for them. Maybe + * it's a missed interrupt somehow, just pretend we got one. + */ + if (sc->tx_watchdog_count > 0) { + if (--sc->tx_watchdog_count == 0) { + dwc_txfinish_locked(sc); + } + } + + /* Gather stats from hardware counters. */ + dwc_harvest_stats(sc); + + /* Check the media status. */ + link_was_up = sc->link_is_up; + mii_tick(sc->mii_softc); + if (sc->link_is_up && !link_was_up) + dwc_txstart_locked(sc); + + /* Schedule another check one second from now. */ + callout_reset(&sc->dwc_callout, hz, dwc_tick, sc); +} + +static void +dwc_init_locked(struct dwc_softc *sc) +{ + struct ifnet *ifp = sc->ifp; + int reg; + + DWC_ASSERT_LOCKED(sc); + + if (ifp->if_drv_flags & IFF_DRV_RUNNING) + return; + + ifp->if_drv_flags |= IFF_DRV_RUNNING; + + dwc_setup_rxfilter(sc); + + /* Initializa DMA and enable transmitters */ + reg = READ4(sc, OPERATION_MODE); + reg |= (MODE_TSF | MODE_OSF | MODE_FUF); + reg &= ~(MODE_RSF); + reg |= (MODE_RTC_LEV32 << MODE_RTC_SHIFT); + WRITE4(sc, OPERATION_MODE, reg); + + WRITE4(sc, INTERRUPT_ENABLE, INT_EN_DEFAULT); + + /* Start DMA */ + reg = READ4(sc, OPERATION_MODE); + reg |= (MODE_ST | MODE_SR); + WRITE4(sc, OPERATION_MODE, reg); + + /* Enable transmitters */ + reg = READ4(sc, MAC_CONFIGURATION); + reg |= (CONF_JD | CONF_ACS | CONF_BE); + reg |= (CONF_TE | CONF_RE); + WRITE4(sc, MAC_CONFIGURATION, reg); + + /* + * Call mii_mediachg() which will call back into dwc_miibus_statchg() + * to set up the remaining config registers based on current media. + */ + mii_mediachg(sc->mii_softc); + callout_reset(&sc->dwc_callout, hz, dwc_tick, sc); +} + +static void +dwc_init(void *if_softc) +{ + struct dwc_softc *sc = if_softc; + + DWC_LOCK(sc); + dwc_init_locked(sc); + DWC_UNLOCK(sc); +} + +inline static uint32_t +dwc_setup_rxdesc(struct dwc_softc *sc, int idx, bus_addr_t paddr) +{ + uint32_t nidx; + + sc->rxdesc_ring[idx].addr = (uint32_t)paddr; + nidx = next_rxidx(sc, idx); + sc->rxdesc_ring[idx].addr_next = sc->rxdesc_ring_paddr + \ + (nidx * sizeof(struct dwc_hwdesc)); + sc->rxdesc_ring[idx].tdes1 = DDESC_RDES1_CHAINED | MCLBYTES; + + wmb(); + sc->rxdesc_ring[idx].tdes0 = DDESC_RDES0_OWN; + wmb(); + + return (nidx); +} + +static int +dwc_setup_rxbuf(struct dwc_softc *sc, int idx, struct mbuf *m) +{ + struct bus_dma_segment seg; + int error, nsegs; + + m_adj(m, ETHER_ALIGN); + + error = bus_dmamap_load_mbuf_sg(sc->rxbuf_tag, sc->rxbuf_map[idx].map, + m, &seg, &nsegs, 0); + if (error != 0) { + return (error); + } + + KASSERT(nsegs == 1, ("%s: %d segments returned!", __func__, nsegs)); + + bus_dmamap_sync(sc->rxbuf_tag, sc->rxbuf_map[idx].map, + BUS_DMASYNC_PREREAD); + + sc->rxbuf_map[idx].mbuf = m; + dwc_setup_rxdesc(sc, idx, seg.ds_addr); + + return (0); +} + +static struct mbuf * +dwc_alloc_mbufcl(struct dwc_softc *sc) +{ + struct mbuf *m; + + 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 +dwc_media_status(struct ifnet * ifp, struct ifmediareq *ifmr) +{ + struct dwc_softc *sc; + struct mii_data *mii; + + sc = ifp->if_softc; + mii = sc->mii_softc; + DWC_LOCK(sc); + mii_pollstat(mii); + ifmr->ifm_active = mii->mii_media_active; + ifmr->ifm_status = mii->mii_media_status; + DWC_UNLOCK(sc); +} + +static int +dwc_media_change_locked(struct dwc_softc *sc) +{ + + return (mii_mediachg(sc->mii_softc)); +} + +static int +dwc_media_change(struct ifnet * ifp) +{ + struct dwc_softc *sc; + int error; + + sc = ifp->if_softc; + + DWC_LOCK(sc); + error = dwc_media_change_locked(sc); + DWC_UNLOCK(sc); + return (error); +} + +static const uint8_t nibbletab[] = { + /* 0x0 0000 -> 0000 */ 0x0, + /* 0x1 0001 -> 1000 */ 0x8, + /* 0x2 0010 -> 0100 */ 0x4, + /* 0x3 0011 -> 1100 */ 0xc, + /* 0x4 0100 -> 0010 */ 0x2, + /* 0x5 0101 -> 1010 */ 0xa, + /* 0x6 0110 -> 0110 */ 0x6, + /* 0x7 0111 -> 1110 */ 0xe, + /* 0x8 1000 -> 0001 */ 0x1, + /* 0x9 1001 -> 1001 */ 0x9, + /* 0xa 1010 -> 0101 */ 0x5, + /* 0xb 1011 -> 1101 */ 0xd, + /* 0xc 1100 -> 0011 */ 0x3, + /* 0xd 1101 -> 1011 */ 0xb, + /* 0xe 1110 -> 0111 */ 0x7, + /* 0xf 1111 -> 1111 */ 0xf, }; + +static uint8_t +bitreverse(uint8_t x) +{ + + return (nibbletab[x & 0xf] << 4) | nibbletab[x >> 4]; +} + +static void +dwc_setup_rxfilter(struct dwc_softc *sc) +{ + struct ifmultiaddr *ifma; + struct ifnet *ifp; + uint8_t *eaddr; + uint32_t crc; + uint8_t val; + int hashbit; + int hashreg; + int ffval; + int reg; + int lo; + int hi; + + DWC_ASSERT_LOCKED(sc); + + ifp = sc->ifp; + + /* + * Set the multicast (group) filter hash. + */ + if ((ifp->if_flags & IFF_ALLMULTI)) + ffval = (FRAME_FILTER_PM); + else { + ffval = (FRAME_FILTER_HMC); + if_maddr_rlock(ifp); + TAILQ_FOREACH(ifma, &sc->ifp->if_multiaddrs, ifma_link) { + if (ifma->ifma_addr->sa_family != AF_LINK) + continue; + crc = ether_crc32_le(LLADDR((struct sockaddr_dl *) + ifma->ifma_addr), ETHER_ADDR_LEN); + + /* Take lower 8 bits and reverse it */ + val = bitreverse(~crc & 0xff); + hashreg = (val >> 5); + hashbit = (val & 31); + + reg = READ4(sc, HASH_TABLE_REG(hashreg)); + reg |= (1 << hashbit); + WRITE4(sc, HASH_TABLE_REG(hashreg), reg); + } + if_maddr_runlock(ifp); + } + + /* + * Set the individual address filter hash. + */ + if (ifp->if_flags & IFF_PROMISC) + ffval |= (FRAME_FILTER_PR); + + /* + * Set the primary address. + */ + eaddr = IF_LLADDR(ifp); + lo = eaddr[0] | (eaddr[1] << 8) | (eaddr[2] << 16) | + (eaddr[3] << 24); + hi = eaddr[4] | (eaddr[5] << 8); + WRITE4(sc, MAC_ADDRESS_LOW(0), lo); + WRITE4(sc, MAC_ADDRESS_HIGH(0), hi); + WRITE4(sc, MAC_FRAME_FILTER, ffval); +} + +static int +dwc_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data) +{ + struct dwc_softc *sc; + struct mii_data *mii; + struct ifreq *ifr; + int mask, error; + + sc = ifp->if_softc; + ifr = (struct ifreq *)data; + + error = 0; + switch (cmd) { + case SIOCSIFFLAGS: + DWC_LOCK(sc); + if (ifp->if_flags & IFF_UP) { + if (ifp->if_drv_flags & IFF_DRV_RUNNING) { + if ((ifp->if_flags ^ sc->if_flags) & + (IFF_PROMISC | IFF_ALLMULTI)) + dwc_setup_rxfilter(sc); + } else { + if (!sc->is_detaching) + dwc_init_locked(sc); + } + } else { + if (ifp->if_drv_flags & IFF_DRV_RUNNING) + dwc_stop_locked(sc); + } + sc->if_flags = ifp->if_flags; + DWC_UNLOCK(sc); + break; + case SIOCADDMULTI: + case SIOCDELMULTI: + if (ifp->if_drv_flags & IFF_DRV_RUNNING) { + DWC_LOCK(sc); + dwc_setup_rxfilter(sc); + DWC_UNLOCK(sc); + } + break; + case SIOCSIFMEDIA: + case SIOCGIFMEDIA: + mii = sc->mii_softc; + error = ifmedia_ioctl(ifp, ifr, &mii->mii_media, cmd); + break; + case SIOCSIFCAP: + mask = ifp->if_capenable ^ ifr->ifr_reqcap; + if (mask & IFCAP_VLAN_MTU) { + /* No work to do except acknowledge the change took */ + ifp->if_capenable ^= IFCAP_VLAN_MTU; + } + break; + + default: + error = ether_ioctl(ifp, cmd, data); + break; + } + + return (error); +} + +static void +dwc_txfinish_locked(struct dwc_softc *sc) +{ + struct dwc_bufmap *bmap; + struct dwc_hwdesc *desc; + struct ifnet *ifp; + + DWC_ASSERT_LOCKED(sc); + + ifp = sc->ifp; + + while (sc->tx_idx_tail != sc->tx_idx_head) { + desc = &sc->txdesc_ring[sc->tx_idx_tail]; + if ((desc->tdes0 & DDESC_TDES0_OWN) != 0) + break; + bmap = &sc->txbuf_map[sc->tx_idx_tail]; + bus_dmamap_sync(sc->txbuf_tag, bmap->map, + BUS_DMASYNC_POSTWRITE); + bus_dmamap_unload(sc->txbuf_tag, bmap->map); + m_freem(bmap->mbuf); + bmap->mbuf = NULL; + dwc_setup_txdesc(sc, sc->tx_idx_tail, 0, 0); + sc->tx_idx_tail = next_txidx(sc, sc->tx_idx_tail); + } + + /* If there are no buffers outstanding, muzzle the watchdog. */ + if (sc->tx_idx_tail == sc->tx_idx_head) { + sc->tx_watchdog_count = 0; + } +} + +static void +dwc_rxfinish_locked(struct dwc_softc *sc) +{ + struct ifnet *ifp; + struct mbuf *m0; + struct mbuf *m; + int error; + int rdes0; + int idx; + int len; + + ifp = sc->ifp; + + for (;;) { + idx = sc->rx_idx; + + rdes0 = sc->rxdesc_ring[idx].tdes0; + if ((rdes0 & DDESC_RDES0_OWN) != 0) + break; + + bus_dmamap_sync(sc->rxbuf_tag, sc->rxbuf_map[idx].map, + BUS_DMASYNC_POSTREAD); + bus_dmamap_unload(sc->rxbuf_tag, sc->rxbuf_map[idx].map); + + len = (rdes0 >> DDESC_RDES0_FL_SHIFT) & DDESC_RDES0_FL_MASK; + if (len != 0) { + m = sc->rxbuf_map[idx].mbuf; + m->m_pkthdr.rcvif = ifp; + m->m_pkthdr.len = len; + m->m_len = len; + ifp->if_ipackets++; + + DWC_UNLOCK(sc); + (*ifp->if_input)(ifp, m); + DWC_LOCK(sc); + } else { + /* XXX Zero-length packet ? */ + } + + if ((m0 = dwc_alloc_mbufcl(sc)) != NULL) { + if ((error = dwc_setup_rxbuf(sc, idx, m0)) != 0) { + /* + * XXX Now what? + * We've got a hole in the rx ring. + */ + } + } else + if_inc_counter(sc->ifp, IFCOUNTER_IQDROPS, 1); + + sc->rx_idx = next_rxidx(sc, sc->rx_idx); + } +} + +static void +dwc_intr(void *arg) +{ + struct dwc_softc *sc; + uint32_t reg; + + sc = arg; + + DWC_LOCK(sc); + + reg = READ4(sc, INTERRUPT_STATUS); + if (reg) { + mii_mediachg(sc->mii_softc); + READ4(sc, SGMII_RGMII_SMII_CTRL_STATUS); + } + + reg = READ4(sc, DMA_STATUS); + if (reg & DMA_STATUS_NIS) { + if (reg & DMA_STATUS_RI) + dwc_rxfinish_locked(sc); + + if (reg & DMA_STATUS_TI) + dwc_txfinish_locked(sc); + } + + if (reg & DMA_STATUS_AIS) { + if (reg & DMA_STATUS_FBI) { + /* Fatal bus error */ + device_printf(sc->dev, + "Ethernet DMA error, restarting controller.\n"); + dwc_stop_locked(sc); + dwc_init_locked(sc); + } + } + + WRITE4(sc, DMA_STATUS, reg & DMA_STATUS_INTR_MASK); + DWC_UNLOCK(sc); +} + +static int +setup_dma(struct dwc_softc *sc) +{ + struct mbuf *m; + int error; + int nidx; + int idx; + + /* + * Set up TX descriptor ring, descriptors, and dma maps. + */ + error = bus_dma_tag_create( + bus_get_dma_tag(sc->dev), /* Parent tag. */ + DWC_DESC_RING_ALIGN, 0, /* alignment, boundary */ + BUS_SPACE_MAXADDR_32BIT, /* lowaddr */ + BUS_SPACE_MAXADDR, /* highaddr */ + NULL, NULL, /* filter, filterarg */ + TX_DESC_SIZE, 1, /* maxsize, nsegments */ + TX_DESC_SIZE, /* maxsegsize */ + 0, /* flags */ + NULL, NULL, /* lockfunc, lockarg */ + &sc->txdesc_tag); + if (error != 0) { + device_printf(sc->dev, + "could not create TX ring DMA tag.\n"); + goto out; + } + + error = bus_dmamem_alloc(sc->txdesc_tag, (void**)&sc->txdesc_ring, + BUS_DMA_COHERENT | BUS_DMA_WAITOK | BUS_DMA_ZERO, + &sc->txdesc_map); + if (error != 0) { + device_printf(sc->dev, + "could not allocate TX descriptor ring.\n"); + goto out; + } + + error = bus_dmamap_load(sc->txdesc_tag, sc->txdesc_map, + sc->txdesc_ring, TX_DESC_SIZE, dwc_get1paddr, + &sc->txdesc_ring_paddr, 0); + if (error != 0) { + device_printf(sc->dev, + "could not load TX descriptor ring map.\n"); + goto out; + } + + for (idx = 0; idx < TX_DESC_COUNT; idx++) { + sc->txdesc_ring[idx].tdes0 = DDESC_TDES0_TXCHAIN; + sc->txdesc_ring[idx].tdes1 = 0; + nidx = next_txidx(sc, idx); + sc->txdesc_ring[idx].addr_next = sc->txdesc_ring_paddr + \ + (nidx * sizeof(struct dwc_hwdesc)); + } + + error = bus_dma_tag_create( + bus_get_dma_tag(sc->dev), /* Parent tag. */ + 1, 0, /* alignment, boundary */ + BUS_SPACE_MAXADDR_32BIT, /* lowaddr */ + BUS_SPACE_MAXADDR, /* highaddr */ + NULL, NULL, /* filter, filterarg */ + MCLBYTES, 1, /* maxsize, nsegments */ + MCLBYTES, /* maxsegsize */ + 0, /* flags */ *** DIFF OUTPUT TRUNCATED AT 1000 LINES ***
Want to link to this message? Use this URL: <https://mail-archive.FreeBSD.org/cgi/mid.cgi?201409251803.s8PI3E5D010096>