3 files changed, 25 insertions, 10 deletions

diff --git a/drivers/net/dsa/sja1105/sja1105_main.c b/drivers/net/dsa/sja1105/sja1105_main.c
index ea2e7f4f96d0..7687ddcae159 100644
--- a/drivers/net/dsa/sja1105/sja1105_main.c
+++ b/drivers/net/dsa/sja1105/sja1105_main.c
@@ -1897,7 +1897,9 @@ static int sja1105_set_ageing_time(struct dsa_switch *ds,
         return sja1105_static_config_reload(priv);
 }
 
-/* Caller must hold priv->tagger_data.meta_lock */
+/* Must be called only with priv->tagger_data.state bit
+ * SJA1105_HWTS_RX_EN cleared
+ */
 static int sja1105_change_rxtstamping(struct sja1105_private *priv,
                                       bool on)
 {
@@ -1954,16 +1956,17 @@ static int sja1105_hwtstamp_set(struct dsa_switch *ds, int port,
                 break;
         }
 
-        if (rx_on != priv->tagger_data.hwts_rx_en) {
-                spin_lock(&priv->tagger_data.meta_lock);
+        if (rx_on != test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state)) {
+                clear_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state);
+
                 rc = sja1105_change_rxtstamping(priv, rx_on);
-                spin_unlock(&priv->tagger_data.meta_lock);
                 if (rc < 0) {
                         dev_err(ds->dev,
                                 "Failed to change RX timestamping: %d\n", rc);
-                        return -EFAULT;
+                        return rc;
                 }
-                priv->tagger_data.hwts_rx_en = rx_on;
+                if (rx_on)
+                        set_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state);
         }
 
         if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
@@ -1982,7 +1985,7 @@ static int sja1105_hwtstamp_get(struct dsa_switch *ds, int port,
                 config.tx_type = HWTSTAMP_TX_ON;
         else
                 config.tx_type = HWTSTAMP_TX_OFF;
-        if (priv->tagger_data.hwts_rx_en)
+        if (test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state))
                 config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
         else
                 config.rx_filter = HWTSTAMP_FILTER_NONE;
@@ -2031,7 +2034,7 @@ static bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
         struct sja1105_private *priv = ds->priv;
         struct sja1105_tagger_data *data = &priv->tagger_data;
 
-        if (!data->hwts_rx_en)
+        if (!test_bit(SJA1105_HWTS_RX_EN, &data->state))
                 return false;
 
         /* We need to read the full PTP clock to reconstruct the Rx
diff --git a/include/linux/dsa/sja1105.h b/include/linux/dsa/sja1105.h
index 79435cfc20eb..897e799dbcb9 100644
--- a/include/linux/dsa/sja1105.h
+++ b/include/linux/dsa/sja1105.h
@@ -31,6 +31,8 @@
 #define SJA1105_META_SMAC                       0x222222222222ull
 #define SJA1105_META_DMAC                       0x0180C200000Eull
 
+#define SJA1105_HWTS_RX_EN                      0
+
 /* Global tagger data: each struct sja1105_port has a reference to
  * the structure defined in struct sja1105_private.
  */
@@ -42,7 +44,7 @@ struct sja1105_tagger_data {
          * from taggers running on multiple ports on SMP systems
          */
         spinlock_t meta_lock;
-        bool hwts_rx_en;
+        unsigned long state;
 };
 
 struct sja1105_skb_cb {
diff --git a/net/dsa/tag_sja1105.c b/net/dsa/tag_sja1105.c
index 9c9aff3e52cf..63ef2a14c934 100644
--- a/net/dsa/tag_sja1105.c
+++ b/net/dsa/tag_sja1105.c
@@ -156,7 +156,11 @@ static struct sk_buff
         /* Step 1: A timestampable frame was received.
          * Buffer it until we get its meta frame.
          */
-        if (is_link_local && sp->data->hwts_rx_en) {
+        if (is_link_local) {
+                if (!test_bit(SJA1105_HWTS_RX_EN, &sp->data->state))
+                        /* Do normal processing. */
+                        return skb;
+
                 spin_lock(&sp->data->meta_lock);
                 /* Was this a link-local frame instead of the meta
                  * that we were expecting?
@@ -187,6 +191,12 @@ static struct sk_buff
         } else if (is_meta) {
                 struct sk_buff *stampable_skb;
 
+                /* Drop the meta frame if we're not in the right state
+                 * to process it.
+                 */
+                if (!test_bit(SJA1105_HWTS_RX_EN, &sp->data->state))
+                        return NULL;
+
                 spin_lock(&sp->data->meta_lock);
 
                 stampable_skb = sp->data->stampable_skb;