diff --git a/drivers/net/dsa/sja1105/sja1105.h b/drivers/net/dsa/sja1105/sja1105.h
index 201bca282884da85dea4228ca1ffc30423fa5453..5f3449351668cdbbbac5cfd21d889924213add6b 100644
--- a/drivers/net/dsa/sja1105/sja1105.h
+++ b/drivers/net/dsa/sja1105/sja1105.h
@@ -131,6 +131,7 @@ struct sja1105_info {
void (*ptp_cmd_packing)(u8 *buf, struct sja1105_ptp_cmd *cmd,
enum packing_op op);
bool (*rxtstamp)(struct dsa_switch *ds, int port, struct sk_buff *skb);
+ void (*txtstamp)(struct dsa_switch *ds, int port, struct sk_buff *skb);
int (*clocking_setup)(struct sja1105_private *priv);
const char *name;
bool supports_mii[SJA1105_MAX_NUM_PORTS];
diff --git a/drivers/net/dsa/sja1105/sja1105_ptp.c b/drivers/net/dsa/sja1105/sja1105_ptp.c
index 62fe05b4cb602e7429874aff86cc028b08c4fe1e..691f6dd7e669738e3316b0909ba578dfed78bcd4 100644
--- a/drivers/net/dsa/sja1105/sja1105_ptp.c
+++ b/drivers/net/dsa/sja1105/sja1105_ptp.c
@@ -79,6 +79,7 @@ static int sja1105_change_rxtstamping(struct sja1105_private *priv,
priv->tagger_data.stampable_skb = NULL;
}
ptp_cancel_worker_sync(ptp_data->clock);
+ skb_queue_purge(&ptp_data->skb_txtstamp_queue);
skb_queue_purge(&ptp_data->skb_rxtstamp_queue);
return sja1105_static_config_reload(priv, SJA1105_RX_HWTSTAMPING);
@@ -451,6 +452,67 @@ bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
return priv->info->rxtstamp(ds, port, skb);
}
+void sja1110_process_meta_tstamp(struct dsa_switch *ds, int port, u8 ts_id,
+ enum sja1110_meta_tstamp dir, u64 tstamp)
+{
+ struct sja1105_private *priv = ds->priv;
+ struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
+ struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
+ struct skb_shared_hwtstamps shwt = {0};
+
+ /* We don't care about RX timestamps on the CPU port */
+ if (dir == SJA1110_META_TSTAMP_RX)
+ return;
+
+ spin_lock(&ptp_data->skb_txtstamp_queue.lock);
+
+ skb_queue_walk_safe(&ptp_data->skb_txtstamp_queue, skb, skb_tmp) {
+ if (SJA1105_SKB_CB(skb)->ts_id != ts_id)
+ continue;
+
+ __skb_unlink(skb, &ptp_data->skb_txtstamp_queue);
+ skb_match = skb;
+
+ break;
+ }
+
+ spin_unlock(&ptp_data->skb_txtstamp_queue.lock);
+
+ if (WARN_ON(!skb_match))
+ return;
+
+ shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(tstamp));
+ skb_complete_tx_timestamp(skb_match, &shwt);
+}
+EXPORT_SYMBOL_GPL(sja1110_process_meta_tstamp);
+
+/* In addition to cloning the skb which is done by the common
+ * sja1105_port_txtstamp, we need to generate a timestamp ID and save the
+ * packet to the TX timestamping queue.
+ */
+void sja1110_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
+{
+ struct sk_buff *clone = SJA1105_SKB_CB(skb)->clone;
+ struct sja1105_private *priv = ds->priv;
+ struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
+ struct sja1105_port *sp = &priv->ports[port];
+ u8 ts_id;
+
+ skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+
+ spin_lock(&sp->data->meta_lock);
+
+ ts_id = sp->data->ts_id;
+ /* Deal automatically with 8-bit wraparound */
+ sp->data->ts_id++;
+
+ SJA1105_SKB_CB(clone)->ts_id = ts_id;
+
+ spin_unlock(&sp->data->meta_lock);
+
+ skb_queue_tail(&ptp_data->skb_txtstamp_queue, clone);
+}
+
/* Called from dsa_skb_tx_timestamp. This callback is just to clone
* the skb and have it available in SJA1105_SKB_CB in the .port_deferred_xmit
* callback, where we will timestamp it synchronously.
@@ -469,6 +531,9 @@ void sja1105_port_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
return;
SJA1105_SKB_CB(skb)->clone = clone;
+
+ if (priv->info->txtstamp)
+ priv->info->txtstamp(ds, port, skb);
}
static int sja1105_ptp_reset(struct dsa_switch *ds)
@@ -885,7 +950,10 @@ int sja1105_ptp_clock_register(struct dsa_switch *ds)
.n_per_out = 1,
};
+ /* Only used on SJA1105 */
skb_queue_head_init(&ptp_data->skb_rxtstamp_queue);
+ /* Only used on SJA1110 */
+ skb_queue_head_init(&ptp_data->skb_txtstamp_queue);
spin_lock_init(&tagger_data->meta_lock);
ptp_data->clock = ptp_clock_register(&ptp_data->caps, ds->dev);
@@ -910,6 +978,7 @@ void sja1105_ptp_clock_unregister(struct dsa_switch *ds)
del_timer_sync(&ptp_data->extts_timer);
ptp_cancel_worker_sync(ptp_data->clock);
+ skb_queue_purge(&ptp_data->skb_txtstamp_queue);
skb_queue_purge(&ptp_data->skb_rxtstamp_queue);
ptp_clock_unregister(ptp_data->clock);
ptp_data->clock = NULL;
diff --git a/drivers/net/dsa/sja1105/sja1105_ptp.h b/drivers/net/dsa/sja1105/sja1105_ptp.h
index bf0c4f1dfed7d7ee98a212a9adb9d0e3b130f9f8..3c874bb4c17b7385b727c436c3482b4ea8529231 100644
--- a/drivers/net/dsa/sja1105/sja1105_ptp.h
+++ b/drivers/net/dsa/sja1105/sja1105_ptp.h
@@ -75,7 +75,12 @@ struct sja1105_ptp_cmd {
struct sja1105_ptp_data {
struct timer_list extts_timer;
+ /* Used only on SJA1105 to reconstruct partial timestamps */
struct sk_buff_head skb_rxtstamp_queue;
+ /* Used on SJA1110 where meta frames are generated only for
+ * 2-step TX timestamps
+ */
+ struct sk_buff_head skb_txtstamp_queue;
struct ptp_clock_info caps;
struct ptp_clock *clock;
struct sja1105_ptp_cmd cmd;
@@ -124,6 +129,7 @@ int sja1105_ptp_commit(struct dsa_switch *ds, struct sja1105_ptp_cmd *cmd,
bool sja1105_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb);
bool sja1110_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb);
+void sja1110_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb);
#else
@@ -189,6 +195,7 @@ static inline int sja1105_ptp_commit(struct dsa_switch *ds,
#define sja1105_rxtstamp NULL
#define sja1110_rxtstamp NULL
+#define sja1110_txtstamp NULL
#endif /* IS_ENABLED(CONFIG_NET_DSA_SJA1105_PTP) */
diff --git a/drivers/net/dsa/sja1105/sja1105_spi.c b/drivers/net/dsa/sja1105/sja1105_spi.c
index f7dd86271891616a9b3566c65605a10e318e23cc..32d00212423cdb9f32505dfdf99554dea5cc93d8 100644
--- a/drivers/net/dsa/sja1105/sja1105_spi.c
+++ b/drivers/net/dsa/sja1105/sja1105_spi.c
@@ -788,6 +788,7 @@ const struct sja1105_info sja1110a_info = {
.fdb_del_cmd = sja1105pqrs_fdb_del,
.ptp_cmd_packing = sja1105pqrs_ptp_cmd_packing,
.rxtstamp = sja1110_rxtstamp,
+ .txtstamp = sja1110_txtstamp,
.clocking_setup = sja1110_clocking_setup,
.port_speed = {
[SJA1105_SPEED_AUTO] = 0,
@@ -836,6 +837,7 @@ const struct sja1105_info sja1110b_info = {
.fdb_del_cmd = sja1105pqrs_fdb_del,
.ptp_cmd_packing = sja1105pqrs_ptp_cmd_packing,
.rxtstamp = sja1110_rxtstamp,
+ .txtstamp = sja1110_txtstamp,
.clocking_setup = sja1110_clocking_setup,
.port_speed = {
[SJA1105_SPEED_AUTO] = 0,
@@ -884,6 +886,7 @@ const struct sja1105_info sja1110c_info = {
.fdb_del_cmd = sja1105pqrs_fdb_del,
.ptp_cmd_packing = sja1105pqrs_ptp_cmd_packing,
.rxtstamp = sja1110_rxtstamp,
+ .txtstamp = sja1110_txtstamp,
.clocking_setup = sja1110_clocking_setup,
.port_speed = {
[SJA1105_SPEED_AUTO] = 0,
@@ -932,6 +935,7 @@ const struct sja1105_info sja1110d_info = {
.fdb_del_cmd = sja1105pqrs_fdb_del,
.ptp_cmd_packing = sja1105pqrs_ptp_cmd_packing,
.rxtstamp = sja1110_rxtstamp,
+ .txtstamp = sja1110_txtstamp,
.clocking_setup = sja1110_clocking_setup,
.port_speed = {
[SJA1105_SPEED_AUTO] = 0,
diff --git a/include/linux/dsa/sja1105.h b/include/linux/dsa/sja1105.h
index b02cf7b515ae1d79896ab5e53c6a249bb71dbef8..b6089b88314c0e99d34ee497b31ad0be69f1fbdf 100644
--- a/include/linux/dsa/sja1105.h
+++ b/include/linux/dsa/sja1105.h
@@ -45,11 +45,14 @@ struct sja1105_tagger_data {
*/
spinlock_t meta_lock;
unsigned long state;
+ u8 ts_id;
};
struct sja1105_skb_cb {
struct sk_buff *clone;
u64 tstamp;
+ /* Only valid for packets cloned for 2-step TX timestamping */
+ u8 ts_id;
};
#define SJA1105_SKB_CB(skb) \
@@ -66,4 +69,24 @@ struct sja1105_port {
u16 xmit_tpid;
};
+enum sja1110_meta_tstamp {
+ SJA1110_META_TSTAMP_TX = 0,
+ SJA1110_META_TSTAMP_RX = 1,
+};
+
+#if IS_ENABLED(CONFIG_NET_DSA_SJA1105_PTP)
+
+void sja1110_process_meta_tstamp(struct dsa_switch *ds, int port, u8 ts_id,
+ enum sja1110_meta_tstamp dir, u64 tstamp);
+
+#else
+
+static inline void sja1110_process_meta_tstamp(struct dsa_switch *ds, int port,
+ u8 ts_id, enum sja1110_meta_tstamp dir,
+ u64 tstamp)
+{
+}
+
+#endif /* IS_ENABLED(CONFIG_NET_DSA_SJA1105_PTP) */
+
#endif /* _NET_DSA_SJA1105_H */
diff --git a/net/dsa/tag_sja1105.c b/net/dsa/tag_sja1105.c
index 37e1d64e07c661dfdcfd899ebcf363c31cc092c0..9c2df9ece01b44a8a3a586f75cc6d6a2be6935de 100644
--- a/net/dsa/tag_sja1105.c
+++ b/net/dsa/tag_sja1105.c
@@ -25,6 +25,9 @@
#define SJA1110_RX_TRAILER_SWITCH_ID(x) (((x) & GENMASK(7, 4)) >> 4)
#define SJA1110_RX_TRAILER_SRC_PORT(x) ((x) & GENMASK(3, 0))
+/* Meta frame format (for 2-step TX timestamps) */
+#define SJA1110_RX_HEADER_N_TS(x) (((x) & GENMASK(8, 4)) >> 4)
+
/* TX header */
#define SJA1110_TX_HEADER_UPDATE_TC BIT(14)
#define SJA1110_TX_HEADER_TAKE_TS BIT(13)
@@ -43,6 +46,8 @@
#define SJA1110_TX_TRAILER_SWITCHID(x) (((x) << 12) & GENMASK(15, 12))
#define SJA1110_TX_TRAILER_DESTPORTS(x) (((x) << 1) & GENMASK(11, 1))
+#define SJA1110_META_TSTAMP_SIZE 10
+
#define SJA1110_HEADER_LEN 4
#define SJA1110_RX_TRAILER_LEN 13
#define SJA1110_TX_TRAILER_LEN 4
@@ -184,6 +189,7 @@ static struct sk_buff *sja1105_xmit(struct sk_buff *skb,
static struct sk_buff *sja1110_xmit(struct sk_buff *skb,
struct net_device *netdev)
{
+ struct sk_buff *clone = SJA1105_SKB_CB(skb)->clone;
struct dsa_port *dp = dsa_slave_to_port(netdev);
u16 tx_vid = dsa_8021q_tx_vid(dp->ds, dp->index);
u16 queue_mapping = skb_get_queue_mapping(skb);
@@ -221,6 +227,12 @@ static struct sk_buff *sja1110_xmit(struct sk_buff *skb,
*tx_trailer = cpu_to_be32(SJA1110_TX_TRAILER_PRIO(pcp) |
SJA1110_TX_TRAILER_SWITCHID(dp->ds->index) |
SJA1110_TX_TRAILER_DESTPORTS(BIT(dp->index)));
+ if (clone) {
+ u8 ts_id = SJA1105_SKB_CB(clone)->ts_id;
+
+ *tx_header |= htons(SJA1110_TX_HEADER_TAKE_TS);
+ *tx_trailer |= cpu_to_be32(SJA1110_TX_TRAILER_TSTAMP_ID(ts_id));
+ }
return skb;
}
@@ -423,6 +435,43 @@ static struct sk_buff *sja1105_rcv(struct sk_buff *skb,
is_meta);
}
+static struct sk_buff *sja1110_rcv_meta(struct sk_buff *skb, u16 rx_header)
+{
+ int switch_id = SJA1110_RX_HEADER_SWITCH_ID(rx_header);
+ int n_ts = SJA1110_RX_HEADER_N_TS(rx_header);
+ struct net_device *master = skb->dev;
+ struct dsa_port *cpu_dp;
+ u8 *buf = skb->data + 2;
+ struct dsa_switch *ds;
+ int i;
+
+ cpu_dp = master->dsa_ptr;
+ ds = dsa_switch_find(cpu_dp->dst->index, switch_id);
+ if (!ds) {
+ net_err_ratelimited("%s: cannot find switch id %d\n",
+ master->name, switch_id);
+ return NULL;
+ }
+
+ for (i = 0; i <= n_ts; i++) {
+ u8 ts_id, source_port, dir;
+ u64 tstamp;
+
+ ts_id = buf[0];
+ source_port = (buf[1] & GENMASK(7, 4)) >> 4;
+ dir = (buf[1] & BIT(3)) >> 3;
+ tstamp = be64_to_cpu(*(__be64 *)(buf + 2));
+
+ sja1110_process_meta_tstamp(ds, source_port, ts_id, dir,
+ tstamp);
+
+ buf += SJA1110_META_TSTAMP_SIZE;
+ }
+
+ /* Discard the meta frame, we've consumed the timestamps it contained */
+ return NULL;
+}
+
static struct sk_buff *sja1110_rcv_inband_control_extension(struct sk_buff *skb,
int *source_port,
int *switch_id)
@@ -439,6 +488,9 @@ static struct sk_buff *sja1110_rcv_inband_control_extension(struct sk_buff *skb,
*/
rx_header = ntohs(*(__be16 *)skb->data);
+ if (rx_header & SJA1110_RX_HEADER_IS_METADATA)
+ return sja1110_rcv_meta(skb, rx_header);
+
/* Timestamp frame, we have a trailer */
if (rx_header & SJA1110_RX_HEADER_HAS_TRAILER) {
int start_of_padding = SJA1110_RX_HEADER_TRAILER_POS(rx_header);