[RFC] Wake-on-LAN flex filters

@@ -53,6 +53,17 @@ struct ethtool_wolinfo {
 	__u32	supported;
 	__u32	wolopts;
 	__u8	sopass[SOPASS_MAX]; /* SecureOn(tm) password */
+	__u32	n_filters;
+};
+
+#define WOL_FILTER_MAX 256
+#define WOL_FILTER_IGNORE 0x100
+/* wake-on-lan flexible filters */
+struct ethtool_wol_filter {
+	__u32	cmd;
+	__u32	index;
+	__u32	len;
+	__u16	mask_val[0];
 };
 
 /* for passing single values */

@@ -372,6 +383,8 @@ struct ethtool_ops {
 	void	(*complete)(struct net_device *);
 	u32     (*get_ufo)(struct net_device *);
 	int     (*set_ufo)(struct net_device *, u32);
+	int	(*get_wol_filter)(struct net_device *, struct ethtool_wol_filter *, u16 *);
+	int	(*set_wol_filter)(struct net_device *, struct ethtool_wol_filter *, u16 *);
 };
 #endif /* __KERNEL__ */
 

@@ -413,6 +426,8 @@ struct ethtool_ops {
 #define ETHTOOL_SUFO		0x00000022 /* Set UFO enable (ethtool_value) */
 #define ETHTOOL_GGSO		0x00000023 /* Get GSO enable (ethtool_value) */
 #define ETHTOOL_SGSO		0x00000024 /* Set GSO enable (ethtool_value) */
+#define ETHTOOL_GWOLFILTER	0x00000025 /* Get WOL flex filter */
+#define ETHTOOL_SWOLFILTER	0x00000026 /* Set WOL flex filter */
 
 /* compatibility with older code */
 #define SPARC_ETH_GSET		ETHTOOL_GSET

@@ -497,5 +512,6 @@ struct ethtool_ops {
 #define WAKE_ARP		(1 << 4)
 #define WAKE_MAGIC		(1 << 5)
 #define WAKE_MAGICSECURE	(1 << 6) /* only meaningful if WAKE_MAGIC */
+#define WAKE_FILTER		(1 << 7)
 
 #endif /* _LINUX_ETHTOOL_H */

@@ -229,6 +229,63 @@ static int ethtool_set_wol(struct net_de
 	return dev->ethtool_ops->set_wol(dev, &wol);
 }
 
+static int ethtool_get_wol_filter(struct net_device *dev, char __user *useraddr)
+{
+	struct ethtool_wol_filter wolfilt = { ETHTOOL_GWOLFILTER };
+	u16 *data;
+
+	if (!dev->ethtool_ops->get_wol_filter)
+		return -EOPNOTSUPP;
+
+	data = kmalloc(WOL_FILTER_MAX*2, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	if (copy_from_user(&wolfilt, useraddr, sizeof(wolfilt)))
+		return -EFAULT;
+
+	dev->ethtool_ops->get_wol_filter(dev, &wolfilt, data);
+
+	if (copy_to_user(useraddr, &wolfilt, sizeof(wolfilt)))
+		return -EFAULT;
+	if (copy_to_user(useraddr + sizeof(wolfilt), data, wolfilt.len*2))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int ethtool_set_wol_filter(struct net_device *dev, char __user *useraddr)
+{
+	struct ethtool_wol_filter wolfilt;
+	u16 *data;
+	int ret;
+
+	if (!dev->ethtool_ops->get_wol_filter)
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&wolfilt, useraddr, sizeof(wolfilt)))
+		return -EFAULT;
+
+	if (wolfilt.len > WOL_FILTER_MAX)
+		return -EOPNOTSUPP;
+		
+	if (wolfilt.len) {
+		data = kmalloc(wolfilt.len*2, GFP_KERNEL);
+		if (!data)
+			return -ENOMEM;
+	}
+
+	ret = -EFAULT;
+	if (copy_from_user(data, useraddr + sizeof(wolfilt), wolfilt.len*2))
+		goto out;
+
+	ret = dev->ethtool_ops->set_wol_filter(dev, &wolfilt, data);
+ out:
+	kfree(data);
+	return ret;
+		
+}
+
 static int ethtool_get_msglevel(struct net_device *dev, char __user *useraddr)
 {
 	struct ethtool_value edata = { ETHTOOL_GMSGLVL };

@@ -934,6 +991,12 @@ int dev_ethtool(struct ifreq *ifr)
 	case ETHTOOL_SGSO:
 		rc = ethtool_set_gso(dev, useraddr);
 		break;
+	case ETHTOOL_GWOLFILTER:
+		rc = ethtool_get_wol_filter(dev, useraddr);
+		break;
+	case ETHTOOL_SWOLFILTER:
+		rc = ethtool_set_wol_filter(dev, useraddr);
+		break;
 	default:
 		rc =  -EOPNOTSUPP;
 	}

@@ -1704,6 +1704,9 @@ e1000_get_wol(struct net_device *netdev,
 
 	default:
 		wol->supported = WAKE_UCAST | WAKE_MCAST |
+#ifdef ETHTOOL_GWOLFILTER
+				 WAKE_FILTER |
+#endif
 				 WAKE_BCAST | WAKE_MAGIC;
 		wol->wolopts = 0;
 

@@ -1716,6 +1719,12 @@ do_defaults:
 			wol->wolopts |= WAKE_BCAST;
 		if (adapter->wol & E1000_WUFC_MAG)
 			wol->wolopts |= WAKE_MAGIC;
+#ifdef ETHTOOL_GWOLFILTER
+		if (adapter->wol & E1000_WUFC_FLX_FILTERS) {
+			wol->wolopts |= WAKE_FILTER;
+			wol->n_filters = 4;
+		}
+#endif
 		return;
 	}
 }

@@ -1770,10 +1779,100 @@ e1000_set_wol(struct net_device *netdev,
 			adapter->wol |= E1000_WUFC_BC;
 		if (wol->wolopts & WAKE_MAGIC)
 			adapter->wol |= E1000_WUFC_MAG;
+#ifdef ETHTOOL_GWOLFILTER
+		if (wol->wolopts & WAKE_FILTER)
+			adapter->wol |= adapter->wol_filters;
+#endif
+	}
+
+	return 0;
+}
+
+#ifdef ETHTOOL_GWOLFILTER
+static int
+e1000_get_wol_filter(struct net_device *netdev,
+		struct ethtool_wol_filter *wolfilt,
+		uint16_t *mask_val)
+{
+	struct e1000_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+	uint32_t mask, mt_val, vt_val, fflt, shift;
+	int i;
+	
+	if (wolfilt->index > E1000_FLEXIBLE_FILTER_COUNT_MAX)
+		return -EOPNOTSUPP;
+	
+	mask = 1 << wolfilt->index;
+	shift = wolfilt->index * 8;
+
+	fflt = E1000_READ_REG_ARRAY(hw, FFLT, wolfilt->index*2);
+	wolfilt->len = fflt & 0xff;
+	for (i = 0; i < wolfilt->len; i++) {
+		mt_val = E1000_READ_REG_ARRAY(hw, FFMT, i*2);
+		vt_val = E1000_READ_REG_ARRAY(hw, FFVT, i*2);
+		mt_val &= mask;
+		vt_val = (vt_val >> shift) & 0xff;
+		if (!mt_val) {
+			mask_val[i] = WOL_FILTER_IGNORE | vt_val;
+		}
+		else
+			mask_val[i] = vt_val;
+	}
+
+	return 0;
+}
+static int
+e1000_set_wol_filter(struct net_device *netdev,
+		struct ethtool_wol_filter *wolfilt,
+		uint16_t *mask_val)
+{
+	struct e1000_adapter *adapter = netdev_priv(netdev);
+	struct e1000_hw *hw = &adapter->hw;
+	int i, shift;
+	uint32_t mask, wufc, mt_val, vt_val, fflt;
+
+	if ((wolfilt->index > E1000_FLEXIBLE_FILTER_COUNT_MAX) ||
+	    (wolfilt->len > E1000_FLEXIBLE_FILTER_SIZE_MAX)) {
+		return -EOPNOTSUPP;
 	}
+	
+	wufc = E1000_READ_REG(hw, WUFC);
+	mask = 1 << wolfilt->index;
+	if (wolfilt->len) {
+		shift = wolfilt->index * 8;
+		/* first, disable all filters */
+		E1000_WRITE_REG(hw, WUFC, (wufc & ~E1000_WUFC_FLX_FILTERS));
+		for (i = 0; i < wolfilt->len; i++) {
+			mt_val = E1000_READ_REG_ARRAY(hw, FFMT, i*2);
+			vt_val = E1000_READ_REG_ARRAY(hw, FFVT, i*2);
+			vt_val &= ~(0xffL << shift);
+			if (mask_val[i] & WOL_FILTER_IGNORE) {
+				mt_val &= ~mask;
+			} else {
+				mt_val |= mask;
+			}
+			vt_val |= ((mask_val[i] & 0xff) << shift);
+			E1000_WRITE_REG_ARRAY(hw, FFMT, i*2, mt_val);
+			E1000_WRITE_REG_ARRAY(hw, FFVT, i*2, vt_val);
+		}
+		adapter->wol_filters |= mask << E1000_WUFC_FLX_OFFSET;
+	} else {
+		/* account for zero-length filters */
+		adapter->wol_filters &= ~(mask << E1000_WUFC_FLX_OFFSET);
+		wufc &= ~(mask << E1000_WUFC_FLX_OFFSET);
+	}
+	
+	fflt = E1000_READ_REG_ARRAY(hw, FFLT, wolfilt->index * 2);
+	fflt &= ~0xff;
+	fflt |= wolfilt->len;
+	E1000_WRITE_REG_ARRAY(hw, FFLT, wolfilt->index * 2, fflt);
+	/* restore active filter settings.  This filter will not be
+	   active until enabled with ETHTOOL_SWOL. */
+	E1000_WRITE_REG(hw, WUFC, wufc);
 
 	return 0;
 }
+#endif  /* ETHTOOL_GWOLFILTER */
 
 /* toggle LED 4 times per second = 2 "blinks" per second */
 #define E1000_ID_INTERVAL	(HZ/4)

@@ -1923,6 +2022,10 @@ static struct ethtool_ops e1000_ethtool_
 	.get_stats_count        = e1000_get_stats_count,
 	.get_ethtool_stats      = e1000_get_ethtool_stats,
 	.get_perm_addr		= ethtool_op_get_perm_addr,
+#ifdef ETHTOOL_GWOLFILTER
+	.get_wol_filter		= e1000_get_wol_filter,
+	.set_wol_filter		= e1000_set_wol_filter,
+#endif
 };
 
 void e1000_set_ethtool_ops(struct net_device *netdev)

@@ -343,6 +343,7 @@ struct e1000_adapter {
 #endif
 	boolean_t smart_power_down;	/* phy smart power down */
 	unsigned long flags;
+	uint32_t wol_filters;
 };
 
 enum e1000_state_t {