[PATCH net-next v2 4/5] net: phy: broadcom: Add support for WAKE_FILTER

[Florian Fainelli <florian.fainelli@broadcom.com>]

[-- Attachment #1: Type: text/plain, Size: 8419 bytes --]

Since the PHY is capable of matching any arbitrary Ethernet MAC
destination as a programmable wake-up pattern, add support for doing
that using the WAKE_FILTER and ethtool::rxnfc API. For instance, in
order to wake-up from the Ethernet MAC address corresponding to the IPv4
multicast IP address of 224.0.0.251 (e.g.: multicast DNS), one could do:

  ethtool -N eth0 flow-type ether dst 01:00:5e:00:00:fb loc 0 action -2
  ethtool -n eth0
  Total 1 rules

  Filter: 0
          Flow Type: Raw Ethernet
          Src MAC addr: 00:00:00:00:00:00 mask: FF:FF:FF:FF:FF:FF
          Dest MAC addr: 01:00:5E:00:00:FB mask: 00:00:00:00:00:00
          Ethertype: 0x0 mask: 0xFFFF
          Action: Wake-on-LAN
  ethtool -s eth0 wol f

Signed-off-by: Florian Fainelli <florian.fainelli@broadcom.com>
---
 drivers/net/phy/bcm-phy-lib.c | 205 +++++++++++++++++++++++++++++++++-
 drivers/net/phy/bcm-phy-lib.h |   5 +
 drivers/net/phy/broadcom.c    |   2 +
 3 files changed, 211 insertions(+), 1 deletion(-)

diff --git a/drivers/net/phy/bcm-phy-lib.c b/drivers/net/phy/bcm-phy-lib.c
index 876f28fd8256..5dfc1a5fde2e 100644
--- a/drivers/net/phy/bcm-phy-lib.c
+++ b/drivers/net/phy/bcm-phy-lib.c
@@ -827,7 +827,8 @@ EXPORT_SYMBOL_GPL(bcm_phy_cable_test_get_status_rdb);
 					 WAKE_MCAST | \
 					 WAKE_BCAST | \
 					 WAKE_MAGIC | \
-					 WAKE_MAGICSECURE)
+					 WAKE_MAGICSECURE | \
+					 WAKE_FILTER)
 
 int bcm_phy_set_wol(struct phy_device *phydev, struct ethtool_wolinfo *wol)
 {
@@ -881,6 +882,21 @@ int bcm_phy_set_wol(struct phy_device *phydev, struct ethtool_wolinfo *wol)
 	ctl &= ~BCM54XX_WOL_DIR_PKT_EN;
 	ctl &= ~(BCM54XX_WOL_SECKEY_OPT_MASK << BCM54XX_WOL_SECKEY_OPT_SHIFT);
 
+	/* For WAKE_FILTER, we have already programmed the desired MAC DA
+	 * and associated mask by the time we get there.
+	 */
+	if (wol->wolopts & WAKE_FILTER)
+		goto program_ctl;
+
+	/* Enabling options other than WAKE_FILTER nullifies the one and only
+	 * network rule that we support.
+	 */
+	if (!(wol->wolopts & WAKE_FILTER)) {
+		ret = bcm_phy_write_exp(phydev, BCM54XX_WOL_SEC_KEY_8B, 0);
+		if (ret < 0)
+			return ret;
+	}
+
 	/* When using WAKE_MAGIC, we program the magic pattern filter to match
 	 * the device's MAC address and we accept any MAC DA in the Ethernet
 	 * frame.
@@ -935,6 +951,7 @@ int bcm_phy_set_wol(struct phy_device *phydev, struct ethtool_wolinfo *wol)
 			return ret;
 	}
 
+program_ctl:
 	if (wol->wolopts & WAKE_MAGICSECURE) {
 		ctl |= BCM54XX_WOL_SECKEY_OPT_6B <<
 		       BCM54XX_WOL_SECKEY_OPT_SHIFT;
@@ -999,6 +1016,16 @@ void bcm_phy_get_wol(struct phy_device *phydev, struct ethtool_wolinfo *wol)
 	if (!(ctl & BCM54XX_WOL_EN))
 		return;
 
+	ret = bcm_phy_read_exp(phydev, BCM54XX_WOL_SEC_KEY_8B);
+	if (ret < 0)
+		return;
+
+	/* Mutualy exclusive with other modes */
+	if (ret) {
+		wol->wolopts |= WAKE_FILTER;
+		return;
+	}
+
 	for (i = 0; i < sizeof(da) / 2; i++) {
 		ret = bcm_phy_read_exp(phydev,
 				       BCM54XX_WOL_MPD_DATA2(2 - i));
@@ -1066,6 +1093,182 @@ int bcm_phy_led_brightness_set(struct phy_device *phydev,
 }
 EXPORT_SYMBOL_GPL(bcm_phy_led_brightness_set);
 
+static int bcm_phy_get_rule(struct phy_device *phydev,
+			    struct ethtool_rxnfc *nfc,
+			    int loc)
+{
+	u8 da[ETH_ALEN];
+	unsigned int i;
+	int ret;
+
+	if (loc != 0)
+		return -EINVAL;
+
+	memset(nfc, 0, sizeof(*nfc));
+	nfc->flow_type = ETHER_FLOW;
+	nfc->fs.flow_type = ETHER_FLOW;
+
+	for (i = 0; i < sizeof(da) / 2; i++) {
+		ret = bcm_phy_read_exp(phydev,
+				       BCM54XX_WOL_MPD_DATA2(2 - i));
+		if (ret < 0)
+			return ret;
+
+		da[i * 2] = ret >> 8;
+		da[i * 2 + 1] = ret & 0xff;
+	}
+	ether_addr_copy(nfc->fs.h_u.ether_spec.h_dest, da);
+
+	for (i = 0; i < sizeof(da) / 2; i++) {
+		ret = bcm_phy_read_exp(phydev,
+				       BCM54XX_WOL_MASK(2 - i));
+		if (ret < 0)
+			return ret;
+
+		da[i * 2] = ~(ret >> 8);
+		da[i * 2 + 1] = ~(ret & 0xff);
+	}
+	ether_addr_copy(nfc->fs.m_u.ether_spec.h_dest, da);
+
+	ret = bcm_phy_read_exp(phydev, BCM54XX_WOL_INNER_PROTO);
+	if (ret < 0)
+		return ret;
+
+	nfc->fs.h_u.ether_spec.h_proto = cpu_to_be16(ret);
+
+	nfc->fs.ring_cookie = RX_CLS_FLOW_WAKE;
+	nfc->fs.location = 0;
+
+	return 0;
+}
+
+static int bcm_phy_set_rule(struct phy_device *phydev,
+			    struct ethtool_rxnfc *nfc)
+{
+	int ret = -EOPNOTSUPP;
+	unsigned int i;
+	const u8 *da;
+	u16 h_proto;
+
+	/* We support only matching on the MAC DA with a custom mask and
+	 * optionally with a specific Ethernet type, reject anything else.
+	 */
+	if (nfc->fs.ring_cookie != RX_CLS_FLOW_WAKE ||
+	    (nfc->fs.location != 0 &&
+	     nfc->fs.location != RX_CLS_LOC_ANY &&
+	     nfc->fs.location != RX_CLS_LOC_FIRST) ||
+	    nfc->fs.flow_type != ETHER_FLOW ||
+	    !is_zero_ether_addr(nfc->fs.h_u.ether_spec.h_source) ||
+	    !is_zero_ether_addr(nfc->fs.m_u.ether_spec.h_source))
+		return ret;
+
+	ret = bcm_phy_read_exp(phydev, BCM54XX_WOL_SEC_KEY_8B);
+	if (ret < 0)
+		return ret;
+
+	if (ret)
+		return -EBUSY;
+
+	if (nfc->fs.location == RX_CLS_LOC_ANY ||
+	    nfc->fs.location == RX_CLS_LOC_FIRST)
+		nfc->fs.location = 0;
+
+	da = nfc->fs.h_u.ether_spec.h_dest;
+	for (i = 0; i < ETH_ALEN / 2; i++) {
+		ret = bcm_phy_write_exp(phydev,
+					BCM54XX_WOL_MPD_DATA2(2 - i),
+					da[i * 2] << 8 | da[i * 2 + 1]);
+		if (ret < 0)
+			return ret;
+	}
+
+	da = nfc->fs.m_u.ether_spec.h_dest;
+	for (i = 0; i < ETH_ALEN / 2; i++) {
+		u16 mask = da[i * 2] << 8 | da[i * 2 + 1];
+		ret = bcm_phy_write_exp(phydev,
+					BCM54XX_WOL_MASK(2 - i),
+					~mask);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Restore default inner protocol field unless overridden by the flow
+	 * specification.
+	 */
+	h_proto = be16_to_cpu(nfc->fs.h_u.ether_spec.h_proto);
+	if (!h_proto)
+		h_proto = ETH_P_8021Q;
+
+	ret = bcm_phy_write_exp(phydev, BCM54XX_WOL_INNER_PROTO,
+				h_proto);
+	if (ret < 0)
+		return ret;
+
+	/* Use BCM54XX_WOL_SEC_KEY_8B as a scratch register to record
+	 * that we installed a filter rule.
+	 */
+	return bcm_phy_write_exp(phydev, BCM54XX_WOL_SEC_KEY_8B, 1);
+}
+
+int bcm_phy_get_rxnfc(struct phy_device *phydev,
+		      struct ethtool_rxnfc *cmd, u32 *rule_locs)
+{
+	int err = 0, rule_cnt = 0;
+
+	err = bcm_phy_read_exp(phydev, BCM54XX_WOL_SEC_KEY_8B);
+	if (err < 0)
+		return err;
+
+	rule_cnt = err;
+	err = 0;
+
+	switch (cmd->cmd) {
+	case ETHTOOL_GRXCLSRLCNT:
+		cmd->rule_cnt = rule_cnt;
+		cmd->data = 1 | RX_CLS_LOC_SPECIAL;
+		break;
+	case ETHTOOL_GRXCLSRULE:
+		err = bcm_phy_get_rule(phydev, cmd, cmd->fs.location);
+		break;
+	case ETHTOOL_GRXCLSRLALL:
+		if (rule_cnt)
+			rule_locs[0] = 0;
+		cmd->rule_cnt = rule_cnt;
+		cmd->data = 1;
+		break;
+	default:
+		err = -EOPNOTSUPP;
+		break;
+	}
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(bcm_phy_get_rxnfc);
+
+int bcm_phy_set_rxnfc(struct phy_device *phydev,
+		      struct ethtool_rxnfc *cmd)
+{
+	int err = 0;
+
+	switch (cmd->cmd) {
+	case ETHTOOL_SRXCLSRLINS:
+		err = bcm_phy_set_rule(phydev, cmd);
+		break;
+	case ETHTOOL_SRXCLSRLDEL:
+		if (cmd->fs.location != 0)
+			return err;
+
+		err = bcm_phy_write_exp(phydev, BCM54XX_WOL_SEC_KEY_8B, 0);
+		break;
+	default:
+		err = -EOPNOTSUPP;
+		break;
+	}
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(bcm_phy_set_rxnfc);
+
 MODULE_DESCRIPTION("Broadcom PHY Library");
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Broadcom Corporation");
diff --git a/drivers/net/phy/bcm-phy-lib.h b/drivers/net/phy/bcm-phy-lib.h
index b52189e45a84..7081edcec06b 100644
--- a/drivers/net/phy/bcm-phy-lib.h
+++ b/drivers/net/phy/bcm-phy-lib.h
@@ -121,4 +121,9 @@ irqreturn_t bcm_phy_wol_isr(int irq, void *dev_id);
 int bcm_phy_led_brightness_set(struct phy_device *phydev,
 			       u8 index, enum led_brightness value);
 
+int bcm_phy_get_rxnfc(struct phy_device *phydev,
+		      struct ethtool_rxnfc *nfc, u32 *rule_locs);
+int bcm_phy_set_rxnfc(struct phy_device *phydev,
+		      struct ethtool_rxnfc *nfc);
+
 #endif /* _LINUX_BCM_PHY_LIB_H */
diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
index 3a627105675a..6c2212bd2779 100644
--- a/drivers/net/phy/broadcom.c
+++ b/drivers/net/phy/broadcom.c
@@ -1107,6 +1107,8 @@ static struct phy_driver broadcom_drivers[] = {
 	.get_wol	= bcm54xx_phy_get_wol,
 	.set_wol	= bcm54xx_phy_set_wol,
 	.led_brightness_set	= bcm_phy_led_brightness_set,
+	.get_rxnfc	= bcm_phy_get_rxnfc,
+	.set_rxnfc	= bcm_phy_set_rxnfc,
 }, {
 	.phy_id		= PHY_ID_BCM5461,
 	.phy_id_mask	= 0xfffffff0,
-- 
2.34.1


[-- Attachment #2: S/MIME Cryptographic Signature --]
[-- Type: application/pkcs7-signature, Size: 4221 bytes --]