Add support for packet filter

coolstar · Jan 19, 2024 · 743eb25 · 743eb25
1 parent 118ef84
commit 743eb25
Show file tree

Hide file tree

Showing 7 changed files with 149 additions and 48 deletions.
diff --git a/adapter.cpp b/adapter.cpp
@@ -59,41 +59,28 @@ Return Value:
 static void
 EvtSetReceiveFilter(
     _In_ NETADAPTER netAdapter,
-    _In_ NETRECEIVEFILTER receiveFilter)
+    _In_ NETRECEIVEFILTER Handle)
 {
     TraceEntryNetAdapter(netAdapter);
     RT_ADAPTER* adapter = RtGetAdapterContext(netAdapter);
 
-    // PASSIVE_LEVEL, nonpaged (resume path)
-    NET_PACKET_FILTER_FLAGS flags = NetReceiveFilterGetPacketFilter(receiveFilter);
-    size_t mcastCount = (flags & NetPacketFilterFlagMulticast)
-        ? NetReceiveFilterGetMulticastAddressCount(receiveFilter)
-        : 0;
-    const NET_ADAPTER_LINK_LAYER_ADDRESS* mcast = mcastCount > 0 ?
-        NetReceiveFilterGetMulticastAddressList(receiveFilter)
-        : nullptr;
-
-    /*MacPacketFilter_t filter = {};
-    if (flags & NetPacketFilterFlagPromiscuous)
+    adapter->PacketFilterFlags = NetReceiveFilterGetPacketFilter(Handle);
+
+    adapter->MCAddressCount = (UINT)NetReceiveFilterGetMulticastAddressCount(Handle);
+    RtlZeroMemory(
+        adapter->MCList,
+        sizeof(NET_ADAPTER_LINK_LAYER_ADDRESS) * RT_MAX_MCAST_LIST);
+
+    if (adapter->MCAddressCount != 0U)
     {
-        filter.PromiscuousMode = true;
+        NET_ADAPTER_LINK_LAYER_ADDRESS const* MulticastAddressList = NetReceiveFilterGetMulticastAddressList(Handle);
+        RtlCopyMemory(
+            adapter->MCList,
+            MulticastAddressList,
+            sizeof(NET_ADAPTER_LINK_LAYER_ADDRESS) * adapter->MCAddressCount);
     }
-    else
-    {
-        filter.PassAllMulticast = 0 != (flags & NetPacketFilterFlagAllMulticast);
-        filter.DisableBroadcast = 0 == (flags & NetPacketFilterFlagBroadcast);
-        SetOneMacAddress(context->regs, 0, context->currentMacAddress,
-            0 != (flags & NetPacketFilterFlagDirected)); // Address[0] can't really be disabled...
-        // Could also use hash-based filtering for additional mcast support, but this seems okay.
-        auto const macAddrCount = context->feature0.MacAddrCount;
-        for (unsigned i = 1; i < macAddrCount; i += 1)
-        {
-            static constexpr UINT8 zero[ETHERNET_LENGTH_OF_ADDRESS] = {};
-            bool const enable = mcastCount > i - 1 && mcast[i - 1].Length >= ETHERNET_LENGTH_OF_ADDRESS;
-            auto const addr = enable ? mcast[i - 1].Address : zero;
-            SetOneMacAddress(context->regs, i, addr, enable);
-        }
-    }*/
+
+    re_set_rx_packet_filter(&adapter->bsdData);
     TraceExit();
 }
 
@@ -105,13 +92,8 @@ RtAdapterSetReceiveFilterCapabilities(
 {
     NET_ADAPTER_RECEIVE_FILTER_CAPABILITIES rxFilterCaps;
     NET_ADAPTER_RECEIVE_FILTER_CAPABILITIES_INIT(&rxFilterCaps, EvtSetReceiveFilter);
-    rxFilterCaps.SupportedPacketFilters =
-        NetPacketFilterFlagDirected |
-        NetPacketFilterFlagMulticast |
-        NetPacketFilterFlagAllMulticast |
-        NetPacketFilterFlagBroadcast |
-        NetPacketFilterFlagPromiscuous;
-    rxFilterCaps.MaximumMulticastAddresses = 1; // TODO: Packet filter.
+    rxFilterCaps.SupportedPacketFilters = RT_SUPPORTED_FILTERS;
+    rxFilterCaps.MaximumMulticastAddresses = RT_MAX_MCAST_LIST;
     NetAdapterSetReceiveFilterCapabilities(adapter->NetAdapter, &rxFilterCaps);
 }
 

diff --git a/adapter.h b/adapter.h
@@ -3,6 +3,17 @@
 #include "if_re_bsd.h"
 #include "bsdexport.h"
 
+// multicast list size
+#define RT_MAX_MCAST_LIST 32
+
+// supported filters
+#define RT_SUPPORTED_FILTERS (          \
+    NetPacketFilterFlagDirected    | \
+    NetPacketFilterFlagMulticast   | \
+    NetPacketFilterFlagBroadcast   | \
+    NetPacketFilterFlagPromiscuous | \
+    NetPacketFilterFlagAllMulticast)
+
 #define RT_MAX_TX_QUEUES (2)
 #define RT_MAX_RX_QUEUES (4)
 #define RT_MAX_QUEUES RT_MAX_RX_QUEUES
