net: sh_eth: add support for VLAN tag filtering

Some controllers have TSU. It can register one VLAN tag, and it can
filter other VLAN tag by hardware.
If vlan_rx_add_vid() is called twice or more, the driver will disable
the filtering.

Signed-off-by: Yoshihiro Shimoda <yoshihiro.shimoda.uh@renesas.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index 99d8ce8379c8b8eb2fe1282a7f6d0f30b5478178..8615961c128761abfdcfba4df8964ce59ea04cd7 100644 (file)
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -1888,6 +1888,62 @@ static void sh_eth_set_multicast_list(struct net_device *ndev)
 
        spin_unlock_irqrestore(&mdp->lock, flags);
 }
+
+static int sh_eth_get_vtag_index(struct sh_eth_private *mdp)
+{
+       if (!mdp->port)
+               return TSU_VTAG0;
+       else
+               return TSU_VTAG1;
+}
+
+static int sh_eth_vlan_rx_add_vid(struct net_device *ndev, u16 vid)
+{
+       struct sh_eth_private *mdp = netdev_priv(ndev);
+       int vtag_reg_index = sh_eth_get_vtag_index(mdp);
+
+       if (unlikely(!mdp->cd->tsu))
+               return -EPERM;
+
+       /* No filtering if vid = 0 */
+       if (!vid)
+               return 0;
+
+       mdp->vlan_num_ids++;
+
+       /*
+        * The controller has one VLAN tag HW filter. So, if the filter is
+        * already enabled, the driver disables it and the filte
+        */
+       if (mdp->vlan_num_ids > 1) {
+               /* disable VLAN filter */
+               sh_eth_tsu_write(mdp, 0, vtag_reg_index);
+               return 0;
+       }
+
+       sh_eth_tsu_write(mdp, TSU_VTAG_ENABLE | (vid & TSU_VTAG_VID_MASK),
+                        vtag_reg_index);
+
+       return 0;
+}
+
+static int sh_eth_vlan_rx_kill_vid(struct net_device *ndev, u16 vid)
+{
+       struct sh_eth_private *mdp = netdev_priv(ndev);
+       int vtag_reg_index = sh_eth_get_vtag_index(mdp);
+
+       if (unlikely(!mdp->cd->tsu))
+               return -EPERM;
+
+       /* No filtering if vid = 0 */
+       if (!vid)
+               return 0;
+
+       mdp->vlan_num_ids--;
+       sh_eth_tsu_write(mdp, 0, vtag_reg_index);
+
+       return 0;
+}
 #endif /* SH_ETH_HAS_TSU */
 
 /* SuperH's TSU register init function */
@@ -2037,6 +2093,8 @@ static const struct net_device_ops sh_eth_netdev_ops = {
        .ndo_get_stats          = sh_eth_get_stats,
 #if defined(SH_ETH_HAS_TSU)
        .ndo_set_rx_mode        = sh_eth_set_multicast_list,
+       .ndo_vlan_rx_add_vid    = sh_eth_vlan_rx_add_vid,
+       .ndo_vlan_rx_kill_vid   = sh_eth_vlan_rx_kill_vid,
 #endif
        .ndo_tx_timeout         = sh_eth_tx_timeout,
        .ndo_do_ioctl           = sh_eth_do_ioctl,
@@ -2141,6 +2199,7 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
                mdp->tsu_addr = ioremap(rtsu->start,
                                        resource_size(rtsu));
                mdp->port = devno % 2;
+               ndev->features = NETIF_F_HW_VLAN_FILTER;
        }
 
        /* initialize first or needed device */

diff --git a/drivers/net/ethernet/renesas/sh_eth.h b/drivers/net/ethernet/renesas/sh_eth.h
index 86b392e22c011c371a786b2cecc8a59103405079..57dc262611166de138727c63708834150f3dfad9 100644 (file)
--- a/drivers/net/ethernet/renesas/sh_eth.h
+++ b/drivers/net/ethernet/renesas/sh_eth.h
@@ -679,6 +679,10 @@ enum TSU_FWSLC_BIT {
        TSU_FWSLC_CAMSEL11 = 0x0002, TSU_FWSLC_CAMSEL10 = 0x0001,
 };
 
+/* TSU_VTAGn */
+#define TSU_VTAG_ENABLE                0x80000000
+#define TSU_VTAG_VID_MASK      0x00000fff
+
 /*
  * The sh ether Tx buffer descriptors.
  * This structure should be 20 bytes.
@@ -781,6 +785,7 @@ struct sh_eth_private {
        char post_fw;           /* POST forward */
        struct net_device_stats tsu_stats;      /* TSU forward status */
        int port;               /* for TSU */
+       int vlan_num_ids;       /* for VLAN tag filter */
 
        unsigned no_ether_link:1;
        unsigned ether_link_active_low:1;