[PATCH net] net: ftmac100: add multicast filtering possibility

[Sergei Antonov <saproj@gmail.com>]

If netdev_mc_count() is not zero and not IFF_ALLMULTI, filter
incoming multicast packets. The chip has a Multicast Address Hash Table
for allowed multicast addresses, so we fill it.

This change is based on the analogous code from the ftgmac100 driver.
Although the hashing algorithm is different.

Signed-off-by: Sergei Antonov <saproj@gmail.com>
---
 drivers/net/ethernet/faraday/ftmac100.c | 39 +++++++++++++++++++++++++
 1 file changed, 39 insertions(+)

diff --git a/drivers/net/ethernet/faraday/ftmac100.c b/drivers/net/ethernet/faraday/ftmac100.c
index 139fe66f8bcd..0ecc0a319520 100644
--- a/drivers/net/ethernet/faraday/ftmac100.c
+++ b/drivers/net/ethernet/faraday/ftmac100.c
@@ -149,6 +149,25 @@ static void ftmac100_set_mac(struct ftmac100 *priv, const unsigned char *mac)
 	iowrite32(laddr, priv->base + FTMAC100_OFFSET_MAC_LADR);
 }
 
+static void ftmac100_setup_mc_ht(struct ftmac100 *priv)
+{
+	u32 maht0 = 0; /* Multicast Address Hash Table bits 31:0 */
+	u32 maht1 = 0; /* ... bits 63:32 */
+	struct netdev_hw_addr *ha;
+
+	netdev_for_each_mc_addr(ha, priv->netdev) {
+		u32 hash = ~ether_crc_le(ETH_ALEN, ha->addr) >> 26;
+
+		if (hash >= 32)
+			maht1 |= 1 << (hash - 32);
+		else
+			maht0 |= 1 << hash;
+	}
+
+	iowrite32(maht0, priv->base + FTMAC100_OFFSET_MAHT0);
+	iowrite32(maht1, priv->base + FTMAC100_OFFSET_MAHT1);
+}
+
 #define MACCR_ENABLE_ALL	(FTMAC100_MACCR_XMT_EN	| \
 				 FTMAC100_MACCR_RCV_EN	| \
 				 FTMAC100_MACCR_XDMA_EN	| \
@@ -187,6 +206,10 @@ static int ftmac100_start_hw(struct ftmac100 *priv)
 		maccr |= FTMAC100_MACCR_RCV_ALL;
 	if (netdev->flags & IFF_ALLMULTI)
 		maccr |= FTMAC100_MACCR_RX_MULTIPKT;
+	else if (netdev_mc_count(netdev)) {
+		maccr |= FTMAC100_MACCR_HT_MULTI_EN;
+		ftmac100_setup_mc_ht(priv);
+	}
 
 	iowrite32(maccr, priv->base + FTMAC100_OFFSET_MACCR);
 	return 0;
@@ -1067,6 +1090,21 @@ static int ftmac100_change_mtu(struct net_device *netdev, int mtu)
 	return 0;
 }
 
+static void ftmac100_set_rx_mode(struct net_device *netdev)
+{
+	struct ftmac100 *priv = netdev_priv(netdev);
+
+	ftmac100_setup_mc_ht(priv);
+
+	if (netdev_mc_count(priv->netdev)) {
+		unsigned int maccr = ioread32(priv->base + FTMAC100_OFFSET_MACCR);
+
+		/* Make sure multicast filtering is enabled */
+		maccr |= FTMAC100_MACCR_HT_MULTI_EN;
+		iowrite32(maccr, priv->base + FTMAC100_OFFSET_MACCR);
+	}
+}
+
 static const struct net_device_ops ftmac100_netdev_ops = {
 	.ndo_open		= ftmac100_open,
 	.ndo_stop		= ftmac100_stop,
@@ -1075,6 +1113,7 @@ static const struct net_device_ops ftmac100_netdev_ops = {
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_eth_ioctl		= ftmac100_do_ioctl,
 	.ndo_change_mtu		= ftmac100_change_mtu,
+	.ndo_set_rx_mode	= ftmac100_set_rx_mode,
 };
 
 /******************************************************************************
-- 
2.37.2