@@ -70,6 +81,11 @@ typedef struct _RT_ADAPTER
     // Pointer to interrupt object
     RT_INTERRUPT* Interrupt;
 
+    // Multicast list
+    NET_PACKET_FILTER_FLAGS PacketFilterFlags;
+    UINT MCAddressCount;
+    NET_ADAPTER_LINK_LAYER_ADDRESS MCList[RT_MAX_MCAST_LIST];
+
     // Configuration
     REG_SPEED_SETTING SpeedDuplex;
     NET_ADAPTER_LINK_LAYER_ADDRESS PermanentAddress;
@@ -114,4 +130,11 @@ UINT16 ConfigRead16(_In_ RT_ADAPTER* adapter, UINT32 reg);
 void ConfigWrite8(_In_ RT_ADAPTER* adapter, UINT32 reg, UINT8 val);
 void ConfigWrite16(_In_ RT_ADAPTER* adapter, UINT32 reg, UINT16 val);
 
-void RtResetQueues(_In_ RT_ADAPTER* adapter);
+void RtResetQueues(_In_ RT_ADAPTER* adapter);
+
+void
+GetMulticastBit(
+    _In_ NET_ADAPTER_LINK_LAYER_ADDRESS const* address,
+    _Out_ _Post_satisfies_(*byte < MAX_NIC_MULTICAST_REG) UCHAR* byte,
+    _Out_ UCHAR* value
+);
diff --git a/bsdexport.h b/bsdexport.h
@@ -24,4 +24,5 @@ void re_hw_d3_para(struct re_softc* sc);
 int re_ifmedia_upd(struct re_softc* sc);
 int re_ifmedia_upd_8125(struct re_softc* sc);
 
-void re_rar_set(struct re_softc* sc, u_int8_t* eaddr);
+void re_rar_set(struct re_softc* sc, u_int8_t* eaddr);
+void re_set_rx_packet_filter(struct re_softc* sc);
diff --git a/if_re.vcxproj b/if_re.vcxproj
@@ -223,6 +223,7 @@
     <ClCompile Include="driver.cpp" />
     <ClCompile Include="interrupt.cpp" />
     <ClCompile Include="link.cpp" />
+    <ClCompile Include="mcast.cpp" />
     <ClCompile Include="pciconfig.cpp" />
     <ClCompile Include="power.cpp" />
     <ClCompile Include="rtlhw.cpp" />

diff --git a/if_re.vcxproj.Filters b/if_re.vcxproj.Filters
@@ -71,6 +71,9 @@
     <ClCompile Include="configuration.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="mcast.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="adapter.h">

diff --git a/mcast.cpp b/mcast.cpp
@@ -0,0 +1,54 @@
+#include "precomp.h"
+#include "adapter.h"
+
+ULONG
+ComputeCrc(
+    _In_reads_(length) UCHAR const* buffer,
+    UINT length
+)
+{
+    ULONG crc = 0xffffffff;
+
+    for (UINT i = 0; i < length; i++)
+    {
+        UCHAR curByte = buffer[i];
+
+        for (UINT j = 0; j < 8; j++)
+        {
+            ULONG carry = ((crc & 0x80000000) ? 1 : 0) ^ (curByte & 0x01);
+            crc <<= 1;
+            curByte >>= 1;
+
+            if (carry)
+            {
+                crc = (crc ^ 0x04c11db6) | carry;
+            }
+        }
+    }
+
+    return crc;
+}
+
+void
+GetMulticastBit(
+    _In_ NET_ADAPTER_LINK_LAYER_ADDRESS const* address,
+    _Out_ _Post_satisfies_(*byte < MAX_NIC_MULTICAST_REG) UCHAR* byte,
+    _Out_ UCHAR* value
+)
+/*++
+
+Routine Description:
+
+    For a given multicast address, returns the byte and bit in
+    the card multicast registers that it hashes to. Calls
+    ComputeCrc() to determine the CRC value.
+
+--*/
+{
+    ULONG crc = ComputeCrc(address->Address, address->Length);
+
+    // The bit number is now in the 6 most significant bits of CRC.
+    UINT bitNumber = (UINT)((crc >> 26) & 0x3f);
+    *byte = (UCHAR)(bitNumber / 8);
+    *value = (UCHAR)((UCHAR)1 << (bitNumber % 8));
+}
diff --git a/rtlhw.cpp b/rtlhw.cpp
@@ -98,7 +98,7 @@ static void re_clrwol			__P((struct re_softc*));
 static void re_set_wol_linkspeed 	__P((struct re_softc*));
 #endif
 
