@@ -2453,6 +2453,18 @@ static enum dsa_tag_protocol ksz_get_tag_protocol(struct dsa_switch *ds,
return proto;
}
+static int ksz_connect_tag_protocol(struct dsa_switch *ds,
+ enum dsa_tag_protocol proto)
+{
+ struct ksz_tagger_data *tagger_data;
+
+ tagger_data = ksz_tagger_data(ds);
+ tagger_data->meta_tstamp_handler = ksz_tstamp_reconstruct;
+ tagger_data->is_ptp_twostep = ksz_is_ptp_twostep;
+
+ return 0;
+}
+
static int ksz_port_vlan_filtering(struct dsa_switch *ds, int port,
bool flag, struct netlink_ext_ack *extack)
{
@@ -2849,6 +2861,7 @@ static int ksz_switch_detect(struct ksz_device *dev)
static const struct dsa_switch_ops ksz_switch_ops = {
.get_tag_protocol = ksz_get_tag_protocol,
+ .connect_tag_protocol = ksz_connect_tag_protocol,
.get_phy_flags = ksz_get_phy_flags,
.setup = ksz_setup,
.teardown = ksz_teardown,
@@ -167,6 +167,59 @@ int ksz_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr)
return ret;
}
+void ksz_port_txtstamp(struct dsa_switch *ds, int port,
+ struct sk_buff *skb)
+{
+ struct ksz_device *dev = ds->priv;
+ struct ksz_port *prt = &dev->ports[port];
+ struct ptp_header *hdr;
+ struct sk_buff *clone;
+ unsigned int type;
+ u8 ptp_msg_type;
+
+ if (!prt->hwts_tx_en)
+ return;
+
+ type = ptp_classify_raw(skb);
+ if (type == PTP_CLASS_NONE)
+ return;
+
+ hdr = ptp_parse_header(skb, type);
+ if (!hdr)
+ return;
+
+ ptp_msg_type = ptp_get_msgtype(hdr, type);
+
+ switch (ptp_msg_type) {
+ case PTP_MSGTYPE_SYNC:
+ if (prt->hwts_tx_en == HWTSTAMP_TX_ONESTEP_P2P)
+ return;
+ case PTP_MSGTYPE_PDELAY_REQ:
+ case PTP_MSGTYPE_PDELAY_RESP:
+ break;
+
+ default:
+ return;
+ }
+
+ clone = skb_clone_sk(skb);
+ if (!clone)
+ return;
+
+ /* caching the value to be used in later */
+ KSZ_SKB_CB(skb)->clone = clone;
+ KSZ_SKB_CB(clone)->ptp_type = type;
+ KSZ_SKB_CB(clone)->ptp_msg_type = ptp_msg_type;
+}
+
+bool ksz_is_ptp_twostep(struct dsa_switch *ds, unsigned int port)
+{
+ struct ksz_device *dev = ds->priv;
+ struct ksz_port *prt = &dev->ports[port];
+
+ return (prt->hwts_tx_en == HWTSTAMP_TX_ON);
+}
+
/* These are function related to the ptp clock info */
static int _ksz_ptp_gettime(struct ksz_device *dev, struct timespec64 *ts)
{
@@ -380,6 +433,31 @@ static int ksz_ptp_start_clock(struct ksz_device *dev)
return 0;
}
+ktime_t ksz_tstamp_reconstruct(struct dsa_switch *ds, ktime_t tstamp)
+{
+ struct ksz_device *dev = ds->priv;
+ struct ksz_ptp_data *ptp_data = &dev->ptp_data;
+ struct timespec64 ts = ktime_to_timespec64(tstamp);
+ struct timespec64 ptp_clock_time;
+ struct timespec64 diff;
+
+ spin_lock_bh(&ptp_data->clock_lock);
+ ptp_clock_time = ptp_data->clock_time;
+ spin_unlock_bh(&ptp_data->clock_lock);
+
+ /* calculate full time from partial time stamp */
+ ts.tv_sec = (ptp_clock_time.tv_sec & ~3) | ts.tv_sec;
+
+ /* find nearest possible point in time */
+ diff = timespec64_sub(ts, ptp_clock_time);
+ if (diff.tv_sec > 2)
+ ts.tv_sec -= 4;
+ else if (diff.tv_sec < -2)
+ ts.tv_sec += 4;
+
+ return timespec64_to_ktime(ts);
+}
+
static const struct ptp_clock_info ksz_ptp_caps = {
.owner = THIS_MODULE,
.name = "Microchip Clock",
@@ -28,6 +28,8 @@ int ksz_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr);
int ksz_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr);
int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p);
void ksz_ptp_irq_free(struct dsa_switch *ds, u8 p);
+ktime_t ksz_tstamp_reconstruct(struct dsa_switch *ds, ktime_t tstamp);
+bool ksz_is_ptp_twostep(struct dsa_switch *ds, unsigned int port);
#else
@@ -64,6 +66,16 @@ static inline int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p)
static inline void ksz_ptp_irq_free(struct dsa_switch *ds, u8 p) {}
+static inline ktime_t ksz_tstamp_reconstruct(struct dsa_switch *ds, ktime_t tstamp)
+{
+ return 0;
+}
+
+bool ksz_is_ptp_twostep(struct dsa_switch *ds, unsigned int port)
+{
+ return 0;
+}
+
#endif /* End of CONFIG_NET_DSA_MICROCHIOP_KSZ_PTP */
#endif
@@ -9,9 +9,25 @@
#include <net/dsa.h>
+/* All time stamps from the KSZ consist of 2 bits for seconds and 30 bits for
+ * nanoseconds. This is NOT the same as 32 bits for nanoseconds.
+ */
+#define KSZ_TSTAMP_SEC_MASK GENMASK(31, 30)
+#define KSZ_TSTAMP_NSEC_MASK GENMASK(29, 0)
+
+static inline ktime_t ksz_decode_tstamp(u32 tstamp)
+{
+ u64 ns = FIELD_GET(KSZ_TSTAMP_SEC_MASK, tstamp) * NSEC_PER_SEC +
+ FIELD_GET(KSZ_TSTAMP_NSEC_MASK, tstamp);
+
+ return ns_to_ktime(ns);
+}
+
struct ksz_tagger_data {
bool (*hwtstamp_get_state)(struct dsa_switch *ds);
void (*hwtstamp_set_state)(struct dsa_switch *ds, bool on);
+ ktime_t (*meta_tstamp_handler)(struct dsa_switch *ds, ktime_t tstamp);
+ bool (*is_ptp_twostep)(struct dsa_switch *ds, unsigned int port);
};
static inline struct ksz_tagger_data *
@@ -7,6 +7,7 @@
#include <linux/dsa/ksz_common.h>
#include <linux/etherdevice.h>
#include <linux/list.h>
+#include <linux/ptp_classify.h>
#include <net/dsa.h>
#include "dsa_priv.h"
@@ -169,6 +170,63 @@ MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ8795, KSZ8795_NAME);
#define KSZ9477_TAIL_TAG_OVERRIDE BIT(9)
#define KSZ9477_TAIL_TAG_LOOKUP BIT(10)
+static void ksz_rcv_timestamp(struct sk_buff *skb, u8 *tag,
+ struct net_device *dev, unsigned int port)
+{
+ struct skb_shared_hwtstamps *hwtstamps = skb_hwtstamps(skb);
+ u8 *tstamp_raw = tag - KSZ9477_PTP_TAG_LEN;
+ struct dsa_switch *ds = dev->dsa_ptr->ds;
+ struct ksz_tagger_data *tagger_data;
+ struct ptp_header *ptp_hdr;
+ unsigned int ptp_type;
+ u8 ptp_msg_type;
+ ktime_t tstamp;
+ s64 correction;
+
+ tagger_data = ksz_tagger_data(ds);
+ if (!tagger_data->meta_tstamp_handler)
+ return;
+
+ /* convert time stamp and write to skb */
+ tstamp = ksz_decode_tstamp(get_unaligned_be32(tstamp_raw));
+ memset(hwtstamps, 0, sizeof(*hwtstamps));
+ hwtstamps->hwtstamp = tagger_data->meta_tstamp_handler(ds, tstamp);
+
+ if (skb_headroom(skb) < ETH_HLEN)
+ return;
+
+ __skb_push(skb, ETH_HLEN);
+ ptp_type = ptp_classify_raw(skb);
+ __skb_pull(skb, ETH_HLEN);
+
+ if (ptp_type == PTP_CLASS_NONE)
+ return;
+
+ ptp_hdr = ptp_parse_header(skb, ptp_type);
+ if (!ptp_hdr)
+ return;
+
+ if (!tagger_data->is_ptp_twostep)
+ return;
+
+ if (tagger_data->is_ptp_twostep(ds, port))
+ return;
+
+ ptp_msg_type = ptp_get_msgtype(ptp_hdr, ptp_type);
+ if (ptp_msg_type != PTP_MSGTYPE_PDELAY_REQ)
+ return;
+
+ /* Only subtract the partial time stamp from the correction field. When
+ * the hardware adds the egress time stamp to the correction field of
+ * the PDelay_Resp message on tx, also only the partial time stamp will
+ * be added.
+ */
+ correction = (s64)get_unaligned_be64(&ptp_hdr->correction);
+ correction -= ktime_to_ns(tstamp) << 16;
+
+ ptp_header_update_correction(skb, ptp_type, ptp_hdr, correction);
+}
+
static struct sk_buff *ksz9477_xmit(struct sk_buff *skb,
struct net_device *dev)
{
@@ -202,8 +260,10 @@ static struct sk_buff *ksz9477_rcv(struct sk_buff *skb, struct net_device *dev)
unsigned int len = KSZ_EGRESS_TAG_LEN;
/* Extra 4-bytes PTP timestamp */
- if (tag[0] & KSZ9477_PTP_TAG_INDICATION)
+ if (tag[0] & KSZ9477_PTP_TAG_INDICATION) {
+ ksz_rcv_timestamp(skb, tag, dev, port);
len += KSZ9477_PTP_TAG_LEN;
+ }
return ksz_common_rcv(skb, dev, port, len);
}
@@ -215,7 +275,7 @@ static const struct dsa_device_ops ksz9477_netdev_ops = {
.rcv = ksz9477_rcv,
.connect = ksz_connect,
.disconnect = ksz_disconnect,
- .needed_tailroom = KSZ9477_INGRESS_TAG_LEN,
+ .needed_tailroom = KSZ9477_INGRESS_TAG_LEN + KSZ9477_PTP_TAG_LEN,
};
DSA_TAG_DRIVER(ksz9477_netdev_ops);
@@ -253,7 +313,7 @@ static const struct dsa_device_ops ksz9893_netdev_ops = {
.rcv = ksz9477_rcv,
.connect = ksz_connect,
.disconnect = ksz_disconnect,
- .needed_tailroom = KSZ_INGRESS_TAG_LEN,
+ .needed_tailroom = KSZ_INGRESS_TAG_LEN + KSZ9477_PTP_TAG_LEN,
};
DSA_TAG_DRIVER(ksz9893_netdev_ops);
@@ -266,10 +326,11 @@ MODULE_ALIAS_DSA_TAG_DRIVER(DSA_TAG_PROTO_KSZ9893, KSZ9893_NAME);
* tag0 : represents tag override, lookup and valid
* tag1 : each bit represents port (eg, 0x01=port1, 0x02=port2, 0x80=port8)
*
- * For rcv, 1 byte is added before FCS.
+ * For rcv, 1/5 bytes is added before FCS.
* ---------------------------------------------------------------------------
- * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|tag0(1byte)|FCS(4bytes)
+ * DA(6bytes)|SA(6bytes)|....|Data(nbytes)|ts(4bytes)|tag0(1byte)|FCS(4bytes)
* ---------------------------------------------------------------------------
+ * ts : time stamp (Present only if bit 7 of tag0 is set)
* tag0 : zero-based value represents port
* (eg, 0x00=port1, 0x02=port3, 0x07=port8)
*/
@@ -313,7 +374,7 @@ static const struct dsa_device_ops lan937x_netdev_ops = {
.rcv = ksz9477_rcv,
.connect = ksz_connect,
.disconnect = ksz_disconnect,
- .needed_tailroom = LAN937X_EGRESS_TAG_LEN,
+ .needed_tailroom = LAN937X_EGRESS_TAG_LEN + KSZ9477_PTP_TAG_LEN,
};
DSA_TAG_DRIVER(lan937x_netdev_ops);