[PATCH net-next v3 3/4] net: ti: icssg-prueth: Add VLAN support for HSR mode

[MD Danish Anwar]

From: Ravi Gunasekaran <r-gunaseka...@ti.com>

Add support for VLAN addition/deletion in HSR mode.
In HSR mode, even if the host port is not a member of
the VLAN domain, the slave ports should simply forward the
frames. So allow forwarding of all VLAN frames in HSR mode.

Signed-off-by: Ravi Gunasekaran <r-gunaseka...@ti.com>
Signed-off-by: MD Danish Anwar <danishan...@ti.com>
---
 drivers/net/ethernet/ti/icssg/icssg_prueth.c | 45 +++++++++++++++++++-
 1 file changed, 44 insertions(+), 1 deletion(-)

diff --git a/drivers/net/ethernet/ti/icssg/icssg_prueth.c 
b/drivers/net/ethernet/ti/icssg/icssg_prueth.c
index 0556910938fa..b4d70c6e0cff 100644
--- a/drivers/net/ethernet/ti/icssg/icssg_prueth.c
+++ b/drivers/net/ethernet/ti/icssg/icssg_prueth.c
@@ -808,6 +808,47 @@ static netdev_features_t emac_ndo_fix_features(struct 
net_device *ndev,
        return features;
 }
 
+static int emac_ndo_vlan_rx_add_vid(struct net_device *ndev,
+                                   __be16 proto, u16 vid)
+{
+       struct prueth_emac *emac = netdev_priv(ndev);
+       struct prueth *prueth = emac->prueth;
+       int untag_mask = 0;
+       int port_mask;
+
+       if (prueth->is_hsr_offload_mode) {
+               port_mask = BIT(PRUETH_PORT_HOST) | BIT(emac->port_id);
+               untag_mask = 0;
+
+               netdev_dbg(emac->ndev, "VID add vid:%u port_mask:%X untag_mask 
%X\n",
+                          vid, port_mask, untag_mask);
+
+               icssg_vtbl_modify(emac, vid, port_mask, untag_mask, true);
+               icssg_set_pvid(emac->prueth, vid, emac->port_id);
+       }
+       return 0;
+}
+
+static int emac_ndo_vlan_rx_del_vid(struct net_device *ndev,
+                                   __be16 proto, u16 vid)
+{
+       struct prueth_emac *emac = netdev_priv(ndev);
+       struct prueth *prueth = emac->prueth;
+       int untag_mask = 0;
+       int port_mask;
+
+       if (prueth->is_hsr_offload_mode) {
+               port_mask = BIT(PRUETH_PORT_HOST);
+               untag_mask = 0;
+
+               netdev_dbg(emac->ndev, "VID del vid:%u port_mask:%X untag_mask  
%X\n",
+                          vid, port_mask, untag_mask);
+
+               icssg_vtbl_modify(emac, vid, port_mask, untag_mask, false);
+       }
+       return 0;
+}
+
 static const struct net_device_ops emac_netdev_ops = {
        .ndo_open = emac_ndo_open,
        .ndo_stop = emac_ndo_stop,
@@ -820,6 +861,8 @@ static const struct net_device_ops emac_netdev_ops = {
        .ndo_get_stats64 = icssg_ndo_get_stats64,
        .ndo_get_phys_port_name = icssg_ndo_get_phys_port_name,
        .ndo_fix_features = emac_ndo_fix_features,
+       .ndo_vlan_rx_add_vid = emac_ndo_vlan_rx_add_vid,
+       .ndo_vlan_rx_kill_vid = emac_ndo_vlan_rx_del_vid,
 };
 
 static int prueth_netdev_init(struct prueth *prueth,
@@ -947,7 +990,7 @@ static int prueth_netdev_init(struct prueth *prueth,
        ndev->netdev_ops = &emac_netdev_ops;
        ndev->ethtool_ops = &icssg_ethtool_ops;
        ndev->hw_features = NETIF_F_SG;
-       ndev->features = ndev->hw_features;
+       ndev->features = ndev->hw_features | NETIF_F_HW_VLAN_CTAG_FILTER;
        ndev->hw_features |= NETIF_PRUETH_HSR_OFFLOAD_FEATURES;
 
        netif_napi_add(ndev, &emac->napi_rx, icssg_napi_rx_poll);
-- 
2.34.1