-static void re_set_rx_packet_filter	__P((struct re_softc*));
+void re_set_rx_packet_filter	__P((struct re_softc*));
 
 static void re_eeprom_ShiftOutBits		__P((struct re_softc*, int, int));
 static u_int16_t re_eeprom_ShiftInBits		__P((struct re_softc*));
@@ -5650,20 +5650,55 @@ void re_stop(struct re_softc* sc) {
     re_reset(sc);
 }
 
-static void re_set_rx_packet_filter(struct re_softc* sc) {
+void re_set_rx_packet_filter(struct re_softc* sc) {
     u_int32_t rxfilt;
-
+    
     rxfilt = CSR_READ_4(sc, RE_RXCFG);
 
-    rxfilt |= RE_RXCFG_RX_INDIV;
+    rxfilt &= ~(RE_RXCFG_RX_ALLPHYS | RE_RXCFG_RX_INDIV |
+        RE_RXCFG_RX_MULTI | RE_RXCFG_RX_BROAD |
+        RE_RXCFG_RX_RUNT | RE_RXCFG_RX_ERRPKT);
+
+    NET_PACKET_FILTER_FLAGS flags = sc->dev->PacketFilterFlags;
 
-    rxfilt |= (RE_RXCFG_RX_ALLPHYS | RE_RXCFG_RX_MULTI);
-    rxfilt |= RE_RXCFG_RX_BROAD;
+    if (flags & NetPacketFilterFlagPromiscuous) {
+        rxfilt |= (
+            RE_RXCFG_RX_ALLPHYS | RE_RXCFG_RX_INDIV |
+            RE_RXCFG_RX_MULTI | RE_RXCFG_RX_BROAD |
+            RE_RXCFG_RX_RUNT | RE_RXCFG_RX_ERRPKT
+            );
+    }
+    else {
+        rxfilt |= ((flags & NetPacketFilterFlagAllMulticast) ? (RE_RXCFG_RX_ALLPHYS | RE_RXCFG_RX_MULTI) : 0);
+        rxfilt |= ((flags & NetPacketFilterFlagMulticast) ? (RE_RXCFG_RX_ALLPHYS | RE_RXCFG_RX_MULTI) : 0);
+        rxfilt |= ((flags & NetPacketFilterFlagBroadcast) ? RE_RXCFG_RX_BROAD : 0);
+        rxfilt |= ((flags & NetPacketFilterFlagDirected) ? RE_RXCFG_RX_INDIV : 0);
+    }
 
     CSR_WRITE_4(sc, RE_RXCFG, rxfilt);
 
-    u_int32_t mask0 = 0xffffffff;
-    u_int32_t mask4 = 0xffffffff;
+    typedef union {
+        struct {
+            uint32_t mask0;
+            uint32_t mask4;
+        };
+        uint8_t bytes[32];
+    } MarRegs;
+
+    MarRegs regs = { 0 };
+
+    if (flags & (NetPacketFilterFlagPromiscuous | NetPacketFilterFlagAllMulticast)) {
+        regs.mask0 = 0xffffffff;
+        regs.mask4 = 0xffffffff;
+    }
+    else {
+        for (UINT i = 0; i < sc->dev->MCAddressCount; i++) {
+            UCHAR byte, bit;
+            NET_ADAPTER_LINK_LAYER_ADDRESS address = sc->dev->MCList[i];
+            GetMulticastBit(&sc->dev->MCList[i], &byte, &bit);
+            regs.bytes[byte] |= bit;
+        }
+    }
 
     u_int8_t  enable_cfg_reg_write = 0;
 
@@ -5672,8 +5707,10 @@ static void re_set_rx_packet_filter(struct re_softc* sc) {
 
     if (enable_cfg_reg_write)
         re_enable_cfg9346_write(sc);
-    CSR_WRITE_4(sc, RE_MAR0, mask0);
-    CSR_WRITE_4(sc, RE_MAR4, mask4);
+
+    CSR_WRITE_4(sc, RE_MAR0, regs.mask0);
+    CSR_WRITE_4(sc, RE_MAR4, regs.mask4);
+
     if (enable_cfg_reg_write)
         re_disable_cfg9346_write(sc);
 }