[PATCH net-next 6/7] net: systemport: Add support for WAKE_FILTER

[Florian Fainelli <f.fainelli@gmail.com>]

The SYSTEMPORT MAC allows up to 8 filters to be programmed to wake-up
from LAN. Verify that we have up to 8 filters and program them to the
appropriate RXCHK entries to be matched (along with their masks).

We need to update the entry and exit to Wake-on-LAN mode to keep the
RXCHK engine running to match during suspend, but this is otherwise
fairly similar to Magic Packet detection.

Signed-off-by: Florian Fainelli <f.fainelli@gmail.com>
---
 drivers/net/ethernet/broadcom/bcmsysport.c | 111 +++++++++++++++++++++++++----
 drivers/net/ethernet/broadcom/bcmsysport.h |  14 +++-
 2 files changed, 109 insertions(+), 16 deletions(-)

diff --git a/drivers/net/ethernet/broadcom/bcmsysport.c b/drivers/net/ethernet/broadcom/bcmsysport.c
index 511caec7030a..8d7ce3df1080 100644
--- a/drivers/net/ethernet/broadcom/bcmsysport.c
+++ b/drivers/net/ethernet/broadcom/bcmsysport.c
@@ -521,25 +521,31 @@ static void bcm_sysport_get_wol(struct net_device *dev,
 	struct bcm_sysport_priv *priv = netdev_priv(dev);
 	u32 reg;
 
-	wol->supported = WAKE_MAGIC | WAKE_MAGICSECURE;
+	wol->supported = WAKE_MAGIC | WAKE_MAGICSECURE | WAKE_FILTER;
 	wol->wolopts = priv->wolopts;
 
-	if (!(priv->wolopts & WAKE_MAGICSECURE))
-		return;
+	if (priv->wolopts & WAKE_MAGICSECURE) {
+		/* Return the programmed SecureOn password */
+		reg = umac_readl(priv, UMAC_PSW_MS);
+		put_unaligned_be16(reg, &wol->sopass[0]);
+		reg = umac_readl(priv, UMAC_PSW_LS);
+		put_unaligned_be32(reg, &wol->sopass[2]);
+	}
 
-	/* Return the programmed SecureOn password */
-	reg = umac_readl(priv, UMAC_PSW_MS);
-	put_unaligned_be16(reg, &wol->sopass[0]);
-	reg = umac_readl(priv, UMAC_PSW_LS);
-	put_unaligned_be32(reg, &wol->sopass[2]);
+	if (priv->wolopts & WAKE_FILTER)
+		bitmap_copy((unsigned long *)wol->sopass, priv->filters,
+			    WAKE_FILTER_BITS);
 }
 
+
 static int bcm_sysport_set_wol(struct net_device *dev,
 			       struct ethtool_wolinfo *wol)
 {
 	struct bcm_sysport_priv *priv = netdev_priv(dev);
 	struct device *kdev = &priv->pdev->dev;
-	u32 supported = WAKE_MAGIC | WAKE_MAGICSECURE;
+	u32 supported = WAKE_MAGIC | WAKE_MAGICSECURE | WAKE_FILTER;
+	unsigned int index, i = 0;
+	u32 reg;
 
 	if (!device_can_wakeup(kdev))
 		return -ENOTSUPP;
@@ -555,6 +561,32 @@ static int bcm_sysport_set_wol(struct net_device *dev,
 			    UMAC_PSW_LS);
 	}
 
