From 854aa05a36c1634481d78985b84ac50ce05a6b1c Mon Sep 17 00:00:00 2001 From: Jamie Smith Date: Sat, 28 Dec 2024 20:23:18 -0800 Subject: [PATCH] Add multicast filter support to STM32 EMAC --- .../drivers/emac/TARGET_STM/mbed_lib.json | 4 + .../drivers/emac/TARGET_STM/stm32xx_emac.cpp | 164 +++++++++++++++++- .../drivers/emac/TARGET_STM/stm32xx_emac.h | 11 ++ 3 files changed, 176 insertions(+), 3 deletions(-) diff --git a/connectivity/drivers/emac/TARGET_STM/mbed_lib.json b/connectivity/drivers/emac/TARGET_STM/mbed_lib.json index 5a86535e70a..1875925cf6c 100644 --- a/connectivity/drivers/emac/TARGET_STM/mbed_lib.json +++ b/connectivity/drivers/emac/TARGET_STM/mbed_lib.json @@ -42,6 +42,10 @@ "eth-phy-duplex-status": { "help" : "Duplex mask information in eth-phy-status-register", "value" : "0x0010" + }, + "max-mcast-subscribes": { + "help" : "Maximum supported number of multicast addresses that the application can subscribe to", + "value" : "8" } }, "target_overrides": { diff --git a/connectivity/drivers/emac/TARGET_STM/stm32xx_emac.cpp b/connectivity/drivers/emac/TARGET_STM/stm32xx_emac.cpp index 638202602ae..97bff757138 100644 --- a/connectivity/drivers/emac/TARGET_STM/stm32xx_emac.cpp +++ b/connectivity/drivers/emac/TARGET_STM/stm32xx_emac.cpp @@ -28,6 +28,7 @@ #include "platform/mbed_power_mgmt.h" #include "platform/mbed_error.h" #include "CacheAlignedBuffer.h" +#include "MbedCRC.h" #include "stm32xx_emac_config.h" #include "stm32xx_emac.h" @@ -294,6 +295,12 @@ bool STM32_EMAC::low_level_init_successful() /* HAL_ETH_Init returns TIMEOUT when Ethernet cable is not plugged */; } + // Set MAC address + writeMACAddress(MACAddr, &EthHandle.Instance->MACA0HR, &EthHandle.Instance->MACA0LR); + + // Enable multicast hash and perfect filter + EthHandle.Instance->MACFFR = ETH_MACFFR_HM | ETH_MACFFR_HPF; + uint32_t TempRegisterValue; if (HAL_ETH_ReadPHYRegister(&EthHandle, 2, &TempRegisterValue) != HAL_OK) { tr_error("HAL_ETH_ReadPHYRegister 2 issue"); @@ -364,6 +371,12 @@ bool STM32_EMAC::low_level_init_successful() return false; } + // Set MAC address + writeMACAddress(MACAddr, &EthHandle.Instance->MACA0HR, &EthHandle.Instance->MACA0LR); + + // Enable multicast hash and perfect filter + EthHandle.Instance->MACPFR = ETH_MACPFR_HMC | ETH_MACPFR_HPF; + memset(&TxConfig, 0, sizeof(ETH_TxPacketConfig)); TxConfig.Attributes = ETH_TX_PACKETS_FEATURES_CSUM | ETH_TX_PACKETS_FEATURES_CRCPAD; TxConfig.ChecksumCtrl = ETH_CHECKSUM_IPHDR_PAYLOAD_INSERT_PHDR_CALC; @@ -934,17 +947,68 @@ void STM32_EMAC::set_link_state_cb(emac_link_state_change_cb_t state_cb) void STM32_EMAC::add_multicast_group(const uint8_t *addr) { - /* No-op at this stage */ + if(numSubscribedMcastMacs >= MBED_CONF_STM32_EMAC_MAX_MCAST_SUBSCRIBES) + { + tr_error("Out of multicast group entries (currently have %d). Increase the 'stm32-emac.max-mcast-subscribes' JSON option!", MBED_CONF_STM32_EMAC_MAX_MCAST_SUBSCRIBES); + return; + } + + memcpy(mcastMacs[numSubscribedMcastMacs++].data(), addr, 6); + populateMcastFilterRegs(); } void STM32_EMAC::remove_multicast_group(const uint8_t *addr) { - /* No-op at this stage */ + // Find MAC address in the subscription list + auto macsEndIter = std::begin(mcastMacs) + numSubscribedMcastMacs; + auto toRemoveIter = std::find_if(std::begin(mcastMacs), macsEndIter, [&](auto element) { + return memcmp(element.data(), addr, 6) == 0; + }); + + if(toRemoveIter == macsEndIter) + { + tr_warning("Tried to remove mcast group that was not added"); + return; + } + + // Swap the MAC addr to be removed to the end of the list, if it is not there already + auto lastElementIter = macsEndIter - 1; + if(toRemoveIter != std::begin(mcastMacs) && toRemoveIter != lastElementIter) + { + std::swap(*toRemoveIter, *lastElementIter); + } + + // 'remove' the last element by changing the length + numSubscribedMcastMacs--; + + // Rebuild the MAC registers with that MAC removed. + // Technically it would be more performance efficient to remove just this MAC address, but that gets complex + // once you throw the hash filter into the mix. Unless you are subscribed to insane numbers of mcast addrs, + // it's easier to just rebuild it all. + populateMcastFilterRegs(); } void STM32_EMAC::set_all_multicast(bool all) { - /* No-op at this stage */ +#if defined(ETH_IP_VERSION_V2) + if(all) + { + EthHandle.Instance->MACPFR |= ETH_MACPFR_PM; + } + else + { + EthHandle.Instance->MACPFR &= ~ETH_MACPFR_PM; + } +#else + if(all) + { + EthHandle.Instance->MACFFR |= ETH_MACFFR_PM; + } + else + { + EthHandle.Instance->MACFFR &= ~ETH_MACFFR_PM; + } +#endif } void STM32_EMAC::power_down() @@ -966,6 +1030,100 @@ STM32_EMAC &STM32_EMAC::get_instance() return emac; } +void STM32_EMAC::populateMcastFilterRegs() { + const size_t NUM_PERFECT_FILTER_REGS = 3; + + const size_t numPerfectFilterMacs = std::min(NUM_PERFECT_FILTER_REGS, numSubscribedMcastMacs); + const size_t numHashFilterMacs = numSubscribedMcastMacs - numPerfectFilterMacs; + + for(size_t perfFiltIdx = 0; perfFiltIdx < NUM_PERFECT_FILTER_REGS; ++perfFiltIdx) + { + // Find MAC addr registers (they aren't in an array :/) + uint32_t volatile * highReg; + uint32_t volatile * lowReg; + + if(perfFiltIdx == 0) + { + highReg = &EthHandle.Instance->MACA1HR; + lowReg = &EthHandle.Instance->MACA1LR; + } + else if(perfFiltIdx == 1) + { + highReg = &EthHandle.Instance->MACA2HR; + lowReg = &EthHandle.Instance->MACA2LR; + } + else + { + highReg = &EthHandle.Instance->MACA3HR; + lowReg = &EthHandle.Instance->MACA3LR; + } + + if(perfFiltIdx < numPerfectFilterMacs) + { + tr_debug("Using perfect filtering for %02" PRIx8 ":%02" PRIx8 ":%02" PRIx8 ":%02" PRIx8 ":%02" PRIx8 ":%02" PRIx8, + mcastMacs[perfFiltIdx][0], mcastMacs[perfFiltIdx][1], mcastMacs[perfFiltIdx][2], + mcastMacs[perfFiltIdx][3], mcastMacs[perfFiltIdx][4], mcastMacs[perfFiltIdx][5]); + writeMACAddress(mcastMacs[perfFiltIdx].data(), highReg, lowReg); + } + else + { + // Write zeroes to disable this mac addr entry + *highReg = 0; + *lowReg = 0; + } + } + +#if defined(ETH_IP_VERSION_V2) + uint32_t volatile * hashRegs[] = { + &EthHandle.Instance->MACHT1R, + &EthHandle.Instance->MACHT0R + }; +#else + uint32_t volatile * hashRegs[] = { + &EthHandle.Instance->MACHTHR, + &EthHandle.Instance->MACHTLR + }; +#endif + + // Reset hash filter regs + *hashRegs[0] = 0; + *hashRegs[1] = 0; + + // Note: as always, the datasheet description of how to do this CRC was vague and slightly wrong. + // This forum thread figured it out: https://community.st.com/t5/stm32-mcus-security/calculating-ethernet-multicast-filter-hash-value/td-p/416984 + // What the datasheet SHOULD say is: + // Compute the Ethernet CRC-32 of the MAC address, with initial value of 1s, final XOR of ones, and input reflection on but output reflection off + // Then, take the upper 6 bits and use that to index the hash table. + + mbed::MbedCRC crcCalc(0xFFFFFFFF, 0xFFFFFFFF, true, false); + for(size_t hashFiltIdx = 0; hashFiltIdx < numHashFilterMacs; ++hashFiltIdx) + { + auto & currMacAddr = mcastMacs[hashFiltIdx + numPerfectFilterMacs]; + + tr_debug("Using hash filtering for %02" PRIx8 ":%02" PRIx8 ":%02" PRIx8 ":%02" PRIx8 ":%02" PRIx8 ":%02" PRIx8, + currMacAddr[0], currMacAddr[1], currMacAddr[2], + currMacAddr[3], currMacAddr[4], currMacAddr[5]); + + // Compute Ethernet CRC-32 of the MAC address + uint32_t crc; + crcCalc.compute(currMacAddr.data(), currMacAddr.size(), &crc); + + // Take upper 6 bits + uint32_t hashVal = crc >> 26; + + // Set correct bit in hash filter + *hashRegs[hashVal >> 5] |= (1 << (hashVal & 0x1F)); + } +} + +void STM32_EMAC::writeMACAddress(const uint8_t *MAC, volatile uint32_t *addrHighReg, volatile uint32_t *addrLowReg) { + /* Set MAC addr bits 32 to 47 */ + *addrHighReg = (static_cast(MAC[5]) << 8) | static_cast(MAC[4]) | ETH_MACA1HR_AE_Msk; + /* Set MAC addr bits 0 to 31 */ + *addrLowReg = (static_cast(MAC[3]) << 24) | (static_cast(MAC[2]) << 16) | + (static_cast(MAC[1]) << 8) | static_cast(MAC[0]); +} + // Weak so a module can override MBED_WEAK EMAC &EMAC::get_default_instance() { diff --git a/connectivity/drivers/emac/TARGET_STM/stm32xx_emac.h b/connectivity/drivers/emac/TARGET_STM/stm32xx_emac.h index cfa67521774..e4c81646a45 100644 --- a/connectivity/drivers/emac/TARGET_STM/stm32xx_emac.h +++ b/connectivity/drivers/emac/TARGET_STM/stm32xx_emac.h @@ -162,6 +162,13 @@ class STM32_EMAC : public EMAC { void enable_interrupts(); void disable_interrupts(); + // Populate multicast filter registers with the contents of mcastMacs. + // Uses the perfect filter registers first, then the hash filter. + void populateMcastFilterRegs(); + + // Write a MAC address into a high and low register pair on the MAC. + static void writeMACAddress(uint8_t const * MAC, uint32_t volatile * addrHighReg, uint32_t volatile * addrLowReg); + mbed_rtos_storage_thread_t thread_cb; #if defined (STM32F767xx) || defined (STM32F769xx) || defined (STM32F777xx)\ || defined (STM32F779xx) @@ -176,6 +183,10 @@ class STM32_EMAC : public EMAC { uint32_t phy_status; int phy_task_handle; /**< Handle for phy task event */ + + // Multicast subscribe information + std::array mcastMacs[MBED_CONF_STM32_EMAC_MAX_MCAST_SUBSCRIBES]; + size_t numSubscribedMcastMacs; }; #endif /* STM32_EMAC_H_ */