From 39e5308b3250666cc92c5ca33a667698ac645bd2 Mon Sep 17 00:00:00 2001 From: Yangbo Lu Date: Tue, 27 Apr 2021 12:22:03 +0800 Subject: net: mscc: ocelot: support PTP Sync one-step timestamping Although HWTSTAMP_TX_ONESTEP_SYNC existed in ioctl for hardware timestamp configuration, the PTP Sync one-step timestamping had never been supported. This patch is to truely support it. - ocelot_port_txtstamp_request() This function handles tx timestamp request by storing ptp_cmd(tx timestamp type) in OCELOT_SKB_CB(skb)->ptp_cmd, and additionally for two-step timestamp storing ts_id in OCELOT_SKB_CB(clone)->ptp_cmd. - ocelot_ptp_rew_op() During xmit, this function is called to get rew_op (rewriter option) by checking skb->cb for tx timestamp request, and configure to transmitting. Non-onestep-Sync packet with one-step timestamp request falls back to use two-step timestamp. Signed-off-by: Yangbo Lu Acked-by: Richard Cochran Signed-off-by: David S. Miller --- drivers/net/ethernet/mscc/ocelot.c | 53 ++++++++++++++++++++++++++++++++++ drivers/net/ethernet/mscc/ocelot_net.c | 8 ++--- 2 files changed, 57 insertions(+), 4 deletions(-) (limited to 'drivers') diff --git a/drivers/net/ethernet/mscc/ocelot.c b/drivers/net/ethernet/mscc/ocelot.c index 3ff4cce1ce7d..0c4283319d7f 100644 --- a/drivers/net/ethernet/mscc/ocelot.c +++ b/drivers/net/ethernet/mscc/ocelot.c @@ -6,6 +6,7 @@ */ #include #include +#include #include #include "ocelot.h" #include "ocelot_vcap.h" @@ -546,6 +547,46 @@ static void ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port, spin_unlock(&ocelot_port->ts_id_lock); } +u32 ocelot_ptp_rew_op(struct sk_buff *skb) +{ + struct sk_buff *clone = OCELOT_SKB_CB(skb)->clone; + u8 ptp_cmd = OCELOT_SKB_CB(skb)->ptp_cmd; + u32 rew_op = 0; + + if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP && clone) { + rew_op = ptp_cmd; + rew_op |= OCELOT_SKB_CB(clone)->ts_id << 3; + } else if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) { + rew_op = ptp_cmd; + } + + return rew_op; +} +EXPORT_SYMBOL(ocelot_ptp_rew_op); + +static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb) +{ + struct ptp_header *hdr; + unsigned int ptp_class; + u8 msgtype, twostep; + + ptp_class = ptp_classify_raw(skb); + if (ptp_class == PTP_CLASS_NONE) + return false; + + hdr = ptp_parse_header(skb, ptp_class); + if (!hdr) + return false; + + msgtype = ptp_get_msgtype(hdr, ptp_class); + twostep = hdr->flag_field[0] & 0x2; + + if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0) + return true; + + return false; +} + int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port, struct sk_buff *skb, struct sk_buff **clone) @@ -553,12 +594,24 @@ int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port, struct ocelot_port *ocelot_port = ocelot->ports[port]; u8 ptp_cmd = ocelot_port->ptp_cmd; + /* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */ + if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) { + if (ocelot_ptp_is_onestep_sync(skb)) { + OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd; + return 0; + } + + /* Fall back to two-step timestamping */ + ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; + } + if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) { *clone = skb_clone_sk(skb); if (!(*clone)) return -ENOMEM; ocelot_port_add_txtstamp_skb(ocelot, port, *clone); + OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd; } return 0; diff --git a/drivers/net/ethernet/mscc/ocelot_net.c b/drivers/net/ethernet/mscc/ocelot_net.c index e99c8fb3cb15..aad33d22c33f 100644 --- a/drivers/net/ethernet/mscc/ocelot_net.c +++ b/drivers/net/ethernet/mscc/ocelot_net.c @@ -514,10 +514,10 @@ static netdev_tx_t ocelot_port_xmit(struct sk_buff *skb, struct net_device *dev) return NETDEV_TX_OK; } - if (ocelot_port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) { - rew_op = ocelot_port->ptp_cmd; - rew_op |= OCELOT_SKB_CB(clone)->ts_id << 3; - } + if (clone) + OCELOT_SKB_CB(skb)->clone = clone; + + rew_op = ocelot_ptp_rew_op(skb); } ocelot_port_inject_frame(ocelot, port, 0, rew_op, skb); -- cgit v1.2.3-58-ga151