summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--drivers/net/ethernet/stmicro/stmmac/common.h3
-rw-r--r--drivers/net/ethernet/stmicro/stmmac/hwif.h2
-rw-r--r--drivers/net/ethernet/stmicro/stmmac/stmmac.h6
-rw-r--r--drivers/net/ethernet/stmicro/stmmac/stmmac_main.c90
-rw-r--r--drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c44
-rw-r--r--drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h1
6 files changed, 84 insertions, 62 deletions
diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index dff7a12047a2..4a811516474b 100644
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -657,7 +657,8 @@ struct mac_device_info {
unsigned int xlgmac;
unsigned int num_vlan;
u16 vlan_hash;
- bool vlan_stag;
+ int vlan_ctags;
+ int vlan_stags;
u32 vlan_filter[32];
bool vlan_fail_q_en;
u8 vlan_fail_q;
diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index a46e819aec29..b8fd4b527393 100644
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -629,7 +629,7 @@ struct stmmac_est_ops {
struct stmmac_vlan_ops {
/* VLAN */
void (*update_vlan_hash)(struct mac_device_info *hw, u32 hash,
- bool is_svlan);
+ int add_ctags, int add_stags);
void (*rx_hw_vlan)(struct mac_device_info *hw, struct dma_desc *rx_desc,
struct sk_buff *skb);
void (*set_hw_vlan_mode)(struct mac_device_info *hw, bool rx_strip,
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index 7ac58e01d846..2053f62c67f2 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -375,8 +375,10 @@ struct stmmac_priv {
void __iomem *mmcaddr;
void __iomem *ptpaddr;
void __iomem *estaddr;
- unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
- unsigned int num_svlans;
+
+ DECLARE_BITMAP(active_cvlans, VLAN_N_VID);
+ DECLARE_BITMAP(active_svlans, VLAN_N_VID);
+
int sfty_irq;
int sfty_ce_irq;
int sfty_ue_irq;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 9dad81d3ffea..d2006d90e7e9 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -7144,18 +7144,56 @@ static u32 stmmac_vid_crc32_le(__le16 vid_le)
return crc;
}
-static int stmmac_vlan_update(struct stmmac_priv *priv, bool is_svlan)
+static int stmmac_update_hw_vlan_rx_hash(struct stmmac_priv *priv, bool add,
+ __be16 proto, u16 vid)
{
+ int add_ctags = 0, add_stags = 0;
+ unsigned long *active_vlans;
u32 crc, hash = 0;
- u16 vid = 0;
+ u16 vid_cpu;
+ int ret;
+
+ if (proto == htons(ETH_P_8021Q)) {
+ active_vlans = priv->active_cvlans;
+ add_ctags = add ? 1 : -1;
+ } else {
+ active_vlans = priv->active_svlans;
+ add_stags = add ? 1 : -1;
+ }
+
+ if (add)
+ ret = test_and_set_bit(vid, active_vlans);
+ else
+ ret = test_and_clear_bit(vid, active_vlans);
+
+ if (ret == add)
+ return 0;
- for_each_set_bit(vid, priv->active_vlans, VLAN_N_VID) {
+ for_each_set_bit(vid, priv->active_cvlans, VLAN_N_VID) {
__le16 vid_le = cpu_to_le16(vid);
crc = bitrev32(~stmmac_vid_crc32_le(vid_le)) >> 28;
hash |= (1 << crc);
}
- return stmmac_update_vlan_hash(priv, priv->hw, hash, is_svlan);
+ for_each_set_bit(vid_cpu, priv->active_svlans, VLAN_N_VID) {
+ __le16 vid_le = cpu_to_le16(vid_cpu);
+ crc = bitrev32(~stmmac_vid_crc32_le(vid_le)) >> 28;
+ hash |= (1 << crc);
+ }
+
+ ret = stmmac_update_vlan_hash(priv, priv->hw, hash, add_ctags, add_stags);
+ if (ret)
+ goto err_revert_bit;
+
+ return 0;
+
+err_revert_bit:
+ if (add)
+ clear_bit(vid, active_vlans);
+ else
+ set_bit(vid, active_vlans);
+
+ return ret;
}
/* FIXME: This may need RXC to be running, but it may be called with BH
@@ -7164,33 +7202,19 @@ static int stmmac_vlan_update(struct stmmac_priv *priv, bool is_svlan)
static int stmmac_vlan_rx_add_vid(struct net_device *ndev, __be16 proto, u16 vid)
{
struct stmmac_priv *priv = netdev_priv(ndev);
- unsigned int num_svlans;
- bool is_svlan = false;
int ret;
ret = pm_runtime_resume_and_get(priv->device);
if (ret < 0)
return ret;
- if (be16_to_cpu(proto) == ETH_P_8021AD)
- is_svlan = true;
-
- set_bit(vid, priv->active_vlans);
- num_svlans = priv->num_svlans + is_svlan;
- ret = stmmac_vlan_update(priv, num_svlans);
- if (ret) {
- clear_bit(vid, priv->active_vlans);
+ ret = stmmac_update_hw_vlan_rx_hash(priv, true, proto, vid);
+ if (ret)
goto err_pm_put;
- }
ret = stmmac_add_hw_vlan_rx_fltr(priv, ndev, priv->hw, proto, vid);
- if (ret && ret != -EINVAL) {
- clear_bit(vid, priv->active_vlans);
- stmmac_vlan_update(priv, priv->num_svlans);
- goto err_pm_put;
- }
-
- priv->num_svlans = num_svlans;
+ if (ret && ret != -EINVAL)
+ stmmac_update_hw_vlan_rx_hash(priv, false, proto, vid);
err_pm_put:
pm_runtime_put(priv->device);
@@ -7204,33 +7228,19 @@ err_pm_put:
static int stmmac_vlan_rx_kill_vid(struct net_device *ndev, __be16 proto, u16 vid)
{
struct stmmac_priv *priv = netdev_priv(ndev);
- unsigned int num_svlans;
- bool is_svlan = false;
int ret;
ret = pm_runtime_resume_and_get(priv->device);
if (ret < 0)
return ret;
- if (be16_to_cpu(proto) == ETH_P_8021AD)
- is_svlan = true;
-
- clear_bit(vid, priv->active_vlans);
- num_svlans = priv->num_svlans - is_double;
- ret = stmmac_vlan_update(priv, num_svlans);
- if (ret) {
- set_bit(vid, priv->active_vlans);
- goto del_vlan_error;
- }
-
ret = stmmac_del_hw_vlan_rx_fltr(priv, ndev, priv->hw, proto, vid);
- if (ret && ret != -EINVAL) {
- set_bit(vid, priv->active_vlans);
- stmmac_vlan_update(priv, priv->num_svlans);
+ if (ret && ret != -EINVAL)
goto del_vlan_error;
- }
- priv->num_svlans = num_svlans;
+ ret = stmmac_update_hw_vlan_rx_hash(priv, false, proto, vid);
+ if (ret)
+ stmmac_add_hw_vlan_rx_fltr(priv, ndev, priv->hw, proto, vid);
del_vlan_error:
pm_runtime_put(priv->device);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
index 64270920de91..b6d4b2c7a2d3 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.c
@@ -7,17 +7,19 @@
#include "stmmac.h"
#include "stmmac_vlan.h"
+static void vlan_update_hash(struct mac_device_info *hw, u32 hash,
+ int add_ctags, int add_stags);
+
static void vlan_write_single(struct net_device *dev, u32 data)
{
void __iomem *ioaddr = (void __iomem *)dev->base_addr;
- u32 val, mask;
+ u32 val;
- mask = VLAN_TAG_ERSVLM | VLAN_TAG_VID;
if (!(data & VLAN_TAG_VID))
data |= VLAN_TAG_VID;
- val = readl(ioaddr + VLAN_TAG) & ~mask;
- val |= VLAN_TAG_ETV | (data & mask);
+ val = readl(ioaddr + VLAN_TAG) & ~VLAN_TAG_VID;
+ val |= VLAN_TAG_ETV | (data & VLAN_TAG_VID);
writel(val, ioaddr + VLAN_TAG);
}
@@ -96,6 +98,8 @@ static int vlan_add_hw_rx_fltr(struct net_device *dev,
return -EPERM;
}
+ vlan_update_hash(hw, hw->vlan_hash, !is_stag, is_stag);
+
vlan_write_single(dev, val);
hw->vlan_filter[0] = val;
@@ -147,6 +151,8 @@ static int vlan_del_hw_rx_fltr(struct net_device *dev,
if (is_stag != !!(hw->vlan_filter[0] & VLAN_TAG_ERSVLM))
return 0;
+ vlan_update_hash(hw, hw->vlan_hash, -!is_stag, -is_stag);
+
vlan_write_single(dev, 0);
hw->vlan_filter[0] = 0;
@@ -176,7 +182,7 @@ static int vlan_del_hw_rx_fltr(struct net_device *dev,
}
static void vlan_update_hash(struct mac_device_info *hw, u32 hash,
- bool is_svlan)
+ int add_ctags, int add_stags)
{
void __iomem *ioaddr = hw->pcsr;
u32 value;
@@ -184,21 +190,23 @@ static void vlan_update_hash(struct mac_device_info *hw, u32 hash,
hw->vlan_hash = hash;
writel(hash, ioaddr + VLAN_HASH_TABLE);
- value = readl(ioaddr + VLAN_TAG);
+ hw->vlan_ctags += add_ctags;
+ hw->vlan_stags += add_stags;
+ value = readl(ioaddr + VLAN_TAG) | VLAN_ETV;
- if (hash) {
- value |= VLAN_VTHM | VLAN_ETV;
- if (is_svlan) {
- hw->vlan_stag = true;
- value |= VLAN_DOVLTC;
- } else {
- hw->vlan_stag = false;
- value &= ~VLAN_DOVLTC;
- }
+ if (hash)
+ value |= VLAN_VTHM;
+ else
+ value &= ~VLAN_VTHM;
+
+ if (hw->vlan_ctags && hw->vlan_stags) {
+ value |= VLAN_DOVLTC;
+ } else if (hw->vlan_stags) {
+ value &= ~VLAN_DOVLTC;
+ value |= VLAN_ERSVLM;
} else {
- hw->vlan_stag = false;
- value &= ~(VLAN_VTHM | VLAN_ETV);
value &= ~VLAN_DOVLTC;
+ value &= ~VLAN_ERSVLM;
}
if (!hw->vlan_filter[0])
@@ -213,7 +221,7 @@ static void vlan_restore_hw_rx_fltr(struct net_device *dev,
int i;
/* Hash-based Rx VLAN Filter */
- vlan_update_hash(hw, hw->vlan_hash, hw->vlan_stag);
+ vlan_update_hash(hw, hw->vlan_hash, 0, 0);
/* Single Rx VLAN Filter */
if (hw->num_vlan == 1) {
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h
index 3ee0022282f1..e38f570e3297 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_vlan.h
@@ -19,6 +19,7 @@
#define VLAN_EDVLP BIT(26)
#define VLAN_VTHM BIT(25)
#define VLAN_DOVLTC BIT(20)
+#define VLAN_ERSVLM BIT(19)
#define VLAN_ESVL BIT(18)
#define VLAN_ETV BIT(16)
#define VLAN_VID GENMASK(15, 0)
generated by cgit v1.2.3 (git 2.39.1) at 2026-09-08 10:45:33 +0000