+
+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;
+}