+	/* We support matching up to 8 filters only */
+	if (wol->wolopts & WAKE_FILTER) {
+		bitmap_copy(priv->filters, (unsigned long *)wol->sopass,
+			    WAKE_FILTER_BITS);
+
+		if (bitmap_weight(priv->filters, WAKE_FILTER_BITS) >
+				  RXCHK_BRCM_TAG_MAX) {
+			bitmap_zero(priv->filters, WAKE_FILTER_BITS);
+			return -ENOSPC;
+		}
+
+		if (bitmap_weight(priv->filters, WAKE_FILTER_BITS) == 0)
+			return -EINVAL;
+
+		for_each_set_bit(index, priv->filters, WAKE_FILTER_BITS) {
+			/* Write the index we want to match within the CID field */
+			reg = rxchk_readl(priv, RXCHK_BRCM_TAG(i));
+			reg &= ~(RXCHK_BRCM_TAG_CID_MASK <<
+				 RXCHK_BRCM_TAG_CID_SHIFT);
+			reg |= index << RXCHK_BRCM_TAG_CID_SHIFT;
+			rxchk_writel(priv, reg, RXCHK_BRCM_TAG(i));
+			rxchk_writel(priv, 0xff00ffff, RXCHK_BRCM_TAG_MASK(i));
+			i++;
+		}
+	}
+
 	/* Flag the device and relevant IRQ as wakeup capable */
 	if (wol->wolopts) {
 		device_set_wakeup_enable(kdev, 1);
@@ -1043,7 +1075,7 @@ static int bcm_sysport_poll(struct napi_struct *napi, int budget)
 
 static void mpd_enable_set(struct bcm_sysport_priv *priv, bool enable)
 {
-	u32 reg;
+	u32 reg, bit;
 
 	reg = umac_readl(priv, UMAC_MPD_CTRL);
 	if (enable)
@@ -1051,12 +1083,32 @@ static void mpd_enable_set(struct bcm_sysport_priv *priv, bool enable)
 	else
 		reg &= ~MPD_EN;
 	umac_writel(priv, reg, UMAC_MPD_CTRL);
+
+	if (priv->is_lite)
+		bit = RBUF_ACPI_EN_LITE;
+	else
+		bit = RBUF_ACPI_EN;
+
+	reg = rbuf_readl(priv, RBUF_CONTROL);
+	if (enable)
+		reg |= bit;
+	else
+		reg &= ~bit;
+	rbuf_writel(priv, reg, RBUF_CONTROL);
 }
 
 static void bcm_sysport_resume_from_wol(struct bcm_sysport_priv *priv)
 {
+	u32 reg;
+
 	/* Stop monitoring MPD interrupt */
-	intrl2_0_mask_set(priv, INTRL2_0_MPD);
+	intrl2_0_mask_set(priv, INTRL2_0_MPD | INTRL2_0_BRCM_MATCH_TAG);
+
+	/* Disable RXCHK, active filters and Broadcom tag matching */
+	reg = rxchk_readl(priv, RXCHK_CONTROL);
+	reg &= ~(RXCHK_BRCM_TAG_MATCH_MASK <<
+		 RXCHK_BRCM_TAG_MATCH_SHIFT | RXCHK_EN | RXCHK_BRCM_TAG_EN);
+	rxchk_writel(priv, reg, RXCHK_CONTROL);
 
 	/* Clear the MagicPacket detection logic */
 	mpd_enable_set(priv, false);
@@ -1085,6 +1137,7 @@ static irqreturn_t bcm_sysport_rx_isr(int irq, void *dev_id)
 	struct bcm_sysport_priv *priv = netdev_priv(dev);
 	struct bcm_sysport_tx_ring *txr;
 	unsigned int ring, ring_bit;
+	u32 reg;
 
 	priv->irq0_stat = intrl2_0_readl(priv, INTRL2_CPU_STATUS) &
 			  ~intrl2_0_readl(priv, INTRL2_CPU_MASK_STATUS);
@@ -1111,7 +1164,14 @@ static irqreturn_t bcm_sysport_rx_isr(int irq, void *dev_id)
 		bcm_sysport_tx_reclaim_all(priv);
 
 	if (priv->irq0_stat & INTRL2_0_MPD)
-		netdev_info(priv->netdev, "Wake-on-LAN interrupt!\n");
+		netdev_info(priv->netdev, "Wake-on-LAN (MPD) interrupt!\n");
+
+	if (priv->irq0_stat & INTRL2_0_BRCM_MATCH_TAG) {
+		reg = rxchk_readl(priv, RXCHK_BRCM_TAG_MATCH_STATUS) &
+				  RXCHK_BRCM_TAG_MATCH_MASK;
+		netdev_info(priv->netdev,
+			    "Wake-on-LAN (filters 0x%02x) interrupt!\n", reg);
+	}
 
 	if (!priv->is_lite)
 		goto out;
@@ -2434,16 +2494,39 @@ static int bcm_sysport_suspend_to_wol(struct bcm_sysport_priv *priv)
 {
 	struct net_device *ndev = priv->netdev;
 	unsigned int timeout = 1000;
+	unsigned int index, i = 0;
 	u32 reg;
 
 	/* Password has already been programmed */
 	reg = umac_readl(priv, UMAC_MPD_CTRL);
-	reg |= MPD_EN;
+	if (priv->wolopts & (WAKE_MAGIC | WAKE_MAGICSECURE))
+		reg |= MPD_EN;
 	reg &= ~PSW_EN;
 	if (priv->wolopts & WAKE_MAGICSECURE)
 		reg |= PSW_EN;
 	umac_writel(priv, reg, UMAC_MPD_CTRL);
 
+	if (priv->wolopts & WAKE_FILTER) {
+		/* Turn on ACPI matching to steal packets from RBUF */
+		reg = rbuf_readl(priv, RBUF_CONTROL);
+		if (priv->is_lite)
+			reg |= RBUF_ACPI_EN_LITE;
+		else
+			reg |= RBUF_ACPI_EN;
+		rbuf_writel(priv, reg, RBUF_CONTROL);
+
+		/* Enable RXCHK, active filters and Broadcom tag matching */
+		reg = rxchk_readl(priv, RXCHK_CONTROL);
+		reg &= ~(RXCHK_BRCM_TAG_MATCH_MASK <<
+			 RXCHK_BRCM_TAG_MATCH_SHIFT);
+		for_each_set_bit(index, priv->filters, WAKE_FILTER_BITS) {
+			reg |= BIT(RXCHK_BRCM_TAG_MATCH_SHIFT + i);
+			i++;
+		}
+		reg |= RXCHK_EN | RXCHK_BRCM_TAG_EN;
+		rxchk_writel(priv, reg, RXCHK_CONTROL);
+	}
+
 	/* Make sure RBUF entered WoL mode as result */
 	do {
 		reg = rbuf_readl(priv, RBUF_STATUS);
@@ -2464,7 +2547,7 @@ static int bcm_sysport_suspend_to_wol(struct bcm_sysport_priv *priv)
 	umac_enable_set(priv, CMD_RX_EN, 1);
 
 	/* Enable the interrupt wake-up source */
-	intrl2_0_mask_clear(priv, INTRL2_0_MPD);
+	intrl2_0_mask_clear(priv, INTRL2_0_MPD | INTRL2_0_BRCM_MATCH_TAG);
 
 	netif_dbg(priv, wol, ndev, "entered WOL mode\n");
 
diff --git a/drivers/net/ethernet/broadcom/bcmsysport.h b/drivers/net/ethernet/broadcom/bcmsysport.h
index d6e5d0cbf3a3..6a64bd8cf787 100644
--- a/drivers/net/ethernet/broadcom/bcmsysport.h
+++ b/drivers/net/ethernet/broadcom/bcmsysport.h
@@ -11,6 +11,8 @@
 #ifndef __BCM_SYSPORT_H
 #define __BCM_SYSPORT_H
 
+#include <linux/bitmap.h>
+#include <linux/ethtool.h>
 #include <linux/if_vlan.h>
 #include <linux/net_dim.h>
 
@@ -155,14 +157,18 @@ struct bcm_rsb {
 #define  RXCHK_PARSE_AUTH		(1 << 22)
 
 #define RXCHK_BRCM_TAG0			0x04
-#define RXCHK_BRCM_TAG(i)		((i) * RXCHK_BRCM_TAG0)
+#define RXCHK_BRCM_TAG(i)		((i) * 0x4 + RXCHK_BRCM_TAG0)
 #define RXCHK_BRCM_TAG0_MASK		0x24
-#define RXCHK_BRCM_TAG_MASK(i)		((i) * RXCHK_BRCM_TAG0_MASK)
+#define RXCHK_BRCM_TAG_MASK(i)		((i) * 0x4 + RXCHK_BRCM_TAG0_MASK)
 #define RXCHK_BRCM_TAG_MATCH_STATUS	0x44
 #define RXCHK_ETHERTYPE			0x48
 #define RXCHK_BAD_CSUM_CNTR		0x4C
 #define RXCHK_OTHER_DISC_CNTR		0x50
 
+#define RXCHK_BRCM_TAG_MAX		8
+#define RXCHK_BRCM_TAG_CID_SHIFT	16
+#define RXCHK_BRCM_TAG_CID_MASK		0xff
+
 /* TXCHCK offsets and defines */
 #define SYS_PORT_TXCHK_OFFSET		0x380
 #define TXCHK_PKT_RDY_THRESH		0x00
@@ -185,6 +191,7 @@ struct bcm_rsb {
 #define  RBUF_RSB_SWAP0			(1 << 22)
 #define  RBUF_RSB_SWAP1			(1 << 23)
 #define  RBUF_ACPI_EN			(1 << 23)
+#define  RBUF_ACPI_EN_LITE		(1 << 24)
 
 #define RBUF_PKT_RDY_THRESH		0x04
 
@@ -524,6 +531,8 @@ struct dma_desc {
 
 #define WORDS_PER_DESC			(sizeof(struct dma_desc) / sizeof(u32))
 
+#define WAKE_FILTER_BITS		(SOPASS_MAX * BITS_PER_BYTE)
+
 /* Rx/Tx common counter group.*/
 struct bcm_sysport_pkt_counters {
 	u32	cnt_64;		/* RO Received/Transmited 64 bytes packet */
@@ -776,6 +785,7 @@ struct bcm_sysport_priv {
 
 	/* Ethtool */
 	u32			msg_enable;
+	DECLARE_BITMAP(filters, SOPASS_MAX * BITS_PER_BYTE);
 
 	struct bcm_sysport_stats64	stats64;
 
-- 
2.14.1