From 58162da5f2e3f94bb19a5412df7e20f484ceb11d Mon Sep 17 00:00:00 2001 From: Sebastian Huber Date: Thu, 5 May 2022 08:37:20 +0200 Subject: if_atsam: Add multicast support Update #4651. --- rtemsbsd/sys/dev/atsam/if_atsam.c | 111 +++++++++++++++++++++++++------------- 1 file changed, 75 insertions(+), 36 deletions(-) diff --git a/rtemsbsd/sys/dev/atsam/if_atsam.c b/rtemsbsd/sys/dev/atsam/if_atsam.c index 22ef14ba..15948be8 100644 --- a/rtemsbsd/sys/dev/atsam/if_atsam.c +++ b/rtemsbsd/sys/dev/atsam/if_atsam.c @@ -50,6 +50,7 @@ #include #include +#include #include #include #include @@ -91,13 +92,6 @@ /** The runtime pin configure list for GMAC */ #define BOARD_GMAC_RUN_PINS BOARD_GMAC_PINS -/** Multicast Enable */ -#define GMAC_MC_ENABLE (1u << 6) -#define HASH_INDEX_AMOUNT 6 -#define HASH_ELEMENTS_PER_INDEX 8 -#define MAC_ADDR_MASK 0x0000FFFFFFFFFFFF -#define MAC_IDX_MASK (1u << 0) - /** RX Defines */ #define GMAC_RX_BUFFER_SIZE 1536 #define GMAC_RX_BUF_DESC_ADDR_MASK 0xFFFFFFFC @@ -992,6 +986,11 @@ if_atsam_init(if_atsam_softc *sc) /* Enable hardware checksum offload for receive */ sc->Gmac_inst.gGmacd.pHw->GMAC_NCFGR |= GMAC_NCFGR_RXCOEN; + /* Use Multicast Hash Filter */ + sc->Gmac_inst.gGmacd.pHw->GMAC_NCFGR |= GMAC_NCFGR_MTIHEN; + sc->Gmac_inst.gGmacd.pHw->GMAC_HRB = 0; + sc->Gmac_inst.gGmacd.pHw->GMAC_HRT = 0; + /* Shut down Transmit and Receive */ GMAC_ReceiveEnable(sc->Gmac_inst.gGmacd.pHw, 0); GMAC_TransmitEnable(sc->Gmac_inst.gGmacd.pHw, 0); @@ -1025,16 +1024,75 @@ if_atsam_init(if_atsam_softc *sc) callout_reset(&sc->tick_ch, hz, if_atsam_tick, sc); } +static int +if_atsam_get_hash_index(const uint8_t *eaddr) +{ + uint64_t eaddr64; + int index; + int i; + + eaddr64 = eaddr[5]; + + for (i = 4; i >= 0; --i) { + eaddr64 <<= 8; + eaddr64 |= eaddr[i]; + } + + index = 0; + + for (i = 0; i < 6; ++i) { + uint64_t bits; + int j; + int hash; + + bits = eaddr64 >> i; + hash = bits & 1; + + for (j = 1; j < 8; ++j) { + bits >>= 6; + hash ^= bits & 1; + } + + index |= hash << i; + } + + return index; +} + static void if_atsam_setup_rxfilter(struct if_atsam_softc *sc) { - Gmac *pHw = sc->Gmac_inst.gGmacd.pHw; + struct ifnet *ifp; + struct ifmultiaddr *ifma; + uint64_t mhash; + Gmac *pHw; + + pHw = sc->Gmac_inst.gGmacd.pHw; if ((sc->ifp->if_flags & IFF_PROMISC) != 0) { pHw->GMAC_NCFGR |= GMAC_NCFGR_CAF; } else { pHw->GMAC_NCFGR &= ~GMAC_NCFGR_CAF; } + + ifp = sc->ifp; + + if ((ifp->if_flags & IFF_ALLMULTI)) + mhash = 0xffffffffffffffffLLU; + else { + mhash = 0; + if_maddr_rlock(ifp); + CK_STAILQ_FOREACH(ifma, &sc->ifp->if_multiaddrs, ifma_link) { + if (ifma->ifma_addr->sa_family != AF_LINK) + continue; + mhash |= 1LLU << if_atsam_get_hash_index( + LLADDR((struct sockaddr_dl *) ifma->ifma_addr)); + } + if_maddr_runlock(ifp); + } + + pHw->GMAC_HRB = (uint32_t)mhash; + pHw->GMAC_HRT = (uint32_t)(mhash >> 32); } static void @@ -1326,33 +1384,6 @@ if_atsam_add_sysctls(device_t dev) "UDP Checksum Errors"); } - -/* - * Calculates the index that is to be sent into the hash registers - */ -static void if_atsam_get_hash_index(uint64_t addr, uint32_t *val) -{ - uint64_t tmp_val; - uint8_t i, j; - uint64_t idx; - int offset = 0; - - addr &= MAC_ADDR_MASK; - - for (i = 0; i < HASH_INDEX_AMOUNT; ++i) { - tmp_val = 0; - offset = 0; - for (j = 0; j < HASH_ELEMENTS_PER_INDEX; j++) { - idx = (addr >> (offset + i)) & MAC_IDX_MASK; - tmp_val ^= idx; - offset += HASH_INDEX_AMOUNT; - } - if (tmp_val > 0) { - *val |= (1u << i); - } - } -} - static int if_atsam_mediaioctl(if_atsam_softc *sc, struct ifreq *ifr, u_long command) { @@ -1404,6 +1435,14 @@ if_atsam_ioctl(struct ifnet *ifp, ioctl_command_t command, caddr_t data) sc->if_flags = ifp->if_flags; IF_ATSAM_UNLOCK(sc); break; + case SIOCADDMULTI: + case SIOCDELMULTI: + if (ifp->if_drv_flags & IFF_DRV_RUNNING) { + IF_ATSAM_LOCK(sc); + if_atsam_setup_rxfilter(sc); + IF_ATSAM_UNLOCK(sc); + } + break; default: rv = ether_ioctl(ifp, command, data); break; @@ -1491,7 +1530,7 @@ static int if_atsam_driver_attach(device_t dev) ifp->if_init = if_atsam_start; ifp->if_ioctl = if_atsam_ioctl; ifp->if_start = if_atsam_enet_start; - ifp->if_flags = IFF_BROADCAST | IFF_SIMPLEX; + ifp->if_flags = IFF_BROADCAST | IFF_MULTICAST | IFF_SIMPLEX; ifp->if_capabilities |= IFCAP_HWCSUM | IFCAP_HWCSUM_IPV6 | IFCAP_VLAN_HWCSUM; ifp->if_capenable = ifp->if_capabilities; -- cgit v1.2.3